Udacity机器人软件工程师课程笔记（三十三) - 蒙特卡洛定位算法（MCL）

一、概述

之前的文章介绍过卡尔曼滤波算法进行定位，我们知道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()
    {
   
   
        // 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