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.
Introduction
In safety-critical robotics, enforcing collision avoidance is paramount. Traditional methods often rely on Control Barrier Functions (CBFs) to maintain safety. However, calculating exact distances between complex polytopic shapes is computationally expensive and typically leads to non-convex optimization problems. To maintain real-time performance, researchers often resort to oversimplifying robot and obstacle geometries using spheres or ellipsoids. While this simplifies the mathematics, it introduces significant conservatism, often preventing robots from navigating narrow passages where the true geometry would permit passage.
Alternatively, using exact geometry within non-convex Model Predictive Control (MPC) frameworks can lead to slow solve times and local minima issues. This paper introduces a middle ground: achieving the geometric precision of polytopes with the speed and reliability of convex optimization.
Methodology: The iMPC-DHOCBF Framework
The core contribution is the iMPC-DHOCBF framework (Iterative Model Predictive Control with Discrete-time High-Order Control Barrier Functions). The approach hinges on two main technical pillars:
- Supporting Hyperplanes: Rather than relying on complex, differentiable distance functions, the algorithm identifies the closest points between the robot and an obstacle. It then constructs a supporting hyperplane between these points. This plane serves as a linear safety constraint; as long as the robot remains on its designated side of the plane, safety is guaranteed.
- Iterative Convexification: Because robot dynamics and rotations are inherently non-linear, the framework linearizes these elements around a nominal trajectory. By solving a sequence of convex optimization problems (Quadratic Programs) and updating the trajectory iteratively, the solution converges to a safe, feasible path.
For multi-robot scenarios, the framework employs a priority-based sequential planning scheme. This allows each robot to treat the predicted paths of higher-priority robots as moving obstacles, avoiding the exponential complexity growth associated with joint state-space planning.
Experimental Evaluation
The framework was evaluated in several challenging simulation environments, including 2D mazes with various robot morphologies and 3D cluttered environments requiring coordinated rotation and translation.
Key performance metrics include:
- Computational Efficiency: Solve times in 2D environments were approximately 5ms. In complex 3D scenarios involving L-shaped robots and long prediction horizons, solve times remained near 28ms, well within the requirements for real-time control.
- Geometric Precision: The method successfully navigated narrow passages where clearance was minimal, demonstrating a significant advantage over spherical approximations.
- Versatility: The framework handled convex (rectangular, triangular) and non-convex (L-shaped) robots with equal efficacy by modeling non-convex shapes as unions of convex polytopes.
Discussion and Future Work
The "iMPC-DHOCBF" framework demonstrates that the separation between polytopes can be expressed as simple linear inequalities, transforming a non-convex challenge into a series of efficient Quadratic Programs. By integrating this into an MPC framework, the system gains "look-ahead" capability, allowing robots to adjust their orientation in anticipation of upcoming obstacles.
Future research will focus on enhancing robustness against sensor noise and model uncertainty. While the current framework assumes perfect information, real-world deployment will require formal safety guarantees that account for geometric and state estimation errors.
Reference: Liu, S., Huang, Z., & Belta, C. A. (2026). Iterative Convex Optimization with Control Barrier Functions for Obstacle Avoidance among Polytopes. arXiv:2603.05916v1 [cs.RO].
Summary updated