有时候危险不是前车急刹那一瞬间出现的而是它出现在你计划变道、逃到旁边车道的那一秒。我最早做自动驾驶换道避撞课题时花了大把时间在路径规划上结果发现人工势场法规划的路径经常被后端的执行模块扔掉后来改成纯模型预测控制MPC全程优化求解器又常常慢到让人想砸键盘。几次被逼无奈干脆把人工势场法和MPC放到同一个框架里分工协作最后这套方案才真正从仿真跑到实车。这篇就来拆解整个技术验证过程中的设计思路、参数标定细节和那些常规文档里不会写的坑。人工势场法负责该往哪躲MPC负责怎么躲才稳听起来是句正确的废话实际把两者耦合好却很讲究。下面先从为什么单打独斗都不行讲起。1. 换道避撞为什么不能只押注单一算法一次意外暴露的致命短板1.1 人工势场法的直观逻辑和三个让我吃大亏的地方人工势场法APF的思路很简单在行驶环境中人为构造一个虚拟势场目标点和期望车道中心产生引力障碍物产生斥力最后把引力和斥力合成一个合力方向沿着合力规划轨迹。因为计算量小、实时性好很多局部避障方案首版都会用它。用起来方便坑也不少。我踩过最明显的三个坑第一是局部极小值。当障碍物相对对称地分布在车前方两侧或者自车陷入某种前方堵死、侧向也被顶住的空间时引力和斥力刚好抵消势场梯度为零规划层输出的路径就是原地踏步。在高速场景里这个犹豫会直接变成追尾。第二是参数突变。障碍物一旦进入斥力影响半径斥力会从零突然跳到一个较大的值路径出现一个明显拐点换道轨迹反复横跳后排乘客能感觉到方向盘的连续修正。第三也是最容易被忽略的势场规划出来的路径完全没有考虑车辆动力学极限。它只回答哪条线能绕过去不回答以当前车速和转向能力这条线能不能跟得住。我用仿真测过一条双移线轨迹最大曲率对应的横向加速度已经超过0.6g普通家用车底盘根本执行不了。1.2 MPC单独上阵时看起来全能、用起来蹩脚的那些体验模型预测控制MPC的逻辑是在每个控制周期基于当前状态和车辆模型预测未来N步的系统行为在满足一系列约束的前提下在线求解一个优化问题取出第一步控制量下发执行然后滚动重复。MPC确实强大——它天然能处理转向角限幅、加速度约束、安全距离约束等各类不等式约束比单纯跟踪一条参考线要懂事得多。但把全部避撞功能都交给MPC我很快遇到三个现实困难一是障碍物约束非凸。避撞条件不要撞上障碍物映射到优化问题里常常是一个非凸可行域。优化求解器很容易陷入局部最优甚至直接判定无解。在模拟里最常见的画面就是求解器报错、车辆保持上一周期控制量眼睁睁往前冲。二是计算时延过高。高速工况预测1.5秒如果控制步长取50ms需要30步预测状态维数稍微上来之后一次求解可能超过80ms。而控制器周期往往是20ms调度器一乱控制量更新就滞后一拍。三是依赖参考轨迹。MPC的输出质量对参考轨迹的合理性极其敏感。没有一条预先存在的目标轨迹光靠权重矩阵和约束去挤出一条避撞路径调参周期非常长而且不同场景参数还不好复用。1.3 组合方案的本质逻辑让规划做规划、控制做控制经历了这些挫败之后我把方案改成双层结构上层用人工势场法做局部路径规划负责生成一条合理的参考路径下层用MPC做轨迹跟踪在跟踪的同时保留安全距离约束作为兜底。简单说就是——势场给MPC指路MPC给势场擦屁股。这样做的本质逻辑有两层。第一层是分工势场规划计算量小、更新频率高可以让参考路径时刻跟随环境变化MPC不再需要从零开始找避撞路径只负责把参考路径和动力学约束、执行器约束揉在一起求解搜索空间小很多求解速度自然上来。第二层是容错就算势场规划因为局部极小值给出一条卡住的路径MPC也会因为安全距离硬约束被触发而强制制动不会让车辆停在那里发呆。后面几章我把这套方案从结构、参数到测试细节完整摊开。2. 系统架构设计势场轨迹怎样进入MPC的口袋2.1 分层控制链路与多频率时序配合整个控制链路我分成五层感知与预测、驾驶意图决策、势场规划、MPC跟踪、底盘执行。看起来层级多但每个层级任务单一不容易出现大模块互相打架的问题。感知预测层负责输出障碍物列表包括位置、速度和预测轨迹。意图决策层判断是否需要换道比如当前车道前方碰撞时间小于阈值同时目标车道有足够纵向间隙就发出换道请求。势场规划层拿到目标车道信息后生成一条局部参考轨迹。MPC层把参考轨迹转化为控制指令底盘执行层负责转向和制动。时序安排上我用了三个不同的频率这比统一用一个频率更贴合实际意图决策跑10Hz势场规划跑20HzMPC跑50Hz。这样设计的原因是势场规划比MPC慢一拍没关系只要每个MPC周期都能拿到最近的参考轨迹即可但如果反过来让MPC等规划层控制周期就废了。实际实现时我在两层之间加了一个环形缓冲区保存最近的参考轨迹MPC取数时同时拿到这帧轨迹和上一帧轨迹做线性插值避免参考轨迹跳变带来的控制量突变。2.2 势场输出如何转成MPC的参考状态序列人工势场法直接输出的是离散路径点但MPC需要的是参考状态序列包含位置、横摆角和曲率。所以我做的第一件事就是把路径点用三次B样条拟合得到连续曲线然后按MPC的预测步长重新采样得到每一步的参考位置、参考航向角和参考曲率。伪代码可以概括为# 势场规划核心流程示意 def plan_reference(ego_state, obstacle_list, lane_center): trajectory [] for s in range(0, lookahead_steps): current_position extrapolate_position(ego_state, s * step) # 引力引导车辆朝目标车道中心线移动 f_att k_att * (lane_center(s) - current_position) # 斥力对每个障碍物计算排斥效果 f_rep np.zeros(2) for obs in obstacle_list: d distance(current_position, obs.position) if d r_rep: f_rep k_rep * (1/d - 1/r_rep) * (current_position - obs.position) / (d**2 1e-6) force f_att f_rep next_state forward_integrate(ego_state, force, step) trajectory.append(next_state) return spline_fit(trajectory)这里有一个很容易踩的细节势场计算出的路径点在进入MPC之前必须做一次曲率可行性检查。如果某一段路径的曲率对应的横向加速度超出车辆在当前车速下的物理极限需要先降低参考车速必要时要抛弃这段路径而不是强行把死弯交给MPC去跟。否则MPC为了跟踪路径会不断触发行程约束最终输出一堆饱和指令车辆反而更危险。另一个细节是动态障碍物的处理。换道避撞场景中障碍物大多是运动的不是静止的。我把障碍物的预测轨迹按时间戳放进势场计算斥力时不光看当前距离还看相对速度。相对接近的障碍物斥力影响半径更大相对远离的障碍物影响半径适当缩小。这样势场规划就能自然表现出等后车先过去再变道的行为。3. 关键参数标定这套方案最花时间的部分3.1 势场参数不是越大越安全一个调参比例尺势场参数看上去只有几个系数实际标定起来非常折磨人。我给出一组高速工况下的参考取值范围和调整原则方便直接复用参数含义高速参考取值调整原则k_att引力增益0.5~2.0太小则对目标车道响应迟钝太大则路径过于贴线、变道生硬k_rep斥力增益2.0~5.0过度增大容易出现幽灵绕行适度减小则路径更顺滑r_rep障碍物斥力影响半径12~25m车速越高取值越大但要避免覆盖到非目标障碍物d_safe最小安全横向间隙1.5~2.5m与传感器误差、车辆宽度共同决定我一开始犯过的错误是为了让车更怕障碍物把k_rep调得特别大结果车辆离障碍物还有几十米就开始横跨车道绕行。在三条车道的高速路上明明旁边车道没有车车辆却画了一个大弧线绕开一个静止锥桶这就是幽灵屏蔽——斥力过强把目标车道的引力全部抵消规划路径严重偏离意图。后来我做了改进把障碍物分成静态障碍物和动态障碍物两套斥力参数。静态障碍物使用较大的r_rep和较小的k_rep保证车辆远距离减速动态障碍物使用较小的r_rep和较大的k_rep保证近距离快速响应同时避免在远处就触发不必要的绕行。这样调整之后换道轨迹明显更接近人类驾驶习惯。3.2 MPC预测时域、控制时域与权重矩阵的耦合关系MPC参数里最折腾的是预测时域N_p、控制时域N_c和权重矩阵Q、R。它们不是独立变量而是相互耦合的。我用的是非线性的车辆运动学模型加线性化MPC框架状态量取[x, y, yaw, v]控制量取[加速度, 前轮转角]。控制周期20ms。在高速工况下我把预测时域N_p取25~30步对应1.25~1.5秒的前视距离控制时域N_c取5~10步。低速工况预测时域可以缩到15步计算更快。权重矩阵方面我的起点是位置权重2.0、航向权重1.5、速度权重0.5控制量权重0.1、控制增量权重0.05。但这只是一个起点不同底盘特性差异很大实车调试时往往需要往返调整。MPC的代价函数核心结构可以写成三部分的和跟踪误差加权、控制量加权、控制增量加权。跟踪误差项主要让车跟着参考路径走控制量项限制执行器的绝对输出控制增量项防止转向命令突变。如果车辆在换道末端出现明显的转向回正抖动先尝试增大控制增量权重而不是盲目增大位置权重。约束的设置也很有讲究。我不建议把所有想约束的都塞进去一来求解变慢二来约束之间会互相矛盾导致无解。我最终只保留了四组核心约束横向位移边界、前轮转角限幅、加速度限幅、与动态障碍物的安全距离。安全距离约束是硬约束其余约束设为软约束允许在紧急情况下轻微突破但会给代价函数增加惩罚项。这个做法缓解了很多无解问题。4. 仿真验证中值得记录的三个典型场景4.1 同向慢车切入与后方来车博弈第一个常测的场景是自车100km/h行驶在中间车道前方有一辆80km/h的慢车右侧车道有一辆110km/h的来车。意图决策层判断左侧车道可换但势场规划发现左侧车道后方来车快速接近于是输出一条先减速至85km/h、延迟0.8秒后再向左变道的参考轨迹。这套行为来自动态斥力的作用。左车道后方来车被建模为一个不断逼近的动态障碍物它的斥力场随时间推移逐渐增强当自车迟到的代价超过直接减速的代价时势场自然给出延迟换道的轨迹。MPC跟踪这条参考轨迹时最大横向加速度控制在0.28g左右横向跟踪误差小于0.12m整个过程车内横向加速度的变化率也没有超过0.2g/s乘客感受比较平稳。如果把这个场景换成强行立刻变道的势场参数模拟结果里自车会与来车产生1.2秒的侧面相对时间余量看起来也能避开但留给后车的反应时间极短极易引发次生事故。这说明势场参数不仅影响能不能避开还影响避让过程是否文明。4.2 连续换道与多障碍物叠加时的路径耦合问题第二个场景更复杂两条相邻车道都有慢车自车需要先向左变道再向右变道回到原来的行驶方向。这是无人驾驶很不喜欢处理的多障碍耦合场景。最容易出现的问题有两个第一个是势场层多障碍物斥力叠加虚拟合力被压在一个极小的数值附近路径陷入局部极小第二个是MPC在连续换道的转折点处产生过大的反向转向。为了解决第一个问题我在势场规划中引入了虚拟子目标点机制——完成第一次换道并稳定后虚拟子目标点更新为下一段目标车道的中心线而不是一直用全局目标终点作为唯一引力源。这等于把连续换道拆成了两个单次换道避免势场全局引力太弱、局部斥力主导的问题。解决第二个问题则靠MPC的终端约束在预测时域末尾将横向速度强制约束在零点附近让MPC在完成换道之前就开始为停车做准备。实测连续换道场景下最大横向加速度被压制在0.35g以内末端没有出现典型的反向甩尾动作。4.3 参数边界失效时的一次“完美翻车”复盘仿真验证里最让我印象深刻的是一次完全失败的参数组合。我把k_rep调到5.0、r_rep调到25m本意是让车辆更保守结果车辆在距离静态障碍物20m处就开始大幅度偏移车道中心线明明旁边车道没有车路径却画出一个大弧线。这不是算法出错而是参数组合走出了合理边界目标车道的引力在整个横向范围内都被斥力覆盖车辆不敢回到车道中心线。反方向的情况发生在MPC预测时域过短的时候。我把N_p降到10步0.5秒面对80km/h相对速度的切入车MPC几乎来不及反应最后的纵向安全距离被压缩到不足8米。事后分析发现预测时域过短导致优化器只看眼前三步完全没有中距离的规划意识MPC被安全距离硬约束反复触发控制指令在制动和转向之间来回跳实际效果比完全不做避撞还要差。这两次失败让我定下一个规矩任何参数改动都要跑一遍随机场景集不能只看一两个固定场景的曲线。5. 从仿真走向实车后的三个工程化修正5.1 传感器噪声在势场中被放大轨迹抖动的真正元凶离线仿真里障碍物位置是精确的但上了实车毫米波雷达和视觉传感器输出的障碍物位置噪声不可忽视短时间抖动能达到±0.3m。势场斥力要对距离求导距离信号一旦有噪声斥力方向就会剧烈变化参考轨迹自然跟着抖。我处理这个问题分了三步。第一步在感知层对障碍物状态做卡尔曼滤波把位置状态变成平滑的估计值第二步在势场计算前对障碍物位置额外做一阶低通滤波时间常数根据车速调整车速越高时间常数越大第三步在斥力计算中使用平滑距离公式避免距离接近零时产生巨大梯度比如用 d_smooth 0.5 * (d sqrt(d^2 eps^2)) 代替直接距离d。实测采用这三步之后参考轨迹的抖动频率下降了大约70%MPC输出的转角指令也稳定了很多。5.2 MPC求解时延和控制周期之间的矛盾怎么妥协离线仿真时MPC求解器平均耗时30ms看着没问题但实车上统计P95竟然能达到85ms超过20ms控制周期好几倍。这意味着有5%的周期里控制器拿到的控制量是滞后3~4个周期的旧值。我的妥协方案是三管齐下第一开启求解器的早停机制设定最大迭代次数宁可优化不精确也要保证在指定时间内返回一组可行解第二把优化问题的约束做预离散化把复杂的非线性约束提前转成线性约束矩阵减少在线计算量第三在极端情况下使用上一拍的控制量做前馈补偿。这么调整之后实车测试中MPC求解P95被压到50ms以内控制指令的连续性已经能接受。这里有个经验统计时延一定要看P95而不是平均值平均值好看解决不了偶发卡顿的问题偶发卡顿恰恰是最危险的。5.3 安全距离约束的“软硬”切换实车教我的最后一课仿真里我坚持用硬约束的安全距离但实车上传感器漏检一次硬约束就会直接触发MPC无解车辆锁死。后来我改成分级安全策略正常工况下安全距离是硬约束当感知置信度低于阈值或障碍物出现时间小于0.5秒时自动切换到软约束并大幅提高对应的惩罚权重让MPC尽力避让而不是放弃治疗。这个改动听起来是工程补偿其实是对算法可靠性的重要修正。因为任何感知系统都有漏检和误检控制层必须有能力在感知不完美的情况下仍然给出合理的、可执行的指令。也正因如此换道避撞系统绝不能只依赖单一的传感器输入多传感器融合在这个环节的价值比在ACC跟车场景中更突出。6. 这套研究留给下一轮迭代的可复用经验仿真、硬件在环、实车测试都跑完回头审视这个课题我最大的感受是人工势场法和MPC都不是新东西真正值钱的是把两者在工程上稳妥地焊在一起的过程。我想留下几条可以直接带走的心得。第一标定顺序不要颠倒。先把势场参数调稳让参考轨迹足够合理再调MPC权重去适配这条轨迹。如果反过来先调MPC你会发现自己永远在给一条不合理的路径擦屁股。第二参数敏感性测试和蒙特卡洛场景覆盖必须成为规范流程而不是乙方调研报告里的一句话。障碍物突然切入、目标车道后方高速来车、连续换道、感知跳帧这四类场景每个都要跑上百次记录最差工况而不是平均工况。很多问题就是在倒数第二次测试里暴露的。第三把参考轨迹和实际跟踪轨迹同时录制并叠加回放。许多看似控制不稳定的问题其实是规划层的参考轨迹在频繁跳变通过对比两条轨迹可以快速定位责任。第四MPC模型的选择要匹配工况。低速和中速工况下运动学自行车模型配合线性化MPC完全够用求解快、调试方便高速大横向加速度工况下必须引入带侧偏刚度的动力学模型否则MPC会在换道末端误差持续累积。最后再分享一个实车测试时的小技巧在车辆后轴中心附近贴一个高精度定位标记点对比真实轨迹和MPC内部状态。很多仿真里看起来没问题的轨迹实车上因为定位延迟会产生20cm级别的横向误差这个误差很难被方向盘转角传感器察觉但乘客能明显感到车身在换道时慢半拍。如果下次你换道落地时总感觉车头指向偏了优先检查定位延迟补偿而不是急着改权重矩阵。