基于激光雷達(dá)的室外實(shí)時(shí)閉環(huán)檢測(cè)算法*

陳學(xué)攀 任明武

（南京理工大學(xué) 南京 210094）

1 引言

在過去的幾十年里，隨著云計(jì)算、5G、人工智能的快速發(fā)展和普及［1］，自動(dòng)駕駛、移動(dòng)智能機(jī)器人等智能化自主系統(tǒng)逐步進(jìn)入大眾視野，不斷影響著人們的日常生活，這不僅包括常見的家用掃地機(jī)器人、倉儲(chǔ)智能移動(dòng)物流機(jī)器人，還包括目前突飛猛進(jìn)的無人駕駛汽車，甚至是目前出現(xiàn)的新型無人機(jī)送貨服務(wù)［2］。基于激光雷達(dá)的同時(shí)定位和建圖（Simultaneous Localization and Mapping，SLAM）技術(shù)在面對(duì)無GPS 信號(hào)或未知的環(huán)境中表現(xiàn)出了其獨(dú)具優(yōu)勢(shì)的一面［3］，包括無需預(yù)先布置場(chǎng)地、不存在尺度漂移、受環(huán)境光照變化影響小、可生成三維點(diǎn)云地圖用于導(dǎo)航等。狀態(tài)估計(jì)、定位和建圖作為實(shí)現(xiàn)反饋控制、避障和規(guī)劃等許多功能的必要條件，實(shí)現(xiàn)成功自主運(yùn)行的先決基礎(chǔ)條件［4］。

然而，在面對(duì)室外規(guī)模場(chǎng)景環(huán)境時(shí)，無論是機(jī)器人還是自主駕駛汽車其在無絕對(duì)信號(hào)輸入的前提下，長時(shí)間SLAM 算法運(yùn)行時(shí)不可避免地會(huì)累積誤差［5］，從而導(dǎo)致不正確的位姿估計(jì)。閉環(huán)檢測(cè)作為SLAM 算法中的一個(gè)重要模塊［6］，它是指機(jī)器人或移動(dòng)的車輛是否能夠識(shí)別當(dāng)前位置之前是否已經(jīng)過，是消除累積里程漂移誤差實(shí)現(xiàn)更準(zhǔn)確定位、構(gòu)建更精確的全局三維點(diǎn)云地圖的最有效途徑［7］。然而，在面對(duì)大規(guī)模室外環(huán)境下，關(guān)鍵幀的數(shù)目隨著SLAM 算法的長時(shí)間運(yùn)行其不斷增長，這會(huì)使得閉環(huán)檢測(cè)算法的計(jì)算時(shí)長以及會(huì)面對(duì)反向匹配、不同車道匹配、旋轉(zhuǎn)匹配等多種情況，這時(shí)提出一種高效準(zhǔn)確的閉環(huán)檢測(cè)算法以面對(duì)各種情況，通過多種形式的傳感器數(shù)據(jù)實(shí)現(xiàn)對(duì)此前已經(jīng)經(jīng)過的位置進(jìn)行識(shí)別用于更新當(dāng)前移動(dòng)平臺(tái)的估計(jì)位姿就顯得非常重要［8］。

本文通過改進(jìn)現(xiàn)有閉環(huán)檢測(cè)算法SCAN-CONTEXT［9］提出的全局描述符，提出了一種新型的基于位置和改進(jìn)的點(diǎn)云全局描述符相融合的閉環(huán)檢測(cè)算法，并設(shè)計(jì)了快速新型的適用于本算法的點(diǎn)云幀間相似度匹配計(jì)算公式，最后使用時(shí)間一致性和基于OpenMP 加速的FPFH［10］點(diǎn)云特征以提供初始初始位姿估計(jì)，隨后基于此初始估計(jì)位姿使用ICP［11］匹配來進(jìn)行幾何一致性校驗(yàn)以可能避免誤匹配。

2 相關(guān)工作

閉環(huán)檢測(cè)問題也即位置識(shí)別問題，其被定義為當(dāng)機(jī)器人在環(huán)境中移動(dòng)時(shí)，傳感器測(cè)量數(shù)據(jù)會(huì)隨著時(shí)間的增加而形成一組序列化數(shù)據(jù)，其中每個(gè)傳感器測(cè)量數(shù)據(jù)zt是指t時(shí)刻在某一空間位置lt獲取的［12］。所以，位置識(shí)別就可以定義為在由測(cè)量數(shù)據(jù)形成的數(shù)據(jù)庫地圖中為當(dāng)前位置查詢找到之前已經(jīng)訪問過的位置的測(cè)量數(shù)據(jù)，然后找到匹配關(guān)系借助傳感器數(shù)據(jù)進(jìn)行相對(duì)位姿變化輸出。位置識(shí)別算法主要由兩個(gè)模塊組成，第一個(gè)為了輕量化處理原始測(cè)量數(shù)據(jù)，原始測(cè)量zt進(jìn)行更緊湊的編碼，也即使用描述函數(shù)f(·) 的描述模塊。第二個(gè)是通過相似度函數(shù)(·) 來度量兩個(gè)原始測(cè)量描述符相似度的度量模塊。

SLAM 算法根據(jù)主要傳感器分為視覺SLAM 和激光SLAM 兩大類［13］。基于視覺的閉環(huán)檢測(cè)研究已經(jīng)進(jìn)行了很長一段時(shí)間，并提出了許多優(yōu)秀成功的方法。大多數(shù)基于圖像的方法提取特征描述符，然后用單詞袋（BoW）［14］，VLAD［15］和Fisher Vector（FV）［16］等方法進(jìn)行編碼。

與基于的視覺閉環(huán)檢測(cè)研究的蓬勃發(fā)展相反，基于激光雷達(dá)的研究在解決這一全局定位問題上仍缺乏強(qiáng)有力的解決方案。由于激光雷達(dá)數(shù)據(jù)對(duì)光照和季節(jié)變化更加穩(wěn)健，因此也逐步受到關(guān)注，大多數(shù)現(xiàn)有的激光雷達(dá)閉環(huán)檢測(cè)方法使用局部結(jié)構(gòu)和點(diǎn)分布等低層次特征，由于些特征對(duì)遮擋和旋轉(zhuǎn)變化敏感，因此不適用于大規(guī)模室外環(huán)境。Bosse 和Zlot［17］從局部三維描述符的數(shù)據(jù)庫中為每個(gè)關(guān)鍵幀位置查詢恒定數(shù)量的最近鄰進(jìn)行投票，并將具有足夠數(shù)量投票的地點(diǎn)確定為可能的匹配位置。然而，這種局部關(guān)鍵點(diǎn)特征通常缺乏分辨性描述的能力來區(qū)分相似的局部結(jié)構(gòu)，因此不夠可靠。Scan Context 方法保留在投影到二維極坐標(biāo)系下柵格的最大高度，為了實(shí)現(xiàn)旋轉(zhuǎn)不變性而逐行計(jì)算所有可能的列移動(dòng)距離，為得到兩幀點(diǎn)云描述符之間的最小距離，需要較長搜索時(shí)間并且保留柵格單點(diǎn)最大高度使得算法的穩(wěn)定性不夠。

3 本文算法描述

3.1 全局描述符編碼

由于每幀激光雷達(dá)均攜帶大量數(shù)據(jù)，本文使用的禾賽Pandar40 型號(hào)激光雷達(dá)其每幀數(shù)據(jù)點(diǎn)數(shù)超過七萬個(gè)，因此可利用旋轉(zhuǎn)激光雷達(dá)的特性將三維點(diǎn)云按旋轉(zhuǎn)方向和激光發(fā)射器射線方向進(jìn)行極坐標(biāo)系扇形柵格投影到二維平面，點(diǎn)云中某一點(diǎn)pi其對(duì)應(yīng)的投影點(diǎn)坐標(biāo)為

其中，ri為投影之后的行索引，si為投影后的列索引，Nr為行最大值，Ns為列最大值，di為pi距離激光雷達(dá)局部坐標(biāo)系原點(diǎn)的距離，θi為水平方位角，Tr、Ts為激光發(fā)射器射線方向和激光雷達(dá)旋轉(zhuǎn)方向的柵格分辨率。

原始Scan Context算法在生成每個(gè)柵格特征時(shí)使用投影到當(dāng)前柵格的所有原始點(diǎn)云點(diǎn)的高度也即z軸方向上的最大值，但是僅靠單點(diǎn)即代表本柵格的特征必然會(huì)存在一定的偶然性，其特征不夠穩(wěn)定。在分析以上問題后，本文提出基于投影到某柵格的點(diǎn)云點(diǎn)取三個(gè)高度的值的平均值，如果不足三個(gè)點(diǎn)在計(jì)算平均高度時(shí)僅計(jì)算有效點(diǎn)數(shù)取均值。這使得特征更加魯棒，降低噪點(diǎn)對(duì)整體算法的影響。

3.2 基于歐氏距離和范數(shù)的快速特征相似度匹配

在得到特征矩陣后，為實(shí)現(xiàn)滿足旋轉(zhuǎn)不變性的快速匹配，本算法采用和Scan Context 相同的基于行向量均值建立Kd-Tree 索引，快速選取候選關(guān)鍵幀。

在得到候選關(guān)鍵幀之后，為進(jìn)一步驗(yàn)證是否正確的進(jìn)行位置識(shí)別需要進(jìn)行當(dāng)前幀和候選幀之間的特征矩陣匹配，本算法改進(jìn)使用兩列向量范數(shù)之間的歐氏距離來改進(jìn)原有使用列向量余弦值進(jìn)行相似度計(jì)算的方式，實(shí)現(xiàn)特征矩陣相似度的快速計(jì)算，提高了效率，具體步驟為

1）將特征矩陣分解為列特征向量，得到兩特征矩陣對(duì)應(yīng)列的范數(shù)值p和q，按照式（3）計(jì)算其相似性。

2）將特征向量每次循環(huán)右移一個(gè)位置，再將得到的新的特征矩陣，直至移動(dòng)次數(shù)正好小于列數(shù)減一時(shí)截止，每次移動(dòng)均計(jì)算與當(dāng)前幀特征向量的相似度，選擇循環(huán)移動(dòng)時(shí)最大的相似度。

其中，qw為候選幀循環(huán)右移時(shí)得到的新的特征矩陣。

3）但是由于基于特征矩陣的相似度匹配可能在面臨場(chǎng)景存在動(dòng)態(tài)目標(biāo)或行駛車道等發(fā)生變化時(shí)，此相似度就會(huì)發(fā)生較大的改變，因此本算法加入兩幀之間位置的歐氏距離判斷來進(jìn)行彌補(bǔ)，改進(jìn)了相似度計(jì)算的方式以實(shí)現(xiàn)更魯棒的閉環(huán)檢測(cè)算法，如下所示：

其中，RS為基于特征矩陣匹配的閾值，RE為基于歐式距離回環(huán)匹配的閾值，最終相似度D(p，q) 分?jǐn)?shù)越低越好，α和β分別為兩得分系數(shù)。

3.3 一致性校驗(yàn)

使用時(shí)間一致性和基于OpenMP 加速的FPFH點(diǎn)云特征以提供初始初始位姿估計(jì)，隨后基于此初始估計(jì)位姿使用ICP 匹配來進(jìn)行幾何一致性校驗(yàn)以可能避免誤匹配。

4 數(shù)據(jù)和實(shí)驗(yàn)

本實(shí)驗(yàn)采用的是基于數(shù)據(jù)采集車在校內(nèi)采集的一段長達(dá)8km左右的室外數(shù)據(jù)，數(shù)據(jù)量大其滿足室外大規(guī)模場(chǎng)景，可有效驗(yàn)證本文提出的算法的有效性，在此數(shù)據(jù)中有較多的回環(huán)，不僅存在正向回環(huán)也存在逆向回環(huán)。

為有效驗(yàn)證本文提出閉環(huán)檢測(cè)算法的有效性，共進(jìn)行了三組實(shí)驗(yàn)，第一組為閉環(huán)檢測(cè)為簡單的基于歐式距離的形式，第二組實(shí)驗(yàn)閉環(huán)檢測(cè)算法為原始Scan Context 算法，第三組實(shí)驗(yàn)為本文提出的算法，其最終位姿預(yù)測(cè)的絕對(duì)誤差結(jié)果如表1所示。

表1 實(shí)驗(yàn)結(jié)果

圖1 數(shù)據(jù)采集車

圖2 數(shù)據(jù)采集路線

三組實(shí)驗(yàn)的結(jié)果如圖3 所示，可以基于本文的回環(huán)檢測(cè)算法的SLAM 系統(tǒng)取得了最后的位姿估計(jì)結(jié)果，在滿足實(shí)時(shí)性的前提下，可基于本算法有效的對(duì)里程累積誤差進(jìn)行消除，提升整個(gè)SLAM 算法的位姿估計(jì)的準(zhǔn)確性和建圖的精確性。

圖3 回環(huán)檢測(cè)算法結(jié)果對(duì)比

特別是，原始Scan Context 算法在本實(shí)驗(yàn)?zāi)嫦蛟俅私?jīng)過圖4 位置時(shí)不能正確的進(jìn)行回環(huán)匹配，而由于本算法加入了歐氏距離的匹配因素，因此即便在基于特征矩陣不能很好地進(jìn)行匹配時(shí)，本文提出的算法仍可以有效地校驗(yàn)出回環(huán)，以完成位置識(shí)別，如圖5所示。

圖4 基于Scan Context未檢測(cè)到反向回環(huán)

圖5 基于本文算法成功反向回環(huán)匹配

5 結(jié)語

本文提出了一種可適用于室外大規(guī)模場(chǎng)景的基于激光雷達(dá)數(shù)據(jù)的閉環(huán)檢測(cè)算法，點(diǎn)云數(shù)據(jù)作為一種非結(jié)構(gòu)化的數(shù)據(jù)，沒有恒定的點(diǎn)云密度且數(shù)據(jù)稀疏性隨著距離的增加而增加，環(huán)境目標(biāo)對(duì)象的密度也即分辨率隨傳感器也發(fā)生較大改變。本文在分析以上問題后，提出了一種基于歐式距離和全局特征描述符相融合的閉環(huán)檢測(cè)算法，并通過實(shí)驗(yàn)驗(yàn)證了其相比于當(dāng)前流行的閉環(huán)檢測(cè)算法存在的優(yōu)勢(shì)，在之后的研究中，由于室外場(chǎng)景車輛行駛的車道會(huì)經(jīng)常發(fā)生變化，可著重對(duì)此問題進(jìn)行解決。