原理
廣度優先搜索是一種基於圖的搜索算法，算法原理比較簡單：從起點不斷的進行膨脹直到最後觸碰到終點爲止。如下圖所示，令綠色是起點，黃色是終點，算法搜索的步驟就是從綠色開始膨脹，然後紅色，然後藍色，直到最後膨脹到終點黃色。以上雖然不是很嚴謹吧，我覺得還是可以說明一些思想的。
其他的一些資料
以下是深藍學院中的路徑規劃課程中介紹的BFS算法。
例子
這個例子是在開源代碼中找到的，給添加了一些註釋；另外該算法雖然是使用python寫的，但其實可以忽略掉一些輔助性的東西而關注於核心思想。
需要注意的是python3的dict彈出的順序與寫入順序有關。current = open_set.pop(list(open_set.keys())[0])，先進先出。
"""
Breadth-First grid planning
author: Erwin Lejeune (@spida_rwin)
See Wikipedia article (https://en.wikipedia.org/wiki/Breadth-first_search)
"""
import math
import matplotlib.pyplot as plt
show_animation = True
class BreadthFirstSearchPlanner:
def __init__(self, ox, oy, reso, rr):
"""
Initialize grid map for bfs planning
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
self.reso = reso
self.rr = rr
self.calc_obstacle_map(ox, oy)
self.motion = self.get_motion_model()
class Node:
def __init__(self, x, y, cost, pind, parent):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.pind = pind
self.parent = parent
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(
self.cost) + "," + str(self.pind)
def planning(self, sx, sy, gx, gy):
"""
Breadth First search based planning
input:
sx: start x position [m]
sy: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
output:
rx: x position list of the final path
ry: y position list of the final path
"""
nstart = self.Node(self.calc_xyindex(sx, self.minx),
self.calc_xyindex(sy, self.miny), 0.0, -1, None)
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
self.calc_xyindex(gy, self.miny), 0.0, -1, None)
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(nstart)] = nstart
while 1:
if len(open_set) == 0:
print("Open set is empty..")
break
current = open_set.pop(list(open_set.keys())[0])
#彈出的節點被放到另外的一個字典中
c_id = self.calc_grid_index(current)
closed_set[c_id] = current
# show graph
if show_animation: # pragma: no cover
plt.plot(self.calc_grid_position(current.x, self.minx),
self.calc_grid_position(current.y, self.miny), "xc")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event:
[exit(0) if event.key == 'escape'
else None])
if len(closed_set.keys()) % 10 == 0:
plt.pause(0.001)
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
# expand_grid search grid based on motion model
for i, _ in enumerate(self.motion):
node = self.Node(current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2], c_id, None)
n_id = self.calc_grid_index(node)
# If the node is not safe, do nothing
if not self.verify_node(node):
continue
if (n_id not in closed_set) and (n_id not in open_set):
node.parent = current
open_set[n_id] = node
rx, ry = self.calc_final_path(ngoal, closed_set)
return rx, ry
def calc_final_path(self, ngoal, closedset):
# generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)]
n = closedset[ngoal.pind]
while n is not None:
rx.append(self.calc_grid_position(n.x, self.minx))
ry.append(self.calc_grid_position(n.y, self.miny))
n = n.parent
return rx, ry
def calc_grid_position(self, index, minp):
"""
calc grid position
:param index:
:param minp:
:return:
"""
pos = index * self.reso + minp
return pos
def calc_xyindex(self, position, min_pos):
return round((position - min_pos) / self.reso)
def calc_grid_index(self, node):
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
#有效且無障礙物
def verify_node(self, node):
px = self.calc_grid_position(node.x, self.minx)
py = self.calc_grid_position(node.y, self.miny)
if px < self.minx:
return False
elif py < self.miny:
return False
elif px >= self.maxx:
return False
elif py >= self.maxy:
return False
# collision check
if self.obmap[node.x][node.y]:
return False
return True
def calc_obstacle_map(self, ox, oy):
self.minx = round(min(ox))
self.miny = round(min(oy))
self.maxx = round(max(ox))
self.maxy = round(max(oy))
print("minx:", self.minx)
print("miny:", self.miny)
print("maxx:", self.maxx)
print("maxy:", self.maxy)
self.xwidth = round((self.maxx - self.minx) / self.reso)
self.ywidth = round((self.maxy - self.miny) / self.reso)
print("xwidth:", self.xwidth)
print("ywidth:", self.ywidth)
# obstacle map generation
self.obmap = [[False for _ in range(self.ywidth)]
for _ in range(self.xwidth)]
for ix in range(self.xwidth):
x = self.calc_grid_position(ix, self.minx)
for iy in range(self.ywidth):
y = self.calc_grid_position(iy, self.miny)
for iox, ioy in zip(ox, oy):
#這個位置存在障礙物
d = math.hypot(iox - x, ioy - y)
if d <= self.rr:
self.obmap[ix][iy] = True
break
@staticmethod
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
def main():
print(__file__ + " start!!")
# start and goal position
sx = 10.0 # [m]
sy = 10.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 2.0 # [m]
robot_radius = 1.0 # [m]
# set obstacle positions
ox, oy = [], []
for i in range(-10, 60):
ox.append(i)
oy.append(-10.0)
for i in range(-10, 60):
ox.append(60.0)
oy.append(i)
for i in range(-10, 61):
ox.append(i)
oy.append(60.0)
for i in range(-10, 61):
ox.append(-10.0)
oy.append(i)
for i in range(-10, 40):
ox.append(20.0)
oy.append(i)
for i in range(0, 40):
ox.append(40.0)
oy.append(60.0 - i)
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "og")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
bfs = BreadthFirstSearchPlanner(ox, oy, grid_size, robot_radius)
rx, ry = bfs.planning(sx, sy, gx, gy)
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.pause(0.01)
plt.show()
if __name__ == '__main__':
main()
其柵格地圖如下圖所示：
其最終搜索的路徑如下圖所示：
