基于MATLAB的蒙特卡羅算法在機器人定位中的設計與比較

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

關鍵詞:Monte Carlo算法;自主定位;MATLAB

1 介紹

1.1 移動機器人的定位

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

1.2 蒙特卡羅(MCL)算法

通過使用不同表述形式，可以有幾種實現概率算法的方法。在這里，我們著眼于Monte Carlo 定位法。首先因為MCL算法相當有效率，我們可以根據計算機的計算能力決定樣本數目，其次MCL算法能夠應用在非常簡單和原始的模型上。

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

(1)從機器人當前狀態信度 Bel ( Xt-1 )隨機抽取粒子Xt-1;

(2)根據機器人運動方程，預測粒子Xt-1在下一時刻的位姿Xt，得到該粒子先驗概率分布 p(xtxt-1， ut-1);

(1)

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

(2)

(3)

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

1.3 Dual-MCL算法

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

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

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

(4)

(5)

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

(6)

(7)

(4)在kd-tree中給xt-1加后驗概率權值。

(8)

1.4 Mixture-MCL算法

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

(9)

2 仿真實驗設計

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

3 程序設計

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

3.1 初始化

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

(1) 房間尺寸: Length=Width1=20m，Width2=6m;(2) 粒子總數 m=1000;(3) 所有粒子的初始信度遵循正態分布規律Bel(x0)=1/m;(4) 高斯噪聲偏差參數 sensor 和白噪聲振幅 Asensor ，這兩個參數用來仿真傳感器讀數; (5) 運動預測模型的平方差 motion 和觀察預測模型的平方差observation兩者均滿足正態分布。(6) 采樣時間間隔T=1;

3.2 計算理想路徑和模擬傳感器讀數

通過機器人運動方程計算出機器人可能的位置，我們假設x方向和y方向運動是相互獨立的。機器人的任務是從A點出發，沿A到CL中點的斜線運動，進入到走廊后沿走廊中線移動。從目前狀態Position(old)得到新的位置Position(new)，如下式

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

其中at表示移動速度，randn(2，1)產生一個1×2的隨機矩陣。

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

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

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

3.3 初步采樣

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

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

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

利用kd-tree采樣即抽取等權力粒子構建kd-tree，然后基于kd-tree對其進行分類形成新的粒子簇。可參考中南大學張恒和樊小平的具體算法[4]。

3.4 狀態預測

采樣后，根據機器人運動模型和當前狀態，通過公式預測下一時刻的狀態。

(12)

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

(13)

3.5 觀測更新

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

通過機器人位置與n個傳感器讀數的幾何關系求得n個位置值，最小二乘法求得機器人位置。

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

(14)

更新信度:

(15)

3.6 權值歸一，更新信度

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

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

4 結果

通過仿真實現，MCL能夠簡單、快速而較高精度的實現定位問題，且所需計算資源少。圖4虛線給出了機器人位置誤差變化情況，實線給出了粒子的標準偏差值。

我們發現改進后的Mixture-MCL在少量粒子的概率分布效果要好于基本MCL，圖5給出了在只有50個樣本粒子情況下基本MCL和Mixture-MCL的信度誤差。實驗證明了這一現象。

參考文獻

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

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