Pixhawk之姿态解算篇(2)_mahony算法分析

一、开篇

        还是没能进入到源码部分研究,对姿态解算过程太过于模糊,所以主要开始研究一下关于姿态解算的过程和实现,本篇博文主要是以mahony的算法为基础理解姿态解算的过程,主要参考的论文就是William Premerlani and Paul Bizard的关于DCM的一篇经典论文《Direction Cosine Matrix IMU_Theory》,一定要搞透这篇论文,没看过它都不敢称自己研究过飞控算法;然后接下来还有madgwick和mahony的论文需要研究,看英文的比较费时间,但是还是得慢慢的啃啊~~~

        然后就是结合国内的一本很不错的书籍《捷联惯性导航技术》,不需要细看,只需要了解其中关于姿态解算部分的即可。国内的课本嘛,大家都懂的,恨不得手把手的教你了。国外的论文就不一样了,继续啃吧。

二、版权声明

博主:summer

声明:喝水不忘挖井人,转载请注明出处。

原文地址:http://blog.csdn.net/qq_21842557

联系方式:[email protected]

技术交流QQ:1073811738

三、实验平台

Software Version:ArduCopter(Ver_3.3)

Hardware Version:pixhawk

IDE:eclipse Juno (Windows)

四、基本知识

1、 陀螺仪和加速计(特性分析)

1)陀螺仪

         Gyro sensitivity、 operating range、offset、drift、calibrationandsaturation must be taken into account in the implementation of DCM。

灵敏度

        测量角速度,具有高动态特性,它是一个间接测量角度的器件其中一个关键参数就是gyro sensitivity(其单位是millivolts per degree persecond,把转速转换到电压值),测量范围越小气灵敏度越好。也就是说测量的是角度的导数,即角速度,要将角速度对时间积分才能得到角度。陀螺仪就是内部有一个陀螺,它的轴由于陀螺效应始终与初始方向平行,这样就可以通过与初始方向的偏差计算出旋转方向和角度。

偏移

        偏移就是在陀螺没有转动的时候却又输出,这个输出量的大小和供电电压以及温度有关,该偏移可以在陀螺仪上电时通过一小段时间的测量来修正。

漂移
        它是由于在时间的积累下偏移和噪声相互影响的结果,例如有一个偏置(offset)0.1dps加在上面,于是测量出来是0.1dps,积分一秒之后,得到的角度是0.1度,1分钟之后是6度,还能忍受,一小时之后是360度,转了一圈,也就是说,陀螺仪在短时间内有很大的参考价值。
白噪声
        电信号的测量中,一定会带有白噪声,陀螺仪数据的测量也不例外。所以获得的陀螺仪数据中也会带有白噪声,而且这种白噪声会随着积分而累加。
积分误差
        对陀螺仪角速度的积分是离散的,长时间的积分会出现漂移的情况。所以要考虑积分误差的问题。

溢出

        就是转速超过了其测量的最大转速范围。关于这个问题的解决办法,在DCM IMU:Theory中给出来三种解决办法,不写了。

2)加速度计
         加速度计的低频特性好,可以测量低速的静态加速度。在无人机上就是对重力加速度g的测量和分析。当把加速度计拿在手上随意转动时,看的是重力加速度在三个轴上的分量值。加速度计在自由落体时,其输出为0。为什么会这样呢?这里涉及到加速度计的设计原理:加速度计测量加速度是通过比力来测量(比力方程,秦永元的书中有介绍),而不是通过加速度。加速度计仅仅测量的是重力加速度,而重力加速度与刚才所说的R坐标系(EarthFrame)是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系。

        加速度计仅仅测量的是重力加速度,3轴加速度计输出重力加速度在加速度计所在机体坐标系3个轴上的分量大小。重力加速度的方向和大小是固定的。通过这种关系,可以得到加速度计所在平面与地面的角度关系。加速度计若是绕着重力加速度的轴转动,则测量值不会改变,也就是说加速度计无法感知这种水平旋转。

Accelerometersare used for roll-pitch drift correction because they have zero drift
        有关陀螺仪和加速度计和关系,姿态解算融合的原理,再把下面这个比喻放到这里一遍。
        机体好似一条船,姿态就是航向(船头的方位),重力是灯塔,陀螺(角速度积分)是舵手,加速度计是瞭望手。舵手负责估计和把稳航向,他相信自己,本来船向北开的,就一定会一直往北开,觉得转了90度弯,那就会往东开。当然如果舵手很牛逼,也许能估计很准确,维持很长时间。不过只信任舵手,肯定会迷路,所以一般都有瞭望手来观察误差。
        瞭望手根据地图灯塔方位和船的当前航向,算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位,那肯定船的当前航向有偏差了,偏差就是ERR=X-Y。舵手收到瞭望手给的ERR报告,觉得可靠,那就听个90%ERR,觉得天气不好、地图误差大,那就听个10%ERR,根据这个来纠正估算航向。

3)磁传感器

        可以测量磁场,在没有其他磁场的情况下,仅仅测量的是地球的磁场,而地磁也是和R坐标系固连的,通过这种关系,可以得到平面A和地平面的关系。(平面A:和磁场方向垂直的平面),同样的,若是沿着磁场方向的轴旋转,测量值不会改变,无法感知这种旋转。

        综合考虑,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系,动态性能好,受外部干扰小,但测量值随时间变化比较大。可以看出,它们优缺点互补,结合起来才能有好的效果。

4)关于数据融合

        现在有了三个传感器,都能在一定程度上测量角度关系,但是究竟相信谁?根据刚才的分析,应该是在短时间内更加相信陀螺仪,隔三差五的问问加速度计和磁传感器,角度飘了多少了?有一点必须非常明确,陀螺仪才是主角,加速度计和磁传感器仅仅是跑龙套的。其实加速度计无法对航向角进行修正,修正航向角需要磁力计。

参考crazypony的分析:http://www.crazepony.com/wiki/mpu6050.html和《DCM IMU:Theory》

五、正文

1、首先就是基于mahony算法的AHRS姿态解算的一套源码,理论基础是DCM IMU:Theory,很有参考价值。先自己分析一下,看看每一行具体是做什么的,是如何实现姿态解算和修正的。然后会给出相应的分析

[plain]  view plain  copy
 
  1.    /*  
  2.     *AHRS  
  3.    */  
  4.    // AHRS.c  
  5.    // Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion  
  6.    // compensation algorithms from my filter [Madgwick] which eliminatesthe need for a reference  
  7.    // direction of flux (bx bz) to be predefined and limits the effect ofmagnetic distortions to yaw  
  8.    // axis only.  
  9.    // User must define 'halfT' as the (sample period / 2), and the filtergains 'Kp' and 'Ki'.  
  10.    // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elementsrepresenting the estimated  
  11.    // orientation.  See my report foran overview of the use of quaternions in this application.  
  12.    // User must call 'AHRSupdate()' every sample period and parsecalibrated gyroscope ('gx', 'gy', 'gz'),  
  13.    // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz')data.  Gyroscope units are  
  14.    // radians/second, accelerometer and magnetometer units are irrelevantas the vector is normalised.                                  
  15.    #include "stm32f10x.h"  
  16.    #include "AHRS.h"  
  17.    #include "Positioning.h"  
  18.    #include   
  19.    #include   
  20. /* Private define------------------------------------------------------------*/  
  21.    #define Kp 2.0f                       // proportional gain governs rate of convergence toaccelerometer/magnetometer  
  22.     #define Ki 0.005f          // integral gain governs rate of convergenceof gyroscope biases  
  23.     #define halfT 0.0025f      // half the sample period    
  24.    #define ACCEL_1G 1000    //theacceleration of gravity is: 1000 mg  
  25. /* Private variables---------------------------------------------------------*/  
  26.    static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing theestimated orientation  
  27.    static float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error  
  28. /* Public variables----------------------------------------------------------*/  
  29.    EulerAngle_Type EulerAngle;       //unit: radian  
  30.     u8InitEulerAngle_Finished = 0;  
  31.    float Magnetoresistor_mGauss_X = 0, Magnetoresistor_mGauss_Y = 0,Magnetoresistor_mGauss_Z = 0;//unit: milli-Gauss                                                                                                                                                                                                        
  32.    float Accelerate_mg_X, Accelerate_mg_Y, Accelerate_mg_Z;//unit: mg                                                                
  33.    float AngularRate_dps_X, AngularRate_dps_Y, AngularRate_dps_Z;//unit:dps: degree per second         
  34.    int16_t Magnetoresistor_X, Magnetoresistor_Y, Magnetoresistor_Z;                                                                                                                                                                                                        
  35.    uint16_t Accelerate_X = 0, Accelerate_Y = 0, Accelerate_Z = 0;                                                                                                                                                                                                
  36.    uint16_t AngularRate_X = 0, AngularRate_Y = 0, AngularRate_Z = 0;  
  37.    u8 Quaternion_Calibration_ok = 0;  
  38.    /* Private macro-------------------------------------------------------------*/  
  39.    /* Private typedef-----------------------------------------------------------*/  
  40.    /* Private function prototypes -----------------------------------------------*/  
  41. /******************************************************************************  
  42.     *Function Name  : AHRSupdate  
  43.     *Description    : None  
  44.     *Input          : None  
  45.     *Output         : None  
  46.     *Return         : None  
  47. *******************************************************************************  
  48.    void AHRSupdate(float gx, float gy, float gz, float ax, float ay, floataz, float mx, float my, float mz) {  
  49.            float norm;  
  50.            float hx, hy, hz, bx, bz;  
  51.            float vx, vy, vz, wx, wy, wz;   
  52.            float ex, ey, ez;  
  53.    
  54.            // auxiliary variables to reduce number of repeated operations  
  55.            float q0q0 = q0*q0;  
  56.            float q0q1 = q0*q1;  
  57.            float q0q2 = q0*q2;  
  58.             float q0q3 = q0*q3;  
  59.            float q1q1 = q1*q1;  
  60.            float q1q2 = q1*q2;  
  61.            float q1q3 = q1*q3;  
  62.            float q2q2 = q2*q2;  
  63.            float q2q3 = q2*q3;  
  64.            float q3q3 = q3*q3;  
  65.             
  66.            // normalise the measurements  
  67.            norm = sqrt(ax*ax + ay*ay + az*az);  
  68.            ax = ax / norm;  
  69.            ay = ay / norm;  
  70.            az = az / norm;  
  71.            norm = sqrt(mx*mx + my*my + mz*mz);  
  72.            mx = mx / norm;  
  73.             my = my / norm;  
  74.            mz = mz / norm;  
  75.             
  76.            // compute reference direction of magnetic field  
  77.            hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);  
  78.            hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);  
  79.            hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 -q2q2);          
  80.            bx = sqrt((hx*hx) + (hy*hy));  
  81.            bz = hz;  
  82.             
  83. // estimated direction of gravity and magnetic field (v and w)  
  84.            vx = 2*(q1q3 - q0q2);  
  85.            vy = 2*(q0q1 + q2q3);  
  86.            vz = q0q0 - q1q1 - q2q2 + q3q3;  
  87.            wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);  
  88.            wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);  
  89.            wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);   
  90.             
  91. // error is sum ofcross product between reference direction of fields and directionmeasured by sensors   
  92.            ex = (ay*vz - az*vy) + (my*wz - mz*wy);  
  93.            ey = (az*vx - ax*vz) + (mz*wx - mx*wz);  
  94.            ez = (ax*vy - ay*vx) + (mx*wy - my*wx);  
  95.             
  96.            // integral error scaled integral gain  
  97.            exInt = exInt + ex*Ki* (1.0f / sampleFreq);  
  98.            eyInt = eyInt + ey*Ki* (1.0f / sampleFreq);  
  99.            ezInt = ezInt + ez*Ki* (1.0f / sampleFreq);  
  100.            // adjusted gyroscope measurements  
  101.            gx = gx + Kp*ex + exInt;  
  102.            gy = gy + Kp*ey + eyInt;  
  103.            gz = gz + Kp*ez + ezInt;  
  104.             
  105.            // integrate quaternion rate and normalize  
  106.            q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;  
  107.            q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;  
  108.            q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;  
  109.            q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;   
  110.             
  111.            // normalise quaternion  
  112.            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);  
  113.            q0 = q0 / norm;  
  114.            q1 = q1 / norm;  
  115.            q2 = q2 / norm;  
  116.            q3 = q3 / norm;  
  117. }  
2、上述算法的源码分析
        先给一个简要的代码注释分析。
[plain]  view plain  copy
 
  1. //陀螺仪、加速度计、磁力计数据融合  
  2.     void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {  
  3.             float norm;  
  4.             float hx, hy, hz, bx, bz;  
  5.             float vx, vy, vz, wx, wy, wz; //v*当前姿态计算得来的重力在三轴上的分量  
  6.             float ex, ey, ez;  
  7.   
  8.             // auxiliary variables to reduce number of repeated operations  
  9.             float q0q0 = q0*q0;  
  10.             float q0q1 = q0*q1;  
  11.             float q0q2 = q0*q2;  
  12.             float q0q3 = q0*q3;  
  13.             float q1q1 = q1*q1;  
  14.             float q1q2 = q1*q2;  
  15.             float q1q3 = q1*q3;  
  16.             float q2q2 = q2*q2;  
  17.             float q2q3 = q2*q3;  
  18.             float q3q3 = q3*q3;  
  19.              
  20.             // normalise the measurements  
  21.             norm = sqrt(ax*ax + ay*ay + az*az);   
  22.             ax = ax / norm;  
  23.             ay = ay / norm;  
  24.             az = az / norm;  
  25.             norm = sqrt(mx*mx + my*my + mz*mz);   
  26.             mx = mx / norm;  
  27.             my = my / norm;  
  28.             mz = mz / norm;  
  29.              
  30.             // compute reference direction of magnetic field  
  31.             hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);  
  32.             hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);  
  33.             hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);           
  34.             bx = sqrt((hx*hx) + (hy*hy));  
  35.             bz = hz;   
  36.              
  37. // estimated direction of gravity and magnetic field (v and w)   
  38. //参考坐标n系转化到载体坐标b系的用四元数表示的方向余弦矩阵第三列即是。  
  39.         //处理后的重力分量  
  40.             vx = 2*(q1q3 - q0q2);  
  41.             vy = 2*(q0q1 + q2q3);  
  42.             vz = q0q0 - q1q1 - q2q2 + q3q3;  
  43. //处理后的mag,反向使用DCM得到  
  44.             wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);  
  45.             wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);  
  46.             wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);    
  47.              
  48. // error is sum of cross product between reference direction of fields and direction measured by sensors 体现在加速计补偿和磁力计补偿,因为仅仅依靠加速计补偿没法修正Z轴的变差,所以还需要通过磁力计来修正Z轴。(公式28)。《四元数解算姿态完全解析及资料汇总》的作者把这部分理解错了,不是什么叉积的差,而叉积的计算就是这样的。计算方法是公式10。  
  49.             ex = (ay*vz - az*vy) + (my*wz - mz*wy);  
  50.             ey = (az*vx - ax*vz) + (mz*wx - mx*wz);  
  51.             ez = (ax*vy - ay*vx) + (mx*wy - my*wx);  
  52.              
  53.             // integral error scaled integral gain   
  54.             exInt = exInt + ex*Ki* (1.0f / sampleFreq);  
  55.             eyInt = eyInt + ey*Ki* (1.0f / sampleFreq);  
  56.             ezInt = ezInt + ez*Ki* (1.0f / sampleFreq);  
  57.             // adjusted gyroscope measurements  
  58. //将误差PI后补偿到陀螺仪,即补偿零点漂移。通过调节Kp、Ki两个参数,可以控制加速度计修正陀螺仪积分姿态的速度。(公式16和公式29)  
  59.             gx = gx + Kp*ex + exInt;  
  60.             gy = gy + Kp*ey + eyInt;  
  61.             gz = gz + Kp*ez + ezInt;  
  62.              
  63.             // integrate quaternion rate and normalize  
  64.  //一阶龙格库塔法更新四元数  
  65.             q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;  
  66.             q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;  
  67.             q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;  
  68.             q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;    
  69.              
  70.             // normalise quaternion  
  71.             norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);  
  72.             q0 = q0 / norm;  
  73.             q1 = q1 / norm;  
  74.             q2 = q2 / norm;  
  75.             q3 = q3 / norm;  
  76. }  
3、在深入一点

        1)无人机控制中,主要就是姿态解算和姿态控制部分,该部分主要介绍姿态解算,下面是来一张比较好理解的框图。

Pixhawk之姿态解算篇(2)_mahony算法分析_第1张图片
Pixhawk之姿态解算篇(2)_mahony算法分析_第2张图片

        由上面两幅图很容易理解整个控制和姿态解算了吧,在第四部分也介绍了关于陀螺仪、加速计、磁力计它们的作用,所以不再单独介绍了。不理解的往上翻翻再看看吧。

        2)在姿态解算过程中,到底用什么表示无人机的姿态呢?姿态表示的方法有很多种,比如欧拉角、四元数、DCM,各有的各的优势,比较常用的就是四元数,方便计算。上面的姿态解算算法也是基于四元数的。下面介绍一个它们三个的优缺点。

姿态解算方法的比较:

算法

优点

缺点

PS

欧拉角法(3参数)

1、  通过欧拉角微分方程直接解算出pitch、roll、yaw

2、  概念直观,易于理解

3、  解算结果永远是正交的,无需再次正交化处理

1、  方程中有三角函数的运算,接超越函数有一定的困难

2、  当俯仰角接近90°时出现退化现象

1、  适应于水平姿态角变化不大的情况

2、  不适用于全姿态运载体

方向余弦法(9参数)

1、  对姿态矩阵微分方程的求解,避免了欧拉角法中出现的退化现象

2、  可以全姿态运行

1、参数量过多,计算量大,实时性不好

很少在工程中使用

四元数法(4参数)

1、  直接求解四元数微分方程

2、  只需要求解四个参数,计算量小

3、算法简单,易于操作

漂移比较严重

可以在实现过程中修正漂移,应用比较广泛

          3)废话不多说,进入正题。上述算法主要就是利用加速度计和磁力计修正陀螺仪的误差,算法是使用了PI反馈控制器实现反馈修正的。如下图:

Pixhawk之姿态解算篇(2)_mahony算法分析_第3张图片

        首先,东北天坐标系是n系(地理坐标系,参考坐标系),载体坐标系是b系,就是我们飞行器的坐标系。对于四元数法的姿态解算,需要求取的就是四元数的值;方向余弦矩阵(用于表示n系和b系的相对关系)中的元素本来应该是三角函数,这里由于使用四元数法,所以矩阵中的元素就成了四元数。所以姿态解算的任务就是求解由四元数构成的方向余弦矩阵nCb(nCb表示从b系到n的转换矩阵,同理,bCn表示从n系到b的转换矩阵,它们的关系是转置)。

        显然,上述矩阵是有误差存在的。对于一个确定的向量n,用不同的坐标系表示时,他们所表示的大小和方向一定是相同的。但是由于这两个坐标系的转换矩阵存在误差,那么当一个向量经过这么一个有误差存在的旋转矩阵变换后,在另一个坐标系中肯定和理论值是有偏差的,我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数,这就是说修正的就是四元数,于是乎姿态就这样被修正了,这才是姿态解算的原理。姿态解算求的是四元数,是通过修正旋转矩阵中的四元数来达到姿态解算的目的,而不要以为通过加速度计和地磁计来修正姿态,加速度计和地磁计只是测量工具和载体,通过这两个器件表征旋转矩阵的误差存在,然后通过算法修正误差,修正四元数,修正姿态。

加速度计修正pitch_roll

        加速度计可以修正pitch_roll,但是我们必须要考虑离心加速度(centrifugalacceleration),离心加速度就等于旋转率向量(即gyro vector)和速度向量的叉积(没有原因,平均得来的并且还相当准确,速度可以用GPS获取)。我们假设飞机方向和X轴平行。

Pixhawk之姿态解算篇(2)_mahony算法分析_第4张图片

        在机体上测得的重力的为:

Pixhawk之姿态解算篇(2)_mahony算法分析_第5张图片

        Pitch_roll的旋转修正向量是由DCM的Z行与归一化以后的重力参考向量的叉积。


        在n系中,加速度计输出为,经过bCn(用四元数表示的转换矩阵)转换之后到b系中的值为;在b系中,加速度计的测量值为,现在和均表示在b系中的竖直向下的向量,由此,我们来做向量积(叉积),得到误差,利用这个误差来修正bCn矩阵,于是四元数就在这样一个过程中被修正了。但是,由于加速度计无法感知z轴上的旋转运动,所以还需要用地磁计来进一步补偿。

Pixhawk之姿态解算篇(2)_mahony算法分析_第6张图片

        PS:公式不好加,只能直接切图了~~~

        加速度计在静止时测量的是重力加速度,是有大小和方向的;同理,地磁计同样测量的是地球磁场的大小和方向,只不过这个方向不再是竖直向下,而是与x轴(或者y轴)呈一个角度,与z轴呈一个角度。记作,假设x轴对准北边,所以by=0,即。倘若知道bx和bz的精确值,那么就可以采用和加速度计一样的修正方法来修正。只不过在加速度计中,在n系中的参考向量是,变成了地磁计的。如果我们知道bx和bz的精确值,那么就可以摆脱掉加速度计的补偿,直接用地磁计和陀螺仪进行姿态解算,但是你看过谁只用陀螺仪和地磁计进行姿态解算吗?没有,因为没人会去测量当地的地磁场相对于东北天坐标的夹角,也就是bx和bz(插曲:关于这个bx和bz的理解:可以对比重力加速度的理解,就像vx vy vz似的,因为在每一处的归一化以后的重力加速度都是0 0 1然后旋转到机体坐标系,而地球每一处的地磁大小都不一样的,不能像重力加速度那样直接旋转得到了,只能用磁力计测量到的数据去强制拟合。)。那么现在怎么办?前面已经讲了,姿态解算就是求解旋转矩阵,这个矩阵的作用就是将b系和n正确的转化直到重合。现在我们假设nCb旋转矩阵是经过加速度计校正后的矩阵,当某个确定的向量(b系中)经过这个矩阵旋转之后(到n系),这两个坐标系在XOY平面上重合(参考DCM IMU:Theory的Drift cancellation部分),只是在z轴旋转上会存在一个偏航角的误差。下图表示的是经过nCb旋转之后的b系和n系的相对关系。可以明显发现加速度计可以把b系通过四元数法从任意角度拉到与n系水平的位置上,此时只剩下一个偏航角误差。这也是为什么加速度计无法修正偏航的原因。

Pixhawk之姿态解算篇(2)_mahony算法分析_第7张图片

Pixhawk之姿态解算篇(2)_mahony算法分析_第8张图片

        为什么在word中使用公式编译器编辑的公式没办法复制到CSDN中呢????????????

        此方式在数学上没有任何问题,但是由于地磁传感器极易受到各种干扰,而此算法又将地磁传感器所指示的方向过度的融入到了姿态当中,导致实际使用中数据会非常不稳定。怪不得crazypony的算法中没有使用磁力计修正YAW,这应该就是原因所在吧。

六、来一点ardupilot源码分析

        就理解了一个通过gyro_vector更新DCM的算法,这个应该是在renormalization直接就需要了解,可以前期没理解到底是干嘛的,现在补上;关于renormalization的算法可以参考上一篇博文。

        姿态解算过程中不仅需要修正陀螺仪的各种errors,还需要实时的更新的DCM。上周一直没能理解的一个问题,在AP_AHRS_DCM.cpp中的matrix_update(delta_t),就是实时更新DCM矩阵的,源码如下,这一部分研究了很久,需要的基础知识比较多。先上源码

[plain]  view plain  copy
 
  1. // update the DCM matrix using only the gyros  
  2. Void AP_AHRS_DCM::matrix_update(float _G_Dt)  
  3. {  
  4.     _omega.zero();  
  5.     // average across first two healthy gyros. This reduces noise on  
  6.     // systems with more than one gyro. We don't use the 3rd gyro  
  7.     // unless another is unhealthy as 3rd gyro on PH2 has a lot more  
  8.     // noise  
  9.     uint8_t healthy_count = 0;  
  10.     Vector3f delta_angle;  
  11.     for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {  
  12.         if (_ins.get_gyro_health(i) && healthy_count < 2) {  
  13.             Vector3f dangle;  
  14.             if (_ins.get_delta_angle(i, dangle)) {  
  15.                 healthy_count++;  
  16.                 delta_angle += dangle;  
  17.             }  
  18.         }  
  19.     }  
  20.     if (healthy_count > 1) {  
  21.         delta_angle /= healthy_count;  
  22.     }  
  23.     if (_G_Dt > 0) {  
  24.         _omega = delta_angle / _G_Dt;  
  25.         _omega += _omega_I;  
  26.         _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);  
  27.     }  
  28. }  

          首先就是通过陀螺仪获取两次gyro_vector,然后取平均以降低误差;然后就是比较专业的算法实现_dcm_matrix.rotate((_omega +_omega_P + _omega_yaw_P) * _G_Dt)了。进入到matrix3.cpp中有rotate()函数,该函数就是实现DCM更新的算法实现,算法主要是用陀螺仪的输出值与DCM矩阵的乘积再加回到DCM中去,处理过程中使用了离散化的概念,即dcm(k+1)=dcm(k)+增量,因为公式里有求导,必须离散化后才能计算机处理,感谢青龙的指导。 

[plain]  view plain  copy
 
  1. // apply an additional rotation from a body frame gyro vector  
  2. // to a rotation matrix.  
  3. template   
  4. void Matrix3::rotate(const Vector3 &g)  
  5. {  
  6.     Matrix3 temp_matrix;  
  7.     temp_matrix.a.x = a.y * g.z - a.z * g.y;  
  8.     temp_matrix.a.y = a.z * g.x - a.x * g.z;  
  9.     temp_matrix.a.z = a.x * g.y - a.y * g.x;  
  10.     temp_matrix.b.x = b.y * g.z - b.z * g.y;  
  11.     temp_matrix.b.y = b.z * g.x - b.x * g.z;  
  12.     temp_matrix.b.z = b.x * g.y - b.y * g.x;  
  13.     temp_matrix.c.x = c.y * g.z - c.z * g.y;  
  14.     temp_matrix.c.y = c.z * g.x - c.x * g.z;  
  15.     temp_matrix.c.z = c.x * g.y - c.y * g.x;  
  16.   
  17.     (*this) += temp_matrix;//增加累加到原始数据上  
  18. }  
        公式太多,没办法还是上图吧~~~
Pixhawk之姿态解算篇(2)_mahony算法分析_第9张图片
        最后来一张神人做的图。
Pixhawk之姿态解算篇(2)_mahony算法分析_第10张图片
六、总结
        算法的理解过程比较艰难,千万不能闭门造车,要多多交流,思想的碰撞才能产生出火花,多谢各位群友的无私帮助。通过上述的分析以后,对整个姿态解算过程有了整体的框架理解,接下来会结合上面算法的分析去分析ardupilot中关于姿态解算的源码,应该会理解的快一点了吧,希望如此~~~
        最近事情好多啊,愁死了,谁能帮我分担一点啊~~~~
        昨天去三星见了一位智能家居group的总监,聊了以后才发现自己有多low啊,我的BLE怎么搞,大公司都在做垄断化、平台化,看来毕业只能硬着头皮进大公司了,希望能找个好工作,谁在外企啊,帮我介绍工作啊,啊啊, 啊, ,啊, 啊。。。。
        写的语无伦次的,凑合看吧

你可能感兴趣的:(uav,姿态解算与控制)