Safety-Critical Control and Planning for Obstacle Avoidance between Polytopes with Control Barrier Functions
This work addresses the problem of real-time, collision-free trajectory planning for robots with complex shapes in cluttered environments, representing an incremental improvement over existing offline methods.
The paper tackles the challenge of obstacle avoidance between polytopes in control and planning by proposing a novel optimization formulation using discrete-time control barrier functions, which reduces computational complexity and enables fast online operation for general nonlinear systems, validated through successful navigation in maze simulations with polytopic obstacles.
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.