1. 路径规划算法概述
在机器人导航领域,路径规划算法扮演着大脑的角色。想象一下你在一个陌生城市开车:GPS提供大方向(全局规划),而你需要实时避开突然出现的行人或车辆(局部调整)。这正是DWA、A*和RRT系列算法协同工作的场景。
算法分类与特点对比:
| 算法类型 | 代表算法 | 适用场景 | 实时性 | 最优性 |
|---|---|---|---|---|
| 全局规划 | A* | 静态环境 | 中等 | 最优 |
| 随机采样 | RRT/RRT* | 高维空间 | 较慢 | 渐进最优 |
| 局部规划 | DWA | 动态环境 | 快速 | 次优 |
实际工程中常采用分层架构:全局规划器生成粗略路径,局部规划器负责实时避障,类似人类"先定大方向,再处理细节"的决策方式。
2. DWA算法深度解析
2.1 动态窗口原理
DWA的核心思想是在速度空间(v, ω)中建立动态窗口,这个窗口的大小由三个关键因素决定:
- 机械限制:电机最大加速度决定的可达速度范围
- 制动距离:当前速度下能安全停止的速度范围
- 障碍物约束:不与障碍物碰撞的速度组合
python复制# 动态窗口计算示例
def calculate_dynamic_window(v, ω, robot_params):
# 机械限制窗口
v_min = max(robot_params.min_v, v - robot_params.max_accel * dt)
v_max = min(robot_params.max_v, v + robot_params.max_accel * dt)
ω_min = max(robot_params.min_ω, ω - robot_params.max_angular_accel * dt)
ω_max = min(robot_params.max_ω, ω + robot_params.max_angular_accel * dt)
# 制动距离窗口
brake_v_max = sqrt(2 * robot_params.max_decel * dist_to_obstacle)
brake_ω_max = sqrt(2 * robot_params.max_angular_decel * angular_dist_to_obstacle)
return (max(v_min, -brake_v_max), min(v_max, brake_v_max),
max(ω_min, -brake_ω_max), min(ω_max, brake_ω_max))
2.2 代价函数设计
优秀的DWA实现需要精心设计多目标代价函数,典型包含:
- 目标导向:方位角偏差的余弦值
- 速度偏好:优先选择更高速度
- 平滑性:相邻周期速度变化惩罚
- 安全距离:与最近障碍物的距离倒数
python复制def evaluate_trajectory(traj, goal, obstacles):
# 目标对准得分(0~1)
heading_score = 0.5*(1 + cos(angle_between(traj[-1], goal)))
# 速度得分(归一化)
velocity_score = traj.velocity / MAX_VELOCITY
# 障碍物距离得分
min_dist = min(calculate_distance(traj, obs) for obs in obstacles)
obstacle_score = 1/(1 + exp(-(min_dist - SAFE_DISTANCE)/DISTANCE_SCALE))
# 平滑得分(速度变化惩罚)
smooth_score = 1 - 0.5*(abs(traj.Δv)/MAX_ACCEL + abs(traj.Δω)/MAX_ANGULAR_ACCEL)
return K1*heading_score + K2*velocity_score + K3*obstacle_score + K4*smooth_score
2.3 工程实践要点
-
参数调优经验:
- 加速度限制应略小于电机实际能力,保留安全余量
- 安全距离建议设为机器人半径的1.5倍
- 代价函数权重需通过实际测试调整
-
常见问题排查:
- 机器人震荡:增大平滑项权重或降低最大加速度
- 陷入局部最优:增加随机采样或引入小幅度扰动
- 反应迟钝:检查动态窗口计算是否过于保守
3. A*算法工程实现
3.1 启发函数选择
不同的启发函数显著影响A*性能:
| 启发函数类型 | 计算公式 | 适用场景 | 是否最优 |
|---|---|---|---|
| 曼哈顿距离 | h | + | |
| 欧几里得距离 | √(x²+y²) | 连续空间 | 是 |
| 对角线距离 | max( | x | , |
python复制# 欧几里得启发函数优化实现
def heuristic(a, b):
dx = abs(a.x - b.x)
dy = abs(a.y - b.y)
return dx + dy + (SQRT2 - 2) * min(dx, dy) # 避免重复开方运算
3.2 数据结构优化
大规模地图中,优先队列性能至关重要:
python复制import heapq
from dataclasses import dataclass, field
from typing import Any
@dataclass(order=True)
class PrioritizedNode:
priority: float
node: Any=field(compare=False)
class PriorityQueue:
def __init__(self):
self._heap = []
self._entry_finder = {}
def add(self, node, priority):
entry = PrioritizedNode(priority, node)
heapq.heappush(self._heap, entry)
def pop(self):
while self._heap:
entry = heapq.heappop(self._heap)
return entry.node
raise KeyError('pop from empty queue')
3.3 实际应用技巧
- 跳点搜索(JPS):利用地图规则性跳过中间节点,提升搜索速度3-10倍
- 分层路径规划:先粗粒度搜索再局部细化,适合超大场景
- 动态重规划:当检测到环境变化时,只更新受影响路径段
在ROS导航栈中,A*的实现通常结合了Voronoi图考虑,使路径尽量远离障碍物
4. RRT系列算法详解
4.1 基本RRT实现要点
python复制class RRTree:
def __init__(self, start):
self.nodes = {start: None} # 节点到父节点的映射
self.kd_tree = KDTree([start]) # 加速最近邻搜索
def extend(self, sample, step_size):
nearest = self.kd_tree.query(sample)[1]
direction = normalize(sample - nearest)
new_point = nearest + direction * min(step_size, distance(nearest, sample))
if not collision(nearest, new_point):
self.nodes[new_point] = nearest
self.kd_tree.insert(new_point)
return new_point
return None
关键参数经验值:
- 步长(step_size):建议为环境尺度的5-10%
- 目标偏置:5-10%概率直接采样目标点
- 终止条件:通常设置最大迭代次数(1e4-1e6)
4.2 RRT*优化原理
RRT*通过两种核心操作提升路径质量:
- 父节点重选:
python复制def choose_parent(new_node, neighbors, tree):
min_cost = float('inf')
best_parent = None
for node in neighbors:
cost = tree.cost[node] + distance(node, new_node)
if cost < min_cost and not collision(node, new_node):
min_cost = cost
best_parent = node
if best_parent:
tree.set_parent(new_node, best_parent)
- 重布线:
python复制def rewire(new_node, neighbors, tree):
for node in neighbors:
new_cost = tree.cost[new_node] + distance(new_node, node)
if new_cost < tree.cost[node]:
if not collision(new_node, node):
tree.set_parent(node, new_node)
4.3 三维扩展实现
对于无人机等三维场景,需要修改几个关键部分:
- 状态表示增加z坐标
- 距离计算使用三维欧式距离
- 碰撞检测考虑三维体素或OBB碰撞
python复制class RRT3D(RRT):
def __init__(self, start):
self.bounds = (min_x, max_x, min_y, max_y, min_z, max_z)
def sample(self):
return (uniform(*self.bounds[0:2]),
uniform(*self.bounds[2:4]),
uniform(*self.bounds[4:6]))
def distance(a, b):
return sqrt((a.x-b.x)**2 + (a.y-b.y)**2 + (a.z-b.z)**2)
5. 多算法融合实践
5.1 DWA与RRT*协同架构
典型分层架构实现:
python复制class HybridPlanner:
def __init__(self):
self.global_plan = None
self.local_target = None
def update_global_plan(self, start, goal):
self.global_plan = rrt_star(start, goal)
self.local_target = self.global_plan.pop(0)
def update_local_plan(self, pose, obstacles):
if distance(pose, self.local_target) < THRESHOLD:
if self.global_plan:
self.local_target = self.global_plan.pop(0)
else:
return None
return dwa(pose, self.local_target, obstacles)
5.2 自适应切换策略
根据环境动态性自动调整算法:
| 环境状态 | 全局规划频率 | 局部规划频率 | 主要算法 |
|---|---|---|---|
| 静态 | 1Hz | 10Hz | A* + DWA |
| 动态 | 0.1Hz | 20Hz | RRT* + DWA |
| 高度动态 | 仅初始 | 30Hz | 纯DWA |
5.3 实际部署问题
- 坐标系对齐:确保全局地图与局部传感器数据坐标系一致
- 时序同步:处理算法计算延迟导致的位姿不同步
- 异常处理:当局部规划失败时,触发紧急停止或全局重规划
6. 自定义地图系统实现
6.1 地图数据结构设计
python复制class OccupancyGrid:
def __init__(self, width, height, resolution):
self.width = width # 网格列数
self.height = height # 网格行数
self.resolution = resolution # 米/格
self.data = np.zeros((height, width), dtype=np.uint8)
def world_to_map(self, x, y):
return (int(x / self.resolution), int(y / self.resolution))
def is_occupied(self, x, y):
mx, my = self.world_to_map(x, y)
return self.data[my][mx] > OCCUPIED_THRESHOLD
6.2 地图生成方法
- 手动编辑:使用工具如PGM Editor创建基础地图
- SLAM生成:通过ROS的gmapping或cartographer构建
- 程序化生成:使用Perlin噪声等算法创建随机环境
6.3 地图预处理技巧
- 膨胀处理:将障碍物膨胀机器人半径,避免碰撞检测误差
python复制def inflate_obstacles(grid, radius):
kernel_size = int(radius / grid.resolution) * 2 + 1
kernel = np.ones((kernel_size, kernel_size))
dilated = cv2.dilate(grid.data, kernel)
return dilated > INFLATION_THRESHOLD
- 路径平滑:对原始路径应用样条插值或梯度下降平滑
- 多分辨率处理:大范围用低分辨率,关键区域用高精度
7. 性能优化与评测
7.1 基准测试设计
python复制class Benchmark:
def __init__(self, map_size, obstacle_density):
self.test_cases = self.generate_test_cases(map_size, obstacle_density)
def run_test(self, planner):
results = []
for start, goal, obstacles in self.test_cases:
t_start = time.time()
path = planner.plan(start, goal, obstacles)
planning_time = time.time() - t_start
if path:
path_length = calculate_path_length(path)
smoothness = calculate_smoothness(path)
clearance = calculate_clearance(path, obstacles)
else:
path_length = float('inf')
results.append((planning_time, path_length, smoothness, clearance))
return results
7.2 各算法对比数据
测试环境:i7-11800H, 16GB RAM, 100次运行平均值
| 算法 | 成功率 | 平均耗时(ms) | 路径长度(m) | 平滑度(°) |
|---|---|---|---|---|
| A* | 100% | 45.2 | 12.34 | 8.7 |
| RRT | 92% | 128.7 | 14.56 | 15.2 |
| RRT* | 88% | 253.4 | 12.78 | 10.5 |
| DWA | 100% | 8.2 | 13.21 | 6.3 |
| 融合 | 100% | 32.1 | 12.45 | 7.1 |
7.3 优化技巧汇编
- 并行计算:将采样、碰撞检测等步骤并行化
- 近似查询:使用KD树等加速最近邻搜索
- 增量更新:环境变化时复用已有搜索树
- 内存池:预分配节点内存减少动态分配开销
在真实机器人项目中,我们最终采用的方案是:
- 全局层:改进A*(带JPS优化)
- 局部层:自适应DWA(根据障碍物密度调整窗口大小)
- 异常处理:当连续3次规划失败时切换为随机探索模式