基于Simscape Multibody的二阶倒立摆建模与LQR控制实战

你刚接触Simulink，就想挑战二阶倒立摆，这个选择有点狠，但路子对了。网上关于倒立摆的资料九成都在讲一级，二阶的要么是一堆看不懂的拉格朗日方程推导，要么就是直接甩给你一个大几百行的S函数源码。我看过太多人卡在第一步：模型还没搭，先被数学吓退了。用Simscape Multibody就完全不用受这个罪，它允许你像搭积木一样把物理结构拼出来，车是车、杆是杆、铰链是铰链，仿真模型和真实机构几乎一一对应。这篇文章就打算带你把整个流程走一遍，从机械结构搭建到参数配置再到LQR控制，用的是小白友好型路线，所有该填的参数我都会给你一份可以直接抄作业的版本。

如果你是做课题、搞毕设，或者单纯想用倒立摆验证一下控制算法，这篇都适合你。就算你没碰过Simscape也没关系，跟着步骤来就行。我先把整体思路理顺，再逐块拆解，最后附上我调试时踩过的坑，尽量让你少走弯路。

1. 为什么说用Simscape搭二阶倒立摆是小白最友好的路线

1.1 传统建模方式的门槛有多高

先说说以前主流的做法，你就明白为什么我要推荐Simscape。经典倒立摆建模，你得先画受力分析图，然后针对小车、一级摆杆、二级摆杆各自列写牛顿方程，或者用拉格朗日方程去推系统的动能和势能。这个过程对数学功底要求非常高，而且二阶倒立摆不是简单地把一级倒立摆模型叠加，摆杆之间的耦合项、非线性项多到你怀疑人生。

等你推导完方程，还得在Simulink里把这些方程一个一个搭成积分模块、乘法模块、加法模块的框图，那种模型密密麻麻，连线连到眼瞎，出错了根本不知道是方程写错了还是框图搭错了。正因如此，我见过不少人花了两个月在建模上，真正的控制算法反而没时间做。这个误区非常可惜，因为倒立摆的核心价值本来就是控制器设计和验证，而不是折磨人的数学推导。

1.2 Simscape的解题思路跟传统方式有什么本质区别

Simscape的本质是物理域建模，它不走"方程驱动"路线，而是走"结构驱动"路线。什么意思呢？就是你不用告诉它"这个系统满足什么微分方程"，你只需要告诉它"这里有一辆小车，小车上装了一个转动铰链，铰链上连着一根摆杆，摆杆顶端又装了一个转动铰链，再连一根摆杆"，然后给它重力、质量、惯量、摩擦参数，Simscape的物理求解器会自动帮你组合出系统的完整动力学行为。

为了让你体会得更直观，我打个比方：传统建模就是你把电脑拆成CPU、内存、硬盘，然后拿电烙铁把每个元器件按电路图焊起来，所有引脚都得自己接对；Simscape则是直接给你一块已经印好线路的主板，你把内存、硬盘插进对应插槽就行。对于二阶倒立摆这种耦合程度极高的系统，"插槽化"的建模方式能帮你省掉海量的计算和排障时间。

1.3 Simscape Multibody到底能帮你省掉什么

具体到二阶倒立摆，Simscape Multibody让你省掉的事情包括：

• 所有运动学关系的自动计算。摆杆之间的角度、角速度、角加速度的传递关系不需要你手动推导。
• 动力学方程的全自动装配。包括惯性力、科里奥利力、重力效应、约束反力，求解器都会动态算好。
• 3D可视化的免费赠送。搭好模型之后自带三维动画效果，你不仅能看控制曲线，还能真实看到倒立摆是"站起来了"还是"转圈掉下去了"，这种反馈感对调参非常有帮助。
• 与控制算法模块的无缝对接。Simscape的物理信号可以通过转换模块转成普通Simulink信号，直接用PID、LQR、滑模控制去作用，流程极其顺畅。

我这里说句实在话：如果你只是想快速把二阶倒立摆跑起来，验证控制算法，Simscape至少能帮你省掉70%的建模时间。这也是这篇文章敢叫"小白必看"的原因，我们直接绕开最劝退的数学关卡。

2. 动手之前，先把这几个概念一次搞透

2.1 Simscape物理域与Simulink数值信号的区别

很多新手第一次接触Simscape，最容易懵的就是"信号类型"这个概念。普通Simulink模块传的是纯数值信号，不管它代表什么含义，在Simulink看来就是一堆double类型的数。但Simscape里传输的是物理信号，它自带物理单位，比如速度就带m/s，力就带N，电压就带V。这种设计的好处是它逼着你把物理世界的东西描述清楚，减少因为单位混淆导致的低级错误。

两个世界之间的桥梁是Simulink-PS Converter（Simulink信号转物理信号）和PS-Simulink Converter（物理信号转Simulink信号），名字有点拗口，记法很简单：从哪边到哪边，就找哪个模块。比如你算出一个控制力，它现在是个普通数值，你要把它作用到小车上的移动副里，那就得先在中间放一个Simulink-PS Converter，把小数转换成带单位的物理信号。

2.2 Simscape Multibody里的刚体坐标系与铰链

Simscape Multibody建模的两个核心基础概念：刚体坐标系和铰链。刚体坐标系就是每个部件的本地参考系，决定这个部件的原点在哪、轴向朝哪。铰链则用来连接两个刚体，限制它们的相对运动。二阶倒立摆里用到的铰链只有两种：移动副（Prismatic Joint）控制小车在滑轨上平动，以及转动副（Revolute Joint）控制摆杆绕轴转动。

这里有个小白的常见误区：以为摆杆之间的转动副只需要设置一个旋转轴就行。实际上，转动副的轴向方向直接决定了摆杆往哪个方向摆，如果装反了，你会发现模型初始状态本身就是倒的，你的控制器怎么救都救不回来。

2.3 二阶倒立摆的动力学特性直觉

虽然我们说不用担心推导方程，但你至少应该对二阶倒立摆的"脾气"有点直觉。二阶倒立摆比一级倒立摆难点主要在于：两级摆杆之间互相影响，中间那根摆杆要同时兼顾下面小车的运动和上面摆杆的稳定，所以控制器必须协调处理多个状态的反馈。

它的稳定状态只有一个，就是两级摆杆都竖直向上直立，但这个平衡点是不稳定的，微小扰动就会让系统飞走。这意味着，如果你的控制器输出为零，模型会在零点几秒内急速倒下，这是完全正常的物理行为，不是模型搭错了。建模的时候不用过度担心这一点，只要后面控制器接对了，就能看到它"立起来"。

3. 正式搭建：二阶倒立摆机械结构与完整参数配置

3.1 搭建前的准备工作与环境设置

我建议你用的MATLAB版本在R2020a以后，Simscape Multibody模块库在旧版本虽然也能用，但界面和部分模块命名有差异，提高版本能减少很多不必要的麻烦。

打开MATLAB后，先在命令行输入smnew，这个是Simscape Multibody官方给的模型模板，用它可以载入一个最基本的双摆模型框架，在这个基础上改效率最高。不建议从完全空白的新模型开始搭，因为你还需要手动配置求解器、物理端口连接规则等一堆基础设置，纯小白容易漏。

然后打开模型配置参数，我一般会做两件事：第一，求解器类型选变步长，求解器选ode23t。原因是Simscape模型通常是刚性问题，ode45会经常报计算发散，而ode23t专门用来解这种中等刚性的动力学系统。第二，在Simscape选项里，把高级类的"局部求解器"设置为自动，这个选项涉及模型内部的微分代数方程处理，小白用默认的"自动"就行，不要手贱去改。

3.2 从库浏览器拖出需要的模块清单

在Simulink库浏览器里，输入"Simscape Multibody"关键字，你会看到全套多体建模模块。这里我先把用到的模块名称和用途列出来，让心理有个数：

• World Frame：世界坐标系，所有模型的绝对参考，相当于物理意义里的"地面"。
• Mechanism Configuration：机构全局配置，填重力。
• Solver Configuration：求解器配置模块，每个物理网络必须且只能有一个。
• Prismatic Joint：移动副，用于小车在导轨上的平动。
• Revolute Joint：转动副，用于摆杆之间的转动。
• Solid：刚体块，用于表示小车、一级摆杆、二级摆杆的几何和惯性属性。
• Transform：坐标变换，用于把刚体之间的连接点挪到正确位置。
• Joint Actuator：关节驱动器，把外部踢力或者力矩信号作用到关节上。
• Joint Sensor：关节传感器，用来测关节位移、速度、角度、角速度。
• Simulink-PS Converter和PS-Simulink Converter：物理信号和普通信号的桥梁。
• Scope：显示波形，不用我说了。

这些模块不是每个都需要从库里慢慢搜，我的习惯是先用smnew出来的模板，把World Frame、Mechanism Configuration、Solver Configuration这些基础设施都保留，然后自己再加刚体和关节。

3.3 机械结构的三个关键步骤

第一块是搭小车。把Solid拖到模型里，双击设置几何和惯性：几何设为长方体，长0.3米、宽0.2米、高0.1米，这就是小车的体积；密度默认用7850（钢铁密度）也行，但我建议直接把质量设成0.5千克，让它的惯性参数与真实实验设备更接近。Solid模块里的质量设置有两种方式，一种是通过几何加密度算出来，另一种是手动覆盖质量，直接指定惯性参数。小白选后者更直观。

第二块是连接小车和一级摆杆。小车上要选一个安装点，一般在Solid模块的Frame端口处生成一个框架，用Transform把坐标系挪到小车顶部中间位置，然后在这个坐标点上接第一个Revolute Joint。这个转动副的旋转轴设为Y轴，使摆杆能在垂直平面内前后摆动。一级摆杆用另一个Solid模块表示，几何设为圆柱体，长0.2米，半径0.02米，质量设成0.2千克。这里要注意的是，Solid的默认坐标系在质心，所以你要用Transform把连接点从质心挪到摆杆的一端，否则转动副连接之后，摆杆的旋转中心会在它中间，看着完全不对劲。

第三块是一级摆杆顶端再接二级摆杆。方法和第二块几乎一样：在一级摆杆的另一端创建一个Transform，然后接第二个Revolute Joint，旋转轴仍然与第一个平行，再挂第二个Solid作为二级摆杆。二级摆杆长度设0.15米，半径0.015米，质量0.15千克。到这里，机械结构就搭完了。模型看起来应该是一根竖线上串了三个方块/圆柱，下面那个沿着导轨滑动。

3.4 完整参数配置参考表

为了让不熟悉参数含义的朋友也能直接抄作业，我把仿真需要的关键参数集中放在一张表里。

有些人做倒立摆仿真，常常忽略阻尼参数，把它设成0。这在理想理论模型里没问题，但如果你后面要拿这个模型跟真实设备对标，阻尼设0会让系统对控制的响应过于灵敏，调好的参数拿到实物上反而稳不住。我的习惯是移动副阻尼设10左右，转动副阻尼设0.001到0.01之间，这样既有一定真实感，又不至于影响系统的可控性。

3.5 初始状态设置的关键细节

二阶倒立摆初始状态怎么设，很多人直接不设置，让模型默认从直立位置开始，结果一仿真就发现摆杆瞬间倒下，还以为是模型搭错了。这里解释一下背后的原因：如果你把初始角度设为0度，Simscape会认为摆杆已经处于精确竖直位置，而竖直位置又是不稳定平衡点，数值上稍微不精确，仿真就朝倒下的方向发展。这其实不算错，它就是真实物理行为。

所以我想说明一个实用技巧：故意给一点初始偏差，比如一级摆杆5度、二级摆杆3度。这样你的控制器一开始就要纠正这个偏差，反而能更直观地判断控制器的镇定效果。如果你设的初始角度太大，比如超过15度，LQR控制器在初始时刻会输出超大的控制力，小车会猛加速，先不说能不能把摆杆立起来，这个动作本身在实物上就很危险。仿真虽然不会炸，但物理机制上已经背离倒立摆"小偏差镇定"的真正意义了。

4. 让它立起来：LQR控制器接入与调参实战

4.1 为什么不用PID而是用LQR

倒立摆小白最常见的问题就是：我想用PID控制倒立摆，该怎么调？对一级倒立摆来说，单回路PID或多回路PID还有可能调出来，但二阶倒立摆是一个高度耦合的欠驱动系统，单单对角度做PID反馈，几乎不可能同时稳住两级摆杆，因为中间那根摆杆的状态会剧烈影响末端的动态，你单独看哪个角度都不全面。

LQR（线性二次型调节器）的原理是对所有状态变量做加权反馈，它天然适合"多个状态必须同时镇定"的场景。你在Simscape里通过传感器测出小车位移、小车速度、一级摆角度、一级摆角速度、二级摆角度、二级摆角速度，这六个状态全部反馈给控制器，然后由控制器算出一个力输出给小车。这样整个系统是一个完整的状态反馈闭环，比PID盲人摸象式的单变量调节要自然得多。

4.2 在线性化环节拿到状态空间矩阵

设计LQR之前，需要系统的状态空间模型，也就是A矩阵和B矩阵，这个可以用Simulink的线性化分析功能从Simscape模型自动算出来。操作方法：在Simulink模型中右键点击"线性化分析"或者用linmod指令，然后指定线性化输入输出点。输入点选小车的移动副执行器力信号，输出点选那六个状态传感器的汇总信号。

在平衡点处的线性化会自动得到6阶模型，这个6阶模型对应的就是六个状态变量。注意，初始状态越接近0，线性化精度越高，所以前面让你给初始角度设5度和3度，是方便之后看控制效果，而不是线性化时刻用的。真正线性化的时候，最好临时把初始角度改小到1度以内，让模型在名义平衡点附近线性化。

4.3 LQR权重矩阵Q和R的设置思路与推荐值

LQR的核心是人为设定Q矩阵和R矩阵，Q的维度与状态个数一致，R对应该系统的单输入所以就是1x1的矩阵。Q矩阵的对角线元素代表对不同状态重视程度：你希望哪个状态更快收敛，对应的Q值就设更大一些。二阶倒立摆推荐一组基础配置：

• 小车位移权重10
• 一级摆角权重100
• 二级摆角权重200
• 对应速度项权重分别设1、0.1、0.1

这组参数的含义是：比位移更重视摆角，且二级摆角的稳定优先级最高，这是因为二级摆角对末端运动敏感，权重太低初期会停不住。用MATLAB命令K = lqr(A, B, Q, R)可以直接算出反馈增益矩阵K，K是一个1x6的行向量，六个数字分别对应六个状态的反馈系数。

有一点我想展开说明：Q矩阵多个数量级的差异很常见，倒立摆这种量级差异极大的系统，速度项和位移项在数值上差着几十倍，如果你需要用同一个数量级去做单位归一化，就得在设置Q值之前先把状态变量做无量纲化。小白建议先照着上面的量级跑起来，看效果再慢慢调。

4.4 在Simulink模型里实现状态反馈闭环

LQR增益算出来之后，在Simulink里搭闭环非常机械：用六个Joint Sensor分别测出位移、速度、一级角度、一级角速度、二级角度、二级角速度，把这些信号通过PS-Simulink Converter转换成普通数值，然后组合成一个6维向量，用Gain矩阵模块乘以K矩阵的转置，得出的结果就是控制力。

把这个控制力取个负号再通过Simulink-PS Converter转回物理信号，接到移动副的Joint Actuator上去，闭环就完成了。有一点别忘了，不论你用的是LQR还是后来的任何控制器，控制力方向必须与移动副的正方向定义一致，不然你给的是正反馈，系统会以更快的速度发散。

4.5 仿真时长与可视化效果验证

闭环搭好以后，把仿真时间设为10秒，点击运行。正常现象是：初始0秒时摆杆稍微倾斜，小车先往倾斜反方向加速，把摆杆往竖直方向推回，然后经过几次小幅震荡后稳定在竖直位置。此时小车可能在导轨上有一段位移，但最终会稳定在某个位置附近。

同时打开Simscape Mechanics Explorer窗口，你可以看到三维动画，那一刻的成就感真的很值。如果能稳定在竖直位置，说明这套模型和控制参数已经搭建成功，你的二阶倒立摆仿真模型就完成了。

5. 排查手册：小白最容易踩的5个坑

5.1 模型能跑但速度极慢，卡得怀疑人生

Simscape模型的仿真速度受求解器选择和误差容限双重影响。如果你用的是ode45，碰到刚性问题时自适应步长会变得极小，仿真时间会指数级上升。解决方法很简单：换成ode23t，或者ode15s，并在求解器设置里把相对误差调成1e-4或1e-3，绝对误差调成auto。这样仿真速度会有数量级级别的提升。

5.2 初始不施力，摆杆直接疯狂转圈掉下

这基本可以肯定不是模型问题，而是初始状态设置问题和控制器尚未接通的问题。在没有控制器输入的情况下，二阶倒立摆本来就会倒下，这是它的本能。你要检查的是控制器有没有真正接入关节执行器，有没有把反馈符号搞反，以及LQR矩阵K有没有正常算出来。

5.3 传感器读数怎么全是NaN

NaN的出现几乎都能追到资料上的问题：物理网络里存在奇异或代数环，比如传感器接到了没有物理意义的位置上，或者移动副的约束定义不到位，导致某个状态自由度无解。常见解决方法是检查所有Solid的坐标变换是否都正确连接到了关节的Frame端口，不要留下悬空端口。另外，把局部求解器从"自动"改为"始终使用局部求解器"，有时候能绕过代数环的问题。

5.4 控制器明明算好了，摆杆却越控越倒

这是反馈符号设置错误造成的典型表现。倒立摆控制是负反馈，你算出的控制力方向必须与状态偏差方向相反。检查一下Gain模块后面有没有加负号，加完负号之后如果系统稳定了，说明确实是符号问题。另一个隐藏点是他算出来的K矩阵有时是求解器带符号的lqr形式，不同MATLAB版本lqr返回的符号可能略有差异，所以你最好在命令窗口手动打印出K值，肉眼检查符号，再填回模型里。

5.5 二级摆杆角度权重到底怎么设才不抽风

在Q矩阵设置上，二级摆角权重一般要比一级摆角权重高，至少1.5倍以上，因为二级摆杆质量更轻更容易受影响，如果不给足够的控制权重，它会处于一种漫无目的的摆动状态。如果二级摆角权重过高，则小车位移权重就会被压得很低，小车会在导轨上疯狂移动来稳住二级摆杆，看起来极其鬼畜。这时把小车位移权重往高了调，找到一个位移响应和摆角响应都均匀的平衡点。

6. 一些拓展思路：模型搭好了还能干什么

模型能立起来只是开始。我个人觉得Simscape这种物理建模方式的优势在于，后续改成各种更复杂的算法都非常方便，因为模型本身就是一套"真实虚拟物理对象"，算法层面的改动完全不会破坏机械结构部分。

你可以试试把LQR换成滑模控制，对比一下两种方法在二阶倒立摆上的响应效果，这是很多论文里喜欢用的对比实验。也可以加上风干扰模块，在摆杆上作用一个随机力矩，测试控制器的鲁棒性。还可以把机械部分的参数稍微调一下，比如缩短二级摆杆长度，看看原套控制器还能不能稳住，这其实就是一种参数摄动鲁棒性测试的思想。甚至你可以通过MATLAB Function模块把MPC控制器或者是强化学习动作网络接进来，整个模型本身就像一台虚拟实验设备。

最后我再分享一个小技巧：Simscape Multibody模型导出的模型[Reference]类型，能让你的模型以状态空间形式参与更底层的仿真验证。而且，当你的二阶倒立摆模型跑通了，回头再去看一级倒立摆的推导方程，你会有一种豁然开朗的感觉，物理直觉也在这个过程中被打通了。这是整个动手过程最让我觉得值回票价的部分。希望这篇文章也能让你感受到这种把抽象理念变成可视化物理对象的美妙体验。