A*路径规划算法的Python实现

写在前面

今天因为要在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):  # 判断这个点是不是在map中
        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):  # 修改这个函数来实现各种H(n)
        # return abs(car_pos[0] - target_pos[0]) + abs(car_pos[1] - target_pos[1])
        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:
        # openlist为空，失败了
        if len(open_list) == 0:
            break
        # 如果openlist中有target_pos，则规划成功了
        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'])]

        # 找到open_list里Fn最小的点
        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

        # 将这个点转移到closed_list中
        point_info = deepcopy(open_list[min_Fn_i])
        open_list.pop(min_Fn_i)
        close_list[min_Fn_i] = point_info

        # 向外扩张 8邻域 并将可行点存入open_list中
        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, []  # 运行到这里一定是规划失败了，返回规划失败