稠密环境下编队飞行的分布式群轨迹优化（二）

稠密环境下编队飞行的分布式群轨迹优化（二）

原文章题目：Distributed Swarm Trajectory Optimization for Formation Flight in
Dense Environments
作者：Lun Quan, Longji Yin, Chao Xu, and Fei Gao
出处：2022 International Conference on Robotics and Automation (ICRA)
原文下载链接：点击下载
背景介绍部分在这里：稠密环境下编队飞行的分布式群轨迹优化（一）
实验部分介绍在这里：稠密环境下编队飞行的分布式集群轨迹优化（三）

三、微分编队相似性指标

   $N$个机器人的编队由无向图$\mathcal{G}=(\mathcal{V},\mathcal{E})$建模，其中 V = { 1 , 2 , . . . , N } {\cal V} = \{ 1,2,...,N\} V={1,2,...,N}是顶点集,$\mathcal{E}\subset \mathcal{V}\times \mathcal{V}$是边缘。在图$\mathcal{G}$中，顶点$i$代表位置向量${\mathbf{p}}_i=[x_i,y_i,z_i]\in {}^3$的第$i^{th}$个机器人。连接顶点$i\in \mathcal{V}$和顶点$j\in \mathcal{V}$的边$e_{ij}\in \mathcal{E}$意味着机器人$i$和$j$可以测量彼此之间的几何距离。在我们的工作中，每个机器人都与所有其他机器人进行通信，因此形成图$\mathcal{G}$是完整的。图$\mathcal{G}$的每条边都与一个非负数作为权重相关联。在这项工作中,边$e_{ij}$的权重由下式给出:
$$w_{ij}={\Vert {\mathbf{p}}_i-{\mathbf{p}}_j\Vert }^2,(i,j)\in \mathcal{E},（1）$$
其中$\parallel \cdot \parallel$表示欧几里德范数。现在确定了编队图$\mathcal{G}$的邻接矩阵$\mathbf{A}\in \mathbb{R}{}^{N\times N}$和度矩阵$\mathbf{D}\in \mathbb{R}{}^{N\times N}$。因此，相应的拉普拉斯矩阵由下式给出：
$$\mathbf{L}=\mathbf{D}-\mathbf{A}.（2）$$
   利用上述矩阵，图$\mathcal{G}$的对称归一化拉普拉斯矩阵定义为
$$\hat{\mathbf{L}}={\mathbf{D}}^{-1/2}\mathbf{L}{\mathbf{D}}^{-1/2}=\mathbf{I}-{\mathbf{D}}^{-1/2}\mathbf{A}{\mathbf{D}}^{-1/2},（3）$$
其中$\mathbf{I}\in \mathbb{R}{}^{N\times N}$是单位矩阵。
   作为图表示矩阵，拉普拉斯包含有关图结构的信息[25]。为了实现所需的群体形成，我们提出了一种形成相似距离度量：
$$f={\Vert \hat{\mathbf{L}}-{\hat{\mathbf{L}}}_{des}\Vert }_F^2=\mathrm{tr}\left\{{\left(\hat{\mathbf{L}}-{\hat{\mathbf{L}}}_{\mathrm{des}}\right)}^T\left(\hat{\mathbf{L}}-{\hat{\mathbf{L}}}_{\mathrm{des}}\right)\right\},（4）$$
其中 t r { ⋅ } tr\{ \cdot \} tr{⋅}表示矩阵的迹，$\hat{\mathbf{L}}$是当前群体编队的对称归一化拉普拉斯算子, ${\hat{\mathbf{L}}}_{\mathrm{des}}$是期望编队的对应项。弗罗贝尼乌斯范数$\parallel \cdot {\parallel }_F$用于我们的距离度量。$f$ 对于编队的平移和旋转本身是不变的，因为相应的图形是由机器人位置之间的绝对距离加权的。缩放不变性是通过公式（3）中的度矩阵对图拉普拉斯算子进行归一化来实现的。
   我们的度量对于每个机器人的位置在分析上是可微的。对于机器人$i$，我们使用其$n$个相邻边的权重 { e i 1 , e i 2 , . . . , , e i n } \{ {e_{i1}},{e_{i2}},...,,{e_{in}}\} {ei1​,ei2​,...,,ein​}形成权重向量${\mathbf{w}}_i=[w_{i1},w_{i2},...,,w_{in}{]}^T$ 。根据链式求导法则，$f$相对于的梯度${\mathbf{p}}_i$可写为
$${\mathbf{w}}_i=[w_{i1},w_{i2},...,,w_{in}{]}^T（5）$$
根据我们的度量（4），$f$相对于每个权重$w_{ij}$的梯度可以计算如下
∂ f ∂ w i j = t r { ( ∂ f ∂ L ^ ) T ( ∂ L ^ ∂ w i j ) } , （ 6 ） \frac{{\partial f}}{{\partial {w_{ij}}}} = tr\{ {(\frac{{\partial f}}{{\partial {\bf{\hat L}}}})^T}(\frac{{\partial {\bf{\hat L}}}}{{\partial {w_{ij}}}})\} ,（6） ∂wij​∂f​=tr{(∂L^∂f​)T(∂wij​∂L^​)},（6）
其中
$$\frac{\partial f}{\partial \hat{\mathbf{L}}}=\frac{\partial ||\hat{\mathbf{L}}-{\hat{\mathbf{L}}}_{des}|{|}_F^2}{\partial \hat{\mathbf{L}}}=2(\hat{\mathbf{L}}-{\hat{\mathbf{L}}}_{des}),（7）$$
$$\frac{\partial \hat{\mathbf{L}}}{\partial w_{ij}}=-\frac{\partial ({\mathbf{D}}^{-1/2}\mathbf{A}{\mathbf{D}}^{-1/2})}{\partial w_{ij}}.（8）$$
那么梯度$\partial f/\partial {\mathbf{w}}_i$可以写为
$$\partial f/\partial {\mathbf{w}}_i=[\partial f/\partial w_{i1},\partial f/\partial w_{i2},...,\partial f/\partial w_{in}{]}^T.（9）$$
对于$\partial {\mathbf{w}}_i/\partial {\mathbf{p}}_i$ ，由于权重函数(1)是可微的，因此可以很容易地导出雅可比行列式。图2显示了正方形编队的度量和梯度的轮廓。

四、编队飞行时空轨迹优化

A.轨迹表示

  在这项工作中，我们采用文献[26]中MINCO表示。这是一种最小控制工作量的多项式轨迹类，用于对平坦输出轨迹进行时空变形。
Ξ M I N C O = { p ( t ) : [ 0 , T Σ ] ↦ R m ∣ c = C ( q , T ) , q ∈ R m ( M − 1 ) , T ∈ R > 0 M } , （ 10 ） {\Xi _{MINCO}} = \{ p(t):[0,{T_\Sigma }] \mapsto \Bbb R {^m}|{\bf{c}} = C({\bf{q}},{\bf{T}}),{\bf{q}} \in \Bbb R {^{m(M - 1)}},{\bf{T}} \in \Bbb R _{ > 0}^M\} , （10） ΞMINCO​={p(t):[0,TΣ​]↦Rm∣c=C(q,T),q∈Rm(M−1),T∈R>0M​},（10）
其中，$\mathbf{c}=(c_1^T,\cdots ,c_M^T{)}^T$为多项式系数，$\mathbf{q}=(q_1,\cdots ,q_{M-1})$中间点, $\mathbf{T}=(T_1,\cdots ,T_M{)}^T$时间向量，$C(\mathbf{q},\mathbf{T})$是根据文献[26]中的定理2构造的参数映射,$T_{\Sigma }=\sum _M^{i=1}{T}_i$是总时间。
   $n$维$M$段轨迹$p(t)$定义为：
$$p(t)=p_i(t-t_{i-1}),\forall t\in [t_{i-1},t_i),（11）$$
第$i^{th}$块轨迹由5次多项式（即 $N=5$）表示
$$p_i(t)=c_i^T\beta (t),\forall t\in [0,T_i],（12）$$
其中，$c_i\in {\mathbb{R}}^{6\times m}$是系数矩阵，$\beta (t)=[1,t,...,t^{\mathrm{N}}{]}^T$是自然基，$T_i=t_i-t_{i-1}$是第$i^{th}$块的时间分配。
   MINCO由$(\mathbf{q},\mathbf{T})$唯一确定。并且参数$\mathbf{c}=C(\mathbf{q},\mathbf{T})$映射以线性时间和空间复杂度将轨迹表示$(\mathbf{c},\mathbf{T})$转换为$(\mathbf{q},\mathbf{T})$，这使得任何二阶连续成本函数$J(\mathbf{c},\mathbf{T})$用$\tilde{J}(\mathbf{q},\mathbf{T})$表示。因此，可以分别通过$\partial \tilde{J}/\partial \mathbf{q}$和$\partial \tilde{J}/\partial \mathbf{T}$求得$\partial J/\partial \mathbf{c}$和$\partial J/\partial \mathbf{T}$。
   特别是，为了处理时间积分约束$\psi (p(t),\cdots ,p^{(3)}(t))\underline{\prec }0$，例如避免碰撞和动力学可行性，我们将它们转换为有限维约束$\psi ({\hat{p}}_{i,j})$通过对轨迹上的约束点${\hat{p}}_{i,j}=p_i((j/{\kappa }_i)\cdot T_i)$进行采样，其中${\kappa }_i$是第$i^{th}$块上的采样点。

B.优化问题的构建

   我们将编队飞行的轨迹生成公式化为无约束优化问题：
$$\underset{\mathbf{c},\mathbf{T}}{\min }\left[J_e,J_t,J_o,J_f,J_r,J_d,J_u\right]\cdot \lambda ,（13）$$
其中，$\lambda$是权衡每个成本函数的权重向量。
1）平滑度惩罚$J_e$：第$i^{th}$块轨迹的三阶控制输入及其梯度写为：
$$J_e=\underset{0}{\overset{T_i}{\int }}\parallel p_i^{(3)}(t){\parallel }^{2}dt,（14）$$
$$\frac{\partial J_e}{\partial c_i}=2\left(\underset{0}{\overset{T_i}{\int }}{\beta }^{(3)}(t){\beta }^{(3)}{(t)}^{T}dt\right)c_i,（15）$$
$$\frac{\partial J_e}{\partial T_i}=c_i^T{\beta }^{(3)}(T_i){\beta }^{(3)}(T_i{)}^T{c}_i.（16）$$
2）总时间惩罚$J_t$：为了保证轨迹的光滑性，我们最小化总时间$J_t=\sum _M^{i=1}T$。梯度由$\partial J_{\mathrm{t}}/\partial \mathbf{c}=0$和$\partial J_{\mathrm{t}}/\partial \mathbf{T}=1$求出。
3）碰撞项惩罚$J_o$：受文献[27]的启发，避障惩罚$J_o$使用欧几里得符号距离场（ESDF）计算。选取靠近障碍物的约束点：
$${\psi }_o({\hat{p}}_{i,j})=\{\begin{array}{ll}{d}_{thr}-d({\hat{p}}_{i,j}),& \text{if d(p^i,j)<dthr}\\ 0,& \text{if d(p^i,j)≥dthr}\end{array}（17）$$
其中, $d_{thr}$是安全阈值，$d({\hat{p}}_{i,j})$是所考虑的点与其周围最近的障碍物之间的距离。然后通过计算采样约束函数的加权和得到避障惩罚：
J o = T i κ i ∑ j = 0 κ i ω ˉ j max ⁡ { ψ o ( p ^ i , j ) , 0 } 3 , （ 18 ） {J_o} = \frac{{{T_i}}}{{{\kappa _i}}} \sum \limits_{j = 0}^{{\kappa _i}} {\bar \omega _j}\max {\{ {\psi _o}({\hat p_{i,j}}),0\} ^3},（18） Jo​=κi​Ti​​j=0∑κi​​ωˉj​max{ψo​(p^​i,j​),0}3,（18）
其中$({\bar{\omega }}_0,{\bar{\omega }}_1,\cdots ,{\bar{\omega }}_{{\kappa }_i-1},{\bar{\omega }}_{{\kappa }_i})=(1/2,1,\cdots ,1,1/2)$是遵循梯形规则的正交系数，如文献[28]。
   $J_o$相对于$c_i$和$T_i$的梯度具体如下：
$$\frac{\partial J_o}{\partial c_i}=\frac{\partial J_o}{\partial {\psi }_o}\frac{\partial {\psi }_o}{\partial c_i},（19）$$
$$\frac{\partial J_o}{\partial T_i}=\frac{J_o}{T_i}+\frac{\partial J_o}{\partial {\psi }_o}\frac{\partial {\psi }_o}{\partial t}\frac{\partial t}{\partial T_i},（20）$$
$$\frac{\partial t}{\partial T_i}=\frac{j}{{\kappa }_i},t=\frac{j}{{\kappa }_i}{T}_i,（21）$$
其中，$t$ 是相对时间。对于$d({\hat{p}}_{i,j})<d_{thr}$的情况，梯度由下式给出
$$\frac{\partial {\psi }_o}{\partial c_i}=-\beta (t)\nabla d^T,\frac{\partial {\psi }_o}{\partial t}=-\nabla d^T\dot{p}(t),（22）$$
其中，$\nabla d$是ESDF在${\hat{p}}_{i,j}$中的梯度。否则，梯度为$\partial {\psi }_o/\partial c_i=0,\partial {\psi }_o/\partial t=0$。
4）集群编队惩罚$J_f$：在第三节中，我们设计了一个可微分度量来量化群体形成之间的相似距离。在优化中，当前地层与期望地层之间的相似性误差通过${\psi }_f=f(p(t),{\bigcup }_{\Phi }{p}_{\varphi }(\tau ))f\left(\cdot \right)$来测量，其中$f\left(\cdot \right)$在（4）中详细说明，$\Phi$表示集合其他智能体的。
   由于$J_f$涉及其他智能体的轨迹，因此我们需要处理自身轨迹的相对时间$t=j{T}_i/{\kappa }_i$和其他智能体轨迹的全局时间戳$\tau =T_1+...+T_{i-1}+j{T}_i/{\kappa }_i$。$J_f$考虑任意$1\le l\le i$的先前时间 ，并用公式表示：
J f = T i κ i ∑ κ i j = 0 ω ˉ j max ⁡ { ψ f ( p ( t ) , ⋃ Φ p ϕ ( τ ) ) , 0 } 3 . （ 23 ） {J_f} = \frac{{{T_i}}}{{{\kappa _i}}} \sum \limits_{{\kappa _i}}^{j = 0} {\bar \omega _j}\max {\{ {\psi _f}(p(t),\bigcup\limits_\Phi {{p_\phi }(\tau )} ),0\} ^3}.（23） Jf​=κi​Ti​​κi​∑j=0​ωˉj​max{ψf​(p(t),Φ⋃​pϕ​(τ)),0}3.（23）
   $J_f$相对于$c_i$和$T_l$的梯度具体如下
$$\frac{\partial J_f}{\partial c_i}=\frac{\partial J_f}{\partial {\psi }_f}\frac{\partial {\psi }_f}{\partial c_i},（24）$$
$$\frac{\partial J_f}{\partial T_l}=\frac{J_f}{T_l}+\frac{\partial J_f}{\partial {\psi }_f}\frac{\partial {\psi }_f}{\partial T_l}.（25）$$
   为了求出$\partial {\psi }_f/\partial T_l$，需要${\psi }_f$对 $t$ 和 $\tau$ 进行求导：
$$\frac{\partial {\psi }_f}{\partial T_l}=\frac{\partial {\psi }_f}{\partial t}\frac{\partial t}{\partial T_l}+\frac{\partial {\psi }_f}{\partial \tau }\frac{\partial \tau }{\partial T_l},（26）$$
$$\frac{\partial t}{\partial T_l}=\{\begin{array}{ll}\frac{j}{{\kappa }_i},& \text{if l=i,}\\ 0,& \text{if l<i}\end{array},\frac{\partial \tau }{\partial T_l}=\{\begin{array}{ll}\frac{j}{{\kappa }_i},& \text{if l=i,}\\ 0,& \text{if l<i}\end{array}（27）$$
${\psi }_f$相对于$c_i$、$t$ 和 $\tau$ 的梯度由下式给出
$$\frac{\partial {\psi }_f}{\partial c_i}=\frac{\partial {\psi }_f}{\partial p(t)}\frac{\partial p(t)}{\partial c_i},（28）$$
$$\frac{\partial {\psi }_f}{\partial t}=\frac{\partial {\psi }_f}{\partial p(t)}\frac{\partial p(t)}{\partial t}=\frac{\partial {\psi }_f}{\partial p(t)}\dot{p}(t),（29）$$
$$\frac{\partial {\psi }_f}{\partial \tau }=\sum _{\Phi }\frac{\partial {\psi }_f}{\partial p_{\varphi }(\tau )}\frac{\partial p_{\varphi }(\tau )}{\partial \tau }=\sum _{\Phi }\frac{\partial {\psi }_f}{\partial p_{\varphi }(\tau )}{\dot{p}}_{\varphi }(\tau ),（30）$$
其中 ${\psi }_f$对 $p(t)$和 $p_{\varphi }(\tau )$ 的梯度见 (5)。
5）集群编队无人机间碰撞惩罚$J_r$：我们惩罚在全局时间戳 处接近其他智能体轨迹的约束点。因此，群体相互回避的成本函数定义为
J r = ∑ Φ T i κ i ∑ κ i j = 0 ω ˉ j max ⁡ { ψ r ϕ ( p ( t ) , τ ) , 0 } 3 , （ 31 ） {{J_r} = \sum \limits_\Phi \frac{{{T_i}}}{{{\kappa _i}}}\sum \limits_{{\kappa _i}}^{j = 0} {{\bar \omega }_j}\max {{\{ {\psi _{{r_\phi }}}(p(t),\tau ),0\} }^3},}（31） Jr​=Φ∑​κi​Ti​​κi​∑j=0​ωˉj​max{ψrϕ​​(p(t),τ),0}3,（31）
$${\psi }_{r_{\varphi }}(p(t),\tau )=D_r^2-d(p(t),p_{\varphi }(\tau ){)}^2,（32）$$
$$d(p(t),p_{\varphi }(\tau ))=\parallel p(t)-p_{\varphi }(\tau )\parallel ,（33）$$
其中，$D_r$ 是每个智能体之间的距离。
   $J_r$相对于 $c_i$ 和 $T_l$ 的梯度与（24）和（25）相似， $\partial {\psi }_{r_{\varphi }}/\partial T_l$与（26）相同。当 ${\mathrm{D}}_r^2\ge d(p(t),p_{\varphi }(\tau ){)}^2$时， ${\psi }_{r_{\varphi }}$ 相对于 $c_i$、$t$ 和 $\tau$ 的梯度为
$$\frac{\partial {\psi }_{r_{\varphi }}}{\partial c_i}=-2\beta (t)(p(t)-p_{\varphi }(\tau ){)}^T,（34）$$
$$\frac{\partial {\psi }_{r_{\varphi }}}{\partial t}=-2(p(t)-p_{\varphi }(\tau ){)}^T\dot{p}(t),（35）$$
$$\frac{\partial {\psi }_{r_{\varphi }}}{\partial \tau }=2(p(t)-p_{\varphi }(\tau ){)}^T{\dot{p}}_{\varphi }(t).（36）$$
6）动力学可行性惩罚 $J_d$：我们限制速度、加速度和加加速度的最大值，以保证智能体可以执行轨迹。读者可以参考[15]了解更多详细信息。
7）约束点的均匀分布 $J_u$：约束点预计是空间均匀的。不均匀的约束点可能会跳过一些小尺寸的障碍物，这可能会降低最终轨迹的安全性。因此，对均匀分布惩罚 $J_u$ 进行优化，以防止约束点聚集在某些位置。读者可以参考[15]了解更多详细信息。

注

  本博客主要是对原文的翻译，对其中的内容细节并没有仔细推敲，翻译难免有误，希望各位看客评论区指出。谢谢！