MPCGPU Math

MPCGPU Math

Mathematical background for the MPCGPU solver.

Direct Trajectory Optimization

MPCGPU uses direct trajectory optimization to solve nonlinear optimal control problems, computing a robot's path through an environment as a series of states and controls:

• States: X = {x₀, ..., xₙ} where x ∈ ℝⁿ
• Controls: U = {u₀, ..., uₙ₋₁} where u ∈ ℝᵐ

Discrete-Time Dynamics

The robot is modeled as a discrete-time dynamical system:

xₖ₊₁ = f(xₖ, uₖ, h)
x₀ = xₛ

where:

• h is the timestep
• xₛ is the starting state
• f(·) represents the system dynamics

Cost Function

The optimization minimizes an additive cost function:

J(X, U) = ℓf(xₙ) + Σₖ₌₀ᴺ⁻¹ ℓ(xₖ, uₖ)

where:

• ℓf(·) is the terminal cost
• ℓ(·) is the running cost

Optimization Process

The solver follows a three-step process:

1. Quadratic Approximation: Compute second-order Taylor expansion around nominal trajectory
2. KKT System Solution: Solve for optimal trajectory update using PCG
3. Line Search: Find step size that ensures descent on original nonlinear problem

Quadratic Program

The Taylor expansion results in:

min_{δX,δU} ½δxₙᵀQₙδxₙ + qₙᵀδxₙ + Σₖ₌₀ᴺ⁻¹ (½δxₖᵀQδxₖ + qᵀδxₖ + ½δuₖᵀRδuₖ + rᵀδuₖ)
s.t. δx₀ = xₛ - x₀
     δxₖ₊₁ - Aₖδxₖ - Bₖδuₖ = 0  ∀k ∈ [0,N)

KKT System

A Karush-Kuhn-Tucker (KKT) system represents the necessary conditions for optimality in the constrained optimization problem, which takes the form:

[G  Cᵀ] [-δZ] = [g]
[C  0 ] [ λ ] = [c]

where:

• G is the Hessian matrix of the cost function
• C is the constraint Jacobian matrix
• δZ contains the state and control updates
• λ represents the Lagrange multipliers
• g and c are the gradients and constraint residuals

This system is solved using the Schur complement method and PCG.

Preconditioned Conjugate Gradient

MPCGPU uses a custom PCG implementation optimized for block-tridiagonal systems, comprised of these four steps:

1. Initialize residual and search direction
2. Iteratively update solution using matrix-vector products
3. Apply preconditioning to improve convergence
4. Check convergence criteria