基于四元数的简单互补滤波姿态解算

序言

鉴于现在飞控的热度不减当年，越来越多“年轻人”加入飞控制作学习的阶段，也有许多新手上来就气势汹汹的进行姿态解算，往往使用的就是较为常用的Mahony的互补滤波姿态解算，笔者在多年前也一样，最开始接触的就是这个算法，但是当时懂得马马虎虎，直接把数据丢进去之后就得到了解算好的四元数，之后按照网上的公式一转换就OK了，但是后来才发现，当你的飞控出现问题的时候，大多数的时候，不见得是控制问题，有时候确实是因为姿态解算不好，所以今天就特别的对这个算法进行讲解，引用一个最近看到的：我希望这篇博客可以帮助到一些人。

姿态解算的基本概念

说是概念，其实确切来讲是应该弄清楚的事情：
1、姿态解算中的传感器的特性。对于加速度，其大小使用三角公式得到一个角度（笔者这么干过，确实行之有效），但是这个角度在旋转瞬间是十分不稳定的，例如在水平面转到10度左右的时候，这个角度很可能超过20度，之后会慢慢回到10度，即所谓的“短期不稳定，长期稳定”；对于角速度，可以通过“角度=∫角速度dt”这样的公式得到角度，但是这样做的时候会发现当在瞬间旋转的时候，得到的角度差是准确的，但是一旦长期不矫正这个角度，这个角度就会远远的偏离实际的角度，即所谓的“短期稳定，长期不稳定”；而磁力计的特性就偏好，短期和长期都可以很稳定，但是在飞机倾斜的时候，原本的磁场数据就会变得不准确，需要校准。
2、姿态解算中谁是主角。在接触飞控之初，我觉得加速度应该是主力（当时还不知道yaw轴长期的漂移现象），毕竟他的数据是长期稳定的，但是很遗憾，在当下接触到的姿态解算算法，大多数是以角速度作为姿态解算的主角，这点希望初学者要谨记在心。
3、DCM矩阵（方向余弦矩阵），这个名词对于飞控的“高阶玩家”确实不应该感到陌生，但是对于初学者，可能知道更多的是四元数，但是笔者在这里还是要说一下，在姿态解算中，甚至到后面的定高、定点算法中，DCM矩阵都会起到很大的作用，而不是四元数。简单的来讲，DCM矩阵的作用就是把A坐标系的数据旋转到B坐标，举个简单的例子，你可以通过DCM矩阵把机体上的三轴加速度计数据旋转到世界坐标系下。

姿态解算算法介绍

void IMUupdate(float gx, float gy, float gz, float ax,float ay, float az)
{
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;

    float q0q0 = q0*q0;
    float q0q1 = q0*q1;
    float q0q2 = q0*q2;
    float q1q1 = q1*q1;
    float q1q3 = q1*q3;
    float q2q2 = q2*q2;
    float q2q3 = q2*q3;
    float q3q3 = q3*q3;

    if(ax*ay*az==0)
        return;

    // 第一步：对加速度数据进行归一化
    norm = sqrt(ax*ax + ay*ay + az*az); 
    ax = ax / norm; 
    ay = ay / norm; 
    az = az / norm; 

    // 第二步：DCM矩阵旋转
    vx = 2*(q1q3 - q0q2); 
    vy = 2*(q0q1 + q2q3); 
    vz = q0q0 - q1q1 - q2q2 + q3q3 ;

    // 第三步：在机体坐标系下做向量叉积得到补偿数据
    ex = ay*vz - az*vy ;
    ey = az*vx - ax*vz ;
    ez = ax*vy - ay*vx ;

    // 第四步：对误差进行PI计算，补偿角速度
    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    gx = gx + Kp*ex + exInt;
    gy = gy + Kp*ey + eyInt;
    gz = gz + Kp*ez + ezInt;

    // 第五步：按照四元数微分公式进行四元数更新
    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;
}

以上就是网上比较流行的姿态解算程序，在此笔者把最精华的部分截取出来进行大概的讲解，在笔者心里，这个算法主要分为五个步骤：
1、对数据的规范化（注意，这里是规范化，程序注解是归一化），什么意思，就是对于姿态解算来讲，数据是要在使用这个算法之前进行标度变换的！首先是角速度，大部分的传感器输出的角速度是“度每秒”，但是实际上，角速度的国际单位是“弧度每秒”，这点是一定要注意的，当你直接拿着传感器的原始数据进行算法实现的时候，会发现输出的四元数就不稳定，一直在变化；再次是加速度，加速度在传入这个算法之前，可以转化为以m/s/s为单位的数据，但是也可以不用，因为在算法内部，加速度的数据被归一化了（这个归一化是很重要的），所以并没有太大影响。
2、标准重力加速度旋转到机体坐标系，

由上图可以看出，算法中第二步的时候，直接使用了DCM矩阵的第三行，这点对于初学者可能百思不得其解，但是如果写为矩阵与向量相乘就很清楚了，即vectorA=matrixC*vectorB，在该算法里面，matrixC是图中所示矩阵的转置（因为图中的矩阵为机体坐标转世界坐标，其转置就是世界坐标转机体坐标，这是DCM矩阵的性质之一）；vectorB是标准的重力加速度（0,0,1），使用标准的重力加速度是因为在机体运行过程中，在世界坐标系下（即以人的角度去观察机体坐标系的话），只受到垂直向下（Z轴）的力的作用，这个力就是重力，加速度为9.8m/s/s,而xy轴不受到力的作用，所以数据是0，注意，这里的标准加速度使用归一化之后的值，这样的使用是用深意的，读者将会在第三步体会到；最后vectorA就是我们得到的标准重力加速度在机体坐标系下的映射啦，读者可以在纸上进行上述公式的演算。
3、对机体加速度与标准加速度的映射进行向量的叉乘，得到补偿量，这一步可以说是本算法中最精髓的部分，我们都知道，向量具有点乘和叉乘两种运算，点乘公式为A·B=|A||B|*cosθ，叉乘的公式为A×B=|A||B|*sinθ,其中θ为两个向量之间的夹角，这两个运算的实际的意义读者可以自行查看，这里不再赘述，这里只说在本算法中，A和B是两个单位向量，因此其模为1，所以这样的公式下来就只剩下sinθ了，而在数学上，当θ极小的时候，sinθ是等于θ的，因此这里的公式走下来之后就得到了机体坐标系下实际加速度向量与理想加速度向量之间的差角，下面的算法使用这个差角作为偏差进行PI运算，补偿角速度，这里要特别解释一下，假如我们没有修正角速度，则四元数会因为角速度的漂移而漂移，因此得到的DCM矩阵也会漂移，这时候标准重力加速度在机体坐标系下的映射也会漂移，但是加速度计的数据是长期稳定的，它会与漂移之后的映射不重合，两个向量之间就会产生角度差（由于时间很短，认为是角速度），使用这个角度差矫正角速度数据，就会使抑制角速度长期不稳定，得到准确的四元数。这里建议读者对两个向量的叉乘在纸上进行演算加深印象。
4、PI运算，这里特别强调一下，因为飞行器在飞行过程中机架的震动很容易传到传感器上，加速度又因为短期不稳定，十分容易受到这样震动的影响，变得不稳定，所以在矫正角速度的时候，PI值不易过大，一般P值小于1，I值小于0.01，当然，如果防震措施较好，则可以适当大一些。
5、四元数的微分公式，读者可以自行到网上或者书上找到，这里不再赘述。