# 倒立摆串级控制：速度环如何给直立环“改目标”
两轮倒立摆有两个目标：摆杆不能倒，小车也不能一直向前或向后漂。直立环负责前者，速度环负责后者。
但这两个环不能各自算一份 PWM 再相加，否则它们会争抢电机。本文讨论的串级结构是：
```text
速度环 PI（外环，10 ms）
-> 输出目标角度偏移
-> 作为直立环的目标
直立环 PD（内环，5 ms）
-> 输出两轮共用的平衡 PWM
```
关键词是“串行”：速度环的输出进入直立环，成为它的一部分目标，而不是在内环之外另加一个电机指令。
## 1. 为什么采用串级
倒立摆主要通过改变车轮加速度，把支撑点移动到重心下方。摆杆角度变化快，因此内环必须用更短周期纠正倾倒；平均速度变化慢，因此由外环慢慢修正。
```text
速度太快 -> 外环让目标倾角向后移 -> 直立环驱动车轮减速
速度太慢 -> 外环让目标倾角向前移 -> 直立环驱动车轮加速
```
外环只改变“应该倾斜多少”，真正的电机输出仍由直立环统一计算。这样外环动作较慢、幅度较小，不会直接破坏摆杆的快速平衡。
## 2. 三个量不要混淆
```c
float ang; /* 摆杆当前实际角度 */
float s_ang_offset; /* 速度环给出的目标角度偏移 */
float out; /* 直立环输出的平衡 PWM */
```
三者关系是：
```text
实测角度 ang 与目标角度 s_ang_offset 比较
-> 误差 ang – s_ang_offset
-> 直立环计算出 out
```
`s_ang_offset` 不是摆杆当前偏离了多少度，而是速度环希望把平衡点向前或向后移动多少度。
例如：
```text
s_ang_offset = 0° -> 希望摆杆平衡在 0°
s_ang_offset = 2° -> 希望摆杆平衡在 2°
```
## 3. 控制周期
当前工程以 5 ms 为基本控制周期：
```text
直立环：5 ms，200 Hz
速度环：10 ms，100 Hz
```
速度环在每两次直立环计算中执行一次：
```c
s_speed_div++;
if (s_speed_div >= 2)
{
s_speed_div = 0;
s_ang_offset = PID_Calc(&s_pid, s_target, s_speed);
}
```
内环必须更快，因为它直接处理摆杆倾倒；外环更慢，因为它只负责平均速度的长期漂移。
## 4. 外环：速度 PI 输出目标角度
编码器每 5 ms 读取一次增量，左右轮增量取平均，得到车辆平均速度：
```c
int dl = Enc_ReadLeftDelta();
int dr = Enc_ReadRightDelta();
s_speed = (dl + dr) * 0.5f;
```
因此 `s_speed` 的单位是：
```text
编码器计数 / 5 ms
```
目标速度 `s_target` 必须使用完全相同的单位。若目标速度给的是 m/s，应先换算：
```c
s_target = speed_mps * ENC_COUNT_PER_METER * CTRL_DT;
```
外环每 10 ms 计算一次：
```c
s_ang_offset = PID_Calc(&s_pid, s_target, s_speed);
```
这里选择的符号约定是：
```text
速度误差 = 目标速度 – 实测速度
外环输出 = 目标角度偏移
```
当前外环输出限制为 `±4°`，积分项最多贡献约 `±1.2°`：
```c
PID_Init(&s_pid, SPEED_KP, SPEED_KI, SPEED_KD, 4.0f);
pid.integral_max = 0.3f * out_max;
```
这两个限制很重要。外环一旦要求摆杆长时间大角度倾斜，直立环会先失去稳定性，速度也就无从修正。
## 5. 内环：直立 PD 跟踪外环目标
直立环的核心只有一行：
```c
float out = (ang – s_ang_offset) * s_bal_kp
+ rate * s_bal_kd;
```
写成公式是：
```text
角度误差 = ang – s_ang_offset
直立输出 = Kp × 角度误差 + Kd × rate
```
当前参数：
```text
Kp = 600
Kd = 35
```
这里的 `ang` 和 `s_ang_offset` 都是角度，单位一致。`rate` 是摆杆角速度，当前从角度差分得到：
```text
rate = (angle[k] – angle[k-1]) / 0.005
```
角度差分噪声较大，所以角速度的平滑和滤波会直接影响 Kd 的可用范围。
直立环通常只用 PD：
1. 直立姿态需要快速响应，积分会引入滞后；
2. 零速度下的静态角度偏差可交给外环修正；
3. D 项提供阻尼，减少摆杆来回摆。
## 6. 完整的串行主链
省略测速底层、显示和转向混合后，核心循环可以简化为：
```c
void Control_Loop(void)
{
Angle_Update();
int dl = Enc_ReadLeftDelta();
int dr = Enc_ReadRightDelta();
s_speed = (dl + dr) * 0.5f;
float ang = Angle_Get();
float rate = Angle_GetRate();
/* 外环：10 ms 执行一次，输出目标角度 */
s_speed_div++;
if (s_speed_div >= 2)
{
s_speed_div = 0;
s_ang_offset = PID_Calc(&s_pid, s_target, s_speed);
}
/* 内环：5 ms 执行一次，输出平衡 PWM */
float out = (ang – s_ang_offset) * s_bal_kp
+ rate * s_bal_kd;
if (out > 7200.0f) out = 7200.0f;
if (out < -7200.0f) out = -7200.0f;
Motor_Left((int)out);
Motor_Right((int)out);
}
```
数据流可以概括为：
```text
编码器 -> 平均速度 -> 速度 PI -> 目标角度
↓
电位器 -> 当前角度 -> 直立 PD -> 平衡 PWM
↑
角速度
```
速度环和直立环共享“角度”这条通道，这正是串级连接。
## 7. 一个数值例子
假设小车比目标速度更快，速度环因此给出：
```text
s_ang_offset = -1°
```
这表示直立环暂时把平衡目标改成向后倾斜 1°，以帮助小车减速。
某一时刻摆杆状态为：
```text
ang = +2°
rate = +10°/s
```
先算角度误差：
```text
ang – s_ang_offset = 2° – (-1°) = 3°
```
再算直立输出：
```text
out = 600 × 3 + 35 × 10
= 1800 + 350
= 2150
```
如果直接比较 `ang=2°` 和 0°，会得到 1200；串级结构下得到 2150。多出的输出不是速度环直接加的 PWM，而是目标角度改变后，直立环重新计算的结果。
## 8. 正确与错误的连接方式
常见错误是并联：
```c
out = angle_pid(ang) + speed_pid(speed);
```
这里两个控制器都在直接争抢电机。速度环可能为了追速度持续增加 PWM，而直立环同时试图消除倾角，两个目标相互拉扯。
当前的串级结构是：
```c
s_ang_offset = speed_pid(s_target, s_speed);
out = angle_pid(ang, s_ang_offset, rate);
```
判断内环目标是否正确，可检查：
```text
s_ang_offset = 0 时，直立环希望平衡在 0°
s_ang_offset 为正时，直立环希望平衡在正角度
s_ang_offset 为负时，直立环希望平衡在负角度
```
若不满足，先检查 `ang – s_ang_offset` 的符号和角度方向，不要急着调参数。
## 9. 调参顺序
### 第一步：方向和单位
确认角度增大方向、电机前进方向、编码器计数方向和速度误差符号一致。方向错误时，增大任何 PID 参数都无法得到稳定控制。
把外环暂时旁路，令 `s_ang_offset=0`，先让直立环单独工作。
### 第二步：内环 Kp
Kd 先设为零，逐步增加 Kp：
– 太小：摆杆倾倒时车轮追不上；
– 合适：能来回修正，但可能持续摆动；
– 太大：高频抖动、电机嗡鸣或 PWM 快速饱和。
### 第三步：内环 Kd
加入 Kd 抑制摆动：
– 太小：摆杆或车头反复摆动；
– 合适：回正过程更利落；
– 太大：角度噪声被放大，电机高频抖动。
Kd 调不上去时，先检查角度噪声和角速度质量。
### 第四步：外环 Kp
恢复速度环，先令 `Ki=0`。逐渐增加速度 Kp：
– 太小：被推一下后长时间漂移；
– 合适：小车能缓慢回到目标速度；
– 太大：为了追速度产生明显的前后摆动。
外环输出必须保持在 `±4°` 内。
### 第五步：外环 Ki
最后从小值开始增加 Ki，消除摩擦和左右轮差异造成的长期偏差。若积分一开启就把目标角度推向限幅，优先检查积分符号、目标速度单位和积分限幅。
## 10. 常见现象及排查
| 现象 | 优先检查 |
|—|—|
| 内环高频抖动 | 角度噪声、Kd、内环 Kp、机械间隙 |
| 前后低频摆动 | 外环 Kp 过大，或外环周期过短 |
| 始终向一个方向漂 | 测速方向、速度误差符号、外环积分 |
| 外环长期卡在 `±4°` | 内环无法实现目标角、参数比例或方向错误 |
| 速度几乎为零但目标角明显偏移 | 积分残留、积分限幅或速度单位错误 |
| 直立稳定但速度不受控 | 外环未执行、反馈符号错误或目标速度不一致 |
## 11. 小结
倒立摆串级控制的关键不是再写一个 PID，而是明确两个环的职责：
```text
外环慢：控制平均速度，输出目标倾角
内环快：控制摆杆姿态，输出电机 PWM
```
速度环通过改变目标角度，间接让直立环带着小车加速或减速。只要保持这条串行数据链、符号约定和输出限幅一致，直立与速度就可以分别调试，也能避免两个控制器直接争抢电机。
##12.实际效果演示视频
倒立摆小车
