zoukankan      html  css  js  c++  java
  • 四旋翼姿态解算——梯度下降法理论推导

    在前面的博文中介绍了使用互补滤波法进行姿态解算的原理以及程序,点击打开链接
    互补滤波法中,我们使用加速度计来补偿陀螺仪,将两者的数据融合解算出四元数下的姿态。
    梯度下降法中,依然是通过加速度计的数据来与陀螺仪的数据进行融合,而不同点是:互补滤波法中,先求出载体坐标系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;
    }

    以上仅是个人见解,希望能对更多人的学习提供帮助。
    ╮(~▽~)╭

    我的其他博文的链接:
    四旋翼姿态解算——基础理论及推导 :
    http://blog.csdn.net/hongbin_xu/article/details/55667899
    四旋翼姿态解算——互补滤波算法及理论推导:
    http://blog.csdn.net/hongbin_xu/article/details/56846490
    四旋翼姿态解算——互补滤波法补充(融合磁力计):
    http://blog.csdn.net/hongbin_xu/article/details/59110226

  • 相关阅读:
    【Access2007】将Excel表导入到Access2007在现有的表成
    IntentFilter
    C++ map
    C++ template
    c++ namespace
    JVM学习笔记
    hibernate配置文件hibernate.cfg.xml的详细解释
    以&运行在后台的程序,关闭terminal后,相应进进程自动关闭
    开市前挂单
    Python 3里,reduce()函数已经被从全局名字空间里移除了,它现在被放置在fucntools模块里
  • 原文地址:https://www.cnblogs.com/xuhongbin/p/6538342.html
Copyright © 2011-2022 走看看