【传感器融合姿态估计】Madgwick算法（下）

在上篇中，讲解了分别从角速度计、加速度计、磁力计估计姿态的方法。本篇文章讲解他们的融合，对误差的处理，以及标定实验。

融合

这一部分在文章中称为filter。
在上一篇文章中，我们能够通过角速度计的读数 ω ，加速度计或磁力计的读数 dS 在每个采样间隔更新姿态四元数 q ：

qt+1=qt+12qt×ωtΔt

qt+1=qt−μ∇f||∇f||

其中 f 是姿态拟合误差，可以通过当前姿态 q 和读数 dS 计算得到。
两者融合有：

qt+1=qt+12qt×ωtΔt−β∇f||∇f||Δt

增量由两部分构成：

第一部分 12qt×ωt 为通过角速度计获取的结果。
如果角速度计是完美的，则只需这部分即可。

第二部分 ∇f||∇f|| 为通过加速度计/磁力计获取的结果。其权重 β 表示角速度计的误差。
如果误差为0，则不需要此项；否则，因为角速度计不完美，当前姿态 q 有误差，导致拟合误差 f 没有达到极小， ∇f 不为0矢量，需要沿着误差下降方向对姿态 q 进行修正，修正幅度和角速度计误差成正比。

本部分证明原文参见Madgwick Internal Report3.3节。原始证明感觉就是凑的尚存疑，此处根据个人理解书写。

磁力计偏转

根据前文推导，世界坐标系下磁力矢量 dE=[0,bx,0,bz] 和传感器坐标系下磁力矢量 dS=[0,mx,my,mz] 应满足如下关系：

[0,bx,0,bz]T=q×[0,mx,my,mz]T×q−1

但由于误差存在（电子仪器干扰，磁铁干扰，地磁偏转等），变换后的y坐标可能不为零，得到结果为 [0,b′x,b′y,b′z] 。

[0,b′x,b′y,b′z]T=q×[0,mx,my,mz]T×q−1

在计算误差 e 和Jacobian矩阵 J 时，可以人工强制消除此误差：

dE=[0,b′2x+b′2y−−−−−−−√,0,b′z]

这样做的另一个好处是，x轴正方向不必强制指北，指向平面任意方向即可。

角速度计误差

存疑：这部分道理不明，作者网站的源码中也没有实现，只简略记下流程。

角速度计误差由两部分组成
漂移（bias drift）： ωζ˙ ，单位rad/s/s。
增益（error）： ωβ， 单位rad/s。

角速度计的三个示数和真实值的关系为：

ωtrue=ωβωread+ωζ˙

两个传感器参数可以通过后面的标定算法测量而知。

进一步可以得到两个系统参数：

β=||[0,ωβ,ωβ,ωβ]||=34−−√ωβ

ζ=||[0,ωζ˙,ωζ˙,ωζ˙]||=34−−√ωζ˙

其中 β 就是前节融合算法中，加速度计/磁力计的权重参数。
而 ζ 则用来直接修正加速度计中的 ω 。

ω′=ω−ζ∫2q−1t×∇f||∇f||dt

其中 ∇f||∇f|| 表示 q 的误差随时间变化率。经过反变换，积分号内部分为 ω 的误差随时间变化率。其对时间的积分表示均值不为零的部分。

标定

使用8台高速（120Hz）红外摄像机，拍摄带有红外标记（杆顶端的亮点）的支架，获取标定支架（三条杆）上传感器（橘黄盒子）的真实姿态。

涉及四个坐标系统：世界（E），摄像机（C），支架（M），传感器（S）。各个坐标系统之间的转换按如下方式确定。

世界(E) ↔ 摄像机(C)

使用棉线悬挂重物，垂线方向的单位向量 cZ 记为摄像机坐标系下世界坐标系的z轴。

使用棉线悬挂一根磁铁杆，杆方向的单位向量 cX 记为摄像机坐标系下世界坐标系的x轴。

垂直轴 cZ 一般较准确，但由于摩擦力等因素，水平轴 cX 可能和z不垂直（不够平）。用以下方法进行修正：

cX=cX−(cTXcZ)cZ

cX=cX/||cX||

即，将 cX 向z轴垂直方向投影，之后再归一化。

y轴由xz轴叉乘得到。

2×3 矩阵 R=[cX,cY,cZ] 即为从摄像机到世界的旋转矩阵，可以将其转换为四元数 CEq 。

摄像机(C) ↔ 支架(M)

设拍摄图像中，三个杆方向的单位向量（参看前图）为 cX,cY,cZ 。并排构成如下 2×3 的矩阵： R=[cX,cY,cZ]=(rij)i=1:2,j=1:3 。
构造如下的 4×4 的矩阵：

该矩阵最大特征值对应的特征向量即为摄像机坐标系下支架的姿态四元数，换言之，得到的是从摄像机到支架的转换 CMq 。
此部分证明超出本文范围，可以参看Bar-Itzhack的论文1。

支架(M) ↔ 传感器(S)

两者之间的相对关系无法测量，必须借助第三方的姿态估计算法。此处使用Kalman算法2，在静止情况下，获得世界坐标系中传感器的姿态四元数 SEqKalman ，将其认为是真实值。从传感器到支架的转换可以通过下式获取：

SMq=CMq×ECq×SEqKalman

综合三个结果，可以通过摄像机获取世界坐标系下的传感器姿态：

SEq=CEq×MCq×SMq

结论

比较摄像机获取的姿态和前述算法估计的姿态，可以对算法进行评估。
作者在自己公司的网站上给出了Madgwick算法的源码。网站上还有许多实际应用例子。

Madgwick算法的一个显著优点是通用性：不需要对运动做任何假设，可以直接套用。但是，在实际应用中，还应该尽量利用先验知识，对加速度、速度或者位置进行重置，避免随时间的漂移。

1. Itzhack Y Bar-Itzhack. New method for extracting the quaternion from a rotation matrix. AIAA Journal of Guidance, Control and Dynamics, 23(6):10851087, Nov.Dec 2000. (Engineering Note). ↩
2. R. E. Kalman. A new approach to linear ltering and prediction problems. Journal of Basic Engineering, 82:35{45, 1960. ↩