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

最新推荐文章于 2023-07-14 23:57:04 发布

顾影_

最新推荐文章于 2023-07-14 23:57:04 发布

阅读量1.6k

点赞数

文章标签：四元数姿态解算梯度下降法四旋翼飞行器梯度下降法姿态解算

转载于：http://blog.csdn.net/nemol1990/article/details/23102643
笔者前面几篇文章讨论的是基于四元数的互补滤波算法，并单独对地磁计融合部分做了详细的讨论和解释。而本文讨论的姿态融合算法叫做梯度下降法，这部分代码可以参见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和列向量因变量[efx efy efz]’，你同样可以定义列向量四元数q和行向量因变量[efx efy efz]。但是注意在求导的时候，注意一下矩阵的行列对应，记住：行向量不能对行向量求导，列向量不能对列向量求导，只能交错进行，这一点一定要记住！因为后面在用梯度计算姿态的时候为了满足矩阵乘法法则需要转置雅可比矩阵。

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

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

从上述公式也可以看出梯度下降法属于一阶收敛，牛顿法在迭代公式中应用到了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的列向量：

根据梯度下降法的计算公式，假设我们经过梯度下降法计算的目标姿态为，那么有：

上述公式就是根据梯度下降算法求得的一组四元数，对梯度归一化是为了得到梯度的方向，其梯度大小由步长ut决定。这样的一个迭代公式显然属于一阶线性收敛，通过计算机的迭代运算可以从初始时刻的姿态每一步经由梯度的相反方向走到多元向量误差函数的一个局部极值点，这个点就是我们消除了旋转矩阵误差的点，就是我们所要求的姿态四元数。

PS一句，这里的步长ut和实际物体运动的角速度相关，和采样时间相关，具体关系为：

上述关系是Madgwick给出的，我只能理解物体运动角速度越快，采样时间越长，那么收敛的步长就必须越长，我觉得是显然的，至于为什么，我无法给出证明。Madgwick也只是一带而过，有想法的网友可以一起交流。

利用梯度下降法可以求得一组姿态四元数，这组姿态四元数是基于加速度计表征出来的四元数，由于加速度计的动态性能不佳，所以这种方法在高速运动之下会有较大的延迟。而在惯性导航中，主要的器件是陀螺仪，这个器件是专门处理高速运动下的较为理想器件。之前笔者研究的互补滤波姿态融合算法和目前绝大多数网友采用的姿态解算算法均是根据四元数微分方程得来。关于这个方程的解释不在本篇文章讨论范围之内。但是需要指出的是，根据这个微分方程求解得出的姿态和梯度下降法求解的姿态都是用于描述一个坐标系相对于另一个坐标系的空间位置。所以，为了充分两者的算法特点，需要对其进行融合。

假设由微分方程求解的姿态四元数为，有：

上面这个式子很和谐，因为我们所要求的最终姿态在等式左边，等式右边由两个部分组成，一个是基于陀螺仪的四元数微分方程求解的姿态，另一个是用梯度下降法求解的姿态四元数，两者都有一定的可信度，前者用于高速运动，后者用于低速运动。

在这里就存在一个很明显的问题，即系数alpha如何取？

很明显，高速运动下alpha要小一些，低速运动下alpha要大一些。也就是说理论上将这应该是一个动态变化的值。至于这个动态值应该怎么取舍就交给那些大牛们去研究吧，我这里就采用常量简单处理了。在常量值处理的前提下，上述公式取得最优值的alpha应该是的收敛速度等于的收敛速度（Madgwick如是说，有网友知道为什么可以和我讨论），所以就有如下公式：

式中的beta就是四元数微分方程求解的姿态算法的收敛速度，这个值就是陀螺仪的测量误差，通过查询IMU器件的PDF手册就可以知道，一般是一个很小的值。等式右边就是梯度下降算法的收敛速度，进而有：

在分析上述公式之前，在这里先明确一点，就是梯度下降算法的收敛速度和物体运动的速度关系，只有当算法的收敛速度快于物体运动速度，该算法才可以实时跟踪到姿态，如果物体运动太快，那么该算法会失效。（说到这里，真心有必要采用动态的步长算法啊！）所以在这里，我们让alpha尽可能大，用于跟踪物体高速运动（不能太大，太大了会导致静态性能差）。那么此时就比较大，由于beta比较小，所以上式就近似为：

Beta本来就小，采样时间又非常小（就是控制周期，大伙一般为2~4ms）。所以近乎为0。

由于alpha假设为比较大的值，那么对于梯度下降法而言：

该公式右边的就占有绝大比分比例，以至于上次姿态的可以忽略不计，也就是说直接由反梯度方向的迭代值就直接可以达到目标姿态（至于为什么，笔者在这里也不是清楚，只能这么解释，关于这点，Madgwick在论文中该部分我看的不是很明白，希望有人可以指导一下）。基于这个假设，有：

现在，将这些公式全部代入到，其中四元数微分方程未给出：

可以得到最终的梯度下降法的姿态融合算法公式：

上述公式的实现流程图：

写到这里，陀螺仪和加速度计的梯度下降法姿态融合算法就解说完毕。

接下来在这个基础之上，进一步解释融合地磁计的梯度下降算法。其实地磁计的融合算法与先前讨论的加速度计融合算法如出一辙。如果单独用地磁计进行梯度下降融合算法，那么和加速度计是一模一样的。现在是两者的融合，所以原先代表加速度计的3X1的变成了现在的6X1的

上式公式中的后3列地磁计部分的由来我就不做详细推导了，这部分内容详见笔者的另一篇文章《四元数姿态解算中的地磁计融合解读》。那么同加速度计处理一样，对上式中的地磁计部分单独求导，得到雅可比矩阵

下面的工作就是得到有地磁融合的梯度计算式：

很明显，公式左边必须满足4X1的矩阵，表示四元数的梯度修正增量。我们现在来看等式右边。根据矩阵乘法的运算法则，首先要满足的条件便是(4X?)x(?X1)=4X1。中间的问号就是我们确定的值。在这里，等式最右边很明显是6X1，这也就是说必须是一个4X6的矩阵。很明显，是一个融合了加速度计和地磁计的雅可比矩阵：

笔者这里就不展开了，自行代入前述公式不难发现这是一个6X4的矩阵，所以，转置一下就可以代入地磁融合的梯度计算公式中。最后可以计算出，这是一个非常长的公式，笔者在A4草稿纸上硬是写不下啊！具体代码稍后给出。地磁计融合的梯度下降算法框图：

最后PS：这里地磁计的XOY平面处理方式我在前一篇文章中已经做了讨论，不明白的可以参考《四元数姿态解算中的地磁计融合解读》。

写到这里，整个梯度下降法的推导和解说就结束了。这些算法都是基于Madgwick的《An efficientorientation filter for inertial and inertial/magneticsensor arrays》这是未发表的详细版，而正式发表则是《Estimation of IMU and MARG orientation usinga gradient descent algorithm》，在IEEE 国际会议的Rehabilitation Robotics上发表的论文，这篇论文去除了之前对于陀螺仪的积分补偿，这部分笔者没做研究，不讨论。

最后说一下这个算法的优缺点。

优点：

1、当步长较小时(0.03左右)三个姿态静态值非常稳定，慢速运动下可以达到较高精度的要求；

2、步长较大时，可以快速的跟随高速运动下的姿态变化；

3、巧妙避免了地磁计补偿需要知道当地磁场的角度。

缺点：

1、步长非动态，系统可相应频带有点窄；

2、地磁计在使用之前必须经过较为精准的Hard补偿和Soft补偿。参见《COMPENSATING FOR TILT, HARD IRONAND SOFT IRON EFFECTS》，否则这个误差会对Pitch和Roll产生干扰，导致姿态精度降低。

3、高速运动时，偏航角响应有严重延迟，导致全姿态错误，必须增加步长来解决，但是静态性能就会降低。

附上x-IMU应用Madgwick的代码：

下面是用梯度下降法融合的陀螺仪和加速度计：

[csharp] view plain copy

print ?

void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}

void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

        // Normalise accelerometer measurement
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;   

        // Auxiliary variables to avoid repeated arithmetic
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _4q0 = 4.0f * q0;
        _4q1 = 4.0f * q1;
        _4q2 = 4.0f * q2;
        _8q1 = 8.0f * q1;
        _8q2 = 8.0f * q2;
        q0q0 = q0 * q0;
        q1q1 = q1 * q1;
        q2q2 = q2 * q2;
        q3q3 = q3 * q3;

        // Gradient decent algorithm corrective step
        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
        s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
        s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;

        // Apply feedback step
        qDot1 -= beta * s0;
        qDot2 -= beta * s1;
        qDot3 -= beta * s2;
        qDot4 -= beta * s3;
    }

    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * (1.0f / sampleFreq);
    q1 += qDot2 * (1.0f / sampleFreq);
    q2 += qDot3 * (1.0f / sampleFreq);
    q3 += qDot4 * (1.0f / sampleFreq);

    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

下面是用梯度下降法融合的加速度计和地磁计：

[cpp] view plain copy

print ?

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth’s magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float hx, hy;
    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
        return;
    }

    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

        // Normalise accelerometer measurement
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;   

        // Normalise magnetometer measurement
        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
        mx *= recipNorm;
        my *= recipNorm;
        mz *= recipNorm;

        // Auxiliary variables to avoid repeated arithmetic
        _2q0mx = 2.0f * q0 * mx;
        _2q0my = 2.0f * q0 * my;
        _2q0mz = 2.0f * q0 * mz;
        _2q1mx = 2.0f * q1 * mx;
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _2q0q2 = 2.0f * q0 * q2;
        _2q2q3 = 2.0f * q2 * q3;
        q0q0 = q0 * q0;
        q0q1 = q0 * q1;
        q0q2 = q0 * q2;
        q0q3 = q0 * q3;
        q1q1 = q1 * q1;
        q1q2 = q1 * q2;
        q1q3 = q1 * q3;
        q2q2 = q2 * q2;
        q2q3 = q2 * q3;
        q3q3 = q3 * q3;

        // Reference direction of Earth's magnetic field
        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
        _2bx = sqrt(hx * hx + hy * hy);
        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
        _4bx = 2.0f * _2bx;
        _4bz = 2.0f * _2bz;

        // Gradient decent algorithm corrective step
        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;

        // Apply feedback step
        qDot1 -= beta * s0;
        qDot2 -= beta * s1;
        qDot3 -= beta * s2;
        qDot4 -= beta * s3;
    }

    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * (1.0f / sampleFreq);
    q1 += qDot2 * (1.0f / sampleFreq);
    q2 += qDot3 * (1.0f / sampleFreq);
    q3 += qDot4 * (1.0f / sampleFreq);

    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

                </div>

顾影_

关注

0
点赞
踩
8

收藏

觉得还不错? 一键收藏
2
评论
四元数姿态的梯度下降法推导和解读

转载于：http://blog.csdn.net/nemol1990/article/details/23102643 笔者前面几篇文章讨论的是基于四元数的互补滤波算法，并单独对地磁计融合部分做了详细的讨论和解释。而本文讨论的姿态融合算法叫做梯度下降法，这部分代码可以参见Sebastian O.H. Madgwick在2010年4月发表的一篇论文（An efficient orientat...
复制链接

扫一扫