  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.
