MATLAB/Simscape仿真二级倒立摆：极点配置与LQR控制算法对比实践

简介：本资源面向自动化、控制工程及相关专业高年级本科生与研究生，提供二级倒立摆这一典型非线性、强耦合、欠驱动系统的完整控制设计与物理仿真解决方案。内容涵盖系统动力学建模、极点配置法与LQR最优控制两种主流状态反馈策略的MATLAB实现，以及降维状态观测器的设计与验证，并通过Simscape Multibody完成高保真物理仿真，兼顾理论严谨性与工程可实现性。压缩包共42个文件（35张结果图用于响应曲线、控制器结构、观测误差等关键分析；3个核心m脚本实现K矩阵计算与控制器搭建；1份PDF报告、1个LaTeX源文件（含bib参考文献）及1个slx仿真模型），总大小2.93MB，目录组织清晰，图文与代码严格对应。已有3330人学习下载，读者可直接复现全部控制效果、理解状态反馈与观测器协同机制，并基于LaTeX源码快速撰写课程报告或实验论文。

1. 项目概述：从“杂技杆”到控制理论的实践场

如果你对自动控制理论感兴趣，或者正在学习《现代控制理论》这门课，那么“二级倒立摆”绝对是一个绕不开的经典研究对象。它就像一个简化版的、在轨道上移动的杂技杆——一个摆杆的末端再连接另一个摆杆，整个系统需要被稳定地倒立起来。这个看似简单的物理模型，却蕴含着丰富的非线性、强耦合和不稳定性，是检验各种控制算法鲁棒性和有效性的“试金石”。这次我们要做的，就是使用MATLAB及其强大的物理建模工具Simscape，来搭建这个二级倒立摆的仿真模型，并分别用经典的极点配置法和最优控制的LQR（线性二次型调节器）方法，让它从摇摇欲坠的状态变得稳如磐石。

为什么选择MATLAB/Simscape？因为对于控制算法的前期验证和理论学习来说，纯数学仿真（比如用微分方程在脚本里跑）虽然快，但总感觉和真实的物理世界隔了一层。Simscape则不同，它允许你像搭积木一样，用物理元件（质量块、转动关节、阻尼器）来构建模型，控制算法作用在模型上产生的力或力矩，驱动整个物理系统运动。这种“物理正确”的仿真，能让你更直观地看到控制效果，排查出那些在简化数学模型里被忽略的问题，比如关节摩擦、传感器噪声、执行器饱和等。对于极点配置和LQR这两种基于状态空间模型的设计方法，在Simscape环境中实现，能让你从理论公式无缝过渡到“看得见、摸得着”的虚拟实物控制，理解会深刻得多。

2. 系统建模：为倒立摆绘制“数字蓝图”

在动手写控制律之前，我们必须先搞清楚控制对象——二级倒立摆——到底是怎么运动的。这一步是后续所有设计的基础，模型不准，控制再好也是南辕北辙。

2.1 物理模型与状态空间方程推导

一个典型的二级倒立摆系统通常由一个小车、第一级摆杆和第二级摆杆组成。小车在水平轨道上受外力驱动，两个摆杆通过无摩擦的转动铰链连接。我们的控制目标是通过控制施加在小车上的水平力，使两个摆杆同时稳定在竖直向上的倒立位置。

为了应用线性控制理论（极点配置和LQR都要求系统是线性的），我们必须在系统的平衡点（即两个摆杆都竖直向上，角度和角速度均为零）附近进行线性化。这是最关键的一步。通过拉格朗日方程或牛顿-欧拉法，可以推导出系统的非线性动力学方程。这个过程涉及大量的偏导数和代数运算，是体力活也是技术活。这里我分享一个心得：一定要把推导过程中的中间变量和参数（如杆长、质量、转动惯量）用清晰的符号标记好，最好整理成一张参数表。因为后续在MATLAB和Simscape中建模时，这些参数会反复用到，一旦某个参数符号搞混，调试起来会非常痛苦。

假设我们完成了线性化，最终会得到一个标准的状态空间方程形式：dx/dt = A*x + B*uy = C*x + D*u其中，状态向量x通常选取为[小车位置， 小车速度， 摆杆1角度， 摆杆1角速度， 摆杆2角度， 摆杆2角速度]。u是小车所受的水平控制力，y是输出（通常我们关心所有状态，所以C可以是单位阵）。矩阵A和B的每一个元素都是系统物理参数的函数。得到A和B后，第一件要做的事就是检查系统的能控性。在MATLAB里用ctrb(A, B)计算能控性矩阵，并判断其秩是否等于系统状态维数。对于二级倒立摆，只要参数合理，它通常是完全能控的，这为我们配置极点提供了可能。

2.2 Simscape多体模型搭建要点

理论模型有了，接下来在Simscape Multibody中把它“造”出来。打开Simulink，从Simscape > Multibody库中拖出需要的模块：World Frame（世界坐标系）、Rigid Transform（刚体变换，用于定义位置和姿态）、Revolute Joint（转动副，模拟摆杆铰链）、Prismatic Joint（移动副，模拟小车轨道）、Body（质量块，赋予质量和惯性）以及Rigid Transform。

搭建顺序建议从“根”到“叶”：

1. 建立世界框架：这是所有物体的参考系。
2. 搭建小车：用一个Prismatic Joint连接世界框架和小车Body。这个移动副定义了小车唯一的运动自由度（水平移动）。关键点：在移动副的属性里，要正确设置其初始位置和初始速度（通常我们希望仿真开始时小车在轨道中央静止）。执行器（即我们的控制力u）就作用在这个移动副的Actuation端口上，选择Force作为驱动方式。
3. 搭建第一级摆杆：用第一个Revolute Joint连接小车Body和第一级摆杆Body。这个转动副的轴线方向要设置为垂直于轨道平面（通常是Z轴），以允许摆杆在竖直平面内摆动。关键点：这个转动副的初始角度设置至关重要，它决定了仿真的起始状态。如果我们想测试控制器从非平衡点启动的能力，可以在这里设置一个小的初始角度（如5度）；如果只想看稳定效果，就设为0。同样，其Actuation端口通常设置为None，因为我们不直接对摆杆施加扭矩，控制全靠小车底部的力来传递。
4. 搭建第二级摆杆：用第二个Revolute Joint连接第一级摆杆的末端和第二级摆杆Body。设置同理。
5. 添加传感器：为了得到状态反馈，我们需要测量所有状态。小车位置和速度可以从移动副的Sensing端口直接读取。两个摆杆的角度和角速度可以从对应的转动副的Sensing端口读取。将这些信号引出，组合成我们的状态向量x。
6. 配置物理参数：在每个Body模块中，准确填写质量、惯性张量、几何尺寸（用于可视化）。惯性张量的计算需要小心，对于均匀细杆，绕其一端旋转的惯性矩是(1/3)*m*L^2。这些参数值必须和之前推导状态空间方程时使用的参数完全一致。

注意：Simscape仿真对初始条件非常敏感。如果所有关节的初始位置和速度设置不当，仿真一开始就可能因为巨大的初始加速度而报错。一个稳妥的做法是，先让所有初始位置和速度都为0，在无控制（u=0）的情况下跑一个很短的时间（如0.1秒），看看系统是否会因为数值误差而“崩溃”。通常，由于完美的竖直倒立是不稳定平衡点，即使有微小的数值误差，摆杆也会开始倒下。这正好符合我们的预期。

3. 控制器设计：两种经典思路的较量

模型准备就绪，状态x也能测量了，现在就是设计大脑——控制器的时候了。我们将对比两种完全状态反馈的设计方法：极点配置法和LQR。

3.1 极点配置法：指定系统的“性格”

极点配置法的核心思想很直观：对于一个能控的线性系统，我们可以通过设计状态反馈矩阵K，使得闭环系统(A-BK)的特征值（即极点）落在我们指定的、复平面左半部分的期望位置上。极点的位置直接决定了系统的动态响应性能：离虚轴越远，响应越快；阻尼比越大，超调越小。

设计步骤：

1. 确定期望极点：这是最体现经验的一步。对于二级倒立摆这样的6阶系统，我们需要指定6个期望极点。通常，我们会为一对主导极点选择一个合适的阻尼比（如0.7）和自然频率，以保证主要的摆动模态有较好的动态性能。其余4个极点则被配置到主导极点左方更远的位置（实部更负），使其对应的模态快速衰减，不影响主导动态。例如，你可以选择：p = [-2+2i, -2-2i, -10, -12, -15, -20]。前两个是主导复极点，后面都是实极点。
2. 计算反馈矩阵K：在MATLAB中，一旦有了A, B矩阵和期望极点向量p，一行代码就能搞定：K = place(A, B, p)。place函数相比古老的acker函数，数值稳定性更好，尤其适用于多输入系统（虽然我们这里是单输入）。
3. 形成控制律：控制输入u = -K * x。这就是我们的状态反馈控制器。

实操心得与坑点：

• 极点不是越“快”越好：把极点拼命往左配，确实能让理论响应飞快。但这会带来一个严重问题：控制量u巨大。在Simscape仿真中，这表现为需要施加成百上千牛顿的力，这在实际的电机和驱动器上是无法实现的（执行器饱和）。仿真可能会因此发散或得到不切实际的结果。所以，选择极点时必须考虑控制量的幅值。一个技巧是，先在脚本里用initial或step函数对线性模型进行初步仿真，观察控制力u的时域曲线，确保其最大值在一个合理的范围内（比如±50N）。
• 对模型误差敏感：极点配置法完全依赖于准确的A和B矩阵。如果Simscape模型中的物理参数（如杆长、质量）和你在脚本中计算A、B时用的参数有细微差别，或者存在模型中未考虑的摩擦，实际闭环性能可能会严重偏离预期，甚至失稳。它缺乏内在的鲁棒性机制。

3.2 LQR方法：在性能与控制代价间寻找最优解

LQR（线性二次型调节器）则采用了一种不同的哲学。它不再直接指定极点，而是通过定义一个二次型性能指标J，来寻求一个最优的反馈矩阵K，使得这个指标最小化。 性能指标通常为：J = ∫(x'Qx + u'Ru) dt，积分从0到无穷。 其中：

• Q是状态加权矩阵，半正定。它惩罚状态偏离平衡点的程度。Q越大，说明我们越希望状态快速归零。
• R是控制加权矩阵，正定。它惩罚控制能量的大小。R越大，说明我们越“舍不得”用力。

设计步骤：

1. 构造加权矩阵Q和R：这是LQR设计的艺术所在。Q和R通常是对角阵。一个常用的起点是Bryson规则：将每个状态的最大允许偏差的平方的倒数作为Q的对角元素，将控制量最大允许值的平方的倒数作为R。例如，如果希望小车位置偏差不超过0.1米，摆杆角度偏差不超过0.1弧度，控制力不超过20N，那么可以设Q = diag([1/0.1^2, 1, 1/0.1^2, 1, 1/0.1^2, 1])（速度项的权重通常先设为1），R = 1/20^2。注意：速度状态通常没有明确的“最大允许偏差”，其初始权重可以设得小一些，或者通过试错调整。
2. 求解Riccati方程得到K：在MATLAB中，这同样是一行代码：[K, S, e] = lqr(A, B, Q, R)。这个函数会返回最优反馈增益矩阵K，以及Riccati方程的解S和闭环系统的极点e。
3. 形成控制律：同样，u = -K * x。

LQR的优势与调参经验：

• 自动平衡性能与能耗：LQR通过调整Q和R，自动在“快速稳定”和“省力”之间寻找最佳折衷。增大Q（相对于R），控制器会更“激进”，响应更快，但控制力更大；增大R，控制器会更“温和”，控制力小，但稳定速度可能变慢。
• 内在的鲁棒性：LQR设计出的控制器，对于系统模型在一定范围内的不确定性，具有较好的鲁棒稳定性（至少保证有60度的相位裕度），这比极点配置法更“皮实”。
• 调参有迹可循：我的个人经验是，先固定R为一个较小的值（比如1），然后主要调整Q。通常优先调整位置和角度状态的权重，因为它们直接关系到稳态误差。可以先让所有状态权重相等（如全1），观察仿真效果。如果摆杆收敛慢，就增大摆杆角度对应的Q元素；如果小车位移过大，就增大小车位置对应的Q元素。每次调整后，观察闭环极点的变化（lqr函数返回的e），它们应该都稳定且分布合理。

4. Simulink仿真集成与对比分析

设计好了两个K矩阵，接下来就是在Simulink环境中把它们和Simscape模型连接起来，进行闭环仿真。

4.1 仿真模型搭建与参数传递

1. 封装控制器：在Simulink中创建一个子系统（Subsystem），输入是6维状态向量x，输出是控制力u。在子系统内部，用Gain模块实现u = -K*x。你需要为极点配置法和LQR法分别创建两个这样的控制器子系统。
2. 连接闭环：将Simscape模型输出的状态信号，连接到控制器子系统的输入；将控制器输出的力信号，连接到Simscape模型中移动副的Actuation端口。这样就形成了一个完整的闭环。
3. 参数初始化：在MATLAB工作区中，定义好所有的物理参数（m, l, g等），并计算A、B矩阵。然后分别用place和lqr函数计算K_place和K_lqr。在Simulink模型中，将这些变量（包括A, B, K_place, K_lqr）通过Model Workspace或Mask的方式传递给对应的Gain模块。确保Simscape Body模块里的物理参数和脚本里计算A、B用的参数完全一致。
4. 设置仿真环境：在Simscape模型的配置参数中，选择合适的求解器。对于这种混合了物理模型和信号控制的系统，ode23t或ode15s这类变步长刚性求解器通常是不错的选择。仿真时间可以设为10-20秒，足够观察稳定过程。

4.2 性能对比与结果分析

分别使用两种控制器进行仿真，并从以下几个维度对比它们的性能：

1. 稳定性：这是最基本的要求。观察两个摆杆的角度和小车位置是否能最终收敛到零附近。通常，在无干扰情况下，两者都应该能做到。
2. 动态响应：给系统一个初始扰动（比如在Simscape中设置摆杆1的初始角度为5度）。对比两种控制器下，摆杆角度收敛到零所需的时间、超调量的大小。绘制状态曲线进行对比。

   % 示例：绘制摆杆1角度对比图 figure; plot(out_place.time, out_place.theta1, ‘b-‘, ‘LineWidth‘, 1.5); hold on; plot(out_lqr.time, out_lqr.theta1, ‘r--‘, ‘LineWidth‘, 1.5); xlabel(‘Time (s)‘); ylabel(‘Theta1 (rad)‘); legend(‘Pole Placement‘, ‘LQR‘); grid on; title(‘Comparison of First Pendulum Angle Response‘);

3. 控制输入：这是关键对比项。绘制控制力u随时间变化的曲线。你会发现，极点配置法（尤其是极点配置得很激进时）的控制力峰值往往远大于LQR方法。LQR通过优化指标，天然地抑制了过大的控制能量。
4. 鲁棒性测试（进阶）：人为地在Simscape模型中引入一些“不匹配”。例如，将某个摆杆的质量或长度参数，修改得与控制器设计时使用的理论值有10%的误差。再次运行仿真。你很可能发现，极点配置法的控制器可能开始出现振荡甚至失稳，而LQR控制器虽然性能有所下降，但依然能保持稳定。这直观地展示了LQR在模型不确定性下的鲁棒性优势。

结果分析表格示例：

从表格和仿真曲线可以清晰看出：极点配置法让你拥有对闭环动态性能（极点位置）的完全掌控权，但需要丰富的经验来平衡性能与可行性，且鲁棒性是其短板。LQR法则通过优化一个明确的工程指标（状态误差和控制代价）来间接确定极点，其设计过程更系统，且通常能获得更好的鲁棒性和更合理的控制量。

5. 常见问题与调试技巧实录

在实际操作中，你几乎一定会遇到下面这些问题。这里是我踩过坑后总结的排查思路。

5.1 仿真报错或发散

• 问题：仿真一开始就报错，提示“导数不连续”、“代数环”或“NaN”。
• 排查：
  1. 检查初始条件：这是最常见的原因。确保Simscape中所有关节（移动副和转动副）的初始位置和速度设置是自洽且物理上合理的。例如，两个摆杆的初始角度都设为0（竖直向上），但初始角速度是否也为0？
  2. 检查控制器输出：在仿真开始的第一个瞬间，控制器是否输出了一个无穷大或巨大的力？可以在控制器输出后加一个Saturation模块，将力限制在一个物理可行的范围内（如±100N），防止初始冲击。
  3. 检查物理参数：质量、惯性矩是否为正值？杆长是否为零或负值？
  4. 简化模型：先去掉控制器，让系统在无控状态下自由运动（u=0），看模型本身是否能正常仿真。如果无控都报错，问题肯定出在Simscape模型搭建上。

5.2 控制器无法稳定系统

• 问题：加上控制器后，系统依然发散，摆杆倒下。
• 排查：
  1. 确认反馈极性：这是最经典的错误！检查你的控制律u = -K*x。负号是否加上了？状态反馈必须是负反馈。你可以先用一个非常小的增益（比如K = [0.1, 0, 0, 0, 0, 0]，只反馈小车位置）测试，如果小车位置为正时，产生的力是负的（试图把它拉回零点），说明极性基本正确。
  2. 检查状态向量顺序：Simscape输出的状态信号顺序，是否与你计算K矩阵时假设的x = [pos; vel; theta1; omega1; theta2; omega2]顺序完全一致？顺序错一个，反馈就全乱了。用Display或Scope模块仔细核对每个信号。
  3. 检查A、B矩阵：将你推导或计算出的A、B矩阵，代入线性模型ss(A,B,eye(6),0)，用step或initial命令看看开环响应。一个不稳定的开环系统（极点有正实部）是正常的（倒立摆本身就不稳定）。再用place或lqr计算K，并用eig(A-B*K)验证闭环极点是否确实被配置到了左半平面。先在纯线性环境下验证你的控制律是有效的，再接入非线性Simscape模型。
  4. 检查参数一致性：再次确认计算A、B用的参数与Simscape模型中Body模块里填写的参数一字不差。一个0.01的误差都可能导致性能严重下降。

5.3 稳态存在微小误差或振荡

• 问题：系统大体稳定了，但小车位置或摆杆角度最终不归零，或者在零点附近持续小幅振荡。
• 排查：
  1. 检查是否有积分环节：我们目前设计的是纯状态反馈，属于比例控制（P控制）。对于存在常值干扰（如建模误差、轻微的不平衡）的系统，P控制无法完全消除稳态误差。解决方案是引入积分器，设计状态反馈+积分的控制器，或者使用LQI（带积分项的LQR）。
  2. 检查传感器噪声与量化：如果你在状态反馈回路中加入了模拟的噪声或量化模块，可能会引入持续的小幅振荡。可以尝试暂时移除噪声，看振荡是否消失。
  3. 调整控制器参数：对于极点配置，尝试将主导极点的实部再向左移动一点（加快衰减），或增加阻尼比。对于LQR，尝试进一步增大状态误差的权重Q（特别是对应小车位置和摆杆角度的元素）。

5.4 性能不达预期

• 问题：调节时间太长，或者超调太大。
• 解决：
  • 对于极点配置法：重新选择一组更“快”的期望极点。但切记要同步观察控制力u是否超出饱和限幅。这是一个权衡过程。
  • 对于LQR法：系统地调整Q和R。记住一个原则：要惩罚哪个状态，就增大Q中对应那个状态的元素；要限制控制量，就增大R。可以采用“单变量调整法”，每次只调整一个权重，观察仿真效果的变化，理解其影响。

最后，分享一个调试的黄金法则：分层验证，由简入繁。不要一下子就在完整的非线性Simscape模型上调试复杂的控制器。先从线性模型开始，验证你的状态空间方程、能控性、以及计算出的K矩阵是否能稳定线性系统。然后再接入Simscape模型，但可以先从更简单的一级倒立摆开始，成功后再扩展到二级。每一步都确保走稳了，再迈下一步，这样能最有效地定位问题所在。

本文还有配套的精品资源，点击获取