总结一下流程：
思维：直接走向终点+穿透障碍
1，从起点 直接向终点做，每次获取一下指向终点的向量，加一下自身坐标，得到下一个坐标。
分支：1.不是障碍，就继续往前走。
2.是障碍，获取四个关键点（障碍前一点，障碍点，伪穿透点（穿透点前一个点），穿透点），计算障碍物最边缘的重要属性 内圈（不可走点集合），外圈（可走点集合）。
具体前面已经说过了，这里在说一下：从障碍点开始，我的邻居=1的点（处理：如果这个邻居的八个邻居里有=0的点，那么就是障碍边缘点，如果是边界点pass掉）
我的邻居=0的点（处理，不在外圈path内，则加入）
3.得到内外圈的点，（实际在计算内外圈时就可以计算出最远距离点），这里可以用另外一种方法做。就是线性规划。障碍前一点和穿透点组成一天直线，可以 把内外圈各分成 两拨也就是两条path，在分别计算最远距离点。path是否可走，就看最远距离点的邻居是不是==1的边界点，是就不可走，不是就可走。
4.最小路径，最远距离点（到椭圆AB点的距离之和），A最初为起点，随后更新为最远距离点，B点为终点。
5.碰到障碍 则计算，返回最远距离点，和穿透点信息。
6.把穿透点更新为当前点，继续走。
#不需要记录路径什么的，记录下 起点 每个最远距离点 终点，这些就够了
#### 1为障碍，0为可走，9为B*算法产出的特殊点，需要A*寻路算法优化
#以下是代码
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)
map_be_search[1][6]=3
map_be_search[25][8]=3
#
#
# print(map_be_search)
# sys.exit()
# 边界
border_x_min = 0
border_x_max = 27
border_y_min = 0
border_y_max = 18
class BStarSearch():
def __init__(self):
# 始点和终点
self.start = {"point": (1, 6), "F": 0, "direct": None, "maker": 1} # maker 代表特殊点
self.end = {"point": (25, 8), "maker": 1}
# # 开启的表 这里也不需要了，我们不管路径的话
# self.openSet = []
# # 关闭的表
# self.closeSet = []
# 边界
self.border_x_min = 0
self.border_x_max = 18
self.border_y_min = 0
self.border_y_max = 18
self.special_point = self.start["point"]
self.special_point_list = [self.start, ]
# 计算点 椭圆上点 到 a(待变化) b终点（这个点不需要传进来，初始化时我们指定了） 就行了，好像不需要穿透点....文章里虽然写了
def get_distance_from_three_point(self, cur, a="", b=""):
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）,==四方向的不要，实际是有问题的，所以用下面8方向的==
def ____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 -1 / 2 <= k <= 1 / 2:
if x_sub < 0:
return (-1, 0)
else:
return (1, 0)
else:
if y_sub > 0:
return (0, 1)
else:
return (0,-1)
# 当前点指向终点的向量。 八方向 通过斜率相近得到方向向量（1,0）（-1,0）--（0，-1）（0,1）----(1,1)(-1,-1)---(-1, 1)(1, -1)
def 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 obstacle_path(self, cur, obstacle_point: tuple):
# 穿透点信息
temp_point=cur["point"]
direct=self.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
#-----------------------这里我们遍历内圈， 外圈也是一样的----------------------------------
# 攀爬的伪穿透点信息 也就是穿透点的前一个障碍点
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.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)
# # 实时显示一下地图障碍内圈 的行走信息
# for p in closeSet:
# map_be_search[p["point"][0]][p["point"][1]] = 8
# print(map_be_search)
# time.sleep(1)
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.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.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]
print(index,"==============")
if index==[]:
return 0,0
else:
if len(index)==1 and index[0]<=path1_length:
special_point1 = self.get_max_distance(closeSet[:path1_length])
return end_info,special_point1
if len(index)==1 and index[0]>path1_length:
special_point2 = self.get_max_distance(closeSet[path1_length:])
return end_info,special_point2
# ----通过两轮攀爬的 路径长度，， 舍去其中一个 end_point,留下一个即可
if path1_length<path_all_length-path1_length:
special_point1 = self.get_max_distance(closeSet[:path1_length])
return end_info,special_point1
else:
special_point2 = self.get_max_distance(closeSet[path1_length:])
return end_info,special_point2
def 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.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(self):
cur = self.start
while True:
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，0 返回一个穿透点 和 特殊点最远距离点
end_point,special_point = self.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"]:
self.special_point_list.append(self.end)
# 把产出的 特殊点显示一下
for p in self.special_point_list:
map_be_search[p["point"][0]][p["point"][1]]=9
print(map_be_search)
return 1
if __name__ == '__main__':
tt = BStarSearch()
path = tt.b_star_search()
扫一扫
338
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
