自动驾驶局部规划算法深度解析：原理、选型与调试实战

做自动驾驶规控这行局部规划几乎是每天都要面对的核心问题。新入行的同事往往上来就问用哪个局部规划方法比较好但真正在项目里跑过各种场景之后会发现方法本身没有绝对的好坏关键看你解决什么问题、在什么约束条件下用、跟上下游怎么配合。这篇内容把我在实际项目里用过的局部规划方法梳理一遍从原理、选型到调试经验一次性说清楚希望能帮正在做规控或者准备入行的朋友少走些弯路。1. 先理清局部规划在整个规控链路里的位置1.1 从全局规划到执行控制的四层架构要理解局部规划得先看清楚它在整个自动驾驶规控链条中的位置。通常我们把规控拆成四层全局规划Route Planning、行为决策Behavior Decision、局部规划Local Planning、运动控制Motion Control。全局规划负责从起点到终点的大致路径一般基于高精地图或者导航拓扑规划频率很低更新间隔在秒级甚至更长。行为决策负责回答接下来该走哪条车道、要不要变道、要不要让行这种带有语义色彩的判断。局部规划就是在行为决策给出了明确意图之后在数十米范围内生成一条安全、平滑、满足动力学约束的轨迹。最后运动控制把这条轨迹转化为油门、刹车、方向盘指令。很多刚接触规控的朋友容易把局部规划和路径规划混为一谈实际上两者有本质区别。路径规划只输出一条几何路径也就是一条静态的线不关心速度是多少、转弯的时候会不会侧翻、避障时能不能刹得住。局部规划输出的是带时间戳的轨迹Trajectory每一个点都包含位置、速度、加速度、航向角甚至曲率信息并且要在每帧重新计算。也就是说局部规划是一个实时滚动的优化或搜索过程通常频率在10到20赫兹甚至更高它既是规划问题也是动态可行性验证问题。1.2 局部规划要解决的核心矛盾安全与效率把局部规划拆到底就两个目标在约束下生存在空间中前进。约束包括车辆本身的动力学约束最大转角、最大加速度、最大横摆角速度、物理世界约束障碍物边界、道路边沿、交通规则以及行为层下发的语义约束比如只能在这个车道内运动。效率则体现在能不能用合理的速度平稳到达目标参考点而不是频繁刹车、反复原地调整方向。实际项目中这两个目标经常互斥。比如在路口右转时如果前方有静止的障碍物占据了部分路权你就得在提前减速绕开和压着障碍物边界极限通过之间做权衡。不同的局部规划方法在这个权衡上的表现差异很大采样类方法倾向于枚举很多候选轨迹然后选最优优化类方法则更擅长在连续空间里找满足所有约束的解但算力消耗也更高。这就是为什么选型之前要先想清楚自己面对的场景复杂度。简单园区低速接驳和高速干线物流对局部规划的要求完全不在一个量级。2. 三类主流局部规划方法的底层逻辑2.1 基于采样的方法把连续问题离散化采样法的核心思想很简单在可行空间里撒点或者撒一批轨迹然后用评价函数挑一个最好的。代表性的主流方案有动态窗口法DWA和轨迹采样法比如在Frenet坐标系下生成横纵向候选轨迹集。这类方法的优点是直观、容易实现、调试起来看得见摸得着缺点是性能跟采样密度强相关稀疏了容易找不到可行路径密集了算力吃紧。DWA的思路是把规划收敛到速度空间在当前状态下根据车辆的加速度限制模拟出未来一小段时间内可以到达的速度组合对每一组线速度和角速度都模拟出一条短轨迹然后用一个评价函数打分选得分最高的那组速度作为当前的期望速度。它的本质是一个滚动优化因为每帧都在重新采样、重新评价所以对动态障碍物也有一定的鲁棒性。轨迹采样法在思路上更进一步不只在速度空间里采样而是把横纵向运动解耦开在Frenet坐标系下撒出不同的横向偏移轨迹和纵向速度曲线再组合成完整的候选轨迹。我在项目里常用这种方式做换道和避障因为横向轨迹可以直接用五次多项式拟合纵向可以用梯形速度曲线或者S形速度曲线控制加减速组合出来的轨迹天然平滑。这类方法最大的好处是每一帧产出的轨迹本身就是一条完整、可解释的曲线控制层拿到手很容易跟踪。2.2 基于图搜索的方法在栅格里找最短路另一类是搜索法比如A*、Dijkstra、混合A*它们通常工作在栅格地图或者状态网格上。局部规划用这类方法时通常是在感知模块输出的**局部占据栅格Occupancy Grid**上跑一轮搜索。纯几何搜索出来的是折线路径还需要经过平滑或者转换成轨迹才能交给控制。混合A和普通A的区别在于节点空间从二维栅格扩展到了三维甚至四维状态空间包含位置和航向角有的还包含速度维度这样搜索出来的路径能隐式满足车辆的最小转弯半径约束。代价是搜索空间爆炸计算时间明显增加所以在局部规划层面混合A*通常只用来做低速泊车或者极限脱困很少用在高速道路上。高速场景下搜索法更多是退守的解法当采样法或者优化法求解失败时如果障碍物本身比较规则、道路空间相对开阔搜索法也能救急找出一条可行通道。从工程角度看搜索法的优势在于完备性只要栅格分辨率设置合理、启发函数不保守过头它能稳定找到一条可行连通路径。问题则是路径质量差有锯齿和多余转折而且对动态场景的适应能力弱因为全局搜索本质上是在一个静态快照上做规划。2.3 基于优化的方法把规划变成数学问题优化类方法把局部规划建模成一个约束优化问题目标函数包含对偏离参考线的惩罚、对加速度或者急动度的惩罚、对舒适度的惩罚约束包含动力学方程、障碍物距离约束、状态边界约束然后交给数值优化器求解。最有代表性的就是模型预测控制MPC严格来说它更多被看作控制方法但在规控一体化的架构里MPC被用来做带约束的运动规划也越来越常见。这块我单独展开聊一下。常用的是线性MPC把车辆运动学模型在每一帧的参考工作点附近线性化得到线性时变系统然后构造二次规划QP问题用序列二次规划迭代求解。非线性MPC则更接近原生模型直接对非线性模型求解精度更高但求解耗时显著增加。当前大部分量产平台还是靠线性MPC或者QP问题因为可以在毫秒级以内求出稳定解。优化类方法最大的优点是能精确处理复杂约束尤其适合狭窄通道、强约束避障这些场景。缺点是调参是个精细活目标函数权重设置不合理很容易出现轨迹震荡或者求解失败而且优化器在极端情况下可能找不到可行解需要另外准备兜底方案。3. 几个代表性算法的拆解与实操要点3.1 DWA动态窗口法速度空间里的贪心选择DWA在低速、小场景里非常能打比如园区巡逻车、服务机器人很多开源导航框架里默认的局部规划就是它。它的工作流程可以拆成四步速度采样、轨迹模拟、轨迹评价、速度输出。速度采样这一步决定了它的能力边界采样时先根据当前速度、最大加减速度和控制周期算出一个可达速度窗口再在这个窗口里按分辨率撒速度点。轨迹模拟则假设车辆在接下来的几秒内以恒定速度运动用运动学模型推出轨迹。评价函数通常包括三个维度对目标点的朝向偏差、与最近障碍物的距离、当前速度的大小。三个维度都要归一化否则量纲差异会直接吞掉某个维度的影响。实操中我踩过不少坑这里提几个重点。第一目标点附近震荡问题。DWA只有一个当前目标点没有对路径的记忆如果目标点设置恰好位于障碍物背后车辆会反复左右摆动。解决办法通常是把全局路径上距离当前位置一定前视距离的点作为局部目标点而不是直接用全局终点。第二评价函数的权重非常敏感。我曾经把距离障碍物项的权重调得过大结果车辆在空旷区域速度飞快稍有障碍物就急停跟车体验很差。后来我把朝向偏差项改成与全局路径的横向偏离整个车的走线就稳了很多。第三DWA对高速场景基本无能为力因为它模拟的是恒定速度没有把纵向加速度规划进去所以高速场景别硬用。3.2 TEB时间弹性带把轨迹当橡皮筋TEB的思路很独特它把轨迹看成一条带时间戳的弹性带。每个轨迹点与前后点的连线构成一个时间弹性带结构通过把路径张力、障碍物距离、时间消耗、运动学约束组合成一个超图Hypergraph用图优化框架求解最小化问题。直观理解就是弹性带本身想缩短省时间又怕碰到障碍物避开还要满足车辆转向能力动力学。这三股劲互相拉扯最后收敛到一个满足约束的状态。我用TEB做过差速底盘和类车底盘的项目体会是它对窄道通过和原地掉头这类场景尤其擅长因为优化过程会自动把轨迹往能过的方式去拉。但要注意几个关键参数。一是时间分辨率dt它决定相邻两个轨迹点之间的时间间隔太小会大幅增加优化变量数量太大又会让轨迹不够精细。我在实践中的经验是根据车速设定中低速场景dt取0.3秒左右算力和精度的平衡相对好。二是轨迹点数量太少则轨迹太粗糙无法表达复杂的避障绕行动作太多则减少优化成功的概率因为超图的规模变大了。三是罚函数权重包括距离障碍物权重、时间最短权重、动力学约束权重这三个调试优先级最高。TEB一个比较麻烦的问题是它对初始化轨迹敏感。如果初始轨迹穿过了障碍物优化过程可能因为罚函数不够大而收敛不到全局可行解最终输出仍然穿过障碍物。我在项目里的对策是初始化轨迹先从A*搜一条路径然后对路径点做平滑再作为TEB的初始带这样极大降低了失败概率。另外TEB在动态障碍物场景下需要配合障碍物速度预测如果有一辆突然插入的车弹性带会剧烈变形输出的轨迹可能出现跳变。我通常会在优化结果后面再加一道低通滤波或者平滑处理才交给控制层。3.3 MPC模型预测控制滚动的优化窗口MPC用在局规上最核心的一点是它把规划和控制统一到了一个框架里。传统的规划-跟踪架构中规划层生成一条参考轨迹控制层去跟踪两层之间如果参数不匹配或者模型不一致就会出现误差累积。MPC的做法是基于系统的预测模型在每一个控制周期滚动求解未来一段时域内的最优控制序列并且中的第一个控制量应用给系统下一帧重新求解。因为每次求解都考虑了未来时域的约束所以它天然能做前馈和避障。实现MPC局规的关键点在三个地方预测模型、约束建模、代价函数。预测模型最简单的做法是使用自行车运动学模型状态量取位置x、位置y、航向角控制量取前轮偏角和加速度。约束建模要区分硬约束和软约束。硬约束是车辆物理极限比如前轮转角最大偏角、加速度上下限这种你要是写成软约束优化器很可能会把速度一路推到极限来满足目标非常危险。软约束给了可缓冲的边界在这种惩罚下优化器倾向于让轨迹离边界远一点但紧急情况也允许短暂违反。代价函数里最常用的是三组对参考路径的跟踪偏差、控制量的平滑惩罚、对急动度加加速度的惩罚用来保证舒适性。实际项目里MPC最头痛的问题就是算力和稳定性。非线性MPC在复杂模型下耗时常爆表而线性化MPC又存在模型失配问题尤其在大角度转向时误差明显。我个人的工程建议是分级处理高速主干道场景用线性MPC做轨迹跟踪加小范围避障在接近极限工况之前把问题交给上层决策去减速或重规划不要让MPC去处理严重威胁安全的大扰动场景否则一旦求解超时或者无解风险控制就变得很难。3.4 Frenet框架下的轨迹采样把横纵向解耦Frenet坐标系是现在很多量产项目的局部规划主流方案因为它的表达方式非常贴合驾驶逻辑以参考线为基准横向偏移量表示偏离车道中心的程度纵向位移表示沿道路前进的距离。在这个坐标系下车辆的运动被拆成两个独立的一维问题——横向运动和纵向运动分别写出多项式方程再合成为最终轨迹。横向轨迹通常用五次多项式拟合给定起终点的横向位置、横向速度、横向加速度六边界条件刚好解六个系数。纵向轨迹则搭配速度规划常用梯形速度曲线或者S形速度曲线来描述加匀速减速三个阶段。这种方案的工程优势非常明显。第一横纵向分开之后换道、跟车、让行这些场景的处理机制是统一的横向决定怎么移过去纵向决定怎么控制距离和速度组合起来就能表达边减速边变道的复杂行为。第二采样出来的轨迹全部带时间戳和二阶导数控制层可以直接使用不需要再做额外平滑。第三每条候选轨迹的语义清晰评价函数可以加变道惩罚压线惩罚这样语义化的项比纯几何评价更容易调参。但实际使用里要注意几个细节。一是参考线的质量问题参考线来自全局路径如果全局路径带有高频抖动或者曲率跳变Frenet坐标系里的偏移量会映射出畸形的笛卡尔轨迹因此在把全局路径投影到Frenet之前需要先做一次平滑。二是横纵向采样的参数要跟车辆能力对齐横向采样范围不能超过车辆转向能力纵向采样最大加速度不能超过车辆制动极限否则你采样出一堆控制层根本跟踪不上的轨迹看起来每一帧都选得很合理实际车辆表现却总是不够贴地。三是最优轨迹的跳变问题因为每一帧都在重新采样重新评价如果评价函数里没有对上一次所选轨迹的偏置项很容易出现上一帧向左让、这一帧又向右绕的抖动必须加一个轨迹一致性的惩罚项或者对连续帧做轨迹融合。4. 工程落地里的那些坑4.1 参数调试的先后顺序局部规划调参是一个很容易让人暴躁的过程因为你面对的评价函数里可能有七八个权重每个都影响全局行为。我这些年摸索出的经验是按优先级排好顺序一层一层调。第一步先把障碍物距离相关的阈值和权重调对。目标是让车在任何时刻都不会撞上静态障碍物这是安全底线。如果这一步的阈值设得太激进后面所有的平滑性调优都是在危险边缘试探。第二步调动力学约束相关参数比如最大车速、最大转角、最大加速度让输出的轨迹满足车辆物理能力。第三步调跟踪性也就是轨迹贴近度看车辆在直线和弯道上的表现。第四步才调舒适性对急动度、曲率变化的惩罚权重逐步加上去。顺序错了的结果通常是你花了一整天在调舒适度权重车依然会在某个弯道里切角压线因为根子问题是障碍物膨胀距离太小视觉上循迹一直在极限状态任何舒适性权重都没办法弥补。4.2 动态障碍物怎么处理很多初版方案里对动态障碍物的处理就只是把它们当成静态地图上的膨胀圆实时更新但这样会有两个明显问题。第一高速相对运动下当前帧的障碍物位置到车辆到达时障碍物已经移动了较多距离规划轨迹看起来没碰实际执行时早就撞上了。第二动态障碍物的运动不确定性被完全忽略一个正在缓慢变道的车辆和一个静止停着的车在Costmap里长得一样但你应对方式应该完全不同。工程上的常见处理是给障碍物加一个速度场预测。简单的做法是把动态障碍物看作恒定速度运动预测轨迹在未来N秒内的位置变化做法可以再重一点用恒转弯率模型或车道模型预测动态障碍物的未来位置再带入到规划的评价函数中。注意在实现层面不要把一个动态障碍物的完整未来轨迹当成另一堵移动的墙那样会把可行空间压缩得过狠导致频繁无解。我在某个项目中采用的做法是以障碍物预测位置为中心生成一个随时间衰减的椭圆叫做风险场在候选轨迹评价时遍历轨迹点与风险场中心的关系距离越近惩罚越大同时考虑时间对齐也就是不同时间戳的轨迹点对应不同时刻的风险场位置。这样算下来不但安全而且行为也更接近真实驾驶员的预判动作。4.3 全局路径和局部规划怎么配合全局路径和局部规划是典型的宏观指导细节关系。全局路径不需要也不可能保证十分平滑精准它只需要提供一个可信的参考方向局部规划则要实时应对感知到的具体障碍物布局在参考方向基础上做出避让动作。如果全局路径的定位精度很差局部规划经常沿着偏差路径走就会频繁出现全局路径让左拐局部规划因障碍物直接右拐的冲突这本质上不是局部规划算法的缺陷而是上游信息质量不过关。我在项目里通常会在全局规划和局部规划之间加一层参考线平滑与坐标投影模块。全局路径首先经过三次B样条平滑然后以固定间隔重采样再把自车位置和感知障碍物都投影到这条参考线上构建Frenet坐标局部规划在Frenet系中完成计算。这样即使全局路径有局部毛刺参考线平滑会将其滤除。另外全局路径关心的场景问题当局部规划由于临时障碍物绕行很远绕完后要重新回到全局路径上如果回切时机不对容易在偏移-回切之间反复横跳。常用做法是设定一个横向偏移阈值当车辆横向偏移过大时强制局部规划以更保守的速度先回归参考线而不是边高速绕行边往回切。5. 常见问题与排查实录5.1 高频问题速查表这里把我在不同项目里遇到的高频问题统一整理成一份速查表方便定位问题方向。注意每一行的解决思路都只是起点具体数值要按照你自己车的参数重新标定。现象可能原因排查思路车辆在目标点附近原地打转局部目标点设置导致震荡DWA朝向偏差项权重不足改用参考线前视目标点增加朝向偏差权重的量级缩短轨迹采样时间或者降低最大速度经过窄通道时频繁卡住障碍物膨胀半径过大采样密度不足评价函数对通道宽度不敏感减小膨胀半径提高横向采样分辨率在代价函数里加入通道通过性评估项轨迹频繁左右摆动评价函数缺少上帧一致性项权重跳变整体采样时间太长加轨迹一致性惩罚降低单帧最大规划时长对连续帧轨迹做中值滤波高速遇到障碍物急刹纵向速度规划没有提前量最大减速度设置过大预测时域太短增大规划时间域限制单帧最大减速度在纵向速度规划中加入舒适度权重拐弯时轨迹切内线严重全局参考线曲率过大局部规划横向约束太松MPC模型线性化误差对参考线做平滑强制横向边界约束换成精度更高的非线性模型增加模型失配补偿动态障碍物插入时无避让动作障碍物速度预测缺失感知延迟太大评价函数对动态物惩罚不够加入恒速预测或恒转弯率预测减小感知延迟引起的轨迹失效时间动态物风险场权重要单独拉高5.2 一次真实调试经历复盘有一次在模拟测试中车在空旷场地里直线行驶前方远处出现一个静止障碍物但车始终不提前变道直到距离很近才开始打方向而且动作幅度很大、很突兀。最初我以为是局部规划的感知范围不够后来逐个排查因素发现是全局路径的参考线离障碍物本来就比较近在Frenet系中障碍物对横向轨迹的惩罚已经产生但纵向速度规划还在按前方畅通的模式加速导致候选轨迹里头出路太窄的时候速度还很快。解法是把横纵联合评价做的时候把障碍物纵向距离纳入到速度规划的前视范围里当检测到前方参考线附近有障碍物时纵向速度规划自动进入提前减速模式而不是等横向避让轨迹已经成形才减速。后来又发现这个前视距离如果太大车辆在高速道路上会很早减速被后车追尾的风险增加。于是最终调成基于相对速度和距离的动态前视公式速度快、距离远时前视距离大一点速度慢时压缩前视找到折中方案。这类问题在真车上尤其值得注意因为整个系统和平顺性直接影响到乘坐体验和安全预期。6. 一些只会在项目经验里才有的细节到这里各种方法的优劣势和调参路径都梳理过了最后分享几个偏手熟的细节。第一局部规划的调试一定要有可视化工具不只是画候选轨迹还要把评价函数的每个分量都单独画出来。我习惯把朝向偏差的代价、障碍物距离的代价、速度惩罚的代价分别用不同颜色的热力图叠加在轨迹预览图上这样一旦某一帧选了奇怪的轨迹立刻就能看出是哪个代价异常而不是猜测。现实中不同项目的可视化面板可以自己基于老牌的仿真平台二次开发工作量不大对调试效率的提升非常明显。第二轨迹输出前要做一次安全校验无论用哪种方法。优化器返回的结果在数学上通常满足了约束但要考虑到上游感知的噪声、控制器的跟踪误差因此校验里至少要检查三件事跟当前车道上所有障碍物的最小距离是否大于安全冗余轨迹终点速度和运动方向是否满足下一帧规划的正常初始化条件整条轨迹的曲率是否连续有没有不可执行的尖角。任何一条不过都要降级处理最简单的降级就是保留上一帧轨迹的一部分继续跟踪同时重新规划。第三规划结果不要直接发给控制层建议中间过一个轨迹平滑滤波器。滤波算法的选择不用太复杂常用的三次B样条拟合或者移动平均都行关键是滤波参数要能够自适应速度高速时滤波窗口加大让轨迹更平滑低速时窗口缩小让轨迹更精确点。这个细节很多人容易忽略但往往就是它决定了实车上的乘坐体验。第四模拟器里的场景和真车实况有本质的差异。仿真里障碍物位置有真实值感知噪声小而在真车上局部规划直接吃的是融合后的障碍物列表目标物在切换、漏检、延迟、跳动这些数据质量问题对轨迹连续性的干扰比重远大于算法参数本身。所以我的建议是在做真车集成测试之前先给感知模块注入一组录制好的带噪声数据把局部规划的鲁棒性在数据较差但数据源稳定的前提下调到位再上车。这一步可以帮你把算法问题从系统问题里分离出来省掉大量的联调时间。这些经验说到底都是围绕一个原则局部规划不是一个孤立的优化问题它整个规控闭环里的一个环节。再好的规划算法也扛不住上游的烂数据和下游的僵硬执行反过来一个合理的系统架构可以让基础的规划算法也跑出非常稳定的表现。希望这翻整理能帮你把局部规划这件事想得更透一点。