简介面向控制理论与自动化方向学习者的一份二级倒立摆Matlab仿真资源基于MATLAB 2021a环境开发用于演示二级倒立摆这一经典非线性系统的建模与动态模拟。资源共5个文件压缩包仅2.39MB包含2个m脚本、1张示意图、1个动画gif和1段操作录像avi两个m脚本分别承担主程序入口与双摆动力学建模便于直接运行和二次修改动画gif可随时预览摆动效果示意图有助于比对模型结构操作录像则记录了完整运行流程可直接用Windows Media Player播放。运行时通过主程序即可输出倒立摆摆动动画与实时角度变化曲线直观观察系统运动状态与实时角度变化规律为深入理解倒立摆平衡控制原理提供实验支撑。录像还特别说明了MATLAB左侧当前文件夹必须指向程序所在目录等关键细节能有效避免新手常见路径错误。已有183人学习使用适合正在学习倒立摆控制、机器人平衡或需要搭建仿真演示环境的本科高年级及研究生参考。1. 二级倒立摆的MATLAB仿真到底仿真什么二级倒立摆看起来比一级倒立摆只多了一根杆但实际仿真工作的重心完全不同单摆的动力学方程在教科书写清楚后剩下就是解微分方程二级摆则需要处理两杆之间的角度耦合以及小车加速度同时施加到两个摆杆上的传递路径。用MATLAB做这套模拟仿真最有价值的产物不是那几张状态曲线而是把摆杆的运动过程变成动画并同步输出实时角度变化让控制效果变得“看得见”。这个场景最适合课程设计、毕业设计和快速验证LQR等状态反馈算法的工程师。我下面给出的路径是先建立可复现的线性化模型再设计LQR然后用ode45驱动动画最后把整个仿真过程录成操作录像。按这条路径走新手能一步步跟上有经验的人也能从中看到参数选择的具体门道。2. 二级倒立摆建模从运动方程到状态空间矩阵在进入MATLAB代码之前先把物理模型限制在一个可行的范围内。这里把两根摆杆简化为末端质点忽略转轴阻尼和小车摩擦。这样做的理由很简单LQR设计只需要在平衡点附近有效的线性模型摩擦和非线性项最终会由反馈的鲁棒性吃掉如果你要把仿真结果搬到实际小车再用实验数据把摩擦项补回来也不迟。2.1 物理模型与参数定义状态向量的定义如下x是小车位移theta1是下摆相对竖直向上的角度theta2是上摆相对下摆延长线的角度。控制输入F是小车水平方向的牵引力。参数取一组接近小型实验台架的数值单位统一为kg、m、sM 0.5; % 小车质量 m1 0.2; % 下摆末端质量 m2 0.1; % 上摆末端质量 l1 0.2; % 下摆长度 l2 0.1; % 上摆长度 g 9.8;这组参数的特点是上摆比下摆轻转动力矩更容易失控适合用来检验控制器的鲁棒性。如果只做演示可以把m2放大到0.15稳定性会变好但动画的真实感下降。各参数含义见下表参数数值说明M0.5 kg小车质量m10.2 kg下摆末端质量m20.1 kg上摆末端质量l10.2 m下摆长度l20.1 m上摆长度2.2 质量矩阵和势能Hessian的数值化对末端质点模型广义坐标q[x; theta1; theta2]的动能矩阵为M_mass [0.8, 0.06, 0.01; 0.06, 0.012, 0.002; 0.01, 0.002, 0.001]这个矩阵的来源是拉格朗日方程中的二阶导数项直接代入上面的参数得到。势能V的Hessian矩阵H在平衡点处的值为H [0, 0, 0; 0, -0.686, -0.098; 0, -0.098, -0.098]这两个矩阵可以直接在MATLAB里声明% 质量矩阵和势能Hessian矩阵 M_mass [0.8, 0.06, 0.01; 0.06, 0.012, 0.002; 0.01, 0.002, 0.001]; H [0, 0, 0; 0, -0.686, -0.098; 0, -0.098, -0.098]; % 检查M_mass条件数 cond(M_mass)这里的关键是H中theta1和theta2对应的位置都是负值说明势能在平衡点是局部极大值系统在不加控制时必然发散。cond(M_mass)返回很小说明这个参数下没有接近奇异后面求逆是安全的。若把m2设成0H第二行会退化状态空间可控性会显著下降这就印证了二级摆比一级摆更容易控制直觉。2.3 构建状态空间模型并检查可控性取状态向量X[x; theta1; theta2; xdot; theta1dot; theta2dot]则连续时间系统的状态矩阵A和输入矩阵B由下面代码给出% 构建状态空间 A [zeros(3), eye(3); -M_mass\H, zeros(3)]; B [zeros(3,1); M_mass\[1;0;0]]; C eye(6); D zeros(6,1); sys ss(A,B,C,D); % 可控性检查 Co ctrb(sys); fprintf(可控性矩阵秩 %d\n, rank(Co));A左下块的-M_mass\H是3x3表示角度与位移到加速度的耦合。B的第二行是M_mass[1;0;0]它表示牵引力F首先推动小车再通过质量矩阵耦合到两个摆角。MATLAB左除\比inv(M)*H在数值上更稳定当矩阵维度变大时这个差异会更明显。运行后如果秩等于6说明系统可控LQR设计的前提成立。如果秩小于6回到参数列表检查m1、m2是否取了相同值等常见原因是l20导致上摆不可控。3. 用LQR让二级倒立摆“立得住”3.1 为什么选LQR而不是PID单级倒立摆还可以用PD串级勉强维持二级摆的角度耦合让PID的每个回路都互相牵制你调theta1的PD结果theta2的响应跟着出现相位滞后再回头调theta2又会激起theta1谐振。LQR直接对全状态做线性反馈控制器能同时看到小车位置、两个摆角以及三个速度量耦合问题在代数上是统一的。我一般先试用LQR原因很简单设计参数只有Q和R两个矩阵调参路径比PID清晰得多。3.2 权重矩阵Q和R的工程设定LQR的目标函数是积分(xQx uRu)其中uF。Q矩阵的物理意义是对各状态偏离平衡点的惩罚R是对控制力大小的惩罚。实际调试时采用对角线矩阵% LQR权重 Q diag([100, 100, 500, 1, 1, 1]); % 位置、下摆角、上摆角、速度、角速度 R 1; K lqr(A, B, Q, R); eig(A - B*K) % 观察闭环极点分布Q的第一个对角元是100对应小车位置要求它快速回中第三个对角元是500对应上摆角度因为它离控制力最远需要更强的惩罚。速度相关的权重只设1避免控制器过度微分放大噪声。R1表示把牵引力约束在合理范围内如果R过小K会变得很大实际电机容易饱和。这里的调参经验可以做一个简单对照Q取值建议现象调整方向上摆权重500上摆收敛快但小车来回摆动降低上摆权重到200上摆权重50上摆修正慢、系统反复晃动提高上摆权重到500-800R0.1控制力峰值过大仿真超调明显增大R到1-5R10响应慢摆杆迟迟不立直减小R到0.5-1这个表格实际是调试时最常看的四个信号。注意每次调完权重后重新执行lqr然后观察eig(A-B*K)是否都在左半平面。3.3 闭环极点与K矩阵的计算闭环极点的位置直接对应控制品质。上例运行后六个极点中会有两对主导共轭复根。虚部过大会导致动画里摆杆来回晃动多次才收住实部绝对值过大则会让响应速度太快而超出线性化假设。我一般在仿真前先检查最大负实部是否小于8如果大于8就适当降低Q中的角度权重避免在起步阶段就触发大角度脱离线性区。K矩阵算出后控制力F由-F -KX产生。在后面的仿真里要把这个反馈写成Acl A - BK用ode45直接对线性闭环系统积分。这里不推荐在求解器内部再调用lqr因为K已经固定每个时间步重复计算纯属浪费。提示换一组Q/R后必须先重新计算K再重新计算A-B*K。直接在原闭环系统上修改Q不会更新K动画里看到的就是上一次的控制律。4. 仿真与动画用ode45驱动摆杆运动4.1 设置仿真时长和输出步长仿真的一大半“真实感”来自数值积分设置。ode45是自适应步长的Runge-Kutta求解器但我们需要等间隔的输出否则动画时间轴会乱。常见做法是给ode45一个指定tspan向量并在options里设置RelTol和AbsTol% 仿真参数 Tspan 0:0.01:10; % 10秒每10毫秒一个输出点 y0 [0; 0.1; -0.05; 0; 0; 0]; % 下摆偏10度上摆相对偏-5度 opts odeset(RelTol, 1e-6, AbsTol, 1e-8); [t, y] ode45((t, y) (A - B*K)*y, Tspan, y0, opts);这里Tspan的第一个元素不能等于后续步长不能写成0:0.01:10以外因为ode45只在tspan指定的点输出内部积分步长是自适应的。RelTol设为1e-6AbsTol设为1e-8对数值稳定性足够再小会造成积分步数爆炸再大会出现摆杆缓慢发散的假象。y0中theta10.1radtheta2-0.05rad这个初始状态在平衡点附近LQR完全能拉回来动画看起来又比较明显。4.2 实时绘制摆杆动画与角度曲线动画的核心是用line对象更新坐标而不是反复plot。先创建两个figure子图左边画小车和摆杆右边画theta1、theta2随时间的变化曲线。在循环里用set函数更新摆杆的XData/YData然后对角度曲线追加新点% 建立画布 figure(Position,[100 100 900 450]); subplot(1,2,1); hold on; axis equal; xlim([-0.8 0.8]); ylim([-0.05 0.4]); plot([-0.5 0.5], [0 0], k, LineWidth, 2); % 导轨 cart rectangle(Position,[-0.05 -0.03 0.1 0.03]); line1 line([0 0],[0 0], Color,b, LineWidth, 2); line2 line([0 0],[0 0], Color,r, LineWidth, 2); subplot(1,2,2); hold on; xlim([0 10]); ylim([-0.4 0.4]); plot_t1 line([0],[0], Color,b); plot_t2 line([0],[0], Color,r); % 动画循环 for i 1:length(t) x y(i,1); th1 y(i,3); th2 y(i,5); x1 x l1*sin(th1); % 下摆末端x y1 l1*cos(th1); x2 x1 l2*sin(th1th2); % 上摆末端x y2 y1 l2*cos(th1th2); set(line1, XData, [x x1], YData, [0 y1]); set(line2, XData, [x1 x2], YData, [y1 y2]); set(cart, Position, [x-0.05 -0.03 0.1 0.03]); set(plot_t1, XData, t(1:i), YData, y(1:i,3)); set(plot_t2, XData, t(1:i), YData, y(1:i,5)); drawnow(limitrate) % 限制刷新率避免占用过多CPU end这段代码里line1和line2是两个“杆”图形的句柄循环里set改变了它们的端点坐标。用line而不是plot是因为set可以直接更新不需要清理旧图形。右下角的plot_t1和plot_t2是用line([0],[0])生成的空曲线循环里把历史时间与角度值整体传入实现了实时角度变化曲线的滚动绘制。drawnow(limitrate)比普通drawnow更高频限制10秒仿真在普通笔记本上大约2-3秒就能播完。4.3 动画卡顿的常见归因如果画面卡成PPT先查三件事仿真输出点数过多、每条曲线都新增图形对象、以及subplot内坐标轴未固定。输出点数在8000以下基本流畅超过20000就明显掉帧。固定坐标轴用xlim/ylim而不是靠数据自动缩放。如果你用了hold on但又每次plot内存很快会被图形对象拖垮。症状常见原因解决办法动画后期越来越慢每次循环增加新line对象用set更新现有句柄角度曲线跳变输出步长过大Tspan步长改小到0.005摆杆端点不在轨道上忘了把theta2作为相对角上摆末端x2要加th1th2动画一帧一帧卡电脑分辨率太高figure缩小或用drawnow limitrate这个表格是我在调试多摆动画时最常用的排查清单。其中第二项和第三项是新手最容易踩的坑theta2是相对下摆的角度不能直接当成绝对角来画上摆。5. 生成仿真操作录像的录制技巧5.1 VideoWriter录制figure窗口把上面的动画录成视频最不需要额外工具的方式是MATLAB自带的VideoWriter。在动画循环开始前创建VideoWriter对象每个循环末尾用getframe抓取当前figure窗口然后写进视频文件v VideoWriter(double_inverted_pendulum.avi); v.FrameRate 30; open(v); for i 1:length(t) % ... 更新动画 ... writeVideo(v, getframe(gcf)); end close(v);这里FrameRate设30表示视频每秒30帧但仿真每10毫秒输出一个点10秒就有1000帧视频时长变为1000/30约33秒。如果希望视频与仿真时间一致应该把FrameRate设为1001/0.01。通常我更喜欢30帧率因为视觉更平滑缺点是不再严格对应仿真时间这条需要根据用途选。5.2 录像的控制与后处理VideoWriter支持MPEG-4格式把文件名后缀换成.mp4并指定MPEG-4即可v VideoWriter(invp.mp4, MPEG-4); v.Quality 90; % 数值0-100越小体积越小Quality在90时清晰度足够体积比avi小一半。录制过程中不要让其他窗口盖住figuregetframe抓的是屏幕上的渲染结果一旦遮住就会录成空白。如果嫌手动操作录像重复可以把整个录制脚本封装成一个函数输入仿真参数输出视频文件这样后续改参数时重新跑一次就行。5.3 增加录制动感的时间戳技巧一个小技巧在figure右上角放一个text对象每个循环用sprintf显示当前仿真时间这样录出来的视频能直观看到“实时”过程。在角度曲线子图里还可以用scatter高亮当前时刻的点让观看者注意力落在变化趋势上。这个普通set更新即可不会对性能造成多少影响。最后如果整个仿真循环里同时做了动画更新和writeVideo建议先将仿真提前算好不要在录制过程中做ode45积分。离线计算后录制循环里只做插值取数据视频即使中途卡顿也不会导致数据错位。本文还有配套的精品资源点击获取