Simultaneous Spatial and Temporal Assignment for Fast UAV Trajectory Optimization using Bilevel Optimization

Trajectory planning has long been a critical problem in robotics. As trajectory planning affects the quality of robots’ motion to a great extent, people have long been investigating this problem for various kinds of robots [1]. Pioneered by Mellinger et al. [2], the minimum-snap method has been dominantly used in trajectory planning of UAVs with differentially flat dynamics (e.g., quadrotors). Such a method takes the snap of the whole trajectory as cost and generates the planning problem in the form of a quadratic program (QP). Meanwhile, various equality or inequality constraints with different requirements on the trajectory can be added to the QP, making it convenient for addressing more complex tasks.

One of the most important features of a trajectory planning algorithm is that it needs to quickly generate a feasible trajectory for the given UAV. The computational efficiency is not a significant issue if the optimization problem is convex, e.g., the minimum-snap formulation [2] with fixed time allocation to the adjustable waypoints.

However, the optimization problem normally becomes non-convex when the time allocation is included as an optimization variable. Since time allocation can largely affect the quality of the planned trajectory, researchers are searching for more efficient ways to optimize time allocation for trajectory planning. Early trials have used heuristics [3] and decoupling methods [4], although they both can lead to inefficient trajectories and burdensome computation time. Mellinger et al. [2] propose to solve this problem by gradient descent, which can be seen as the prototype of the bilevel optimization method, though the finite difference is used to approximate the gradient. Sun et al. [5] finalize the idea of using bilevel optimization to determine time allocation using analytical gradients. In their design, the lower-level problem optimizes the trajectory with fixed time allocation, which is generated by the upper-level problem that optimizes the time allocation using analytical gradients.

Another feature that burdens computational efficiency is the requirement of collision avoidance. To fulfill the collision-free requirement, a flight corridor, which is a space with safety assurance, must be specified in advance. Ways to plan a trajectory with flight corridors vary, e.g., sampling method [2], two-staged planning strategy [6], Bernstein polynomials [7, 5]. Notwithstanding, all of the methods mentioned above are characterized as inequality constraints, introducing burdens on QP’s computation time as a QP can be solved in a much shorter time when equality constraints are involved only.

We propose a bilevel optimization framework to plan a trajectory subject to variable time allocation and safety-set constraints. Our formulation is similar to that of [5]: the lower-level problem is convex and solved for the global minimum; the upper-level problem is non-convex, and a new “solution” is obtained by one-step gradient descent to reduce the optimization cost. The uniqueness of our approach is that we exclude the inequality constraint for the safety set in the lower-level problem, making it a QP with equality constraints only. Specifically, the feasible waypoints in the safety set originally characterized by the inequality constraints are fixed in the lower-level problem. Such a formulation can significantly reduce the computation time on the lower-level QP since it can be turned into an unconstrained optimization problem. Correspondingly, the upper-level problem will adjust the time allocation and the feasible waypoints using analytical gradients that are efficiently obtained via OptNet [8]. Compared with the upper-level problem formulation in [5], where only the time allocation is updated via analytical gradients, the extra computation time to obtain the gradient for the spatial update (of the feasible waypoints) is negligible. This comparison concludes the major reason why our formulation can significantly reduce the computation time of that in [5], where we validated the comparison in simulations with a scalability test. While both methods minimize the cost to a similar level, our approach’s computation time scales linearly with the number of waypoints, whereas the one by [5] scales exponentially.

Our contributions are summarized as follows: 1. We reformulate the bilevel optimization in [5] by excluding the inequality constraints in the lower-level problem, which leads to drastically reduced computation time for an optimal solution. 2. We use analytical gradients in the upper-level problem to simultaneously update the spatial and temporal assignment, which is more accurate and efficient than using finite-difference methods or other numerical approximations.

The remainder of the paper is organized as follows: Section II reviews the related work. Section III introduces the background of trajectory planning and the existing bilevel optimization formulation. Section IV shows our formulation of the bilevel optimization, with simulations results given in Section V showing the advantage of our method in drastically reducing the computation time. Finally, Section VI summarizes the paper and discusses future work.

We use piecewise polynomials of time to parameterize the UAV trajectories. Since the differential flatness of UAVs’ dynamics excludes the necessity of explicitly enforcing dynamics (e.g., quadrotors [2]), we can characterize the trajectories as a smooth curve in the space of flat outputs $\bm{\sigma}:[t_{0},t_{m}]\rightarrow\mathbb{R}^{3}\times SO(2)$ as $\bm{\sigma}(t)=[x(t),y(t),z(t),\psi(t)]^{\top}$, which contains the coordinates of the vehicle’s center of mass in the world coordinate system $\bm{r}=[x,y,z]^{\top}$ and yaw angle $\psi$. Consider $m$ piecewise polynomials that characterize the entire trajectory, where each piece is an $n$-th order polynomial, i.e., for $i\in\{1,2,\dots,m\}$,

The problem is to find the polynomial coefficients $\{\bm{\sigma}_{ik}\}_{i\in\mathcal{I},k\in\mathcal{K}}$ and the temporal assignment $\bm{T}=[t_{1},t_{2},\dots,t_{m-1}]^{\top}$ such that the following cost function is minimized

where $k_{r}$ and $k_{\psi}$ refer to the order of derivative of the coordinate $\bm{r}$ and yaw angle $\psi$, respectively. For the minimum-snap cost in [2], $k_{r}=4$ and $k_{\psi}=2$ are used. The constraints include convex set $\mathcal{C}_{i}\subseteq\mathbb{R}^{3}$ of adjustable waypoints at time instances, i.e.,

The sets $\{\mathcal{C}_{i}\}_{i=0:m}$ serve as one type of safety constraint, e.g., each set can be a corner connecting two corridors such that the UAV must pass through the set to ensure safety. (The usage of Bernstein polynomials can characterize the corridors as safety sets [7, 5], which is out of the scope of this paper and will not be discussed.)

Without loss of generality, the feasible sets $\mathcal{C}_{i}$’s are polytopes, yielding simple characterization by linear inequalities. The continuity between adjacent polynomials is enforced by

for $i\in\{1,2,\dots,m-1\}$, $k_{1}\in\{0,1,\dots,k_{r}\}$, and $k_{2}\in\{0,1,\dots,k_{\psi}\}$. Finally, the temporal constraint requires the time allocation to satisfy

The optimization problem seeks to find the polynomial coefficients $\bm{\sigma}$ and temporal assignment $\bm{T}$ that minimize the objective function in (2) subject to the constraints in (3)–(5). Since polynomials are highly structured basis functions with coefficients captured by $\bm{\sigma}$, the problem can be conveniently presented as

where $\bm{\sigma}$ denotes a permutation of all polynomial coefficients $\{\bm{\sigma}_{ik}\}_{i\in\mathcal{I},k\in\mathcal{K}}$. The inequality constraint $G(\bm{T})\bm{\sigma}\preceq\bm{h}$ associates with the safety-set constraint (3), whereas the equality constraint $A(\bm{T})=\bm{b}$ associates with the continuity constraints (4). All the coefficients $P(\bm{T})$, $\bm{q}(\bm{T})$, $G(\bm{T})$, $\bm{h}$, $A(\bm{T})$, and $\bm{b}$ are derived based on the objective function (2) and the constraints (3) and (4). The matrix $P(\bm{T})$ is positive semi-definite.

In general, the problem (P) is a non-convex problem. Therefore, bilevel optimization has been widely applied to solve the problem (P), e.g., [2, 10, 5]. The bilevel optimization has an upper-level problem:

where the $\mathcal{F}$ denotes the feasible set of $\bm{\sigma}$ such that $\mathcal{F}=\{\bm{\sigma}:G(\bm{T})\bm{\sigma}\preceq\bm{h},A(\bm{T})\bm{\sigma}=\bm{b}\}$. Embedded in the upper-level problem, the lower-level problem is

Note that in the lower-level problem, the time assignment $\bm{T}$ is a parameter of the problem with a known value (obtained via solving (6)). Hence, (7) reduces to the minimum-snap formulation in [2] and turns into a quadratic program, which is convex and efficiently solvable. The challenges come from the upper-level optimization (6), which is non-convex. A typical approach in solving (6) is to use gradient descent, where the temporal assignment $\bm{T}$ is iteratively updated. In [2], the descent direction is obtained by computing the directional derivative along unit vectors. In [5], the descent direction is the gradient of the optimal lower-level cost $\nabla_{\bm{T}}J^{*}(\bm{\sigma}^{*}(\bm{T}),\bm{T})$, which is computed as an analytical gradient by implicit differentiation of the KKT condition of the low-level problem (7).

In this section, we describe our reformulation of the bilevel-optimization problem based on [5]. Our reformulation can drastically reduce the computation time to that of [5] by reducing the computation time of the lower-level QP problem. Such a QP problem is reported in [5] to dominate the computation time despite being convex. We keep equality constraints only in the lower-level problem, which turns into an unconstrained problem that can be solved in a shorter time.

Our formulation starts by introducing a new optimization variable, which allows decomposing the inequality constraint $G(\bm{T})\bm{\sigma}\preceq\bm{h}$ into an equality constraint and an inequality constraint. Specifically, let the adjustable waypoint $\bm{\xi}_{i}\in\mathbb{R}^{3}$ be the new optimization variable such that $\bm{\xi}_{i}\in\mathcal{C}_{i}$ for $i\in\{0,1,\dots,m\}$. Then the constraint (3) breaks down to

for $i\in\{1,\dots,m\}$. Correspondingly, the inequality constraint $G(\bm{T})\bm{\sigma}\preceq\bm{h}$ turns into

where $\bm{\xi}^{\top}=[\bm{\xi}_{0}^{\top}\ \bm{\xi}_{1}^{\top}\ \dots\ \bm{\xi}_{m}^{\top}]$. The matrix $C(\bm{T})$ evaluates the positions of the trajectory on the allocated time instances in $\bm{T}$, whereas the inequality $R\bm{\xi}\preceq\bm{s}$ is the half-space representation of the polytopes $\{\mathcal{C}_{i}\}_{i=0:m}$. With the newly introduced variable and constraints, problem (P) turns into

The introduction of the new variable $\bm{\xi}$ and additional constraints preserves the equivalence between (P) and (AP): from a solution of one problem, a solution of the other is readily found, and vice versa [17]. Similar to (P), problem (AP) also can be solved using bilevel optimization. However, the formulation of (AP) permits only keeping the equality constraints in the lower-level optimization:

where the spatial and temporal assignments, $\bm{\xi}$ and $\bm{T}$, are generated from the upper-level optimization:

Here, the set $\mathcal{F}(\bm{\xi},\bm{T})$ denotes the feasible set of $\bm{\sigma}$ such that $\mathcal{F}(\bm{\xi},\bm{T})=\{\bm{\sigma}:C(\bm{T})\bm{\sigma}=\bm{\xi},A(\bm{T})\bm{\sigma}=\bm{b}\}$ and $\mathcal{X}$ denotes the feasible waypoints such that $\mathcal{X}=\{\bm{\xi}:R\bm{\xi}\preceq\bm{s}\}$.

A few notes follow our construction of the lower-upper-level problems. First, the lower-level optimization is still a convex problem: a quadratic program with equality constraints only. The insight here is to significantly reduce the computation time of the quadratic program by keeping the equality constraints only. Such a problem can be reduced to an equivalent unconstrained problem by introducing Lagrangian multipliers, which reduces to solving a system of linear equations  [17]. Second, the upper-level problem, despite different forms than in [2, 10, 5], is generally hard to solve due to its non-convexity. Coordinate-descent may be applied to successively solve (LLP) and (ULP) in a row despite the fact that the computation time can be forbidden for real-time planning. Instead, we use gradient-descent on (ULP) to only update on $\bm{\xi}$ and $\bm{T}$ once (LLP) is solved. Consequently, the updated values of $\bm{\xi}$ and $\bm{T}$ are plugged into (LLP) as parameters. In order to update $\bm{\xi}$ and $\bm{T}$, we need $\partial J^{*}/\partial\bm{\xi}$ and $\partial J^{*}/\partial\bm{T}$ to update $\bm{\xi}$ and $\bm{T}$ by

where $\alpha_{1},\alpha_{2}>0$ are the user-selected step sizes and $P_{\mathcal{Y}}(y)$ denotes the projection operator [18] that projects $y$ onto the set $\mathcal{Y}$. The step sizes $\alpha_{1}$ and $\alpha_{2}$ dominate the update of the spatial assignment and time allocation, respectively. Since the two variables $\bm{\xi}$ and $\bm{T}$ in space and time have totally different physical units, individual step sizes are needed for efficient parameter updates. For the gradient descent, the key is to obtain the partial derivatives $\partial J^{*}/\partial\bm{\xi}$ and $\partial J^{*}/\partial\bm{T}$, which stand for the sensitivity of the optimal cost of (LLP) to $\bm{\xi}$ and $\bm{T}$, respectively. We use OptNet [8] to obtain these two gradients. Specifically, the OptNet allows computing the analytical gradient of the optimal cost with respect to the coefficients of a QP (e.g., $P(\bm{T})$, $\bm{q}(\bm{T})$, $C(\bm{T})$, $A(\bm{T})$, $\bm{\xi}$, and $\bm{b}$ in (LLP)) by implicitly differentiating the KKT condition. Using the method enabled by OptNet, we can directly obtain

Using chain rule, we can obtain

where $\frac{\partial P(\bm{T})}{\partial\bm{T}}$, $\frac{\partial\bm{q}(\bm{T})}{\partial\bm{T}}$, $\frac{\partial C(\bm{T})}{\partial\bm{T}}$, $\frac{\partial A(\bm{T})}{\partial\bm{T}}$ are easy to compute since $P(\bm{T})$, $\bm{q}(\bm{T})$, $C(\bm{T})$, $A(\bm{T})$ are explicit functions of $\bm{T}$. We summarize our solution method to (AP) in Alg. 1.

All the simulations shown in this section are implemented using MATLAB R2021b on a computer with a 2.20 GHz Intel i7-8750H CPU. We use quadprog as the QP solver, in which we choose the interior-point method instead of the active-set method in quadprog because the former provided better solution quality with shorter computation time than the latter in our simulations. This observation is consistent with the solver comparison shown in [5]. We compare our method with that of [5] (referred to as “compared method” in this section) in three experiments. In the first one, we fix the number of adjustable waypoints and show a breakdown of the computation time. In the second one, we conduct a scalability experiment to show how the computation time scales with the number of adjustable waypoints. In the third one, we fix the number of adjustable waypoints and include the dynamic constraints. Note that the number of adjustable waypoints equals the number of safety sets as indicated in (8).

We present a novel method of UAV trajectory planning based on bilevel optimization by simultaneously conducting spatial and temporal assignments. Our bilevel optimization is composed of a lower-level problem that is a QP with equality constraints only (which contributes majorly to the reduced computation time) and an upper-level problem that uses analytical gradients to make the spatial and temporal update on the adjustable waypoints. Simulation results show that our method has great advantages in computation time compared to the existing bilevel optimization method, where the former scales linearly and the latter scales exponentially to the number of adjustable waypoints. Future work will deploy our method on a real quadrotor to demonstrate its capability for onboard and real-time trajectory planning in complex environments.