PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Akshay Thirugnanam, Jun Zeng, Koushil Sreenath

arXiv:2109.12313v4cs.ROcs.SYeess.SYmath.OC

TL;DR

Obstacle avoidance between polytopes remains challenging for nonlinear-system planning because existing approaches are restricted or require long-horizon offline optimization. The paper incorporates dual polytope constraints into DCBF-based NMPC, and reports successful maze navigation with different robot shapes.

  • Problem

    Polytope obstacle avoidance for nonlinear systems remains challenging because mixed-integer methods rely on simplified dynamics, while dual distance-based methods need long horizons and offline solving.

    Source: 3) Obstacle Avoidance between Polytopes · p. 2

  • Method

    The paper incorporates dual obstacle-avoidance constraints between polytopes as DCBF constraints in an NMPC formulation for general nonlinear systems.

    Source: B. Contributions · p. 2

  • Results

    The proposed planner generated dynamically feasible, collision-free trajectories for different robot shapes in tight maze environments.

    Source: §IV · p. 5, §V · p. 7

  • Takeaways & Limitations

    The formulation is presented as a fast online approach for control and planning with general nonlinear dynamical systems.

    Source: §I · p. 1, §V · p. 7

    The NMPC-DCBF problem is nonconvex and requires a good initial guess for faster numerical convergence.

    Source: 5) Warm Start · p. 6

Abstract

from arXiv · show

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.

I. INTRODUCTION

The introduction motivates dynamically feasible, collision-free planning for general nonlinear systems when robot and obstacle shapes must be represented as polytopes. It presents DCBF-based optimization as a fast approach to that problem.

I. INTRODUCTION

  • The paper proposes DCBF-constrained optimization with dual variables to generate dynamically feasible, collision-free trajectories and serve as a fast controller or local planner for general nonlinear systems.The introduction presents the formulation as addressing obstacle avoidance between polytopes.Source: §I · p. 1

1) Graph Search-based and Sampling-based Approaches:

  • Graph-search and sampling methods can produce dynamically infeasible paths, while kinodynamic variants cannot check collisions using the robot’s exact nonlinear dynamics.The paper identifies the need for local generation of dynamically feasible, collision-free trajectories for general dynamical systems.Source: 1) Graph Search-based and… · p. 1

2) Optimization-based Control and Trajectory Planning:

3) Obstacle Avoidance between Polytopes:

  • Existing polytope methods either rely on mixed-integer optimization suited to linear systems or require long horizons and offline solving for nonlinear systems.A prior dual CBF formulation is limited to one-step control for continuous-time affine systems with relative degree one.Source: 3) Obstacle Avoidance between Polytopes · p. 2

4) Obstacle Avoidance with DCBFs:

Prior DCBF work supports shorter-horizon obstacle avoidance, but its polytope formulation remains an open problem. The paper incorporates dual polytope constraints into NMPC and reviews the safety conditions and horizon structure underlying that approach.

4) Obstacle Avoidance with DCBFs:

  • DCBF constraints can support obstacle avoidance with shorter horizons and help prevent local deadlock, but prior applications considered points or hyperspheres rather than polytopes.The section identifies polytopic obstacle avoidance with DCBFs as unresolved in the prior work it discusses.Source: 4) Obstacle Avoidance with DCBFs · p. 2

B. Contributions

  • The paper formulates dual obstacle-avoidance constraints between polytopes as DCBF constraints and incorporates them into NMPC for fast online control and planning of general nonlinear systems.It also reports numerical validation on convex and nonconvex robot shapes navigating tight maze environments.Source: B. Contributions · p. 2

A. Optimization Formulation using DCBFs

  • The background models a discrete-time system with states in X and inputs in U, with continuous dynamics function f and compact input set U.These conditions are stated for the optimization formulation using DCBFs.Source: A. Optimization Formulation using DCBFs · p. 2

1) Discrete-time CBFs:

  • The safe set S is the zero-superlevel set of h, and h is a DCBF when each state in S admits an input satisfying its barrier condition.The DCBF condition gives a lower bound on the next value of h that decays at rate γ_k.Source: 1) Discrete-time CBFs · p. 2
  • Starting in S and choosing inputs from K(x) keeps the trajectory in S; the relaxation variable balances constraint feasibility and safety.The text notes that γ near 1 can make the constraint infeasible, while γ near 0 can approach the boundary quickly and become unsafe.Source: 1) Discrete-time CBFs · pp. 2–3
  • The NMPC formulation separates prediction and safety horizons, with NCBF no greater than N, and optimizes predicted inputs and states over the horizon.At each time step, the first optimized input is applied and the optimization is solved again using the updated state.Source: 1) Discrete-time CBFs · p. 3

B. Obstacle Avoidance between Polytopic Sets

This section sets the obstacle-avoidance scope and describes how polytopic robot-obstacle separation is represented and constrained. It also identifies why directly evaluating the DCBF condition is computationally problematic.

B. Obstacle Avoidance between Polytopic Sets

  • The setting assumes one controlled robot, no static obstacles, and robot and obstacle geometries over-approximated by unions of convex polytopes.The polytopes are assumed bounded and non-empty.Source: B. Obstacle Avoidance between… · p. 3
  • Each obstacle and the robot are represented by polytopes, with facet counts denoted separately for obstacle and robot geometry.The robot’s polytope representation varies with state.Source: B. Obstacle Avoidance between… · p. 3
  • The squared minimum distance between each obstacle and robot polytope defines h_i(x), is zero exactly when they intersect, and can be computed by a convex QP.Pairwise DCBF constraints define safe sets whose intersection gives the overall safe set.Source: B. Obstacle Avoidance between… · p. 3
  • Directly evaluating h_i(f(x,u)) requires a numerical QP, creating implicit non-differentiable constraints that increase computation time.The next section derives explicit differentiable constraints intended to preserve the feasible set of safe inputs.Source: B. Obstacle Avoidance between… · p. 3

III. OPTIMIZATION WITH DUAL DCBF CONSTRAINTS

The section introduces a duality-based optimization to generate obstacle-avoidance maneuvers in tight environments.

III. OPTIMIZATION WITH DUAL DCBF CONSTRAINTS

  • The section derives a duality-based optimization for generating obstacle-avoidance maneuvers in tight environments.The supplied passage states the section’s purpose but does not specify the derivation.Source: §III · p. 3

A. Dual Optimization Problem

The dual optimization problem provides a convex maximization counterpart to the primal minimization problem. Convexity, linear constraints, and a well-defined nonnegative optimum support strong duality.

B. Obstacle Avoidance with Dual Variables

The paper uses feasible dual variables to replace implicit evaluation of the distance-related function in the DCBF constraint. Strong duality makes the resulting constraint equivalent in feasible inputs.

C. Optimization Formulation

The proposed multi-step NMPC formulation applies DCBF safety constraints between robot-obstacle polytope pairs. Its dual-variable formulation does not require control-affine dynamics or differentiable dual variables.

1) Exponential DCBF Constraint:

The exponential DCBF formulation reduces repeated distance-function computation, while smaller obstacle-avoidance horizons can reduce optimization complexity. Numerical simulations report collision-free, dynamically feasible maze navigation across several robot shapes.

1) Exponential DCBF Constraint:

  • Rolling out the decay rate makes each exponential DCBF constraint depend on hi(xt|t) on its right-hand side, so only the current hi needs calculation.The change affects neither feasibility nor safety.Source: 1) Exponential DCBF Constraint · p. 5

2) Horizon Length Selection:

  • Compared with distance constraints, DCBF constraints support effective obstacle avoidance with a smaller horizon, reducing formulation complexity.Source: 2) Horizon Length Selection · p. 5

IV. NUMERICAL RESULTS

  • The algorithm generated dynamically feasible, collision-free trajectories through tight mazes for different robot shapes.Source: §IV · p. 5

1) Environment:

  • The L-shaped robot is non-convex but is represented by two convex polytopes in the maze environment.Source: 1) Environment · p. 5

2) System Dynamics:

The system uses a kinematic bicycle model and an A*-generated global path to guide local NMPC-DCBF trajectory optimization. The local planner tracks a projected reference while applying obstacle constraints to convex robot parts and nearby obstacles.

2) System Dynamics:

  • The kinematic bicycle model uses rear-axle position, speed, and yaw as states, with acceleration and steering angle as inputs.The wheel base is 0.1 m; steering and acceleration are limited to ±0.5 rad and ±1 m/s2, respectively.Source: 2) System Dynamics · p. 5

3) Global Planning:

  • A* generates a global path that is safe only at grid nodes and is not dynamically feasible; line-of-sight reduction then shortens it to fewer waypoints.Collision checks use a safety margin smaller than at least one robot dimension.Source: 3) Global Planning · p. 5

4) Local Trajectory Generation:

  • The local reference starts at the robot’s projection onto the global path and follows its orientation at constant speed; NMPC-DCBF optimizes tracking while avoiding obstacles.The implementation uses N = 11 and N_CBF = 6, with decay rate ω_k = 0.8.Source: 4) Local Trajectory Generation · p. 5
  • For an L-shaped robot, the obstacle constraints are applied to each convex part against each convex obstacle.The L-shape is represented by two convex polytopes.Source: 4) Local Trajectory Generation · p. 6, Figure 2 · p. 6
  • Only obstacles within a radius based on tracking speed, prediction horizon, and maximum deceleration are considered to reduce optimization complexity.Source: 4) Local Trajectory Generation · p. 6

5) Warm Start:

A braking controller supplies initial trajectory and input guesses for the nonconvex NMPC-DCBF optimization. Simulations report tight, collision-free navigation across robot shapes and maze environments, while parameter choices trade tracking, safety, and deadlock avoidance.

5) Warm Start:

  • A braking controller provides the initial trajectory and control-input guesses at each time step, while the previous dual solution initializes horizon-wide dual variables.Maximum deceleration is applied with zero steering until the robot halts, after which acceleration is set to zero.Source: 5) Warm Start · p. 6

6) Simulation Results:

  • The optimization is reported sufficiently fast for trajectory generation across different-shaped robots and maze environments.The paper reports tight-fitting obstacle avoidance and reversing motion to avoid deadlock; Table I presents solver-time statistics.Source: 6) Simulation Results · pp. 6–7, Table I · p. 7
  • Safety tuning favors a horizon long enough to stop the robot and a decay rate closer to 1, while larger terminal cost can improve deadlock avoidance but increase velocity and risk unsafe motion.Source: 6) Simulation Results · p. 7

V. CONCLUSION

  • The proposed DCBF-based nonlinear optimization is validated on maze navigation with varied robot shapes and polytopic obstacles.Source: §V · p. 7

Related papers

Loading 2109.12313v4…