3、全局路径规划：A*算法原理与实现
从启发式函数设计到路径平滑，手把手带你写一个A*。
大家好，我是老张。今天咱们聊聊A*算法。说实话，这玩意儿在机器人领域里，就像炒菜用的盐——看着普通，但少了它，很多菜就做不成了。
我记得刚入行那会儿，带我的师傅扔给我一句话：“小张，把A*搞明白，你就算入门了。”我当时还不服气，心想不就是个寻路算法嘛。结果真到项目里，才发现坑多着呢。
3.1 为什么是A*？
先说说背景。全局路径规划，说白了就是给机器人找一条从起点到终点的路。但问题来了——路不是随便找的。你得考虑障碍物、考虑路径长度、考虑计算时间。
你可能会问：“Dijkstra不行吗？”行，但太慢了。它像个无头苍蝇，四面八方都搜一遍。“贪心算法呢？”快是快，但容易撞墙。它只顾眼前利益，经常钻进死胡同。
A*聪明在哪？它把两种思路结合了。既看已经走了多远，又估算到终点还有多远。嗯，这就是它的核心——启发式搜索。
核心公式： f(n) = g(n) + h(n)
- g(n)：从起点到当前节点n的实际代价
- h(n)：从节点n到终点的估计代价（启发式函数）
- f(n)：综合代价，算法根据这个值决定先探索哪个节点
我在项目中遇到过一个问题：如果h(n)设计得不好，A*就会退化。h(n)太小，它变成Dijkstra，慢得要命；h(n)太大，它变成贪心算法，路径可能不是最优的。所以，启发式函数的设计，是A*的灵魂。
3.2 启发式函数怎么选？
启发式函数，说白了就是“猜”一下到终点还有多远。怎么猜？得看你的地图类型。
| 地图类型 | 推荐启发式函数 | 说明 |
|---|---|---|
| 网格地图（四方向移动） | 曼哈顿距离 | |x1-x2| + |y1-y2| |
| 网格地图（八方向移动） | 切比雪夫距离 | max(|x1-x2|, |y1-y2|) |
| 任意方向移动 | 欧几里得距离 | sqrt((x1-x2)² + (y1-y2)²) |
我个人习惯用曼哈顿距离。为什么？因为大多数机器人是在网格地图上移动的，比如扫地机器人、仓储AGV。你想想看，一个扫地机器人能斜着走吗？一般不能。所以曼哈顿距离最合适。
小技巧： 如果你不确定用哪个，先试试曼哈顿距离。它计算简单，而且能保证找到最短路径。我在一个仓储机器人项目里就是这么干的，效果不错。
3.3 手把手写一个A*
光说不练假把式。咱们直接上代码。我会用Python写一个最简版本的A*，让你看清它的骨架。
import heapq
def a_star(start, goal, grid):
# 定义方向：上下左右
directions = [(0, 1), (0, -1), (1, 0), (-1, 0)]
# 开放列表：用优先队列存储 (f值, 节点)
open_list = []
heapq.heappush(open_list, (0, start))
# 记录从哪里来的，用于回溯路径
came_from = {}
# g值：从起点到当前节点的实际代价
g_score = {start: 0}
# f值：g + h
f_score = {start: heuristic(start, goal)}
while open_list:
# 取出f值最小的节点
current = heapq.heappop(open_list)[1]
# 到达终点？
if current == goal:
return reconstruct_path(came_from, current)
# 探索邻居
for dx, dy in directions:
neighbor = (current[0] + dx, current[1] + dy)
# 检查是否在地图内且不是障碍物
if not is_valid(neighbor, grid):
continue
# 计算新的g值
tentative_g = g_score[current] + 1
# 如果这个邻居还没访问过，或者找到了更短的路径
if neighbor not in g_score or tentative_g < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
heapq.heappush(open_list, (f_score[neighbor], neighbor))
# 没找到路径
return None
def heuristic(a, b):
# 曼哈顿距离
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def is_valid(node, grid):
x, y = node
return 0 <= x < len(grid) and 0 <= y < len(grid[0]) and grid[x][y] != 1
def reconstruct_path(came_from, current):
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
path.reverse()
return path代码不长，但核心逻辑都在了。我解释一下关键点：
- 优先队列：用heapq实现，每次取出f值最小的节点。这是A*效率的关键。
- g_score和f_score：分开存储，方便更新。我见过有人只存f，结果路径回溯时出问题。
- 启发式函数：这里用了曼哈顿距离。你可以换成别的，但记得要满足“可采纳性”——即h(n)不能超过实际代价。
注意： 这段代码假设网格中0表示可通行，1表示障碍物。实际项目中，你可能需要处理动态障碍物、不同地形代价等。但核心骨架不变。
3.4 路径平滑：让机器人走得更顺
A*找到的路径，往往是锯齿状的。为什么？因为它在网格里走，只能上下左右。你想想看，一个机器人走这种路，会怎么样？
它会不停地急转弯。轻则磨损轮子，重则翻车。我在一个AGV项目里就吃过这个亏——路径规划好了，但机器人跑起来一卡一卡的，像喝醉了酒。
所以，我们需要路径平滑。常用的方法有两种：
- 线性插值：在路径点之间插入中间点，让转弯更平滑。简单，但效果一般。
- 贝塞尔曲线：用控制点生成平滑曲线。效果好，但计算稍复杂。
我个人推荐用贝塞尔曲线。虽然多写几行代码，但效果立竿见影。下面是一个简单的实现：
def bezier_smooth(path, num_points=10):
# 用贝塞尔曲线平滑路径
# path: 路径点列表
# num_points: 每个线段之间插入的点数
smooth_path = []
for i in range(len(path) - 1):
p0 = path[i]
p1 = path[i + 1]
for t in range(num_points):
t_normalized = t / num_points
# 线性贝塞尔：其实就是插值
x = p0[0] * (1 - t_normalized) + p1[0] * t_normalized
y = p0[1] * (1 - t_normalized) + p1[1] * t_normalized
smooth_path.append((x, y))
return smooth_path这段代码很简单，但够用。如果你想更平滑，可以用三次贝塞尔曲线，需要两个控制点。嗯，这个咱们后面再细聊。
3.5 避坑指南
最后，分享几个我踩过的坑：
- 启发式函数不可采纳：我曾经用欧几里得距离做网格地图的启发式，结果路径不是最优的。因为网格里只能走直线，欧几里得距离低估了实际代价。
- 开放列表太大：如果地图很大，开放列表会膨胀。我建议用双向A*，从起点和终点同时搜索，能快不少。
- 路径平滑过度：平滑后的路径可能穿过障碍物。记得做碰撞检测，别让机器人撞墙。
总结一下： A*的核心是f = g + h。h的设计决定了算法的效率和最优性。路径平滑是锦上添花，但别忽视它。记住，一个能跑的算法和一个好用的算法，差距往往就在这些细节里。
好了，这一章就到这里。A*的原理和实现，说白了就是这些。但别小看它，很多高级算法，比如D*、RRT*，都是在A*的基础上改进的。把A*吃透，后面的路就好走了。