1. 姿态角简介

  • 俯仰角（Pitch）：绕X轴旋转角度，表示前后倾斜
  • 滚转角（Roll）：绕Y轴旋转角度，表示左右倾斜
  • 偏航角（Yaw）：绕Z轴旋转角度，表示左右旋转（需要磁力计辅助，MPU6050无磁力计）

2. 传感器数据的特点

  • 加速度计：测量重力方向，适合计算静态倾斜角，但对动态运动噪声敏感
  • 陀螺仪：测量角速度，短期精度高，但长期存在漂移

3. 互补滤波算法

通过加权融合加速度计和陀螺仪数据，利用陀螺仪短期稳定性和加速度计长期稳定性，实现准确的姿态估计。

公式示例：Angle=α×(Angle+GyroRate×dt)+(1−α)×AccelAngle

  • α 是滤波系数，一般取0.95左右
  • dt 是采样时间间隔
  • GyroRate是陀螺仪测得角速度
  • AccelAngle是由加速度计计算得到的角度

4. 代码实现（Arduino示例）

#include <Wire.h>

const int MPU_ADDR = 0x68;

float gyroXangle = 0, gyroYangle = 0;
float compAngleX = 0, compAngleY = 0;

unsigned long timer, timerPrev;
float dt;

void setup() {
  Wire.begin();
  Serial.begin(9600);

  // 唤醒MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  timerPrev = micros();
}

void loop() {
  unsigned long timerNow = micros();
  dt = (timerNow - timerPrev) / 1000000.0; // 转换成秒
  timerPrev = timerNow;

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true);

  int16_t AcX = Wire.read() << 8 | Wire.read();
  int16_t AcY = Wire.read() << 8 | Wire.read();
  int16_t AcZ = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read(); // 跳过温度数据
  int16_t GyX = Wire.read() << 8 | Wire.read();
  int16_t GyY = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read(); // 跳过GyZ

  // 加速度计角度计算（单位：度）
  float AccXangle = atan2(AcY, AcZ) * 180 / PI;
  float AccYangle = atan2(-AcX, sqrt(AcY * AcY + AcZ * AcZ)) * 180 / PI;

  // 陀螺仪角速度转换（单位：度/s，默认±250°/s量程）
  float gyroXrate = GyX / 131.0;
  float gyroYrate = GyY / 131.0;

  // 角度积分
  gyroXangle += gyroXrate * dt;
  gyroYangle += gyroYrate * dt;

  // 互补滤波融合
  const float alpha = 0.96;
  compAngleX = alpha * (compAngleX + gyroXrate * dt) + (1 - alpha) * AccXangle;
  compAngleY = alpha * (compAngleY + gyroYrate * dt) + (1 - alpha) * AccYangle;

  Serial.print("Pitch: ");
  Serial.print(compAngleX);
  Serial.print("  Roll: ");
  Serial.println(compAngleY);

  delay(10);
}

5. 说明

  • 使用 atan2 函数计算加速度计对应的角度
  • 角速度除以131是因为陀螺仪默认250°/s量程对应16位数据范围
  • 互补滤波的权重 α 可以调节，常见0.95~0.98之间
  • 该代码仅计算俯仰和滚转角，偏航角需用磁力计配合计算

6. 后续内容建议

  • 使用卡尔曼滤波进一步提高姿态估计精度
  • 增加偏航角计算，接入磁力计（MPU9250或外接HMC5883L）
  • 基于STM32或其他MCU的MPU6050驱动与姿态融合代码
  • 实际项目应用案例：无人机姿态控制、机器人平衡控制