PythonRobotics机器人算法-SLAM算法之基于SLAM的扩展卡尔曼滤波(Extended kalman filter based SLAM)


1.概述

卡尔曼滤波是由卡尔曼(Kalman)提出的用于时变线性系统的递归滤波器。Sunahara等人提出了扩展卡尔曼滤波(extended Kalman filter,EKF),通过将非线性的系统线性化,将卡尔曼滤波应用到非线性化领域。Smith等人奠定了SLAM问题的全状态估计架构,提出使用EKF来估计状态向量的均值和方差。Bailey等人对EKF-SLAM算法进行详细地论述和实验验证,同时对EKF-SLAM算法进行了优化,使得对于状态向量维数很高时,算法效率得到了提高。当路标数量较多时,EKF-SLAM计算代价相当可观。同时由于EKF在线性化的过程中,无可避免的引入了线性化误差,这样对于状态向量的估计难以达到最优。为了降低计算复杂度和减少线性化误差,研宄人员提出一系列改进方法。基于压缩扩展卡尔曼滤波(compressed extended Kalman filter,CEKF)的SLAM算法在不改变滤波的基础上,通过局部地图划分来提高算法效率。Nerurkar提出提前保留延迟过程中的主要信息以实现信息丢失的最小化Power-SLAM算法。降低计算复杂度的方法还有不变扩展卡尔曼滤波(Invariant extended Kalman filter,IEKF)、平均扩展卡尔曼滤波(Mean extended Kalman filter,MEKF)、分层(Hierarchical)SLAM等。对于提高算法精确度主要从运动模型的改进和减少由线性化带来的误差着手。Skraypczynsk利用两次扫描匹配间的平移和角度变化建立了新的里程计模型,减少了方向角估计误差。也有学者将模糊逻辑、神经网络等智能算法应用到里程计误差校准中。无迹卡尔曼滤波(unscented Kalman filter, UKF)使用UT(unscented transform)变换通过Sigma点重构出状态向量的统计特性(均值和误差),为非线性化的SLAM问题提供了一种解决思路。Martinez-Cantin等人提出将UKF应用于机器人位姿和地图的估计。曲丽萍等人提出基于平方根的无迹卡尔曼滤波,在具有与UKF滤波相同的精度下,确保了计算的稳定性和协方差矩阵的半正定性。在UKF-SLAM的基础上,学者们提出迭代平方根的UKF-SLAM、AUKF-SLAM(augmented UKF-SLAM)等算法。更多信息可参见《PythonRobotics机器人算法-定位算法之扩展/无损卡尔曼滤波定位》

2.基于SLAM的扩展卡尔曼滤波

下面展示一下基于python实现基于SLAM的扩展卡尔曼滤波。

(1)定义初始参数

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

#EKF state covariance
Cx = np.diag([0.5, 0.5, math.radians(30.0)])* * 2

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

DT = 0.1          # time tick [s]
SIM_TIME = 50.0   # simulation time [s]
MAX_RANGE = 20.0  # maximum observation range
M_DIST_TH = 2.0   # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3    # State size [x,y,yaw]
LM_SIZE = 2       # LM srate size [x,y]

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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
def ekf_slam(xEst, PEst, u, z):
    # Predict
    S = STATE_SIZE
    xEst[0:S] = motion_model(xEst[0:S], u)
    G, Fx = jacob_motion(xEst[0:S], u)
    PEst[0:S, 0:S] = G.T * PEst[0:S, 0:S] * G + Fx.T * Cx * Fx
    initP = np.eye(2)
    # Update
    for iz in range(len(z[:, 0])):  # for each observation
        minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2])
        nLM = calc_n_LM(xEst)
        if minid == nLM:
            print("New LM")
            # Extend state and covariance matrix
            xAug = np.vstack((xEst, calc_LM_Pos(xEst, z[iz, :])))
            PAug = np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))),
                              np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
            xEst = xAug
            PEst = PAug
        lm = get_LM_Pos_from_state(xEst, minid)
        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
        K = PEst * H.T * np.linalg.inv(S)
        xEst = xEst + K * y
        PEst = (np.eye(len(xEst)) - K * H) * PEst
    xEst[2] = pi_2_pi(xEst[2])
    return xEst, PEst

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 = RFID[i, 0] - xTrue[0, 0]
        dy = RFID[i, 1] - xTrue[1, 0]
        d = math.sqrt(dx* * 2 + dy* * 2)
        angle = pi_2_pi(math.atan2(dy, dx))
        if d <= MAX_RANGE:
            dn = d + np.random.randn() * Qsim[0, 0]          # add noise
            anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise
            zi = np.matrix([dn, anglen, i])
            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, 1.0, 0],
                   [0, 0, 1.0]])
    B = np.matrix([[DT * math.cos(x[2, 0]), 0],
                   [DT * math.sin(x[2, 0]), 0],
                   [0.0, DT]])
    x = F * x + B * u
    return x

def calc_n_LM(x):
    n = int((len(x) - STATE_SIZE) / LM_SIZE)
    return n

def jacob_motion(x, u):
    Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
        (STATE_SIZE, LM_SIZE * calc_n_LM(x)))))
    jF = np.matrix([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
                    [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
                    [0.0, 0.0, 0.0]])
    G = np.eye(STATE_SIZE) + Fx.T * jF * Fx
    return G, Fx,

def calc_LM_Pos(x, z):
    zp = np.zeros((2, 1))
    zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
    zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
    return zp

def get_LM_Pos_from_state(x, ind):
    lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]
    return lm

def search_correspond_LM_ID(xAug, PAug, zi):
    """
    Landmark association with Mahalanobis distance
    """

    nLM = calc_n_LM(xAug)
    mdist = []
    for i in range(nLM):
        lm = get_LM_Pos_from_state(xAug, i)
        y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
        mdist.append(y.T * np.linalg.inv(S) * y)
    mdist.append(M_DIST_TH)  # new landmark
    minid = mdist.index(min(mdist))
    return minid

def calc_innovation(lm, xEst, PEst, z, LMid):
    delta = lm - xEst[0:2]
    q = (delta.T * delta)[0, 0]
    zangle = math.atan2(delta[1], delta[0]) - xEst[2]
    zp = [math.sqrt(q), pi_2_pi(zangle)]
    y = (z - zp).T
    y[1] = pi_2_pi(y[1])
    H = jacobH(q, delta, xEst, LMid + 1)
    S = H * PEst * H.T + Cx[0:2, 0:2]
    return y, S, H

def jacobH(q, delta, x, i):
    sq = math.sqrt(q)
    G = np.matrix([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
                   [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])
    G = G / q
    nLM = calc_n_LM(x)
    F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM))))
    F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))),
                    np.eye(2), np.zeros((2, 2 * nLM - 2 * i))))
    F = np.vstack((F1, F2))
    H = G * F
    return H

def pi_2_pi(angle):
    return (angle + math.pi) % (2 * math.pi) - math.pi

(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
def main():
    print(_ _file__ + " start!!")
    time = 0.0
    # RFID positions [x, y]
    RFID = np.array([[10.0, -2.0],
                     [15.0, 10.0],
                     [3.0, 15.0],
                     [-5.0, 20.0]])
    # State Vector [x y yaw v]'
    xEst = np.matrix(np.zeros((STATE_SIZE, 1)))
    xTrue = np.matrix(np.zeros((STATE_SIZE, 1)))
    PEst = np.eye(STATE_SIZE)
    xDR = np.matrix(np.zeros((STATE_SIZE, 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 = ekf_slam(xEst, PEst, ud, z)
        x_state = xEst[0:STATE_SIZE]
        # store data history
        hxEst = np.hstack((hxEst, x_state))
        hxDR = np.hstack((hxDR, xDR))
        hxTrue = np.hstack((hxTrue, xTrue))
        if show_animation:
            plt.cla()
            plt.plot(RFID[:, 0], RFID[:, 1], "*k")
            plt.plot(xEst[0], xEst[1], ".r")
            # plot landmark
            for i in range(calc_n_LM(xEst)):
                plt.plot(xEst[STATE_SIZE + i * 2],
                         xEst[STATE_SIZE + i * 2 + 1], "xg")
            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")
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)

if _ _ name__ == '_ _main__':
    main()

执行结果如下。下面为基于SLAM的扩展卡尔曼滤波实例。蓝色曲线为实际轨迹,黑色曲线为航位推算轨迹,红色曲线为基于SLAM的扩展卡尔曼滤波算法估算的轨迹。绿色叉点为估计的地标。

参考文献

  1. 吕太之. 移动机器人路径规划和地图创建研究[D]. 南京理工大学, 2017.
  2. PythonRobotics机器人算法-定位算法之扩展/无损卡尔曼滤波定位(Extended/Unscented Kalman Filter localization)
  3. Musoff H, Zarchan P. Fundamentals of Kalman filtering: a practical approach[M]. American Institute of Aeronautics and Astronautics, 2009.
  4. Kalman R E. A new approach to linear filtering and prediction problems[J]. Journal of basic Engineering, 1960, 82(1): 35-45.
  5. https://zh.wikipedia.org/wiki/卡尔曼滤波
  6. SLAM-同步定位与地图构建
  7. http://www.probabilistic-robotics.org/
  8. https://pythonrobotics.readthedocs.io/en/latest/
  9. https://github.com/AtsushiSakai/PythonRobotics/
  10. PythonRobotics: a Python code collection of robotics algorithms

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

赞赏

微信赞赏支付宝赞赏

Have any Question or Comment?

发表评论

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

分类目录

博客统计

  • 15,775 点击次数