彗星般的人生可以短暂，但绝不黯淡或沉沦。
— 纳兰容若
🏰代码及环境配置：请参考 环境配置和代码运行!
深度优先搜索（Depth-First-Search，简称DFS）是一种基于图或搜索树的算法，从起始顶点开始选择某一路径深度试探查找目标顶点，当该路径上不存在目标顶点时，回溯到起始顶点继续选择另一条路径深度试探查找目标顶点，直到找到目标顶点或试探完所有顶点后回溯到起始顶点，完成搜索。
4.2.1 数据结构-栈
栈是一种**后进先出(LIFO)**的容器，如下图：
4.2.2 DFS算法的实现
由于DFS是以后进先出的方式遍历顶点，因此可以使用栈或者递归的方式来实现，以栈实现DFS为例，其算法步骤如下：
初始化：
创建一个栈 S，用于存储待访问的节点。创建一个数组 visited，用于标记已访问过的节点。将起始节点放入栈，并标记为已访问。
从栈中弹出（或称为“出栈”）一个节点，记为 v，若节点v就是目标节点goal，则提前返回。访问节点 v的所有未访问的邻接节点。对于每个未访问的邻接节点，将其加入栈，并标记为已访问，同时记录当前节点 v为邻接节点的父节点。当栈不为空时，执行以下操作：
从目标节点开始，通过父节点向回追溯，直到起始节点，最终得到一条路径。
DFS的非递归实现（使用栈的数据结构）：
Algorithm DFS_iterative(G, start, goal):
let **S** be a stack
**S**.push(**start**)
mark **start** as visited
while **S** is not empty do
v = **S**.pop()
if v is the **goal**:
return v
for **all neighbours** n of v in Graph G do
if n is not visited:
**S**.push(n)
n.parent = v
mark n as visited
4.2.3 DFS算法的图解
以从下面的无权图中找到从节点0 到节点3 的一条路径（注意：节点2 和节点3 之间不连通）为例，说明一下DFS算法的工作流程：
初始化一个空的栈和一个空的数组，其中栈用来存储未被访问过的节点，数组用来存储节点是否被访问过。
访问节点 0，并将它未被访问过的邻近节点（3，1，2）放入栈中，邻近节点（3，1，2）的父节点是0，其中放入栈中的顺序是自己定义的。
现在，节点 1在栈的顶部，因此访问节点1并从栈中取出节点1，然后将节点1所有未被访问过的邻近节点放入栈中（节点1没有邻近节点）。
节点 2在栈的顶部，因此访问节点2并从栈中取出节点2，然后将节点2所有未被访问过的邻近节点（4）放入栈中，节点4的父节点为2。
节点 4在栈的顶部，因此访问节点4并从栈中取出节点4，然后将节点4所有未被访问过的邻近节点放入栈中（节点4没有邻近节点）。
节点 3在栈的顶部，因此访问节点3并从栈中取出节点3，节点3就是目标节点，DFS算法遍历结束，并通过回溯父节点找到一条路径（0 —> 3）。
4.2.4 DFS算法优缺点
优点： DFS算法相对简单，容易理解和实现 DFS算法特别适用于某些特定类型的问题，如寻找解的存在性、图的连通性检测等。 缺点： 在寻找最短路径的问题中，DFS不保证找到最优解，而是第一条符合要求的路径。 对于深度很大的图，DFS的递归深度也会很大，这可能会导致栈溢出等问题。
本节提供了DFS（深度优先搜索）的代码测试：
python3 tests/search_based_planning/dfs_test.py
4.2.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.2.c.2 DFS的代码实现
DFS的实现较为简单，按照理论部分一步步实现即可，其中：
sx:起始点的x坐标的值
sy:起始点的y坐标的值
gx:目标点的x坐标的值
gy:目标点的y坐标的值
最终返回一条路径：rx, ry
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())[-1])
c_id = self.calc_grid_index(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],
)
plt.savefig(gif_creator.get_image_path())
plt.pause(0.01)
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:
open_set[n_id] = node
closed_set[n_id] = node
node.parent = current
rx, ry = self.calc_final_path(ngoal, closed_set)
return rx, ry
4.2.c.3 DFS的代码测试
在main 函数中设置起始点，目标点，grid的分辨率和机器人的半径，在创建grid map之后，并运行DFS算法，即可找到一条路径。如下图所示，红色路径即为最终DFS搜出来的路径。值得注意的是，DFS算法和子节点的存放顺序相关，因此其路径并不是一条最优的路径，只是一条可行路径。
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()
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)
参考
https://en.wikipedia.org/wiki/Depth-first_search
https://www.geeksforgeeks.org/depth-first-search-or-dfs-for-a-graph/
推荐阅读:
🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn
