A*算法初学者贴及python程序详解_python

提示：本人是第一次学习A*算法，记录自己的学习过程，捋清其原理步骤，并利用python做一个小例子实现。

A*算法的基本原理
- A*算法的应用场景
- A*算法的思想
- A*算法的定义
A*算法的路径规划步骤
- A*算法路径搜索的图示演示
A*算法的代码举例
- 1.定义一张定宽高的地图并设定起点和终点
- 2.开始路径规划
总结

A*算法的基本原理

将从应用场景、思想、基本的定义进行说明

A*算法的应用场景

一副地图中有坐标A和B，而A和B之间可能存在一些障碍，需要找到一条路径从A到B尽可能最短的安全路径。这样的问题就称作路径规划问题。
A*算法是处理路径规划问题的一种算法，其他的还有Dijkstra算法、贪心算法、广度优先（Bread First Search，BFS）和深度优先算法（Depth First Search，DFS）。BFS的思想就是以起点A为圆心，先搜索A周围的所有点，形成一个类似圆的搜索区域，再扩大搜索半径，直到终点B进入搜索区域内被找到；DFS的思想就是从起点A出发找沿着AB这条线的方向，并且离A点最远的点。
研究表明，A-star算法搜索效率较高。

A*算法的思想

A*算法利用估价的思想，其规划的过程如下：

在代遍历列表中（刚开始只有A点），我们在列表中查找一个估价（见估价公式）最小的点K
对点K进行一次广度优先搜索，也就是它移动到一次到的下一个坐标（上下左右、左上右上、左下右下）不包含已经遍历过的点和不能到达的点，将能查找的点添加到队列中，并将点K从队列中移除
重复1、2步骤直到到达B点，或者队列已经为空，表示没有路径可以到达点B

估价公式
F = G + H
G = 从起点A移动到下一移动位置的代价，沿着到达该位置而生成的路径。约定直行移动一次的代价是10，对角线的移动代价为14。
H = 从指定的位置移动到终点B的估价。计算从当前位置横向或者纵向移动到达终点所经过的距离（距离的计算见下面）乘10，忽略对角移动。

A*算法的定义

open list : 记录下所有被考虑来寻找最短路径的格子
close list : 记录下不会再被考虑的格子
Point（属性） : 是否为障碍物，父节点

用来计算代价的距离：欧拉距离和曼哈顿距离

欧拉距离：
(x₁-x₂)²+ (y₁-y₂)²
曼哈顿距离：
|x₁-x₂|+ |y₁-y₂|

A*算法的路径规划步骤

首先定义一张定宽高的地图（定义好Point点的地图，其中有Is_Wall属性，属性标记是否有障碍物）
设定好起始点和终点（起始点A，终点为B）
开始寻路
① 从起点A开始，把A作为一个等待检查的方格，放入到open list中（open list存放每一个等待检查的方格）
② 寻找起点A周围可以到达的方格（最多八个），将它们放入到open list，并设置它们的父节点为A
③ 从open list中删除起点A，并将A放入到close list中（close list存放的是不再需要检查的方格）
④ 计算open list中的方格的代价值（F）
⑤ 从open list中选择F最小的方格K，如果F一样，那么选择唯一一个G值最小的方格K，将其从open list中删除，放入close list中
⑥ 检查K所有邻近并且可达的方格
a）障碍物和close list中的方格不考虑
b）如果这些方格还不在open list里面，就将它们加入到open list，并且计算这些方格的F值，并设置父节点为K
c）如果K相邻的方格C已经在open list中，计算新的路径从A到方格C（即经过K的路径），判断是否需要更新：G值是否更低一点
如果新的G值更低，则修改父节点为方格K，重新计算F值，H值不需要改变，因为方格到达目标点的预计消耗是固定的。
如果新的G值比较高，则说明新的路径消耗更高，则值不做改变（G值不变也不更新）
⑦ 继续从open list中找出F值最小的，从open list中删除，添加到close list，再继续找出周围可以到达的方块，如此循环。
⑧ 结束判断：当open list中出现终点B时，说明路径已经找到；当open list中没有了数据，则说明没有合适路径。

A*算法路径搜索的图示演示

每一个方格的表示如下：

左上角代表F，即代价值；左下角代表G，右下角代表H，箭头指向当前节点的父节点
距离采用曼哈顿距离。

计算邻近点的代价，它们的父节点都是起点，找出最小估价，为右上角的方块。可以规定估价计算的顺序。
对上个步骤选出来的右上角的方块（紫色方块）的八个方向的坐标进行估值（已经估值的以及障碍不用计算），G值最小最后访问的点为父节点。
对上个步骤的紫色方块放入close list，即深绿色方块，重新寻找F值最小的方块。
重复上述步骤
循环结束
循环结束的条件就是，终点在open list中，即找到了终点或者open list中没有了值，这就说明没有找到路径，由此终点依次查找它们的父节点直到起点，将坐标点逆序，就是最短安全路径。

A*算法的代码举例

在了解了以上的原理与步骤后，开始组织程序

1.定义一张定宽高的地图并设定起点和终点

这个步骤要求定义一个定宽高的地图，并且在地图中要标明障碍物，利用一个二维数组来定义，并且需要定义是否有障碍
根据上面分析用的图，可以发现是一个10×10的方形区域
规定：

数字	0	1	3	4	6
含义	障碍物	可以通行	open list	close list	最后的路径

代码如下：

#coding=utf-8
import numpy as np
import pandas as pd
import matplotlib as mpl
import matplotlib.pyplot as plt

map_grid=[[1 for j in range(0,10)] for i in range(0,10)]  #定义列表
map_grid = np.array(map_grid)  #将列表转化为数组，因为只有数组才有维度的概念，方便切片
map_grid[4:8,4]=0   #障碍物
map_grid[2:4,6]=0
map_grid[4,2]=3     #起点
map_grid[6,8]=6     #终点
plt.imshow(map_grid,cmap=plt.cm.hot,interpolation='nearest',vmin=0,vmax=10)  #绘制热力图
plt.colorbar()
plt.xlim(-1,10)     #x轴的范围
plt.ylim(-1,10)
my_x_ticks = np.arange(0,11,1)   #x轴标号的范围
my_y_ticks = np.arange(0,11,1)
plt.xticks(my_x_ticks)
plt.yticks(my_y_ticks)
plt.grid(True)   #开启栅格  可以不开启
plt.show()  #可视化

效果：
与上述分析的地图一致

将其打包成一个函数

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

2.开始路径规划

在这个部分需要设置两个列表open_list，close_list，并且不断更新上面定义的二维数组中的值
①从起点A开始，把A作为一个等待检查的方格，放入open list中（open list存放每一个等待检查的方格）

open_list = [4,2]
G = 0
H = (abs(xn - x0)+abs(yn - y0))*10
F = G+H
open_list = {"x":4,"y":2,"Fvalue":F,"Gvalue":G,"Hvalue":H,"f_x":None,"f_y":None}
open_list = pd.DataFrame(open_list,index=[0])
#分别代表x,y,F值,G值,H值,父节点x,父节点y

②寻找起点A周围可以到达的方格，放入open_list中，并设置父节点，并计算F、G、H的值
，将此寻周围方格的过程打包为一个函数：如果节点K的邻节点在open_list，如果新的G值更小就更新G值，并重新计算F值，并更改K的父节点；否则不变

#②寻找起点A周围可以到达的方格，放入openlist中,并设置其父节点为A，并计算出F值(包装成一个函数来实现)
#close_list中不搜索，障碍不搜索，已经搜索过(在openlist里面)的不再搜索
def find_way(open_list,close_list,x0,y0):
    #定义一个搜索顺序，右->右上->上->左上->左->左下->下->右下
    m = close_list.iloc[close_list.index[(close_list['x'] == x0) & (close_list['y'] == y0)], 3].values.astype(int)
    if y0 + 1 < 10 and map_grid[x0, y0 + 1] != 0 and map_grid[x0, y0 + 1] !=4:
        x1 = x0
        y1 = y0 + 1
        G = 10 + m
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 + 1 < 10 and x0 + 1 < 10 and map_grid[x0 + 1, y0 + 1] != 0 and map_grid[x0+1, y0 + 1] !=4:
        G = 14 + m
        x1 = x0+1
        y1 = y0 + 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

        #pen_list.loc[len(open_list.index)] = [x1, y1, F, G, H, x0, y0]


    if x0 + 1 < 10 and map_grid[x0 + 1, y0] != 0 and map_grid[x0 + 1, y0] !=4:
        G = 10 + m
        x1 = x0+1
        y1 = y0
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if x0 + 1 < 10 and y0 -1 >=0 and map_grid[x0 + 1, y0 - 1] != 0 and map_grid[x0 + 1, y0 - 1] !=4:
        G = 14 + m
        x1 = x0+1
        y1 = y0-1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 - 1 >= 0 and map_grid[x0, y0 - 1] != 0 and map_grid[x0, y0 - 1] !=4:
        G = 10 + m
        x1 = x0
        y1 = y0 - 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 - 1 >= 0 and x0 - 1 >= 0  and map_grid[x0 - 1, y0 - 1] != 0 and map_grid[x0 - 1, y0 - 1] !=4:
        G = 14 + m
        x1 = x0-1
        y1 = y0 - 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if x0 - 1 >= 0 and map_grid[x0 - 1, y0] != 0 and map_grid[x0 - 1, y0] !=4:
        G = 10 + m
        x1 = x0-1
        y1 = y0
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 + 1 < 10 and x0 - 1 >=0 and map_grid[x0 - 1, y0 + 1] != 0 and map_grid[x0 - 1, y0 + 1] !=4:
        G = 14 + m
        x1 = x0-1
        y1 = y0 + 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    return open_list

③从open_list中删除起点A，并将A放入close_list中

#③从open list中删除起点A，并将A放入close list中(close list存放不需要再检查的方格)
insertRow = open_list.iloc[open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)]]
close_list = insertRow
close_list = pd.DataFrame(close_list)
# print(open_list)                        #此时openlist里面就有所有可以到达的方格
open_list = open_list.drop(open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)])  # 删除起点A
open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
map_grid[x0, y0] = 4  # close_list

open_list = find_way(open_list,close_list,x0,y0)                #调用该函数
open_list = pd.DataFrame(open_list)

④从open_list中选择F最小的方格K，如果F最小的方格不只一个，那么优先搜索G比较大的方格，将其从open_list中删除，放入close_list，如果G一样大，那么就只是搜索列表中的第一个。

#④从open_list中选择F最小的方格K，如果F最小的方格不只一个，那么优先搜索G比较大的方格，将其从open_list中删除，放入close_list
def move_min_to_close(open_list,close_list,x0,y0):
    insertRow = open_list.loc[open_list.index[(open_list['Fvalue'] == open_list['Fvalue'].min())]]
    insertRow = insertRow.loc[insertRow.index[(insertRow['Gvalue'] == insertRow['Gvalue'].min())]]
    insertRow = insertRow.loc[[insertRow.index[0]]]
    open_list = open_list.drop(insertRow.index)  # 删除起点K
    open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
    close_list = pd.concat([close_list, insertRow])
    close_list = close_list.reset_index(drop=True)  # 重新排列index(索引)
    insertRow = insertRow.reset_index(drop=True)  # 重新排列index(索引)
    x0 = insertRow.iloc[0, 0]
    y0 = insertRow.iloc[0, 1]
    return [open_list,close_list,x0,y0]

⑤循环上述步骤，找出F最小的，找出周围可以到达的方块，从open list中删除，添加到close list；
循环结束的条件:当open list中出现终点B时，说明路径已经找到；当open list中没有了数据，则说明没有合适路径

#⑤循环上述步骤，找出F最小的，找出周围可以到达的方块，从open list中删除，添加到close list
#循环结束的条件:当open list中出现终点B时，说明路径已经找到；当open list中没有了数据，则说明没有合适路径
while (0 not in open_list['Hvalue'].values.astype(int) and pd.isnull(open_list.loc[0,'x'])==False):

    [open_list, close_list,x0,y0] = move_min_to_close(open_list, close_list, x0, y0)
    map_grid[x0, y0] = 4  # close_list
    open_list = pd.DataFrame(open_list)
    close_list = pd.DataFrame(close_list)
    open_list = find_way(open_list, close_list, x0, y0)
    #print(x0,y0)

⑥总结路径，并绘制图
当open list中出现终点B时，说明路径已经找到，绘制热力图，并打印路径节点；
当open list中没有了数据，则说明没有合适路径，绘制热力图，并发送‘No way’

if 0 in open_list['Hvalue'].values.astype(int):
    print("find it")
    list = [[xn,yn]]
    [x,y] = open_list.loc[((open_list['x'].values == xn) & (open_list['y'].values == yn)), ['f_x','f_y']].values.astype(int)[0]
    list.append([x,y])
    map_grid[x, y] = 6  # 终点
    while close_list.loc[((close_list['x'].values == x) & (close_list['y'].values == y)), 'f_x'].values[0]!=None:
        [x,y] = close_list.loc[((close_list['x'].values == x) & (close_list['y'].values == y)), ['f_x','f_y']].values.astype(int)[0]
        map_grid[x, y] = 6  # 终点
        list.append([x,y])
    draw_effect(map_grid)
    print(list)

if(pd.isnull(open_list.loc[0,'x'])==True):
    print("No way")
    draw_effect(map_grid)

效果图：

总程序附录

#coding=utf-8
import numpy as np
import pandas as pd
import matplotlib as mpl
import matplotlib.pyplot as plt
import warnings

warnings.filterwarnings("ignore")

#1.建立一个地图，明确障碍物和能够同行的路和起点、终点
map_grid=[[1 for j in range(0,10)] for i in range(0,10)]  #定义列表
map_grid = np.array(map_grid)  #将列表转化为数组，因为只有数组才有维度的概念，方便切片
map_grid[4:8,4]=0   #障碍物
map_grid[2:4,6]=0
map_grid[4,2]=1     #起点
map_grid[6,8]=6     #终点
x0 = 4
y0 = 2
xn = 6
yn = 8
i=0
#建图完成，其中map_grid里面就是每个方格的属性
def draw_effect(map_grid):
    plt.imshow(map_grid, cmap=plt.cm.hot, interpolation='nearest', vmin=0, vmax=10)  # 绘制热力图
    plt.colorbar()
    plt.xlim(-1, 10)  # x轴的范围
    plt.ylim(-1, 10)
    my_x_ticks = np.arange(0, 11, 1)  # x轴标号的范围
    my_y_ticks = np.arange(0, 11, 1)
    plt.xticks(my_x_ticks)
    plt.yticks(my_y_ticks)
    plt.grid(True)  # 开启栅格  可以不开启
    plt.show()  # 可视化


#2.开始寻路

#①从起点A开始，把A作为一个等待检查的方格，放入open list中（存放每一个等待检查的方格）
open_list = [4,2]
G = 0
H = (abs(xn - x0)+abs(yn - y0))*10
F = G+H
open_list = {"x":4,"y":2,"Fvalue":F,"Gvalue":G,"Hvalue":H,"f_x":None,"f_y":None}
open_list = pd.DataFrame(open_list,index=[0])
#分别代表x,y,F值,G值,H值,父节点x,父节点y
#print(open_list.iloc[open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)], 3])


#②寻找起点A周围可以到达的方格，放入openlist中,并设置其父节点为A，并计算出F值(包装成一个函数来实现)
#close_list中不搜索，障碍不搜索，已经搜索过(在openlist里面)的不再搜索
def find_way(open_list,close_list,x0,y0):
    #定义一个搜索顺序，右->右上->上->左上->左->左下->下->右下
    m = close_list.iloc[close_list.index[(close_list['x'] == x0) & (close_list['y'] == y0)], 3].values.astype(int)
    if y0 + 1 < 10 and map_grid[x0, y0 + 1] != 0 and map_grid[x0, y0 + 1] !=4:
        x1 = x0
        y1 = y0 + 1
        G = 10 + m
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 + 1 < 10 and x0 + 1 < 10 and map_grid[x0 + 1, y0 + 1] != 0 and map_grid[x0+1, y0 + 1] !=4:
        G = 14 + m
        x1 = x0+1
        y1 = y0 + 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

        #pen_list.loc[len(open_list.index)] = [x1, y1, F, G, H, x0, y0]


    if x0 + 1 < 10 and map_grid[x0 + 1, y0] != 0 and map_grid[x0 + 1, y0] !=4:
        G = 10 + m
        x1 = x0+1
        y1 = y0
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if x0 + 1 < 10 and y0 -1 >=0 and map_grid[x0 + 1, y0 - 1] != 0 and map_grid[x0 + 1, y0 - 1] !=4:
        G = 14 + m
        x1 = x0+1
        y1 = y0-1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 - 1 >= 0 and map_grid[x0, y0 - 1] != 0 and map_grid[x0, y0 - 1] !=4:
        G = 10 + m
        x1 = x0
        y1 = y0 - 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 - 1 >= 0 and x0 - 1 >= 0  and map_grid[x0 - 1, y0 - 1] != 0 and map_grid[x0 - 1, y0 - 1] !=4:
        G = 14 + m
        x1 = x0-1
        y1 = y0 - 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if x0 - 1 >= 0 and map_grid[x0 - 1, y0] != 0 and map_grid[x0 - 1, y0] !=4:
        G = 10 + m
        x1 = x0-1
        y1 = y0
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    if y0 + 1 < 10 and x0 - 1 >=0 and map_grid[x0 - 1, y0 + 1] != 0 and map_grid[x0 - 1, y0 + 1] !=4:
        G = 14 + m
        x1 = x0-1
        y1 = y0 + 1
        H = (abs(xn - x1) + abs(yn - y1)) * 10
        F = G + H
        if map_grid[x1, y1] == 3:
            n = open_list.iloc[open_list.index[(open_list['x'] == x1) & (open_list['y'] == y1)], 3].values.astype(int)
            if G < n:
                # print(len(open_list.index))
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Gvalue'] = G
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'Fvalue'] = F
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = x0
                open_list.loc[(open_list['x'] == x1) & (open_list['y'] == y1), 'f_x'] = y0

        else:
            insertRow = {"x": x1, "y": y1, "Fvalue": F, "Gvalue": G, "Hvalue": H, "f_x": x0, "f_y": y0}
            insertRow = pd.DataFrame(insertRow, index=[0])
            # print(len(open_list.index))
            open_list = pd.concat([open_list, insertRow])
            open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
            # open_list.loc[len(open_list.index)]=[x1,y1,F,G,H,x0,y0]
            map_grid[x1, y1] = 3  # openlist里面的坐标所对应的值设置为3，方便绘图

    return open_list


#③从open list中删除起点A，并将A放入close list中(close list存放不需要再检查的方格)
insertRow = open_list.iloc[open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)]]
close_list = insertRow
close_list = pd.DataFrame(close_list)
# print(open_list)                        #此时openlist里面就有所有可以到达的方格
open_list = open_list.drop(open_list.index[(open_list['x'] == x0) & (open_list['y'] == y0)])  # 删除起点A
open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
map_grid[x0, y0] = 4  # close_list

open_list = find_way(open_list,close_list,x0,y0)                #调用该函数
open_list = pd.DataFrame(open_list)


#④从open_list中选择F最小的方格K，如果F最小的方格不只一个，那么优先搜索G比较大的方格，将其从open_list中删除，放入close_list
def move_min_to_close(open_list,close_list,x0,y0):
    insertRow = open_list.loc[open_list.index[(open_list['Fvalue'] == open_list['Fvalue'].min())]]
    insertRow = insertRow.loc[insertRow.index[(insertRow['Gvalue'] == insertRow['Gvalue'].min())]]
    insertRow = insertRow.loc[[insertRow.index[0]]]
    open_list = open_list.drop(insertRow.index)  # 删除起点K
    open_list = open_list.reset_index(drop=True)  # 重新排列index(索引)
    close_list = pd.concat([close_list, insertRow])
    close_list = close_list.reset_index(drop=True)  # 重新排列index(索引)
    insertRow = insertRow.reset_index(drop=True)  # 重新排列index(索引)
    x0 = insertRow.iloc[0, 0]
    y0 = insertRow.iloc[0, 1]
    return [open_list,close_list,x0,y0]

#print(open_list)
#⑤循环上述步骤，找出F最小的，找出周围可以到达的方块，从open list中删除，添加到close list
#循环结束的条件:当open list中出现终点B时，说明路径已经找到；当open list中没有了数据，则说明没有合适路径
while (0 not in open_list['Hvalue'].values.astype(int) and pd.isnull(open_list.loc[0,'x'])==False):

    [open_list, close_list,x0,y0] = move_min_to_close(open_list, close_list, x0, y0)
    map_grid[x0, y0] = 4  # close_list
    open_list = pd.DataFrame(open_list)
    close_list = pd.DataFrame(close_list)
    open_list = find_way(open_list, close_list, x0, y0)
    #print(x0,y0)

if 0 in open_list['Hvalue'].values.astype(int):
    print("find it")
    list = [[xn,yn]]
    [x,y] = open_list.loc[((open_list['x'].values == xn) & (open_list['y'].values == yn)), ['f_x','f_y']].values.astype(int)[0]
    list.append([x,y])
    map_grid[x, y] = 6  # 终点
    while close_list.loc[((close_list['x'].values == x) & (close_list['y'].values == y)), 'f_x'].values[0]!=None:
        [x,y] = close_list.loc[((close_list['x'].values == x) & (close_list['y'].values == y)), ['f_x','f_y']].values.astype(int)[0]
        map_grid[x, y] = 6  # 终点
        list.append([x,y])
    draw_effect(map_grid)
    print(list)

if(pd.isnull(open_list.loc[0,'x'])==True):
    print("No way")
    draw_effect(map_grid)

总结

程序是自己一句一句写的，没有参考别人的程序，看着有点复杂，其实只要花时间读一下代码，很容易理解，适合初学者。但总算把A*捋清楚了，也学会了python里面list，dataframe，array的索引和用法。
接下来准备写dijkstra算法。

参考文献：

python栅格地图上路径规划作图
A*算法（超级详细讲解，附有举例的详细手写步骤）
寻路算法——A*算法详解并附带实现代码
A*算法详解一看就懂（python）

欢迎分享，转载请注明来源：内存溢出

原文地址: http://outofmemory.cn/langs/735531.html

A*算法初学者贴及python程序详解

发表评论

评论列表（0条）