MPU6050使用卡尔曼滤波来获取数据 |
您所在的位置：网站首页 › mpu6050零点漂移处理 › MPU6050使用卡尔曼滤波来获取数据 |
MPU6050使用卡尔曼滤波来获取数据
|
文章目录
1、使用滤波来获取姿态角2、程序设计3、源码
关于滤波的方法来获取数据我们在第一篇文章其实就讲到了，但是没有讲怎么去使用他，因此这里就挑选一个用起来比较好的卡尔曼滤波来说明下，关于其他的比较好的滤波方式，可以见：https://zhuanlan.zhihu.com/p/228805569
1、使用滤波来获取姿态角
前面提到MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角。 MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan() 可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要融合使用，这就是为什么要采用滤波的方法的原因了！ 2、程序设计这里我们首先定义两个卡尔曼参数的结构体，如下所示 kalman.c /* * kalman.c * * Created on: Mar 21, 2022 * Author: LX */ #include #include "kalman.h" #define RAD_TO_DEG 57.295779513082320876798154814105 extern IMU_Parameter IMU_Data; uint32_t timer; Kalman_t KalmanX = { .Q_angle = 0.001f, .Q_bias = 0.003f, .R_measure = 0.03f}; Kalman_t KalmanY = { .Q_angle = 0.001f, .Q_bias = 0.003f, .R_measure = 0.03f, }; void MPU6050_Read_All(IMU_Parameter *IMU_Data) { double dt = (double)(HAL_GetTick() - timer) / 1000; timer = HAL_GetTick(); double roll; double roll_sqrt = sqrt(IMU_Data->Accel_X * IMU_Data->Accel_X + IMU_Data->Accel_Z * IMU_Data->Accel_Z); if (roll_sqrt != 0.0) { roll = atan(IMU_Data->Accel_Y / roll_sqrt) * RAD_TO_DEG; } else { roll = 0.0; } double pitch = atan2(-IMU_Data->Accel_X, IMU_Data->Accel_Z) * RAD_TO_DEG; if ((pitch KalmanAngleY > 90) || (pitch > 90 && IMU_Data->KalmanAngleY KalmanAngleY = pitch; } else { IMU_Data->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, IMU_Data->Gyro_Y, dt); } if (fabs(IMU_Data->KalmanAngleY) > 90) IMU_Data->Gyro_X = -IMU_Data->Gyro_X; IMU_Data->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, IMU_Data->Gyro_X, dt); } double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt) { double rate = newRate - Kalman->bias; Kalman->angle += dt * rate; Kalman->P[0][0] += dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle); Kalman->P[0][1] -= dt * Kalman->P[1][1]; Kalman->P[1][0] -= dt * Kalman->P[1][1]; Kalman->P[1][1] += Kalman->Q_bias * dt; double S = Kalman->P[0][0] + Kalman->R_measure; double K[2]; K[0] = Kalman->P[0][0] / S; K[1] = Kalman->P[1][0] / S; double y = newAngle - Kalman->angle; Kalman->angle += K[0] * y; Kalman->bias += K[1] * y; double P00_temp = Kalman->P[0][0]; double P01_temp = Kalman->P[0][1]; Kalman->P[0][0] -= K[0] * P00_temp; Kalman->P[0][1] -= K[0] * P01_temp; Kalman->P[1][0] -= K[1] * P00_temp; Kalman->P[1][1] -= K[1] * P01_temp; return Kalman->angle; };kalman.h /* * kalman.h * * Created on: Mar 21, 2022 * Author: LX */ #ifndef KALMAN_H_ #define KALMAN_H_ #include "main.h" #include #include "i2c.h" #include "mpu6050.h" typedef struct { double Q_angle; double Q_bias; double R_measure; double angle; double bias; double P[2][2]; } Kalman_t; void MPU6050_Read_All(IMU_Parameter *IMU_Data); double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt); #endif /* KALMAN_H_ */ |
编写卡尔曼融合函数（这里可以直接从那篇知乎文章借鉴） 
将角速度，加速度数据处理放到卡尔曼处理函数中 
将初始化代码，加入并在程序中验证 
将程序下载到开发板，并进入调试查看参数 
【本文地址】
今日新闻 |
推荐新闻 |