代码收藏家技术教程 2022-08-12

D*算法原理与程序详解（Python）

提示：前文写了D搜索算法，是一种贪心算法。

文章目录

一、D*算法是什么？

二、原理以及代码步骤

1.原理分析

2.代码解释

总结

一、D*算法是什么？

D*算法也是用于机器人路径规划问题的启发式方法，它是一种局部规划方法，即仅仅已知一部分地形，对地形的未知部分进行假设，并在这些假设下找到当前坐标到目标坐标的最短路径。然后机器人沿着这条路走，当它观察到新的地图信息（如从前未知的障碍）时，将这些信息添加到地图中，并在必要时重新规划从当前坐标到给定目标坐标的新的最短路径。重复这个过程，直到达到目标坐标或无法达到目标坐标。

二、原理以及代码步骤

1.原理分析

如上图的空间，给定起点（绿色点）、目标点（G蓝色）、障碍（红色），如何进行路径规划？

首次搜索
将终点G置于openlist中，采用Dijkstra进行搜索，规定上下左右的代价为10，斜着方向的代价为14。不熟悉Dijkstra算法的朋友看我之前的博文Dijkstra算法在python中的实现。
大概说一下，每个格子包含有三个量（节点名，代价，父节点），从终点A开始，搜索周围的邻节点，给它们分别标号：B,C,D,E,F，代价按照规定的计算（上下左右的代价为10，斜着方向的代价为14），并把终点A放入close list里面（不再遍历）。然后找出代价最小的节点作为新节点，继续搜索，在搜索过程中，如果有节点K已经有代价，但是以新的点为父节点，这个节点K的代价值更小，那么更新其代价值和父节点。

搜索结束条件就是终点从openlist中弹出进入了closelist。
最终，我们搜索到了起点，结束搜索。从起点开始找父节点，一路搜索到终点。
遇到障碍
如果机器人在按照原来规划路径行进的时候，路上遇到障碍（不在规划路径上的障碍忽略不计）。例如（3，3）遇到障碍。

修改这个点的h值为无穷大（inf），并且令障碍的所有子节点的h都为无穷大。

图中浅绿色节点是路径上障碍的子节点X，子节点X的h变为inf，将节点X从closelist中弹出，放入openlist列表。
接下来就是将这个改变进行扩散。
先取出openlist中k值最小的节点，还是浅绿色节点X（k=50,h=inf）
找X的周围邻接点Y，如果能够让X的h值变小，就让Y成为X的父节点，这样X（3，2）的父节点变为（4，3），hx变为58，此时X的子节点如（3，1)，而hx+10(x和y的代价)不等于hy(60)，说明障碍的影响没有扩散到子节点，所以更改子节点（3，1）的h值为hx+10。
因为（4，3）节点到目标点的路径其实是之前计算过的，所以不必计算。
扩散的结束条件就是k_min(openlist中所有节点最小的k值)>=hx(当前点X的h值)。
就能找到一条路径：

2.代码解释

D算法首次搜索

# coding=utf-8
import matplotlib.pyplot as plt
import numpy as np
import math

map_grid = [[1 for j in range(0, 8)] for i in range(0, 8)]  # 定义列表
map_grid = np.array(map_grid)  # 将列表转化为数组，因为只有数组才有维度的概念，方便切片
map_grid[3:6, 1] = 0  # 障碍物
map_grid[3:6, 5] = 0
map_grid[0, 3] = 5  # 起点
map_grid[7, 3] = 6  # 终点


def draw_effect(map_grid,second_path):
    plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)  # 绘制热力图
    plt.colorbar()
    plt.xlim(-1, 8)  # x轴的范围
    plt.ylim(-1, 8)
    my_x_ticks = np.arange(0, 8, 1)  # x轴标号的范围
    my_y_ticks = np.arange(0, 8, 1)
    plt.xticks(my_x_ticks)
    plt.yticks(my_y_ticks)
    second_path = np.array(second_path)
    plt.plot(second_path[:, 1:2],second_path[:, 0:1],'-')
    # plt.grid(True)  # 开启栅格  可以不开启
    plt.show()  # 可视化


open_list = [[7, 3, 0, 0, None, None]]  # 将终点置于open列表中列表中分别有x0,y0坐标,h值,父节点X、Y坐标
close_list = []


# draw_effect(map_grid)
# 将邻域放入open_list中
def open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list):
    if 0 <= x1 <= 7 and 0 <= y1 <= 7 and map_grid[x1, y1] != 4 and map_grid[x1, y1] != 0:  # 左边没有越界并且没有在closelist里面
        if map_grid[x1, y1] == 3:  # 如果是在open_list中,h要更新
            open_list = np.array(open_list)
            if (h1 + h0) < open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 2]:
                h = h1 + h0
                k = h1 + h0
                open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 2] = h
                open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 3] = k
                open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = x0
                open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1)), 4] = y0
            open_list = list(open_list.tolist())

        else:  # 是new节点
            h = h1 + h0
            k = h1 + h0
            # open_list = list(open_list)
            open_list.append([x1, y1, h, k, x0, y0])
            map_grid[x1, y1] = 3

    return open_list


# 首次搜索
def first_search(open_list, close_list, map_grid):  # 给出终点坐标，完成首次遍历
    # 采用D算法遍历
    # 选openlist中h最小的,将openlist按照h排序，取第一个，并删除第一个，将它放到close_list里面
    open_list = list(open_list)
    open_list.sort(key=lambda x: x[2])
    # open_list.pop(0)
    insert_list = open_list[0]  # 引入中间列表，用来存储每一次被选中的遍历的点
    x0 = int(insert_list[0])
    y0 = int(insert_list[1])
    open_list.pop(0)
    close_list.append(list(insert_list))
    map_grid[x0, y0] = 4  # 被加入到close_list里面

    # 找insert_list的邻域 ----->寻找顺序：从左边开始逆时针
    h0 = int(insert_list[2])

    x1 = x0
    y1 = y0 - 1
    h1 = 10
    open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)

    x1 = x0 - 1
    y1 = y0 - 1
    h1 = 14
    open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)

    x1 = x0 - 1
    y1 = y0
    h1 = 10
    open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)

    x1 = x0 - 1
    y1 = y0 + 1
    h1 = 14
    open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)

    x1 = x0
    y1 = y0 + 1
    h1 = 10
    open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)

    x1 = x0 + 1
    y1 = y0 + 1
    h1 = 14
    open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)

    x1 = x0 + 1
    y1 = y0
    h1 = 10
    open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)

    x1 = x0 + 1
    y1 = y0 - 1
    h1 = 14
    open_list = open_list_append(x0, y0, x1, y1, h0, h1, map_grid, open_list)

    return [open_list, close_list, map_grid]


while map_grid[0, 3] != 4 and open_list != []:
    [open_list, close_list, map_grid] = first_search(open_list, close_list, map_grid)

# 首次搜索完成
first_path = []
close_list = np.array(close_list)
xn = 0
yn = 3
while xn != 7 or yn != 3:
    list1 = list(close_list[np.where((close_list[:, 0] == xn) & (close_list[:, 1] == yn))][0])
    xn = int(list1[4])
    yn = int(list1[5])
    first_path.append(list1)

first_path.append([7, 3, 0, 0, None, None])

没开始搜索前的地图

第一步搜索图

设置随机障碍，开始第二次搜索

# 通过上面的程序已经找到了一条路径，完成了第一次的搜索，此时每个节点的h和k是相等的。此时开始点在close list里面，最短路径在firstpath中。
# 可以看出，每个节点的父节点都是该节点的八个邻节点中k值最小的哪个。
# 当出现动态变化时，我们可以利用这个图尽快修正我们的路径，而不是重新规划。
# 当我们检测到某点被阻碍了：1、修改这个点的h值，h变为inf，把它放入openlist中。注意此时该节点的k还是小值，是原来哪个h的值，因此它将立即被取出
# 2、把这个修改扩散出去，直到kmin >=h
# 设置一个突然出现的障碍
map_grid[3, 3] = 0

close_list[np.where((close_list[:, 4] == 3) & (close_list[:, 5] == 3)), 2] = math.inf
close_list[np.where((close_list[:, 0] == 3) & (close_list[:, 1] == 3)), 2] = math.inf
insertRow = list(close_list[np.where((close_list[:, 4] == 3) & (close_list[:, 5] == 3))][0])
x = int(insertRow[0])
y = int(insertRow[1])
open_list.append(insertRow)  # ->>>>>>open_list是列表格式
map_grid[x, y] = 3
close_list = list(close_list.tolist())  # ----->>>>>close_list是列表格式
close_list.remove(insertRow)
open_list.sort(key=lambda x: x[3])  # 先排序，选择k最小的节点
k_min = open_list[0][3]  #
hx = open_list[0][2]


# 接下来把这个点扩散出去

def find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list):
    close_list = np.array(close_list)
    hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0]
    if (hy <= k_old) and (hx > hy + h1):
        close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 4] = x1
        close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 5] = y1
        close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0)), 2] = hy + h1
        hx = hy + h1
    return [hx, list(close_list.tolist())]

def find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, map_grid):
    close_list = np.array(close_list)
    hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0]
    if (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] == x0 and close_list[
        np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] == y0 and (hy != hx + h1)) or (
            (hy > hx + h1) and (
            (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0) or (
            close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0))):
        close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] = x0
        close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0
        close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1
        Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0])
        # 把Y放入open_list中
        close_list = list(close_list.tolist())
        close_list.remove(Y)
        open_list.append(Y)
        map_grid[x1, y1] = 3
    return [open_list, close_list, map_grid]

def find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list, map_grid):
    close_list = np.array(close_list)
    hy = close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2][0]
    if (close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] == x0 and close_list[
        np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] == y0 and (hy != hx + h1)):
        close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] = x0
        close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] = y0
        close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 2] = hx + h1
        Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0])
        # 把Y放入open_list中
        close_list = list(close_list.tolist())
        close_list.remove(Y)
        open_list.append(Y)
        map_grid[x1, y1] = 3
    else:
        if ((close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0 or close_list[
            np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0) and (hy > hx + h1)):
            #print(list(close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0))][0]))
            if map_grid[x0,y0]!=3:
                X = list(close_list[np.where((close_list[:, 0] == x0) & (close_list[:, 1] == y0))][0])
                close_list = list(close_list.tolist())
                close_list.remove(X)
                open_list.append(X)
            else:
                open_list = np.array(open_list)
                X = list(open_list[np.where((open_list[:, 0] == x0) & (open_list[:, 1] == y0))][0])
                open_list = list(open_list.tolist())
        #     # 把Y放入open_list中
            map_grid[x0, y0] = 3
        else:
            if ((close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 4] != x0 or close_list[
                np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1)), 5] != y0) and (hx > hy + h1)) and \
                    map_grid[x1, y1] == 4 and hy > k_old:
                if map_grid[x1, y1] != 3:
                    Y = list(close_list[np.where((close_list[:, 0] == x1) & (close_list[:, 1] == y1))][0])
                    close_list = list(close_list.tolist())
                    close_list.remove(Y)
                    open_list.append(Y)
                else:
                    open_list = np.array(open_list)
                    Y = list(open_list[np.where((open_list[:, 0] == x1) & (open_list[:, 1] == y1))][0])
                    open_list = list(open_list.tolist())
                # 把Y放入open_list中
                map_grid[x1, y1] = 3

    return [open_list, close_list, map_grid]

# 扩散程序 process-state:先弹出openlist列表中k最小的节点，并删除这个节点。然后分类处理：
def process_state(open_list, close_list, map_grid):
    # 修改这个点的h值
    open_list.sort(key=lambda x: x[3])  # 先排序，选择k最小的节点
    X = open_list[0]  # X表示k最小的节点
    x0 = int(X[0])
    y0 = int(X[1])
    close_list.append(X)  # 将它放入closelist
    map_grid[x0, y0] = 4
    open_list.remove(X)
    # 从openlist中删除这个节点
    # 分类处理：（该节点处于lower状态，该节点处于lower状态）
    k_old = X[3]
    hx = X[2]
    # print(close_list)

    if k_old < hx:  # k_old是上升状态
        x1 = x0
        y1 = y0 - 1
        h1 = 10
        [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)

        x1 = x0 - 1
        y1 = y0 - 1
        h1 = 14
        [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)

        x1 = x0 - 1
        y1 = y0
        h1 = 10
        [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)

        x1 = x0 - 1
        y1 = y0 + 1
        h1 = 14
        [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)

        x1 = x0
        y1 = y0 + 1
        h1 = 10
        [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)

        x1 = x0 + 1
        y1 = y0 + 1
        h1 = 14
        [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)

        x1 = x0 + 1
        y1 = y0
        h1 = 10
        [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)

        x1 = x0 + 1
        y1 = y0 - 1
        h1 = 14
        [hx, close_list] = find_neighbor(x0, y0, x1, y1, k_old, hx, h1, close_list)
        # 找它的邻节点，看能不能让它的h降低
        #print(hx)

    # if k_old == hx:  # 该节点x处于lower状态
    #     x1 = x0
    #     y1 = y0 - 1
    #     h1 = 10
    #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    #     x1 = x0 - 1
    #     y1 = y0 - 1
    #     h1 = 14
    #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    #     x1 = x0 - 1
    #     y1 = y0
    #     h1 = 10
    #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    #     x1 = x0 - 1
    #     y1 = y0 + 1
    #     h1 = 14
    #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    #     x1 = x0
    #     y1 = y0 + 1
    #     h1 = 10
    #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    #     x1 = x0 + 1
    #     y1 = y0 + 1
    #     h1 = 14
    #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    #     x1 = x0 + 1
    #     y1 = y0
    #     h1 = 10
    #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    #     x1 = x0 + 1
    #     y1 = y0 - 1
    #     h1 = 14
    #     [open_list, close_list, map_grid] = find_neighbor2(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    # else:
    #     x1 = x0
    #     y1 = y0 - 1
    #     h1 = 10
    #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    #     x1 = x0 - 1
    #     y1 = y0 - 1
    #     h1 = 14
    #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #     #
    #     x1 = x0 - 1
    #     y1 = y0
    #     h1 = 10
    #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #     #
    #     x1 = x0 - 1
    #     y1 = y0 + 1
    #     h1 = 14
    #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #     #
    #     x1 = x0
    #     y1 = y0 + 1
    #     h1 = 10
    #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #     x1 = x0 + 1
    #     y1 = y0 + 1
    #     h1 = 14
    #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #
    #     x1 = x0 + 1
    #     y1 = y0
    #     h1 = 10
    #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                        map_grid)
    #     x1 = x0 + 1
    #     y1 = y0 - 1
    #     h1 = 14
    #     [open_list, close_list, map_grid] = find_neighbor3(x0, y0, x1, y1, k_old, hx, h1, close_list, open_list,
    #                                                    map_grid)

    open_list.sort(key=lambda x: x[3])  # 先排序，选择k最小的节点
    k_min = open_list[0][3]  #

    return [open_list, list(close_list), map_grid,k_min,hx]

while k_min<hx:
    [open_list, close_list, map_grid,k_min,hx] = process_state(open_list, close_list, map_grid)


#避障
second_path = []
close_list = np.array(close_list)
xn = 0
yn = 3
while xn != 7 or yn != 3:
    list1 = list(close_list[np.where((close_list[:, 0] == xn) & (close_list[:, 1] == yn))][0])
    xn = int(list1[4])
    yn = int(list1[5])
    second_path.append(list1)

second_path.append([7, 3, 0, 0, None, None])
draw_effect(map_grid,second_path)
print("Find it")

完成第二次搜索图。