Trajectory Optimization Using MPPI

This project is a compact C++17 demonstration of Model Predictive Path Integral (MPPI) trajectory optimization. It samples possible acceleration and steering sequences, predicts the resulting vehicle motion, scores each trajectory, and combines the best samples into an optimized trajectory. Python scripts are included for plotting and animating the result.

All tunable numeric parameters are stored in config/mppi_configs.yaml. Changes to that file take effect the next time the demo runs; no C++ rebuild is required.

Prerequisites

• CMake 3.16 or newer
• A C++17 compiler
  • Windows: Visual Studio Build Tools with the C++ workload
  • Linux: GCC or Clang
  • macOS: Apple Clang/Xcode Command Line Tools
• Python 3 (for visualization)
• Python packages: pandas and matplotlib

Install the visualization dependencies with:

python -m pip install pandas matplotlib

Build and run

Run all commands from the project root (the directory containing CMakeLists.txt).

Windows

Configure and build the project:

cmake -S . -B build
cmake --build build --config Debug

Run the demo:

.\build\Debug\trajectory_optimization_demo.exe

Animate the generated trajectory:

python .\visualization\animate_trajectory.py

After the initial CMake configuration, run_demo.bat can build, run, and launch the animation in one step:

.\run_demo.bat

Linux and macOS

Configure, build, and run:

cmake -S . -B build
cmake --build build
./build/trajectory_optimization_demo
python visualization/animate_trajectory.py

To display a static plot instead of the animation, run:

cd visualization
python plot_trajectory.py
cd ..

The executable writes these files to output/:

• reference_trajectory.csv - desired path
• mppi_trajectory.csv - trajectory selected by the MPPI planner

What is MPPI?

MPPI is a sampling-based, receding-horizon control method. Unlike an optimizer that requires derivatives of the dynamics or cost function, MPPI explores many possible control sequences by simulation. This makes it useful for nonlinear vehicle models and costs that may be difficult to differentiate.

At a high level, one planning update works as follows:

1. Begin with a nominal sequence of acceleration and steering controls over a finite prediction horizon.
2. Generate many candidate sequences by adding Gaussian noise to the nominal controls.
3. Roll each candidate forward through the vehicle model.
4. Assign a cost to every rollout based on path tracking, heading, velocity, control effort, and obstacle proximity.
5. Convert the costs into importance weights, giving lower-cost rollouts more influence.
6. Compute an updated control sequence from the weighted samples.
7. Roll out the updated controls to produce the optimized trajectory.

The importance weight used by this implementation is proportional to:

weight_k = exp(-(cost_k - minimum_cost) / lambda)

Subtracting the minimum cost improves numerical stability. The temperature lambda controls how strongly the update favors the lowest-cost samples: a smaller value is more selective, while a larger value spreads influence across more samples.

MPPI configuration in this demo

The default configuration uses:

Parameter	Value	Meaning
Time step	0.1 s	Time between predicted states
Horizon	60 steps	Number of future controls and states
Samples	1000	Candidate control sequences per iteration
Wheelbase	2.7 m	Vehicle-model wheelbase
Optimization iterations	3	Sampling and update passes per plan

Candidate controls use Gaussian exploration noise with standard deviations of 0.7 m/s^2 for acceleration and 0.25 rad for steering. Acceleration is limited to [-2.0, 2.0] m/s^2, and steering is limited to [-0.5, 0.5] rad.

Each rollout is scored using:

• Position tracking error
• Heading tracking error
• Velocity tracking error
• Acceleration and steering effort
• A strong penalty for entering an obstacle safety region

This repository is simplified MPPI implementation. In a full receding-horizon controller, the planner would run repeatedly: apply the first optimized control, measure the new state, shift the control horizon, and optimize again.

Project structure

.
|-- CMakeLists.txt
|-- config/
|   `-- mppi_configs.yaml
|-- include/
|   |-- common_types.h
|   |-- mppi_planner.h
|   `-- vehicle_model.h
|-- src/
|   |-- main.cpp
|   |-- mppi_planner.cpp
|   `-- vehicle_model.cpp
|-- output/
|-- visualization/
|   |-- animate_trajectory.py
|   `-- plot_trajectory.py
`-- run_demo.bat

Customizing the example

• Change the initial state, reference path, obstacle, sampling settings, actuator limits, cost weights, or temperature in config/mppi_configs.yaml.
• Increase the number of samples for broader exploration at the cost of additional computation.
• Increase the horizon to plan farther ahead at the cost of longer rollouts.

Rebuild only after changing C++ code. Configuration-only changes do not require a rebuild.