卡尔曼滤波获取MPU6050欧拉角

GOAT_CR7

已于 2024-04-21 21:33:44 修改

阅读量2.2k

点赞数 28

分类专栏： MPU6050陀螺仪文章标签： stm32 嵌入式硬件单片机

于 2024-04-21 00:21:09 首次发布

本文链接：https://blog.csdn.net/cr7_goat_/article/details/138013666

版权

MPU6050陀螺仪专栏收录该内容

3 篇文章 3 订阅

订阅专栏

2、读取原始数据并计算出减去偏移量的加速度值和角速度值

2、1首先需要读取原始数据，按照公式计算出加速度和角速度，最后在减去对应轴的偏移量得到最终数值。

3、计算加速度和角速度各轴的偏移量。

3、1先算各轴平均数，再减去各轴标准数，最后得到偏移量。

3、计算加速度角度，角速度角度。

4、1计算加速度的Roll角、Pitch角（MPU6050加速度计求不到Yaw角）。计算角速度的Roll角、Pitch角、Yaw角。

5、对加速度角度与角速度角度进行卡尔曼滤波

一、程序框图

获取欧拉角需要提前读取MPU6050的原始数据，读取到之后需要将原始数据转换成对应的加速度值和角速度值，在求角度之前需要对MPU6050进行零点校准。通过加速度可以求出 Roll 和 Pitch ；通过角速度可以求出Roll 、Pitch、Yaw，最后加速度角度和角速度角度通过卡尔曼滤波得到最终角度。

二、代码部分

📢读取MPU6050原始数据

0、头文件

#include "stm32f10x.h"                  // Device header
#include "MPU6050.h"
#include "Delay.h"
#include "OLED.h"
#include "Serial.h"
#include <math.h>
#include "Kalman.h"


#define AccSPL        16384.0       //加速度计灵敏度±2g        
#define GyroSPL       16.4          //角速度灵敏度±2000°/s


/*定义各轴偏移量变量*/
float Offset_ax = 0.0;
float Offset_ay = 0.0;
float Offset_az = 0.0;
float Offset_gx = 0.0;
float Offset_gy = 0.0;
float Offset_gz = 0.0;

1、初始化程序

1.1初始化MPU6050、计算偏移量。

/**
  * 函数功能：卡尔曼滤波初始化
  * 入口参数：无
  * 返 回 值：无
  */
void Kalman_Init(void)
{
    /*MPU6050初始化*/
    MPU6050_Init();
    
    /*计算水平时MPU6050的偏移量*/
    Kalman_GetMPU6050_Offset(&Offset_ax, &Offset_ay, &Offset_az, &Offset_gx, &Offset_gy, &Offset_gz);
    
}

2、读取原始数据并计算出减去偏移量的加速度值和角速度值

2、1首先需要读取原始数据，按照公式计算出加速度和角速度，最后在减去对应轴的偏移量得到最终数值。

/**
  * 函数功能：读取MPU6050加速度和角速度数据
  * 入口参数：AccX ：获取X轴加速度
  * 入口参数：AccY ：获取Y轴加速度
  * 入口参数：AccX ：获取Z轴加速度
  * 入口参数：GyroX：获取X轴角速度
  * 入口参数：GyroY：获取Y轴角速度
  * 入口参数：GyroZ：获取Z轴角速度
  * 返 回 值：无
  */
void Kalman_GetMPU6050_Data(float *AccX, float *AccY, float *AccZ,
                            float *GyroX, float *GyroY, float *GyroZ)
{
    /*定义暂存加速度原始值变量*/
    int16_t AccX_Temp;
    int16_t AccY_Temp;
    int16_t AccZ_Temp;
    
    /*定义暂存加速度原始值变量*/
    static int16_t GyroX_Temp;
    static int16_t GyroY_Temp;
    static int16_t GyroZ_Temp;
    
    /*获取MPU6050原始数据*/
    MPU6050_GetData(&AccX_Temp, &AccY_Temp, &AccZ_Temp, &GyroX_Temp, &GyroY_Temp, &GyroZ_Temp);
    
    /*** 加速度值 = 原始数据 / 灵敏度 ***/
    *AccX = AccX_Temp / AccSPL - Offset_ax;    
    *AccY = AccY_Temp / AccSPL - Offset_ay;
    *AccZ = AccZ_Temp / AccSPL - Offset_az;
    
    /*** 角速度值 = 原始数据 / 灵敏度 ***/
    *GyroX = GyroX_Temp / GyroSPL - Offset_gx;
    *GyroY = GyroY_Temp / GyroSPL - Offset_gy;
    *GyroZ = GyroZ_Temp / GyroSPL - Offset_gz;
    
}

3、计算加速度和角速度各轴的偏移量。

3、1先算各轴平均数，再减去各轴标准数，最后得到偏移量。

/**
  * 函数功能：获取MPU6050水平时各轴的偏移量
  * 入口参数：Offset_ax：获取加速度X轴偏移量
  * 入口参数：Offset_ay：获取加速度Y轴偏移量
  * 入口参数：Offset_az：获取加速度Z轴偏移量
  * 入口参数：Offset_gx：获取角速度X轴偏移量
  * 入口参数：Offset_gy：获取角速度Y轴偏移量
  * 入口参数：Offset_gz：获取角速度Z轴偏移量
  * 返 回 值：无
  */
void Kalman_GetMPU6050_Offset(float *Offset_ax, float *Offset_ay, float *Offset_az, 
                                float *Offset_gx, float *Offset_gy, float *Offset_gz)
{
    /*定义求平均值的个数*/
    uint16_t i;                 //重复次数变量
    float Count = 100.0;         //求平均数个数变量
    
    /*定义加速度角速度暂存变量*/
    float AccX, AccY, AccZ;
    float GyroX, GyroY, GyroZ;
    
    /*定义偏移量暂存变量*/
    static float Offset_a[3];
    static float Offset_g[3];
    
    Delay_ms(10);
    
    /*获取数据总和*/
    for (i = 0; i < Count; i++)
    {
        /*获取加速度角速度数据*/
        Delay_us(20);
        Kalman_GetMPU6050_Data(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ); 
        
        /*获取数据总和*/
        Offset_a[0] += AccX;
        Offset_a[1] += AccY;
        Offset_a[2] += AccZ;
        Offset_g[0] += GyroX;
        Offset_g[1] += GyroY;
        Offset_g[2] += GyroZ; 
    }
    
    /*求出平均值*/
    Offset_a[0] /= Count;
    Offset_a[1] /= Count;
    Offset_a[2] /= Count;
    Offset_g[0] /= Count;
    Offset_g[1] /= Count;
    Offset_g[2] /= Count;
    
    /*求出偏移量*/
    *Offset_ax = Offset_a[0] - 0.0;
    *Offset_ay = Offset_a[1] - 0.0;
    *Offset_az = Offset_a[2] - 1.0;  // Z轴加速度标准是等于1g
    *Offset_gx = Offset_g[0] - 0.0;
    *Offset_gy = Offset_g[1] - 0.0;
    *Offset_gz = Offset_g[2] - 0.0; 
}

3、计算加速度角度，角速度角度。

4、1计算加速度的Roll角、Pitch角（MPU6050加速度计求不到Yaw角）。计算角速度的Roll角、Pitch角、Yaw角。

/**
  * 函数功能：获取加速度、角速度角度
  * 入口参数：Roll_A ：获取加速度横滚角度
  * 入口参数：Pitch_A：获取加速度俯仰角度
  * 入口参数：Roll_G ：获取角速度横滚角度
  * 入口参数：Pitch_G：获取角速度俯仰角度
  * 入口参数：Yaw_G  ：获取角速度偏航角度
  * 返 回 值：无
  */
void Kalman_GetEuler_TempAngle(float *Roll_A, float *Pitch_A,
                                float *Roll_G, float *Pitch_G, float *Yaw_G)
{
    /*定义积分时间变量*/
    static float dt = 0.01;       
    
    /*定义暂存变量*/
    float AccX, AccY, AccZ;
    static float GyroX, GyroY, GyroZ;

    /*读取加速度和角速度*/
    Kalman_GetMPU6050_Data(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);
    
    /*加速度求夹角*/
    /* 横滚角 = atan2(AccY, Accz) * 180 / Π  */
    *Roll_A = atan2(AccY, AccZ) * 180 / 3.141593;
    
    /* 俯仰角 = -atan2(AccX, sqrt(AccY * AccY + AccZ * Accz)) * 180 / Π   */
    *Pitch_A = -atan2(AccX, sqrt( AccY * AccY + AccZ * AccZ ) ) * 180 / 3.141593;
    
    
    /*角速度求夹角*/
    *Roll_G  += GyroX *dt;    //公式： 角度 += 角速度 * dt
    *Pitch_G += GyroY *dt;
    *Yaw_G   += GyroZ *dt;
    
}

5、对加速度角度与角速度角度进行卡尔曼滤波

5、1Roll角卡尔曼滤波：

/**
  * 函数功能：卡尔曼滤波Roll
  * 入口参数：Angle_a：加速度Roll
  * 入口参数：Angle_g：角速度Roll
  * 返 回 值：无
  */
float Kalman_Filter_Roll(float Angle_a, float Angle_g)
{
    static float Angle;              //最终角度
    
    float dt      = 0.01;    //采样周期即计算任务周期10ms
    
    float Q_Angle = 0.001;   //角度数据置信度，角度噪声的协方差
    float Q_Gyro  = 0.003;   //角速度数据置信度，角速度噪声协方差
    float R_Angle = 0.5;     //加速度计测量噪声的协方差
    
    float Q_Bias;            //上次最优估计值偏差
    float Angle_err;         //角度偏差
    float K_0;               //用于计算最优估计值
    float K_1;               //用于计算最优估计值的偏差
    
    static float P[2][2] = { {1, 0}, {0, 1} };   //过程协方差矩阵P，初始值为单位阵
    

    /*Step1: 先验估计*/
    /* 状态方程,角度值等于上次最优角度加角速度减零漂后积分 */
    Angle += (Angle_g - Q_Bias) * dt;          
    
    /*Step2: 预测协方差矩阵  */
    P[0][0] += ( Q_Angle - P[0][1] - P[1][0] * dt );
    P[0][1] += -P[1][1] * dt;
    P[1][0] += -P[1][1] * dt;
    P[1][1] += Q_Gyro * dt; 
    
    /*Step3: 计算卡尔曼增益  */
    K_0 = P[0][0] / ( R_Angle + P[0][0] );
    K_1 = P[0][1] / ( R_Angle + P[0][0] );
    
    /*Step4: 更新协方差矩阵 */
    P[0][0] -= K_0 * P[0][0];
    P[0][1] -= K_0 * P[0][1];
    P[1][0] -= K_1 * P[0][0];
    P[1][1] -= K_1 * P[0][1];
    
    /*Step5: 计算最优角度值 */
    Angle_err = Angle_a - Angle;      //先验估计，计算角度偏差
    Angle     += K_0 * Angle_err;     //后验估计，得到最优估计值
    Q_Bias   += K_1 * Angle_err;     //后验估计，得到最优估计偏差
    
    return Angle;
}

5、2 Pitch卡尔曼滤波：

**
  * 函数功能：卡尔曼滤波Pitch
  * 入口参数：Angle_a：加速度Pitch
  * 入口参数：Angle_g：角速度Pitch
  * 返 回 值：无
  */
float Klaman_Filter_Pitct(float Angle_a, float Angle_g)
{
    static float Angle;
    
    static float dt      = 0.01;  //采样周期即计算任务周期10ms
    static float Q_Angle = 0.001; //角度数据置信度，角度噪声的协方差
    static float Q_Gyro  = 0.003; //角速度数据置信度，角速度噪声协方差
    static float R_Angle = 0.5;   //加速度计测量噪声。抖动时数值越大曲线越平滑,但是响应也会变慢。
    
    static float Q_Bias;            //上次最优估计值偏差
    static float Angle_err;         //角度偏差
    static float K_0;               //用于计算最优估计值
    static float K_1;               //用于计算最优估计值的偏差
    
    static float P[2][2] = { {1, 0}, {0, 1} };   //过程协方差矩阵
    
    /*Step1: 先验估计 */
    Angle += (Angle_g - Q_Bias) * dt;
    
    /*Step2: 计算过程协方差矩阵的微分 */
    P[0][0] += ( Q_Angle - P[0][1] - P[1][0] * dt );
    P[0][1] += -P[1][1] * dt;
    P[1][0] += -P[1][1] * dt;
    P[1][1] += Q_Gyro * dt;
    
    /*Step3: 计算卡尔曼增益  */
    K_0 = P[0][0] / ( R_Angle + P[0][0] );
    K_1 = P[0][1] / ( R_Angle + P[0][0] );
    
    /*Step4: 后验估计误差协方差  */
    P[0][0] -= K_0 * P[0][0];
    P[0][1] -= K_0 * P[0][1];
    P[1][0] -= K_1 * P[0][0];
    P[1][1] -= K_1 * P[0][1];
    
    /*Step: 计算最优角度  */
    Angle_err = Angle_a - Angle;  //先验估计，计算角度偏差
    Angle    +=K_0 * Angle_err;   //后验估计，得到最优估计值
    Q_Bias   +=K_1 * Angle_err;   //后验估计，得到最优估计偏差
    
    return Angle;
}

5、3 调用卡尔曼滤波函数：

/**
  * 函数功能：获取卡尔曼滤波后的欧拉角
  * 入口参数： Roll：获取经过卡尔曼滤波的横滚角
  * 入口参数：Pitch：获取经过卡尔曼滤波的俯仰角
  * 入口参数：  Yaw：获取没有经过卡尔曼滤波的偏航角(这个角度不准)
  * 返 回 值：无
  */
void Kalman_GetEuler_Angle(float *Roll, float *Pitch, float *Yaw)
{
    /*定义暂存加速度欧拉角变量*/
    static float Roll_a;
    static float Pitch_a;
    static float Yaw_a;  
    
    /*定义暂存角速度欧拉角变量*/
    static float Roll_g;
    static float Pitch_g;
    static float Yaw_g;
     
    /*读取加速度和角速度的原始欧拉角*/
    Kalman_GetEuler_TempAngle(&Roll_a, &Pitch_a, &Roll_g, &Pitch_g, &Yaw_g);
    
    /*对加速度和角速度欧拉角进行卡尔曼滤波*/
    *Roll  = Kalman_Filter_Roll(Roll_a, Roll_g);
    *Pitch = Klaman_Filter_Pitct(Pitch_a, Pitch_g);
    *Yaw   = Yaw_g * 5.62;

     
}