详解互补滤波四元数中向量叉积与陀螺仪角速度补偿问题（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]，还有机体坐标系标准的重力向量，下面我们就要对它们做叉积运算：
首先我们先来一点向量叉积的预备知识

由上面知识我们得到了下面的计算矩阵：
⎡⎣⎢⎢exeyez⎤⎦⎥⎥=⎡⎣⎢⎢0az−ay−az0axay−ax0⎤⎦⎥⎥⎡⎣⎢⎢vxvyvz⎤⎦⎥⎥

 ex = ay*vz - az*vy ;
 ey = az*vx - ax*vz ;
 ez = ax*vy - ay*vx ;

得到的这个向量叉积就是姿态误差向量，Mahony论文中是这样说的： the error e is the relative rotation between the measured inertial direction v¯ and the predicted direction v̂  .翻译过来就是：误差e就是测量得到的 v¯ 和预测得到的 v̂  之间的相对旋转。这里的 v¯ 就是[ax,ay,az]’, v̂  就是[vx,vy,vz]’。
怎么理解这句话呢？
通过整个程序的流程我们知道欧拉角是用更新后的四元数解得的，设是t时刻更新后的四元数为Q(t)，假定在t+T时刻（下一时刻）Q(t+T)=Q(t),即用上一时刻的四元数预测当前时刻的四元数仍为Q(t)。然后将加速度计测得的值归一化后与预测四元数表示的重力向量做叉积运算。

这张图上从Mahony论文中截取的，从中也可以看出，归一化后的加速度计值与上一时刻预测得到的四元数做叉积运算，只要机体存在旋转也就是说只要机体姿态发生偏移，叉积结果必然不为0，那么该“误差”值就会一直做PI运算对陀螺仪进行补偿，直到机体停止旋转。若停止旋转后叉积值仍不为0，就说明我们预测的值不正确，同样会进行PI互补滤波重新对当前时刻进行预测，直到预测值收敛到加速度计值。
总结为二点：
1、机体做旋转运动时，叉积结果就是当前时刻加速度计结果相对于上一时刻重力向量的旋转和测量结果误差的叠加。
2、机体静止时，没有旋转；叉积结果若不为0，说明预测值有误差，因而就会一直做PI运算，直到结果收敛到加速度计归一化值。

三、分析为什么叉积运算结果能对陀螺仪角速度进行补偿？

一、分析公式
两个向量的叉积得到的结果是两个向量的模与他们之间夹角正弦的乘积 a×v=|a||v|sinθ ,加速度计测量得到的重力向量和预测得到的机体重力向量（前面已经详细解释了这个是由世界坐标系下的[0,01]旋转得到的）已经经过单位化，因而他们的模是1，也就是说它们向量的叉积结果仅与 sinθ 有关，当角度很小时，叉积结果可以近似于角度成正比。
二、物理意义
这个进行叉积得到的误差结果近似等于当期时刻相对于上一时刻旋转的角度，这个角度即是旋转角度和预测误差角度的叠加，总称为向量误差，角度越大，误差越大。这里是不是很神奇，我们用加速度计的值间接得出了误差变化的角度相关值。
三、陀螺仪误差由来

陀螺仪由于本身精度问题，测量的角速度存在误差，在积分过程中这个误差会一直累加，我们要做的就是去消除或是补偿这个误差，因为加速度计长期的测量值是准确的，所以可以用加速度计来进行修正。
四、如何修正
void IMUupdate(float gx, float gy, float gz, float ax,float ay, float az){}
如何找到一个另一个角速度量纲的值来修正陀螺仪的角速度值呢？这里明明只有陀螺仪可以测量角速度！！！
这时候前面提到的向量叉积得到的误差向量就帮上大忙了，这个误差向量不就是反映出了角度变化量吗。算法巧妙的将加速度相关量转化为角度相关量，因而可以用这个角度值乘一个系数来修正陀螺仪的角速度，因为在偏差角度很小的情况下，我们可以将陀螺仪角速度误差和加速度计求得的角度差看做正比的关系，也就说明陀螺仪积分误差和向量叉积存在正比关系。

ω=ω+kp∗θacc
最后得到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;

其中：
1、 比例增益kp控制收敛到加速度计速率
2、 积分增益ki控制陀螺仪偏差的收敛速率
有人会问，为什么不能直接将预测的重力向量和测量得到的重力向量直接相减呢?上面已经解释了用叉积运算时有相应的物理意义的，二直接相减得到的向量值与角度无关，是不能用来补偿角速度的。
啰啰嗦嗦这么多，到这里就算写完了，若有不当之处，恳请指正！