基于双焦单目视觉的航天器间相对位姿确定算法

冯春 吴洪涛 乔兵 陈柏

（1 南京航空航天大学机电学院，南京210016）（2 南京航空航天大学航天学院，南京 210016）

1 引言

利用计算机视觉进行空间目标物位姿估计是光学测量方向和航天工程等领域的研究热点之一。基于视觉的相对位姿确定方法在机器人视觉伺服、卫星编队和航天器交会对接等方面有广泛的应用［1－2］，在航天器空间交会的最后阶段，通常采用CCD 相机完成追踪航天器与目标航天器间的相对位姿估计，国内外学者已经对相关问题进行了大量的工作［3－6］。其中，传统的方法就是利用单目视觉进行测量，该方法简便、可靠，也是双目和多目立体视觉测量方法的基础［7－11］。但是基于单目视觉求解航天器间相对位姿参数方法存在下列问题：采用迭代求解二次非线性方程时存在实时性、准确性以及收敛性等方面都存在难点；采用解析法求解二次非线性方程时存在多值结果，这将会带来识别方面的困难。

Mukundan等采用3个非共线特征点，基于四元数理论得到一个一元四次方程，求解方程获得追踪航天器与目标航天器间的相对位置和姿态的解析解，但存在的问题是求解四次方程时会出现多值问题，需要加以区分和判别，这就带来了识别方面的困难，而且在特定形式下会出现奇异值［12］。本文是在此方法上的一种改进，同文献［12］一致，采用3个“T”形配置的目标航天器非共线特征光点，基于单目视觉获取四元数分量和两个航天器之间距离的5个约束方程。首先利用双焦单目视觉［13］理论获取两个航天器之间的距离，然后对5个约束方程进行变量替换求出相对位置和姿态参数的解析解，从而有效解决了文献［12］中的多值问题和奇异值问题。同时，对所求得的解析解进行修正。

2 单目视觉相对位姿确定算法

为了分析问题方便，建立了如图1所示的3种坐标系：目标航天器坐标系OAXAYAZA，摄像机坐标系OCXCYCZC和图像平面坐标系OiUiVi，各坐标系定义如下：

图1 特征光点设置及航天器和相机坐标系Fig.1 Feature points setting and the spacecraft and camera reference system

1）目标航天器坐标系OAXAYAZA：该坐标系定义在目标航天器的特征点上，坐标轴可以依据所需特征光点的几何关系设定，由于本文采用的3个特征光点s1，s2，s3构成“T”形，故坐标系如图1所示设定。

2）摄像机坐标系OCXCYCZC：原点OC作为摄像机的光心，XC、ZC轴分别与图像平面坐标系的Ui、Vi轴平行，YC轴为摄像机的光轴，与图像平面垂直。

3）图像平面坐标系OiUiVi：是以像素为单位的坐标系，Ui、Vi是每一像素在图像中列值和行值，它的原点Oi是摄像机光轴与图像平面的交点，它与摄像机坐标系原点OC之间的距离即为摄像机的焦距f。

2.1 双焦单目视觉深度估计原理

双焦成像算法［13］的基本原理为：空间物点深度定义为不同焦距下成像的矢量位移和成像焦距的函数，在两幅变焦图像中找到匹配点对，获取所成像的矢量位移的长度并给出焦距值，从而计算出空间物点深度信息。

假定摄像机模型为理想的针孔透视变换模型，依据图2所示几何模型的关系可以推导出深度Z的计算公式：

式中 f1，f2分别为较小和较大焦距值；r1，r2分别为相应的像点矢量大小；Δr＝r2－r1；a＝（f2－f1）／f1。

双焦镜头的调焦方法可以依据变焦镜头自动调焦方法，而相机的自动调焦技术已经实现了追踪运动物体的连续自动调焦。双焦成像需要同步获取不同焦距下的两幅图像，而自动调焦会出现时延，可利用误差校正方法［13］对提取的特征点进行校正，以消除两幅双焦图像相对位姿的小偏差对深度估计精度的影响。

图2 成像几何模型Fig.2 Imaging geometry model

2.2 单目视觉四元数位姿测量方法

本文提出利用基于双焦成像的深度估计方法获取两航天器之间的距离ty，将单目视觉四元数位姿测量方法获得的5个未知数的方程组简化为4个变量，经过化简得到一元二次方程，获得唯一解。本文为完整起见，参考文献［12］并首先获取位置信息进而推导姿态参数。在相机坐标系中像平面可以表示为Y＝－f。特征光点si（xi，yi，zi）在相机坐标系的对应点坐标为Si（Xi，Yi，Zi），则由刚体的旋转和平移变换的运动可得：

式中 t＝（tx，ty，tz）T为平移矢量；R 表示旋转矩阵，其四元数为

图1中3个特征光标点s1，s2，s3在目标坐标系的坐标表示为

目标航天器上的特征光点在相机坐标系坐标SiXiYiZi（i＝1，2，3）与图像坐标系OiUiVi（i＝1，2，3）之间的关系可以利用透视投影变换方程表示为

实际应用中ty远远大于特征光点之间的深度差，故式（4）变换为

将特征光点si（xi，yi，zi）代入式（2）获取它们在相机坐标系的坐标值为

将式（5）代入式（6），可得四元数的4个分量与ty的关系为

本文提出利用基于双焦成像的深度估计方法，首先获取ty，则可以得到另外的两个位置坐标：

对式（6）进行分析变换，得到q0、q1间存在如下关系：

由式（7）的第一个式子和式（3）可得：

由式（9）、（10）可得：

考虑到两个航天器之间的旋转角度不应该太大，而当旋转角满足＜90°时有q0＞q1，故此时式（11）选择正的平方根，即：

通过式（10）～（12）可得：

当q0取正值或负值时，所得到的相应的四元数表示绕同一旋转轴的相同旋转，故本文选取正值作为q0的计算值

得到q0、q1后再利用式（7）的后两式可求得q2、q3的值为

上述过程完成了四元数各分量的求取，根据欧拉角与姿态矩阵元素的对应关系，可以得到四元数的4个分量与航天器相对姿态角θ（俯仰），ψ（偏航），φ（旋转）的关系：

3 误差分析

下面对基于双焦成像深度估计误差的精度进行讨论，给出影响误差大小的各种因素；并对正交比例投影误差进行分析，给出误差的修正结果。

3.1 深度估计误差分析

由航天器位姿参数的求解过程可知，姿态参数的误差取决于航天器的定位误差。故所有位姿参数的精度可以归结为双焦单目深度估计精度的问题。这里假定清晰成像，并且相机各参数精确标定，仅考虑特征匹配的精度对深度估计精度的影响。由双焦深度估计原理［13］可得深度估计的误差为

式中 x为大焦距图像的像矢量偏差；r1＝f1R／Z；r2＝f2R／［Z－（f2－f1）］。下面分别独立改变Z 和R 来分析深度估计误差。

文献［13］只局限于1m 以内的深度估计问题，而航天器对接最后阶段的距离范围小于20m。仿真试验表明，在两航天器距离较大时增大焦距可以尽量减小误差，本文对此距离范围内的精度问题做了深入的研究。首先假定f1＝0.6m、f2＝0.9m，理论深度为Z＝12m，对不同R 时的x 与深度误差ΔZ 关系进行研究，结果如图3所示。由图3可知，在深度确定的情况下，深度误差与R 成反比关系，即远离主点，深度估计精度高。故在进行深度估计情况下，在保证特征点位于视野范围的同时，尽量使得特征点远离主点，此时获取的深度值精度较高。

然后，取定R＝0.8m，对不同的深度值Z条件下x与深度误差ΔZ关系进行研究，结果如图4所示。从图4可知，对于深度不同的目标物，匹配的精度会不同程度影响深度估计精度，当深度值越大时，相同匹配精度下的深度误差越大，精度越低。故在相机参数确定情况下，对于深度值很大的深度估计问题，基于双焦单目测距方法并不适用。最后，假定f1＝0.6m、f2＝0.9m，小焦距和大焦距图像的特征点提取和匹配误差均存在，且假定为x＝0.1mm，R＝Z／4，则深度估计误差与深度值的关系曲线如图5所示。

图3 不同R 时x 与ΔZ 的关系曲线Fig.3 Relationship between xandΔZ at different R

图4 不同Z 时x 与ΔZ 的关系曲线Fig.4 Relationship between x and ΔZ at different Z

图5 Z 与ΔZ 的关系曲线Fig.5 Relationship between Z and ΔZ

3.2 正交比例投影误差

在两航天器之间距离ty远大于特征点景深（Yi－Yj）（i≠j）条件下，正交比例投影可以作为透视投影的近似，即利用式（5）代替式（4）进行近似计算。

图1选定的3个特征光点中的s1，s3处于同像平面平行的平面内，而s2在距平面b 处，所以式（5）结果会存在误差。为了准确求解，依据文献［12］方法进行如下修正，则相应地式（7）的后两式变为

式中 K3′＝K3＋U2／（2fty）；K4′＝K4＋V2／（2fty）；则式（9）修正为

将修正的η′代入式（14）和（9）计算得到修正后的q0、q1，四元数另外两个分量修正结果如下：

4 数值仿真

为了验证本文算法获取的航天器位姿参数解析解的有效性，选用Matlab软件对此算法进行数学仿真。仿真相关的条件参照文献［12］设置如下。

目标航天器上的特征光点为si（xi，yi，zi）（i＝1，2，3），并且在目标坐标系中坐标表示为相机的焦距f1＝0.6m，f2＝0.9m，假定特征点提取及匹配等综合误差为x＝0.1mm，取R＝ty／4。首先假定两航天器姿态一致，即四元数q＝［1 0 0 0］，3个坐标轴的相对平移为tx∶ty∶tz＝5.2∶20∶2，目标航天器与相机距离从20m 变化到1m（在交会对接的最后阶段，两航天器相对姿态比较小，可以作上述的假设），取20m 深度估计最大误差作为ty的误差，未修正之前的姿态仿真结果如图6所示。从图7中可以看到通过对解析解的修正，相对姿态的参数估计精度有了较大的改善。修正后的相对姿态角估计误差在5m 以外时小于0.8°。随着相对距离的减小，相对姿态参数估计误差逐渐增大，在小于3m时，误差急剧增加。考虑到仿真的ty的误差条件是取20m 时最大深度估计误差，当相对距离小于3m 时，由图4可知ty的误差非常小，所以3m 内相对姿态参数误差远小于图7所示，限于篇幅不作具体讨论。

图6 深度估计误差确定时相对姿态参数修正前估计Fig.6 Previous modification errors of relative attitude parameters estimation when the depth estimation errors were given

图7 深度估计误差确定时相对姿态参数修正后估计Fig.7 Modification errors of relative attitude parameters estimation when the depth estimation errors were given

下面分析两航天器相对姿态大小对其相对姿态测量参数误差的影响。假定两航天器之间相对位置关系不变，即t＝［5.2 20 2］，固定3个姿态参数中的两个，同时增大另一个姿态参数值，使单轴姿态角变化20°，则仿真结果如图8所示。由图8可知，相对姿态最大估计误差为0.586°，由于修正项是基于标称相对位置和姿态得出的，所以对于相对姿态偏差较小情况，本文的算法完全可以满足测量精度的要求。

本文提出的基于双焦单目视觉的航天器间相对位姿参数估计算法与文献 ［6］、［7］相比，不需要相机之间的复杂配置，测量结构上具有简单、方便的特点；另外，本文算法对特征光点配置要求比较低，仅需要3个非共线的光点，容易保证所有光点均在视场范围内；此外，本文算法与其他方法相比同样需要精确标定相机，而且需要对双焦图像进行准确的特征提取和精确的匹配，从而保证位姿参数估计的准确性。

图8 位置不变时相对姿态参数误差tive pose parameters when the position parameters were maintained

5 结束语

基于双焦单目深度估计方法，仅利用3个非共线特征光点，在确定航天器间相对位置的条件下，完成对两航天器相对姿态参数的估计，克服了文献［12］解析解的多值和奇异值问题，获取了唯一解，同时避免了利用迭代法的初始值、收敛性、准确性等难题。鉴于位姿参数的精度很大程度上取决于双焦深度估计的精度，本文对此进行了详细的讨论，并扩展了文献［13］的深度估计范围，使其满足航天器交会对接最后阶段的距离要求。在合理选择相机焦距以及保证特征光点提取和匹配精度的条件下，可以确保一定深度范围内的深度估计精度，从而保证位姿参数估计精度。通过数学仿真，验证了本文提出的基于双焦单目视觉航天器交会对接最后阶段相对位姿参数估计算法的可行性和有效性。

［1］JOHN L J，DECLAN C H，KARIM P W，et al.Vision－based navigation for rendezvous，docking and proximity operations［A］.AAS99－021，1999.

［2］BEARD R W，LAWTON J，HADAEGH F Y.A coordination architecture for formation control［J］.IEEE Transactions on Control System Technology，2001，9（6）：777－790.

［3］PHILIP N K，ANANTHASAYANAM M R.Relative position and attitude estimation and control schemes for the final phase of an autonomous docking mission of spacecraft［J］.Acta Astronautica，2003，52：511－522.

［4］林来兴，李灿.交会对接最后逼近阶段CCD 相机的测量方法 ［J］.宇航学报，1994，15（2）：24－34.LIN LAIXING，LI CAN.CCD camera－based measurement for the final phase of RVD mission［J］.Journal of Astronautics，1994，15（2）：24－34.

［5］杜小平，赵继广，崔占忠，等.基于计算机视觉的航天器间相对状态测量系统 ［J］.光学技术，2003，29（6）：664－666.DU XIAOPING，ZHAO JIGUANG，CUI ZHANZHONG，et al.Optical method for position－attitude determination between spacecrafts based on computer vision［J］.Optical Technique，2003，29（6）：664－666.

［6］张庆君，胡修林，叶斌，等.基于双目视觉测量的航天器相对位置和姿态的测量方法 ［J］.宇航学报，2008，29（1）：156－161.ZHANG QINGJUN，HU XIULIN，YE BIN，et al.Binocular vision based relative position and attitude determination between spacecrafts［J］.Journal of Astronautics，2008，29（1）：156－161.

［7］徐文福，梁斌，李成，等.基于立体视觉的航天器相对位姿测量方法与仿真研究［J］.宇航学报，2009，30（4）：1421－1428.XU WENFU，LIANG BIN，LI CHENG，et al.The approach and simulation study of the relative pose measurement between space－crafts based on stereo vision［J］.Journal of Astronautics，2009，30（4）：1421－1428.

［8］王保丰，李广云，陈继华，等.航天器交会对接中测量靶标的两种设计方法 ［J］.宇航学报，2008，29（1）：162－166.WANG BAOFENG，LI GUANGYUN，CHEN JIHUA，et al.Two methods of coded targets used in rendezvous and docking［J］.Journal of Astronautics，2008，29（1）：162－166.

［9］WILSON J R.Satellite hopes ride on orbital express［J］.Aerospace America，2007，45（2）：30－35.

［10］TAFAZOLI M.A study of on－orbit spacecraft failures［J］.Acta Astronautica，2009，64（2／3）：195－205.

［11］徐文福，梁斌，李成，等.空间机器人捕获非合作目标的测量与规划方法［J］.机器人，2010，32（1）：61－69.XU WENFU，LIANG BIN，LI CHENG，et al.Measurement and planning approach of space robot for capturing non－cooperative target［J］.ROBOT，2010，32（1）：61－69.

［12］MUKUNDAN R，RAMAKRISHNAN K R.A quaternion solution to the pose determination problem for rendezvous and docking simulations［J］.Mathematics and Computers in Simulation，1995，39：143－153.

［13］王剑，王元庆.基于双焦成像的单眼立体视觉算法 ［J］.机器人，2007，29（1）：41－44.WANG JIAN，WANG YUANQING.A monocular stereo vision algorithm based on bifocal imaging［J］.ROBOT，2007，29（1）：41－44.