自动驾驶算法基础：路径规划与SLAM
深入理解A*、Dijkstra、RRT路径规划算法，以及SLAM定位建图原理，配合Python代码实例帮你建立自动驾驶的核心知识框架。
自动驾驶 SLAM 路径规划 算法
自动驾驶算法基础：路径规划与SLAM
自动驾驶的核心问题可以归结为两个：我在哪？（定位与建图，即SLAM）和怎么去？（路径规划）。无论你是在做DonkeyCar的循线自动驾驶，还是JetBot的室内导航，这两个问题都绑在一起。本文用代码把核心算法讲透。
路径规划算法概览
Dijkstra算法——最短路径的基石
Dijkstra是经典的最短路径算法，保证找到全局最优解，但计算量大，适合小规模地图。
import heapq
import numpy as np
def dijkstra(grid, start, goal):
"""
grid: 2D数组，0=可通行，1=障碍物
start/goal: (row, col) 元组
返回: 最短路径坐标列表
"""
rows, cols = grid.shape
# 优先队列: (cost, row, col, path)
pq = [(0, start[0], start[1], [start])]
visited = set()
while pq:
cost, r, c, path = heapq.heappop(pq)
if (r, c) == goal:
return path
if (r, c) in visited:
continue
visited.add((r, c))
# 四方向移动
for dr, dc in [(-1,0), (1,0), (0,-1), (0,1)]:
nr, nc = r + dr, c + dc
if 0 <= nr < rows and 0 <= nc < cols and grid[nr][nc] == 0:
if (nr, nc) not in visited:
heapq.heappush(pq, (cost + 1, nr, nc, path + [(nr, nc)]))
return None # 无路径
# 测试
grid = np.array([
[0, 0, 0, 1, 0],
[0, 1, 0, 1, 0],
[0, 1, 0, 0, 0],
[0, 0, 0, 1, 0],
[0, 1, 0, 0, 0]
])
path = dijkstra(grid, (0, 0), (4, 4))
print(f"路径长度: {len(path)}, 路径: {path}")A*算法——带启发函数的Dijkstra
A*通过引入启发函数h(n)（通常是到目标的欧氏距离或曼哈顿距离），大幅减少搜索空间：
import heapq
import numpy as np
def heuristic(a, b):
"""曼哈顿距离"""
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def a_star(grid, start, goal):
rows, cols = grid.shape
# (f_score, g_score, row, col, path)
open_set = [(heuristic(start, goal), 0, start[0], start[1], [start])]
g_scores = {start: 0}
visited = set()
while open_set:
f, g, r, c, path = heapq.heappop(open_set)
current = (r, c)
if current == goal:
return path
if current in visited:
continue
visited.add(current)
for dr, dc in [(-1,0), (1,0), (0,-1), (0,1)]:
nr, nc = r + dr, c + dc
neighbor = (nr, nc)
if 0 <= nr < rows and 0 <= nc < cols and grid[nr][nc] == 0:
new_g = g + 1
if neighbor not in g_scores or new_g < g_scores[neighbor]:
g_scores[neighbor] = new_g
f = new_g + heuristic(neighbor, goal)
heapq.heappush(open_set, (f, new_g, nr, nc, path + [neighbor]))
return NoneA*在网格地图上的效率通常是Dijkstra的3-5倍，是2D导航的首选。
RRT（快速随机搜索树）——高维空间的利器
当环境复杂或维度很高（比如考虑车辆运动学约束），RRT比网格搜索更实用：
import numpy as np
import random
class RRT:
def __init__(self, start, goal, bounds, obstacles, step_size=0.5, max_iter=5000):
self.start = np.array(start)
self.goal = np.array(goal)
self.bounds = bounds # [(x_min, x_max), (y_min, y_max)]
self.obstacles = obstacles # [(cx, cy, radius), ...]
self.step_size = step_size
self.max_iter = max_iter
self.tree = {tuple(start): None}
def nearest(self, point):
nodes = list(self.tree.keys())
dists = [np.linalg.norm(np.array(n) - point) for n in nodes]
return nodes[np.argmin(dists)]
def is_collision_free(self, p1, p2):
"""检查线段是否与障碍物碰撞"""
for cx, cy, r in self.obstacles:
# 线段到圆心的最短距离
d = p2 - p1
t = max(0, min(1, np.dot(np.array([cx,cy]) - p1, d) / np.dot(d, d)))
proj = p1 + t * d
if np.linalg.norm(proj - np.array([cx, cy])) < r:
return False
return True
def plan(self):
for _ in range(self.max_iter):
# 随机采样（10%概率采样目标点加速收敛）
if random.random() < 0.1:
rand_point = self.goal
else:
rand_point = np.array([
random.uniform(self.bounds[0][0], self.bounds[0][1]),
random.uniform(self.bounds[1][0], self.bounds[1][1])
])
near_node = self.nearest(rand_point)
direction = rand_point - np.array(near_node)
dist = np.linalg.norm(direction)
if dist == 0:
continue
new_point = np.array(near_node) + direction / dist * self.step_size
if self.is_collision_free(near_node, new_point):
self.tree[tuple(new_point)] = near_node
# 检查是否到达目标
if np.linalg.norm(new_point - self.goal) < self.step_size:
return self._extract_path(new_point)
return None
def _extract_path(self, end):
path = [end]
node = end
while self.tree[tuple(node)] is not None:
node = self.tree[tuple(node)]
path.append(node)
return path[::-1]
# 使用示例
obstacles = [(3, 3, 1), (5, 5, 1.5), (7, 2, 1)]
rrt = RRT(start=(0, 0), goal=(9, 9), bounds=[(0, 10), (0, 10)], obstacles=obstacles)
path = rrt.plan()
print(f"找到路径，共{len(path)}个点" if path else "未找到路径")SLAM：同时定位与建图
SLAM（Simultaneous Localization and Mapping）解决的是”机器人在未知环境中，一边建图一边确定自己位置”的问题。
SLAM的核心循环
1. 预测（Prediction）：根据里程计/IMU数据，估算机器人新位置
2. 观测（Observation）：用传感器（激光雷达、摄像头）观测环境特征
3. 更新（Update）：将观测与地图匹配，修正位姿估计
4. 建图（Mapping）：将新观测到的特征加入地图常用SLAM方案对比
| 方案 | 传感器 | 适用场景 | 计算量 |
|---|---|---|---|
| GMapping | 2D激光雷达 | 室内平面环境 | 低 |
| Cartographer | 2D/3D激光雷达 | 室内外通用 | 中 |
| ORB-SLAM3 | 单目/双目/RGB-D相机 | 视觉导航 | 高 |
| RTAB-Map | RGB-D或立体相机 | 3D建图 | 高 |
在ROS2中使用SLAM Toolbox
# 安装
sudo apt install ros-humble-slam-toolbox -y
# 启动SLAM（配合激光雷达节点）
ros2 launch slam_toolbox online_async_launch.py \
params_file:=~/my_slam_params.yaml \
use_sim_time:=false典型参数配置：
slam_toolbox:
ros__parameters:
solver_plugin: solver_plugins::CeresSolver
ceres_solver_type: ceres::DenseNormalCholesky
mode: mapping # 或 localization（纯定位模式）
debug_logging: false
transform_timeout: 0.2
map_frame: map
odom_frame: odom
base_frame: base_link
scan_topic: /scan
map_file_name: ~/my_map
map_start_pose: [0.0, 0.0, 0.0]用Python实现简单的粒子滤波定位
粒子滤波是SLAM中定位部分的经典方法：
import numpy as np
class ParticleFilter:
def __init__(self, n_particles, map_bounds):
self.n = n_particles
# 随机初始化粒子 [x, y, theta, weight]
self.particles = np.zeros((n_particles, 4))
self.particles[:, 0] = np.random.uniform(map_bounds[0], map_bounds[1], n_particles) # x
self.particles[:, 1] = np.random.uniform(map_bounds[2], map_bounds[3], n_particles) # y
self.particles[:, 2] = np.random.uniform(-np.pi, np.pi, n_particles) # theta
self.particles[:, 3] = 1.0 / n_particles # weight
def predict(self, velocity, angular_vel, dt, noise=[0.1, 0.1, 0.05]):
"""根据运动模型预测新位置"""
self.particles[:, 0] += velocity * np.cos(self.particles[:, 2]) * dt + \
np.random.normal(0, noise[0], self.n)
self.particles[:, 1] += velocity * np.sin(self.particles[:, 2]) * dt + \
np.random.normal(0, noise[1], self.n)
self.particles[:, 2] += angular_vel * dt + \
np.random.normal(0, noise[2], self.n)
def update(self, landmarks, measurements):
"""根据观测更新粒子权重"""
for i, p in enumerate(self.particles):
likelihood = 1.0
for meas_dist, meas_angle, lm_id in measurements:
lm = landmarks[lm_id]
expected_dist = np.sqrt((p[0]-lm[0])**2 + (p[1]-lm[1])**2)
expected_angle = np.arctan2(lm[1]-p[1], lm[0]-p[0]) - p[2]
# 高斯似然
dist_err = abs(meas_dist - expected_dist)
likelihood *= np.exp(-dist_err**2 / (2 * 0.5**2))
self.particles[i, 3] = likelihood
# 归一化权重
total = self.particles[:, 3].sum()
if total > 0:
self.particles[:, 3] /= total
def resample(self):
"""系统重采样"""
cumsum = np.cumsum(self.particles[:, 3])
positions = (np.arange(self.n) + np.random.random()) / self.n
indices = np.searchsorted(cumsum, positions)
self.particles = self.particles[indices].copy()
self.particles[:, 3] = 1.0 / self.n
def estimate(self):
"""返回加权平均位姿"""
return np.average(self.particles[:, :3], weights=self.particles[:, 3], axis=0)在模型车项目中的实践建议
- 先跑通2D激光SLAM：用RPLidar A1 + ROS2 slam_toolbox，在客厅就能建图
- 路径规划用Nav2：它封装了A*/Dijkstra + DWA局部规划器，开箱即用
- 视觉SLAM作为进阶：当你有了双目摄像头或RGB-D相机，再尝试ORB-SLAM3
- 仿真先行：用Gazebo或Webots仿真验证算法，再部署到实车
路径规划和SLAM是自动驾驶的两大支柱。理解它们的原理后，你就能针对自己的模型车场景选择合适的方案，而不是盲目堆砌传感器。