python實現A*尋路算法

Posted on 2021-06-12 by WalkonNet

A* 算法簡介

A* 算法需要維護兩個數據結構：OPEN 集和 CLOSED 集。OPEN 集包含所有已搜索到的待檢測節點。初始狀態，OPEN集僅包含一個元素：開始節點。CLOSED集包含已檢測的節點。初始狀態，CLOSED集為空。每個節點還包含一個指向父節點的指針，以確定追蹤關系。

A* 算法會給每個搜索到的節點計算一個G+H 的和值F：

F = G + H
G：是從開始節點到當前節點的移動量。假設開始節點到相鄰節點的移動量為1，該值會隨著離開始點越來越遠而增大。
H：是從當前節點到目標節點的移動量估算值。
- 如果允許向4鄰域的移動，使用曼哈頓距離。
- 如果允許向8鄰域的移動，使用對角線距離。

算法有一個主循環，重復下面步驟直到到達目標節點：
1 每次從OPEN集中取一個最優節點n（即F值最小的節點）來檢測。
2 將節點n從OPEN集中移除，然後添加到CLOSED集中。
3 如果n是目標節點，那麼算法結束。
4 否則嘗試添加節點n的所有鄰節點n’。

鄰節點在CLOSED集中，表示它已被檢測過，則無需再添加。
鄰節點在OPEN集中：
- 如果重新計算的G值比鄰節點保存的G值更小，則需要更新這個鄰節點的G值和F值，以及父節點；
- 否則不做操作
否則將該鄰節點加入OPEN集，設置其父節點為n，並設置它的G值和F值。

有一點需要註意，如果開始節點到目標節點實際是不連通的，即無法從開始節點移動到目標節點，那算法在第1步判斷獲取到的節點n為空，就會退出

關鍵代碼介紹

保存基本信息的地圖類

地圖類用於隨機生成一個供尋路算法工作的基礎地圖信息

先創建一個map類，初始化參數設置地圖的長度和寬度，並設置保存地圖信息的二維數據map的值為0，值為0表示能移動到該節點。

class Map():
	def __init__(self, width, height):
		self.width = width
		self.height = height
		self.map = [[0 for x in range(self.width)] for y in range(self.height)]

在map類中添加一個創建不能通過節點的函數，節點值為1表示不能移動到該節點。

	def createBlock(self, block_num):
		for i in range(block_num):
			x, y = (randint(0, self.width-1), randint(0, self.height-1))
			self.map[y][x] = 1

在map類中添加一個顯示地圖的函數，可以看到，這邊隻是簡單的打印出所有節點的值，值為0或1的意思上面已經說明，在後面顯示尋路算法結果時，會使用到值2，表示一條從開始節點到目標節點的路徑。

	def showMap(self):
		print("+" * (3 * self.width + 2))
		for row in self.map:
			s = '+'
			for entry in row:
				s += ' ' + str(entry) + ' '
			s += '+'
			print(s)
		print("+" * (3 * self.width + 2))

添加一個隨機獲取可移動節點的函數

	def generatePos(self, rangeX, rangeY):
		x, y = (randint(rangeX[0], rangeX[1]), randint(rangeY[0], rangeY[1]))
		while self.map[y][x] == 1:
			x, y = (randint(rangeX[0], rangeX[1]), randint(rangeY[0], rangeY[1]))
		return (x , y)

搜索到的節點類

每一個搜索到將到添加到OPEN集的節點，都會創建一個下面的節點類，保存有entry的位置信息（x，y），計算得到的G值和F值，和該節點的父節點（pre_entry）。

class SearchEntry():
	def __init__(self, x, y, g_cost, f_cost=0, pre_entry=None):
		self.x = x
		self.y = y
		# cost move form start entry to this entry
		self.g_cost = g_cost
		self.f_cost = f_cost
		self.pre_entry = pre_entry
	
	def getPos(self):
		return (self.x, self.y)

算法主函數介紹

下面就是上面算法主循環介紹的代碼實現，OPEN集和CLOSED集的數據結構使用瞭字典，在一般情況下，查找，添加和刪除節點的時間復雜度為O(1), 遍歷的時間復雜度為O(n), n為字典中對象數目。

def AStarSearch(map, source, dest):
	...
	openlist = {}
	closedlist = {}
	location = SearchEntry(source[0], source[1], 0.0)
	dest = SearchEntry(dest[0], dest[1], 0.0)
	openlist[source] = location
	while True:
		location = getFastPosition(openlist)
		if location is None:
			# not found valid path
			print("can't find valid path")
			break;
		
		if location.x == dest.x and location.y == dest.y:
			break
		
		closedlist[location.getPos()] = location
		openlist.pop(location.getPos())
		addAdjacentPositions(map, location, dest, openlist, closedlist)
	
	#mark the found path at the map
	while location is not None:
		map.map[location.y][location.x] = 2
		location = location.pre_entry

我們按照算法主循環的實現來一個個講解用到的函數。
下面函數就是從OPEN集中獲取一個F值最小的節點，如果OPEN集會空，則返回None。

	# find a least cost position in openlist, return None if openlist is empty
	def getFastPosition(openlist):
		fast = None
		for entry in openlist.values():
			if fast is None:
				fast = entry
			elif fast.f_cost > entry.f_cost:
				fast = entry
		return fast

addAdjacentPositions 函數對應算法主函數循環介紹中的嘗試添加節點n的所有鄰節點n’。

	# add available adjacent positions
	def addAdjacentPositions(map, location, dest, openlist, closedlist):
		poslist = getPositions(map, location)
		for pos in poslist:
			# if position is already in closedlist, do nothing
			if isInList(closedlist, pos) is None:
				findEntry = isInList(openlist, pos)
				h_cost = calHeuristic(pos, dest)
				g_cost = location.g_cost + getMoveCost(location, pos)
				if findEntry is None :
					# if position is not in openlist, add it to openlist
					openlist[pos] = SearchEntry(pos[0], pos[1], g_cost, g_cost+h_cost, location)
				elif findEntry.g_cost > g_cost:
					# if position is in openlist and cost is larger than current one,
					# then update cost and previous position
					findEntry.g_cost = g_cost
					findEntry.f_cost = g_cost + h_cost
					findEntry.pre_entry = location

getPositions 函數獲取到所有能夠移動的節點，這裡提供瞭2種移動的方式：

允許上，下，左，右 4鄰域的移動
允許上，下，左，右，左上，右上，左下，右下 8鄰域的移動

	def getNewPosition(map, locatioin, offset):
		x,y = (location.x + offset[0], location.y + offset[1])
		if x < 0 or x >= map.width or y < 0 or y >= map.height or map.map[y][x] == 1:
			return None
		return (x, y)
		
	def getPositions(map, location):
		# use four ways or eight ways to move
		offsets = [(-1,0), (0, -1), (1, 0), (0, 1)]
		#offsets = [(-1,0), (0, -1), (1, 0), (0, 1), (-1,-1), (1, -1), (-1, 1), (1, 1)]
		poslist = []
		for offset in offsets:
			pos = getNewPosition(map, location, offset)
			if pos is not None:			
				poslist.append(pos)
		return poslist

isInList 函數判斷節點是否在OPEN集或CLOSED集中

	# check if the position is in list
	def isInList(list, pos):
		if pos in list:
			return list[pos]
		return None

calHeuristic 函數簡單得使用瞭曼哈頓距離，這個後續可以進行優化。
getMoveCost 函數根據是否是斜向移動來計算消耗（斜向就是2的開根號，約等於1.4）

	# imporve the heuristic distance more precisely in future
	def calHeuristic(pos, dest):
		return abs(dest.x - pos[0]) + abs(dest.y - pos[1])
		
	def getMoveCost(location, pos):
		if location.x != pos[0] and location.y != pos[1]:
			return 1.4
		else:
			return 1

代碼的初始化

可以調整地圖的長度，寬度和不可移動節點的數目。
可以調整開始節點和目標節點的取值范圍。

WIDTH = 10
HEIGHT = 10
BLOCK_NUM = 15
map = Map(WIDTH, HEIGHT)
map.createBlock(BLOCK_NUM)
map.showMap()

source = map.generatePos((0,WIDTH//3),(0,HEIGHT//3))
dest = map.generatePos((WIDTH//2,WIDTH-1),(HEIGHT//2,HEIGHT-1))
print("source:", source)
print("dest:", dest)
AStarSearch(map, source, dest)
map.showMap()

執行的效果圖如下，第一個表示隨機生成的地圖，值為1的節點表示不能移動到該節點。
第二個圖中值為2的節點表示找到的路徑。

在這裡插入圖片描述

完整代碼

使用python3.7編譯

from random import randint

class SearchEntry():
	def __init__(self, x, y, g_cost, f_cost=0, pre_entry=None):
		self.x = x
		self.y = y
		# cost move form start entry to this entry
		self.g_cost = g_cost
		self.f_cost = f_cost
		self.pre_entry = pre_entry
	
	def getPos(self):
		return (self.x, self.y)

class Map():
	def __init__(self, width, height):
		self.width = width
		self.height = height
		self.map = [[0 for x in range(self.width)] for y in range(self.height)]
	
	def createBlock(self, block_num):
		for i in range(block_num):
			x, y = (randint(0, self.width-1), randint(0, self.height-1))
			self.map[y][x] = 1
	
	def generatePos(self, rangeX, rangeY):
		x, y = (randint(rangeX[0], rangeX[1]), randint(rangeY[0], rangeY[1]))
		while self.map[y][x] == 1:
			x, y = (randint(rangeX[0], rangeX[1]), randint(rangeY[0], rangeY[1]))
		return (x , y)

	def showMap(self):
		print("+" * (3 * self.width + 2))

		for row in self.map:
			s = '+'
			for entry in row:
				s += ' ' + str(entry) + ' '
			s += '+'
			print(s)

		print("+" * (3 * self.width + 2))
	

def AStarSearch(map, source, dest):
	def getNewPosition(map, locatioin, offset):
		x,y = (location.x + offset[0], location.y + offset[1])
		if x < 0 or x >= map.width or y < 0 or y >= map.height or map.map[y][x] == 1:
			return None
		return (x, y)
		
	def getPositions(map, location):
		# use four ways or eight ways to move
		offsets = [(-1,0), (0, -1), (1, 0), (0, 1)]
		#offsets = [(-1,0), (0, -1), (1, 0), (0, 1), (-1,-1), (1, -1), (-1, 1), (1, 1)]
		poslist = []
		for offset in offsets:
			pos = getNewPosition(map, location, offset)
			if pos is not None:			
				poslist.append(pos)
		return poslist
	
	# imporve the heuristic distance more precisely in future
	def calHeuristic(pos, dest):
		return abs(dest.x - pos[0]) + abs(dest.y - pos[1])
		
	def getMoveCost(location, pos):
		if location.x != pos[0] and location.y != pos[1]:
			return 1.4
		else:
			return 1

	# check if the position is in list
	def isInList(list, pos):
		if pos in list:
			return list[pos]
		return None
	
	# add available adjacent positions
	def addAdjacentPositions(map, location, dest, openlist, closedlist):
		poslist = getPositions(map, location)
		for pos in poslist:
			# if position is already in closedlist, do nothing
			if isInList(closedlist, pos) is None:
				findEntry = isInList(openlist, pos)
				h_cost = calHeuristic(pos, dest)
				g_cost = location.g_cost + getMoveCost(location, pos)
				if findEntry is None :
					# if position is not in openlist, add it to openlist
					openlist[pos] = SearchEntry(pos[0], pos[1], g_cost, g_cost+h_cost, location)
				elif findEntry.g_cost > g_cost:
					# if position is in openlist and cost is larger than current one,
					# then update cost and previous position
					findEntry.g_cost = g_cost
					findEntry.f_cost = g_cost + h_cost
					findEntry.pre_entry = location
	
	# find a least cost position in openlist, return None if openlist is empty
	def getFastPosition(openlist):
		fast = None
		for entry in openlist.values():
			if fast is None:
				fast = entry
			elif fast.f_cost > entry.f_cost:
				fast = entry
		return fast

	openlist = {}
	closedlist = {}
	location = SearchEntry(source[0], source[1], 0.0)
	dest = SearchEntry(dest[0], dest[1], 0.0)
	openlist[source] = location
	while True:
		location = getFastPosition(openlist)
		if location is None:
			# not found valid path
			print("can't find valid path")
			break;
		
		if location.x == dest.x and location.y == dest.y:
			break
		
		closedlist[location.getPos()] = location
		openlist.pop(location.getPos())
		addAdjacentPositions(map, location, dest, openlist, closedlist)
		
	#mark the found path at the map
	while location is not None:
		map.map[location.y][location.x] = 2
		location = location.pre_entry	

	
WIDTH = 10
HEIGHT = 10
BLOCK_NUM = 15
map = Map(WIDTH, HEIGHT)
map.createBlock(BLOCK_NUM)
map.showMap()

source = map.generatePos((0,WIDTH//3),(0,HEIGHT//3))
dest = map.generatePos((WIDTH//2,WIDTH-1),(HEIGHT//2,HEIGHT-1))
print("source:", source)
print("dest:", dest)
AStarSearch(map, source, dest)
map.showMap()

到此這篇關於python實現A*尋路算法的文章就介紹到這瞭,更多相關python A*尋路算法內容請搜索WalkonNet以前的文章或繼續瀏覽下面的相關文章希望大傢以後多多支持WalkonNet！

python實現A*尋路算法

A* 算法簡介

關鍵代碼介紹

保存基本信息的地圖類

搜索到的節點類

算法主函數介紹

代碼的初始化

完整代碼

推薦閱讀：

發佈留言取消回覆

近期文章

A* 算法簡介

關鍵代碼介紹

保存基本信息的地圖類

搜索到的節點類

算法主函數介紹

代碼的初始化

完整代碼

推薦閱讀：

發佈留言 取消回覆

近期文章

標籤

發佈留言取消回覆