路径规划算法

路径规划算法

BFS 广度优先遍历

广度优先遍历与最短路径 | 菜鸟教程 (runoob.com)

5.1 Graph Traversals - BFS & DFS -Breadth First Search and Depth First Search - YouTube

Dijkstra 算法

3.6 Dijkstra Algorithm - Single Source Shortest Path - Greedy Method - YouTube

A*

Introduction to the A* Algorithm (redblobgames.com)

路径规划之 A* 算法 - 知乎 (zhihu.com)

D*

LPA*

D* lite

RRT 快速随机探索树

RRT与RRT算法具体步骤与程序详解（python）_问题很多de流星的博客-CSDN博客_rrt算法

Rapidly-exploring random tree - Wikipedia

图示算法

算法步骤

1. 初始化整个空间，定义初始点qinit、终点qend、采样点数number of vertices in RRT K 、点与点之间的步长incremental distance Δq等信息
2. 在空间中随机产生一个点qrand
3. 在已知树的点集合中找到距离这个随机点最近的点 qnear
4. 在qnear到 qrand 的直线方向上从qnear 以步长Δq截取点 qnew
5. 判断从qnear到qnew之间是否存在障碍物，若存在则舍弃该点
6. 将qnew点加入到树集合中
7. 循环2~6，循环结束条件：有一个new点在终点的设定邻域内或生长树达到要求长度

伪代码

Algorithm BuildRRT
    Input: Initial configuration qinit, number of vertices in RRT K, incremental distance Δq
    Output: RRT graph G

    G.init(qinit)
    for k = 1 to K do //while 1 
        qrand ← RAND_CONF()
        qnear ← NEAREST_VERTEX(qrand, G)
        qnew ← NEW_CONF(qnear, qrand, Δq)
        G.add_vertex(qnew)
        G.add_edge(qnear, qnew)
    return G // if qnew near qend break
        

• "←" denotes assignment. For instance, "largest ← item" means that the value of largest changes to the value of item.
• "return" terminates the algorithm and outputs the following value.

In the algorithm above, "RAND_CONF" grabs a random configuration q**rand in C. This may be replaced with a function "RAND_FREE_CONF" that uses samples in C**free, while rejecting those in C**obs using some collision detection algorithm.

"NEAREST_VERTEX" is a function that runs through all vertices v in graph G, calculates the distance between q**rand and v using some measurement function thereby returning the nearest vertex.

"NEW_CONF" selects a new configuration q**new by moving an incremental distance Δq from q**near in the direction of q**rand. (According to [4] in holonomic problems, this should be omitted and q**rand used instead of q**new.)

代码示例

RRT*

(31条消息) 【规划】RRT算法图解_笑扬轩逸的博客-CSDN博客_rrt算法

路径平滑

三次样条插值

贝塞尔曲线

二次曲线拟合