1. 从“车怎么自己找路”说起路径规划到底在解决什么问题很多人第一次接触自动驾驶脑子里冒出来的第一个问题往往是“车怎么知道往哪开”。摄像头、激光雷达负责“看”感知模块负责“认”而真正决定方向盘往哪打、油门刹车怎么配合的是下游的路径规划。你可以把整套自动驾驶系统想象成一个人开车眼睛和耳朵是传感器大脑皮层负责理解看到的东西而真正下达“左转、减速、绕开前面那辆慢车”指令的是大脑里那套决策与规划机制。路径规划就是这套机制里最核心的一环。我刚开始啃这块内容的时候最大的困惑不是算法本身有多难而是不知道这些算法各自适合什么场景。Dijkstra、A*、BFS、DFS、贪心最佳优先……名字一大堆论文里每个都说得头头是道但真放到一辆车上哪个能用、哪个不能用、哪个只能用来做预处理心里完全没底。后来跟着几个实际项目摸爬滚打才慢慢理清楚这些经典搜索算法并不是互相替代的关系而是各自守着一块地盘解决不同粒度、不同约束下的寻路问题。这篇文章想做的事情很明确把自动驾驶路径规划里最常被提到的几类经典搜索算法从它们到底在搜什么、为什么这么搜、搜出来的结果能不能直接给车用这三个角度讲透。不管你是刚入门自动驾驶的学生还是做机器人、游戏AI、物流调度想借鉴寻路思路的开发者都能从里面找到能直接上手的东西。我不会只给你甩一堆伪代码而是会把每个算法背后的直觉、参数怎么调、什么情况下会翻车都掰开揉碎讲清楚。先给一个全局认知在自动驾驶的规划栈里搜索算法通常不是直接输出“方向盘转角”的它更多是在离散化的空间里找一条从起点到终点的可行路径这条路径再交给后续的轨迹优化模块去平滑、去满足车辆动力学约束。所以理解搜索算法关键是理解它工作的那个“图”是怎么建的以及它在这个图上找的“最优”到底是什么意思。2. 把连续世界掰成图搜索算法赖以生存的离散化前提2.1 为什么不能直接在连续空间里搜现实世界是连续的车的位置可以用一对浮点数精确表示理论上从A点到B点有无数条路径。但计算机搜索算法几乎都建立在图Graph这种数据结构上——图由节点和边组成是离散的。所以第一步必须把连续的行驶空间转换成一张图。这个转换过程叫离散化它直接决定了后面所有搜索算法的效果上限。我见过不少新手一上来就研究A的启发函数怎么设计结果路径搜出来歪歪扭扭、贴着障碍物边缘走问题根本不在A而在于他建的那张图太粗糙。离散化没做好再牛的搜索算法也救不回来。这一点必须先建立意识。常见的离散化方式有三种各有各的脾气栅格图Grid Map把空间切成一个个小方格每个格子是一个节点相邻格子之间有边。最直观实现最简单但分辨率一高节点数量就爆炸。一个100米乘100米、分辨率0.1米的区域就是100万个格子。路网图Roadmap只在“有意义”的地方放节点比如道路中心线、车道线的采样点、路口的关键点。节点少搜索快但依赖高精地图的准确性。状态格State Lattice不仅考虑位置还把车辆的朝向、甚至曲率也编码进节点。节点之间的边是预先算好的、满足车辆运动学的短轨迹。这是自动驾驶里非常实用的一种方式因为它搜出来的路径天然就是“车能开出来的”。2.2 栅格分辨率与搜索代价的权衡计算这里给一个实打实的计算帮你建立“分辨率不是越高越好”的直觉。假设我们要在一片200米×200米的园区里做路径搜索用栅格图。分辨率单边格子数总节点数四连通边数约八连通边数约1.0 米2004万8万16万0.5 米40016万32万64万0.2 米1000100万200万400万0.1 米2000400万800万1600万从1米降到0.1米节点数翻了100倍。Dijkstra这类算法的时间复杂度大致是O(E log V)E是边数V是节点数节点数翻100倍耗时可能翻上百倍。而实际收益呢路径的精细度提升但车辆的宽度就有近2米0.1米的精度对“能不能通过”这个判断来说边际收益极低。提示在自动驾驶的全局路径规划阶段栅格分辨率通常取0.2米到0.5米就够用了。真正需要高精度的地方交给局部规划和轨迹优化去做。全局搜索要的是“快”和“大致对”不是“精”。2.3 节点连接方式四连通还是八连通栅格建好后还要决定节点之间怎么连。四连通只允许上下左右移动路径会呈现明显的“曼哈顿风格”拐来拐去都是直角。八连通允许斜向移动路径更自然但斜向移动的实际距离是格边长的√2倍如果代价函数里不体现这个差异搜出来的路径会“偷懒”走斜线导致实际行驶距离被低估。我的经验是如果只是做可行性搜索八连通加正确的对角线代价√2倍是更好的选择。如果后续有轨迹优化模块兜底四连通也能用因为优化模块会把直角磨圆。但如果你偷懒用八连通却不修正对角线代价A*会系统性地偏向斜线路径最后车开出来的路线会莫名其妙地“抄近道”这在有障碍物的场景里很危险。3. 广度优先与深度优先理解搜索的两极才能理解A*为什么聪明3.1 广度优先搜索像水波一样均匀扩散广度优先搜索BFS的逻辑极其朴素从起点开始先把所有距离起点“一步”的节点访问完再访问“两步”的以此类推。它像往水里扔了块石头波纹一圈圈均匀向外扩。BFS有一个非常重要的性质在边权都相等的图里它找到的路径一定是最短的经过的边数最少。为什么因为它是一层一层推进的第一次碰到终点时走的层数一定是最少的。这个性质让BFS成为很多算法的基础。但它的致命伤也在这里它是“盲目”的不管终点在哪个方向它都均匀地向四面八方扩散。如果终点在起点正东方向BFS还是会先把西边、北边、南边的节点都翻一遍浪费大量算力。在自动驾驶里BFS很少直接用来做全局路径规划因为道路网络通常不是均匀的栅格而且我们往往对“往哪个方向走”有很强的先验知识。但BFS的思想无处不在比如在变道决策里判断“当前车道和相邻车道未来几秒的占用情况”本质上就是一种状态空间的广度扩展。3.2 深度优先搜索一条道走到黑深度优先搜索DFS和BFS正好相反它从起点出发沿着一条路一直走走到死胡同再退回来换一条。它不需要存储整层的节点内存占用小但找到的路径通常又长又绕而且不保证最短。DFS在自动驾驶路径规划里直接用的场景更少但它在拓扑地图的连通性检查里很有用。比如判断“从当前路口出发能不能不经过某个施工区域到达目的地”DFS可以快速给一个是或否的答案不需要关心路径长短。另外在迷宫类场景比如地下停车场里如果只要求“能出去”DFS配合一些剪枝也能凑合用。3.3 两者的共同短板与启发式搜索的动机BFS和DFS的共同问题是它们都没有利用终点位置的信息。BFS均匀扩散DFS乱撞它们都不知道“终点在哪个方向”。这就引出了一个关键思想如果我能给每个节点一个“离终点还有多远”的估计搜索时优先扩展那些估计值小的节点是不是就能更快找到终点这就是启发式搜索的核心动机。而A*就是把“已经走了多远”和“估计还要走多远”结合起来的最经典算法。理解BFS和DFS是为了理解A*到底在哪些地方做了改进——它不是凭空冒出来的而是针对盲目搜索的低效有针对性地加了一个“方向感”。4. Dijkstra不带方向感的“最稳选手”什么时候反而比A*更合适4.1 Dijkstra的核心机制贪心地扩展最近节点Dijkstra算法解决的是带权图上的单源最短路径问题。和BFS只处理边权相等不同Dijkstra能处理边权各不相同的图——这在自动驾驶里太重要了因为不同路段的代价本来就不一样直道代价低转弯代价高靠近障碍物的代价更高。它的工作方式是这样的维护一个“待处理节点集合”每次从里面取出当前累计代价最小的那个节点把它标记为已确定然后更新它所有邻居的累计代价。重复这个过程直到终点被确定或者集合为空。你可以把它想象成从起点开始不断问“目前我能确定的最短路径能到哪个节点”然后以这个节点为中转站看看能不能让其他节点变得更近。它是一圈一圈“确定”下来的和BFS的均匀扩散有点像但扩散的边界不是按层数而是按累计代价。4.2 为什么Dijkstra在自动驾驶里依然有一席之地A比Dijkstra多了启发函数通常更快那Dijkstra是不是就过时了不是。在自动驾驶里Dijkstra有一个A替代不了的优势一次搜索可以得到起点到所有节点的最短路径。这个性质在什么场景下有用举个例子车辆在路口需要评估“去左边车道、直行车道、右边车道”哪个更优。如果用A*你得跑三次搜索每次针对一个终点。而用Dijkstra从当前位置跑一次就能得到到所有候选目标点的最短代价直接比较就行。再比如做全局路由时需要知道从当前位置到地图上任意一个充电站、任意一个出口的代价Dijkstra一次搞定。另外Dijkstra不依赖启发函数所以它不会因为启发函数设计不当而搜出次优路径。A*如果启发函数高估了真实代价不可采纳就可能错过最优解。Dijkstra没有这个风险它是“稳”的代名词。4.3 代价函数设计Dijkstra真正的灵魂所在Dijkstra本身很简单难的是边权怎么定。在自动驾驶里代价函数往往是一个加权和cost w1 * 距离 w2 * 转弯惩罚 w3 * 障碍物接近惩罚 w4 * 车道偏好惩罚 ...我踩过的一个坑是一开始只用了距离作为代价结果搜出来的路径虽然最短但紧贴着障碍物边缘走坐上去体验极差而且稍微有点定位误差就撞上了。后来加了“障碍物接近惩罚”让路径离障碍物至少保持一定距离舒适性和安全性都上来了。还有一个细节转弯惩罚要按角度累积。不是“转一次弯加固定分”而是“转的角度越大惩罚越高”。这样搜出来的路径会倾向于少转弯、转小弯更符合人类驾驶习惯。注意代价函数的权重没有标准答案必须根据实际场景调。园区低速场景可以更看重舒适性高速场景必须把安全性权重拉满。调权重的过程本质上是在定义“什么叫好路径”。5. A*给搜索装上“方向感”但启发函数是把双刃剑5.1 A*的评估函数拆解g和h各管什么A*的核心就一个公式f(n) g(n) h(n)g(n)从起点到节点n的实际累计代价这个是算出来的实打实。h(n)从节点n到终点的估计代价这个是猜的叫启发函数。f(n)对“经过节点n的完整路径总代价”的估计。搜索时A每次从待处理集合里取出f值最小的节点来扩展。g保证它不会忘记已经走过的路h给它方向感让它优先往终点方向探索。如果h设计得好A扩展的节点数会远少于Dijkstra速度快很多。5.2 启发函数的可采纳性与一致性两个必须搞懂的概念可采纳性Admissibilityh(n)永远不能高估从n到终点的真实代价。只要不高估A就保证能找到最优解。为什么因为如果h高估了A可能会觉得某条路“看起来很远”而放弃它结果那条路其实很近。一致性Consistency也叫单调性要求h(n) ≤ 实际边代价(n, n) h(n)。一致性比可采纳性更强满足一致性的启发函数一定可采纳反之不一定。一致性带来的好处是每个节点只需要被扩展一次实现更简单效率更高。在栅格地图里最常用的启发函数是曼哈顿距离四连通和欧几里得距离八连通。欧几里得距离是可采纳的因为它就是两点间直线距离不可能比实际路径更长。但如果用八连通且对角线代价是√2倍格边长那欧几里得距离依然可采纳但一致性需要验证——实际上在标准八连通栅格上欧几里得距离是满足一致性的。5.3 启发函数权重的调节在最优与最快之间走钢丝有时候我们愿意牺牲一点最优性换取更快的搜索速度。做法是给h乘一个大于1的权重f(n) g(n) w * h(n), w 1这叫加权A*。w越大搜索越偏向终点方向扩展节点越少速度越快但路径可能不是最优的。w1时就是标准A*保证最优。w很大时退化成贪心最佳优先搜索快但路径质量差。我的经验是在自动驾驶的全局规划里w取1.1到1.5之间比较合适。再大就容易搜出明显绕远的路径虽然后续优化能补救一些但没必要给下游添麻烦。如果场景对实时性要求极高比如高速上紧急避障的局部搜索可以临时把w调大先保安全再谈最优。5.4 A*在自动驾驶里的典型用法与局限A*在自动驾驶里最典型的用法是全局路径规划在高精地图的路网图上从当前位置搜到目的地给出一条粗略的行驶路径。这条路径不需要很精细因为后面还有局部规划和轨迹优化。但A也有明显的局限。第一它假设图是静态的而实际道路上的障碍物是动态出现的。第二它搜出来的路径是折线不满足车辆运动学约束车不能直接沿着折线开。第三当环境变化时A需要重新搜索实时性压力大。所以实际系统里A*通常只负责“大方向”动态避障交给局部规划器比如基于采样的RRT或者基于优化的方法。6. 从算法到上车经典搜索在真实规划栈里的位置与调参心得6.1 分层规划架构搜索算法站在哪一层真实的自动驾驶规划系统通常是分层的全局路由层用Dijkstra或A*在路网图上搜一条从起点到终点的粗略路径关注的是“走哪条路”不关心具体怎么开。行为决策层决定“跟车、换道、超车、让行”等宏观行为这一层更多用状态机或规则搜索算法用得少。局部规划层在全局路径附近结合实时感知到的障碍物搜一条短距离的、可行驶的轨迹。这一层可能用A*的变种、状态格搜索、或者采样类算法。轨迹优化层把局部规划给出的折线路径平滑成满足车辆动力学约束的轨迹输出给控制模块。经典搜索算法主要活跃在全局路由层和局部规划层。理解这个分层你就知道为什么搜索算法不需要输出“方向盘转角”——那不是它的活。6.2 搜索算法选型对照表算法最优性保证速度适用场景自动驾驶中的典型用途BFS边权相等时最优慢无权图、连通性检查状态空间探索、拓扑检查DFS无快但不稳定迷宫、连通性判断停车场出口搜索、连通性验证Dijkstra有较慢单源多目标、代价复杂全局路由、多目标评估A*启发函数可采纳时最优快单目标、有方向先验全局路径规划、局部路径搜索加权A*无次优很快实时性优先紧急局部搜索、快速重规划这张表不是让你死记而是帮你建立“什么场景掏什么工具”的条件反射。我见过有人拿Dijkstra去做实时局部避障结果算力吃满还来不及这就是选型错了。6.3 几个让我印象深刻的调参教训第一个教训关于启发函数的选择。有一次在园区场景里我用欧几里得距离做启发函数搜出来的路径总是偏向走直线穿过草坪——因为草坪在栅格图里没有障碍物标记代价低。后来把“不可行驶区域”的代价设成无穷大问题才解决。启发函数没错错的是代价函数没把语义信息编码进去。第二个教训关于搜索粒度。在高速场景做局部搜索时我一开始用了0.1米的栅格结果单次搜索要几百毫秒根本来不及。后来改成0.5米配合后续的轨迹优化整体效果反而更好。搜索是粗活优化是细活别让搜索干优化的活。第三个教训关于动态障碍物。A*搜出来的路径是静态的如果前方突然出现障碍物必须重新搜索。但频繁重搜索会导致路径抖动车会来回摆。解决办法是加一个“路径稳定性惩罚”让新路径和旧路径的差异不要太大同时设置重搜索的触发阈值不是一有变化就重搜。6.4 给想动手复现的人几条实在建议如果你想自己写一套经典搜索算法来跑自动驾驶路径规划我的建议是先用Python把BFS、DFS、Dijkstra、A*都实现一遍在同一个栅格地图上对比效果。不要一上来就追求性能先理解行为差异。可视化是你的好朋友。把搜索过程中扩展的节点用不同颜色画出来你能直观看到BFS的均匀扩散、Dijkstra的代价波前、A*的方向性。这比看十篇论文都管用。代价函数从简单开始先只用距离跑通了再加转弯惩罚、障碍物惩罚。每加一项观察路径怎么变。测试用例要覆盖极端情况起点终点被障碍物隔开、起点就在障碍物里、终点不可达。这些情况在实际系统里都会遇到算法必须能优雅处理而不是崩溃。别忽视数据结构。Dijkstra和A*的待处理集合用优先队列堆实现性能差距巨大。用普通列表每次线性找最小节点一多就卡死。7. 写在最后经典算法没过时只是需要放对位置这些年深度学习在规划领域很火端到端规划、模仿学习、强化学习层出不穷。但我始终觉得经典搜索算法是绕不过去的基本功。它们逻辑清晰、可解释、可调试在全局路由这种“确定性高、实时性要求相对宽松”的场景里依然是主力。而且很多学习类方法在底层依然依赖搜索来做后处理或安全兜底。我自己的体会是不要指望一个算法包打天下。全局用A*或Dijkstra局部用状态格搜索或采样动态避障用优化或学习各司其职系统才稳。把经典搜索算法吃透你才有能力判断在哪个环节该用什么工具而不是盲目追新。最后分享一个我常用的调试技巧在A*的搜索过程中把每个节点的g值和h值都打印出来随机抽查几个节点手动算一下f值对不对。这个笨办法帮我抓到过好几次启发函数写错、代价更新遗漏的bug。算法不难难的是细节不出错。