[发明专利]运动旋转物体的转动轴方向与转动角速度解算方法

权利要求书

1.运动旋转物体的转动轴方向与转动角速度解算方法，其特征在于，包含以下步骤：

步骤1，构建局部轨迹矩阵：当求解t_i时刻的旋转轴方向与旋转角速度时，将t_i时刻的点云利用最近点迭代点云配准算法与t_i-1时刻点云配准得到配准矩阵M_i-1；利用t_i时刻的点云和t_i-1时刻点云得到的配准矩阵M_i-1的信息对下一时刻点云的配准计算进行初始化，进一步迭代得到t_i到t_i+1时刻的配准矩阵M_i+1，利用配准矩阵M_i-1、M_i+1构建局部轨迹矩阵TM；

步骤2，求取点的位置和速度：对t_i时刻的点云上的任意的一点P，利用步骤1所得点的局部轨迹矩阵乘以该点的扩充向量得到其位置轨迹PS，对该点位置轨迹PS进行曲线拟合，根据拟合曲线求出t_i时刻该点在各个方向的速度v_x、v_y、v_z；

步骤3，求解旋转参数：选取t_i时刻的点云中的任意n个点，按照步骤2的方法求取在t_i时刻点云上所选取的n个点分别在t_i时刻的位置轨迹PS和速度；构造运动目标的运动方程，联立t_i时刻点云中n个点的位置和速度，求解得到运动旋转目标t_i时刻各个轴的分角速度；进而根据分角速度求出运动旋转物体t_i的转动速度和转轴方向。

2.根据权利要求1所述的运动旋转物体的转动轴方向与转动角速度解算方法，其特征在于，步骤1中所述点云是通过敏感器进行扫描采样得到。

3.根据权利要求1所述的运动旋转物体的转动轴方向与转动角速度解算方法，其特征在于，步骤1中的构造的配准矩阵M形式如下：

式中，R为旋转变化矩阵，T为平移向量；将t_i时刻点云数据扩展为D_i＝[x_i,k y_i,k z_i,k1]_m×4，k∈[1,m],m为该帧点云中点总个数；根据配准公式可得D_iM_i-1＝D_i-1；对于匀速转动或者高频率采样的点云，由M_i-1能得到M′_i+1，将M′_i+1作为求解M_i+1的最近点云配准算法的初始解进行迭代，M′_i+1的构造方式如下：

4.根据权利要求1所述的运动旋转物体的转动轴方向与转动角速度解算方法，其特征在于，步骤1中的构造局部轨迹矩阵TM代表t_i时刻整体点云的运动信息，局部轨迹矩阵TM具体形式为：

式中，I_4×4为大小为4的单位矩阵。