第三章：全局路径规划概述：图搜索算法与采样算法的原理与对比

各位同学，欢迎来到路径规划的核心章节。今天我们来聊聊全局路径规划里最经典的两大类方法：图搜索和采样算法。说实话，我刚入行那会儿，觉得路径规划不就是找条路嘛，能有多难？后来在AGV项目里被现实狠狠教育了一顿——嗯，这里面的门道可不少。

3.1 图搜索算法：Dijkstra与A*

图搜索算法的核心思想，就是把环境抽象成一张图。节点代表位置，边代表可通行的路径。然后在这张图上找最短路径。我个人习惯把这类方法叫做“地毯式搜索”，因为它们会系统地遍历整个图。

3.1.1 Dijkstra算法：稳扎稳打的“老实人”

Dijkstra算法，说白了就是一个贪心的广度优先搜索。它从起点开始，一层层往外扩张，每次选当前代价最小的节点。我当年在教科书上看到这个算法时，觉得它太笨了——明明终点就在右边，它非要往左边也搜一遍。

但后来我在一个仓库机器人项目里，发现Dijkstra有个巨大的优点：它一定能找到最短路径。只要图里存在路径，它就不会漏掉。代价就是慢，非常慢。尤其是在大场景下，它会把整个地图都搜一遍才罢休。

核心要点：Dijkstra使用优先队列维护待扩展节点，每次弹出代价最小的节点。它的代价函数只考虑从起点到当前节点的实际代价g(n)。

// 伪代码示例：Dijkstra核心逻辑
function Dijkstra(graph, start, goal):
    openSet = PriorityQueue()
    openSet.add(start, 0)
    cameFrom = empty map
    gScore = map with default value Infinity
    gScore[start] = 0
    
    while openSet is not empty:
        current = openSet.pop()
        if current == goal:
            return reconstructPath(cameFrom, current)
        
        for each neighbor of current:
            tentative_g = gScore[current] + dist(current, neighbor)
            if tentative_g < gScore[neighbor]:
                cameFrom[neighbor] = current
                gScore[neighbor] = tentative_g
                openSet.add(neighbor, tentative_g)
    
    return failure // 无路可达

避坑指南：我曾经在一个动态障碍物场景里直接用Dijkstra做重规划，结果每次地图变化都要重新搜全图，CPU直接拉满。后来我改用增量式Dijkstra（D* Lite）才解决问题。记住：Dijkstra不适合频繁变化的动态环境。

3.1.2 A*算法：带“指南针”的搜索

A*算法是Dijkstra的升级版。它加了一个启发式函数h(n)，用来估计当前节点到终点的距离。你想想看，有了这个“指南针”，搜索就有了方向，不会像Dijkstra那样无头苍蝇一样乱撞。

我建议初学者先理解这个公式：f(n) = g(n) + h(n)。其中g(n)是起点到当前节点的实际代价，h(n)是当前节点到终点的估计代价。A*的聪明之处就在于，它用f(n)来指导搜索方向。

个人经验：我在做自动驾驶路径规划时，常用欧几里得距离作为启发函数。但要注意，如果h(n)低估了实际代价（即h(n) ≤ 真实代价），A*保证能找到最优解。如果高估了，搜索会更快，但可能不是最优路径。这个权衡，你得根据项目需求来定。

// 伪代码示例：A*核心逻辑
function AStar(graph, start, goal):
    openSet = PriorityQueue()
    openSet.add(start, 0)
    cameFrom = empty map
    gScore = map with default value Infinity
    gScore[start] = 0
    fScore = map with default value Infinity
    fScore[start] = heuristic(start, goal)
    
    while openSet is not empty:
        current = openSet.pop() // 弹出f值最小的节点
        if current == goal:
            return reconstructPath(cameFrom, current)
        
        for each neighbor of current:
            tentative_g = gScore[current] + dist(current, neighbor)
            if tentative_g < gScore[neighbor]:
                cameFrom[neighbor] = current
                gScore[neighbor] = tentative_g
                fScore[neighbor] = gScore[neighbor] + heuristic(neighbor, goal)
                openSet.add(neighbor, fScore[neighbor])
    
    return failure
特性 Dijkstra A*
搜索方向 各向均匀扩展 朝向目标方向扩展
最优性 保证最优 h(n)可采纳时保证最优
搜索效率 低（全图搜索） 高（有导向搜索）
适用场景 小规模、静态环境 中大规模、静态环境

3.2 采样算法：RRT与RRT*

图搜索算法有个硬伤：它需要把环境离散化成网格或图。在高维空间（比如机械臂的关节空间），网格数量会爆炸。这时候，采样算法就派上用场了。

3.2.1 RRT：快速探索随机树

RRT（Rapidly-exploring Random Tree）的思路很直接：在空间里随机撒点，然后从已有的树结构向新点生长。说白了，就是“瞎猫碰死耗子”——但实践证明，这招在高维空间里特别管用。

我记得第一次在6轴机械臂上跑RRT，看着它像藤蔓一样在关节空间里疯长，那种感觉挺奇妙的。它不需要精确的环境模型，只要有个碰撞检测器就行。这也是为什么RRT在机器人领域这么流行。

// 伪代码示例：RRT核心逻辑
function RRT(start, goal, environment):
    tree = Tree(start)
    
    for i = 1 to maxIterations:
        randomPoint = sampleRandomPoint(environment)
        nearestNode = findNearest(tree, randomPoint)
        newNode = steer(nearestNode, randomPoint, stepSize)
        
        if not collision(nearestNode, newNode, environment):
            tree.addNode(newNode)
            tree.addEdge(nearestNode, newNode)
            
            if distance(newNode, goal) < threshold:
                return extractPath(tree, newNode)
    
    return failure

避坑指南：我曾经在一个狭窄通道场景里用RRT，结果算法死活找不到路径。为什么？因为随机采样在狭窄通道里的命中率太低了。后来我加了目标偏置采样（以一定概率直接采样目标点），才解决了这个问题。记住：纯随机采样在狭窄通道里效率极低。

3.2.2 RRT*：追求最优的进化版

RRT能找到路径，但不保证最优。RRT*在RRT的基础上加了两个关键操作：重选父节点和重布线。每次插入新节点后，它会检查附近有没有更好的父节点，以及能不能通过新节点优化已有路径。

你想想看，这就像你到了一个陌生城市，先随便找了家酒店住下。然后发现隔壁街有家更好的酒店，价格一样但离景点更近——于是你换了过去。RRT*干的就是这个事。

核心区别：RRT找到的是“一条路径”，RRT*找到的是“渐近最优路径”。随着采样点增多，RRT*的路径会逐渐收敛到最优解。代价是每插入一个节点，都要做局部搜索和重连，计算量更大。

// 伪代码示例：RRT*核心逻辑（重选父节点部分）
function RRTStar(start, goal, environment):
    tree = Tree(start)
    
    for i = 1 to maxIterations:
        randomPoint = sampleRandomPoint(environment)
        nearestNode = findNearest(tree, randomPoint)
        newNode = steer(nearestNode, randomPoint, stepSize)
        
        if not collision(nearestNode, newNode, environment):
            // 重选父节点：在半径r内找更好的父节点
            nearNodes = findNearNodes(tree, newNode, radius)
            bestParent = nearestNode
            minCost = cost(nearestNode) + dist(nearestNode, newNode)
            
            for each node in nearNodes:
                if not collision(node, newNode, environment):
                    newCost = cost(node) + dist(node, newNode)
                    if newCost < minCost:
                        bestParent = node
                        minCost = newCost
            
            tree.addNode(newNode)
            tree.addEdge(bestParent, newNode)
            
            // 重布线：尝试通过新节点优化附近节点的路径
            for each node in nearNodes:
                if not collision(newNode, node, environment):
                    newCost = cost(newNode) + dist(newNode, node)
                    if newCost < cost(node):
                        tree.rewire(node, newNode)
    
    return extractBestPath(tree, goal)
特性 RRT RRT*
最优性 不保证 渐近最优
收敛速度 快（找到路径即可） 慢（需要大量采样）
路径质量 粗糙，常需后处理 平滑，接近最优
适用场景 高维、实时性要求高 高维、路径质量要求高

3.3 算法对比与选型建议

说了这么多，到底该用哪个？我个人习惯这样选：

  • 二维栅格地图，环境不大：用A*。又快又好，还能保证最优。我90%的AGV项目都用它。
  • 二维栅格地图，环境动态变化：用D* Lite或A*的增量变体。别傻乎乎地每次重搜全图。
  • 高维空间（机械臂、无人机）：用RRT或RRT*。图搜索在这里会维度爆炸。
  • 需要高质量路径，不着急：用RRT*。让它慢慢优化，你喝杯咖啡回来就有好结果。
  • 需要快速找到路径，质量无所谓：用RRT。先动起来再说，路径可以后续平滑。

我的建议：别死磕一种算法。我在实际项目中经常混合使用——先用RRT快速找到一条可行路径，再用A*在局部做优化。或者用A*生成粗路径，然后用RRT*在关键区域做精细调整。算法是工具，解决问题才是目的。

好了，这一章的内容就到这里。下一章我们会深入A*的工程实现细节，包括如何设计高效的启发函数、如何处理动态障碍物。到时候我会分享一些我在实际项目中踩过的坑，保证让你少走弯路。

← 上一章 📖 返回目录 下一章 →