4. 路径规划基础：图搜索算法与采样方法

大家好，我是你们的老朋友。今天我们来聊聊路径规划里最基础、也最核心的一块——图搜索和采样方法。说实话，我刚入行那会儿，觉得路径规划不就是找条路嘛，有啥难的？直到我在一个仓储机器人项目里，看着机器人在货架间来回撞墙，我才意识到：选对算法，比写对代码更重要。

这一章，我会把Dijkstra、A*、RRT、RRT*和PRM这五个算法掰开揉碎讲清楚。嗯，咱们不搞虚的，直接上干货。

4.1 图搜索算法：Dijkstra与A*

先说说图搜索。你想想看，机器人要在地图里找一条从起点到终点的路，本质上就是在图里找最短路径。图是什么？说白了就是一堆节点和连接它们的边。

4.1.1 Dijkstra算法：稳扎稳打的“老黄牛”

Dijkstra算法，我习惯叫它“地毯式搜索”。它从起点开始，一层一层往外扩，直到找到终点。它的核心思想就一句话：每次选当前代价最小的节点扩展。

我在一个AGV调度项目里用过Dijkstra。当时地图不大，节点也就几百个，跑起来没问题。但后来地图扩大到几千个节点，那速度就有点捉急了。为什么？因为它太“老实”了，不管终点在哪个方向，它都一视同仁地往外扩。

// Dijkstra伪代码
function Dijkstra(graph, start, goal):
    openSet = {start}
    g[start] = 0
    
    while openSet is not empty:
        current = node in openSet with smallest g
        if current == goal:
            return reconstructPath()
        
        openSet.remove(current)
        for each neighbor of current:
            tentative_g = g[current] + cost(current, neighbor)
            if tentative_g < g[neighbor]:
                g[neighbor] = tentative_g
                parent[neighbor] = current
                if neighbor not in openSet:
                    openSet.add(neighbor)
    return failure

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

A*算法，说白了就是在Dijkstra的基础上加了个“指南针”。这个指南针就是启发式函数h(n)，它估算从节点n到终点的距离。

我个人习惯用欧几里得距离作为启发式函数。但要注意，h(n)必须满足可采纳性——它不能高估实际代价。否则A*就找不到最优解了。

我曾经在一个无人机路径规划项目里用A*。当时地图是三维的，节点数上万。用Dijkstra要算好几秒，换成A*后，0.5秒就出结果了。差距就是这么明显。

// A*伪代码
function AStar(graph, start, goal):
    openSet = {start}
    g[start] = 0
    f[start] = h(start, goal)
    
    while openSet is not empty:
        current = node in openSet with smallest f
        if current == goal:
            return reconstructPath()
        
        openSet.remove(current)
        for each neighbor of current:
            tentative_g = g[current] + cost(current, neighbor)
            if tentative_g < g[neighbor]:
                g[neighbor] = tentative_g
                f[neighbor] = g[neighbor] + h(neighbor, goal)
                parent[neighbor] = current
                if neighbor not in openSet:
                    openSet.add(neighbor)
    return failure

算法	优点	缺点	适用场景
Dijkstra	保证最优解，实现简单	搜索范围大，速度慢	小地图、需要最优解
A*	搜索效率高，有方向性	依赖启发式函数设计	中大型地图、实时性要求高

4.2 采样方法：RRT、RRT*与PRM

图搜索算法有个硬伤——它需要把地图离散化成网格或节点。如果地图很大，或者维度很高（比如机械臂的关节空间），那节点数会爆炸。这时候，采样方法就派上用场了。

4.2.1 RRT算法：快速探索随机树

RRT，全称Rapidly-exploring Random Tree。说白了就是在地图里随机撒点，然后从起点开始，像树一样往外长。每次随机采样一个点，然后从最近的已有节点向它延伸一步。

我记得第一次用RRT是在一个狭窄通道的路径规划里。Dijkstra和A*都因为网格分辨率的问题找不到路，但RRT轻松就搞定了。为什么？因为它不需要离散化地图，直接在连续空间里采样。

// RRT伪代码
function RRT(start, goal, maxIterations):
    tree = {start}
    for i = 1 to maxIterations:
        randomPoint = sampleRandom()
        nearestNode = findNearest(tree, randomPoint)
        newNode = extend(nearestNode, randomPoint, stepSize)
        if collisionFree(nearestNode, newNode):
            tree.add(newNode)
            if distance(newNode, goal) < threshold:
                return reconstructPath(newNode)
    return failure

4.2.2 RRT*算法：渐近最优的RRT

RRT有个问题——它找到的路径通常不是最优的。RRT*就是在RRT的基础上加了两个优化步骤：重选父节点和重布线。

说白了，RRT*在每次添加新节点后，会检查附近有没有更好的父节点。如果有，就重新连接。这样随着采样点增多，路径会逐渐逼近最优解。

我在一个自动驾驶项目的局部路径规划里用过RRT*。虽然它比RRT慢一些，但路径平滑多了，车辆开起来不会忽左忽右。

// RRT*伪代码（关键部分）
function RRTStar(start, goal, maxIterations):
    tree = {start}
    for i = 1 to maxIterations:
        randomPoint = sampleRandom()
        nearestNode = findNearest(tree, randomPoint)
        newNode = extend(nearestNode, randomPoint, stepSize)
        if collisionFree(nearestNode, newNode):
            // 重选父节点
            nearNodes = findNear(tree, newNode, radius)
            for each node in nearNodes:
                if collisionFree(node, newNode):
                    if g[node] + cost(node, newNode) < g[newNode]:
                        parent[newNode] = node
                        g[newNode] = g[node] + cost(node, newNode)
            
            tree.add(newNode)
            
            // 重布线
            for each node in nearNodes:
                if collisionFree(newNode, node):
                    if g[newNode] + cost(newNode, node) < g[node]:
                        parent[node] = newNode
                        g[node] = g[newNode] + cost(newNode, node)
    return failure

4.2.3 PRM算法：概率路线图

PRM和RRT的思路不太一样。PRM是先建图，再查路。它在地图里随机采样大量点，然后连接相邻的、无碰撞的点，形成一个路线图。之后要查路径时，就在这个图上用Dijkstra或A*找路。

PRM特别适合多次查询的场景。比如一个工厂里的多个机器人，共享同一个地图。你只需要建一次PRM图，所有机器人都可以用。

我曾经在一个仓库调度系统里用PRM。地图有5000多平方米，我们提前建好PRM图，然后每个AGV要导航时，直接在图里搜就行。响应时间从秒级降到了毫秒级。

// PRM伪代码
function PRM(numSamples, k):
    // 建图阶段
    graph = empty
    for i = 1 to numSamples:
        point = sampleFree()
        graph.addNode(point)
    
    for each node in graph:
        neighbors = findKNearest(node, k)
        for each neighbor in neighbors:
            if collisionFree(node, neighbor):
                graph.addEdge(node, neighbor)
    
    // 查询阶段
    function query(start, goal):
        connect start and goal to graph
        path = Dijkstra(graph, start, goal)
        return path

4.3 知识体系总览

说了这么多，我画了一张图来总结这五个算法的关系。你一看就明白了。

从这张图你可以看到，路径规划算法大致分两类：图搜索和采样方法。图搜索里，Dijkstra是基础，A*是它的升级版。采样方法里，RRT是基础，RRT*是它的优化版，而PRM走的是另一条路——先建图再查路。

好了，这一章的内容就到这里。这些算法都是路径规划的基石，我建议你动手实现一遍，光看是学不会的。嗯，我在做第一个RRT实现时，光调试碰撞检测就花了两天——但搞明白之后，后面的路就顺了。