MPU6050移动后静置姿态值漂移问题排查与代码修改咨询

问题现象 #

• 串口监视器启动后初始姿态值全为0
• 晃动设备后姿态数值持续偏移累积，将设备放回水平桌面静止后，输出的姿态值仍为非零状态，无法回归零位
• 现有测试代码如下：

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// ============== LEDs Setup ================
int roll_Left = 9;
int pitch_Up = 10;
int center = 11;
int pitch_Down = 12;
int roll_Right = 13;
// =================================================

// Timers
unsigned long timer = 0;
float timeStep = 0.01;

// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;

void setup() 
{
  Serial.begin(115200);

    //============= LED Pin Init ===========
  pinMode(roll_Left, OUTPUT);
  pinMode(pitch_Up, OUTPUT);
  pinMode(center, OUTPUT);
  pinMode(pitch_Down, OUTPUT);
  pinMode(roll_Right, OUTPUT);
  //======================================

  // Initialize MPU6050
  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }
  
  // Calibrate gyroscope. The calibration must be at rest.
  mpu.calibrateGyro();

  // Set threshold sensivty. Default 3.
  mpu.setThreshold(3);
}

void loop()
{
  timer = millis();
  // Read normalized gyro values
  Vector norm = mpu.readNormalizeGyro();

  // Calculate Pitch, Roll and Yaw by pure gyro integration
  pitch = pitch + norm.YAxis * timeStep;
  roll = roll + norm.XAxis * timeStep;
  yaw = yaw + norm.ZAxis * timeStep;

  // LED control logic
  if ((pitch <= 2) && (pitch >= -2) && (roll <= 2 )&& (roll >= -2)) {
    digitalWrite(center, HIGH);
  }else {
    digitalWrite(center, LOW);
  }
  
  Serial.print(" Pitch = ");
  Serial.print(pitch);
  if (pitch > 3) {
    digitalWrite(pitch_Up, HIGH);
  }else if (pitch < -3) {
    digitalWrite(pitch_Down, HIGH);
  }else {
    digitalWrite(pitch_Up, LOW);
    digitalWrite(pitch_Down, LOW);
  }
  Serial.print(" Roll = ");
  Serial.print(roll);
    if (roll > 3) {
    digitalWrite(roll_Right, HIGH);
  }else if (roll < -3) {
    digitalWrite(roll_Left, HIGH);
  }else {
    digitalWrite(roll_Left, LOW);
    digitalWrite(roll_Right, LOW);
  }
  Serial.print(" Yaw = ");
  Serial.println(yaw);

  // Wait to full timeStep period
  delay((timeStep*1000) - (millis() - timer));
}

根本原因 #

现有代码存在三个核心问题，必然导致漂移：

• 纯陀螺仪积分计算姿态：陀螺仪输出的是角速度，直接对角速度积分得到角度的方式，只要传感器存在微小的零偏误差，误差就会随时间不断累积，最终完全偏离真实角度，这是所有MEMS陀螺仪的固有特性，不是单次校准就能完全消除的
• 完全没有用到加速度计数据：MPU6050自带的加速度计在静态/低动态下可以准确测量重力方向，算出绝对的俯仰、横滚角度，这个值不会漂移，现有代码完全没读取加速度计数据，没有参考值校正积分误差
• 时间步长写死：固定用0.01s作为积分步长，但实际循环运行时间受代码执行、串口输出影响不可能完全等于10ms，步长误差会进一步放大积分漂移

修复方案 #

按优先级修改即可解决静态漂移问题：

1. 同时读取加速度计归一化数值，在静态下用加速度计计算无漂移的俯仰、横滚参考角
2. 用互补滤波融合陀螺仪和加速度计数据：陀螺仪数据负责动态响应，加速度计数据负责校正长期漂移，实现简单、算力占用极低，完全满足Arduino这类主控的需求
3. 替换固定时间步长，每次循环实际计算和上一次循环的时间差，作为真实积分步长
4. 注意：MPU6050没有磁力计，偏航角（yaw）没有绝对参考，依然会随时间漂移，这是硬件限制，没有办法完全消除，只能靠额外加磁力计校正

核心修改后代码示例 #

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// LED引脚定义
int roll_Left = 9;
int pitch_Up = 10;
int center = 11;
int pitch_Down = 12;
int roll_Right = 13;

// 计时变量
unsigned long lastTime = 0;
float timeStep = 0.01;

// 姿态角
float pitch = 0;
float roll = 0;
float yaw = 0;
// 互补滤波系数，越大越信任陀螺仪，越小越信任加速度计，一般取0.95~0.98
const float alpha = 0.96;

void setup() 
{
  Serial.begin(115200);
  // 初始化LED引脚
  pinMode(roll_Left, OUTPUT);
  pinMode(pitch_Up, OUTPUT);
  pinMode(center, OUTPUT);
  pinMode(pitch_Down, OUTPUT);
  pinMode(roll_Right, OUTPUT);

  // 初始化MPU6050
  while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  {
    Serial.println("未检测到MPU6050，请检查接线！");
    delay(500);
  }
  
  // 静止时校准陀螺仪
  mpu.calibrateGyro();
  // 陀螺仪零偏阈值
  mpu.setThreshold(3);
  lastTime = millis();
}

void loop()
{
  // 计算真实时间步长
  unsigned long now = millis();
  timeStep = (now - lastTime) / 1000.0f;
  lastTime = now;

  // 同时读取陀螺仪、加速度计归一化数据
  Vector gyro = mpu.readNormalizeGyro();
  Vector acc = mpu.readNormalizeAccel();

  // 1. 先通过陀螺仪积分得到动态角度
  float pitchGyro = pitch + gyro.YAxis * timeStep;
  float rollGyro = roll + gyro.XAxis * timeStep;
  yaw = yaw + gyro.ZAxis * timeStep; // yaw无加速度参考，只能积分，必然漂移

  // 2. 通过加速度计计算静态绝对角度
  float pitchAcc = atan2(acc.YAxis, acc.ZAxis) * 180 / PI;
  float rollAcc = atan2(-acc.XAxis, sqrt(acc.YAxis*acc.YAxis + acc.ZAxis*acc.ZAxis)) * 180 / PI;

  // 3. 互补滤波融合两个角度
  pitch = alpha * pitchGyro + (1-alpha) * pitchAcc;
  roll = alpha * rollGyro + (1-alpha) * rollAcc;

  // 原有LED控制和串口输出逻辑
  if ((pitch <= 2) && (pitch >= -2) && (roll <= 2 )&& (roll >= -2)) {
    digitalWrite(center, HIGH);
  }else {
    digitalWrite(center, LOW);
  }
  
  Serial.print(" Pitch = ");
  Serial.print(pitch);
  if (pitch > 3) {
    digitalWrite(pitch_Up, HIGH);
  }else if (pitch < -3) {
    digitalWrite(pitch_Down, HIGH);
  }else {
    digitalWrite(pitch_Up, LOW);
    digitalWrite(pitch_Down, LOW);
  }
  Serial.print(" Roll = ");
  Serial.print(roll);
    if (roll > 3) {
    digitalWrite(roll_Right, HIGH);
  }else if (roll < -3) {
    digitalWrite(roll_Left, HIGH);
  }else {
    digitalWrite(roll_Left, LOW);
    digitalWrite(roll_Right, LOW);
  }
  Serial.print(" Yaw = ");
  Serial.println(yaw);
}

校准陀螺仪的时候一定要保证传感器完全静止，不要触碰，否则校准得到的零偏不准，依然会有明显漂移。如果对精度要求更高，可以把互补滤波换成卡尔曼滤波，逻辑是一致的，都是用加速度计的绝对参考校正陀螺仪漂移。

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