PythonRobotics机器人算法-定位算法之粒子滤波定位(Particle filter localization)


1.概述

粒子滤波(Particle Filter,PF)的思想基于蒙特卡洛方法(Monte Carlo methods),它是利用粒子集来表示概率,可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布,是一种顺序重要性采样法(Sequential Importance Sampling)。简单来说,粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似,以样本均值代替积分运算,从而获得状态最小方差分布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。

尽管算法中的概率分布只是真实分布的一种近似,但由于非参数化的特点,它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约,能表达比高斯模型更广泛的分布,也对变量参数的非线性特性有更强的建模能力。因此,粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布,可以用于解决SLAM问题。

粒子滤波技术在非线性、非高斯系统表现出来的优越性,决定了它的应用范围非常广泛。另外,粒子滤波器的多模态处理能力,也是它应用广泛的原因之一。国际上,粒子滤波已被应用于各个领域。在经济学领域,它被应用在经济数据预测;在军事领域已经被应用于雷达跟踪空中飞行物,空对空、空对地的被动式跟踪;在交通管制领域它被应用在对车或人视频监控;它还用于机器人的全局定位。

虽然粒子滤波算法可以作为解决SLAM问题的有效手段,但是该算法仍然存在着一些问题。其中最主要的问题是需要用大量的样本数量才能很好地近似系统的后验概率密度。机器人面临的环境越复杂,描述后验概率分布所需要的样本数量就越多,算法的复杂度就越高。因此,能够有效地减少样本数量的自适应采样策略是该算法的重点。另外,重采样阶段会造成样本有效性和多样性的损失,导致样本贫化现象。如何保持粒子的有效性和多样性,克服样本贫化,也是该算法研究重点。

2.粒子滤波定位(Particle filter localization)实例

粒子滤波与卡尔曼滤波的本质区别有两个,第一个,准则不同,粒子滤波本质上是以最大后验概率为准则,而卡尔曼滤波本质上是以最小误差为准则;第二个重要区别是:粒子滤波不对系统做线性假设和后验概率的高斯假设,是一种非线性、非高斯滤波。下面展示一下基于python实现的粒子滤波定位。

(1)定义初始参数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
import numpy as np
import math
import matplotlib.pyplot as plt

#Estimation parameter of PF
Q = np.diag([0.1])* * 2                      # range error
R = np.diag([1.0, math.radians(40.0)])* * 2  # input error

#Simulation parameter
Qsim = np.diag([0.2])* * 2
Rsim = np.diag([1.0, math.radians(30.0)])* * 2

DT = 0.1          # time tick [s]
SIM_TIME = 50.0   # simulation time [s]
MAX_RANGE = 20.0  # maximum observation range

#Particle filter parameter
NP = 100        # Number of Particle
NTh = NP / 2.0  # Number of particle for re-sampling

show_animation = True

(2)算法过程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
def calc_input():
    v = 1.0        # [m/s]
    yawrate = 0.1  # [rad/s]
    u = np.matrix([v, yawrate]).T
    return u

def observation(xTrue, xd, u, RFID):
    xTrue = motion_model(xTrue, u)
    # add noise to gps x-y
    z = np.matrix(np.zeros((0, 3)))
    for i in range(len(RFID[:, 0])):
        dx = xTrue[0, 0] - RFID[i, 0]
        dy = xTrue[1, 0] - RFID[i, 1]
        d = math.sqrt(dx* * 2 + dy* * 2)
        if MAX_RANGE > d:
            dn = d + np.random.randn() * Qsim[0, 0]  # add noise
            zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
            z = np.vstack((z, zi))
    # add noise to input
    ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
    ud = np.matrix([ud1, ud2]).T
    xd = motion_model(xd, ud)
    return xTrue, z, xd, ud

def motion_model(x, u):
    F = np.matrix([[1.0, 0, 0, 0],
                   [0, 1.0, 0, 0],
                   [0, 0, 1.0, 0],
                   [0, 0, 0, 0]])
    B = np.matrix([[DT * math.cos(x[2, 0]), 0],
                   [DT * math.sin(x[2, 0]), 0],
                   [0.0, DT],
                   [1.0, 0.0]])
    x = F * x + B * u
    return x

def gauss_likelihood(x, sigma):
    p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * math.exp(-x ** 2 / (2 * sigma ** 2))
    return p

def calc_covariance(xEst, px, pw):
    cov = np.matrix(np.zeros((3, 3)))
    for i in range(px.shape[1]):
        dx = (px[:, i] - xEst)[0:3]
        cov += pw[0, i] * dx * dx.T
    return cov

def pf_localization(px, pw, xEst, PEst, z, u):
    """
    Localization with Particle filter
    """

    for ip in range(NP):
        x = px[:, ip]
        w = pw[0, ip]
        #  Predict with ramdom input sampling
        ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
        ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
        ud = np.matrix([ud1, ud2]).T
        x = motion_model(x, ud)
        #  Calc Inportance Weight
        for i in range(len(z[:, 0])):
            dx = x[0, 0] - z[i, 1]
            dy = x[1, 0] - z[i, 2]
            prez = math.sqrt(dx* * 2 + dy* * 2)
            dz = prez - z[i, 0]
            w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))
        px[:, ip] = x
        pw[0, ip] = w
    pw = pw / pw.sum()  # normalize
    xEst = px * pw.T
    PEst = calc_covariance(xEst, px, pw)
    px, pw = resampling(px, pw)
    return xEst, PEst, px, pw

def resampling(px, pw):
    """
    low variance re-sampling
    """

    Neff = 1.0 / (pw * pw.T)[0, 0]  # Effective particle number
    if NTh > Neff:
        wcum = np.cumsum(pw)
        base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
        resampleid = base + np.random.rand(base.shape[1]) / NP
        inds = []
        ind = 0
        for ip in range(NP):
            while resampleid[0, ip] > wcum[0, ind]:
                ind += 1
            inds.append(ind)
        px = px[:, inds]
        pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP  # init weight
    return px, pw

(3)结果可视化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
def plot_covariance_ellipse(xEst, PEst):
    Pxy = PEst[0:2, 0:2]
    eigval, eigvec = np.linalg.eig(Pxy)
    if eigval[0] >= eigval[1]:
        bigind = 0
        smallind = 1
    else:
        bigind = 1
        smallind = 0
    t = np.arange(0, 2 * math.pi + 0.1, 0.1)
    #eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
    #close to 0 (~10^-20), catch these cases and set the respective variable to 0
    try: a = math.sqrt(eigval[bigind])
    except ValueError: a = 0
    try: b = math.sqrt(eigval[smallind])
    except ValueError: b = 0
    x = [a * math.cos(it) for it in t]
    y = [b * math.sin(it) for it in t]
    angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
    R = np.matrix([[math.cos(angle), math.sin(angle)],
                   [-math.sin(angle), math.cos(angle)]])
    fx = R * np.matrix([x, y])
    px = np.array(fx[0, :] + xEst[0, 0]).flatten()
    py = np.array(fx[1, :] + xEst[1, 0]).flatten()
    plt.plot(px, py, "--r")

def main():
    print(_ _ file _ _ + " start!!")
    time = 0.0
    # RFID positions [x, y]
    RFID = np.array([[10.0, 0.0],
                     [10.0, 10.0],
                     [0.0, 15.0],
                     [-5.0, 20.0]])
    # State Vector [x y yaw v]'
    xEst = np.matrix(np.zeros((4, 1)))
    xTrue = np.matrix(np.zeros((4, 1)))
    PEst = np.eye(4)
    px = np.matrix(np.zeros((4, NP)))  # Particle store
    pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP  # Particle weight
    xDR = np.matrix(np.zeros((4, 1)))  # Dead reckoning
    # history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue
    while SIM_TIME >= time:
        time += DT
        u = calc_input()
        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
        xEst, PEst, px, pw = pf_localization(px, pw, xEst, PEst, z, ud)
        # store data history
        hxEst = np.hstack((hxEst, xEst))
        hxDR = np.hstack((hxDR, xDR))
        hxTrue = np.hstack((hxTrue, xTrue))
        if show_animation:
            plt.cla()
            for i in range(len(z[:, 0])):
                plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k")
            plt.plot(RFID[:, 0], RFID[:, 1], "*k")
            plt.plot(px[0, :], px[1, :], ".r")
            plt.plot(np.array(hxTrue[0, :]).flatten(),
                     np.array(hxTrue[1, :]).flatten(), "-b")
            plt.plot(np.array(hxDR[0, :]).flatten(),
                     np.array(hxDR[1, :]).flatten(), "-k")
            plt.plot(np.array(hxEst[0, :]).flatten(),
                     np.array(hxEst[1, :]).flatten(), "-r")
            plot_covariance_ellipse(xEst, PEst)
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)

if _ _ name _ _ == '_ _ main _ _':
    main()

执行结果如下。该算法利用粒子滤波实现传感器融合定位。蓝色曲线为实际轨迹,黑色曲线为航位推算轨迹,红色曲线为粒子滤波算法估算的轨迹。该算法假设机器人能够测量与地标(RFID)之间的距离,而粒子滤波定位需要用该测量结果。

参考文献

  1. https://zh.wikipedia.org/wiki/粒子滤波器
  2. https://baike.baidu.com/item/粒子滤波/2128986?fr=aladdin
  3. https://pythonrobotics.readthedocs.io/en/latest/
  4. https://github.com/AtsushiSakai/PythonRobotics/
  5. PythonRobotics: a Python code collection of robotics algorithms
  6. http://www.probabilistic-robotics.org/
  7. 胡士强, 敬忠良. 粒子滤波原理及其应用[M]. 科学出版社, 2010.

进化学习团队将会根据大家意见和建议持续修改、维护与更新。转载请注明出处(进化学习: https://www.evolutionarylearn.com/paper/ra-pythonrobotics-localization-pfl/)。

赞赏

微信赞赏支付宝赞赏

Have any Question or Comment?

发表评论

电子邮件地址不会被公开。 必填项已用*标注

热门主题 & 页面

分类目录

博客统计

无点击次数。