退化環境中多線激光雷達融合IMU的里程計改進方法

0 引言

近年來，我們在生活中越來越多地看到自主移動機器人出現在生活中，如家庭中使用的掃地機器人可以根據室內場景環境，規劃軌跡路徑，將全屋無死角打掃；餓了么和美團研發的無人自動售貨車，在現實環境的馬路上可以自主運動與避障；百度小米等公司研發的完全自動駕駛汽車在智能網聯汽車管理法實施之后，已經開始在實際環境中完全脫離人工控制自主駕駛測試。這一切的發展都依賴于同步定位與建圖（Simultaneous Localization and Mapping，SLAM）技術近些年的快速發展，SLAM技術是指機器人處于未知環境中，通過傳感器在運動過程中估計自身位姿，同時完成環境地圖的構建。

由于機器人承擔的任務與工作的環境越來越復雜，當前室外情況下SLAM技術主流的傳感器使用的是視覺傳感器或者多線激光雷達，由于視覺傳感器極易受環境與天氣影響，如暗光環境中感光元件寬容度不夠無法獲取充足信息等不穩定情況，多線激光雷達是更加穩定的選擇。2014年，卡耐基梅隆大學的Zhang Ji等人提出了LOAM算法，LOAM算法是三維SLAM中表現十分亮眼的算法，在使用多線激光雷達的三維SLAM中是里程碑的存在，該算法最有貢獻的地方是提出了一種通過特征點提取構成約束的前端高頻低精準度的激光里程計和后端低頻高精準的掃描幀與地圖匹配的地圖構建，通過這種方法對掃描匹配的方法進行了拆分，前端幀幀匹配的激光里程計有著很好的實時性，后端幀與地圖的匹配提高了精度保證建圖的準確性，實現了系統資源的合理分配，系統框架如圖1。

圖1 LOAM算法系統流程圖

1 研究背景與內容

由于LOAM算法僅依賴多線激光雷達進行定位與建圖，前端激光里程計使用的是特征提取的方法，在城市道路環境中，道路兩邊建筑物結特征明顯且豐富，激光里程計定位較為準確，當處于一些結構特征不明顯，不豐富的情況場景中會產生退化現象，使得機器人無法正確地估計自身的位姿，導致定位與建圖的結果與真實場景產生較大的偏差，當定位不準確，地圖構建錯誤，機器人也就喪失了自主移動的能力。

退化現象的產生原因是因為機器人所處的環境中結構幾何信息缺乏方向約束，導致機器人在相應方向上移動定位與建圖的準確性明顯降低。退化場景可以分為單方向結構特征稀疏場景，如高速公路、鄉村道路、橋梁、長直走廊、單側或兩側由灌木叢或者墻體構成的道路等。在此類場景中，使用多線激光雷達構成的激光里程計中對于前后方向上不容易提取出有特征性的集合約束，所以在建圖過程中，單方向結構特征稀疏場景容易造成激光里程計定位軌跡估計過長或過短；多方向結構特征稀疏場景，如空曠的操場，廣場等，這時使用多線激光雷達的機器人在SLAM過程中在前后左右方向均缺少約束，容易造成激光里程計運動估計軌跡左右偏移或著長短發生變化。

退化現象是由于多線激光雷達對此類場景難以處理，是由于傳感器本體原理所限制的，所以處理此類問題必須引入新的傳感器與多線激光雷達形成互補。本文在使用LOAM算法的基礎上改進激光里程計，引入IMU傳感器，通過使用IMU預積分為激光里程計幀間約束在退化環境中約束不足提供正確的運動估計，保證建圖與定位的準確性，改進激光里程計框圖如圖2所示。

圖2 改進激光里程計框圖

2 激光IMU里程計構成

2.1 激光特征殘差函數構建

由于激光點云信息量較為豐富，匹配消耗資源，本文使用LOAM算法提出的根據點的曲率大小進行特征點的提取，計算公式如式（1）：

其中，S代表在t時刻以點i為中心構成的點云集，p、p分別是S中的點k與點l，且不是同一個點。

通過計算曲率后將曲率大的作為角特征點，曲率小的作為面特征點，角特征點在不同線束的點云中尋找最近一點構成線特征，面特征點從相同線束和不同線束的點云中各選取最近的一點構成面特征。示意圖如圖3。

圖3 線特征與面特征

通過計算當前點云中的角特征點i點到上一幀點云的線特征直線ab的距離構成線約束，距離計算公式為式（2）。

計算當前點云中的平面特征點i到上一幀點云的面特征平面mjl的距離構成面約束，計算公式如式（3）。

2.2 IMU預計分殘差函數構成

將當前幀j與上一幀i之間IMU的狀態進行積分，可以獲得在世界坐標系下的當前幀j的姿態（q），速度（v），位置q，從i幀到j幀的狀態轉移公式如式（4）。

至此，可以對兩幀之間狀態進行約束，IMU預計分殘差如式（7）

其中，e、e、e、e、e分別表示位置、姿態、速度、加速度偏置、角速度偏置的殘差。

2.3 整體誤差函數構建

通過激光點云信息和IMU預計分信息共同構建里程計求解位姿變化，需要求估計解量的i時刻狀態量如式（8）。

在上文中構建的點云殘差函數和IMU預計分殘差函數的基礎上構建整體的誤差函數如式（9）。

需要求解使得誤差函數最小時X的取值，此時這個問題已經是一個非線性最小二乘的優化問題，本文使用Google開源的非線性優化庫Ceres Solver對本問題進行迭代求解，最終得出最優位姿估計，當運動狀態由這些優化后的結果不斷累加，就構成了激光IMU里程計。可以在雷達處于退化環境中缺少約束時，由IMU提供約束，保證定位的準確。

3 實驗驗證

實驗部分使用開源數據集KITTI與真實退化中實驗驗證平臺對本文構建的激光IMU里程計與LOAM方法的激光里程計進行對比分析。

3.1 KITTI數據集驗證

KITTI數據集01軌跡采集于高速公路，使用此軌跡數據集運行LOAM方法與本文提出方法記錄軌跡，通過evo工具將軌跡畫出，與數據集提供真實軌跡對比，軌跡對比圖如圖4。

圖4 軌跡圖對比

其中，LOAM方法估計的軌跡長度是2006.542米，本文提出的Lidar-imu方法估計的軌跡是2451.735米，真實軌跡的長度是2453.259米，從輸出軌跡長度來看，相比于LOAM的方法，本文提出的方法在高速公路退化環境中對于軌跡長度的軌跡要遠好于LOAM方法。從圖4的軌跡圖中可以看出，本文提出的Lidar-imu在軌跡上更接近于真值，角度的估計上也好于LOAM方法。

3.2 真實環境測試

圖5是本實驗中使用的驗證平臺，由一個16線的速騰創的多線激光雷達和一個IMU，以及一臺筆記本電腦構成，實驗環境運行在Ubuntu18.04+ROS Melodic環境之中，通過實驗平臺采集真實數據記錄成bag數據包，通過真實環境數據包運行LOAM方法和本文提出方法。

圖5 實驗平臺

實驗場景如圖6為一被樹木包圍的空曠廣場上進行。實驗中按照順時針方向在廣場上移動。在移動過程中，實驗平臺的左側一直面向廣場缺少約束的環境，右側一直是樹木這種幾何結構缺少約束的環境。最終結果如圖7，左側LOAM算法在此退化環境中獲取不到足夠約束，定位建圖失敗，右側是本文提出方法，通過添加了IMU對位姿進行了估計，很好地完成了定位與建圖的任務。

圖6 真實實驗場景環境

圖7 退化環境中LOAM和本文改進方法定位與建圖結果

4 總結

本文為了改善LOAM的激光里程計在退化環境中難以獲得約束的缺點，在LOAM的框架上使用Lidar+IMU構建了里程計，通過IMU運動估計補償激光里程計在退化環境中效果不佳的現象，通過實驗驗證，本文提出方法取得了良好的效果。