python简单实现B*寻路算法

穿透障碍，到达对面的点 ，，这里只是粗率的实现了一下，好像还是有bug的，下一篇文章开始讲解B*寻路算法的完整穿透思维，和优化，最后免费全部代码（python终极简化版）

import math
import sys
import time
import numpy as np

map_be_search = np.array([
    [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, 1, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1],
    [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 1],
    [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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)
print(map_be_search[18][18])
# 边界
border_x_min = 0
border_x_max = 18
border_y_min = 0
border_y_max = 18


class BStarSearch():
    def __init__(self):
        # 始点和终点
        self.start = {"point": (9, 1), "F": 0, "direct": None,"maker":1}
        self.end = {"point": (6, 17),"maker":1}

        # 当前点
        self.cur = self.start

        # 开启的表
        self.openSet = []
        # 关闭的表
        self.closeSet = []



        # 边界
        self.border_x_min = 0
        self.border_x_max = 18
        self.border_y_min = 0
        self.border_y_max = 18


    # 启发式方法  欧里几德距离,计算F的值
    def get_F(self, cur):
        F = math.sqrt((cur[0] - self.end["point"][0]) ** 2 + (cur[1] - self.end["point"][1]) ** 2)
        F = round(F, 2)
        return F

    # 获取周围八个点的坐标
    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 result

    # 当前点指向下一个点的向量。  这里没有存储为父节点，用父节点指向该点的向量表示，通过与向量的加减也可以得到父节点
    def get_two_point_direct(self,cur, next_):
        direct = (next_[0] - cur[0]), (next_[1] - cur[1])
        return direct


    # 当前点指向终点的向量。  四个方向  通过斜率相近 得到方向向量（1,0），（-1,0）（0，-1）（0,1）
    def get_direct(self, cur):
        x_sub, y_sub = (self.end["point"][0] - cur[0]), (self.end["point"][1] - cur[1])

        if x_sub==0 :
            # 可能是 （1,0）或者（-1,0） 所以除以绝对值
            return x_sub,int(y_sub/abs(y_sub))

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

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



    # 撞墙后 方向 变为相对的 左右方向
    def direct_split(self, direct):
        if direct[0] == 0:
            return (-1, 0), (1, 0)
        else:
            return (0, -1), (0, -1)






    # 返回爬墙路径 和 直线穿透障碍的第一个点,,    传入的是 当前点 {}字典信息  和 前进一步的障碍点坐标元祖  （）
    def obstacle_path(self, cur, obstacle_point: tuple):
        # 攀爬的终点信息
        temp_point=cur["point"]
        while True:
            # 传进来的点,沿着终点方向，穿透障碍，得到可以探索的第一个点     ：地图内的任意两点连线都不可能穿过地图边界
            end_point = temp_point[0] + cur["direct"][0], temp_point[1] + cur["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
        # end_info  的direct 需要攀爬过后才能知道


        # 开始爬墙，先 把障碍周围所有可走的点加入 self.openSet
        self.obstacle_around_openSet(obstacle_point)
        #----------------第一轮 攀爬-----------
        l1=len(self.closeSet)
        # 开启的表,
        start = cur

        openSet = [start]
        while openSet != []:
            cur=openSet.pop()
            self.closeSet.append(cur)

            map_be_search[cur["point"][0]][cur["point"][1]] = 5
            print(map_be_search)
            time.sleep(0.5)

            # 当前点已经是 穿透后的点了，
            if cur["point"] == end_point:
                break

            # 对当前格相邻的8格中的每一个
            neighbors = self.get_neighbors(cur["point"][0], cur["point"][1])
            next_point_info = {}
            for neighbor in neighbors:
                #  该邻居 在障碍周围的可探索点中。 并且没有关闭（走过），则下一个点就是它
                if neighbor in self.openSet and neighbor not in [p["point"] for p in self.closeSet]:
                    next_point_info["point"] = neighbor
                    next_point_info["direct"] = self.get_two_point_direct(cur["point"], neighbor)
                    # 但是对于最初的一个点来说，它的邻居 左右有两个点是符合的，这里打断一下只取一个。所以初始开放列表是 两个一样的，
                    break

            if next_point_info:
                openSet.append(next_point_info)



        # ------------第二轮攀爬--------------
        l2=len(self.closeSet)
        openSet = [start]
        while openSet != []:
            cur=openSet.pop()
            self.closeSet.append(cur)
            map_be_search[cur["point"][0]][cur["point"][1]] = 5
            print(map_be_search)
            time.sleep(0.5)
            # 当前点已经是 穿透后的点了，
            if cur["point"] == end_point:
                break
            # 对当前格相邻的8格中的每一个
            neighbors = self.get_neighbors(cur["point"][0], cur["point"][1])
            next_point_info = {}
            for neighbor in neighbors:
                #   在障碍周围的可探索点中。 并且没有关闭，则下一个点就是它
                if neighbor in self.openSet and neighbor not in [p["point"] for p in self.closeSet]:
                    next_point_info["point"] = neighbor
                    next_point_info["direct"] = self.get_two_point_direct(cur["point"], neighbor)
                    # 但是对于最初的一个点来说，它的邻居 左右有两个点是符合的，这里打断一下只取一个。所以初始开放列表是 两个一样的，
                    break
            if next_point_info:
                openSet.append(next_point_info)


        # ，，如果只有一条路径到达，那么探索过 end_point点只有一个，否则有两个相同的 end_point，但是他们的父节点不一样，回溯路径是不好处理，
        l3=len(self.closeSet)

        last_index=[self.closeSet.index(p) for p in self.closeSet if p["point"]==end_point]
        if last_index==[]:
            return 0
        else:
            if len(last_index)==1:
                return self.closeSet[last_index[0]]
            # ----通过两轮攀爬的 路径长度，， 舍去其中一个 end_point,留下一个即可
            if l2-l1<l3-l2:
                self.closeSet.pop(last_index[1])
                return self.closeSet[last_index[0]]
            else:
                self.closeSet.pop(last_index[0])
                return self.closeSet[last_index[1]]




    # 通过一个障碍内的点，得到障碍周围一圈的可探索点，，如果是环形障碍，其内部的点虽然加到全局开放列表但实际没有用到
    def obstacle_around_openSet(self, obstacle_point):
        # 局部开启的表
        obstacle_openSet = [obstacle_point,]
        # 局部关闭的表
        obstacle_closeSet = []
        while obstacle_openSet != []:
            # 切换到局部关闭列表
            cur=obstacle_openSet.pop()
            obstacle_closeSet.append(cur)

            # 对当前格相邻的8格中的每一个
            neighbors = self.get_neighbors(cur[0], cur[1])
            for neighbor in neighbors:
                # 可以走 且不在 全局开放表中
                if map_be_search[neighbor[0]][neighbor[1]] == 0 and neighbor not in self.openSet:
                    self.openSet.append(neighbor)

                # =1 这是障碍内的点，且在边界内   =1的点可能是边界点，所以联合判定
                if map_be_search[neighbor[0]][neighbor[1]] == 1 and border_x_min < neighbor[0] < border_x_max and border_y_min < neighbor[1] < border_y_max:
                    # 已经在 "关闭列表" 中 不做处理
                    if neighbor in obstacle_closeSet:
                        continue
                    # 它不在局部 开启列表中，这个障碍点可能可以探索
                    if neighbor not in obstacle_openSet:
                        # 遍历邻居   相邻的8格中的每一个
                        neighbors_list=self.get_neighbors(neighbor[0],neighbor[1])
                        for neighbor_neighbor in neighbors_list:
                            # 如果该邻居周围的格子里有一个 0， 说明它在障碍边缘， 该0 的坐标不在全局开放表中  则加入
                            if map_be_search[neighbor_neighbor[0]][neighbor_neighbor[1]] == 0 and neighbor_neighbor not in self.openSet:
                                obstacle_openSet.append(neighbor)
                                break


    def b_star_search(self, cur):
        while True:
            for p in [p["point"] for p in self.closeSet]:
                map_be_search[p[0]][p[1]] = 5
            print(map_be_search)
            time.sleep(0.5)

            self.closeSet.append(cur)

            direct=self.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，爬过了返回一个带信息的点
                end_point = self.obstacle_path(cur, next_point)
                if end_point==0:
                    return 0
                else:
                    next_point_info=end_point
            else:
                # 更新这个点的信息
                next_point_info["point"]=next_point
                next_point_info["direct"]=self.get_two_point_direct(cur["point"],next_point)

            # 下一个点交换为当前点
            cur=next_point_info
            #
            if next_point==self.end["point"]:
                return 1

    def main(self):
        code=self.b_star_search(self.start)
        if code==0:
            print("没有路劲到达终点")
            return None
        else:
            return self.closeSet


# 路径  从最后点信息 反向解析出  上一个点
def reverse_direct_path(path):
    real_path = []
    child=path[-1]
    real_path.append(child["point"])
    while True:
        parent = child["point"][0] - child["direct"][0], child["point"][1] -child["direct"][1]
        real_path.append(parent)
        for p in path:
            if p["point"] == parent:
                child = p

        if child["direct"] == None:
            break
    return real_path

if __name__ == '__main__':
    tt=BStarSearch()
    path=tt.main()
    if path:
        real_path = reverse_direct_path(path)
        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)