四元数姿态的梯度下降法推导和解读

四元数姿态的梯度下降法推导和解读

笔者前面几篇文章讨论的是基于四元数的互补滤波算法，并单独对地磁计融合部分做了详细的讨论和解释。而本文讨论的姿态融合算法叫做梯度下降法，这部分代码可以参见Sebastian O.H. Madgwick在2010年4月发表的一篇论文（An efficient orientation filter for inertial andinertial/magneticsensor arrays），这篇论文利用四元数微分方程求解当前姿态，然后分别利用加速度计和地磁计进行补偿，推导出两种姿态融合算法。两种算法均为梯度下降法，而其中地磁计的处理方式笔者已经在《四元数姿态解算中的地磁计融合解读》一文中详细讨论了，这里笔者将对Madgwick对于加速度计和地磁计的梯度下降法做出详细的解释，期间一定有个人不足的地方，仅供参考，希望和各位网友一起学习！

首先来谈谈什么是梯度。维基百科中解释的是“标量场中某一点上的梯度指向标量场增长最快的方向，梯度的长度是这个最大的变化率。”很显然，梯度和变化率有关。现在我们引入标量函数f(x)，对标量函数f(x)求导，不难得到f’(x)就是梯度，就是曲线在某一点的斜率。梯度下降法就是我们顺着这个在某一点下降速度最快的反方向一直走，走到一个极值点，这个点就是最优解（稳定解）。

那么这个梯度的概念和我们的姿态解算有什么关系？

我在前面的文章中已经说明：我们求解姿态就是求解的转换矩阵（矩阵元素就是四元数）。这个转换矩阵是有误差的，我们所要做的工作就是采用某种算法，消除误差，最后得到的解就是我们的近似精确解，也就是姿态四元数了。消除误差四个字，在实际的实现过程中，是通过误差函数来实现的。定义误差函数ef(x)，那么我们的工作就是令ef(x)=0，求解上述方程得到x的值。我们在求解高阶方程的时候，一般的方法就是求导，求极值点，根据这些值来判断精确值个数和位置。这是我们高中所学习到的知识，在这里是一样的。只不过，这里的误差函数ef(x) 不再是之前讨论的简单的标量函数了，他的自变量x变成了向量[q0 q1 q2 q3]。这也就是说，原先的标量函数ef(q) 变成了如今的标量函数ef([q0 q1 q2 q3]) ，他仍然是标量函数，但是自变量是向量[q0 q1 q2 q3]。

对上述自变量是向量的标量函数，我们要用梯度法求解，就必须求导。标量函数对向量求导很简单，只需要分别对向量中的各个变量求偏导即可：

但是，我们的姿态解算是三维姿态，不是一维姿态，所以，这里的ef(q)并不是一个标量函数，实质上是一个向量函数ef ( q )，这个向量函数里面有三个元素，分别对应xyz轴的三个分量，每个分量又由一个四元数向量q构成。那么现在就引入了一个较为复杂的误差函数ef ( q )，该误差函数不光自变量是一个向量，并且因变量也是一个向量，这种函数叫做多元向量函数。那么我们现在的问题就转化为求多元向量函数的极值问题。

针对上述极值问题，在计算机中，多采用数值解法，如最速下降法、牛顿法、共轭梯度法。我们这里讨论就是第一种算法，又叫做梯度下降法。PS：梯

度下降法为一阶线性收敛，牛顿法为二阶线性收敛，所以从收敛速度和精度来讲牛顿法要好，但是需要计算Hessian矩阵，计算量大，笔者在这里就不细说了。（或许今后笔者会尝试牛顿法求解姿态）

在梯度下降法中，需要对多元向量函数求导，当然，前人已经研究过这个问题了，美其名曰雅可比矩阵（Jacobian）。不要被他吓到，雅可比矩阵就是按照规矩来一个个求导的过程。上述讨论了标量函数对向量的导数的情况，当标量函数变为向量时，只需要继续对每一个向量再求偏导即可。对于上式（假设四元数向量定义为行向量），再对每一个因变量求偏导：

上式就是多元向量函数的导数（也称为雅可比矩阵）。在这里，我定义的是行向量的四元数q和列向量因变量[efxefyefz]’，你同样可以定义列向量四元数q和行向量因变量[efxefyefz]。但是注意在求导的时候，注意一下矩阵的行列对应，记住：行向量不能对行向量求导，列向量不能对列向量求导，只能交错进行，这一点一定要记住！因为后面在用梯度计算姿态的时候为了满足矩阵乘法法则需要转置雅可比矩阵。

这里，我们由引例考虑实际情况。现在考虑陀螺仪和加速度计的梯度下降法姿态融合算法。

先给出梯度下降法计算公式：

从上述公式也可以看出梯度下降法属于一阶收敛，牛顿法在迭代公式中应

用到了f(x)的导数，所以属于二阶收敛。对于上述公式的理解，就是在处的梯度（导数），前面的负号表示梯度的反方向，这个方向就是当前最快的下降收敛速度，我们按照这个方向走步长，就从走到了。在这里，我们取函

数的方向，即。根据前述讨论，这里的函数就是误差函数，并且是

多元向量函数，自变量是四元数向量，因变量是三维坐标。由于是利用加速度计表征的四元数矩阵误差，所以该多元向量误差函数记作

上述公式的得来其实很简单，就是重力加速度g在地理坐标系中的值通过四元数法旋转到载体坐标系中的值，也就是旋转矩阵的第三列，然后减去当前加速度计测得的值作差，就是通过加速度计表征的旋转矩阵的误差。很显然，这个误差函数就是典型的多元向量函数，当这个函数等于0时，我们就认为得到了旋转矩阵没有误差，也就是姿态四元数是精确的，从而得到了飞行器的精确姿态。

为了求解上述方程，我们对其求导，根据前述阐述，上述函数的导数就是其对应的雅可比矩阵：

注意，上式中的自变量四元数为行向量，因变量三维向量是列向量。进行到这里，还没有应用到梯度下降算法，只是做好了前提准备。为了更好的理解梯度算法的原理，这里从标量函数梯度开始看起。设标量函数f(x)，导数有f’(x)，对于x上的一个增量dx，有dy=f’(x)dx。当dy->0时，我们认为已经到达稳定解，梯

度法计算完成。同理，x变为向量四元数，y变为三维向量，那么f’(x)变为雅可比矩阵，有梯度计算公式：

在这里要特别注意公式中的雅可比矩阵转置符号T 。前面红色部分说过对于多元向量函数求导涉及到分子和分母的行列向量问题，在这里，为了满足矩阵乘法的要求，如果我们不对雅可比矩阵进行转置，那么等式右边就是(3x4)X(3x1)，显然不满足矩阵乘法要求，所以，在这里对之前求出来的雅可比矩阵转置。得到一个(4x3)X(3x1)=4X1的列向量：