第四章 图搜索基础：从数据结构到遍历策略
图搜索，说白了就是回答两个问题：“怎么走？”和“走过了没？”。做轨迹规划这么多年，我见过太多新手一上来就怼 A*、RRT*，结果连图的基本表示都没搞明白，调试时一脸懵。这一章咱们把地基打牢——图论基础、两种存储方式、BFS 和 DFS。这些东西看似简单，但实际项目中踩的坑，往往就出在这些基础环节。
4.1 图论基础：节点与边的世界
图由两部分组成：顶点（Vertex）和边（Edge）。你可以把顶点想象成地图上的路口，边就是连接路口的道路。
- 有向图 vs 无向图：有向图的边有方向，好比单行道；无向图的边没方向，好比双向车道。
- 加权图 vs 无权图：加权图的边带权重，比如距离、时间、能耗；无权图所有边代价相同。
- 连通图 vs 非连通图：连通图任意两点都能通过路径到达；非连通图存在孤岛。
核心概念：在轨迹规划中，顶点通常代表机器人的位姿或关键点，边代表两个位姿之间的可行运动。权重就是代价——时间最短、能耗最低、或者安全性最高。
我个人习惯把图分为稀疏图和稠密图。稀疏图边少，比如道路网络；稠密图边多，比如全连接的状态空间。这个区分直接决定了你选哪种存储方式。
4.2 邻接矩阵与邻接表：两种存储哲学
图的存储方式，我建议你掌握两种：邻接矩阵和邻接表。它们各有优劣，选错了，性能会差一个数量级。
4.2.1 邻接矩阵
用一个二维数组 adj[n][n] 存储所有顶点对的关系。adj[i][j] = 1 表示顶点 i 到 j 有边，0 表示无边。加权图则存权重，无穷大表示不连通。
// 邻接矩阵示例（无权无向图）
int adj[5][5] = {
{0, 1, 1, 0, 0},
{1, 0, 0, 1, 0},
{1, 0, 0, 1, 1},
{0, 1, 1, 0, 0},
{0, 0, 1, 0, 0}
};| 特性 | 邻接矩阵 | 邻接表 |
|---|---|---|
| 空间复杂度 | O(V²) | O(V + E) |
| 查询边是否存在 | O(1) | O(degree(v)) |
| 遍历所有邻接点 | O(V) | O(degree(v)) |
| 适用场景 | 稠密图 | 稀疏图 |
我的经验：在机器人地图中，顶点数可能上万，但每个顶点连接的边通常不超过 10 条。这时候用邻接矩阵，内存直接爆炸。我踩过这个坑——第一次做城市路网规划，用了邻接矩阵，5000 个顶点就占了 100MB，后来换成邻接表，降到 2MB。
4.2.2 邻接表
每个顶点维护一个链表或动态数组，只存储它实际连接的邻居。对于稀疏图，这是更明智的选择。
// 邻接表示例（C++ vector 实现）
vector<int> adj[5];
adj[0] = {1, 2};
adj[1] = {0, 3};
adj[2] = {0, 3, 4};
adj[3] = {1, 2};
adj[4] = {2};注意：邻接表查询边是否存在需要遍历链表，最坏情况 O(V)。如果你需要频繁查询边是否存在（比如在动态规划中），邻接矩阵反而更快。别盲目跟风，要根据你的操作模式选。
4.3 广度优先搜索（BFS）：层层推进的探索者
BFS 的核心思想：从起点开始，一层一层往外扩。它保证找到的路径是最短路径（在无权图中）。
4.3.1 算法流程
- 将起点放入队列，标记已访问。
- 从队列取出一个顶点，访问它所有未访问的邻居。
- 将未访问的邻居入队，标记已访问。
- 重复步骤 2-3，直到队列为空或找到目标。
void BFS(int start, vector<int> adj[], int n) {
vector<bool> visited(n, false);
queue<int> q;
visited[start] = true;
q.push(start);
while (!q.empty()) {
int u = q.front(); q.pop();
cout << u << " "; // 访问当前节点
for (int v : adj[u]) {
if (!visited[v]) {
visited[v] = true;
q.push(v);
}
}
}
}关键点：BFS 使用队列（FIFO），这保证了“先发现的节点先扩展”，从而实现逐层推进。如果你用栈代替队列，那就变成 DFS 了。
4.3.2 BFS 在轨迹规划中的应用
BFS 常用于栅格地图中的最短路径搜索。每个栅格是一个顶点，上下左右四个邻居是边。BFS 从起点开始，一层层扩散，直到碰到终点。
我曾经在一个仓库机器人项目中用 BFS 做全局路径规划。地图是 100x100 的栅格，BFS 能在 0.1 秒内找到最短路径。但要注意——如果地图太大，BFS 会遍历大量无关节点。这时候就需要启发式搜索（比如 A*）来剪枝。
避坑指南：BFS 的 visited 标记一定要在入队时设置，而不是出队时。我曾经犯过这个错——在出队时标记，结果同一个节点被多次入队，队列膨胀到几万，内存直接爆了。
4.4 深度优先搜索（DFS）：一条路走到黑
DFS 的核心思想：从起点出发，沿着一条路径走到头，再回溯。它不保证最短路径，但能快速找到一条可行路径。
4.4.1 算法流程
- 从起点开始，标记已访问。
- 选择一个未访问的邻居，递归进入。
- 如果当前节点没有未访问的邻居，回溯到上一个节点。
- 重复直到所有节点都被访问或找到目标。
void DFS(int u, vector<int> adj[], vector<bool>& visited) {
visited[u] = true;
cout << u << " "; // 访问当前节点
for (int v : adj[u]) {
if (!visited[v]) {
DFS(v, adj, visited);
}
}
}关键点：DFS 使用栈（递归调用栈或显式栈），这保证了“后发现的节点先扩展”，从而实现深度优先。递归实现简洁，但深度太大时可能栈溢出。我建议在深度超过 1000 时改用显式栈。
4.4.2 DFS 在轨迹规划中的应用
DFS 常用于拓扑排序、连通分量检测和迷宫求解。在轨迹规划中，DFS 可以用来快速判断两个点是否连通，或者生成一条不考虑最优性的可行路径。
我记得有一次做无人机航迹规划，需要快速判断起点和终点之间是否存在可行路径。用 BFS 要遍历几千个节点，用 DFS 只遍历了几百个就找到了。虽然路径不是最短的，但作为可行性检查，DFS 效率更高。
注意：DFS 在极端情况下可能陷入无限循环——如果图中有环且没有 visited 标记。所以 visited 数组是必须的。另外，递归深度过大会导致栈溢出，建议在递归深度超过 500 时改用迭代实现。
4.5 BFS vs DFS：如何选择？
| 对比维度 | BFS | DFS |
|---|---|---|
| 数据结构 | 队列（FIFO） | 栈（LIFO） |
| 路径最优性 | 保证最短路径（无权图） | 不保证 |
| 空间复杂度 | O(宽度)，可能很大 | O(深度)，通常较小 |
| 适用场景 | 最短路径、层级遍历 | 连通性检测、拓扑排序 |
说白了，选哪个取决于你的目标：要最优路径，用 BFS；要快速找到一条路，用 DFS。在实际项目中，我经常先用 DFS 做可行性检查，再用 BFS 或 A* 找最优路径。
4.6 本章知识体系
下面这张图帮你梳理本章的核心逻辑：
4.7 小结
这一章我们覆盖了图搜索的四个核心模块：
- 图论基础：顶点、边、有向/无向、加权/无权、连通/非连通
- 邻接矩阵：O(1) 查询，O(V²) 空间，适合稠密图
- 邻接表：O(V+E) 空间，适合稀疏图，实际项目首选
- BFS：队列实现，保证最短路径，适合无权图
- DFS：栈实现，快速找可行路径，适合连通性检测