基于MATLAB的蒙特卡羅算法在機(jī)器人定位中的設(shè)計(jì)與比較

摘要:自主定位是移動(dòng)機(jī)器人研究領(lǐng)域的重要課題。自主定位最關(guān)鍵的問題是全局定位，因?yàn)闄C(jī)器人不知道它的初始位置，因此需要全方面定位自己。近幾年，粒子濾波器在機(jī)器人定位領(lǐng)域中被廣泛應(yīng)用，它作為Monte Carlo方法的一種應(yīng)用，通過使用一組離散的帶權(quán)粒子表示機(jī)器人在環(huán)境中所處位置的概率分布，利用先驗(yàn)環(huán)境地圖信息經(jīng)過初步采樣、狀態(tài)預(yù)測、更新權(quán)值、重新采樣幾個(gè)步驟遞歸運(yùn)算完成濾波。本文介紹三種蒙特卡羅算法即基本MCL、Dual-MCL和Mixture-MCL，并通過MATLAB軟件模擬，實(shí)現(xiàn)三種蒙特卡羅算法的設(shè)計(jì)與比較。

關(guān)鍵詞:Monte Carlo算法;自主定位;MATLAB

1 介紹

1.1 移動(dòng)機(jī)器人的定位

自主定位是移動(dòng)機(jī)器人研究領(lǐng)域的重要課題。首先賦予機(jī)器人一個(gè)環(huán)境地圖知識(shí)，然后移動(dòng)中通過傳感器得到的信息估算機(jī)器人相對位置的問題，自主定位最關(guān)鍵的問題是全局定位，因?yàn)闄C(jī)器人不知道它的初始位置，因此需要全方面定位自己。近幾年，粒子濾波器在機(jī)器人定位領(lǐng)域中被廣泛應(yīng)用，它作為Monte Carlo方法的一種應(yīng)用，本文介紹三種蒙特卡羅算法即基本MCL、Dual-MCL和Mixture-MCL，并通過MATLAB軟件模擬，實(shí)現(xiàn)三種蒙特卡羅算法在機(jī)器人全局定位中的設(shè)計(jì)與分析。

1.2 蒙特卡羅(MCL)算法

通過使用不同表述形式，可以有幾種實(shí)現(xiàn)概率算法的方法。在這里，我們著眼于Monte Carlo 定位法。首先因?yàn)镸CL算法相當(dāng)有效率，我們可以根據(jù)計(jì)算機(jī)的計(jì)算能力決定樣本數(shù)目，其次MCL算法能夠應(yīng)用在非常簡單和原始的模型上。

MCL方法是基于Markov假設(shè)和Bayes濾波的位置估算自主定位法。通過使用一組離散的帶權(quán)粒子表示機(jī)器人在環(huán)境中所處位置的概率分布，利用先驗(yàn)環(huán)境地圖信息經(jīng)過初步采樣、狀態(tài)預(yù)測、更新權(quán)值、重新采樣幾個(gè)步驟遞歸運(yùn)算。具體算法如下[1]:

(1)從機(jī)器人當(dāng)前狀態(tài)信度 Bel ( Xt-1 )隨機(jī)抽取粒子Xt-1;

(2)根據(jù)機(jī)器人運(yùn)動(dòng)方程，預(yù)測粒子Xt-1在下一時(shí)刻的位姿Xt，得到該粒子先驗(yàn)概率分布 p(xtxt-1， ut-1);

(1)

(3)通過傳感器觀測，更新觀測模型重要度因子(importance factor)p( ytxt )分派給粒子，得到新樣本信度Bel( xt );

(2)

(3)

(4)重復(fù)步驟(1)~(3)，最后規(guī)范新樣本信度Bel( xt )的重要度因子，使之相加和為1。

1.3 Dual-MCL算法

基本MCL算法是從前一刻狀態(tài)和運(yùn)動(dòng)模型預(yù)測采樣粒子，當(dāng)傳感器不是很敏感時(shí)效果比較好，而傳感器低噪聲的時(shí)候卻會(huì)產(chǎn)生很大的誤差，這主要是運(yùn)動(dòng)模型和觀測模型信息非常不匹配造成的[2]。因此Fox和Thrun提出了基于kd-tree的Dual-MCL算法，采樣時(shí)更多依靠觀測模型中傳感器的作用。算法如下[3]:

(1)將 bt-1 (St-1 ) 轉(zhuǎn)化為kd-tree;

(2)從預(yù)期分布p( ytxt )采樣xt ;

(4)

(5)

(3)從分布 p(xtxt-1， ut-1)采樣xt-1;

(6)

(7)

(4)在kd-tree中給xt-1加后驗(yàn)概率權(quán)值。

(8)

1.4 Mixture-MCL算法

基本MCL算法由于傳感器太精確而失效，Dual-MCL由于傳感器不敏感失效，結(jié)合兩種算法的特點(diǎn)，F(xiàn)ox和Thrun又提出了Mixture-MCL算法，即將兩種預(yù)期分布概率通過公式(9)結(jié)合。

(9)

2 仿真實(shí)驗(yàn)設(shè)計(jì)

實(shí)驗(yàn)中，設(shè)計(jì)了如圖1的房間，房間尺寸如圖所示，即兩個(gè)20×20m的方形房間中間有一個(gè)20×6m的走廊，我們要解決的是房間中移動(dòng)機(jī)器人的全局定位問題。機(jī)器人可以在房間中任意移動(dòng)，并機(jī)器人頭部安裝有激光掃描傳感器以獲得它的位置數(shù)據(jù)。同時(shí)在房間的每個(gè)角A~L都裝有數(shù)據(jù)接收裝置，當(dāng)激光照射到數(shù)據(jù)接收裝置上時(shí)，返回的激光具有較大的能量從而使我們獲得機(jī)器人與接收裝置間的距離。

3 程序設(shè)計(jì)

三種MCL算法雖然具體過程不一樣，但都包含了初步采樣、狀態(tài)預(yù)測、更新權(quán)值、重新采樣幾個(gè)步驟。輸入:t-1時(shí)刻的帶權(quán)粒子xt-1，控制輸入ut，機(jī)器人對環(huán)境的觀測y，輸出:t 時(shí)刻的帶權(quán)粒子xt。

3.1 初始化

設(shè)機(jī)器人初始信度Bel( x0 )，從Bel( x0 )采樣得到m個(gè)樣本粒子。機(jī)器人對自己的初始位置一無所知。在程序中設(shè)置如下參數(shù)并初始化:

(1) 房間尺寸: Length=Width1=20m，Width2=6m;(2) 粒子總數(shù) m=1000;(3) 所有粒子的初始信度遵循正態(tài)分布規(guī)律Bel(x0)=1/m;(4) 高斯噪聲偏差參數(shù) sensor 和白噪聲振幅 Asensor ，這兩個(gè)參數(shù)用來仿真?zhèn)鞲衅髯x數(shù); (5) 運(yùn)動(dòng)預(yù)測模型的平方差 motion 和觀察預(yù)測模型的平方差observation兩者均滿足正態(tài)分布。(6) 采樣時(shí)間間隔T=1;

3.2 計(jì)算理想路徑和模擬傳感器讀數(shù)

通過機(jī)器人運(yùn)動(dòng)方程計(jì)算出機(jī)器人可能的位置，我們假設(shè)x方向和y方向運(yùn)動(dòng)是相互獨(dú)立的。機(jī)器人的任務(wù)是從A點(diǎn)出發(fā)，沿A到CL中點(diǎn)的斜線運(yùn)動(dòng)，進(jìn)入到走廊后沿走廊中線移動(dòng)。從目前狀態(tài)Position(old)得到新的位置Position(new)，如下式

Position(new)=Position(old)+T .at+sensor . randn(2，1)(10)

其中at表示移動(dòng)速度，randn(2，1)產(chǎn)生一個(gè)1×2的隨機(jī)矩陣。

式(11)模擬了增加了高斯噪聲和白噪聲的實(shí)際傳感器讀數(shù)。

sensor_reading=sersor_ideal+sensor . randn(4，1)+A . randn(4，1) (11)

sensor_ideal值為機(jī)器人與接收裝置間的距離理想值。由于添加了噪聲，因此虛擬傳感器讀數(shù)有可能超出范圍，在本程序中，給傳感器添加了飽和度，即當(dāng)讀數(shù)為負(fù)值時(shí)，我們將值調(diào)整為零;當(dāng)讀數(shù)超出最大值時(shí)，將值調(diào)整為最大值。

3.3 初步采樣

在基本MCL算法中，程序采用了輪盤賭選擇法，即每一個(gè)粒子的信度表示輪盤的每一部分，高信度粒子則占有輪盤大比例部分，那么當(dāng)我們投出一個(gè)彈球的時(shí)候，就有更多的機(jī)會(huì)被選到。

Dual-MCL算法和Mixture-MCL算法則采用了kd-tree。

圖2給出了輪盤賭選擇法流程圖。

利用kd-tree采樣即抽取等權(quán)力粒子構(gòu)建kd-tree，然后基于kd-tree對其進(jìn)行分類形成新的粒子簇?？蓞⒖贾心洗髮W(xué)張恒和樊小平的具體算法[4]。

3.4 狀態(tài)預(yù)測

采樣后，根據(jù)機(jī)器人運(yùn)動(dòng)模型和當(dāng)前狀態(tài)，通過公式預(yù)測下一時(shí)刻的狀態(tài)。

(12)

為了防止機(jī)器人走出房間，給機(jī)器人的位置作了限制。通過選擇的粒子計(jì)算后驗(yàn)概率 p( ytxt-1 ， ut-1 )。

(13)

3.5 觀測更新

在3.2中，我們已經(jīng)得到了模擬的傳感器讀數(shù)，即機(jī)器人與接收裝置間的距離理想值sensor_reading。但這些數(shù)據(jù)不能直接告訴我們機(jī)器人的位置。在本文中，我們需要從傳感器數(shù)據(jù)往回計(jì)算得到機(jī)器人的觀測位置值。圖3給出了機(jī)器人位置與傳感器讀數(shù)的幾何關(guān)系。

通過機(jī)器人位置與n個(gè)傳感器讀數(shù)的幾何關(guān)系求得n個(gè)位置值，最小二乘法求得機(jī)器人位置。

然后得到觀測概率密度值 p( ytx ):

(14)

更新信度:

(15)

3.6 權(quán)值歸一，更新信度

在3.5中，已經(jīng)得出了j粒子的Bel j( st )，然后將上述過程重復(fù)m次，得到m個(gè)粒子的St j( st )和。將這些信度歸一

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

4 結(jié)果

通過仿真實(shí)現(xiàn)，MCL能夠簡單、快速而較高精度的實(shí)現(xiàn)定位問題，且所需計(jì)算資源少。圖4虛線給出了機(jī)器人位置誤差變化情況，實(shí)線給出了粒子的標(biāo)準(zhǔn)偏差值。

我們發(fā)現(xiàn)改進(jìn)后的Mixture-MCL在少量粒子的概率分布效果要好于基本MCL，圖5給出了在只有50個(gè)樣本粒子情況下基本MCL和Mixture-MCL的信度誤差。實(shí)驗(yàn)證明了這一現(xiàn)象。

參考文獻(xiàn)

[1]Sebastian Thrun， Probabilistic Algorithms in Robotics[J]. AI Magazine， Vol 21， No 4 2000:93-109.

[2]Dieter Fox， Sebastian Thrun， Wolfram Burgard， Frank Dellaert. Particle Filters for Mobile Robot Localization[M]. Springer-Verlag，2001:483.

[3]Dieter Fox， Sebastian Thrun， Wolfram Burgard， Frank Dellaert. Particle Filters for Mobile Robot Localization[M]. Springer-Verlag，2001:485.

[4]張恒，樊曉平，瞿志華.基于多假設(shè)跟蹤的移動(dòng)機(jī)器人自適應(yīng)蒙特卡羅定位研究[J].自動(dòng)化學(xué)報(bào)，2007(9):941-946.

[5]Dieter Fox， Sebastian Thrun， Wolfram Burgard， Frank Dellaert. Particle Filters for Mobile Robot Localization[M]. Springer-Verlag，2001:488.