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

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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 \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\α}\sin \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}\\ \cos \mathrm{\β}\sin \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}\\ -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\β}\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₁时刻测得的Ｎ点坐标分别为：

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

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

$P_1^{\′}(x_1^{\′},y_1^{\′},z_1^{\′}),P_2^{\′}(x_2^{\′},y_2^{\′},z_2^{\′})......P_n^{\′}(x_n^{\′},y_n^{\′},z_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’),取 ${x_c}^{,}=\frac{1}{n}{\mathrm{\Σ}}_1^n{x_i}^{,},$ ${y_c}^{,}=\frac{1}{n}{\mathrm{\Σ}}_1^n{y_i}^{,},$ ${z_c}^{,}=\frac{1}{n}{\mathrm{\Σ}}_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}^{\′}-{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$

$=\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)$

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

所述步骤四采用奇异值分解方法求解方程（4）得到旋转矩阵R，在矩阵病态的情况下，求取最小二乘解中存在的唯一最小范数解，实现多点间的平差优化。

所述奇异值分解具体方法为：将矩阵奇异值分解，则R＝U*V^T，U、V为奇异值分解后的两正交矩阵，由式（1）求得平移矩阵t。

由旋转矩阵R求解旋转角的过程通过非线性方程的优化求解实现。

本发明所取得的有益效果为：

本发明所述多特征点位置姿态冗余解算方法以特征点在视觉测量系统中的三维坐标为输入条件，获取数据后通过计算特征点到空间虚拟点的距离寻找匹配点对，若所有特征点均能找到匹配点对，则直接进行去重心化操作；若无法找到匹配点对，则该点被自动舍弃，记录匹配点对个数，动态调整后续算法数据入口大小，由剩余点解算该时刻的姿态位置信息；完成匹配后，通过去重心化实现平移信息和旋转信息的分离，单独解算旋转矩阵；为了消除病态矩阵的影响，求解优化最小二乘解，采用了SVD方法，进而通过非线性方程的优化平差算法求出六自由度位置姿态信息；至此，算法完成了一个解算周期，通过该周期的反复循环运算可以实现位姿信息的连续动态实时输出。本发明解决了物体空间位置姿态测量的六自由度解算问题，提高算法精度的同时使算法保持较高的实时性能。实验结果显示，当采用的特征光点达到5个时，该姿态测量算法的均方根误差保持在0.05°以内，而刷新速度达到200帧/秒。且数据采集过程的局部测量点丢失不影响解算功能，鲁棒性好，数据准确可靠。

附图说明

图1为本发明所述多特征点位置姿态冗余解算方法流程图。

具体实施方式

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

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

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 \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\α}\sin \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}\\ \cos \mathrm{\β}\sin \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}\\ -\sin \mathrm{\β}& \sin \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\β}\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轴的平移量。

通过立体视觉技术获取三个以上特征点的坐标，利用上述几何关系建立方程即可求解旋转矩阵及平移矩阵，实现姿态解算。

如图1所示，本发明所述多特征点位置姿态冗余解算方法包括如下步骤：

步骤一：获取初始点坐标：通过立体视觉技术获取物体上特征点的三维坐标，要求特征点的个数大于3个，以下均以N个特征点为例介绍技术方案，设T₁时刻测得的N点坐标分别为：P₁(x₁，y₁，z₁)，P₂(x₂，y₂，z₂)……R_n(x_n，y_n，z_n)，T₂时刻测得的N点坐标分别为： $P_1^{\′}(x_1^{\′},y_1^{\′},z_1^{\′}),P_2^{\′}(x_2^{\′},y_2^{\′},z_2^{\′})......P_n^{\′}(x_n^{\′},y_n^{\′},z_n^{\′});$

步骤二：特征点匹配：立体视觉设备输出的点坐标顺序是按照坐标顺序排序的，物体姿态发生变化后，会导致不同时刻输出点坐标顺序不一致，如T₁时刻为P_l,P₂,P₃......P_n,T₂时刻为则需要利用特征点间的空间约束关系，匹配不同时刻的特征点排序。该匹配过程采用计算特征点到空间虚拟点的距离实现，首先设T₁时刻空间虚拟点P_c(x_c,y_c，z_c)，取 分别计算N个点到P_c的距离|P₁P_c|、|P₂P_c|……|P_nP_c|;同理，设T₂时刻空间虚拟点Ｐ_c’(x_c’,y_c’,z_c’),取 ${x_c}^{,}=\frac{1}{n}{\mathrm{\&Sigma;}}_1^n{x_i}^{,},$ ${y_c}^{,}=\frac{1}{n}{\mathrm{\&Sigma;}}_1^n{y_i}^{,},$ ${z_c}^{,}=\frac{1}{n}{\mathrm{\&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|分别进行比较，距离相同（或相差小于误差限）的点视为匹配点对，例如-0.01<|P₁'P_c’|-|P₁P_c<0.01,则判断P₁与P′₁为匹配点对，若无匹配点对，则证明立体视觉测量特征点有误，舍弃该特征点。以此方法依次判断所有特征点间距，实现特征点匹配。该算法步骤的设计提高了系统的鲁棒性，在特征点采集过程中局部测量点丢失和错乱不会影响解算功能。将匹配好的特征点按照统一的顺序进行排列，后续计算过程依次读取即可。

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

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)

式（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)$

步骤四：平差优化：若直接用最小二乘法优化则会因为矩阵奇异，其求取结果将不正确，而直接采用优化正交迭代的方法耗时较高。本文采用奇异值分解（SVD）方法求解方程（4）得到旋转矩阵R，在矩阵病态的情况下，求取最小二乘解中存在的唯一最小范数解，实现多点间的平差优化。将矩阵奇异值分解，则R=U*V^T（U,V为奇异值分解后的两正交矩阵），由式（1）求得平移矩阵t。由旋转矩阵求解旋转角的过程可以通过非线性方程的优化求解实现，进一步优化坐标值的平差效果。

Claims (4)

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

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

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 \mathrm{\&beta;}\cos \mathrm{\&gamma;}& -\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}& \sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}\\ \cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}\\ -\sin \mathrm{\&beta;}& \sin \mathrm{\&alpha;}\cos \mathrm{\&beta;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&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}{\mathrm{\&Sigma;}}_1^n{x_i}^{,},$ ${y_c}^{,}=\frac{1}{n}{\mathrm{\&Sigma;}}_1^n{y_i}^{,},$ ${z_c}^{,}=\frac{1}{n}{\mathrm{\&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)$

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

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

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

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