这个写手不太冷1. 项目概述：从算法使用者到创造者的跃迁
在自动驾驶技术栈中，路径规划算法如同车辆的"大脑决策层"，直接决定了车辆如何从A点安全高效地抵达B点。许多开发者能够调用现成的Dijkstra、A*等算法库完成基础功能，但遇到复杂场景（如动态避障、多车协同）时往往束手无策。这正是因为缺乏对算法底层原理的透彻理解——就像只会用现成家具的人，永远无法根据房间尺寸定制最合适的储物方案。
本项目的核心目标是通过Python实现经典Dijkstra算法，并逐步进行以下深度改造：
- 基础实现：还原算法论文中的原始版本
- 性能优化：引入堆结构提升计算效率
- 场景适配：增加动态障碍物处理能力
- 工程扩展：构建完整的自动驾驶规划模块
提示：本文代码已通过ROS 2 Humble环境实测，完整代码库见文末Github链接
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 环境配置与基础工具链
2.1 开发环境搭建
推荐使用Miniconda创建独立环境：
bash复制conda create -n path_planning python=3.9
conda activate path_planning
pip install numpy matplotlib networkx
2.2 可视化工具选择
- 基础可视化：Matplotlib + Networkx
- 高级仿真：PyGame（适合动态障碍物演示）
- 工程级开发：ROS 2 + RViz（需额外安装）
踩坑记录：Networkx的spring_layout布局算法可能导致节点重叠，建议手动设置pos参数固定节点位置
3. Dijkstra算法原理解析
3.1 算法核心思想拆解
Dijkstra算法的本质是贪心策略与动态规划的结合：
- 初始化起点距离为0，其他节点为无穷大
- 每次从"未处理集合"中选取距离最小的节点
- 更新该节点所有邻居的累积距离
- 重复直到所有节点都被处理
python复制def dijkstra_raw(graph, start):
distances = {node: float('inf') for node in graph}
distances[start] = 0
unvisited = set(graph.keys())
while unvisited:
current = min(unvisited, key=lambda node: distances[node])
unvisited.remove(current)
for neighbor, weight in graph[current].items():
new_dist = distances[current] + weight
if new_dist < distances[neighbor]:
distances[neighbor] = new_dist
return distances
3.2 时间复杂度优化方案
原始版本O(V²)的瓶颈在于每次查找最小距离节点。采用优先队列可优化至O(E + VlogV)：
python复制import heapq
def dijkstra_heap(graph, start):
distances = {node: float('inf') for node in graph}
distances[start] = 0
heap = [(0, start)]
while heap:
current_dist, current = heapq.heappop(heap)
if current_dist > distances[current]:
continue
for neighbor, weight in graph[current].items():
distance = current_dist + weight
if distance < distances[neighbor]:
distances[neighbor] = distance
heapq.heappush(heap, (distance, neighbor))
return distances
4. 自动驾驶场景适配改造
4.1 代价函数设计
真实道路需考虑：
- 路径长度（基础代价）
- 转弯惩罚（舒适性）
- 坡度变化（能耗）
- 交通规则（红绿灯等待时间）
python复制def cost_function(segment):
base_cost = segment.length
turn_cost = 20 if segment.is_turn else 0
slope_cost = 5 * abs(segment.slope)
return base_cost + turn_cost + slope_cost
4.2 动态障碍物处理
通过"时间膨胀"方法将动态问题转化为静态：
- 预测障碍物运动轨迹
- 在时间-空间维度构建三维栅格图
- 将动态障碍物转换为时空立方体中的障碍物
python复制class DynamicGrid:
def __init__(self, static_map):
self.time_layers = [static_map.copy() for _ in range(10)]
def add_obstacle(self, trajectory):
for t, pos in enumerate(trajectory):
if t < len(self.time_layers):
self.time_layers[t][pos] = float('inf')
5. 完整工程实现案例
5.1 基于ROS 2的架构设计
code复制path_planning_pkg/
├── config/
│ └── cost_params.yaml
├── launch/
│ └── sim.launch.py
├── scripts/
│ ├── dijkstra_planner.py
│ └── visualization.py
└── test/
└── test_dijkstra.py
5.2 关键接口实现
python复制class PathPlanner(Node):
def __init__(self):
super().__init__('dijkstra_planner')
self.subscription = self.create_subscription(
OccupancyGrid, 'map', self.map_callback, 10)
def map_callback(self, msg):
graph = self._build_graph(msg)
path = dijkstra_with_costs(
graph,
start=(msg.info.origin.position.x, msg.info.origin.position.y),
goal=self.goal,
cost_fn=cost_function)
self._publish_path(path)
6. 性能优化实战技巧
6.1 启发式改进
虽然Dijkstra是无启发算法，但可以引入可行性剪枝：
python复制if current_node in self.dead_ends:
continue # 提前终止无效分支
6.2 并行计算方案
使用多进程处理不同区域：
python复制from concurrent.futures import ProcessPoolExecutor
def parallel_dijkstra(zones):
with ProcessPoolExecutor() as executor:
results = list(executor.map(solve_zone, zones))
return merge_results(results)
7. 常见问题排查指南
| 问题现象 | 可能原因 | 解决方案 |
|---|---|---|
| 路径突然中断 | 地图连通性断裂 | 检查occupancy_grid的连通区域 |
| 计算时间过长 | 节点数量爆炸 | 采用分层路径规划策略 |
| 路径抖动严重 | 代价函数权重失衡 | 调整turn_cost/slope_cost系数 |
| 避障反应迟钝 | 时间层数不足 | 增加time_layers数组长度 |
8. 算法测试与验证方案
8.1 单元测试设计
python复制def test_obstacle_avoidance():
graph = {
'A': {'B': 1, 'C': float('inf')},
'B': {'D': 1},
'C': {'D': 1},
'D': {}
}
assert dijkstra(graph, 'A')['D'] == 2
8.2 仿真测试流程
- 在CARLA仿真器中构建测试场景
- 注入不同交通密度和障碍物模式
- 收集以下指标：
- 路径规划耗时
- 平均车速
- 急刹车次数
9. 进阶发展方向
9.1 融合机器学习
- 使用GNN预测道路拥堵模式
- 基于强化学习优化代价函数
9.2 多车协同规划
python复制class MultiAgentPlanner:
def resolve_conflicts(self, paths):
for i, path1 in enumerate(paths):
for j, path2 in enumerate(paths[i+1:]):
if self._detect_collision(path1, path2):
paths[j] = self._replan(path2)
return paths
完整项目代码见：github.com/autonomous-path-planning
在实际工程中，我发现当节点数超过1万时，单纯的Dijkstra算法已经难以满足实时性要求。这时可以采用分层规划策略——先用低分辨率地图做全局规划，再在高分辨率局部地图做精细调整。这种"分而治之"的思路往往能带来数量级的性能提升。