荒野環(huán)境中的移動(dòng)機(jī)器人定位方法研究*

鐘釬釬 郭劍輝

（南京理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院 南京 210094）

1 引言

近年來(lái)移動(dòng)機(jī)器人的應(yīng)用范圍越來(lái)越廣泛，而如何實(shí)現(xiàn)移動(dòng)機(jī)器人在所處運(yùn)行環(huán)境中自主準(zhǔn)確的定位導(dǎo)航，是移動(dòng)機(jī)器人技術(shù)的關(guān)鍵問(wèn)題之一。在室外環(huán)境中，基于激光雷達(dá)的定位方法以其可以準(zhǔn)確測(cè)量障礙點(diǎn)的角度與距離、無(wú)需預(yù)先布置場(chǎng)景、可融合多傳感器、可以在光線較差的環(huán)境中工作以及能夠生成便于導(dǎo)航的地圖等優(yōu)勢(shì)，成為目前不可或缺的一種定位方案。

當(dāng)前基于激光雷達(dá)的定位方法主要包含了基于濾波器的激光同時(shí)定位與建圖（Simultaneous Localization and Mapping，SLAM）方案以及基于圖優(yōu)化的激光SLAM。Smith R 等［1］提出了使用最大似然算法進(jìn)行數(shù)據(jù)關(guān)聯(lián)的擴(kuò)展卡爾曼濾波SLAM 方法（EKF-SLAM）。Montemerlo M 等［2］提出了Fast-SLAM 方法［2］，該方法將SLAM 問(wèn)題分解成移動(dòng)機(jī)器人的定位問(wèn)題與已知移動(dòng)機(jī)器人當(dāng)前位置條件下的地圖構(gòu)建問(wèn)題。Grisetti G 等［3］提出了一種使用了自適應(yīng)重采樣技術(shù)的Gmapping 方法。Lu F等［4］首次提出利用圖優(yōu)化的數(shù)學(xué)框架優(yōu)化SLAM問(wèn)題，通過(guò)非線性最小二乘方法來(lái)優(yōu)化建圖過(guò)程中累積的誤差。Konolige K 等［5］提出了首個(gè)基于圖優(yōu)化框架的開(kāi)源SLAM 方法Karto SLAM，谷歌的Cartographer開(kāi)源方法［6］在其基礎(chǔ)上融合了多傳感器數(shù)據(jù)創(chuàng)建局部子圖以及采用了閉環(huán)檢測(cè)的掃描匹配策略。在3D 激光SLAM 領(lǐng)域中，Zhang J 等人提出了基于特征點(diǎn)的LOAM 方法［7］，并在其基礎(chǔ)上結(jié)合視覺(jué) 提 出 了V-LOAM 方 法［8～10］；Daniel Lawrence Lu等［11］提出了增強(qiáng)視覺(jué)結(jié)合激光雷達(dá)的VELO方法。

本文所提出的定位方案針對(duì)荒野環(huán)境中特征稀少的特點(diǎn)，在建圖過(guò)程中使用RTK-GPS、IMU 以及激光雷達(dá)三種傳感器獲取的信息，利用GPS信息優(yōu)化正態(tài)分布變換算法的配準(zhǔn)過(guò)程，建立高精度的離線點(diǎn)云地圖；定位過(guò)程則先加載離線地圖，使用激光雷達(dá)和IMU兩種傳感器獲取的信息，然后結(jié)合正態(tài)分布變換配準(zhǔn)算法與無(wú)跡卡爾曼濾波算法得到位姿估計(jì)，并對(duì)得到的位姿估計(jì)添加地圖修正量，得到最終的位姿估計(jì)。

2 NDT-OMP算法和UKF算法

2.1 NDT-OMP配準(zhǔn)算法

正態(tài)分布變換（Normal Distribution Transform，NDT）算法由Biber 和Stra?er 提出［12］，Magnusson 等人將其拓展到了三維空間中［13］，算法的核心思想是將點(diǎn)云映射到平滑表面來(lái)表示，使用一組局部概率密度函數(shù)（PDF）來(lái)描述，每個(gè)PDF 描述表面的一部分的形狀［14～15］。算法首先將掃描所占用的空間細(xì)分為單元網(wǎng)格，基于單元網(wǎng)格內(nèi)點(diǎn)的分布計(jì)算每個(gè)單元網(wǎng)格的PDF。假設(shè)可以通過(guò)對(duì)單元網(wǎng)格內(nèi)點(diǎn)的分布的刻畫(huà)來(lái)生成x→的位置，并且參考掃描表面點(diǎn)的位置是由D維正態(tài)隨機(jī)過(guò)程產(chǎn)生的，則測(cè)量得到的的概率：

對(duì)目標(biāo)函數(shù)進(jìn)行數(shù)學(xué)形式的簡(jiǎn)化后得到

2.2 UKF算法

3 基于NDT 算法與UKF 算法的定位方法

傳統(tǒng)的基于特征點(diǎn)匹配的的SLAM 方法在環(huán)境特征較少的環(huán)境中，缺乏足夠的特征點(diǎn)進(jìn)行匹配定位，本文方法的定位過(guò)程使用了RTK-GPS、IMU以及激光點(diǎn)云信息，利用NDT-OMP 算法實(shí)現(xiàn)點(diǎn)云間的配準(zhǔn)，并使用GPS 信息對(duì)配準(zhǔn)過(guò)程進(jìn)行了優(yōu)化，最后使用g2o 框架對(duì)配準(zhǔn)后得到的位姿估計(jì)進(jìn)一步優(yōu)化，最后建立高精度的離線地圖；定位過(guò)程使用了離線點(diǎn)云地圖、IMU 以及激光點(diǎn)云的信息，結(jié)合了NDT-OMP 算法和UKF 算法得到當(dāng)前的位姿估計(jì)，并添加地圖誤差修正量，得到移動(dòng)機(jī)器人當(dāng)前位姿的最終估計(jì)。

3.1 建立離線地圖

建圖過(guò)程如圖1 所示，首先對(duì)點(diǎn)云進(jìn)行預(yù)處理，由于激光雷達(dá)獲得點(diǎn)云每一幀都包含了大量的點(diǎn)云數(shù)據(jù)，其中也包含了一部分野值點(diǎn)，所以需要對(duì)點(diǎn)云下采樣以及野值點(diǎn)的去除。完成對(duì)點(diǎn)云的預(yù)處理后，為了提升配準(zhǔn)算法的穩(wěn)定性，利用RTK-GPS 信息計(jì)算得到相鄰兩幀點(diǎn)云之間的位姿變換的初始估計(jì)值，然后使用NDT-OMP 算法進(jìn)行相鄰幀之間的配準(zhǔn)，得到位姿估計(jì)，再利用g2o 框架添加GPS以及IMU的約束，對(duì)點(diǎn)云地圖進(jìn)一步優(yōu)化后建立離線地圖。

圖1 本文算法的建圖過(guò)程

3.2 定位

定位過(guò)程如圖2 所示，第一步，加載建立好的離線地圖，對(duì)離線地圖進(jìn)行預(yù)處理后作為配準(zhǔn)時(shí)的參考幀，然后接受當(dāng)前時(shí)刻的點(diǎn)云同樣進(jìn)行預(yù)處理后作為配準(zhǔn)時(shí)的當(dāng)前幀；第二步，將IMU 信息中的線加速度與角速度作為UKF 算法的預(yù)測(cè)量結(jié)合上一時(shí)刻移動(dòng)機(jī)器人的位姿獲得當(dāng)前時(shí)刻的位姿初始估計(jì)值，將其作為配準(zhǔn)時(shí)的初始值；第三步，將當(dāng)前點(diǎn)云與離線地圖進(jìn)行配準(zhǔn)，得到配準(zhǔn)結(jié)果，將其作為UKF 算法的觀測(cè)量；第四步，使用UKF 算法計(jì)算得到當(dāng)前時(shí)刻的位姿估計(jì)值，最后添加地圖修正量，修正建圖過(guò)程帶來(lái)的誤差，得到當(dāng)前時(shí)刻移動(dòng)機(jī)器人的最終位姿估計(jì)。

圖2 本文算法的定位過(guò)程

其中地圖修正量通過(guò)當(dāng)前點(diǎn)云與路標(biāo)點(diǎn)云間的配準(zhǔn)結(jié)果計(jì)算得到：首先在特征多的區(qū)域選取路標(biāo)并保存，定位時(shí)先依據(jù)當(dāng)前點(diǎn)云位置與各個(gè)路標(biāo)之間的距離選取與當(dāng)前點(diǎn)云相對(duì)應(yīng)的路標(biāo)，然后使用ICP 算法進(jìn)行當(dāng)前點(diǎn)云與路標(biāo)點(diǎn)云之間的配準(zhǔn)，計(jì)算得到當(dāng)前點(diǎn)云與路標(biāo)點(diǎn)云之間的距離d 以及變換關(guān)系T，并設(shè)定距離閾值τ，當(dāng)d＜τ，認(rèn)為當(dāng)前點(diǎn)云與路標(biāo)點(diǎn)云配準(zhǔn)成功，每一次點(diǎn)云配準(zhǔn)成功時(shí)記錄距離d，通過(guò)比較得到其中的最小值，將對(duì)應(yīng)的轉(zhuǎn)換關(guān)系T 作為當(dāng)前時(shí)刻點(diǎn)云與路標(biāo)之間的變換關(guān)系，當(dāng)點(diǎn)云配準(zhǔn)成功的次數(shù)大于3，計(jì)算并更新當(dāng)前的地圖修正量。

4 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證本文所提出的定位方案的有效性，在內(nèi)蒙古阿拉善的荒野環(huán)境中采集了所需的數(shù)據(jù)，并進(jìn)行了實(shí)驗(yàn)驗(yàn)證。本文所使用的激光雷達(dá)是禾賽科技的激光雷達(dá)Pandar40P，探測(cè)距離為200m，垂直視角范圍為-25°～+15°，最小垂直分辨率0.33°，系統(tǒng)的運(yùn)行環(huán)境為Ubuntu16.04，所用計(jì)算機(jī)的處理器為Intel Core i5-8600，顯卡為GeForce GTX 1050 Ti，內(nèi)存大小16G。

4.1 建圖結(jié)果

圖3 是建圖結(jié)果的對(duì)比，其中（a）、（b）分別為開(kāi)源算法lego_loam 與本文建圖算法的建圖結(jié)果對(duì)比，結(jié)果表明在特征點(diǎn)較少的區(qū)域，lego_loam 算法無(wú)法很好地匹配，無(wú)法建立完整的地圖，而本文的建圖算法有效地建立了高精度的離線點(diǎn)云地圖；圖（c）、（d）分別為同一環(huán)境下配準(zhǔn)算法優(yōu)化前后的建圖效果對(duì)比，可以看出，利用GPS 信息對(duì)配準(zhǔn)過(guò)程所做的優(yōu)化解決了在特征較少的環(huán)境中可能會(huì)出現(xiàn)的配準(zhǔn)失敗的情況。

圖3 建圖結(jié)果對(duì)比圖

另外，在提升配準(zhǔn)算法穩(wěn)定性的同時(shí)也提升了配準(zhǔn)算法的效率，本文分別在340m×470m 的矩形區(qū)域與8000m 的不重復(fù)路段兩種不同的環(huán)境在進(jìn)行了驗(yàn)證，優(yōu)化前后算法的平均配準(zhǔn)時(shí)間的對(duì)比如表1 所示，結(jié)果表明優(yōu)化后的配準(zhǔn)算法效率有了明顯的提升。

表1 點(diǎn)云配準(zhǔn)的平均時(shí)間對(duì)比

4.2 定位結(jié)果

圖4 為本文算法的定位結(jié)果，在已有離線地圖的基礎(chǔ)上，實(shí)現(xiàn)了移動(dòng)機(jī)器人的精確定位。

圖4 本文算法的定位結(jié)果

圖5 為本文定位算法在地圖誤差修正前與誤差修正后的定位誤差對(duì)比，修正前最大誤差為4.77m，平均誤差為2.39m，修正后的最大誤差為2.57m，平均誤差為0.60m。從修正前后的誤差對(duì)比可以看出，本文的地圖誤差修正算法有效地減少了建圖過(guò)程所帶來(lái)的誤差，提高了定位的精度。

圖5 定位誤差分析

5 結(jié)語(yǔ)

為了解決在特征點(diǎn)較少的荒野荒野環(huán)境中的定位問(wèn)題，本文提出了一種先建立離線地圖，再進(jìn)行定位的方法，建圖過(guò)程使用GPS、IMU、激光點(diǎn)云信息，利用GPS 信息優(yōu)化NDT-OMP 配準(zhǔn)算法的配準(zhǔn)過(guò)程，建立高精度的離線點(diǎn)云地圖；定位過(guò)程先加載離線地圖，使用激光點(diǎn)云、IMU 信息，結(jié)合正態(tài)分布變換配準(zhǔn)算法與無(wú)跡卡爾曼濾波算法得到位姿估計(jì)，并對(duì)得到的位姿估計(jì)添加地圖修正量，得到最終的位姿估計(jì)。

實(shí)驗(yàn)結(jié)果表明，本文方法能有效地利用GPS信息建立高精度的離線地圖，并在無(wú)GPS條件下利用離線地圖實(shí)現(xiàn)了移動(dòng)機(jī)器人的精確定位。