荒野環境中的移動機器人定位方法研究*

鐘釬釬 郭劍輝

（南京理工大學計算機科學與工程學院 南京 210094）

1 引言

近年來移動機器人的應用范圍越來越廣泛，而如何實現移動機器人在所處運行環境中自主準確的定位導航，是移動機器人技術的關鍵問題之一。在室外環境中，基于激光雷達的定位方法以其可以準確測量障礙點的角度與距離、無需預先布置場景、可融合多傳感器、可以在光線較差的環境中工作以及能夠生成便于導航的地圖等優勢，成為目前不可或缺的一種定位方案。

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

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

2 NDT-OMP算法和UKF算法

2.1 NDT-OMP配準算法

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

對目標函數進行數學形式的簡化后得到

2.2 UKF算法

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

傳統的基于特征點匹配的的SLAM 方法在環境特征較少的環境中，缺乏足夠的特征點進行匹配定位，本文方法的定位過程使用了RTK-GPS、IMU以及激光點云信息，利用NDT-OMP 算法實現點云間的配準，并使用GPS 信息對配準過程進行了優化，最后使用g2o 框架對配準后得到的位姿估計進一步優化，最后建立高精度的離線地圖；定位過程使用了離線點云地圖、IMU 以及激光點云的信息，結合了NDT-OMP 算法和UKF 算法得到當前的位姿估計，并添加地圖誤差修正量，得到移動機器人當前位姿的最終估計。

3.1 建立離線地圖

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

圖1 本文算法的建圖過程

3.2 定位

定位過程如圖2 所示，第一步，加載建立好的離線地圖，對離線地圖進行預處理后作為配準時的參考幀，然后接受當前時刻的點云同樣進行預處理后作為配準時的當前幀；第二步，將IMU 信息中的線加速度與角速度作為UKF 算法的預測量結合上一時刻移動機器人的位姿獲得當前時刻的位姿初始估計值，將其作為配準時的初始值；第三步，將當前點云與離線地圖進行配準，得到配準結果，將其作為UKF 算法的觀測量；第四步，使用UKF 算法計算得到當前時刻的位姿估計值，最后添加地圖修正量，修正建圖過程帶來的誤差，得到當前時刻移動機器人的最終位姿估計。

圖2 本文算法的定位過程

其中地圖修正量通過當前點云與路標點云間的配準結果計算得到：首先在特征多的區域選取路標并保存，定位時先依據當前點云位置與各個路標之間的距離選取與當前點云相對應的路標，然后使用ICP 算法進行當前點云與路標點云之間的配準，計算得到當前點云與路標點云之間的距離d 以及變換關系T，并設定距離閾值τ，當d＜τ，認為當前點云與路標點云配準成功，每一次點云配準成功時記錄距離d，通過比較得到其中的最小值，將對應的轉換關系T 作為當前時刻點云與路標之間的變換關系，當點云配準成功的次數大于3，計算并更新當前的地圖修正量。

4 實驗結果與分析

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

4.1 建圖結果

圖3 是建圖結果的對比，其中（a）、（b）分別為開源算法lego_loam 與本文建圖算法的建圖結果對比，結果表明在特征點較少的區域，lego_loam 算法無法很好地匹配，無法建立完整的地圖，而本文的建圖算法有效地建立了高精度的離線點云地圖；圖（c）、（d）分別為同一環境下配準算法優化前后的建圖效果對比，可以看出，利用GPS 信息對配準過程所做的優化解決了在特征較少的環境中可能會出現的配準失敗的情況。

圖3 建圖結果對比圖

另外，在提升配準算法穩定性的同時也提升了配準算法的效率，本文分別在340m×470m 的矩形區域與8000m 的不重復路段兩種不同的環境在進行了驗證，優化前后算法的平均配準時間的對比如表1 所示，結果表明優化后的配準算法效率有了明顯的提升。

表1 點云配準的平均時間對比

4.2 定位結果

圖4 為本文算法的定位結果，在已有離線地圖的基礎上，實現了移動機器人的精確定位。

圖4 本文算法的定位結果

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

圖5 定位誤差分析

5 結語

為了解決在特征點較少的荒野荒野環境中的定位問題，本文提出了一種先建立離線地圖，再進行定位的方法，建圖過程使用GPS、IMU、激光點云信息，利用GPS 信息優化NDT-OMP 配準算法的配準過程，建立高精度的離線點云地圖；定位過程先加載離線地圖，使用激光點云、IMU 信息，結合正態分布變換配準算法與無跡卡爾曼濾波算法得到位姿估計，并對得到的位姿估計添加地圖修正量，得到最終的位姿估計。

實驗結果表明，本文方法能有效地利用GPS信息建立高精度的離線地圖，并在無GPS條件下利用離線地圖實現了移動機器人的精確定位。