路径规划基础：构型空间概念，基于图搜索的路径规划（Dijkstra、A*），路径与轨迹的区别

各位同学，今天我们来聊聊路径规划里最基础、也最绕不开的几个概念。说实话，我刚入行那会儿，觉得路径规划不就是找条路嘛，有啥难的？直到我在一个AGV项目里，机器人死活绕不过一根柱子，我才意识到——嗯，构型空间这玩意儿，不搞明白真不行。

一、构型空间（C-Space）——换个角度看世界

什么叫构型空间？说白了，就是把机器人的每一个可能姿态，都映射成一个点。你想想看，一个在平面上移动的机器人，它的位置可以用(x, y, θ)来描述。这三个参数撑起来的空间，就是它的构型空间。

我在做六轴机械臂项目时，最头疼的就是避障。在笛卡尔空间里，障碍物是一堆乱七八糟的几何体。但一旦映射到C-Space里，机器人本体就缩成了一个点，障碍物则膨胀成了“禁止区域”。

举个例子：一个两关节的机械臂，每个关节角度范围是0°到360°。那么它的C-Space就是一个二维的方形区域。如果某个角度组合会导致机械臂撞到桌子，那这个点就是障碍物点。剩下的区域，就是自由空间。

二、基于图搜索的路径规划——Dijkstra与A*

有了C-Space，接下来就是怎么在里面找一条路。最经典的方法，就是把C-Space离散化成图，然后用图搜索算法。

2.1 Dijkstra算法——稳扎稳打的“老实人”

Dijkstra算法的思路很简单：从起点出发，一层层往外扩张，每次选当前代价最小的节点。它保证能找到最短路径，但代价是——它像个无头苍蝇一样往四面八方都搜一遍。

我记得有一次做仓储机器人调度，地图有上千个节点。Dijkstra跑一次要好几秒，对于实时控制来说太慢了。我当时就在想，有没有办法让它“聪明”一点？

// Dijkstra伪代码
function Dijkstra(Graph, start):
    dist[start] = 0
    for each vertex v in Graph:
        if v != start:
            dist[v] = INFINITY
        add v to unvisited_set
    
    while unvisited_set is not empty:
        u = vertex with smallest dist in unvisited_set
        remove u from unvisited_set
        
        for each neighbor v of u:
            alt = dist[u] + weight(u, v)
            if alt < dist[v]:
                dist[v] = alt
                prev[v] = u
    return dist, prev

2.2 A*算法——带“指南针”的搜索

A*算法在Dijkstra的基础上加了一个启发式函数。这个函数估计当前节点到终点的距离。有了这个“指南针”，A*会优先朝着终点方向搜索，效率高得多。

启发式函数怎么选？我个人的习惯是：

• 在网格地图上，用曼哈顿距离（只能上下左右移动时）
• 在连续空间里，用欧几里得距离
• 如果允许对角线移动，用切比雪夫距离

你想想看，如果启发式函数是0，A*就退化成Dijkstra。如果启发式函数恰好等于真实距离，A*会沿着最优路径直达终点，一步不差。但现实中，我们往往只能估计一个下界。

// A*伪代码
function AStar(Graph, start, goal):
    openSet = {start}
    gScore[start] = 0
    fScore[start] = heuristic(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 each 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] + heuristic(neighbor, goal)
                if neighbor not in openSet:
                    openSet.add(neighbor)
    return failure

三、路径与轨迹的区别——别搞混了

这个问题，我面试过不少工程师，十有八九会搞混。其实很简单：

维度	路径（Path）	轨迹（Trajectory）
定义	空间中的一条几何曲线	路径 + 时间信息
包含	位置序列 (x, y, z)	位置 + 速度 + 加速度 (x, y, z, v, a)
关注点	是否避障、是否最短	是否平滑、是否满足动力学约束
例子	A*找到的一系列网格点	用S形速度曲线插值后的运动

说白了，路径只告诉你“走哪条路”，轨迹才告诉你“怎么走”。

我在做数控机床插补时，体会特别深。路径规划算出来一条折线，如果直接让电机跟着跑，每个拐角都会产生巨大的加速度冲击。必须用轨迹规划把折线平滑成连续的速度曲线。

四、知识体系总览

下面这张图，是我自己梳理的本章知识结构。你可以把它当作一个思维导图来看：

嗯，这张图把本章的核心逻辑串起来了。从构型空间开始，到图搜索算法，再到路径与轨迹的区别。每一步都是下一环的基础。

---

公众号:蓝海资料掘金营，微信deep3321