基于截斷最小二乘和半正定規(guī)劃的空間非合作目標相對位姿估計*

潘 漢, 曹姝清, 武海雷, 敬忠良, 頓向明

（1.上海交通大學 航空航天學院·上?！?00240；2. 上海航天控制技術(shù)研究所·上?！?01109；3. 上海市空間智能控制技術(shù)重點實驗室·上?！?01109）

0 引 言

在軌服務(wù)是指在空間中通過人、空間機器人或者兩者協(xié)同工作的方式，實現(xiàn)各類航天器在復(fù)雜空間環(huán)境下長時間穩(wěn)定運行的空間操作[1-3]。具體來說，在軌服務(wù)一般包含在軌維護、在軌加注、在軌組裝、模塊更換等。美國、歐盟國家、日本等航天強國，以在軌演示驗證項目需求為牽引，驗證在軌服務(wù)技術(shù)，如軌道快車[4]、鳳凰計劃[5]等。一些發(fā)達國家研制了相關(guān)的航天器，如德國的“試驗服務(wù)衛(wèi)星”和“軌道壽命延長飛行器”等。國內(nèi)的在軌服務(wù)技術(shù)，在優(yōu)勢單位（航天五院502所、航天八院803所、上海交通大學、哈爾濱工業(yè)大學、天津大學等）的大力攻關(guān)下，也取得了長足的發(fā)展[6-7]。然而，上述的在軌演示驗證項目、空間機器人大部分面向合作目標開展關(guān)鍵技術(shù)攻關(guān)，無法處理空間非合作目標。具體來說，空間非合作目標是指無應(yīng)答、無標識的空間目標，如失效的翻滾衛(wèi)星、空間碎片等。針對空間非合作目標，空間機器人自主感知是在軌服務(wù)的核心關(guān)鍵技術(shù)之一?？紤]到空間復(fù)雜光照環(huán)境、非合作目標相對位置、姿態(tài)與尺度變化強耦合等因素，使得空間非合作目標相對位姿估計比較困難。因此，迫切需要開展面向空間非合作目標的空間機器人自主感知技術(shù)研究。

空間非合作目標相對位姿估計是空間機器人自主感知的核心關(guān)鍵技術(shù)之一[7]。該技術(shù)通過多傳感器測量并估計出空間非合作目標相對于服務(wù)航天器的相對位置、相對姿態(tài)、相對速度以及相對角速度等。常用的視覺傳感器有：單目相機（Monocular camera）、雙目相機（Stereo camera）、掃描式激光雷達（Scanning Lidar）、閃光式激光雷達（Flash Lidar），以及飛行時間（Time-of-Flight，TOF）相機。這些傳感器都有各自的優(yōu)缺點，在目標形態(tài)測量、3D重建以及相對運動學與動力學分析等應(yīng)用場景中得到了應(yīng)用[7]。目前，已有相關(guān)的研究團隊設(shè)計了新的傳感器以適應(yīng)空間復(fù)雜光場環(huán)境的不利影響，如過曝、視場角較小、衛(wèi)星表面覆蓋層帶來的散射光影響等[8-11]。然而，空間操作任務(wù)的復(fù)雜性，使得視覺傳感器在軌跡規(guī)劃、逼近過程、姿態(tài)控制、制導(dǎo)等方面的應(yīng)用與信息處理也變得復(fù)雜[12]。例如，在軌跡規(guī)劃過程中就需要考慮視覺傳感器的視場角指向以及己方航天器的動力約束等因素。

此外，空間非合作目標的相對運動學與動力學的復(fù)雜性，亦使得空間非合作目標相對位姿估計比較困難。具體來說，服務(wù)航天器無法獲得或只能獲得較少的空間非合作目標的先驗知識，如尺寸、大小、旋轉(zhuǎn)角度及旋轉(zhuǎn)軸等參數(shù)，從而導(dǎo)致服務(wù)航天器的多傳感器無法準確測量并估計?，F(xiàn)有的文獻基于即時定位與地圖構(gòu)建（Simul-taneous Localization and Mapping，SLAM）技術(shù)框架[12-13]，研究空間非合作目標的3D建模與狀態(tài)估計，并取得了一定的進展。但是，這些工作大部分使用單目相機或是雙目相機，易受復(fù)雜光場的影響，因此應(yīng)用場景受限。

與此同時，一些文獻基于TOF相機中的點云信息，提取目標的3D幾何結(jié)構(gòu)特征，研究了空間非合作目標的相對位置與姿態(tài)估計方法。現(xiàn)有基于3D點云的相對位置與姿態(tài)估計方法可分為兩種：1）基于特征匹配的方法[8-11]；2）基于同時位姿測量與匹配的方法[14-19]?；谔卣髌ヅ涞姆椒ǖ闹饕悸肥鞘紫葯z測局部或全局特征描述子并匹配3D特征點，然后再使用隨機抽樣一致性（Random Sample Consensus，RANSAC）算法或是應(yīng)用魯棒配準來估計相對位姿。同時位姿測量與匹配的方法思想是在基于特征匹配的配準以及計算最優(yōu)相對位姿之間進行切換。然而，這些方法都沒有考慮實際場景中傳感器噪聲以及目標（如立方星等目標）幾何結(jié)構(gòu)特性的差異化，因此，這些方法的應(yīng)用潛力有限。

本文面向在軌服務(wù)任務(wù)需求，針對空間光場復(fù)雜、非合作目標尺度變化及相對姿態(tài)與位置強耦合等難題，基于TOF相機產(chǎn)生的點云信息，充分利用空間目標的典型幾何特征（如快速點特征直方圖（Fast Point Feature Histograms，F(xiàn)PFH）[20]）進行匹配,將相對位姿估計過程抽象為基于截斷最小二乘（Truncated Least Squares，TLS）的最小化問題，通過對相對位置和相對姿態(tài)進行解耦，提取FPFH特征[20]，構(gòu)造基于半正定規(guī)劃的相對姿態(tài)估計方法。通過使用非合作目標的3D模型進行仿真，驗證了該方法在不同強度的噪聲特性下具有較好的估計特性，可以為后續(xù)的任務(wù)實施提供理論與技術(shù)支撐。

1 基于截斷最小二乘的空間非合作目標相對位姿估計

（1）

（2）

需要注意的是，式（2）不對輸入的異常點直接建模，而是設(shè)置了一個上界。實際上，在應(yīng)用過程中可以發(fā)現(xiàn)無法對異常點有效地建模。因此，使用一個上界解決對異常點進行描述的問題是一種簡便且可行的方法。

2 空間非合作目標的相對尺度、姿態(tài)與位置的解耦與估計

2.1 相對尺度、姿態(tài)與位置的解耦

本節(jié)基于式（2），給出了空間非合作目標的相對尺度、姿態(tài)與位置的解耦過程[21]，實現(xiàn)了空間非合作目標的相對位姿估計。通過觀察可以發(fā)現(xiàn)，Pb與相對位置t相關(guān)，也就是說，相對位置具有不變性。具體來說，給定兩個點bi和bj，根據(jù)式（2），可得這兩個點的相對位置為

bi-bj=sR（ai-aj）+（zi-zj）+（φi-φj）

（3）

（4）

其中，zij=zi-zj具有類似的定義。

更進一步地，還可以得到位置與姿態(tài)的不變性。通過觀測式（4）可知，該式仍然與姿態(tài)R相關(guān)，因此，首先計算一個相對位置不變向量的范數(shù)，也就是

（5）

對于內(nèi)點來說，zij=0，且滿足式（6）

（6）

（7）

2.2 相對位置、尺度與姿態(tài)的估計

這里給出空間非合作目標相對位姿估計的主要流程，如表1所示。對于大部分異常點，本文使用最大團選擇法[22]（Maximal Clique Selection）。最大團選擇法[23]的思想是首先進行體素下采樣，獲得一個圖g（v,ζ）。其中，v是圖的頂點，ζ是圖中邊的集合。然后，利用該方法計算圖的最大團g（v,ζ′）（最大團對應(yīng)的邊的集合）。

表1 基于截斷最小二乘和半正定規(guī)劃的空間非合作目標相對位姿估計流程

2.2.1 相對尺度估計

首先，基于式（7）使用截斷最小二乘方法，結(jié)合給定的量測sij以及上界εij情況，估計出相對尺度

（8）

其中，K代表具有不變性的量測；εk代表給定內(nèi)點噪聲的上限值。此外，sk=sij。

2.2.2 相對姿態(tài)估計

（9）

2.2.3 相對位置估計

在分別得到了相對尺度與相對姿態(tài)后，可以將這兩個參數(shù)代入式（2），估計相對位置t，則可以得到

（10）

其中，χi代表給定的內(nèi)點噪聲的上限值。

3 仿真實驗及結(jié)果分析

3.1 仿真測試實驗條件

本文使用BlenSor[24-25]對TOF相機進行仿真。使用了斯坦福bunny、風云系列航天器和立方星（cubsat）的CAD模型渲染圖，如圖1所示。追蹤航天器觀測空間非合作目標的仿真軌跡，如圖2所示。其中，追蹤航天器螺旋逼近目標，目標自旋角速度5（°）/s。TOF相機的分辨率為640×570。整個仿真的實驗平臺采用Ubuntu 16.04操作系統(tǒng)，8G內(nèi)存，4.7Hz Intel CPU。

3.2 相對位置估計實驗結(jié)果

本節(jié)在斯坦福bunny、風云系列航天器和立方星CAD模型的基礎(chǔ)上，開展相對位置估計精度對比分析實驗。實驗結(jié)果如表2所示。其中，斯坦福bunny、風云系列航天器、立方星CAD模型的期望相對位置為（x,y,z）=（5, 5, 5），單位為m。具體來說，在仿真過程中，對上述CAD模型進行剛體變換，產(chǎn)生位置偏移，也就是相對平移。輸入高斯噪聲強度均值都為0，方差分別是0.01, 0.05, 0.1。本文使用估計值與期望相對位置值的L2范數(shù)來度量相對位置估計精度。表2中，符號“—”代表算法崩潰，無法產(chǎn)生相對位置估計值，標注為粗體的數(shù)字代表較好的結(jié)果。本文所比較的相對位置估計算法是Open3D框架中的迭代最近點（Iterative Closest Point，ICP）算法[26]。由表2可知，所提出的算法具有較好的相對位置估計精度。

（a） 斯坦福bunny模型

（b） 風云系列航天器模型

（c） 立方星模型圖1 3個CAD模型的渲染圖Fig.1 Effect drawing of the three CAD models

圖2 航天器觀測空間非合作目標的仿真軌跡Fig.2 Simulation trajectory of spacecraft observing space non-cooperative targets

表2 相對位置估計精度對比分析

圖3和圖4所示為立方星模型匹配過程結(jié)果圖。圖3（a）給出了輸入的點云數(shù)據(jù)，其中綠色代表立方星的原始點云，紅色代表加了噪聲（強度為0.1）以及相對位移與相對姿態(tài)變換后的點云。圖3（b）給出了基于FPFH的特征匹配結(jié)果，其中，藍色的線代表成功匹配后的結(jié)果?；诘玫降南鄬ξ恢门c相對姿態(tài)，可以得到最終的匹配結(jié)果，如圖4所示，綠色代表原模型的點云，紅色代表仿真給出的點云。實驗結(jié)果表明，本文算法具有較好的相對位置估計性能。

（a） 輸入的點云數(shù)據(jù)

（b） 基于FPFH的特征匹配結(jié)果圖3 立方星模型的特征匹配結(jié)果圖Fig.3 The feature matched results on cubsat model

圖4 立方星模型的匹配結(jié)果圖Fig.4 The matched results on cubsat model

3.3 相對姿態(tài)估計實驗結(jié)果

為了驗證相對姿態(tài)估計算法的有效性，本文基于斯坦福bunny、風云系列航天器、立方星CAD模型開展相對姿態(tài)估計精度實驗。實驗結(jié)果如表3所示。其中，斯坦福bunny、風云系列航天器、立方星CAD模型的期望相對姿態(tài)，以旋轉(zhuǎn)矩陣的形式表示為

其中，相對姿態(tài)的單位是rad，輸入的高斯噪聲強度分別為0.001, 0.005, 0.1。本文使用估計值與期望相對姿態(tài)值的絕對值最大值來度量相對姿態(tài)的估計精度。表3中，符號“—”代表算法崩潰，無法產(chǎn)生相對姿態(tài)估計值，標注為粗體的數(shù)字代表較好的結(jié)果。采用Open3D框架中的ICP算法完成相對姿態(tài)估計，實驗結(jié)果表明，所提出的算法具有較好的相對姿態(tài)估計精度。

表3 相對姿態(tài)估計精度對比分析

最后，斯坦福bunny模型的匹配結(jié)果如圖5所示。圖5（a）給出了斯坦福bunny模型的點云。圖5（b）給出了輸入的點云數(shù)據(jù)，其中綠色代表立方星的原始點云，紅色代表加了噪聲（強度為0.1）以及相對位移與相對姿態(tài)變換后的點云。圖5（c）給出了基于FPFH的特征匹配結(jié)果，其中，黑色的線代表成功匹配后的結(jié)果。

（a） 斯坦福bunny模型的點云

（b） 輸入的點云數(shù)據(jù)

（c） 基于FPFH的特征匹配結(jié)果圖5 斯坦福bunny模型的匹配結(jié)果圖Fig.5 The matched results on Stanford bunny model

4 結(jié) 論

本文面向空間在軌服務(wù)，針對空間非合作目標中尺度、姿態(tài)與位置的強耦合特性，挖掘點云信息的內(nèi)在特點，采用FPFH特征，提出了基于截斷最小二乘的空間非合作目標相對位姿估計目標函數(shù)。其中，相對姿態(tài)的求解采用基于半正定規(guī)劃的求解方法。本文通過TOF相機仿真，基于斯坦福bunny、風云系列航天器、立方星CAD模型，模擬了航天器逼近空間非合作目標的過程。仿真實驗結(jié)果驗證了本文算法的有效性，可為后續(xù)的工程應(yīng)用提供理論與技術(shù)支撐。