模型预测路径积分控制器
MPPI控制器实现了一个`模型预测路径积分控制器 <https://ieeexplore.ieee.org/document/7487277>`_。新的Nav2 MPPI控制器是一个预测控制器，是TEB和纯路径跟踪MPC控制器的继任者。它使用基于采样的方法来选择最优轨迹，在连续迭代之间进行优化。它包含了基于插件的目标函数，用于自定义和扩展各种行为和行为属性。
目前适用于差分、全向和Ackermann机器人。该控制器在一台性能适中的Intel处理器（第四代i5）上以50+赫兹运行。
MPPI算法是一种MPC变体，它使用迭代方法为机器人找到控制速度。利用上一个时间步的最佳控制解和机器人的当前状态，从高斯分布中随机采样一组扰动控制。这些带噪声的控制被正向模拟，生成机器人运动模型内的一组轨迹。然后，使用一组基于插件的评价函数对这些轨迹进行评分，以找到批次中的最佳轨迹。输出的评分用于通过软最大函数设置最佳控制。然后，重复此过程多次，直到获得收敛解。将此解作为下一个时间步的初始控制的基础。
这项工作的一个强大结果是能够利用不需要是凸的、也不需要是可微分的目标函数，为设计者在行为上提供更大的灵活性。
请参阅软件包的“README”以获取更完整的信息。
MPPI 参数
- 运动模型
类型
默认
string
"DiffDrive"
- 描述
用于轨迹规划的期望运动模型。选项包括``DiffDrive``（差分驱动）、``Omni``（全向）或``Ackermann``（阿克曼）。差分驱动机器人可以使用前进/后退和角速度；全向机器人添加了横向运动；而阿克曼则添加了最小曲率约束。
- 批评家
类型
默认
字符串向量
N/A
- 描述
要使用的批评家插件函数向量，无需
mppi::critic::命名空间，在加载时会自动添加。
- iteration_count
类型
默认
int
1
- 描述
MPPI 算法中的迭代次数。建议保持为 1，并选择更大的批次大小。
- batch_size
类型
默认
int
1000
- 描述
在给定迭代中，从当前最优控制序列中随机抽样的候选轨迹数量。以每秒50次的频率进行1000次或以每秒30次的频率进行2000次似乎能产生良好的结果。
- 时间步数
类型
默认
int
56
- 描述
候选轨迹中的时间步数（点数）
- model_dt
类型
默认
双精度
0.05
- 描述
每个时间步长``dt``的长度，以秒为单位。``time_steps * model_dt``是预测的时间范围。
- vx_std
类型
默认
双精度
0.2
- 描述
Vx的采样标准差
- vy_std
类型
默认
双精度
0.2
- 描述
Vy的采样标准差
- wz_std
类型
默认
双精度
0.2
- 描述
Wz（角速度）的采样标准差
- vx_max
类型
默认
双精度
0.5
- 描述
目标最大前向速度（米/秒）。
- vy_max
类型
默认
双精度
0.5
- 描述
如果使用``Omni``运动模型，则为目标最大横向速度（米/秒）。
- vx_min
类型
默认
双精度
-0.35
- 描述
最大反向速度（米/秒）。
- wz_max
类型
默认
双精度
1.9
- 描述
最大旋转速度（弧度/秒）
- temperature
类型
默认
双精度
0.3
- 描述
通过其成本选择轨迹的选择性（该值越接近0，我们越考虑成本较低的控制），0表示使用具有最佳成本的控制，较大的值将导致只考虑所有轨迹的平均成本。
- gamma
类型
默认
双精度
0.015
- 描述
光滑性（高）和能量消耗（低）之间的权衡。这是一个复杂的参数，通常不需要从默认值进行更改。详细信息请参见《信息论模型预测控制：理论与自动驾驶应用》第3D-2节。
- visualize
类型
默认
布尔值
false
- 描述
是否发布用于可视化的调试轨迹。这可能会大大减慢控制器的速度（例如，每30hz有1000个大小为56的批次数据）。
- retry_attempt_limit
类型
默认
int
1
- 描述
在软重置失败后，尝试寻找可行轨迹的次数，然后报告总体失败。
- reset_period
类型
默认
双精度
1.0
- 描述
重置优化器所需的不活动时间（仅在Humble中由于后向兼容ABI策略）。
轨迹可视化
- 轨迹步长
类型
默认
int
5
- 描述
要可视化的轨迹之间的步长，以对候选轨迹池进行下采样。
- 时间步长
类型
默认
int
3
- 描述
在轨迹上的点之间用于可视化的步长，以降低轨迹密度。
路径处理器
- transform_tolerance
类型
默认
双精度
0.1
- 描述
使用TF进行数据转换的时间容差（秒）。
- prune_distance
类型
默认
双精度
1.5
- 描述
从路径上最近点到机器人之前的距离，用于修剪路径（米）。
- max_robot_pose_search_dist
类型
默认
双精度
代价地图尺寸的一半
- 描述
机器人姿势前方的最大积分距离，在路径循环的情况下，用于搜索最近的路径点。
阿克曼运动模型
- 最小转向半径
类型
默认
双精度
0.2
- 描述
车辆平台可能的最小转弯半径（米）。
约束评估器
- cost_weight
类型
默认
双精度
4.0
- 描述
应用于评估器项的权重
- cost_power
类型
默认
int
1
- 描述
应用于项的幂次
目标角度评价器
- cost_weight
类型
默认
双精度
3.0
- 描述
应用于评估器项的权重
- cost_power
类型
默认
int
1
- 描述
应用于项的幂次
- 考虑的阈值
类型
默认
双精度
0.4
- 描述
机器人与目标之间的最小距离（米），超过该距离将考虑角度目标成本。
目标评估器
- cost_weight
类型
默认
双精度
5.0
- 描述
应用于评估器项的权重
- cost_power
类型
默认
int
1
- 描述
应用于项的幂次
- 考虑的阈值
类型
默认
双精度
1.0
- 描述
机器人与目标之间的最小距离（米），超过该距离将考虑目标距离成本。
障碍物评估器
- 关键权重
类型
默认
双精度
20.0
- 描述
在距离小于``collision_margin_distance``的近距离碰撞中，为了仅仅作为虚拟扩大足迹的方法防止近距离碰撞，应用于评论家的权重。这不应该被用于一般性地影响避障行为，使其远离关键性碰撞。
- 排斥权重
类型
默认
双精度
1.5
- 描述
在一般情况下更喜欢低成本空间中的路径的评论家的权重。这与关键术语分开，以便在动态场景中通过路径对齐对障碍物行为进行微调，而不影响可能直接导致近距离碰撞的动作。这在与障碍物的``inflation_radius``距离内应用。
- cost_power
类型
默认
int
1
- 描述
应用于项的幂次
- 考虑足迹
类型
默认
布尔值
false
- 描述
是否使用点成本（如果机器人是圆形的或计算能力较低）或计算SE2足迹成本。
- 碰撞成本
类型
默认
双精度
10000.0
- 描述
在轨迹中应用于真实碰撞的成本。
- collision_margin_distance
类型
默认
双精度
0.10
- 描述
从碰撞到施加严厉惩罚的边缘距离（米），类似于足迹扩大。0.05-0.2之间是合理的范围。请注意，它将极大地影响控制器不进入比此更狭窄的空间，因此确保该参数设置为比机器人预计需要穿越的最窄空间更低的值。
- near_goal_distance
类型
默认
双精度
0.50
- 描述
距离目标附近的距离（米），用于停止应用偏好障碍项，以便机器人能够在靠近障碍物的情况下平稳地收敛到目标姿态。
- cost_scaling_factor（成本缩放因子）
类型
默认
双精度
10.0
- 描述
膨胀半径的指数衰减因子。这应该与膨胀层的设置相同（仅适用于Humble）。
- inflation_radius（膨胀半径）
类型
默认
双精度
0.55
- 描述
在致命障碍物周围膨胀代价地图的半径。这应该与膨胀层的半径相同（仅适用于Humble版本）
路径对齐评论家
- cost_weight
类型
默认
双精度
10.0
- 描述
应用于评估器项的权重
- cost_power
类型
默认
int
1
- 描述
应用于项的幂次
- 考虑的阈值
类型
默认
双精度
0.4
- 描述
机器人与目标之间的距离（米），**停止**考虑路径对齐并允许目标评论器接管。
- offset_from_furthest（离最远处的偏移量）
类型
默认
int
20
- 描述
检查候选轨迹是否在其路径上足够远，以应用对齐评论器。这样可以确保只有在实际跟踪路径时才考虑路径对齐，防止尴尬的初始化动作阻止机器人离开路径以实现适当的航向。
- max_path_occupancy_ratio（最大路径占用比例）
类型
默认
双精度
0.07
- 描述
在存在场景中的动态物体的情况下，避障和路径跟随评估者避开障碍物并按照路径意图进行跟随之前，路径中可以被占用的最大比例。介于0-1之间，表示0-100%。
路径角度评论器
- cost_weight
类型
默认
双精度
2.0
- 描述
应用于评估器项的权重
- cost_power
类型
默认
int
1
- 描述
应用于项的幂次
- 考虑的阈值
类型
默认
双精度
0.4
- 描述
机器人与目标之间的距离（以米为单位），用于**停止**考虑路径角度并允许目标评估者接管。
- offset_from_furthest（离最远处的偏移量）
类型
默认
int
20
- 描述
计算相对于路径角度时，任何轨迹实现的最远点之后的路径点数。
- max_angle_to_furthest（与最远处的最大角度）
类型
默认
双精度
1.2
- 描述
机器人与目标之间的角度距离（以弧度为单位），超过该距离开始考虑路径角度成本。
路径跟随评论家
- cost_weight
类型
默认
双精度
5.0
- 描述
应用于评估器项的权重
- cost_power
类型
默认
int
1
- 描述
应用于项的幂次
- 考虑的阈值
类型
默认
双精度
0.4
- 描述
机器人与目标之间的距离（以米为单位），用于**停止**考虑路径跟随并允许目标评估者接管。
- offset_from_furthest（离最远处的偏移量）
类型
默认
int
6
- 描述
在任何轨迹实现驱动路径跟踪之后，最远点后的路径点数。
优先正向评论家
- cost_weight
类型
默认
双精度
5.0
- 描述
应用于评估器项的权重
- cost_power
类型
默认
int
1
- 描述
应用于项的幂次
- 考虑的阈值
类型
默认
双精度
0.4
- 描述
机器人与目标之间的距离（米），考虑前进优先并允许目标评论家接管的**停止**。
旋转评论家
- cost_weight
类型
默认
双精度
10.0
- 描述
应用于评估器项的权重
- cost_power
类型
默认
int
1
- 描述
应用于项的幂次
示例
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
reset_period: 1.0 # (only in Humble)
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstrains:
min_turning_r: 0.2
critics: ["ConstraintCritic", "ObstaclesCritic", "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.0
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
threshold_to_consider: 0.4
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 0.4
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
inflation_radius: 0.55 # (only in Humble)
cost_scaling_factor: 10.0 # (only in Humble)
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 3
threshold_to_consider: 0.40
offset_from_furthest: 20
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
offset_from_furthest: 5
threshold_to_consider: 0.6
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
offset_from_furthest: 4
threshold_to_consider: 0.40
max_angle_to_furthest: 1.0
# TwirlingCritic:
# enabled: true
# twirling_cost_power: 1
# twirling_cost_weight: 10.0
用户注意事项
智慧的一般性言论
model_dt``参数通常应设置为控制频率的持续时间。因此，如果控制频率为20hz，则应设置为``0.05。但是，您也可以将其设置得更低**但不能更高**。
使用``visualize``可视化轨迹会使用计算资源来计算可视化所需的轨迹，因此会降低计算时间。在部署使用时，不建议将此参数设置为``true``，但在调整系统时，它是一个有用的调试工具，但请谨慎使用。以30 Hz的频率可视化2000批次、每批56个点，是非常大的负担。
您可能希望首先更改的最常见参数是速度配置文件（vx_max、vx_min、wz_max``以及如果是全向的，则为``vy_max）以及与您的车辆对应的``motion_model``。明智的做法是将路径规划的``prune_distance``与您的最大速度和预测视野成比例地考虑。根据您的特定设置，可能需要调整的更深层次的参数仅限于障碍物评论家的``repulsion_weight``，因为其调整与膨胀层的半径成比例。较大的半径应对由于惩罚形成（例如``inflation_radius - min_dist_to_obstacle``）而导致减小的``repulsion_weight``。如果此惩罚过高，机器人在从非成本空间进入成本空间或在狭窄的走廊中抖动时会显著减速。值得注意的是，但可能不需要更改的是，如果``consider_footprint = true``，障碍物评论家可以使用完整的足迹信息，但会增加计算成本。
除此之外，这些参数已经由友好的导航器进行了精细调整，以便为您提供一个不错的起点，希望您只需要轻微地重新调整以满足特定的需求（如果需要的话）。调整代价图参数或最大速度是需要特别关注的操作，如下所述：
预测视野、成本地图尺寸和偏移量
由于这是一个预测性规划器，用户在调整应用程序时应该注意最大速度、预测时间和代价图大小之间的某种关系。如果控制器服务器代价图的大小设置为3.0米，这意味着在机器人中心，机器人两侧各有1.5米的信息。当您的预测视野（time_steps * model_dt）在最大速度（vx_max）下大于这个值时，机器人在最大速度和行为上将受到代价图限制的人为限制。例如，如果您以0.5米/秒的最大速度向前预测3秒（每步0.05秒，60步），则**最小**所需的代价图半径为1.5米，或者总宽度为3米。
相同的原理适用于最远点的路径跟踪和对齐偏移量。在同样的示例中，如果我们可以考虑的最远点已经在成本地图的边缘，则进一步的偏移将被阈值化，因为它们是不可用的。因此，在选择这些参数时，确保理论偏移可以存在于选择的具有所需最大预测视野和速度的成本地图设置中非常重要。
路径跟随评价器无法使速度超过可用路径上可投影距离的最大值。路径对齐评价器``offset_from_furthest``表示在跟踪路径时，轨迹通过的路径点数量。如果将其设置得非常低（例如5），它可能会在机器人仅仅试图开始路径跟踪时触发，从而导致一些次优的行为和局部极小值。如果相对于路径分辨率和代价图大小将其设置得非常高（例如50），则评价器可能永远不会触发，或者只会在全速运行时触发。在这里要注意取得平衡。选择这个值时，应该是最大速度距离预测距离的约30%（例如，如果规划器每2.5厘米产生一个点，在1.5米的本地代价图半径上可以容纳60个点。如果最大速度为0.5米/秒，预测时间为3秒，则20个点代表了沿路径投影的最大
障碍物、膨胀层和路径跟踪
还存在着成本地图配置和障碍物批评者配置之间的关系。如果障碍物批评者没有与成本地图参数（膨胀半径、比例）进行良好的调整，当机器人试图采取略微更低成本的轨迹以换取颠簸的运动时，它会导致机器人明显摇晃。在自由空间中，它也可能执行尴尬的机动，以尝试在一个无成本的小区域中最大化时间，而不是采取更自然的运动，这涉及移动到某个低成本区域。最后，如果增益设置远高于路径跟随评分以鼓励机器人沿着路径移动，当在一个自由的0成本空间中开始时，它可能完全拒绝进入有成本的空间。这是因为在自由空间停留的批评成本比进入即使是轻度成本的空间来推进任务更具吸引力。
因此，在选择障碍物评估函数的权重时，应注意与代价图膨胀半径和比例配合使用，以避免机器人出现这样的问题。我（史蒂夫，你友好的附近导航员）调整的方法是首先创建与膨胀层参数相配合的合适的障碍物评估函数行为。值得注意的是，障碍物评估函数将代价转化为与障碍物的距离，因此膨胀中代价的分布性质并不是非常重要。然而，膨胀半径和比例将定义分布的末端代价，即自由空间与半径内最低代价值相交的位置。因此，在超过该阈值时测试质量行为应该被考虑。
当你增加或减小对障碍物的权重时，你可能会注意到上述行为（例如无法克服从无成本到非无成本的阈值）。为了克服这些问题，增加FollowPath批评者的成本以增加轨迹规划器继续朝着目标移动的欲望。但要确保不过度调整，保持平衡。理想的结果是平滑的运动，大致位于空间的中心，与障碍物没有显著的近距离交互。它不应完全按照路径移动，输出速度也不应摇摆不定。
一旦你调整好了障碍物避免行为，并与合适的路径跟踪惩罚相匹配，就可以调整路径对齐评估函数以与路径对齐。如果设计了精确路径对齐行为，就可以跳过障碍物评估函数的步骤，因为高度调整系统以遵循路径将使其减少避开障碍物的能力（虽然速度会减慢并停止）。将障碍物评估函数的权重调整为较高将能够避免近碰撞，但斥力权重对你来说很大程度上是不必要的。对于希望获得更多动态行为的其他人来说，逐渐降低障碍物评估函数的权重会有利于给路径对齐评估函数提供更多操作空间。如果你的路径是使用具有代价感知的规划器（如Nav2提供的所有规划器）生成的，并且为您的满意度提供了远离障碍物的路径，那么稍微降低障碍物评估函数并使用路径对齐评估函数会有很好的效果。不过，过分加重路径对齐评估函数会使机器人能够偏离路径以绕过场景中的动态障碍物或先前在路径规划过程中未考虑的其他障碍物。对于你的应用来说，最佳行为是主观的，但已经证明MPPI可以是一个精确的路径跟踪器和/或能够非常流畅地避开动态障碍