Distributed Swarm Trajectory Optimization for Formation Flight in Dense Environments学习第四章之后部分

【论文笔记】Distributed Swarm Trajectory Optimization for Formation Flight in Dense Environments_UESTC_Chenlin的博客-CSDN博客摘要：对于空中蜂群，在各种情况下，在规定的编队中导航得到广泛应用。然而，相关的规划策略通常缺乏在杂乱环境中避免障碍的能力。为了解决这一不足，我们提出了一种基于优化的方法，确保编队飞行的无碰撞轨迹生成。本文提出了一种新的可微度量来量化编队之间的总体相似距离。然后，我们将该度量公式化为一个优化框架，该框架使用多项式轨迹实现时空规划。该框架还将碰撞惩罚最小化，从而可以同时处理编队保持和避障问题。为了验证我们的方法的有效性，我们进行了与其他前沿工作的基准比较。该方法与一个自治的分布式航空群系统相结合，在多障碍物..https://blog.csdn.net/clzsl/article/details/122602060 先叠buff，本人仅是本科生，出于爱好进行了学习和翻译，没有任何不尊重论文作者的意思，同时由于知识范围有限，肯定有一定错误，求指出！！！！！

在学习高飞老师的Distributed Swarm Trajectory Optimization for Formation Flight in Dense Environments论文时，我参考了上文博客，奈何博客作者没写全，这里我将从他开始不写的第四部分写起。前三部分可以看上文博客。

4.针对编队飞行的时空轨迹优化

A.轨迹表示：

在这项工作中，我们采用MINCO表示[26]，一种最小控制力多项式轨迹类来对平坦输出轨迹进行时空变形。

其中 $c = (c_{1}^{T},...,c_{M}^{T})$ 是多项式系数，q=（q1，···，qM−1)是中间点（这里不太懂intermediate points是什么），T=（T1，···，TM $)^{T}$ 是时间向量，C(q,T)是根据[26]中的定理2构造的参数映射， $T_{\sum}=\sum_{i=1}^{M} T_{i}$ 为总时间。

m维的M截轨迹p（t）定义为

$p(t) = p_i(t-t_{i-1}), \forall t\in [t_{i-1}, t_i)$ (11)

第i条轨迹由N=5次多项式表示

其中ci∈ $R^{6*m}$ 是系数矩阵，β（t）=[1，t，···， $t^N]^T$ 是自然基（这里不太懂natural basis是什么)， $T_i=t_i-t_{i-1}$ 是第i件的时间分配。

MINCO由（q，T）唯一确定。参数映射c=C（q，T）将轨迹表示（c，T）转换为具有线性时间和空间复杂性的（q，T），这允许任何二阶连续成本函数J（c，T）用 $\tilde{J}$ （q，T）表示。因此∂ $\tilde{J}$ /∂q和∂ $\tilde{J}$ /∂T可有效地从∂J/∂c和∂J/∂T分别表示。

特别是，为了处理时间积分约束ψ（p（t），··， $p^{(3)}$ （t））小于等于 0，例如碰撞避免和动力学可行性，我们通过对轨迹上的约束点 $\hat{p}_{i,j}$ = $p_i$ （（ $j/k_i$ ) · $T_i$ ）进行采样，将其转换为有限维约束ψ（ $\hat{p}_{i,j}$ ），其中 $k_i$ 是第i块上的样本数.

B.优化问题公式

我们将编队飞行的轨迹生成描述为一个无约束优化问题

其中λ是权衡每个成本函数的权重向量

1）控制力 $J_e$

（控制力这个翻译感觉有点问题，期待更好的翻译)：第i个工件轨迹及其梯度的三阶控制输入写为

2）总时间 $J_t$

为了确保轨迹的积极性（这里不太清楚aggressivenessof the trajectory的意义，我甚至之前翻译成了轨道的侵略性？？？），我们最小化总时间 $J_t = \sum_{i=1}^{M}T_i$ 。认为

3）避障惩罚 $J_o$

受[27]启发，使用欧几里德符号距离场（ESDF）计算避障惩罚 $J_o$ 。通过以下方式选择靠近障碍物的约束点：

其中， $d_{thr}$ 是安全阈值， $d(\hat{p}_{i,j})$ 是所考虑的点与周围最近障碍物之间的距离。然后通过计算采样约束函数的加权和得到避障惩罚

其中

是遵循梯形规则的正交系数[28]。（这里也不太懂什么是梯形规则的正交系数，搜索后看到下面这一篇文章也有，可以做个对照）

【论文笔记】Decentralized Spatial-Temporal Trajectory Planning for Multicopter Swarms_UESTC_Chenlin的博客-CSDN博客摘要：分布式的多无人机群具有灵活性和鲁棒性，而有效的时空轨迹规划仍然是一个挑战。本文介绍了一种分布式时空轨迹规划方法，该方法将一种良好的MINCO轨迹表示方法引入到多Agent场景中。我们的方法确保每一个agent的高质量本地规划，无论是群体的协同还是杂乱环境中的安全要求。然后，将局部轨迹生成问题转化为无约束优化问题，在毫秒内得到有效求解。此外，还设计了一个分散异步机制来触发每个agent的本地规划。给出了一个系统的解决方案，带有工程考虑的详细描述。广泛的基准和室内/室外实验验证了其广泛的适用性和高质量。我https://blog.csdn.net/clzsl/article/details/122895156 $J_o$ 关于 $c_i$ 和 $T_i$ 的梯度可以详细表述为

其中t是一段中的有关时间（relative time on the piece 这是原文不确定翻译是否正确），对于 $d(\hat{p}_{i,j})<d_{thr}$ 的情况，梯度可以由下式给出

其中∇d是 $\hat{p}_{i,j}$ 中ESDF的梯度.否则，梯度将变为∂ψo/∂ci=0，∂ψo/∂t=0。

4）蜂群形成相似性 $J_f$

在第三节中，我们设计了一个可微度量来量化群体之间的相似距离。在优化中，当前地层和所需地层之间的相似性误差通过 $\psi _f = f(p(t),U_\phi p_\phi (\tau ))$ 来测量，其中f（·）在（4）中详细说明，Φ表示其他影响（agents不知道怎么翻译）的集合

tr{·}表示矩阵的迹，L是当前编队的正则化拉普拉斯矩阵，Ldes是期望编队的拉普拉斯矩阵。我们的距离度量中使用了Frobenius范数·F。f对编队的平移和旋转是固有不变的，因为相应的图是由在机器人位置间的绝对距离加权的。而尺度缩放不变性是通过公式（3）中的度矩阵对图拉普拉斯进行归一化来实现的。

由于Jf涉及其他影响（agents）的轨迹，我们需要处理自身轨迹的相对时间t=jTi/ki和其他轨迹的全局时间戳 $\tau =T_1+...+T_{i-1}+jT_i/k_i$ 。 $J_f$ 考虑任何前一时间 $T_l$ (对任意1<=l<=i)和如下公式

$J_f$ 有关 $c_i,T_l$ 的梯度由下式计算

为了获得∂ψf/∂Tl，ψf需要用t和 $\tau$ 来区分：

$\psi _f$ 关于 ci、t和 $\tau$ 的梯度如下所示：

其中 $\psi _f$ 对p（t）的梯度和 $p_\phi (\tau )$ 在算式（5）中被写出

5）群体互惠回避 $J_r$

我们在全局时间戳 $\tau$ 处惩罚与其他Agent轨迹接近的约束点。因此，群互惠回避的代价函数定义为

其中 $D_r$ 是每个agents之间的间隙。

$J_r$ 关于 $c_i,T_l$ 的与式子（24）（25）相同，∂ $\psi _{rf}$ /∂Tl与式子（26）相同。

当 $D_r^2\geq d(p(t),p_\phi (\tau ))^2$ ，那么 $\psi _{r\phi }$ 关于 $c_i,t,\tau$ 的梯度是

6）动态可行性 $J_d$

我们限制速度、加速度和急动的最大值，以确保轨迹可以由agent执行。读者可参考[15]了解更多详细信息,

7）约束点的均匀分布 $J_u$

约束点应在空间上均匀分布。非均匀约束点可能会跳过一些小型障碍物，这可能会降低由此产生的障碍物的安全性。因此，对均匀分布惩罚 $J_u$ 进行了优化，以防止约束点在某些位置聚集。读者可参考[15]了解更多详细信息

5检测

为了证明我们的方法的有效性和鲁棒性，将基准测试与最先进的编队控制方法进行了比较。我们将我们的工作与周的方法【23】和特平的方法【29】进行了比较。我们在Turpin的工作中实现了编队控制方法，并通过添加我们自己的避障策略使其适应密集环境。然而，与我们在【23】和【29】中的工作不同，飞行期间不允许改变编队的比例和旋转。因此，为了公平地评估性能，提出了一种新的编队飞行总体位置误差指标。
受【30】的启发，为了评估当前编队 $F^c$ 和所需 $F^d$ 之间的总体位置误差 $e_{dist}$ ，我们解决了以下非线性优化问题，以找到将 $F^c$ 与 $F^d$ 对齐的最佳相似性变换（Sim（3）变换）

符号 $p_i^d$ 和 $p_i^c$ 分别表示第i个机器人在 $F^d$ 和 $F^c$ 编队中的位置。Sim（3）变换由旋转R∈ SO（3）组成，翻译t∈ $R^3$ 和一个标度扩展s∈ $R_+$ 。通过优化式子（37）中的变换并应用于地层（formations），可以排除缩放和旋转的影响，以便通过测量有关所需地层的位置误差进行公平评级。误差 $e_{dist}$ 越大， $F^c$ 与所需形状 $F^d$ 的偏差越大。除了位置误差 $e_{dist}$ 外，还对我们在第三节中提出的成功率和编队相似性误差 $e_{sim}$ 进行了比较.

我们模拟了七架以规则六边形编队飞行的无人机，从一个多障碍地图的一侧飞行到另一侧，速度限制为0.5m/s。杂乱区域大小为30×15m，测试了三种障碍密度以进行比较。对参数进行了微调，以获得每种比较方法的最佳性能。

结果总结在表一中。它表明，在存在密集障碍的情况下，周的方法【23】的成功率并不令人满意。在他们的工作中使用的多个相互作用的势场往往会在走廊附近产生局部极小值。因此，无人机常常陷于僵局。Turpin的方法【29】通过为每对代理分配所需的相对位置，然后最小化相对误差来维持编队。然而，这些约束在杂乱的场景中几乎无法满足，这增加了找到可行解决方案的难度，并导致表I中的成功率较低。此外，Turpin的策略侧重于单个跟踪误差，而不是整体编队形状，这可能会导致编队飞行的不利轨迹，如图3所示。

表格1

编队导航方法比较

脚本方法成功率编队位置误差（米）地层相似性误差

图3注：Turpin方法（上排）和我们的方法（下排）之间的基准比较。通过特平的方法，机器人可以走非碰撞路线，这实际上会破坏编队。相比之下，使用我们的方法，机器人会做出更合理的决定来保持队形。各编队位置误差 $e_{dist}$ 如图中所示。

在表1中，我们的方法在位置误差 $e_{dist}$ 和编队相似性误差方面 $e_{sim}$ 取得了更好的性能。此外，即使在复杂的环境中，我们的成功率也很有希望，因为所提出的度量(proposed metric)的缩放和旋转不变性为生成运动平面提供了更大的灵活性。

6 真实世界和模拟实验

A、实施细节

我们的方法与图6所示的自治分布式空中蜂群系统集成。蜂群无人机（swarms）通过广播网络（broadcast network）（广播网络的搭建我不太懂）共享轨迹，广播网络是所有四旋翼机之间的唯一连接。

每个四旋翼无人机都配备Intel RealSense D435立体摄像头（下面链接），用于图像和深度传感。此外，包括状态估计、环境感知、轨迹规划和飞行控制在内的软件模块都在机载计算机Xavier NX4（下面链接）上实时运行。

Depth Camera D435 – Intel® RealSense™ Depth and Tracking Camerashttps://www.intelrealsense.com/depth-camera-d435/https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-xavier-nx/https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-xavier-nx/

在真实世界和模拟实验中，我们通过每秒求解式子（13）次来生成局部轨迹，并以100Hz的频率运行碰撞检查。无约束优化问题由开源库LBFGS-Lite（下面链接）解决。所有仿真均在配备Intel i9-9900K CPU的桌面上实时运行。

https://github.com/ZJU-FAST-Lab/LBFGS-Litehttps://github.com/ZJU-FAST-Lab/LBFGS-Lite

B. 真实世界实验

通过实际实验验证了该方法的可行性和鲁棒性。在第一个实验中，如图1所示，一个由七个四旋翼组成的二维六边形编队成功地穿越了一个障碍物密集的区域，没有发生任何碰撞。该区域放置了12个直径为0.3m的圆柱体障碍物。该测试表明，我们的方法能够在未知复杂环境中维持大规模蜂群的形成。

图1注：正六边形编队的七个四旋翼机群正在穿越未知的障碍物密集区。（a）：编队飞行的快照。（b）：局部地图和执行轨迹的可视化。需要指出的是，在本文中，蓝色线段只表示形状的轮廓，而不表示图形的连通性。

在第二个实验中，如图4所示，三维规则四面体中的四个四旋翼能够安全通过狭窄的走廊。在飞行过程中，蜂群根据环境变化自适应地旋转和压缩编队形状。该测试证明，缩放和旋转不变性为约束空间中的编队飞行提供了更大的灵活性

图4注：穿过走廊的规则四面体的合成快照。蜂群从右向左穿过狭窄的空间。

C、模拟实验

为了验证我们的方法对大规模不规则地层的有效性，我们设计了一个由十个四旋翼组成的心形地层。在模拟中，设置了一个80×20m的杂波区，其中有300个圆柱形障碍物和80个圆形障碍物。如图5所示，蜂群成功地避开了障碍物，并且在飞行过程中保持了理想的编队。

图5注:一个由十个四旋翼组成的大型心形编队从左侧到右侧穿过一个未知的密集环境。快照（a）-（e）在杂乱区域均匀采样。彩色曲线显示编队飞行的执行轨迹

7总结和未来工作

本文提出了一种密集环境下编队飞行的分布式群轨迹优化方法。提出了一种新的度量方法来度量编队相似性，并将其与时空优化框架相结合来生成群轨迹。通过仿真和实际实验，验证了该方法的实用性和有效性。
未来，我们将致力于进一步提高我们方法的稳健性。当一些机器人的通信范围无法预测地缩小时，挑战就出现了。此外，在高度受限的环境中，机器人之间的任务重新分配可能是及时解决死锁所必需的。最后，我们希望为机器人界提供密集环境中编队导航的完整解决方案。

原文参考文献

[1] L. Marconi, C. Melchiorri, M. Beetz, D. Pangercic, R. Siegwart, S. Leutenegger, R. Carloni, S. Stramigioli, H. Bruyninckx, P. Doherty, A. Kleiner, V. Lippiello, A. Finzi, B. Siciliano, A. Sala, and N. Tomatis, “The sherpa project: Smart collaboration between humans and ground-aerial robots for improving rescuing activities in alpine environments,” in 2012 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), 2012, pp. 1–4.
[2] N. Mahdoui, V. Fr ́emont, and E. Natalizio, “Communicating multi-uav system for cooperative slam-based exploration,” Journal of Intelligent & Robotic Systems, vol. 98, no. 2, pp. 325–343, 2020.
[3] K. Dorling, J. Heinrichs, G. G. Messier, and S. Magierowski, “Vehicle routing problems for drone delivery,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 47, no. 1, pp. 70–85, 2016.
[4] A. Jahn, R. J. Alitappeh, D. Salda ̃na, L. C. Pimenta, A. G. Santos, and M. F. Campos, “Distributed multi-robot coordination for dynamic perimeter surveillance in uncertain environments,” in 2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017, pp. 273–278.
[5] K.-K. Oh, M.-C. Park, and H.-S. Ahn, “A survey of multi-agent formation control,” Automatica, vol. 53, pp. 424–440, 2015.
[6] J. Van Den Berg, S. J. Guy, M. Lin, and D. Manocha, “Reciprocal n-body collision avoidance,” in Robotics research. Springer, 2011, pp. 3–19.
[7] J. Van Den Berg, J. Snape, S. J. Guy, and D. Manocha, “Reciprocal collision avoidance with acceleration-velocity obstacles,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 3475–3482.
[8] D. Bareiss and J. Van den Berg, “Reciprocal collision avoidance for robots with linear dynamics using lqr-obstacles,” in 2013 IEEE International Conference on Robotics and Automation. IEEE, 2013, pp. 3847–3853.
[9] S. H. Arul and D. Manocha, “Dcad: Decentralized collision avoidance with dynamics constraints for agile quadrotor swarms,” IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 1191–1198, 2020.
[10] C. E. Luis and A. P. Schoellig, “Trajectory generation for multiagent point-to-point transitions via distributed model predictive control,” IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 375–382, 2019.
[11] J. Park, J. Kim, I. Jang, and H. J. Kim, “Efficient multi-agent trajectory planning with feasibility guarantee using relative bernstein polynomial,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 434–440.
[12] D. Zhou, Z. Wang, S. Bandyopadhyay, and M. Schwager, “Fast, online collision avoidance for dynamic vehicles using buffered voronoi cells,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 1047– 1054, 2017.
[13] Y. Chen, M. Cutler, and J. P. How, “Decoupled multiagent path planning via incremental sequential convex programming,” in 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015, pp. 5954–5961.
[14] T. Baca, D. Hert, G. Loianno, M. Saska, and V. Kumar, “Model predictive trajectory tracking and collision avoidance for reliable outdoor deployment of unmanned aerial vehicles,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 6753–6760.
[15] X. Zhou, Z. Wang, X. Wen, J. Zhu, C. Xu, and F. Gao, “Decentralized spatial-temporal trajectory planning for multicopter swarms,” arXiv preprint arXiv:, 2021.
[16] M. A. Lewis and K.-H. Tan, “High precision formation control of mobile robots using virtual structures,” Autonomous robots, vol. 4, no. 4, pp. 387–403, 1997.
[17] M. C. De Gennaro and A. Jadbabaie, “Formation control for a cooperative multi-agent system using decentralized navigation functions,” in 2006 American Control Conference. IEEE, 2006, pp. 6–pp.
[18] T. Balch and R. C. Arkin, “Behavior-based formation control for multirobot teams,” IEEE transactions on robotics and automation, vol. 14, no. 6, pp. 926–939, 1998.
[19] Z. Lin, W. Ding, G. Yan, C. Yu, and A. Giua, “Leader–follower formation via complex laplacian,” Automatica, vol. 49, no. 6, pp. 1900–1906, 2013.
[20] Z. Han, L. Wang, and Z. Lin, “Local formation control strategies with undetermined and determined formation scales for co-leader vehicle networks,” in 52nd IEEE Conference on Decision and Control. IEEE, 2013, pp. 7339–7344.

[21] S. Zhao, “Affine formation maneuver control of multiagent systems,” IEEE Transactions on Automatic Control, vol. 63, no. 12, pp. 4140–4155, 2018.
[22] J. Alonso-Mora, E. Montijano, M. Schwager, and D. Rus, “Distributed multi-robot formation control among obstacles: A geometric and optimization approach with consensus,” in 2016 IEEE international conference on robotics and automation (ICRA). IEEE, 2016, pp.5356–5363.
[23] D. Zhou, Z. Wang, and M. Schwager, “Agile coordination and assistive collision avoidance for quadrotor swarms using virtual structures,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 916–923, 2018.
[24] R. Van Parys and G. Pipeleers, “Distributed model predictive formation control with inter-vehicle collision avoidance,” in 2017 11th Asian Control Conference (ASCC). IEEE, 2017, pp. 2399–2404.
[25] M. Tantardini, F. Ieva, L. Tajoli, and C. Piccardi, “Comparing methods for comparing networks,” Scientific Reports, vol. 9, 11 2019.
[26] Z. Wang, X. Zhou, C. Xu, and F. Gao, “Geometrically constrained trajectory optimization for multicopters,” arXiv preprint arXiv:, 2021.
[27] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 4, no. 4, pp. 3529–3536, 2019.
[28] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery, Numerical Recipes 3rd Edition: The Art of Scientific Computing, 3rd ed. USA: Cambridge University Press, 2007.
[29] M. Turpin, N. Michael, and V. Kumar, “Trajectory design and control for aggressive formation flight with quadrotors,” Autonomous Robots, vol. 33, no. 1, pp. 143–156, 2012.
[30] P. C. Lusk, X. Cai, S. Wadhwania, A. Paris, K. Fathian, and J. P. How, “A distributed pipeline for scalable, deconflicted formation flying,” IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 5213–5220, 2020