一种多特征点位置姿态冗余解算方法

摘要

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

主权项

一种多特征点位置姿态冗余解算方法，其特征在于：根据刚体变换原理，物体姿态变化时物体上的点坐标满足：P_i'＝R*P_i+t其中R为旋转矩阵，t为平移矩阵；$R=\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right]=R_z{R}_y{R}_x$$=\left[\begin{array}{ccc}\cos \beta \cos \gamma & -\cos \alpha \sin \gamma +\sin \alpha \sin \beta \cos \gamma & \sin \alpha \sin \gamma +\cos \alpha \sin \beta \cos \gamma \\ \cos \beta \sin \gamma & \cos \alpha \cos \gamma +\sin \alpha \sin \beta \sin \gamma & -\sin \alpha \cos \gamma +\cos \alpha \sin \beta \sin \gamma \\ -\sin \beta & \sin \alpha \cos \beta & \cos \alpha \cos \beta \end{array}\right]$其中α，β，γ分别为绕x，y，z轴的旋转角；$t=\left[\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right],$x_t,y_t,z_t分别为沿x，y，z轴的平移量；该方法包括如下步骤：步骤一：获取初始点坐标：获取物体上特征点的三维坐标，特征点的个数大于3个，设T₁时刻测得的N点坐标分别为：P₁(x₁，y₁,z₁)，R₂(x₂,y₂，z₂)......P_n(x_n,y_n，z_n)，T₂时刻测得的N点坐标分别为：$P_1^{\prime }(x_1^{\prime },y_1^{\prime },z_1^{\prime }),P_2^{\prime }(x_2^{\prime },y_2^{\prime },z_2^{\prime })......P_n^{\prime }(x_n^{\prime },y_n^{\prime },z_n^{\prime });$步骤二：特征点匹配：该匹配过程采用计算特征点到空间虚拟点的距离实现：设T₁时刻空间虚拟点P_c(x_c，y_。，z_c)，取分别计算N个点到P_c的距离|P₁P_c|、|P₂P_c|......|P_nP_c|;设T₂时刻空间虚拟点P_c’(x_c’,y_c’,z_c’),取${x_c}^{,}=\frac{1}{n}{\Sigma }_1^n{x_i}^{,},$${y_c}^{,}=\frac{1}{n}{\Sigma }_1^n{y_i}^{,},$${z_c}^{,}=\frac{1}{n}{\Sigma }_1^n{z_i}^{,},$分别计算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点联立后可得方程组：${\left[\begin{array}{ccc}{x_1}^{\prime }-{x_c}^{\prime }& {y_1}^{\prime }-{y_c}^{\prime }& {z_1}^{\prime }-{z_c}^{\prime }\\ ...& ...& ...\\ {x_i}^{\prime }-{x_c}^{\prime }& {y_i}^{\prime }-{y_c}^{\prime }& {z_i}^{\prime }-{z_c}^{\prime }\end{array}\right]}^T$$=\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right]{\left[\begin{array}{ccc}{x}_1-x_c& y_1-y_c& z_1-z_c\\ ...& ...& ...\\ x_i-x_c& y_i-y_c& z_i-z_c\end{array}\right]}^T$即：$L_n^T=R*L_1^T---\left(4\right)$步骤四：实现多点间的平差优化。