横纵向控制实车方案：纵向PID与横向MPC的二自由度模型解析

做自动驾驶控制的人绕不开横纵向控制这套基本功。我最近在整理一套已经在实车上跑通的方案纵向用PID、横向用MPC模型基础是经典的二自由度车辆动力学模型。这套组合在行业内算比较典型的配置也是我推荐给身边同事做入门到进阶控制的参考路径之一。标题看着简单真正落地的时候会碰到一堆建模细节、参数整定和横纵向耦合的问题这篇文章把整条链路拆开讲。1. 控制方案选型复盘为什么纵向敢用PID横向却必须上MPC先回到最开始的问题既然已经有动力学模型了为什么不一鼓作气把横纵向都做成MPC为什么不干脆全部用PID这两个问题我在项目评审会上被问过很多次每次的回答其实都指向同一个逻辑——控制对象本身的物理特性决定了用什么控制器。1.1 纵向控制的物理本质标量跟随、执行器响应快、扰动可前馈纵向控制管的是车速或者车距本质上是一个标量跟随问题。期望值是一维的被控量也是一维的系统从控制输入到车速响应的延迟和滞后在工程可接受范围内。这种场景正是PID最擅长的比例项提供快速响应积分项消除稳态误差微分项在整定得当的情况下抑制超调。纵向系统还有个很方便的特点——扰动来源相对明确。坡道阻力、风阻、滚动阻力这些都可以通过前馈方式粗略补偿PID只需要处理前馈补偿不准的那部分误差。我见过很多实际项目纵向PID只保留PI两项D项直接去掉效果反而更稳因为微分项在车速信号有噪声时很容易把油门刹车抖出来。1.2 横向控制的核心挑战预瞄性、曲率变化、执行器延迟横向控制面对的是完全不同的局面。车辆横向运动本质上是侧向加速度和横摆角速度受前轮转角控制但车辆本身是一个惯性系统前轮转动后轮胎力建立需要时间横摆角速度响应再到横向位移变化又有积分关系。简单说你打方向盘之后车要过一小段时间才真正开始转弯。这个滞后意味着纯反馈控制去跟踪一条弯曲道路时天然会落后——等发现横向偏差大了再去打方向车已经冲出去一截了。MPC的核心优势正好落在“预瞄”两个字上。它把未来的参考轨迹、当前车辆状态和动力学模型放在一起往前推若干个采样周期算出未来一段时间的控制序列取第一步执行。这个过程相当于司机提前看前方的路而不是只盯着车头前面那一点。再加上MPC能显式处理约束——前轮转角限幅、转角变化率限制、横摆角速度边界——这在实际车辆上非常重要。1.3 二自由度模型的定位不是所有控制都在模型上做这里说清楚一个容易混淆的点。标题里写“根据的是车辆二自由度车辆动力学模型”这套模型最核心的作用是给横向MPC提供预测用的状态方程。纵向PID走的是误差反馈路径并没有依赖这个模型顶多在做油门刹车标定映射时用到一些逆向纵向动力学的概念。实际项目中二自由度模型主要负责描述车辆侧向运动和横摆运动用来预测前轮转角对横向偏差、航向角偏差的影响趋势。模型不负责精确到厘米级的预测但趋势对了MPC的滚动优化就能做出正确决策。2. 车辆二自由度动力学模型从推导到工程可用状态空间二自由度模型在教材里叫自行车模型本质上是把车辆的四轮系统简化成前轴后轴两个等效车轮忽略侧倾和俯仰自由度只保留侧向运动和横摆运动两个自由度。这个简化在侧向加速度不大于约0.4g的场景下精度是够用的绝大多数正常驾驶工况都在这个范围内。2.1 模型假设与适用边界建模前必须明确假设。第一车辆纵向速度视为恒定至少在单个预测时域内不变第二轮胎工作在线性区侧偏力与侧偏角成线性关系用侧偏刚度C表示第三前轮转角小可以近似取sin(delta)约等于delta、cos(delta)约等于1第四忽略空气动力学侧向力、道路超高和侧倾效应。这些假设决定了模型的使用上限——高速紧急变道、极限操稳工况下模型会失真MPC预测出来的轨迹会和实际偏差很大这时候就得切换更高精度的模型或者降级控制模式。2.2 状态空间方程的建立我直接给出一套工程上常用的状态空间形式。定义状态量为横向偏差e1、横向偏差变化率e1_dot、航向角偏差e2、横摆角速度偏差e2_dot控制量为前轮转角delta。这里的横向偏差e1是车辆质心到参考轨迹的横向距离航向角偏差e2是车辆航向与参考轨迹切线方向的夹角差。标准形式为x_dot A * x B * delta D * psi_ref_dot其中A [0, 1, 0, 0; 0, -(CfCr)/(m*vx), (CfCr)/m, (-a*Cfb*Cr)/(m*vx); 0, 0, 0, 1; 0, -(a*Cf-b*Cr)/(Iz*vx), (a*Cf-b*Cr)/Iz, -(a^2*Cfb^2*Cr)/(Iz*vx)] B [0; Cf/m; 0; a*Cf/Iz] D [0; (-a*Cfb*Cr)/(m*vx)-vx; 0; (-a^2*Cf-b^2*Cr)/(Iz*vx)]m是整车质量Iz是绕z轴的转动惯量a是质心到前轴距离b是质心到后轴距离Cf和Cr是前后轴等效侧偏刚度vx是纵向车速。需要特别提醒的是侧偏刚度在不同符号约定下可能带负号直接用教科书公式之前一定要对照自己系统的坐标系定义核对一遍我见过不止一次因为符号问题导致MPC方向反了的bug。2.3 参数辨识与灵敏度分析模型参数如果直接抄同款车型的数据控制器大概率跑不好。每个车的质量、质心位置、轮胎特性都不一样。工程上常见的做法是做阶跃转向试验车辆匀速直线行驶给一个固定的前轮转角阶跃记录横摆角速度响应曲线。将实测曲线与模型仿真输出对比利用最小二乘或遗传算法反求Cf、Cr、Iz这些关键参数。从我的经验看Cf和Cr对MPC性能的影响非常显著。如果侧偏刚度估计偏大模型会认为车辆转向“更灵敏”MPC给出的前轮转角往往偏小实际过弯会出现转向不足导致的横向偏差反过来估计偏小控制器会过度打方向车容易左右摆动。另外质心位置a和b的变化比很多人想象中敏感载重变化会导致质心后移后排坐满人和空载时同一个MPC参数表现完全不一样所以量产车上通常会做成a、b随载重估算值变化的调度表。2.4 模型失配时的表现与规避模型失配不会直接导致发散因为MPC本身有反馈校正环节每一拍都会重新用当前实测状态初始化预测模型不准的后果更多体现在“控制品质”上比如弯道里的横向偏差比模型准确时大、方向盘动作更毛躁。规避手段主要有三条一是把侧偏刚度做随速度变化的查表因为轮胎侧偏特性本身随车速和载荷变化二是在干扰较大的工况比如强侧风下给MPC的目标函数中适当增大横向偏差权重让控制器更“用力”地纠偏三是和实际道路测试数据对齐采集几组双移线工况的实测轨迹反推模型参数。3. 纵向PID控制增量式设计、抗饱和与踏板标定映射纵向控制的整体架构是这样的外层是速度跟踪PID输入是期望车速和实际车速的差值输出是期望加速度内层根据期望加速度查表生成油门踏板开度或者制动压力/制动踏板开度。分层的好处是逻辑清晰而且方便在期望加速度层面加入舒适性约束——比如限制加速度变化率让加减速不那么突兀。3.1 期望加速度到踏板开度的映射把PID输出的期望加速度换算成执行器指令是纵向控制落地中最容易被低估的一步。粗略的做法是直接用一张二维表横轴是当前车速纵轴是期望加速度输出是油门开度。这张表的底层逻辑是逆向纵向动力学——某个车速下要产生某个加速度发动机/电机需要输出多少驱动力换算成踏板开度大致是多少。更精细的做法是在查表基础上加阻力补偿根据当前车速计算滚动阻力和空气阻力根据坡度计算坡道阻力把这些阻力对应的驱动力加到需求值上。这样车辆在坡道上能保持稳定速度不至于出现平路标定好的PID上了坡就掉速四五公里的情况。一个我踩过的坑是查表只做了油门方向制动方向单独一套标定这没问题但两套标定在加速度为0附近的切换一定要做死区处理否则车会在油门和刹车之间频繁切换车内体验非常差。3.2 增量式PID设计纵向PID我推荐用增量式而不是位置式。两者的区别在于增量式计算的是控制量的增量再对增量累加得到实际输出相当于控制器内部自带了一个积分器。这样做的好处是切换手动/自动模式或者某个瞬间误差突变时控制量不会因为积分项的累积而瞬间蹦到限幅值系统更平滑安全。典型的增量式PI实现思路是err v_ref - v p_out Kp * err i_out Ki * (err - err_prev) ae_out p_out i_out a_ref ae_out err_prev err注意这里积分项用的是误差的变化量所以不会出现传统位置式PID那样积分不断累积导致的大超调。每次计算出来的a_ref还需要做限幅比如限定在[-3, 3] m/s²之间并把限幅后的值反馈回积分环节防止控制器输出已经饱和了还在继续积分这就是抗积分饱和处理。3.3 纵向调参路径纵向PID整定遵循先P后I的顺序D项在纵向控制里我一般不加。先把I设成0P从小到大试当车辆在目标车速附近有轻微振荡但能跟上目标时P就差不多了然后慢慢加I直到匀速跟车时稳态误差降到允许范围内。这个过程中要特别留意执行器延迟如果油门响应偏慢同样的P值在响应快的电动车上可能振荡在响应慢的燃油车上却表现平稳所以换平台之后参数必须重新整定。另一个容易被忽略的点是采样周期和控制周期的匹配。纵向控制周期通常取10ms到50ms如果PID计算周期过快但车辆CAN总线上车速信号更新频率只有20Hz那么PID看到的误差在两次更新之间是不变的微分项在这种条件下只会放大信号噪声这也是我不加D项的另一个原因。4. 横向MPC控制器预测模型、滚动优化与约束处理横向MPC的框架一句话就能说清在每个控制周期用当前车辆状态作为起点基于二自由度动力学模型向前预测Np步在满足控制量约束的前提下求解一串最优的前轮转角序列使得预测轨迹与参考轨迹的偏差最小。然后只执行序列中的第一个转角下一周期重新来过——这就是滚动优化。4.1 状态量与控制量的选取状态量就是前面状态方程里的四个[e1, e1_dot, e2, e2_dot]。控制量u是前轮转角delta。有些方案会把vx也放进状态里做成时变系统的MPC但在二自由度模型框架下vx是作为时变参数出现在A矩阵里的。每个控制周期要用当前实测车速重新计算A矩阵和B向量这一步不能省因为A矩阵里大量项分母上都带vx。参考输入方面需要从规划模块拿到参考轨迹再计算出每个预瞄点的参考横摆角速度psi_ref_dot。如果轨迹是用离散点表示的相邻点之间的航向差除以时间间隔就是参考横摆角速度。这一步的精度直接影响MPC的跟踪效果我见过不少问题都出在参考横摆角速度滤波太狠或者没滤波前者导致过弯延迟后者导致方向盘抖动。4.2 离散化与预测方程连续状态方程需要离散化才能用于MPC。考虑到预测时域内车速基本不变可以在每个周期用一阶近似或者零阶保持法把A、B离散成A_d、B_dA_d I A * Ts B_d B * Ts其中Ts是控制周期。这个近似在Ts不超过50ms时精度足够再大就得用矩阵指数精确离散。基于离散模型从第k步的状态x0出发可以递推得到未来Np步的状态表达式x(k1) A_d * x(k) B_d * u(k) x(k2) A_d^2 * x(k) A_d * B_d * u(k) B_d * u(k1) ...把所有这些步骤展开可以整理成一个大矩阵形式把未来状态统一表示为当前状态的线性函数和控制序列的线性函数。这一步是MPC求解的基础所有优化变量都在这个统一形式下组装成二次规划问题。4.3 目标函数与权重设计目标函数最常用的形式是J sum(q1 * e1(ki)^2 q2 * e2(ki)^2) sum(r1 * delta(ki)^2 r2 * delta_rate(ki)^2)第一项是横向偏差和航向角偏差的惩罚第二项是控制量大小和控制量变化率的惩罚。q1和q2的分工不同横向偏差权重让车更贴近目标轨迹航向偏差权重让车更顺滑地跟随轨迹方向。r1压制方向盘转角幅度r2压制方向盘打得太快。权重之间本质上是矛盾关系——想要跟得准就要容忍方向盘动作大想要坐得舒服就要接受横向偏差稍大一点。实际标定时我会先固定r1和r2只调q1和q2。测出当前权重下的最大横向偏差和方向盘角速度峰值如果偏差超标就加大q1同时略微加大r2防止抖动如果方向盘太贼就加大r2或者减小q1。这套方法虽然土但在工程上非常有效比纯理论推导快得多。4.4 约束设置执行器限位与变化率平滑约束是MPC相对PID最硬核的优势。最基本的约束有两个前轮转角限幅和转角变化率限幅。前轮转角限幅由转向执行器机械极限决定比如[-0.5, 0.5] rad超过这个范围执行器根本输出不了转角变化率限幅由转向电机转速决定它决定了方向盘能打多快在高速工况下这个约束对车辆稳定性的影响甚至比转角限幅还大。把转角变化率约束加进去的方式是引入增量的决策变量。也就是说MPC不直接优化delta序列而是优化增量序列delta_rate再通过累加得到delta序列。这样角度的限幅和角速度的限幅都能显式写进约束矩阵里求解器会很自然地遵守。这个处理还有一个附带的好处——优化结果天然平滑不需要额外滤波。4.5 求解器选择与实时性思考MPC在实车上最大的门槛是实时性。一个典型的配置是预测时域Np20、控制时域Nc10决策变量数量大约20到30个加上约束规模并不算大。求解这类问题目前工程上公认比较稳的选择是OSQP或者qpOASES两者都是开源求解器都能在几十毫秒内完成求解支持实时嵌入式部署。我自己在某个实车项目上用OSQP控制周期20ms从状态反馈到拿到控制量平均耗时不到5ms余量非常充足。如果你用的是ROS或者Simulink环境做原型验证也可以先用内置的quadprog或者第三方二次规划求解器跑通逻辑等整车部署时再换成嵌入式版本。需要留意的是不同求解器对数值病态的敏感度不一样模型矩阵中某些项的数值量级差异很大时就容易求解失败工程上常用手段是给状态量做归一化或者缩放因子让目标函数中的各项数值处于相近数量级。5. 横纵向联调中的耦合问题速度变化对MPC模型的影响把纵向PID和横向MPC放在同一个系统里跑的时候真正的难题才开始出现。两个控制器并非独立——纵向速度变化会直接改变横向MPC的预测模型参数。5.1 速度项进入状态矩阵的机理回头看二自由度模型的状态矩阵A里面多处出现vx。当车速从60km/h变到30km/hA矩阵中多个元素会变化同一组MPC权重在不同车速下对应的闭环特性完全不同。最直观的例子是高速时车辆横摆响应更快同样的前轮转角产生的横摆角速度更大所以MPC在高速时倾向于输出更小的转角。如果MPC没有感知到车速变化还是按低速的参数输出转向指令高速下会过度转向非常危险。因此横向MPC模块必须实时接收纵向车速反馈在每个周期重新组装A矩阵。这属于时变MPC也是标题里“二自由度动力学模型”在横向控制中的核心体现。5.2 联调中最常见的两种问题第一个问题是弯道减速时方向盘突然小幅摆动。原因是车速进入弯道前半段还在下降MPC每一拍的预测模型都在变同一个参考轨迹算出的转角序列每拍都不同如果纵向减速度很大预测模型变化速度超过了MPC的响应带宽控制量就会震荡。解决思路是给MPC的目标函数里增大r2控制量变化率惩罚让控制器对模型变化不那么敏感。第二个问题是加速过程中横向偏差阶段性变大。纵向PID急加速时车辆侧向动力学特性快速变化但MPC的预测时域可能只覆盖了1秒左右加上车辆质心载荷转移导致二自由度模型精度下降偏差就被放大了。工程上处理方式是限制纵向加速度变化率不过度追求加速性能让车速在MPC的预测尺度内尽量平缓变化。5.3 工程上实用的解耦与补偿手段比较实用的做法是做一个简单的增益调度根据当前车速查表不同组的MPC权重。低速时q1可以设大一些追求贴线精度高速时q2和r2设大一些追求稳定性和平滑性。车速作为调度变量在工程实现上很自然控制器本身就接收车速信号做一张二维查表成本很低效果立竿见影。另一个值得一试的思路是纵向控制增加一个横向状态感知——当检测到当前横向偏差较大或者道路曲率较大时纵向PID适当降低目标车速。这相当于把横向控制器的能力边界同步给纵向控制器让整个系统主动降低横向控制的难度。两套控制器虽然独立设计但在系统层面通过策略联动是量产自动驾驶上非常主流的工程手法。6. 参数整定顺序与调试心法整个系统从仿真到实车验证我总结了一套固定的参数整定顺序按照这套顺序来能少走很多弯路。6.1 从仿真到实车的调参路径第一步在仿真环境里做模型在环或者软件在环把纵向PID和横向MPC的参数粗调到一个能稳定跟踪参考轨迹的水准。仿真里不用追求精细目标是确认控制器逻辑正确、无符号错误、约束处理有效。第二步做硬件在环把真实ECU和执行器的延迟特性加进去通常会发现仿真里表现不错的参数在硬件在环里开始振荡这时候主要调整控制周期和MPC权重让系统对延迟有裕量。第三步才上实车从停车场低速场地开始逐步提高车速每轮跑完看数据重点看横向偏差、方向盘角速度、加速度变化率这三个指标。6.2 关键参数参考表参数参考范围整定方向说明纵向P0.3-1.5太小跟车慢太大振荡纵向I0.05-0.3先P后I稳态误差消不掉再加纵向加速度限幅±2~3 m/s²舒适性硬约束乘客感受敏感区MPC预测时域Np15-25太短看不到前方弯道太长计算量上升MPC控制时域Nc8-12一般Nc明显小于Np横向偏差权重q10.5-5过大方向贼过小贴线差航向偏差权重q21-10高速时适当增大控制量权重r10.01-0.5压制方向盘幅度变化率权重r20.5-5压制方向盘速度舒适性关键注意这个表只给初始范围和调节方向具体数值必须按照你的车辆参数、控制周期和轨迹特性去整定直接抄参数是最不靠谱的。6.3 调试陷阱与应对一个常见的坑是把MPC权重调得“过于完美”——仿真里横向偏差控制在几厘米以内一到实车就方向抖动。原因在于仿真模型没有包含传感器噪声和执行器延迟MPC对参考轨迹的每个变化都非常敏锐一点噪声就会让转角序列出现高频抖振。应对方法是给参考轨迹做预滤波同时适当提高权重r2牺牲一点点跟踪精度换回舒适性。另一个坑是纵向PID在低速和高速下表现差异太大。低速情况下车辆动力学非线性强、执行器死区明显高速时风阻增大、转向更敏感。如果只用一组PID参数跑全部速度范围效果一定不好。建议至少分低速、中速、高速三段标定段间用线性插值过渡这个处理在MPC权重调度上同样适用。最后说一个我自己的习惯每次实车跑完我都会把横纵向控制数据同时录下来放在同一张图里对比时间轴。很多横纵向耦合的异常只有把车速曲线、加速度曲线、前轮转角曲线和横向偏差曲线放在一起看才能看清因果链条。比如某次方向盘左右摇摆单独看横向偏差一点儿问题没有但把车速曲线叠上来就发现每次摇摆都发生在加速度符号切换的瞬间——纵向加减速切换带来的载荷转移才是真正的扰动源。这种问题靠单个控制器的参数调整很难根治最终是通过协调纵向加加速度限制解决的。能看到这一层说明你对这套系统的理解已经超出“纵向PID、横向MPC”这八个字本身了。