一种固定翼无人机编队的热启动轨迹规划方法技术

本发明专利技术提供一种基于最优控制与人工势场法的热启动轨迹规划方法，属于无人系统的三维轨迹规划领域。该规划方法针对传统的简单模型在轨迹规划时存在的不足，结合最优控制方法在求解具有复杂运动学约束的非线性系统的优势，设计高质量的初始化求解策略。本发明专利技术首先设计了一种改进的人工势场方法，以消除局部振荡、实现队内协同和满足物理约束，得到高质量的同伦初解。然后将无人机编队的轨迹规划问题重构为标准的最优控制问题，再将最优控制问题离散化并转化为非线性规划。最后带入求得的初解进行轨迹规划。并将所提的规划器应用于固定翼无人机编队的轨迹规划问题中，所提出的方法是有效和鲁棒的，其中所设计的热启动策略对收敛有很大贡献。很大贡献。很大贡献。

【技术实现步骤摘要】
一种固定翼无人机编队的热启动轨迹规划方法


[0001]本专利技术属于无人机系统的三维轨迹规划领域，涉及一种固定翼无人机编队热启动轨迹规划方法，特别是涉及一种基于最优控制与人工势场法的固定翼无人机编队热启动轨迹规划方法。

技术介绍

[0002]随着控制工程和传感器技术的快速发展，无人机开始在各种场景中发挥越来越重要的作用，如快递投放、无线通信支持和灾难救援。由于载荷限制，单架无人机很难携带足够的燃料和有效物资来完成任务。因此，目前的应用趋势是将多架无人机作为一个编队，以加强任务执行能力。在现实世界中，三维空间中有许多不规则的障碍物(如建筑物、山丘、树木)，这些障碍物会阻碍无人机的飞行。如何保持或改变队形对无人机编队的运动控制提出了很高的要求。与旋翼无人机相比，固定翼无人机由于其更复杂的运动学方程而更难控制。固定翼无人机编队的运动规划是在环境中找到从初始状态到预设的终端状态的可行路径，无人机需要满足其运动方程，实现避障功能，保持或改变其队形，并满足一些其他的约束。
[0003]传统的无人机运动规划方法可分为基于几何的方法、智能优化算法和最优控制方法。基于几何的方法通过建立无人机与环境之间的几何关系生成可行路径。这类方法通常将无人机视为质点，但固定翼无人机本质上是具有复杂运动约束的非线性系统。因此，基于几何的方法更适用于旋翼无人机，固定翼无人机很难成功跟踪此类方法规划的路径。智能优化算法是一种受自然现象启方的规划方法。然而，此类方法的固有随机性在进化过程中会产生大量不可行解。虽然这些不可行解可以在迭代进化过程中调整，但它们带来了很高的计算负担。这些缺点阻碍了它们在效率要求高的场景中应用。
[0004]最优控制方法给无人机的路径提供了时间信息，即轨迹。此类方法驱动无人机到达目标状态，同时满足运动的机动性和避障需求。数学上，轨迹规划可以建模为具有约束的最优控制问题。由于固定翼无人机的非线性运动方程和复杂的空间约束，轨迹规划问题将转化为一个大规模的非线性问题，该问题不能直接用软件包求解。一般地，可以通过设计问题框架和寻找高质量的初始猜测两方面进行研究。解空间本质上是严重非凸的，这类问题严重依赖于高质量的初始化策略。尽管对初始猜测的研究在无人机编队中的应用较少，但它们在自动驾驶和移动机器人等领域取得了许多丰硕成果。如果能够提供高质量的初始猜测，会有助于无人机编队的轨迹规划。高效规划固定翼无人机编队的轨迹对于快递调度、无人战争等军民领域均有重要的意义。
[0005]本专利技术由国家自然基金项目(12102077,12161076)和中央高校基本科研业务费(DUT22RC(3)010，DUT22LAB305，DUT22QN223)资助。

技术实现思路

[0006]本专利技术的目的在于克服基于几何的方法、智能优化算法的不足，提供一种基于最优控制方法和人工势场法的热启动轨迹规划方法，可实现固定翼无人机编队的轨迹规划。
[0007]本专利技术解决上述问题的技术方案如下：
[0008]一种固定翼无人机编队热启动轨迹规划方法基于最优控制与人工势场法实现，包括以下步骤：
[0009]步骤1：根据实际任务场景确定固定翼无人机规模N，第i架固定翼无人机的初始位置坐标任务目标位置坐标(x
goal
,y
goal
,z
goal
)，场景有M个障碍物O＝{O1,O2,
…
,O
m
,
…
,O
M
}的信息；
[0010]步骤2：真实的障碍物都是不规则物体，本方法将其简化为圆柱体和半球两类。当障碍物O
m
是圆柱时，圆柱水平圆的圆心和半径和当障碍物O
m
是半球时，半球的球心和半径分别是和
[0011]步骤3：预设人工势场法的参数，所述的参数包括引力因子K
att
，斥力因子K
rep
，内部力因子K
int
，斥力范围d0，斥力场相对目标点的衰减因子n，振荡角度阈值为θ；
[0012]步骤4：由改进的人工势场法计算编队轨迹的初始猜测；具体步骤如下：
[0013]4.1：计算t时刻第i架固定翼无人机的引力场U
att
‑
i
(t)和相应的吸引力F
att
‑
i
(t)：
[0014][0015][0016]其中，d
g
是将引力场分段的距离阈值；对第i架固定翼无人机，d
V
(t)表示在t时刻其与目标的欧氏距离，V表示目标点的位置(x
goal
,y
goal
,z
goal
)，P
i
表示其在t时刻的位置(x
i
(t),y
i
(t),z
i
(t))，表示由P
i
指向V的方向；
[0017]4.2：计算t时刻第i架固定翼无人机的斥力场和相应的排斥力F
rep
‑
i
(t)：
[0018][0019][0020]其中，表示在t时刻第i架固定翼无人机与障碍物O
m
间的欧氏距离，W
m
表示障碍物O
m
的位置；的位置；的位置；表示由W
m
指向P
i
的方向；
[0021]4.3：为防止固定翼无人机之间的碰撞，设计无人机间的内部力以改进人工势场法。当第i架无人机和第j架无人机之间的欧氏距离小于设定值时，表示对第i架无人机的排斥力；当它们之间的欧氏距离大于设定值时；表示对第i架固定翼无人机的吸引力，其计算方法如下：
[0022][0023]其中，对于第i架与第j架固定翼无人机，表示其在t时刻的欧氏距离；表示其间的期望距离；ρ0是固定翼无人机间的距离阈值，表示由P
i
指向P
j
的方向；
[0024]因此，第i架固定翼无人机在t时刻受到的合力F
res
‑
i
(t)：
[0025][0026]4.4：为消除固定翼无人机通过多障碍物区域时可能出现的局部振荡，将相邻两个时刻的合力融合在一起，以改善局部振荡。第i架固定翼无人机在t时刻实际受到的合力F
prac
‑
i
(t)如下：
[0027][0028][0029]其中，λ1和λ2是权重因子，满足λ1+λ2＝1；Δθ是合力F
res
‑
i
(t)与合力F
res
‑
i
(t
‑
1)间的夹角；
[0030]4.4：计算理想状态下第i架固定翼无人机在(t+1)时刻的航向角χ
i
(t+1)：
[0031][0032][0033]其中，ε＝Δt*ω
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种固定翼无人机编队的热启动轨迹规划方法，其特征在于，包括以下步骤：步骤1：根据实际任务场景确定固定翼无人机规模N，第i架固定翼无人机的初始位置坐标任务目标位置坐标(x
goal
，y
goal
，z
goal
)，场景有M个障碍物O＝{O1，O2，
…
，O
m
，
…
，O
M
}的信息；步骤2：将真实障碍物简化为圆柱体和半球两类；当障碍物O
m
是圆柱时，圆柱水平圆的圆心和半径和当障碍物O
m
是半球时，半球的球心和半径分别是和步骤3：预设人工势场法的参数；步骤4：由改进的人工势场法计算编队轨迹的初始猜测；步骤5：将固定翼无人机编队的轨迹规划问题重构为最优控制问题；步骤6：将固定翼无人机编队约束惩罚到性能指标函数，并构造增广目标函数其中b是松弛因子；步骤7：将步骤5的最优控制问题(22)转化为如下问题：步骤8：为数值求解问题，将其在时域[0，t
f
]上离散化，将问题(9)转换为非线性规划问题；定义(N
fe
+1)个采样时刻{t
p
|p＝0，1，
…
，N
fe
}，要求所述采样时刻均匀分布在时域[0，t
f
]上，即t
p
‑
t
p
‑1＝(t
f
‑
0)/N
fe
＝q，p＝1，2，
…
，N
fe
，其中q是在t
p
‑1和t
p
之间的等距时间间隔；然后引入变量{s
p
|p＝0，1，
…
，N
fe
}和{u
p
|p＝0，1，
…
，N
fe
}来分别表示s(t)和u(t)；步骤9：采用四阶龙格
‑
库塔方法在区间[t
p
，t
p+1
]上离散化运动方程：其中，k1＝f(s
p
，u
p
，t
p
)，k4＝f(s
p
+k3*q，u
p
，t
p
+q)；k1是编队的运动学方程在时间t
p
处以s
p
为状态，以u
p
为控制得到的解；k2是编队的运动学方程在时间处以为状态，以u
p
为控制得到的解；k3是编队的运动学方程在时间处以为状态，以u
p
为控制得到的解；k4是编队的运动学方程在时间t
p
+q处以s
p
+k3*q为状态，以u
p
为控制得到的解；步骤10：在所有子区间[t
p
，t
p+1
]重复步骤9，即可形成一个非线性规划问题：
其中，G(s
p
，u
p
，t
f
)表示边界条件和物理约束的集合；公式(25)所示的非线性规划问题的解也是连续时间最优控制问题(23)的数值近似解；步骤11：将步骤4得到的高质量的初始猜测和步骤10得到的问题列式(25)输入非线性规划求解器中，进行无人机编队的轨迹规划，得到编队飞行轨迹。2.根据权利要求1所述的一种固定翼无人机编队的热启动轨迹规划方法，其特征在于，所述步骤4具体如下：4.1：计算t时刻第i架固定翼无人机的引力场U
att
‑
i
(t)和相应的吸引力F
att
‑
i
(t)：(t)：其中，d
g
是将引力场分段的距离阈值；对第i架固定翼无人机，d
V
(t)表示在t时刻其与目标的欧氏距离，V表示目标点的位置(x
goal
，y
goal
，z
goal
)，P
i
表示其在t时刻的位置(x
i
(t)，y
i
(t)，z
i
(t))，表示由P
i
指向V的方向；4.2：计算t时刻第i架固定翼无人机的斥力场和相应的排斥力F
rep
‑
i
(t)：(t)：其中，表示在t时刻第i架固定翼无人机与障碍物O
m
间的欧氏距离，W
m
表示障碍物O
m
的位置；的位置；的位置；表示由W
m
指向P
i
的方向；4.3：为防止固定翼无人机之间的碰撞，设计无人机间的内部力以改进人工势场法；当第i架无人机和第j架无人机之间的欧氏距离小于设定值时，表示对第i架无人机的排斥力；当它们之间的欧氏距离大于设定值时；表示对第i架固定翼无人机的吸引力，其计算方法如下：
其中，对于第i架与第j架固定翼无人机，表示其在t时刻的欧氏距离；表示其间的期望距离；ρ0是固定翼无人机间的距离阈值，表示由P
i
指向P
j
的方向；因此，第i架固定翼无人机在t时刻受到的合力F
res
‑
i
(t)：4.4：为消除固定翼无人机通过多障碍物区域时可能出现的局部振荡，将相邻两个时刻的合力融合在一起，以改善局部振荡；第i架固定翼无人机在t时刻实际受到的合力F
prac
‑
i
(t)如下：(t)如下：其中，λ1和λ2是权重因子，满足λ1+λ2＝1；Δθ是合力F
res
‑
i
(t)与合力F
res
‑
i
(t
‑
1)间的夹角；4.4：计算理想状...

【专利技术属性】
技术研发人员：王昕炜，王磊，李昕，易雪玲，邓芝龙，高晓华，张盛，
申请(专利权)人：大连理工大学，
类型：发明
国别省市：