Control Barrier Functions for Collision Avoidance Between Strongly Convex Regions

TL;DR

This work addresses real-time collision avoidance between robots and obstacles with state-dependent convex safe regions by formulating collision constraints as control barrier functions (CBFs). The authors introduce strongly convex maps for safe regions, ensuring that the minimum distance h^{ij} is continuously differentiable and its gradient can be computed from KKT solutions of the distance problem, which can be propagated along trajectories via a KKT-solution ODE. They prove that the minimum-distance derivative exists and provide a CBF-QP with a sparsity structure that guarantees strong safety under minimal assumptions, even when the CBF-QP solution is not unique or Lipschitz. Simulation on a quadrotor navigating through obstacle-laden corridors demonstrates real-time (up to 500–1000 Hz) enforcement of CBF constraints without overapproximations, validating the approach for state-dependent convex sets and enabling robust, deadlock-free collision avoidance in complex environments.

Abstract

In this paper, we focus on non-conservative collision avoidance between robots and obstacles with control affine dynamics and convex shapes. System safety is defined using the minimum distance between the safe regions associated with robots and obstacles. However, collision avoidance using the minimum distance as a control barrier function (CBF) can pose challenges because the minimum distance is implicitly defined by an optimization problem and thus nonsmooth in general. We identify a class of state-dependent convex sets, defined as strongly convex maps, for which the minimum distance is continuously differentiable, and the distance derivative can be computed using KKT solutions of the minimum distance problem. In particular, our formulation allows for ellipsoid-polytope collision avoidance and convex set algebraic operations on strongly convex maps. We show that the KKT solutions for strongly convex maps can be rapidly and accurately updated along state trajectories using a KKT solution ODE. Lastly, we propose a QP incorporating the CBF constraints and prove strong safety under minimal assumptions on the QP structure. We validate our approach in simulation on a quadrotor system navigating through an obstacle-filled corridor and demonstrate that CBF constraints can be enforced in real time for state-dependent convex sets without overapproximations.

Figures (5)

• Figure 1: Examples of safe regions for a quadrotor system with state $x^q = (p^q, R^q, v^q) \in SE(3) \times \mathbb{R}^3$. In \ref{['subfig:quadrotor-uncertainty-safe-set', 'subfig:quadrotor-corridor-safe-set']}, the set $\mathcal{C}^{q1}(x^q)$ represents the shape of the quadrotor at the state $x^q$. \ref{['subfig:quadrotor-uncertainty-safe-set']} depicts a safe region that considers the position uncertainty (represented by $\mathcal{C}^{q2}(x^q)$) of the quadrotor. The robust safe region $\mathcal{C}^{q}(x^q)$ is constructed using the Minkowski sum of the quadrotor shape $\mathcal{C}^{q1}(x^q)$ and uncertainty set $\mathcal{C}^{q2}(x^q)$. \ref{['subfig:quadrotor-corridor-safe-set']} depicts a safe region that expands the quadrotor shape set along a braking corridor $\mathcal{C}^{q3}(x^q)$ (see \ref{['subsec:example-cbf-obstacle avoidance']}). The braking distance vector is given by $\xi(v^q, R^q)$ and parameterized by the velocity and orientation of the quadrotor. The dynamic safe region $\mathcal{C}^{q}(x^q)$ is constructed using the Minkowski sum of the quadrotor shape $\mathcal{C}^{q1}(x^q)$ and braking corridor $\mathcal{C}^{q3}(x^q)$. The safe regions for both examples can be represented by strongly convex maps (see \ref{['def:strongly-convex-set']}) and are used for the results in \ref{['sec:results']}.
• Figure 1: A flowchart of the important results in the paper; the results in \ref{['sec:ncbfs-for-strictly-convex-sets']} are the main contributions. \ref{['sec:problem-description-outline-main-results']} defines smooth convex and strongly convex maps and states the problem considered in the paper. \ref{['sec:background']} introduces Filippov solutions, nonsmooth control barrier functions (NCBFs), and strong safety for the closed-loop system. \ref{['sec:minimum-distance-between-strongly-convex-maps-smoothness-properties']} identifies the smoothness properties of the KKT solution of the minimum distance problem. Finally, \ref{['sec:ncbfs-for-strictly-convex-sets']} states the CBF-QP and proves the strong safety property.
• Figure 1: Verification of the KKT solution ODE, \ref{['thm:distance-ode']}, and the derivative of the minimum distance, \ref{['thm:minimum-distance-derivative']}. The simulation environment, \ref{['subfig:example-kkt-ode-env']}, consists of a static polytope and a quadrotor system with a given reference trajectory. The safe region of the quadrotor comprises the quadrotor shape and a state-dependent position uncertainty set, as depicted in \ref{['subfig:quadrotor-uncertainty-safe-set']}. The initial KKT solution is found by solving the minimum distance problem \ref{['eq:strict-convex-min-dist']}, and subsequently by integration of the KKT solution ODE, \ref{['thm:distance-ode']}. The solution obtained via the KKT solution ODE is compared to the actual KKT solution (obtained by solving \ref{['eq:strict-convex-min-dist']} at each timestep) in \ref{['subfig:example-kkt-ode-kkt', 'subfig:example-kkt-ode-dist']}. The minimum distance derivative from \ref{['thm:minimum-distance-derivative']} is compared to the actual derivative of the minimum distance (obtained via finite difference method) in \ref{['subfig:example-kkt-ode-Ddist']}.
• Figure 2: Minimum distance problem with its corresponding separating vector and sets of constraints. For a given state $x$, the figure illustrates the separating vector $s^*=z^{i*}-z^{j*}$ in green. Under \ref{['assum:strongly-convex-pair']} and when $h(x) > 0$, there is a unique optimal solution $z^* = (z^{i*}, z^{j*})$, and the gradients of the constraints, $\nabla_{z^i} A^i_1$ at $z^{i*}$ and $\nabla_{z^j} A^j_1$ and $\nabla_{z^j} A^j_2$ at $z^{j*}$ are shown. The KKT condition \ref{['subeq:strict-convex-kkt-gradient']} indicates that a conic combination of $\nabla_{z^j} A^j_k$ must be equal to $s^*=z^{i*}-z^{j*}$ (the dual variables are the coefficients). From the figure, we can see that $s^*$ lies in the cone generated by $\nabla_{z^j} A^j_1$ and $\nabla_{z^j} A^j_2$ (and similarly for $i$), and that $\lambda^{*j}_2 = \lambda^{*i}_2 = 0$. Thus the index sets (active set $\mathcal{J}_0$ and the strictly active set $\mathcal{J}_1$) at the state $x$ are $\mathcal{J}^i_0 = \mathcal{J}^i_1 = \{1\}, \mathcal{J}^j_0 = \{1,2\}$, and $\mathcal{J}^j_1 = \{1\}$. The combined index sets can be written as: $\mathcal{J}_0 = \{(i,1), (j,1), (j,2)\}$, $\mathcal{J}_1 = \{(i,1), (j,1)\}$, and $\mathcal{J}_2 = \{(j,2)\}$.
• Figure 2: Verification of the collision avoidance result \ref{['thm:cbf-qp']} for a quadrotor system navigating through an obstacle-filled corridor. The simulation environment, \ref{['subfig:example-cbf-env']}, consists of a quadrotor, $7$ polytopic obstacles, and $4$ walls. The control task is to navigate through the corridor while avoiding obstacles. The CBF-QP \ref{['thm:cbf-qp']} is used to guarantee the safety of the quadrotor system. In \ref{['subfig:example-cbf-dist', 'subfig:example-cbf-rel-dist']}, the colored regions show the range of a quantity across all $11$ collision pairs, while the solid lines show the mean values. Note that the $y$-axes of all the plots are in the log scale. Since, by \ref{['subfig:example-cbf-dist']}, the minimum distance across all collision pairs is greater than safety margin $\epsilon_\text{dist}$, the quadrotor system safely navigates through the corridor.

Theorems & Definitions (31)

• Definition 2.1: Smooth convex map
• Definition 2.2: Strongly convex map
• Definition 3.1: Semi-continuity
• Definition 3.2: Caratheodory solution
• Definition 3.3: Filippov solution
• Proposition 3.4: Existence of Filippov solution
• Remark 3.5: Definition of the safe set
• Definition 3.6: Nonsmooth control barrier function and strong safety
• Lemma 3.7: CBF constraint
• Lemma 4.1: Uniqueness of optimal solution and Lipschitz continuity of $h$