关于滤波的方法来获取数据我们在第一篇文章其实就讲到了,但是没有讲怎么去使用他,因此这里就挑选一个用起来比较好的卡尔曼滤波来说明下,关于其他的比较好的滤波方式,可以见:https://zhuanlan.zhihu.com/p/228805569
1、使用滤波来获取姿态角
前面提到MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角(加速度不能得到 Yaw 角),就是说有两组 Pitch、Roll 角。
MPU6050 的加速度计和陀螺仪各有优缺点,三轴的加速度值没有累积误差,且通过算 tan() 可以得到倾角,但是它包含的噪声太多(因为待测物运动时会产生加速度,电机运行时振动会产生加速度等),不能直接使用;陀螺仪对外界振动影响小,精度高,通过对角速度积分可以得到倾角,但是会产生累积误差。所以,不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角,需要融合使用,这就是为什么要采用滤波的方法的原因了!
2、程序设计
这里我们首先定义两个卡尔曼参数的结构体,如下所示
编写卡尔曼融合函数(这里可以直接从那篇知乎文章借鉴)
将角速度,加速度数据处理放到卡尔曼处理函数中
将初始化代码,加入并在程序中验证
将程序下载到开发板,并进入调试查看参数
3、源码
kalman.c
/*
* kalman.c
*
* Created on: Mar 21, 2022
* Author: LX
*/
#include <math.h>
#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 < -90 && IMU_Data->KalmanAngleY > 90) || (pitch > 90 && IMU_Data->KalmanAngleY < -90))
{
KalmanY.angle = pitch;
IMU_Data->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 <stdint.h>
#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_ */