追蹤與目標航天器相對位姿參數四元數的解析算法

馮 春 吳洪濤 陳 柏 喬 兵

南京航空航天大學，南京，210016

0 引言

利用計算機視覺進行空間目標物位姿估計是當前光學測量和航天工程等領域的研究熱點之一?；谝曈X的相對位姿確定方法在機器人視覺伺服、衛星編隊和航天器交會對接等方面有著廣泛的應用［1－2］。在航天器空間交會的最后階段，通常采用CCD相機進行追蹤和估計航天器與目標航天器間的相對位姿，國內外學者已經就相關問題開展了大量的研究工作［3－13］，其中傳統的方法就是利用單目視覺進行測量，該方法簡單、可靠且方便，是雙目和多目視覺測量方法的基礎。

Mukundan等［7］提出采用3個非共線特征點，利用四元數理論得到一個一元四次方程，求解方程獲得追蹤航天器與目標航天器間的相對位置和姿態的解析解，但求解四次方程時會出現多值問題，需要加以區分和判別，帶來了識別方面的困難，且在特定形式下會出現奇異值。李建鋒等［8］采用5個非共面特征光點，選取其中3個對稱特征光點為一組，共兩組，每一組利用數值迭代的方法求解相應的參數并將兩組結果取平均值作為最后計算值，該方法是原始問題研究中的一個進步，解決了多解判斷的問題，但迭代方法存在實時性、準確性及收斂性等困難。

本文采用5個非共面特征光點構成兩組均為3個“T”形配置的目標航天器非共線特征光點，基于單目視覺獲取四元數分量和2個航天器之間距離的5個方程。然后對這些方程進行變量替換，求出相對位置和姿態參數的解析解，同時對所求得的解析解進行修正，從而有效解決了文獻［7］中的多值問題和奇異值問題，避免了文獻［8］中采用迭代方法計算時的初值選取、迭代實時性和收斂困難等問題，是對文獻［7］和文獻［8］的一種改進。最后利用數學仿真驗證了本文算法的有效性，是當前這類問題一種理想的解決方案。

1 位姿測量系統結構

利用光學相機進行最后階段航天器交會對接相對位姿測量的系統結構如圖1所示。特征目標是在目標航天器上以固定幾何關系分布的特征點光源。相對位姿參數測量系統主要包括：相機標定、圖像采集、圖像畸變校正及濾波、特征點提取、相對位姿解算等步驟。

圖1 位姿參數測量系統結構圖

對于特征光點的結構，本文采用5個非共面特征光點構成特定幾何關系配置的形式，這是考慮到利用3個非共線的特征點（其確定的平面不與相機的光軸正交）就可以唯一確定相對姿態參數，但為了保證任何時刻光學相機視場范圍內至少有3個非共線的特征光點，故特征光點的設計采用了圖2所示的結構和坐標，該結構既便于測量，又能使目標特征光點具有一定的冗余性［7－8］。

圖2 特征光點設置及航天器和相機坐標系

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

為了分析問題方便，建立了圖2中的三種坐標系，即目標航天器坐標系o（x，y，z）、攝像機坐標系O（X，Y，Z）和像平面坐標系Oi（U，V）。三坐標系具有的特點如下：

（1）目標航天器坐標系o（x，y，z）定義在目標航天器的特征點上，采用3個特征光點構成“T”形，坐標軸可依據所需特征光點的幾何關系設定。

（2）攝像機坐標系O（X，Y，Z）將原點O作為攝像機的光心，X、Z軸分別與圖像坐標系的U、V軸平行，Y軸為攝像機的光軸，與圖像平面垂直。

（3）圖像坐標系Oi（U，V）是以像素為單位的坐標系，U、V是每一像素在圖像中的列值和行值，它的原點Oi是攝像機光軸與圖像平面的交點，它與攝像機坐標系原點O之間的距離即為攝像機的焦距f。

2.1 單目視覺四元數位姿測量方法

假定相機已經精確標定，并且特征光點位置坐標與圖像坐標均正確匹配，則在相機坐標系中的像平面可以表示為Y＝－f。為完整起見，本文仿照文獻［8］并加以推廣，將特征光點s i（x i，y i，zi）在相機坐標系中的對應點坐標標記為S i（X i，Y i，Zi），從而由剛體的旋轉和平移變換可得

式中，R為旋轉矩陣；t為平移矩陣。

用四元數表示的旋轉矩陣為

單位四元數的4個參數qi（i＝0，1，2，3）的約束方程為

在圖2中取“T”形結構的3個特征光標點s1、s2、s3，它們在目標坐標系中的坐標可表示為

目標航天器上的特征光點在相機坐標系中的坐標S i（Xi，Y i，Zi）（i＝ 1，2，3）與 圖 像 坐 標 系（U i，V i）（i＝1，2，3）之間的關系可利用透視投影變換方程表示為

實際應用中兩航天器之間的距離ty遠遠大于特征光點之間的深度差（即a，b?t y，則有Y i≈t y），故上述方程變換為

將特征光點s i（x i，y i，zi）代入式（1），則其在相機坐標系中的坐標值為

將式（4）代入式（5）、式（6）、式（7），可得四元數的4個分量與ty的關系表達式：

同理，“T”形結構的3個特征光標點s4、s2、s5在目標坐標系中的坐標可表示為

四元數的4個分量與t y的關系式為

聯立式（8）和式（10）有等式

考慮到t y一般為正，因此取正平方根，而且Ki（i＝1，2，…，8）不會同時為零，故有

另外2個參數tx、tz和另外4個未知量的求解表達式為

由式（8）和式（2）可得

由式（15）、式（16）可得

考慮到2個航天器之間的旋轉角度不應該太大，而當旋轉角滿足｜φ｜＜90°時有q0＞q1，故此時式（17）選擇正的平方根，即

通過式（15）、式（16）和式（18）可得

由于數學上q0取正號或者負號的時候，獲取的相應四元數表示繞同一旋轉軸的相同旋轉，經驗值一般選取q0＞0，故有表達式：

由η＝q0q1可得

得到q0、q1后，若式（17）右端不為0，則可利用式（8）和式（10）求得q2、q3，其表達式為

2.2 正交比例投影誤差修正

在兩航天器之間距離ty遠大于特征點景深（Y i－Y j）（i≠j）條件下，正交比例投影可以作為透視投影的近似，即利用式（4）代替式（3）進行近似計算。圖2選定的5個特征光點中的s1、s3、s4、s5處于同像平面平行的平面內，而s2在距離該平面b處，故式（4）的結果會存在誤差，需要進行修正，其表達式為

相應地，式（8）的第三分式和第四分式與式（10）的第三分式和第四分式變為

則式（15）修正為

將修正的η′代入式（20）和式（21）計算得到修正后的q0、q1，四元數的另外2個分量的修正表達式為

在完成了上述四元數各分量的求取后，根據歐拉角與姿態矩陣元素的對應關系，可以得到四元數的4個分量與航天器相對姿態角θ（俯仰）、ψ（偏航）、φ（滾動）的關系表達式：

上述分析表明：文獻［7］的本質是將一元四次方程的四元數特征和兩組特征點化為一組遞進的一元二次方程組，并經優化選擇，從而獲得（ty，q0，q1，q2，q3）5個變量的唯一解。本文將幾何、數學和光學測量方法相結合，采用顯式方式求取5個變量的唯一解，避免了數值方法的初值選取與收斂性判斷等繁復不便。

進一步，當兩航天器距離越來越接近，即正交比例投影的近似條件不再滿足時，相關Ki系數需要修正，此時解析方程組形式不變，仍可用上述方程求解。

3 數值仿真

為了驗證本文算法獲取的航天器位姿參數解析解的有效性，選用MATLAB軟件對該算法進行數學仿真。仿真的相關條件參照文獻［7］進行設置。仿真結果如圖3、圖4、圖5所示。

目標航天器上的特征光點為s i（x i，y i，zi）（i＝1，2，…，5），目標坐標系中的坐標值表達式為

圖3 修正前的相對位姿參數估計誤差

圖4 修正后的相對位姿參數估計誤差

相機的焦距f＝0.1m。假定兩航天器姿態一致，即四元數q＝［1 0 0 0］，3個坐標軸的相對平移量為t x∶t y∶tz＝5.2∶20∶2，目標航天器與相機距離從20m變化到1m（在交會對接的最后階段，兩航天器相對姿態比較小，可以作上述假設）。圖3為未修正之前的位姿參數誤差仿真結果。

圖5 位置不變時的相對位姿參數誤差

圖4為修正后的相對位姿參數估計誤差，從圖中可以看出，通過對解析解的修正，相對姿態的參數估計精度有了較大的改善，修正后的相對姿態角估計誤差在5m以外時小于0.7°。隨著相對距離的減小，相對姿態參數估計誤差逐漸增大，在小于5m時，誤差急劇增大。原因是比例正交投影近似的條件是兩航天器之間的距離ty需遠大于特征點的景深（Yi－Y j）（i≠j）。

下面分析兩航天器相對姿態大小對其相對位姿測量參數誤差的影響，假定兩航天器之間相對位置關系不變，即t＝［5.2 20 2］，固定3個姿態參數的其中2個，增大另一個，使單軸姿態角變化20°。由圖5仿真結果可知，相對位置最大估計誤差為0.022m，相對姿態最大估計誤差為0.63°。由于修正項是基于標稱相對位置和姿態得出的，所以當相對姿態偏差較小時，本文的算法完全可以滿足測量精度的要求。

4 結語

本文提出的解析算法簡化了相對位姿參數確定問題的計算過程，計算量小，計算速度快，可以滿足航天器交會對接單目視覺測量的實時性要求。通過數學仿真，驗證了本文提出的交會對接最后階段相對位姿參數估計算法的可行性和有效性。

［1］John L J，Declan C H，Karim P W，et al.Visionbased Navigation for Rendezvous，Docking and Proximity Operations［R］.Cranfield：American Astronomioal Society，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 Docing 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］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.

［8］李建鋒，杜小平，張彗星.基于四元數的航天器間相對姿態光學測量方法研究［J］.航天控制，2011，29（2）：46－51.

Li Jianfeng，Du Xiaoping，Zhang Huixing.The Research on Spacecraft Relative State Optical Measurement Based on Quaternion［J］.Aerospace Control，2011，29（2）：46－51.

［9］徐文福，梁斌，李成，等.基于立體視覺的航天器相對位姿測量方法與仿真研究［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.

［10］王保豐，李廣云，陳繼華，等.航天器交會對接中測量靶標的兩種設計方法［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.

［11］Wilson J R.Satellite Hopes Ride on Orbital Express［J］.Aerospace America，2007，45（2）：30－35.

［12］Tafazoli M.A Study of on－orbit Spacecraft Failures［J］.Acta Astronautica，2009，64（2/3）：195－205.

［13］徐文福，梁斌，李成，等.空間機器人捕獲非合作目標的測量與規劃方法［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.

（編輯 何成根）