加速计陀螺仪6轴数据融合算法解析

  四旋翼和云台都离不开加速计和陀螺仪。加速计提供实时的重力加速度方向，进而可以计算出机体姿态角度。陀螺仪提供实时的旋转角度，通过积分也能换算成机体姿态角度。加速计对机体振动非常敏感，稍有扰动，加速度方向就会偏移（外力与重力的合力），但是长期运行时，经过平滑滤波算法之后的加速计数据就比较可信。相比而言，陀螺仪则对外部扰动不敏感，反而是长时间积分后会出现角度漂移。因此实际应用中，都是将二者采集的数据进行融合，相互取长补短。

  下面这段代码是网上比较常见的6轴融合算法，原作者只添加了几句简单的英文注释，几处关键段落阅读起来非常费解。而网上的大多数引用页面对这段代码也都没有做进一步的分析，我怀疑大家也都是一知半解，拿来就用，用得顺手便没有再去深究。这个周末我仔细研究了一下，对各关键公式进行了理论推导，供大家参考。三个关键段落的推导过程详见后文Note1，Note2，Note3。

void IMUupdate() {
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;
        float ax,ay,az,gx,gy,gz;
        float q0,q1,q2,q3;
        float q0temp,q1temp,q2temp,q3temp;

        ax=Data.pSensor->accel[0];
        ay=Data.pSensor->accel[1];
        az=Data.pSensor->accel[2];

        gx=Data.pSensor->gyro[0];
        gy=Data.pSensor->gyro[1];
        gz=Data.pSensor->gyro[2];

        q0=Data.q[0];
        q1=Data.q[1];
        q2=Data.q[2];
        q3=Data.q[3];

        // normalise the measurements
        norm = sqrt(ax*ax + ay*ay + az*az);
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;

        //*Note1
        // estimated direction of gravity
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

        //*Note2
        // error is sum of cross product between reference direction of field and direction measured by sensor
        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);

        // integral error scaled integral gain
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;

        // adjusted gyroscope measurements， PID
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;

        q0temp=q0;
        q1temp=q1;
        q2temp=q2;
        q3temp=q3;

        //*Note3
        // integrate quaternion rate and normalise
        q0 = q0temp + (-q1temp *gx - q2temp *gy - q3temp *gz)*halfT;
        q1 = q1temp + (q0temp *gx + q2temp *gz - q3temp *gy)*halfT;
        q2 = q2temp + (q0temp *gy - q1temp *gz + q3temp *gx)*halfT;
        q3 = q3temp + (q0temp *gz + q1temp *gy - q2temp *gx)*halfT;

        // normalise quaternion
        norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 * norm;
        q1 = q1 * norm;
        q2 = q2 * norm;
        q3 = q3 * norm;

        Data.q[0]=q0;
        Data.q[1]=q1;
        Data.q[2]=q2;
        Data.q[3]=q3;
}

*Note1

*Note2

 

*Note3