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

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

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

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

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

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

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

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

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

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

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

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

1.1 特征向量提取

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

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

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

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

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

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

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

1.2 特征描述和相似度計算

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

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

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

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

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

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

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

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

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

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

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

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

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

式中：，

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

1.3 位姿變換

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

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

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

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

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

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

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

1.4 因子圖優(yōu)化

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

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

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

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

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

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

02 實驗結(jié)果與討論

2.1 準(zhǔn)確率和召回率實驗

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

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

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

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

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

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

2.2 定位精度實驗

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

圖7 井下仿真實驗環(huán)境

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

實驗的4個場景分別使用軌跡真實值進(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 實驗場景點云地圖

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

表1 定位精度實驗結(jié)果

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

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

圖9 Sim_mine_1定位實驗軌跡

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

表2 算法各模塊運行時間

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

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

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

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

03 結(jié)語

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

①充分利用了井下巷道的結(jié)構(gòu)特征，相似度檢測不受位姿信息約束，通過曲率分別提取通道四周墻壁邊角處和平面處的特征向量，利用特征向量和點云中心點之間的角度關(guān)系和點云通道和中心點之間的距離關(guān)系構(gòu)建了全局特征描述子；

②提高了計算速度，基于邊角點和平面點配準(zhǔn)策略，進(jìn)行高效率的位姿變換算法研究；

③全局一致性優(yōu)，采用因子圖優(yōu)化獲得了使全局誤差最小的位姿，校正了累積誤差。

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

編輯：黃飛