[发明专利]一种多特征点位置姿态冗余解算方法

权利要求书

1.一种多特征点位置姿态冗余解算方法，其特征在于：

根据刚体变换原理，物体姿态变化时物体上的点坐标满足：

P_i'＝R*P_i+t

其中R为旋转矩阵，t为平移矩阵；

其中α，β，γ分别为绕x，y，z轴的旋转角；x_t,y_t,z_t分别为沿x，y，z轴的平移量；

该方法包括如下步骤：

步骤一：获取初始点坐标：获取物体上特征点的三维坐标，特征点的个数大于3个，设T₁时刻测得的N点坐标分别为：

P₁(x₁，y₁,z₁)，P₂(x₂,y₂，z₂)......P_n(x_n,y_n，z_n)，

T₂时刻测得的N点坐标分别为：

步骤二：特征点匹配：该匹配过程采用计算特征点到空间虚拟点的距离实现：设T₁时刻空间虚拟点P_c(x_c，y_c，z_c)，取分别计算N个点到P_c的距离|P₁P_c|、|P₂P_c|......|P_nP_c|;设T₂时刻空间虚拟点P_c’(x_c’,y_c’,z_c’),取分别计算N个点到P_c'的距离|P₁’P_c’｜、｜P₂’P_c’|......|P_n’P_c’|;将|P₁’P_c’|与|P₁P_c|、 |P₂P_c|......|P_nP_c|分别进行比较，距离相同或相差小于误差限的点视为匹配点对，若无匹配点对，则舍弃该特征点；将匹配好的特征点按照统一的顺序进行排列，后续计算过程依次读取即可；

步骤三：去重心化：物体姿态变化时物体上的点坐标满足：

P_i'＝R*P_i+t(1)

上式对上述虚拟点亦成立，则：

P_c’＝R*P_c+t(2)

式（1）减去式（2）得：

P_i’-P_c’＝R*(P_i-P_c)(3)

N点联立后可得方程组：

即：

步骤四：实现多点间的平差优化。

2.根据权利要求1所述的多特征点位置姿态冗余解算方法，其特征在于：所述步骤四采用奇异值分解方法求解方程（4）得到旋转矩阵R，在矩阵病态的情况下，求取最小二乘解中存在的唯一最小范数解，实现多点间的平差优化。

3.根据权利要求2所述的多特征点位置姿态冗余解算方法，其特征在于： 所述奇异值分解具体方法为：将矩阵奇异值分解，则R＝U*V^T，U、V为奇异值分解后的两正交矩阵，由式（1）求得平移矩阵t。

4.根据权利要求1所述的多特征点位置姿态冗余解算方法，其特征在于：由旋转矩阵R求解旋转角的过程通过非线性方程的优化求解实现。