演示扩展卡尔曼滤波在无人驾驶多传感器融合中的应用

一、背景及功能介绍

在无人驾驶系统中，精确的状态估计是安全导航的核心基础。车辆需要实时了解自身位置(x,y)、速度(v)和航向角(θ)。然而，任何单一传感器都存在局限性：

• GPS：易受建筑物遮挡，更新频率低(1-10Hz)
• IMU：短时间内精度高，但存在累积误差
• LiDAR：精度高但受天气影响，计算开销大

扩展卡尔曼滤波(EKF) 通过融合多传感器数据，克服单一传感器的局限，提供更鲁棒的状态估计。EKF是标准卡尔曼滤波在非线性系统上的扩展，通过局部线性化（雅可比矩阵）处理非线性运动模型。

本文实现了一个完整的EKF多传感器融合系统，包含：

1. 车辆运动模型仿真（含加速度和转向控制）
2. 多传感器噪声模型（GPS, IMU, LiDAR）
3. 完整EKF预测-更新流程
4. 实时动画可视化系统

二、设计思路

2.1 系统架构

传感器数据 → EKF融合中心 → 状态估计↑             ↑            ↑GPS          预测模型      可视化IMU         更新模型LiDAR

2.2 核心算法流程

1. 预测阶段：

   • 使用非线性运动模型预测状态
   • 通过雅可比矩阵线性化系统
   • 更新状态协方差

2. 更新阶段：

   • 根据传感器可用性动态构建观测模型
   • 计算卡尔曼增益
   • 融合观测值更新状态
   • 处理角度归一化

2.3 传感器模型

传感器	测量值	频率	噪声模型
GPS	(x,y)	1Hz	σ=2.0m
IMU速度	v	20Hz	σ=0.5m/s
IMU加速度	a	20Hz	σ=0.3m/s²
IMU航向	θ	20Hz	σ=0.05rad
LiDAR	(x,y)	10Hz	σ=0.3m

三、完整代码

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
from matplotlib.animation import FuncAnimation
from math import cos, sin, atan2, sqrt
import math
from pylab import mpl
mpl.rcParams["font.sans-serif"] = ["SimHei"] ## win
plt.rcParams['font.family']=['SimHei'] ## win
plt.rcParams['font.family'] = 'SimHei'
#正常显示坐标轴负号
plt.rcParams['axes.unicode_minus'] = Falseclass ExtendedKalmanFilter:def __init__(self, state, P, Q, R):"""初始化扩展卡尔曼滤波器参数:state: 初始状态向量 [x, y, v, theta]P: 初始状态协方差矩阵Q: 过程噪声协方差R: 测量噪声协方差 (包含所有传感器)"""self.state = state  # [x, y, v, theta]self.P = P          # 状态协方差self.Q = Q          # 过程噪声协方差self.R = R          # 测量噪声协方差 (所有传感器联合)self.state_dim = len(state)self.measurement_dim = 5  # GPS(2) + IMU(3) + LiDAR(2)def predict(self, dt):"""预测步骤参数:dt: 时间步长"""# 解包状态x, y, v, theta = self.state# 非线性状态转移函数self.state_pred = np.array([x + v * cos(theta) * dt,y + v * sin(theta) * dt,v,  # 保持速度不变 (实际应用中应有IMU加速度输入)theta  # 保持航向不变 (实际应用中应有IMU角速度输入)])# 状态转移雅可比矩阵F = np.array([[1, 0, cos(theta)*dt, -v*sin(theta)*dt],[0, 1, sin(theta)*dt, v*cos(theta)*dt],[0, 0, 1, 0],[0, 0, 0, 1]])# 预测协方差self.P = F @ self.P @ F.T + self.Qdef update(self, measurements, sensor_mask):"""更新步骤 (多传感器融合)参数:measurements: 联合测量向量 [GPS_x, GPS_y, IMU_v, IMU_a, IMU_theta, LiDAR_x, LiDAR_y]sensor_mask: 指示哪些传感器数据可用的布尔掩码 [gps, imu_v, imu_a, imu_theta, lidar]"""# 如果没有可用传感器，跳过更新if not any(sensor_mask):return# 解包预测状态x_pred, y_pred, v_pred, theta_pred = self.state_pred# 创建实际的测量向量 (只包含可用传感器)real_measurements = []H_rows = []R_rows = []# GPS传感器 (位置测量)if sensor_mask[0]:# 测量函数 (直接测量位置)h_gps = np.array([x_pred, y_pred])# 雅可比矩阵H_gps = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])real_measurements.extend(measurements[0:2])H_rows.append(H_gps)R_rows.extend([0, 1])  # 噪声矩阵中的GPS行# IMU速度传感器if sensor_mask[1]:h_imu_v = np.array([v_pred])H_imu_v = np.array([[0, 0, 1, 0]])real_measurements.extend([measurements[2]])H_rows.append(H_imu_v)R_rows.append(2)  # 噪声矩阵中的IMU速度行# IMU加速度传感器 (非线性测量)if sensor_mask[2]:# 加速度测量: a = dv/dt, 但在我们的模型中是隐含的h_imu_a = np.array([0])  # 简化处理，实际应用中加速度会影响速度# 雅可比矩阵H_imu_a = np.array([[0, 0, 0, 0]])  # 不直接影响当前状态real_measurements.extend([measurements[3]])H_rows.append(H_imu_a)R_rows.append(3)  # 噪声矩阵中的IMU加速度行# IMU航向传感器if sensor_mask[3]:h_imu_theta = np.array([theta_pred])H_imu_theta = np.array([[0, 0, 0, 1]])real_measurements.extend([measurements[4]])H_rows.append(H_imu_theta)R_rows.append(4)  # 噪声矩阵中的IMU航向行# LiDAR传感器 (位置测量)if sensor_mask[4]:# 假设LiDAR经过坐标转换，可直接测量全局位置h_lidar = np.array([x_pred, y_pred])H_lidar = np.array([[1, 0, 0, 0],[0, 1, 0, 0]])real_measurements.extend(measurements[5:7])H_rows.append(H_lidar)R_rows.extend([5, 6])  # 噪声矩阵中的LiDAR行# 组合所有可用传感器的雅可比矩阵和测量噪声H = np.vstack(H_rows) if H_rows else np.zeros((0, self.state_dim))R_sub = self.R[R_rows, :][:, R_rows] if R_rows else np.zeros((0, 0))# 如果没有有效测量，跳过更新if H.size == 0:return# 转换为numpy数组real_measurements = np.array(real_measurements)# 计算卡尔曼增益S = H @ self.P @ H.T + R_subK = self.P @ H.T @ np.linalg.inv(S)# 计算残差y_residual = real_measurements - np.hstack([h_gps if sensor_mask[0] else [],h_imu_v if sensor_mask[1] else [],h_imu_a if sensor_mask[2] else [],h_imu_theta if sensor_mask[3] else [],h_lidar if sensor_mask[4] else []])# 更新状态和协方差self.state = self.state_pred + K @ y_residual# 角度归一化 (如果更新了航向)if sensor_mask[3]:self.state[3] = atan2(sin(self.state[3]), cos(self.state[3]))# 更新协方差self.P = (np.eye(self.state_dim) - K @ H) @ self.Preturn self.state.copy()def simulate_vehicle(dt=0.1, total_time=30):"""模拟车辆运动和传感器数据参数:dt: 时间步长 (秒)total_time: 总模拟时间 (秒)返回:true_states: 真实状态序列sensor_data: 传感器数据序列sensor_mask: 传感器可用性序列"""n_steps = int(total_time / dt)true_states = np.zeros((n_steps, 4))  # [x, y, v, theta]sensor_data = np.zeros((n_steps, 7))  # [GPS_x, GPS_y, IMU_v, IMU_a, IMU_theta, LiDAR_x, LiDAR_y]sensor_mask = np.zeros((n_steps, 5), dtype=bool)  # [GPS, IMU_v, IMU_a, IMU_theta, LiDAR]# 初始状态x, y, v, theta = 0, 0, 10, 0true_states[0] = [x, y, v, theta]# 传感器噪声参数gps_noise = 2.0imu_v_noise = 0.5imu_a_noise = 0.3imu_theta_noise = 0.05lidar_noise = 0.3# 传感器更新频率gps_freq = 1.0  # 1Hzimu_freq = 20.0  # 20Hzlidar_freq = 10.0  # 10Hz# 模拟车辆运动 (带加速度和转向)acceleration = 0.0angular_velocity = 0.0for i in range(1, n_steps):t = i * dt# 控制输入变化 (加速/减速和转弯)if t < 5:acceleration = 1.5angular_velocity = 0.0elif t < 10:acceleration = 0.0angular_velocity = 0.3elif t < 15:acceleration = -1.0angular_velocity = -0.2elif t < 20:acceleration = 1.0angular_velocity = 0.15elif t < 25:acceleration = -1.5angular_velocity = 0.25else:acceleration = 0.2angular_velocity = 0.1# 更新真实状态v_prev = vtheta_prev = thetav = v + acceleration * dttheta = theta + angular_velocity * dtx = x + v * cos(theta) * dty = y + v * sin(theta) * dttrue_states[i] = [x, y, v, theta]# 生成传感器数据 (带噪声)# GPS传感器if i % int(1/(gps_freq*dt)) == 0:sensor_data[i, 0] = x + np.random.normal(0, gps_noise)sensor_data[i, 1] = y + np.random.normal(0, gps_noise)sensor_mask[i, 0] = True  # GPS可用# IMU传感器if i % math.ceil(1/(imu_freq*dt)) == 0:# 速度测量sensor_data[i, 2] = v + np.random.normal(0, imu_v_noise)sensor_mask[i, 1] = True  # IMU速度可用# 加速度测量accel = (v - v_prev) / dtsensor_data[i, 3] = accel + np.random.normal(0, imu_a_noise)sensor_mask[i, 2] = True  # IMU加速度可用# 航向测量sensor_data[i, 4] = theta + np.random.normal(0, imu_theta_noise)sensor_mask[i, 3] = True  # IMU航向可用# LiDAR传感器if i % int(1/(lidar_freq*dt)) == 0:# 位置测量 (相对坐标系转换)# 实际中LiDAR测量相对位置，但这里假设转换为全局坐标lidar_x = x + np.random.normal(0, lidar_noise)lidar_y = y + np.random.normal(0, lidar_noise)sensor_data[i, 5] = lidar_xsensor_data[i, 6] = lidar_ysensor_mask[i, 4] = True  # LiDAR可用return true_states, sensor_data, sensor_maskdef run_ekf_simulation():# 参数设置dt = 0.1total_time = 30n_steps = int(total_time / dt)# 模拟车辆运动和传感器数据print("模拟车辆运动和传感器数据...")true_states, sensor_data, sensor_mask = simulate_vehicle(dt, total_time)# EKF初始化print("初始化EKF...")# 初始状态估计 (错误初始化以演示滤波器收敛)init_state = np.array([-5, -5, 5, 0.1])# 初始协方差矩阵init_cov = np.diag([10.0, 10.0, 5.0, 1.0])# 过程噪声协方差 (建模不确定性)Q = np.diag([0.1, 0.1, 0.2, 0.05])**2# 测量噪声协方差 (所有传感器联合)R = np.diag([2.0, 2.0,       # GPS (x, y)0.5, 0.3, 0.05, # IMU (v, a, theta)0.3, 0.3        # LiDAR (x, y)])**2# 创建EKFekf = ExtendedKalmanFilter(state=init_state,P=init_cov,Q=Q,R=R)# 存储结果ekf_states = np.zeros((n_steps, 4))ekf_cov = np.zeros((n_steps, 2))  # 只存储位置协方差# 传感器激活计数sensor_updates = np.zeros(5)  # GPS, IMU_v, IMU_a, IMU_theta, LiDAR# 运行EKFprint("运行扩展卡尔曼滤波...")for i in range(n_steps):# 预测步骤ekf.predict(dt)# 更新步骤 (使用当前所有可用传感器)mask = sensor_mask[i]if any(mask):update_state = ekf.update(sensor_data[i], mask)# 计数传感器更新if mask[0]: sensor_updates[0] += 1  # GPSif mask[1]: sensor_updates[1] += 1  # IMU_vif mask[2]: sensor_updates[2] += 1  # IMU_aif mask[3]: sensor_updates[3] += 1  # IMU_thetaif mask[4]: sensor_updates[4] += 1  # LiDAR# 存储结果ekf_states[i] = update_stateelse:ekf_states[i] = ekf.state_pred# 存储位置协方差ekf_cov[i] = [ekf.P[0, 0], ekf.P[1, 1]]print("传感器更新统计:")print(f"  GPS: {sensor_updates[0]} 次")print(f"  IMU速度: {sensor_updates[1]} 次")print(f"  IMU加速度: {sensor_updates[2]} 次")print(f"  IMU航向: {sensor_updates[3]} 次")print(f"  LiDAR: {sensor_updates[4]} 次")return true_states, ekf_states, ekf_cov, sensor_data, sensor_mask# 创建动画
def create_animation(true_states, ekf_states, ekf_cov, sensor_data, sensor_mask):# 设置绘图fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(14, 12), gridspec_kw={'height_ratios': [3, 1]})# 轨迹图ax1.set_xlim(-50, 250)ax1.set_ylim(-50, 350)ax1.set_xlabel('X Position (m)')ax1.set_ylabel('Y Position (m)')ax1.set_title('无人驾驶多传感器融合: GPS, IMU, LiDAR')ax1.grid(True)# 传感器可用性图ax2.set_xlim(0, len(true_states) * 0.1)ax2.set_ylim(-0.1, 1.1)ax2.set_xlabel('Time (s)')ax2.set_ylabel('Sensor Availability')ax2.set_yticks([0, 1])ax2.set_yticklabels(['Off', 'On'])# 创建绘图元素# 轨迹图元素true_path, = ax1.plot([], [], 'b-', lw=2, label='真实轨迹')ekf_path, = ax1.plot([], [], 'g-', lw=2, label='EKF估计')gps_scatter = ax1.scatter([], [], c='r', marker='o', s=30, label='GPS测量')lidar_scatter = ax1.scatter([], [], c='m', marker='x', s=30, label='LiDAR测量')cov_ellipse = ax1.add_patch(mpatches.Ellipse((0, 0), 0, 0, angle=0, color='purple', alpha=0.2, label='位置不确定性(3σ)'))# 车辆表示vehicle_body = ax1.add_patch(mpatches.Rectangle((0,0), 4, 2, angle=0, color='blue', alpha=0.7))vehicle_direction = ax1.arrow(0, 0, 0, 0, color='red', width=0.5, head_width=1.5, head_length=2)# 传感器可用性图元素gps_line, = ax2.plot([], [], 'r-', lw=2, label='GPS')imu_v_line, = ax2.plot([], [], 'c-', lw=2, label='IMU速度')imu_a_line, = ax2.plot([], [], 'y-', lw=2, label='IMU加速度')imu_theta_line, = ax2.plot([], [], 'b-', lw=2, label='IMU航向')lidar_line, = ax2.plot([], [], 'm-', lw=2, label='LiDAR')# 时间文本time_text = ax1.text(0.05, 0.95, '', transform=ax1.transAxes, fontsize=12, bbox=dict(facecolor='white', alpha=0.7))# 速度文本speed_text = ax1.text(0.75, 0.95, '', transform=ax1.transAxes, fontsize=12, bbox=dict(facecolor='white', alpha=0.7))# 添加图例ax1.legend(loc='upper right')ax2.legend(loc='upper right', ncol=5)# 动画更新函数def update(frame):# 更新轨迹true_path.set_data(true_states[:frame, 0], true_states[:frame, 1])ekf_path.set_data(ekf_states[:frame, 0], ekf_states[:frame, 1])# 更新GPS测量点gps_mask = sensor_mask[:frame, 0]gps_x = sensor_data[:frame, 0][gps_mask]gps_y = sensor_data[:frame, 1][gps_mask]gps_scatter.set_offsets(np.column_stack((gps_x, gps_y)))# 更新LiDAR测量点lidar_mask = sensor_mask[:frame, 4]lidar_x = sensor_data[:frame, 5][lidar_mask]lidar_y = sensor_data[:frame, 6][lidar_mask]lidar_scatter.set_offsets(np.column_stack((lidar_x, lidar_y)))# 更新协方差椭圆if frame > 0:cov = ekf_cov[frame-1]w = 3 * np.sqrt(cov[0])h = 3 * np.sqrt(cov[1])center = ekf_states[frame-1, :2]cov_ellipse.set_center(center)cov_ellipse.set_width(w)cov_ellipse.set_height(h)# 更新车辆位置和方向x, y, v, theta = ekf_states[frame-1] if frame > 0 else ekf_states[0]# 车辆车身vehicle_body.set_xy([x-2, y-1])vehicle_body.set_angle(np.degrees(theta))# 车辆方向箭头dx = 3 * cos(theta)dy = 3 * sin(theta)vehicle_direction.set_data(x=x, y=y, dx=dx, dy=dy)# 更新时间time_text.set_text(f'时间: {frame * 0.1:.1f}s')speed_text.set_text(f'速度: {v:.1f} m/s')# 更新传感器可用性图time_points = np.arange(frame) * 0.1gps_line.set_data(time_points, sensor_mask[:frame, 0])imu_v_line.set_data(time_points, sensor_mask[:frame, 1]*0.8)imu_a_line.set_data(time_points, sensor_mask[:frame, 2]*0.6)imu_theta_line.set_data(time_points, sensor_mask[:frame, 3]*0.4)lidar_line.set_data(time_points, sensor_mask[:frame, 4]*0.2)return (true_path, ekf_path, gps_scatter, lidar_scatter, cov_ellipse,vehicle_body, vehicle_direction, time_text, speed_text,gps_line, imu_v_line, imu_a_line, imu_theta_line, lidar_line)# 创建动画n_frames = len(true_states)print("创建动画...")ani = FuncAnimation(fig, update, frames=n_frames, interval=50, blit=True)plt.tight_layout()plt.show()return ani# 主函数
def main():# 运行仿真true_states, ekf_states, ekf_cov, sensor_data, sensor_mask = run_ekf_simulation()# 创建动画ani = create_animation(true_states, ekf_states, ekf_cov, sensor_data, sensor_mask)if __name__ == "__main__":main()

四、代码讲解

4.1 EKF核心类

class ExtendedKalmanFilter:def __init__(self, state, P, Q, R):# 初始化状态和协方差self.state = state  # [x, y, v, theta]self.P = P          # 状态协方差self.Q = Q          # 过程噪声协方差self.R = R          # 测量噪声协方差def predict(self, dt):# 非线性状态预测x, y, v, theta = self.stateself.state_pred = np.array([x + v * cos(theta) * dt,y + v * sin(theta) * dt,v,  # 速度保持不变theta  # 航向保持不变])# 计算雅可比矩阵F = np.array([[1, 0, cos(theta)*dt, -v*sin(theta)*dt],[0, 1, sin(theta)*dt, v*cos(theta)*dt],[0, 0, 1, 0],[0, 0, 0, 1]])# 更新协方差self.P = F @ self.P @ F.T + self.Qdef update(self, measurements, sensor_mask):# 动态构建观测模型H_rows = []real_measurements = []# GPS处理if sensor_mask[0]:H_rows.append(np.array([[1,0,0,0],[0,1,0,0]]))real_measurements.extend(measurements[0:2])# IMU速度处理if sensor_mask[1]:H_rows.append(np.array([[0,0,1,0]]))real_measurements.append(measurements[2])# ...其他传感器类似处理...# 组合雅可比矩阵H = np.vstack(H_rows)# 计算卡尔曼增益S = H @ self.P @ H.T + R_subK = self.P @ H.T @ np.linalg.inv(S)# 更新状态self.state = self.state_pred + K @ (real_measurements - H @ self.state_pred)# 航向角归一化if sensor_mask[3]:self.state[3] = atan2(sin(self.state[3]), cos(self.state[3]))

4.2 车辆运动仿真

def simulate_vehicle(dt=0.1, total_time=30):# 初始化状态true_states = np.zeros((n_steps, 4))# 运动控制逻辑for i in range(1, n_steps):t = i * dt# 分段控制策略if t < 5: acceleration, angular_velocity = 1.5, 0.0elif t < 10: acceleration, angular_velocity = 0.0, 0.3# ...其他阶段...# 更新真实状态v = v + acceleration * dttheta = theta + angular_velocity * dtx = x + v * cos(theta) * dty = y + v * sin(theta) * dt# 生成带噪声的传感器数据if i % gps_interval == 0:  # GPSsensor_data[i, 0:2] = [x, y] + np.random.normal(0, gps_noise, 2)sensor_mask[i, 0] = True# ...其他传感器...

4.3 可视化动画系统

def create_animation(true_states, ekf_states, ...):# 创建轨迹图fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(14, 12))# 轨迹可视化元素true_path, = ax1.plot([], [], 'b-', label='真实轨迹')ekf_path, = ax1.plot([], [], 'g-', label='EKF估计')cov_ellipse = ax1.add_patch(mpatches.Ellipse(...))# 传感器可用性可视化gps_line, = ax2.plot([], [], 'r-', label='GPS')# ...其他传感器...def update(frame):# 更新轨迹true_path.set_data(true_states[:frame, 0], true_states[:frame, 1])# 更新协方差椭圆w = 3 * np.sqrt(ekf_cov[frame-1, 0])h = 3 * np.sqrt(ekf_cov[frame-1, 1])cov_ellipse.set_width(w)cov_ellipse.set_height(h)# 更新车辆方向x, y, v, theta = ekf_states[frame]vehicle_body.set_xy([x-2, y-1])vehicle_body.set_angle(np.degrees(theta))# 更新传感器状态time_points = np.arange(frame) * 0.1gps_line.set_data(time_points, sensor_mask[:frame, 0])return all_elementsani = FuncAnimation(fig, update, frames=n_frames, interval=50)plt.show()

五、系统运行效果

运行程序后，将生成一个包含两部分的动画：

1. 主轨迹视图：

   • 蓝色曲线：车辆真实轨迹
   • 绿色曲线：EKF估计轨迹
   • 红色点：GPS测量值
   • 紫色椭圆：位置不确定性（3σ范围）
   • 蓝色矩形：车辆当前位置和方向

2. 传感器状态视图：

   • 多条色带表示不同传感器的可用性
   • 直观展示各传感器的更新频率

六、总结

本文实现了一个完整的扩展卡尔曼滤波多传感器融合系统，关键技术点包括：

1. 非线性处理：通过雅可比矩阵对非线性运动模型进行局部线性化

F = np.array([[1, 0, cos(theta)*dt, -v*sin(theta)*dt],[0, 1, sin(theta)*dt, v*cos(theta)*dt],[0, 0, 1, 0],[0, 0, 0, 1]
])

2. 多传感器融合：动态构建观测模型，灵活处理传感器可用性

if sensor_mask[0]:  # GPS可用H_rows.append(np.array([[1,0,0,0],[0,1,0,0]]))

3. 鲁棒性设计：

   • 错误的初始状态（[-5, -5] vs 真实[0,0]）
   • 传感器部分失效模拟
   • 角度归一化处理

4. 可视化创新：

   • 实时协方差椭圆展示不确定性
   • 传感器状态时间线
   • 车辆方向指示器

实际应用中，可进一步优化：

1. 添加IMU加速度计对速度的更新
2. 实现传感器异步更新处理
3. 增加自适应噪声估计
4. 结合SLAM进行环境建图