王曉君,裴?？?劉紅云

(北京工業(yè)大學(xué)電子信息與控制工程學(xué)院,北京 100124)

一種改進馬氏距離的最近鄰數(shù)據(jù)關(guān)聯(lián)算法

王曉君,裴?？?劉紅云

(北京工業(yè)大學(xué)電子信息與控制工程學(xué)院,北京 100124)

針對最近鄰數(shù)據(jù)關(guān)聯(lián)算法關(guān)聯(lián)正確率較低,容易導(dǎo)致錯誤關(guān)聯(lián)的問題,本文提出了一種改進的最近鄰數(shù)據(jù)關(guān)聯(lián)算法。該算法通過計算傳感器的測量值與地圖已有特征之間數(shù)據(jù)關(guān)聯(lián)的概率,分析了預(yù)測向量和觀測向量的協(xié)方差對數(shù)據(jù)關(guān)聯(lián)算法的影響機制,提出了預(yù)測向量和觀測向量的協(xié)方差輔助計算馬氏距離的改進最近鄰數(shù)據(jù)關(guān)聯(lián)算法,并給出了算法的具體實現(xiàn)流程。最后,通過仿真實驗結(jié)果表明,改進的最近鄰算法在幾乎不增加計算時間的情況下,具有更高的數(shù)據(jù)關(guān)聯(lián)正確率,將該算法應(yīng)用到同時定位與地圖創(chuàng)建中,運動軌跡的估計更接近于實際的軌跡。

同時定位與地圖創(chuàng)建;數(shù)據(jù)關(guān)聯(lián);最近鄰算法;馬氏距離

文獻[1-2]于1986年提出機器人同時定位與創(chuàng)建地圖(simultaneous localization and mapping, SLAM),并成為了在室內(nèi)、室外、海下等多種環(huán)境中解決機器人自主導(dǎo)航的重要技術(shù)。SLAM算法可描述為:首先,移動機器人在運動過程中,為了逐步增量式地建立一個連續(xù)的環(huán)境地圖,根據(jù)自身的傳感器來測量周圍環(huán)境信息;然后,利用數(shù)據(jù)關(guān)聯(lián)對傳感器測量的環(huán)境特征與地圖中保存的特征進行匹配,建立地圖輔助的觀測信息模型;最后,應(yīng)用最優(yōu)估計方法完成對機器人位姿和環(huán)境地圖的同步估計[3]。因此,在SLAM計算過程中,為了完成最后的狀態(tài)估計,數(shù)據(jù)關(guān)聯(lián)是其中關(guān)鍵的條件與基礎(chǔ),在對機器人定位與地圖創(chuàng)建的準(zhǔn)確性上,數(shù)據(jù)關(guān)聯(lián)的正確率對其造成很大的影響。數(shù)據(jù)關(guān)聯(lián)算法必須滿足2個要求:(1)關(guān)聯(lián)準(zhǔn)確率高;(2)算法運行時間少。當(dāng)前,最近鄰域(nearest neighbor,NN)[4]算法、聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)算法[5](joint probabilistic data association,JPDA)以及聯(lián)合相容分枝定界算法[6](joint compatibility branch and bound,JCBB)是最基本的數(shù)據(jù)關(guān)聯(lián)算法。JPDA和JCBB算法雖然數(shù)據(jù)關(guān)聯(lián)正確率略高,但是若所觀測的特征較多時,由于其計算復(fù)雜度較高,限制了其實時應(yīng)用。而NN算法由于其原理簡單,易于實現(xiàn)的特點,仍然是應(yīng)用最為廣泛的數(shù)據(jù)關(guān)聯(lián)算法。但是,NN算法采用馬氏距離最短的特征作為最佳匹配條件,其數(shù)據(jù)關(guān)聯(lián)正確率低,容易造成機器人的狀態(tài)估計誤差。針對NN算法的問題,很多學(xué)者提出了NN算法的改進算法。文獻[7]提出了一種最近鄰聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)方法,其思想是將JPDA算法與NN算法相融合;文獻[8]分析了最近鄰數(shù)據(jù)關(guān)聯(lián)算法的不足,并在此基礎(chǔ)上提出了聯(lián)合兼容性檢驗的關(guān)聯(lián)算法;文獻[9]提出了一種基于最近鄰動態(tài)聯(lián)合數(shù)據(jù)關(guān)聯(lián)算法,該算法采用多幀觀測數(shù)據(jù)的關(guān)聯(lián)結(jié)果來動態(tài)去除觀測特征中的偽特征;文獻[10]在基于馬氏距離規(guī)則最近鄰算法基礎(chǔ)上提出了一種基于多規(guī)則的數(shù)據(jù)關(guān)聯(lián)算法,該算法采用多規(guī)則聯(lián)合匹配的方式,從多個可能的關(guān)聯(lián)假設(shè)中識別出正確的關(guān)聯(lián)假設(shè)。上述幾種算法存在以下不足:1)均通過融合其他關(guān)聯(lián)算法來提高數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性,不可避免的增加了計算復(fù)雜度;2)均重點關(guān)注了關(guān)聯(lián)匹配正確率,并未考慮傳感器的測量值與地圖已有特征之間數(shù)據(jù)相關(guān)聯(lián)的概率,忽略了觀測信息與預(yù)測信息誤差對匹配結(jié)果的影響。

在SLAM計算過程中,錯誤的數(shù)據(jù)關(guān)聯(lián)有以下三種可能:1)由于對機器人運動方程和傳感器觀測方程的線性化處理而導(dǎo)致的數(shù)據(jù)關(guān)聯(lián)假設(shè)錯誤;2)環(huán)境特征密集而造成的測量誤差;3)機器人位置誤差的積累。針對這些問題,最近鄰算法僅依靠馬氏距離并不能得到正確的數(shù)據(jù)關(guān)聯(lián)。本文通過將預(yù)測向量和觀測向量協(xié)方差引入到數(shù)據(jù)關(guān)聯(lián)公式,提出了一種改進的最近鄰關(guān)聯(lián)算法,重點解決由測量誤差所帶來的錯誤數(shù)據(jù)關(guān)聯(lián)問題。將本文提出的改進算法與基于馬氏距離的數(shù)據(jù)關(guān)聯(lián)算法相比較,改進算法在計算復(fù)雜度和系統(tǒng)運行時間兩方面性能幾乎與NN算法相同,但數(shù)據(jù)關(guān)聯(lián)的正確率顯著提高,將改進的算法應(yīng)用到SLAM系統(tǒng)中,仿真結(jié)果表明了改進后的算法估計精度更高,運動軌跡更加接近其真實路徑。

1 基于最近鄰算法的數(shù)據(jù)關(guān)聯(lián)

1.1 SLAM數(shù)據(jù)關(guān)聯(lián)的數(shù)學(xué)模型

數(shù)據(jù)關(guān)聯(lián)是用于完成傳感器的觀測值與地圖中已有特征之間的關(guān)聯(lián)匹配,是SLAM算法實現(xiàn)的關(guān)鍵技術(shù)。

即在一個狀態(tài)向量中,將地圖環(huán)境特征與機器人位姿狀態(tài)存儲進去,通過傳感器來觀測周圍環(huán)境的特征信息并建立觀測向量。利用最優(yōu)估計理論來估計地圖特征和機器人位姿,從而實現(xiàn)對機器人的同時定位與創(chuàng)建地圖,SLAM算法流程如圖1所示[10]。

從圖1中可以看出,數(shù)據(jù)關(guān)聯(lián)是SLAM過程的關(guān)鍵步驟,也是整個算法實現(xiàn)狀態(tài)估計的前提和基礎(chǔ),其關(guān)聯(lián)正確率將直接影響機器人定位與地圖創(chuàng)建的準(zhǔn)確性。

圖1 SLAM算法流程圖

1.2 最近鄰算法

在SLAM過程中,應(yīng)用最近鄰數(shù)據(jù)關(guān)聯(lián)算法的具體步驟如下。

2 改進的最近鄰關(guān)聯(lián)算法

2.1 最近鄰算法的局限性

在數(shù)據(jù)關(guān)聯(lián)過程中,導(dǎo)致錯誤關(guān)聯(lián)的可能性有3種:(1)把已有特征看作新的特征,導(dǎo)致計算復(fù)雜度增加;(2)把新的特征看作已有的特征,有可能導(dǎo)致地圖的發(fā)散;(3)把已有的特征與另一已有的特征相關(guān)聯(lián),從而出現(xiàn)錯誤的數(shù)據(jù)關(guān)聯(lián)。

雖然基于馬氏距離的最近鄰算法考慮了傳感器觀測周圍環(huán)境的概率特性和特征參數(shù)表達的位置信息,但仍然不可避免的存在錯誤關(guān)聯(lián)的可能性,最近鄰算法造成錯誤關(guān)聯(lián)的情況主要有以下三種情況:

(1)由于對機器人的運動方程和傳感器的觀測方程進行了線性化處理,使機器人的位姿和地圖出現(xiàn)過于樂觀的估計,從而使得正確的數(shù)據(jù)關(guān)聯(lián)假設(shè)造成過大的馬氏距離,導(dǎo)致其不能識別正確的數(shù)據(jù)關(guān)聯(lián)假設(shè),出現(xiàn)多重數(shù)據(jù)關(guān)聯(lián)[11],而為了解決該問題,則需要增大馬氏距離的閾值,但增大閾值又會導(dǎo)致計算量的增加,影響計算速度。文獻[11]引用了動態(tài)閾值的概念,對關(guān)聯(lián)門進行限制,從而減少了所需計算的數(shù)據(jù)關(guān)聯(lián)數(shù)量,在不影響數(shù)據(jù)的正確關(guān)聯(lián)率的情形下,縮小了計算時間。

(2)在環(huán)境特征相對密集,并且機器人里程計的誤差又較大的情況下,容易出現(xiàn)錯誤的數(shù)據(jù)關(guān)聯(lián)。

(3)而隨著機器人運動距離的增加,其位置誤差會不斷積累,僅僅依據(jù)馬氏距離進行數(shù)據(jù)關(guān)聯(lián)將無法給出正確的關(guān)聯(lián)結(jié)果。文獻[8]的思想是將數(shù)據(jù)關(guān)聯(lián)的范圍限制在局部的可能區(qū)域中,為了實現(xiàn)這一思想調(diào)整傳感器的有效量程和機器人的位姿,在一定程度上改善了由于位置誤差積累所造成錯誤的數(shù)據(jù)關(guān)聯(lián)假設(shè)。

綜上所述,數(shù)據(jù)關(guān)聯(lián)的正確與否關(guān)鍵在于測量誤差的大小,而最近鄰算法中的馬氏距離作為數(shù)據(jù)關(guān)聯(lián)的依據(jù)無法保證產(chǎn)生正確的數(shù)據(jù)關(guān)聯(lián)結(jié)果,在實際應(yīng)用中,最近鄰算法其中一項復(fù)雜而又難以完成的任務(wù)就是找到一個適合的馬氏距離閾值。本文研究的重點在于如何改進馬氏距離來減小由于測量誤差所帶來的影響,從而提高關(guān)聯(lián)正確率。

2.2 改進的最近鄰算法

針對最近鄰數(shù)據(jù)關(guān)聯(lián)算法的不足,一般的改進方法是引入其他算法來動態(tài)調(diào)整馬氏距離的閾值,從而提高最近鄰算法的準(zhǔn)確性。但是,這些改進方法不可避免的會增加算法的計算復(fù)雜度,同時,這些方法只是關(guān)注了關(guān)聯(lián)匹配過程本身,并未考慮由于傳感器測量誤差帶來的影響。

由(11)式可知,地圖中已有特征與觀測值的關(guān)聯(lián)程度不僅僅與馬氏距離有關(guān),而觀測值和預(yù)測值的協(xié)方差同樣也影響著數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性。

為了闡明觀測值和預(yù)測值的協(xié)方差對數(shù)據(jù)關(guān)聯(lián)準(zhǔn)確性的影響,分別在如圖2所示的兩種不同的情況下進行分析。圖2中,O1和O2表示觀測值, P1和P2表示預(yù)測值,左圖表示兩個觀測值同時對應(yīng)于同一個預(yù)測值,右圖表示同一個觀測值對應(yīng)于兩個預(yù)測值。從左圖可以看出,無論應(yīng)用NN算法還是NLML算法,其數(shù)據(jù)關(guān)聯(lián)的結(jié)果均為O1與P1相匹配。相反,右圖中若應(yīng)用NN算法,其馬氏距離O1P1O1P2,O1與P2相匹配,則得到正確的關(guān)聯(lián)結(jié)果。由以上兩種情況可知,當(dāng)判定數(shù)據(jù)關(guān)聯(lián)時,其馬氏距離的最小化并不等同于NLML的最小化,觀測值和預(yù)測值的協(xié)方差也影響著關(guān)聯(lián)匹配結(jié)果,而當(dāng)環(huán)境相對復(fù)雜時,僅僅應(yīng)用馬氏距離并不能得到正確的關(guān)聯(lián)結(jié)果,而必須考慮觀測值和預(yù)測值的協(xié)方差的影響。

基于以上分析,在關(guān)聯(lián)過程中,本文通過引入觀測與預(yù)測信息,提出了改進馬氏距離的數(shù)據(jù)關(guān)聯(lián)算法,該改進算法通過計算傳感器所觀測到的特征值與地圖中已有特征相關(guān)聯(lián)的可能性概率,并對其所求概率取負(fù)對數(shù)得到,簡稱NLML算法, 當(dāng)NLML取值最小時,則其關(guān)聯(lián)概率最高,同時得到關(guān)聯(lián)最佳匹配。本文提出的數(shù)據(jù)關(guān)聯(lián)算法的具體流程如表1所示。

圖2 NN算法與NLML算法的對比分析

表1 NLML算法的流程

3 仿真與分析

3.1 仿真實驗?zāi)Ｐ?/p>

本文所采用的SLAM問題的數(shù)學(xué)模型為:一個在未知環(huán)境中自主移動的機器人,其模型示意圖如圖3所示。

圖3 移動車輛模型

圖3中,xL與yL是車輛中心在全局坐標(biāo)系下的位置坐標(biāo),L為車輛前軸與后軸之間的距離,a為激光器中心到車輛后軸中心之間的距離,b為激光器中心到車輛中軸之間的距離。移動機器人的運動模型通常用以下模型表示為

式(12)中,X(k)表示機器人在第k步時的運動狀態(tài),u(k)表示機器人的運動控制,w(k)為高斯噪聲,其協(xié)方差為Q(k)。則在全局坐標(biāo)系中機器人的運動模型可以表示為式(13),其中x(k)、y(k)、φ(k)為k時刻機器人的位置和方位角,α表示車輪轉(zhuǎn)過的角度,vc表示車輛行駛的速度,ΔT為采樣時間間隔。則移動機器人的運動模型表示為

圖4 仿真環(huán)境示意圖

SLAM系統(tǒng)的觀測模型如式(14)所示,

式(14)中,Z(k)表示為k時刻觀測到的環(huán)境特征點,h(X(k))為特征點的觀測函數(shù),ε(k)為高斯白噪聲,其協(xié)方差為R(k)。在全局坐標(biāo)系下機器人的觀測模型可表示為

式(15)中,下標(biāo)p表示環(huán)境特征點的坐標(biāo)在機器人坐標(biāo)系下的投影。

3.2 仿真結(jié)果分析

實驗所用的環(huán)境是文獻[8]設(shè)計的仿真實驗平臺,如圖4所示。在仿真試驗中,系統(tǒng)圍繞一個正方形(10 m×10 m)的軌跡移動,總共走了168 步,圖4中圓點表示可觀測的路標(biāo),三角形表示移動機器人,在移動過程中機器人用激光傳感器持續(xù)測量了88個路標(biāo),且路標(biāo)平均分布在軌跡的內(nèi)外兩側(cè)。

圖5分別為采用NN、NLML兩種數(shù)據(jù)關(guān)聯(lián)算法時每一步所需的執(zhí)行時間。NLML算法與NN算法相比,雖然理論上其計算復(fù)雜度有所增加,但從圖5中的仿真結(jié)果可以看出,NLML算法在數(shù)據(jù)關(guān)聯(lián)過程中的執(zhí)行時間并沒有大幅度的延長,每一步的運行時間僅比NN算法平均多用0.007 s。圖6和圖7中黑色區(qū)域表示系統(tǒng)每一步的正確關(guān)聯(lián)率,白色區(qū)域表示系統(tǒng)每一步的錯誤關(guān)聯(lián)率。其中:

圖5 NN和NLML每一步運行時間對比

·True positive:相關(guān)聯(lián)的正確率。

·True negative:新路標(biāo)加入地圖中的正確率。

圖7 基于NLML算法的關(guān)聯(lián)狀況

由圖6中可見,由于受機器人運動模型和觀測模型線性化處理以及里程計誤差較大的影響,基于NN算法的每一步數(shù)據(jù)關(guān)聯(lián)正確率并不高,相比之下,由條件概率所求得的NLML算法,考慮到了傳感器觀測特征與地圖已有特征的所有關(guān)聯(lián)可能性,能夠很好的彌補NN算法的局限性。從圖6和圖7的分析比較可看出,基于NLML算法的每一步關(guān)聯(lián)正確率大大提高,同時將傳感器所觀測到的特征值作為新路標(biāo)加入地圖中的正確率也有很大的提高。

通過圖8可以看出:與NN算法相比較,本文的NLML算法具有更高的定位精度,車輛在x方向、y方向以及角度的定位誤差更小。

圖9和圖10是分別采用NN算法與NLML算法實現(xiàn)的SLAM算法的估計結(jié)果:圖9和圖10中紅色線為車輛的真實路徑,紅色點為環(huán)境的真實路標(biāo)點,藍色線為SLAM系統(tǒng)估計的車輛運動軌跡,藍色點為SLAM系統(tǒng)估計的特征點坐標(biāo)。將圖9和圖10進行對比分析可以看出,相對于基于NN算法的SLAM系統(tǒng),基于NLML算法的SLAM系統(tǒng)所估計的機器人運動軌跡也更接近于真實的路徑,特征點也更接近于真實的特征點。

由以上仿真實驗可以證明,NLML算法有效彌補了NN算法的局限性,同時NLML算法的計算復(fù)雜度并不會影響系統(tǒng)的整體實時性,大大提高了數(shù)據(jù)關(guān)聯(lián)的正確率。相較于NN算法,基于NLML算法的SLAM系統(tǒng)具有更好的估計精度。

4 結(jié)束語

針對最近鄰數(shù)據(jù)關(guān)聯(lián)算法關(guān)聯(lián)準(zhǔn)確度問題,本文通過對數(shù)據(jù)關(guān)聯(lián)的條件概率進行分析,可以得出數(shù)據(jù)關(guān)聯(lián)度的精度不僅僅與馬氏距離有關(guān),觀測值和預(yù)測值的協(xié)方差同樣影響著數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性?；谶@一分析,本文將觀測值和預(yù)測值的協(xié)方差引入到數(shù)據(jù)關(guān)聯(lián)過程中,提出了一種改進馬氏距離計算方法的最近鄰數(shù)據(jù)關(guān)聯(lián)算法,并給出了算法的具體實現(xiàn)流程。最后,通過仿真實驗對提出的改進最近鄰算法進行驗證,通過仿真結(jié)果可以看出,本文提出的改進最近鄰算法在幾乎不影響算法運行時間的情況下,數(shù)據(jù)關(guān)聯(lián)正確率有了明顯的提高。同時,將該算法應(yīng)用到SLAM系統(tǒng)中,仿真結(jié)果表明應(yīng)用該算法的SLAM系統(tǒng)能夠很好地估計出車輛的運動軌跡,更加接近機器人的真實路徑。因此,本文提出的改進最近鄰數(shù)據(jù)關(guān)聯(lián)算法具有更好的計算效率和關(guān)聯(lián)準(zhǔn)確度,更適合于SLAM系統(tǒng)的實際應(yīng)用。

圖8 基于NN和NLML算法定位誤差對比

圖9 基于NN算法的運動軌跡

圖10 基于NLML算法的運動軌跡

[1] DISSANAYAKE M W M G,NEWMAN P,CLARK S,et al.A solution to the simultaneous localization and map building (SLAM)problem[J].IEEE Transactions on Robotics and Automation,2001,17(3):229-241.

[2] SMITH R,SELF M,CHEESEMAN P.Estimating uncertain spatial relationships in robotics[EB/OL].(2005-10-12) [2014-08-12].http://www.cs.uml.edu/～holly/91.549/readings/smith90stochastic.pdf.

[3] DURANT-WHYTE H,BAILEY T.Simultaneous localization and mapping:part I the essential algorithms[J].Robotics and Automation Magazine,2006,13(2):99-110.

[4] BAILEY T.Mobile robot localisation and mapping in extensive outdoor environments[D].Sydney:The University of Sydney,2002.

[5] FITZGERALD R J.Development of practical PDA logic for multitarget tracking by microprocessor[C]//The Institute of Electrical and Electronics Engineers(IEEE).Proceedings of American Control Conference,1986.Seattle:IEEE,1986:889-898.

[6] NIETO J,GUIVANT J,NEBOT E.DenseSLAM:simultaneous localization and dense mapping[J].The International Journal of Robotics Research,2006,25(8):711-744

[7] CHANG K C,BAR S Y.Joint probabilistic data association for multitarget tracking with possibly unresolved measurements and maneuvers[C]//The Institute of Electrical and Electronics Engineers(IEEE).Proceedings of American Control Conference,1983.San Francisco:IEEE,1983:466-471.

[8] NEIRA J,TARDòS J D.Data association in stochastic mapping using the joint compatibility test[J].IEEE Transactions on Robotics and Automation,2001,17(6):890-897.

[9] 周武,趙春霞,張浩峰,等.動態(tài)聯(lián)合最近鄰算法[J].電子學(xué)報,2010,38(2):359-365.

[10]郭帥,馬書根,李斌,等.VorSLAM算法中基于多規(guī)則的數(shù)據(jù)關(guān)聯(lián)方法[J].自動化學(xué)報,2013,39(6):883-894.

[11]HUANG Shoudong,DISSANAYAKE G.Convergence analysis for extended Kalman filter based SLAM[EB/OL].(2006-01-31)[2014-08-12].http://services.eng.uts.edu.au/～sdhuang/ICRA06_438_final_IEEE.pdf.

[12]STONE L D,BARLOW C A,CORWIN T L.Bayesian multiple target tracking[M].Norwood,MA:Artech House,1999.

Novel Nearest Neighbor Data Association Algorithm Based on Improved Mahalanobis Distance

WANG Xiaojun,PEI Fujun,LIU Hongyun
(College of Electronic Information and Control Engineering,Beijing University of Technology,Beijing 100124,China)

In the mobile robot simultaneous localization and map building,nearest neighbor data association algorithm has been widely applied,because of its simple principle and easy to implement,but its correct association is low,and easily to cause the error correlation.In view of its shortcomings,this paper puts forward an improved nearest neighbor data association algorithm.By calculating the probability of the data association between the observed value of the sensor and the existing map features,analysis of the prediction and observation vector covariance influence mechanism on data association algorithm,proposed the prediction vector and the observation vector covariance auxiliary calculating the Mahalanobis distance to improve the nearest neighbor data association algorithm,and gives the specific implementation process of the algorithm.In the end,the simulation results show that the improved mahalanobis distance algorithm can’t affect the overall system uptime,at the same time,its data association has a higher accuracy,applied to the robot positioning and map building,the estimated trajectory is more accurate.

simultaneous localization and mapping;data association;nearest neighbor;mahalanobis distance

P228

A

2095-4999(2015)-04-0050-07

2014-10-18

北京市青年拔尖人才培育計劃(CITTCD201304046)。

王曉君(1990—),女,黑龍江大慶人,碩士生,從事機器人自主導(dǎo)航算法的研究。

王曉君,裴福俊,劉紅云.一種改進馬氏距離的最近鄰數(shù)據(jù)關(guān)聯(lián)算法[J].導(dǎo)航定位學(xué)報,2015,3(4):50-56,73.WANG Xiaojun,PEI Fujun,LIU Hongyun.Novel Nearest Neighbor Data Association Algorithm Based on Improved Mahalanobis Distance[J].Journal of Navigation and Positioning,2015,3(4):50-56,73.

10.16547/j.cnki.10-1096.20150410