MPPI Parameters Tuning in NAV2

1. Introduction

In the world of autonomous robotics, achieving optimal navigation is crucial. NAV2 is a powerful framework designed to handle navigation tasks for robots. Within this framework, the Model Predictive Path Integral (MPPI) control algorithm plays a significant role. However, to harness the full potential of MPPI, careful tuning of its hyperparameters is essential. This article will explore the importance of parameter tuning for MPPI in NAV2 and discuss how practicaly it can be handle.

2. Overview of NAV2

Navigation stack for ROS 2 also named Navigation2 (NAV2) is an advanced framework for robotic navigation that builds on the capabilities of the Robot Operating System 2 (ROS 2). It offers a comprehensive set of tools and algorithms for path planning, control, and recovery behaviors. MPPI is one of the control algorithms used within NAV2 to ensure accurate and efficient navigation.

NAV2 is designed to be highly modular, enabling developers to customize and extend its functionalities. Key components of NAV2 include global and local planners, controllers, recovery behaviors, and behavior trees. MPPI, as a local control algorithm, is responsible for generating smooth and feasible trajectories for the robot to follow in real-time.

3. Understanding MPPI in NAV2

MPPI is a sampling-based control algorithm that generates a set of potential trajectories and evaluates them based on a cost function. The algorithm then selects the trajectory with the lowest cost, ensuring optimal navigation. The cost function typically considers factors such as distance to the goal, obstacles, control efforts, ect. Taking a look at the paper behind this algorithm may help to understand how it works (specifically for the gamma parameter):

G. Williams et al. "Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving"

• URL : https://ieeexplore.ieee.org/document/8558663
• PDF : https://arxiv.org/pdf/1707.02342.pdf

In the context of NAV2, MPPI excels at handling dynamic environments and non-linear dynamics, making it suitable for complex navigation tasks. The algorithm's performance, however, heavily depends on the tuning of its hyperparameters, which control various aspects of the sampling and cost evaluation processes.

Created by Aleksei Budyakov and adapted & developed for NAV2 by Steve Macenski, the aim of MPPI is to become "the new default controller in NAV2" as it is the most"advanced predictive trajectory planner in the stack".

4. MPPI Controller Parameters Tuning in Humble

MPPI Parameters Official Description

Parameter	Type	Default	Definition
motion_model	string	DiffDrive	Type of model [DiffDrive, Omni, Ackermann]. AckermannConstraints Parameter Type Default Definition min_turning_r double 0.2 Minimum turning radius for ackermann motion model.
critics	string	None	Critics (plugins) names
iteration_count	int	1	Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches.
batch_size	int	1000	Count of randomly sampled candidate trajectories
time_steps	int	56	Number of time steps (points) in each sampled trajectory
model_dt	double	0.05	Time interval (s) between two sampled points in trajectories.
prune_distance	double	1.5	Distance ahead of nearest point on path to robot to prune path to.
vx_std	double	0.2	Sampling standard deviation for VX
vy_std	double	0.2	Sampling standard deviation for VY
wz_std	double	0.4	Sampling standard deviation for Wz
vx_max	double	0.5	Max VX (m/s)
vy_max	double	0.5	Max VY in either direction, if holonomic. (m/s)
vx_min	double	-0.35	Min VX (m/s)
wz_max	double	1.9	Max WZ (rad/s)
temperature	double	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
gamma	double	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.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information.
visualize	bool	false	Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. TrajectoryVisualizer Parameter Type Default Definition trajectory_step int 5 The step between trajectories to visualize to downsample candidate trajectory pool. time_step int 3 The step between points on trajectories to visualize to downsample trajectory density.
retry_attempt_limit	int	1	Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure.
regenerate_noises	bool	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.
max_robot_pose_search_dist	double	Costmap half-size	Max integrated distance ahead of robot pose to search for nearest path point in case of path looping.
transform_tolerance	double	0.1	Time tolerance for data transformations with TF.
enforce_path_inversion	bool	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.
inversion_xy_tolerance	double	0.2	Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion.
inversion_yaw_tolerance	double	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.

Tuning

Parameter	Tuning
motion_model	Has to suit your robot's motion model. AckermannConstraints min_turning_r Cannot be set to 0 to handle turning-in-place behavior (the value can be low but not zero).
critics	It's crucial to choose the right critics to use, and not all critics can be used with all motion models, here are the minimum critics for each model: - DiffDrive:       ["GoalCritic", "GoalAngleCritic", "ObstaclesCritic", "PathAngleCritic", "PathFollowCritic", "PreferForwardCritic"] - Omni:              ["GoalCritic", "GoalAngleCritic", "ObstaclesCritic", "TwirlingCritic", "PathFollowCritic", "PreferForwardCritic"] - Ackermann:  ["GoalCritic", "GoalAngleCritic", "ObstaclesCritic", "PathAngleCritic", "PathFollowCritic", "PreferForwardCritic"] These critics were determined by looking at the controller benchmark in the MPPI plugin code.
iteration_count	Can be left at 1.
batch_size	Can be left at 1000 (1000 at 50 Hz or 2000 at 30 Hz seems to produce good results).
time_steps	Must be adjusted to match model_dt and prune_distance constraints.
model_dt	Should be set to the duration of your control frequency. So if your control frequency is 20hz, this should be 0.05. However, you may also set it lower but not larger. Length of each time step's dt timestep, in seconds. time_steps * model_dt is the prediction horizon.
prune_distance	Reduce the length of the path to be followed by the robot, by eliminating parts of the path that are too far away to be relevant or necessary at any given time. This parameter relates to the prediction of the physical range of the trajectory, while the prediction horizon relates to the temporal range of the trajectory. I recommend keeping this parameter within the same order of magnitude as the prediction horizon, or at least proportional to it. Warning: this parameter is critical, a prune_distance too low compared to the desired speed will result in a truncated speed. prune_distance must therefore remain proportional to the target speed and prediction horizon.
vx_std	Indicates the extent to which VX, VY and WZ values may differ from their population average when MPPI is looking for a trajectory to follow. A value that is too low will not give the algorithm enough freedom and result in no trajectory being followed, and a value that is too high may result in non-optimal trajectories being chosen. A good tip is to leave them at their default values.
vy_std
wz_std
vx_max	Target speed for the DiffDrive and Ackermann motion models and X speed target for the Omni motion models
vy_max	+/-Y speed target for the Omni motion models
vx_min	Target speed when reversing
wz_max	Rotational target speed
temperature	Can be left at 0.3.
gamma	For more detailed information, I recommend referring to the associated research paper. In the vast majority of cases, this parameter can remain at 0.015.
visualize	Useful to activate when setting MPPI parameters. TrajectoryVisualizer trajectory_step The higher the value, the fewer paths will be displayed. I advise you to display only the main path when setting the parameters, by setting both parameters to 100 for example. time_step
retry_attempt_limit	Can be left at 1.
regenerate_noises	Can be left at false.
max_robot_pose_search_dist	Can remain unconfigured
transform_tolerance	Common parameters, you can manage it as you are used to. Can be left at 0.1.
enforce_path_inversion	Make driving in reverse predominant over driving forwards. To be enabled if this is the desired behavior.
inversion_xy_tolerance
inversion_yaw_tolerance

6. MPPI Critics Tuning in Humble

Constraint Critic

Official Description

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

Tuning

Depending on the motion model's kinematics, this critic determines whether the trajectory speed will comply with the maximum and minimum limits requested and penalises any deviation. Score function here.

Parameter	Tuning
cost_weight	Adjusts the relative influence of the critic in the overall cost calculation. A higher weight makes the critic more influential, which increases its impact on optimisation decisions.
cost_power	Affects the sensitivity of the cost compared with variations in the critic.

Goal Angle Critic

Official Description

Parameter	Type	Default	Definition
cost_weight	double	3.0	Weight to apply to critic term.
cost_power	int	1	Power order to apply to term.
threshold_to_consider	double	0.5	Minimal distance between robot and goal above which angle goal cost considered.

Tuning

This critic enables you to control the extent to which the orientation of the robot must coincide with the orientation of the target.
In my experience, the weight of this critic is generally one of the last to be modified if the final orientation is not to your liking. Please note: you must first be satisfied with the GoalChecker's yaw_goal_tolerance params in the controller.

Parameter	Tuning
cost_weight	Adjusts the relative influence of the critic in the overall cost calculation. A higher weight makes the critic more influential, which increases its impact on optimisation decisions.
cost_power	Affects the sensitivity of the cost compared with variations in the critic.
threshold_to_consider	Should be set at Path Angle Critic's threshold_to_consider for a smooth transition.

Goal Critic

Official Description

Parameter	Type	Default	Definition
cost_weight	double	5.0	Weight to apply to critic term.
cost_power	int	1	Power order to apply to term.
threshold_to_consider	double	1.4	Distance between robot and goal above which goal cost starts being considered

Tuning

This critic enables you to control the extent to which the position of the robot must coincide with the position of the target.
In my experience, the weight of this critic is generally one of the last to be modified if the final position is not to your liking. Please note: you must first be satisfied with the GoalChecker's xy_goal_tolerance params in the controller.

Parameter	Tuning
cost_weight	Adjusts the relative influence of the critic in the overall cost calculation. A higher weight makes the critic more influential, which increases its impact on optimisation decisions.
cost_power	Affects the sensitivity of the cost compared with variations in the critic.
threshold_to_consider	Can be set at prediction horizon for a smooth transition to the path-following critic.

Path Angle Critic

Official Description

Parameter	Type	Default	Definition
cost_weight	double	2.0	Weight to apply to critic term.
cost_power	int	1	Power order to apply to term.
threshold_to_consider	double	0.5	Distance between robot and goal above which path angle cost stops being considered
offset_from_furthest	int	4	Number of path points after furthest one any trajectory achieves to compute path angle relative to.
max_angle_to_furthest	double	1.2	Angular distance between robot and goal above which path angle cost starts being considered
forward_preference	bool	true	Whether or not your robot has a preference for which way is forward in motion. Different from if reversing is generally allowed, but if you robot contains no particular preference one way or another.

Tuning

This critic enables you to control the extent to which the orientation of the robot must coincide with the local upcoming orientation of the path generated by the planner.

Parameter	Tuning
cost_weight	Adjusts the relative influence of the critic in the overall cost calculation. A higher weight makes the critic more influential, which increases its impact on optimisation decisions.
cost_power	Affects the sensitivity of the cost compared with variations in the critic.
threshold_to_consider	Ideally, for a smooth transition, it should be defined at the distance of offset_from_furthest. Therefore, I suggest starting by setting it slightly greater than the prune_distance, and then adjusting it according to the desired behavior if necessary.
offset_from_furthest	furthest point = point on the path where at least one of the MPPI trajectories has approached furthest. Defined here. offset_from_furthest is the number of points after the furthest one that will be considerated to estimate the angle. The higher the value, the less the angular variation of the path will affect the MPPI trajectory. Note that this point is therefore dependent on the length of the paths generated by MPPI, in other words on the prune_distance.
max_angle_to_furthest	Threshold angle at which critic is activated. In other words, the robot is not realigned within this tolerance angle.
forward_preference	Different from the reversing parameter, in the case where there is no constraint, will favour or not the forward orientation.

Path Align Critic

Official Description

Parameter	Type	Default	Definition
cost_weight	double	10.0	Weight to apply to critic term.
cost_power	int	1	Power order to apply to term.
threshold_to_consider	double	0.5	Distance between robot and goal above which path align cost stops being considered
offset_from_furthest	int	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.
trajectory_point_step	int	4	Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable.
max_path_occupancy_ratio	double	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.
use_path_orientations	bool	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.

Tuning

This critic enables you to control the extent to which the position of the robot must coincide with the position of the path generated by the planner based on the costmap. Indeed, the aim is to minimise deviations from the path.
Note that this critic must be balanced with the deviating critics, Cost Critic or Obstacles Critic, who will tend to stray from the path.

Parameter	Tuning
cost_weight	Adjusts the relative influence of the critic in the overall cost calculation. A higher weight makes the critic more influential, which increases its impact on optimisation decisions.
cost_power	Affects the sensitivity of the cost compared with variations in the critic.
threshold_to_consider	May be set at Goal Critic's threshold_to_consider for a smooth transition.
offset_from_furthest	furthest point = point on the path where at least one of the MPPI trajectories has approached furthest. Defined here. offset_from_furthest is the number of points after the furthest one that will be considerated to estimate that the path is followed. The higher the value, the less the deviations from the path will affect the MPPI trajectory. Note that this point is therefore dependent on the length of the paths generated by MPPI, in other words on the prune_distance.
trajectory_point_step	Specifies the step of trajectory points to evaluate for path distance. This reduces computational effort by evaluating path alignment at spaced intervals rather than every single point.
max_path_occupancy_ratio	Sets the maximum proportion of the path that can be occupied by obstacles before this critic is no longer considered. This allows the algorithm to focus on path-following even in dynamic environments, adjusting its behavior based on obstacle presence. The higher the value, the later the avoidance.
use_path_orientations	If set to true, the algorithm considers changes in path orientation, potentially influencing directional adjustments based on the planner's guidance. If set to false, it adheres strictly to the path's intended trajectory without incorporating directional changes based on orientation cues.

Path Follow Critic

Official Description

Parameter	Type	Default	Definition
cost_weight	double	5.0	Weight to apply to critic term.
cost_power	int	1	Power order to apply to term.
offset_from_furthest	int	6	Number of path points after furthest one any trajectory achieves to drive path tracking relative to.
threshold_to_consider	float	1.4	Distance between robot and goal above which path follow cost stops being considered

Tuning

This critic is designed to encourage the robot to choose actions that move it along the planned trajectory towards its goal. This means that the system favours movements that progress in the direction of the goal rather than sideways or away from it.

Parameter	Tuning
cost_weight	Adjusts the relative influence of the critic in the overall cost calculation. A higher weight makes the critic more influential, which increases its impact on optimisation decisions.
cost_power	Affects the sensitivity of the cost compared with variations in the critic.
offset_from_furthest	furthest point = point on the path where at least one of the MPPI trajectories has approached furthest. Defined here. offset_from_furthest is the number of points after the furthest one that will be considerated to estimate that the path is followed. The higher the value, the less the deviations from the path will affect the MPPI trajectory. Note that this point is therefore dependent on the length of the paths generated by MPPI, in other words on the prune_distance.
threshold_to_consider	Can be set at prediction horizon for a smooth transition with goal critics.

Prefer Forward Critic

Official Description

Parameter	Type	Default	Definition
cost_weight	double	5.0	Weight to apply to critic term.
cost_power	int	1	Power order to apply to term.
threshold_to_consider	double	0.5	Distance between robot and goal above which prefer forward cost stops being considered

Tuning

This critic incentivizes moving in the forward direction, rather than reversing.

Parameter	Tuning
cost_weight	Adjusts the relative influence of the critic in the overall cost calculation. A higher weight makes the critic more influential, which increases its impact on optimisation decisions.
cost_power	Affects the sensitivity of the cost compared with variations in the critic.
threshold_to_consider	Can be set at Goal Angle Critic's threshold_to_consider for a smooth transition.

Twirling Critic

Official Description

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

Tuning

This criticism only concerns the Omni model, to avoid getting a spinning top robot when the track is followed.

Parameter	Tuning
cost_weight	Adjusts the relative influence of the critic in the overall cost calculation. A higher weight makes the critic more influential, which increases its impact on optimisation decisions.
cost_power	Affects the sensitivity of the cost compared with variations in the critic.

Velocity Deadband Critic

Official Description

Parameter	Type	Default	Definition
cost_weight	double	35.0	Weight to apply to critic term.
cost_power	int	1	Power order to apply to term.
deadband_velocities	double[]	[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.

Tuning

Since MPPI includes speed and acceleration limits, the velocity_smoother has become practically irrelevant. The only thing missing is the deadband and with this Velocity Deadband Critic we can now get rid of the velocity_smoother.

Cost Critic and Obstacles Critic

Cost Critic and Obstacles Critic can be difficult to differentiate and knowing which of these two avoidance critics to choose can be confusing.
Based on the original Cost Critic issue, this is what can be learned from it:

Critic	Advantages	Disadvantages
Obstacles Critic	Fine distance evaluation: provides a precise assessment of collision risks. Detailed collision management: granular risk management for close obstacles. Parameter flexibility: allows fine-tuning for behavior around obstacles.	Increased complexity: adds algorithmic complexity with distance calculations from costs. Higher computational load: estimating distances and footprint inflation increases computational load. Quantization effect: cost-to-distance conversion may introduce quantization effects. Inscribed area precision issue: score remains the same regardless of the exact distance to the obstacle.
Cost Critic (proposed InflationCostCritic)	Simplicity: directly uses cost values, simplifying the calculation. Efficiency: reduces computational load by avoiding complex conversions. Adaptive behavior: flexible in narrow and wide spaces due to the exponential factor. Easier tuning: fewer parameters to adjust, reducing the risk of quantization effects.	Less precision: does not account for precise distances to obstacles. Fixed critical score: less granular risk management. Dependence on costmap quality: performance relies on the quality and resolution of the costmap.

With that in mind, it's up to you to decide which of these critics is best suited to your use case. One point to note is that in the event of a narrow zone crossing, Cost Critic seems more likely to generate centred navigation between obstacles than Obstacles Critic.

Cost Critic

Official Description

Parameter	Type	Default	Definition
consider_footprint	bool	False	Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost.
cost_weight	double	3.81	Wight to apply to critic to avoid obstacles.
cost_power	int	1	Power order to apply to term.
collision_cost	double	1000000.0	Cost to apply to a true collision in a trajectory.
critical_cost	double	300.0	Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles.
near_goal_distance	double	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.
inflation_layer_name	string	""	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.

Obstacles Critic

Official Description

Parameter	Type	Default	Definition
consider_footprint	bool	False	Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost.
critical_weight	double	20.0	Weight to apply to critic for near collisions closer than collision_margin_distance to prevent near collisions only as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from critical collisions.
repulsion_weight	double	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 inflation_radius distance from obstacles.
cost_power	int	1	Power order to apply to term.
collision_cost	double	10000.0	Cost to apply to a true collision in a trajectory.
collision_margin_distance	double	0.10	Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable.
near_goal_distance	double	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.
cost_scaling_factor	double	10.0	Exponential decay factor across inflation radius. This should be the same as for your inflation layer (Humble only)
inflation_radius	double	0.55	Radius to inflate costmap around lethal obstacles. This should be the same as for your inflation layer (Humble only)

8. Conclusion

Tuning is a critical step in optimizing the performance of the MPPI controller in ROS 2 NAV2. By carefully adjusting parameters, significant improvements in navigation efficiency and robustness can be achieved.

9. References

• NAV2 Documentation.
• NAV2_mppi_controller github
• NAV2_mppi_controller github Notes to Users

These references provide comprehensive information on configuring the MPPI controller in the Navigation2 stack, including detailed documentation, source code, and user notes, which form the basis of this discussion.