基于全局特征描述子的激光SLAM回環檢測方法

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

(1. 上海交通大學 機械與動力工程學院，上海 200240； 2. 中煤科工集團上海有限公司，上海 201100)

同步定位和建圖(SLAM)是實現機器人在未知環境下的定位和移動的重要技術方法[1].定位精度是井下巡檢的核心指標，高精度的定位算法是巡檢過程中導航和避障的基礎.但井下環境復雜，具有低照度、弱紋理、圖像特征難以識別的特點[2]，給基于視覺的SLAM算法帶來了極大的困難.而激光SLAM算法測量距離遠、精度高，利用環境的結構特征進行定位[3]，在井下環境中更具應用前景[4-5].

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

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

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

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

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

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

1 基于全局特征描述子的回環檢測

設計的定位系統整體分為2個模塊：里程計模塊和回環檢測模塊.

里程計模塊參考LOAM算法[13]實現, 輸出里程計位姿信息，并根據點云三維曲率完成了特征點云的篩選，將邊角特征點云Pedg和平面特征點云Ppla傳遞給回環檢測模塊.

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

1.1 特征向量提取

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

步驟1計算鄰近k點的質心：

(1)

步驟2計算協方差矩陣C，即質心oc到鄰近點云中所有點pi的向量與其轉置的內積：

(2)

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

(3)

且λ1<λ2<λ3

(4)

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

步驟4由|λjE-C|n=0解得對應的特征向量n1,n2,n3.

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

1.2 特征描述和相似度計算

設計全局特征描述子的計算方法.通過計算全局點云P中所有點的三維坐標平均值得到當前幀的點云質心oc，以點云Pedg和Ppla中所有點對應的特征向量的均值作為質心oc處對應的特征向量nc，進而計算點云的全局特征描述子，邊角點云Pedg和平面點云Ppla分開計算.

遍歷點云P中的所有點，確定以質心oc為原點o的參考坐標系uvw：

u=nc

(5)

(6)

w=u×v

(7)

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

圖2 全局點云下的uvw坐標系Fig.2 uvw coordinate system of global point cloud

以上述計算得到的uvw坐標系為基準，計算點云特征向量相對于基準坐標系3個坐標軸的角度分布，則有：

α=v·nt

(8)

(9)

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

(10)

d=‖pt-oc‖2

(11)

式中：d為點pt,oc之間的歐氏距離；α為特征向量nt與坐標軸v之間的夾角，值域為[-1, 1]；φ為pt,oc之間連線方向與坐標軸u之間的夾角，值域為[-1, 1]；θ為特征向量nt投影至wou平面時與坐標軸w之間的夾角，值域為[-π/2, π/2].

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

遍歷點云Pedg和Ppla中所有點，與質心點oc構成點對，計算對應的α,φ,θ,d四要素.將α,φ,θ的值域區間進行等分，統計全局點云三要素在各區間內的分布情況，構成特征描述子的角度分量.

計算全局點云P中距離質心點oc最遠的位置，將最大距離均分為多個區間，統計各個點相對于質心距離在區間中的分布，以此構成特征描述子的尺度分量.

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

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

圖3 旋轉和平移不變性實驗Fig.3 Experiments of rotation and translation invariance

使用點云全局特征描述子的皮爾遜相關系數[15]作為衡量點云相似度的標準，其計算公式如下：

(12)

式中：H1=[h11h12…h1i…]，H2=[h21h22…h2i…]為歸一化后的點云全局特征描述子；μH1和μH2分別為H1和H2元素的平均值向量；E為數學期望;σ為方差；SH1, H2為相關系數，其取值范圍為[0, 1].當SH1, H2接近于1時，說明特征描述子為正相關關系，點云的相似度越高[16].

1.3 位姿變換

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

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

(13)

對于配準后的相似幀點云，構建需要求解的目標函數，使用Levenberg-Marquardt法迭代求解使目標函數最小的位姿變換，其目標函數如下：

F=φe+φp=

(14)

(15)

(16)

圖4 邊角特征和平面特征點云配準Fig.4 Registration of edge features and plane features

1.4 因子圖優化

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

圖5 因子圖優化模型Fig.5 Model of factor graph optimization

對于1次測量事件，t1和t2時刻位姿測量值分別為st1,st2，對應的路標點集合為mt2測量事件構成的約束函數如下：

J′=

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

(17)

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

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

(18)

因子圖優化的方法將圖映射為信息矩陣的表達方式進行求解，利用圖的稀疏性特點，在求解過程中保持較低的計算量和錯誤的數據關聯，優于一般的濾波方法[17].

2 實驗結果與討論

2.1 準確率和召回率實驗

準確率和召回率是衡量回環檢測模塊中相似度檢測算法性能的重要指標，在回環檢測算法設計中需首先保證較高的準確率，其次在滿足高準確率的同時盡可能提高相似度檢測模塊的召回率.

準確率和召回率的計算方法如下：

(19)

(20)

式中：η為準確率；ε為召回率；TP為正確識別出同一場景的數量；FP為將不同場景錯誤識別為同一場景的數量；FN為同一場景被錯誤識別為不同場景的數量.

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

由圖6可知，在保證準確率為100%的條件下，召回率最高可以達到50.76%，超過一半的相似場景會被召回，此時閾值ω為0.95，隨著閾值ω進一步降低，召回率增大，準確率降低，回環檢測可能出現錯誤匹配，得到錯誤的位姿信息，對定位和建圖產生嚴重的影響、因此應保證高準確率，在后續算法實驗中均將閾值ω設置為0.95，此時準確率為100%.

圖6 準確率和召回率曲線Fig.6 Curves of precision and recall rates

2.2 定位精度實驗

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

圖7 井下仿真實驗環境Fig.7 Environment of underground simulation experiments

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

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

圖8 實驗場景點云地圖Fig.8 Point cloud maps of experiments

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

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

表1 定位精度實驗結果Tab.1 Results of localization accuracy experiments

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

圖9 Sim_mine_1定位實驗軌跡Fig.9 Trajectory of localization experiment Sim_mine_1

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

表2 算法各模塊運行時間Tab.2 Running time of each module of proposed algorithm

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

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

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

圖10 因子圖優化前后軌跡Fig.10 Localization trajectory before and after graph optimization

3 結語

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

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