4. 全局规划器：NavFn、Theta*、Smac Planner 2D配置与调参，代价地图（Costmap2D）原理

各位工程师朋友，欢迎来到第4章。前面我们聊了导航的整体框架，今天咱们深入聊聊“大脑”里的“思考”部分——全局规划器。

说白了，全局规划就是机器人出发前，在地图上画一条从A到B的“大路线”。这条路线好不好，直接决定了后面局部规划能不能跑得顺。我个人习惯把全局规划器比作“战略家”，它不关心路上的小石子，只看大方向对不对。

4.1 代价地图（Costmap2D）——规划器眼中的世界

在讲规划器之前，必须先搞懂代价地图。因为所有规划器都是“看”着代价地图来画线的。

代价地图，你可以把它想象成一张“热力图”。每个格子都有一个数值，代表这个位置的危险程度。

为什么会这样设计？你想想看，如果机器人只知道“有障碍”和“没障碍”，那它贴着墙走你也没办法。但有了膨胀层，机器人就会自动和墙保持距离。嗯，这里要注意，膨胀半径设得太大，机器人过窄门就会卡住；设得太小，又容易刮蹭。我在项目中遇到过，一个客户把膨胀半径设成了0.5米，结果机器人死活进不去一个0.6米宽的走廊……

代价地图分为全局代价地图和局部代价地图。全局的覆盖整个已知地图，用于全局规划；局部的只关注机器人周围一小块区域，用于局部规划。它们共享相同的代价计算逻辑，但参数可以独立配置。

4.2 全局规划器三剑客

ROS2 Nav2里，全局规划器主要有三个选择：NavFn、Theta* 和 Smac Planner 2D。它们各有千秋，我一个个说。

4.2.1 NavFn——经典中的经典

NavFn是Nav1时代就有的老将。它基于Dijkstra算法，从起点到终点做波前传播，找到一条最短路径。

优点：稳定、可靠、计算快。对于大多数场景，它都能给出一个不错的解。

缺点：路径不够“聪明”。它只会走最短路径，不会考虑路径的平滑性。你经常会看到它贴着障碍物走，或者走出一些锯齿状的折线。

配置示例（nav2_params.yaml）：

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5          # 终点容差，米
      use_astar: false        # 是否使用A*加速
      allow_unknown: true     # 是否允许通过未知区域

4.2.2 Theta*——路径更直，更自然

Theta* 是A*的改进版。它允许路径上的节点之间“跳着走”，而不是只能走网格的邻居。说白了，它能切角，让路径更直。

优点：路径更短、更平滑，看起来更自然。特别适合有开阔空间的场景。

缺点：计算量比NavFn大一些，但现代CPU完全扛得住。

我个人习惯在室内场景优先用Theta*。它画出来的路径，机器人走起来更顺，局部规划器也更容易跟踪。

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_theta_star_planner/ThetaStarPlanner"
      tolerance: 0.5
      allow_unknown: true
      w_euc_cost: 1.0         # 欧氏距离权重
      w_traversal_cost: 0.5   # 遍历代价权重
      w_heuristic_cost: 1.0   # 启发式代价权重

4.2.3 Smac Planner 2D——全能选手

Smac Planner 2D是Nav2的新成员，也是我最推荐的。它支持多种算法模式，包括A*、Smac（Smooth A* with Cost）和Hybrid-A*。

核心优势：

• 路径平滑：内置平滑器，路径不会出现尖锐拐角。
• 代价感知：不仅找最短路径，还会尽量走代价低的区域。
• 可配置性强：你可以调整平滑程度、搜索范围、甚至是否允许倒车。

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner2D"
      tolerance: 0.5
      allow_unknown: true
      max_iterations: 1000000
      max_on_approach_iterations: 1000
      cost_travel_multiplier: 2.0   # 代价权重，越大越避障
      smooth_path: true             # 是否平滑路径
      smooth_max_time: 2.0          # 平滑最大时间（秒）
      smooth_max_curvature: 0.5     # 最大曲率

4.3 调参实战——让规划器更聪明

光知道配置项没用，得知道怎么调。我总结几个核心原则：

1. 先看代价地图：如果规划器画出来的路径贴着墙，先检查膨胀半径，别急着改规划器参数。
2. 容忍度（tolerance）：设得太大，机器人可能停不到目标点；设得太小，规划器可能找不到路径。我一般设0.3-0.5米。
3. 代价权重（cost_travel_multiplier）：这个值越大，规划器越倾向于绕远路走安全区域。如果机器人总爱走危险区，就加大这个值。
4. 平滑参数：Smac Planner的平滑功能很强大。如果路径有锯齿，开启smooth_path并适当调整smooth_max_curvature。

4.4 三种规划器对比

特性	NavFn	Theta*	Smac Planner 2D
算法基础	Dijkstra / A*	A* + 视线检查	A* / Smac / Hybrid-A*
路径平滑度	低（锯齿状）	中（可切角）	高（内置平滑）
计算开销	低	中	中高
代价感知	弱	中	强
适用场景	简单、开阔环境	室内、走廊	复杂、动态环境
推荐指数	⭐⭐⭐	⭐⭐⭐⭐	⭐⭐⭐⭐⭐

嗯，到这里，全局规划器和代价地图的核心内容就差不多了。记住，规划器只是“画图”的，真正让机器人走好，还得靠下一章的局部规划器。不过那是后话了，先把今天的内容消化掉。