摘要：生物啟發(fā)的無(wú)線復(fù)眼導(dǎo)航技術(shù)是新型的機(jī)器人導(dǎo)航方案，將分布在環(huán)境中的分布式智能代替了傳統(tǒng)的集中式智能。蒙特卡洛定位是近來(lái)流行的機(jī)器人自主定位算法，將這種算法應(yīng)用在分布式視覺(jué)傳感器機(jī)器人的定位中，并針對(duì)多視覺(jué)傳感器觀測(cè)值的最優(yōu)選擇，提出了一種分布式的基于熵的觀測(cè)量選擇方法，目的是選擇那些對(duì)提高定位精度更有效的觀測(cè)信息，在保證定位精度的前提下，提高了定位的實(shí)時(shí)性和可靠性。仿真實(shí)驗(yàn)結(jié)果證明了這種算法的可行性。

關(guān)鍵詞：機(jī)器人定位；貝葉斯濾波；蒙特卡洛定位；分布式視覺(jué)；熵

中圖分類號(hào)：TP242.6 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：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 引言

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

2 基于貝葉斯濾波的位置估計(jì)

移動(dòng)機(jī)器人定位可以描述為：機(jī)器人本體與其所處環(huán)境組成的動(dòng)態(tài)系統(tǒng)中，根據(jù)初始狀態(tài)概率分布p(x0)和測(cè)量數(shù)據(jù)流ok，來(lái)估計(jì)系統(tǒng)的當(dāng)前位姿狀態(tài)xk ，從統(tǒng)計(jì)學(xué)的角度看，xk的估計(jì)是一個(gè)貝葉斯濾波問(wèn)題，可以通過(guò)估計(jì)后驗(yàn)密度分布p(xk，|ok)，實(shí)現(xiàn)用信度Bel(xk)=p(xk|ok)表示當(dāng)前的狀態(tài)估計(jì)。根據(jù)Markov假設(shè)和貝葉斯規(guī)則，Bel(xk)可以通過(guò)以下兩個(gè)步驟求解：

2.1 預(yù)測(cè)階段

若測(cè)量數(shù)據(jù)是運(yùn)動(dòng)信息ok=ak，則使用運(yùn)動(dòng)模型p(xk+1|xk，ak)預(yù)測(cè)系統(tǒng)在下一時(shí)刻的狀態(tài)，即通過(guò)公式(1)計(jì)算先驗(yàn)概率密度分布：

■(1)

2.2更新階段

若測(cè)量數(shù)據(jù)是觀測(cè)信息ok+1=zk+1 ，則使用感知模型p(zk|xk)更新系統(tǒng)的狀態(tài)，即通過(guò)公式(2)計(jì)算后驗(yàn)概率密度分布：

■(2)

式中p(ok+1|ok)是常數(shù)，根據(jù)Markov假設(shè)，即當(dāng)前狀態(tài)已知的前提下，過(guò)去測(cè)量值與當(dāng)前測(cè)量值相互獨(dú)立得到。

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

■ (3)

■即表示移動(dòng)機(jī)器人當(dāng)前的狀態(tài)估計(jì)。

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

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

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

第二步為觀測(cè)更新過(guò)程：在經(jīng)過(guò)運(yùn)動(dòng)預(yù)測(cè)采樣得到新的樣本集合后，融合k時(shí)刻的觀測(cè)信息zk，應(yīng)用感知模型p(zk|xik)重新計(jì)算每一樣本的可信度：

■ (4)

遞歸調(diào)用運(yùn)動(dòng)預(yù)測(cè)和觀測(cè)更新，移動(dòng)機(jī)器人就不斷更新帶權(quán)重的樣本集Sk，并利用Sk估計(jì)全局位置，Sk中權(quán)重值最大的樣本值即為機(jī)器人當(dāng)前位置。

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

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

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

利用狀態(tài)觀測(cè)方程，依據(jù)貝葉斯準(zhǔn)則，機(jī)器人狀態(tài)的后驗(yàn)概率為：

■(5)

其信息熵為：

■ (6)

通過(guò)融合k+1時(shí)刻第i個(gè)視覺(jué)傳感器的觀測(cè)量zik+1，得到最新位置的后驗(yàn)概率p(xk+1|zik+1)，它的信息熵為：

■ (7)

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

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

初始化：

設(shè)機(jī)器人初始位姿可信度為Bel(x0)，從Bel(x0)采樣得到N個(gè)樣本，樣本權(quán)值wi0=1/N，對(duì)機(jī)器人的初始位置一無(wú)所知?jiǎng)t可以用均值為1/N 的均勻分布采樣來(lái)描述初始信任度。

運(yùn)動(dòng)預(yù)測(cè)：

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

a 產(chǎn)生[0，1]均勻分布的隨機(jī)數(shù)r；

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

c 選j所對(duì)應(yīng)的Sik-1，令sik-1=sjk-1；

2) 根據(jù)sik-1所對(duì)應(yīng)的權(quán)值，以運(yùn)動(dòng)模型p(xk|xk-1，ak)進(jìn)行采樣得到新的樣本sik，用它代替舊的樣本sik-1，并令樣本的權(quán)值為1/N。

3) 重復(fù)上面的過(guò)程N(yùn)次，直至全部找到N個(gè)新的樣本，刪去舊樣本。

觀測(cè)更新：

當(dāng)外部視覺(jué)傳感器測(cè)量到機(jī)器人的位置信息Z時(shí)

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

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

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

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

經(jīng)過(guò)多輪的迭代之后，權(quán)值最大的sik即是機(jī)器人的位置。

6 仿真實(shí)驗(yàn):

仿真試驗(yàn)采用兩輪機(jī)器人的運(yùn)動(dòng)模型：

xk={xk，yk，θk}表示k時(shí)刻機(jī)器人的坐標(biāo)和方位角，若采樣周期為T(mén)，運(yùn)動(dòng)速度為vk，角速度為ωk，δ為零均值高斯噪聲的權(quán)重因子，則運(yùn)動(dòng)模型可以表示為：

■ (8)

感知模型：采用高斯分布來(lái)表示：

■(9)

其中mk代表已知地圖上從xk到攝像機(jī)距離和角度，zk代表攝像機(jī)測(cè)得的到機(jī)器人的實(shí)際位置，δ為分布模型的標(biāo)準(zhǔn)差，取決于機(jī)器人狀態(tài)空間的離散程度和距離測(cè)量的不確定程度。

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

仿真結(jié)果：

由仿真試驗(yàn)結(jié)果可見(jiàn)，采用基于最大信息熵的觀測(cè)量選擇方法綜合3個(gè)攝像機(jī)觀測(cè)信息對(duì)機(jī)器人定位能夠取得更好的效果

7 結(jié)論

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

參考文獻(xiàn):

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

[2] DELLAERT F，F(xiàn)OX 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.可移動(dòng)機(jī)器人的馬爾可夫自定算法研究[J].自動(dòng)化學(xué)報(bào)，2003，29(1):154-160.

[6] 譚民，王碩，曹志強(qiáng).多機(jī)器人系統(tǒng)[M].北京:清華大學(xué)出版社，2005.