蒙特卡羅定位（Particle Filter Localization）筆記

善始善終，這篇文章是Coursera課程Robotics: Estimation and Learning最后一周的課程總結。里面的小哥講得不是很清晰，留下的作業很花功夫（第二周課程也是醬紫）。

這周講的是使用蒙特卡羅定位法（Monte Carlo Localization，也作Particle Filter Localization）進行機器人定位（Localization）。這篇總結分為兩部分：

• 問題介紹和算法步驟
• 使用雷達數據進行的小實驗

1. 蒙特卡羅定位

在第三周中，我們講到了機器人在不知道地圖卻知道自己位置的情況下如何利用傳感器信息構建地圖（Mapping）。在這一周中，我們的問題相反，機器人知道地圖信息的情況下如何利用傳感器信息確定自己的位置（Localization）。

有人會說，定位是不需要地圖信息的。機器人知道初始位置，知道左右輪的速度，就可以算出在一段時間內左右輪分別走了多少距離，進而算出機器人的轉角和位移，以便更新位置信息。但是顯然，這種方法存在很大的問題。首先，速度是傳感器獲得的，然而傳感器是有精度限制的，這就意味着誤差的存在，對時間積分求距離誤差就更大了；另外，機器人也可能存在打滑之類的機械問題。

結合地圖來對機器人進行定位能有效減小誤差。

和第三周課程類似，我們使用激光傳感器，它能夠測量機器人各個方向和最近障礙物之間的距離。在每一個時間點，機器人都會獲得激光傳感器的測量值。如下圖，綠色三角形是機器人，紅色的線是激光束，黃色的格子是機器人在該激光方向上檢測到的最近的障礙物。

地圖是第三周中的占據柵格地圖（Occupancy Grid Map）。比如，下面的地圖中，淺色（白色）的格子表示障礙物，深色（黑色）的格子表示空白位置。

那么，在這個時間點，我們要做的就是把機器人放到地圖中去，使得激光傳感器的讀數盡可能符合地圖信息（如下圖所示）。

這樣，對於一個時間點的定位問題就變成了求解最優函數的問題了。然而這個最優化函數太難求解了（坐標和角度都是連續變化的，而地圖是一個一個格子的數值）。

我們需要注意到兩點。第一，對於給定的機器人位置信息，我們可以很輕松地計算出和地圖的吻合程度；第二，相鄰兩個時間點機器人位置的變化不會太大。基於這兩點，我們引出蒙特卡羅定位法。這個算法的核心思想是用高斯分布描述機器人位置信息的噪音，用大量的粒子來描述機器人可能的位置。

具體來說，假如估測的機器人位置信息為（和表示坐標，表示機器人朝向），我們會記錄機器人的位置信息符合（視具體情況而定）的多元高斯分布（見第一周的內容）。在算法實現中，我們用高斯分布采樣出的個粒子來表示機器人的位置。如下左圖所示的單元高斯模型，下面藍色的點是采樣的粒子，上面是對應的高斯分布的模型；如下右圖所示是二元高斯模型采樣的情況。

蒙特卡羅定位法分為四步：

下面，我們將結合具體例子來說明算法的步驟。

2. 實踐：利用激光傳感器定位

前面我們介紹了為什么使用蒙特卡洛法進行機器人定位以及大體步驟，在這一節我們將介紹算法實現的具體細節。我們需要編寫matlab函數：

function [ myPose ] = particleLocalization(ranges, scanAngles, map, param)

scanAngles是的數組，表示同一時間發射的束激光與機器人前進方向的夾角。ranges是的矩陣，表示個時間點的激光傳感器數據。map是的矩陣，表示已知的占據柵格地圖。param是一個結構體，包含一些必要的輸入，其中param.resol表示分辨率，即一米的方格數量，param.origin是在占據柵格地圖中的起始位置。我們需要計算出一個的矩陣myPose，機器人在個時間點的坐標和朝向。

第一步，初始化粒子群。由於初始位置已經給出了准確值（param.origin），我們只需要設定種群大小之后利用repmat方法復制個初始位置作為初始種群。在實際應用（比如SLAM）中，如果初始位置不確定，可以使用高斯分布隨機采樣的方法（randn函數）初始化粒子群。

第二，模擬粒子運動。在第二周中，我們講了利用卡爾曼濾波為機器人運動建模，這是實際應用中一種十分有效的方法。在這里，我們采用一種簡單粗暴的方法，不對機器人運動進行建模，直接假定機器人在兩個采樣的時間點間隔內運動的范圍有限，然后利用randn函數隨機生成可能運動到的位置。

第三，計算粒子評分。對於每一條激光射線，傳感器讀數與地圖的匹配有四種情況如下表。

我實踐的時候發現，雷達定位出的空白區域太多了，計算這些柵格十分耗時，所以只計算傳感器定位的障礙物與地圖中障礙物的符合個數（即上表的評分改為0，0，0，+1）。對每一個粒子評分結束后，我們選擇得分最高的粒子作為該時間點機器人的位置。

第四，粒子群重采樣。在評分結束后，我們會發現有的粒子評分很低，即嚴重偏離可能位置，對於這些粒子我們需要舍棄。而有一些粒子的評分很高且很接近（比如傳感器讀數與地圖吻合度80%以上的粒子），我們需要把它們都保留下來。這就是粒子群重采樣。在實際操作中，我們直接將評分過低的粒子舍棄，對評分高的粒子進行復制，重采樣之后保持粒子群數量基本不變。

完整的matlab代碼為：

function myPose = particleLocalization(ranges, angles, map, param)

% occupancy value of unexplored pixels
unknown = mode(reshape(map, size(map,1)*size(map,2), 1));

N = size(ranges, 2); % number of poses to calculate
myPose = zeros(3, N); % initialize return value

resol = param.resol; % map resolution
origin = param.origin; % origin in pixel

sig = [0.08, 0, 0; 0, 0.08, 0; 0, 0, 0.08]; % noise for particle movement

myPose(:,1) = param.init_pose; % init position

M = 200; % number of particles

P = repmat(myPose(:,1), [1, M]);

thr = ceil(3.5/5*size(angles,1)); % set the score threshold as 70%

for j = 2:N
    maxscore = 0;
    while maxscore < thr
        Q=P+(randn(size(P,2),3)*sig)'; % particles movement
        score = zeros(size(Q,2), 1); % scores
        for k = 1:size(Q,2) % calculate score for each particle
            occ_x = ceil( (ranges(:,j) .* cos(angles+Q(3,k)) + Q(1,k) )  * resol + origin(1) );
            occ_y = ceil( (-ranges(:,j) .* sin(angles+Q(3,k)) + Q(2,k) ) * resol + origin(2) );
            ids = occ_x > 0 & occ_x <= size(map,2) & occ_y > 0 & occ_y <= size(map,1);
            score(k) = size( map( map (sub2ind(size(map), occ_y(ids), occ_x(ids)) ) > unknown ), 1);
        end
        [maxscore, index] = max(score); % select particle with maximum score
    end
    myPose(:,j) = Q(:,index); % set pose(j) as the optimal particle

    Q = Q(:,score >= thr); % select particles with high score
    P = repmat(Q, 1, ceil(M/size(Q,2)) ); % regenerate particles
end

end

 

於是，我們就得到了測試數據的定位圖了：

總結

本周講了經典的定位算法——蒙特卡羅定位法，最早的論文可見"Monte Carlo localization for mobile robots"，這種方法十分實用。當然，我們只講了地圖已知的情況下如何定位。在地圖未知的時候，Mapping和Localization需要同時進行，也就是SLAM，這四個步驟就會有很多變化，但是算法的框架並不會變。

對於這整個課程來說，感覺第一周和第三周的課講得特別清晰，課后的練習都能很輕松地干掉。尤其是第一周講的高斯分布，內容很多卻講得很清楚。第二周和第四周的內容都只講了大概，很多細節留到了作業中要自己去揣摩。作業做起來有些蛋疼，需要反復理解，但其中算法的思想都是十分經典的。值得一上！