Vehicle Dynamics Model

The MPC uses a kinematic bicycle model to predict the vehicle’s motion. This model is simpler than a full dynamic model but captures the essential behavior needed for path tracking at moderate speeds.

Bicycle Model

The bicycle model simplifies the four-wheeled vehicle into a two-wheeled “bicycle” by combining the front wheels into a single wheel and the rear wheels into a single wheel.

ψ (heading)
     ↑
     │
┌────┴────┐
│  Front  │
│  Wheel  │  ← δ (steering angle)
└─────────┘
     │
     │ L_f (front wheelbase)
     │
┌────●────┐
│   CG    │ ← (x, y, ψ)
│         │
└─────────┘
     │
     │ L_r (rear wheelbase)
     │
┌─────────┐
│  Rear   │
│  Wheel  │
└─────────┘
     ↓
  v (velocity)

State Vector

The vehicle state consists of 5 variables:

\[\begin{split}\mathbf{x} = \begin{bmatrix} x \\ y \\ \psi \\ v \\ \theta_{prev} \end{bmatrix}\end{split}\]

Where:

  • \(x, y\): Position in the global coordinate frame (meters)

  • \(\psi\): Heading angle (radians, 0 = East, counterclockwise positive)

  • \(v\): Forward velocity (m/s)

  • \(\theta_{prev}\): Previous steering angle (radians)

Note: The previous steering angle is included in the state to enable rate constraints on steering.

Control Vector

The control inputs are:

\[\begin{split}\mathbf{u} = \begin{bmatrix} a \\ \dot{\theta} \end{bmatrix}\end{split}\]

Where:

  • \(a\): Longitudinal acceleration (m/s²)

  • \(\dot{\theta}\): Steering angle rate of change (rad/s)

By controlling steering rate rather than absolute angle, we can enforce smooth steering motions and avoid discontinuous control inputs.

Continuous-Time Dynamics

The kinematic bicycle model equations are:

\[\begin{split}\beta &= \arctan\left(\frac{L_r}{L_f + L_r} \tan(\theta_{prev})\right) \\ \\ \dot{x} &= v \cos(\psi + \beta) \\ \dot{y} &= v \sin(\psi + \beta) \\ \dot{\psi} &= \frac{v}{L_r} \sin(\beta) \\ \dot{v} &= a \\ \dot{\theta}_{prev} &= \dot{\theta}\end{split}\]

Where:

  • \(\beta\): Slip angle at the center of gravity

  • \(L_f\): Distance from CG to front axle (0.79 m)

  • \(L_r\): Distance from CG to rear axle (0.79 m)

Physical Interpretation:

  • The slip angle \(\beta\) accounts for the fact that the vehicle doesn’t instantaneously pivot around its center of gravity

  • The velocity components use \(\psi + \beta\) because the vehicle moves in a direction slightly different from its heading due to the slip angle

  • The heading rate \(\dot{\psi}\) is proportional to velocity and depends on the slip angle

Implementation Details

Discrete-Time Integration

Since the MPC operates in discrete time (with time step \(\Delta t = 0.1\) seconds), we need to convert the continuous dynamics to discrete-time dynamics. This is done using numerical integration.

The implementation uses 4th-order Runge-Kutta (RK4) integration, which provides a good balance between accuracy and computational cost:

# Pseudocode for RK4 integration
def integrate_rk4(x0, u, dt):
    k1 = dt * f(x0, u)
    k2 = dt * f(x0 + k1/2, u)
    k3 = dt * f(x0 + k2/2, u)
    k4 = dt * f(x0 + k3, u)
    x_next = x0 + (k1 + 2*k2 + 2*k3 + k4) / 6
    return x_next

Where \(f(\mathbf{x}, \mathbf{u})\) represents the continuous-time dynamics (right-hand side of the differential equations above).

See: mpc/mpc/mpc_controller.py:228-274 for the full implementation.

Model Accuracy

The kinematic bicycle model is accurate when:

  • Speeds are moderate (< 15 m/s)

  • Lateral acceleration is not extreme

  • Tire slip is minimal

For Formula Electric racing, these assumptions generally hold, especially during early testing and development. For higher speeds and more aggressive maneuvers, a dynamic model (accounting for tire forces, weight transfer, etc.) would be more accurate but significantly more complex.

Vehicle Parameters

The current vehicle parameters are configured in all_settings/all_settings/all_settings.py:

L_F: float = 1.58/2  # 0.79 m
L_R: float = 1.58/2  # 0.79 m

These represent the 2024 vehicle geometry. For different vehicles, these parameters should be measured and updated accordingly.

Jacobians (Linearization)

For some advanced uses (e.g., computing terminal cost matrices via Linear Quadratic Regulator theory), the MPC implementation can compute Jacobians of the dynamics:

\[A = \frac{\partial f}{\partial \mathbf{x}}, \quad B = \frac{\partial f}{\partial \mathbf{u}}\]

These are computed symbolically using CasADi’s automatic differentiation:

A = ca.Function('A', [x0, u0], [ca.jacobian(x1, x0)])
B = ca.Function('B', [x0, u0], [ca.jacobian(x1, u0)])

See: mpc/mpc/mpc_controller.py:256-257

Alternative: Midpoint Integration

The implementation also supports midpoint integration as an alternative to RK4:

# Midpoint method (2nd order)
x_mid = x0 + f(x0, u) * (dt/2)
x_next = x0 + f(x_mid, u) * dt

Midpoint is less accurate than RK4 but requires fewer function evaluations. In practice, RK4 is preferred for its accuracy, and the additional computational cost is negligible.

Validation

The dynamics model can be validated by:

  1. Simulation comparison: Compare MPC predictions against a high-fidelity simulator

  2. Hardware tests: Record vehicle trajectories and compare against model predictions

  3. Stability analysis: Verify controllability of the linearized system (rank of controllability matrix should be 4)

The controllability check is implemented (currently commented out) in mpc/mpc/mpc_controller.py:99-102:

# A = np.array(self.A([0, 0, 0, 10], [0, 0]))
# B = np.array(self.B([0, 0, 0, 10], [0, 0]))
# assert 3.9 < np.linalg.matrix_rank(ct.ctrb(A, B)) < 4.1