自平衡小车实战：IMU + PID 让两轮车自己站起来

桌上放着一辆两轮小车，两个轮子左右排开，没有第三个支撑点。松手，它本该往一边栽下去。但通电之后，它在原地微微抖动着站住了——你伸手推它一把，它先是往后退了几厘米，然后又自己晃回原位，稳稳立着。

这就是自平衡小车。它没有任何机械支撑，全靠每秒几百次的"测量—计算—驱动"循环，把一个本该倒下的系统硬生生维持在直立。这是 PID 控制最经典、也最有成就感的一个实战项目：抽象的 P、I、D 三个字母，在这里变成了你能用手推、能看它回正的真实物理反馈。

这篇假设你已经啃过两块前置内容，本篇不会重复它们：

• PID 三个参数到底各管什么、怎么从直觉上理解，看 robot-pid。下面会直接用角度环 PID，不再推导。
• MPU6050 怎么接线、寄存器怎么读、I2C 地址是多少，看 /sensor/mpu6050/ 和 I2C 原理。本篇默认你已经能读出加速度和角速度的原始值。

倒立摆：为什么往倾倒方向加速反而能扶正

自平衡小车的物理模型是两轮倒立摆——把一根杆子竖在小车上，重心在轮轴之上。重心越高，杆子越"想"倒，但有意思的是，重心高一点反而更好调，后面机械要点会讲为什么。

直立时，重心正好在轮轴正上方，系统处于不稳定平衡——任何微小扰动都会被放大。控制的核心逻辑只有一句话：

车往哪边倒，轮子就往哪边加速，用车身的"反冲"把头顶的重心接住。

想象你手心立一根扫帚。扫帚往前倒，你的手就得往前移动去接住它的底端；往后倒，手往后移。轮子之于车身，就是手之于扫帚。整个闭环长这样：

车身倾角偏离直立 (0°)
      │
      ▼
  IMU 测到 pitch 偏差
      │
      ▼
  PID 计算输出 = Kp·误差 + Kd·角速度
      │
      ▼
  两个电机同向加速，往倾倒方向冲
      │
      ▼
  车身被"接"回直立 → 误差变小 → 循环

注意这里电机是两个同向驱动（都往前或都往后），负责平衡前后倾倒。左右转向是另一套差速逻辑，本篇先不管，只做"站住"。

一句话讲清传感器融合

你可能会问：MPU6050 里既有加速度计又有陀螺仪，到底用哪个测角度？答案是两个都用，因为它们各有致命缺陷：

• 加速度计能直接算出当前倾角（靠重力方向），但车一动、电机一抖，它就被振动和加速度污染，读数像心电图一样跳。它长期准，短期噪。
• 陀螺仪测的是角速度，把角速度对时间积分就能得到角度，瞬时非常平滑干净。但积分会累积微小误差，几秒钟就"漂"出去十几度。它短期准，长期漂。

把两者的优点拼起来，最简单的方法是互补滤波：

pitch = 0.98 × (pitch_上次 + 陀螺仪角速度 × dt) + 0.02 × 加速度计算出的角度

陀螺仪积分项占 98%（保证平滑），加速度计项占 2%（每一帧悄悄把漂移往真值拉回来）。这一行公式就能给你一个又稳又不漂的 pitch 角，足够把车立住。想要更高精度可以上卡尔曼滤波，但对入门小车，互补滤波的性价比无人能及——别一上来就折腾卡尔曼。

完整可跑代码骨架

下面是主循环骨架。MPU6050 的读取细节复用 /sensor/mpu6050/，电机驱动复用 robot-motor-driver 和 电机控制指南，这里只示意引脚，以你的实际接线为准。

#include <Wire.h>

// ===== 引脚示意(以接线为准) =====
const int AIN1 = 5, AIN2 = 6, PWMA = 9;   // 左电机
const int BIN1 = 7, BIN2 = 8, PWMB = 10;  // 右电机

// ===== PID 参数(这是你唯一要反复调的三个数) =====
float Kp = 22.0;   // 先从这个量级试，下面讲怎么调
float Ki = 0.0;    // 入门先不开 I,容易积分饱和窜车
float Kd = 0.8;

float targetAngle = 0.0;  // 直立零点,需校准
float pitch = 0.0;
unsigned long lastT = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu6050_init();                 // 见 /sensor/mpu6050/
  pinMode(AIN1, OUTPUT); pinMode(AIN2, OUTPUT); pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT); pinMode(BIN2, OUTPUT); pinMode(PWMB, OUTPUT);
  lastT = millis();
}

void loop() {
  // 1) 读 MPU6050 原始值
  float ax, ay, az, gx, gy, gz;
  mpu6050_read(&ax, &ay, &az, &gx, &gy, &gz);

  // 2) 算 dt
  unsigned long now = millis();
  float dt = (now - lastT) / 1000.0;
  lastT = now;

  // 3) 互补滤波得 pitch
  float accAngle = atan2(ay, az) * 57.2958;       // 加速度计算角度(度)
  pitch = 0.98 * (pitch + gx * dt) + 0.02 * accAngle;

  // 4) 角度环 PID
  float error = targetAngle - pitch;
  float output = Kp * error + Kd * (-gx);          // D 项直接用角速度,更干净
  output = constrain(output, -255, 255);

  // 5) 驱动两个电机(同向)
  driveMotors(output);

  delay(5);   // 约 200Hz,够用
}

void driveMotors(float out) {
  int pwm = (int)fabs(out);
  bool fwd = out > 0;
  // 左
  digitalWrite(AIN1, fwd);  digitalWrite(AIN2, !fwd);  analogWrite(PWMA, pwm);
  // 右
  digitalWrite(BIN1, fwd);  digitalWrite(BIN2, !fwd);  analogWrite(PWMB, pwm);
}

几个关键设计选择，我直接给你结论：

• D 项用角速度 -gx，不用误差的差分。陀螺仪本身就直接测角速度，干净无噪声，省掉一次容易放大噪声的求导。这是自平衡小车的标准做法。
• I 项入门先设 0。积分项在小车上极易饱和，一饱和就突然全速窜出去，新手很难驾驭。先把 PD 调好能站住，再考虑要不要加一点点 I 消除静态零点偏差。
• 输出限幅在 ±255（8 位 PWM 满量程），防止 PID 算出离谱的值。

调试顺序：一步都别跳

调自平衡车最大的坑是"想一口吃成胖子"——三个参数一起调，最后哪个都不对。正确顺序是单变量、分阶段：

第一步：让车能读到准确的角度。 别急着开电机。把车手扶到正直立，串口打印 pitch，看读数是不是接近 0；慢慢前后倾车身，看 pitch 是否平滑地跟着变、方向对不对。这一步不过关，后面全白搭。同时记下直立时 pitch 的真实值，填进 targetAngle（零点校准）。

第二步：单独调 P，找能站住的临界。 Ki=0、Kd=0，只留 Kp。从小往大加：Kp 太小，车软绵绵地倒下去，电机"扶不动"；Kp 太大，车会高频剧烈抖动（过冲了）。把 Kp 加到车开始明显抖、但还能勉强维持的那个值附近——这就是临界 P。

第三步：加 D 压抖。 保持 Kp，从 0 开始慢慢加 Kd。D 是阻尼，专治抖动。加对了，刚才那个抖动会肉眼可见地平息，车从"哆嗦"变成"沉稳地站着"。Kd 太大反而会引入新的高频噪声抖动，往回收一点。

第四步：微调零点和参数。 如果车总是缓慢往固定一边漂，多半是 targetAngle 零点没校准准，微调它。这时若还有稳态偏差，可以加极小的 Ki（比如 0.05 起）试试。

你应该看到什么

调好之后，把架空的车轻轻放到地上松手，你应该看到：

• 车在原地站住，轮子来回做小幅度的快速微调（几毫米的前后蹭动），整体位置基本不漂。
• 你用手指轻推一下车身上沿，车会先朝被推方向冲出去几厘米"接重心"，然后晃一两下回到直立——而不是被推倒。
• 把车稍微倾斜着放下，它能自己摆正站起来。

如果是这个状态，恭喜，你已经把倒立摆控制住了。

故障排查表

现象	最可能的原因	怎么办
直接倒地、电机像没力	Kp 太小，或电机/电源功率不够	加大 Kp；换更有力的电机、电压拉到电机额定
站着但高频剧烈抖动	Kp 太大过冲，或 Kd 太小	减小 Kp、加大 Kd
总往固定一边缓慢窜	targetAngle 零点没校准准	串口看直立真实 pitch，回填零点
角度读数缓慢漂移	互补滤波系数不对 / 陀螺仪零偏	加大加速度计权重（0.98→0.97）；开机静止时采零偏
一动就突然全速窜出去	I 项积分饱和	先把 Ki 设 0，或给积分项加限幅
方向完全相反、越扶越倒	电机接线或 pitch 符号反了	把 driveMotors 里 fwd 取反，或 output 加负号

两个变体

变体一：加速度环做前进后退。 现在的车只会原地站着。要让它走，思路是串级控制：外层是速度环（你想要的目标速度 → 算出一个目标倾角），内层还是本篇的角度环。想前进，就给一个很小的前倾目标角，车为了"接住"前倾的重心，会持续往前滚——倒立摆走路本质上就是"可控地一直往前倒"。

变体二：蓝牙遥控。 接一个 HC-05 蓝牙模块，手机发指令改 targetAngle（控制前进后退）和左右电机的差速（控制转向）。平衡环完全不动，遥控只是在上面叠加偏置，互不干扰。

动手挑战

让车站稳之后，加一个目标速度=0 的速度外环，让它不光能站住，还能保持在原地不漂移（现在的纯角度环车会缓慢向某个方向溜走，因为它只管角度不管位置）。进阶一点：给一个固定的小前倾角，让它能稳定直行 1 米不倒。把你的 Kp/Kd 和直行视频记下来，这是你调过的第一个真正的串级控制系统。

小结与下一步

自平衡小车把 PID 从公式变成了你能用手推的实物：IMU 测倾角、互补滤波出稳定角度、PD 算出电机该出多大力、电机往倾倒方向加速接住重心。调试的全部秘诀就是那个顺序——先准角度，再单调 P 找临界，加 D 压抖，最后微调零点，一步都别跳。

站住只是开始。两轮平衡解决的是"如何控制一个不稳定系统"，接下来可以往两个方向走：

• 想玩多自由度的精确运动控制，去看 机械臂正逆运动学——从控制平衡升级到控制空间位置。
• 想把小车接入更大的机器人系统、用消息和节点组织起来，去看 机器人与 ROS。

更多机器人项目见 /robot/，传感器专题见 /sensor/。