MINCO: Geometrically Constrained Trajectory Optimization for Multicopters

Source: [1] Z. Wang, X. Zhou, C. Xu, and F. Gao, "Geometrically constrained trajectory optimization for multicopters", IEEE Trans. Robot., vol. 38, no. 5, pp. 3259–3278, Oct. 2022, doi: 10.1109/TRO.2022.3160022.

1 Preliminaries

1.1 Differential Flatness

Consider a dynamical system of the following type

$$\begin{array}{}\text{(1)}& \dot{\mathit{x}}=\mathit{f}(\mathit{x})+\mathit{g}(\mathit{x})\mathit{u}\end{array}$$

with $\mathit{f}:{\mathbb{R}}^n\mapsto {\mathbb{R}}^n$, $\mathit{g}:{\mathbb{R}}^n\mapsto {\mathbb{R}}^{n\times m}$, state $\mathit{x}\in {\mathbb{R}}^n$, and input $\mathit{u}\in {\mathbb{R}}^m$. The map $\mathit{g}$ is assumed to have rank $m$. The system is said to be differentially flat, if there exists a flat output $\mathit{z}\in {\mathbb{R}}^m$ determined by $\mathit{x}$ and finite derivatives of $\mathit{u}$, such that $\mathit{x}$ and $\mathit{u}$ can both be parameterized by finite derivatives of $\mathit{z}$:

$$\begin{array}{}\text{(2)}& \mathit{x}={\mathrm{\Psi }}_x(\mathit{z},\dot{\mathit{z}},\dots ,{\mathit{z}}^{(s-1)})\text{(3)}& \mathit{u}={\mathrm{\Psi }}_u(\mathit{z},\dot{\mathit{z}},\dots ,{\mathit{z}}^{(s)}),\end{array}$$

where ${\mathrm{\Psi }}_x:{\left({\mathbb{R}}^m\right)}^{s-1}\mapsto {\mathbb{R}}^n$ and ${\mathrm{\Psi }}_u:{\left({\mathbb{R}}^m\right)}^s\mapsto {\mathbb{R}}^m$ are both induced by $\mathit{f}$ and $\mathit{g}$. Intuitively, the state and control can be determined from $\mathit{z}$ without explicit integration of the system dynamics $(1)$.

Leveraging the flatness of a system, the trajectory generation is convenient when there are only differential constraints in $(1)$. If we introduce a new control variable $\mathit{v}={\mathit{z}}^{(s)}$ and denote ${\mathit{z}}^{[s-1]}\in {\mathbb{R}}^{ms}$ as

$$\begin{array}{}\text{(4)}& {\mathit{z}}^{[s-1]}={({\mathit{z}}^{\mathrm{\top }},{\dot{\mathit{z}}}^{\mathrm{\top }},\dots ,{\mathit{z}}^{{(s-1)}^{\mathrm{\top }}})}^{\mathrm{\top }},\end{array}$$

the input $\mathit{u}={\mathrm{\Psi }}_u({\mathit{z}}^{[s-1]},\mathit{v})$ then exactly linearizes the original flat system into $m$ decoupled chains of $s$-integrators. Let $z_i$ denote the $i$-th entry in $\mathit{z}$, $v_i$ the $i$-th entry in $\mathit{v}$ and ${\mathit{z}}_i^{[s-1]}={(z_i,{\dot{z}}_i,\dots ,z_i^{(s-1)})}^{\mathrm{\top }}$. The $i$-th integrator chain is

$$\begin{array}{}\text{(5)}& {\dot{\mathit{z}}}_i^{[s-1]}=\left(\begin{array}{cc}\mathbf{0}& {\mathbf{I}}_{s-1}\\ 0& {\mathbf{0}}^{\mathrm{\top }}\end{array}\right){\mathit{z}}_i^{[s-1]}+\left(\begin{array}{c}\mathbf{0}\\ 1\end{array}\right)v_i,\end{array}$$

where $\mathbf{0}$ and $\mathbf{I}$ are a zero matrix and an identity matrix with appropriate sizes, respectively. Given an initial state and a goal state, boundary values of each integrator chain $(5)$ can be algebraically computed. Thus any trajectory integrated from these $m$ integrator chains can be transformed into a feasible trajectory for the original flat system via $(2)$ and $(3)$.

For dynamics with a small $m$, the flatness maps ${\mathrm{\Psi }}_x$ and ${\mathrm{\Psi }}_u$ further reduce the trajectory dimension and eliminate the differential constraints $(1)$, which is illustrated in Fig. 1. As a side effect, nonlinearity coming from both ${\mathrm{\Psi }}_x$ and ${\mathrm{\Psi }}_u$ brings additional difficulties in trajectory generation for $\mathit{z}$ when there are additional state-input constraints for $(1)$. However, such an effect is relieved if the flat-output space coincides with the configuration space of the relevant planning problem.

Figure 1[1]: Transform $\Psi_u$ and $\Psi_x$ of a flat system eliminate differential constraints (blue surface) from dynamics in the state-input space (left coordinate). The original state-input constraint $\mathcal{G}_\mathcal{D}$ (red area) is also transformed into a new constraint $\mathcal{G}$ (blue volume) in the flat-output space (right coordinate).

1.2 Direct Optimization in Flat-Output Space

Fortunately, the differential flatness of multicopters has been well studied and shown to have physically meaningful flat-output space which overlaps with the configuration space. Explicit forms of ${\mathrm{\Psi }}_x$ and ${\mathrm{\Psi }}_u$ are available in previous research (See original paper for details) for a variety of underactuated multicopters. More importantly, their flat outputs share the same form in general:

$$\begin{array}{}\text{(6)}& \mathit{z}={(p_x,p_y,p_z,\psi )}^{\mathrm{\top }}\end{array}$$

where ${(p_x,p_y,p_z)}^{\mathrm{\top }}$ is the translation of the Center of Gravity (CoG) and $\psi$ the yaw angle of the vehicle. The flat output $\mathit{z}$, especially its translation, provides a lot of convenience for the multicopter motion planning with complex spatial constraints.

To generate feasible motions for a multicopter, we first optimize the trajectory $\mathit{z}(t):[0,T]\mapsto {\mathbb{R}}^m$ in its flat-output space such that most of the spatial constraints are directly enforced. Then, the flatness maps ${\mathrm{\Psi }}_x$ and ${\mathrm{\Psi }}_u$ are applied to transform $\mathit{z}(t)$ into the state-input trajectory $\mathit{x}(t)$ and $\mathit{u}(t)$.

For motion smoothness, the quadratic control effort with time regularization is adopted as a cost functional of $\mathit{z}(t)$. General constraints on multicopters can be classified into configuration constraints and user-defined dynamic constraints. Normally, a collision-free motion implies

$$\begin{array}{}\text{(7)}& \mathit{z}(t)\in \mathcal{F},\mathrm{\forall }t\in [0,T],\end{array}$$

where $\mathcal{F}$ is the concerned obstacle-free region in configuration space. Besides, user-defined state-input constraints such as actuator limits or task-specific constraints are denoted by

$$\begin{array}{}\text{(8)}& {\mathcal{G}}_{\mathcal{D}}(x(t),u(t))⪯\mathbf{0},\mathrm{\forall }t\in [0,T].\end{array}$$

Exploiting ${\mathrm{\Psi }}_x$ and ${\mathrm{\Psi }}_u$, the corresponding constraints on $\mathit{z}(t)$ are computed as

$$\begin{array}{}\text{(9)}& {\mathcal{G}}_{\mathcal{D}}({\mathrm{\Psi }}_x(z^{[s-1]}(t)),{\mathrm{\Psi }}_u(z^{[s]}(t)))⪯\mathbf{0},\mathrm{\forall }t\in [0,T].\end{array}$$

Apparently, via the flatness, a constraint on $x$ and $u$ has its equivalent form on the finite derivatives of $z(t)$. For simplicity, we denote $(9)$ hereafter by

$$\begin{array}{}\text{(10)}& \mathcal{G}(z(t),\dot{z}(t),\dots ,z^{(s)}(t))⪯\mathbf{0},\mathrm{\forall }t\in [0,T],\end{array}$$

where $\mathcal{G}$ consists of $n_g$ equivalent constraints.

It is worth noting that we do not make further assumptions on the multicopter dynamics and flatness maps. In other words, the proposed framework supports a wide range of multicopters.

1.3 Problem Formulation

Concluding above descriptions gives the following problem:

$$\begin{array}{}\text{(11a)}& \underset{\mathit{z}(t),T}{min}& {\int }_0^T\mathit{v}(t{)}^{\mathrm{\top }}\mathbf{W}\mathit{v}(t)\mathrm{d}t+\rho (T),\text{(11b)}& \mathrm{s}.\mathrm{t}.& {\mathit{z}}^{(s)}(t)=\mathit{v}(t),\mathrm{\forall }t\in [0,T],\text{(11c)}& & \mathcal{G}(\mathit{z}(t),\dots ,{\mathit{z}}^{(s)}(t))⪯\mathbf{0},\mathrm{\forall }t\in [0,T],\text{(11d)}& & \mathit{z}(t)\in \mathcal{F},\mathrm{\forall }t\in [0,T],\text{(11e)}& & {\mathit{z}}^{[s-1]}(0)={\overline{\mathit{z}}}_o,{\mathit{z}}^{[s-1]}(T)={\overline{\mathit{z}}}_f,\end{array}$$

where $\mathbf{W}\in {\mathbb{R}}^{m\times m}$ is a positive diagonal matrix, $\rho :[0,\mathrm{\infty })\mapsto [0,\mathrm{\infty }]$ the time regularization, ${\overline{\mathit{z}}}_o\in {\mathbb{R}}^{ms}$ the initial condition and ${\overline{\mathit{z}}}_f\in {\mathbb{R}}^{ms}$ the terminal condition. The control input $\mathit{v}$ is allowed to be discontinuous in a finite number of time instants.

The trajectory optimization $(11)$ is nontrivial because of the continuous-time constraints $\mathcal{G}$ and the nonconvex set $\mathcal{F}$. We further specify some reasonable conditions to make it a well-defined problem. As for time regularization $\rho$, it trades off between the control effort and the expectation of total time,

$$\begin{array}{}\text{(12)}& {\rho }_s(T)=\sum _{i=0}^{M_T}{b}_i{T}^i,\end{array}$$

where $b_{M_T}$ is positive. Common choices are ${\rho }_s(T)=k_{\rho }T$ and ${\rho }_s(T)=k_{\rho }(T-T_{\mathrm{\Sigma }}{)}^2$ with an expected time $T_{\mathrm{\Sigma }}$. Besides, $\rho$ can also be defined to strictly fix the total time:

$$\begin{array}{}\text{(13)}& {\rho }_f(T)=\{\begin{array}{ll}0& \mathrm{if}T=T_{\mathrm{\Sigma }},\\ \mathrm{\infty }& \mathrm{if}T\ne T_{\mathrm{\Sigma }}.\end{array}\end{array}$$

As for nonlinear constraints $\mathcal{G}$, they are required to be $C^2$, i.e., twice continuously differentiable. As for the feasible region $\mathcal{F}$ in the configuration space, we approximate it geometrically by the union of $M_{\mathcal{P}}$ closed convex sets as

$$\begin{array}{}\text{(14)}& \mathcal{F}\simeq \tilde{\mathcal{F}}=\bigcup _{i=1}^{M_{\mathcal{P}}}{\mathcal{P}}_i.\end{array}$$

For simplicity, locally sequential connection is assumed on these convex sets:

$$\begin{array}{}\text{(15)}& \{\begin{array}{ll}{\mathcal{P}}_i\cap {\mathcal{P}}_j=\varnothing & \mathrm{if}|i-j|=2,\\ \mathrm{Int}({\mathcal{P}}_i\cap {\mathcal{P}}_j)\ne \varnothing & \mathrm{if}|i-j|⩽1,\end{array}\end{array}$$

where $\mathrm{Int}(\cdot )$ means the interior of a set. The translation of ${\overline{\mathit{z}}}_o$ and ${\overline{\mathit{z}}}_f$ is inscribed in ${\mathcal{P}}_1$ and ${\mathcal{P}}_{M_{\mathcal{P}}}$, respectively. As for $\tilde{\mathcal{F}}$, we consider the case that each ${\mathcal{P}}_i$ is a closed $m$-dimensional ball:

$$\begin{array}{}\text{(16)}& {\mathcal{P}}_i^{\mathcal{B}}=\{\mathit{x}\in {\mathbb{R}}^m|\parallel \mathit{x}-{\mathit{o}}_i{\parallel }_2⩽r_i\},\end{array}$$

or, more generally, a bounded convex polytope described by its $\mathcal{H}$-representation with potentially redundant constraints:

$$\begin{array}{}\text{(17)}& {\mathcal{P}}_i^{\mathcal{H}}=\{\mathit{x}\in {\mathbb{R}}^m|{\mathbf{A}}_i\mathit{x}⪯{\mathit{b}}_i\}.\end{array}$$

For the optimization in $(11)$, we aim to construct a computationally efficient solver while retaining the flexibility to handle different task-specific constraints ${\mathcal{G}}_{\mathcal{D}}$ in $(8)$.

2 Multi-Stage Control Effort Minimization

In this section, we analyze the multi-stage control effort minimization without functional constraints.

For this problem, we propose easy-to-use optimality conditions for general cases, which are proved to be necessary and sufficient. Leveraging our conditions, the optimal trajectory is directly constructed in linear complexity of time and space, without evaluating the cost functional explicitly or implicitly.

Base on them, a novel trajectory class along with linear-complexity spatial-temporal deformation is designed to meet user-defined objectives in various trajectory planning scenarios.

2.1 Unconstrained Control Effort Minimization

When constraint $\mathcal{F}$ exists, adjusting the waypoints or control points of a trajectory helps to ensure safety. When constraint $\mathcal{G}$ exists, adjusting the time allocation also helps to enforce physical limits. Therefore, spatial and temporal parameters are both vital to a flexible trajectory representation. A natural problem is to generate a smooth trajectory subject to these parameters.

We solve Linear Quadratic Minimum-Time (LQMT) problems to generate trajectories from spatial-temporal parameters. Although the LQMT problems have extensive studies and applications, only single-stage problems are considered in the literature. We study the multi-stage problems where intermediate points and time vector are fixed in advance for multi-piece trajectories. Consider an $M$-stage control effort minimization without $\mathcal{F}$ and $\mathcal{G}$,

$$\begin{array}{}\text{(18a)}& \underset{\mathit{z}(t)}{min}& {\int }_{t_0}^{t_M}\mathit{v}(t{)}^{\mathrm{\top }}\mathbf{W}\mathit{v}(t)\mathrm{d}t,\text{(18b)}& \mathrm{s}.\mathrm{t}.& {\mathit{z}}^{(s)}(t)=\mathit{v}(t),\mathrm{\forall }t\in [t_0,t_M],\text{(18c)}& & {\mathit{z}}^{[s-1]}(t_0)={\overline{\mathit{z}}}_o,{\mathit{z}}^{[s-1]}(t_M)={\overline{\mathit{z}}}_f,\text{(18d)}& & {\mathit{z}}^{[d_i-1]}(t_i)={\overline{\mathit{z}}}_i,1⩽i<M,\text{(18e)}& & t_{i-1}<t_i,1⩽i⩽M.\end{array}$$

The time interval $[t_0,t_M]$ is split into $M$ stages by $M+1$ fixed timestamps, with constant boundary conditions ${\overline{\mathit{z}}}_o,{\overline{\mathit{z}}}_f\in {\mathbb{R}}^{ms}$. Intermediate conditions ${\overline{\mathit{z}}}_i\in {\mathbb{R}}^{m{d}_i}$ with $d_i⩽s$ specify the value of $\mathit{z}(t_i),\dot{\mathit{z}}(t_i),\dots ,{\mathit{z}}^{(d_i-1)}(t_i)$, where $d_i$ is number of derivatives fixed at $t_i$. For example, if $\mathit{z}(t)$ is only required to pass a given position at $t_i$, then $d_i=1$ because ${\overline{\mathit{z}}}_i$ contains the $0$-order derivative and nothing else.

Existing works focus on the necessary conditions for special cases of $(18)$. In aerial robotics area, the QP formulation and the closed-form one implicitly or explicitly optimize unknown knot derivatives, taking parameterization as a priori. This extra computation actually makes them less efficient.

In control area, a special case where $d_i=1$ is also studied via controllability Gramian. The result is for general linear systems with possibly nonpolynomial solutions while it is less intuitive considering the computational aspect. These necessary conditions can cause potential degeneracy in trajectory representation and sensitivity, if further parametric optimization on spatial-temporal parameters is needed.

2.2 Optimality Conditions

We propose necessary and sufficient optimality conditions for $(18)$ with all possible settings of $d_i$, ${\overline{\mathit{z}}}_i$, and $t_i$. Thus, an optimal trajectory can be directly constructed from spatial-temporal parameters. Furthermore, the existence and uniqueness of the optimal trajectory are always guaranteed.

We transform $(18)$ into the Mayer form in which a new state $\mathit{y}\in {\mathbb{R}}^{ms+1}$ augmented by $\tilde{y}\in \mathbb{R}$ is defined as

$$\begin{array}{}\text{(19)}& \mathit{y}=\left(\begin{array}{c}{\mathit{z}}^{[s-1]}\\ \tilde{y}\end{array}\right).\end{array}$$

The augmented system $\dot{\mathit{y}}=\hat{f}(\mathit{y},\mathit{v})$ has the structure

$$\begin{array}{}\text{(20)}& \dot{\mathit{y}}=\left(\begin{array}{cc}\overline{\mathbf{A}}& \mathbf{0}\\ {\mathbf{0}}^{\mathrm{\top }}& 0\end{array}\right)\mathit{y}+\left(\begin{array}{c}\mathbf{0}\\ \mathit{v}\\ {\mathit{v}}^{\mathrm{\top }}\mathbf{W}\mathit{v}\end{array}\right),\end{array}$$

where

$$\begin{array}{}\text{(21)}& \overline{\mathbf{A}}=\left(\begin{array}{cc}\mathbf{0}& {\mathbf{I}}_{m(s-1)}\\ {\mathbf{0}}_{m\times m}& {\mathbf{0}}^{\mathrm{\top }}\end{array}\right)\in {\mathbb{R}}^{ms\times ms}.\end{array}$$

We design a running process for the augmented system in $M$ stages, of which the $i$-th is ${\mathrm{\Delta }}_i=[t_{i-1},t_i]$. It is worth noting that state switching occurs in this running process. Strictly speaking, the state switching only occurs on $\tilde{y}$ at the beginning of each stage.

Denote by ${\mathit{y}}_{[i]}:{\mathrm{\Delta }}_i\mapsto {\mathbb{R}}^{ms+1}$ the augmented state trajectory in the $i$-th stage, which consists of two parts, ${\mathit{z}}_{[i]}^{[s-1]}$ and ${\tilde{y}}_{[i]}$.

At each timestamp $t_i$, the state transfers from ${\mathit{y}}_{[i]}$ to ${\mathit{y}}_{[i+1]}$, and the part $\tilde{y}$ is reset as

$$\begin{array}{}\text{(22)}& {\tilde{y}}_{[i+1]}(t_i)=0,0⩽i<M,\end{array}$$

thus switching the partial state from ${\tilde{y}}_{[i]}(t_i)$ to $0$.

The ${\mathit{z}}^{[s-1]}$ part remains continuous between stages, which means

$$\begin{array}{}\text{(23)}& {\mathit{z}}_{[i]}^{[s-1]}(t_i)={\mathit{z}}_{[i+1]}^{[s-1]}(t_i),1⩽i<M.\end{array}$$

The conditions in $(18c)$ and $(18d)$ are still satisfied, i.e.,

$$\begin{array}{}\text{(24)}& & {\mathit{z}}_{[1]}^{[s-1]}(t_0)={\overline{\mathit{z}}}_o,{\mathit{z}}_{[M]}^{[s-1]}(t_M)={\overline{\mathit{z}}}_f,\text{(25)}& & {\mathit{z}}_{[i]}^{[d_i-1]}(t_i)={\overline{\mathit{z}}}_i,1⩽i<M.\end{array}$$

In this process, the cost functional in $(18)$ is converted into the sum of terminal cost of each stage for the augmented system, i.e., $\sum _{i=1}^M{\tilde{y}}_{[i]}(t_i)$. Therefore, the optimal trajectories for the augmented system and the original one are identical in ${\mathit{z}}^{[s-1]}$.

We utilize the Hybrid Maximum Principle to derive necessary conditions for the optimal solution.

Theorem 1: Hybrid Maximum Principle

Let $t_0<\cdots <t_M$ be real numbers and ${\mathrm{\Delta }}_k=[t_{k-1},t_k]$. For any collection of absolute continuous functions ${\mathit{x}}_k:{\mathrm{\Delta }}_k\mapsto {\mathbb{R}}^{n_k}$, define a vector, ${\mathit{x}}_{\mathrm{\Sigma }}\in {\mathbb{R}}^{\overline{n}}$ where $\overline{n}=2\sum _{k=1}^M{n}_k$, as

$$\begin{array}{}\text{(26)}& {\mathit{x}}_{\mathrm{\Sigma }}={({\mathit{x}}_1^{\mathrm{\top }}(t_0),{\mathit{x}}_1^{\mathrm{\top }}(t_1),\dots ,{\mathit{x}}_M^{\mathrm{\top }}(t_{M-1}),{\mathit{x}}_M^{\mathrm{\top }}(t_M))}^{\mathrm{\top }}.\end{array}$$

On the time interval $\mathrm{\Delta }=[t_0,t_M]$ consider the problem

$$\begin{array}{}\text{(27a)}& \underset{{\mathit{u}}_k,{\mathit{x}}_k}{min}& J({\mathit{x}}_{\mathrm{\Sigma }}),\text{(27b)}& \mathrm{s}.\mathrm{t}.& {\dot{\mathit{x}}}_k(t)={\mathit{f}}_k({\mathit{x}}_k(t),{\mathit{u}}_k(t)),\text{(27c)}& & {\mathit{u}}_k(t)\in U_k\subseteq {\mathbb{R}}^{r_k},\text{(27d)}& & \mathrm{\forall }t\in {\mathrm{\Delta }}_k,k=1,\dots ,M,\text{(27e)}& & \mathit{\eta }({\mathit{x}}_{\mathrm{\Sigma }})=\mathbf{0},\end{array}$$

where ${\mathit{f}}_k:{\mathbb{R}}^{n_k}\times {\mathbb{R}}^{r_k}\mapsto {\mathbb{R}}^{n_k}$, $J:{\mathbb{R}}^{\overline{n}}\mapsto \mathbb{R}$ and $\mathit{\eta }:{\mathbb{R}}^{\overline{n}}\mapsto {\mathbb{R}}^q$ are continuously differentiable, ${\mathit{u}}_k:\mathbb{R}\mapsto {\mathbb{R}}^{r_k}$ are measurable and bounded on the corresponding ${\mathrm{\Delta }}_k$.

Denote an optimal process for $(27)$ by $({\mathit{x}}^{\ast }(t),{\mathit{u}}^{\ast }(t))$. Then, there exists a collection $(\alpha ,\mathit{\gamma },{\mathit{\psi }}_1,\dots ,{\mathit{\psi }}_M)$, where $\alpha ⩾0$, $\mathit{\gamma }\in {\mathbb{R}}^q$ and ${\mathit{\psi }}_k:{\mathrm{\Delta }}_k\mapsto {\mathbb{R}}^{n_k}$ are Lipschitz continuous. It generates $M$ Pontryagin functions

$$\begin{array}{}\text{(28)}& H_k({\mathit{\psi }}_k,{\mathit{x}}_k,{\mathit{u}}_k)={\mathit{\psi }}_k^{\mathrm{\top }}{\mathit{f}}_k({\mathit{x}}_k,{\mathit{u}}_k),t\in {\mathrm{\Delta }}_k,\end{array}$$

and a Lagrange function $L({\mathit{x}}_{\mathrm{\Sigma }})=\alpha J({\mathit{x}}_{\mathrm{\Sigma }})+{\mathit{\gamma }}^{\mathrm{\top }}\mathit{\eta }({\mathit{x}}_{\mathrm{\Sigma }})$. The following conditions are satisfied for all $k=1,\dots ,M$.

• Nontriviality condition:

$$\begin{array}{}\text{(29)}& (\alpha ,{\mathit{\gamma }}^{\mathrm{\top }})\ne \mathbf{0};\end{array}$$

• Adjoint equations: for almost all $t\in {\mathrm{\Delta }}_k$,

$$\begin{array}{}\text{(30)}& {\dot{\mathit{\psi }}}_k(t)=-\frac{\partial H_k}{\partial {\mathit{x}}_k}({\mathit{\psi }}_k(t),{\mathit{x}}_k^{\ast }(t),{\mathit{u}}_k^{\ast }(t));\end{array}$$

• Transversality conditions:

$$\begin{array}{}\text{(31)}& \{\begin{array}{c}{\mathit{\psi }}_k(t_{k-1})=L_{{\mathit{x}}_k(t_{k-1})}({\mathit{x}}_{\mathrm{\Sigma }}^{\ast }),\\ {\mathit{\psi }}_k(t_k)=-L_{{\mathit{x}}_k(t_k)}({\mathit{x}}_{\mathrm{\Sigma }}^{\ast });\end{array}\end{array}$$

• Maximality conditions: for all $t\in {\mathrm{\Delta }}_k$,

$$\begin{array}{rl}& H_k({\mathit{\psi }}_k(t),{\mathit{x}}_k^{\ast }(t),{\mathit{u}}_k^{\ast }(t))\\ =& \underset{{\mathit{u}}_k\in U_k}{sup}{H}_k({\mathit{\psi }}_k(t),{\mathit{x}}_k^{\ast }(t),{\mathit{u}}_k)\\ \text{(32)}& =& 0.\end{array}$$

Proof:

The proof can be directly adapted from Theorem 4 by Dmitruk and Kaganovich. Here we only consider each system $f_k$ to be time-invariant and all intervals ${\mathrm{\Delta }}_k$ to be fixed. Besides, no inequality constraints are specified on ${\mathit{x}}_{\mathrm{\Sigma }}$.

$◻$

According to Theorem 1, the costate ${\mathit{\psi }}_{[i]}:{\mathrm{\Delta }}_i\mapsto {\mathbb{R}}^{ms+1}$ in the $i$-th stage is defined as

$$\begin{array}{}\text{(33)}& {\mathit{\psi }}_{[i]}=\left(\begin{array}{c}{\mathit{\lambda }}_{[i]}\\ {\mu }_{[i]}\end{array}\right)={({{\mathit{\lambda }}_{[i]}}_1,{{\mathit{\lambda }}_{[i]}}_2,\dots ,{{\mathit{\lambda }}_{[i]}}_s,{\mu }_{[i]})}^{\mathrm{\top }},\end{array}$$

where ${\mu }_{[i]}:{\mathrm{\Delta }}_i\mapsto \mathbb{R}$. ${{\mathit{\lambda }}_{[i]}}_j:{\mathrm{\Delta }}_i\mapsto {\mathbb{R}}^m$ is the $j$-th map in ${\mathit{\lambda }}_{[i]}:{\mathrm{\Delta }}_i\mapsto {\mathbb{R}}^{ms}$. The $i$-th Pontryagin function of $(20)$ is

$$\begin{array}{rl}& H_i({\mathit{\psi }}_{[i]},{\mathit{y}}_{[i]},{\mathit{v}}_{[i]})={\mathit{\psi }}_{[i]}^{\mathrm{\top }}\hat{f}({\mathit{y}}_{[i]},{\mathit{v}}_{[i]})\\ \text{(34)}& & ={\mathit{\lambda }}_{[i]}^{\mathrm{\top }}\overline{\mathbf{A}}{\mathit{z}}_{[i]}^{[s-1]}+{{\mathit{\lambda }}_{[i]}^{\mathrm{\top }}}_s{\mathit{v}}_{[i]}+{\mu }_{[i]}{\mathit{v}}_{[i]}^{\mathrm{\top }}\mathbf{W}{\mathit{v}}_{[i]}.\end{array}$$

By applying the adjoint equation $(30)$ for ${\mu }_{[i]}$, we have ${\dot{\mu }}_{[i]}=0$, which means ${\mu }_{[i]}(t)={\overline{\mu }}_i\in \mathbb{R}$ is a constant in ${\mathrm{\Delta }}_i$. Therefore, $H_i$ is always a quadratic function of ${\mathit{v}}_{[i]}$,

$$\begin{array}{}\text{(35)}& H_i({\mathit{\psi }}_{[i]},{\mathit{y}}_{[i]},{\mathit{v}}_{[i]})={\mathit{\lambda }}_{[i]}^{\mathrm{\top }}\overline{\mathbf{A}}{\mathit{z}}_{[i]}^{[s-1]}+{{\mathit{\lambda }}_{[i]}^{\mathrm{\top }}}_s{\mathit{v}}_{[i]}+{\overline{\mu }}_i{\mathit{v}}_{[i]}^{\mathrm{\top }}\mathbf{W}{\mathit{v}}_{[i]}.\end{array}$$

By applying the adjoint equation for ${\mathit{\lambda }}_{[i]}$, we obtain

$$\begin{array}{}\text{(36)}& {\dot{\mathit{\lambda }}}_{[i]}=-{\overline{\mathbf{A}}}^{\mathrm{\top }}{\mathit{\lambda }}_{[i]},\end{array}$$

which is expanded as

$$\begin{array}{}\text{(37)}& {{\dot{\mathit{\lambda }}}_{[i]}}_j=\{\begin{array}{ll}\mathbf{0}& \mathrm{if}j=1,\\ -{{\mathit{\lambda }}_{[i]}}_{j-1}& \mathrm{if}2⩽j⩽s.\end{array}\end{array}$$

It is obvious that ${{\mathit{\lambda }}_{[i]}}_s(t)$ is an $s-1$ degree polynomial.

According to maximality conditions $(32)$, the supremum of $H_i$ is always $0$ in ${\mathrm{\Delta }}_i$. Thus the positive definiteness of $\mathbf{W}$ implies ${\overline{\mu }}_i⩽0$. If ${\overline{\mu }}_i=0$, then $(35)$ becomes a linear function of ${\mathit{v}}_{[i]}$. The zero supremum means that ${{\mathit{\lambda }}_{[i]}}_s(t)=\mathbf{0}$ in ${\mathrm{\Delta }}_i$. As the result of $(36)$, ${\mathit{\psi }}_{[i]}(t)=\mathbf{0}$ holds for all $t$ in ${\mathrm{\Delta }}_i$. In such a case, a contradiction occurs that the nontriviality condition $(29)$ and the transversality conditions $(31)$ cannot be satisfied at the same time. Therefore, ${\overline{\mu }}_i<0$ always holds in the whole ${\mathrm{\Delta }}_i$. The optimal control ${\mathit{v}}_{[i]}^{\ast }$ can be obtained from

$$\begin{array}{}\text{(38)}& \frac{\partial H_i}{\partial {\mathit{v}}_{[i]}}({\psi }_{[i]},y_{[i]}^{\ast },{\mathit{v}}_{[i]}^{\ast })={{\mathit{\lambda }}_{[i]}}_s+2{\overline{\mu }}_i\mathbf{W}{\mathit{v}}_{[i]}^{\ast }=\mathbf{0},\end{array}$$

i.e.,

$${\mathit{v}}_{[i]}^{\ast }(t)=-\frac{1}{2{\overline{\mu }}_i}{\mathbf{W}}^{-1}{{\mathit{\lambda }}_{[i]}}_s(t),\mathrm{\forall }t\in {\mathrm{\Delta }}_i.$$

Then, ${\mathit{z}}_{[i]}^{\ast }$ produced by a chain of $s$-integrators from ${{\mathit{\lambda }}_{[i]}}_s(t)$, is a $2s-1$ degree polynomial.

To further explore structures of the solution, we generate the Lagrange function using the cost of augmented system along with all constraints in $(23)$, $(24)$ and $(25)$. We have

$$\begin{array}{rl}L(y_{\mathrm{\Sigma }})=& \alpha \sum _{i=1}^M{\tilde{y}}_{[i]}(t_i)+\sum _{i=0}^{M-1}{\gamma }_i{\tilde{y}}_{[i+1]}(t_i)\\ & +\sum _{i=1}^{M-1}({\mathit{\zeta }}_i^{\mathrm{\top }},{\mathit{\sigma }}_i^{\mathrm{\top }})({\mathit{z}}_{[i]}^{[s-1]}(t_i)-{\mathit{z}}_{[i+1]}^{[s-1]}(t_i))\\ & +{\mathit{\theta }}_o^{\mathrm{\top }}({\mathit{z}}_{[1]}^{[s-1]}(t_0)-{\overline{\mathit{z}}}_o)+{\mathit{\theta }}_f^{\mathrm{\top }}({\mathit{z}}_{[M]}^{[s-1]}(t_M)-{\overline{\mathit{z}}}_f)\\ \text{(40)}& & +\sum _{i=1}^{M-1}{\mathit{\theta }}_i^{\mathrm{\top }}({\mathit{z}}_{[i]}^{[d_i-1]}(t_i)-{\overline{\mathit{z}}}_i),\end{array}$$

where ${\gamma }_i\in \mathbb{R}$, ${\mathit{\zeta }}_i\in {\mathbb{R}}^{m{d}_i}$, ${\mathit{\sigma }}_i\in {\mathbb{R}}^{m(s-d_i)}$, ${\mathit{\theta }}_o\in {\mathbb{R}}^{ms}$, ${\mathit{\theta }}_f\in {\mathbb{R}}^{ms}$ and ${\mathit{\theta }}_i\in {\mathbb{R}}^{m{d}_i}$ are all constant coefficients of corresponding constraints, $y_{\mathrm{\Sigma }}$ is defined as in $(26)$. Following transversality conditions $(31)$, taking the derivative of $L$ w.r.t. ${\mathit{y}}_{\mathrm{\Sigma }}$ gives the boundary values of costates ${\mathit{\psi }}_{[i]}$ and ${\mathit{\psi }}_{[i+1]}$, i.e.,

$$\begin{array}{}\text{(41)}& {\mathit{\lambda }}_{[i]}(t_i)=-\left(\begin{array}{c}{\mathit{\zeta }}_i+{\mathit{\theta }}_i\\ {\mathit{\sigma }}_i\end{array}\right),{\mathit{\lambda }}_{[i+1]}(t_i)=-\left(\begin{array}{c}{\mathit{\zeta }}_i\\ {\mathit{\sigma }}_i\end{array}\right),\text{(42)}& {\mu }_{[i]}(t_i)={\mu }_{[i+1]}(t_{i+1})=-\alpha .\end{array}$$

Because ${\mu }_{[i+1]}(t)={\overline{\mu }}_{i+1}$ in ${\mathrm{\Delta }}_{i+1}$, we have

$$\begin{array}{}\text{(43)}& {\overline{\mu }}_i=-\alpha ,1⩽i⩽M.\end{array}$$

Finally, by substituting $(36)$, $(41)$ and $(43)$ into $(39)$, we obtain that the optimal controls of two consecutive stages satisfy

$$\begin{array}{}\text{(44)}& {{\mathit{v}}^{\ast }}_{[i]}^{(j)}(t_i)={{\mathit{v}}^{\ast }}_{[i+1]}^{(j)}(t_i),0⩽j<(s-d_i).\end{array}$$

We finally know that the optimal control of the problem $(18)$ is actually $s-d_i-1$ times continuously differentiable at the timestamp $t_i$. Accordingly, the optimal state trajectory, consisting of $M$ polynomials with $2s-1$ degree, is indeed $2s-d_i-1$ times continuously differentiable at $t_i$.

Now we conclude the conditions derived from both $(39)$ and $(44)$ in the following theorem, which are proved to be necessary and sufficient optimality conditions of $(18)$.

Theorem 2: Optimality Conditions

A trajectory, denoted by ${\mathit{z}}^{\ast }(t):[t_0,t_M]\mapsto {\mathbb{R}}^m$, is optimal for the problem $(18)$, if and only if the following conditions are satisfied:

• The map ${\mathit{z}}^{\ast }(t):[t_{i-1},t_i]\mapsto {\mathbb{R}}^m$ is parameterized as a $2s-1$ degree polynomial for any $1⩽i⩽M$;
• The boundary conditions in $(18c)$;
• The intermediate conditions in $(18d)$;
• ${\mathit{z}}^{\ast }(t)$ is ${\overline{d}}_i-1$ times continuously differentiable at $t_i$ for any $1⩽i<M$ where ${\overline{d}}_i=2s-d_i$.

Moreover, a unique trajectory exists for these conditions.

Proof (Sketch):

Details of Proof (Sketch)

The proof of necessity is evident in the analyses from $(33)$ to $(44)$ that are directly derived from Theorem 1. The proof of sufficiency is sketched below:

• The first and fourth conditions always determine a linear spline space of dimension $2s+\sum _{i=1}^{M-1}{d}_i$ for any sequence of $d_i$;
• The second and third conditions are shown to form a square coefficient matrix on a basis of the spline space;
• The matrix is proved to be nonsingular since $t_{i-1}<t_i$ for each $i$, implying the existence and uniqueness of solution;
• The existence and uniqueness for the necessary conditions yield their sufficiency. This proof of sufficiency is detailed in Appendix of Paper (TODO).

To further explain the optimality conditions, we take the multi-stage jerk minimization as an example.

In this example, the position, velocity and acceleration are states of the jerk-controlled system ($s=3$). There are intermediate points ($d_i=1$) that the trajectory should pass through at certain timestamps.

The continuity of state only requires the continuity up to acceleration of the minimum-jerk trajectory, while jerk and snap of the optimal trajectory are also continuous everywhere. Accordingly, if we enforce all these continuity conditions, then Theorem 2 guarantees that only one trajectory exists, which is exactly the optimal one.

2.3 Minimization Without Cost Functional

Theorem 2 provides a direct way to construct the unique optimal trajectory. The computation enjoys linear complexity in time and space. It does not even require explicit or implicit evaluation of the cost functional or its gradient.

Consider an $m$-dimensional trajectory whose $i$-th piece is denoted by an $N=2s-1$ degree polynomial:

$$\begin{array}{}\text{(45)}& {\mathit{p}}_i(t)={\mathbf{c}}_i^{\mathrm{\top }}\mathit{\beta }(t-t_{i-1}),t\in [t_{i-1},t_i],\end{array}$$

where $\mathit{\beta }(x)={(1,x,\dots ,x^N)}^{\mathrm{\top }}$ is the basis and ${\mathbf{c}}_i\in {\mathbb{R}}^{2s\times m}$ the coefficients. For simplicity, we use the timeline relative to $t_0=0$. The trajectory is described by a coefficient matrix $\mathbf{c}\in {\mathbb{R}}^{2Ms\times m}$ and a time vector $\mathbf{T}\in {\mathbb{R}}_{>0}^M$ defined by

$$\begin{array}{}\text{(46)}& \mathbf{c}={({\mathbf{c}}_1^{\mathrm{\top }},\dots ,{\mathbf{c}}_M^{\mathrm{\top }})}^{\mathrm{\top }},\mathbf{T}={(T_1,\dots ,T_M)}^{\mathrm{\top }},\end{array}$$

where $T_i$ means the duration of the $i$-th piece. Then we have the timestamp $t_i=\sum _{j=1}^i{T}_j$ and the total duration $T=\parallel \mathbf{T}{\parallel }_1$. The $M$-piece trajectory $\mathit{p}:[0,T]\mapsto {\mathbb{R}}^m$ is defined by

$$\begin{array}{}\text{(47)}& \mathit{p}(t)={\mathit{p}}_i(t),\mathrm{\forall }t\in [t_{i-1},t_i),\mathrm{\forall }i\in \{1,\dots ,M\}.\end{array}$$

To compute the unique solution for $(18)$, we directly enforce optimality conditions on the coefficient matrix $\mathbf{c}$. Denote by ${\mathbf{D}}_0,{\mathbf{D}}_M\in {\mathbb{R}}^{s\times m}$ and ${\mathbf{D}}_i\in {\mathbb{R}}^{d_i\times m}$ the specified derivatives at boundaries and intermediate timestamp $t_i$, respectively. Each column of ${\mathbf{D}}_i$ is related to one dimension. Then, conditions at $t_i$ are formulated by ${\mathbf{E}}_i,{\mathbf{F}}_i\in {\mathbb{R}}^{2s\times 2s}$:

$$\begin{array}{}\text{(48)}& \left(\begin{array}{cc}{\mathbf{E}}_i& {\mathbf{F}}_i\end{array}\right)\left(\begin{array}{c}{\mathbf{c}}_i\\ {\mathbf{c}}_{i+1}\end{array}\right)=\left(\begin{array}{c}{\mathbf{D}}_i\\ {\mathbf{0}}_{{\overline{d}}_i\times m}\end{array}\right),\end{array}$$

$$\begin{array}{rl}{\mathbf{E}}_i=(& \mathit{\beta }(T_i),\dots ,{\mathit{\beta }}^{(d_i-1)}(T_i),\\ \text{(49)}& & \mathit{\beta }(T_i),\dots ,{\mathit{\beta }}^{({\overline{d}}_i-1)}(T_i){)}^{\mathrm{\top }},\text{(50)}& {\mathbf{F}}_i=(& \mathbf{0},-\mathit{\beta }(0),\dots ,-{\mathit{\beta }}^{({\overline{d}}_i-1)}(0){)}^{\mathrm{\top }}.\end{array}$$

Especially, define ${\mathbf{F}}_0,{\mathbf{E}}_M\in {\mathbb{R}}^{s\times 2s}$ as

$$\begin{array}{}\text{(51)}& {\mathbf{F}}_0=& {(\mathit{\beta }(0),\dots ,{\mathit{\beta }}^{(s-1)}(0))}^{\mathrm{\top }},\text{(52)}& {\mathbf{E}}_M=& {(\mathit{\beta }(T_M),\dots ,{\mathit{\beta }}^{(s-1)}(T_M))}^{\mathrm{\top }}.\end{array}$$

The linear system for the optimal coefficient matrix is

$$\begin{array}{}\text{(53)}& \mathbf{M}\mathbf{c}=\mathbf{b}\end{array}$$

where $\mathbf{M}\in {\mathbb{R}}^{2Ms\times 2Ms}$ and $\mathbf{b}\in {\mathbb{R}}^{2Ms\times m}$ are

$$\begin{array}{}\text{(54)}& \mathbf{M}=\left(\begin{array}{ccccc}{\mathbf{F}}_0& \mathbf{0}& \mathbf{0}& \cdots & \mathbf{0}\\ {\mathbf{E}}_1& {\mathbf{F}}_1& \mathbf{0}& \cdots & \mathbf{0}\\ \mathbf{0}& {\mathbf{E}}_2& {\mathbf{F}}_2& \cdots & \mathbf{0}\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \cdots & {\mathbf{F}}_{M-1}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \cdots & {\mathbf{E}}_M\end{array}\right),\end{array}$$

$$\begin{array}{}\text{(55)}& \mathbf{b}={({\mathbf{D}}_0^{\mathrm{\top }},{\mathbf{D}}_1^{\mathrm{\top }},{\mathbf{0}}_{m\times {\overline{d}}_1},\dots ,{\mathbf{D}}_{M-1}^{\mathrm{\top }},{\mathbf{0}}_{m\times {\overline{d}}_{M-1}},{\mathbf{D}}_M^{\mathrm{\top }})}^{\mathrm{\top }}.\end{array}$$

It is essential that the uniqueness in Theorem 2 ensures the nonsingularity of $\mathbf{M}$ for any time vector $\mathbf{T}\succ \mathbf{0}$. Consequently, the unique solution $\mathbf{c}$ can be obtained via linear equation system $(53)$ with a banded matrix $\mathbf{M}$, i.e., a banded system.

As for a nonsingular banded system, its Banded PLU Factorization always exists, which can be employed to compute the solution with $O(M)$ time and space complexity. Therefore, without the need of cost functional, the unique solution of problem $(18)$ is obtained in the lowest complexity, by directly applying our optimality conditions.

2.4 MINCO Trajectories With Spatial-Temporal Deformation

For multicopters, there are often task-specific requirements apart from feasibility, such as the perception quality in active SLAM or the occlusion rate in aerial videography. These user-defined requirements majorly need to flexibly and adaptively deform both the spatial and temporal profile of a trajectory. Therefore, we select the intermediate points and the time vector as two salient parameters in $(18)$. Fortunately, the existence and uniqueness of solution guarantee the smoothness of sensitivity for them. An iterative procedure is then designed to conduct the spatial-temporal deformation with the lowest computation complexity per iteration.

We denote the intermediate points by $\mathbf{q}=({\mathit{q}}_1,\dots ,{\mathit{q}}_{M-1})$ where ${\mathit{q}}_i\in {\mathbb{R}}^m$ is a specified zero-order derivative at $t_i$. Denote by $\mathbf{T}={(T_1,\dots ,T_M)}^{\mathrm{\top }}$ the time vector where $T_i\in {\mathbb{R}}_{>0}$. For any pair of $\mathbf{q}$ and $\mathbf{T}$, Theorem 2 naturally determines a trajectory belonging to a class of control effort minimizers, named MINCO hereafter. The MINCO trajectory class, denoted by ${\mathfrak{T}}_{\mathrm{MINCO}}$, is defined as

$$\begin{array}{rl}{\mathfrak{T}}_{\mathrm{MINCO}}=\{& p(t):[0,T]\mapsto {\mathbb{R}}^m|\mathbf{c}=\mathbf{c}(\mathbf{q},\mathbf{T})\text{ determined}\\ & \text{by Theorem 2,}\mathrm{\forall }\mathbf{q}\in {\mathbb{R}}^{m\times (M-1)},\mathbf{T}\in {\mathbb{R}}_{>0}^M\}.\end{array}$$

The dimension $m$, the system order $s$, initial and terminal conditions are omitted here for brevity.

Intuitively, all trajectories in ${\mathfrak{T}}_{\mathrm{MINCO}}$ are compactly parameterized by only $\mathbf{q}$ and $\mathbf{T}$. Evaluating an element in ${\mathfrak{T}}_{\mathrm{MINCO}}$ directly follows our linear-complexity formulation.

We denote any user-defined objective (or constraint) on a trajectory by a $C^2$ function $\mathcal{K}(\mathbf{c},\mathbf{T})$ with available gradient. This objective on ${\mathfrak{T}}_{\mathrm{MINCO}}$ can be computed as

$$\begin{array}{}\text{(56)}& \mathcal{W}(\mathbf{q},\mathbf{T})=\mathcal{K}(\mathbf{c}(\mathbf{q},\mathbf{T}),\mathbf{T}).\end{array}$$

To accomplish deformation of ${\mathfrak{T}}_{\mathrm{MINCO}}$, the function $\mathcal{W}$ together with its gradient $\partial \mathcal{W}/\partial \mathbf{q}$ and $\partial \mathcal{W}/\partial \mathbf{T}$ are needed for a high-level optimizer to optimize the objective.

Obviously, evaluating $\mathcal{W}$ shares the same complexity as evaluating any trajectory in ${\mathfrak{T}}_{\mathrm{MINCO}}$. The key procedure is to compute the gradient.

Now we give a linear-complexity scheme to compute $\partial \mathcal{W}/\partial \mathbf{q}$ and $\partial \mathcal{W}/\partial \mathbf{T}$ from the given $\partial \mathcal{K}/\partial \mathbf{c}$ and $\partial \mathcal{K}/\partial \mathbf{T}$. We first rewrite the linear equation system $(53)$ as

$$\begin{array}{}\text{(57)}& \mathbf{M}(\mathbf{T})\mathbf{c}(\mathbf{q},\mathbf{T})=\mathbf{b}(\mathbf{q}).\end{array}$$

Without causing ambiguity, we omit parameters in $\mathbf{M},\mathbf{b},\mathbf{c},\mathcal{K}$ and $\mathcal{W}$ temporarily for simplicity. Any notation involving $\mathbf{c}$ is interpreted as $\mathbf{c}(\mathbf{q},\mathbf{T})$. Denote by $q_{i,j}$ the $j$-th entry in $q_i$.

As for the gradient w.r.t. $\mathbf{q}$, we first differentiate both sides of $(57)$ w.r.t. $q_{i,j}$, which gives

$$\begin{array}{}\text{(58)}& \frac{\partial \mathbf{c}}{\partial q_{i,j}}={\mathbf{M}}^{-1}\frac{\partial \mathbf{b}}{\partial q_{i,j}}.\end{array}$$

Then,

$$\begin{array}{rl}\frac{\partial \mathcal{W}}{\partial q_{i,j}}& =\mathrm{Tr}\left\{{\left(\frac{\partial \mathbf{c}}{\partial q_{i,j}}\right)}^{\mathrm{\top }}\frac{\partial \mathcal{K}}{\partial \mathbf{c}}\right\}\\ & =\mathrm{Tr}\left\{{\left({\mathbf{M}}^{-1}\frac{\partial \mathbf{b}}{\partial q_{i,j}}\right)}^{\mathrm{\top }}\frac{\partial \mathcal{K}}{\partial \mathbf{c}}\right\}\\ \text{(59)}& & =\mathrm{Tr}\left\{{\left(\frac{\partial \mathbf{b}}{\partial q_{i,j}}\right)}^{\mathrm{\top }}\left({\mathbf{M}}^{-\mathrm{\top }}\frac{\partial \mathcal{K}}{\partial \mathbf{c}}\right)\right\},\end{array}$$

where $\mathrm{Tr}(\cdot )$ is the trace operation.

The definition of $\mathbf{b}(\mathbf{q})$ in $(55)$ implies that $\partial \mathbf{b}/\partial q_{i,j}$ only has a single nonzero entry $1$ at its $(2i-1)s+1$ row and $j$ column. Thus, stacking all the resultant scalars gives

$$\begin{array}{}\text{(60)}& \frac{\partial \mathcal{W}}{\partial q_i}={\left({\mathbf{M}}^{-1}\frac{\partial \mathcal{K}}{\partial \mathbf{c}}\right)}^{\mathrm{\top }}{e}_{(2i-1)s+1},\end{array}$$

where $e_j$ is the $j$-th column of ${\mathbf{I}}_{2Ms}$. Now that we have already conducted the banded PLU factorization for $\mathbf{M}$ when we compute $\mathbf{c}$. We can reuse the factorization to avoid inverting ${\mathbf{M}}^{\mathrm{\top }}$. Define a matrix $\mathbf{G}\in {\mathbb{R}}^{2Ms\times m}$ as

$$\begin{array}{}\text{(61)}& {\mathbf{M}}^{\mathrm{\top }}\mathbf{G}=\frac{\partial \mathcal{K}}{\partial \mathbf{c}}.\end{array}$$

We only need to compute $\mathbf{G}$ once to obtain $\partial \mathcal{W}/\partial q_i$ for all $1⩽i<M$. Denote the factorization of $\mathbf{M}$ as $\mathbf{M}=\mathbf{P}\mathbf{L}\mathbf{U}$. Specifically, $\mathbf{L}$ is a banded matrix with zero upper bandwidth and all-ones diagonal entries. $\mathbf{U}$ is a banded matrix with zero lower bandwidth and nonzero diagonal entries because of the nonsingularity of $\mathbf{M}$. The pivoting matrix $\mathbf{P}$ simply changes the row order of the operand, satisfying ${\mathbf{P}}^{\mathrm{\top }}\mathbf{P}=\mathbf{I}$. Consequently, the transpose also has a Banded LUP Factorization. Specifically, ${\mathbf{M}}^{\mathrm{\top }}=\overline{\mathbf{L}}\overline{\mathbf{U}}{\mathbf{P}}^{\mathrm{\top }}$, where

$$\begin{array}{}\text{(62)}& \overline{\mathbf{L}}={\mathbf{U}}^{\mathrm{\top }}{\left(\mathbf{U}\circ \mathbf{I}\right)}^{-1},\overline{\mathbf{U}}=\left(\mathbf{I}\circ \mathbf{U}\right){\mathbf{L}}^{\mathrm{\top }},\end{array}$$

where the inverse is only done for a diagonal matrix and $\circ$ the Hadamard product. Then, $\mathbf{G}$ can be also computed in linear complexity through such a factorization. For convenience, we partition $\mathbf{G}$ into

$$\begin{array}{}\text{(63)}& \mathbf{G}={({\mathbf{G}}_0^{\mathrm{\top }},{\mathbf{G}}_1^{\mathrm{\top }},\dots ,{\mathbf{G}}_{M-1}^{\mathrm{\top }},{\mathbf{G}}_M^{\mathrm{\top }})}^{\mathrm{\top }}\end{array}$$

such that ${\mathbf{G}}_0,{\mathbf{G}}_M\in {\mathbb{R}}^{s\times m}$ and ${\mathbf{G}}_i\in {\mathbb{R}}^{2s\times m}$ for $1⩽i<M$. After that, the gradient of $\mathcal{W}$ w.r.t. $\mathbf{q}$ is determined as

$$\begin{array}{}\text{(64)}& \frac{\partial \mathcal{W}}{\partial \mathbf{q}}=({\mathbf{G}}_1^{\mathrm{\top }}{e}_1,\dots ,{\mathbf{G}}_{M-1}^{\mathrm{\top }}{e}_1),\end{array}$$

where $e_1$ is the first column of ${\mathbf{I}}_{2s}$. This operation takes out $M-1$ specific columns in ${\mathbf{G}}^{\mathrm{\top }}$.

As for the gradient w.r.t. $\mathbf{T}$, differentiating both sides of $(57)$ w.r.t. $T_i$ gives

$$\begin{array}{}\text{(65)}& \frac{\partial \mathbf{M}}{\partial T_i}\mathbf{c}+\mathbf{M}\frac{\partial \mathbf{c}}{\partial T_i}=\mathbf{0}.\end{array}$$

Thus,

$$\begin{array}{rl}\frac{\partial \mathcal{W}}{\partial T_i}& =\frac{\partial \mathcal{K}}{\partial T_i}+\mathrm{Tr}\left\{{\left(\frac{\partial \mathbf{c}}{\partial T_i}\right)}^{\mathrm{\top }}\frac{\partial \mathcal{K}}{\partial \mathbf{c}}\right\}\\ & =\frac{\partial \mathcal{K}}{\partial T_i}-\mathrm{Tr}\left\{{\left(\frac{\partial \mathbf{M}}{\partial T_i}\right)}^{\mathrm{\top }}{\mathbf{M}}^{-1}\frac{\partial \mathcal{K}}{\partial \mathbf{c}}\right\}\\ \text{(66)}& & =\frac{\partial \mathcal{K}}{\partial T_i}-\mathrm{Tr}\left\{{\mathbf{G}}_i^{\mathrm{\top }}\frac{\partial \mathbf{M}}{\partial T_i}\mathbf{c}\right\}\end{array}$$

The banded structure of $\mathbf{M}$ implies that

$${\mathbf{G}}^{\mathrm{\top }}\frac{\partial \mathbf{M}}{\partial T_i}\mathbf{c}={\mathbf{G}}_i^{\mathrm{\top }}\frac{\partial {\mathbf{E}}_i}{\partial T_i}{\mathbf{c}}_i.$$

Then we obtain the gradient w.r.t. $T_i$ computed as

$$\begin{array}{}\text{(68)}& \frac{\partial \mathcal{W}}{\partial T_i}=\frac{\partial \mathcal{K}}{\partial T_i}-\mathrm{Tr}\left\{{\mathbf{G}}_i^{\mathrm{\top }}\frac{\partial {\mathbf{E}}_i}{\partial T_i}{\mathbf{c}}_i\right\},\end{array}$$

where $\partial {\mathbf{E}}_i/\partial T_i$ can be analytically derived from $(64)$. Computing $(68)$ for every $1⩽i⩽M$ gives $\partial \mathcal{W}/\partial \mathbf{T}$.

Finally, we finish the computation of $\partial \mathcal{W}/\partial \mathbf{q}$ and $\partial \mathcal{W}/\partial \mathbf{T}$. It can be verified from both $(64)$ and $(68)$ that the gradient propagation is also done in $O(M)$ complexity. As for $\mathcal{K}$, we make no assumption on its concrete form. Actually, the smoothness of $\mathcal{K}$ is not even needed if only the resultant $\mathcal{W}$ is $C^2$. In other word, the linear-complexity gradient propagation enjoys both efficiency and flexibility. By incorporating it into common optimizers, we can accomplish the spatial-temporal deformation of ${\mathfrak{T}}_{\mathrm{MINCO}}$ for a wide range of planning purposes while maintaining the local smoothness of trajectories.

3. Geometrically Constrained Flight Trajectory Optimization

In this section, we provide a unified framework for flight trajectory optimization with different time regularization $\rho (T)$, spatial constraints $\tilde{\mathcal{F}}$ and continuous-time constraints $\mathcal{G}$. This framework indeed relaxes the original problem into ${\mathfrak{T}}_{\mathrm{MINCO}}$.

The spatial-temporal deformation is utilized to meet various feasibility requirements. Lightweight schemes are specially designed to eliminate geometrical constraints such that the trajectory can be freely deformed. For continuous-time constraints, a time integral penalty functional is proposed to ensure the feasibility without sacrificing the scalability.

Finally, our framework transforms the constrained trajectory optimization into a sparse unconstrained one which can be reliably solved.

3.1 Temporal Constraint Elimination

Figure 2[1]: Left: Domain of $J$ on an $M$-piece trajectory with total time fixed as $T_\Sigma$. The domain is indeed the relative interior of an $(M-1)$-simplex in $\mathbb{R}^M_{>0}$. Right: Contour of $\ln{J}$ with $M=3$. The function goes to infinity as the time vector approaches the boundary of the open domain in $\mathbb{R}^2_{>0}$.

Deforming MINCO needs standard optimizers that are often designed for Euclidean spaces. However, the trajectory definition and cost functional~$(11)$ both restrict the domain of $\mathbf{T}$ to simple manifolds, on which frequent retractions are needed during optimization. We give explicit diffeomorphisms for $\mathbf{T}$ such that surrogate variables are in Euclidean spaces. Thus, common efficient optimizers can be conveniently applied.

For polynomial splines, the control effort in $(11)$ is a function of $\mathbf{c}$ and $\mathbf{T}$, denoted by $J_c(\mathbf{c},\mathbf{T})$. Analytical expressions of $J_c$, $\partial J_c/\partial \mathbf{c}$, and $\partial J_c/\partial \mathbf{T}$ are available in literatures(See original paper). Now that ${\mathfrak{T}}_{\mathrm{MINCO}}$ are polynomial splines with coefficients determined by $\mathbf{c}(\mathbf{q},\mathbf{T})$, the cost functional of $(11)$ can be written as

$$\begin{array}{}\text{(69)}& J(\mathbf{q},\mathbf{T})=J_q(\mathbf{q},\mathbf{T})+\rho (\parallel \mathbf{T}{\parallel }_1),\end{array}$$

where $J_q$ is defined as $J_q(\mathbf{q},\mathbf{T})=J_c(\mathbf{c}(\mathbf{q},\mathbf{T}),\mathbf{T})$. Obviously, computing $J_q$, $\partial J_q/\partial \mathbf{q}$, and $\partial J_q/\partial \mathbf{T}$ from any provided $J_c$, $\partial J_c/\partial \mathbf{c}$, and $\partial J_c/\partial \mathbf{T}$ can be done in $O(M)$ complexity, as already shown in deformation of ${\mathfrak{T}}_{\mathrm{MINCO}}$.

It is natural to optimize $\mathbf{T}$ via $\partial J/\partial \mathbf{T}$. However, $J_q(\mathbf{q},\mathbf{T})$ has its definition over $\mathbf{T}\in {\mathbb{R}}_{>0}^M$. It becomes unbounded when any $T_i$ approaches zero and no consecutively repeating points appear in $\mathbf{q}$. Besides, ${\rho }_f$ defined in $(13)$ further restricts the domain of $J$ to $\sum _{i=1}^{M-1}{T}_i<T_{\mathrm{\Sigma }}$, as shown in Fig. 2.

We use diffeomorphisms to eliminate constraints for ${\rho }_f$ and ${\rho }_s$. Consider the domain of ${\rho }_f$ in $(13)$,

$$\begin{array}{}\text{(70)}& {\mathcal{T}}_f=\{\mathbf{T}\in {\mathbb{R}}^M|\parallel \mathbf{T}{\parallel }_1=T_{\mathrm{\Sigma }},\mathbf{T}\succ \mathbf{0}\}.\end{array}$$

It is clear that $J(\mathbf{q},\cdot )$ is finite for a nontrivial $\mathbf{q}$ if and only if $\mathbf{T}\in \mathrm{RelInt}({\mathcal{T}}_f)$, i.e., the relative interior of ${\mathcal{T}}_f$.

Proposition 1:

${\mathcal{T}}_f$ defined by $(70)$ is diffeomorphic to ${\mathbb{R}}^{M-1}$. Denote by $\mathit{\tau }=({\tau }_1,\dots ,{\tau }_{M-1})$ an element in ${\mathbb{R}}^{M-1}$. A $C^{\mathrm{\infty }}$ diffeomorphism is given by the map below for $1⩽i<M$:

$$\begin{array}{}\text{(71)}& T_i=\frac{e^{{\tau }_i}}{1+\sum _{j=1}^{M-1}{e}^{{\tau }_j}}{T}_{\mathrm{\Sigma }},T_M=T_{\mathrm{\Sigma }}-\sum _{j=1}^{M-1}{T}_j.\end{array}$$

By exploiting the explicit diffeomorphism $(71)$, we directly minimize the cost function $J$ over ${\mathbb{R}}^{M-1}$ via $\mathit{\tau }$, because the domain constraints are satisfied by default.

Optimizing $\mathit{\tau }$ requires gradient propagation. We partition the gradient as $\partial J_q/\partial \mathbf{T}={(g_a^{\mathrm{\top }},g_b)}^{\mathrm{\top }}$, where $g_a\in {\mathbb{R}}^{M-1}$ and $g_b\in \mathbb{R}$.

Differentiating the layer in $(71)$ yields the gradient of $J$ w.r.t. $\mathit{\tau }$,

$$\begin{array}{}\text{(72)}& \frac{\partial J}{\partial \mathit{\tau }}=\frac{(g_a-g_b\mathbf{1})\circ e^{[\mathit{\tau }]}}{1+\parallel e^{[\mathit{\tau }]}{\parallel }_1}-\frac{(g_a^{\mathrm{\top }}{e}^{[\mathit{\tau }]}-g_b\parallel e^{[\mathit{\tau }]}{\parallel }_1)e^{[\mathit{\tau }]}}{{(1+\parallel e^{[\mathit{\tau }]}{\parallel }_1)}^2},\end{array}$$

where $e^{[\cdot ]}$ is the entry-wise exponential map, and $\mathbf{1}$ an all-ones vector. If an initial guess $\mathbf{T}$ is specified, the corresponding $\mathit{\tau }$ can be computed via the inverse map of the diffeomorphism, given by ${\tau }_i=\ln \left(T_i/T_M\right)$ for $1⩽i<M$. As for ${\rho }_s$ in $(12)$, only $\mathbf{T}\succ \mathbf{0}$ needs to be ensured. It suffices to use $\mathbf{T}=e^{[\mathit{\tau }]}$ as the diffeomorphism between ${\mathbb{R}}^M$ and ${\mathbb{R}}_{>0}^M$.

For either ${\rho }_f$ or ${\rho }_s$, we denote the diffeomorphism by $\mathbf{T}(\mathit{\tau })$. Unconstrained optimization on $\mathit{\tau }$ can be directly conducted to minimize $J(\mathbf{q},\mathbf{T}(\mathit{\tau }))$. Although $\mathbf{T}(\mathit{\tau })$ does not preserve convexity, the original cost $J(\mathbf{q},\mathbf{T})$ is already nonconvex as given in $(57)$.

Thus, the only concern is whether $\mathbf{T}(\mathit{\tau })$ brings new local minima in the space of $\mathit{\tau }$ or eliminates local minima in the space of $\mathbf{T}$.

Proposition 2

Denote by $F:{\mathbb{D}}_{\mathrm{F}}\mapsto \mathbb{R}$ any $C^2$ function with a convex open domain ${\mathbb{D}}_{\mathrm{F}}\in {\mathbb{R}}^N$. Given any $C^2$ diffeomorphism $\mathbf{G}:{\mathbb{R}}^N\mapsto {\mathbb{D}}_{\mathrm{F}}$, define $H:{\mathbb{R}}^N\mapsto \mathbb{R}$ as $H(y)=F(\mathbf{G}(y))$ for $y\in {\mathbb{R}}^N$. For any $x\in {\mathbb{D}}_{\mathrm{F}}$ and $y\in {\mathbb{R}}^N$ satisfying $x=\mathbf{G}(y)$ or equivalently $y={\mathbf{G}}^{-1}(x)$, the following statements always hold:

• $\mathrm{\nabla }F(x)=\mathbf{0}$ if and only if $\mathrm{\nabla }H(y)=\mathbf{0}$;
• ${\mathrm{\nabla }}^{2}F(x)$ is positive-definite (or positive-semidefinite) at $\mathrm{\nabla }F(x)=\mathbf{0}$, if and only if ${\mathrm{\nabla }}^{2}H(y)$ is positive-definite (or positive-semidefinite) at $\mathrm{\nabla }H(y)=\mathbf{0}$.

Proof: See Appendix in the original paper.

Proposition 2 confirms that $\mathbf{T}(\mathit{\tau })$ preserves the first/second-order necessary optimality conditions and second-order sufficient optimality conditions. It is also applicable to substitute the exponential map in this subsection with any $C^2$ diffeomorphism from $\mathbb{R}$ to ${\mathbb{R}}_{>0}$ for a better numerical condition. In the sense of commonly-used optimality conditions, our constraint elimination does not produce extra spurious local minima or cancel any existing one.

3.2 Spherical Spatial Constraint Elimination

Figure 3[1]: Inverse stereographic projection $f_s$ maps the Euclidean space $\mathbb{R}^n$ onto a sphere without north pole $\mathcal{S}^n_\odot$ in an $(n+1)$-dimensional space. The orthographic projection $f_o$ maps $\mathcal{S}^n_\odot$ onto an $n$-dimensional ball $\mathcal{B}^n$. The variable $\xi$ moves freely in $\mathbb{R}^n$ while the transformed variable $q$ stays in $\mathcal{B}^n$. Optimization on $\xi$ becomes unconstrained when $q$ is constrained by a ball.

We enforce motion safety by confining trajectories into the feasible region $\tilde{\mathcal{F}}$. Although $\tilde{\mathcal{F}}$ is nonconvex, it is a union of convex primitives that are sequentially connected. If all pieces have been assigned into these primitives, the safety constraint on each piece becomes convex and thus can be conveniently encoded in $\mathcal{G}$. Owing to the feature of MINCO, the traverse time for every primitive can be directly optimized. Thus, we fix the piece assignment before optimization, rather than resorting to integer variables during optimization. Consequently, intermediate points should be contained by the overlap between primitives, forming inequalities. For Inequality Constrained Problems (ICPs), general methods successively approximate the constraints via additional parameters. However, we aim to apply the constraints directly and efficiently. Therefore, we propose spatial constraint elimination to enforce them exactly, leveraging their geometrical properties.

Consider the constraint $q\in \mathcal{P}\subset {\mathbb{R}}^n$ where $\mathcal{P}$ is a closed ball. Its dimension satisfies $n⩽m$ since a low-dimensional constraint also exists in ${\mathbb{R}}^m$. If $\mathcal{P}$ is a closed ball ${\mathcal{P}}^{\mathcal{B}}$ centered at point $o$ with radius $r$,

$$\begin{array}{}\text{(73)}& {\mathcal{P}}^{\mathcal{B}}=\{\mathit{x}\in {\mathbb{R}}^n|\parallel \mathit{x}-o{\parallel }_2⩽r\},\end{array}$$

We utilize a smooth surjection to map ${\mathbb{R}}^n$ to ${\mathcal{P}}^{\mathcal{B}}$ such that optimization over ${\mathbb{R}}^n$ implicitly satisfies the constraint ${\mathcal{P}}^{\mathcal{B}}$. As illustrated in Fig. 3, the map is a composition of the inverse stereographic projection and the orthographic projection. First, we utilize the inverse stereographic projection to map ${\mathbb{R}}^n$ to ${\mathcal{S}}_{\odot }^n$, where ${\mathcal{S}}_{\odot }^n$ is a unit sphere without north pole, i.e.,

$$\begin{array}{}\text{(74)}& {\mathcal{S}}_{\odot }^n=\{\mathit{x}\in {\mathbb{R}}^{n+1}|\parallel \mathit{x}{\parallel }_2=1,x_{n+1}<1\}.\end{array}$$

The inverse stereographic projection $f_s$ is define as

$$\begin{array}{}\text{(75)}& f_s(x)=\frac{{(2x^{\mathrm{\top }},x^{\mathrm{\top }}x-1)}^{\mathrm{\top }}}{x^{\mathrm{\top }}x+1}\in {\mathcal{S}}_{\odot }^n,\mathrm{\forall }x\in {\mathbb{R}}^n.\end{array}$$

Note that $f_s$ is a diffeomorphism between ${\mathbb{R}}^n$ and ${\mathcal{S}}_{\odot }^n$. We then project ${\mathcal{S}}_{\odot }^n$ from ${\mathbb{R}}^{n+1}$ back in ${\mathbb{R}}^n$ to obtain

$$\begin{array}{}\text{(76)}& {\mathcal{B}}^n=\{\mathit{x}\in {\mathbb{R}}^n|\parallel \mathit{x}{\parallel }_2\le 1\}.\end{array}$$

The map is described by

$$\begin{array}{}\text{(77)}& f_o(x)={(x_1,\dots ,x_n)}^{\mathrm{\top }}\in {\mathcal{B}}^n,\mathrm{\forall }x\in {\mathcal{S}}_{\odot }^n,\end{array}$$

which is indeed a smooth surjection onto ${\mathcal{B}}^n$. Each point in ${\mathcal{B}}^n$, except the center, is paired with two points in ${\mathcal{S}}_{\odot }^n$. The composition of $f_s$, $f_o$, and a linear transformation, is a smooth surjection:

$$\begin{array}{}\text{(78)}& f_{\mathcal{B}}(x)=o+\frac{2rx}{x^{\mathrm{\top }}x+1}\in {\mathcal{P}}^{\mathcal{B}},\mathrm{\forall }x\in {\mathbb{R}}^n.\end{array}$$

The map $f_{\mathcal{B}}$ introduces a new coordinate, denoted by $\xi$, such that optimizing $\xi$ over ${\mathbb{R}}^n$ always satisfies the constraint on $q$ described by ${\mathcal{P}}^{\mathcal{B}}$. For the $i$-th intermediate point $q_i$, denote by ${\xi }_i$ the corresponding new coordinate.

Accordingly, denote by $\mathit{\xi }$ the new coordinate for $\mathbf{q}$. Optimizing $\mathit{\xi }$ requires gradient propagation for $\partial J/\partial \mathbf{q}$.

Denote by $g_i$ the $i$-th entry $\partial J/\partial q_i$ in $\partial J/\partial \mathbf{q}$. Differentiating the layer $f_{\mathcal{B}}$ gives the gradient

$$\begin{array}{}\text{(79)}& \frac{\partial J}{\partial {\xi }_i}=\frac{2r_i{g}_i}{{\xi }_i^{\mathrm{\top }}{\xi }_i+1}-\frac{4r_i({\xi }_i^{\mathrm{\top }}{g}_i){\xi }_i}{({\xi }_i^{\mathrm{\top }}{\xi }_i+1{)}^2}.\end{array}$$

If the optimization needs to start from an initial guess $\mathbf{q}$, the backward evaluation of $\mathit{\xi }$ can be done by using a local inverse of $f_{\mathcal{B}}$, given by ${\xi }_i$ for $1⩽i<M$:

$$\begin{array}{}\text{(80)}& {\xi }_i=\frac{r_i-\sqrt{r_i^2-\parallel q_i-o_i{\parallel }_2^2}}{\parallel q_i-o_i{\parallel }_2^2}(q_i-o_i).\end{array}$$

Figure 4[1]: Constrained minimum $q^*$ of a convex function $J(q)$ within a 2-D ball. Transformed by $f_\mathcal{B}$, the resultant function $J(f_\mathcal{B}(\xi))$ becomes nonconvex but it preserves the local minimum $\xi^*$ satisfying $q^*=f_\mathcal{B}(\xi^*)$ with no additional local minimum introduced.

Similarly, we analyze influences that the smooth surjection $f_{\mathcal{B}}$ imposes on the constrained local minima in ${\mathcal{P}}^{\mathcal{B}}$. Although $f_{\mathcal{B}}$ lacks the one-to-one correspondence as diffeomorphisms possess, its components are all well-formed. Firstly, $f_o$ only takes the first $n$ entries of a point. This operation preserves at least the first-order necessary conditions for local minima in either ${\mathcal{B}}^n$ or ${\mathcal{S}}_{\odot }^n$. Secondly, $f_s$ is a diffeomorphism between ${\mathcal{S}}_{\odot }^n$ and ${\mathbb{R}}^n$, thus satisfying Proposition 2. Therefore, we can also confirm that $f_{\mathcal{B}}$ does not produce extra spurious local minima or cancel any existing one. As shown in Fig. 4, the constrained minimum within a 2-D ball is transformed into an unconstrained minimum.

3.3 Polyhedral Spatial Constraint Elimination

Figure 5[1]: Transformations on a convex polytope. A convex polytope $\mathcal{P}^\mathcal{H}$ with $\hat{n}+1$ vertices is indeed a standard $\hat{n}$-simplex in the barycentric coordinate. The simplex $\mathcal{P}^\mathcal{H}_w$ is then the image of an entry-wise square map $[\cdot]^2$ with ball-shaped domain, which can be eliminated as in Fig. 3.

Now we consider the elimination of polyhedral constraints. Specifically, $\mathcal{P}$ is a closed convex polytope ${\mathcal{P}}^{\mathcal{H}}$ defined by

$$\begin{array}{}\text{(81)}& {\mathcal{P}}^{\mathcal{H}}=\{x\in {\mathbb{R}}^n|\mathbf{A}x⪯b\}.\end{array}$$

where $\mathrm{Int}\left({\mathcal{P}}^{\mathcal{H}}\right)\ne \varnothing$ according to $(15)$. Common optimization algorithms use the $\mathcal{H}$-representation of ${\mathcal{P}}^{\mathcal{H}}$ as linear inequality constraints. In our framework, we exploit their geometrical property to eliminate these constraints so that ${\mathfrak{T}}_{\mathrm{MINCO}}$ can be freely deformed.

To achieve this, we use the $\mathcal{V}$-representation of ${\mathcal{P}}^{\mathcal{H}}$ instead, where any $q\in {\mathcal{P}}^{\mathcal{H}}$ has a (general) barycentric coordinate, i.e., a convex combination of vertices. To obtain the vertices, we apply the efficient convex hull algorithm to the dual of ${\mathcal{P}}^{\mathcal{H}}$ based on an interior point calculated by Seidel's algorithm. Note that this procedure produces negligible overhead in our case ($n⩽4$).

The procedure to eliminate a polytope constraint is illustrated in the Fig. 5. We denote all $\hat{n}+1$ vertices of ${\mathcal{P}}^{\mathcal{H}}$ by $(v_0,\dots ,v_{\hat{n}})$, where $v_i\in {\mathbb{R}}^n$ for each $i$. The barycentric coordinate of a point $q\in {\mathcal{P}}^{\mathcal{H}}$ consists of the weights for these vertices. To obtain a more compact form, define ${\hat{v}}_i=v_i-v_0$ and $\hat{\mathbf{V}}=({\hat{v}}_1,\dots ,{\hat{v}}_{\hat{n}})$, then the position can be calculated as

$$\begin{array}{}\text{(82)}& q=v_0+\hat{\mathbf{V}}\mathit{w},\end{array}$$

where $\mathit{w}={(w_1,\dots ,w_{\hat{n}})}^{\mathrm{\top }}\in {\mathbb{R}}^{\hat{n}}$ is the last $\hat{n}$ entries in the barycentric coordinate. The set of coordinates in convex combinations can also be written as

$$\begin{array}{}\text{(83)}& {\mathcal{P}}_w^{\mathcal{H}}=\{\mathit{w}\in {\mathbb{R}}^{\hat{n}}|\mathit{w}⪰\mathbf{0},\parallel \mathit{w}{\parallel }_1⩽1\}.\end{array}$$

The Main Theorem of Polytope Theory confirms the equivalence between ${\mathcal{P}}_w^{\mathcal{H}}$ and ${\mathcal{P}}^{\mathcal{H}}$ under $(82)$. The polytope is exactly converted into a standard $(\hat{n}+1)$-simplex by simply adding auxiliary variables and applying a linear map to $q$.

This process does not produce additional nonlinearity in the optimization problem except that the dimension of decision variables is increased. Therefore, we only consider the decision variables on $q$ as the corresponding $w$ hereafter.

The simplex $(83)$ can be eliminated by nonlinear transformations. We first use an entry-wise square map $[\cdot {]}^2:{\mathbb{R}}^{\hat{n}}\mapsto {\mathbb{R}}^{\hat{n}}$ to eliminate nonnegativity constraints using $w=[x{]}^2$. Then, the constraint ${\mathcal{P}}_w^{\mathcal{H}}$ on $w$ is transformed into a closed unit ball ${\mathcal{B}}^{\hat{n}}$ on $x$,

$$\begin{array}{}\text{(84)}& {\mathcal{B}}^{\hat{n}}=\{\mathit{x}\in {\mathbb{R}}^{\hat{n}}|\parallel \mathit{x}{\parallel }_2⩽1\}.\end{array}$$

Consequently, we can utilize the smooth surjection $f_{\mathcal{B}}$ in $(78)$ again. The composition of $(82)$, $[\cdot {]}^2$, and $f_{\mathcal{B}}$ yields a smooth surjection $f_{\mathcal{H}}$ from ${\mathbb{R}}^{\hat{n}}$ onto ${\mathcal{P}}^{\mathcal{H}}$:

$$\begin{array}{}\text{(85)}& f_{\mathcal{H}}(\mathit{x})=v_0+\frac{4\hat{\mathbf{V}}[\mathit{x}{]}^2}{({\mathit{x}}^{\mathrm{\top }}\mathit{x}+1{)}^2}\in {\mathcal{P}}^{\mathcal{H}},\mathrm{\forall }\mathit{x}\in {\mathbb{R}}^{\hat{n}}.\end{array}$$

A new coordinate $\mathit{\xi }$ is introduced by $f_{\mathcal{H}}$, where any $\mathit{\xi }\in {\mathbb{R}}^{\hat{n}}$ ensures $q\in {\mathcal{P}}^{\mathcal{H}}$. The boundary of ${\mathcal{P}}^{\mathcal{H}}$ is also attainable. Similarly, $\mathit{\xi }$ is the new coordinate for $\mathbf{q}$. Optimizing $\mathit{\xi }$ requires gradient propagation. Denote by $g_i$ the $i$-th gradient $\partial J/\partial q_i$ in $\partial J/\partial \mathbf{q}$, then differentiating the layer $f_{\mathcal{H}}$ gives

$$\begin{array}{}\text{(86)}& \frac{\partial J}{\partial {\xi }_i}=\frac{8{\xi }_i\circ {\hat{\mathbf{V}}}^{\mathrm{\top }}{g}_i}{({\xi }_i^{\mathrm{\top }}{\xi }_i+1{)}^2}-\frac{16g_i^{\mathrm{\top }}\hat{\mathbf{V}}[{\xi }_i{]}^2}{({\xi }_i^{\mathrm{\top }}{\xi }_i+1{)}^3}{\xi }_i.\end{array}$$

If an initial guess $\mathbf{q}$ is specified, the corresponding $\mathit{\xi }$ can be computed via the local inverse of $f_{\mathcal{H}}$. The barycentric coordinate of each $q_i$ can be obtained using the analytic approach by Warren et al.. After that the analytic local inverses of $[\cdot {]}^2$ and $f_{\mathcal{B}}(\cdot )$ give us the desired ${\xi }_i$. Another flexible way is to directly minimize the squared distance between $f_{\mathcal{H}}(\xi )$ and the given $q_i$. Both approaches have negligible time consumption but promising results.

The map $[\cdot {]}^2$ in $f_{\mathcal{H}}$ presents additional nonlinearity into optimization. Fortunately, variable transformation via $[\cdot {]}^2$ is a special case of the inequality-to-equality conversion. Concretely, the inequality constraints are $-\mathit{w}⪯\mathbf{0}$. By introducing additional variables $x$, the equivalent equality constraints are $-w+[x{]}^2=\mathbf{0}$, yielding $w=[x{]}^2$. Such type of constraint conversion is proved to preserve first/second-order necessary conditions and second-order sufficient conditions for ICPs by Bertsekas as provided in literature. We confirm that the additional nonlinearity in $f_{\mathcal{H}}$ does not exclude the desired minimum or produce undesired minimum practically.

Direct constraints on $\mathbf{q}$ are eliminated for either ${\mathcal{P}}^{\mathcal{B}}$ or ${\mathcal{P}}^{\mathcal{H}}$ using a smooth surjection $\mathbf{q}(\mathit{\xi })$. We can conduct unconstrained optimization on $\mathit{\xi }$ to minimize $J(\mathbf{q}(\mathit{\xi }),\mathbf{T}(\mathit{\tau }))$ hereafter.

3.4 Time Integral Penalty Functional

After eliminating direct constraints, ${\mathfrak{T}}_{\mathrm{MINCO}}$ can be freely deformed to meet the continuous-time constraints $\mathcal{G}$. However, enforcing $\mathcal{G}$ over the entire trajectory involves infinitely many inequalities that cannot be solved by constrained optimization. It further needs temporal discretization that usually produces a large number of decision variables. To preserve the sparsity of trajectory parameterization, we decouple the resolution of constraint evaluation from the number of decision variables. Inspired by the constraint transcription, we transform $\mathcal{G}$ into finite constraints by integral of constraint violations.

For a trajectory $\mathit{p}:[0,T]\mapsto {\mathbb{R}}^m$, we define

$$\begin{array}{}\text{(87)}& I_{\mathcal{G}}^k\left[\mathit{p}\right]={\int }_0^{T}max{[\mathcal{G}(\mathit{p}(t),\dots ,{\mathit{p}}^{(s)}(t)),\mathbf{0}]}^k\mathrm{d}t,\end{array}$$

where $k\in {\mathbb{R}}_{>0}$ and $max{[\cdot ,\mathbf{0}]}^k$ is the composition of the entry-wise maximum and an entry-wise power function. Specifically, smoothing is needed if $k\le 1$. The functional-type constraint is then equivalent to equality constraints $I_{\mathcal{G}}^k\left[p\right]=\mathbf{0}$. Actually, $I_{\mathcal{G}}^k\left[p\right]$ is a function of trajectory parameters, which we adopt as penalty terms. If $k=1$, it forms a nonsmooth but exact penalty. If $k>1$, it forms a differentiable strictly convex penalty. Thus either $I_{\mathcal{G}}^3\left[p\right]$ or a smoothing approximation of $I_{\mathcal{G}}^1\left[p\right]$ can be adopted. For simplicity, we utilize $I_{\mathcal{G}}^3\left[p\right]$ hereafter unless otherwise specified. There are two reasons for choosing a penalty function method. Firstly, the integral in $(87)$ can only be evaluated numerically, making the constraint approximation inevitable.

Secondly, penalty methods have no requirement on a feasible initial guess which is nontrivial to construct.

We define the time integral penalty functional for $\mathit{p}(t)$ as

$$\begin{array}{}\text{(88)}& I_{\mathcal{G}}\left[\mathit{p}\right]={\chi }^{\mathrm{\top }}{I}_{\mathcal{G}}^k\left[\mathit{p}\right].\end{array}$$

where $\chi \in {\mathbb{R}}_{⩾0}^{n_g}$ is a weight vector. Normally, $\chi$ should contain large constants. If no constraint is violated, $I_{\mathcal{G}}\left[\mathit{p}\right]$ remains zero. Otherwise, if any part on $\mathit{p}(t)$ violates any constraint in $\mathcal{G}$, the penalty functional $I_{\mathcal{G}}\left[\mathit{p}\right]$ grows rapidly. By incorporating $I_{\mathcal{G}}\left[\mathit{p}\right]$ into the cost functional, continuous-time constraints are enforced within an acceptable tolerance.

Practically, $I_{\mathcal{G}}\left[\mathit{p}\right]$ can only be evaluated by quadrature. To conduct the quadrature, we first define a sampled function ${\mathcal{G}}_{\tau }:{\mathbb{R}}^{2s\times m}\times {\mathbb{R}}_{>0}\times [0,1]\mapsto {\mathbb{R}}^{n_g}$ as

$$\begin{array}{}\text{(89)}& {\mathcal{G}}_{\tau }({\mathbf{c}}_i,T_i,\tau )=\mathcal{G}({\mathbf{c}}_i^{\mathrm{\top }}\mathit{\beta }(T_i\cdot \tau ),\dots ,{\mathbf{c}}_i^{\mathrm{\top }}{\mathit{\beta }}^{(s)}(T_i\cdot \tau )),\end{array}$$

where $\tau \in [0,1]$ is a normalized stamp. Then the quadrature for $I_{\mathcal{G}}\left[p\right]$, denoted by $I:{\mathbb{R}}^{2Ms\times m}\times {\mathbb{R}}_{>0}^M\mapsto {\mathbb{R}}_{>0}$, is computed as a weighted sum of the sampled penalty,

$$\begin{array}{}\text{(90)}& I(\mathbf{c},\mathbf{T})=\sum _{i=1}^M\frac{T_i}{{\kappa }_i}\sum _{j=0}^{{\kappa }_i}{\overline{\omega }}_j{\chi }^{\mathrm{\top }}max{[{\mathcal{G}}_{\tau }({\mathbf{c}}_i,T_i,\frac{j}{{\kappa }_i}),\mathbf{0}]}^k,\end{array}$$

where ${\kappa }_i$ controls the resolution. We choose the trapezoidal rule $({\overline{\omega }}_0,{\overline{\omega }}_1,\dots ,{\overline{\omega }}_{{\kappa }_i-1},{\overline{\omega }}_{{\kappa }_i})=(1/2,1,\dots ,1,1/2)$ because of its reliable performance for ill-shaped $C^2$ integrands in our practice. Intuitively, $I(\mathbf{c},\mathbf{T})$ is a differentiable approximation to $I_{\mathcal{G}}[p]$, whose precision is adjustable through ${\kappa }_i$. The value and gradient at most timestamps can be parallelly computed then directly combined as one.

3.5 Trajectory Optimization via Unconstrained NLP

Due to $\mathcal{G}$ and $\mathcal{F}$ in $(11)$, the optimal trajectory parameterization is generally hard to know. Unlike traditional methods approximating solutions via a large number of variables, we propose to solve a lightweight relaxed optimization via unconstrained NLP, where the spatial-temporal deformation of ${\mathfrak{T}}_{\mathrm{MINCO}}$ is applied. The relaxation to $(11)$ is defined as

$$\begin{array}{}\text{(91)}& \underset{\mathit{\xi },\mathit{\tau }}{min}J(\mathbf{q}(\mathit{\xi }),\mathbf{T}(\mathit{\tau }))+I(\mathbf{c}(\mathbf{q}(\mathit{\xi }),\mathbf{T}(\mathit{\tau })),\mathbf{T}(\mathit{\tau })),\end{array}$$

where $J$ is the time-regularized control effort $(69)$ for ${\mathfrak{T}}_{\mathrm{MINCO}}$ and $I$ is the quadrature of penalty functional $(90)$. Note that any task-specific requirement, either objectives or constraints, can be combined in $(91)$ without affecting its structure.

To generate trajectories for a flat multicopter, we first parameterize its flat-output trajectory as ${\mathfrak{T}}_{\mathrm{MINCO}}$. After assigning a fixed number of pieces into each ${\mathcal{P}}_i$, variable transformations are applied to eliminate all direct constraints.

User-defined ${\mathcal{G}}_{\mathcal{D}}$ are also transformed into $\mathcal{G}$ via ${\mathrm{\Psi }}_x$ and ${\mathrm{\Psi }}_u$. Finally, we obtain the cost function $(91)$.

Apparently, the gradient propagation is derived for all layers except ${\mathrm{\Psi }}_x$ and ${\mathrm{\Psi }}_u$. One can either apply Automatic Differentiation (AD) to ${\mathrm{\Psi }}_x$ and ${\mathrm{\Psi }}_u$ or derive the gradient propagation analytically by following the reverse-mode AD. The efficiency is the same as the flatness map as ensured by Baur-Strassen Theorem. The differentiation is only needed for the given flat dynamics once and for all. With available gradient, the relaxation $(91)$ is then solved by the L-BFGS algorithm.

4 Conclusion (By GPT-5.2 in Github Copilot)

MINCO can be viewed as a back-end trajectory generator + lightweight unconstrained optimizer: given a piece assignment and boundary/intermediate conditions, it constructs the unique minimum-control-effort spline (solving a multi-stage LQMT like $(18)$), and then deforms its spatial and temporal parameters to satisfy geometry and continuous-time constraints (relaxing the original problem $(11)$ into an unconstrained NLP $(91)$).

Below is a practical procedure you can follow to implement the MINCO pipeline.

MINCO procedure (implementation-oriented)

Inputs

• Flatness order $s$ (e.g., jerk $s=3$ or snap $s=4$), weights $\mathbf{W}$, and time regularization $\rho (\cdot )$ in $(11)$, $(12)$, $(13)$.
• Start/goal boundary derivatives ${\overline{\mathit{z}}}_o,{\overline{\mathit{z}}}_f$ (see $(11e)$).
• Free-space approximation $\tilde{\mathcal{F}}=\bigcup _i{\mathcal{P}}_i$ (see $(14)$–$(17)$), plus piece assignment (e.g., $K$ pieces per primitive).
• Continuous-time dynamic constraints $\mathcal{G}$ (obtained from ${\mathcal{G}}_{\mathcal{D}}$ via flatness maps, see $(9)$–$(10)$).

Decision variables

• Spatial variables $\mathit{\xi }$ that generate intermediate points $\mathbf{q}(\mathit{\xi })$ using a smooth surjection (e.g., ball case $(78)$, polytope case via $f_{\mathcal{H}}$).
• Time variables $\mathit{\tau }$ that generate $\mathbf{T}(\mathit{\tau })$ via a diffeomorphism (e.g., fixed-sum case $(71)$; gradient uses $(72)$).

Algorithm

1. Discretize the topology (piece assignment)

   • Choose $M$ polynomial pieces and assign each piece to a convex primitive ${\mathcal{P}}_i$ (sequential connection as in $(15)$).
   • Define intermediate points $\{q_i\}$ that must lie in overlaps of adjacent primitives (handled later by the surjection $\mathbf{q}(\mathit{\xi })$).

2. Parameterize the flat-output trajectory as a MINCO spline

   • Represent each piece with polynomial coefficients ${\mathbf{c}}_i$ and duration $T_i$.
   • Use the MINCO construction to obtain coefficients from spatial-temporal parameters: $\mathbf{c}=\mathbf{c}(\mathbf{q},\mathbf{T})$ (banded linear system such as $(57)$).
   • Compute the smoothness+time cost$$J(\mathbf{q},\mathbf{T})=J_q(\mathbf{q},\mathbf{T})+\rho (\parallel \mathbf{T}{\parallel }_1)(69)\text{.}$$

3. Eliminate direct constraints by variable transformations

   • Time positivity / fixed total time: optimize over $\mathit{\tau }$ and map to $\mathbf{T}(\mathit{\tau })$ (e.g., $(71)$).
   • Geometry of intermediate points: optimize over $\mathit{\xi }$ and map to $\mathbf{q}(\mathit{\xi })$ (e.g., $(78)$ for balls; analogous smooth surjection for polytopes).
   • After this step, constraints like $\mathbf{T}\succ \mathbf{0}$ and $q_i\in \mathcal{P}$ are satisfied by construction.

4. Handle continuous-time constraints via integral penalty

   • Convert the infinite constraints $\mathcal{G}(\mathit{z}(t),\dots ,{\mathit{z}}^{(s)}(t))⪯\mathbf{0}$ into a penalty functional $(87)$–$(88)$.
   • Numerically evaluate it by quadrature, yielding $I(\mathbf{c},\mathbf{T})$ in $(90)$.

5. Solve the unconstrained NLP

   • Minimize the relaxed objective$$\underset{\mathit{\xi },\mathit{\tau }}{min}J(\mathbf{q}(\mathit{\xi }),\mathbf{T}(\mathit{\tau }))+I(\mathbf{c}(\mathbf{q}(\mathit{\xi }),\mathbf{T}(\mathit{\tau })),\mathbf{T}(\mathit{\tau }))(91)\text{.}$$
   • Use L-BFGS (as described at the end of §3.5). For gradients, propagate through layers:
     • MINCO layer $\mathbf{c}(\mathbf{q},\mathbf{T})$: linear-complexity gradients w.r.t. $\mathbf{q}$ and $\mathbf{T}$ (see $(64)$, $(68)$).
     • Time map $\mathbf{T}(\mathit{\tau })$: use $(72)$ (or the exponential map case).
     • Spatial map $\mathbf{q}(\mathit{\xi })$: use the analytic Jacobians (e.g., $(86)$).
     • Flatness maps ${\mathrm{\Psi }}_x,{\mathrm{\Psi }}_u$: use AD or derive reverse-mode gradients once (see §3.5).

6. Recover the feasible multicopter trajectory

   • Output the optimized flat trajectory $\mathit{z}(t)$, then map to state/input using $(2)$–$(3)$.
   • Validate $\mathit{z}(t)\in \mathcal{F}$ and $\mathcal{G}⪯\mathbf{0}$ using the same sampling resolution ${\kappa }_i$ as in $(90)$.

This procedure is exactly the “MINCO-as-a-module” viewpoint: (i) construct the unique minimum-effort spline from $\mathbf{q},\mathbf{T}$, and (ii) optimize $\mathbf{q},\mathbf{T}$ (via $\mathit{\xi },\mathit{\tau }$) to satisfy geometry and dynamics.