【传感器】IMU (加速度计 + 陀螺仪)PI数据融合以及结算四元数并求解欧拉角

参考文章:
四元数完全解析及资料汇总
mpu6050姿态解算与卡尔曼滤波（1）数学

写在开头, 首先我不太想做一个搬运工, 这样没有一点意思, 我会从我的视角(小白)来尝试理解以下问题:

我们从IMU得到的数据有什么, 物理意义是什么.
我们需要什么样子的数据, 这个和我们从IMU读到的数据之间怎么转换.
学习模板代码

我们从IMU得到的数据有什么, 物理意义是什么.

以BMI088为例,手册上明确说了这个IMU是由Accelerometer(加速度计)和Gyroscope(陀螺仪)两部分构成,
也能读到 Accelerometer 的单位是 mG (0.0098m/s^2) 以及 Gyroscope 的单位是 °/s.
对于Accelerometer, 因为在地球上受到重力, 大小为1G, 故当IMU静止时,只受到重力 , 故 s q r t ( a x ∗ a x + a y ∗ a x + a z ∗ a z ) sqrt(ax*ax+ay*ax+az*az) sqrt(ax∗ax+ay∗ax+az∗az) 约等于 1G 也就算 1000mG.
对于Gyroscope, 表示绕IMU坐标系 x, y, z 轴旋转的角速度.
具体的细节, 如测量范围,测量精度这些,暂且不谈.

我们需要什么样子的数据, 这个和我们从IMU读到的数据之间怎么转换.

以云台为例, 首先我需要一个世界坐标系, 并且我能获得以世界坐标系为参考点的绕z轴旋转的角度yaw和绕着x轴旋转的角度pitch.

首先先定义世界坐标系为n系: X轴指向东，Y轴指向北，Z轴指向天
定义IMU的坐标系为b系:x轴指云台正前方,Y轴指向云台正左方,z轴指向天

首先我们有重力,那么将有一个向量恒指向地,也就是我们的n系恒等于等于[0,0,-g], 同样b系通过Accelerometer 也能得到[ax, ay, az],
这时候我们有了在两个不同坐标系下对同一个向量的两种描述, 这就可以求解两个坐标系的变换方式, 也就能得到为为想要的以世界坐标系为参考的IMU的坐标变换的yaw 和 pitch.

以下是坐标系变换的推演过程,参考:mpu6050姿态解算与卡尔曼滤波（1）数学
不需要看四元数和微分部分.这一部分是另外一种求解的方法.
结论:

这里就可以得到γ和θ, 也就是 pitch, 但无法推出 yaw.
这个也很好理解因为只知道重力, 因此可以通过三角函数知道绕X,Y轴的角度,但不知道绕Z轴的角度.
看到这里, 是不是发现了我们还没有用到Gyroscope,
Gyroscope描述了相对于IMU坐标系的x,y,z轴的旋转角速度,这个物理意义不就是对IMU坐标系的微分
可以看到对上面的T 矩阵进行微分很复杂,所以需要引入一个数序模型四元数,详情参考四元数完全解析及资料汇总
结论 :

1, 四元数 Q 转换到姿态矩阵T

2, 四元数与欧拉角的转换

3, 一阶龙格库塔 推出四元数微分迭代形式

4, 融合Accelerometer 和 Gyroscope ,消除积分误差

到这里, 思路就很明显了:

默认初始角度为[0,0,0] 对应四元数 [1,0,0,0]
上一步四元数估计重力向量,将n=[0,0,G] 带入下式, 得到估算重力向量V
测量的重力向量与估算的重力向量差积求出向量间的误差
将误差PI后补偿到陀螺仪
推出这一次的四元素
单位化
转欧拉角

下面代码示例:

/***************************************************************************************声 明本项目代码仅供个人学习使用，可以自由移植修改，但必须保留此声明信息。移植过程中出现
其他不可估量的BUG，天际智联不负任何责任。请勿商用！程序版本：V1.01
程序日期：2018-1-26
程序作者：愤怒的小孩 E-mail：1138550496@qq.com
版权所有：西安天际智联信息技术有限公司
****************************************************************************************/
#include "imu.h"
#include "math.h"
#include "filter.h"
#include "mpu9250.h"
#include "control.h"
#include "ICM20948.h"#define Kp_New      0.9f              //互补滤波当前数据的权重
#define Kp_Old      0.1f              //互补滤波历史数据的权重
//#define G                   9.80665f            // m/s^2
//#define RadtoDeg    57.324841f                //弧度到角度 (弧度 * 180/3.1415)
//#define DegtoRad    0.0174533f                //角度到弧度 (角度 * 3.1415/180)
#define Acc_Gain    0.0001220f              //加速度变成G (初始化加速度满量程-+4g LSBa = 2*4/65535.0)
#define Gyro_Gain   0.0609756f              //角速度变成度 (初始化陀螺仪满量程+-2000 LSBg = 2*2000/65535.0)
#define Gyro_Gr     0.0010641f            //角速度变成弧度(3.1415/180 * LSBg)       FLOAT_ANGLE Att_Angle;                       //飞机姿态数据 //姿态解算后的角度
FLOAT_XYZ   Gyr_rad,Gyr_radold;               //把陀螺仪的各通道读出的数据，转换成弧度制 //三轴浮点型
FLOAT_XYZ   Acc_filt,Gry_filt,Acc_filtold;    //滤波后的各通道数据
//FLOAT_XYZ   Hmc_filt;
float   accb[3],DCMgb[3][3];                  //方向余弦阵（将 惯性坐标系 转化为 机体坐标系）
uint8_t AccbUpdate = 0;/**************************实现函数*********************************************************************
函  数：static float invSqrt(float x)
功　能: 快速计算 1/Sqrt(x)
参  数：要计算的值
返回值：结果
备  注：比普通Sqrt()函数要快四倍See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
*********************************************************************************************************/
static 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;
}/*********************************************************************************************************
*函  数：void Prepare_Data(void)
*功　能：对陀螺仪去零偏后的数据滤波及赋予物理意义
*参  数：无
*返回值：无
*备  注：此函数对原始数据进行校准（去零偏，滤波处理，赋予物理意义 为姿态解算做准备
**********************************************************************************************************/
void Prepare_Data(void)
{static uint8_t IIR_mode = 1;MPU9250_Read();    //触发读取 ，立即返回//  ICM_ReadAccelGyroData();MPU9250_Offset();  //对MPU6050进行处理，减去零偏。如果没有计算零偏就计算零偏//   Aver_FilterXYZ(&MPU9250_ACC_RAW,&Acc_filt,12);//对加速度原始数据进行滑动窗口滤波SortAver_FilterXYZ(&MPU9250_ACC_RAW,&Acc_filt,12);//对加速度原始数据进行去极值滑动窗口滤波//加速度AD值 转换成 米/平方秒 Acc_filt.X = (float)Acc_filt.X * Acc_Gain * G;Acc_filt.Y = (float)Acc_filt.Y * Acc_Gain * G;Acc_filt.Z = (float)Acc_filt.Z * Acc_Gain * G;//printf("ax=%0.2f ay=%0.2f az=%0.2f\r\n",Acc_filt.X,Acc_filt.Y,Acc_filt.Z);//陀螺仪AD值 转换成 弧度/秒    Gyr_rad.X = (float) MPU9250_GYRO_RAW.X * Gyro_Gr;  Gyr_rad.Y = (float) MPU9250_GYRO_RAW.Y * Gyro_Gr;Gyr_rad.Z = (float) MPU9250_GYRO_RAW.Z * Gyro_Gr;if(IIR_mode){Acc_filt.X = Acc_filt.X * Kp_New + Acc_filtold.X * Kp_Old;Acc_filt.Y = Acc_filt.Y * Kp_New + Acc_filtold.Y * Kp_Old;Acc_filt.Z = Acc_filt.Z * Kp_New + Acc_filtold.Z * Kp_Old;
//      Gyr_rad.X = Gyr_rad.X * Kp_New + Gyr_radold.X * Kp_Old;
//      Gyr_rad.Y = Gyr_rad.Y * Kp_New + Gyr_radold.Y * Kp_Old;
//      Gyr_rad.Z = Gyr_rad.Z * Kp_New + Gyr_radold.Z * Kp_Old;Acc_filtold.X =  Acc_filt.X;Acc_filtold.Y =  Acc_filt.Y;Acc_filtold.Z =  Acc_filt.Z;
//      Gyr_radold.X = Gyr_rad.X;
//      Gyr_radold.Y = Gyr_rad.Y;
//      Gyr_radold.Z = Gyr_rad.Z;}accb[0] = Acc_filt.X;accb[1] = Acc_filt.Y;accb[2] = Acc_filt.Z;if(accb[0]&&accb[1]&&accb[2]){AccbUpdate = 1;}
}
/*********************************************************************************************************
*函  数：void IMUupdate(FLOAT_XYZ *Gyr_rad,FLOAT_XYZ *Acc_filt,FLOAT_ANGLE *Att_Angle)
*功　能：获取姿态角
*参  数：Gyr_rad 指向角速度的指针（注意单位必须是弧度）
*        Acc_filt 指向加速度的指针
*        Att_Angle 指向姿态角的指针
*返回值：无
*备  注：求解四元数和欧拉角都在此函数中完成
**********************************************************************************************************/
//kp=ki=0 就是完全相信陀螺仪
#define Kp 1.50f                         // proportional gain governs rate of convergence to accelerometer/magnetometer//比例增益控制加速度计，磁力计的收敛速率
#define Ki 0.005f                        // integral gain governs rate of convergence of gyroscope biases  //积分增益控制陀螺偏差的收敛速度
#define halfT 0.005f                     // half the sample period 采样周期的一半float q0 = 1, q1 = 0, q2 = 0, q3 = 0;     // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral errorvoid IMUupdate(FLOAT_XYZ *Gyr_rad,FLOAT_XYZ *Acc_filt,FLOAT_ANGLE *Att_Angle)
{uint8_t i;float matrix[9] = {1.f,  0.0f,  0.0f, 0.0f,  1.f,  0.0f, 0.0f,  0.0f,  1.f };//初始化矩阵float ax = Acc_filt->X,ay = Acc_filt->Y,az = Acc_filt->Z;float gx = Gyr_rad->X,gy = Gyr_rad->Y,gz = Gyr_rad->Z;float vx, vy, vz;float ex, ey, ez;float norm;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;if(ax*ay*az==0)return;//加速度计测量的重力向量(机体坐标系) norm = invSqrt(ax*ax + ay*ay + az*az); ax = ax * norm;ay = ay * norm;az = az * norm;
//  printf("ax=%0.2f ay=%0.2f az=%0.2f\r\n",ax,ay,az);//陀螺仪积分估计重力向量(机体坐标系) vx = 2*(q1q3 - q0q2);                                              vy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;// printf("vx=%0.2f vy=%0.2f vz=%0.2f\r\n",vx,vy,vz); //测量的重力向量与估算的重力向量差积求出向量间的误差 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);//用上面求出误差进行积分exInt = exInt + ex * Ki;                                 eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;//将误差PI后补偿到陀螺仪gx = gx + Kp*ex + exInt;                              gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt;//这里的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;//单位化四元数 norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 * norm;q1 = q1 * norm;q2 = q2 * norm;  q3 = q3 * norm;//矩阵R 将惯性坐标系(n)转换到机体坐标系(b) matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11(前列后行)matrix[1] = 2.f * (q1q2 + q0q3);      // 12matrix[2] = 2.f * (q1q3 - q0q2);      // 13matrix[3] = 2.f * (q1q2 - q0q3);      // 21matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22matrix[5] = 2.f * (q2q3 + q0q1);        // 23matrix[6] = 2.f * (q1q3 + q0q2);     // 31matrix[7] = 2.f * (q2q3 - q0q1);      // 32matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33//四元数转换成欧拉角(Z->Y->X) Att_Angle->yaw += Gyr_rad->Z *RadtoDeg*0.01f;
//  Att_Angle->yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)* 57.3f; // yawAtt_Angle->pit = -asin(2.f * (q1q3 - q0q2))* 57.3f;                                 // pitch(负号要注意) Att_Angle->rol = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3)* 57.3f ; // rollfor(i=0;i<9;i++){*(&(DCMgb[0][0])+i) = matrix[i];}//失控保护 (调试时可注释掉)
//  Safety_Check();
}