Optimization-Based Collision Avoidance

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.

I. Introduction

Maneuvering autonomous systems in an environment with obstacles is a challenging problem that arises in a number of practical applications, including robotic manipulators and trajectory planning for autonomous systems, such as self-driving cars and quadcopters. In almost all of those applications, a fundamental feature is the system’s ability to avoid collision with obstacles, which are, for example, humans operating in the same area, other autonomous systems, or static objects such as walls.