基于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