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

關(guān)鍵詞： 重定位； 幾何約束分枝定界算法； 同時(shí)定位與地圖創(chuàng)建； 聯(lián)合相容

中圖分類號： TN98?34； TP24 文獻(xiàn)標(biāo)識碼： 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 引 言

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

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

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

1 SLAM問題的一般數(shù)學(xué)模型

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

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

式中：[Xvk=[xvk yvk Φvk]T，][xvk，yvk]是機(jī)器人的位置坐標(biāo)，[Φvk]是機(jī)器人的姿態(tài)角；[M=[x1y1x2y2…xnyn]T]表示各個(gè)地圖特征的位置坐標(biāo)，對于靜止特征，位置坐標(biāo)是不隨時(shí)間變化的。

協(xié)方差矩陣的表達(dá)式為：

[Pk=PvvPvmPTvmPmm] （2）

式中：[Pvv]是機(jī)器人位姿估計(jì)協(xié)方差；[Pmm]是特征位置估計(jì)協(xié)方差；[Pvm]是交叉協(xié)方差。

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

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

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

2 GCBB算法

2.1 幾何約束

幾何約束包括一元（unary）幾何約束與二元（binary）幾何約束，當(dāng)缺少位置估計(jì)時(shí)，可以利用幾何約束實(shí)現(xiàn)數(shù)據(jù)關(guān)聯(lián)。

（1） 一元幾何約束

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

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

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

（2） 二元幾何約束

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

[bjl=fb（X）] （4）

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

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

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

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

則認(rèn)為[pij=（Ei，F(xiàn)j）]與[pkl=（Ek，F(xiàn)l）]滿足二元幾何約束。若觀測數(shù)目為[m，]則對應(yīng)有[m?（m-1）2]個(gè)二元幾何約束。

2.2 聯(lián)合相容檢驗(yàn)

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

機(jī)器人的觀測方程可寫為：

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

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

設(shè)關(guān)聯(lián)假設(shè)[Hm={j1，j2，…， jm}，]則可列出聯(lián)合觀測方程：

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

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

聯(lián)合新息[vHm]與協(xié)方差[SHm]的表達(dá)式為：

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

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

而觀測[zk]與[Hm]中相應(yīng)的地圖特征滿足聯(lián)合相容的條件為聯(lián)合馬氏距離：

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

式中[d=dim（zk）]。

將幾何約束和分枝定界法兩者進(jìn)行結(jié)合，便形成了幾何約束分枝定界（GCBB）重定位算法，具體的推導(dǎo)過程可參見文獻(xiàn)[15]。

3 改進(jìn)的GCBB重定位算法

由于GCBB算法的計(jì)算復(fù)雜度與觀測數(shù)目[m]成指數(shù)關(guān)系，當(dāng)[m]較大時(shí)，算法的執(zhí)行速度將大幅度降低，直接影響到算法的實(shí)際應(yīng)用。下面將從兩個(gè)方面對GCBB算法進(jìn)行改進(jìn)。

3.1 數(shù)據(jù)關(guān)聯(lián)的分組處理

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

3.2 關(guān)聯(lián)特征的局部化選取

在進(jìn)行數(shù)據(jù)關(guān)聯(lián)時(shí)，所采用的地圖特征數(shù)目越多，計(jì)算量越大。考慮到任一時(shí)刻機(jī)器人所攜帶的傳感器的探測范圍是有限的，為減少計(jì)算量，可根據(jù)傳感器的探測范圍選取相應(yīng)的局部區(qū)域內(nèi)的地圖特征，實(shí)施數(shù)據(jù)關(guān)聯(lián)。

首先，假設(shè)地圖中有[n]個(gè)特征，定義[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]個(gè)特征的位置；[R]由傳感器探測范圍確定。只有滿足[Cij=1，]才能執(zhí)行下一層的聯(lián)合相容配對。

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

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

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”數(shù)據(jù)集提供了豐富的GPS數(shù)據(jù)、激光雷達(dá)數(shù)據(jù)與DR數(shù)據(jù)，為了對FGCBB算法進(jìn)行驗(yàn)證，本節(jié)利用該數(shù)據(jù)集進(jìn)行仿真。設(shè)置傳感器探測范圍為[R=16 m，][N=6]。計(jì)算平臺CPU為Intel[?] CoreTM i5?3210M，主頻為2.5 GHz，算法通過Matlab R2013a實(shí)現(xiàn)。

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

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

5 結(jié) 語

當(dāng)觀測數(shù)目較多時(shí)，GCBB算法難以滿足計(jì)算的實(shí)時(shí)性要求。為此文中研究了GCBB算法的改進(jìn)方法，并提出了FGCBB算法，其核心有兩點(diǎn)：分組實(shí)施數(shù)據(jù)關(guān)聯(lián)，使計(jì)算復(fù)雜度隨觀測數(shù)目的變化呈線性關(guān)系；結(jié)合傳感器的探測范圍，在局部區(qū)域中選擇特征進(jìn)行數(shù)據(jù)關(guān)聯(lián)?；凇癡ictoria Park”數(shù)據(jù)集的仿真結(jié)果表明，F(xiàn)GCBB算法能夠保證重定位效果，且在觀測數(shù)目較多時(shí)，其計(jì)算速度明顯優(yōu)于GCBB。

參考文獻(xiàn)

[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] 郭劍輝.移動機(jī)器人同時(shí)定位與地圖創(chuàng)建方法研究[D].南京：南京理工大學(xué)，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] 郭劍輝，趙春霞，石杏喜.一種改進(jìn)的聯(lián)合相容SLAM數(shù)據(jù)關(guān)聯(lián)方法[J].儀器儀表學(xué)報(bào)，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] 陳白帆，蔡自興，鄒智榮.一種移動機(jī)器人SLAM中的多假設(shè)數(shù)據(jù)關(guān)聯(lián)方法[J].中南大學(xué)學(xué)報(bào)（自然科學(xué)版），2012，43（2）：521?526.

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

[11] 周武，趙春霞.SLAM問題的一種優(yōu)化數(shù)據(jù)關(guān)聯(lián)算法[J].機(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.