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

摘要

本发明属于物体位置姿态测量技术领域，具体涉及一种多特征点位置姿态冗余解算方法。方法以特征点在视觉测量系统中的三维坐标为输入条件，获取数据后通过计算特征点到空间虚拟点的距离寻找匹配点对，若所有特征点均能找到匹配点对，则直接进行去重心化操作；若无法找到匹配点对，则该点被自动舍弃，记录匹配点对个数，动态调整后续算法数据入口大小，由剩余点解算该时刻的姿态位置信息；完成匹配后，通过去重心化实现平移信息和旋转信息的分离，单独解算旋转矩阵。本发明解决了物体空间位置姿态测量的六自由度解算问题，提高算法精度的同时使算法保持较高的实时性能。

主权项

一种多特征点位置姿态冗余解算方法，其特征在于：根据刚体变换原理，物体姿态变化时物体上的点坐标满足：Pi'＝R*Pi+t其中R为旋转矩阵，t为平移矩阵；其中α，β，γ分别为绕x，y，z轴的旋转角；xt,yt,zt分别为沿x，y，z轴的平移量；该方法包括如下步骤：步骤一：获取初始点坐标：获取物体上特征点的三维坐标，特征点的个数大于3个，设T1时刻测得的N点坐标分别为：P1(x1，y1,z1)，P2(x2,y2，z2)......Pn(xn,yn，zn)，T2时刻测得的N点坐标分别为：步骤二：特征点匹配：该匹配过程采用计算特征点到空间虚拟点的距离实现：设T1时刻空间虚拟点Pc(xc，yc，zc)，取分别计算N个点到Pc的距离|P1Pc|、|P2Pc|......|PnPc|;设T2时刻空间虚拟点Pc’(xc’,yc’,zc’),取分别计算N个点到Pc'的距离|P1’Pc’｜、｜P2’Pc’|......|Pn’Pc’|;将|P1’Pc’|与|P1Pc|、 |P2Pc|......|PnPc|分别进行比较，距离相同或相差小于误差限的点视为匹配点对，若无匹配点对，则舍弃该特征点；将匹配好的特征点按照统一的顺序进行排列，后续计算过程依次读取即可；步骤三：去重心化：物体姿态变化时物体上的点坐标满足：Pi'＝R*Pi+t  (1)上式对上述虚拟点亦成立，则：Pc’＝R*Pc+t  (2)式（1）减去式（2）得：Pi’‑Pc’＝R*(Pi‑Pc)  (3)N点联立后可得方程组：即：步骤四：实现多点间的平差优化。