2、路径规划核心算法：A*算法原理、Dijkstra算法、RRT算法、路径平滑与优化
好，咱们进入正题。路径规划的核心，说白了就是「怎么从A点走到B点，而且走得漂亮」。我这些年做动画路径，踩过最多的坑就是算法选型。选对了，事半功倍；选错了，调试到怀疑人生。
今天咱们聊四个核心算法：A*、Dijkstra、RRT，以及最后的路径平滑。每个我都会结合项目经验来讲，希望能帮你少走弯路。
2.1 Dijkstra算法：最朴素的「地毯式搜索」
先说说Dijkstra。这算法其实很直白——它就像个强迫症患者，一定要把所有可能的路都探一遍，然后告诉你哪条最短。
核心思想：从起点开始，一层层往外扩散，每次选当前已知最短的路径继续走。直到覆盖到终点。
我举个例子。假设你在一个网格地图上，每个格子移动代价是1。Dijkstra会像水波一样，一圈圈往外扩散。第一圈是起点周围8个格子，第二圈是16个，以此类推。
适用场景：
- 地图不大（几百个节点以内）
- 需要保证找到最短路径
- 所有边的权重都是正数
但说实话，我在实际项目中很少直接用Dijkstra。为什么？因为它太「笨」了。你想想看，如果终点就在起点旁边，它还是要先把整个地图扫一遍。我曾经在一个1000x1000的网格上跑Dijkstra，结果等了快10秒才出结果——这谁受得了？
避坑指南：我曾经在一个机器人导航项目里用了Dijkstra，结果每次路径规划都要卡顿2-3秒。后来换成A*，响应时间直接降到100毫秒以内。如果你的场景需要实时响应，Dijkstra基本可以pass了。
2.2 A*算法：带「方向感」的搜索
A*算法，说白了就是Dijkstra的「开挂版」。它多了一个东西——启发函数。这个函数就像个指南针，告诉算法「往这个方向走更有可能找到终点」。
核心公式：
F = G + H
G = 从起点到当前节点的实际代价
H = 从当前节点到终点的估计代价（启发函数）
F = 总估计代价算法每次从待选节点中，选F值最小的那个继续探索。这样就不会像Dijkstra那样盲目扩散了。
我个人习惯用曼哈顿距离作为启发函数，尤其是在网格地图上。为什么？因为它计算快，而且保证能找到最短路径（只要H值不超过实际距离）。
小技巧：如果你想让A*跑得更快，可以适当「放大」H值。比如把曼哈顿距离乘以1.1。这样算法会更「激进」地往终点方向冲，但代价是可能找不到真正的最短路径。我在做游戏AI时经常这么干——玩家根本看不出那5%的路径差异，但响应速度快了3倍。
代码示例（简化版）：
def a_star(start, goal, grid):
open_set = {start}
came_from = {}
g_score = {start: 0}
f_score = {start: heuristic(start, goal)}
while open_set:
current = min(open_set, key=lambda x: f_score[x])
if current == goal:
return reconstruct_path(came_from, current)
open_set.remove(current)
for neighbor in get_neighbors(current, grid):
tentative_g = g_score[current] + 1
if tentative_g < g_score.get(neighbor, float('inf')):
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
open_set.add(neighbor)
return None # 没找到路径嗯，这里要注意：上面的代码只是个骨架。实际项目中你还要考虑障碍物动态变化、多目标点、内存优化等问题。我建议你先把基础版本跑通，再逐步加功能。
2.3 RRT算法：随机撒点，碰运气？
RRT（快速随机扩展树）算法，名字听着高大上，其实思路很「野」——随机撒点，然后尝试连接。
为什么需要RRT？你想想看，如果地图特别大，或者障碍物形状很复杂（比如迷宫），A*和Dijkstra就有点力不从心了。它们需要遍历太多节点。这时候RRT就派上用场了。
核心步骤：
- 在空间中随机选一个点
- 找到树上离它最近的点
- 朝随机点方向延伸一小段距离
- 如果这段路径没碰到障碍物，就加入树中
- 重复，直到树碰到终点区域
我在做无人机路径规划时用过RRT。当时要在满是障碍物的仓库里找路径，A*跑了半天出不来，RRT几秒钟就搞定了。当然，它找到的路径通常不是最短的，甚至有点「丑」。
RRT的优缺点：
| 优点 | 缺点 |
|---|---|
| 适合高维空间 | 路径不是最优的 |
| 处理复杂障碍物能力强 | 结果不稳定（每次跑可能不一样） |
| 速度快，尤其在大地图上 | 需要后处理平滑路径 |
避坑指南：我曾经在一个项目里直接用RRT的原始路径去控制机器人，结果机器人走起来一抖一抖的，像喝醉了酒。后来才意识到，RRT生成的路径必须经过平滑处理。记住：RRT只是「找到路径」，不是「走好路径」。
2.4 路径平滑与优化：让路径「顺眼」起来
好，现在路径找到了。但你看RRT生成的路径——全是锯齿状的折线。A*在网格上也是走「直角弯」。这样的路径，动画角色走起来会非常生硬。
常用的平滑方法：
- 贝塞尔曲线插值：用控制点生成平滑曲线。我常用三次贝塞尔，效果不错。
- B样条曲线：比贝塞尔更灵活，可以局部调整而不影响整体。
- 路径剪枝：去掉冗余的中间点。比如三个点共线，中间那个就可以删掉。
- 梯度下降优化：把路径看作一条橡皮筋，通过「拉紧」来缩短长度。
我个人习惯的做法是：先用路径剪枝去掉明显多余的节点，再用贝塞尔曲线做平滑。这样计算量小，效果也够用。
小技巧：做路径平滑时，别忘了考虑「运动学约束」。比如你的角色不能瞬间转向180度，那平滑后的路径曲率就不能太大。我曾经吃过这个亏——平滑后的路径虽然好看，但角色根本走不出来。
代码示例（贝塞尔曲线平滑）：
def bezier_curve(points, t):
"""计算贝塞尔曲线上的点，points是控制点列表"""
n = len(points) - 1
x = 0
y = 0
for i, (px, py) in enumerate(points):
coeff = comb(n, i) * (t ** i) * ((1 - t) ** (n - i))
x += coeff * px
y += coeff * py
return (x, y)
# 使用示例：在路径点之间插值
smoothed_path = []
for i in range(len(raw_path) - 1):
for t in [i/10 for i in range(11)]: # 每段插10个点
pt = bezier_curve([raw_path[i], raw_path[i+1]], t)
smoothed_path.append(pt)好了，四种核心算法都聊完了。总结一下我的个人经验：
- 小地图、需要最优解 → 用A*
- 大地图、复杂障碍物 → 用RRT
- 路径出来后，一定要做平滑处理
- 别忘了考虑运动学约束
下一章咱们会聊「动态避障与多智能体协同」，到时候这些算法还会再派上用场。有什么问题，欢迎在评论区交流。