摘 要: 聯(lián)合相容分支定界算法(Joint Compatibility Branch and Bound,JCBB)充分考慮傳感器量測(cè)之間的相關(guān)性和重新匹配關(guān)聯(lián)的可能,但計(jì)算量隨觀(guān)測(cè)數(shù)目成指數(shù)增長(zhǎng)。為優(yōu)化其計(jì)算復(fù)雜度和關(guān)聯(lián)準(zhǔn)確度,以最近鄰算法(Nearest Neighbour,NN)進(jìn)行關(guān)聯(lián),對(duì)符合重復(fù)度和經(jīng)過(guò)設(shè)定步數(shù)的情況使用JCBB進(jìn)行特征匹配,并以互斥準(zhǔn)則和最優(yōu)準(zhǔn)則來(lái)提高關(guān)聯(lián)準(zhǔn)確度。引入機(jī)器學(xué)習(xí)領(lǐng)域的評(píng)價(jià)測(cè)度對(duì)改進(jìn)后算法和JCBB算法進(jìn)行比較,結(jié)果表明,改進(jìn)后的關(guān)聯(lián)算法能夠保證更好的關(guān)聯(lián)準(zhǔn)確度。
關(guān)鍵詞: 聯(lián)合相容分支定界算法(JCBB);數(shù)據(jù)關(guān)聯(lián);特征匹配;準(zhǔn)確度
0 引言
數(shù)據(jù)關(guān)聯(lián)源于目標(biāo)跟蹤領(lǐng)域,用于確定傳感器量測(cè)信息和目標(biāo)源之間的對(duì)應(yīng)關(guān)系,錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)不僅影響導(dǎo)航和定位精度,甚至?xí)?dǎo)致整個(gè)關(guān)聯(lián)算法不一致或者發(fā)散[1]。
Singer等人提出的最近鄰(Nearest Neighbor,NN)[2]算法是最早也是最簡(jiǎn)單的數(shù)據(jù)關(guān)聯(lián)方法,當(dāng)觀(guān)測(cè)量和特征之間的統(tǒng)計(jì)距離度量最小或者殘差概率密度最大時(shí)認(rèn)為兩者可以關(guān)聯(lián),在環(huán)境特征密度較大的情況下,容易發(fā)生關(guān)聯(lián)失敗現(xiàn)象。Bar-Shallom和Jaffer提出的概率數(shù)據(jù)關(guān)聯(lián)(Probability Data Association,PDA)算法充分利用過(guò)去一定時(shí)間內(nèi)的數(shù)據(jù)信息,不依賴(lài)于過(guò)去數(shù)據(jù)關(guān)聯(lián)的正確性,提高了算法的收斂性,但對(duì)計(jì)算開(kāi)銷(xiāo)要求有所提高。對(duì)PDA改進(jìn)后的聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)(Joint Probability Data Association,JPDA)[3]算法對(duì)所有可能的關(guān)聯(lián)假設(shè)集合進(jìn)行搜索,并以此為基礎(chǔ)尋找最優(yōu)關(guān)聯(lián)。針對(duì)NN算法忽略環(huán)境特征之間相關(guān)性的問(wèn)題,Jose Neira 等人提出了聯(lián)合相容性檢驗(yàn)(Joint Compatibility test,JC test)算法,檢驗(yàn)一次觀(guān)測(cè)獲得的所有觀(guān)測(cè)和地圖特征之間的聯(lián)合相容性,聯(lián)合相容分支定界(Joint Compatibility Branch and Bound,JCBB)[4]算法能排除一些NN無(wú)法排除的關(guān)聯(lián)假設(shè),結(jié)合分支定界法和相容性的遞增式計(jì)算搜索解釋樹(shù)的方法來(lái)獲得最優(yōu)解。數(shù)據(jù)關(guān)聯(lián)方法還有多假設(shè)數(shù)據(jù)關(guān)聯(lián)[5]、基于圖論的關(guān)聯(lián)算法[6]、惰性數(shù)據(jù)關(guān)聯(lián)[7]、基于信息理論關(guān)聯(lián)[8]等,它們都尋求在計(jì)算復(fù)雜度和關(guān)聯(lián)準(zhǔn)確度之間獲得更好效果,在目標(biāo)跟蹤、數(shù)據(jù)融合等領(lǐng)域[9-11]都有廣泛涉及。
在保證計(jì)算復(fù)雜度不增加的前提下,考慮算法計(jì)算復(fù)雜度和準(zhǔn)確度兩要素,將最近鄰算法與聯(lián)合相容分支定界算法結(jié)合使用,并以互斥準(zhǔn)則、最優(yōu)準(zhǔn)則約束誤關(guān)聯(lián)情況,從而提高關(guān)聯(lián)準(zhǔn)確度,降低錯(cuò)誤關(guān)聯(lián)導(dǎo)致整個(gè)算法發(fā)散的可能,進(jìn)而保證定位和更新精度。
1 問(wèn)題定義
1.1 特征關(guān)聯(lián)
移動(dòng)機(jī)器人在導(dǎo)航過(guò)程中需要構(gòu)建環(huán)境地圖并且確定自身在地圖中的位姿,數(shù)據(jù)關(guān)聯(lián)是機(jī)器人在觀(guān)測(cè)到環(huán)境特征之后對(duì)觀(guān)測(cè)量進(jìn)行分類(lèi)應(yīng)用或整合的過(guò)程,用以確定當(dāng)前的一個(gè)或者多個(gè)觀(guān)測(cè)量是否應(yīng)該對(duì)應(yīng)到地圖中的已有特征以及對(duì)應(yīng)到哪一個(gè)特征。
圖1中三角形表示移動(dòng)機(jī)器人,其頂點(diǎn)作為傳感器所在位置,以圓形表示機(jī)器人構(gòu)建地圖中已有的特征點(diǎn),記為F1~F5,以方形表示當(dāng)前傳感器的觀(guān)測(cè)量z1~z5,橢圓表示以某種距離度量表示的地圖特征的匹配范圍,忽略傳感器所得觀(guān)測(cè)量是虛警信息的情況。z1和z2分別落在F1和F2的可匹配范圍之內(nèi),兩對(duì)觀(guān)測(cè)—特征對(duì)可以進(jìn)行匹配;對(duì)于z3和F3以及z3和F5均可視為匹配對(duì),若僅以直觀(guān)距離最近原則選擇會(huì)舍去其與F5的匹配,因其與F3距離更近;觀(guān)測(cè)量z4未落在任一特征的匹配范圍內(nèi),將其視作待加入地圖的新特征F6。對(duì)所有觀(guān)測(cè)量和特征點(diǎn)進(jìn)行匹配即完成了數(shù)據(jù)關(guān)聯(lián)過(guò)程,如圖1右圖所示的關(guān)聯(lián)結(jié)果直接影響地圖創(chuàng)建中特征位置的更新,以及新特征的加入,進(jìn)而影響機(jī)器人自身定位過(guò)程。
1.2 馬氏距離與卡方檢驗(yàn)
馬氏距離(Mahalanobis Distance)表示數(shù)據(jù)協(xié)方差之間的距離,是計(jì)算兩個(gè)未知樣本集之間相似程度的有效方法。其與歐氏距離的不同在于它考慮不同特征之間的相關(guān)性且與測(cè)量尺度無(wú)關(guān)。對(duì)均值為、協(xié)方差為的N維觀(guān)測(cè)量z,其馬氏距離的平方為:
利用Cholesky分解=CCT替換變量,則以變量y=C-1(z-),得:
由此得馬氏距離的平方服從自由度為N的卡方分布。以符合不同自由度和準(zhǔn)確度要求的卡方分布檢驗(yàn)兩個(gè)樣本集之間是否足夠相似,這種方法在關(guān)聯(lián)問(wèn)題中得到廣泛應(yīng)用。
2 算法
2.1 聯(lián)合相容分支定界算法
JCBB算法的核心是聯(lián)合相容準(zhǔn)則,用以檢驗(yàn)所有觀(guān)測(cè)值與地圖特征點(diǎn)之間的相容性。相容性檢驗(yàn)標(biāo)準(zhǔn)也采用馬氏距離,由于同時(shí)考慮所有特征與機(jī)器人之間的相關(guān)性,其匹配準(zhǔn)確度高于最近鄰算法。對(duì)于一次關(guān)聯(lián)對(duì)應(yīng)假設(shè)集為Hk={j1,j2,…,jk}時(shí),擴(kuò)展卡爾曼濾波過(guò)程中的聯(lián)合觀(guān)測(cè)方程表示為:
在當(dāng)前估計(jì)狀態(tài)處的線(xiàn)性化過(guò)程為:
聯(lián)合信息及其協(xié)方差為:
聯(lián)合相容檢測(cè)準(zhǔn)則為:
其中,d是聯(lián)合觀(guān)測(cè)量的維數(shù),是要求的置信度。如果馬氏距離滿(mǎn)足式(7),則認(rèn)為關(guān)聯(lián)解Hk滿(mǎn)足聯(lián)合相容條件。然后對(duì)關(guān)聯(lián)解空間采用分支定界方法進(jìn)行遍歷,以配對(duì)數(shù)目單調(diào)非減規(guī)則為定界條件,以聯(lián)合相容條件為分支準(zhǔn)則,搜索并最終決定觀(guān)測(cè)值和地圖特征點(diǎn)之間的最佳關(guān)聯(lián)解。
2.2 改進(jìn)算法
初始使用最近鄰算法對(duì)多個(gè)觀(guān)測(cè)值進(jìn)行數(shù)據(jù)關(guān)聯(lián),若無(wú)多個(gè)觀(guān)測(cè)值對(duì)應(yīng)同一個(gè)特征的情況,則接受所得關(guān)聯(lián)結(jié)果。若出現(xiàn)干涉現(xiàn)象則調(diào)用聯(lián)合相容分支定界算法完成關(guān)聯(lián)。若在JCBB關(guān)聯(lián)過(guò)程中仍出現(xiàn)干涉現(xiàn)象,則以一次關(guān)聯(lián)中僅允許一個(gè)地圖特征與一個(gè)觀(guān)測(cè)量完成關(guān)聯(lián),若再有此特征關(guān)聯(lián)則被拒絕,避免多觀(guān)測(cè)量對(duì)同一地圖特征的重復(fù)匹配。
搜索解空間過(guò)程若有多個(gè)符合最大匹配數(shù)目的關(guān)聯(lián)解,最優(yōu)準(zhǔn)則選定為:選擇其中Mahalanobis距離最小的關(guān)聯(lián)解作為關(guān)聯(lián)結(jié)果。
改進(jìn)算法針對(duì)關(guān)于計(jì)算復(fù)雜度的考慮,結(jié)合NN計(jì)算復(fù)雜度低的特點(diǎn),并以相應(yīng)準(zhǔn)則解決JCBB可能存在的干涉現(xiàn)象和存在多個(gè)可能的最優(yōu)解的情況以保證關(guān)聯(lián)準(zhǔn)確性。
3 實(shí)驗(yàn)
3.1 評(píng)定指標(biāo)
引入機(jī)器學(xué)習(xí)中的評(píng)價(jià)測(cè)度對(duì)關(guān)聯(lián)性能進(jìn)行評(píng)定。以真/假(true/false)說(shuō)明判斷正誤,以正/負(fù)(positive/negative)表示判定結(jié)果,正例判定為正例稱(chēng)為真正(true positive,TP),負(fù)例判定為負(fù)例稱(chēng)為真負(fù)(true negative,TN),正例判定為負(fù)例稱(chēng)為假負(fù)(false negative,F(xiàn)N),負(fù)例判定為正例稱(chēng)為假正(false positive,F(xiàn)P)。準(zhǔn)確率(Accuracy)反映關(guān)聯(lián)算法的整體判定能力(能將正例判定為正例,負(fù)例判定為負(fù)例),精確度(Precision)反映判定的正例中真正的正例樣本的比重,召回率(Recall)反映被正確判定的正例占總的正例的比重。用Precision和Recall評(píng)估一種算法,當(dāng)兩者均更高時(shí),才能說(shuō)明分類(lèi)算法的性能更優(yōu)于另一種算法。然而事實(shí)上兩者在某些情況下是矛盾的,采用評(píng)價(jià)測(cè)度F Score(F Measure)可以綜合考慮精確度和召回率,它是二者的加權(quán)調(diào)和平均。Accuracy(記為A)、Precision(記為P)、Recall(記為R)、F Score(記為F),依次定義為:
A=(TP+TN)/(TP+TN+FP+FN)(8)
P=TP/(TP+FP)(9)
R=TP/(TP+FN)(10)
F=((a2+1)P×R)/(a2P+R)(11)
特別地,當(dāng)參數(shù)a=1時(shí),成為最常見(jiàn)的F1 Score(F1 Measure)測(cè)度,即
F1=2P×R/(P+R)(12)
3.2 實(shí)驗(yàn)參數(shù)設(shè)置
仿真實(shí)驗(yàn)設(shè)置為機(jī)器人從世界坐標(biāo)(0,0)出發(fā),在規(guī)模為180 m×250 m的環(huán)境中,沿規(guī)定路徑運(yùn)動(dòng),依據(jù)激光測(cè)距儀傳回的觀(guān)測(cè)信息,對(duì)環(huán)境中的62個(gè)特征點(diǎn)進(jìn)行定位,并最終返回起始點(diǎn)。仿真參數(shù)設(shè)置如下:將生成隨機(jī)噪聲的種子設(shè)置為23,運(yùn)動(dòng)噪聲和觀(guān)測(cè)噪聲的方差分別設(shè)定為:ν=0.7 m/s,?酌=3°;ρ=0.3 m,b=4°。機(jī)器人運(yùn)動(dòng)速度為4 m/s,最大轉(zhuǎn)向角速度為±20°/s,最大轉(zhuǎn)向角±30°,激光最大掃描距離30 m,掃描范圍0°~180°,控制周期和觀(guān)測(cè)周期均為0.1 s,前后輪間距為4 m。
3.3 實(shí)驗(yàn)結(jié)果分析
通過(guò)圖2~圖5針對(duì)不同指標(biāo)比較結(jié)果可知,當(dāng)改進(jìn)前關(guān)聯(lián)算法的準(zhǔn)確率和精確度變低時(shí),改進(jìn)后算法的準(zhǔn)確率和精確度仍保持較高;以召回率和精確度都較高或者以綜合了精確度和召回率的F1 Score作為評(píng)價(jià)原則,改進(jìn)后算法關(guān)聯(lián)性能都優(yōu)于改進(jìn)前。調(diào)整噪聲參數(shù),改進(jìn)后算法仍能保持更優(yōu)的關(guān)聯(lián)性能。
4 結(jié)束語(yǔ)
針對(duì)聯(lián)合相容分支定界算法計(jì)算復(fù)雜度較高的缺點(diǎn),為改進(jìn)其關(guān)聯(lián)性能,考慮最近鄰算法計(jì)算復(fù)雜度低及可能出現(xiàn)的干涉現(xiàn)象和搜索最優(yōu)解可能出現(xiàn)的匹配數(shù)相同的情況,將其與最近鄰算法和最優(yōu)及互斥準(zhǔn)則融合,改進(jìn)算法提高了關(guān)聯(lián)準(zhǔn)確度,降低了錯(cuò)誤關(guān)聯(lián)引起算法發(fā)散的概率,進(jìn)而減少機(jī)器人對(duì)自身位姿和構(gòu)圖的不確定性。在更復(fù)雜環(huán)境下的關(guān)聯(lián)方法選擇和計(jì)算復(fù)雜度處理是待研究的問(wèn)題。
參考文獻(xiàn)
[1] DURRANT-WHYTE H, BAILEY T. Simultaneous localization and mapping: part I[J]. Robotics & Automation Magazine, IEEE, 2006,13(2):99-110.
[2] BEIS J S, LOWE D G. Shape indexing using approximate nearest-neighbour search in high-dimensional spaces[C]. Proceedings of the IEEE 1997 Computer Society Conference on Computer Vision and Pattern Recognition, 1997:1000-1006.
[3] FORTMANN T E, BAR-SHALOM Y, SCHEFFE M. Sonar tracking of multiple targets using joint probabilistic data association[J]. Oceanic Engineering, IEEE Journal of, 1983,8(3): 173-184.
[4] NEIRA J, TARD?魷S J D. Data association in stochastic mapping using the joint compatibility test[J]. Robotics and Automation, IEEE Transactions on, 2001,17(6):890-897.
[5] COX I J, LEONARD J J. Modeling a dynamic environment using a Bayesian multiple hypothesis approach[J]. Artificial Intelligence, 1994,66(2):311-344.
[6] BAILEY T, NEBOT E M, ROSENBLATT J, et al. Data association for mobile robot navigation: a graph theoretic approach[C]. Robotics and Automation, 2000. Proceedings. ICRA′00. IEEE International Conference on. 2000:2512-2517.
[7] H?魧HNEL D, THRUN S, WEGBREIT B, et al. Towards lazy data association in SLAM[C]. Robotics Research. 2005, Springer: 421-431.
[8] KAESS M, DELLAERT F. Covariance recovery from a square root information matrix for data association[J]. Robotics and Autonomous Systems, 2009,57(12):1198-1210.
[9] CHONG C Y, KUMAR S P. Sensor networks: evolution, opportunities, and challenges[J]. Proceedings of the IEEE, 2003, 91(8): 1247-1256.
[10] DALLIL A, OUSSALAH M, OULDALI A. Sensor fusion and target tracking using evidential data association[J]. Sensors Journal, IEEE, 2013,13(1):285-293.
[11] WU Z, THANGALI A, SCLAROFF S, et al. Coupling detection and data association for multiple object tracking[C]. Computer Vision and Pattern Recognition (CVPR), 2012 IEEE Conference on. 2012: 1948-1955.