路径规划——D*算法

路径规划——D*算法

D Star算法是一种用于动态环境下的算法，它可以在环境变化时快速更新路径。

算法原理

D Star算法是一种反向增量式搜索算法，反向即算法从目标点开始向起点逐步搜索；增量式搜索，即算法在搜索过程中会计算每一个节点的距离度量信息H(x)，在动态环境中若出现障碍物无法继续沿预先路径搜索，算法会根据原先已经得到的每个点的距离度量信息在当前状态点进行路径再规划，无需从目标点进行重新规划。

算法流程：

1.起初，起点和终点的位置是已知的，环境中的障碍物未知或部分未知。初始化一个开放列表openlist(优先队列)，将终点添加到该列表中，并将起点作为搜索目标。
2.在路径搜索过程中，D Star算法不使用启发信息，更像Dijkstra算法，由于是反向搜索，D Star算法计算每个节点到终点的成本代价，然后通过扩展节点来寻找从起点到终点的最优路径。在初次运行时，它使用Dijkstra算法生成一条从起点到终点的最短路径。
3.然而，当环境发生变化时，例如，检测到新的障碍物或路径变得不可通行时，要求算法可以动态更新路径。它通过回溯先前的路径，从遇到障碍物的节点开始重新计算新的路径。
4.在发生变化的区域重新执行D Star算法，从变化点开始，重新计算新的路径。D Star算法的一个关键特性是局部修复，即它只需要重新计算受到变化影响的部分路径，而不是整个路径。这使得算法在动态环境中能够高效地适应环境变化。
5.一旦路径更新完成，算法继续按照新的最短路径前进。每当环境再次变化时，重复上述步骤，直到达到终点。

D*算法有三个重要的函数：
1.process_state()：该函数是用来降低openlist表中的某个节点x(state)的h(x)值或者传播节点x的h(x)值变更信息给邻居节点，让邻居节点进行相应变更的同时进行路径搜索查找的；
2.insert(x,val)：该函数是用来修改节点x的状态以及h(x)值和k(x)值的；
3.modify_cost(x,y,val)：该函数是用来修改节点x和y之间的移动代价cost(x,y)，而且根据节点y的状态t(y)的情况，可能对节点y的h(y)值和状态t(y)进行修改的

函数process_state()是D*算法的核心，具体代码如下：

def process_state(self):
    node =self.min_state() # Find the node whose value of node.k is the smallest.
    self.searched.append(node.position)

    if node is None:
        return -1

    k_old = self.get_kmin() # Find the minimum node.k as k_old
    self.delete(node) # Delete the node from openlist, and modify node.t from 'OPEN' to 'CLOSE'

    neighbors, distances = self.get_neighbors(node)
    # RASIE State, its path cost may not be optimal
    if k_old < node.h:                                                                              
        for neighbor,distance in zip(neighbors,distances):                                          #
            if neighbor.h <= k_old and node.h > neighbor.h + distance:                              #
                node.parent = neighbor.position # Added obstacles, replanning, find better neighbor #
                node.h = neighbor.h + distance                                                      # 检查node的最优相邻状态，如果存在则降低 h 值

 	# LOWER State, its path cost is optimal since h ( X ) is equal to the old k_min .
    if k_old == node.h:
        for neighbor,distance in zip(neighbors,distances):
            if neighbor.t == 'NEW' or (neighbor.parent == node.position and neighbor.h != node.h+distance) \
            or (neighbor.parent != node.position and neighbor.h > node.h+distance):
                neighbor.parent = node.position # Searching commonly
                self.insert(neighbor,node.h+distance)
    else:
        for neighbor,distance in zip(neighbors,distances):
            if neighbor.t == 'NEW' or (neighbor.parent == node.position and neighbor.h != node.h+distance):
                neighbor.parent = node.position
                self.insert(neighbor,node.h+distance)
            else:
                if neighbor.parent != node.position and neighbor.h > node.h + distance:
                    self.insert(node,node.h)
                else:
                      if neighbor.parent != node.position and node.h > neighbor.h+distance \
                      and neighbor.t == 'CLOSED' and neighbor.h > k_old:
                          self.insert(neighbor,neighbor.h)

	return self.get_kmin()

下面解析此函数：

# RASIE State, its path cost may not be optimal
if k_old < node.h:                                                                              #
    for neighbor,distance in zip(neighbors,distances):                                          #
        if neighbor.h <= k_old and node.h > neighbor.h + distance:                              #
            node.parent = neighbor.position # Added obstacles, replanning, find better neighbor #
            node.h = neighbor.h + distance   

“RASIE”状态，其路径代价可能不是最优的，可以通过重新计算邻居节点来寻找最优路径；主要是在障碍物更新之后发挥作用。
当前访问的节点受到了新增障碍物的影响(比如，当前节点的父节点是新增障碍物),向h值越小的方向扩展，即从当前节点向靠近目标节点的方向扩展，并更新当前节点的父节点以更新路径.

# LOWER State, its path cost is optimal since h ( X ) is equal to the old k_min .
if k_old == node.h:
    for neighbor,distance in zip(neighbors,distances):
        if neighbor.t == 'NEW' or (neighbor.parent == node.position and neighbor.h != node.h+distance) \
        or (neighbor.parent != node.position and neighbor.h > node.h+distance):
            neighbor.parent = node.position # Searching commonly
            self.insert(neighbor,node.h+distance)

“LOWER”