电赛四轴小结——姿态解算篇【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系（我是这么理解的nature地理坐标系,哈哈），载体坐标系是b系（body机载坐标系）。在姿态解算过程中，姿态表示的方法有很多种，比如欧拉角、四元数、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) 
           //用四元数表示的方向余弦矩阵解算出理论载体坐标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