永远不要说永远，总有一些事情是可能的。— 《放牛班的春天》
🏰代码及环境配置：请参考 环境配置和代码运行!
广度优先搜索（Breadth-First Search，简称BFS）是一种用于遍历或搜索树或图的算法。该算法从一个节点开始，先访问这个节点的所有邻接节点，然后再依次访问这些邻接节点的未被访问的邻接节点，以此类推，直到访问完所有可达的节点。它适用于无向图和有向图。
4.3.1 数据结构-队列
队列是一种**先进先出(FIFO)**的容器，如下图：
4.3.2 BFS算法的实现
使用BFS算法找寻路径的实现步骤如下：
初始化：
创建一个队列 Q，用于存储待访问的节点。创建一个数组 visited，用于标记已访问过的节点。将起始节点放入队列，并标记为已访问。
从队列中弹出（或称为“出队”）一个节点，记为 v，若节点v就是目标节点goal，则提前返回。访问节点 v的所有未访问的邻接节点。对于每个未访问的邻接节点，将其加入队列，并标记为已访问，同时记录当前节点 v为邻接节点的父节点。当队列不为空时，执行以下操作：
从目标节点开始，通过父节点向回追溯，直到起始节点，最终得到一条路径。
其伪代码如下：
Algorithm BFS_iterative(G, start, goal):
let **Q** be a queue
**Q**.enqueue(**start**)
mark **start** as visited
while **Q** is not empty do
v = **Q**.dequeue()
if v is the **goal**:
return v
for **all neighbours** n of v in Graph G do
if n is not visited:
**Q**.enqueue(n)
n.parent = v
mark n as visited
4.3.3 BFS算法的图解
以从下面的无权图中找到从节点0 到节点4 的最短路径为例，说明一下BFS算法的工作流程：
初始化一个空的队列和一个空的数组，其中队列用来存储未被访问过的节点，数组用来存储节点是否被访问过。
将节点 0放入队列，并将其标记为已访问。
从队列头部移除节点 0，并将其未被访问过的邻接节点（1，2）放入队列，邻接节点（1，2）的父节点是0。
从队列头部移除节点 1，并将其未被访问过的邻接节点（3）放入队列，邻接节点（3）的父节点是1。
从队列头部移除节点 2，并将其未被访问过的邻接节点（4）放入队列，邻接节点（4）的父节点是2。
从队列头部移除节点 3，并将其未被访问过的邻接节点（无）放入队列。
从队列头部移除节点 4，节点4即为目标节点，BFS算法遍历结束，并通过回溯父节点找到一条路径（0 —> 2—> 4）。
4.3.4 BFS算法特点
优点： 可以找到从起点到终点的最短路径（在无权图中）。 实现简单，易于理解和实现。 缺点： 需要较多的存储空间来保存待访问的节点（使用队列）。 在有权图中，BFS不一定能找到最短路径（因为BFS只考虑路径上的边数，而不考虑边的权重）。
本节提供了BFS（广度优先搜索）的代码测试：
python3 tests/search_based_planning/bfs_test.py
4.3.c.1 构图的代码实现
基于图搜的运动规划中最重要的一步是构图，构建的图比较简单，主要包含map border和obstacles，读者也可根据需求修改构图方式。
def construct_env_info():
border_x = []
border_y = []
ox = []
oy = []
# Map border.
for i in range(-10, 60):
border_x.append(i)
border_y.append(-10.0)
for i in range(-10, 60):
border_x.append(60.0)
border_y.append(i)
for i in range(-10, 61):
border_x.append(i)
border_y.append(60.0)
for i in range(-10, 61):
border_x.append(-10.0)
border_y.append(i)
# Obstacle 1.
for i in range(40, 55, 1):
for j in range(5, 15, 1):
ox.append(i)
oy.append(j)
# Obstacle 2.
for i in range(40):
for j in range(20, 25, 1):
ox.append(j)
oy.append(i)
# Obstacle 3.
for i in range(30):
for j in range(40, 45, 1):
ox.append(j)
oy.append(58.0 - i)
# Obstacle 4.
for i in range(0, 20, 1):
for j in range(35, 40, 1):
ox.append(i)
oy.append(j)
return border_x, border_y, ox, oy
4.3.c.2 BFS的代码实现
BFS的实现较为简单，按照理论部分一步步实现即可。首先定义了Node ，这是图的基础元素，代表节点，然后实现BFS的核心代码-planning ，其中：
sx:起始点的x坐标的值
sy:起始点的y坐标的值
gx:目标点的x坐标的值
gy:目标点的y坐标的值
最终返回一条路径：rx, ry
class Node:
def __init__(self, x, y, cost, parent_index, parent):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.parent_index = parent_index
self.parent = parent
def planning(self, sx, sy, gx, gy):
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 True:
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
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.parent_index = current.parent_index
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
4.3.c.3 BFS的代码测试
在main 函数中设置起始点，目标点，grid的分辨率和机器人的半径，在创建grid map之后，并运行BFS算法，即可找到一条路径。如下图所示，红色路径即为最终BFS搜出来的路径。可以看到在无权图中，BFS可以搜索到一条最优路径。
def main():
# start and goal position
start_x = 10.0 # [m]
start_y = 10.0 # [m]
goal_x = 50.0 # [m]
goal_y = 0.0 # [m]
grid_size = 2.0 # [m]
robot_radius = 1.0 # [m]
# construct environment info.
border_x, border_y, ox, oy = construct_env_info()
if show_animation: # pragma: no cover
plt.plot(border_x, border_y, "s", color=(0.5, 0.5, 0.5), markersize=10)
plt.plot(ox, oy, "s", color="k")
plt.plot(start_x, start_y, "og", markersize=10)
plt.plot(goal_x, goal_y, "ob", markersize=10)
plt.grid(True)
plt.axis("equal")
ox.extend(border_x)
oy.extend(border_y)
dfs = DepthFirstSearchPlanner(ox, oy, grid_size, robot_radius)
rx, ry = dfs.planning(start_x, start_y, goal_x, goal_y)
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.savefig(gif_creator.get_image_path())
plt.pause(0.01)
gif_creator.create_gif()
plt.show()
4.3.5 参考
https://www.geeksforgeeks.org/queue-data-structure/
https://en.wikipedia.org/wiki/Breadth-first_search
https://www.geeksforgeeks.org/breadth-first-search-or-bfs-for-a-graph/
推荐阅读:
自动驾驶小白说官网: https://www.helloxiaobai.cn
