Safety-Critical Control and Planning for Obstacle Avoidance between Polytopes with Control Barrier Functions

TL;DR

This paper introduces a fast, online control and planning method using discrete-time control barrier functions to ensure obstacle avoidance between polytopes, suitable for nonlinear systems and validated through simulations.

Contribution

It presents a novel optimization formulation with dual variables based on DCBFs that reduces computational complexity for real-time obstacle avoidance.

Findings

Successful navigation through maze environments in simulations

Lower computational complexity compared to existing methods

Effective obstacle avoidance for various robot shapes

Abstract

Obstacle avoidance between polytopes is a challenging topic for optimal control and optimization-based trajectory planning problems. Existing work either solves this problem through mixed-integer optimization, relying on simplification of system dynamics, or through model predictive control with dual variables using distance constraints, requiring long horizons for obstacle avoidance. In either case, the solution can only be applied as an offline planning algorithm. In this paper, we exploit the property that a smaller horizon is sufficient for obstacle avoidance by using discrete-time control barrier function (DCBF) constraints and we propose a novel optimization formulation with dual variables based on DCBFs to generate a collision-free dynamically-feasible trajectory. The proposed optimization formulation has lower computational complexity compared to existing work and can be used as a fast online algorithm for control and planning for general nonlinear dynamical systems. We validate our algorithm on different robot shapes using numerical simulations with a kinematic bicycle model, resulting in successful navigation through maze environments with polytopic obstacles.