1. 项目整体拆解：从标题读懂这套轨迹跟踪仿真到底在做什么
刚看到这个标题的时候，我的第一反应是：这大概率是车辆横向控制课程作业或某个仿真课题的经典组合——MPC、PID、Stanley，外加一个"二自由度动力学模型"作为被控对象。标题里PID出现了两次，我实际搭建的时候把其中一个作为横向PID（方向控制），另一个作为纵向PID（车速保持）来处理。如果你只打算做横向控制对比，也可以把第二个PID看成一个带前馈的变体版本，或者直接用纵向速度环来补位，结论不受影响。
这套东西的核心任务很明确：让一辆车沿着给定参考轨迹走，且行驶过程中横向误差和航向误差尽可能小。二自由度模型不是指"前后两个自由度"，而是忽略了悬架、垂向运动等因素，只保留车辆的侧向运动和横摆运动两个自由度，也就是大家常说的自行车模型——前轮一个等效转向角，后轮固定，车辆绕垂直轴旋转。用这个模型做轨迹跟踪控制器设计，计算量小、物理概念清晰，尤其在车速不高、侧向加速度不大的工况下，精度足够接近真实车辆响应，这也是为什么它成为控制算法验证的标准入门平台。
这套仿真模型适合谁来学？如果你是刚接触车辆控制的学生，或者是从其他方向转来做自动驾驶横向控制、需要快速对比不同算法效果，再或者是已经用过Carsim这类商业软件、想自己写一个轻量仿真环境来跑算法，这篇文章能帮你在一天之内把模型和四种算法全部跑通，并且告诉你每个参数调起来是什么手感、哪个量影响最大、为什么MPC在低速弯道里反而可能不如Stanley。
我自己的习惯是先用纯脚本把被控对象和控制器跑通，再用可视化画轨迹、画误差曲线，最后才考虑要不要迁移到Simulink里做更复杂的联合仿真。脚本的好处是调试方便、每一步都知道在算什么，比一上来就拖模块找线要快得多。下面所有内容都是沿着这条"先模型、后算法、再调试"的路线展开的。
2. 二自由度动力学模型完全解析：公式推导、状态空间表达与离散化
2.1 为什么选择二自由度模型，而不是运动学模型或者七自由度模型
很多人一开始会纠结：车辆模型到底用运动学模型还是动力学模型。运动学模型好写，状态方程简单，适合低速场景；动力学模型引入了轮胎力、质量和转动惯量，能在更高车速下反映出车辆的"惯性滞后"和"横摆阻尼"，这对控制算法的对比更重要。尤其当你要验证MPC的预测能力时，只用运动学模型会让MPC看起来和纯几何方法没什么区别，体现不出"模型预测"的优势。
所以我最终选择的是二自由度线性动力学模型。它的本质是把真实车辆在"平面运动+横摆"层面的响应浓缩成两个微分方程，等效侧偏刚度在一个工况点线性化。这样做有三个明显好处：一是状态变量少，只有侧向速度、横摆角速度、横向位置、航向角四个状态，控制器计算负担很低；二是轮胎线性区间内模型精度尚可，足够支撑控制算法对比；三是MPC内部使用的预测模型和被控对象模型可以一致，这样仿真结果不会被"模型失配"干扰，问题排查时也更简单。
2.2 公式推导与状态空间形式，以及纵向PID的作用位置
二自由度模型的基本微分方程如下（忽略纵向速度变化，把vx视为时变参数）：
m * (v_dot_y + vx * r) = Fyf + Fyr
Iz * r_dot = a * Fyf - b * Fyr
其中m是整车质量，vx是纵向速度，vy是侧向速度，r是横摆角速度，Iz是绕垂直轴的转动惯量，a是质心到前轴的距离，b是质心到后轴的距离。轮胎侧偏力在线性区内近似为：
Fyf = -Cf * alfaf
Fyr = -Cr * alfar
侧偏角表达式为：
alfaf = delta - (vy + a * r) / vx
alfar = - (vy - b * r) / vx
这里delta是前轮转角。把所有式子合在一起，整理成状态空间形式：
状态量x = [vy, r, y_e, theta_e]，其中y_e是横向误差，theta_e是航向误差，控制量u = delta。写成A矩阵和B矩阵，具体展开式很多控制论文里都有，这里不重复粘贴公式，重点说状态变量和误差变量的衔接方式。仿真里每一时刻根据车辆当前位置找参考轨迹上的最近点，把车辆坐标系下的横向偏差和航向偏差算出来，喂给控制器，控制器输出前轮转角，再回到微分方程里积分，这就是完整的闭环。
纵向PID的作用是维持vx恒定。比如你设定目标车速10m/s，纵向PID根据实际车速偏差输出一个等效的驱动力矩或加速度，叠加到纵向运动方程里。换句话说，整个模型并不是单纯的横向控制，而是横向+纵向组合，只是纵向通过PID保持车速稳定，不给横向控制器的对比增加额外的干扰变量。
2.3 离散化与仿真步长的选择逻辑
控制器是离散的，被控对象是连续的，所以仿真必然要做数值积分。我用的离散化方法是零阶保持法，也就是在每个控制周期内认为控制输入保持不变，用四阶Runge-Kutta做被控对象的状态递推，控制周期设定为20ms，相当于50Hz。这个频率对MPC来说足够，对PID和Stanley更是绰绰有余。
有人会问：能不能用一阶欧拉离散？我的建议是，如果只是控制器内部预测模型用一阶欧拉离散，可以接受；但被控对象的动力学积分不建议用一阶欧拉，尤其是在车速高于15m/s、轮胎侧偏刚度取值较大的情况下，一阶欧拉的数值误差容易累积，最后表现为车辆横向误差周期性抖动。实测下来四阶Runge-Kutta从精度和计算时间的平衡来看是性价比最高的选择。
提示：仿真步长不是越小越好。步长取1ms在一些情况下会暴露数值噪声，20ms到50ms的控制周期配合1ms到5ms的积分步长是最稳的组合。先固定控制周期20ms，把积分步长设在1ms，再观察误差曲线是否平滑。
3. 四种控制算法的核心原理与适配场景：MPC、PID、PID、Stanley到底各自强在哪
3.1 MPC：预测未来，滚动优化，约束全程在线
MPC的思路是所有算法里最接近"驾驶员预判"的。它基于当前车辆状态，用内部模型预测未来Np步的车辆响应，然后通过优化一个包含跟踪误差、控制量变化量的代价函数，求解出未来Nc步的最优控制序列，但只执行第一步，下一时刻重新滚动优化。因为每一步都在重新求解优化问题，所以它能持续对抗模型误差和外部扰动，这就是"模型预测控制"名字的由来。
代价函数我通常写成这样的形式：
J = sum((y_ref - y_pred)^T * Q * (y_ref - y_pred)) + sum(delta_u^T * R * delta_u) + rho * eps^2
Q矩阵对应横向误差和航向误差的权重，R矩阵对应控制增量惩罚，rho是松弛变量系数，eps是松弛变量，用于保证约束在某些情况下不至于导致求解失败。约束主要设置前轮转角幅度限制和前轮转角变化率限制，比如delta_max = 0.5rad，delta_rate_max = 0.05rad每步。这些约束直接对应真实转向执行机构的物理极限，也是MPC相比PID和Stanley最大的优势——它天生知道前轮转角不可能无限大。
调参的时候有一个重要的点：Q矩阵里的横向误差和航向误差单位不同，一个量纲是米，一个是弧度，直接比较数值大小没有意义，需要根据你的实际性能需求去试。我的初始做法是先用确定性等价原则，把横向误差权重设为100，航向误差权重设为10，R设为10，跑一遍双移线工况看结果再调整。如果横向误差收敛过慢，就加大Q；如果控制量波动太剧烈，就加大R。这个经验在多种工况下都比较管用。
3.2 横向PID：最朴素的反馈，用误差直接打方向盘
横向PID的思路就简单直接得多。我取横向误差ey作为PID的输入，输出前轮转角delta：
delta = Kp * ey + Ki * integral(ey) + Kd * d(ey)/dt
Kp决定了"看到偏差打多大方向"的力度，Ki负责消除稳态误差，Kd负责抑制超调和平稳过渡。实际实现时有一个坑：如果直接对横向误差求微分，误差噪声会被放大得非常厉害，导致方向盘高频抖动。我的解决办法是先对ey做一阶低通滤波，再进微分项，滤波时间常数取0.1s左右；或者干脆用航向误差替代微分项，即加一个"航向误差反馈"的组成部分，这在低速下效果很接近纯微分项，但噪声小得多。
横向PID适合什么场景？直线和曲率半径较大的弯道表现不错，因为误差的变化比较平缓；但遇到S弯、双移线这种连续曲率变化场景，纯PID会出现明显的滞后和超调，因为它是"看到误差才反应"的，本质上没有预判能力。这也是为什么它总是被拿来当对照组——简单、可用、上限明显。
3.3 纵向PID：车速闭环，让横向控制器在稳定车速下工作
标题里第二次出现的PID，在整车模型里我安排在了纵向通道。它的输入是目标车速和实际车速的偏差ev = v_ref - vx，输出是纵向控制量（可以理解为油门/制动等效加速度指令）：
a_cmd = Kp_v * ev + Ki_v * integral(ev) + Kd_v * d(ev)/dt
在二自由度模型中，纵向控制量不会直接出现在横向动力学方程里，但会影响vx，而vx出现在侧偏角的计算公式中，因此纵向车速的稳定性间接影响横向控制效果。如果纵向PID调得不好，车速波动超过±0.5m/s，横向误差曲线就会跟着抖，尤其对Stanley这种包含v在分母的算法，影响会更明显。
纵向PID的整定相对独立，可以先断开横向控制器，只让车辆直行，调整纵向PID让车速快速稳定地贴合目标值。通常Kp_v取200左右，Ki_v取10左右，Kd_v取0到5，具体值取决于你的车辆质量和油门模型增益。注意在一个二自由度模型里，纵向控制只是外挂的速度调节器，不用强求它的响应曲线有多完美，稳定且无静差就够了。
3.4 Stanley：几何直觉，用前轮指向参考路径的切向修正误差
Stanley方法在2004年被提出后，长期是低速自动驾驶横向控制的实用选择。它的原理非常直观：把前轮转角分成两部分——第一部分消除航向误差，第二部分根据横向误差产生一个与车速相关的偏转角。
delta = theta_e + arctan(k * ey / vx)
其中theta_e是当前车辆航向与参考轨迹最近点切线方向的夹角，ey是车辆前轴中心到参考轨迹最近点的横向偏差，k是增益系数，vx是纵向车速。当车辆偏离轨迹时，角度arctan(k * ey / vx)会把前轮转向偏差方向，且车速越低转向幅度越大，这正好符合低速泊车、窄路掉头的驾驶习惯。
Stanley的优点是没有调不完的参数，核心就一个k。k太小，误差收敛慢；k太大，前轮转角容易饱和，出现振荡。我通常从k = 0.5开始试，低速工况下1到2之间比较常见。它的短板也很明显：高速急弯时因为arctan项饱和，跟踪精度会下降；而且它只针对最近点的几何误差做反馈，完全没有考虑车辆惯性和执行机构约束，所以MPC能扛住的极限工况，Stanley往往先扛不住。
3.5 四种算法横向对比速查表
| 算法 | 模型依赖 | 计算量 | 约束能力 | 低速表现 | 高速大曲率表现 | 调参复杂度 |
|---|---|---|---|---|---|---|
| MPC | 高，依赖预测模型 | 高，需QP求解 | 强，可加多类约束 | 好，但延迟浪费算力 | 好，模型准确时优势明显 | 高，Q/R/Np/Nc都要调 |
| 横向PID | 无 | 极低 | 弱，只能靠限幅 | 好 | 一般，滞后明显 | 低，三个参数 |
| 纵向PID | 无 | 极低 | 弱 | — | — | 低，三个参数 |
| Stanley | 低，依赖路径几何 | 极低 | 弱，只能靠限幅 | 很好，方向感直觉强 | 一般，饱和后误差大 | 极低，一个k |
这个表是我跑完同一组双移线工况后整理出来的个人结果。MPC在横向误差峰值和平均误差上都能压住其他算法，但代价是每一步都要调用QPsolver；Stanley在低速时表现惊艳，一个k值就能跑出和PID不相上下的效果，而且不会出现PID那种先超调再回正的过程。
4. 从建模到闭环的完整实操流程：跑通第一版仿真
4.1 车辆参数初始设定与实际参考轨迹生成
先给出一组我自己跑通时使用的车辆参数，你可以直接拿去用：
| 参数名称 | 符号 | 数值 | 单位 |
|---|---|---|---|
| 整车质量 | m | 1412 | kg |
| 横摆转动惯量 | Iz | 1536 | kg·m² |
| 质心到前轴距离 | a | 1.015 | m |
| 质心到后轴距离 | b | 1.895 | m |
| 前轮等效侧偏刚度 | Cf | 80000 | N/rad |
| 后轮等效侧偏刚度 | Cr | 80000 | N/rad |
参考轨迹我用了双移线工况，这是车辆横纵向控制最常用的测试场景，模拟的是高速变道避险。定义参考轨迹的方式有两种：一种是用x和y的解析表达式逐点生成，另一种是预先存储轨迹点序列。解析生成的好处是轨迹平滑、可重复，缺点是双移线这种分段函数在拼接点会有曲率不连续，导致控制器在拼接点附近误差变大。我的做法是生成后用Savitzky-Golay滤波对轨迹做一次平滑处理，再做三次样条插值，这样很多曲率突变带来的困难就提前消除了。
生成轨迹后需要保存成结构体数组，至少包含x坐标、y坐标、航向角、曲率半径、路程里程这几个字段。横向控制算法在每一时刻都需要快速找到参考轨迹上的最近点，我直接用了简单的遍历+距离判断，因为轨迹点数量不多，实时性没有问题；如果你用的是数千个点的长轨迹，建议用KD树或者按里程增量二分搜索。
4.2 被控对象集成为S函数的Simulink模型，或者写成可复用的脚本类
两种方案我分别实现过，先说脚本。把二自由度动力学写成一个函数文件，输入是当前状态x和当前控制量delta，输出是状态导数，然后在主循环里调用四阶Runge-Kutta递推。核心循环长这样：
for k = 1:sim_steps % 获取参考轨迹最近点 [x_ref, y_ref, theta_ref, kappa_ref] = find_target_point(traj, x(3), x(4)); % 计算横向误差与航向误差 ey = compute_lateral_error(x(1), x(2), x_ref, y_ref, theta_ref); theta_e = normalize_angle(theta_ref - x(4)); % 控制器输出 switch controller_type case 'mpc' delta = solve_mpc(x, x_ref, theta_e, Np, Nc, Q, R, lb, ub, delta_prev); case 'pid' delta = pid_controller(ey, theta_e, dt, filter_state); case 'stanley' delta = theta_e + atan(k * ey / vx); end % 被控对象积分 x_next = rk4_step(@vehicle_dynamics, x, delta, dt); x = x_next; % 记录误差曲线和控制量 log_error(k, ey, theta_e, delta, vx); end这里用字符串switch来选择控制器，调对比实验时比在Simulink里切开关方便得多。车辆动力学函数里只需要按上一节公式计算侧偏力、叠加到状态导数上，代码不到三十行。
Simulink方案则用S-Function封装二自由度模型，控制器可以用MATLAB Function模块实现，也可以用Stateflow搭状态机。我用Simulink时最大的感受是：模型框图可视化很直观，适合汇报展示，但调试时效率明显不如脚本。如果你要调MPC内部矩阵，脚本能直接断点观察每一步的预测轨迹和最优控制序列；在Simulink里还要拉Scope，多一层麻烦。
4.3 MPC求解器的选择与参数化配置示例
MPC核心是一个带约束的二次规划问题，求解器我用了两种，简单对比一下：
- 如果只是验证原理，用MATLAB内置的quadprog就够了，优点是上手快，缺点是在线求解速度一般，Np取20、Nc取5时单步求解约5ms到15ms，在50Hz控制周期下勉强够用。
- 如果要跑更高实时性或者部署到嵌入式环境，可以换成qpoases或OSQP。qpoases对中小规模凸QP非常高效，主动集方法在约束变化不大的工况下能秒解；OSQP用ADMM，对大规模问题更稳，但需要额外调容差参数。
我最终用的是OSQP，因为它的容差控制和热启动机制让MPC在连续滚动优化时计算时间更稳定。关键配置是这样的：
settings = osqp; settings.eps_abs = 1e-4; settings.eps_rel = 1e-4; settings.max_iter = 2000; settings.polish = true; settings.verbose = false;代价函数权重我用的是：
| 权重 | 数值 | 说明 |
|---|---|---|
| 横向误差权重 | 100 | 主要惩罚横向偏离 |
| 航向误差权重 | 10 | 惩罚航向偏差 |
| 控制增量权重 | 10 | 抑制控制量的剧烈波动 |
| 松弛变量惩罚 | 1000 | 保证约束软化的收敛性 |
| 预测时域Np | 20 | 约0.4s的预测长度 |
| 控制时域Nc | 5 | 约0.1s的可变控制长度 |
预测时域为什么取20？因为在20ms控制周期下，Np=20意味着预测0.4秒后的车辆状态。车速10m/s时这个距离预测约4米，对双移线工况来说已经足够覆盖一个完整的变道过程。如果车速提高到25m/s，我通常会把Np加到30到40，让预测距离保持在10米以上。预测时域太短MPC会变得短视，看不出预判优势；太长则计算量上升，而且线性模型预测远处的状态偏差会变大，实际效果反而变差。
4.4 航向误差处理与横向误差符号约定
写代码时最容易被忽略的坑是：航向误差必须归一化到[-pi, pi]区间。无论你用atan2还是直接把误差角度塞进控制器，最终都要确保角度在合理范围内，否则在参考轨迹航向角跨越±180度的时候，误差会瞬间跳变接近360度，控制器会误判为一个剧烈的偏差，导致输出一个巨大的转向角。
我的归一化函数很简单：
theta_e = mod(theta_e + pi, 2*pi) - pi;横向误差的符号也很讲究。我统一约定：车辆在参考轨迹左侧时ey为正，右侧为负；航向角误差theta_e = theta_ref - theta_vehicle，取正值表示车辆需要右转来对齐参考航向。如果你的坐标系定义不同，控制器输出的正负号就会反过来，调参时看不到正常的收敛趋势，先检查符号约定，别急着改增益。
5. 调试过程与结果分析方法：误差曲线、控制量曲线和工况选择
5.1 双移线工况下的误差曲线对比与参数调整顺序
把所有算法模板搭好后，我第一个跑的是10m/s的双移线工况。横坐标是时间，纵坐标是横向误差和控制量，图上四个算法的曲线叠在一起。第一次跑出来的结果和大多数人预感的完全不同：MPC的横向误差峰值并不是最小的，反而是Stanley的第一轮误差峰值最小。这是怎么回事？
后来一查，原因出在MPC的参考轨迹怎么给。我一开始给MPC的参考量是最近点的位置和航向角，但MPC内部的代价函数里同时使用了横向误差和航向误差这两个量，符号和坐标变换如果不完全统一，预测出来的轨迹会有相位滞后，导致MPC在弯道入口处反应过慢。我把代价函数里的横向误差改为基于预测状态直接计算的位置误差，并确保横向误差和航向误差都是用预测轨迹状态与参考轨迹点之间的距离计算后，MPC的误差曲线才明显优于其他算法。
这个调试经历说明一个通用原则：MPC的预测模型、误差计算方式和被控对象的坐标定义要保持完全一致，否则预测再准也没用。PID和Stanley因为模型依赖低，不太容易出现这类"算法内部自洽性"问题，很容易上手，但这也正是它们在高性能需求下不够用的原因。
5.2 车速变化对算法排序的影响：从10m/s到25m/s的实测结果
我在相同轨迹上把目标车速从10m/s逐步提升到25m/s，每隔5m/s记录一次横向误差峰值。结果非常典型：
- 车速10m/s时：Stanley和PID峰值误差相近，MPC略低。Stanley胜在算法简单、响应快，没有PID那种积分拖尾现象。
- 车速15m/s时：MPC的优势开始体现，峰值误差明显低于Stanley，因为Stanley的arctan项在高速时前轮转角幅度受限，误差修正力度不够。
- 车速20m/s以上：PID的滞后已经非常严重，横向误差峰值超过0.8m；Stanley在曲率大的路段出现前轮转角饱和，误差波动显著；MPC因为带约束优化，仍然能把峰值误差控制在0.2m以内。
这说明什么？选算法不能脱离工况。如果你的场景是园区低速巡逻，Stanley完全够用；如果是高速换道，MPC几乎是唯一选择。PID呢？它适合作为某个单一子系统的内环，或者对性能要求不高的简单路径跟踪。
5.3 PID和Stanley的调参边界条件
PID的三个参数不是越多越好。我的调参顺序是：先设Ki和Kd为0，只调Kp。逐渐增大Kp到误差曲线出现第一次轻微振荡，然后后退30%作为基准Kp。接着加Kd，观察是否抑制了振荡，Kd数值从Kp的0.1倍开始尝试，增大到误差收敛变慢为止。最后加Ki，用来消除静态偏差。双移线工况下横向误差本身稳态不会长期存在，Ki的作用其实不明显，所以Ki设成很小甚至0都可以。
Stanley的k值调起来更粗暴：从k=0.5开始，跑一次记录峰值误差，再按15%的步长增大k，直到峰值误差不再下降甚至开始增大为止，取那个拐点附近的值。我在10m/s双移线工况下k=2.0左右效果最好；车速提高到20m/s时，k降到0.8更稳定。无论怎么调，Stanley在DS段这种曲率突变点都会出现一个不小的误差尖峰，这是几何算法本身没有预测能力导致的，可以接受。
5.4 误差指标统计与结果输出格式
为了对比四种算法，我至少记录以下指标：
- 横向误差均方根值RMSE；
- 横向误差峰值；
- 航向误差均值；
- 控制量的标准差，反映方向盘平稳性；
- 单步平均计算时间。
把这些指标汇总成表格，再从误差曲线里截图保存。你跑完自己的模型后如果也想输出对比结果，直接按这个列表收集数据，汇报和写文档都够用了。有一个小技巧：把误差曲线绘制和统计函数封装成一个公共工具，四个算法都调用同一套接口，这样结果可比性最强，不会因为不同算法用了不同的绘图代码导致图形格式不统一。
6. 常见问题与排查技巧实录：我在这套仿真里踩过的坑
6.1 车辆状态发散：侧偏刚度和初始状态的常见错误
状态发散是最常见的问题。初次跑通时车辆横向位移几秒内就飞出去了，检查下来有两个原因。第一是侧偏刚度符号写反。线性轮胎模型里，侧偏力定义为Fy = -C * alpha，如果漏了负号，系统会变成一个正反馈结构，车辆自然发散。第二是初始状态设置不合理。比如初始航向角和参考轨迹的航向角差得太多，比如差30度以上，任何控制器在一开始都会产生一个很大的转向角，二自由度线性模型在这样的大角度下本身就不成立，状态容易发散。解决办法是把初始状态尽量贴近参考轨迹的起点，或者给控制器加一个启动阶段的限幅处理。
6.2 MPC求解失败或解不稳定：松弛变量、Q权重和热启动
MPC求解失败的典型现象是控制量突然跳变到边界，或者每次求解都提示不可行。我的处理手段是三步：第一步确认约束是否互相矛盾，比如控制量上下限和变化率约束同时设得太严格；第二步加大松弛变量惩罚系数rho，并允许松弛变量参与优化，让MPC在约束不可满足时能够软性过渡；第三步启用热启动，把上一时刻的最优解作为当前时刻的初值，这样不仅求解更快，还能避免每次从零开始导致的控制序列突变。
有时MPC解本身没问题但控制量振荡很厉害，原因是R权重太小或者控制时域Nc太长。Nc等于预测时域Np时，相当于每个控制周期都在重新规划一条"完整的最优轨迹"，相邻两个周期求出的第一个控制量可能差异很大，表现出来就是高频抖振。把Nc限制在3到8之间，并加大R，通常能解决这个问题。
6.3 Stanley低速抖振和高速饱和的权衡
Stanley算法的公式里有vx在分母上，车速趋近0时arctan项会剧烈变化，低速下车辆本身车速不稳定，就会出现方向盘的反复修正，形成抖振。我的解决办法是做一个车速保护：当vx小于0.5m/s时，把arctan项里的vx替换为0.5，这样低速时输出一个有限角度，不会因为除零产生巨大转向指令。
高速饱和的问题则来自前轮转角限幅。如果你在代码里对delta做了限幅，比如±0.5rad，那么高速时arctan项很容易超过这个限幅，实际输出被截断后，横向误差的修正量不足，表现为车辆一直偏向弯道内侧或外侧。此时单纯增大k没有用，因为已经饱和了，正确的做法是加入航向误差的前馈补偿，用参考轨迹曲率对应的稳态转角作为前馈基准，Stanley只负责修正偏差，相当于给几何控制器加了一层预瞄能力。
6.4 常见问题速查表
| 现象 | 可能原因 | 解决手段 |
|---|---|---|
| 车辆几秒内飞出去 | 侧偏刚度符号错误 | 检查Fy = -C*alpha中的负号 |
| MPC报不可行 | 约束过严 | 加松弛变量，放宽变化率约束 |
| MPC控制量抖振 | Nc过长或R过小 | Nc设为5，R适当加大 |
| PID超调过大 | Kp过大或Kd不足 | 先只调Kp并后退30%，再加Kd |
| Stanley低速抖振 | vx接近0导致arctan项异常 | 下限保护，vx<0.5时用0.5替代 |
| 横向误差稳态不消失 | 缺少积分项或前馈 | PID加Ki，Stanley加曲率前馈 |
| 双移线拼接点误差尖峰 | 参考轨迹曲率不连续 | 用Savitzky-Golay或样条平滑轨迹 |
6.5 从仿真到实车移植时的注意事项
这套仿真模型和算法如果要往实车上搬，有几个地方必须先改。二自由度线性模型的侧偏刚度是在某个工况点标定的，实车侧偏刚度会随载荷和路面附着变化，MPC里的内部模型必须加参数在线辨识或者至少做成lookup table。其次，MPC的求解器从PC换到嵌入式平台后，计算延迟不再是20ms控制周期里可以忽略的量，要把求解耗时算进控制时延里，考虑用模型预测控制中的时延补偿。最后，PID和Stanley在实车上通常不是直接用前轮转角指令，而是转成方向盘转角指令，中间还隔着转向比和转向执行机构的动态响应，仿真里忽略的这些环节在实车上经常成为抖振的源头。
我个人在实际调试中的经验是：先让模型在中等车速、中等曲率工况下跑通并画出误差曲线，再逐步加大难度，最后才用双移线这种带曲率突变的工况压测。遇到问题时优先检查坐标系定义和符号约定，而不是急着改参数，这两点至少能省掉一半的调试时间。这个仿真项目后续还可以扩展出很多方向：比如加入路面附着系数变化来测试控制器的鲁棒性，或者把参考轨迹换成从路径规划模块实时输出的曲线、接入更高阶的车辆模型做联合仿真，甚至可以把MPC里的线性模型换成非线性模型并对比两者的性能差异。回到这套四算法对比模型本身，跑通只是第一步，真正有价值的是在一个统一平台上量化和理解每种控制算法的行为边界。把这些边界摸清楚以后，你再去学那些更复杂的控制理论，心里会踏实得多。
