ROS2与Navigation2入门教程-配置Smac规划器

说明：

• 介绍如何配置Smac规划器

概述

• Smac规划器的源代码及含有设计、解释和指标的自述文件可以在Github网站上找到。

• Smac规划器插件实现了一个2D A和混合-A（Hybrid-A*）路径规划器。

• 重要的是要知道，至2021年6月，该规划器进行了一项重大更新，改善了路径质量且运行时间提高了2-3倍。

• Hybrid-A*规划器的一个示例如下图所示，在73毫秒内规划了一个60米的路径。

• 对于大多数环境，通常的规划时间会低于100毫秒，偶尔会达到200毫秒。

• 是为该类型选择的相应规划器插件ID。

参数

• .tolerance参数：

数据类型：double
默认值：0.5
描述：用于设置请求的目标位姿与路径终点之间的容许误差，单位为m。

• .downsample_costmap参数：

数据类型：bool
默认值：False
描述：用于设置是否将成本地图下采样到另一个分辨率以进行搜索。

• .downsampling_factor参数：

数据类型：int
默认值：1
描述：用于设置对成本地图进行下采样的乘数因子（例如，如果对 5cm分辨率的成本地图进行下采样的乘数因子为2，则会输出分辨率为10cm的成本地图）。

• .allow_unknown参数：

数据类型：bool
默认值：True
描述：用于设置是否允许在未知空间中进行遍历/搜索。

• .max_iterations参数：

数据类型：int
默认值：1000000
描述：用于设置限制计算时间失败之前的最大搜索迭代次数，通过设置为-1来禁用。

• .max_on_approach_iterations参数：

数据类型：int
默认值：1000
描述：用于设置最大迭代次数，如果未找到精确的路径，则在返回使用最佳启发式算法计算的近似路径之前且搜索位于容许误差范围之内的最大迭代次数。仅限在2D A*中使用。

• .max_planning_time参数：

数据类型：double
默认值：5.0
描述：用于设置最长的规划时间，单位为秒。

• .analytic_expansion_ratio参数：

数据类型：double
默认值：3.5
描述：用于设置分析扩展比率，SE2节点会尝试完成与此参数值和最小启发式成比例的分析扩展。

• .motion_model_for_search参数：

数据类型：string
默认值：“DUBIN”
描述：设置用于搜索的运动模型枚举字符串。对于Hybrid-A*，默认值为“DUBIN”。对于2D A*，默认值为“MOORE”。对于SE2节点，此参数值的可选项为DUBIN或REEDS_SHEPP。对于2D A*节点，此参数值的可选项为MOORE或VON_NEUMANN。对于状态点阵（state lattice），则要使用STATE_LATTICE作为此参数值。

• .cost_travel_multiplier参数：

数据类型：double
默认值：2.0
描述：用于设置行进成本乘数。对于2D A*，成本乘数会应用于搜索以避开高成本区域。较大的值放置在过道的中心会更准确（如果有non-FREE成本潜力字段的话），但计算时间会稍长。为了优化速度，值1.0是合理的。一个合理的权衡值为2.0。值0.0会有效地禁止机器人驶离障碍物，且表现得像朴素二分搜索A*算法一样。

• .angle_quantization_bins参数：

数据类型：int
默认值：1
描述：用于设置SE2搜索用的角度格网（angular bins）数。对于 2D A*，其值必须为1；对于SE2，其值可以是任何数字，但一个好的基线是72（对于每个角度格网为5度的情况而言）。

• .minimum_turning_radius参数：

数据类型：double
默认值：0.4
描述：用于设置车辆最小转弯半径（单位为m）。此参数还会用于平滑器以计算最大曲率。

• .reverse_penalty参数：

数据类型：double
默认值：2.0
描述：用于设置反向搜索的惩罚力度。如果反向搜索，则会对SE2节点进行启发式惩罚。仅在REEDS_SHEPP运动模型中使用。

• .change_penalty参数：

数据类型：double
默认值：0.15
描述：用于设置改变搜索方向的惩罚力度。如果在搜索中改变方向，则会对SE2节点进行启发式惩罚。

• .non_straight_penalty参数：

数据类型：double
默认值：1.5
描述：用于设置非直线方向搜索的惩罚力度。如果在非直线方向上搜索，则会对SE2节点进行启发式惩罚。

• .cost_penalty参数：

数据类型：double
默认值：1.7
描述：用于设置位姿成本惩罚力度。针对位姿成本对SE2节点进行启发式惩罚。允许Hybrid-A*算法具有成本意识。

• .lattice_filepath参数：

数据类型：string
默认值：“”
描述：用于设置状态点阵图的文件路径。

• .lookup_table_size参数：

数据类型：double
默认值：20.0
描述：用于设置要缓存的dubin/reeds-sheep距离窗口大小，单位为m。

• .cache_obstacle_heuristic参数：

数据类型：bool
默认值：false
描述：用于设置是否缓存障碍物启发式，即是否缓存相同目标位置的后续重新规划之间的障碍物地图动态编程距离扩展启发式。如果成本地图基本上是静态的，则可显著提高重新规划的性能（高达40倍）。

• .smoother.max_iterations参数：

数据类型：int
默认值：1000
描述：用于设置平滑器最大迭代次数。

• .smoother.w_smooth参数：

数据类型：double
默认值：0.3
描述：设置要应用于平滑数据点的平滑器权重。

.smoother.w_data参数：

数据类型：double
默认值：0.2
描述：设置要应用于保留原始数据信息的平滑器权重。

• .smoother.tolerance参数：

数据类型：double
默认值：1e-10
描述：用于设置要终止平滑会话的参数容差变化量。

示例

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    use_sim_time: True

    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner"
      tolerance: 0.5                      # tolerance for planning if unable to reach exact pose, in meters, for 2D node
      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, 2D only
      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"    # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally
      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/Lattice 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.
      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.15                # For Hybrid/Lattice nodes: penalty to apply if motion is changing directions, must be >= 0
      non_straight_penalty: 1.50          # For Hybrid/Lattice nodes: penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 1.7                   # For Hybrid/Lattice 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.
      lattice_filepath: ""                # For Lattice node: the filepath to the state lattice graph
      lookup_table_size: 20               # For Hybrid/Lattice nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: True      # For Hybrid/Lattice 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.
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10

参考：

• https://navigation.ros.org/configuration/packages/configuring-smac-planner.html