第18章 全局路径规划
课程:ROS 2 C++17 编程(COM260 Kit / Humble)
章节:第18章
课时:2 课时(90 分钟)
教学方式:讲授 + 演示
先完成教案示例运行说明的双端准备。C++ 示例在 COM260 上运行,Gazebo / Nav2 / RViz 在 x86 上运行。
学习目标
本章学习目标包括:理解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. 将更新后的优先级加入 open_list(旧条目出堆时跳过)
3. return 失败 (无可行路径)
18.2.2 C++ 实现
#pragma once
#include <limits>
#include <queue>
#include <utility>
#include <costmap_demo/costmap.hpp>
using Point = std::pair<double, double>;
using Path = std::vector<Point>;
struct SearchResult {
Path path;
double cost = std::numeric_limits<double>::infinity();
int expanded = 0;
};
inline bool inside_map(const Costmap2D & map, Point point) {
return std::isfinite(point.first) && std::isfinite(point.second) &&
point.first >= map.origin_x && point.first < map.origin_x + map.width * map.resolution &&
point.second >= map.origin_y && point.second < map.origin_y + map.height * map.resolution;
}
inline bool clear_segment(const Costmap2D & map, Point a, Point b) {
if (!inside_map(map, a) || !inside_map(map, b)) { return false; }
auto [x, y] = map.world_to_grid(a.first, a.second);
const auto [end_x, end_y] = map.world_to_grid(b.first, b.second);
const double dx = b.first - a.first, dy = b.second - a.second;
const int sx = dx > 0 ? 1 : -1, sy = dy > 0 ? 1 : -1;
const double infinity = std::numeric_limits<double>::infinity();
const double step_x = dx == 0 ? infinity : map.resolution / std::abs(dx);
const double step_y = dy == 0 ? infinity : map.resolution / std::abs(dy);
double tx = dx == 0 ? infinity : (map.origin_x + (x + (sx > 0)) * map.resolution - a.first) / dx;
double ty = dy == 0 ? infinity : (map.origin_y + (y + (sy > 0)) * map.resolution - a.second) / dy;
auto free = [&](int gx, int gy) { return map.inside(gx, gy) && map.data[gy * map.width + gx] < 253; };
while (free(x, y)) {
if (x == end_x && y == end_y) { return true; }
// Check both neighbours when crossing a grid corner.
if (std::abs(tx - ty) < 1e-12) {
if (!free(x + sx, y) || !free(x, y + sy)) { return false; }
x += sx; y += sy; tx += step_x; ty += step_y;
} else if (tx < ty) { x += sx; tx += step_x; }
else { y += sy; ty += step_y; }
}
return false;
}
inline SearchResult search_grid(const Costmap2D & map, Point start, Point goal, bool astar) {
SearchResult result;
if (!inside_map(map, start) || !inside_map(map, goal)) { return result; }
auto [sx, sy] = map.world_to_grid(start.first, start.second);
auto [gx, gy] = map.world_to_grid(goal.first, goal.second);
auto free = [&](int x, int y) {
return map.inside(x, y) && map.data[y * map.width + x] < 253;
};
if (!free(sx, sy) || !free(gx, gy)) { return result; }
const int first = sy * map.width + sx, last = gy * map.width + gx;
const int size = map.width * map.height;
std::vector<double> cost(size, std::numeric_limits<double>::infinity());
std::vector<int> parent(size, -1);
std::vector<bool> closed(size, false);
auto heuristic = [&](int x, int y) {
const int dx = std::abs(x - gx), dy = std::abs(y - gy);
return astar ? dx + dy + (std::sqrt(2.0) - 2) * std::min(dx, dy) : 0.0;
};
using Entry = std::pair<double, int>;
std::priority_queue<Entry, std::vector<Entry>, std::greater<Entry>> open;
cost[first] = 0;
open.push({heuristic(sx, sy), first});
while (!open.empty()) {
const int current = open.top().second;
open.pop();
if (closed[current]) { continue; }
closed[current] = true;
++result.expanded;
if (current == last) {
result.cost = cost[last];
for (int cell = last; cell != -1; cell = parent[cell]) {
result.path.push_back(map.grid_to_world(cell % map.width, cell / map.width));
}
std::reverse(result.path.begin(), result.path.end());
return result;
}
const int x = current % map.width, y = current / map.width;
for (int dy = -1; dy <= 1; ++dy) {
for (int dx = -1; dx <= 1; ++dx) {
if ((dx == 0 && dy == 0) || !free(x + dx, y + dy)) { continue; }
// A diagonal move must not cut between blocked adjacent cells.
if (dx && dy && (!free(x + dx, y) || !free(x, y + dy))) { continue; }
const int next = (y + dy) * map.width + x + dx;
const double candidate = cost[current] + std::hypot(dx, dy) *
(1.0 + map.data[next] / 50.0);
if (candidate < cost[next]) {
cost[next] = candidate;
parent[next] = current;
open.push({candidate + heuristic(x + dx, y + dy), next});
}
}
}
}
return result;
}完整源码:grid_path_planner。astar=false 时启发式为 0,即 Dijkstra;每次发现更小代价都重新入堆,已扩展节点跳过。栅格中心输出世界坐标,253/254/255 均不可通行,斜向移动不得穿过相邻障碍的夹角。最优性是相对此处非负栅格代价而言。
18.2.3 Dijkstra的特点
**优点:**保证找到最短路径,算法完备(有解必能找到),实现简单。
**缺点:**搜索范围大、效率低,不考虑目标位置而向所有方向等距离扩展,在大地图上计算时间过长。
18.3 A*算法
18.3.1 算法原理
A*算法在Dijkstra的基础上加入启发式函数,引导搜索方向,提高效率。
评价函数:
F(n) = G(n) + H(n)
其中 G(n) 表示起点到当前节点n的实际代价,H(n) 表示当前节点n到目标点的启发式估计代价。
启发式函数的选择:
| 启发式 | 公式(dx、dy 为坐标差绝对值) | 适用情况 |
|---|---|---|
| 曼哈顿距离 | dx + dy | 四邻域、单位步长 |
| 欧氏距离 | sqrt(dx² + dy²) | 八邻域的可采纳下界 |
| 切比雪夫距离 | max(dx, dy) | 直走和斜走代价都为 1 |
| 对角距离 | dx + dy + (sqrt(2) − 2) × min(dx, dy) | 直走 1、斜走 sqrt(2),本例采用 |
18.3.2 C++ 实现
Costmap2D map(5, 5, 0.1);
std::fill(map.data.begin(), map.data.end(), 0);
for (int y = 5; y < 40; ++y) { map.data[y * map.width + 25] = 254; }
auto result = search_grid(map, {-2, -2}, {2, 2}, true);与上一节共用搜索循环,仅启发式不同。该片段取自下面的完整可执行示例。
18.3.3 A* vs Dijkstra
#include <chrono>
#include <fstream>
#include <iostream>
#include "grid_path_planner/search.hpp"
int main() {
Costmap2D map(5, 5, 0.1);
std::fill(map.data.begin(), map.data.end(), 0);
for (int y = 5; y < 40; ++y) { map.data[y * map.width + 25] = 254; }
for (bool astar : {false, true}) {
const auto begin = std::chrono::steady_clock::now();
const auto result = search_grid(map, {-2, -2}, {2, 2}, astar);
const double ms = std::chrono::duration<double, std::milli>(
std::chrono::steady_clock::now() - begin).count();
std::cout << (astar ? "A*" : "Dijkstra") << " points=" << result.path.size()
<< " cost=" << result.cost << " expanded=" << result.expanded << " ms=" << ms << '\n';
if (result.path.empty()) { return 1; }
std::ofstream svg(astar ? "astar.svg" : "dijkstra.svg");
svg << "<svg xmlns=\"http://www.w3.org/2000/svg\" viewBox=\"0 0 500 500\">"
<< "<rect width=\"500\" height=\"500\" fill=\"white\"/>"
<< "<rect x=\"250\" y=\"100\" width=\"10\" height=\"350\" fill=\"black\"/>"
<< "<polyline fill=\"none\" stroke=\"blue\" stroke-width=\"3\" points=\"";
for (auto [x, y] : result.path) { svg << 100 * (x + 2.5) << ',' << 100 * (2.5 - y) << ' '; }
svg << "\"/></svg>\n";
}
}运行 ros2 run grid_path_planner grid_path_planner,会输出路径点数、总代价、扩展节点数和当前机器耗时,并生成两张 SVG。比较耗时应使用相同输入与构建类型;单次测量不能代表所有地图。
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曲线连接。
#pragma once
#include <array>
#include <grid_path_planner/search.hpp>
using Pose = std::array<double, 3>;
inline double wrapped_angle(double angle) { return std::atan2(std::sin(angle), std::cos(angle)); }
inline bool footprint_clear(const Costmap2D & map, Pose pose, double radius = 0.22) {
if (pose[0] - radius < map.origin_x || pose[1] - radius < map.origin_y ||
pose[0] + radius >= map.origin_x + map.width * map.resolution ||
pose[1] + radius >= map.origin_y + map.height * map.resolution) { return false; }
auto [x, y] = map.world_to_grid(pose[0], pose[1]);
const int cells = static_cast<int>(std::ceil(radius / map.resolution)) + 1;
for (int dy = -cells; dy <= cells; ++dy) {
for (int dx = -cells; dx <= cells; ++dx) {
const auto center = map.grid_to_world(x + dx, y + dy);
const double gap_x = std::max(0.0, std::abs(center.first - pose[0]) - map.resolution / 2);
const double gap_y = std::max(0.0, std::abs(center.second - pose[1]) - map.resolution / 2);
if (std::hypot(gap_x, gap_y) <= radius &&
(!map.inside(x + dx, y + dy) || map.data[(y + dy) * map.width + x + dx] >= 254)) {
return false;
}
}
}
return true;
}
inline std::vector<Pose> hybrid_plan(const Costmap2D & map, Pose start, Pose goal,
double turning_radius = 0.5, double reverse_penalty = 2.0) {
for (double value : {start[0], start[1], start[2], goal[0], goal[1], goal[2],
turning_radius, reverse_penalty}) {
if (!std::isfinite(value)) { return {}; }
}
if (turning_radius <= 0 || reverse_penalty < 1 ||
!footprint_clear(map, start) || !footprint_clear(map, goal)) { return {}; }
constexpr double pi = 3.14159265358979323846;
auto key = [&](Pose pose) {
auto [x, y] = map.world_to_grid(pose[0], pose[1]);
int heading = static_cast<int>(std::floor((wrapped_angle(pose[2]) + pi) / (pi / 12))) % 24;
return (y * map.width + x) * 24 + heading;
};
struct Node { Pose pose; double cost; int parent; };
std::vector<Node> nodes{{start, 0, -1}};
std::vector<double> best(map.width * map.height * 24, std::numeric_limits<double>::infinity());
using Entry = std::pair<double, int>;
std::priority_queue<Entry, std::vector<Entry>, std::greater<Entry>> open;
best[key(start)] = 0;
open.push({std::hypot(start[0] - goal[0], start[1] - goal[1]), 0});
while (!open.empty()) {
const int index = open.top().second;
open.pop();
const Node current = nodes[index];
if (current.cost > best[key(current.pose)] + 1e-9) { continue; }
if (std::hypot(current.pose[0] - goal[0], current.pose[1] - goal[1]) < 0.5 &&
std::abs(wrapped_angle(current.pose[2] - goal[2])) <= pi / 12) {
std::vector<Pose> path;
for (int i = index; i != -1; i = nodes[i].parent) { path.push_back(nodes[i].pose); }
std::reverse(path.begin(), path.end());
return path;
}
for (int gear : {1, -1}) {
for (int steer = -2; steer <= 2; ++steer) {
if (gear < 0 && std::abs(steer) == 2) { continue; }
const double length = gear > 0 ? 0.15 : -0.06;
const double curvature = steer / (2.0 * turning_radius);
const int steps = std::max(1, static_cast<int>(std::ceil(std::abs(length) / (map.resolution / 2))));
Pose next = current.pose;
bool clear = true;
for (int i = 0; i < steps && clear; ++i) {
const double ds = length / steps;
next[0] += ds * std::cos(next[2] + ds * curvature / 2);
next[1] += ds * std::sin(next[2] + ds * curvature / 2);
next[2] = wrapped_angle(next[2] + ds * curvature);
clear = footprint_clear(map, next);
}
if (!clear) { continue; }
const double cost = current.cost + std::abs(length) * (gear < 0 ? reverse_penalty : 1) +
std::abs(curvature) * 0.05;
const int cell = key(next);
if (cost + 1e-9 >= best[cell]) { continue; }
best[cell] = cost;
nodes.push_back({next, cost, index});
open.push({cost + std::hypot(next[0] - goal[0], next[1] - goal[1]),
static_cast<int>(nodes.size() - 1)});
}
}
}
return {};
}这是运动基元搜索教学实现:hybrid_astar_planner。它检查沿途圆形足迹,使用地图分辨率划分位置、24 个朝向格;前进步长 0.15 m、后退步长 0.06 m,目标位置误差小于 0.5 m、朝向误差不超过 15°。它没有实现解析 Reeds–Shepp 直连;本节提到的解析扩展由下一节实际加载的 Smac Hybrid 插件实现。
18.4.3 Nav2规划器配置
planner_server:
ros__parameters:
planner_plugins:
- GridBased
GridBased:
plugin: nav2_smac_planner/SmacPlannerHybrid
tolerance: 0.15
allow_unknown: false
max_planning_time: 2.0
minimum_turning_radius: 0.2
motion_model_for_search: REEDS_SHEPP
angle_quantization_bins: 72
reverse_penalty: 2.0
change_penalty: 0.5
non_straight_penalty: 1.1hybrid 模式的最小转弯半径为 0.2 m、倒车惩罚为 2.0、转向变化惩罚为 0.5、非直线惩罚为 1.1,目标容忍距离为 0.15 m。允许倒车的 REEDS_SHEPP 搜索模型与 DWB 默认只前进的约束需要一起核对,不能认为任意 Hybrid 路径都可被该控制器直接执行。
18.4.4 规划器切换
先查询当前插件:
ros2 param get /planner_server planner_plugins
ros2 param get /planner_server GridBased.pluginHumble 的插件在生命周期配置阶段创建,修改 planner_plugins 参数本身不会热替换已加载实例。按教案示例运行说明停止当前仿真,再分别以 navfn、astar、smac2d、hybrid 启动;每次使用新 RUN_ID,重新确认节点 active、地图、TF 和初始位姿后发送相同目标。
18.4.5 官方要点——Nav2 官方规划器全家福与选型指引
Humble 提供 NavFn(Dijkstra/A*)、Smac 2D、Smac Hybrid、Smac Lattice 和 Theta* 等规划器。圆形差速底盘可使用二维栅格搜索;需要约束朝向与转弯半径时可选 Hybrid,使用预生成运动基元时可选 Lattice。具体路径形状、内存和耗时需在相同地图与参数下测量,不能仅凭名称判断优劣。
nav2_navfn_planner 的 tolerance 用于目标不可达时搜索附近的可达位置;返回成功时还应检查最终路径点与请求目标的距离。当前课程固定版本的实现见 Nav2 源码。
18.4.6 官方要点——Smac Hybrid-A* 的运动学细节
nav2_smac_planner 文档对 Hybrid-A* 给出了与本章 18.4.2 呼应的参数语义:motion_model_for_search 选择 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 路径平滑
路径平滑去除路径中的锐角转折:
#pragma once
#include <grid_path_planner/search.hpp>
inline double distance(Point a, Point b) { return std::hypot(a.first - b.first, a.second - b.second); }
inline Point blend(Point a, Point b, double t) {
return {a.first + t * (b.first - a.first), a.second + t * (b.second - a.second)};
}
inline bool path_clear(const Costmap2D & map, const Path & path) {
for (std::size_t i = 1; i < path.size(); ++i) {
if (!clear_segment(map, path[i - 1], path[i])) { return false; }
}
return !path.empty() && inside_map(map, path[0]) && map.get_cost(path[0].first, path[0].second) < 253;
}
inline Path smooth_path(const Path & path, const Costmap2D & map, int iterations = 100) {
auto result = path;
for (int n = 0; n < iterations; ++n) {
for (std::size_t i = 1; i + 1 < result.size(); ++i) {
Point candidate{
result[i].first + 0.2 * (0.5 * (path[i].first - result[i].first) +
(0.3 + 0.2) * (result[i - 1].first + result[i + 1].first - 2 * result[i].first)),
result[i].second + 0.2 * (0.5 * (path[i].second - result[i].second) +
(0.3 + 0.2) * (result[i - 1].second + result[i + 1].second - 2 * result[i].second))};
if (clear_segment(map, result[i - 1], candidate) && clear_segment(map, candidate, result[i + 1])) {
result[i] = candidate;
}
}
}
return result;
}
inline Path bspline_path(const Path & path, const Costmap2D & map, int samples = 100) {
if (path.size() < 4 || samples < 2) { return path; }
const int count = static_cast<int>(path.size());
std::vector<double> knots(count + 4);
for (int i = 0; i < count + 4; ++i) {
knots[i] = std::clamp((i - 3.0) / (count - 3), 0.0, 1.0);
}
Path result;
for (int sample = 0; sample < samples; ++sample) {
const double t = static_cast<double>(sample) / (samples - 1);
const int span = t == 1 ? count - 1 : static_cast<int>(
std::upper_bound(knots.begin(), knots.end(), t) - knots.begin()) - 1;
Point points[4];
for (int j = 0; j <= 3; ++j) { points[j] = path[span - 3 + j]; }
for (int level = 1; level <= 3; ++level) {
for (int j = 3; j >= level; --j) {
const int i = span - 3 + j;
const double alpha = (t - knots[i]) / (knots[i + 4 - level] - knots[i]);
points[j] = blend(points[j - 1], points[j], alpha);
}
}
result.push_back(points[3]);
}
return path_clear(map, result) ? result : path;
}完整路径优化源码保留端点,并检查更新后的路径段是否穿过障碍。梯度更新步长为 0.2。三次 B 样条用 De Boor 算法计算夹持曲线,端点不变;这里将路径点作为控制点,与 SciPy 插值拟合并非逐点数值等价。曲线碰撞时保留原路径。
18.5.2 路径修剪
inline Path prune_path(const Path & path, const Costmap2D & map, double epsilon = 0.1) {
if (path.size() < 3) { return path; }
const Point first = path.front(), last = path.back();
const double squared = std::pow(distance(first, last), 2);
double farthest = -1;
std::size_t split = 1;
for (std::size_t i = 1; i + 1 < path.size(); ++i) {
const double t = squared == 0 ? 0 : std::clamp(
((path[i].first - first.first) * (last.first - first.first) +
(path[i].second - first.second) * (last.second - first.second)) / squared, 0.0, 1.0);
const double error = distance(path[i], blend(first, last, t));
if (error > farthest) { farthest = error; split = i; }
}
if (farthest <= epsilon && clear_segment(map, first, last)) { return {first, last}; }
auto left = prune_path(Path(path.begin(), path.begin() + split + 1), map, epsilon);
auto right = prune_path(Path(path.begin() + split, path.end()), map, epsilon);
left.insert(left.end(), right.begin() + 1, right.end());
return left;
}
inline Path resample_path(const Path & path, double step = 0.1) {
if (!std::isfinite(step) || step <= 0) { throw std::invalid_argument("step must be positive"); }
if (path.size() < 2) { return path; }
Path result{path.front()};
double remaining = step;
for (std::size_t i = 1; i < path.size(); ++i) {
Point from = path[i - 1];
double length = distance(from, path[i]);
while (length >= remaining && length > 0) {
from = blend(from, path[i], remaining / length);
result.push_back(from);
length -= remaining;
remaining = step;
}
remaining -= length;
}
if (distance(result.back(), path.back()) > 1e-9) { result.push_back(path.back()); }
return result;
}Douglas–Peucker 修剪必须同时满足误差上限和连线可通行;重采样按整条折线的累计弧长分配间距,末尾不足一个间距时保留终点。
18.5.3 代价感知路径优化
inline Path optimize_cost(const Path & path, const Costmap2D & map, double max_shift = 0.3) {
auto result = path;
for (int n = 0; n < 50; ++n) {
for (std::size_t i = 1; i + 1 < result.size(); ++i) {
const double dx = result[i + 1].first - result[i - 1].first;
const double dy = result[i + 1].second - result[i - 1].second;
const double length = std::hypot(dx, dy);
if (length == 0 || map.get_cost(result[i].first, result[i].second) <= 50) { continue; }
for (double sign : {-1.0, 1.0}) {
Point next{result[i].first - sign * dy / length * map.resolution,
result[i].second + sign * dx / length * map.resolution};
if (distance(next, path[i]) <= max_shift &&
map.get_cost(next.first, next.second) < map.get_cost(result[i].first, result[i].second) &&
clear_segment(map, result[i - 1], next) && clear_segment(map, next, result[i + 1])) {
result[i] = next;
}
}
}
}
return result;
}max_shift=0.3 限制点相对原路径的移动量,不代表保证 0.3 m 的障碍净距。这里在法线两侧寻找更低代价格,接受移动前检查前后线段;实际安全距离仍取决于足迹和膨胀层。
18.5.4 官方要点——路径平滑与规划器协同
Humble 的 smoother_server 提供 SmoothPath Action,可在规划后独立调用。Simple Smoother 通过 w_data 与 w_smooth 权衡原路径和局部平滑,并检查代价地图;不能仅凭启动了服务就认为每条路径都经过平滑。实际是否调用由行为树决定。完成练习 6 的三层框架时,需要设计粗细地图之间的转换、子目标与不可行时的回退。
18.6 实战案例
18.6.1 路径规划在Nav2中的完整流程
#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include "grid_path_planner/search.hpp"
int main(int argc, char ** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("global_planner_demo");
const Point start{node->declare_parameter("start_x", 0.0), node->declare_parameter("start_y", 0.0)};
const Point goal{node->declare_parameter("goal_x", 3.0), node->declare_parameter("goal_y", 2.0)};
auto publisher = node->create_publisher<nav_msgs::msg::Path>("planned_path", rclcpp::QoS(1).transient_local());
bool finished = false, success = false;
auto subscription = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
"/global_costmap/costmap", rclcpp::QoS(1).transient_local(),
[&](nav_msgs::msg::OccupancyGrid::ConstSharedPtr message) {
if (finished) { return; }
const auto & info = message->info;
if (!info.width || !info.height || info.resolution <= 0 ||
message->data.size() != info.width * info.height ||
std::abs(info.origin.orientation.w - 1) > 1e-6) { return; }
Costmap2D map(info.width * double(info.resolution), info.height * double(info.resolution), info.resolution);
map.width = info.width; map.height = info.height;
map.data.resize(message->data.size());
map.origin_x = info.origin.position.x;
map.origin_y = info.origin.position.y;
for (std::size_t i = 0; i < message->data.size(); ++i) {
const int value = message->data[i];
map.data[i] = value < 0 ? 255 : value == 100 ? 254 : value == 99 ? 253 : value * 252 / 98;
}
const auto result = search_grid(map, start, goal, true);
nav_msgs::msg::Path path;
path.header = message->header;
for (auto [x, y] : result.path) {
geometry_msgs::msg::PoseStamped pose;
pose.header = path.header;
pose.pose.position.x = x; pose.pose.position.y = y;
pose.pose.orientation.w = 1;
path.poses.push_back(pose);
}
publisher->publish(path);
success = !path.poses.empty();
finished = true;
RCLCPP_INFO(node->get_logger(), "A* 路径: %zu 点,代价=%.3f,扩展=%d",
path.poses.size(), result.cost, result.expanded);
});
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(20);
while (rclcpp::ok() && !finished && std::chrono::steady_clock::now() < deadline) {
rclcpp::spin_some(node);
rclcpp::sleep_for(std::chrono::milliseconds(20));
}
rclcpp::sleep_for(std::chrono::seconds(1));
if (!success) { RCLCPP_ERROR(node->get_logger(), "没有获得有效地图或可行路径"); }
rclcpp::shutdown();
return success ? 0 : 1;
}该节点订阅实际全局代价地图,保留其原点和分辨率,将 OccupancyGrid 的 -1/99/100 分别处理为未知/内切/致命障碍,默认从 (0, 0) 规划到 (3, 2),发布 /planned_path。使用 C++ global_planner_client 调用 Nav2 官方规划器进行对照,运行步骤见教案示例运行说明。
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 的连接。
运行步骤
按教案示例运行说明启动 x86 仿真。在 COM260:
source ~/.config/ros2-course-com260-ch18-ch19/env.bash
ros2 run global_planner_client global_planner_client --ros-args \
-p use_sim_time:=true -p goal_x:=1.0 -p goal_y:=0.0
ros2 run navigation_client_cpp bt_navigator_client --ros-args \
-p use_sim_time:=true -p x:=1.0 -p y:=0.0规划与执行分别检查;只生成 Path 不会驱动机器人。
观察结果
RViz 中可观察全局路径;目标是否被接受、是否产生 /plan 以及机器人是否运动,要分别看 Action、Path 和 /odom 输出。
源码与边界
教学算法位于 src_k3_com260_kit/grid_path_planner/、hybrid_astar_planner/ 和 path_optimizer/;C++ Action 客户端复用 global_planner_client/、navigation_client_cpp/。本章独立配置见 planning.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