倒立摆串级 PID 复习博客（完整版）

覆盖课件 7.1 – 7.7，共 62 张幻灯片。所有插图取自原始课件（已裁去空白边距），
所有数值用 Python 复算过。

考前速览请用 → 精简版（一页纸）

原理总图：① 三层串级结构 ② 把非线性搬进单片机的实现链 ③ 参数与公式速查。建议先把这张图看熟。

〇、课程全景：七份课件是一条链，不是七个知识点

一条主线：

控制方案 建立模型 拼成框图 消除非线性 整定与实现 7.1 三级串级PID 7.2 电机闭环1/(s/40+1) 7.4 完整框图 7.5 逆解算ẍ=f(θ̈,θ) 7.6 Kp / Ki 7.3 倒立摆 θ̈=f(θ,ẍ) 把非线性搬进单片机 7.7 C 代码

符号约定（全文统一）

排版提示：θ̇ 用的是「θ + U+0307 组合上点」。Windows 上的微软雅黑、Calibri、Arial、Times 都会把这个点丢掉（显示成 θ），
只有 Consolas 和宋体/仿宋/楷体等少数字体支持。浏览器通常会自动回退，若看到「θ」请换字体。

一、串级 PID 控制方案（7.1）

1.1 被控对象：车轮上的倒立摆

把车身的上半部分近似成一个质量集中于一点的摆（摆质量 mp、摆长 lp），下面是一只能前后驱动的车轮。
控制目标是让摆保持在竖直位置，即 θ → 0。

课件 7.1 p2：左边是正常的摆（稳定平衡点在下），右边是倒立摆（稳定平衡点在上）。

控制直觉非常关键：车体前倾 → 车轮向前加速；车体后倾 → 车轮向后加速。
就像用手托住一根正在倒下的扫帚，底盘要往它倒下去的方向「追」过去，把角追回竖直。
这也解释了为什么唯一可控输入是加速度 ẍ —— 控制摆角靠的就是底盘的加速度。

课件 7.1 p3：单级 PID —— 只能控制轮胎转速 ω，不能保证车身平衡。

1.2 传感器与执行器

关键点：MPU6050 能同时给出 θ 和 θ̇，这正是能再加一层环的硬件前提。
如果传感器只给角度，就只能做两级串级；正因为能直接测到 θ̇，才做成了三级串级。

1.3 从单级到三级：串级 PID 的产生过程

• 第 1 步 · 单级 PID（p3）：ωref 与 ω 相减 → PID → 电机模型 → ω，反馈来自编码器。只能控转速。
• 第 2 步 · 两级串级（p4）：外环 PID 吃 θref 与 θ 的误差，输出 ωref 交给内环，形成「姿态环 + 转速环」。
• 第 3 步 · 三级串级（p5）：既然 θ̇ 可以直接测量，就在角度环与电机环之间插入角速度环。

课件 7.1 p4：两级串级 —— 内环控制轮胎转速 ω，外环控制车身姿态 θ。

课件 7.1 p5：三级串级 PID —— 最终方案。橙色框是角速度环，蓝色框是电机环。

最终三级方案的信号流：

θref →(Σ)→ PID_角度 → θ̇ref →(Σ)→ PID_角速度 → ωref →(Σ)→ PID_电机 → 电机模型 → ω → 车身模型 → θ̇ →(1/s)→ θ ↑ -θ ↑ -θ̇ ↑ -ω └──── 角度环 ──────────────┴──── 角速度环 ───────────┴──── 电机环 ────┘

1.4 整定总策略：从内向外逐级整定

串级结构的整定不能一次把三个环的参数一起调。7.1 p6 给出统一方法：

1. 通过波特图整定该级 PID 参数；
2. 计算该级的闭环响应函数，把它当作一个已知方块交给外环；
3. 换到下一级（更外的一环）重复。

顺序：电机环 → 角速度环 → 角度环。7.1 的 p7 / p8 / p9 正是这三步的示意，
每一张图里被绿色框高亮的部分，就是当前正在整定的那一级。

课件 7.1 p7：第 1 步 —— 整定电机环参数，并把它化简成「电机闭环」方块。

课件 7.1 p8：第 2 步 —— 用电机闭环整定角速度环。

为什么必须从内向外？外环的「被控对象」包含内环。只有内环的闭环行为先确定下来（成为已知传函），
外环才有确定的模型可以画波特图。反过来先调外环，内环一动外环的设计就全部作废。

二、电机闭环传递函数（7.2）

2.1 目标：把「PID + 电机」打包成一个方块

课件 7.2 p3：把整个闭环替换成一个「电机闭环传递函数」方块。

2.2 结构框图化简规则

课件 7.2 p4：串联、并联、反馈、分离点前后移、比较点前后移。

2.3 已知量

速度环 PID 参数已经由波特图整定好（p2），电机模型由辨识得到（p5）：

1 5.591 G_PID(s) = Kp + Ki·─ + Kd·s , Gm(s) = ────────────────────────────── s (s/14.2 + 1)(s/1962.2 + 1)

课件 7.2 p5：Kp = 0.5、Ki = 7、Kd = 0。

先把参数钉死（复算过）：Kp = 0.5、Ki = 7、Kd = 0。
于是 PID 零点在 Ki/Kp =14 rad/s，而电机模型恰好有一个 14.2 rad/s 的极点 ——
两者几乎重合，这就是整节的题眼。低频增益 Kp·Gm(0) = 0.5 × 5.591 = 2.7955。

2.4 化简步骤

(1) 把 PID 写成零极点形式： G_PID(s) = Kp + Ki/s = (0.5s + 7) / s = 0.5(s + 14) / s = 7(s/14 + 1) / s ← 零点在 14 rad/s (2) 串联相乘，得开环传递函数： G(s) = G_PID(s) · Gm(s) 7(s/14 + 1) 5.591 = ───────────────────────── · ────────────────────────────── s (s/14.2 + 1)(s/1962.2 + 1) 39.14 (s/14 + 1) = ─────────────────────────────────────── s (s/14.2 + 1)(s/1962.2 + 1)

课件 7.2 p7：G(s) = G_pid(s)·G_m(s)。

2.5 两条近似：为什么可以只留一个积分环节

• 零极点靠得很近（p8）：零点 14 与极点 14.2 几乎重合，对波特图几乎没有影响，可以相消；
• 很大的极点可以忽略（p8）：极点 1962.2 对应很高的频率，远在穿越频率之外，可以忽略。

课件 7.2 p8：零极点相消 + 忽略大极点，结果 39.4/s。

⚠️39.4 的正确含义：它是「穿越点附近的等效增益」，不是低频渐近线。
两个数要分清楚：

• 低频渐近线：Kp·Gm(0) = 0.5 × 5.591 =2.80/s，第一个转折约在 1 rad/s；
• 实际穿越频率：38.78 rad/s，相位裕度 89.1° —— 这才是课件 39.4 的出处。

如果把 39.4/s 当成低频渐近线去起手画幅频图，会得到错误的高度；
但用它估穿越频率、再写出闭环 1/(s/40+1)，结论是对的。
考试安全写法：「穿越频率 ≈ 39 rad/s，闭环主导极点 ≈ −40 rad/s」。

2.6 闭环：一阶惯性环节

G(s) 39.4/s 39.4 1 1 Φ(s) = ─────────────── = ─────────── = ─────────── = ──────────── ≈ ──────────── 1 + G(s)H(s) 1 + 39.4/s s + 39.4 s/39.4 + 1 s/40 + 1

课件 7.2 p9：电机环闭环 —— 等效一阶惯性环节 1/(s/40 + 1)。

结论（后面每一节都会用到）：整个「PID + 电机 + 编码器反馈」打包之后，
等效为一个时间常数约 1/40 s 的一阶惯性环节 Φ(s) = 1/(s/40 + 1)，带宽约 39 rad/s（≈ 6.2 Hz）。
从此刻起，电机环在框图里就是这一个方块，不再拆开。

2.7 闭环极点的精确校验

不做任何近似，直接解闭环特征方程 s(s/14.2+1)(s/1962.2+1) + (0.5s + 7)×5.591 = 0：

对消误差只有 0.107 rad/s（相对 14 约 0.8%），所以课件说「零极点靠得很近」是有依据的。

2.8 两条近似的适用条件

零极点相消（14 与 14.2）与忽略大极点（1962.2），前提都是穿越频率远低于这些频率。
本例穿越在 38.8 rad/s，只有 14.2 的 2.7 倍，所以严格说第一条近似是「刚好够用」的程度，
不是非常宽松；若把带宽再抬高就会失效。

三、倒立摆建模（7.3）

3.1 物理图景

课件 7.3 p2：输入是轴做直线运动的加速度 ẍ，输出是倒立摆的角加速度 θ̈。

3.2 符号与方向约定

3.3 受力分析

把摆球当作质点，它只受两个力：重力 mp·g和底盘加速带来的惯性力 mp·ẍ。
两个力都分解到「垂直于摆杆」和「沿摆杆」两个方向上，
其中垂直于摆杆的分量之差，就是让摆产生角加速度的切向合力。

课件 7.3 p3：切向合力 FT → 扭矩 T → 转动定律 T = Jp·θ̈ 的完整推导。

① 切向合力（垂直于摆杆的两个分量相减） FT = mp·g·sinθ − mp·ẍ·cosθ ② 力矩（力臂 = 摆长 lp） T = FT · lp ③ 摆的转动惯量（质点绕端点） Jp = mp·lp² ④ 转动定律（对照平动的 F = m·a） T = Jp · θ̈

3.4 合成倒立摆的数学模型

Jp·θ̈ = FT·lp ⇒ mp·lp²·θ̈ = (mp·g·sinθ − mp·ẍ·cosθ)·lp 消去 mp·lp 后得到： g·sinθ − ẍ·cosθ θ̈ = ──────────────────── lp 写成方块： ẍ ──▶【 倒立摆 】──▶ θ̈

课件 7.3 p4：非线性，不能写成传递函数的形式。

关键结论：式中含 sinθ 与 cosθ，是非线性的，因此不能直接写成传递函数。
这正是 7.5 要做「逆解算」的原因 —— 不是数学上变线性了，而是把非线性搬到了控制器内部。

3.5 符号勘误（很重要）

• 7.3 p3 现场推出的 (1) 式写成了FT = mp·ẍ·cosθ − mp·g·sinθ，与最终方块中的(g·sinθ − ẍ·cosθ)/lp符号相反。
• 7.7 p3 上把这一项的负号划掉、改成了绿色修订版：FT = mp·g·sinθ − mp·ẍ·cosθ；
  7.7 p2 的绿字也写成θ̈ = (g·sinθ − ẍ·cosθ)/lp。
• 以绿色修订版为准。物理校验：θ > 0（往 +x 侧倒）时，加大 ẍ（往 +x 加速）会让 θ̈ 变小，
  角被追回来，说明控制方向正确。

四、绘制系统结构框图（7.4）

把已经拿到的两块模型拼起来，并补上物理世界里剩下的两个运动学关系。

4.1 先看角速度环

课件 7.4 p5：上半张是展开形式，下半张是「电机闭环 + 倒立摆数学模型」的化简形式。

下半张图里出现两个新方块：绿色Rw·s与含 sinθ / cosθ 的非线性方块。

4.2 补上运动学环节

车轮的线位移与转角之间是纯几何关系，求导两次即可得到 ẍ 与 ω 的关系：

x = Rw·θ ← 几何关系（Rw 为轮半径） ↓ 求导 ẋ = Rw·θ̇ = Rw·ω ← θ̇ 就是轮胎转速 ω ↓ 求导 ẍ = Rw·dω/dt ↓ 拉普拉斯变换 ẍ(s) = Rw·s·ω(s) ẍ(s) ⇒ ───────── = Rw·s ← 图中绿色方块 ω(s)

课件 7.4 p6：由 x = Rw·θ 一路求导到 ẍ(s)/ω(s) = Rw·s。

4.3 补上积分环节

角加速度积分一次得到角速度，在框图里就是乘一个 1/s：

θ̇ = ∫ θ̈ dt ──拉普拉斯变换──▶ θ̇(s) = θ̈(s)·(1/s) ⇒ θ̇(s)/θ̈(s) = 1/s

课件 7.4 p8：框图里补上的第二个方块 1/s。

4.4 完整框图

课件 7.4 p10：上：三个环的层次；下：化简后可直接对照代码的形式。

五、倒立摆逆解算（7.5）

5.1 问题：非线性卡在中间

课件 7.5 p2：非线性求不出传递函数，想办法把它消掉。

正着看框图，前向通路是：ẍ → 【非线性 θ̈ = f(θ, ẍ)】 → θ̈。
中间这个方块含 sinθ、cosθ，没有传递函数，后面的 PID 也就无从整定。

5.2 关键洞察：θ 是已知的

θ 由 MPU6050实时测得。既然 θ 已知，它就不再是「未知变量」，而只是公式里的一个已知系数。
于是可以把整条式子反过来解出 ẍ：

g·sinθ − ẍ·cosθ θ̈ = ───────────────────── ⇒ lp·θ̈ = g·sinθ − ẍ·cosθ lp ⇒ ẍ·cosθ = g·sinθ − lp·θ̈ ⇒ ẍ = (g·sinθ − lp·θ̈) / cosθ ← 逆解算

课件 7.5 p5：两个式子并列 —— 上为正解算（分析用），下为逆解算（控制器用）。

5.3 结构上的意义：非线性被搬进了单片机

课件 7.5 p4：虚线左侧是「单片机里的程序」，右侧是「物理实在」。

反解之后，非线性模块从「物理世界」被搬到了控制器内部（单片机里）。
整条前向通路上只剩线性环节 —— 电机闭环 1/(s/40+1) 与运动学 Rw·s —— PID 的整定就可以照常进行。

课件 7.5 p6：逆解算方块移入单片机内部。

5.4 正问题与逆问题对照

5.5 逆解算里用哪个 θ̈

逆变里用的 θ̈ 并不是从物理世界测来的，而是从位置 / 角度环 PID 得到的期望值 θ̈ref。
实际系统中 θ 与 θ̇ 由 MPU6050 直接测量，它们构成所有反馈量。

5.6 车轮半径环节的位置（易错）

注意：框图里是 ẍ(s)/ω(s) = Rw·s，Rw 是物理世界的几何常数。
当这个环节被搬进单片机、用于由 ẍref 反算 ωref 时，要除以 Rw，而不是乘。
代码里写作1.0f / rw（见第七节）。

六、参数整定（7.6）

整定顺序依然是从内向外：电机环（7.2 已完成）→ 角速度环 → 角度环。

6.1 第 1 级：电机环（回顾 7.2）

6.2 第 2 级：角速度环

先把被控对象整理清楚：角速度环的内环被控对象 = 电机闭环 × 车身模型（含积分）。
合并积分环节后，开环对象是：

1 1 1 G_θ̇(s) = ─────────── · ─── = ──────────────────── s/40 + 1 s s(s/40 + 1)

课件 7.6 p3：把角速度环的被控对象合并为 1/[s(s/40+1)]。

用G_PID(s) = 10 + 10/s做波特图整定（p6）。图中黑线是原对象幅频、红线是 PID 的幅频、
绿线是补偿后的开环幅频。复算结果：开环穿越9.05 rad/s，相位裕度70.9°。

课件 7.6 p6：角速度环波特图整定 —— G_PID(s) = 10 + 10/s。

整定完成后求闭环响应函数（p9）。课件写成两个实数极点 14.8 与 20.4 的形式：

1 Φ_θ̇(s) = ────────────────────────────────────────────── (s/14.8 + 1)(s/20.4 + 1)

课件 7.6 p9：确定了角速度环 PID 参数（Kp = 10, Ki = 10），并算出闭环响应函数。

🚩这处极点值对不上，已复算。
按图面参数直接算：内环对象 1/(s/40+1)，PID = 10 + 10/s，
闭环分母恒为s² + 440s + 400，两个根是−439.09 与 −0.911。
课件写的 14.8 与 20.4（对应分母 s² + 35.2s + 301.92）与它不符：
极点之和差 12.5 倍、之积差 1.3 倍。

我也试过把 1/s 放回环内（按 7.6 p3 图面写法）：分母变成 s³/40 + s² + 10s + 10，
根为 −1.12、−14.80、−24.08 —— 其中 −14.80 与课件的 14.8 巧合，
但另一个是 24.08 而不是 20.4，而且该配置含右半平面极点、本身不稳定，所以不是课件的本意。

考试安全写法：按图面参数老老实实算 s² + 440s + 400，答「−439.1 与 −0.9」；
若题目沿用课件的 14.8/20.4，则照抄这两个数继续往下做，不要自行改动。

6.3 第 3 级：角度环

把角速度环整体当成一个方块，前面再串一个积分 1/s，得到角度环的开环对象（p10–p11）。
注意这里用的是课件给的极点 14.8 / 20.4：

1 G_θ(s) = ────────────────────────────────────────────────────── s (s/14.8 + 1)(s/20.4 + 1) 展开分母： s³ + 35.2 s² + 301.92 s

课件 7.6 p11：开环传函（积分 + 两个闭环极点）。

角度环这一级只用比例控制（p14）：G_PID(s) = 4，即

4·G_θ(s) Φ_θ(s) = ───────────────────────── 1 + 4·G_θ(s)

课件 7.6 p14：G_PID(s) = 4，纯比例。

用 Kp = 4 复算这级的裕度：取课件的 14.8 / 20.4 时，穿越频率2.92 rad/s、相位裕度70.7°；
若改用内环的精确极点（−439.1 / −0.911），则穿越1.50 rad/s、相位裕度31.0°。
两种取法都稳定，但裕度差别不小 —— 这也是上面那处极点存疑会连带影响的地方。

6.4 最终 PID 参数表

把三个环的 Kp 放在一起看：0.5 → 10 → 4。内环增益最小、角速度环最大，
这是「内环快、外环慢」的典型分布：角速度环的开环穿越（9.05 rad/s）明显高于角度环（2.9 rad/s）。

6.5 整定步骤总结

(1) 电机环 波特图定 Kp=0.5, Ki=7 → 求闭环 Φ = 1/(s/40+1)（主导极点 −40.8） (2) 角速度环 把 (1) 当已知方块 → 波特图定 Kp=10, Ki=10 → 闭环极点 −439.1 / −0.91 （课件写 14.8 / 20.4，与图面参数不符） (3) 角度环 把 (2) 当已知方块，串 1/s → 波特图定 Kp=4

七、编写代码（7.7）

7.1 最终框图（带参数标注）

课件 7.7 p2：单片机内部两个 PID（Kp=4；Kp=10, Ki=10）+ 逆解算 + 1/(Rw·s)。

7.2 从框图到代码的对应关系

7.3 输出限幅

课件 7.7 p5：θ̇ ∈ [−4π, +4π] rad/s，θ̈ ∈ [−40π, +40π] rad/s²。

限幅放在 PID 输出之后、进入积分之前的位置。它有两个作用：

• 不让期望角速度 / 角加速度大到物理上做不到，避免控制器提出无法执行的要求；
• 一旦传感器异常或状态发散，输出不会无界增长，保护电机与机械结构。

7.4 逆解算与积分的实现

框图上的 1/(Rw·s) 由一个积分环节（1/s）和一个常数 1/Rw 组成。
在代码里，积分就是「每个控制周期累加一次」：

课件 7.7 p6：ωref = (1/Rw)·∫ ẍref dt，代码即一行累加。

/* 轮半径换算 + 积分，一步完成 */omega_ref+=1.0f/rw*x_dot_dot_ref*deltaT;

1 ωref = ───────── · ∫ ẍref dt Rw

7.5 一帧控制流程（把上面的内容串起来）

每个控制周期 deltaT 执行一次： 1. 读取传感器 θ ← MPU6050（pitch） θ̇ ← MPU6050（gyro） ω ← 编码器 2. 角度环 PID θ̇ref = PID_angle(θref − θ) Kp = 4 3. 角速度环 PID θ̈ref = PID_rate(θ̇ref − θ̇) Kp = 10, Ki = 10 4. 输出限幅 θ̇ref, θ̈ref 限幅到 ±4π, ±40π 5. 逆解算 ẍref = (g·sinθ − lp·θ̈ref) / cosθ 6. 换算 + 积分 omega_ref += (1.0f / rw) * ẍref * deltaT ← 1/(Rw·s) 7. 电机调速 Motor_SetSpeed(omega_ref) ← 电机环闭环已在底层实现

7.6 代码文件组织

附录 A：公式速查表

附录 B：数值验算表

所有关键数字用 Python 复算，精确值与课件值对照。

附录 C：课件勘误汇总

附录 D：自测清单

1. 为什么要在角度环和电机环之间再加一层角速度环？（提示：MPU6050 能直接测 θ̇）
2. 写出从 G(s) = 39.14(s/14+1)/[s(s/14.2+1)(s/1962.2+1)] 化简到只剩积分环节的两条近似理由，
   并说明 39.4 是穿越点等效增益而不是低频渐近线。
3. 倒立摆模型为什么不能直接写成传递函数？逆解算是怎么绕过这个问题的？
4. 框图里 Rw·s 在物理世界一侧，代码里为什么变成1.0f / rw？
5. 三个环的 Kp / Ki 分别是多少？整定顺序为什么必须「从内向外」？
6. 输出限幅限的是哪两个量？限幅值是多少？不加限幅会怎样？
7. 如果发现摆往 +x 侧倒下时小车也跟着往 +x 走却越倒越快，最可能是哪个符号写错了？
8. 电机闭环的主极点是 −40.8；角速度环按图面参数算出的闭环极点是 −439.1 与 −0.91，而课件写 14.8/20.4。哪个对？
9. 为什么「内环必须比外环快」？用 9.05 rad/s（角速度环）与 2.92 rad/s（角度环）两个穿越频率说明。