基于路標(biāo)重觀測的月面巡視器激光雷達(dá)定位方法研究

李 碩，余 萌，曹 濤，鄭 博，胡 濤

（1.南京航空航天大學(xué) 航天學(xué)院，南京 211106；2.上海航天控制技術(shù)研究所，上海 201109）

引 言

月球由于其獨(dú)特的科學(xué)價(jià)值及豐富的潛在開發(fā)資源，已成為人類開展深空探測的首選目標(biāo)，與地球相比，月面環(huán)境更加復(fù)雜且呈現(xiàn)非結(jié)構(gòu)化的地貌表征，如月表崎嶇不平、土壤松軟、紋理貧瘠且月面光照條件不穩(wěn)定，導(dǎo)致地面上成熟的導(dǎo)航定位算法無法簡單移植到月球探測任務(wù)中[1]。因此，月面巡視器作為月表探測的科學(xué)儀器有效載體，需具備高可靠性、高精度的自主定位與地形研判能力[2]。

目前對于月面漫游巡視任務(wù)的研究大多聚焦于自主或半自主模式，定位與建圖算法往往采用多傳感器融合的方式，使用激光雷達(dá)、慣性測量單元（Inertial Measurement Unit，IMU）和相機(jī)融合定位與建圖，但該方法受外界環(huán)境的影響較大[3]。在崎嶇路段行駛時(shí)，隨著時(shí)間的推移系統(tǒng)會(huì)累積較大的誤差，狀態(tài)估計(jì)的不確定性也會(huì)隨著巡視器的前進(jìn)而快速增長，降低了漫游巡視任務(wù)的可靠性與靈活性[4]。

針對月面環(huán)境巡視器定位難的問題，文獻(xiàn)[5]提出了基于視覺慣性融合的組合導(dǎo)航算法，使用一種多層光流跟蹤算法改進(jìn)了視覺慣性里程計(jì)前端數(shù)據(jù)關(guān)聯(lián)較大的問題；文獻(xiàn)[6]提出了一種基于慣導(dǎo)/視覺/天文組合自主導(dǎo)航方法，采用集中自適應(yīng)濾波算法提高了月面巡視器自主定位的精度。文獻(xiàn)[7]提出了一種激光IMU緊耦合的旋轉(zhuǎn)約束（Lidar Inertial Odometry and Mapping，LIO-Mapping）算法，將圖優(yōu)化方法應(yīng)用其中，使用旋轉(zhuǎn)約束方法將運(yùn)動(dòng)估計(jì)與全局地圖對齊，但該方法處理速度較慢，尤其影響實(shí)時(shí)性。

為進(jìn)一步提高巡視器定位與建圖的精度，從點(diǎn)云地貌的顯著性入手研究了一種基于歷史路標(biāo)重觀測的月面巡視器主動(dòng)導(dǎo)航方法，該方法旨在對歷史路標(biāo)的重新配準(zhǔn)，進(jìn)而實(shí)現(xiàn)巡視器位姿的歷史估計(jì)值與當(dāng)前觀測值的融合更新，提高了定位與建圖的精確性。該算法的整體流程如圖1所示，首先在激光雷達(dá)（Lidar Odometry and Mapping，LOAM）算法與慣性測量單元預(yù)積分技術(shù)松耦合的基礎(chǔ)上[8]，通過對點(diǎn)云的顯著性研判來提取重觀測的路標(biāo)，并將激光雷達(dá)和IMU的狀態(tài)估計(jì)誤差作為重觀測的觸發(fā)條件，隨后使用層次分析法進(jìn)行備選路標(biāo)的優(yōu)先級排序，最終使用迭代最近點(diǎn)（Iterative Closest Point，ICP）算法進(jìn)行點(diǎn)云配準(zhǔn)來優(yōu)化當(dāng)前位姿。

圖1 整體流程圖Fig.1 Overall flow chart

1 重觀測路標(biāo)提取

考慮月面漫游巡視任務(wù)，在巡視器漫游過程中，大范圍的點(diǎn)云顯著性研判會(huì)占用星載計(jì)算機(jī)較多的計(jì)算資源，本文設(shè)計(jì)了一種縮減檢索方法，每經(jīng)過時(shí)間t就對當(dāng)前幀點(diǎn)云進(jìn)行顯著性研判，并標(biāo)注出顯著值最高的點(diǎn)云地貌作為后續(xù)重觀測的待匹配路標(biāo)[7]，顯著地貌提取的步驟如圖2 所示。

圖2 顯著地貌標(biāo)注Fig.2 Significant landform annotation

為提高顯著性研判的可靠性與實(shí)時(shí)性，將當(dāng)前幀點(diǎn)云均勻分成4塊區(qū)域A、B、C、D，并求取每塊區(qū)域的中心點(diǎn)Centre_A、Centre_B、Centre_C、Centre_D，以4個(gè)中心點(diǎn)為中心，用Range Search算法做范圍搜索，逐步擴(kuò)充點(diǎn)云直至每個(gè)集合包含足夠數(shù)量的點(diǎn)云，使得這些點(diǎn)云能代表本區(qū)域的地貌。將4個(gè)點(diǎn)云集合標(biāo)記為ContainerA、ContainerB、ContainerC、ContainerD，每個(gè)集合中的點(diǎn)云實(shí)時(shí)地進(jìn)行SVD（Singular Value Decomposition）分解，得到3個(gè)特征值 β1，β2，β3，且令 β1＞ β2＞ β3，構(gòu)建表達(dá)式為

當(dāng)S小于閾值γ時(shí)，認(rèn)為點(diǎn)云集合中有了足夠數(shù)量的點(diǎn)，可停止范圍搜索。在獲得點(diǎn)云集合后，需要通過顯著性研判來確定重觀測的待匹配路標(biāo)。采取基于協(xié)方差矩陣行列式的顯著性評判方法：首先構(gòu)建描述點(diǎn)云混亂程度的數(shù)量熵N和描述點(diǎn)云幾何分布特征的函數(shù)g，再定義有關(guān)N和g的協(xié)方差矩陣行列式，通過行列式值的大小判斷顯著性。

1）構(gòu)建數(shù)量熵為

使用激光LOAM算法提取每幀點(diǎn)云的邊緣點(diǎn)和平面點(diǎn)，ni為邊緣點(diǎn)的數(shù)量，p(ni)為數(shù)量直方圖中邊緣點(diǎn)數(shù)量的概率值。熵可描述一個(gè)系統(tǒng)的混亂程度，點(diǎn)云排列越是有序，熵值就越??；反之越是無序熵值就越大，因此數(shù)量熵的值越大，表明該處地貌的顯著程度越高。將數(shù)量熵做歸一化處理，即

2) 定義幾何特性函數(shù)g

對一個(gè)點(diǎn)云集合中所有的點(diǎn)進(jìn)行SVD分解，得到3個(gè)特征值λ1,λ2,λ3，且令λ1＞λ2＞λ3；

如果λ1?λ2和λ3，則該點(diǎn)集呈線性結(jié)構(gòu)；

如果λ1?λ2?λ3，則該點(diǎn)集呈平面結(jié)構(gòu)；

定義幾何特性函數(shù)g為

若函數(shù)接近0，則該點(diǎn)集近似為線性結(jié)構(gòu)；若函數(shù)近似為1，則該點(diǎn)集近似為平面結(jié)構(gòu)。

3) 定義函數(shù)f為

其中：f為2維特征向量。

定義協(xié)方差矩陣為

其中：n為點(diǎn)云的個(gè)數(shù)。

方差用來度量單維變量偏離均值的程度，協(xié)方差矩陣可以用來衡量特征向量和期望之間的偏離程度。協(xié)方差矩陣的行列式的值越大，表示特征空間中隨機(jī)變量分布得越無序，與期望之間的偏離程度越大，反之該值越小，表示特征空間中隨機(jī)變量分布得越有序，與期望之間的偏離程度越小[9]。行列式|Cr|的值越大，表示點(diǎn)集在幾何、數(shù)量特征空間中的變化越顯著，因此行列式|Cr|值的大小可以作為重觀測路標(biāo)的選取依據(jù)。找到最大|Cr|值對應(yīng)的點(diǎn)云集合，將其作為重觀測的備選路標(biāo)。

2 重觀測的觸發(fā)機(jī)制

考慮星載計(jì)算機(jī)計(jì)算能力的限制與導(dǎo)航算法實(shí)時(shí)性要求，通過路標(biāo)重觀測頻繁的更新自身位姿難以適合月面作業(yè)場景，需研究一種重觀測的有效觸發(fā)機(jī)制。主動(dòng)導(dǎo)航重觀測用來主動(dòng)地降低系統(tǒng)狀態(tài)估計(jì)的不確定性，因此重觀測的觸發(fā)機(jī)制要與狀態(tài)估計(jì)的不確定性關(guān)聯(lián)。本文設(shè)計(jì)了一種基于激光雷達(dá)與慣性測量單元狀態(tài)估計(jì)誤差差值的重觀測觸發(fā)方法：計(jì)算激光雷達(dá)及慣性測量單元的狀態(tài)估計(jì)值誤差差值的2-范數(shù)，若該值大于預(yù)定閾值 ζ，則認(rèn)為當(dāng)前的狀態(tài)估計(jì)誤差較大，需觸發(fā)路標(biāo)重觀測，算法整體流程如圖3所示。

圖3 重觀測觸發(fā)機(jī)制Fig.3 Re-observation trigger mechanism

k時(shí)刻巡視器在全局系下的位置為

k～k+1時(shí)刻激光雷達(dá)測得的位姿變換矩陣，則k+1時(shí)刻激光雷達(dá)的位置估計(jì)為

慣性測量單元在k+1時(shí)刻測得的位置估計(jì)為

其中：Δt表示k和k+1時(shí)刻的時(shí)間間隔；表示加速度計(jì)的測量值；和分別表示加速度計(jì)的漂移和白噪聲；k表示加速度計(jì)的測量值；和表示陀螺儀的漂移和白噪聲；Rk和Rk+1分別表示k和k+1時(shí)刻從載體系到全局系的旋轉(zhuǎn)矩陣[10]。

二者測量的狀態(tài)估計(jì)誤差差值為

其中：ρ為慣性測量單元和激光雷達(dá)的安裝誤差。

若Δδ＞σ，則認(rèn)為當(dāng)前慣性雷達(dá)位姿估計(jì)欠缺一致性，需要觸發(fā)重觀測。

3 路標(biāo)優(yōu)先級排序

在巡視器行進(jìn)過程中實(shí)時(shí)存儲(chǔ)4處路標(biāo)，當(dāng)新的路標(biāo)被提取時(shí)，最早存儲(chǔ)的路標(biāo)將被釋放。在觸發(fā)重觀測后巡視器將停止行進(jìn)，綜合顯著性、距離、觀測角度等因素，對4處備選路標(biāo)進(jìn)行優(yōu)先級排序，選擇一處最佳路標(biāo)進(jìn)行配準(zhǔn)。

使用層次分析法對路標(biāo)的優(yōu)先級排序，該方法通過建立一種多層次的結(jié)構(gòu)模型，定量地求出備選方案的權(quán)重系數(shù)，從而做出最優(yōu)的決策[11]。

層次分析法的第1步是建立層次結(jié)構(gòu)模型，在本方案設(shè)計(jì)中，共建立3層層次模型，分別是目標(biāo)層、準(zhǔn)則層和方案層[12]。目標(biāo)層即選擇重觀測路標(biāo)；準(zhǔn)則層為影響路標(biāo)選擇的因素，包括顯著值和距離；方案層為待匹配的路標(biāo)。重觀測的層次分析法結(jié)構(gòu)模型如圖4所示。

圖4 重觀測層次分析法結(jié)構(gòu)圖Fig.4 Structure diagram of re observation analytic hierarchy process

層次分析法的第2步首先構(gòu)建成對比較矩陣，隨后將影響因素兩兩進(jìn)行對比，對比的結(jié)果作為成對比較矩陣的元素。

準(zhǔn)則層相對于方案層的成對比較矩陣如表1所示，該成對比較矩陣的意義可以描述為顯著值和距離相對于自身的重要性是1，顯著值相對于距離的重要性是2。由于該成對比較矩陣只有兩個(gè)指標(biāo)，所以能通過一致性檢驗(yàn)。

表1 準(zhǔn)則層成對比較矩陣Table 1 Criterion layer pairwise comparison matrix

顯著值和距離因素隨著巡視器的行進(jìn)不斷改變，因此方案層相對于準(zhǔn)則層的成對比較矩陣是動(dòng)態(tài)變化的。本算法設(shè)計(jì)了一種基于商值的動(dòng)態(tài)成對比較矩陣，如表2和表3所示。

表2 基于顯著值的方案層成對比較矩陣Table 2 Scheme layer pairwise comparison matrix based on significance value

表3 基于距離的方案層成對比較矩陣Table 3 Pairwise comparison matrix of scheme layer based on distance

由于成對比較矩陣是正互反矩陣，以主對角線為對稱軸，對應(yīng)元素互為倒數(shù)，在確定矩陣（i,j）處的元素aij時(shí)，將不同路標(biāo)i和路標(biāo)j的顯著值Li和Lj進(jìn)行對比，若Li＞Lj，則把Li除以Lj得到的商值四舍五入取整記為aij；若Li＜Lj，則把Lj除以Li得到的商值四舍五入取整記為aji，則aij=1/aji。

通過后續(xù)實(shí)驗(yàn)驗(yàn)證得知，該方法得到的成對比較矩陣具有極高的一致性檢驗(yàn)通過率。若當(dāng)前成對比較矩陣未通過一致性檢驗(yàn)，則本次重觀測選取顯著值最大的路標(biāo)。最終匯總得到各路標(biāo)權(quán)重矩陣如表4所示。表中b1i表示路標(biāo)i對顯著值的權(quán)重，b2i表示路標(biāo)i對距離的權(quán)重。各路標(biāo)權(quán)重系數(shù)為

表4 各路標(biāo)權(quán)重Table 4 Weight of each road sign

比較4個(gè)權(quán)重系數(shù)的大小，選取最大權(quán)重系數(shù)對應(yīng)的區(qū)域作為重觀測路標(biāo)。

4 重觀測配準(zhǔn)

重觀測的配準(zhǔn)選用ICP算法，該算法是一種經(jīng)典的點(diǎn)云匹配算法，用于估計(jì)兩幀點(diǎn)云之間的位姿變換[13]。點(diǎn)云配準(zhǔn)的表述式為

其中：R和t分別表示旋轉(zhuǎn)矩陣和平移矩陣；Pt和PS分別表示目標(biāo)點(diǎn)云和源點(diǎn)云。

ICP的一般流程為：獲取巡視器的初始位姿（初始位姿對最終的配準(zhǔn)結(jié)果影響較大）；迭代運(yùn)行在PS中尋找點(diǎn)云Pt的匹配點(diǎn)，依據(jù)匹配結(jié)果，計(jì)算Pt、PS的位姿變換[13]。

在傳統(tǒng)的 ICP 點(diǎn)云配準(zhǔn)算法中，要將當(dāng)前幀點(diǎn)云與地圖整體點(diǎn)云進(jìn)行匹配，但是進(jìn)行大范圍的點(diǎn)云檢索不僅占用過多的星載計(jì)算機(jī)資源，也降低了配準(zhǔn)的成功率，影響了可靠性與實(shí)時(shí)性[4]。因此在本算法中根據(jù)路標(biāo)的距離范圍在全局地圖中進(jìn)行對應(yīng)的檢索，并將當(dāng)前的觀測路標(biāo)與地圖中檢索得到的點(diǎn)云進(jìn)行精確配準(zhǔn)，從而大大降低了計(jì)算量。

5 仿真驗(yàn)證

5.1 仿真實(shí)驗(yàn)條件

本文共設(shè)置2個(gè)仿真驗(yàn)證環(huán)節(jié)，分別是數(shù)學(xué)仿真以及月面模擬實(shí)地測試。在數(shù)學(xué)仿真中，選取3組模擬地景，對巡視器在月球表面的巡視場景仿真模擬，仿真的總時(shí)長350 s，巡視器上裝配有激光雷達(dá)和慣性測量單元，激光雷達(dá)有效探測距離50 m，水平視場360°，垂直視場30°，根據(jù)模擬場景在軟件環(huán)境內(nèi)虛擬實(shí)時(shí)點(diǎn)云與慣性測量單元測量數(shù)據(jù)，其中對激光雷達(dá)和慣性測量單元的狀態(tài)估計(jì)的高斯噪聲也進(jìn)行了模擬，噪聲配置參數(shù)如表5所示。

表5 噪聲參數(shù)配置表Table 5 Noise parameter configuration table

在半實(shí)物仿真測試中，使用一輛4輪移動(dòng)單位，裝載Velodyne激光雷達(dá)以及車規(guī)級慣性測量單元，測試本文算法場景如圖5所示。實(shí)地場景共布置7處顯著性地標(biāo)，以供模擬巡視器（以下簡稱移動(dòng)單位）進(jìn)行點(diǎn)云重觀測，移動(dòng)單位執(zhí)行定位建圖功能，共行駛229.6 m，其中車載RTK（Real - Time Kinematic）提供位置解算的評測真值。

圖5 實(shí)地場景測試Fig.5 Experimental setup

5.2 實(shí)驗(yàn)結(jié)果

為了驗(yàn)證本算法對不同地形的適用性，選用多種點(diǎn)云地貌重新進(jìn)行仿真測試，地貌1、地貌2和地貌3的全局點(diǎn)云地圖以及巡視器行駛軌跡如圖6～8所示。

圖6 地貌1點(diǎn)云地圖與行駛路線Fig.6 Geomorphology 1 point cloud map and formal route

圖7 地貌2點(diǎn)云地圖與行駛路線Fig.7 Geomorphology 2 point cloud map and driving route

圖8 地貌3點(diǎn)云地圖與行駛路線Fig.8 Geomorphology 3 point cloud map and driving route

經(jīng)過數(shù)次仿真測試，本算法取 γ=68，某一幀原始點(diǎn)云和提取到的顯著地貌如圖9所示。將激光雷達(dá)當(dāng)前所采集到的點(diǎn)云均分成4塊區(qū)域，每塊區(qū)域用不同的顏色表示，在每個(gè)區(qū)域選取中心點(diǎn)并進(jìn)行點(diǎn)云集合的擴(kuò)充，獲得的點(diǎn)云集合用黃色表示。隨后進(jìn)行點(diǎn)云地貌的顯著性研判，并分別計(jì)算出每塊區(qū)域的|Cr|值進(jìn)行比較，將最大|Cr|值對應(yīng)的區(qū)域標(biāo)記為重觀測的備選路標(biāo)。

圖9 原始點(diǎn)云與顯著路標(biāo)提取Fig.9 Extraction of original point cloud and prominent landmarks

在重觀測觸發(fā)環(huán)節(jié)中，選取觸發(fā)閾值ζ =0.36，為了比較重觀測觸發(fā)機(jī)制的有效性，如圖10所示，將地貌1環(huán)境下未使用重觀測的誤差曲線、每20 s定時(shí)啟動(dòng)重觀測的誤差曲線以及使用觸發(fā)機(jī)制的重觀測誤差曲線進(jìn)行對比。圖10中黑色曲線表示單獨(dú)使用IMU和輪速里程計(jì)的誤差曲線，紅色曲線表示定時(shí)啟動(dòng)的誤差曲線，藍(lán)色曲線表示使用觸發(fā)機(jī)制的誤差曲線。從圖10可以看出，使用重觀測方法大大降低了狀態(tài)估計(jì)誤差，提高了定位與建圖的可靠性；而定時(shí)啟動(dòng)重觀測的方法在一定程度上抑制了誤差的增長，但與使用觸發(fā)機(jī)制的效果相比，該方法抑制誤差的效果較差。地貌2和地貌3對應(yīng)的未使用重觀測的誤差曲線、每20 s定時(shí)啟動(dòng)重觀測的誤差曲線以及進(jìn)行使用觸發(fā)機(jī)制的重觀測誤差曲線對比圖如圖11和圖12所示。

圖10 地貌1環(huán)境下誤差曲線對比圖Fig.10 Comparison diagram of error curve under landform 1 environment

圖11 地貌2環(huán)境下誤差曲線對比圖Fig.11 Comparison diagram of error curve under landform 2 environment

圖12 地貌3環(huán)境下誤差曲線對比圖Fig.12 Comparison diagram of error curve under landform 3 environment

從圖11和圖12可以看出更換不同的地貌后，重觀測方法依然能有效地控制誤差，且使用觸發(fā)機(jī)制的重觀測方法抑制誤差的效果依然優(yōu)于定時(shí)啟動(dòng)的重觀測方法，從而證明本算法的適用性、閾值的可靠性以及觸發(fā)機(jī)制的有效性。

在外場測試中選擇激光雷達(dá)LOAM算法[14]作為對比，控制移動(dòng)單位在外場行駛20 min，以RTK的位置估計(jì)為真值對本文所提算法與LOAM算法進(jìn)行比較，其中本文所提出的重觀測觸發(fā)、ICP重觀測匹配算法在ROS平臺(tái)下與LOAM算法進(jìn)行集成封裝，進(jìn)程間的雷達(dá)定位、備選點(diǎn)云集合示例如圖13所示。

圖13 外場測試過程中的重觀測點(diǎn)云選擇示例Fig.13 Example of candidate point cloud re-observation and SLAM process

移動(dòng)單位在行進(jìn)過程中，RTK位置估計(jì)、基于Velodyne16線激光雷達(dá)下的標(biāo)準(zhǔn)LOAM算法，以及本文所提算法的定位結(jié)果如圖14所示。

圖14 RTK與基于Velodyne-16線激光雷達(dá)的兩種算法里程估計(jì)Fig.14 Comparison between RTK and Velodyne-16 in terms of vehicle position estimation history

在實(shí)際測試中，本文所提算法根據(jù)當(dāng)前位置估計(jì)狀態(tài)、慣性測量單元估計(jì)狀態(tài)進(jìn)行重定位時(shí)機(jī)的研判，當(dāng)有必要執(zhí)行重定位時(shí)，在ROS環(huán)境下訂閱LOAM算法的當(dāng)前位置鄰近點(diǎn)云（前后20 m），并從中運(yùn)用點(diǎn)云顯著性研判進(jìn)行重觀測點(diǎn)云集的篩選（見圖13中左上部位示意），與當(dāng)前觀測點(diǎn)云集進(jìn)行ICP匹配更新全局位置。所提算法與標(biāo)準(zhǔn)LOAM算法的三軸位置估計(jì)誤差比較如圖15所示（其中RTK收集的經(jīng)緯高信息已進(jìn)行坐標(biāo)轉(zhuǎn)換，并與定位建圖初始位置映射對齊）。

本文所提算法在X、Y、Z軸的位置估計(jì)精度與與標(biāo)準(zhǔn)LOAM算法相比均有一定程度的提升，標(biāo)準(zhǔn)LOAM算法的最終定位誤差為3.3 m或 1.4%R（3.3 m/229 m），這也與目前國際上公開的精度評測結(jié)果相近（加入慣性測量單元定位精度為1.2%R左右），相比而言，本文算法的最終定位誤差為1.9 m或0.82%R，其中精度提升的主因?yàn)橹鲃?dòng)地選擇顯著性的路標(biāo)進(jìn)行點(diǎn)云重觀測定位。

6 結(jié) 論

本文主要針對月面巡視器定位與建圖精度問題進(jìn)行研究，在計(jì)算機(jī)計(jì)算能力可控的范圍內(nèi)提出了一種月面復(fù)雜地貌下基于歷史路標(biāo)重觀測的主動(dòng)導(dǎo)航方法，并通過數(shù)字和實(shí)物仿真驗(yàn)證，本算法可有效降低狀態(tài)估計(jì)的不確定性，并提高了定位與建圖的準(zhǔn)確性與可靠性。