python如何实现小车行走_[详细推导]基于EKF的小车运动模型的python编程实现

一、任务介绍

在本任务中，您将使用可用的测量值和运动模型来递归估计车辆沿轨迹的位置。车辆有了非常简单的LIDAR传感器，可以返回与环境中各个地标相对应的范围(range)和方位测量值(bearing)。假定地标的所有位置是已知的并假设他们已知的数据关联，即哪个度量属于哪个界标。

数据链接：https://pan.baidu.com/s/10jQlD0fOyJ81NWP2FOoxAA​pan.baidu.com

提取码：dzkl

二、运动和测量模型

1.运动模型

车辆运动模型将线速度和角速度里程计读数作为输入，并输出车辆的状态(即2D状态)：

是当前车辆的状态，

和

是线速度和角速度的里程表读数，并作为模型的输入值，过程噪声

服从正太分布(均值为0)和常数协方差

2.测量模型

测量模型将车辆的当前姿态与激光雷达范围和方位测量值相关联

.

和

是 标记值

的真实值

,

和

表示当前车辆的状态

表示机器人中心和前轮激光测距仪(laser rangefinde)的距离 (LIDAR)

标记值的测量噪声

服从正太分布(均值为0)和常数协方差

.

由于上述模型是非线性的，因此我们建议使用扩展卡尔曼滤波器(EKF)作为状态估计器。具体来说，需要实现两个环节：预测环节，使用测距法测量和运动模型在给定的时间步长产生状态和协方差估计值

修正环节，使用范围和方位角激光雷达提供的测量值以纠正状态和他们的协方差估计

三、加载数据

import pickle

import numpy as np

import matplotlib.pyplot as plt

import math

with open('data/data.pickle', 'rb') as f:

data = pickle.load(f)

t = data['t'] # timestamps [s]

x_init = data['x_init'] # initial x position [m]

y_init = data['y_init'] # initial y position [m]

th_init = data['th_init'] # initial theta position [rad]

# input signal

v = data['v'] # translational velocity input [m/s]

om = data['om'] # rotational velocity input [rad/s]

# bearing and range measurements, LIDAR constants

b = data['b'] # bearing to each landmarks center in the frame attached to the laser [rad]

r = data['r'] # range measurements [m]

l = data['l'] # x,y positions of landmarks [m]

d = data['d'] # distance between robot center and laser rangefinder [m]

注意这里的参数d，即机器人中心到LIDAR的距离放到了d变量中。

如果实现了该任务，相应的轨迹和角度为：

注意方向值以弧度变成了

的范围.

四、初始化参数

现在已经加载了数据，我们可以开始为求解程序进行设置了。设计滤波器的最重要方面之一是确定输入和测量噪声协方差矩阵，以及初始状态和协方差值，设置值如下：

v_var = 0.01 # translation velocity variance

om_var = 0.1 # rotational velocity variance

r_var = 0.1 # range measurements variance

b_var = 0.1 # bearing measurement variance

Q_km = np.diag([v_var, om_var]) # input noise covariance

cov_y = np.diag([r_var, b_var]) # measurement noise covariance

x_est = np.zeros([len(v), 3]) # estimated states, x, y, and theta

P_est = np.zeros([len(v), 3, 3]) # state covariance matrices

x_est[0] = np.array([x_init, y_init, th_init]) # initial state

P_est[0] = np.diag([1, 1, 0.1]) # initial state covariance

注意: 有必要调整测量噪声的方差 r_var, b_var使得滤波器表现更好

为了使方向的估计值和方位测量值一致, 有必要把所有测量的

values 换到

范围.

# Wraps angle to (-pi,pi] range

def wraptopi(x):

if x > np.pi:

x = x - (np.floor(x / (2 * np.pi)) + 1) * 2 * np.pi

elif x < -np.pi:

x = x + (np.floor(x / (-2 * np.pi)) + 1) * 2 * np.pi

return x

这个函数可以将一个单独的值换算到范围内，还考虑过这种写法

def wraptopi1(x):

x[x>np.pi]=x[x>np.pi] - (np.floor(x[x>np.pi] / (2 * np.pi)) + 1) * 2 * np.pi

x[x

return x

```

这样定义函数，就可以把数组内的所有值换到该范围内。以下的函数都是基于第一个定义方法写的。

五、修正环节

1.在

计算测量模型的雅可比矩阵

2.计算卡尔曼增益

3.修正预测状态

4.修正协方差

把车辆想象成一根棍子，

是车辆的中心位置，车辆最前面是雷达的位置，

是车辆要取的位置，即landmark标记点，那么很容易根据正切值算出来这个变化的角度。

因此，这里详细推导一下

和

，$

是

对

求偏导，而

就是

是(2,1)的矩阵，分别对里面的

、

、

求偏导那么可以得到(2,3)的矩阵，即

的维数是(2,3)

为了表示方便，采用三个中间变量A、B、C来简化过程:

因此求导后的

为:

变量维度

x_k(3,1)

y_k(2,1)

K_k(3,2)

F_k(3,3)

L_k(3,2)

Q_k(2,2)

P_k(3,3)

H_k(2,3)

M_k(2,2)

X_est(501,3)

P_est(501,3,3)

def measurement_update(lk, rk, bk, P_check, x_check):

theta = x_check[:,2]

bk = wraptopi(bk)

theta= wraptopi(theta)

A = lk[0] - x_check[:,0] - d * math.cos(theta)

B = lk[1] - x_check[:,1] - d * math.sin(theta)

C = (A ** 2 + B ** 2) ** (0.5)

# 1. Compute measurement Jacobian

M = np.identity(2)

H = np.array([[-A/C,-B/C,(d/C)*(A*math.sin(theta)- B*math.cos(theta))],

[B/(C**2),-A/(C**2),((-d/(C**2))*math.cos(theta)*(A+B))-1]])

H = H.reshape(2,3) ## the dimension of H is (2,3,1),so you need to reshape it.

# 2. Compute Kalman Gain

K = P_check @ H.T @ np.linalg.inv(H @ P_check @ H.T + M @cov_y @ M.T )

# 3. Correct predicted state (remember to wrap the angles to [-pi,pi])

h = math.atan2(B,A) - theta

h = wraptopi(h)

ss = K @ np.array([rk-C,bk-h])

x_check = x_check + ss.reshape(1,3)

theta = x_check[:,2]

theta= wraptopi(theta)

# 4. Correct covariance

P_check = (np.identity(3)-K @ H) @ P_check

return x_check, P_check

六、预测环节

现在，实现主滤波器循环，使用提供的运动模型定义EKF的预测步骤：

其中，

那么同样求一下

,由于

就是

,类似地，

是(3,1)的矩阵，对里面的

、

、

求偏导那么可以得到(3,3)的矩阵，即

的维数是(3,3)

由于

是

,那么带入后

那么每一列分别对

求导可以得到

类似地，对

求导显而易见,就是前面的系数

#### 5. Main Filter Loop #######################################################################

for k in range(1, len(t)): # start at 1 because we've set the initial prediciton

delta_t = t[k] - t[k - 1] # time step (difference between timestamps)

x_check = x_est[k - 1,:]

P_check = P_est[k - 1,:,:]

theta = x_check[2]

theta = wraptopi(theta)

# 1. Update state with odometry readings (remember to wrap the angles to [-pi,pi])

cc = delta_t*np.array([[math.cos(theta),0],[math.sin(theta),0],[0,1]])@ np.array([[v[k]],[om[k]]])

x_check = x_check.reshape(1,3)

x_check = x_check + cc.T

theta = x_check[0,2]

theta = wraptopi(theta)

# 2. Motion model jacobian with respect to last state

F_km = np.array([[1,0,-delta_t * v[k] *math.sin(theta)],[0,1,delta_t * v[k]* math.cos(theta)],[0,0,1]])

# 3. Motion model jacobian with respect to noise

L_km = delta_t * np.array([[math.cos(theta),0],[math.sin(theta),0],[0,1]])

# 4. Propagate uncertainty

P_check = F_km @ P_check @ F_km.T + L_km @ Q_km @L_km.T

# 5. Update state estimate using available landmark measurements

for i in range(len(r[k])):

x_check, P_check = measurement_update(l[i], r[k, i], b[k, i], P_check, x_check)

# Set final state predictions for timestep

x_est[k, 0] = x_check[0,0]

x_est[k, 1] = x_check[0,1]

x_est[k, 2] = x_check[0,2]

P_est[k, :, :] = P_check

那么显示一下结果

e_fig = plt.figure()

ax = e_fig.add_subplot(111)

ax.plot(x_est[:, 0], x_est[:, 1])

ax.set_xlabel('x [m]')

ax.set_ylabel('y [m]')

ax.set_title('Estimated trajectory')

plt.show()

e_fig = plt.figure()

ax = e_fig.add_subplot(111)

ax.plot(t[:], x_est[:, 2])

ax.set_xlabel('Time [s]')

ax.set_ylabel('theta [rad]')

ax.set_title('Estimated trajectory')

plt.show()

咋差那么多呢！噪声！合理的设计噪声也很关键！

如果想实时显示运动轨迹，可以运行一下代码(jupyter效果很差)

ax = [] # 定义一个 x 轴的空列表用来接收动态的数据

ay = [] # 定义一个 y 轴的空列表用来接收动态的数据

plt.ion() # 开启一个画图的窗口

for i in range(501): # 遍历0-99的值

ax.append(x_est[i, 0]) # 添加 x 到 x 轴的数据中

ay.append(x_est[i, 1]) # 添加 y 到 y 轴的数据中

plt.clf() # 清除之前画的图

plt.plot(ax,ay) # 画出当前 ax 列表和 ay 列表中的值的图形

plt.pause(0.1) # 暂停一秒

plt.ioff() # 关闭画图的窗口

同样，随时间变化的角度也可以显示

ax = [] # 定义一个 x 轴的空列表用来接收动态的数据

ay = [] # 定义一个 y 轴的空列表用来接收动态的数据

plt.ion() # 开启一个画图的窗口

for i in range(501): # 遍历0-99的值

ax.append(i) # 添加 时间t 到 x 轴的数据中

ay.append(x_est[i, 2]) # 添加 theta 到 y 轴的数据中

plt.clf() # 清除之前画的图

plt.plot(ax,ay) # 画出当前 ax 列表和 ay 列表中的值的图形

plt.pause(0.1) # 暂停一秒

plt.ioff() # 关闭画图的窗口

结果路径差不太多，但是噪声太多了，那么按照一下思路，更改一下方差v_var不做更改(v_var=0.01)

- 15>om_var>5

- 1>r_var>0.001

- 1>b_var>0.001

如取 om_var=5，r_var=0.001 ，b_var=0.001