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

This paper proposes a novel iterative convex MPC-DCBF framework that leverages supporting hyperplanes derived from exact polytope closest-point computations to enable fast, real-time, collision-free navigation for polytopic robots in complex environments without relying on geometric approximations.

The Gist

Imagine you are trying to guide a very clumsy, non-spherical robot (like an L-shaped table or a triangle) through a crowded room filled with sharp, angular furniture. Your goal is to get the robot from the door to the exit without bumping into anything, all while moving as efficiently as possible.

This is the problem the paper solves. Here is the breakdown of their solution using simple analogies:

The Problem: The "Smoothie" vs. The "Real Thing"

Most existing robot navigation systems try to make life easier by pretending the robot and the obstacles are smooth balls or eggs (spheres or ellipsoids).

• The Analogy: Imagine trying to fit a square peg into a round hole. If you pretend the square peg is a circle, you might think it fits, but in reality, the corners will crash into the wall.
• The Issue: Pretending everything is round makes the math easy, but it's inaccurate. It forces the robot to take huge detours to stay safe, or worse, it might think it's safe when it's actually about to crash.
• The Alternative: Some methods try to use the exact, jagged shapes of the obstacles. But the math for jagged shapes is so messy and complicated (like trying to solve a puzzle with 1,000 pieces) that the robot's computer freezes, and it can't react fast enough to avoid a crash.

The Solution: "Iterative Convex Optimization"

The authors propose a clever middle ground. They don't pretend the shapes are round, and they don't try to solve the impossible jagged puzzle all at once. Instead, they use a strategy called Iterative Convex Optimization.

Here is how it works, step-by-step:

1. The "Flashlight" Strategy (Closest Points)

Instead of looking at the whole complex shape of the robot and the wall, the robot shines a "flashlight" to find the single closest point between itself and the nearest obstacle.

• Analogy: Imagine you are walking through a dark forest with a flashlight. You don't need to see the whole tree; you just need to see the branch closest to your nose.

2. The "Magic Wall" (Supporting Hyperplanes)

Once the robot finds that closest point, it draws an invisible, flat wall (a plane) right through that point, perpendicular to the line connecting the robot and the obstacle.

• Analogy: Think of this like a security guard standing between you and a wall. The guard says, "You can't cross this line." Because the robot and the obstacle are both convex (bulging outward), this single flat wall is enough to guarantee they won't touch, even if the shapes are weird.
• Why it's cool: A flat wall is mathematically simple (linear). It turns a scary, jagged problem into a simple "don't cross the line" rule.

3. The "Rehearsal" Loop (Iterative Process)

The robot doesn't just draw one line and hope for the best. It runs a quick "rehearsal" in its brain:

1. Guess: "If I move there, where will I be?"
2. Check: "Okay, at that new spot, where is the closest point to the wall? Let's draw a new 'Magic Wall' there."
3. Solve: "Now, find the best path that respects all these new flat walls."
4. Repeat: It does this over and over, very quickly (in milliseconds), refining the path until it's perfect.

Because every step of this rehearsal uses simple flat walls, the math stays convex. In math-speak, "convex" means the problem has a single, clear "bottom of the bowl" to find, so the computer never gets lost or stuck.

The Results: Fast, Safe, and 3D

• Speed: Because the math is simplified into flat walls, the robot can solve these problems in milliseconds. It's fast enough to react in real-time, like a human dodging a ball.
• Complex Shapes: It works perfectly for weird shapes like L-shaped robots or triangles, not just round balls.
• Teams: It can even handle a whole team of robots. They take turns planning their paths (like a relay race), treating each other as moving obstacles, without crashing into one another.
• 3D: It works in 3D space, allowing robots to navigate complex mazes with walls and ceilings, not just on a flat floor.

The Bottom Line

The authors built a system that lets robots see the world exactly as it is (jagged and complex) but solve the navigation problem as if it were simple (flat and linear). By constantly updating their "flat walls" based on where they are, they achieve the safety of a perfect map with the speed of a simple calculation. It's like having a GPS that instantly redraws the road rules every time you take a step, ensuring you never hit a wall, no matter how twisty the road gets.

Technical Summary

Here is a detailed technical summary of the paper "Iterative Convex Optimization with Control Barrier Functions for Obstacle Avoidance among Polytopes."

1. Problem Statement

The paper addresses the challenge of safe trajectory planning and control for robots with polytopic geometries (including non-convex shapes like L-shapes) navigating in environments filled with polytopic obstacles.

• Core Difficulty: Existing methods face a trade-off between geometric accuracy and computational tractability:
  • Smooth Approximations (Spheres/Ellipsoids): Allow for differentiable distance functions and convex optimization but distort the true geometry, restricting the feasible set and potentially causing collisions in tight spaces.
  • Exact Polytope Methods: Preserve geometric exactness but often result in non-convex optimization problems (e.g., using Nonlinear MPC or nonsmooth CBFs), which are computationally expensive and difficult to solve in real-time, especially for high-dimensional or multi-robot systems.
• Goal: Develop a framework that maintains geometric exactness (handling arbitrary convex and non-convex polytopes) while ensuring the underlying optimization problem remains convex to enable real-time, millisecond-level performance for nonlinear dynamics.

2. Methodology

The authors propose an Iterative Convex Model Predictive Control (MPC) framework integrated with Discrete-time High-Order Control Barrier Functions (DHOCBFs). The core innovation lies in linearizing the non-convex collision constraints iteratively.

A. Geometric Representation

• Both the robot $R(x)$ and obstacles $O_{io}$ are modeled as convex polytopes defined by linear inequalities ($Ax \leq b$).
• Non-convex robots are represented as unions of convex polytopes.
• Safety is defined as the non-intersection of the robot and obstacle sets: $R(x_t) \cap O = \emptyset$.

B. Iterative Linearization Strategy

The method operates in a receding horizon fashion with an inner iterative loop at each time step $t$:

1. Nominal Trajectory Initialization: Start with a nominal trajectory from the previous time step (or a safe initial guess).
2. Closest-Point Computation: For the current nominal state, solve a small Quadratic Program (QP) to find the exact closest points between the robot polytope and each active obstacle polytope.
3. Supporting Hyperplane Construction: Using the vector connecting the closest points, construct a supporting hyperplane that separates the robot and the obstacle. This hyperplane serves as a linear approximation of the collision boundary.
4. Linearization of Dynamics and Geometry:
   • System dynamics are linearized around the nominal trajectory.
   • The robot's geometry matrices ($A_R, b_R$) are evaluated at the nominal state and treated as constants for the current iteration.
5. Convex Optimization (CFTOC): Solve a Convex Finite-Time Optimal Control (CFTOC) problem. The constraints include:
   • Linearized system dynamics.
   • Linear DHOCBF constraints derived from the supporting hyperplanes.
   • State and input bounds.
   • Slack variables are introduced to ensure feasibility without compromising safety guarantees.
6. Iteration: The solution from the CFTOC becomes the new nominal trajectory for the next iteration. This process repeats until convergence or a maximum iteration count is reached.

C. Multi-Robot Extension

For multi-robot systems, the authors employ a sequential scheme:

• Robots are assigned a priority order.
• Robot 1 solves its MPC considering only static obstacles.
• Subsequent robots treat the predicted trajectories of higher-priority robots as dynamic polytopic obstacles.
• This preserves the convexity of the optimization for each robot while avoiding a centralized, non-convex global problem.

3. Key Contributions

1. Exact Polytope Handling: The framework constructs linear DCBF constraints using exact closest-point computations and supporting hyperplanes, avoiding the geometric distortion inherent in smooth approximations (spheres/ellipsoids).
2. Iterative Convex Framework: By iteratively linearizing both the system dynamics and the robot-obstacle geometry, the method transforms a potentially non-convex problem into a sequence of convex QPs, ensuring global convergence properties of the solver and real-time performance.
3. Generalizability:
   • Supports non-convex robots (modeled as unions of polytopes).
   • Extends naturally to 3-D environments and multi-robot teams.
   • Applicable to general nonlinear dynamics via linearization.
4. Safety Guarantees: The use of DHOCBFs with relative degree $m$ ensures forward invariance of the safe set, guaranteeing collision avoidance even with the linearization approximations (mitigated by slack variables and safety margins).

4. Numerical Results

The authors validated the framework through extensive simulations:

• Environments: 2-D and 3-D cluttered maze scenarios with narrow passages.
• Robot Geometries: Rectangles, Triangles, and L-shaped (non-convex) robots.
• Scenarios: Single robot navigation, multi-robot interaction (3 robots), and 3-D traversal.
• Performance Metrics:
  • Solve Time: Consistently achieved millisecond-level solve times (e.g., ~5ms to ~65ms depending on horizon and complexity), suitable for real-time control.
  • Comparison: Outperformed previous non-convex approaches (e.g., [20]) in terms of computational efficiency while maintaining geometric exactness.
  • Robustness: Successfully navigated tight passages and avoided deadlocks where smooth approximations would fail or restrict movement.

5. Significance

This work bridges a critical gap in robotics control between geometric fidelity and computational efficiency.

• Practical Impact: It enables the deployment of safety-critical controllers for complex robots (like drones or manipulators with non-convex shapes) in cluttered, real-world environments where precise obstacle avoidance is required.
• Theoretical Advancement: It demonstrates that exact polytope distances can be integrated into real-time convex optimization without resorting to aggressive geometric simplifications, provided an iterative linearization strategy is used.
• Scalability: The sequential approach for multi-robot systems offers a scalable solution for cooperative navigation without the computational explosion of centralized non-convex optimization.

In summary, the paper presents a robust, real-time capable framework that allows robots to navigate complex, polytopic environments with high geometric precision, overcoming the limitations of both smooth approximations and non-convex exact methods.