电赛四轴小结——姿态解算篇（mahony算法）

前言：前段时间一直在备赛没时间做总结，其实无人机控制算法中主要是姿态解算系统和姿态控制系统，这里先总结下姿态解算系统的相关要点，研究一下关于姿态解算的过程和实现，本篇博文主要是以mahony的算法为基础理解姿态解算的过程。

传感器：

陀螺仪计（gyroscope，也称作 gyro）：

灵敏度：陀螺仪计测量的是角速度，具有高动态特性，关键参数就是gyro sensitivity（其单位是millivolts per degree persecond，把转速转换到电压值），测量范围越小气灵敏度越好。也就是说测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。陀螺仪的每个通道检测一个轴的旋转，这样就可以通过与初始方向的偏差计算出旋转方向和角度。

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

漂移：它是由于在时间的积累下偏移和噪声相互影响的结果，例如有一个偏置（offset）0.1dps加在上面，于是测量出来是0.1dps，积分一秒之后，得到的角度是0.1度，1分钟之后是6度，还能忍受，一小时之后是360度，转了一圈，也就是说，陀螺仪在短时间内有很大的参考价值，但是长时间观测会造成很大的误差。

白噪声：电信号的测量中，一定会带有白噪声，陀螺仪数据的测量也不例外。所以获得的陀螺仪数据中也会带有白噪声，而且这种白噪声会随着积分而累加。

积分误差：对陀螺仪角速度的积分是离散的，长时间的积分会出现漂移的情况。所以要考虑积分误差的问题。

溢出：就是转速超过了其测量的最大转速范围。关于这个问题的解决办法，传闻在APM的代码中有解决方法，还没看到暂且不谈。

这是由于陀螺仪计融合出来的姿态存在这么多的误差，所以我们必须要使用其它传感器辅助校正。

加速度计（accelerometer）：

       加速度计的低频特性好，其它瞬间加速度可以忽略，可以比较准确测量低速的静态加速度。当把加速度计拿在手上随意转动时，看的是重力加速度在三个轴上的分量值。加速度计在自由落体时，其输出为0。为什么会这样呢？这里涉及到加速度计的设计原理：加速度计测量的加速度是通过比力来测量(比力方程，《惯性导航》秦永元的书中有介绍)，而不是直接测量得到加速度。加速度计仅仅测量的是重力加速度在机载坐标下的三个轴上的分量，而重力加速度与刚才所说的R坐标系（EarthFrame）是固连的，通过这种关系,可以得到加速度计所在平面与地面的角度关系。

注意：加速度计若是绕着重力加速度的轴转动，则测量值不会改变，也就是说加速度计无法感知这种水平旋转，在飞机上来说就是加速度计是没法测量航向的。
这里有一篇翻译了一篇详细讲解了陀螺仪和加计的文章：https://blog.csdn.net/lovewubo/article/details/9084291

电子罗盘（地磁计）：

       测量磁场，在没有其他磁场的情况下，仅仅测量的是地球的磁场，而地磁也是和R坐标系固连的，通过这种关系，可以得到和磁场方向垂直的平面（平面A）和地平面的关系。和加速度计一样，若是沿着磁场方向的轴旋转，测量值不会改变，无法感知这种旋转。加速度计在静止时测量的是重力加速度，是有大小和方向的；同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向与x轴（或者y轴）呈一个角度，与z轴呈一个角度。实际上对水平方向的两个分量来说，他们的矢量和总是指向磁北的，罗盘中的航向角（Azimuth）就是当前方向和磁北的夹角。所以当罗盘保持水平时，只需要用磁力计水平方向两轴（通常为X轴和Y轴）的检测数据就可以计算出航向角。当罗盘水平旋转的时候，航向角在0- 360之间变化。

关于电子罗盘工作原理的参考文章：https://blog.csdn.net/wxlinwzl/article/details/6903548

姿态解算：

       在传感器中陀螺仪才是主角，加速度计和磁传感器仅仅是起辅助校正作用的。其中加速度计无法对航向角进行修正，修正航向角需要磁力计。在常用坐标系中，无人机一般采用以下两种坐标系：参考坐标系是n系（地理坐标系），载体坐标系是b系（机载坐标系）。在姿态解算过程中，姿态表示的方法有很多种，比如欧拉角、四元数、DCM（方向余弦），各有的各的优势，比较常用的就是四元数。

这里以四元数解算代码实现过程结合数学原理分析来论述一下：

        对于四元数法的姿态解算，最终需求的就是四元数的值；方向余弦矩阵（用于表示n系和b系的相对关系）中的元素本来应该是三角函数，这里由于使用四元数法，所以矩阵中的元素就成了四元数。所以姿态解算的任务就是求解由四元数构成的方向余弦矩阵nCb（nCb表示从b系到n的转换矩阵，同理，bCn表示从n系到b的转换矩阵，它们的关系是转置）。

       显然，对于一个确定的向量n，用不同的坐标系表示时，理论上它们所表示的大小和方向一定是相同的。但是实际上我们的转换矩阵在这两个坐标系下并不相等，而是存在误差，那么当一个向量经过这么一个有误差存在的旋转矩阵变换后，在另一个坐标系中肯定和理论值是有偏差的，我们通过这个偏差来修正这个旋转矩阵。这个旋转矩阵的元素是四元数，这就是说修正的就是四元数，于是姿态就这样被修正了，这才是姿态解算本质原理。

       姿态解算求的是四元数，是通过修正旋转矩阵中的四元数来达到姿态解算的目的，而不要以为通过加速度计和地磁计来修正姿态，加速度计和地磁计只是测量工具和载体，通过这两个器件表征旋转矩阵的误差存在，然后通过算法修正误差，修正四元数以修正姿态。

       算法主要思路就是利用加速度计和磁力计修正陀螺仪的误差，使用了PI反馈控制器实现反馈修正。大致思路如下：

1、观测加速度计数据，因为已知地理坐标系的标准重力加速度 [0 0 1]，然后变换到机体坐标系，理论与实际叉乘求出误差
2、观测电子罗盘数据，由于标准重力加速度在所有地方都是一样的，可以由其直接从地理坐标系变换到机体坐标系，但是在地理坐标系中，地磁大小却不是相同的，需要根据实际情况测出来，因此磁力计数据的来源则必然是时刻测量更新。所以首先进行机体坐标系变换到地理坐标系，相当于求出理论地磁，之后处理和加速度计差不多，就是变换到机体坐标系中，最后利用理论和实际测量叉乘求出误差
3、将误差累加，利用PI补偿至陀螺仪即可求解得四元数。

下面贴出具体代码实现过程：

//陀螺仪、加速度计、磁力计数据融合出姿态四元数
    void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
            float norm;
            float hx, hy, hz, bx, bz;
            float vx, vy, vz, wx, wy, wz; //v*当前姿态计算得来的重力在机载坐标下三轴上的分量
            float ex, ey, ez;
 
            // auxiliary variables to reduce number of repeated operations
            //先计算好相关乘积项为了后面做矩阵运算做准备
            float q0q0 = q0*q0;
            float q0q1 = q0*q1;
            float q0q2 = q0*q2;
            float q0q3 = q0*q3;
            float q1q1 = q1*q1;
            float q1q2 = q1*q2;
            float q1q3 = q1*q3;
            float q2q2 = q2*q2;
            float q2q3 = q2*q3;
            float q3q3 = q3*q3;
           
            // normalise the measurements
            //加速度计和地磁计向量标准化
            norm = sqrt(ax*ax + ay*ay + az*az); 
            ax = ax / norm;
            ay = ay / norm;
            az = az / norm;
            norm = sqrt(mx*mx + my*my + mz*mz); 
            mx = mx / norm;
            my = my / norm;
            mz = mz / norm;
           
            // compute reference direction of magnetic field
            //这里计算得到的是理论地磁参考坐标系下的三个轴的分量
            hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
            hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
            hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);   
            
            //bx计算的是当前航向角和磁北的夹角，也就是北天东坐标下的航向角
            //当罗盘水平旋转的时候，航向角在0-360之间变化
            bx = sqrt((hx*hx) + (hy*hy));
            bz = hz; 
           
           // estimated direction of gravity and magnetic field (v and w) 
           //用四元数表示的方向余弦矩阵从参考坐标n系转化到载体坐标b系。
           //加速度计重力向量转换到b系
            vx = 2*(q1q3 - q0q2);
            vy = 2*(q0q1 + q2q3);
            vz = q0q0 - q1q1 - q2q2 + q3q3;
           //地磁计n系下磁向量转换到b系下啊，反向使用DCM得到
            wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
            wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
            wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
           
            // error is sum of cross product between reference direction of fields and direction measured by sensors 
            //体现在加速计补偿和磁力计补偿，因为仅仅依靠加速计补偿没法修正Z轴的变差，所以还需要通过磁力计来修正Z轴。
            //下面是叉积运算的过程
            ex = (ay*vz - az*vy) + (my*wz - mz*wy);
            ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
            ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
           
            // integral error scaled integral gain 
            exInt = exInt + ex*Ki* (1.0f / sampleFreq);
            eyInt = eyInt + ey*Ki* (1.0f / sampleFreq);
            ezInt = ezInt + ez*Ki* (1.0f / sampleFreq);
            // adjusted gyroscope measurements
            //将误差PI后补偿到陀螺仪，即补偿零点漂移。通过调节Kp、Ki两个参数，可以控制加速度计修                正陀螺仪积分姿态的速度。（公式16和公式29）
            gx = gx + Kp*ex + exInt;
            gy = gy + Kp*ey + eyInt;
            gz = gz + Kp*ez + ezInt;
           
            // integrate quaternion rate and normalize
            //一阶龙格库塔法更新四元数
            q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
            q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
            q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
            q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
           
            // normalise quaternion
            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
            q0 = q0 / norm;
            q1 = q1 / norm;
            q2 = q2 / norm;
            q3 = q3 / norm;
}

结合代码我们来讨论下具体到底是怎么实现加计和罗盘修正姿态的：

1、加速度计修正pitch和roll

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

2、地磁计修正yaw

       上面我们说了地磁计测量的是地球磁场的大小和方向，只不过这个方向是指向磁北。现记x轴对准北边，所以by=0，即bx = sqrt((hx*hx) + (hy*hy))。如果可以直接测得bx和bz的准确值，那么就可以采用和加速度计一样的修正方法来修正，甚至可以摆脱掉加速度计的补偿，直接用地磁计和陀螺仪进行姿态解算，但是没人会去测量当地的地磁场相对于东北天坐标的夹角，也就是bx和bz，这里可以对比重力加速度，就像vx vy vz似的，因为在每一处的归一化以后的重力加速度都是0 0 1然后旋转到机体坐标系，然而地球每一处的地磁大小都不一样的，不能像重力加速度那样直接旋转得到了，只能用磁力计测量到的数据去强制拟合，这里面的干扰因素太多了，得到的地磁值相对加速度计来说正确率要低，所以基本没人只会拿磁力计和陀螺仪做数据融合。前面已经讲了，姿态解算就是求解旋转矩阵，这个矩阵的作用就是将b系和n正确的转化直到重合，现在我们假设nCb旋转矩阵是经过加速度计校正后的矩阵，当某个确定的向量（b系中）经过这个矩阵旋转之后（到n系），这两个坐标系在XOY平面上重合，只剩下在z轴旋转上会存在一个偏航角的误差。下图表示的是经过nCb旋转之后的b系和n系的相对关系。可以明显发现加速度计可以把b系通过四元数法从任意角度拉到与n系水平的位置上，此时只剩下一个偏航角误差。

 

        我们先把b系中的观测值旋转到n系中然后算得n系中的再转换到b系中和原输出做比较即可得到误差，然后根据误差修正旋转即可。利用地磁计修正偏航的具体过程如下：

设地磁计在b系中的观测为，经过nCb矩阵旋转之后得到n系中的，令在n系下XOY平面的投影为bx = sqrt((hx*hx) + (hy*hy))，bz=hz，这样就得到了n系中的，然后经过bCn矩阵旋转回b系里面即可得到,然后再拿这个值和b系的原始输出做叉乘得到误差，然后拿这个误差和上面加计的误差叠加，再做PI补偿到陀螺仪数据即可修正bCn矩阵，从而得到了没有偏差的的实时姿态。

PS：龙格-库塔(Runge-Kutta)方法是一种在工程上应用广泛的高精度单步算法，其中包括著名的欧拉法，用于数值求解微分方程，该算法是构建在数学支持的基础之上的。详解：https://www.sohu.com/a/168421601_464087

 

 

参考文章：

hhttp://www.crazepony.com/wiki/mpu6050.html

ttps://blog.csdn.net/zhanghuaichao/article/details/48314121

http://www.sohu.com/a/159300038_464087

https://blog.csdn.net/qq_21842557/article/details/50993809

https://blog.csdn.net/Leyvi_Hsing/article/details/54293690（此文中有些说法在本人看来有些说法并不正确）

 

在实际做的过程中还会 遇到各种各样的问题，关键还是要理解透原理之后动手实践起来。

By Linzs 

2019.08.22