融合NDT的2D激光數(shù)據(jù)掃描匹配遺傳算法

穆莉莉, 陳 凱, 何世政, 姚潘濤, 祁娜娜

(安徽理工大學(xué) 機(jī)械工程學(xué)院,安徽 淮南 232001)

0 引 言

移動機(jī)器人在連續(xù)運動過程中需要求解各個時刻的位姿,在同步定位與環(huán)境建圖 (simultaneous location and mapping,SLAM)領(lǐng)域通常采用激光測距儀作為外部傳感器,利用掃描匹配方法求解出連續(xù)的激光幀之間的相對位姿變換。常采用正態(tài)分布變換(normal distribution transform,NDT)方法解決激光掃描匹配問題[1],通過建立分段連續(xù)可微的概率分布,無需建立激光點之間明確的對應(yīng)關(guān)系。然而在初始位姿誤差較大的情況下使用NDT方法求解容易陷入局部極值,從而導(dǎo)致結(jié)果錯誤[2]。遺傳算法(genetic algorithm,GA)作為一種全局優(yōu)化方法,具有極強的搜索能力[3]。近年來,有許多學(xué)者嘗試將遺傳算法引入到激光掃描配準(zhǔn)問題中來,并取得了一些進(jìn)展[4～10]。Tom等人[4]采用地圖一致性和緊湊性作為適應(yīng)度判斷,逐步細(xì)分種群進(jìn)行全局搜索,得到了較為準(zhǔn)確的結(jié)果,但該方法計算代價大、速度慢,無法滿足實時性要求；Kristijan等人[5]提出GLASM(genetic look-up based algorithm for scan matching)遺傳方法,將參考幀激光柵格化建立查找表,通過查表的方式對種群進(jìn)行適應(yīng)度計算,能夠快速求解出位姿變換,該方法在精度上有一定損失,且初始位姿誤差大時匹配效果差；Feng D J等人[6]提出遺傳粒子濾波算法求解高度非線性、非高斯的SLAM掃描匹配問題；陳煥等人[7]提出的遺傳迭代最近點掃描匹配算法(generalized ICP,GICP)將改進(jìn)的迭代最近點(iterative closest point,ICP)關(guān)聯(lián)匹配規(guī)則作為適應(yīng)度計算標(biāo)準(zhǔn)進(jìn)行求解,能在里程計誤差較大或者機(jī)器人綁架情況下快速收斂,一定程度上解決了掃描匹配算法中任意的配準(zhǔn)問題?？傮w來說,基于遺傳算法的激光掃描匹配算法目前仍存在速度慢、計算代價大、匹配效果不佳等問題。

本文提出一種融合NDT的改進(jìn)遺傳算法,采用正態(tài)分布變換作為適應(yīng)度計算標(biāo)準(zhǔn),同時將參考幀柵格進(jìn)一步細(xì)分,建立查找表加速求解速度,利用遺傳算法不斷迭代求解出最優(yōu)位姿變換。

1 基于NDT算法的激光數(shù)據(jù)掃描匹配

設(shè)參考掃描Sref={p1,p2,…,pn},pi={xi,yi}∈R2是n

典型的NDT算法分為兩步：

1)構(gòu)建NDTcell

將Sref劃分為N個cell,統(tǒng)計cellj(j=1,2,…,N)里面的激光點數(shù)量,然后對于激光點數(shù)n大于3的cellj通過式(1)和式(2)計算其均值qj和方差Σj

(1)

(2)

2)最小化評價函數(shù)

(3)

優(yōu)化函數(shù)定義為所有點的得分總和

(4)

NDT采用牛頓法來實現(xiàn)極小化過程,牛頓法通常用Jacobian矩陣和Hessian矩陣代替函數(shù)本身和梯度向量。由于不需要對score函數(shù)線性化,因此可以直接求解Hessian矩陣。

2 融合NDT的2D激光數(shù)據(jù)掃描匹配遺傳算法

遺傳算法借鑒自然選擇過程和適者生存原則,具有全局搜索能力。本文提出融合NDT的2D激光數(shù)據(jù)掃描匹配遺傳(genetic algorithms for scanning matching of 2D laser data fused with NDT,NGASM)算法,將NDT方法引入遺傳算法作為適應(yīng)度評價,在全局空間進(jìn)行搜索,提高了求解精度的同時避免了算法陷入局部極值,同時將NDT 進(jìn)一步細(xì)分建立查找表以避免遺傳算法進(jìn)化時反復(fù)計算個體適應(yīng)度消耗大量時間,加快了求解速度。

2.1 種群編碼

將二維平面中機(jī)器人的運動用平移和旋轉(zhuǎn)變量表示為T(x,y,θ),初始值由里程計獲取,通過在初始值附近的搜索空間內(nèi),求取修正值ΔT(Δx,Δy,Δθ)修正誤差,采用二進(jìn)制格雷碼編碼,編碼長度為24,均勻隨機(jī)產(chǎn)生N個個體樣本初始化種群。

2.2 適應(yīng)度評價

對于如圖1(a)所示的參考幀Sref,首先劃分cell,統(tǒng)計cell里面的激光點數(shù),然后根據(jù)式(1)和式(2)計算對應(yīng)的均值qj和方差Σj,得到該cell的PDF。進(jìn)一步,將cell進(jìn)行柵格細(xì)分,構(gòu)造查找表,如圖1(b)所示。表中每個單元表示二維平面上的一個激光掃描點,落到相應(yīng)的cellj中就用對應(yīng)的均值qj和方差Σj利用式(3)計算其得分值,查找表的整體尺寸稍大于激光傳感器范圍以覆蓋所有可能的激光點。將Scur變換到Sref坐標(biāo)系下,生成S′cur,對于S′cur中的每一個點,在查找表中直接查找其得分值,作為該點的適應(yīng)度值,顯然,個體適應(yīng)度的值score定義為S′cur中每個點在柵格查找表中的得分之和。

本文采用標(biāo)準(zhǔn)NDT即固定大小尺寸進(jìn)行劃分cell,柵格大小取0.5～2 m。為了解決固定尺寸劃分激光點過于分散的問題,同時采用了K均值聚類算法對激光點進(jìn)行聚類劃分的方法,然后進(jìn)行比對分析,其中采用固定尺寸劃分cell的NGASM簡稱為FNGASM,采用K均值聚類劃分cell的NGASM簡稱為KNGASM。如圖1(c)和圖1(d)所示,分別是固定尺寸劃分cell和K均值聚類劃分cell的NDT圖,圖中越亮的地方表示的分值越高。

圖1 NDT適應(yīng)度查找表

當(dāng)直接將S′cur中的激光點代入式(3)計算時,需要消耗大量時間,而且遺傳算法中進(jìn)行個體適應(yīng)度計算時每個個體都要進(jìn)行求解,所以算法的實時性得不到滿足。本文提出建立查找表,只需在創(chuàng)建查找表時計算一遍得分值,遺傳算法所有個體的得分值直接到表中進(jìn)行查找,節(jié)約了大量時間。地圖離散化帶來的一個缺點是精度下降,可以通過縮小查找表柵格尺寸來一定程度上抵消這種現(xiàn)象,但隨之而來的是創(chuàng)建查找表時間的增加以及內(nèi)存的增長,所以要在兩者之間權(quán)衡。

2.3 選擇、交叉和變異

選擇操作首先求出每個個體適應(yīng)度占總適應(yīng)度的比例λi,設(shè)立閾值λb,直接淘汰λi<λb的M個個體,然后再在λi>λb的個體中按比例依概率隨機(jī)復(fù)制生成M個個體補足種群。

交叉操作采用均勻交叉,pc是交叉率,隨機(jī)生成交叉概率pr,如果pr>pc,對選中的父本進(jìn)行交叉；變異操作直接翻轉(zhuǎn)二進(jìn)制某一位置值,設(shè)定一個極小的變異率pm,以降低突變對種群多樣性產(chǎn)生的不良影響。

2.4 終止準(zhǔn)則

假設(shè)種群的數(shù)量是N,對于種群中的每個個體 ,都有對應(yīng)的里程計數(shù)據(jù)(xi,yi,θi),通過下面的公式求得所有基因的里程計數(shù)據(jù)均值(xmean,ymean)以及所有個體的里程計數(shù)據(jù)的方差

(5)

(6)

當(dāng)方差cov小于某常量C時 ,算法達(dá)到收斂。

融合NDT的2D激光數(shù)據(jù)掃描匹配遺傳算法的流程圖如圖2所示。

圖2 NGASM流程

3 實驗結(jié)果與分析

為了對NGASM算法掃描匹配效果進(jìn)行驗證,選取標(biāo)準(zhǔn)的DNT算法以及同樣創(chuàng)建查找表的GLASM算法進(jìn)行對比,使用MATLAB軟件進(jìn)行實驗。以下實驗數(shù)據(jù)均是在Intel Core—i5—3200M 2.6 GHz雙核處理器平臺的Windows系統(tǒng)上獲取。實驗中查找表柵格大小l=0.05 m,固定尺寸的NDT cell的尺寸L=1 m,種群的數(shù)量N=50,交叉概率pr=0.9,變異概率pm=0.001。實驗環(huán)境如圖3所示,隨機(jī)選取了5個位姿。

圖3 MATLAB實驗環(huán)境

3.1 耗時分析

如表1所示,GLASM僅需0.012 s即創(chuàng)建了查找表,FNGASM花費了0.406 s來創(chuàng)建查找表,是GLASM的3.38倍,而KNGASM在創(chuàng)建表格消耗時間就達(dá)到秒(s)級,用時2.128 0 s,主要是對激光點進(jìn)行聚類時不斷迭代消耗大量時間。在進(jìn)行適應(yīng)度評價時,GLASM和NGASM均直接在查找表中查找,但由于GLASM需要查找每個激光點柵格值以及其四周的柵格值,而NGASM直接查找激光點得分值,NGASM耗費時間比GLASM減少42.8 %。表中完成配準(zhǔn)對GLASM和NGASM兩種遺傳算法來說是進(jìn)化10次,NDT完成配準(zhǔn)僅需1.766 0 s,遺傳算法在實時性方面優(yōu)勢不足,GLASM進(jìn)化10次就耗費了5.092 s,NGASM由于快速查表節(jié)省時間,進(jìn)化10次后兩種不同方式劃分cell的NGASM就分別比GLASM少耗時1.39,0.721 s,隨著進(jìn)化次數(shù)的增加,NGASM相比GLASM在實時性方面優(yōu)勢更加明顯。

表1 不同激光掃描匹配所需的時間 s

3.2 精度分析

如圖3所示,機(jī)器人經(jīng)過隨機(jī)生成的5個點,求解位姿,誤差結(jié)果如表2所示。三種算法都能達(dá)到很高的精度,從MDE指標(biāo)來看,其中GLASM和NDT兩種算法精度接近,GLASM的0.029 25小于NDT的0.3142,表明在平移誤差方面GLASM的精度要高,NGASM精度最高,其中效果最好的KNGASM比誤差最大的NDT精度提高了21.6 %。MRE反映了旋轉(zhuǎn)誤差,NGASM同樣誤差最小,KNGASM比NDT精度提高了27.3 %。綜合表2數(shù)據(jù),NGASM無論是在平移誤差還是在旋轉(zhuǎn)誤差方面,均提高了機(jī)器人的定位精度,其中KNGASM由于考慮了激光點的分布位置,在精度方面得到了進(jìn)一步提升。

表2 不同激光掃描匹配的精度(MDE=每米平移誤差均值；MRE=每米旋轉(zhuǎn)誤差均值；VDE=平移誤差協(xié)方差；VRE=旋轉(zhuǎn)誤差協(xié)方差)

3.3 收斂性

圖4為NGASM算法的收斂圖,共進(jìn)行了50次實驗,可以看出采用FNGASM和KNGASM算法基本都在15代左右收斂,收斂速度較快。

圖4 NGASM算法的收斂性

4 結(jié) 論

通過在仿真環(huán)境中的實驗結(jié)果表明：本文算法提高了移動機(jī)器人的定位精度,相比于GLASM算法,計算速度得到了提升。下一步針對遺傳算法,將進(jìn)行并行改進(jìn),以進(jìn)一步提高算法運行速度,滿足對系統(tǒng)實時性的要求。