CN114911263B - 基于同伦法的多无人机同步到达轨迹规划方法、存储介质及设备 - Google Patents

Abstract

基于同伦法的多无人机同步到达轨迹规划方法、存储介质及设备，属于多无人机轨迹规划领域。为了解决目前的多无人机轨迹规划或者控制方法不能很好的满足多无人机同步到达的问题。本发明基于架无人机的运动学模型和轨迹角和控制变量遵守约束构建多无人机同步到达问题的模型，并对同步到达问题进行离散化，然后基于无人机的初始位置、初始姿态、目标位置、目标姿态，轨迹角的上、下界，轨迹角的控制变量的上、下界，航向角的控制变量的上界，以及距离步长，经过同步到达规划方法得到同步轨迹并基于同伦法对无人机的轨迹进行具体规划。本发明主要用于多无人机的同步到达轨迹规划。

Description

基于同伦法的多无人机同步到达轨迹规划方法、存储介质及设备

技术领域

本发明涉及多无人机同步到达轨迹规划方法，属于多无人机轨迹规划领域。

背景技术

单无人机能力有限，往往不足以处理真实场景中的各项任务，对此可以利用多无人机系统改进。多无人机系统的效率取决于无人机协同执行各种动作时的动态性能和精度。目前针对多无人机协同行动的控制已经有了许多研究，如监视、包围、交会等。在具有各种复杂任务的大规模场景中，协同行动需要涵盖跨越多个时间点的行动。例如，无人机群在接近目标时应该保持同步，即在接近目标的过程中，所有无人机与相应目标之间的距离保持相等。然而，在大规模场景中，无人机之间难以保证实时、稳定的通信，协同控制性也将受到限制。因此，需要提前规划多无人机同步到达的轨迹。

多无人机轨迹规划算法已经有了深入的研究。无人机的运动学模型是轨迹规划算法的基础。曲率有界轨迹模型作为运动学模型被普遍运用在多无人机轨迹规划上。曲率有界轨迹模型使用曲率有界的曲线来描述无人机的轨迹，无人机的轨迹则由曲线的几何结构决定。

基于圆弧/直线的曲率有界模型可以有效地应用在多无人机的轨迹规划中。1957年，Dubins提出了基于圆弧/直线的连接给定两点的最短曲率有界轨迹。Shkel和Lumelsky提出了基于逻辑分类结构的路径规划的Dubins集。许多轨迹规划问题都基于Dubins理论的早期工作进行了研究。穿越障碍环境的最短轨迹问题在曲率有界的条件下进行了研究。曲率有界的无人机轨迹长度调整方法被研究用于广域搜索和运动目标拦截。通过多个点的Dubins轨迹规划算法也得到了研究。

此外，问题的维度也给多无人机的轨迹规划带来了挑战。随着多无人机应用场景的复杂化，轨迹规划问题的维度也在明显地增加。为此，引入了同伦方法来研究交通工具的轨迹规划。机器人轨迹规划问题的解可以通过参数摄动或启发式搜索进行搜索。同伦方法可以有效地应对由拓扑约束、曲率约束、和不确定环境引起的额外复杂性。将分治策略和球面算法与同伦方法结合，可以处理复杂的轨迹规划问题。

发明内容

本发明是为了解决目前的多无人机轨迹规划或者控制方法不能很好的满足多无人机同步到达的问题。

基于同伦法的多无人机同步到达轨迹规划方法，针对包含N架异构无人机的场景，首先对N架无人机进行同步达到规划，规划的过程包括以下步骤：

多无人机同步到达问题会为每架无人机分配一个目标；对于无人机n，其初始位置表示为(x_on,y_on,z_on)＝P_on；其初始姿态表示为

目标的位置表示为(x_tn,y_tn,z_tn)＝P_tn；目标的姿态表示为

多无人机同步到达问题的模型如下：

d_n(t)＝‖p_n(t)-P_tn‖,   (6)

d_max＝max{{d_n(t)}_t≥0}_n＝1,...,N    (8)

式中d表示无人机与对应目标之间的距离；TOA_dn是到达目标的时间；

p_n(TOA_dn)＝P_tn,   (9)

P(n),n＝1,...,N是从初始位置P_on以初始姿态

开始的所有可航行的轨迹集合；P(1)×...×P(N)是笛卡尔积；

假设1.无人机的运动基于轨迹规划；当忽略轨迹跟踪中的偏差时，无人机的运动学模型由以下微分方程表示：

其中p_pn表示无人机n的规划轨迹；p_pn(t)表示无人机n在时刻t时的规划位置；

假设2.将问题(3)离散化后与问题(4)合并，即

其中d₀,d₁,...,d_K表示从无人机到目标的控制距离；

基于无人机的初始位置、初始姿态、目标位置、目标姿态，轨迹角的上、下界，轨迹角的控制变量的上、下界，航向角的控制变量的上界，以及距离步长，经过同步到达规划方法得到同步轨迹并基于同伦法对无人机的轨迹进行具体规划。

进一步地，无人机的运动学模型如下：

其中(x_n(t),y_n(t),z_n(t))＝p_n(t)代表无人机n的位置坐标；v_n(t)代表t时刻的航行速度；

代表无人机n的航向角；θ_n(t)代表无人机n的轨迹角；

和u_nθ分别代表航向角

和轨迹角θ_n(t)的控制变量。

进一步地，无人机的轨迹角和控制变量遵守约束条件如下：

θ_minn和θ_maxn代表无人机n的轨迹角的上下界；u_θminn和u_θmaxn代表无人机n的轨迹角控制变量u_nθ的上下界；

代表无人机n的航向角控制变量

的上界。

进一步地，基于同伦法对无人机的轨迹进行具体规划的过程包括以下步骤：

步骤1：构造局部同伦结构：

其中

和

是初始状态和结束状态，对应初始位置、初始航向角、初始轨迹角和结束位置、结束航向角、结束轨迹角；u是控制变量，p是一条有特定初始状态和结束状态的可航行的轨迹；

基于回转-直线飞行模型构造轨迹同伦：规划的轨迹由有限数量的弧线/直线段描述；直线段表示无人机沿直线前进，弧线段表示无人机转弯；左转时弧线段的曲率为正；右转时弧线段的曲率为负；曲率必须小于上界：

其中

代表无人机n的航向角控制变量

的上界；

令速度为常数，即v_n(t)＝V_n，

设A₁、A₂分别为弧线/直线段的起点和终点；

和

分别为起点和终点的切线向量；κ和γ分别为线段的曲率和转角，τ为通过线段的航行时间，且

γ＝V_nκτ   (16)

得到如下的转换：

其中：

式中，s和q分别是表示无人机高度和轨迹角的额外变量；

规划出的轨迹的飞行时间通过轨迹的长度进行估计；因此，快速计算轨迹p_pn的飞行时间：

其中L(p_pn,0,t)是轨迹段p_pn从0时刻到t时刻，在水平面上的长度；

基于式(20)中的快速计算方法，在设计同伦结构时只需要考虑轨迹长度；

步骤2：构造洋葱状同伦结构，洋葱状同伦基于局部同伦由以下的递归构造得到：

由于d₀＝0，

为了方便起见，令

基于式(32)、式(33)，设计适用于上述同步轨迹规划中的具体轨迹规划。

进一步地，在步骤1中设计同伦结构时只需要考虑轨迹长度的过程中，为满足轨迹长度的需求，设计四种轨迹同伦结构；四种轨迹同伦结构分别是左绕行同伦、右绕行同伦、左盘旋同伦和右盘旋同伦；

同伦结构基于弧线-直线-弧线的Dubins轨迹，连接起点

和终点

W_A和W_B是Dubins轨迹中直线段的两个端点；γ_A，κ_A，γ_B，κ_B是Dubins轨迹中两条弧线段的转角和曲率。

进一步地，在左右绕行同伦中，一条轨迹包含四条相同的圆弧；轨迹在水平面上的长度为：

其中，γ_d＝πε_du/2，

这里<～,～>表示内积，u是同伦参数，且

当u＝1时l_detour是最大值，而当u＝0时l_detour是最小值；无人机n处于左/右绕行同伦的轨迹被描述为：

其中

s₁，s₂，s₃，s₄和s是表示无人机高度的额外变量；q₁，q₂，q₃，q₄和q是表示无人机轨迹角的额外变量。

进一步地，在左/右盘旋同伦中，一条轨迹包含一个完整的圆；轨迹长度为：

其中，

κ_h＝ε_h(1-u/2)κ_max   (28)

这里u表示同伦参数，且

当u＝1时l_hover是最大值，而当u＝0时l_hover是最小值；

无人机n处于左/右盘旋同伦的轨迹被描述为：

其中

s₁，s₂，s₃和s是表示无人机高度的额外变量；q₁，q₂，q₃和q是表示无人机轨迹角的额外变量。

进一步地，在构造洋葱状同伦结构之后还需要进行轨迹角平滑化处理，具体过程如下：

给定起点

终点

和A、B之间的点C，以广义坐标(l_C,z_C)来描述点C，其中l_C是轨迹的位置，z_C是C点的高度，θ_C是点C处的轨迹角；

根据A、C点间的轨迹段是弧线或直线段，得到式(17)的转化形式，且

s＝z_C-z_A,q＝θ_C   (36)

其中z_A为点A的高度；

采用线性轨迹角平滑策略生成附加变量无人机飞行高度s和轨迹角q；首先，将两个圆连接到A点，针对于A点，以无人机的方向作为两个圆的切线，且两个圆位于切线两侧；以相同的方式将两个圆连接到B点；A点、B点切线上方的圆有着相同的曲率，记作κ_up，切线下方的圆也有着相同的曲率，记作κ_down；κ_up和κ_down由下式计算：

然后，针对四个圆，移除于A点和B点上轨迹角大于θ_maxn或小于θ_minn的圆对应的部分，从而得到四个圆弧，找到被这四个圆弧夹住的最短轨迹，即实现了轨迹角平滑化。

一种存储介质，所述存储介质中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现基于同伦法的多无人机同步到达轨迹规划方法。

一种基于同伦法的多无人机同步到达轨迹规划设备，所述设备包括处理器和存储器，所述存储器中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现基于同伦法的多无人机同步到达轨迹规划方法。

有益效果：

1本发明基于曲率有界轨迹模型，形式化定义了多无人机同步到达问题。对同步到达问题进行离散化，便于迭代求解。本发明设计了同步到达的算法结构，因此本发明可以很好的解决多无人机同步达到的问题。

2针对无人机轨迹解空间的构造问题，提出了基于同伦的轨迹描述方法。针对轨迹规划问题中的性能指标解耦问题，本发明提出了一种新的洋葱状同伦结构，可以很好的解决轨迹规划问题中的性能指标解耦问题。

3基于无人机的绕行模型和盘旋模型设计了局部轨迹同伦结构，并在同伦结构中搜索最优轨迹。本发明提出的一种直线轨迹角平滑策略来生成无人机的飞行轨迹，能够极大地提升轨迹规划的效果。

附图说明

图1为四种局部轨迹同伦结构，其中图1(a)为左/右绕行同伦，图1(b)为左/右盘旋同伦；

图2无人机n的洋葱状同伦结构；

图3为轨迹角θ_n、控制变量

和任务距离d_n(t)的示意图；其中A为轨迹起点，B为轨迹终点，C为轨迹中点，z表示点的高度，θ表示点处的轨迹角。

具体实施方式

具体实施方式一：

本发明主要用于解决多无人机同步到达问题，本实施方式所述的基于同伦法的多无人机同步到达轨迹规划方法，包括同步到达算法、基于同伦的轨迹规划两部分。

(一)同步到达算法：

对存在N架异构无人机的场景，其中第n架无人机的运动学模型如下所示：

其中(x_n(t),y_n(t),z_n(t))＝p_n(t)代表无人机n的位置坐标；v_n(t)代表t时刻的航行速度；

代表无人机n的航向角；θ_n(t)代表无人机n的轨迹角；

和u_nθ分别代表航向角

和轨迹角θ_n(t)的控制变量。

轨迹角和控制变量遵守约束条件如下：

θ_minn和θ_maxn代表无人机n的轨迹角的上下界；u_θminn和u_θmaxn代表控制变量u_nθ的上下界；

代表控制变量

的上界。

多无人机同步到达问题会为每架无人机分配一个目标。对于无人机n，其初始位置表示为(x_on,y_on,z_on)＝P_on；其初始姿态表示为

目标的位置表示为(x_tn,y_tn,z_tn)＝P_tn；目标的姿态表示为

同步接近问题有如下两个目标：

(ⅰ)无人机群预计将同时到达它们的目标；

(ⅱ)在无人机群到达目标之前，不同无人机与其各自对应的目标之间的距离始终保持相同。

多无人机同步到达问题的模型如下：

其中，

d_n(t)＝‖p_n(t)-P_tn‖,   (6)

d_max＝max{{d_n(t)}_t≥0}_n＝1,...,N   (8)

式中d表示无人机与对应目标之间的距离；

TOA_dn是到达目标的时间，有

p_n(TOA_dn)＝P_tn,   (9)

P(n),n＝1,...,N是在运动学模型(1)和(2)下，从初始位置P_on以初始姿态

开始的所有可航行的轨迹集合。P(1)×...×P(N)是笛卡尔积。

为求解同步到达问题，做出以下假设：

假设1.无人机的运动基于轨迹规划。当忽略轨迹跟踪中的偏差时，其运动学模型可由以下微分方程近似表示：

其中p_pn表示无人机n的规划轨迹。p_pn(t)表示无人机n在时刻t时的规划位置。

假设2.将问题(3)离散化后与问题(4)合并，即，

其中d₀,d₁,...,d_K表示从无人机到目标的控制距离。

假设1确保了无人机群的运动仅与轨迹规划相关。假设2将问题(3)的目标从无限维改为了有限维，使得问题(3)变为可解。

然后获取初始位置P_o1,...,P_oN，初始姿态

和θ_o1,...,θ_oN，目标位置P_t1,...,P_tN，目标姿态

和θ_t1,...,θ_tN，轨迹角的上、下界θ_min1,...,θ_minN和θ_max1,...,θ_maxN，轨迹角的控制变量的上、下界u_θmin1,...,u_θminN和u_θmax1,...,u_θmaxN，航向角的控制变量的上界

距离步长d_step，经过同步到达规划方法，即可得到同步轨迹p_p1,...,p_pN。

(二)基于同伦的轨迹规划

给出一个多元同伦函数

P_on和P_tn分别是初始位置和结束位置。

和

分别是初始航向角和结束航向角。θ_on和θ_tn分别是初始轨迹角和结束轨迹角。u₁,...,u_K是控制变量。对任意参数输入，输出为一条可航行的轨迹，即

输入参数

描述了基于同伦的轨迹。轨迹同伦是轨迹规划问题的解空间，为访问可航行轨迹集提供了一种简便的方法。同伦中的轨迹被视为解空间中的一个点，基于同伦的描述是描述该轨迹的坐标。在基于同伦的描述中，

是无人机n的初始状态和结束状态；u₁,...,u_K控制轨迹的性能。

步骤1：构造局部同伦结构：

其中

和

是初始状态和结束状态。对于任意的u∈[0,1]，p是一条有着特定初始状态和结束状态的可航行的轨迹。

轨迹同伦基于回转-直线飞行模型构造。规划的轨迹由有限数量的弧线/直线段描述。直线段表示无人机沿直线前进，弧线段表示无人机转弯。左转时弧线段的曲率为正；右转时弧线段的曲率为负。曲率必须小于上界：

利用该模型，假设速度为常数，即v_n(t)＝V_n，

设A₁、A₂分别为弧线/直线段的起点和终点。

和

分别为起点和终点的切线向量。κ和γ分别为线段的曲率和转角，τ为通过线段的航行时间，且

γ＝V_nκτ   (16)

κ＝0表示直线段。

可以得到如下的转换：

其中：

式中，s和q分别是表示无人机高度和轨迹角的额外变量。

规划出的轨迹的飞行时间可以通过轨迹的长度进行估计。因此，快速计算轨迹p_pn的飞行时间的方法如下所示：

其中L(p_pn,0,t)是轨迹段p_pn从0时刻到t时刻，在水平面上的长度。基于式(20)中的快速计算方法，在设计同伦结构时只需要考虑轨迹长度。

为满足轨迹长度的需求，设计了四种轨迹同伦结构，在实际每段路径的规划中，使用其中一种同伦结构即可。如图1所示，四种轨迹同伦结构分别是左绕行同伦、右绕行同伦、左盘旋同伦和右盘旋同伦。

同伦结构基于弧线-直线-弧线的Dubins轨迹，连接起点

和终点

W_A和W_B是Dubins轨迹中直线段的两个端点。γ_A，κ_A，γ_B，κ_B是Dubins轨迹中两条弧线段的转角和曲率。

在左右绕行同伦中，一条轨迹包含四条相同的圆弧。轨迹在水平面上的长度为：

其中，γ_d＝πε_du/2，

这里<～,～>表示内积，u是同伦参数，且

当u＝1时l_detour是最大值，而当u＝0时l_detour是最小值。以无人机n为例，处于左/右绕行同伦的轨迹可被描述为：

其中

s₁，s₂，s₃，s₄和s是表示无人机高度的额外变量。q₁，q₂，q₃，q₄和q是表示无人机轨迹角的额外变量。

在左/右盘旋同伦中，一条轨迹包含一个完整的圆。轨迹长度为：

其中，

κ_h＝ε_h(1-u/2)κ_max (28)

这里u表示同伦参数，且

当u＝1时l_hover是最大值，而当u＝0时l_hover是最小值。

以无人机n为例，处于左/右盘旋同伦的轨迹可被描述为：

其中

s₁，s₂，s₃和s是表示无人机高度的额外变量。q₁，q₂，q₃和q是表示无人机轨迹角的额外变量。

步骤2：构造洋葱状同伦结构：

如图2所示，图2为无人机n的洋葱状同伦结构，其中

为局部同伦结构的递归过程；洋葱状同伦基于局部同伦由以下的递归构造得到：

由于d₀＝0，

为了方便起见，令

基于式(32)、式(33)，设计适用于上述同步轨迹规划中的具体轨迹规划，实际上本发明的该部分(基于同伦的轨迹规划)也可以单独用于轨迹规划。

实际上经过上述过程已经能够实现多无人机的同步轨迹规划，为了提升规划的轨迹效果，还可以在上述规划的轨迹上进一步进行轨迹角平滑化；

步骤3：轨迹角平滑化

给定起点

终点

和A、B之间的点C，以广义坐标(l_C,z_C)来描述点C，其中l_C是轨迹的位置，z_C是C点的高度，θ_C是点C处的轨迹角。

根据A、C点间的轨迹段是弧线或直线段，得到式(17)的转化形式(具体形式)，且

s＝z_C-z_A,q＝θ_C    (36)

其中z_A为点A的高度。

采用线性轨迹角平滑策略生成附加变量无人机飞行高度s和轨迹角q。首先，将两个圆连接到A点，针对于A点，以无人机的方向作为两个圆的切线，且两个圆位于切线两侧，即相对于切线，一个圆朝上，一个圆朝下。类似地，再将两个圆连接到B点。朝上的圆有着相同的曲率，记作κ_up，朝下的圆也有着相同的曲率，记作κ_down。κ_up和κ_down由下式计算：

然后，针对四个圆，移除于A点和B点上轨迹角大于θ_maxn或小于θ_minn的圆对应的部分，这样就能得到如图3所示的四个圆弧，找到被这四个圆弧夹住的最短轨迹，θ_C和z_C分别为轨迹上点C的角度和高度。图3为轨迹角θ_n、控制变量

和任务距离d_n(t)的示意图；其中A为轨迹起点，B为轨迹终点，C为轨迹中点，z表示点的高度，θ表示点处的轨迹角。

实施例1：

本实施例为同步到达规划规划：

步骤1.1：获取初始位置P_o1,...,P_oN，初始姿态

和θ_o1,...,θ_oN，目标位置P_t1,...,P_tN，目标姿态

和θ_t1,...,θ_tN，轨迹角的上、下界θ_min1,...,θ_minN和θ_max1,...,θ_maxN，轨迹角的控制变量的上、下界u_θmin1,...,u_θminN和u_θmax1,...,u_θmaxN，航向角的控制变量的上界

距离步长d_step；

步骤1.2：获取最短轨迹p_p1,...,p_pN；

步骤1.3：计算无人机到目标点的最大距离d_max＝max{{d_n(t)}_t≥0}_n＝1,...,N；

步骤1.4：将最大距离d_max除以距离步长后向下取整得到

步骤1.5：将无人机轨迹分为K段，从K到0依次执行步骤1.6～1.8；

步骤1.6：计算第k段无人机距目标点最大距离，即控制距离d_k＝k·d_step；

步骤1.7：对每架无人机计算令无人机到达距离目标d_k处的时间

并得到无人机群到达距离目标d_k处的最长时间t_m(d_k)＝max{t_d1(d_k),...,t_dN(d_k)}；

步骤1.8：对每架无人机，基于t_m(d_k)利用轨迹规划算法修正p_pn；

步骤1.9：得到同步轨迹p_p1,...,p_pN作为输出。

实施例2：

本实施例为对无人机n的基于同伦的轨迹规划方法。

步骤2.1：获取初始轨迹p_pn，控制距离下标k，控制距离d_k，到达距离的时间t_m(d_k)，初始位置P_n(k+1)，初始姿态

和θ_n(k+1)，目标位置P_tn，目标姿态

和θ_tn，轨迹角的上下界θ_minn和θ_maxn，轨迹角的控制变量的上下界u_θminn和u_θmaxn，航向角的控制变量的上界

步骤2.2：根据无人机群到达距离目标d_k处的最长时间t_m(d_k)从图1中的四种同伦结构中选取构造一种轨迹同伦H_Pn(k+1)(u)；

步骤2.3：获取令无人机到达距离目标d_k处的时间与无人机群到达距离目标d_k处的最长时间差值最小的控制变量

步骤2.4：由轨迹同伦得到一条可航行的待优化轨迹

步骤2.5：计算令待优化轨迹到达目标距离d_k处的最小时间

步骤2.6：得到新的轨迹

并将点P_n(k+1)后的轨迹线段p_pn替换为

步骤2.7：输出p_pn，t_Δn,k，P_nk。

具体实施方式二：

本实施方式为一种存储介质，所述存储介质中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现所述的基于同伦法的多无人机同步到达轨迹规划方法。

应当理解，包括本发明描述的任何方法对应的可以被提供为计算机程序产品、软件或计算机化方法，其可以包括其上存储有指令的非暂时性机器可读介质，所述指令可以用于编程计算机系统，或其他电子装置。存储介质可以包括但不限于磁存储介质，光存储介质；磁光存储介质包括：只读存储器ROM、随机存取存储器RAM、可擦除可编程存储器(例如，EPROM和EEPROM)以及闪存层；或者适合于存储电子指令的其他类型的介质。

具体实施方式三：

本实施方式为基于同伦法的多无人机同步到达轨迹规划设备，所述设备包括处理器和存储器，应当理解，包括本发明描述的任何包括处理器和存储器的设备，设备还可以包括其他通过信号或指令进行显示、交互、处理、控制等以及其他功能的单元、模块；

所述存储器中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现所述的基于同伦法的多无人机同步到达轨迹规划方法。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (3)

1.基于同伦法的多无人机同步到达轨迹规划方法，其特征在于，针对包含N架异构无人机的场景，首先对N架无人机进行同步达到规划，规划的过程包括以下步骤：

多无人机同步到达问题会为每架无人机分配一个目标；对于无人机n，其初始位置表示为(x_on,y_on,z_on)＝P_on；其初始姿态表示为

目标的位置表示为(x_tn,y_tn,z_tn)＝P_tn；目标的姿态表示为

多无人机同步到达问题的模型如下：

d_n(t)＝‖p_n(t)-P_tn‖, (6)

d_max＝max{{d_n(t)}_t≥0}_n＝1,...,N (8)

式中d表示无人机与对应目标之间的距离；TOA_dn是到达目标的时间；

p_n(TOA_dn)＝P_tn, (9)

P(n)是从初始位置P_on以初始姿态

开始的所有可航行的轨迹集合,n＝1,...,N；P(1)×...×P(N)是笛卡尔积；

假设1.无人机的运动基于轨迹规划；当忽略轨迹跟踪中的偏差时，无人机的运动学模型由以下微分方程表示：

其中p_pn表示无人机n的规划轨迹；p_pn(t)表示无人机n在时刻t时的规划位置；

假设2.将问题(3)离散化后与问题(4)合并，即

其中d₀,d₁,...,d_K表示从无人机到目标的控制距离；

基于无人机的初始位置、初始姿态、目标位置、目标姿态，轨迹角的上、下界，轨迹角的控制变量的上、下界，航向角的控制变量的上界，以及距离步长，经过同步到达规划方法得到同步轨迹并基于同伦法对无人机的轨迹进行具体规划；

无人机的运动学模型如下：

其中(x_n(t),y_n(t),z_n(t))＝p_n(t)代表无人机n的位置坐标；v_n(t)代表t时刻的航行速度；

代表无人机n的航向角；θ_n(t)代表无人机n的轨迹角；

和u_θn分别代表航向角

和轨迹角θ_n(t)的控制变量；

无人机的轨迹角和控制变量遵守约束条件如下：

θ_minn和θ_maxn代表无人机n的轨迹角的上下界；u_θminn和u_θmaxn代表无人机n的轨迹角控制变量u_θn的上下界；

代表无人机n的航向角控制变量

的上界；

基于同伦法对无人机的轨迹进行具体规划的过程包括以下步骤：

步骤1：构造局部同伦结构：

其中A,

θ_A和B,

θ_B是初始状态和结束状态，对应初始位置、初始航向角、初始轨迹角和结束位置、结束航向角、结束轨迹角；u是控制变量，p是一条有特定初始状态和结束状态的可航行的轨迹；

基于回转-直线飞行模型构造轨迹同伦：规划的轨迹由有限数量的弧线/直线段描述；直线段表示无人机沿直线前进，弧线段表示无人机转弯；左转时弧线段的曲率为正；右转时弧线段的曲率为负；曲率必须小于上界：

其中

代表无人机n的航向角控制变量

的上界；

令速度为常数，即v_n(t)＝V_n，

设A₁、A₂分别为弧线/直线段的起点和终点；

和

分别为起点和终点的切线向量；κ和γ分别为线段的曲率和转角，τ为通过线段的航行时间，且

γ＝V_nκτ (16)

得到如下的转换：

其中：

式中，s和q分别是表示无人机高度和轨迹角的额外变量；

规划出的轨迹的飞行时间通过轨迹的长度进行估计；因此，快速计算轨迹p_pn的飞行时间：

其中L(p_pn,0,t)是轨迹段p_pn从0时刻到t时刻，在水平面上的长度；

基于式(20)中的快速计算方法，在设计同伦结构时只需要考虑轨迹长度；

在步骤1中设计同伦结构时只需要考虑轨迹长度的过程中，为满足轨迹长度的需求，设计四种轨迹同伦结构；四种轨迹同伦结构分别是左绕行同伦、右绕行同伦、左盘旋同伦和右盘旋同伦；

同伦结构基于弧线-直线-弧线的Dubins轨迹，连接起点

和终点

W_A和W_B是Dubins轨迹中直线段的两个端点；γ_A，κ_A，γ_B，κ_B是Dubins轨迹中两条弧线段的转角和曲率；

在左右绕行同伦中，一条轨迹包含四条相同的圆弧；轨迹在水平面上的长度为：

其中，γ_d＝πε_du/2，

这里<～,～>表示内积，u是同伦参数，且

当u＝1时l_detour是最大值，而当u＝0时l_detour是最小值；

无人机n处于左/右绕行同伦的轨迹被描述为：

其中

s₁，s₂，s₃，s₄和s是表示无人机高度的额外变量；q₁，q₂，q₃，q₄和q是表示无人机轨迹角的额外变量；

在左/右盘旋同伦中，一条轨迹包含一个完整的圆；轨迹长度为：

其中，

κ_h＝ε_h(1-u/2)κ_max (28)

这里u表示同伦参数，且

当u＝1时l_hover是最大值，而当u＝0时l_hover是最小值；

无人机n处于左/右盘旋同伦的轨迹被描述为：

其中

s₁，s₂，s₃和s是表示无人机高度的额外变量；q₁，q₂，q₃和q是表示无人机轨迹角的额外变量；

步骤2：构造洋葱状同伦结构，洋葱状同伦基于局部同伦由以下的递归构造得到：

k＝K,...,0,

由于d₀＝0，

为了方便起见，令

基于式(32)、式(33)，设计适用于上述同步轨迹规划中的具体轨迹规划；

在构造洋葱状同伦结构之后还需要进行轨迹角平滑化处理，具体过程如下：

给定起点

终点

和A、B之间的点C，以广义坐标(l_C,z_C)来描述点C，其中l_C是轨迹的位置，z_C是C点的高度，θ_C是点C处的轨迹角；

根据A、C点间的轨迹段是弧线或直线段，得到式(17)的转化形式，且

s＝z_C-z_A,q＝θ_C   (36)

其中z_A为点A的高度；

采用线性轨迹角平滑策略生成附加变量无人机飞行高度s和轨迹角q；首先，将两个圆连接到A点，针对于A点，以无人机的方向作为两个圆的切线，且两个圆位于切线两侧；以相同的方式将两个圆连接到B点；A点、B点切线上方的圆有着相同的曲率，记作κ_up，切线下方的圆也有着相同的曲率，记作κ_down；κ_up和κ_down由下式计算：

然后，针对四个圆，移除于A点和B点上轨迹角大于θ_maxn或小于θ_minn的圆对应的部分，从而得到四个圆弧，找到被这四个圆弧夹住的最短轨迹，即实现了轨迹角平滑化。

2.一种存储介质，其特征在于，所述存储介质中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现如权利要求1所述的基于同伦法的多无人机同步到达轨迹规划方法。

3.一种基于同伦法的多无人机同步到达轨迹规划设备，其特征在于，所述设备包括处理器和存储器，所述存储器中存储有至少一条指令，所述至少一条指令由处理器加载并执行以实现如权利要求1所述的基于同伦法的多无人机同步到达轨迹规划方法。

Images