退化環(huán)境中多線激光雷達(dá)融合IMU的里程計(jì)改進(jìn)方法

0 引言

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

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

圖1 LOAM算法系統(tǒng)流程圖

1 研究背景與內(nèi)容

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

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

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

圖2 改進(jìn)激光里程計(jì)框圖

2 激光IMU里程計(jì)構(gòu)成

2.1 激光特征殘差函數(shù)構(gòu)建

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

其中，S代表在t時(shí)刻以點(diǎn)i為中心構(gòu)成的點(diǎn)云集，p、p分別是S中的點(diǎn)k與點(diǎn)l，且不是同一個(gè)點(diǎn)。

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

圖3 線特征與面特征

通過計(jì)算當(dāng)前點(diǎn)云中的角特征點(diǎn)i點(diǎn)到上一幀點(diǎn)云的線特征直線ab的距離構(gòu)成線約束，距離計(jì)算公式為式（2）。

計(jì)算當(dāng)前點(diǎn)云中的平面特征點(diǎn)i到上一幀點(diǎn)云的面特征平面mjl的距離構(gòu)成面約束，計(jì)算公式如式（3）。

2.2 IMU預(yù)計(jì)分殘差函數(shù)構(gòu)成

將當(dāng)前幀j與上一幀i之間IMU的狀態(tài)進(jìn)行積分，可以獲得在世界坐標(biāo)系下的當(dāng)前幀j的姿態(tài)（q），速度（v），位置q，從i幀到j(luò)幀的狀態(tài)轉(zhuǎn)移公式如式（4）。

至此，可以對兩幀之間狀態(tài)進(jìn)行約束，IMU預(yù)計(jì)分殘差如式（7）

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

2.3 整體誤差函數(shù)構(gòu)建

通過激光點(diǎn)云信息和IMU預(yù)計(jì)分信息共同構(gòu)建里程計(jì)求解位姿變化，需要求估計(jì)解量的i時(shí)刻狀態(tài)量如式（8）。

在上文中構(gòu)建的點(diǎn)云殘差函數(shù)和IMU預(yù)計(jì)分殘差函數(shù)的基礎(chǔ)上構(gòu)建整體的誤差函數(shù)如式（9）。

需要求解使得誤差函數(shù)最小時(shí)X的取值，此時(shí)這個(gè)問題已經(jīng)是一個(gè)非線性最小二乘的優(yōu)化問題，本文使用Google開源的非線性優(yōu)化庫Ceres Solver對本問題進(jìn)行迭代求解，最終得出最優(yōu)位姿估計(jì)，當(dāng)運(yùn)動(dòng)狀態(tài)由這些優(yōu)化后的結(jié)果不斷累加，就構(gòu)成了激光IMU里程計(jì)。可以在雷達(dá)處于退化環(huán)境中缺少約束時(shí)，由IMU提供約束，保證定位的準(zhǔn)確。

3 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)部分使用開源數(shù)據(jù)集KITTI與真實(shí)退化中實(shí)驗(yàn)驗(yàn)證平臺對本文構(gòu)建的激光IMU里程計(jì)與LOAM方法的激光里程計(jì)進(jìn)行對比分析。

3.1 KITTI數(shù)據(jù)集驗(yàn)證

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

圖4 軌跡圖對比

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

3.2 真實(shí)環(huán)境測試

圖5是本實(shí)驗(yàn)中使用的驗(yàn)證平臺，由一個(gè)16線的速騰創(chuàng)的多線激光雷達(dá)和一個(gè)IMU，以及一臺筆記本電腦構(gòu)成，實(shí)驗(yàn)環(huán)境運(yùn)行在Ubuntu18.04+ROS Melodic環(huán)境之中，通過實(shí)驗(yàn)平臺采集真實(shí)數(shù)據(jù)記錄成bag數(shù)據(jù)包，通過真實(shí)環(huán)境數(shù)據(jù)包運(yùn)行LOAM方法和本文提出方法。

圖5 實(shí)驗(yàn)平臺

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

圖6 真實(shí)實(shí)驗(yàn)場景環(huán)境

圖7 退化環(huán)境中LOAM和本文改進(jìn)方法定位與建圖結(jié)果

4 總結(jié)

本文為了改善LOAM的激光里程計(jì)在退化環(huán)境中難以獲得約束的缺點(diǎn)，在LOAM的框架上使用Lidar+IMU構(gòu)建了里程計(jì)，通過IMU運(yùn)動(dòng)估計(jì)補(bǔ)償激光里程計(jì)在退化環(huán)境中效果不佳的現(xiàn)象，通過實(shí)驗(yàn)驗(yàn)證，本文提出方法取得了良好的效果。