一種改進馬氏距離的最近鄰數據關聯算法

王曉君,裴?？?劉紅云

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

一種改進馬氏距離的最近鄰數據關聯算法

王曉君,裴福俊,劉紅云

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

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

同時定位與地圖創建;數據關聯;最近鄰算法;馬氏距離

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

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

1 基于最近鄰算法的數據關聯

1.1 SLAM數據關聯的數學模型

數據關聯是用于完成傳感器的觀測值與地圖中已有特征之間的關聯匹配,是SLAM算法實現的關鍵技術。

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

從圖1中可以看出,數據關聯是SLAM過程的關鍵步驟,也是整個算法實現狀態估計的前提和基礎,其關聯正確率將直接影響機器人定位與地圖創建的準確性。

圖1 SLAM算法流程圖

1.2 最近鄰算法

在SLAM過程中,應用最近鄰數據關聯算法的具體步驟如下。

2 改進的最近鄰關聯算法

2.1 最近鄰算法的局限性

在數據關聯過程中,導致錯誤關聯的可能性有3種:(1)把已有特征看作新的特征,導致計算復雜度增加;(2)把新的特征看作已有的特征,有可能導致地圖的發散;(3)把已有的特征與另一已有的特征相關聯,從而出現錯誤的數據關聯。

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

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

(2)在環境特征相對密集,并且機器人里程計的誤差又較大的情況下,容易出現錯誤的數據關聯。

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

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

2.2 改進的最近鄰算法

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

由(11)式可知,地圖中已有特征與觀測值的關聯程度不僅僅與馬氏距離有關,而觀測值和預測值的協方差同樣也影響著數據關聯的準確性。

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

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

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

表1 NLML算法的流程

3 仿真與分析

3.1 仿真實驗模型

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

圖3 移動車輛模型

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

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

圖4 仿真環境示意圖

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

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

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

3.2 仿真結果分析

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

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

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

·True positive:相關聯的正確率。

·True negative:新路標加入地圖中的正確率。

圖7 基于NLML算法的關聯狀況

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

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

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

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

4 結束語

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

圖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] 周武,趙春霞,張浩峰,等.動態聯合最近鄰算法[J].電子學報,2010,38(2):359-365.

[10]郭帥,馬書根,李斌,等.VorSLAM算法中基于多規則的數據關聯方法[J].自動化學報,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—),女,黑龍江大慶人,碩士生,從事機器人自主導航算法的研究。

王曉君,裴?？?劉紅云.一種改進馬氏距離的最近鄰數據關聯算法[J].導航定位學報,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