追蹤與目標(biāo)航天器相對(duì)位姿參數(shù)四元數(shù)的解析算法

馮 春 吳洪濤 陳 柏 喬 兵

南京航空航天大學(xué)，南京，210016

0 引言

利用計(jì)算機(jī)視覺(jué)進(jìn)行空間目標(biāo)物位姿估計(jì)是當(dāng)前光學(xué)測(cè)量和航天工程等領(lǐng)域的研究熱點(diǎn)之一?；谝曈X(jué)的相對(duì)位姿確定方法在機(jī)器人視覺(jué)伺服、衛(wèi)星編隊(duì)和航天器交會(huì)對(duì)接等方面有著廣泛的應(yīng)用［1－2］。在航天器空間交會(huì)的最后階段，通常采用CCD相機(jī)進(jìn)行追蹤和估計(jì)航天器與目標(biāo)航天器間的相對(duì)位姿，國(guó)內(nèi)外學(xué)者已經(jīng)就相關(guān)問(wèn)題開(kāi)展了大量的研究工作［3－13］，其中傳統(tǒng)的方法就是利用單目視覺(jué)進(jìn)行測(cè)量，該方法簡(jiǎn)單、可靠且方便，是雙目和多目視覺(jué)測(cè)量方法的基礎(chǔ)。

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

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

1 位姿測(cè)量系統(tǒng)結(jié)構(gòu)

利用光學(xué)相機(jī)進(jìn)行最后階段航天器交會(huì)對(duì)接相對(duì)位姿測(cè)量的系統(tǒng)結(jié)構(gòu)如圖1所示。特征目標(biāo)是在目標(biāo)航天器上以固定幾何關(guān)系分布的特征點(diǎn)光源。相對(duì)位姿參數(shù)測(cè)量系統(tǒng)主要包括：相機(jī)標(biāo)定、圖像采集、圖像畸變校正及濾波、特征點(diǎn)提取、相對(duì)位姿解算等步驟。

圖1 位姿參數(shù)測(cè)量系統(tǒng)結(jié)構(gòu)圖

對(duì)于特征光點(diǎn)的結(jié)構(gòu)，本文采用5個(gè)非共面特征光點(diǎn)構(gòu)成特定幾何關(guān)系配置的形式，這是考慮到利用3個(gè)非共線的特征點(diǎn)（其確定的平面不與相機(jī)的光軸正交）就可以唯一確定相對(duì)姿態(tài)參數(shù)，但為了保證任何時(shí)刻光學(xué)相機(jī)視場(chǎng)范圍內(nèi)至少有3個(gè)非共線的特征光點(diǎn)，故特征光點(diǎn)的設(shè)計(jì)采用了圖2所示的結(jié)構(gòu)和坐標(biāo)，該結(jié)構(gòu)既便于測(cè)量，又能使目標(biāo)特征光點(diǎn)具有一定的冗余性［7－8］。

圖2 特征光點(diǎn)設(shè)置及航天器和相機(jī)坐標(biāo)系

2 單目視覺(jué)相對(duì)位姿確定算法

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

（1）目標(biāo)航天器坐標(biāo)系o（x，y，z）定義在目標(biāo)航天器的特征點(diǎn)上，采用3個(gè)特征光點(diǎn)構(gòu)成“T”形，坐標(biāo)軸可依據(jù)所需特征光點(diǎn)的幾何關(guān)系設(shè)定。

（2）攝像機(jī)坐標(biāo)系O（X，Y，Z）將原點(diǎn)O作為攝像機(jī)的光心，X、Z軸分別與圖像坐標(biāo)系的U、V軸平行，Y軸為攝像機(jī)的光軸，與圖像平面垂直。

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

2.1 單目視覺(jué)四元數(shù)位姿測(cè)量方法

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

式中，R為旋轉(zhuǎn)矩陣；t為平移矩陣。

用四元數(shù)表示的旋轉(zhuǎn)矩陣為

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

在圖2中取“T”形結(jié)構(gòu)的3個(gè)特征光標(biāo)點(diǎn)s1、s2、s3，它們?cè)谀繕?biāo)坐標(biāo)系中的坐標(biāo)可表示為

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

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

將特征光點(diǎn)s i（x i，y i，zi）代入式（1），則其在相機(jī)坐標(biāo)系中的坐標(biāo)值為

將式（4）代入式（5）、式（6）、式（7），可得四元數(shù)的4個(gè)分量與ty的關(guān)系表達(dá)式：

同理，“T”形結(jié)構(gòu)的3個(gè)特征光標(biāo)點(diǎn)s4、s2、s5在目標(biāo)坐標(biāo)系中的坐標(biāo)可表示為

四元數(shù)的4個(gè)分量與t y的關(guān)系式為

聯(lián)立式（8）和式（10）有等式

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

另外2個(gè)參數(shù)tx、tz和另外4個(gè)未知量的求解表達(dá)式為

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

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

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

通過(guò)式（15）、式（16）和式（18）可得

由于數(shù)學(xué)上q0取正號(hào)或者負(fù)號(hào)的時(shí)候，獲取的相應(yīng)四元數(shù)表示繞同一旋轉(zhuǎn)軸的相同旋轉(zhuǎn)，經(jīng)驗(yàn)值一般選取q0＞0，故有表達(dá)式：

由η＝q0q1可得

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

2.2 正交比例投影誤差修正

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

相應(yīng)地，式（8）的第三分式和第四分式與式（10）的第三分式和第四分式變?yōu)?/p>

則式（15）修正為

將修正的η′代入式（20）和式（21）計(jì)算得到修正后的q0、q1，四元數(shù)的另外2個(gè)分量的修正表達(dá)式為

在完成了上述四元數(shù)各分量的求取后，根據(jù)歐拉角與姿態(tài)矩陣元素的對(duì)應(yīng)關(guān)系，可以得到四元數(shù)的4個(gè)分量與航天器相對(duì)姿態(tài)角θ（俯仰）、ψ（偏航）、φ（滾動(dòng)）的關(guān)系表達(dá)式：

上述分析表明：文獻(xiàn)［7］的本質(zhì)是將一元四次方程的四元數(shù)特征和兩組特征點(diǎn)化為一組遞進(jìn)的一元二次方程組，并經(jīng)優(yōu)化選擇，從而獲得（ty，q0，q1，q2，q3）5個(gè)變量的唯一解。本文將幾何、數(shù)學(xué)和光學(xué)測(cè)量方法相結(jié)合，采用顯式方式求取5個(gè)變量的唯一解，避免了數(shù)值方法的初值選取與收斂性判斷等繁復(fù)不便。

進(jìn)一步，當(dāng)兩航天器距離越來(lái)越接近，即正交比例投影的近似條件不再滿足時(shí)，相關(guān)Ki系數(shù)需要修正，此時(shí)解析方程組形式不變，仍可用上述方程求解。

3 數(shù)值仿真

為了驗(yàn)證本文算法獲取的航天器位姿參數(shù)解析解的有效性，選用MATLAB軟件對(duì)該算法進(jìn)行數(shù)學(xué)仿真。仿真的相關(guān)條件參照文獻(xiàn)［7］進(jìn)行設(shè)置。仿真結(jié)果如圖3、圖4、圖5所示。

目標(biāo)航天器上的特征光點(diǎn)為s i（x i，y i，zi）（i＝1，2，…，5），目標(biāo)坐標(biāo)系中的坐標(biāo)值表達(dá)式為

圖3 修正前的相對(duì)位姿參數(shù)估計(jì)誤差

圖4 修正后的相對(duì)位姿參數(shù)估計(jì)誤差

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

圖5 位置不變時(shí)的相對(duì)位姿參數(shù)誤差

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

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

4 結(jié)語(yǔ)

本文提出的解析算法簡(jiǎn)化了相對(duì)位姿參數(shù)確定問(wèn)題的計(jì)算過(guò)程，計(jì)算量小，計(jì)算速度快，可以滿足航天器交會(huì)對(duì)接單目視覺(jué)測(cè)量的實(shí)時(shí)性要求。通過(guò)數(shù)學(xué)仿真，驗(yàn)證了本文提出的交會(huì)對(duì)接最后階段相對(duì)位姿參數(shù)估計(jì)算法的可行性和有效性。

［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］林來(lái)興，李燦.交會(huì)對(duì)接最后逼近階段CCD相機(jī)的測(cè)量方法［J］.宇航學(xué)報(bào)，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ì)算機(jī)視覺(jué)的航天器間相對(duì)狀態(tài)測(cè)量系統(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］張慶君，胡修林，葉斌，等.基于雙目視覺(jué)測(cè)量的航天器相對(duì)位置和姿態(tài)的測(cè)量方法［J］.宇航學(xué)報(bào)，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］李建鋒，杜小平，張彗星.基于四元數(shù)的航天器間相對(duì)姿態(tài)光學(xué)測(cè)量方法研究［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］徐文福，梁斌，李成，等.基于立體視覺(jué)的航天器相對(duì)位姿測(cè)量方法與仿真研究［J］.宇航學(xué)報(bào)，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］王保豐，李廣云，陳繼華，等.航天器交會(huì)對(duì)接中測(cè)量靶標(biāo)的兩種設(shè)計(jì)方法［J］.宇航學(xué)報(bào)，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ī)器人捕獲非合作目標(biāo)的測(cè)量與規(guī)劃方法［J］.機(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.

（編輯 何成根）