汪小龍 滕滕

摘要：目的：針對(duì)三維激光SLAM閉環(huán)檢測(cè)存在的難以提取高重復(fù)性的獨(dú)特關(guān)鍵點(diǎn)，或者局部匹配始終遭受描述能力缺乏的問(wèn)題，提出了一種三維激光SLAM閉環(huán)檢測(cè)算法——Delay-CLD。方法：該算法首先利用前端里程計(jì)設(shè)置一個(gè)延遲閾值，然后利用全局3D描述符匹配算法M2DP高效的確定閉環(huán)候選者，接著在確定的閉環(huán)候選者之間進(jìn)行基于Super G-4PCS的粗配準(zhǔn)，以確保不會(huì)處理錯(cuò)誤的閉環(huán)檢測(cè)，最后，使用改進(jìn)的ICP算法來(lái)完善轉(zhuǎn)換估計(jì)，最終完成閉環(huán)檢測(cè)。結(jié)論：論文的最后通過(guò)兩組實(shí)驗(yàn)證明了這種方法的優(yōu)越性。

關(guān)鍵詞：三維激光SLAM;閉環(huán)檢測(cè);3D全局描述符;Super G-4PCS;ICP

中圖分類號(hào)：TP391文獻(xiàn)標(biāo)志碼：A文章編號(hào)：1008-4657（2020）03-0005-07

0?引言

使用可移動(dòng)平臺(tái)搭載傳感器同時(shí)定位與繪制環(huán)境是一個(gè)研究了三十多年的話題，自從1988年SLAM技術(shù)被首次提出[1]之后，一直是一個(gè)高度活躍的研究領(lǐng)域。其中，面向3D激光SLAM的閉環(huán)檢測(cè)算法作為一種能夠有效降低SLAM建圖、定位誤差的方法也吸引了眾多研究學(xué)者的目光。

不局限于三維激光雷達(dá)獲取的3D點(diǎn)云數(shù)據(jù)，當(dāng)前的基于3D數(shù)據(jù)進(jìn)行閉環(huán)檢測(cè)的技術(shù)大致可以分為三類：基于局部關(guān)鍵點(diǎn)檢測(cè)結(jié)合詞袋（BoW）的匹配方法、基于幾何圖元或?qū)ο笳w的匹配方法以及基于全局描述符匹配方法。

第一類方法最早是由Steder B等[2]在2011年提出，此類方法通常是檢測(cè)點(diǎn)云中的顯著關(guān)鍵點(diǎn)，通過(guò)計(jì)算這些關(guān)鍵點(diǎn)位置的簽名，建立BoW，最后在不同的掃描中進(jìn)行匹配，對(duì)每次匹配的相似度進(jìn)行評(píng)分，如果評(píng)分分?jǐn)?shù)高于可接受閾值，則認(rèn)為此次匹配就確定了閉環(huán)檢測(cè)的候選者。很顯然，這類方法的關(guān)鍵是對(duì)于點(diǎn)云數(shù)據(jù)中關(guān)鍵點(diǎn)的檢測(cè)以及描述，很多文獻(xiàn)也提出了許多關(guān)鍵點(diǎn)檢測(cè)器，例如固有形狀簽名（ISS）[3]，Harris 3D[4]，Sift 3D[5]，NARF[6]，以及許多描述符，例如Spin Image[7]和SHOT[8]。然而，盡管有這么多的選擇，但是具有高重復(fù)性的獨(dú)特關(guān)鍵點(diǎn)的檢測(cè)提取仍然是一個(gè)挑戰(zhàn)。

第二類方法著重于基于完整對(duì)象或平面的位置識(shí)別。Fernandez-Moral E等[9]在2013年提出了一種基于平面地圖的位置識(shí)別算法：將這些平面累積在一個(gè)graph圖中，并使用樹(shù)的形式來(lái)匹配子圖，最后對(duì)匹配子圖的平面進(jìn)行幾何一致性檢驗(yàn)。這項(xiàng)工作由Fernández-Moral E等[10]于2016年進(jìn)行了擴(kuò)展，用平面參數(shù)的協(xié)方差代替平面上的點(diǎn)數(shù)進(jìn)行匹配。但是，他們的方法僅適用于小的室內(nèi)人造環(huán)境，無(wú)法擴(kuò)展到室外場(chǎng)景。除此之外，還有Finman R等[11]在2015年提出的使用RGB-D攝像機(jī)在室內(nèi)環(huán)境中進(jìn)行基于對(duì)象的閉環(huán)檢測(cè)方法以及Dubé R等[12]在2016年提出的基于段的方法。但是這些方法同樣受到工作環(huán)境的強(qiáng)大限制，會(huì)由于不適應(yīng)某些場(chǎng)景而導(dǎo)致最終閉環(huán)檢測(cè)失敗。

第三類方法即使用全局描述符，全局描述符通常以直方圖的形式出現(xiàn)，比較典型的是Rusu R B等[13]在2009年提出的快速點(diǎn)特征直方圖FPFH。最近研究人員還傾向于使用卷積神經(jīng)網(wǎng)絡(luò)（CNN）來(lái)學(xué)習(xí)特征描述符以及以統(tǒng)一的方式匹配它們，但是這種方法需要大量的訓(xùn)練數(shù)據(jù)，這給此類方法帶來(lái)了很大的局限性。2016年，He L等[14]提出了一種新型全局3D描述符——多視圖2D投影（M2DP）。這種全局描述符方法相比直方圖形式和CNN學(xué)習(xí)方法，在準(zhǔn)確性和效率方面都有了很大的提升，但是盡管如此，這種全局描述符與其局部的匹配仍然會(huì)受到描述能力或不變性不足的限制，從而會(huì)出現(xiàn)檢測(cè)不到閉環(huán)或者檢測(cè)到過(guò)多的錯(cuò)誤閉環(huán)。基于以上分析，本文提出了一種完整的3D激光SLAM閉環(huán)檢測(cè)算法Delay-CLD（Delay Closed-loop Detection）來(lái)解決這些問(wèn)題。

1?Delay-CLD結(jié)構(gòu)

整個(gè)閉環(huán)檢測(cè)系統(tǒng)主要包括兩個(gè)步驟：

1.1?確定強(qiáng)閉環(huán)候選者

首先，利用SLAM前端里程計(jì)的位姿信息來(lái)判斷移動(dòng)平臺(tái)當(dāng)前位置與初始位置之間的位置關(guān)系，從而設(shè)定一個(gè)延遲檢測(cè)閾值，即Delay檢測(cè)。這一步的目的是避免從SLAM運(yùn)行初始就開(kāi)始檢測(cè)閉環(huán)候選者所帶來(lái)大量的無(wú)意義計(jì)算。然后，利用全局3D描述符M2DP[14]不依賴點(diǎn)云中表面法線的估計(jì)，且在描述符的匹配準(zhǔn)確性和效率方面有良好表現(xiàn)的特點(diǎn)來(lái)檢測(cè)出激光雷達(dá)數(shù)據(jù)中的強(qiáng)閉環(huán)候選者。

1.2?閉環(huán)候選者驗(yàn)證

當(dāng)確定了閉環(huán)候選者之后，為了確保我們不會(huì)達(dá)成錯(cuò)誤的閉環(huán)，下一步將尋求使兩片點(diǎn)云對(duì)齊的變換：首先采用基于Super G-4PCS[15]的全局對(duì)齊技術(shù)，以獲得對(duì)閉環(huán)兩端之間轉(zhuǎn)換的粗略估計(jì);粗略對(duì)齊后，再使用改進(jìn)的ICP算法來(lái)完善轉(zhuǎn)換估計(jì)，如果最終匹配的殘差很小，那么閉環(huán)檢測(cè)條件達(dá)成。

整個(gè)閉環(huán)檢測(cè)系統(tǒng)的結(jié)構(gòu)如圖1所示。

2?Delay-CLD算法實(shí)現(xiàn)

2.1?Delay檢測(cè)

Delay檢測(cè)的主要部分是SLAM前端里程計(jì)，這里我們采用LOAM算法[16]的里程計(jì)算法。假設(shè)激光雷達(dá)的運(yùn)動(dòng)是勻速的，算法以激光雷達(dá)最后一次掃描的點(diǎn)云集P-k、當(dāng)前掃描的增長(zhǎng)點(diǎn)云集P-k+1和最后一次遞歸的姿態(tài)TLk+1作為輸入。在開(kāi)始新的掃描時(shí)，將TLk+1設(shè)置為0，然后從P-k+1中提取特征點(diǎn)，構(gòu)造特征點(diǎn)集εk+1（邊緣特征點(diǎn)集）和Hk+1（平面點(diǎn)集）。對(duì)于每個(gè)特征點(diǎn)，在P-k中找到它們的對(duì)應(yīng)關(guān)系，并根據(jù)它們的對(duì)應(yīng)關(guān)系為每個(gè)特征點(diǎn)分配雙平方權(quán)重：互相之間距離較大的特征點(diǎn)被分配較小的權(quán)重，而距離大于閾值的特征點(diǎn)則被視為離群點(diǎn)并被分配零權(quán)重。然后將姿態(tài)變換更新一次，如果找到收斂或達(dá)到最大迭代次數(shù)，則非線性優(yōu)化終止。其中優(yōu)化公式如下

如果當(dāng)前幀計(jì)算完成，則將當(dāng)前的點(diǎn)云P-k+1重新投影到時(shí)間戳tk+2，否則，僅返回變換TLk+1，并進(jìn)行下一輪遞歸。到這里，我們已經(jīng)可以獲得激光雷達(dá)在每一時(shí)刻的位姿，接下來(lái)我們就可以通過(guò)測(cè)試每個(gè)新計(jì)算的位置是否接近我們之前已經(jīng)訪問(wèn)過(guò)的位置，來(lái)決定是否進(jìn)入下一階段的任務(wù)。

設(shè)定一個(gè)閾值D，該閾值等于自SLAM運(yùn)行開(kāi)始以來(lái)總軌跡長(zhǎng)度的15%，只有當(dāng)檢測(cè)出當(dāng)前的姿態(tài)距離初始姿態(tài)在閾值距離以外，則認(rèn)為該位置時(shí)可以進(jìn)行下一步任務(wù)的，這樣就可以避免前期大量的無(wú)意義計(jì)算。

2.2?M2DP全局描述符匹配

大多數(shù)現(xiàn)有的3D描述符在構(gòu)建的時(shí)候都利用點(diǎn)的法線，但是對(duì)于具有嘈雜數(shù)據(jù)的點(diǎn)云，通常很難獲得一個(gè)點(diǎn)的準(zhǔn)確法線，而且這一過(guò)程通常很耗時(shí)。除了用簽名和直方圖，使用諸如Spin Image[7]或ESF[17]之類的非常規(guī)方法獲取的描述符也會(huì)由于缺乏空間信息從而阻礙它們捕獲一個(gè)有效且準(zhǔn)確的描述符。而由He L等[14]提出的M2DP算法所提取的全局描述符并不依賴點(diǎn)云中點(diǎn)的法線，且易于計(jì)算并能夠捕捉點(diǎn)云的局部幾何細(xì)節(jié)，這是在閉環(huán)檢測(cè)中非常重要的兩個(gè)屬性。

在閉環(huán)檢測(cè)中，描述符在3D空間中的移動(dòng)和旋轉(zhuǎn)不變性是至關(guān)重要的。借鑒了直方圖全局描述符如VFH、CVFH中的方法，為確保平移、旋轉(zhuǎn)不變，M2DP使用輸入點(diǎn)云的質(zhì)心作為描述符參考系的原點(diǎn)。然后，通過(guò)使用主元分析法PCA估計(jì)點(diǎn)云的兩個(gè)主要方向來(lái)定義參考系，將兩個(gè)主要方向設(shè)置為描述符參考幀的x軸和y軸。在構(gòu)建3D描述符時(shí)，先將3D數(shù)據(jù)投影到穿過(guò)原點(diǎn)的不同的2D平面上，其中，每個(gè)平面都可以使用方位角θ和仰角φ表示，而每對(duì)參數(shù)θ，φ就決定了一個(gè)唯一平面X，該平面的法向矢量m表示為

而投影在該平面上的點(diǎn)ux通過(guò)ux=u-uTmm22m給出。

為了捕獲投影平面上的點(diǎn)的結(jié)構(gòu)，將2D平面劃分成數(shù)個(gè)倉(cāng)：以投影質(zhì)心為中心，生成l個(gè)同心圓，這些同心圓的半徑依次為r，22r，…，l2r，且最大半徑設(shè)置為與質(zhì)心和最遠(yuǎn)點(diǎn)之間的距離。將每個(gè)環(huán)劃分為t個(gè)倉(cāng)，因此定義了l × t個(gè)不同的倉(cāng)。對(duì)于每個(gè)倉(cāng)，計(jì)算其中的投影點(diǎn)數(shù)，生成一個(gè)lt × 1的描述投影在平面X上的點(diǎn)的特征向量vx

其中，n是平面上投影的總點(diǎn)數(shù)。

通過(guò)使用p個(gè)不同的方位角θs和q個(gè)不同的仰角φs生成多個(gè)2D平面，其中方位角上的步幅為πp，仰角上的步幅為π2q。因此，共存在pq個(gè)不同的平面，對(duì)于每個(gè)2D平面，我們生成上一部分中描述的2D簽名，它是lt × 1的矢量，這樣就構(gòu)成了一個(gè)pq × lt的簽名矩陣A，矩陣的每一行對(duì)應(yīng)一個(gè)簽名矢量vx。最后，在矩陣A上進(jìn)行SVD分解，并且將第一個(gè)左右奇異矢量連接起來(lái)以形成最終描述符。

2.3?Super G-4PCS全局粗配準(zhǔn)

前面描述的算法提供了閉環(huán)候選者，但是，正如前文所說(shuō)，僅僅基于3D全局描述符的閉環(huán)檢測(cè)方法會(huì)受到描述符描述能力不足從而可能導(dǎo)致的閉環(huán)檢測(cè)結(jié)果呈假陽(yáng)性的結(jié)果，即當(dāng)前檢測(cè)出的閉環(huán)候選者并不足以達(dá)成閉環(huán)檢測(cè)。如果想進(jìn)一步的提高閉環(huán)檢測(cè)的準(zhǔn)確性，我們需要對(duì)檢測(cè)出來(lái)的閉環(huán)候選者與起點(diǎn)處的點(diǎn)云進(jìn)行配準(zhǔn)，如果配準(zhǔn)的結(jié)果顯示誤差較大，那么將當(dāng)前的閉環(huán)候選者舍棄，繼續(xù)下一輪的閉環(huán)候選者檢測(cè)。

由于檢測(cè)出的閉環(huán)候選者處的點(diǎn)云與起點(diǎn)處的點(diǎn)云發(fā)生了很大的方向變化，為此，我們需要一種能夠處理方向變化較大的配準(zhǔn)算法，即點(diǎn)云的粗配準(zhǔn)方法。面向點(diǎn)云的粗配準(zhǔn)方法有很多，大體可分為兩類：一類是基于窮舉搜索的配準(zhǔn)方法，通過(guò)遍歷整個(gè)變換空間以選取使誤差函數(shù)最小化的變換關(guān)系或者列舉出使最多點(diǎn)對(duì)滿足的變換關(guān)系，如RANSAC算法[18]、四點(diǎn)一致集配準(zhǔn)算法（4-PCS）[19]等;另一類是基于特征匹配的配準(zhǔn)算法，通過(guò)被測(cè)物體本身所具備的形態(tài)特性構(gòu)建點(diǎn)云間的匹配對(duì)應(yīng)，然后采用相關(guān)算法對(duì)變換關(guān)系進(jìn)行估計(jì)，如基于直方圖的FPFH點(diǎn)特征的SAC-IA、FGR等算法以及基于點(diǎn)SHOT特征的AO算法等。

但是在SLAM的閉環(huán)檢測(cè)過(guò)程中，激光雷達(dá)所經(jīng)過(guò)的路徑往往都是動(dòng)態(tài)的，特別是面向室外的SLAM系統(tǒng)，室外場(chǎng)景中的行人、車輛等物體的出現(xiàn)與消失很容易會(huì)導(dǎo)致基于局部特征的配準(zhǔn)失敗。2008年由Aiger等[19]提出的四點(diǎn)一致集配準(zhǔn)算法4-PCS是一種不依賴特征提取的全局點(diǎn)云配準(zhǔn)技術(shù)。4-PCS算法的基本思想是基于RANSAC算法，區(qū)別在于將RANSAC原本的隨機(jī)選擇三個(gè)不同的點(diǎn)修改為以源點(diǎn)云中共面的四點(diǎn)為基，在目標(biāo)點(diǎn)云中尋找近似仿射相等的共面四點(diǎn)對(duì)，再確認(rèn)這些共面四點(diǎn)對(duì)與選定的共面四點(diǎn)標(biāo)準(zhǔn)對(duì)在一定的距離約束下是否相等。4-PCS算法的運(yùn)行時(shí)間復(fù)雜度為On2+k，其中n是源點(diǎn)集Q中的點(diǎn)數(shù)，k是要報(bào)告的全等基數(shù)。

4-PCS算法有兩個(gè)主要的瓶頸：第一個(gè)是從源點(diǎn)集Q中提取四點(diǎn)對(duì)，這是在On2的時(shí)間復(fù)雜度內(nèi)窮舉執(zhí)行的;第二個(gè)就是大量報(bào)告的一致集，這導(dǎo)致4-PCS算法的驗(yàn)證步驟代價(jià)高昂。針對(duì)這一點(diǎn)，Mohamad M等[15]在Super-4PCS中通過(guò)解決這兩個(gè)瓶頸，將算法的總運(yùn)行時(shí)間復(fù)雜度降低到On+k1+k2。但是這兩種配準(zhǔn)方法都強(qiáng)制選取共面的4點(diǎn)，這在處理3D數(shù)據(jù)時(shí)常常會(huì)由于點(diǎn)云的對(duì)稱導(dǎo)致配準(zhǔn)出錯(cuò)。2015年Mellado N等[20]在4-PCS和Super-4PCS的基礎(chǔ)上提出了Super G-4PCS，該算法不限制4點(diǎn)必須在一個(gè)平面內(nèi)，如圖2所示。

在Super G-4PCS中，非共面4點(diǎn)對(duì)的仿射不變性將由重新定義的兩個(gè)不變比來(lái)描述：

此外，定義了ab與cd之間的夾角為α，x、y之間的距離為3D交點(diǎn)距離d3：

Super G-4PCS算法主要分為以下幾個(gè)步驟：

（1）給定固定不變的d1、d2（分別是ab和cd的距離），用Super-4PCS高效地提取出源點(diǎn)集Q和目標(biāo)點(diǎn)集P中所有滿足不變性的線段。窮舉計(jì)算源點(diǎn)集Q中提取的每個(gè)d1點(diǎn)對(duì)和所有d2點(diǎn)對(duì)構(gòu)成的直線之間的潛在3D交點(diǎn)，如果計(jì)算出這兩個(gè)點(diǎn)對(duì)不平行，則獲得一組r1、r2、d3、α的值。為了確定這對(duì)3D交叉點(diǎn)對(duì)是有效的，設(shè)定一個(gè)參數(shù)σ，若此處的d3與源點(diǎn)集Q的直徑的比值小于σ，則認(rèn)為這對(duì)3D交叉點(diǎn)對(duì)有效。

（2）初始化一個(gè)正四維哈希表H，將所有有效3D交叉的線段索引存儲(chǔ)在哈希表中，該4維哈希表的維數(shù)對(duì)應(yīng)于不變量r1、r2、d3、α。這樣做的好處是只需要構(gòu)建一次四維哈希表，而且對(duì)于之后的每個(gè)RANSAC迭代，僅需查詢一次與源點(diǎn)集Q中的4點(diǎn)基一致的目標(biāo)點(diǎn)集P中的四點(diǎn)基。

（3）Super G-4PCS的最后一步是使用最大公共點(diǎn)集（LCP）標(biāo)準(zhǔn)檢驗(yàn)利用四點(diǎn)基及其一致集計(jì)算的剛性變換。LCP標(biāo)準(zhǔn)規(guī)定應(yīng)計(jì)算變換后源點(diǎn)集中與目標(biāo)點(diǎn)云集中的點(diǎn)的距離在參數(shù)δ以內(nèi)的點(diǎn)數(shù)，產(chǎn)生最大LCP的轉(zhuǎn)換被認(rèn)為是真正的轉(zhuǎn)換。

2.4?ICP精匹配

Super G-4PCS方法產(chǎn)生了從源到目標(biāo)點(diǎn)云的粗略轉(zhuǎn)換，但不能完美的對(duì)齊它們。因此，還需要對(duì)經(jīng)過(guò)Super G-4PCS配準(zhǔn)的閉環(huán)候選者進(jìn)行進(jìn)一步的精配準(zhǔn)。一般地，在對(duì)三維激光雷達(dá)連續(xù)掃描的點(diǎn)云進(jìn)行精配準(zhǔn)時(shí)，大多數(shù)文獻(xiàn)提出的方法都是基于迭代最近點(diǎn)（ICP）算法。顧名思義，它是一種迭代算法，每次迭代主要包含四個(gè)主要步驟：（1）點(diǎn)選擇（Point Selection）;（2）對(duì)應(yīng)估計(jì)（Correspondence Estimation）;（3）加權(quán)（Weighting）;（4）變換估計(jì)（Transfor-mation Estimation）。標(biāo)準(zhǔn)ICP算法如圖3所示。

但是標(biāo)準(zhǔn)的ICP算法在處理三維激光雷達(dá)掃描生成的稀疏且密度不均勻的點(diǎn)云時(shí)有一些嚴(yán)重的局限性，很難在兩個(gè)連續(xù)的點(diǎn)云（源點(diǎn)云和目標(biāo)點(diǎn)云）之間找到良好的對(duì)應(yīng)關(guān)系，因?yàn)樵诖蠖鄶?shù)情況下，確定的對(duì)應(yīng)點(diǎn)對(duì)并不代表空間中的同一物理點(diǎn)。這導(dǎo)致即使經(jīng)過(guò)大量迭代后點(diǎn)云也無(wú)法準(zhǔn)確對(duì)齊，因此會(huì)錯(cuò)誤估計(jì)激光雷達(dá)的姿態(tài)，且收斂過(guò)程將非常緩慢。因此，我們通過(guò)使用一種基于ICP的改進(jìn)算法[21]來(lái)完善轉(zhuǎn)換。該ICP算法提供了額外的驗(yàn)證，可以得出兩點(diǎn)云產(chǎn)生真實(shí)的閉環(huán)的結(jié)論。如果ICP的殘差（定義為所有對(duì)應(yīng)點(diǎn)之間的平均距離）太大，仍將丟棄閉環(huán)候選。

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

3.1?實(shí)驗(yàn)環(huán)境

操作系統(tǒng)：Ubuntu14.04

硬件開(kāi)發(fā)環(huán)境：Intel Core i5-7300HQ CPU 2.50 GHz，8G RAM，64位操作系統(tǒng)

算法開(kāi)發(fā)環(huán)境：ROS Indigo

3D SLAM算法框架：LOAM

3.2?實(shí)驗(yàn)數(shù)據(jù)集采集

實(shí)驗(yàn)平臺(tái)搭載“北科天繪”公司生產(chǎn)的16線三維激光雷達(dá)“R-Fans-16”，實(shí)驗(yàn)在“北科天繪”公司4樓辦公室內(nèi)進(jìn)行，數(shù)據(jù)采集場(chǎng)景如圖4所示。數(shù)據(jù)采集過(guò)程中小車勻速行駛，在ROS環(huán)境下通過(guò)rosbag命令錄制出bag格式的“R-Fans“數(shù)據(jù)集。該數(shù)據(jù)集包含桌椅，窗戶，墻壁，廊道等典型室內(nèi)場(chǎng)景，可以對(duì)本文提出的Delay-CLD閉環(huán)檢測(cè)算法進(jìn)行比較全面的精度驗(yàn)證。

另外，為了更好的測(cè)試出Delay-CLD的準(zhǔn)確性，我們還對(duì)比了4種使用不同描述符的閉環(huán)檢測(cè)算法，依次為：基于M2DP的原始方法、基于ESF的方法、基于Spin Image的方法和基于SHOT的方法。對(duì)比實(shí)驗(yàn)使用了KITTI基準(zhǔn)的“00”和“05”兩個(gè)序列數(shù)據(jù)集。

3.3?實(shí)驗(yàn)過(guò)程及結(jié)果分析

為了更直觀的顯示出本文提出的Delay-CLD閉環(huán)檢測(cè)算法在實(shí)際應(yīng)用中的效果，將Delay-CLD應(yīng)用到具有代表性的3D激光SLAM算法LOAM算法中，并將原始LOAM算法和加入了Delay-CLD閉環(huán)檢測(cè)算法的LOAM算法在數(shù)據(jù)集“R-Fans”上的運(yùn)行效果進(jìn)行對(duì)比。結(jié)果如圖5所示。

從運(yùn)行的結(jié)果來(lái)看，原始LOAM算法在建圖和定位過(guò)程中發(fā)生了明顯的漂移，而且隨著時(shí)間的推移，漂移越來(lái)越大，到最后小車完成實(shí)際的閉環(huán)路徑之后，起點(diǎn)處和終點(diǎn)處的點(diǎn)云之間產(chǎn)生了約45o的偏差。而通過(guò)Delay-CLD對(duì)L-OAM進(jìn)行改進(jìn)之后，在建圖和定位時(shí)無(wú)明顯漂移，起點(diǎn)和終點(diǎn)處的點(diǎn)云重合，里程計(jì)軌跡閉合，完成了對(duì)室內(nèi)環(huán)境的準(zhǔn)確建圖與定位。提取出兩次運(yùn)行的里程計(jì)信息，進(jìn)行誤差分析，結(jié)果如表1所示。

分析表1中數(shù)據(jù)，可以看出在平均誤差上，改進(jìn)后要比改進(jìn)前提高了4.57倍的精度;在中間誤差上改進(jìn)后比改進(jìn)前提高了5.08倍的精度。同時(shí)可以看出原始LOAM的平均誤差與中間誤差存在明顯變化，這符合圖5中原始LOAM在中后程發(fā)生明顯漂移的實(shí)驗(yàn)結(jié)果。

按照準(zhǔn)確率的計(jì)算公式

計(jì)算在KITTI基準(zhǔn)中的“00”和“05”號(hào)序列的數(shù)據(jù)集上對(duì)不同回路檢測(cè)的準(zhǔn)確率實(shí)驗(yàn)結(jié)果，如表2所示。式中，TP表示真陽(yáng)性（True Positive），F(xiàn)P表示假陽(yáng)性（False Positive）。

從實(shí)驗(yàn)結(jié)果來(lái)看，在KITTI “00”和“05”序列的數(shù)據(jù)集上，基于Delay-CLD的方法的閉環(huán)檢測(cè)準(zhǔn)確率均要優(yōu)于其他4種閉環(huán)檢測(cè)方法，特別時(shí)在“05”序列數(shù)據(jù)集上，Delay-CLD方法的準(zhǔn)確率更是達(dá)到了94.2%，相較于其他四種方法中表現(xiàn)最好的原始M2DP方法在“05”序列數(shù)據(jù)集上的閉環(huán)檢測(cè)準(zhǔn)確率更是高出10.9%。

4?結(jié)論

對(duì)于現(xiàn)有的各種激光SLAM閉環(huán)檢測(cè)算法，大都存在閉環(huán)檢測(cè)準(zhǔn)確率偏低的困境，究其原因是因?yàn)楝F(xiàn)有的激光雷達(dá)掃描生成的點(diǎn)云數(shù)據(jù)存在稀疏、密度不均勻以及信息豐富度（如紋理信息）不足等。本文在全局3D描述符匹配閉環(huán)檢測(cè)思路的基礎(chǔ)上，通過(guò)兩次配準(zhǔn)將前期工作檢測(cè)出的閉環(huán)候選者加以篩選，最終形成了一個(gè)新的閉環(huán)檢測(cè)方法Delay-CLD。通過(guò)在KITTI數(shù)據(jù)集上的實(shí)驗(yàn)證明了Delay-CLD算法相對(duì)于其他閉環(huán)檢測(cè)算法的優(yōu)越性，并在具有代表性的3D激光SLAM算法LOAM上用Delay-CLD對(duì)其進(jìn)行了改造實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果證明了Delay-CLD在實(shí)際應(yīng)用中的效果是優(yōu)秀的。

參考文獻(xiàn)：

[1] Smith R，Self M，Cheeseman P.Estimating Uncertain Spatial Relationshipsin Robotics[C]//Autonomous Robot Vehicles.New York：Springer，1990：167-193.

[2]Steder B，Ruhnke M，Grzonka S，et al.Place Recognition in 3D Scans Using a Combination of Bag of Words and Point Feature Based Relative Pose Estimation[C]//International Conference on Intelligent Robots and Systems.San Francisco：IEEE，2011：1 249-1 255.

[3]Zhong Y.Intrinsic Shape Signatures：A Shape Descriptorfor 3D Object Recognition[C]//International Conference on Computer Vision Workshops.Kyoto：IEEE，2009：689-696.

[4]Sipiran I，Bustos B.A Robust 3D Interest Points Detector Based on Harris Operator[C]//Eurographics Workshop on 3D Object Retrieval.Norrkoping，Swenden：ACM，2010：7-14.

[5]Scovanner P，Ali S，Shah M.A 3-Dimensional Sift Descriptor and its Application to Action Recognition[C]//International Conference on Multimedia.New York：ACM，2007：357-360.

[6]Steder B，Rusu R B，Konolige K，et al.NARF：3D Range Image Featuresfor Object Recognition[C]//Intelligent Robots and Systems （IROS）.Taipei：IEEE/RSJ，2010：44.

[7]Johnson A.Spin-Images：A Representation for 3-D Surface Matching[D].Pittsburgh：Carnegie Mellon University，1997.

[8]Salti S，Tombari F，Di Stefano L.Shot：Unique Signatures of Histograms for Surface and Texture Description[J].Computer Vision and Image Understanding，2014，125：251-264.

[9]Fernandez-Moral E，Mayol-Cuevas W，Arevalo V，et al.Fast Place Recognitionwith Plane-based Maps[C]//International Conference on Robotics and Automation.Karlsruhe，Germany：IEEE，2013：2 719-2 724.

[10]Fernández-Moral E，Rives P，Arévalo V，et al.Scene Structure Registration for Localization and Mapping[J].Robotics and Autonomous Systems，2016，75：649-660.

[11]Finman R，Paull L，Leonard J J.Toward Object-based Place Recognitionin Dense Rgb-D Maps[C]//ICRA Workshop Visual Place Recognition in Changing Environments.Seattle：ICRA，2015：76.

[12]Dubé R，Dugas D，Stumm E，et al.Segmatch：Segment Based Loop-closure for 3D Point Clouds[C]//International Conference on Robotics and Atuomation.Marina Bay，Singapore：IEEE，2017：5 266-5 272.

[13]Rusu R B，Blodow N，Beetz M.Fast Point Feature Histograms （FPFH） for 3D Registration[C]//International Conference on Roboticsand Automation.Kobe，Japan：IEEE，2009：3 212-3 217.

[14]He L，Wang X，Zhang H.M2DP：A Novel 3D Point Cloud Descriptorand its Application in Loop Closure Detection[C]//Intelligent Robots and Systems （IROS）.Daejeon，South Korea：IEEE，2016：231-237.