康國(guó)華，馬云，喬思元，郭玉潔，金晨迪

南京航空航天大學(xué)微小衛(wèi)星研究中心，南京210016

失效航天器的位姿估計(jì)技術(shù)是當(dāng)前在軌服務(wù)的研究熱點(diǎn)。失效航天器屬于非合作目標(biāo)，通常沒有先驗(yàn)的信息、可供辨識(shí)或捕獲的機(jī)構(gòu);沒有姿態(tài)控制能力，在重力梯度、光壓等空間攝動(dòng)力作用下處于翻滾狀態(tài);因碰撞等故障，在周圍通常伴有碎片等干擾。在這種空間場(chǎng)景下，服務(wù)航天器需要估計(jì)出失效航天器的相對(duì)位姿信息，對(duì)環(huán)境進(jìn)行探測(cè)和建模，從而為后期的導(dǎo)航、控制等在軌服務(wù)提供信息，具有重要的研究意義［1］。

上述情形可以簡(jiǎn)單描述為在未知環(huán)境中，任務(wù)主體自身處于運(yùn)動(dòng)的同時(shí)，開展一系列導(dǎo)航任務(wù)。這類難題，在地面移動(dòng)機(jī)器人和無(wú)人機(jī)領(lǐng)域已經(jīng)獲得廣泛而深入的研究，它可以采用同步定位與環(huán)境構(gòu)建(Simultaneous Localization and Mapping，SLAM)技術(shù)來(lái)解決［2-5］，即利用傳感器不斷地采集外界環(huán)境數(shù)據(jù)，在移動(dòng)中通過數(shù)據(jù)處理實(shí)現(xiàn)對(duì)自身位置的估計(jì)，并實(shí)時(shí)更新環(huán)境地圖。

近年來(lái)，國(guó)內(nèi)外開始有學(xué)者把SLAM方法應(yīng)用到空間和水下未知運(yùn)動(dòng)目標(biāo)的位姿觀測(cè)和估計(jì)問題，做了許多值得借鑒的工作。Augenstein等［6］在研究中針對(duì)目標(biāo)的六自由度姿態(tài)和三維構(gòu)型問題采用了一種遞歸估計(jì)算法，也稱作基于FastSLAM的啟發(fā)式姿態(tài)估計(jì)與重構(gòu)算法。該方法不需要目標(biāo)的先驗(yàn)信息，僅僅利用單目相機(jī)對(duì)圖像處理和提取特征，和擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)算法相比，采用的粒子濾波算法的測(cè)量更新效率更高，并且不受單峰概率的限制。在此之后Augenstein等［7］又提出一種實(shí)時(shí)的幀間姿態(tài)估計(jì)方法，它結(jié)合貝葉斯算法和測(cè)量反演方法，估計(jì)出了移動(dòng)機(jī)器人平臺(tái)和翻滾目標(biāo)之間的相對(duì)姿態(tài)，并進(jìn)行地圖的構(gòu)建。Schnitzer等［8］在研究中模擬了一種未知非合作的目標(biāo)物體，在對(duì)SLAM提取的稀疏點(diǎn)云采用隨機(jī)一致性采樣(Random Sample Consensus，RANSAC)處理后，對(duì)非合作目標(biāo)的三維構(gòu)型進(jìn)行了重建，為后期的視覺導(dǎo)航處理任務(wù)提供先驗(yàn)信息。Cho等［9］利用視覺對(duì)目標(biāo)物圖像的特征進(jìn)行檢測(cè)，在目標(biāo)丟失的情況下，提出了一種魯棒性強(qiáng)的特征配準(zhǔn)算法，并基于因子圖的概率圖模型進(jìn)行平滑估計(jì)，計(jì)算出位置變化和六自由度的狀態(tài)。Tweddle［10-11］在研究中分析了空間非合作目標(biāo)的特點(diǎn)，闡述非合作目標(biāo)本身的線速度、角速度、慣性主軸、質(zhì)心等信息對(duì)近距離捕獲任務(wù)具有重要意義，并且針對(duì)典型SLAM算法不適用于高速旋轉(zhuǎn)物體問題，提出一種解決圍繞任意軸旋轉(zhuǎn)的非合作目標(biāo)的SLAM算法。該方法依據(jù)牛頓第二定律和歐拉轉(zhuǎn)動(dòng)方程，對(duì)運(yùn)動(dòng)物體未來(lái)的狀態(tài)進(jìn)行預(yù)測(cè)，把這些信息作為因子，再采用增量式平滑的優(yōu)化方法對(duì)位姿進(jìn)行估計(jì)，最后通過試驗(yàn)評(píng)估算法的收斂性和精度。Thomas等［12］研究了一種單目SLAM算法，在更新相機(jī)的位置和姿態(tài)的同時(shí)，對(duì)目標(biāo)的三維特征進(jìn)行跟蹤，用于軌道上衛(wèi)星的近距離自主捕獲任務(wù)。郝剛濤等［13］將視覺SLAM的濾波模型應(yīng)用到失效非合作航天器的位姿量測(cè)，融合無(wú)損卡爾曼濾波和粒子濾波算法對(duì)目標(biāo)的位姿進(jìn)行估計(jì)，仿真結(jié)果表明該算法實(shí)現(xiàn)了較優(yōu)的精度和速度。

上述研究相對(duì)集中在視覺SLAM方面，且設(shè)置的應(yīng)用場(chǎng)景相對(duì)簡(jiǎn)單，然而太空環(huán)境中光照變化劇烈，導(dǎo)致視覺SLAM算法的適用范圍受到限制，并且失效航天器通常處于自由翻滾運(yùn)動(dòng)，其周圍充斥著太空垃圾或碎片等障礙物的干擾。考慮上述諸多因素的空間應(yīng)用場(chǎng)景下的SLAM算法的研究相對(duì)較少，值得進(jìn)一步的研究。本文選用激光雷達(dá)作為傳感器，研究相對(duì)復(fù)雜的應(yīng)用場(chǎng)景下的圖優(yōu)化SLAM。

1 圖優(yōu)化SLAM和先驗(yàn)子圖檢測(cè)

1．1 通用的圖優(yōu)化SLAM方法

SLAM問題，也就是通過服務(wù)航天器搭載傳感器，在移動(dòng)過程中，對(duì)服務(wù)航天器的位置姿態(tài)進(jìn)行實(shí)時(shí)估計(jì)，同時(shí)繪制出符合任務(wù)需求的環(huán)境地圖。

當(dāng)服務(wù)航天器在未知環(huán)境中移動(dòng)時(shí)，它的軌跡可以由離散的變量 x1，x2，…，xk來(lái)描述，運(yùn)動(dòng)傳感器(慣性測(cè)量單元)的數(shù)據(jù)稱為輸入值，用u1，u2，…，uk表示，傳感器的噪聲用 ωk表示，運(yùn)動(dòng)方程可以表示為:

服務(wù)航天器在位置xk對(duì)環(huán)境中路標(biāo)(包括失效航天器、碎片等)lj的觀測(cè)量用zk，j表示，觀測(cè)數(shù)據(jù)的噪聲用vk，j表示，觀測(cè)方程可以表示為:

把SLAM問題建模成狀態(tài)估計(jì)問題，從上述聯(lián)立的方程組中估計(jì)出狀態(tài)變量。狀態(tài)估計(jì)問題的求解有很多種方法。目前主流的圖優(yōu)化方法在解決狀態(tài)估計(jì)問題時(shí)具有優(yōu)越的性能［14］。本文采用圖優(yōu)化方法，并用因子圖進(jìn)行描述［15］。

通用的圖優(yōu)化SLAM框架如圖1所示。

圖1 通用的圖優(yōu)化SLAM框架Fig.1 General SLAM scheme of graph-based optimization

通用圖優(yōu)化SLAM框架的主要組成部分如下:

1)傳感器數(shù)據(jù)。激光雷達(dá)采集空間環(huán)境信息生成點(diǎn)云，慣性傳感器對(duì)服務(wù)航天器自身運(yùn)動(dòng)信息進(jìn)行測(cè)量。

2)順序數(shù)據(jù)關(guān)聯(lián)。對(duì)激光雷達(dá)掃描到的相鄰幀進(jìn)行配準(zhǔn)，結(jié)合慣性傳感器的數(shù)據(jù)，估計(jì)出服務(wù)航天器在相鄰時(shí)刻的位姿變換關(guān)系。

3)回環(huán)檢測(cè)。回環(huán)檢測(cè)判斷服務(wù)航天器是否到達(dá)過先前的位置，它可以通過多個(gè)節(jié)點(diǎn)之間的約束來(lái)判斷，通常是一種強(qiáng)約束。假如檢測(cè)到該約束信息，可以用于位姿優(yōu)化。

4)位姿圖優(yōu)化。后端接受不同時(shí)刻估計(jì)出的服務(wù)航天器的位姿，以及回環(huán)檢測(cè)的約束信息，找到平衡所有約束的最優(yōu)位姿配置解，從而得到全局一致的軌跡。

5)構(gòu)建地圖。用合適的地圖來(lái)表示空間環(huán)境模型信息。

將通用的圖優(yōu)化SLAM應(yīng)用到空間失效航天器的應(yīng)用場(chǎng)景中。本文選取空間長(zhǎng)期失效航天器作為服務(wù)對(duì)象，服務(wù)星在繞飛階段進(jìn)行同步定位與地圖構(gòu)建，估計(jì)相對(duì)位姿，為后期的導(dǎo)航、規(guī)劃和近距離操控等任務(wù)提供有效信息。

1．2 先驗(yàn)子圖檢測(cè)

圖優(yōu)化SLAM要求應(yīng)用場(chǎng)景相對(duì)于參考坐標(biāo)系處于靜止?fàn)顟B(tài)。而空間場(chǎng)景中待觀測(cè)的一部分路標(biāo)(失效航天器)處于緩慢運(yùn)動(dòng)狀態(tài)，導(dǎo)致位姿估計(jì)在長(zhǎng)時(shí)間運(yùn)行后產(chǎn)生累積誤差的問題。為了解決該問題，從失效目標(biāo)航天器的自身運(yùn)動(dòng)特點(diǎn)著手，引入先驗(yàn)子圖檢測(cè)法。

根據(jù)對(duì)失效目標(biāo)航天器運(yùn)動(dòng)特性的分析，它通常圍繞慣量主軸處于緩慢旋轉(zhuǎn)的運(yùn)動(dòng)狀態(tài)［16］。存在能量耗散的航天器，當(dāng)航天器章動(dòng)時(shí)，內(nèi)部可活動(dòng)質(zhì)點(diǎn)的運(yùn)動(dòng)隨著能量的損耗，趨向于最小動(dòng)能狀態(tài);由于角動(dòng)量守恒，航天器的動(dòng)能不可能全部消失，在最小動(dòng)能狀態(tài)下，航天器運(yùn)動(dòng)不再引起能量損耗，將圍繞某一主慣量軸穩(wěn)定自旋。因此，空間長(zhǎng)期失效航天器經(jīng)過長(zhǎng)年累月的能量損耗，其穩(wěn)定狀態(tài)是圍繞最大慣量主軸緩慢自旋，或具有很小的章動(dòng)角。

為簡(jiǎn)化問題，考慮處于緩慢自旋的失效航天器。這類失效航天器的運(yùn)動(dòng)處于周期性變化。假定服務(wù)航天器攜帶的激光雷達(dá)在初始時(shí)刻t0觀測(cè)到的失效航天器的姿態(tài)如圖2所示，然后失效航天器緩慢自旋一周后，在特定時(shí)刻tk的姿態(tài)和初始時(shí)刻t0的姿態(tài)保持一致，這兩個(gè)時(shí)刻失效航天器和空間環(huán)境碎片的位姿保持不變，即處于相對(duì)靜止?fàn)顟B(tài)，服務(wù)航天器在時(shí)刻tk和初始時(shí)刻t0相當(dāng)于對(duì)靜態(tài)場(chǎng)景進(jìn)行觀測(cè)，通過對(duì)這兩個(gè)時(shí)刻觀測(cè)到的點(diǎn)云進(jìn)行配準(zhǔn)，可以獲得服務(wù)航天器在不同時(shí)刻的位姿之間的約束關(guān)系。

圖2 先驗(yàn)子圖檢測(cè)示意Fig.2 Sketch of prior submap detecting

先驗(yàn)子圖是指具有先驗(yàn)信息的關(guān)鍵幀。將初始時(shí)刻t0激光雷達(dá)觀測(cè)到的點(diǎn)云記為先驗(yàn)關(guān)鍵幀，當(dāng)服務(wù)航天器攜帶激光雷達(dá)的移動(dòng)過程中，把觀測(cè)到的每個(gè)關(guān)鍵幀和先驗(yàn)關(guān)鍵幀進(jìn)行點(diǎn)云配準(zhǔn)，如果滿足配準(zhǔn)要求，則確定激光雷達(dá)在不同時(shí)刻的位姿關(guān)系，相應(yīng)地，通過坐標(biāo)變換獲得服務(wù)航天器之間的位姿約束信息。

可以將檢測(cè)出來(lái)的位姿關(guān)系添加到位姿圖，記為不連續(xù)節(jié)點(diǎn)之間的約束因子。由于這一過程利用了初始時(shí)刻的先驗(yàn)關(guān)鍵幀以及失效航天器的運(yùn)動(dòng)特點(diǎn)，將這種方法稱為先驗(yàn)子圖檢測(cè)法。

2 基于先驗(yàn)子圖檢測(cè)改進(jìn)的圖優(yōu)化SLAM算法

2．1 總體框架

基于先驗(yàn)子圖檢測(cè)改進(jìn)的圖優(yōu)化SLAM框架，如圖3所示，其中，先驗(yàn)子圖檢測(cè)是框架的重要環(huán)節(jié)。

圖3 基于先驗(yàn)子圖檢測(cè)改進(jìn)的圖優(yōu)化SLAM框架Fig.3 Improved SLAM scheme of graph-based optimization based on prior submap detecting

先驗(yàn)子圖檢測(cè)，和回環(huán)檢測(cè)的目的類似，利用不連續(xù)的位姿節(jié)點(diǎn)之間的約束關(guān)系，對(duì)位姿圖進(jìn)行調(diào)整。由于長(zhǎng)期失效航天器處于緩慢自旋，它的運(yùn)動(dòng)狀態(tài)呈現(xiàn)出周期性重復(fù)的先驗(yàn)特點(diǎn)，根據(jù)這一特點(diǎn)，采用先驗(yàn)子圖檢測(cè)算法獲得位姿節(jié)點(diǎn)間的約束信息用于圖優(yōu)化。

2．2 前端位姿圖構(gòu)建

位姿圖構(gòu)建過程如圖4所示。

通過將傳感器獲得的點(diǎn)云聯(lián)系起來(lái)，從而在位姿節(jié)點(diǎn)間構(gòu)成約束關(guān)系。由于激光雷達(dá)是固定在服務(wù)航天器上，當(dāng)空間環(huán)境相對(duì)于參考坐標(biāo)系近似于靜止時(shí)，兩幀激光數(shù)據(jù)之間的相對(duì)位姿變換，就對(duì)應(yīng)位姿圖中的邊，從而構(gòu)建位姿聯(lián)系。

具體步驟如下。

(1)特征提取

前端的任務(wù)是建立激光雷達(dá)的位姿圖，它根據(jù)相鄰幀的點(diǎn)云來(lái)估計(jì)出粗略的激光雷達(dá)運(yùn)動(dòng)，給后端提供較為不錯(cuò)的初始值。

圖4 位姿圖構(gòu)建過程Fig.4 Procedure of constructing pose graph

特征是指在激光雷達(dá)運(yùn)動(dòng)之中保持穩(wěn)定的點(diǎn)，它通常表現(xiàn)為地圖中的路標(biāo)。

通過激光雷達(dá)掃描到的點(diǎn)云，估計(jì)出法線向量，然后對(duì)近鄰點(diǎn)進(jìn)行查找和判斷，提取出邊界點(diǎn)作為特征點(diǎn)。

當(dāng)給定向量空間M中點(diǎn)集P={p1，p2，…，pn}與待搜索點(diǎn)q∈M，那么q的相近元素N(q，P)∈P。搜索查詢點(diǎn)近鄰點(diǎn)，統(tǒng)計(jì)其法線向量，找到邊界點(diǎn)。

(2)相鄰幀的運(yùn)動(dòng)估計(jì)

采用迭代最近點(diǎn)(Iterative Closest Point，ICP)方法求取相鄰幀的位置和姿態(tài)變換。

假設(shè)相鄰幀的邊界特征點(diǎn)構(gòu)成的子集為P ，Q，相應(yīng)的點(diǎn)為 pi，qi，則有 P={p1，p2，…，pn}和 Q={q1，q2，…，qn}。把 ICP 方法計(jì)算相鄰幀的旋轉(zhuǎn)分量R以及平移分量t這個(gè)問題轉(zhuǎn)換為求解一個(gè)最小二乘問題，

式中:minf(R，t)為誤差大小;ωi為每一對(duì)的匹配點(diǎn)在整體中所占的權(quán)重。

ICP問題可以通過非線性優(yōu)化的方法求解，目標(biāo)函數(shù)可以表示為:

在計(jì)算過程中，通過不斷迭代直到函數(shù)值取得極小值。

(3)構(gòu)建位姿圖

位姿圖中的節(jié)點(diǎn)代表的是激光雷達(dá)的位姿，連接節(jié)點(diǎn)的邊則代表激光雷達(dá)從前一時(shí)刻到下一時(shí)刻進(jìn)行的運(yùn)動(dòng)變換，由一連串變換序列繪制服務(wù)航天器的位姿圖。

2．3 后端位姿優(yōu)化

基于先驗(yàn)子圖檢測(cè)的后端位姿優(yōu)化如圖5所示。

(1)全因子圖模型

令服務(wù)航天器在時(shí)刻ti的位姿用xi表示，服務(wù)航天器在當(dāng)前時(shí)刻k的狀態(tài)集用Xk表示，

圖5 基于先驗(yàn)子圖檢測(cè)的后端優(yōu)化過程Fig.5 Procedure of backend optimization based on prior submap detecting

對(duì)聯(lián)合概率密度函數(shù)因式分解，則聯(lián)合分布函數(shù)為:

式中:操作符d(·)定義為消耗的代價(jià)函數(shù)。

對(duì)于高斯噪聲分布，通用的因子可以表示為:

若因子表示的是量測(cè)模型，那么

式中:h(·)為量測(cè)函數(shù)，通過它可以從給定的狀態(tài)預(yù)估出傳感器的量測(cè)值。

根據(jù)式(5)～(9)可知，狀態(tài)變量的最優(yōu)估計(jì)也就是讓下式最小化:

根據(jù)不同的量測(cè)模型，對(duì)本文用到的因子進(jìn)行描述。全因子圖模型如圖6所示。

1)Prior因子。對(duì)于特定的狀態(tài)變量有先驗(yàn)信息ν，于是Prior因子為:

2)IMU因子。服務(wù)航天器的狀態(tài)變量x可以用連續(xù)的非線性微分方程表示:

圖6 全因子圖模型Fig.6 Factor graph model

式中:fb，ωb是慣性測(cè)量單元在本體系下測(cè)量的力和角加速度。，式(12)對(duì)應(yīng)的離散方程為:

根據(jù)式(13)，可以通過狀態(tài)xk和傳感器測(cè)量值預(yù)測(cè)出下個(gè)時(shí)刻的狀態(tài)x，而預(yù)測(cè)值和實(shí)k+1際的xk+1存在的差值體現(xiàn)為IMU因子的誤差函數(shù):

3)Lidar因子。激光雷達(dá)的量測(cè)方程可以表示為:

式中:路標(biāo)l是全局參考系的坐標(biāo);旋轉(zhuǎn)矩陣R和平移向量t表示激光雷達(dá)坐標(biāo)系和全局參考系的轉(zhuǎn)換關(guān)系。Lidar因子為:

4)約束因子。變量節(jié)點(diǎn)包括服務(wù)航天器狀態(tài)和路標(biāo)節(jié)點(diǎn)。在不連續(xù)的變量節(jié)點(diǎn)之間存在約束關(guān)系，可以用于優(yōu)化。假定在初始時(shí)刻t1和時(shí)刻tk存在約束關(guān)系，量測(cè)方程可以表示為:

式中:hConstr為約束節(jié)點(diǎn)之間的量測(cè)函數(shù);nConstr為量測(cè)噪聲。約束因子可以表示為:

在圖6中，約束因子的連線用虛線表示，約束因子fConstr包括先驗(yàn)子圖約束因子fSubmap和閉環(huán)因子fLoop兩類。

(2)先驗(yàn)子圖檢測(cè)方法

本文采用RANSAC算法來(lái)實(shí)現(xiàn)先驗(yàn)子圖檢測(cè)方法。RANSAC算法通過迭代的方式來(lái)估計(jì)數(shù)據(jù)中的模型參數(shù)。估算的數(shù)據(jù)集合包括有效點(diǎn)(Inliers)和無(wú)效點(diǎn)(Outliners)，其中有效點(diǎn)處于所需的數(shù)學(xué)模型附近，異常點(diǎn)可能是一些環(huán)境干擾、測(cè)量誤差等產(chǎn)生的。

本文采用的是多點(diǎn)RANSAC算法。具體過程如下:

1)在初始先驗(yàn)關(guān)鍵幀中，選擇若干參數(shù)點(diǎn)d(特征點(diǎn))，失效目標(biāo)航天器m，周圍環(huán)境n，總共m+n參數(shù)點(diǎn);將待匹配幀的相關(guān)聯(lián)的參數(shù)點(diǎn)，通過平移與旋轉(zhuǎn)計(jì)算出對(duì)應(yīng)的d'，再把d'和d相對(duì)比，若誤差小于一定的閾值，視為有效參數(shù)點(diǎn)。

2)將所有對(duì)應(yīng)參數(shù)點(diǎn)進(jìn)行上述的運(yùn)算，如果有效參數(shù)點(diǎn)的數(shù)量滿足指定的要求，那么判斷兩幀匹配成功，并且上一步中計(jì)算出的平移向量和旋轉(zhuǎn)矩陣有效。

3)若2)中未達(dá)到足夠有效點(diǎn)，視為無(wú)效。

在RANSAC算法中，如果當(dāng)前幀和先驗(yàn)關(guān)鍵幀的點(diǎn)云配準(zhǔn)成功，計(jì)算出的旋轉(zhuǎn)矩陣和平移向量表示了激光雷達(dá)和失效航天器等環(huán)境信息之間的相對(duì)位姿關(guān)系，然后計(jì)算出服務(wù)航天器在當(dāng)前時(shí)刻和初始時(shí)刻的位姿之間的約束信息。這一信息可以作為節(jié)點(diǎn)之間的具有較強(qiáng)約束的邊，用于局部位姿圖的調(diào)整和優(yōu)化。

(3)圖優(yōu)化

通過前端順序數(shù)據(jù)關(guān)聯(lián)構(gòu)建出位姿圖之后，依據(jù)先驗(yàn)子圖檢測(cè)得到局部節(jié)點(diǎn)的約束信息，回環(huán)檢測(cè)獲得環(huán)中大范圍內(nèi)節(jié)點(diǎn)的約束關(guān)系，就可以對(duì)位姿圖進(jìn)行優(yōu)化。

對(duì)于因子圖，因子節(jié)點(diǎn)處于待優(yōu)化的位姿節(jié)點(diǎn)之間，包含了位姿節(jié)點(diǎn)之間的約束關(guān)系，它們來(lái)自于運(yùn)動(dòng)方程和觀測(cè)方程。例如，順序數(shù)據(jù)關(guān)聯(lián)，可以用慣性測(cè)量單元因子來(lái)描述相鄰時(shí)刻位姿之間的約束關(guān)系;先驗(yàn)子圖的檢測(cè)，獲得不同的局部節(jié)點(diǎn)的制約關(guān)系;回環(huán)檢測(cè)則獲得環(huán)中更多節(jié)點(diǎn)的制約關(guān)系。對(duì)因子圖的優(yōu)化，就是調(diào)整各個(gè)變量的值，使得它們的因子乘積最大化，可以采取增量平滑與地圖構(gòu)建方法(incremental Smoothing And Mapping， iSAM) 來(lái) 優(yōu) 化［17］。

SLAM數(shù)據(jù)關(guān)聯(lián)和位姿優(yōu)化的偽代碼如下:

1 Factor_Graph←InitializationNodesFactors()

2 for keyframe KP_k do

3 Features(k)←ExtractFeatures(KP_k)

4 {Rk，Tk} ← ICP(Features(k)，F(xiàn)eatures(k－1))

5 Curr_Pose← NewPoseNodeAndFactor(Rk，Tk)

6 Factor_Graph←Curr_Pose

7 {R，T} ← {R，T，Rk，Tk}

8 end for

9 {R，T} ← iSAM_Smooth(R，T)

10 for keyframe KP_k do

11 {Rs_k0，Ts_k0，F(xiàn)lags_k} ← SubmapDetect_RANSAC(KP_k，KP_0)

12 if IsPriorSubmap(Flags_k)then

13 Factor_Graph←NewContraintFactorAndNode(Curr_Pose，Rs_k0，Ts_k0)

14 end if

15 {Rloop_k0，Tloop_k0，F(xiàn)lagloop_k} ← LoopDetect(KP_k，KP_0)

16 if IsLoop(Flagloop_k)then

17 Factor_Graph←NewContraintFactorAndNode(Curr_Pose，Rloop_k0，Tloop_k0)

18 end if

19 Factor_Graph←iSAM_Update(Factor_Graph)20 end for

3 仿真結(jié)果與分析

試驗(yàn)系統(tǒng)環(huán)境采用的是機(jī)器人操作系統(tǒng)(Robot Operating System，ROS)，版本為 Indigo，以及仿真開發(fā)環(huán)境MATLAB 2015b。

仿真試驗(yàn)過程主要為:

1)仿真環(huán)境搭建。通過ROS搭建系統(tǒng)環(huán)境，同時(shí)創(chuàng)建任務(wù)節(jié)點(diǎn)，ROS主機(jī)和MATLAB節(jié)點(diǎn)通過局域網(wǎng)連接并通信;利用Gazebo搭建空間環(huán)境模型，激光雷達(dá)掃描模型生成點(diǎn)云數(shù)據(jù)，模擬慣性測(cè)量單元的數(shù)據(jù)，并通過ROS以話題的方式發(fā)布。

2)核心部分。SLAM算法，ROS主機(jī)訂閱話題獲取傳感器數(shù)據(jù)，進(jìn)行位姿圖構(gòu)建、先驗(yàn)子圖檢測(cè)、回環(huán)檢測(cè)和后端位姿圖優(yōu)化等操作。

3)位姿軌跡的顯示。ROS主機(jī)將位姿圖(服務(wù)航天器的軌跡)數(shù)據(jù)發(fā)布出來(lái)，MATLAB節(jié)點(diǎn)訂閱該數(shù)據(jù)并進(jìn)行結(jié)果顯示和分析。

在繞飛階段服務(wù)航天器對(duì)失效航天器進(jìn)行環(huán)境建模，SLAM算法應(yīng)用場(chǎng)景如圖7所示，包含有失效衛(wèi)星模型和碎片障礙物。

圖7 SLAM算法應(yīng)用場(chǎng)景Fig.7 Application scenario of SLAM algorithm

仿真參數(shù)設(shè)定如下:

1)失效航天器本體的尺寸，長(zhǎng)、寬、高分別為220cm、180cm、240cm，太陽(yáng)翼的長(zhǎng)、寬分別為600cm、160cm，太陽(yáng)翼的三角支架和翼板相接的邊為160cm，高為130cm。

2)失效航天器繞著最大慣量主軸處于緩慢自旋狀態(tài)，旋轉(zhuǎn)角速率為5(°)/s。

3)失效航天器周圍伴隨若干碎片障礙物，障礙物在參考坐標(biāo)系下保持相對(duì)靜止。

4)在參考坐標(biāo)系下，初始時(shí)刻的服務(wù)航天器位于(0，0，0)，失效航天器位于(25，0，0)。

5)服務(wù)航天器在失效航天器同一軌道平面內(nèi)繞飛，繞飛半徑25m，理想軌跡如圖7所示。

SLAM算法的仿真軌跡如圖8所示，綠色點(diǎn)劃線表示通用SLAM算法估計(jì)出的軌跡，紫色實(shí)線表示基于先驗(yàn)子圖檢測(cè)改進(jìn)后的SLAM算法估計(jì)出的軌跡。

圖8 SLAM算法的軌跡比較Fig.8 Trajectories comparison of SLAM algorithms

由圖8可知，通用的圖優(yōu)化SLAM算法的位姿和軌跡真值相比偏差較大，它是由動(dòng)態(tài)場(chǎng)景下長(zhǎng)時(shí)間的位姿估計(jì)產(chǎn)生的累積誤差造成的，僅靠回環(huán)檢測(cè)對(duì)位姿優(yōu)化的效果并不顯著;而基于先驗(yàn)子圖檢測(cè)改進(jìn)后的SLAM算法，通過先驗(yàn)子圖檢測(cè)獲得局部的位姿約束信息，對(duì)位姿進(jìn)行后端優(yōu)化，進(jìn)一步減少累積誤差，改善位姿估計(jì)精度。

對(duì)比基于先驗(yàn)子圖檢測(cè)改進(jìn)的SLAM算法和通用的圖優(yōu)化SLAM算法，兩種SLAM算法的位置誤差比較如圖9(a)所示，姿態(tài)誤差比較如圖9(b)所示。從圖中可以看出，未改進(jìn)的通用SLAM算法由于累積誤差的影響導(dǎo)致位置和姿態(tài)誤差較大，基于先驗(yàn)子圖檢測(cè)改進(jìn)后的SLAM算法，位置和姿態(tài)估計(jì)精度均有提高，并且位置的改善效果更為顯著。

圖9 SLAM算法誤差比較Fig.9 Error comparison of SLAM algorithms

基于先驗(yàn)子圖檢測(cè)改進(jìn)后的SLAM算法的位置誤差曲線如圖10(a)所示，姿態(tài)角誤差曲線如圖10(b)所示，從中可以看出，位置精度優(yōu)于0．08m，姿態(tài)角精度優(yōu)于 0．6°。

基于先驗(yàn)子圖檢測(cè)改進(jìn)后的SLAM算法的誤差值如表1所示。由表可知，改進(jìn)后算法的位姿估計(jì)誤差小，精度較高。

圖優(yōu)化SLAM算法作為星上算法，以空間長(zhǎng)期失效航天器為對(duì)象。在實(shí)際的應(yīng)用場(chǎng)景中，還需要考慮失效航天器翻滾速度、傳感器的測(cè)量范圍、分辨率、遮擋等因素的約束。還可以根據(jù)失效航天器的運(yùn)動(dòng)學(xué)特性，估計(jì)運(yùn)動(dòng)變化之間的動(dòng)力學(xué)關(guān)系，將動(dòng)力學(xué)信息添加到SLAM因子圖，從而將SLAM方法應(yīng)用到更廣泛真實(shí)的運(yùn)動(dòng)場(chǎng)景。

圖10 基于先驗(yàn)子圖改進(jìn)后的SLAM算法誤差曲線Fig.10 Deviation curve of improved SLAM algorithm based on prior submap detecting

表1 改進(jìn)后算法估計(jì)誤差Table 1 Comparison of improved algorithm for estimation deviation

改進(jìn)圖優(yōu)化SLAM算法，在應(yīng)用場(chǎng)景中，提高了精度，位置精度優(yōu)于0．08 m，姿態(tài)角精度優(yōu)于0．6°，可以滿足相對(duì)導(dǎo)航任務(wù)需求，為后續(xù)導(dǎo)航、控制等在軌任務(wù)提供重要信息。

4 結(jié)束語(yǔ)

本文開展了失效航天器的激光雷達(dá)SLAM技術(shù)研究。通過對(duì)空間長(zhǎng)期失效航天器的特點(diǎn)進(jìn)行分析，在通用圖優(yōu)化SLAM基礎(chǔ)上，給出一種基于先驗(yàn)子圖檢測(cè)改進(jìn)的SLAM方法。仿真試驗(yàn)表明，本文提出的方法獲得的航天器相對(duì)位姿精度較高，位置精度優(yōu)于0．08 m，姿態(tài)角精度優(yōu)于0．6°，可滿足相對(duì)導(dǎo)航任務(wù)需求。