ROS2 Nav2 - Smac 规划器

系列文章目录

---

前言

        SmacPlanner 是 Nav2 Planner 服务器的插件。它目前包括 3 个不同的插件：

• SmacPlannerHybrid：高度优化的完全可重新配置的 Hybrid-A* 实现，支持 Dubin 和 Reeds-Shepp 模型（足式、阿克曼和汽车模型）。
• SmacPlannerLattice：高度优化的完全可重新配置的状态网格实现，支持可配置的最小控制集，并为阿克曼、足式、差分和全向模型提供了控制集。
• SmacPlanner2D：高度优化的完全可重新配置的基于网格的 A* 实现，支持 8 个连接的邻域模型。

它还介绍了以下基本构建模块：

• CostmapDownsampler（成本映射下采样器）：一个用于接收成本映射对象并将其下采样到另一种分辨率的库。
• AStar： 一个通用且高度优化的 A* 模板库，用于规划插件的搜索。还可使用它提供额外的规划模板。
• CollisionChecker（碰撞检查器） 根据机器人的半径或足迹进行碰撞检查。
• Smoother（平滑器） 简单的路径平滑器，用于平滑 2D、混合-A* 和状态网格路径。

有用户报告在以下设备上使用了该功能

• 快递机器人
• 工业机器人
• 垂直农业
• 太阳能农场

有关其他参数说明，请参阅配置指南页面。

---

一、简介

        nav2_smac_planner 软件包包含一个优化的模板化 A* 搜索算法，用于为多种类型的机器人平台创建多个基于 A* 的规划器。它由 Steve Macenski 在三星研究院工作时创建。我们使用 SmacPlanner2D 规划器支持圆（circular）差分驱动和全向驱动机器人，该规划器实现了成本感知 A* 规划器。我们使用 SmacPlannerHybrid 插件（该插件实现了混合 A* 规划器）支持腿式、车式、类车式和阿克曼式车辆。我们使用 SmacPlannerLattice 插件支持非圆形、任意形状和任何模型的车辆，该插件实现了状态网格规划器。它包含用于阿克曼、腿式、差速驱动和全向车辆的控制集和生成器，但您也可以为其他机器人类型或不同的规划行为提供自己的控制集和生成器。后两个插件对于曲率约束或运动学上可行的规划也很有用，比如在规划高速行驶的机器人时，可以确保它们不会翻转或打滑失控。它还适用于需要基于姿势进行碰撞检查的非圆形机器人（如采用差动/单向传动系统的大型矩形或任意形状机器人）。

        SmacPlannerHybrid 实现了《自动驾驶路径规划中的实用搜索技术》中提出的 Hybrid-A* 规划器，并对启发式遍历函数进行了修改，以提高路径质量，而无需昂贵的基于优化的平滑处理。

        SmacPlannerLattice 实现了状态网格规划器。虽然我们的实现方式与状态网格中的最优、平滑、非全局性移动机器人运动规划（使用离散状态空间中受限运动规划的近似最小跨度控制集生成）并不完全相同，但两者非常相似，可以作为很好的参考。此外，我们还使用了更多的优化方法来进行逼近分析展开和改进启发式函数，这与 Hybrid-A* 的方法基本一致，以便它们能够共享这些优化实现，从而推动搜索更快地实现目标。

总之...

SmacPlannerHybrid 可与以下机器人配合使用：

• 阿克曼、汽车和类似汽车的机器人
• 高速或曲率受限的机器人（以免在高速行驶时翻转、打滑或倾倒负载）
• 任意形状、非圆形差分或全方向机器人，需要通过 SE2 碰撞检查进行运动学上可行的规划
• 足式机器人

SmacPlannerLattice 可与以下机器人配合使用

• 任意形状、非圆形机器人，要求运动学上可行的规划，并利用动力传动系统的全部功能进行 SE2 碰撞检测
• 可灵活使用其他机器人模型类型或提供非圆形差分、阿克曼和全向支持

SmacPlanner2D 设计用于以下情况

• 圆形、差分或全向机器人
• 相对于环境尺寸较小的机器人（如走廊中的遥控车或会展中心中的大型机器人），可近似使用圆形足迹。

二、特点

        我们从以下几个方面对 Hybrid-A* 进行了进一步改进：

• 通过使用 10 倍更小的运动基元进行搜索（与上采样率相同），从而消除上采样的需要。
• 多分辨率搜索，允许在更宽的空间内以更粗的分辨率进行规划（O(N^2)更快）。
• 在搜索中使用成本感知惩罚函数，使规划更加平滑（进一步降低平滑要求）。
• 梯度下降，基本但快速的平滑器
• 通过高度优化模板 A* 算法，规划速度比原始论文更快。
• 通过自定义预计算启发式、运动基元和其他功能实现更快的规划。
• 通过运动模型、代价图分辨率和分仓大小自动调整搜索运动模型大小。
• 如果无法找到精确路径或在无效空间内，可在容差范围内找到最接近的路径。
• 多模型混合搜索，包括 Dubin 和 Reeds-Shepp 模型。还可添加更多模型。
• 高单元和集成测试覆盖率，doxygen 文档。
• 使用现代 C++14 语言功能，单个组件可轻松重用。
• 速度优化：主循环中无数据结构图查找，主循环拷贝近乎为零，动态生成图和基于动态编程的障碍启发式，对同一目标的后续规划请求可选择重新计算启发式等。
• 模板化节点和 A* 实现，支持更多机器人扩展。
• 对每个目标/地图或每次迭代的障碍物启发式进行选择性重新评估，可将后续重新规划的速度提高 20 倍或更多。

        SmacPlanner2D 和 SmacPlannerLattice 插件也提供了这些功能中的大部分（多分辨率、模型、平滑器等）。

        2D A* 实现也没有 NavFn Planner 中基于梯度波前的 2D A* 实现所引入的任何怪异人工痕迹。虽然这种 2D A* 规划器速度稍慢，但我相信它完全值得路径质量的提高。

注：在之前的版本中，CG 平滑器在很大程度上实现了最初的混合 A* 论文。但是，这个平滑器无法持续提供有用的结果，而且耗费了太多的计算时间，因此已被弃用。虽然在 95% 的时间内平滑路径似乎是个 “不错 ”的解决方案，但在实际应用中，我们需要更可靠的解决方案。由于我们的工作对象是移动机器人，而不是时速 60 英里的自动驾驶汽车，因此我们可以在平滑过程中采取一些不同的自由度，因为我们的本地轨迹规划器非常聪明。如果你正在寻找它，它现在作为成本感知平滑器存在于新的平滑服务器中。这种平滑器已被更简单的优化解决方案所取代，后者速度更快、更一致，也更易于理解。虽然这种平滑器不具有成本意识，但我们在规划器中添加了成本意识惩罚函数，以推动计划远离高成本空间，而且我们还会检查平滑段的有效性，以确保可行性。当路径与环境发生碰撞时，它将终止。不过，如果您想使用这种平滑器，可以在平滑器服务器中使用，不过需要额外的计算时间。

三、指标

        最初的 Hybrid-A* 实施方案的规划时间为 50-300 毫秒，规划范围包括 102,400 个单元图和 72 个角度分区。在我们的评估中，我们看到了更快的结果：

• 对 147,456 个（比原来大 1.4 倍）具有 72 个角度分区的单元映射进行规划时，规划时间为 2-20ms。
• 规划 344 128 个（3.3 倍大）72 个角度分区的单元图时，需要 30-200 毫秒。

        下面是 3 个规划器规划约 75 米路径的示例。

• 2D A* 在 243 毫秒内计算出路径（面板 1）
• Hybrid-A* 在 144 毫秒内计算出路径（图 2）
• State Lattice 在 113ms 内计算出路径（面板 3）

        供参考： NavFn 在 146 毫秒内计算出路径，包括一些令人讨厌的路径不连续性伪影

四、设计

        基本设计集中了一个模板化的 A* 实现，用于处理节点图的搜索。该实现由节点 NodeT 模板化，节点 NodeT 包含计算统计量、旅行成本和搜索邻域所需的方法。这样设计的结果就是一个标准的 A* 实现，只要能创建一个节点模板，它就能用于遍历任何类型的图。

        目前，我们默认提供 3 个节点。2D 节点模板 (Node2D)，可使用 4 或 8 个连接邻域进行 2D 网格搜索。我们还提供了混合 A* 节点模板（NodeHybrid），可在 Dubin 或 Reeds-Shepp 运动模型上进行 SE2（X、Y、θ）搜索和碰撞检查。我们还提供了用于状态网格规划的网格（NodeLattice）节点，可利用差分和全向机器人的更广泛速度选项。其他模板也可以很容易地制作和加入，用于三维网格搜索和非网格基础搜索（如路由）。

        在面向 ROS2 的插件中，我们接收了全局目标，并对数据进行了预处理，以便将其输入到所使用的模板 A* 中。这包括处理任何请求，将成本映射缩小到另一种分辨率以加快搜索速度，以及平滑生成的 A* 路径（由于生成的网格取决于成本映射分辨率，因此无法用于状态网格）。对于 SmacPlannerHybrid 和 SmacPlannerLattice 插件，由于在分支搜索中使用了运动学上有效的模型，因此路径在运动学上是可行的。对于差分和全向机器人来说，2D A* 也是可行的。

        我们将 A*、costmap 下采样器、平滑器和节点模板对象从 ROS2 中分离出来，使它们可以独立于 ROS 或规划器进行测试。唯一使用 ROS 的地方是规划器插件本身。

五、参数

        请参阅 SmacPlanner 中参数的内联说明。其中包括适用于 SmacPlanner2D 和 SmacPlanner 的特定参数的注释。

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]

    GridBased:
      plugin: "nav2_smac_planner::SmacPlannerHybrid"
      tolerance: 0.5                      # tolerance for planning if unable to reach exact pose, in meters
      downsample_costmap: false           # whether or not to downsample the map
      downsampling_factor: 1              # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      allow_unknown: false                # allow traveling in unknown space
      max_iterations: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_on_approach_iterations: 1000    # maximum number of iterations to attempt to reach goal once in tolerance
      terminal_checking_interval: 5000     # number of iterations between checking if the goal has been cancelled or planner timed out
      max_planning_time: 3.5              # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
      motion_model_for_search: "DUBIN"    # For Hybrid Dubin, Redds-Shepp
      cost_travel_multiplier: 2.0         # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
      angle_quantization_bins: 64         # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
      analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 3.0    # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius
      analytic_expansion_max_cost: true   # For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be valid (except when necessary on approach to goal)
      analytic_expansion_max_cost_override: false  #  For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
      minimum_turning_radius: 0.40        # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
      reverse_penalty: 2.1                # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.0                 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
      non_straight_penalty: 1.20          # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 2.0                   # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      retrospective_penalty: 0.025        # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0
      rotation_penalty: 5.0               # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings.
      lookup_table_size: 20.0               # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: True      # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.  
      allow_reverse_expansion: False      # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step.   
      smooth_path: True                   # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes.
      debug_visualizations: True                # For Hybrid/Lattice nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. 
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: true               # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further

六、话题

Topic	Type
unsmoothed_path	nav_msgs/Path

七、安装

sudo apt-get install ros-<ros2-distro>-nav2-smac-planner

八、等（重要附注）

8.1 潜在字段

        我发现许多用户和默认导航配置文件都忽略了膨胀层的意义。虽然您确实可以简单地在墙壁周围充气一个小半径，但膨胀层的真正价值是在整个地图周围创建一个一致的势场。

        一些最受欢迎的导航/导航 2 调整指南甚至特别指出，在应用刻划成本后，在整个地图宽度上创建一个平缓的势场有很大的好处，但很少有用户会这样做。

        这种习惯实际上导致 NavFn、Global Planner 和现在的 SmacPlanner 生成的路径有些不理想。它们真正想要寻找的是平滑的潜在区域，而不是开阔的 0 成本空间，以便停留在空间中间，更好地处理近距离移动的障碍物。

        因此，我建议在使用此软件包以及 ROS 中所有其他成本感知搜索规划器时，增加膨胀层的成本比例，以便在整个地图上充分生成平滑的势场。对于非常大的开放空间，在中间设置 0 成本区域就可以了，但对于大厅、过道等类似区域，请创建一个平滑的势能，以提供最佳性能。

8.2 混合 A* 和状态网格转弯半径

        一个非常合理且合乎逻辑的假设是将最小转弯半径设置为车辆的运动极限。对于阿克曼汽车来说，这是一个物理量；而对于差分或全向机器人来说，则需要考虑机器人能够进行何种转弯。显然，将其设置得非常小（如 20 厘米）意味着您有很多选择，但也可能意味着当您在 2 米多宽的过道上工作时，原始输出方案不会非常平直和顺畅。

        我认为，在设置时还应考虑您的工作环境。虽然您绝对不应该将其设置得比您车辆的实际限制还要小，但在实际使用中，增加该值还是有一些有用的副作用的。如果您要在比机器人转弯半径更宽的区域内工作，您可以选择一些选项来最终提高规划器的性能（在 CPU 和计算时间方面），并直接从规划器生成更 “平滑 ”的路径，而不需要任何显式路径平滑。

        默认情况下，0.4m 是我认为对于较小规模的工业级机器人（如 Simbe、小型 Fetch 或 Locus 机器人）来说比较 “合理 ”的设置，它能产生更快的计划和更少的 “摇摆 ”运动，无需进行后期平滑处理，从而进一步提高 CPU 性能。我之所以选择 0.4 米，是为了在上述实用机器人与业余爱好者使用的小乌龟机器人-3 之间进行权衡，后者可能仍需要在一些较小的空腔内进行导航。

8.3 成本地图分辨率

        我们为 Hybrid-A*、State Lattice 和 2D A* 实现提供了代价图下采样器选项。当在较大空间中规划很长的路径时，这将大有裨益。SE2 规划和 2D 规划中邻域搜索的运动模型与代价图分辨率成正比。通过降低采样率，可以减少实现特定目标所需的扩展次数 N^2。不过，分辨率越低，出现的小障碍物就越大，也就无法超级接近障碍物。这需要进行权衡和测试。我看到的一些数据是，降低 2-3 倍采样率，CPU 规划时间就会减少 2-4 倍。对于长而复杂的路径，我只用了 2 倍的下采样率，就能使规划时间 << 100 毫秒，否则规划时间将高达 400 毫秒。

        我建议用户使用 5 厘米分辨率的成本地图，并使用不同的下采样率值，直到达到他们认为的最佳性能（最少的扩展次数与实现精细目标姿势的必要性）。然后，我建议将全局成本映射分辨率改为这个新值。这样你就不需要计算降采样和维护一个没有使用的更高分辨率成本映射。

        请记住，全局代价图只是为规划者提供一个工作环境。即使分辨率更高的代价图更 “讨人喜欢”，它也不是用来供人查看的。如果在规划器服务器中使用多个规划器，则应为最需要的规划器使用最高分辨率，然后使用 下采样器将采样率降至 Hybrid-A* 分辨率。

8.4 惩罚函数调整

        所有规划器的惩罚函数默认值都是根据 5 厘米成本图调整的。虽然对其进行一些调整不会对默认行为产生很大影响，但还是应该根据具体应用和需求进行调整。默认值的调整是为了在大量平台和分辨率下具有良好的开箱即用性能。在大多数情况下，您应该不需要对它们进行调整。

        不过，由于状态网格规划器可以使用任意数量的自定义生成的最小控制集，因此该规划器可能需要进行更多调整才能获得良好的性能。状态网格的默认值是使用本软件包中的 5cm Ackermann 文件作为初始示例生成的。在对 Hybrid-A* 规划器的公式进行修改后，默认的变化惩罚关闭似乎能产生不错的结果，但请根据您的应用需求和运行速度要求进行调整。

        调整时，每种惩罚的 “合理 ”范围如下。虽然您显然可以在这些范围之外进行调整，但我发现它们提供了很好的权衡，在这些范围之外的行为很快就会变得不理想。

• Cost（成本）：1.7 - 6.0
• Non-Straight（Non-Straight）：1.0 - 1.3
• Change（变化）： 0.0 - 0.3
• Reverse（反向） 1.3 - 5.0

请注意，变化惩罚必须大于 0.0。非直线、反向和成本惩罚必须严格大于 1.0。

8.5 未找到明确有效的目标或计算时间过长的路径

        在讨论以下部分之前，请确保您已适当设置了最大迭代参数。如果你有一个 1 平方公里大小的仓库，5000 次扩展显然是不够的。如果无法实现目标，可以尝试增加该值，或者使用 -1 值禁用该值，看看现在是否能在合理的时间内完成计划。如果您仍有问题，最好注意一下可能出现的次生效应。

        在差距或漏洞较小的地图上，您可能会发现在规划某些区域时出现问题。如果空隙小到不可逆转，但又大到通货膨胀无法用膨胀的成本将其关闭，那么建议对地图进行轻微修整或增加通货膨胀，将这些空间从非致命空间中移除。

        请看下图，你会发现有人试图在地图上规划出一个 “U ”形区域。第一张图显示了地图上的一个小缺口（来自一次不完美的 SLAM 会话），它几乎可以穿越，但并不完全。从起始位置看，该缺口是通往目标的最短路径，因此启发式方法会尝试将路径引向该方向。然而，在脚印设定的情况下，实际上不可能通过一条运动学上有效的路径。因此，规划器会展开最多 1,000,000 次迭代，试图通过它（红色显示）。如果允许无限次迭代，最终会找到一条有效路径，但可能需要大量时间。

        只需增加占地面积（有点麻烦，最好的办法是编辑地图，使该区域无法通行），那么现在该缺口就会被正确地封锁为不可通行区域。在第二张图中，你可以看到启发式方法影响了可导航路线的扩展，并能在不到 10,000 次的迭代（或约 110 毫秒）中找到一条路径。现在很容易了！

        因此，如果您的 SLAM 地图比较稀疏，地图上存在缺口或漏洞，建议您轻轻地对其进行后处理，以填补这些缺口或增加足迹的填充或半径，使这些区域无效。如果不这样做，可能会在这条小走廊上浪费扩展空间： A）您不希望您的机器人实际使用这些区域；B）这些区域实际上可能无效，是 SLAM 的伪影；C）如果有更开阔的空间，您更希望机器人使用这些空间。

        从第二张图中可以看到一个有趣的现象，那就是在开放空间中出现了一些扩展。这是由于行程/启发式值非常相似，调整惩罚权重值会对其产生相当大的影响。默认设置是大型开放空间和狭窄过道之间的良好中间点（可针对具体环境进行调整，以减少特定地图的扩张数量，从而加快规划器的运行速度）。实际上，搜索区域/环境越是狭窄，规划器的运行速度就越快，但即使是在宽阔的开放区域，规划器的运行速度仍然很快！

        有时，可视化扩展对于调试潜在的问题（为什么这个目标需要更长的计算时间，为什么我找不到路径等）非常有用，如果你偶尔遇到问题的话。您可以在 SmacHybrid 的/expansions 主题中使用 debug_visualizations 参数启用扩展的发布： true，但请注意，这只能用于调试，因为它会增加大量的 CPU 占用率。