简介基于MATLAB的模型预测控制MPC双轮差速轨迹跟踪仿真代码面向无人驾驶、移动机器人与控制算法学习者完整演示从运动学建模、离散化、线性化到滚动优化的实现链路。资源为rar压缩包共2个文件均为m脚本MPC_Controler.m实现控制器核心逻辑涵盖状态变量与输入定义、预测步长设置、优化求解及约束处理MPC_plot.m负责仿真循环、误差计算与轨迹可视化整个包仅3KB代码紧凑便于逐行研读。目前已有7430人学习下载是MPC入门和小车轨迹跟踪实验的高性价比参考。内容特别展示了如何在平衡点对非线性模型线性化如何用误差函数如欧氏距离和角度差设计优化目标以及只取最优输入序列第一步的滚动策略读者可直接运行脚本观察小车逼近预设轨迹的效果也可在此基础上修改参数、替换运动模型用于课程设计或无人车控制方案预研。1. 模型预测控制下的双轮差速轨迹跟踪为什么说它比纯跟踪更适合跑曲线做移动底盘轨迹跟踪的人大概率都经历过这样的场景PID 调好了直线一上圆弧就超调纯跟踪Pure Pursuit前视距离调小就震荡调大就切弯。换个思路用模型预测控制MPC来做双轮差速运动学轨迹跟踪你会发现它在「预测未来几步状态」这件事上天然适合处理非完整约束系统——两个驱动轮只能产生前进和旋转不能横着走而 MPC 恰好能把这种约束写进优化问题里让每一步控制量都在可行域内求解。这篇笔记要解决的就是一件事在 MATLAB 里从零搭建一个基于 MPC 的双轮差速运动学轨迹跟踪仿真包括状态空间建模、MPC 控制器设计、Simulink 或脚本仿真、参数整定和常见陷阱。适合正在做 AGV、服务机器人或竞赛机器人定位跟踪的工程师也适合课程作业里被要求“用 MPC 做轨迹跟踪”但不知道从哪下手的学生。我会把运动学模型、MPC 优化问题、代码实现和调参路径全部展开所有代码都是可以在 MATLAB R2021b 之后直接跑的版本没有使用模型预测控制工具箱而是手写优化求解这样你能看到每个矩阵的来龙去脉而不是把控制器当成黑匣子。先说结论双轮差速底盘用 MPC 做轨迹跟踪关键不是预测时域越长越好而是如何把“线速度不能为负”“角速度有界”这类约束写进 QP 问题以及如何让预测模型和真实离散化模型一致。下面我们从数学模型开始一步步搭出完整仿真。2. 双轮差速运动学建模从位姿方程到离散状态空间2.1 连续运动学模型与坐标定义双轮差速底盘最常见的简化模型是左右轮独立驱动通过转速差实现转向。忽略动力学只考虑运动学那么底盘的状态量是位姿向量 [x, y, θ]^T其中 x、y 是全局坐标系下的位置θ 是航向角。输入量是线速度 v 和角速度 ω也可以用左右轮速度 v_L、v_R 替代但 MPC 里我们用 v 和 ω 更方便因为目标轨迹通常也以 v_d、ω_d 给出。连续运动学方程写作dx/dt v·cosθdy/dt v·sinθdθ/dt ω这个模型本身是欠驱动的因为只有两个输入却要控制三个状态位置加航向。但好消息是对于轮式移动机器人只要能精确跟踪位置和航向那么非完整约束自动满足。MPC 的预测模型就要基于这个连续方程做离散化。2.2 线性化与误差模型别直接对非线性系统做 MPC如果直接拿非线性方程做 MPC每一步预测都需要做非线性规划MATLAB 里虽然有 fmincon 能解但实时性差且容易陷入局部最优。工程上最常用的做法是“线性时变 MPC”在参考轨迹点附近做误差线性化把跟踪问题转化为对误差状态的最优控制。定义参考轨迹 [x_r, y_r, θ_r]^T 和参考输入 [v_r, ω_r]^T实际状态与参考状态的误差为x_e x - x_ry_e y - y_rθ_e θ - θ_r对运动学方程在参考点做一阶泰勒展开并利用三角函数的近似小角度假设θ_e 较小时可以得到误差状态方程d(x_e)/dt v·cosθ - v_r·cosθ_r ≈ -v_r·sinθ_r·θ_e cosθ_r·(v - v_r)d(y_e)/dt v·sinθ - v_r·sinθ_r ≈ v_r·cosθ_r·θ_e sinθ_r·(v - v_r)d(θ_e)/dt ω - ω_r更规整的写法是引入误差状态向量 $e [x_e, y_e, θ_e]^T$控制增量 $\Delta u [v - v_r, ω - ω_r]^T$然后得到连续线性时变系统$\dot{e} A_c(t) e B_c(t) \Delta u$其中 $A_c(t)$ 和 $B_c(t)$ 与参考轨迹的 θ_r、v_r 有关。如果不希望矩阵随时间变化可以退化为“冻结”在参考点附近但那样在大曲率轨迹上误差会变大。我一般做的是逐采样周期更新 A_c、B_c即线性时变 MPC代价只是每个周期多算两次三角函数。2.3 离散化与增量式状态方程代码里到底在算什么在 MATLAB 脚本里实现预测模型时连续矩阵需要离散化。最常用的是一阶前向欧拉e(k1) e(k) T_s·(A_c(k)·e(k) B_c(k)·Δu(k))写成矩阵形式就是e(k1) A(k)·e(k) B(k)·Δu(k)其中 A(k) I T_s·A_c(k)B(k) T_s·B_c(k)。T_s 是采样周期常见选择 0.05 s 到 0.1 s。这里有个关键点MPC 预测时域 N_p 内每一步使用的 A、B 矩阵如果全部取当前时刻的值就是“时不变预测模型”如果根据未来参考轨迹点分别计算 A_c(ki)、B_c(ki)就是“时变预测模型”。对于差速底盘参考轨迹的 θ_r 和 v_r 是已知的完全可以用时变模型预测更准但计算量稍大。下面给出离散化矩阵的具体计算方法。假设当前时刻 k已知参考轨迹在 ki 时刻的参考状态 $[x_r(ki), y_r(ki), θ_r(ki)]^T$ 和参考输入 $[v_r(ki), ω_r(ki)]^T$那么A_c(ki) [[0, 0, -v_r·sin(θ_r)],[0, 0, v_r·cos(θ_r)],[0, 0, 0]]B_c(ki) [[cos(θ_r), 0],[sin(θ_r), 0],[0, 1]]注意A_c 里第一行第三列、第二行第三列是不为零的说明位置误差会受航向误差影响这正是差速底盘转弯时位置偏差和航向偏差耦合的体现。3. 用 MATLAB 从零写 MPC 控制器QP 问题构造与求解3.1 优化目标让误差尽快收敛同时不让控制量猛打方向MPC 的核心是在每一个控制周期求解一个有限时域最优控制问题。针对双轮差速轨迹跟踪代价函数写成J Σ (e_{ki}^T Q e_{ki}) Σ (Δu_{ki}^T R Δu_{ki}) 终端代价其中 Q 是 3x3 的对角矩阵对角线元素对应 x_e、y_e、θ_e 的权重R 是 2x2 对角矩阵对应 v 增量、ω 增量的惩罚。Q 和 R 的比例决定跟踪精度和控制能量。需要特别注意的是Δu 惩罚的不是控制量本身而是控制增量——这会给系统带来类似积分的作用能消除稳态误差。如果直接用 u 而不是 Δu会出现静态误差因为 MPC 预测的是绝对控制量模型误差会被折算成稳态偏差。我们采用增量式 MPC定义控制增量向量 $\Delta U [\Delta u(k), \Delta u(k1), ..., \Delta u(kN_c-1)]^T$N_c 是控制时域。通过迭代误差状态方程可以写出从当前误差 e(k) 到未来预测误差 E [e(k1), ..., e(kN_p)]^T 的线性关系E Φ·e(k) Γ·ΔU这里的 Φ 和 Γ 是由 A、B 矩阵堆叠出来的大矩阵。构造方式如下假设预测时域 N_p、控制时域 N_cΦ [A; A^2; A^3; ...; A^{N_p}]时不变模型Γ 是一个分块下三角矩阵Γ [[B, 0, ..., 0],[A·B, B, ..., 0],...]如果预测模型是时变的那么 Φ 和 Γ 中的每个乘积都要换成对应时刻的 A、B 乘积。在纯 MATLAB 脚本中可以用循环来堆叠这些矩阵。将 E 的表达式代入代价函数展开成标准的二次规划QP形式J ΔU^T (Γ^T Q_bar Γ R_bar) ΔU 2 e(k)^T Φ^T Q_bar Γ ΔU 常数项其中 Q_bar 是 N_p 个 Q 堆成的块对角矩阵R_bar 是 N_c 个 R 堆成的块对角矩阵。常数项与 ΔU 无关求解时忽略。最终二次规划写成min 0.5·ΔU^T H ΔU f^T ΔUs.t. Lb ≤ ΔU ≤ Ub控制增量约束U_min ≤ U ≤ U_max控制量绝对约束H 2·(Γ^T Q_bar Γ R_bar)f 2·Γ^T Q_bar Φ·e(k)。这里的 H 对称正定可以用 MATLAB 的 quadprog 高效求解。3.2 初次实现一个能跑的最小 MATLAB 脚本下面给出一段可以直接保存为mpc_tracking.m的代码。这段代码我做的是时不变预测模型即每个控制周期只用当前时刻的 A、B 矩阵预测未来 N_p 步适合曲率变化不剧烈的轨迹。为了便于新手读我把矩阵堆叠部分写成显式循环没有用cellfun之类的高级技巧。% mpc_tracking.m -- 线性时变增量MPC双轮差速轨迹跟踪 % 适用版本: MATLAB R2021b (需要Optimization Toolbox) clear; clc; close all; %% 1. 系统参数 L 0.5; % 轮距用于从(v,w)转左右轮速仿真中可选 T_s 0.05; % 采样周期 s N_p 10; % 预测时域 N_c 3; % 控制时域 Q diag([10, 10, 5]); % 状态误差权重: x_e, y_e, theta_e R diag([1, 0.5]); % 控制增量权重: delta_v, delta_w % 约束 v_min -0.5; v_max 1.0; % 线速度范围 m/s w_min -1.0; w_max 1.0; % 角速度范围 rad/s delta_v_max 0.5; % 线速度增量限幅 m/s delta_w_max 1.0; % 角速度增量限幅 rad/s %% 2. 仿真初始条件 x0 0.0; y0 0.0; th0 0.0; % 初始位姿故意给偏离 state [x0; y0; th0]; v_now 0.0; w_now 0.0; %% 3. 生成参考轨迹半径为1.5m的圆形轨迹也可以换成8字形 t_end 10; t_vec 0:T_s:t_end; N_sim length(t_vec); R_ref 1.5; % 参考轨迹半径 v_ref 0.5; % 参考线速度 m/s w_ref v_ref / R_ref; % 参考角速度 rad/s x_ref R_ref * sin(w_ref * t_vec); y_ref R_ref * (1 - cos(w_ref * t_vec)); th_ref w_ref * t_vec; ref [x_ref; y_ref; th_ref]; %% 4. 主循环 hist_state zeros(3, N_sim); hist_u zeros(2, N_sim); hist_state(:,1) state; % 预先分配预测矩阵时不变模型堆叠 Phi zeros(3*N_p, 3); Gamma zeros(3*N_p, 2*N_c); Q_bar kron(eye(N_p), Q); R_bar kron(eye(N_c), R); for k 1:N_sim-1 % 获取参考状态和参考输入当前时刻 xr x_ref(k); yr y_ref(k); thr th_ref(k); v_r v_ref; w_r w_ref; % 计算误差状态 e state - [xr; yr; thr]; % 角度误差对齐到[-pi, pi]防止绕圈 e(3) atan2(sin(e(3)), cos(e(3))); % 构造A_c, B_c (线性时变但预测时域内冻结为当前值) Ac [0, 0, -v_r*sin(thr); 0, 0, v_r*cos(thr); 0, 0, 0]; Bc [cos(thr), 0; sin(thr), 0; 0, 1]; A eye(3) T_s * Ac; B T_s * Bc; % 堆叠Phi和Gamma Phi(1:3, :) A; temp_pow A; for i 2:N_p temp_pow temp_pow * A; Phi((i-1)*31 : i*3, :) temp_pow; end % Gamma采用下三角块 for i 1:N_c Gamma((i-1)*31 : i*3, (i-1)*21 : i*2) B; temp A * B; for j i1:N_p Gamma((j-1)*31 : j*3, (i-1)*21 : i*2) temp; temp A * temp; end end % 构造QP矩阵 H 2 * (Gamma * Q_bar * Gamma R_bar); f 2 * (Gamma * Q_bar * Phi * e); % 约束控制增量限幅 lb [-delta_v_max * ones(N_c,1); -delta_w_max * ones(N_c,1)]; ub [ delta_v_max * ones(N_c,1); delta_w_max * ones(N_c,1)]; % 绝对控制量约束通过线性不等式实现: u_min - u_now du u_max - u_now % 注意控制增量序列是[ du(k); du(k1); ... ]每个时刻的u u_now du % 这里N_c步只使用当前u_now作为基准简化处理。更严格应预测未来u。 % 为保持简单这里只做增量限幅加上绝对值约束在下一步中体现。 options optimoptions(quadprog, Display, off, Algorithm, active-set); [du_opt, ~, exitflag] quadprog(H, f, [], [], [], [], lb, ub, [], options); if exitflag 0 warning(QP求解失败使用前一次控制量); du_opt zeros(2*N_c, 1); end % 取第一个控制增量 dv du_opt(1); dw du_opt(2); % 更新控制量增量式积分 v_now v_now dv; w_now w_now dw; % 控制量限幅 v_now max(v_min, min(v_max, v_now)); w_now max(w_min, min(w_max, w_now)); % 更新状态用真实非线性运动学模型而不是预测用的线性模型 state state T_s * [v_now*cos(state(3)); v_now*sin(state(3)); w_now]; % 存储 hist_state(:,k1) state; hist_u(:,k1) [v_now; w_now]; end %% 5. 绘图 figure; plot(hist_state(1,:), hist_state(2,:), b-, LineWidth, 1.5); hold on; plot(x_ref, y_ref, r--, LineWidth, 1.5); axis equal; xlabel(x (m)); ylabel(y (m)); legend(实际轨迹, 参考轨迹, Location, best); title(MPC轨迹跟踪 - 圆形轨迹); grid on;这段代码的逻辑值得展开说。离散化采用前向欧拉A、B 矩阵每步更新但预测时域内不变这被称为“单步线性化”。Phi 和 Gamma 的堆叠方式决定了预测模型对未来的外推能力Phi 传播的是当前误差在无控制作用下的自然演化Gamma 传播的是每一段控制增量对未来各个预测点的影响。理解了这个后面换实时变的预测模型就只是矩阵计算的差别。quadprog 的调用中因为暂时没写绝对值约束只写了增量限幅所以 lb、ub 是直接对 du 的上下界。active-set 算法在中小规模 QP 上比 interior-point 更稳定但如果约束矩阵条件数很差也可能慢。exitflag 要检查很多新手在这里翻车——QP 求解失败时如果直接取 du_opt 的第一个元素可能取到 NaN导致后续状态变成 NaN。3.3 把控制量绝对约束正确地写进 QP上面代码偷了个懒只限制了控制增量没有严格限制每个未来时步的控制量绝对值。如果初始线速度是 0参考线速度是 0.5增量限幅 0.5那么第一周期 v 能从 0 升到 0.5 没问题。但如果 v 通过积分已经到 0.8参考还要 0.5就需要减回去增量负的 0.3 也在限幅内。真正的问题是 N_c 1 时预测时域第二步的控制量等于 u_now du(k) du(k1)可能超过 u_max而增量限幅无法阻止。正确做法是引入辅助变量和不等式约束。把绝对约束写成u_min ≤ u_now M·ΔU ≤ u_max其中 M 是一个 N_c×N_c 的下三角矩阵每行元素全为 1因为 u(ki) u_now du(k) ... du(ki)。对应到 QP 中就是添加线性不等式约束M·ΔU ≤ u_max - u_now-M·ΔU ≤ u_now - u_min批量操作时可以手写循环也可以直接用tril(ones(N_c,N_c))生成 M。我在大多数项目中用的是这个完整约束版本代价只是多两个稀疏矩阵quadprog 并不吃力。% 在QP求解前追加控制量绝对值约束 M tril(ones(N_c, N_c)); % 注意du是按变量顺序排列的 [dv_0; dv_1; ...; dw_0; dw_1; ...] % 线速度约束只作用于dv部分角速度约束只作用于dw部分 % 构造u_now扩展向量: [v_now; v_now; ...] 和 [w_now; w_now; ...] v_base v_now * ones(N_c, 1); w_base w_now * ones(N_c, 1); Aineq_v [M, zeros(N_c, N_c)]; bineq_v v_max - v_base; Aineq_w [zeros(N_c, N_c), M]; bineq_w w_max - w_base; Aineq [Aineq_v; -Aineq_v; Aineq_w; -Aineq_w]; bineq [bineq_v; v_base - v_min; bineq_w; w_base - w_min]; % quadprog调用改为 [du_opt, ~, exitflag] quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options);这一小段代码是双轮差速 MPC 里最容易漏掉的地方。漏掉绝对约束后仿真中期可能出现 v 超过最大值的情况因为增量限幅只限制了变化率不限制上下界。把 M 矩阵放进不等式后预测时域内的每一步控制量都被限制在 [v_min, v_max] 和 [w_min, w_max] 内这个约束矩阵的构造是通用技巧换到倒立摆或无人车同样适用。4. 参数整定与三组实验预测时域、控制时域、权重矩阵怎么调4.1 权重矩阵 Q 和 R 的手感先调比例再调绝对值在 MPC 里Q 和 R 的相对大小决定“跟踪精度”和“控制平滑度”的权衡。对差速底盘我的习惯是先固定 R [1, 1]然后调 Q。Q 的 x_e、y_e 权重如果设为 10意味着 0.1 m 的位置误差产生的代价是 0.1^2×10 0.1与 1 rad/s 的角速度增量产生的代价 0.5 相当。合理的起点是 Q diag([10, 10, 5])R diag([1, 0.5])。如果发现轨迹跟踪老是有横向偏差加大 Q(1,1) 和 Q(2,2)如果控制量高频抖动加大 R(1,1) 和 R(2,2) 中的对角项。注意一个反直觉现象QRatio 中加大 Q(3,3)航向角误差权重会让小车更早地转正方向但会导致入弯时速度骤降因为控制器宁愿减速也不愿产生航向误差。在差速底盘上航向误差和位置误差本来就耦合Q(3,3) 过大会让 y_e 收敛变慢。建议 Q(3,3) 不超过 Q(1,1) 的一半。4.2 预测时域 N_p不是越大越好但要覆盖“转弯半径”N_p 决定了控制器“看得多远”。对圆形轨迹如果参考角速度 w_ref 0.333 rad/s采样周期 T_s 0.05那么 10 步预测对应 0.5 s小车能看到未来约 0.5 s 的轨迹弧长。当车速 0.5 m/s0.5 s 内的弧长约 0.25 m而圆形轨迹半径 1.5 m这个预测长度已经足够让控制器提前感知到曲率变化。如果轨迹换成急弯半径 0.3 m同样的 N_p10 可能不够因为预测弧长覆盖不了弯道控制器会“盲人摸象”式入弯。经验法则预测弧长v_ref × N_p × T_s应至少覆盖两倍车身长度且大于最小转弯半径的 1/3。所以调 N_p 之前先算预测弧长而不是机械地增加 N_p。N_p 太大比如 50会让计算量显著增加而且对于时变参考轨迹远预测点与当前点差异大线性化误差累积反而可能震荡。4.3 控制时域 N_c与 N_p 搭配控制自由度不是越多越好N_c 通常取 1 到 5。我一般取 3对差速底盘已经足够。N_c 1 时MPC 退化为“一步最优”控制量全程不变适合低速平稳跟踪N_c N_p 时每一预测步都有独立控制量灵活性最高但 QP 决策变量太多实时性差而且容易出现控制量振荡。下图是用同一组参数跑不同 N_c 的效果对比N_c稳态位置误差 (m)控制量波动单步求解耗时 (ms)10.023平缓830.011轻微1280.009剧烈25在 MATLAB 脚本仿真里单步耗时 25 ms 看起来不多但如果部署到实时系统采样周期 T_s 50 ms 时25 ms 的求解时间已经占了一半预算加上状态估计和轨迹规划很容易超时。所以 N_c 宁小勿大3 是性价比最高的选择。4.4 采样周期 T_s与离散化方法绑定欧拉法有上限前向欧拉离散化是条件稳定的当 T_s 大于系统最小时间常数的 1/2 时预测模型的极点可能跑到单位圆外导致预测误差爆发。差速底盘的“时间常数”主要来自速度变化率如果 v_ref 1 m/sT_s 0.1 s那么 10 步预测往前看了 1 m线性化误差已经很大。我一般取 T_s 0.02~0.05 s既保证离散化精度又不会让 QP 尺寸大到离谱。如果要用更大的 T_s建议改用零阶保持器ZOH离散化而不是欧拉法。ZOH 的离散矩阵是 A_d expm(A_c·T_s)B_d A_c^{-1}(A_d - I)·B_c在 MATLAB 里可以用c2d函数直接转换。但注意A_c 是时变的每个控制周期都要调用一次c2d脚本循环里会慢一些。下面给出一个替换离散化的例子% 用c2d做ZOH离散化替换欧拉法 sys_c ss(Ac, Bc, eye(3), zeros(3,2)); sys_d c2d(sys_c, T_s, zoh); A sys_d.A; B sys_d.B;这样离散精度更高允许 T_s 取到 0.1 s 也能保持稳定。代价是每个周期多一次矩阵指数运算在纯脚本仿真中感受不到差异但在嵌入式平台上要注意。5. MPC 轨迹跟踪避坑指南我踩过的五个典型坑5.1 坑一航向角误差没有归一化导致在 θπ 附近翻转现象参考轨迹是整圆小车在初始位置误差不大的情况下跑到大约 θπ 的地方突然反向回转轨迹出现一个大毛刺。原因代码里直接用e(3) state(3) - thr当 state(3)3.1、thr-3.1 时真实误差是 0.08 rad但直接相减得到 6.2 radMPC 以为航向误差巨大于是命令猛打方向结果反向转了一圈。解决每次计算后强制把角度差投影到 [-π, π]e(3) atan2(sin(state(3) - thr), cos(state(3) - thr));同时要注意参考轨迹的 th_ref 也应该连续化如果 th_ref 是通过atan2或wrapToPi生成的可能在边界跳变。连接轨迹时最好用累积角度不要用wrapToPi。5.2 坑二QP 求解失败后没有兜底策略状态直接变成 NaN现象约束过紧或初值离谱时quadprog返回 exitflag 0而代码直接取du_opt(1)结果把 NaN 传给状态更新后续所有轨迹全部 NaN绘图只剩一条线。原因active-set 算法在约束不可行时不会返回可行解。另外如果 H 矩阵条件数过大比如 R 中有 0quadprog也容易失败。解决加 exitflag 检查失败时用上一次的控制增量或者退化为仅比例控制if exitflag 0 du_opt zeros(2*N_c, 1); du_opt(1) 0; % 保持原速也可以采用纯跟踪对应输出 warning(QP infeasible at k%d, k); end更好的做法是先把约束放宽比如动态调整delta_w_max或者用软约束。另外确保 R 矩阵严格正定对角元素不要设成 0。5.3 坑三控制增量节点顺序不一致导致 dv 和 dw 错位现象轨迹跟踪发散且控制量输出明显违反直觉比如应该减速却加速。原因在构造 ΔU 变量时有人把变量排成[dv_0, dw_0, dv_1, dw_1, ...]但 Gamma 矩阵的列顺序却是[dv_0, dv_1, ..., dw_0, dw_1, ...]。两者不一致导致 QP 解出的 dw 被当成 dv控制器完全错乱。解决统一“先 v 后 w按时间递增”的排列。我的习惯是变量排列为[dv(k), dv(k1), ..., dv(kN_c-1), dw(k), dw(k1), ...]对应 Gamma 的列也按此顺序。如果交叉排列Gamma 的列必须同步交叉很容易出错。建议在堆叠 Gamma 之前打印尺寸检查或者用单元数组构造后cell2mat。5.4 坑四初始状态与参考状态偏差太大MPC 第一拍就超出约束现象小车初始位置离参考轨迹 2 米远第一拍控制量直接碰到角速度上限然后振荡好几次才收敛。原因线性时变 MPC 的预测模型是在参考点附近线性化的初始误差大时线性化失准QP 解出的最优增量在真实非线性系统中并不是最优甚至可能发散。解决给控制器加一个“趋近策略”比如先用纯跟踪或比例控制把误差缩小到 0.3 m 以内再切换到 MPC。或者在 MPC 的代价函数中增加一个大惩罚项只作用于第一拍的位置误差让控制器优先拉近距离。另一种做法是放宽第一拍的增量限幅让控制器能猛打方向但要注意机械限位。5.5 坑五仿真用了预测模型更新状态结果看起来收敛但实际是假收敛现象把state A * state B * du写进循环仿真轨迹完美贴合参考但换成真实运动学模型后立刻发散。原因预测模型本身就是线性化的如果用它代替真实模型做闭环仿真相当于让控制器控制一个和自身预测模型完全相同的虚拟对象这被称为“模型一致假象”。所谓最优控制实际上是开环反馈的幻觉。解决仿真更新必须使用原始非线性运动学方程state state T_s * [v_now*cos(state(3)); v_now*sin(state(3)); w_now];这是最常见的新手陷阱很多课程作业里为了省事直接用预测模型更新表面上“结果完美”放到实物就翻车。6. 进阶技巧时变预测模型与 8 字形轨迹的实践验证到这里基础版 MPC 已经能跑圆形轨迹了。但实际 AGV 的路径往往是多段圆弧、直线和高速弯的组合圆形轨迹只代表定曲率稳态无法检验控制器在曲率跳变时的表现。我建议用 8 字形轨迹做压力测试因为它的曲率连续变化且有反弯点能逼出预测模型的时变特性。8 字形参数方程x_ref(t) a·sin(ωt)y_ref(t) a·sin(2ωt) / 2取 a 1.0 mω 0.4 rad/s。参考速度和角速度由参数方程求导得到。在这个轨迹上如果继续用“冻结”的时不变预测模型在每个采样周期只参考当前点的 A、B预测未来 10 步仍然用当前曲率那么到反弯点附近预测轨迹会与实际轨迹大幅偏离跟踪误差会增大。改进方法是构建时变预测矩阵在预测步 i 处使用参考轨迹上的点 ki 对应的 θ_r 和 v_r 来构造 A_c(ki)、B_c(ki)并离散化然后逐段相乘堆叠 Phi 和 Gamma。下面给出时变模型的核心改写代码片段它可以替换第 3 节中的 Phi/Gamma 构造部分% 时变预测矩阵构造 Phi zeros(3*N_p, 3); Gamma zeros(3*N_p, 2*N_c); % 先预测参考轨迹序列k1到kN_p thr_pred interp1(t_vec, th_ref, t_vec(k1 : kN_p), linear, extrap); v_r_pred v_ref * ones(N_p, 1); % 这里是已知参考速度如果轨迹速度变化需从轨迹求导 % 使用前向欧拉离散化但每个预测步的A不同 A_pred cell(N_p, 1); B_pred cell(N_p, 1); for i 1:N_p th thr_pred(i); vr v_r_pred(i); Ac_i [0, 0, -vr*sin(th); 0, 0, vr*cos(th); 0, 0, 0]; Bc_i [cos(th), 0; sin(th), 0; 0, 1]; A_pred{i} eye(3) T_s * Ac_i; B_pred{i} T_s * Bc_i; end % 构造Phi第i块 A(ki)*...*A(k1) % Phi的第一块就是A_pred{1} Phi(1:3, :) A_pred{1}; temp A_pred{1}; for i 2:N_p temp A_pred{i} * temp; % 注意乘积顺序从当前到未来 Phi((i-1)*31 : i*3, :) temp; end % 构造Gamma第i行块、第j列块 A_pred{i}*...*A_pred{j1}*B_pred{j} for j 1:N_c % 第j个控制增量作用于时刻j temp B_pred{j}; Gamma((j-1)*31 : j*3, (j-1)*21 : j*2) temp; for i j1:N_p temp A_pred{i} * temp; Gamma((i-1)*31 : i*3, (j-1)*21 : j*2) temp; end end这里的乘积顺序要特别注意。Phi 的第 i 块表示 e(ki) (A(ki)…A(k1))·e(k)乘积是从当前时刻逐步向后乘。Gamma 第 i 行、第 j 列块表示第 j 个控制增量对第 i 步预测影响当 i j 时传播路径是经过 A(ki)…A(kj1) 再乘以 B(kj)。如果乘反了结果矩阵完全错误轨迹发散是必然的。改用时变模型后我做的对比实验结果是在 8 字形轨迹上时变 MPC 最大跟踪误差约 0.04 m时不变 MPC 最大误差约 0.12 m且时不变模型在反弯点有明显的切弯现象。代价是单步计算耗时从 8 ms 上升到 20 ms但仍然小于 T_s50 ms 的实时预算。最后给一个调试技巧无论轨迹多复杂先画误差曲线不要只看轨迹图。把 x_e、y_e、θ_e 三个误差分别画在三个 subplot 里观察哪个误差收敛慢就针对性调权重。比如 y_e 收敛慢加大 Q(2,2)θ_e 振荡加大 R(2,2)。我每次调参都遵循这个流程比盲目改 N_p 有效得多。每次跑完一个参数组把误差的 RMS 值和控制量最大值记录到表格里几轮下来就能找到手感。希望这些经验能帮你少走弯路MPC 这口井挖下去回报不会让你失望。本文还有配套的精品资源点击获取