导航算法A*的简单实现

• 开发语言：python3

• 开发工具：VScode

• 参考博客：https://blog.csdn.net/weixin_44489823/article/details/89382502

---

1、 部分说明

代码中，保存在GInfo中的F、G，H分别对应于如下估计函数：
$$F=G+H^{\ast }.$$
F： 估计函数值。
G： 起点到当前点的实际距离。
H： 终点到当前点的估计值（本文采取欧式距离进行估算）。

2、运行结果

从左侧底部走到右上方（1、不走对角线 2、可走对角线）

3、代码

import matplotlib.pyplot as plt
import numpy as np
from MapGenerate import MapGenerate as mg
import math
import operator

# 地图范围
r = 50
# 随机种子
seed = 25
# 障碍物密度
density = .3
G = mg.plotMap(seed, r, density)
# 绘制
# plt.imshow(G, cmap='Greys',  interpolation='nearest')
# plt.show()
# 起点/终点
start = (r-1, 0)
end = (0, r-1)
# 像素色深
G[start[0]][start[1]] = 0.2
G[end[0]][end[1]] = 0.5

# 像素距离信息
GInfo = {}
for i in np.arange(0, r, 1):
    for j in np.arange(0, r, 1):
        GInfo[i, j] = {'F': math.inf,
                       'G': math.inf,
                       'H': ((i - end[0])**2 + (j - end[1])**2) ** 0.5,
                       'P': (i, j)
                       }
GInfo[start[0], start[1]].update({'F': GInfo[start[0], start[1]].get('H'), 'G': 0, 'P': (start[0], start[1])})
# print(len(GInfo.keys()))

closeList = []
openList = []
openList.append((start[0], start[1]))
print(openList)

path = []
while True:
    # 找不到出口
    if len(openList) == 0:
        gInfoKey = GInfo.get(closeList.pop(len(closeList)-1)).get('P')
        mg.savePath(start, gInfoKey, GInfo, path)
        break
    index = mg.selectMinOne(openList, GInfo)
    gInfoKey = openList[index]
    openList.remove(gInfoKey)
    closeList.append(gInfoKey)
    if operator.eq(end, gInfoKey):
        gInfoKey = GInfo.get(gInfoKey).get('P')
        mg.savePath(start, gInfoKey, GInfo, path)
        break
    # 更新邻居状态--不可走对角
    # mg.updateNeighborInfo2(gInfoKey, GInfo, openList, closeList, G, r)
    # 更新邻居节点信息--可以走对角
    mg.updateNeighborInfo(gInfoKey, GInfo, openList, closeList, G, r)


# 路径上色
if len(path) > 0:
    step = 0.3/len(path)
    c = 0.5
    for gInfoKey in path:
        c = c - step
        G[gInfoKey[0]][gInfoKey[1]] = c


# 绘制
plt.imshow(G, cmap='Greys',  interpolation='nearest')
plt.show()

# import matplotlib.pyplot as plt
import numpy as np
import math
import operator


class MapGenerate:

    def plotMap(seedNum, randomRange, offset):
        np.random.seed(seedNum)
        g = np.floor(np.random.random((randomRange, randomRange)) + offset)
        # plt.imshow(g, cmap='Greys',  interpolation='nearest')
        # plt.savefig('blkwht.png')
        # plt.show()
        return g

    # gg = plotMap(10, 10, .2)
    # print(gg)

    # 筛选估值函数最小像素点
    def selectMinOne(openList, GInfo):
        if len(openList) == 1:
            return 0
        min = math.inf
        temp = 0
        for i in np.arange(0, len(openList), 1):
            if GInfo.get(openList[i]).get('F') < min:
                min = GInfo.get(openList[i]).get('F')
                temp = i
        return temp

    # 保存路径
    def savePath(start, gInfoKey, GInfo, path):
        while not operator.eq(start, gInfoKey):
            # 保存路径
            path.append(gInfoKey)
            gInfoKey = GInfo.get(gInfoKey).get('P')

    # 更新邻居节点信息--可以走对角
    def updateNeighborInfo(gInfoKey, GInfo, openList, closeList, GG, r):
        ops = {0: operator.mul, 1: operator.add, 2: operator.sub}
        for i in np.arange(0, 3, 1):
            for j in np.arange(0, 3, 1):
                x = ops[i](gInfoKey[0], 1)
                y = ops[j](gInfoKey[1], 1)
                if x < 0 or x > r-1 or y < 0 or y > r-1 or ((x, y) in closeList) or (GG[x, y] == 1.0):
                    continue
                else:
                    G = math.inf
                    if (x > gInfoKey[0] and y > gInfoKey[1]) or (x < gInfoKey[0] and y < gInfoKey[1]):
                        G = GInfo.get(gInfoKey).get('G') + 1.4
                    else:
                        G = GInfo.get(gInfoKey).get('G') + 1
                    # print(x, y)
                    if G < GInfo[x, y].get('G'):
                        GInfo[x, y].update({'F': GInfo[x, y].get('H') + G, 'G': G, 'P': gInfoKey})
                        if (x, y) not in openList:
                            openList.append((x, y))

    # 更新邻居节点信息--不可跨越对角
    def updateNeighborInfo2(gInfoKey, GInfo, openList, closeList, GG, r):
        ops = {0: operator.mul, 1: operator.add, 2: operator.sub}
        for i in np.arange(0, 3, 1):
            for j in np.arange(0, 3, 1):
                x = ops[i](gInfoKey[0], 1)
                y = ops[j](gInfoKey[1], 1)
                if x < 0 or x > r-1 or y < 0 or y > r-1 or ((x, y) in closeList) or (GG[x, y] == 1.0) or (i > 0 and j > 0):
                    continue
                else:
                    G = GInfo.get(gInfoKey).get('G') + 1
                    # print(x, y)
                    if G < GInfo[x, y].get('G'):
                        GInfo[x, y].update({'F': GInfo[x, y].get('H') + G, 'G': G, 'P': gInfoKey})
                        if (x, y) not in openList:
                            openList.append((x, y))