写在前面
今天因为要在Python上实现机器人建图导航的仿真,写了A*算法的Python实现,过来分享一下。
关于A*算法的原理网上有很多,这里就不再赘述了,直接贴代码。
open_list 和 close_list 都通过dict实现,因为dict底层是hash_map,代码整体效率还行。
Python代码
from copy import deepcopy
import numpy as np
import math
def path_plan_Astar(car_pos, target_pos, map):
'''
:param car_pos: [x,y] 出发点位置
:param target_pos: [x,y] 终点位置
:param map: map是二维list或二维ndarray,map[y][x]=value value: 1:可行区域 其他:不可行区域
:return: bool:是否成功规划 int:步数=len(path) list:path经过的各个点[x,y]组成的列表,方向从 car_pos -> target_pos
'''
def is_point_in_map(map, point):
height = len(map)
width = len(map[0])
x = point[0]
y = point[1]
if x >= 0 and x < width and y >= 0 and y < height:
return True
return False
def Hn(car_pos, target_pos):
x_err = car_pos[0] - target_pos[0]
y_err = car_pos[1] - target_pos[1]
return math.sqrt(x_err * x_err + y_err * y_err)
map = np.array(deepcopy(map))
car_pos = [int(car_pos[0]), int(car_pos[1])]
target_pos = [int(target_pos[0]), int(target_pos[1])]
open_list = {}
close_list = {}
open_list[str(car_pos)] = {"pos": car_pos, "Gn": 0, "parent_pos": None}
path = []
while True:
if len(open_list) == 0:
break
if open_list.get(str(target_pos)) is not None:
current_point_info = open_list[str(target_pos)]
while True:
path.append(current_point_info['pos'])
if current_point_info['parent_pos'] is None:
path.append(current_point_info['pos'])
path.reverse()
return True, len(path), path
current_point_info = close_list[str(current_point_info['parent_pos'])]
min_Fn = -1
min_Fn_i = None
for i in open_list:
Fn = open_list[i]['Gn'] + Hn(open_list[i]['pos'], target_pos)
if Fn < min_Fn or min_Fn < 0:
min_Fn_i = i
min_Fn = Fn
point_info = deepcopy(open_list[min_Fn_i])
open_list.pop(min_Fn_i)
close_list[min_Fn_i] = point_info
for x in range(point_info['pos'][0] - 1, point_info['pos'][0] + 1 + 1):
for y in range(point_info['pos'][1] - 1, point_info['pos'][1] + 1 + 1):
if x != point_info['pos'][0] or y != point_info['pos'][1]:
if is_point_in_map(map, [x, y]):
if map[y][x] == 1 and (open_list.get(str([x, y])) is None) and (
close_list.get(str([x, y])) is None):
new_point = {"pos": [x, y], "Gn": point_info["Gn"] + 1, "parent_pos": point_info['pos']}
open_list[str(new_point['pos'])] = new_point
return False, 0, []