基于陀螺仪积分与加速度计融合的卡尔曼滤波姿态估计方法及其漂移校正机制研究

《Robotics》：A Comparative Study of a Real-Time Ankle Mobility Monitoring Wearable System Giovanni Mastrangelo, Betsy Dayana Marcela Chaparro Rico, Matteo Russo, Marco Ceccarelli and Daniele Cafolla

　　

研究人员提出了一种结合陀螺仪动态积分与加速度计静态参考的姿态角估计方法，用于解决惯性导航系统中因累积积分导致的长期漂移问题。该方法采用离散时间卡尔曼滤波（Kalman Filter, KF）框架，将陀螺仪输出的角速度ωx、ωy、ωz通过积分获得预测姿态角θroll、θpitch、θyaw，并利用加速度计测量的重力矢量计算参考姿态角进行校正。过程噪声协方差矩阵Q被建模为对角矩阵，其中Q11=σ2roll、Q22=σ2pitch，分别表征陀螺仪在横滚与俯仰方向的积分不确定性。测量噪声协方差矩阵R同样设为对角形式，R11=σ2roll,acc、R22=σ2pitch,acc，反映加速度计在非重力加速度干扰下的测量噪声。滤波过程中，误差协方差矩阵P依据P=P+Q进行时间更新，卡尔曼增益K则通过K=P(P+R)-1计算，最终通过θcorr=θpred+K(θacc?θpred)实现姿态角的闭环校正。实验采用实测IMU数据验证，选取σroll=0.001°、σpitch=0.003°及σroll,acc=σpitch,acc=0.03°作为参数配置。

研究背景与意义

在移动机器人及人体运动追踪领域，惯性测量单元（Inertial Measurement Unit, IMU）因其低成本、高采样率及无外部依赖特性而被广泛应用。然而，纯陀螺仪积分虽能提供短期高精度动态响应，却因角速度测量中的微小偏差累积导致随时间增长的姿态漂移；加速度计虽可提供长期稳定的重力参考，但在动态运动中易受非重力加速度干扰。传统互补滤波难以自适应平衡两类传感器的权重，而卡尔曼滤波可通过统计最优估计有效融合异构传感器数据。本研究针对六自由度姿态估计中的漂移抑制问题，构建了适用于嵌入式系统的离散卡尔曼滤波模型，并在《Robotics》期刊发表，为便携式运动追踪设备提供了可参数化的解决方案。

关键技术方法

研究人员采用基于状态空间模型的离散卡尔曼滤波架构。系统状态向量包含横滚角θ_roll与俯仰角θ_pitch，状态转移由陀螺仪角速度积分驱动，观测值来自加速度计计算的倾角。过程噪声协方差矩阵Q表征陀螺仪积分的不确定性，测量噪声协方差矩阵R反映加速度计的噪声水平。通过调节Q与R的相对大小，可控制滤波器对陀螺仪动态响应与加速度计静态稳定性的信任权重。实验数据来源于实际IMU传感器采集的三轴陀螺仪与三轴加速度计时间序列，参数经初步测试确定为σ_roll=0.001°、σ_pitch=0.003°、σ_roll,acc=σ_pitch,acc=0.03°。

研究结果

1. 1.
   陀螺仪积分模型：研究人员建立了θ_roll(t)=θ_roll(t₀)+∫₀^tω_x(τ)dτ、θ_pitch(t)=θ_pitch(t₀)+∫₀^tω_y(τ)dτ、θ_yaw(t)=θ_yaw(t₀)+∫₀^tω_z(τ)dτ的动态方程，验证了纯积分方法在短时域内可精确跟踪快速运动，但存在不可忽略的长期漂移。
2. 2.
   过程噪声建模：Q矩阵被设计为对角形式，Q=diag([σ²_rollσ²_pitch])，其中σ_roll=0.001°、σ_pitch=0.003°的小方差设置允许滤波器逐步校正累积漂移而不影响稳定性。
3. 3.
   测量噪声建模：R矩阵同样采用对角结构，R=diag([σ²_roll,accσ²_pitch,acc])，σ_roll,acc=σ_pitch,acc=0.03°的较高噪声值降低了滤波器对加速度计动态干扰的敏感性。
4. 4.
   卡尔曼增益计算：通过K=P(P+R)^-1实现自适应权重分配，当P较大时滤波器更多依赖加速度计，P较小时则信任陀螺仪预测。
5. 5.
   姿态校正更新：采用θ_corr=θ_pred+K(θ_acc?θ_pred)完成闭环校正，实验表明该方法可有效抑制陀螺仪漂移同时保留动态响应能力。

讨论与结论

研究人员指出，所提方法通过合理设置Q与R矩阵，在动态运动与静态稳定之间实现了最优权衡。较小的Q值保证了陀螺仪的高频响应特性，较大的R值避免了加速度计受运动加速度干扰导致的估计波动。卡尔曼增益的自适应调整机制使系统无需手动切换滤波模式即可适应不同运动状态。该方法的局限性在于未考虑偏航角（yaw）的长期漂移校正，因加速度计无法提供绝对航向参考。研究结论证实，基于对角噪声协方差矩阵的简化卡尔曼滤波可在计算资源受限的嵌入式平台上实现实时、鲁棒的姿态估计，为消费级IMU应用提供了实用化解决方案。