卡尔曼滤波在机器人里程计与惯导融合中的应用（python实现）

1、简介

卡尔曼滤波（Kalman Filter, KF）作为一种最优状态估计算法，非常适合解决机器人里程计与惯导（IMU）的融合问题。

• 里程计特性：通过轮速或关节角度计算位移，短期精度高但存在累积误差（如轮子打滑、地面不平）。
• 惯导特性：通过加速度计和陀螺仪直接测量运动状态，无累积误差但存在漂移（如零偏、噪声）。
• 融合目标：利用卡尔曼滤波的最优估计特性，结合两者优势，输出更准确、鲁棒的机器人状态。

2、python代码实现

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilterFusion:
    def __init__(self, dt):
        # 状态向量维度：[x, y, theta, vx, vy, omega, bgx, bgy, bgtheta]
        self.n_states = 9
        self.n_obs = 6  # 里程计3维位置 + 惯导3维角速度
        
        # 初始化状态转移矩阵F
        self.F = np.eye(self.n_states)
        self.F[0, 3] = dt  # x与vx的关系
        self.F[1, 4] = dt  # y与vy的关系
        self.F[2, 5] = dt  # theta与omega的关系
        
        # 初始化过程噪声协方差Q
        self.Q = np.diag([0.01, 0.01, 0.01, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001])
        
        # 初始化观测矩阵H
        self.H = np.zeros((self.n_obs, self.n_states))
        self.H[0, 0] = 1; self.H[1, 1] = 1; self.H[2, 2] = 1  # 里程计位置观测
        self.H[3, 5] = 1; self.H[3, 6] = -1  # omega_x = 状态omega - 零偏bgx
        self.H[4, 5] = 1; self.H[4, 7] = -1  # omega_y = 状态omega - 零偏bgy
        self.H[5, 5] = 1; self.H[5, 8] = -1  # omega_z = 状态omega - 零偏bgtheta
        
        # 初始化观测噪声协方差R
        self.R = np.diag([0.05, 0.05, 0.05, 0.1, 0.1, 0.1])  # 里程计与惯导噪声
        
        # 初始化状态与协方差
        self.x = np.zeros((self.n_states, 1))  # 初始状态
        self.P = np.eye(self.n_states) * 1.0  # 初始协方差
    
    def predict(self, u):
        """预测阶段：根据控制输入更新状态"""
        # u为控制输入[线速度v, 角速度omega]
        v, omega = u[0], u[1]
        self.x[3] = v * np.cos(self.x[2])  # 线速度vx
        self.x[4] = v * np.sin(self.x[2])  # 线速度vy
        self.x[5] = omega              # 角速度
        
        # 状态预测
        self.x = self.F @ self.x
        
        # 协方差预测
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x[:3]  # 返回预测的位置[x, y, theta]
    
    def update(self, z_odo, z_imu):
        """更新阶段：融合里程计与惯导观测"""
        # 构建观测向量
        z = np.zeros((self.n_obs, 1))
        z[0:3] = z_odo.reshape(3, 1)  # 里程计位置
        z[3:6] = z_imu.reshape(3, 1)  # 惯导角速度
        
        # 计算卡尔曼增益
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        
        # 状态更新
        self.x = self.x + K @ (z - self.H @ self.x)
        
        # 协方差更新
        I = np.eye(self.n_states)
        self.P = (I - K @ self.H) @ self.P
        return self.x[:3]  # 返回更新后的位置[x, y, theta]

# 仿真测试
def run_simulation():
    dt = 0.01  # 10ms采样周期
    kf = KalmanFilterFusion(dt)
    
    # 生成仿真数据（含噪声）
    time = np.arange(0, 10, dt)
    true_x, true_y, true_theta = [], [], []
    noisy_odo, noisy_imu = [], []
    fused_states = []
    
    # 初始状态
    x, y, theta = 0, 0, 0
    v, omega = 0.5, 0.1  # 线速度0.5m/s，角速度0.1rad/s
    
    for t in time:
        # 真实状态更新
        x += v * np.cos(theta) * dt
        y += v * np.sin(theta) * dt
        theta += omega * dt
        true_x.append(x)
        true_y.append(y)
        true_theta.append(theta)
        
        # 生成带噪声的里程计数据
        odo_noise = np.random.normal(0, [0.01, 0.01, 0.01])
        odo = np.array([x, y, theta]) + odo_noise
        noisy_odo.append(odo)
        
        # 生成带噪声和零偏的惯导数据
        imu_bias = np.array([0.005, 0.005, 0.005])  # 固定零偏
        imu_noise = np.random.normal(0, [0.01, 0.01, 0.01])
        imu = np.array([omega, 0, 0]) + imu_bias + imu_noise  # 仅z轴有角速度
        noisy_imu.append(imu)
        
        # 卡尔曼滤波融合
        fused_pos = kf.update(odo, imu)
        fused_states.append(fused_pos.flatten())
    
    # 绘制结果
    plt.figure(figsize=(12, 8))
    plt.subplot(2, 1, 1)
    plt.plot(true_x, true_y, 'g-', label='真实轨迹')
    plt.plot([s[0] for s in fused_states], [s[1] for s in fused_states], 'r-', label='融合轨迹')
    plt.legend()
    plt.title('机器人轨迹融合结果')
    
    plt.subplot(2, 1, 2)
    plt.plot(time, true_theta, 'g-', label='真实航向角')
    plt.plot(time, [s[2] for s in fused_states], 'r-', label='融合航向角')
    plt.legend()
    plt.title('航向角融合结果')
    
    plt.tight_layout()
    plt.show()

if __name__ == "__main__":
    run_simulation()

3、仿真结果展示