郁 豐，趙 依，汪永生

（南京航空航天大學 航天學院，南京 210016）

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

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

因此，有學者提出了利用故障星幾何形狀來測量相對位姿的方法。Hillenbrand等通過計算機數值模擬產生距離像數據，利用一定的準則和前后時刻的點集，實現了自由漂浮的空間目標的運動估計和模型識別[9]。Lichter提出了一種基于一系列距離像來估計空間物體的狀態(tài)、形狀和慣量參數的估計方法[10]，該方法首先利用距離像數據粗略計算出空間目標的位姿參數，然后利用位姿粗參數估計目標的姿態(tài)、角速度、相對距離和速度等參數，最后利用估計的位姿信息和原始距離像來估計空間目標的形狀。

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

1 基于點云矩形面特征相對位姿解算

人造衛(wèi)星上具有較多矩形面特征的部件，如衛(wèi)星主體、太陽帆板等，幵且對于特定的衛(wèi)星，其矩形面尺寸也是固定的。基于矩形面點云數據可以計算出點云分布矩陣，該值反映了矩形面的尺寸參數，可以用來識別具體的矩形面，幵實現位姿參數測量。本文選擇衛(wèi)星主體上的矩形面迚行位姿解算，首先采用RANSAC算法對目標星迚行點云矩形面的提取，然后計算矩形面的點云分布矩陣，最后通過矩陣的特征值分解計算出相對位姿。

假設通過傳感器觀測到目標平面點云中有m個特征點，則形心在測量坐標系中的矢量為：

其中，ri為第i個特征點在測量坐標系中的坐標。

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

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

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

圖1 矩形面點云坐標系的定義Fig.1 Definition of rectangular point cloud system

根據姿態(tài)矩陣可以計算出矩形面本體系相對于測量坐標系的姿態(tài)四元數q：

由點云特征矩形面的對稱結構可知，在將矩形面繞某一坐標軸旋轉180°后，計算四元數時會因矩形面的對稱性而產生四種情況，其本質是矩陣特征值分解沒有觃定特征值的排列順序導致的，如圖2所示。

圖2 多解問題描述與原理Fig.2 Description and principle of multisolution problem

不妨假定圖2中(a)為當前時刻的坐標系，在下一時刻采用點云特征分解計算時，可能出現如圖2中所示的四種坐標軸分布，判斷方法是用濾波器中的四元數一步預測值與當前時刻測量得到四元數作差：

其中為四元數的差值為四元數的一步預測值。

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

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

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

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

2 相對位置參數濾波器設計

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

由于本文研究范圍為近圓交會對接逼近階段，目標星與追蹤星的相對距離較小，Hill方程具有足夠的精確性。根據Hill方程，在目標星軌道系中建立兩星質心間的相對位置動力學模型為：

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

2.2 觀測方程的建立

在空間近距離接近過程中，兩星的觀測關系如圖3所示。

圖3 兩星觀測關系示意圖Fig.3 Observation between two satellites

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

由圖3得出追蹤星、目標星以及形心間的相對關系如下：

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

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

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

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

其中，為觀測噪聲。

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

3 目標星姿態(tài)濾波器設計

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

目標星為故障衛(wèi)星，角速度無法通過姿態(tài)敏感器直接獲得。根據姿態(tài)動力學方程可知，目標星的姿態(tài)運動學模型為

其中：qt為目標星的姿態(tài)四元數；為角速度；It為轉動慣量；M為目標星所受的外力矩，對于自由翻滾的故障衛(wèi)星可認為是0。

由于四元數的四個參數不獨立，會造成協(xié)方差陣的奇異，所以對式(13)圍繞預測值泰勒展開形成誤差增量方程，幵選取目標星四元數誤差的矢量部分以及目標星的角速度誤差作為狀態(tài)變量，建立如下狀態(tài)方程：

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

3.2 觀測方程的建立

由于追蹤星上的姿態(tài)確定系統(tǒng)可以實時測量追蹤星的姿態(tài)，即認為追蹤星姿態(tài)已知，結合解算出來的相對姿態(tài)，可得出目標星的姿態(tài)：

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

將式(15)線性化幵圍繞預測值泰勒展開后有觀測方程如下：

其中，為目標星四元數誤差的矢量部分。

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

4 仿真實驗與分析

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

仿真中設定追蹤星位于目標星正后方125 m，目標星繞Z軸具有 1 (°)/s的角速度，且轉動慣量矩陣It=diag(30,15, 20) kg?m2，追蹤星三軸穩(wěn)定?；邳c云面特征解算出的目標星追蹤星的相對位姿誤差如圖5～7所示。

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

圖5 相對位置誤差Fig.5 Relative position error

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

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

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

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

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

將圖5和圖7分別與圖8和圖10做比較，可以看出，濾波效果較好，相對位置、目標星姿態(tài)角誤差較原始測量值均有所減小。由圖8～9可知：相對位置速度在很短時間內收斂，收斂后波動很小，通過計算可得三軸誤差的最大值分別為 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 可知：目標星姿態(tài)角雖然存在小的波動，但精度仍然優(yōu)于0.1°，目標星角速度誤差優(yōu)于0.01 (°)/s，具有較好的估計精度，幵且通過計算得出姿態(tài)角誤差的最大值分別為0.06°、0.06°、0.03°，濾波穩(wěn)定后角速度誤差的最大值分別為 0.001 (°)/s、0.001 (°)/s、0.002 (°)/s。

圖8 相對位置誤差Fig.8 Relative position error

圖9 相對速度誤差Fig.9 Relative velocity error

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

圖11 目標星角速度誤差Fig.11 Angular velocity error of target satellite

5 結 論

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

參考文獻（References）：

[1] 梁斌, 杜曉東, 李成. 空間機器人非合作航天器在軌服務研究迚展[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] 曹彩秀. 非合作航天器位姿在軌測量方法的研究[D].北京郵電大學, 2015.

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

[6] 苗錫奎, 朱楓, 郝穎明, 等. 基于太陽能帆板部件的空間非合作飛行器視覺位姿測量方法[J]. 高技術通訊,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] 張丼敏, 朱楓, 郝穎明, 等. 基于圓特征和異面點特征的位姿測量[J]. 光子學報, 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.