【滤波跟踪】基于卡尔曼滤波kalman实现东北天坐标系IMU姿态解算附Matlab代码

matlab科研助手

于 2025-02-12 15:15:01 发布

阅读量878

点赞数 21

CC 4.0 BY-SA版权

文章标签： matlab 开发语言

本文链接：https://blog.youkuaiyun.com/matlab_dingdang/article/details/145593062

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

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

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

🌿 往期回顾可以关注主页，点击搜索

🔥 内容介绍

惯性测量单元 (Inertial Measurement Unit, IMU) 作为一种自主式导航设备，在各种应用场景中扮演着至关重要的角色，例如机器人导航、无人机控制、姿态估计以及运动捕捉等。IMU主要由加速度计和陀螺仪组成，分别测量载体的线加速度和角速度。然而，直接利用IMU数据进行姿态解算面临着诸多挑战，包括传感器噪声、零偏误差以及积分漂移等问题，这些误差会随着时间的推移而累积，导致姿态估计的精度显著下降。为了解决这些问题，卡尔曼滤波 (Kalman Filter) 作为一种强大的状态估计算法，被广泛应用于IMU姿态解算中，能够有效地抑制噪声干扰，提高姿态估计的准确性和鲁棒性。

本文旨在探讨如何利用卡尔曼滤波算法，在东北天坐标系 (NED Coordinate System) 下实现IMU姿态解算。文章将首先简要介绍东北天坐标系和IMU的原理，随后详细阐述基于卡尔曼滤波的姿态解算框架，包括状态方程和观测方程的建立，以及卡尔曼滤波的具体步骤。最后，将讨论影响姿态解算精度的关键因素，并展望未来研究方向。

一、东北天坐标系与IMU原理

东北天坐标系是一种右手坐标系，其定义如下：X轴指向正北方向，Y轴指向正东方向，Z轴指向地心方向 (向下)。在NED坐标系下进行姿态解算，更易于与实际应用场景进行关联，例如导航定位等。

IMU主要由三个轴向的加速度计和三个轴向的陀螺仪组成。加速度计测量载体在三个轴向上的线加速度，陀螺仪测量载体绕三个轴向的角速度。然而，实际的IMU数据往往包含噪声和误差，主要包括：

传感器噪声: 加速度计和陀螺仪本身固有的随机噪声，通常建模为高斯白噪声。
零偏误差 (Bias): 加速度计和陀螺仪的输出与真实值之间的固定偏差，会随着时间和温度的变化而缓慢漂移。
标度因子误差 (Scale Factor Error): 加速度计和陀螺仪的输出与其真实值之间的比例系数误差。
轴向不对准误差 (Misalignment Error): IMU传感器轴向与载体坐标系轴向之间的不对准角度。

这些误差的存在会导致姿态解算结果出现漂移和误差累积，因此需要采用合适的滤波算法进行补偿和校正。

二、基于卡尔曼滤波的姿态解算框架

卡尔曼滤波是一种递推式的状态估计算法，其核心思想是通过融合系统的过程模型 (状态方程) 和测量模型 (观测方程)，并结合上一时刻的最优估计值和当前时刻的测量值，来迭代地估计系统的状态。

在基于卡尔曼滤波的IMU姿态解算中，需要构建合适的状态方程和观测方程。

1. 状态方程

状态方程描述了系统状态随时间的变化规律。在IMU姿态解算中，状态向量通常包含以下几个部分：

姿态角 (Roll, Pitch, Yaw): 分别表示载体绕X轴 (横滚)、Y轴 (俯仰) 和Z轴 (偏航) 的旋转角度。
陀螺仪零偏: 陀螺仪的零偏误差。
加速度计零偏: 加速度计的零偏误差。

因此，可以将状态向量定义为：

x = [Roll, Pitch, Yaw, Bias_gyro_x, Bias_gyro_y, Bias_gyro_z, Bias_acc_x, Bias_acc_y, Bias_acc_z]^T

状态方程可以表示为：

x(k+1) = F * x(k) + w(k)

其中：

x(k) 表示 k 时刻的状态向量。
x(k+1) 表示 k+1 时刻的状态向量。
F 表示状态转移矩阵，描述了状态向量从 k 时刻到 k+1 时刻的演化关系。
w(k) 表示过程噪声，用于描述模型误差和未建模的干扰，通常建模为高斯白噪声，其协方差矩阵为 Q。

姿态角可以使用角速度进行积分来更新，例如使用四元数微分方程或者欧拉角微分方程。陀螺仪零偏和加速度计零偏可以建模为随机游走过程，即：

Bias(k+1) = Bias(k) + w_bias(k)

其中 w_bias(k) 表示零偏的噪声，通常建模为高斯白噪声。

2. 观测方程

观测方程描述了系统状态与测量值之间的关系。在IMU姿态解算中，常用的观测值包括：

磁力计测量值: 磁力计可以测量地球磁场在三个轴向上的分量，可以用于修正偏航角。
全球导航卫星系统 (GNSS) 位置信息: GNSS可以提供载体的位置信息，可以用于修正姿态角。
重力加速度信息: 在静止或者匀速运动情况下，加速度计可以测量重力加速度，可以用于修正横滚角和俯仰角。

因此，观测方程可以表示为：

z(k) = H * x(k) + v(k)

其中：

z(k) 表示 k 时刻的测量向量。
H 表示观测矩阵，描述了状态向量与测量值之间的关系。
v(k) 表示测量噪声，通常建模为高斯白噪声，其协方差矩阵为 R。

观测矩阵 H 的具体形式取决于所使用的观测值。例如，如果使用磁力计测量值，则 H 矩阵需要将姿态角与磁场测量值联系起来。如果使用重力加速度信息，则 H 矩阵需要将姿态角与重力加速度联系起来。

3. 卡尔曼滤波步骤

卡尔曼滤波算法包含两个主要步骤：预测步骤和更新步骤。

预测步骤:

其中：
- x(k+1|k) 表示在 k 时刻对 k+1 时刻状态的预测值。
- P(k+1|k) 表示在 k 时刻对 k+1 时刻状态协方差矩阵的预测值。
- x(k|k) 表示在 k 时刻的最优状态估计值。
- P(k|k) 表示在 k 时刻的最优状态协方差矩阵。
- 状态预测: x(k+1|k) = F * x(k|k)
- 协方差预测: P(k+1|k) = F * P(k|k) * F^T + Q
更新步骤:

其中：
- K(k+1) 表示卡尔曼增益，用于调整预测值和测量值的权重。
- x(k+1|k+1) 表示在 k+1 时刻的最优状态估计值。
- P(k+1|k+1) 表示在 k+1 时刻的最优状态协方差矩阵。
- I 表示单位矩阵。
- 卡尔曼增益: K(k+1) = P(k+1|k) * H^T * (H * P(k+1|k) * H^T + R)^(-1)
- 状态更新: x(k+1|k+1) = x(k+1|k) + K(k+1) * (z(k+1) - H * x(k+1|k))
- 协方差更新: P(k+1|k+1) = (I - K(k+1) * H) * P(k+1|k)

通过迭代执行预测步骤和更新步骤，卡尔曼滤波能够不断地更新状态估计值，并降低状态估计的不确定性，从而实现高精度的姿态解算。

三、影响姿态解算精度的关键因素

基于卡尔曼滤波的IMU姿态解算精度受到多种因素的影响，主要包括：

IMU传感器性能: IMU传感器的精度直接影响姿态解算的精度。更高精度的IMU传感器通常具有更低的噪声和零偏，从而能够提供更准确的测量数据。
过程噪声和测量噪声的协方差矩阵 (Q 和 R) 的设置: Q 和 R 的设置直接影响卡尔曼滤波的性能。如果 Q 设置过大，则滤波器的响应速度较快，但容易受到噪声干扰。如果 Q 设置过小，则滤波器的响应速度较慢，但能够更好地抑制噪声。R 的设置与测量噪声的精度相关。
状态方程和观测方程的建模精度: 状态方程和观测方程的建模精度直接影响卡尔曼滤波的性能。如果模型与实际情况不符，则会导致滤波器的性能下降。
初始状态的设置: 初始状态的设置对滤波器的收敛速度和精度有一定的影响。如果初始状态设置不准确，则会导致滤波器的收敛速度变慢或者出现错误。
计算精度: 在进行矩阵运算时，计算精度可能会受到限制，导致舍入误差。可以使用更高精度的浮点数类型来提高计算精度。
外部辅助传感器融合: 融合其他传感器信息，例如GNSS、磁力计、气压计等，可以有效提高姿态解算的精度和鲁棒性。例如，GNSS可以提供位置信息，磁力计可以提供偏航角信息，气压计可以提供高度信息。

⛳️ 运行结果

🔗 参考文献

[1] 谭菊.基于Kalman滤波的目标轨迹预测[J].重庆文理学院学报(自然科学版), 2009.DOI:JournalArticle/5af44b11c095d718d81812f8.

[2] 员丽琼,景占荣.SINS/GPS组合导航系统Kalman滤波仿真研究[J].自动化仪表, 2011.DOI:CNKI:SUN:ZDYB.0.2011-01-022.

📣 部分代码

function end_angle = CalEndAngle_Zcoord( begin_angle,add_angle )    begin_rad = begin_angle*pi/180;     add_rad = add_angle*pi/180 ;    add_matrix = [cos(add_rad),-sin(add_rad),0;sin(add_rad),cos(add_rad),0;0,0,1];    begin_xy = [cos(begin_rad),sin(begin_rad),0]';    end_xy = add_matrix*begin_xy;    end_rad = atan2(end_xy(2),end_xy(1));    end_a