A*寻路算法+B*寻路算法（5）联合 python实现全部代码在此

import math
import sys
import time
from collections import Counter
import numpy as np

map_be_search = np.array([
    #0  1  2  3  4  5  6  7  8  9  10
    [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],
    [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
    [1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
    [1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1],
    [1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
]
)
print(map_be_search.shape)
# 边界
border_x_min = 0
border_x_max = 27
border_y_min = 0
border_y_max = 18


class Auto_Search_path_from_map():
    def __init__(self,start,end):
        # 始点和终点
        self.start = {"point": start, "G_cost": 0,"F_cost":0,"H_cost":0, "parent": None}
        self.end = {"point": end,"G_cost": 0,"F_cost":0,"H_cost":0}
        # 边界
        self.border_x_min = 0
        self.border_x_max = 18
        self.border_y_min = 0
        self.border_y_max = 18
        # A 点和特殊点列表
        self.special_point = self.start["point"]
        self.special_point_list = []

    #  计算点椭圆上点到 a(待变化) b终点
    def B_star_get_distance_from_three_point(self, cur,):
        a= math.sqrt((cur[0] - self.end["point"][0]) ** 2 + (cur[1] - self.end["point"][1]) ** 2)
        b=math.sqrt((cur[0] - self.special_point[0]) ** 2 + (cur[1] - self.special_point[1]) ** 2)
        return round(a+b, 2)

    # 获取周围八个点的坐标，判定是否越界
    def get_neighbors(self, x, y):
        up = x, y + 1
        down = x, y - 1
        left = x - 1, y
        right = x + 1, y
        left_up = x - 1, y + 1
        right_up = x + 1, y + 1
        left_down = x - 1, y - 1
        right_down = x + 1, y - 1
        result = [up, down, left, right, left_up, right_up, left_down, right_down]
        return [p for p in result if border_x_min < p[0] < border_x_max and border_y_min < p[1] < border_y_max]

    # 当前点指向终点的向量。 八方向  通过斜率相近得到方向向量（1,0）（-1,0）--（0，-1）（0,1）----(1,1)(-1,-1)---(-1, 1)(1, -1)
    def B_star_get_direct(self, cur):
        x_sub, y_sub = (self.end["point"][0] - cur[0]), (self.end["point"][1] - cur[1])
        # 说明 垂直 x轴上， k = y_sub / x_sub  0为被除数
        if x_sub == 0:
            # 除以绝对值
            return x_sub, y_sub / abs(y_sub)
        # 计算斜率
        k = y_sub / x_sub
        if 3 / 2 < k or k <= -3 / 2:
            if x_sub < 0:
                return (0, -1)
            else:
                return (0, 1)

        if 1 / 2 < k <= 3 / 2:
            if x_sub < 0:
                return (-1, -1)
            else:
                return (1, 1)

        if -1 / 2 < k <= 1 / 2:
            if x_sub < 0:
                return (-1, 0)
            else:
                return (1, 0)

        if -3 / 2 < k <= -1 / 2:
            if x_sub < 0:
                return (-1, 1)
            else:
                return (1, -1)

    # 爬墙路径    这里对于我们来说，只需要返回  最远点（maker 标记一下）和穿透点      两个点就可以了
    def B_star_obstacle_path(self, cur, obstacle_point: tuple):
        # 穿透点信息
        temp_point=cur["point"]
        direct=self.B_star_get_direct(temp_point)
        while True:
            # 传进来的点,沿着终点方向，穿透障碍，得到可以探索的第一个点     ：地图内的任意两点连线都不可能穿过地图边界
            end_point = temp_point[0] + direct[0], temp_point[1] + direct[1]
            if map_be_search[end_point[0]][end_point[1]] == 0:
                break
            temp_point = end_point
        end_info = {}
        end_info["point"] = end_point

        #-----------------------这里我们遍历内圈， 要求障碍最小3*3，如果是1*1或者2*2可能有问题  外圈也是一样的----------------------------------
        # 攀爬的伪穿透点信息    也就是穿透点的前一个障碍点
        obstacle_end_point = temp_point

        # 开启的表,
        openSet = [{"point":obstacle_point},{"point":obstacle_point}]
        # 关闭的表
        closeSet = []

        # 因为两条路都要走，openSet有两个相同的点 所以在第二次取到时，计算下前面一条完整路径的长度
        path1_length=0
        while openSet != []:
            # 切换到关闭列表
            cur=openSet.pop()
            cur["distance"] = self.B_star_get_distance_from_three_point(cur["point"])
            closeSet.append(cur)

            # # 当前点已经是 穿透后的点了， 则返回起点
            # #！！！！这里可以直接把closeSet里的起点干掉，必有一条路到终点，如果closeSet里没有起点，那么一条路，如果有则两条路，如果连终点都不在closeSet则无路  ！！！！！
            if cur["point"] == obstacle_end_point:
                cur = openSet.pop(0)

            # 因为两条路都要走，openSet有两个相同的点 所以在第二次取到时，计算下前面一条 path1  完整路径的长度
            if cur["point"]==obstacle_point:
                path1_length = len(closeSet)

            neighbors = self.get_neighbors(cur["point"][0], cur["point"][1])
            next_point_info = {}
            # 对当前格相邻的8格中的每一个
            for neighbor in neighbors:
                # 第一次到达伪终点后，终点信息已经加到 closeSet 里面了，导致第二次不能加入，这里手动加入，并中断遍历
                if neighbor==obstacle_end_point:
                    closeSet.append({"point":neighbor,"distance":self.B_star_get_distance_from_three_point(neighbor)})
                    break
                # ==1 且不在关闭表内，，边界判定在获取邻居时做了
                if map_be_search[neighbor[0]][neighbor[1]] == 1 and neighbor not in [p["point"] for p in closeSet]:
                    neighbors_list = self.get_neighbors(neighbor[0], neighbor[1])
                    for neighbor_neighbor in neighbors_list:
                        # 如果该邻居周围的格子里有一个 0， 说明它在障碍边缘，
                        if map_be_search[neighbor_neighbor[0]][neighbor_neighbor[1]] == 0:
                            next_point_info["point"] = neighbor

                #这里很巧妙。对第一个点，它周围是有两个或者三个点符合条件的分别属于两个分支     打断我们只取第一个， 第一个分支
                # 最开始的开启表内有两个同一起点，最开始取了一个，而且接下来每次从openset中取最后一个点，如过又取到了第一个点，说明回到原点，这下就取了第二个分支
                if next_point_info:
                    break
            if next_point_info:
                openSet.append(next_point_info)

        path_all_length=len(closeSet)
        # ----------获得了第一条路径的长度 和总长度，，我们可以切成两段，看看是不是有伪终点在里面，然后找出最小路径 最远距离点就可以了
        if path1_length==path_all_length:
            special_point1 = self.B_star_get_max_distance(closeSet[:path1_length])
            return end_info, special_point1

        index=[closeSet.index(p) for p in closeSet if p["point"]==obstacle_end_point]
        if index==[]:
            return 0,0
        else:
            if len(index)==1 and index[0]<=path1_length:
                special_point1 = self.B_star_get_max_distance(closeSet[:path1_length])
                return end_info,special_point1

            if len(index)==1 and index[0]>path1_length:
                special_point2 = self.B_star_get_max_distance(closeSet[path1_length:])
                return end_info,special_point2

            # ----通过两轮攀爬的 路径长度，， 舍去其中一个 end_point,留下一个即可
            if path1_length<path_all_length-path1_length:
                special_point1 = self.B_star_get_max_distance(closeSet[:path1_length])
                return end_info,special_point1
            else:
                special_point2 = self.B_star_get_max_distance(closeSet[path1_length:])
                return end_info,special_point2

    def B_star_get_max_distance(self, path,obstacle_inside_path=True):
        # a: [1, 3, 4, 5, 2, 7, 9]
        # 排序后[9, 7, 5, 4, 3, 2, 1]
        # 元素索引序列： [6, 5, 3, 2, 1, 4, 0]
        sorted_id = sorted(range(len(path)), key=lambda k: path[k]['distance'], reverse=True)
        # print('元素索引序列：', sorted_id)
        index = sorted_id[0]

        # 内圈 需要计算最远距离点的周围的可走点  在返回可走点的最远距离点
        if obstacle_inside_path==True:
            tmp=[]
            distance_list=[]
            neighbors=self.get_neighbors(path[index]["point"][0],path[index]["point"][1])
            for neighbor in neighbors:
                if map_be_search[neighbor[0]][neighbor[1]] == 0:
                    tmp.append(neighbor)
                    d=self.B_star_get_distance_from_three_point(neighbor)
                    distance_list.append(d)

            index=distance_list.index(max(distance_list))
            return {"point":tmp[index],"distance":distance_list[index],"maker":1}
        return path[index]

    def B_Star_Search_Path(self):
        cur = self.start
        while True:
            direct = self.B_star_get_direct(cur["point"])
            # 当前点  + 指向终点的指向向量   相加得到下一个点的坐标
            next_point = cur["point"][0] + direct[0], cur["point"][1] + direct[1]

            next_point_info = {}
            if map_be_search[next_point[0]][next_point[1]] == 1:
                # 这个点是障碍就爬墙  没爬过返回  0，0  返回一个穿透点 和最远距离点
                end_point,special_point = self.B_star_obstacle_path(cur, next_point)
                if end_point == 0:
                    return 0
                else:
                    # 把椭圆A点更新一下为最远距离点，，B点一直是终点，不变
                    self.special_point=special_point["point"]
                    self.special_point_list.append(special_point)
                    next_point_info = end_point
            else:
                # 更新这个点的信息
                next_point_info["point"] = next_point
            # 下一个点交换为当前点
            cur = next_point_info
            # 到达终点
            if next_point == self.end["point"]:
                return 1



    # 主函数 
    def Auto_Find_Path(self):
        code=self.B_Star_Search_Path()
        if code==1:
            special_point_list=[self.start,]
            if self.special_point_list:
                for p in self.special_point_list:
                    special_point_list.append({'point': p["point"], 'G_cost': 0, 'F_cost': 0, 'H_cost': 0,})
            special_point_list.append(self.end)

            path = []
            index=0
            start = special_point_list[index]
            while True:
                index = index + 1
                end = special_point_list[index]
                close_set= self.A_star_search_path(start,end)
                path.extend(close_set)
                if index + 1 == len(special_point_list):
                    break
                start = close_set[-1]
            return self.reverse_path(path)
        else:
            return None



    # 反向解析路径
    def reverse_path(self,path=[]):
            child_point=path.pop()
            real_path=[]
            real_path.append(child_point["point"])
            while True:
                parent_point = child_point["parent"]
                real_path.append(parent_point)

                for p in path:
                    if p["point"] ==parent_point:
                        child_point = p
                        path.remove(p)
                        break
                # 到达起点
                if child_point["parent"] == None:
                    break
            for k, v in zip(real_path[::-1], range(1, len(real_path) + 1)):
                map_be_search[k[0]][k[1]] = v
            print(map_be_search)
            return real_path[::-1]




    def A_star_search_path(self,start={},end={}):
        # 开启的表
        openSet = [start, ]
        # 关闭的表
        closeSet = []
        while openSet != []:
            # 将cur_grid重置为开启列表的F值最小的栅格
            cur = self.A_star_get_openSet_Point(openSet)

            # 切换到关闭列表
            openSet.remove(cur)
            closeSet.append(cur)

            # 对当前格相邻的8格中的每一个
            neighbors = self.get_neighbors(cur["point"][0], cur["point"][1])
            for neighbor in neighbors:
                # (它不可通过 | | 已经在 "关闭列表" 中)
                if map_be_search[neighbor[0]][neighbor[1]] == 1 or neighbor in [i["point"] for i in closeSet]:
                    continue
                # (它不在开启列表中)
                if neighbor not in [i["point"] for i in openSet]:
                    # 把它添加进"开启列表", 把当前格作为这一格的父节点, 并根据终点格，计算这一格的FGH
                    G_cost, H_cost, F_cost = self.A_star_get_G_F_H_cost(cur, neighbor,end)
                    openSet.append(
                        {"point": neighbor, "G_cost": G_cost, "H_cost": H_cost, "F_cost": F_cost, "parent": cur["point"]})
                    continue

                # (它已经在开启列表中)
                if neighbor in [i["point"] for i in openSet]:
                    index = [i["point"] for i in openSet].index(neighbor)
                    G_cost, H_cost, F_cost = self.A_star_get_G_F_H_cost(cur, neighbor,end)
                    # (用 G 值为参考检查新的路径是否更好, 更低的G值意味着更好的路径)#把这一格的父节点改成当前格, 并且重新计算这一格的 GF 值.
                    if G_cost < openSet[index]["G_cost"]:
                        openSet[index] = {"point": neighbor, "G_cost": G_cost, "H_cost": H_cost, "F_cost": F_cost,
                                          "parent": cur["point"]}

            # (终点已经在 "关闭列表" 找到路径;
            if end["point"] in [i["point"] for i in openSet]:
                closeSet.extend([i for i in openSet if end["point"]==i["point"]])
                return closeSet
            #
            # # # 实时显示一下地图障碍内圈 的行走信息
            # for p in closeSet:
            #     map_be_search[p["point"][0]][p["point"][1]] = 8
            # print(map_be_search)
            # time.sleep(1)




    # 开启列表中 F值最小的点
    def A_star_get_openSet_Point(self, openSet):
        sorted_id = sorted(range(len(openSet)), key=lambda k: openSet[k]["F_cost"], reverse=True)
        index = sorted_id.pop()

        return openSet[index]

    # 启发式方法  欧里几德距离
    def A_star_get_G_F_H_cost(self, parent, child,end):
        G_cost = parent["G_cost"] + math.sqrt(
            (child[0] - parent["point"][0]) ** 2 + (child[1] - parent["point"][1]) ** 2)
        H_cost = math.sqrt((child[0] - end["point"][0]) ** 2 + (child[1] - end["point"][1]) ** 2)
        F_cost = G_cost + H_cost
        return G_cost, H_cost, F_cost




if __name__ == '__main__':
    tt = Auto_Search_path_from_map(start=(1, 1),end= (26, 17))
    tt.Auto_Find_Path()