CN110111356B - 运动旋转物体的转动轴方向与转动角速度解算方法 - Google Patents

Abstract

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

Description

运动旋转物体的转动轴方向与转动角速度解算方法

技术领域

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

背景技术

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

目前转动轴方向及转动角速度求解算法有很多种，根据主要原理大致分为两种。第一种是通过特征点轨迹来求取，主要步骤为：(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的构造方式如下：

对于t_i时刻点云任意一点P(x，y，z)的构建其对应的扩充向量为X＝(x，y，z，1)，设其对应t_i+1时刻点云上的点的扩充向量为X_i+1(x_i+1，y_i+1，z_i+1，1)，其可以通过以下方式计算：

X_i+1＝XM_i+1

该点在t_i时刻的局部轨迹，通过某点乘以配准矩阵序列得到，设计X的扩充矩阵XR形式如下：

设计局部轨迹矩阵如下：

再将矩阵XR和矩阵TM相乘，即可求出该点位置轨迹PS：

PS第一列、第二列、第三列分别为该点x轴、y轴、z轴的位置轨迹。

分别该点x轴、y轴、z轴位置轨迹进行以时间为变量，位置为函数值的一元二次多项式函数拟合F＝f(T)，对拟合函数进行求导即可得到局部轨迹上速度与时间的变化关系v＝f′(T)，进而求出t_i时刻该点在各个方向的速度v_x、v_y、v_z。

设从t_i时刻点云中选取n个点为

其中k＝1，...，n；利用点云轨迹矩阵分别求取n个点在t_i时刻的速度

其中k＝1，...，n，得到n个点的在t_i时刻位置和各方向速度后，进行旋转参数的解算；

步骤3，旋转参数参数的求解：首先根据运动方程，构建出可以根据上述步骤中求解出来t时刻(泛指t_i时刻)位置和速度信息求解出旋转轴与旋转角速度的方程，

运动方程的构建：根据物理运动理论，刚体的运动可分解为由随质心的平动和绕通过质心的转轴的转动两部分组成，本发明将非合作目标视为刚体；设目标的t时刻(泛指t_i时刻)的质心坐标为O＝[o_x o_y o_z]^T，平动速度为V_o＝[v_ox v_oy v_oz]^T，根据刚体转动理论，其绕旋转轴的转动可以分解为绕其质心的转动，其转动角速度为Ω_t＝[ω_t，x ω_t，y ω_t，z]^T，则刚体上的任意一点P_i＝[p_ix p_iy p_iz]的在t时刻的运动速度V_i＝[v_t，i，x v_t，i，y v_t，i，z]^T都可以表示为其运动形式可以表示如下：

V_i＝V_o+Ω_t×OP_i (1)

式中，OP_i为P_i点到质心O的距离；

将(1)式展开为矩阵形式：

由计算式(2)得到

等式(3)右侧的结果只与目标转动角速度、目标质心坐标和质心平动速度有关，与选取点云上的点无关，也就是说对于某个时刻的点云而言，等式最右侧为一个常数向量，将其用C＝[c_x c_y c_z]^T表示，则上式变为：

对(4)进行整理得：

上等式中，共有6个未知参数ω_x、ω_y、ω_z、c_x、c_y、c_z，其中，ω_t，x，ω_t，y，ω_t，z即为运动旋转物体t时刻x轴，y轴以及z轴的分角速度，c_x、c_y、c_z均为常数；对于任意一个特征点都可以建立上述矩阵等式，可得到三个方程；在同一帧点云中选取两个及多于两点就可以解出这6个未知数，对同一时刻同一点云的n个点进行联立，可得下面方程式：

即Y＝Hθ，对参数进行最小二乘法求解，最小二乘解为：

求解出来的解

形式如下：

式中，ω_t，x，ω_t，y，ω_t，z即为运动旋转物体t时刻各个轴的分角速度；

将步骤2中求取的任意n个点在t时刻求出的位置和速度代入步骤3中的(6)式，得到目标在t时刻的旋转角速度为

此处t时刻泛指步骤1中的t₁时刻、t₂时刻、t₃时刻......中的任意时刻，旋转轴指向由ω_x、ω_y、ω_z根据矢量相加规则确定。

与现有技术相比，本发明至少具有以下有益效果：

1、本发明所述解算方法摆脱了对特征点的依赖，通过使用局部轨迹矩阵来描述目标在某个时刻的运动状况；对由于目标自遮挡而造成点云局部缺失有很好的鲁棒性，避免了因序列点云中特征点轨迹不完整而造成的误差；

2、本发明综合考虑点云的整体配准情况，因为局部轨迹矩阵是由相邻帧点云整体配准而得到的，所以通过使用局部轨迹矩阵来产生的某点的轨迹，不仅仅反映了某个点的运动轨迹，也包含了目标整体的在该时刻的运动情况，避免了因为单个点轨迹误差造成的结果计算误差；

3、相比于使用单个配准矩阵解算，本发明在描述某个点某个时刻的运动情况时既考虑的该时刻之前该点的运动情况又考虑了该时刻之后该点的运动情况，因此拟合出来的该点的运动速度更加准确。

附图说明

图1为本发明流程示意图；

图2为本发明的局部轨迹矩阵求解解算框图；

图3为本发明实施例中非合作目标模型；

图4为本发明实施例中敏感器对非合作目标的扫描点云；

图5为本发明实施例中本发明与根据罗德里格斯公式建立方程解算转动角速度的对比；

图6为本发明实施例中本发明与根据罗德里格斯公式建立方程解算转轴方向的对比。

具体实施方式

下面结合附图和具体实施例，对本发明进行详细阐述。

如图1所示，本发明所述的一种运动旋转目标的转动轴方向与转动角速度解算方法，该方法的具体步骤如下：

对于获取到的序列点云，按照时间顺序命名为t₀时刻点云，t₁时刻点云，t₂时刻点云，t₃时刻点云，......；由于在t₀时刻之前没有转动信息，因此无法对该时刻的转动参数进行准确估计，本发明估计从t₁时刻以及之后时刻目标对应的转轴方向和转动角速度。

步骤1，构建t_i时刻的局部轨迹矩阵：

步骤11，获取配准矩阵M_i-1和M_i+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的最近点云配准算法的初始解进行迭代，M′_t+1的构造方式如下：

步骤12，构建局部轨迹矩阵：使用配准矩阵M_i-1和M_i+1构造局部轨迹矩阵TM，矩阵TM代表t_i时刻整体点云的运动信息，具体形式为：

其中，I_4×4为大小为4的单位矩阵；

步骤2，获取多个点的轨迹，对于每个点进行如下操作：

对于t_i时刻点云任意一点P(x，y，z)的构建其对应的扩充向量为X＝(x，y，z，1)，设其对应t_i+1时刻点云上的点的扩充向量为X_i+1(x_i+1，y_i+1，z_i+1，1)，其可以通过以下方式计算：

X_i+1＝XM_i+1

该点在t_i时刻的局部轨迹，通过某点乘以配准矩阵序列得到，设计X的扩充矩阵XR形式如下：

步骤21，选取t_i时刻点云上的某点P(x，y，z)的扩充矩阵为X＝(x，y，z，1)，设计X的扩充矩阵XR如下

则t_i-1、t_i、t_i+1三个时刻的这个点位置轨迹PS求解过程如下：

PS第一列、第二列和第三列分别为该点x轴、y轴和z轴的位置变化。

步骤22，分别该点对x轴、y轴和z轴三个方向的位置变化进行以时间为变量，位置为函数值的轨迹进行一元二次多项式函数拟合，得到轨迹函数F＝f(T)，对轨迹函数进行求导即可得到局部轨迹上速度与时间的变化关系v＝f′(T)，进而求出t_i时刻该点在各个方向的速度v_x、v_y和v_z。

设从t_i时刻点云中选取n个点为

其中k＝1，...，n；利用点云轨迹矩阵分别求取n个点在t_i时刻的速度

其中k＝1，...，n，得到n个点的在t_i时刻位置和各方向速度后，进行旋转参数的解算；

步骤3，旋转轴方向与转动角速度解算：

运动方程的构建：根据物理运动理论可知，刚体的运动可分解为由随质心的平动和绕通过质心的转轴的转动两部分组成，本发明将非合作目标视为刚体；设目标的t时刻(泛指t_i时刻)的质心坐标为O＝[o_x o_y o_z]^T，平动速度为V_o＝[v_ox v_oy v_oz]^T，根据刚体转动理论，其绕旋转轴的转动可以分解为绕其质心的转动，设其转动角速度为Ω_t＝[ω_t，x ω_t，yω_t，z]^T。则刚体上的任意一点P_i＝[p_ix p_iy p_iz]的在t时刻的运动速度V_i＝[v_t，i，x v_t，i，yv_t，i，z]^T都可以表示为其运动形式可以表示如下形式：

V_i＝V_o+Ω_t×OP_i (1)

式中，OP_i为P_i点到质心O的距离；

将(1)式展开为矩阵形式为：

对(2)式进行整理，可得到

等式(3)右侧的结果只与目标转动角速度、目标质心坐标和质心平动速度有关，与选取点云上的点无关，也就是说对于某个时刻的一幅点云而言，等式右侧为一个常数向量，将其用C＝[c_x c_y c_z]^T表示，则上式变为：

对(4)进行整理得：

上等式中，共有6个未知参数ω_x、ω_y、ω_z、c_x、c_y、c_z，对于任意一个特征点都可以建立上述矩阵等式，可得到三个方程；在同一帧点云中选取两个及多于两点就可以解出这6个未知数，对同一时刻同一点云的n个点进行联立，可得下面方程式：

即Y＝Hθ，对参数进行最小二乘法求解，最小二乘解为：

求解出来的解

形式如下：

式中，ω_t，x，ω_t，y，ω_t，z即为运动旋转物体t时刻各个轴的分角速度；

将步骤2中求取的任意n个点在t时刻求出的位置和速度代入步骤3中的(6)式，得到目标在t时刻的旋转角速度为

此处t时刻泛指步骤1中的t₁时刻、t₂时刻、t₃时刻......中的任意时刻。旋转轴指向由ω_x、ω_y、ω_z根据矢量相加规则确定。

从t时刻(此处泛指t_i时刻)点云中，选取任意n个点其位置为p_k(x_t，k，y_t，k，z_t，k)，其中k＝1，...，n，重复步骤2利用点云局部轨迹矩阵分别求取n个点在t时刻的速度v_t，k，x、v_t，k，y、v_t，k，z；在本实施例中，选取n＝20，利用n个点位置信息构造位置相关信息矩阵H，n个点速度信息构建速度信息向量Y。

位置相关信息矩阵H具体形式如下：

速度信息向量Y具体形式如下：

步骤32，将步骤31所得位置相关信息矩阵H、构造速度信息向量Y带入到分角速度的求解公式即可求出分角速度，分角速度的求解公式具体为：

求解出解

形式如下：

式中，ω_t，x，ω_t，y，ω_t，z即为运动旋转目标t时刻各个轴的分角速度；旋转角速度为

转动轴旋转方向由ω_t，x、ω_t，y、ω_t，z根据矢量相加规则确定。

实施例：

以下将使用本发明所提出的方法与使用罗德里格斯公式(Rodrigues’s Formula)通过单个配准矩阵解算旋转与转动角速度方法(下文简称“罗氏方法”)进行对比，验证本发明所提出的方法的有效性及优越性；为更好地对比两种算法，在对比的过程中使用同一模型采样得到的同一点云序列，并且两种对比算法使用得到的同一配准矩阵，只有使用配准矩阵的后续步骤不同。

对图4所示模型进行模拟敏感器进行扫描采样，在采样过程中，模型绕旋转轴进行变速运动；模拟敏感器产生序列点云，点云效果如图4所示；对得到的序列点云使用本发明所提出的方法与罗氏方法在旋转轴方向与转动角速度进行解算效果对比。

图5为本发明所提出算法与罗氏算法得到转动角速度解算效果的对比实验结果，图5中左侧配准矩阵解算方法即使用t_i时刻点云与t_i-1点云(序列点云中的上一帧点云)配准得到的配准矩阵使用罗氏方法解算，右侧配准矩阵解算方法及使用t_i时刻点云与t_i+1点云(序列点云中的下一帧点云)配准得到的配准矩阵使用罗氏方法解算，可以看出罗氏算法当进行匀速转动时，效果较好，但当速度变化时，使用左侧配准矩阵的罗氏算法会出现“滞后”情况，使用右侧配准矩阵时罗氏算法会出现“超前”的情况，本发明所提出算法无论是在变速情况还是匀速转动情况，都能很好估计出转动角速度。

图6为本发明所提出的算法与罗氏算法(包括左侧配准矩阵和右侧配准矩阵)对转轴方向误差的对比实验效果。当转轴固定不变时，两种算法都能很好地估计转轴方向。本发明提出的算法误差较为稳定，而无论是使用左侧配准矩阵还是右侧配准矩阵误差都会有较大波动。

上述实施方法用于对原理进行说明，方便该领域人员对于本发明进行理解，需要指出的是，本发明并不只是仅仅局限于此，本领域人员可在权利要求范围对本发明进行更改；本发明未详细阐述的算法为本领域内公知常识。

Claims (9)

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的单位矩阵。

5.根据权利要求4所述的运动旋转物体的转动轴方向与转动角速度解算方法，其特征在于，步骤2中点轨迹拟合与速度求取的具体步骤为：

首先，选取t_i时刻点云上的任意一点P(x,y,z)，其扩充向量为X＝(x,y,z,1)，对应t_i+1时刻点云中该点的扩充向量为X_i+1(x_i+1,y_i+1,z_i+1,1)，其可以通过以下方式计算：

X_i+1＝XM_i+1

该点在t_i时刻的局部轨迹，通过该点乘以配准矩阵序列得到，该点的扩充矩阵XR形式如下：

则t_i-1、t_i、t_i+1三个时刻的该点位置轨迹PS具体形式如下：

PS中第一列、第二列和第三列分别对应为该点x轴、y轴和z轴的位置变化；

然后，分别对x轴、y轴以及z轴三个方向的位置进行以时间为变量，位置为函数的一元二次多项式函数拟合，得到轨迹函数F＝f(T)，对拟合得到轨迹函数进行求导，即可得到局部轨迹上速度与时间的变化关系v＝f′(T)，进而求出t_i时刻该点在各个方向的速度v_x、v_y和v_z。

6.根据权利要求1所述的运动旋转物体的转动轴方向与转动角速度解算方法，其特征在于，步骤3中，对点云中任意点所构建的运动方程为Y＝Hθ，其中，H为位置相关信息矩阵，Y为速度信息向量，θ为需要求解的未知参数，具体的形式如下：

其中，

即为运动旋转物体t_i时刻x轴，y轴以及z轴的分角速度，c_x、c_y、c_z均为常数；刚体上的任意一点P_i＝[p_ix p_iy p_iz]。

7.根据权利要求6所述的运动旋转物体的转动轴方向与转动角速度解算方法，其特征在于，步骤3中，运动方程为Y＝Hθ的最小二乘解为：

其形式如下：

其中，

即为运动旋转物体t_i时刻各个轴的分角速度，c_x、c_y、c_z均为常数；旋转角速度为

转动轴旋转方向由

根据矢量相加规则求解。

8.根据权利要求1所述的运动旋转物体的转动轴方向与转动角速度解算方法，其特征在于，步骤3中的n的取值范围为[2,+∞)。

9.根据权利要求8所述的运动旋转物体的转动轴方向与转动角速度解算方法，其特征在于，n＝20。

Images