RuyiSDK Board Docs

RISC-V ROS2 机器人操作系统编程技术

Source repository

ch18 · 全局路径规划

TextbookProgramming languageEnvironmentLessonLab
RISC-VC++17SpacemiT K3 CoM260 Kit / Bianbu 4.0.6 / Humble, with an x86 Ubuntu 22.04 / Humble / Harmonic course containerRead lessonStart lab
x86PythonUbuntu 22.04 / Humble or Ubuntu 24.04 / JazzyRead lessonStart lab

This document is not yet available in English. The Chinese original is shown below.

第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.1

hybrid 模式的最小转弯半径为 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.plugin

Humble 的插件在生命周期配置阶段创建,修改 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.3

18.6.3 常见问题

问题1:路径规划超时:可降低代价地图分辨率、减小搜索范围,或使用更高效的规划器。

问题2:路径穿过障碍物:多为膨胀半径过小,应检查代价地图是否正确更新,并检查足迹;目标容忍度不能替代避障检查。

问题3:路径不平滑:可使用路径平滑器后处理、改用Hybrid-A*规划器,或降低代价地图分辨率。

课后练习

  1. 原理题: 比较Dijkstra算法和A*算法的异同,说明启发式函数H(n)如何影响搜索效率和路径质量。

  2. 编程题: 实现A*路径规划算法,在给定代价地图上搜索从起点到终点的最短路径,并可视化路径。

  3. 分析题: 分析Hybrid-A与标准A的区别,说明为什么非全向机器人需要Hybrid-A*规划器。

  4. 配置题: 在Nav2中配置Smac Planner Hybrid,调整最小转弯半径、后退惩罚等参数以适应差速机器人。

  5. 操作题: 在仿真环境中使用不同的全局规划器进行导航,对比各规划器生成路径的平滑度和计算时间。

  6. 设计题: 设计一个三层路径规划框架:上层(全局搜索)使用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 和仿真启动时序影响。截图、视频和测量结果见教案示例运行说明及运行证据。

学习材料: