一种基于粒子群算法的无人机无冲突四维航迹规划方法与流程

本发明属于空中交通管理领域，尤其涉及一种基于粒子群算法的无人机无冲突四维航迹规划方法。

背景技术：

1、无人机的自主性是其区别有人机的重要特征，增强其与有人机的共域飞行能力对大幅提升空域的利用率有较大意义。

2、在未来复杂高密度的空域运行环境中，尤其是在中低空飞行条件下，执行各种飞行活动的无人机需要根据任务要求和环境约束进行三维空间的动态路径规划，使其规避地形障碍和碰撞风险，安全到达目标点。此外，在实际运行条件下，无人机能否在预定时间到达目标点也是必需考虑的限定条件，由此产生了加入飞行时间约束的四维路径规划问题。

3、综上所述，构建四维空间的任务路径规划系统成为提高无人机自主性的研究方向。近年来，无人机任务/路径规划已引起许多学者的关注并采用各种方法进行研究。

4、目前，国内的研究主要集中在二维、三维空间中的静态路径规划。国外学者在70年代初提出了基于四维路径规划的空中交通管制概念，但是针对无人机的四维航迹的规划方法研究比较少；同时、现有的无人机4d航迹规划方法主要是在无人机3d航迹规划的基础上，通过调整速度实现无人机的最优到达时间，但该方法的计算复杂度较高，计算延迟时间长，不符合多无人机情况下快速实现无人机无冲突解脱，不利于无人机的高效运行。

5、而且，现在的无人机四维航迹规划很少考虑管制区域内有人机情况下的无人机无冲突航迹规划。因此，需要提出一种能够实现无人机高效4d航迹规划的方法。

技术实现思路

1、针对下一代融合运行区域内，当有人机和无人机融合运行4d航迹规划发展需求时，为了快速实现无人机群体无冲突四维航迹规划，提出了一种基于粒子群算法的无人机无冲突四维航迹规划方法。

2、包括如下步骤：

3、步骤一、输入需规划的无人机群体无冲突四维航迹规划的约束条件、初始位置以及到达目的地信息；

4、约束条件包括：无人机最大速度限制、最大加速度限制、过点时间误差限制、最大转弯半径限制、与障碍物的安全距离限制，以及融合运行区域内的有人机位置平移不确定性区域

5、其中，无人机最大速度限制、最大加速度限制、过点时间误差限制、最大转弯半径限制、与障碍物的安全距离限制，具体限制范围根据实际情况人为设定。

6、有人机位置平移不确定性区域计算如下：

7、通过对有人机的历史飞行数据进行贝叶斯统计均值，得到有人机位置分布的先验信息；在获取观测样本后，按照样本和参数的联合分布，通过贝叶斯理论对下一时刻有人机位置平移随机变量分布进行预测，根据预测分布的均值和协方差计算得到有人机位置分布的不确定区域。

8、有人机的位置平移随机变量的期望和协方差通过概率模型表示，公式如下：

9、

10、其中，期望和协方差通过对有人机历史飞行数据进行贝叶斯统计得到。n(.)表示分布函数，k＝1,2,3....m表示有人机可能运动模式的数量，t表示时间步长，j'表示管制区内的第j'个有人机。

11、步骤二：基于输入的规划信息，生成该无人机群体初始的四维航迹；

12、步骤三：基于粒子群算法对初始的四维航迹进行无冲突航迹优化，得到全局最优位置；

13、主要包括以下步骤：

14、步骤3-1：构建包括约束条件的飞行距离最小的优化指标函数：

15、

16、

17、aiξ＜amax,i＝1,2,…n,ξ＝x,y,z

18、viξ＜vmax,i＝1,2,…n,ξ＝x,y,z

19、其中，j表示飞行距离最小和过点时间误差最小的指标函数，li表示无人机i的最短飞行距离，ej表示无人机j的过点时间误差，n表示无人机的数量，cij表示相邻无人机i和j之间的碰撞，一般用0和1表示，0表示无碰撞，1表示碰撞；aiξ表示无人机i在x,y,z三个方向上的加速度，viξ表示无人机i在x,y,z三个方向上的速度，amax,vmax分别表示无人机加速度和速度的最大值。

20、步骤3-2：根据上述优化指标，结合约束条件，形成无人机四维航迹规划的代价函数f：

21、

22、其中，ωc,ωa,ωv分别表示碰撞权值、加速度限制权值和速度限制权值，fc,fa,fv是碰撞、加速度和速度限制的约束条件不满足时对应的惩罚函数。

23、步骤3-3：对粒子群算法中粒子的位置和速度初始化，并对局部最优位置初始化；

24、步骤3-4，当局部最优位置对应的代价函数小于全局最优位置对应的代价函数，更新全局最优位置；否则，结束。

25、初始的全局最优位置人为规定；

26、步骤3-5：遍历所有粒子，更新第l个粒子的位置和速度；

27、

28、xl(m+1)＝xl(m)+vl(m+1),l＝1,2,…l

29、

30、l初始值为1；

31、vl(m)表示第m次迭代中粒子的速度，ω是迭代权值，分别表示认知系数，rand表示0～1的随机数，pbestl(m),gbestl(m)分别表示第l个粒子在最近m次迭代过程中的局部最优解和全局最优解，xl(m)是第m次迭代中粒子的位置，l表示粒子总数。ωmaxωmin分别表示迭代权值的最大值和最小值，maxiteration是最大迭代次数。

32、步骤3-6：当第l个粒子更新后的位置对应的代价函数f(xl)小于局部最优解对应的代价函数，即满足f(xl)＜f(pbestl)，则更新局部最优位置，返回步骤3-4；

33、否则，保持局部最优解不变；返回步骤3-5进行下一次迭代，直至达到最大迭代次数。

34、步骤四、判断全局最优位置的无人机群体是否存在航迹冲突，如果是，进入步骤五；否则，输出全局最优位置作为最优航迹规划集合；

35、步骤五、基于延迟起飞时间算法实现无人机群体的冲突解脱；

36、首先，从全局最优位置的无人机群体集合中，选取存在冲突的无人机，形成冲突集合；并按照冲突次数由多到少对无人机排序；依次选择冲突最多的无人机的计划起飞时间，将其推迟时间t；然后，更新冲突信息，再次判断更新后的无人机群体对应的航迹是否存在冲突，如果是，继续将冲突最多的无人机的计划起飞时间推迟，直至达到最大迭代次数或者所有无人机都实现无冲突的四维航迹规划；

37、步骤六、当延迟起飞算法达到最大迭代次数后，判断无人机群体规划的航迹是否仍旧存在冲突，如果是，则通过在航迹上增加航路点实现无人机群体冲突解脱，否则，输出最优化航迹集合和最优起飞时间；

38、首先，初始化航路点数量；并找到所有未解决的冲突航迹，按照冲突次数由多到少将无人机进行排序；对于冲突最多的航迹增加航路点，基于pso获取冲突最多的航迹的最优航路点坐标、航迹段等参数；

39、然后，更新所有无人机航迹的参数并再次进行航迹冲突探测，更新冲突信息，直至达到最大迭代次数或者所有无人机都实现无冲突的四维航迹规划；

40、步骤七、判断增加航路点算法达到最大迭代次数后，无人机群体规划的航迹是否仍旧存在冲突，如果是，返回步骤三，否则，输出航路点的最优化坐标和航迹参数，得到得到最终的无人机群体对应的无冲突四维航迹规划。

41、本发明的优点在于：

42、本发明一种基于粒子群算法的无人机无冲突四维航迹规划方法，为无人机无冲突四维航迹规划提供了一种快速实现方法；其次，为未来有人机无人机无冲突融合运行提供了技术支持；同时，本发明为下一代空管系统实现4d航迹精准预测提供技术支撑。