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 米 | 200 | 4万 | 8万 | 16万 |
| 0.5 米 | 400 | 16万 | 32万 | 64万 |
| 0.2 米 | 1000 | 100万 | 200万 | 400万 |
| 0.1 米 | 2000 | 400万 | 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 启发函数的可采纳性与一致性：两个必须搞懂的概念
可采纳性（Admissibility）：h(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越大，搜索越偏向终点方向，扩展节点越少，速度越快，但路径可能不是最优的。w=1时就是标准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。算法不难，难的是细节不出错。
