一、概述

之前的文章介紹過卡爾曼濾波算法進行定位，我們知道kalman算法適合用于線性的高斯分布的狀態環境中，我們也介紹了EKF，來解決在非高斯和非線性環境下的機器人定位算法。但是他們在現實應用中存在計算量，內存消耗上不是很高效。這就引出了MCL算法。

粒子濾波很粗淺的說就是一開始在地圖空間很均勻的撒一把粒子，然后通過獲取機器人的motion來移動粒子，比如機器人向前移動了一米，所有的粒子也就向前移動一米，不管現在這個粒子的位置對不對。使用每個粒子所處位置模擬一個傳感器信息跟觀察到的傳感器信息（一般是激光）作對比，從而賦給每個粒子一個概率。之后根據生成的概率來重新生成粒子，概率越高的生成的概率越大。這樣的迭代之后，所有的粒子會慢慢地收斂到一起，機器人的確切位置也就被推算出來了

MCL的計算步驟：

1. 隨機產生一堆粒子：粒子可以有位置、方向和/或任何其他需要估計的狀態變量。每一個都有一個權值(概率)，表明它與系統的實際狀態匹配的可能性有多大。用相同的權重初始化每個變量。
2. 預測粒子的下一個狀態：根據真實系統行為的預測來移動粒子。
3. 更新：根據測量結果更新粒子的權重。與測量值密切匹配的粒子的權重要高于與測量值不太匹配的粒子。
4. 重新采樣：拋棄高度不可能的粒子，代之以更可能的粒子的副本。
5. 計算估計值：可選地，計算粒子集的加權平均值和協方差來得到狀態估計。

粒子濾波的基本步驟為上面所述的5步，其本質是使用一組有限的加權隨機樣本（粒子）來近似表征任意狀態的后驗概率密度。粒子濾波的優勢在于對復雜問題的求解上，比如高度的非線性、非高斯動態系統的狀態遞推估計或概率推理問題。

此部分轉自知乎專欄

二、MCL算法

蒙特卡羅定位算法的偽代碼如上圖所示，其由兩個主要部分組成 由兩個for循環表示。第一個部分是運動和傳感器更新，第二個是重采樣過程。

若給出一張環境地圖，MCL的目標是確定由可信度代表的機器人姿態。在每次迭代中 算法都采用之前的可信度作為啟動命令 ，傳感器測量作為輸入。

最初，可信度是通過隨機生成m個粒子獲得的。然后 ，在第一個for循環中，假設的狀態是在機器人移動時計算出來的。接下來 ，用新的傳感器測量值計算粒子的權重，然后更新每一個粒子的狀態。

在MCL的第二個循環中進行重采樣，在這里 具有高概率的粒子存活下來并在下一次迭代中被重新繪制出來，低概率粒子則被拋棄。

最后 算法輸出新的可信度，然后啟動新的迭代，通過讀取新的傳感器測量值來實現下一個運動。

三、C++代碼實現

以下是基礎代碼，我們需要在mian中完成相關程序

#define _USE_MATH_DEFINES
//#include "src/matplotlibcpp.h"//Graph Library
#include <iostream>
#include <string>
#include <math.h>
#include <vector>
#include <stdexcept> // throw errors
#include <random> //C++ 11 Random Numbers//namespace plt = matplotlibcpp;
using namespace std;// Landmarks
double landmarks[8][2] = { { 20.0, 20.0 }, { 20.0, 80.0 }, { 20.0, 50.0 },{ 50.0, 20.0 }, { 50.0, 80.0 }, { 80.0, 80.0 },{ 80.0, 20.0 }, { 80.0, 50.0 } };// Map size in meters
double world_size = 100.0;// Random Generators
random_device rd;
mt19937 gen(rd());// Global Functions
double mod(double first_term, double second_term);
double gen_real_random();class Robot {
public:Robot(){// Constructorx = gen_real_random() * world_size; // robot's x coordinatey = gen_real_random() * world_size; // robot's y coordinateorient = gen_real_random() * 2.0 * M_PI; // robot's orientationforward_noise = 0.0; //noise of the forward movementturn_noise = 0.0; //noise of the turnsense_noise = 0.0; //noise of the sensing}void set(double new_x, double new_y, double new_orient){// Set robot new position and orientationif (new_x < 0 || new_x >= world_size)throw std::invalid_argument("X coordinate out of bound");if (new_y < 0 || new_y >= world_size)throw std::invalid_argument("Y coordinate out of bound");if (new_orient < 0 || new_orient >= 2 * M_PI)throw std::invalid_argument("Orientation must be in [0..2pi]");x = new_x;y = new_y;orient = new_orient;}void set_noise(double new_forward_noise, double new_turn_noise, double new_sense_noise){// Simulate noise, often useful in particle filtersforward_noise = new_forward_noise;turn_noise = new_turn_noise;sense_noise = new_sense_noise;}vector<double> sense(){// Measure the distances from the robot toward the landmarksvector<double> z(sizeof(landmarks) / sizeof(landmarks[0]));double dist;for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++) {dist = sqrt(pow((x - landmarks[i][0]), 2) + pow((y - landmarks[i][1]), 2));dist += gen_gauss_random(0.0, sense_noise);z[i] = dist;}return z;}Robot move(double turn, double forward){if (forward < 0)throw std::invalid_argument("Robot cannot move backward");// turn, and add randomness to the turning commandorient = orient + turn + gen_gauss_random(0.0, turn_noise);orient = mod(orient, 2 * M_PI);// move, and add randomness to the motion commanddouble dist = forward + gen_gauss_random(0.0, forward_noise);x = x + (cos(orient) * dist);y = y + (sin(orient) * dist);// cyclic truncatex = mod(x, world_size);y = mod(y, world_size);// set particleRobot res;res.set(x, y, orient);res.set_noise(forward_noise, turn_noise, sense_noise);return res;}string show_pose(){// Returns the robot current position and orientation in a string formatreturn "[x=" + to_string(x) + " y=" + to_string(y) + " orient=" + to_string(orient) + "]";}string read_sensors(){// Returns all the distances from the robot toward the landmarksvector<double> z = sense();string readings = "[";for (int i = 0; i < z.size(); i++) {readings += to_string(z[i]) + " ";}readings[readings.size() - 1] = ']';return readings;}double measurement_prob(vector<double> measurement){// Calculates how likely a measurement should bedouble prob = 1.0;double dist;for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++) {dist = sqrt(pow((x - landmarks[i][0]), 2) + pow((y - landmarks[i][1]), 2));prob *= gaussian(dist, sense_noise, measurement[i]);}return prob;}double x, y, orient; //robot posesdouble forward_noise, turn_noise, sense_noise; //robot noisesprivate:double gen_gauss_random(double mean, double variance){// Gaussian randomnormal_distribution<double> gauss_dist(mean, variance);return gauss_dist(gen);}double gaussian(double mu, double sigma, double x){// Probability of x for 1-dim Gaussian with mean mu and var. sigmareturn exp(-(pow((mu - x), 2)) / (pow(sigma, 2)) / 2.0) / sqrt(2.0 * M_PI * (pow(sigma, 2)));}
};// Functions
double gen_real_random()
{// Generate real random between 0 and 1uniform_real_distribution<double> real_dist(0.0, 1.0); //Realreturn real_dist(gen);
}double mod(double first_term, double second_term)
{// Compute the modulusreturn first_term - (second_term)*floor(first_term / (second_term));
}double evaluation(Robot r, Robot p[], int n)
{//Calculate the mean error of the systemdouble sum = 0.0;for (int i = 0; i < n; i++) {//the second part is because of world's cyclicitydouble dx = mod((p[i].x - r.x + (world_size / 2.0)), world_size) - (world_size / 2.0);double dy = mod((p[i].y - r.y + (world_size / 2.0)), world_size) - (world_size / 2.0);double err = sqrt(pow(dx, 2) + pow(dy, 2));sum += err;}return sum / n;
}
double max(double arr[], int n)
{// Identify the max element in an arraydouble max = 0;for (int i = 0; i < n; i++) {if (arr[i] > max)max = arr[i];}return max;
}int main()
{...    
}

（1）運動和傳感器更新

首先完成偽代碼的第一個部分：

主函數如下，

int main()
{//Practice Interfacing with Robot ClassRobot myrobot;myrobot.set_noise(5.0, 0.1, 5.0);myrobot.set(30.0, 50.0, M_PI / 2.0);myrobot.move(-M_PI / 2.0, 15.0);//cout << myrobot.read_sensors() << endl;myrobot.move(-M_PI / 2.0, 10.0);//cout << myrobot.read_sensors() << endl;// 實例化1000個粒子，每個粒子具有隨機的位置和方向int n = 1000;Robot p[1000];//For each particle add noise: Forward_Noise=0.05, Turn_Noise=0.05, and Sense_Noise=5.0for (int i = 0; i < n; i++) {p[i].set_noise(0.05, 0.05, 5.0);cout << p[i].show_pose() << endl;}//重新初始化myrobot對象并初始化一個測量向量myrobot = Robot();vector<double> z;//移動機器人，然后感知環境myrobot = myrobot.move(0.1, 5.0);z = myrobot.sense();// 以機器人運動模擬每個粒子的運動Robot p2[1000];for (int i = 0; i < n; i++) {p2[i] = p[i].move(0.1, 5.0);p[i] = p2[i];}//根據機器人的測量產生粒子的權重double w[1000];for (int i = 0; i < n; i++) {w[i] = p[i].measurement_prob(z);cout << w[i] << endl;}return 0;
}

（2）重采樣

來看一組只有五個數值的例子，

來計算歸一化的各個粒子的概率

#include <iostream>using namespace std;double w[] = { 0.6, 1.2, 2.4, 0.6, 1.2 };
double sum = 0;void ComputeProb(double w[], int n)
{// 計算總權重Wfor (int i = 0; i < n; i++) {sum = sum + w[i];}// 計算歸一化后的權重for (int j = 0; j < n; j++) {w[j] = w[j] / sum;cout << "P" << j + 1 << "=" << w[j] << endl;}
}int main()
{ComputeProb(w, sizeof(w) / sizeof(w[0]));return 0;
}

重采樣偽代碼如下

多次重采樣數據，再利用評估函數來評估誤差

int main()
{//Practice Interfacing with Robot ClassRobot myrobot;myrobot.set_noise(5.0, 0.1, 5.0);myrobot.set(30.0, 50.0, M_PI / 2.0);myrobot.move(-M_PI / 2.0, 15.0);myrobot.move(-M_PI / 2.0, 10.0);// 實例化1000個粒子，每個粒子具有隨機的位置和方向int n = 1000;Robot p[1000];//For each particle add noise: Forward_Noise=0.05, Turn_Noise=0.05, and Sense_Noise=5.0for (int i = 0; i < n; i++) {p[i].set_noise(0.05, 0.05, 5.0);// cout << p[i].show_pose() << endl;}// 重新初始化myrobot對象并初始化一個測量向量myrobot = Robot();vector<double> z;// 在一組粒子上迭代50次int steps = 50;for (int t = 0; t < steps; t++) {// 移動機器人，然后感知環境myrobot = myrobot.move(0.1, 5.0);z = myrobot.sense();// 模擬每個粒子的機器人運動Robot p2[1000];for (int i = 0; i < n; i++) {p2[i] = p[i].move(0.1, 5.0);p[i] = p2[i];}// 根據機器人的測量產生粒子的權重double w[1000];for (int i = 0; i < n; i++) {w[i] = p[i].measurement_prob(z);//cout << w[i] << endl;}// 對粒子重新采樣，采樣概率與重要性權重成正比Robot p3[1000];int index = gen_real_random() * n;//cout << index << endl;double beta = 0.0;double mw = max(w, n);//cout << mw;for (int i = 0; i < n; i++) {beta += gen_real_random() * 2.0 * mw;while (beta > w[index]) {beta -= w[index];index = mod((index + 1), n);}p3[i] = p[index];}for (int k = 0; k < n; k++) {p[k] = p3[k];//cout << p[k].show_pose() << endl;}// 評估誤差cout << "Step = " << t << ", Evaluation = " << evaluation(myrobot, p, n) << endl;} //End of Steps loopreturn 0;
}

輸出如下

Step = 0, Evaluation = 3.22026
Step = 1, Evaluation = 3.3267
Step = 2, Evaluation = 3.6514
Step = 3, Evaluation = 4.42686
Step = 4, Evaluation = 3.97611
Step = 5, Evaluation = 3.1907
Step = 6, Evaluation = 2.56729
Step = 7, Evaluation = 2.15039
Step = 8, Evaluation = 1.83402
Step = 9, Evaluation = 1.48423
Step = 10, Evaluation = 1.4108
Step = 11, Evaluation = 1.39957
Step = 12, Evaluation = 1.40748
Step = 13, Evaluation = 1.37557
Step = 14, Evaluation = 1.33284
Step = 15, Evaluation = 1.36003
Step = 16, Evaluation = 1.44411
Step = 17, Evaluation = 1.55345
Step = 18, Evaluation = 1.60188
Step = 19, Evaluation = 1.53727
Step = 20, Evaluation = 1.48127
Step = 21, Evaluation = 1.43857
Step = 22, Evaluation = 1.37033
Step = 23, Evaluation = 1.3693
Step = 24, Evaluation = 1.42263
Step = 25, Evaluation = 1.43937
Step = 26, Evaluation = 1.40299
Step = 27, Evaluation = 1.39867
Step = 28, Evaluation = 1.42217
Step = 29, Evaluation = 1.41403
Step = 30, Evaluation = 1.44112
Step = 31, Evaluation = 1.43527
Step = 32, Evaluation = 1.41816
Step = 33, Evaluation = 1.45077
Step = 34, Evaluation = 1.51069
Step = 35, Evaluation = 1.58236
Step = 36, Evaluation = 1.47355
Step = 37, Evaluation = 1.40463
Step = 38, Evaluation = 1.41416
Step = 39, Evaluation = 1.40608
Step = 40, Evaluation = 1.44435
Step = 41, Evaluation = 1.47949
Step = 42, Evaluation = 1.53257
Step = 43, Evaluation = 1.56387
Step = 44, Evaluation = 1.52004
Step = 45, Evaluation = 1.45646
Step = 46, Evaluation = 1.42782
Step = 47, Evaluation = 1.439
Step = 48, Evaluation = 1.42743
Step = 49, Evaluation = 1.40226

四、繪圖

以下內容在虛擬機Linux環境中完成。

下載udacity提供的源文件。

修改main.cpp。

//Compile with: g++ solution.cpp -o app -std=c++11 -I/usr/include/python2.7 -lpython2.7
#include "src/matplotlibcpp.h" //Graph Library
#include <iostream>
#include <string>
#include <math.h>
#include <stdexcept> // throw errors
#include <random> //C++ 11 Random Numbers
#include <vector>namespace plt = matplotlibcpp;
using namespace std;// Landmarks
double landmarks[8][2] = { { 20.0, 20.0 }, { 20.0, 80.0 }, { 20.0, 50.0 },{ 50.0, 20.0 }, { 50.0, 80.0 }, { 80.0, 80.0 },{ 80.0, 20.0 }, { 80.0, 50.0 } };// Map size in meters
double world_size = 100.0;// Random Generators
random_device rd;
mt19937 gen(rd());// Global Functions
double mod(double first_term, double second_term);
double gen_real_random();class Robot {
public:Robot(){// Constructorx = gen_real_random() * world_size; // robot's x coordinatey = gen_real_random() * world_size; // robot's y coordinateorient = gen_real_random() * 2.0 * M_PI; // robot's orientationforward_noise = 0.0; //noise of the forward movementturn_noise = 0.0; //noise of the turnsense_noise = 0.0; //noise of the sensing}void set(double new_x, double new_y, double new_orient){// Set robot new position and orientationif (new_x < 0 || new_x >= world_size)throw std::invalid_argument("X coordinate out of bound");if (new_y < 0 || new_y >= world_size)throw std::invalid_argument("Y coordinate out of bound");if (new_orient < 0 || new_orient >= 2 * M_PI)throw std::invalid_argument("Orientation must be in [0..2pi]");x = new_x;y = new_y;orient = new_orient;}void set_noise(double new_forward_noise, double new_turn_noise, double new_sense_noise){// Simulate noise, often useful in particle filtersforward_noise = new_forward_noise;turn_noise = new_turn_noise;sense_noise = new_sense_noise;}vector<double> sense(){// Measure the distances from the robot toward the landmarksvector<double> z(sizeof(landmarks) / sizeof(landmarks[0]));double dist;for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++) {dist = sqrt(pow((x - landmarks[i][0]), 2) + pow((y - landmarks[i][1]), 2));dist += gen_gauss_random(0.0, sense_noise);z[i] = dist;}return z;}Robot move(double turn, double forward){if (forward < 0)throw std::invalid_argument("Robot cannot move backward");// turn, and add randomness to the turning commandorient = orient + turn + gen_gauss_random(0.0, turn_noise);orient = mod(orient, 2 * M_PI);// move, and add randomness to the motion commanddouble dist = forward + gen_gauss_random(0.0, forward_noise);x = x + (cos(orient) * dist);y = y + (sin(orient) * dist);// cyclic truncatex = mod(x, world_size);y = mod(y, world_size);// set particleRobot res;res.set(x, y, orient);res.set_noise(forward_noise, turn_noise, sense_noise);return res;}string show_pose(){// Returns the robot current position and orientation in a string formatreturn "[x=" + to_string(x) + " y=" + to_string(y) + " orient=" + to_string(orient) + "]";}string read_sensors(){// Returns all the distances from the robot toward the landmarksvector<double> z = sense();string readings = "[";for (int i = 0; i < z.size(); i++) {readings += to_string(z[i]) + " ";}readings[readings.size() - 1] = ']';return readings;}double measurement_prob(vector<double> measurement){// Calculates how likely a measurement should bedouble prob = 1.0;double dist;for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++) {dist = sqrt(pow((x - landmarks[i][0]), 2) + pow((y - landmarks[i][1]), 2));prob *= gaussian(dist, sense_noise, measurement[i]);}return prob;}double x, y, orient; //robot posesdouble forward_noise, turn_noise, sense_noise; //robot noisesprivate:double gen_gauss_random(double mean, double variance){// Gaussian randomnormal_distribution<double> gauss_dist(mean, variance);return gauss_dist(gen);}double gaussian(double mu, double sigma, double x){// Probability of x for 1-dim Gaussian with mean mu and var. sigmareturn exp(-(pow((mu - x), 2)) / (pow(sigma, 2)) / 2.0) / sqrt(2.0 * M_PI * (pow(sigma, 2)));}
};// Functions
double gen_real_random()
{// Generate real random between 0 and 1uniform_real_distribution<double> real_dist(0.0, 1.0); //Realreturn real_dist(gen);
}double mod(double first_term, double second_term)
{// Compute the modulusreturn first_term - (second_term)*floor(first_term / (second_term));
}double evaluation(Robot r, Robot p[], int n)
{//Calculate the mean error of the systemdouble sum = 0.0;for (int i = 0; i < n; i++) {//the second part is because of world's cyclicitydouble dx = mod((p[i].x - r.x + (world_size / 2.0)), world_size) - (world_size / 2.0);double dy = mod((p[i].y - r.y + (world_size / 2.0)), world_size) - (world_size / 2.0);double err = sqrt(pow(dx, 2) + pow(dy, 2));sum += err;}return sum / n;
}
double max(double arr[], int n)
{// Identify the max element in an arraydouble max = 0;for (int i = 0; i < n; i++) {if (arr[i] > max)max = arr[i];}return max;
}void visualization(int n, Robot robot, int step, Robot p[], Robot pr[])
{//Draw the robot, landmarks, particles and resampled particles on a graph//Graph Formatplt::title("MCL, step " + to_string(step));plt::xlim(0, 100);plt::ylim(0, 100);//Draw particles in greenfor (int i = 0; i < n; i++) {plt::plot({ p[i].x }, { p[i].y }, "go");}//Draw resampled particles in yellowfor (int i = 0; i < n; i++) {plt::plot({ pr[i].x }, { pr[i].y }, "yo");}//Draw landmarks in redfor (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++) {plt::plot({ landmarks[i][0] }, { landmarks[i][1] }, "ro");}//Draw robot position in blueplt::plot({ robot.x }, { robot.y }, "bo");//Save the image and close the plotplt::save("./Images/Step" + to_string(step) + ".png");plt::clf();
}int main()
{//Practice Interfacing with Robot ClassRobot myrobot;myrobot.set_noise(5.0, 0.1, 5.0);myrobot.set(30.0, 50.0, M_PI / 2.0);myrobot.move(-M_PI / 2.0, 15.0);//cout << myrobot.read_sensors() << endl;myrobot.move(-M_PI / 2.0, 10.0);//cout << myrobot.read_sensors() << endl;// Create a set of particlesint n = 1000;Robot p[n];for (int i = 0; i < n; i++) {p[i].set_noise(0.05, 0.05, 5.0);//cout << p[i].show_pose() << endl;}//Re-initialize myrobot object and Initialize a measurment vectormyrobot = Robot();vector<double> z;//Iterating 50 times over the set of particlesint steps = 50;for (int t = 0; t < steps; t++) {//Move the robot and sense the environment afterwardsmyrobot = myrobot.move(0.1, 5.0);z = myrobot.sense();// Simulate a robot motion for each of these particlesRobot p2[n];for (int i = 0; i < n; i++) {p2[i] = p[i].move(0.1, 5.0);p[i] = p2[i];}//Generate particle weights depending on robot's measurementdouble w[n];for (int i = 0; i < n; i++) {w[i] = p[i].measurement_prob(z);//cout << w[i] << endl;}//Resample the particles with a sample probability proportional to the importance weightRobot p3[n];int index = gen_real_random() * n;//cout << index << endl;double beta = 0.0;double mw = max(w, n);//cout << mw;for (int i = 0; i < n; i++) {beta += gen_real_random() * 2.0 * mw;while (beta > w[index]) {beta -= w[index];index = mod((index + 1), n);}p3[i] = p[index];}for (int k = 0; k < n; k++) {p[k] = p3[k];//cout << p[k].show_pose() << endl;}//Evaluate the Errorcout << "Step = " << t << ", Evaluation = " << evaluation(myrobot, p, n) << endl;//####   DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####//TODO: Graph the position of the robot and the particles at each stepif (t % 5 == 0 ){visualization(n, myrobot, t, p2, p);}} //End of Steps loopreturn 0;
}

編譯程序

$ g ++ main.cpp -o app -std = c ++ 11 -I / usr / include / python2.7 -lpython2.7

運行程序

./app

效果如圖

總結

以上是生活随笔為你收集整理的Udacity机器人软件工程师课程笔记（三十三) - 蒙特卡洛定位算法（MCL）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。