Model Predictive Control

The Model Predictive Control (MPC) module computes optimal control commands (acceleration and steering) to follow a reference trajectory while respecting vehicle constraints. The MPC predicts future vehicle behavior over a receding horizon and solves a constrained optimization problem at each time step to minimize tracking error.

Overview

MPC is an advanced control technique that:

1. Predicts future vehicle states over a finite horizon using a dynamics model

2. Optimizes control inputs to minimize trajectory tracking error

3. Enforces physical constraints (speed limits, steering limits, acceleration limits)

4. Applies the first control input and repeats the process at the next time step

This approach is particularly well-suited for autonomous racing because it can explicitly handle vehicle limits while maximizing performance.

Key Features

• Real-time capable: Achieves 1-5ms solve times with C code generation

• Nonlinear dynamics: Uses bicycle kinematic model with numerical integration

• Hard constraints: Enforces velocity, acceleration, and steering rate limits

• Warm-starting: Leverages previous solutions for faster convergence

• Structure-exploiting solver: Uses FATROP for efficient optimization

• Flexible tuning: Adjustable cost weights for different driving styles

System Integration

The MPC controller integrates with the autonomy stack as follows:

Inputs:

• Vehicle state from SLAM module (position, heading, velocity)

• Reference trajectory from global or local path planner

• Previous control inputs for warm-starting

Outputs:

• Acceleration commands (m/s²)

• Steering angle rate commands (rad/s)

• Predicted trajectory for visualization

Control Loop:

• Runs at 100 Hz (10ms period)

• MPC horizon: 10 time steps × 100ms = 1 second lookahead

• Typical lookahead distance: ~10m at 10 m/s

MPC Documentation

• Overview
  • What is Model Predictive Control?
  • System Architecture
  • Control Flow
  • Key Features
  • Integration with System
  • Performance Characteristics
• Vehicle Dynamics Model
  • Bicycle Model
  • State Vector
  • Control Vector
  • Continuous-Time Dynamics
  • Implementation Details
  • Vehicle Parameters
  • Jacobians (Linearization)
  • Alternative: Midpoint Integration
  • Validation
• Optimization Problem
  • Problem Formulation
  • Cost Function Components
  • Constraints
  • Solver Configuration
  • Warm-Starting
  • Performance Optimizations
  • Typical Solve Times
  • Debugging and Diagnostics
• Configuration and Tuning
  • Settings Structure
  • Prediction Horizon Parameters
  • Vehicle Parameters
  • Velocity Constraints
  • Acceleration Constraints
  • Steering Constraints
  • Cost Function Weights
  • Solver Configuration
  • Runtime Configuration
  • Example Configurations
  • Performance Optimization
  • ROS Topics

Published Topics

Topic	Type	Description
/cmd	ackermann_msgs/AckermannDriveStamped	Control commands (acceleration, steering)
/control/throttle	std_msgs/Float64	Acceleration command only
/control/steer	std_msgs/Float64	Steering angle command only
/mpc/viz	geometry_msgs/PoseArray	Predicted and reference trajectories for visualization

Subscribed Topics

Topic	Type	Description
/slam/state	feb_msgs/State	Current vehicle state (x, y, heading, velocity)
/path/global	feb_msgs/FebPath	Reference trajectory from global planner (after first lap)
/path/local	feb_msgs/FebPath	Reference trajectory from local planner (exploration lap)
/ground_truth/wheel_speeds	eufs_msgs/WheelSpeedsStamped	Current steering angle feedback
/end_race	std_msgs/Bool	Emergency stop signal (triggers maximum braking)

Performance Characteristics

Typical Operating Conditions

• Prediction horizon: 1 second (10 steps × 100ms)

• Control frequency: 100 Hz

• Solve time: 1-5 ms with compiled solver, 10-20 ms without

• Tracking accuracy: <0.1m RMS lateral error on smooth paths

• Maximum speed: 10 m/s (configurable up to 15+ m/s)

Computational Requirements

• CPU: ~5-10% of one core at 100 Hz (with compiled solver)

• Memory: ~50 MB (including Python runtime and dependencies)

• Dependencies: CasADi, NumPy, SciPy, FATROP or IPOPT

API Reference

class mpc.mpc_controller.MPCPathFollower(N, DT, L_F, L_R, V_MIN, V_MAX, A_MIN, A_MAX, A_DOT_MIN, A_DOT_MAX, DF_MIN, DF_MAX, DF_DOT_MIN, DF_DOT_MAX, Q, R, RUNTIME_FREQUENCY, ivpsolver, **kwargs)

Bases: object

construct_solver(generate_c=False, compile_c=False, use_c=False, gcc_opt_flag='-Ofast')

handles codegeneration and loading.

Parameters

• generate_c (bool, optional) – whether or not to generate new C code. Defaults to False.

• compile_c (bool, optional) – whether or not to look for and compile the C code. Defaults to False.

• use_c (bool, optional) – whether or not to load the compiled C code. Defaults to False.

• gcc_opt_flag (str, optional) – optimization flags to pass to GCC. can be -O1, -O2, -O3, or -Ofast depending on how long you’re willing to wait. Defaults to ‘-Ofast’.

solve(x0, u_prev, trajectory, P=None)

crunch the numbers; solve the problem.

Parameters

• x0 – length 4 vector of the current car state.

• u_prev – length 2 vector of the previous control command

• trajectory – shape (6, N) trajectory of target states and controls.

• P (optional) – terminal cost matrix. If not passed, use default from 10 m/s fwd driving.

Returns

control result. [[acc], [theta]]

Return type

array of shape (2, 1)

make_dynamics(n, method='rk4')

construct functions for the dynamics of the car. uses bicycle model.

Parameters

• n (int) – number of steps the integrator should take.

• method (str, optional) – integration method to use. one of (‘midpoint’, ‘rk4’). Defaults to ‘rk4’ for a 4th order explicit runge-kutta method

Returns

discrete dynamics w/ augmented state, continuous dynamics, jac(f, x), jac(f, u). All casadi.Function objects.

Return type

(F, f, A, B)

mpc.main.makepose(x)

class mpc.main.MPC(*args, **kwargs)

Bases: rclpy.node.Node

end_race_callback(msg)

steer_callback(msg)

Return Steering Angle from steering angle sensor on car

Parameters

msg (eufs_msgs.msg.WheelSpeedsStamped) –

acc_callback(msg)

Set curr_acc to value recieved from

Parameters

msg (std_msgs.msg.Float64) –

global_path_callback(msg)

Input: msg.PathState -> List of State (from State.msg) vectors Returns: List of numpy state vectors (len 4: x, y, velocity, heading) Description: Takes in a local or global path depending on what lap we’re in and converts to np.array of state vectors

Parameters

msg (feb_msgs.msg.FebPath) –

update_term_cost_mats()

local_path_callback(msg)

Input: msg.PathState -> List of State (from State.msg) vectors Returns: List of numpy state vectors (len 4: x, y, velocity, heading) Description: Takes in a local or global path depending on what lap we’re in and converts to np.array of state vectors

Parameters

msg (feb_msgs.msg.FebPath) –

state_callback(carstate)

Input: Float64[4] Description: Converts State msg to numpy vector,

updates variables initalized in __init__ (e.g. self.z_ref, self.z_curr, etc.) with get_update_dict and self.update, and solves mpc problem -> stores result in self.prev_soln

Parameters

carstate (feb_msgs.msg.State) –

run_control()

mpc.main.main(args=None)