六自由度机械臂mpc模型预测控制+倒立摆+二自由度机械臂

一、整体功能概述

本套MATLAB代码实现了二自由度机械臂的模型预测控制（MPC） ，核心目标是通过实时非线性模型线性化与滚动时域优化，使机械臂关节角度精准跟踪期望轨迹，并输出稳定的控制力矩。代码涵盖机械臂动力学建模、正运动学计算、轨迹生成、MPC控制器设计及仿真结果可视化全流程，适用于二自由度机械臂的运动控制算法验证与性能分析。

二、核心文件功能拆解

1. 动力学模型文件（RobDyn.m）

功能定位

基于拉格朗日方程，计算机械臂的状态导数（角速度与角加速度），是机械臂运动仿真的核心动力学模块。

输入参数

• tao：2×1向量，机械臂两个关节的控制力矩（输入控制量）；
• X：4×1向量，机械臂当前状态，包含[q1, q2, dq1, dq2]（关节1角度、关节2角度、关节1角速度、关节2角速度）；
• ROB：结构体，存储机械臂物理参数（质量、长度、转动惯量等）。

关键计算步骤

1. 提取物理参数：从ROB中读取关节质量（m1/m2）、连杆长度（l1/l2）、质心距离（r1/r2）、转动惯量（I1/I2）及重力加速度（g0=9.81）；
2. 构建核心矩阵：
   - M：惯性矩阵，反映机械臂运动惯性与关节间耦合关系，含cos(q2)项（体现关节2角度对惯性的影响）；
   - C：科里奥利-离心力矩阵，由Sq（耦合项矩阵）与角速度向量乘积得到，反映运动中的非线性离心力与科里奥利力；
   - G：重力矩阵，含cos(q1)与cos(q1+q2)项，反映重力对关节力矩的影响；
3. 计算角加速度：通过惯性矩阵的伪逆（pinv(M)）求解动力学方程M·ddq + C·dq + G = tao，得到角加速度ddq；
4. 输出状态导数：返回dX = [dq1; dq2; ddq1; ddq2]，为龙格-库塔法积分提供状态更新依据。

2. MPC控制器文件（RobDynMPC.m）

功能定位

实现模型预测控制的核心逻辑，包括线性化建模、优化目标构建、约束处理及控制力矩求解，是机械臂精准跟踪的核心控制模块。

输入参数

• Q/R：4×4/2×2对角矩阵，状态量/控制量权重（调节跟踪精度与控制能耗的平衡）；
• x_k：4×1向量，当前时刻机械臂状态；
• xd：4×1向量，期望状态（角度与角速度）；
• N：整数，MPC控制时域（预测未来N个时刻的状态与控制量）；
• step：控制周期（与ROB.dt一致）；
• tao：上一时刻控制力矩（用于迭代更新）；
• ROB：机械臂物理参数与约束（力矩上下限lb/ub）。

关键子函数与流程

1. getmcg子函数：
   - 功能：计算当前状态下的惯性矩阵M、科里奥利-离心力矩阵C、重力矩阵G（与RobDyn.m逻辑一致，为线性化提供基础）；
2. 线性化建模：
   - 构建连续时间状态空间模型dx = Ac·x + Bc·(tao - G)，其中Ac = [0 0 I; 0 -M⁻¹C]，Bc = [0; M⁻¹]；
   - 离散化：通过零阶保持（ZOH）将连续模型离散为x(k+1) = A·x(k) + B·u(k)，其中A = I + step·Ac，B = step·Bc；
3. 轨迹生成与优化目标构建：
   - 调用getTraj生成未来N个时刻的期望状态序列qrArr（线性插值当前状态与期望状态）；
   - 构建二次规划（QP）目标函数：J = (X - Xref)ᵀQbar(X - Xref) + UᵀRbarU，其中Qbar/Rbar为N+1/N维块对角权重矩阵（状态终端权重F=Q）；
   - 将目标函数转化为QP标准形式J = 0.5·taoallᵀH·taoall + taouᵀ·taoall，其中H = CᵀQbarC + Rbar，tao_u为线性项；
4. 约束处理与求解：
   - 设置控制力矩约束lb ≤ taoall ≤ ub（每个时刻力矩均需在ROB.lb与ROB.ub之间）；
   - 调用quadprog求解QP问题，得到未来N个时刻的控制力矩序列taoall；
   - 输出当前时刻控制力矩taou = taoall(1:2) + G（补偿重力项，确保控制精度）。

3. 正运动学文件（fkRob.m）

功能定位

计算机械臂连杆端点（含基座、关节1、关节2）的笛卡尔坐标，用于末端位置可视化与轨迹跟踪验证。

输入参数

• ROB：机械臂连杆长度l1/l2；
• q：2×1向量，关节角度[q1, q2]；

输出

• P：3×2矩阵，每行分别为基座（0,0）、关节1端点（l1cosq1, l1sinq1）、关节2端点（l1cosq1+l2cos(q1+q2), l1sinq1+l2sin(q1+q2)）的坐标。

4. 轨迹生成文件（getTraj.m）

功能定位

为MPC提供未来控制时域内的期望状态参考轨迹，采用线性插值确保轨迹平滑性。

输入参数

• x：当前状态（4×1）；
• x_des：期望目标状态（4×1）；
• N：控制时域长度；

输出

• x_ref：4×N矩阵，每行对应一个状态量（q1/q2/dq1/dq2），每列对应未来一个时刻的期望状态（线性过渡）。

5. 主程序文件（main.m）

功能定位

整合所有模块，实现机械臂MPC控制的全流程仿真，包括参数配置、循环控制、状态更新、结果可视化。

关键步骤

1. 参数配置：
   - 设置机械臂物理参数（m1=m2=1kg，l1=l2=1m，I1=I2=10kg·m²等）；
   - 控制参数：控制周期dt=0.01s，总仿真时间T=7s，MPC控制时域p_t=20；
   - 权重矩阵Q（状态权重，角度项500×900，角速度项2e-2×900）与R（控制量权重1×1e-1）；
2. 初始化：
   - 状态数组q（4×N，存储所有时刻状态）、控制量数组u（2×N，存储所有时刻力矩）；
   - 初始状态q0=[0,0,0,0]（关节角度与角速度均为0），期望状态qd=[pi0.5, pi0.8, 0, 0]（关节1期望角度π/2，关节2期望角度0.8π，角速度为0）；
3. 循环控制（核心迭代）：
   - 每个时刻调用RobDynMPC求解当前控制力矩u(:,i)；
   - 采用四阶龙格-库塔法（RK4）更新状态：q(:,i+1) = q(:,i) + dt/6·(rk1 + 2rk2 + 2rk3 + rk4)，其中rk1~rk4为RK4的四步导数计算；
4. 结果可视化：
   - 状态跟踪图（图1）：分上下子图显示关节1/2的实际角度与期望角度对比；
   - 控制力矩图（图2）：显示两个关节的实时控制力矩；
   - MPC滚动优化力矩图：显示未来N个时刻的优化力矩序列；
   - 机械臂运动动画：实时绘制连杆端点轨迹，标记当前末端位置（黑色）与期望末端位置（红色），直观观察跟踪效果。

6. 符号计算文件（testsym.m）

功能定位

基于符号计算推导机械臂动力学模型的线性化矩阵（A/B矩阵），为控制器设计提供理论验证（代码中默认注释，可用于算法优化）。

关键功能

1. 定义符号变量（关节角度、角速度、力矩、物理参数等）；
2. 推导动力学方程F = [dq1; dq2; M⁻¹(tao - G - C)]；
3. 计算状态矩阵A = ∂F/∂Z（Z为状态向量）与输入矩阵B = ∂F/∂tao；
4. 生成MATLAB函数文件getA.m与getB.m，用于后续线性化模型调用。

三、核心算法原理

1. 二自由度机械臂动力学

基于拉格朗日方程L = T - V（动能T减势能V），推导得到非线性动力学方程：

六自由度机械臂mpc模型预测控制+倒立摆+二自由度机械臂

M(q)·ddq + C(q, dq)·dq + G(q) = tao

• M(q)：惯性矩阵（对称正定），体现关节位置对惯性的影响；
• C(q, dq)：科里奥利-离心力矩阵，体现角速度与位置的耦合；
• G(q)：重力矩阵，体现重力场对关节的力矩作用；
• tao：关节控制力矩（输入）。

2. 模型预测控制（MPC）

核心思想

通过“预测-优化-滚动实施”实现非线性系统的稳定控制：

1. 预测：基于当前状态与线性化模型，预测未来N个时刻的系统状态；
2. 优化：在控制时域内，最小化“状态跟踪误差”与“控制量能耗”的二次目标函数，同时满足控制量约束；
3. 滚动实施：仅执行优化得到的第一个时刻控制量，下一时刻重复上述步骤（基于新状态更新模型）。

优势

• 可直接处理控制量约束（如力矩上限），避免机械硬件过载；
• 通过实时线性化适应非线性动力学，兼顾控制精度与计算效率；
• 滚动时域优化可抑制扰动，提升系统鲁棒性。

四、仿真结果说明

1. 角度跟踪曲线（图2）

• 横轴：仿真时间（0~7s）；
• 纵轴：关节角度（弧度）；
• 曲线含义：红色虚线为实际角度（q1/q2），蓝色实线为期望角度（dirq1/dirq2）；
• 预期效果：实际角度应快速收敛至期望角度，稳态误差接近0（需通过调整Q/R权重优化收敛速度与超调）。

2. 控制力矩曲线（图3/4）

• 横轴：仿真时间（0~8s）；
• 纵轴：控制力矩（N·m）；
• 曲线含义：两条曲线分别对应关节1（tao1）与关节2（tao2）的控制力矩；
• 关键特征：初始时刻力矩较大（用于克服惯性与重力，驱动关节快速响应），稳态时力矩趋于稳定（仅补偿重力与微小扰动），且力矩值始终在[-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中的仿真状态更新替换为硬件传感器数据（如编码器角度），控制力矩输出至电机驱动器，实现实际机械臂的控制。