【滤波跟踪】基于扩展卡尔曼滤波实现里程计和 GPS 的融合附Matlab代码

最新推荐文章于 2025-04-28 23:54:11 发布

matlab科研社

最新推荐文章于 2025-04-28 23:54:11 发布

阅读量722

点赞数 16

CC 4.0 BY-SA版权

文章标签： matlab 开发语言

本文链接：https://blog.youkuaiyun.com/Matlab_dashi/article/details/145423382

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

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

🎁 私信更多全部代码、Matlab仿真定制

🔥 内容介绍

在机器人导航、自动驾驶以及其他需要精确位置估计的应用领域中，可靠且准确的定位至关重要。单一的传感器往往无法提供足够鲁棒性和精度的定位信息。里程计，通过测量轮子的转动来推断车辆的位姿，虽然短期精度较高，但会随着时间的推移积累误差。全球定位系统 (GPS) 虽然能够提供绝对位置信息，但其精度易受环境因素影响，例如信号遮挡和多径效应，且更新频率较低。为了克服单一传感器的局限性，多传感器融合技术应运而生，而卡尔曼滤波作为一种经典的融合算法，在实践中得到了广泛应用。本文将重点讨论如何利用扩展卡尔曼滤波（EKF）融合里程计和 GPS 数据，以实现高精度定位。

1. 里程计和 GPS 的特点分析

在深入探讨融合算法之前，我们必须充分理解里程计和 GPS 的特性。

1.1 里程计的特性

里程计主要通过测量车辆轮子的转动次数或转速来推算车辆的相对位姿变化。其优点包括：

高频率: 里程计可以以较高的频率提供位置更新，使得能够实时跟踪车辆的运动轨迹。
短期精度高: 在短时间内，里程计能够提供相对精确的位姿估计，误差累积较慢。
成本较低: 里程计的硬件成本相对较低，易于集成到车辆系统中。

然而，里程计也存在一些明显的局限性：

累积误差: 由于轮子的滑动、打滑以及测量误差等因素，里程计的误差会随着时间的推移而累积，导致长期位置估计不准确。
对环境敏感: 路面状况、轮子磨损等因素都会影响里程计的精度。
无法提供绝对位置: 里程计只能推算相对位姿变化，需要一个初始位置作为参考。

1.2 GPS 的特性

GPS 利用卫星发送的信号来计算接收机的位置。其优点包括：

提供绝对位置: GPS 可以提供全球坐标系下的绝对位置信息，不受初始位置的限制。
不易受累积误差影响: GPS 误差相对独立，不会随着时间的推移而累积。

然而，GPS 也存在一些不足：

精度受环境影响: 建筑物、树木等障碍物会遮挡 GPS 信号，导致精度下降或信号丢失。
更新频率较低: GPS 的更新频率通常较低，无法实时捕捉车辆的运动细节。
易受多径效应影响: GPS 信号在传播过程中可能发生反射，导致接收机接收到多个信号，从而产生误差。

2. 扩展卡尔曼滤波（EKF）原理

扩展卡尔曼滤波是卡尔曼滤波的一种非线性扩展，主要用于解决非线性系统的状态估计问题。其核心思想是：利用系统模型和测量模型，结合系统噪声和测量噪声，递推估计系统状态。EKF 的过程主要分为两个步骤：

预测（Prediction）：根据上一时刻的状态估计和系统模型，预测当前时刻的状态。
更新（Update）：利用当前时刻的测量值，修正预测的状态，得到更精确的状态估计。

具体而言，假设系统状态向量为 x, 系统模型为 f(x, u) (其中 u 是控制输入), 测量向量为 z, 测量模型为 h(x)。则 EKF 的基本方程可以描述如下：

2.1 预测步骤

状态预测:

xk|k-1 = f( xk-1|k-1, uk )

其中 xk|k-1 是 k 时刻的状态预测值，xk-1|k-1 是 k-1 时刻的状态估计值。
误差协方差矩阵预测:

Pk|k-1 = Fk-1 Pk-1|k-1 Fk-1T + Qk-1

其中 P 是误差协方差矩阵，Q 是系统噪声协方差矩阵，F 是系统模型的雅可比矩阵。

2.2 更新步骤

卡尔曼增益计算:

Kk = Pk|k-1 HkT (Hk Pk|k-1 HkT + Rk)-1

其中 R 是测量噪声协方差矩阵，H 是测量模型的雅可比矩阵，K 是卡尔曼增益。
状态更新:

xk|k = xk|k-1 + Kk (zk - h(xk|k-1))

其中 xk|k 是 k 时刻的状态估计值，zk 是 k 时刻的测量值。
误差协方差矩阵更新:

Pk|k = ( I - Kk Hk ) Pk|k-1

其中 I 是单位矩阵。

3. 基于 EKF 的里程计和 GPS 融合

现在我们应用扩展卡尔曼滤波来融合里程计和 GPS 数据，从而获得更精确的定位信息。

3.1 系统模型构建

首先需要定义系统状态向量 x。通常，一个二维的定位系统可以包括以下状态：

x = [ x, y, θ, v ]T

其中 x 和 y 表示车辆在全局坐标系下的位置坐标，θ 表示车辆的航向角，v 表示车辆的线速度。

系统模型 f(x, u) 可以描述为：

xk = xk-1 + vk-1 Δt cos(θk-1)
yk = yk-1 + vk-1 Δt sin(θk-1)
θk = θk-1 + ωk-1 Δt
vk = vk-1

其中 Δt 是时间间隔，ω 是车辆的角速度，由里程计提供。控制输入 u = [ω]T。

3.2 测量模型构建

测量向量 z 包括来自 GPS 和里程计的数据。GPS 提供车辆的绝对位置，因此测量模型 h(x) 可以定义为：

zGPS = [x, y]T

里程计可以提供车辆的线速度和角速度信息，但这些信息已经在系统模型中使用了。我们还可以利用里程计的位姿变化信息，即相对位置变化来辅助更新。

3.3 雅可比矩阵计算

为了应用 EKF，我们需要计算系统模型和测量模型的雅可比矩阵。

系统模型雅可比矩阵 F:

F =
[1 0 - vk-1 Δt sin(θk-1) Δt cos(θk-1) ]
[0 1 vk-1 Δt cos(θk-1) Δt sin(θk-1) ]
[0 0 1 0 ]
[0 0 0 1]

GPS 测量模型雅可比矩阵 HGPS:

HGPS =
[ 1 0 0 0]
[ 0 1 0 0]

3.4 噪声协方差矩阵设置

系统噪声协方差矩阵 Q 和测量噪声协方差矩阵 R 需要根据实际情况进行设置。 Q 描述了系统模型的不确定性，而 R 描述了测量值的不确定性。合理设置这些参数对滤波效果至关重要。通常，我们会根据传感器的精度和误差特性来调整这些参数。

4. EKF 融合流程

以下是利用 EKF 融合里程计和 GPS 数据的流程：

初始化：初始化状态向量 x 和误差协方差矩阵 P。
预测步骤：
- 利用里程计数据和系统模型，进行状态预测 xk|k-1 和误差协方差矩阵预测 Pk|k-1。
更新步骤：
- 如果有 GPS 测量数据可用，则利用 GPS 测量值和测量模型，计算卡尔曼增益 Kk。
- 利用 Kk 更新状态估计 xk|k 和误差协方差矩阵 Pk|k。
重复步骤 2 和 3: 循环执行预测和更新步骤，持续进行定位。

5. 实验结果与分析

通过仿真实验或实际环境测试，我们可以验证基于 EKF 融合里程计和 GPS 的定位效果。通常，通过融合，我们能够获得比单独使用里程计或 GPS 更准确和更鲁棒的定位结果。在 GPS 信号良好时，融合结果会更接近 GPS 的绝对位置；在 GPS 信号受干扰时，融合结果仍然可以依赖里程计的短期精度，避免出现大的位置跳变。

📣 部分代码

    close all;    clear all;    disp('EKF Start!')    time = 0;    global endTime; % [sec]    endTime = 60;    global dt;    dt = 0.1; % [sec]    removeStep = 5;    nSteps = ceil((endTime - time)/dt);    estimation.time=[];    estimation.u=[];    estimation.GPS=[];    estimation.xOdom=[];    estimation.xEkf=[];