韓 超， 陳 敏， 黃宇昊， 趙明輝， 杜乾坤， 梁慶華

(1. 上海交通大學(xué) 機(jī)械與動(dòng)力工程學(xué)院，上海 200240； 2. 中煤科工集團(tuán)上海有限公司，上海 201100)

同步定位和建圖(SLAM)是實(shí)現(xiàn)機(jī)器人在未知環(huán)境下的定位和移動(dòng)的重要技術(shù)方法[1].定位精度是井下巡檢的核心指標(biāo)，高精度的定位算法是巡檢過程中導(dǎo)航和避障的基礎(chǔ).但井下環(huán)境復(fù)雜，具有低照度、弱紋理、圖像特征難以識(shí)別的特點(diǎn)[2]，給基于視覺的SLAM算法帶來了極大的困難.而激光SLAM算法測(cè)量距離遠(yuǎn)、精度高，利用環(huán)境的結(jié)構(gòu)特征進(jìn)行定位[3]，在井下環(huán)境中更具應(yīng)用前景[4-5].

目前，SLAM算法通過估計(jì)的相鄰幀之間的位姿變換不斷疊加進(jìn)行位姿信息計(jì)算，隨著巡檢時(shí)間的增加，每次位姿估計(jì)中的偏差不斷累積形成較大的累積誤差，制約了算法的定位精度.

利用圖像語義信息豐富的特點(diǎn)，視覺SLAM算法如ORB-SLAM[6]、VINS[7]等，多使用基于詞袋模型[8]的回環(huán)檢測(cè)方法，利用預(yù)先訓(xùn)練的視覺特征單詞構(gòu)建詞典，將圖像中的特征描述為一個(gè)單詞，圖像中單詞種類和數(shù)量構(gòu)成圖像的詞袋向量，通過詞袋向量之間的相似度進(jìn)行回環(huán)檢測(cè).

圖1 基于全局特征描述子的回環(huán)檢測(cè)算法流程Fig.1 Flow chart of loop closure detection algorithm flow based on global feature descriptor

但激光雷達(dá)分辨率低，傳遞信息不如圖像豐富，基于詞袋模型的回環(huán)檢測(cè)方法難以應(yīng)用.在LeGO-LOAM[9]和LIO-SAM[10]等激光SLAM方案中使用基于里程計(jì)的回環(huán)檢測(cè)方法，通過當(dāng)前位置信息判斷是否可能存在回環(huán)，如當(dāng)前位置一定范圍內(nèi)，存在歷史軌跡點(diǎn)，則與對(duì)應(yīng)關(guān)鍵幀進(jìn)行匹配, 并通過迭代最近點(diǎn)(ICP)配準(zhǔn)方法進(jìn)行位姿計(jì)算，實(shí)現(xiàn)累計(jì)誤差的校正.此類方法效果受巡檢距離限制，當(dāng)累積誤差超過一定范圍時(shí)出現(xiàn)無法檢測(cè)到回環(huán)或錯(cuò)誤檢測(cè)的問題，仍未解決累積誤差不斷增長的問題，受傳感器精度影響大，且ICP方法對(duì)全部點(diǎn)云進(jìn)行配準(zhǔn)計(jì)算，計(jì)算時(shí)間較長.

文獻(xiàn)[11-12]通過正態(tài)分布變換(NDT)或特征直方圖檢測(cè)閉環(huán)，通過距離信息判定當(dāng)前位置閾值與歷史軌跡的一致性，使用NDT方法或特征直方圖方法進(jìn)行位姿估計(jì)與二次回環(huán)檢測(cè)，有效提升了匹配準(zhǔn)確率，但召回率以及回環(huán)檢測(cè)的實(shí)時(shí)性無法適用于井下無人機(jī)的定位需求.因此，使用回環(huán)檢測(cè)方法解決井下無人機(jī)巡檢保持長時(shí)間高精度定位的問題，需避免對(duì)里程計(jì)的依賴，使用場(chǎng)景相似特征進(jìn)行回環(huán)檢測(cè)，同時(shí)提高計(jì)算效率，保證實(shí)時(shí)性.

本文提出一種適用于激光SLAM的點(diǎn)云全局特征描述子回環(huán)檢測(cè)方案，不依賴于位姿信息，利用礦井通道的幾何結(jié)構(gòu)特征進(jìn)行匹配，通過計(jì)算與誤差較小的歷史場(chǎng)景之間的位姿變換，消除累積誤差，并完成了完整SLAM算法框架的搭建，實(shí)時(shí)性好.該框架里程計(jì)部分基于激光里程計(jì)和建圖(LOAM)[13]算法計(jì)算不帶回環(huán)的位姿信息，回環(huán)檢測(cè)部分基于曲率劃分的特征點(diǎn)與點(diǎn)云質(zhì)心之間的角度關(guān)系和尺度關(guān)系進(jìn)行相似度檢測(cè)，使用平面點(diǎn)和邊角點(diǎn)配準(zhǔn)方法優(yōu)化計(jì)算速度，并由因子圖優(yōu)化方法保證全局一致性.最終, 通過仿真和開源數(shù)據(jù)集實(shí)驗(yàn)驗(yàn)證了所提算法的有效性，本文研究可為激光SLAM的全局特征提取及回環(huán)算法提供理論基礎(chǔ).

1 基于全局特征描述子的回環(huán)檢測(cè)

設(shè)計(jì)的定位系統(tǒng)整體分為2個(gè)模塊：里程計(jì)模塊和回環(huán)檢測(cè)模塊.

里程計(jì)模塊參考LOAM算法[13]實(shí)現(xiàn), 輸出里程計(jì)位姿信息，并根據(jù)點(diǎn)云三維曲率完成了特征點(diǎn)云的篩選，將邊角特征點(diǎn)云Pedg和平面特征點(diǎn)云Ppla傳遞給回環(huán)檢測(cè)模塊.

回環(huán)檢測(cè)模塊中進(jìn)行回環(huán)識(shí)別和位姿的更新.為實(shí)現(xiàn)對(duì)井下通道的幾何描述，在激光雷達(dá)相對(duì)坐標(biāo)系下，進(jìn)行特征向量提取和中心點(diǎn)計(jì)算，基于點(diǎn)云的角度分布和尺度分布關(guān)系，構(gòu)建點(diǎn)云的全局特征描述子；以多維描述子之間的相關(guān)性作為點(diǎn)云相似度的評(píng)價(jià)指標(biāo)；使用特征點(diǎn)配準(zhǔn)方法以改進(jìn)全局配準(zhǔn)計(jì)算耗時(shí)過大的問題；利用檢測(cè)到的相似場(chǎng)景建立回環(huán)約束，通過因子圖優(yōu)化消除累積誤差并保證軌跡和地圖的全局一致性.回環(huán)檢測(cè)模塊是本文研究的重點(diǎn)，算法流程框圖如圖1所示.其中：nedg和npla分別為邊角特征點(diǎn)云和平面特征點(diǎn)云對(duì)應(yīng)的特征向量；Hk為關(guān)鍵幀k的全局描述子向量；pedg,k和ppla,k分別為關(guān)鍵幀k的邊角特征和平面特征.

1.1 特征向量提取

使用全局點(diǎn)云中具有代表性信息的邊角特征和平面特征進(jìn)行數(shù)據(jù)處理，對(duì)里程計(jì)模塊中提取的邊角特征點(diǎn)云Pedg={pedg,1,pedg,2, …,pedg,k, …}和平面特征點(diǎn)云Ppla={ppla,1,ppla,2, …,ppla,k, …}分別提取特征向量.取pedg,k或ppla,k中的任意一點(diǎn)pi，使用k-維(KD)樹搜索其所在點(diǎn)云中鄰近的k個(gè)點(diǎn)，計(jì)算該部分點(diǎn)云的協(xié)方差矩陣，具體步驟如下.

步驟1計(jì)算鄰近k點(diǎn)的質(zhì)心：

(1)

步驟2計(jì)算協(xié)方差矩陣C，即質(zhì)心oc到鄰近點(diǎn)云中所有點(diǎn)pi的向量與其轉(zhuǎn)置的內(nèi)積：

(2)

步驟3對(duì)協(xié)方差矩陣進(jìn)行奇異值分解(SVD)，求特征值：

(3)

且λ1<λ2<λ3

(4)

式中：λj(j=1,2,3)為矩陣C對(duì)應(yīng)的特征值；nj(j=1,2,3)為矩陣C對(duì)應(yīng)的特征向量；Λ為對(duì)角線λi組成的對(duì)角矩陣；U,V為奇異值分解后的實(shí)正交矩陣.

步驟4由|λjE-C|n=0解得對(duì)應(yīng)的特征向量n1,n2,n3.

其中，邊角特征提取線向量為特征向量nedg，鄰近點(diǎn)投影至該方向上的向量方差最最小，為最大特征值λ3對(duì)應(yīng)的特征向量n3；平面特征提取法向量為特征向量npla，鄰近點(diǎn)投影至該方向上的向量方差最最大，為最小特征值λ1對(duì)應(yīng)的特征向量n1[14].

1.2 特征描述和相似度計(jì)算

設(shè)計(jì)全局特征描述子的計(jì)算方法.通過計(jì)算全局點(diǎn)云P中所有點(diǎn)的三維坐標(biāo)平均值得到當(dāng)前幀的點(diǎn)云質(zhì)心oc，以點(diǎn)云Pedg和Ppla中所有點(diǎn)對(duì)應(yīng)的特征向量的均值作為質(zhì)心oc處對(duì)應(yīng)的特征向量nc，進(jìn)而計(jì)算點(diǎn)云的全局特征描述子，邊角點(diǎn)云Pedg和平面點(diǎn)云Ppla分開計(jì)算.

遍歷點(diǎn)云P中的所有點(diǎn)，確定以質(zhì)心oc為原點(diǎn)o的參考坐標(biāo)系uvw：

u=nc

(5)

(6)

w=u×v

(7)

式中：pt為點(diǎn)云Pedg或Ppla中任意選取的一點(diǎn)；nc為點(diǎn)云質(zhì)心處的歸一化特征向量；‖pt-oc‖2為點(diǎn)pt到質(zhì)心oc之間的歐氏距離.以pt,oc確定的坐標(biāo)系如圖2所示，其中：nt為點(diǎn)pt處歸一化之后的特征向量.

圖2 全局點(diǎn)云下的uvw坐標(biāo)系Fig.2 uvw coordinate system of global point cloud

以上述計(jì)算得到的uvw坐標(biāo)系為基準(zhǔn)，計(jì)算點(diǎn)云特征向量相對(duì)于基準(zhǔn)坐標(biāo)系3個(gè)坐標(biāo)軸的角度分布，則有：

α=v·nt

(8)

(9)

θ=arctan(w·nt,u·nt)

(10)

d=‖pt-oc‖2

(11)

式中：d為點(diǎn)pt,oc之間的歐氏距離；α為特征向量nt與坐標(biāo)軸v之間的夾角，值域?yàn)閇-1, 1]；φ為pt,oc之間連線方向與坐標(biāo)軸u之間的夾角，值域?yàn)閇-1, 1]；θ為特征向量nt投影至wou平面時(shí)與坐標(biāo)軸w之間的夾角，值域?yàn)閇-π/2, π/2].

利用上述公式進(jìn)行計(jì)算后，α,φ,θ,d為非線性相關(guān)的，可以替代兩點(diǎn)的(x,y,z,nx,ny,nz)共12維數(shù)據(jù)對(duì)點(diǎn)云的分布進(jìn)行幾何關(guān)系描述.

遍歷點(diǎn)云Pedg和Ppla中所有點(diǎn)，與質(zhì)心點(diǎn)oc構(gòu)成點(diǎn)對(duì)，計(jì)算對(duì)應(yīng)的α,φ,θ,d四要素.將α,φ,θ的值域區(qū)間進(jìn)行等分，統(tǒng)計(jì)全局點(diǎn)云三要素在各區(qū)間內(nèi)的分布情況，構(gòu)成特征描述子的角度分量.

計(jì)算全局點(diǎn)云P中距離質(zhì)心點(diǎn)oc最遠(yuǎn)的位置，將最大距離均分為多個(gè)區(qū)間，統(tǒng)計(jì)各個(gè)點(diǎn)相對(duì)于質(zhì)心距離在區(qū)間中的分布，以此構(gòu)成特征描述子的尺度分量.

最終設(shè)計(jì)的點(diǎn)云全局描述特征描述子為230維向量，其中前105維為邊角特征點(diǎn)云Pedg計(jì)算得到的角度分量αe,φe,θe，中間105維數(shù)據(jù)為平面特征點(diǎn)云Ppla計(jì)算得到的角度分量αp,φp,θp，均以35維的向量表示，最后20維數(shù)據(jù)為當(dāng)前幀點(diǎn)云P全部點(diǎn)的尺度分量βd.

本文提出的全局描述子計(jì)算方法以點(diǎn)云中心建立相對(duì)坐標(biāo)系進(jìn)行計(jì)算，具有旋轉(zhuǎn)不變性和平移不變性，如圖3所示.圖3(a)為原始點(diǎn)云P及其旋轉(zhuǎn)45°、平移10 m后的點(diǎn)云，經(jīng)旋轉(zhuǎn)和平移變換后，如圖3(b)和3(c)所示，點(diǎn)云的全局特征描述子不發(fā)生變化.其中：DH為描述子H的維度信息；VH為對(duì)應(yīng)維度的數(shù)值信息.旋轉(zhuǎn)不變性和平移不變性說明，對(duì)于同一場(chǎng)景下，激光雷達(dá)傳感器處于不同角度和位置時(shí)，仍能進(jìn)行準(zhǔn)確地進(jìn)行回環(huán)檢測(cè).

圖3 旋轉(zhuǎn)和平移不變性實(shí)驗(yàn)Fig.3 Experiments of rotation and translation invariance

使用點(diǎn)云全局特征描述子的皮爾遜相關(guān)系數(shù)[15]作為衡量點(diǎn)云相似度的標(biāo)準(zhǔn)，其計(jì)算公式如下：

(12)

式中：H1=[h11h12…h(huán)1i…]，H2=[h21h22…h(huán)2i…]為歸一化后的點(diǎn)云全局特征描述子；μH1和μH2分別為H1和H2元素的平均值向量；E為數(shù)學(xué)期望;σ為方差；SH1, H2為相關(guān)系數(shù)，其取值范圍為[0, 1].當(dāng)SH1, H2接近于1時(shí)，說明特征描述子為正相關(guān)關(guān)系，點(diǎn)云的相似度越高[16].

1.3 位姿變換

對(duì)篩選后相似度大于閾值的當(dāng)前幀點(diǎn)云Pcur和歷史相似幀點(diǎn)云Psim進(jìn)行相對(duì)位姿變換的計(jì)算.相對(duì)位姿的計(jì)算使用點(diǎn)云的邊角特征和平面特征配準(zhǔn)方法，即使用三維點(diǎn)的曲率計(jì)算公式[13]計(jì)算點(diǎn)云不同位置處的光滑度，以光滑度為依據(jù)提取點(diǎn)云的邊角特征和平面特征，只對(duì)邊角特征和平面特征進(jìn)行點(diǎn)云配準(zhǔn)，提高了計(jì)算速度.

為減少點(diǎn)云的誤匹配概率，使用曲率估計(jì)方法對(duì)點(diǎn)云配準(zhǔn)效果進(jìn)行驗(yàn)證.當(dāng)前幀邊角點(diǎn)和平面點(diǎn)分別在相似幀邊角特征和平面特征中搜索匹配點(diǎn)，對(duì)相似幀的特征進(jìn)行主成分分析，計(jì)算其特征值λ1,λ2,λ3(λ1>λ2>λ3).其中，當(dāng)點(diǎn)云為邊角特征時(shí)，掃描點(diǎn)按照線方向分布；為平面特征時(shí)，掃描點(diǎn)呈現(xiàn)平面式分布，在垂直平面方向上的方差最小.邊角特征和平面特征應(yīng)分別滿足如下關(guān)系式：

(13)

對(duì)于配準(zhǔn)后的相似幀點(diǎn)云，構(gòu)建需要求解的目標(biāo)函數(shù)，使用Levenberg-Marquardt法迭代求解使目標(biāo)函數(shù)最小的位姿變換，其目標(biāo)函數(shù)如下：

F=φe+φp=

(14)

(15)

(16)

圖4 邊角特征和平面特征點(diǎn)云配準(zhǔn)Fig.4 Registration of edge features and plane features

1.4 因子圖優(yōu)化

通過位姿變換方法可以獲得當(dāng)前關(guān)鍵幀與歷史相似幀之間的相對(duì)位姿變換，以此建立新的回環(huán)約束，構(gòu)建因子圖如圖5所示.其中：g為測(cè)量函數(shù)；R為測(cè)量噪聲協(xié)方差矩陣；因子圖由一系列不同時(shí)刻位姿測(cè)量值sa(a=0,1,2,…)和地圖路標(biāo)點(diǎn)ma(a=0,1,2,…)的觀測(cè)量構(gòu)成.每條實(shí)線為1次前后幀之間的位姿累積測(cè)量事件，每條虛線為1次檢測(cè)到的回環(huán)約束測(cè)量事件，將因子圖表達(dá)轉(zhuǎn)換為線性化的約束的線性矩陣Ω的形式，每一個(gè)新的測(cè)量值都會(huì)致使信息矩陣發(fā)生更新.

圖5 因子圖優(yōu)化模型Fig.5 Model of factor graph optimization

對(duì)于1次測(cè)量事件，t1和t2時(shí)刻位姿測(cè)量值分別為st1,st2，對(duì)應(yīng)的路標(biāo)點(diǎn)集合為mt2測(cè)量事件構(gòu)成的約束函數(shù)如下：

J′=

[st2-g(mt2,st1)]TR-1[st2-g(mt2,st1)]

(17)

因子圖優(yōu)化的過程為求解目標(biāo)函數(shù)得到概率最大的地圖和位姿信息路徑，其目標(biāo)函數(shù)為測(cè)量約束的集合，目標(biāo)函數(shù)公式如下：

∑[st2-g(mt2,st1)]TR-1[st2-g(mt2,st1)]

(18)

因子圖優(yōu)化的方法將圖映射為信息矩陣的表達(dá)方式進(jìn)行求解，利用圖的稀疏性特點(diǎn)，在求解過程中保持較低的計(jì)算量和錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)，優(yōu)于一般的濾波方法[17].

2 實(shí)驗(yàn)結(jié)果與討論

2.1 準(zhǔn)確率和召回率實(shí)驗(yàn)

準(zhǔn)確率和召回率是衡量回環(huán)檢測(cè)模塊中相似度檢測(cè)算法性能的重要指標(biāo)，在回環(huán)檢測(cè)算法設(shè)計(jì)中需首先保證較高的準(zhǔn)確率，其次在滿足高準(zhǔn)確率的同時(shí)盡可能提高相似度檢測(cè)模塊的召回率.

準(zhǔn)確率和召回率的計(jì)算方法如下：

(19)

(20)

式中：η為準(zhǔn)確率；ε為召回率；TP為正確識(shí)別出同一場(chǎng)景的數(shù)量；FP為將不同場(chǎng)景錯(cuò)誤識(shí)別為同一場(chǎng)景的數(shù)量；FN為同一場(chǎng)景被錯(cuò)誤識(shí)別為不同場(chǎng)景的數(shù)量.

實(shí)驗(yàn)采用蘇黎世聯(lián)邦理工學(xué)院公開數(shù)據(jù)集[18]進(jìn)行測(cè)試，該數(shù)據(jù)集為手持激光雷達(dá)在真實(shí)礦井中掃描得到的數(shù)據(jù).改變相似度檢測(cè)算法的閾值ω(0<ω<1)，以此測(cè)試算法在不同閾值情況下的準(zhǔn)確率和召回率，結(jié)果如圖6所示.

由圖6可知，在保證準(zhǔn)確率為100%的條件下，召回率最高可以達(dá)到50.76%，超過一半的相似場(chǎng)景會(huì)被召回，此時(shí)閾值ω為0.95，隨著閾值ω進(jìn)一步降低，召回率增大，準(zhǔn)確率降低，回環(huán)檢測(cè)可能出現(xiàn)錯(cuò)誤匹配，得到錯(cuò)誤的位姿信息，對(duì)定位和建圖產(chǎn)生嚴(yán)重的影響、因此應(yīng)保證高準(zhǔn)確率，在后續(xù)算法實(shí)驗(yàn)中均將閾值ω設(shè)置為0.95，此時(shí)準(zhǔn)確率為100%.

圖6 準(zhǔn)確率和召回率曲線Fig.6 Curves of precision and recall rates

2.2 定位精度實(shí)驗(yàn)

本文進(jìn)行了多組仿真和開源數(shù)據(jù)集下的定位精度實(shí)驗(yàn).實(shí)驗(yàn)平臺(tái)CPU為AMD 3600，內(nèi)存16 GB，算法采用C++語言編寫，均在Ubuntu 18.0的機(jī)器人操作系統(tǒng)(ROS)進(jìn)行測(cè)試.仿真實(shí)驗(yàn)中，使用文獻(xiàn)[19]提供的開源數(shù)據(jù)進(jìn)行測(cè)試，該數(shù)據(jù)來自于美國國防部高級(jí)研究計(jì)劃局(DARPA)在匹斯堡舉行的“DARPA地下煤礦挑戰(zhàn)賽”.使用該數(shù)據(jù)集進(jìn)行了4組實(shí)驗(yàn)，其中Sim_mine_1和Sim_mine_2為仿真實(shí)驗(yàn)，將井下三維模型導(dǎo)入Gazebo仿真系統(tǒng)，添加傳感器進(jìn)行數(shù)據(jù)采集，模擬巡檢工作.搭建的仿真實(shí)驗(yàn)環(huán)境如圖7所示，仿真實(shí)驗(yàn)中以無人機(jī)作為載體，上方搭載Velodyne-16激光雷達(dá)進(jìn)行數(shù)據(jù)采集.

圖7 井下仿真實(shí)驗(yàn)環(huán)境Fig.7 Environment of underground simulation experiments

M100_Underground和M100_Tunnel分別為在真實(shí)礦井中大疆M100無人機(jī)搭載激光雷達(dá)在井下巷道和礦山隧道巡檢時(shí)錄制的數(shù)據(jù)包文件，其中包含了雷達(dá)運(yùn)動(dòng)過程中的里程計(jì)信息.共進(jìn)行了4組井下環(huán)境的實(shí)驗(yàn)，實(shí)驗(yàn)中每次巡檢結(jié)束時(shí)，傳感器回到起點(diǎn)附近.實(shí)驗(yàn)對(duì)照算法分別為LOAM[13]，以及復(fù)現(xiàn)文獻(xiàn)[9]的回環(huán)檢測(cè)算法，以LOAM作為前端里程計(jì)，使用近鄰點(diǎn)搜索和ICP配準(zhǔn)進(jìn)行位姿變換計(jì)算的LOAM+ICP算法.將3種定位算法結(jié)果分別與仿真實(shí)驗(yàn)中記錄的真實(shí)值或開源數(shù)據(jù)集中記錄的里程計(jì)值進(jìn)行對(duì)比.

實(shí)驗(yàn)的4個(gè)場(chǎng)景分別使用軌跡真實(shí)值進(jìn)行建圖, 地圖情況如圖8所示.圖8(a)和8(b)分別為仿真環(huán)境的地圖，為開采礦井的巷道場(chǎng)景，路段分叉多，通道為非結(jié)構(gòu)化場(chǎng)景，缺乏明顯特征；圖8(c)和8(d)為M100無人機(jī)搭載激光雷達(dá)掃描得到的地圖，圖8(c)環(huán)境與圖8(a)和8(b)類似，分叉路段多，通道為挖掘面，而圖8(d)為礦井入口隧道，采用水泥澆筑，具有支撐柱等結(jié)構(gòu)化特征.

圖8 實(shí)驗(yàn)場(chǎng)景點(diǎn)云地圖Fig.8 Point cloud maps of experiments

算法定位精度實(shí)驗(yàn)中，進(jìn)行初始坐標(biāo)系對(duì)齊，以均方根誤差，全局最大誤差值以及終點(diǎn)處的絕對(duì)誤差作為評(píng)價(jià)定位精度的指標(biāo)，實(shí)驗(yàn)結(jié)果如表1所示，其中：序號(hào)1，2，3，4分別為實(shí)驗(yàn)Sim_mine_1、實(shí)驗(yàn)Sim_mine_2、實(shí)驗(yàn)M100_Underground和實(shí)驗(yàn)M100_Tunnel.

4次實(shí)驗(yàn)中，與沒有回環(huán)檢測(cè)模塊的LOAM算法相比，LOAM+ICP算法和本文算法在最大誤差，終點(diǎn)絕對(duì)誤差以及均方根絕對(duì)誤差3個(gè)指標(biāo)上均得到明顯提高.沒有回環(huán)檢測(cè)模塊的定位算法隨運(yùn)行時(shí)間增長，累積誤差逐步增大，體現(xiàn)為LOAM算法的終點(diǎn)絕對(duì)誤差明顯大于其他兩種算法，這是造成LOAM算法全局軌跡的均方根誤差較大的主要原因.

表1 定位精度實(shí)驗(yàn)結(jié)果Tab.1 Results of localization accuracy experiments

使用定位評(píng)價(jià)和可視化工具(EVO)集繪制 Sim_mine_1實(shí)驗(yàn)中的定位軌跡，初始坐標(biāo)軸x,y,z對(duì)齊后的軌跡曲線如圖9所示，其中：t為時(shí)間.3種算法在z坐標(biāo)下的差值明顯，LOAM算法沒有回環(huán)檢測(cè)模塊，累積誤差未得到校正，最終偏移誤差絕對(duì)值最大；LOAM+ICP和本文算法在加入回環(huán)檢測(cè)后，在巡檢軌跡中間段產(chǎn)生一定偏移，但通過回環(huán)檢測(cè)校正了累積誤差，全局軌跡的均方根誤差較小.

圖9 Sim_mine_1定位實(shí)驗(yàn)軌跡Fig.9 Trajectory of localization experiment Sim_mine_1

與LOAM+ICP算法相比，本文算法的定位精度更高，均方根誤差較小，但兩者終點(diǎn)絕對(duì)誤差較為接近.其原因在于LOAM+ICP算法計(jì)算速度慢，影響了里程計(jì)算法的精度，中期誤差大，但當(dāng)檢測(cè)到回環(huán)時(shí)仍能計(jì)算出準(zhǔn)確的位姿，消除累積誤差.實(shí)驗(yàn)中LOAM+ICP算法和本文算法回環(huán)檢測(cè)部分各模塊的計(jì)算耗時(shí)如表2所示.

表2 算法各模塊運(yùn)行時(shí)間Tab.2 Running time of each module of proposed algorithm

上述實(shí)驗(yàn)數(shù)據(jù)均為實(shí)驗(yàn)過程中記錄的單次完整執(zhí)行對(duì)應(yīng)模塊需要的平均耗時(shí).實(shí)驗(yàn)結(jié)果顯示，相似度檢測(cè)模塊中，LOAM+ICP基于位置進(jìn)行判斷，而本文算法需要計(jì)算點(diǎn)云描述子，算法步驟較多，在該模塊中耗時(shí)增加 3.591 9 ms；而在位姿計(jì)算模塊，LOAM+ICP通過ICP方法計(jì)算相似幀之間的位姿變換，對(duì)點(diǎn)云中的所有點(diǎn)進(jìn)行配準(zhǔn)、目標(biāo)函數(shù)的構(gòu)建和迭代求解，而本文算法基于曲率計(jì)算方法，僅對(duì)邊角和平面特征進(jìn)行處理和計(jì)算，優(yōu)化了計(jì)算效率，平均耗時(shí)由 122.366 8 ms降至 67.405 3 ms，降低了44.92%；回環(huán)時(shí)間反映了完整執(zhí)行一次回環(huán)檢測(cè)模塊需要的平均時(shí)間，得益于位姿計(jì)算時(shí)間的明顯下降，回環(huán)檢測(cè)模塊的整體耗時(shí)由 147.049 1 ms降低為 88.727 4 ms.

在LOAM+ICP算法中由于回環(huán)檢測(cè)模塊平均耗時(shí)超過100 ms，低于點(diǎn)云數(shù)據(jù)采集頻率10 Hz.且基于全局描述子的回環(huán)檢測(cè)方法解決了 LOAM+ICP算法召回率隨累積誤差增大不斷減小的問題，在長時(shí)間運(yùn)行中更加具有優(yōu)勢(shì).

因子圖優(yōu)化模塊對(duì)于保證軌跡的全局一致性和平滑性有重要作用.Sim_mine_1實(shí)驗(yàn)使用本文算法進(jìn)行因子圖優(yōu)化前后的軌跡如圖10所示.其中：綠色軌跡為因子圖優(yōu)化前軌跡；紅色軌跡為因子圖優(yōu)化后軌跡.由局部放大圖可以看出，優(yōu)化后點(diǎn)云地圖無重影，全局一致性良好，因子圖優(yōu)化前軌跡的均方根誤差為 0.888 733 m，大于優(yōu)化后的 0.588 270 m.

圖10 因子圖優(yōu)化前后軌跡Fig.10 Localization trajectory before and after graph optimization

3 結(jié)語

針對(duì)井下巡檢時(shí)定位誤差隨時(shí)間不斷累積的問題，提出一種適用于激光SLAM的基于點(diǎn)云全局特征描述子的回環(huán)檢測(cè)方法.該方法具有以下優(yōu)勢(shì)：① 充分利用了井下巷道的結(jié)構(gòu)特征，相似度檢測(cè)不受位姿信息約束，通過曲率分別提取通道四周墻壁邊角處和平面處的特征向量，利用特征向量和點(diǎn)云中心點(diǎn)之間的角度關(guān)系和點(diǎn)云通道和中心點(diǎn)之間的距離關(guān)系構(gòu)建了全局特征描述子；② 提高了計(jì)算速度，基于邊角點(diǎn)和平面點(diǎn)配準(zhǔn)策略，進(jìn)行高效率的位姿變換算法研究；③全局一致性優(yōu)，采用因子圖優(yōu)化獲得了使全局誤差最小的位姿，校正了累積誤差.

實(shí)驗(yàn)結(jié)果表明，本文算法定位精度相較于不加回環(huán)檢測(cè)的LOAM算法和使用距離信息進(jìn)行回環(huán)檢測(cè)的LOAM+ICP算法得到較大提升，在長時(shí)間長距離的實(shí)驗(yàn)中提升效果顯著.實(shí)驗(yàn)結(jié)果對(duì)于井下執(zhí)行長時(shí)間的巡檢任務(wù)具有指導(dǎo)意義，未來工作中考慮進(jìn)一步研究，將算法應(yīng)用于井下巡檢無人機(jī)平臺(tái)上，探究在實(shí)際應(yīng)用場(chǎng)景中的可行性.