Geometrically Constrained Trajectory Optimization for Multicopters 论文分析

这篇论文题目为"针对多旋翼飞行器的几何约束轨迹优化方法"(Geometrically Constrained Trajectory Optimization for Multicopters),主要讲了一种高效求解多旋翼无人机在复杂环境下轨迹规划问题的优化框架。

1. 引言

多旋翼无人机要实现自主导航,需要能够在复杂环境中快速规划出安全且动力学可行的轨迹。但现有的轨迹规划方法在求解质量、计算效率和约束满足度等方面难以兼顾。本文提出了一种轻量级且灵活的轨迹优化框架,可有效平衡上述需求。

该框架的理论基础是针对无约束最优控制问题提出了必要充分条件,可直接构造出最优解的解析形式。基于此,设计了一种新的轨迹表示方法MINCO(Minimum Control),可灵活控制轨迹形状和时间分配。利用光滑映射可方便地消除各种几何约束。提出了一种分离约束评估和轨迹参数化的方法来支持各种状态输入约束。最终,原问题可转化为一个无约束优化问题高效求解。

2.预备知识

2.1 微分平坦性

多旋翼动力学通常可写为仿射非线性系统的形式:
$$\dot{x}=f(x)+g(x)u$$
其中$x\in {\mathbb{R}}^n$为状态,$u\in {\mathbb{R}}^m$为输入。如果存在一个平坦输出$z\in {\mathbb{R}}^m$,使得状态和输入都可由其有限阶导数表示:
$$\begin{array}{rl}x& ={\Psi }_x(z,\dot{z},\dots ,z^{(s-1)})\\ u& ={\Psi }_u(z,\dot{z},\dots ,z^{(s)})\end{array}$$
则称该系统是微分平坦的。平坦映射${\Psi }_x,{\Psi }_u$消除了系统的微分约束。例如四旋翼的平坦输出通常选取为:
$$z=(p_x,p_y,p_z,\psi {)}^T$$
其中$(p_x,p_y,p_z)$为质心平移,$ \psi$为偏航角。利用$\Psi_x,\Psi_u$可将对$z$的约束转化为对状态输入的等价约束。

2.2 平坦输出空间的直接优化

基于上述性质,可以直接在平坦输出空间对$z(t):[0,T]\mapsto {\mathbb{R}}^m$进行优化,再通过${\Psi }_x,{\Psi }_u$映射回状态输入空间。目标函数选取为正则化的二次型控制量:
$${\int }_0^{T}v(t{)}^{T}Wv(t)dt+\rho (T)$$

其中 $v(t)=z^{(s)}(t)$,$W\succ 0$。几何约束为 $z(t)\in \mathcal{F},\forall t\in [0,T]$,表示轨迹要始终在一个无碰撞区域 $\mathcal{F}$ 内。状态输入约束则表示为

$$G_D(x(t),u(t))⪯0,\forall t\in [0,T]$$
它可以通过${\Psi }_x,{\Psi }_u$等价转化为对$z$的约束:
$$G(z(t),\dot{z}(t),\dots ,z^{(s)}(t))⪯0,\forall t\in [0,T]$$
最终,可得到如下平坦空间的轨迹优化问题:
$$\begin{array}{rl}\underset{z(t),T}{\min }& {\int }_0^{T}v(t{)}^{T}Wv(t)dt+\rho (T)\\ \text{s.t.}& \begin{array}{rl}{z}^{(s)}(t)& =v(t),\forall t\in [0,T]\\ G(z(t),\dots ,z^{(s)}(t))& ⪯0,\forall t\in [0,T]\\ z(t)& \in \mathcal{F},\forall t\in [0,T]\\ z^{[s-1]}(0)& ={\bar{z}}_o,z^{[s-1]}(T)={\bar{z}}_f\end{array}\end{array}$$
其中${\bar{z}}_o,{\bar{z}}_f$为边界条件。无碰撞区域$\mathcal{F}$用凸集的并$\mathcal{F}\approx \tilde{\mathcal{F}}={\cup }_{i=1}^{M_P}{\mathcal{P}}_i$近似表示,其中${\mathcal{P}}_i$可以是球形或多面体区域。

3. 多阶段控制量最小化

本节分析在没有函数不等式约束时的多阶段控制量最小化问题,提出了一般情况下的最优性必要充分条件,利用该条件可直接构造出全局最优解的解析形式。基于此设计了MINCO(Minimum Control)轨迹类及其空间-时间变形方法,以满足不同任务的需求。

3.1 无约束控制量最小化

为得到一种灵活表征轨迹的方式,考虑求解如下M阶段无约束最优控制问题:
$$\begin{array}{rl}\underset{z(t)}{\min }& {\int }_{t_0}^{t_M}v(t{)}^{T}Wv(t)dt\\ \text{s.t.}& \begin{array}{rl}{z}^{(s)}(t)& =v(t),\forall t\in [t_0,t_M]\\ z^{[s-1]}(t_0)={\bar{z}}_o,z^{[s-1]}& (t_M)={\bar{z}}_f\\ z^{[d_i-1]}(t_i)& ={\bar{z}}_i,1\le i<M\\ t_{i-1}& <t_i,1\le i\le M\end{array}\end{array}$$
其中$t_0,\dots ,t_M$为 fixed 时间戳,把整个时域$[t_0,t_M]$分成 M 段。 ${\bar{z}}_o,{\bar{z}}_f\in {\mathbb{R}}^{ms}$为边界条件, ${\bar{z}}_i\in {\mathbb{R}}^{m{d}_i}(d_i\le s)$为中间点$t_i$ 处指定的$d_i$阶导数值。

现有方法大多只研究了特殊情形下的必要条件。本文给出了一般情况下该问题的最优性必要充分条件:

定理2(最优性条件): 如果$z^{\ast }(t):[t_0,t_M]\mapsto {\mathbb{R}}^m$满足以下条件,则它是问题的全局最优解,反之亦然:

• 映射$z^{\ast }(t):[t_{i-1},t_i]\mapsto {\mathbb{R}}^m$ 可参数化为2s-1次多项式,$\forall 1\le i\le M$;
• 满足边界条件$z^{[s-1]}(t_0)={\bar{z}}_o$和$z^{[s-1]}(t_M)={\bar{z}}_f$;
• 满足中间时刻$t_i$的条件$z^{[d_i-1]}(t_i)={\bar{z}}_i$;
• $z^{\ast }(t)$在$t_i$点处${\bar{d}}_i-1$阶连续可微,其中${\bar{d}}_i=2s-d_i,\forall 1\le i<M$。

此外,满足上述条件的轨迹是唯一的。

3.2 无需显式目标函数的最小化

定理2提供了直接构造问题最优解的方法,计算复杂度是关于M的线性阶。不需要显式或隐式地计算目标函数及其梯度。

考虑一条第i段轨迹表示为2s-1次多项式:
$$p_i(t)=c_i^T\beta (t-t_{i-1}),t\in [t_{i-1},t_i]$$
其中$\beta (x)=(1,x,\dots ,x^N{)}^T$为基函数,$c_i\in {\mathbb{R}}^{2s\times m}$为系数矩阵。整条轨迹可表示为系数矩阵$c\in {\mathbb{R}}^{2Ms\times m}$和时间向量$\mathbf{T}\in {\mathbb{R}}_{>0}^M$的组合:
$$c=\left(\begin{array}{c}{c}_1^T\\ ⋮\\ c_M^T\end{array}\right),\mathbf{T}=(T_1,\dots ,T_M{)}^T$$
其中$T_i$表示第i段的持续时间,$t_i={\sum }_{j=1}^i{T}_j$为时间戳, $T=\Vert \mathbf{T}{\Vert }_1$为总时长。

为计算式(3)的解析解,我们直接对系数矩阵$c$施加最优性条件。记$D_0\in {\mathbb{R}}^{s\times m},D_M\in {\mathbb{R}}^{s\times m},D_i\in {\mathbb{R}}^{d_i\times m}$分别为边界和中间点处的导数值。在$t_i$点的条件可表示为:
$$(E_i{F}_i)\left(\begin{array}{c}{c}_i\\ c_{i+1}\end{array}\right)=\left(\begin{array}{c}{D}_i\\ 0_{{\bar{d}}_i\times m}\end{array}\right)$$
其中
$$\begin{array}{rl}{E}_i& =(\beta (T_i),\dots ,{\beta }^{(d_i-1)}(T_i),\beta (T_i),\dots ,{\beta }^{({\bar{d}}_i-1)}(T_i){)}^T\\ F_i& =(0,-\beta (0),\dots ,-{\beta }^{({\bar{d}}_i-1)}(0){)}^T\end{array}$$
此处${\beta }^{(k)}$表示$\beta$的k阶导数。边界条件可类似表示:
$$\begin{array}{rl}{F}_0& =(\beta (0),\dots ,{\beta }^{(s-1)}(0){)}^T\\ E_M& =(\beta (T_M),\dots ,{\beta }^{(s-1)}(T_M){)}^T\end{array}$$

将所有片段的条件组合可得关于最优系数$c$的线性方程组$\mathbf{M}c=b$,其中
$$\mathbf{M}=\left(\begin{array}{ccccc}{F}_0& & & & \\ E_1& F_1& & & \\ & E_2& F_2& & \\ & & \ddots & \ddots & \\ & & & E_{M-1}& F_{M-1}\\ & & & & E_M\end{array}\right)$$
$$b=\left(\begin{array}{c}{D}_0^T\\ D_1^T\\ 0_{m\times {\bar{d}}_1}\\ ⋮\\ D_{M-1}^T\\ 0_{m\times {\bar{d}}_{M-1}}\\ D_M^T\end{array}\right)$$

定理2保证了对于任意$\mathbf{T}\succ 0$, 系数矩阵$\mathbf{M}$是非奇异的。因此,唯一解$c$可通过求解带状线性方程组$\mathbf{M}c=b$得到,计算复杂度为$\mathcal{O}(M)$。

这种无需显式目标函数,直接施加最优性条件构造解的方法,具有简单、高效、稳定、易扩展等优点。基于它设计的MINCO轨迹类可作为优化框架的可靠子模块。

3.3 带空间-时间变形的MINCO轨迹

为灵活满足各种任务需求,我们选择中间点$q=(q_1,\dots ,q_{M-1})$和时间向量$\mathbf{T}$作为式(3)的参数。对于任意$q,\mathbf{T}$,定理2可唯一确定出一条无约束最优解,我们将其定义为MINCO(Minimum Control)轨迹类:

T MINCO = { p ( t ) : [ 0 , T ] ↦ R m ∣ c = c ( q , T )  由定理2确定 , ∀ q ∈ R m × ( M − 1 ) , T ∈ R > 0 M } \mathcal{T}_{\text{MINCO}}=\{p(t):[0,T]\mapsto\mathbb{R}^m| c=c(q,\mathbf{T}) \text{ 由定理2确定}, \forall q\in\mathbb{R}^{m\times(M-1)}, \mathbf{T}\in\mathbb{R}_{>0}^M\} TMINCO​={p(t):[0,T]↦Rm∣c=c(q,T) 由定理2确定,∀q∈Rm×(M−1),T∈R>0M​}

MINCO类的任意元素都可由$q$和$\mathbf{T}$紧致参数化。设计如下的迭代过程实现轨迹的空间-时间变形:

对任意的目标函数$K(c,\mathbf{T})$,在MINCO类上可表示为关于$q,\mathbf{T}$的函数:
$$
W(q,\mathbf{T})=K(c(q,\mathbf{T}),\mathbf{T})为优化该目标函数,需要计算梯度$\partial W/\partial q$和$\partial W/\partial \mathbf{T}$。下面给出了一种线性复杂度的传播方案。

将线性方程组(12)改写为:
$$\mathbf{M}(\mathbf{T})c(q,\mathbf{T})=b(q)$$

对$q_{i,j}$求导可得:
$$\frac{\partial c}{\partial q_{i,j}}={\mathbf{M}}^{-1}\frac{\partial b}{\partial q_{i,j}}$$

进而有:
$$\frac{\partial W}{\partial q_{i,j}}=\text{Tr}\left\{{\left(\frac{\partial c}{\partial q_{i,j}}\right)}^T\frac{\partial K}{\partial c}\right\}=\text{Tr}\left\{{\left(\frac{\partial b}{\partial q_{i,j}}\right)}^T\left({\mathbf{M}}^{-T}\frac{\partial K}{\partial c}\right)\right\}$$

由$b(q)$的定义(15)可知,$\partial b/\partial q_{i,j}$仅在$(2i-1)s+1$行和$j$列有一个非零元素1。将所有结果堆叠即得:
$$\frac{\partial W}{\partial q_i}={\left({\mathbf{M}}^{-T}\frac{\partial K}{\partial c}\right)}^T{e}_{(2i-1)s+1}$$

其中$e_j$为${\mathbf{I}}_{2Ms}$的第$j$列。定义矩阵$\mathbf{G}\in {\mathbb{R}}^{2Ms\times m}$为:
$${\mathbf{M}}^T\mathbf{G}=\frac{\partial K}{\partial c}$$

只需计算一次$\mathbf{G}$即可得到所有$1\le i<M$的$\partial W/\partial q_i$。由于已经对$\mathbf{M}$做了带状PLU分解,可直接利用分解结果避免求逆${\mathbf{M}}^T$。设$\mathbf{M}=\mathbf{PLU}$,其中$\mathbf{P}$为置换阵,$\mathbf{L}$为下三角带状阵,$\mathbf{U}$为上三角带状阵,则有:
$${\mathbf{M}}^T=\bar{\mathbf{L}}\bar{\mathbf{U}}{\mathbf{P}}^T$$
其中
$$\bar{\mathbf{L}}={\mathbf{U}}^T(\mathbf{U}\circ \mathbf{I}{)}^{-1},\bar{\mathbf{U}}=(\mathbf{I}\circ \mathbf{U}){\mathbf{L}}^T$$

$\circ$表示Hadamard积,即逐元素乘积。这样$\mathbf{G}$也能以线性复杂度求解。将$\mathbf{G}$分块为:
$$\mathbf{G}=\left(\begin{array}{c}{\mathbf{G}}_0^T\\ {\mathbf{G}}_1^T\\ ⋮\\ {\mathbf{G}}_{M-1}^T\\ {\mathbf{G}}_M^T\end{array}\right)$$
其中${\mathbf{G}}_0\in {\mathbb{R}}^{s\times m},{\mathbf{G}}_M\in {\mathbb{R}}^{s\times m},{\mathbf{G}}_i\in {\mathbb{R}}^{2s\times m},1\le i<M$。最终$\partial W/\partial q$可表示为:

$$\frac{\partial W}{\partial q}=\left(\begin{array}{c}{\mathbf{G}}_1^T{e}_1\\ ⋮\\ {\mathbf{G}}_{M-1}^T{e}_1\end{array}\right)$$
其中$e_1$为${\mathbf{I}}_{2s}$的第一列。该操作相当于取出${\mathbf{G}}^T$中$M-1$个特定的列。

类似地,对${\mathbf{T}}_i$求导可得:
$$\frac{\partial \mathbf{M}}{\partial {\mathbf{T}}_i}c+\mathbf{M}\frac{\partial c}{\partial {\mathbf{T}}_i}=0$$
从而:
$$\frac{\partial W}{\partial {\mathbf{T}}_i}=\frac{\partial K}{\partial {\mathbf{T}}_i}+\text{Tr}\left\{{\left(\frac{\partial c}{\partial {\mathbf{T}}_i}\right)}^T\frac{\partial K}{\partial c}\right\}=\frac{\partial K}{\partial {\mathbf{T}}_i}-\text{Tr}\left\{{\left(\frac{\partial \mathbf{M}}{\partial {\mathbf{T}}_i}c\right)}^T{\mathbf{M}}^{-T}\frac{\partial K}{\partial c}\right\}=\frac{\partial K}{\partial {\mathbf{T}}_i}-\text{Tr}\left\{{\mathbf{G}}^T\frac{\partial \mathbf{M}}{\partial {\mathbf{T}}_i}c\right\}$$

根据$\mathbf{M}$的带状结构,有:
$${\mathbf{G}}^T\frac{\partial \mathbf{M}}{\partial {\mathbf{T}}_i}c={\mathbf{G}}_i^T\frac{\partial E_i}{\partial {\mathbf{T}}_i}{c}_i$$

因此$\partial W/\partial {\mathbf{T}}_i$可表示为:
$$\frac{\partial W}{\partial {\mathbf{T}}_i}=\frac{\partial K}{\partial {\mathbf{T}}_i}-\text{Tr}\left\{{\mathbf{G}}_i^T\frac{\partial E_i}{\partial {\mathbf{T}}_i}{c}_i\right\}$$

式中$\partial E_i/\partial {\mathbf{T}}_i$可由(13)解析求导得到。对每个$1\le i\le M$计算(34)式即得$\partial W/\partial \mathbf{T}$。

至此,我们完成了$\partial W/\partial q$和$\partial W/\partial \mathbf{T}$的计算,其复杂度均为$\mathcal{O}(M)$。将该方法与常见优化器结合,即可高效实现${\mathcal{T}}_{\text{MINCO}}$的空间-时间变形,同时保持局部光滑性。

这一节主要研究了一类无约束的多阶段控制量最小化问题, 总结下：

首先,考虑如下的多阶段最优控制问题:
$$\begin{array}{rl}\underset{z(t)}{\min }& {\int }_{t_0}^{t_M}v(t{)}^{T}Wv(t)dt\\ \text{s.t.}& \begin{array}{rl}{z}^{(s)}(t)& =v(t),\forall t\in [t_0,t_M]\\ z^{[s-1]}(t_0)={\bar{z}}_o,z^{[s-1]}& (t_M)={\bar{z}}_f\\ z^{[d_i-1]}(t_i)& ={\bar{z}}_i,1\le i<M\\ t_{i-1}& <t_i,1\le i\le M\end{array}\end{array}$$

这里$t_0,\dots ,t_M$为固定的时间戳,将整个时域$[t_0,t_M]$分成$M$段。${\bar{z}}_o,{\bar{z}}_f\in {\mathbb{R}}^{ms}$为边界条件,${\bar{z}}_i\in {\mathbb{R}}^{m{d}_i}(d_i\le s)$为中间时刻$t_i$处的$d_i$阶导数值。目标是找到一条$s$阶导数为$v(t)$的轨迹$z(t)$使得控制量$\int v(t{)}^{T}Wv(t)dt$最小。

本文给出了上述问题最优解应满足的必要充分条件,称为 定理2 :

若映射$z^{\ast }(t):[t_0,t_M]\mapsto {\mathbb{R}}^m$满足:

1. 在每段$[t_{i-1},t_i]$上可参数化为$2s-1$次多项式;
2. 满足边界条件$z^{[s-1]}(t_0)={\bar{z}}_o$和$z^{[s-1]}(t_M)={\bar{z}}_f$;
3. 满足中间点条件$z^{[d_i-1]}(t_i)={\bar{z}}_i$;
4. 在$t_i$点处$2s-d_i-1$阶连续可微;

则$z^{\ast }(t)$为问题的唯一最优解,反之亦然。

该定理来源于最优控制的Pontryagin极大值原理,证明涉及将原问题转化为等价的Mayer型问题并分析其一阶最优性条件。虽然细节较多,但结论非常简洁:最优轨迹可用一系列$2s-1$次多项式精确表示,多项式系数由定理2的条件唯一确定。

基于定理2,文中提出了一种称为MINCO(Minimum Control)的轨迹表示方法:
$$\begin{array}{rl}{p}_i(t)& =c_i^T\beta (t-t_{i-1}),t\in [t_{i-1},t_i]\\ \beta (t)& =(1,t,\dots ,t^{2s-1}{)}^T\end{array}$$

这里$c_i\in {\mathbb{R}}^{2s\times m}$为第$i$段轨迹的多项式系数矩阵。将MINCO的分段表达式和定理2的条件结合,可得关于系数$c=(c_1^T,\dots ,c_M^T{)}^T$的线性方程组:
$$\mathbf{M}c=b$$

这里的$\mathbf{M}$是由多项式基函数在$t_i$处的值构成的分块矩阵,反映了轨迹连续性条件;$b$则由边界条件和中间点条件构成。定理2保证了$\mathbf{M}$可逆,因此$c={\mathbf{M}}^{-1}b$可唯一确定MINCO类的最优轨迹。

该结论的重要意义在于,它给出了无约束控制量最小化问题的解析解,且求解复杂度为$O(M)$。这为后续研究轨迹的参数敏感性和变形奠定了基础。文中进一步分析了MINCO关于中间点$q_i$和时间分配$T_i$的梯度,给出了线性时间复杂度的梯度传播方案,使得MINCO能以很低的计算代价适应不同的任务需求。

总的来说,定理2是多阶段控制量最小化的理论基础,而MINCO是其高效的数值求解方法。本节在理论和应用两个层面上系统地分析了这类问题,这些结论为后文构建更一般的轨迹优化框架提供了支撑。

4. 几何约束下的飞行轨迹优化

本节提出了一个统一的优化框架,可处理带不同时间正则项$\rho (T)$、空间约束$\tilde{\mathcal{F}}$和连续时间约束$G$的飞行轨迹优化问题。该框架本质上将原问题松弛到MINCO类,并利用空间-时间变形来满足各种可行性要求。
设计了轻量级方案消除几何约束,使轨迹能自由变形。对连续时间约束,提出了时间积分罚函数以保证可行性且不牺牲可扩展性。最终,带约束的轨迹优化被转化为一个稀疏无约束优化,可高效可靠地求解。

4.1 时间约束消除

对于固定总时间$T_{\Sigma }$的$M$片轨迹,其时间向量$\mathbf{T}$的可行域是${\mathbb{R}}_{>0}^M$中的一个$(M-1)$-单纯形的相对内部,如图3所示。常见的优化算法大多针对欧式空间设计,在流形上需要频繁收缩,计算效率低。为此,我们给出了$\mathbf{T}$的显式微分同胚,将其映射到欧式空间,从而方便使用高效的无约束优化器。

对多项式样条而言,式(11)中的控制量是$c$和$\mathbf{T}$的函数,记为$J_c(c,\mathbf{T})$。文献[28]给出了$J_c$及其梯度的解析表达式。由于MINCO的系数由$c(q,\mathbf{T})$决定,式(11)的目标函数可改写为:
$$J(q,\mathbf{T})=J_q(q,\mathbf{T})+\rho (\Vert \mathbf{T}{\Vert }_1)$$
其中$J_q(q,\mathbf{T})=J_c(c(q,\mathbf{T}),\mathbf{T})$。显然,由$J_c$及其梯度计算$J_q$及其梯度的复杂度为$\mathcal{O}(M)$。

直接用$\partial J/\partial \mathbf{T}$来优化$\mathbf{T}$会遇到问题,因为$J_q(q,\mathbf{T})$的定义域为$\mathbf{T}\in {\mathbb{R}}_{>0}^M$。当任意$T_i\to 0$且$q$中没有连续重复点时,$J_q$无界。此外,固定总时间正则项${\rho }_f$进一步限制了$J$的定义域为${\sum }_{i=1}^{M-1}{T}_i<T_{\Sigma }$。

为消除这些隐式约束,考虑${\rho }_f$对应的可行域:
T f = { T ∈ R M ∣ ∥ T ∥ 1 = T Σ , T ≻ 0 } \mathcal{T}_f=\{\mathbf{T}\in\mathbb{R}^M | \|\mathbf{T}\|_1=T_\Sigma,\mathbf{T}\succ0\} Tf​={T∈RM∣∥T∥1​=TΣ​,T≻0}

容易看出,当$q$非平凡时,$J(q,\cdot )$有界的充要条件是$\mathbf{T}\in \text{RelInt}({\mathcal{T}}_f)$,即${\mathcal{T}}_f$的相对内部。我们有如下结论:

命题1: ${\mathcal{T}}_f$与${\mathbb{R}}^{M-1}$微分同胚。设$\tau =({\tau }_1,\dots ,{\tau }_{M-1})\in {\mathbb{R}}^{M-1}$,一个$C^{\infty }$微分同胚由下式给出($1\le i<M$):
$$T_i=\frac{e^{{\tau }_i}}{1+{\sum }_{j=1}^{M-1}{e}^{{\tau }_j}}{T}_{\Sigma },T_M=T_{\Sigma }-\sum _{j=1}^{M-1}{T}_j$$

利用该微分同胚,可直接在${\mathbb{R}}^{M-1}$上关于$\tau$最小化$J$,因为可行域约束被自动满足了。优化$\tau$需要梯度传播。将$\partial J_q/\partial \mathbf{T}$分块为$(g_a^T,g_b{)}^T$,其中$g_a\in {\mathbb{R}}^{M-1},g_b\in \mathbb{R}$。对(37)式求导可得$J$关于$\tau$的梯度:
$$\frac{\partial J}{\partial \tau }=\frac{(g_a-g_{b}1)\circ e^{[\tau ]}}{1+\Vert e^{[\tau ]}{\Vert }_1}-\frac{(g_a^T{e}^{[\tau ]}-g_b\Vert e^{[\tau ]}{\Vert }_1)e^{[\tau ]}}{(1+\Vert e^{[\tau ]}{\Vert }_1{)}^2}$$
其中$e^{[\cdot ]}$表示逐元素指数映射,$1$为全1向量。若指定了初始猜测$\mathbf{T}$,对应的$\tau$可由微分同胚的逆映射计算:
$${\tau }_i=\ln (T_i/T_M),1\le i<M$$

对于线性时间正则项${\rho }_s$,只需确保$\mathbf{T}\succ 0$。此时微分同胚取$T=e^{[\tau ]}$即可。

我们统一将微分同胚记为$\mathbf{T}(\tau )$。直接在$\tau$上无约束优化$J(q,\mathbf{T}(\tau ))$即可。尽管$\mathbf{T}(\tau )$不保持凸性,但原目标函数$J(q,\mathbf{T})$本身就是非凸的。因此唯一的顾虑是$\mathbf{T}(\tau )$是否引入了额外的局部极小值。我们有如下结论:

命题2: 设$F:D_F\mapsto \mathbb{R}$为定义在凸开集$D_F\in {\mathbb{R}}^N$上的任意$C^2$函数。对任意$C^2$微分同胚$G:{\mathbb{R}}^N\mapsto D_F$,定义$H:{\mathbb{R}}^N\mapsto \mathbb{R}$为$H(y)=F(G(y)),\forall y\in {\mathbb{R}}^N$。对任意$x\in D_F,y\in {\mathbb{R}}^N$满足$x=G(y)$或等价地$y=G^{-1}(x)$,下列结论总成立:

• 当且仅当$\nabla H(y)=0$时有$\nabla F(x)=0$;
• 当$\nabla F(x)=0$时,${\nabla }^{2}F(x)$正定(或半正定)当且仅当${\nabla }^{2}H(y)$正定(或半正定)。

命题2确认了$\mathbf{T}(\tau )$保持了一阶/二阶必要最优性条件和二阶充分最优性条件。

4.2 球形区域的空间约束消除

为保证运动安全,需要将轨迹限制在可行域$\tilde{\mathcal{F}}$内。尽管$\tilde{\mathcal{F}}$非凸,但它由一系列局部连通的凸集组成。若每段轨迹都被分配到这些凸集中,则每段的安全约束就变为凸的,可方便地并入$G$中。借助MINCO的特性,每个凸集的飞行时间可直接优化。因此,我们在优化前固定轨迹分段,而不是引入整数变量。相应地,中间点应被限制在相邻凸集的交集内,形成不等式约束。对于带不等式约束的优化问题,常规方法通过引入额外变量逐步逼近约束。而我们希望直接高效地施加约束,为此提出了空间约束消除方法。

考虑约束$q\in \mathcal{P}\subset {\mathbb{R}}^n$,其中$\mathcal{P}$为闭球域。其维数满足$n\le m$,因为${\mathbb{R}}^m$中也存在低维约束。若$\mathcal{P}$为球形区域${\mathcal{P}}_B$:
P B = { x ∈ R n ∣ ∥ x − o ∥ 2 ≤ r } \mathcal{P}_B=\{x\in\mathbb{R}^n|\|x-o\|_2\leq r\} PB​={x∈Rn∣∥x−o∥2​≤r}
其中$o$为球心,$r$为半径。我们利用一个从${\mathbb{R}}^n$到${\mathcal{P}}_B$的光滑满射,使得在${\mathbb{R}}^n$上优化就隐式地满足了${\mathcal{P}}_B$约束。如图4所示,该映射由反球极投影(inverse stereographic projection)和正交投影(orthographic projection)复合而成。

首先利用反球极投影将${\mathbb{R}}^n$映射到${\mathbb{S}}^n$,其中${\mathbb{S}}^n$为${\mathbb{R}}^{n+1}$中的无北极的单位球面:
S n = { x ∈ R n + 1 ∣ ∥ x ∥ 2 = 1 , x n + 1 < 1 } \mathbb{S}^n=\{x\in\mathbb{R}^{n+1} | \|x\|_2=1,x_{n+1}<1\} Sn={x∈Rn+1∣∥x∥2​=1,xn+1​<1}
反球极投影$f_s$定义为:
$$f_s(x)=\frac{(2x^T,x^{T}x-1{)}^T}{x^{T}x+1}\in {\mathbb{S}}^n,\forall x\in {\mathbb{R}}^n$$
注意$f_s$是${\mathbb{R}}^n$到${\mathbb{S}}^n$的微分同胚。

然后将${\mathbb{S}}^n$从${\mathbb{R}}^{n+1}$正交投影回${\mathbb{R}}^n$得到${\mathbb{B}}^n$:
B n = { x ∈ R n ∣ ∥ x ∥ 2 ≤ 1 } \mathbb{B}^n=\{x\in\mathbb{R}^n|\|x\|_2\leq1\} Bn={x∈Rn∣∥x∥2​≤1}
该映射为:
$$f_o(x)=(x_1,\dots ,x_n{)}^T\in {\mathbb{B}}^n,\forall x\in {\mathbb{S}}^n$$
它是从${\mathbb{S}}^n$到${\mathbb{B}}^n$的光滑满射。${\mathbb{B}}^n$中除球心外的每个点,在${\mathbb{S}}^n$中都有两个对应点。将$f_s,f_o$及一个线性变换复合,得到从${\mathbb{R}}^n$到${\mathcal{P}}_B$的光滑满射:
$$f_B(x)=o+\frac{2rx}{x^{T}x+1}\in {\mathcal{P}}_B,\forall x\in {\mathbb{R}}^n$$

$f_B$引入了一个新坐标$\xi$,在${\mathbb{R}}^n$中优化$\xi$总能满足$q$的${\mathcal{P}}_B$约束。对第$i$个中间点$q_i$,记其新坐标为${\xi }_i$。相应地,令$\xi$表示$q$的新坐标。优化$\xi$需要传播$\partial J/\partial q$的梯度。记$\partial J/\partial q_i$的第$i$项为$g_i$,对$f_B$求导得到$J$关于${\xi }_i$的梯度:
$$\frac{\partial J}{\partial {\xi }_i}=\frac{2r_i{g}_i}{{\xi }_i^T{\xi }_i+1}-\frac{4r_i({\xi }_i^T{g}_i){\xi }_i}{({\xi }_i^T{\xi }_i+1{)}^2}$$

若优化需要从初始猜测$q$开始,可用$f_B$的局部逆计算对应的$\xi$。对$1\le i<M$,有:
$${\xi }_i=\frac{r_i-\sqrt{r_i^2-\Vert q_i-o_i{\Vert }_2^2}}{\Vert q_i-o_i{\Vert }_2^2}(q_i-o_i)$$

类似地,我们分析$f_B$对${\mathcal{P}}_B$内约束极小值的影响。尽管$f_B$不具备微分同胚的一一对应性,但其分量都很规则。首先,$f_o$只取点的前$n$个分量,该操作至少保持了${\mathbb{B}}^n$或${\mathbb{S}}^n$内极小值点处的一阶必要条件。其次,$f_s$是${\mathbb{S}}^n$到${\mathbb{R}}^n$的微分同胚,满足命题2。因此,我们同样可以确认$f_B$不会引入额外的伪极小值点或消除已有的极小值点。如图5所示,2维球内的约束极小值被变换为无约束极小值。

4.3 多面体区域的空间约束消除

现在考虑消除多面体约束。设$\mathcal{P}$为有界凸多面体${\mathcal{P}}_H$:
P H = { x ∈ R n ∣ A x ⪯ b } \mathcal{P}_H=\{x\in\mathbb{R}^n|Ax\preceq b\} PH​={x∈Rn∣Ax⪯b}
其中$\text{Int}({\mathcal{P}}_H)\ne \varnothing$。常见优化算法将${\mathcal{P}}_H$的H表示(超平面表示)作为线性不等式约束。本文利用其几何性质来消除这些约束,使MINCO能自由变形。为此,我们改用${\mathcal{P}}_H$的V表示(顶点表示),其中${\mathcal{P}}_H$内任意点$q$可表示为所有顶点的带权重的凸组合,即广义重心坐标(barycentric coordinate)。为获得顶点,我们对${\mathcal{P}}_H$的对偶多面体应用凸包算法,基于Seidel算法计算的内点。注意对我们的低维情形($n\le 4$),该过程引入的计算开销可忽略。

消除多面体约束的过程如图6所示。记${\mathcal{P}}_H$的$\hat{n}+1$个顶点为$(v_0,\dots ,v_{\hat{n}})$,其中$v_i\in {\mathbb{R}}^n,\forall i$。点$q\in {\mathcal{P}}_H$的重心坐标即为这些顶点对应的权重。为得到更紧致的形式,定义${\hat{v}}_i=v_i-v_0$和$\hat{V}=({\hat{v}}_1,\dots ,{\hat{v}}_{\hat{n}})$,则$q$可表示为:
$$q=v_0+\hat{V}w$$
其中$w=(w_1,\dots ,w_{\hat{n}}{)}^T\in {\mathbb{R}}^{\hat{n}}$为重心坐标的最后$\hat{n}$项。凸组合中坐标的集合可写为:
P H w = { w ∈ R n ^ ∣ w ⪰ 0 , ∥ w ∥ 1 ≤ 1 } \mathcal{P}_H^w=\{w\in\mathbb{R}^{\hat{n}}|w\succeq0,\|w\|_1\leq1\} PHw​={w∈Rn^∣w⪰0,∥w∥1​≤1}

多面体理论的主定理确认了${\mathcal{P}}_H^w$与${\mathcal{P}}_H$在变换(57)下的等价性。通过为$q$添加辅助变量并做线性映射,多面体被精确转化为${\mathbb{R}}^{\hat{n}+1}$中的标准单纯形。这个过程除了增加了决策变量的维数外,没有给优化问题引入额外的非线性。因此我们后面只考虑$q$对应的$w$作为决策变量。

单纯形约束(58)可通过非线性变换消除。首先利用逐元素平方映射$[\cdot {]}^2:{\mathbb{R}}^{\hat{n}}\mapsto {\mathbb{R}}^{\hat{n}}$消除非负约束:
$$w=[x{]}^2$$
这样,$w$上的约束${\mathcal{P}}_H^w$被转化为$x$上的闭单位球约束${\mathbb{B}}^{\hat{n}}$:
B n ^ = { x ∈ R n ^ ∣ ∥ x ∥ 2 ≤ 1 } \mathbb{B}^{\hat{n}}=\{x\in\mathbb{R}^{\hat{n}}|\|x\|_2\leq1\} Bn^={x∈Rn^∣∥x∥2​≤1}
因此,我们可以再次利用映射$f_B$。复合(57)、$[\cdot {]}^2$和$f_B$可得从${\mathbb{R}}^{\hat{n}}$到${\mathcal{P}}_H$的光滑满射$f_H$:
$$f_H(x)=v_0+\frac{4\hat{V}[x{]}^2}{(x^{T}x+1{)}^2}\in {\mathcal{P}}_H,\forall x\in {\mathbb{R}}^{\hat{n}}$$

$f_H$引入了一个新坐标$\xi$,任意$\xi \in {\mathbb{R}}^{\hat{n}}$都能确保$q\in {\mathcal{P}}_H$。${\mathcal{P}}_H$的边界也是可达的。类似地,令$\xi$表示$q$的新坐标。优化$\xi$需要梯度传播。记$\partial J/\partial q$中的第$i$项梯度为$g_i$,则$f_H$层的梯度为:
$$\frac{\partial J}{\partial {\xi }_i}=\frac{8{\xi }_i\circ {\hat{V}}^T{g}_i}{({\xi }_i^T{\xi }_i+1{)}^2}-\frac{16g_i^T\hat{V}[{\xi }_i{]}^2}{({\xi }_i^T{\xi }_i+1{)}^3}{\xi }_i$$

若指定了初始猜测$q$,对应的$\xi$可通过$f_H$的局部逆计算。$q_i$的重心坐标可用Warren等人的解析方法求得。之后再利用$[\cdot {]}^2$和$f_B$的解析局部逆即得所需的${\xi }_i$。另一种灵活的方法是直接最小化$f_H(\xi )$与给定$q_i$间的平方距离。这两种方法的计算开销都可忽略,但结果很有前景。

$f_H$中的$[\cdot {]}^2$给优化引入了额外的非线性。幸运的是,通过$[\cdot {]}^2$做变量替换是不等式转等式的一个特例。具体来说,不等式约束为$-w⪯0$。引入辅助变量$x$,等价的等式约束为$-w+[x{]}^2=0$,即$w=[x{]}^2$。Bertsekas在文献[55]的4.3节证明了这类约束转换对带不等式约束的优化问题的一阶/二阶必要条件和二阶充分条件是保持的。我们确认$f_H$中的额外非线性在实践中不会排除期望的极小值或产生非期望的极小值。

对$q$的直接约束,不管是球形${\mathcal{P}}_B$还是多面体${\mathcal{P}}_H$,都通过光滑满射$q(\xi )$消除了。此后我们就可以在$\xi$上无约束地优化$J(q(\xi ),\mathbf{T}(\tau ))$。

4.4 时间积分罚函数

消除直接约束后,MINCO轨迹可自由变形以满足连续时间约束$G$。然而,在整条轨迹上施加$G$涉及无穷多不等式,无法用约束优化求解。进一步还需要时间离散化,通常会产生大量决策变量。为保持轨迹参数化的稀疏性,我们将约束评估的分辨率与决策变量数解耦。受约束转录(constraint transcription)的启发,提出用约束违反度的积分将$G$转化为有限个约束。

对轨迹$p:[0,T]\mapsto {\mathbb{R}}^m$,定义
$$I_G^k[p]={\int }_0^T\max [G(p(t),\dots ,p^{(s)}(t)),0{]}^{k}dt$$
其中$k\in {\mathbb{R}}_{>0}$,max$[\cdot ,0{]}^k$为逐元素取正部再幂的复合函数。对应的泛函型约束等价于等式约束$I_G^k[p]=0$。实际上,$I_G^k[p]$是轨迹参数的函数,我们将其作为罚项。当$k=1$时为非光滑的精确罚函数,当$k>1$时为可微的严好的,我继续讲解并给出代码示例。

从上文可知,$I_G^k[p]$在$k=1$时为非光滑的精确罚函数,在$k>1$时为可微的严格凸罚函数。因此,我们可以采用$I_G^3[p]$或$I_G^1[p]$的光滑逼近。为简洁起见,后面均使用$I_G^3[p]$,除非特别说明。我们选择罚函数法有两个原因:一是式(68)中的积分只能数值计算,约束逼近不可避免;二是罚函数法对初值的可行性没要求,而构造可行初值本就不平凡。

定义$p(t)$的时间积分罚函数为:
$$I_G[p]={\chi }^T{I}_G^k[p]$$
其中$\chi \in {\mathbb{R}}_{\ge 0}^{n_g}$为权重向量,通常取较大常数。若无违反,则$I_G[p]$保持为零;否则若$p(t)$的任何部分违反$G$中任意约束,$I_G[p]$就会迅速增大。将$I_G[p]$并入目标泛函,即可在可接受的公差内满足连续时间约束。

实际中$I_G[p]$只能用求积公式计算。为此,先定义采样函数$G_{\tau }:{\mathbb{R}}^{2s\times m}\times {\mathbb{R}}_{>0}\times [0,1]\mapsto {\mathbb{R}}^{n_g}$为:
$$G_{\tau }(c_i,T_i,\tau )=G(c_i^T\beta (T_i\cdot \tau ),\dots ,c_i^T{\beta }^{(s)}(T_i\cdot \tau ))$$
其中$\tau \in [0,1]$为归一化时间戳。则$I_G[p]$的求积公式$I:{\mathbb{R}}^{2Ms\times m}\times {\mathbb{R}}_{>0}^M\mapsto {\mathbb{R}}_{\ge 0}$为罚项采样值的加权和:
$$I(c,\mathbf{T})=\sum _{i=1}^M{T}_i\sum _{j=0}^{{\kappa }_i}{\bar{\omega }}_j{\chi }^T\max [G_{\tau }(c_i,T_i,\frac{j}{{\kappa }_i}),0{]}^k$$
其中${\kappa }_i$控制分辨率。我们选用了梯形公式$({\bar{\omega }}_0,{\bar{\omega }}_1,\dots ,{\bar{\omega }}_{{\kappa }_i-1},{\bar{\omega }}_{{\kappa }_i})=(\frac{1}{2},1,\dots ,1,\frac{1}{2})$,因为它对$C^2$的病态被积函数的数值积分表现可靠。直观上,$I(c,\mathbf{T})$是$I_G[p]$的可微逼近,其精度可通过${\kappa }_i$调节。大多数时间戳处的函数值和梯度可并行计算再直接组合。

下面给出计算积分罚函数$I(c,\mathbf{T})$的C++代码示例:

// MINCO trajectory: defined by (45)-(47)
struct MINCO {
    int s, m;              // system order and dimension
    vector<MatrixXd> c;    // coefficient matrices for M pieces 
    VectorXd T;            // time durations for M pieces
};

// Evaluates the integral penalty functional I(c,T) in (75)
double evalPenalty(const MINCO& traj, 
                   const VectorXd& chi, 
                   function<VectorXd(Vector3d)> G,
                   int k = 3) 
{
    int M = traj.T.size();                      
    vector<Vector3d> dcp(M), dcv(M), dca(M), dcj(M);  // p,v,a,j
    
    double I = 0.0;
    for (int i = 0; i < M; i++) {
        double Ii = 0.0;
        for (int j = 0; j <= KAPPA; j++) {
            const double tau = j * 1.0 / KAPPA;
            betaVector(tau, traj.T[i], traj.s - 1, dcp[i]); 
            betaVector(tau, traj.T[i], traj.s - 2, dcv[i]);
            betaVector(tau, traj.T[i], traj.s - 3, dca[i]);
            betaVector(tau, traj.T[i], traj.s - 4, dcj[i]);
            
            Vector3d p = dcp[i].transpose() * traj.c[i];
            Vector3d v = dcv[i].transpose() * traj.c[i];
            Vector3d a = dca[i].transpose() * traj.c[i]; 
            Vector3d j = dcj[i].transpose() * traj.c[i];
            
            VectorXd Gtau = G(p, v, a, j);
            
            double w = (j == 0 || j == KAPPA) ? 0.5 : 1.0;
            Ii += w * chi.dot(Gtau.cwiseMax(0.0).array().pow(k));
        }
        I += traj.T[i] * Ii;
    }
    return I;
}

其中betaVector根据计算基函数在$\tau$处的值。可见,惩罚项的计算可方便地并行化。此外,取$k=1$时可构造非光滑精确罚函数,若$G$过于复杂也可考虑自动微分。

4.5 无约束非线性规划求解轨迹优化

由于式中的几何约束$\mathcal{F}$和连续时间约束$G$,最优的轨迹参数化通常难以知晓。有别于传统方法用大量变量逼近解,本文提出通过稀疏的无约束非线性规划松弛原问题,并利用MINCO类的空间-时间变形。定义如下松弛:
$$\underset{\xi ,\tau }{\min }J(q(\xi ),\mathbf{T}(\tau ))+I(c(q(\xi ),\mathbf{T}(\tau )),\mathbf{T}(\tau ))$$
其中$J$为MINCO类的时间正则化控制量;$I$为罚函数的求积公式。注意任何特定任务的需求,无论是目标还是约束,都可并入而不影响其结构。

为生成微分平坦多旋翼的轨迹,首先将其平坦输出参数化为MINCO类。在固定数量的片段分配到每个${\mathcal{P}}_i$后,变量替换被施加以自动满足全部直接约束,如正则项${\rho }_s$对应的$\mathbf{T}\in {\mathbb{R}}_{>0}$、${\rho }_f$对应的$\Vert \mathbf{T}{\Vert }_1<T_{\Sigma }$以及中间点约束$q_i\in {\mathcal{P}}_{di/Ke}\cap {\mathcal{P}}_{d(i+1)/Ke},\forall i$。同时,将给定的状态-输入约束$G_D$通过平坦映射${\Psi }_x,{\Psi }_u$转化为对平坦输出的等价约束$G$。至此,我们得到了(76)的目标函数。很明显,除${\Psi }_x$和${\Psi }_u$外,其他所有层的梯度传播都已推导。对${\Psi }_x$和${\Psi }_u$,要么采用自动微分,要么参照反向模式自动微分的思路解析推导。Baur-Strassen定理保证其计算效率与平坦映射本身一致。微分只需对给定的平坦动力学做一次性推导。有了梯度信息,就可用L-BFGS算法求解松弛问题。

下面是用C++求解无约束松弛问题的代码框架:

// Unconstrained relaxation of problem (11)
// Objective function: J(q(xi),T(tau)) + I(c(q(xi),T(tau)),T(tau))
struct Objective {
    MINCO traj;                        // trajectory representation
    ConstrG;                          // equivalent constraints
    Penalty I;                         // integral penalty functional
    Reg rho;                           // time regularization term

    // L-BFGS callback
    double operator()(const VectorXd& x, VectorXd& grad) 
    {
        // Unpack optimization variables xi and tau from x
        VectorXd xi  = x.head(DIM_XI);   
        VectorXd tau = x.tail(DIM_TAU);

        // Get intermediate points q and time allocation T   
        VectorXd q = inverseSimplexMap(xi);    // Sec. 4.3, Eq.(85) 
        VectorXd T = diffeoTimeMap(tau);       // Sec. 4.1, Eq.(37)

        // Calculate trajectory parameters c(q,T)
        SparseMatrix<double> M = getM(traj.s, T);   // Eq.(14)
        VectorXd b = getB(traj.s, q, T);             // Eq.(15)
        SparseLU<SparseMatrix<double>> solver;  
        solver.compute(M);
        MatrixXd c = solver.solve(b); 
        
        // Compute objective and gradient  
        double J  = ctrlEffort(c, T) + rho(T.sum());    // Eq.(36)
        double I  = integralPenalty(G, c, T);            // Eq.(75) 

        MatrixXd dJdc, dJdT, dIdc, dIdT;
        ctrlEffortGrad(c, T, dJdc, dJdT); 
        integralPenaltyGrad(G, c, T, dIdc, dIdT);

        MatrixXd dc_dq, dc_dT;
        coeffSensitivity(q, T, dc_dq, dc_dT);    // Sec. 3.3

        VectorXd dq_dxi = simplexMapJacobian(xi);         // Eq.(86)
        MatrixXd dT_dtau = diffeoTimeMapJacobian(tau);    // Eq.(38)

        VectorXd dJdq = (dJdc * dc_dq).rowwise().sum();
        VectorXd dJdT = dJdT + (dJdc * dc_dT).rowwise().sum();  
        VectorXd dIdq = (dIdc * dc_dq).rowwise().sum();
        VectorXd dIdT = dIdT + (dIdc * dc_dT).rowwise().sum();

        grad.head(DIM_XI) = dq_dxi.cwiseProduct(dJdq + dIdq);
        grad.tail(DIM_TAU) = dT_dtau.transpose() * (dJdT + dIdT);

        return J + I; 
    }
};

// Trajectory generation by solving problem (76)
MatrixXd trajOpt(const MINCO& traj, const ConstrG& G, 
                 const Penalty& I, const Reg& rho)  
{
    // Initialize optimization variables
    VectorXd x(DIM_XI + DIM_TAU);
    x.head(DIM_XI) = initSimplexParam(traj.q);  
    x.tail(DIM_TAU) = initDiffeoTimeParam(traj.T);

    // Setup NLP solver  
    Objective obj({traj, G, I, rho});

    LBFGSParam<double> param;
    param.epsilon = 1e-6;
    param.max_iterations = 500;

    LBFGSSolver<double> solver(param);

    // Solve the problem
    double fx;
    int niter = solver.minimize(obj, x, fx);

    // Recover optimal trajectory 
    traj.q = inverseSimplexMap(x.head(DIM_XI));
    traj.T = diffeoTimeMap(x.tail(DIM_TAU));

    SparseMatrix<double> M = getM(traj.s, traj.T);
    VectorXd b = getB(traj.s, traj.q, traj.T);         
    SparseLU<SparseMatrix<double>> slu;
    slu.compute(M);
    MatrixXd c_star = slu.solve(b);
    
    return c_star;
}

其中展示了目标函数Objective的实现和轨迹优化入口函数trajOpt。可见,核心是目标函数和梯度的计算,涉及第3.3节和第4.1-4.4节的相关内容。coeffSensitivity对应式(64)(68)中系数对中间点和时间分配的敏感性分析。几何参数和时间参数分别采用多面体单纯形映射和微分同胚来表示。

5. 应用

本节通过大量仿真实验和实际飞行,展示了所提优化框架在无约束控制量最小化、安全飞行走廊内的轨迹生成以及SE(3)运动规划等方面的性能。实验结果证明了该方法在求解效率、轨迹质量、鲁棒性和通用性上的优越表现。限于篇幅,本文未展示部分应用的技术细节,感兴趣的读者可参考原文。

6. 结论

本文提出了一个灵活高效的多旋翼轨迹规划优化框架,其核心为基于最优性条件的MINCO(Minimum Control)轨迹、基于光滑映射的约束消除方案、基于约束转录的惩罚泛函方法以及平坦映射的反向微分。所有这些技术的高效性和通用性源于其低计算复杂度和少的先验假设。大量仿真实验表明,该框架的求解速度比现有方法高出数个数量级,同时具有顶尖的解的质量。多样化的应用场景展示了该框架

具体的应用案例请看我的下一篇blog