王昕宇 解永鋒 陳 益 王國(guó)輝

1.北京宇航系統(tǒng)工程研究所，北京 100076 2.中國(guó)運(yùn)載火箭技術(shù)研究院，北京 100076

0 引言

在軌服務(wù)與空間碎片清理是近年來(lái)各國(guó)關(guān)注的焦點(diǎn)[1]，作為其核心技術(shù)之一，近距離的空間非合作目標(biāo)的位姿測(cè)量技術(shù)也被廣泛研究[2-3]。根據(jù)非合作目標(biāo)模型是否已知，分為已知模型和未知模型，對(duì)于已知模型的位姿測(cè)量，多采用模板匹配的方式[4-5]，已有大量研究較為成熟，而目前對(duì)未知模型的位姿測(cè)量的研究則充滿挑戰(zhàn)。

針對(duì)模型未知的空間非合作目標(biāo)位姿測(cè)量，常用的解決方案為同時(shí)定位和建圖(SLAM)。SLAM最早在機(jī)器人領(lǐng)域提出，它指的是：機(jī)器人從未知環(huán)境的未知地點(diǎn)出發(fā)，在運(yùn)動(dòng)過(guò)程中通過(guò)重復(fù)觀測(cè)到的環(huán)境特征定位自身位置和姿態(tài)，再根據(jù)自身位置構(gòu)建周?chē)h(huán)境的增量式地圖，從而達(dá)到同時(shí)定位和地圖構(gòu)建的目的。

文獻(xiàn)[6]基于SLAM的貝葉斯濾波估計(jì)模型,提出了基于視覺(jué)的非合作目標(biāo)的位姿測(cè)量方法，實(shí)現(xiàn)了對(duì)翻滾目標(biāo)的位姿測(cè)量。文獻(xiàn)[7]將經(jīng)典視覺(jué)SLAM算法ORB-SLAM應(yīng)用于空間非合作目標(biāo)位姿測(cè)量，取得了較好的測(cè)量精度。文獻(xiàn)[8]基于共線投影原理創(chuàng)立了雙目視覺(jué)的測(cè)量模型，融合慣性元件進(jìn)行組合導(dǎo)航。然而空間光環(huán)境復(fù)雜且變化劇烈，純視覺(jué)方法應(yīng)用受限。激光雷達(dá)作為主動(dòng)式傳感器，具有較好的光照不變性，被航天器廣泛應(yīng)用[9]。因此基于激光雷達(dá)的位姿測(cè)量技術(shù)被越來(lái)越多的學(xué)者重視。

文獻(xiàn)[10]以迭代最近點(diǎn)(ICP)算法為基礎(chǔ)，提出的考慮累積誤差的參數(shù)傳遞法進(jìn)行序列點(diǎn)云配準(zhǔn)。文獻(xiàn)[11]分析了非合作目標(biāo)相對(duì)位姿測(cè)量中的漂移問(wèn)題，提出了基于位姿平均的漂移修正方法。文獻(xiàn)[12]將2D-3D多特征融合，求解目標(biāo)位姿。而文獻(xiàn)[13]則利用位姿圖優(yōu)化技術(shù)，結(jié)合閉環(huán)檢測(cè)，提出了解決累計(jì)誤差的完整SLAM框架。

上述提到的點(diǎn)云配準(zhǔn)多以Besl等人1992年提出的ICP算法為基礎(chǔ)[14]。但面對(duì)非稠密點(diǎn)云，ICP的最近點(diǎn)匹配退化嚴(yán)重。文獻(xiàn)[15]中擴(kuò)展ICP算法整合了點(diǎn)與點(diǎn)和點(diǎn)與面的匹配，拓展了ICP算法適用范圍。但I(xiàn)CP算法每次迭代均需搜索最近點(diǎn)，計(jì)算代價(jià)較高。文獻(xiàn)[16]提出了NDT算法，通過(guò)匹配相鄰幀正態(tài)分布柵格，無(wú)需迭代搜索最近點(diǎn)，效率更高。

LOAM算法[17]為提高實(shí)時(shí)性，采用提取邊緣點(diǎn)和平面點(diǎn)匹配的方式，減少了待匹配點(diǎn)數(shù)量，同時(shí)為了降低運(yùn)動(dòng)拖影的干擾，里程計(jì)與建圖分為了高頻和低頻2部分。針對(duì)低成本的固態(tài)激光雷達(dá)單線收發(fā)、不規(guī)則采樣的特點(diǎn)，Livox_LOAM[18]基于LOAM增加了壞點(diǎn)剔除和分段匹配的步驟，提高了里程計(jì)的魯棒性。本文在此基礎(chǔ)上，結(jié)合空間非合作目標(biāo)的特點(diǎn)，改進(jìn)了特征點(diǎn)選擇和位姿迭代優(yōu)化流程，提出了一種適用于固態(tài)激光雷達(dá)的空間非合作目標(biāo)位姿測(cè)量方案。

1 特征點(diǎn)提取

1.1 點(diǎn)云采樣

本文主要研究基于固態(tài)激光雷達(dá)位姿估計(jì)，下面以livox公司推出的mid-40激光雷達(dá)為例說(shuō)明點(diǎn)云的采樣特點(diǎn)。

如圖1(a)和(b)所示，激光雷達(dá)采用單線激光束在一個(gè)圓形平面內(nèi)掃描，形成一個(gè)夾角38.4°的錐形掃描視野。掃描軌跡如圖2所示，形狀類(lèi)似花瓣。為增加視場(chǎng)內(nèi)的掃描點(diǎn)覆蓋率，該激光雷達(dá)采用了非重復(fù)掃描模式，掃描花瓣在不斷旋轉(zhuǎn)。不過(guò)這也使得以搜索最近點(diǎn)為核心的ICP算法效果大打折扣。

圖1 mid-40激光雷達(dá)有效視場(chǎng)范圍

圖2 mid-40激光雷達(dá)掃描軌跡

(1)

1.2 邊緣點(diǎn)提取

為了計(jì)算每幀點(diǎn)云本地坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換(即位姿)，采用特征點(diǎn)匹配的方式，下面詳細(xì)介紹如何針對(duì)空間非合作目標(biāo)提取特征點(diǎn)。

1)轉(zhuǎn)折點(diǎn)

激光雷達(dá)在以花瓣?duì)钴壽E掃描過(guò)程中，得到的序列點(diǎn)在遇到物體邊緣時(shí)會(huì)出現(xiàn)轉(zhuǎn)折，體現(xiàn)在點(diǎn)云坐標(biāo)上則為相鄰點(diǎn)之間的微分向量夾角增大。

(2)

式(2)計(jì)算出某點(diǎn)Pi和前后兩點(diǎn)之間向量的夾角，以此作為轉(zhuǎn)折點(diǎn)的判斷標(biāo)準(zhǔn)。但需注意以下2點(diǎn)：

a.剔除視野邊緣點(diǎn)。由圖2知，掃描花瓣在視野中央，軌跡接近直線，而在視野邊緣有近180°轉(zhuǎn)變，這將影響式(1)的判斷。因此將距x軸15°外的點(diǎn)均不作轉(zhuǎn)折點(diǎn)判斷。由于一般將目標(biāo)航天器鎖定在視野中央，因此不會(huì)明顯降低特征點(diǎn)數(shù)量。

b.距離雷達(dá)近優(yōu)先。圖3(a)中，A、B兩點(diǎn)均可由式(1)判斷為轉(zhuǎn)折點(diǎn)，而顯然點(diǎn)A是由于點(diǎn)B的遮擋而轉(zhuǎn)折，更換視角后，點(diǎn)A將不再是邊緣點(diǎn)，因此在檢測(cè)到轉(zhuǎn)折點(diǎn)后需判斷當(dāng)前點(diǎn)與下一個(gè)點(diǎn)哪一個(gè)距離雷達(dá)更近，將更近點(diǎn)選為邊緣點(diǎn)。

圖3 激光雷達(dá)采樣點(diǎn)示例

2)斷點(diǎn)

在空間環(huán)境中，如果激光束沒(méi)有照射到目標(biāo)航天器上，則意味著無(wú)法被反射回雷達(dá)，測(cè)量結(jié)果為無(wú)窮遠(yuǎn)。而激光雷達(dá)掃描中從航天器到宇宙空間過(guò)程中必然經(jīng)過(guò)邊緣點(diǎn)，本文稱(chēng)之為斷點(diǎn)，如圖3(b)所示。因此如果某點(diǎn)的一側(cè)為無(wú)窮遠(yuǎn)，而該點(diǎn)在另一側(cè)連續(xù)，則可以為是斷點(diǎn)。

1.3 平面點(diǎn)提取

判斷某一點(diǎn)是否在一個(gè)平面上，很難通過(guò)單線掃描的序列點(diǎn)完成，本文借鑒主成分分析算法，提出一種單線掃描下的平面點(diǎn)提取方案，步驟如下：

a.選擇待判斷點(diǎn)附近一定距離內(nèi)點(diǎn)(如5 cm內(nèi))，假設(shè)為k個(gè)，由圖2可知，0.1s時(shí)掃描覆蓋率已經(jīng)較高，因此k個(gè)點(diǎn)不會(huì)來(lái)自共線的連續(xù)掃描。

b.計(jì)算k個(gè)點(diǎn)的均值與協(xié)方差矩陣，并計(jì)算協(xié)方差矩陣的特征值與特征向量。

c.根據(jù)主成分分析理論，協(xié)方差矩陣的特征值越小，在該特征方向的分布越集中，近似壓縮在一個(gè)平面上，如圖3 (c)所示。因此將最小特征值是次小的三分之一作為k個(gè)點(diǎn)是否組成平面的依據(jù)，并據(jù)此判斷該點(diǎn)是否在平面上，即平面點(diǎn)。

然而，值得注意的是，圖3(d)中平面與激光束夾角很小，此時(shí)激光束會(huì)在平面上照射出拉長(zhǎng)的光斑，測(cè)距精度將顯著下降，因此對(duì)該種情況的平面點(diǎn)進(jìn)行剔除。

2 迭代位姿優(yōu)化

為解算出激光雷達(dá)繞空間目標(biāo)移動(dòng)過(guò)程中的位姿變化，需要計(jì)算每一幀本地坐標(biāo)系下的點(diǎn)云相對(duì)于第一幀的世界坐標(biāo)系之間的旋轉(zhuǎn)和平移，即(Ri,Ti)。本文采用上一節(jié)介紹的算法提取每一幀的特征點(diǎn)，將其與之前所有幀的特征點(diǎn)進(jìn)行匹配，迭代優(yōu)化Ri與Ti使得匹配誤差最小。

2.1 特征點(diǎn)匹配

由于固態(tài)激光雷達(dá)的不重復(fù)掃描特點(diǎn)，直接點(diǎn)與點(diǎn)的匹配很難實(shí)現(xiàn)，因此采用邊緣特征點(diǎn)與直線和平面特征點(diǎn)與平面的匹配方式。

為提高迭代效率，設(shè)置合適位姿初值，本文以i-1與i-2幀間位姿變化預(yù)估第i幀位姿，在齊次坐標(biāo)系下表示為：

(3)

1)點(diǎn)與線

(4)

圖4 匹配誤差示意圖

2)點(diǎn)與面

(5)

2.2 迭代優(yōu)化

2.1節(jié)分別計(jì)算出點(diǎn)與線、點(diǎn)與面匹配的誤差，一幀點(diǎn)云總的誤差為

(6)

圖5 位姿優(yōu)化算法流程

圖5的算法流程中，特征點(diǎn)匹配完成后，增加一步誤差加權(quán)：

(7)

式中：d為誤差值，ρ(d)為加權(quán)系數(shù)，h為給定參數(shù)，設(shè)為0.01m。經(jīng)過(guò)該步驟，匹配誤差較大項(xiàng)在總誤差中占比會(huì)下降，減少了離群點(diǎn)對(duì)最終結(jié)果的影響。

算法最后判斷優(yōu)化是否結(jié)束，若前后兩輪優(yōu)化結(jié)果差別很小或達(dá)到最大輪次，則結(jié)束優(yōu)化，輸出結(jié)果。否則算法將以最新的估計(jì)位姿(Ri,Ti)更新特征點(diǎn)在世界坐標(biāo)系中的坐標(biāo)，迭代優(yōu)化。

2.3 運(yùn)動(dòng)補(bǔ)償

固態(tài)激光雷達(dá)單線掃描特點(diǎn)，導(dǎo)致同一幀點(diǎn)云并非同時(shí)采樣獲得，而是在一個(gè)時(shí)間段內(nèi)依次采樣，而采樣時(shí)雷達(dá)自身在不斷運(yùn)動(dòng)，由此產(chǎn)生運(yùn)動(dòng)拖影，本文采用線性插值方法進(jìn)行運(yùn)動(dòng)補(bǔ)償。

以(Ri,Ti)為第i幀最后一個(gè)采樣點(diǎn)的雷達(dá)位姿，則第i幀最后一點(diǎn)(時(shí)刻ti)到第i-1幀最后一點(diǎn)(時(shí)刻ti-1)之間的位姿變換為：

(8)

(9)

式中：旋轉(zhuǎn)角θ可由ΔRi的跡計(jì)算，而旋轉(zhuǎn)向量經(jīng)ΔRi的旋轉(zhuǎn)不會(huì)改變，所以旋轉(zhuǎn)向量n為ΔRi的特征值為1的特征向量。旋轉(zhuǎn)角線性插值后使用羅德里格斯公式[19]轉(zhuǎn)換回矩陣：

(10)

(11)

3 實(shí)驗(yàn)校驗(yàn)

為了校驗(yàn)所提算法的有效性，本文對(duì)氣浮臺(tái)半實(shí)物實(shí)驗(yàn)所得數(shù)據(jù)進(jìn)行處理。

3.1 實(shí)驗(yàn)條件

為模擬太空中追蹤航天器與目標(biāo)航天器的真實(shí)相對(duì)運(yùn)動(dòng)情況，采用了氣浮模擬器繞飛目標(biāo)模擬器的方式進(jìn)行演示實(shí)驗(yàn)，如圖6所示。

圖6 實(shí)驗(yàn)裝置

追蹤模擬器距離目標(biāo)模擬器2m，以1.5(°)/s的角速度繞飛。追蹤模擬器上安裝有光學(xué)追蹤器，使用OptiTrack外部光學(xué)測(cè)量系統(tǒng)測(cè)量位姿，將其測(cè)量值作為算法參考的位姿真值。

3.2 結(jié)果及分析

追蹤模擬器繞飛目標(biāo)模擬器一周后，根據(jù)計(jì)算得到的每一幀點(diǎn)云位姿，將點(diǎn)云坐標(biāo)統(tǒng)一變換到世界坐標(biāo)系下，得到圖7的目標(biāo)三維模型。

圖7 目標(biāo)模擬器三維模型

圖8為追蹤模擬器的繞飛軌跡誤差熱力圖，圖9展示了三軸坐標(biāo)系下激光雷達(dá)測(cè)量軌跡與真實(shí)軌跡的誤差，圖中可知，兩條軌跡基本吻合。圖10量化統(tǒng)計(jì)了測(cè)量軌跡與真實(shí)軌跡的累積誤差，即圖中APE。絕對(duì)累積誤差并未持續(xù)增加而是上下震蕩，表明算法中每一幀點(diǎn)云均和全局地圖匹配的設(shè)計(jì)，有一定的回環(huán)修正能力。

圖8 測(cè)量軌跡誤差熱力圖

圖9 三軸坐標(biāo)系下軌跡誤差

圖10 絕對(duì)軌跡誤差信息

本文將所提出算法與Livox公司開(kāi)源算法對(duì)比，結(jié)果如表1所示。在最大誤差相似情況下，本文算法的軌跡均值誤差與均方根誤差均明顯低于對(duì)照算法，能夠滿足航天器相對(duì)位姿測(cè)量的要求。

表1 算法軌跡誤差對(duì)比

4 結(jié)論

為測(cè)量模型未知的空間非合作目標(biāo)位姿，本文提出了基于固態(tài)激光雷達(dá)的測(cè)量方法。該方法針對(duì)空間目標(biāo)提取特征點(diǎn)，與全局地圖匹配后迭代優(yōu)化位姿，克服了固態(tài)激光雷達(dá)不規(guī)則采樣的缺點(diǎn)，在半實(shí)物實(shí)驗(yàn)中準(zhǔn)確測(cè)量了相對(duì)位姿變化，證明了方法的有效性。