（5-4）D*算法：探索机器人的新家

5.3.2  探索机器人的新家

1. 问题描述

在一个虚拟的机器人世界中，有一个机器人要找到它的新家。这个世界由一个8x8的网格地图组成，机器人可以在地图上移动，但某些区域可能被标记为障碍物，机器人不能穿过这些障碍物。机器人的起点是地图上的一个特定位置，它的新家是另一个特定位置。机器人必须找到一条能够避开障碍物的最短路径到达新家。

2. 要求

1. 给出一个8x8的网格地图，其中包含了机器人的起点、新家的位置以及一些障碍物的位置。请在地图上使用合适的符号标记出这些位置，例如：起点用"S"表示，新家用"E"表示，障碍物用"#"表示。
2. 使用D*算法，找到机器人从起点到达新家的最短路径，并在地图上用特定符号标记出这条路径。
3. 给出了另一个情景，其中某些障碍物的位置发生了变化。重新计算机器人从起点到达新家的最短路径，并标记出新路径。
4. 最后，使用可视化函数显示地图及其路径，让学生直观地了解机器人如何规划路径并避开障碍物。

实例5-2：动态路径规划系统（codes/5/lu.py）

本实例实现了基于D*算法的动态路径规划系统，具备动态环境适应性、高效路径规划、用户友好界面和开放可扩展性等功能。实例文件lu.py的具体实现流程如下所示。

（1）下面的代码创建了一个8*8的网格地图，并在地图上放置了起点和终点，以及一些障碍物。随后，使用numpy数组表示地图，方便进行操作。随后，代码将障碍物放置在地图的指定位置，将起点标记为5，将终点标记为6。

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  # 终点

（2）定义函数draw_effect，用于绘制地图和路径。函数接受两个参数：map_grid表示地图的二维数组，second_path表示路径的坐标点列表。函数draw_effect首先使用plt.imshow绘制地图的热力图，并设置了颜色映射为热图样式，边界插值方式为最近邻插值，并设置了颜色映射的取值范围。接着添加了颜色条，限定了图像的x轴和y轴范围，并设置了刻度标签。然后，将路径坐标点列表转换为NumPy数组，并使用plt.plot绘制路径。最后，通过plt.show()展示绘制的图像。

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()  # 可视化

（3）初始化两个列表open_list和close_list，用于实现路径搜索算法中的开放列表和关闭列表。

1. open_list 是一个包含单个元素的列表，该元素是一个列表，包含终点的坐标 (7, 3)、h 值为 0、父节点的坐标为 None。
2. close_list 是一个空列表，用于存储已经处理过的节点的信息。

上述列表在路径搜索算法中起到了关键作用，用于记录搜索过程中的节点状态。

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

（4）定义函数 open_list_append，用于将邻域放入开放列表 open_list 中。函数接受参数包括当前节点 (x0, y0)、邻域节点 (x1, y1)、两节点之间的代价 h0 和 h1、地图网格 map_grid 以及当前的开放列表 open_list。函数 open_list_append的具体实现流程如下所示。

1. 首先检查邻域节点是否越界且不在关闭列表中，并且不是障碍物。
2. 如果邻域节点在开放列表中，则更新其代价 h 和 k，以及父节点的坐标。若新的代价小于当前节点的代价，则更新邻域节点的相关信息。
3. 如果邻域节点不在开放列表中，则将其添加到开放列表，并将其标记为新节点。
4. 最后返回更新后的开放列表。

函数 open_list_append是路径搜索算法中的关键步骤，用于逐步扩展搜索范围并更新节点的状态信息。

# 将邻域放入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

（5）函数first_search实现了首次搜索的过程，主要包括如下所示的步骤：

1. 初始化起点和终点，并将终点放入开放列表 open_list 中，起点坐标 (7, 3)。
2. 在 while 循环中，当起点未被访问且开放列表不为空时，执行首次搜索。
3. 在 first_search 函数中，根据 D 算法的思想，选择开放列表中代价最小的节点进行扩展。首先从开放列表中选取代价最小的节点，将其放入关闭列表 close_list 中，然后对其周围的邻域进行搜索。
4. 针对当前节点 (x0, y0)，根据其邻域节点的代价和位置信息，更新邻域节点的状态并将其添加到开放列表中。
5. 循环直至起点被访问或开放列表为空。
6. 在首次搜索完成后，根据关闭列表中节点的父节点信息，构建路径 first_path。

此外，函数first_search的代码中还进行了一些额外的处理，例如标记起点和终点、处理障碍物等。

# 首次搜索
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])


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]

（6）实现了D*算法的关键部分，包括地图的初始化、首次搜索、邻居节点的更新等。首先，通过定义地图并设置起点、终点和障碍物，初始化了open列表和close列表。然后，在首次搜索中，采用D算法遍历地图，选取open列表中h值最小的节点进行扩展，更新其邻居节点的代价信息，并将其加入close列表。接着，根据D*算法的特性，对地图进行更新和路径优化，直至找到最优路径。

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())]

（7）函数find_neighbor2用于在给定的地图上查找指定点的邻居节点，并根据特定条件更新这些节点的信息。具体而言，该函数根据传入的参数和条件更新close_list中节点的信息，并将满足条件的节点添加到open_list中，同时更新地图上相应位置的状态。

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]

（8）函数find_neighbor3根据传入的参数和条件更新close_list中节点的信息，并将满足条件的节点添加到open_list中，同时更新地图上相应位置的状态。具体而言，函数find_neighbor3考虑了多种情况下的节点更新和操作，包括节点已被访问但需要更新、节点未被访问但需要加入open_list、以及节点已被访问但不需要更新等情况。

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]

（9）函数process_state用于迭代地更新open_list、close_list和地图状态，直到满足终止条件为止。在每一次迭代中，程序会从open_list中弹出具有最小k值的节点，并将其加入close_list中。然后，根据节点的k值和h值，以及与之相邻的节点的状态，进行分类处理。如果节点处于上升状态，则更新其邻居节点的信息；如果节点处于下降状态，则根据其邻居节点的状态进行相应的操作。最后，当open_list中最小的k值大于等于当前节点的h值时，停止迭代。然后，根据close_list中记录的路径信息，生成避障路径，并将结果可视化输出。

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)

    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")

执行后会绘制避障可视化图，如图5-2所示。

图5-2  避障可视化图