★ FEATURED ARTICLE 换道避撞是我最近大半年一直在磕的一个方向。背景很直白高速上开着开着前车突然减速或者旁边车道插进来一台慢车自动驾驶系统需要在两三秒内完成“要不要换”“怎么换”“换的时候方向盘打多少、油门抬多少”这一串判断。这套动作放在人身上是本能放在车身上就是标题里那套组合人工势场法做上层路径推算MPC模型做下层轨迹跟踪与避撞约束两层摞起来去解决“看到慢车知道怎么绕、绕的时候还不能吓到乘客”的问题。如果你正在做自动驾驶规划控制相关的项目或者准备用这个方向写论文做仿真这篇文章应该能帮你省掉不少弯路。我会把势场建模、MPC目标函数设计、两层怎么衔接、参数怎么调、哪些坑容易踩全部展开讲最后附上我自己调试时的参数表和排查逻辑。1. 换道避撞为什么难先理清决策、规划、控制三级分工1.1 一次换道动作背后的完整链路很多人一开始容易把换道避撞想成一个“找一条不撞的线”就完事的问题。但真正做下来你会发现哪怕只考虑单车智能换道动作也横跨了好几个层面。首先是感知层给出障碍物的位置、速度、类型然后决策层判断“当前车道是否安全、是否可以换到目标车道”。决策通过之后规划层要生成一条从当前位置到目标车道中心线的参考轨迹这条轨迹要避开车流、满足曲率连续、不能出现突变。最后控制层拿到参考轨迹结合车辆动力学模型计算出前轮转角和加减速指令再交给执行器。难点就在于这条链路里每一环都有滞后。感知有噪声决策有判断阈值规划更新有周期转向系统还有响应延迟。你规划出来的轨迹再好控制层跟不住照样压线甚至撞上去。换道避撞本质上是一个在动态约束下的运动规划与控制一体化问题单独靠某一种算法很难解决。我早期试过只做一个几何路径规划器避开障碍后直接把路径丢给PID去跟随。结果低速下还行速度一到80km/h就露馅横向误差来回跳车辆轨迹像蛇一样扭。所以才把思路调整为“规划层给意图、控制层兜物理极限”的两层架构。1.2 技术路线选型为什么是人工势场法配MPC解决换道避撞的常见路线其实有三类采样式规划、图搜索式规划、响应式规划。每一个单拎出来都有它的适用场景但也都有明显短板。采样式规划的代表是RRT及其变种它适合在高维空间里找可行路径但生成的路径往往不够光滑还需要额外做样条平滑而且动态环境下障碍一变就要重采样计算代价偏高。图搜索式规划典型是栅格A*它在结构化道路上能给出全局路径但网格分辨率直接决定计算量更新周期很难压缩到100ms以内用来做紧急避撞不够实时。响应式规划就是我们常说的人工势场法它把道路环境抽象成力场计算量极小、表达直观非常适合做局部避障和参考路径推演缺点也明显缺少全局视野容易陷入局部极小点。如果把三层架构放进来看更合理的分工是用人工势场法做局部实时避障与参考线生成用MPC做轨迹跟踪和动力学约束执行。MPC的强项在于它能把预测模型、目标函数和约束放在一个优化框架里滚动求解天然适配“既要跟踪又要避撞还要保证车辆稳定”的需求。两者结合等于一个出思路一个抓落实。从我实际开发体验来说这套组合的真正价值在于“把问题拆成两个可调试的小问题”而不是在一个算法里同时扛起所有矛盾。势场法负责让车“知道往哪走”MPC负责让车“按动力学走得稳”出问题时能定位到具体层面。2. 人工势场法的工程化拆解从力场公式到可用路径2.1 引力场、斥力场和车道约束场怎么建模人工势场法的核心思想特别简单可以理解成在一个力场里推一个小球。目标点产生引力拉着车往前跑障碍物产生斥力把车推开车道边界或者车道中心线产生约束力让车别乱跑。三者叠加后的合力方向就是车辆下一步应该移动的方向。引力场一般和车辆到目标点的距离成正比。写成工程上常用的形式就是F_att k_att * (P_target - P_vehicle)k_att是引力增益P_target是目标点位置P_vehicle是车辆当前位置。这个力越大车就越想往目标点靠。斥力场要复杂一些因为不能让斥力无限大。我使用的是带作用半径的倒指数模型F_rep η * (1/ρ - 1/ρ0)^2 * (1/ρ^2) * 单位方向向量ρ是车辆到障碍物的距离ρ0是斥力最大作用半径η是斥力增益。车辆进入ρ0范围内斥力迅速增大超过ρ0斥力直接置零这样远处障碍不会干扰车辆行驶。车道约束场是我踩坑后补上的一个关键项。如果没有它车辆虽然能绕过障碍物但很可能绕到旁边的车道边缘甚至骑线行驶因为势场规划只考虑了“不撞墙”没考虑“待在自己的道里”。车道约束场可以理解成一组朝向车道中心的偏置力车辆偏离车道中心线越远这个回拉力越强F_lane -k_lane * (y - y_lane_center)这三项叠加之后合力的方向可以被积分成一条参考路径。注意这里的“积分”是数值积分每一步按一个小的步长在合力方向上推进形成一系列离散路径点再扔给后续平滑模块。2.2 参数陷阱局部极小、目标不可达与路径振荡如果只看教科书上的势场公式会觉得一切都很顺畅。但实际跑起来经典势场法至少有三个臭名昭著的毛病局部极小点、目标不可达GNRON和路径振荡。局部极小点是你设置的目标点、障碍物和车辆的相对位置恰好让引力和斥力方向相反、大小相等时合力为零车直接卡在原地不动。最常见的情况就是障碍物正好挡在车辆和目标点之间车会被“压”在障碍物正前方既过不去也不后退。目标不可达问题发生在目标点附近存在障碍物时。目标点被障碍物的斥力场覆盖斥力大于引力车在接近目标点时会被推走永远无法精确到达目标位置。路径振荡在狭窄通道或目标点附近尤其明显。当车辆进入两个障碍物之间斥力从左右两侧交替占优最终路径会出现锯齿状摆头。如果直接把这条摆动的路径交给控制层转向机构会受不了乘客更是直接晕车。我调试时用了一张“势场等高线图”把车辆附近的势场值画成色阶图这样可以直观看到极小点出现在哪里。真正动手调参后你会发现这些坑不是靠一个万能参数能绕开的必须在算法结构上做工程化改造。2.3 工程化改造虚拟目标点、速度势场和势场屏蔽经过几轮调试我最后在经典势场法上叠加了三个改造才让路径真正能用在换道场景。第一个是虚拟目标点法。检测到局部极小趋势时比如车辆速度降到很低但离目标点还很远算法会在目标车道前方设置一个临时虚拟目标点。这个虚拟目标点把车辆先引导到侧向避让的位置等越过障碍物后再把目标点恢复成原来位置。相当于给势场解决“死锁”的能力。第二个是速度势场。原始势场只把障碍物当成静态点处理对于移动车辆来说不够用。我把障碍物的当前速度做一个外推把它的位置按未来1到2秒的预测位置做膨胀处理相当于在障碍物前进方向上多了一个“影子斥力场”。这样车在前车尚未完全减速时就会提前动方向而不是等逼近了才仓促避让横向加速度曲线会平滑很多。第三个是势场屏蔽。如果不做屏蔽对向车道的车也被算进斥力场里那车辆在任何情况下都不敢换道因为旁边车道永远有车。我的做法是根据当前决策目标只把“本车道前方”和“目标车道前后”的障碍物计入斥力场其余车辆不参与计算。这个改造听着简单但直接决定换道策略能不能在密集车流中正常激活。这三个改造做完后势场法才从一个理论玩具变成了真正能嵌入到自动驾驶架构里的路径生成器。3. MPC的避撞执行层模型、目标函数与滚动求解3.1 换道场景用什么车辆预测模型MPC的预测能力来自模型模型的精度决定了优化结果的可行性。换道场景车速通常在60km/h以上这时候如果只用一个简单的运动学模型会高估车辆的转向能力给出的控制指令执行器根本跟不上。我使用的是经典的单轨自行车模型。它忽略左右轮差异把前轮和后轮各自合并成一个轮子状态量取横向位置、纵向位置、横摆角、纵向速度、前轮转角。控制量是前轮转角增量和纵向加速度增量。这个模型在中高速换道场景下够用又不会像全动力学模型那样参数标定成本过高。模型的核心关系可以写成这样一套递推形式x(k1) x(k) v(k) * cos(ψ(k)) * dt y(k1) y(k) v(k) * sin(ψ(k)) * dt ψ(k1) ψ(k) v(k) / L * tan(δ(k)) * dt v(k1) v(k) a(k) * dt δ(k1) δ(k) dδ(k) * dt其中L是轴距δ是前轮转角dδ是前轮转角变化率。这些式子不复杂但已经能把“打方向盘带来的横摆变化”和“踩油门带来的速度变化”表达出来。顺便提一句如果你要处理更低附着系数的路面或更高车速建议在此基础上加入侧偏角约束侧向加速度不能超过轮胎附着极限。这个约束在干燥路面问题不大但在雨雪路面就是保命项。我在仿真里用的是近似约束形式直接限制侧向加速度绝对值不超过设定阈值。3.2 目标函数和约束把“不撞车、坐得稳”写成数学模型预测控制真正厉害的地方在于它能把多个相互矛盾的目标写进同一个优化问题。为了理解这一点我一般把目标函数拆成四项来看第一项是轨迹跟踪误差让车尽量贴近势场生成的参考线第二项是控制量本身的大小避免大角度转向和急加减速第三项是控制增量的变化率避免方向盘的抖动第四项是避撞软约束惩罚把障碍物距离太近或者进入不安全区域换算成一个惩罚值。用符号直观表达就是J Σ( w1 * ||轨迹误差||^2 w2 * ||控制量||^2 w3 * ||控制增量||^2 w4 * ||避撞惩罚||^2 )w1到w4是权重系数。你可能好奇为什么要设置w3这项目因为只限制控制量的大小并不能保证平滑方向盘的绝对角度可以很大但相邻时刻的变化量必须是连续的。加入控制增量惩罚后换道过程中方向盘不会一格一格地跳车辆横向加速度曲线会明显平滑。避撞部分我不建议只写成硬约束。硬约束的意思是“必须满足、不满足就无解”如果障碍物突然贴身靠近求解器很可能直接告诉你infeasible不可行系统就瘫痪了。我采用的是“软约束为主、硬约束兜底”的策略日常情况用惩罚项把车推离障碍物极端情况下才触发硬约束封锁危险区域。这样即使优化问题遇到困难系统也会优先选择远离障碍的方向而不是卡死。3.3 滚动时域的求解流程与关键参数MPC的运转方式我在纸面上画出来其实很简单每个控制周期内算法根据当前状态和预测模型向前看一个有限时域算出这一段时间内的最优控制序列但只执行第一个控制量下一个周期再重新观测、重新优化。这就是滚动时域的含义。伪代码长这样for k in (1...T): 读取当前车辆状态 x_k 预测障碍物在[k1, kN]的位置轨迹 构建目标函数与约束条件车辆模型、边界约束、避撞约束 求解二次规划问题得到最优控制序列 只执行控制序列中的第一个控制量 等待下一时刻重复以上步骤这个流程里最影响效果的两个参数是采样时间dt和预测时域N。我自己常用的组合是dt0.05s、N20预测时域总长约1秒乘以速度跨度足够覆盖一次换道的大部分动态过程。N太短时优化器看不到远处的障碍避撞动作会滞后N太长时求解速度会下降难以满足实时性要求。另外一个容易忽略的点是无障碍场景下的冗余优化。我发现如果把目标函数里的参考轨迹做得好MPC在无障碍场景下几乎工作在“跟踪模式”避撞惩罚项完全不起作用。而一旦势场检测到危险参考线开始偏移MPC会自然跟着偏移同时车辆动力学约束会自动限制转向速率。这种从“跟踪”到“避撞”的平滑切换比单独做一个避撞逻辑要自然得多。4. APF与MPC结合架构设计、场景参数和关键实现4.1 两层框架的分工与配合逻辑既然确定了用势场法做参考线、MPC做执行层那两层具体怎么同步就成一个核心问题。我的做法是上层势场模块按100ms周期运行它读取当前车辆状态和经过追踪处理的障碍物信息输出一段局部参考路径包含未来若干步的目标位置、目标横摆角、目标速度下层MPC按20ms到50ms周期运行把参考路径作为跟踪目标同时把障碍物预测轨迹输入到避撞约束里。这里有一个分工细节需要强调势场法输出的路径并不要求绝对精确甚至可以有一定的抖动因为它只是给MPC一个“大致往哪开”的先验方向。MPC的优化器在这个参考路径附近搜索可行解同时把车辆动力学约束和障碍物约束考虑进去。换句话说势场法负责把复杂环境简化成一个可跟踪的意图MPC负责把意图落到车辆动力学上。这样做的好处是两层可以在各自的时间尺度上独立调试。势场偏移过大导致路径太激进我不用去动MPCMPC跟踪时横向误差偏大我不用去改势场参数。这对后期问题定位特别友好。4.2 双车道避撞场景的参数设计为了把方案跑通我搭了一个双车道高速公路仿真场景车道宽3.75米主车从右侧车道以80km/h巡航前方100米处有一台60km/h的缓慢移动目标车左侧车道前后有足够空档允许超车。主车需要在3到5秒内完成换道避让并回到目标车道继续行驶。主要参数表如下都是我自己调试后觉得比较稳健的值你可以作为起点使用参数数值说明主车初始速度80 km/h常见高速巡航车速障碍车速度60 km/h慢车场景轴距2.8 m典型家用车数值最大前轮转角0.45 rad转向物理极限前轮转角变化率0.1 rad/s转向平滑性约束最大允许侧向加速度3.0 m/s²舒适性阈值斥力作用半径ρ020 m距离太远不造成干扰引力增益k_att1.0目标吸引力系数斥力增益η2.0避撞强度系数预测时域N20MPC滚动优化步数采样时间0.05 sMPC控制周期需要注意的是不同车辆参数差异很大比如轴距长、质量大的车最大前轮转角限制通常更保守侧向加速度阈值也要相应降低。上面这组数据适合在仿真环境里跑通流程真车标定前必须重新获取实际参数。4.3 实现要点与核心伪代码具体实现时我建议先用离线回放的方式调通流程再上实时仿真。离线回放的意思是把传感器信息事先录制下来然后用这套APFMPC算法去复算控制指令对比实际轨迹和预期轨迹。这套流程跑顺了闭环节目再难也不会差太多。核心代码结构是一段Python风格伪代码用于说明主循环逻辑# 主循环 while simulation_running: state read_vehicle_state() # 当前车辆状态 obstacles get_obstacle_list() # 经过检测跟踪后的障碍物 # 上层人工势场法生成参考线 ref_line apf_plan_reference_line( statestate, obstaclesobstacles, target_lane_centertarget_lane_center, paramsapf_params) # 下层MPC优化控制指令 control_seq mpc_solve( statestate, ref_lineref_line, obstacle_predictionspredict_obstacles_trajectory(obstacles), vehicle_paramsvehicle_params, mpc_paramsmpc_params) # 只执行第一帧控制 apply_control(control_seq[0])这段代码只是一个示意结构真正跑起来还需要处理很多细节比如势场在离散栅格上的更新效率、MPC求解器的热启动、多目标加权系数在线调节等等。但架构思路体现得很清楚势场提供参考MPC做优化求解二者主从关系稳定。5. 仿真结果对比从“能换道”到“换得稳”5.1 基础场景前车慢行正常换道超车先看最基础的场景。主车以80km/h巡航前车以60km/h占道左侧车道空闲。势场法检测到前方车辆进入斥力范围后开始生成一条逐渐向左偏移的参考线。MPC跟踪这条参考线前轮转角一步一步加大车辆横向位置在约3.2秒内从本车道中心平稳过渡到左侧车道中心。这个场景下侧向加速度峰值大概在2.1m/s²左右全程没有触发3m/s²的舒适性阈值。车辆与障碍车的最小横向间距约为1.2米虽然横向间距不算特别大但足够确保安全。换道曲线上没有明显的锯齿和突变方向盘转角曲线也比较光滑。这套结果说明两点第一势场法生成的参考线方向正确没有把车引向障碍物第二MPC能够稳定跟踪参考线且没有因为动力学约束导致跟踪失败。基础场景跑通后我开始在这个基础上加难度。5.2 极限场景前车突然急刹第二个场景我设置了前车在接近后紧急制动车速在1秒内从60km/h降到20km/h。这个场景第一次跑的时候失败了势场斥力突然增大把参考线猛地推向左侧MPC为了跟踪这个激进参考线给出的横向加速度一度冲到4.8m/s²明显超出舒适范围。更严重的是车辆被推到左侧车道边缘后参考线又因为目标点恢复得太快MPC来不及收回车辆出现了一次明显的横向回摆。问题的根源是势场只按当前距离算斥力没有把前车的减速趋势提前反映到参考线上。解决办法我在前面已经提到给障碍物速度增加预测性扩展项。前车速度变化率越大斥力场的等效半径越远。这样车辆会在前车明显减速的初始阶段就提前开始转向而不是等距离逼近到阈值才突然反应。改进后的结果是主车提前约0.5秒开始横向偏移最小横向间距提升到1.6米侧向加速度峰值被压在3.0m/s²以内。这个场景的对比数据我整理成了表格方便你直观参考指标改造前改造后开始换道时刻距前车约25m距前车约35m侧向加速度峰值4.8 m/s²2.9 m/s²与障碍车最小间距0.7 m1.6 m路径是否回摆明显回摆基本平稳5.3 对比实验为什么不单独用APF或单独用MPC可能有人会问既然势场本身就能生成路径MPC又能直接做避撞为什么非要把两者组合起来。我单独跑过三个对比方案结论还挺明确。只用人工势场法时路径生成阶段容易在狭窄通道里振荡尤其是当旁边车道还有一辆静止车时左右斥力交替变化路径会呈波浪形。直接把这条路径作为轨迹的话执行器跟踪会在“贴左侧障碍”和“贴右侧障碍”之间来回切换换道过程会变得极其不稳定。只用MPC时如果不给参考路径而是直接让它从起点优化到目标点计算量会明显上升。因为优化器要同时搜索可行的横向偏移轨迹、纵向速度变化和转向动作相当于自己在一堆解里找那个满足所有约束的组合。结果是换道动作启动滞后在紧急避撞场景下尤其明显。APFMPC组合在三个方案里表现最稳起决定作用的点在于势场法给出的参考线极大缩小了MPC的搜索空间。MPC不需要从零开始找路径它只需要在“大致偏离多少、什么时候回正”这个局部范围内做精细调整。这也是工程上很实用的思路——能做到不是因为单层算法更聪明而是因为分工合理。6. 实操踩坑记录与参数调试经验6.1 三个高频率问题贴边、不可行、方向盘摆振仿真跑多了一定会遇到一些“理论书上没写”的问题。我印象最深的有三个。第一个是贴边问题。车辆绕过障碍物时会贴着车道边缘走再往旁边一偏就压线。排查下来是车道约束势场的增益k_lane设得太小斥力场把车推到旁边后没有足够的回拉力把它拉回车道中心。把k_lane从0.5调到1.2之后路径明显规整很多。第二个是MPC求解不可行。障碍物速度突变时硬约束可能把可行域压缩成空集求解器直接报错。处理办法是降低硬约束的触发条件增加一个低置信度的避撞软约束。这样系统永远有一组可行解只是可能在极端工况下稍微牺牲一点距离裕量。第三个是方向盘摆振。更换控制周期后有时候方向盘会在同一路段来回小幅摆动乘客能感觉到车辆“发飘”。这个问题的根源是输入增量权重w3太低控制器对高频指令变化不敏感。把w3提高两倍、同时把前轮转角变化率限幅从0.1rad/s降低到0.05rad/s后摆振立即消失。这些问题的共同点是表面症状在控制层但根源可能在规划层参数。调bug时一定要养成先看上层参考线是否有问题、再看下层是否跟得住的习惯。6.2 调试顺序与参数速查表我每次重开一个类似项目都会按固定顺序调参先调纯势场路径让路径在仿真里看起来顺眼会主动避开障碍且保持车道再调MPC跟踪误差权重确保正常直道和轻微弯道上的横向误差小于0.2米最后调避撞软约束权重处理紧急工况。参数调得过高或过低现象完全不同。下面这张速查表是我自己的经验总结参数过高表现过低表现建议起始值引力增益 k_att路径太直避障滞后车辆龟速接近目标1.0斥力增益 η路径过度绕行曲率大车辆离障碍过近2.0车道约束增益 k_lane换道动作僵硬压线、贴线行驶1.0MPC跟踪误差权重 w1控制量过大、振荡参考线跟踪不准100MPC控制量权重 w2响应迟钝指令过于激进10MPC控制增量权重 w3动作太慢方向盘抖动20避撞软约束权重 w4过早远离障碍距离过近、危险50表格只是起点不同车型、不同路况下具体值会有浮动。调试时每次只改一个参数记录现象不要同时动三个以上否则你根本不知道问题是谁引入的。6.3 移入高保真仿真或实车前要补的功课把这套算法从简单仿真搬到高保真仿真甚至实车前有三个功课必须做。第一个是时延补偿。实车感知链路通常有100ms到200ms延迟这在我的简化仿真里其实没有体现。如果不做时延补偿MPC算出来的控制指令总比真实状态慢半拍尤其高速换道时非常明显。我在预测模型里加入了“状态延迟补偿”环节把当前时刻往前推一个时延周期再送入MPC优化。第二个是障碍物轨迹预测。我的简化仿真直接给了准确的障碍物位置和速度但实车上这些值是带噪声的。我在读取障碍物信息后用匀速运动模型加卡尔曼滤波做了预处理把位置噪声平滑后再输入势场否则势场力中心会随着噪声抖动参考路径会肉眼可见地晃。第三个是舒适性指标的标定。3m/s²的侧向加速度阈值是我自己设定的舒适上限不同乘客的评价标准差异很大。如果你做的是商务车或者消防车这个阈值差异会非常明显。建议在目标用户实际体验测试后再回头标定这个约束值。一片真心话收尾整套APFMPC方案调试下来我最大的体会是不要指望任何一个单层算法能解决所有问题优秀的方案往往来自于合理的分工。势场法把环境压缩成方向信息MPC把方向信息翻译成车辆的物理动作两者互补问题就被拆成了两个可验证的小问题。最后分享一个我个人的小习惯调试这类系统永远先跑无障碍场景再跑单障碍场景再跑双障碍场景一层层加难度。一旦参数乱了你要能快速判断是上层规划错了还是下层跟踪炸了。这个分层排查的思路比多一个算法、多两个参数都管用。
阅读完成 · 觉得有帮助？
