KF，EKF，IEKF 算法的基本原理并构建推导出四轮前驱自主移动机器人的运动学模型和观测模型附Matlab代码

最新推荐文章于 2026-01-02 10:17:02 发布

原创最新推荐文章于 2026-01-02 10:17:02 发布 · 841 阅读

9 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #matlab #开发语言

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

一、引言

在自主移动机器人的定位、导航与控制任务中，状态估计是核心技术之一，其目的是从含噪声的传感器数据中准确推断机器人的位置、姿态、速度等关键状态信息。卡尔曼滤波（Kalman Filter, KF）及其扩展形式——扩展卡尔曼滤波（Extended Kalman Filter, EKF）、迭代扩展卡尔曼滤波（Iterated Extended Kalman Filter, IEKF）是应用最广泛的状态估计方法。同时，机器人的运动学模型和观测模型是滤波算法实现的基础，直接决定了状态估计的精度。本文将首先阐述KF、EKF、IEKF算法的基本原理，再针对四轮前驱（Four-Wheel Front-Drive, 4WDD）自主移动机器人，完成运动学模型和观测模型的构建与推导。

二、KF、EKF、IEKF算法基本原理

2.1 卡尔曼滤波（KF）算法

KF算法是由Rudolf E. Kalman于1960年提出的一种线性、无偏、最优状态估计方法，适用于<加粗>线性高斯系统</加粗>。其核心思想是通过“预测-更新”的迭代过程，结合系统的运动模型和观测模型，最小化状态估计的均方误差。

2.1.1 核心假设

系统动力学模型和观测模型均为线性；
过程噪声和观测噪声均服从零均值、高斯分布，且两者互不相关。

2.1.2 数学模型

设离散时间线性系统的状态方程和观测方程分别为：

状态方程：\( \mathbf{x}_{k} = \mathbf{A}_{k} \mathbf{x}_{k-1} + \mathbf{B}_{k} \mathbf{u}_{k} + \mathbf{w}_{k-1} \)

观测方程：\( \mathbf{z}_{k} = \mathbf{C}_{k} \mathbf{x}_{k} + \mathbf{v}_{k} \)

其中：

\( \mathbf{x}_{k} \in \mathbb{R}^n \) 为k时刻系统状态向量；
\( \mathbf{u}_{k} \in \mathbb{R}^m \) 为k时刻控制输入向量；
\( \mathbf{z}_{k} \in \mathbb{R}^p \) 为k时刻观测向量；
\( \mathbf{A}_{k} \in \mathbb{R}^{n \times n} \) 为状态转移矩阵；
\( \mathbf{B}_{k} \in \mathbb{R}^{n \times m} \) 为控制输入矩阵；
\( \mathbf{C}_{k} \in \mathbb{R}^{p \times n} \) 为观测矩阵；
\( \mathbf{w}_{k-1} \sim \mathcal{N}(0, \mathbf{Q}_{k-1}) \) 为过程噪声，\( \mathbf{Q}_{k-1} \) 为过程噪声协方差矩阵；
\( \mathbf{v}_{k} \sim \mathcal{N}(0, \mathbf{R}_{k}) \) 为观测噪声，\( \mathbf{R}_{k} \) 为观测噪声协方差矩阵。

2.1.3 核心迭代步骤

KF算法通过以下两步迭代实现状态估计：

<加粗>预测步</加粗>：基于上一时刻的状态估计值和控制输入，预测当前时刻的先验状态和先验协方差矩阵。状态预测：\( \hat{\mathbf{x}}_{k|k-1} = \mathbf{A}_{k} \hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_{k} \mathbf{u}_{k} \)协方差预测：\( \mathbf{P}_{k|k-1} = \mathbf{A}_{k} \mathbf{P}_{k-1|k-1} \mathbf{A}_{k}^T + \mathbf{Q}_{k-1} \)其中，\( \hat{\mathbf{x}}_{k|k-1} \) 为k时刻先验状态估计值，\( \hat{\mathbf{x}}_{k-1|k-1} \) 为k-1时刻后验状态估计值；\( \mathbf{P}_{k|k-1} \) 为先验协方差矩阵，\( \mathbf{P}_{k-1|k-1} \) 为k-1时刻后验协方差矩阵。
<加粗>更新步</加粗>：结合当前时刻的观测数据，对先验估计进行修正，得到后验状态估计值和后验协方差矩阵。卡尔曼增益计算：\( \mathbf{K}_{k} = \mathbf{P}_{k|k-1} \mathbf{C}_{k}^T (\mathbf{C}_{k} \mathbf{P}_{k|k-1} \mathbf{C}_{k}^T + \mathbf{R}_{k})^{-1} \)状态更新：\( \hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_{k} (\mathbf{z}_{k} - \mathbf{C}_{k} \hat{\mathbf{x}}_{k|k-1}) \)协方差更新：\( \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_{k} \mathbf{C}_{k}) \mathbf{P}_{k|k-1} \)其中，\( \mathbf{K}_{k} \) 为卡尔曼增益，用于权衡先验估计和观测数据的可信度；\( \mathbf{I} \) 为单位矩阵。

2.2 扩展卡尔曼滤波（EKF）算法

实际工程场景中，大多数系统（如自主移动机器人）的动力学模型和观测模型均为<加粗>非线性</加粗>，无法直接应用KF算法。EKF算法通过“一阶泰勒展开线性化”的方式，将非线性系统近似为线性系统，进而沿用KF的框架实现状态估计，适用于<加粗>弱非线性高斯系统</加粗>。

2.2.1 核心思想

在当前状态估计值的邻域内，对非线性的状态方程和观测方程进行一阶泰勒展开，忽略高阶小项，得到近似的线性化模型，再将线性化后的状态转移矩阵、控制输入矩阵和观测矩阵代入KF的迭代公式，完成状态估计。

2.2.2 数学模型

设离散时间非线性系统的状态方程和观测方程分别为：

状态方程：\( \mathbf{x}_{k} = f(\mathbf{x}_{k-1}, \mathbf{u}_{k}) + \mathbf{w}_{k-1} \)

观测方程：\( \mathbf{z}_{k} = h(\mathbf{x}_{k}) + \mathbf{v}_{k} \)

其中，\( f(\cdot) \) 为非线性状态转移函数，\( h(\cdot) \) 为非线性观测函数，其余参数定义与KF一致。

2.2.3 核心迭代步骤

EKF的迭代过程同样分为“预测-更新”两步，但增加了线性化环节：

<加粗>预测步</加粗>：状态预测（非线性递推）：\( \hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k}) \)线性化（计算雅可比矩阵）：对状态转移函数 \( f(\cdot) \) 在 \( \hat{\mathbf{x}}_{k-1|k-1} \) 处求导，得到雅可比矩阵 \( \mathbf{F}_{k} = \frac{\partial f}{\partial \mathbf{x}} \bigg|_{\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k}} \)（近似替代KF中的 \( \mathbf{A}_{k} \)）；若控制输入为非线性关联，还需计算控制输入雅可比矩阵 \( \mathbf{G}_{k} = \frac{\partial f}{\partial \mathbf{u}} \bigg|_{\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_{k}} \)（近似替代 \( \mathbf{B}_{k} \)）。协方差预测：\( \mathbf{P}_{k|k-1} = \mathbf{F}_{k} \mathbf{P}_{k-1|k-1} \mathbf{F}_{k}^T + \mathbf{G}_{k} \mathbf{Q}_{k-1} \mathbf{G}_{k}^T \)（若控制输入为线性，\( \mathbf{G}_{k} = \mathbf{B}_{k} \)）。
<加粗>更新步</加粗>：线性化（计算观测雅可比矩阵）：对观测函数 \( h(\cdot) \) 在 \( \hat{\mathbf{x}}_{k|k-1} \) 处求导，得到雅可比矩阵 \( \mathbf{H}_{k} = \frac{\partial h}{\partial \mathbf{x}} \bigg|_{\hat{\mathbf{x}}_{k|k-1}} \)（近似替代KF中的 \( \mathbf{C}_{k} \)）。卡尔曼增益计算：\( \mathbf{K}_{k} = \mathbf{P}_{k|k-1} \mathbf{H}_{k}^T (\mathbf{H}_{k} \mathbf{P}_{k|k-1} \mathbf{H}_{k}^T + \mathbf{R}_{k})^{-1} \)状态更新：\( \hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_{k} (\mathbf{z}_{k} - h(\hat{\mathbf{x}}_{k|k-1})) \)（其中 \( h(\hat{\mathbf{x}}_{k|k-1}) \) 为非线性观测预测值）协方差更新：\( \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_{k} \mathbf{H}_{k}) \mathbf{P}_{k|k-1} \)

EKF的局限性：一阶泰勒展开的近似误差会随着系统非线性程度的增加而增大，可能导致滤波发散；雅可比矩阵的计算复杂，易出现数值不稳定问题。

2.3 迭代扩展卡尔曼滤波（IEKF）算法

IEKF是EKF的改进形式，针对EKF一阶线性化近似误差较大的问题，通过“多次迭代线性化”的方式提升非线性系统状态估计的精度，适用于<加粗>中度非线性高斯系统</加粗>。

核心思想

IEKF在EKF的更新步中引入迭代过程：以EKF的先验状态估计值为初始迭代点，计算观测函数的雅可比矩阵并完成第一次状态更新；将第一次更新后的状态估计值作为新的线性化点，重新计算雅可比矩阵和卡尔曼增益，再次更新状态；重复上述过程，直至状态估计值收敛（或达到预设迭代次数），最终得到后验状态估计值。通过迭代，线性化点不断逼近真实状态，从而减小线性化近似误差。

核心迭代步骤

IEKF的预测步与EKF完全一致，差异仅在于更新步的迭代过程：

<加粗>预测步</加粗>：与EKF相同，得到先验状态 \( \hat{\mathbf{x}}_{k|k-1} \) 和先验协方差 \( \mathbf{P}_{k|k-1} \)。
<加粗>更新步（迭代过程）：初始化迭代参数：设迭代次数 \( i = 0 \)，初始迭代状态 \( \hat{\mathbf{x}}_{k|k}^{(0)} = \hat{\mathbf{x}}_{k|k-1} \)，迭代收敛阈值 \( \epsilon \)（预设小正数）。迭代计算：确定后验状态和协方差：将收敛后的状态估计值作为后验状态 \( \hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k}^{(i+1)} \)；协方差矩阵通常取最后一次迭代的结果，或采用 \( \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_{k}^{(i+1)} \mathbf{H}_{k}^{(i+1)}) \mathbf{P}_{k|k-1} \)。
1. 计算观测雅可比矩阵：\( \mathbf{H}_{k}^{(i)} = \frac{\partial h}{\partial \mathbf{x}} \bigg|_{\hat{\mathbf{x}}_{k|k}^{(i)}} \)
2. 计算卡尔曼增益：\( \mathbf{K}_{k}^{(i)} = \mathbf{P}_{k|k-1} (\mathbf{H}_{k}^{(i)})^T (\mathbf{H}_{k}^{(i)} \mathbf{P}_{k|k-1} (\mathbf{H}_{k}^{(i)})^T + \mathbf{R}_{k})^{-1} \)
3. 状态更新：\( \hat{\mathbf{x}}_{k|k}^{(i+1)} = \hat{\mathbf{x}}_{k|k}^{(i)} + \mathbf{K}_{k}^{(i)} (\mathbf{z}_{k} - h(\hat{\mathbf{x}}_{k|k}^{(i)})) \)
4. 收敛判断：若 \( \|\hat{\mathbf{x}}_{k|k}^{(i+1)} - \hat{\mathbf{x}}_{k|k}^{(i)}\| < \epsilon \)，则迭代收敛，停止迭代；否则，令 \( i = i + 1 \)，重复上述步骤（直至达到预设最大迭代次数）。

IEKF的优势：通过迭代减小了线性化近似误差，估计精度高于EKF；局限性：迭代过程增加了计算量，实时性略低于EKF，需在精度和实时性之间权衡。

三、四轮前驱自主移动机器人模型推导

四轮前驱自主移动机器人的运动特性由其机械结构决定：前轮为驱动轮（提供动力）和转向轮（控制方向），后轮为从动轮（仅随动，无驱动和转向功能）。模型推导需基于机器人的运动学约束，建立世界坐标系与机器人本体坐标系之间的转换关系，进而得到状态方程（运动学模型）和观测方程（观测模型）。

3.1 坐标系定义

为明确机器人的位置和姿态，定义两个核心坐标系：

<加粗>世界坐标系（惯性坐标系）：记为 \( O_w - X_w Y_w \)，为固定坐标系，用于描述机器人在全局环境中的位置。原点 \( O_w \) 可任意设定（如环境起始点），\( X_w \) 轴和 \( Y_w \) 轴分别为水平和垂直方向。
<加粗>机器人本体坐标系（随动坐标系）：记为 \( O_r - X_r Y_r \)，固连于机器人本体，原点 \( O_r \) 取机器人几何中心（或后轮轴中点），\( X_r \) 轴沿机器人前进方向（向前为正），\( Y_r \) 轴垂直于 \( X_r \) 轴（向左为正），符合右手坐标系规则。

机器人的姿态由本体坐标系相对于世界坐标系的偏转角 \( \theta \) 描述（即 \( X_r \) 轴与 \( X_w \) 轴的夹角，逆时针为正）。

3.2 运动学模型推导

运动学模型描述机器人状态随时间的变化规律，核心是建立状态向量对时间的导数与控制输入之间的关系（连续时间模型），再通过离散化得到适用于滤波算法的离散时间模型。

3.2.1 状态向量与控制输入向量定义

状态向量（描述机器人核心运动状态）：\( \mathbf{x} = [x, y, \theta, v, \omega]^T \)
控制输入向量（机器人可直接控制的物理量）：\( \mathbf{u} = [v_c, \delta_c]^T \)

3.2.2 运动学约束分析

四轮前驱机器人的运动约束主要包括：

非滑动约束：机器人运动过程中，车轮与地面之间无相对滑动（纯滚动），即车轮接触点的速度在车轮切线方向的分量等于机器人本体速度，垂直方向分量为零。
非侧滑约束：后轮为从动轮，无转向功能，其运动方向始终沿机器人前进方向（\( X_r \) 轴），因此后轮接触点的速度在 \( Y_r \) 轴方向的分量为零。

3.2.3 连续时间运动学模型

根据坐标系转换关系和运动学约束，推导机器人状态的时间导数：

线速度与位置的关系：机器人本体坐标系下的线速度 \( v \) 可分解为世界坐标系下的 \( X_w \) 和 \( Y_w \) 方向分量，因此位置的时间导数为： \( \dot{x} = v \cos\theta \)\( \dot{y} = v \sin\theta \)
角速度与姿态角的关系：姿态角 \( \theta \) 的时间导数等于机器人的角速度 \( \omega \)，即： \( \dot{\theta} = \omega \)
控制输入与线速度、角速度的关系：前轮转向角 \( \delta \)（实际转向角，理想情况下等于指令 \( \delta_c \)）决定了机器人的转向半径 \( R \)。设机器人轮距为 \( L \)（前后轮轴之间的距离），则转向半径 \( R = \frac{L}{\tan\delta} \)（当 \( \delta \neq 0 \) 时）。根据圆周运动规律，角速度 \( \omega = \frac{v}{R} = \frac{v \tan\delta}{L} \)。

综合上述关系，连续时间运动学模型（状态方程）为：

\( \dot{\mathbf{x}} = \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \\ \dot{v} \\ \dot{\omega} \end{bmatrix} = \begin{bmatrix} v \cos\theta \\ v \sin\theta \\ \omega \\ a \\ \dot{\omega} \end{bmatrix} = f(\mathbf{x}, \mathbf{u}) \)

其中，\( a = \frac{v - v_{prev}}{\Delta t} \) 为线加速度（由前后时刻的线速度差计算），\( \dot{\omega} = \frac{\omega - \omega_{prev}}{\Delta t} \) 为角加速度；若忽略加速度动态（简化模型），可令 \( \dot{v} = 0 \)、\( \dot{\omega} = 0 \)，此时 \( v = v_c \)、\( \omega = \frac{v_c \tan\delta_c}{L} \)，简化后的连续模型为：

\( \dot{\mathbf{x}} = \begin{bmatrix} v_c \cos\theta \\ v_c \sin\theta \\ \frac{v_c \tan\delta_c}{L} \\ 0 \\ 0 \end{bmatrix} \)

3.2.4 离散时间运动学模型

滤波算法通常基于离散时间模型，需对连续时间模型进行离散化（采用一阶欧拉离散化，适用于采样时间 \( \Delta t \) 较小的场景）。设采样时间为 \( \Delta t \)，k时刻与k-1时刻的状态满足：

\( \mathbf{x}_k = \mathbf{x}_{k-1} + \dot{\mathbf{x}}_{k-1} \cdot \Delta t + \mathbf{w}_{k-1} \)

代入简化后的连续模型，得到离散时间运动学模型（状态方程）：

\( \begin{bmatrix} x_k \\ y_k \\ \theta_k \\ v_k \\ \omega_k \end{bmatrix} = \begin{bmatrix} x_{k-1} + v_{k-1} \cos\theta_{k-1} \cdot \Delta t \\ y_{k-1} + v_{k-1} \sin\theta_{k-1} \cdot \Delta t \\ \theta_{k-1} + \omega_{k-1} \cdot \Delta t \\ v_{c,k} \\ \frac{v_{c,k} \tan\delta_{c,k}}{L} \end{bmatrix} + \mathbf{w}_{k-1} \)

其中，\( \mathbf{w}_{k-1} \sim \mathcal{N}(0, \mathbf{Q}) \) 为过程噪声，主要来自驱动误差、转向误差和地面干扰，\( \mathbf{Q} \) 为过程噪声协方差矩阵（对角元素分别对应位置、姿态、速度、角速度的噪声方差）。

3.3 观测模型推导

观测模型描述机器人传感器的测量值与状态向量之间的关系。四轮前驱自主移动机器人常用的观测传感器包括：里程计（测量车轮转速，间接获取线速度）、陀螺仪（测量角速度）、GPS（测量全局位置）、激光雷达/视觉传感器（测量相对障碍物的位置，间接修正自身状态）。此处以“里程计+GPS”的观测方案为例，推导观测模型。

3.3.1 观测向量定义

观测向量为传感器测量值的集合：\( \mathbf{z} = [x_{gps}, y_{gps}, v_{odom}, \omega_{gyro}]^T \)

其中，\( x_{gps} \)、\( y_{gps} \) 为GPS测量的机器人全局位置；\( v_{odom} \) 为里程计测量的机器人线速度；\( \omega_{gyro} \) 为陀螺仪测量的机器人角速度。

3.3.2 观测模型建立

观测模型的核心是建立观测向量与状态向量之间的映射关系。理想情况下，传感器测量值等于对应的状态量，因此非线性观测函数 \( h(\cdot) \) 为：

\( h(\mathbf{x}) = \begin{bmatrix} x \\ y \\ v \\ \omega \end{bmatrix} \)

考虑观测噪声（如GPS测量噪声、里程计误差、陀螺仪漂移），实际观测方程（离散时间）为：

\( \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k = \begin{bmatrix} x_k \\ y_k \\ v_k \\ \omega_k \end{bmatrix} + \mathbf{v}_k \)

其中，\( \mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R}) \) 为观测噪声，\( \mathbf{R} \) 为观测噪声协方差矩阵（对角元素分别对应GPS位置、里程计速度、陀螺仪角速度的噪声方差）。

3.3.3 雅可比矩阵计算（EKF/IEKF用）

由于观测函数 \( h(\cdot) \) 为线性函数（上述映射关系为线性），其雅可比矩阵 \( \mathbf{H} \) 为常数矩阵，计算如下：

\( \mathbf{H} = \frac{\partial h}{\partial \mathbf{x}} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \)

若采用激光雷达等视觉传感器（观测值为相对障碍物的距离和角度），观测函数会变为非线性，需重新推导 \( h(\cdot) \) 并计算对应的雅可比矩阵。

四、总结

本文系统阐述了KF、EKF、IEKF算法的基本原理：KF适用于线性高斯系统，通过“预测-更新”迭代实现最优估计；EKF通过一阶泰勒展开线性化非线性系统，扩展了KF的应用范围，但存在线性化误差；IEKF在EKF的更新步引入迭代，减小了线性化误差，提升了估计精度。在此基础上，针对四轮前驱自主移动机器人，通过定义坐标系、分析运动学约束，推导得到了离散时间运动学模型（状态方程）和基于“GPS+里程计”的观测模型，并给出了EKF/IEKF所需的观测雅可比矩阵。该模型可直接用于机器人的状态估计与导航控制，为后续滤波算法的工程实现提供了理论基础。