3、静态路径规划算法（A*）：A*算法的原理、启发式函数设计、Python实现与可视化
好，咱们进入正题。静态路径规划，说白了就是在已知地图上找一条最短路径。你想想看，机器人要在一个完全已知的环境里从A点走到B点，怎么走最省事？这就是A*算法要干的事。
我个人习惯把A*看作是Dijkstra的升级版。Dijkstra像个无头苍蝇一样四面八方乱撞，而A*则像个老司机，知道往哪个方向开更有可能到达目的地。这个「知道往哪个方向开」的能力，就是靠启发式函数实现的。
3.1 A*算法的核心原理
A*的核心公式其实特别简单：
f(n) = g(n) + h(n)我来解释一下这三个参数：
- g(n)：从起点到当前节点n的实际代价。说白了就是已经走了多远。
- h(n)：从当前节点n到终点的估计代价。这是启发式函数，是A*的灵魂。
- f(n)：总代价，也就是g(n)和h(n)的和。
算法每次从待选节点中挑f(n)最小的那个去扩展。嗯，这里要注意，如果h(n)始终为0，A*就退化成了Dijkstra。我在项目中遇到过有人把h(n)设得太大，结果算法跑得飞快但路径不是最优的——这就是所谓的「不可采纳」了。
关键点：A*算法保证找到最优路径的条件是：启发式函数h(n)必须是「可采纳的」，即h(n) ≤ 从n到终点的真实代价。说白了就是不能高估距离。
3.2 启发式函数设计
启发式函数怎么设计？这得看你的地图是什么类型的。我总结了几种常见情况：
| 地图类型 | 推荐启发式函数 | 说明 |
|---|---|---|
| 网格地图（四方向移动） | 曼哈顿距离 | |x1-x2| + |y1-y2| |
| 网格地图（八方向移动） | 切比雪夫距离 | max(|x1-x2|, |y1-y2|) |
| 任意方向移动 | 欧几里得距离 | √((x1-x2)² + (y1-y2)²) |
| 路网/图结构 | 直线距离 | 两点间直线距离 |
我曾经在一个仓储机器人项目里吃过亏。当时用的是曼哈顿距离，但机器人实际上可以斜着走。结果路径规划出来总是绕远路，后来改成切比雪夫距离，效果立竿见影。你想想看，启发式函数选错了，算法再牛也白搭。
小技巧：如果你不确定用哪种距离，可以试试「加权A*」。把h(n)乘以一个系数w（比如1.2），算法会更快但可能不是最优。这在实时性要求高的场景下很实用。
3.3 Python实现
好，咱们直接上代码。我习惯用面向对象的方式写，清晰好维护。
import heapq
import math
import matplotlib.pyplot as plt
import numpy as np
class AStar:
def __init__(self, grid, start, goal):
self.grid = grid # 0表示可通行，1表示障碍物
self.start = start
self.goal = goal
self.rows = len(grid)
self.cols = len(grid[0])
def heuristic(self, a, b):
"""使用欧几里得距离作为启发式函数"""
return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
def get_neighbors(self, node):
"""获取四方向邻居节点"""
neighbors = []
for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]:
nx, ny = node[0] + dx, node[1] + dy
if 0 <= nx < self.rows and 0 <= ny < self.cols:
if self.grid[nx][ny] == 0: # 可通行
neighbors.append((nx, ny))
return neighbors
def search(self):
"""A*主搜索逻辑"""
# 优先队列：存储 (f值, g值, 节点)
open_set = []
heapq.heappush(open_set, (0, 0, self.start))
# 记录从哪里来的
came_from = {}
# g值表
g_score = {self.start: 0}
# f值表
f_score = {self.start: self.heuristic(self.start, self.goal)}
while open_set:
_, current_g, current = heapq.heappop(open_set)
if current == self.goal:
# 找到路径，回溯
path = []
while current in came_from:
path.append(current)
current = came_from[current]
path.append(self.start)
path.reverse()
return path
for neighbor in self.get_neighbors(current):
tentative_g = current_g + 1 # 每步代价为1
if neighbor not in g_score or tentative_g < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f = tentative_g + self.heuristic(neighbor, self.goal)
f_score[neighbor] = f
heapq.heappush(open_set, (f, tentative_g, neighbor))
return None # 没找到路径这段代码我用了好几年，基本没大改过。你注意看第38行，我用了欧几里得距离。如果换成曼哈顿距离，把heuristic函数改一下就行。
注意：代码里我用了四方向移动。如果你需要八方向，get_neighbors方法要加上对角线方向，同时代价也要改成√2。我曾经有个学生没改代价，结果路径看起来歪歪扭扭的。
3.4 可视化展示
光有代码不够，咱们得看到效果。下面这个可视化函数，我建议你直接拿去用：
def visualize_path(grid, path, start, goal):
"""可视化路径规划结果"""
grid_vis = np.array(grid, dtype=float)
# 标记路径
if path:
for node in path:
grid_vis[node[0]][node[1]] = 0.5 # 灰色表示路径
# 标记起点和终点
grid_vis[start[0]][start[1]] = 0.8 # 浅色
grid_vis[goal[0]][goal[1]] = 0.9 # 更浅
plt.figure(figsize=(8, 8))
plt.imshow(grid_vis, cmap='viridis', interpolation='nearest')
plt.colorbar(label='可通行性')
plt.title('A*路径规划结果')
plt.grid(True, alpha=0.3)
# 标注起点和终点
plt.text(start[1], start[0], 'S', ha='center', va='center', fontsize=12, color='white')
plt.text(goal[1], goal[0], 'G', ha='center', va='center', fontsize=12, color='white')
plt.show()
# 使用示例
if __name__ == "__main__":
# 创建一个10x10的地图，中间有障碍物
grid = [[0]*10 for _ in range(10)]
for i in range(3, 7):
grid[i][5] = 1 # 竖着的障碍物
start = (0, 0)
goal = (9, 9)
astar = AStar(grid, start, goal)
path = astar.search()
if path:
print(f"找到路径，共{len(path)}步")
visualize_path(grid, path, start, goal)
else:
print("无法到达目标点")运行这段代码，你会看到一条从左上角到右下角的路径，绕过了中间的障碍物。嗯，这就是A*的威力。
3.5 避坑指南
最后，我分享几个实战中踩过的坑：
- 启发式函数不可采纳：我曾经把h(n)设成欧几里得距离的2倍，结果路径不是最优的。后来改成1倍就好了。
- 地图太大导致内存爆炸：1000x1000的地图，open_set里可能存几十万个节点。我建议用双向A*或者跳点搜索（JPS）来优化。
- 动态障碍物：A*是静态算法，如果障碍物会动，你得用D* Lite或者RRT*。这个咱们后面章节会讲。
我的建议：刚开始学A*，先用小地图（10x10）调试。等逻辑跑通了，再慢慢放大。别一上来就搞1000x1000，debug起来你会崩溃的。
好了，A*的核心内容就这些。说白了就是三个东西：f=g+h的公式、合适的启发式函数、以及优先队列的实现。你把这个吃透了，后面学D*、RRT*都会轻松很多。