Smac Planner

SmacPlanner 是 Nav2 Planner 的插件。它目前包括 3 个不同的插件：
1.SmacPlannerHybrid：
高度优化完全可重新配置的 Hybrid-A* 实现，支持 Dubin 和 Reeds-Shepp 模型（腿足模型、ackermann 模型和汽车模型）。
2.SmacPlannerLattice:
高度优化完全可重新配置的状态格实现，支持可配置的最小控制集，并为阿克曼模型、腿足模型、差速模型和全向模型提供了控制集。
3.SmacPlanner2D：高度优化完全可重构的基于网格的 A* 实现，支持 8 个连接的邻域模型。

它还介绍了以下基本构建块
CostmapDownsampler：一个库，用于接收代价图对象并将其下采样到另一个分辨率。
AStar：规划插件使用搜索通用且高度优化的 A* 模板库。使用它还可以提供用于规划的其他模板。CollisionChecker：基于机器人半径或足迹的碰撞检查。
Smoother：一个简单的路径平滑器，用于平滑 2D、Hybrid-A* 和 State Lattice 路径。

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

介绍说明：
nav2_smac_planner 包含一个优化的模板化 A* 搜索算法，用于为多种类型的机器人平台创建多个基于 A* 的规划器。它是由 Steve Macenski 在三星研究院时建造的。使用实现代价感知 A* 规划器的 SmacPlanner2D 规划器支持圆形差速驱动和全向驱动机器人。使用实现 Hybrid-A* 规划器的 SmacPlannerHybrid 插件支持腿足机器人、汽车、类车和阿克曼车辆。支持实现状态栅格规划器的 SmacPlannerLattice 插件可用于非圆形、任意形状的任何模型车辆。它包含用于阿克曼、腿式、差速驱动和全向车辆，但您可以为另一种机器人类型提供自己的，或具有不同的规划行为。最后两个插件对于曲率约束或运动学上可行的规划也很有用，例如在高速规划机器人以确保它们不会翻转或以其他方式打滑时失控。它也适用于需要基于姿势的碰撞检查的非圆形机器人（例如差速/全向传动系统的大矩形或任意形状的机器人）。

SmacPlannerHybrid 实现了 Hybrid-A* 规划器，如 Practical Search Techniques in Path Planning for Autonomous Driving 中提出的那样，修改了启发式遍历函数以提高路径质量，而无需昂贵的基于优化的平滑处理。

SmacPlannerLattice 实现了 State Lattice 规划器。虽然我们没有与State Lattices中的最优、平滑、非完整移动机器人运动规划（Optimal, Smooth, Nonholonomic MobileRobot Motion Planning in State Lattices）完全相同的方式实现它（使用为离散状态空间中的约束运动规划生成近最小跨度控制集找到控制集），但是它非常相似，可以作为一个很好的参考。使用了对接近分析扩展和改进的启发式函数的额外优化，在很大程度上与 Hybrid-A* 相匹配，以允许它们共享这些优化的实现，以更快地推动搜索朝着目标前进。

总结：
SmacPlannerHybrid 设计用于：
1.阿克曼、汽车和类车机器人
2.高速或曲率受限的机器人（以免高速翻转、打滑或倾卸负载）
3.任意形状、非圆形差速或全向机器人需要运动学上可行的规划和 SE2 碰撞检查
4.腿式机器人

SmacPlannerLattice 设计用于： 
1.任意形状的非圆形机器人需要运动学上可行的规划，并使用传动系统的全部功能进行 SE2 碰撞检查 
2.灵活使用其他机器人模型类型或提供非圆形差速器、ackermann 和 omni支持

SmacPlanner2D 设计用于：
1.圆形、差速或全向机器人
2.相对于环境大小的相对较小的机器人（例如走廊中的遥控车或会议中心中的大型机器人），可以用圆形足迹来近似。

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

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

2D A* 实现也没有任何由 NavFn Planner 中基于梯度波前的 2D A* 实现引入的奇怪伪影。虽然这个 2D A* 规划器稍微慢一些，但我相信路径质量的提高是非常值得的。

注意：在以前的版本中，CG 平滑器主要实现了原始的 Hybrid-A* 论文。但是，此平滑器无法始终如一地提供有用的结果，占用了过多的计算时间，因此已被弃用。虽然在 95% 的时间里平滑路径似乎是一个“好的”解决方案，但我们需要一些更可靠的方法来实际使用。由于我们使用的是移动机器人而不是时速 60 英里的自动驾驶汽车，我们可以在平滑方面采取一些不同的思路，因为我们知道我们的局部轨迹规划器非常智能。如果您正在寻找它，它现在作为成本感知平滑器存在于新的平滑服务器中。这个平滑器已被更简单的优化启发解决方案所取代，该解决方案更快、更一致且更易于理解。虽然这种平滑器不具有代价意识，但我们在规划器本身中添加了具有代价意识的惩罚函数，以将计划推离高代价空间，并且我们会检查平滑部分的有效性以确保可行性。当路径与环境发生冲突时，它将终止。但是，如果您想使用此平滑器，它可以在平滑器服务器中使用，但它会花费一些额外的计算时间。

指标：
最初的 Hybrid-A* 实现拥有 50-300 毫秒的规划时间，可以规划 102,400 个的单元地图。我们在评估中看到更快的结果：
1.2-20 毫秒，用于规划 147,456 个（大 1.4 倍）的单元图。
2.30-200 毫秒，用于规划 344,128 个（大 3.3 倍）的单元图。

下面是 3 个规划器的示例，规划了大约 75 m 的路径。
2D A* 在 243 毫秒内计算出路径（图 1）
Hybrid-A* 在 144 毫秒内计算出路径（图 2）
State Lattice 在 113 毫秒内计算出路径（图 3）
供参考：NavFn 在 146 毫秒内计算路径，包括一些令人讨厌的路径不连续伪影

图 1

图 2

图 3

设计：
基本设计集中了一个模板化的 A* 实现，用于处理节点图的搜索。该实现由节点 NodeT 模板化，其中包含计算启发式方法、旅行成本和搜索邻域所需的方法。这种设计的结果是一个标准的 A* 实现，可用于遍历任何类型的图，只要可以为其创建节点模板即可。

目前默认提供3个节点。 2D 节点模板 (Node2D)，它使用 4 或 8 连接的邻域进行 2D 网格搜索。还提供了一个混合 A* 节点模板 (NodeHybrid)，它在 Dubin 或 Reeds-Shepp 运动模型上进行 SE2 (X, Y, theta) 搜索和碰撞检查。还提供用于状态网格规划的 Lattice (NodeLattice) 节点，利用差速和全向机器人可用的更广泛的速度选项。可以轻松制作并包含其他模板，用于 3D 网格搜索和非网格基础搜索（如路由）。

在面向 ROS2 的插件中，采用全局目标并预处理数据以输入所使用的模板化 A*。这包括处理任何将成本图下采样到另一个分辨率的请求，以加快搜索速度并平滑生成的 A* 路径（由于生成的格取决于成本图分辨率，因此不适用于 State Lattice）。对于 SmacPlannerHybrid 和 SmacPlannerLattice 插件，由于分支搜索中使用的运动学有效模型，路径被证明在运动学上是可行的。二维 A* 也有望用于差速和全向机器人。

从 ROS2 中分离出 A*、costmap downsampler、smoother 和 Node 模板对象，使它们能够独立于 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
      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
      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.
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-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

其他：

势场：

发现的许多用户和默认导航配置文件确实缺少膨胀层的要点。虽然您确实可以简单地在墙壁周围膨胀一个小半径，但膨胀层的真正价值在于在整个地图周围创造一个一致的势场。

一些最流行的 Navigation / Nav2 调整指南甚至特别指出，在应用规定代价后，在整个地图宽度上创建一个平缓的势场有很大的好处，但很少有用户这样做。

这种习惯实际上导致 NavFn、Global Planner 和现在的 SmacPlanner 生成的路径在某种程度上不是最优的。他们真的很想寻找一个平滑的势场而不是开阔的 0 成本空间，以便留在空间中间并更好地处理附近移动的障碍物。

因此，建议使用此软件包以及 ROS 中可用的所有其他具有代价的搜索规划器，以增加您的通货膨胀层代价规模，以便在整个地图上充分产生平滑的势场。对于非常大的开放空间，中间有 0 成本的区域很好，但对于大厅、过道等；请创造一个平滑的势场，以提供最佳性能。

Hybrid-A* 和 State Lattice 转向半径

一个非常合理和合乎逻辑的假设是将 minimum_turning_radius 设置为车辆的运动学极限。对于阿克曼汽车，这是一个物理量；而对于差速或全向机器人，它有点像围绕希望机器人能够做出什么样的转弯。显然，将其设置为极小的值（如 20 厘米）意味着有很多选择，但也可能意味着当有 2 米以上宽的过道可以工作时，原始输出计划将不会非常笔直和流畅。

在设置时还应该考虑您在其中操作的环境。虽然绝对不应将此值设置为小于车辆的实际限制，但在实际使用中增加此值会产生一些有用的副作用。如果在比机器人的转弯圈更宽的区域工作，您有一些选项可以最终提高规划器的性能（在 CPU 和计算时间方面）以及直接生成更“平滑”的路径规划器——不需要任何明确的路径平滑。

默认情况下，0.4m 是认为对于较小规模的工业级机器人（想想 Simbe、小型 Fetch 或 Locus 机器人）来说“合理”的设置，从而导致更快的计划和更少的“摇摆不定”的动作，不需要后期平滑——进一步提高 CPU 性能。我选择 0.4m 作为上述实用机器人和使用 tiny-little-turtlebot-3 的业余爱好者用户之间的权衡，它可能仍需要在一些较小的空间周围导航。

代价地图分辨率

为 Hybrid-A*、State Lattice 和 2D A* 实现提供代价图下采样器选项。在较大空间中规划很长的路径时，这可能非常有用。 2D 规划中用于 SE2 规划和邻域搜索的运动模型与代价图分辨率成正比。通过对其进行下采样，您可以 以N^2 减少实现特定目标所需的扩展次数。但是，分辨率越低，出现的小障碍物就越大，您将无法离障碍物太近。这是设计和测试的权衡。我看到的一些数字是计划 CPU 时间下降 2-4 倍，以实现 2-3 倍的下采样率。对于又长又复杂的路径，我能够以 2 倍的下采样率从计划中获得 << 100 毫秒，否则需要 400 毫秒以上的时间。

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

请记住，全局代价图只是为规划者提供一个工作环境。即使更精细的分辨率代价图更让人“愉悦”，它也不是供人类查看的。如果您在规划器服务器中使用多个规划器，那么您将希望为最需要的规划器使用最高分辨率，然后使用下采样器下采样到 Hybrid-A* 分辨率。

惩罚函数调整

基于 5cm 代价图为所有规划器调整惩罚函数默认值。虽然这方面的某些更改不会对默认行为产生很大影响，但针对您的特定应用程序和需求进行调整可能会很好。默认值已调整为具有适用于大量平台和分辨率的开箱即用行为。在大多数情况下，您不需要去更改它。

然而，由于 State Lattice 规划器的性质能够使用任意数量的自定义生成的最小控制集，因此该规划器可能需要更多调整才能获得良好的行为。 State Lattice 的默认值是使用 5cm Ackermann 文件生成的，您可以在此包中找到作为初始示例的文件。在 Hybrid-A* 规划器的公式发生变化后，默认的更改惩罚关闭似乎产生了良好的结果，但请根据您的应用程序需求和运行时速度要求进行调整。

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

Cost: 1.7 - 6.0
Non-Straight: 1.0 - 1.3
Change: 0.0 - 0.3
Reverse: 1.3 - 5.0

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

对于有效目标找不到路径或计算出路径时间太长

在处理以下部分之前，请确保您已适当设置最大迭代次数参数。如果你有一个 1 km2 大小的仓库，显然 5000 个扩展是不够的。如果您无法实现目标或使用 -1 值禁用它以查看您现在是否能够在合理的时间段内进行计划，请尝试增加此值。如果您仍然有问题，则可能会发生次要影响，需要注意这一点。

在具有小间隙或孔洞的地图中，您可能会看到规划到某些区域的问题。如果间隙小到无法穿越但又大到膨胀不会因膨胀而关闭它，那么建议轻轻修改地图或增加膨胀以将这些空间从非致命空间中移除。

查看下图，您将看到在地图上规划成“U”形区域的尝试。第一张图显示了地图中的小间隙（来自不完美的 SLAM ），它几乎可以穿越，但不是完全可以穿越。从起始位置开始，该间隙产生了到达目标的最短路径，因此启发式算法将尝试在该方向上产生路径。但是，实际上不可能通过具有足迹集的运动学有效路径。结果，规划器扩展了所有最大 1,000,000 次迭代，试图通过它（以红色显示）。如果允许无限次迭代，最终会找到一条有效路径，但可能会花费大量时间。

通过简单地增加足迹大小（最好的解决方案是编辑地图使该区域无法通行），然后该间隙现在被正确地阻止为不可导航。在第二张图中，您可以看到启发式算法会影响沿可导航路线的扩展，并且能够在不到 10,000 次迭代（或大约 110 毫秒）内找到路径。现在很容易！

因此，如果您的地图中有稀疏的 SLAM 地图、间隙或孔洞，建议您对它们进行轻微的后处理以填充这些间隙或增加足迹的填充或半径以使这些区域无效。没有它，它可能会浪费在这个小走廊上的扩展。