imuncle.github.io
imuncle.github.io copied to clipboard
imuncle
MPU9250姿态解析
前段时间尝试使用陀螺仪作为机器人云台的位置环反馈值，在底盘跟随的大前提下，这样做是更符合第一视角的操作的。
我去年比赛的时候买了几个维特智能的型号为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;
}
参考
红线是电机码盘，绿线是陀螺仪数据（yaw轴），几乎完全重合，并且无漂移
你好前辈，非常感谢！
在2021年1月25日 11:40，big_uncle[email protected] 写道：
@zcf0619 在这里： https://github.com/imuncle/Embedded_Peripheral_Libs
— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub, or unsubscribe.
前辈你好， 冒昧致函。想向您请教在AHRS解算中，Kp和Ki参数的大致范围应该如何确定呢。 望不吝赐教。
赵存飞
北京航空航天大学宇航学院 图像处理中心 通讯地址：北京市昌平区沙河高教园南三街9号 邮政编码：102206 在2021年1月25日 11:42，赵存飞[email protected] 写道：
你好前辈，非常感谢！
在2021年1月25日 11:40，big_uncle[email protected] 写道：
@zcf0619 在这里： https://github.com/imuncle/Embedded_Peripheral_Libs
— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub, or unsubscribe.