Optimization-Based Collision Avoidance

by   Xiaojing Zhang, et al.
ETH Zurich
berkeley college

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.


page 13

page 16

page 17

page 19

page 20

page 21


Efficient Formulation of Collision Avoidance Constraints in Optimization Based Trajectory Planning and Control

To be applicable to real world scenarios trajectory planning schemes for...

Motion Planning around Obstacles with Convex Optimization

Trajectory optimization offers mature tools for motion planning in high-...

Collision Avoidance for Dynamic Obstacles with Uncertain Predictions using Model Predictive Control

We propose a Model Predictive Control (MPC) for collision avoidance betw...

Distributed Swarm Trajectory Optimization for Formation Flight in Dense Environments

For aerial swarms, navigation in a prescribed formation is widely practi...

Differentiable Collision Avoidance Using Collision Primitives

A central aspect of robotic motion planning is collision avoidance, wher...

MADER: Trajectory Planner in Multi-Agent and Dynamic Environments

This paper presents MADER, a 3D decentralized and asynchronous trajector...

Data-Driven Risk-sensitive Model Predictive Control for Safe Navigation in Multi-Robot Systems

Safe navigation is a fundamental challenge in multi-robot systems due to...

Code Repositories


Optimization-Based Collision Avoidance - a path planner for autonomous parking and navigation

view repo

1 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.

Optimization-based trajectory planning algorithms such as Model Predictive Control (MPC) have received significant attention recently, ranging from (unmanned) aircraft to robots to autonomous cars RichardsHow_ACC2002 ; BorrelliKeviczkyBalas_CDC2004 ; Diehl2006 ; BlackmoreWilliamsManipularObstacleAvoid2006 ; gao2010predictive ; GoulartTrajectory2011 ; BlackmoreOnoWilliams2011 ; Salmah2013 ; SchulmanAbeelColisionChecking2014 ; Liniger_OCAM2015 ; rosolia2017autonomousrace ; FunkeGerdes2017 . This can be attributed to the increase in computational resources, the availability of robust numerical algorithms for solving optimization problems, as well as MPC’s ability to systematically encode system dynamics and constraints inside its formulation.

One fundamental challenge in optimization-based trajectory planning is the appropriate formulation of collision avoidance constraints, which are known to be non-convex and computationally difficult to handle in general. While a number of formulations have been proposed in the literature for dealing with collision avoidance constraints, they are typically limited by one of the following features: The collision avoidance constraints are approximated through linear constraint, and it is difficult to establish the approximation error SchulmanAbeelColisionChecking2014 ; Existing formulations focus on point-mass controlled objects, and are not applicable to full-dimensional objects; When the obstacles are polyhedral, then the collision avoidance constraints are often reformulated using integer variables GrossmannReviewDisjunctive2002 . While this reformulation is attractive for linear systems with convex constraints since in this case a mixed-integer convex optimization problem can be solved, integer variables should generally be avoided when dealing with nonlinear systems when designing real-time controllers for robotic systems.

In this paper, we focus on a controlled object that moves in a general -dimensional space while avoiding obstacles, and propose a novel approach for modeling obstacle avoidance constraints that overcomes the aforementioned limitations. Specifically, the contributions of this paper can be summarized as follows:

  • We show that if the controlled object and the obstacles are described by convex sets such as polytopes or ellipsoids (or can be decomposed into a finite union of such convex sets), then the collision avoidance constraints can be exactly and non-conservatively reformulated as a set of smooth non-convex constraints. This is achieved by appropriately reformulating the distance-function between two convex sets using strong duality of convex optimization.

  • We provide a second formulation for collision avoidance based on the notion of signed distance, which characterizes not only the distance between two objects but also their penetration. This reformulation allows us to compute “least-intrusive” trajectories in case collisions cannot be avoided.

  • We demonstrate the efficacy of the proposed obstacle avoidance reformulations on a quadcopter trajectory planning problem and autonomous parking application, where the controlled vehicles must navigate in tight environments. We show that both the distance reformulation and the signed distance reformulation enable real-time path planning and find trajectories even in challenging circumstances.

Furthermore, since both our formulations allow the incorporation of system dynamics and input constraints, the generated trajectories are kinodynamically feasible, and hence can be tracked by simple low-level controllers.

This paper is organized as follows: Section 2 introduces the problem setup. Section 3 presents the collision avoidance and minimum-penetration formulations for the case when the controlled object is a point mass. These results are then extended to full-dimensional controlled objects in Section 4. Numerical experiments demonstrating the efficacy of the proposed method are given in Sections 5 and 6, and conclusions are drawn in Section 7. The Appendix contains auxiliary results needed to prove the main results of the paper. The source code of a quadcopter navigation example and autonomous parking example described in Sections 5 and 6 is provided at https://github.com/XiaojingGeorgeZhang/OBCA.

Related Work

A large body of work exists on the topic of obstacle avoidance. In this paper, we do not review, or compare, optimization-based collision avoidance methods with alternative approaches such as those based on dynamic programming SchildbachDPParking2016 , reachability analysis TomlinPappasSastry1998 ; MargellosLygerosTAC2011 ; MargellosLygerosTCST2013 , graph search LikhachevFergusonPlanningLongDyn2009 ; DolgovThrunPathPlanning2010 ; LikhachevMultiAstar_IJRR2016 , (random) sampling LaValle2001 ; Ziegler2008 ; KaramanFrazzoli2011 ; BanzhafHybridCurvature2017

, or interpolating curves

Vorobieva2014 . Indeed, collision avoidance problems are known to be NP-hard in general canny1988complexity

, and all practical methods constitute some sort of “heuristics”, whose performance depends on the specific problem and configuration at hand. In the following, we briefly review optimization-based approaches, and refer the interested reader to

lavalle2006planning ; Campbell2010 ; Katrakazas2015416 ; PadenFrazolliSurveyMotionPlanning2016 ; GonzalezReviewMotionPlanning2016 for a comprehensive review on existing trajectory planning and obstacle avoidance algorithms.

The basic idea in optimization-based methods is to express the collision avoidance problem as an optimal control problem, and then solve it using numerical optimization techniques. One way of dealing with obstacle avoidance is to use unconstrained optimization, in which case the objective function is augmented with “artificial potential fields” that represent the obstacles Khatib1986 ; KalakrishnanSTOMP_2011 ; ParkITOMP_2012 ; ZuckerDraganCHOMP_2013 ; Gennert2016_BiRRTOpt . More recently, methods based on constrained optimization has attracted attention in the control community, due to its ability to explicitly formulate collision avoidance through constraints RichardsHow_ACC2002 ; BlackmoreWilliamsManipularObstacleAvoid2006 ; GoulartTrajectory2011 ; SchulmanAbeelColisionChecking2014 . Broadly speaking, constrained optimization-based collision-avoidance algorithms can be divided into two cases based on the modeling of the controlled object: point-mass models and full-dimensional objects. Due to its conceptual simplicity, the vast majority of literature focuses on collision avoidance for point-mass models, and consider the shape of the controlled object by inflating the obstacles. The obstacles are generally assumed to be either polytopes or ellipsoids. For polyhedral obstacles, disjunctive programming can be used to ensure collision avoidance, which is often reformulated as a mixed-integer optimization problem RichardsHow_ACC2002 ; BlackmoreWilliamsManipularObstacleAvoid2006 ; GrossmannReviewDisjunctive2002 . In case of ellipsoidal obstacles, the collision avoidance constraints can be formulated as a smooth non-convex constraint Rosolia2017 ; nageli2017real

, and the resulting optimization problem can be solved using generic non-linear programming solvers.

The case of full-dimensional controlled objects has, to be best of the authors’ knowledge, not been widely studied in the context of optimization-based methods, with the exception of SchulmanAbeelColisionChecking2014 ; LiShao2015 . In LiShao2015 , the authors model the controlled object through its vertices and, under the assumption that all involved object are rectangles, ensure collision avoidance by keeping all vertices of the controlled object outside the obstacle. A more general way of handling collision avoidance for full-dimensional controlled object has been proposed in SchulmanAbeelColisionChecking2014 using the notion of signed distance, where the authors also propose a sequential linearization technique to deal with the non-convexity of the signed distance function.

The approach most closely related to our formulation is probably the work of

GoulartTrajectory2011 , where the authors propose a smooth reformulation of the collision avoidance constraint for point-mass controlled objects and polyhedral obstacles. However, our approach differs from GoulartTrajectory2011 as our approach generalizes to full-dimensional controlled objects, and we are, based on the notion of penetration, also able to compute least-intrusive trajectories in case collisions cannot be avoided.


Given a proper cone

and two vectors

, then is equivalent to . If is the standard cone, then is equivalent to the standard (element-wise) inequality . Moreover, is the dual norm of , and is the dual cone of . The “space” occupied by the controlled object (e.g., a drone, vehicle, or robot in general) is denoted as ; similarly, the space occupied by the obstacles is denoted as .

2 Problem Description

2.1 Dynamics, Objective and Constraints

We assume that the dynamics of the controlled object takes the form


where is the state of the controlled object at time step given an initial state , is the control input, and describes the dynamics of the system. In most cases, the state contains information such as the position and angles of the controlled object, as well the velocities and angular rates . In this paper, we assume that no disturbance is present, and that the system is subject to input and state constraints of the form


where , is the number of constraints, and the inequality in (2) is interpreted element-wise. Our goal is to find a control sequence, over a horizon , which allows the controlled object to navigate from the initial state to its final state , while optimizing some objective function , where is a stage cost, and avoiding obstacles . Throughout this paper, we assume that the functions , and are smooth. Smoothness is assumed for simplicity, although all forthcoming statements apply equally to cases when those functions are twice continuously differentiable.

2.2 Obstacle and Controlled Object Modeling

Given the state , we denote by the “space” occupied by the controlled object at time , which we assume is a subset of . The collision avoidance constraint at time is now given by111In this paper, we only consider collision avoidance constraints that are associated with the position and geometric shape of the controlled object, which are typically defined by its position and angles . This is not a restriction of the theory as the forthcoming approaches can be easily generalized to collision avoidance involving other states, but done to simplify exposition of the material.


Constraint (3) is non-differentiable in general, e.g., when the obstacles are polytopic GoulartTrajectory2011 ; SchulmanAbeelColisionChecking2014 . In this paper, we will remodel (3) in such a way that both continuity and differentiability are preserved. To this end, we assume that the obstacles are convex compact sets with non-empty relative interior222Non-convex obstacles can often be approximated/decomposed as the union of convex obstacles, and can be represented as


where , , and is a closed convex pointed cone with non-empty interior. Representation (4) is entirely generic since any compact convex set admits a conic representation of the form (4) (rockafellar1970, , p.15). In particular, polyhedral obstacles can be represented as (4) by choosing ; in this case corresponds to the well-known element-wise inequality . Likewise, ellipsoidal obstacles can be represented by letting be the second-order cone, see boyd2004convex for details. To simplify the upcoming exposition, the same cone is assumed for all obstacles; the extension to obstacle-specific cones is straight-forward.

In this paper, we will consider controlled objects that are modeled as point-masses as well as full-dimensional objects. In the former case, simply extracts the position from the state , i.e.,

In the latter case, we will model the controlled object as the rotation and translation of an “initial” convex set , i.e.,
where is an (orthogonal) rotation matrix and is the translation vector. The matrices and the cone , which we assume is closed, convex and pointed, define the shape of our initial (compact) set and are assumed known. Often, the rotation matrix depends on the angles of the controlled object, while the translation vector depends on the position of the controlled object. We assume throughout that the functions and are smooth.

2.3 Optimal Control Problem with Collision Avoidance

By combining (1)–(3), the constrained finite-horizon optimal control problem with collision avoidance constraint is given by


where is either given by (5a) (point-mass model) or (5b) (full-dimensional set), is the collection of all states, and is the collection of all inputs. A key difficulty in solving problem (6), even for linear systems with convex objective function and convex state/input constraints, is the presence of the collision-avoidance constraints , which in general are non-convex and non-differentiable GoulartTrajectory2011 ; SchulmanAbeelColisionChecking2014 . In the following, we present two novel approaches for modeling collision avoidance constraints that preserve continuity and differentiability, and are amendable for use with existing off-the-shelf gradient- and Hessian-based optimization algorithms.

2.4 Collision Avoidance

A popular way of formulating collision avoidance is based on the notion of signed distance SchulmanAbeelColisionChecking2014


where and are the distance and penetration function, and are defined as


Roughly speaking, the signed distance is positive if and do not intersect, and negative if they overlap. Therefore, collision avoidance can be ensured by requiring . Unfortunately, directly enforcing inside the optimization problem (6) is generally difficult since it is non-convex and non-differentiable in general SchulmanAbeelColisionChecking2014 . Furthermore, for optimization algorithms to be numerically efficient, they require an explicit representation of the functions they are dealing with, in this case . This, however, is difficult to obtain in practice since itself is the solution of the optimization problems (8a) and (8b). As a result, existing algorithms approximate (7) through local linearization SchulmanAbeelColisionChecking2014 , for which it is difficult to establish bounds on approximation errors.

In the following, we propose two reformulation techniques for obstacles avoidance that overcome the issues of non-differentiability and that do not require an explicit representation of the signed distance. We begin with point-mass models in Section 3, and treat the general case of full-dimensional controlled objects in Section 4.

3 Collision Avoidance for Point-Mass Models

In this section, we first present a smooth reformulation of (6) when in Section 3.1, and then extend the approach in Section 3.2 to generate minimum-penetration trajectories in case collisions cannot be avoided. To simplify notation, the time indices are omitted in the remainder of this section.

3.1 Collision-Free Trajectory Generation

Proposition 1.

Assume that the obstacle and the controlled object are given as in (4) and (5a), respectively, and let be a desired safety margin between the controlled object and the obstacle. Then we have:


It follows from (4) and (8a) that . Following (boyd2004convex, , p.401), its dual problem is given by , where is the dual norm associated to and is the dual cone of . Since is assumed to have non-empty relative interior, strong duality holds, and . Hence, for any non-negative scalar , is satisfied if, and only if, there exists . The desired result follows from identity (5a). ∎

Intuitively speaking, any variable satisfying the right-hand-side of (9) is a certificate verifying the condition . Since is equivalent to , the optimal control problem (6) for the point-mass model (5a) is given by


where is the position of the controlled object at time , is the dual variable associated with obstacle at time step , and the optimization is performed over the states , the inputs and the dual variables . We emphasize that (10) is an exact reformulation of (6) and that the optimal trajectory obtained by solving (6) is kinodynamically feasible.

Remark 1.

Without further assumptions on the norm and the cone , the last two constraints in (10) are not guaranteed to be smooth, a property that many general-purpose non-linear optimization algorithms require333Strictly speaking, these solvers often require the cost function and constraints to be twice continuously differentiable only. Smoothness is assumed in this paper for the sake of simplicity.. Fortunately, it turns out that these constraints are smooth for the practically relevant cases of being the Euclidean distance and either the standard cone or the second-order cone, which allows us to model polyhedral and ellipsoidal obstacles. In these cases, and under the assumption that the functions , and are smooth, (10) is a smooth nonlinear optimization problem that is amendable to general-purpose non-linear optimization algorithms such as IPOPT WachterIPOPT2006 . Without going into details, we point out that smoothness is retained when is a general -norm, with , and is the cartesian product of -order cones , with . In this case, the dual norm is given by and the dual cone is , where satisfies , see boyd2004convex for details on dual norms and dual cones.

While reformulation (10) can be used for obstacle avoidance, it is limited to finding collision-free trajectories. Indeed, in case collisions cannot be avoided, the above formulation is not able to find “least-intrusive” trajectories by softening the constraints. Intuitively speaking, this is because (10) is based on the notion of distance, and the distance between two overlapping objects (as is in the case of collision), is always zero, regardless of the penetration. From a practical point of view, this implies that slack variables cannot be included in the constraints of (9), because the optimal control problem is not able to distinguish between “severe” and “less severe” colliding trajectories. Furthermore, in practice, it is often desirable to soften constraints and include slack variables to ensure feasibility of the (non-convex) optimization problem, since (local) infeasibilities in non-convex optimization problem are known to cause numerical difficulties. In the following, we show how the above limitations can be overcome by considering the notion of penetration and softening the collision avoidance constraints.

3.2 Minimum-Penetration Trajectory Generation

In this section, we consider the design of minimum-penetration trajectories for cases when collision cannot be avoided and the goal is to find a “least-intrusive” trajectory. Following the literature CameronCulleyTranslationalDistance1986 ; Dobkin1993 , we measure “intrusion” in terms of penetration as defined in (8b).

Proposition 2.

Assume that the obstacle and controlled object are given as in (4) and (5a), respectively, and let be a desired maximum penetration of the controlled object and the obstacle. Then we have:


The proof, along with auxiliary lemmas, is given in the Appendix. ∎

Proposition 2 resembles Proposition 1 with the difference that the convex inequality constraint is replaced with the non-convex equality constraint . In the following, we will see that Propositions 1 and 2 can be combined to represent the signed distance as defined in (7).

Theorem 1.

Assume that the obstacle and the controlled object are given as in (4) and (5a), respectively. Then, for any , we have:


By definition, if , and if . Let , in which case (12) follows directly from (11). If , then we have from (9) that is equivalent to . Due to homogeneity with respect to , if the previous condition is satisfied, then there always exists a (scaled) dual multiplier such that . This concludes the proof. ∎

Reformulation (12) is similar to reformulation (9), with the difference that (12) holds for all , while (9) only holds for . The “price” we pay for this generalization is that the convex constraint is turned into the non-convex equality constraint which, as we will see later on, generally results in longer computation times. Nevertheless, Theorem 1 allows us to compute trajectories of least penetration whenever collision cannot be avoided by solving the following soft-constrained minimum-penetration problem:


where is the position of the controlled object at time , is the slack variable associated to the object at time step , and is a weight factor that keeps the slack variable as close to zero as possible. Without going into details, we point out that the weight should be chosen “big enough” such that the slack variables only become active when the original problem is infeasible, i.e., when obstacle avoidance is not possible borrelli2017predictive . Notice that a positive slack variable implies a colliding trajectory, where the penetration depth is given by . We close this section by pointing out that if, a priori, it is known that a collision-free trajectory can be generated, then formulation (10) should be given preference over formulation (13) because the former has fewer decision variables, and because the constraint is convex, which generally leads to improved solution times. Smoothness of (13) is ensured if is the Euclidean distance, and is either the standard cone or the second-order cone, see Remark 1 for details.

4 Collision Avoidance for Full-Dimension Controlled Objects

The previous section provided a framework for computing collision-free and minimum-penetration trajectories for controlled objects that are described by point-mass models. While such models can be used to generate trajectories for “ball-shaped” controlled objects, done by setting the minimum distance equal to the radius of the controlled object (see Section 5 for such an example), it can be restrictive in other cases. For example, modeling a car in a parking lot as a Euclidean ball can be very conservative, and prevent the car from finding a parking spot. To alleviate this issue, we show in this section how the results of Section 3 can be extended to full-dimensional controlled objects.

4.1 Collision-Free Trajectory Generation

Similar to Section 3, we begin by first reformulating the distance function, which will allow us to generate collision-free trajectories:

Proposition 3.

Assume that the controlled object and the obstacle are given as in (5b) and (4), respectively, and let be a desired safety margin. Then we have:


Recall that , where the last equality follows from (5b). The dual of this minimization problem is given by , see e.g., (boyd2004convex, , Section 8.2) for the derivation, where is the dual norm, and and are the dual cones of and , respectively. Since and are assumed to have non-empty relative interior, strong duality holds, and . ∎

Compared to Proposition 1, we see that full-dimensional controlled objects require the introduction of the additional dual variables , one for each obstacle . By setting , we obtain now the following reformulation of (6) for full-dimensional objects:


where and are the dual variables associated with the obstacle at step , and are the collection of all and , respectively, and the optimization is performed over . Notice that (15) is an exact reformulation of (6). Smoothness of (15) is ensured if is the Euclidean distance, and and are either the standard cone or the second-order cone, see also Remark 1 for details.

Similar to the point-mass case in Section 3.1, the optimal control problem (15) is able to generate collision-free trajectories, but unable to find “least-intrusive” trajectories in case collision-free trajectories do not exist. This limitation is addressed next.

4.2 Minimum-Penetration Trajectory Generation

We overcome the above limitation by considering again the notion of penetration. We begin with the following result:

Proposition 4.

Assume that the obstacles and controlled object are given as in (4) and (5b), respectively, and let be a maximal penetration depth. Then we have:


It follows from CameronCulleyTranslationalDistance1986 that , where is the Minkowski difference. Furthermore, we have from the proof of Proposition 2 that . Using strong duality of convex optimization, we can dualize the inner maximization problem as . Hence, . Eliminating the -variable using the first equality constraint and following the steps of the proof of Proposition 2 gives the desired result. ∎

The following theorem shows that Propositions 3 and 4 can be combined to represent the signed distance function.

Theorem 2.

Assume that the obstacles and controlled object are given as in (4) and (5b), respectively. Then, for any , we have:


By definition, if , and if . Consider now , in which case (17) follows directly from (16). Assume now that ; then we have from (14) that is equivalent to . Due to homogeneity with respect to and , if the previous condition is satisfied, then there also exists a and such that . This concludes the proof. ∎

Theorem 2 allows us to formulate the following soft-constrained minimum-penetration optimal control problem


where is the slack variable associated to obstacle at time step , and is a weight factor that keeps the slack variable as small as possible. Smoothness of (18) is ensured if is the Euclidean distance, and and are either the standard cone or the second-order cone, see Remark 1 for details.

In the following sections, we illustrate our obstacle avoidance formulation on two applications: a quadcopter path planning problem where the point-mass formulation is used (Section 5), and an automated parking problem, where the full-dimensional obstacle avoidance problem formulation is used (Section 6).

5 Example 1: Quadcopter Path Planning

In this section, we illustrate reformulations (10) and (13) on a quadcopter navigation problem, where the quadcopter must find a path from one end of the room to the other end, while avoiding a low-hanging wall and passing through a small window hole, see Fig. 1.

Figure 1: Setup of quadcopter example together with a (locally optimal) point-to-point trajectory. The start position is behind the blue wall at , and the end position in front of red wall at . The quadcopter needs to fly below the blue wall and pass through a window in the red wall, see https://youtu.be/7WLNJhaHcoQ for an animation. The black circles illustrate the safety distance  m which models the shape of the quadcopter.

5.1 Environment and Obstacle Modeling

The size of the room is  m, and we see from Fig. 1 that the direct path between the start and end position is blocked by two obstacles. The first obstacle, a low-hanging wall, blocks the entire upper part of the room, and can only be passed from below. The second obstacle, another wall, blocks the entire room, but has a small window through which the quadcopter must pass to reach its target position. We approximate the shape of the quadcopter by a (Euclidean) sphere of radius 0.25 m. In the framework of (10) and (13), the shape of the quadcopter can be taken into account by requiring a safety distance of  m.

The first wall, which can only be passed from below, is placed at = 2 m, and the passage below is 0.85 m high. The second wall is placed at = 7 m, and the window (size m) is placed in the middle of the second wall at a height of  m. Finally the depth of both walls is 0.5 m. This obstacle formation can be formally formulated using five axis-aligned rectangles, where the first obstacle is represented by one such rectangle and the window can be modeled as the union of four rectangles, see Fig. 1. The collision avoidance constraints with respect to the four outer walls of the room are achieved by appropriately upper- and lower-bounding the coordinates of the quadcopter.

5.2 Quadcopter Model

We consider the standard quadcopter model as used in Mellinger2012 , which is derived by finding the equation of motion of the center of gravity (CoG) and summarized next. In this model, denote the position of the CoG in the world frame, and we use the -- Euler angles do describe the rotation of the quadcopter, where is the pitch angle, is the roll angle, and is the yaw angle. The rotation matrix that translates from the world to the body frame, which is defined with respect to the CoG, is hence given by

where and . The accelerations of the CoG can be derived by considering the sum of the forces produced by the four rotors which point in positive z-direction in the body frame, and the gravity force which acts on the negative z-direction in the world frame, resulting in the following equation of motion,

where is the mass of the quadcopter and , and are the second time derivatives of , and , respectively. The attitude dynamics of the quadcopter is derived in the body rates , , and which are related to the Euler angles through the following rotation matrix,
The body rates are given by the following equation of motion, which is driven by the four rotor forces

, as well as the corresponding moments

and has the following form,
where is the inertia matrix which in our case is diagonal and is the distance from the CoG to the rotor. The rotor forces and moments depend quadratically on the motor speed , which are the control inputs and are defined as follows,
where, and are constants depending on the rotor blades. Hence, the state of the quadcopter is and the inputs are the four rotor speeds .

The parameters of the model, as well as the bounds on the inputs, are taken from MellingerPhD , which corresponds to a quadcopter which weighs 0.5 kg and has a diameter of half a meter. Bounds on the angles, velocities and body rates are considered, and the dynamics can be brought into the form (1) using a (forward) Euler discretization, such that , where is the sampling time, and is the continuous-time dynamics that can be obtained from (19a)–(19d).

5.3 Cost function

Our control objective is to navigate the quadcopter as fast as possible, while avoiding excessive control inputs. We combine these competing goals as a weighted sum of the form , where is the final time and , and are weighting factors. Motivated by Fahroo2000 , we do not directly minimize ; instead, observing that , we will treat the discretization time as a decision variable. This allows the use of the slightly modified cost function


which will be used in the numerical simulations later on.

Treating as an optimization variable has the additional benefit that the duration of the maneuver does not need to be fixed a priori, allowing us to avoid feasibility issues caused by a too short maneuver lengths. We point out that having as a decision variable comes at the cost of introducing an additional decision variable , which renders the dynamics “more non-linear”, which can be seen when looking at the Euler discretization in the previous section.

5.4 Choice of Initial Guess

Recall that (10) and (13) are non-convex optimization problems, and hence computationally challenging to solve in general. In practice, one has to content oneself with a locally optimal solution that, for instance, satisfied the Karush-Kuhn-Tucker (KKT) conditions WachterIPOPT2006 , since most numerical solvers operate locally. Furthermore, it is well-known that the solution quality critically depends on the initial guess (“warm starting point”) that is provided to the solvers, and that different initial guesses can lead to different (local) optima. Unfortunately, computing a good initial guess is often difficult and highly problem dependent; ideally, the initial guess should be obstacle-free and approximately satisfy the system dynamics.

For the quadcopter example, we have observed that the well-known A algorithm is able to provide good initial guesses. A is a graph search algorithm that is able to find obstacle-free paths by gridding the position space. It is similar to Dijkstra’s algorithm, but uses a so-called heuristic function to perform a “best-first” search, see Hart1968 ; Hart1972 for details. In our quadcopter example, we use the A algorithm to find an obstacle-free path in the position space, which we use to initialize the states that correspond to the quadcopter’s position. The remaining states are initialized with zero, while inputs are initialized with the steady state input that keeps the quadcopter in a hoovering position. The dual variables are initialized with 0.05, and the discretization time with 0.25. Fig. 2 depicts the initial guess used to generate the trajectory shown in Fig. 1. Notice that, due to gridding, the path in Fig. 2 exhibits a zigzag pattern.

Figure 2: A sample warm start trajectory, obtained from the A algorithm, is shown. Notice that this trajectory avoids obstacles but does not satisfy the dynamic constraints of the quadcopter, which will be “corrected” by solving (10) and (13). The black tube is the safety distance which is used to represent the shape of the quadcopter.

5.5 Simulation Results

To verify the performance and robustness of our approach, we considered 36 path planning scenarios, each starting and ending in a hovering position. The starting point is always located at  m, and the finishing point is always located behind the wall with the window at  m, but with varying and coordinates. The final positions are generated by gridding the space with nine points in the direction and four points in the direction as shown in Fig. 3. We tested both the distance formulation (10) as well as the signed distance formulation (13). The horizon equals the number of steps performed by the algorithm, and takes values between 100 and 129 for the given setup and a grid size of 0.1 m. The sampling time is restricted to lie between  s and  s. The optimization problems are implemented with the modeling toolbox JuMP in the programming language Julia DunningHuchetteLubin2017 , and solved using the general purpose nonlinear solver IPOPT WachterIPOPT2006 . The problems are solved on a 2013 MacBook Pro with an i7 processor clocked at 2.6 GHz.

Table 1 lists the minimum, maximum and average computation time of the A algorithm, and the time required to solve problems (10) and (13). Fig. 3 reports the solution time as a function of the finishing position, where a circle indicates that IPOPT has successfully found a solution. We see from Fig. 3 that both the distance and signed distance formulation are able to compute all paths successfully. Interestingly, however, the computation time pattern of these two approaches are not correlated; in other words, a “difficult” scenario for the distance formulation might be “easy” for the signed distance formulation, and vice versa. In practice, this implies that to obtain feasible trajectories as fast as possible, the navigation problem should be solved with both obstacle avoidance formulations, and the first solution should be taken. In our setup, such an approach would result in a worst case computation time of 28.9 s, as opposed to 48.0 s and 59.1 s if the distance and signed distance reformulation are considered individually.

We close this section by pointing out that, with a maximum computation time of 2.8 s, the time for A to find an initial guess is considerably lower than that for solving the optimization problems, see Table 1. This is not surprising since A only plans a path in the -space and ignores the system dynamics which leads to zigzag behavior, see Fig. 2. A dynamically feasible path is only obtained after solving the optimal control problems (10) and (13) which, by explicitly taking into account system dynamics, smoothen and locally optimize the path provided by the A algorithm.

Figure 3: Solution time for quadcopter trajectory planning with distance formulation (10) (top) and signed-distance formulation (13) (right).
Quadcopter navigation min max mean
warm start (A) 0.5724 s 2.8157 s 1.6207 s
distance formulation (10) 4.6806 s 47.9762 s 14.9716 s
signed distance formulation (13) 4.7638 s 59.1031 s 14.3962 s
Table 1: Computation time of A, distance formulation (10) and signed distance formulation (13).

6 Example 2: Autonomous Parking

As a second application for our collision avoidance formulation, we consider the autonomous parking problem for self-driving cars. In contrast to the quadcopter case, modeling a car as a point-mass and then approximating its shape with a ball can be very conservative and prevent the car from finding a feasible parking trajectory, especially when the environment is tight. In this section, we model the car as a rectangle, and then employ the full-dimensional formulation described in Section 4. We show that our modelling framework allows us to find obstacle-free parking trajectories even in tight environments. Two scenarios are considered: reverse parking (Fig. 4) and parallel parking (Fig. 5).

Figure 4: Reverse parking maneuver. The controlled vehicle is shown in green at every time step. Vehicle starts on the left facing to the right, and ends facing upwards, see https://youtu.be/V7IUPW2qDFc for an animation.
Figure 5: Parallel parking maneuver. The controlled vehicle is shown in green at every time step. Vehicle starts facing to the right, and ends facing to the right, see https://youtu.be/FST7li4M6lU for an animation.

6.1 Environment and Obstacle Modeling

For the reverse parking scenario, the parking spot is assumed 2.6 m wide and 5.2 m long. The width of the road, where the car can maneuver in, is 6 m, see Fig. 4 for an illustration. For the parallel parking scenario, the parking spot is 2.5 m deep and 6 m long, and the space to maneuver is 6 m wide (Fig. 5). Note that the obstacles in the reverse parking scenario can be described by three axis aligned rectangles, while the obstacles in the parallel parking scenario can be described by four axis aligned rectangles. In both cases, the controlled vehicle is modeled as a rectangle of size  m, whose orientation is determined by the car’s yaw angle.

6.2 System Dynamics and Cost Function

The car is described by the classical kinematic bicycle model, which is well-suited for velocities used in typical parking scenarios. The states correspond to the center of the rear axes, while is the yaw angle with respect to the X-axis, and is the velocity with respect to the rear axes. The inputs are the steering angle and the acceleration . Hence, the continuous-time dynamics of the car is given by

where  m is the wheel base of the car. The steering angle is limited between  rad (approximately 34 deg), with rate constraints  rad/s; acceleration is limited to be between  m/s. We limit the car’s velocity to lie between and  m/s. Similar as in the quadcopter case, the continuous-time dynamics are discretized using a forward Euler scheme. Finally, the same cost function as in the quadcopter is used, i.e., a weighted sum between the discretization-time and control effort is considered.

6.3 Initial Guess

Similar to the previous example, the solution quality of the non-convex optimization problems heavily depends on the initial guess provided to the numerical solvers. Unfortunately, it turns out that the A algorithm used in the quadcopter example generally provides a poor warm start as it is unable to take into account the vehicle’s non-holonomic dynamics444Roughly speaking, A will return trajectories that would require the vehicle to move sideways. Simulations indicate that the numerical solvers are typically not able to “correct” such a behavior and unable to recover a feasible solution when initialized with A.. To address this issue, we resort to a modified version of A, called Hybrid A