Iterative Convex Optimization with Control Barrier Functions for Obstacle Avoidance among Polytopes

TL;DR

This paper introduces an iterative convex optimization framework using control barrier functions for obstacle avoidance among polytopic robots and obstacles, enabling fast, real-time, safe trajectory planning in complex environments.

Contribution

It develops a novel iterative convex MPC-DCBF approach with exact polytope distances, improving computational efficiency and applicability to multi-robot and 3D scenarios.

Findings

Achieves millisecond-level solve times in complex maze navigation.

Ensures collision-free trajectories with convex optimization.

Extends to multi-robot and 3D environments.

Abstract

Obstacle avoidance of polytopic obstacles by polytopic robots is a challenging problem in optimization-based control and trajectory planning. Many existing methods rely on smooth geometric approximations, such as hyperspheres or ellipsoids, which allow differentiable distance expressions but distort the true geometry and restrict the feasible set. Other approaches integrate exact polytope distances into nonlinear model predictive control (MPC), resulting in nonconvex programs that limit real-time performance. In this paper, we construct linear discrete-time control barrier function (DCBF) constraints by deriving supporting hyperplanes from exact closest-point computations between convex polytopes. We then propose a novel iterative convex MPC-DCBF framework, where local linearization of system dynamics and robot geometry ensures convexity of the finite-horizon optimization at each iteration. The resulting formulation reduces computational complexity and enables fast online implementation for safety-critical control and trajectory planning of general nonlinear dynamics. The framework extends to multi-robot and three-dimensional environments. Numerical experiments demonstrate collision-free navigation in cluttered maze scenarios with millisecond-level solve times.