【动手学运动规划】 4.3 BFS 广度优先遍历_广度优先搜索运动规划参考文献-CSDN博客

本文链接：https://blog.csdn.net/2403_86993842/article/details/143955281

永远不要说永远，总有一些事情是可能的。

— 《放牛班的春天》

🏰代码及环境配置：请参考环境配置和代码运行!

广度优先搜索（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只考虑路径上的边数，而不考虑边的权重）。

4.3.c 代码解析

本节提供了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