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 上姿态估计的轻量级卡尔曼滤波器替代方案。
使用方式
使用方式
以 50 Hz 以上的频率从 9 自由度 IMU 收集测量值:3 轴加速度计、3 轴陀螺仪和 3 轴磁力计。
将加速度计和磁力计向量归一化为单位长度;存储陀螺仪原始速率向量;应用任何已知的传感器校准(偏差、比例因子)。
计算测量误差相对于四元数的梯度;沿最小化误差的方向(梯度下降)更新四元数;积分陀螺仪速率以预测四元数演化。
公式
\nabla f = \frac{\partial f}{\partial \vec{q}}通过单个参数(beta)将梯度下降校正与陀螺仪积分融合,该参数控制校正强度;每次更新时输出四元数。
将四元数转换为欧拉角(横滚、俯仰、偏航)或直接使用四元数;报告姿态;根据传感器可靠性和运动状态评估置信度。
常见陷阱
- 选择过高的 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)。
相关方法
参考文献
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.
打开出版物文献已复制。Madgwick, S. O. H. (2010). An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Report x-io Technologies, University of Bristol, UK.
打开出版物文献已复制。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 文献已复制。