二自由度机械臂MPC控制MATLAB代码功能说明

六自由度机械臂mpc模型预测控制倒立摆二自由度机械臂一、整体功能概述本套MATLAB代码实现了二自由度机械臂的模型预测控制MPC核心目标是通过实时非线性模型线性化与滚动时域优化使机械臂关节角度精准跟踪期望轨迹并输出稳定的控制力矩。代码涵盖机械臂动力学建模、正运动学计算、轨迹生成、MPC控制器设计及仿真结果可视化全流程适用于二自由度机械臂的运动控制算法验证与性能分析。二、核心文件功能拆解1. 动力学模型文件RobDyn.m功能定位基于拉格朗日方程计算机械臂的状态导数角速度与角加速度是机械臂运动仿真的核心动力学模块。输入参数tao2×1向量机械臂两个关节的控制力矩输入控制量X4×1向量机械臂当前状态包含[q1, q2, dq1, dq2]关节1角度、关节2角度、关节1角速度、关节2角速度ROB结构体存储机械臂物理参数质量、长度、转动惯量等。关键计算步骤提取物理参数从ROB中读取关节质量m1/m2、连杆长度l1/l2、质心距离r1/r2、转动惯量I1/I2及重力加速度g09.81构建核心矩阵-M惯性矩阵反映机械臂运动惯性与关节间耦合关系含cos(q2)项体现关节2角度对惯性的影响-C科里奥利-离心力矩阵由Sq耦合项矩阵与角速度向量乘积得到反映运动中的非线性离心力与科里奥利力-G重力矩阵含cos(q1)与cos(q1q2)项反映重力对关节力矩的影响计算角加速度通过惯性矩阵的伪逆pinv(M)求解动力学方程M·ddq C·dq G tao得到角加速度ddq输出状态导数返回dX [dq1; dq2; ddq1; ddq2]为龙格-库塔法积分提供状态更新依据。2. MPC控制器文件RobDynMPC.m功能定位实现模型预测控制的核心逻辑包括线性化建模、优化目标构建、约束处理及控制力矩求解是机械臂精准跟踪的核心控制模块。输入参数Q/R4×4/2×2对角矩阵状态量/控制量权重调节跟踪精度与控制能耗的平衡x_k4×1向量当前时刻机械臂状态xd4×1向量期望状态角度与角速度N整数MPC控制时域预测未来N个时刻的状态与控制量step控制周期与ROB.dt一致tao上一时刻控制力矩用于迭代更新ROB机械臂物理参数与约束力矩上下限lb/ub。关键子函数与流程getmcg子函数- 功能计算当前状态下的惯性矩阵M、科里奥利-离心力矩阵C、重力矩阵G与RobDyn.m逻辑一致为线性化提供基础线性化建模- 构建连续时间状态空间模型dx Ac·x Bc·(tao - G)其中Ac [0 0 I; 0 -M⁻¹C]Bc [0; M⁻¹]- 离散化通过零阶保持ZOH将连续模型离散为x(k1) A·x(k) B·u(k)其中A I step·AcB step·Bc轨迹生成与优化目标构建- 调用getTraj生成未来N个时刻的期望状态序列qrArr线性插值当前状态与期望状态- 构建二次规划QP目标函数J (X - Xref)ᵀQbar(X - Xref) UᵀRbarU其中Qbar/Rbar为N1/N维块对角权重矩阵状态终端权重FQ- 将目标函数转化为QP标准形式J 0.5·taoallᵀH·taoall taouᵀ·taoall其中H CᵀQbarC Rbartao_u为线性项约束处理与求解- 设置控制力矩约束lb ≤ taoall ≤ ub每个时刻力矩均需在ROB.lb与ROB.ub之间- 调用quadprog求解QP问题得到未来N个时刻的控制力矩序列taoall- 输出当前时刻控制力矩taou taoall(1:2) G补偿重力项确保控制精度。3. 正运动学文件fkRob.m功能定位计算机械臂连杆端点含基座、关节1、关节2的笛卡尔坐标用于末端位置可视化与轨迹跟踪验证。输入参数ROB机械臂连杆长度l1/l2q2×1向量关节角度[q1, q2]输出P3×2矩阵每行分别为基座0,0、关节1端点l1cosq1, l1sinq1、关节2端点l1cosq1l2cos(q1q2), l1sinq1l2sin(q1q2)的坐标。4. 轨迹生成文件getTraj.m功能定位为MPC提供未来控制时域内的期望状态参考轨迹采用线性插值确保轨迹平滑性。输入参数x当前状态4×1x_des期望目标状态4×1N控制时域长度输出x_ref4×N矩阵每行对应一个状态量q1/q2/dq1/dq2每列对应未来一个时刻的期望状态线性过渡。5. 主程序文件main.m功能定位整合所有模块实现机械臂MPC控制的全流程仿真包括参数配置、循环控制、状态更新、结果可视化。关键步骤参数配置- 设置机械臂物理参数m1m21kgl1l21mI1I210kg·m²等- 控制参数控制周期dt0.01s总仿真时间T7sMPC控制时域p_t20- 权重矩阵Q状态权重角度项500×900角速度项2e-2×900与R控制量权重1×1e-1初始化- 状态数组q4×N存储所有时刻状态、控制量数组u2×N存储所有时刻力矩- 初始状态q0[0,0,0,0]关节角度与角速度均为0期望状态qd[pi0.5, pi0.8, 0, 0]关节1期望角度π/2关节2期望角度0.8π角速度为0循环控制核心迭代- 每个时刻调用RobDynMPC求解当前控制力矩u(:,i)- 采用四阶龙格-库塔法RK4更新状态q(:,i1) q(:,i) dt/6·(rk1 2rk2 2rk3 rk4)其中rk1~rk4为RK4的四步导数计算结果可视化-状态跟踪图图1分上下子图显示关节1/2的实际角度与期望角度对比-控制力矩图图2显示两个关节的实时控制力矩-MPC滚动优化力矩图显示未来N个时刻的优化力矩序列-机械臂运动动画实时绘制连杆端点轨迹标记当前末端位置黑色与期望末端位置红色直观观察跟踪效果。6. 符号计算文件testsym.m功能定位基于符号计算推导机械臂动力学模型的线性化矩阵A/B矩阵为控制器设计提供理论验证代码中默认注释可用于算法优化。关键功能定义符号变量关节角度、角速度、力矩、物理参数等推导动力学方程F [dq1; dq2; M⁻¹(tao - G - C)]计算状态矩阵A ∂F/∂ZZ为状态向量与输入矩阵B ∂F/∂tao生成MATLAB函数文件getA.m与getB.m用于后续线性化模型调用。三、核心算法原理1. 二自由度机械臂动力学基于拉格朗日方程L T - V动能T减势能V推导得到非线性动力学方程六自由度机械臂mpc模型预测控制倒立摆二自由度机械臂M(q)·ddq C(q, dq)·dq G(q) taoM(q)惯性矩阵对称正定体现关节位置对惯性的影响C(q, dq)科里奥利-离心力矩阵体现角速度与位置的耦合G(q)重力矩阵体现重力场对关节的力矩作用tao关节控制力矩输入。2. 模型预测控制MPC核心思想通过“预测-优化-滚动实施”实现非线性系统的稳定控制预测基于当前状态与线性化模型预测未来N个时刻的系统状态优化在控制时域内最小化“状态跟踪误差”与“控制量能耗”的二次目标函数同时满足控制量约束滚动实施仅执行优化得到的第一个时刻控制量下一时刻重复上述步骤基于新状态更新模型。优势可直接处理控制量约束如力矩上限避免机械硬件过载通过实时线性化适应非线性动力学兼顾控制精度与计算效率滚动时域优化可抑制扰动提升系统鲁棒性。四、仿真结果说明1. 角度跟踪曲线图2横轴仿真时间0~7s纵轴关节角度弧度曲线含义红色虚线为实际角度q1/q2蓝色实线为期望角度dirq1/dirq2预期效果实际角度应快速收敛至期望角度稳态误差接近0需通过调整Q/R权重优化收敛速度与超调。2. 控制力矩曲线图3/4横轴仿真时间0~8s纵轴控制力矩N·m曲线含义两条曲线分别对应关节1tao1与关节2tao2的控制力矩关键特征初始时刻力矩较大用于克服惯性与重力驱动关节快速响应稳态时力矩趋于稳定仅补偿重力与微小扰动且力矩值始终在[-100, 100]约束范围内。五、参数调整与使用建议1. 权重矩阵Q/R调整Q矩阵增大角度项权重如Q(1,1)/Q(2,2)可提升角度跟踪精度但可能导致控制力矩增大增大角速度项权重Q(3,3)/Q(4,4)可抑制角速度超调但可能降低响应速度R矩阵增大R矩阵权重可减小控制力矩降低能耗但可能牺牲跟踪精度需在“精度”与“能耗”间平衡。2. 控制时域N调整增大N可提升预测精度与系统鲁棒性但会增加QP求解计算量可能导致实时性下降减小N计算效率提升但预测范围变小可能导致跟踪误差增大建议取值范围5~30需结合控制周期dt选择。3. 机械臂物理参数ROB调整若需适配实际机械臂需将ROB结构体中的m1/m2质量、l1/l2长度、I1/I2转动惯量等参数替换为实际测量值确保动力学模型准确性。4. 约束lb/ub调整根据机械臂电机最大输出力矩设置ROB.lb与ROB.ub避免力矩超限导致电机损坏建议预留10%~20%安全余量。六、适用场景与扩展方向1. 适用场景二自由度旋转关节机械臂的位置控制如桌面机械臂、教学实验平台MPC控制算法在非线性系统中的验证与教学演示机械臂运动控制的仿真预研避免直接在硬件上调试的风险。2. 扩展方向多自由度扩展参考testsym.m中的6自由度符号计算逻辑将代码扩展至3/6自由度机械臂轨迹类型扩展修改main.m中的qd生成逻辑支持正弦轨迹、圆弧轨迹等复杂轨迹跟踪扰动与鲁棒性优化在动力学模型中加入扰动项如摩擦力、负载变化并通过增大Q矩阵权重或采用鲁棒MPC提升抗扰能力硬件联调将main.m中的仿真状态更新替换为硬件传感器数据如编码器角度控制力矩输出至电机驱动器实现实际机械臂的控制。