做智能车换道仿真的那段时间我踩过很多规划轨迹很漂亮、控制却跟不住的坑。规划端甩出一条五次多项式换道轨迹横纵向曲线看着都挺顺滑结果放进Simulink里让车辆模型去跟踪要么前轮转角来回抖要么横向误差一直压不下来严重的时候模型直接跑飞。后来把五次多项式换道轨迹规划和MPC轨迹跟踪控制放进同一个Simulink模型里闭环调了几轮又整理出一套带说明文档的版本这套问题才算彻底捋顺。这篇文章就围绕这个Simulink模型本身来写为什么换道轨迹偏偏选五次多项式而不是三次、MPC控制器怎么设计、模型在Simulink里怎么搭、以及我实际调试时踩过的坑和处理链路。适合正在做车辆横纵向控制仿真、或者想快速搭一套有完整说明文档的换道控制Demo的同学参考。1. 为什么换道轨迹规划选了五次多项式而不是三次1.1 三次多项式的边界条件根本不够用换道轨迹的本质是规划一条从当前车道中心过渡到目标车道中心的横移曲线。坐标选择上一般把道路方向作为纵向x垂直道路方向作为横向y然后去设计y关于时间t的函数。很多从教科书开始接触这个问题的同学第一反应是用三次多项式因为形式简单、求解容易。三次多项式只有4个待定系数意味着最多只能满足4个边界条件。换道工况下我们希望起点和终点的横向速度都为零这样车辆切入车道的时候不会带着漂移感可三次多项式把这4个条件用完以后横向加速度已经没得约束了。结果就是起点横向加速度从某个非零值突然出现终点横向加速度又突然被切断。反映到实际跟踪效果上就是前轮转角命令在换道开始和结束的瞬间出现阶跃车身明显抖一下。仿真里还能跑但指标很难看实车上更不可能这么来。1.2 五个系数刚好对应六个边界条件五次多项式的一般形式是y(t) a0 a1·t a2·t² a3·t³ a4·t⁴ a5·t⁵它有6个待定系数刚好匹配换道轨迹的6个边界条件位置速度加速度起点 y(0)0起点 y(0)0起点 y(0)0终点 y(T)d终点 y(T)0终点 y(T)0这里d是车道宽度T是换道总时长。所谓无冲击换道核心就在于起止两端的横向加速度都为0。加速度连续了前轮转角就不会突然跳变后面接MPC的时候控制量也不会一开始就顶在约束边界上。把6个边界条件代入能得到一个6×6线性方程组。手算也不算难但用符号工具更直观% 求解五次多项式系数 syms T d A [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 2 0 0 0; 1 T T^2 T^3 T^4 T^5; 0 1 2*T 3*T^2 4*T^3 5*T^4; 0 0 2 6*T 12*T^2 20*T^3]; b [0; 0; 0; d; 0; 0]; a A\b;因为前3个边界条件都是0解出来a0、a1、a2全部为0只剩下后面三项。很多人一开始不相信结果这么简单我直接给结论a3 10·d / T³a4 -15·d / T⁴a5 6·d / T⁵也就是说整条换道轨迹的横向位移、横向速度、横向加速度全部由d和T两个参数决定。这个结论写进说明文档以后后面调参只需要动两个数非常省事。1.3 换道时长T和横向偏移d的工程取值d一般取车道宽度城市道路常见的是3.5m高速路肩和车道线之间按3.75m算也可以按项目需求自定义。T的选取则直接决定换道过程的剧烈程度。T越短轨迹越急横向加速度峰值越大T越长轨迹越平缓但过长的换道在高速场景下反而会让人感觉车辆犹豫。我在仿真里常用的范围是2.5s到4s。以d3.5m、T3s为例横向加速度峰值大约在0.5m/s²量级远低于人体舒适度阈值乘坐体验是OK的。如果T压到2s以内峰值加速度会明显变大跟踪误差也更难控制。提示换道轨迹生成出来以后第一件事不是接控制而是直接把横向加速度曲线画出来看一眼。峰值超过1m/s²说明T给得太小后面控制器调得再好也白搭。1.4 从横向轨迹合成完整的参考轨迹序列只算出一组多项式系数还不够MPC需要的是未来若干个采样周期内的参考坐标和参考航向。通常的做法是设定纵向速度v先假设匀速横向轨迹关于时间t的多项式按控制周期Ts生成时间序列t_k 0:Ts:T计算每个时刻的横向偏移y、横向速度dy/dt、横向加速度d²y/dt²纵向位置x_k v·t_k参考航向yaw_ref ≈ atan2(dy/dt, v)。如果车速变化不大小角度下也可以用dy/dt除以v来近似参考曲率kappa_ref (d²y/dt²) / v²这个值后面给前馈转角用。生成的结果可以在MATLAB里打印成一张图横向位置是平滑S形曲线横向速度从0增大再减小到0横向加速度是一个完整的先正后负波形首尾都没有跳变。看到这样的曲线我才敢说轨迹规划这部分是合格的。2. MPC跟踪控制器的模型设计与参数标定逻辑2.1 为什么换道跟踪选MPC而不是PID或纯跟踪横向控制里经常提的纯跟踪Pure Pursuit、Stanley和PID在换道工况下都能跑但各有明显短板。纯跟踪本质是几何跟踪它对路径曲率的响应有滞后换道中段容易切内PID则没有预测能力也不方便处理前轮转角幅值和变化率的硬约束。MPC的核心优势是两件事一是看得远它会沿着预测时域Np往前看未来一段轨迹提前对弯道或换道拐点做响应二是有约束前轮转角限幅、转角变化率限幅可以直接写进优化问题不会像PID那样调大增益后控制量飙到天上去。代价也很明显每次控制周期要在线求解一个二次规划问题计算量比PID大得多。但在Simulink仿真环境里这一步完全可接受而且换道场景的模型规模不大Np取到20~30优化变量也就十来个quadprog轻松能算。2.2 运动学自行车模型的推导与离散化MPC需要一个预测模型来推演未来状态。换道仿真里最常用的是运动学自行车模型也叫几何单车模型。状态量取[x, y, yaw]控制量取[v, delta]。连续时间状态方程是dx/dt v·cos(yaw)dy/dt v·sin(yaw)dyaw/dt v·tan(delta) / L其中L是轴距。低速工况一般指车速低于20m/s下这个模型能较好反映车辆运动趋势不用考虑轮胎侧偏角参数只有一个轴距L非常容易标定。离散化一般用欧拉法就够了控制周期Ts在0.02~0.05s时精度损失很小。核心代码就三行function [x_next, y_next, yaw_next] bicycle_step(x, y, yaw, v, delta, dt, L) x_next x v * cos(yaw) * dt; y_next y v * sin(yaw) * dt; yaw_next yaw v * tan(delta) / L * dt; end提示如果车速比较高或者想模拟更接近实车的行为最好把轮胎侧偏刚度加进去替换成动力学自行车模型。但这个模型内部状态更多参数标定成本也更高不是第一次做换道Demo时的最优选择。2.3 线性时变MPC的优化问题形式直接拿非线性模型做优化太复杂。工程上更常见的是线性时变MPC在每个控制周期根据当前车辆状态和参考轨迹点把非线性模型在参考点附近做一阶泰勒展开得到误差线性化模型然后在这个误差模型上构造二次规划问题。目标函数是这样的形式min J Σ(k1→Np) [ Q_y·(e_y,k)² Q_yaw·(e_yaw,k)² ] Σ(k0→Nc-1) [ R_delta·(δ_k)² R_ddelta·(Δδ_k)² ]约束条件包括约束类型实际含义常用范围前轮转角幅值δ_max限制转向极限30°左右控制增量幅值Δδ_max限制转向速率0.3~0.5 rad/步可选横向偏差限制车辆偏离参考轨迹过远根据车道设定这里Q_y的单位是每米偏差的权重Q_yaw是每弧度偏差的权重因为量纲不同数值数量级差异很大。单位问题往往是初次调参失败的最大原因。2.4 别忘了前馈转角MPC不是万能的调试时我发现一个有意思的现象纯用MPC反馈也能跟踪换道轨迹但航向误差总是慢半拍尤其在换道中段参考曲率最大的地方横向误差容易被甩出来。原因不复杂MPC毕竟是反馈控制它要等偏差出现以后才会出手。解决办法是在MPC反馈控制量之上叠加一个前馈转角。前馈转角可以由参考曲率直接反算δ_ff arctan(L · kappa_ref)然后把MPC优化出来的控制量只当作修正量δ_cmd δ_ff δ_mpc加上这路前馈以后跟踪精度提升非常明显。横向误差峰值通常能降低一半以上而且MPC的压力小了很多控制增量约束很少被触发。这个组合思路我后来也一直保留写进了说明文档。2.5 参数推荐表从这套模型可以直接起步我给出一组我自己仿真验证过的参数适合轴距约2.7m、车速15~20m/s的乘用车换道场景参数推荐值说明控制周期Ts0.02~0.05s越小控制越细但计算量变大预测时域Np25~30对应1s左右的前瞻时间控制时域Nc4~6比Np小很多减少优化变量Q_y5~10横向偏差权重Q_yaw50~100航向偏差权重注意弧度量纲R_delta0.1~1前轮转角权重调响应速度R_ddelta10~30转角变化率权重抑制抖振注意一点Q_yaw取几十甚至上百看着吓人但这是合理的。因为航向偏差的数值通常只有0.01~0.05弧度如果不加大权重在目标函数里它对总代价的贡献远小于以米为单位的横向偏差。3. Simulink模型架构梳理信号流、步长与接口设计3.1 顶层模块划分这个模型推荐用四个大模块来组织层级清晰后面出问题也好定位。Trajectory_Planner负责根据当前状态和换道指令生成参考轨迹MPC_Controller负责完成状态误差计算、QP求解、前馈叠加Vehicle_Model负责接收控制量用自行车模型更新车辆状态Data_Logger负责把关键信号记录下来方便后处理画图。模块间信号用Bus总线组织不要到处用Goto/From。最开始我贪方便用了Goto和From结果总线信号一多模型根本没法查一个信号来自哪都找不到。后来全部改成Bus每个模块的输入输出接口一眼就能看出来。顶层接口可以设计成一张表格写进说明文档里模块输入信号输出信号Trajectory_Planner当前时间、换道触发标志、当前车速ref_t, ref_x, ref_y, ref_yaw, ref_kappaMPC_Controller车辆状态[x,y,yaw]参考窗口delta_cmd, infoVehicle_Modeldelta_cmd, vx, y, yawData_Logger所有信号工作区变量3.2 固定步长与求解器设置Simulink配置里求解器务必选固定步长。我一般用ode4经典四阶龙格库塔步长设成0.01s。原因是控制器本质是离散的如果求解器用变步长控制器的触发时刻会变得不规律MPC预测时域对应的参考点序列就容易错位。同时要小心代数环。MPC控制器输出的delta_cmd进入车辆模型车辆状态又反馈回MPC控制器这是一个明显的闭环回路。如果模块内部某个环节没有存储状态Simulink会为了解代数环去迭代计算速度变慢不说还可能出现奇怪的非因果现象。我的做法是在反馈状态进入MPC控制器之前加一个Unit Delay让反馈延迟一个控制周期。这样做的代价是等效控制延迟一个Ts但换来的是仿真稳定性和可解释性。3.3 轨迹规划模块滑动参考窗口的实现轨迹规划的职责不是只生成一条完整轨迹而是随时给出MPC需要的未来一段时间窗口的参考点序列。预生成轨迹时把整条换道轨迹按控制周期Ts离散成一个N×5的矩阵包含时间、x、y、yaw、kappa。到运行时MPC控制器根据当前仿真时间t_now在这个矩阵里找到对应索引然后往后截取Np个点。如果轨迹剩余点数不足Np就用车道中心直线补齐而不是用零填充。function refs get_ref_window(traj, t_now, Np) idx find(traj.t t_now, 1, last); if isempty(idx) idx 1; end remain numel(traj.t) - idx 1; if remain Np refs traj.t(idx:idxNp-1, :); else refs [traj.t(idx:end, :); extend_straight(traj, traj.t(end)traj.Ts, Np-remain)]; end end这一步是当初最容易被忽略的地方。轨迹规划的起始时间要和仿真时间对齐否则查出来的参考窗口根本不对。3.4 MPC控制器模块MATLAB Function里的QP求解MPC控制器用一个MATLAB Function模块实现输入当前车辆状态和参考窗口输出前轮转角。内部逻辑分成四步根据线性时变模型构建预测矩阵计算参考窗口内未来时刻的状态误差组装成标准二次规划问题调用quadprog求解取解的第一帧叠加前馈转角输出delta_cmd。为了讲解方便模型里用quadprog这样一个通用求解器代码不超过一页A4纸。缺点是每次调用有函数调用开销但仿真时完全可接受。如果后续要做实时运行或硬件在环再换成Hildreth投影算法或OCP专用求解器也不迟。3.5 车辆模型与数据记录车辆模型直接用S-Function或者MATLAB Function实现自行车模型输入是控制量delta_cmd输出是x、y、yaw三个状态。不做额外封装方便看到全部内部实现。Data_Logger里用几个To Workspace模块记录仿真时间、横向误差、航向误差、前轮转角命令。仿真结束以后跑一句脚本就能画出一组四个子图的曲线每次调参都靠这张图判断。4. 闭环调试实录从发散到稳定的完整排查链路4.1 案例一模型一跑就发散问题出在轨迹查询相位现象特别典型按下仿真启动车辆不是去换道而是直接朝某个方向冲出去横向误差达到几十米整个曲线完全没法看。一开始怀疑MPC控制器里的参数错了但单独测试控制器又没有问题。后来把Trajectory_Planner的输出单独画出来发现参考轨迹的时间戳和仿真时间对不上。原因是轨迹生成模块从仿真启动时刻就开始计时而参考轨迹内部的换道起始时间却是0。仿真运行到2s时触发换道可查询函数还在找t0附近的参考点等于MPC从一开始盯着的就是错误的参考片段。修复方式是在轨迹生成模块里维护一个起始时间戳查询时用当前仿真时间减去这个起始时间再换算到轨迹内部时间轴。模型瞬间就稳定了下来。4.2 案例二换道结束以后横向误差来回抖动模型能跑通以后又出现一个新问题换道完成以后车辆本来应该稳稳待在目标车道中心但横向误差总在±0.1m附近来回振荡好几秒都消不掉。排查后确认问题出在参考窗口的尾部。换道轨迹在T3s处就结束了但MPC预测时域Np30乘以Ts0.05s意味着控制器要往前看1.5s。轨迹剩余点数不够查询函数用零来填充结果就是预测窗口的后半段一股脑地指向原点和真实目标车道中心差了两三个车道MPC自然拼命往错误方向拉。解法是把参考轨迹延长换道结束后接一段足够长的目标车道直线让预测窗口在任意时刻都能看到真实有效的参考点。振荡问题直接消失尾部误差收敛到厘米级。4.3 案例三quadprog太慢仿真速度比真实运行还慢把Np从20提到30以后仿真速度明显变慢一个6秒的换道工况仿真时间却跑了一分多钟。排查后确认瓶颈在MATLAB Function里反复调用quadprog。这个函数每次调用都有较大的解析和检查开销虽然优化问题本身很小但架不住每个控制周期都调一次。后来在说明文档里给了两个方向的解决方案一个是把控制时域Nc从10降到5优化变量少了一半求解耗时明显下降另一个是换成Hildreth投影法代码量不大但每个周期只是几十次简单的向量循环速度提升非常明显。对这种教学型模型尤其推荐后者因为它不需要额外的工具箱授权可移植性也更好。4.4 调Q/R权重的一个实用套路Q和R权重没有万能组合但调参可以按一套顺序来不用瞎试。第一阶段先保证稳定把Q_y设成1Q_yaw设成1R_delta和R_ddelta都设成0.1这时候车辆通常能跟上轨迹但会有明显的过冲或振荡。第二阶段提高Q_yaw到50以上抑制航向偏差横向误差曲线会变得更贴参考轨迹。第三阶段再根据响应速度调节R_delta。响应太慢就减小R_delta抖振严重就加大R_ddelta。我习惯把不同权重组合下的仿真曲线叠在一张图里非常直观。调参记录写进说明文档后面改工况时能省很多时间。现象调整方向横向过冲大增大Q_yaw或增大R_delta响应慢弯内跟随不足增大Q_y或减小R_delta控制量高频抖振增大R_ddelta或适当增加Nc稳定横向偏差大检查前馈转角而不是盲目加大Q4.5 验证方法不只是跑一条直线换道模型调通以后我用双移线工况做了额外验证。将五次多项式轨迹拼接成两段连续换道中间加一段直线段跑出来的横向误差峰值依然能控制在5cm以内前轮转角始终在约束范围内。这个验证的意义在于证明MPC控制器不依赖单条轨迹的特殊性换一组起点和终点控制器依然有效。后续如果要做实车或更复杂工况这套验证流程是绕不开的。5. 说明文档里值得记录的关键内容5.1 模型文件的使用顺序说明文档第一件事是告诉拿到模型的人从打开到出结果的完整顺序。我用的是打开init_params.m检查参数设置运行该脚本把参数加载到工作区打开main_model.slx点击Run运行仿真运行plot_results.m画出跟踪误差和控制量曲线。顺序写清楚以后别人拿到的就不是一个黑盒子而是可以复现的学习资料。5.2 参数表与接口清单说明文档里必须包含一张完整的参数清单至少覆盖下面这些参数推荐值含义注意点L2.7m车辆轴距换车需同步改v_ref15~20m/s纵向速度高速需换动力学模型lane_width3.5m目标车道偏移按实际道路设置T_change3s换道时长直接影响加速度峰值Ts0.02~0.05s控制周期影响预测窗口长度Np25~30预测时域越大越激进但更慢Nc4~6控制时域减小可加快求解delta_max0.5rad前轮转角限幅按执行器设定接口清单就是3.1那张表但说明文档里要更详细地标注每个信号的单位和含义方便别人二次开发。5.3 模型的适用范围与假设条件这部分最容易被初学者忽略但对使用者最重要。这套模型的适用边界是车速建议低于25m/s运动学模型在高速下预测精度下降假设车辆在平整路面行驶没有坡度、侧风、轮胎非线性执行器理想化前轮转角响应无延迟仅在幅值和变化率上有限制MPC预测模型是参考轨迹点附近的线性时变模型适合曲率变化不剧烈的换道场景。把这些假设写清楚使用者就不会在高速大侧偏工况下拿这个模型硬套然后反过来怀疑代码有问题。5.4 可以继续扩展的三个方向说明文档最后我一般会留一节扩展方向给拿到模型的人指路方向一是把纵向速度规划也做起来。五次多项式同样可以用来规划纵向速度曲线再把纵横向轨迹联合输出MPC控制量从[v,delta]变成[a,delta]就是完整的横纵向协同换道。方向二是升级预测模型。把运动学自行车模型替换成带轮胎侧偏刚度的动力学模型MPC预测矩阵变成线性时变动力学形式这样场景可以扩展到高速工况。方向三是把MPC求解器替换成嵌入式可用的实时求解器。这个方向对想走向实车部署的同学很有价值代码框架不需要大改只换求解器内核即可。最后说一个我自己保留到现在的习惯换道轨迹生成以后先不急着接MPC先把它单独画出来看看横向加速度峰值是否在合理区间曲率序列是否平滑。这个步骤只要10秒钟却能在后期挡掉一大堆控制器参数没调好的误判。这个Simulink模型我反复用了很长时间每次给新同学讲解也都是从轨迹生成的那张图开始而不是直接打开MPC控制器。控制问题很多是轨迹问题——轨迹给得不合理再好的控制律也救不回来。