CN102032871A - 一种基于特征线的运动目标位姿光学测量方法 - Google Patents

Abstract

一种基于特征线的运动目标位姿光学测量方法。它涉及视觉测量中的一种运动目标位姿测量技术，它解决了传统的监测系统测量精度低和稳定性差，以及对于特征点装配位置要求高的缺点。利用目标上两条不平行的直线的光学测量方法。提出了基于直线的位置姿态求取公式。使用该发明方法对位姿测量误差进行了仿真分析，同时给出了实际的测量误差。该发明方法对摄像机模型没有限制，且由于增加了带测量的测量基线，所以减少了位置姿态参数的测量精度对光学系统的依赖，实验结果表明，该方法可满足运动目标相对位置姿态的高精度测量的需求。

Description

一种基于特征线的运动目标位姿光学测量方法

技术领域

本发明涉及视觉测量中的一种运动目标位姿测量技术，具体涉及一种基于特征线的位姿光学测量方法。

背景技术

传统的监测系统一般采用在运动目标上直接安装特征点，通过对特征点进行拍摄，处理其图像信息获得飞行器运动参数的方法，此种方法特征点的布局直接影响测量精度，具体而言，特征点间的测量基线越长，相同特征点定位误差条件下测量精度越高；反之测量精度低。按照现有的方法和技术，为了提高测量精度必须增加特征点间的距离，但特征点间的距离受限于运动目标的尺寸，同时特征点距离的增加导致配合目标重量的增加。而已有的传统方法每个特征点至少需要两台摄像机。当采用目标上特征点对目标运动参数进行测量时，其测量精度不仅与特征点在两个坐标系中的坐标测量精度有关，而且受特征点间的相对位置的影响。

发明内容

本发明为了解决传统的监测系统测量精度低和稳定性差，以及对于特征点装配位置要求高的缺点，而提出了一种基于特征线的运动目标位姿光学测量方法。

本发明的一种基于特征线的运动目标位姿光学测量方法的测量过程如下：

在运动目标上安装特征点，首次设运动目标上存在不平行的两条直线L₁和L₂，P₁、P₂、P₃和P₄分别为直线上的点，其中点P₁和P₃位于直线L₁上，点P₂和P₄位于直线L₂上，所述的P₁、P₂、P₃和P₄四个点为特征点，定义所述四个特征点在目标坐标系和测量坐标系中的坐标分别为P_mi，P_gi，其中i＝1，2，3，4；

目标坐标系与运动目标固连，其原点O_m取在运动目标的质心上，则P_mi的值为定值；

测量坐标系随运动目标的运动而变化，测量坐标系是确定运动目标的质心在空间的坐标位置及其在空间的姿态的参考基准，其中空间的坐标位置是三个方向的平动：X方向位置、Y方向位置和Z方向位置，空间的姿态是绕三个方向轴的转动：俯仰角、偏航角和滚转角，则P_gi的值是测量值，是未知值；

在任意时刻，同一特征点在目标坐标系和测量坐标系中的坐标均满足如下关系：

P_gi＝C_gmP_mi+T_mgo    i＝1，2，3，4                   公式一

式中C_gm——目标坐标系相对于测量坐标系的旋转矩阵C_mg的逆；

T_mgo——目标坐标系原点在测量坐标系中的位置向量；

把公式一中i＝1和i＝3时的等式两边分别作差，再把公式一中i＝2和i＝4时的等式两边也分别作差，可得：

P_g(j+2)-P_gj＝C_gm(P_m(j+2)-P_mj)  j＝1，2                公式二

对其两端向量进行归一化，如下式所示：

$\frac{P_{g(j+2)}-P_{\mathrm{gj}}}{|P_{g(j+2)}-P_{\mathrm{gj}}|}=\frac{C_{\mathrm{gm}}(P_{m(j+2)}-P_{\mathrm{mj}})}{\left|C_{\mathrm{gm}}(P_{m(j+2)}-P_{\mathrm{mj}})\right|}$ j＝1，2

公式三

又因为旋转矩阵C_gm为正交阵，所以公式三可进一步表示为：

$\frac{P_{g(j+2)}-P_{\mathrm{gj}}}{|P_{g(j+2)}-P_{\mathrm{gj}}|}=C_{\mathrm{gm}}\frac{P_{m(j+2)}-P_{\mathrm{mj}}}{|P_{m(j+2)}-P_{\mathrm{mj}}|}$ j＝1，2

公式四

定义

$\left\{\begin{array}{c}{A}_{\mathrm{gj}}\stackrel{\mathrm{\Δ}}{=}\frac{P_{g(j+2)}-P_{\mathrm{gj}}}{|P_{g(j+2)}-P_{\mathrm{gj}}|}\\ A_{\mathrm{mj}}\stackrel{\mathrm{\Δ}}{=}\frac{P_{m(j+2)}-P_{\mathrm{mj}}}{|P_{m(j+2)}-P_{\mathrm{mj}}|}\end{array}\right.$ j＝1，2

公式五

则，公式四更简洁、直观地表示为：

A_gj＝C_gmA_mj       j＝1，2              公式六

因为旋转矩阵C_gm是正交阵，所以对于向量A_g1和A_g2，如下关系也成立：

A_g1×A_g2＝(C_gmA_m1)×(C_gmA_m2)＝C_gm(A_m1×A_m2)            公式七

依据四个特征点不共线的条件，可知公式七两边均为非零向量，对公式七两端进行归一化：

$\frac{A_{g1}\×A_{g2}}{|A_{g1}\×A_{g2}|}=C_{\mathrm{gm}}\frac{A_{m1}\×A_{m2}}{|A_{m1}\×A_{m2}|}$ 公式八

同理定义

$\left\{\begin{array}{c}{B}_g\stackrel{\mathrm{\Δ}}{=}\frac{A_{g1}\×A_{g2}}{|A_{g1}\×A_{g2}|}\\ B_m\stackrel{\mathrm{\Δ}}{=}\frac{A_{m1}\×A_{m2}}{|A_{m1}\×A_{m2}|}\end{array}\right.$ 公式九

把B_g和B_m代入公式八，可得：

B_g＝C_gmB_m                      公式十

把公式六和公式十两端的向量组合成矩阵，并定义

$\left\{\begin{array}{c}{D}_g\stackrel{\mathrm{\Δ}}{=}(A_{g1},A_{g2},B_g)\\ D_m\stackrel{\mathrm{\Δ}}{=}(A_{m1},A_{m2},B_m)\end{array}\right.$

则公式六和公式十可用一个矩阵等式表示为：

D_g＝C_gmD_m                     公式十一

由D_g和D_m的构造方法，以及四个特征点不重合且不共线的条件可知：D_g和D_m均满秩，因此，可以通过公式十二求取目标坐标系相对于测量坐标系的旋转矩阵C_mg：

$C_{\mathrm{mg}}=C_{\mathrm{gm}}^{-1}={\left(D_g{D}_m^{-1}\right)}^{-1}$ 公式十二

分别求取P₁和P₃所在直线L₁与P₂和P₄所在直线L₂的交点或公垂线的中点在目标坐标系中的坐标Xm和在测量坐标系中的坐标Xg；把Cgm、Xm和Xg代入公式十三，就可以求得目标坐标系相对于测量坐标系的位置向量T_mgo，用显式可表示为：

T_mgo＝X_g-C_gmX_m                 公式十三

将求得的旋转矩阵C_mg和位置向量T_mgo带入到公式一中，根据P_mi的值就能够得到P_gi的值，即得到测量结果。

有益效果：本发明提出了一种基于特征线的运动目标相对位置姿态的光学测量方法，并对测量误差进行了分析。与其他测量方法相比，它具有如下特点：①无需量测特征光点在目标坐标系中坐标；②对摄像机模型没有限制；③位置姿态的测量值具有解析解。实际测量结果验证了该方法的可行性和有效性。

附图说明

图1是本发明的原理图；图2至图7是使用本发明方法测得的运动目标相对位置姿态测量误差直方图仿真结果，图2是俯仰角误差，图3是偏航角误差，图4是滚转角误差，图5是X方向位置误差，图6是Y方向位置误差，图7是Z方向位置误差；图8和图9是使用本发明方法对目标零位位置姿态已知时R_x实测数据分析；图10和图11是使用本发明方法对目标零位位置姿态已知时T_x实测数据分析。

具体实施方式

具体实施方式一：结合图1说明本实施方式，本实施方式的测量过程如下：

在运动目标上安装特征点，首次设运动目标上存在不平行的两条直线L₁和L₂，P₁、P₂、P₃和P₄分别为直线上的点，其中点P₁和P₃位于直线L₁上，点P₂和P₄位于直线L₂上，所述的P₁、P₂、P₃和P₄四个点为特征点，定义所述四个特征点在目标坐标系和测量坐标系中的坐标分别为P_mi，P_gi，其中i＝1，2，3，4；

目标坐标系与运动目标固连，其原点O_m取在运动目标的质心上，则P_mi的值为定值；

测量坐标系随运动目标的运动而变化，测量坐标系是确定运动目标的质心在空间的坐标位置及其在空间的姿态的参考基准，其中空间的坐标位置是三个方向的平动：X方向位置、Y方向位置和Z方向位置，空间的姿态是绕三个方向轴的转动：俯仰角、偏航角和滚转角，则P_gi的值是测量值，是未知值；

在任意时刻，同一特征点在目标坐标系和测量坐标系中的坐标均满足如下关系：

P_gi＝C_gmP_mi+T_mgo   i＝1，2，3，4                 公式一

式中C_gm——目标坐标系相对于测量坐标系的旋转矩阵C_mg的逆；

T_mgo——目标坐标系原点在测量坐标系中的位置向量；

把公式一中i＝1和i＝3时的等式两边分别作差，再把公式一中i＝2和i＝4时的等式两边也分别作差，可得：

P_g(j+2)-P_gj＝C_gm(P_m(j+2)-P_mj)     j＝1，2        公式二

当特征点不重合时，公式二两边均不是零向量。此时，可对其两端向量进行归一化，如下式所示：

$\frac{P_{g(j+2)}-P_{\mathrm{gj}}}{|P_{g(j+2)}-P_{\mathrm{gj}}|}=\frac{C_{\mathrm{gm}}(P_{m(j+2)}-P_{\mathrm{mj}})}{\left|C_{\mathrm{gm}}(P_{m(j+2)}-P_{\mathrm{mj}})\right|}$ j＝1，2

公式三

又因为旋转矩阵C_gm为正交阵，所以公式三可进一步表示为：

$\frac{P_{g(j+2)}-P_{\mathrm{gj}}}{|P_{g(j+2)}-P_{\mathrm{gj}}|}=C_{\mathrm{gm}}\frac{P_{m(j+2)}-P_{\mathrm{mj}}}{|P_{m(j+2)}-P_{\mathrm{mj}}|}$ j＝1，2

公式四

定义

$\left\{\begin{array}{c}{A}_{\mathrm{gj}}\stackrel{\mathrm{\Δ}}{=}\frac{P_{g(j+2)}-P_{\mathrm{gj}}}{|P_{g(j+2)}-P_{\mathrm{gj}}|}\\ A_{\mathrm{mj}}\stackrel{\mathrm{\Δ}}{=}\frac{P_{m(j+2)}-P_{\mathrm{mj}}}{|P_{m(j+2)}-P_{\mathrm{mj}}|}\end{array}\right.$ j＝1，2

公式五

则，公式四更简洁、直观地表示为：

A_gj＝C_gmA_mj    j＝1，2              公式六

因为旋转矩阵C_gm是正交阵，所以对于向量A_g1和A_g2，如下关系也成立：

A_g1×A_g2＝(C_gmA_m1)×(C_gmA_m2)＝C_gm(A_m1×A_m2)       公式七

依据四个特征点不共线的条件，可知公式七两边均为非零向量，对公式七两端进行归一化：

$\frac{A_{g1}\×A_{g2}}{|A_{g1}\×A_{g2}|}=C_{\mathrm{gm}}\frac{A_{m1}\×A_{m2}}{|A_{m1}\×A_{m2}|}$ 公式八

同理定义

$\left\{\begin{array}{c}{B}_g\stackrel{\mathrm{\Δ}}{=}\frac{A_{g1}\×A_{g2}}{|A_{g1}\×A_{g2}|}\\ B_m\stackrel{\mathrm{\Δ}}{=}\frac{A_{m1}\×A_{m2}}{|A_{m1}\×A_{m2}|}\end{array}\right.$ 公式九

把B_g和B_m代入公式八，可得：

B_g＝C_gmB_m                 公式十

把公式六和公式十两端的向量组合成矩阵，并定义

$\left\{\begin{array}{c}{D}_g\stackrel{\mathrm{\Δ}}{=}(A_{g1},A_{g2},B_g)\\ D_m\stackrel{\mathrm{\Δ}}{=}(A_{m1},A_{m2},B_m)\end{array}\right.$

则公式六和公式十可用一个矩阵等式表示为：

D_g＝C_gmD_m              公式十一

由D_g和D_m的构造方法，以及四个特征点不重合且不共线的条件可知：D_g和D_m均满秩，因此，可以通过公式十二求取目标坐标系相对于测量坐标系的旋转矩阵C_mg：

$C_{\mathrm{mg}}=C_{\mathrm{gm}}^{-1}={\left(D_g{D}_m^{-1}\right)}^{-1}$ 公式十二

若两条直线共面，则分别求取P₁和P₃所在直线L₁与P₂和P₄所在直线L₂的交点在目标坐标系中的坐标Xm和在测量坐标系中的坐标Xg；若两条直线异面，则分别求取P₁和P₃所在直线L₁与P₂和P₄所在直线L₂的公垂线的中点在目标坐标系中的坐标Xm和在测量坐标系中的坐标Xg；把Cgm、Xm和Xg代入公式十三，就可以求得目标坐标系相对于测量坐标系的位置向量T_mgo，用显式可表示为：

T_mgo＝X_g-C_gmX_m           公式十三

将求得的旋转矩阵C_mg和位置向量T_mgo带入到公式一中，根据P_mi的值就能够得到P_gi的值，即得到测量结果。

具体实施方式二：结合图1至图11说明本实施方式，本实施方式是具体实施方式一的具体验证过程，假设两束光线的夹角为15°，两光斑接收屏的距离为13米，运动目标的姿态角变化范围为±15°，位置变化范围为：x：(-1.5，+1.5)m，y：(-1，+1)m，z：(-2，+2)m。图2给出了使用本发明方法测得的运动目标相对位姿测量误差直方图，图2至图7可知：1)俯仰角(绕z轴的旋转角度)的测量精度受光斑的坐标提取精度影响较大。这主要是俯仰角的测量基线较其它两个姿态角的测量基线较小的原因。2)z方向的位置测量精度受光斑的坐标提取精度影响较大。这主要是因为光束夹角较小，同样的偏差，z方向的变化最大。图8至图11给出了实际验证结果。本发明方法已经成功应用到了一测量系统，在测量系统中，在运动目标上安装的两两共线的激光器，其发出的光束也是两两共线，测量光束的方程也可实现对目标上直线的测量。系统中光学器件的基本参数是：镜头焦距为28mm，图像分辨率为1280×1024.运动目标的测量数据如图8至图11所示(以Rx和T_x为例)，从图2至图9中可以看出，姿态角测量误差小于1角分，而位置测量误差小于1毫米。可以满足高精度运动目标位姿测量的需求。其它组成和连接方式与具体实施方式一相同。

本发明内容不仅限于上述各实施方式的内容，其中一个或几个具体实施方式的组合同样也可以实现发明的目的。

Claims (1)

1.一种基于特征线的运动目标位姿光学测量方法，其特征在于它的测量过程如下：

在运动目标上安装特征点，首次设运动目标上存在不平行的两条直线L₁和L₂，P₁、P₂、P₃和P₄分别为直线上的点，其中点P₁和P₃位于直线L₁上，点P₂和P₄位于直线L₂上，所述的P₁、P₂、P₃和P₄四个点为特征点，定义所述四个特征点在目标坐标系和测量坐标系中的坐标分别为P_mi，P_gi，其中i＝1，2，3，4；

目标坐标系与运动目标固连，其原点O_m取在运动目标的质心上，则P_mi的值为定值；

测量坐标系随运动目标的运动而变化，测量坐标系是确定运动目标的质心在空间的坐标位置及其在空间的姿态的参考基准，其中空间的坐标位置是三个方向的平动：X方向位置、Y方向位置和Z方向位置，空间的姿态是绕三个方向轴的转动：俯仰角、偏航角和滚转角，则P_gi的值是测量值，是未知值；

在任意时刻，同一特征点在目标坐标系和测量坐标系中的坐标均满足如下关系：

P_gi＝C_gmP_mi+T_mgo   i＝1，2，3，4                公式一

式中C_gm——目标坐标系相对于测量坐标系的旋转矩阵C_mg的逆；

T_mgo——目标坐标系原点在测量坐标系中的位置向量；

把公式一中i＝1和i＝3时的等式两边分别作差，再把公式一中i＝2和i＝4时的等式两边也分别作差，可得：

P_g(j+2)-P_gj＝C_gm(P_m(j+2)-P_mj)  j＝1，2        公式二

对其两端向量进行归一化，如下式所示：

$\frac{P_{g(j+2)}-P_{\mathrm{gj}}}{|P_{g(j+2)}-P_{\mathrm{gj}}|}=\frac{C_{\mathrm{gm}}(P_{m(j+2)}-P_{\mathrm{mj}})}{\left|C_{\mathrm{gm}}(P_{m(j+2)}-P_{\mathrm{mj}})\right|}$ j＝1，2

公式三

又因为旋转矩阵C_gm为正交阵，所以公式三可进一步表示为：

$\frac{P_{g(j+2)}-P_{\mathrm{gj}}}{|P_{g(j+2)}-P_{\mathrm{gj}}|}=C_{\mathrm{gm}}\frac{P_{m(j+2)}-P_{\mathrm{mj}}}{|P_{m(j+2)}-P_{\mathrm{mj}}|}$ j＝1，2

公式四

定义

$\left\{\begin{array}{c}{A}_{\mathrm{gj}}\stackrel{\mathrm{\Δ}}{=}\frac{P_{g(j+2)}-P_{\mathrm{gj}}}{|P_{g(j+2)}-P_{\mathrm{gj}}|}\\ A_{\mathrm{mj}}\stackrel{\mathrm{\Δ}}{=}\frac{P_{m(j+2)}-P_{\mathrm{mj}}}{|P_{m(j+2)}-P_{\mathrm{mj}}|}\end{array}\right.$ j＝1，2

公式五

则，公式四更简洁、直观地表示为：

A_gj＝C_gmA_mj j＝1，2               公式六

因为旋转矩阵C_gm是正交阵，所以对于向量A_g1和A_g2，如下关系也成立：

A_g1×A_g2＝(C_gmA_m1)×(C_gmA_m2)＝C_gm(A_m1×A_m2)          公式七

依据四个特征点不共线的条件，可知公式七两边均为非零向量，对公式七两端进行归一化：

$\frac{A_{g1}\×A_{g2}}{|A_{g1}\×A_{g2}|}=C_{\mathrm{gm}}\frac{A_{m1}\×A_{m2}}{|A_{m1}\×A_{m2}|}$ 公式八

同理定义

$\left\{\begin{array}{c}{B}_g\stackrel{\mathrm{\Δ}}{=}\frac{A_{g1}\×A_{g2}}{|A_{g1}\×A_{g2}|}\\ B_m\stackrel{\mathrm{\Δ}}{=}\frac{A_{m1}\×A_{m2}}{|A_{m1}\×A_{m2}|}\end{array}\right.$ 公式九

把B_g和B_m代入公式八，可得：

B_g＝C_gmB_m              公式十

把公式六和公式十两端的向量组合成矩阵，并定义

$\left\{\begin{array}{c}{D}_g\stackrel{\mathrm{\Δ}}{=}(A_{g1},A_{g2},B_g)\\ D_m\stackrel{\mathrm{\Δ}}{=}(A_{m1},A_{m2},B_m)\end{array}\right.$

则公式六和公式十可用一个矩阵等式表示为：

D_g＝C_gmD_m             公式十一

由D_g和D_m的构造方法，以及四个特征点不重合且不共线的条件可知：D_g和D_m均满秩，因此，可以通过公式十二求取目标坐标系相对于测量坐标系的旋转矩阵C_mg：

$C_{\mathrm{mg}}=C_{\mathrm{gm}}^{-1}={\left(D_g{D}_m^{-1}\right)}^{-1}$ 公式十二

分别求取P₁和P₃所在直线L₁与P₂和P₄所在直线L₂的交点或公垂线的中点在目标坐标系中的坐标Xm和在测量坐标系中的坐标Xg；把Cgm、Xm和Xg代入公式十三，就可以求得目标坐标系相对于测量坐标系的位置向量T_mgo，用显式可表示为：

T_mgo＝X_g-C_gmX_m                 公式十三

将求得的旋转矩阵C_mg和位置向量T_mgo带入到公式一中，根据P_mi的值就能够得到P_gi的值，即得到测量结果。

Images