Model Predictive Path Integral Controller

:rotating_light: :rotating_light: While this project repository is absolutely critical and led to MPPI’s inclusion into the Nav2 framework by the amazing work and efforts by @artofnothingness, the version in Nav2’s repository fixes a number of bugs major and minor and will continue to receive community improvements and be maintained alongside the rest of the navigation stack into the future. Please see that version moving forward! This repository continues to be an important historical project to reference decisions made and Git histories of changes during development. :rotating_light: :rotating_light:

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

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 standart deviation for VX | | vy_std | double | Default 0.2. Sampling standart deviation for VY | | wx_std | double | Default 0.4. Sampling standart deviation for WX | | 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 considiration 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. |

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. |

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

Obstacles Critic

| Parameter | Type | Definition | | ————— | —— | ———————————————————————————————————– | | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | | critical_weight | double | Default 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 criticial collisions. |

File truncated at 100 lines see the full file