请选择 进入手机版 | 继续访问电脑版

前馈科技

 找回密码
 立即注册
搜索
热搜: 活动 交友 discuz
查看: 914|回复: 0

Madgwick互补滤波收敛速度

[复制链接]

97

主题

97

帖子

539

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
539
发表于 2020-6-22 21:07:44 | 显示全部楼层 |阅读模式
    近日调试基于STM32F405的小车自动控制,姿态测量采用MPU9250,考虑到小车所处的电磁环境比较恶劣,未使用磁力计数据,姿态解算算法采用Madgwick互补滤波算法.由于对Madgwick互补滤波算法理解不深,在网上搜索并采用了其他大神所写好的现成代码:

void  madgwick(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_INFO *att) {
    float accelNorm, recipNorm;
    float qDot1, qDot2, qDot3, qDot4;

    float q0 = att->q0;
    float q1 = att->q1;
    float q2 = att->q2;
    float q3 = att->q3;

    float gx = gyroXYZ[0];
    float gy = gyroXYZ[1];
    float gz = gyroXYZ[2];

    float ax = accelXYZ[0];
    float ay = accelXYZ[1];
    float az = accelXYZ[2];

    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

    accelNorm = sqrtf(ax * ax + ay * ay + az * az);

    // Compute feedback only if accelerometer abs(vector)is not too small to avoid a division by a small number
    if (accelNorm > 0.01) {
        float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
        float s0, s1, s2, s3;
        float accelConfidence;

        // Normalise accelerometer measurement
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;

        // Auxiliary variables to avoid repeated arithmetic
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _4q0 = 4.0f * q0;
        _4q1 = 4.0f * q1;
        _4q2 = 4.0f * q2;
        _8q1 = 8.0f * q1;
        _8q2 = 8.0f * q2;
        q0q0 = q0 * q0;
        q1q1 = q1 * q1;
        q2q2 = q2 * q2;
        q3q3 = q3 * q3;

        // Gradient decent algorithm corrective step
        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
        s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
        s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;

        // Apply feedback step
        accelConfidence = calculateAccConfidence(accelNorm, &att->accMagP);
        qDot1 -= m_beta * s0 * accelConfidence;
        qDot2 -= m_beta * s1 * accelConfidence;
        qDot3 -= m_beta * s2 * accelConfidence;
        qDot4 -= m_beta * s3 * accelConfidence;
    }

    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * dt;
    q1 += qDot2 * dt;
    q2 += qDot3 * dt;
    q3 += qDot4 * dt;

    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;

    att->q0 = q0;
    att->q1 = q1;
    att->q2 = q2;
    att->q3 = q3;
}
     根据作者的演示,开始结算后姿态迅速收敛至正常值,然而在我这儿却不是这么回事,小车处于水平状态下上电后至少需要30s才能收敛到pitch和roll均接近于0.0°,折腾两天也未能找到原因!
    考虑到作者提供的Madgwick互补滤波算法的代码经很多人使用,因此算法代码应该不存在问题,于是着重分析调用ahrs_update_madgwick_imu函数之前的程序.经过仔细分析发现了一个说不清的参数,而这参数高度依赖于系统运行时序:


static void callback(float *accel, float *gyro, float *mag) {
    static uint32_t last_time = 0;
    float dt = timer_seconds_elapsed_since(last_time);
    last_time = timer_time_now();

    if(!imu_ready && ST2MS(chVTGetSystemTimeX() - init_time) > 1000){
        update_parameters(  m_settings.accel_confidence_decay,  m_settings.madgwick_beta);
        imu_ready = true;
    }
    // Rotate axes (ZYX)
    float s1 = sinf(m_settings.rot_yaw * M_PI / 180.0);
    float c1 = cosf(m_settings.rot_yaw * M_PI / 180.0);
    float s2 = sinf(m_settings.rot_pitch * M_PI / 180.0);
    float c2 = cosf(m_settings.rot_pitch * M_PI / 180.0);
    float s3 = sinf(m_settings.rot_roll * M_PI / 180.0);
    float c3 = cosf(m_settings.rot_roll * M_PI / 180.0);

    float m11 = c1 * c2;    float m12 = c1 * s2 * s3 - c3 * s1;    float m13 = s1 * s3 + c1 * c3 * s2;
    float m21 = c2 * s1;    float m22 = c1 * c3 + s1 * s2 * s3;    float m23 = c3 * s1 * s2 - c1 * s3;
    float m31 = -s2;         float m32 = c2 * s3;                float m33 = c2 * c3;

    m_accel[0] = accel[0] * m11 + accel[1] * m12 + accel[2] * m13;
    m_accel[1] = accel[0] * m21 + accel[1] * m22 + accel[2] * m23;
    m_accel[2] = accel[0] * m31 + accel[1] * m32 + accel[2] * m33;

    m_gyro[0] = gyro[0] * m11 + gyro[1] * m12 + gyro[2] * m13;
    m_gyro[1] = gyro[0] * m21 + gyro[1] * m22 + gyro[2] * m23;
    m_gyro[2] = gyro[0] * m31 + gyro[1] * m32 + gyro[2] * m33;

    m_mag[0] = mag[0] * m11 + mag[1] * m12 + mag[2] * m13;
    m_mag[1] = mag[0] * m21 + mag[1] * m22 + mag[2] * m23;
    m_mag[2] = mag[0] * m31 + mag[1] * m32 + mag[2] * m33;

    // Accelerometer and Gyro offset compensation and estimation
    for (int i = 0;i < 3;i++) {
        m_accel -= m_settings.accel_offsets;
        m_gyro -= m_settings.gyro_offsets;

        if (m_settings.gyro_offset_comp_fact > 0.0) {
            utils_step_towards(&m_gyro_offset, m_gyro, m_settings.gyro_offset_comp_fact * dt);
            utils_truncate_number_abs(&m_gyro_offset, m_settings.gyro_offset_comp_clamp);
        } else {
            m_gyro_offset = 0.0;
        }

        m_gyro -= m_gyro_offset;
    }

    float gyro_rad[3];
    gyro_rad[0] = m_gyro[0] * M_PI / 180.0;
    gyro_rad[1] = m_gyro[1] * M_PI / 180.0;
    gyro_rad[2] = m_gyro[2] * M_PI / 180.0;

    madgwick(gyro_rad, m_accel, dt, (ATTITUDE_INFO*)&m_att);
}
      系统定时读取IMU后回掉该函数,该函数中的 if(!imu_ready && ST2MS(chVTGetSystemTimeX() - init_time) > 1000)语句让人十分迷惑,为何要在IMU初始化1000ms后才设置系统结算参数?
      在不明就里的情况下只能实验了!将1000改为500,实验发现收敛的更慢了,再将它改为2000,效果立刻就出来了,单片机上电后3s左右就收敛了!
经过一番分析,大概明白原因了:
       Madgwick互补滤波原理如下图所示:
Madgwick.png
    图中beta是四元数微分方程求解的姿态算法的收敛速度,beta越大收敛越快,但稳态精度越差,beta越小收敛越慢但稳态精度好,高速运动下beta要小一些,低速运动下beta要大一些.
    在IMU初始化阶段执行了如下函数:
   update_parameters(1.0,  2.0);
   将beta设为2.0,使得姿态尽快收敛,1000ms后调用如下函数:
   update_parameters(  m_settings.accel_confidence_decay,  m_settings.madgwick_beta);
   将beta设为正常值保证精度.由于我采用的单片机已经单片机上运行的其他app对系统时序的影响,导致1000ms后姿态还未能收敛,此时将beta设为正常值(很小)后就需要很长时间姿态才能收敛了!

回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

Archiver|手机版|小黑屋|计算机控制

GMT+8, 2024-3-29 16:34 , Processed in 0.068615 second(s), 21 queries .

Powered by Discuz! X3.4

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表