MPU9250/MPU6050静止时Yaw值漂移及纯陀螺仪获取Yaw可行性咨询

核心问题解答：仅靠陀螺仪能否获取有效Yaw值？ #

• 短期（几秒到几十秒）可以得到相对准确的Yaw值，但长期必然会出现漂移——这是MEMS陀螺仪的硬件特性，零偏误差会随时间积分积累，导致Yaw值持续偏移。
• 你看到的YouTube博主“无异常”，要么是测试时间短，要么是做了陀螺仪零偏校准+固定采样间隔的优化，抵消了部分漂移，并非完全消除。中断模式的核心作用是保证采样时间稳定，减少积分误差，而非解决漂移本身。

你的代码存在的关键问题 #

1. 未做陀螺仪零偏校准：静止时陀螺仪z轴输出并非绝对为0，存在固定零偏，直接积分会不断积累误差，导致Yaw持续上升。
2. 每次循环重复配置传感器：acc_gyro_mag()里每次都写0x1A（DLPF配置）和0x1B（陀螺仪量程）寄存器，完全没必要，会增加I2C通信延迟，导致采样间隔不稳定。
3. 采样时间基准不稳定：用millis()计算elapsedTime，但loop()的执行时间不固定，且注释掉了固定间隔的代码，积分步长波动会放大漂移。
4. 无漂移补偿逻辑：即使做了校准，长时间仍会有漂移，但你的代码完全没有处理这部分。

修复方案及代码示例 #

1. 先完成陀螺仪零偏校准 #

静止放置传感器，采集多组z轴数据计算零偏，后续读取时减去该值。

2. 固定采样间隔 #

用micros()严格控制每次采样的时间间隔，保证积分步长一致。

3. 移走重复的传感器配置 #

将传感器初始化配置全部放到setup()中。

修改后的代码：

#include <Arduino.h>
#include <Wire.h>

const int mpuAccGyro = 0x68; // ADO接地为0x68，接5V为0x69

// 陀螺仪原始数据及零偏误差
float x_gyro, y_gyro, z_gyro;
float gyro_z_offset = 0.0; // z轴零偏校准值

// 姿态角
float yaw = 0.0;

// 时间控制
uint32_t previousTime = 0;
const uint32_t sampleInterval = 4000; // 4ms采样间隔（250Hz）

void setup() {
  Serial.begin(115200);
  pinMode(2, OUTPUT);
  digitalWrite(2, HIGH);
  delay(100);
  digitalWrite(2, LOW);

  // I2C初始化
  Wire.setClock(400000);
  Wire.begin();
  delay(250);

  // 唤醒传感器
  Wire.beginTransmission(mpuAccGyro);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  delay(10);

  // 配置陀螺仪量程（±500°/s）
  Wire.beginTransmission(mpuAccGyro);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();
  delay(10);

  // 配置DLPF（低通滤波，减少噪声）
  Wire.beginTransmission(mpuAccGyro);
  Wire.write(0x1A);
  Wire.write(0x01);
  Wire.endTransmission();
  delay(10);

  // 陀螺仪零偏校准：静止采集500次数据
  digitalWrite(2, HIGH);
  float z_sum = 0.0;
  for (int i = 0; i < 500; i++) {
    Wire.beginTransmission(mpuAccGyro);
    Wire.write(0x43);
    Wire.endTransmission();
    Wire.requestFrom(mpuAccGyro, 2);
    int16_t Z_gyro = (Wire.read() << 8) | Wire.read();
    z_sum += (float)Z_gyro / 65.5;
    delay(2);
  }
  gyro_z_offset = z_sum / 500.0;
  digitalWrite(2, LOW);
  Serial.print("Gyro Z Offset: ");
  Serial.println(gyro_z_offset);

  previousTime = micros();
}

void loop() {
  // 固定采样间隔
  while (micros() - previousTime < sampleInterval);
  uint32_t currentTime = micros();
  float elapsedTime = (currentTime - previousTime) / 1000000.0; // 转换为秒
  previousTime = currentTime;

  // 读取陀螺仪z轴数据
  Wire.beginTransmission(mpuAccGyro);
  Wire.write(0x43 + 4); // 直接读取z轴寄存器（0x47、0x48）
  Wire.endTransmission();
  Wire.requestFrom(mpuAccGyro, 2);
  int16_t Z_gyro = (Wire.read() << 8) | Wire.read();
  z_gyro = ((float)Z_gyro / 65.5) - gyro_z_offset; // 减去零偏

  // 积分计算Yaw
  yaw += z_gyro * elapsedTime;

  Serial.print("Yaw value: ");
  Serial.println(yaw);
}

代码说明 #

• 零偏校准：在setup()中采集500次静止时的z轴数据，计算平均值作为偏移量，后续读取时减去该值，消除固定零偏。
• 固定采样间隔：用micros()控制每次采样间隔为4ms，保证积分步长稳定，减少误差积累。
• 简化数据读取：直接读取z轴寄存器，减少不必要的I2C通信，提高效率。

额外说明 #

即使做了以上优化，纯陀螺仪方案的Yaw仍会在几十分钟后出现明显漂移。如果需要长期稳定的Yaw值，必须结合磁力计（航向角校准）或GPS（户外场景）进行融合修正，比如用卡尔曼滤波或互补滤波。

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