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

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

背景技术：

1、随着控制工程和传感器技术的快速发展，无人机开始在各种场景中发挥越来越重要的作用，如快递投放、无线通信支持和灾难救援。由于载荷限制，单架无人机很难携带足够的燃料和有效物资来完成任务。因此，目前的应用趋势是将多架无人机作为一个编队，以加强任务执行能力。在现实世界中，三维空间中有许多不规则的障碍物(如建筑物、山丘、树木)，这些障碍物会阻碍无人机的飞行。如何保持或改变队形对无人机编队的运动控制提出了很高的要求。与旋翼无人机相比，固定翼无人机由于其更复杂的运动学方程而更难控制。固定翼无人机编队的运动规划是在环境中找到从初始状态到预设的终端状态的可行路径，无人机需要满足其运动方程，实现避障功能，保持或改变其队形，并满足一些其他的约束。

2、传统的无人机运动规划方法可分为基于几何的方法、智能优化算法和最优控制方法。基于几何的方法通过建立无人机与环境之间的几何关系生成可行路径。这类方法通常将无人机视为质点，但固定翼无人机本质上是具有复杂运动约束的非线性系统。因此，基于几何的方法更适用于旋翼无人机，固定翼无人机很难成功跟踪此类方法规划的路径。智能优化算法是一种受自然现象启方的规划方法。然而，此类方法的固有随机性在进化过程中会产生大量不可行解。虽然这些不可行解可以在迭代进化过程中调整，但它们带来了很高的计算负担。这些缺点阻碍了它们在效率要求高的场景中应用。

3、最优控制方法给无人机的路径提供了时间信息，即轨迹。此类方法驱动无人机到达目标状态，同时满足运动的机动性和避障需求。数学上，轨迹规划可以建模为具有约束的最优控制问题。由于固定翼无人机的非线性运动方程和复杂的空间约束，轨迹规划问题将转化为一个大规模的非线性问题，该问题不能直接用软件包求解。一般地，可以通过设计问题框架和寻找高质量的初始猜测两方面进行研究。解空间本质上是严重非凸的，这类问题严重依赖于高质量的初始化策略。尽管对初始猜测的研究在无人机编队中的应用较少，但它们在自动驾驶和移动机器人等领域取得了许多丰硕成果。如果能够提供高质量的初始猜测，会有助于无人机编队的轨迹规划。高效规划固定翼无人机编队的轨迹对于快递调度、无人战争等军民领域均有重要的意义。

4、本发明由国家自然基金项目(12102077,12161076)和中央高校基本科研业务费(dut22rc(3)010，dut22lab305，dut22qn223)资助。

技术实现思路

1、本发明的目的在于克服基于几何的方法、智能优化算法的不足，提供一种基于最优控制方法和人工势场法的热启动轨迹规划方法，可实现固定翼无人机编队的轨迹规划。

2、本发明解决上述问题的技术方案如下：

3、一种固定翼无人机编队热启动轨迹规划方法基于最优控制与人工势场法实现，包括以下步骤：

4、步骤1：根据实际任务场景确定固定翼无人机规模n，第i架固定翼无人机的初始位置坐标任务目标位置坐标(xgoal,ygoal,zgoal)，场景有m个障碍物o＝{o1,o2,…,om,…,om}的信息；

5、步骤2：真实的障碍物都是不规则物体，本方法将其简化为圆柱体和半球两类。当障碍物om是圆柱时，圆柱水平圆的圆心和半径和当障碍物om是半球时，半球的球心和半径分别是和

6、步骤3：预设人工势场法的参数，所述的参数包括引力因子katt，斥力因子krep，内部力因子kint，斥力范围d0，斥力场相对目标点的衰减因子n，振荡角度阈值为θ；

7、步骤4：由改进的人工势场法计算编队轨迹的初始猜测；具体步骤如下：

8、4.1：计算t时刻第i架固定翼无人机的引力场uatt-i(t)和相应的吸引力fatt-i(t)：

9、

10、

11、其中，dg是将引力场分段的距离阈值；对第i架固定翼无人机，dv(t)表示在t时刻其与目标的欧氏距离，v表示目标点的位置(xgoal,ygoal,zgoal)，pi表示其在t时刻的位置(xi(t),yi(t),zi(t))，表示由pi指向v的方向；

12、4.2：计算t时刻第i架固定翼无人机的斥力场和相应的排斥力frep-i(t)：

13、

14、

15、其中，表示在t时刻第i架固定翼无人机与障碍物om间的欧氏距离，wm表示障碍物om的位置；表示由wm指向pi的方向；

16、4.3：为防止固定翼无人机之间的碰撞，设计无人机间的内部力以改进人工势场法。当第i架无人机和第j架无人机之间的欧氏距离小于设定值时，表示对第i架无人机的排斥力；当它们之间的欧氏距离大于设定值时；表示对第i架固定翼无人机的吸引力，其计算方法如下：

17、

18、其中，对于第i架与第j架固定翼无人机，表示其在t时刻的欧氏距离；表示其间的期望距离；ρ0是固定翼无人机间的距离阈值，表示由pi指向pj的方向；

19、因此，第i架固定翼无人机在t时刻受到的合力fres-i(t)：

20、

21、4.4：为消除固定翼无人机通过多障碍物区域时可能出现的局部振荡，将相邻两个时刻的合力融合在一起，以改善局部振荡。第i架固定翼无人机在t时刻实际受到的合力fprac-i(t)如下：

22、

23、

24、其中，λ1和λ2是权重因子，满足λ1+λ2＝1；δθ是合力fres-i(t)与合力fres-i(t-1)间的夹角；

25、4.4：计算理想状态下第i架固定翼无人机在(t+1)时刻的航向角χi(t+1)：

26、

27、

28、其中，ε＝δt*ωmax表示两个相邻时刻中无人机方向允许的最大变化，δt是采样时间间隔；ωmax固定翼无人机可承受的最大角速度；表示合力fprac-i(t)的方向角；

29、4.5：计算理想状态下第i架固定翼无人机在(t+1)时刻的飞行路径角γi(t+1)：

30、

31、

32、4.5：当航向角较小时，固定翼无人机可加速到最大速度。当航向角过大时，需要减速。计算第i架固定翼无人机在(t+1)时刻的速度：

33、

34、其中，vmax和vmin分别为固定翼无人机可接受的最大和最小速度，a是其加速度；δχ1和δχ2是两个角度阈值，满足δχ1<δχ2<ε；vconst表示固定翼无人机的期望速度；

35、4.6：计算第i架固定翼无人机在t时刻的路径点：

36、

37、4.7：从t＝0开始重复步骤4.1-4.6，计算在每一个采样时刻的路径点。直到∈是一个常数，衡量无人机与目标点的接近程度。结束人工势场法的计算，将人工势场法计算的一串路径点作为轨迹规划问题的初始猜测；

38、步骤5：将固定翼无人机编队的轨迹规划问题重构为最优控制问题；具体步骤如下：

39、5.1：描述第i架固定翼无人机的运动学方程：

40、

41、其中，i＝{1,2,…,n}表示第i架固定翼无人机；时间域t∈[0,tf]，tf为需要求解的飞行终止时间。对于第i架无人机，(xi(t),yi(t),zi(t))是其三维位置信息；vi(t)为速度；γi(t)为飞行路径角；χi(t)为航向角；ti(t)，φi(t)和ni(t)分别为固定翼无人机的发动机推力，倾斜角和负载因子；分别为xi(t),yi(t),zi(t),vi(t),χi(t),γi(t)的一阶导数；g为重力加速度；空气动力学阻力di＝αρvi(t)scd0+αρvi(t)skcdl，其中α为常系数；ρ为空气密度；s为无人机的机翼面积；cd0为零升阻系数；k为感应电阻系数；为升力系数，其中li(t)＝ni(t)mg是该无人机的升力，m是固定翼无人机质量。

42、对于第i架固定翼无人机，状态量si(t)＝[xi(t),yi(t),zi(t),vi(t),χi(t),γi(t)]；控制量ui(t)＝[ti(t),φi(t),ni(t)]。因此，其运动方程可以简单表示为其中fi(·):表示第i架无人机的动力学；

43、5.2：对于固定翼无人机编队，状态s(t)＝[s1(t),s2(t),…,sn(t)]；控制u(t)＝[u1(t),u2(t),…,un(t)]；编队的动力学f(t)＝[f1,f2,…,fn,]。故编队的运动学方程如下：

44、

45、5.3：建立最优控制问题的边界条件。对于固定翼无人机的轨迹规划问题，关注于其初始位置和目标位置(xgoal,ygoal,zgoal)，对其他状态量的边界条件不做要求。因此第i架固定翼无人机的边界条件如下：

46、

47、5.4：确定固定翼无人机的物理约束。对于第i架固定翼无人机，将状态si(t)限制在区间si(t)∈其中和分别为状态的最小值和最大值；类似地，将控制ui(t)也限制在区间其中和分别为控制的最小值和最大值；

48、为了协同，固定翼无人机编队中的无人机需要保持通信，因此任何两架固定翼无人机都不能相距太远。将通信约束设置为：

49、

50、其中，dcomm是无人机间的最远通讯距离，具体数值根据无人机类型和具体应用场景设定。di,j(t)是在t时刻第i架无人机与第j架无人机间的欧氏距离；

51、为保证安全，固定翼无人机编队中的固定翼无人机之间需要保持一定的距离。将安全约束设置为：

52、

53、其中，dsafe是无人机间的最小安全距离，具体数值根据无人机类型和具体应用场景设定；

54、为实现固定翼无人机的避障功能，将障碍物约束设置为：

55、

56、其中，β是无人机与障碍物om间的最小距离；是第i架无人机与障碍物om间的欧氏距离；rm是障碍物的半径；当障碍物om是圆柱时，距离当障碍物om是半球时，距离

57、为了让固定翼无人机编队在飞行过程中保持特定的几何结构，设置第1架固定翼无人机为核心机，其他固定翼无人机与其保持特定的位置关系。故将编队约束设置为：

58、

59、其中，i＝2,3,…,n；j＝1是核心机的索引；表示第i架无人机相对于核心机的位置坐标；

60、5.5：本方法期望最小化固定翼无人机编队飞行的终止时间，将目标函数设定为

61、5.6：固定翼无人机编队的轨迹规划问题可以构建为如下的最优控制问题：

62、

63、步骤6：为提高求解过程的效率，将固定翼无人机编队约束惩罚到性能指标函数，并构造增广目标函数其中b是松弛因子；

64、步骤7：最优控制问题(22)可以被转化为如下问题：

65、

66、步骤8：为数值求解问题，将其在时域[0,tf]上离散化，以便将问题(9)转换为非线性规划问题。定义(nfe+1)个采样时刻{tp|p＝0,1,…,nfe}，要求所述采样时刻均匀分布在时域[0,tf]上，即tp-tp-1＝(tf-0)/nfe＝q,p＝1,2,…,nfe，其中q是在tp-1和tp之间的等距时间间隔。然后引入变量{sp|p＝0,1,…,nfe}和{up|p＝0,1,…,nfe}来分别表示s(t)和u(t)；

67、步骤9：采用四阶龙格-库塔方法在区间[tp,tp+1]上离散化运动方程：

68、

69、其中，k1＝f(sp,up,tp)，k4＝f(sp+k3*q,up,tp+q)；k1是编队的运动学方程在时间tp处以sp为状态，以up为控制得到的解；k2是编队的运动学方程在时间处以为状态，以up为控制得到的解；k3是编队的运动学方程在时间处以为状态，以up为控制得到的解；k4是编队的运动学方程在时间tp+q处以sp+k3*q为状态，以up为控制得到的解；

70、步骤10：在所有子区间[tp,tp+1]重复步骤9，即可形成一个非线性规划问题：

71、

72、其中，g(sp,up,tf)表示边界条件和物理约束的集合。

73、公式(25)所示的非线性规划问题的解也是连续时间最优控制问题(23)的数值近似解。

74、步骤11：将步骤4得到的高质量的初始猜测和步骤10得到的问题列式(25)输入到非线性规划求解器ipopt中，进行无人机编队的轨迹规划，得到编队飞行轨迹。

75、本发明的有益效果如下：

76、(1)本发明的改进人工势场法能够改善传统人工势场法的诸多缺点，消除局部振荡，实现队内协同和满足物理约束，实用性更强。尤其在为轨迹规划问题提供初始猜测时，加速了最优控制问题的收敛，效果提升很明显。

77、(2)本发明首先应用改进的人工势场法提供高质量的初始猜测，其次将固定翼无人机编队的轨迹规划问题转化为最优控制问题，并进行离散化，最后通过非线性规划求解器ipopt进行求解，得到固定翼无人机编队的飞行轨迹。本发明解决了基于几何方法难以处理复杂运动学约束的问题和智能优化算法计算负担大的问题，所建立的固定翼无人机编队轨迹规划方法能够在障碍物复杂的三维场景下成功求解问题，编队飞行过程中可以保持期望队形，能给灵活通过空间狭小的缝隙。计算简洁、高效、规划效率高，在固定翼无人机编队的轨迹规划问题中可以得到广泛的应用。