Dijkstra Floyd RRT A* D* LPA* D*-Lite 算法比较

代码请参考guyueju的代码

本文用的代号:

min_cost: 距离起始点的路径长度(代价)
U: 待处理的节点, 也会用 openList
S: 已处理的节点, 也会用 closeList
从U放到S的节点就是本次要处理的节点current_point

parent 父节点
best_path 最优路径

goal 目标
start_point 起始点

current_point 当前处理的node,也会用X代替
near_points   表示处理的node的临近节点, 也会用Y代替
childNodes 除了在closeList以外的周围的节点, 2d的 话有8个, 和near_points 意义相同

line_cost 直线距离长度 norm(point1 - point2), 用于计算相邻点的距离

min_cost 节点到start_point(有些算法是goal)的最短距离, path上所有点的line_cost的合
step 步数, 一格算一步, 有些能斜着走,斜着走的距离有些是1 有些是 (2^0.5)
全局path距离预估值 = 起点到当前点的min_cost + 当前点到终点的step

一, Dijkstra算法

1. 将起始点的min_cost设为0,其余设为inf.起始点放到S,此时起始点是current_point
2. while true循环
(1) 更新起始点的near_points的best_path和parent , 方法是比较current_point作为该节点的parent, 是否会降低其min_cost (第一次由于是inf,一定会更新), 并同步更新其parent
(2) 将near_points都放到U,作为待处理的节点
(3) 在U里取最小min_cost的节点,这个是最有希望成为best_path的, 作为下次处理的节点current_point ,并将其放到S
(4) 如果U是empty. 就跳出去,这时候表示所有需要处理的都处理完了,如果还没有更新到goal, 表示没有解
   
3. 如果goal的parent 不是inf, 表示有解

缺点:
min_cost只考虑距离start_point的距离,而不是预估的全局路径
如果障碍物会动就不能用了
 三维建模复杂

二, Floyd算法
n 网格总数

用一张大表不断的更新所有节点的best_path, 总共更新约n^3次数(每一格对其他格的min_cost都要更新2次,有n个格子,所以是n^3)
,最后得到一张大表,记录所有node到另一个node的best_path

缺点:
起始阶段对空间和时间的要求很高
min_cost只考虑距离start_point的距离,而不是预估的全局路径
如果障碍物会动就不能用了
三维建模复杂

优点: 算完后, 所有节点的最优路径都知道了, 可以随时更改start_point和goal,不需要重新计算

三, RRT, RRT*, RRT*-smart
step_length 前进的步长
search_range 搜索空间
tree 树
leaf 叶子节点
new_leaf 新生成的节点
1. while true循环
(1) 从起始点开始, 在search_range随机生成一个点 (也可以50%的次数直接使用goal) 作为sample
(2) 在距离sample最近的leaf 往sample前进一个step_length的点, 检查是否遇到障碍物, 并检查这个点和最近的leaf之间是否有障碍物
, 如果有就从新选取sample,如果没有就new_leaf加入tree, 更新其parent 和 min_cost
(3) RRT* 此时会寻找new_leaf周围 (step_length的倍数) 的节点, 查看这些节点如果用new_leaf作为parent是否能减少其min_cost, 如果会就更新, 这样做的好处是可以持续优化出最佳的路径, 例如图下start_point 和goal 之间没有障碍物, 最优路径应该是一条直线. 但如果是RRT, 最后结果还是一条弯曲的线, RRT*相对而言比较直 (RRT*-smart的算法,就是一条完全笔直的路径)

 

2. 如果new_leaf距离goal不到一个step_length, 且之间没有障碍物, 就找到path了,这个path一般不是全局best_path
3. RRT*-smart会检查RRT*生成的弯弯曲曲的path, 是否可以部分走直线 (机械臂运动中转弯的地方需要运动规划)

缺点: 随机生成tree的生长方向, 所以最后的best_path通常不是全局最优解
优点: 不需要对环境建模(栅格化) , 三维世界建模非常复杂, 机械臂在3d工作, 且工作空间小, 只要不碰到障碍物, 是不是最优路径差别不大, RRT是首选

RRT*-Smart 路径规划matlab实现
https://blog.csdn.net/howard789/article/details/123056180

四, A*算法
主要改进就是用全局路径预估长度作为更新节点的标准 (之前都是start_point到当前点)

g = start_point 到 current_point 的 line_cost
h = current_point 到 gaol 的预估step (行差 + 列差)
f = g+h
每个节点需要2个数据, f 和 best_path(其实有parent就可以)

建立一个path表包含2列
(1) node_id
(2) best_path: 从start_point到该节点的所有路径点的list

等于有node_id就知道他的最优路径是什么(不一定需要这种记录方式)

1. 取得start_point周围的node资讯(id g h f)
2. 把 start_point 加入closeList,其near_points加入openList, 更新其ghf,取最小 f 的node_id,作为current_node
3. while true循环
(1) 把 current_node 的near_points加入openList, 更新其ghf
(2) 选择 min(f)的点作为current_node,如果current_node是goal就结束,否则继续

缺点:
如果障碍物会动就不能用了
三维建模复杂

优点: 如果障碍物不会动的2d搜索, 且不需要变换start_point或goal, 是最好的方法

五, D*算法
动态A*算法, 就是考虑障碍物会变动, 当障碍物改变时, ,需要用一个方法记录这个点是否需要重新处理, 所以就要有至少2个变量(这里是k和h), 当一个node突然变成障碍物,
我们先将所有障碍物node的t设为inf, 之后逐一比较, 如果本来的k就是inf, 就是状态不变, 只需要考虑near_points否则就是要处理这个node和near_points,  

当 k=h 表示状况不变,当k<h表示新的障碍物出现了, 当k>h表示更优的路径出现了,以下用h_new表示

goal作为起始点开始更新数据, 途中遇到障碍物的时候, starrt_point变成机器人当前位置, 但是goal不会变, 只要多设立一个跳出循环的条件就可以共用代码

openList 记录(1) node id (2) k值(这里的k会等于node的k, 因为放进openList之前会更新k), k值的目的就是用来比较是否是最小值

node信息:
id
t: 状态(new/open/closed)
k: 记录的距离goal的min_cost (以下用k_old表示) ,初始状态都是inf
h: 重新计算的min_cost(以下用h_new表示),初始状态都是inf
parent

while true
1 选取openList里min(k_old) 作为current_node
2 对于current_node (X) 首先更新自己的h和parent
3 如果自己没有受到影响 k=h, 就把周围需要更新的node(Y)放到openList, 包括
   (1) t = new
   (2) 用current_node是parent,但是min_cost改变的
   (3) 不是goal,且可以用current_node作为parent降低min_cost
4 如果自己受到了影响, (1)(2) 依然放入openList, 但是(3)就要考虑到底是current_node作为父节点还是子节点
    Nodes(Y).h > Nodes(X).h + cost_X_Y 表示X作为父节点, 所以把X放入openList先更新一下,下一轮再更新Y
    Nodes(X).h > Nodes(Y).h + cost_X_Y 表示Y作为父节点, 所以把Y放入openList更新
    Y.t = closed
    Nodes(Y).h > X.k_old

5. 第一阶段完成的条件是openList空了,

6. 第二阶段将障碍物的h改成inf,开始更新 , 第二阶段完成的条件是 min(k)>h 表示所有都更新完了

   
缺点:
min_cost只考虑距离start_point的距离,而不是预估的全局路径
三维建模复杂

备注: D*算法的代码好像有问题, 某些情况会卡住

六, LPA* 算法
重新考虑了全局最短路径 k1 (起始点到current_node的line_cost + 到goal的预估step ), 如果2个node的k1相同的时候, 再考虑 k2(起始点到current_node的line_cost)
key = [k1, k2]

关于U:
就是openList, 里面包含三个资讯 id , k1 ,k2 , 比较的时候取min(k1) 如果k1相同取 min(k2), k1=k2(current_point到起始点的min_cost) + h (current_point 到 gaol 的预估step (行差 + 列差, 可以斜着走,距离算2^0.5) , k1就是全局path距离的预估值. k2 就是min_cost

之前的算法只有用一个值 (k1 或 k2) ,这里考虑了如果只有一个指标而值相同的情况, 其实这种情况出现的几率微乎其微, 而且出现的时候就随便选一个, 问题也不大

放入U之前会把rhs改为inf,使其不一致

node的讯息:
id
g: 记录的min_cost
rhs : 最新的min_cost
parent

比起D*, LPA*不再需要t(new/open/closed) 的资讯了, 用了超级复杂且容易混乱的方法使得 rhs != g的时候代表open,  rhs == g 的时候代表closed,  rhs==g==inf 的时候可能是new 也可能是closed, 虽然少了一个参数, 但是代码逻辑变得超级复杂, 不一定好

1. 先把所有的node的g和rhs都设为inf, 这是均衡的状态
2. 改变start_point的rhs =0 , 计算key并放入U
3. while true
(1) 选取U里的min(key) 作为current_point
(2)如果current_point(X) 是goal 或是 current_point 状态均衡, 表示没有需要处理的了, 跳出
(3)比较rhs 和 g

状态一: 如果 rhs > g 表示第一次处理(rhs=inf) 或是有障碍物出现了, 原来的最优解不存在了, min_cost增加了
    先把更新g=rhs
    1) 如果X是start_point,  处理完了
    2) 如果X不是start_point, 重新计算rhs, 如果g != rhs 重新放入U表示还没处理完, 否则就是处理完了
    3) 无论是不是start_point 都要取得near_points, 更新g后放入U

状态二: 如果rhs == g 或 rhs < g (表示g可能需要更新了)
    先把rhs=inf
    1) 如果X是start_point, 放入U(因为g是0, 下一轮走rhs>g, 等于重新初始化)
    2) 如果X不是start_point, 如果重新计算的rhs是inf就结束,表示是障碍物, 否则放入U(下一轮走rhs>g, 更新rhs相同后结束)
    3) 无论是不是start_point 都要取得near_points, 更新g后放入U    

4. 新的障碍物出现, 把障碍物的点rhs和g都改成inf放入U进行循环

总结来说, start_point和一般的格子,会在状态一结束, 障碍物会在状态二结束.

缺点: 三维建模复杂

七, D*-Lite 算法
和LPA*相比, 以goal为起始点(和D*相同),

增加km减少重新规划时的计算量
第一阶段的h (h1)是current_point到start_point的距离, km是0
第二阶段的h (h2)是current_point到机器人位置的距离, km是机器人到start_point的距离 (km只需要计算一次)
h2+km = h1

缺点: 三维建模复杂

优点: 引入km,相比LPA*进一步降低计算量

总结:
2d空间:
障碍物不变: 如果在同一个空间下, 需要随时变换start_point或goal, 可以考虑Floyd算法, 否则就用A*
障碍物会变: D*-Lite

3d空间: RRT (唯一不需要将空间栅格化的方法)