張清宇,崔麗珍,杜秀鐸,馬寶良
(內蒙古科技大學信息工程學院,內蒙古 包頭 014010)
近年來,我國煤礦智能化發(fā)展迅速,礦山環(huán)境下的精確定位導航技術研發(fā)更是重中之重。由于煤炭工業(yè)的工作環(huán)境相對頑劣,安全問題一直制約著煤礦的生產(chǎn)建設。同步定位與地圖構建是無人駕駛領域解決惡劣環(huán)境建圖定位問題的關鍵技術,三維激光雷達SLAM算法正被廣泛應用于煤礦環(huán)境的高精度制圖工作。
針對煤礦復雜環(huán)境,文獻[1]設計了一種煤礦巡檢機器人,以滿足實際礦山探測工作的需求。文獻[2]利用地面激光雷達計算復雜地形土方量。文獻[3]對煤礦井下巷道的難定位問題提出了一種改進的巷道環(huán)境建模算法。文獻[4]針對煤礦環(huán)境地圖構建過程中出現(xiàn)的點云運動畸變提出了點云雙邊濾波算法。文獻[5]研究了煤礦井下無GPS的機器人自主定位問題,實現(xiàn)了三維激光雷達SLAM算法的仿真試驗。
對于三維激光雷達SLAM算法的應用與發(fā)展,文獻[6]提出的LOAM改變了特征提取的方式,通過點云匹配對雷達位姿進行優(yōu)化。文獻[7]提出了迭代最近點ICP算法。文獻[8]提出了通過慣性測量單元(inertial measurement unit,IMU)估計運動狀態(tài)消除激光雷達運動畸變的方法。文獻[9]提出的BALM算法,在雷達建圖過程中引入光速法平差以降低建圖過程中的累積誤差。文獻[10]提出了激光-慣性里程計相結合的方法LIO。文獻[11]提出的LOL方法,將 LOAM和分割場景識別算法相結合。文獻[12]提出的LIRO方法,將UWB、雷達和慣導緊耦合。文獻[13—14]提出了LeGO-LOAM和LIO-SAM算法,前者在算法的后端添加地面優(yōu)化且特征提取方式更輕量級,提高了特征點的提取效率;后者將LiDAR里程計因子、IMU預積分因子、GPS因子和回環(huán)因子通過因子圖優(yōu)化的方式融合,實現(xiàn)多傳感器融合定位。針對激光雷達耗時長、受動態(tài)障礙物影響大的問題,文獻[15]在閉環(huán)檢測部分添加了SC-Context。文獻[16]提出了FAST-LIO算法,緊耦合迭代卡爾曼濾波和雷達慣性里程計,提高了穩(wěn)健性。
本文為了解決礦山復雜環(huán)境下建圖定位的難點,首先對各傳感器進行內外參數(shù)標定,然后通過巡檢機器人采集到的數(shù)據(jù),對比多種三維激光雷達SLAM算法,以期驗證在礦山環(huán)境中多傳感器融合算法LIO-SAM具有更好的建圖效果與定位精度,并構建在LIO-SAM特征提取部分利用點的強度和環(huán)境值消除不必要的特征點的IALIO算法,以期在定位精度上有進一步的提高。
單一的激光雷達、里程計、慣性測量單元很難單獨滿足高效、實時、準確、穩(wěn)健等性能,因此多傳感器融合的方式應用于復雜環(huán)境下的建圖定位是目前SLAM算法主要的發(fā)展趨勢。
對于LiDAR融合IMU的系統(tǒng),為解決機器人運動產(chǎn)生的激光雷達運動畸變,需要標定LiDAR與IMU的外參。本文采用LiDAR_align的方法進行激光雷達與IMU的外參標定,利用NLopt庫處理定義沖突問題并改寫里程計接口為IMU接口,錄制包含大量旋轉量和平移量的LiDAR數(shù)據(jù)及IMU數(shù)據(jù),多次標定迭代后取收斂結果最小的參數(shù)。
IMU內參未標定時會造成建圖的漂移現(xiàn)象甚至無法建圖,嚴重影響定位精度。本文采用的IMU的觀測模型中存在兩種誤差:一種是變化緩慢的零偏b;另一類是變化比較劇烈的測量白噪聲n。其中,零偏是由傳感器的溫度等物理因素產(chǎn)生的傳感器內部誤差的綜合參數(shù);測量白噪聲是由于模/數(shù)轉換器引起的外部噪聲,角速度測量值ωo和真實值ω之間的關系為
ωo(t)=ω(t)+b(t)+n(t)
(1)
本文通過內參標定工具imu_utils進行IMU內參的標定,即標定隨機游走噪聲和白噪聲。通過ceres優(yōu)化庫分析IMU的Allan方差,將IMU保持靜止錄制2 h的數(shù)據(jù)包,確定最終的高斯白噪聲和隨機游走噪聲。
不同SLAM算法的特征提取方式是有差別的,特征點的獲取決定了機器人相對位姿數(shù)量的多少,直接影響建圖效果與定位精度。
LOAM通過計算當前幀點云pk中每個點i的平滑度c,將c最小的點作為平面點,c最大的點作為邊緣點,將一幀掃描到的點云分為4段,每段只提取最多4個平面點和2個邊緣點,以保證特征點的分布均勻,其中平滑度c的計算公式為
(2)
LeGO-LOAM是在LOAM基礎上改進的激光雷達建圖方法,與LOAM相比,在輕量級和地面優(yōu)化部分有了顯著提升,可以在平坦路面有效處理點線特征和點面特征,但是由于提取的線面特征較多,會使得累計誤差較大。LeGO-LOAM的主要特點包括:①對地面進行分割,減小了特征搜索范圍;②提取特征之前進行聚類分析,提高了特征質量;③以幀為單位進行優(yōu)化,使得全局地圖可以多次調整;④增加了回環(huán)修正。LeGO-LOAM的地面優(yōu)化主要是在LiDAR Odometry的特征提取部分,通過使用采集到的地面面特征優(yōu)化高度和水平角、采集到的線特征優(yōu)化水平位移和航向角,構建點與線之間、面與面之間的對應關系,具體框架如圖1所示。
圖1 LiDAR Odometry框架
本身促進兩者聯(lián)動的活動較少。社工機構是讓社會工作和志愿服務的聯(lián)動的主要組織一,在這個載體上可以開展很多活動。但目前,我國社會工作和志愿服務的聯(lián)動模式和機制都處于探索過程中,還沒有完全探索出本土化的聯(lián)動模式和機制,因此,除了政府購買的社會服務合作項目之外,社工機構很少有能力有主動性去開展合作項目和活動,還是自上而下的項目比較多,有社工機構主導的自下而上的合作項目相對較少,還處于初始階段。
本文提出LIO-SAM在特征提取部分僅計算每個點的曲率,通過曲率的大小提取角點和平面點,規(guī)定當前激光點為pi,利用pi前后的5個點計算當前的曲率K。前后5個點與pi的距離差值為M,曲率K為距離差值的平方,具體計算公式為
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ū)分“平面”與“近似平面”,將沒有計算分類的點全部加入“近似平面”中,會丟失一部分符合曲率要求的特征點。本文采用的IALIO算法增加了曲率的閾值,利用點的強度和環(huán)境刪除不必要的點,一定程度上彌補了丟失點的問題,在定位精度上也有一定提高。
LIO-SAM是通過激光雷達、IMU和GPS多種傳感器融合,利用因子圖優(yōu)化的方式在因子圖的頂端設置了激光雷達慣性里程計,收集不同來源的多種相對測量和絕對測量值作為因子,全部合并到系統(tǒng)中。將激光雷達里程計因子、IMU預積分因子、GPS因子和回環(huán)因子全部優(yōu)化,在礦山復雜環(huán)境下能夠針對性地應用。
IMU預積分因子部分的具體流程如圖2所示。
圖2 IMU預積分因子
其中,Xi表示每個關鍵幀,線條表示IMU預積分因子,它由兩個相鄰關鍵幀之間的IMU測量積分得到。IMU的角速度和加速度的測量值公式為
(5)
(6)
(7)
(8)
(9)
(10)
(11)
(12)
式中,ΔVij、ΔPij和ΔRij分別為時間i至j之間的相對速度、位置和旋轉矩陣;Vi和Vj分別為i、j時刻的速度;Pi和Pj分別為i、j時刻的位置;Ri和Rj分別為i、j時刻的旋轉矩陣;g為重力加速度;Δtij為i、j時刻的時間間隔。
圖3 激光里程計因子
構建得到的局部地圖Mi公式為
(13)
在后端優(yōu)化部分,LIO-SAM引入了回環(huán)檢測,添加了回環(huán)因子,回環(huán)檢測因子如圖4所示。
圖4 回環(huán)檢測因子
當因子圖中加入一個新的狀態(tài)Xi+1時,首先搜索整個因子圖,找到與之相近的先驗狀態(tài),X3是返回的候選之一,然后嘗試匹配Fi+1到子關鍵幀{F3-m,…F3,…,F3+m}。本文對LeGO-LOAM算法的回環(huán)檢測部分添加了Scan-Context描述子,定位精度更優(yōu)。
為驗證礦山環(huán)境三維激光點云SLAM算法建圖定位的有效性,采用了集成Velodyne VLP-16 LiDAR、CH110-usb IMU、北斗星通C201 GNSS系統(tǒng)和NVIDAI AGX工控機的巡檢機器人,如圖5所示。
圖5 試驗設備
利用巡檢機器人,本文選擇采集3組場景(街區(qū)道路、礦山斜坡、采礦區(qū))進行了數(shù)據(jù)獲取,礦山數(shù)據(jù)采集地點為哈爾烏素露天煤礦。數(shù)據(jù)采集前,為實現(xiàn)激光雷達與IMU的時間同步,將雷達數(shù)據(jù)點云格式由XYZI更改為XYZIRT,并用本文方法對各傳感器參數(shù)進行標定。在試驗區(qū)中,街區(qū)道路旁有大量樹木與房屋,并且有移動車輛和行人的干擾。礦山斜坡具有輕微的坡度,地形較為平坦。采礦區(qū)地形復雜多變,有嚴重的拐角和地面顛簸。3組場景的數(shù)據(jù)集長度均大于1 km,采集時間均在500 s以上。本文采用軌跡評估工具EVO繪制誤差曲線,各組試驗軌跡誤差對比的真值均為GNSS信息。
街區(qū)道路的建圖效果與軌跡對比如圖6所示,圖6 (c)為LIO-SAM算法與GNSS信息的軌跡對比曲線。
圖6 街區(qū)道路試驗
由圖6可以看出街區(qū)道路試驗在拐角處軌跡漂移的現(xiàn)象比較嚴重,利用軌跡評估工具EVO,得到街區(qū)道路下多種SLAM算法的定位誤差,見表1。
表1 街區(qū)道路SLAM軌跡APE統(tǒng)計結果 m
由表1可知,ALOAM的RMSE為6.267 415 m,采用Eigen庫和Ceres優(yōu)化庫直接計算旋轉矩陣與雅可比矩陣的方式,能夠在街區(qū)道路取得更優(yōu)的定位結果。由于受樹木、行人車輛的影響,LOAM單激光與LeGO-LOAM地面優(yōu)化的方式均沒有多傳感器融合算法LIO-SAM定位精度高,而在LIO-SAM特征提取部分增加強度特征的IALIO算法,可以提取更多的特征點,定位誤差較LIO-SAM降低了20.17%。
礦山斜坡的建圖效果與軌跡對比如圖7所示,圖7 (c)為LIO-SAM算法與GNSS信息的軌跡對比。
圖7 礦山斜坡試驗
可以看出,在礦山斜坡環(huán)境拐彎時會因運動畸變產(chǎn)生輕微的失真漂移現(xiàn)象,受建筑物的影響較小,地形較為平坦,建圖效果平滑。利用EVO工具對礦山斜坡環(huán)境下多種SLAM算法的定位誤差結果見表2。
表2 礦山斜坡SLAM軌跡APE統(tǒng)計結果 m
由表2可知,在礦山斜坡地形環(huán)境,LIO-SAM的RMSE為0.811 041 m,相較于ALOAM和LeGO-LOAM分別提高了2.70%和45.28%。因此在礦山斜坡地形環(huán)境下,多傳感器融合算法LIO-SAM可以有效地解決表面傾斜所造成的運動畸變問題,使建圖效果更平滑清晰。
采礦區(qū)的建圖效果與軌跡對比如圖8所示,圖8(c)為LIO-SAM算法與GNSS信息的軌跡對比曲線。
圖8 采礦區(qū)試驗
由圖8可以看出,采礦區(qū)環(huán)境巡檢機器人在朝一個方向行進時誤差累積較大,且在拐角處軌跡偏移量較大,軌跡誤差見表3。
表3 采礦區(qū)SLAM軌跡APE統(tǒng)計結果 m
由表3可知,在采礦區(qū)地形顛簸及周圍環(huán)境特征不明顯的問題下,多傳感器融合算法LIO-SAM更能適應復雜環(huán)境,具有更好的定位結果,它的RMSE為22.055 274 m,相較于ALOAM和SC-LeGO誤差分別降低了21.53%和55.18%。LIO-SAM以局部地圖優(yōu)化的方式可以有效減少誤差的累積,而對LIO-SAM特征提取部分添加強度特征的算法IALIO在采礦區(qū)有更好的定位精度,它的RMSE為21.855 380 m,相較于LIO-SAM誤差降低了0.91%。
本文針對礦山地形未知環(huán)境中建圖定位需求, 首先配置了激光雷達、IMU、GPS等多傳感融合的巡檢機器人,并對激光雷達和IMU進行內外參數(shù)標定,解決了建圖漂移失真問題??紤]礦山環(huán)境的復雜性,巡檢機器人分別采集 3組場景(街區(qū)道路、礦山斜坡、采礦區(qū))的數(shù)據(jù)集,以GNSS信息作為軌跡真值,對比了多種三維激光雷達SLAM算法的建圖效果并分析了軌跡誤差與定位精度。試驗表明,在采礦區(qū)復雜環(huán)境,LIO-SAM通過多傳感器融合,不僅以局部地圖優(yōu)化的方式減少了累積誤差,更在后端優(yōu)化部分添加了回環(huán)因子,具有更優(yōu)的定位精度。針對LIO-SAM特征提取部分計算曲率提取特征點的不足之處,引入了強度特征,驗證了IALIO算法定位精度在采礦區(qū)更優(yōu)于LIO-SAM。
本文驗證了礦山環(huán)境下三維激光雷達SLAM算法的可行性,在解決礦山環(huán)境地形多變造成的累積誤差問題上,LOAM和LeGO-LOAM算法的穩(wěn)健性和定位精度都較差,使用多傳感器融合算法LIO-SAM才能得到精確的定位精度。LIO-SAM的特征提取部分還有所欠缺,改變特征點的提取方式對提高定位精度具有可行之處。