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