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

1. 启发式函数 (Heuristic)

A*算法的核心在于一个优雅的数学公式。在探索每一个新网格时，A* 都会计算一个综合得分 $f(n)$，并永远优先选择 $f(n)$ 最小（即总代价最低）的网格去走：

$$f(n) = g(n) + h(n)$$

• $g(n)$（过去的代价）： 从起点走到当前格子 $n$ 的实际已走步数。这和 Dijkstra 算法一模一样，代表了“踏实”。它确保算法不会为了抄近道而走入代价极其昂贵的死胡同
• $h(n)$（未来的预估，即“指南针”）： 从当前格子 $n$ 到终点的预估代价。这就是被称为启发式函数（Heuristic）的魔法部分，代表了“聪明”。它让算法具有了方向感，像有一股引力一样把搜索方向朝着终点拉扯

2. 未来的预估

机器人在走到一半时，实际上并不知道前面有没有墙壁挡着，那它怎么“预估”到终点的距离呢？通常有两种最基础的预估方法：

• 曼哈顿距离 (Manhattan Distance)： 假设机器人只能上下左右走。预估距离就是两点在横纵坐标上的绝对差值之和：$h = |x_{goal} - x_n| + |y_{goal} - y_n|$
• 欧几里得距离 (Euclidean Distance)： 就是两点之间的直线距离。使用勾股定理计算：$h = \sqrt{(x_{goal} - x_n)^2 + (y_{goal} - y_n)^2}$

3. 看上去是什么样的？

A* 算法在运行时，它的形状更像是一束“手电筒的光”或者一颗“拉伸的水滴”，笔直地指向终点方向。

1. 它站在起点，看向四周的邻居
2. 它在笔记上计算每个邻居的 $f(n)$。终点方向的邻居，因为 $h(n)$ 更小，所以总分 $f(n)$ 也更小
3. 它毫不犹豫地朝着终点方向迈出第一步
4. 遇到障碍物怎么办？ 假设它直直地撞向了一堵墙，它发现沿着墙走的 $g(n)$（已走步数）越来越大，导致总分 $f(n)$ 飙升。此时记录的、之前被忽略的那些稍微绕远但没有墙的格子的 $f(n)$ 反而变得更小了。于是，A* 会聪明地“回退”或绕开墙壁，继续向终点探索

4. 可采纳的

A* 算法有一个非常重要的前提：你的“指南针”可以不准，但绝对不能“夸大其词”。

在数学上，这被称为启发式函数必须是可采纳的（Admissible）。也就是说，你预估的距离 $h(n)$ 必须小于或等于实际真实的距离。

• 如果不小心高估了（比如预估距离是实际距离的2倍）： A*可能会因为觉得某条路未来太难走而过早放弃，从而错过真正的绝对最短路径。此时 A* 就退化成了单纯的贪心算法，虽然找路极快，但不保证路径最短
• 如果低估了（比如预估全都是0）： 那 $h(n)$ 就失去了意义，$f(n) = g(n)$，A* 就瞬间退化成了 Dijkstra 算法，慢吞吞地寻找绝对最优解

5. 伪代码

A* 的伪代码结构与 Dijkstra 非常相似，同样需要使用优先队列（笔记）来时刻挑出 $f(n)$ 最小的节点。

// Graph: 地图网络
// source: 起点
// target: 终点
// h(n): 启发函数，计算节点 n 到终点的预估距离
// d(u, v): 节点 u 到相邻节点 v 的单步实际代价

function A_Star(Graph, source, target):

    // ---------------- 第 1 步：初始化 ----------------
    // 创建一个优先队列 OpenSet，存放待探索的节点。按 f[n] 从小到大排序
    OpenSet = PriorityQueue()
    OpenSet.add(source)

    // 记录路径回溯
    came_from = {}

    // g_score: 记录从起点到每个节点的最短实际代价，初始全部为无穷大
    g_score = map with default value Infinity
    g_score[source] = 0

    // f_score: 记录每个节点的综合总代价 (g + h)，初始全部为无穷大
    f_score = map with default value Infinity
    f_score[source] = h(source) // 起点的 g 是 0，所以 f 就是 h

    // ---------------- 第 2 步：主循环 ----------------
    while OpenSet is not empty:

        // 贪心策略：取出当前 OpenSet 中 f_score 最小的节点
        current = OpenSet.extract_min()

        // 如果已经走到了终点，开始回溯路径并结束算法
        if current == target:
            return reconstruct_path(came_from, current)

        // ---------------- 第 3 步：扫描邻居与松弛 ----------------
        for each neighbor of current:

            // 计算：如果经过 current 走到 neighbor，新的 g 代价是多少
            tentative_g_score = g_score[current] + d(current, neighbor)

            // 如果发现了更短的实际路径（松弛操作）
            if tentative_g_score < g_score[neighbor]:

                // 1. 记录这步好棋是从哪走过来的
                came_from[neighbor] = current

                // 2. 更新该邻居的实际代价 g
                g_score[neighbor] = tentative_g_score

                // 3. 计算并更新该邻居的综合估价 f = g + h
                f_score[neighbor] = tentative_g_score + h(neighbor)

                // 4. 如果这个邻居还不在待探索队列里，把它加进去
                if neighbor not in OpenSet:
                    OpenSet.add(neighbor)
                else:
                    // 如果已经在队列里，更新它在队列里的排序优先级
                    OpenSet.update_priority(neighbor, f_score[neighbor])

    // 如果队列空了还没找到终点，说明没有路
    return failure

对比总结：
你会发现，如果在上面的伪代码中，强制让所有的预估函数 $h(n) = 0$，那么 $f(n)$ 就完全等于 $g(n)$。此时，A*的代码逻辑就和 Dijkstra 变得一模一样了。所以，Dijkstra 只是 A* 在一种极端情况（毫无方向感）下的特例。

采样算法

RRT

如果说 A* 和 Dijkstra 是在棋盘上一格一格严谨下棋的棋手，那么 RRT（Rapidly-exploring Random Tree，快速探索随机树） 就是一个在广阔空间里疯狂生长的爬山虎。
RRT 属于基于采样（Sampling-based）的算法。当空间特别大，或者机器人关节很多（比如一个 7 自由度的机械臂，它的空间是 7 维的），把空间划分成网格根本不现实。这时候，RRT 的“随机撒点”策略就派上大用场了。

1. 生长

你可以把它想象成在黑暗中摸索着长大的树根：

1. 扎根： 算法把“起点”作为树的根节点。
2. 随机撒点（掷飞镖）： 算法在整个地图的空白区域随机产生一个目标点（随机撒个点）。
3. 寻找最亲近的树枝： 算法在已经长出来的树结构中，找到距离这个随机点最近的那个节点。
4. 努力生长： 算法让这个最近的节点，朝着随机点的方向长出一小截“树枝”（步长是固定的）。
5. 安全检查： 如果这根新长出来的树枝没有戳到墙壁（障碍物），就把这根树枝保留下来，作为一个新节点加入树中；如果撞墙了，就直接砍掉，重新掷飞镖。
6. 开花结果： 不断重复撒点、寻找最近点、生长、检查的过程。直到某一次，新长出来的树枝刚好够到了“终点”的判定范围内，算法就宣告成功！顺着树枝一路往回找，就能得出一条从起点到终点的路径。

2. 优缺点

• 优点（极速且灵活）： 它不需要提前建构极其消耗内存的全局网格地图。在极其空旷或者维度极高的空间里，它的探索速度快得惊人，非常适合给机械臂做动作规划，或者给无人机做三维空间飞行规划。
• 缺点（像闪电一样曲折）： 因为它是靠“纯随机”长出来的，所以最终找到的路径往往像一道闪电或者歪歪扭扭的折线。它绝对不是最短路径，而且因为转折太多，机器人根本没法直接按照这条线开。在实际应用中，我们找到路径后，还要用额外的数学方法（比如 B 样条曲线）把这条路“拉直变平滑”。

3. 数学表达

在 RRT 中，机器人的整个世界被定义为配置空间（Configuration Space, 简写为 $\mathcal{C}$）。

• $\mathcal{C}_{obs}$：障碍物空间（撞墙的区域）
• $\mathcal{C}_{free}$：自由空间（安全区域），满足 $\mathcal{C}_{free} = \mathcal{C} \setminus \mathcal{C}_{obs}$

RRT 的目标是在 $\mathcal{C}_{free}$ 中建立一棵树 $\mathcal{T} = (V, E)$，其中 $V$ 是节点集合（存放坐标），$E$ 是边集合（存放节点间的连线）。

核心的数学步骤如下：

1. 随机采样 (Random Sampling)：
   在整个空间中按照某种概率分布（通常是均匀分布）产生一个随机状态点 $q_{rand}$

   $$q_{rand} \in \mathcal{C}$$

2. 寻找最近邻 (Nearest Neighbor)：
   在当前树 $\mathcal{T}$ 已经长出的所有节点集合 $V$ 中，找到一个距离 $q_{rand}$ 最近的节点 $q_{near}$。这里需要定义一个距离度量函数 $\rho$（通常使用欧几里得距离）

   $$q_{near} = \arg\min_{q \in V} \rho(q, q_{rand})$$

3. 步进生长 (Steering / Extend)：
   从 $q_{near}$ 出发，沿着指向 $q_{rand}$ 的方向，前进一个固定的步长 $\Delta q$，从而生成一个全新的节点 $q_{new}$
   在二维或三维欧式空间中，这个操作可以用向量数学表示为：

   $$q_{new} = q_{near} + \min(\Delta q, |q_{rand} - q_{near}|) \frac{q_{rand} - q_{near}}{|q_{rand} - q_{near}|}$$

   (注：如果 $q_{rand}$ 距离比步长 $\Delta q$ 还近，就直接走到 $q_{rand}$；否则就只走 $\Delta q$ 的距离)

4. 碰撞检测 (Collision Checking)：
   这是最关键的安全约束。必须确保从 $q_{near}$ 到 $q_{new}$ 的这根线段（或者说机器人的运动轨迹）完全处于自由空间中

   $$\text{LineSegment}(q_{near}, q_{new}) \subset \mathcal{C}_{free}$$

   如果满足上述数学条件，就把 $q_{new}$ 加入节点集合 $V$，把这根线段加入边集合 $E$

4. 伪代码

// C_free: 连续的自由空间
// q_start: 起点
// q_goal: 终点
// \Delta q: 每次生长的固定步长
// K: 最大迭代次数（防止死循环）

function RRT(C_free, q_start, q_goal, \Delta q, K):

    // ---------------- 第 1 步：初始化 ----------------
    Tree.init(q_start)  // 树的根节点是起点

    // ---------------- 第 2 步：主循环 ----------------
    for i = 1 to K:

        // 1. 掷飞镖：在空间中产生一个随机点
        // (工程中通常会加入 Goal Bias：比如 10% 的概率直接让 q_rand = q_goal)
        q_rand = Sample_Free_Space()

        // 2. 找最近的树枝
        q_near = Nearest_Node(Tree, q_rand)

        // 3. 沿着方向长出新节点
        q_new = Steer(q_near, q_rand, \Delta q)

        // 4. 碰撞检测：确保新树枝没有穿过障碍物
        if Collision_Free(q_near, q_new, C_free):

            // 安全！将新节点和连线加入树中
            Tree.add_node(q_new)
            Tree.add_edge(q_near, q_new)

            // 5. 检查是否已经够到了终点附近
            if Distance(q_new, q_goal) < Threshold:
                // 找到了！从终点顺着树枝往回找，提取路径
                return Extract_Path(Tree, q_start, q_new)

    // 达到最大迭代次数还没找到终点
    return "Path not found"


// ---------------- 辅助函数：步进生长 (Steer) ----------------
function Steer(q_near, q_rand, \Delta q):
    direction = normalize(q_rand - q_near)
    return q_near + direction * \Delta q

RRT 的伪代码中没有代价更新（没有 $g(n)$、$f(n)$ 的比大小运算），也没有优先队列。

它完全摒弃了图搜索算法那种“计算代价、挑最小代价走”的沉重包袱。它的哲学是“天下武功，唯快不破”：不追求走得多完美，而是利用极低计算成本的“随机连线 + 碰撞检测”，以恐怖的迭代速度迅速铺满整个高维空间。这也正是它能在多关节机械臂规划中大放异彩的根本原因。

5. RRT*（RRT-Star）

RRT* 是 RRT 的一个改进版本，旨在解决 RRT 找到的路径质量不高的问题。它在 RRT 的基础上引入了路径优化的机制，使得随着迭代次数的增加，找到的路径会逐渐趋近于最优。
RRT* 的核心思想是在每次成功添加新节点 $q_{new}$ 后，不仅要连接到最近的节点 $q_{near}$，还要检查周围一定范围内的其他节点，看看是否可以通过 $q_{new}$ 来优化它们的路径。
具体来说，RRT* 在添加新节点后会执行以下步骤：

1. 寻找邻居节点： 在树中找到所有距离 $q_{new}$ 在某个半径 $r$ 内的节点集合 $Q_{near}$
2. 选择最佳父节点： 对于 $Q_{near}$ 中的每个节点 $q_{near}$，计算通过 $q_{new}$ 连接到 $q_{near}$ 的路径代价。如果这个代价比当前 $q_{near}$ 的路径代价更小，就将 $q_{new}$ 作为 $q_{near}$ 的新父节点
3. 重新连接子节点： 对于 $Q_{near}$ 中的每个节点 $q_{near}$，如果通过 $q_{new}$ 连接到 $q_{near}$ 的路径代价更小，就将 $q_{near}$ 重新连接到 $q_{new}$

通过这种方式，RRT* 不仅能够快速找到一条可行路径，还能不断优化路径质量，使得最终找到的路径更接近最优解。

再优化

计算浪费： 撒点是完全盲目的。如果随机生成的点刚好落在障碍物内部（称为不可用节点 Unusable Nodes），算法虽然会丢弃它，但已经白白消耗了计算资源 。在复杂环境中，高达 40% 的生成节点可能都是不可用的 。

路径不平滑： RRT* 找出的路径是由许多随机线段拼接而成的，往往曲折且包含很多不必要的拐角，不利于机器人实际行驶 。

预处理：可行区域映射 (Feasible Region Mapping)

为了解决“盲目撒点”的问题，“拉直与剔除”策略 ：

1. 降维拉直： 将二维的网格地图（$M \times N$ 矩阵）直接“拉直”成一个一维的数组（行向量）

2. 建立白名单： 遍历这个一维数组，把所有属于“障碍物区域”的坐标直接删掉

3. 精准撒点： 经过处理后，剩下的数组就是一个纯粹的“可行区域白名单” 。之后 RRT* 在随机撒点时，只会在这个白名单里挑
   效果： 这从根本上杜绝了随机点落在障碍物里的可能性，让算法的每一次计算都落在实处，大大缩短了计算时间

后处理：“基因突变”与“节点修剪”

拿到初始路径后，算法进入后处理阶段，使用两种受生物学启发的算子来把路径“拉直拉顺” 。

• 算子 A：细菌突变 (Bacterial Mutation Operator)

  • 算法会把初始路径（细菌）复制出好几个“克隆体”
  • 在这些克隆体上随机选取一段路径进行“突变”（用地图上其他的可行节点替换掉原来的节点）
  • 接着，算法会用一个代价函数 (Cost Function) 来给每个克隆体打分。这个打分不仅看路径长度，还会对碰撞进行严厉惩罚，并考量转弯的平滑度
  • 得分最好的那个克隆体会被保留下来，继续下一代突变 。这能有效缩短整体路径长度

• 算子 B：节点删除 (Node Deletion Operator)

  • 这是基于几何学中三角形不等式 (Triangle Inequality)（两边之和大于第三边）原理的操作
  • 算法会审视路径上的节点。如果删除中间的一个拐点节点，让路径直接从前一个节点连到后一个节点，且这条新连线没有撞到障碍物，那么这个拐点就是多余的，直接删除
  • 效果： 这一步能大幅减少路径上的节点数量，把“Z”字形的曲折道路直接拉成直线