imuncle commented 5 years ago

前段时间尝试使用陀螺仪作为机器人云台的位置环反馈值，在底盘跟随的大前提下，这样做是更符合第一视角的操作的。

我去年比赛的时候买了几个维特智能的型号为WT901的九轴，当时时间紧没有用上，结果不用不知道，一用吓一跳，陀螺仪延迟高的吓人，使用J-Scope看到的图像如下：

从图中可以看出延迟大约有200ms，这种延时已经不再具备方位参考值了。

我在仓库里东翻西找，找到了两个三十多块的MPU9250，相比于我买的一百来块的陀螺仪来说便宜了很多，于是我开始尝试手动解析数据。

读取原始数据

MPU9250内部其实是由6轴陀螺仪MPU6500和磁力计AK8963组成，可以测量三轴加速度，三轴角速度和三轴磁场强度，此外还可以测量温度，但这里我没有用到温度的数据。

数据解析的第一步就是读取原始数据。MPU6500支持IIC通信和SPI通信，AK8963只支持IIC通信，但可以通过MPU9250为中介使用SPI通信，只是比较麻烦，所以这里我选择了IIC通信。

IIC地址

MPU9250的IIC地址取决于AD0口的电平高低，如果AD0接GND，则地址为0x68，如果接VCC，则地址为0x69,。

这里说一下一个坑：

在读取时，注意需要人工将地址左移1位（I2C读写为左对齐，第8位要存读写标志位）

最开始我不知道这个，一直把地址写成0x68，结果死活读不到数据，后来才发现要写成0xD0。

MPU9250的其他地址可以查看它的寄存器数据手册，我这里列出了我用到的一部分寄存器：

#define MPU9250_ADDRESS 0xD0   //AD0接GND时地址为0x68，接VCC时地址为0x69
#define MPU_PWR_MGMT1_REG       0X6B    //电源管理寄存器1
#define MPU_GYRO_CFG_REG        0X1B    //陀螺仪配置寄存器
#define MPU_ACCEL_CFG_REG       0X1C    //加速度计配置寄存器
#define MPU_SAMPLE_RATE_REG     0X19    //陀螺仪采样频率分频器
#define MPU_INT_EN_REG          0X38    //中断使能寄存器
#define MPU_USER_CTRL_REG       0X6A    //用户控制寄存器
#define MPU_FIFO_EN_REG         0X23    //FIFO使能寄存器
#define MPU_INTBP_CFG_REG       0X37    //中断/旁路设置寄存器
#define MPU_DEVICE_ID_REG       0X75    //器件ID寄存器
#define MPU_PWR_MGMT2_REG       0X6C    //电源管理寄存器2
#define MPU_CFG_REG             0X1A    //配置寄存器 低通滤波器配置寄存器

#define MPU_TEMP_OUTH_REG       0X41    //温度值高8位寄存器
#define MPU_TEMP_OUTL_REG       0X42    //温度值低8位寄存器

#define MPU_ACCEL_XOUTH_REG     0X3B    //加速度值，X轴高8位寄存器
#define MPU_ACCEL_XOUTL_REG     0X3C    //加速度值，X轴低8位寄存器
#define MPU_ACCEL_YOUTH_REG     0X3D    //加速度值，Y轴高8位寄存器
#define MPU_ACCEL_YOUTL_REG     0X3E    //加速度值，Y轴低8位寄存器
#define MPU_ACCEL_ZOUTH_REG     0X3F    //加速度值，Z轴高8位寄存器
#define MPU_ACCEL_ZOUTL_REG     0X40    //加速度值，Z轴低8位寄存器

#define MPU_GYRO_XOUTH_REG      0X43    //陀螺仪值，X轴高8位寄存器
#define MPU_GYRO_XOUTL_REG      0X44    //陀螺仪值，X轴低8位寄存器
#define MPU_GYRO_YOUTH_REG      0X45    //陀螺仪值，Y轴高8位寄存器
#define MPU_GYRO_YOUTL_REG      0X46    //陀螺仪值，Y轴低8位寄存器
#define MPU_GYRO_ZOUTH_REG      0X47    //陀螺仪值，Z轴高8位寄存器
#define MPU_GYRO_ZOUTL_REG      0X48    //陀螺仪值，Z轴低8位寄存器

#define AK8963_ADDRESS 0x18     //磁力计地址0x0C
#define AK8963_CNTL1        0x0A  //磁力计读取寄存器
#define AK8963_HXL          0x03    //磁力计X轴低8位寄存器
#define AK8963_HXH          0x04    //磁力计X轴高8位寄存器
#define AK8963_HYL          0x05    //磁力计Y轴低8位寄存器
#define AK8963_HYH          0x06    //磁力计Y轴高8位寄存器
#define AK8963_HZL          0x07    //磁力计Z轴低8位寄存器
#define AK8963_HZH          0x08    //磁力计Z轴高8位寄存器

配置MPU9250

读取数据之前要对MPU9250进行一系列的配置，比如设置采样频率，设置量程等等，具体的如下：

void MPU9250_Init(void)
{
    unsigned char pdata;
    //检查设备是否准备好
    HAL_I2C_IsDeviceReady(&hi2c1, MPU9250_ADDRESS, 10, HAL_MAX_DELAY);
    //检查总线是否准备好
    HAL_I2C_GetState(&hi2c1);

    pdata=0x80; //复位MPU
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_PWR_MGMT1_REG, 1, &pdata, 1, HAL_MAX_DELAY);
    HAL_I2C_IsDeviceReady(&hi2c1, MPU9250_ADDRESS, 10, HAL_MAX_DELAY);

    HAL_Delay(500);  //复位后需要等待一段时间，等待芯片复位完成

    pdata=0x01; //唤醒MPU
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_PWR_MGMT1_REG, 1, &pdata, 1, HAL_MAX_DELAY);

    pdata=3<<3; //设置量程为2000
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_GYRO_CFG_REG, 1, &pdata, 1, HAL_MAX_DELAY); 

    pdata=01;   //设置加速度传感器量程±4g
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_ACCEL_CFG_REG, 1, &pdata, 1, HAL_MAX_DELAY); 

    pdata=0; //陀螺仪采样分频设置
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_SAMPLE_RATE_REG, 1, &pdata, 1, HAL_MAX_DELAY); 

    pdata=0;    //关闭所有中断
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_INT_EN_REG, 1, &pdata, 1, HAL_MAX_DELAY); 

    pdata=0;    //关闭FIFO
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_FIFO_EN_REG, 1, &pdata, 1, HAL_MAX_DELAY); 

    pdata=0X02; //设置旁路模式，直接读取AK8963磁力计数据
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_INTBP_CFG_REG, 1, &pdata, 1, HAL_MAX_DELAY); 
    HAL_Delay(10);  //需要一段延时让磁力计工作

    pdata = 4;  //设置MPU9250的数字低通滤波器
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_CFG_REG, 1, &pdata, 1, HAL_MAX_DELAY);

    pdata=0;    //使能陀螺仪和加速度工作
    HAL_I2C_Mem_Write(&hi2c1, MPU9250_ADDRESS, MPU_PWR_MGMT2_REG, 1, &pdata, 1, HAL_MAX_DELAY);

    pdata = 0x01;
    HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS, AK8963_CNTL1, 1, &pdata, 1, HAL_MAX_DELAY);
    HAL_Delay(10);
}

读取数据

配置好了之后就可以读取数据了，这就很简单了，直接从指定寄存器读取就可以了。这里要注意的是磁力计不能读取太频繁，AK8963本身的数据采样就有一个硬件上限，大约200Hz左右，但实测只能以一百多赫兹的频率读取。且每次读取都需要往磁力计的CNTL1寄存器写1，否则会读不到数据。

读取数据的具体步骤如下：

void GetImuData(void)
{
    uint8_t imu_data[14]={0};
    uint8_t mag_data[6] = {0};
    uint8_t pdata;
    short accx,accy,accz;
    short gyrox,gyroy,gyroz;
    short magx,magy,magz;

    float gyro_sensitivity = 16.384f;
    int acc_sensitivity = 8192;

    static short mag_count = 0;

    HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDRESS, MPU_ACCEL_XOUTH_REG, 1, imu_data, 14, HAL_MAX_DELAY); //读取陀螺仪和加速度计的数据
    accx = (imu_data[0]<<8)|imu_data[1];
    accy = (imu_data[2]<<8)|imu_data[3];
    accz = (imu_data[4]<<8)|imu_data[5];
    gyrox = (imu_data[8]<<8)|imu_data[9];
    gyroy = (imu_data[10]<<8)|imu_data[11];
    gyroz = (imu_data[12]<<8)|imu_data[13];

    mpu9250.gyro.x = (float)(gyrox-GYROX_BIAS)/gyro_sensitivity;
    mpu9250.gyro.y = (float)(gyroy-GYROY_BIAS)/gyro_sensitivity;
    mpu9250.gyro.z = (float)(gyroz-GYROZ_BIAS)/gyro_sensitivity;

    mpu9250.acc.x = (float)(accx-ACCX_BIAS)/acc_sensitivity;
    mpu9250.acc.y = (float)(accy-ACCY_BIAS)/acc_sensitivity;
    mpu9250.acc.z = (float)(accz-ACCZ_BIAS)/acc_sensitivity;

    mag_count++;
    if(mag_count == 10) //磁力计不能读取太频繁
    {
        HAL_I2C_Mem_Read(&hi2c1, AK8963_ADDRESS, AK8963_HXL, 1, mag_data, 6, HAL_MAX_DELAY);    //读取磁力计数据
        magx = (mag_data[0]<<8)|mag_data[1];
        magy = (mag_data[2]<<8)|mag_data[3];
        magz = (mag_data[4]<<8)|mag_data[5];
        mpu9250.mag.x = (float)magy/1000.0f;        //磁力计的坐标方位不同
        mpu9250.mag.y = (float)magx/1000.0f;
        mpu9250.mag.z = -(float)magz/1000.0f;
        pdata = 1;
        HAL_I2C_Mem_Write(&hi2c1, AK8963_ADDRESS, AK8963_CNTL1, 1, &pdata, 1, HAL_MAX_DELAY);   //为下一次读取磁力计数据做准备
        mag_count = 0;
    }
}

这个函数的执行为1kHz，也就是加速度和陀螺仪数据采样频率是1kHz，磁力计采样频率为100Hz。

姿态解析

光拿到这九轴的数据是不行的，对我们有用的是欧拉方位角，那么拿到数据之后怎么才能得到欧拉角呢？使用四元数就能解决这个问题。

四元数

说实话四元数真不太容易理解，我现在也还是一脸懵逼，但不用慌，我们只需要掌握四元数与欧拉角之间的转换就行了，公式如下：

对四元数本身感兴趣的可以看这个视频（地址），看过之后我还是不太懂。。。

那么怎么根据我们现有的九轴数据求解四元数呢？

其实都用不到九轴，我们只需要用到陀螺仪的数据，也就是角速度就可以求解到四元数，计算公式如下：

（这里利用一阶龙格库塔公式进行积分求解四元数）

也就是说姿态解析中陀螺仪才是主角，我们通过陀螺仪积分得到角度，但是时间久了陀螺仪会产生误差，也就是漂移，时间短还可以忍受，时间一长误差就很大了，所以需要加速度和磁力计来纠正陀螺仪的数据。

互补滤波

加速度计主要测量的是地球的重力加速度，根据重力加速度在三个轴方向的分量可以推算出当前物体的姿态。磁力计也是相同的道理。

加速度计可以纠正pitch轴和roll轴的角度误差，因为航向角不管指向何方，加速度的读数都是一样的，所以需要磁力计来纠正yaw轴的角度误差。

那么怎么纠正呢？我这里使用的是互补滤波。

因为我们要求的是物体坐标相对于地面坐标的三轴偏转角度，所以要把加速度的各个分量（在物体的坐标系下）转换为在地面坐标的分量。进而得到物体的方位，因为陀螺仪本身有误差，所以由陀螺仪计算出来的方位肯定与加速度计算出来的方位有误差，这个误差用向量的叉乘表示，将这个误差叠加在陀螺仪计算出来的结果上就可以抵消误差，进而达到纠正角度的作用。磁力计同上。

这里使用互补滤波来进行纠正。公式如下：

这里构造了一个比例-积分控制器，有一个比例系数和一个积分系数。比例系数越大，表示越信任加速度计和磁力计，积分系数越大，表示越信任陀螺仪。

把误差叠加上去就行了。

上述步骤不断循环，最后使误差越来越小，角度越来越接近真实值。

代码参考

最后附上我的姿态解析代码：

void imuUpdate(struct Axisf acc, struct Axisf gyro, struct Axisf mag)
{
    float q0q0 = q0 * q0;
    float q1q1 = q1 * q1;
    float q2q2 = q2 * q2;
    float q3q3 = q3 * q3;

    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q3 = q2 * q3;

    float normalise;
    float ex, ey, ez;
    float halfT;
    float hx, hy, hz, bx, bz;
    float vx, vy, vz, wx, wy, wz;

    now_update = HAL_GetTick(); //单位ms
    halfT = ((float)(now_update - last_update) / 2000.0f);
    last_update = now_update;

    gyro.x *= DEG2RAD;  /*度转弧度*/
    gyro.y *= DEG2RAD;
    gyro.z *= DEG2RAD;

    /* 对加速度计数据进行归一化处理 */
    if(acc.x != 0 || acc.y != 0 || acc.z != 0)
    {
        normalise = sqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
        acc.x /= normalise;
        acc.y /= normalise;
        acc.z /= normalise;
    }

    /* 对磁力计数据进行归一化处理 */
    if(mag.x != 0 || mag.y != 0 || mag.z != 0)
    {
        normalise = sqrt(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
        mag.x /= normalise;
        mag.y /= normalise;
        mag.z /= normalise;
    }

    /* 计算磁力计投影到物体坐标上的各个分量 */
    hx = 2.0f*mag.x*(0.5f - q2q2 - q3q3) + 2.0f*mag.y*(q1q2 - q0q3) + 2.0f*mag.z*(q1q3 + q0q2);
    hy = 2.0f*mag.x*(q1q2 + q0q3) + 2.0f*mag.y*(0.5f - q1q1 - q3q3) + 2.0f*mag.z*(q2q3 - q0q1);
    hz = 2.0f*mag.x*(q1q3 - q0q2) + 2.0f*mag.y*(q2q3 + q0q1) + 2.0f*mag.z*(0.5f - q1q1 - q2q2);         
    bx = sqrt((hx*hx) + (hy*hy));
    bz = hz; 

    /* 计算加速度计投影到物体坐标上的各个分量 */
    vx = 2.0f*(q1q3 - q0q2);
    vy = 2.0f*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    /* 处理过后的磁力计新分量 */
    wx = 2.0f*bx*(0.5f - q2q2 - q3q3) + 2.0f*bz*(q1q3 - q0q2);
    wy = 2.0f*bx*(q1q2 - q0q3) + 2.0f*bz*(q0q1 + q2q3);
    wz = 2.0f*bx*(q0q2 + q1q3) + 2.0f*bz*(0.5f - q1q1 - q2q2); 

    /* 叉积误差累计，用以修正陀螺仪数据 */
    ex = (acc.y*vz - acc.z*vy) + (mag.y*wz - mag.z*wy);
    ey = (acc.z*vx - acc.x*vz) + (mag.z*wx - mag.x*wz);
    ez = (acc.x*vy - acc.y*vx) + (mag.x*wy - mag.y*wx);

    /* 互补滤波 PI */
    exInt += ex * Ki * halfT;
    eyInt += ey * Ki * halfT;   
    ezInt += ez * Ki * halfT;
    gyro.x += Kp*ex + exInt;
    gyro.y += Kp*ey + eyInt;
    gyro.z += Kp*ez + ezInt;

    /* 使用一阶龙格库塔更新四元数 */
    q0 += (-q1 * gyro.x - q2 * gyro.y - q3 * gyro.z) * halfT;
    q1 += ( q0 * gyro.x + q2 * gyro.z - q3 * gyro.y) * halfT;
    q2 += ( q0 * gyro.y - q1 * gyro.z + q3 * gyro.x) * halfT;
    q3 += ( q0 * gyro.z + q1 * gyro.y - q2 * gyro.x) * halfT;

    /* 对四元数进行归一化处理 */
    normalise = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 /= normalise;
    q1 /= normalise;
    q2 /= normalise;
    q3 /= normalise;

    /* 由四元数求解欧拉角 */
    mpu9250.attitude.x = -asinf(-2*q1*q3 + 2*q0*q2) * RAD2DEG;  //pitch
    mpu9250.attitude.y = atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1) * RAD2DEG;   //roll
    mpu9250.attitude.z = atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 - 2*q3*q3 + 1) * RAD2DEG;   //yaw

    yaw = mpu9250.attitude.z*100;       //用于J-Scope读取
    pitch = mpu9250.attitude.x*100;
    roll = mpu9250.attitude.y*100;
}