上一篇概述了运动规划的三大分类。今天开始深入图搜索——这是最经典的全局规划方法，也是A*、Dijkstra等算法的基础。

图搜索的核心思想就一句话：把连续的空间切成格子，把规划问题变成"在格子上找路"的问题。就像你手机上的导航地图——地球表面是连续的，但导航系统把它离散化成路网，然后在这个图上搜索最短路径。机器人规划也是同样的道理。

一、为什么要离散化？

机器人的运动空间是连续的——关节角度可以取任意实数值，末端位置也可以在三维空间里任意移动。连续空间理论上包含无穷多个点，计算机没法处理无穷多个元素。

离散化就是把连续空间切成有限个"格子"（或者叫"节点"）。每个格子代表一个离散状态，格子之间的连接代表状态转移。

# 2D网格离散化示例
# 空间范围 [0, 10] x [0, 10]
# 分辨率 1.0 → 10x10 = 100个格子

resolution = 1.0
grid_size = (10, 10)
# 每个格子用 (i, j) 索引
# 格子 (3, 5) 对应物理坐标 (3.5, 5.5)

离散化之后，原本"在连续空间找一条无碰撞曲线"的问题，变成了"在图上找一条从起点节点到终点节点的路径"。后者是经典的图论问题，有成熟的算法可以解决。

二、图的构建方式

离散化有几种常见方式：

1. 规则网格（Grid）

最直观的方式。把空间切成均匀的正方形（2D）或立方体（3D）格子。

# 2D网格的邻居连接
# 4连接：上下左右
neighbors_4 = [(-1,0), (1,0), (0,-1), (0,1)]
# 8连接：加上对角线
neighbors_8 = [(-1,-1), (-1,0), (-1,1),
               (0,-1),           (0,1),
               (1,-1),  (1,0),  (1,1)]

优点：简单、规则、容易实现。 缺点：维度灾难。3D空间分辨率0.1m，10m×10m×3m的房间就有300万个格子。6D构型空间？想都别想。

2. 可见图（Visibility Graph）

只在障碍物的顶点之间连线。只有两个顶点之间"看得见"（连线不穿过障碍物）时，才建立连接。

优点：节点数少，路径贴着障碍物走（通常是最短路径）。 缺点：只适合2D多边形障碍物，3D场景很难用。路径贴障碍物太近，实际执行不安全。

3. Voronoi图

把空间按"离障碍物最远"的原则划分。Voronoi图的边是离最近障碍物等距的点的集合。

优点：路径离障碍物最远，安全裕度最大。在移动机器人导航中特别实用。 缺点：路径通常不是最短的，绕来绕去。计算Voronoi图在3D场景下比较慢，高维空间几乎不可行。

4. 路标图（Waypoint Graph）

人工或者自动在空间中放置一些"路标点"，路标之间建立连接。

优点：灵活，可以针对具体场景优化。工厂里的固定路线导航常用这种方式。 缺点：需要人工设计，覆盖率不保证。环境变化后需要重新设计路标。

三、图搜索的基本框架

不管用哪种离散化方式，图搜索的基本框架是一样的：

def graph_search(graph, start, goal):
    open_set = {start}      # 待探索的节点
    closed_set = set()       # 已探索的节点
    came_from = {}           # 路径回溯

    while open_set:
        # 从open_set中选一个"最好"的节点
        current = select_best(open_set)
        
        if current == goal:
            return reconstruct_path(came_from, current)
        
        open_set.remove(current)
        closed_set.add(current)
        
        for neighbor in graph.neighbors(current):
            if neighbor in closed_set:
                continue
            if neighbor in open_set:
                # 检查是否找到了更好的路径
                update_if_better(came_from, neighbor, current)
            else:
                open_set.add(neighbor)
                came_from[neighbor] = current
    
    return None  # 无解

这个框架的关键在于select_best——从open_set里选哪个节点来扩展。不同的选择策略就是不同的算法：

• 选g(n)最小的 → Dijkstra算法
• 选f(n) = g(n) + h(n)最小的 → A*算法
• 选h(n)最小的 → 贪心最佳优先搜索

四、代价函数和启发式

图搜索有两个核心概念：

g(n)：从起点到节点n的实际代价。Dijkstra算法只关心这个值——选g(n)最小的节点扩展，保证找到最短路径。

h(n)：从节点n到终点的估计代价（启发式函数）。A*算法用h(n)来"引导"搜索方向——优先探索"看起来离终点近"的节点。

# 启发式函数示例（2D网格）
def manhattan_distance(a, b):
    """曼哈顿距离：只能走上下左右"""
    return abs(a[0]-b[0]) + abs(a[1]-b[1])

def euclidean_distance(a, b):
    """欧几里得距离：可以走对角线"""
    return ((a[0]-b[0])**2 + (a[1]-b[1])**2) ** 0.5

启发式函数的选择直接影响算法行为。h(n)=0时，A*退化成Dijkstra。h(n)越接近真实代价，搜索越快。但h(n)不能超过真实代价（可采纳性），否则可能找不到最优解。

五、离散化的代价

离散化不是免费的，有几个坑需要注意：

分辨率 vs 计算量：分辨率越高，路径质量越好，但节点数指数增长。工程上需要在路径质量和计算时间之间权衡。

离散化误差：离散化后的路径只能沿网格方向走，会出现"锯齿形"路径。解决方法：后处理平滑，或者用更高阶的连接方式（比如8连接比4连接好）。

拓扑信息丢失：离散化可能丢失空间的拓扑结构。比如两个障碍物之间的狭窄通道，如果分辨率太粗，通道可能被"填满"，导致找不到本来存在的路径。这就是所谓的"狭窄通道问题"——采样规划也面临同样的挑战。

维度灾难：这是离散化最大的问题。构型空间每增加一维，节点数就乘以分辨率的倒数。6轴机械臂，每轴分辨率1度，就有360^6 ≈ 2万亿个节点。根本不可能用网格搜索。这也是为什么高维规划必须用采样方法——RRT、PRM这些算法不依赖网格，在高维空间依然可行。

六、面试实战

Q：图搜索算法的时间复杂度是多少？ A：最坏情况O(V+E)，V是节点数，E是边数。因为每个节点最多被访问一次，每条边最多被检查一次。A*在最坏情况下也是O(V+E)，但好的启发式函数可以大幅减少实际访问的节点数。

Q：4连接和8连接有什么区别？ A：4连接只能走上下左右，路径是"之字形"的，路径长度偏大。8连接多了对角线，路径更平滑更接近真实最短路径。但8连接的对角线代价是sqrt(2)而不是1，计算时注意不要搞错。工程上移动机器人常用8连接，机械臂关节空间规划有时用更多连接（比如16连接）。

Q：为什么机械臂规划很少用网格搜索？ A：因为维度太高。6轴机械臂的构型空间是6D的，用网格离散化节点数爆炸。所以机械臂通常用采样规划（RRT/PRM），不用图搜索。图搜索适合2D或3D的低维空间。

小结

图搜索的核心：把连续空间离散化成图，然后用图搜索算法找路径。离散化方式有网格、可见图、Voronoi图、路标图等。

不同的选择策略对应不同的算法——Dijkstra、A*、贪心搜索。启发式函数h(n)引导搜索方向，决定了算法的效率。

离散化最大的问题是维度灾难——高维空间用不了。这也是为什么高维规划要用采样方法（RRT、PRM等）。

下一篇讲Dijkstra算法——最短路径的经典解法。

---

如果这篇文章对你有帮助，欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。

「机器人软件开发面试·从入门到精通」连载系列 

上一篇：第206篇 运动规划概述——全局/局部/反应式规划的分类

下一篇预告：第208篇 Dijkstra算法——最短路径的经典解法

有任何问题欢迎评论区留言，我会尽量回复。