[发明专利]基于A*搜索的无人机路径动态规划方法

摘要

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

主权项

1.一种基于A*搜索的无人机路径动态规划方法，其特征在于包括以下步骤：步骤1：无人机动态路径规划初始数据获取与算法参数设置；1.1)获取初始数据，包括获取无人机当前动态路径规划的起始点和目标点坐标以及威胁区域的参数数据；①获取无人机初始位置坐标WP1(xS,yS)和目标位置坐标WPE(xG,yG)；②威胁区域的参数数据包括：建模为圆形的威胁区域个数M，每个圆形威胁区域C_i的圆心和半径(x_i,y_i,r_i)，其中i＝1,2,…,M；建模为多边形的威胁区域个数N,各个多边形威胁区域顶点个数(n₁,n₂,…,n_N)，每个多边形威胁区域P_j顶点坐标集其中j＝1,2,…,N；1.2)设置算法参数；①圆形威胁区域外延安全间距d1，多边形威胁区域外延安全间距d2；圆形威胁区域的外延距离，也即在威胁圆(xi,yi,ri)的基础上生成(xi,yi,(ri+d1))的辅助圆，进而在圆(xi,yi,(ri+d1))上生成具有缓冲安全距离的点；多边形威胁区域的外延距离d2，也即在某个多边形的顶点处生成距离该点为d2的点以备后续算法使用；外延安全间距的大小依据飞机的机动性能、飞行速度条件约束加以选择设置；②设置代价函数权值参数；搜索算法代价函数为f(i)＝w1·g(i)+w2·h(i)；其中，f(i)为节点i的代价函数，g(i)为子起始点到预生成的两步航点总的距离代价，为了得到全局较优的路径，使w1i1,j1与该外包圆对应的多边形威胁区域没有相交，则置P_Flag为0；否则，计算交点与子起始点WP_i1之间的距离，求最短距离d_Pi1，将该多边形威胁区域标识为P_j，同时将该最短距离对应交点所在的多边形边的两个顶点分别标识为置P_Flag为1；b)如果Li1,j1与多个外包圆有交点，进一步判断线段Li1,j1与各个相交的外包圆对应的多边形威胁区域是否相交；具体如下：如果线段Li1,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.3)如果C_Flag为1且P_Flag为1，则表明该线段与圆形威胁区域以及多边形威胁区域都有交点，如果dci1小于等于dPi1，则置P_Flag为0，否则置C_Flag为0；步骤4：基于步骤3标识的威胁圆或多边形进一步生成具有安全缓冲距离的两个外延点；4.1)如果C_Flag为1，则基于标识的圆形威胁区域Ci和子起始点WPi1生成具有安全缓冲距离为d1的两个外延点，具体步骤如下：①构造辅助圆C_i2，该辅助圆的圆心为半径为②通过辅助圆Ci2与圆形威胁区域Ci的两个标准方程作差，求得子起始点WPi1向标识的圆形威胁区域Ci作切线时，两个切点CO1'、CO2'所在的直线Lco1,co2；③求直线Lco1,co2和基于圆形威胁区域Ci生成的外延圆Ci3的两个交点WPi1O1、WPi1O2，WPi1O1即为CO1'点的外延点，WPi1O2即为CO2'点的外延点，上述外延圆Ci3的圆心和半径为(xi,yi,(ri+d1))；4.2)如果P_Flag为1，则基于标识的多边形威胁区域P_j以及顶点生成具有安全缓冲距离为d₂的两个外延点WP_i1O1、WP_i1O2；具体如下：①分别求得顶点对应多边形顶角的角平分线；②分别在对应角平分线所在直线上选取远离对应顶点距离为d2的外延点WPi1O1、WPi1O2；步骤5：如果C_Flag为1或P_Flag为1，则进行外延点是否落入其他威胁区域内部的判断；如果生成的外延点WPi1O1、WPi1O2都处于威胁区域内部，则置INthreat_Flag为1，否则置INthreat_Flag为0；步骤6：如果INthreat_Flag为0，进行外延点是否进入死区的判断与逃离处理，即将生成的外延点与存放在pathclose表中已规划的路径点进行是否重合的判断；若二者重合，生成新的外延点来避免和pathclose中的路径点重合；具体如下：6.1)对某一外延点WPi1OK与pathclose表中已规划的路径点进行是否存在重合的判断，具体步骤如下：①如果WPi1OK不在pathclose中，即没有重合，表明该点没有进入威胁死区，置flag_WPi1为1，否则转入步骤6.1).②；②如果WPi1OK为WPi1O1，则将WPi1O1所对应的多边形的顶点序号减1对应的顶点为基础生成相应的外延点WPi1O0；如果WPi1OK为WPi1O2，则将WPi1O2所对应的多边形的顶点序号加1对应的顶点为基础生成相应的外延点WPi1O3，转入步骤6.1).③；③判断新生成外延点WPi1O0或WPi1O3是否在其他威胁区域内部，同时判断其是否在pathclose中；如果新生成外延点不在其他威胁区域内部同时也不在pathclose中，则置flag_WPi1为1，并标识外延点WPi1O0或WPi1O3，否则置flag_WPi1为0；6.2)经过步骤5的判断，如果只有一个外延点不在威胁区域内部，将该外延点执行步骤6.1)的判断，此时如果flag_WPi1为0，则置dead_flag_WPi1为1；6.3)经过步骤5的判断，如果两个外延点WPi1O1、WPi1O2都不在威胁区域内部，依次对这两个外延点执行步骤6.1)的判断，并且得到每个外延点对应的flag_WPi1；如果两个外延点WPi1O1、WPi1O2得到的flag_WPi1都是0，则置dead_flag_WPi1为1；步骤7：如果dead_flag_WPi1不为1，将步骤6的所有可行外延点依次作为子目标点与子起始点WPi1构成的线段与各威胁区域进行是否相交的判断，具体如下：7.1)如果可行外延点依次作为子目标点与子起始点WPi1构成的线段与各威胁区域都不存在相交，置local_check_flag_WPi1为1；7.2)如果存在相交，依次将与威胁区域相交的外延点作为子目标点，执行步骤3～步骤6；7.3)若新生成的外延点个数大于0，则对新的外延点再次作为子目标点与子起始点WPi1构成的线段与各威胁区域进行是否相交的判断，即执行步骤7.1)～7.2)；7.4)如果子起始点WPi1对应的可行外延点个数为0，则置local_check_flag_WPi1为0；7.5)如果当前所有与威胁区域不相交的子起始点WPi1对应的外延点的个数大于等于Ni1，终止步骤3～步骤6的执行，置local_check_flag_WPi1为1；7.6)如果local_check_flag_WPi1为1，则将子起始点WPi1对应的所有可行外延点存入openpath表中；步骤8：如果local_check_flag_WPi1为1，则将存入openpath表中的所有外延点转存至openpath1表中生成子起始点WPi1的一步外延点集，其外延点个数为NUM_1；依次以存储在openpath1表中的每一个外延点Pstep1_k作为子起始点，以系统的目标点为子目标点，依次执行步骤3～步骤7，如果生成的可行外延点个数不为零，则依次将步骤7生成openpath表中的所有外延点转存至openpath2_k表中，生成子起始点WPi1一步外延点对应的二步外延点集；如果openpath1表中的每一个外延点Pstep1_k作为子起始点，以系统的目标点为子目标点，依次执行步骤3～步骤7，生成的可行外延点个数都为零，则置step2_fail_flag为零；其中，k＝1,2,…,NUM_1；步骤9：如果step2_fail_flag不为零，以子起始点WPi1的存入openpath1表中的一步外延点Pstep1_k、相应的openpath2_k表中第二步外延点以及最终目标点为基础，依据代价函数最小生成子起始点WPi1的下一个路径点WPi1+1(xi1+1,yi1+1)，存入closepath表中，转入步骤11；代价函数为f(i)＝w1·g(i)+w2·h(i)；其中g(i)为子起始点到预生成的两步航点总的距离代价，即为子起始点WPi1到一步外延点的距离与该一步外延点到相应第二步外延点距离之和，h(i)为估计代价，即相应的第二步外延点位置到目标点距离；步骤10：针对从当前子起始点WPi1生成一步外延点个数为零或者一步外延点不为零但第二步外延点总数为零的情形，选用相应的备用点，以该备用点作为子起始点，以目标点作为子目标点，再转入步骤3；备用点选择步骤如下：10.1)如果当前子起始点WPi1是外界输入的系统起始点WP1(xS,yS)，生成备用点的步骤如下：①连接起始点WP1(xS,yS)和目标点WPE(xG,yG)构成线段LS,G；②在线段L_S,G上，生成距离起始点WP₁为R_1,E的点如果点不在威胁区域内部且起始点与该点的连线与所有各威胁区域没有交点，则点即为备用点；否则，以起始点WP₁为圆心，以点WP₁与点为半径左右旋转各60°形成的圆弧段上选择满足不在威胁区域内部且起始点与该点的连线与所有各威胁区域没有交点的点作为备用点；10.2)如果当前子起始点WPi1不是系统起始点WP1(xS,yS)，执行如下步骤：①将当前子起始点WPi1从closepath表中删除；②将WPi1‑1作为当前子起始点，从存储该点的一步外延点的表openpath1中删除WPi1点，然后依据每一个一步外延点生成对应的第二步外延点集，通过求取代价函数最小值选取相应的新的路径点WPi1'；③将WPi1'作为新的子起始点，如果依据航点WPi1'生成的第二步外延点总个数为零且openpath1不为空，则转入步骤10.2).①；④如果openpath1为空，则子起始点进一步回撤，直至回撤到起始点WP1(xS,yS)，执行步骤10.1)；步骤11：判断生成的路径点WPi1+1是否与目标点WPE(xG,yG)重合，如果重合，则规划路径搜索结束，输出规划路径点集closepath表；如果不重合，以生成的路径点WPi1+1为子起始点，以目标点WPE(xG,yG)作为子目标点，转入步骤3。