Generating Large-Scale Trajectories Efficiently using
Double Descriptions of Polynomials

First, we explore the relation between parameterization spaces of both descriptions. We denote by $\mathbb{P}_{\mathrm{c}}:\mathbb{R}^{2Ms}\times\mathbb{R}_{>0}^{M}$ the space for $\{{c,T}\}$ and by $\mathbb{P}_{\mathrm{d}}:\mathbb{R}^{2Ms}\times\mathbb{R}_{>0}^{M}$ the space for $\left\{{d,T}\right\}$. The relation between $\mathbb{P}_{\mathrm{c}}$ and $\mathbb{P}_{\mathrm{d}}$ is given as below.

Proposition 1 gives analytic transformations between double descriptions, which have long been evaluated unwisely. In [1], $\mathbf{A}_{b}(t)$ is numerically computed by inverting $\mathbf{A}_{f}(t)$ for any given $t$. In [2], the structure of $\mathbf{A}_{f}(t)$ is exploited so that $\mathbf{A}_{b}(t)$ is evaluated efficiently and stably via Schur complement, where only the inverse of a sub-matrix is needed. Our previous work [10] has the fewest online computations, where coefficients in $\mathbf{A}_{b}(t)$ is offline numerically computed by setting $t=1$. However, all of these schemes involve numerically unstable matrix inverse. Proposition 1 is free of these drawbacks for the exact analytic expression. The diffeomorphism structure can be further utilized to greatly improve the efficiency and quality of trajectory generation.

Consider the following trajectory generation problem,

	$\displaystyle\min_{c,T}$	$\displaystyle~{}{J(c,T)}=\int_{0}^{\tau_{M}}{p^{(s)}(t)^{2}}\mathrm{d}t,$		(11)
	$\displaystyle s.t.~{}$	$\displaystyle~{}p^{(j)}(0)=d_{0,j},~{}0\leq j<s,$		(12)
		$\displaystyle~{}p^{(j)}(\tau_{M})=d_{M,j},~{}0\leq j<s,$		(13)
		$\displaystyle~{}p(\tau_{i})=q_{i},~{}0\leq i<M.$		(14)

The initial and terminal derivatives are specified by $d_{0}=(d_{0,0},\dots,d_{0,s-1})^{\mathrm{T}}$ and $d_{M}=(d_{M,0},\dots,d_{M,s-1})^{\mathrm{T}}$, respectively. Each $q_{i}$ is a specified intermediate waypoint at $\tau_{i}$. The $s-1$ times continuous differentiability of $p(t)$ on $[0,\tau_{M}]$ is implicitly required by the definition of $J(c,T)$, which is not explicitly formulated as equality constraints here.

Although a closed-form solution of (11) is given by Bry et al. [4], its efficiency is limited by the numerical evaluation of $\mathbf{A}_{b}(t)$ and the sparse permutation matrix inverse. To attain the linear complexity, Burke et al. [8] leverage the problem structure to calculate both primal and dual variables through a block tridiagonal linear equation system. However, it still needs frequently inverting sub-blocks. The size of the system is also redundant because of the dual variables. Therefore, we give a linear-complexity scheme with minimal problem size, where the matrix inverse is totally eliminated.

Consider the $\left\{{d,T}\right\}$ description of $p(t)$. We only need to obtain all unspecified entries in $d$, which are indeed $p^{(j)}(\tau_{i})$ for $1\leq i<M$ and $1\leq j<s$. Rewrite $d$ as

$$d=\mathbf{P}(\bar{d}+\mathbf{B}\widetilde{d})$$

(15)

where $\widetilde{d}\in\mathbb{R}^{(M-1)(s-1)}$ is a vector containing all unspecified entries. The constant vector $\bar{d}\in\mathbb{R}^{(M+1)s}$ is defined as

$$\bar{d}=({d_{0}^{\mathrm{T}},q_{1},0_{s-1},\dots,q_{M-1},0_{s-1},d_{M}^{\mathrm{T}}})^{\mathrm{T}}.$$

(16)

where $0_{s-1}\in\mathbb{R}^{s-1}$ is a zero vector. The permutation matrix $\mathbf{P}=({\mathbf{P}_{1}^{\mathrm{T}},\dots,\mathbf{P}_{M}^{\mathrm{T}}})^{\mathrm{T}}$ is defined as

$$\mathbf{P}_{i}=\left({\mathbf{0}_{2s\times(i-1)s},\mathbf{I}_{2s},\mathbf{0}_{2s\times(M-i)s}}\right).$$

(17)

The matrix $\mathbf{B}=({\mathbf{B}_{1},\dots,\mathbf{B}_{M-1}})$ is defined as

$$\mathbf{B}_{i}=\left({\mathbf{0}_{(s-1)\times is},\mathbf{D},\mathbf{0}_{(s-1)\times(M-i)s}}\right)^{\mathrm{T}},$$

(18)

where $\mathbf{D}=({0_{s-1},\mathbf{I}_{s-1}})$. It should be noted that computing (15) only involves orderly accessing entries. We do not really need to compute the matrix product considering that $\mathbf{P}$ and $\mathbf{B}$ is highly structured.

The cost function $J(c,T)$ indeed takes a quadratic form

$$J(c,T)=c^{\mathrm{T}}\mathbf{Q}_{\Sigma}(T)c$$

(19)

in which $\mathbf{Q}_{\Sigma}(T)=\oplus_{i=1}^{M}{\mathbf{Q}(T_{i})}$. Note that the symmetric matrix $\mathbf{Q}(t)$ has an analytical form whose entries are simple power functions of $t$. Its analytical form is provided by Bry et al. in the appendix of [4], to which we refer for details. Substituting (3) and (15) into (19) gives

$$J=(\bar{d}+\mathbf{B}\widetilde{d})^{\mathrm{T}}\mathbf{P}^{\mathrm{T}}\mathbf{H}_{\Sigma}(T)\mathbf{P}(\bar{d}+\mathbf{B}\widetilde{d})$$

(20)

in which $\mathbf{H}_{\Sigma}(T)=\oplus_{i=1}^{M}{\mathbf{H}(T_{i})}$ is a symmetric matrix. All its diagonal blocks is fully determined by the matrix function

$$\mathbf{H}(t)=\mathbf{A}_{b}^{\mathrm{T}}(t)\mathbf{Q}(t)\mathbf{A}_{b}(t).$$

(21)

Obviously, the analytical form of $\mathbf{H}(t)$ can be easily derived for a fixed $s$ by combining our Proposition 1 and the result from Bry et al. [4]. Therefore, we omit the parameter $T$ and denote $\mathbf{H}_{\Sigma}=\oplus_{i=1}^{M}{\mathbf{H}_{i}}$ hereafter because it is trivial to obtain all diagonal blocks for any given $T$ in linear time and space.

Differentiating $J$ w.r.t. $\widetilde{d}$ gives

$${\mathrm{d}{J}}/{\mathrm{d}{\widetilde{d}}}=2\mathbf{B}^{\mathrm{T}}\mathbf{P}^{\mathrm{T}}\mathbf{H}_{\Sigma}\mathbf{P}(\mathbf{B}\widetilde{d}+\bar{d}).$$

(22)

The optimal $\widetilde{d}$ satisfies $\|{{\mathrm{d}{J}}/{\mathrm{d}{\widetilde{d}}}}\|=0$, i.e.,

$$\mathbf{M}\widetilde{d}=\bar{b}$$

(23)

where $\mathbf{M}=\mathbf{B}^{\mathrm{T}}\mathbf{P}^{\mathrm{T}}\mathbf{H}_{\Sigma}\mathbf{P}\mathbf{B}$ and $\bar{b}=-\mathbf{B}^{\mathrm{T}}\mathbf{P}^{\mathrm{T}}\mathbf{H}_{\Sigma}\mathbf{P}\bar{d}$.

Actually, the computation for $\mathbf{M}$ and $\bar{b}$ is quite easy. We partition each $\mathbf{H}_{i}$ into four square blocks as

$$\mathbf{H}_{i}=\begin{pmatrix}\boldsymbol{\Gamma}_{i}&\boldsymbol{\Lambda}_{i}\\ \boldsymbol{\Phi}_{i}&\boldsymbol{\Omega}_{i}\end{pmatrix}.$$

(24)

Expanding $\mathbf{M}$ gives

$$\mathbf{M}=\begin{pmatrix}\boldsymbol{\alpha}_{1}&\boldsymbol{\beta}_{2}&\mathbf{0}&\cdots&\mathbf{0}&\mathbf{0}\\ \boldsymbol{\gamma}_{2}&\boldsymbol{\alpha}_{2}&\boldsymbol{\beta}_{3}&\cdots&\mathbf{0}&\mathbf{0}\\ \mathbf{0}&\boldsymbol{\gamma}_{3}&\boldsymbol{\alpha}_{3}&\cdots&\mathbf{0}&\mathbf{0}\\ \vdots&\vdots&\vdots&\ddots&\vdots&\vdots\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\cdots&\boldsymbol{\alpha}_{M-2}&\boldsymbol{\beta}_{M-1}\\ \mathbf{0}&\mathbf{0}&\mathbf{0}&\cdots&\boldsymbol{\gamma}_{M-1}&\boldsymbol{\alpha}_{M-1}\end{pmatrix}$$

(25)

where

	$$\displaystyle\boldsymbol{\alpha}_{i}=\mathbf{D}(\boldsymbol{\Omega}_{i}+\boldsymbol{\Gamma}_{i+1})\mathbf{D}^{\mathrm{T}},$$		(26)
	$$\displaystyle\boldsymbol{\beta}_{i}=\mathbf{D}\boldsymbol{\Lambda}_{i}\mathbf{D}^{\mathrm{T}},~{}\boldsymbol{\gamma}_{i}=\mathbf{D}\boldsymbol{\Phi}_{i}\mathbf{D}^{\mathrm{T}}.$$		(27)

Similarly, we partition $\bar{d}$ as

$$\bar{d}=({\kappa_{0}^{\mathrm{T}},\kappa_{1}^{\mathrm{T}},\dots,\kappa_{M-1}^{\mathrm{T}},\kappa_{M}^{\mathrm{T}}})^{\mathrm{T}}$$

(28)

where each $\kappa_{i}\in\mathbb{R}^{s}$ is a constant vector. Expanding $\bar{b}$ gives

$$\bar{b}=({b_{1}^{\mathrm{T}},b_{2}^{\mathrm{T}},\dots,b_{M-2}^{\mathrm{T}},b_{M-1}^{\mathrm{T}}})^{\mathrm{T}}$$

(29)

in which the $i$-th part can be computed as

$$b_{i}=-\mathbf{D}\left({\boldsymbol{\Phi}_{i}\kappa_{i-1}+(\boldsymbol{\Omega}_{i}+\boldsymbol{\Gamma}_{i+1})\kappa_{i}+\boldsymbol{\Lambda}_{i}\kappa_{i+1}}\right).$$

(30)

Now we conclude the procedure to obtain the optimal trajectory. Firstly, compute each $\mathbf{H}_{i}$ for any given $T$ using the analytical function $\mathbf{H}(t)$ defined in (21). Secondly, compute $\bar{d}$ according to its definition (16) for the specified derivatives. Thirdly, compute nonzero entries in $\mathbf{M}$ and $\bar{b}$ according to (26), (27) and (30). Fourthly, solve the linear equation system (23) using Banded PLU Factorization [17]. Finally, recover the optimal coefficient vector $c$ through (15) and (3).

The above-concluded procedure only costs $O(M)$ linear time and space complexity. There is no numerical matrix inverse needed in the whole procedure because $\mathbf{A}_{b}$ is overcome by deriving its analytical form. Moreover, the linear equation system needed to be solved only involves necessary primal decision variables, thus achieving the minimal problem size.

Although the double descriptions make it possible to obtain the solution of (11) in a cheap way, the resultant trajectory quality is still determined by the parameter of the problem, i.e., intermediate waypoints $q=({q_{1},\dots,q_{M-1}})^{\mathrm{T}}$ and piece times $T$. Further optimization on $q$ and $T$ is needed to improve the trajectory quality while maintaining feasibility. Therefore, we utilize the diffeomorphism between double descriptions of the trajectory to derive the analytical gradient w.r.t. $q$ and $T$. The gradient helps to obtain optimal $q$ and $T$, which bridges the gap in many traditional trajectory planning methods such as [18] and [19].

For any pair of $q$ and $T$, denote by $\widetilde{d}(q,T)$ the corresponding optimal $\widetilde{d}$, which satisfies

$$\widetilde{d}(q,T)=\operatorname*{arg\,min}_{\widetilde{d}}{J(\mathbf{A}_{\mathrm{B}}(T)\mathbf{P}(\bar{d}+\mathbf{B}\widetilde{d}),T)}.$$

(31)

Then, the optimal $c$, denoted by $c(q,T)$, is computed as

$$c(q,T)=\mathbf{A}_{\mathrm{B}}(T)\mathbf{P}(\bar{d}+\mathbf{B}\widetilde{d}(q,T)).$$

(32)

Define a new cost as $\hat{J}(q,T):=J(c(q,T),T)$. The gradient we concern is indeed for $\hat{J}(q,T)$, i.e., $\partial\hat{J}/\partial{q}$ and $\partial\hat{J}/\partial{T}$.

Without causing ambiguity, we temporarily omit the parameters in $\hat{J}(q,T)$, $J(c,T)$, $\widetilde{d}(q,T)$, $c(q,T)$, $\mathbf{A}_{\mathrm{B}}(T)$ and $\mathbf{A}_{\mathrm{F}}(T)$ for simplicity. As for the cost function in (31), $\widetilde{d}$ is a stationary point, which means its gradient w.r.t. $\widetilde{d}$ satisfies $\|{({{\partial J}/{\partial c}})^{\mathrm{T}}\mathbf{A}_{\mathrm{B}}\mathbf{P}\mathbf{B}}\|=0$. Now we know that the gradient computation in either $\mathbb{P}_{c}$ or $\mathbb{P}_{d}$ possesses its convenience. In $\mathbb{P}_{c}$, the gradient $\partial J/\partial T_{i}$ and $\partial J/\partial c_{i}$ both have easy-to-derive analytic expressions. In $\mathbb{P}_{d}$, the gradient of the cost (31) by $\widetilde{d}$ is already zero. Thus, we first express $\partial\hat{J}/\partial{T}$ and $\partial\hat{J}/\partial{q}$ by $\partial J/\partial T_{i}$ and $\partial J/\partial c_{i}$. Then, the diffeomorphism in Proposition 1 is utilized to transform them into $\mathbb{P}_{d}$ such that terms relevant to $\widetilde{d}$ can be eliminated.

As for the gradient of $\hat{J}$ w.r.t. $T_{i}$, we have

	$\displaystyle\frac{\partial\hat{J}}{\partial T_{i}}$	$\displaystyle=\frac{\partial J}{\partial T_{i}}+\left({\frac{\partial J}{\partial c}}\right)^{\mathrm{T}}\frac{\partial\mathbf{A}_{\mathrm{B}}}{\partial T_{i}}\mathbf{P}(\bar{d}+\mathbf{B}\widetilde{d})~{}~{}~{}~{}~{}~{}~{}$
		$\displaystyle+\left({\frac{\partial J}{\partial c}}\right)^{\mathrm{T}}\mathbf{A}_{\mathrm{B}}\mathbf{P}\mathbf{B}\frac{\partial\widetilde{d}}{\partial T_{i}}$
		$\displaystyle=\frac{\partial J}{\partial T_{i}}+\left({\frac{\partial J}{\partial c}}\right)^{\mathrm{T}}\frac{\partial\mathbf{A}_{\mathrm{B}}}{\partial T_{i}}\mathbf{A}_{\mathrm{F}}c$
		$\displaystyle=\frac{\partial J}{\partial T_{i}}+\left({\frac{\partial J}{\partial c_{i}}}\right)^{\mathrm{T}}\dot{\mathbf{A}}_{b}(T_{i})\mathbf{A}_{f}(T_{i})c_{i}.$		(33)

As for the gradient of $\hat{J}$ w.r.t. $q_{i}$, we have

	$\displaystyle\frac{\partial\hat{J}}{\partial q_{i}}$	$\displaystyle=\left({\frac{\partial J}{\partial c}}\right)^{\mathrm{T}}\mathbf{A}_{\mathrm{B}}\mathbf{P}\left({\frac{\partial\bar{d}}{\partial q_{i}}+\mathbf{B}\frac{\partial\widetilde{d}}{\partial q_{i}}}\right)$
		$\displaystyle=\left({\frac{\partial J}{\partial c}}\right)^{\mathrm{T}}\mathbf{A}_{\mathrm{B}}\mathbf{P}\frac{\partial\bar{d}}{\partial q_{i}}$
		$\displaystyle=\sum_{k=0,1}{\left({\frac{\partial J}{\partial c_{i+k}}}\right)^{\mathrm{T}}\mathbf{A}_{b}(T_{i+k})e_{(1-k)s+1}}.$		(34)

where $e_{j}$ is the $j$-th column vector of $\mathbf{I}_{2s}$. As for $\partial J/\partial T_{i}$ and $\partial J/\partial c_{i}$, their analytic expressions are clearly derived in the appendix of [4].

It is obvious that the gradient computations given in (IV-C) and (IV-C) are both irrelevant to the piece number $M$. Thus, computing the gradient w.r.t. $q$ and $T$ only costs $O(M)$ linear time and space complexity. Besides, all involved formulas have smooth analytic forms for $T\in\mathbb{R}^{M}_{>0}$, which much increase the efficiency of gradient computation.

Aside from efficiency, our analytical gradient scheme enjoy advantages in numerical stability. Specifically, the major numerical issue comes from the structure of $\hat{J}({q,T})$ on $T$. As analyzed in [10], if any piece time goes to zero, the trajectory energy goes to infinity, thus making $\hat{J}$ behave like a barrier function. Consequently, any scheme that evaluates gradient indirectly suffers instability from bad accuracy because this requires unrealistically small step size for finite difference [6] near the barrier. In comparison, ours is free from this issue because of the available accurate gradient.

One natural application of the previous linear-complexity solution scheme is that we can compute minimum-jerk/snap trajectory with extreme efficiency. Actually, there are already many reliable trajectory generation schemes for this topic. One thing that really matters is whether we can use a scheme as a black box without even worrying about its computation burden. We show that our scheme almost satisfies the requirements on this black box.

To demonstrate the efficiency of our scheme, we benchmark it with several conventional schemes, including the QP scheme by Mellinger et al. [6], the closed-form scheme by Bry et al. [4] and the linear-complexity scheme by Burke et al. [8]. As for Mellinger’s scheme, we use OSQP [20] to solve the QP formed by linear conditions and quadratic cost. As for Bry’s scheme, both the dense and sparse linear system solvers are implemented to calculate the closed-form solution. As for Burke’s scheme, we implement an optimized version of our own for the sake of fairness. More specifically, it only costs several seconds to calculate a minimum-snap trajectory with $500,000$ pieces, while the one in their paper is reported to cost more than $2$ minutes [8]. All schemes are implemented in C++11 without any explicit hardware acceleration.

We benchmark all these five schemes on $3$-dimensional minimum jerk ($s=3$) and minimum snap ($s=4$) problems. All comparisons are conducted on an Intel Core i7-8700 CPU under Linux environment. For small-scale problems, the piece number ranges from $2$ to $2^{7}$. In each case, $100$ sub-problems are randomly generated to be solved by these schemes. The results are provided in Fig. 2. For large-scale problems, we only benchmark two linear-complexity schemes, where the piece number is sparsely sampled from $2^{10}$ to $2^{20}$. The results are given in Fig. 3.

As shown in Fig. 2, Bry’s closed-form solution and Burke’s scheme are faster than Mellinger’s scheme for small piece numbers. The dense version of Bry’s scheme becomes the slowest since the cubic complexity for the dense solver dominates the time. Its sparse version still retains the efficiency as piece number grows. Due to the linear complexity, Burke’s scheme and our scheme consume significantly less time than all other schemes. Moreover, ours is nearly an order of magnitude faster than Burke’s at any problem scale in Fig. 2 or Fig. 3. Intuitively, ours is able to generate trajectories with $10^{6}$ pieces in less than $1$ second.

Now we give another application for the linear-complexity solution and gradient. We provide a simple example here to significantly improve the efficiency of large-scale global trajectory optimization.

A drawback in traditional minimum snap based schemes is the lack of flexibility to adjust the piece times and waypoints. Such kind of scheme can only obtain gradient information unwisely by solving several perturbed problems [6]. To avoid this drawback, Gao et al. [7] propose a more flexible framework. It alternately optimizes the geometrical and temporal profile of a trajectory through two well-designed convex formulations. This framework generates high-quality large-scale trajectories within safe flight corridors while it cannot be done in real-time.

We assume that a polyhedron-shaped flight corridor has been generated as in [7]. The flight corridor $\mathcal{F}$ is defined as

$$\mathcal{F}=\bigcup_{i=1}^{M}{\mathcal{P}_{i}}$$

(35)

where each $\mathcal{P}_{i}$ is a finite convex polyhedron

$$\mathcal{P}_{i}=\left\{{x\in\mathbb{R}^{3}~{}\Big{|}~{}\mathbf{A}_{i}x\preceq b_{i}}\right\}.$$

(36)

Besides, locally sequential connection is also assumed

$$\begin{cases}\mathcal{P}_{i}\cap\mathcal{P}_{j}=\varnothing&\mathit{if}~{}|{i-j}|=2,\\ \mathcal{P}_{i}\cap\mathcal{P}_{j}\neq\varnothing&\mathit{if}~{}|{i-j}|\leq 1.\end{cases}$$

(37)

For such a $\mathcal{F}$, the start and goal position is located in $\mathcal{P}_{1}$ and $\mathcal{P}_{M}$, respectively. As is shown in Fig. 4, we assign each $3$-dimensional intermediate waypoint $q_{i}$ in the intersection $\mathcal{P}_{i}\cap\mathcal{P}_{i+1}$, which roughly ensures the trajectory safety.

To obtain the spatial-temporal optimal trajectory within $\mathcal{F}$, we optimize the following cost function

$$J_{\Sigma}(q,T)=\hat{J}(q,T)+J_{F}(q)+J_{D}(q,T).$$

(38)

The cost term $\hat{J}(q,T)$ is also the $3$-dimensional version. The cost term $J_{F}(q,T)$ is just a logarithmic barrier term to ensure that each $q_{i}$ is confined within $\mathcal{P}_{i}\cap\mathcal{P}_{i+1}$, defined as

$$J_{F}(q)=-\kappa\sum_{i=1}^{M-1}{\sum_{j=i}^{i+1}\mathbf{1}^{\mathrm{T}}\ln{[b_{j}-\mathbf{A}_{j}q_{i}]}},$$

(39)

where $\kappa$ is a constant barrier coefficient, $\mathbf{1}$ an all-ones vector with an appropriate length and $\ln{[\cdot]}$ the entry-wise natural logarithm. Actually, any $C^{2}$ clamped barrier function is an alternative to further eliminate the potential of the barrier in the interior of $\mathcal{P}_{i}\cap\mathcal{P}_{i+1}$, which can be easily constructed by following [21]. The cost term $J_{D}(q,T)$ is just a penalty to adjust the trajectory aggressiveness. It is defined as

	$\displaystyle J_{D}(q,T)=\rho_{t}\sum_{i=1}^{M}{T_{i}}+$		(40)
	$\displaystyle\rho_{v}\sum_{i=1}^{M-1}{g\left({\left\|{\frac{q_{i+1}-q_{i-1}}{T_{i+1}+T_{i}}}\right\|^{2}-v_{m}^{2}}\right)}+$
	$\displaystyle\rho_{a}\sum_{i=1}^{M-1}{g\left({\left\|{\frac{(q_{i+1}-q_{i})/T_{i+1}-(q_{i}-q_{i-1})/T_{i}}{(T_{i+1}+T_{i})/2}}\right\|^{2}-a_{m}^{2}}\right)}$

where $g(x)=\max{\{{x,0}\}}^{3}$ is a $C^{2}$ penalty function, $v_{m}$ the velocity limit and $a_{m}$ acceleration limit. The constant $\rho_{t}$ prevents the entire duration from growing too large. Constants $\rho_{v}$ and $\rho_{a}$ prevent the trajectory from being too aggressive. It also costs linear-complexity time for the value and gradient computation on $J_{\Sigma}(q,T)$. Then, we utilize the L-BFGS [22] with strong Wolfe conditions as an efficient quasi-Newton method to minimize the cost function.

As for constraints, it is possible that the interior part of a piece becomes unsafe or violates the limit too much. To handle such situations, we first utilize the feasibility checker proposed in [10] to locate such a piece. Then, the corresponding $\mathcal{P}_{i}$ is split into two intersecting parts, $\mathcal{P}_{i,0}$ and $\mathcal{P}_{i,1}$, as shown in Fig. 5. An intermediate waypoint $q_{i-1,i}$ is added as decision variables. Accordingly, we increase the corresponding penalty or barrier coefficient only for these two new pieces. In general, a larger penalty coefficient and shorter piece length make the soft constraint $J_{D}(q,T)$ tighter. A larger barrier coefficient in $J_{F}(q)$ makes $q_{i-1,i}$ more likely to be in the interior of $\mathcal{P}_{i,0}\cap\mathcal{P}_{i,1}$, which helps to ensure the safety. Empirically, these operations are seldom needed according to our simulations.

We benchmark our simple scheme with the global trajectory optimizer in Teach-Repeat-Replan [7] which minimizes $J(c,T)+\rho_{t}\sum_{i=1}^{M}{T_{i}}$ under the same constraints. The interval allocation optimization in FASTER [14] is also compared here since it supports polyhedron-shaped corridor constraints. Due to the fact that it does not optimize time, we initialize it using total time from Teach-Repeat-Replan. We set $s=3$, $\rho_{t}=32.0$, $v_{m}=4.0$ and $a_{m}=5.0$ for all schemes and $\rho_{v}=\rho_{a}=128.0$ for the proposed one. The relative cost tolerance is set as $10^{-4}$ for ours and the default value for the other two open-source implementations. The time out is set as $1.5s$. For each size, the computation time is averaged over $10$ randomly generated corridors. The results from three methods is shown in Fig. 6. An intuitive comparison for large-scale trajectory generation is also provided in Fig. 1 where a spatial-temporal optimal trajectory is generated by our scheme using significantly less time.