分布式視覺機器人導航中的定位算法研究

摘要：生物啟發的無線復眼導航技術是新型的機器人導航方案，將分布在環境中的分布式智能代替了傳統的集中式智能。蒙特卡洛定位是近來流行的機器人自主定位算法，將這種算法應用在分布式視覺傳感器機器人的定位中，并針對多視覺傳感器觀測值的最優選擇，提出了一種分布式的基于熵的觀測量選擇方法，目的是選擇那些對提高定位精度更有效的觀測信息，在保證定位精度的前提下，提高了定位的實時性和可靠性。仿真實驗結果證明了這種算法的可行性。

關鍵詞：機器人定位；貝葉斯濾波；蒙特卡洛定位；分布式視覺；熵

中圖分類號：TP242.6 文獻標識碼：A 文章編號：1009-3044(2008)28-0230-04

Research on Distributed Vision based Robot Localization

GAO Ye-kun，ZHU Jin，JIANG Ping

(Control Theory and Control Engineering，Tongji University， Shanghai 201804，China)

Abstract: Wireless Mosaic Eye system (WIME) is a new solution for robot navigation，robots are navigated by distributed intelligence in an environment instead of the conventional centralized intelligence. Monte Carlo Localization (MCL) for mobile robots has become popular these days. In this paper， the MCL algorithm is applied in a scenario of distributed vision guided robot navigation. Taking into account observations from multiple cameras， this paper presents an observation selection method for enhancement of localization precision. It evaluates entropy of each observation and selects one with the richest information for robot localization. This method can increase localization precision and improve computational efficiency. Simulations are carried out to verify its feasibility.

Key words: robot localization; bayes filter; monte carlo location; distributed visions; entropy

1 引言

自主定位是移動機器人研究領域的重要課題，是移動機器人路徑規劃，導航，決策和執行任務的前提?；诟怕实亩ㄎ环椒ㄖ饕幸韵聨追N，卡爾曼濾波定位方法[1]，Markov定位算法[4]，和Monte Carlo定位算法[1-3]，Monte Carlo定位是基于貝葉斯濾波的Markov定位的實現方法之一，該方法不需要直接求解概率分布函數，而使用若干個帶權重的采樣來描述移動機器人在狀態空間的可能位置分布。這種方法能夠表示多峰分布，能用于全局定位，降低了存貯空間，計算效率很高，易于實現，是一種很有效的定位方法，已經得到迅速的推廣。生物啟發的無線復眼導航技術（WiME）是結合了生物學科發現和先進信息技術于一體的新型機器人導航方案。它以分布在環境中的分布式智能代替了傳統的集中式智能，來實現對移動機器人的導航。WiME的實現有望極大地提升在目前智能和計算水平下機器人的可移動性。在課題中，我們將傳統集中式智能機器人自身的視覺傳感器，分布到環境當中，通過環境中的多個視覺傳感器，獲得機器人的位姿信息，再結合機器人自身的傳感器信息對機器人進行定位。由于機器人可以同時從多個視覺傳感器得到自身的位姿信息，如何選擇那些對提高定位精度更有效的觀測信息是面臨的問題。該文將Monte Carlo定位方法應用在分布式視覺機器人的定位當中，并提出了一種分布式的基于熵的觀測量選擇方法，選擇那些對提高定位精度更有效的觀測信息。

2 基于貝葉斯濾波的位置估計

移動機器人定位可以描述為：機器人本體與其所處環境組成的動態系統中，根據初始狀態概率分布p(x0)和測量數據流ok，來估計系統的當前位姿狀態xk ，從統計學的角度看，xk的估計是一個貝葉斯濾波問題，可以通過估計后驗密度分布p(xk，|ok)，實現用信度Bel(xk)=p(xk|ok)表示當前的狀態估計。根據Markov假設和貝葉斯規則，Bel(xk)可以通過以下兩個步驟求解：

2.1 預測階段

若測量數據是運動信息ok=ak，則使用運動模型p(xk+1|xk，ak)預測系統在下一時刻的狀態，即通過公式(1)計算先驗概率密度分布：

■(1)

2.2更新階段

若測量數據是觀測信息ok+1=zk+1 ，則使用感知模型p(zk|xk)更新系統的狀態，即通過公式(2)計算后驗概率密度分布：

■(2)

式中p(ok+1|ok)是常數，根據Markov假設，即當前狀態已知的前提下，過去測量值與當前測量值相互獨立得到。

令ak+1=p(ok+1|ok)-1，把式(1)代入式(2)得：

■ (3)

■即表示移動機器人當前的狀態估計。

3 蒙特卡洛(MCL)自主定位原理

MCL概念的關鍵是應用N個帶權值的隨機樣本來表示機器人位姿的可信度Bel(xk)，樣本空間S={si|i=1，2...，N}。樣本集通過離散化狀態來近似表示機器人實際位姿的可信度。樣本空間中每一樣本Si包括了一個表示機器人位置的點xi(xi，yi，θi)，和一個表示機器人位于該點處概率的權值wi，∑■wi。蒙特卡洛定位方法包括運動預測和觀測更新兩個主要步驟:

第一步為運動預測過程：當機器人k時刻采取動作a后，先從原來的所有樣本s■(i=1，2，...，N)中，根據其當前的概率權值w■和運動模型P(xk|xk-1，ak-1)，依概率分布采樣得到新的樣本sik，用它替換原來樣本Sik-1，并令新的樣本權值為wik=1/N。

第二步為觀測更新過程：在經過運動預測采樣得到新的樣本集合后，融合k時刻的觀測信息zk，應用感知模型p(zk|xik)重新計算每一樣本的可信度：

■ (4)

遞歸調用運動預測和觀測更新，移動機器人就不斷更新帶權重的樣本集Sk，并利用Sk估計全局位置，Sk中權重值最大的樣本值即為機器人當前位置。

4 基于最大信息熵的觀測量選擇方法

在分布式視覺環境中，能觀測到機器人的視覺傳感器數為N，同一時刻能得到機器人位姿的N個觀測值。我們的目的是希望在所有這些有效的觀測量中，選擇出那些對提高定位精度更加有效的觀測量，在保證滿足定位精度的前提下，提高定位效率。本文針對以上問題，提出了一種分布式的基于熵的觀測量選擇方法，選擇那些對提高定位精度更有效的觀測信息，在保證一定定位精度的前提下，提高定位的效率、實時性和可靠性。

信息熵(Entropy)，定義：對任意連續分布Px，■可以理解為概率分布px的“純度”。如果該分布高度集中于一點的周圍，那么其信息熵就低;反之，如果該分布很分散，那么其信息熵就高。即：所含信息量越多，其信息熵越小。

利用狀態觀測方程，依據貝葉斯準則，機器人狀態的后驗概率為：

■(5)

其信息熵為：

■ (6)

通過融合k+1時刻第i個視覺傳感器的觀測量zik+1，得到最新位置的后驗概率p(xk+1|zik+1)，它的信息熵為：

■ (7)

H(p(xk+1|zik+1))越小，意味著經過融合觀測量zik+1，機器R的位置的不確定性越小，它的最新位置的后驗概率P(xk+1|zik+1)分布更集中于某個位置，也即提高了其位置的精確度。當利用這些觀測量來更新位置估計時，能更有效地減小位置的不確定性，獲得大的信息量，從而提高機器人定位精度。

5 基于最大信息熵的蒙特卡洛定位算法

初始化：

設機器人初始位姿可信度為Bel(x0)，從Bel(x0)采樣得到N個樣本，樣本權值wi0=1/N，對機器人的初始位置一無所知則可以用均值為1/N 的均勻分布采樣來描述初始信任度。

運動預測：

1) 根據下面的步驟，從舊的樣本集中選擇一樣本 Sik-1={xik-1，wik-1}，舊的樣本被采集的可能性正比例于它的權重，這樣一些權重較小的樣本被剔除，提高收斂的速度：

a 產生[0，1]均勻分布的隨機數r；

b 根據概率分布表，采用對分法尋找滿足Cjk-1<=r的最大j值，cjk-1為前j個樣本的權值的累加和，即cjk=∑ji=1wik；

c 選j所對應的Sik-1，令sik-1=sjk-1；

2) 根據sik-1所對應的權值，以運動模型p(xk|xk-1，ak)進行采樣得到新的樣本sik，用它代替舊的樣本sik-1，并令樣本的權值為1/N。

3) 重復上面的過程N次，直至全部找到N個新的樣本，刪去舊樣本。

觀測更新：

當外部視覺傳感器測量到機器人的位置信息Z時

1) 首先根據基于最大信息熵的觀測量選擇方法，選擇對提高定位精度最有效的一個觀測信息zi；

2) 利用感知模型計算樣本權值wik=p(zi|xik)wik-1;

3) 計算歸一化因子：n=∑ ■wik;

4) 權值歸一化：wik=wik/n；

經過多輪的迭代之后，權值最大的sik即是機器人的位置。

6 仿真實驗:

仿真試驗采用兩輪機器人的運動模型：

xk={xk，yk，θk}表示k時刻機器人的坐標和方位角，若采樣周期為T，運動速度為vk，角速度為ωk，δ為零均值高斯噪聲的權重因子，則運動模型可以表示為：

■ (8)

感知模型：采用高斯分布來表示：

■(9)

其中mk代表已知地圖上從xk到攝像機距離和角度，zk代表攝像機測得的到機器人的實際位置，δ為分布模型的標準差，取決于機器人狀態空間的離散程度和距離測量的不確定程度。

在仿真試驗中，采用3個不同位置的攝像機觀測機器人的位置，機器人運動速度1/T，采樣周期T，為便于驗證算法，機器人直線行走從(0，0) 到(10，10)，角速度為0rad/s，在10個采樣周期內，首先分別用3個攝像機觀測的數據進行定位，然后采用基于最大信息熵的觀測量選擇方法綜合3個攝像機的信息進行定位。

仿真結果：

由仿真試驗結果可見，采用基于最大信息熵的觀測量選擇方法綜合3個攝像機觀測信息對機器人定位能夠取得更好的效果

7 結論

該文討論了分布式視覺環境下的機器人定位算法，仿真試驗表明將蒙特卡洛定位方法應用在分布式視覺機器人的定位中是可行的，并且采用基于最大信息熵的觀測量選擇方法提高了定位的精度和效率。

參考文獻:

[1] THRUN S，FOX D，BURGARD W.Robust Monte Carlo localization for mobile robots [J].Artificial Intelligence，2001(128):99-141.

[2] DELLAERT F，FOX D，BURGARD W.Monte Carlo Localization for Mobile Robots[J].IEEE ICRA，1999(5):1322-1328.

[3] Lyne E Parker，ALLIANCE.An Architecture for fault to1erant multirobot cooperation[J].IEEE Trans，On Robotics and Automation，1998 14(2):220-24.

[4] FOX D，BURGARD W，THRUN S.Markov Localization for Mobile Robots in Dynamic Environments [J].Journal of Artifcia1 Intelligence Research，1999(11):391-427.

[5] 吳慶祥，Bell DayId.可移動機器人的馬爾可夫自定算法研究[J].自動化學報，2003，29(1):154-160.

[6] 譚民，王碩，曹志強.多機器人系統[M].北京:清華大學出版社，2005.