Madgwick算法详细解读

Madgwick算法详细解读

极品巧克力

前言

接上一篇文章《Google Cardboard的九轴融合算法》。

Madgwick算法是另一种九轴融合的方法，普遍应用在旋翼飞行器上，效果也蛮不错的。网上已经有不少madgwick算法的源代码了。

本文结合参考文献，按照我本身的理解，把Madgwick算法的前因后果讲清楚。

1.加权融合

因为姿态都是相对的，用来表示水平朝北静止时的物体，表明运动后的物体。因此，运动时的物体相对水平朝北静止时的物体的姿态能够用四元数来表示。

主要基于Madgwick算法对传感器数据进行融合。Madgwick算法的本质是加权整合t时刻的陀螺仪算出的姿态和加速度计磁场计共同算出的姿态，从而获得最终的姿态。其加权公式以下。

其中，和是加权系数，它们是由各自的偏差占整体偏差的比重所决定的，偏差所占的比重越小则加权系数越大。设采样时间间隔为。陀螺仪的单位时间的偏差能够经过查陀螺仪的手册获得，通常是一个很小的值，因此陀螺仪的偏差为。而加速度计磁场计共同算出的姿态的偏差是由计算方法决定的，计算方法如梯度降低法、高斯牛顿迭代法、牛顿法、共轭梯度法等，因为采用的方法是梯度降低法，因此其偏差为梯度降低法中所选取的步长，步长越长则其计算结果的偏差越大。因此，整体偏差为。

是陀螺仪算出的姿态的加权系数。

是加速度计磁场计所算出的姿态的加权系数。

因而，接下来须要获得的是陀螺仪计算出的姿态和加速度计磁场计共同算出的姿态。

2.陀螺仪姿态估计

用陀螺仪的数据来计算出姿态。

三轴陀螺仪返回的数据是自身分别绕轴、轴、轴的角速度，这三个角速度分别用、、来表示。则陀螺仪返回的数据能够当作实部为零的四元数，用来表示。

而姿态四元数变化的速度与当前的姿态和角速度有关，其计算公式以下。

如今已知时刻的四元数和角速度，以及时刻的角速度，系统采样间隔为，求时刻的四元数。相似于常微分方程，套用改进的欧拉公式。

这里用改进的欧拉方法，其实就是近似估计物体四元数在时刻到时刻之间的平均变化速度。而在实际过程当中，每每并不能获得时刻的准确的四元数，而是一个最优的估计值。

从而获得，

另外，在实际过程当中，受传感器特性的影响，陀螺仪、加速度计、磁场计的最大采样速度不同。系统采样间隔是为了知足这三个传感器中最低的采样速度，使得能同时采样。而实际上，陀螺仪的采样速度能够比另外两个传感器的速度快不少。因此，若是为了追求更高的精度的话，能够分开采样。假设陀螺仪的采样间隔是，则可用代替上面公式中的，用欧拉公式一步一步地日后计算，重复次，直到，就获得了高精度的下一个系统采样时刻的四元数，而且这个四元数与另外两个传感器同步。

3.加速度计磁场计姿态估计

要用加速度计和磁场计来共同算出姿态。

当物体水平静止朝北时，加速度计的理论输出和磁场计的理论输出分别以下，均可以当作是实部为0的四元数。

将和的虚部组成一个向量。

而在物体运动以后，在时刻，上述的两个传感器的数据都发生了变化。加速度传感器的归一化后的数据和磁场计的归一化后的数据分别以下，均可以当作是实部为0的四元数。

将和的虚部组成一个向量。

设这个时候的物体的姿态为，则按照四元数的矢量旋转性质可创建和的关系，和的关系，获得以下方程。

分别用旋转矩阵表示。

将组合成一个新的矩阵。

因而，和的关系，用矩阵来表示。

在上式中，和是已知的，因此能够由上式再反过来去求出。即求出一个，，由此转换成，使得偏差平方和最小。

其中，是一个多元向量函数，而求多元向量函数的极值问题，在计算机中通常采用数值解法，如梯度降低法、高斯牛顿迭代法、牛顿法、共轭梯度法等。采用梯度降低法。

设的初值为，为4行1列的矩阵，则偏差平方和为。假设这个初值四元数须要修正的量为，则修正后的偏差平方和为。

由泰勒公式展开可得，

其中，为4行1列的矩阵。为1行4列的矩阵，以下所示。

则梯度大小以下。

其中，为向量与单位向量的夹角。因此，当为时，上式有最小值，即偏差平方和降得最快。

若要让为，则要让上述两个向量的方向相同，即要知足以下的等式关系。

其中，为一个大于零的比例系数。上式中的关于四元数的偏导，能够计算以下。

其中，表明中第行第列的元素。的表达式以下。

而就是关于四元数的偏导，即其雅克比矩阵。

因此， 的计算能够转换成，

进行归一化，除以其2范数，获得梯度的方向，表达式以下。

而后再乘以步长，就获得了各个自变量要改变的值。各个自变量现有的值加上要修正的量，获得了新的对的估计，以下面公式所示。

梯度降低法是一个不停迭代的过程。用上一时刻的姿态做为初值，即已知上一次的估计，代入上述公式中。重复以前的步骤，进行屡次迭代，直到小于某一阈值，此时，为的最佳估计值。或者通过屡次迭代，直到知足，此时，为的最佳估计值。

步长须要经过人为来设定。步长越小，则最终结果的精度越高，但迭代的次数也会越多。步长越大，则最终结果的偏差越大，但迭代的次数也会越少。

用来表示最终经过梯度降低法融合加速度计和磁场计来共同估计出的姿态，即上述的最佳估计值。

4.算法简化

由前面的陀螺仪积分的结果和加速度计磁场计优化的结果加权，就能够获得高精度的融合结果。

可是，在实际工程中，须要权衡计算精度和计算速度。在梯度降低法的迭代过程，虽然以前的方法可使得结果的精度更高，可是也增长了计算量。在实际工程中，若是要追求速度的话，能够对这些地方进行简化。

在时刻到时刻之间的变化速度能够用以下公式近似。

因此，的计算公式也可简化以下。

而梯度降低法的迭代过程，也能够只用一步来简化，即认为一步就能够近似达到最佳估计值。那就是要设置步长，使得迭代一次就能最接近最佳估计值。

其中，是一个根据实际状况调节的量，用来弥补加速度计和磁场计的测量偏差。上式中的，若是要追求更高的精度，仍可以使用以前的计算公式。若是追求速度，采用简化公式，因此，简化后的计算公式以下。

最终，将和加权融合，获得最佳的姿态估计。

其中，。在上式的分母中，因为，因此，因而上式能够进一步简化成，

至关因而陀螺仪计算出来的四元数变化速度与加速度计磁场计计算出来的变化速度加权整合，前者的权重是1，后者的权重是。

而后，对进行归一化，获得。

5.磁场计修正

因为物体是在不停地运动之中的，物体周围的磁场容易受环境变化影响，即水平静止朝北时的磁场计理论输出可能会因为环境的变化而发生改变。而水平静止朝北时的加速度计理论输出则几乎不受环境影响。因此，为了能获得一个更加精确的姿态估计，不能像那样采用一个固定值，而须要实时修正。

在传感器刚开始运行的时候，即第一帧的时候，传感器可能处于任意一种姿态，几乎不会是水平静止朝北的，因此磁场计的输出几乎不会是。因此，是未知的。而这时候，能够用加速度计的输出来计算出物体的姿态。

用旋转矩阵表示。

在上式中，和是已知的，因此能够由上式再反过来去求出第一帧的姿态。即求出一个，转换成，使得最小。

用高斯牛顿迭代法来寻找这个最佳的四元数。先计算其雅克比矩阵，

假设当前四元数各个元素的偏差为4行1列的矩阵，则。用最小二乘法来计算出。

因此，现有的四元数的值减去偏差，获得新的四元数。

的初值能够设为以下。

重复上述公式，迭代屡次，直到达到最小值。

因而就获得加速度计估计出的第一帧的姿态。

因此，根据四元数的坐标系旋转性质，能够把坐标系转到水平的位置上，但并不能保证朝北。对于向量来说，坐标系逆着四元数转回去，就至关因而向量顺着四元数继续转，获得在这个水平坐标系中的磁场的向量。

其中，和是在这个坐标系中的轴和轴上的份量，因此能够获得。

以上是第一帧的时候获得的方法。

再将加速度计估计出来的姿态做为初值，将、、、代入到以前公式中，用梯度降低法迭代，获得高精度的第一帧的姿态。

当物体发生运动以后，因为周围环境的影响，每一帧都要对进行修正。假设已经用以前的方法获得上一帧的最佳的姿态估计，则这一帧的计算以下。

6.求赞扬

您以为，本文值多少？ 

7.参考文献

1. Madgwick S O H, Harrison A J L, Vaidyanathan R. Estimation of IMU and MARG orientation using a gradient descent algorithm[C]//Rehabilitation Robotics (ICORR), 2011 IEEE International Conference on. IEEE, 2011: 1-7.