无人机与多个飞行器的动态冲突检测和解脱方法与流程

本发明涉及无人机，尤其涉及一种无人机与多个飞行器的动态冲突检测和解脱方法。

背景技术：

1、随着无人机系统在工业、农业、军事等多个领域的广泛应用，无人机飞出“隔离空域”并与有人机共享空域执行多样化任务已成为当前的趋势。当前制约无人机全域飞行的关键技术为可靠的飞行冲突解脱能力。与有人机主要靠空中交通管理、空中防撞系统等飞行冲突解脱方式不同，无人机因指令操作延迟时间长、载荷相对较小、动力学模型较为复杂、面临的威胁不确定性高等特点，冲突解脱技术与有人机有较大不同。为确保避障性能的可靠性，应尽量避免无人机状态的连续改变，在最少机动的基础上实现避撞与执行任务功能，便于操作，保证飞行安全。

2、现有技术中，多机动态冲突解脱大多采用基于机器学习的避障方法，但该类方法在飞行场景发生改变时，避障准确性降低，需要对模型进行重新训练，模型离线学习训练的耗时较长且不易收敛，代价相对较高。

3、现有技术中，基于速度障碍法的避障方法在单机冲突领域取得了不错的效果。速度障碍法通过两者的相对速度来计算运动过程的相互间的最近距离以判断无人机与障碍物之间是否存在飞行冲突。基于速度障碍法的避障方法，在判断存在潜在冲突后，一般通过航向调配或速度调配进行解脱，该方法的计算复杂度低，实时性好，并且避障准确性较高。但是，现有的基于速度障碍法的避障方法大多用于解决单机冲突问题，关于多机冲突问题的研究较少，具体来说，现有的基于速度障碍法的避障方法中侧重于求解无人机避障的最优速度或最优航向，而未给出无人机如何规划能够同时躲避多个飞行器的速度和/航向调配参数的具体策略。

4、此外，现有的基于速度障碍法的避障方法中，未考虑到无人机实际的机动性约束，即没有考虑到无人机的速度和航向调整是一个渐变过程，具体的例如无人机的速度不可能马上从10m/s跃升到20m/s，导致无人机实际无法到达规划的解脱点位置，对解脱效果产生不可预测的风险，避障安全性差，使无人机无法继续执行任务。

技术实现思路

1、鉴于上述的分析，本发明实施例旨在提供一种无人机与多个飞行器的动态冲突检测与解脱方法，用以解决现有的基于速度障碍法的避障方法不能使无人机实现多机动态冲突解脱的技术问题。

2、本发明实施例提供了一种无人机与多个飞行器的动态冲突检测和解脱方法，所述方法包括：

3、无人机执行飞行任务时，基于速度障碍法检测当前空域中是否有与无人机存在潜在碰撞风险的潜在碰撞飞行器；

4、若检测到有多个潜在碰撞飞行器，则对于每个潜在碰撞飞行器分别进行单机冲突解脱计算，以分别获取无人机对于每个潜在碰撞飞行器能够实现解脱的调速方式和/或旋转方向以及相应的调配参数；所述调速方式包括加速方式和减速方式，所述旋转方向包括逆时针方向和顺时针方向，所述调配参数包括避障飞行距离、避障飞行时间、速度调整量、旋转角度；

5、比较所有的潜在碰撞飞行器的单机冲突解脱计算结果，根据一致性原则和安全性原则选择一种调速方式和/或一种旋转方向作为无人机多机冲突解脱的调速方式和/或旋转方向，然后根据取大原则从所述无人机多机冲突解脱的调速方式和/或旋转方向对应的调配参数中获取无人机实际的调配参数，从而获取实际的解脱规划结果；

6、根据所获取的实际的解脱规划结果控制无人机的速度和/或航向以及避障飞行距离，以实现多机冲突解脱。

7、基于上述方法的进一步改进，所述根据一致性原则和安全性原则选择一种调速方式和/或一种旋转方向作为无人机多机冲突解脱的调速方式和/或旋转方向，包括：

8、比较所有的潜在碰撞飞行器获得的单机冲突解脱计算结果后，若两种调速方式和/或两种旋转方向均能够使无人机与所有的潜在碰撞飞行器解脱成功，则根据无人机和潜在飞行器的相对速度矢量与该潜在碰撞飞行器安全圆的相对位置判断无人机对于该潜在碰撞飞行器的更安全的调速方式和/或旋转方向，并将其作为优选调速方式和/或优选旋转方向；

9、若无人机对于各潜在碰撞飞行器的优选调速方式和/或优选旋转方向一致，则选择所述优选调速方式和/或所述优选旋转方向作为无人机多机冲突解脱的调速方式和/或旋转方向。

10、基于上述方法的进一步改进，所述根据一致性原则和安全性原则选择一种调速方式和/或一种旋转方向作为无人机多机冲突解脱的调速方式和/或旋转方向，包括：

11、若无人机对各潜在碰撞飞行器的优选调速方式和/或优选旋转方向不一致，则比较两种调速方式和/或两种旋转方向对应的最长的避障飞行时间，基于安全性原则选取其中的较小值对应的调速方式和/或旋转方向作为无人机多机冲突解脱的调速方式和/或旋转方向。

12、基于上述方法的进一步改进，所述根据一致性原则和安全性原则选择一种调速方式和/或一种旋转方向作为无人机多机冲突解脱的调速方式和/或旋转方向，包括：

13、比较所有的潜在碰撞飞行器获得的单机冲突解脱计算结果后，若只有一种调速方式和/或一种旋转方向能够使无人机与所有的潜在碰撞飞行器解脱成功，则选择该调速方式和/或旋转方向作为无人机多机冲突解脱的调速方式和/旋转方向。

14、基于上述方法的进一步改进，所述对于每个潜在碰撞飞行器分别进行单机冲突解脱计算，包括：

15、无人机具有速度调配避障模式、航向调配避障模式以及速度和航向混合调配避障模式三种避障方式；

16、按照预设的顺序选择避障模式，并基于所选择的避障模式，根据当前无人机与该潜在碰撞飞行器的速度和位置信息以及无人机的机动性参数进行单机冲突解脱计算；所述机动性参数包括最大纵向加速度、最大纵向速度、最小纵向速度和最大角速度。

17、基于上述方法的进一步改进，对于每个潜在碰撞飞行器分别优先选择所述速度调配避障模式进行单机冲突解脱计算，当经过计算和比较采用所述速度调配避障模式不能实现多机冲突解脱时，则基于最少机动原则选择所述航向调配避障模式进行单机冲突解脱计算，当经过计算和比较所述航向调配避障模式不能实现多机冲突解脱时，则基于所述速度和航向混合调配避障模式进行单机冲突解脱计算；或者，

18、对于每个潜在碰撞飞行器分别优先基于所述速度调配避障模式进行单机冲突解脱计算，当经过计算和比较采用所述速度调配避障模式不能实现多机冲突解脱时，则基于所述速度和航向混合调配避障模式进行单机冲突解脱计算。

19、基于上述方法的进一步改进，所述速度调配避障模式为：按照所获取的无人机多机冲突解脱的调速方式和无人机实际的速度调整量δva_1，无人机以所述最大纵向加速度amax经过t1时间将速度从初始速度va_0调整至避障速度va_0+δva_1，之后无人机以避障速度va_0+δva_1匀速飞行t2时间，直至无人机的飞行距离达到避障飞行距离lad；

20、采用如下方法基于速度调配避障模式进行单机解脱计算：

21、定义无人机的位置为a；目标飞行器的位置为o，安全圆半径为r，飞行速度为v0；当前无人机与目标飞行器之间的距离为lao；

22、根据无人机与目标飞行器的相对速度矢量和目标飞行器的安全圆，确定加速方式对应的解脱点位置b和减速方式对应的解脱点位置c；

23、根据如下公式计算加速方式和减速方式对应的无人机的避障飞行距离lad和达到避障飞行距离lad所需要的避障飞行时间ttotal：

24、||va_1||＝||-v0||·(sinα/sinβ)；

25、||vr_1||＝||-v0||·(sin(α+β)/sinβ)；

26、

27、ttotal＝lad/||va_1||；

28、式中，为无人机避开潜在飞行器的相对速度矢量，加速模式中减速模式中，α为-v0与之间的夹角；va_1为假定无人机能够瞬间变速时对应的能够实现冲突解脱的速度，va_1与va_0的方向相同；β为va_1与之间的夹角；vr_1为va_1与v0的相对速度矢量，vr_1＝va_1-v0；

29、根据所述无人机的避障飞行距离lad、所述避障飞行时间ttotal和无人机的机动性参数分别判断无人机通过加速方式或减速方式是否能解脱成功，公式为：

30、

31、

32、

33、式中，va_max和va_min分别为无人机的最大纵向速度和最小纵向速度，va_max＞va_min＞0；va_limit为无人机的速度限值；lmax无人机实际可飞行的最大距离；tmin为无人机达到速度限值所需的最小时间；a为无人机的加速度，加速方式中a＝amax，减速方式中a＝-amax；

34、若lmax＜lad，则不能够解脱成功；若lmax≥lad，则能够解脱成功；

35、若能够解脱成功，则根据避障飞行距离lad、所述避障飞行时间ttotal计算无人机的速度调整量δva_1、无人机加速至避障速度va_0+δva_1所需要的时间t1和无人机以避障飞行速度va_0+δva_1匀速飞行的时间t2，公式为：

36、lad＝va_0·t1+(a·t12)/2+(va_0+a·t1)·(ttotal-t1)；

37、δva_1＝a·t1；

38、t2＝ttotal-t1。

39、基于上述方法的进一步改进，所述航向调配避障模式为：按照所获取的无人机多机冲突解脱的旋转方向和无人机实际的旋转角度θ，无人机以最大角速度ω旋转角度θ，并以初始速度va_0匀速飞行，直至无人机的飞行距离达到避障飞行距离lad；

40、采用如下方法基于航向调配避障模式进行单机解脱计算：

41、定义无人机的位置为a；目标飞行器的位置为o，安全圆半径为r，飞行速度为v0；当前无人机与目标飞行器之间的距离为lao；

42、根据无人机与目标飞行器的相对速度矢量和目标飞行器的安全圆，确定逆时针方向对应的解脱点位置b′和顺时针方向对应的解脱点位置c'；

43、根据如下公式计算逆时针方向和顺时针方向对应的无人机的旋转角度θ、避障飞行距离lad和达到避障飞行距离lad所需要的避障飞行时间t′total，公式为：

44、||va_2||/sinα＝||-v0||/sinδ；

45、||va_2||＝||va_0||；

46、vr_2＝va_2-v0；

47、δ＝arcsin(||-v0||·sinα/||va_2||)；

48、θ＝δ-β；

49、

50、lad＝||va_0||·t′total；

51、式中，va_2为无人机旋转角度θ后的速度矢量；vr_2为无人机旋转角度θ后与目标飞行器的相对速度矢量；无人机避开目标飞行器的相对速度矢量，逆时针旋转时顺时针旋转时，α为-v0与之间的夹角；β为速度矢量va_0与矢量的夹角；δ为速度矢量va_2与vr_2的夹角；无人机逆时针旋转时，0＜θ＜90°；无人机顺时针旋转时，-90°＜θ＜0；

52、获取无人机的旋转角度θ和无人机的避障飞行时间t′total后，根据无人机的最大角速度ω判断无人机按照逆时针方向或顺时针方向旋转是否能够解脱成功，

53、若θ≤ω·t′total，则无人机能够解脱成功，若θ＞ω·t′total，则无人机不能解脱成功。

54、基于上述方法的进一步改进，所述速度和航向混合调配避障模式为：按照所获取的无人机多机冲突解脱的调速方式和旋转方向以及无人机实际的速度调整量δva_1和旋转角度θm，无人机以最大角速度ω旋转角度θm，同时无人机以所述最大纵向加速度amax经过t1'时间将速度从初始速度va_0调整至避障速度va_0+δva_2，之后无人机以避障速度va_0+δva_2匀速飞行t′2时间，直至无人机的飞行距离达到避障飞行距离lad；

55、采用如下方法基于速度和航向混合调配避障模式进行单机解脱计算：

56、定义无人机的位置为a；目标飞行器的位置为o，安全圆半径为r，飞行速度为v0；当前无人机与目标飞行器之间的距离为lao；

57、根据无人机与目标飞行器的相对速度矢量和目标飞行器的安全圆确定解脱点位置b和c，并分别确定解脱点位置b和c对应的调速方式和旋转方向；

58、根据如下公式计算解脱点位置b和c对应的调速方式和旋转方向对应的无人机的避障飞行距离lad和达到避障飞行距离lad所需要的避障飞行时间t″total、无人机的旋转角度θm、速度调整量δva_2、无人机变速至避障速度va_0+δva_2所需要的时间t1'和无人机以避障速度va_0+δva_2匀速飞行的时间t′2：

59、||va_2||＝||-v0||·(sinα/sin(θm+β))；

60、||vr_2||＝||-v0||·(sin(θm+β+α)/sin(θm+β))；

61、

62、

63、

64、

65、

66、

67、lad＝va_0·t1+(a·t1')/2+(va_0+a·t1)·(t″total-t1')；

68、δva_2＝a·t1'；

69、t′2＝t″total-t1'；

70、式中，为无人机避开目标飞行器的相对速度矢量，解脱点位置b对应的调速方式和旋转方向中解脱点位置c对应的调速方式和旋转方向中va_2为无人机旋转角度θm后的速度矢量；vr_2为无人机旋转角度θm后与目标飞行器的相对速度矢量；α为-v0与之间的夹角；β为va_0与之间的夹角；va_max和va_min分别为无人机的最大纵向速度和最小纵向速度，va_max＞va_min＞0；va_limit为无人机的速度限值；l'max为无人机实际可飞行的最大距离；tmin为无人机达到速度限值所需的最小时间；无人机逆时针旋转时，0＜θm＜90°；无人机顺时针旋转时，-90°＜θm＜0；a为无人机的加速度，加速方式中a＝amax，减速方式中a＝-amax。

71、基于上述方法的进一步改进，所述基于速度障碍法检测当前空域中是否有与无人机存在潜在碰撞风险的潜在碰撞飞行器，包括如下步骤：

72、判断无人机与目标飞行器之间的高度差hs是否超过高度差阈值h0，三维距离d3d是否超过距离阈值d0；

73、若hs≤h0且d3d≤d0，则判断目标飞行器的中心点与无人机和目标飞行器的速度矢量差vr之间的距离dl是否超过目标飞行器的安全圆半径r；

74、若dl≤r，则判定所述目标飞行器为潜在碰撞飞行器。

75、与现有技术相比，本发明至少可实现如下有益效果之一：

76、1、本发明提供了一种基于速度障碍法的避障方法，用于解决多机动态冲突问题，通过对无人机的速度和/或航向以及避障飞行距离进行规划，使无人机能够同时躲避多个飞行器，数学模型简单，计算快速，满足避障实时性要求。

77、2、本发明的方法中，当无人机与多个飞行之间存在潜在碰撞风险时，首先分别对于每个潜在碰撞飞行器进行单机冲突解脱计算，并且在获取单机冲突解算结果中包括所有能够实现冲突解脱的调速方式和/或旋转方向以及相应的调配参数，然后对所有比较所有的潜在碰撞飞行器的单机冲突解脱计算结果，以选出能够适用于所有的潜在飞行器并且最优的解脱规划结果。本发明进行单机冲突解脱计算时，全面考虑了加速方式和减速方式两种调速方式，以及逆时针方向和顺时针方向两种旋转方向，进而根据一致性原则、安全性原则和取大原则确定无人机的实际的解脱规划结果，不仅提高了实现多机动态冲突解脱的可能性，同时提高了无人机的避障安全性。

78、其中，一致性原则用于选出适用于所有潜在冲突飞行器的调速方式和/或旋转方向，安全性原则用于选出更优的调速方式和/或旋转方向，取大原则用于能够选出适用于所有潜在冲突飞行器的调配参数。

79、本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。