九轴MARG传感器梯度下降法

介绍

本文主要是对Madgwick在2010年发表的文章《An efficient orientation filter for inertial and inertial/magnetic sensor arrays》进行分析，讨论Madgwick所提出的梯度下降法在四轴姿态求解四元数更新中的实用性。

四元数求解姿态角

在空间中，一个刚体由一个位置到另一个位置的表现形式常用的有两种。
一种是欧拉角，将刚体分别绕负Z轴旋转航向角φ角，再绕正X轴旋转滚转角θ角，最后绕正Y轴旋转俯仰角γ角，可以与目的地位置的刚体重合。


另一种是旋转矢量表示法，在空间中可以找到一根轴r，沿着这根轴，刚体锥面旋转θ角，与目的地位置的刚体重合。

设想一下，刚体在整个过程中不动，动的是刚体所在的坐标系，即在刚体所在的坐标系中，刚体是保持静止的，刚体所在坐标系相对于惯性系有运动，因此在惯性系中看来，刚体的坐标也发生了变化。对应于四轴飞行器，其所在坐标系被称为机体坐标系
b系（之后简称b系），而惯性系被称为游移方位坐标系p系（之后简称p系，若为指北方位，则p系与导航坐标系重合，若为游移方位，则与导航系有个游移角偏差），飞行器姿态的变化可以看作是b系相对于p系的运动。
与刚体运动表现形式相同，坐标系的运动也一样，初始时，b系与p系保持一致，即四轴在b系与p系的坐标是相同的，用欧拉角来说就是航向角、滚转角和俯仰角均为0，用旋转矢量来说是旋转轴可任取，旋转角度为0。
一切在四轴动起来之后发生变化。
欧拉角在坐标变化中演变成了旋转矩阵。

旋转矢量用四元数来进行数学表达，可在图中找到旋转轴与旋转角度的对应关系。

在进行下一步的计算前需要对四元数的运算有所了解。
范数：

乘法（不可逆）：

在下面的图中，Bv是描述在B系中的向量v，Av是描述在A系中的向量v, ABq是指描述B系相对于A系运动的四元数，Av左乘四元数q右乘q的共轭即将向量v所在的坐标系由A系转到了B系，四元数在这里的作用相当于矩阵R，如下图，矩阵R是经过推导所得，具体过程可参考秦永元06版《惯性导航》。

上述矩阵R就是我们前面所得到的旋转矩阵，通过对应关系我们可以推得三个欧拉角，即四轴的三个姿态角。

梯度下降算法

滤波融合算法原理

本文所描述的四元数格式为：

依据陀螺仪的输出可以求得一组四元数，我们把它标记为：（陀螺仪四元数）

依据加速度计和磁力计可用梯度下降法得到一组四元数，我们把它标记为：（梯度四元数）

我们最后需要的四元数为：

接下来分别对两个四元数和参数γt进行分析。

陀螺仪四元数

根据推导我们有如下微分方程：


其中ωb为陀螺仪的输出，Qω为陀螺仪四元数。
当我们在进行代入计算时，可以写成以下形式：

这里Qω，t为陀螺仪此时刻的四元数，Qt-1为上一时刻的估计四元数，根据上一时刻的估计四元数和陀螺仪此时刻的数据来更新陀螺仪此时刻的导数。
另外，陀螺仪此时刻的四元数可以用下面的式子来表示：

由于上一时刻的陀螺仪四元数不是精确的，需要用上一时刻校准好的估计四元数和陀螺仪四元数导数来更新陀螺仪四元数。

梯度四元数

加速度计

加速度计所测量的结果在机体坐标系b系中，坐标轴随着机体的姿态发生着变化，
加速度计所测的大致为重力的值，加速度测量值可用下式表示。（已进行标准化）

在游移方位坐标系p系中，重力加速度可用上式表示（已进行标准化）。

从p系转换到b系的姿态矩阵为：

将p系中的重力转换到b系中来，再与加速度测量值求差，得到的是加速度计的误差，体现的是四轴的加速度（除重力外的加速度）对于四元数更新的影响。

求解误差函数fg对四元数Q的偏导数，得到雅各比矩阵Jg。（fgnb！jgnb！rua！）

误差函数的梯度为：

磁力计

和加速度计相同，磁力计所测量的值也是在机体坐标系中，测量的值可表示为：

这里我们引入一个默认的磁场参数，我们认为其X轴正向始终朝向北方，那么可以得到：

根据磁力计的特点（可参考磁力计) 我们知道X轴与Y轴的磁力计数据矢量和的方向始终指向北方，那么在这里Y轴的数据就为0。
关于bx和bz的取值问题，我们可以用以下方法：

先将磁力计的数据从机体系b系转换到游移系p系中（注意这里用到的是Cpb，而不是Cbp），用其X和Y轴的矢量和模值作为b的x值，Z轴的数值直接赋给bz。
接下来的工作与加速度计相类似。
首先求磁力计实际数据与我们假设的指北参数之间的误差函数fb，它体现的是方位误差对四元数更新的影响。

得到误差函数之后再求雅各比矩阵，再求梯度（这里梯度先用公式表示，不具体算出来了）。

综合

我们分别得到了加速度计和磁力计误差函数及其雅各比矩阵，将其放到一块进行计算，得到总体的梯度。

得到总体梯度，可以用梯度下降法对四元数进行校正，得到的是梯度四元数。

梯度下降法可以参考（梯度下降法）
在此处我的想法是：我们要让误差函数f(g，b)的值减小，沿着函数梯度的方向增加f(g，b)的因变量值，f(g，b)的值会增加，所以我们需要沿着误差函数f(g，b)的梯度的反方向对f(g，b)的因变量，也即四元数Q进行迭代，迭代的步长为μt，这一步骤明显能使误差函数的值减小，也就是减小机体本身加速度和航向偏角对四元数更新的影响，这里是在前一时刻所估计的四元数的基础上进行迭代，得到了可信的梯度四元数。
在上式中μt的最优取值可用下式来表达：

其中α为增益，为了减小加速度计和磁力计测量误差带来的影响。

四元数融合参数γt取值与四元数更新

我们来回顾一下之前的原理式：

根据Madgwick在论文中所说，当陀螺仪四元数的收敛速度和梯度四元数的收敛速度与它们前面的权重相当时，γt的取值最佳。
有下式：

其中β为陀螺仪四元数的收敛速度，μt/∆t为梯度四元数的收敛速度。
对上式进行分析：

对于

而言，当增益α很大时，μt的值也很大，那么Qt-1的值在迭代值面前就微不足道了，于是对于梯度四元数而言有如下过程：


另外我们在前面又提到过

所以有

上面是原文中的方法，我觉得中间的简化过程有些复杂并且在逻辑上有些说不通，所以我自己总结了一下，下面是我的理解：

上式可以简化为：

所以我们可以得到：

其中dotQt是估计的方向改变率，也是最后我们用到的值。
我们在得到了dotQt之后可以直接用前面的式子求解Qt，也可以利用微分方程

来求解Qt（采用二阶毕卡求解或者龙格库塔法），这个可以实际对比一下两种方式的误差。
最后我们需要讨论的是β的取值问题，论文中提到β的取值与陀螺仪的随机误差有关，即陀螺仪传感器中给出的随机误差范围。
有下式

陀螺仪漂移补偿

陀螺仪有零点漂移现象，在温度变化时和运动时这样的现象加剧，为了补偿此漂移，我们需要用到dotQ(ω,t)。
根据式

我们可以引申得到

接下来我们对误差进行累加

然后进行补偿

这里的参数ε取决于陀螺仪的漂移系数

当我们得到梯度后会对陀螺仪的漂移值进行补偿，然后再进行计算。

流程图

论文代码