传感器应用之ICM20602六轴

传感器应用之
ICM20602六轴

基础知识

坐标系

姿态角（Euler角）

六轴基础

三轴陀螺仪

三轴加速度计

姿态解算之互补滤波

基础知识

互补滤波算
法理论推导

示例代码（基于
i.MX.RT1064逐飞库）

初始化

读取六轴数据

陀螺仪零漂初始化

换算六轴原
始数据单位

互补滤波算法

四元数转换成欧拉角

1 基础知识

1.1 坐标系

• 地面坐标系（earth-surface inertial reference frame）Sg
  
  ①在地面上选一点Og
  ②使xg轴在水平面内并指向某一方向
  ③zg轴垂直于地面并指向地心
  ④yg轴在水平面内垂直于xg轴，其指向按右手定则确定
• 机体坐标系(Aircraft-body coordinate frame) Sb
  
  ①原点O取在飞机质心处，坐标系与飞机固连
  ②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头
  ③y轴垂直于飞机对称平面指向机身右方
  ④z轴在飞机对称平面内，与x轴垂直并指向机身下方

1.2 姿态角（Euler角）

• 俯仰角θ（pitch）：载体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。
  

• 偏航角ψ（yaw）：载体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。
  

• 滚转角Φ（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。
  

1.3 六轴基础

ICM20602是一个六轴运动跟踪装置，它结合了三轴陀螺仪和三轴加速度计

1.3.1 三轴陀螺仪

• 基础介绍
    一般用陀螺仪检测偏航角ψ（yaw），但是它实际检测的是角速度，换算角度时需要对角速度积分，这样就会存在积分误差，误差值与积分时间Dt成正比。此外，陀螺仪还存在零点漂移现象，每次上电时需要重新校准偏移值，后续检测时减去偏移值即可消除此误差。

零点漂移：由于各种原因，陀螺仪上往往被作用有人们所不希望的各种干扰力矩，在这些很小干扰力矩的作用下，陀螺仪的陀螺会产生进动，从而使角动量慢慢偏离原来的方向。

• 规格参数
    从官方规格表可以看出，陀螺仪量程为±250 dps、±500 dps、±1000 dps和±2000 dps（degree per second，即°/s），分别对应的灵敏度为131、65.5、32.8和16.4 LSB/dps。
  量程与灵敏度解释：
    ICM20602数据寄存器是一个16位的，由于最高位是符号位，故数据寄存器的输出范围是-7FFF~7FFF， 即-32767~32767。所以当灵敏度为16.4 LSB/dps、读取的陀螺仪值为32767时，对应的角速度为2000°/s。在姿态解算时，需要把角度换算成弧度，即:$$1°/s=\frac{\pi }{180}=57.3rad/s$$

1.3.2 三轴加速度计

• 基础介绍
    一般用加速度计检测俯仰角θ（pitch），它通过检测器件在各个方向的形变情况而采用得到受力数据，根据F = ma转换，传感器直接输出加速度数据。由于地球存在重力场，重力任何时刻都会作用与传感器，所以静止时检测出加速度为1g，只有当传感器做自由落体运动时才会输出0g。
    加速度计不会区分重力加速度与外力加速度，当物体运动时，它也会在运动的方向上检测出加速度，所以加速度计对振动之类的噪声比较敏感。
• 规格参数
  
    从官方规格表可以看出，加速度计量程为±2 g、±4 g、±8 g和±16 g，分别对应的灵敏度为16384、8192、4096和2048 LSB/g。上述已解释含义，不再赘述。

参考资料：什么是姿态角

2 姿态解算之互补滤波

  加速度计的静态稳定性更好，而在运动时其数据相对不可靠；陀螺仪的动态稳定性更好，但是静止时数据相对不可靠。因此，我们可以短期相信陀螺仪，长期相信加速度计，使用互补滤波算法可以很好的解决这个问题，即通过加速度计的输出来修正陀螺仪的积分累积误差。

2.1 基础知识

• 姿态
    所谓的姿态，就是公式+系数。比如：欧拉角公式和欧拉角的系数（翻滚、倾仰、偏航）。 姿态的数据来源有5个：重力、地磁、陀螺仪、加速度计、电子罗盘。其中前两个来自地面坐标系，后三个来自机体坐标系。
• 四元数
    在复数域里面，二维坐标通过对复数的加减乘除运算可以快速方便地表达出来，尤其是旋转。现在考虑三维空间的复数向量的拉伸和旋转，或者更高维度。那么就需要一个复数域坐标系，容易想到的形式就是h=a+bi+cj，事实证明在二维复数域里面简单添加一元j是无法构成三维复数空间的，实际上需要四个参数才能够构建三维复数空间（两个变量决定轴的方向，一个变量决定旋转角度，一个变量决定伸缩比例），即:$$q=w+x\ast i+y\ast j+z\ast k$$
    为什么姿态解算要用到四元数呢？这是因为欧拉角表示姿态会有万向节死锁问题，且运算比较复杂，所以一般处理数据时会使用四元数，处理完后再把四元数转换成欧拉角。
• 四元数求解
    采用四元数对飞行器进行姿态解算，只需要在每个时钟采样周期循环迭代运算出新的四元数，就可以求得欧拉角信息。在此，构建四元数关于时间的微分方程，来研究四元数的变化规律。
    由于载体的运动，四元数Q是变量，即q0、q1、q2、q3是时间的函数，刚体绕瞬时转轴ve转过θ角，其中上标e代表earth地理坐标系，上标b为机体坐标系，四元数为:
  
    对时间进行微分后可用一阶龙格库塔法求解出四元微分方程，最终得到互补滤波后的四元数，然后将四元数转换为欧拉角。
  此处参考：无人机姿态解算

2.2 互补滤波算法理论推导

参考：四旋翼姿态解算——互补滤波算法及理论推导

3 示例代码（基于i.MX.RT1064逐飞库）

3.1 初始化

笔者使用的是硬件SPI通信，SPI底层代码就不贴出了。

• 陀螺仪自检函数

void icm20602_self3_check(void)
{
    uint8 dat=0;
    while(0x12 != dat)   //读取ICM20602 ID
    {
        icm_spi_r_reg_byte(ICM20602_WHO_AM_I, &dat);
        systick_delay_ms(10);
        //卡在这里原因有以下几点
        //1 ICM20602坏了，如果是新的这样的概率极低
        //2 接线错误或者没有接好
        //3 可能你需要外接上拉电阻，上拉到3.3V
    }
}

说明：陀螺仪初始化成功的话，可以从ICM20602_WHO_AM_I寄存器(0x75)读出8 bits 设备ID（与设备地址不是同一个概念），默认值即可0x12，反之初始化失败。

• 初始化函数

void icm20602_init_spi(void)
{
    uint8 val = 0x0;

    systick_delay_ms(10);  //上电延时
    
    (void)spi_init(SPI_NUM, SPI_SCK_PIN, SPI_MOSI_PIN, SPI_MISO_PIN, SPI_CS_PIN, 3, 10*1000*1000);//硬件SPI初始化

    icm20602_self3_check();//检测
    
    icm_spi_w_reg_byte(ICM20602_PWR_MGMT_1,0x80);//复位设备
    systick_delay_ms(2);
    do
    {//等待复位成功
        icm_spi_r_reg_byte(ICM20602_PWR_MGMT_1,&val);
    }while(0x41 != val);
    
    icm_spi_w_reg_byte(ICM20602_PWR_MGMT_1,     0x01);      //时钟设置
    icm_spi_w_reg_byte(ICM20602_PWR_MGMT_2,     0x00);      //开启陀螺仪和加速度计
    icm_spi_w_reg_byte(ICM20602_CONFIG,         0x01);      //176HZ 1KHZ
    icm_spi_w_reg_byte(ICM20602_SMPLRT_DIV,     0x07);      //! FCHOICE_B[1:0]位为0, 且SMPLRT_DIV > 7，所以不分频。即分频系数不起作用,所以采样频率 = 1K
                                                            //! 采样速率 SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV)
    icm_spi_w_reg_byte(ICM20602_GYRO_CONFIG,    0x18);      //±2000 dps
    icm_spi_w_reg_byte(ICM20602_ACCEL_CONFIG,   0x10);      //±8g
    icm_spi_w_reg_byte(ICM20602_ACCEL_CONFIG_2, 0x03);      //Average 4 samples   44.8HZ   //0x23 Average 16 samples
}

说明：六轴采样频率初始化为1K（采样周期为0.001s），陀螺仪测量范围为±2000 dps ，对应灵敏度为16.4 LSB/dps；加速度计测量范围为±8g， 对应灵敏度为4096 LSB/g。

3.2 读取六轴数据

• 读取陀螺仪

void get_icm20602_accdata_spi(void)
{
    struct
    {
        uint8 reg;
        uint8 dat[6];
    }buf;

    buf.reg = ICM20602_ACCEL_XOUT_H | ICM20602_SPI_R;
    
    icm_spi_r_reg_bytes(&buf.reg, 7);
    icm_acc_x = (int16)(((uint16)buf.dat[0]<<8 | buf.dat[1]));
    icm_acc_y = (int16)(((uint16)buf.dat[2]<<8 | buf.dat[3]));
    icm_acc_z = (int16)(((uint16)buf.dat[4]<<8 | buf.dat[5]));
}

• 读取加速度计数据

void get_icm20602_gyro_spi(void)
{
    struct
    {
        uint8 reg;
        uint8 dat[6];
    }buf;

    buf.reg = ICM20602_GYRO_XOUT_H | ICM20602_SPI_R;
    
    icm_spi_r_reg_bytes(&buf.reg, 7);
    icm_gyro_x = (int16)(((uint16)buf.dat[0]<<8 | buf.dat[1]));
    icm_gyro_y = (int16)(((uint16)buf.dat[2]<<8 | buf.dat[3]));
    icm_gyro_z = (int16)(((uint16)buf.dat[4]<<8 | buf.dat[5]));
}

3.3 陀螺仪零漂初始化

gyro_param_t GyroOffset;               // 陀螺仪校准值 
/**
 * @brief 陀螺仪零漂初始化
 * 通过采集一定数据求均值计算陀螺仪零点偏移值。
 * 后续 陀螺仪读取的数据 - 零飘值，即可去除零点偏移量。
 */
void gyroOffsetInit(void)     
{
    GyroOffset.Xdata = 0;
    GyroOffset.Ydata = 0;
    GyroOffset.Zdata = 0;
    for (uint16_t i = 0; i < 100; ++i) 
	{
        get_icm20602_gyro_spi();    // 获取陀螺仪角速度
        GyroOffset.Xdata += icm_gyro_x;
        GyroOffset.Ydata += icm_gyro_y;
        GyroOffset.Zdata += icm_gyro_z;
        systick_delay_ms(5);    // 最大 1Khz
    }

    GyroOffset.Xdata /= 100;
    GyroOffset.Ydata /= 100;
    GyroOffset.Zdata /= 100;
}

说明：每次上电重新校准陀螺仪零点偏移值，后续读取数据值减去此偏移值即可消除陀螺仪零漂偏差。

3.4 换算六轴原始数据单位

/**
 * @brief 将采集的数值转化为实际物理值, 并对陀螺仪进行去零漂处理
 * 加速度计初始化配置 -> 测量范围: ±8g        对应灵敏度: 4096 LSB/g
 * 陀螺仪初始化配置   -> 测量范围: ±2000 dps  对应灵敏度: 16.4 LSB/dps   (degree per second)
 * @tips: gyro = (gyro_val / 16.4) °/s = ((gyro_val / 16.4) * PI / 180) rad/s
 */
void icmGetValues(void) 
{
    float alpha = 0.3;

    //一阶低通滤波，单位g
    icm_data.acc_x = (((float) icm_acc_x) * alpha) / 4096 + icm_data.acc_x * (1 - alpha);
    icm_data.acc_y = (((float) icm_acc_y) * alpha) / 4096 + icm_data.acc_y * (1 - alpha);
    icm_data.acc_z = (((float) icm_acc_z) * alpha) / 4096 + icm_data.acc_z * (1 - alpha);

    //! 陀螺仪角速度必须转换为弧度制角速度: deg/s -> rad/s
    icm_data.gyro_x = ((float) icm_gyro_x - GyroOffset.Xdata) * PI / 180 / 16.4f; 
    icm_data.gyro_y = ((float) icm_gyro_y - GyroOffset.Ydata) * PI / 180 / 16.4f;
    icm_data.gyro_z = ((float) icm_gyro_z - GyroOffset.Zdata) * PI / 180 / 16.4f;
}

说明：使用一阶低通滤波算法滤除加速度计部分噪声，并将加速度计读取的原始数据除以4096（灵敏度为4096 LSB/g），将其单位换算成g(m/s^2)。前面说过使用互补滤波算法必须将陀螺仪角速度换算成弧度制的角速度，所以将减去零点偏移值的数据(float) icm_gyro_z - GyroOffset.Zdata，先乘以π/180，然后除以16.4（灵敏度为16.4 LSB/dps），单位就从deg/s换算成了rad/s。

3.5 互补滤波算法

#define delta_T     0.001f  // 采样周期1ms 即频率1KHZ
#define PI          3.1415926f

float I_ex, I_ey, I_ez;  // 误差积分
quater_param_t Q_info = {1, 0, 0, 0};  // 四元数初始化
euler_param_t eulerAngle;              // 欧拉角
icm_param_t icm_data;                  // ICM20602采集的六轴数值
float icm_kp= 0.17;    // 加速度计的收敛速率比例增益 
float icm_ki= 0.004;   // 陀螺仪收敛速率的积分增益
/**
 * @brief 用互补滤波算法解算陀螺仪姿态(即利用加速度计修正陀螺仪的积分误差)
 * 加速度计对振动之类的噪声比较敏感，长期数据计算出的姿态可信；陀螺仪对振动噪声不敏感，短期数据可信，但长期使用积分误差严重(内部积分算法放大静态误差)。
 * 因此使用姿态互补滤波，短期相信陀螺仪，长期相信加速度计。
 * @tips: n - 导航坐标系； b - 载体坐标系
 */
void icmAHRSupdate(icm_param_t* icm)            
{
    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;

    // 正常静止状态为-g 反作用力。
    if(icm->acc_x * icm->acc_y * icm->acc_z == 0) // 加计处于自由落体状态时(此时g = 0)不进行姿态解算，因为会产生分母无穷大的情况
        return;

    // 对加速度数据进行归一化 得到单位加速度 (a^b -> 载体坐标系下的加速度)
    float norm = myRsqrt(icm->acc_x * icm->acc_x + icm->acc_y * icm->acc_y + icm->acc_z * icm->acc_z); 
    icm->acc_x = icm->acc_x * norm;
    icm->acc_y = icm->acc_y * norm;
    icm->acc_z = icm->acc_z * norm;

    // 载体坐标系下重力在三个轴上的分量
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    // g^b 与 a^b 做向量叉乘，得到陀螺仪的校正补偿向量e的系数
    ex = icm->acc_y * vz - icm->acc_z * vy;
    ey = icm->acc_z * vx - icm->acc_x * vz;
    ez = icm->acc_x * vy - icm->acc_y * vx;

    // 误差累加
    I_ex += halfT * ex;  
    I_ey += halfT * ey;
    I_ez += halfT * ez;

    // 使用PI控制器消除向量积误差(陀螺仪漂移误差)
    icm->gyro_x = icm->gyro_x + icm_kp* ex + icm_ki* I_ex;
    icm->gyro_y = icm->gyro_y + icm_kp* ey + icm_ki* I_ey;
    icm->gyro_z = icm->gyro_z + icm_kp* ez + icm_ki* I_ez;

    // 一阶龙格库塔法求解四元数微分方程，其中halfT为测量周期的1/2，gx gy gz为b系陀螺仪角速度。
    q0 = q0 + (-q1 * icm->gyro_x - q2 * icm->gyro_y - q3 * icm->gyro_z) * halfT;
    q1 = q1 + (q0 * icm->gyro_x + q2 * icm->gyro_z - q3 * icm->gyro_y) * halfT;
    q2 = q2 + (q0 * icm->gyro_y - q1 * icm->gyro_z + q3 * icm->gyro_x) * halfT;
    q3 = q3 + (q0 * icm->gyro_z + q1 * icm->gyro_y - q2 * icm->gyro_x) * halfT;

    // 单位化四元数在空间旋转时不会拉伸，仅有旋转角度，下面算法类似线性代数里的正交变换
    norm = myRsqrt(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;  // 用全局变量记录上一次计算的四元数值
}

说明：关于互补滤波算法的理论推导可参考上文链接里的博客。在此说明几个参数。halfT为采样周期的一半，初始化周期为0.001s，所以该值为0.0005s。icm_kp与icm_ki为位置式PI控制器的比例系数和积分系数，通过调节这两个参数补偿陀螺仪零点漂移。

3.6 四元数转换成欧拉角

    float q0 = Q_info.q0;
    float q1 = Q_info.q1;
    float q2 = Q_info.q2;
    float q3 = Q_info.q3;
    // atan2返回输入坐标点与坐标原点连线与X轴正方形夹角的弧度值
    eulerAngle.pitch = asin(2 * q0 * q2 - 2 * q1 * q3) * 180 / PI; 
    eulerAngle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / PI; 
    eulerAngle.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / PI;  

后续更新说明：加速计修正横滚角与俯仰角有不错的效果，但长期修正偏航角时会存在累积误差，可以加地磁计来补偿。

END