mpu6050卡尔曼滤波有延迟

Question

mpu6050卡尔曼滤波得到的数据不能使平衡小车平衡，dmp库的可以，波型显示卡尔曼有延迟怎么解决

void task_control_car(void *pvParameters)
{
    BaseType_t err = pdFALSE;

    while (1)
    {
        if (SemMpuIntr != NULL)
        {
            err = xSemaphoreTake(SemMpuIntr, portMAX_DELAY); // 获取信号量
            if (err == pdTRUE)                                 // 获取信号量成功
            {
                
                
                //===读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反
                Encoder_A = Read_Encoder(2);     // 减1是因为编码器的固定误差
                Encoder_B = Read_Encoder(3);

                // 获取小车的平衡角度
                Car_Read_Angle(); 
                // 读取传感器数据
//        mpu_dmp_get_data(&pitch, &roll, &yaw);
//        MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);
//                Car_Angle_Balance= roll * 10;
//                Balance_Pwm = Car_Balance(Car_Angle_Balance, gyrox);
//                Car_Angle_Balance= angle * 10;
//                Car_Angle_Balance= Car_Angle_Balance * 10;
                Balance_Pwm = Car_Balance(Car_Angle_Balance, Gyro_Balance);
                Velocity_Pwm = Car_Velocity(Encoder_A, Encoder_B);
                if (1 == Flag_Left || 1 == Flag_Right)
                    Turn_Pwm = Car_Turn(Encoder_A, Encoder_B, Gyro_Turn); //===转向环PID控制
                else
                    Turn_Pwm = -0.4 * Gyro_Turn;

                PWMA = Balance_Pwm + Velocity_Pwm;// + Turn_Pwm
                PWMB = Balance_Pwm + Velocity_Pwm;// - Turn_Pwm

                // 角度太大时小车会倒下，停止电机
                if (Car_Angle_Balance >= 400 || Car_Angle_Balance <= -400)
                {
                    PWMA = PWMB = 0;
                    Flag_Qian = Flag_Hou = Flag_Left = Flag_Right = 0;
                    Encoder = Encoder_Integral = 0;
                }

                Set_Pwm(PWMA, -PWMB); // pwm更新
            
            }
        }
        else if (err == pdFALSE)
        {
            printf("%s, %d, SemMpuIntr is NULL\r\n", __FILE__, __LINE__);
            vTaskDelay(10); // 延时10ms，也就是10个时钟节拍
        }
    }
}

/********************************************
函数功能：获取角度
*********************************************/
void Car_Read_Angle(void)
{
    short Acce[3]; // 加速度计缓存
    short Gryo[3]; // 陀螺仪缓存
    float Gyro_X, Gyro_Z;  // X轴和Z轴角速度
    float Accel_Y, Accel_Z; // Y轴和Z轴加速度

    MPU_Get_Gyroscope(&Gryo[0], &Gryo[1], &Gryo[2]);
    MPU_Get_Accelerometer(&Acce[0], &Acce[1], &Acce[2]);

    // 数据转换（修改为X轴相关参数）
    Gyro_X = (float)Gryo[0];  // 改为读取X轴陀螺仪数据
    Gyro_Z = (float)Gryo[2];
    Accel_Y = (float)Acce[1]; // 改为读取Y轴加速度数据
    Accel_Z = (float)Acce[2];

    // 数据类型转换（2's complement）
    if (Gyro_X > 32768) Gyro_X -= 65536;
    if (Gyro_Z > 32768) Gyro_Z -= 65536;
    if (Accel_Y > 32768) Accel_Y -= 65536;
    if (Accel_Z > 32768) Accel_Z -= 65536;

    Gyro_X = -Gyro_X; // 根据传感器安装方向调整符号

    // 更新参数（偏移量需要重新校准）
    Gyro_Balance = Gyro_X ;// - 7.5 X轴角速度偏移校准值（示例值，需实测调整）
    Gyro_Turn = Gyro_Z  ;    //- 16 Z轴偏移保持不变

    // 计算绕X轴的倾斜角度（Roll）
    // 当传感器绕X轴旋转时，使用Y和Z轴加速度计算角度
    Angle_Balance = atan2(Accel_Y, Accel_Z) * 180 / 3.14;//pitch角

    // 量程转换（保持16.4 LSB/(deg/s)不变）
    Car_Gyro_Balance = Gyro_X / 16.4;  // X轴角速度
    
    // 卡尔曼滤波
//     angle =  Kalman_Filter(Angle_Balance, Car_Gyro_Balance);
        
        Kalman_Filter(Angle_Balance, Car_Gyro_Balance);//roll轴
    Car_Angle_Balance= angle * 10; // 更新最终角度
}

/*
********************************************************************************************************
文件名：Filter.c
功  能：滤波算法
*********************************************************************************************************
*/

#include "Filter.h"

// 卡尔曼滤波参数
float K1 = 0.02;
float angle, angle_dot;
float Q_angle = 0.001; // 过程噪声的协方差
float Q_gyro = 0.003;  // 0.03 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle = 0.5;   // 测量噪声的协方差 既测量偏差
float dt = 0.005;       //
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] = {0, 0, 0, 0};
float PP[2][2] = {{1, 0}, {0, 1}};

// 简易卡尔曼滤波
// 加速度、角速度
void Kalman_Filter(float Accel, float Gyro)
{
    angle += (Gyro - Q_bias) * dt;             // 先验估计
    Pdot[0] = Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

    Pdot[1] = -PP[1][1];
    Pdot[2] = -PP[1][1];
    Pdot[3] = Q_gyro;
    PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
    PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;

    Angle_err = Accel - angle; // zk-先验估计

    PCt_0 = C_0 * PP[0][0];
    PCt_1 = C_0 * PP[1][0];

    E = R_angle + C_0 * PCt_0;

    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;

    t_0 = PCt_0;
    t_1 = C_0 * PP[0][1];

    PP[0][0] -= K_0 * t_0; // 后验估计误差协方差
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;

    angle += K_0 * Angle_err;  // 后验估计
    Q_bias += K_1 * Angle_err; // 后验估计
    angle_dot = Gyro - Q_bias; // 输出值(后验估计)的微分=角速度
}

Answer 1

阿里嘎多学长整理AIGC生成，因移动端显示问题导致当前答案未能完全显示，请使用PC端查看更加详细的解答过程

mpu6050卡尔曼滤波有延迟

卡尔曼滤波算法是一种常用的状态估计算法，用于估计系统的状态。MPU6050是一个常用的加速度计和陀螺仪组合，用于实现小车的平衡控制。卡尔曼滤波算法可以用于MPU6050的数据处理，以提高小车的平衡性能。

然而，卡尔曼滤波算法也存在一些缺点，例如延迟。延迟是指卡尔曼滤波算法对数据的处理速度较慢，导致小车的控制延迟。

解决卡尔曼滤波算法的延迟问题，可以尝试以下几种方法：

1. 提高采样率：提高MPU6050的采样率，可以减少卡尔曼滤波算法的延迟。例如，可以从100Hz提高到200Hz或400Hz。
2. 优化卡尔曼滤波算法：可以尝试优化卡尔曼滤波算法的参数，例如调整权重系数、选择合适的滤波器等。
3. 使用DMP库：DMP库是MPU6050的官方库，提供了预先编译好的卡尔曼滤波算法，可以减少开发时间和提高性能。
4. 使用其他滤波算法：可以尝试使用其他滤波算法，例如Kalman滤波算法、Moving Average滤波算法等。

以下是一个简单的卡尔曼滤波算法示例代码：

void kalman_filter(float *accel_x, float *accel_y, float *accel_z, float *gyro_x, float *gyro_y, float *gyro_z) {
    // 设置卡尔曼滤波器的参数
    float P = 0.1; // 过程噪声
    float Q = 0.01; // 测量噪声
    float R = 0.1; // 状态噪声
    float K = 0.1; // 控制参数

    // 计算卡尔曼滤波器的状态
    float x = 0;
    float y = 0;
    float z = 0;
    float vx = 0;
    float vy = 0;
    float vz = 0;

    // 计算卡尔曼滤波器的预测值
    float x_pred = x + vx * dt;
    float y_pred = y + vy * dt;
    float z_pred = z + vz * dt;

    // 计算卡尔曼滤波器的残差
    float dx = x_pred - x;
    float dy = y_pred - y;
    float dz = z_pred - z;

    // 计算卡尔曼滤波器的更新值
    float x_upd = x_pred + K * dx;
    float y_upd = y_pred + K * dy;
    float z_upd = z_pred + K * dz;

    // 更新卡尔曼滤波器的状态
    x = x_upd;
    y = y_upd;
    z = z_upd;
    vx = vx;
    vy = vy;
    vz = vz;

    // 返回卡尔曼滤波器的输出值
    *accel_x = x;
    *accel_y = y;
    *accel_z = z;
    *gyro_x = vx;
    *gyro_y = vy;
    *gyro_z = vz;
}

这个示例代码只是一个简单的卡尔曼滤波算法，实际实现中需要根据具体的应用场景和需求进行调整和优化。