自平衡机器人模拟器：倒立摆 PID 控制

控件

开始、暂停和重置用来运行这个回路。推像手指那样施加一次冲量，而环开关会同时禁用两个控制器，好让你看清裸露的被控对象是怎么倒下去的。

最直接的控制方式就是机器人本身：拖动机身到任意角度然后松手。你留下的那个位姿就是控制器接手的地方——没有重新布防这一步，而且增益滑块会立刻作用于正在运行的仿真。在它站住的时候把微分增益调到零，它就在同一秒里开始摇晃。

调参菜单会加载四个预设：一个能成功，以及三个分别以真实平衡车失败的三种方式失败的预设。

把画布聚焦之后，用空格、S、L、R 和 F 可以从键盘上使用同样的控制。

原理

让一台机器人用两个轮子站住，意味着要把一个倒立摆扶正。质心位于车轴上方，所以竖直是一个平衡点，其意义和一支立在笔尖上的铅笔一样：数学上真实存在，物理上没有持续修正就完全没戏。把它倾斜任何角度，重力都会产生一个让它倾得更远的扭矩。倾得越远，倒得越快。

机器人对抗这个扭矩只有一条办法——它可以移动车轴。轮子向前加速会把底盘推回质心下方，从而把机身转回竖直。这就是为什么这个修正一开始感觉是反的：要止住向前倒，你就必须向前开，就像你把手放在一把正在平衡的扫帚下面，而不是拿开它。

这就给机器人设下了一个硬极限。大约超过 35° 之后，所需的水平加速度会超过轮子在机身撞上地板之前所能提供的量，再多的增益也无济于事。曲线图上的虚线标出的就是这条边界。

算法

控制器是级联的——两个 PID 回路，一个喂给另一个。

内层回路跑得快，负责保持一个倾角。它把测得的角度与目标作比较，并指令电机扭矩：

error   = measured_angle − target_angle
motor   = Kp·error + Ki·∫error + Kd·d(measured)/dt

注意误差是测量值减目标值，而不是教科书上的目标减测量。要接住一次向前倒，就必须把轮子向前开，所以一个正的倾角必须产生一个正的电机指令。这个符号是第一台平衡车一开机就把自己拍到地板上的最常见原因。

外层回路跑得慢，负责保持一个位置。它无法直接指令电机——电机已经被内层占用了——所以它改为给内层回路指令一个倾角目标：

target_angle = Kp_pos·(desired_position − position) − Kd_pos·velocity

当它落在想要的位置后面时，就要求一个小的前倾；内层回路随后驱动轮子保持这个倾角，机器人就向前移动。这个倾角被刻意限制在几度之内。一个被允许要求大角度的外层回路，会递给内层一个它保持不住的目标，于是两个回路一起垮掉。

积分项在这里比在大多数回路里都更重要，因为正是它吸收了 IMU 认为的竖直与真实竖直之间的差值。它也是为什么积分项必须在电机饱和时停止累积——否则它会在一次自己无力阻止的跌倒过程中饱和，然后在恢复阶段反过来与控制器作对。

源代码

// Inner loop: tilt → motors. Runs every 5 ms off a timer, not in loop().
float angleError = angle - tiltTarget;          // measured minus target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);
float command   = Kp * angleError
                + Ki * angleIntegral
                + Kd * gyroRate;                // rate straight from the gyro

command = constrain(command, -255, 255);
if (fabs(angle) > FALL_ANGLE) command = 0;      // past saving; cut the motors
driveMotors(command, command);

// Outer loop: position → tilt target. Runs ten times slower, and may only
// ever ask for a small lean.
if (++slowTick >= 10) {
  slowTick = 0;
  float positionError = 0 - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);          // radians, about ±7°
}

有两处细节可以直接从这个模拟器搬到实物上。微分项要取自陀螺仪角速度，而不要通过对角度做差分得到：陀螺仪直接测量这个角速度，而且没有微分运算会放大的那种噪声。另外，超过恢复角度之后要切断电机，而不是任由控制器继续硬撑——这既保护了减速箱，也让故障变得明显而不是狂暴。

硬件

传感器是 MPU6050，它同时包含加速度计和陀螺仪，因为单独任何一个都不可用。 加速度计知道哪个方向是下的，但会被机器人自身的运动淹没；陀螺仪能干净地跟踪 旋转，却在几秒内就漂移。互补滤波把两者融合 成一个既稳定又快捷的角度。

电机需要足够低的减速比来提供扭矩，又需要足够快来接住一次跌倒—— N20 减速电机在 200 RPM 左右是常见选择，由 TB6612FNG 驱动；当回路每秒要求几千次小修正时，它的低压降就很要紧。

在自平衡车项目里把它真正做出来； 如果 IMU 对你来说是新东西，就从倾角读取教程开始。

假设与硬件差异

这个模型是一个 2D 倒立摆，只有一个倾转轴、刚性连杆、没有轮子打滑， IMU 报告真实角度和角速度，只带上你调出来的漂移和噪声。真实的平衡车 会感到随电池电压变化的电机扭矩、编码器量化、IMU 安装偏移和温度漂移， 而且它还可能朝侧面倒——这是一个自由度，而本实验台把它完全去掉了。 这里展示的级联 PID 结构和陀螺角速度微分可以直接迁移，但安装配平、 滤波整定和跌倒角度截止值都必须在实物底盘上重新找出来。

电路图

有两条接线规则决定这台机器人能不能工作。

**电机电池只接驱动器的 VM 引脚，别接别处。**电机电流绝不能流经 Arduino， 而且每一处地——板子、驱动器、IMU、电池负极——都必须连在一起， 否则控制信号没有共同的参考点，故障会悄无声息而不是显而易见。

**编码器从逻辑电源供电，而不是从电机电源轨。**用电机电源给编码器供电， 要么烧掉它，要么让计数里塞满开关噪声。还要把编码器的 A 通道接在一个 支持中断的引脚上；在 loop() 里轮询它会丢计数，而丢计数意味着外层回路 会慢慢跟丢机器人的位置。

把 IMU 装在靠近车轴的地方。装得高会把它在每次修正时看到的线性加速度 成倍放大，这只会让融合问题更难，却不带来任何好处。