基于A*搜索的无人机路径动态规划方法与流程

本发明涉及一种无人机路径动态规划方法，特别涉及一种基于A*搜索的无人机路径动态规划方法。

背景技术：

无人机路径规划是在规划区域内，在给定的约束条件下为无人机寻找出一条从起点到终点的最优或可行的路径。无人机飞行过程中当突然出现危险/威胁需要规避或发生任务变更情形时，需要动态生成一条安全可行的路径，路径动态规划问题本质上是一个约束众多的多目标优化问题。

文献“基于Laguerre图的自优化A*无人机航路规划算法，系统工程与电子技术，2015,Vol.37(3),p577-582”公开了一种LA-Star算法用于无人机航路规划。该算法首先将多边形威胁区域和多边形禁飞区采用“外切圆法”简化为二维平面的一组圆；然后采用Laguerre图算法进行航路预规划，在此基础上简化二次规划空间的范围，该简化的规划空间区域的顶点由预规划航路点组成；之后恢复威胁区域和禁飞区的真实形状并使用A*算法在简化的规划空间区域内实施二次航路规划，最后对生成的航路进行自优化处理。

针对复杂环境下存在长条形状的多边形威胁区域或禁飞区情形，上述算法用圆形替代多边形威胁区域或禁飞区与实际形状会存在较大差异，用圆形替代威胁区域生成的预规划航路点构成简化的二次规划空间搜索区域，在此搜索空间基于A*算法进行二次航路规划，该算法二次规划时恢复威胁区域和禁飞区的真实形状，也无法消除一次规划用圆形替代长条形状的多边形威胁区域或禁飞区带来的显著差异，上述算法不适应于存在长条多边形威胁区或禁飞区复杂环境下的路径规划。

技术实现要素：

为了克服现有方法对复杂环境中存在长条形状的多边形威胁区域或禁飞区情形适应性差的不足，本发明提供一种基于A*搜索的无人机路径动态规划方法。该方法首先构造多边形威胁区域或禁飞区的外包圆，判断起始点和目标点构成的线段与各多边形外包圆是否相交。如果该线段与外包圆有相交则进一步判断该线段与多边形威胁区域或禁飞区是否有交点，如果有交点，则基于A*搜索算法规划生成直接规避多边形威胁区域或禁飞区的路径，在该路径规划过程中采用了死区逃离以及两步寻优的策略逐次规划生成各路径点。本发明能够适应于复杂环境中存在长条形状的多边形威胁区或禁飞区情形，同时还能够适应存在圆形或者多边形威胁区域或禁飞区相互重叠的情形。

本发明解决其技术问题所采用的技术方案：一种基于A*搜索的无人机路径动态规划方法，其特点是包括以下步骤：

步骤1：无人机动态路径规划初始数据获取与算法参数设置。

1)获取初始数据，包括获取无人机当前动态路径规划的起始点和目标点坐标以及威胁区域的参数数据。

①获取无人机初始位置坐标WP₁(x_S,y_S)和目标位置坐标WP_E(x_G,y_G)。

②威胁区域的参数数据包括：建模为圆形的威胁区域个数M，每个圆形威胁区域C_i的圆心和半径(x_i,y_i,r_i)，其中i＝1,2,…,M；建模为多边形的威胁区域个数N,各个多边形威胁区域顶点个数(n₁,n₂,…,n_N)，每个多边形威胁区域P_j顶点坐标集其中j＝1,2,…,N。

2)设置算法参数。

①圆形威胁区域外延安全间距d₁，多边形威胁区域外延安全间距d₂。圆形威胁区域的外延距离，也即在威胁圆(x_i,y_i,r_i)的基础上生成(x_i,y_i,(r_i+d₁))的辅助圆，进而在圆(x_i,y_i,(r_i+d₁))上生成具有缓冲安全距离的点；多边形威胁区域的外延距离d₂，也即在某个多边形的顶点处生成距离该点为d₂的点以备后续算法使用。外延安全间距的大小依据飞机的机动性能、飞行速度条件约束加以选择设置。

②设置代价函数权值参数。

搜索算法代价函数为f(i)＝w₁·g(i)+w₂·h(i)。

其中，f(i)为节点i的代价函数，g(i)为子起始点到预生成的两步航点总的距离代价，h(i)为预生成的第二步航点位置到目标点距离的代价。为了得到全局较优的路径，使w₁<w₂，选取w₁＝0.3，w₂＝0.7。

步骤2：分别求各个多边形威胁区域对应的外包圆。

多边形的外包圆也即包含该多边形所有顶点的最小圆。一个多边形外包圆的求取过程如下：

1)计算该多边形所有顶点两两之间的距离，取距离最大值；

2)以该距离最大值线段的中点作为圆心；

3)以该圆心到该多边形各个顶点距离的最大值为半径；

4)以步骤2.2)确定的圆心和步骤2.3)确定的半径形成该多边形的外包圆。

步骤3：连接当前子起始点WP_i1(x_i1,y_i1)和子目标点GP_j1(x_j1,y_j1)构成线段L_i1,j1，判断该线段是否与各个威胁区域有交点。如果该线段和威胁区域无交点，置C_Flag为0且P_Flag为0，如果子目标点GP_j1(x_j1,y_j1)为最终目标点，则转入步骤11。如果该线段和威胁区域有交点，求取距离子起始点WP_i1最近的交点CP_k1，标识交点CP_k1所对应的威胁区域，如果交点CP_k1落在多边形的边上，将交点所在的多边形对应边的两个顶点进行标识。具体步骤如下：

1)将线段L_i1,j1分别与各个圆形威胁区域进行是否有交点的判断，

①如果L_i1,j1只与所有圆形威胁区域中的一个威胁区域有交点，计算交点与子起始点WP_i1之间的距离，求最短距离d_ci1，将该威胁区域标识为C_i,置C_Flag为1；

②如果L_i1,j1和多个圆形威胁区域有交点，计算各交点与子起始点WP_i1之间的距离，求最短距离d_ci1，并将最短距离对应交点所在的威胁区域标识为C_i,置C_Flag为1；

③如果L_i1,j1和所有圆形威胁区域均无交点，置C_Flag为0。

2)将线段L_i1,j1和所有的多边形威胁区域进行是否有交点的判断，

①判断线段L_i1,j1与所有多边形的外包圆是否有交点，如果该线段和所有多边形的外包圆都没有交点，置P_Flag为0；

②如果线段L_i1,j1与外包圆有交点，进一步判断如下：

a)如果L_i1,j1与一个外包圆有交点，进一步判断线段L_i1,j1与该外包圆对应的多边形威胁区域是否相交。

如果线段L_i1,j1与该外包圆对应的多边形威胁区域没有相交，则置P_Flag为0；否则，计算交点与子起始点WP_i1之间的距离，求最短距离d_Pi1，将该多边形威胁区域标识为P_j，同时将该最短距离对应交点所在的多边形边的两个顶点分别标识为置P_Flag为1。

b)如果L_i1,j1与多个外包圆有交点，进一步判断线段L_i1,j1与各个相交的外包圆对应的多边形威胁区域是否相交。具体如下：

如果线段L_i1,j1与所有相交的外包圆对应的多边形均没有交点，置P_Flag为0；

如果线段L_i1,j1与所有相交的外包圆对应的多边形中的一个多边形有交点，计算交点与子起始点WP_i1之间的距离，求最短距离d_Pi1，将该多边形威胁区域标识为P_j，同时将该最短距离对应交点所在的多边形边的两个顶点分别标识为置P_Flag为1；

如果线段L_i1,j1与所有相交的外包圆对应的多边形中的多个多边形有交点，分别计算各个交点与子起始点WP_i1之间的距离，求最短距离d_Pi1，标识该最短距离对应的交点所在的多边形区域P_j以及对应交点所在边的两个顶点为置P_Flag为1。

3)如果C_Flag为1且P_Flag为1，则表明该线段与圆形威胁区域以及多边形威胁区域都有交点，如果d_ci1小于等于d_Pi1，则置P_Flag为0，否则置C_Flag为0。

步骤4：基于步骤3标识的威胁圆或多边形进一步生成具有安全缓冲距离的两个外延点。

1)如果C_Flag为1，则基于标识的圆形威胁区域C_i和子起始点WP_i1生成具有安全缓冲距离为d₁的两个外延点，具体步骤如下：

①构造辅助圆C_i2，该辅助圆的圆心为半径为

②通过辅助圆C_i2与圆形威胁区域C_i的两个标准方程作差，求得子起始点WP_i1向标识的圆形威胁区域C_i作切线时，两个切点CO1'、CO2'所在的直线L_co1,co2；

③求直线L_co1,co2和基于圆形威胁区域C_i生成的外延圆C_i3的两个交点WP_i1 O1、WP_i1O2，WP_i1O1即为CO1'点的外延点，WP_i1O2即为CO2'点的外延点，上述外延圆C_i3的圆心和半径为(x_i,y_i,(r_i+d₁))。

2)如果P_Flag为1，则基于标识的多边形威胁区域P_j以及顶点生成具有安全缓冲距离为d₂的两个外延点WP_i1O1、WP_i1O2。具体如下：

①分别求得顶点对应多边形顶角的角平分线；

②分别在对应角平分线所在直线上选取远离对应顶点距离为d₂的外延点WP_i1O1、WP_i1O2。

步骤5：如果C_Flag为1或P_Flag为1，则进行外延点是否落入其他威胁区域内部的判断。如果生成的外延点WP_i1O1、WP_i1O2都处于威胁区域内部，则置IN_threat_Flag为1，否则置IN_threat_Flag为0。

步骤6：如果IN_threat_Flag为0，进行外延点是否进入死区的判断与逃离处理，即将生成的外延点与存放在pathclose表中已规划的路径点进行是否重合的判断。若二者重合，生成新的外延点来避免和pathclose中的路径点重合。具体如下：

1)对某一外延点WP_i1OK与pathclose表中已规划的路径点进行是否存在重合的判断，具体步骤如下：

①如果WP_i1OK不在pathclose中，即没有重合，表明该点没有进入威胁死区，置flag_WP_i1为1，否则转入步骤6.1).②；

②如果WP_i1OK为WP_i1O1，则将WP_i1O1所对应的多边形的顶点序号减1对应的顶点为基础生成相应的外延点WP_i1O0；如果WP_i1OK为WP_i1O2，则将WP_i1O2所对应的多边形的顶点序号加1对应的顶点为基础生成相应的外延点WP_i1O3，转入步骤6.1).③；

③判断新生成外延点WP_i1O0或WP_i1O3是否在其他威胁区域内部，同时判断其是否在pathclose中。如果新生成外延点不在其他威胁区域内部同时也不在pathclose中，则置flag_WP_i1为1，并标识外延点WP_i1O0或WP_i1O3，否则置flag_WP_i1为0。

2)经过步骤5的判断，如果只有一个外延点不在威胁区域内部，将该外延点执行步骤6.1)的判断，此时如果flag_WP_i1为0，则置dead_flag_WP_i1为1。

3)经过步骤5的判断，如果两个外延点WP_i1O1、WP_i1O2都不在威胁区域内部，依次对这两个外延点执行步骤6.1)的判断，并且得到每个外延点对应的flag_WP_i1。如果两个外延点WP_i1O1、WP_i1O2得到的flag_WP_i1都是0，则置dead_flag_WP_i1为1。

步骤7：如果dead_flag_WP_i1不为1，将步骤6的所有可行外延点依次作为子目标点与子起始点WP_i1构成的线段与各威胁区域进行是否相交的判断，具体如下：

1)如果可行外延点依次作为子目标点与子起始点WP_i1构成的线段与各威胁区域都不存在相交，置local_check_flag_WP_i1为1。

2)如果存在相交，依次将与威胁区域相交的外延点作为子目标点，执行步骤3～步骤6。

3)若新生成的外延点个数大于0，则对新的外延点再次作为子目标点与子起始点WP_i1构成的线段与各威胁区域进行是否相交的判断，即执行步骤7.1)～7.2)。

4)如果子起始点WP_i1对应的可行外延点个数为0，则置local_check_flag_WP_i1为0。

5)如果当前所有与威胁区域不相交的子起始点WP_i1对应的外延点的个数大于等于N_i1，终止步骤3～步骤6的执行，置local_check_flag_WP_i1为1。

6)如果local_check_flag_WP_i1为1，则将子起始点WP_i1对应的所有可行外延点存入openpath表中。

步骤8：如果local_check_flag_WP_i1为1，则将存入openpath表中的所有外延点转存至openpath1表中生成子起始点WP_i1的一步外延点集，其外延点个数为NUM_1。依次以存储在openpath1表中的每一个外延点P_step1__k作为子起始点，以系统的目标点为子目标点，依次执行步骤3～步骤7，如果生成的可行外延点个数不为零，则依次将步骤7生成openpath表中的所有外延点转存至openpath2_k表中，生成子起始点WP_i1一步外延点对应的二步外延点集。

如果openpath1表中的每一个外延点P_step1__k作为子起始点，以系统的目标点为子目标点，依次执行步骤3～步骤7，生成的可行外延点个数都为零，则置step2_fail_flag为零。

其中，k＝1,2,…,NUM_1。

步骤9：如果step2_fail_flag不为零，以子起始点WP_i1的存入openpath1表中的一步外延点P_step1__k、相应的openpath2_k表中第二步外延点以及最终目标点为基础，依据代价函数最小生成子起始点WP_i1的下一个路径点WP_i1+1(x_i1+1,y_i1+1)，存入closepath表中，转入步骤11。

代价函数为f(i)＝w₁·g(i)+w₂·h(i)。

其中g(i)为子起始点WP_i1到一步外延点的距离与该一步外延点到相应第二步外延点距离之和，h(i)为估计代价，即相应的第二步外延点位置到目标点距离。

步骤10：针对从当前子起始点WP_i1生成一步外延点个数为零或者一步外延点不为零但第二步外延点总数为零的情形，选用相应的备用点，以该备用点作为子起始点，以目标点作为子目标点，再转入步骤3。备用点选择步骤如下：

1)如果当前子起始点WP_i1是外界输入的系统起始点WP₁(x_S,y_S)，生成备用点的步骤如下：

①连接起始点WP₁(x_S,y_S)和目标点WP_E(x_G,y_G)构成线段L_S,G；

②在线段L_S,G上，生成距离起始点WP₁为R_1,E的点如果点不在威胁区域内部且起始点与该点的连线与所有各威胁区域没有交点，则点即为备用点；否则，以起始点WP₁为圆心，以点WP₁与点为半径左右旋转各60°形成的圆弧段上选择满足不在威胁区域内部且起始点与该点的连线与所有各威胁区域没有交点的点作为备用点。

2)如果当前子起始点WP_i1不是系统起始点WP₁(x_S,y_S)，执行如下步骤：

①将当前子起始点WP_i1从closepath表中删除；

②将WP_i1-1作为当前子起始点，从存储该点的一步外延点的表openpath1中删除WP_i1点，然后依据每一个一步外延点生成对应的第二步外延点集，通过求取代价函数最小值选取相应的新的路径点WP_i1'；

③将WP_i1'作为新的子起始点，如果依据航点WP_i1'生成的第二步外延点总个数为零且openpath1不为空，则转入步骤10.2).①；

④如果openpath1为空，则子起始点进一步回撤，直至回撤到起始点WP₁(x_S,y_S)，执行步骤10.1)。

步骤11：判断生成的路径点WP_i1+1是否与目标点WP_E(x_G,y_G)重合，如果重合，则规划路径搜索结束，输出规划路径点集closepath表；如果不重合，以生成的路径点WP_i1+1为子起始点，以目标点WP_E(x_G,y_G)作为子目标点，转入步骤3。

本发明的有益效果是：该方法首先构造多边形威胁区域或禁飞区的外包圆，判断起始点和目标点构成的线段与各多边形外包圆是否相交。如果该线段与外包圆有相交则进一步判断该线段与多边形威胁区域或禁飞区是否有交点，如果有交点，则基于A*搜索算法规划生成直接规避多边形威胁区域或禁飞区的路径，在该路径规划过程中采用了死区逃离以及两步寻优的策略逐次规划生成各路径点。本发明能够适应于复杂环境中存在长条形状的多边形威胁区或禁飞区情形，同时还能够适应存在圆形或者多边形威胁区域或禁飞区相互重叠的情形。由于采用了基于A*搜索的无人机路径动态规划方法，使得无人机飞行过程中突然出现危险/威胁或发生任务变更需要执行动态路径规划时，能够有效的适应复杂环境中存在长条形状的多边形威胁区或禁飞区以及存在圆形或者多边形威胁区域或禁飞区相互重叠的情形。

下面结合附图和具体实施方式对本发明作详细说明。

附图说明

图1是本发明基于A*搜索的无人机路径动态规划方法的流程图。

图2是本发明基于A*搜索的无人机路径动态规划方法中线段求交算法原理图。

图3是本发明基于A*搜索的无人机路径动态规划方法中路径点局部检测判断原理图。

图4是本发明基于A*搜索的无人机路径动态规划方法中回撤至起始点时辅助离散点生成图。

图5是本发明基于A*搜索的无人机路径动态规划方法在典型场景下规划生成的路径图。

图6是本发明基于A*搜索的无人机路径动态规划方法在典型场景下预判两步规划一步策略效果图。

具体实施方式

参照图1-6。本发明基于A*搜索的无人机路径动态规划方法具体步骤如下：

仿真是在{(x,y)|-110km≤x≤110km,-90km≤y≤90km}的二维平面矩形规划空间进行，规划空间中包括圆形、多边形以及相互叠加的威胁区域。

步骤1：无人机动态路径规划初始数据获取与算法参数设置。

1)初始数据获取。

①获取无人机初始位置坐标WP₁(50,80)和目标位置坐标WP_E(-110,-90)；

②威胁区域的参数数据包括：建模为圆形的威胁区域个数M＝13，每个圆形威胁区域C_i的圆心和半径(x_i,y_i,r_i)，其中i＝1,2,…,M；建模为多边形的威胁区域个数N＝10,各个多边形威胁区域顶点个数(n₁,n₂,…,n_N)，每个多边形威胁区域P_j顶点坐标集其中j＝1,2,…,N。

表1圆形威胁区域参数(单位：km)

表2各多边形威胁区域顶点个数

表3各多边形威胁区域顶点坐标集

2)设置算法参数。

①圆形威胁区域外延安全间距d₁＝2km,多边形威胁区域外延安全间距d₂＝2km；

②代价函数权值参数设置：w₁＝0.3，w₂＝0.7。

步骤2：分别求所有多边形威胁区域对应的外包圆。

表4外包圆参数(单位：km)

步骤3：连接当前子起始点WP_i1(x_i1,y_i1)和子目标点GP_j1(x_j1,y_j1)构成线段L_i1,j1，判断该线段是否与各个威胁区域有交点。如果该线段和威胁区域无交点，置C_Flag为0且P_Flag为0，如果子目标点GP_j1(x_j1,y_j1)为最终目标点，则转入步骤11。如果该线段和威胁区域有交点，求取距离子起始点WP_i1最近的交点CP_k1，标识交点CP_k1所对应的威胁区域，如果交点CP_k1落在多边形的边上，需要将交点所在的多边形对应边的两个顶点进行标识。

步骤4：基于步骤3标识的威胁区域生成具有一定安全缓冲距离的两个外延点WP_i1O1、WP_i1O2。参照图2。

步骤5：如果C_Flag为1或P_Flag为1，则进行外延点是否落入其他威胁区域内部的判断。如果生成的外延点WP_i1O1、WP_i1O2都处于威胁区域内部，则置IN_threat_Flag为1，否则置IN_threat_Flag为0。

步骤6：如果IN_threat_Flag为0，进行外延点是否进入死区的判断与逃离处理，具体说明如下：

1)对外延点WP_i1OK(即WP_i1O1或WP_i1O2)与pathclose表中已规划的路径点进行是否存在重合的判断，具体步骤如下：

①如果WP_i1OK不在pathclose中，即没有重合，表明该点没有进入威胁死区，置flag_WP_i1为1，否则转入6.1).②；

②如果WP_i1OK为WP_i1O1，则将WP_i1O1所对应的多边形的顶点序号减1对应的顶点为基础生成相应的外延点WP_i1O0；如果WP_i1OK为WP_i1O2，则将WP_i1O2所对应的多边形的顶点序号加1对应的顶点为基础生成相应的外延点WP_i1O3，转入6.1).③；

③判断新生成外延点WP_i1O0或WP_i1O3是否在其他威胁区域内部，同时判断其是否在pathclose中。如果新生成外延点不在其他威胁区域内部同时也不在pathclose中，则置flag_WP_i1为1，并标识外延点WP_i1O0或WP_i1O3，否则置flag_WP_i1为0。

2)经过步骤5的判断，如果只有一个外延点不在威胁区域内部，将该外延点执行步骤6.1)的判断，此时如果flag_WP_i1为0，则置dead_flag_WP_i1为1。

3)经过步骤5的判断，如果两个外延点WP_i1O1、WP_i1O2都不在威胁区域内部，依次对这两个外延点执行步骤6.1)的判断，并且得到每个外延点对应的flag_WP_i1。如果两个外延点WP_i1O1、WP_i1O2得到的flag_WP_i1都是0，则置dead_flag_WP_i1为1。

步骤7：如果dead_flag_WP_i1不为1，将步骤6的所有可行外延点依次作为子目标点与子起始点WP_i1构成的线段与各威胁区域进行是否相交的判断。参照图3。

1)如果可行外延点依次作为子目标点与子起始点WP_i1构成的线段与各威胁区域都不存在相交，置local_check_flag_WP_i1为1。

2)如果存在相交，依次将与威胁区域相交的外延点作为子目标点，执行步骤3～步骤6。

3)若新生成的外延点个数大于0，则对新的外延点再次作为子目标点与子起始点WP_i1构成的线段与各威胁区域进行是否相交的判断，即执行步骤7.1)～7.2)。

4)如果子起始点WP_i1对应的可行外延点个数为0，则置local_check_flag_WP_i1为0。

5)如果当前所有与威胁区域不相交的子起始点WP_i1对应的外延点的个数大于等于N_i1，终止步骤3～步骤6的执行，置local_check_flag_WP_i1为1。

6)如果local_check_flag_WP_i1为1，则将子起始点WP_i1对应的所有可行外延点存入openpath表中。

步骤8：如果local_check_flag_WP_i1为1，则将存入openpath表中的所有外延点转存至openpath1表中生成子起始点WP_i1的一步外延点集，其外延点个数为NUM_1。依次以存储在openpath1表中的每一个外延点P_{step1_k}(k＝1,2,…,NUM_1)作为子起始点，以系统的目标点为子目标点，依次执行步骤3～步骤7，如果生成的可行外延点个数不为零，则依次将步骤7生成openpath表中的所有外延点转存至openpath2_k表中，生成子起始点WP_i1一步外延点对应的二步外延点集。

如果openpath1表中的每一个外延点P_{step1_k}(k＝1,2,…,NUM_1)作为子起始点，以系统的目标点为子目标点，依次执行步骤3～步骤7，生成的可行外延点个数都为零，则置step2_fail_flag为零。

步骤9：如果step2_fail_flag不为零，以子起始点WP_i1的存入openpath1表中的一步外延点P_{step1_k}、相应的openpath2_k表中第二步外延点以及最终目标点为基础，依据代价函数最小生成子起始点WP_i1的下一个路径点WP_i1+1(x_i1+1,y_i1+1)，存入closepath表中，转入步骤11。

步骤10：针对从当前子起始点WP_i1生成一步外延点个数为零或者一步外延点不为零但第二步外延点总数为零的情形，选用相应的备用点，以该备用点作为子起始点，以目标点作为子目标点，再转入步骤3。参照图4。

步骤11：判断生成的路径点WP_i1+1是否与目标点WP_E(-110,-90)重合，如果重合，则规划路径搜索结束，输出规划路径点集closepath表；否则以生成的路径点WP_i1+1为子起始点，以目标点WP_E(-110,-90)作为子目标点，转入步骤3。

表5路径点坐标数据

仿真结果如附图5和附图6所示。其中，附图5是基于A*搜索的无人机路径动态规划算法生成的路径，威胁区域及禁飞区建模为附图5中的圆形/多边形以及相互叠加的区域，通过A*算法规划生成从起始点WP₁到目标点WP_E的规避威胁区域及禁飞区的路径；附图6为基于A*搜索的无人机路径动态规划算法预判两步规划一步策略示意图，图中各路径分支显示了路径生成过程中的多种路径生成可能，最终通过代价函数规划生成附图5中的路径。