第三章 路径规划基础：图搜索与采样方法

做机器人这么多年，我越来越觉得路径规划就像是在迷宫里找出口。你手里有地图，但怎么走最快、最安全，这就是算法要解决的问题。今天咱们聊聊几个最经典的路径规划算法——Dijkstra、A*、RRT和PRM。

说实话，我刚入行那会儿，总觉得这些算法是数学家的玩具。直到有一次做仓储机器人项目，机器人卡在货架中间转圈圈，我才意识到——嗯，路径规划不是花架子，是真能救命的。

3.1 图搜索基础：Dijkstra算法

Dijkstra算法，说白了就是「地毯式搜索」。它从起点开始，一层层往外扩，直到找到终点。

我习惯把它比作「水波纹扩散」——你往池塘里扔块石头，水波一圈圈往外推，最先碰到目标的那一圈，就是最短路径。

核心思想：维护一个距离表，记录从起点到每个节点的最短距离。每次选择距离最小的未访问节点，更新其邻居的距离。

伪代码长这样：

function Dijkstra(Graph, start, goal):
    dist[start] = 0
    prev = {}
    unvisited = all nodes in Graph
    
    while unvisited is not empty:
        current = node in unvisited with smallest dist
        if current == goal:
            return reconstruct_path(prev, goal)
        
        unvisited.remove(current)
        
        for each neighbor of current:
            alt = dist[current] + weight(current, neighbor)
            if alt < dist[neighbor]:
                dist[neighbor] = alt
                prev[neighbor] = current
    
    return failure

优点：一定能找到最短路径，前提是边权非负。

缺点：慢。地图越大，搜索范围越广，效率直线下降。

我的经验：Dijkstra适合静态环境、小规模地图。我在做室内导航时用过它，地图节点数在1000以内时表现还行。超过这个数，你就得考虑更高效的算法了。

3.2 启发式搜索：A*算法

A*算法是Dijkstra的升级版。它加了一个「方向感」——用一个启发函数估计当前节点到终点的距离，优先搜索「看起来更靠谱」的方向。

你想想看，Dijkstra像个没头苍蝇一样四面八方乱撞，A*则像个老司机，知道往哪个方向开更有可能到目的地。

A*的核心公式：

f(n) = g(n) + h(n)
  • g(n)：从起点到当前节点n的实际代价
  • h(n)：从节点n到终点的估计代价（启发函数）
  • f(n)：总估计代价

启发函数h(n)怎么选？我常用的有几种：

地图类型 推荐启发函数 说明
网格地图（四方向） 曼哈顿距离 |x1-x2| + |y1-y2|
网格地图（八方向） 切比雪夫距离 max(|x1-x2|, |y1-y2|)
任意方向移动 欧几里得距离 √((x1-x2)² + (y1-y2)²)

注意：启发函数h(n)必须满足「可采纳性」——不能高估实际距离。否则A*可能找不到最短路径。我曾经在这个坑里栽过跟头，用了一个高估的启发函数，结果路径绕了一大圈。

A*的代码实现：

function AStar(Graph, start, goal, h):
    openSet = {start}
    cameFrom = {}
    gScore = map with default value Infinity
    gScore[start] = 0
    fScore = map with default value Infinity
    fScore[start] = h(start, goal)
    
    while openSet is not empty:
        current = node in openSet with lowest fScore
        if current == goal:
            return reconstruct_path(cameFrom, current)
        
        openSet.remove(current)
        
        for neighbor of current:
            tentative_g = gScore[current] + weight(current, neighbor)
            if tentative_g < gScore[neighbor]:
                cameFrom[neighbor] = current
                gScore[neighbor] = tentative_g
                fScore[neighbor] = gScore[neighbor] + h(neighbor, goal)
                if neighbor not in openSet:
                    openSet.add(neighbor)
    
    return failure

我的建议：实际项目中，A*是图搜索的首选。它比Dijkstra快得多，而且只要启发函数选得好，结果几乎一样好。我在AGV调度系统里就用A*，几千个节点跑起来毫无压力。

3.3 随机采样方法：RRT算法

图搜索算法有个硬伤——它需要把环境离散成网格或节点。如果环境是连续的、高维的，那网格数量会爆炸。

RRT（Rapidly-exploring Random Tree）算法换了个思路：不建图，直接随机采样，从起点开始「长」出一棵树。

说白了，就是蒙着眼睛乱戳，戳到哪算哪，但每次戳都往没去过的地方伸一点。

RRT的基本步骤：

  1. 从起点开始，初始化一棵树
  2. 在环境中随机采样一个点
  3. 找到树上离采样点最近的节点
  4. 从该节点向采样点方向延伸一小步
  5. 如果延伸路径无障碍，把新节点加入树中
  6. 重复2-5步，直到树触及终点区域
function RRT(start, goal, environment, maxIterations):
    tree = {start}
    
    for i = 1 to maxIterations:
        sample = randomSample(environment)
        nearest = findNearest(tree, sample)
        new = steer(nearest, sample, stepSize)
        
        if not collision(nearest, new, environment):
            tree.add(new)
            new.parent = nearest
            
            if distance(new, goal) < goalThreshold:
                return reconstruct_path(new)
    
    return failure

RRT的优势：不需要显式构建环境地图，适合高维空间和复杂约束。我在机械臂避障规划中经常用它，7自由度的机械臂，用A*根本没法搞，RRT几分钟就能找到可行路径。

不过RRT有个毛病——路径通常很「丑」，弯弯绕绕的，不是最优。所以实际中我一般会加一个「路径平滑」的后处理步骤。

3.4 概率路图：PRM算法

PRM（Probabilistic Roadmap）和RRT有点像，都是随机采样。但PRM的思路是：先建一张「路图」，然后在路图上做查询。

我习惯把PRM分成两个阶段：

  • 学习阶段：随机采样一堆点，把能连起来的点连成一张图
  • 查询阶段：把起点和终点连到图上，然后用Dijkstra或A*找路径

PRM的伪代码：

// 学习阶段
function buildPRM(environment, numSamples, k):
    nodes = randomSample(environment, numSamples)
    edges = {}
    
    for each node in nodes:
        neighbors = findKNearest(node, nodes, k)
        for each neighbor in neighbors:
            if not collision(node, neighbor, environment):
                edges.add((node, neighbor))
    
    return Graph(nodes, edges)

// 查询阶段
function queryPRM(graph, start, goal):
    connect start and goal to graph
    return Dijkstra(graph, start, goal)

我的经验：PRM适合「多次查询」的场景。比如同一个工厂地图，今天要规划10条不同的路径，PRM只需要建一次图，后面每次查询都很快。RRT则是每次规划都要从头跑一遍。

PRM的缺点也很明显——如果环境是动态的（比如有移动障碍物），建好的路图可能失效，得重新建。

3.5 算法对比与选型建议

算法 适用场景 优点 缺点
Dijkstra 小规模静态地图 保证最优解 搜索范围大，效率低
A* 中等规模静态地图 效率高，保证最优解（启发函数合适时） 需要设计启发函数
RRT 高维空间、复杂约束 不需要显式地图，适合高维 路径非最优，不稳定
PRM 多次查询、静态环境 一次建图多次使用 不适合动态环境

避坑指南：我曾经在一个项目中用RRT做AGV的路径规划，结果发现AGV在仓库里走出来的路径歪歪扭扭，差点撞到货架。后来我加了一个路径平滑步骤，用B样条曲线把路径修整了一下，才解决问题。所以记住——RRT找到的路径，一定要做后处理！

3.6 本章知识体系

下面这张图帮你理清这四种算法的关系：

路径规划算法分类 路径规划 图搜索算法 采样方法 Dijkstra A* RRT PRM 选型建议 • 地图小、要最优解 → Dijkstra • 地图中等、要效率 → A* • 高维空间、复杂约束 → RRT • 多次查询、静态环境 → PRM

这张图把四种算法的关系理清楚了。图搜索和采样方法是两条不同的路，各有各的适用场景。我个人建议，初学者先从A*入手，它最实用，也最容易理解。等你把A*玩熟了，再去看RRT和PRM，会发现很多概念是相通的。

好了，路径规划的基础就聊到这儿。记住一点——没有万能的算法，只有合适的场景。选算法的时候，多想想你的环境是什么样的、对实时性要求高不高、要不要保证最优解。想清楚了，选起来就不难。

可可精品资料库

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