倒立摆系统MPC控制MATLAB代码功能说明
六自由度机械臂mpc模型预测控制倒立摆二自由度机械臂该MATLAB代码包实现了小车-单摆倒立摆系统的模型预测控制MPC通过动力学建模、实时线性化可选、MPC优化计算及数值积分实现倒立摆从初始偏移状态到垂直稳定状态的闭环控制并提供状态可视化与动态仿真。一、代码文件结构与核心功能代码包包含5个.m文件各文件功能分工明确形成“参数定义→动力学建模→MPC优化→仿真运行→可视化”的完整控制流程具体如下表所示文件名函数类型核心功能输入输出getParm.m参数获取函数定义倒立摆系统的物理参数确保所有模块参数一致性输出小车质量mc、摆杆质量mp、摆长L、重力加速度gRobDyn.m动力学模型函数建立系统非线性状态空间模型计算状态导数加速度输入控制力f、当前状态X[x, q, dx, dq]小车位置/摆角/小车速度/摆角速度输出状态导数dX[dx, dq, ddx, ddq]小车加速度/摆角加速度getTraj.m参考轨迹生成函数生成从“当前状态”到“期望状态”的线性过渡参考轨迹输入当前状态x、期望状态xdes、时域步数h输出4×h参考轨迹矩阵xrefRobMPC.mMPC控制器核心函数基于线性化模型构建二次规划QP问题求解最优控制序列输入状态权重Q/终端权重F/控制权重R、当前状态x_k、期望状态xd、预测时域N、控制周期dt、前一控制量ff、线性化开关flagRealtimeLinearization输出当前时刻最优控制力fmain.m主程序脚本初始化参数、运行闭环控制仿真、绘制状态曲线与动态动画无显式输入输出通过修改内部参数如初始状态、权重配置仿真二、核心模块详细说明1. 系统物理模型与动力学getParm.m RobDyn.m1物理参数定义getParm.m固定系统关键参数默认值为小车质量mc 1 kg摆杆质量mp 1 kg摆杆长度L 1 m重力加速度g 9.81 m/s²2非线性动力学建模RobDyn.m基于拉格朗日方程建立小车-单摆系统的动力学模型核心公式遵循“惯性矩阵×加速度 科里奥利/离心力 控制力 重力矩”具体实现惯性矩阵M描述系统质量分布对加速度的影响与摆角q相关含cos(q)项科里奥利/离心力矩阵C描述速度耦合效应含sin(q)*dq项重力矩向量tau仅作用于摆杆含mpgL*sin(q)项控制矩阵B控制力f仅作用于小车B[1; 0]加速度计算通过矩阵求逆M\(...)解算小车加速度ddx和摆角加速度ddq最终输出状态导数dX。2. 模型预测控制MPC核心RobMPC.mMPC的核心是“在线求解有限时域内的二次规划问题仅执行第一个控制量”该文件分3个关键步骤实现1系统线性化可选实时线性化MPC需基于线性模型构建优化问题提供两种线性化方式默认线性化flagRealtimeLinearization0使用平衡位置摆角q0、角速度dq0的近似线性化模型矩阵Ac状态矩阵和Bc输入矩阵为固定值实时线性化flagRealtimeLinearization1调用getAB()函数基于当前状态q, dq和前一控制量f进行线性化更适用于大偏移量场景线性化结果包含sin(q)、cos(q)、dq²等与当前状态相关的项。2离散化处理将连续时间线性模型Ac, Bc通过零阶保持ZOH离散化为离散时间模型A, B离散化公式状态矩阵A Ac*dt eye(4)eye(4)为4阶单位矩阵输入矩阵B Bc*dtdt为控制周期。3MPC优化问题构建与求解参考轨迹生成调用getTraj()生成“当前状态→期望状态”的N步线性参考轨迹Xref并拼接为向量Xref_vec权重矩阵构建-Qbar(N1)×4维状态权重矩阵前N步用Q终端步用F强调终端状态跟踪精度-RbarN×1维控制权重矩阵每步用R抑制过大控制力二次规划QP问题- 目标函数min (1/2)u^T H u f^T u其中HC^T Qbar C Rbar Hessian矩阵f(xk^T M^T - Xrefvec^T)Qbar C线性项- 约束控制力范围[-30, 30] N通过lb/ub设置求解与输出调用quadprog()求解QP问题输出第一个控制量MPC滚动时域特性。3. 仿真与可视化main.m主程序是代码的“入口”实现闭环控制仿真与结果展示流程如下1仿真参数初始化控制周期dt 0.04 s25 Hz控制频率总仿真时间Tsum 12 s总循环次数N Tsum/dt 300预测时域p_h 20每次优化未来20步控制量初始状态q0 [0 m, π/6 rad, 0 m/s, 0 rad/s]小车初始在原点摆杆初始偏移30°初速度为0期望状态qd [0, 0, 0, 0]小车停在原点摆杆垂直稳定权重矩阵Qdiag([1000,1000,60,20])重点跟踪小车位置x和摆角qR1e-4弱抑制控制力。2闭环控制仿真通过for循环实现每步控制逻辑调用RobMPC()计算当前最优控制力u(i)调用RobDyn()结合四阶龙格-库塔RK4积分更新系统状态RK4确保积分精度适合非线性系统记录时间t、状态q、控制量u。3结果可视化生成3类图形直观展示控制效果图1状态曲线绘制小车位置x和摆角q随时间的变化验证是否收敛到期望状态图2控制量曲线绘制控制力u随时间的变化验证控制量是否在约束范围内且平滑图3动态动画实时绘制小车蓝色矩形、摆杆黑色线段、摆球红色圆点的运动直观观察倒立摆的稳定过程pause(0.02)控制动画速度drawnow强制刷新图像。三、代码核心特性与适用场景1. 核心特性非线性建模基于拉格朗日方程的精确动力学模型而非简化模型灵活线性化支持“平衡位置线性化”和“实时线性化”适配不同初始偏移场景高精度仿真RK4积分保证状态更新精度MPC滚动时域确保控制鲁棒性直观可视化动态动画静态曲线便于验证控制效果。2. 适用场景倒立摆系统的稳定控制研究教学/科研常用案例MPC算法在非线性系统中的应用验证线性化方法平衡位置/实时对控制性能的影响对比。四、使用建议参数调整- 若摆杆稳定速度慢增大Q中摆角q的权重如从1000改为2000- 若控制力波动大增大R如从1e-4改为1e-3- 若控制延迟大减小预测时域p_h如从20改为15功能扩展- 增加扰动如小车受力干扰验证MPC的抗扰能力- 修改期望状态qd如x2 m实现小车轨迹跟踪摆杆稳定注意事项-quadprog()需MATLAB Optimization Toolbox支持- 若实时线性化时出现数值不稳定可减小初始偏移角度如从π/6改为π/12。六自由度机械臂mpc模型预测控制倒立摆二自由度机械臂
)
