基于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_i1O1、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。