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πσ2)−12exp{ −12(x−μ)2σ2}G(x)=(2\pi\sigma^2)^{-\frac{1}{2}} exp\{ -\frac{1}{2} \frac{(x-\mu)^2}{\sigma^2} \} G(x)=(2πσ2)−21​exp{ −21​σ2(x−μ)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^{}$