[Arduino] Madgwick算法的数学推演

Madgwick算法的数学推演：基于惯性导航元件的位姿估计

《An efficient orientation filter for inertial and inertial/magnetic sensor arrays》 Sebastian O.H. Madgwick

• 文章简介：这篇文章详细的介绍了基于惯性导航元件位姿估计的Madgwick算法的推导过程并将实验结果和基于卡尔曼滤波算法的结果比对，证明了Madgwick算法的优秀性能。在摘要中主要提到了三个亮点：
  1) Wadgwick算法对于IMU（Inertial Measurement Unit）每次迭代仅使用109次运算，对于MARG（IMU + magntetometers）传感器使用277次运算，非常适合开发轻量级惯导设备。
  2) Wadgwick算法在低采样频率的情况下仍保持良好性能。
  3) 对于IMU传感器有一个可调参数，对于MARG传感器有仅有两个可调参数。下面对文章中的数学过程进行逐一翻译和梳理。
   
• 需要提前掌握的数学内容：
  四元数的概念，四元数的乘法以及含义，四元数的共轭，用四元数计算向量的旋转，四元数与旋转矩阵的转换，四元数与欧拉角的转换。文章中的四元数${}_B^A{\hat{\mathbf{q}}}_{}$含义为：坐标系B相对于坐标系A的单位旋转四元数。
   
• 算法核心思想：
  Wadgwick算法的核心思想如下：首先，通过陀螺仪的角速度初步计算传感器方向偏移 ${}_E^S{\mathbf{q}}_{\omega ,t}$；然后，通过对重力加速度和地磁这两个恒定量在传感器坐标系下的投影计算传感器方向偏移 ${}_E^S{\mathbf{q}}_{\nabla ,t}$；最后通过互补滤波器融合 ${}_E^S{\mathbf{q}}_{\omega ,t}$ 和 ${}_E^S{\mathbf{q}}_{\nabla ,t}$ 来得到一个更加可信的结果。除此之外还需要校正地磁计数据畸变和陀螺仪零点漂移。我们下面会一步一步看各部分的数学推演。
   
• 通过陀螺仪的角速度对${}_E^S{\mathbf{q}}_{\omega ,t}$的计算：
  从陀螺仪中直接读取的数据为${}^S\mathbf{\omega }=[0,{\omega }_x,{\omega }_y,{\omega }_z]$。地面参考系（Earth）相对于传感器参考系（Sensor）的旋转可以由其四元数的导数对时间的积分获得。
   
  ${}_E^S{\mathbf{q}}_{\omega ,t}{=}_E^S{\hat{\mathbf{q}}}_{est,t-1}{+}_E^S{\dot{\mathbf{q}}}_{\omega ,t}\Delta t$
   
  其中${}_E^S{\hat{\mathbf{q}}}_{est,t-1}$是在t-1时刻得到的最佳单位位姿四元数。而${}_E^S{\dot{\mathbf{q}}}_{\omega ,t}$可以用David et al.提出的Npsnet中的数学模型进行运算，公式如下：
   
  ${}_E^S{\dot{\mathbf{q}}}_{\omega ,t}={\frac{1}{2}}_E^S{\hat{\mathbf{q}}}_{est,t-1}{\times }^S{\mathbf{\omega }}_t$
   
  公式中的角速度可以直接从传感器读出，上一时刻的位姿也是已知，时间间隔也可以从传感器芯片中的获得。可以轻松的计算出${}_E^S{\mathbf{q}}_{\omega ,t}$的数值结果。
   
• 通过恒定向量对${}_E^S{\mathbf{q}}_{\nabla ,t}$的计算：
  这一部分是利用外部恒定的向量在各个角度的投影来估算传感器的位姿。在IMU中将利用重力加速度，而在MARG中将同时利用重力加速度和地磁方向来计算。对于在Earth参考系下的恒定向量${}^E\hat{\mathbf{d}}=[0,d_x,d_y,d_z]$，在Sensor坐标系下通过传感器读出的数据为${}^S\hat{\mathbf{s}}=[0,s_x,s_y,s_z]$。在理论上，这二者存在如下关系：
   
  ${}^S\hat{\mathbf{s}}{=}_E^S{\hat{\mathbf{q}}}^{\ast }{\times }^E\hat{\mathbf{d}}{\times }_E^S\hat{\mathbf{q}}$
   
  但是这二者是通过不同的方式获得的，其间的等式关系基本不存在，这也恰好提供了一个估计 ${}_E^S\hat{\mathbf{q}}$ 的一个好方法，就是：构建优化目标函数 $f{=}_E^S{\hat{\mathbf{q}}}^{\ast }{\times }^E\hat{\mathbf{d}}{\times }_E^S\hat{\mathbf{q}}{-}^S\hat{\mathbf{s}}$，在 $f$ 取得最小值时的 ${}_E^S\hat{\mathbf{q}}$ 就是我们想要的结果。优化算法使用的是大名鼎鼎的梯度下降法，迭代公式如下：
   
  ${}_E^S{\mathbf{q}}_{k+1}{=}_E^S{\mathbf{q}}_k-\mu \frac{\nabla \mathbf{f}}{\Vert \nabla \mathbf{f}\Vert }$
   
  其中$\mu$是学习率，$\nabla \mathbf{f}$在论文中也给出了解析表达式，可以方便查阅。如果使用的是IMU，仅需要将其中的${}^E\hat{\mathbf{d}}$看成重力加速度$[0,0,0,1]$，${}^S\hat{\mathbf{s}}$就是加速度计的单位四元数。若使用的是MARG，在考虑重力加速度的基础上还需要将当地的准确地磁和地磁传感器的读出考虑进来。最终通过梯度下降的方法就能够得到${}_E^S{\mathbf{q}}_{\nabla ,t}$的结果。值得一提的是，学习率的选择上需要满足条件：
   
  ${\mu }_t=\alpha {\Vert }_E^S{\dot{\mathbf{q}}}_{\omega ,t}\Vert \Delta t$，其中$\alpha >1$
   
  上述式子的含义是${}_E^S{\mathbf{q}}_{\nabla ,t}$的收敛速度需要大于${}_E^S{\mathbf{q}}_{\omega ,t}$的发散速度。其中${}_E^S{\dot{\mathbf{q}}}_{\omega ,t}$就是${}_E^S{\mathbf{q}}_{\omega ,t}$的发散速度，而$\frac{{\mu }_t}{\Delta t}$则是通过梯度下降运算中的${}_E^S{\mathbf{q}}_{\nabla ,t}$的收敛速度。
   
• 通过互补滤波方法融合 ${}_E^S{\mathbf{q}}_{w,t}$ 和 ${}_E^S{\mathbf{q}}_{\nabla ,t}$ 的结果：
  互补滤波的方法非常简单，就是这两个部分的加权平均而已：
   
  ${}_E^S{\mathbf{q}}_{est,t}={{\gamma }_t}_E^S{\mathbf{q}}_{\nabla ,t}+(1-{\gamma }_t{)}_E^S{\mathbf{q}}_{w,t}$，其中$0<{\gamma }_t<1$
   
  如果假设 $\beta$ 是 ${}_E^S{\mathbf{q}}_{\omega ,t}$ 的发散速度，一般用陀螺仪的误差四元数导数的模来表示。如果 ${}_E^S{\mathbf{q}}_{\omega ,t}$ 的发散速度和 ${}_E^S{\mathbf{q}}_{\nabla ,t}$ 的收敛速度相等，则${\gamma }_t$有：
   
  $(1-{\gamma }_t)\beta ={\gamma }_t\frac{{\mu }_t}{\Delta t}$
   
  进而得到 ${\gamma }_t=\frac{\beta }{\frac{{\mu }_t}{\Delta t}+\beta }$
   
  假设 ${}_E^S{\mathbf{q}}_{\nabla }$ 的收敛速度突破了物理极限，则 $\alpha$ 会远远大于 ${\mu }_t$，这会带来一些近似的化简（很简单，这里不详细列出）。这些化简步骤将会进一步减少运算。最终我们得到的迭代公式如下：
   
  ${}_E^S{\mathbf{q}}_{est,t}{=}_E^S{\hat{\mathbf{q}}}_{est,t}+{(}_E^S{\dot{\mathbf{q}}}_{\omega ,t}-\beta \frac{\nabla \mathbf{f}}{\Vert \nabla \mathbf{f}\Vert })\Delta t$
   
• 地磁计数据畸变校正：
  地磁计的畸变产生是由于周围的电磁波的干扰。在传感器坐标系下静止的外界电磁干涉源可以通过地磁计的校准进行校正剔除。而在Earth坐标系下的干扰源将会主要产生地磁的测量偏差。在t时刻的地磁方向${}^E{\hat{\mathbf{h}}}_t$可以通过地磁计的读数${}^S{\hat{\mathbf{m}}}_t$计算：
   
  ${}^E{\hat{\mathbf{h}}}_t=[0,h_x,h_y,h_z]{=}_E^S{\hat{\mathbf{q}}}_{est,t-1}{\times }^S{\hat{\mathbf{m}}}_t{\times }_E^S{\hat{\mathbf{q}}}_{est,t-1}^{\ast }$
   
  最终参与位姿估计运算的地磁向量${}^E{\hat{\mathbf{b}}}_t$为：
   
  ${}^E{\hat{\mathbf{b}}}_t=[0,\sqrt{h_x^2+h_y^2},0,h_z]$
   
  这样的校准计算可以将外界的电磁干扰影响约束在对传感器指向的方向估计中。而且不需要预先给定地磁方向。
• 陀螺仪零点漂移校正：
  ${}_E^S{\dot{\hat{\mathbf{q}}}}_{ϵ}$代表了陀螺仪在各个轴的角误差，使用前文提到的${}_E^S{\dot{\mathbf{q}}}_{\omega ,t}={\frac{1}{2}}_E^S{\hat{\mathbf{q}}}_{est,t-1}{\times }^S{\mathbf{\omega }}_t$公式的逆运算可以得出陀螺仪当前度数的可能偏差：
   
  ${}^S{\mathbf{\omega }}_{ϵ,t}=2_E^S{\hat{\mathbf{q}}}_{est,t-1}^{\ast }{\times }_E^S{\dot{\hat{\mathbf{q}}}}_{ϵ,t}$
   
  那么陀螺仪随着时间的漂移量为：
   
  ${}^S{\mathbf{\omega }}_{b,t}=\zeta \sum {}^S{\mathbf{\omega }}_{ϵ,t}\Delta t$
   
  最后得到校正后的陀螺仪读数为：
   
  ${}^S{\mathbf{\omega }}_{c,t}{=}^S{\mathbf{\omega }}_t{-}^S{\mathbf{\omega }}_{b,t}$
   
• 算法的整体流程图：
  
• 论文结尾附有该算法在C中的实现，详情请查阅论文。