DIRECT: A Differential Dynamic Programming Based Framework for Trajectory Generation

The seminal work [4] firstly proved the differential flatness for quadrotor and proposed the minimum snap piecewise polynomial trajectory generation problem, which can be transformed into a quadratic programming (QP) problem with safety corridors being included as linear constraints on discretized points and hence can be solved efficiently via solver. The time allocation and dynamical feasibility problems were approached by projected gradient with backtracking line search in an outer loop and temporal scaling, respectively. Later, the authors in [5] proposed a closed-form solution for the trajectory generation problem with a generalized cost where the time allocation, safety, and dynamical feasibility aspects are handled by gradient descent of joint energy-time cost, addition of waypoints, and temporal scaling, respectively. In [8], the authors presented an efficient safe flight corridor construction method and a temporal scaling step (similar to [5]) on time allocation to generate safe and dynamical feasible trajectories. To avoid high nonlinearity related to joint space-time optimization, some recent works propose methods that iteratively optimize spatial and time variables in two separate subroutines. The authors in [6] introduced a spatial-temporal optimization method which iteratively refines the geometrical coefficients and time-warping functions via QP and second order cone program (SOCP), respectively. In [9], the authors decomposed the trajectory optimization problem as a bi-level optimization problem where the gradient required in the outer loop is analytically obtained from the dual solution of the inner loop QP. An alternating minimization scheme was proposed in [10] where complex safety constraints were not considered. Recently in [11], the authors formulate an unconstrained nonlinear optimization that can be solved by quasi-Newton methods efficiently, but the safety and dynamical feasibility are enforced as penalty functions on discretized points instead of hard constraints. In terms of large scale trajectory ($\geq 10^{3}$), the authors in [12] proved that the minimum snap problem with fixed time allocation can be solved in linear complexity by exploiting the structure of optimality conditions. The computational complexity has been further improved in [13] by eliminating a matrix inversion operation. An outer loop with analytical projected gradient was introduced for [12] in [14] such that the time allocation can be optimized. However, the computational complexity is unspecified for the case with safety and dynamical feasibility constraints.

The DDP algorithm firstly introduced in [15] enjoys linear computational complexity (w.r.t. horizon) and local quadratic convergence [16] and has been applied in trajectory optimization of various systems, e.g., biped walking [17] and robotic manipulation [18]. Much research has been devoted to generalizing the DDP to the case with inequality constraints and can be generally classified into two categories. The first category converts the constrained problems to unconstrained ones via penalty [19] while the other deals with the constraints explicitly by identifying the active ones [20]. To avoid the combinatorial problem regarding the active constraints, a constrained version of Bellman’s principle of optimality has been introduced in [7, 21], which augments the control input with dual variables. However, these DDP algorithms have only been applied to a short-duration (e.g. $2\sim 3\mathrm{s}$, $200\sim 300$ steps of sampling period $0.01\mathrm{s}$) quadrotor planning problem and takes quite long time for simple constraints ($\geq 5\mathrm{s}$, see Table 7 in [21]). In this paper, we shall exploit the differential flatness of quadrotors and the linear complexity property of DDP algorithm to generate long-duration smooth trajectories efficiently.

We consider the problem of generating an $N$-segment $n$-th order polynomial trajectory in $d$-dimensional space with the $k$-th segment $\mathbf{p}_{k}\in\mathbb{R}^{d}$ being parametrized by:

where $\mathbf{C}_{k}\in\mathbb{R}^{(n+1)\times d}$ is the coefficient matrix and $\mathbf{b}(t)=[1,t,\dots,t^{n}]^{\top}$ is the $n$-order monomial basis. Hence, the entire trajectory on $[0,\tau_{N-1}]$ can be defined with

for $t\in[\tau_{k-1},\tau_{k}]$ where $\tau_{-1}=0$ and $\tau_{k}=\sum_{i=0}^{k}t_{k}$, $k\in\mathcal{I}_{N}$.

Following [4, 13, 11], we consider the problem of enforcing the smoothness of trajectory up to the $(m-1)$-th ($m=\frac{n+1}{2}$) order derivative and optimizing on the energy cost up to the $m$-th order derivative.

We consider a single segment $k$. By (1), the collection of trajectory up to $(m-1)$-th order derivative can be written as follows:

Collecting both the endpoints, one has:

where $\mathbf{F}_{k}$ can be expressed explicitly with a $2$-by-$2$ block matrix [13], i.e., $\mathbf{F}_{k}=\left[\begin{smallmatrix}\mathbf{F}_{k,11}&\mathbf{0}\\ \mathbf{F}_{k,21}&\mathbf{F}_{k,22}\\ \end{smallmatrix}\right]$, where each block is of size $m\times m$, and

Next, we partition $\mathbf{C}_{k}$ according to the block structure of $\mathbf{F}_{k}$, i.e., $\mathbf{C}_{k}=[\mathbf{C}_{k,1}^{\top},\mathbf{C}_{k,2}^{\top}]^{\top}$ and obtain the following equality:

where $\mathbf{C}_{k,1}$ was substituted with $\mathbf{F}_{k,11}^{-1}(\mathbf{p}_{k}^{[m]}(0))^{\top}$.

Letting $\mathbf{x}_{k}=\operatorname{vec}(\mathbf{p}_{k}^{[m]}(0))$, $\mathbf{v}_{k}=\operatorname{vec}(\mathbf{C}_{k,2}^{\top})$, $\mathbf{u}_{k}=[\mathbf{v}_{k}^{\top},t_{k}]^{\top}$ and by the continuity condition $\mathbf{p}_{k}^{[m]}(t_{k})=\mathbf{p}_{k+1}^{[m]}(0)$, one has

where $\mathbf{A}(t_{k})=(\mathbf{F}_{k,21}\mathbf{F}_{k,11}^{-1})\otimes\mathbf{I}_{d}$ and $\mathbf{B}(t_{k})=\mathbf{F}_{k,22}\otimes\mathbf{I}_{d}$. As shown in Fig. 2, this operation transfers the original representation of trajectory into a state-space equation, where the system state is the state (position, velocity, acceleration, etc.) at each endpoint and the control input is a concatenation of half of the coefficients of the segment (i.e., $\mathbf{C}_{k,2}$) and duration (i.e., $t_{k}$). Unlike previous application of DDP algorithms in quadrotor planning and control where direct discretization is used, this representation enables the characterization of a long-duration trajectory with fewer number of variables.

Previous results on generating polynomial trajectories (e.g., [4, 13]) usually consider the so-called interpolating splines [22], where the spline must pass some prescribed waypoints exactly at specific times. However, for the trajectory generation problem for time-critical aggressive flights, quite often it is sufficient that the generated trajectories only pass through the vicinity of these waypoints, i.e., smoothing splines [23]. In this paper, we aim to find a smoothing spline such that the following objective function can be minimized:

where $\mathcal{C}:=\{\mathbf{C}_{k}\}_{k\in\mathcal{I}_{N}}$ and $\mathcal{T}:=\{t_{k}\}_{k\in\mathcal{I}_{N}}$. In this cost function, the first term is designed to minimize the discrepancy between the current state $\mathbf{x}_{k}$ and the desired state $\mathbf{x}_{k,\mathrm{g}}$ at the beginning of each segment, which “attracts” the endpoint to a prescribed waypoint; the second and third terms penalize the aggressiveness of planned trajectory up to the $m$-th order and flight time during the $k$-th segment, respectively; the last term penalizes the discrepancy between the terminal state $\mathbf{x}_{N}$ and the goal state $\mathbf{x}_{N,\mathrm{g}}$; and $\mathbf{Q}_{k}$, $\eta_{i,k}$, $w_{k}$, ($k\in\mathcal{I}_{N}$), and $\mathbf{Q}_{N}$ denote the weights for each term. It can be found that different from the literature (e.g., [4, 13]) where hard constraints for the endpoints of $\mathbf{p}_{k}$ were used, we have softened these constraints by incorporating them into an objective function.

For usually considered minimum jerk or minimum snap trajectories, $\mathbf{Q}_{k}=\mathbf{0}$, $\eta_{i,k}=0$ for all $i=1,\dots,m-1$ and $k\in\mathcal{I}_{N}$. In this case, one can partition $\mathbf{b}^{(m)}(t)(\mathbf{b}^{(m)}(t))^{\top}$ into $2$-by-$2$ block matrix with the size of each block being $m\times m$.

Observing that only the right-bottom block is non-zero and denoting it by $\mathbf{R}_{\mathbf{v}}(t)$, one can rewrite (7) as

with $\mathcal{U}:=\{\mathbf{u}_{k}\}_{k\in\mathcal{I}_{N}}$ and $\mathbf{R}_{k}=\operatorname{diag}(\eta_{m,k}\int_{0}^{t_{k}}\mathbf{R}_{\mathbf{v}}(t)\differential t,w_{k})$.

In general, there are two types of methods handling the safety and dynamical feasibility constraints. The first method discretizes each segment of trajectory and adds inequality constraints on the states at these time instants, while the issue is that the feasibility for the continuous time interval cannot be guaranteed. The other method is to add constraints on a set of control points, which are the vertices of the convex hull containing the segment, but it suffers from conservativeness. A recent work [24] proposes the MINVO basis, which has been shown to be less conservative than the widely used Bezier basis. We use the MINVO basis to construct a set of control points for our polynomial trajectory. Denote the $i$-th order control points by $\bm{\Xi}_{k}^{\langle i\rangle}\in\mathbb{R}^{(n+1-i)\times d}$, which can be computed by:

where $\mathbf{T}^{\langle i\rangle}(t_{k})\in\mathbb{R}^{(n+1-i)\times(n+1)}$ represents the transformation matrix from polynomial coefficients to the $i$-th order control points.

Assuming that a safe flight corridor (consists of $N$ convex polyhedra) can be generated from perception system and polyhedron $k$ is represented by $s_{k}$ hyperplanes. With (9), the safety constraint can be written as:

where each row of $\mathbf{W}_{k}\in\mathbb{R}^{s_{k}\times d}$ and $\mathbf{h}_{k}^{\langle 0\rangle}\in\mathbb{R}^{s_{k}}$ corresponds to the parameters of a single hyperplane in polyhedron $k$.

On the other hand, the dynamical feasibility is encoded by the following box constraint on the $i$-th order control points $\bm{\Xi}_{k}^{\langle i\rangle}$ with $i\in[1,m-1]$:

where $\underline{h}_{k}^{\langle i\rangle}<0$ and $\overline{h}_{k}^{\langle i\rangle}>0$ are the lower and upper bounds (e.g., minimal / maximal velocity / acceleration), respectively.

Additionally, one more inequality constraint on time can be introduced to avoid negative time allocation during optimization:

where $\underline{t}\in\mathbb{R}_{+}$.

Concatenating the vectorized forms of (10) and (11) with (12), one has

where

and

With the objective function and constraints constructed above, we can formulate the trajectory generation problem as the following multi-stage constrained optimal control problem:

It should be noted that the constraint and objective function in (14) are respectively in linear and quadratic forms, in fact, all the matrices involved are functions of $t_{k}$ and hence they are not linear-quadratic. Therefore, we shall solve the formulated problem via IPDDP [7], which generalizes the traditional unconstrained DDP to a constrained version. Similar to DDP, the IPDDP algorithm iterates between backward pass, which computes control inputs to minimize a quadratic approximation of cost in the vicinity of the current trajectory, and forward pass, which updates the current trajectory to a new one, until some terminating condition is triggered.

Define $V_{N}=\|\mathbf{x}_{N}-\mathbf{x}_{N,\mathrm{g}}\|_{\mathbf{Q}_{N}}^{2}$. Let $\ell_{k}=\|\mathbf{x}_{k}-\mathbf{x}_{k,\mathrm{g}}\|_{\mathbf{Q}_{k}}^{2}+\|\mathbf{u}_{k}\|_{\mathbf{R}_{k}}^{2}+\bm{\lambda}_{k}^{\top}\mathbf{g}_{k}$, and $L_{k}=\ell_{k}+V_{k+1}$ for all $k\in\mathcal{I}_{N}$, where $\bm{\lambda}_{k}\succeq\mathbf{0}$ is the Lagrange multiplier. In the following, we shall omit subscript $k$ and denote $(\cdot)_{k+1}$ by $(\cdot)^{+}$ for clarity. Letting $(\cdot)_{\mathbf{a}}:=\partialderivative{(\cdot)}{\mathbf{a}}$ and $(\cdot)_{\mathbf{ab}}:=\partialderivative{(\cdot)}{\mathbf{a}}{\mathbf{b}}$, one has the following iteration which resembles traditional DDP:

where $\odot$ denotes the tensor contraction and $\mathbf{f}$ is defined in (6). In addition to the decision variable $\delta\mathbf{u}_{k}$ (i.e., the first order variation of $\mathbf{u}_{k}$), the primal-dual version of DDP introduces the additional decision variable $\delta\bm{\lambda}_{k}$. Both $\delta\mathbf{u}_{k}$ and $\delta\bm{\lambda}_{k}$ can be represented in an affine form of $\delta\mathbf{x}_{k}$ by solving the optimality conditions of an approximation of constrained Bellman equation (see $(6)$ in [7]), i.e.,

where $\mathbf{k}^{\mathbf{u}}$, $\mathbf{K}^{\mathbf{u}}$, $\mathbf{k}^{\bm{\lambda}}$ and $\mathbf{K}^{\bm{\lambda}}$ are the update gains for $\delta\mathbf{u}_{k}$ and $\delta\bm{\lambda}_{k}$, respectively; $\mathbf{r}=\bm{\Lambda}\mathbf{g}+\mu\mathbf{1}$, $\mathbf{G}=\operatorname{diag}(\mathbf{g})$, $\bm{\Lambda}=\operatorname{diag}(\bm{\lambda})$ and $\mu$ is a perturbation constant.

With these at hand, the gradient and Hessian of $V_{k}$ can be updated by

with the following equation:

where $\delta\bm{\lambda}_{k}$ was eliminated.

The forward pass resembles the traditional DDP while updating both the control input $\mathbf{u}_{k}$ and the dual variable $\bm{\lambda}_{k}$:

where $\mathbf{x}^{\dagger}$, $\mathbf{u}^{\dagger}$, and $\bm{\lambda}^{\dagger}$ denote the new state, control variable, and dual variable, respectively.

Algorithm 1 presents the entire algorithm for generating a polynomial trajectory using IPDDP. It should be noted that the presented algorithm requires a feasible initial trajectory. This can be generated from a similar adaptation of the Infeasible-IPDDP algorithm in [7].

It can be easily found that the above algorithm can be used for constrained polynomial trajectory generation with preallocated time. In particular, letting $t_{k}=\operatorname{const}$ and redefining $\mathbf{u}_{k}=\mathbf{v}_{k}$ for all $k\in\mathcal{I}_{N}$, $\eqref{eq:state}$ turns to be a linear-time-varying system and $\eqref{eq:cons}$ is a linear-time-varying constraint w.r.t. $[\mathbf{x}_{k}^{\top},\mathbf{v}_{k}^{\top}]^{\top}$²²2The “time-varying” is in the sense of $k$.. The problem can be formally written as:

where $\mathcal{V}:=\{\mathbf{v}_{k}\}_{k\in\mathcal{I}_{N}}$. In this case, Algorithm 1 can still be used for solving such type of problem³³3It is also a typical linear-time-varying model predictive control (MPC) problem, which can be solved by existing MPC algorithms., with all $\ell$ and $\mathbf{f}$ related terms in (15) being precomputed and saved during the initialization stage instead of being reevaluated at each iteration.

If neither safety nor dynamical feasibility constraint is considered, one has the following problem:

We show that (21) is indeed a LQT problem by proving that $\mathbf{R}_{k}\succ\mathbf{0}$. Firstly, the positive semi-definiteness $\mathbf{R}_{k}=\eta_{m,k}\int_{0}^{t_{k}}\mathbf{R}_{\mathbf{v}}(t)\differential t$ can be easily established by the linearity of integral. Secondly, denoting the last $m$ elements of $\mathbf{b}^{(m)}(t)$ by $\bm{\sigma}(t)$, one has

On the other hand, $\bm{\tau}$ can be written as $\bm{\tau}=e^{\underline{\mathbf{A}}^{\top}t}\mathbf{e}_{1}$ with $\underline{\mathbf{A}}=\left[\begin{smallmatrix}\mathbf{0}_{m-1}&\mathbf{I}_{m-1}\\ 0&\mathbf{0}_{m-1}^{\top}\end{smallmatrix}\right]$ and $\mathbf{e}_{1}=[1,\mathbf{0}_{m-1}^{\top}]^{\top}$. Since the pair $(\underline{\mathbf{A}},\mathbf{e}_{1}^{\top})$ is observable, the matrix $\int_{0}^{\tau}e^{\underline{\mathbf{A}}^{\top}t}\mathbf{e}_{1}\mathbf{e}_{1}^{\top}e^{\underline{\mathbf{A}}t}\differential t$ is nonsingular for any $\tau>0$. Therefore,

is positive definite and this shows that (21) is an LQT problem.

Let $\mathbf{P}_{N}=\mathbf{Q}_{N}$. The backward iteration for the resulting LQT problem reduces to

where $\mathbf{K}_{k}$ and $\mathbf{k}_{k}$ are the parameters for expressing the new control inputs as a linear form of current state (resembles $\mathbf{k}^{\mathbf{u}}$ and $\mathbf{K}^{\mathbf{u}}$ in (16)). The forward pass reads:

which iteratively computes the control input and next state.

Algorithm 2 summarizes the above iteration and this gives the analytical solution to the unconstrained fixed-time polynomial trajectory generation problem (21). It should be noted that Algorithm 2 can be regarded as a simplified version of Algorithm 1 with exact one iteration.

For trajectory generation with time optimization as well as safety and dynamical feasibility constraints, we compare our algorithm with the alternating spatial-temporal optimization[6] and bi-level optimization with analytic gradient[9].⁴⁴4According to Fig. 9 of [11], these two algorithms are the state-of-the-art methods for generating a safe and dynamical feasible trajectory with time optimization. The code for [11] is currently unavailable. In [6] the energy and time costs are separately optimized while in [11] the overall cost function takes a similar form as ours, but without the terminal cost and the time cost takes the form of $w_{k}t_{k}$. The benchmark is conducted using an i7-8550U CPU. Our algorithm is implemented in three steps: $1)$ initialize our algorithm with Infeasible-IPDDP [7] and zero initial condition to find a feasible solution with $w_{k}=1$ and $\mathbf{Q}_{N}=\mathbf{I}$; $2)$ feed the obtained solution into Algorithm 1 with $w_{k}=20$ and $\mathbf{Q}_{N}=100\mathbf{I}$ to jointly optimize energy and time; and $3)$ feed the obtained solution into Algorithm 1 with fixed time allocation setting (see Section IV-A). The algorithm in [6] is implemented as it is (the coefficient of time is set as $10$). The algorithm in [9] is implemented in C++ with MOSEK⁵⁵5https://www.mosek.com/ solver for the inner loop QP and backtracking line search for the outer loop and the coefficient of time is set as $20$. We randomly generate $10$ paths with $64$ safety flight corridors (the facet number ranges from $6$ to $117$, and is $28$ on average) and extract $2\sim 64$ of them to implement these three algorithms. The initial allocation time is set by the subroutine in [6]. We consider the problem of generating minimum jerk trajectory with $n=5$, where the maximal velocity and acceleration are set as $2\mathrm{m/s}$ and $2\mathrm{m/s^{2}}$, respectively. The success rate versus number of segments is shown in Fig. 3. The success rate is dropping in general with the increasing number of segments. The success rate of our algorithm is marginally higher than Gao’s in some cases since the terminal constraint is softened in our formulation. As a result of terminal constraints and enforcing the dynamical feasibility via control points in inner QP, Sun’s method has the lowest success rate. The overall computation time and the reduction rate of the flight time (computed by $[\sum_{k}^{N-1}(t_{k}^{\mathrm{ini}}-t_{k}^{\ast})]/\sum_{k}^{N-1}t^{\mathrm{ini}}_{k}$, where $t_{k}^{\mathrm{ini}}$ is the initial allocated time for segment $k$ and $t_{k}^{\ast}$ is the optimized allocated time computed by each algorithm) and the control effort are also compared in Fig. 3. It can be found that for middle number of segments ($N\leq 40$), our algorithm consumes less time than the other two algorithms. For $N>40$, our algorithm still consumes less time than Gao’s while more than Sun’s. The reason is that the outer loop of Sun’s method terminates in only a few iterations and gets stuck in local minimum in these cases, which can be seen from the relatively low reduction rate in Fig. 3. In addition, there is a trade-off between flight time and control effort: a shorter flight time (i.e., higher reduction rate) corresponds to a higher control effort. Unlike Gao’s method which has increasing reduction rate and control effort w.r.t. $N$, our algorithm maintains relatively consistent reduction rate and level of control energy. Since our formulation also incorporates the terminal cost term (see (14)), the change of this term affects the other two terms and hence causes the fluctuation of control energy in our approach, as observed in Fig. 3. Overall, for the application of trajectory generation with $N\in[10,30]$, our algorithm achieves comparable results with Gao’s method while consumes less runtime.

For trajectory generation with fixed time allocation and without constraint, we compare our Algorithm 2 with [13]. Our parameters are set as $\eta_{m,k}=10^{-5}$, $\mathbf{Q}_{k}=100\mathbf{I}$, $k\in\mathcal{I}_{N}$, $\mathbf{Q}_{N}=100\mathbf{I}$. We randomly sample $100$ times for each number of waypoints and run $4$ algorithms ($2$ for minimum jerk trajectory and $2$ for minimum snap trajectory). The average runtime versus number of segments is shown in Fig. 4. It can be found that all the algorithms enjoy linear complexity w.r.t. number of segments. Although our algorithm for minimum jerk trajectory consumes around three times the time of [13], the gap for minimum snap trajectory is much narrower(around 1.5 times). Furthermore, based on the results reported in [13], our algorithm is faster than the optimized version of algorithm in [12], which is one order of magnitude slower than [13] (we do not plot the result of [12] as the code is not available.)

The proposed framework is implemented in ROS and we conduct multiple flight experiments of a small quadrotor executing the trajectories generated using our method. We use stereo camera fusion with IMU[25] for localization and state estimation of the quadrotor in the environment and use the robust and perfect tracking (RPT) controller [26] for trajectory tracking. Firstly, we conduct a set of experiments where a long trajectory is generated in a pre-built map consisting of two rooms and multiple obstacles, as shown in Fig. 1. The trajectory is generated by the i7 onboard computer and executed by the quadrotor immediately. We use multiple settings of dynamical feasibility. The trajectory with maximal velocity and acceleration set as $2.5\mathrm{m/s}$ and $3\mathrm{m/s^{2}}$ is shown in Fig. 5. In the second set of experiments the onboard computer generates safe and feasible trajectories online to reach target points set by a commander remotely. The video of the experiments can be found online⁶⁶6https://youtu.be/BM8_ABM_2VM or in the supplemental material.