Constrained smoother

源代码在 Github.

nav2_smoother 的平滑器插件基于 nav2_smac_planner 中原有的已弃用的平滑器，由“RoboTech Vision”投入运行。 适用于需要将规划的全局路径推离障碍物和/或 Reeds-Shepp 运动模型的应用程序。它针对大型 Ceres 优化程序中的路径长度、平滑度、与障碍物的距离和曲率进行优化。 .. TruncatePathLocal BT Node: bt-plugins/actions/TruncatePathLocal.html

重要提示：约束平滑器使用相当繁重的优化算法，因此建议在周期性截断的路径上使用。 TruncatePathLocal BT Node 可用于实现适当的路径长度，DistanceController BT Node 可用于实现周期性。

下图描述了约束平滑器如何改善输入路径的质量（青色，由过时的 Smac Planner 版本生成，故意未进行最佳配置以突出平滑器的强大功能）， 增加其平滑度和与障碍物的距离。结果路径以绿色标记。注意：由于此路径上使用 TruncatePathLocal，因此最后几个路径姿势未平滑。

更平滑的服务器参数

reversing_enabled:

Type	Default
bool	true

Description

是否检测正向/反向方向和尖点。对于未指定方向的路径，应设置为 false

path_downsampling_factor:

Type	Default
int	1

Description

路径上的每 n 个节点都会被优化。有助于提高速度

path_upsampling_factor:

Type	Default
int	1

Description

用于细化的上采样因子。0 - 路径保持下采样（参见``path_downsampling_factor``），1 - 使用三次贝塞尔将路径上采样回原始粒度，2… - 更多上采样

keep_start_orientation:

Type	Default
bool	true

Description

是否防止起始方向被平滑

keep_goal_orientation:

Type	Default
bool	true

Description

是否防止目标定位被平滑

minimum_turning_radius:

Type	Default
double	0.4

Description

机器人可以执行的最小转弯半径。对于 diff-drive/holonomic 机器人，可以设置为 0.0（或将 w_curve 设置为 0.0 以获得相同效果）

w_curve:

Type	Default
double	30.0

Description

强制最小转弯半径的重量

w_dist:

Type	Default
double	0.0

Description

将路径与原件绑定的权重作为成本权重的可选替代方案

w_smooth:

Type	Default
double	2000000.0

Description

权重最大化路径平滑度

w_cost:

Type	Default
double	0.015

Description

控制机器人避免碰撞的重量和成本

w_cost_cusp_multiplier:

Type	Default
double	3.0

Description 在正向/反向方向改变期间使用更高权重的选项，帮助优化器收敛或在这些问题部分添加额外的避障。 下图显示了使用“w_cost_cusp_multiplier”（绿色）的路径与没有使用“w_cost_cusp_multiplier”（紫色）的路径相比的改进。原始路径为青色。

cusp_zone_length:

Type	Default
double	2.5

Description

节点使用“w_cost_cusp_multiplier”的尖点周围部分的长度（w_cost 在区域内向尖点逐渐上升，其成本图权重等于 w_cost*w_cost_cusp_multiplier）

cost_check_points:

Type	Default
array of double	[]

Description

机器人框架中用于获取代价地图值的点。格式：[x1, y1, weight1, x2, y2, weight2, …]。

重要提示：需要大量优化器迭代才能真正改善路径。仅在真正需要时使用（高度拉长/不对称的机器人）

下图描述了如何使用两个成本检查点来提高矩形机器人的成本意识。

optimizer.max_iterations:

Type	Default
int	100

Description

优化器迭代的最大次数

optimizer.debug_optimizer:

Type	Default
bool	false

Description

是否打印优化器调试信息

optimizer.linear_solver_type:

Type	Default
string	“SPARSE_NORMAL_CHOLESKY”

Description

优化器使用的线性求解器类型。有效值为“SPARSE_NORMAL_CHOLESKY”和“DENSE_QR”

optimizer.gradient_tol:

Type	Default
bool	1e-10

Description

梯度容差优化终止准则

optimizer.fn_tol:

Type	Default
bool	1e-7

Description

函数容差优化终止准则

optimizer.param_tol:

Type	Default
bool	1e-15

Description

参数容差优化终止准则

Example

smoother_server:
  ros__parameters:
    use_sim_time: True
    smoother_plugins: ["SmoothPath"]

    SmoothPath:
      plugin: "nav2_constrained_smoother/ConstrainedSmoother"
      reversing_enabled: true       # whether to detect forward/reverse direction and cusps. Should be set to false for paths without orientations assigned
      path_downsampling_factor: 3   # every n-th node of the path is taken. Useful for speed-up
      path_upsampling_factor: 1     # 0 - path remains downsampled, 1 - path is upsampled back to original granularity using cubic bezier, 2... - more upsampling
      keep_start_orientation: true  # whether to prevent the start orientation from being smoothed
      keep_goal_orientation: true   # whether to prevent the gpal orientation from being smoothed
      minimum_turning_radius: 0.40  # minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 with the same effect) for diff-drive/holonomic robots
      w_curve: 30.0                 # weight to enforce minimum_turning_radius
      w_dist: 0.0                   # weight to bind path to original as optional replacement for cost weight
      w_smooth: 2000000.0           # weight to maximize smoothness of path
      w_cost: 0.015                 # weight to steer robot away from collision and cost

      # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes)
      w_cost_cusp_multiplier: 3.0   # option to use higher weight during forward/reverse direction change which is often accompanied with dangerous rotations
      cusp_zone_length: 2.5         # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight eqals w_cost*w_cost_cusp_multiplier)

      # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...]
      # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots)
      # cost_check_points: [-0.185, 0.0, 1.0]

      optimizer:
        max_iterations: 70            # max iterations of smoother
        debug_optimizer: false        # print debug info
        gradient_tol: 5e3
        fn_tol: 1.0e-15
        param_tol: 1.0e-20