引言
- 记录一下这段时间了解到的路径规划算法，并进行代码的实现
- 目前主流的路径规划算法可以分为:
- 基于采样，如RRT、RRT*、PRM
- 基于节点，如Dijistra、A、D
- 基于数学模型，如MILP、NLP
- 基于生物启发式，如NN、GN
- 多融合，如PRM-Nodebased
Dijistra algorithm
问题描述
Dijistra算法主要用于求解最短路径。
可以将问题抽象为：对于无向图G=<V,E>,每条边E[i]的长为W[i]，找出由顶点每个点的最短路径。其中V为点集，E为边集。
核心思想
- 维护一个数组，表示从起点到这个点的最短路径
- 引入两个集合
S和U，S用于记录已经求出最短路径的顶点，U用于记录未求出最短路径的顶点 - 用过遍历
S中的元素，选取U中最优的下一个节点，加入S中，并不断重复，知道找出所有点的最优路径。
伪代码
- 将起点设为最新点，更新最短路径数组
- 选取最短路径中的最小值，作为新的最新点，更新最短路径数组
- 重复第二步直至U集合为空
算法实例
step1：S={A}
| A | B | C | D | E | F | G |
|---|---|---|---|---|---|---|
| 0 | 6 | 3 | ∞ | ∞ | ∞ | ∞ |
选择C为最新的拓展点并加入S集中
step2：S={A,C}
| A | B | C | D | E | F | G |
|---|---|---|---|---|---|---|
| 0 | 5 | 3 | ∞ | 7 | ∞ | ∞ |
在U集合中，B最小，遂取B为新的拓展点并加入S中
step3：S={A,C,B}
| A | B | C | D | E | F | G |
|---|---|---|---|---|---|---|
| 0 | 5 | 3 | 11 | 7 | ∞ | ∞ |
在U集合中，E最小，加入S集合
step4：S={A,C,B,E}
| A | B | C | D | E | F | G |
|---|---|---|---|---|---|---|
| 0 | 5 | 3 | 9 | 7 | 12 | 9 |
在U集合中D，G都为最小，假设取D放入S集中
step5：S={A,C,B,E,D}
| A | B | C | D | E | F | G |
|---|---|---|---|---|---|---|
| 0 | 5 | 3 | 9 | 7 | 12 | 10 |
将G放入S中
step6：S={A,C,B,E,D,G}
| A | B | C | D | E | F | G |
|---|---|---|---|---|---|---|
| 0 | 5 | 3 | 9 | 7 | 12 | 10 |
F的最小路径不用更改
此时U集合为空集，结束规划。
代码实例
A* algorithm
背景
A算法最初发表于1968年，由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael发表。它可以被认为是Dijkstra算法的扩展。
由于借助启发函数的引导，A算法通常拥有更好的性能。
核心思想
\(g(n)=f(n)+h(n)\)
\(h(n)\) 为启发式函数