Overview

The Local Path Planner generates optimal short-horizon trajectories during the exploration phase (first lap) when the complete track map is not yet available. It processes locally observed cones from SLAM and computes a time-optimal path that the MPC controller can track.

Purpose

During the first lap, the vehicle doesn’t have a complete map of the track. The local path planner enables autonomous navigation by:

  1. Processing partial observations: Works with limited cone visibility from SLAM’s local map

  2. Computing safe trajectories: Generates paths that stay within track boundaries

  3. Enabling exploration: Allows the vehicle to complete the first lap and build a global map

  4. Real-time replanning: Updates the path as new cones are observed

Once the first lap is complete and a global map is available, the global path planner takes over.

System Architecture

Local Map (SLAM)
       ↓
Cone Ordering ────→ Ordered Left/Right Boundaries
       ↓
Time-Optimal ────→ Trajectory (states + controls)
Optimization
       ↓
Path Message ────→ MPC Controller

The local planner operates in a pipeline:

  1. Input: Receives local cone observations from SLAM (typically 10-30 cones ahead)

  2. Cone Ordering: Sorts cones into left and right track boundaries

  3. Trajectory Optimization: Solves for time-optimal path between boundaries

  4. Output: Publishes trajectory for MPC to track

Key Characteristics

Short Horizon

  • Optimizes over N = 10 waypoints (configurable)

  • Lookahead distance: ~10-20 meters depending on track curvature

  • Computation time: ~50-200ms per solve

Time-Optimal Objective

The optimization minimizes total time to traverse the path:

\[\min \sum_{i=1}^N \Delta t_i\]

This encourages maximum velocity through corners while respecting vehicle dynamics and friction limits.

Bicycle Dynamics Model

Uses the same kinematic bicycle model as the MPC controller:

  • State: [x, y, ψ, v, θ]

  • Controls: [a, θ̇] (acceleration, steering rate)

  • Constraints: Velocity, acceleration, steering angle, and friction circle limits

Track Representation

The track is represented as N pairs of left/right boundary points. The optimization variable t determines where along each pair the vehicle should drive:

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

Where t ∈ [0.45, 0.55] (constrained to 0.5 at start and end). This allows the optimizer to find the racing line between the boundaries.

Integration with SLAM

The local path planner subscribes to the SLAM local map, which typically contains:

  • Range: Cones within ~15-20 meters of the vehicle

  • Update rate: Varies based on SLAM solve frequency (~1-10 Hz)

  • Cone colors: Blue (left boundary), yellow (right boundary)

When the SLAM module publishes a global map (after completing the first lap), the local path planner automatically shuts down to allow the global planner to take over.

Robustness Features

Minimum Cone Requirements

The planner requires a minimum number of cones to operate:

  • At least 2 left cones AND 2 right cones

  • OR at least 1 left cone and 2+ right cones

  • OR at least 1 right cone and 2+ left cones

If insufficient cones are available, the planner waits for more observations.

Solver Failure Handling

If the optimization fails (e.g., due to infeasible cone configuration):

  • The planner tracks consecutive failures

  • Allows up to 2 failures before stopping output

  • Previous solution is reused until a new valid path is found

Angle Wrapping

Special handling for heading angles near ±π to avoid discontinuities:

\[\text{wrapped\_error} = \sin(\psi / 2)\]

This ensures smooth optimization even when the vehicle crosses the ±π boundary.

Output Format

The planner publishes a FebPath message containing:

States (at each waypoint):

  • x, y: Position (meters)

  • psi: Heading angle (radians)

  • v: Velocity (m/s)

  • th: Steering angle (radians)

Controls (between waypoints):

  • a: Acceleration (m/s²)

  • thdot: Steering rate (rad/s)

The path is interpolated to a constant time grid (10ms intervals) to match the MPC control frequency.

Typical Performance

Computation Time

  • Cone ordering: 5-20ms

  • Optimization solve: 50-150ms

  • Path interpolation: 5-10ms

  • Total: 60-180ms per update

Path Quality

  • Smooth trajectories that respect vehicle dynamics

  • Maximum speed through corners (friction-limited)

  • Lap times typically within 10-15% of optimal (global planner)

Update Frequency

  • Triggered by SLAM local map updates (~1-5 Hz)

  • Not time-critical since path is relatively stable

  • MPC extrapolates if path update is delayed

Limitations

Short Horizon

The limited lookahead means:

  • May not plan optimally for distant corners

  • Doesn’t consider global track topology

  • Can be conservative compared to global planner

Cone Ordering Challenges

The distance-based ordering algorithm can fail on:

  • Tracks with very wide sections (ambiguous pairing)

  • Sharp hairpin turns (ordering discontinuities)

  • Missing cone observations (gaps in boundaries)

Note: An alternative cone ordering algorithm is under development to handle edge cases more robustly, but is not yet fully validated.

No Obstacle Avoidance

Currently, the planner does not explicitly avoid cones in the optimization (collision constraints are disabled). The track boundary representation implicitly provides clearance, but tight situations may require manual tuning of the boundary interpolation bounds.

Transition to Global Planner

When SLAM publishes the global map (signaling first lap completion):

  1. Local planner publishes /path/finished message

  2. Local planner node shuts down

  3. Global planner activates and computes optimal racing line

  4. MPC switches to tracking global path

This handoff is seamless from the MPC perspective since both planners output the same FebPath message format.