一種多假設聯合相容SLAM數據的關聯方法

徐伊岑，曹小兵，郭劍輝

（1.無錫商業職業技術學院機電技術學院，江蘇無錫 214153；2.無錫職業技術學院控制技術學院，江蘇無錫 214121；3.南京理工大學計算機科學與工程學院，南京 210094）

0 引言

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

在SALM領域所涉及的重難點問題中，數據關聯是其中之一。數據關聯源自目標跟蹤中的數據融合技術，在SLAM 中用于處理不同時空獲得的傳感器測量之間，測量與已有地圖特征之間的對應關系，以確定它們是否來自共同源的問題，還包括了新特征的確定過程。由于狀態估計是SLAM 問題的核心，而數據關聯又是狀態估計的基礎，因此直接影響到最終的定位與建圖結果，不準確的數據關聯甚至會導致SLAM 的發散［9］。

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

隨著機器人實際使用環境的日益復雜化，環境中各種不確定因素的干擾等進一步加大，需要建立更為有效的SLAM 數據關聯方法。為此，本文將JCBB 算法融入到MHT的框架之下，吸取兩種算法的優點，提出了一種多假設聯合相容分支定界算法（Multi-Hypothesis Joint Compatibility Branch and Bound，MHJCBB）。與NN、JCBB 等算法每個時刻只保留了1個最優關聯假設不同，MHJCBB 算法在實施數據關聯時，保留了多個聯合相容的關聯結果，形成多個機器人航跡假設分支，并計算每個航跡假設分支得分。為減少計算量保證計算效率，將得分較低的假設分支在剪枝過程中去除，而得分最高的假設分支則被選擇輸出。試驗結果證明了MHJCBB方法的有效性。

1 擴展卡爾曼濾波的SLAM建模

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

定義系統在時刻k的狀態變量

式中：Xvk＝［xvkyvkΦvk］T，其中的分量分別為機器人的位置坐標與姿態角；

其中的分量分別為各地圖特征的位置坐標，對于靜態地圖特征，其位置坐標為常值。EKF 假設系統狀態為高斯分布，k時刻的狀態可用估計均值Xk和協方差Pk來表示，

式中：Pv是機器人位置和姿態的估計協方差矩陣；Pm是地圖特征位置的估計協方差矩陣；Pvm是機器人位姿和地圖特征位置的交叉協方差矩陣。

在使用概率方法求解SLAM 問題時，通常將運動與觀測模型按照一個馬爾可夫過程來處理，即根據系統前一時刻的狀態來預測后一個時刻的狀態，與其他歷史狀態無關，由此可進行遞推處理。系統的運動模型描述如下：

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

在EKF SLAM 中，利用運動模型進行預測的過程為

觀測方程為

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

機器人在獲得環境的最新探測數據后，可對狀態向量的預測值予以濾波更新：

2 聯合相容分枝定界算法

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

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

假如已有數據關聯結果Hm＝｛j1，j2，…，jm｝，聯合觀測方程為

聯合新息及協方差為

式中：

如果聯合馬氏距離滿足

則認為這種關聯結果是可接受的，即觀測zk可與Hm中相應的地圖特征配對。式（12）采用χ2檢驗，自由度d ＝dim（zk），置信水平為1－α，可取95%可在χ2分布表中查得結果。

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

3 多假設聯合相容分枝定界算法

3.1 算法的提出

JCBB算法根據所有觀測值的綜合關聯情況來確定關聯解，以提高關聯的正確率。但在不確定性較多的復雜場合，這種處理方法也易造成誤關聯，使得關聯正確率明顯下降。MHT 主要用于處理非常復雜環境下的目標跟蹤問題，它將可能的關聯情況都作為一種假設分支保留下來，通過多周期的信息積累，來判斷那種關聯選擇是最佳的，并且可以對之前的錯誤關聯進行回溯改正，但計算復雜度高。吸取MHT 算法和JCBB 算法的優點，設計了一種可用于復雜環境中SLAM的數據關聯算法MHJCBB。

常規JCBB算法在數據關聯時僅保留配對數最大且聯合馬氏距離最小的1 個關聯假設，即使這個關聯解是錯誤的，在后續過程中也無法進行修改。因此，為防止誤關聯，MHJCBB 算法保留了配對數最大且聯合馬氏距離最小的NH個關聯假設，通過同時維持多個關聯假設的方式來保護可能被忽略的最優關聯解，以提高數據關聯的準確率。NH為常數，實際應用時可根據需要設置。MHJCBB算法的主要步驟為：①機器人位置預測，按式（5）、（6）計算；②進行JCBB 數據關聯，保留多個關聯假設；③根據關聯假設，生成多個機器人航跡假設分支；④按照定義的評價函數，計算航跡假設分支得分；⑤N-Best 剪枝，只保留得分最高的N個假設分支；⑥輸出得分最高的航跡假設分支。圖1 給出了MHJCBB算法的具體處理流程。

圖1 MHJCBB算法流程圖

3.2 假設分支的生成

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

圖2 航跡假設分支示意圖

3.3 假設分支得分評估

對于每條航跡假設分支，航跡的似然比可以用下式迭代計算：

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

采用對數似然比（Log Likelihood Ratio）來表示航跡得分（Track Score）：

因此，航跡得分的遞歸計算方式為

在N-Best剪枝階段，根據航跡得分的高低，保留得分高的N個航跡假設分支。與MHT 在目標跟蹤領域應用時存在多個目標共享觀測導致要計算最優假設分支組合的情況不同，MHJCBB不存在組合爆炸問題，因為SLAM 問題類似于單目標跟蹤的問題，不存在假設分支相容組合爆炸問題，航跡得分最大的航跡假設分支即為最優假設。

3.4 計算量分析

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

MHJCBB整個算法的計算量最終取決于保留的航跡假設分支數，與航跡假設分支數N 大致成倍數關系。因為形成多個關聯假設與常規JCBB 計算量幾乎差不多，而航跡得分計算中在JCBB 中也是本來要計算的，所以計算得分不涉及矩陣運算，只增加了對標量按式（16）計算；其次是得分排序，排序的復雜度為O（N2），由于N一般可取10 左右，增加計算量也是很有限的。所以，每條航跡假設分支的計算量與常規的SLAM算法計算量是大致一樣的。

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

4 試驗與結果分析

4.1 SLAM模型

在SLAM的狀態向量中，機器人的位置和姿態可表示為

通過航跡推算可獲得k時刻機器人位姿的相對變化量

運動方程為

觀測方程為

式中，（xj，yj）為特征的平面坐標。

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

4.2 試驗結果分析

試驗環境為一方形走廊，其中共有168 個靜態特征，為模擬實際環境中的運動物體的干擾，假設在該區域內存在10 個處于隨機移動狀態的物體，物體的移動速度為0.05 m／s，加速度為0.01 m／s2，初始位置隨機。移動機器人從（0，0）點開始按逆時針方向勻速穿過該區域，行駛至轉角處時機器人以原地轉動方式實現90°轉彎，每次轉彎分10 次完成。圖3 給出了試驗環境中的特征分布、機器人及干擾物體運動情況，其中：“*”是特征的位置分布；“△”是機器人理論行駛路徑；“---”是干擾物體運動軌跡。試驗將在不同的測量誤差條件下測試數據關聯的正確率。

圖3 特征分布、機器人理論路徑及移動物體軌跡

利用Monte Carlo 方法重復進行20 次試驗，并將獲得數據進行平均用于比較分析。參數設置如下：PD＝0.9，PFA＝0.05，pFA＝0.1，N ＝12，NH＝2。數據關聯準確率定義為該時刻正確配對的觀測個數（包括觀測與特征正確對應、虛警判別正確兩部分）與參與配對的總觀測個數的比值。

當測量誤差為σ時，NN、JCBB和MHJCBB 3 種方法數據關聯正確率的結果對比如圖4 所示。圖5、6 分別是此時x、y 方向的估計誤差比較。從圖中可以看出，JCBB和MHJCBB都取得了90%的關聯正確率，差別不明顯，但是NN由于誤差的積累，在后期關聯正確率迅速下滑，導致估計誤差迅速的增大，SLAM 算法發散。

圖4 測量誤差為σ時關聯正確率比較

圖5 測量誤差為σ時x方向誤差比較

圖6 測量誤差為σ時y方向誤差比較

圖7 測量誤差為2σ時關聯正確率比較

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

圖8 測量誤差為4σ時關聯正確率比較

試驗結果表明，傳感器測量誤差的增大，會引起整個處理過程不確定性的增加，此時NN、JCBB算法的關聯準確率都會隨之下降，易造成觀測與特征之間的配對出錯，影響機器人的位姿估計，并使得所建立的地圖不確定性增加，甚至會導致SLAM 算法的發散。而MHJCBB算法則通過保留多個航跡假設分支，獲得了較好的穩定性和可靠性。

5 結語

數據關聯問題的求解是SLAM研究領域中的關鍵難點之一，尤其對于經常使用的經典濾波算法如卡爾曼濾波等，少數的幾次誤關聯便容易導致算法的發散。JCBB算法是SLAM數據關聯時廣泛使用且行之有效的一種數據關聯方法，但當環境中的不確定性增加時，其關聯正確率將會出現明顯下降。文中將多假設跟蹤（MHT）算法與JCBB 算法的優點有機結合，提出了一種基于多假設跟蹤的MHJCBB。該算法的主要具有如下特征：①每次關聯時保留多個聯合相容關聯假設，形成多個機器人航跡假設分支；②為每個航跡分支定義了評價函數；③依據評價函數，通過剪枝過程，將機器人航跡限制在一定數量內。理論分析與試驗結果表明，與經典NN、JCBB等算法相比，在不確定性較大時，MHJCBB算法仍能獲得穩定可靠的數據關聯結果。