蒙特卡洛粒子滤波定位算法_机器人粒子滤波定位（蒙特卡罗定位）

机器人定位问题

General schematic for mobile robot localization

以下面的两幅图a、b为例，对移动机器人定位问题进行说明。假如机器人从一个已知的位置开始运动，利用里程计(编码器)可以对其运动进行跟踪，由于里程计的误差，运动一段时间后，机器人对其位置的不确定度将变得很大。为了使位置的不确定性不至于无限增大，机器人必须参考其外部环境对自己进行定位。为了进行定位，机器人会使用外部传感器(例如，超声、激光、视觉传感器等)对环境进行测量。如下图a所示的预测阶段：起始位置x0已知，所以该位置上概率密度函数是δ函数(狄拉克函数)。当机器人开始移动，由于里程计的误差，它的不确定性增加，并随时间积累(In short, mobile robot effectors introduce uncertainty about future state. Therefore the simple act of moving tends to increase the uncertainty of a mobile robot)。下图b所示为感知阶段：机器人利用外部传感器测量到右墙的距离为d，计算出当前位置为x'2，这与预测的位置x2有冲突。融合两个估计的概率密度，将位置校正，其不确定性得到减小(图b中的实线)。

The MCL Algorithm

1. Randomly generate a bunch of particles

Particles can have position, heading, and/or whatever other state variable you need to estimate. Each has a weight (probability) indicating how likely it matches the actual state of the system. Initialize each with the same weight.

2. Predict next state of the particles

Move the particles based on how you predict the real system is behaving.

3. Update

Update the weighting of the particles based on the measurement. Particles that closely match the measurements are weighted higher than particles which don't match the measurements very well.

4. Resample

Discard highly improbable particle and replace them with copies of the more probable particles.

5. Compute Estimate

Optionally, compute weighted mean and covariance of the set of particles to get a state estimate.

粒子滤波定位又称为蒙特卡罗定位(MCL, or Monte Carlo Localization, a localization algorithm based on particlefilters)。粒子滤波的基本步骤为上面所述的5步，其本质是使用一组有限的加权随机样本(粒子)来近似表征任意状态的后验概率密度。粒子滤波的优势在于对复杂问题的求解上，比如高度的非线性、非高斯动态系统的状态递推估计或概率推理问题。下面考虑一个2维平面内机器人定位的问题。机器人定位技术可分为相对定位和绝对定位两类。相对定位是通过测量机器人相对于初始位置的距离和方向来确定机器人的当前位置，通常称为航迹推算法，常用的传感器包括里程计及惯性导航系统(陀螺仪、加速度计等)；绝对定位主要采用导航信标、主动或被动标识、地图匹配或卫星导航技术进行定位。下图中的机器人采用路标定位，路标一般定义为环境中被动的物体，当它们处在机器人传感器测量范围内时，提供了高的定位准确度。基于路标的导航中，一般路标都有固定的已知位置，机器人自身传感器能对路标进行识别，通过与路标间的相对定位实现机器人在地图中的绝对定位。如下图所示，机器人通过视觉传感器识别球场边上用特定颜色标记的柱子(landmarks)来定位。

基本的MCL算法步骤如下：

即用M个粒子来描述状态向量的后验概率分布。算法第4行从系统转移模型中取样，将这些取样的粒子作为先验值。第5行使用测量模型修正粒子的权值。下面举一个简单的例子，来理解MCL算法：

如下图(a)所示，机器人在水平方向沿一维直线移动。为了推测自身位置，初始时刻在其运动范围内随机生成均匀分布的粒子。粒子的X轴坐标表示推测的机器人位置，Y坐标表示处于这个位置的概率。机器