Optimization-Based Collision Avoidance

Abstract

This article presents a novel method for exactly reformulating nondifferentiable collision avoidance constraints into smooth, differentiable constraints using strong duality of convex optimization. We focus on a controlled object whose goal is to avoid obstacles while moving in an n-dimensional space. The proposed reformulation is exact, does not introduce any approximations, and applies to general obstacles and controlled objects that can be represented as the union of convex sets. We connect our results with the notion of signed distance, which is widely used in traditional trajectory generation algorithms. Our method can be applied to generic navigation and trajectory planning tasks, and the smoothness property allows the use of general-purpose gradient- and Hessian-based optimization algorithms. Finally, in case a collision cannot be avoided, our framework allows us to find “least-intrusive” trajectories, measured in terms of penetration. We demonstrate the efficacy of our framework on an automated parking problem, where our numerical experiments suggest that the proposed method is robust and enables real-time optimization-based trajectory planning in tight environments. Sample code of our example is provided at https://github.com/XiaojingGeorgeZhang/OBCA.