详解互补滤波四元数中向量叉积与陀螺仪角速度补偿问题（Mahony算法）

作者：Leyvi
时间：2017.1.10

一、归一化与坐标转换

很多做四轴的网友对互补滤波四元数姿态解算代码中的向量叉积和陀螺仪积分补偿问题有疑问，我也查了很多资料，写下这篇博文与大家共同学习。
先放一段互补滤波和四元数姿态解算的代码：

/**
 * 6DOF 互补滤波姿态估计(via Mahony)
 * @param[in] halfT:状态估计周期的一半
 */
const float Kp = 3.5, Ki = 0.05;
float exInt, eyInt, ezInt;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // roll,pitch,yaw 都为 0 时对应的四元数值。
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;

    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0/norm;
    q1 = q1/norm;
    q2 = q2/norm;
    q3 = q3/norm;

    roll =  atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3;     
    pitch =  asinf(2*q1*q3 - 2*q0*q2)*57.3;                                                          
    yaw  =  -atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3; 
}

这段代码中[ax,ay,az]是加速度计测量得到的重力向量，然后对其归一化，但是为什么要对它归一化处理呢？不是画蛇添足，而是后面大有用途！
[vx,vy,vz]是世界坐标系重力分向量[0,0,1]经过DCM旋转矩阵计算得到的机体坐标系中的重力向量,即
（该图摘自网络）
这个旋转矩阵在秦永元的惯性导航书中有详细解释，这里就看出来了，旋转得到的正是机体坐标系归一化后的重力向量。
有人可能纳闷为什么要进行旋转？这就涉及到世界坐标系和机体坐标系的概念了，我从网上摘了几张图进行简单讲解。

图1中盒子模型中的小球处于失重状态，坐标系与世界坐标系重合。
图2中盒子坐标系仍与世界坐标系重合，描述为[x,y,z],不同的是小球收到重力作用，盒子模型坐标系中重力向量为[0,0,1],世界坐标系中也是[0,0,1].
图3中盒子在世界坐标系中与地面夹角为45度，但是盒子坐标系仍是[x,y,z]，重力向量变为了[-0.71,0,-0.71]，注意这个向量是盒子模型坐标系的向量值，转换到世界坐标系仍是[0,0,1].
所以无论盒子怎么旋转重力向量在世界坐标系中都是[0,0,1],但是我们有加速度计读到的值是基于盒子模型坐标系的，也就是我们所说的机体坐标系。现在就明白了，刚才为什么要用四元数将[0，0，1]乘一个旋转矩阵，我们之后关于加速度计和陀螺仪的计算都是基于机体坐标系的。

二、叉乘运算

重点来了，我们得到了加速度计测得的[ax,ay,az]，还有机体坐标系标准的重力向量，下面我们就要对它们做叉积运算：
首先我们先来一点向量叉积的预备知识