30+路径规划算法终极图解：5分钟从搜索式到采样式快速上手
项目地址: https://gitcode.com/gh_mirrors/pa/PathPlanning 如果你家里有台扫地机器人，多半见过它"卡墙角"的名场面：反复试探、原地打转，最后靠乱撞脱身。真正决定它聪不聪明的，是藏在背后的路径规划算法。开源项目 PathPlanning 把这些算法一网打尽——收录 30 余种常用路径规划算法，覆盖搜索式与采样式两大流派，还为每个算法都配了动画演示，让"看不见的思考过程"变得一目了然。
读完这篇文章，你会弄清楚两件事：路径规划为什么会分化出两个"性格迥异"的流派，以及面对真实问题时该挑哪把钥匙。
一次"卡墙角"事故，暴露了路径规划的本质难题
先别急着看代码，想一个最简单的场景：机器人在房间里，要从 A 点走到 B 点，中间横着几面墙。数学上，这就是在"所有可能位置构成的空间"里，找一条从起点到终点、且不穿过障碍的连续路径。
听起来简单，难点却藏在细节里：
- 地图是"已知"还是"边走边发现"？
- 路径"能走通"就够，还是必须"最短最省"？
- 空间是 2D 平面，还是带朝向、带运动约束的高维空间？
不同的答案，催生了不同的算法家族。PathPlanning 项目正是按这个逻辑组织的：Search_based_Planning 与 Sampling_based_Planning 两大目录，分别对应两套截然不同的世界观。
搜索式规划：摊开地图，精打细算的"完美主义者"
把空间切成格子：搜索式规划的底层世界观
搜索式规划先把环境离散化成一张栅格地图，每个格子要么可通行、要么是障碍。规划问题随之变成图搜索问题：在一张巨大的网格图里找最短路径。项目中的 Search_based_Planning/Search_2D 目录就是这座"格子世界"的试验场，BFS、DFS、Dijkstra、A* 等经典算法一字排开。
A* 算法为什么快？关键在于一个"预感"
A* 是搜索式家族里最出名的成员。它和 Dijkstra 的唯一区别，是给每个节点算分时多了一个"启发项"，核心逻辑在 Search_based_Planning/Search_2D/Astar.py 里只有一行：
def f_value(self, s):
return self.g[s] + self.heuristic(s)
g 是从起点走到当前节点已经付出的代价，heuristic 则是"预估"到终点还要走多远（项目支持曼哈顿距离与欧氏距离两种）。你可以把 A* 理解成一个既有账本、又有直觉的向导——账本保证不偏航，直觉让它直奔目标。运行 Astar.py 后会自动生成动画：
动画里蓝色点为起点、绿色点为终点、灰色为障碍物，你能清楚看到搜索区域如何"有方向地"扩张，最后收敛出一条最短路径。
当环境会"变脸"：D* Lite 与增量重规划
搜索式的短板也很明显：一旦障碍物变化（比如有人挪了把椅子），整张图可能要重新搜一遍。于是诞生了 D*、D* Lite 这类"增量"算法——它们只修补受影响的局部，而不是推倒重来，非常适合机器人在部分未知环境中边走边修正：
采样式规划：不建全图，摸黑探路的"冒险家"
RRT 的三板斧：随机点、最近邻、迈一步
搜索式在高维空间里会遭遇"维度灾难"——格子爆炸，根本搜不完。采样式规划换了个思路：不枚举所有格子，而是随机撒点、边探边走。RRT（快速探索随机树）是这一派的代表作，它的核心循环在 Sampling_based_Planning/rrt_2D/rrt.py 里只有三步：
- 随机采样：以一定概率直接采目标点（让树"有奔头"），其余时候在地图里随机取点；
- 最近邻：从已长成的树里找到离采样点最近的节点；
- 迈一步：朝随机点方向走固定步长，无碰撞就长出新节点。
动画里那棵疯狂生长的树，就是机器人"盲人摸象"般探索环境的过程。你也一眼能看出它的缺点：路径歪歪扭扭，既不光滑也谈不上最优。RRT-Connect 让两棵树从起点和终点同时生长、双向奔赴，收敛速度立刻翻倍。
从"找得到路"到"找得到好路"：最优化的军备竞赛
如果说第一代采样算法解决的是"能不能到"，接下来的演进全在回答"能不能到得漂亮"：
- RRT*：长出新节点后多一步"重连"（rewire），不断用更短的路径替换旧连接，最终逼近最优解；
- Informed RRT*：找到一条可行路径后，把采样范围收窄到以起终点为焦点的椭圆内，集中火力优化；
- FMT* 与 BIT*：引入启发式与批量搜索思想，让随机探索变得"有计划"。
对比 RRT 与 RRT* 的动画，你会直观体会到：前者是碰运气，后者是在"优化—再优化"的循环里稳步收敛。这正是现代自动驾驶、无人机航迹规划普遍选用"星字辈"算法的原因。
从 2D 到 3D，从折线到曲线：真实机器人的最后一公里
平面上的折线路径对轮式机器人毫无意义——它有朝向，还有最小转弯半径。PathPlanning 的第三块拼图在这里补齐：
- rrt_3D 与 Search_3D 目录：把二维算法扩展到三维空间，如 rrt_star3D.py、informed_rrt_star3D.py、Astar3D.py，适用于无人机与机械臂场景；
- CurvesGenerator 目录：专门解决"折线没法直接执行"的问题。Dubins 曲线与 Reeds-Shepp 曲线考虑最小转弯半径和前进/后退，贝塞尔曲线、B 样条、五次多项式则负责把折线磨平成可跟踪的光滑轨迹。
所以一套完整的机器人导航方案通常是"算法找路 + 曲线平滑"的组合拳：先用规划算法算出大方向，再用曲线生成器把路径变成机器人真正开得动、转得了的轨迹。
路径规划算法快速上手：五个步骤跑通第一个 Demo
纸上得来终觉浅，动手最快，全程大约五分钟：
- 克隆仓库：
git clone https://gitcode.com/gh_mirrors/pa/PathPlanning - 先跑最经典的 A*：
python Search_based_Planning/Search_2D/Astar.py，弹出动画窗口； - 再跑 RRT：
python Sampling_based_Planning/rrt_2D/rrt.py，感受两种流派的视觉差异； - 打开 Search_based_Planning/Search_2D/env.py 和 Sampling_based_Planning/rrt_2D/env.py，改几个障碍物坐标，观察算法如何适应；
- 对照各 gif 目录里的成品动画，验证你的理解。
这里藏着一个不错的实验：把 Astar.py 里的 heuristic_type 在 "manhattan" 与 "euclidean" 之间来回切换，观察搜索效率的变化——这是理解 A* 精髓成本最低的实践。
场景选型速查表：面对实际问题，该拿哪把钥匙
| 你的场景 | 推荐算法 | 代码位置 |
|---|---|---|
| 静态地图、追求最短路径 | Dijkstra / A* | Search_based_Planning/Search_2D/ |
| 环境动态变化、需要边走边重规划 | D* Lite / Anytime D* | Search_based_Planning/Search_2D/ |
| 高维空间、约束复杂 | RRT* / Informed RRT* / BIT* | Sampling_based_Planning/rrt_2D/ |
| 车辆或无人机需要运动学约束 | Dubins / Reeds-Shepp + 样条平滑 | CurvesGenerator/ |
| 三维空间任务 | Astar3D / rrt_star3D 系列 | Search_3D 与 rrt_3D 目录 |
写在最后：把动画当"显微镜"，从模仿走向理解
路径规划算法常被论文里的数学公式劝退，但它的核心思想其实很朴素：搜索式靠"全盘计算"求最优，采样式靠"随机试探"破维度，而所有后续改进，都在用启发式、增量、批量等技巧让这两个极端互相靠拢。PathPlanning 的最大价值，是给每个抽象算法配了一台"显微镜"——动画把搜索顺序、树的生长、路径的收敛过程全部可视化。建议你每学一个算法，先看动画、再读代码、最后回到论文，沿着这条由浅入深的路线，很快就能建立起对路径规划算法的完整直觉。
创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
