基于截?cái)嘧钚《撕桶胝ㄒ?guī)劃的空間非合作目標(biāo)相對(duì)位姿估計(jì)*

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

（1.上海交通大學(xué) 航空航天學(xué)院·上海·200240；2. 上海航天控制技術(shù)研究所·上海·201109；3. 上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室·上海·201109）

0 引 言

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

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

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

與此同時(shí)，一些文獻(xiàn)基于TOF相機(jī)中的點(diǎn)云信息，提取目標(biāo)的3D幾何結(jié)構(gòu)特征，研究了空間非合作目標(biāo)的相對(duì)位置與姿態(tài)估計(jì)方法。現(xiàn)有基于3D點(diǎn)云的相對(duì)位置與姿態(tài)估計(jì)方法可分為兩種：1）基于特征匹配的方法[8-11]；2）基于同時(shí)位姿測(cè)量與匹配的方法[14-19]。基于特征匹配的方法的主要思路是首先檢測(cè)局部或全局特征描述子并匹配3D特征點(diǎn)，然后再使用隨機(jī)抽樣一致性（Random Sample Consensus，RANSAC）算法或是應(yīng)用魯棒配準(zhǔn)來估計(jì)相對(duì)位姿。同時(shí)位姿測(cè)量與匹配的方法思想是在基于特征匹配的配準(zhǔn)以及計(jì)算最優(yōu)相對(duì)位姿之間進(jìn)行切換。然而，這些方法都沒有考慮實(shí)際場(chǎng)景中傳感器噪聲以及目標(biāo)（如立方星等目標(biāo)）幾何結(jié)構(gòu)特性的差異化，因此，這些方法的應(yīng)用潛力有限。

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

1 基于截?cái)嘧钚《说目臻g非合作目標(biāo)相對(duì)位姿估計(jì)

（1）

（2）

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

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

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

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

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

（3）

（4）

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

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

（5）

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

（6）

（7）

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

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

表1 基于截?cái)嘧钚《撕桶胝ㄒ?guī)劃的空間非合作目標(biāo)相對(duì)位姿估計(jì)流程

2.2.1 相對(duì)尺度估計(jì)

首先，基于式（7）使用截?cái)嘧钚《朔椒ǎY(jié)合給定的量測(cè)sij以及上界εij情況，估計(jì)出相對(duì)尺度

（8）

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

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

（9）

2.2.3 相對(duì)位置估計(jì)

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

（10）

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

3 仿真實(shí)驗(yàn)及結(jié)果分析

3.1 仿真測(cè)試實(shí)驗(yàn)條件

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

3.2 相對(duì)位置估計(jì)實(shí)驗(yàn)結(jié)果

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

（a） 斯坦福bunny模型

（b） 風(fēng)云系列航天器模型

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

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

表2 相對(duì)位置估計(jì)精度對(duì)比分析

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

（a） 輸入的點(diǎn)云數(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 相對(duì)姿態(tài)估計(jì)實(shí)驗(yàn)結(jié)果

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

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

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

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

（a） 斯坦福bunny模型的點(diǎn)云

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

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

4 結(jié) 論

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