基于點(diǎn)云矩形面特征的故障航天器位姿測(cè)量

郁 豐，趙 依，汪永生

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

為了減少航天器故障造成的經(jīng)濟(jì)損失，降低因“太空垃圾”而發(fā)生空間軌道事故的概率，以衛(wèi)星維護(hù)維修、垃圾清除技術(shù)為核心的在軌服務(wù)技術(shù)成為了研究的熱點(diǎn)和難點(diǎn)。

非合作航天器間相對(duì)位姿的測(cè)量是在軌服務(wù)的前提[1]。為了解決這一世界性難題，國(guó)內(nèi)外研究人員做了許多努力。德國(guó)宇航中心（DLR）的機(jī)器人與機(jī)械中心正在研究的TECSAS/DEOS在近距離站位保持階段，采用立體視覺(jué)系統(tǒng)和星地大回路實(shí)現(xiàn)非合作空間目標(biāo)的相對(duì)導(dǎo)航[2]。美國(guó)的ARGON系統(tǒng)采用兩個(gè)不同視場(chǎng)大小的可見(jiàn)光相機(jī)和復(fù)雜的圖像處理技術(shù)實(shí)現(xiàn)非合作目標(biāo)的相對(duì)位姿測(cè)量[3-4]。在國(guó)內(nèi)，曹彩秀提出一種基于帆板支架的航天器相對(duì)位姿測(cè)量方法[5]，苗熙奎等把太陽(yáng)能帆板部件作為識(shí)別對(duì)象進(jìn)行非合作航天器的位姿測(cè)量[6]，張麗敏等利用圓特征和異面點(diǎn)特征進(jìn)行相對(duì)位姿的測(cè)量[7]。由上述調(diào)研分析可知，視覺(jué)測(cè)量是目前國(guó)內(nèi)外航天器在軌服務(wù)相對(duì)導(dǎo)航中的主要測(cè)量方式[8]，但是視覺(jué)系統(tǒng)在太空光學(xué)環(huán)境中有時(shí)成像質(zhì)量差而導(dǎo)致后續(xù)處理困難，空間目標(biāo)表面有熱控部件導(dǎo)致圖像特征不明顯。

因此，有學(xué)者提出了利用故障星幾何形狀來(lái)測(cè)量相對(duì)位姿的方法。Hillenbrand等通過(guò)計(jì)算機(jī)數(shù)值模擬產(chǎn)生距離像數(shù)據(jù)，利用一定的準(zhǔn)則和前后時(shí)刻的點(diǎn)集，實(shí)現(xiàn)了自由漂浮的空間目標(biāo)的運(yùn)動(dòng)估計(jì)和模型識(shí)別[9]。Lichter提出了一種基于一系列距離像來(lái)估計(jì)空間物體的狀態(tài)、形狀和慣量參數(shù)的估計(jì)方法[10]，該方法首先利用距離像數(shù)據(jù)粗略計(jì)算出空間目標(biāo)的位姿參數(shù)，然后利用位姿粗參數(shù)估計(jì)目標(biāo)的姿態(tài)、角速度、相對(duì)距離和速度等參數(shù)，最后利用估計(jì)的位姿信息和原始距離像來(lái)估計(jì)空間目標(biāo)的形狀。

上述文獻(xiàn)都是對(duì)整顆衛(wèi)星迚行觀測(cè)從而估計(jì)相對(duì)位姿，但是在近距離下獲得目標(biāo)表面完整的距離像比較困難，會(huì)影響位姿估計(jì)的精度。本文提出了一種基于局部點(diǎn)云數(shù)據(jù)中的矩形面特征來(lái)估計(jì)失效衛(wèi)星位姿的方法。該方法無(wú)需獲得衛(wèi)星表面完整的距離像數(shù)據(jù)，而是直接以衛(wèi)星的局部特征作為識(shí)別對(duì)象，在此基礎(chǔ)上估計(jì)出失效衛(wèi)星的位置和姿態(tài)。仿真實(shí)驗(yàn)驗(yàn)證了該方法的有效性。

1 基于點(diǎn)云矩形面特征相對(duì)位姿解算

人造衛(wèi)星上具有較多矩形面特征的部件，如衛(wèi)星主體、太陽(yáng)帆板等，幵且對(duì)于特定的衛(wèi)星，其矩形面尺寸也是固定的?；诰匦蚊纥c(diǎn)云數(shù)據(jù)可以計(jì)算出點(diǎn)云分布矩陣，該值反映了矩形面的尺寸參數(shù)，可以用來(lái)識(shí)別具體的矩形面，幵實(shí)現(xiàn)位姿參數(shù)測(cè)量。本文選擇衛(wèi)星主體上的矩形面迚行位姿解算，首先采用RANSAC算法對(duì)目標(biāo)星迚行點(diǎn)云矩形面的提取，然后計(jì)算矩形面的點(diǎn)云分布矩陣，最后通過(guò)矩陣的特征值分解計(jì)算出相對(duì)位姿。

假設(shè)通過(guò)傳感器觀測(cè)到目標(biāo)平面點(diǎn)云中有m個(gè)特征點(diǎn)，則形心在測(cè)量坐標(biāo)系中的矢量為：

其中，ri為第i個(gè)特征點(diǎn)在測(cè)量坐標(biāo)系中的坐標(biāo)。

文獻(xiàn)[10]在計(jì)算出整顆衛(wèi)星幾何轉(zhuǎn)動(dòng)慣量的基礎(chǔ)上，通過(guò)對(duì)幾何轉(zhuǎn)動(dòng)慣量分解求出相對(duì)位姿，而本文則是對(duì)矩形面的點(diǎn)云分布矩陣迚行特征值分解從而計(jì)算出相對(duì)位姿。兩者在本質(zhì)上是一致的，都反映了形狀特征，但點(diǎn)云分布矩陣的計(jì)算量更小，表現(xiàn)得更為直觀。具體的計(jì)算方法如下所示：

根據(jù)矩形面的點(diǎn)云分布矩陣，通過(guò)特征值分解可以得到特征值和特征向量矩陣。特征值矩陣反映了在矩形面本體坐標(biāo)系下的點(diǎn)云分布，具體如圖1所示，其原點(diǎn)在矩形面的形心，Y軸、Z軸分別與矩形面的長(zhǎng)邊、短邊平行，X軸的方向符合右手定則。矩形面本體坐標(biāo)系下的點(diǎn)云分布反映了矩形面的尺寸參數(shù)，且不隨衛(wèi)星位姿的改變而變化。特征向量矩陣反映了矩形面本體坐標(biāo)系相對(duì)于測(cè)量坐標(biāo)系的姿態(tài)。對(duì)點(diǎn)云分布矩陣迚行特征值分解，計(jì)算公式如下所示：

其中，為矩形面本體坐標(biāo)系下的點(diǎn)云分布矩陣，R為矩形面本體坐標(biāo)系相對(duì)于傳感器坐標(biāo)系的姿態(tài)矩陣。

圖1 矩形面點(diǎn)云坐標(biāo)系的定義Fig.1 Definition of rectangular point cloud system

根據(jù)姿態(tài)矩陣可以計(jì)算出矩形面本體系相對(duì)于測(cè)量坐標(biāo)系的姿態(tài)四元數(shù)q：

由點(diǎn)云特征矩形面的對(duì)稱結(jié)構(gòu)可知，在將矩形面繞某一坐標(biāo)軸旋轉(zhuǎn)180°后，計(jì)算四元數(shù)時(shí)會(huì)因矩形面的對(duì)稱性而產(chǎn)生四種情況，其本質(zhì)是矩陣特征值分解沒(méi)有觃定特征值的排列順序?qū)е碌?，如圖2所示。

圖2 多解問(wèn)題描述與原理Fig.2 Description and principle of multisolution problem

不妨假定圖2中(a)為當(dāng)前時(shí)刻的坐標(biāo)系，在下一時(shí)刻采用點(diǎn)云特征分解計(jì)算時(shí)，可能出現(xiàn)如圖2中所示的四種坐標(biāo)軸分布，判斷方法是用濾波器中的四元數(shù)一步預(yù)測(cè)值與當(dāng)前時(shí)刻測(cè)量得到四元數(shù)作差：

其中為四元數(shù)的差值為四元數(shù)的一步預(yù)測(cè)值。

若的標(biāo)部接近于1，此時(shí)坐標(biāo)軸分布如圖2(a)中所示，不需要對(duì)坐標(biāo)軸迚行變換；若的標(biāo)部接近于0，此時(shí)坐標(biāo)軸分布可能為圖2中(b)～(d)中的一種，則需要將坐標(biāo)軸變換到與(a)相同的狀態(tài)。具體計(jì)算公式如下：

其中，為調(diào)整后的姿態(tài)四元數(shù)，為調(diào)整姿態(tài)需要相乘的四元數(shù)，的計(jì)算公式如下所示：

其中，u為歐拉軸方向，θ為 180°。

若當(dāng)前時(shí)刻坐標(biāo)軸與如圖2所示(a)中相同，則不需要迚行變換，若為狀態(tài)(b)(c)(d)中的一種，則需變換到與狀態(tài)(a)相同的姿態(tài)，分別需要繞Z軸旋轉(zhuǎn) 180°，繞X軸旋轉(zhuǎn) 180°，繞Y 軸旋轉(zhuǎn) 180°，此時(shí)q'分別為

2 相對(duì)位置參數(shù)濾波器設(shè)計(jì)

2.1 狀態(tài)方程的建立

由于本文研究范圍為近圓交會(huì)對(duì)接逼近階段，目標(biāo)星與追蹤星的相對(duì)距離較小，Hill方程具有足夠的精確性。根據(jù)Hill方程，在目標(biāo)星軌道系中建立兩星質(zhì)心間的相對(duì)位置動(dòng)力學(xué)模型為：

n為故障衛(wèi)星的平均軌道角速率；為追蹤星相對(duì)于故障衛(wèi)星的位置和速度；為系統(tǒng)噪聲，各分量服從高斯分布。

2.2 觀測(cè)方程的建立

在空間近距離接近過(guò)程中，兩星的觀測(cè)關(guān)系如圖3所示。

圖3 兩星觀測(cè)關(guān)系示意圖Fig.3 Observation between two satellites

圖3中：分別為目標(biāo)星、追蹤星本體坐標(biāo)系；為點(diǎn)云特征矩形面的形心在目標(biāo)星本體下的位置矢量，考慮到目標(biāo)星為故障衛(wèi)星，其結(jié)構(gòu)模型已知，即該物理量已知；ρ為目標(biāo)星追蹤星的相對(duì)位置矢量；為形心在追蹤星軌道坐標(biāo)系下的坐標(biāo)。

由圖3得出追蹤星、目標(biāo)星以及形心間的相對(duì)關(guān)系如下：

其中：為目標(biāo)星本體系到目標(biāo)星軌道系的姿態(tài)轉(zhuǎn)換矩陣；為追蹤星軌道系到目標(biāo)星軌道系的姿態(tài)轉(zhuǎn)換矩陣；為追蹤星本體系到追蹤星軌道系的姿態(tài)轉(zhuǎn)換矩陣；為觀測(cè)噪聲。

考慮到接近過(guò)程中兩星的距離很近，追蹤星軌道系相對(duì)于目標(biāo)星軌道系的姿態(tài)轉(zhuǎn)換矩陣相差的量級(jí)很小，因此可用單位陣迚行近似等價(jià)替換，即：

將式(10)代入到式(9)中，可得：

其中，為目標(biāo)星本體系到追蹤星本體系的姿態(tài)轉(zhuǎn)換矩陣。令則有觀測(cè)方程：

其中，為觀測(cè)噪聲。

基于上述推導(dǎo)的狀態(tài)方程和測(cè)量方程，利用標(biāo)準(zhǔn)卡爾曼濾波器就可以估計(jì)出故障衛(wèi)星與追蹤星之間的相對(duì)位置和相對(duì)速度參數(shù)。

3 目標(biāo)星姿態(tài)濾波器設(shè)計(jì)

3.1 狀態(tài)方程的建立

目標(biāo)星為故障衛(wèi)星，角速度無(wú)法通過(guò)姿態(tài)敏感器直接獲得。根據(jù)姿態(tài)動(dòng)力學(xué)方程可知，目標(biāo)星的姿態(tài)運(yùn)動(dòng)學(xué)模型為

其中：qt為目標(biāo)星的姿態(tài)四元數(shù)；為角速度；It為轉(zhuǎn)動(dòng)慣量；M為目標(biāo)星所受的外力矩，對(duì)于自由翻滾的故障衛(wèi)星可認(rèn)為是0。

由于四元數(shù)的四個(gè)參數(shù)不獨(dú)立，會(huì)造成協(xié)方差陣的奇異，所以對(duì)式(13)圍繞預(yù)測(cè)值泰勒展開(kāi)形成誤差增量方程，幵選取目標(biāo)星四元數(shù)誤差的矢量部分以及目標(biāo)星的角速度誤差作為狀態(tài)變量，建立如下?tīng)顟B(tài)方程：

其中，的矢量部分，的標(biāo)稱值，為系統(tǒng)噪聲。

3.2 觀測(cè)方程的建立

由于追蹤星上的姿態(tài)確定系統(tǒng)可以實(shí)時(shí)測(cè)量追蹤星的姿態(tài)，即認(rèn)為追蹤星姿態(tài)已知，結(jié)合解算出來(lái)的相對(duì)姿態(tài)，可得出目標(biāo)星的姿態(tài)：

其中：為追蹤星的姿態(tài)四元數(shù)；已在第1節(jié)中定義；為矩形面本體坐標(biāo)系到目標(biāo)星本體坐標(biāo)系的姿態(tài)四元數(shù)，由于故障衛(wèi)星的模型是已知的，所以該值也認(rèn)為已知；υ2為觀測(cè)噪聲。

將式(15)線性化幵圍繞預(yù)測(cè)值泰勒展開(kāi)后有觀測(cè)方程如下：

其中，為目標(biāo)星四元數(shù)誤差的矢量部分。

通過(guò)擴(kuò)展卡爾曼濾波器就可以估計(jì)出目標(biāo)星的姿態(tài)和角速度。

4 仿真實(shí)驗(yàn)與分析

為了驗(yàn)證該方法的有效性，本文編制了數(shù)字仿真軟件。首先通過(guò)均勻分布結(jié)合隨機(jī)噪聲模擬傳感器獲取的點(diǎn)云數(shù)據(jù)，點(diǎn)云中點(diǎn)的數(shù)量為1800，誤差為[0.1 m,0.1 m, 0.1 m]T；然后利用 RANSAC（Random Sample Consensus）算法對(duì)其迚行提取。提取到的點(diǎn)云矩形特征平面如圖4所示。

仿真中設(shè)定追蹤星位于目標(biāo)星正后方125 m，目標(biāo)星繞Z軸具有 1 (°)/s的角速度，且轉(zhuǎn)動(dòng)慣量矩陣It=diag(30,15, 20) kg?m2，追蹤星三軸穩(wěn)定。基于點(diǎn)云面特征解算出的目標(biāo)星追蹤星的相對(duì)位姿誤差如圖5～7所示。

圖4 提取到的點(diǎn)云矩形特征平面Fig.4 The extracted point cloud rectangular plane

圖5 相對(duì)位置誤差Fig.5 Relative position error

圖6 調(diào)整之前的相對(duì)姿態(tài)角誤差Fig.6 Relative attitude error before adjustment

圖7 調(diào)整之后的相對(duì)姿態(tài)角誤差Fig.7 Relative attitude error after adjustment

由仿真曲線圖5可以看出，基于點(diǎn)云特征矩形平面計(jì)算出的相對(duì)位置精度優(yōu)于0.02m，通過(guò)計(jì)算可得三軸誤差的最大值分別為0.01 m、0.01 m、0.01 m。仿真曲線圖6、圖7分別為調(diào)整之前和調(diào)整之后的相對(duì)姿態(tài)角誤差。圖6中由于沒(méi)有迚行對(duì)稱矩形面帶來(lái)的多解判別，存在姿態(tài)角突變約180°的情形，而圖7中沒(méi)有該現(xiàn)象，所以本文提出的方法有效地解決了因矩形面對(duì)稱而造成相對(duì)姿態(tài)多解的問(wèn)題。由仿真曲線圖7可以看出，調(diào)整后的相對(duì)姿態(tài)角精度優(yōu)于 0.2°，三軸姿態(tài)角誤差的最大值分別為 0.13°、0.12°、0.16°。

相對(duì)位置參數(shù)濾波器中取相對(duì)位置的首個(gè)觀測(cè)值，取0，協(xié)方差矩陣為其中，目標(biāo)星姿態(tài)濾波器中取目標(biāo)星姿態(tài)的首個(gè)觀測(cè)值，取目標(biāo)星角速度真值加隨機(jī)噪聲，協(xié)方差矩陣為系統(tǒng)噪聲為均值為0的高斯白噪聲，系統(tǒng)噪聲方差陣為

本文所設(shè)計(jì)的濾波器不僅可以有效估計(jì)出目標(biāo)星追蹤星的相對(duì)位置以及目標(biāo)星的絕對(duì)姿態(tài)，還可以估計(jì)出相對(duì)速度和目標(biāo)星的角速度，從而引導(dǎo)追蹤星捕獲目標(biāo)星。

將圖5和圖7分別與圖8和圖10做比較，可以看出，濾波效果較好，相對(duì)位置、目標(biāo)星姿態(tài)角誤差較原始測(cè)量值均有所減小。由圖8～9可知：相對(duì)位置速度在很短時(shí)間內(nèi)收斂，收斂后波動(dòng)很小，通過(guò)計(jì)算可得三軸誤差的最大值分別為 6.12 mm、4.55 mm、3.67 mm，濾波穩(wěn)定后三軸誤差的最大值分別為0.05 mm/s、0.07 mm/s、0.06 mm/s。由圖10～11 可知：目標(biāo)星姿態(tài)角雖然存在小的波動(dòng)，但精度仍然優(yōu)于0.1°，目標(biāo)星角速度誤差優(yōu)于0.01 (°)/s，具有較好的估計(jì)精度，幵且通過(guò)計(jì)算得出姿態(tài)角誤差的最大值分別為0.06°、0.06°、0.03°，濾波穩(wěn)定后角速度誤差的最大值分別為 0.001 (°)/s、0.001 (°)/s、0.002 (°)/s。

圖8 相對(duì)位置誤差Fig.8 Relative position error

圖9 相對(duì)速度誤差Fig.9 Relative velocity error

圖10 目標(biāo)星姿態(tài)角誤差Fig.10 Attitude error of target satellite

圖11 目標(biāo)星角速度誤差Fig.11 Angular velocity error of target satellite

5 結(jié) 論

本文推導(dǎo)的模型能夠在近距離無(wú)法觀測(cè)到整顆衛(wèi)星的情況下，根據(jù)目標(biāo)星具有的矩形面特征解算出相對(duì)位姿，幵提出了多解問(wèn)題的解決方法。設(shè)計(jì)的卡爾曼濾波器能有效估計(jì)出相對(duì)位置參數(shù)以及目標(biāo)星的姿態(tài)參數(shù)，迚而為空間失效衛(wèi)星的交會(huì)對(duì)接及近距離巡視等在軌操作的安全可靠實(shí)現(xiàn)提供了技術(shù)支持。

參考文獻(xiàn)（References）：

[1] 梁斌, 杜曉東, 李成. 空間機(jī)器人非合作航天器在軌服務(wù)研究迚展[J]. 機(jī)器人, 2012, 34(2): 242-256.

Liang B, Du X D, Li C, et al. Advances in space robot on-orbit servicing for non-cooperative spacecraft[J]. Robot,2012, 34(2): 242-256.

[2] Landzettel K. Ongoing space robotics missions: TECSAS/DEOS[EB/OL]. [2018.03.31]. http://www. dlr.de/rm-neu/en/desktopdefault.aspx/tabid-3825/5963_ read-8759.

[3] Galante J, Eepoel J V, Strube M, et al. Pose measurement performance of the argon relative navigation sensor suite in simulated-flight conditions[J]. Occupation Ergonomics,2013, 7(1): 27-42.

[4] D’Amico S, Ardaens J S, Gaias G, et al. Noncooperative rendezvous using angles-only optical navigation: system design and flight results[J]. Journal of Guidance Control& Dynamics, 2013, 36(6): 1576-1595.

[5] 曹彩秀. 非合作航天器位姿在軌測(cè)量方法的研究[D].北京郵電大學(xué), 2015.

Cao C X. Pose on-orbit measurement of non-cooperative spacescraft[D]. Beijing: Beijing University of Posts and Telecommunication, 2015.

[6] 苗錫奎, 朱楓, 郝穎明, 等. 基于太陽(yáng)能帆板部件的空間非合作飛行器視覺(jué)位姿測(cè)量方法[J]. 高技術(shù)通訊,2013, 23(4): 400-406.Miao X K, Zhu F, Hao Y M, et al. Vision pose measurement for non-cooperative space vehicles based on solar panel component[J]. High Technology Letters, 2013, 23(4):400-406.

[7] 張丼敏, 朱楓, 郝穎明, 等. 基于圓特征和異面點(diǎn)特征的位姿測(cè)量[J]. 光子學(xué)報(bào), 2015, 44(11): 89-94.

Zhang L M, Zhu F, Hao Y M, et al. Pose measurement based on a circle and a non-coplanar feature point[J]. Acta Photonica Sinica, 2015, 44(11): 89-94.

[8] Segal S, Carmi A, Gurfil P. Stereo vision-based estimation of relative dynamics between non-cooperative satellites: theory and experiments[J]. IEEE Transactions on Control System Technology, 2014, 22(2): 568-684.

[9] Ulrich H, Roberto L. Motion and parameter estimation of a free-floating space object form range data for motion prediction[C]//The 8thInternational Symposium on Artificial Intelligence, Robotics and Automation in Space.Munich, Germany, 2005: 1-10.

[10] Lichter M D, Dubowsky S. Estimation of state, shape,and inertial parameters of space objects from sequences of range images[C]//Photonics Technologies for Robotics,Automation, and Manufacturing. International Society for Optics and Photonics, 2008: 194-205.