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

2024-07-17 07:52| 来源: 网络整理| 查看: 265

参考文章: 四元数完全解析及资料汇总 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：[email protected] 版权所有：西安天际智联信息技术有限公司 ****************************************************************************************/ #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 error void 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); // 12 matrix[2] = 2.f * (q1q3 - q0q2); // 13 matrix[3] = 2.f * (q1q2 - q0q3); // 21 matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 matrix[5] = 2.f * (q2q3 + q0q1); // 23 matrix[6] = 2.f * (q1q3 + q0q2); // 31 matrix[7] = 2.f * (q2q3 - q0q1); // 32 matrix[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; // yaw Att_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 ; // roll for(i=0;i

【本文地址】

公司简介

联系我们

今日新闻

推荐新闻

专题文章