適用于激光SLAM的點(diǎn)云全局特征描述子回環(huán)檢測方案

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

同步定位和建圖(SLAM)是實(shí)現(xiàn)機(jī)器人在未知環(huán)境下的定位和移動的重要技術(shù)方法[1]。定位精度是井下巡檢的核心指標(biāo)，高精度的定位算法是巡檢過程中導(dǎo)航和避障的基礎(chǔ)。但井下環(huán)境復(fù)雜，具有低照度、弱紋理、圖像特征難以識別的特點(diǎn)[2]，給基于視覺的SLAM算法帶來了極大的困難。而激光SLAM算法測量距離遠(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)檢測方法，利用預(yù)先訓(xùn)練的視覺特征單詞構(gòu)建詞典，將圖像中的特征描述為一個(gè)單詞，圖像中單詞種類和數(shù)量構(gòu)成圖像的詞袋向量，通過詞袋向量之間的相似度進(jìn)行回環(huán)檢測。

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

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

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

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

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

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

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

圖1 基于全局特征描述子的回環(huán)檢測算法流程

1.1 特征向量提取

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

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

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

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

式中：為矩陣對應(yīng)的特征值；為矩陣對應(yīng)的特征向量；為對角線組成的對角矩陣；為奇異值分解后的實(shí)正交矩陣。

步驟4 由解得對應(yīng)的特征向量。

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

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

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

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

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

圖2 全局點(diǎn)云下的uvw坐標(biāo)系

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

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

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

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

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

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

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

圖3 旋轉(zhuǎn)和平移不變性實(shí)驗(yàn)

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

式中：，

為歸一化后的點(diǎn)云全局特征描述子；和分別為和元素的平均值向量；為數(shù)學(xué)期望；為方差；為相關(guān)系數(shù)，其取值范圍為[0， 1]。當(dāng)接近于1時(shí)，說明特征描述子為正相關(guān)關(guān)系，點(diǎn)云的相似度越高[16]。

1.3 位姿變換

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

為減少點(diǎn)云的誤匹配概率，使用曲率估計(jì)方法對點(diǎn)云配準(zhǔn)效果進(jìn)行驗(yàn)證。當(dāng)前幀邊角點(diǎn)和平面點(diǎn)分別在相似幀邊角特征和平面特征中搜索匹配點(diǎn)，對相似幀的特征進(jìn)行主成分分析，計(jì)算其特征值。

其中，當(dāng)點(diǎn)云為邊角特征時(shí)，掃描點(diǎn)按照線方向分布；為平面特征時(shí)，掃描點(diǎn)呈現(xiàn)平面式分布，在垂直平面方向上的方差最小。邊角特征和平面特征應(yīng)分別滿足如下關(guān)系式：

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

式中：上標(biāo)L和W分別為局部和世界坐標(biāo)系；上劃線-為測量值，~為估計(jì)值；為當(dāng)前邊角特征點(diǎn)云中一點(diǎn)在激光雷達(dá)局部坐標(biāo)系下的坐標(biāo)；為當(dāng)前平面特征點(diǎn)云中一點(diǎn)在激光雷達(dá)局部坐標(biāo)系下的坐標(biāo)；，分別為相似幀邊角特征點(diǎn)云在世界坐標(biāo)系下距離最近的兩點(diǎn)坐標(biāo)；，，分別為相似幀平面特征在世界坐標(biāo)系下距離最近的三點(diǎn)坐標(biāo)；，分別為邊角和平面特征的目標(biāo)函數(shù)，其含義為配準(zhǔn)點(diǎn)到線或平面的距離；和為待求解變量，分別為當(dāng)前坐標(biāo)系到世界坐標(biāo)系下的旋轉(zhuǎn)四元數(shù)估計(jì)值和平移向量估計(jì)值。

邊角特征和平面特征的配準(zhǔn)如圖4所示，其中：黑色點(diǎn)云為當(dāng)前幀；綠色點(diǎn)云為歷史相似幀；黑色加粗點(diǎn)為當(dāng)前幀邊角特征點(diǎn)坐標(biāo)和平面特征點(diǎn)坐標(biāo)；綠色加粗點(diǎn)分別為相似幀的邊角特征點(diǎn)，構(gòu)成直線，以及平面特征點(diǎn)，，構(gòu)成平面。

圖4 邊角特征和平面特征點(diǎn)云配準(zhǔn)

1.4 因子圖優(yōu)化

通過位姿變換方法可以獲得當(dāng)前關(guān)鍵幀與歷史相似幀之間的相對位姿變換，以此建立新的回環(huán)約束，構(gòu)建因子圖如圖5所示。其中：為測量函數(shù)；為測量噪聲協(xié)方差矩陣；因子圖由一系列不同時(shí)刻位姿測量值和地圖路標(biāo)點(diǎn)

的觀測量構(gòu)成。每條實(shí)線為1次前后幀之間的位姿累積測量事件，每條虛線為1次檢測到的回環(huán)約束測量事件，將因子圖表達(dá)轉(zhuǎn)換為線性化的約束的線性矩陣的形式，每一個(gè)新的測量值都會致使信息矩陣發(fā)生更新。

圖5 因子圖優(yōu)化模型

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

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

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

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

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

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

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

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

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

圖6 準(zhǔn)確率和召回率曲線

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

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

本文進(jìn)行了多組仿真和開源數(shù)據(jù)集下的定位精度實(shí)驗(yàn)。實(shí)驗(yàn)平臺CPU為AMD 3600，內(nèi)存16 GB，算法采用C++語言編寫，均在Ubuntu 18.0的機(jī)器人操作系統(tǒng)(ROS)進(jìn)行測試。仿真實(shí)驗(yàn)中，使用文獻(xiàn)[19]提供的開源數(shù)據(jù)進(jìn)行測試，該數(shù)據(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)境

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

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

圖8 實(shí)驗(yàn)場景點(diǎn)云地圖

算法定位精度實(shí)驗(yàn)中，進(jìn)行初始坐標(biāo)系對齊，以均方根誤差，全局最大誤差值以及終點(diǎn)處的絕對誤差作為評價(jià)定位精度的指標(biāo)，實(shí)驗(yàn)結(jié)果如表1所示，其中：序號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。

表1 定位精度實(shí)驗(yàn)結(jié)果

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

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

圖9 Sim_mine_1定位實(shí)驗(yàn)軌跡

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

表2 算法各模塊運(yùn)行時(shí)間

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

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

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

圖10 因子圖優(yōu)化前后軌跡

03 結(jié)語

針對井下巡檢時(shí)定位誤差隨時(shí)間不斷累積的問題，提出一種適用于激光SLAM的基于點(diǎn)云全局特征描述子的回環(huán)檢測方法。該方法具有以下優(yōu)勢：

①充分利用了井下巷道的結(jié)構(gòu)特征，相似度檢測不受位姿信息約束，通過曲率分別提取通道四周墻壁邊角處和平面處的特征向量，利用特征向量和點(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)檢測的LOAM算法和使用距離信息進(jìn)行回環(huán)檢測的LOAM+ICP算法得到較大提升，在長時(shí)間長距離的實(shí)驗(yàn)中提升效果顯著。實(shí)驗(yàn)結(jié)果對于井下執(zhí)行長時(shí)間的巡檢任務(wù)具有指導(dǎo)意義，未來工作中考慮進(jìn)一步研究，將算法應(yīng)用于井下巡檢無人機(jī)平臺上，探究在實(shí)際應(yīng)用場景中的可行性。

編輯：黃飛