Skip to content
返回搜索结果

Madgwick 滤波器

提出者
Sebastian Madgwick
年份
2010
类型
Filter algorithm
类别
Gradient Descent Filtering

本文为机器翻译,尚未经过审校。 阅读英文原文

字号

它有什么作用?

Madgwick 滤波器是一种计算量轻量级的姿态估计算法,它融合了惯性测量(加速度计、陀螺仪)和磁性测量(磁力计)来计算四元数方向。该算法由 Sebastian Madgwick 于 2010 年提出,使用梯度下降优化来最小化测量值与预期传感器输出之间的误差,从而在计算成本极低的情况下,在嵌入式系统上实现精确、无漂移的姿态估计。Madgwick 滤波器现已广泛应用于消费电子、机器人和航空航天系统。

方法背后的直觉

其思想是找到最能解释观测到的传感器测量的四元数(方向)。加速度计和罗盘告诉我们重力和地磁北极的方向;陀螺仪告诉我们旋转的速度。该算法计算姿态调整的方向(梯度),使估计值与测量值更接近,然后朝着该方向小步移动。每秒重复多次,即可产生平滑、实时的姿态估计,且不会漂移。

历史学

Sebastian Madgwick 在研究运动捕捉和人体运动分析期间于 2010 年左右开发了该算法。该算法于 2011 年发表在 IEEE 康复机器人国际会议上,并作为开源代码发布。由于其简单性和有效性,它在 DIY 机器人和创客社区中迅速普及。主要的消费电子公司(智能手机、VR 头显、健身追踪器)采用了其变体。Madgwick 滤波器在嵌入式系统和教育领域仍然很受欢迎,经常作为传感器融合的入门介绍。

何时使用

当您需要在资源受限的设备(微控制器、手机、可穿戴设备)上进行轻量级、实时姿态估计时,请使用 Madgwick 滤波器。非常适合卡尔曼滤波器过于复杂的应用。在室内和室外、车辆、无人机和可穿戴设备中均有效。当您拥有 9 自由度 IMU 并希望避免调整多个卡尔曼滤波器参数时,请部署。

局限与注意事项

  • 启发式,非最优;不像卡尔曼滤波器那样最小化任何形式的成本函数。
  • 性能严重依赖于 beta(步长);传感器质量或运动状态变化时无自动适应。
  • 磁力计干扰会破坏航向;需要手动校准;无法自动检测或拒绝不良测量。
  • 大的加速度(在 MARG 变体中)会导致暂时的误差,因为加速度计与重力对齐不正确;滤波器恢复是启发式的,非原理性的。

来源中的条件不能替代独立的科学审查或分析引擎的验证。

优势

  • 计算成本极低;代码行数少于 100 行;可在微控制器和移动处理器上运行。
  • 无需矩阵或复杂的线性代数;仅需四元数代数和点积。
  • 提供仅 IMU(6 自由度)和 MARG(9 自由度)变体;可适应可用传感器。
  • 单个调优参数(beta)使其易于定制;无需协方差矩阵或矩阵求逆。

应用

  • 智能手机和可穿戴设备运动追踪:无需大量计算即可实现手势识别和增强现实。
  • 四旋翼和无人机稳定:为资源受限的自动驾驶仪提供飞行控制所需的横滚、俯仰、偏航估计。
  • 虚拟现实(VR)头显运动追踪:为沉浸式显示器提供实时头部方向。
  • 运动捕捉和人体动画:追踪步态分析和动画的身体片段方向。
  • 机器人和业余无人机:Arduino、Raspberry Pi 上姿态估计的轻量级卡尔曼滤波器替代方案。

使用方式

使用方式

1

以 50 Hz 以上的频率从 9 自由度 IMU 收集测量值:3 轴加速度计、3 轴陀螺仪和 3 轴磁力计。

2

将加速度计和磁力计向量归一化为单位长度;存储陀螺仪原始速率向量;应用任何已知的传感器校准(偏差、比例因子)。

3

计算测量误差相对于四元数的梯度;沿最小化误差的方向(梯度下降)更新四元数;积分陀螺仪速率以预测四元数演化。

公式
\nabla f = \frac{\partial f}{\partial \vec{q}}
4

通过单个参数(beta)将梯度下降校正与陀螺仪积分融合,该参数控制校正强度;每次更新时输出四元数。

5

将四元数转换为欧拉角(横滚、俯仰、偏航)或直接使用四元数;报告姿态;根据传感器可靠性和运动状态评估置信度。

常见陷阱

  • 选择过高的 beta 会导致快速振荡和发散;过低的 beta 会导致响应迟缓;典型范围为 0.01–0.1,通过经验调优。
  • 忽略磁力计校准;软铁和硬铁失真会导致航向误差;准确的偏航角需要精确校准。
  • 假设加速度计始终有效;在剧烈加速期间,加速度计无法用于横滚/俯仰估计;一些算法会在机动期间检测并忽略加速度计。
  • 未经初始化部署;算法在几秒钟内收敛,但最好在车辆静止时进行初始化(静态初始化)。

常见问题

如何选择 beta 参数?

Beta 控制梯度下降校正的速率。从 beta = 0.041(Madgwick 使用的值)开始。如果滤波器响应迟缓(对运动响应慢),则增加 beta(最高可达 0.1–0.5)。如果滤波器振荡或漂移,则降低 beta(最低可达 0.01)。通过观察姿态对已知运动的响应来经验性地调优。

IMU(6 自由度)和 MARG(9 自由度)变体之间有什么区别?

IMU 变体仅使用加速度计和陀螺仪;它提供横滚和俯仰,但偏航会漂移而没有参考。MARG 变体增加了磁力计以获得绝对偏航;它提供稳定的 3 自由度姿态,但对磁场干扰敏感。短时间飞行使用 IMU;静止或慢速导航使用 MARG。

我可以在 32 位微控制器上运行 Madgwick 滤波器吗?

可以。该算法效率很高;ARM Cortex-M4 可在 200 Hz 以上运行。即使是 8 位微控制器(带浮点库)也能处理 50 Hz 以上的频率。性能取决于时钟速率和浮点支持。使用定点算术,它可以扩展到更慢的处理器。

如何检测磁场干扰?

监测磁力计幅度;地磁场约为 50 微特斯拉;较大的偏差表明存在干扰。比较罗盘航向(来自磁力计)与陀螺仪积分的航向;较大的差异表明存在干扰。缓解措施:在干扰期间暂时忽略磁力计更新,或降低其权重(某些变体中降低 beta_m)。

相关方法

参考文献

  1. Madgwick, S. O. H., Harrison, A. J. L., & Vaidyanathan, R. (2011). Estimation of IMU and MARG orientation using a gradient descent algorithm. IEEE International Conference on Rehabilitation Robotics (ICORR), 1–7.

    打开出版物文献
    已复制。
  2. Madgwick, S. O. H. (2010). An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Report x-io Technologies, University of Bristol, UK.

    打开出版物文献
    已复制。
  3. Sabatini, A. M. (2006). Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing. IEEE Transactions on Biomedical Engineering, 53(7), 1346–1356.

    打开 DOI 文献
    已复制。