【运动传感器】Madgwick算法（上）

Madgwick算法能够综合多种传感器参数得到传感器的姿态。传感器可以采用以下两种配置：

1. IMU：包含三轴线加速度计，测量物体坐标系下的三轴线加速度；三轴角速度计，测量物体坐标系下的三轴欧拉角变化率。
2. MARG或AHRS：除了前述两类，还包含三轴磁力计，测量地球磁力线在物体坐标系下投影。

以下从原始传感器参数逐步推导出物体姿态。在推导过程中，大量使用四元数(quaternion)来表示旋转和姿态，不熟悉的同学可以参看这篇文章。

本文分上下两篇，上篇讲解各个传感器独立结果，下篇讲解融合方法、对误差的处理，以及标定实验。
本篇公式较多，可以直接关注各分段结尾处的“结论”。

欧拉角变化率和角速度（gyro$\to \omega$）

问题：已知欧拉角变化率，求角速度。

二维空间中的角速度$\omega$是一个伪标量（pseudoscale）。其大小为单位时间转动过的弧度，其方向垂直于所在平面符合，和旋转方向符合右手定则，换言之，其方向为旋转轴。
三维空间中的角速度$\omega$是一个伪矢量（pseudovector），其大小为单位时间转动过的弧度，其方向和旋转方向符合右手定则。

用矢量表示角速度时，可以直接使用矢量加法表示角速度的叠加。
绕$x$轴旋转的角速度为${\omega }_{x}i$，其中$i$表示旋转轴$[1,0,0]$；类似地，绕$y,z$轴旋转的角速度为$ \omega_y j,\omega_z k $。

总体角速度和欧拉角变化率具有以下关系：
$$\omega ={\omega }_{x}i+{\omega }_{y}j+{\omega }_{z}k$$

其中，$i,j,k$为三个轴单位矢量。

完整的证明较为繁琐，可以定性地考察一下这个结论。
考虑绕$x$轴的旋转，该旋转发生在$yz$平面内，旋转矢量和$yz$平面垂直，即和$x$轴平行。
$\omega$在$x$轴投影为${\omega }_x$，就是绕$x$轴的旋转速度。

角速度$\omega$的旋转轴（归一化长度）为：
$$\frac{[{\omega }_x,{\omega }_y,{\omega }_z]}{\sqrt{{\omega }_x^2+{\omega }_y^2+{\omega }_z^2}}$$

旋转速度（单位时间旋转的角度）为：
$$\sqrt{{\omega }_x^2+{\omega }_y^2+{\omega }_z^2}$$

此部分证明原文可以参看维基百科角速度词条。

结论：$\omega =[0,{\omega }_x,{\omega }_y,{\omega }_z]$。

角速度和姿态四元数（$\omega \to q$）

问题：已知传感器坐标系下角速度和当前姿态四元数，求姿态四元数变化率，进而求姿态四元数。

首先考虑一个稍简单的情况，已知世界坐标系下角速度$\bar{\omega }$，以及当前姿态四元数$q$，求$\dot{q}$。（上标一点表示对时间的一次微分）。

考虑空间中任意一点$R_0=[0,x_0,y_0,z_0]$，当前时刻的位置为$R_t$，以下通过两个方面来写出当前线速度$\dot{R_t}$的表达。

第一方面，由于$R_t$是$R_0$经过变换$q$得到的，故：
$$R_t=q\times R_0\times q^{-1}(1)$$

对时间$t$求导，注意只有$q$是$t$的函数，$R_0$常量：
$$\dot{R_t}=\dot{q}\times R_0\times q^{-1}+q\times R_0\times \dot{q^{-1}}(2)$$

根据式(1)（此处存疑，叉乘不满足结合律？），有：

$$q^{-1}\times R_t=R_0\times q^{-1}$$

$$R_t\times q=q\times R_0$$

代入式(2)：
$$\dot{R_t}=\dot{q}\times q^{-1}\times R_t+R_t\times q\times \dot{q^{-1}}(3)$$

考察$q$和其反变换 q − 1 q^{-1}