EI-路径规划

Dijkstra，A-star，RRT

基本概念

想象你要在一个陌生的城市里找一家餐厅，你需要什么？地图、你的当前位置、餐厅的位置，以及避开修路或堵车的路线。
机器人的逻辑也是一样的，它需要构建一个配置空间（Configuration Space, C-Space）。

• 状态（State）： 机器人的当前位姿（位置和方向）。
• 起点（Start）与终点（Goal）： 任务的出发状态和目标状态。
• 障碍物（Obstacles）： 环境中不能通过的区域。为了安全，算法通常会将障碍物的边界按照机器人的体积“膨胀”一圈，这样机器人就可以被抽象成一个没有体积的“点”，大大简化了计算。
• 代价函数（Cost Function）： 评估一条路径好坏的标准。通常是路径的长度，但也可能包含时间、能量消耗或安全性（距离障碍物有多远）。

图搜索算法

Dijkstra

它看起来就像是在平静的湖面上，往“起点”扔了一块石头，荡起的水波纹一层一层均匀地向四周扩散，直到某一圈波纹触碰到了“终点”。

1. 贪心与公平

Dijkstra 算法的终极目标是：找到从起点到图中所有其他点的绝对最短路径。
为了做到这一点，它采用了一种极其稳妥但略显“笨拙”的策略：绝对公平地向四周探索，永远先走目前看来代价最小（距离起点最近）的一步。

它完全没有“方向感”（不知道终点在哪个方向），只知道自己现在走了多远。

2. 执行

我们可以把整个地图想象成一个棋盘，算法手里拿着一个小本本（我们称之为优先队列），上面记录着所有它“看得到但还没走过”的格子，以及这些格子距离起点的步数。

1. 初始状态： 算法站在起点。它在起点记下步数 0，然后环顾四周的邻居格子。
2. 记录邻居： 它把四周紧挨着的邻居格子都记在小本本上，并在这些格子上标明步数（比如上下左右走一步，步数都是 1）。
3. 挑选最便宜的（贪心）： 算法看了一眼小本本，找出目前步数最少的那个格子。由于第一圈的邻居步数都是 1，它就随便挑一个走过去。
4. 确认并扩散： 一旦走到这个新格子，算法就认为“我已经找到了从起点到这个格子的最短路径”，把它从本本上划掉。然后，它站在这个新格子上，继续环顾它周围的新邻居，算出走到这些新邻居的步数（前面的步数 + 1），并把新邻居记到本本上。
5. 发现捷径（更新）： 如果算法在看某个新邻居时，发现这个邻居已经在小本本上了，它会比较一下：“通过我现在站的地方走过去”和“小本本上原来记录的走法”，哪个步数更少？如果新的走法更近，它就把本本上的数字改小（这就是更新代价）。
6. 重复循环： 重复第 3 到 5 步，不断从小本本上挑最小的、走过去、看邻居、更新本本。直到算法走到了终点，或者小本本空了（说明四周全被死胡同堵死，找不到终点）。

3. 优缺点

• 优点（绝对可靠）： 只要起点和终点之间有路，它百分之百能找到，而且找到的绝对是最短路径。
• 缺点（盲目低效）： 它是“盲目”搜索的代表。假设起点在左边，终点在右边，它依然会老老实实地向左、向北、向南同时扩散。在开阔的场地上，它会把周围一大片区域全扫描一遍才能摸到终点，浪费了大量计算资源。

时间复杂度：$O((V + E) \log V)$，其中 $V$ 是节点数，$E$ 是边数。对于稀疏图（边数远小于节点数的平方），这个算法效率还算不错；但对于密集图，效率就会大幅下降。

4. 数学表达

它的核心灵魂可以用数学中的一个经典操作来概括——松弛操作 (Relaxation)。

• 图的定义： 设地图为一个有向或无向图 $G = (V, E)$。

  • $V$ (Vertices) 代表所有的网格点或节点。
  • $E$ (Edges) 代表节点之间的连线（路径）。

• 代价/权重： 图中每一条边 $(u, v)$ 都有一个具体的走法代价 $w(u, v)$（比如距离、时间、能量消耗）。

  • 绝对限制条件： $w(u, v) \ge 0$。Dijkstra 算法要求所有路径的代价必须是非负数（不能有“越走代价越小”的负权重路，否则算法会陷入无限循环死锁）。

• 距离记录： 设起点为 $s$。定义 $d(v)$ 为从起点 $s$ 到达图中任意节点 $v$ 的当前已知最短距离。

状态转移 (Relaxation)：

当它站在节点 $u$ 上，观察它周围的邻居节点 $v$ 时：

$$d(v) = \min(d(v), d(u) + w(u, v))$$

到达节点 $v$ 的最短记录，等于 ‘原来记录的到达 $v$ 的距离 $d(v)$’ 与 ‘先从起点走到当前的 $u$，然后再从 $u$ 迈一步走到 $v$ 的总距离 $(d(u) + w(u, v))$’ 这两者中的较小值。

通过不断对所有节点进行这种“松弛”，直到所有节点的最短距离不再发生变化，整个地图的最短路径就被彻底计算出来了。

5. 伪代码

// Graph: 地图网络
// source: 起点节点
// target: 终点节点 (可选，如果只需单点寻路)

function Dijkstra(Graph, source, target):

    // ---------------- 第 1 步：初始化 ----------------
    for each vertex v in Graph.Vertices:
        d[v] = ∞             // 将所有点到起点的距离初始设为无穷大
        prev[v] = NULL       // 用于记录最短路径是从哪个节点走过来的（方便最后画出路径）

    d[source] = 0            // 起点到自己的距离是 0

    // 创建一个优先队列 Q，存入所有节点。它会根据 d[v] 的值自动从小到大排好序
    Q = PriorityQueue(Graph.Vertices)


    // ---------------- 第 2 步：主循环（水波扩散） ----------------
    while Q is not empty:

        // 贪心策略：从队列中取出当前 d[u] 最小的节点 u（即水波扩散的最前沿）
        u = Q.extract_min()

        // (可选优化) 如果取出的点刚好是终点，说明到终点的绝对最短路已经找到，提前结束
        if u == target:
            break


        // ---------------- 第 3 步：扫描与松弛 ----------------
        // 遍历当前节点 u 的所有相邻邻居 v
        for each neighbor v of u:

            // 只有当 v 还在队列 Q 中（即还没被彻底确定最短距离）时才处理
            if v is in Q:
                // 计算：如果经过 u 走到 v，新的总代价是多少？
                alt = d[u] + w(u, v)

                // 核心数学操作：松弛 (Relaxation)
                if alt < d[v]:              // 如果发现了更短的捷径！
                    d[v] = alt              // 1. 更新本本上的最短距离记录
                    prev[v] = u             // 2. 记下这个捷径是从 u 拐过来的
                    Q.decrease_priority(v, alt) // 3. 告诉优先队列，v 的距离变小了，赶紧重新排序！

    // 算法结束，返回每个点的最短距离数组 d，以及用于回溯路径的 prev
    return d, prev

A-star

采样算法

RRT