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

王曉君,裴?？?劉紅云

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

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

王曉君,裴?？?劉紅云

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

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

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

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

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

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

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

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

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

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

圖1 SLAM算法流程圖

1.2 最近鄰算法

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

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

2.1 最近鄰算法的局限性

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

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

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

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

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

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

2.2 改進(jìn)的最近鄰算法

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

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

為了闡明觀測(cè)值和預(yù)測(cè)值的協(xié)方差對(duì)數(shù)據(jù)關(guān)聯(lián)準(zhǔn)確性的影響,分別在如圖2所示的兩種不同的情況下進(jìn)行分析。圖2中,O1和O2表示觀測(cè)值, P1和P2表示預(yù)測(cè)值,左圖表示兩個(gè)觀測(cè)值同時(shí)對(duì)應(yīng)于同一個(gè)預(yù)測(cè)值,右圖表示同一個(gè)觀測(cè)值對(duì)應(yīng)于兩個(gè)預(yù)測(cè)值。從左圖可以看出,無論應(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)時(shí),其馬氏距離的最小化并不等同于NLML的最小化,觀測(cè)值和預(yù)測(cè)值的協(xié)方差也影響著關(guān)聯(lián)匹配結(jié)果,而當(dāng)環(huán)境相對(duì)復(fù)雜時(shí),僅僅應(yīng)用馬氏距離并不能得到正確的關(guān)聯(lián)結(jié)果,而必須考慮觀測(cè)值和預(yù)測(cè)值的協(xié)方差的影響。

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

圖2 NN算法與NLML算法的對(duì)比分析

表1 NLML算法的流程

3 仿真與分析

3.1 仿真實(shí)驗(yàn)?zāi)Ｐ?/p>

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

圖3 移動(dòng)車輛模型

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

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

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

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

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

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

3.2 仿真結(jié)果分析

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

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

圖5 NN和NLML每一步運(yùn)行時(shí)間對(duì)比

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

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

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

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

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

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

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

4 結(jié)束語

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

圖8 基于NN和NLML算法定位誤差對(duì)比

圖9 基于NN算法的運(yùn)動(dòng)軌跡

圖10 基于NLML算法的運(yùn)動(dòng)軌跡

[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] 周武,趙春霞,張浩峰,等.動(dòng)態(tài)聯(lián)合最近鄰算法[J].電子學(xué)報(bào),2010,38(2):359-365.

[10]郭帥,馬書根,李斌,等.VorSLAM算法中基于多規(guī)則的數(shù)據(jù)關(guān)聯(lián)方法[J].自動(dòng)化學(xué)報(bào),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

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

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

王曉君,裴福俊,劉紅云.一種改進(jìn)馬氏距離的最近鄰數(shù)據(jù)關(guān)聯(lián)算法[J].導(dǎo)航定位學(xué)報(bào),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