第18章 全局路径规划
课程:ROS2 Python 编程
章节:第18章
课时:2 课时(90 分钟)
教学方式:讲授 + 演示
学习目标
本章学习目标包括:理解Dijkstra算法和A*算法的原理与实现,掌握Nav2 Smac Planner的工作原理,熟悉全局路径规划的代价函数设计,能够配置和切换不同的全局规划器,掌握路径优化和平滑技术。
18.1 全局路径规划概述
18.1.1 问题定义
全局路径规划是在已知环境地图中,寻找从起点到终点的无碰撞最优路径。
**输入:**环境地图(代价地图)、起点位姿 (x_start, y_start, θ_start) 和终点位姿 (x_goal, y_goal, θ_goal)。
**输出:**路径,即一系列连续的位姿点 {(x₁,y₁,θ₁), ..., (xₙ,yₙ,θₙ)}。
优化目标:
path* = argmin f(path)
其中 f(path) 通常为路径总长度,也可包含其他约束(如转弯代价、安全距离)。
18.1.2 路径规划的评价指标
| 指标 | 含义 | 重要性 |
|---|---|---|
| 路径长度 | 从起点到终点的总距离 | 高 |
| 计算时间 | 规划所需时间 | 高(实时性) |
| 平滑度 | 路径曲率变化 | 中 |
| 安全距离 | 距离障碍物的最近距离 | 高 |
| 完备性 | 是否存在可行解 | 高 |
18.2 Dijkstra算法
18.2.1 算法原理
Dijkstra算法是一种广度优先的最短路径搜索算法,它从起点开始,逐步扩展,保证找到最短路径。
**核心思想:**维护从起点到每个节点的最短距离 G(n),每次选择 G(n) 最小的节点进行扩展,直到扩展到目标点。
伪代码:
1. 初始化:
- open_list = {start}
- closed_list = {}
- g(start) = 0, g(其他) = ∞
2. while open_list 不为空:
3. 从 open_list 中取出 g 值最小的节点 n
4. if n == goal: 回溯路径,返回成功
5. 将 n 移入 closed_list
6. 遍历 n 的相邻节点 m:
7. if m in closed_list: continue
8. if m 是障碍物: continue
9. new_g = g(n) + cost(n, m)
10. if new_g < g(m):
11. g(m) = new_g
12. parent(m) = n
13. if m not in open_list: 加入 open_list
3. return 失败 (无可行路径)
18.2.2 Python实现
import numpy as np
import heapq
from enum import Enum
class CellState(Enum):
FREE = 0
OBSTACLE = 1
START = 2
GOAL = 3
PATH = 4
class DijkstraPlanner:
"""Dijkstra全局路径规划器"""
def __init__(self, costmap: np.ndarray, resolution: float = 0.05):
self.costmap = costmap
self.resolution = resolution
self.height, self.width = costmap.shape
# 8邻域连接
self.neighbors = [
(-1, -1), (-1, 0), (-1, 1),
(0, -1), (0, 1),
(1, -1), (1, 0), (1, 1)
]
self.movement_costs = [
np.sqrt(2), 1.0, np.sqrt(2),
1.0, 1.0,
np.sqrt(2), 1.0, np.sqrt(2)
]
def world_to_grid(self, wx: float, wy: float) -> tuple:
"""世界坐标转栅格坐标"""
# 假设地图原点在 (0, 0)
return int(wy / self.resolution), int(wx / self.resolution)
def grid_to_world(self, row: int, col: int) -> tuple:
"""栅格坐标转世界坐标"""
return col * self.resolution, row * self.resolution
def plan(self, start_world: tuple, goal_world: tuple) -> list:
"""
执行Dijkstra路径规划
参数:
start_world: (x, y) 起点世界坐标
goal_world: (x, y) 终点世界坐标
返回:
path: [(x1,y1), (x2,y2), ...] 世界坐标路径点
"""
start = self.world_to_grid(*start_world)
goal = self.world_to_grid(*goal_world)
if not self._is_valid(start) or not self._is_valid(goal):
print("起点或终点无效")
return []
# 初始化
g_score = np.full((self.height, self.width), np.inf)
g_score[start] = 0
parent = {}
# 优先队列: (g_score, (row, col))
open_heap = [(0, start)]
closed_set = set()
while open_heap:
# 取出g值最小的节点
current_g, current = heapq.heappop(open_heap)
if current in closed_set:
continue
# 到达目标
if current == goal:
return self._reconstruct_path(parent, start, goal)
closed_set.add(current)
# 扩展邻居
for i, (dr, dc) in enumerate(self.neighbors):
neighbor = (current[0] + dr, current[1] + dc)
if neighbor in closed_set:
continue
if not self._is_valid(neighbor):
continue
# 代价 = 移动代价 + 地图代价
movement_cost = self.movement_costs[i]
map_cost = self._get_map_cost(neighbor)
step_cost = movement_cost * (1.0 + map_cost / 50.0)
tentative_g = current_g + step_cost
if tentative_g < g_score[neighbor]:
g_score[neighbor] = tentative_g
parent[neighbor] = current
heapq.heappush(open_heap, (tentative_g, neighbor))
return [] # 无可行路径
def _is_valid(self, cell: tuple) -> bool:
"""检查栅格是否有效"""
r, c = cell
if not (0 <= r < self.height and 0 <= c < self.width):
return False
return self.costmap[r, c] < 254 # 非障碍物
def _get_map_cost(self, cell: tuple) -> float:
"""获取地图代价值"""
r, c = cell
cost = self.costmap[r, c]
if cost >= 254:
return float('inf')
return float(cost)
def _reconstruct_path(self, parent: dict, start: tuple,
goal: tuple) -> list:
"""回溯重建路径"""
path_grid = []
current = goal
while current != start:
path_grid.append(current)
current = parent.get(current)
if current is None:
return []
path_grid.append(start)
path_grid.reverse()
# 转换为世界坐标
path_world = [self.grid_to_world(r, c) for r, c in path_grid]
return path_world18.2.3 Dijkstra的特点
**优点:**保证找到最短路径,算法完备(有解必能找到),实现简单。
**缺点:**搜索范围大、效率低,不考虑目标位置而向所有方向等距离扩展,在大地图上计算时间过长。
18.3 A*算法
18.3.1 算法原理
A*算法在Dijkstra的基础上加入启发式函数,引导搜索方向,提高效率。
评价函数:
F(n) = G(n) + H(n)
其中 G(n) 表示起点到当前节点n的实际代价,H(n) 表示当前节点n到目标点的启发式估计代价。
启发式函数的选择:
def manhattan_distance(a: tuple, b: tuple) -> float:
"""曼哈顿距离(四邻域连接时使用)"""
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def euclidean_distance(a: tuple, b: tuple) -> float:
"""欧氏距离(八邻域连接时使用)"""
return np.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
def chebyshev_distance(a: tuple, b: tuple) -> float:
"""切比雪夫距离"""
return max(abs(a[0] - b[0]), abs(a[1] - b[1]))
def diagonal_distance(a: tuple, b: tuple, d1: float = 1.0,
d2: float = np.sqrt(2)) -> float:
"""对角线距离(考虑对角线移动)"""
dx = abs(a[0] - b[0])
dy = abs(a[1] - b[1])
return d1 * (dx + dy) + (d2 - 2 * d1) * min(dx, dy)18.3.2 Python实现
import heapq
import numpy as np
class AStarPlanner:
"""A*路径规划器"""
def __init__(self, costmap: np.ndarray, resolution: float = 0.05):
self.costmap = costmap
self.resolution = resolution
self.height, self.width = costmap.shape
# 8邻域
self.neighbors = [
(-1, -1), (-1, 0), (-1, 1),
(0, -1), (0, 1),
(1, -1), (1, 0), (1, 1)
]
self.movement_costs = [
np.sqrt(2), 1.0, np.sqrt(2),
1.0, 1.0,
np.sqrt(2), 1.0, np.sqrt(2)
]
def plan(self, start_world: tuple, goal_world: tuple) -> list:
"""执行A*路径规划"""
start = self._world_to_grid(*start_world)
goal = self._world_to_grid(*goal_world)
if not self._is_valid(start) or not self._is_valid(goal):
return []
g_score = {start: 0.0}
f_score = {start: self._heuristic(start, goal)}
open_heap = [(f_score[start], start)]
open_set = {start}
closed_set = set()
parent = {}
while open_heap:
_, current = heapq.heappop(open_heap)
if current == goal:
return self._reconstruct_path(parent, start, goal)
open_set.discard(current)
closed_set.add(current)
for i, (dr, dc) in enumerate(self.neighbors):
neighbor = (current[0] + dr, current[1] + dc)
if neighbor in closed_set:
continue
if not self._is_valid(neighbor):
continue
move_cost = self.movement_costs[i]
map_cost = self._get_map_cost(neighbor)
step_cost = move_cost * (1.0 + map_cost / 50.0)
tentative_g = g_score[current] + step_cost
if neighbor not in g_score or tentative_g < g_score[neighbor]:
parent[neighbor] = current
g_score[neighbor] = tentative_g
f_score[neighbor] = tentative_g + self._heuristic(neighbor, goal)
if neighbor not in open_set:
heapq.heappush(open_heap, (f_score[neighbor], neighbor))
open_set.add(neighbor)
return []
def _heuristic(self, cell: tuple, goal: tuple) -> float:
"""启发式函数:对角线距离"""
dr = abs(cell[0] - goal[0])
dc = abs(cell[1] - goal[1])
return (1.0 * (dr + dc) + (np.sqrt(2) - 2 * 1.0) * min(dr, dc))
def _world_to_grid(self, wx: float, wy: float) -> tuple:
return int(wy / self.resolution), int(wx / self.resolution)
def _grid_to_world(self, row: int, col: int) -> tuple:
return col * self.resolution, row * self.resolution
def _is_valid(self, cell: tuple) -> bool:
r, c = cell
if not (0 <= r < self.height and 0 <= c < self.width):
return False
return self.costmap[r, c] < 254
def _get_map_cost(self, cell: tuple) -> float:
r, c = cell
cost = self.costmap[r, c]
return float('inf') if cost >= 254 else float(cost)
def _reconstruct_path(self, parent: dict, start: tuple,
goal: tuple) -> list:
path_grid = []
current = goal
while current != start:
path_grid.append(current)
current = parent.get(current)
if current is None:
return []
path_grid.append(start)
path_grid.reverse()
return [self._grid_to_world(r, c) for r, c in path_grid]18.3.3 A* vs Dijkstra
def compare_algorithms(costmap: np.ndarray,
start: tuple, goal: tuple):
"""对比Dijkstra和A*的性能"""
import time
# Dijkstra
dijkstra = DijkstraPlanner(costmap)
t1 = time.time()
path_d = dijkstra.plan(start, goal)
t_d = time.time() - t1
# A*
astar = AStarPlanner(costmap)
t1 = time.time()
path_a = astar.plan(start, goal)
t_a = time.time() - t1
print(f"Dijkstra: {len(path_d)}点, {t_d*1000:.1f}ms")
print(f"A*: {len(path_a)}点, {t_a*1000:.1f}ms")
if path_d and path_a:
len_d = sum(np.linalg.norm(np.array(path_d[i+1]) - np.array(path_d[i]))
for i in range(len(path_d)-1))
len_a = sum(np.linalg.norm(np.array(path_a[i+1]) - np.array(path_a[i]))
for i in range(len(path_a)-1))
print(f"Dijkstra路径长度: {len_d:.2f}m")
print(f"A*路径长度: {len_a:.2f}m")18.3.4 官方要点——A* 的理论根基与启发式可采纳性
A* 出自 Hart、Nilsson 与 Raphael 1968 年在 SRI 的论文《A Formal Basis for the Heuristic Determination of Minimum Cost Paths》,其核心结论是:启发式 h(n) 若"可采纳"(不高估真实代价,admissible),A* 保证找到最优路径;若再满足一致性,则首次扩展节点即为最优。欧氏距离是 8 邻域可采纳启发式,而切比雪夫距离或对角线加权(如本章 18.3.2 的 √2 对角代价)在 8 邻域中更贴合实际步长,能显著减少无效扩展——这正是练习第 1 题的理论支撑。
官方文档同时提醒:在代价地图上 A* 的"最优"只是相对栅格代价的最优,若膨胀层 cost_scaling_factor 配置不当,规划器会为绕开低代价区走出怪异长路;调路径形状应先调代价地图,再考虑换规划器。
18.4 Nav2 Smac Planner
18.4.1 Smac Planner概述
Smac Planner 是Nav2内置的高级规划器,支持多种规划算法:
| 规划器 | 算法 | 特点 | 适用场景 |
|---|---|---|---|
| NavfnPlanner | 导航函数 | 经典算法,路径平滑 | 简单环境 |
| SmacPlanner2D | A* | 标准A*在2D代价地图 | 通用场景 |
| SmacPlannerHybrid | Hybrid-A* | 带运动学约束 | 非全向机器人 |
| SmacPlannerLattice | 状态格 | 满足动力学约束 | 阿克曼转向 |
| ThetaStarPlanner | Theta* | 任意角度路径 | 全向机器人 |
18.4.2 Hybrid-A*算法
Hybrid-A* 在标准A*基础上加入运动学约束,适用于非全向移动机器人。
**与A*的区别:**状态空间从 (x, y) 扩展到 (x, y, θ),考虑最小转弯半径,支持前进和后退,并使用Reeds-Shepp曲线连接。
import numpy as np
import heapq
class HybridAStarNode:
"""Hybrid-A*节点"""
def __init__(self, x: float, y: float, theta: float):
self.x = x
self.y = y
self.theta = theta
self.g = float('inf')
self.f = float('inf')
self.parent = None
def __lt__(self, other):
return self.f < other.f
class HybridAStarPlanner:
"""Hybrid-A*路径规划器(简化版)"""
def __init__(self, costmap: np.ndarray,
min_turning_radius: float = 0.5,
resolution: float = 0.05):
self.costmap = costmap
self.resolution = resolution
self.min_turning_radius = min_turning_radius
# 运动控制采样(速度+转角)
self.motion_primitives = self._generate_motion_primitives()
def _generate_motion_primitives(self) -> list:
"""生成运动基元"""
primitives = []
# 前进
for steer in [-0.4, -0.2, 0.0, 0.2, 0.4]:
primitives.append({
'v': 0.3, # 前进速度
'steer': steer, # 转角
'dt': 0.5, # 时间步长
'gear': 1 # 前进
})
# 后退
for steer in [-0.2, 0.0, 0.2]:
primitives.append({
'v': -0.2,
'steer': steer,
'dt': 0.3,
'gear': -1
})
return primitives
def _simulate_motion(self, x: float, y: float, theta: float,
primitive: dict) -> tuple:
"""模拟运动基元"""
v = primitive['v']
steer = primitive['steer']
dt = primitive['dt']
# 自行车模型
new_x = x + v * np.cos(theta) * dt
new_y = y + v * np.sin(theta) * dt
new_theta = theta + v * np.tan(steer) / 0.5 * dt
return new_x, new_y, new_theta
def _is_collision_free(self, x: float, y: float) -> bool:
"""检查位姿是否无碰撞"""
gx = int(y / self.resolution)
gy = int(x / self.resolution)
if not (0 <= gx < self.costmap.shape[0] and
0 <= gy < self.costmap.shape[1]):
return False
# 检查足迹范围内是否有障碍物
robot_radius_cells = int(0.22 / self.resolution)
for dr in range(-robot_radius_cells, robot_radius_cells + 1):
for dc in range(-robot_radius_cells, robot_radius_cells + 1):
if dr*dr + dc*dc <= robot_radius_cells*robot_radius_cells:
nr, nc = gx + dr, gy + dc
if (0 <= nr < self.costmap.shape[0] and
0 <= nc < self.costmap.shape[1]):
if self.costmap[nr, nc] >= 254:
return False
return True
def plan(self, start: tuple, goal: tuple) -> list:
"""
执行Hybrid-A*规划
参数:
start: (x, y, theta)
goal: (x, y, theta)
"""
start_node = HybridAStarNode(*start)
goal_node = HybridAStarNode(*goal)
# 离散化分辨率
xy_resolution = 0.2 # 比代价地图粗
theta_resolution = np.radians(15)
# 离散化状态空间
def discretize(x, y, theta):
dx = int(x / xy_resolution)
dy = int(y / xy_resolution)
dt = int(theta / theta_resolution)
return (dx, dy, dt)
start_node.g = 0.0
start_node.f = self._heuristic(start, goal)
open_heap = [(start_node.f, id(start_node), start_node)]
open_dict = {discretize(*start): start_node}
closed_set = set()
while open_heap:
_, _, current = heapq.heappop(open_heap)
current_key = discretize(current.x, current.y, current.theta)
if current_key in closed_set:
continue
# 检查是否到达目标
dist_to_goal = np.sqrt((current.x - goal[0])**2 +
(current.y - goal[1])**2)
if dist_to_goal < 0.5:
# 使用Reeds-Shepp曲线连接
return self._reconstruct_path(current)
closed_set.add(current_key)
# 扩展运动基元
for primitive in self.motion_primitives:
new_x, new_y, new_theta = self._simulate_motion(
current.x, current.y, current.theta, primitive
)
# 碰撞检测
if not self._is_collision_free(new_x, new_y):
continue
new_key = discretize(new_x, new_y, new_theta)
if new_key in closed_set:
continue
# 计算代价
motion_cost = abs(primitive['v']) * primitive['dt']
steering_penalty = abs(primitive['steer']) * 0.5
step_cost = motion_cost + steering_penalty
new_g = current.g + step_cost
if new_key not in open_dict or new_g < open_dict[new_key].g:
new_node = HybridAStarNode(new_x, new_y, new_theta)
new_node.g = new_g
new_node.f = new_g + self._heuristic(
(new_x, new_y), (goal[0], goal[1])
)
new_node.parent = current
open_dict[new_key] = new_node
heapq.heappush(open_heap, (new_node.f, id(new_node), new_node))
return []
def _heuristic(self, a: tuple, b: tuple) -> float:
"""启发式函数:欧氏距离"""
return np.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
def _reconstruct_path(self, node: HybridAStarNode) -> list:
"""重建路径"""
path = []
current = node
while current:
path.append((current.x, current.y, current.theta))
current = current.parent
path.reverse()
return path18.4.3 Nav2规划器配置
# 规划器配置
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5 # 目标容忍距离
use_astar: true # 使用A*加速
allow_unknown: false # 不允许未知区域
# Smac Planner Hybrid可选配置
# GridBased:
# plugin: "nav2_smac_planner/SmacPlannerHybrid"
# tolerance: 0.5
# max_planning_time: 2.0 # 最大规划时间(s)
# minimum_turning_radius: 0.2 # 最小转弯半径(m)
# reverse_penalty: 2.0 # 后退惩罚
# change_penalty: 0.5 # 方向变化惩罚
# non_straight_penalty: 1.1 # 非直线惩罚18.4.4 规划器切换
# 查看当前使用的规划器
ros2 param get /planner_server planner_plugins
# 动态切换规划器
ros2 param set /planner_server planner_plugins "['SmacPlanner']"
# 重新配置后需要重新规划18.4.5 官方要点——Nav2 官方规划器全家福与选型指引
官方 Planner Tables 页面对比了四类内置全局规划器:NavFn(Dijkstra/A* 的代价地图搜索实现,速度优先、质量一般)、Smac 2D(栅格 A*,支持 4/8 邻域,比 NavFn 更省内存)、Smac Hybrid-A*(运动学约束,供阿克曼/有最小转弯半径的底盘使用)、Smac Lattice(状态栅格法,用预生成运动基元搜索,适合差速与全向机器人)。文档给出的选型原则与本章 18.4 一致:圆形差速底盘用 NavFn/Smac 2D 足够;需考虑朝向与转弯半径时才引入 Hybrid-A*;Lattice 在要求路径可精确复现时最合适。
nav2_navfn_planner 文档还说明了 tolerance 参数的语义:目标不可达时按该半径搜索"最近可达点"并直接规划过去,这与本章 18.6.2 的 GridBased.tolerance 命令行示例对应。
18.4.6 官方要点——Smac Hybrid-A* 的运动学细节
nav2_smac_planner 文档对 Hybrid-A* 给出了与本章 18.4.2 呼应的参数语义:motion_model_for_search_type 选择 DUBIN(前向 Only)/REEDS_SHEPP(允许倒车)等运动模型;minimum_turning_radius 决定可行运动基元的曲率下限,设置过小会产生底盘物理上转不出来的急弯;reverse_penalty、change_penalty、non_straight_penalty 分别惩罚倒车、朝向突变与非直行段,是"路径像不像人开车"的直接旋钮。文档建议的调参顺序是:先把 analytic_expansion_ratio 与 analytic_expansion_max_length 调到能稳定生成解析端点(Dubins/Reeds-Shepp 直连段),再逐步收紧其余惩罚项。
Smac Lattice 则依赖离线生成的运动基元(Motion Primitives)文件,基元分辨率必须与 costmap 分辨率匹配,这是配置时最常见的坑。
18.5 路径优化
18.5.1 路径平滑
路径平滑去除路径中的锐角转折:
import numpy as np
from scipy import interpolate
class PathSmoother:
"""路径平滑器"""
def __init__(self, weight_data: float = 0.5,
weight_smooth: float = 0.3,
weight_length: float = 0.2):
self.weight_data = weight_data
self.weight_smooth = weight_smooth
self.weight_length = weight_length
def smooth(self, path: np.ndarray,
num_iterations: int = 100) -> np.ndarray:
"""
梯度下降路径平滑
参数:
path: (N, 2) 原始路径点
num_iterations: 迭代次数
"""
new_path = path.copy()
n = len(path)
for _ in range(num_iterations):
for i in range(1, n - 1):
# 保持接近原始点
d_data = self.weight_data * (path[i] - new_path[i])
# 平滑项(与邻居的差距)
d_smooth = self.weight_smooth * (
new_path[i-1] + new_path[i+1] - 2 * new_path[i]
)
# 长度约束(均匀间距)
d_length = self.weight_length * (
new_path[i-1] - 2 * new_path[i] + new_path[i+1]
)
new_path[i] += d_data + d_smooth + d_length
return new_path
def b_spline_smooth(self, path: np.ndarray,
num_points: int = 100) -> np.ndarray:
"""B样条平滑"""
n = len(path)
t = np.linspace(0, 1, n)
t_new = np.linspace(0, 1, num_points)
# B样条插值
tck, _ = interpolate.splprep([path[:, 0], path[:, 1]], s=0)
smooth_points = interpolate.splev(t_new, tck)
return np.column_stack(smooth_points)18.5.2 路径修剪
class PathOptimizer:
"""路径优化器"""
def __init__(self):
pass
def remove_redundant_points(self, path: np.ndarray,
epsilon: float = 0.1) -> np.ndarray:
"""
移除冗余路径点(Douglas-Peucker算法)
"""
if len(path) < 3:
return path
# 找最远点
start, end = path[0], path[-1]
line_vec = end - start
line_len = np.linalg.norm(line_vec)
if line_len == 0:
return np.array([start, end])
line_unit = line_vec / line_len
max_dist = 0
max_idx = 0
for i in range(1, len(path) - 1):
vec = path[i] - start
proj_len = np.dot(vec, line_unit)
proj_point = start + proj_len * line_unit
dist = np.linalg.norm(path[i] - proj_point)
if dist > max_dist:
max_dist = dist
max_idx = i
if max_dist > epsilon:
# 递归
left = self.remove_redundant_points(path[:max_idx + 1], epsilon)
right = self.remove_redundant_points(path[max_idx:], epsilon)
return np.vstack((left[:-1], right))
else:
return np.array([start, end])
def interpolate_path(self, path: np.ndarray,
step_size: float = 0.1) -> np.ndarray:
"""路径插值(均匀间距)"""
if len(path) < 2:
return path
new_path = [path[0]]
remaining = 0.0
for i in range(len(path) - 1):
segment = path[i+1] - path[i]
seg_len = np.linalg.norm(segment)
if seg_len == 0:
continue
seg_unit = segment / seg_len
pos = path[i]
while remaining < seg_len:
pos = pos + seg_unit * step_size
new_path.append(pos.copy())
remaining += step_size
remaining -= seg_len
new_path.append(path[-1])
return np.array(new_path)18.5.3 代价感知路径优化
class CostAwarePathOptimizer:
"""代价感知路径优化"""
def __init__(self, costmap: np.ndarray, resolution: float = 0.05):
self.costmap = costmap
self.resolution = resolution
def optimize_path(self, path: np.ndarray,
safety_margin: float = 0.3) -> np.ndarray:
"""
调整路径点,使其远离高代价区域
"""
optimized = path.copy()
for iteration in range(50):
max_change = 0
for i in range(1, len(path) - 1):
# 获取当前点的代价
cost = self._get_cost(path[i])
# 如果处于高代价区域,向外推
if cost > 50:
# 计算法线方向
prev_vec = path[i] - path[i-1]
next_vec = path[i+1] - path[i]
# 法线 = 垂直路径方向
tangent = (next_vec + prev_vec) / 2
if np.linalg.norm(tangent) > 0:
normal = np.array([-tangent[1], tangent[0]])
normal = normal / np.linalg.norm(normal)
else:
normal = np.zeros(2)
# 朝低代价方向移动
push_force = min(cost / 100.0, 0.5) * 0.05
optimized[i] += normal * push_force
change = np.linalg.norm(optimized[i] - path[i])
max_change = max(max_change, change)
if max_change < 0.001:
break
return optimized
def _get_cost(self, point: np.ndarray) -> float:
"""获取栅格代价值"""
gx = int(point[1] / self.resolution)
gy = int(point[0] / self.resolution)
if (0 <= gx < self.costmap.shape[0] and
0 <= gy < self.costmap.shape[1]):
return float(self.costmap[gx, gy])
return 25518.5.4 官方要点——路径平滑与规划器协同
Nav2 官方文档把后处理交给独立的 smoother_server:内置 Simple Smoother(基于共轭梯度的迭代平滑,避免路径"抄近道"穿墙,通过 smoother.w_smooth/smoother.w_data 权衡平滑度与贴合度)、Conjugate Gradient Smoother 与可选的 TH*-like 换代实现。官方默认行为树中 ComputePathToPose 之后常串联 SmoothPath Action,使"折线路径 + 平滑输出"成为标准管线——练习第 6 题的三层框架(A* 粗规划 → Hybrid-A* 细化 → 平滑器后处理)在 Nav2 中可以直接用"planner_server + smoother_server"两个服务拼接实现。Robotics Back-End 的对比实验还指出:先平滑后跟踪时,控制器(DWB/RPP/MPPI)的横向误差显著降低,平滑器不是可选装饰而是规划-控制之间的质量缓冲。
18.6 实战案例
18.6.1 路径规划在Nav2中的完整流程
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path, OccupancyGrid
from geometry_msgs.msg import PoseStamped
import numpy as np
class GlobalPathPlannerDemo(Node):
"""全局路径规划演示节点"""
def __init__(self):
super().__init__('global_planner_demo')
self.costmap_sub = self.create_subscription(
OccupancyGrid, '/global_costmap/costmap',
self.costmap_callback, 10)
self.path_pub = self.create_publisher(Path, '/planned_path', 10)
self.costmap = None
self.planner = None
def costmap_callback(self, msg: OccupancyGrid):
"""接收代价地图并执行规划"""
if self.costmap is None:
h, w = msg.info.height, msg.info.width
data = np.array(msg.data).reshape(h, w)
self.costmap = data
self.planner = AStarPlanner(self.costmap, msg.info.resolution)
# 示例规划
start = (0.0, 0.0)
goal = (3.0, 2.0)
path = self.planner.plan(start, goal)
if path:
self.publish_path(path, msg.header.frame_id)
self.get_logger().info(f'路径规划成功: {len(path)}点')
def publish_path(self, path_world: list, frame_id: str):
"""发布规划路径"""
msg = Path()
msg.header.frame_id = frame_id
msg.header.stamp = self.get_clock().now().to_msg()
for x, y in path_world:
pose = PoseStamped()
pose.header = msg.header
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.orientation.w = 1.0
msg.poses.append(pose)
self.path_pub.publish(msg)18.6.2 命令行调试
# 查看规划路径
ros2 topic echo /plan --once
# 查看代价地图
ros2 topic echo /global_costmap/costmap --once
# 查看当前规划器参数
ros2 param describe /planner_server GridBased.plugin
# 设置规划容忍度
ros2 param set /planner_server GridBased.tolerance 0.318.6.3 常见问题
问题1:路径规划超时:可降低代价地图分辨率、减小搜索范围,或使用更高效的规划器。
问题2:路径穿过障碍物:多为膨胀半径过小,应检查代价地图是否正确更新,并增加规划容忍度。
问题3:路径不平滑:可使用路径平滑器后处理、改用Hybrid-A*规划器,或降低代价地图分辨率。
课后练习
-
原理题: 比较Dijkstra算法和A*算法的异同,说明启发式函数H(n)如何影响搜索效率和路径质量。
-
编程题: 实现A*路径规划算法,在给定代价地图上搜索从起点到终点的最短路径,并可视化路径。
-
分析题: 分析Hybrid-A与标准A的区别,说明为什么非全向机器人需要Hybrid-A*规划器。
-
配置题: 在Nav2中配置Smac Planner Hybrid,调整最小转弯半径、后退惩罚等参数以适应差速机器人。
-
操作题: 在仿真环境中使用不同的全局规划器进行导航,对比各规划器生成路径的平滑度和计算时间。
-
设计题: 设计一个三层路径规划框架:上层(全局搜索)使用A在中分辨率地图规划,中层使用Hybrid-A细化路径,下层使用路径平滑器后处理。
仿真结合实例(当前仓库):Nav2 GridBased 规划器生成路径
目标与知识点对应
使用预置 Museum 地图和 Nav2 的 GridBased/NavFn 全局规划器,从地图坐标目标生成无碰撞路径,观察规划器、地图服务器与 BT Navigator 的连接。
运行步骤
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch navigation_sim_demo_ros2 nav2_demo.launch.py \
use_gazebo:=true use_rviz:=true gz_headless:=falseros2 run navigation_sim_demo_ros2 nav_goal_runner \
--ros-args -p goal_x:=1.0 -p goal_y:=0.0
ros2 topic echo /plan --once观察结果
RViz 中可观察全局路径;目标是否被接受、是否产生 /plan 以及机器人是否运动,要分别看 Action、Path 和 /odom 输出。
源码与边界
Nav2 参数位于 src/navigation_sim_demo_ros2/params/nav2_params.yaml,目标节点位于 src/navigation_sim_demo_ros2/navigation_sim_demo_ros2/nav_goal_runner.py,地图位于 src/navigation_sim_demo_ros2/maps/Software_Museum.yaml。
路径规划结果受初始位姿、TF 和仿真启动时序影响,不能只凭启动成功判断规划完成。
学习材料:
- Nav2 官方文档 —— Planner Tables 与 Smac Planner:https://docs.nav2.org/
- docs.ros.org —— nav2_smac_planner / nav2_navfn_planner / nav2_smoother 文档:https://docs.ros.org/en/jazzy/p/nav2-smac-planner/
- Hart, Nilsson & Raphael (1968) —— A* 原始论文(SRI):https://ieeexplore.ieee.org/document/4082128
- The Construct —— ROS 2 全局规划课程:https://www.theconstructsim.com/
- Robotics Back-End —— Nav2 规划器对比与调参:https://roboticsbackend.com/
RuyiSDK Board Docs