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

说明书

技术领域

本发明属于物体位置姿态测量技术领域，具体涉及一种多特征点位置姿态冗余解算方法。

背景技术

随着航空航天领域的迅猛发展，对飞行器运动过程位置姿态测量的要求已经越来越广泛，如航天器对接、级间分离姿态的测量等等。目前，主流方法是通过立体摄影测量技术实现对飞行器上特征点的三维坐标动态测量，进而通过特征点的三维坐标反推物体运动的六自由度姿态参数，这个过程即为姿态解算。

传统解算方法，按照特征点在运动前后两个坐标系中的几何关系建立方程求取姿态的闭式解或通过优化方法求取数值解。

根据刚体运动原理，只需三个特征点即可解算物体的姿态参数，但由于该方法的求解方程建立在严格准确的几何约束基础上，对噪声干扰的敏感性高，一般采用增加特征点的方式，提高数值解对噪声干扰的免疫力。但特征点数目的增加又给不同时刻间的特征点匹配带来了困难，极易造成解算结果的错误；同时，使用数目较多的特征点来进行迭代计算，严重增加了计算耗时，限制了该方法的应用范围。

发明内容

本发明的目的在于提供一种求解物体六自由度位置姿态的冗余解算方法，此方法根据刚体运动学原理利用物体上多个特征点坐标作为输入条件，通过特征点匹配、去重心化、多点平差等步骤，实现对物体位置姿态的快速精确求解。

为达到上述目的，本发明所采取的技术方案为：

一种多特征点位置姿态冗余解算方法，根据刚体变换原理，物体姿态变化时物体上的点坐标满足：

P_i’＝R*P_i+t

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

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

该方法包括如下步骤：

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

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）得：