你刚接触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 ConverterSimulink信号转物理信号和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 完整参数配置参考表为了让不熟悉参数含义的朋友也能直接抄作业我把仿真需要的关键参数集中放在一张表里。参数项数值说明重力加速度9.81 m/s²在Mechanism Configuration里设置方向默认沿Y轴负方向小车质量0.5 kgSolid质量覆盖小车几何0.3 x 0.2 x 0.1 m长宽高一级摆杆质量0.2 kg一级摆杆长度0.2 m旋转中心到下一连接点一级摆杆转动惯量依几何自动算圆柱绕端部不在质心时不要手动填错二级摆杆质量0.15 kg二级摆杆长度0.15 m移动副位置下限-1.5 m小车从左边界到右边界范围移动副位置上限1.5 m移动副阻尼10 N·s/m摩擦阻尼如果太小小车来回晃不停转动副阻尼0.001 N·m·s/rad摆杆铰链处阻尼不宜过大过大系统响应会变钝一级摆杆初始角度5度轻微的初始扰动如果设成精准0度后面控制器反而不太能测试鲁棒性二级摆杆初始角度3度二级初始偏离别太大否则LQR在初始时刻可能拉不回来有些人做倒立摆仿真常常忽略阻尼参数把它设成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)可以直接算出反馈增益矩阵KK是一个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 传感器读数怎么全是NaNNaN的出现几乎都能追到资料上的问题物理网络里存在奇异或代数环比如传感器接到了没有物理意义的位置上或者移动副的约束定义不到位导致某个状态自由度无解。常见解决方法是检查所有Solid的坐标变换是否都正确连接到了关节的Frame端口不要留下悬空端口。另外把局部求解器从自动改为始终使用局部求解器有时候能绕过代数环的问题。5.4 控制器明明算好了摆杆却越控越倒这是反馈符号设置错误造成的典型表现。倒立摆控制是负反馈你算出的控制力方向必须与状态偏差方向相反。检查一下Gain模块后面有没有加负号加完负号之后如果系统稳定了说明确实是符号问题。另一个隐藏点是他算出来的K矩阵有时是求解器带符号的lqr形式不同MATLAB版本lqr返回的符号可能略有差异所以你最好在命令窗口手动打印出K值肉眼检查符号再填回模型里。5.5 二级摆杆角度权重到底怎么设才不抽风在Q矩阵设置上二级摆角权重一般要比一级摆角权重高至少1.5倍以上因为二级摆杆质量更轻更容易受影响如果不给足够的控制权重它会处于一种漫无目的的摆动状态。如果二级摆角权重过高则小车位移权重就会被压得很低小车会在导轨上疯狂移动来稳住二级摆杆看起来极其鬼畜。这时把小车位移权重往高了调找到一个位移响应和摆角响应都均匀的平衡点。6. 一些拓展思路模型搭好了还能干什么模型能立起来只是开始。我个人觉得Simscape这种物理建模方式的优势在于后续改成各种更复杂的算法都非常方便因为模型本身就是一套真实虚拟物理对象算法层面的改动完全不会破坏机械结构部分。你可以试试把LQR换成滑模控制对比一下两种方法在二阶倒立摆上的响应效果这是很多论文里喜欢用的对比实验。也可以加上风干扰模块在摆杆上作用一个随机力矩测试控制器的鲁棒性。还可以把机械部分的参数稍微调一下比如缩短二级摆杆长度看看原套控制器还能不能稳住这其实就是一种参数摄动鲁棒性测试的思想。甚至你可以通过MATLAB Function模块把MPC控制器或者是强化学习动作网络接进来整个模型本身就像一台虚拟实验设备。最后我再分享一个小技巧Simscape Multibody模型导出的模型[Reference]类型能让你的模型以状态空间形式参与更底层的仿真验证。而且当你的二阶倒立摆模型跑通了回头再去看一级倒立摆的推导方程你会有一种豁然开朗的感觉物理直觉也在这个过程中被打通了。这是整个动手过程最让我觉得值回票价的部分。希望这篇文章也能让你感受到这种把抽象理念变成可视化物理对象的美妙体验。