四旋翼姿态解算——梯度下降法理论推导-CSDN博客

在前面的博文中介绍了使用互补滤波法进行姿态解算的原理以及程序，点击打开链接。
在互补滤波法中，我们使用加速度计来补偿陀螺仪，将两者的数据融合解算出四元数下的姿态。
在梯度下降法中，依然是通过加速度计的数据来与陀螺仪的数据进行融合，而不同点是：互补滤波法中，先求出载体坐标系b下对加速度做叉积求出误差，补偿给陀螺仪以校正误差；梯度下降法中，通过加速度计的数据求出一组四元数这里写图片描述，然后通过四元数的微分方程，并使用陀螺仪得到的数据求解出另外一组四元数。因为在高速运动状态下，陀螺仪数据更可靠；而在低速运动状态下，加速度计数据更可靠。所以两组四元数分别乘以权重再相加，就得到了期望的输出结果。

大概地介绍了下之后，再讲讲梯度：
在向量微积分中，标量场的梯度是一个向量场。标量场中某一点上的梯度指向标量场增长最快的方向，梯度的长度是这个最大的变化率。
在单变量的实值函数的情况，梯度只是导数，或者，对于一个线性函数，也就是线的斜率。（摘自百度百科：点击打开链接）
我的理解是：一个多元函数在某点上的梯度就是在该点上最大的变化率，其方向就是函数值增大的最陡的方向。
举个例子来看看：（图画的比较难看，请见谅）
这里写图片描述
如上图所示，红线表示的是一个标量函数，黑线是图中标注点上关于该函数的切线。很容易得到，在该点上的梯度与切线方向相同。
如果我们想要求得函数的极小值，首先选取函数曲线上的任意一个点，那么，沿着梯度的反方向走一个比较小的步长这里写图片描述，得到一个新的点。再在这个新的点上重复之前的步骤，最后求解出极小值。
接着给出梯度下降法的迭代公式：

式中：，这里写图片描述表示运算前后自变量x的值；表示梯度下降的步长；表示函数在自变量x等于时，函数的梯度；负号表示沿着梯度的负方向逼近极点。

梯度下降法就是，在某一点增加最快的方向的反方向（下降最快）一直走，当走到某一个极值点（极小值）时，这个点就是最优解。

接下来开始使用梯度下降法求解姿态的算法介绍：
假设一个误差函数这里写图片描述，我们希望其满足：，即误差为0。接着，我们要求解这个方程，得到x的值。此时x值对应最优解。
使用梯度下降法求解，首先我们就需要先求出它的导数。
将x换成四元数。函数也相应变化，其导数变为如下形式：

因为我们求解的姿态是三维姿态，而上式的因变量只对应了一维的情况。所以将因变量推广到xyz轴下，变成一个多元向量函数：

求它的导数：

（也叫作雅克比矩阵）

准备工作完成了，接下来结合算法流程图来推导算法：
这里写图片描述
取定导航坐标系n中标准重力加速度g，定义为，从导航坐标系n转换到载体坐标系b下，乘以旋转矩阵。
这里直接给出旋转矩阵，推导步骤在前面的博文中，不再赘述。

上式代入这里写图片描述，推导出：

将这里写图片描述与归一化之后的加速度计数据作差。
得到误差函数：

当这个误差函数为0（即最小值）时，我们认为旋转矩阵没有误差，姿态是精确的。
那么，接下来的任务就是使用前面介绍的梯度下降法相关公式来求解了。
这里写图片描述
用前面阐述的方法求这个多元向量函数的导数（即它的雅克比矩阵）：

代入公式求出梯度：

由于线性代数中的矩阵乘法法则知：两个矩阵相乘行数与列数应当相同。所以将雅克比矩阵进行转置再与原函数相乘，得到梯度这里写图片描述。
将得到的梯度值再套入公式，累加，最后会趋近于最小值0，而收敛速度取决于步长：

通过离散叠加替代积分可以求解出一组四元数这里写图片描述，将其乘以权重与陀螺仪数据通过四元数微分方程求解得到的另一组四元数（这部分在前面的其他博文中已经推导过）。（注：这一部分只是简单处理了一下，与框图中的不同）
最终的公式：

为最终输出的四元数；
为权重；
为加速度计的数据通过梯度下降法得到的四元数；
为陀螺仪的数据通过四元数微分方程求解得到的四元数。

就介绍到这了，后面这段Madgwick的代码网上也有很多地方能下载到。其中用到的理论和公式在前面的介绍中也大致过了一遍，主要使用的也就是这个公式：

步长这里写图片描述和权重在程序中仅仅是简单处理了一下，直接赋予了一个常量。
然而实际中权重与运动速度有关：

高速运动下，相信陀螺仪更多一些，所以权重要小一点；
低速运动下，相信加速度计更多一些，所以权重要大一点。
若要取得最佳的权重，应当是与的收敛速度相等时的值。

步长也与物体运动的角速度、采样时间等相关。

关于如何取值，取多大合适，这个我也不清楚。

好了，不说废话，上代码：

//=====================================================================================================
// MadgwickAHRS.c
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date         Author          Notes
// 29/09/2011   SOH Madgwick    Initial release
// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
// 19/02/2012   SOH Madgwick    Magnetometer measurement is normalised
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

#include "MadgwickAHRS.h"
#include <math.h>

//---------------------------------------------------------------------------------------------------
// Definitions

#define sampleFreq  512.0f      // sample frequency in Hz
#define betaDef     0.1f        // 2 * proportional gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float beta = betaDef;                              // 2 * proportional gain (Kp)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrt(float x);
//---------------------------------------------------------------------------------------------------
// IMU algorithm update

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;
}

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float invSqrt(float x) {
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;
    i = 0x5f3759df - (i>>1);
    y = *(float*)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}