一種多假設(shè)聯(lián)合相容SLAM數(shù)據(jù)的關(guān)聯(lián)方法

徐伊岑，曹小兵，郭劍輝

（1.無錫商業(yè)職業(yè)技術(shù)學(xué)院機(jī)電技術(shù)學(xué)院，江蘇無錫 214153；2.無錫職業(yè)技術(shù)學(xué)院控制技術(shù)學(xué)院，江蘇無錫 214121；3.南京理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院，南京 210094）

0 引言

同時(shí)定位與地圖創(chuàng)建（Simultaneous Localization and Mapping，SLAM）也稱CML（Concurrent Mapping and Localization）問題，是移動(dòng)機(jī)器人實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵問題之一［1-5］，其目的是使機(jī)器人在缺乏先驗(yàn)信息的未知環(huán)境中憑借所攜帶的傳感器構(gòu)建所在環(huán)境的地圖（模型），同時(shí)結(jié)合已創(chuàng)建的地圖對(duì)自身進(jìn)行定位，建圖與定位兩者密切相關(guān)、相互制約。1986 年Smith等［6］提出SLAM 問題起，就吸引了大量的研究者，迄今為止已分別基于濾波器與圖優(yōu)化兩種不同的思路提出了多種實(shí)現(xiàn)方法［7-8］。

在SALM領(lǐng)域所涉及的重難點(diǎn)問題中，數(shù)據(jù)關(guān)聯(lián)是其中之一。數(shù)據(jù)關(guān)聯(lián)源自目標(biāo)跟蹤中的數(shù)據(jù)融合技術(shù)，在SLAM 中用于處理不同時(shí)空獲得的傳感器測量之間，測量與已有地圖特征之間的對(duì)應(yīng)關(guān)系，以確定它們是否來自共同源的問題，還包括了新特征的確定過程。由于狀態(tài)估計(jì)是SLAM 問題的核心，而數(shù)據(jù)關(guān)聯(lián)又是狀態(tài)估計(jì)的基礎(chǔ)，因此直接影響到最終的定位與建圖結(jié)果，不準(zhǔn)確的數(shù)據(jù)關(guān)聯(lián)甚至?xí)?dǎo)致SLAM 的發(fā)散［9］。

在SLAM數(shù)據(jù)關(guān)聯(lián)問題的求解方法中，經(jīng)典的數(shù)據(jù)關(guān)聯(lián)方法有最鄰近算法（Nearest Neighbor，NN）［10］、多假設(shè)跟蹤算法（Multi-Hypothesis Tracker，MHT）［11］以及聯(lián)合相容分枝定界算法（Joint Compatibility Branch and Bound，JCBB）［12］等。JCBB算法主要存在兩點(diǎn)不足：①觀測數(shù)較多時(shí)，計(jì)算量成指數(shù)增加；②當(dāng)環(huán)境干擾因素造成過程不確定增加時(shí)，JCBB的關(guān)聯(lián)準(zhǔn)確度也會(huì)出現(xiàn)明顯下降。這是因?yàn)镴CBB算法是將含有最大配對(duì)數(shù)目的1 個(gè)關(guān)聯(lián)假設(shè)作為結(jié)果，易出現(xiàn)雖然所有觀測都獲得了特征匹配，但實(shí)際上其中的部分觀測卻是新特征或虛警的情況。另一方面，當(dāng)不確定性較大時(shí)，含有最大配對(duì)數(shù)的關(guān)聯(lián)假設(shè)往往不止一個(gè)［13］，但JCBB 算法僅從其中選擇最先得到那個(gè)關(guān)聯(lián)假設(shè)作為結(jié)果，也易引起誤關(guān)聯(lián)。目前，對(duì)JCBB算法的各種改進(jìn)［14-15］，如快速JCBB（FJCBB）算法等，主要是圍繞前者，即解決計(jì)算復(fù)雜度問題，而有關(guān)提升JCBB算法關(guān)聯(lián)準(zhǔn)確度的研究則較少。

隨著機(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)合相容分支定界算法（Multi-Hypothesis Joint Compatibility Branch and Bound，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è)分支在剪枝過程中去除，而得分最高的假設(shè)分支則被選擇輸出。試驗(yàn)結(jié)果證明了MHJCBB方法的有效性。

1 擴(kuò)展卡爾曼濾波的SLAM建模

采用經(jīng)典的基于擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）方法求解SLAM問題。

定義系統(tǒng)在時(shí)刻k的狀態(tài)變量

式中：Xvk＝［xvkyvkΦvk］T，其中的分量分別為機(jī)器人的位置坐標(biāo)與姿態(tài)角；

其中的分量分別為各地圖特征的位置坐標(biāo)，對(duì)于靜態(tài)地圖特征，其位置坐標(biāo)為常值。EKF 假設(shè)系統(tǒng)狀態(tài)為高斯分布，k時(shí)刻的狀態(tài)可用估計(jì)均值Xk和協(xié)方差Pk來表示，

式中：Pv是機(jī)器人位置和姿態(tài)的估計(jì)協(xié)方差矩陣；Pm是地圖特征位置的估計(jì)協(xié)方差矩陣；Pvm是機(jī)器人位姿和地圖特征位置的交叉協(xié)方差矩陣。

在使用概率方法求解SLAM 問題時(shí)，通常將運(yùn)動(dòng)與觀測模型按照一個(gè)馬爾可夫過程來處理，即根據(jù)系統(tǒng)前一時(shí)刻的狀態(tài)來預(yù)測后一個(gè)時(shí)刻的狀態(tài)，與其他歷史狀態(tài)無關(guān)，由此可進(jìn)行遞推處理。系統(tǒng)的運(yùn)動(dòng)模型描述如下：

式中：fv（·）是非線性函是機(jī)器人操縱向量；uk＋1是機(jī)器人速度；αk＋1是舵角；wk＋1是高斯白噪聲，方差為Qk；Xvk＋1可利用航跡推算得到。

在EKF SLAM 中，利用運(yùn)動(dòng)模型進(jìn)行預(yù)測的過程為

觀測方程為

式中：b（·）是狀態(tài)變量Xk的非線性函數(shù)；nk是均值為零方差為Rk的高斯噪聲。

機(jī)器人在獲得環(huán)境的最新探測數(shù)據(jù)后，可對(duì)狀態(tài)向量的預(yù)測值予以濾波更新：

2 聯(lián)合相容分枝定界算法

SLAM時(shí)，機(jī)器人利用自身傳感器探測環(huán)境，在k時(shí)刻，傳感器探測得到數(shù)據(jù)zk，i（i ＝1，2，…，m），假設(shè)zk，i來自環(huán)境特征Ei。進(jìn)行關(guān)聯(lián)目的是要確定觀測zk，i與地圖中已有的特征Fj（j ＝1，2，…，n）的關(guān)系，并得到關(guān)聯(lián)假設(shè)

將每一個(gè)觀測zk，i與地圖特征Fji配對(duì)。如果某個(gè)觀測是新特征或者虛警，則在Hm中用某個(gè)值ji＝0 表示。

假如已有數(shù)據(jù)關(guān)聯(lián)結(jié)果Hm＝｛j1，j2，…，jm｝，聯(lián)合觀測方程為

聯(lián)合新息及協(xié)方差為

式中：

如果聯(lián)合馬氏距離滿足

則認(rèn)為這種關(guān)聯(lián)結(jié)果是可接受的，即觀測zk可與Hm中相應(yīng)的地圖特征配對(duì)。式（12）采用χ2檢驗(yàn)，自由度d ＝dim（zk），置信水平為1－α，可取95%可在χ2分布表中查得結(jié)果。

Neira等［12］采用聯(lián)合相容作為關(guān)聯(lián)約束，通過分枝定界搜索算法搜索關(guān)聯(lián)空間，提出了JCBB 方法。JCBB期望每次盡量讓每個(gè)觀測與特征配對(duì)成功，它將配對(duì)數(shù)目單調(diào)非減規(guī)則作為剪枝策略去除不可能導(dǎo)出最優(yōu)解的節(jié)點(diǎn)，以減少搜索量，并將聯(lián)合馬氏距離最小規(guī)則用于啟發(fā)搜索新節(jié)點(diǎn)，優(yōu)先搜索相容性效果更好的節(jié)點(diǎn)，從而保證得到的結(jié)果是最優(yōu)的。

3 多假設(shè)聯(lián)合相容分枝定界算法

3.1 算法的提出

JCBB算法根據(jù)所有觀測值的綜合關(guān)聯(lián)情況來確定關(guān)聯(lián)解，以提高關(guān)聯(lián)的正確率。但在不確定性較多的復(fù)雜場合，這種處理方法也易造成誤關(guān)聯(lián)，使得關(guān)聯(lián)正確率明顯下降。MHT 主要用于處理非常復(fù)雜環(huán)境下的目標(biāo)跟蹤問題，它將可能的關(guān)聯(lián)情況都作為一種假設(shè)分支保留下來，通過多周期的信息積累，來判斷那種關(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ù)過程中也無法進(jìn)行修改。因此，為防止誤關(guān)聯(lián)，MHJCBB 算法保留了配對(duì)數(shù)最大且聯(lián)合馬氏距離最小的NH個(gè)關(guān)聯(lián)假設(shè)，通過同時(shí)維持多個(gè)關(guān)聯(lián)假設(shè)的方式來保護(hù)可能被忽略的最優(yōu)關(guān)聯(lián)解，以提高數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確率。NH為常數(shù)，實(shí)際應(yīng)用時(shí)可根據(jù)需要設(shè)置。MHJCBB算法的主要步驟為：①機(jī)器人位置預(yù)測，按式（5）、（6）計(jì)算；②進(jìn)行JCBB 數(shù)據(jù)關(guān)聯(lián)，保留多個(gè)關(guān)聯(lián)假設(shè)；③根據(jù)關(guān)聯(lián)假設(shè)，生成多個(gè)機(jī)器人航跡假設(shè)分支；④按照定義的評(píng)價(jià)函數(shù)，計(jì)算航跡假設(shè)分支得分；⑤N-Best 剪枝，只保留得分最高的N個(gè)假設(shè)分支；⑥輸出得分最高的航跡假設(shè)分支。圖1 給出了MHJCBB算法的具體處理流程。

圖1 MHJCBB算法流程圖

3.2 假設(shè)分支的生成

航跡假設(shè)分支的生成過程如圖2 所示。在k ＝1時(shí)刻，保留了NH個(gè)關(guān)聯(lián)假設(shè)H1，H2，…，HNH，根據(jù)關(guān)聯(lián)假設(shè)，生成NH條航跡假設(shè)分支，對(duì)于每條航跡假設(shè)分支，在k ＝2 時(shí)刻可分別生成NH條航跡假設(shè)分支，此時(shí)可判斷航跡假設(shè)分支數(shù)目是否超過最大允許值N，如沒有，在k ＝3 時(shí)刻同理進(jìn)行分支。如果假設(shè)分支數(shù)目大于允許的數(shù)目N，則需要進(jìn)行剪枝處理，圖中虛線表示已刪除的假設(shè)分支。

圖2 航跡假設(shè)分支示意圖

3.3 假設(shè)分支得分評(píng)估

對(duì)于每條航跡假設(shè)分支，航跡的似然比可以用下式迭代計(jì)算：

式中：PD是特征檢測概率；PFA是特征虛警概率；pFA是干擾目標(biāo)的概率密度函數(shù)分別表示新息與新息協(xié)方差（見式（11））；N（·）表示高斯分布。

采用對(duì)數(shù)似然比（Log Likelihood Ratio）來表示航跡得分（Track Score）：

因此，航跡得分的遞歸計(jì)算方式為

在N-Best剪枝階段，根據(jù)航跡得分的高低，保留得分高的N個(gè)航跡假設(shè)分支。與MHT 在目標(biāo)跟蹤領(lǐng)域應(yīng)用時(shí)存在多個(gè)目標(biāo)共享觀測導(dǎo)致要計(jì)算最優(yōu)假設(shè)分支組合的情況不同，MHJCBB不存在組合爆炸問題，因?yàn)镾LAM 問題類似于單目標(biāo)跟蹤的問題，不存在假設(shè)分支相容組合爆炸問題，航跡得分最大的航跡假設(shè)分支即為最優(yōu)假設(shè)。

3.4 計(jì)算量分析

MHJCBB算法的性能和計(jì)算量主要通過以下2 個(gè)參數(shù)進(jìn)行控制：關(guān)聯(lián)假設(shè)的保留數(shù)目NH以及允許保留的最大航跡假設(shè)分支數(shù)N。一般NH可取2～4，以保證假設(shè)關(guān)聯(lián)的多樣性。多假設(shè)跟蹤雖然是延遲決策的方式，但每個(gè)時(shí)間周期可以輸出當(dāng)前最優(yōu)的，以保證實(shí)時(shí)性。

MHJCBB整個(gè)算法的計(jì)算量最終取決于保留的航跡假設(shè)分支數(shù)，與航跡假設(shè)分支數(shù)N 大致成倍數(shù)關(guān)系。因?yàn)樾纬啥鄠€(gè)關(guān)聯(lián)假設(shè)與常規(guī)JCBB 計(jì)算量幾乎差不多，而航跡得分計(jì)算中在JCBB 中也是本來要計(jì)算的，所以計(jì)算得分不涉及矩陣運(yùn)算，只增加了對(duì)標(biāo)量按式（16）計(jì)算；其次是得分排序，排序的復(fù)雜度為O（N2），由于N一般可取10 左右，增加計(jì)算量也是很有限的。所以，每條航跡假設(shè)分支的計(jì)算量與常規(guī)的SLAM算法計(jì)算量是大致一樣的。

另外，需要注意的是，由于狀態(tài)濾波更新的計(jì)算量比較大，可放在N-Best 剪枝后進(jìn)行，避免對(duì)被剪枝的航跡分支進(jìn)行狀態(tài)更新。

4 試驗(yàn)與結(jié)果分析

4.1 SLAM模型

在SLAM的狀態(tài)向量中，機(jī)器人的位置和姿態(tài)可表示為

通過航跡推算可獲得k時(shí)刻機(jī)器人位姿的相對(duì)變化量

運(yùn)動(dòng)方程為

觀測方程為

式中，（xj，yj）為特征的平面坐標(biāo)。

激光雷達(dá)的最大量程為4 m，角度探測范圍為［－π／2，π／2］。傳感器測量均方根誤差為σ，其中距離每米的誤差為0.01 m，角度誤差為0.2°。

4.2 試驗(yàn)結(jié)果分析

試驗(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.05 m／s，加速度為0.01 m／s2，初始位置隨機(jī)。移動(dòng)機(jī)器人從（0，0）點(diǎn)開始按逆時(shí)針方向勻速穿過該區(qū)域，行駛至轉(zhuǎn)角處時(shí)機(jī)器人以原地轉(zhuǎn)動(dòng)方式實(shí)現(xiàn)90°轉(zhuǎn)彎，每次轉(zhuǎn)彎分10 次完成。圖3 給出了試驗(yàn)環(huán)境中的特征分布、機(jī)器人及干擾物體運(yùn)動(dòng)情況，其中：“*”是特征的位置分布；“△”是機(jī)器人理論行駛路徑；“---”是干擾物體運(yùn)動(dòng)軌跡。試驗(yàn)將在不同的測量誤差條件下測試數(shù)據(jù)關(guān)聯(lián)的正確率。

圖3 特征分布、機(jī)器人理論路徑及移動(dòng)物體軌跡

利用Monte Carlo 方法重復(fù)進(jìn)行20 次試驗(yàn)，并將獲得數(shù)據(jù)進(jìn)行平均用于比較分析。參數(shù)設(shè)置如下：PD＝0.9，PFA＝0.05，pFA＝0.1，N ＝12，NH＝2。數(shù)據(jù)關(guān)聯(lián)準(zhǔn)確率定義為該時(shí)刻正確配對(duì)的觀測個(gè)數(shù)（包括觀測與特征正確對(duì)應(yīng)、虛警判別正確兩部分）與參與配對(duì)的總觀測個(gè)數(shù)的比值。

當(dāng)測量誤差為σ時(shí)，NN、JCBB和MHJCBB 3 種方法數(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ā)散。

圖4 測量誤差為σ時(shí)關(guān)聯(lián)正確率比較

圖5 測量誤差為σ時(shí)x方向誤差比較

圖6 測量誤差為σ時(shí)y方向誤差比較

圖7 測量誤差為2σ時(shí)關(guān)聯(lián)正確率比較

測量誤差為2σ時(shí)，3 種算法數(shù)據(jù)關(guān)聯(lián)正確率的比較如圖7 所示。可以看出此時(shí)MHJCBB 開始體現(xiàn)出優(yōu)勢(shì)，尤其在50～130 步之間其關(guān)聯(lián)結(jié)果明顯優(yōu)于JCBB算法。進(jìn)一步增加測量的不確定性，當(dāng)測量誤差達(dá)到4σ時(shí)，關(guān)聯(lián)正確率的結(jié)果對(duì)比見圖8。從圖8 可知，隨著測量誤差不斷增加，JCBB 的關(guān)聯(lián)正確率會(huì)突然下降，類似于圖4 中NN 的表現(xiàn)，很容易造成SLAM 算法的發(fā)散。但是MHJCBB卻保持了較好的性能，正確率都在0.72 以上，而JCBB 后期接近于0.42，MHJCBB 優(yōu)勢(shì)明顯。

圖8 測量誤差為4σ時(shí)關(guān)聯(lián)正確率比較

試驗(yàn)結(jié)果表明，傳感器測量誤差的增大，會(huì)引起整個(gè)處理過程不確定性的增加，此時(shí)NN、JCBB算法的關(guān)聯(lián)準(zhǔn)確率都會(huì)隨之下降，易造成觀測與特征之間的配對(duì)出錯(cuò)，影響機(jī)器人的位姿估計(jì)，并使得所建立的地圖不確定性增加，甚至?xí)?dǎo)致SLAM 算法的發(fā)散。而MHJCBB算法則通過保留多個(gè)航跡假設(shè)分支，獲得了較好的穩(wěn)定性和可靠性。

5 結(jié)語

數(shù)據(jù)關(guān)聯(liá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。該算法的主要具有如下特征：①每次關(guān)聯(lián)時(shí)保留多個(gè)聯(lián)合相容關(guān)聯(lián)假設(shè)，形成多個(gè)機(jī)器人航跡假設(shè)分支；②為每個(gè)航跡分支定義了評(píng)價(jià)函數(shù)；③依據(jù)評(píng)價(jià)函數(shù)，通過剪枝過程，將機(jī)器人航跡限制在一定數(shù)量內(nèi)。理論分析與試驗(yàn)結(jié)果表明，與經(jīng)典NN、JCBB等算法相比，在不確定性較大時(shí)，MHJCBB算法仍能獲得穩(wěn)定可靠的數(shù)據(jù)關(guān)聯(lián)結(jié)果。