4. 基础数据结构：图、树、堆、栈在路径规划中的应用
说实话，很多初学者一上来就扎进A*算法、RRT这些高级玩意儿里，结果代码写到一半卡住了。为什么？因为底层的数据结构没吃透。我做了这么多年SLAM和路径规划，可以负责任地告诉你——数据结构就是算法的骨架。骨架歪了，肌肉再发达也跑不起来。
今天咱们就聊聊四个最常用的数据结构：图、树、堆、栈。它们在路径规划里到底扮演什么角色？怎么用代码实现？有哪些坑？我一个一个说。
4.1 图——路径规划的地图模型
路径规划的第一步，就是把现实世界抽象成计算机能理解的东西。图，就是最自然的抽象方式。
一个图由节点和边组成。节点代表位置，边代表连通关系。比如机器人要在一个房间里移动，我们可以把房间的每个角落、门口、障碍物边缘都设为节点，能直接走通的节点之间连上边。
核心概念：
- 有向图 vs 无向图：无向图表示双向通行，有向图表示单向。我做过一个仓储机器人项目，货架间的通道是单向的，那就必须用有向图。
- 加权图 vs 无权图：加权图的每条边带一个权重，比如距离、时间、能耗。无权图只关心连通性。
- 邻接矩阵 vs 邻接表：邻接矩阵适合稠密图，邻接表适合稀疏图。实际场景中，地图通常很稀疏，所以我个人习惯用邻接表。
来看一个简单的邻接表实现：
class Graph:
def __init__(self):
self.adj = {} # 邻接表，key是节点，value是(邻居, 权重)列表
def add_edge(self, u, v, weight=1):
if u not in self.adj:
self.adj[u] = []
if v not in self.adj:
self.adj[v] = []
self.adj[u].append((v, weight))
self.adj[v].append((u, weight)) # 无向图
def get_neighbors(self, node):
return self.adj.get(node, [])嗯，这里要注意：get_neighbors 是路径规划算法最频繁调用的函数。如果这个函数写得慢，整个算法都会拖垮。我在项目中遇到过，有人用邻接矩阵存了10000个节点，每次取邻居都要遍历一整行，结果A*算法跑一次要好几秒。换成邻接表后，直接降到几十毫秒。
我的建议：路径规划中，图结构一定要支持快速查询邻居。邻接表是默认选择。如果节点数超过10万，可以考虑用CSR（压缩稀疏行）格式，性能更好。
4.2 树——搜索与决策的骨架
树是一种特殊的图——没有环，连通。路径规划里，树主要用来做搜索和决策。
最常见的应用是搜索树。比如BFS、DFS，本质上就是在隐式树上做遍历。还有RRT（快速随机搜索树），它显式地构建一棵树，从起点开始不断向未探索区域生长。
我印象很深的一个项目是无人机避障。我们用RRT*算法，每扩展一个节点都要检查是否与障碍物碰撞。如果树结构设计得好，碰撞检测可以复用父节点的信息，避免重复计算。说白了，树的高度和分支因子直接影响搜索效率。
另一个重要应用是决策树。在行为规划中，机器人需要根据当前状态选择下一步动作。比如：前方有障碍物？是绕行还是等待？这些判断可以用决策树来组织。
树的表示（Python）：
class TreeNode:
def __init__(self, state, parent=None):
self.state = state # 节点状态（位置、速度等）
self.parent = parent # 父节点
self.children = [] # 子节点列表
def add_child(self, child_node):
self.children.append(child_node)
child_node.parent = self
def path_to_root(self):
"""从当前节点回溯到根节点，得到完整路径"""
path = []
node = self
while node is not None:
path.append(node.state)
node = node.parent
return path[::-1] # 反转，从根到当前你看，path_to_root 这个方法在RRT和A*里都特别常用。找到目标后，沿着父指针一路回溯，就得到了完整路径。我曾经见过有人用递归写这个回溯，结果树深度超过1000层时直接栈溢出。所以，我建议用迭代方式，别用递归。
避坑指南：我曾经在一个项目中，树的节点数达到几十万，每个节点都存了完整的路径副本。结果内存直接爆了。正确的做法是只存父节点指针，路径在需要时再回溯生成。
4.3 堆——优先级队列的幕后英雄
堆，说白了就是能快速拿到最大或最小元素的数据结构。在路径规划里，堆最常见的角色就是优先级队列。
想想看，A*算法每步都要从开放集中取出f值最小的节点。如果每次都用线性扫描，复杂度是O(n)，节点一多就完蛋。用堆的话，取最小值和插入都是O(log n)，效率天差地别。
Python里直接用 heapq 模块就行：
import heapq
class PriorityQueue:
def __init__(self):
self._heap = []
self._counter = 0 # 用于处理相同优先级的情况
def push(self, item, priority):
heapq.heappush(self._heap, (priority, self._counter, item))
self._counter += 1
def pop(self):
return heapq.heappop(self._heap)[-1]
def is_empty(self):
return len(self._heap) == 0你想想看，这里为什么要加一个 _counter？因为当两个节点的优先级相同时，堆会比较元组的第二个元素。如果第二个元素是自定义对象，而对象没有实现比较方法，就会报错。加一个递增计数器，既保证唯一性，又保证先进先出。
我的经验：在Dijkstra和A*中，经常需要更新已入队节点的优先级。但标准堆不支持直接更新。我常用的技巧是：把新节点直接push进去，并在节点中标记旧版本。pop时如果发现是旧版本，就跳过。这叫"惰性删除"，简单有效。
4.4 栈——深度优先与回溯利器
栈，后进先出。在路径规划里，栈主要用在深度优先搜索和回溯场景。
DFS用栈来保存待探索的节点。每次从栈顶取一个节点，把它的邻居压入栈顶。这样就会一直往深处走，直到走不通再回头。
还有一个经典应用是拓扑排序。在任务规划中，有些任务必须在其他任务完成后才能开始。比如"先充电，再移动，最后抓取"。拓扑排序用栈来记录完成顺序，非常自然。
看一个DFS的栈实现：
def dfs_stack(graph, start, goal):
stack = [(start, [start])] # (当前节点, 路径)
visited = set()
while stack:
node, path = stack.pop()
if node == goal:
return path
if node in visited:
continue
visited.add(node)
for neighbor, _ in graph.get_neighbors(node):
if neighbor not in visited:
stack.append((neighbor, path + [neighbor]))
return None # 没找到路径嗯，这里有个细节：path + [neighbor] 会创建新列表，如果路径很长，内存开销不小。我一般会改用父指针回溯的方式，就像前面树结构里那样。
注意：DFS不保证找到最短路径。如果你需要最短路径，用BFS（队列）或A*（优先级队列）。DFS适合的场景是：只要找到一条路径就行，或者搜索空间特别大、内存有限的情况。
4.5 四种数据结构的对比与选择
说了这么多，咱们来总结一下。我画了一张图，帮你理清思路：
最后，我整理了一个速查表，方便你实际开发时快速决策：
| 数据结构 | 核心操作 | 时间复杂度 | 路径规划典型场景 |
|---|---|---|---|
| 图（邻接表） | 查询邻居 | O(deg(v)) | 地图建模、拓扑分析 |
| 树（父指针） | 回溯路径 | O(depth) | RRT、搜索树、决策树 |
| 堆（二叉堆） | push/pop | O(log n) | A*、Dijkstra的开放集 |
| 栈（数组/链表） | push/pop | O(1) | DFS、拓扑排序、回溯 |
说白了，这四种数据结构是路径规划的"四件套"。图负责描述世界，树负责组织搜索，堆负责高效排序，栈负责深度探索。把它们吃透了，后面学A*、RRT、Dijkstra这些算法，你会觉得豁然开朗。
我刚开始做路径规划时，也踩过不少坑。比如用列表当优先级队列，节点一多就慢得离谱。后来老老实实换成堆，性能直接起飞。所以，别嫌数据结构基础，它们才是真正的效率之源。