礦山環(huán)境三維激光雷達(dá)SLAM算法建圖與定位

張清宇,崔麗珍,杜秀鐸,馬寶良

(內(nèi)蒙古科技大學(xué)信息工程學(xué)院,內(nèi)蒙古 包頭 014010)

近年來,我國煤礦智能化發(fā)展迅速,礦山環(huán)境下的精確定位導(dǎo)航技術(shù)研發(fā)更是重中之重。由于煤炭工業(yè)的工作環(huán)境相對(duì)頑劣,安全問題一直制約著煤礦的生產(chǎn)建設(shè)。同步定位與地圖構(gòu)建是無人駕駛領(lǐng)域解決惡劣環(huán)境建圖定位問題的關(guān)鍵技術(shù),三維激光雷達(dá)SLAM算法正被廣泛應(yīng)用于煤礦環(huán)境的高精度制圖工作。

針對(duì)煤礦復(fù)雜環(huán)境,文獻(xiàn)[1]設(shè)計(jì)了一種煤礦巡檢機(jī)器人,以滿足實(shí)際礦山探測(cè)工作的需求。文獻(xiàn)[2]利用地面激光雷達(dá)計(jì)算復(fù)雜地形土方量。文獻(xiàn)[3]對(duì)煤礦井下巷道的難定位問題提出了一種改進(jìn)的巷道環(huán)境建模算法。文獻(xiàn)[4]針對(duì)煤礦環(huán)境地圖構(gòu)建過程中出現(xiàn)的點(diǎn)云運(yùn)動(dòng)畸變提出了點(diǎn)云雙邊濾波算法。文獻(xiàn)[5]研究了煤礦井下無GPS的機(jī)器人自主定位問題,實(shí)現(xiàn)了三維激光雷達(dá)SLAM算法的仿真試驗(yàn)。

對(duì)于三維激光雷達(dá)SLAM算法的應(yīng)用與發(fā)展,文獻(xiàn)[6]提出的LOAM改變了特征提取的方式,通過點(diǎn)云匹配對(duì)雷達(dá)位姿進(jìn)行優(yōu)化。文獻(xiàn)[7]提出了迭代最近點(diǎn)ICP算法。文獻(xiàn)[8]提出了通過慣性測(cè)量單元(inertial measurement unit,IMU)估計(jì)運(yùn)動(dòng)狀態(tài)消除激光雷達(dá)運(yùn)動(dòng)畸變的方法。文獻(xiàn)[9]提出的BALM算法,在雷達(dá)建圖過程中引入光速法平差以降低建圖過程中的累積誤差。文獻(xiàn)[10]提出了激光-慣性里程計(jì)相結(jié)合的方法LIO。文獻(xiàn)[11]提出的LOL方法,將 LOAM和分割場(chǎng)景識(shí)別算法相結(jié)合。文獻(xiàn)[12]提出的LIRO方法,將UWB、雷達(dá)和慣導(dǎo)緊耦合。文獻(xiàn)[13—14]提出了LeGO-LOAM和LIO-SAM算法,前者在算法的后端添加地面優(yōu)化且特征提取方式更輕量級(jí),提高了特征點(diǎn)的提取效率;后者將LiDAR里程計(jì)因子、IMU預(yù)積分因子、GPS因子和回環(huán)因子通過因子圖優(yōu)化的方式融合,實(shí)現(xiàn)多傳感器融合定位。針對(duì)激光雷達(dá)耗時(shí)長、受動(dòng)態(tài)障礙物影響大的問題,文獻(xiàn)[15]在閉環(huán)檢測(cè)部分添加了SC-Context。文獻(xiàn)[16]提出了FAST-LIO算法,緊耦合迭代卡爾曼濾波和雷達(dá)慣性里程計(jì),提高了穩(wěn)健性。

本文為了解決礦山復(fù)雜環(huán)境下建圖定位的難點(diǎn),首先對(duì)各傳感器進(jìn)行內(nèi)外參數(shù)標(biāo)定,然后通過巡檢機(jī)器人采集到的數(shù)據(jù),對(duì)比多種三維激光雷達(dá)SLAM算法,以期驗(yàn)證在礦山環(huán)境中多傳感器融合算法LIO-SAM具有更好的建圖效果與定位精度,并構(gòu)建在LIO-SAM特征提取部分利用點(diǎn)的強(qiáng)度和環(huán)境值消除不必要的特征點(diǎn)的IALIO算法,以期在定位精度上有進(jìn)一步的提高。

1 傳感器參數(shù)標(biāo)定

單一的激光雷達(dá)、里程計(jì)、慣性測(cè)量單元很難單獨(dú)滿足高效、實(shí)時(shí)、準(zhǔn)確、穩(wěn)健等性能,因此多傳感器融合的方式應(yīng)用于復(fù)雜環(huán)境下的建圖定位是目前SLAM算法主要的發(fā)展趨勢(shì)。

對(duì)于LiDAR融合IMU的系統(tǒng),為解決機(jī)器人運(yùn)動(dòng)產(chǎn)生的激光雷達(dá)運(yùn)動(dòng)畸變,需要標(biāo)定LiDAR與IMU的外參。本文采用LiDAR＿align的方法進(jìn)行激光雷達(dá)與IMU的外參標(biāo)定,利用NLopt庫處理定義沖突問題并改寫里程計(jì)接口為IMU接口,錄制包含大量旋轉(zhuǎn)量和平移量的LiDAR數(shù)據(jù)及IMU數(shù)據(jù),多次標(biāo)定迭代后取收斂結(jié)果最小的參數(shù)。

IMU內(nèi)參未標(biāo)定時(shí)會(huì)造成建圖的漂移現(xiàn)象甚至無法建圖,嚴(yán)重影響定位精度。本文采用的IMU的觀測(cè)模型中存在兩種誤差:一種是變化緩慢的零偏b;另一類是變化比較劇烈的測(cè)量白噪聲n。其中,零偏是由傳感器的溫度等物理因素產(chǎn)生的傳感器內(nèi)部誤差的綜合參數(shù);測(cè)量白噪聲是由于模/數(shù)轉(zhuǎn)換器引起的外部噪聲,角速度測(cè)量值ωo和真實(shí)值ω之間的關(guān)系為

ωo(t)=ω(t)+b(t)+n(t)

(1)

本文通過內(nèi)參標(biāo)定工具imu＿utils進(jìn)行IMU內(nèi)參的標(biāo)定,即標(biāo)定隨機(jī)游走噪聲和白噪聲。通過ceres優(yōu)化庫分析IMU的Allan方差,將IMU保持靜止錄制2 h的數(shù)據(jù)包,確定最終的高斯白噪聲和隨機(jī)游走噪聲。

2 特征匹配及相對(duì)位姿估計(jì)

不同SLAM算法的特征提取方式是有差別的,特征點(diǎn)的獲取決定了機(jī)器人相對(duì)位姿數(shù)量的多少,直接影響建圖效果與定位精度。

LOAM通過計(jì)算當(dāng)前幀點(diǎn)云pk中每個(gè)點(diǎn)i的平滑度c,將c最小的點(diǎn)作為平面點(diǎn),c最大的點(diǎn)作為邊緣點(diǎn),將一幀掃描到的點(diǎn)云分為4段,每段只提取最多4個(gè)平面點(diǎn)和2個(gè)邊緣點(diǎn),以保證特征點(diǎn)的分布均勻,其中平滑度c的計(jì)算公式為

(2)

LeGO-LOAM是在LOAM基礎(chǔ)上改進(jìn)的激光雷達(dá)建圖方法,與LOAM相比,在輕量級(jí)和地面優(yōu)化部分有了顯著提升,可以在平坦路面有效處理點(diǎn)線特征和點(diǎn)面特征,但是由于提取的線面特征較多,會(huì)使得累計(jì)誤差較大。LeGO-LOAM的主要特點(diǎn)包括:①對(duì)地面進(jìn)行分割,減小了特征搜索范圍;②提取特征之前進(jìn)行聚類分析,提高了特征質(zhì)量;③以幀為單位進(jìn)行優(yōu)化,使得全局地圖可以多次調(diào)整;④增加了回環(huán)修正。LeGO-LOAM的地面優(yōu)化主要是在LiDAR Odometry的特征提取部分,通過使用采集到的地面面特征優(yōu)化高度和水平角、采集到的線特征優(yōu)化水平位移和航向角,構(gòu)建點(diǎn)與線之間、面與面之間的對(duì)應(yīng)關(guān)系,具體框架如圖1所示。

圖1 LiDAR Odometry框架

本身促進(jìn)兩者聯(lián)動(dòng)的活動(dòng)較少。社工機(jī)構(gòu)是讓社會(huì)工作和志愿服務(wù)的聯(lián)動(dòng)的主要組織一，在這個(gè)載體上可以開展很多活動(dòng)。但目前，我國社會(huì)工作和志愿服務(wù)的聯(lián)動(dòng)模式和機(jī)制都處于探索過程中，還沒有完全探索出本土化的聯(lián)動(dòng)模式和機(jī)制，因此，除了政府購買的社會(huì)服務(wù)合作項(xiàng)目之外，社工機(jī)構(gòu)很少有能力有主動(dòng)性去開展合作項(xiàng)目和活動(dòng)，還是自上而下的項(xiàng)目比較多，有社工機(jī)構(gòu)主導(dǎo)的自下而上的合作項(xiàng)目相對(duì)較少，還處于初始階段。

本文提出LIO-SAM在特征提取部分僅計(jì)算每個(gè)點(diǎn)的曲率,通過曲率的大小提取角點(diǎn)和平面點(diǎn),規(guī)定當(dāng)前激光點(diǎn)為pi,利用pi前后的5個(gè)點(diǎn)計(jì)算當(dāng)前的曲率K。前后5個(gè)點(diǎn)與pi的距離差值為M,曲率K為距離差值的平方,具體計(jì)算公式為

M=M[i-5]+M[i-4]+M[i-3]+M[i-2]+

M[i-1]+M[i]·10+M[i+1]+M[i+2]+

M[i+3]+M[i+4]+M[i+5]

(3)

K=M·M=M2

(4)

這種方法沒有區(qū)分“平面”與“近似平面”,將沒有計(jì)算分類的點(diǎn)全部加入“近似平面”中,會(huì)丟失一部分符合曲率要求的特征點(diǎn)。本文采用的IALIO算法增加了曲率的閾值,利用點(diǎn)的強(qiáng)度和環(huán)境刪除不必要的點(diǎn),一定程度上彌補(bǔ)了丟失點(diǎn)的問題,在定位精度上也有一定提高。

3 多傳感器融合算法LIO-SAM

LIO-SAM是通過激光雷達(dá)、IMU和GPS多種傳感器融合,利用因子圖優(yōu)化的方式在因子圖的頂端設(shè)置了激光雷達(dá)慣性里程計(jì),收集不同來源的多種相對(duì)測(cè)量和絕對(duì)測(cè)量值作為因子,全部合并到系統(tǒng)中。將激光雷達(dá)里程計(jì)因子、IMU預(yù)積分因子、GPS因子和回環(huán)因子全部優(yōu)化,在礦山復(fù)雜環(huán)境下能夠針對(duì)性地應(yīng)用。

3.1 IMU預(yù)積分因子

IMU預(yù)積分因子部分的具體流程如圖2所示。

圖2 IMU預(yù)積分因子

其中,Xi表示每個(gè)關(guān)鍵幀,線條表示IMU預(yù)積分因子,它由兩個(gè)相鄰關(guān)鍵幀之間的IMU測(cè)量積分得到。IMU的角速度和加速度的測(cè)量值公式為

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

式中,ΔVij、ΔPij和ΔRij分別為時(shí)間i至j之間的相對(duì)速度、位置和旋轉(zhuǎn)矩陣;Vi和Vj分別為i、j時(shí)刻的速度;Pi和Pj分別為i、j時(shí)刻的位置;Ri和Rj分別為i、j時(shí)刻的旋轉(zhuǎn)矩陣;g為重力加速度;Δtij為i、j時(shí)刻的時(shí)間間隔。

3.2 激光里程計(jì)因子

圖3 激光里程計(jì)因子

構(gòu)建得到的局部地圖Mi公式為

(13)

3.3 回環(huán)檢測(cè)因子

在后端優(yōu)化部分,LIO-SAM引入了回環(huán)檢測(cè),添加了回環(huán)因子,回環(huán)檢測(cè)因子如圖4所示。

圖4 回環(huán)檢測(cè)因子

當(dāng)因子圖中加入一個(gè)新的狀態(tài)Xi+1時(shí),首先搜索整個(gè)因子圖,找到與之相近的先驗(yàn)狀態(tài),X3是返回的候選之一,然后嘗試匹配Fi+1到子關(guān)鍵幀{F3-m,…F3,…,F3+m}。本文對(duì)LeGO-LOAM算法的回環(huán)檢測(cè)部分添加了Scan-Context描述子,定位精度更優(yōu)。

4 試驗(yàn)與分析

4.1 試驗(yàn)設(shè)備及試驗(yàn)方案

為驗(yàn)證礦山環(huán)境三維激光點(diǎn)云SLAM算法建圖定位的有效性,采用了集成Velodyne VLP-16 LiDAR、CH110-usb IMU、北斗星通C201 GNSS系統(tǒng)和NVIDAI AGX工控機(jī)的巡檢機(jī)器人,如圖5所示。

圖5 試驗(yàn)設(shè)備

利用巡檢機(jī)器人,本文選擇采集3組場(chǎng)景(街區(qū)道路、礦山斜坡、采礦區(qū))進(jìn)行了數(shù)據(jù)獲取,礦山數(shù)據(jù)采集地點(diǎn)為哈爾烏素露天煤礦。數(shù)據(jù)采集前,為實(shí)現(xiàn)激光雷達(dá)與IMU的時(shí)間同步,將雷達(dá)數(shù)據(jù)點(diǎn)云格式由XYZI更改為XYZIRT,并用本文方法對(duì)各傳感器參數(shù)進(jìn)行標(biāo)定。在試驗(yàn)區(qū)中,街區(qū)道路旁有大量樹木與房屋,并且有移動(dòng)車輛和行人的干擾。礦山斜坡具有輕微的坡度,地形較為平坦。采礦區(qū)地形復(fù)雜多變,有嚴(yán)重的拐角和地面顛簸。3組場(chǎng)景的數(shù)據(jù)集長度均大于1 km,采集時(shí)間均在500 s以上。本文采用軌跡評(píng)估工具EVO繪制誤差曲線,各組試驗(yàn)軌跡誤差對(duì)比的真值均為GNSS信息。

4.2 街區(qū)道路

街區(qū)道路的建圖效果與軌跡對(duì)比如圖6所示,圖6 (c)為LIO-SAM算法與GNSS信息的軌跡對(duì)比曲線。

圖6 街區(qū)道路試驗(yàn)

由圖6可以看出街區(qū)道路試驗(yàn)在拐角處軌跡漂移的現(xiàn)象比較嚴(yán)重,利用軌跡評(píng)估工具EVO,得到街區(qū)道路下多種SLAM算法的定位誤差,見表1。

表1 街區(qū)道路SLAM軌跡APE統(tǒng)計(jì)結(jié)果 m

由表1可知,ALOAM的RMSE為6.267 415 m,采用Eigen庫和Ceres優(yōu)化庫直接計(jì)算旋轉(zhuǎn)矩陣與雅可比矩陣的方式,能夠在街區(qū)道路取得更優(yōu)的定位結(jié)果。由于受樹木、行人車輛的影響,LOAM單激光與LeGO-LOAM地面優(yōu)化的方式均沒有多傳感器融合算法LIO-SAM定位精度高,而在LIO-SAM特征提取部分增加強(qiáng)度特征的IALIO算法,可以提取更多的特征點(diǎn),定位誤差較LIO-SAM降低了20.17%。

4.3 礦山斜坡

礦山斜坡的建圖效果與軌跡對(duì)比如圖7所示,圖7 (c)為LIO-SAM算法與GNSS信息的軌跡對(duì)比。

圖7 礦山斜坡試驗(yàn)

可以看出,在礦山斜坡環(huán)境拐彎時(shí)會(huì)因運(yùn)動(dòng)畸變產(chǎn)生輕微的失真漂移現(xiàn)象,受建筑物的影響較小,地形較為平坦,建圖效果平滑。利用EVO工具對(duì)礦山斜坡環(huán)境下多種SLAM算法的定位誤差結(jié)果見表2。

表2 礦山斜坡SLAM軌跡APE統(tǒng)計(jì)結(jié)果 m

由表2可知,在礦山斜坡地形環(huán)境,LIO-SAM的RMSE為0.811 041 m,相較于ALOAM和LeGO-LOAM分別提高了2.70%和45.28%。因此在礦山斜坡地形環(huán)境下,多傳感器融合算法LIO-SAM可以有效地解決表面傾斜所造成的運(yùn)動(dòng)畸變問題,使建圖效果更平滑清晰。

4.4 采礦區(qū)

采礦區(qū)的建圖效果與軌跡對(duì)比如圖8所示,圖8(c)為LIO-SAM算法與GNSS信息的軌跡對(duì)比曲線。

圖8 采礦區(qū)試驗(yàn)

由圖8可以看出,采礦區(qū)環(huán)境巡檢機(jī)器人在朝一個(gè)方向行進(jìn)時(shí)誤差累積較大,且在拐角處軌跡偏移量較大,軌跡誤差見表3。

表3 采礦區(qū)SLAM軌跡APE統(tǒng)計(jì)結(jié)果 m

由表3可知,在采礦區(qū)地形顛簸及周圍環(huán)境特征不明顯的問題下,多傳感器融合算法LIO-SAM更能適應(yīng)復(fù)雜環(huán)境,具有更好的定位結(jié)果,它的RMSE為22.055 274 m,相較于ALOAM和SC-LeGO誤差分別降低了21.53%和55.18%。LIO-SAM以局部地圖優(yōu)化的方式可以有效減少誤差的累積,而對(duì)LIO-SAM特征提取部分添加強(qiáng)度特征的算法IALIO在采礦區(qū)有更好的定位精度,它的RMSE為21.855 380 m,相較于LIO-SAM誤差降低了0.91%。

5 結(jié) 語

本文針對(duì)礦山地形未知環(huán)境中建圖定位需求, 首先配置了激光雷達(dá)、IMU、GPS等多傳感融合的巡檢機(jī)器人,并對(duì)激光雷達(dá)和IMU進(jìn)行內(nèi)外參數(shù)標(biāo)定,解決了建圖漂移失真問題?？紤]礦山環(huán)境的復(fù)雜性,巡檢機(jī)器人分別采集 3組場(chǎng)景(街區(qū)道路、礦山斜坡、采礦區(qū))的數(shù)據(jù)集,以GNSS信息作為軌跡真值,對(duì)比了多種三維激光雷達(dá)SLAM算法的建圖效果并分析了軌跡誤差與定位精度。試驗(yàn)表明,在采礦區(qū)復(fù)雜環(huán)境,LIO-SAM通過多傳感器融合,不僅以局部地圖優(yōu)化的方式減少了累積誤差,更在后端優(yōu)化部分添加了回環(huán)因子,具有更優(yōu)的定位精度。針對(duì)LIO-SAM特征提取部分計(jì)算曲率提取特征點(diǎn)的不足之處,引入了強(qiáng)度特征,驗(yàn)證了IALIO算法定位精度在采礦區(qū)更優(yōu)于LIO-SAM。

本文驗(yàn)證了礦山環(huán)境下三維激光雷達(dá)SLAM算法的可行性,在解決礦山環(huán)境地形多變?cè)斐傻睦鄯e誤差問題上,LOAM和LeGO-LOAM算法的穩(wěn)健性和定位精度都較差,使用多傳感器融合算法LIO-SAM才能得到精確的定位精度。LIO-SAM的特征提取部分還有所欠缺,改變特征點(diǎn)的提取方式對(duì)提高定位精度具有可行之處。