基于Arduino Mega 2560的MPU6886转向角计算及漂移问题

问题根源分析 #

你的转向角每整圈漂移1°，核心问题出在以下几点：

• 加速度计角度计算完全失效：错误重复使用同一轴加速度数据，导致互补滤波器无法修正陀螺仪积分误差
• 旋转检测逻辑矛盾：归一化后的角度永远无法触发整圈计数条件，角度更新逻辑混乱
• 运动检测判断错误：误用当前角度值判断运动状态，导致静止时误判、运动时漏判

具体修复方案与代码修改 #

1. 修复加速度计数据读取与角度计算

绕Z轴旋转时，重力分量会落在X/Y轴上，需读取这两个轴的数据计算水平角度：

// 修改readAccelData函数，读取X和Y轴加速度
void readAccelData(int16_t *accelX, int16_t *accelY) {
    Wire.beginTransmission(MPU6886_ADDR);
    Wire.write(MPU6886_ACCEL_XOUT_H);
    if (Wire.endTransmission(false) != 0) {
        return;
    }

    Wire.requestFrom(MPU6886_ADDR, 4); // 读取X、Y轴共4字节数据
    if (Wire.available() == 4) {
        *accelX = Wire.read() << 8 | Wire.read();
        *accelY = Wire.read() << 8 | Wire.read();
    }
}

// 修改互补滤波器函数
float getComplementaryFilterReading(float dt) {
    int16_t gyroZ;
    readGyroData(&gyroZ);
    int16_t accelX, accelY;
    readAccelData(&accelX, &accelY);

    // 转换陀螺仪Z轴为度/秒
    float gyroZ_degPerSec = (gyroZ - gyroZ_zero) / 131.0;

    // 陀螺仪积分更新角度
    float angleFromGyro = totalAngle + gyroZ_degPerSec * dt;

    // 加速度计计算水平角度（绕Z轴旋转时的倾斜角）
    float angleFromAccel = atan2(accelY, accelX) * 180.0 / PI;
    // 转换为0-360°范围
    if (angleFromAccel < 0) angleFromAccel += 360.0;

    // 互补滤波器融合数据
    float alpha = 0.98;
    totalAngle = alpha * angleFromGyro + (1 - alpha) * angleFromAccel;

    // 处理整圈计数与角度归一化
    if (totalAngle >= 360.0) {
        rotationCount++;
        totalAngle -= 360.0;
    } else if (totalAngle < 0) {
        rotationCount--;
        totalAngle += 360.0;
    }

    return totalAngle;
}

2. 修复运动检测逻辑

改用陀螺仪角速度判断运动状态，避免角度值干扰：

// 修改loop中的运动检测部分
void loop() {
    unsigned long currentTime = millis();
    float dt = (currentTime - previousTime) / 1000.0;
    previousTime = currentTime;

    // 先读取陀螺仪角速度用于运动检测
    int16_t gyroZ;
    readGyroData(&gyroZ);
    float gyroZ_degPerSec = abs((gyroZ - gyroZ_zero) / 131.0);

    // 基于角速度判断运动状态
    if (gyroZ_degPerSec > movementThreshold) {
        isMoving = true;
        steadyStartTime = currentTime;
    } else if (isMoving && (currentTime - steadyStartTime > steadyDuration)) {
        isMoving = false;
        // 静止时强制用加速度计校准角度
        int16_t accelX, accelY;
        readAccelData(&accelX, &accelY);
        float angleFromAccel = atan2(accelY, accelX) * 180.0 / PI;
        if (angleFromAccel < 0) angleFromAccel += 360.0;
        totalAngle = angleFromAccel;
    }

    // 仅在运动时更新角度
    float angle;
    if (isMoving) {
        angle = getComplementaryFilterReading(dt);
    } else {
        angle = totalAngle;
    }

    // 移除原loop中的整圈检测代码（已移到互补滤波器内）
    // ... 其余代码保留
}

3. 优化周期性校准逻辑

仅在传感器静止时执行校准，避免引入错误零偏：

// 修改周期性校准部分
if (currentTime - lastRecalibrationTime > recalibrationInterval && !isMoving) {
    calibrateGyro();
    lastRecalibrationTime = currentTime;
}

4. 修复编码器重置逻辑

原编码器代码中增减后的重置条件错误，修正为：

void updateEncoder1() {
    // ... 其余代码保留
    if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011)
        encoderValue1++;
    if (encoderValue1 >= 10000) { // 修正为>=
        encoderValue1 = 0;
    }

    if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000)
        encoderValue1--;
    if (encoderValue1 <= -10000) { // 增加负方向重置
        encoderValue1 = 0;
    }
    // ... 其余代码保留
}

// updateEncoder2函数做相同修改

最终效果说明 #

• 修复后的互补滤波器会用加速度计的水平角度修正陀螺仪积分误差，消除整圈漂移
• 运动检测更准确，静止时自动校准角度到加速度计测量值
• 整圈计数逻辑正常，角度始终保持0-360°范围且无积累误差

内容的提问来源于stack exchange，提问作者Durvesh Choudhary