MPPI算法

MPPI算法

概述

这是一种预测控制器（局部轨迹规划器），它实现了模型预测路径积分（MPPI）算法，以跟踪具有自适应防撞功能的路径。它包含基于插件的critic函数来影响算法的行为。它由阿列克谢·布迪亚科夫创作，史蒂夫·马肯斯基为Nav2改编和开发。

该插件实现了nav2_core:：Controller接口，使其能够在导航堆栈中用作控制器服务器的动作服务器（Controller_server）中的本地轨迹规划器。

该控制器在英特尔处理器（第四代i5）上以50赫兹以上的频率运行。有关其他参数说明，请参阅其配置指南页面。

它目前适用于差分、全向和阿克曼机器人。

描述

MPPI算法是一种MPC变体，它使用迭代方法为机器人找到控制速度。使用前一个时间步长的最佳控制解和机器人的当前状态，应用一组来自高斯分布的随机采样扰动。这些带噪声的控制被正向模拟，以在机器人的运动模型内生成一组轨迹。

接下来，使用一组基于插件的critic函数对这些轨迹进行评分，以找到批次中的最佳轨迹。输出分数用于通过软最大函数设置最佳控制。

然后，此过程重复多次，并返回收敛的解决方案。该解决方案随后被用作下一时间步骤的初始控制的基础。

特性

预测MPC轨迹规划器

利用基于插件的批评者，用户可以轻松地更换、调整或替换这些批评者

使用矢量化和张量运算高度优化的CPU性能

支持多种常见的运动模型，包括Ackermann、Differential Drive和Omni-directional

包括在升级到恢复行为之前处理软故障的回退机制

使用Doxygen实现高质量代码、高单元测试覆盖率、文档和参数指南

易于扩展以支持MPPI的现代研究变体

为实现开箱即用的良好性能而进行了预调

参数说明

Controller

Parameter	Type	Definition
motion_model（运动模型）	string	Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann].
critics（评论者）	string	Default: None. Critics (plugins) names
iteration_count（迭代计数）	int	默认值1。MPPI算法中的迭代次数。建议保留为1，更喜欢多批次。
batch_size（批次大小）	int	默认值为1000。随机采样的候选轨迹计数
time_steps（时间步长）	int	默认值56。每个采样轨迹中的时间步数（点）
model_dt（时间间隔）	double	Default: 0.05. 两个采样点之间的时间间隔
vx_std（标准误差）	double	Default 0.2. Sampling standard deviation for VX
vy_std	double	Default 0.2. Sampling standard deviation for VY
wz_std	double	Default 0.4. Sampling standard deviation for Wz
vx_max	double	Default 0.5. Max VX (m/s)
vy_max	double	Default 0.5. Max VY in either direction, if holonomic. (m/s)
vx_min	double	Default -0.35. Min VX (m/s)
wz_max	double	Default 1.9. Max WZ (rad/s)
temperature	double	默认值：0.3。轨迹按成本的选择性（该值越接近0，我们就越“越多”考虑成本较低的控制），0平均使用控制具有最佳成本，巨大的价值将导致只取所有轨迹的平均值而不考虑成本
gamma	double	默认值：0.015。平滑度（高）和低能量（低）之间的权衡。这是一个复杂的参数，可能不需要从默认值0.1更改，默认值0.1适用于各种情况。有关详细信息，请参阅“信息论模型预测控制：自动驾驶的理论和应用”中的3D-2部分。
visualize	bool	默认值：false。发布轨迹的可视化，这可能会显著降低控制器的速度。仅用于调试。
retry_attempt_limit（ 重试尝试限制 ）	int	默认值1。在报告故障之前，为软重置寻找故障可行轨迹的尝试次数。
regenerate_noises（再生噪音）	bool	默认值为false。是在每次迭代中重新生成噪声，还是使用初始化和重置时计算的单个噪声分布。实际上，这被发现工作良好，因为轨迹是从正态分布随机采样的，并且减少了运行时由于线程唤醒而导致的计算抖动，以重新采样正态分布。

Trajectory Visualizer（可视化的参数）

Parameter	Type	Definition
trajectory_step	int	Default: 5. The step between trajectories to visualize to downsample candidate trajectory pool.
time_step	int	Default: 3. The step between points on trajectories to visualize to downsample trajectory density.

Path Handler（路径处理）

Parameter	Type	Definition
max_robot_pose_search_dist（最大机器人姿势搜索距离）	double	默认值：成本图的一半大小。在路径循环的情况下，机器人姿态前搜索最近路径点的最大积分距离。
prune_distance（修剪距离）	double	默认值：1.5。要修剪路径的机器人路径上最近点之前的距离。
transform_tolerance（tf2转换的误差容忍时间）	double	Default: 0.1. Time tolerance for data transformations with TF.
enforce_path_inversion（强制路径版本）	double	默认值：False。如果为true，可以让我们设计一个特定点，在这个点附近机器人会强制转向一个角度
inversion_xy_tolerance（反向xy公差）	double	默认值：0.2。到路径反转点的笛卡尔接近度（m）被认为是“实现”的，以便在路径反转点后通过路径的其余部分。 （没看懂）
inversion_yaw_tolerance（反向偏航公差）	double	默认值：0.4。与路径反转点的角度接近度（弧度）被视为“实现”，以便在路径反转点后通过路径的其余部分。0.4弧度=23度。

Ackermann Motion Model（阿克曼模型）

Parameter	Type	Definition
min_turning_r	double	minimum turning radius for ackermann motion model

Constraint Critic（约束评论家）

Parameter	Type	Definition
cost_weight	double	Default 4.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.

Goal Angle Critic（目标角度评论家）

Parameter	Type	Definition
cost_weight	double	Default 3.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. 机器人和目标之间的最小距离，超过该角度的目标成本被考虑在内。

Goal Critic

Parameter	Type	Definition
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 1.4. 机器人和目标之间的距离，超过该距离开始考虑目标成本

Obstacles Critic（障碍物评论家）

Parameter	Type	Definition
consider_footprint	bool	默认值：False。是使用点成本（如果机器人是圆形或低计算能力）还是计算SE2占地面积成本。
critical_weight	double	默认值20.0。对于比“collision_margin_distance”更接近的近距离碰撞，要应用于critic的权重，以防止近距离碰撞仅作为一种虚拟膨胀足迹的方法。这通常不应用于影响避开临界碰撞的障碍物。
repulsion_weight（排斥权重）	double	默认值1.5。适用于评论家的权重，因为他们通常更喜欢低成本空间的路线。这与临界项分离，以允许在动态场景中通过路径对齐对障碍物行为进行微调，而不会影响可能直接导致近距离碰撞的动作。这是在距离障碍物的“充气半径”范围内应用的。
cost_power	int	Default 1. Power order to apply to term.
collision_cost（碰撞成本）	double	Default 10000.0. 应用于轨迹中的真实碰撞的成本。
collision_margin_distance（碰撞边缘位置）	double	Default 0.10. 从碰撞到应用严厉惩罚的边际距离，类似于足迹膨胀。在0.05-0.2之间是合理的。
near_goal_distance	double	Default 0.5. 目标附近的距离停止应用优先障碍项，使机器人在接近障碍物时平滑地收敛到目标姿势。

*SE2：

SE(2)是一个在机器人运动学和控制领域经常使用的术语，表示二维特殊欧几里德群（Special Euclidean group in 2D）。这个概念涉及到描述平面上物体的位姿（平移和旋转）的数学模型。

具体来说，SE(2)将平面上的刚体运动建模为平移和旋转的组合。在二维空间中，SE(2)由三个自由度组成：x 和 y 分别表示平移方向，θ 表示绕 z 轴的旋转角度。因此，SE(2)可以用一个3x3的矩阵来表示，该矩阵包括平移和旋转的信息。

对于机器人路径规划和运动控制，SE(2)提供了一种方便的方式来描述机器人在平面上的位置和朝向。它也被广泛应用于SLAM（Simultaneous Localization and Mapping，同时定位与地图构建）算法以及轨迹规划等领域。

简而言之，SE(2)代表了平面上刚体运动的数学模型，用于描述机器人或物体在平面上的位姿变换。

Path Align Critic

Parameter	Type	Definition
cost_weight	double	Default 10.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. 机器人和目标之间的距离，超过该距离，路径角度成本将停止考虑（超多了就不考路径角度成本而先注重到达）
offset_from_furthest（最远偏移）	double	默认值20。检查候选轨迹在跟踪路径的过程中是否足够远，以应用对齐评论家。这确保了只有在实际跟踪路径时才考虑路径对齐，防止了尴尬的初始化运动，防止了机器人离开路径以实现适当的航向。
trajectory_point_step	double	默认值4。轨迹点的步骤，以评估路径距离，从而减少计算时间。1-10之间通常是合理的。
max_path_occupancy_ratio（占用率最大值）	double	默认0.07（7%）。评论家将避免超过该阈值的路径，以避免碰撞
use_path_orientations（是否使路径定向）	bool	默认值为false。是否在路径对齐中考虑路径的方向，当与可行的smac规划者配对时，这可能很有用，只有在smac规划者提出要求时，才能激励方向改变。如果你想让机器人偏离并反转控制器认为合适的方向，请保持为false。如果您的计划不包含方向信息（例如navfn），则保留为false。

Note: There is a “Legacy” version of this critic also available with the same parameters of an old formulation pre-October 2023.

Path Angle Critic

Parameter	Type	Definition
cost_weight	double	Default 2.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 path angle cost stops being considered
offset_from_furthest	int	Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to.
max_angle_to_furthest	double	Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered
mode	int	默认值0（正向首选项）。路径角度评论家的操作模式的枚举类型取决于路径输入类型和行为期望。0：正向偏好，惩罚相对于机器人方向的高路径角度，以激励转向路径。1：没有方向偏好，当特定的行进方向不优选时，会惩罚相对于机器人的方向或镜像方向（例如反向）的高路径角，后者会更小。2：考虑可行路径方向，当使用可行路径时，而路径点有方向信息（例如Smac Planners），考虑路径请求的行进方向，以惩罚路径角度，使机器人沿着请求的方向跟随路径。

Path Follow Critic

Parameter	Type	Definition
cost_weight	double	Default 5.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.
offset_from_furthest	int	Default 6. 任何轨迹中距离驱动路径跟踪最远的路径点的数量。 （值越大，生成的路径点越密集）
threshold_to_consider	float	Default 1.4. Distance between robot and goal above which path follow cost stops being considered

Prefer Forward Critic

Parameter	Type	Definition
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

Twirling Critic

Parameter	Type	Definition
cost_weight	double	Default 10.0. Weight to apply to critic term.
cost_power	int	Default 1. Power order to apply to term.