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

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

背景技术：

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

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

技术实现要素：

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

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

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

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

将ti时刻点云数据扩展为di＝[xi,kyi,kzi,k1]m×4，k∈[1,m]，m为该帧点云中点总个数；根据配准公式可得dimi-1＝di-1；对于序列点云，当求取ti时刻到ti+1时刻点云之间的配准矩阵时，如果i≥1，使用mi-1得到m′i+1作为求解mi+1的最近点云配准算法的初始解，使用最近点迭代点云配准算法(icp)迭代，m′i+1的构造方式如下：

对于ti时刻点云任意一点p(x,y,z)的构建其对应的扩充向量为x＝(x,y,z,1)，设其对应ti+1时刻点云上的点的扩充向量为xi+1(xi+1,yi+1,zi+1,1)，其可以通过以下方式计算：

xi+1＝xmi+1

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

设计局部轨迹矩阵如下：

再将矩阵xr和矩阵tm相乘，即可求出该点位置轨迹ps：

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

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

设从ti时刻点云中选取n个点为其中k＝1,...,n；利用点云轨迹矩阵分别求取n个点在ti时刻的速度其中k＝1,...,n，得到n个点的在ti时刻位置和各方向速度后，进行旋转参数的解算；

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

运动方程的构建：根据物理运动理论，刚体的运动可分解为由随质心的平动和绕通过质心的转轴的转动两部分组成，本发明将非合作目标视为刚体；设目标的t时刻(泛指ti时刻)的质心坐标为o＝[oxoyoz]^t，平动速度为vo＝[voxvoyvoz]^t，根据刚体转动理论，其绕旋转轴的转动可以分解为绕其质心的转动，其转动角速度为ωt＝[ωt,xωt,yωt,z]^t，则刚体上的任意一点pi＝[pixpiypiz]的在t时刻的运动速度vi＝[vt,i,xvt,i,yvt,i,z]^t都可以表示为其运动形式可以表示如下：

vi＝vo+ωt×opi(1)

式中，opi为pi点到质心o的距离；

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

由计算式(2)得到

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

对(4)进行整理得：

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

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

求解出来的解形式如下：

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

将步骤2中求取的任意n个点在t时刻求出的位置和速度代入步骤3中的(6)式，得到目标在t时刻的旋转角速度为此处t时刻泛指步骤1中的t1时刻、t2时刻、t3时刻……中的任意时刻，旋转轴指向由ωx、ωy、ωz根据矢量相加规则确定。

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

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

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

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

附图说明

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

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

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

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

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

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

具体实施方式

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

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

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

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

步骤11，获取配准矩阵mi-1和mi+1：对待求解ti时刻的旋转轴与旋转角速度，结合对应的ti时刻的点云分别与ti-1时刻和ti+1时刻对应点云，利用最近点迭代点云配准算法，得到配准矩阵mi-1和mi+1；配准矩阵m形式如下：

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

将ti时刻点云数据扩展为di＝[xi,kyi,kzi,k1]m×4，k∈[1,m]，m为该帧点云中点总个数。根据配准公式可得dimi-1＝di-1；对于序列点云，当求取ti时刻到ti+1时刻点云之间的解配准矩阵时，如果i≥1，使用mi-1得到m′i+1作为求解mi+1的最近点云配准算法的初始解进行迭代，m′t+1的构造方式如下：

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

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

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

对于ti时刻点云任意一点p(x,y,z)的构建其对应的扩充向量为x＝(x,y,z,1)，设其对应ti+1时刻点云上的点的扩充向量为xi+1(xi+1,yi+1,zi+1,1)，其可以通过以下方式计算：

xi+1＝xmi+1

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

步骤21，选取ti时刻点云上的某点p(x,y,z)的扩充矩阵为x＝(x,y,z,1)，设计x的扩充矩阵xr如下

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

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

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

设从ti时刻点云中选取n个点为其中k＝1,...,n；利用点云轨迹矩阵分别求取n个点在ti时刻的速度其中k＝1,...,n，得到n个点的在ti时刻位置和各方向速度后，进行旋转参数的解算；

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

运动方程的构建：根据物理运动理论可知，刚体的运动可分解为由随质心的平动和绕通过质心的转轴的转动两部分组成，本发明将非合作目标视为刚体；设目标的t时刻(泛指ti时刻)的质心坐标为o＝[oxoyoz]^t，平动速度为vo＝[voxvoyvoz]^t，根据刚体转动理论，其绕旋转轴的转动可以分解为绕其质心的转动，设其转动角速度为ωt＝[ωt,xωt,yωt,z]^t。则刚体上的任意一点pi＝[pixpiypiz]的在t时刻的运动速度vi＝[vt,i,xvt,i,yvt,i,z]^t都可以表示为其运动形式可以表示如下形式：

vi＝vo+ωt×opi(1)

式中，opi为pi点到质心o的距离；

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

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

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

对(4)进行整理得：

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

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

求解出来的解形式如下：

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

将步骤2中求取的任意n个点在t时刻求出的位置和速度代入步骤3中的(6)式，得到目标在t时刻的旋转角速度为此处t时刻泛指步骤1中的t1时刻、t2时刻、t3时刻……中的任意时刻。旋转轴指向由ωx、ωy、ωz根据矢量相加规则确定。

从t时刻(此处泛指ti时刻)点云中，选取任意n个点其位置为pk(xt，k，yt，k，zt，k)，其中k＝1,...,n，重复步骤2利用点云局部轨迹矩阵分别求取n个点在t时刻的速度vt,k,x、vt,k,y、vt,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’sformula)通过单个配准矩阵解算旋转与转动角速度方法(下文简称“罗氏方法”)进行对比，验证本发明所提出的方法的有效性及优越性；为更好地对比两种算法，在对比的过程中使用同一模型采样得到的同一点云序列，并且两种对比算法使用得到的同一配准矩阵，只有使用配准矩阵的后续步骤不同。

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

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

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

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