基于激光慣性融合的煤礦井下移動(dòng)機(jī)器人SLAM算法

楊 林，馬宏偉，王 巖

(1.西安科技大學(xué) 機(jī)械工程學(xué)院，陜西 西安 710054； 2.陜西省礦山機(jī)電裝備智能監(jiān)測(cè)重點(diǎn)實(shí)驗(yàn)室，陜西 西安 710054)

同步定位和建圖(SLAM)是實(shí)現(xiàn)煤礦井下機(jī)器人自主導(dǎo)航的基礎(chǔ)，而機(jī)器人利用各種傳感器，在三維空間中進(jìn)行6DOF狀態(tài)估計(jì)，是實(shí)現(xiàn)高性能同步定位和建圖(SLAM)的關(guān)鍵。雖然基于激光雷達(dá)的方法可實(shí)時(shí)獲得三維空間的精確場(chǎng)景測(cè)距信息，且對(duì)環(huán)境照明條件的不變性。但是，由于激光雷達(dá)測(cè)量存在運(yùn)動(dòng)失真、低頻更新和點(diǎn)云稀疏的問(wèn)題，這些特性決定了純激光雷達(dá)不利于機(jī)器人處理激進(jìn)的移動(dòng)或重復(fù)的結(jié)構(gòu)，如煤礦巷道或狹窄走廊。值得注意的是，激光雷達(dá)的缺陷可以通過(guò)融合IMU來(lái)彌補(bǔ)。與激光雷達(dá)不同，IMU不受結(jié)構(gòu)特征和環(huán)境的劇烈變化影響，并可以提供高頻率和短時(shí)間的高精度位姿估計(jì)，但由于IMU傳感器的噪聲和偏置，以及誤差累積會(huì)隨著時(shí)間的積累而漂移。為了克服獨(dú)立傳感器的這些缺點(diǎn)，可采用多傳感器融合的方法來(lái)提高狀態(tài)估計(jì)的可靠性。

多傳感器融合的相關(guān)工作，根據(jù)不同的融合框架被分為：松耦合和緊耦合?；谒神詈系娜诤戏椒ㄍǔ７謩e處理各傳感器輸入的測(cè)量，并對(duì)其測(cè)量結(jié)果進(jìn)行加權(quán)融合，以推斷機(jī)器人的運(yùn)動(dòng)狀態(tài)。LOAM和LeGO-LOAM是松耦合系統(tǒng)，IMU測(cè)量通常用于減輕無(wú)特征環(huán)境中激光退化的問(wèn)題，只以IMU計(jì)算出的姿態(tài)作為激光雷達(dá)掃描配準(zhǔn)的初始估計(jì)值和運(yùn)動(dòng)失真校正的先驗(yàn)，而不作為全局優(yōu)化的約束條件。為了提高自主機(jī)器人的定位精度，在LIO松耦合框架中融入輪速計(jì)，利用輪速計(jì)測(cè)量的速度和距離建立了EKF的觀測(cè)方程。Cartographer是基于圖優(yōu)化實(shí)現(xiàn)了激光雷達(dá)、IMU、里程計(jì)和GNSS的融合，靈活處理掃描匹配和IMU聯(lián)合約束以及其他觀測(cè)約束。一般來(lái)說(shuō)，松耦合的方法計(jì)算效率高，實(shí)現(xiàn)也相對(duì)靈活。然而，由于忽略了IMU內(nèi)部狀態(tài)的修正，它們很容易受到噪聲的干擾。與松耦合方法不同，緊耦合的方法通常會(huì)將激光雷達(dá)的原始特征點(diǎn)與IMU數(shù)據(jù)相融合，表現(xiàn)出較高的魯棒性和準(zhǔn)確性，并可分為基于優(yōu)化和基于濾波的方法。對(duì)于基于優(yōu)化的方法，LIO-mapping引入了IMU預(yù)積分，最小化IMU和激光雷達(dá)測(cè)量的殘差，并且可以通過(guò)回環(huán)檢測(cè)來(lái)糾正長(zhǎng)時(shí)間的漂移，取得了較好的精度，而該算法的實(shí)時(shí)應(yīng)用則較耗時(shí)。作為目前最先進(jìn)的算法之一，LIO-SAM通過(guò)引入激光雷達(dá)關(guān)鍵幀的滑動(dòng)窗口來(lái)限制計(jì)算復(fù)雜度，利用因子圖進(jìn)行聯(lián)合IMU和激光雷達(dá)約束優(yōu)化，以獲得更高的精度。它是一種基于因子圖的融合框架，適用于多傳感器融合。而基于濾波的方法，經(jīng)常使用擴(kuò)展卡爾曼濾波(EKF)。LINS提出了一種以機(jī)器人為中心的激光雷達(dá)慣性估計(jì)器，通過(guò)使用誤差狀態(tài)迭代卡爾曼濾波器。該方法雖然具有較高的精度，計(jì)算效率高，但在長(zhǎng)期導(dǎo)航過(guò)程中也存在漂移問(wèn)題。為了減少線性化導(dǎo)致的估計(jì)誤差對(duì)系統(tǒng)帶來(lái)的影響，R-LINS，F(xiàn)AST-LIO和FAST-LIO2都采用了迭代卡爾曼濾波器。與R-LINS和FAST-LIO相比，F(xiàn)AST-LIO2提出了一種新的數(shù)據(jù)結(jié)構(gòu)ikd-Tree來(lái)保存地圖，它可以在不提取特征的情況下直接將原始點(diǎn)注冊(cè)到地圖上，使算法的工作速度更快、更準(zhǔn)確。

盡管三維激光雷達(dá)性能優(yōu)越，但結(jié)合煤礦井下的特殊環(huán)境存在空間非結(jié)構(gòu)化、環(huán)境復(fù)雜、光照不均勻、空間狹窄等實(shí)際情況，此類(lèi)場(chǎng)景給激光SLAM帶來(lái)了新的挑戰(zhàn)：① 激光雷達(dá)在環(huán)境中識(shí)別到的幾何特征，通常是用數(shù)學(xué)方法人工定義的特征(例如點(diǎn)、線、面)，當(dāng)機(jī)器人在無(wú)幾何特征、環(huán)境相似、對(duì)稱(chēng)結(jié)構(gòu)的環(huán)境中運(yùn)行時(shí)，基于激光雷達(dá)的方案很容易特征退化。② 激光雷達(dá)測(cè)量到的數(shù)據(jù)會(huì)在某一維度上呈現(xiàn)高度的相似，主要原因是煤礦巷道環(huán)境中的側(cè)壁，只能提供橫向定位參考，而不能提供縱向定位參考。在這種情況下，側(cè)壁的特征匹配很容易導(dǎo)致錯(cuò)誤的姿態(tài)約束估計(jì)。③ 重復(fù)的激光特征會(huì)導(dǎo)致縱向上的誤導(dǎo)性約束和姿態(tài)漂移，以致點(diǎn)云配準(zhǔn)收斂至錯(cuò)誤結(jié)果，定位性能會(huì)快速劣化或完全失效。嚴(yán)重地影響了SLAM整體的性能，也限制了機(jī)器人在此類(lèi)特殊環(huán)境下自主導(dǎo)航的能力。

綜上所述，機(jī)器人在煤礦井下如何保證SLAM系統(tǒng)的估計(jì)精度、實(shí)時(shí)性、累計(jì)誤差和魯棒性之間的權(quán)衡，仍然是LIO面臨的一大難題。因此，本文提出了一種以LiDAR和IMU兩種傳感器作為輸入信息源基于緊耦合的激光慣性融合SLAM算法，并圍繞其展開(kāi)研究工作具有重要意義。

1 系統(tǒng)框架

考慮到系統(tǒng)接收來(lái)自3D LiDAR、IMU傳感器的數(shù)據(jù)。目標(biāo)是機(jī)器人進(jìn)行實(shí)時(shí)6DOF的狀態(tài)估計(jì)，并建立全局一致的地圖。本文所提出的算法系統(tǒng)框架主要由前端迭代卡爾曼濾波和后端位姿圖優(yōu)化兩部分組成，如圖1所示。在前端將傳感器數(shù)據(jù)預(yù)處理，分別建立IMU預(yù)測(cè)模型和激光雷達(dá)觀測(cè)模型，構(gòu)建迭代卡爾曼濾波器，將機(jī)器人先驗(yàn)位姿經(jīng)過(guò)預(yù)測(cè)和觀測(cè)的狀態(tài)傳播過(guò)程，使其狀態(tài)更新后的姿態(tài)更加準(zhǔn)確，進(jìn)而循環(huán)迭代得到基于緊耦合的激光慣性里程計(jì)。在后端使用關(guān)鍵幀判斷當(dāng)前位姿是否被加入到全局優(yōu)化，使用回環(huán)檢測(cè)判斷當(dāng)前位置是否被重新訪問(wèn)，這可以表述成一個(gè)最大后驗(yàn)(MAP)的問(wèn)題。此外，再優(yōu)化框架中添加了回環(huán)檢測(cè)和地面約束，優(yōu)化相鄰關(guān)鍵幀之間的相對(duì)位姿，減少累積漂移，保持全局一致，提高定位精度。最后，在全局優(yōu)化后輸出運(yùn)動(dòng)軌跡、全局地圖、狀態(tài)估計(jì)等信息。

圖1 系統(tǒng)框架

2 數(shù)據(jù)預(yù)處理

2.1 IMU預(yù)積分

根據(jù)IMU預(yù)積分算法。為了避免再傳播，而將世界坐標(biāo)系從中分離開(kāi)，只計(jì)算IMU系下通過(guò)連續(xù)兩幀和+1的位置、速度和姿態(tài)，可以表示為

(1)

(2)

2.2 運(yùn)動(dòng)補(bǔ)償

由于激光雷達(dá)傳感器內(nèi)部的旋轉(zhuǎn)機(jī)制，導(dǎo)致激光雷達(dá)測(cè)量不同步，因此容易產(chǎn)生當(dāng)前幀點(diǎn)云失真的情況?？紤]了激光雷達(dá)的自身以恒定的角速度和線速度進(jìn)行勻速運(yùn)動(dòng)。所以，獲取了每個(gè)點(diǎn)的時(shí)間戳信息，并使用線性插值操作，得到每個(gè)時(shí)刻對(duì)應(yīng)的姿態(tài)變換矩陣，通過(guò)使用IMU測(cè)量來(lái)進(jìn)行當(dāng)前幀點(diǎn)云的運(yùn)動(dòng)補(bǔ)償。

(3)

(4)

2.3 特征提取

本文的特征提取和匹配方法與LOAM中引入的方法相似：首先使用平滑度評(píng)價(jià)變量來(lái)選擇邊緣點(diǎn)和平面點(diǎn)，然后選擇其他關(guān)鍵幀中相應(yīng)的邊緣線點(diǎn)和平面點(diǎn)，并用于構(gòu)造激光雷達(dá)觀測(cè)模型的殘差。當(dāng)一幀新的激光雷達(dá)點(diǎn)云數(shù)據(jù)經(jīng)過(guò)運(yùn)動(dòng)補(bǔ)償后，接下來(lái)進(jìn)行特征提取。首先通過(guò)計(jì)算局部平滑度，可以提取邊緣和平面特征。此外，點(diǎn)的反射率也被用作一個(gè)額外的行列式。如果一個(gè)點(diǎn)的反射率與相鄰的閾值不同，則也將其視為另一個(gè)邊點(diǎn)。根據(jù)點(diǎn)的曲率來(lái)計(jì)算平面光滑度作為提取當(dāng)前幀特征信息的指標(biāo)。則定義評(píng)估局部曲面的平滑度為

(5)

可以將特征點(diǎn)劃分為兩大類(lèi)：平面點(diǎn)和邊緣點(diǎn)。然后，根據(jù)值對(duì)掃描中的點(diǎn)進(jìn)行排序，選擇最大點(diǎn)作為邊緣點(diǎn)，即在三維空間中處于尖銳邊緣上的點(diǎn)，其和周?chē)c(diǎn)的大小差距較大，曲率較高，平滑度較高。選擇最小值點(diǎn)作為平面點(diǎn)，即在三維空間中處于平滑平面上的點(diǎn)，其和周?chē)c(diǎn)的大小差距不大，曲率較低，平滑度較低。提取的邊緣線和平面在第幀激光雷達(dá)掃描的特征分別表示為：和。則組成一個(gè)在第幀所有提取的特征的集合，可以表示為={,}。

2.4 提取地面約束

根據(jù)文獻(xiàn)[9]可知，當(dāng)?shù)孛鏅C(jī)器人在不同的空間區(qū)域工作時(shí)，添加地面約束可以為關(guān)鍵幀節(jié)點(diǎn)軸位移提供額外的約束條件，同時(shí)通過(guò)使用更多的約束信息來(lái)提高姿態(tài)估計(jì)的精度來(lái)減少累積誤差。建立地面約束的關(guān)鍵是準(zhǔn)確地提取道路平面，如何花最短的時(shí)間來(lái)完成平面分割是一個(gè)關(guān)鍵問(wèn)題，并選擇一種迭代次數(shù)少、抗噪聲能力強(qiáng)的魯棒估計(jì)方法——隨機(jī)樣本共識(shí)(RANSAC)。根據(jù)RANSAC的基本原理，從點(diǎn)云的每一幀中任意選擇3個(gè)點(diǎn)，得到一個(gè)平面。常用的平面方程為：++=，其中一個(gè)++=1，>0，(，，)是平面法向量，為激光雷達(dá)傳感器到平面的距離。這4個(gè)參數(shù)可以確定一個(gè)平面。

由于機(jī)器人所經(jīng)過(guò)的地面不平整，不能以固定平面為節(jié)點(diǎn)構(gòu)造姿態(tài)圖進(jìn)行約束。為了保證所構(gòu)建的約束與實(shí)際情況相一致，提出了基于子圖的地面提取方法。根據(jù)構(gòu)建的局部點(diǎn)云圖，提取真實(shí)的平面參數(shù)為：=[,,,]。為了提高地面提取的效率，取當(dāng)前位置作為原點(diǎn)，并在激光雷達(dá)測(cè)量距離半徑范圍內(nèi)的子圖搜索點(diǎn)云。根據(jù)當(dāng)前時(shí)刻的三維激光雷達(dá)姿態(tài)，利用式(6)和(7)將地面參數(shù)轉(zhuǎn)換到激光雷達(dá)坐標(biāo)系：

(6)

(7)

式中，為提取巷道平面參數(shù)；[,]為激光雷達(dá)在時(shí)刻的位姿；為激光雷達(dá)在時(shí)刻的旋轉(zhuǎn)；為激光雷達(dá)在時(shí)刻的平移；，，分別為平面方程++=，軸、軸、軸坐標(biāo)；′為在時(shí)刻的平移后，激光雷達(dá)傳感器到平面的距離。

可以得到，姿態(tài)節(jié)點(diǎn)與地平面節(jié)點(diǎn)之間的誤差,計(jì)算可表示為式(8)。

(8)

(9)

3 前端：緊耦合激光慣性里程計(jì)

3.1 狀態(tài)定義

(10)

式中，?為四元數(shù)乘積；exp(·)為歐拉角轉(zhuǎn)四元數(shù)。

3.2 運(yùn)動(dòng)方程的定義

將線性化連續(xù)時(shí)間下的運(yùn)動(dòng)方程定義為

(11)

(12)

(13)

+1=(+Δ)

(14)

(15)

式中，為的協(xié)方差矩陣；為的協(xié)方差矩陣，通過(guò)傳感器校準(zhǔn)離線獲得。

3.3 觀測(cè)方程的定義

觀測(cè)模型主要由激光雷達(dá)對(duì)點(diǎn)云所提取特征的匹配獲得，可以定義為

(16)

式中，為(·)的測(cè)量噪聲；為測(cè)量噪聲的協(xié)方差矩陣；(·)為一個(gè)由點(diǎn)到邊對(duì)或點(diǎn)到平面對(duì)計(jì)算出的堆疊殘差向量。

(17)

(18)

3.4 狀態(tài)更新

本文采用的是迭代卡爾曼濾波(IEKF)，狀態(tài)更新可以抽象為一個(gè)優(yōu)化問(wèn)題。考慮誤差狀態(tài)是由先驗(yàn)狀態(tài)的偏差和觀測(cè)模型的殘差兩部分組成，因此可以將誤差狀態(tài)更新問(wèn)題變成一個(gè)求解先驗(yàn)狀態(tài)偏差最小和觀測(cè)模型相對(duì)姿態(tài)解算偏差最小的聯(lián)合優(yōu)化問(wèn)題?？梢员硎緸?/p>

(19)

式中，‖·‖為范數(shù)。

因此，迭代更新的卡爾曼增益方程可以表示為式(20)和后驗(yàn)狀態(tài)的均值可以表示為式(21)。

(20)

(21)

(22)

3.5 狀態(tài)合成

(23)

4 后端：位姿圖優(yōu)化

4.1 初始化

4.2 關(guān)鍵幀選取

為了保持有限數(shù)量的姿態(tài)進(jìn)行估計(jì)，在后端部署了關(guān)鍵幀的選取策略。關(guān)鍵幀的選取可以極大地提高計(jì)算效率，特別是它可以確保算法能夠在較大的環(huán)境地圖上實(shí)時(shí)運(yùn)行。選擇關(guān)鍵幀時(shí)，必須減少匹配錯(cuò)誤和冗余關(guān)鍵幀，以達(dá)到節(jié)省計(jì)算量的目的，同時(shí)稀疏的關(guān)鍵幀會(huì)增加幀間觀測(cè)的不確定性，導(dǎo)致地圖質(zhì)量下降。基于實(shí)時(shí)點(diǎn)云的關(guān)鍵幀選取標(biāo)準(zhǔn)，通常第一幀點(diǎn)云被用作關(guān)鍵幀，其余的則根據(jù)連續(xù)幀之間的相對(duì)位姿變化來(lái)判斷，設(shè)第-1關(guān)鍵幀的姿態(tài)為-1和第關(guān)鍵幀的姿態(tài)為，可使用式(24)獲得連續(xù)關(guān)鍵幀的相對(duì)位姿變化：

(24)

其中，Δ為連續(xù)2個(gè)關(guān)鍵幀之間變化的旋轉(zhuǎn)；Δ為連續(xù)2關(guān)鍵幀之間變化的平移；Δ為連續(xù)2關(guān)鍵幀之間變換的時(shí)間。

(25)

(26)

Δ=--1

(27)

當(dāng)以上3個(gè)標(biāo)準(zhǔn)中的任何一個(gè)超過(guò)設(shè)定的閾值時(shí)，當(dāng)前第幀點(diǎn)云選取為關(guān)鍵幀。2個(gè)關(guān)鍵幀之間的其他激光雷達(dá)幀則被丟棄。以這種方式添加關(guān)鍵幀不僅可以實(shí)現(xiàn)地圖密度和內(nèi)存消耗之間的平衡，而且在空間中均勻分布，這適用于實(shí)時(shí)非線性?xún)?yōu)化。

4.3 地圖構(gòu)建

(28)

4.4 回環(huán)檢測(cè)

為了減小傳感器的累積誤差，添加了回環(huán)檢測(cè)，當(dāng)機(jī)器人在運(yùn)行過(guò)程中檢測(cè)到歷史關(guān)鍵幀，將結(jié)合歷史關(guān)鍵幀中的所有相關(guān)位姿進(jìn)行全局優(yōu)化。在執(zhí)行循環(huán)檢測(cè)時(shí)，首先將當(dāng)前的實(shí)時(shí)關(guān)鍵幀與歷史關(guān)鍵幀之間進(jìn)行比較，然后選擇候選回環(huán)幀。滿足以下條件：① 當(dāng)前關(guān)鍵幀的索引大于歷史關(guān)鍵幀的索引；② 當(dāng)前關(guān)鍵幀與歷史關(guān)鍵幀的軌跡距離之差大于設(shè)定的閾值；③ 當(dāng)前關(guān)鍵幀與歷史關(guān)鍵幀之間的相對(duì)平移距離小于設(shè)定的閾值。最后，通過(guò)比較候選回環(huán)幀和當(dāng)前已注冊(cè)的關(guān)鍵幀之間，選擇最高得分的候選回環(huán)幀。將關(guān)鍵幀和候選回環(huán)幀作為節(jié)點(diǎn)添加到SLAM圖優(yōu)化中，邊緣約束為點(diǎn)云配準(zhǔn)得到的相對(duì)姿態(tài)。

算法1 回環(huán)檢測(cè)算法Input:Pm and Tm from the current frame point cloudOutput:TICP1:if(Tm,Pm).isKeyframe() then2: if DT≠? or DP≠? then3: T'm← KDtree.RadiusSearch(Tm,DT,r);4: M ← registerPointCloud(DT,DP);5: if T'm≠0 then6: TICP← ComputeICP(Pm,M,Tm);7: end if8: end if9: T'm← 010:DT=DT∪Tm11:DP=DP∪Pm12:end if

只有獲得當(dāng)前幀的點(diǎn)云在世界坐標(biāo)中的當(dāng)前姿態(tài)，可以將和反饋給全局優(yōu)化進(jìn)行狀態(tài)校正。狀態(tài)校正可以表示為(29)進(jìn)行修改，通過(guò)求解以下代價(jià)函數(shù)：

(29)

4.5 全局優(yōu)化

為了有效地解決大規(guī)模同步定位和建圖問(wèn)題，人們通常采用集束調(diào)整(BA)的方法，即利用傳感器姿態(tài)和空間點(diǎn)進(jìn)行的圖優(yōu)化。但是，隨著機(jī)器人運(yùn)行的軌跡將持續(xù)增長(zhǎng)，BA方法會(huì)使系統(tǒng)隨地圖規(guī)模的持續(xù)增加而計(jì)算效率降低。針對(duì)這一問(wèn)題，所采用的位姿圖優(yōu)化只將其作為位姿估計(jì)的約束條件，不再對(duì)特征點(diǎn)的位姿估計(jì)進(jìn)行優(yōu)化。并基于位姿圖優(yōu)化理論，計(jì)算出關(guān)鍵幀姿態(tài)、地面約束和回環(huán)檢測(cè)約束，構(gòu)建出基于多約束因子的位姿圖優(yōu)化代價(jià)函數(shù)。

姿態(tài)圖優(yōu)化問(wèn)題可以通過(guò)標(biāo)準(zhǔn)的優(yōu)化方法有效地解決，如高斯-牛頓或黎文堡馬夸特(LM)算法。它已經(jīng)集成到優(yōu)化庫(kù)的通用圖優(yōu)化(G2O)。目標(biāo)是估計(jì)機(jī)器人6DOF的狀態(tài)估計(jì)，并同時(shí)建立一個(gè)全局一致的地圖。假設(shè)這些測(cè)量誤差符合零均值高斯分布時(shí)，求解最大后驗(yàn)(MAP)問(wèn)題等價(jià)于最小化負(fù)對(duì)數(shù)似然，負(fù)對(duì)數(shù)似然可以寫(xiě)成馬氏距離平方的形式：

(30)

5 實(shí)驗(yàn)和討論

對(duì)本文所提出的激光慣性融合SLAM算法性能進(jìn)行了一系列的定量和定性分析實(shí)驗(yàn)，并與其他最先進(jìn)的激光SLAM方法進(jìn)行比較。并且，所有的方法都是在相同的條件下進(jìn)行測(cè)試的。使用的硬件平臺(tái)是帶有傳感器和機(jī)載計(jì)算機(jī)的輪式移動(dòng)機(jī)器人平臺(tái)，如圖2所示。此處實(shí)驗(yàn)使用的傳感器包含Velodyne VLP-16激光雷達(dá)和hipnuc-CH110的IMU，該激光雷達(dá)的采樣頻率為10 Hz，而IMU的采樣頻率為200 Hz。機(jī)載計(jì)算機(jī)是一臺(tái)英特爾酷睿i7，具有2.7 GHz主頻，八核和16 G內(nèi)存。所有的算法都在C++中實(shí)現(xiàn)，并使用ROS的medoic版本在Ubuntu18.04系統(tǒng)中執(zhí)行。

圖2 輪式移動(dòng)機(jī)器人

由于煤礦井下無(wú)法獲取真值作比較，為了驗(yàn)證本文所提出方法的性能(精度、絕對(duì)誤差和魯棒性)，同時(shí)保證實(shí)驗(yàn)的合理性。首先使用了M2DGR公共數(shù)據(jù)集的hall_05序列進(jìn)行定量分析。注意，所有的算法都在沒(méi)有回環(huán)的情況下進(jìn)行了測(cè)試。為了驗(yàn)證所提出方法的精度和絕對(duì)誤差，分別進(jìn)行了不同算法與真值之間軌跡誤差(平移和旋轉(zhuǎn))的對(duì)比和絕對(duì)位姿誤差(平移和旋轉(zhuǎn))的對(duì)比。為了測(cè)試4種不同算法所得到軌跡的全局一致性，進(jìn)行了在SE(3)下同時(shí)考慮旋轉(zhuǎn)和平移的絕對(duì)位姿誤差分析及箱型圖。其次，為了驗(yàn)證所提出的方法在不同煤礦井下環(huán)境，是否具有良好的魯棒性。用筆者的試驗(yàn)平臺(tái)記錄了3組自采數(shù)據(jù)集進(jìn)行定性分析，并將該方法與a_loam，lego_loam和lio_sam算法做比較，通過(guò)不同算法之間的橫向?qū)Ρ群筒煌瑘?chǎng)景之間的縱向?qū)Ρ?，?yàn)證了所提出算法具有良好的魯棒性。此外，還設(shè)計(jì)了消融實(shí)驗(yàn)分別驗(yàn)證了添加地面約束和回環(huán)約束對(duì)整個(gè)優(yōu)化結(jié)果的影響。最后，針對(duì)主要模塊進(jìn)行了運(yùn)行時(shí)間分析，驗(yàn)證了其可以滿足系統(tǒng)實(shí)時(shí)性的需求。

5.1 定量分析

為了評(píng)估所提出方法的精度和誤差狀態(tài)，選擇使用M2DGR公共數(shù)據(jù)集的hall_05序列進(jìn)行不同算法之間的測(cè)試?？梢钥闯?，aloam所產(chǎn)生的軌跡與真值之間差別較大，lego_loam和lio_sam所產(chǎn)生的軌跡與真值之間除部分區(qū)域有細(xì)微差別外，其他部分基本相同，提出的算法所產(chǎn)生的軌跡最接近真值。為了探究不同算法之間的差別，本文采用EVO工具對(duì)其進(jìn)行定量分析，不同算法所得到運(yùn)動(dòng)軌跡和真值之間的對(duì)比如圖3所示，平移誤差如圖4所示，旋轉(zhuǎn)誤差如圖5所示。

圖3 不同算法所得到運(yùn)動(dòng)軌跡和真值之間的對(duì)比

圖4 不同算法所產(chǎn)生軌跡平移誤差與真值之間的比較

圖5 不同算法所產(chǎn)生軌跡旋轉(zhuǎn)誤差與真值之間的比較

為了測(cè)試4種不同算法所得到軌跡的全局一致性，分別統(tǒng)計(jì)了M2DGR公共數(shù)據(jù)集的hall_05序列運(yùn)行4種不同算法，不同軌跡和真值之間的絕對(duì)位姿誤差統(tǒng)計(jì)，見(jiàn)表3。以下所得到數(shù)據(jù)中最好的結(jié)果以粗體顯示。在SE(3)下分析了同時(shí)考慮旋轉(zhuǎn)和平移誤差的絕對(duì)位姿誤差如圖6所示，其對(duì)應(yīng)的箱型圖如圖7所示。

圖6 同時(shí)考慮平移和旋轉(zhuǎn)的絕對(duì)位姿誤差

圖7 絕對(duì)位姿誤差箱型

表3 4種不同算法與真值之間絕對(duì)位姿誤差統(tǒng)計(jì)

統(tǒng)計(jì)結(jié)果表明，在M2DGR公共數(shù)據(jù)集hall_05序列上進(jìn)行不同算法之間的測(cè)試，所提出的算法相對(duì)于其他3種算法而言，具有更小的絕對(duì)位姿誤差。因此，所提出算法狀態(tài)估計(jì)的精度更優(yōu)。

5.2 定性分析

為了驗(yàn)證本文所提出算法在煤礦井下的實(shí)際表現(xiàn)，分別進(jìn)行了3次相同配置不同環(huán)境下的測(cè)試，3個(gè)自采數(shù)據(jù)集包括煤礦巷道長(zhǎng)廊場(chǎng)景1、煤礦巷道水泵房和配電站場(chǎng)景2、煤礦巷道工作面場(chǎng)景3，并將本文所提出算法分別與aloam，lego_loam和lio_sam方法進(jìn)行了分析與比較，其在當(dāng)前實(shí)驗(yàn)環(huán)境所構(gòu)建的點(diǎn)云地圖，如圖8所示。

根據(jù)圖8所示，4種不同的激光SLAM算法分別構(gòu)建煤礦井下3種不同場(chǎng)景的點(diǎn)云地圖，圖8(a)為典型的“長(zhǎng)廊”結(jié)構(gòu)，圖8(b)包含豐富的場(chǎng)景變化存在回環(huán)結(jié)構(gòu)，圖8(c)為典型的“口字型”回環(huán)結(jié)構(gòu)。由于3種測(cè)試場(chǎng)景皆為煤礦井下感知退化環(huán)境，其點(diǎn)云相對(duì)特征較少，因此給測(cè)試算法帶來(lái)了很大的挑戰(zhàn)。對(duì)于4種不同的算法來(lái)說(shuō)，aloam在3種場(chǎng)景下也有不錯(cuò)的表現(xiàn)，但在圖8(a)場(chǎng)景下由于出現(xiàn)連續(xù)的相似旋轉(zhuǎn)和平移，導(dǎo)致點(diǎn)云產(chǎn)生了少量的誤匹配。lego_loam在圖8(a)和(b)兩種場(chǎng)景下，環(huán)境的大致輪廓可以構(gòu)建出完整點(diǎn)云地圖，但由于軸上出現(xiàn)了明顯的漂移，導(dǎo)致所生成點(diǎn)云地圖的結(jié)構(gòu)變形和不完整。在圖8(c)場(chǎng)景下，由于口字型場(chǎng)景中存在相似結(jié)構(gòu)，導(dǎo)致原始點(diǎn)云數(shù)據(jù)的提取的特征受到了大量的相似點(diǎn)云干擾，而使整個(gè)SLAM系統(tǒng)失敗。lio_sam在圖8(a),(b)和(c)三種場(chǎng)景下，可以在短時(shí)間內(nèi)構(gòu)建出周?chē)h(huán)境的幾何結(jié)構(gòu)，但隨著激光里程計(jì)里程的增加，由于部分場(chǎng)景相似，導(dǎo)致建立了錯(cuò)誤的回環(huán)約束，或發(fā)生軸漂移，以致點(diǎn)云地圖構(gòu)建失敗。而本文所提出的方法在3種場(chǎng)景下，所構(gòu)建的點(diǎn)云地圖在完整性和幾何結(jié)構(gòu)真實(shí)性方面均有著優(yōu)秀的表現(xiàn)。由此可見(jiàn)本文，所提出的算法在煤礦井下環(huán)境，具有良好的魯棒性。

圖8 當(dāng)前實(shí)驗(yàn)環(huán)境采用4種SLAM方法構(gòu)建的的點(diǎn)云地圖

總之，4種不同算法在相同環(huán)境下所構(gòu)建出不同效果的點(diǎn)云地圖，是由于自身的優(yōu)缺點(diǎn)所導(dǎo)致。a_loam的方案采用勻速運(yùn)動(dòng)模型假設(shè)，利用IMU數(shù)據(jù)來(lái)進(jìn)行點(diǎn)云的畸變校正，系統(tǒng)實(shí)時(shí)性好，但是沒(méi)有閉環(huán)檢測(cè)模塊，導(dǎo)致它在大規(guī)模測(cè)試中也存在漂移。Lego_loam的方法是loam的優(yōu)化版本，對(duì)激光雷達(dá)掃描的原始數(shù)據(jù)幀進(jìn)行了分割和聚類(lèi)，將地面點(diǎn)云進(jìn)行了標(biāo)記，通過(guò)兩步LM優(yōu)化來(lái)提高狀態(tài)估計(jì)的精度。但是由于其過(guò)于依賴(lài)點(diǎn)面點(diǎn)云的提取，一旦地面點(diǎn)云提取失敗，會(huì)導(dǎo)致?tīng)顟B(tài)估計(jì)誤差的增加。Lio_sam是lego-loam的擴(kuò)展版本，通過(guò)不斷加入各種因子，聯(lián)合來(lái)求解最小二乘的問(wèn)題。但是回環(huán)檢測(cè)對(duì)正確數(shù)據(jù)的關(guān)聯(lián)性要求較高，錯(cuò)誤的匹配會(huì)導(dǎo)致整個(gè)SLAM系統(tǒng)的失敗。本文所提出的方法，融合了上述算法的優(yōu)點(diǎn)。首先，前端分別采用了loam中的勻速模型假設(shè)和lego_loam中提取地面特征的思想，但是通過(guò)RANSAC的方法進(jìn)行點(diǎn)云地面的提取，改進(jìn)了LINS中的卡爾曼濾波模型，提出了一種迭代卡爾曼濾波模型，消除錯(cuò)誤匹配引起的誤差，以實(shí)現(xiàn)緊耦合的激光慣性里程計(jì)。其次，后端借鑒了lio_sam中因子圖優(yōu)化的思想，通過(guò)關(guān)鍵幀的選取策略，添加地面約束和回環(huán)檢測(cè)約束，構(gòu)建位姿圖優(yōu)化的代價(jià)函數(shù)，通過(guò)求解最大后驗(yàn)的問(wèn)題，進(jìn)一步提高狀態(tài)估計(jì)的精度。

5.3 消融實(shí)驗(yàn)

為了進(jìn)一步分析本文所提出算法的定位和建圖效果。分別分析了該算法的2個(gè)后端優(yōu)化模塊的影響，地面約束和回環(huán)檢測(cè)。

5.3.1 地面約束的影響

通過(guò)實(shí)驗(yàn)分析，系統(tǒng)中后端優(yōu)化的地面約束對(duì)建圖結(jié)果有很大的影響。如圖9所示。圖9(a)沒(méi)有在后端優(yōu)化中添加地面約束，所構(gòu)造的點(diǎn)云地圖無(wú)法進(jìn)行校正，使其向一定的角度傾斜。圖9(b)在后端優(yōu)化中添加了地面約束，修正了定位和建圖過(guò)程中的軸漂移，最后構(gòu)建了一個(gè)全局一致的點(diǎn)云地圖。

圖9 地面約束對(duì)定位和建圖的影響

5.3.2 回環(huán)檢測(cè)的影響

通過(guò)實(shí)驗(yàn)分析，系統(tǒng)中后端優(yōu)化的回環(huán)檢測(cè)對(duì)地圖的構(gòu)造有很大的影響，如圖10所示。圖10(a)在后端優(yōu)化中無(wú)回環(huán)約束，這導(dǎo)致了構(gòu)建的點(diǎn)云圖有較大的偏差，以致右上角部分區(qū)域出現(xiàn)部分地圖重影，且初始位置和最終位置發(fā)生漂移。圖10(b)在后端優(yōu)化中添加了回環(huán)約束。對(duì)全局地圖的累積誤差進(jìn)行了修正，最后構(gòu)造了一個(gè)全局一致的點(diǎn)云地圖。

圖10 回環(huán)檢測(cè)對(duì)定位和建圖的影響

5.4 運(yùn)行時(shí)間分析

為了探索所提出算法的實(shí)時(shí)性能，分別在自采數(shù)據(jù)集和M2DGR公共數(shù)據(jù)集上進(jìn)行了實(shí)驗(yàn)，詳見(jiàn)表4。由表4可知，由于自采數(shù)據(jù)集來(lái)自VLP-16數(shù)據(jù)，而M2DGR數(shù)據(jù)集來(lái)自HDL-32數(shù)據(jù)。因此，對(duì)于激光SLAM系統(tǒng)來(lái)說(shuō)，在采樣頻率相同的情況下，處理M2DGR數(shù)據(jù)集各個(gè)模塊則需要更長(zhǎng)的時(shí)間。針對(duì)自采數(shù)據(jù)進(jìn)行分析，對(duì)于前端激光點(diǎn)云和IMU數(shù)據(jù)預(yù)處理，每幀的平均時(shí)間僅為10 ms左右，整個(gè)過(guò)程沒(méi)有較大的偏差。對(duì)于激光慣性里程計(jì)，掃描匹配大約需要3 ms，最長(zhǎng)需要10 ms迭代匹配才能收斂。構(gòu)建局部地圖的構(gòu)建大約需要2 ms。由于回環(huán)檢測(cè)需要循環(huán)判斷，也需要在回環(huán)幀之間進(jìn)行基于ICP的配準(zhǔn)，進(jìn)行姿態(tài)優(yōu)化，平均回環(huán)檢測(cè)時(shí)間約35 ms，全局優(yōu)化的平均時(shí)間約13 ms，可以快速滿足收斂條件。

表4 各主要模塊平均運(yùn)行時(shí)間統(tǒng)計(jì)

結(jié)合系統(tǒng)框架(圖1)可知，首先將200 Hz工作頻率的IMU預(yù)積分后和10 Hz工作頻率的LiDAR進(jìn)行緊耦合，通過(guò)緊耦合后同步輸出10 Hz的激光慣性里程計(jì)到后端優(yōu)化模塊，而構(gòu)建的LiDAR-IMU融合SLAM系統(tǒng)前端和后端為2個(gè)獨(dú)立的線程進(jìn)行工作，因此前后端各模塊的每一幀數(shù)據(jù)處理時(shí)間之和只要小于100 ms(10 Hz)，即可滿足系統(tǒng)實(shí)時(shí)性的要求。

6 結(jié) 論

(1)提出了一種針對(duì)煤礦井下環(huán)境的移動(dòng)機(jī)器人基于激光慣性融合的SLAM算法。通過(guò)前端構(gòu)建迭代卡爾曼濾波器和后端位姿圖優(yōu)化的方案，增強(qiáng)了SLAM系統(tǒng)針對(duì)此類(lèi)非結(jié)構(gòu)化場(chǎng)景的魯棒性。通過(guò)在優(yōu)化框架中添加了回環(huán)檢測(cè)和地面約束，優(yōu)化了相鄰關(guān)鍵幀之間的相對(duì)位姿，減小了傳感器的累計(jì)誤差，提高了系統(tǒng)的精度，確保了全局地圖一致性。

(2)該算法在公共數(shù)據(jù)集和自采數(shù)據(jù)集上分別進(jìn)行了定量和定性的分析，并與現(xiàn)有的激光SLAM算法進(jìn)行比較。結(jié)果表明，該算法在此類(lèi)特殊環(huán)境中能夠?qū)崿F(xiàn)穩(wěn)健的6DOF狀態(tài)估計(jì)和全局一致的同步定位與建圖，并且在全局優(yōu)化后輸出運(yùn)動(dòng)軌跡、全局地圖、狀態(tài)估計(jì)等信息。

(3)該算法分別從地面約束和回環(huán)檢測(cè)2個(gè)方面進(jìn)行消融實(shí)驗(yàn)，分析了各自對(duì)整個(gè)系統(tǒng)的影響，解釋了該算法的合理性。并分別在不同數(shù)據(jù)集上統(tǒng)計(jì)分析了系統(tǒng)前端和后端各主要模塊的運(yùn)行時(shí)間，證明了該算法可以滿足實(shí)時(shí)性的要求。