基于icm20609姿态更新计算的四元数算法

前言

在目前的六轴姿态传感器中，网上对于icm系列的贴子相对于mpu系列要少很多，而icm20609的贴子为零。

在官网上对于MPU系列（和一款icm系列芯片）已经不做推荐了

我这里也在官网上看了下icm20609和icm20602的区别，因为这篇贴子主要是根据icm20602的讲解所改动的。

可以看出，icm20609具有超薄封装以及4K的FIFO，而icm20602更低功耗，OK，本帖不用内置的dmp，因此不回去用FIFO，所以实际上我觉得差不多。

 

问题：为什么不直接转化成欧拉角来表示而要引入四元数呢?

一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时解算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“GimbalLock”。

所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。

四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。

 

四元数是一种超复数。如把四元数的集合考虑成多维实数空间的话，四元数就代表

k i j 着一个四维空间，相对于复数为二维空间。

简而言之，四元数包含了刚体旋转的所有信息，而在四旋翼飞行器的姿态解算中，往往使用的是四元数微分方程对四元数进行更新

 

 

正文

1、首先看看icm20609的配置参数

参考代码:

https://github.com/golaced/Oldx_fly_controller/blob/master/firmware/Back_FC/applications/icm20602.c

int8_t icm20609_init_per20ms(void)
{
    int8_t icm20609_init_returnvalue = 0xff;
    u8 offset_and_data[2];
    switch (icm20609_init_State)
    {
        case icm20609_init_pre_state:
            icm20609_ReadID();
            icm20609_init_State = icm20609_init_step1_state;
            break;
        case icm20609_init_step1_state:
            if (icm20609_init_delay == 0x00)
            {
                icm20609_init_delay++;
                Icm20609acceptdata.PWR_MGMT_1_REG = 0x00;
                ICM20609_WriteReg(ICM20609_PWR_MGMT_1_REG, 0x80);
            }
            else
            {
                do {
                    ICM20609_ReadReg(ICM20609_PWR_MGMT_1_REG, &Icm20609acceptdata.PWR_MGMT_1_REG);
                }while(Icm20609acceptdata.PWR_MGMT_1_REG & 0x80);
                
                icm20609_init_delay = 0;
                icm20609_init_State = icm20609_init_step2_state;
            }
            break;

        case icm20609_init_step2_state:
            icm20609_init_delay++;
            switch (icm20609_init_delay)
            {
                case 0x01:
                    icm20609_power_on();//ICM20609_PWR_MGMT_1_REG set 0x01
                    break;
                case 0x02:
                    /*Reset digital signal path*/
                    ICM20609_WriteReg(ICM20609_SIGNAL_PATH_RESET_REG, 0x03);
                    break;
                case 0x03:
                    /*clears all the sensor registers*/
                    ICM20609_WriteReg(ICM20609_USER_CTRL_REG, 0x01);
                    break;
                case 0x04:
                    ICM20609_WriteReg(ICM20609_PWR_MGMT_2_REG,0x00);
                    break;
                case 0x05:
                    icm20609_set_odr(200);  //200 Hz  1000/200-1
                    break;
                case 0x06:    
                    ICM20609_WriteReg(ICM20609_CONFIG_REG,IAM20609_GYRO_BW_20HZ); //gtro and temp LPF
                    break;
                case 0x07:
                    icm20609_set_gyro_fsr((ICM20609_GYRO_FSR_2000 << 3));  //+-2000dps
                    break;
                case 0x08:
                    icm20609_set_accel_fsr((ICM20609_ACCEL_FSR_8G << 3));//+-8g
                    break;
                case 0x09:
                    ICM20609_WriteReg(ICM20609_ACCEL_CONFIG2_REG, ICM20609_ACCEL_BW_21HZ);/*accel LPF 20HZ*/
                    break;
                case 0x0A:
                    ICM20609_WriteReg(ICM20609_LP_MODE_CFG_REG, 0x00);/*关闭低功耗*/
                    break;
                case 0x0B:
                    ICM20609_WriteReg(ICM20609_FIFO_EN_REG, 0x00);/*关闭FIFO*/
                    icm20609_init_State = icm20609_init_finish_state;
                    break;
                default:
                    break;
            }
            break;
        case icm20609_init_finish_state:
            icm20609_init_returnvalue = 0x01;
            break;
        default:
            break;
    }

    return iam20609_init_returnvalue;
}

其实最主要的是通过ICM20609_PWR_MGMT_1_REG寄存器reset芯片，并确认是否reset了；接着就是对采样频率和陀螺仪和加速度计的量程进行设置；最后是对LPF（低通滤波）寄存器进行设置。

2、自检和校准

有需要的话可以参考这篇帖子：https://blog.csdn.net/yyw_0429/article/details/86659332

这份参考贴并没有贴出所有代码，这里就写一下原理：

自检：

• 芯片处于no motion状态，读取加速度计和陀螺仪输出数据，计算平均值；
• GYRO_CONFIG寄存器和ACCEL_CONFIG寄存器使能自检，此时芯片内部会自动模拟外力 施加给加速度计和陀螺仪，此时输出的加速度计和陀螺仪数据相较不使能自检状态会有所变化，计算平均值；
• 读取自检寄存器输出数据，获得厂家出厂时固化的OTP；
• 最后直接使用驱动程序中给出的转化表和输出值进行比较，来判断自检是否通过

 

校准：

• 读取初始状态下加速度计输出值，计算平均值
• 使能校准
• 读取出厂时XA_OFFS寄存器里的校准值
• 校准值减去平均值后写入XA_OFFS寄存器

 

这里并没有进行自检和校准，不再赘述。

 

3、数据采集

• 这里使用软件IIC来进行采集原始数据。
• 采样率是200HZ，程序中也是每5ms采集并更新一次姿态
• 陀螺仪的范围选择±2000，而对应的精度是16.4 LSB/(°/s)，而在四轴姿态计算中，我们通常要把角度换算成弧度。我们知道2Pi代表360度，那么1度换算成弧度就是： 2Pi/360=(2*3.1415926)/360=0.0174532=1/57.30。
• 加速度计选择的量程是±8G，精度为4096LSB/g

 

软件iic进行读取原始数据：

int8_t icm20609_accelAndGyro_read(accel_t *accel, gyro_t *gyro, int16_t temp)
{
    u8  reg_data[14];
    ICM20609_ReadData(ICM20609_ACCEL_XOUT_H_REG, (u8 *)®_data, 14);
    
    accel->x = (int16_t)(reg_data[0] << 8 | reg_data[1]);
    accel->y = (int16_t)(reg_data[2] << 8 | reg_data[3]);
    accel->z = (int16_t)(reg_data[4] << 8 | reg_data[5]);
    temp     = (int16_t)(reg_data[6] << 8 | reg_data[7]);
    gyro->x  = (int16_t)(reg_data[8] << 8 | reg_data[9]);
    gyro->y  = (int16_t)(reg_data[10] << 8 | reg_data[11]);
    gyro->z  = (int16_t)(reg_data[12] << 8 | reg_data[13]);
    return 0x01;
}

 

将原始数据进行转换并传给姿态解算的函数：

//get accel and gyro from iam20609 
// 对accel一阶低通滤波(参考匿名)，对gyro转成弧度每秒(2000dps)
#define new_weight           0.35f
#define old_weight           0.65f
void IMU_getValues(float * values, accel_t * accelval, gyro_t * gyroval) 
{  
    static float lastaccel[3]= {0,0,0};
    int i;
    values[0] = ((float)accelval->x) * new_weight + lastaccel[0] * old_weight;
    values[1] = ((float)accelval->y) * new_weight + lastaccel[1] * old_weight;
    values[2] = ((float)accelval->z) * new_weight + lastaccel[2] * old_weight;
    for(i=0; i<3; i++)
    {
        lastaccel[i] = values[i];
    }

    values[3] = ((float)gyroval->x) * M_PI / 180 / 16.4f;
    values[4] = ((float)gyroval->y) * M_PI / 180 / 16.4f;
    values[5] = ((float)gyroval->z) * M_PI / 180 / 16.4f;

    //
}

 

4、姿态解算

按照 原始数据->四元数->欧拉角的方式进行姿态解算并从匿名上位机中进行观察

#define delta_T      0.005f  //5ms计算一次

float I_ex, I_ey, I_ez;  // 误差积分
quaterInfo_t Q_info;  // 全局四元数
eulerianAngles_t eulerAngle; //欧拉角
float param_Kp = 50.0;   // 加速度计(磁力计)的收敛速率比例增益50 
float param_Ki = 0.20;   //陀螺仪收敛速率的积分增益 0.2

/**
  * brief IMU_AHRSupdate_noMagnetic  姿态解算融合，是Crazepony和核心算法
  * 使用的是互补滤波算法，没有使用Kalman滤波算法
  * param float gx, float gy, float gz, float ax, float ay, float az
  *
  * return None
  */
static void IMU_AHRSupdate_noMagnetic(float gx, float gy, float gz, float ax, float ay, float az)
{
    float halfT = 0.5 * delta_T;
    float vx, vy, vz;    //当前的机体坐标系上的重力单位向量
    float ex, ey, ez;    //四元数计算值与加速度计测量值的误差
    float q0 = Q_info.q0;
    float q1 = Q_info.q1;
    float q2 = Q_info.q2;
    float q3 = Q_info.q3;
    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;
    float delta_2 = 0;

• 输入参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒。
• 输入参数ax，ay，az分别对应三个轴的加速度数据，由于加速度的噪声较大，在读取的时候该数据是采用低通滤波。

当电子罗盘数据有效的时候，需要融合电子罗盘的数据。这里没有使用磁力计，所以只进行加速度数据融合。

    //对加速度数据进行归一化 得到单位加速度
    float norm = invSqrt(ax*ax + ay*ay + az*az);       
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;

对加速度数据进行归一化，得到单位加速度。这里介绍一种快速计算1/sqrt(x)的函数：

float invSqrt(float x) {
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

    vx = 2*(q1q3 - q0q2);
    vy = 2*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

把飞行器上次计算得到的姿态（四元数）换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx、vy、vz，其实就是用上一次飞行器机体姿态（四元数）换算出来的在当前的机体坐标系上的重力单位向量。

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

在机体坐标系上，加速度计测出来的重力向量是ax、ay、az。由上次解算的姿态推算出的重力向量是vx、vy、vz。它们之间的误差向量，就是上次姿态和加速度计测出来的姿态之间的误差。

向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

所以上面一段代码含义就是获得叉乘误差。

    //用叉乘误差来做PI修正陀螺零偏，
    //通过调节 param_Kp，param_Ki 两个参数，
    //可以控制加速度计修正陀螺仪积分姿态的速度。
    I_ex += delta_T * ex;   // integral error scaled by Ki
    I_ey += delta_T * ey;
    I_ez += delta_T * ez;

    gx = gx+ param_Kp*ex + param_Ki*I_ex;
    gy = gy+ param_Kp*ey + param_Ki*I_ey;
    gz = gz+ param_Kp*ez + param_Ki*I_ez;

上面一段代码，叉乘误差进行积分。用叉乘误差来做PI修正陀螺零偏，通过调节param_Kp，param_Ki 两个参数，可以控制加速度计修正陀螺仪积分姿态的速度。

到此为止，使用加速度计数据对陀螺仪数据的修正已经完成，这就是姿态解算中的深度融合。

下面就是四元数微分方程，使用修正后的陀螺仪数据对时间积分，得到飞行器的当前姿态（四元数表示）。然后进行四元数的单位化处理。

    //四元数微分方程，其中halfT为测量周期的1/2，gx gy gz为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程
    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;

//    delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);
    // 整合四元数率    四元数微分方程  四元数更新算法，二阶毕卡法
//    q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT;			
//    q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;
//    q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;
//    q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT;	

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

 

 

这里贴一下融合磁力计的四元数更新算法：

static void IMU_AHRSupdate_withMagnetic(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
    float norm;
    float halfT = 0.5 * delta_T;
    float hx, hy, hz;
    float bx, bz;
    float vx, vy, vz;
    float wx, wy, wz;
    float ex, ey, ez;
    float delta_2 = 0;
    float q0 = Q_info.q0;
    float q1 = Q_info.q1;
    float q2 = Q_info.q2;
    float q3 = Q_info.q3;
    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;

    norm = invSqrt(ax*ax + ay*ay + az*az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;

    norm = invSqrt(mx*mx + my*my + mz*mz);          
    mx = mx * norm;
    my = my * norm;
    mz = mz * norm;

    // compute reference direction of flux 通量的计算参考方向
    hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
    hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
    hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);         
    bx = sqrt((hx*hx) + (hy*hy));
    bz = hz;     

    // estimated direction of gravity and flux (v and w)
    vx = 2*(q1q3 - q0q2);				  	// 估计方向的重力
    vy = 2*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;
    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

    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);

    if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
    {

        I_ex += delta_T * ex;	// integral error scaled by Ki
        I_ey += delta_T * ey;
        I_ez += delta_T * ez;
        //exdev=(ex[1]-ex[0]) / halfT;
        //eydev=(ey[1]-ey[0]) / halfT;
        // ezdev=(ez[1]-ez[0]) / halfT;
        // adjusted gyroscope measurements

        gx = gx+ param_Kp*ex + param_Ki*I_ex;//+ Kd*exdev;
        gy = gy+ param_Kp*ey + param_Ki*I_ey;//+ Kd*eydev;
        gz = gz+ param_Kp*ez + param_Ki*I_ez;//+ Kd*ezdev;
    }
    delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);
    // 整合四元数率    四元数微分方程  四元数更新算法，二阶毕卡法
    q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT;			
    q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT;	
    // normalise quaternion
    norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    Q_info.q0 = q0 * norm;
    Q_info.q1 = q1 * norm;
    Q_info.q2 = q2 * norm;
    Q_info.q3 = q3 * norm;
}

 

5、通过四元数计转换欧拉角

根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角

void IMU_quaterToEulerianAngles()
{
    float q0 = Q_info.q0;
    float q1 = Q_info.q1;
    float q2 = Q_info.q2;
    float q3 = Q_info.q3;
    eulerAngle.pitch = asin(-2*q1*q3 + 2*q0*q2) * 180/M_PI; // pitch
    eulerAngle.roll = atan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1) * 180/M_PI; // roll
    eulerAngle.yaw = atan2(2*q1*q2 + 2*q0*q3, -2*q2*q2 - 2*q3*q3 + 1) * 180/M_PI; // yaw

/*   可以不用作姿态限度的限制
    if(eulerAngle.roll>90 || eulerAngle.roll<-90)
    {
        if(eulerAngle.pitch > 0)
        {
            eulerAngle.pitch = 180-eulerAngle.pitch;
        }
        if(eulerAngle.pitch < 0)
        {
            eulerAngle.pitch = -(180+eulerAngle.pitch);
        }
    }
    if(eulerAngle.yaw > 180)
    {
        eulerAngle.yaw -=360;
    }
    else if(eulerAngle.yaw <-180)
    {
        eulerAngle.yaw +=360;
    } 
    */
}

 

最后感谢：

http://www.crazepony.com/wiki/software-algorithm.html