从 MPU6050 加速度计与陀螺仪读取倾角

MPU6050是“上哪”最便宜也最诚实的答案。它在一颗芯片上装着两台独立的传感器——一颗三轴加速度计和一颗三轴陀螺仪——而一台自平衡车两样都要，因为每一台单独用时都完全没用，而另一台恰好能补偿它。

引言

本文带你从一块裸模块走到一个静止时你可以信赖的、以度为单位的倾角。让这个角度在机器人运动时依然站得住，就是互补滤波，那是下一篇，并且依赖这里的全部内容。

每台传感器实际在量什么

加速度计并不量倾角。它量加速度，包括重力带来的那份加速度——这正是它能当倾角传感器用的原因：静止时唯一的加速度就是指向下的 1 g，而那个矢量在三个轴上如何分布，就告诉了你朝向。

“静止”正是关键。传感器分不出重力和任何其他加速度。你的轮子一动，加速度计报的就是重力与机器人自身运动的矢量和，于是报出一个并不存在的倾角。在平衡车上——按定义它从不停止加速——这个误差是持续的。

陀螺仪量角速率，单位是度每秒。把速率对时间积分就得到角度。陀螺对线性加速度免疫，所以在机器人运动时保持诚实。

它的弱点是积分会累积误差。速率读数里任何微小的恒定偏置，都会积成一个无限增长的角。一颗完全静止时读 0.5 °/s 的陀螺仪，一分钟后会声称你转过了 30°。这就是漂移，而且它没法靠焊得更好来修。

所以：加速度计长期正确、瞬时错误；陀螺仪瞬时正确、长期错误。正是这种对称性构成了传感器融合存在的全部理由。

接线

MPU6050 说的是 I²C，所以要四根线——而如果扫描器在 0x68 上找不到它，那一页解释清楚了为什么它的手册写的是 0xD0。

在平衡车上，把模块装成它的一根轴真正水平、并且尽可能靠近轮轴。装得越高，它在一次修正期间看到的线性加速度就越大，这让融合问题更难却不带来任何好处。

读原始值

你要的寄存器是连续的，所以一次突发读就把全部拿到：

#include <Wire.h>

const uint8_t MPU = 0x68;        // 0x69 if AD0 is tied high

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);              // PWR_MGMT_1
  Wire.write(0);                 // clear sleep bit — it boots asleep
  Wire.endTransmission(true);
}

void readRaw(int16_t& ax, int16_t& ay, int16_t& az,
             int16_t& gx, int16_t& gy, int16_t& gz) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);              // ACCEL_XOUT_H
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)14, (uint8_t)true);

  ax = Wire.read() << 8 | Wire.read();
  ay = Wire.read() << 8 | Wire.read();
  az = Wire.read() << 8 | Wire.read();
  Wire.read(); Wire.read();      // temperature, discarded
  gx = Wire.read() << 8 | Wire.read();
  gy = Wire.read() << 8 | Wire.read();
  gz = Wire.read() << 8 | Wire.read();
}

模块开机时是睡着的。忘了清掉 PWR_MGMT_1 里的睡眠位，会得到一串稳定的零，它读起来和接线故障一模一样，让人们白跑一趟烙铁。

把计数换算成单位

原始值是跨过传感器所配置量程的有符号 16 位计数。在默认设置下：

• 加速度计：±2 g 对应 ±32768 计数 → 每 g 16384 计数
• 陀螺仪：±250 °/s 对应 ±32768 计数 → 每 °/s 131 计数

float accelX = ax / 16384.0;     // g
float rateY  = gy / 131.0;       // degrees per second

从加速度计求倾角

机器人竖直、板子竖着装的情况下，机器人倾倒所在那个平面内的倾角来自三个轴中的两个：

float pitch = atan2(accelX, accelZ) * 180.0 / PI;

用 atan2，不要用单轴的 asin。atan2 吃两个分量，所以它在全部四个象限里都正确，而且不会在一个轴趋近零时爆掉。单轴 asin 那个版本在台架上小角度看着没问题，然后在机器人第一次倾过 45° 时就开始行为古怪。

标定偏移

每一颗 MPU6050 都有制造偏移。你的陀螺仪静止时不会读零，你的加速度计也不会在一根轴上正好读 1 g、其余读零。在开机时量一次误差，量的时候机器人要保持静止：

float gyroBiasY = 0;

void calibrate() {
  const int samples = 1000;
  long total = 0;
  for (int i = 0; i < samples; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    readRaw(ax, ay, az, gx, gy, gz);
    total += gy;
    delay(2);
  }
  gyroBiasY = (total / (float)samples) / 131.0;
}

之后每次速率读数都把 gyroBiasY 减掉。这一步就是“陀螺仪每分钟漂一度”和“每秒钟漂一度”之间的区别。

关于它有两点要知道。标定期间传感器必须真正静止——你手扶着机器人做标定，就会把手抖烤进偏置里。而偏置随温度变化，所以冷标定的机器人会随着电子器件发热而稍微漂一点。对平衡机器人来说，控制环里的积分项会吸收掉那点残余，这也是平衡控制器几乎总是需要一个积分项的原因之一。

安装偏差与传感器偏置不是同一回事

还有第二个偏移是标定去不掉的：你的 IMU 装得并不完美。如果板子相对真正的竖直偏了 1.5°，一颗标定完美的传感器在机器人真正竖直时仍然会报 1.5°。

这在平衡车上极其要紧，而在台架上完全看不出来。控制环会忠实地保持你告诉它“这就是竖直”的那个角度——所以 1.5° 的误差意味着机器人保持着一个永久的 1.5° 前倾，这意味着持续加速，这意味着它会一路开过房间直到撞上东西。

量它的办法是：把机器人用手扶到真正平衡站住，记下角度读数是多少。把那个数当常数减掉。你可以在平衡模拟器里看到这个确切的失效——安装偏差滑块注入一个安装误差，机器人一边坚持自己水平、一边漂走。

在它之上盖东西之前先验证

把角度打印出来，用手核对三件事：

1. **竖直时读到接近零。**不是的话，你有安装偏差要减掉。
2. 把机器人向前倾，角度增大；向后倾，角度减小。符号反了就在这里换掉，而不要在控制环里补偿——那会在后面让你更困惑。
3. **静止时读数稳定，一动就立刻响应。**静止时嘈杂说明去查接线和电源；发滞说明某处平均得太过头。

一旦角度在静止时可信，你就准备好用互补滤波让它在运动时也可信，然后在级联 PID 平衡控制里闭合这个环了。