Autonomous Systems & Planning

Path-Planning and Collision Avoidance of Ground Vehicles

Developed nonlinear and linearized MPC controllers in Julia for constrained vehicle racing and collision avoidance, including a dynamic-bicycle demonstration with six stationary obstacles.

Key Result In a 25 s closed-loop simulation, nonlinear MPC with a six-state dynamic bicycle model completed 2.29 laps past six stationary obstacles, with all 250 MPC updates accepted and no physical contact.

Overview

This project develops a Julia framework for constrained trajectory planning and collision avoidance of ground vehicles with model predictive control (MPC). It combines nonlinear and successively linearized optimization, kinematic and nonlinear dynamic bicycle models, a piecewise-curvature track representation, and stationary-obstacle avoidance. The optimal control problems are built with JuMP and solved with Ipopt or OSQP, and every controller is evaluated in reproducible closed-loop simulation on a multi-curvature circuit, with independent validation of each plan and measured computation time.

The original CORE Lab project (Summer 2023 – Summer 2024) produced the Julia vehicle model and the linear and nonlinear MPC formulations with collision avoidance. The open-source implementation, VehicleMPC.jl, was expanded afterwards; the dynamic-bicycle controller, the multi-obstacle formulation and the results on this page come from that later work.

Multi-obstacle collision avoidance

Closed-loop nonlinear MPC past six stationary obstacles. The overlay is drawn from the recorded run: Ipopt status and plan acceptance for every cycle, MPC computation time, speed, steering, lap progress, and current and minimum obstacle clearance. The vehicle body is illustrative; the collision proxy is a 5 cm disk.

The vehicle is a six-state nonlinear dynamic bicycle model with lateral tyre forces, controlled by nonlinear MPC solved with JuMP and Ipopt at every 0.1 s control step. It races the original 34.85 m multi-curvature circuit past six stationary obstacles placed in a left-hand and a right-hand 1 m bend, a 2 m bend and on three straights; two of them lie 1.8 m apart on opposite sides of the same straight, so both fall inside a single prediction horizon.

The obstacles are part of the optimization problem, not decoration: every obstacle within reach of the horizon is imposed simultaneously as a separation constraint, and the controller chooses steering and acceleration to satisfy all of them while maximizing progress. The trajectory in the video is not a prerecorded path. It is the result of 250 consecutive MPC solves, each applied to an independently integrated plant and checked for track departure and obstacle contact before it is accepted.

Selected results

6

stationary obstacles, constrained simultaneously

250/250

MPC updates accepted

2.29

laps in 25 s of simulated time

0

physical contacts; minimum clearance 1.94 cm

Six-obstacle dynamic-bicycle run, horizon N = 10, 0.1 s control step. These results come from an offline closed-loop simulation; computation time is measured and discussed below, not guaranteed.

Nonlinear MPC racing on a multi-curvature circuit

Full-track racing with the same controller and vehicle, without obstacles. The overlay shows the recorded vehicle state, solver status and MPC computation time of every cycle.

Without obstacles, the same controller races the full circuit from the same initial condition. It maximizes forward progress subject to steering and acceleration limits, while the dynamic model accounts for lateral velocity and yaw rate. The circuit alternates straights with tight turns of both signs of curvature, including a left-hand 1 m semicircle followed immediately by a right-hand one. As in the obstacle run, the trajectory is generated by repeated MPC solves on a simulated plant, and the animation shows the actual state, controller status and computation time of each cycle.

Problem formulation

At each control step the controller measures the vehicle state, solves a finite-horizon optimal control problem, applies the first input, and repeats:

min_{u0, …, uN−1}  J(x₀, U)
subject to  x_k+1 = f_d(x_k, u_k),   x_k ∈ 𝒳,   u_k ∈ 𝒰,
track-corridor and obstacle-separation constraints,  x₀ = measured state.

The objective is a progress-maximizing surrogate for minimum time: it rewards the arc length gained over the horizon and penalizes deviation from the centreline, heading error, input magnitude and input changes. All terms are normalized so that the weights are dimensionless:

J = −w_s (s_N − s₀) / (v_max N Δt) + Σ_k [ q_y (e_y,k / e_y,max)² + q_ψ (e_ψ,k / e_ψ,max)² ]
+ Σ_k [ r_δ (δ_k / δ_max)² + r_a (a_k / a_ref)² + r_Δδ ((δ_k − δ_k−1) / δ_max)² + r_Δa ((a_k − a_k−1) / a_ref)² ]

Showcase weights: w_s = 10, q_y = q_ψ = 0.1, r_δ = r_a = 0.01, r_Δδ = 1, r_Δa = 0.1. Because progress is measured only at the end of a fixed horizon, nothing guarantees that the closed loop is time-optimal.

For the dynamic controller the constraints are: the lateral offset stays within the corridor (|e_y| ≤ 0.30 m on the 0.70 m road), the heading error within 60°, speed within 0.8–5 m/s, steering within ±25°, acceleration within ±5 m/s², tyre slip angles within 0.12 rad so the linear tyre model stays valid, and lateral acceleration within 4 m/s². Speed, slip and lateral-acceleration limits are imposed at every integration sub-step, not only at the knots.

Vehicle dynamics and track representation

Kinematic bicycle (four states). Referenced to the rear axle and written in track coordinates, the state is progress s, lateral offset e_y, heading error e_ψ and speed v; the inputs are steering δ and acceleration a. With wheelbase L and track curvature κ(s):

ṡ = v cos e_ψ / (1 − κ e_y),    ė_y = v sin e_ψ,    ė_ψ = v tan δ / L − κ ṡ,    v̇ = a

This model is cheap to optimize and is used for the controller comparisons below. The nonlinear MPC uses its exact zero-order-hold discretization in Cartesian coordinates.

Dynamic bicycle (six states). Referenced to the centre of mass, the state is z = [X, Y, ψ, v_x, v_y, r], with body-frame velocities and yaw rate, and the same inputs. Lateral and yaw dynamics come from linear axle tyre forces:

Ẋ = v_x cos ψ − v_y sin ψ,    Ẏ = v_x sin ψ + v_y cos ψ,    ψ̇ = r
v̇_x = a − F_yf sin δ / m + r v_y,    v̇_y = (F_yf cos δ + F_yr) / m − r v_x,    ṙ = (l_f F_yf cos δ − l_r F_yr) / I_z
F_yf = C_f [δ − atan((v_y + l_f r) / v_x)],    F_yr = −C_r atan((v_y − l_r r) / v_x)

The two models use different reference points (rear axle and centre of mass, related by p_CoM = p_rear + l_r(cos ψ, sin ψ)), and both showcases start from the same physical pose. The parameters describe a small-scale vehicle (1.98 kg, 0.25 m wheelbase) with synthetic, uncalibrated tyre stiffness. The model is only defined for v_x > 0, so the controller keeps v_x ≥ 0.8 m/s, a bound chosen to keep the fast lateral and yaw modes within the stability limit of the RK4 integrator.

Track geometry. The circuit is a sequence of eleven constant-curvature segments: straights of 2–6 m, an S-bend formed by a left and a right semicircle of 1 m radius, and four 2 m quarter turns, for a 34.85 m lap on a 0.70 m road. Vehicle states are expressed in Frenet coordinates (progress, lateral offset and heading error relative to the centreline) and converted back to Cartesian poses with exact segment-local formulas. This representation makes the corridor constraint simple, but it introduces three numerical difficulties: curvature jumps at segment boundaries, a narrow corridor in the tight bends, and progress that keeps increasing across repeated lap crossings. The nonlinear controllers handle them by assigning each prediction knot to a segment, re-checking that assignment after every solve, and carrying the lap offset in the assigned segment, so the start/finish line needs no special treatment.

Nonlinear and linearized MPC

Nonlinear MPC

JuMP + Ipopt

• Nonlinear vehicle dynamics, direct multiple shooting with RK4
• Kinematic (four-state) and dynamic (six-state) variants
• Nonconvex obstacle-separation constraints, several obstacles at once in the dynamic variant
• Warm starts from the shifted previous plan, with alternative initial guesses when needed
• Every plan re-simulated and validated before it is applied

Successively linearized MPC

JuMP + OSQP

• Kinematic model only: local linearization of the exact discrete-time map, z_k+1 = A_kz_k + B_ku_k + c_k
• Convex quadratic programs with affine track-coordinate approximations
• Supporting-halfspace obstacle constraints, softened with penalized slacks
• Trust region and successive re-linearization within a cycle
• Mandatory nonlinear rollout of each candidate plan

The linearized controller was faster in every recorded benchmark, but its convex approximations cost progress, and a cycle may need several QP iterations before a candidate passes validation. The nonlinear controller predicts with the exact model and found plans with better objective values, at a higher cost per solve. The dynamic six-state controller and the six-obstacle demonstration are nonlinear MPC with Ipopt only; the linearized controller has no dynamic-model counterpart.

Collision-avoidance constraints

Each obstacle i is a circle with centre o_i and radius r_i, and the vehicle is approximated by a disk of radius r_ego at its centre of mass. The nonlinear controller imposes

‖p_k − o_i‖₂² ≥ D_i²,    D_i = r_ego + r_i + d_b,i

In the showcase r_ego = 5 cm, r_i = 10 cm and the planning buffer d_b = 3 cm, so D = 18 cm, while physical contact occurs at 15 cm.

The squared form is smooth and needs no choice of passing side inside the constraint. All six obstacles have their own constraint slot in the same nonlinear program; a slot is active whenever its obstacle lies within the distance the horizon could cover, so nothing is rebuilt as obstacles come into range. The constraints are imposed at every prediction knot and at every RK4 sub-step position.

Because Ipopt is a local solver, the initial guess determines which side of an obstacle the vehicle passes. For newly relevant obstacles the controller generates candidate guesses that pass each of the first two obstacles on either side, rolls them out with the dynamic model, and ranks them by feasibility and clearance before solving. In the linearized controller the circle is replaced by a supporting halfspace, n^⊤(p − o) ≥ D + ε with n pointing from the obstacle to the nominal position, which is a conservative convex inner approximation of the collision-free set.

The disks are simplified collision proxies, not a calibrated vehicle footprint.

Numerical implementation

• Accurate discretization. The dynamic NLP integrates with RK4 (four sub-steps per 0.1 s interval), giving each sub-step and RK4 stage its own variables. This keeps the expression graph linear in the horizon; nesting the stages symbolically made a solve about 12× slower in measurement. The kinematic controllers use an exact zero-order-hold map with a series-evaluated sinc function that stays smooth at zero curvature.
• Persistent optimization models. Models are built once. The measured state, previous input, segment geometry and obstacle data are JuMP parameters updated each cycle, and the linearized controller updates its OSQP coefficients in place instead of rebuilding the QP.
• Automatic differentiation. Jacobians for linearization come from ForwardDiff and are tested against central differences.
• Curvature transitions. Knots are assigned to track segments, reassigned from the Cartesian solution when a solve moves them across a boundary, and bounded to their segment if the reassignment would cycle, a failure found and fixed during the final experiments.
• Initialization and warm starts. Shifted warm starts, cold pure-pursuit guesses and side-sequence guesses for obstacles, with the work per cycle bounded by count (at most five initialization attempts and four reassignments) rather than by wall-clock time, so outcomes do not depend on machine speed.
• Independent validation. Before a plan is applied, its inputs are re-simulated with a 1 ms RK4 integrator, checked against the plan, the real track geometry, the operating limits and a dense 50-sample-per-interval obstacle check. A rejected plan is never applied; the simulation stops instead.
• End-to-end timing. Compilation is timed separately, runs are warmed up, and every cycle's complete computation time is recorded, including projection, initial guesses, solves and validation.

Validation and simulation results

Each run is evaluated on the independently integrated plant trajectory: dynamics consistency between plan and rollout, track-boundary clearance, input and handling limits, obstacle separation sampled between constrained points, the closed-loop outcome, and the measured computation time of every cycle.

Recorded dynamic-bicycle showcase runs (25 s simulated, N = 10, 0.1 s step)

Measurement	Full-track racing	Six-obstacle racing
Completed laps	2.56	2.29
Accepted MPC updates	250/250	250/250
Median complete MPC cycle	87.5 ms	131.6 ms
95th-percentile cycle	175.9 ms	268.7 ms
Cycles within the 100 ms period	78.0%	7.2%
Minimum track clearance	4.77 cm	4.77 cm
Physical obstacle contacts	–	0
Minimum clearance above contact	–	1.94 cm

Both runs completed without a rejected plan, track departure or plant violation. In the obstacle run the trajectory never touched an obstacle, but between constrained sub-step points it entered the 3 cm planning buffer in 14 sampled positions, by at most 1.06 cm; contact itself is rejected by the validation.

Computation time is the main caveat. The median six-obstacle cycle in the recorded run exceeds the 0.1 s control period, and the simulation does not model computation delay: each control is applied at the start of its period regardless of how long the solve took. These are offline closed-loop simulations with measured computation time, not a demonstration of real-time control. Timing also varied between sessions on the same laptop: in three repeated benchmark runs of the same code the six-obstacle median was 62 ms, with 81–83% of cycles within 0.1 s. The trajectories, laps and clearances were identical in every repetition.

Controller comparison and computational performance

A separate benchmark compares the kinematic nonlinear and linearized controllers under matched conditions (same vehicle, track, start states, horizon N = 10, period and weights; one session on one laptop). These are kinematic-model results and are not directly comparable with the dynamic-bicycle runs above.

Ten stationary-obstacle scenarios on the circuit, 250 cycles each (kinematic model)

Measurement	Linearized MPC (OSQP)	Nonlinear MPC (Ipopt)
Completed runs	30/30	9/10
Progress	2.22–2.32 laps	2.46–2.58 laps
Median cycle time	4.7–5.4 ms	9.9–10.8 ms
Minimum clearance above contact	2.6 cm	2.6 cm

The linearized controller completed all thirty runs (three per scenario) with about half the median computation time. The nonlinear controller made 7–10% more progress in every scenario it completed; its one incomplete run stopped when an 80 ms time budget skipped a segment-assignment correction. The comparison illustrates the trade-off between model fidelity and cost: exact nonlinear predictions give better plans, while convex approximations are faster but need trust regions, softened constraints and nonlinear validation to remain safe. The six-state dynamic model raises the cost further, to roughly an order of magnitude above the kinematic nonlinear controller per cycle.

Reproducibility and code

The implementation is the VehicleMPC.jl repository. Every showcase run solves the MPC afresh; nothing is loaded from an earlier run, and the video is rendered only from the data the run exports. With Julia 1.10 or newer (and Python 3.10 or newer for rendering), the two demonstrations are reproduced from the repository root with:

julia --project=. -e 'using Pkg; Pkg.instantiate()'
python -m pip install -r examples/showcase/requirements.txt

julia --project=. examples/showcase/race.jl --model=dynamic --video
julia --project=. examples/showcase/multiple_obstacles.jl --model=dynamic --video

The repository also contains the automated test suite, the benchmark scripts and their recorded reports, and the mathematical documentation of each controller.

Limitations and future directions

• All results are from simulation. The plant and the prediction model are the same equations with different integrators, so there is no model mismatch, noise or state estimation, and the vehicle parameters are synthetic.
• Vehicle and obstacles are simplified disks. Collision constraints hold at the constrained points and are checked densely in between, but there is no continuous-time safety certificate.
• Ipopt returns local solutions and OSQP solves local convex approximations: trajectories are not globally optimal, and there is no recursive-feasibility guarantee or fallback controller.
• There is no hard real-time guarantee; some dynamic-controller cycles exceed the simulated control period.

Natural next steps follow from these limits: modelling computation delay and plant–model mismatch, a low-speed or blended model for standing starts, multi-obstacle support in the kinematic and linearized controllers, and moving obstacles.

Skills

• Julia
• JuMP
• Ipopt
• OSQP
• Nonlinear MPC
• Vehicle dynamics
• Collision avoidance
• Numerical optimization
• Object-oriented programming