系列文章目录
前言
这是一个预测控制器（局部轨迹规划器），用于实现模型预测路径积分（MPPI）算法，以跟踪具有自适应避障功能的路径。它包含基于插件的 critic 函数，可影响算法的行为。它由 Aleksei Budyakov 创建，由 Steve Macenski 为 Nav2 进行改编和开发。
该插件实现了 nav2_core::Controller（控制器）接口，可在整个导航堆栈中作为控制器服务器的动作服务器（controller_server）中的局部轨迹规划器使用。
据测定，该控制器可在中等英特尔处理器（第四代 i5）上以 50+ Hz 的频率运行。有关其他参数说明，请参阅其 “配置指南 ”页面。
目前可与差分、全向和阿克曼机器人配合使用。
一、MPPI 描述
MPPI 算法是 MPC 的一种变体，它采用迭代法为机器人找到控制速度。利用前一时间步的最佳控制解决方案和机器人的当前状态，从高斯分布中随机采样一组扰动。对这些噪声控制进行正向仿真，在机器人的运动模型中生成一组轨迹。
接下来，使用一组基于插件的 critic 函数对这些轨迹进行评分，以找出批次中的最佳轨迹。输出分数用于使用软最大值函数设置最佳控制。
这个过程会重复多次，并返回一个收敛的解决方案。该解决方案将作为下一个时间步骤初始控制的基础。
二、特点
- 预测性 MPC 轨迹规划器
- 采用基于插件的标准，用户可轻松更换、调整或替换这些标准
- 利用矢量化和张量运算高度优化了仅在 CPU 上运行的性能
- 支持多种常见运动模型，包括阿克曼、差动驱动和全向运动模型
- 包括在升级到恢复行为之前处理软故障的后备机制
- 通过 Doxygen、高单元测试覆盖率、文档和参数指南实现高质量代码
- 易于扩展，可支持 MPPI 的现代研究变体
- 经过预调整，具有良好的开箱即用性能
三、配置
3.1 控制器
| 参数 | 类型 | 定义 |
|---|---|---|
| motion_model | string | Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann]. 默认值： DiffDrive。模型类型 [DiffDrive、Omni、Ackermann]。 |
| critics | string | Default: None. Critics (plugins) names 默认值： 无。Critics（插件）名称 |
| iteration_count | int | Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches. 默认值 1。MPPI 算法中的迭代次数。建议保留为 1，并优先选择更多批次。 |
| batch_size | int | Default 1000. Count of randomly sampled candidate trajectories 默认为 1000。随机取样的候选轨迹数 |
| time_steps | int | Default 56. Number of time steps (points) in each sampled trajectory 默认值 56。每个采样轨迹的时间步数（点数） |
| model_dt | double | Default: 0.05. Time interval (s) between two sampled points in trajectories. 默认值：0.05。轨迹中两个采样点之间的时间间隔（秒）。 |
| vx_std | double | Default 0.2. Sampling standard deviation for VX 默认值 0.2。VX 的采样标准偏差 |
| vy_std | double | Default 0.2. Sampling standard deviation for VY 默认值 0.2。VY 的采样标准偏差 |
| wz_std | double | Default 0.4. Sampling standard deviation for Wz 默认值 0.4。Wz 的采样标准偏差 |
| vx_max | double | Default 0.5. Max VX (m/s) 默认值 0.5。最大 VX（米/秒） |
| vy_max | double | Default 0.5. Max VY in either direction, if holonomic. (m/s) 默认为 0.5。任一方向上的最大 VY（如果是整体工程学）。(米/秒） |
| vx_min | double | Default -0.35. Min VX (m/s) 默认值 -0.35。最小 VX（米/秒） |
| wz_max | double | Default 1.9. Max WZ (rad/s) 默认为 1.9。最大 WZ（弧度/秒） |
| temperature | double | Default: 0.3. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in consideration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration 默认值：0.3。根据成本选择轨迹（该值越接近 0，我们就越多地考虑成本较低的控制），0 表示使用成本最优的控制，巨大的值将导致只考虑所有轨迹的平均值而不考虑成本。 |
| gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of 默认值：0.015。平滑度（高）和低能量（低）之间的权衡。这是一个复杂的参数，很可能不需要更改默认值 0.1，因为默认值 0.1 适用于多种情况。参见 "信息论模型预测控制： 中的第 3D-2 节。 |
| visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. 默认值：false。发布可视化轨迹，这会大大降低控制器的运行速度。仅用于调试。 |
| retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. 默认值 1。软重置失败时，在报告失败前寻找可行轨迹的尝试次数。 |
| regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. 默认为 false。是每次迭代都重新生成噪声，还是使用初始化和重置时计算的单一噪声分布。实际上，由于轨迹是从正态分布中随机抽样的，因此这种方法效果很好，还能减少运行时由于唤醒线程重新采样正态分布而造成的计算抖动。 |
3.2 轨迹可视化
| 参数 | 类型 | 定义 |
|---|---|---|
| trajectory_step | int | Default: 5. The step between trajectories to visualize to downsample candidate trajectory pool. 默认值：5.为降低候选轨迹库采样率而进行可视化的轨迹间距。 |
| time_step | int | Default: 3. The step between points on trajectories to visualize to downsample trajectory density. 默认值：3.可视化轨迹点之间的步长，以降低轨迹密度。 |
3.3 路径处理器
| 参数 | 类型 | 定义 |
|---|---|---|
| max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. 默认值： 成本地图半径。机器人姿态前方的最大综合距离，用于在路径循环时搜索最近的路径点。 |
| prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. 默认值：1.5。修剪路径时机器人与路径上最近点的前方距离。 |
| transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. 默认值：0.1。使用 TF 进行数据转换的时间容差。 |
| enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. 默认值： 假。如果为 “true”，它将为改变方向（如路径反转）的线段剪切包含尖点的路径，这样控制器将被迫在规划器要求的反转点或其附近改变方向。这主要是针对拥有可行路径的 Smac Planner 用户，他们需要机器人在特别要求的地方转换方向。 |
| inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 默认值：0.2。与路径反转点的笛卡尔距离（米），在路径反转后被认为 “达到 ”了路径的其余部分。 |
| inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. 默认值：0.4。与路径反转点的角度距离（弧度），在路径反转后被视为 “达到 ”通过其余路径的要求。0.4 弧度 = 23 度。 |
3.4 阿克曼运动模型
| 参数 | 类型 | 定义 |
|---|---|---|
| min_turning_r | double | minimum turning radius for ackermann motion model 阿克曼运动模型的最小转弯半径 |
3.5 约束 Critic
| 参数 | 类型 | 定义 |
|---|---|---|
| cost_weight | double | Default 4.0. Weight to apply to critic term. 默认值 4.0。适用于 critic 的权重。 |
| cost_power | int | Default 1. Power order to apply to term. |
3.6 目标角度 Critic
| 参数 | 类型 | 定义 |
|---|---|---|
| cost_weight | double | Default 3.0. Weight to apply to critic term. 默认值 3.0。适用于 Critic 的权重。 |
| cost_power | int | Default 1. Power order to apply to term. |
| threshold_to_consider | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered. 默认值 0.5。机器人与目标之间的最小距离，超过该距离时将考虑角度目标成本。 |
3.7 目标 Critic
| 参数 | 类型 | 定义 |
|---|---|---|
| cost_weight | double | Default 5.0. Weight to apply to critic term. 默认值 5.0。适用于 Critic 的权重。 |
| cost_power | int | Default 1. Power order to apply to term. |
| threshold_to_consider | double | Default 1.4. Distance between robot and goal above which goal cost starts being considered 默认值 1.4. 机器人与目标之间的距离，超过此距离时开始考虑目标成本 |
3.8 障碍物 Critic
利用代价成本和膨胀参数估算与障碍物的距离，以避开障碍物
| 参数 | 类型 | 定义 |
|---|---|---|
| consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. 默认值： 假。是否使用点成本（如果机器人是圆形或计算能力较低）或计算 SE2 轮廓成本。 |
| critical_weight | double | Default 20.0. Weight to apply to critic for near collisions closer than 默认值 20.0。用于对距离碰撞边距（collision_margin_distance）较近的近距离碰撞进行批评的权重，仅用于防止近距离碰撞，作为一种虚拟扩大轮廓的方法。在一般情况下，不应用此权重来影响避开临界碰撞的障碍物。 |
| repulsion_weight | double | Default 1.5. Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the 默认为 1.5。对 critic 的权重，一般用于优先选择成本较低的路线。该权重与临界项分离，以便在不影响可能直接导致近距离碰撞的行动的情况下，对动态场景中的障碍物行为与路径对齐进行微调。该值应用于与障碍物的膨胀半径距离内。 |
| cost_power | int | Default 1. Power order to apply to term. |
| collision_cost | double | Default 100000.0. Cost to apply to a true collision in a trajectory. 默认值 100000.0。适用于轨迹中真实碰撞的成本。 |
| collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. 默认值 0.10。距离碰撞的边际距离，用于施加严重惩罚，类似于轮廓膨胀。0.05-0.2 之间比较合理。 |
| near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. 默认值 0.5。目标附近的距离，用于停止应用障碍物优先项，让机器人在接近障碍物时平稳地向目标姿势靠拢。 |
| inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. 默认为“”。膨胀层名称。如果为空，则使用成本计算图中的最后一个膨胀层。如果有多个膨胀层，可能需要指定要使用的膨胀层名称。 |
3.9 成本 Critic
直接使用膨胀的代价图成本来规避障碍
| 参数 | 类型 | 定义 |
|---|---|---|
| consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. 默认值： 假。是否使用点成本（如果机器人是圆形或计算能力较低）或计算 SE2 轮廓成本。 |
| cost_weight | double | Default 3.81. Wight to apply to critic to avoid obstacles. 默认 3.81。为避开障碍物而对 critic 施加的重量。 |
| cost_power | int | Default 1. Power order to apply to term. |
| collision_cost | double | Default 1000000.0. Cost to apply to a true collision in a trajectory. 默认值 1000000.0。适用于轨迹中真实碰撞的成本。 |
| critical_cost | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles. 默认值 300.0。应用于膨胀空间中任意点的姿势的成本，优先考虑与障碍物的距离。 |
| near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. 默认值 0.5。目标附近的距离，用于停止应用障碍物优先项，让机器人在接近障碍物时平稳地向目标姿势靠拢。 |
| inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. 默认为“”。膨胀层名称。如果为空，则使用成本计算图中的最后一个膨胀层。如果有多个膨胀层，可能需要指定要使用的膨胀层名称。 |
| trajectory_point_step | int | Default 2. Step of trajectory points to evaluate for costs since otherwise so dense represents multiple points for a single costmap cell. 默认 2. 对成本进行评估的轨迹点的步长，否则密集的轨迹点代表一个成本计算单元的多个点。 |
3.10 路径对齐 Critic
| 参数 | 类型 | 定义 |
|---|---|---|
| cost_weight | double | Default 10.0. Weight to apply to critic term. 默认为 10.0。适用于 critic 项的权重。 |
| cost_power | int | Default 1. Power order to apply to term. |
| threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path align cost stops being considered 默认值 0.5。机器人与目标之间的距离，超过此距离时将不再考虑路径调整成本 |
| offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. 默认值 20。检查候选轨迹是否已在足够远的路径上进行跟踪，以便应用对齐 critic。这样就能确保只有在实际追踪路径时才会考虑路径对齐问题，从而避免机器人在初始化过程中出现笨拙的动作，使其无法偏离路径以获得合适的航向。 |
| trajectory_point_step | int | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. 默认 4. 为减少计算时间而评估路径距离的轨迹点步长。通常在 1-10 之间比较合理。 |
| max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. 默认值 0.07 (7%)。在不考虑该 critic 之前可占用路径的最大比例，以允许障碍物和路径跟随 critic 在场景中出现动态物体时，在遵循路径意图的同时避开障碍物。 |
| use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. 默认为 false。是否在路径对齐时考虑路径的方向，这在与可行的 smac 规划器搭配时非常有用，只有在 smac 规划器要求改变方向时，才会激励机器人改变方向。如果希望机器人在控制器认为合适的地方偏离或反转方向，请保留为 “假”。如果您的计划不包含方向信息（例如 navfn），请保留为 false。 |
3.11 路径角度 Critic
| 参数 | 类型 | 定义 |
|---|---|---|
| cost_weight | double | Default 2.2. Weight to apply to critic term. 默认 2.2. 适用于 critic 的权重。 |
| cost_power | int | Default 1. Power order to apply to term. |
| threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered 默认值 0.5。机器人与目标之间的距离，超过此距离时将不再考虑路径角度成本 |
| offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. 默认值 4。任何轨迹达到最远点后的轨迹点数量，用于计算相对于该轨迹的轨迹角度。 |
| max_angle_to_furthest | double | Default 0.785398. Angular distance between robot and goal above which path angle cost starts being considered 默认值 0.785398。机器人与目标之间的角度距离，超过此距离时开始考虑路径角度成本 |
| mode | int | Default 0 (Forward Preference). Enum type for mode of operations for the path angle critic depending on path input types and behavioral desires. 0: Forward Preference, penalizes high path angles relative to the robot's orientation to incentivize turning towards the path. 1: No directional preference, penalizes high path angles relative to the robot's orientation or mirrored orientation (e.g. reverse), which ever is less, when a particular direction of travel is not preferable. 2: Consider feasible path orientation, when using a feasible path whereas the path points have orientation information (e.g. Smac Planners), consider the path's requested direction of travel to penalize path angles such that the robot will follow the path in the requested direction. 默认为 0（前向偏好）。枚举类型，用于根据路径输入类型和行为愿望对路径角度批评者的操作模式进行判定。0：前进偏好，惩罚相对于机器人方向的高路径角度，以鼓励转向路径。1: 无方向偏好，当某一行进方向不可取时，惩罚相对于机器人方向或镜像方向（如反向）的高路径角度，以较小者为准。2: 考虑可行路径方向，在使用可行路径时，如果路径点有方向信息（如 Smac Planners），则考虑路径要求的行进方向，以惩罚路径角度，从而使机器人沿着要求的方向行进。 |
3.12 路径跟踪 Critic
| 参数 | 类型 | 定义 |
|---|---|---|
| cost_weight | double | Default 5.0. Weight to apply to critic term. 默认值 5.0。适用于 critic 的权重。 |
| cost_power | int | Default 1. Power order to apply to term. |
| offset_from_furthest | int | Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. 默认值 6。任何轨迹达到最远点后的轨迹点数量，用于驱动相对于该轨迹点的轨迹跟踪。 |
| threshold_to_consider | float | Default 1.4. Distance between robot and goal above which path follow cost stops being considered 默认值 1.4. 机器人与目标之间的距离，超过此距离时将不再考虑路径跟随成本 |
3.13 倾向前进（Prefer Forward）Critic
| 参数 | 类型 | 定义 |
|---|---|---|
| cost_weight | double | Default 5.0. Weight to apply to critic term. |
| cost_power | int | Default 1. Power order to apply to term. |
| threshold_to_consider | double | Default 0.5. Distance between robot and goal above which prefer forward cost stops being considered 默认值 0.5。机器人与目标之间的距离，超过此距离时将不再考虑前进成本 |
3.14 旋转 Critic
| 参数 | 类型 | 定义 |
|---|---|---|
| cost_weight | double | Default 10.0. Weight to apply to critic term. |
| cost_power | int | Default 1. Power order to apply to term. |
3.15 速度死区 Critic
| 参数 | 类型 | 定义 |
|---|---|---|
| cost_weight | double | Default 35.0. Weight to apply to critic term. |
| cost_power | int | Default 1. Power order to apply to term. |
| deadband_velocities | double[] | Default [0.0, 0.0, 0.0]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action. 默认值 [0.0,0.0,0.0]。死区速度数组 [vx、vz、wz]。如果数组为零，则表示 Critic不会采取任何行动。 |
四、XML 配置示例
controller_server:
ros__parameters:
controller_frequency: 30.0
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05
batch_size: 2000
vx_std: 0.2
vy_std: 0.2
wz_std: 0.4
vx_max: 0.5
vx_min: -0.35
vy_max: 0.5
wz_max: 1.9
iteration_count: 1
prune_distance: 1.7
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
visualize: false
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.2
critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
ConstraintCritic:
enabled: true
cost_power: 1
cost_weight: 4.0
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 1.4
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
threshold_to_consider: 0.5
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 0.5
# Option to replace Cost and use Obstacles instead
# ObstaclesCritic:
# enabled: true
# cost_power: 1
# repulsion_weight: 1.5
# critical_weight: 20.0
# consider_footprint: false
# collision_cost: 10000.0
# collision_margin_distance: 0.1
# near_goal_distance: 0.5
CostCritic:
enabled: true
cost_power: 1
cost_weight: 3.81
critical_cost: 300.0
consider_footprint: true
collision_cost: 1000000.0
near_goal_distance: 1.0
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
threshold_to_consider: 0.5
offset_from_furthest: 20
use_path_orientations: false
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
offset_from_furthest: 5
threshold_to_consider: 1.4
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
offset_from_furthest: 4
threshold_to_consider: 0.5
max_angle_to_furthest: 1.0
forward_preference: true
# VelocityDeadbandCritic:
# enabled: true
# cost_power: 1
# cost_weight: 35.0
# deadband_velocities: [0.05, 0.05, 0.05]
# TwirlingCritic:
# enabled: true
# twirling_cost_power: 1
# twirling_cost_weight: 10.0五、话题
| 话题 | 类型 | 描述 |
|---|---|---|
trajectories | visualization_msgs/MarkerArray | Randomly generated trajectories, including resulting control sequence 随机生成的轨迹，包括由此产生的控制序列 |
transformed_global_plan | nav_msgs/Path | Part of global plan considered by local planner 局部规划器考虑的全局规划器的一部分 |
六、用户须知
6.1 一般设置
model_dt 参数一般应设置为控制频率的持续时间。因此，如果您的控制频率为 20 赫兹，则参数应为 0.05。不过，您也可以将其设置得更低，但不能过大。
使用可视化功能对轨迹进行可视化会占用计算资源，以倒退出轨迹进行可视化，因此会减慢计算时间。不建议在部署使用时将该参数设置为 true，但在调整系统时，该参数是一个有用的调试工具，但应谨慎使用。在 30 赫兹的频率下可视化 2000 个批次（56 个点）是一个很大的数字。
最常见的参数是速度曲线（vx_max、vx_min、wz_max 和 vy_max，如果是整体动力学参数）和运动模型（motion_model）。明智的做法是根据最大速度和预测范围来考虑路径规划的剪枝距离。唯一需要根据具体设置进行调整的深层参数是障碍物批判的斥力权重（repulsion_weight），因为该参数的调整与膨胀层的半径成正比。半径越大，斥力权重就越小，因为会形成惩罚（例如膨胀半径 - 距离障碍物的最小距离）。如果惩罚过高，机器人在从非成本空间进入成本空间时就会明显减速，或者在狭窄的通道中抖动。值得注意的是，如果 consider_footprint = true，障碍物批判器可以使用完整的轮廓信息，但计算成本会增加，这一点可能无需修改。
如果不需要路径跟随行为（例如，只想跟随目标姿势，让模型预测元素决定实现目标的最佳方式），可以轻松移除 PathAlign、PathFollow 和 PathAngle 批判器。
默认情况下，控制器经过调整并具备 PathAlign/Obstacle（路径对齐/障碍物）判据中规定的功能，在没有障碍物阻挡时通常会紧跟路径，但在受阻时能够偏离路径。详情请参见 PathAlignCritic::score()，但当本地路径受阻时，该功能会被禁用，因此在这种状态下，障碍物评判器会接管该功能。
如果你想进一步减慢接近目标的速度，可以考虑提高阈值_to_consider参数，让路径跟踪批判者更快地转向目标接近批判者，然后根据你感兴趣的配置文件调整这些批判者。
6.2 预测范围、成本计算图大小和偏移量
由于这是一个预测规划器，因此最大速度、预测时间和成本图大小之间存在一定的关系，用户在根据自己的应用进行调整时应牢记。如果将控制器服务器成本图的大小设置为 3.0 米，这意味着机器人位于中心位置时，机器人两侧的信息量为 1.5 米。如果最大速度（vx_max）下的预测范围（time_steps * model_dt）大于这个值，那么机器人的最大速度和行为就会受到成本地图限制的人为限制。例如，如果您以 0.5 米/秒的最大速度预测前进 3 秒（60 步 @ 0.05 秒/步），那么所需的最小成本映射半径为 1.5 米，即总宽度为 3 米。
这同样适用于路径跟随和对齐偏移。在同一示例中，如果我们可以考虑的最远点已经位于成本映射的边缘，那么更远的偏移就会因为无法使用而被阈值化。因此，在选择这些参数时，重要的是要确保理论偏移量可以存在于所选的成本图设置中，并具有所需的最大预测范围和速度。将 “路径跟随 ”和 “目标批判 ”中的考虑阈值设置为与预测视距相同，可以确保它们之间的交接干净利落，否则 “路径跟随 ”在到达作为其标记的最终目标姿态后，会试图稍微减速。
路径跟随 "批判者在滚动代价图的可用路径上所驱动的速度不能大于该速度的可投影距离。路径对齐批判器的偏移量（offset_from_furthest）表示轨迹跟踪时经过的路径点数量。如果偏移量设置得过低（例如 5），当机器人试图开始路径跟踪时，就会触发偏移量，导致机器人在执行任务时出现一些次优行为和局部最小值。如果相对于路径分辨率和代价图大小设置得过高（例如 50），那么批评者可能永远不会触发，或者只有在全速运行时才会触发。在此保持平衡是明智之举。将该值选择为投影的最大速度距离的 30% 左右是比较好的（例如，如果规划器每 2.5 厘米产生一个点，那么 1.5 米的本地成本图半径上就可以容纳 60 个点。如果最大速度为 0.5 米/秒，预测时间为 3 秒，那么 20 个点代表在预测范围内投射到路径上的最大速度的 33%）。如果有疑问，prediction_horizon_s * max_speed / path_resolution / 3.0 是一个很好的基准。
6.3 障碍物、膨胀层和路径跟踪
成本图配置与障碍物批判器配置之间也存在一定的关系。如果障碍物批判器与成本图参数（膨胀半径、规模）之间的关系不协调，就会导致机器人大幅摇摆，因为它会试图以稍低的成本来换取生硬的运动，从而选择成本更低的有限轨迹。默认行为是针对小型 AMR（如海龟机器人或类似机器人）调整的，因此如果使用较大的机器人，可能需要相应减少斥力权重。在自由空间中，它还可能会做出一些笨拙的动作，试图在一个小的 0 成本口袋中尽量延长时间，而不是采用更自然的运动方式，即移动到一些低成本区域。最后，如果为鼓励机器人沿路径移动而设定的增益比路径跟随得分高得不成比例，那么当机器人从自由的 0 成本空间开始时，一般可能根本不会进入成本空间。这是因为停留在自由空间的批评成本要比进入哪怕是低成本空间来换取任务的进展更具吸引力。
因此，在选择障碍物批判器的权重时，应注意结合成本地图的膨胀半径和比例，这样机器人就不会出现此类问题。我（Steve，你们的友好邻居导航员）的调整方法是，首先结合膨胀层参数创建适当的障碍物批判器行为。值得注意的是，“障碍物批判者 ”会将成本转换为与障碍物的距离，因此成本在膨胀中的分布性质并不重要。不过，膨胀半径和比例将确定成本分布末端的成本，即自由空间与半径内最低成本值的距离。因此，在超过该临界值时，应考虑对质量行为进行测试。
当您增加或减少障碍物的权重时，您可能会注意到上述行为（例如，无法克服从自由空间到非自由空间的临界值）。为了克服这些问题，可以增加 “跟随路径 ”批判成本，以增强轨迹规划器继续向目标移动的愿望。但要确保不要过高，保持平衡。理想的结果是大致在空间中心平稳移动，而不与障碍物发生明显的近距离交互。它不应该完全沿着一条路径前进，输出速度也不应该参差不齐地摇摆。
调整好避障行为并匹配适当的路径跟随惩罚后，再调整路径对齐批判器，使其与路径对齐。如果您设计的是精确的路径对齐行为，则可以跳过障碍物批判器这一步，因为高度调整系统以遵循路径会降低其偏离以避开障碍物的能力（尽管它会减速和停止）。将 “障碍物批判器 ”的批判权重调高可以避免近距离碰撞，但斥力权重对你来说基本没有必要。对于希望获得更多动态行为的人来说，慢慢降低障碍物批判器的权重，为路径对齐批判器提供更多的工作空间，可能会有所裨益。如果你的路径是用成本感知规划器（如 Nav2 提供的所有规划器）生成的，并提供了足够远离障碍物的路径，那么略微降低障碍物批判器和路径对齐批判器的权重会对你很有帮助。路径对齐批判器的权重不要过高，这将允许机器人偏离路径，以绕过场景中的动态障碍物或之前在路径规划时未考虑到的其他障碍物。对于您的应用而言，最佳行为是主观的，但事实证明，MPPI 可以成为精确的路径跟踪器和/或非常流畅地避开动态障碍物，以及介于两者之间的任何地方。所提供的默认设置基本符合平衡初始权衡的要求。
