使用A*算法实现三维迷宫并进行可视化

# author:qjj  time: 2022/7/26
import tornado.web

# 所有节点的g值并没有初始化为无穷大
# 当两个子节点的f值一样时，程序选择最先搜索到的一个作为父节点加入closed
# 对相同数值的不同对待，导致不同版本的A*算法找到等长的不同路径
# 最后closed表中的节点很多，如何找出最优的一条路径
# 撞墙之后产生较多的节点会加入closed表，此时开始删除closed表中不合理的节点

import numpy
from pylab import *
import copy
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D

# 定义一个含有障碍物的100×100的栅格地图
# 10表示可通行点
# 0表示障碍物
# 7表示起点
# 5表示终点

map_grid = numpy.full((100, 100, 100), int(10), dtype=numpy.int8)
map_grid[2:12, 3:8, 5:9] = 0
map_grid[5:8, 4:10, 13:16] = 0
map_grid[3:10, 7:9, 6:8] = 0
map_grid[10:14, 3:8, 8:10] = 0
map_grid[17:19, 13:17, 3:14] = 0
map_grid[10:17, 3:10, 13:19] = 0
map_grid[10:15, 8:16, 13:17] = 0
map_grid[2, 2, 2] = 7
map_grid[99, 99, 99] = 5

def min_index(data): # 寻找最小值的所有索引
    index = []  # 创建列表,存放最小值的索引
    dim_1 = data.ravel()  # 转为一维数组
    min_n = min(dim_1)  # 最大值max_n
    for i in range(len(dim_1)):
        if dim_1[i] == min_n: 
            pos = np.unravel_index(i, data.shape, order='C') 
            index.append(pos)  # 存入index
    return np.array(index)

def select_index(data, value): # 寻找指定值的所有索引
    index = []  # 创建列表,存放最大值的索引
    dim_1 = data.ravel()  # 转为一维数组
    for i in range(len(dim_1)):
        if dim_1[i] == value:  # 遍历寻找最大值，并全部索引值进行操作
            pos = np.unravel_index(i, data.shape, order='C')  # 返回一维索引对应的多维索引，譬如4把转为(1,1),即一维中的第5个元素对应在二维里的位置是2行2列
            index.append(pos)  # 存入index
    return np.array(index)

class AStar(object):
    def __init__(self):
        # self.g = 0  # g初始化为0
        self.start = numpy.array([2, 2, 2])  # 起点坐标
        self.goal = numpy.array([99, 99, 99])  # 终点坐标
        self.open = numpy.array([[], [], [], [], [], [], [], []])  # 先创建一个空的open表, 记录坐标，方向，g值，f值
        self.closed = numpy.array([[], [], [], [], [], [], [], []])  # 先创建一个空的closed表
        self.best_path_array = numpy.array([[], [], []])  # 回溯路径表

    def h_value_tem(self, son_p):
        """
        计算拓展节点和终点的h值
        :param son_p: 子搜索节点坐标
        :return:
        """
        h = (son_p[0] - self.goal[0]) ** 2 + (son_p[1] - self.goal[1]) ** 2 + (son_p[2] - self.goal[2]) ** 2
        h = numpy.sqrt(h)  # 计算h
        return h

    # def g_value_tem(self, son_p, father_p):
    #     """
    #     计算拓展节点和父节点的g值
    #     其实也可以直接用1或者1.414代替
    #     :param son_p:子节点坐标
    #     :param father_p:父节点坐标，也就是self.current_point
    #     :return:返回子节点到父节点的g值，但不是全局g值
    #     """
    #     g1 = father_p[0] - son_p[0]
    #     g2 = father_p[1] - son_p[1]
    #     g = g1 ** 2 + g2 ** 2
    #     g = numpy.sqrt(g)
    #     return g

    def g_accumulation(self, son_point, father_point):
        """
        累计的g值
        :return:
        """
        g1 = father_point[0] - son_point[0]
        g2 = father_point[1] - son_point[1]
        g3 = father_point[2] - son_point[2]
        g = g1 ** 2 + g2 ** 2 + g3 ** 2
        g = numpy.sqrt(g) + father_point[-2]  # 加上累计的g值
        return g

    def f_value_tem(self, son_p, father_p):
        """
        求出的是临时g值和h值加上累计g值得到全局f值
        :param father_p: 父节点坐标
        :param son_p: 子节点坐标
        :return:f
        """
        f = self.g_accumulation(son_p, father_p) + self.h_value_tem(son_p)
        return f

    def child_point(self, x):
        """
        拓展的子节点坐标
        :param x: 父节点坐标
        :return: 子节点存入open表，返回值是每一次拓展出的子节点数目，用于撞墙判断
        当搜索的节点撞墙后，如果不加处理，会陷入死循环
        """
        # 开始遍历周围26个节点
        for j in range(-1, 2, 1):
            for q in range(-1, 2, 1):
                for p in range(-1, 2, 1):
                    if j == 0 and q == 0 and p ==0:  # 搜索到父节点去掉
                        continue
                    m = [x[0] + j, x[1] + q, x[2] + p]
                    # print(m)
                    if m[0] < 0 or m[0] > 99 or m[1] < 0 or m[1] > 99 or m[2] < 0 or m[2] > 99:  # 搜索点出了边界去掉
                        continue

                    if map_grid[int(m[0]), int(m[1]), int(m[2])] == 0:  # 搜索到障碍物去掉
                        continue

                    record_g = self.g_accumulation(m, x)
                    record_f = self.f_value_tem(m, x)  # 计算每一个节点的f值

                    x_direction, y_direction, z_direction = self.direction(x, m)  # 每产生一个子节点，记录一次方向

                    para = [m[0], m[1], m[2], x_direction, y_direction, z_direction, record_g, record_f]  # 将参数汇总一下
                    # print(para)

                    # 在open表中，则去掉搜索点，但是需要更新方向指针和self.g值
                    # 而且只需要计算并更新self.g即可，此时建立一个比较g值的函数
                    a, index = self.judge_location(m, self.open)
                    if a == 1:
                        # 说明open中已经存在这个点
                        if record_f <= self.open[7][index]:
                            self.open[7][index] = record_f
                            self.open[6][index] = record_g
                            self.open[5][index] = z_direction
                            self.open[4][index] = y_direction
                            self.open[3][index] = x_direction

                        continue

                    # 在closed表中,则去掉搜索点
                    b, index2 = self.judge_location(m, self.closed)
                    if b == 1:

                        if record_f <= self.closed[7][index2]:
                            self.open[7][index2] = record_f
                            self.open[6][index2] = record_g
                            self.open[5][index2] = z_direction
                            self.open[4][index2] = y_direction
                            self.open[3][index2] = x_direction
                            self.closed = numpy.delete(self.closed, index2, axis=1)
                            self.open = numpy.c_[self.open, para]
                        continue

                    self.open = numpy.c_[self.open, para]  # 参数添加到open中
                    # print(self.open)

    def judge_location(self, m, list_co):
        """
        判断拓展点是否在open表或者closed表中
        :return:返回判断是否存在，和如果存在，那么存在的位置索引
        """
        jud = 0
        index = 0
        for i in range(list_co.shape[1]):

            if m[0] == list_co[0, i] and m[1] == list_co[1, i] and m[2] == list_co[2, i]:

                jud = jud + 1
                index = i
                break
            else:
                jud = jud
        # if a != 0:
        #     continue
        return jud, index

    def direction(self, father_point, son_point):
        """
        建立每一个节点的方向，便于在closed表中选出最佳路径
        非常重要的一步，不然画出的图像参考1.1版本
        x记录子节点和父节点的x轴变化
        y记录子节点和父节点的y轴变化
        如（0，1）表示子节点在父节点的方向上变化0和1
        :return:
        """
        x = son_point[0] - father_point[0]
        y = son_point[1] - father_point[1]
        z = son_point[2] - father_point[2]
        return x, y, z

    def path_backtrace(self):
        """
        回溯closed表中的最短路径
        :return:
        """
        best_path = list(self.goal)  # 回溯路径的初始化
        self.best_path_array = numpy.array([[99], [99], [99]])
        j = 0
        close = self.closed
        print(close)
        while j <= self.closed.shape[1]:
            for i in range(self.closed.shape[1]):
                if best_path[0] == self.closed[0][i] and best_path[1] == self.closed[1][i] and best_path[2] == self.closed[2][i]:
                    x = self.closed[0][i] - self.closed[3][i]
                    y = self.closed[1][i] - self.closed[4][i]
                    z = self.closed[2][i] - self.closed[5][i]
                    best_path = [x, y, z]
                    self.best_path_array = numpy.c_[self.best_path_array, best_path]
                    break  # 如果已经找到，退出本轮循环，减少耗时
                else:
                    continue
            j = j + 1
        return self.best_path_array

    # def save(self):


    def main(self):
        """
        main函数
        :return:
        """
        best = self.start  # 起点放入当前点，作为父节点
        h0 = self.h_value_tem(best)
        init_open = [best[0], best[1], best[2], 0, 0, 0, 0, h0]  # 将方向初始化为（0，0），g_init=0,f值初始化h0
        self.open = numpy.column_stack((self.open, init_open))  # 起点放入open, open初始化（column_stack将两个矩阵按列合并

        ite = 1  # 设置迭代次数小于200，防止程序出错无限循环
        while ite <= 1000000:
            # open列表为空，退出
            if self.open.shape[1] == 0:
                print('没有搜索到路径！')
                return

            self.open = self.open.T[numpy.lexsort(self.open)].T  # open表中最后一行排序(联合排序

            # 选取open表中最小f值的节点作为best，放入closed表
            best = self.open[:, 0]
            print('检验第%s次当前点坐标*******************' % ite)
            print(best)
            self.closed = numpy.c_[self.closed, best]  #（numpy.c_将切片对象沿第二个轴（按列）连接。

            if best[0] == 99 and best[1] == 99 and best[2] == 99:  # 如果best是目标点，退出
                print('搜索成功！')
                return

            self.child_point(best)  # 生成子节点并判断数目
            # print(self.open)
            self.open = numpy.delete(self.open, 0, axis=1)  # 删除open中最优点

            # print(self.open)

            ite = ite + 1

def draw_map(a):
    fig = plt.figure()
    ax = Axes3D(fig)
    ob_index = min_index(map_grid)
    ob_x = ob_index[:,0]
    ob_y = ob_index[:,1]
    ob_z = ob_index[:,2]
    # plt.show()
    ax.scatter(ob_x, ob_y, ob_z, s=30, c='b')
    ax.scatter(2, 2, 2, s=30, c='r')
    ax.scatter(99, 99, 99, s=30, c='r')
    print('打印direction长度：')
    print(a.best_path_array.shape[1])
    best_index = a.best_path_array

    best_x = best_index[0,:]
    best_y = best_index[1,:]
    best_z = best_index[2,:]
    close = a.closed
    print(best_x)
    ax.scatter(best_x, best_y, best_z, s=30, c='r')
    print(close)

    plt.plot(best_x, best_y, best_z, lw=5, c='r')
    ax.set_zlabel('Z', fontdict={
    
    'size': 15, 'color': 'red'})
    ax.set_ylabel('Y', fontdict={
    
    'size': 15, 'color': 'red'})
    ax.set_xlabel('X', fontdict={
    
    'size': 15, 'color': 'red'})
    # map_direction = copy.deepcopy(map_grid)
    # for i in range(a.best_path_array.shape[1]):
    #     x = a.best_path_array[:, i]
    #     map_direction[int(x[0]), int(x[1])] = 6
    plt.show()

if __name__ == '__main__':
    a1 = AStar()
    a1.main()
    a1.path_backtrace()
    # a1.save()
    draw_map(a1)