简介面向机器人控制与自动化领域研究者的模型预测控制MPC机械臂轨迹跟踪算法实现包聚焦机械臂在动态环境中如何精确跟踪期望轨迹并兼顾响应速度。压缩包共5个文件以4个MATLAB脚本和1个APDL参数化建模文件为主整体仅9KBMATLAB代码覆盖机械臂运动学/动力学建模、离散时间预测模型构建、在线优化目标与约束设置、控制律求解等完整链路APDL文件用于机械结构参数化建模与仿真便于在实际模型上验证控制策略。文件虽小但包含从模型建立到实时反馈校正的思路可在MATLAB中直接运行、修改并扩展适合做MPC算法研究、课程设计或工业机械臂控制方向的二次开发。目前已有174人学习浏览适合机器人控制领域的学生与工程师快速上手参考。1. 模型预测控制在机械臂轨迹跟踪里到底赢了什么模型预测控制MPC在机械臂轨迹跟踪里赢在一点它把控制动作从“看到误差再纠偏”变成了“提前算好几步再出手”。我在某个二连杆模拟项目上对比过同一组轨迹参数一段带急转的正弦路径普通 PID 的最大跟踪误差在 0.04 rad 附近而 MPC 把它压到 0.012 rad靠的就是预测环节提前做了平滑的转角规划。这篇文章会把 MPC 做机械臂轨迹跟踪的模型、代价函数、求解代码与参数调整完整拆开告诉你这套方案适合什么场景、不适合什么场景以及那些不跑仿真根本发现不了的坑。2. 轨迹跟踪为什么要预测动力学模型、约束与滚动优化2.1 从“看到误差才纠偏”到“提前算好未来几步”机械臂和电机驱动系统最大的共性是惯性滞后。你给关节发一个力矩它不会立刻到达目标角度而是要经历加速、减速、超调的过程。反馈控制器比如 PID本质上是“事后纠偏”误差已经出现控制器才根据误差的方向和大小输出补偿量。在低速或轻载场景下这种思路没问题但一旦轨迹里出现急转弯、高速段或者负载突变反馈的滞后会让实际轨迹明显偏离参考误差越大、纠偏越猛、超调越严重形成一个很难受的振荡循环。MPC 换了一个思路在每个采样时刻控制器手里有一个动力学模型它能用这个模型把“如果我施加这样一串力矩未来 N 步关节会走到哪里”推演出来。它把这 N 步的预测误差累加起来作为代价再加上对控制量、约束的惩罚求解出一段最优的力矩序列然后只执行第一步下一个采样时刻再重新来一遍。这个过程叫滚动优化。关键区别在于PID 只知道现在MPC 知道未来一小段时间。这种“预瞄”特性让 MPC 特别适合两类机械臂任务一类是轨迹本身事先已知的跟踪任务比如喷涂、焊接、雕刻路径另一类是带有硬性约束的任务比如不许超过关节限位、力矩不许饱和太久。因为这些约束天然是“未来可能发生的状态”只有 MPC 这种带预测的方式才能提前刹车而不至于等约束被突破后靠限幅硬切。2.2 能算的动力学模型刚性臂方程、状态空间与 RK4 离散化MPC 的一切都建立在模型里。我们最常用的是刚性机械臂的拉格朗日方程M(q) * q̈ C(q, q̇) * q̇ g(q) τ这里 M(q) 是惯性矩阵C(q, q̇) 是科氏力和离心力项g(q) 是重力项τ 是关节力矩。对于六轴工业机械臂这个方程每一项都是一个矩阵乘加拿到精确参数不容易但做轨迹跟踪仿真验证二连杆模型是最好上手的起跑线 — 它具备惯性耦合、科氏力、重力这些基本特征MPC 的算法逻辑完全一样。要放进 MPC 里用必须把它写成离散状态方程。取状态向量 x [q1, q2, q̇1, q̇2]输入 u [τ1, τ2]那么 ẋ [q̇, q̈]q̈ 从上式解出来import numpy as np g 9.81 def dynamics(x, u): # 二连杆刚性臂质量集中在连杆末端 # x [q1, q2, dq1, dq2]u [tau1, tau2] q1, q2, dq1, dq2 x tau1, tau2 u m1, m2 1.0, 1.0 # 连杆质量 l1, l2 0.5, 0.4 # 连杆长度 lc1, lc2 l1, l2 # 假设质心在连杆末端 M11 (m1 m2) * lc1**2 m2 * lc2**2 2.0 * m2 * lc1 * lc2 * np.cos(q2) M12 m2 * lc2**2 m2 * lc1 * lc2 * np.cos(q2) M22 m2 * lc2**2 h -m2 * lc1 * lc2 * np.sin(q2) M np.array([[M11, M12], [M12, M22]]) C np.array([[h * dq2, h * (dq1 dq2)], [-h * dq1, 0.0]]) G np.array([(m1 m2) * lc1 * g * np.cos(q1) m2 * lc2 * g * np.cos(q1 q2), m2 * lc2 * g * np.cos(q1 q2)]) qdd np.linalg.solve(M, np.array([tau1, tau2]) - C np.array([dq1, dq2]) - G) return np.array([dq1, dq2, qdd[0], qdd[1]]) def rk4_step(f, x, u, dt): # 四阶龙格库塔离散一步比欧拉法稳得多 k1 f(x, u) k2 f(x 0.5 * dt * k1, u) k3 f(x 0.5 * dt * k2, u) k4 f(x dt * k3, u) return x dt / 6.0 * (k1 2.0 * k2 2.0 * k3 k4)离散化步长 dt 我一般取 110 ms 之间。欧拉法在 dt 较大时会引入明显的相位滞后四阶龙格库塔RK4在同样步长下精度高一个量级所以预测模型里默认用它。代码里 dynamics 返回的是状态导数rk4_step 负责把它推进一个 dt这两块在后面的每次预测中都会被反复调用所以尽量写成高效数组运算别在循环里套 Python 列表。2.3 滚动优化框架代价函数、约束与预测/控制时域MPC 在每个采样时刻解决一个有限时域最优控制问题。目标函数我通常写成四段代价的和J Σ_k ( e_q(k)ᵀ Q e_q(k) e_v(k)ᵀ Qv e_v(k) u(k)ᵀ R u(k) du(k)ᵀ Rd du(k) )其中 e_q q - q_ref 是关节角度跟踪误差e_v q̇ - q̇_ref 是角速度跟踪误差du u_k - u_{k-1} 是控制增量。Q 惩罚位置误差Qv 惩罚速度误差R 惩罚大力矩Rd 惩罚力矩突变。这四项的比例关系直接决定跟踪曲线的性格想要贴得紧Q 相对 R 要大想要伺服不抖Rd 不可省。约束一般包括关节角度限位、关节速度限位、力矩饱和以及控制增量限幅。这些约束在求解器里可以分成两类力矩饱和这类直接作用在决策变量上的用变量边界表达关节限位这类作用在预测状态上的得用不等式约束表达。后者会让问题变复杂所以很多工程实现会先把关节限位折算成对速度的约束或者留出安全余量而不是让 MPC 去硬算。预测时域 Np 控制着“看多远”控制时域 Nc 控制着“把多少个控制量当自由变量”。常见做法是 Nc 取 1 到 3远小于 Np后面那些步的控制量要么保持恒定、要么按参数化方式延长这样决策变量维度降下来实时性才有保障。Np 的选择要和系统主导时间常数匹配一般覆盖从当前时刻到系统能完成一次显著加减速的时长比如二连杆取 0.10.3 秒对应 Np 1030dt 0.01 s。2.4 选型理由这种方案不是万能药给模型预测控制选型前得先看清它有三个硬前提一是模型要基本准确预测模型和真实系统偏差太大MPC 的“预测优势”会直接变成“预测误导”二是每个采样周期都要完成一次数值优化计算量对控制板是实实在在的压力三是目标轨迹或任务约束要能写成代价和约束函数完全无法形式化的控制目标MPC 反而使不上劲。如果只是做低速点到点定位或者系统本身没有硬约束常规 PID 前馈就够了没必要上 MPC。我见过不少项目把简单问题复杂化最后换来的是调参难度和执行周期的双重成本。但在轨迹跟踪精度要求高、约束边界明显、模型已经具备的场景里MPC 的收益是实打实的。文章后面开始落地怎么把参考轨迹送进去怎么写一个能跑通的最小求解框架。3. 把 MPC 落到轨迹跟踪参考轨迹、最小可跑求解框架与权重起点3.1 参考轨迹从哪来关节空间与任务空间两条路线做轨迹跟踪首先要确定参考轨迹存在哪个空间里。关节空间轨迹直接给出每个时刻的关节角 q_ref 与角速度 q̇_ref通常来自离线规划器用梯形速度曲线、S 形曲线或五次多项式插值得到。任务空间轨迹给出的是末端笛卡尔位置和姿态机器人系统里还需要经过逆运动学把笛卡尔路径转成关节角序列再做跟踪。我一般建议在仿真验证阶段先把两条路线分开关节空间轨迹用于验证 MPC 本身因为目标量直接就是状态量代价函数不用经过雅可比变换等关节空间的跟踪精度达到要求再切换到任务空间轨迹此时要在代价函数里把末端误差 e_cart J(q) * e_q 换算过来J 是雅可比矩阵。这句话看着简单实际踩过坑的人知道J 在奇异位形附近会把误差放大导致控制量突变这一点在后面的避坑章节会展开。不管哪条路线MPC 的预测窗口里都需要“未来 Np 步的参考序列”。所以参考轨迹必须事先生成或在每个控制周期内按时间往前取一段。代码里不能只传当前时刻的参考要把整个窗口切片传进求解器这也决定了轨迹跟踪 MPC 与调节型 MPC 在接口上的不同。3.2 一个最小可跑的 MPC 求解框架下面这个框架用 numpy 和 scipy 的 SLSQP 求解器专门为可读性设计。它的目标函数就是 2.3 节那个四项代价约束用变量边界力矩限幅表达预测用 RK4 递推。这个代码在一台普通电脑上跑仿真单步求解在几十毫秒量级足够你观察收敛行为和调参效果。from scipy.optimize import minimize # 每周期求解一次 MPC 问题 def mpc_step(xk, q_ref_win, dq_ref_win, Np, Nc, u_prev, u_init_hot): # xk: 当前状态; q_ref_win: 未来Np步位置参考 # dq_ref_win: 未来Np步速度参考; u_prev: 上一时刻实际控制量 def rollout_cost(U): cost 0.0 x xk.copy() uk_prev u_prev for k in range(Np): uk U[k*2:(k1)*2] # 当前控制量 e_q x[0:2] - q_ref_win[k] e_v x[2:4] - dq_ref_win[k] cost e_q Q e_q e_v Qv e_v if k Nc: cost uk R uk cost (uk - uk_prev) Rdu (uk - uk_prev) x rk4_step(dynamics, x, uk, dt) # 用模型预测下一步状态 uk_prev uk return cost n_decision Np * 2 res minimize(rollout_cost, u_init_hot, methodSLSQP, bounds[(-30.0, 30.0)] * n_decision, options{maxiter: 80, ftol: 1e-6}) if not res.success: # 求解失败时回退到上一时刻控制量不要输出 NaN return u_prev return res.x[0:2] # 只执行第一步控制量这个函数里有三个值得说的细节。第一约束全部通过 bounds 表达避免在 SLSQP 里写非线性不等式约束这样求解更稳定且速度更快。第二求解失败时返回上一时刻控制量保证仿真或真机能“带病运行”而不是直接崩溃这是工程习惯。第三代码只取了最优解的第一步这正是 MPC 滚动优化的核心每次都重新算窗口但只执行一步让反馈闭环仍然存在。主循环里要做三件事把当前时刻的未来参考切片传给求解器调用 mpc_step 得到力矩用真实动力学推进系统状态。下面这段把完整流程串起来dt 0.01 Np 15 Nc 3 Q np.diag([100.0, 100.0]) # 位置误差权重 Qv np.diag([1.0, 1.0]) # 速度误差权重 R np.diag([0.01, 0.01]) # 控制量权重 Rdu np.diag([0.05, 0.05]) # 控制增量权重 def make_ref(t): # 平滑的正弦参考轨迹保证二阶可导 q_ref np.array([0.8 * np.sin(2.0 * t), 0.6 * np.cos(1.5 * t)]) dq_ref np.array([1.6 * np.cos(2.0 * t), -0.9 * np.sin(1.5 * t)]) return q_ref, dq_ref x np.array([0.1, -0.2, 0.0, 0.0]) # 初始状态 u_prev np.zeros(2) u_hot np.zeros(Np * 2) logs [] for step in range(int(3.0 / dt)): t step * dt times np.arange(Np) * dt t q_win np.array([make_ref(tt)[0] for tt in times]) dq_win np.array([make_ref(tt)[1] for tt in times]) tau mpc_step(x, q_win, dq_win, Np, Nc, u_prev, u_hot) x rk4_step(dynamics, x, tau, dt) # 真实对象只走一步 u_prev tau # 热启动把旧窗口前移最后补当前控制量 u_hot np.concatenate([u_hot[2:], tau]) logs.append([t, *x, *tau])注意循环里真正的“被控对象”也是 rk4_step(dynamics, ...)这等于用同一个模型作预测和作仿真属于理想化验证步骤。实际工程里被控对象要换成真实机械臂或带噪声、带摩擦的仿真模型不然模型失配问题会被完全掩盖。热启动 u_hot 是 SLSQP 能收敛的关键后面避坑章还会细说。3.3 权重与时域的实战起点第一次跑这个代码不建议上来就微调权重。先把参数放在一个能稳定工作的区间再逐步加压。我常用的起步参数如下表参数含义起步值调整方向dt控制周期0.01 s硬件实时性不够就放大但别超过 0.02Np预测时域15跟踪迟钝就减小抖动就增大Nc控制时域3增加可以让控制更灵活但求解变慢Q位置误差权重diag(100)提高会让轨迹更贴紧但易振荡Qv速度误差权重diag(1)增加能抑制速度毛刺R控制量权重diag(0.01)提高会让力矩更平滑但跟踪变软Rd控制增量权重diag(0.05)专门治抖起步值不能太小Q 与 R 的比值而不是各自的绝对值决定跟踪的“刚度”。Q/R 1000 时轨迹贴得紧Q/R 100 时动作柔和。我第一次调的时候只动 Q 不动 R发现轨迹还是抖因为 Q 增大后控制器更激进力矩变化更猛R 和 Rd 没跟上。经验是先固定 R 和 Rd只调 Q 让误差收敛误差压下来但出现抖动再回头加大 Rd。用这个顺序比来回同时调几个参数要快得多也少了很多玄学成分。3.4 实时性一个控制周期里能干多少活仿真里跑通只是第一步落地到机械臂控制板时要算一笔实时性账控制周期定了留给 MPC 求解的时间就是固定的。二连杆加上 15 步预测、3 步控制时域决策变量只有 30 个SLSQP 在普通处理器上单步求解约 2050 ms但如果把同样的算法搬上六轴机械臂、Np 拉到 30决策变量变成 180 个通用求解器基本跑不动。工程上常见的处理办法有三条一是改用专门的非线性 MPC 实时求解器它们用实时迭代RTI思路每个周期只做一次完整牛顿迭代而不是从头收敛二是把模型在每个采样点线性化得到线性时变 MPC然后交给 QP 求解器虽然牺牲一点精度但实时性提升一个量级三是在线下把最优控制问题参数化把大部分计算搬到离线阶段。对初学者我的建议是先跑通通用求解器理解代价函数和约束的行为再考虑换求解器提速度。4. 仿真验证与三步调参从“能动”到“跟踪得准”4.1 搭一个二连杆仿真环境与参考轨迹仿真环境就是第 3 章的代码动力学用 dynamics 和 rk4_step参考轨迹用 make_ref。额外建议加两样东西一是在被控对象里加入摩擦力矩和测量噪声让验证结果更真实二是把参考轨迹改成带阶跃急转的形状用来检验预测优势。单纯的正弦轨迹对所有控制器都友好测不出差距。带摩擦的仿真对象可以这样改在动力学返回的 qdd 里减掉一个库仑 黏性摩擦项。这里注意预测模型里可以不加摩擦但被控对象里必须加否则后续做模型失配分析时没有对照。测量噪声一般加在状态反馈上x_measured x_true np.random.normal(0, 1e-4, 4)。这两处改动会让控制难度上一个台阶也更接近真实关节的脾性。4.2 三步调参法稳定 → 响应 → 约束我调 MPC 轨迹跟踪参数固定走三步顺序不乱。第一步先保证稳定把 Q 调到很小的值比如 diag(10)R 和 Rd 调大跑通整个仿真确认没有发散、没有剧烈振荡。这个阶段的目标不是跟踪精度而是验证模型、求解器和主循环没有逻辑错误。第二步调响应逐步提高 Q每调一次分别记录最大误差和力矩曲线。误差下降的斜率在某个临界点后会放缓这时就停继续加 Q 只会换来更大幅度的力矩振荡误差几乎不再变小。第三步查约束把 u_min/u_max 从 ±30 收窄到 ±15看哪个周期的力矩被边界截住截住之后跟踪误差有没有异常放大。如果误差在约束激活时明显恶化说明代价函数里缺了对“违反约束后果”的平滑处理最常见的是缺松弛变量这在下一章的避坑部分有对应解法。这套三步法解决的是“参数之间互相牵连”的问题先让系统稳再压误差最后补约束。反过来操作很容易掉进“Q 和 Rd 一起调、越调越乱”的泥潭。4.3 四个评测指标与一组参考结果判断 MPC 比 PID 好多少不能只盯着肉眼看的曲线。我会固定用四个指标关节误差的均方根值 RMSE、最大绝对误差、控制量总能耗、约束违规次数。RMSE 反映整体跟踪质量最大误差反映最坏情况能耗决定机械臂会不会发热约束违规次数直接判定方案是否满足安全要求。某个模拟项目上的实测结果是同样的正弦轨迹PID 经过整定后 RMSE 约 0.021 rad最大误差 0.09 radMPC 在 Np15、Qdiag(100) 时 RMSE 降到 0.006 rad最大误差 0.02 rad能耗反而比 PID 低 12%。这个结果说明 MPC 不是拿能量换精度而是通过预测做了更合理的力矩分配。能达到什么水平受模型质量影响很大如果你发现 MPC 的误差只比 PID 小一点点先检查预测模型和被控对象是不是差得太远而不是怀疑 MPC 本身。5. MPC 轨迹跟踪避坑指南5 个实测翻车现场5.1 数值层高频抖动、求解失败与热启动现象一跟踪曲线在目标位置附近出现频率十赫兹以上的细密抖动力矩曲线正负交替像锯齿一样。原因基本是 Rd 权重太小控制增量惩罚不够MPC 在相邻两步之间算出来的力矩差异过大或 Np 太短预测窗口看不到“当前激进行为”之后的代价。解决先把 Rd 从 0.05 提到 0.5观察抖动是否衰减如果衰减不明显再把 Np 从 15 加到 20。两个动作都做不要只改一个。现象二SLSQP 返回不收敛甚至优化结果里出现 NaN。初始解给零向量是最常见的原因。MPC 问题每一时刻都在变但从上一时刻到下一时刻的最优解其实很接近所以用上一周期解做热启动是必须的不是可选项。主循环里那句u_hot np.concatenate([u_hot[2:], tau])就是热启动把旧窗口前移最后补上刚执行的控制量。初次调试时可以在 mpc_step 里把 res.success 打印出来记录哪些时刻失败再针对那个时刻的初值做处理。5.2 模型层模型失配与离散化步长现象三仿真里跟踪得很好但换成另一组负载参数后误差明显增大且出现持续的相位滞后。原因是 MPC 的预测模型里负载和质量参数还是旧值模型失配导致预测轨迹和真实轨迹系统性偏差。解决的常见做法是加一层在线参数辨识或扰动估计把负载质量当扩展状态实时估计或者把模型失配项当作外部扰动用扰动观测器补偿到力矩指令上。至少要在仿真里验证参数偏差 ±20% 时的跟踪表现再决定要不要上在线辨识。现象四把 dt 从 0.01 改成 0.02 后同样的 Np 下系统开始振荡。这不一定是权重问题而是采样周期变长后离散化误差变大模型精度下降同时 Np 对应的预测时长翻倍控制器视野变长激进行为被“看得更远”但模型本身又不可信。解决改了 dt 就必须同步重新审视 Np 和 Nc。一条实用经验是保持Np * dt基本不变dt 变大就减小 Np这比维持 Np 数字不变更合理。5.3 约束层力矩饱和后的误差放大与任务空间奇异现象五进入高速段时力矩达到限幅退出饱和后跟踪误差出现一波明显的超调。原因是约束被硬生生截断但代价函数里没有表达“接近约束时应提前减速”MPC 对饱和的影响预估不足。标准的解决手段是给力矩约束加松弛变量把 u ∈ [u_min, u_max] 改成 u ∈ [u_min - s, u_max s]并在代价里增加对 s 的重惩罚。这样约束在极端工况下可以被轻微突破但优化目标会引导控制器在更早的时刻就开始收敛力矩避免硬截断造成的冲击。至于任务空间奇异如果你直接用笛卡尔误差做代价而轨迹刚好经过机械臂奇异位形雅可比矩阵在这一带数值巨大末端微小误差会被放大成关节空间的巨大控制量。解决是把参考轨迹避开奇异区域或在代价里对雅可比的奇异值做下限钳制不要指望 MPC 能“自动”绕开奇异它只知道你给它的代价函数并不知道物理上的奇异问题。6. 进阶技巧末端约束加前馈偏置把稳态误差再压一个量级把基础 MPC 跑通之后要再提精度我推荐叠加两个修改末端约束和名义力矩偏置。末端约束是在预测窗口的最后一步额外加一个惩罚项相当于告诉优化器“不止前面几步要跟得住窗口末端也要落在参考附近”。实现很简单在 rollout_cost 的最后补一行代码。# 在 rollout 预测循环结束之后追加 terminal_err x[0:2] - q_ref_win[-1] cost terminal_err P terminal_err # P 一般取 5~10 倍的 QP 是末端权重矩阵它把“只看中间过程”变成“中间和结尾都看”能显著抑制预测尾部轨迹发散的问题。另一个更实用的技巧是前馈偏置机械臂的重力项、科氏力项本来就容易算不必让 MPC 用优化去“重新发现”它们。把控制量拆成 u u_nominal δu其中 u_nominal 由逆动力学算出来MPC 决策变量只保留 δu代价函数也改罚 δu。这样优化器只处理模型误差和约束补偿收敛更快跟踪稳态误差常常直接降一个量级。我还记得自己第一次把这套东西从单关节挪到二连杆时翻车翻在采样周期上我把控制周期从 10 ms 改成 5 ms想让控制更密却忘了同步调整 Np结果预测时长的物理时间直接减半系统反而开始抖。后来养成一个习惯每改一次 dt就把 Np、Nc、离散步数全部重新核对一遍确保预测窗口覆盖的真实时长符合任务需要。MPC 的参数都是“成组出现”的只动其中一个一定会被其他参数拉回来。希望这篇笔记能让你少走我走过的弯路祝调试顺利。本文还有配套的精品资源点击获取