Optimization-Based Collision Avoidance

Ayush Agrawal, Debajyoti Chakrabarti, Radha Lahoti

MAE 271D Course Research Project · Spring 2025

Optimal Control Model Predictive Control Collision Avoidance Autonomous Parking Dynamic Obstacles

Presentation Code Reference Paper ← Back to Home

Overview

Collision constraints are typically nonconvex and difficult to include directly in a trajectory optimizer. This project implements an optimization-based collision-avoidance formulation that uses convex duality to express the separation between a full-dimensional vehicle and convex obstacles as smooth nonlinear constraints.

We studied the formulation in both a one-shot optimal control problem (OCP) and a receding-horizon model predictive controller (MPC). The experiments cover reverse and parallel parking, strict collision avoidance, minimum-penetration trajectories, and moving obstacles.

Reverse-parking trajectories comparing strict collision avoidance with a minimum-penetration formulation. — One-shot reverse parking. The collision-free formulation enforces a safety margin, while the softened formulation penalizes unavoidable penetration.

Two Collision-Constraint Formulations

Geometry and finite-horizon control

Each convex obstacle is represented in half-space form. The controlled vehicle is a translated and rotated copy of a fixed base shape, so the optimizer reasons about the complete vehicle footprint rather than a point-mass approximation.

The nominal finite-horizon problem minimizes state and control cost subject to the initial and terminal states, vehicle dynamics, state/input bounds, and geometric separation:

Presentation slide showing the convex obstacle and full-dimensional vehicle sets together with the finite-horizon optimal-control problem and its constraints. — Problem statement from the presentation: convex obstacle and vehicle geometry, dynamics, bounds, terminal condition, and collision-avoidance constraint.

Collision-free and minimum-penetration reformulations

Collision-free trajectories enforce a positive separation between the vehicle and every obstacle throughout the prediction horizon. The dual reformulation preserves the full rectangular vehicle geometry without introducing integer variables.

Minimum-penetration trajectories soften the separation constraint with a nonnegative slack variable and penalize that slack in the objective. This provides a least-intrusive solution when strict avoidance is infeasible in a tight environment.

Presentation comparison of the dual collision-free and minimum-penetration optimization formulations and their cost functions. — The two nonlinear programs from the presentation. Minimum penetration replaces the hard clearance with nonnegative slack and penalizes total intrusion by the weight kappa.

Formulation	Design parameter	Behavior
Collision-free	Minimum separation, d_min	Maintains a hard safety margin
Minimum penetration	Penetration weight, κ	Allows controlled overlap when necessary

A larger penetration weight suppresses obstacle intrusion, but also makes the nonlinear program harder and slower to solve. We additionally penalized changes in acceleration and steering to produce smoother control profiles.

Autonomous Parking

The vehicle is modeled with nonlinear bicycle dynamics and its complete rectangular footprint is included in the optimization. One-shot OCP successfully reached exact terminal poses in both reverse- and parallel-parking scenarios.

Presentation figure showing bicycle-model geometry and the discrete vehicle dynamics used by the optimizer. — Bicycle-model geometry and discrete dynamics used in the OCP and MPC implementations.

Animated one-shot optimal-control trajectory for a vehicle performing reverse parking. — Animated reverse-parking trajectory from the presentation.

Animated one-shot optimal-control trajectory for a vehicle performing parallel parking. — Animated parallel-parking trajectory from the presentation.

Dynamic Obstacles

We extended the original static-obstacle formulation with time-varying polytope constraints. At every prediction step, the obstacle's half-space representation is updated from its prescribed translation and rotation.

Presentation slide defining the time-varying half-space matrices for translating and rotating obstacles. — Time-varying obstacle representation from the presentation. Translation and rotation update the half-space matrices at every timestep.

Animated MPC trajectory avoiding a rectangular obstacle as it translates through the parking entrance. — Translating obstacle. The vehicle times its approach while the obstacle moves vertically through the parking entrance.

Animated MPC trajectory around a rotating rectangular obstacle at the parking entrance. — Rotating obstacle. MPC plans through the entrance while the rectangular obstacle changes orientation.

Key Findings

Minimum-penetration optimization was more robust in tight environments, at the cost of extra variables and computation time.
One-shot OCP with an exact terminal constraint reliably reached the parking goal when a sufficiently long horizon was available.
MPC improved goal tracking without a hard terminal constraint, but recursive feasibility was not guaranteed around moving obstacles.
Dynamic-obstacle performance was highly sensitive to prediction horizon; neither a uniformly longer nor shorter horizon was always better.
Small errors in the estimated obstacle state could destabilize MPC, motivating robust or stochastic extensions.

Key Takeaway

Smooth geometric constraints make it possible to optimize directly over the vehicle's full footprint, while a softened minimum-penetration variant provides a practical fallback for tight or temporarily blocked environments.

Presentation Code Reference Paper ← Back to Home