李猛鋼，胡而已，朱華

（1.中國(guó)礦業(yè)大學(xué) 機(jī)電工程學(xué)院，江蘇 徐州 221116；2.江蘇省礦山智能采掘裝備協(xié)同創(chuàng)新中心，江蘇 徐州 221008；3.中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所 機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，遼寧 沈陽(yáng) 110016；4.應(yīng)急管理部 信息研究院，北京 100029）

0 引言

煤炭是我國(guó)的主要能源和重要戰(zhàn)略資源。目前我國(guó)煤礦智能化水平較低，井下作業(yè)人員仍然較多、安全風(fēng)險(xiǎn)大，加快煤炭企業(yè)對(duì)煤礦機(jī)器人的研發(fā)和應(yīng)用是破解當(dāng)前安全生產(chǎn)矛盾、實(shí)現(xiàn)減人增效的重要途徑和現(xiàn)實(shí)需求。對(duì)于各類煤礦移動(dòng)機(jī)器人，精確定位與地圖構(gòu)建是其推廣應(yīng)用亟待實(shí)現(xiàn)的共性關(guān)鍵技術(shù)[1]。同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）具有在定位的同時(shí)實(shí)時(shí)重構(gòu)場(chǎng)景模型的功能，為輔助駕駛、自主導(dǎo)航等提供定位建圖功能，近10 a來(lái)得到了長(zhǎng)足的發(fā)展并已逐漸走向工業(yè)應(yīng)用領(lǐng)域。

在煤礦井下SLAM方法方面，國(guó)外較早開(kāi)展了相關(guān)研究，但主要采用手持設(shè)備[2]、移動(dòng)小車[3]、探測(cè)機(jī)器人[4]、礦車[5]搭載激光雷達(dá)及慣性測(cè)量單元（Inertial Measurement Unit，IMU）等傳感器進(jìn)行環(huán)境建模。D.Tardioli等[6]分析了其所在課題組多年來(lái)在地下隧道機(jī)器人領(lǐng)域開(kāi)展的相關(guān)研究進(jìn)展，主要探討了基于激光雷達(dá)、視覺(jué)相機(jī)、里程計(jì)等傳感器的SLAM方法在隧道環(huán)境的應(yīng)用。美國(guó)國(guó)防部高級(jí)研究計(jì)劃局（Defense Advanced Research Projects Agency，DARPA）于2018?2021年舉辦的SubT賽事面向城市地下空間、隧道和天然洞穴環(huán)境下的機(jī)器人應(yīng)用，相關(guān)研究針對(duì)光照變化[7]、場(chǎng)景退化[8]及巷道特征利用[9]等地下場(chǎng)景SLAM中的典型問(wèn)題提出了新的解決方案。國(guó)內(nèi)方面，馬宏偉等[10]提出了基于深度相機(jī)進(jìn)行井下定位建圖實(shí)現(xiàn)移動(dòng)機(jī)器人自主導(dǎo)航的方案。陳先中等[11]提出了一種煤礦地下毫米波雷達(dá)點(diǎn)云成像、結(jié)合深度學(xué)習(xí)處理稀疏點(diǎn)云的SLAM方法構(gòu)想。楊健健等[12]研究了基于Hector?SLAM的掘進(jìn)機(jī)器人巷道環(huán)境感知方法。Li Menggang等[13]提出了一種基于激光雷達(dá)的NDT（正態(tài)分布變換）?Graph SLAM方法，利用平面約束減輕地圖漂移，用于煤礦救援機(jī)器人的高效建圖任務(wù)。高士崗等[14]在國(guó)家能源神東煤炭榆家梁煤礦初步應(yīng)用了搭載Exscan激光掃描儀的巡檢機(jī)器人，基于SLAM技術(shù)構(gòu)建了動(dòng)態(tài)數(shù)字化工作面，三維點(diǎn)云模型實(shí)測(cè)精度為0.2 m。總體來(lái)看，針對(duì)煤礦機(jī)器人SLAM的研究集中于解決激光雷達(dá)、相機(jī)、毫米波雷達(dá)等傳感器在特定場(chǎng)景、特定條件下的初步應(yīng)用和性能改善問(wèn)題，對(duì)井下多種條件耦合的復(fù)雜環(huán)境長(zhǎng)期定位建模技術(shù)仍有待進(jìn)一步攻關(guān)。

在三維激光SLAM研究方面，以迭代最近點(diǎn)（Iterative Closest Point，ICP）為代表的掃描匹配方法研究較早[15]。6D SLAM[16]的前端利用大量原始點(diǎn)云進(jìn)行ICP配準(zhǔn)，后端使用全局松弛化，實(shí)現(xiàn)了非實(shí)時(shí)的高精地圖構(gòu)建。與使用全部點(diǎn)云不同，為了減少計(jì)算量、提高算法精度，另一類基于特征的方法被提出。Zhang Ji等[17]提出了一種激光里程計(jì)與建圖（LiDAR Odometry and Mapping,LOAM）方法，通過(guò)計(jì)算曲率分割邊點(diǎn)和面點(diǎn)，構(gòu)建點(diǎn)?線和點(diǎn)?面優(yōu)化問(wèn)題，是目前諸多激光里程計(jì)采用的方法，但由于缺乏回環(huán)檢測(cè)，LOAM方法在長(zhǎng)距離應(yīng)用中仍然有較大的漂移。Shan Tixiao等[18]基于LOAM方法提出LeGO?LOAM（Light Weight and Ground?optimized LOAM）方法，采用了與LOAM方法同樣的特征提取方法，通過(guò)深度圖像分割剔除地面的干擾。J.Weingartent等[19]和A.J.B.Trevor等[20]均提出構(gòu)建平面特征進(jìn)行數(shù)據(jù)關(guān)聯(lián)，后端分別基于擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）和圖優(yōu)化構(gòu)建SLAM。此類方法必須要求環(huán)境中有大量可提取的平面，形成稀疏平面特征地圖。

在激光?慣性融合定位建圖方面，目前主要分為松耦合和緊耦合2種方式。松耦合通過(guò)構(gòu)建慣性導(dǎo)航獨(dú)立進(jìn)行狀態(tài)傳播，利用激光進(jìn)行觀測(cè)更新。緊耦合方法近年來(lái)逐漸得到關(guān)注。Ye Haoyang等[21]提出了一種雷達(dá)慣性里程計(jì)與建圖（Lidar Inertial Odometry and Mapping，LIO?mapping）方法，使用面特征構(gòu)建相鄰幀之間的點(diǎn)?面約束，同時(shí)構(gòu)建IMU預(yù)積分約束，基于因子圖聯(lián)合優(yōu)化所有幀間觀測(cè)，在特征較多的大場(chǎng)景條件下難以滿足實(shí)時(shí)運(yùn)行需求。Shan Tixiao等[22]提出了一種基于平滑建圖的雷達(dá)慣性里程計(jì)（Lidar Inertial Odometry via Smoothing and Mapping，LIO?SAM）方法，前端同樣采用LOAM進(jìn)行特征提取，后端利用因子圖框架融合IMU，取得了較好的效果。除了優(yōu)化方案，濾波方法也被用于激光慣性傳感器融合中。Qin Chao等[23]提出的用于魯棒和高效導(dǎo)航的雷達(dá)慣性狀態(tài)估計(jì)器（Lidar?Inertial State Estimator for Robust and Efficient Navigation，LINS）和Xu Wei等[24]提出的快速雷達(dá)慣性里程計(jì)（Fast Lidar Inertial Odometry,Fast?LIO）系列方法，基于迭代擴(kuò)展卡爾曼濾波（Iterative Extended Kalman Filter，IEKF）實(shí)現(xiàn)激光和IMU緊耦合，在觀測(cè)更新時(shí)采用了類似優(yōu)化方法的迭代過(guò)程，但沒(méi)有使用歷史數(shù)據(jù)，在復(fù)雜場(chǎng)景中的精度和魯棒性較差。楊林等[25]提出了激光慣性融合SLAM方法，前端利用IEKF設(shè)計(jì)了激光慣性里程計(jì)，通過(guò)構(gòu)建關(guān)鍵幀在后端采用位姿圖優(yōu)化進(jìn)行聯(lián)合優(yōu)化，降低了系統(tǒng)累計(jì)誤差。

煤礦井下機(jī)器人SLAM是當(dāng)前研究熱點(diǎn)，但針對(duì)提高激光SLAM在井下復(fù)雜條件下的精度、魯棒性的研究仍然不足；傳統(tǒng)激光SLAM方法在此類環(huán)境下存在累計(jì)誤差迅速增大、旋轉(zhuǎn)過(guò)程魯棒性差、特征關(guān)聯(lián)錯(cuò)誤率高等問(wèn)題；現(xiàn)有激光?慣性融合的定位建圖緊耦合融合機(jī)制仍需進(jìn)一步探討，以期進(jìn)一步提高激光?慣性融合SLAM方法對(duì)煤礦井下復(fù)雜環(huán)境的適應(yīng)能力。為此，本文提出了一種煤礦機(jī)器人LiDAR/IMU緊耦合SLAM方法（Lidar?Inertial SLAM，LI?SLAM方法）。基于點(diǎn)?線和點(diǎn)?面掃描匹配構(gòu)建激光相對(duì)位姿約束，融合IMU預(yù)積分因子約束和回環(huán)因子約束，利用因子圖優(yōu)化實(shí)現(xiàn)了LiDAR/IMU緊耦合的SLAM。構(gòu)建了雷達(dá)相對(duì)位姿因子約束模型，解析推導(dǎo)了雷達(dá)相對(duì)位姿約束的殘差、雅可比和協(xié)方差矩陣；基于因子圖模型，構(gòu)建了激光相對(duì)位姿約束與IMU預(yù)積分約束的緊耦合數(shù)據(jù)融合機(jī)制，提升了對(duì)煤礦井下顛簸路面、機(jī)器人劇烈旋轉(zhuǎn)等復(fù)雜工況的適應(yīng)能力。該方法可幫助煤礦移動(dòng)機(jī)器人在井下復(fù)雜地形與環(huán)境工況中實(shí)現(xiàn)魯棒精確的定位建圖，為煤礦移動(dòng)機(jī)器人自主導(dǎo)航提供關(guān)鍵技術(shù)支撐。

1 LI?SLAM框架

LI?SLAM框架如圖1所示。激光約束構(gòu)建階段包括5個(gè)步驟：①雷達(dá)數(shù)據(jù)預(yù)處理：利用IMU的短時(shí)狀態(tài)估計(jì)傳播運(yùn)動(dòng)狀態(tài)，校正時(shí)段內(nèi)采集的雷達(dá)線掃運(yùn)動(dòng)畸變，進(jìn)行區(qū)域?yàn)V波等預(yù)處理過(guò)程。②特征提?。禾崛↑c(diǎn)云中的邊和面特征。③關(guān)鍵幀與子圖構(gòu)建：提取關(guān)鍵幀并構(gòu)建局部子圖。④ 特征關(guān)聯(lián)：利用特征關(guān)聯(lián)和掃描匹配計(jì)算相對(duì)位姿變換。⑤雷達(dá)相對(duì)位姿因子構(gòu)建：基于幀到子圖的掃描匹配建立局部雷達(dá)相對(duì)位姿因子。完成激光約束構(gòu)建后，執(zhí)行激光?慣性緊耦合融合機(jī)制：與同時(shí)段內(nèi)慣性預(yù)積分因子在滑動(dòng)窗口中進(jìn)行聯(lián)合優(yōu)化，實(shí)現(xiàn)狀態(tài)估計(jì)，利用雷達(dá)相對(duì)位姿因子限制慣導(dǎo)零偏等參數(shù)的漂移，實(shí)現(xiàn)緊耦合的聯(lián)合狀態(tài)估計(jì)，同時(shí)利用子圖關(guān)鍵幀構(gòu)建全局地圖。迭代執(zhí)行上述過(guò)程，可實(shí)現(xiàn)在線激光?慣性緊耦合SLAM。

圖1 LI?SLAM方法框架Fig.1 Framework of the LI-SLAM method

定義慣導(dǎo)坐標(biāo)系B，雷達(dá)坐標(biāo)系L，激光慣性里程計(jì)坐標(biāo)系O。LI?SLAM的因子圖模型如圖2所示。待估計(jì)的變量xOBi是優(yōu)化窗口內(nèi)所有的IMU相對(duì)于局部坐標(biāo)系下的位置p、速度v、姿態(tài)q、加速度零偏ba（a為加速度）、角速度零偏bω（ω為角速度）及從慣導(dǎo)到雷達(dá)坐標(biāo)系間的外參TBL。從i，i+1，…，j時(shí)刻所有的待估計(jì)變量xOBi，xOBi+1，…， xOBj組 成的集合為χ：

圖2 因子圖模型Fig.2 Factor graph model

優(yōu)化窗中第i時(shí)刻對(duì)應(yīng)的狀態(tài)變量和外參分別表示為

采用子圖進(jìn)行掃描匹配過(guò)程。在局部?jī)?yōu)化過(guò)程中，僅滑動(dòng)窗口以內(nèi)的各類因子參與優(yōu)化過(guò)程，窗口外的狀態(tài)利用邊緣化技術(shù)固定?；谝蜃訄D模型的優(yōu)化目標(biāo)函數(shù)為

2 激光約束構(gòu)建

2.1 雷達(dá)數(shù)據(jù)預(yù)處理

點(diǎn)云畸變?cè)跈C(jī)械式激光雷達(dá)高速運(yùn)動(dòng)問(wèn)題中是必須解決的問(wèn)題。現(xiàn)有手段通?；趧蛩龠\(yùn)動(dòng)假設(shè)，利用線性插值重新部署激光點(diǎn)云的位置。這種方法對(duì)于劇烈的非線性運(yùn)動(dòng)適應(yīng)性差。本文使用高頻IMU進(jìn)行狀態(tài)傳播，以恢復(fù)每個(gè)點(diǎn)的運(yùn)動(dòng)。設(shè)定第i時(shí)刻和第j時(shí)刻對(duì)應(yīng)的雷達(dá)觀測(cè)幀為Si和Sj。對(duì)第Si幀與第Sj幀之間的IMU觀測(cè)進(jìn)行積分，作為激光掃描匹配的初值。相鄰采樣時(shí)刻間認(rèn)為加速度和角速度未發(fā)生變化，加速度、角速度零偏是固定值。IMU加速度觀測(cè)值為，角速度為從t（t=i，i+1，…， j）時(shí)刻傳播的狀態(tài)包括位置、速度方向。

式中：vWBt為t時(shí)刻世界坐標(biāo)系下的速度；gO為里程計(jì)坐標(biāo)系下的重力加速度；和為t時(shí)刻相對(duì)于世界坐標(biāo)系和里程計(jì)坐標(biāo)系的旋轉(zhuǎn)矩陣。

2.2 特征提取

采用LOAM[17]的特征提取方法設(shè)計(jì)點(diǎn)云粗糙度指標(biāo)作為線特征和面特征的區(qū)分方式。設(shè)定某個(gè)雷達(dá)掃描周期K內(nèi)點(diǎn)云為PK，點(diǎn)云PK中2個(gè)點(diǎn)(PA,PU)∈PK，在雷達(dá)坐標(biāo)系下的位置坐標(biāo)可以表示為()。s為點(diǎn)云PK中點(diǎn)PA所在的行中的點(diǎn)組成的連續(xù)點(diǎn)集，則點(diǎn)PA所在局部曲面的粗糙度為

式中A，U為點(diǎn)云中的2個(gè)點(diǎn)對(duì)應(yīng)的序號(hào)。

根據(jù)以下規(guī)則提取特征：①邊緣點(diǎn)：取粗糙度C最大的某些個(gè)點(diǎn)排序即為曲率較大的邊緣點(diǎn)。② 平面點(diǎn)：取粗糙度C最大的某些個(gè)點(diǎn)排序即為曲率較小的平面點(diǎn)。③為了使點(diǎn)云特征在雷達(dá)坐標(biāo)系下的各個(gè)區(qū)域分布盡可能均勻，將每個(gè)掃描幀等分為若干區(qū)域，每個(gè)區(qū)域提取2個(gè)邊點(diǎn)和4個(gè)平面點(diǎn)。

2.3 關(guān)鍵幀與子圖構(gòu)建

為處理高頻激光觀測(cè)數(shù)據(jù)，采用關(guān)鍵幀進(jìn)行特征匹配和雷達(dá)約束的構(gòu)建。預(yù)設(shè)最小平移距離閾值、最小旋轉(zhuǎn)角度閾值，若達(dá)到其中一個(gè)條件，則設(shè)置當(dāng)前幀為關(guān)鍵幀。關(guān)鍵幀位姿變化計(jì)算公式可以參考文獻(xiàn)[13]。

在前端匹配中，采用幀到子圖的方式進(jìn)行掃描匹配，組合固定數(shù)量的關(guān)鍵幀形成子圖。設(shè)關(guān)鍵幀k的邊和面特征集合為Fek和Fhk（e，h分別表示邊和面，下同），n個(gè)關(guān)鍵幀組成的特征集合為{Fk?n,Fk?n+1···,Fk},Fk=Fek∪Fhk。利用各關(guān)鍵幀的變換可以將特征轉(zhuǎn)換到局部里程計(jì)坐標(biāo)系O下。轉(zhuǎn)換后，拼接關(guān)鍵幀，組成局部子圖Mk，其中包含變換到里程計(jì)坐標(biāo)系后的邊和面兩類特征， Mk={Mke∪Mkh}。采用下采樣方法剔除同一個(gè)體素柵格中的重復(fù)特征，避免冗余的特征匹配。Mke和Mkh的下采樣精度根據(jù)環(huán)境特點(diǎn)、計(jì)算量按照經(jīng)驗(yàn)設(shè)置，此處設(shè)為0.1 m和0.2 m，子圖內(nèi)關(guān)鍵幀數(shù)量為n=20。

2.4 幀到子圖的特征關(guān)聯(lián)與掃描匹配

圖3 2類特征點(diǎn)數(shù)據(jù)關(guān)聯(lián)方法Fig.3 Data association method for two kinds of features

至此，構(gòu)建點(diǎn)到線的距離de：

構(gòu)建點(diǎn)到面的距離dh：

式中：Xkh+1,I為關(guān)鍵幀k+1對(duì)應(yīng)的面特征I的坐標(biāo)；為關(guān)鍵幀k對(duì)應(yīng)的面特征A，U，V對(duì)應(yīng)的坐標(biāo)。

利用幀與子圖中的點(diǎn)到線的距離de與點(diǎn)到面的距離dh，可以組成距離矩陣d，基于掃描匹配方法，可以建立以下代價(jià)函數(shù) f來(lái)求解幀到子圖的最優(yōu)位姿變換關(guān)系

利用列文伯格馬切爾特（LM）方法可以構(gòu)建以下優(yōu)化方程來(lái)求解幀間位姿的變換：

式中：J為雅可比矩陣，J=?f/?Tk+1；D為系數(shù)矩陣；μ為信賴區(qū)域半徑。

迭代推導(dǎo)出最優(yōu)估計(jì)：

式中λ為阻尼因子。

采用以上列文伯格馬切爾特優(yōu)化方法收斂后可以得到當(dāng)前位姿估計(jì)值Tk+1=。位姿Tk和Tk+1之間的相對(duì)位姿變換為

2.5 相對(duì)位姿因子構(gòu)建

利用雷達(dá)相對(duì)位姿構(gòu)建關(guān)鍵幀位姿之間的約束關(guān)系，在滑窗中聯(lián)合慣性預(yù)積分一起參與優(yōu)化。相鄰位姿間的優(yōu)化目標(biāo)項(xiàng)為

式中：x為待優(yōu)化狀態(tài)變量；rL為相對(duì)位姿因子殘差；xi和xj分別為i和j時(shí)刻對(duì)應(yīng)的運(yùn)動(dòng)狀態(tài)；Σ為約束不確定度的協(xié)方差矩陣， Σ∈R6×6，R為旋轉(zhuǎn)矩陣。由于估計(jì)過(guò)程是基于IMU坐標(biāo)系，相對(duì)位姿觀測(cè)需要轉(zhuǎn)換到LiDAR坐標(biāo)系：

2.5.1 殘差計(jì)算

從i時(shí)刻關(guān)鍵幀到j(luò)時(shí)刻關(guān)鍵幀之間的位姿變換的觀測(cè)和期望之間的殘差可以表示為

式中rL(χ)為 雷達(dá)坐標(biāo)系下相對(duì)于觀測(cè)zBBij 和待估計(jì)變量χ的殘差，為從j時(shí)刻到i時(shí)刻IMU坐標(biāo)系下的觀測(cè)。

推導(dǎo)可得相對(duì)位姿因子殘差的旋轉(zhuǎn)部分rθ和平移部分rξ：

2.5.2 雅可比矩陣計(jì)算

（1）向量空間：平移殘差向量在向量空間參數(shù)塊的雅可比矩陣為

（2）流形空間：平移與旋轉(zhuǎn)殘差向量在流形空間參數(shù)塊的雅可比為

2.5.3 協(xié)方差計(jì)算

由于前端配準(zhǔn)采用了掃描到子圖的方法，因此可通過(guò)計(jì)算所有匹配成功特征點(diǎn)來(lái)計(jì)算整體掃描匹配的協(xié)方差。計(jì)算當(dāng)前掃描幀c的特征點(diǎn)u的坐標(biāo)Xuc在子圖w中對(duì)應(yīng)特征點(diǎn)u的坐標(biāo)Xuw：

所有匹配成功的雷達(dá)特征計(jì)算的協(xié)方差矩陣H為

定義噪聲矩陣Λ為

式中σx，σy，σz為x，y，z方向的的噪聲西格瑪值，受到2個(gè)點(diǎn)Xuw和Xuc噪聲的影響，參考激光雷達(dá)的噪聲參數(shù)，設(shè)置噪聲典型值為5 cm。

協(xié)方差矩陣對(duì)應(yīng)的信息矩陣?為

利用式（22）從初始時(shí)刻的06×6矩陣開(kāi)始迭代計(jì)算信息矩陣，隨著運(yùn)動(dòng)距離增加，信息矩陣的不確定度逐漸增長(zhǎng)。

3 IMU預(yù)積分約束構(gòu)建

IMU預(yù)積分作為一種高頻慣性觀測(cè)數(shù)據(jù)處理方法，在視覺(jué)慣性里程計(jì)、激光慣性里程計(jì)中逐漸得到應(yīng)用，其核心思想是將相鄰關(guān)鍵幀之間的大量慣性觀測(cè)進(jìn)行集成，在關(guān)鍵幀的局部慣性坐標(biāo)系下開(kāi)始積分，獲得獨(dú)立的相對(duì)運(yùn)動(dòng)約束，與其他約束聯(lián)合優(yōu)化，實(shí)現(xiàn)完整的狀態(tài)估計(jì)。預(yù)積分原理如圖4所示，橫坐標(biāo)代表時(shí)間戳。預(yù)積分過(guò)程就是將關(guān)鍵幀So、Si和Sj之間的高頻IMU觀測(cè)獨(dú)立建立為IMU運(yùn)動(dòng)約束。本文采用視覺(jué)慣性導(dǎo)航系統(tǒng)（Visual Inertial Navigation System,VINS）的預(yù)積分方法對(duì)IMU進(jìn)行積分約束，預(yù)積分觀測(cè)方程、殘差、雅可比矩陣和協(xié)方差矩陣推導(dǎo)參考文獻(xiàn)[26]。

圖4 預(yù)積分原理Fig.4 Principleof pre-integration

4 回環(huán)檢測(cè)約束構(gòu)建

回環(huán)檢測(cè)優(yōu)化對(duì)于獲得全局一致的地圖有很大作用。本文采用文獻(xiàn)[13]提出的回環(huán)檢測(cè)方法，通過(guò)以下3個(gè)步驟實(shí)現(xiàn)回環(huán)檢測(cè)與約束構(gòu)建。

（1）里程判斷：當(dāng)新的關(guān)鍵幀建立后，首先搜索因子圖并找到與新關(guān)鍵幀位姿在歐氏距離上接近的關(guān)鍵幀。小于設(shè)定距離閾值的歷史關(guān)鍵幀將被作為回環(huán)候選關(guān)鍵幀。

（2）形貌相似度判斷：利用ICP掃描匹配方法匹配當(dāng)前關(guān)鍵幀的點(diǎn)云，與回環(huán)候選關(guān)鍵幀前后設(shè)定的某些幀構(gòu)成局部子圖。利用最近點(diǎn)對(duì)的均方根最小距離計(jì)算擬合率，與設(shè)定閾值比較來(lái)確定是否為真實(shí)回環(huán)關(guān)鍵幀。

（3）回環(huán)因子構(gòu)建：利用以上雙重判斷選擇出回環(huán)關(guān)鍵幀后，通過(guò)式（12）計(jì)算相對(duì)變換因子，并將相對(duì)變換因子添加到全局因子圖中。

5 實(shí)驗(yàn)分析

5.1 野外顛簸路面試驗(yàn)

為了驗(yàn)證LI?SLAM方法在顛簸路面和復(fù)雜場(chǎng)景的精度與魯棒性，基于輪式移動(dòng)機(jī)器人試驗(yàn)平臺(tái)在野外開(kāi)展了試驗(yàn)。機(jī)器人搭載VLP?16 LiDAR，MTi?G710 IMU。輪式移動(dòng)機(jī)器人搭載的運(yùn)算平臺(tái)配置為i7?8700k，32G內(nèi)存?；谳喪揭苿?dòng)機(jī)器人的試驗(yàn)平臺(tái)和3個(gè)試驗(yàn)場(chǎng)地如圖5所示。

圖5 輪式移動(dòng)機(jī)器人試驗(yàn)平臺(tái)及野外試驗(yàn)場(chǎng)地Fig.5 Wheeled mobile robot test platform and field testsenvironment

5.1.1 精度分析

為了分析LI?SLAM方法的優(yōu)越性，將其與當(dāng)前最優(yōu)的激光算法LOAM方法、LINS方法、LIO?mapping方法進(jìn)行了對(duì)比。幾種方法的軌跡對(duì)比及其對(duì)齊到衛(wèi)星地圖上的結(jié)果如圖6?圖8所示?？煽闯觯涸谒性囼?yàn)場(chǎng)地中，LI?SLAM方法和LOAM方法的地圖一致性最好，與真實(shí)路線基本吻合，LI?SLAM方法對(duì)旋轉(zhuǎn)有更佳的適應(yīng)能力。LIO?mapping方法無(wú)法實(shí)時(shí)運(yùn)行，在0.5倍速下可以獲得完整軌跡，但在初始運(yùn)動(dòng)階段出現(xiàn)了較大程度的方向偏移，初始化過(guò)程容易失敗。LINS方法由于僅利用了最新的觀測(cè)信息，在復(fù)雜地形下出現(xiàn)了漂移。

圖6 試驗(yàn)場(chǎng)地1中各種方法的運(yùn)行軌跡Fig.6 Trajectoriesfor different methods in different test scenario 1

圖8 試驗(yàn)場(chǎng)地3中各種方法的運(yùn)行軌跡Fig.8 Trajectoriesfor different methods in different test scenario 3

不同方法在機(jī)器人起點(diǎn)、終點(diǎn)間測(cè)量距離與真值的誤差見(jiàn)表1，測(cè)量距離通過(guò)圖上的坐標(biāo)差獲得，距離真值可以通過(guò)全站儀等測(cè)量和計(jì)算獲得。從表1可看出：LI?SLAM方法在試驗(yàn)場(chǎng)地1、試驗(yàn)場(chǎng)地2中誤差最小，在試驗(yàn)場(chǎng)地3中與LOAM方法的誤差接近，均明顯優(yōu)于其他方法。

圖7 試驗(yàn)場(chǎng)地2中各種方法的運(yùn)行軌跡Fig.7 Trajectoriesfor different methods in different test scenario 2

表1 不同方法起點(diǎn)、終點(diǎn)間距離與真值的誤差Table 1 Thedistance and truth valueerror between thestarting point and the end point of different methods

5.1.2 運(yùn)算速度分析

LI?SLAM方法各個(gè)模塊耗時(shí)的均值和最大值見(jiàn)表2，可看出回環(huán)檢測(cè)模塊耗時(shí)最長(zhǎng)，其次是建圖模塊。實(shí)際應(yīng)用中，回環(huán)檢測(cè)和地圖構(gòu)建模塊本身不需要實(shí)時(shí)，可以根據(jù)環(huán)境和需求人為設(shè)定。其余模塊均滿足10 Hz雷達(dá)采樣頻率的實(shí)時(shí)運(yùn)行需求。

表2 野外試驗(yàn)時(shí)LI?SLAM方法中各模塊耗時(shí)均值與最大值Table 2 The averageand maximum time consumption of each moduleof LI-SLAM method in field tests ms

5.1.3 回環(huán)檢測(cè)效果分析

在試驗(yàn)場(chǎng)地1中機(jī)器人起始和終止位置發(fā)生的回環(huán)優(yōu)化前后的效果如圖9所示。圖9（a）中回環(huán)尚未發(fā)生，在位置2的樹(shù)木和道路出現(xiàn)明顯的模糊，圖9（b）中回環(huán)優(yōu)化后樹(shù)木辨別更加清晰，獲得了全局一致性的地圖，證明了回環(huán)檢測(cè)的有效性。

圖9 回環(huán)優(yōu)化效果Fig.9 Effect of loop optimization

5.2 地下車庫(kù)少結(jié)構(gòu)環(huán)境試驗(yàn)

為了驗(yàn)證LI?SLAM方法在少結(jié)構(gòu)的井底車場(chǎng)等環(huán)境的應(yīng)用效果，采用自主研發(fā)的CUMTV?C煤礦救援機(jī)器人平臺(tái)[27]進(jìn)行了地下車庫(kù)的模擬試驗(yàn)，如圖10所示，在機(jī)器人上方水平和傾斜30°各部署1個(gè)16線激光雷達(dá)，分別用于定位和構(gòu)建稠密地圖。機(jī)器人搭載Xsens MTi?G?710 IMU，用于慣性數(shù)據(jù)測(cè)量。

圖10 地下車庫(kù)環(huán)境及機(jī)器人平臺(tái)Fig.10 Underground garage environment and robot platform

LI?SLAM方法的建模效果如圖11所示，綠色到紅色漸變的球形表示機(jī)器人的關(guān)鍵幀位姿，球形之間連線為相對(duì)位姿約束與回環(huán)約束。從圖11可看出，LI?SLAM方法具有較好的建模精度，局部精細(xì)化程度高。

圖11 LI?SLAM方法在地下車庫(kù)建模效果Fig.11 Modeling resultsof LI?SLAM method in underground garage

不同方法的運(yùn)動(dòng)軌跡如圖12所示，由于地面較為平坦，LINS、LOAM、LI?SLAM方法均獲得了平滑的運(yùn)動(dòng)軌跡，LIO?Mapping方法在初始化時(shí)出現(xiàn)了偏移，表明該方法在缺乏足夠運(yùn)動(dòng)激勵(lì)時(shí)難以獲得魯棒的狀態(tài)估計(jì)。

圖12 不同方法軌跡對(duì)比Fig.12 Comparison of trajectoriesof different methods

不同方法起點(diǎn)、終點(diǎn)間距離的誤差見(jiàn)表3，可看出LOAM和LI?SLAM方法的誤差最小，證明了緊耦合方法可以提升定位精度。

表3 不同方法起點(diǎn)、終點(diǎn)間距離的誤差Table3 The distance error between the starting point and theend point of different methods

LI?SLAM方法各模塊耗時(shí)均值與最大值見(jiàn)表4，可以看出：去畸變、特征提取、優(yōu)化模塊均可滿足10 Hz運(yùn)行條件，回環(huán)檢測(cè)與地圖構(gòu)建模塊無(wú)高頻運(yùn)行需求。

表4 地下車庫(kù)試驗(yàn)時(shí)LI?SLAM方法中各模塊耗時(shí)均值與最大值Table4 Theaverage and maximum time consumption of each module in LI-SLAM method in underground garage tests ms

5.3 煤礦井下現(xiàn)場(chǎng)工業(yè)性測(cè)試

2020年11月2日，在晉能控股山西煤業(yè)股份有限公司塔山煤礦開(kāi)展了煤礦救援機(jī)器人工業(yè)性驗(yàn)證試驗(yàn)，如圖13所示。采用自主研發(fā)的CUMTV?C煤礦救援機(jī)器人平臺(tái)進(jìn)行試驗(yàn)[27]，機(jī)器人實(shí)時(shí)運(yùn)行LI?SLAM方法，通過(guò)無(wú)線信號(hào)傳輸（帶寬大于120 Mbit/s）實(shí)時(shí)顯示構(gòu)建的地圖，用于人員遙控和自主導(dǎo)航。

圖13 煤礦井下現(xiàn)場(chǎng)試驗(yàn)Fig.13 Field testsin underground coal mine

機(jī)器人在行走過(guò)程遇到了各類復(fù)雜工況，包括大量水汽、斜坡、岔口、顛簸路面等，如圖14所示。搭載LI?SLAM方法的煤礦救援機(jī)器人在各類地形環(huán)境中均可穩(wěn)定運(yùn)行，說(shuō)明LI?SLAM方法滿足魯棒性、實(shí)時(shí)性需求。

圖14 機(jī)器人在井下各類復(fù)雜環(huán)境中運(yùn)行情況Fig.14 Running process of the robot in several complex environment in underground coal mine

LI?SLAM方法實(shí)際運(yùn)行中的一組建模結(jié)果如圖15所示。機(jī)器人行駛巷道直線距離為273 m，將地圖上的距離作為測(cè)量值，利用全站儀測(cè)量獲得人工標(biāo)志點(diǎn)的定位結(jié)果之間的距離作為真值進(jìn)行對(duì)比，分析30組距離結(jié)果表明，平均誤差小于15 cm，具有較高的定位和建模精度，基本滿足煤礦移動(dòng)機(jī)器人的定位建模精度需求。

圖15 煤礦井下建模效果Fig.15 Modeling resultsin underground coal mine

6 結(jié)論

（1）提出了一種基于因子圖優(yōu)化框架的LiDAR和IMU緊耦合SLAM（LI?SLAM）方法，基于點(diǎn)?線和點(diǎn)?面掃描匹配構(gòu)建激光相對(duì)位姿約束，融合IMU預(yù)積分因子約束和回環(huán)因子約束，利用因子圖優(yōu)化實(shí)現(xiàn)了LiDAR/IMU緊耦合的SLAM。

（2）詳細(xì)分析了激光約束的構(gòu)建過(guò)程，設(shè)計(jì)了點(diǎn)云去畸變、特征提取、關(guān)鍵幀與子圖構(gòu)建、特征關(guān)聯(lián)策略，解析推導(dǎo)了雷達(dá)相對(duì)位姿因子的殘差、雅可比與協(xié)方差矩陣。

（3）設(shè)計(jì)了雷達(dá)相對(duì)位姿因子、慣性預(yù)積分因子、回環(huán)檢測(cè)因子的代價(jià)函數(shù)，構(gòu)建了激光慣性里程計(jì)因子圖緊耦合模型，并設(shè)計(jì)了無(wú)約束優(yōu)化目標(biāo)函數(shù)。

（4）在野外顛簸路面、地下車庫(kù)進(jìn)行了大量測(cè)試，在真實(shí)煤礦井下現(xiàn)場(chǎng)進(jìn)行了工業(yè)性試驗(yàn)。結(jié)果表明：與LOAM方法、LINS方法、LIO?mapping方法相比，LI?SLAM方法在精度、魯棒性方面表現(xiàn)更佳，對(duì)于顛簸、少結(jié)構(gòu)、水汽等復(fù)雜煤礦井下環(huán)境的移動(dòng)機(jī)器人精確定位與地圖構(gòu)建有更好的適用性，具有較高的定位和建模精度，基本滿足煤礦移動(dòng)機(jī)器人的定位與建模精度需求，對(duì)井下各類移動(dòng)機(jī)器人的定位建圖與自主導(dǎo)航具有較強(qiáng)的指導(dǎo)意義和參考價(jià)值。