Duality-based Convex Optimization for Real-time Obstacle Avoidance between Polytopes with Control Barrier Functions

TL;DR

The paper tackles real-time obstacle avoidance between polytopes for multiple robots with nonlinear, control-affine dynamics by introducing a duality-based safety-critical control framework that leverages nonsmooth control barrier functions (NCBFs). By deriving the dual of the minimum-distance QP, it obtains a conservative, computable lower bound on the distance dynamics $\dot{h}$ and enforces safety via a QP that optimizes the control input $u$ while preserving the NCBF constraint. The authors prove that the dual variables are continuous and right-differentiable, enabling reliable real-time enforcement even when primal solutions may be non-smooth; safety is guaranteed under Filippov dynamics. The framework is validated on a moving L-shaped sofa navigating a tight corridor, demonstrating real-time performance and non-conservative maneuvers, and the authors discuss trade-offs between polytope fidelity and computation time, as well as potential extensions to other convex shapes and robustness guarantees.

Abstract

Developing controllers for obstacle avoidance between polytopes is a challenging and necessary problem for navigation in tight spaces. Traditional approaches can only formulate the obstacle avoidance problem as an offline optimization problem. To address these challenges, we propose a duality-based safety-critical optimal control using nonsmooth control barrier functions for obstacle avoidance between polytopes, which can be solved in real-time with a QP-based optimization problem. A dual optimization problem is introduced to represent the minimum distance between polytopes and the Lagrangian function for the dual form is applied to construct a control barrier function. We validate the obstacle avoidance with the proposed dual formulation for L-shaped (sofa-shaped) controlled robot in a corridor environment. We demonstrate real-time tight obstacle avoidance with non-conservative maneuvers on a moving sofa (piano) problem with nonlinear dynamics.

Figures (4)

• Figure 1: At any two configurations, the minimum distance between the robots $i$ and $j$ is the same as the minimum distance between the affine spaces Aff$^i$ and Aff$^j$, illustrated in blue. These spaces are affine extensions of some two faces of the robots. The points on robot $i$ and $j$ with the least distance are $z^{*i}$ and $z^{*j}$ respectively, and $s^*$ represents the vector with the smallest norm. The dynamics of the minimum distance between the robots is a hybrid system, with the discrete states being the pair of faces of the robots generating these affine spaces. For each discrete state, the minimum distance varies smoothly as the distance between the two affine spaces.
• Figure 2: Square of minimum distance (NCBF) between the arms of the sofa and the walls, where Wall $W^1$ (left figure) is the left wall, Wall $W^2$ (right figure) the upper one, and Wall $W^3$ the inner wall of the corridor. Since the minimum distance is always greater than zero, the L-shaped sofa never collides with the obstacles over its trajectory. Both plots have log-scale on the y-axis.
• Figure 3: NCBF constraint enforcement between Arm 1 and Wall 1 (top figure) and Arm 1 and Wall 3 (bottom figure). Illustration of safety performance of the system. The red lines are $\dot{h}(t)$, the yellow lines are the lower bounds of $\dot{h}(t)$, and blue line is the RHS of the NCBF constraint. $\bar{\alpha}^{1j}(t) = -\alpha(h^{1j}(t)-\epsilon_1^2)$, where $j$ is the wall. The system safety is guaranteed with the red line being always above the blue line.
• Figure 4: Dual optimal solutions for Arm 1 (left figure) and Arm 2 (right figure) of the robot corresponding to the obstacle wall 3. The dual optimal solutions are unique, continuous, and right-differentiable as shown in Sec. \ref{['sec:dual-formulation']}. Note that pair of dual variables for Arm 1-Wall 3 are different from that of Arm 2-Wall 3, i.e. the vector $\lambda^{W^3}(t)$ is different for Arms 1 and 2. The subscripts represent the components of the vectors, and only the non-zero components have been plotted.

Theorems & Definitions (16)

• Lemma 1
• Remark 1
• Lemma 2
• proof
• Lemma 3
• proof
• Lemma 4
• proof
• Lemma 5
• proof