一種多假設(shè)聯(lián)合相容分支定界算法

一種多假設(shè)聯(lián)合相容分支定界算法

0多假設(shè)聯(lián)合容分析定界算法同時(shí)，定位和地圖創(chuàng)建（sla）也被稱為cml（courrentremotecontroland局部管理），是移動(dòng)機(jī)器實(shí)現(xiàn)獨(dú)立導(dǎo)航的中心問(wèn)題之一。在SALM領(lǐng)域所涉及的重難點(diǎn)問(wèn)題中，數(shù)據(jù)關(guān)聯(lián)是其中之一。數(shù)據(jù)關(guān)聯(lián)源自目標(biāo)跟蹤中的數(shù)據(jù)融合技術(shù)，在SLAM中用于處理不同時(shí)空獲得的傳感器測(cè)量之間，測(cè)量與已有地圖特征之間的對(duì)應(yīng)關(guān)系，以確定它們是否來(lái)自共同源的問(wèn)題，還包括了新特征的確定過(guò)程。由于狀態(tài)估計(jì)是SLAM問(wèn)題的核心，而數(shù)據(jù)關(guān)聯(lián)又是狀態(tài)估計(jì)的基礎(chǔ)，因此直接影響到最終的定位與建圖結(jié)果，不準(zhǔn)確的數(shù)據(jù)關(guān)聯(lián)甚至?xí)?dǎo)致SLAM的發(fā)散在SLAM數(shù)據(jù)關(guān)聯(lián)問(wèn)題的求解方法中，經(jīng)典的數(shù)據(jù)關(guān)聯(lián)方法有最鄰近算法(NearestNeighbor,NN)隨著機(jī)器人實(shí)際使用環(huán)境的日益復(fù)雜化，環(huán)境中各種不確定因素的干擾等進(jìn)一步加大，需要建立更為有效的SLAM數(shù)據(jù)關(guān)聯(lián)方法。為此，本文將JCBB算法融入到MHT的框架之下，吸取兩種算法的優(yōu)點(diǎn)，提出了一種多假設(shè)聯(lián)合相容分支定界算法(MultiHypothesisJointCompatibilityBranchandBound,MHJCBB)。與NN、JCBB等算法每個(gè)時(shí)刻只保留了1個(gè)最優(yōu)關(guān)聯(lián)假設(shè)不同，MHJCBB算法在實(shí)施數(shù)據(jù)關(guān)聯(lián)時(shí)，保留了多個(gè)聯(lián)合相容的關(guān)聯(lián)結(jié)果，形成多個(gè)機(jī)器人航跡假設(shè)分支，并計(jì)算每個(gè)航跡假設(shè)分支得分。為減少計(jì)算量保證計(jì)算效率，將得分較低的假設(shè)分支在剪枝過(guò)程中去除，而得分最高的假設(shè)分支則被選擇輸出。試驗(yàn)結(jié)果證明了MHJCBB方法的有效性。1地理單元模型特征的位置坐標(biāo)采用經(jīng)典的基于擴(kuò)展卡爾曼濾波(ExtendedKalmanFilter,EKF)方法求解SLAM問(wèn)題。定義系統(tǒng)在時(shí)刻k的狀態(tài)變量式中:X其中的分量分別為各地圖特征的位置坐標(biāo)，對(duì)于靜態(tài)地圖特征，其位置坐標(biāo)為常值。EKF假設(shè)系統(tǒng)狀態(tài)為高斯分布，k時(shí)刻的狀態(tài)可用估計(jì)均值X式中:P在使用概率方法求解SLAM問(wèn)題時(shí)，通常將運(yùn)動(dòng)與觀測(cè)模型按照一個(gè)馬爾可夫過(guò)程來(lái)處理，即根據(jù)系統(tǒng)前一時(shí)刻的狀態(tài)來(lái)預(yù)測(cè)后一個(gè)時(shí)刻的狀態(tài)，與其他歷史狀態(tài)無(wú)關(guān)，由此可進(jìn)行遞推處理。系統(tǒng)的運(yùn)動(dòng)模型描述如下:式中:f在EKFSLAM中，利用運(yùn)動(dòng)模型進(jìn)行預(yù)測(cè)的過(guò)程為式中:觀測(cè)方程為式中:b(·)是狀態(tài)變量X機(jī)器人在獲得環(huán)境的最新探測(cè)數(shù)據(jù)后，可對(duì)狀態(tài)向量的預(yù)測(cè)值予以濾波更新:式中:2數(shù)據(jù)關(guān)聯(lián)檢驗(yàn)SLAM時(shí)，機(jī)器人利用自身傳感器探測(cè)環(huán)境，在k時(shí)刻，傳感器探測(cè)得到數(shù)據(jù)z將每一個(gè)觀測(cè)z假如已有數(shù)據(jù)關(guān)聯(lián)結(jié)果H聯(lián)合新息及協(xié)方差為式中:如果聯(lián)合馬氏距離滿足則認(rèn)為這種關(guān)聯(lián)結(jié)果是可接受的，即觀測(cè)zNeira等3多因素聯(lián)合評(píng)價(jià)方法3.1數(shù)據(jù)關(guān)聯(lián)算法JCBB算法根據(jù)所有觀測(cè)值的綜合關(guān)聯(lián)情況來(lái)確定關(guān)聯(lián)解，以提高關(guān)聯(lián)的正確率。但在不確定性較多的復(fù)雜場(chǎng)合，這種處理方法也易造成誤關(guān)聯(lián)，使得關(guān)聯(lián)正確率明顯下降。MHT主要用于處理非常復(fù)雜環(huán)境下的目標(biāo)跟蹤問(wèn)題，它將可能的關(guān)聯(lián)情況都作為一種假設(shè)分支保留下來(lái)，通過(guò)多周期的信息積累，來(lái)判斷那種關(guān)聯(lián)選擇是最佳的，并且可以對(duì)之前的錯(cuò)誤關(guān)聯(lián)進(jìn)行回溯改正，但計(jì)算復(fù)雜度高。吸取MHT算法和JCBB算法的優(yōu)點(diǎn)，設(shè)計(jì)了一種可用于復(fù)雜環(huán)境中SLAM的數(shù)據(jù)關(guān)聯(lián)算法MHJCBB。常規(guī)JCBB算法在數(shù)據(jù)關(guān)聯(lián)時(shí)僅保留配對(duì)數(shù)最大且聯(lián)合馬氏距離最小的1個(gè)關(guān)聯(lián)假設(shè)，即使這個(gè)關(guān)聯(lián)解是錯(cuò)誤的，在后續(xù)過(guò)程中也無(wú)法進(jìn)行修改。因此，為防止誤關(guān)聯(lián)，MHJCBB算法保留了配對(duì)數(shù)最大且聯(lián)合馬氏距離最小的N3.2假定分支機(jī)構(gòu)生成航跡假設(shè)分支的生成過(guò)程如圖2所示。在k=1時(shí)刻，保留了N3.3航跡得分tracksceare對(duì)于每條航跡假設(shè)分支，航跡的似然比可以用下式迭代計(jì)算:式中:P采用對(duì)數(shù)似然比(LogLikelihoodRatio)來(lái)表示航跡得分(TrackScore):因此，航跡得分的遞歸計(jì)算方式為在N-Best剪枝階段，根據(jù)航跡得分的高低，保留得分高的N個(gè)航跡假設(shè)分支。與MHT在目標(biāo)跟蹤領(lǐng)域應(yīng)用時(shí)存在多個(gè)目標(biāo)共享觀測(cè)導(dǎo)致要計(jì)算最優(yōu)假設(shè)分支組合的情況不同，MHJCBB不存在組合爆炸問(wèn)題，因?yàn)镾LAM問(wèn)題類似于單目標(biāo)跟蹤的問(wèn)題，不存在假設(shè)分支相容組合爆炸問(wèn)題，航跡得分最大的航跡假設(shè)分支即為最優(yōu)假設(shè)。3.4航跡假設(shè)分支機(jī)構(gòu)MHJCBB算法的性能和計(jì)算量主要通過(guò)以下2個(gè)參數(shù)進(jìn)行控制:關(guān)聯(lián)假設(shè)的保留數(shù)目NMHJCBB整個(gè)算法的計(jì)算量最終取決于保留的航跡假設(shè)分支數(shù)，與航跡假設(shè)分支數(shù)N大致成倍數(shù)關(guān)系。因?yàn)樾纬啥鄠€(gè)關(guān)聯(lián)假設(shè)與常規(guī)JCBB計(jì)算量幾乎差不多，而航跡得分計(jì)算中的v另外，需要注意的是，由于狀態(tài)濾波更新的計(jì)算量比較大，可放在N-Best剪枝后進(jìn)行，避免對(duì)被剪枝的航跡分支進(jìn)行狀態(tài)更新。4試驗(yàn)與結(jié)果分析4.1位姿運(yùn)動(dòng)方程在SLAM的狀態(tài)向量中，機(jī)器人的位置和姿態(tài)可表示為通過(guò)航跡推算可獲得k時(shí)刻機(jī)器人位姿的相對(duì)變化量運(yùn)動(dòng)方程為觀測(cè)方程為式中，(x激光雷達(dá)的最大量程為4m，角度探測(cè)范圍為[-π/2，π/2]。傳感器測(cè)量均方根誤差為σ，其中距離每米的誤差為0.01m，角度誤差為0.2°。4.2測(cè)量誤差對(duì)關(guān)聯(lián)正確率的影響試驗(yàn)環(huán)境為一方形走廊，其中共有168個(gè)靜態(tài)特征，為模擬實(shí)際環(huán)境中的運(yùn)動(dòng)物體的干擾，假設(shè)在該區(qū)域內(nèi)存在10個(gè)處于隨機(jī)移動(dòng)狀態(tài)的物體，物體的移動(dòng)速度為0.05m/s，加速度為0.01m/s利用MonteCarlo方法重復(fù)進(jìn)行20次試驗(yàn)，并將獲得數(shù)據(jù)進(jìn)行平均用于比較分析。參數(shù)設(shè)置如下:P當(dāng)測(cè)量誤差為σ時(shí)，NN、JCBB和MHJCBB3種方法數(shù)據(jù)關(guān)聯(lián)正確率的結(jié)果對(duì)比如圖4所示。圖5、6分別是此時(shí)x、y方向的估計(jì)誤差比較。從圖中可以看出，JCBB和MHJCBB都取得了90%的關(guān)聯(lián)正確率，差別不明顯，但是NN由于誤差的積累，在后期關(guān)聯(lián)正確率迅速下滑，導(dǎo)致估計(jì)誤差迅速的增大，SLAM算法發(fā)散。測(cè)量誤差為2σ時(shí)，3種算法數(shù)據(jù)關(guān)聯(lián)正確率的比較如圖7所示?？梢钥闯龃藭r(shí)MHJCBB開(kāi)始體現(xiàn)出優(yōu)勢(shì)，尤其在50～130步之間其關(guān)聯(lián)結(jié)果明顯優(yōu)于JCBB算法。進(jìn)一步增加測(cè)量的不確定性，當(dāng)測(cè)量誤差達(dá)到4σ時(shí)，關(guān)聯(lián)正確率的結(jié)果對(duì)比見(jiàn)圖8。從圖8可知，隨著測(cè)量誤差不斷增加，JCBB的關(guān)聯(lián)正確率會(huì)突然下降，類似于圖4中NN的表現(xiàn)，很容易造成SLAM算法的發(fā)散。但是MHJCBB卻保持了較好的性能，正確率都在0.72以上，而JCBB后期接近于0.42,MHJCBB優(yōu)勢(shì)明顯。試驗(yàn)結(jié)果表明，傳感器測(cè)量誤差的增大，會(huì)引起整個(gè)處理過(guò)程不確定性的增加，此時(shí)NN、JCBB算法的關(guān)聯(lián)準(zhǔn)確率都會(huì)隨之下降，易造成觀測(cè)與特征之間的配對(duì)出錯(cuò)，影響機(jī)器人的位姿估計(jì)，并使得所建立的地圖不確定性增加，甚至?xí)?dǎo)致SLAM算法的發(fā)散。而MHJCBB算法則通過(guò)保留多個(gè)航跡假設(shè)分支，獲得了較好的穩(wěn)定性和可靠性。5多假設(shè)跟蹤的數(shù)據(jù)關(guān)聯(lián)算法數(shù)據(jù)關(guān)聯(lián)問(wèn)題的求解是SLAM研究領(lǐng)域中的關(guān)鍵難點(diǎn)之一，尤其對(duì)于經(jīng)常使用的經(jīng)典濾波算法如卡爾曼濾波等，少數(shù)的幾次誤關(guān)聯(lián)便容易導(dǎo)致算法的發(fā)散。JCBB算法是SLAM數(shù)據(jù)關(guān)聯(lián)時(shí)廣泛使用且行之有效的一種數(shù)據(jù)關(guān)聯(lián)方法，但當(dāng)環(huán)境中的不確定性增加時(shí)，其關(guān)聯(lián)正確率將會(huì)出現(xiàn)明顯下降。文中將多假設(shè)跟蹤(MHT)算法與JCBB算法的優(yōu)點(diǎn)有機(jī)結(jié)合，提出了一種基于多假設(shè)跟蹤的MHJCBB。該算法的