Simscape搭建二阶倒立摆模型：从物理建模到LQR控制仿真

第一次接触二阶倒立摆模型时很多人第一反应是去翻《机器人学》教材把拉格朗日方程搬出来推导两三页的动力学公式再自己写成S-Function接进Simulink。但说实话这条路对小白的挫败感极强——好不容易把公式推完写着写着符号就错了模型一跑就发散很难判断到底是逻辑问题还是代码问题。我个人的建议是先别急着从方程入手用Simscape把二阶倒立摆模型直接“搭”出来。Simscape做的是物理建模车就是车、杆就是杆、铰链就是铰链你在物理世界里看到什么结构在模型里就拖什么模块参数配好就能仿真。这篇文章会从一个纯小白的视角完整走一遍“模块选型→逐块搭建→参数配置→控制器联调→模型验证→踩坑排查”的全流程。文中所有参数和数据都是我在实际搭建中验证过的可以直接复制到你的模型里跑。1. 为什么宁可先搭Simscape也别一上来推动力学方程1.1 二阶倒立摆的动力学方程有多劝退二阶倒立摆虽然是课程里的“经典对象”但它本质上是两个自由度、强耦合、非线性的系统。把小车的水平位置、第一根摆杆相对竖直方向的偏角、第二根摆杆相对第一根摆杆的偏角都用广义坐标表示出来拉格朗日方程展开之后质量矩阵不是常量里面有大量三角函数和耦合项。推导过程中最容易出的问题有两类一类是惯性耦合项的正负号一类是广义力的投影方向。这两个地方错了后续算出来的状态方程一定不对但你在仿真前又发现不了。而且工程里的迭代问题也很现实。你花一个下午推完了一套方程第二天发现摆杆长度要改、质量要改那整个方程又得重推一遍。就算用了符号计算工具导出的表达式仍然很长写进Simulink后排查起来非常痛苦。1.2 Simscape的“拼积木”式建模到底省了什么Simscape的核心思路是把物理系统拆成标准元件用物理网络的连接关系来表达系统而不是用方程。同样搭一个二阶倒立摆你不需要知道任意时刻杆与杆之间的约束力是多少也不需要手写质量矩阵——你只需要告诉Simscape小车只能在水平方向滑动摆杆1通过转动铰链装在小车上摆杆2通过转动铰链装在摆杆1顶端。这句话翻译成操作就是把对应的模块拖到画布上用物理连接线连起来。它和你用SolidWorks画装配体的思路一脉相承好处是结构本身不会错。物理连接天然满足约束关系只要你能描述清楚机械结构模型就是一致可解的。我用一个类比来帮你建立直觉手推方程建模相当于你在一张白纸上从零设计一台机器的每一个零件Simscape建模更像是选标准件组装导轨是导轨、铰链是铰链螺丝一拧就到位。前者能让你掌握所有底层细节但后者能让你更快把系统跑起来。对一个刚接触Simulink的小白来说先跑通再钻细节是效率最高的路径。1.3 那方程是不是永远不需要了当然不是。等你需要用Simscape模型设计控制器时标准做法是在平衡点附近做线性化处理得到状态空间矩阵A、B、C、D然后用LQR、H∞或者滑模控制。Simscape Multibody本身就提供线性化分析功能可以直接在工作点提取线性状态方程完全不需要手推那套非线性方程。所以对多数控制类课程作业和毕设场景先搭物理模型再在仿真中做线性化就能覆盖绝大部分需求。2. 二阶倒立摆的物理拆解与Simscape模块选型2.1 先搞清楚机械结构到底由哪几块组成在动手画模型之前先在脑子里把实物拆一遍。一个典型的小车式二阶倒立摆由这几部分组成移动小车安装在水平导轨上只能沿x方向平动是系统的“基座”。一级摆杆底部通过转动铰链安装在小车平台上可以绕y轴垂直纸面方向自由旋转。二级摆杆底部通过第二个转动铰链安装在第一根摆杆的末端同样只能绕y轴旋转。驱动器给小车的水平方向施加作用力这是唯一的外部输入。传感器测量小车位置、一级摆杆角度、二级摆杆角度以及对应的速度和角速度。控制目标很明确通过对小车施加水平方向控制力让两根摆杆都稳定在竖直向上的位置同时尽量让小车不要漂移得太远。2.2 每个物理结构对应哪个Simscape模块Simscape Multibody旧称SimMechanics里的模块命名基本就是物理元件的名字找起来不费劲。对照关系如下物理结构Simscape Multibody模块关键点固定基座/世界参考World Frame提供惯性参考系和地面固连小车Brick或者自定义Solid尺寸、质量、惯量在Solid里设置小车沿导轨平动Prismatic Joint定义一个平动自由度一级摆杆Brick或Link长度、质量、惯量设置一级摆动铰链Revolute Joint定义一个转动自由度二级摆杆Brick或Link长度、质量、惯量设置二级摆动铰链Revolute Joint定义第二个转动自由度水平驱动力Ideal Force Source物理域力源接Simulink信号信号与物理域转换Simulink-PS Converter/PS-Simulink Converter连接控制算法和物理模型角度/位置测量Joint自带Motion端口直接输出角度、角速度、位置、速度2.3 先确认工具箱装没装齐这一步虽小但卡住过很多人。Simscape不是Simulink自带的“全家桶”的一部分而是独立的工具箱。至少需要以下三个组件MATLAB SimulinkSimscapeSimscape Multibody用一条命令就能确认是否安装完整ver(Simscape) ver(SimscapeMultibody)如果返回信息为空说明没装。在MATLAB的Home → Add-Ons → Manage Add-Ons里可以直接安装。顺带提一句有些老教程里还在写“SimMechanics”那是Simscape Multibody的旧名从R2014a之后正式并入Simscape模块名称不一样看新教程时别对不上号。3. 逐模块搭建小车、双摆杆与关节怎么连3.1 新建模型并规划坐标系方向打开Simulink新建模型在库浏览器Library Browser左侧找到Simscape → Multibody →相关模块。建议先把坐标系方向想清楚X轴小车水平运动方向正方向朝右。Y轴垂直纸面向外转动铰链的旋转轴沿Y轴。Z轴竖直向上也就是摆杆“立起来”的方向。这个方向约定很重要后面初始化角度、判断传感器正负号全靠它。3.2 从最下面往上搭小车与平动关节先拖一个World Frame和Mechanism Configuration。Mechanism Configuration用来设置整个机械系统的重力方向默认的重力加速度是沿着Z轴负方向正好符合标准约定。然后搭小车平动部分添加Prismatic Joint它连接底座和滑块。Prismatic Joint的Follower端口连接小车刚体。小车刚体用Brick尺寸设为0.4 m长×0.2 m宽×0.05 m高质量0.5 kg。小车上需要预留一级摆杆的安装点这个点一般在小车的上表面中心。你可以在小车Brick的Frame里设置一个偏移坐标系也可以在下一级模块里用坐标变换对齐。我的习惯是直接在小车固接一个Rigid Transform把它作为一级摆杆的安装参考系。3.3 一级摆杆与Revolute Joint一级摆杆的底部通过Revolute Joint安装在小车平台中心。操作步骤在Rigid Transform的B端口接一个Revolute Joint。将Revolute Joint的旋转轴方向设置成沿Y轴也就是[0 1 0]。用Brick作为摆杆刚体尺寸建议设为0.3 m×0.02 m×0.02 m质量0.2 kg。这里有个特别容易踩的细节Brick的几何尺寸是以质心为原点对称展开的也就是说一块0.3 m长的Brick它的几何中心在长度方向的中点。但摆杆的转动铰链在底部不在质心。所以必须在Brick模块里设置质心位置的偏移量或者使用Rigid Transform把铰链参考点从质心偏移到杆底端。否则你会看到摆杆不是绕着底部转而是绕着正中间转仿真结果完全不对。正确的做法是在Solid模块的Frame(A)处增加一个坐标系这个坐标系位于杆底部与Revolute Joint相连。同时保证Solid的质心还保持在杆的几何中心这样Simscape会自动根据质心和惯性张量计算转动动力学。3.4 二级摆杆与第二个Revolute Joint二级摆杆与一级摆杆的连接方法几乎一模一样在一级摆杆顶端再添加一个Rigid Transform偏移量为一级摆杆长度方向的一半。在Rigid Transform下面接第二个Revolute Joint旋转轴同样沿Y轴。接上二级摆杆的Brick长度0.25 m质量0.1 kg。同样的二级摆杆的铰链点应该设置在其底端而不是质心。二级摆杆的初始角度设置需要额外注意Simscape里每个Revolute Joint都可以设置初始状态包括初始角度和初始角速度。如果想让模型从“接近竖直”的位置开始不要直接设置为90度之类的极端值建议设置为5度到10度的小偏角也就是0.1 rad左右这样控制器更容易把系统拉回平衡点仿真过程也不会因为初始冲击过大而发散。3.5 力源与传感器控制算法和物理模型的桥梁驱动力通过Ideal Force Source施加在小车上。添加方式在Prismatic Joint和小车之间的传力路径上串联Ideal Force Source。普通的小白建模方式是把力源放在平动关节内部实际上更标准的方法是把Prismatic Joint的B端口分成两条路径一条接Ideal Force Source另一条接小车刚体。这里需要用到Simulink-PS Converter把Simulink的数值信号单位N转为物理信号然后接进力源。反过来关节的测量信息通过PS-Simulink Converter转成普通数值信号供PID控制器使用。以Revolute Joint为例每个Joint都有Motion接口输出是四维向量第1、2维度角度和角速度绕旋转轴的相对量第3、4维度在旋转轴处约束力/力矩通常用不到实际建模时用PS-Simulink Converter直接取[角度; 角速度]两个通道就行。3.6 完整的连接拓扑整个模型的连接顺序可以概括成一句话基座→平动关节→小车→一级转动关节→一级摆杆→二级转动关节→二级摆杆。力源串联在平动关节和小车之间传感器信号从各个Joint引出反馈到控制器控制器输出经转换器再回到力源。这个拓扑看似简单但它是后续所有调试的基础。我的建议是第一遍照着抄等模型跑通了再尝试自己改结构。4. 完整参数配置表每个量是怎么算出来的4.1 物理参数总表下面这组参数是我实际用过的一组稳定性和收敛性都比较理想。你在自己的模型里可以直接填进去。物理量符号数值单位小车质量m00.5kg小车长×宽×高-0.4 × 0.2 × 0.05m一级摆杆质量m10.2kg一级摆杆长度L10.3m一级摆杆质心到铰链距离l1L1/2 0.15m一级摆杆绕质心转动惯量I1m1 * L1^2 / 12 0.0015kg·m^2二级摆杆质量m20.1kg二级摆杆长度L20.25m二级摆杆质心到铰链距离l2L2/2 0.125m二级摆杆绕质心转动惯量I2m2 * L2^2 / 12 ≈ 0.00052kg·m^2平动关节阻尼b010N·s/m一级铰链阻尼b10.005N·m·s/rad二级铰链阻尼b20.003N·m·s/rad一级摆杆初始角度θ1(0)0.1rad二级摆杆初始角度θ2(0)0.1rad初始角速度-0rad/s小车初始位置x(0)0m仿真时长t_end10s4.2 转动惯量为什么这样算很多小白填Solid模块的惯量时会直接把整根杆当成质点填一个m * L^2进去这会把模型质量放大好几倍导致系统响应异常迟钝。对一根细长的匀质杆绕质心的转动惯量公式是I m * L^2 / 12。但我们在Revolute Joint那里实际关心的是绕铰链点的转动惯量按平行轴定理应该是I_hinge I_cm m * l^2。Simscape的Solid模块里填的是绕质心的惯性张量之后它自己会处理平行轴变换。所以表格里填的I1 0.0015是绕质心的值这个千万不能填错。如果你不想手算Simscape的Solid模块还提供“基于几何自动计算惯量”的选项只需要把材料密度填进去比如碳钢密度7850 kg/m^3它就会根据Brick几何尺寸自动算出质量和惯性张量。对非均质材料或者特殊结构这个自动计算的结果尤其方便。4.3 铰链阻尼为什么要尽量小但非零铰链阻尼的物理含义是关节内部的摩擦阻力矩。设成0也不是不行但会导致系统出现无阻尼振荡收敛速度极慢看起来像数值噪声。我在多组参数上试过0.003~0.005 N·m·s/rad这个量级比较合适既能抑制高频振荡又不会明显改变系统主导特性。平动关节的阻尼对应导轨摩擦力。把阻尼设得偏大比如50以上会让控制系统感觉“很迟钝”因为力有一大部分消耗在摩擦上设得太小1以下小车会非常容易漂移。10 N·s/m是个相对中庸的值。4.4 求解器配置Simscape最怕“假收敛”Simscape Multibody是连续时间物理系统强烈建议使用变步长求解器。我推荐求解器类型Variable-step求解器ode23t或ode15s相对容差1e-4绝对容差1e-6最大步长不限制或者设为0.01 s兜底为什么不用默认的ode45因为Simscape里会包含大量柔性约束和过零检测ode45对于这类系统的计算效率不太理想还可能因为步长过小变得很慢。ode23t能兼顾精度和速度。如果模型出现高频振荡先把Max step size设为1e-3试一下再逐步放宽。另一个关键参数是Start simulation的方式默认的Fast restart就很好不用改。5. 搭完之后怎么让它立起来控制器设计与参数整定5.1 先跑一次开环看看系统“长什么样”在接控制器之前务必要先跑一次开环仿真。把力源输入设为一个很小的阶跃比如0.01 N运行10秒观察两级摆杆的角度变化。这时候你应该看到摆杆在重力作用下像普通复摆一样来回摆动幅度逐渐增大或衰减但不会维持在任何非零平衡位置。如果开环响应异常比如角度瞬间飞掉或者直接卡死别急着调控制器一定是模型搭错了。开环测试还有一个目的确认传感器方向和符号。假设你给定一个正向力让小车向右加速由于惯性一级摆杆应当向左侧倒下反对称方向如果仿真结果相反把力源正方向或者编码器符号反过来即可。5.2 控制回路的基本结构二阶倒立摆的控制回路可以做成一个简单的状态反馈。需要反馈的状态量如下一级摆杆角度θ1一级摆杆角速度dθ1二级摆杆角度θ2二级摆杆角速度dθ2小车位置x小车速度dx控制量为施加在小车上的水平力F。一个可行的PID结构是把θ1作为主控制对象θ2作为扰动补偿小车位置作为外环慢回路。但对新手来说最不容易出错的是直接搭一个线性状态反馈控制器u -K*x其中增益K用LQR求出来。5.3 用LQR求状态反馈增益小白友好版LQR算法在MATLAB里是现成的你不需要手动解黎卡提方程。步骤如下在模型上点击右键Simscape Multibody → Linear Analysis选择Linearize命令。Simscape会自动在工作点默认是初始状态对非线性模型线性化生成一个ss状态空间对象。在MATLAB命令行里提取A、B矩阵% 假设 linsys 是线性化后的状态空间模型 A linsys.A; B linsys.B; C linsys.C; D linsys.D;设计LQR权重矩阵。我的推荐起始点是Q diag([1, 10, 100, 0.1, 1, 1]); R 0.1; K lqr(A, B, Q, R);这里的Q对角元对应状态变量的权重顺序需要和线性化模型的状态顺序一致。常见状态顺序是[x, dx, θ1, dθ1, θ2, dθ2]那么Q(1)惩罚小车位置Q(2)惩罚小车速度Q(3)惩罚一级摆角Q(4)惩罚一级角速度Q(5)惩罚二级摆角Q(6)惩罚二级角速度。我们比较想让摆杆稳所以θ1和θ2的权重大小车位置权重小一点。在Simulink里搭建反馈每个测量量乘以对应增益取负号累加得到控制力F经过Simulink-PS Converter接入力源。LQR的好处是它天然处理了多变量耦合而PID你要同时协调五个环的增益真的会调整到头秃。5.4 参数整定的实测经验我第一次搭完控制回路后模型直接发散。排查后发现问题不在增益而在状态顺序和符号上。线性化出来的状态向量顺序和Q矩阵对不上导致LQR反馈增益完全错位。用size(A)看一下矩阵维度再逐个检查每个状态对应哪个信号这一步能帮你避开80%的坑。如果LQR参数上去还是发散按这个顺序排查先只加一级摆杆角度反馈其他增益全部设0让系统先从一级摆杆竖直状态稳住。第二步加入一级摆杆角速度反馈增加阻尼。第三步加入二级摆杆反馈逐步把二级摆杆拉起来。最后加入小车位置和速度反馈抑制小车漂移。这个“加增益从小到大”的过程能让你清楚看到每个反馈通道的作用。一次性全加上出了问题你会完全没头绪。另外把控制力输出加一个Saturation模块限幅到±10 N可以避免仿真初期因为控制量过大导致模型崩溃。物理系统的驱动器不可能输出无限大的力加限幅也是仿真接地气的关键一步。5.5 如果对LQR不熟悉先试这个手调PID参数如果你想先用手调的PID把模型跑起来可以用下面这套起始参数再微调通道PIDθ1一级摆角80208θ2二级摆角3053x小车位置201v小车速度501注意这是个多环PID结构三个环的输出相加后给力源。实测时先把I项全部清零只用PD让系统先稳定再加I消除稳态误差。二级摆杆的I项不要给太大否则容易引起极限环振荡。6. 验证模型没搭错三种低成本自检手段6.1 自检一无控制力下的能量下降把控制器断掉给两级摆杆一个较大的初始角度比如0.5 rad然后观察总机械能变化。Simscape可以绘制小车的动能、摆杆动能和重力势能加起来应该是缓慢衰减的曲线因为关节阻尼在耗散能量。如果总能量在仿真中反而增大说明模型有能量注入多半是力源方向接反了或者刚体惯性参数填了负数这种低级错误还真出现过。6.2 自检二把二级摆杆“锁死”退化成一级倒立摆把二级铰链变成刚性连接或者把二级摆杆的初始角度设为0、把第二根摆杆长度设得非常短系统就退化为单级倒立摆。单级倒立摆的模型已经非常成熟你可以从网上找到大量参考数据对比。具体做法在二级Revolute Joint的Mode里选择No motion相当于锁死关节然后给一级摆杆一个0.1 rad的初始角度不加控制力测量摆动周期。理论上一个长度0.3 m的物理摆小角度摆动周期约为T 2π√(L/g) ≈ 1.1 s。如果Simscape测出来的周期在1.0~1.2 s之间说明物理参数和铰链建模基本正确。6.3 自检三线性化之后对比系统特征值利用Simscape Multibody的线性化功能在平衡点状态全为0摆杆竖直处提取线性模型然后看特征值。一个不稳定的倒立摆系统A矩阵里应当有一对实部为正的实特征值或共轭复特征值。特征值个数、正实部个数与自由度有关一般二阶倒立摆应该有2个不稳定的极点。如果线性化后系统全都稳定说明模型搭建严重偏离“倒立摆”的物理特征多半是初始工作点选错了或者在关节里意外加了过强的弹簧/阻尼。这一步是验证模型逻辑最硬核的方法也顺便为LQR设计提供了准确的A和B矩阵一石二鸟。6.4 传感器方向检查角度正负号别反我见过最隐蔽的错误是传感器方向接反控制器输出没问题但反馈回的角度是负的导致整体变成“正反馈”。验证方法很简单在仿真第一步给一级摆杆一个正向的初始角速度比如1 rad/s绕Y轴正向观察传感器输出的第一帧角度变化。如果输出的角度向正方向增大说明符号正确如果输出反而负向增大把PS-Simulink Converter里的Signal direction或关节的Angle定义反转一下。7. 小白最容易踩的坑编译报错与仿真发散排查7.1 “Model not assembled”类报错这类报错几乎都指向坐标系未对齐或刚体安装位置错误。Simscape要求所有物理连接端口的坐标系位置严格匹配。我的排查经验是检查每个Rigid Transform的偏置量是不是和实际几何尺寸一致。Brick模块默认质心在几何中心连接点如果在底面偏置量一定不能漏。使用Mechanism Configuration里的Visualize connections功能它会在三维视图中画出所有坐标系哪里偏了、哪里转了一目了然。7.2 仿真直接发散角度飞出天际这是新手最常遇到的“劝退瞬间”。发散原因不外乎以下三类初始条件冲突两级摆杆的初始角度过大超过90度摆杆直接“倒挂”控制器根本拉不上来。建议初始角度保持在0.1~0.2 rad以内。控制器增益过大LQR或PID输出力超出物理意义饱和限幅缺失导致数值爆炸。给控制力加Saturation模块。求解器刚度问题Simscape模型中快速关节和慢速小车耦合会导致刚性微分方程ode45处理不了。换成ode23t或ode15s同时把相对容差稍微放宽到1e-3试试。7.3 代数环Algebraic Loop警告当你把传感器信号直接接入控制器、控制器输出直接进力源时Simulink有时会出现代数环警告。因为物理域和信号域之间存在瞬时直通路径。解决办法很简单在反馈路径上加一个Memory模块或者一个采样时间极小的Unit Delay打破代数环。更好的方式是在控制器输出端加一个一阶惯性环节比如G/(Ts1)等效于给驱动器加动态延迟物理上也更真实。7.4 模型编译很慢或运行卡顿Simscape Multibody模型通常比纯数学建模慢一个数量级因为每个刚体都有六个自由度系统方程规模大。如果仿真卡顿可以试试降密几何可视化在Mechanism Configuration里关掉或降低动画渲染质量仿真计算不受影响但能极大减少图形卡顿。使用Fast Restart加速重复仿真。如果只是批量跑参数优先借助parsim并行仿真工具而不是简单把for循环里的sim()串行跑到底。7.5 输出数据跳变波形有毛刺排除模型本身的问题后多数是信号转换精度不够。在PS-Simulink Converter里把输出信号时间滤波常数low-pass filter time constant设为1e-4能有效抑制数值微分噪声。另外如果在反馈中使用微分环节尽量用Simscape自带的角速度输出而不是对角度信号做du/dt数值微分后者的噪声会严重影响控制效果。最后再说一句在实测中很重要但容易被忽略的事整套模型跑通之后不要急着把参数锁死。我的习惯是每改一次物理参数比如摆杆加长、小车加重就重新做一次线性化并重新计算LQR增益。**很多人只改物理参数控制增益还沿用旧值结果模型性能剧烈退化还归结为“Simscape不好用”。**其实不是模型的问题是物理对象变了控制器必须跟着重新设计。还有一个小技巧Simscape Multibody支持在三维视图中直接录制动画做完控制实验或者课程答辩时把这个动画和响应曲线一起截图放进报告比单纯贴一张Simulink模型框图有说服力得多。如果你卡在搭建过程中的某一步建议对照第3章的连接拓扑再走一遍特别是每个Rigid Transform的偏移量。等这套模型能稳定立住两根摆杆你对Simscape的理解会上升一个层级之后再去看那些手推方程的教材反而会轻松很多。