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

羅恒杰， 鮑 泓， 徐 成

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

0 引 言

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

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

針對激光雷達(dá)SLAM大規(guī)模建圖時閉環(huán)檢測匹配速度慢和閉環(huán)誤檢測的問題，本文提出了一種基于FastDTW地磁匹配和激光雷達(dá)點云匹配相結(jié)合的快速閉環(huán)檢測方法。實驗結(jié)果表明，通過FastDTW地磁序列搜索減小閉環(huán)檢測時激光雷達(dá)點云搜索匹配的范圍，可以有效地提高閉環(huán)檢測的實時性，同時因為搜索范圍的控制，誤檢的問題也得到了很好解決。

1 點云匹配策略

(1)

(2)

所以有

(3)

(4)

(5)

(6)

因此,每次匹配都會有誤差增量

(7)

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

(8)

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

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

(9)

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

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

DTW可以很好地比較兩個序列的相似度，其核心思想是計算出這條路徑經(jīng)過的所有點的坐標(biāo)(i,j)對應(yīng)的X和Y兩個時間序列的點Xi和Yj的歐氏距離之和，距離越小，兩個序列越相似。如圖1根據(jù)動態(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算法會不停地按照式(10)去尋找成本矩陣元素D(i,j)，每次執(zhí)行都會搜索所有元素，執(zhí)行效率低。當(dāng)兩個序列都是長序列時，算法的實時性將會很低，縮減算法的搜索空間可以有效地降低算法的復(fù)雜度，F(xiàn)astDTW[14]對DTW算法的改進(jìn)主要有兩點：

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

圖2 兩種約束

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

圖3 FastDTW算法

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

(11)

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

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

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

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

1：定義相似距離集合L，相似距離l，空值序列DC′，實數(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 實驗與結(jié)果分析

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

圖4 地磁匹配與激光雷達(dá)SLAM實驗平臺

圖5 實驗場景平面圖

圖6 實驗場景的地磁強度采樣圖

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

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

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

表1 搜索匹配算法效率對比

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

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

表2 100次實驗中誤檢次數(shù)對比

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

表3 四種算法的實驗誤差對比

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

4 結(jié)束語

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