移動機器人三維激光SLAM 算法研究

歐陽仕晗，劉振宇，趙怡巍，秦圣然，劉 瀟

（1.沈陽工業(yè)大學信息科學與工程學院， 沈陽110870；2.中國華能集團燃料有限公司營口分公司，營口115007；3.東北大學機器人科學與工程學院，沈陽110016）

1 引 言

移動機器人（Mobile Robot）相對于固定工作的臂式機器人具有更大的工作范圍，是當今機器人研究的熱點。同時定位與地圖構(gòu)建（Simultaneous Localization and Mapping， SLAM）技術(shù)是移動機器人實現(xiàn)自主移動與定位的關鍵，得到廣泛的研究與應用[1]。與視覺傳感器相比，激光雷達可提供更加穩(wěn)定和準確的信息，不受外部干擾，因此能夠為SLAM 提供一個更為可靠的解決方案。在過去20 年里基于激光雷達的SLAM 一直是移動機器人導航研究的基礎。隨著現(xiàn)代激光SLAM 技術(shù)的發(fā)展，三維激光SLAM成為目前最先進的移動機器人SLAM 技術(shù)之一。主流的三維激光SLAM 算法有：基于關鍵點的LOAM及其改進算法LeGO_LOAM 和A_LOAM、LIO；基于數(shù)據(jù)驅(qū)動的SegMatch 及改其改進算法SegMap；基于掃描點的BLAM、IMLS-SLAM、ICP-LO；基于圖優(yōu)化的Karto SLAM 和Cartographer 等[2]。張吉等人提出一個低漂移實時激光雷達測距建圖算法[3-4]（Lidar Odometry and Mapping，LOAM），在公開數(shù)據(jù)集KITTI（Karlsruhe Institute of Technology and Toyota Technological Institute）里程計數(shù)據(jù)集上僅使用激光雷達來估計移動機器人位姿的精度最高的算法[5]，但LOAM沒有閉環(huán)檢測功能，導致了漂移誤差會隨時間增加。鐵小山等人提出一種輕量級和地面優(yōu)化的激光雷達測距和建圖算法[6]（Lightweight and Ground-Optimized Lidar Odometry and Mapping，LeGO_LOAM），相比于LOAM，在減少計算量的情況下實現(xiàn)了相似或更好的精度及建圖工作，其在線執(zhí)行閉環(huán)檢測的能力使其成為長時間導航任務的有用工具，但它的閉環(huán)檢測有時會出現(xiàn)錯誤或識別遺漏。秦通等人提出了LOAM的高級實現(xiàn)（Advanced implementation of LOAM， A_LOAM），用Eigen 和Ceres Solver 來簡化代碼結(jié)構(gòu)[7]，相比于LOAM，代碼更為簡潔，無需復雜的數(shù)學推導和冗余操作。R. Dubé 等人提出了在三維點云中基于分割匹配的位置識別算法[8]（Segment based loopclosure for 3D pointclouds， SegMatch），它可以用在室內(nèi)和室外環(huán)境，然而算法依賴于初始位姿，沒有全球定位系統(tǒng)傳感器就無法工作。隨后R.Dubé 等人又提出了SegMatch 的改進算法：使用數(shù)據(jù)驅(qū)動描述子進行3D 片段建圖算法[9]（3D Segment Mapping using Data-Driven Descriptors， SegMap），該算法引入一種新穎的數(shù)據(jù)驅(qū)動的片段描述符，提取的語義信息可以提高對環(huán)境變化的魯棒性，但基于分割匹配的三維激光SLAM 算法SegMatch 及其改進算法SegMap產(chǎn)生的漂移隨著時間推移累計成顯著漂移。Google在ICRA 會議上公布了Cartographer 開源項目[10]，在工程實現(xiàn)上構(gòu)建了一套完整的室內(nèi)雷達SLAM 系統(tǒng)而非簡單的算法，其技術(shù)框架完整，建圖效率很高。文獻[11]提出了一種新的基于三維激光雷達數(shù)據(jù)的SLAM 系統(tǒng)，使用隱式移動最小二乘（IMLS）表面來表示模型，提高了系統(tǒng)魯棒性。Behley 和Stachniss提出了一種基于面元的運動估計建圖方法SuMa[12]，該方法允許表示大規(guī)模環(huán)境并且維護點云的詳細幾何信息，但是稀疏點云對其是一個挑戰(zhàn)。廖瑞杰等人提出了以邊匹配為主要依據(jù)的基于K-公共子圖判定的室外場景三維點云閉環(huán)檢測算法并命名為SegGraph[13]，采用公開數(shù)據(jù)集KITTI 進行評估，實驗結(jié)果顯示該算法具有良好的準確度和運行效率。除了上述幾種傳統(tǒng)SLAM 方法外，研究人員們還研究了使用深度學習的里程估算方法[14-15]，但尚未達到同等水平。

針對上述研究現(xiàn)狀及存在的問題，鑒于LOAM及其改進算法LeGO_LOAM 和A_LOAM 是非常低漂移、實時的激光雷達測距方法，以下詳細討論分析新松大尺度室外環(huán)境和公開數(shù)據(jù)集KITTI 下這三種算法的工作原理和相對位姿估計誤差。

2 三維激光SLAM 算法

2.1 LOAM 算法原理

LOAM 是一種使用3D 激光雷達進行狀態(tài)估計和建圖的實時、低漂移的激光雷達測距方法。其關鍵思想是將復雜的SLAM 問題劃分為高頻的運動估計和低頻的環(huán)境建圖，利用測程算法和建圖算法的協(xié)作尋求同時優(yōu)化大量變量，實現(xiàn)實時精確的運動估計和建圖。其中測程算法以高頻率但低保真度（低精度）進行里程測量以估計激光雷達的速度，消除了激光雷達運動引起的點云失真，通過保證快速計算來找到特征點的對應關系。建圖算法利用未失真的點云以低一個數(shù)量級的頻率但高保真度（高精度）運行用于點云的精確匹配和配準。將兩個估計融合在一起即可產(chǎn)生高頻率和高精度的單個運動估計。一方面，兩種算法的組合允許實現(xiàn)實時建圖，并行算法結(jié)構(gòu)保證了實時求解問題的可行性，能夠?qū)崿F(xiàn)低漂移和低計算復雜度且無需高精度測距或慣性測量；另一方面，算法的基于特征的激光掃描數(shù)據(jù)匹配方法對邊緣/平面執(zhí)行點-特征掃描匹配，以此找到掃描之間的對應關系。相比于基于散點的傳統(tǒng)方法，此法定位精度更高，易于擴展到大場景。LOAM 主要有四個模塊組成：掃描匹配、激光雷達測程算法、激光雷達建圖算法和變換融合。其軟件系統(tǒng)如圖1 所示。

圖1 LOAM 軟件系統(tǒng)結(jié)構(gòu)圖

首先令P?為激光雷達一次掃描接收到的點，每次掃描期間，P?在激光雷達坐標系中配準，在掃描K期間，配準后的點云形成PK。用兩種算法處理PK。激光雷達測程算法獲取點云并計算兩次連續(xù)掃描之間的激光雷達運動。估計的運動用于校正PK的失真。該算法以大約10Hz 的頻率運行。輸出通過激光雷達建圖算法進一步處理，算法以1Hz 的頻率將未失真的點云配準到地圖上。最后將兩種算法發(fā)布的位姿變換集成在一起，以產(chǎn)生相對于地圖上的激光雷達位姿大約10Hz 的變換輸出。

2.2 LeGO_LOAM 算法原理

LeGO_LOAM 是一種輕量級和地面優(yōu)化的激光雷達測距和建圖方法，適用于具有可變地形的復雜環(huán)境，對移動機器人進行位姿估計，并實時估計地面車輛的六自由度位姿。與原始廣義的LOAM 框架相比，LeGO_LOAM 旨在提高地面車輛的效率和準確性。算法主要由五個模塊組成：分割、特征提取、激光雷達測距算法、激光雷達建圖算法和變換融合。其軟件系統(tǒng)和激光雷達測程算法框架分別如圖2 和圖3 所示。

圖2 LeGO_LOAM 軟件系統(tǒng)結(jié)構(gòu)圖

圖3 LeGO_LOAM 激光雷達測距模塊兩步優(yōu)化

首先在分割模塊中采用一次掃描的點云，并將其投影到深度圖像上進行分割，利用點云分割濾除噪聲。然后將分割的點云發(fā)送到特征提取模塊進行特征提取，以獲得獨特的平面和邊緣特征，丟棄在地面分離之后可能表示不可靠特征的無價值點。接著在激光雷達測程模塊中使用從特征提取模塊中提取的特征來找到在連續(xù)連續(xù)掃描過程中機器人位置的相關轉(zhuǎn)換，采用兩步Levenberg-Marquardt 優(yōu)化方法分別計算位姿變換的不同分量來估計位姿，使用平面和邊緣特征來解決連續(xù)掃描中六自由度變換的不同分量。從地面提取的平面特征用于在第一步中獲得[tz，θroll，θpitch]，從地面特征中確認機器人當前位姿z、roll 和pitch。在第二步中通過匹配從分割的點云提取的邊緣特征來獲得變換的其余部分[tx，ty，θyaw]，在每一幀點云數(shù)據(jù)匹配的過程中得到機器人位姿x、y 和yaw。之后這些特征在激光雷達建圖模塊中進一步處理，配準到全局點云圖，用于激光雷達的點云建圖。最后的變換集成模塊融合激光雷達測程算法和激光雷達建圖算法的位姿估計結(jié)果并輸出最終位姿估計。LeGO_LOAM 依賴于PCL、OpenCV、GTSAM 三個庫。其中，GTSAM 是一個在機器人領域和計算機視覺領域用于平滑和建圖的C++庫[16]，它與g2o 不同，后者采用稀疏矩陣的方式求解一個非線性優(yōu)化問題[17]，而GTSAM 則是采用因子圖（factor graphs）優(yōu)化方法和基于貝葉斯樹的iSAM2方法的方式最大化后驗概率。

2.3 A_LOAM 算法特點

A_LOAM 是LOAM 的高級實現(xiàn)，其原理類似于LOAM，使用Eigen 和Ceres Solver 來簡化代碼結(jié)構(gòu)。相比于LOAM，該算法的可讀性更高、非常適合學習、無需復雜的數(shù)學推導和冗余操作。尤其是雅各比的部分，LOAM 對其的推導方法為了解析地求出雅各比的表達式使用了一些小技巧來統(tǒng)一不同特征點的殘差函數(shù)導數(shù)表達，較難理解；而A_LOAM 直接使用Google 開發(fā)的在C++環(huán)境下的非線性最小二乘問題的自動求導工具包Cere Solver[7]72，節(jié)省開發(fā)的時間，得到準確的解。整個SLAM 最復雜的運算不需要推導。

3 軟硬件實驗平臺

為驗證算法的可行性、可靠性、有效性、準確性和優(yōu)劣性，已在環(huán)境復雜度和路徑長度依次增長的新松智慧產(chǎn)業(yè)園三期C5 廠區(qū)外圍及停車場、公開數(shù)據(jù)集KITTI 的五個大尺度室外場景中進行了LOAM及其改進算法LeGO_LOAM 和A_LOAM 的相對位姿估計誤差實驗。實驗選擇一點作為移動機器人的行駛起點，同時也將該點設為移動機器人的行駛終點，即移動機器人路徑為一個閉環(huán)。行駛路線為從起點開始，行駛一段距離，分別圍繞C5 廠區(qū)外圍一周和停車場，最終駛回初始位置。實驗環(huán)境中既有結(jié)構(gòu)化場景又有非結(jié)構(gòu)化場景；移動機器人行駛路面整體較為平坦，但也有局部的起伏；行駛距離分別約為300m、650m、960m、2200m 和3700m，實驗環(huán)境的規(guī)模與復雜程度符合預定要求。如圖4 所示，即為從新松智慧產(chǎn)業(yè)園三期C5 廠區(qū)外圍和停車場中拍攝的實驗場地照片。

圖4 新松C5 廠區(qū)室外場景

如圖5 所示為所搭建的硬件實驗驗證平臺——新松室外移動機器人平臺。它的中層配備了1.8GHz四核處理器的小米筆記本電腦，接收各種信息進行處理的工作。上層是三維激光傳感器Velodyne VLP-16，是系統(tǒng)的傳感單元。慣性導航使用A0010。底層是移動機器人BlueCar，它是整個系統(tǒng)的執(zhí)行單元，最高移動速度為1m/s。

圖5 實驗硬件平臺

選取Velodyne 公司生產(chǎn)的型號為Velodyne VLP-16 的激光傳感器，如圖6 所示。該型號保留了電機轉(zhuǎn)速可調(diào)節(jié)功能，能夠?qū)崟r上傳周圍距離和反射率的測量值，具有100 米的遠量程測量距離。Velodyne VLP-16 的外觀設計十分精巧，重量只有830g，非常便于安裝，尤其適于小型無人機和小型移動機器人，對移動機器人的大眾市場需求來說有重大的意義。Velodyne VLP-16 具有每秒高達30 萬點數(shù)據(jù)輸出、±15°的垂直視場和360°的水平視場掃描，支持兩次回波接收，可以測量第一次回波和最后一次回波的距離值和反射強度值，能夠掃描探測出16 個層面的環(huán)境信息，而成本較之于64 級的下降了10 倍，因此多應用在汽車、建圖測量、無人機、機器人導航避障、環(huán)境三維建模、自動化等行業(yè)。

圖6 Velodyne VLP-16 激光傳感器實物圖

導航系統(tǒng)通過USB 接口將激光獲得到的距離數(shù)據(jù)傳輸給筆記本電腦，用于精確的地圖構(gòu)建。此處選取基于MEMS（微機電系統(tǒng)）慣性傳感元件的超小型、高精度慣性測算系統(tǒng)A0010 陀螺轉(zhuǎn)角儀，如圖7所示。該系統(tǒng)可在任意時刻、任意運動狀態(tài)下精確輸出載體的航向角以及其它輔助傳感信號（加速度、溫度等）。由于系統(tǒng)采用了可借助先進MEMS 技術(shù)進行標準化、可批量化大規(guī)模生產(chǎn)的元器件，大幅度降低了產(chǎn)品成本。該模塊可被廣泛應用于AGV 小車、航空、航天、航海、無人機、機器人、工業(yè)設備監(jiān)控、汽車電子等場合。

圖7 高性能陀螺傳感器A0010

選取Ubuntu 16.04 操作系統(tǒng)下Kinetic 版本的ROS 作為軟件平臺。ROS 的眾多優(yōu)點，如點對點設計、支持多種編程語言、代碼復用及免費與開源等，使其成為機器人研究的熱點。導航系統(tǒng)的調(diào)試及界面顯示使用ROS 中的可視化工具Rviz，可在其中構(gòu)建移動機器人模型和環(huán)境地圖，同時實時查看機器人從初始點到設定目標點之間執(zhí)行導航任務的過程?？衫肦OS 中的tf 坐標轉(zhuǎn)換功能包完成激光傳感器和移動機器人坐標的自動轉(zhuǎn)換。

在此采用的數(shù)據(jù)集測試平臺是公開數(shù)據(jù)集KITTI。KITTI 由德國卡爾斯魯厄理工學院和豐田美國技術(shù)研究院聯(lián)合創(chuàng)辦，是目前國際上最大的自動駕駛場景下的計算機視覺算法評測數(shù)據(jù)集。該數(shù)據(jù)集用于評測立體圖像、光流、視覺測距、3D 物體檢測和3D 跟蹤等計算機視覺技術(shù)在車載環(huán)境下的性能。該數(shù)據(jù)集主要涵蓋三種類型的環(huán)境：周圍有建筑物的“城市”，在場景中有植被的“鄉(xiāng)村小路”，以及道路寬闊、周圍環(huán)境相對干凈的“公路”。KITTI 包含這些場景下采集的真實圖像數(shù)據(jù)，每張圖像中最多達15 輛車和30 個行人，還有各種程度的遮擋與截斷。整個數(shù)據(jù)集由389 對立體圖像和光流圖、39.2km 視覺測距序列以及超過200k3D 標注物體的圖像組成，以10Hz 的頻率采樣及同步。

如圖8 所示，公開數(shù)據(jù)集KITTI 采集平臺包括一個Velodyne 3D 激光雷達、兩個彩色立體攝像機、兩個灰度立體攝像機、四個光學鏡頭以及一個高精度GPS 導航系統(tǒng)。激光雷達數(shù)據(jù)以10Hz 的頻率記錄。圖9 給出了兩個來自城市住宅區(qū)環(huán)境的相應視覺圖像。

圖8 公開數(shù)據(jù)集KITTI 的數(shù)據(jù)采集平臺

圖9 序列的視覺圖像

4 實驗結(jié)果及分析

為驗證地圖的全局一致性，在此進行多次實驗，將每個實驗的初始位姿設置為[0，0，0，0]作為真值，在LOAM、LeGO_LOAM 和A_LOAM 算法中將移動機器人返回起始位置時計算的最終位姿數(shù)據(jù)取多次實驗的平均值并導出，與真值進行對比，從而比較相對位姿估計誤差，如表1 所示。從表中可見，對于前四個場景，有IMU 輔助的新松“C5”大樓周圍的道路及停車場和序列05 的一致性較好。在五個場景中，LeGO_LOAM 和A_LOAM 的誤差都相對較小。對于前三個場景，有IMU 輔助的新松“C5”大樓周圍道路及停車場中，LeGO_LOAM 和A_LOAM 的誤差在1m以內(nèi)。在新松“C5”大樓周圍的道路中，LeGO_LOAM的位姿估計精度，相對于LOAM，在平移和旋轉(zhuǎn)上分別提高了91.67%和26.13%；A_LOAM 的位姿估計精度在平移和旋轉(zhuǎn)上分別提高了83.33%和70.13%；在新松“C5”大樓周圍道路及小范圍停車場中，LeGO_LOAM 的位姿估計精度在平移和旋轉(zhuǎn)上分別提高了95.15%和24.97%；A_LOAM 的位姿估計精度在平移和旋轉(zhuǎn)上分別提高了86.94%和53.94%；在新松“C5”大樓周圍道路及大范圍停車場中，LeGO_LOAM 的位姿估計精度在平移和旋轉(zhuǎn)上分別提高了91.79%和81.11%；A_LOAM 的位姿估計精度在平移和旋轉(zhuǎn)上分別提高了91.79%和83.54%；在序列05 中，LeGO_LOAM 的位姿估計精度在平移和旋轉(zhuǎn)上分別提高了18.32%和96.06%，A_LOAM 的位姿估計精度在平移和旋轉(zhuǎn)上分別提高了24.50%和97.09%；在序列00 中，LeGO_LOAM的位姿估計精度在平移和旋轉(zhuǎn)上分別提高了92.70%和92.03%。A_LOAM 的位姿估計精度在平移和旋轉(zhuǎn)上分別提高了92.41%和80.83%。

表1 移動機器人返回起始位置時的相對位姿估計誤差

結(jié)果表明，由于LeGO_LOAM 具有閉環(huán)檢測功能且A_LOAM 的算法復雜度較低，因此與LOAM 相比，LeGO_LOAM 和A_LOAM 可以達到更小的相對位姿估計誤差。

5 研究難題與思路

5.1 需進一步研究的問題

雖然三維激光SLAM 已得到深入的研究，但在以下兩方面還存在問題：

1）LOAM 及其改進算法A_LOAM 無法實現(xiàn)閉環(huán)。由于LOAM 及其改進算法A_LOAM 是一個非常完整的系統(tǒng)，所以在LOAM 及其改進算法A_LOAM中集成閉環(huán)并不是一種簡單易行的方法。未來要做的工作主要涉及通過閉環(huán)修復LOAM 及其改進算法A_LOAM 的運動估計漂移。

2）僅使用視覺傳感器或激光傳感器在SLAM應用中都有局限性和缺點。一方面，視覺測距方法中的視覺傳感器，例如深度相機，需要為每個圖像計算ORB 特性，這需要很長時間，在特征點稀少處無法正確定位，且需要適度的照明條件，如果不能充分獲得不同的視覺特征則會失敗。在極端情況下，特征點提取和匹配算法由于外部光線的干擾而無法追蹤特征點。另一方面，由于激光雷達在連續(xù)運動期間的不同時間接收距離測量導致通過移動激光雷達的運動估計涉及點云中的運動失真，因此運動通常必須用大量變量來解決。在諸如由平面區(qū)域支配的退化場景中，掃描匹配也難以奏效。

基于激光和視覺傳感器融合的SLAM 方法能夠有效利用各個傳感器的優(yōu)勢，彌補傳感器在某些特殊環(huán)境下的劣勢，成為當前研究的熱點之一。還有待做出更多的研究工作，進一步實現(xiàn)將視覺傳感器與激光傳感器融合并，研究融合算法，使之提高導航的精度和魯棒性。

5.2 未來研究思路及展望

鑒于上述問題的存在，今后的研究應主要應在以下方面實現(xiàn)突破：

1、LOAM 及其改進算法A_LOAM 的閉環(huán)檢測方面。具體思路有兩個：第一，使用FPGA 代替ROS功能包從激光雷達波束中心提取點的傳感器中提取信號，并將其與ANN（人工神經(jīng)網(wǎng)絡）與GA（遺傳算法）相結(jié)合，以更好地更新當前狀態(tài)；第二，鑒于LOAM 及其改進算法A_LOAM 的映射是提高SLAM性能的關鍵，應在LOAM 及其改進算法A_LOAM 的低頻高精度的環(huán)境建圖模塊中修改相關參數(shù)。

2、傳感器融合方面，借鑒例如張吉等人提出的V_LOAM 方案[18]。針對傳感器數(shù)據(jù)融合這一關鍵問題，采用如估計理論數(shù)據(jù)融合、分類數(shù)據(jù)融合、推理數(shù)據(jù)融合、人工智能數(shù)據(jù)融合等方法，根據(jù)所處環(huán)境合理地選擇單個傳感器或傳感器融合數(shù)據(jù)作為關鍵幀構(gòu)建地圖[19]，這種根據(jù)環(huán)境狀況合理選擇傳感器數(shù)據(jù)的方式能夠降低數(shù)據(jù)融合的計算復雜度并且更好地適應于復雜的動態(tài)環(huán)境。

除此之外，在后續(xù)研究中，還將研究原始點云的濾波方法，以快速去除動態(tài)物體并提高抗干擾能力；同時，將應用范圍擴大到地下車庫場景，以解決無人駕駛車輛的自動停車問題。

6 結(jié) 束 語

對三維激光SLAM 技術(shù)進行了探討總結(jié)，從激光雷達測距與建圖算法LOAM 及其改進算法LeGO_LOAM 和A_LOAM 的關鍵組成模塊及其具體作用進行了深入和充分的回顧與歸納。鑒于LOAM 及其改進算法LeGO_LOAM 和A_LOAM 的低漂移、強實時的特性，以新松智慧產(chǎn)業(yè)園三期C5廠區(qū)外圍及停車場為實例，詳細討論分析了大尺度室外環(huán)境，以及公開數(shù)據(jù)集KITTI 下的LOAM 及其改進算法LeGO_LOAM 和A_LOAM 的相對位姿估計誤差。實驗結(jié)果表明，相對于LOAM，LeGO_LOAM和A_LOAM 可以達到更小的相對位姿估計誤差。對于當前存在的問題及難點，也提出了詳細合理的研究思路及展望。