基于FastDTW地磁匹配的快速閉環(huán)檢測(cè)方法研究*

羅恒杰， 鮑 泓， 徐 成

(北京聯(lián)合大學(xué) 北京市信息服務(wù)工程重點(diǎn)實(shí)驗(yàn)室,北京 100101)

0 引 言

即時(shí)定位與建圖(simultaneous localization and mapping，SLAM)是獲取環(huán)境地圖和定位信息的關(guān)鍵技術(shù)。SLAM技術(shù)考慮到了環(huán)境的真實(shí)拓?fù)浣Y(jié)構(gòu)，可以直接有效地獲取環(huán)境地圖，并且可以隨著環(huán)境的改變更新地圖。基于激光雷達(dá)(LiDAR)的SLAM可以精確地測(cè)量障礙物點(diǎn)的角度和距離，工作時(shí)不受光照影響，被廣泛應(yīng)用于無人車、室內(nèi)外機(jī)器人導(dǎo)航和三維重建等領(lǐng)域。但當(dāng)建圖場(chǎng)景的結(jié)構(gòu)具有一定的相似性時(shí)，激光雷達(dá)點(diǎn)云將不足以清楚地描述真實(shí)環(huán)境，并且在SLAM閉環(huán)檢測(cè)時(shí)容易產(chǎn)生誤報(bào)，從而降低SLAM的性能，導(dǎo)致地圖與真實(shí)環(huán)境不一致。融合輔助信息可以有效地減少SLAM中由于高局部相似性引起的誤檢，提高閉環(huán)檢測(cè)的效率，降低累積誤差[1]。使用超寬帶(ultra wide band，UWB)和2D激光雷達(dá)進(jìn)行融合，利用UWB精確定位的特性，可以很好地降低累積誤差，但由于UWB信號(hào)的遮擋和衰減問題，這種方法不適用于有遮擋的大場(chǎng)景[2]。結(jié)合視覺傳感器和激光雷達(dá)傳感器提出了同時(shí)考慮掃描和圖像數(shù)據(jù)的成本函數(shù)，將具有視覺特征的詞袋模型應(yīng)用到閉環(huán)檢測(cè)中，加快了閉環(huán)檢測(cè)的速度，但在光照條件不好的情況下這種方法的效果將會(huì)變的很差。

隨著對(duì)地磁的深入研究[3]，文獻(xiàn)[4]首次利用地磁來解決SLAM問題，并證明了實(shí)驗(yàn)的可行性。文獻(xiàn)[5]證明了建筑物內(nèi)部磁場(chǎng)的異常幾乎是靜態(tài)的,并且它們具有足夠的局部可變性，可以提供用于定位的唯一磁性指紋。由于地磁定位不需要基礎(chǔ)設(shè)施，越來越多的研究人員開始了對(duì)地磁定位的研究[5]。文獻(xiàn)[6]提出了一種使用智能手機(jī)室內(nèi)定位系統(tǒng)來定位室內(nèi)目標(biāo)行人的位置，文獻(xiàn)[7]認(rèn)為手機(jī)中的低成本磁力計(jì)無法滿足定位精度需求，提出將地磁定位與其他定位方法結(jié)合使用。文獻(xiàn)[8]將地磁與運(yùn)動(dòng)模式關(guān)聯(lián)，提出了一種基于關(guān)鍵幀的姿勢(shì)圖SLAM。文獻(xiàn)[9]提出了一種WiFi和磁場(chǎng)融合的多點(diǎn)定位的室內(nèi)定位方法。使用中值濾波算法處理原始磁場(chǎng)數(shù)據(jù)，并使用協(xié)方差插值算法生成磁場(chǎng)圖，有效地減少了地磁波動(dòng)引起的干擾，從而提高定位精度。為了增強(qiáng)地磁定位的適應(yīng)性和準(zhǔn)確性,文獻(xiàn)[9～12]將動(dòng)態(tài)時(shí)間規(guī)整(dynamic time warping,DTW)應(yīng)用到了地磁定位中以提高地磁匹配速度和定位精度。文獻(xiàn)[13]將蟻群算法與DTW算法融合，雖然獲得了較高的地磁定位精度，但該方法復(fù)雜度高，實(shí)時(shí)性較差。文獻(xiàn)[14]針對(duì)地磁序列匹配定位中DWT算法實(shí)時(shí)性差的問題提出一種基于快速動(dòng)態(tài)時(shí)間規(guī)整(fast dynamic time warping,FastDTW)的地磁定位算法，在保持了較高定位精度的情況下，采用減小搜索的策略提高了定位的實(shí)時(shí)性。

針對(duì)激光雷達(dá)SLAM大規(guī)模建圖時(shí)閉環(huán)檢測(cè)匹配速度慢和閉環(huán)誤檢測(cè)的問題，本文提出了一種基于FastDTW地磁匹配和激光雷達(dá)點(diǎn)云匹配相結(jié)合的快速閉環(huán)檢測(cè)方法。實(shí)驗(yàn)結(jié)果表明，通過FastDTW地磁序列搜索減小閉環(huán)檢測(cè)時(shí)激光雷達(dá)點(diǎn)云搜索匹配的范圍，可以有效地提高閉環(huán)檢測(cè)的實(shí)時(shí)性，同時(shí)因?yàn)樗阉鞣秶目刂疲`檢的問題也得到了很好解決。

1 點(diǎn)云匹配策略

(1)

(2)

所以有

(3)

(4)

(5)

(6)

因此,每次匹配都會(huì)有誤差增量

(7)

當(dāng)激光雷達(dá)沿直線運(yùn)動(dòng)時(shí)，角度變化不大，可認(rèn)為直線運(yùn)動(dòng)時(shí)的角度變化視為i-1=0，因此，此時(shí)的累積誤差為

(8)

因此,不管是否有角度變化，隨著匹配幀數(shù)的增加，累積誤差會(huì)逐漸增加。

閉環(huán)檢測(cè)指的是無人車到達(dá)已經(jīng)建好地圖的區(qū)域時(shí)執(zhí)行的一個(gè)掃描—搜索—匹配的過程。對(duì)已經(jīng)建好的地圖進(jìn)行第二次掃描觀測(cè)，當(dāng)獲得新的掃描幀時(shí)，在附近一定范圍內(nèi)搜索最優(yōu)匹配幀。如果最優(yōu)匹配幀滿足要求，則認(rèn)為它是一個(gè)閉環(huán)。并通過兩次掃描觀測(cè)的差異對(duì)地圖進(jìn)行調(diào)整，減少SLAM的累積誤差，搜索匹配的問題可以描述為

(9)

式中W為搜索空間，Mnearest為與該定位點(diǎn)對(duì)應(yīng)的網(wǎng)格點(diǎn)的M值。在W空間中尋找可靠性最高的最優(yōu)解。最簡(jiǎn)單粗暴的辦法是一幀一幀地進(jìn)行匹配，即將當(dāng)前幀與搜索空間中的每一幀執(zhí)行式(9)進(jìn)行非線性最小二乘計(jì)算，匹配求解的剛性變換ξ=(ξx，ξy，ξθ)，將無人車的初始位置與姿態(tài)相乘，即可得到此時(shí)的位姿(xi,yi,θi)。然后根據(jù)當(dāng)前幀位姿與最優(yōu)匹配幀位姿對(duì)地圖進(jìn)行調(diào)整。

2 地磁匹配與激光雷達(dá)SLAM融合策略

DTW可以很好地比較兩個(gè)序列的相似度，其核心思想是計(jì)算出這條路徑經(jīng)過的所有點(diǎn)的坐標(biāo)(i,j)對(duì)應(yīng)的X和Y兩個(gè)時(shí)間序列的點(diǎn)Xi和Yj的歐氏距離之和，距離越小，兩個(gè)序列越相似。如圖1根據(jù)動(dòng)態(tài)規(guī)劃的思想有

D(i,j)=Dist(i,j)+min[D(i-1,j),D(i,j-1)，

D(i-1,j-1)]

(10)

圖1 具有最小距離扭曲路徑的成本矩陣

DTW算法會(huì)不停地按照式(10)去尋找成本矩陣元素D(i,j)，每次執(zhí)行都會(huì)搜索所有元素，執(zhí)行效率低。當(dāng)兩個(gè)序列都是長(zhǎng)序列時(shí)，算法的實(shí)時(shí)性將會(huì)很低，縮減算法的搜索空間可以有效地降低算法的復(fù)雜度，F(xiàn)astDTW[14]對(duì)DTW算法的改進(jìn)主要有兩點(diǎn)：

1)約束:限制在成本矩陣中評(píng)估的單元格的數(shù)量，例如Sakoe-Chuba Band約束[15]和Itakura Parallelogram約束[16],如圖2所示。

圖2 兩種約束

2)抽象：首先將兩個(gè)長(zhǎng)時(shí)間序列組成的大矩陣進(jìn)行粗粒度化，然后使用DTW算法算出最短路徑，接著在控制路徑的搜索范圍的條件下進(jìn)行細(xì)粒度化，如圖3所示。

圖3 FastDTW算法

使用FastDTW進(jìn)行地磁匹配時(shí)，目標(biāo)是找到地磁數(shù)據(jù)序列的最小相似距離。選擇地磁場(chǎng)的模值(三軸地磁分量的平方和的算術(shù)平方根)作為地磁特征量。例如X軸方向的地磁分量為dx，Y軸方向的地磁分量為dy，Z軸方向的地磁分量為dz，則地磁的模值為

(11)

假設(shè)待匹配的地磁數(shù)據(jù)序列長(zhǎng)度為m，DCM=(DC1,DC2,…,DCm)，地磁數(shù)據(jù)庫(kù)序列長(zhǎng)度為n(n>m),DCN=(DC1,DC2,…,DCn),在進(jìn)行地磁匹配時(shí)，從DCN上截取與DCM相同長(zhǎng)度的數(shù)據(jù)進(jìn)行匹配，即DCN的第1到m個(gè)數(shù)據(jù)與DCM匹配，然后是第2到m+1個(gè)數(shù)據(jù)與DCM匹配，直到DCN最后的m個(gè)數(shù)據(jù)與DCM進(jìn)行匹配。根據(jù)FastDTW求取每對(duì)數(shù)據(jù)的相似距離L，所有數(shù)據(jù)匹配完之后得到一個(gè)相似距離集合L=(l1,l2,…,ls)，集合中最小值對(duì)應(yīng)的數(shù)據(jù)即為匹配數(shù)據(jù)。在地磁匹配與激光雷達(dá)SLAM融合時(shí)，無人車在進(jìn)行環(huán)境地圖掃描的同時(shí)獲取對(duì)應(yīng)的三軸地磁數(shù)據(jù)，計(jì)算地磁模值并加入地磁數(shù)據(jù)庫(kù)。當(dāng)環(huán)境地圖掃描完成時(shí)，地磁庫(kù)數(shù)據(jù)DCN建立完成。在激光雷達(dá)SLAM進(jìn)行閉環(huán)搜索時(shí)，采用子圖—子圖的匹配，當(dāng)新的子圖submapnew構(gòu)建完成時(shí)，對(duì)應(yīng)生成一個(gè)地磁數(shù)據(jù)序列DCnew，使用FastDTW算法在DCN中搜索與DCnew存在最小相似距離的地磁序列DCmatch，然后將與DCmatch對(duì)應(yīng)的子圖submapmatch中的位姿節(jié)點(diǎn)posematch加入到閉環(huán)檢測(cè)候選位姿集合p中,然后使用式(9)計(jì)算出最佳閉環(huán)位姿ξ。地磁匹配與激光雷達(dá)SLAM融合算法的偽代碼如下：

算法1 地磁匹配與激光雷達(dá)SLAM融合算法

輸入：新生成的地磁序列DCnew，與DCnew對(duì)應(yīng)的位姿posenew，地磁數(shù)據(jù)庫(kù)序列DCN

輸出：最佳閉環(huán)位姿ξ

1：定義相似距離集合L，相似距離l，空值序列DC′，實(shí)數(shù)index，候選位姿集合p

2:fori=1 tolength(DCN)do

3: if(length(DCnew)+i)>(length(DCN))

4: end if

5: else

6:DC′=(DCi,DCi+1,…,DCi+m-1)

7:l=FastDTW(DC′,DCnew)

8:Li=l

9: end if

10:end for

11:Lindex=min(L1,L2,L3,…)

12:DCmatch=DCindex

13:posematch=poseindex

14:posematchadd top

15:W=p.num

16:fori=1 toWdo

18:end for

19:returnξ

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

本文所使用的實(shí)驗(yàn)平臺(tái)為北京聯(lián)合大學(xué)聯(lián)合彩虹系列無人車，所使用到的傳感器有鐳神智能16線激光雷達(dá)和Sparkfun 9軸IMU，地磁匹配與激光雷達(dá)SLAM實(shí)驗(yàn)平臺(tái)如圖4所示，本次實(shí)驗(yàn)所選的實(shí)驗(yàn)場(chǎng)景為北京聯(lián)合大學(xué)實(shí)驗(yàn)樓五樓，平面圖如圖5所示，實(shí)驗(yàn)所使用的數(shù)據(jù)均為實(shí)驗(yàn)場(chǎng)景實(shí)地采集，圖6為實(shí)驗(yàn)場(chǎng)景的地磁強(qiáng)度采樣圖。

圖4 地磁匹配與激光雷達(dá)SLAM實(shí)驗(yàn)平臺(tái)

圖5 實(shí)驗(yàn)場(chǎng)景平面圖

圖6 實(shí)驗(yàn)場(chǎng)景的地磁強(qiáng)度采樣圖

閉環(huán)檢測(cè)可以根據(jù)達(dá)到閉環(huán)時(shí)的位姿差異減少累積誤差，但是在發(fā)生閉環(huán)誤檢測(cè)的時(shí)候，地圖會(huì)發(fā)生漂移，導(dǎo)致地圖不能使用，如圖7所示。

圖7 正確閉環(huán)與閉環(huán)誤檢測(cè)建圖結(jié)果對(duì)比

使用輔助搜索匹配算法可以提升閉環(huán)檢測(cè)的效率，對(duì)逐幀法、分支定界法、DTW融合法和FastDTW融合法做了對(duì)比實(shí)驗(yàn)，在其他條件不變的基礎(chǔ)上，每個(gè)算法運(yùn)行了100次，記錄結(jié)果，取其均值進(jìn)行閉環(huán)檢測(cè)和誤差分析。在進(jìn)行閉環(huán)檢測(cè)耗時(shí)對(duì)比時(shí)，為了保證實(shí)驗(yàn)的公平，讓小車行駛到地圖上的隨機(jī)位置，選用前100幀掃描幀進(jìn)行搜索匹配，對(duì)比結(jié)果如表1。

表1 搜索匹配算法效率對(duì)比

逐幀法的搜索匹配效率最低，滿足閉環(huán)檢測(cè)得分閾值的候選集一共157個(gè)，單次搜索匹配耗時(shí)為12 968 ms。分支定界法在搜索步長(zhǎng)上進(jìn)行了優(yōu)化，候選集減少為58個(gè)，單次搜索匹配耗時(shí)為6 638 ms。DTW融合法根據(jù)地磁匹配的結(jié)果極大地縮小了搜索范圍，候選集數(shù)量為23個(gè)，單次搜索匹配耗時(shí)為943 ms，本文使用的FastDTW融合法，不但縮小了搜索范圍，而且加快了地磁匹配的速度，候選集數(shù)量減少至14個(gè)，單次搜索匹配耗時(shí)僅為806 ms。從實(shí)驗(yàn)結(jié)果可知，F(xiàn)astDTW融合法與逐幀法和分支定界法相比，大大縮短了閉環(huán)檢測(cè)的耗時(shí)，與表現(xiàn)較優(yōu)的DTW融合法相比，閉環(huán)檢測(cè)速度提升了17 %，可以有效地提升閉環(huán)檢測(cè)的實(shí)時(shí)性。

在進(jìn)行閉環(huán)檢測(cè)時(shí)，正確閉環(huán)位姿僅有一個(gè)，而候選集的位姿卻有多個(gè)，所以，候選集的個(gè)數(shù)與誤檢發(fā)生的概率是成反比的，候選集越多，發(fā)生誤檢的可能性越大，每種算法100次運(yùn)行的結(jié)果證明此觀點(diǎn)是正確的，如表2所示。因此本文算法與其他三種算法相比有著較低的誤檢概率。

表2 100次實(shí)驗(yàn)中誤檢次數(shù)對(duì)比

在進(jìn)行誤差對(duì)比實(shí)驗(yàn)時(shí)，均取100次實(shí)驗(yàn)結(jié)果的平均，表3為四種算法的實(shí)驗(yàn)誤差對(duì)比。

表3 四種算法的實(shí)驗(yàn)誤差對(duì)比

從表3可知,本文算法與分支定界算法相比，平移誤差和旋轉(zhuǎn)誤差均降低了30 %，與DTW融合算法相比，平移誤差降低了16 %，旋轉(zhuǎn)誤差降低了10 %。由此可知，本文算法可以有效地提高建圖的精確度。

4 結(jié)束語

本文針對(duì)激光雷達(dá)SLAM的閉環(huán)檢測(cè)環(huán)節(jié)進(jìn)行改進(jìn)，提出了一種FastDTW地磁匹配與激光SLAM融合的快速閉環(huán)算法。實(shí)驗(yàn)結(jié)果表明該方法可以提高閉環(huán)檢測(cè)的速度，保持較低的誤檢水平，還能有效地降低激光雷達(dá)SLAM的平均絕對(duì)誤差。