Model Predictive Path Integral Controller

Overview

This is a predictive controller (local trajectory planner) that implements the Model Predictive Path Integral (MPPI) algorithm to track a path with adaptive collision avoidance. It contains plugin-based critic functions to impact the behavior of the algorithm. It was created by Aleksei Budyakov and adapted & developed for Nav2 by Steve Macenski.

This plugin implements the nav2_core::Controller interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server’s action server (controller_server).

This controller is measured to run at 50+ Hz on a modest Intel processor (4th gen i5). See its Configuration Guide Page for additional parameter descriptions.

It works currently with Differential, Omnidirectional, and Ackermann robots.

MPPI Description

The MPPI algorithm is an MPC variant that finds a control velocity for the robot using an iterative approach. Using the previous time step’s best control solution and the robot’s current state, a set of randomly sampled perturbations from a Gaussian distribution are applied. These noised controls are forward simulated to generate a set of trajectories within the robot’s motion model.

Next, these trajectories are scored using a set of plugin-based critic functions to find the best trajectory in the batch. The output scores are used to set the best control with a soft max function.

This process is then repeated a number of times and returns a converged solution. This solution is then used as the basis of the next time step’s initial control.

Features

• Predictive MPC trajectory planner
• Utilizes plugin-based critics which can be swapped out, tuned, or replaced easily by the user
• Highly optimized CPU-only performance using vectorization and tensor operations
• Supports a number of common motion models, including Ackermann, Differential-Drive, and Omni-directional
• Includes fallback mechanisms to handle soft-failures before escalating to recovery behaviors
• High-quality code implementation with Doxygen, high unit test coverage, documentation, and parameter guide
• Easily extensible to support modern research variants of MPPI
• Comes pre-tuned for good out-of-the-box behavior

Configuration

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 | Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches. | | batch_size | int | Default 1000. Count of randomly sampled candidate trajectories | | time_steps | int | Default 56. Number of time steps (points) in each sampled trajectory | | model_dt | double | Default: 0.05. Time interval (s) between two sampled points in trajectories. | | 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 | 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 | | 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.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 | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | | reset_period | double | Default 1.0. required time of inactivity to reset optimizer (only in Humble due to backport ABI policies) | | 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. |

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 | 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. | | transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with 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. | | 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. | | 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. |

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. Minimal distance between robot and goal above which angle goal cost considered. |

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. Distance between robot and goal above which goal cost starts being considered |

File truncated at 100 lines see the full file