导航算法A*的简单实现

开发语言：python3
开发工具：VScode
参考博客：https://blog.csdn.net/weixin_44489823/article/details/89382502

1、部分说明

代码中，保存在GInfo中的F、G，H分别对应于如下估计函数：
F=G+H∗.F = G+H^*\,. F=G+H∗.
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)breakindex = 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.5for gInfoKey in path:c = c - stepG[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 operatorclass 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 0min = math.inftemp = 0for i in np.arange(0, len(openList), 1):if GInfo.get(openList[i]).get('F') < min:min = GInfo.get(openList[i]).get('F')temp = ireturn 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):continueelse:G = math.infif (x > gInfoKey[0] and y > gInfoKey[1]) or (x < gInfoKey[0] and y < gInfoKey[1]):G = GInfo.get(gInfoKey).get('G') + 1.4else: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):continueelse: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))

导航算法A*的简单实现相关推荐

零起点学算法03——超简单模仿算术题
零起点学算法03--超简单模仿算术题 Description 计算机计算能力很强哦. 让计算机帮你算其实也很容易,不过要学好计算机语言哦. 下面请看一个例子:要求计算机计算1+2的值并输出. 这 ...
零起点学算法02——输出简单的句子
零起点学算法02--输出简单的句子 Description 会输出Hello World!了,那换个句子也会吧? Input 没有输入 Output 现在要求你输出下面红色的字 Nice to me ...
算法一之简单选择排序
一. 选择排序的思想选择排序的基本思想是:每一趟在n-i+1(i=1,2,-n-1)个记录中选取关键字最小的记录作为有序序列中第i个记录.基于此思想的算法主要有简单选择排序.树型选择排序和堆排序. ...
【简单排序算法】：简单选择排序、直接插入排序和冒泡排序
[简单排序算法]:简单选择排序.直接插入排序和冒泡排序简单选择排序: 原理:设所排序序列的记录个数为n.i取1,2,-,n-1,每次从所有n-i+1个记录(Ri,Ri+1,-,Rn)中找出最小的记录 ...
鱼眼校正c语言算法,一种简单而精确的鱼眼图像校正算法研究
舒旭摘要: 针对鱼眼图像的校正提出了一种有效区域提取算法,并在鱼眼图像的球面物投影平面展开时与目标半立方体的校正平面建立线性映射.实验结果表明,该算法能有效地提取鱼眼图像轮廓的有效区域,同时半立方 ...
简析穷举算法，及其简单应用
简析穷举算法,及其简单应用穷举概述穷举法又称列举法,其基本思想是逐一列举问题所涉及的所有情况. 穷举法常用于解决"是否存在"或"有多少种可能"等问题. 应用 ...
ros之模拟导航算法
导航算法在move_base节点中,当修改一些规划路线的参数时,又不想每次实际去跑,又想有定量的分析. 就可以用rosbag方式. 实现步骤: 1.记录log数据 rosbag recoved -O ...
九天创新智能科技有限公司诚招--SLAM/规划/控制/感知/定位/导航算法工程师
公司团队:海归高层次人才团队创办,拥有国际领先的全栈式动驾驶核心技术.创始团队来自新加坡国立大学.新加坡南洋理工大学.中国科学技术大学等世界一流学府. 项目已经是面向园区---专注智慧清洁领域一.控 ...
算法其实很简单—克鲁斯卡尔算法
目录 1. 克鲁斯卡尔算法介绍 2. 公交站问题 2.1 克鲁斯卡尔算法图解 2.2 克鲁斯卡尔算法分析 2.3 如何判断是否构成回路 3. 代码实现 1. 克鲁斯卡尔算法介绍 1)克鲁斯卡尔(Kru ...

导航算法A*的简单实现

开发语言：python3

开发工具：VScode

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

1、部分说明

2、运行结果

3、代码

导航算法A*的简单实现相关推荐

最新文章

热门文章

导航算法A*的简单实现

开发语言：python3

开发工具：VScode

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

1、 部分说明

2、运行结果

3、代码

导航算法A*的简单实现相关推荐

最新文章

热门文章

1、部分说明