蒙特卡洛粒子滤波定位算法_蒙特卡罗定位算法(基于粒子滤波的定位算法) ——原理、理解与仿真...

1 算法原理

1.1 机器人定位问题

关于机器人定位，有三大问题，它们分别是：

(1)“全局定位”：指初始位置未知，机器人靠自身运动确定自己在地图中的位姿。

(2)“位姿跟踪”：指已知自身位姿或者已经通过“全局定位”得到了一个较好的位姿估计，在后续运动时补偿精度较差的运动控制误差；

(3)“绑架劫持”：指机器人在已知自身位姿的情况下，得到了一个错误的位姿信息或者外界将其放到另外一个位姿，而里程计信息给出了错误的信息甚至没有给出控制信息。

1.2 粒子滤波步骤(可结合2中例题)

(1)初始状态：用大量粒子模拟运动状态，使粒子在空间内均匀分布；

(2)预测阶段：根据状态转移方程，将每一个粒子带入，得到一个预测粒子；

(3)校正阶段：对预测粒子进行评价(计算权重)，越接近于真实状态的粒子，其权重越大；

(4)重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；

(5)滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤(2)。

2 一个二维定位的例子 (基于粒子滤波的定位算法)

2.1 问题

在二维空间，假设运动小车的初始位置、状态方程(运动预测方程)、传感器测量数据，用粒子滤波方法进行对其进行定位。

2.2 预设参数

(1)粒子总数N=200;

(2)运动时间T=10秒，且假设每秒进行一步动作;

(3)运动场地大小WorldSize*WorldSize=100*100平方米;

(4)控制小车运动的方程，小车沿着某曲线运动(但实际的运动情况肯定和给的控制有些差别，这理解为叠加在理想运动方程上的控制误差，控制误差假设为5米)；

(5)传感器测量的小车位置数据同样也和实际情况不一致，这也可以理解为叠加在真实位置上的测量误差，测量误差假设为5米；

(6)小车初始位置已知，假设处在100*100场地中的(50,20)位置；

2.3 步骤与理解分析

(1)初始化粒子群

在整个场地内，将总粒子数N进行均匀分布，得到如图所示结果。

(2)小车开始运动(同时每运动一步进行一次测量)

小车按照控制给定的叠加了控制噪声的运动方程进行运动，运动达到下一位置后，传感器对当前位置进行测量，并得到一个测量的位置(不准确,含有测量噪声)

(3)粒子群更新(预测步骤)

把粒子群中的全部粒子逐个带入小车的运动方程中，得到粒子群的下一步位置。同时，计算此时每个粒子的位置和测量得到的小车位置 这两个位置之间的几何距离，按照距离的不同给每个粒子添加一个权重，用于重采样。显然，距离越近，权重越大(权重和距离关系的函数ÿ