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

说明书

本发明提供了运动旋转物体的转动轴方向与转动角速度解算方法，首先使用最近点迭代点云配准算法对该时刻相邻点云进行配准，利用配准矩阵构建局部轨迹矩阵；其次根据局部轨迹矩阵求取点位置信息，并点轨迹拟合，求解出某时刻某点此时的速度，最后根据刚体转动物理过程建立方程，进行旋转参数解算步骤；本发明摆脱了对特征点的依赖，使用局部轨迹矩阵来描述目标在某个时刻的运动状况；对由于目标自遮挡而造成点云局部缺失有很好的鲁棒性，避免了因序列点云中特征点轨迹不完整而造成的误差；能有效求解转动轴与转动角速度，特别是当旋转物体角速度为变速运动时，求解结果能更好地跟踪设定角速度的变化，求解精度更高，求解优势更为明显。

技术领域

本发明属于运动物体旋转参数解算及扫描点云领域，具体涉及一种运动旋转物体的转动轴方向与转动角速度解算方法。

背景技术

在航天领域，对于空间非合作目标的运动参数解算是进行目标跟踪、侦查以及在轨接近的前提，因此对空间非合作目标的运动参数研究是一个很重要的研究领域。常用的一种技术手段是利用激光类或相机类敏感器对非合作目标进行观察扫描，得到非合作目标的序列三维点云。一般情况下可以根据序列点云来获取非合作目标的运动参数，如运动速度、转动轴方向及转动角速度。

目前转动轴方向及转动角速度求解算法有很多种，根据主要原理大致分为两种。第一种是通过特征点轨迹来求取，主要步骤为：(1)选取点云中的特征点或者标记点；(2)从序列点云中找出特征点的位置轨迹；(3)建立运动方程求解转动轴方向及转动角速度。这种方法依赖于特征点选取及其轨迹质量的好坏，其算法有两个局限：第一个局限是特征点需要人为地根据经验来设计规则来选取特征点，因此可能出现有些非合作目标在某种规则下，特征点较少或者缺失的情况；第二种局限是敏感器在对非合作目标进行扫描时，因为非合作目标的自遮挡影响，所以会出现在序列点云中的连续几帧点云上特征点消失的情况，进而造成特征点轨迹的不完整的情况。第二种算法是通过相邻帧的点云进行点云配准，根据配准矩阵根据罗德里格斯公式建立方程从而解算出来转动轴方向和转动角速度，这种使用点云之间的配准矩阵来解算出来的非合作目标旋转角速度和转动轴方向，实际上是非合作目标在这段时间内的一个平均转动速度和平均转动轴方向，并不能反映某个时刻的真实运动情况。当非合作目标转动速度发生改变时，这种算法估计出来的速度会产生较大的误差。

发明内容

针对现有技术的不足，本发明提出了一种利用序列点云数据解算转轴方向与转动角速度的方法。本发明通过构建局部轨迹矩阵来描述某个时刻非合作目标的整体运动状况来解算转轴方向与转动角速度，这种方法摆脱了特征点的依赖，又有效提高了转动角速度解算的准确度。

根据本发明提供的运动旋转目标的转动轴方向与转动角速度解算方法，包含以下步骤：

步骤1，当求解t_i时刻的旋转轴与旋转角速度时，将对应的t_i时刻的点云，分别与t_i-1时刻、t_i+1时刻点云，利用最近点迭代点云配准算法得到配准矩阵M_i-1和M_i+1；配准矩阵M形式如下：

式中，R为大小为3×3的旋转变化矩阵，T为大小为1×3的平移向量；

将t_i时刻点云数据扩展为D_i＝[x_i，k y_i，k z_i，k 1]_m×4，k∈[1，m]，m为该帧点云中点总个数；根据配准公式可得D_iM_i-1＝D_i-1；对于序列点云，当求取t_i时刻到t_i+1时刻点云之间的配准矩阵时，如果i≥1，使用M_i-1得到M′_i+1作为求解M_i+1的最近点云配准算法的初始解，使用最近点迭代点云配准算法(ICP)迭代，M′_i+1的构造方式如下：