Skip to content

Robot-Nav/SA-MPPI

BranchesTags

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

16 Commits
src
src
 
 
LICENSE
LICENSE
 
 
README.md
README.md
 
 
README_CN.md
README_CN.md
 
 
setup_sa_mppi_noetic.sh
setup_sa_mppi_noetic.sh
 
 

Repository files navigation

SA MPPI (Shape Aware MPPI) Local Planner: Real-time Trajectory Planning for Arbitrarily Shaped Robots

C++17 MPPI Cross Platform xtensor License Status

Declaration: 👉 README_CN This repository is a decoupled version of the MPPI local planner from Navigation2. Due to limited technical expertise, there may be some "hacky" modifications and non-standard code. Some implementation logic may differ from the original version. The code is under continuous development. If there are any inaccuracies, please feel free to point them out — your understanding is greatly appreciated! Issues and PRs are welcome.

Highly recommend website：👉 Different MPPI image

Reference Blogs:

  1. Shape-Aware MPPI (SA MPPI) Algorithm: Real-time Trajectory Optimization for Arbitrarily Shaped Robots Based on RC-ESDF

  2. RC-ESDF Explained: Robot-Centric Euclidean Signed Distance Field

  3. MPPI_C++: Decoupled MPPI Local Planner from Nav2 (Hackable)

  4. SA-MPPI Cost Calculator Online Interactive Webpage

Table of Contents

1. Project Overview

A decoupled implementation of the MPPI (Model Predictive Path Integral) controller extracted from the ROS2 Nav2 framework, written in C++17 and adapted for the ROS1 environment. The core controller has no dependencies on Nav2 infrastructure (rclcpp, nav2_core, nav2_costmap_2d, pluginlib, etc.), requiring only standard C++17 and the header-only xtensor numerical library. Combined with the RC-ESDF algorithm, it builds a zoned collision detection mechanism based on RC-ESDF's precise distance queries. In the risk zone, RC-ESDF's analytical gradient is used to bias MPPI sampling, guiding candidate trajectories to expand toward safer regions and improving the flexibility and smoothness of dynamic obstacle avoidance for arbitrarily shaped robots.

1.1 Core Positioning

  • Framework Agnostic: No dependency on ROS2/Nav2 lifecycle nodes, Costmap2D, pluginlib, or other infrastructure — embeddable into any C++ project
  • LIDAR Native: Directly uses laser point clouds as obstacle input, building a local BFS grid distance field for fast distance queries without relying on global costmaps. The local distance field replaces Nav2-MPPI's costmap dependency, decoupling obstacle detection from centralized global layers to a robot-centric perception loop. This distance field module can further integrate RC-ESDF (Robot-Centric Euclidean Signed Distance Field), which precomputes signed distance fields offline centered on the robot, supporting precise distance queries and zoned collision detection for arbitrary polygon footprints. In the risk zone, its analytical gradient can be used to bias MPPI sampling and guide candidate trajectories toward safer regions. It offers significant advantages in narrow-space navigation and non-circular robot obstacle avoidance. See SA-MPPI: Real-time Trajectory Optimization for Arbitrarily Shaped Robots Based on RC-ESDF for details.
  • Lightweight Deployment: Depends only on xtensor, simple compilation, suitable for embedded and real-time scenarios
  • Feature Complete: Retains the core algorithm logic of Nav2 MPPI with several practical enhancements

1.2 Core Capabilities

  • Real-time Trajectory Optimization: Optimizes control sequences at 20Hz (50ms control period)
  • LIDAR Obstacle Avoidance: Efficient collision detection using BFS-based local grid distance field
  • Multi-threaded Sampling: Configurable thread pool for parallel trajectory generation
  • Dynamic Obstacle Support: Predictive collision avoidance for moving obstacles, extensible
  • Blocking Detection: Prevents spinning-in-place behavior when blocked by obstacles
  • Multiple Motion Models: Supports differential drive, omnidirectional, and Ackermann steering models
  • Omni/Diff Automatic Switching: Automatically switches motion models based on obstacle distance and path deviation

1.3 Basic Demo

SA-MPPI.mp4

2. Features

2.1 Core Features

Feature Description
Batch Sampling 1000+ trajectory samples per control cycle
Grid Distance Field BFS-based local distance field for O(1) obstacle distance queries
Multi-threading Configurable thread count for parallel trajectory integration
Adaptive Temperature Dynamic temperature adjustment based on cost distribution
SG Filtering Savitzky-Golay filter for smooth control sequences
Footprint Support Collision detection for non-circular robots (polygon footprint)
Dynamic Obstacles Velocity-based linear prediction for collision avoidance
Blocking Detection Detects spinning-in-place and outputs zero velocity
Omni/Diff Switching Automatically switches motion models based on environment
Path Blocking Detection Automatically disables alignment cost when path is heavily occupied
Path Point Validity Skips path points blocked by obstacles
Startup Assist Resolves zero-velocity startup issues / redundant, removable

2.2 Cost Function Components

Cost Function Purpose Activation Condition
ObstaclesCritic Exponential repulsion-based collision avoidance (grid distance field + dynamic prediction) When away from goal
PathFollowCritic Endpoint tracking along global path When away from goal
PathAlignCritic Trajectory alignment with path geometry (includes path blocking detection) When path is not blocked
PathAngleCritic Heading alignment with path direction (three modes) When away from goal
GoalCritic Final position convergence When near goal
GoalAngleCritic Final heading alignment When near goal
PreferForwardCritic Penalizes backward motion When away from goal
ConstraintCritic Kinematic constraint enforcement (diff/omni/ackermann) Always
TwirlingCritic Suppresses spinning-in-place behavior When away from goal
VelocityDeadbandCritic Low-speed stability improvement Always

2.3 Cross-Platform Portability

Platform Support Notes
Linux (x86/ARM) Full Primary development platform
ROS1 Full Reference implementation provided
ROS2 Compatible Uses ROS2 message type wrappers
Embedded Linux Full Self-testable
Real-time OS Compatible No dynamic memory allocation in hot path

Dependencies (Core Controller Only):

  • C++17 compiler
  • xtensor 0.21 (header-only numerical library)
  • Standard C++ library only

3. Mathematical Principles

3.1 MPPI Core Algorithm

The MPPI algorithm solves stochastic optimal control problems by sampling trajectories and computing weighted averages of control sequences. Unlike traditional MPC that requires solving constrained optimization problems, MPPI transforms the optimal control problem into a path integral estimation problem, solved via Monte Carlo sampling without complex gradient computations or constraint handling.

State Dynamics (Differential Drive)

$$\dot{x} = v_x \cos\theta$$

$$\dot{y} = v_x \sin\theta$$

$$\dot{\theta} = \omega_z$$

Where:

  • $(x, y)$: Robot position in world coordinates
  • $\theta$: Robot heading
  • $v_x$: Linear velocity
  • $\omega_z$: Angular velocity

Discretized Motion Model

$$x_{t+1} = x_t + (v_x \cos\theta - v_y \sin\theta) \cdot \Delta t$$

$$y_{t+1} = y_t + (v_x \sin\theta + v_y \cos\theta) \cdot \Delta t$$

$$\theta_{t+1} = \theta_t + \omega \cdot \Delta t$$

Where $v_y$ is non-zero only in the omnidirectional motion model.

Trajectory Sampling

For each batch $i$ and time step $t$:

$$u_t^{(i)} = \bar{\nu}_t + \epsilon_t^{(i)}, \quad \epsilon_t^{(i)} \sim \mathcal{N}(0, \Sigma)$$

Where:

  • $\bar{\nu}_t$: Mean control from previous iteration (current control sequence)
  • $\epsilon_t^{(i)}$: Gaussian noise with standard deviation $\sigma$

Trajectory Cost Function

Total cost for trajectory $i$:

$$S(\tau^{(i)}) = \sum_{t=0}^{T-1} \left[ q(x_t^{(i)}) + \frac{\gamma}{2\sigma^2} \bar{\nu}_t^2 + \frac{\gamma}{\sigma^2} \bar{\nu}_t \cdot \epsilon_t^{(i)} \right] \cdot \Delta t$$

Where:

  • $q(x_t^{(i)})$: State cost (computed by each Critic function)
  • $\gamma$: Control cost weight parameter
  • $\sigma$: Sampling noise standard deviation
  • $\bar{\nu}_t$: Current control sequence
  • $\epsilon_t^{(i)}$: Noise for trajectory $i$
  • $T$: Time horizon (number of steps)

The cost function consists of three parts:

  1. State cost $q(x_t^{(i)})$: Independently computed and accumulated by 10 Critic functions
  2. Control regularization $\frac{\gamma}{2\sigma^2}\bar{\nu}_t^2$: Penalizes excessive control magnitudes
  3. Control-noise cross term $\frac{\gamma}{\sigma^2}\bar{\nu}_t \cdot \epsilon_t^{(i)}$: Ensures unbiased optimal control update

Softmax Weighting

The optimal control update uses softmax weights:

$$w^{(i)} = \frac{\exp\left(-\frac{1}{\lambda}(S(\tau^{(i)}) - S_{\min})\right)}{\sum_{j=1}^{K} \exp\left(-\frac{1}{\lambda}(S(\tau^{(j)}) - S_{\min})\right)}$$

Where:

  • $\lambda$: Temperature parameter controlling weight distribution sharpness
  • $S_{\min}$: Minimum cost among all trajectories (for numerical stability)
  • $K$: Batch size (number of samples)

Mean normalization mode is also supported:

$$w^{(i)} = \frac{\exp\left(-\frac{1}{\lambda}(S(\tau^{(i)}) - \bar{S})\right)}{\sum_{j=1}^{K} \exp\left(-\frac{1}{\lambda}(S(\tau^{(j)}) - \bar{S})\right)}$$

Where $\bar{S}$ is the mean cost across all trajectories.

Control Update

Updated control sequence:

$$\bar{\nu}_t^{\text{new}} = \sum_{i=1}^{K} w^{(i)} \cdot u_t^{(i)}$$

3.2 Obstacle Cost Function

The obstacle cost function uses exponential repulsion cost:

$$C_{\text{obs}}(d) = \begin{cases} C_{\text{collision}} & \text{if } d < d_{\text{margin}} \ w_{\text{repulsion}} \cdot \exp(-\alpha \cdot (d - d_{\text{margin}})) & \text{if } d_{\text{margin}} \le d < r_{\text{inflation}} \ 0 & \text{otherwise} \end{cases}$$

Where:

  • $d$: Distance to nearest obstacle
  • $d_{\text{margin}}$: Collision boundary distance (collision_margin)
  • $r_{\text{inflation}}$: Inflation radius (inflation_radius)
  • $\alpha$: Cost scaling factor (cost_scaling)
  • $w_{\text{repulsion}}$: Repulsion weight (repulsion_weight)
  • $C_{\text{collision}}$: Collision cost (very large, e.g., 100000)

3.3 Grid Distance Field

The local distance field is computed using BFS (Breadth-First Search) centered on the robot:

For each grid cell (i, j):
  D[i,j] = min_{obstacle cells} ||(i,j) - (i_obs, j_obs)||

Construction process:

  1. Map all obstacle points to grid coordinates with distance 0
  2. BFS four-neighbor propagation from all obstacle cells
  3. Each neighbor's distance = current distance + grid resolution

Querying is O(1) via direct coordinate mapping.

Default parameters: 0.05m resolution, 100×100 grid, covering 5m×5m local area.

3.4 Acceleration Constraints

Velocity updates obey acceleration limits:

$$v_{t+1} = v_t + \text{clamp}(v_{\text{desired}} - v_t, -a_{\max} \Delta t, a_{\max} \Delta t)$$

Where:

  • $a_{\max}$: Maximum acceleration
  • $\Delta t$: Time step

This constraint is implemented in the motion model's predict() method, ensuring generated trajectories satisfy physical acceleration limits.

3.5 Blocking Detection

The controller uses rotation ratio to detect blocking:

$$\text{ratio} = \frac{|\omega_z|}{|v_x| + \epsilon}$$

If $\text{ratio} &gt; \theta_{\text{threshold}}$ for $N$ consecutive frames, the robot is considered blocked and commanded to zero velocity.

Blocking detection has two layers:

  1. TwirlingCritic: Soft constraint via cost function penalizing "low forward + high rotation" behavior
  2. Optimizer::isBlocked(): Hard constraint detecting average wz/vx ratio in control sequence, outputting zero velocity when exceeding threshold for consecutive frames

3.6 Savitzky-Golay Filtering

Control sequence smoothing uses SG filter coefficients:

$$\nu_t^{\text{filtered}} = \sum_{k=-2}^{2} c_{k+2} \cdot \nu_{t+k}$$

Default coefficients: $c = [-0.085714, 0.342857, 0.485714, 0.342857, -0.085714]$ (5-point, 2nd-order fit).

Boundary handling: When indices exceed control sequence range, control history (previous frames' actual control) is used for padding.

3.7 Adaptive Temperature

When adaptive temperature is enabled, the temperature parameter adjusts dynamically based on cost distribution:

$$T = \text{clamp}\left(T_{\text{base}} \cdot \left(1 + \frac{\ln(S_{\max} - S_{\min} + 1)}{5}\right), T_{\min}, T_{\max}\right)$$

Automatically increases temperature to smooth weights when cost distribution range is large, and decreases temperature to highlight optimal trajectories when cost distribution is concentrated.

4. Architecture Design

4.1 Layered Architecture

Layered architecture separates core algorithms from framework-specific implementations:

┌─────────────────────────────────────────────────────────────────┐
│                        ROS1 Application Layer                    │
│  ┌──────────────────────────────────────────────────────────┐   │
│  │              mppi_ros1_node.cpp                          │   │
│  │  Subscribes: /plan, /odom, /scan  |  Publishes: /cmd_vel, debug │   │
│  │  Parameter loading | Startup assist | TF | Exception handling │   │
│  └──────────────────────┬───────────────────────────────────┘   │
└─────────────────────────┼───────────────────────────────────────┘
                          │
┌─────────────────────────▼───────────────────────────────────────┐
│                     Controller Interface Layer                   │
│  ┌──────────────────────────────────────────────────────────┐   │
│  │              MPPIController (controller.hpp)              │   │
│  │  Init | Cost param setting | Obstacle update | Motion model switch │   │
│  │  Path point validity | Omni/diff auto-switching          │   │
│  └──────────┬──────────────────────────┬────────────────────┘   │
└─────────────┼──────────────────────────┼────────────────────────┘
              │                          │
┌─────────────▼──────────┐  ┌────────────▼────────────────────────┐
│      Optimizer Core     │  │         Cost Function Layer         │
│  ┌──────────────────┐  │  │  ┌─────────────────────────────┐    │
│  │  Optimizer       │  │  │  │     CriticManager           │    │
│  │  Sampling | Scoring│  │  │  ┌───────────────────────┐  │    │
│  │  Weighted update │  │  │  │ ObstaclesCritic       │  │    │
│  │  Filtering       │  │  │  │ PathFollowCritic      │  │    │
│  │  Blocking detect │  │  │  │ PathAlignCritic       │  │    │
│  │  Path pruning    │  │  │  │ PathAngleCritic       │  │    │
│  └───────┬──────────┘  │  │  │ GoalCritic            │  │    │
│          │              │  │  │ GoalAngleCritic       │  │    │
│  ┌───────▼──────────┐  │  │  │ PreferForwardCritic   │  │    │
│  │  NoiseGenerator  │  │  │  │ ConstraintCritic      │  │    │
│  │  Async noise     │  │  │  │ TwirlingCritic        │  │    │
│  └──────────────────┘  │  │  │ VelocityDeadbandCrit  │  │    │
│  ┌──────────────────┐  │  │  └───────────────────────┘  │    │
│  │  MotionModel     │  │  └─────────────────────────────┘    │
│  │  Diff | Omni     │  │                                      │
│  │  Ackermann       │  │                                      │
│  └──────────────────┘  │                                      │
└────────────────────────┘  └──────────────────────────────────────┘
              │                          │
┌─────────────▼──────────────────────────▼──────────────────────────┐
│                        Data Model Layer                            │
│  types | constraints | control_sequence | state | trajectories   │
│  path  | optimizer_settings                                        │
├───────────────────────────────────────────────────────────────────┤
│                        Utilities Layer                             │
│  math_utils (angle normalization, clamp) | noise_generator       │
├───────────────────────────────────────────────────────────────────┤
│                        External Dependencies (Minimal)            │
│  xtensor (header-only) | xsimd (SIMD) | xtl (base) | C++17       │
└───────────────────────────────────────────────────────────────────┘

4.2 Key Design Principles

  1. Zero ROS Dependency in Core: All headers under include/ use only standard C++17 and xtensor, containing no ROS headers
  2. Modular Separation: Split into 20+ header files by functional responsibility, each focused on a single concern
  3. Inheritance-based Design: Motion models and cost functions define interfaces via base classes with derived implementations
  4. Asynchronous Noise Generation: Independent thread pre-generates noise to reduce main loop latency
  5. Multi-threaded Trajectory Integration: Configurable thread count for parallel computation
  6. Deterministic Execution: No non-deterministic operations in the control loop

4.3 File Organization

mppi_laser_example/
├── include/
│   ├── controller.hpp              # Public interface: MPPIController class
│   ├── optimizer.hpp               # Optimizer core: sampling, scoring, weighted update
│   ├── motion_models.hpp           # Motion models: DiffDrive/Omni/Ackermann
│   ├── models/
│   │   ├── types.hpp               # Basic types: Point2D, Pose2D, Twist2D, Control, ObstaclePrediction
│   │   ├── constraints.hpp         # Constraint parameters: ControlConstraints, SamplingStd
│   │   ├── control_sequence.hpp    # Control sequence data structure
│   │   ├── state.hpp               # State container (batch velocities + controls)
│   │   ├── trajectories.hpp        # Trajectory container (batch positions + headings)
│   │   ├── path.hpp                # Global path data structure
│   │   └── optimizer_settings.hpp  # Optimizer configuration parameters
│   ├── critics/
│   │   ├── critic_function.hpp     # Cost function base class
│   │   ├── critic_data.hpp         # Evaluation data structure
│   │   ├── critic_manager.hpp      # Cost function manager
│   │   ├── obstacles_critic.hpp    # Obstacle cost (grid distance field)
│   │   ├── path_follow_critic.hpp  # Path following cost
│   │   ├── path_align_critic.hpp   # Path alignment cost
│   │   ├── path_angle_critic.hpp   # Path angle cost
│   │   ├── goal_critic.hpp         # Goal point cost
│   │   ├── goal_angle_critic.hpp   # Goal heading cost
│   │   ├── prefer_forward_critic.hpp # Prefer forward cost
│   │   ├── constraint_critic.hpp   # Kinematic constraint cost
│   │   ├── twirling_critic.hpp     # Spinning penalty cost
│   │   └── velocity_deadband_critic.hpp # Velocity deadband cost
│   └── tools/
│       ├── math_utils.hpp          # Math utilities: angle normalization, clamp
│       └── noise_generator.hpp     # Asynchronous Gaussian noise generator
├── src/
│   └── mppi_ros1_node.cpp          # ROS1 main node
├── scripts/
│   ├── mppi_path_publisher.py      # Path publisher (straight/rectangle/S-curve)
│   ├── mppi_path_publisher_fz.py   # Enhanced path publisher (with rotation support)
├── config/
│   ├── mppi_params.yaml            # Parameter config (detailed Chinese comments)
│   └── mppi_test.rviz              # RViz visualization config
├── CMakeLists.txt                  # Catkin build config
├── package.xml                     # ROS1 package manifest
├── xtensor/                        # Third-party: multi-dimensional arrays
├── xsimd/                          # Third-party: SIMD vectorization
└── xtl/                            # Third-party: xtensor base library

5. Module Details

5.1 Data Model Layer (models/)

types.hpp — Basic Data Types

Type Fields Description
Point2D x, y 2D point
Pose2D x, y, theta 2D pose
Twist2D vx, vy, wz 2D velocity
Control vx, vy, wz Single-step control
ObstaclePrediction x, y, vx, vy, radius Dynamic obstacle prediction with positionAt(t) method

ObstaclePrediction::positionAt(t) performs linear prediction based on obstacle velocity:

$$\hat{x}(t) = x_0 + v_x \cdot t, \quad \hat{y}(t) = y_0 + v_y \cdot t$$

constraints.hpp — Control Constraints

  • ControlConstraints: Velocity limits (vx_max/min, vy_max, wz_max), acceleration limits (ax/ay/az_max), collision cost threshold
  • SamplingStd: Sampling noise standard deviations (vx, vy, wz)

control_sequence.hpp — Control Sequence

ControlSequence: Control per time step (vx[], vy[], wz[]) stored as xt::xtensor<float, 1>. Supports reset(time_steps).

state.hpp — State Container

State: Controls and velocities for all sampled trajectories as [batch, time] 2D tensors:

  • cvx, cvy, cwz: Noised controls (sampling results)
  • vx, vy, wz: Velocities after acceleration constraints
  • pose: Current pose (Pose2D)
  • speed: Current velocity (Twist2D)

trajectories.hpp — Trajectory Container

Trajectories: Positions and headings for all sampled trajectories as [batch, time] 2D tensors:

  • x, y: Positions
  • yaws: Headings
  • times: Timestamps

path.hpp — Global Path

Path: Discrete path point sequence (x[], y[], yaws[]) with getGoal() for endpoint and reset(size).

optimizer_settings.hpp — Optimizer Settings

OptimizerSettings: All configurable parameters — see Parameter Configuration section.

5.2 Utilities Layer (tools/)

math_utils.hpp — Basic Math Utilities

Function Purpose
normalizeAngle(angle) Normalize angle to $[-\pi, \pi]$
shortestAngularDistance(from, to) Shortest signed angular difference
clamp(val, min_val, max_val) Value clamping

Constants: PI = 3.14159265f, TWO_PI = 6.28318530f, EPSILON = 1e-6f

noise_generator.hpp — Asynchronous Gaussian Noise Generator

NoiseGenerator uses a separate thread for asynchronous pre-generation:

Main thread:   setNoisedControls() → wait for noise ready → read noise → trigger next generation
Noise thread:  wait for trigger → generate noise → notify ready → wait for trigger
  • Uses xt::random::randn for Gaussian noise generation
  • std::mutex + std::condition_variable for thread synchronization
  • setNoisedControls(): Applies noise to control sequence
  • generateNextNoises(): Triggers asynchronous pre-generation of next noise batch

5.3 Cost Function Layer (critics/)

critic_function.hpp — Cost Function Base Class

class CriticFunction {
    virtual void initialize() = 0;
    virtual void score(CriticData & data) = 0;
    void setEnabled(bool enabled);
    bool isEnabled() const;
    void setName(const std::string& name);
    std::string getName() const;
};

critic_data.hpp — Evaluation Data Structure

CriticData: Data collection passed to cost functions, containing:

  • state: State container
  • trajectories: Trajectory container
  • path: Global path
  • costs: Cost array (accumulated by each Critic)
  • model_dt: Time step
  • fail_flag: Failure flag (set true when all trajectories collide)
  • motion_model: Current motion model
  • furthest_reached_path_point: Furthest reachable path point index
  • dynamic_obstacles: List of dynamic obstacles
  • path_pts_valid: Path point validity flags

critic_manager.hpp — Cost Function Manager

CriticManager: Manages all cost functions, executes evalTrajectoriesScores(data) for batch scoring, iterating through all registered Critics and calling score().

obstacles_critic.hpp — Obstacle Cost (Core)

ObstaclesCritic is the most complex cost function with core flow:

  1. Grid Reconstruction: Build local grid centered at robot position, map obstacle points, BFS four-neighbor distance field propagation
  2. Trajectory Scoring: Query distance field point-by-point for each trajectory
  3. Circle Mode: Query distance from trajectory point to nearest obstacle, subtract robot radius
  4. Footprint Mode: Transform footprint points to world coordinates, query distance field per point, take minimum distance
  5. Dynamic Obstacles: Predict obstacle positions over time based on velocity, compute distances to trajectory points
  6. Collision Detection: Distance < collision_margin → collision, assign collision_cost
  7. Repulsion Cost: Distance in [collision_margin, inflation_radius) → exponential decay repulsion
  8. Total Collision Detection: Set fail_flag when all trajectories collide

path_follow_critic.hpp — Path Following Cost

Computes squared distance between trajectory endpoint and path reference point:

$$ C_{\mathrm{follow}} = w \cdot \left| \mathbf{p}_{\mathrm{traj_end}} - \mathbf{p}_{\mathrm{path_ref}} \right|^2 $$

Reference point determined by furthest_reached_path_point + offset. Supports skipping path points blocked by obstacles.

path_align_critic.hpp — Path Alignment Cost

Computes average distance between each trajectory point and its nearest path point:

$$ C_{\text{align}} = w \cdot \frac{1}{N} \sum_{j=0}^{N-1} \left| \mathbf{p}_{\text{traj}}(j) - \mathbf{p}_{\text{path}}(\text{closest}(j)) \right| $$

Key features:

  • Path Cumulative Distance Matching: Nav2-style cumulative distance matching to avoid brute force search

  • Path Blocking Detection: Automatically disabled when path occupancy ratio exceeds max_path_occupancy_ratio

  • Heading Consideration: Optionally incorporates heading deviation into distance:

    $$ d' = d \cdot (1 + 0.5 \cdot |\Delta\theta|) $$

path_angle_critic.hpp — Path Angle Cost

Penalizes heading deviation between trajectory endpoint and path reference point. Three modes:

Mode Value Description
FORWARD_PREFERENCE 0 Prefers forward direction, penalizes heading misalignment
NO_DIRECTIONAL_PREFERENCE 1 Allows reverse motion, penalizes heading deviation magnitude only
CONSIDER_FEASIBLE_PATH_ORIENTATIONS 2 Selects reference heading based on velocity direction

goal_critic.hpp — Goal Point Cost

Penalizes squared distance between trajectory endpoint and goal point when near goal:

$$ C_{\mathrm{goal}} = w \cdot \left| \mathbf{p}_{\mathrm{traj_end}} - \mathbf{p}_{\mathrm{goal}} \right|^2 $$

Active only when within goal_threshold distance of goal.

goal_angle_critic.hpp — Goal Heading Cost

Penalizes squared heading deviation between trajectory endpoint and goal heading when near goal:

$$ C_{\mathrm{goal_angle}} = w \cdot (\theta_{\mathrm{traj_end}} - \theta_{\mathrm{goal}})^2 $$

Active only when within goal_angle_threshold distance of goal.

prefer_forward_critic.hpp — Prefer Forward Cost

Penalizes negative (reverse) velocities in control sequence:

$$C_{\text{forward}} = w \cdot \sum_{t} \max(0, -v_x(t))$$

Active only when distance to goal > prefer_forward_threshold.

constraint_critic.hpp — Constraint Cost

Encourages robot to stay within kinematic constraints, penalizing velocity limit violations. Supports three motion models:

  • Differential drive: Penalizes non-zero vy
  • Omnidirectional: No additional constraints
  • Ackermann: Penalizes turning radius below minimum

twirling_critic.hpp — Spinning Penalty Cost

Penalizes "low forward + high rotation" behavior to prevent spinning-in-place:

$$C_{\text{twirl}} = w \cdot \sum_{t} \max(0, |w_z(t)| - |v_x(t)|)$$

Automatically disabled within twirling_threshold distance of goal to allow rotational alignment.

velocity_deadband_critic.hpp — Velocity Deadband Cost

Penalizes velocities below deadband thresholds to avoid low-speed instability:

$$C_{\text{deadband}} = w \cdot \sum_{t} \left[\mathbb{1}(|v_x| < \epsilon_{vx}) + \mathbb{1}(|v_y| < \epsilon_{vy}) + \mathbb{1}(|w_z| < \epsilon_{wz})\right]$$

5.4 Core Algorithm Layer

motion_models.hpp — Motion Models

Model Class Omnidirectional Special Constraints
Differential Drive DiffDriveMotionModel No None
Omnidirectional OmniMotionModel Yes None
Ackermann Steering AckermannMotionModel No Minimum turning radius

All models share base class MotionModel acceleration constraint logic:

$$v_{\text{new}} = v_{\text{prev}} + \text{clamp}(u - v_{\text{prev}}, -a_{\max} \Delta t, a_{\max} \Delta t)$$

Ackermann model additional constraint:

$$\text{if } R = \left|\frac{v_x}{\omega_z}\right| < R_{\min}: \quad \omega_z = \text{sign}(\omega_z) \cdot \frac{|v_x|}{R_{\min}}$$

optimizer.hpp — Optimizer Core

Optimizer is the core of MPPI algorithm with execution flow:

evalControl()
  ├── prepare()                    # Update pose, prune path
  ├── [Iteration loop]
  │   ├── generateNoisedTrajectories()  # Sampling + forward simulation
  │   │   ├── setNoisedControls()       # Apply noise
  │   │   ├── applyControlConstraintsBatch()  # Velocity boundary constraints
  │   │   ├── applyConstraints()        # Kinematic constraints (e.g., Ackermann)
  │   │   ├── updateStateVelocities()   # Acceleration constraints
  │   │   ├── integrateStateVelocities() # Multi-threaded trajectory integration
  │   │   └── generateNextNoises()      # Trigger async noise generation
  │   ├── findFurthestReachedPathPoint() # Find furthest reachable path point
  │   ├── evalTrajectoriesScores()      # All Critics scoring
  │   ├── updateControlSequence()       # Softmax weighted update
  │   └── fallback()                    # Collision recovery
  ├── isBlocked()                  # Blocking detection
  ├── savitskyGolayFilter()        # SG filtering
  └── getControlFromSequence()     # Take first control

controller.hpp — Controller Interface

MPPIController class encapsulates optimizer and cost function manager, providing:

  • initialize(): Initialize controller, create all Critics
  • updateStaticObstacles(): Update obstacles + path validity + motion model switching
  • computeVelocityCommands(): Main loop entry point
  • Omni/diff auto-switching logic

5.5 ROS1 Integration Layer

mppi_ros1_node.cpp — ROS1 Main Node

MPPIRos1Node class implements ROS1 system interaction:

  • Subscribed Topics: plan (path), odom (odometry), scan (laser scan)
  • Published Topics: cmd_vel (velocity command), debug_optimal_trajectory (debug trajectory)
  • Control Loop: 50ms timer (20Hz)
  • Parameter Loading: Reads all MPPI parameters from ROS parameter server
  • Startup Assist: tryApplyStartupAssist() — resolves zero-velocity startup issues
  • Coordinate Transform: Converts laser points from robot frame to global frame
  • Exception Handling: Outputs zero velocity and resets controller on MPPI failure
  • Arrival Detection: Stops when within 0.20m of goal point

6. Installation and Compilation

6.1 Prerequisites

  • ROS1 Melodic/Noetic
  • C++17 compatible compiler (GCC ≥ 7)
  • xtensor, xsimd, xtl libraries

6.2 Download Third-party Libraries (Re-download if repository ones are invalid)

Execute in the mppi_laser_example directory:

# Enter package directory
cd src/mppi_laser_example

# Download xtl (xtensor base library, install first)
git clone https://github.com/xtensor-stack/xtl.git
cd xtl && git checkout 0.7.0 && cd ..

# Download xsimd (SIMD acceleration library)
git clone https://github.com/xtensor-stack/xsimd.git
cd xsimd && git checkout 7.4.10 && cd ..

# Download xtensor (main numerical library, version 0.21.0)
git clone https://github.com/xtensor-stack/xtensor.git
cd xtensor && git checkout 0.21.0 && cd ..

# Return to workspace root
cd ../..

All three libraries are header-only — simply reference via include_directories in CMakeLists.txt, no separate compilation/installation required.

6.3 Build Steps

# 1. Enter workspace
cd ~/MPPI_ws_ros1sim

# 2. Build
catkin_make

# 3. Source workspace
source devel/setup.bash

6.4 Dependency Description

ROS1 dependencies declared via package.xml:

  • roscpp, rospy, nav_msgs, geometry_msgs, std_msgs, sensor_msgs, tf

7. Usage

7.1 Start Controller

# Terminal 1: Start MPPI controller
rosrun mppi_laser_example mppi_laser_example_node _params_file:=src/mppi_laser_example/config/mppi_params.yaml

# Or load parameters via rosparam then start
rosparam load src/mppi_laser_example/config/mppi_params.yaml /mppi_ros1_node
rosrun mppi_laser_example mppi_laser_example_node

7.2 Publish Global Path

# Terminal 2: Publish path (supports multiple path types)
rosrun mppi_laser_example mppi_path_publisher.py

Path publisher commands:

  • s — Switch to straight line path (25m)
  • t — Switch to three-sided rectangle path (total 40m)
  • a — Switch to arc-line combination path
  • p — Print path information

7.3 Visualization

# Terminal 3: Visualize in RViz
rviz -d src/mppi_laser_example/config/mppi_test.rviz

7.4 Complete Run Flow

# 1. Build
cd ~/MPPI_ws_ros1sim && catkin_make && source devel/setup.bash

# 2. Start simulation environment (configure according to your robot simulation)
roslaunch your_robot_sim launch_file.launch

# 3. Load MPPI parameters
rosparam load src/mppi_laser_example/config/mppi_params.yaml /mppi_ros1_node

# 4. Start MPPI controller
rosrun mppi_laser_example mppi_laser_example_node

# 5. Publish global path
rosrun mppi_laser_example mppi_path_publisher.py

# 6. Visualize
rviz -d src/mppi_laser_example/config/mppi_test.rviz

# Or follow this project's flow:
# ROS1: MPPI
# 1.
source devel/setup.bash
# Step 1: Load YAML to parameter server
rosparam load $(rospack find mppi_laser_example)/config/mppi_params.yaml
# Step 2: Start node (node will automatically read parameters)
rosrun mppi_laser_example mppi_ros1_node

# 2. Publish path
rosrun mppi_laser_example mppi_path_publisher_fz.py _path_type:=three_side_rectangle _path_points:=300
# or
rosrun mppi_laser_example JZJ_path.py _path_type:=arc_line
# or
rosrun mppi_laser_example clean_path.py

# 3. rviz
rosrun rviz rviz -d /home/shaoyu/MPPI_ws_ros1sim/src/mppi_laser_example/config/mppi_test.rviz

# 4. (Diff/Omni)
roslaunch pioneer_utils bizhang.launch

# Static TF transform or start SLAM node
rosrun tf static_transform_publisher 0 0 0 0 0 0 map odom 100

8. Parameter Configuration

All parameters are configured in config/mppi_params.yaml, each with detailed Chinese comments. Listed by category below:

8.1 MPPI Core Parameters

Parameter Default Description
batch_size 1500 Number of trajectory samples, affects optimization accuracy and computation speed
time_steps 40 Prediction time steps, horizon = time_steps × model_dt
model_dt 0.05 Time step (seconds), controls trajectory prediction temporal resolution
temperature 0.2 Softmax temperature, controls weight distribution sharpness
adaptive_temperature True Enable adaptive temperature
adaptive_temperature_min 0.1 Adaptive temperature lower bound
adaptive_temperature_max 1.0 Adaptive temperature upper bound
gamma 0.015 Control cost weight, balances smoothness and trajectory quality
iteration_count 1 Optimization iterations per cycle
control_period_ms 50 Control period (ms), 50ms = 20Hz
thread_count 4 Parallel computation thread count
prune_distance 3.5 Path pruning distance (meters)

8.2 Control Constraints

Parameter Default Description
vx_max 1.2 Maximum linear velocity (m/s)
vx_min -0.25 Minimum linear velocity (m/s), 0.0 disallows reverse
vy_max 1.2 Maximum lateral velocity (m/s), omnidirectional only
wz_max 2.0 Maximum angular velocity (rad/s)
ax_max 1.5 Maximum linear acceleration (m/s²)
ay_max 1.35 Maximum lateral acceleration (m/s²)
az_max 2.5 Maximum angular acceleration (rad/s²)

8.3 Noise Sampling Standard Deviations

Parameter Default Description
vx_std 0.20 Linear velocity noise std dev (m/s)
vy_std 0.20 Lateral velocity noise std dev (m/s), set 0.0 for diff drive
wz_std 0.30 Angular velocity noise std dev (rad/s)

8.4 Obstacle Cost Parameters

Parameter Default Description
obstacle_repulsion_weight 1.0 Obstacle repulsion weight
obstacle_collision_cost 100000.0 Collision trajectory cost
obstacle_collision_margin 0.3 Collision boundary distance (m)
collision_cost_threshold 50000.0 Collision cost threshold
obstacle_inflation_radius 0.8 Obstacle inflation radius (m)
obstacle_cost_scaling 4.0 Cost scaling factor
obstacle_near_goal_distance 0.3 Distance to ignore obstacles near goal
robot_radius 0.25 Robot radius (m)
consider_footprint false Whether to consider robot footprint
grid_resolution 0.05 Distance field resolution (m/cell)
grid_width 100 Grid map width
grid_height 100 Grid map height

8.5 Blocking Detection Parameters

Parameter Default Description
twirling_weight 0.5 Spinning penalty weight (soft constraint)
twirling_threshold 0.5 Goal distance threshold to disable TwirlingCritic
spinning_ratio_threshold 8.0 wz/vx ratio threshold (hard constraint)
spinning_detect_frames 8 Consecutive detection frames

8.6 Startup Assist Parameters

Parameter Default Description
startup_assist_enabled false Enable startup assist
startup_boost_vx 0.06 Startup linear velocity boost value
startup_boost_wz_gain 1.2 Startup angular velocity gain coefficient
startup_boost_wz_limit 0.6 Startup angular velocity limit
startup_cmd_vx_eps 0.01 Startup command velocity threshold
startup_speed_vx_eps 0.02 Startup actual speed threshold
startup_goal_distance 0.30 Startup goal distance threshold
startup_lookahead 8 Startup lookahead steps

8.7 Path Alignment Cost Parameters

Parameter Default Description
path_align_weight 15.0 Path alignment weight
path_align_offset 4 Steps to offset forward from furthest path point
path_align_threshold 0.3 Distance threshold (m)
path_align_traj_step 1 Trajectory point sampling step
path_align_obstacle_check_radius 0.15 Path alignment obstacle check radius
path_align_max_occupancy_ratio 0.45 Path blocking ratio threshold
path_align_use_orientations false Whether to consider path headings

8.8 Path Angle Cost Parameters

Parameter Default Description
path_angle_weight 5.0 Path angle weight
path_angle_offset 4 Steps to offset forward from furthest path point
path_angle_threshold 0.3 Distance threshold (m)
path_angle_max 0.15 Maximum allowed angle deviation (rad)
path_angle_mode 0 Angle mode: 0=prefer forward, 1=no preference, 2=consider path heading

8.9 Path Following Cost Parameters

Parameter Default Description
path_follow_weight 8.0 Path following weight
path_follow_offset 4 Steps to offset forward from furthest path point
path_follow_threshold 0.4 Distance threshold (m)

8.10 Goal Cost Parameters

Parameter Default Description
goal_weight 5.0 Goal point weight
goal_threshold 0.8 Distance threshold to activate goal cost
goal_angle_weight 3.0 Goal angle weight
goal_angle_threshold 0.6 Distance threshold to activate goal angle cost

8.11 Other Cost Parameters

Parameter Default Description
prefer_forward_weight 3.0 Prefer forward weight
prefer_forward_threshold 0.5 Distance threshold
constraint_weight 3.0 Constraint cost weight
motion_model_type 0 Motion model type: 0=diff, 1=omni, 2=ackermann
velocity_deadband_weight 0.5 Velocity deadband weight
velocity_deadband_vx 0.05 Linear velocity deadband threshold
velocity_deadband_vy 0.05 Lateral velocity deadband threshold
velocity_deadband_wz 0.1 Angular velocity deadband threshold

8.12 SG Filter Parameters

Parameter Default Description
use_sg_filter true Enable SG filter
shift_control_sequence false Enable control sequence temporal smoothing
retry_attempt_limit 2 Fallback retry attempt limit
use_mean_normalization false Use mean normalization

8.13 Motion Model Parameters

Parameter Default Description
motion_model "DiffDrive" Motion model: DiffDrive/Omni/Ackermann
ackermann_min_turning_radius 0.3 Ackermann minimum turning radius (m)

8.14 Omni/Diff Auto-Switching Parameters

Parameter Default Description
enable_omni_switching true Enable omni/diff auto-switching
omni_trigger_obstacle_dist 0.9 Obstacle distance threshold to trigger omni mode
omni_trigger_path_deviation 0.3 Path deviation threshold to trigger omni mode
diff_restore_path_threshold 0.20 Path deviation threshold to restore diff mode
omni_switch_delay_frames 3 Mode switch delay frames

8.15 Diff Mode Configuration Notes

When switching to diff mode, modify these key parameters:

motion_model: "DiffDrive"          # Motion model set to diff
motion_model_type: 0               # Model type in constraint cost
vx_min: -0.0                       # Disallow reverse (or set negative to allow)
vy_std: 0.0                        # Lateral velocity noise set to 0
enable_omni_switching: false       # Disable omni switching (pure diff mode) hack part
consider_footprint: true           # Recommend enabling footprint detection
robot_radius: 0.35                 # Adjust according to actual robot size

9. Topics and Interfaces

9.1 Subscribed Topics

Topic Type Description
/plan nav_msgs/Path Global path to follow
/odom nav_msgs/Odometry Robot pose and velocity
/scan sensor_msgs/LaserScan Laser scan for obstacle detection

9.2 Published Topics

Topic Type Description
/cmd_vel geometry_msgs/Twist Output velocity command
/debug_optimal_trajectory nav_msgs/Path Optimal trajectory (for RViz visualization)

9.3 Core C++ Interface

// Initialize controller
mppi::MPPIController controller;
controller.initialize(settings, "DiffDrive", ackermann_radius);

// Set each Critic parameters
controller.getObstaclesCritic()->setParams(...);
controller.getPathAlignCritic()->setParams(...);
// ...

// Control loop
controller.setPath(path_poses);
controller.updateStaticObstacles(laser_points, robot_pose);
mppi::Twist2D cmd = controller.computeVelocityCommands(robot_pose, robot_speed);

10. Algorithm Details

10.1 Control Loop Flow

1. Receive laser scan → transform to global coordinates
2. Receive odometry → update robot pose and velocity
3. Receive global path → store path points
4. Timer trigger (50ms):
   a. Check goal arrival (< 0.20m)
   b. Update obstacles + path validity + motion model switching
   c. Compute velocity command:
      i.   Prune path (centered at robot, keep points within prune_distance)
      ii.  Iterative optimization:
           - Apply noise to generate K candidate trajectories
           - Multi-threaded forward simulation
           - All Critics scoring
           - Softmax weighted update control sequence
      iii. Collision detection and recovery
      iv.  Blocking detection
      v.   SG filtering
      vi.  Take first control
   d. Startup assist (optional)
   e. Publish velocity command
   f. Publish debug trajectory

10.2 Path Pruning

Nav2-style path pruning making index 0 correspond to robot's current position:

  1. Find nearest path point to robot position
  2. From that point, accumulate distance along path, keep points within prune_distance
  3. Create pruned path

10.3 Furthest Reachable Path Point

Find the nearest point on the path for each sampled trajectory endpoint, take the maximum index as the furthest reachable path point. This index determines the reference point position for PathFollowCritic, PathAlignCritic, and PathAngleCritic.

10.4 Collision Recovery Mechanism

When all trajectories collide:

  1. Set fail_flag = true
  2. Attempt fallback(): reset control sequence, resample
  3. Retry up to retry_attempt_limit times
  4. After exceeding retries, throw exception — ROS1 node catches and outputs zero velocity

10.5 Omni/Diff Auto-Switching

Switch conditions:

Direction Condition Action
Diff→Omni Nearest obstacle < omni_trigger_obstacle_dist OR path deviation > omni_trigger_path_deviation Switch to OmniMotionModel
Omni→Diff Nearest obstacle > omni_trigger_obstacle_dist × 1.2 AND path deviation < diff_restore_path_threshold Restore base motion model

Switch execution:

  1. optimizer_->softReset(): Reset control sequence
  2. Create new motion model
  3. optimizer_->setMotionModel(): Update motion model
  4. Update ConstraintCritic parameters

Delay mechanism: Switch only after omni_switch_delay_frames consecutive frames meet conditions, preventing frequent switching.

10.6 Startup Assist

When robot speed is near zero and no obstacles present, apply initial velocity based on path direction:

$$v_x = v_{\text{boost}}, \quad \omega_z = \text{clamp}(K_p \cdot \Delta\theta, -\omega_{\max}, \omega_{\max})$$

11. Performance Optimization

11.1 Asynchronous Noise Pre-generation

Noise generation uses a separate thread running in parallel with the main control loop:

Timeline:
Main thread:   [use noise A] → [score with noise A] → [use noise B] → [score with noise B] → ...
Noise thread:  [generate noise A] → [generate noise B] → [generate noise C] → ...

While the main thread scores with current noise, the noise thread pre-generates the next batch, reducing main loop wait time.

11.2 Multi-threaded Trajectory Integration

Trajectory integration uses std::thread for parallel computation:

// Distribute batch_size trajectories across thread_count threads
unsigned int batch_per_thread = (batch_size + thread_count - 1) / thread_count;
for (unsigned int t = 0; t < thread_count; ++t) {
    threads.emplace_back(worker, start, end);
}

Each thread independently computes forward simulation for its batch of trajectories, avoiding data races.

11.3 Grid Distance Field O(1) Query

After building the local BFS distance field centered at the robot, trajectory scoring requires only coordinate mapping for direct distance lookup — O(1) time complexity, eliminating brute force search through all obstacle points.

11.4 xt::noalias Optimization

Use xt::noalias in operations like noise addition to skip alias checking, improving xtensor assignment performance:

xt::noalias(state.cvx) = xt::view(control_sequence.vx, xt::newaxis(), xt::all()) + noises_vx_;

11.5 Path Cumulative Distance Caching

PathAlignCritic uses path cumulative distance matching instead of brute force search, enabling early termination via monotonically increasing distance sequences:

for (size_t i = start_idx + 1; i < path_distances.size(); ++i) {
    float diff = std::abs(path_distances[i] - target_distance);
    if (diff < min_diff) { min_diff = diff; closest_idx = i; }
    else if (diff > min_diff) { break; }  // Early termination
}

12. Differences from Nav2 Official MPPI Controller

12.1 Framework Dependencies

Feature This Project Nav2 MPPI
ROS2 Dependencies None Strong (rclcpp, nav2_core, nav2_costmap_2d, etc.)
ROS1 Support Native Not yet supported
Parameter System External setParams ROS2 parameter server + ParametersHandler
Plugin Mechanism Compile-time static registration pluginlib dynamic loading
Lifecycle Management None LifecycleNode state machine
Code Organization Modular multi-file Inline implementation within plugin

12.2 Obstacle Handling

Feature This Project Nav2 MPPI
Obstacle Source Laser point cloud (direct input) Costmap2D global costmap
Distance Query Self-built local grid BFS distance field, can integrate RC-ESDF for zoned collision detection and risk-zone gradient-biased sampling Costmap2D inflation layer + FootprintCollisionChecker
Dynamic Obstacles Built-in linear prediction support None (relies on costmap updates)
Footprint Detection Self-implemented coordinate transform + grid query nav2_costmap_2d::FootprintCollisionChecker

12.3 Numerical Library

Feature This Project Nav2 MPPI
Matrix Operations xtensor Eigen
Random Generation xt::random::randn std::normal_distribution

12.4 Algorithm Enhancements (some hacks)

Additional features built on top of Nav2 MPPI:

Enhancement Description Nav2 Status
Blocking Detection wz/vx ratio with consecutive frame detection, identifies spinning and outputs zero velocity Not present
Omni/Diff Auto-Switching Automatically switches motion models based on obstacle distance and path deviation Manual configuration
Adaptive Temperature Dynamically adjusts softmax temperature based on cost distribution Fixed temperature
Mean Normalization Option Mean normalization as alternative to min normalization Min normalization only
Path Point Validity Check PathFollowCritic can skip path points blocked by obstacles Basic implementation
Path Blocking Detection PathAlignCritic auto-disabled when path heavily blocked Basic implementation
Dynamic Obstacle Prediction ObstaclesCritic supports velocity-based dynamic obstacle position prediction None (relies on costmap updates)
Startup Assist Resolves zero-velocity startup issues / redundant, ineffective Not present
Arbitrary Shape Robot Collision Avoidance Builds RC-ESDF-based zoned collision detection; risk-zone analytical gradients bias MPPI sampling Not present

12.5 Critic Differences

Critic This Project Nav2 MPPI
CostCritic None Present (based on costmap values)
ObstaclesCritic BFS grid distance field + RC-ESDF zoned collision detection + risk-zone gradient-biased sampling + dynamic prediction Costmap inflation layer query
PathAlignCritic Path blocking detection + heading consideration Basic implementation
PathAngleCritic Three angle modes Three angle modes
VelocityDeadbandCritic Present Present
TwirlingCritic Present (includes goal distance deactivation logic) Present

12.6 Interface Differences

Feature This Project Nav2 MPPI
Main Interface computeVelocityCommands(Pose2D, Twist2D) returns Twist2D computeVelocityCommands(PoseStamped, Twist, Path) returns TwistStamped
Path Handling Integrated in Optimizer::prunePath() Handled by separate PathHandler node
Parameter Setting Via setParams() method Via ROS2 parameter server

12.7 Threading Model

Feature This Project Nav2 MPPI
Noise Generation Separate thread + xt::random Separate thread + std::normal_distribution
Trajectory Integration std::thread multi-threaded parallel Eigen vectorization (single-threaded)

12.8 Code Organization Differences

Feature This Project Nav2 MPPI
Responsibility Division Single responsibility per file Multiple classes inline in same file, plugin-based
Maintainability High (modular independent, clear interfaces) Medium (depends on understanding)
Compile Dependencies Only xtensor Eigen + Nav2 full stack

13. License

Copyright © 2025 MPPI ROS1 Project

This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program. If not, see https://www.gnu.org/licenses/.

14. References

14.1 MPPI Algorithm Papers

  • Williams G, Drews P, Goldfain B, Rehg J M, Theodorou E A. Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving[J]. IEEE Transactions on Robotics, 2018, 34(6): 1603-1622.
  • Williams G, Aldrich A, Theodorou E A. Model Predictive Path Integral Control: From Theory to Parallel Computation[J]. Journal of Guidance, Control, and Dynamics, 2017, 40(2): 344-357.

14.2 Nav2 MPPI Controller

14.3 Third-party Libraries

14.4 Related Blogs

About

用于轮/足式机器人的改进MPPI 局部控制器。main（结合RC-ESDF的SA-MPPI：提升动态绕障能力）、MPPI_C++（标准）、 OptiIO-MPPI（魔改避障）、DC-MPPI （Disturbance-Compensated MPPI，扰动补偿型 MPPI：提高湿滑地面以及扰动甲板下的跟踪能力）等

blog.csdn.net/qq_56908984/article/details/160439414

Topics

mppi sa-mppi rc-esdf dc-mppi

Resources

Readme

License

Apache-2.0 license
Activity

Stars

34 stars

Watchers

1 watching

Forks

3 forks
Report repository

Releases

No releases published

Packages

 
 
 

Contributors

Languages