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 FyrIz * r_dot a * Fyf - b * Fyr其中m是整车质量vx是纵向速度vy是侧向速度r是横摆角速度Iz是绕垂直轴的转动惯量a是质心到前轴的距离b是质心到后轴的距离。轮胎侧偏力在线性区内近似为Fyf -Cf * alfafFyr -Cr * alfar侧偏角表达式为alfaf delta - (vy a * r) / vxalfar - (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^2Q矩阵对应横向误差和航向误差的权重R矩阵对应控制增量惩罚rho是松弛变量系数eps是松弛变量用于保证约束在某些情况下不至于导致求解失败。约束主要设置前轮转角幅度限制和前轮转角变化率限制比如delta_max 0.5raddelta_rate_max 0.05rad每步。这些约束直接对应真实转向执行机构的物理极限也是MPC相比PID和Stanley最大的优势——它天生知道前轮转角不可能无限大。调参的时候有一个重要的点Q矩阵里的横向误差和航向误差单位不同一个量纲是米一个是弧度直接比较数值大小没有意义需要根据你的实际性能需求去试。我的初始做法是先用确定性等价原则把横向误差权重设为100航向误差权重设为10R设为10跑一遍双移线工况看结果再调整。如果横向误差收敛过慢就加大Q如果控制量波动太剧烈就加大R。这个经验在多种工况下都比较管用。3.2 横向PID最朴素的反馈用误差直接打方向盘横向PID的思路就简单直接得多。我取横向误差ey作为PID的输入输出前轮转角deltadelta Kp * ey Ki * integral(ey) Kd * d(ey)/dtKp决定了看到偏差打多大方向的力度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在横向误差峰值和平均误差上都能压住其他算法但代价是每一步都要调用QPsolverStanley在低速时表现惊艳一个k值就能跑出和PID不相上下的效果而且不会出现PID那种先超调再回正的过程。4. 从建模到闭环的完整实操流程跑通第一版仿真4.1 车辆参数初始设定与实际参考轨迹生成先给出一组我自己跑通时使用的车辆参数你可以直接拿去用参数名称符号数值单位整车质量m1412kg横摆转动惯量Iz1536kg·m²质心到前轴距离a1.015m质心到后轴距离b1.895m前轮等效侧偏刚度Cf80000N/rad后轮等效侧偏刚度Cr80000N/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保证约束软化的收敛性预测时域Np20约0.4s的预测长度控制时域Nc5约0.1s的可变控制长度预测时域为什么取20因为在20ms控制周期下Np20意味着预测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.8mStanley在曲率大的路段出现前轮转角饱和误差波动显著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值调起来更粗暴从k0.5开始跑一次记录峰值误差再按15%的步长增大k直到峰值误差不再下降甚至开始增大为止取那个拐点附近的值。我在10m/s双移线工况下k2.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设为5R适当加大PID超调过大Kp过大或Kd不足先只调Kp并后退30%再加KdStanley低速抖振vx接近0导致arctan项异常下限保护vx0.5时用0.5替代横向误差稳态不消失缺少积分项或前馈PID加KiStanley加曲率前馈双移线拼接点误差尖峰参考轨迹曲率不连续用Savitzky-Golay或样条平滑轨迹6.5 从仿真到实车移植时的注意事项这套仿真模型和算法如果要往实车上搬有几个地方必须先改。二自由度线性模型的侧偏刚度是在某个工况点标定的实车侧偏刚度会随载荷和路面附着变化MPC里的内部模型必须加参数在线辨识或者至少做成lookup table。其次MPC的求解器从PC换到嵌入式平台后计算延迟不再是20ms控制周期里可以忽略的量要把求解耗时算进控制时延里考虑用模型预测控制中的时延补偿。最后PID和Stanley在实车上通常不是直接用前轮转角指令而是转成方向盘转角指令中间还隔着转向比和转向执行机构的动态响应仿真里忽略的这些环节在实车上经常成为抖振的源头。我个人在实际调试中的经验是先让模型在中等车速、中等曲率工况下跑通并画出误差曲线再逐步加大难度最后才用双移线这种带曲率突变的工况压测。遇到问题时优先检查坐标系定义和符号约定而不是急着改参数这两点至少能省掉一半的调试时间。这个仿真项目后续还可以扩展出很多方向比如加入路面附着系数变化来测试控制器的鲁棒性或者把参考轨迹换成从路径规划模块实时输出的曲线、接入更高阶的车辆模型做联合仿真甚至可以把MPC里的线性模型换成非线性模型并对比两者的性能差异。回到这套四算法对比模型本身跑通只是第一步真正有价值的是在一个统一平台上量化和理解每种控制算法的行为边界。把这些边界摸清楚以后你再去学那些更复杂的控制理论心里会踏实得多。
