一種改進的幾何約束分枝定界SLAM重定位算法

摘 要： 重定位技術是機器人在已有SLAM地圖的環境中依靠自身傳感器重新獲得定位信息的關鍵技術。幾何約束分枝定界重定位（GCBB）算法是一種有效的方法，但是其存在計算速度慢的缺點。針對GCBB算法的不足，從兩個方面對其進行改進：一是采用分組方式進行數據關聯；二是結合傳感器探測范圍在局部區域中選擇特征進行數據關聯。仿真結果表明，所提出的快速幾何約束分枝定界重定位（FGCBB）算法能夠正確實現重定位，且計算復雜度與觀測數目兩者之間服從線性關系，當處理觀測數目較多的問題時，FGCBB的計算效率明顯優于GCBB算法。

關鍵詞： 重定位； 幾何約束分枝定界算法； 同時定位與地圖創建； 聯合相容

中圖分類號： TN98?34； TP24 文獻標識碼： A 文章編號： 1004?373X（2016）21?0141?04

An improved geometric constraints branch and bound relocation algorithm

for simultaneous localization and mapping

CAO Xiaobing1， XU Yicen2， GUO Jianhui3， RUI Changying1

（1. School of Control Technology， Wuxi Institute of Technology， Wuxi 214121， China；

2. School of Mechanical and Electrical Technology， Wuxi Vocational Institute of Commerce， Wuxi 214153， China；

3. The 28th Research Institute of China Electronics Technology Group Corporation， Nanjing 210007， China）

Abstract： The relocation technology is a key technology for robot to recover the location information by its sensor in the existing SLAM （simultaneous localization and mapping） environment. The geometric constraints branch and bound （GCBB） algorithm is an effective method， but its computation speed is slow. To overcome the shortcoming of GCBB algorithm， the algorithm was improved in two aspects： the packet mode is selected for data association； the feature is selected in local region for data association according to the detection range of the sensor. The simulation results show that the proposed fast geometric constrains branch and bound （FGCBB） algorithm can relocate correctly， both the computational complexity and observation quantity are in accord with the linear relation， and the calculation efficiency of FGCBB algorithm is better than that of GCBB algorithm while processing much observation quantity.

Keywords： relocation； geometric constraint branch and bound algorithm； simultaneous localization and mapping； joint compatibility

0 引 言

當環境未知時，為使移動機器人實現自主行走，必須同時解決定位和地圖創建問題，也稱SLAM問題（Simultaneous Localization and Mapping）[1]。而其中的重定位則是研究在缺乏初始位姿等先驗知識的前提下，使機器人根據其所攜帶的傳感器測量值，確定自身在給定地圖中的位置及姿態，此后可重新開始進行同時定位與地圖創建。當無法獲得GPS信號時，重定位是惟一有效的方法，適用于以下三種場合：在定位誤差較大時自行恢復；在機器人“迷失”時重新啟動SLAM過程；在循環較大時使其能夠安全地結束。

要處理重定位問題，目前主要有兩種不同的思路[2]：以最大團[3]、假設檢驗[4]與聯合相容分枝定界（JCBB）[5?6]等方法為代表，其實質是在地圖中尋找與傳感器探測信息相一致的特征，再參照機器人與這些特征之間的相對距離與方位，最終確定機器人在地圖中的位置；以蒙特卡洛定位[7]和馬爾可夫定位[8]等方法為代表，其實質是在機器人可能所處的各位置，分別對傳感器探測信息與地圖間的一致性進行檢驗。第一種思路的關鍵是數據關聯，數據關聯[9?10]可在傳感器觀測與地圖特征之間建立對應的關系，并直接影響到SLAM中的狀態估計，而實際中各種不確定性與干擾因素的作用使得數據關聯更為復雜與困難。

為解決數據關聯問題，研究者們提出多種不同的算法[11?14]。其中，最鄰近（NN）算法是最早提出的，其算法簡單、易于實現，但對傳感器測量精度要求較高，且不適用于干擾程度較嚴重的環境。文獻[15]將分枝定界和幾何約束相結合，提出了一種性能較好的幾何約束分枝定界（GCBB）算法，然而當觀測數目增大時計算量也呈指數關系增加，使得實時性變差，限制了其實際應用。鑒于此，本文將從兩個方面對GCBB算法進行改進：在觀測數目較多時，分組進行關聯，以降低計算量；利用機器人傳感器的測量范圍將數據關聯的范圍限制在局部可能區域內，以減少數據關聯時的搜索空間，從而進一步降低計算量。在此基礎上，設計了一種快速幾何約束分枝定界（Fast Geometric Constrains Branch and Bound，FGCBB）算法，并利用悉尼大學提供的“Victoria Park”數據集[16]對FGCBB算法進行驗證，以期為重定位問題提供一種新的解決思路。

1 SLAM問題的一般數學模型

SLAM問題中，利用狀態向量及其協方差矩陣可將地圖信息表示為[Map={Xk，Pk}]。系統的狀態向量可表示為：

[Xk=[XTvk MT]T] （1）

式中：[Xvk=[xvk yvk Φvk]T，][xvk，yvk]是機器人的位置坐標，[Φvk]是機器人的姿態角；[M=[x1y1x2y2…xnyn]T]表示各個地圖特征的位置坐標，對于靜止特征，位置坐標是不隨時間變化的。

協方差矩陣的表達式為：

[Pk=PvvPvmPTvmPmm] （2）

式中：[Pvv]是機器人位姿估計協方差；[Pmm]是特征位置估計協方差；[Pvm]是交叉協方差。

設[k]時刻，機器人所帶傳感器獲得環境特征[Ei]的觀測為[zk，i（i=1，2，…，m），]以[Fj={xj，yj}（j=1，2，…，n）]表示地圖中的已有特征，為建立[zk，i]和[Fj]之間的關系，需要通過數據關聯將地圖特征[Fji]與每一觀測[zk，i]進行對應，進而得到關聯假設：

[Hm={j1，j2，…，jm}]

當[Hm]中存在某個[ji=0，]則表明觀測[zk，i]可能是虛假觀測或新特征，故無法與已有特征相匹配。數據關聯本質上是一個搜索解空間的過程。設觀測數為[m，]可能與第[i]個觀測相匹配的特征數為[（n+1）]，那么可知需搜索的空間大小為[（n+1）m，]與觀測數[m]成指數關系。

2 GCBB算法

2.1 幾何約束

幾何約束包括一元（unary）幾何約束與二元（binary）幾何約束，當缺少位置估計時，可以利用幾何約束實現數據關聯。

（1） 一元幾何約束

設[pij=（Ei，Fj）]為所給定的“觀測?特征”配對，以[ui=（ui，Pi），][uj=（uj，Pj）]分別代表[Ei]與[Fj]的某幾何特性參數，如長度、半徑和角度等。[pij=（Ei，Fj）]滿足一元幾何約束的條件是：

[D2ij=（ui-uj）T（Pi+Pj）-1（ui-uj）≤χ2d，1-α] （3）

式中：[d=dim（ui）；][1-α]是給定的置信水平，通常取95%。[χ2d，1-α]的值可在卡方分布表中依據自由度[d]與置信水平[1-α]查表得到。若觀測數目為[m，]則對應有[m]個一元幾何約束。

（2） 二元幾何約束

設[pij=（Ei，Fj）]與[pkl=（Ek，Fl）]為所給定“觀測?特征”配對，則二元幾何約束是指[Ei，][Ek]間的幾何關系與[Fj，][Fl]間的幾何關系必須一致。以[fb（·）]表示兩者間的幾何關系函數，則[Fj，][Fl]之間的幾何關系估計值與方差的表達式分別為：

[bjl=fb（X）] （4）

[Pjl=JjPFjFjJTj+JlPFlFlJTl+JjPFjFlJTl+JlPFlFjJTj] （5）

式中：[Jj=?fb?XFjX]，[Jl=?fb?XFlX]均為雅可比矩陣。

同理，可得到觀測[Ei，][Ek]之間的幾何關系估計值[bik]與方差[Pik]。若有：

[D2ikjl=（bik-bjl）T（Pik+Pjl）-1（bik-bjl）≤χ2d，1-α] （6）

則認為[pij=（Ei，Fj）]與[pkl=（Ek，Fl）]滿足二元幾何約束。若觀測數目為[m，]則對應有[m?（m-1）2]個二元幾何約束。

2.2 聯合相容檢驗

利用2.1節中的幾何約束將觀測和特征配對后，還需通過聯合相容（Joint Compatibility，JC）檢驗所得配對的一致性，并獲得一致性最佳的配對。在進行檢驗時，聯合相容所采用的依據是特征間的相對誤差，隨著配對數目的增加，單個錯誤配對與其余配對聯合相容的概率將降低。目前，大部分的重定位算法都是基于聯合相容來檢驗關聯的一致性，其效果差別較小，但計算效率卻存在較大差異。

機器人的觀測方程可寫為：

[Zk=h（Xk）+nk] （7）

式中：[h（·）]為非線性函數；[nk]是方差為[Rk]的高斯白噪聲。

設關聯假設[Hm={j1，j2，…， jm}，]則可列出聯合觀測方程：

[ZHm=hHm（Xk）+nHm] （8）

[hHm（Xk）=hj1（Xk）?hjm（Xk）] （9）

聯合新息[vHm]與協方差[SHm]的表達式為：

[vHm=zk-hHm（Xk，k-1）SHm=HHmPk，k-1HTHm+RHm] （10）

式中：[zk=[zk，1 … zk，m]]表示[k]時刻的所有觀測值；[Xk，k-1，][Pk，k-1]分別表示[k]時刻機器人的預測位姿及其預測方差；[HHm=?hHm?X（Xk，k-1）]為雅可比矩陣。

而觀測[zk]與[Hm]中相應的地圖特征滿足聯合相容的條件為聯合馬氏距離：

[D2Hm=vTHmS-1HmvHm≤χ2d，1-α] （11）

式中[d=dim（zk）]。

將幾何約束和分枝定界法兩者進行結合，便形成了幾何約束分枝定界（GCBB）重定位算法，具體的推導過程可參見文獻[15]。

3 改進的GCBB重定位算法

由于GCBB算法的計算復雜度與觀測數目[m]成指數關系，當[m]較大時，算法的執行速度將大幅度降低，直接影響到算法的實際應用。下面將從兩個方面對GCBB算法進行改進。

3.1 數據關聯的分組處理

當觀測數目較大時，先將所有觀測進行分組，再對各組進行數據關聯。設每組觀測數目的上限為[N，]此時搜索空間的大小由[（n+1）m]變為[mN?（n+1）N，]相應地，計算復雜度與觀測數目[m]之間也從指數關系變成線性關系，計算效率得到了較大提高。按分組方式進行數據關聯時，配對出錯的概率隨著[N]的增加而降低，為有效地防止錯誤關聯，一般要求[N≥6，]考慮到隨著[N]的增大，計算量也會迅速增加，故[N]通常在[6～10]之間選取。

3.2 關聯特征的局部化選取

在進行數據關聯時，所采用的地圖特征數目越多，計算量越大。考慮到任一時刻機器人所攜帶的傳感器的探測范圍是有限的，為減少計算量，可根據傳感器的探測范圍選取相應的局部區域內的地圖特征，實施數據關聯。

首先，假設地圖中有[n]個特征，定義[n×n]維矩陣[C，]則：

[Cij=1， if （xi-xj）2+（yi-yj）2≤R0， if （xi-xj）2+（yi-yj）2>R i，j=1，2，…，n] （12）

式中：[（xi，yi），（xj，yj）]分別表示地圖中第[i，j]個特征的位置；[R]由傳感器探測范圍確定。只有滿足[Cij=1，]才能執行下一層的聯合相容配對。

3.3 快速幾何約束分枝定界算法（FGCBB）

綜合第3.1，3.2節所提出的改進，便可得到FGCBB算法，其處理流程的偽代碼如下所示。需要注意的是，與常規GCBB偽代碼相比，在分枝判定中增加了[Cij==1，]以考慮特征的區域性。

FGCBB處理流程偽代碼：

Procedure： FGCBB

loop=[mN；]（四舍五入求整）

for [i=1：loop]

[a=（i-1）?N+1]；

[b=i?N]；

if （[i]==loop） [b=m；]

[H（i）=GCBB（observations（a∶b））]；

end

[Hm=H（1）…H（loop）]；

GCBB處理流程偽代碼：

Procedure：GCBB[（observation（l：mm），H，i）]

if [i>mm]

if pairings[（H）>]pairings（Best）

estimate location[（H）]

if joint compatibility[（H）]

Best[=H]

end

end

else

for [j=l∶n]

if unary [（i，j）]binary[（i，j）Cij==1]

GCBB（observation[（l：mm），[Hj]，i+l]）

end

end

if pairings[（H）+mm-i>]pairing（Best）

GCBB（observation）[（l：mm），[H0]，i+l]）

end

end

4 算例仿真與分析

由于“Victoria Park”數據集提供了豐富的GPS數據、激光雷達數據與DR數據，為了對FGCBB算法進行驗證，本節利用該數據集進行仿真。設置傳感器探測范圍為[R=16 m，][N=6]。計算平臺CPU為Intel[?] CoreTM i5?3210M，主頻為2.5 GHz，算法通過Matlab R2013a實現。

將EKF?SLAM算法[2]執行1 000步以獲得環境地圖，其中含有[99]個樹特征。設一元、二元幾何約束分別取為樹干半徑與樹間距離。繼續執行EKF?SLAM算法，在第1 001～2 500步對重定位算法進行驗證，而每一步計算所得的機器人位姿則為重定位結果提供參考。

1 001步時激光雷達測量中提取的樹特征分布如圖1所示，此時觀測數目為[m=7]。圖2給出了此時快速幾何約束分枝定界算法重定位的結果。從圖2知，所有的觀測與地圖特征進行了正確關聯，重定位位姿與參考位姿相差很小。

5 結 語

當觀測數目較多時，GCBB算法難以滿足計算的實時性要求。為此文中研究了GCBB算法的改進方法，并提出了FGCBB算法，其核心有兩點：分組實施數據關聯，使計算復雜度隨觀測數目的變化呈線性關系；結合傳感器的探測范圍，在局部區域中選擇特征進行數據關聯。基于“Victoria Park”數據集的仿真結果表明，FGCBB算法能夠保證重定位效果，且在觀測數目較多時，其計算速度明顯優于GCBB。

參考文獻

[1] FERNANDEZ?MADRIGAL J A， CLARACO J L B. Simulta?neous localization and mapping for mobile robots： introduction and methods [M]. US： IGI Publishing， 2012.

[2] 郭劍輝.移動機器人同時定位與地圖創建方法研究[D].南京：南京理工大學，2008.

[3] BAILEY T， NEBOT E M， ROSENBLATT J K， et al. Data association for mobile robot navigation： a graph theoretic approach [C]// Proceedings of 2000 IEEE International Confe?rence on Robotics and Automation. Piscataway， NJ： IEEE， 2000： 2512?2517.

[4] LIM J H， LEONARD J J. Mobile robot relocation from echolocation constraints [J]. IEEE transactions on pattern analysis and machine intelligence， 2000， 22（9）： 1035?1041.

[5] NEIRA J， TARDOS J D. Data association in stochastic mapping using the joint compatibility test [J]. IEEE transactions on robotics and automation， 2001， 17（6）： 890?897.

[6] 郭劍輝，趙春霞，石杏喜.一種改進的聯合相容SLAM數據關聯方法[J].儀器儀表學報，2008，29（11）：2260?2265.

[7] FOX D， BURGARD W， DELLAERT F， et al. Monte?Carlo localization： efficient position estimation for mobile robots [C]// Proceedings of the Sixteenth National Conference on Artificial Intelligence. Menlo Park： American Association for Artificial Intelligence， 1999： 343?349.

[8] FOX D， BURGARD W， THRUN S. Active Markov localization for mobile robots [J]. Robotics and autonomous systems， 1998， 25（3/4）： 195?207.

[9] 陳白帆，蔡自興，鄒智榮.一種移動機器人SLAM中的多假設數據關聯方法[J].中南大學學報（自然科學版），2012，43（2）：521?526.

[10] 苑全德，洪炳镕，關毅，等.一種基于特征點三維信息的自然路標提取與快速匹配方法[J].智能計算機與應用，2015，5（1）：25?28.

[11] 周武，趙春霞.SLAM問題的一種優化數據關聯算法[J].機器人，2009，31（3）：217?223.

[12] BAR?SHALOM Y， LI X R. Multitarget?multisensor tracking： principles and techniques [M]. Storrs： YBS Publishing， 1995.

[13] GUO Jianhui， ZHANG Rongtao. Novel implementation of track?oriented multiple hypothesis tracking algorithm [J]. Chinese journal of electronics， 2012， 21（4）： 770?774.

[14] RAGO C， WILLETT P， STREIT R. Direct data fusion using the PMHT [C]// Proceedings of 1995 American Control Confe?rence. Piscataway， NJ： IEEE， 1995： 1698?1702.

[15] NEIRA J， TARDOS J D， CASTELLANOS J A. Linear time vehicle relocation in SLAM [C]// Proceedings of 2003 IEEE International Conference on Robotics and Automation. Pisca?taway， NJ： IEEE， 2003： 427?433.

[16] GUIVANT J E， NEBOT E M. Optimization of the simulta?neous localization and map building algorithm for real time implementation [J]. IEEE transactions on robotics and automation， 2001， 17（3）： 242?257.