馮春 吳洪濤 喬兵 陳柏

（1 南京航空航天大學(xué)機電學(xué)院，南京210016）（2 南京航空航天大學(xué)航天學(xué)院，南京 210016）

1 引言

利用計算機視覺進行空間目標(biāo)物位姿估計是光學(xué)測量方向和航天工程等領(lǐng)域的研究熱點之一?；谝曈X的相對位姿確定方法在機器人視覺伺服、衛(wèi)星編隊和航天器交會對接等方面有廣泛的應(yīng)用［1－2］，在航天器空間交會的最后階段，通常采用CCD 相機完成追蹤航天器與目標(biāo)航天器間的相對位姿估計，國內(nèi)外學(xué)者已經(jīng)對相關(guān)問題進行了大量的工作［3－6］。其中，傳統(tǒng)的方法就是利用單目視覺進行測量，該方法簡便、可靠，也是雙目和多目立體視覺測量方法的基礎(chǔ)［7－11］。但是基于單目視覺求解航天器間相對位姿參數(shù)方法存在下列問題：采用迭代求解二次非線性方程時存在實時性、準確性以及收斂性等方面都存在難點；采用解析法求解二次非線性方程時存在多值結(jié)果，這將會帶來識別方面的困難。

Mukundan等采用3個非共線特征點，基于四元數(shù)理論得到一個一元四次方程，求解方程獲得追蹤航天器與目標(biāo)航天器間的相對位置和姿態(tài)的解析解，但存在的問題是求解四次方程時會出現(xiàn)多值問題，需要加以區(qū)分和判別，這就帶來了識別方面的困難，而且在特定形式下會出現(xiàn)奇異值［12］。本文是在此方法上的一種改進，同文獻［12］一致，采用3個“T”形配置的目標(biāo)航天器非共線特征光點，基于單目視覺獲取四元數(shù)分量和兩個航天器之間距離的5個約束方程。首先利用雙焦單目視覺［13］理論獲取兩個航天器之間的距離，然后對5個約束方程進行變量替換求出相對位置和姿態(tài)參數(shù)的解析解，從而有效解決了文獻［12］中的多值問題和奇異值問題。同時，對所求得的解析解進行修正。

2 單目視覺相對位姿確定算法

為了分析問題方便，建立了如圖1所示的3種坐標(biāo)系：目標(biāo)航天器坐標(biāo)系OAXAYAZA，攝像機坐標(biāo)系OCXCYCZC和圖像平面坐標(biāo)系OiUiVi，各坐標(biāo)系定義如下：

圖1 特征光點設(shè)置及航天器和相機坐標(biāo)系Fig.1 Feature points setting and the spacecraft and camera reference system

1）目標(biāo)航天器坐標(biāo)系OAXAYAZA：該坐標(biāo)系定義在目標(biāo)航天器的特征點上，坐標(biāo)軸可以依據(jù)所需特征光點的幾何關(guān)系設(shè)定，由于本文采用的3個特征光點s1，s2，s3構(gòu)成“T”形，故坐標(biāo)系如圖1所示設(shè)定。

2）攝像機坐標(biāo)系OCXCYCZC：原點OC作為攝像機的光心，XC、ZC軸分別與圖像平面坐標(biāo)系的Ui、Vi軸平行，YC軸為攝像機的光軸，與圖像平面垂直。

3）圖像平面坐標(biāo)系OiUiVi：是以像素為單位的坐標(biāo)系，Ui、Vi是每一像素在圖像中列值和行值，它的原點Oi是攝像機光軸與圖像平面的交點，它與攝像機坐標(biāo)系原點OC之間的距離即為攝像機的焦距f。

2.1 雙焦單目視覺深度估計原理

雙焦成像算法［13］的基本原理為：空間物點深度定義為不同焦距下成像的矢量位移和成像焦距的函數(shù)，在兩幅變焦圖像中找到匹配點對，獲取所成像的矢量位移的長度并給出焦距值，從而計算出空間物點深度信息。

假定攝像機模型為理想的針孔透視變換模型，依據(jù)圖2所示幾何模型的關(guān)系可以推導(dǎo)出深度Z的計算公式：

式中 f1，f2分別為較小和較大焦距值；r1，r2分別為相應(yīng)的像點矢量大小；Δr＝r2－r1；a＝（f2－f1）／f1。

雙焦鏡頭的調(diào)焦方法可以依據(jù)變焦鏡頭自動調(diào)焦方法，而相機的自動調(diào)焦技術(shù)已經(jīng)實現(xiàn)了追蹤運動物體的連續(xù)自動調(diào)焦。雙焦成像需要同步獲取不同焦距下的兩幅圖像，而自動調(diào)焦會出現(xiàn)時延，可利用誤差校正方法［13］對提取的特征點進行校正，以消除兩幅雙焦圖像相對位姿的小偏差對深度估計精度的影響。

圖2 成像幾何模型Fig.2 Imaging geometry model

2.2 單目視覺四元數(shù)位姿測量方法

本文提出利用基于雙焦成像的深度估計方法獲取兩航天器之間的距離ty，將單目視覺四元數(shù)位姿測量方法獲得的5個未知數(shù)的方程組簡化為4個變量，經(jīng)過化簡得到一元二次方程，獲得唯一解。本文為完整起見，參考文獻［12］并首先獲取位置信息進而推導(dǎo)姿態(tài)參數(shù)。在相機坐標(biāo)系中像平面可以表示為Y＝－f。特征光點si（xi，yi，zi）在相機坐標(biāo)系的對應(yīng)點坐標(biāo)為Si（Xi，Yi，Zi），則由剛體的旋轉(zhuǎn)和平移變換的運動可得：

式中 t＝（tx，ty，tz）T為平移矢量；R 表示旋轉(zhuǎn)矩陣，其四元數(shù)為

圖1中3個特征光標(biāo)點s1，s2，s3在目標(biāo)坐標(biāo)系的坐標(biāo)表示為

目標(biāo)航天器上的特征光點在相機坐標(biāo)系坐標(biāo)SiXiYiZi（i＝1，2，3）與圖像坐標(biāo)系OiUiVi（i＝1，2，3）之間的關(guān)系可以利用透視投影變換方程表示為

實際應(yīng)用中ty遠遠大于特征光點之間的深度差，故式（4）變換為

將特征光點si（xi，yi，zi）代入式（2）獲取它們在相機坐標(biāo)系的坐標(biāo)值為

將式（5）代入式（6），可得四元數(shù)的4個分量與ty的關(guān)系為

本文提出利用基于雙焦成像的深度估計方法，首先獲取ty，則可以得到另外的兩個位置坐標(biāo)：

對式（6）進行分析變換，得到q0、q1間存在如下關(guān)系：

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

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

考慮到兩個航天器之間的旋轉(zhuǎn)角度不應(yīng)該太大，而當(dāng)旋轉(zhuǎn)角滿足＜90°時有q0＞q1，故此時式（11）選擇正的平方根，即：

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

當(dāng)q0取正值或負值時，所得到的相應(yīng)的四元數(shù)表示繞同一旋轉(zhuǎn)軸的相同旋轉(zhuǎn)，故本文選取正值作為q0的計算值

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

上述過程完成了四元數(shù)各分量的求取，根據(jù)歐拉角與姿態(tài)矩陣元素的對應(yīng)關(guān)系，可以得到四元數(shù)的4個分量與航天器相對姿態(tài)角θ（俯仰），ψ（偏航），φ（旋轉(zhuǎn)）的關(guān)系：

3 誤差分析

下面對基于雙焦成像深度估計誤差的精度進行討論，給出影響誤差大小的各種因素；并對正交比例投影誤差進行分析，給出誤差的修正結(jié)果。

3.1 深度估計誤差分析

由航天器位姿參數(shù)的求解過程可知，姿態(tài)參數(shù)的誤差取決于航天器的定位誤差。故所有位姿參數(shù)的精度可以歸結(jié)為雙焦單目深度估計精度的問題。這里假定清晰成像，并且相機各參數(shù)精確標(biāo)定，僅考慮特征匹配的精度對深度估計精度的影響。由雙焦深度估計原理［13］可得深度估計的誤差為

式中 x為大焦距圖像的像矢量偏差；r1＝f1R／Z；r2＝f2R／［Z－（f2－f1）］。下面分別獨立改變Z 和R 來分析深度估計誤差。

文獻［13］只局限于1m 以內(nèi)的深度估計問題，而航天器對接最后階段的距離范圍小于20m。仿真試驗表明，在兩航天器距離較大時增大焦距可以盡量減小誤差，本文對此距離范圍內(nèi)的精度問題做了深入的研究。首先假定f1＝0.6m、f2＝0.9m，理論深度為Z＝12m，對不同R 時的x 與深度誤差ΔZ 關(guān)系進行研究，結(jié)果如圖3所示。由圖3可知，在深度確定的情況下，深度誤差與R 成反比關(guān)系，即遠離主點，深度估計精度高。故在進行深度估計情況下，在保證特征點位于視野范圍的同時，盡量使得特征點遠離主點，此時獲取的深度值精度較高。

然后，取定R＝0.8m，對不同的深度值Z條件下x與深度誤差ΔZ關(guān)系進行研究，結(jié)果如圖4所示。從圖4可知，對于深度不同的目標(biāo)物，匹配的精度會不同程度影響深度估計精度，當(dāng)深度值越大時，相同匹配精度下的深度誤差越大，精度越低。故在相機參數(shù)確定情況下，對于深度值很大的深度估計問題，基于雙焦單目測距方法并不適用。最后，假定f1＝0.6m、f2＝0.9m，小焦距和大焦距圖像的特征點提取和匹配誤差均存在，且假定為x＝0.1mm，R＝Z／4，則深度估計誤差與深度值的關(guān)系曲線如圖5所示。

圖3 不同R 時x 與ΔZ 的關(guān)系曲線Fig.3 Relationship between xandΔZ at different R

圖4 不同Z 時x 與ΔZ 的關(guān)系曲線Fig.4 Relationship between x and ΔZ at different Z

圖5 Z 與ΔZ 的關(guān)系曲線Fig.5 Relationship between Z and ΔZ

3.2 正交比例投影誤差

在兩航天器之間距離ty遠大于特征點景深（Yi－Yj）（i≠j）條件下，正交比例投影可以作為透視投影的近似，即利用式（5）代替式（4）進行近似計算。

圖1選定的3個特征光點中的s1，s3處于同像平面平行的平面內(nèi)，而s2在距平面b 處，所以式（5）結(jié)果會存在誤差。為了準確求解，依據(jù)文獻［12］方法進行如下修正，則相應(yīng)地式（7）的后兩式變?yōu)?/p>

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

將修正的η′代入式（14）和（9）計算得到修正后的q0、q1，四元數(shù)另外兩個分量修正結(jié)果如下：

4 數(shù)值仿真

為了驗證本文算法獲取的航天器位姿參數(shù)解析解的有效性，選用Matlab軟件對此算法進行數(shù)學(xué)仿真。仿真相關(guān)的條件參照文獻［12］設(shè)置如下。

目標(biāo)航天器上的特征光點為si（xi，yi，zi）（i＝1，2，3），并且在目標(biāo)坐標(biāo)系中坐標(biāo)表示為相機的焦距f1＝0.6m，f2＝0.9m，假定特征點提取及匹配等綜合誤差為x＝0.1mm，取R＝ty／4。首先假定兩航天器姿態(tài)一致，即四元數(shù)q＝［1 0 0 0］，3個坐標(biāo)軸的相對平移為tx∶ty∶tz＝5.2∶20∶2，目標(biāo)航天器與相機距離從20m 變化到1m（在交會對接的最后階段，兩航天器相對姿態(tài)比較小，可以作上述的假設(shè)），取20m 深度估計最大誤差作為ty的誤差，未修正之前的姿態(tài)仿真結(jié)果如圖6所示。從圖7中可以看到通過對解析解的修正，相對姿態(tài)的參數(shù)估計精度有了較大的改善。修正后的相對姿態(tài)角估計誤差在5m 以外時小于0.8°。隨著相對距離的減小，相對姿態(tài)參數(shù)估計誤差逐漸增大，在小于3m時，誤差急劇增加?？紤]到仿真的ty的誤差條件是取20m 時最大深度估計誤差，當(dāng)相對距離小于3m 時，由圖4可知ty的誤差非常小，所以3m 內(nèi)相對姿態(tài)參數(shù)誤差遠小于圖7所示，限于篇幅不作具體討論。

圖6 深度估計誤差確定時相對姿態(tài)參數(shù)修正前估計Fig.6 Previous modification errors of relative attitude parameters estimation when the depth estimation errors were given

圖7 深度估計誤差確定時相對姿態(tài)參數(shù)修正后估計Fig.7 Modification errors of relative attitude parameters estimation when the depth estimation errors were given

下面分析兩航天器相對姿態(tài)大小對其相對姿態(tài)測量參數(shù)誤差的影響。假定兩航天器之間相對位置關(guān)系不變，即t＝［5.2 20 2］，固定3個姿態(tài)參數(shù)中的兩個，同時增大另一個姿態(tài)參數(shù)值，使單軸姿態(tài)角變化20°，則仿真結(jié)果如圖8所示。由圖8可知，相對姿態(tài)最大估計誤差為0.586°，由于修正項是基于標(biāo)稱相對位置和姿態(tài)得出的，所以對于相對姿態(tài)偏差較小情況，本文的算法完全可以滿足測量精度的要求。

本文提出的基于雙焦單目視覺的航天器間相對位姿參數(shù)估計算法與文獻 ［6］、［7］相比，不需要相機之間的復(fù)雜配置，測量結(jié)構(gòu)上具有簡單、方便的特點；另外，本文算法對特征光點配置要求比較低，僅需要3個非共線的光點，容易保證所有光點均在視場范圍內(nèi)；此外，本文算法與其他方法相比同樣需要精確標(biāo)定相機，而且需要對雙焦圖像進行準確的特征提取和精確的匹配，從而保證位姿參數(shù)估計的準確性。

圖8 位置不變時相對姿態(tài)參數(shù)誤差tive pose parameters when the position parameters were maintained

5 結(jié)束語

基于雙焦單目深度估計方法，僅利用3個非共線特征光點，在確定航天器間相對位置的條件下，完成對兩航天器相對姿態(tài)參數(shù)的估計，克服了文獻［12］解析解的多值和奇異值問題，獲取了唯一解，同時避免了利用迭代法的初始值、收斂性、準確性等難題。鑒于位姿參數(shù)的精度很大程度上取決于雙焦深度估計的精度，本文對此進行了詳細的討論，并擴展了文獻［13］的深度估計范圍，使其滿足航天器交會對接最后階段的距離要求。在合理選擇相機焦距以及保證特征光點提取和匹配精度的條件下，可以確保一定深度范圍內(nèi)的深度估計精度，從而保證位姿參數(shù)估計精度。通過數(shù)學(xué)仿真，驗證了本文提出的基于雙焦單目視覺航天器交會對接最后階段相對位姿參數(shù)估計算法的可行性和有效性。

［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］.宇航學(xué)報，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］杜小平，趙繼廣，崔占忠，等.基于計算機視覺的航天器間相對狀態(tài)測量系統(tǒng) ［J］.光學(xué)技術(shù)，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］張慶君，胡修林，葉斌，等.基于雙目視覺測量的航天器相對位置和姿態(tài)的測量方法 ［J］.宇航學(xué)報，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］.宇航學(xué)報，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］王保豐，李廣云，陳繼華，等.航天器交會對接中測量靶標(biāo)的兩種設(shè)計方法 ［J］.宇航學(xué)報，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］徐文福，梁斌，李成，等.空間機器人捕獲非合作目標(biāo)的測量與規(guī)劃方法［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.