Trajectory Optimization

The local path planner solves a time-optimal trajectory optimization problem to find the fastest path between the ordered left and right track boundaries. This page describes the mathematical formulation and implementation.

Optimization Problem

Objective: Minimize total time to traverse the path

\[\min_{t, \psi, v, \theta, a, \dot{\theta}, \Delta t} \sum_{i=1}^N \Delta t_i\]

Subject to:

Vehicle dynamics constraints
Track boundary constraints
Velocity limits
Acceleration limits
Steering angle limits
Steering rate limits
Friction circle constraint

Decision Variables

For each waypoint i = 1, …, N:

Variable	Description	Bounds
\(t_i\)	Track position parameter (interpolation between left/right)	[0.45, 0.55]
\(\psi_i\)	Heading angle	unrestricted
\(v_i\)	Velocity	[V_MIN, V_MAX]
\(\theta_i\)	Steering angle	[-DF_MAX, DF_MAX]
\(a_i\)	Acceleration	[ACC_MIN, ACC_MAX]
\(\dot{\theta}_i\)	Steering rate	[-DF_DOT_MAX, DF_DOT_MAX]
\(\Delta t_i\)	Time step duration	[0, 2.0] seconds

Total: 7 variables × N waypoints

Track Position Parameter

The variable \(t_i \in [0.45, 0.55]\) determines where the vehicle drives between the left and right boundaries:

\[\mathbf{p}_i = (1 - t_i) \cdot \mathbf{left}_i + t_i \cdot \mathbf{right}_i\]

\(t_i = 0.45\): Closer to left boundary (20% from center toward left)
\(t_i = 0.50\): Centerline (exact middle)
\(t_i = 0.55\): Closer to right boundary (20% from center toward right)

The parameter is constrained near 0.5 to prevent the vehicle from driving too close to the cones. The exact value at each waypoint is optimized to find the fastest racing line.

Boundary conditions:

\(t_1 = 0.5\) (start on centerline)
\(t_N = 0.5\) (end on centerline)

This ensures smooth entry and exit from the planned section.

Vehicle Dynamics Constraints

The optimization enforces that the predicted states follow the bicycle dynamics model:

\[\mathbf{x}_{i+1} = F(\mathbf{x}_i, \mathbf{u}_i, \Delta t_i)\]

Where:

\(\mathbf{x}_i = [x_i, y_i, \psi_i, v_i, \theta_i]\) is the state at waypoint i
\(\mathbf{u}_i = [a_i, \dot{\theta}_i]\) is the control input
\(F(\cdot)\) is the discrete-time dynamics function (RK4 integration)

See: Vehicle Dynamics Model for details on the bicycle model.

The dynamics function is computed using a custom integrator with 3 substeps for accuracy:

self.dynamics = discrete_custom_integrator(n=3, **car_params)

Angle Wrapping

To handle heading angles near ±π, the dynamics error uses a wrapped difference:

\[\text{error}_\psi = \sin(\psi_i / 2)\]

This prevents discontinuities when the heading crosses the ±π boundary.

See: local_path/local_path/local_opt_compiled.py:142-160

Velocity Constraints

Simple box constraints on velocity:

\[V_{\text{MIN}} \leq v_i \leq V_{\text{MAX}}\]

Default: [0.0, 25.0] m/s

The optimizer will naturally maximize velocity where possible to minimize time.

Acceleration Constraints

Minimum (Braking)

\[a_i \geq A_{\text{MIN}}\]

Default: -3.0 m/s² (moderate braking)

Maximum (Throttle)

The maximum acceleration can be velocity-dependent (to model motor torque curves):

\[a_i \leq A_{\text{MAX}}(v_i)\]

Default: 2.0 m/s² (constant)

For more realistic motor modeling, this can be a function: ACC_MAX_FN = Function('acc_max', [v], [torque(v) / mass])

Steering Constraints

Steering Angle

\[-\text{DF\_MAX} \leq \theta_i \leq \text{DF\_MAX}\]

Default: ±0.5 rad (±28.6°)

Steering Rate

\[-\text{DF\_DOT\_MAX} \leq \dot{\theta}_i \leq \text{DF\_DOT\_MAX}\]

Default: ±0.5 rad/s

This limits how quickly the steering can change, improving stability.

Friction Circle Constraint

The most important constraint for cornering performance is the friction circle, which limits combined longitudinal and lateral acceleration:

\[\sqrt{a_{\text{long}}^2 + a_{\text{lat}}^2} \leq \mu \cdot g\]

Where:

\(a_{\text{long}} = a_i\) (longitudinal acceleration from control input)
\(a_{\text{lat}} = \frac{v_i^2}{L_r} \sin(\beta_i)\) (lateral/centripetal acceleration)
\(\beta_i = \arctan\left(\frac{L_r}{L_f + L_r} \tan(\theta_i)\right)\) (slip angle)
\(\mu \cdot g\) is the maximum friction (FRIC_MAX parameter)

Implementation:

\[a_i^2 + a_{\text{centripetal}}^2 \leq \text{FRIC\_MAX}^2\]

Default: FRIC_MAX = 12.0 m/s² (~1.2g)

This constraint prevents the optimizer from planning trajectories that would exceed tire grip limits.

See: local_path/local_path/local_opt_compiled.py:266-273

Initial Condition Constraints

The optimization must respect the current vehicle state:

Velocity Tolerance

\[|v_1 - v_{\text{current}}| \leq 0.2 \text{ m/s}\]

Allows small deviation to account for state estimation noise.

Heading Tolerance

\[|\psi_1 - \psi_{\text{current}}| \leq \frac{\pi}{16} \text{ rad} \approx 11.25°\]

Steering Angle

\[\theta_1 = \theta_{\text{current}}\]

Must match exactly (no tolerance) to ensure smooth handoff to MPC.

See: local_path/local_path/local_opt_compiled.py:228-257

Terminal Constraints

To ensure the path can be safely extended:

Final Velocity

\[0.5 \leq v_N \leq 1.0 \text{ m/s}\]

Prevents planning paths that end at zero velocity (would require infinite time to restart).

Final Control

\[a_N = 0, \quad \dot{\theta}_N = 0\]

Zero control inputs at the end for smooth continuation.

See: local_path/local_path/local_opt_compiled.py:235-245

Solver Configuration

The optimization uses IPOPT (Interior Point OPTimizer) with MA57 linear solver:

DEFAULT_SOPTS = {
    'ipopt': {
        'ipopt.linear_solver': 'ma57',
        'expand': True,
        'honor_original_bounds': True,
        'acceptable_tol': 1e-2,
        'acceptable_obj_change_tol': 1e-3,
    }
}

Key settings:

expand=True: Expands MX expressions to SX for faster evaluation
acceptable_tol=1e-2: Relaxed tolerance for faster convergence
acceptable_obj_change_tol=1e-3: Stop when objective changes by <0.001s

Warm-Starting

The solver uses the previous solution to initialize each new solve. If no previous solution exists, an initial guess is constructed:

# Initial guess for first solve
x0 = [
    t_i = 0.5 for all i,              # Start on centerline
    psi_i = cumulative angles from cones,
    v_i = min(current_velocity, 0.1),  # Conservative initial speed
    theta_i = current_steering for all i,
    a_i = 0 for all i,
    thdot_i = 0 for all i,
    dt_i = 0.1s for all i
]

The heading angles are initialized from the geometric direction between consecutive centerline points.

See: local_path/local_path/local_opt_compiled.py:426-436

Solver Performance

Typical solve times on modern hardware:

Warm-started: 50-100ms
Cold start: 100-200ms
Iteration count: 10-50 iterations

The solver tracks status and can detect failures:

if solver.stats()['return_status'] == 'Invalid_Number_Detected':
    # Cone ordering produced overlapping boundaries
    raise AssertionError('cone ordering failed!')

if not solver.stats()['success']:
    # Optimization failed to converge
    raise AssertionError("Solver failed to converge")

Path Interpolation

The optimization produces N waypoints with non-uniform time spacing \(\Delta t_i\). For MPC compatibility, this is converted to a uniform time grid:

def to_constant_tgrid(dt, z, u, t):
    # dt = 0.01s (100 Hz for MPC)
    # t = cumulative times [0, t1, t1+t2, ...]

    current_time = 0
    states = []
    controls = []

    while current_time < t[-1]:
        # Find which segment we're in
        idx = find_segment(current_time, t)

        # Integrate from segment start to current_time
        extra_time = current_time - t[idx]
        state = dynamics(z[idx], u[idx], extra_time)
        states.append(state)
        controls.append(u[idx])

        current_time += dt

    return states, controls

This produces a path at constant 100 Hz that the MPC can directly use.

The path is also padded to ensure sufficient horizon:

# Pad until at least 2× MPC horizon
while len(states) * dt < MPC_N * MPC_DT * 2:
    # Continue straight with zero acceleration
    states.append(integrate(states[-1], [0, u_last[1]], dt))
    controls.append([0, u_last[1]])

See: local_path/local_path/local_opt_compiled.py:365-397

Debugging

Constraint Violation

If the solver fails, check which constraints are violated:

# Access constraint table
self.gtable = {
    'dynamics': (0, 45),      # Indices 0-44
    'vel': (45, 55),          # Indices 45-54
    'acc_min': (55, 65),      # etc.
    ...
}

# Check specific constraint
lam_g = solver_result['lam_g']  # Lagrange multipliers
dynamics_multipliers = lam_g[0:45]

# Large multipliers indicate active/violated constraints

Visualization

The local planner publishes the optimized path for visualization:

Topics:

/path/local/viz (PointCloud): Path waypoints as 3D points
/path/local/vizpath (Path): Path with full pose information

These can be displayed in RViz to inspect the planned trajectory.