路径规划算法

Jiankyeer

已于 2023-04-21 20:34:42 修改

阅读量389

点赞数

文章标签：算法

于 2023-04-21 20:24:36 首次发布

本文链接：https://blog.csdn.net/meFranky/article/details/130296287

版权

Search-based planning

Breadth-First Searching (BFS)
Depth-First Searching (DFS)
Best-First Searching
Dijkstra
A*
Bidirectional A*
Anytime Repairing A*
Learning Real-time A* (LRTA*)
Real-time Adaptive A* (RTAA*)
Lifelong Planning A* (LPA*)
Dynamic A* (D*)
D* Lite
Anytime D*

Sampling-based Planning

RRT
RRT-Connect
Extended-RRT
Dynamic-RRT
RRT*
Informed RRT*
RRT* Smart
Anytime RRT*
Closed-Loop RRT*
Spline-RRT*
Fast Marching Trees (FMT*)
Batch Informed Trees (BIT*)

Dijkstra

Dijkstra 算法是典型最短路算法，用于计算一个节点到其他所有节点的最短路径

定义：

代价：F(n) = g(n)
g(n) ：从起点到节点 n 的代价（距离）
open list ：存放当前可到达、且未确定最小代价路径的节点
closed list ：存放已经找到最小代价路径的节点

流程：

从起点开始逐步扩展，每一步为一个节点找到代价最小的路径，即：
- 从 open list 选择代价最小的节点，将其收录到 closed list
- 遍历新收录节点的所有可访问邻节点，更新代价

特性：

完备性：如果在起始点和目标点之间有路径解存在，就一定可以得到解；如果得不到解，就一定说明没有解存在
最优性：对于某个评价指标（一般为路径的长度），规划得到的路径是最优的

算法

将起点放入 open list

执行循环：

- 如果 open list 为空：搜索失败，结束
- 取 open list 中代价（即，g(n)）最小的节点（记作 Node1），将其放入 closed list
- 如果节点 Node1 为终点：找到目标路径，结束
- 遍历节点 Node1 的（不在 closed list 中的）邻接节点
    - 记当前遍历的邻接节点为 Node2
    - 如果节点 Node2 在 open list 中：更新节点 Node2 的代价
    - 如果节点 Node2 在 open list 中：计算节点 Node2 的代价，并将其加入 open list

待循环结束，即得到从起点到终点的代价最小路径

Dijkstra 算法的 C++ 实现可参考：Dijkstra

示例

问题描述

寻找一条从节点 v1 到节点 v6 的最短路径

解题流程

第一步（从 v1 出发）：

将 v1 加入 open list
由于 open list 只有 v1 这一个节点，将 v1 加入到 closed list
v1 的邻接节点有 v2 和 v4
- 计算从 v1 到达 v2 的代价为 2，将 v2 加入到 open list
- 计算从 v1 到达 v4 的代价为 1，将 v4 加入到 open list
更新 open list 和 closed list
- open list ：v2（代价为 2），v4（代价为 1）
- closed list ：v1（代价为 0）

第二步（从 v1–v4 出发，计算 v1 通过 v4 到其它节点的距离）：

open list 中代价最小的节点为 v4 ，将 v4 加入到 closed list
v4 的邻接节点有 v3，v6 和 v7
- 计算从 v1 经 v4 到达 v3 的代价为 3，将 v3 加入到 open list
- 计算从 v1 经 v4 到达 v6 的代价为 9，将 v6 加入到 open list
- 计算从 v1 经 v4 到达 v7 的代价为 5，将 v7 加入到 open list
更新 open list 和 closed list
- open list ：v2（代价为 2），v3（代价为 3），v6（代价为 9），v7（代价为 5）
- closed list ：v1（代价为 0），v4（代价为 1）

第三步（从 v1–v2 出发，计算 v1 通过 v2 到其它节点的距离）：

open list 中代价最小的节点为 v2 ，将 v2 加入到 closed list
v2 的邻接节点有 v4 和 v5 ，其中，v4 已经在 closed list，即，无需再次考虑到达 v4 的代价
- 计算从 v1 经 v2 到达 v5 的代价为 13，将 v5 加入到 open list
更新 open list 和 closed list
- open list ：v3（代价为 3），v6（代价为 9），v7（代价为 5），v5（代价为 13）
- closed list ：v1（代价为 0），v4（代价为 1），v2（代价为 2）

第四步（从 v1–v4–v3 出发，计算 v1 通过 v4、v3 到其它节点的距离）：

open list 中代价最小的节点为 v3 ，将 v3 加入到 closed list
v3 的邻接节点有 v1 和 v6 ，其中，v1 已经在 closed list
- v6 已经在 open list 中，考虑是否更新 v6 的代价：从 v1 经 v4、v3 到达 v6 的代价为 8，小于此前所得的代价 9 ，故而将 v1 到 v6 的代价更新为 8
更新 open list 和 closed list
- open list ：v6（代价为 8），v7（代价为 5），v5（代价为 13）
- closed list ：v1（代价为 0），v4（代价为 1），v2（代价为 2），v3（代价为 3）

第五步（从 v1–v4–v7 出发，计算 v1 通过 v4、v7 到其它节点的距离）：

open list 中代价最小的节点为 v7 ，将 v7 加入到 closed list
v7 的邻接节点有 v6
- v6 已经在 open list 中，考虑是否更新 v6 的代价：从 v1 经 v4、v7 到达 v6 的代价为 6，小于此前所得的代价 8，故而将 v1 到 v6 的代价更新为 6
更新 open list 和 closed list
- open list ：v6（代价为 6），v5（代价为 13）
- closed list ：v1（代价为 0），v4（代价为 1），v2（代价为 2），v3（代价为 3），v7（代价为 5）

第六步（从 v1–v4–v7–v6 出发）：

open list 中代价最小的节点为 v6 ，将 v6 加入到 closed list
节点 v6 为终点，因此，已经找到 v1 到达 v6 的代价最小路径，最小代价为 6，对应的路径为 v1–v4–v7–v6

最优性证明

以 v4 为例，证明 Dijkstra 的最优性：

在将 v4 收录进 closed list 的时候，为什么就已经找到了到达 v4 的代价最小路径？

可利用反证法证明

假设：在将 v4 收录进 closed list 的时候，未找到到达 v4 的代价最小路径。即，v1–v4 不是代价最小路径，v1–v2–v4 才是代价最小路径（注意：v4 是在第二步被收录进 closed list 的）
基于假设可知，v1–v2–v4 的代价小于 v1–v4 的代价，进而，v1–v2 的代价也小于 v1–v4 的代价。因此，在第一步结束时，v4 并不是 open list 中代价最小的节点，v2 才是代价最小的节点。即，第二步应将 v2 收录到 closed list ，而不是将 v4 收录到 closed list ，与已知条件相矛盾。因此，假设不成立

类似地，可以证明每一个被收录的节点都已经找到从起点出发的、代价最小的路径

A*

A*（A-Star）算法是一种静态路网中求解最短路最有效的方法

A* 算法和 Dijistra 算法的区别在于有无估算代价
A* 算法中估算代价为 0 的情况即为 Dijistra 算法

定义：

代价：F(n) = g(n) + h(n)
g(n) ：从起点到节点 n 的实际代价（距离）
h(n) ：启发式函数，用于描述从节点 n 到终点的最佳路径的估算代价

流程：

从起点开始逐步扩展，每一步为一个节点找到代价最小的路径，即：
- 从 open list 选择代价最小的节点，将其收录到 closed list
- 遍历新收录节点的所有可访问邻节点，更新代价

特性：

完备性
最优性（确保最优性的前提：节点 n 到终点的估算代价 h(n) 应小于或等于节点 n 到终点的实际代价）

与 Dijkstra 算法相比，A* 算法减少了收录的栅格数目，搜索速度更快

算法

将起点放入 open list

执行循环：

 - 如果 open list 为空：搜索失败，结束
 - 取 open list 中代价（即，**g(n) + h(n)**）最小的节点（记作 Node1），将其放入 closed list
 - 如果节点 Node1 为终点：找到目标路径，结束
 - 遍历节点 Node1 的（不在 closed list 中的）邻接节点
    - 记当前遍历的邻接节点为 Node2
    - 对节点 Node2 做以下处理
      - 如果节点 Node2 在 open list 中：更新节点 Node2 的代价
      - 否则：计算节点 Node2 的代价，并将其加入 open list

待循环结束，即得到从起点到终点的代价最小路径