一、概述
之前的文章介绍过卡尔曼滤波算法进行定位，我们知道kalman算法适合用于线性的高斯分布的状态环境中，我们也介绍了EKF，来解决在非高斯和非线性环境下的机器人定位算法。但是他们在现实应用中存在计算量，内存消耗上不是很高效。这就引出了MCL算法。
粒子滤波很粗浅的说就是一开始在地图空间很均匀的撒一把粒子，然后通过获取机器人的motion来移动粒子，比如机器人向前移动了一米，所有的粒子也就向前移动一米，不管现在这个粒子的位置对不对。使用每个粒子所处位置模拟一个传感器信息跟观察到的传感器信息（一般是激光）作对比，从而赋给每个粒子一个概率。之后根据生成的概率来重新生成粒子，概率越高的生成的概率越大。这样的迭代之后，所有的粒子会慢慢地收敛到一起，机器人的确切位置也就被推算出来了
MCL的计算步骤：
- 随机产生一堆粒子：粒子可以有位置、方向和/或任何其他需要估计的状态变量。每一个都有一个权值(概率)，表明它与系统的实际状态匹配的可能性有多大。用相同的权重初始化每个变量。
- 预测粒子的下一个状态：根据真实系统行为的预测来移动粒子。
- 更新：根据测量结果更新粒子的权重。与测量值密切匹配的粒子的权重要高于与测量值不太匹配的粒子。
- 重新采样：抛弃高度不可能的粒子，代之以更可能的粒子的副本。
- 计算估计值：可选地，计算粒子集的加权平均值和协方差来得到状态估计。
粒子滤波的基本步骤为上面所述的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()
{
// Constructor
x = gen_real_random() * world_size; // robot's x coordinate
y = gen_real_random() * world_size; // robot's y coordinate
orient = gen_real_random() * 2.0 * M_PI; // robot's orientation
forward_noise = 0.0; //noise of the forward movement
turn_noise = 0.0; //noise of the turn
sense_noise = 0.0; //noise of the sensing
}
void set(double new_x, double new_y, double new_orient)
{
// Set robot new position and orientation
if (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 filters
forward_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 landmarks
vector<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 command
orient = orient + turn + gen_gauss_random(0.0, turn_noise);
orient = mod(orient, 2 * M_PI);
// move, and add randomness to the motion command
double dist = forward + gen_gauss_random(0.0, forward_noise);
x = x + (cos(orient) * dist);
y = y + (sin(orient) * dist);
// cyclic truncate
x = mod(x, world_size);
y = mod(y, world_size);
// set particle
Robot 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 format
return "[x=" + to_string(x) + " y=" + to_string(y) + " orient=" + to_string(orient) + "]";
}
string read_sensors()
{
// Returns all the distances from the robot toward the landmarks
vector<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 be
double 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 poses
double forward_noise, turn_noise, sense_noise; //robot noises
private:
double gen_gauss_random(double mean, double variance)
{
// Gaussian random
normal_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. sigma
return 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 1
uniform_real_distribution<double> real_dist(0.0, 1.0); //Real
return real_dist(gen);
}
double mod(double first_term, double second_term)
{
// Compute the modulus
return first_term - (second_term)*floor(first_term / (second_term));
}
double evaluation(Robot r, Robot p[], int n)
{
//Calculate the mean error of the system
double sum = 0.0;
for (int i = 0; i < n; i++) {
//the second part is because of world's cyclicity
double 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 array
double 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 Class
Robot 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.0
for (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)
{
// 计算总权重W
for (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 Class
Robot 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.0
for (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 loop
return 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()
{
// Constructor
x = gen_real_random() * world_size; // robot's x coordinate
y = gen_real_random() * world_size; // robot's y coordinate
orient = gen_real_random() * 2.0 * M_PI; // robot's orientation
forward_noise = 0.0; //noise of the forward movement
turn_noise = 0.0; //noise of the turn
sense_noise = 0.0; //noise of the sensing
}
void set(double new_x, double new_y, double new_orient)
{
// Set robot new position and orientation
if (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 filters
forward_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 landmarks
vector<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 command
orient = orient + turn + gen_gauss_random(0.0, turn_noise);
orient = mod(orient, 2 * M_PI);
// move, and add randomness to the motion command
double dist = forward + gen_gauss_random(0.0, forward_noise);
x = x + (cos(orient) * dist);
y = y + (sin(orient) * dist);
// cyclic truncate
x = mod(x, world_size);
y = mod(y, world_size);
// set particle
Robot 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 format
return "[x=" + to_string(x) + " y=" + to_string(y) + " orient=" + to_string(orient) + "]";
}
string read_sensors()
{
// Returns all the distances from the robot toward the landmarks
vector<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 be
double 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 poses
double forward_noise, turn_noise, sense_noise; //robot noises
private:
double gen_gauss_random(double mean, double variance)
{
// Gaussian random
normal_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. sigma
return 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 1
uniform_real_distribution<double> real_dist(0.0, 1.0); //Real
return real_dist(gen);
}
double mod(double first_term, double second_term)
{
// Compute the modulus
return first_term - (second_term)*floor(first_term / (second_term));
}
double evaluation(Robot r, Robot p[], int n)
{
//Calculate the mean error of the system
double sum = 0.0;
for (int i = 0; i < n; i++) {
//the second part is because of world's cyclicity
double 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 array
double 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 Format
plt::title("MCL, step " + to_string(step));
plt::xlim(0, 100);
plt::ylim(0, 100);
//Draw particles in green
for (int i = 0; i < n; i++) {
plt::plot({ p[i].x }, { p[i].y }, "go");
}
//Draw resampled particles in yellow
for (int i = 0; i < n; i++) {
plt::plot({ pr[i].x }, { pr[i].y }, "yo");
}
//Draw landmarks in red
for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++) {
plt::plot({ landmarks[i][0] }, { landmarks[i][1] }, "ro");
}
//Draw robot position in blue
plt::plot({ robot.x }, { robot.y }, "bo");
//Save the image and close the plot
plt::save("./Images/Step" + to_string(step) + ".png");
plt::clf();
}
int main()
{
//Practice Interfacing with Robot Class
Robot 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 particles
int 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 vector
myrobot = Robot();
vector<double> z;
//Iterating 50 times over the set of particles
int steps = 50;
for (int t = 0; t < steps; t++) {
//Move the robot and sense the environment afterwards
myrobot = myrobot.move(0.1, 5.0);
z = myrobot.sense();
// Simulate a robot motion for each of these particles
Robot 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 measurement
double 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 weight
Robot 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 Error
cout << "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 step
if (t % 5 == 0 )
{
visualization(n, myrobot, t, p2, p);
}
} //End of Steps loop
return 0;
}
编译程序
$ g ++ main.cpp -o app -std = c ++ 11 -I / usr / include / python2.7 -lpython2.7
运行程序
./app
效果如图
1022
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
