路径规划：基于搜索的算法D*

文章目录

一、基本知识

1、概述

基本原理：
由终点向起点进行搜索，使用Dijkstra算法，存储路网中目标点到每个点的最短路径长度k, 和该节点到目标点的实际长度值h，开始情况下k=h并且存储每个节点的上一个节点，保证能够沿着链接走下去。
计算结束后，获取了一条当时最优路径。当到某个节点时，发现该节点已经无法通行，则对已存储的路网信息一些相关点的h值进行修改变大），选择一个邻居点满足仍然h==k的，即仍然是最优路径上的点，作为下一个点，然后走到终点。

应用场景
A*在静态路网非常有效，但不适用在动态路网，环境如权重等不断变化的动态环境下。

D*是一种启发式的路径搜索算法，是火星探测器采用的寻路算法，适合面对周围环境未知或者周围环境存在动态变化的场景。

2、符号定义

（1） D 维护要评估的节点列表，称为“OPEN list”。G表示终点,两个点的记号中会省略。
c(X，Y): x和y的cost。
（2） t(x)表示x的状态，包括：
NEW：它从未被列入OPEN list
OPEN：当前在OPEN list中
CLOSED：已经从OPEN list中剔除
（3） D的估计函数包括h(G,X)和k(G,X)：h函数会随着拓扑结构的变化而变化，key始终是h中最小的那一个。
Kmin：当前OPEN中key的最小值
Kold：上一次的最小值

OPEN的节点分为两类：
RAISE：它的成本比上次OPEN list时要高
LOWER：它的成本比上次OPEN list时要低

二、算法流程

1、主要流程：

算法步骤：
从目标点出发使用类似dijkstr的方法计算每个节点的h，直到起始点被OPEN表弹出。得到了静态环境下所有节点的信息（h and k）。
从起始点开始利用第一步获得的指针b（从一个节点指向下一个节点）一步步走向终点。若在某一步发现会碰撞，那么将该节点的h置为无穷大后放入OPEN表，开始更新指针b（h也会更新），从而更新规划路径，这样只会改变周围节点的信息而无需replan，大大减少了规划时间。

2、伪代码：

文末参考论文中伪代码如下：
在这里插入图片描述

(图片来源于参考论文)

具体D*主要包括两个部分：
PROCESS-STATE: 计算终点到当前节点的最优cost.
MODIFY-COST: 用来修正cost。
具体如下：
(1) 将所有节点的tag设置为NEW,h(G)设为0，将G放置在OPEN中。
（2）循环，直到当前的节点X从OPEN中移除，表示找到了一条从X出发到达终点的路径。
（3）根据获得的路径，从节点X往后移动，直到达到终点或者检测到cost发生变化。
（4）当cost发生变化时，调用MODIFY-COST，并将因为障碍物而cost受到影响的节点重新放入OPEN中。
假设Y是发现状态时机器人所处的节点。通过调用PROCESS−STATE直到kmin ≥ h(Y), 此时cost的变化已经传播到Y，因此可以找到一条新的从Y到终点的最优路径。

3、流程理解：

（1）第一个函数会被反复调用，直到机器人的状态X从列表中删除，或者返回-1值，此时序列{x}要么已经计算出来，要么不存在。
然后机器人继续跟随序列中的反向指针，直到它达到目标或发现弧代价函数中的错误(例如，由于检测到障碍物)。
（2）第二个函数立即被调用，以纠正并将受影响的状态放到列表中。
设Y为机器人在其中发现错误时的状态。通过调用第二个函数直到它返回Kmin >= h(Y)，成本变化被传播到状态Y。此时，构建了一个新的序列{Y}，机器人继续沿着序列中的反向指针朝着目标前进。
（3）初始：所有结点状态t(x)被设置为new，h(G)为0，终点加入OPEN list中。
（4）MIN - STATE返回open list中最小的K（）值或者空。Delete(x)从openlist中删除状态x,令t(x)=closed;
（5）Insert(x, Hnew),
如果t(x)=new,计算k(x)=Hnew;
如果t(x)=OPEN,计算k(x)=min(k(x),Hnew);
如果t(x)=closed,计算k(x)=Hnew,
设置h(x)=Hnew,t(x)=OPEN,在k排序的open list中放置状态x;
（6）弹出open表中最小的节点，并删除这个节点。然后分类处理之：
（7）k_old<h(x): 说明该节点x处于raise状态，可以设想x是之前那个父节点突变为墙的子节点。当前h(x)升高说明原来的路径已经不是最优的了，如果在x周围能找到一个点，h.y+c（x,y）更小，那就修改x的父节点，重置其h的值。
（8）k_old=h(x): 该节点x处于lower的状态，并没有受到障碍影响，或者还在第一遍遍历的阶段。if后面的判断代表：周围是标记为new的节点，代表第一次被遍历到。or 它的父节点是X，但是h.y却不等, 这说明h.y被更改了，但是父节点还没有变。