系列文章目录
前言
本文主要是将采集到的ICM20602的加速度计和陀螺仪原始数据，通过四元数法进行姿态解算，得到相对准确的俯仰角和横滚角数据
一、原始数据
ICM20602传感器和MPU6050传感器的数据几乎相同都位于0x3B到0x48这14个字节的寄存器中，每两个寄存器代表加速度计或者陀螺仪一个轴的数据，每个数据都是两个字节。
因为每两个寄存器代表一个轴的加速度计或者陀螺仪数据，所以使用IIC采集到原始数据后需要对数据进行整理，下面是从ICM20602的0x3B寄存器读取原始数据进行整理的代码。
acc[0] = (int16)((datared[0]<<8) | datared[1]); //加速度计的X轴数据ACC_X
acc[1] = (int16)((datared[2]<<8) | datared[3]); //加速度计的Y轴数据ACC_Y
acc[2] = (int16)((datared[4]<<8) | datared[5]); //加速度计的Z轴数据ACC_Z
gyr[0] = (int16)((datared[8]<<8) | datared[9] ); //角速度计的X轴数据GYR_X
gyr[1] = (int16)((datared[10]<<8)| datared[11]); //角速度计的Y轴数据GYR_Y
gyr[2] = (int16)((datared[12]<<8)| datared[13]); //角速度计的Z轴数据GYR_Z
二、四元数法计算角度
1.四元数代码
下面首先介绍原始数据要代入的四元数代码
//------------------计算角度-----------------------
#define Pi 3.14159265f
#define Kp 0.8f // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.001f // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.05f // 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滚角
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
norm = sqrt(ax*ax + ay*ay + az*az); //把加速度计的三维向量转成单维向量
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// 下面是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
// 根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素
// 所以这里的vx vy vz，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的
// 重力单位向量。
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 ;
ex = (ay*vz - az*vy) ;
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
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;
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
Pitch = asin(2*(q0*q2 - q1*q3 )) * 57.2957795f; // 俯仰 换算成度
Roll = asin(2*(q0*q1 + q2*q3 )) * 57.2957795f; // 横滚
}
上面就是ICM20602原始数据要代入的四元数姿态解算代码，如果没有特殊情况，这个代码不需要改变参数。
但是特别要注意的就是，里面的halfT这个宏定义需要改为你自身采集数据的时间周期。
我运行代码时从串口助手上看到1s中采集了10组ICM20602传感器数据，所以采样频率是dt = 1/10，而halfT这个变量代表1/2*dt = 0.05，所以代码里定义为#define halfT 0.05f 。
这个数据一定要改好，我运行代码的时候，就是一开始这个时间不对，所以计算出来的俯仰角、横滚角一直在跳变。
2.计算角度
现在有了原始数据，有了四元数代码，只要将原始数据代入四元数函数代码中即可，但在原始数据代入四元数函数代码之前，还需要对原始数据进行处理。
代码如下（示例）：
void Compute_Angle(void)
{
Angle_ax = acc[0]/8192.0;
Angle_ay = acc[1]/8192.0;
Angle_az = acc[2]/8192.0;
Angle_gx = gyr[0]/131;
Angle_gy = gyr[1]/131;
Angle_gz = gyr[2]/131;
IMUupdate(Angle_gx/57.3,Angle_gy/57.3,Angle_gz/57.3,Angle_ax,Angle_ay,Angle_az);
}
上面的数据可以看到加速度计的数据除以了8192，陀螺仪的数据除以了131。
首先分析加速度计的数据，加速度计除以8192是因为，我在使用IIC采集ICM20602传感器的原始数据时，对ICM20602传感器进行了初始化。
void Sensor_Init(void)
{
writeRegister(0x1B,0x00); //陀螺仪250dps
writeRegister(0x1C,0x08); //加速度计±4g
writeRegister(0x6B,0x00);
writeRegister(0x6C,0x00);
}
<hr style=" border:solid; width:100px; height:1px;" color=#000000 size=1">
可以看到，我将加速度计的量程设置为了±4g，下面是从数据手册中截取的图形，从图中可以看到加速度计量程±4g对应的精度是8192。
我们从数据寄存器采集到的数据是16位的，而且最高位是符号位，就是说ADC读取的值的范围是±32768，对应±4g，那么1g对应的ADC值就是32768/4 = 8192。
代码中将原始加速度数据除以8192，就相当于把单位换算成了g，因为1g就等于8192。
使用时将8192修改为自己初始化的加速度计量程对应的精度即可。
同理陀螺仪也是如此，将单位换算为了度。
陀螺仪的数据本应该减去偏移值再除以131的，偏移值就是静止时陀螺仪本该为0却不为零的数据。
但因为我采集到的原始数据在静止不动时，数据值都接近0，所以就没有对陀螺仪数据进行校准，没有减去偏移值。
在代入IMUupdate（）函数时，陀螺仪的数据又除以了57.3，这是将陀螺仪数据转换为度后又转为弧度，就是将陀螺仪弧度的数据代入了函数。
总结
以上就是今天要讲的内容，本文仅仅简单介绍了采集到陀螺仪和加速度的原始数据后，将数据代入四元数算法的代码中得到俯仰角和横滚角的过程。
因自身能力有限，若有问题欢迎指出。