1. 为什么选择MPC做轨迹跟踪1.1 轨迹跟踪问题的本质做车辆控制的人都知道轨迹跟踪这个课题的核心不是“让车走到某个点”而是让车辆沿着一条预先给定的路径曲线在满足横纵向运动约束的前提下尽可能精确地贴住目标轨迹。这句话听起来简单真正在MATLAB里跑起来就会发现它牵扯到三个相互矛盾的目标精度要够高、控制动作不能太猛、系统必须稳定。我在做这个课题的时候第一件事不是写代码而是把“轨迹跟踪”和“路径规划”分清楚。路径规划解决“怎么走”的问题它输出一条参考路径轨迹跟踪解决“怎么跟上走”的问题它输出控制量比如前轮转角。很多新手一上来就把两者混在一起结果仿真里一会让MPC去找路一会让它跟踪路Q矩阵和预测时域怎么调都别扭。这一步理顺之后整个仿真才能站住脚。1.2 MPC相比PID和纯跟踪优势在哪里市场上常用的轨迹跟踪方法主要有三种PID控制、纯跟踪Pure Pursuit和模型预测控制。我给它们做过一次横向对比结果很有意思。PID的思路是“看到误差再修正”它没有预测能力。遇到直线路段PID可以把横向误差压得很小但遇到曲率变化大的弯道误差已经出现在前面了PID却要等车真正偏离之后才开始反应于是弯道内侧的跟踪误差总是慢半拍车速越高越明显。纯跟踪的原理更简单它拿当前车辆位置和目标点之间做一个圆弧几何关系算出要打多少方向盘。纯跟踪对低速小曲率路径很有效参数好调工程上也常用。但它只看前方一个目标点完全不管后续路径的约束遇到S弯或连续折线时车头会来回甩而且很难把速度、转向角变化率这类物理约束一起考虑进来。MPC的不同之处在于它在每一个控制周期内都会利用一个预测模型把未来N个步长的控制效果“提前算一遍”然后在这个预测结果上做带约束的优化再把第一步控制量真正输出到车辆上。到了下一个控制周期它重新测量状态重新预测重新优化。这种“滚动优化、反馈校正”的结构相当于下棋时你先想三步再落子而不是看到对手走了一步才应对。对轨迹跟踪这种强耦合、多约束的系统MPC天然就比前两种方法更适合。1.3 这个仿真项目能帮你解决什么我这次不借助MATLAB自带的MPC Toolbox而是从零手写了一个基于二次规划求解的MPC控制器的车辆轨迹跟踪仿真。为什么不用现成工具箱因为自己搭建一遍预测模型、构造QP矩阵、写约束条件才能真正理解预测时域、控制时域、权重矩阵这些参数到底在干什么。工具箱一句话调用虽然方便但出了问题你连排查方向都没有这对学习阶段是致命的。这个仿真的核心收益有三个完整复现一条参考轨迹我用的是带变化的正弦复合弯道比纯直线或圆形更有代表性从车辆运动学模型出发建立误差状态空间方程并离散化成适合MPC求解的形式用quadprog完成带约束的在线优化最终输出前轮转角控制量绘制跟踪过程全景图。如果你是研究生、高年级本科生或者刚进入智能车辆行业的控制工程师这个项目可以帮你把MPC“黑箱”变成“白箱”。2. 建模是仿真的地基2.1 从自行车模型开始推导运动学方程轨迹跟踪仿真里最常用的车辆模型是自行车模型也叫二轮模型。它把车辆简化为前轮、后轮和一个刚性车体忽略侧倾、俯仰以及轮胎的非线性侧偏特性。在低速或中等速度工况下这个模型的精度足够支撑控制算法验证。自行车模型的状态量我取三个车辆在大地坐标系下的横坐标x、纵坐标y、航向角ψ。控制量我取两个纵向速度v和前轮转角δ。这里为了项目聚焦我把车速设为恒定值重点考察前轮转角对横向跟踪误差的影响。运动学方程如下ẋ v·cos(ψ) ẏ v·sin(ψ) ψ̇ (v / L)·tan(δ)其中L是车辆轴距也就是前轴与后轴之间的距离。这个方程组的物理意义很直观车辆沿着航向角方向前进前轮转角决定了航向角的变化速率。轴距越短同样的前轮转角带来的横摆角速度就越大车就显得越“灵活”。我经常拿它类比骑自行车你握车把向一侧转一个角度车身并不是立刻横向移动而是先改变前进方向再带动整体轨迹偏移。这个滞后关系正是轨迹跟踪控制要重点处理的动态特性。2.2 误差模型推导与线性化MPC控制器内部需要一个“预测模型”用来计算未来每个时刻车辆会跑到哪里。直接使用上面的非线性模型当然可以但这样一来MPC的优化问题就变成非线性规划求解难度和计算量都会大幅上升。工程上更常用的做法是将模型在参考轨迹点附近线性化得到一个线性时变模型在每一个控制周期内重新计算一次系数矩阵。这样做既保留了模型精度又能把优化问题转换成标准的二次规划QP求解速度快MATLAB的quadprog函数可以直接处理。为了更贴合轨迹跟踪的目的我不用x、y、ψ这三个绝对量而是把状态量定义成相对于参考路径的误差量。标准的穿差误差模型如下e1 车辆位置到参考轨迹的横向距离 e2 航向误差即车辆航向角与参考轨迹切线方向之差在“航向误差较小”的假设下可以推导出简化的连续误差模型e1̇ v·e2 e2̇ (v / L)·δ这里的“小角度假设”在大多数实际弯道中是成立的误差通常只有几度到十几度。如果你的仿真里有超过30°的大航向偏差那这个线性化模型就会失真需要改用非线性MPC或分段线性模型。对这套连续模型做离散化采样时间Ts选择0.05秒得到x(k1) A·x(k) B·u(k)其中状态 x [e1; e2]控制量 u δ系数矩阵为A [1, v·Ts; 0, 1] B [0; (v / L)·Ts]这个2×1的B矩阵正是MPC预测矩阵的核心也是调参时最容易出错的地方。请注意B中第一行是0说明当前采样时刻的控制输入不会立即改变横向位置误差要通过航向误差在下一步才能体现出来这正是车辆运动学固有的迟滞特性。2.3 参考轨迹的生成与坐标换算仿真里参考轨迹我选择了一条多项式组合曲线。这样做的好处是曲率连续可导避免产生跳变的参考航向角。参考轨迹公式x_ref(t) t y_ref(t) 2·sin(0.15·t) 0.5·sin(0.05·t)实际代码生成时我先创建一个时间数组再计算每个点的y_ref和对应的参考航向角。参考航向角用位置点的切线斜率计算公式是ψ_ref atan2(dy_ref/dx, 1)在MATLAB里实现很简单提前用gradient函数对y_ref做一次数值微分然后atan2求角度即可。这里要提醒一个常被忽略的细节跟踪误差e1的定义不是简单的目标y减去实际y。因为车辆是在XY平面内运动的如果参考轨迹本身是倾斜或弯曲的必须把绝对坐标误差投影到参考轨迹的法线方向。但在我的简化模型中参考轨迹是“y f(x)”形式的单调曲线参考航向角变化不大直接使用误差模型的垂直偏差近似在仿真范围内精度完全够用。如果你想扩展到圆形或八字形闭合路径就必须引入完整的坐标变换否则误差定义会失真。3. MPC控制器设计与MATLAB实现3.1 预测矩阵的构造细节MPC的预测模型要解决这样一个问题假设当前控制周期已知状态x(k)并且我算出未来Nc步的控制序列Δu(k), Δu(k1), ... Δu(kNc-1)那么未来Np步的状态序列是什么样的对于线性系统 x(k1) A x(k) B u(k)我们可以展开x(k1) A x(k) B u(k) x(k2) A² x(k) AB u(k) B u(k1) ...写成矩阵形式就是 X F x(k) G U。这里的F矩阵和G矩阵是整个MPC实现的“发动机”。在MATLAB实现时我建议用循环来构造F和G而不是直接套公式。虽然嵌套循环在MATLAB里效率不算高但可读性强而且构造一次预测矩阵的时间占比远小于求解QP的时间完全不影响整体仿真速度。等代码理顺之后再优化成矩阵乘法的形式也不迟。实际参数我取预测时域Np30控制时域Nc10采样时间Ts0.05秒。这意味着控制器每次优化会预测未来1.5秒的车辆运动状态并在前0.5秒内考虑控制量的变化。这个比例关系几乎决定了控制性能Np太小预测窗口太短看不出弯道前方的趋势Np太大计算量成倍上升而且预测后期的误差对未来决策的影响已经非常微弱容易造成矩阵病态。3.2 约束条件与代价函数MPC的一个核心优势是“约束能进问题”这是PID和纯跟踪做不到的。我在仿真里设置了两类约束控制量约束前轮转向角限制在 ±30°即 ±0.524 rad控制增量约束每个控制周期内转向角变化不能超过 ±3°防止方向盘抖动。注意到我这里说的是“控制增量约束”这是MPC实现中的一个经典做法。直接约束u(k)会让优化问题表现得过于僵硬比如车速较高时转角可能需要平滑变化直接对u限幅可能让系统在边界上来回碰撞而约束Δu让优化器自己决定如何平滑过渡效果实际好得多。代价函数我采用标准的二次型形式J Σ (x(ki|k))ᵀ Q x(ki|k) Σ Δu(ki)ᵀ R Δu(ki)其中Q是2×2对角矩阵对角线元素分别是横向误差和航向误差的权重R是控制增量的惩罚系数。我将Q设置为diag([100, 10])R设置为5。为什么横向误差权重取100而不是1因为这个项目的核心目标是轨迹跟踪精度横向误差的物理单位是米实际偏差通常在厘米级到分米级如果不把权重提高优化器会觉得几厘米的偏差“无伤大雅”实际表现就是轨迹贴合很差。R取5则是对转向增量的一种“阻尼”防止控制量剧烈跳动。3.3 完整的MATLAB仿真代码下面是我最终跑通的核心代码按模块划分方便直接修改参数验证。%% 参数初始化 clear; clc; close all; Ts 0.05; % 采样时间 Np 30; % 预测时域 Nc 10; % 控制时域 v 3.0; % 恒定车速 m/s L 2.5; % 轴距 m delta_max deg2rad(30); % 转向角上限 ddelta_max deg2rad(3); % 控制增量上限 % 权重矩阵 Q diag([100, 10]); % 状态权重 R 5; % 控制增量权重 %% 参考轨迹生成 t_total 0:Ts:25; % 仿真时长25秒 x_ref t_total; y_ref 2*sin(0.15*t_total) 0.5*sin(0.05*t_total); psi_ref atan2(gradient(y_ref), gradient(x_ref)); % 生成更密集的参考点用于内插 t_interp 0:0.01:25; x_ri t_interp; y_ri 2*sin(0.15*t_interp) 0.5*sin(0.05*t_interp); psi_ri atan2(gradient(y_ri), gradient(x_ri)); x_ri x_ri; y_ri y_ri; psi_ri psi_ri; %% 离散模型 A [1, v*Ts; 0, 1]; B [0; (v/L)*Ts]; nx size(A, 1); nu size(B, 2); %% 构造预测矩阵 F, G % 状态序列 X F*x0 G*U F zeros(nx*Np, nx); G zeros(nx*Np, Nc*nu); for i 1:Np Fi eye(nx); for j 1:i-1 Fi A * Fi; end F((i-1)*nx1 : i*nx, :) Fi; end for i 1:Np for j 1:min(i, Nc) G((i-1)*nx1 : i*nx, (j-1)*nu1 : j*nu) A^(i-j) * B; end end %% 代价函数的QP矩阵 H G * kron(eye(Np), Q) * G kron(eye(Nc), R); H (H H) / 2; % 保证对称 %% 仿真循环 x_init zeros(2, 1); % 初始误差状态 delta_prev 0; U_prev zeros(Nc, 1); state_history zeros(2, length(t_total)); delta_history zeros(1, length(t_total)); pos_history zeros(2, length(t_total)); for k 1:length(t_total) % 当前参考点的索引用于获取参考位置 [~, idx] min(abs(t_total(k) - t_interp)); % 误差状态计算实际车辆状态 - 参考状态投影 % 简化处理直接用参考轨迹上的坐标差近似 cur_x x_ref(k); cur_y y_ref(k); ex cur_x - x_ri(idx); ey cur_y - y_ri(idx); epsi psi_ref(k) - psi_ri(idx); % 将绝对误差转换为横向/航向误差 % 横向误差 e_y * cos(psi_ref) - e_x * sin(psi_ref) e1 ey * cos(psi_ref(k)) - ex * sin(psi_ref(k)); e2 epsi; x_k [e1; e2]; % 计算QP中的线性项 f f 2 * x_k * F * kron(eye(Np), Q) * G; f f; % 约束矩阵 % 控制量约束 -delta_max delta delta_max % 需转换为增量形式delta U_prev cumsum(deltaU) M_low tril(ones(Nc)); lb -delta_max * ones(Nc, 1) - M_low * U_prev; ub delta_max * ones(Nc, 1) - M_low * U_prev; % 增量约束 lb max(lb, -ddelta_max * ones(Nc, 1)); ub min(ub, ddelta_max * ones(Nc, 1)); A_ineq []; b_ineq []; % 求解二次规划 options optimoptions(quadprog, Display, off, Algorithm, active-set); [deltaU, ~, exitflag] quadprog(H, f, A_ineq, b_ineq, [], [], lb, ub, [], options); if exitflag 0 warning(第%d步QP求解失败使用上一周期控制量, k); deltaU zeros(Nc, 1); end % 当前控制量 delta_current delta_prev deltaU(1); delta_current max(min(delta_current, delta_max), -delta_max); % 应用控制量更新真实车辆模型 psi_dot v / L * tan(delta_current); psi_next psi_ref(k) psi_dot * Ts; % 简化 x_next x_ref(k) v * cos(psi_next) * Ts; y_next y_ref(k) v * sin(psi_next) * Ts; % 记录 delta_history(k) delta_current; state_history(:, k) x_k; pos_history(:, k) [x_next; y_next]; delta_prev delta_current; U_prev [deltaU(1:end-1); 0]; % 注意实际应使用完整序列 end %% 绘图 figure(Color, w); plot(y_ri, x_ri, k-, LineWidth, 2); hold on; plot(pos_history(2, :), pos_history(1, :), r--, LineWidth, 1.5); xlabel(Y / m); ylabel(X / m); legend(参考轨迹, MPC跟踪轨迹, Location, best); grid on; figure(Color, w); subplot(2,1,1); plot(t_total, state_history(1,:)*100, b, LineWidth, 1.5); ylabel(横向误差 / cm); grid on; subplot(2,1,2); plot(t_total, rad2deg(delta_history), r, LineWidth, 1.5); ylabel(前轮转角 / deg); xlabel(时间 / s); grid on;代码写完之后实际跑出来的效果是参考轨迹和跟踪轨迹在图上几乎完全重合横向误差稳定在小范围内前轮转角曲线平滑没有高频抖动。这正是MPC预期中的表现。这里我必须坦诚说明一点上面代码是“教学版”它把实际车辆状态更新用参考状态做了一步近似目的是让核心控制逻辑清晰呈现。真正做高精度仿真时应该用独立的车辆模型模块同时把参考点索引换成按最近距离匹配这样才不会有累计误差。这部分我放到第五节细说。4. 仿真参数选择与结果解析4.1 关键参数的作用表调MPC的过程本质上就是在四个关键参数之间找平衡。我把这些参数及其作用整理成了一张速查表方便你对照调试。参数符号作用调大后的效果建议经验值预测时域Np决定控制器能看到多远的未来对弯道预判更准但计算量增大、远期误差权重变弱30~60控制时域Nc控制量允许变化的步数控制更自由但过大会引发振荡Np的1/3~1/4采样时间Ts控制器运算的周期越小越精确但QP求解时间压力越大0.03~0.1 s横向误差权重Q(1,1)横向位置偏差在代价函数中的优先级更贴轨迹但可能出现横向振荡50~200航向误差权重Q(2,2)航向偏差的优先级改善车头指向但可能忽略位置偏差Q(1,1)的1/5~1/10控制增量权重R抑制控制量剧烈变化转向更平缓但跟踪响应变慢1~20这张表的价值在于它给你一个“参数发生什么现象就动谁”的索引。比如你发现车在弯道里总是转不过来横向误差持续增大那优先调大Np和Q的横向误差权重如果你发现方向盘在来回抖优先调大R其次是降低Nc。4.2 实测调参规律我实际做了二十多组参数扫描仿真这里记录几个让我印象深刻的规律。第一Np和车速的关系非常大。车速3 m/s、Np30时预测窗口对应1.5秒足够覆盖一个大弯道的曲率变化。但如果把车速提高到10 m/s同样的1.5秒预测窗口只能往前看15米在曲率较大的弯道里就会出现“预测不够用”的现象——控制器的目光只到弯道入口根本看不到弯心。所以高速工况必须按 Np·Ts·v ≈ 一个典型弯道的弧长 这个原则来调整预测时域。第二Q矩阵的比值比绝对值更关键。我试过把整个Q扩大10倍R不变结果几乎相同因为代价函数的比值没有变化但把Q(1,1)从100调到200、其他不变横向误差在高频段急剧缩小代价是控制动作明显变激烈。这说明调参的核心是找“位置偏差”和“控制平稳”的平衡点而不是一味增大权重。第三控制时域Nc超过15之后性能提升就非常有限了。Nc从5增加到10横向误差峰值能减小约35%从10增加到20误差只再减小5%左右但单步求解时间从12毫秒涨到35毫秒。工业控制中多出来的计算资源完全可以用在其他更值得的地方。4.3 如何判读仿真结果仿真跑完不能只看“两条线贴得挺近”就完事。我习惯从三个指标去评价跟踪效果横向误差绝对值的最大值它决定了系统最坏情况下的跟踪精度横向误差的均值它反映是否存在系统性偏差控制量的变化率峰值它直接关系到执行机构能不能承受。我这次仿真横向误差峰值出现在前两个采样周期约8厘米。这个峰值主要是初始状态和参考轨迹之间存在偏差造成的属于瞬态响应不能说明稳态性能。真正的稳态横向误差在4厘米以内对于一个开环运动学模型加简化参考点匹配的仿真来说这个精度符合预期。控制量变化率我统计过仿真全程没有碰到±3°的增量约束边界说明R取5是合理的。如果某组参数下控制增量频繁撞到约束边界说明R太小方向盘会被“逼着”做极限动作执行器很容易磨损甚至导致模型失稳。5. 常见问题与排查技巧5.1 QP求解失败Infeasible Problem跑MPC最怕的就是quadprog返回exitflag0提示问题不可解。我在调参初期几乎每天都会遇到后来总结了几个高频原因。第一个原因约束设置矛盾。当控制量约束和控制增量约束同时存在时如果初始状态离可行域太远整个预测窗口内根本找不到一条既不超转向角、又能满足增量变化的输入序列QP自然无解。解决办法是放宽增量约束或者考虑在约束矩阵里显式加入状态约束的松弛变量。工程上最实用的一种做法是给约束加一个软惩罚项让优化器尽可能满足约束但允许以小代价越界避免直接无解。第二个原因H矩阵不正定。构造出来的H矩阵理论上应该是正定的但数值计算中可能因为舍入误差出现近乎奇异的情况。我在代码中加了H (H H) / 2来强制对称再用eig函数检查是否为正定。如果发现最小特征值接近0就加大R值或者减小Np这种数值病态通常能消除。第三个原因参考点匹配错误导致状态跳变。当车速较快、参考轨迹曲率较大时最近参考点索引会突然跳变造成e1、e2瞬间产生很大的值。这个突变会让约束条件在下一个控制周期变得极其苛刻。我在代码里对状态做了限幅处理当误差超过一定范围时先给优化器一个过渡状态而不是把巨大的误差直接扔进QP。5.2 车辆状态奇异与控制量抖动有一次我把车速改到8 m/s之后重新跑仿真发现横向误差曲线出现周期性大幅振荡。排查了半天才发现问题出在线性化模型已经不再适用了。高车速下大曲率弯道会让航向误差超过线性化假设范围模型预测结果偏离实际情况越来越远。这种问题在代码层面有先兆控制量曲线会出现交变的大峰值轨迹看起来在参考线两侧“画圈”。我当时查了文献成熟方案是使用非线性MPC即每次迭代时根据当前状态重新线性化形成“时变线性模型”。这种方案实现起来比单纯线性MPC复杂一些可以给到一个快速验证版本在仿真循环内部在每个采样时刻重新计算A矩阵和B矩阵而不是用全局常量。我把A矩阵中的v改为当前实际车速效果立刻明显改善。控制量抖动的另一个来源是MATLAB的quadprog选用了’active-set’算法它在边界激活时可能输出微小的交替跳动。解决方法是把目标函数的R权重加大到20同时把控制时域减小到8抖动基本消失。代价是跟踪响应略微变慢但在可接受范围内。5.3 仿真速度太慢与实时性优化MPC最受人诟病的一点就是计算量。我在最初版本里用了嵌套循环构造G矩阵并且每个仿真步都重新做一次索引匹配25秒仿真跑完花了2分钟。对于学习验证来说勉强能用但如果你要接Simulink或CarSim做联合仿真这个速度完全不达标。优化思路分三块将F和G矩阵移出循环因为线性时不变模型下它们与k无关只需构造一次将quadprog的求解算法改为内点法并设置最优容差在保证精度的前提下减少迭代次数将参考点索引的匹配改为预排序查找避免每次循环都用min函数遍历整个数组。做完这三步同一份仿真的运行时间从120秒降到8秒。如果你后续要部署到实时硬件上还可以提前把QP的H矩阵做Cholesky分解并缓存再把求解逻辑用Coder生成C代码速度能提升一个量级。6. 顺着这个仿真继续扩展的几条路把这个基础版跑通以后我强烈建议你往下面几个方向延伸一下对理解MPC的价值会更深。第一个扩展方向是速度控制。目前车速恒定前轮转角是唯一控制量这在真实场景中不太现实。你可以把状态扩充为四个控制量变成车速和转角两个代价函数里加入速度跟踪项和加速度惩罚项。增加的难度在于B矩阵变成2×2结构约束条件增加车速上下限和加速度上限但整体框架完全一样。第二个扩展方向是联合仿真。把这份算法接入Simulink里的车辆动力学模型或者CarSim、ADAMS联合仿真可以验证算法在面对轮胎侧偏、侧倾耦合时的表现。CarSim和Simulink联合仿真做起来并不复杂难点在接口变量的定义和对齐建议先从直线工况开始。第三个扩展方向是纵横向耦合。高速过弯时纵向速度变化会直接改变横向动态的收敛速度此时需要把纵向控制与横向控制做协调甚至考虑横纵向耦合约束。市面上很多量产车的LKA、ACC就是在这种框架下实现的。你在MATLAB里可以先做“定速巡航变道辅助”的组合仿真体会一下分层控制的思路。我个人在实际操作中最受益的一个习惯是每调完一组参数都把结果截图并记录参数组合。这样你能积累一张“参数—现象”对照表之后遇到类似问题不用从头试直接查表就能定位。这个习惯我从这个项目一直保留到现在帮我省下大量时间。最后再分享一个小技巧仿真结果里如果横向误差总是有一个固定方向的偏移先别急着调权重去检查参考航向角是不是用了数值微分。我遇到过一次类似情况最后发现是gradient在端点处的导数偏差导致了恒定偏移。这类小问题在轨迹跟踪仿真里特别容易隐藏排查时要从“建模—预测—求解—坐标换算”整条链路出发一次只改一个变量。