Optimization-Based Collision Avoidance

Xiaojing Zhang; Alexander Liniger; Francesco Borrelli

Source-linked AI summary

Optimization-Based Collision Avoidance

Xiaojing Zhang, Alexander Liniger, Francesco Borrelli

arXiv:1711.03449v3Nov 9, 2017math.OCcs.ROeess.SY

TL;DR

Optimization-based trajectory planning requires collision avoidance constraints that are difficult to formulate because they are generally non-convex. The paper uses strong duality to construct exact smooth reformulations for finite unions of convex sets, including signed-distance and penetration handling, and reports real-time-feasible planning in quadcopter and parking examples.

Problem
Collision avoidance constraints are generally non-convex and difficult to obtain in practice because signed distance itself is defined through optimization problems.
Method
The paper reformulates distance-based collision avoidance exactly and non-conservatively as smooth nonlinear constraints using strong duality, for objects and obstacles represented as finite unions of convex sets.
Results
The proposed reformulations enable real-time-feasible trajectory planning in quadcopter navigation and autonomous parking examples, including challenging tight-environment scenarios.
Takeaways & Limitations
The framework supports both point-mass and full-dimensional controlled objects while incorporating system dynamics and input constraints.
Takeaways & Limitations
The reported parking trajectories can be challenging for low-level tracking despite being collision-free and kinodynamically feasible.

Abstract

from arXiv · show

This paper presents a novel method for reformulating non-differentiable collision avoidance constraints into smooth nonlinear 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 does not introduce approximations, and applies to general obstacles and controlled objects that can be represented in an n-dimensional space as the finite union of convex sets. Furthermore, we connect our results with the notion of signed distance, which is widely used in traditional trajectory generation algorithms. Our method can be used in 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 a quadcopter navigation and automated parking problem, and our numerical experiments suggest that the proposed methods enable real-time optimization-based trajectory planning problems in tight environments. Source code of our implementation is provided at https://github.com/XiaojingGeorgeZhang/OBCA.

1. Introduction

The paper addresses the difficulty of formulating collision avoidance for optimization-based trajectory planning. It proposes exact smooth reformulations for finite unions of convex sets, supports full-dimensional objects, and demonstrates real-time planning in tight environments.

Collision avoidance constraints are non-convex and computationally difficult in optimization-based trajectory planning.
Existing approaches may use approximations with difficult-to-establish errors, restrict controlled objects to point masses, or introduce integer variables for polyhedral obstacles.
For controlled objects and obstacles represented as finite unions of convex sets, collision avoidance can be exactly and non-conservatively reformulated as smooth non-convex constraints.The reformulation uses strong duality to reformulate the distance function between convex sets.
A signed-distance formulation characterizes both separation and penetration, enabling least-intrusive trajectories when collisions cannot be avoided.
The reformulations are demonstrated on quadcopter trajectory planning and autonomous parking tasks in tight environments.Both formulations are reported to enable real-time path planning and find trajectories in challenging circumstances.
Because system dynamics and input constraints can be incorporated, the generated trajectories are kinodynamically feasible and can be tracked by simple low-level controllers.

Related Work

Prior optimization-based collision-avoidance methods commonly use point-mass models, while full-dimensional objects receive less attention. Existing approaches include potential fields, disjunctive or mixed-integer formulations, smooth non-convex constraints, and signed-distance linearization.

Optimization-based collision avoidance formulates obstacle avoidance through constraints within an optimal control problem.
Most prior work uses point-mass models and represents controlled-object shape by inflating obstacles.
Polyhedral obstacles can be handled with disjunctive programming, often producing mixed-integer optimization problems.
Ellipsoidal obstacles admit smooth non-convex collision-avoidance constraints solvable with generic nonlinear-programming solvers.
Full-dimensional controlled objects have been less widely studied, with prior methods modeling vertices or using signed distance and sequential linearization.
Compared with closely related point-mass formulations, this approach generalizes to full-dimensional objects and computes least-intrusive trajectories when collisions cannot be avoided.

Notation

The paper uses proper cones to express generalized inequalities and dual norms, while representing the occupied spaces of controlled objects and obstacles as subsets of R^n.

For a proper cone K, a ⪯K b means that b − a belongs to K; the standard element-wise inequality is a special case.
The dual norm ∥·∥* and dual cone K* provide the dual objects associated with the chosen norm and cone.
E denotes the space occupied by the controlled object, while O denotes the space occupied by obstacles, both subsets of R^n.

2. Problem Description

The paper formulates finite-horizon optimal control with collision avoidance for point-mass and full-dimensional objects. It then targets smooth reformulations of generally non-convex, non-differentiable constraints using convex-set representations and optimization-based constructions.

2.1. Dynamics, Objective and Constraints: The controlled object follows smooth dynamics xk+1 = f(xk, uk), with states, inputs, and state/input constraints over a finite horizon.
2.1. Dynamics, Objective and Constraints: The objective is to find a control sequence moving the system from initial state xS to final state xF while optimizing J.
2. Problem Description: Collision avoidance requires E(xk) ∩ O(m) = ∅ for every obstacle, but this constraint is generally non-differentiable.
2.2. Obstacle and Controlled Object Modeling: Obstacles are assumed to be convex compact sets with conic representations, which can describe compact convex sets and polyhedral obstacles.
2.2. Obstacle and Controlled Object Modeling: The controlled object may be a point mass or a full-dimensional set modeled through rotation and translation of an initial shape.
2.3. Optimal Control Problem with Collision Avoidance: The resulting optimal-control problem remains difficult because collision-avoidance constraints are non-convex and non-differentiable.
2.4. Collision Avoidance: Two reformulations preserve continuity and differentiability, enabling existing gradient- and Hessian-based optimization algorithms without explicitly representing signed distance.
2.4. Collision Avoidance: Signed distance is positive for non-intersecting objects and negative for overlapping objects, but is difficult to enforce directly because it is generally non-convex, non-differentiable, and itself defined by optimization problems.

3. Collision Avoidance for Point-Mass Models

The paper reformulates point-mass collision-avoidance constraints exactly through convex duality, yielding smooth nonlinear programs in practically relevant norm and cone settings. A signed-distance extension also supports minimum-penetration trajectories when collision-free motion is infeasible.

Collision-free reformulation: Strong duality converts the point-mass distance condition into constraints over states, inputs, and obstacle-associated dual variables.The dual variable provides a certificate for satisfying the required separation.
Collision-free reformulation: The distance formulation is an exact reformulation of the original optimal control problem.The resulting trajectory remains kinodynamically feasible.
Smoothness: Without additional assumptions on the norm and cone, the final constraints are not guaranteed to be smooth.This limits unconditional applicability to algorithms requiring smooth constraints.
Smoothness: The formulation is smooth for Euclidean distance with standard or second-order cones and smooth system and cost functions.These cases include polyhedral and ellipsoidal obstacles and permit general-purpose nonlinear solvers such as IPOPT.
Minimum penetration: Distance-only constraints cannot distinguish different penetration depths, so softened constraints cannot select least-intrusive colliding trajectories.Overlapping objects have zero distance regardless of penetration.
Minimum penetration: Signed distance combines distance and penetration formulations and enables soft-constrained minimum-penetration trajectories for any threshold d ∈ R.The generalization replaces the convex dual-norm inequality with a non-convex equality, which generally increases computation time.

4. Collision Avoidance for Full-Dimension Controlled Objects

The full-dimensional formulation models controlled objects with geometric extent rather than point masses, using additional dual variables to obtain exact collision-avoidance constraints. Its signed-distance version extends the framework to minimum-penetration trajectories, with smoothness under specified norm and cone choices.

Full-dimensional modeling: Point-mass models can be overly conservative for objects such as cars, motivating an extension to full-dimensional controlled objects.The paper specifically notes that ball approximations may prevent feasible parking in tight environments.
Collision-free reformulation: Strong duality reformulates full-dimensional separation constraints using obstacle dual variables λ and additional object-related dual variables µ.The optimization is performed over states, inputs, λ, and µ.
Collision-free reformulation: The full-dimensional collision-free formulation is exact and smooth for Euclidean distance when the relevant cones are standard or second-order cones.The exact reformulation is given for the original optimal control problem.
Minimum penetration: Like the point-mass distance formulation, the full-dimensional collision-free program cannot produce least-intrusive trajectories when collision-free trajectories do not exist.The paper addresses this limitation by introducing penetration-based soft constraints.
Minimum penetration: The signed-distance reformulation uses a non-convex dual-norm equality and supports negative signed-distance thresholds for colliding configurations.Slack variables are weighted to remain as small as possible, representing penetration depth when active.
Applications: The full-dimensional framework is applied to autonomous parking with rectangular cars and obstacles in reverse and parallel parking scenarios.These applications target tight environments where point-mass approximations can be restrictive.

5. Example 1: Quadcopter Path Planning

The quadcopter experiment evaluates point-mass collision-avoidance formulations in a room containing walls and a narrow window. Both formulations find the tested paths, while solving both in parallel reduces the worst-case computation time.

Environment: The quadcopter must cross a room while passing below a low-hanging wall and through a small window in a second wall.The room measures 10.5 × 10.5 × 5.5 m, and the window is 1 × 1 m.
Model and objective: The quadcopter is modeled as a point mass with a 0.25 m Euclidean safety distance representing its shape.The same safety distance is used in the distance and signed-distance formulations.
Model and objective: The nonlinear optimal control problem uses a standard quadcopter model, rotor speeds as inputs, and a cost balancing final time against control effort.The discretization time is optimized rather than fixed, allowing maneuver duration to vary.
Initialization: A⋆ supplies an obstacle-free position-space warm start, but gridding produces a zigzag path and ignores the quadcopter dynamics.The optimization subsequently generates a dynamically feasible trajectory.
Simulation results: Both distance and signed-distance formulations successfully compute all tested quadcopter paths, but their computation-time patterns are not correlated.A difficult case for one formulation may be easy for the other.
Simulation results: 28.9 s is the worst-case computation time when both formulations are solved and the first solution is accepted, versus 48.0 s and 59.1 s individually.The paper recommends solving both formulations to obtain feasible trajectories as fast as possible.
Simulation results: 2.8 s is the maximum computation time for A⋆ to generate the initial guess, below the time required by either optimization formulation.A⋆ plans only in position space and ignores system dynamics.

6. Example 2: Autonomous Parking

The autonomous-parking experiment uses the full-dimensional formulation to plan rectangular-car maneuvers in tight reverse- and parallel-parking environments. Both formulations succeed across the tested initial conditions, while distance formulations solve faster and the combined framework supports reported real-time parking computation.

Problem and modeling: Modeling the car as a rectangle avoids the conservatism of ball approximations and enables obstacle-free parking trajectories in tight environments.The experiments consider reverse and parallel parking.
Problem and modeling: The car uses a kinematic bicycle model with position, yaw, velocity, steering, and acceleration variables subject to stated bounds.The vehicle dimensions are 4.7 × 2 m and its wheel base is 2.7 m.
Initialization: Hybrid A⋆ provides the parking warm start because ordinary A⋆ cannot account for the vehicle’s non-holonomic dynamics.Hybrid A⋆ searches a gridded state space using a simplified vehicle model and discrete steering inputs.
Reverse parking: 84 initial conditions were tested for reverse parking, and both formulations found feasible parking trajectories for all of them.Starting position showed no obvious relation to solution time.
Computation time: 0.60 s versus 1.03 s are the mean computation times for distance and signed-distance formulations in reverse parking.The signed-distance formulation takes longer because it includes slack variables.
Parallel parking: 100% success was reported for parallel parking, with mean computation times of 0.87 s for distance and 1.67 s for signed distance.Parallel parking was computationally more demanding than reverse parking in these experiments.
Initialization: Hybrid A⋆ paths are collision-free and kinodynamically feasible but can be difficult for low-level controllers to track.They omit velocity information and steering and acceleration rate constraints, permitting aggressive maneuvers.
Computation time: 7.7 s and 9.2 s were the maximum overall computation times for reverse and parallel parking using Hybrid A⋆ with signed distance.The paper states that this enables real-time autonomous parking in tight environments.

7. Conclusion

The paper presents exact, non-conservative smooth reformulations for collision avoidance involving finite unions of convex sets, covering point-mass and full-dimensional controlled vehicles. Numerical studies indicate robust, real-time-feasible trajectory generation when appropriately initialized, including dynamically feasible trajectories and least-intrusive solutions when collision-free motion is unavailable.

The framework handles controlled objects and obstacles represented as finite unions of convex sets.
Dualization preserves differentiability, enabling gradient- and Hessian-based optimization methods for non-differentiable polytopic constraints.
The reformulations are exact and non-conservative for both point-mass and full-dimensional controlled vehicles.
When collision-free trajectories cannot be generated, the framework computes least-intrusive trajectories measured by penetration.
Numerical studies on quadcopter planning and autonomous car parking indicate robust, real-time-feasible behavior when the method is appropriately initialized.
The generated trajectories are dynamically feasible, while initialization remains problem-dependent and computation-time improvements remain an active focus.

Appendix: Proof of Proposition 2

The appendix invokes standard convex-analysis results for compact convex sets, defining supporting half-spaces and hyperplanes through support-function maximization. It also refers to the distance definition used in the proof.

For a compact convex set C, HC(z) is the supporting half-space with normal vector z.
The boundary ∂HC(z) is the supporting hyperplane with normal vector z, obtained by replacing the half-space inequality with equality.
The proof uses the paper’s previously defined distance function dist(·, ·).

Proof of Proposition 2

The proof expresses penetration through supporting hyperplanes and then applies strong duality to replace an inner support-function maximization with a conic dual problem. This yields an equivalent optimization characterization involving dual variables and a dual-norm constraint.

Penetration equals the distance from the controlled object to the complement of the obstacle.
Penetration can be written as the infimum over dual-unit supporting hyperplanes of their distance from the controlled object.
Strong duality replaces the obstacle support-function maximization with a conic dual minimization over λ satisfying A^Tλ = z and λ ⪰K* 0.
Substitution produces an equivalent formulation minimizing (b − AE(x))^Tλ subject to ∥A^Tλ∥* = 1 and λ ⪰K* 0.
The penetration-threshold condition is equivalent to requiring the resulting infimum to be below pmax.