飞行器路径动态规划方法、计算机系统及介质

1.本发明属于路径规划技术领域，具体涉及一种飞行器路径动态规划方法、计算机系统及介质。


背景技术：

2.飞行器路径规划是在规划区域内，在给定的环境约束和飞行器自身运动学约束条件下为飞行器寻找出一条从起始位姿点到目标位姿点的最优或可行的路径。飞行器飞行过程中当突然出现威胁/障碍物需要规避或发生任务变更情形时，通常需要动态生成一条安全可行且满足起始和目标方向约束的路径，路径动态规划问题本质上是一个约束众多的多目标优化问题。文献“基于dubins路径的a*算法的多无人机路径规划，电光与控制，2018，vol.25(11)，p25-29”公开了一种结合dubins路径的a*算法用于多无人机路径规划。该算法针对多个不重叠圆形障碍物场景，采用基于dubins路径下的单圆障碍物规避方法进行节点扩展，考虑所有当前冲突障碍物，以切入冲突障碍物圆的位姿点作为可能的节点，并以dubins距离作为估计代价进行a*搜索。
3.上述方法仅以切入当前冲突的障碍物圆的位姿点作为可能的备选节点进行扩展，存在从当前节点到备选节点路径均与其他障碍物冲突导致规划失效的可能性；并且上述规划方法未考虑障碍物可能存在重叠的情形。


技术实现要素：

4.针对现有技术方法在完备性及适应性方面的不足，本发明提供一种基于dubins路径的a*算法搜索的飞行器路径动态规划方法、计算机系统和介质。
5.本发明实施例的第一方面，提供了一种飞行器路径动态规划方法。该方法包括如下s1～s5。s1，获取初始数据，所述初始数据包括飞行器最小转弯约束半径、起始位姿点和目标位姿点的位姿信息、以及威胁区的参数数据；其中，威胁区的参数数据包括m个威胁区中每个威胁区的圆心和半径，其中，m为大于或等于1的整数。s2，根据所述m个威胁区之间的重叠情况，将所述m个威胁区划分为独立威胁区、被包含威胁区和威胁区群，并输出独立威胁区和威胁区群的信息；其中，所述重叠情况包括内含和相交，所述相交包括直接相交和间接相交。s3，针对s2输出的每个威胁区群，构建每个威胁区群的威胁区群凸包，所述威胁区群凸包为以每个威胁区群中的至少两个威胁区的圆周上的部分圆弧和该至少两个威胁区的外公切线连接而成的封闭区域，其中，所述威胁区群凸包包含了对应威胁区群中的所有威胁区。s4，不断遍历得到的每个威胁区群凸包，将每个威胁区群凸包和与自己具有所述重叠情况的所有的独立威胁区以及其他的威胁区群凸包合并为新的威胁区群凸包，直至威胁区群凸包和/或独立威胁区彼此之间不存在所述重叠情况。s5，在四种初始情况下，分别运用基于dubins路径的a*算法搜索从所述起始位姿点到达所述目标位姿点的路径，以得到四条路径；其中，所述四种初始情况分别为所述飞行器在所述起始位姿点和所述目标位姿点的转弯方向为左左、左右、右右和右左；在所述四条路径中的每条路径中所述飞行器均须绕
开s4中产生的威胁区群凸包和/或独立威胁区。s6，输出所述四条路径中最短路径的信息。
6.根据本发明的实施例，所述s3包括：寻找每个威胁区群中的所有极威胁区；以及由每个威胁区群中的相邻的极威胁区之间的外公切线段和所有极威胁区上位于两个外公切线段之间的最短圆弧，组成每个威胁区群的威胁区群凸包的边界。其中，所述寻找每个威胁区群中的所有极威胁区包括：在每个威胁区群中寻找第一个极威胁区，包括：根据每个威胁区群中所有威胁区圆周上的点的坐标，寻找至少一个维度上坐标值最小的点，将找到的点所在的威胁区确定为所述第一个极威胁区；以所述第一个极威胁区作为初始的当前极威胁区，按照逆时针顺序，通过构造外公切线的方式在每个威胁区群中寻找新的极威胁区，并将每次找到的新的极威胁区更新为当前极威胁区；以及当更新后的当前极威胁区为所述第一个极威胁区时，确定找到了所有极威胁区，并终止寻找新的极威胁区。
7.根据本发明的实施例，所述寻找所述第一个极威胁区包括：分别查找当前威胁区群中所有威胁区的圆周上y坐标最小点，进行比较；若找到唯一的y坐标最小点，则确定该y坐标最小点所在的威胁区为所述第一个极威胁区；若找到的y坐标最小点不唯一，则对找到的y坐标最小点的x坐标进行比较，并以其中x坐标最小点对应的威胁区作为所述第一个极威胁区。
8.根据本发明的实施例，所述以所述第一个极威胁区作为初始的当前极威胁区，按照逆时针顺序，通过构造外公切线的方式在每个威胁区群中寻找新的极威胁区包括：构造当前极威胁区到威胁区群中每个其他威胁区的外公切线，每一条外公切线的切出点与当前极威胁区的圆周上作为所述威胁区群凸包边界的切入点之间构成一段圆弧；若存在唯一的最短圆弧，确定所述唯一的最短圆弧对应的外公切线所切入的威胁区为寻找到的新的极威胁区；以及若最短圆弧不唯一，则以多个最短圆弧对应的外公切线中，最长外公切线段所切入的威胁区，作为寻找到的新的极威胁区。
9.根据本发明的实施例，所述s4包括：获取所有的威胁区群凸包与独立威胁区的信息；以及判定每个威胁区群凸包与独立威胁区是否存在所述重叠情况。其中，判定每个威胁区群凸包与独立威胁区是否存在所述重叠情况具体包括：计算威胁区群凸包ci和独立威胁区bj的x坐标范围和y坐标范围；当威胁区群凸包ci和独立威胁区bj的x坐标范围和y坐标范围同时存在交集时，判断独立威胁区bj是否与构成威胁区群凸包ci的边界的所有切线段存在重叠；若存在重叠，则确定威胁区群凸包ci和独立威胁区bj存在所述重叠情况；若不存在重叠，判断独立威胁区bj的圆心是否在威胁区群凸包ci的内部；若是，则确定威胁区群凸包ci和独立威胁区bj存在所述重叠情况，其中，威胁区群凸包ci包含独立威胁区bj；否则，确定威胁区群凸包ci和独立威胁区bj不存在所述重叠情况。
10.根据本发明的实施例，所述s4还包括判定两个威胁区群凸包是否存在所述重叠情况，具体包括：计算威胁区群凸包ci和cj的x坐标范围和y坐标范围；当威胁区群凸包ci和cj的x坐标范围和y坐标范围同时存在交集时，判定威胁区群凸包ci与威胁区群凸包cj中的每个极威胁区是否存在所述重叠情况，得到第一轮判定结果；判定威胁区群凸包cj与威胁区群凸包ci中的每个极威胁区是否存在所述重叠情况，得到第二轮判定结果；若所述第一轮判定结果和所述第二轮判定结果中均存在相交，则威胁区群凸包ci与威胁区群凸包cj相交；若所述第一轮判定结果均为包含，则威胁区群凸包ci包含威胁区群凸包cj；若所述第二轮判定结果均为包含，则威胁区群凸包cj包含威胁区群凸包ci；若所述第一轮判定结果和/或第
二轮判定结果中同时存在包含和不重叠，则威胁区群凸包ci与威胁区群凸包cj相交；若所述第一轮判定结果与第二轮判定结果均为不重叠，则威胁区群凸包ci与威胁区群凸包cj不存在所述重叠情况。
11.根据本发明的实施例，所述s5中运用基于dubins路径的a*算法搜索从所述起始位姿点到达所述目标位姿点的路径时，代价函数f＝g+h中，g代表所述起始位姿点到当前位姿点的实际路径长度；h代表当前位姿点到所述目标位姿点的预估路径长度，其中，所述预估路径长度为dubins路径长度。
12.根据本发明的实施例，所述s1还包括对威胁区进行预处理，具体包括：将所述m个威胁区中每个威胁区的半径外延安全间距d
safe
，以扩展每个威胁区；以及当扩展后的威胁区中存在半径小于所述飞行器最小转弯约束半径rc的极小威胁区时，将所述极小威胁区构造为半径等于rc的威胁区。
13.本发明实施例的第二方面，提供了一种计算机系统。所述计算机系统包括一个或多个处理器和存储器。所述存储器用于存储一个或多个程序。其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行上述的方法。
14.本发明实施例的第三方面，提供了一种计算机可读存储介质，其上存储有计算机程序指令，该计算机程序指令被处理器执行时实现上述的方法。
15.本发明实施例针对圆形威胁区存在重叠的情形，构建以重叠威胁区圆为主体的威胁区群凸包，将存在威胁区重叠场景转化为威胁区群凸包不重叠场景；在结合dubins路径和a*算法搜索方法的基础上，引入基本可视图(essential visibility graphs)和迭代的方法进行节点扩展。本发明实施例的方法可适应于规划环境中威胁区/障碍物圆存在重叠的情形，而且避免了从当前节点到备选节点路径均与其他威胁区冲突导致可能的路径规划失效的问题。
附图说明
16.图1是本发明一实施例中基于dubins路径的a*算法搜索的飞行器路径动态规划方法的整体流程图。
17.图2是本发明一实施例中基于dubins路径的a*算法搜索的飞行器路径动态规划的a*算法搜索流程图。
18.图3是本发明一实施例中基于dubins路径的a*算法搜索的飞行器路径动态规划的当前节点的可达节点寻找整体流程图。
19.图4是本发明一实施例中基于dubins路径的a*算法搜索的飞行器路径动态规划的切线段处理子程序流程图。
20.图5是本发明一实施例中基于dubins路径的a*算法搜索的飞行器路径动态规划的凸包内部冲突处理子程序流程图。
21.图6是本发明一实施例中基于dubins路径的a*算法搜索的飞行器路径动态规划的凸包上节点特殊扩展原理示意图。
22.图7是本发明一实施例中基于dubins路径的a*算法搜索的飞行器路径动态规划的威胁区群凸包内外切线示意图。
23.图8是本发明一实施例中基于dubins路径的a*算法搜索的飞行器路径动态规划的
当前节点的可达节点寻找示意图。
24.图9是本发明一实施例中在典型场景下基于dubins路径的a*算法搜索的飞行器路径动态规划算法规划生成的路径结果图。
具体实施方式
25.下面结合附图和实施例对本发明作进一步说明。
26.如图1所示，参阅图2～图5，本发明一实施例的飞行器路径动态规划方法包括以下步骤：
27.步骤1：飞行器动态路径规划初始数据获取。
28.初始数据获取包括获取飞行器当前动态路径规划的最小转弯约束半径、起始位姿点和目标位姿点的位姿信息以及威胁区的参数数据。
29.1)获取飞行器最小转弯约束半径rc。
30.2)获取飞行器起始位姿点的位姿和目标位姿点的位姿
31.3)获取威胁区的参数数据：建模为圆形的m个威胁区每个圆形威胁区的圆心和半径(xi，yi，ri)，其中i＝1，2，
…
，m。
32.步骤2：对威胁区进行预处理，包括如下步骤2.1)～步骤2.3)。
33.2.1)将圆形威胁区(xi，yi，ri)外延安全间距d
safe
，生成辅助圆(xi，yi，r
′i)，其中r
′i＝ri+d
safe
，将威胁区扩展为安全间距d
safe
为综合考虑飞行器大小、控制精度、导航定位精度而设定的常值。
34.2.2)针对威胁区中，威胁区圆半径小于飞行器最小转弯约束半径rc的情况，构造半径等于rc的同心辅助圆。经此处理后，威胁区圆记为o1，
…
，om。
35.2.3)威胁区重叠预处理。
36.威胁区重叠分为内含和相交两种情况。将威胁区分类为独立威胁区、被包含威胁区和威胁区群。其中，独立威胁区不与任何其他非被包含威胁区重叠；将具有直接相交或间接相交关系的威胁区组成集合，作为一个威胁区群。直接相交表明两威胁区具有几何相交关系。若威胁区oi与威胁区oj、威胁区oj与威胁区ok分别直接相交，但oi与ok不直接相交，称oi与ok具有通过直接相交传递到的间接相交关系。
37.威胁区重叠预处理具体方法说明如下：
38.①
获取所有威胁区o1，
…
，om信息。
39.②
对威胁区oi(i＝1，
…
，m)分别对其后续的威胁区oj(j＝i+1，...，m)进行遍历，两两判断其位置关系，构建邻接矩阵。圆与圆相切，此处等同视为不相交；若为内含关系，对被包含威胁区进行标记；若为相交关系，在邻接矩阵的对应位置进行标记。
40.③
依次访问m个威胁区，若一个威胁区其未被访问过，且不为被包含威胁区，则对其依据邻接矩阵执行深度优先搜索访问，将搜索结果存入新的威胁区集合中。
41.④
输出获得的威胁区集合，包括威胁区群ai(i＝1，...，m1)和独立威胁区bj(j＝1，...，m2)，同时忽略被包含威胁区。
42.步骤3：针对威胁区群ai(i＝1，...，m1)，参考jarvis步进法，构建以威胁区圆为主
体的威胁区群凸包ci(i＝1，...，m1)。威胁区群凸包(简称“凸包”)构建具体方法步骤包括如下步骤3.1)～步骤3.3)，具体说明如下：
43.3.1)根据lowest-then-leftmost法则找到第一个构成凸包的威胁区。构成凸包的威胁区称为极威胁区。
44.寻找第一个极威胁区方法说明如下：
45.分别找到当前威胁区群中所有威胁区圆上y坐标最小点，进行比较。若某一威胁区圆上的y坐标最小点为全局最小点，则该威胁区为第一个极威胁区；若上述找到的点中全局y坐标最小点不唯一，对这些全局y坐标最小点的x坐标进行比较，其中x坐标最小点对应的威胁区为第一个极威胁区。第一个极威胁区上参与比较的点称之为ltl点。
46.3.2)对于新找到的极威胁区，视为当前极威胁区，按照逆时针顺序找到下一个相邻极威胁区，当前极威胁区与它的下一个相邻极威胁区的外公切线为凸包的组成部分。
47.从当前极威胁区寻找下一个相邻极威胁区方法说明如下：
48.构造当前极威胁区到所在威胁区群中每个其他威胁区的外公切线，每一条外公切线的切出点与当前极威胁区作为凸包组成的切入点或ltl点构成一段圆弧，最短圆弧对应外公切线切入的威胁区即为当前极威胁区对应的下一个相邻极威胁区。如果最短圆弧不唯一，则最短圆弧对应的最长外公切线段切入的威胁区为当前极威胁区的下一个相邻极威胁区。
49.3.3)判断步骤3中3.2)找到的下一个相邻极威胁区是否为第一个极威胁区。
50.①
若不为第一个极威胁区，转至步骤3.2)；
51.②
若为第一个极威胁区，则极威胁区寻找结束。凸包由所有极威胁区上相应的最短圆弧以及相邻极威胁区之间的外公切线段组成。
52.步骤4：威胁区群凸包的重叠检测与化解。
53.获取所有凸包与独立威胁区信息；不断重复下述步骤直至不发生重叠或者归并到一个凸包；对凸包之间以及凸包与独立威胁区之间两两进行凸包重叠检测，建立位置关系邻接矩阵；利用深度优先搜索将各组相交的凸包或独立威胁区合并为大凸包，舍弃掉被包含的凸包或独立威胁区。
54.凸包重叠检测包括检测威胁区群凸包和独立威胁区之间以及威胁区群凸包之间是否产生重叠。具体方法可以包括步骤4.1)～步骤4.2)，说明如下：
55.4.1)凸包ci与独立威胁区bj重叠判定。
56.a)获取凸包ci与独立威胁区bj信息。
57.b)计算ci和bj的x坐标范围和y坐标范围，仅当二者的x坐标范围和y坐标范围同时存在交集，二者才可能产生重叠，继续进行判定；否则，二者不发生重叠，判定结束。
58.c)判断bj是否与构成凸包ci的所有切线段存在重叠，若存在重叠，则ci与bj相交，判定结束；否则继续判定。
59.d)判断bj的圆心是否在ci的内部，若是，表明ci包含bj；否则二者不发生重叠。
60.4.2)凸包ci和cj之间重叠判定。
61.a)获取两凸包ci和cj信息。
62.b)计算凸包ci和cj的x坐标范围和y坐标范围，仅当二者的x坐标范围和y坐标范围同时存在交集，二者才可能产生重叠，继续进行判定；否则，二者不发生重叠，判定结束。
63.c)凸包ci对cj的每一个极威胁区分别进行一次类似于上述步骤4.1)中的重叠判定，称为第一轮判定；凸包cj对ci的每一个极威胁区分别进行一次类似上述步骤4.1的重叠判定，称为第二轮判定。若两轮判定中存在相交，则ci与cj相交；若第一轮判定均为包含，则ci包含cj；若第二轮的判定均为包含，则cj包含ci；若某一轮中既存在包含也存在不重叠，则ci与cj相交；若两轮均为不重叠时，ci与cj不重叠。
64.接下来如图2所示，通过下文介绍的步骤5～步骤11，以飞行器在所述起始位姿点和所述目标位姿点的转弯方向为左左为例，详细介绍运用基于dubins路径的a*算法搜索从起始位姿点到达目标位姿点的路径的算法实现过程。
65.步骤5：建立并清空open表和close表，初始化起始位姿点和目标位姿点的转弯方向均为向左。
66.步骤6：计算起始位姿点代价函数值，并将起始位姿点加入到open表。
67.代价函数为f＝g+h。其中g代表起始位姿点到当前位姿点的实际路径长度。当前位姿点为起始位姿点时，该值为零。h代表当前位姿点到目标位姿点的预估路径长度，此处选取当前位姿点到目标位姿点的dubins路径长度，并且该长度不考虑与威胁区的冲突。后续将加入到open表和close表中的位姿点称为节点。
68.步骤7：从open表中找到f值最小的节点，把它作为当前要处理的节点，简称为当前节点，并将当前节点移至close表中。
69.步骤8：基于基本可视图和迭代的方法寻找当前节点的可达节点，并对可达节点进行相应的处理，具体包括下述步骤8.1)和步骤8.2)。参考图3～图5，详述如下。
70.8.1)当前节点的可达节点寻找，具体方法步骤说明如下。
71.①
设置并初始化可达节点集合s
pr
、已检查切线集合s
lc
、冲突集合s
cn
、标志self_sign为零。
72.②
当前节点对应转弯圆到终止位姿点对应转弯圆的切线段l
cg
，将切线段l
cg
作为当前切线段(如图7或图8中的虚线所示)。
73.③
若当前切线段在已检查切线集合s
lc
中，则本次切线段处理结束；否则将当前切线段加入到已检查切线集合s
lc
。
74.④
计算与当前切线段冲突的威胁区元素，包括独立威胁区和威胁区群凸包。
75.a)如果当前节点对应转弯圆属于威胁区群凸包，且存在冲突的威胁区元素为该威胁区群凸包(如图6所示)，则按照当前节点的转弯方向沿着凸包的边缘前进，将下一个极威胁区上的切入点加入到可达节点集合s
pr
，并置self_sign为1。
76.b)将剩余的冲突的威胁区元素加入到冲突集合s
cn
中。
77.⑤
若集合s
cn
为空且self_sign为1，则本次切线段处理结束。若切线段l
cg
处理已结束，转至步骤8.1).若集合s
cn
为空且self_sign为0，执行下述步骤8.1).
⑤
.a)；否则转至步骤8.1).
⑥
。
78.a)若当前切线段切入点对应转弯圆圆心与目标位姿点转弯圆圆心相同，则将目标位姿点加入到可达节点集合s
pr
，本次切线段处理结束；否则继续执行下述步骤；
79.b)若当前切线段切入点在可达节点集合s
pr
中，本次切线段处理结束；否则继续执行下述步骤；
80.c)将当前切线段切入点加入到可达节点集合s
pr
中，本次切线段处理结束。
81.⑥
从集合s
cn
中找到判定距离最小的冲突独立威胁区或威胁区群凸包，此即为第一个冲突元素。
82.a)威胁区判定距离规定为：过威胁区圆心o
in
作当前切线段的垂线，垂足设为p
fo
，在计算威胁区的判定距离时，以切线段起点p
cu
到垂足p
fo
的距离计。
83.b)规定威胁区群凸包的判定距离为威胁区群凸包第一个极威胁区的判定距离。
84.⑦
计算当前节点对应转弯圆到第一个冲突元素的外切线段，用当前节点信息和该外切线段信息跳转至步骤8.1).
③
，直至该切线段处理已结束。
85.威胁区群凸包的外切线段计算说明如下：若当前节点的转弯方向为顺(或逆)时针，构造当前节点对应转弯圆到威胁区群凸包所有极威胁区的外切线段，基于当前切线段对所有外切线上的切入点进行to-left判定，排除掉结果为假(或真)的切线，此时剩下的切线偏离方向均为逆(或顺)时针方向，偏离角度最大的外切线即为所求。若威胁区群凸包的偏离角度最大外切线不唯一，选择距离当前节点更远的切入点对应的切线段作为该威胁区群凸包相对于当前节点的外切线段。
86.to-left判定为：通过三角形的有向面积判断点s是否在由点p和q所确定的有向直线的左侧。若是，返回真；否则，返回假。
87.⑧
计算当前节点对应转弯圆到第一个冲突元素的内切线段，如图8所示，用当前节点信息和该内切线段信息跳转至步骤8.1).
③
，直至该切线段处理已结束。
88.威胁区群凸包的内切线段计算说明如下：若当前节点的转弯方向为顺(或逆)时针，构造当前节点对应转弯圆到威胁区群凸包所有极威胁区的内切线段，基于当前切线段对所有内切线上的切入点进行to-left判定，排除掉结果为真(或假)的切线，此时剩下的切线偏离方向均为顺(或逆)时针方向，偏离角度最大的内切线即为所求。若威胁区群凸包的偏离角度最大内切线不唯一，选择距离当前节点更远的切入点对应的切线段作为该威胁区群凸包相对于当前节点的内切线段。
89.⑨
计算当前节点对应转弯圆到所有冲突的独立威胁区或威胁区群凸包的内外切线段，找到最大偏离冲突内外切线。
90.最大偏离冲突内外切线段定义说明如下：比较当前节点对应转弯圆到所有冲突元素之间的内切线和外切线，其中与当前切线段夹角最大的内切线即为最大偏离冲突内切线，与当前切线段夹角最大的外切线即为最大偏离冲突外切线。
91.⑩
用当前节点信息和最大偏离冲突外切线段信息跳转至步骤8.1).
③
，直至该切线段处理已结束。
92.用当前节点信息和最大偏离冲突内切线段信息跳转至步骤8.1).
③
，直至该切线段处理已结束。
93.当前节点的可达节点寻找结束。
94.8.2)针对可达节点进行相应处理，首先判断所有可达节点是否已经在open表中，据此将可达节点分为两类，分别进行如下处理：
95.①
将不在open表中的可达节点加入open表中，并把当前节点设置为它或它们(即，当前节点的可达节点)的父亲节点，计算并记录这些节点的总代价f。
96.②
对于已经在open表中的可达节点，检查从起始位姿点经由当前节点而到达该节点的路径长度是否比原路径(即，上一次更新的路径)长度更小。若是，则将该节点的父亲节
点更新为当前节点，重新计算该节点的代价g和f；若不是，则不对该节点进行操作。
97.步骤9：判断当次搜索是否结束，具体步骤说明如下：
98.1)判断open表是否为空。若是，表明当前场景不存在可行路径，当次搜索结束，转至步骤11；否则，继续执行下述判断；
99.2)判断目标位姿点是否在open表中。若不在，表明当次搜索未结束，转至步骤7；否则，表明路径已经找到，当次搜索结束。
100.步骤10：从目标位姿点沿着父节点回溯到起始位姿点，此即为通过基于dubins路径的a*算法搜索得到的一条路径，保存该路径。
101.步骤11：清空open表和close表。第一次到达本步骤时是以起始、目标位姿点转弯方向为左左进行的，从而在第一次达到本步骤之后起始、目标位姿点转弯方向还剩余三种状态，分别为左右、右右和右左。自此以后每次按照顺序使用接下来的状态初始化起始和目标位姿点，并转至步骤6继续执行。当遍历完所有状态之后，执行步骤12。
102.步骤12：比较所得的四条路径，输出最短路径的信息。
103.本发明实施例提供了一种基于dubins路径的a*算法搜索的飞行器路径动态规划方法，该方法针对圆形威胁区存在重叠的情形，采用以圆为主体的凸包预处理方法，使得飞行器面对威胁区各种空间组合的重叠场景，能够规划出一条较优的可行路径。并且由于引入基本可视图和迭代的方法进行节点扩展，避免了从当前节点到备选节点路径均与其他威胁区冲突导致可能的路径规划失效，同时能够进行高效率地规划。
104.本发明另一实施例，通过仿真对本发明实施例的飞行器路径动态规划方法进行了模拟。
105.如图9所示，仿真是在{(x，y)|0km≤x≤200km，0km≤y≤200km}的二维平面矩形规划空间进行，规划空间中包括圆形威胁区，威胁区可能存在重叠的情形。下述坐标的单位为千米，角度的单位为弧度。
106.步骤s1：飞行器动态路径规划初始数据获取。
107.1)获取飞行器最小转弯约束半径rc＝2.5km。
108.2)获取飞行器起始位姿点的位姿gps(0，0，0)和目标位姿点的位姿gpg(200，200，0)。
109.3)获取威胁区的参数数据：建模为圆形的m＝16个威胁区每个圆形威胁区的圆心和半径(xi，yi，ri)，其中i＝1，2，
…
，m。
110.表1圆形威胁区参数(单位：km)
[0111][0112]
步骤s2：威胁区预处理
[0113]
1)将圆形威胁区(xi，yi，ri)外延安全间距d
safe
＝1km，将威胁区扩展为
[0114]
表2圆形扩展威胁区参数(单位：km)
[0115][0116][0117]
2)针对威胁区中，威胁区圆半径小于飞行器最小转弯约束半径rc的情况，构造半径等于rc的同心辅助圆。经此处理后，威胁区圆记为o1，
…
，om。
[0118]
3)威胁区重叠预处理。
[0119]
表3各威胁区群组成
[0120]
序号构成该威胁区群的威胁区下标1(1，15)2(2，5，10，11，12，14，16)3(3，6)4(7，9)
[0121]
步骤s3：针对威胁区群ai(i＝1，...，4)，参考jarvis步进法，构建以威胁区圆为主体的威胁区群凸包ci(i＝1，...，4)。
[0122]
表4逆时针方向构成凸包的威胁区下标序列
[0123]
序号凸包威胁区下标序列
1{1，15}2{2，14，5，11，12，16}3{3，6}4{7，9}
[0124]
步骤s4：威胁区群凸包的重叠检测与化解。本实例构建威胁区群凸包后不再产生重叠，无需进行重叠化解。
[0125]
步骤s5：建立并清空open表和close表，初始化起始位姿点和目标位姿点的转弯方向均为左。
[0126]
步骤s6：计算起始位姿点代价函数值，并将起始位姿点加入open表。
[0127]
步骤s7：从open表中找到总代价函数值f最小的节点，把它作为当前要处理的节点，简称为当前节点，并将当前节点移至close表中。
[0128]
步骤s8：基于基本可视图和迭代的方法寻找当前节点的可达节点，并对可达节点进行相应的处理。
[0129]
1)当前节点的可达节点寻找，具体方法步骤说明如下：
[0130]
①
设置并初始化可达节点集合s
pr
和已检查切线集合s
lc
、冲突集合s
cn
、标志self_sign为零。
[0131]
②
作当前节点对应转弯圆到终止位姿点对应转弯圆的切线段l
cg
，将切线段l
cg
作为当前切线段。
[0132]
③
若当前切线段在已检查切线集合s
lc
中，则本次切线段处理结束；否则将当前切线段加入到已检查切线集合s
lc
。
[0133]
④
计算与当前切线段冲突的威胁区元素，包括独立威胁区和威胁区群凸包。
[0134]
a)如果当前节点对应转弯圆属于威胁区群凸包，且存在冲突元素为该威胁区群凸包，则按照当前节点的转弯方向沿着凸包的边缘前进，将下一个极威胁区上的切入点加入到可达节点集合s
pr
，并置self_sign为1，参见图6。
[0135]
b)将剩余的冲突威胁区元素加入到冲突集合s
cn
中。
[0136]
⑤
若集合s
cn
为空且self_sign为1，则本次切线段处理结束，若切线段l
cg
处理已结束，转至步骤s8中1).若集合s
cn
为空且self_sign为0，执行下述步骤s8中1).
⑤
.a)；否则转至步骤s8中1).
⑥
。
[0137]
a)若当前切线段切入点对应转弯圆圆心与目标位姿点转弯圆圆心相同，则将目标位姿点加入到可达节点集合s
pr
，本次切线段处理结束；否则继续执行下述步骤；
[0138]
b)若当前切线段切入点在可达节点集合s
pr
中，本次切线段处理结束；否则继续执行下述步骤；
[0139]
c)将当前切线段切入点加入到可达节点集合s
pr
中，本次切线段处理结束。
[0140]
⑥
从集合s
cn
中找到判定距离最小的冲突独立威胁区或威胁区群凸包，此即为第一个冲突元素。
[0141]
⑦
计算当前节点对应转弯圆到第一个冲突元素的外切线段，用当前节点信息和该外切线段信息跳转至步骤s8中1).
③
，直至该切线段处理已结束。
[0142]
⑧
计算当前节点对应转弯圆到第一个冲突元素的内切线段，用当前节点信息和该内切线段信息跳转至步骤s8中1).
③
，直至该切线段处理已结束。
[0143]
⑨
计算当前节点对应转弯圆到所有冲突的独立威胁区或威胁区群凸包的内外切线段，找到最大偏离冲突内外切线，参见图8。
[0144]
⑩
用当前节点信息和最大偏离冲突外切线段信息跳转至步骤s8中1).
③
，直至该切线段处理已结束。
[0145]
用当前节点信息和最大偏离冲突内切线段信息跳转至步骤s8中1).
③
，直至该切线段处理已结束。
[0146]
当前节点的可达节点寻找结束。
[0147]
2)针对可达节点进行相应处理，首先判断所有可达节点是否已经在open表中，据此将可达节点分为两类，分别进行如下处理：
[0148]
①
将不在open表中的可达节点加入open表中，并把当前节点设置为它或它们的父亲，计算并记录这些节点的总代价f。
[0149]
②
对于已经在open表中的可达节点，检查从起始位姿点经由当前节点而到达该节点的路径长度是否比原路径长度更小。若是，则将该节点的父亲更新为当前节点，重新计算该节点的代价g和f；若不是，则不对该节点进行操作。
[0150]
步骤s9：判断当次搜索是否结束，具体步骤说明如下：
[0151]
1)判断open表是否为空。若是，表明当前场景不存在可行路径，当次搜索结束，转至步骤s11；否则，继续执行下述判断；
[0152]
2)判断目标位姿点是否在open表中。若不在，表明当次搜索未结束，转至步骤s7；否则，表明路径已经找到，当次搜索结束。
[0153]
步骤s10：从目标位姿点沿着父节点回溯到起始位姿点，此即为通过基于dubins路径的a*算法搜索得到的一条路径，保存该路径。
[0154]
步骤s11：清空open表和close表。第一次到达本步骤时起始、目标位姿点转弯方向剩余三种状态，分别为左右、右右和右左。自此以后每次按照顺序使用接下来的状态初始化起始和目标位姿点，并转至步骤s6继续执行。当遍历完所有状态之后，执行下一步骤。
[0155]
步骤s12：比较所得的四条路径，输出最短路径的信息。
[0156]
表5四种路径长度信息
[0157]
类型基于lsl基于lsr基于rsr基于rsl长度(km)334.072299.129314.836349.779
[0158]
表6威胁区规避规划路径位姿点数据
[0159]
[0160][0161]
仿真结果如附图9所示，其中粗实线所示的是基于dubins路径的a*算法搜索的飞行器路径动态规划方法生成的路径，细点划线所示的是规划路径对应的起始和目标位姿点之间的dubins路径。威胁区建模为附图9中的圆形以及圆形叠加的区域，通过构建叠加区域的凸包进行预处理，利用基于基本可视图并结合dubins路径的a*算法搜索快速地规划从起始位姿点到目标位姿点规避威胁区的路径。

技术特征：
1.一种飞行器路径动态规划方法，包括：s1，获取初始数据，所述初始数据包括飞行器最小转弯约束半径、起始位姿点和目标位姿点的位姿信息、以及威胁区的参数数据；其中，威胁区的参数数据包括m个威胁区中每个威胁区的圆心和半径，其中，m为大于或等于1的整数；s2，根据所述m个威胁区之间的重叠情况，将所述m个威胁区划分为独立威胁区、被包含威胁区和威胁区群，并输出独立威胁区和威胁区群的信息；其中，所述重叠情况包括内含和相交，所述相交包括直接相交和间接相交；s3，针对s2输出的每个威胁区群，构建每个威胁区群的威胁区群凸包，所述威胁区群凸包为以每个威胁区群中的至少两个威胁区的圆周上的部分圆弧和该至少两个威胁区的外公切线连接而成的封闭区域，其中，所述威胁区群凸包包含了对应威胁区群中的所有威胁区；s4，不断遍历得到的每个威胁区群凸包，将每个威胁区群凸包和与自己具有所述重叠情况的所有的独立威胁区以及其他的威胁区群凸包合并为新的威胁区群凸包，直至威胁区群凸包和/或独立威胁区彼此之间不存在所述重叠情况；s5，在四种初始情况下，分别运用基于dubins路径的a*算法搜索从所述起始位姿点到达所述目标位姿点的路径，以得到四条路径；其中，所述四种初始情况分别为所述飞行器在所述起始位姿点和所述目标位姿点的转弯方向为左左、左右、右右和右左；在所述四条路径中的每条路径中所述飞行器均须绕开s4中产生的威胁区群凸包和/或独立威胁区；以及s6，输出所述四条路径中最短路径的信息。2.根据权利要求1所述的方法，其中，所述s3包括：寻找每个威胁区群中的所有极威胁区；以及由每个威胁区群中的相邻的极威胁区之间的外公切线段和所有极威胁区上位于两个外公切线段之间的最短圆弧，组成每个威胁区群的威胁区群凸包的边界；其中，所述寻找每个威胁区群中的所有极威胁区包括：在每个威胁区群中寻找第一个极威胁区，包括：根据每个威胁区群中所有威胁区圆周上的点的坐标，寻找至少一个维度上坐标值最小的点，将找到的点所在的威胁区确定为所述第一个极威胁区；以所述第一个极威胁区作为初始的当前极威胁区，按照逆时针顺序，通过构造外公切线的方式在每个威胁区群中寻找新的极威胁区，并将每次找到的新的极威胁区更新为当前极威胁区；当更新后的当前极威胁区为所述第一个极威胁区时，确定找到了所有极威胁区，并终止寻找新的极威胁区。3.根据权利要求2所述的方法，其中，所述寻找所述第一个极威胁区包括：分别查找当前威胁区群中所有威胁区的圆周上y坐标最小点，进行比较；若找到唯一的y坐标最小点，则确定该y坐标最小点所在的威胁区为所述第一个极威胁区；若找到的y坐标最小点不唯一，则对找到的y坐标最小点的x坐标进行比较，并以其中x坐标最小点对应的威胁区作为所述第一个极威胁区。4.根据权利要求3所述的方法，其中，所述以所述第一个极威胁区作为初始的当前极威
胁区，按照逆时针顺序，通过构造外公切线的方式在每个威胁区群中寻找新的极威胁区包括：构造当前极威胁区到威胁区群中每个其他威胁区的外公切线，每一条外公切线的切出点与当前极威胁区的圆周上作为所述威胁区群凸包边界的切入点之间构成一段圆弧；若存在唯一的最短圆弧，确定所述唯一的最短圆弧对应的外公切线所切入的威胁区为寻找到的新的极威胁区；以及若最短圆弧不唯一，则以多个最短圆弧对应的外公切线中，最长外公切线段所切入的威胁区，作为寻找到的新的极威胁区。5.根据权利要求1所述的方法，其中，所述s4包括：获取所有的威胁区群凸包与独立威胁区的信息；判定每个威胁区群凸包与独立威胁区是否存在所述重叠情况，具体包括：计算威胁区群凸包c
i
和独立威胁区b
j
的x坐标范围和y坐标范围；当威胁区群凸包c
i
和独立威胁区b
j
的x坐标范围和y坐标范围同时存在交集时，判断独立威胁区b
j
是否与构成威胁区群凸包c
i
的边界的所有切线段存在重叠；若存在重叠，则确定威胁区群凸包c
i
和独立威胁区b
j
存在所述重叠情况；若不存在重叠，判断独立威胁区b
j
的圆心是否在威胁区群凸包c
i
的内部；若是，则确定威胁区群凸包c
i
和独立威胁区b
j
存在所述重叠情况，其中，威胁区群凸包c
i
包含独立威胁区b
j
；否则，确定威胁区群凸包c
i
和独立威胁区b
j
不存在所述重叠情况。6.根据权利要求5所述的方法，其中，所述s4还包括判定两个威胁区群凸包是否存在所述重叠情况，具体包括：计算威胁区群凸包c
i
和c
j
的x坐标范围和y坐标范围；当威胁区群凸包cx和c
j
的x坐标范围和y坐标范围同时存在交集时，判定威胁区群凸包c
i
与威胁区群凸包c
j
中的每个极威胁区是否存在所述重叠情况，得到第一轮判定结果；判定威胁区群凸包c
j
与威胁区群凸包c
i
中的每个极威胁区是否存在所述重叠情况，得到第二轮判定结果；若所述第一轮判定结果和所述第二轮判定结果中均存在相交，则威胁区群凸包c
i
与威胁区群凸包c
j
相交；若所述第一轮判定结果均为包含，则威胁区群凸包c
i
包含威胁区群凸包c
j
；若所述第二轮判定结果均为包含，则威胁区群凸包c
j
包含威胁区群凸包c
i
；若所述第一轮判定结果和/或第二轮判定结果中同时存在包含和不重叠，则威胁区群凸包c
i
与威胁区群凸包c
j
相交；若所述第一轮判定结果与第二轮判定结果均为不重叠，则威胁区群凸包c
i
与威胁区群凸包c
j
不存在所述重叠情况。7.根据权利要求1所述的方法，其中，所述s5中运用基于dubins路径的a*算法搜索从所述起始位姿点到达所述目标位姿点的路径时，代价函数f＝g+h中，g代表所述起始位姿点到当前位姿点的实际路径长度；h代表当前位姿点到所述目标位姿点的预估路径长度，其中，所述预估路径长度为
dubins路径长度。8.根据权利要求1所述的方法，其中，所述s1还包括对威胁区进行预处理，具体包括：将所述m个威胁区中每个威胁区的半径外延安全间距d
safe
，以扩展每个威胁区；以及当扩展后的威胁区中存在半径小于所述飞行器最小转弯约束半径r
c
的极小威胁区时，将所述极小威胁区构造为半径等于r
c
的威胁区。9.一种计算机系统，包括：一个或多个处理器；存储器，用于存储一个或多个程序，其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行权利要求1～8中任一项所述的方法。10.一种计算机可读存储介质，其上存储有计算机程序指令，该计算机程序指令被处理器执行时实现权利要求1～8中任一项所述的方法。

技术总结
本发明提供了一种飞行器路径动态规划方法、计算机系统和介质。该方法包括：根据M个威胁区之间的重叠情况，将M个威胁区划分为独立威胁区、被包含威胁区和威胁区群；然后构建每个威胁区群的威胁区群凸包，并将每个威胁区群凸包和与自己具有重叠情况的所有的独立威胁区以及其他的威胁区群凸包合并为新的威胁区群凸包，直至最终的威胁区群凸包和/或独立威胁区彼此之间不存在重叠情况；然后在四种初始情况下，分别运用基于Dubins路径的A*算法搜索从起始位姿点到达目标位姿点的路径，以得到四条路径；其中，该四种初始情况分别为飞行器在起始位姿点和目标位姿点的转弯方向为左左、左右、右右和右左；最后输出四条路径中最短路径的信息。的信息。的信息。


技术研发人员：谭雁英 廖俊智
受保护的技术使用者：西北工业大学
技术研发日：2023.05.10
技术公布日：2023/8/28