面向自平衡机器人的级联 PID 控制

引言

你已经有一个可信的倾角了。现在要让机器人站在这个倾角上保持平衡——然后，最终，还要让它待在原地，而不是名义上站得笔直却一路漂过整个房间。

这是两件不同的事，而几乎所有人最先犯的错，就是想用一个环把它们一起干完。本文把它们搭成嵌套的两个环，真实的平衡机器人就是这么控制的。

单个环为什么不行

你想要的是一台停在哪就待在哪的机器人。你能下发的是电机转矩。夹在两者之间有一个令人不方便的事实：改变机器人位置的唯一办法，是先改变它的倾角。

试过直接控位置——按位置误差的比例驱动电机——轮子会从质心底下猛地窜出去。机身朝反方向转，机器人瞬间倒下，而且在倒下的路上还朝错误的方向挪了一段。这不是调参问题；初始响应的符号是真的反的，这个性质叫非最小相位。

出路是用级联的方式，以不同速度控制这两个量。

结构

   position error → [ OUTER PID ] → tilt target
                                        ↓
   tilt error     → [ INNER PID ] → motor command → robot
                                        ↑
                                   measured tilt

内环只做一件事：把它被告知的那个倾角保持住。它跑得很快——200 Hz 或更高——因为它对抗的是重力，而重力不会等你。

外环做另一件事：把机器人带到某个位置。它碰不了电机，所以靠向内环要求一个小的前倾来实现。当它想往后退时，它要求一个向前的前倾；内环驱动轮子维持这个前倾；维持一个向前的前倾，就意味着持续地向前加速。它的频率大约比内环慢十倍。

这种频率分离就是整个设计。如果外环的速度接近内环，两者会互相打架——外环改变目标的速度超过内环跟上的速度，机器人就会把自己振荡到地板上。

内环

float angleError = angle - tiltTarget;      // measured MINUS target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);

float command = Kp * angleError
              + Ki * angleIntegral
              + Kd * gyroRate;

command = constrain(command, -255, 255);

这里面有三处细节，比增益本身更重要。

误差是测量减目标，和教科书写法 设定点 − 测量 正好相反。要接住一台向前倒的机器人，你得把轮子往前驱动，钻到下落的质心下面去，就像你把手托到倒下来的扫帚下面一样。所以正倾角必须产生正的电机指令。符号弄反了，控制器一上电就会把机器人直接开进地板——这是一个很有辨识度的剧烈故障，几乎总是这个符号的问题。

微分项直接用陀螺仪角率，而不是对连续角度做差分。陀螺仪的原生输出就是角速率。对一个含噪的角度信号做差分，恰好会放大你最不想被放大的噪声，这也是很多第一次尝试需要把 D 增益压得可疑地低才能稳住的原因。

**积分要钳位。**摔倒过程中电机饱和而误差继续增长，不钳位的积分会卷绕成一个巨大的数字，回程时再和恢复过程对着干。把它钳住，并且在输出饱和时干脆停止累积。

外环

if (++slowTick >= 10) {                     // ten times slower
  slowTick = 0;
  float positionError = targetPosition - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);      // radians, about ±7°
}

wheelPosition 来自对编码器计数求和。如果你没有编码器，也可以把电机指令积分起来作为一个粗糙的代理，但这个环会明显变差。

钳位不是可选项。一个被允许要求 30° 的外环，会交给内环一个它物理上根本保持不住的目标——机器人认了一个它救不回来的前倾，然后在完美执行指令的同时摔倒。几度就够了；7° 的前倾已经能让机器人相当快地加速。

注意速度项是减掉的。它对接近过程提供阻尼，避免机器人冲过目标再折回来。

按行得通的顺序整定

先单独调内环。把 KpPos 和 KdPos 设为零，让外环完全不起作用、tiltTarget 保持为零。机器人会原地平衡并缓慢漂移——这是预期的，现在这样就行。

1. Ki 和 Kd 置零。加大 Kp，直到机器人对一次推挤有干脆的反应。太低就一路瘫到地板上、根本没真正对抗过；太高就嗡嗡响。
2. 加大 Kd，直到振荡被压掉。正是这一项把一台自己振倒的机器人变成一台能稳下来的机器人。在平衡模拟器里，你可以对一台正在跑的机器人把 D 置零，数一数它倒下之前摆了几次。
3. Ki 最后加，而且要温和。它消除 IMU 安装偏差造成的缓慢前倾。太多会让机器人出现一种缓慢增长的晃动，那是积分超调后再解卷出来的。

之后才启用外环，KpPos 从大约“看起来合理”的值乘十分之一起步。它的效果是间接的——你要看的是机器人不再漂移，而不是它反应有多干脆。

失效模式及其含义

过了可恢复角度就切电机

if (fabs(angle) > FALL_ANGLE) {             // about 35 degrees
  driveMotors(0, 0);
  angleIntegral = 0;
  return;
}

超过大约 35° 之后，要把质心重新拉回水平所需的加速度，已经超过轮子能在机身落地之前提供的能力。控制器并不知道这一点，会一路满转矩地较劲，这对减速箱很苛刻，也让故障变得没必要地吵闹。

切除输出同时会重置积分，所以把机器人重新扶起来时，你得到的是一个干净的环，而不是一个还带着摔倒时卷绕起来的修正量的环。

下一步去哪

这里的增益相互作用比单环 PID里强得多，因为被控对象是不稳定的——不存在一个“大致正确”只是性能差一点的档位。它要么站得起来，要么站不起来。

这让模拟器显得格外有用：你可以对一台正在平衡的机器人把 D 置零，精确观察那个故障，然后撤销它，而不用修任何东西。先在平衡模拟器里试预设，然后在自平衡车项目里做一台真的。