2-41 一根倒立摆偏离竖直后，怎样用 LQR 把它重新稳定下来？

一句话介绍：序以小车—单级倒立摆为对象，用户可以设置小车质量、摆杆质量、摆杆长度、LQR 权重、初始摆角、初始位置以及仿真时间和步长，程序建立线性状态空间模型，通过 lqr 自动计算状态反馈增益 K，再利用 lsim 得到摆角、角速度、小车位置和速度响应，并同步进行三维动画演示；工程中还保留了 Riccati、LMI 与 Simulink 形式的 H∞ 控制示例。

先看它解决什么问题

倒立摆最直观的特点就是：

摆杆竖直向上时是一个天然不稳定平衡点。

只要产生一点角度偏差，如果小车不及时运动补偿，摆杆就会继续倒下。

因此程序真正要解决的不是“让一个摆杆动起来”，而是把下面几件事连成完整闭环：

根据小车和摆杆参数建立动力学模型；

把摆角、角速度、小车位置和速度作为系统状态；

用 LQR 根据状态误差和控制代价自动求反馈增益；

根据初始摆角、初始位置计算闭环自由响应；

观察摆角、小车位置以及两个速度量怎样回到平衡状态；

把数值仿真结果同步转换成小车—摆杆三维动画。

主 GUI 的实际处理链可以概括为：

模型参数 + LQR 权重 + 初始状态 → 状态空间模型 →lqr求 K → 闭环矩阵 A-BK →lsim仿真 → 四状态曲线 + 3D 动画

需要特别注意一点：

当前 GUI 默认外部输入 U=0，因此它主要演示的是“存在初始偏差时，状态反馈怎样把系统拉回平衡”，而不是持续外部扰动作用下的抗扰仿真。

1. 当前工程真正由哪些部分组成？

这套工程并不只有一个 .m 文件，而是同时包含 GUIDE GUI、状态反馈脚本、H∞ 设计脚本和一个 Simulink 示例。

此外还有 chap16_1.m，它使用 sdpvar、set 和 solvesdp 求解一个独立的 LMI 状态反馈问题，更接近通用 LMI 示例，并不处于 dlb GUI 的调用链中。

因此如果只是体验项目名称所描述的：“小车倒立摆控制 + GUI 动画演示”

2. GUI 默认建立的是怎样一个倒立摆？

dlb.fig 中已经保存了一组默认输入。

程序把重力加速度固定为：g = 9.8 m/s²

其中小车位置滑块的范围为：-2～2 m

摆杆角度滑块的范围为：-1～1 rad

但“滑块允许输入到 ±1 rad”并不等于线性模型在整个范围内都具有同样的物理精度，这一点后面还会专门说明。

3. 程序怎样把倒立摆变成状态空间模型？

主 GUI 采用四维状态描述倒立摆。

状态顺序是：x = [θ，θ̇，s，ṡ]ᵀ

其中：

θ：摆杆相对竖直位置的角度；

θ̇：摆杆角速度；

s：小车水平位置；

ṡ：小车水平速度。

源码首先计算摆杆绕质心的转动惯量：I = mL² / 12

并使用：l = L / 2

表示摆杆质心到转轴的距离。

为了让后面的状态方程更紧凑，可以把源码中的公共分母写成：

Δ = (M+m)I + Mml²

随后得到四个动力学系数：

t₁ = m(M+m)gl / Δ

t₂ = -m²gl² / Δ

t₃ = -ml / Δ

t₄ = (I+ml²) / Δ

当前 GUI 的线性状态方程本质上就是：

θ̇ = θ̇

θ̈ = t₁θ + t₃u

ṡ = ṡ

s̈ = t₂θ + t₄u

对应源码中的矩阵：

ẋ = Ax + Bu

这里最值得注意的是：

模型中没有出现 sin(θ)、cos(θ) 一类完整非线性项。

因此当前控制器设计针对的是倒立平衡点附近的线性化模型。

4. 为什么开环倒立摆自己不会稳定？

以 GUI 默认的：

M=1 kg，m=1 kg，L=1 m

代入当前源码，可以得到：

t₁ = 23.52、t₂ = -5.88、t₃ = -1.20、t₄ = 0.80

最关键的是摆角加速度中存在：

θ̈ = 23.52θ - 1.20u

当没有控制输入时，θ 前面的系数是正值。

这意味着摆杆一旦偏离竖直方向，系统自身并不会产生“把它拉回来”的恢复作用，反而会继续放大这种偏离。

所以倒立摆控制的核心任务就是：

根据当前摆角、角速度、小车位置和速度，实时计算一个合适的小车驱动力 u，把原本不稳定的平衡点变成稳定平衡点。

5. LQR 在这里到底做了什么？

GUI 没有要求用户直接填写四个反馈增益，而是让用户输入：

q₁、q₂、q₃、q₄、R

然后由 MATLAB：lqr(A,B,Q,R)自动计算 K。

其中：

Q = diag(q₁,q₂,q₃,q₄)

LQR 要平衡两类代价：

状态偏离不能太大，同时控制动作也不能无限大。

它对应的性能指标可以简化表示为：

J = ∫₀∞ (xᵀQx + uᵀRu) dt

得到 K 后，控制规律为：u = -Kx

于是闭环系统变成：ẋ = (A-BK)x

这也是 GUI 在启动仿真时真正使用的：At=A-B*K

所以“计算 K 值”并不是一个单纯的显示功能，它实际上决定了后面整套闭环仿真的系统矩阵。

6. 默认参数下 K 大约是多少？

按 GUI 自带默认值：M=1，m=1，L=1

以及：Q = diag(5,5,5,5)，R=5

对当前源码中的连续 LQR 问题进行等价复算，可以得到：

K ≈ [-49.5669，-10.8858，-1.0000，-2.6675]

对应状态顺序仍然是：

[摆角，角速度，小车位置，小车速度]

将这组 K 代入 A-BK 后，闭环特征值约为：

-5.5138、-4.2965、-0.5593 + 0.4285i、-0.5593 - 0.4285i

它们的实部全部小于 0。

这说明在当前线性模型和默认权重下：

LQR 已经把原本不稳定的倒立平衡点变成了一个渐近稳定的闭环系统。

这里的 K 是按当前源码和默认 GUI 参数复算得到的确定值，并不是程序包中预先保存的一组实验结果。

7. 初始摆角和初始位置怎样进入仿真？

GUI 并没有开放四个状态的全部初值，而是只让用户直接设置：

初始摆角；

初始小车位置。

仿真开始时，源码构造：

x₀ = [初始摆角，0，初始小车位置，0]ᵀ

也就是说：

初始角速度固定为 0，初始小车速度也固定为 0。

然后程序生成：t = 0:Step:Tmax

并令：U = 0

最后调用 lsim 计算闭环响应。

因此当前 GUI 本质上做的是：

给系统一个初始角度/位置偏差 → 不再施加额外外部输入 → 看 LQR 怎样靠状态反馈把四个状态重新压回平衡点附近

这也解释了一个很容易忽略的现象：

如果保持 GUI 默认的“初始摆角=0、初始位置=0”，同时外部输入又是 0，那么理论上整个状态会一直保持在 0。

所以如果想真正看到“倒立摆被控制回来”的过程，需要先把初始摆角或小车位置设成非零值，再启动仿真。

8. GUI 最终观察的是哪四条响应？

lsim 的结果保存在 odraw 中。

源码依次提取：

odraw(:,1)：摆杆角度；

odraw(:,2)：摆杆角速度；

odraw(:,3)：小车位置；

odraw(:,4)：小车速度。

GUI 中对应四个曲线区域：Angle、Angle_Velocity、Displace、Velocity

因此判断当前控制是否有效，不能只看“摆杆最后有没有竖起来”。

还可以同时观察：

摆角是否衰减；

是否存在明显振荡；

小车为了扶正摆杆移动了多远；

小车和摆杆的速度峰值有多大；

Q、R 改变后这些动态指标怎样变化。

主 GUI 没有单独画出控制输入 u。

如果需要查看控制量，工程中另一条 Simulink/H∞ 示例链会把 ut 保存到工作区，并由 chap16_3plot.m 绘制控制输入曲线。

9. 三维动画是怎样跟数值仿真联动的？

dlb.m 打开后，会先用 patch 建立一个蓝色小车和红色摆杆，再用 surf 建立摆杆顶部的球形结构。

仿真结束得到全部状态序列后，程序逐点读取：

小车位置 s → 平移小车和摆杆底座

摆杆角度 θ → 更新摆杆几何顶点

于是数值状态：

θ(t)、s(t)

被转换成连续的三维动画。

这部分有两个值得注意的实现特点。

动画尺寸是示意比例，不是动力学参数 L 的真实缩放

三维绘图内部使用固定几何尺寸，例如：

reall=15，ll=19

而不是读取 GUI 中用户输入的摆杆长度 L。

因此：

L 会改变动力学模型和 LQR 增益，但不会等比例改变动画里画出来的摆杆长度。

当前三维视图更准确地说是“状态动画示意”，而不是按实际米制尺寸建立的几何数字孪生模型。

动画播放时间不等于物理实时

循环内部固定使用：pause(0.02)

因此动画播放快慢主要由采样点数量和这个暂停时间决定，并不是按照仿真步长一比一实时播放。

界面上的 T = ... s 显示的是仿真时间进度。

10. Q 和 R 为什么是这套程序最值得实验的参数？

LQR 的有趣之处就在于：

用户不直接“猜 K”，而是通过 Q 和 R 告诉控制器什么更重要。

当前 Q 的四个对角元素分别对应：

摆角 → 角速度 → 小车位置 → 小车速度

如果提高某一个 q，LQR 通常会更积极地抑制对应状态的偏离。

例如：

增大摆角权重：更强调尽快把摆杆扶正；

增大小车位置权重：更不希望小车偏离原点太远；

增大速度权重：更重视动态过程中的速度约束趋势。

R 则表示：

控制输入本身有多“贵”。

R 较大时，控制器通常会更加克制；

R 较小时，控制器更愿意用较强控制动作换取更快的状态收敛。

因此这个 GUI 很适合做一类非常直观的控制实验：

保持倒立摆参数和初值不变 → 修改 Q/R → 重新计算 K → 对比四条响应

这样能够直接观察“性能”和“控制代价”之间的权衡。

11. 工程里的 H∞、Riccati 和 LMI 文件是什么关系？

除了 LQR GUI，压缩包还带有另一套倒立摆控制示例。

它使用的模型参数不是 GUI 默认值，而是：

M=1.0 kg，m=0.1 kg，L=0.5 m

因此不能把两套程序里的 K 直接混用。

chap16_2riccati.m 使用连续代数 Riccati 方程求 H∞ 控制器，并最终计算：

K = -B₂ᵀX

chap16_2LMI.m 则把 H∞ 条件写成矩阵不等式，通过 sdpvar 和 solvesdp 求解，再得到：

K = P₂P₁⁻¹

chap16_3ctrl.m 中已经写入两组控制增益：

Riccati：[29.0040 1.0395 5.3031 2.2403]

LMI：[36.3149 1.8765 6.3851 3.6704]

当前源码内部：

M=1

因此默认实际选择的是 Riccati 那一组；只有把控制器文件内部的 M 改成 2，才会切换到 LMI 增益。

这里还不能简单根据两组 K 的大小判断哪一种设计“更好”。

因为两个设计脚本的扰动输入矩阵并不相同：

Riccati 脚本使用的 B₁ 第三、第四项为 0.1；

LMI 脚本中对应项为 1。

也就是说，两套设计条件本身就存在差异。

因此这部分更适合作为：

两种 H∞ 控制器构造方式的代码示例

而不是已经控制所有条件、可以直接做公平性能排名的算法对比实验。

12. Simulink 示例实际怎样工作？

chap16_3sim.mdl 把：控制器 S-Function → 倒立摆 Plant S-Function → 状态反馈连接成闭环。

其中：

chap16_3ctrl.m：输入 4 个状态，输出控制量；

chap16_3plant.m：接收控制量并计算 4 个连续状态；

To Workspace：分别保存 t、x 和 ut；

chap16_3plot.m：绘制状态响应和控制输入。

当前 .mdl 文件保存的主要仿真设置为：

Stop Time：30 s；

Solver：ode113；

Simulation Mode：normal。

chap16_3plant.m 虽然预留了扰动通道 B₁，但源码实际写的是：

w = 0 × sin(t)

也就是默认扰动仍然为 0。

因此这条 Simulink 链同样主要展示闭环初始响应。

工程中还存在：

chap16_3sim.mdl.r14 (sp3)

但这个旧备份内部仍引用 chap13_9ctrl 和 chap13_9plant，而压缩包中并没有这两个文件。

所以当前工程应优先使用：

chap16_3sim.mdl

而不要把 .mdl.r14 (sp3) 备份作为正式运行入口。

13. 当前源码有哪些必须知道的边界？

这套程序很适合控制理论教学和倒立摆状态反馈实验，但源码里有几个边界不能忽略。

1. 主 GUI 是线性模型，不是完整非线性倒立摆

动力学方程没有完整的三角非线性项。

因此它最适合研究：

竖直平衡点附近的小角度稳定控制。

GUI 虽然允许摆角滑块到 ±1 rad，但角度越大，线性化模型与真实倒立摆之间的差异通常越不能忽略。

2. GUI 默认没有持续扰动

主 GUI 的：U=zeros(size(t))

意味着它测试的是初值响应。

因此不能仅根据当前默认仿真就声称已经验证了随机扰动、持续外力或复杂噪声下的鲁棒性。

3. “计算 K”必须先于“启动仿真”

A、B 和 K 都是在“计算K值”按钮回调中建立的全局变量。

因此直接打开 GUI 后，应先计算 K，再启动仿真。

4. “重置”按钮实际上会清空参数

当前 reset_Callback 会把：

M、m、L；q₁～q₄；R；K₁～K₄

全部清为空字符串。

它并不是重新写回 .fig 中的默认值。

因此点过“重置”后，需要重新输入参数再计算 K。

5. 三维越界提示没有真正终止后续动画

程序检查小车位置是否超过 ±2 m，并弹出：

“3D动画超出范围”

但用于记录越界的 tishi 后续没有参与停止判断。

也就是说，这个提示主要是警告，当前源码并没有据此完整中断后面的绘图与动画流程。

6. 工程使用的是较早期 MATLAB GUI/图形写法

主界面采用 GUIDE 的 .fig + .m 结构，并使用 gui_mainfcn、EraseMode 等旧式图形接口。

所以它更适合在兼容这类 GUIDE/HG1 代码的 MATLAB 环境中直接使用；在较新的 MATLAB 图形系统中，旧属性可能需要做兼容性调整。

14. 怎么运行？

如果主要体验小车倒立摆 LQR GUI，先把以下文件放在同一 MATLAB 工作目录或加入 MATLAB 路径：

dlb.m、dlb.fig、model.jpg

然后在命令行运行：dlb

打开界面后，推荐按照实际程序依赖顺序操作：

确认 M/m/L → 设置 q₁～q₄ 和 R → 设置初始摆角/小车位置 → 点击“计算K值” → 设置仿真时间和步长 → 点击“启动仿真”

如果希望看到明显的稳定过程，应把初始摆角或初始位置设成非零值；如果两者都保持 0，在当前无扰动模型下响应也会保持在平衡点。

15. 哪些参数最值得修改？

当前程序里真正值得主动实验的不是绘图尺寸或动画参数，而是下面这些会直接改变闭环动态的输入。

其中最适合做控制规律实验的是：Q、R 与初始摆角。

而 M、m、L 一旦改变，就已经不是单纯“调控制器”，而是在改变被控对象本身，因此应重新点击“计算K值”。

16. 一句话看懂这个项目

这是一个小车倒立摆控制与动画仿真程序：主 GUI 根据用户输入的小车质量 M、摆杆质量 m、摆长 L 建立四状态线性倒立摆模型，以摆角、角速度、小车位置和速度组成状态向量，再由 Q=diag(q₁,q₂,q₃,q₄) 和 R 通过lqr自动求状态反馈增益 K，利用 A-BK 和lsim计算无外部扰动条件下的闭环初值响应，最终同步输出摆角、角速度、位置、速度曲线和三维小车倒立摆动画；GUI 默认 M=m=L=1、q₁～q₄=5、R=5，按当前模型等价复算 K≈[-49.5669，-10.8858，-1.0000，-2.6675]，闭环极点实部均为负。工程还附带 Riccati、LMI 和 Simulink 形式的 H∞ 控制示例，但它们使用不同模型参数和扰动设置，不能与 GUI 的 LQR 增益直接混用；同时当前主模型属于竖直平衡点附近的线性化教学仿真，更适合控制算法学习、参数对比和 GUI 动画演示，而不能直接等同于大角度非线性倒立摆或工业级实物控制系统。