Trajectory Optimization Using MPC

This project is a compact C++17 demonstration of Model Predictive Control (MPC) for trajectory optimization. It predicts vehicle motion over a finite horizon, evaluates tracking and obstacle-avoidance costs, and optimizes acceleration and steering commands. The prediction dynamics can be switched between bicycle, Ackermann, and unicycle models without rebuilding. Python scripts are included for plotting and animating the resulting trajectory.

The MPC parameters are defined in config/mpc_configs.yaml and loaded when the demo starts. Configuration-only changes do not require a C++ rebuild. Select the control solver with control_solver. The built-in OSQP is the default solver and is downloaded automatically by CMake. The built-in optimizeControls solver remains available; IPOPT and ACADO retain dispatch points that require their libraries and adapters before use.

Select the prediction model with kinematic_model:

• bicycle uses a centre-of-gravity kinematic bicycle with a slip angle.
• ackermann uses a rear-axle, constant-curvature Ackermann model.
• unicycle uses a unicycle model; steering is interpreted as yaw rate in radians per second for this model.

Each model implements the common VehicleModel interface, so the selected model is used for both MPC cost evaluation and final trajectory rollout.

Prerequisites

• CMake 3.18 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:

python visualization/plot_trajectory.py

The executable writes these files to output/:

• reference_trajectory.csv — desired path
• mpc_trajectory.csv — trajectory generated by the MPC planner

What is MPC?

Model Predictive Control is an optimization-based, receding-horizon control method. At each planning update, MPC predicts how the system will move over a finite horizon and selects the control sequence that minimizes a cost function while respecting control limits.

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

1. Begin with a sequence of acceleration and steering controls over a finite prediction horizon.
2. Roll the controls forward through the configured kinematic model.
3. Score the predicted trajectory using path tracking, heading, velocity, control effort, and obstacle-proximity costs.
4. Use the configured control solver to update acceleration and steering. The built-in optimizer uses central-difference gradients; OSQP solves bounded local quadratic updates.
5. Clamp controls to their permitted limits.
6. Repeat the optimization and roll out the final controls to produce the MPC trajectory.

For a control value u, this implementation estimates the numerical gradient with a central difference:

dJ/du = (J(u + epsilon) - J(u - epsilon)) / (2 * epsilon)

It then performs a gradient-descent update:

u = clamp(u - learning_rate * dJ/du, minimum_control, maximum_control)

MPC configuration in this demo

The default configuration uses:

Parameter	Value	Meaning
Time step	0.1 s	Time between predicted states
Horizon	30 steps	Number of future controls and states
Kinematic model	bicycle	Dynamics used during prediction
Wheelbase	2.7 m	Vehicle-model wheelbase
Optimization iterations	50	Gradient-descent passes per plan
Learning rate	0.01	Size of each control update
Gradient epsilon	0.001	Perturbation used for numerical gradients

Select a solver in config/mpc_configs.yaml:

control_solver: OSQP  # optimizeControls, IPOPT, OSQP, or ACADO

Select a vehicle model in the same file:

kinematic_model: bicycle  # bicycle, ackermann, or unicycle
wheelbase: 2.7            # used by bicycle and ackermann

For bicycle and ackermann, Control::steering is a steering angle in radians. For unicycle, it is a yaw rate in radians per second, and wheelbase is not used.

Acceleration is limited to [-2.0, 2.0] m/s², and steering is limited to [-0.5, 0.5] rad.

Each candidate control sequence 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 contains a simplified MPC implementation. In a full receding-horizon controller, the planner runs repeatedly: apply the first optimized control, measure the new state, shift the control horizon, and optimize again. Production MPC systems also commonly use dedicated solvers such as IPOPT, OSQP, ACADO, or CasADi.

Project structure

.
|-- CMakeLists.txt
|-- README.md
|-- config/
|   `-- mpc_configs.yaml
|-- include/
|   |-- ackermann_model.h
|   |-- bicycle_model.h
|   |-- common_types.h
|   |-- mpc_config.h
|   |-- mpc_planner.h
|   |-- unicycle_model.h
|   `-- vehicle_model.h
|-- src/
|   |-- ackermann_model.cpp
|   |-- bicycle_model.cpp
|   |-- main.cpp
|   |-- mpc_config.cpp
|   |-- mpc_control_solvers.cpp
|   |-- mpc_planner.cpp
|   `-- unicycle_model.cpp
|-- output/
|-- visualization/
|   |-- animate_trajectory.py
|   `-- plot_trajectory.py
`-- run_demo.bat

Customizing the example

• Change the initial state, reference path, obstacle, model, solver, or planner settings in config/mpc_configs.yaml.
• Increase the horizon to plan farther ahead at the cost of more computation.
• Increase the optimization iterations for additional refinement at the cost of a longer planning time.
• Adjust the learning rate carefully. A value that is too large can make the optimization unstable, while a very small value can converge slowly.
• Rebuild after changing C++ code.