潘 鋒，蔣 林，2，胡雨欣，顏俊杰，張文俊

(1.武漢科技大學(xué)冶金裝備及其控制教育部重點(diǎn)實(shí)驗(yàn)室，湖北 武漢，430081；2.武漢科技大學(xué)機(jī)器人與智能系統(tǒng)研究院，湖北 武漢，430081；3.立得空間信息技術(shù)股份有限公司，湖北 武漢，430073)

回環(huán)檢測(cè)算法用于判斷機(jī)器人是否回到了曾經(jīng)到過(guò)的位置，進(jìn)而得到約束以消除機(jī)器人在即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping, SLAM)過(guò)程中產(chǎn)生的不可避免的累計(jì)誤差[1-2]。根據(jù)所使用的傳感器類(lèi)型的不同，該算法可分為基于視覺(jué)的回環(huán)檢測(cè)以及基于激光雷達(dá)的回環(huán)檢測(cè)[3]。

基于視覺(jué)的回環(huán)檢測(cè)算法通常利用詞袋模型(BoW)[4]，例如，Glvez-López等[5]采用FAST+BRIEF特征提取方法構(gòu)建詞袋，在多個(gè)數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)，得到?jīng)]有假陽(yáng)性的回環(huán)檢測(cè)結(jié)果；Cummins等[6]利用概率方法改進(jìn)詞袋模型并降低了計(jì)算復(fù)雜度；董蕊芳等[7]利用圖像線(xiàn)特征提取二進(jìn)制描述子進(jìn)行視覺(jué)詞典的構(gòu)建，所得到的詞袋中視覺(jué)單詞評(píng)分間的區(qū)分度更高，從而提高了回環(huán)檢測(cè)準(zhǔn)確率，然而，該類(lèi)方法容易受到環(huán)境如光照、天氣和季節(jié)等變化的影響[8]。

激光雷達(dá)對(duì)環(huán)境中光照等條件的變化有較強(qiáng)的抗干擾能力，因此其在回環(huán)檢測(cè)中得到更多的應(yīng)用[9]，常用的有基于局部描述符和基于全局描述符的回環(huán)檢測(cè)算法。Rusu等[10]設(shè)置了一個(gè)16維的特征來(lái)描述點(diǎn)云的局部幾何形狀，通過(guò)特征計(jì)算對(duì)點(diǎn)云進(jìn)行匹配以實(shí)現(xiàn)回環(huán)檢測(cè)。Bosse等[11]構(gòu)建了3D Gestalt局部描述符，采用關(guān)鍵點(diǎn)投票的方法構(gòu)建分布模型來(lái)實(shí)現(xiàn)回環(huán)檢測(cè)。Salti等[12]首先找到點(diǎn)云中的關(guān)鍵點(diǎn)，然后對(duì)關(guān)鍵點(diǎn)周?chē)狞c(diǎn)進(jìn)行劃分，將劃分結(jié)果編碼為直方圖進(jìn)行特征匹配。這幾種經(jīng)典的基于局部描述符的回環(huán)檢測(cè)算法需要對(duì)點(diǎn)云中的特征進(jìn)行詳細(xì)描述，因此需要提取點(diǎn)云中的關(guān)鍵點(diǎn)和局部幾何信息進(jìn)行大量計(jì)算，算法準(zhǔn)確率較高但運(yùn)行效率較低，故而實(shí)時(shí)性較差。Muhammad等[13]、He等[14]則將三維點(diǎn)云投影到二維平面來(lái)生成點(diǎn)云的全局描述符，其方法雖然提高了回環(huán)匹配的計(jì)算速度但容易受噪聲影響，不能保證較高的準(zhǔn)確率。Kim等[15]提出了更加有效的全局描述符Scan Context，將點(diǎn)云的高度值編碼為矩陣，保證了較快的計(jì)算速度，但在準(zhǔn)確性方面仍有所欠缺，容易出現(xiàn)假陽(yáng)性的情況。Wang等[16]隨后提出了全局描述符ISC，利用強(qiáng)度信息將點(diǎn)云編碼為二值化矩陣，雖然這能提高匹配速度，但二值化過(guò)程損失了部分點(diǎn)云信息，回環(huán)檢測(cè)的準(zhǔn)確率仍然較低。上述基于全局描述符的方法都僅僅使用單一的點(diǎn)云信息，檢測(cè)準(zhǔn)確率有待提高。另外，這些方法為了解決機(jī)器人在相同位置但不同朝向的情況下的回環(huán)檢測(cè)問(wèn)題(即旋轉(zhuǎn)不變性問(wèn)題)，都要對(duì)與當(dāng)前點(diǎn)云匹配的候選點(diǎn)云進(jìn)行不斷的旋轉(zhuǎn)，重復(fù)計(jì)算相似度，使得計(jì)算量大為增加。

針對(duì)上述問(wèn)題，本文提出一種同時(shí)利用點(diǎn)云幾何信息和強(qiáng)度信息的回環(huán)檢測(cè)算法。該算法使用兩種方式分別將一幀點(diǎn)云的高度信息與強(qiáng)度信息編碼為兩個(gè)矩陣，構(gòu)成能更加準(zhǔn)確描述點(diǎn)云的全局描述符。同時(shí)，該算法利用SLAM前端得到的每一幀點(diǎn)云位姿，將點(diǎn)云的姿態(tài)變換到世界坐標(biāo)系下，再對(duì)變換后的點(diǎn)云進(jìn)行編碼，這樣可以解決旋轉(zhuǎn)不變性問(wèn)題，從而減少計(jì)算量。

1 基于點(diǎn)云強(qiáng)度與高度信息的全局描述符

通過(guò)激光雷達(dá)不僅可以得到周?chē)h(huán)境的幾何信息，同時(shí)還可以獲取每一束激光返回的強(qiáng)度值。一般的回環(huán)檢測(cè)算法僅僅利用點(diǎn)云中的幾何信息，但點(diǎn)云的強(qiáng)度信息同樣能體現(xiàn)其特征[17-18]。

本文算法中的全局描述符由兩層構(gòu)成，分別利用兩種編碼方式對(duì)點(diǎn)云的高度和強(qiáng)度信息進(jìn)行編碼。兩種編碼方式可以理解為從兩個(gè)不同的視角來(lái)描述同一幀點(diǎn)云，這樣得到的全局描述符能對(duì)一幀點(diǎn)云進(jìn)行更全面和精確的表示，從而提高點(diǎn)云間的匹配精度，進(jìn)而實(shí)現(xiàn)更高的回環(huán)檢測(cè)精度。如圖1(a)所示為一幀原始的三維點(diǎn)云，圖1(b)為本文算法生成的全局描述符，由上下兩層矩陣構(gòu)成。

(a)原始三維激光點(diǎn)云

1.1 點(diǎn)云高度信息編碼

全局描述符的第一層是將點(diǎn)云投影到二維平面進(jìn)行編碼得到對(duì)應(yīng)的矩陣，其具體的編碼方式如下:首先，由于雷達(dá)的測(cè)量精度隨距離的增加會(huì)逐漸降低，所以要對(duì)輸入的點(diǎn)云進(jìn)行預(yù)處理，即定義一個(gè)距離閾值Dmax，選取與點(diǎn)云中心距離小于Dmax的點(diǎn)，得到新點(diǎn)云P，這樣保留下來(lái)的點(diǎn)云數(shù)據(jù)更加可靠；然后，對(duì)點(diǎn)云P進(jìn)一步劃分，先根據(jù)半徑將點(diǎn)云均勻地劃分為若干扇形區(qū)域，扇形數(shù)量為Nc，然后用間距相等的同心圓將所有扇形劃分為若干區(qū)域，設(shè)同心圓間距為Dr，則每個(gè)扇形被劃分為Nr=Dmax/Dr個(gè)區(qū)域，劃分結(jié)果如圖2所示。

圖2 點(diǎn)云的劃分

點(diǎn)云一共被劃分為Nr×Nc個(gè)區(qū)域，用Emn(m=1,2,…,Nr；n=1,2,…,Nc)來(lái)表示，Emn與大小為Nr×Nc的矩陣元素一一對(duì)應(yīng)。若Emn區(qū)域內(nèi)沒(méi)有點(diǎn)，則對(duì)應(yīng)矩陣元素取值為0，否則取每個(gè)區(qū)域中最高點(diǎn)的高度值為矩陣中對(duì)應(yīng)元素的值，得到高度矩陣MH，用于表示一幀點(diǎn)云，其構(gòu)成本文回環(huán)檢測(cè)算法全局描述符的第一層。

1.2 點(diǎn)云強(qiáng)度信息編碼

點(diǎn)云全局描述符的第二層利用點(diǎn)云強(qiáng)度信息構(gòu)建，采用文獻(xiàn)[19]中的編碼方式。

將點(diǎn)云P中的點(diǎn)對(duì)應(yīng)到大小為h×w的矩陣MI中。點(diǎn)云中每個(gè)點(diǎn)pn(x,y,z)和MI中元素的映射關(guān)系可以表示為：

(1)

式中：r為點(diǎn)到點(diǎn)云中心的距離；f為激光雷達(dá)垂直方向上的視角，例如三維激光雷達(dá)VLP-16的f=30°；v和u分別代表點(diǎn)所對(duì)應(yīng)元素在矩陣中的行與列。

在點(diǎn)云中所有點(diǎn)對(duì)應(yīng)到矩陣之后，將點(diǎn)的強(qiáng)度值賦予矩陣對(duì)應(yīng)元素。例如，將KITTI數(shù)據(jù)集[20]中的一幀點(diǎn)云按上述方式映射得到的矩陣如圖1(b)中第二層所示，因?yàn)椴杉疜ITTI數(shù)據(jù)集使用的是64線(xiàn)三維激光雷達(dá)，所以對(duì)應(yīng)矩陣的行數(shù)和列數(shù)分別取h=64、w=900。

1.3 點(diǎn)云間相似度計(jì)算

(2)

第二層矩陣之間的相似度S2可以通過(guò)相同方式計(jì)算得到。設(shè)置一個(gè)相似度的閾值τ，若S1+S2≥τ，則認(rèn)為這兩幀點(diǎn)云構(gòu)成回環(huán)。

但是，如果機(jī)器人從不同的方向經(jīng)過(guò)同一位置，此時(shí)得到的兩層矩陣的列都會(huì)發(fā)生平移，如圖3所示，其中兩層矩陣的列發(fā)生平移的方向不一致是由不同的編碼方式所導(dǎo)致。在這種情況下，直接計(jì)算矩陣之間的相似度并不能正確評(píng)價(jià)兩幀點(diǎn)云的相似關(guān)系，即存在旋轉(zhuǎn)不變性問(wèn)題。為了解決這個(gè)問(wèn)題，現(xiàn)有方法都是在計(jì)算相似度時(shí)讓候選點(diǎn)云的矩陣保持不變，將當(dāng)前點(diǎn)云矩陣所有的列向左平移一個(gè)單位，同時(shí)矩陣最左端的列移至矩陣最右端，從而得到一個(gè)新的矩陣，再重新計(jì)算新矩陣和候選點(diǎn)云矩陣之間的相似度,因此總共需要平移和計(jì)算的次數(shù)等于矩陣的列數(shù)，然后取其中最大的相似度作為最終結(jié)果。

(a)第一層描述符

對(duì)矩陣的列平移相當(dāng)于對(duì)點(diǎn)云進(jìn)行旋轉(zhuǎn)，如果兩幀點(diǎn)云取自相同位置的不同方向，那么上述的最大相似度即為兩幀點(diǎn)云旋轉(zhuǎn)后對(duì)齊時(shí)的相似度，可以正確反映兩幀點(diǎn)云的相似關(guān)系。但歷史點(diǎn)云的數(shù)量會(huì)隨機(jī)器人的運(yùn)動(dòng)不斷累積，而激光雷達(dá)每得到新的一幀點(diǎn)云，就需要和所有的歷史點(diǎn)云進(jìn)行匹配，計(jì)算量會(huì)快速增加。因此本文算法在生成描述符之前，先利用SLAM過(guò)程中估計(jì)的當(dāng)前幀點(diǎn)云姿態(tài)R對(duì)每幀點(diǎn)云進(jìn)行一個(gè)旋轉(zhuǎn)變換。如圖4所示，當(dāng)前點(diǎn)云經(jīng)過(guò)變換后生成的描述符圖4(b)和候選點(diǎn)云描述符圖4 (c)的偏差較小，因此只需將當(dāng)前點(diǎn)云矩陣進(jìn)行左右平移，每平移一個(gè)單位計(jì)算一次相似度，且最多向左或向右平移3至5個(gè)單位，取其中最大相似度即可正確表示兩幀點(diǎn)云之間的相似關(guān)系，這樣可以明顯減少相似度的計(jì)算。

(a)當(dāng)前點(diǎn)云全局描述符

2 回環(huán)檢測(cè)算法

本文算法的整體流程如圖5所示。首先通過(guò)旋轉(zhuǎn)矩陣將當(dāng)前幀點(diǎn)云姿態(tài)變換為世界坐標(biāo)系下的姿態(tài)，再將點(diǎn)云編碼為全局描述符，得到兩個(gè)矩陣，進(jìn)一步將矩陣簡(jiǎn)化為向量，利用KD樹(shù)結(jié)構(gòu)快速搜索2N幀可能發(fā)生回環(huán)的候選點(diǎn)云，再和當(dāng)前點(diǎn)云進(jìn)行精度更高的相似度計(jì)算，確定回環(huán)。

圖5 本文回環(huán)檢測(cè)算法流程

為了避免在所有歷史點(diǎn)云中搜索與當(dāng)前點(diǎn)云可能會(huì)發(fā)生回環(huán)的點(diǎn)云導(dǎo)致計(jì)算量隨時(shí)間逐漸增大，進(jìn)而影響算法的實(shí)時(shí)性，本文算法首先將全局描述符的矩陣轉(zhuǎn)換為列向量，取矩陣每行所有元素的平均值作為列向量對(duì)應(yīng)行的值。由全局描述符的兩層矩陣M1、MH分別得到對(duì)應(yīng)的列向量VI、VH:

(3)

(4)

所有歷史點(diǎn)云都對(duì)應(yīng)兩個(gè)列向量，故可以通過(guò)兩個(gè)KD樹(shù)來(lái)保存所有歷史點(diǎn)云的列向量，利用最鄰近算法在KD樹(shù)中進(jìn)行高效搜索：①通過(guò)當(dāng)前點(diǎn)云的兩個(gè)列向量，分別在歷史點(diǎn)云向量構(gòu)成的兩個(gè)KD樹(shù)中找到與當(dāng)前點(diǎn)云可能發(fā)生回環(huán)的N幀歷史點(diǎn)云，這樣一共能得到2N幀候選點(diǎn)云；②計(jì)算當(dāng)前點(diǎn)云和所有候選點(diǎn)云的相似度，選出其中最大的相似度，通過(guò)閾值判斷當(dāng)前點(diǎn)云是否和歷史某幀點(diǎn)云發(fā)生回環(huán)。通過(guò)這樣的兩步搜索策略，在保證計(jì)算速度的同時(shí)也能實(shí)現(xiàn)較高的匹配精度。

3 實(shí)驗(yàn)與結(jié)果分析

3.1 KITTI數(shù)據(jù)集實(shí)驗(yàn)測(cè)試

為了檢測(cè)本文回環(huán)檢測(cè)算法的性能，首先采用KITTI數(shù)據(jù)集的多個(gè)數(shù)據(jù)序列進(jìn)行實(shí)驗(yàn)，在不同閾值τ下得到算法的多組準(zhǔn)確率(precision)與召回率(recall)統(tǒng)計(jì)結(jié)果，繪制準(zhǔn)確率-召回率曲線(xiàn)，并與文獻(xiàn)[15]采用全局描述符Scan Context的算法進(jìn)行對(duì)比，如圖6所示。表1所示為兩個(gè)算法在保證準(zhǔn)確率為100%情況下的最大召回率。

從圖6(a)、圖6(b)可以看出，對(duì)于KITTI數(shù)據(jù)集的00、02序列來(lái)說(shuō)，本文算法和文獻(xiàn)[15]算法的結(jié)果接近，主要原因可能是由于數(shù)據(jù)集的場(chǎng)景所限，本文編碼方式產(chǎn)生了退化，所以和Scan Context全局描述符相比優(yōu)勢(shì)不大。結(jié)合圖6(c)、圖6(d)和表1可見(jiàn)，本文算法在KITTI數(shù)據(jù)集的05、08序列上的表現(xiàn)明顯優(yōu)于對(duì)比算法，在保證回環(huán)檢測(cè)準(zhǔn)確率為100%的同時(shí)提高了召回率。這主要是因?yàn)楸疚乃惴ɡ脙煞N編碼方式對(duì)點(diǎn)云的兩類(lèi)信息進(jìn)行編碼，得到的全局描述符整體上能更加準(zhǔn)確地描述點(diǎn)云，而且利用全局描述符的兩個(gè)矩陣分別進(jìn)行匹配，綜合兩個(gè)匹配結(jié)果實(shí)現(xiàn)互補(bǔ)，提高了回環(huán)檢測(cè)的整體精度。

(a) KITTI 00序列

表1 兩個(gè)算法在準(zhǔn)確率為100%時(shí)的最大召回率

兩個(gè)算法對(duì)每幀點(diǎn)云的計(jì)算時(shí)間如圖7所示，平均計(jì)算時(shí)間見(jiàn)表2，可以看到，本文算法比文獻(xiàn)[15]算法的計(jì)算效率提高了約40%。

(a) KITTI 00序列

表2 兩個(gè)算法對(duì)點(diǎn)云的平均計(jì)算時(shí)間(單位：ms)

3.2 真實(shí)環(huán)境實(shí)驗(yàn)測(cè)試

下面在室外真實(shí)環(huán)境中運(yùn)行本文回環(huán)檢測(cè)算法。機(jī)器人實(shí)驗(yàn)平臺(tái)如圖8所示,機(jī)器人底盤(pán)上搭載VLP-16多線(xiàn)激光雷達(dá)、IMU和Intel NUC8 i7 HVK計(jì)算單元。在室外選擇長(zhǎng)約430 m、寬約120 m的矩形區(qū)域進(jìn)行實(shí)驗(yàn)，圖9為該區(qū)域整體點(diǎn)云地圖，其中藍(lán)線(xiàn)代表機(jī)器人的運(yùn)動(dòng)軌跡，而圓圈內(nèi)是機(jī)器人發(fā)生回環(huán)的區(qū)域。

圖8 室外移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)

圖9 室外實(shí)驗(yàn)環(huán)境的點(diǎn)云地圖

首先，機(jī)器人在SLAM過(guò)程中使用常規(guī)基于距離的回環(huán)檢測(cè)算法，在發(fā)生回環(huán)的區(qū)域內(nèi)最終得到的點(diǎn)云地圖和軌跡如圖10所示。從圖10(b)可以看出，機(jī)器人經(jīng)過(guò)長(zhǎng)距離地圖構(gòu)建后產(chǎn)生較大的累計(jì)誤差，在Z軸方向出現(xiàn)較大偏移，無(wú)法成功進(jìn)行回環(huán)檢測(cè)，最終導(dǎo)致回環(huán)區(qū)域的點(diǎn)云不能正確匹配而產(chǎn)生重影，如圖10(a)所示，兩個(gè)黃色方框部分實(shí)為同一個(gè)區(qū)域。

(a) 三維點(diǎn)云地圖

將回環(huán)檢測(cè)部分替換為本文算法后的實(shí)驗(yàn)結(jié)果如圖11所示，可以看出，最終系統(tǒng)識(shí)別出機(jī)器人回到了原來(lái)的位置，正確地進(jìn)行了回環(huán)，機(jī)器人的運(yùn)動(dòng)軌跡被修正，如圖11(b)所示，相同區(qū)域的三維點(diǎn)云得到了準(zhǔn)確匹配，如圖11(a)所示。

(a)三維點(diǎn)云地圖

文獻(xiàn)[15]算法和本文算法對(duì)真實(shí)環(huán)境中回環(huán)幀的計(jì)算時(shí)間如圖12所示。本文算法在每個(gè)回環(huán)幀上的計(jì)算平均用時(shí)為1.015 ms，而對(duì)比算法的平均用時(shí)為1.780 ms，即本文算法的計(jì)算效率提高了約43%。

圖12 室外實(shí)驗(yàn)中兩個(gè)算法的計(jì)算時(shí)間對(duì)比

4 結(jié)語(yǔ)

針對(duì)現(xiàn)有基于激光雷達(dá)并采用全局描述符的回環(huán)檢測(cè)存在的不足，本文提出了一種同時(shí)利用點(diǎn)云幾何信息和強(qiáng)度信息的回環(huán)檢測(cè)算法,其首先利用機(jī)器人在SLAM過(guò)程中得到的旋轉(zhuǎn)矩陣對(duì)點(diǎn)云進(jìn)行姿態(tài)變換，再通過(guò)兩種不同的編碼方式對(duì)同一幀點(diǎn)云的高度與強(qiáng)度信息進(jìn)行編碼來(lái)構(gòu)建點(diǎn)云的全局描述符，最后通過(guò)兩步匹配策略搜索出發(fā)生回環(huán)的點(diǎn)云。與采用Scan Context全局描述符的同類(lèi)型算法比較，本算法在公開(kāi)數(shù)據(jù)集KITTI上的實(shí)驗(yàn)中整體表現(xiàn)出更高的回環(huán)檢測(cè)精度和更快的運(yùn)行速度。在真實(shí)室外環(huán)境中運(yùn)行了集成本文回環(huán)檢測(cè)算法的SLAM算法，其建圖效果也有明顯改善。