不完整地圖中移動機器人蒙特卡羅定位研究

摘 要:針對移動機器人在不完整地圖中定位的問題，提出了一種改進的粒子聚類蒙特卡羅定位(Monte Carlo localization， MCL)算法。在定位過程中，將機器人的位姿分為六種狀態，每一種狀態對應一個粒子簇。在機器人運動的過程中，這六種狀態之間可以相互轉移，在計算狀態轉移概率的基礎上，實現了不完整地圖中移動機器人蒙特卡羅定位算法。實驗驗證了該算法在解決移動機器人在不完整地圖中定位問題的有效性。

關鍵詞:移動機器人; 蒙特卡羅定位; 狀態轉移; 不完整地圖

中圖分類號:TP24

文獻標志碼:A

文章編號:1001-3695(2010)02-0509-03

doi:10.3969/j.issn.1001-3695.2010.02.029

Mobile robot Monte Carlo localization in partial map

ZHANG Heng， LIU Yan-li， SUN Jin

(School of Information Engineering， East China Jiaotong University， Nanchang 330013， China)

Abstract:In order to overcome the difficulty of a mobile robot to perform localization in partial map， this paper proposed an improved particle-clustered Monte Carlo localization(MCL) algorithm. During the process of localization， the robot’s states were classed to 6 types， and one type of these was corresponding to a particle cluster. Based on computing the transition probability， realized a MCL algorithm in partial environment， which broke the restriction that the traditional MCL algorithm could only be used in the situation of complete map. Experiment results illustrate the validity of the proposed approach in solving problems of localization in partial map.

Key words:mobile robot; Monte Carlo localization(MCL); state transition; partial map

機器人定位問題分為兩類:位姿跟蹤問題和全局定位問題。前者指機器人的初始位姿已知，根據傳感器的觀測信息來補償碼盤的誤差;如果機器人的初始位姿未知，或者在跟蹤過程中出現了“綁架”現象(即在未通知機器人的情況下改變機器人的位姿)，那么就成了全局定位問題。近年來，越來越多的研究者把概率理論應用到移動機器人定位中[1]。對于解決全局定位問題，最具有代表性的是蒙特卡羅定位(MCL)算法[2]和馬爾可夫定位(Markov localization)算法[3]。其中MCL算法具有實現簡單、定位精度高、所需計算資源少等優點而受到研究者的青睞，并對其提出了許多改進的方法，其中具代表性的有Mixture-MCL[4]、自適應采樣MCL[5]、遺傳粒子濾波定位[6]、粒子聚類蒙特卡羅定位[7]等。

現有的定位方法大多是在地圖完整的前提條件下進行的，即機器人的位姿總是在地圖模型所對應的環境區域(本文中簡稱地圖區域)中。但在有些情況下地圖往往是不完整的，如地圖是由機器人自主構建而非人工創建的，這種地圖很難保證是完整覆蓋工作空間的。隨著移動機器人的自主性越來越高，這種不完整地圖的情況會越來越多。另外，在多機器人同步定位與地圖構建問題中，可以利用不完整地圖機器人定位技術實現地圖的合并。為了使機器人可以在不完整地圖中定位，本文利用先前提出的粒子聚類蒙特卡羅定位算法[7]的基本思想對MCL算法進行了進一步擴展。將定位過程中機器人的位姿分為六種狀態，每一種狀態對應一個粒子簇。與文獻[7]中算法不同的是，在機器人運動的過程中，這六種狀態之間可以轉移，在計算轉移概率的基礎上實現了不完整地圖中移動機器人蒙特卡羅定位算法。

1 問題分析

完整地圖和非完整地圖中機器人定位的區別在于:對于前者，機器人總是在地圖區域內運動，因此在蒙特卡羅定位過程中，粒子總是落入地圖內部;而對于后者，任何時刻機器人可能在地圖區域內也可能在地圖區域外。根據前一時刻和當前時刻機器人位姿情況的不同組合，可分為六種情況，或稱為機器人的六種狀態，如表1所示。

表1 機器人六種狀態分類

狀態編號前一時刻機器人位置當前時刻機器人位置歷史狀況

1地圖區域外地圖區域外從來就沒有進入過地圖區域

2地圖區域外地圖區域內首次進入地圖區域

3地圖區域內地圖區域內―

4地圖區域內地圖區域外―

5地圖區域外地圖區域外曾經進入過地圖區域

6地圖區域外地圖區域內再次進入地圖區域

記t時刻這六種狀態的概率分別為B1，t，B2，t，…，B6，t，∑6k=1Bk，t=1。本文的基本思路就是將這六種狀態對應的粒子歸為對應的六個粒子簇，利用文獻[7]介紹的粒子聚類蒙特卡羅算法來解決不完整地圖機器人定位問題。下面介紹這六種情況粒子的生成和對應權值的計算方法。

2 新一代粒子生成

由于實際環境區域未知，無法對第一種情況生成有效的粒子，但是如果能計算B1，t，則并不影響在地圖區域內的定位估計，B1，t的具體計算方法見下文。為生成第二種情況的粒子，首先給出如下的定義和合理的假設:

定義1 地圖的探測邊界(exploration frontiers)。是指已探測區域與為探測區域有通道的交界處，具體的就是地圖已知區域與未知區域(地圖外區域)的臨界處，且已知區域這邊為空閑。

假設1 機器人只能從地圖的探測邊界進入地圖區域。

于是，第二種情況的粒子在地圖探測邊界附近的地圖空閑區域內按均勻分布生成，該區域的大小根據機器人的當前運動步長(當前時刻與前一個時刻機器人位姿差)決定，樣本的個數N2在本文中取一固定值。

在機器人運動過程中其狀態之間可以轉移，設pij，t表示在t-1時刻機器人處于第i種狀態的情況下，由于機器人移動使得t時刻轉為第j種狀態的概率。分析具體的情況可以得出如圖1所示的狀態概率轉移圖。

根據圖1，新一代第3類粒子有三個來源:前一代第2類、第3類、第6類中部分粒子;新一代第4類粒子也有三個來源:前一代第2類、第3類、第6類中部分粒子;新一代第5類粒子有兩個來源:前一代第4類、第5類中部分粒子;新一代第6類粒子有兩個來源:前一代第4類、第5類中部分粒子。總之，采用前一代粒子和當前控制輸入，根據運動模型生成新一代粒子，然后對它們重新分類，具體實現見算法1。

3 權值計算

與文獻[7]的粒子聚類蒙特卡羅定位算法一樣，權值計算包括兩部分:a)機器人處于各種狀態的概率;b)各粒子簇中粒子的權值。由于存在狀態轉移，整個計算分為兩個階段，即預測階段和環境觀測階段。

3.1 預測階段

根據圖1，顯然有如下一些等式成立:

p11，t+p12，t=1， p23，t+p24，t=1， p33，t+p34，t=1

p45，t+p46，t=1， p55，t+p56，t=1， p63，t+p64，t=1(1)

在t時刻機器人對環境觀測之前，各種狀態的概率可根據t-1時刻的值遞推得到:

B′1，t=B1，t-1#8226;p11，t(2)

B′2，t=B1，t-1#8226;p12，t(3)

B′3，t=B2，t-1#8226;p23，t+B3，t-1#8226;p33，t+B6，t-1#8226;p63，t(4)

B′4，t=B2，t-1#8226;p24，t+B3，t-1#8226;p34，t+B6，t-1#8226;p64，t(5)

B′5，t=B4，t-1#8226;p45，t+B5，t-1#8226;p55，t(6)

B′6，t=B4，t-1#8226;p46，t+B5，t-1#8226;p56，t(7)

要計算上述各式，需確定轉移概率p12，t、p23，t、p34，t、p45，t、p56，t和p63，t(或p11，t、p24，t、p33，t、p46，t、p55，t和p64，t)的值。由于p12，t的值無法計算，可以給定一常數(一般取一較小的值，本文中取0.1)。設t-1時刻，第2~6類粒子簇為{(Ck，t-1，Bk，t-1)|k=2，…，6}。其中，Ck，t-1={〈ξ(j)k，t-1，w(j)k，t-1〉|j=1，2，…，Nk，t-1}，∑Nk，t-1j=1w(j)k，t-1=1;記ξ′(j)k，t為根據運動模型得到的t-1時刻第k類粒子簇中第j號粒子的預測值，即ξ′(j)k，t~p(ξt|ξ(j)k，t-1，ut)。根據ξ′(j)k，t是否在地圖區域中和其前驅樣本ξ(j)k，t-1的類別號k，可以確定新樣本ξ′(j)k，t的類別，最終得到新的粒子簇{(Ck，t，Bk，t)|k=2，…，6}。其他轉移概率計算方法如下(這里ξ∈M表示ξ處于地圖M的表示區域中，否則為ξM):

p23，t=∑ξ′(i)2，t∈Mw(i)2，t-1∑N2，t-1i=1w(i)2，t-1(8)p34，t=∑ξ′(i)3，tMw(i)3，t-1∑N3，t-1i=1w(i)3，t-1(9)

p45，t=∑ξ′(i)4，tMw(i)4，t-1∑N4，t-1i=1w(i)4，t-1(10) p56，t=∑ξ′(i)5，t∈Mw(i)5，t-1∑N5，t-1i=1w(i)5，t-1(11)

p63，t=∑ξ′(i)6，t∈Mw(i)6，t-1∑N6，t-1i=1w(i)6，t-1(12)

記w′(j)k，t為樣本ξ′(j)k，t的全局權值(用系統中所有樣本作為普通粒子濾波器的樣本集時各樣本的權值稱為該樣本的全局權值)。在這一階段，由于沒有考慮機器人對環境的觀測信息，新一代粒子的全局權值不變:

w′(j)k，t=w(j)k，t-1#8226;Bk，t-1(13)

3.2 環境觀測階段

機器人對環境觀測后，機器人所處各狀態的概率和粒子的權值需進一步更新。對于新一代粒子中2、3、6這三類粒子，機器人觀測信息可與地圖進行匹配，權值的更新方法與完整地圖中蒙特卡羅定位算法類似;對于第4、5類粒子，觀測信息無法利用，粒子權值不用再更新，即

w(i)k，t=w′(j)m，t#8226;p(zt|ξ(i)k，t)if k=2，3，6w′(j)m，t

if k=4，5(14)

在計算Bk，t之前，先對Ck，t(k=2，…，6)中粒子的權值w(i)k，t作歸一化處理。根據文獻[7]中的結論，Bk，t的計算方法如下:

B1，t=B′1，t(15)

B2，t=α2#8226;(B′2，t+B′3，t+B′6，t)α2+α3+α6(16)

B3，t=α3#8226;(B′2，t+B′3，t+B′6，t)α2+α3+α6(17)

B4，t=B′4，t(18)

B5，t=B′5，t(19)

B6，t=α6#8226;(B′2，t+B′3，t+B′6，t)α2+α3+α6(20)

其中:α2=B′2，t#8226;∑(ξ(i)2，tw(i)2，t)∈C2，t(w(i)2，t#8226;p(zt|ξ(i)2，t))(21)

α3=B′3，t#8226;∑(ξ(i)3，tw(i)3，t)∈C3，t(w(i)3，t#8226;p(zt|ξ(i)3，t))(22)

α6=B′6，t#8226;∑(ξ(i)6，tw(i)6，t)∈C6，t(w(i)6，t#8226;p(zt|ξ(i)6，t))(23)

最后，對{Bk，t|k=1，…，6}作歸一化處理。具體的不完整地圖蒙特卡羅定位算法如算法1。

算法1 不完整地圖蒙特卡羅定位算法

輸入:t-1時刻的粒子簇{(Ck，t-1，Bk，t-1)|k=2，…，6}，控制輸入ut，機器人對環境的觀測zt，機器人在t-1時刻處于狀態1的情況下在t時刻變為狀態2的概率p12。

輸出:t時刻新的粒子簇{(Ck，t，Bk，t)|k=2，…，6}。

/ 在預測階段第k類粒子集預測生成Nk個新一代粒子 /

1 for k=2 to 6 do Ck，t置空;

2 for i=1 to N2 do

3在地圖探測邊界區域隨機生成一位姿樣本ξ(i)2，t

4w(i)2，t=p(zt|ξ(i)2，t);C2，t=C2，t∪{〈ξ(i)2，t，w(i)2，t〉};

5 end

6 p23，t=0;p34，t=0;p45，t=0;p56，t=0;p63，t=0;

7 for k=2 to 6 do

8for i=1 to Nk do

9

從Ck，t-1根據權值{w(i)k，t-1}隨機抽取一樣本ξ(n)k，t-1

10

ξ′(i)k，t~p(ξt|ξ(n)k，t-1，ut);w′(i)k，t=1Nk;

11switch k do

12

case 2

13

if ξ′(i)k，t∈M then

14

C3，t=C3，t∪{〈ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

15

p23，t=p23，t+w′(i)k，t;

16

else

17

C4，t=C4，t∪{〈ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

18

case 3

19

if ξ′(i)k，t∈M then

20

C3，t=C3，t∪{〈ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

21

else

22

C4，t=C4，t∪{ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

23

p34，t=p34，t+w′(i)k，t;

24

case 4

25

if ξ′(i)k，t∈M then

26

C6，t=C6，t∪{〈ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

27

else

28

C5，t=C5，t∪{〈ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

29

p45，t=p45，t+w′(i)k，t;

30

case 5

31

if ξ′(i)k，t∈M then

32

C6，t=C6，t∪{〈ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

33

p56，t=p56，t+w′(i)k，t;

34

else

35

C5，t=C5，t∪{〈ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

36

case 6

37

if ξ′(i)k，t∈M then

38

C3，t=C3，t∪{〈ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

39

p63，t=p63，t+w′(i)k，t;

40

else

41

C4，t=C4，t∪{〈ξ′(i)k，t，w′(i)k，t#8226;Bk，t-1〉};

42end

43 end

44 end

45 B′1，t=B1，t-1#8226;(1-p12);B′2，t=B1，t-1#8226;p12;

46 B′3，t=B2，t-1#8226;p23，t+B3，t-1#8226;(1-p34，t)+B6，t-1#8226;p63，t;

47 B′4，t=B2，t-1#8226;(1-p23，t)+B3，t-1#8226;p34，t+B6，t-1#8226;(1-p63，t);

48 B′5，t=B4，t-1#8226;p45，t+B5，t-1#8226;(1-p56，t);B′6，t=B4，t-1#8226;(1-p45，t)+B5，t-1#8226;p56，t;

49 for i=1 to N2，t do w(i)2，t=w(i)2，t#8226;p(zt|ξ(i)2，t);

50 for i=1 to N3，t do w(i)3，t=w(i)3，t#8226;p(zt|ξ(i)3，t);

51 for i=1 to N6，tdo w(i)6，t=w(i)6，t#8226;p(zt|ξ(i)6，t);

52 for k=2 to 6 do Ck，t中粒子權值歸一化;

53 根據式(15)~(23)計算Bk，t;

54 返回{(Ck，t，Bk，t)|k=2，…，6};

4 實驗及結果分析

針對PlayerStage(http://playerstage.sourceforge.net)移動機器人軟件平臺，筆者編寫了一個新的player插件程序(pmcl)實現了本文提出的不完整地圖蒙特卡羅定位算法。

實驗之前，需得到環境的不完整地圖。用筆者設計開發的改進粒子濾波同步定位與地圖構建(simultaneous localization and mapping， SLAM)算法[8]得到一個模擬環境(實際上為一完整柵格地圖，如圖2(a)所示)的不完整地圖，如圖2(b)所示。與文獻[8]不同的是，該SLAM算法也實現為一個player插件程序(rbpfslam)。利用playerv工具控制機器人在模擬環境中的運動并不斷對環境進行觀測，rbpfslam插件程序將模擬的控制輸入信息和觀測信息作為數據源，在線地實現機器人的定位和地圖的構建，也可將數據序列保存為log文件離線地執行SLAM。這里，將SLAM后的柵格地圖作為本文實驗的不完整地圖。實驗過程與剛才的SLAM實驗差不多，不同的是將rbpfslam插件(實際上是cfg文件中設定的機器人驅動器)換成根據本文算法實現的pmcl插件，模擬環境不變。同樣利用playerv工具控制機器人在模擬環境中運動并不斷地對環境進行觀測，pmcl插件程序將模擬的控制輸入信息和觀測信息作為數據源，在線地實現機器人在不完整地圖中的全局定位，定位過程如圖2所示。圖中只顯示了第2類粒子(邊界處)和第3類粒子(地圖內部)。

對比圖2(c)和(d)可以看出，開始定位時，第2類粒子均勻地分布在地圖內部區域;在定位過程中，第2類粒子逐漸聚集于若干簇，也就是若干個位姿假設。與完整地圖定位不同的是，實際的機器人位姿不一定對應于這些位姿假設中的一個，如圖2(e)和(f)所示，當機器人處在地圖范圍之外就不對應了。但這并不影響機器人在后面過程的定位，因為在整個定位過程中不斷地生成第3類粒子，一旦機器人進入地圖區域，剛生成的第3類粒子將部分轉換為第2類粒子并獲得較大的權值，如圖2(g)和(h)所示。

5 結束語

本文利用粒子聚類蒙特卡羅定位算法的基本思想對MCL算法進行了進一步擴展。將定位過程中機器人的位姿分為六種狀態，每一種狀態對應一個粒子簇，從而可以運用粒子聚類蒙特卡羅定位算法。與文獻[7]不同的是，這六種狀態之間可以轉移，也就是說各粒子簇不是完全獨立地對應于一個基本的MCL過程。在計算轉移概率的基礎上，實現了新一代粒子生成方法和粒子權值計算方法。實驗結果初步證明了該算法的有效性。

參考文獻:

[1]厲茂海， 洪炳熔. 移動機器人的概率定位方法研究進展[J]. 機器人，2005， 27(4): 380-384.

[2]FOX D， BURGARD W， DELLAERT F， et al. Monte Carlo localisation: efficient position estimation for mobile robots[C]//Proc of the National Conference on Artificial Intelligence. Menlo Park:AAAI Press， 1999:343-349.

[3]FOX D. Markov localization:a probabilistic framework for mobile robot localization and navigation[D]. Bonn，Germany: University of Bonn， 1998.

[4]THRUN S， FOX D， BURGARD W， et al. Robust Monte Carlo loca-lization for mobile robots[J]. Artificial Intelligence， 2001， 128(1，2):99-141.

[5]JIANG Zheng-wei， GU Yuan-tao. Novel adaptive particle filters in robot localization[J]. Acta Automatica Sinica， 2005， 31(6):833-838.

[6]LUO Rong-hua， HONG Bing-rong， PIAO Song-hao， et al. Coevolution-based adaptive particle filters for global localization[J]. Chinese Journal of Electronics， 2005， 14(3):458-462.

[7]張恒， 樊曉平， 瞿志華. 基于多假設跟蹤的移動機器人自適應蒙特卡羅定位研究[J]. 自動化學報， 2007， 33(9):941-946.

[8]張恒， 劉艷麗. 基于固定滯后Gibbs采樣粒子濾波的移動機器人SLAM[J]. 計算機應用研究， 2008， 25(11):3292-3295.