飞控控制悬停自转时机体漂移

Question

姿态解算采用madgwick解算四元数，yaw通过角速度融合地磁计算，高度通过加速度融合气压计，xy轴速度由加速度融合光流模块计算，积分得机体位置。

测试时候关闭位置环仅使用速度环，控制xy轴速度为0，摇杆输入yaw角速度到pid控制器实现旋转，机体无法定点或保持在小范围内。
是旋转状态下imu的读数有问题吗，需要怎么进行补偿

Answer 1

下午好🌅🌅🌅
本答案参考通义千问

在你描述的飞控系统中，姿态解算、传感器融合和控制逻辑是关键环节。当前的问题是：在悬停自转时，机体出现漂移，即使关闭了位置环，仅使用速度环，仍然无法保持稳定或定点。这表明问题可能出在姿态解算、传感器数据融合、或者控制环路的稳定性。

---

✅ 一、问题分析

1. 姿态解算与传感器融合

• 你提到使用 Madgwick算法 来解算四元数，这是比较常见的姿态解算方法。
• YAW 是通过角速度和地磁计融合计算的，但地磁计容易受到磁场干扰（如电机、金属结构）。
• 高度 由加速度和气压计融合，可能存在噪声或延迟。
• XY轴速度 由加速度和光流模块融合，积分得到位置，这可能导致积分漂移。

重点：

• YAW 的准确性对旋转控制至关重要，若YAW存在误差，会导致方向偏移。
• IMU读数 在旋转过程中是否准确？尤其是 角速度 和 加速度 是否被正确补偿？

---

✅ 二、可能原因及解决方案

1. YAW 精度不足

🧩 原因：

• 地磁计受干扰，导致 YAW 计算不准确。
• Madgwick算法未正确校准或参数设置不当。

🔧 解决方案：

• 校准地磁计：确保地磁计在无干扰环境中进行校准。
• 提高Madgwick算法的采样率，并优化 beta 参数（通常在0.1~0.2之间）。
• 增加YAW的权重：在融合中适当提升地磁计的权重（根据实际环境调整）。
• 使用互补滤波器：结合角速度和地磁计，避免纯角速度积分导致的YAW漂移。

✅ 示例代码（Madgwick算法优化）：

// 假设你使用的是Madgwick算法，可调整 beta 参数
float beta = 0.1f; // 根据实际测试调整

// 更新四元数
void MadgwickUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt) {
    float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
    float ex = 0.0f, ey = 0.0f, ez = 0.0f;

    // 重力向量
    float _ax = ax * (1.0f / 16384.0f);
    float _ay = ay * (1.0f / 16384.0f);
    float _az = az * (1.0f / 16384.0f);

    // 重力矢量误差
    float _v[3] = { 
        2*(q2*q4 - q1*q3), 
        2*(q1*q2 + q3*q4), 
        2*(0.5 - q2*q2 - q3*q3)
    };
    float _e = _ax - _v[0];
    ex += _e;
    _e = _ay - _v[1];
    ey += _e;
    _e = _az - _v[2];
    ez += _e;

    // 地磁计误差（假设已校准）
    float _mx = mx * (1.0f / 16384.0f);
    float _my = my * (1.0f / 16384.0f);
    float _mz = mz * (1.0f / 16384.0f);

    float _b[3] = {
        2*(q2*q4 - q1*q3),
        2*(q1*q2 + q3*q4),
        2*(0.5 - q2*q2 - q3*q3)
    };

    float _h[3] = {
        _mx - (_b[0] * (q2*q4 + q1*q3) - _b[1] * (q1*q2 - q3*q4) + _b[2] * (q1*q4 + q2*q3)),
        _my - (_b[1] * (q2*q4 - q1*q3) + _b[0] * (q1*q2 + q3*q4) - _b[2] * (q1*q4 - q2*q3)),
        _mz - (_b[2] * (q2*q4 + q1*q3) + _b[0] * (q1*q2 - q3*q4) - _b[1] * (q1*q4 + q2*q3))
    };

    ex += _h[0];
    ey += _h[1];
    ez += _h[2];

    // 更新四元数
    float s = 1.0f - beta * (ex * ex + ey * ey + ez * ez);
    float qDot1 = -q2 * gx - q3 * gy - q4 * gz + beta * ex;
    float qDot2 = q1 * gx - q4 * gy + q3 * gz + beta * ey;
    float qDot3 = q4 * gx + q1 * gy - q2 * gz + beta * ez;
    float qDot4 = -q3 * gx + q2 * gy + q1 * gz;

    q[0] += (qDot1 * dt * s);
    q[1] += (qDot2 * dt * s);
    q[2] += (qDot3 * dt * s);
    q[3] += (qDot4 * dt * s);

    // 归一化
    float norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    q[0] /= norm;
    q[1] /= norm;
    q[2] /= norm;
    q[3] /= norm;
}

---

2. IMU 读数不稳定或未补偿

🧩 原因：

• IMU 未进行零偏校准，导致角速度和加速度数据漂移。
• 旋转状态下，IMU 的 离心力影响 没有被正确补偿。

🔧 解决方案：

• 进行IMU零偏校准：在静止状态下采集一定时间的IMU数据，取平均值作为偏移量。
• 添加陀螺仪补偿机制：在旋转过程中，考虑 离心力引起的加速度变化，防止误判为真实运动。
• 使用卡尔曼滤波或互补滤波：对加速度、角速度等数据进行更精确的融合。

✅ 示例代码（IMU零偏校准）：

// 静止状态下的IMU校准
void imu_calibration() {
    int16_t gx_sum = 0, gy_sum = 0, gz_sum = 0;
    int16_t ax_sum = 0, ay_sum = 0, az_sum = 0;
    const int samples = 1000;

    for (int i = 0; i < samples; i++) {
        read_imu(&gx, &gy, &gz, &ax, &ay, &az);
        gx_sum += gx;
        gy_sum += gy;
        gz_sum += gz;
        ax_sum += ax;
        ay_sum += ay;
        az_sum += az;
    }

    gx_offset = gx_sum / samples;
    gy_offset = gy_sum / samples;
    gz_offset = gz_sum / samples;
    ax_offset = ax_sum / samples;
    ay_offset = ay_sum / samples;
    az_offset = az_sum / samples;
}

// 使用时减去偏移量
gx -= gx_offset;
gy -= gy_offset;
gz -= gz_offset;
ax -= ax_offset;
ay -= ay_offset;
az -= az_offset;

---

3. 积分漂移问题

🧩 原因：

• XY轴速度由加速度和光流融合计算，积分后会产生位置漂移，特别是在长时间运行时。
• 如果没有外部参考（如GPS），漂移会越来越严重。

🔧 解决方案：

• 加入外部参考（如GPS或视觉定位）来修正位置。
• 使用互补滤波：将光流和加速度融合，减少积分漂移。
• 限制积分项：在PID控制器中加入积分限幅，防止过大累积。

✅ 示例代码（积分限幅）：

// PID 控制器示例
float pid_update(float error, float dt) {
    static float integral = 0.0f;
    static float last_error = 0.0f;

    integral += error * dt;
    if (integral > MAX_INTEGRAL) integral = MAX_INTEGRAL;
    else if (integral < -MAX_INTEGRAL) integral = -MAX_INTEGRAL;

    float derivative = (error - last_error) / dt;
    last_error = error;

    return Kp * error + Ki * integral + Kd * derivative;
}

---

✅ 三、总结建议

| 问题点 | 原因 | 解决方案 | |--------|------|----------| | YAW 不稳定 | 地磁计干扰或Madgwick参数不合理 | 校准地磁计、优化Beta参数、增加YAW权重 | | IMU 数据漂移 | 未校准或未补偿离心力 | 进行零偏校准、添加离心力补偿 | | 积分漂移 | 速度积分未限制 | 使用互补滤波、加入积分限幅 |

---

✅ 四、建议调试步骤

1. 关闭所有控制环，观察IMU原始数据是否稳定。
2. 单独测试YAW控制，确认旋转是否准确。
3. 检查光流和加速度融合结果，是否有明显漂移。
4. 逐步开启控制环，观察漂移是否改善。

---

如果你能提供具体的IMU型号、传感器数据格式或PID参数，我可以进一步帮助你优化代码和算法。希望这些内容对你有帮助！