Python模拟粒子滤波器定位过程

前言

定位技术在无人驾驶汽车与自主机器人领域有着关键作用，基于概率的常用的手段是借助Range Sensor(通常使用的是激光雷达)，或者摄像头(单目或者双目)进行前期建图，在定位时将传感器扫描得到的数据与地图进行匹配，并结合使用滤波器，确定机器人状态向量在状态空间中的概率分布。本文将忽略扫描匹配的具体过程，基于已经获得高精度地图，可以直接测量地标距离这一前提，使用粒子滤波器进行二维平面中的机器人定位过程的模拟。机器人默认为原型两轮机器人，使用差速方式转向。

编程实现

库

from matplotlib import pyplot as plt
from math import *
import random

需要使用的库

• random
  用于生成粒子
• math
  各种计算

Robot类

以下介绍Robot类中的Method

1. 初始化

class robot:
    def __init__(self):
        # 以下三行用于随机生成粒子
        self.x = random.random() * world_size
        self.y = random.random() * world_size
        self.orientation = random.random() * 2.0 * pi
        # 设定噪声
        self.forward_noise = 0.0
        self.turn_noise    = 0.0
        self.sense_noise   = 0.0

机器人的位姿包括在二维平面中的坐标，以及方位角。方位角为0的方向即X轴正方向。开始时机器人的位姿是未知的，需要使粒子遍布整个“世界”。
而噪声包括运动噪声与测量噪声。运动噪声又分为向前运动的噪声与转向的噪声。

关于噪声

传感器的测量都是有误差的，本文将传感器的测量值建模为真实值加上高斯噪声，用来模拟真实环境下的传感器测量值。程序中设定的noise为高斯噪声的方差。初始化为0

2. 位置设定

    def set(self, new_x, new_y, new_orientation):
        if new_x < 0 or new_x >= world_size:
            raise (ValueError, 'X coordinate out of bound')
        if new_y < 0 or new_y >= world_size:
            raise (ValueError, 'Y coordinate out of bound')
        if new_orientation < 0 or new_orientation >= 2 * pi:
            raise (ValueError, 'Orientation must be in [0..2pi]')
        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation)

初始化中将机器人的位置设定为随机值，是为了方便生成粒子，而这个method是用来设定机器人的实际位姿。

3. 噪声设定

    def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.forward_noise = float(new_f_noise);
        self.turn_noise    = float(new_t_noise);
        self.sense_noise   = float(new_s_noise);

噪声的设定，前面解释过了。机器人本身以及每个粒子的噪声值都需要设定

4. 距离测量

    def sense(self):
        # measure distances of landmarks and get a result with gaussian noise
        Z = []
        for i in range(len(landmarks)):
            # true value
            dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
            dist += random.gauss(0.0, self.sense_noise)
            Z.append(dist)
        return Z

测量机器人到各个地标的距离，依然是真实值与高斯噪声之和。
其中真实值的计算利用勾股定理，高斯噪声则是使用了随机数中gauss()方法

5. 移动

    def move(self, turn, forward):
        if forward < 0:
            raise (ValueError, 'Robot cant move backwards')
        # 转向
        orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
        orientation %= 2 * pi
        
        # 前进
        dist = float(forward) + random.gauss(0.0, self.forward_noise)
        x = self.x + (cos(orientation) * dist)
        y = self.y + (sin(orientation) * dist)
        x %= world_size    # cyclic truncate
        y %= world_size
        
        # 返回移动后的机器人
        res = robot()
        res.set(x, y, orientation)
        res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
        return res

我们假设机器人只能原地转向或者向前移动，在每次移动的过程中先转向，然后前进，这个method更新机器人与粒子的位姿，返回一个新的对象

6. 高斯函数

    def Gaussian(self, mu, sigma, x):
        return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))

高斯函数的公式如下
$$G(x)=(2\pi {\sigma }^2{)}^{-\frac{1}{2}}exp\{-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2}\}$$

7. 权重计算

    def measurement_prob(self, measurement):
        prob = 1.0
        for i in range(len(landmarks)):
            dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
            prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
        return prob

这是粒子滤波器的关键一步。在粒子滤波器的迭代依赖于基于权值的重采样，权值的大小决定了一次迭代中哪些粒子能够存。

权值的计算

基于权值的重采样相当于卡尔曼滤波器中的Measurement Update。首先机器人本身对路标进行距离测量。例如对$Landmar{k}_i$进行测量，得到测量值$Z_i$。而对于每个粒子来说，权重即为在该粒子所在的点测量到$Z_i$的“概率”。

举例说明

机器人的真实坐标为$(x_0,y_0)$，路标的坐标为$(x_{L_1},y_{L_1})$，则真实的距离为
$$dist=\sqrt{(x_0-x_{L1}{)}^2+(y_0-y_{L1}{)}^2}$$
而根据模型，测量到的距离还要在此基础上加上高斯误差，即为$Z_i$
测量的高斯噪声方差为${\sigma }_Z^2$ ，均值$\mu =0$
假设粒子群中的某个粒子$p_i$坐标为$(x_{pi},y_{pi})$ (这不是测出来的，这是随机生成的，因此真实坐标是已知的)。同样利用勾股定理计算出$p_i$与$Landmar{k}_i$的距离$dis{t}_{p_i}$
回顾一下高斯函数
$$G(x)=(2\pi {\sigma }^2{)}^{-\frac{1}{2}}exp\{-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2}\}$$
将它改写为
$$G(\mu ,{\sigma }^2,x)=(2\pi {\sigma }^2{)}^{-\frac{1}{2}}exp\{-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2}\}$$
则$G(dis{t}_p,{\sigma }_Z^2,Z_i)$即为在粒子$p_i$处测量到$Z_i$的“概率”。(这里概率要加引号是由于高斯函数的值并不等于单点的概率)
由于我们的路标有多个，因此需要将对于每个路标的概率相乘。因此权重
$$w_{p_i}=\sum _{i=1}^{n}G(dis{t}_p,{\sigma }_Z^2,Z_i)$$

主程序

myrobot = robot()
myrobot = myrobot.set_noise(0.05, 0.05, 5.0)
Z = myrobot.sense()
N = 1000
T = 10 

创建一个机器人对象，并对路标进行测量。$Z$保存测量的值。$N$与$T$分别为粒子数量与迭代次数。

p = []
for i in range(N):
    r = robot()
    r.set_noise(0.05, 0.05, 5.0)
    p.append(r)

创建粒子群，$p$是一个用来保存粒子群的列表。robot类的初始化函数可以随机生成一个粒子，且使其在二维平面内服从均匀分布。由此生成1000个粒子的表示的初始分布。

以下开始进行迭代

for t in range(T):
    myrobot = myrobot.move(0.1, 5.0)
    Z = myrobot.sense()

移动机器人并进行一次测量。

    p2 = []
    for i in range(N):
        p2.append(p[i].move(0.1, 5.0))
    p = p2

移动所有的粒子，更新它们的位置，并显示出来。
我们知道在卡尔曼滤波器中，运动更新将会增加机器人位置测量的不确定性。这里由于robot.move()这个方法是引入了高斯噪声的，因此每个粒子移动的距离以及转过的角度都会产生一定差异，从而看上去有种发散的感觉。

    w = []
    for i in range(N):
        w.append(p[i].measurement_prob(Z))

计算每个粒子的权重。前面说过了，权重就是在该粒子处测量到$Z$的“概率”。

重采样

    p3 = []
    index = int(random.random() * N)
    beta = 0.0
    mw = max(w)
    for i in range(N):
        beta += random.random() * 2.0 * mw
        while beta >  w[index]:
            beta -= w[index]
            index = (index + 1) % N
        p3.append(p[index])
    p = p3

我称之为 幸运轮盘 算法 (Wheel of Fortune)

粒子滤波器的迭代依赖于基于权重的重采样，权重一定程度上表示了在重采样的过程中，采到该粒子的概率，也可以认为权重反映了粒子的概率分布。在实现重采样的过程中，我们将所有粒子的集合看作一个轮盘，用饼状图来表示权重大小，权重越大的粒子占据的圆弧越长，权重越小的粒子占的越短。例如这张图上被分成10块，分别代表10个粒子，每块的大小表示权重的大小。可以看到区域5表示的粒子权重最大，因此，在这个粒子处的测量值与机器人本身的测量值一致的可能性最高。

然后从中随机取$N$个值。取到的粒子即组成新的粒子群。就像幸运轮盘一样，让它转起来，看指针停在哪个区域。在上图中区域5占据圆弧最大，重采样采到这个粒子的概率就比较高，而像区域4这种，就可能在重采样中被淘汰。因此你转动轮盘10次，有可能指针停在区域5有2次，而一次也没有停在区域4，这样你重采样得到的10个粒子，就出现了两个重合的，看起里就好像是区域4的粒子消失了。这就是粒子滤波器的迭代过程。

虽然粒子在测量更新(Measurement Update)中被淘汰，但由于运动更新(Motion Update)中还存在误差，所以在每一次运动更新之后，会感觉粒子好像散开了，然后在下一次测量更新中再次收敛，如此循环往复。

结果

按照上面的流程进行运行，并使用matplotlib进行可视化处理得到结果如下

初始时按照据均匀分布随机生成了1000个粒子，并和机器人本体做相同运动，走了一段距离。即经过一次运动更新。

然后计算权重，重采样得到的粒子如下，可以看到很多粒子“消失了”，并产生了很多重叠。

机器人再次运动，粒子也随之运动，由于高斯噪声的存在，粒子群再次扩散开来。

再次经过测量更新，粒子开始集中。

又一次运动更新。

测量更新。可以看到粒子已经比较集中了。

传感器的噪声参数会影响滤波器的效果，而由于其中存在很多的随机过程，每次运行的效果也可能存在很大差异。增加粒子数量可以一定程度上优化定位效果，但可能给处理器带来运算上的负担。

待修改、更新