无人驾驶7：粒子滤波

粒子滤波定位原理和过程:

1.初始化：在已知地图上随机初始化N个粒子(N取合适值，太小，不足以估算真实位置，太大，增大计算量)；

2.运动更新：根据运动模型，更新每个粒子状态；

3.测量更新：根据传感器测量融合信息，更新所有粒子状态；

4.重采样： 根据步骤3中测量更新后的状态信息，重新采集样本，置信度越高的点给予更高权重，反之权重越低；

重复2,3,4过程。

说明：

$\underline{粒子滤波本质上还是基于贝叶斯滤波原理，与无损卡尔曼滤波有些类似，不同的是，这里采用随机的粒子，无损卡尔曼滤波采用特定粒子。}$

下面以一个实例来实现详细过程:

假设场景：

在一个100x100的二维地图上，有四个landmark标记(可以用传感器测量相对车辆的距离)，用粒子滤波实现无人车定位。

step1:初始化

在地图上初始化1000个随机粒子，每个粒子状态包括二维坐标 $p_x, p_y$, 偏向角 $\sigma$;

landmarks  = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0

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

step2: 运动更新

为计算简便，假设车辆为恒速的自行车模型，车辆运动的位移和转向角都符合高斯分布；

代码实现：

def move(self, turn, forward):
        if forward < 0:
            raise ValueError('Robot cant move backwards')         
        
        # turn, and add randomness to the turning command
        orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
        orientation %= 2 * pi
        
        # move, and add randomness to the motion command
        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
        
        # set particle
        res = robot()
        res.set(x, y, orientation)
        res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
        return res

step3: 测量更新

传感器可以测量车辆相对landmark的距离，根据贝叶斯公式，可以估算当前粒子的存在概率：

def sense(self):
        Z = []
        for i in range(len(landmarks)):
            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
def measurement_prob(self, measurement):
        
        # calculates how likely a measurement should be
        
        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

当然，还有个很重要的步骤，测量更新后，必须做归一化。

step4: 重采样

测量更新后的1000个粒子，都有各自状态向量和置信度。我们需要对这些粒子做一个重新采用，留下置信度高的粒子，丢掉置信度低的粒子。

方法是， $\underline{给每个粒子分配一个权重值，置信度越高，也就是越靠近真实值的粒子，权重越高。}$

伪代码实现：

代码实现：

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

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

w = []
for i in range(N):
    w.append(p[i].measurement_prob(Z))
    
p3 = []
index = int(random.random()*N)
beta = 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
for it in p:
    print(it) 

执行一个周期，观察结果，坐标值都相近，但偏航角差别很大，这是因为测量更新只考虑距离，每检测偏航角；滤波周期执行多次，即可纠正偏航角。

更新权重后，得到如下粒子分布；

以上就是粒子滤波的实现过程，基本原理基于贝叶斯公式，

误差计算

每个滤波周期都计算残差来衡量估算的准确性。

def eval(r, p):
    sum = 0.0;
    for i in range(len(p)): # calculate mean error
        dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
        dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
        err = sqrt(dx * dx + dy * dy)
        sum += err
    return sum / float(len(p))

完整代码实现：

# In this exercise, write a program that will
# run your previous code twice.
# Please only modify the indicated area below!

from math import *
import random

landmarks  = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0

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;
    
    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)
    
    
    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);
    
    
    def sense(self):
        Z = []
        for i in range(len(landmarks)):
            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
    
    
    def move(self, turn, forward):
        if forward < 0:
            raise ValueError('Robot cant move backwards')         
        
        # turn, and add randomness to the turning command
        orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
        orientation %= 2 * pi
        
        # move, and add randomness to the motion command
        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
        
        # set particle
        res = robot()
        res.set(x, y, orientation)
        res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
        return res
    
    def Gaussian(self, mu, sigma, x):
        
        # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
        return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
    
    
    def measurement_prob(self, measurement):
        
        # calculates how likely a measurement should be
        
        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
      
    def __repr__(self):
        return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))

def eval(r, p):
    sum = 0.0;
    for i in range(len(p)): # calculate mean error
        dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
        dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
        err = sqrt(dx * dx + dy * dy)
        sum += err
    return sum / float(len(p))

myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
T = 20 #Leave this as 10 for grading purposes.

p = []
for i in range(N):
    r = robot()
    r.set_noise(0.05, 0.05, 5.0)
    p.append(r)
print(eval(myrobot,p))
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

    w = []
    for i in range(N):
        w.append(p[i].measurement_prob(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
    #enter code here, make sure that you output 10 print statements.
    print(eval(myrobot,p))

38.56806777763878
4.223734544168689
3.623085199035639
1.5824953749965067
1.2021239349056985
1.3621575808132773
1.4713924981368087
1.6472471763744074
1.7397327076620819
1.798154509807016
1.7167944621956188
1.693532590208658
1.670570869829746
1.661596686869687
1.6772282610901224
1.7292401916400235
1.8071726509916188
1.8811843973410884
1.9572594169118382
1.9254692875952362
1.8669139729295758

粒子滤波器与卡尔曼滤波，直方图滤波对比

说明：粒子滤波器容易实现，但不适合三维以上模型