基于激光慣性融合的煤礦井下移動機器人SLAM算法

楊 林，馬宏偉，王 巖

(1.西安科技大學 機械工程學院，陜西 西安 710054； 2.陜西省礦山機電裝備智能監(jiān)測重點實驗室，陜西 西安 710054)

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

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

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

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

1 系統(tǒng)框架

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

圖1 系統(tǒng)框架

2 數據預處理

2.1 IMU預積分

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

(1)

(2)

2.2 運動補償

由于激光雷達傳感器內部的旋轉機制，導致激光雷達測量不同步，因此容易產生當前幀點云失真的情況。考慮了激光雷達的自身以恒定的角速度和線速度進行勻速運動。所以，獲取了每個點的時間戳信息，并使用線性插值操作，得到每個時刻對應的姿態(tài)變換矩陣，通過使用IMU測量來進行當前幀點云的運動補償。

(3)

(4)

2.3 特征提取

本文的特征提取和匹配方法與LOAM中引入的方法相似：首先使用平滑度評價變量來選擇邊緣點和平面點，然后選擇其他關鍵幀中相應的邊緣線點和平面點，并用于構造激光雷達觀測模型的殘差。當一幀新的激光雷達點云數據經過運動補償后，接下來進行特征提取。首先通過計算局部平滑度，可以提取邊緣和平面特征。此外，點的反射率也被用作一個額外的行列式。如果一個點的反射率與相鄰的閾值不同，則也將其視為另一個邊點。根據點的曲率來計算平面光滑度作為提取當前幀特征信息的指標。則定義評估局部曲面的平滑度為

(5)

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

2.4 提取地面約束

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

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

(6)

(7)

式中，為提取巷道平面參數；[,]為激光雷達在時刻的位姿；為激光雷達在時刻的旋轉；為激光雷達在時刻的平移；，，分別為平面方程++=，軸、軸、軸坐標；′為在時刻的平移后，激光雷達傳感器到平面的距離。

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

(8)

(9)

3 前端：緊耦合激光慣性里程計

3.1 狀態(tài)定義

(10)

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

3.2 運動方程的定義

將線性化連續(xù)時間下的運動方程定義為

(11)

(12)

(13)

+1=(+Δ)

(14)

(15)

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

3.3 觀測方程的定義

觀測模型主要由激光雷達對點云所提取特征的匹配獲得，可以定義為

(16)

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

(17)

(18)

3.4 狀態(tài)更新

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

(19)

式中，‖·‖為范數。

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

(20)

(21)

(22)

3.5 狀態(tài)合成

(23)

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

4.1 初始化

4.2 關鍵幀選取

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

(24)

其中，Δ為連續(xù)2個關鍵幀之間變化的旋轉；Δ為連續(xù)2關鍵幀之間變化的平移；Δ為連續(xù)2關鍵幀之間變換的時間。

(25)

(26)

Δ=--1

(27)

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

4.3 地圖構建

(28)

4.4 回環(huán)檢測

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

算法1 回環(huán)檢測算法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

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

(29)

4.5 全局優(yōu)化

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

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

(30)

5 實驗和討論

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

圖2 輪式移動機器人

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

5.1 定量分析

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

圖3 不同算法所得到運動軌跡和真值之間的對比

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

圖5 不同算法所產生軌跡旋轉誤差與真值之間的比較

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

圖6 同時考慮平移和旋轉的絕對位姿誤差

圖7 絕對位姿誤差箱型

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

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

5.2 定性分析

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

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

圖8 當前實驗環(huán)境采用4種SLAM方法構建的的點云地圖

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

5.3 消融實驗

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

5.3.1 地面約束的影響

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

圖9 地面約束對定位和建圖的影響

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

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

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

5.4 運行時間分析

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

表4 各主要模塊平均運行時間統(tǒng)計

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

6 結 論

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

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

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