九轴MARG传感器梯度下降法

本文主要是对的Madgwick2010年发表的文章《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，旋转角为0。 四轴移动后一切都发生了变化。 在坐标变化中，欧拉角演变成旋转矩阵。 旋转矢量用四元数进行数学表达，可以在图中找到旋转轴与旋转角度的对应关系。 在计算下一步之前，需要了解四元数的运算。 范数： 乘法(不可逆): 在下图中，BvB系中的向量描述v，Av它描述了A系中的向量v, ABq指描述B系相对于A系运动的四元数，Av左乘四元数q右乘q的共轭即将从A系转移到B系，这里的四元数相当于矩阵R，如下图所示，矩阵R是通过推导获得的，具体过程可参考秦永元06版《惯性导航》。 上述矩阵R是我们之前获得的旋转矩阵。通过相应的关系，我们可以推三个欧拉角，即四轴的三个姿态角。

本文描述的四元数格式为： 我们可以根据陀螺仪的输出获得一组四元数，并将其标记为：（陀螺仪四元数） 根据加速度计和磁力计的梯度下降法，我们将其标记为：（梯度四元） 我们最后需要的四元数是： 接下来是两个四元数和参数γt进行分析。

我们有以下微分方程： 其中ωb输出陀螺仪，Qω陀螺仪四元数。 在进行代入计算时，可以写成以下形式： 这里Qω，t这一刻是陀螺仪的四元数，Qt-1是上一刻估计的四元数，根据上一刻估计的四元数和陀螺仪此时的数据更新陀螺仪此时的导数。 此外，陀螺仪此时的四元数可以用以下公式表示： 由于上一刻陀螺仪的四元数不准确，需要用上一刻校准的四元数和陀螺仪的四元数导数来更新陀螺仪的四元数。

在机体坐标系b系中，坐标轴随着机体姿势的变化而变化， 加速度计测量的值大致为重力值，加速度测量值可以用下表示。(已标准化) 在游移位坐标系p系中，重力加速度可以用上表示(已标准化)。 从p系到b系的姿态矩阵是： 将P系中的重力转换为B系，然后与加速度测量值差得到加速度计的误差，反映了四轴加速度（重力除外）对四元数更新的影响。

解决误差函数fg雅各比矩阵对四元数Q的偏导数Jg。 误差函数的梯度为：

和加速度计相同，磁力计所测量的值也是在机体坐标系中，测量的值可表示为： 在这里，我们介绍了一个默认的磁场参数，我们认为它的X轴总是朝北，所以我们可以得到： 根据磁力计的特点(可参考磁力计) 我们知道X轴和Y轴的磁力计数据矢量和方向总是指向北方，所以Y轴的数据是0。 关于bx和bz我们可以使用以下方法： 首先将磁力计数据从系统b系转换为游移系统p系(请注意，这里使用的是Cpb，而不是Cbp），用其X和Y作为的矢量和模值x值，Z直接赋予轴的值bz。 接下来的工作与加速度计相类似。 首先要求磁力计实际数据与我们假设的指北参数之间的误差函数fb，它反映了方位误差对四元数更新的影响。 得到误差函数后，再求雅各比矩阵，再求梯度(梯度先用公式表示，不具体计算)。

我们得到了加速度计、磁力计误差函数和雅各比矩阵，并将其放在一起计算，以获得整体梯度。 得到整体梯度，可以用梯度下降法校正四元数，得到梯度四元数。 可参考梯度下降法(梯度下降法) 我在这里的想法是：我们想让误差函数f(g，b)值降低，沿函数梯度方向增加f(g，b)因变量值，f(g，b)的值会增加，所以我们需要沿着误差函数f(g，b)梯度的相反方向是正确的f(g，b)因变量，即四元数Q迭代，迭代步长为μt，这一步可以显著降低误差函数的值，也就是说，在前一刻估计的四元数的基础上，减少了身体加速度和航向偏角对四元数更新的影响，获得了可信的梯度四元数。 在上式中μt以下公式表达最优值： 其中α为增益，减少加速度计和磁力计测量误差的影响。

陀螺仪有零点漂移现象，在温度变化时和运动时这样的现象加剧，为了补偿此漂移，我们需要用到dotQ(ω,t)。 根据式 我们可以引申得到 接下来我们对误差进行累加 然后进行补偿 这里的参数ε取决于陀螺仪的漂移系数 当我们得到梯度后会对陀螺仪的漂移值进行补偿，然后再进行计算。