企业官网建设流程全解析

换道场景一直是自动驾驶控制算法验证的试金石。前一阵我完整过了一遍这个“五次多项式换道轨迹规划 + MPC轨迹跟踪控制”的Simulink模型，带着说明文档跑下来，整体链路非常清晰，从轨迹生成到闭环跟踪一步到位。这篇博文我不打算做流水账式的模型介绍，而是把这套方案里涉及的核心算法原理、Simulink模块搭建逻辑、参数怎么调、仿真时容易踩什么坑，结合我实际跑模型的经验一次讲清楚。不管你是刚接触轨迹规划的研究生，还是在做车辆控制落地的工程师，这篇文章都能给你一份可以照着复现的参考。

先说结论：这个模型本质上是一个分层式控制架构，上层用五次多项式生成一条平滑的换道参考轨迹，下层用MPC（模型预测控制）设计跟踪控制器，让车辆沿着参考轨迹完成换道。Simulink作为仿真载体，把规划、控制、车辆动力学三个环节串成了闭环。用这套组合的理由也很直接——五次多项式能保证轨迹的连续性和舒适性，MPC能显式处理转向角、加速度等约束，两者搭配在换道这种强约束、时变场景下非常合适。

1. 项目整体拆解：五次多项式换道 + MPC跟踪的系统架构

1.1 这个模型到底在解决什么问题

先说清楚这套模型所在的问题域。换道是自动驾驶中典型的高风险场景，完整的决策链路是“感知 → 决策 → 规划 → 控制”。这个模型覆盖的是其中两个关键环节：规划层负责生成一条从当前车道到目标车道的参考轨迹，控制层负责让车辆实际跟上去。

但这里有个容易被忽视的点：规划和跟踪往往是两层皮。很多初学者把轨迹规划单独做一遍仿真，曲线很漂亮，一到闭环跟踪就翻车。原因在于规划层只给了位置点序列，没有给速度、航向角等运动状态信息，或者轨迹本身曲率不连续，跟踪控制器很难稳定跟上。这个模型的优势就在于，它把五次多项式规划的轨迹输出做了完整定义，位置、速度、航向角都有解析表达式，MPC跟踪时能直接用这些信息计算误差和预测，闭环自然顺利很多。

从我实际使用体验来说，这个模型把“规划”和“控制”放在同一个Simulink工程里，信号流直观，调试效率很高。对比之前我见过的一些把规划写成单独脚本、控制又放在另一个工程里的方案，这个模型的综合性和可复现性明显更好。

1.2 为什么用五次多项式规划换道轨迹

换道轨迹规划的数学本质，就是在给定初始状态和终端状态的条件下，找一条满足约束的平滑路径。常见的做法有多项式曲线、贝塞尔曲线、样条曲线等。其中多项式曲线因为计算简单、解析性好，使用最广泛。

三次多项式是很多教材里的入门方案，它能约束位置和速度的边界条件，但加速度的连续性没有保证。换道场景下，加速度突变会直接影响乘坐舒适性，严重时会让跟踪控制器产生剧烈抖动的控制输出。五次多项式多引入了两个自由度，能够同时约束位置、速度、加速度六个边界条件，恰好对应换道场景中“起终点位置确定、起终点速度为0、起终点加速度为0”的完整约束需求。

这里需要强调一下，所谓“五次多项式”通常指的是多项式最高次数为5次的函数：
d(t) = a0 + a1t + a2t² + a3t³ + a4t⁴ + a5*t⁵

在换道场景中，一般用这个多项式来表示车辆的横向位移随时间的变化关系。6个系数对应6个边界条件，构成了一个闭式可解的问题，不需要迭代优化，实时性很好。这也是它在Simulink仿真和实车应用中都很受欢迎的原因。

1.3 为什么用MPC做轨迹跟踪

轨迹跟踪控制的经典方法包括纯跟踪（Pure Pursuit）、PID、LQR和MPC。不同方法的取舍逻辑简单说一下。

纯跟踪算法本质是基于几何关系的横向控制，实现简单、参数少，但它只以前视点为目标，对前方轨迹的变化响应不够主动，高速换道场景下容易产生超调和振荡。

PID控制更直白，只基于当前误差做反馈调整，根本不知道前方轨迹即将怎么变化，在弯道和换道这种轨迹曲率剧烈变化的场景里，控制效果受限于增益整定，很难同时平衡动态响应和稳态精度。

LQR（线性二次型调节器）是一种基于状态空间的最优控制方法，它能综合考虑状态误差和控制代价，设计了反馈增益矩阵K。LQR的问题在于它本质上是一个无约束的线性控制器，无法显式考虑转向角极限、加速度极限这些物理约束。

MPC的核心优势恰恰在约束处理上。它在每个采样时刻基于当前状态和预测模型，向前滚动优化一段时域内的控制序列，目标函数里同时包含跟踪误差的惩罚和控制量的惩罚，并且把执行器约束、状态约束直接写入优化问题中。换道场景下，转向角有物理极限，横向加速度受舒适性约束，这些MPC都能在求解时直接考虑进去，这是它胜出的关键原因。

打个比方，PID就像一个只看眼前一步的人，MPC则是一个手里拿着地图、能提前规划好几步路线的人。遇到前方有弯道要提前打方向的时候，两者的表现有本质区别。

2. 核心算法原理：从数学公式到工程落地

2.1 五次多项式换道轨迹的推导

这一节我把五次多项式的推导过程完整走一遍，这是理解整个模型的关键起点。

首先明确边界条件。假设车辆在t=0时刻开始换道，在t=T时刻完成换道。横向位移用d(t)表示，它的含义是车辆相对车道中心线的横向偏移。边界条件为：

初始时刻：
d(0) = d0（初始横向偏移，通常为0）
d'(0) = v0（初始横向速度，通常为0）
d''(0) = a0（初始横向加速度，通常为0）

终端时刻：
d(T) = dT（目标车道的横向偏移，通常为一个车道宽度，即3.5m左右）
d'(T) = vT（终端横向速度，通常为0，保证平稳进入目标车道）
d''(T) = aT（终端横向加速度，通常为0，保证乘坐舒适）

把d(t)表达式代入这些边界条件，可以得到一个线性方程组。写成矩阵形式是：

| 1 0 0 0 0 0 | | a0 | | d0 | | 0 1 0 0 0 0 | | a1 | | v0 | | 0 0 2 0 0 0 | | a2 | | a0 | | 1 T T² T³ T⁴ T⁵ | | a3 | | dT | | 0 1 2T 3T² 4T³ 5T⁴ | | a4 | | vT | | 0 0 2 6T 12T² 20T³ | | a5 | | aT |

对于最常见的换道场景——初始和终端的状态都是静止的，即d0=0，v0=0，a0=0，vT=0，aT=0——方程组可以大大简化：

d(t) = a3t³ + a4t⁴ + a5*t⁵

dT = a3T³ + a4T⁴ + a5T⁵
0 = 3a3T² + 4a4T³ + 5a5T⁴
0 = 6a3T + 12a4T² + 20a5*T³

手工解这个方程组的技巧是先把中间表达式变换成标准形式，消元后可以直接得到系数表达式：

a3 = 10dT / T³
a4 = -15dT / T⁴
a5 = 6*dT / T⁵

这三个系数公式是这个模型里最核心的数学结论。

2.2 一个具体的换道轨迹计算实例

理论推导完成后，我带着参数实际算一遍，确保大家能照抄。设定场景为一个标准的高速公路换道：

• 车道宽度：3.5m，即dT = 3.5
• 纵向速度：20m/s（约72km/h）
• 换道时长：T = 6s

代入系数公式：

a3 = 10 * 3.5 / 6³ = 35 / 216 ≈ 0.162
a4 = -15 * 3.5 / 6⁴ = -52.5 / 1296 ≈ -0.0405
a5 = 6 * 3.5 / 6⁵ = 21 / 7776 ≈ 0.0027

因此横向轨迹方程为：
d(t) = 0.162t³ - 0.0405t⁴ + 0.0027t⁵

对这个轨迹求导，可以算出换道过程中的最大横向速度。从物理直觉判断，最大横向速度出现在轨迹中段，计算结果是大约每秒0.63m的横向速度，横向加速度峰值出现在换道初期和末期，约为0.18m/s²。这个数值远低于人体舒适度阈值（通常认为0.5g即约4.9m/s²以下舒适），说明6秒的换道时长在舒适性上是安全的。

上面说的是横向轨迹，纵向运动通常保持匀速，即x(t) = vx * t，当然也可以把五次多项式用在不同坐标系中。在实际Simulink模型中，输出给MPC的参考轨迹往往是[x_ref, y_ref, psi_ref, v_ref]的组合，把横向、纵向轨迹合成为车辆坐标系下的位置序列。

2.3 MPC轨迹跟踪的预测模型与优化问题

MPC的核心要素有三个：预测模型、目标函数、约束条件。我这里把每个要素在换道跟踪场景下的具体形式展开说明。

预测模型在大多数车辆控制工程中采用自行车模型，也就是把车辆简化为前轮、后轮两个轮子，忽略左右轮的差异。运动学形式的状态方程可以写为：

x(k+1) = x(k) + v(k) * cos(psi(k)) * Ts
y(k+1) = y(k) + v(k) * sin(psi(k)) * Ts
psi(k+1) = psi(k) + v(k) * tan(delta(k)) / L * Ts
v(k+1) = v(k) + a(k) * Ts

状态量是X = [x, y, psi, v]，分别代表纵向位置、横向位置、航向角、速度。控制量是U = [delta, a]，分别代表前轮转角、加速度。Ts是采样周期，L是车辆轴距。

目标函数在换道跟踪场景下，通常需要对以下四个方面做权衡：

• 与被控车辆的横向偏差
• 航向角误差
• 控制输入幅值（转向角、加速度）
• 控制输入变化率

标准形式为：
J = Σ (X_ref - X)^T * Q * (X_ref - X) + U^T * R * U

Q矩阵是对状态误差的惩罚权重，R矩阵是对控制量的惩罚权重。调参的逻辑后面我会专门讲。

约束条件方面，前轮转角delta通常受限，比如±30度，这个来源于车辆转向机构物理极限；控制加速度a受限在[-3m/s², 2m/s²]左右，这是舒适性约束和动力性能的折中。

每个采样时刻，MPC求解器在预测时域N_p内求解这个带约束的二次规划问题，得到最优控制序列，然后只执行第一个控制量，下一个采样时刻再重新求解，这就是“滚动优化”的精髓。预测时域的引入让控制器有前瞻性，这是MPC相对LQR等无约束最优控制的本质优势。

3. Simulink模型搭建与模块配置实操

3.1 模型整体架构和仿真信号流

打开这个Simulink模型，第一眼看到的是清晰的模块分区布局。完整模型从信号流角度可以拆成四块：参考轨迹生成模块、MPC控制器模块、车辆动力学模型模块、示波器及数据记录模块。

• 参考轨迹生成模块：内部是MATLAB Function或者子系统，输入是换道触发信号，输出是参考轨迹的时间序列，包含x_ref、y_ref、psi_ref、v_ref。
• MPC控制器模块：输入是当前车辆状态（实际位置、航向、速度）和参考轨迹状态，输出是控制量delta和a。
• 车辆动力学模型模块：这里用的是带执行器动力学的运动学自行车模型，输入控制量，输出车辆状态。
• 数据记录模块：主要是Scope和To Workspace模块，用于观察跟踪效果。

仿真中的信号连续性是关键。MPC控制器和车辆模型都使用固定步长求解器运行，我在实际运行时把步长设为了0.1s，MPC的预测时域设为10步（即向前预测1秒），这个组合在跟踪精度和求解速度之间取得了很好的平衡。

3.2 轨迹规划模块的实现细节

轨迹规划模块内部用MATLAB Function实现最方便，因为五次多项式系数的解析式计算量小，也便于调试。这个Function的核心输入输出逻辑非常清晰：

function [x_ref, y_ref, psi_ref, v_ref] = traj_planner(t, t_start, T, vx, dT) % 输入： % t：当前仿真时间 % t_start：换道起始时刻 % T：换道持续时长（如6s） % vx：纵向速度（假设恒定） % dT：总横向偏移（如3.5m） tt = t - t_start; % 相对换道起始时刻的时间 % 五次多项式系数（初始和终端横向速度、加速度均为0） a3 = 10 * dT / T^3; a4 = -15 * dT / T^4; a5 = 6 * dT / T^5; if tt < 0 d = 0; d_dot = 0; elseif tt <= T d = a3 * tt^3 + a4 * tt^4 + a5 * tt^5; d_dot = 3*a3*tt^2 + 4*a4*tt^3 + 5*a5*tt^4; else d = dT; d_dot = 0; end x_ref = vx * tt; y_ref = d; psi_ref = atan2(d_dot, vx); % 航向角近似 v_ref = vx; end

这里有个实操经验值得强调：航向角的计算。很多初学者直接用y_ref对x_ref的导数来算航向角，但因为在换道场景下纵向速度是常量、横向速度在变化，更稳妥的做法是直接根据横向速度和纵向速度的比值，调用atan2(d_dot, vx)算出航向角。这个细节直接影响MPC中航向误差的计算精度。

3.3 MPC控制器的两种实现方式

MPC控制器在Simulink中有两种常见实现路线：使用官方MPC Toolbox模块，或者自己编写MATLAB Function求解器。

方式一：MPC Toolbox模块
这种方式适合快速验证。你只需要在MATLAB工作空间里提前用mpc命令建立控制器对象，指定被控对象模型、预测时域Np、控制时域Nc、约束条件和权重矩阵，然后在Simulink里拖一个MPC Controller模块，关联对应的模型变量即可。

典型代码段如下：

% 建立线性被控对象模型（在换道跟踪中通常是线性化的车辆模型） Ts = 0.1; A = [...]; % 状态矩阵 B = [...]; % 控制矩阵 C = [...]; D = 0; plant = ss(A, B, C, D, 'Ts', Ts); % 创建MPC对象 mpcobj = mpc(plant, Ts); mpcobj.PredictionHorizon = 10; mpcobj.ControlHorizon = 3; % 定义约束 mpcobj.ManipulatedVariables(1).Min = -0.5; % 转向角下限 mpcobj.ManipulatedVariables(1).Max = 0.5; % 转向角上限 mpcobj.ManipulatedVariables(2).Min = -3; % 加速度下限 mpcobj.ManipulatedVariables(2).Max = 2; % 加速度上限 % 设置权重 mpcobj.Weights.OutputVariables = [1 1 0.1 0.1]; % 对应 [x y psi v] mpcobj.Weights.ManipulatedVariables = [0.1 0.1];

用MPC Toolbox的最大好处是内置了求解器，数值稳定性有保证。但它的控制器对象是建立在线性模型基础上的，如果车辆模型非线性较强，直接用线性MPC性能可能打折扣。

方式二：自定义MATLAB Function实现非线性MPC
这种方式更适合研究用途和换道场景下的非线性控制。核心思想是在MATLAB Function内调用fmincon或quadprog求解优化问题。需要注意的一点是，Simulink中MATLAB Function内的代码在仿真时默认会被解析编译，直接调用外部求解器函数会报错，需要用coder.extrinsic声明。

function [delta, a] = mpc_controller(x, y, psi, v, x_ref, y_ref, psi_ref) coder.extrinsic('fmincon'); % 声明外部函数 % 定义优化变量初值、上下界等 % ... % 调用fmincon求解 u_opt = fmincon(@obj_fun, u0, [], [], [], [], lb, ub, [], options); delta = u_opt(1); a = u_opt(2); end

自定义方式的好处是预测模型可以用完整的非线性自行车模型，控制器对车辆在大曲率、高速工况下的表现会更好。代价则是实时性更难保证，需要仔细调整优化器的最大迭代次数和容差。

从我实际调Simulink模型的经验来看，如果模型里已经给定了MPC模块，先基于MPC Toolbox把整个闭环跑通，确认规划层和控制层信号衔接无误后，再考虑是否替换成自定义求解器版本。不要一上来就用自定义代码，那会在调试硬件约束和求解器参数时消耗大量精力。

4. 参数调优与避坑指南

4.1 换道轨迹参数怎么定才合理

换道轨迹的参数主要是换道时长T和纵向速度vx。这两个参数直接决定了轨迹的最大横向速度、最大横向加速度和换道距离，因此它们决定了乘坐舒适性和安全性。

换道时长T在5-8秒之间是工程上的常见选择。T过小（比如小于4秒），最大横向加速度迅速增大。计算一下就会发现，T从6秒降到4秒，最大横向加速度会增大到原来的2.25倍（因为最大横向加速度正比于dT/T²）。T过大虽然舒适性提高，但换道时间长，在真实交通中会给出更大的暴露风险窗口期，而且影响通行效率。

纵向速度vx也不宜过快偏离巡航车速。如果vx在换道过程中是恒定值，那么换道距离等于vx*T，速度越高换道纵向距离越长，对前方空间的判断就要越保守。

还有一个容易忽略的参数：参考轨迹输出频率。如果Simulink模型不是用固定步长，而参考轨迹生成模块的采样率与MPC控制器不一致，会导致时序错位。建议将整个模型设为固定步长，步长与MPC采样周期一致或成整数倍关系。

4.2 MPC权重矩阵调参经验

MPC调参是使用这个模型时最花时间的环节。权重矩阵Q和R的相对比例决定了控制器的行为倾向。这部分我给出一个实践中有效的调参顺序和默认起点。

先说一个典型误区：新手拿到模型后，把输出权重全部设成1，控制权重也设成1，然后发现跟踪效果很差，就不知所措。实际上Q和R的绝对值并不重要，重要的是比例关系。

一个合理的默认起点是：

• Q中横向位置误差的权重设为10，纵向位置误差权重设为1
• 航向角误差权重设为5
• 速度跟踪误差权重设为1
• R中转向角权重设为100，加速度权重设为10

为什么转向角权重比加速度权重大这么多？因为转向角的变化直接影响车辆的横摆运动，一个很小的转向角变化就能产生可观的航向改变，如果不给它施加较大的惩罚，MPC求解出的转向角序列会非常激进，导致车辆蛇形运动。

调参时遵循的最小可行组合是：先调横向位置权重和转向角权重，让车辆的横向跟踪收敛；再加入航向角误差权重消弭稳态误差；最后微调加速度权重，抑制控制量抖动。整个过程中参考一个关键指标：横向跟踪误差绝对值是否在0.1m以内。

另外，预测时域Np的选取也有规律。Np太小，控制器短视，对于换道这种大曲率变化的轨迹会出现明显的滞后；Np太大，计算量上升，而且超出系统动态响应范围的预测意义不大。经验上是让Np * Ts 覆盖换道时间的1/4到1/3，比如换道6秒，Np*Ts在1.5到2秒之间比较合适。

4.3 仿真常见问题速查表

我在运行这个模型的过程中遇到了几个典型问题，也看到不少同学踩过相似的坑。这里整理成速查表供大家参考。

仿真遇到发散问题时，先不要急着大量调整参数。一个高效的复现方法是：先手动运行几次，检查参考轨迹模块输出的位置曲线是否连续平滑。如果规划层输出本身有突变，跟踪控制器无论怎么调都会出问题。

还有一点值得单独提醒：在Simulink模型中修改MPC参数时，修改完成后一定要重新运行初始化脚本或重新调用mpc命令，确保工作空间中的MPC对象更新到最新参数值。这个细节被很多人忽略，导致模型界面里改了参数但仿真没有任何变化，误以为是参数方向选错了。

结尾

跑完整套模型后，我个人最深的体会是：换道轨迹的平滑性直接决定跟踪控制的难度，规划层多投入一份精心的参数设计，控制层的压力就小很多。五次多项式带来的加速度连续性，以及MPC对转向角和加速度约束的显式处理，这两者缺一不可。如果后续想在这个模型基础上扩展，推荐两个方向：一是把运动学自行车模型替换为带轮胎侧偏特性的动力学模型，在更高车速的工况下检验跟踪效果；二是把车道宽度的动态变化引入轨迹规划，考虑多车道连续换道的场景。这两个方向都能让这套基础模型往更贴近实际的道路测试方向靠拢。