一种基于改进A星算法的空投飞行器路径规划方法及系统

技术领域

本发明涉及飞行器路径规划技术领域，具体涉及一种基于改进A星算法的空投飞行器路径规划方法及系统。

背景技术

子母式飞行器是一种空投飞行器，其由载机搭载或挂载多架子机，利用载机和子机协同作业完成目标任务。在实际任务场景中，载机在目标区域投放子机后继续进行高空监视或执行下一阶段任务；子机体积小，具有更强的环境适应性，适合深入狭窄偏僻、危险性高的区域进行抵近侦察、跟踪监视等任务；此外，子机也可为载机护航，以提升巡航、侦察的安全性。空投飞行器兼具了大型飞行器续航时间长、探测范围广及小型飞行器灵活性好、机动性高的优势，能够适应复杂多变的作业环境，满足远距离、长时间航行任务要求，在人员搜救、故障排查、地形勘探等民用领域也有较高的应用前景。

区别于传统多智能体路径规划问题，空投飞行器路径规划由于存在空投分离的操作过程而具有以下特点：①子机在投放前需挂载在载机上，即载机与子机在分离前的飞行轨迹应当一致；②规划时需要考虑障碍物位置、威胁区域及载机与子机的多个终点目标位置，得出未知且可变的空投分离位置；③在投放子机后，载机与子机各自执行相应任务，对应的飞行路径需要与不同的任务类型匹配。在具体实现中，规划工作需承接感知模块提供的环境信息，根据预先设定的评价方法(如路径长度及平滑度、能量消耗、到障碍物及威胁区域距离等)，规划出一条最优或次最优的路径，并转送到控制模块执行。

目前常用于飞行器路径规划的算法可分为搜索式算法、采样式算法及仿生智能算法等。搜索式算法的特点是将实际的连续地图环境离散成足够精细的栅格等有限空间形式，根据任务需要所构建的代价函数，不断选择和扩展代价值最小的栅格节点，最终完成最优路径的搜索，代表算法有Dijkstra算法、A星算法等；采样式算法的特点是对地图空间进行离散处理，通过可行空间的随机采样并根据提前设定的步长确定下一节点，再基于特定的连接和重连接规则完成各个节点之间路径的构建，代表算法有RRT*算法、Informed RRT*算法、BIT*算法等；仿生智能算法是受生物群体智能行为或生物体结构和功能启发而创造的一类算法，代表算法有遗传算法、蚁群算法、粒子群算法、差分进化算法等。

从算法的特点和适用范围上看，仿生智能类算法具有良好的通用性，能够解决许多不易构建出数学形式的问题。但是由于该类算法收敛速度慢、控制变量多，同时普遍容易陷入局部最优等缺点在实际系统中较少使用。采样类算法主要优势在于高维度构型空间的可行状态搜索，尤其在高自由度机械臂、无人机/无人车载机械臂等一体化系统的路径规划中得到广泛研究。但是由于随机性导致的同一问题下多次规划结果可能存在一定差别等缺点，在模型结构简单的路径规划问题中较少使用。而搜索类算法由于能够保障搜索路径的最优性、完备性、多次规划的一致性以及很高的搜索效率，在大量实际系统中取得了良好的应用效果。

发明内容

为此，本发明提出一种基于改进A星算法的空投飞行器路径规划方法及系统，以搜索类算法中具有代表性和广泛使用的A星算法为基础，改进其算法流程及实现，在规划过程中同时给出分离点与载机及子机各自路径信息，完成空投飞行器路径规划任务。

根据本发明的一方面，提供一种基于改进A星算法的空投飞行器路径规划方法，该方法包括以下步骤：

步骤一、构建子母式飞行器所飞行区域的三维栅格地图，并确定飞行起点和多个子机终点位置；

步骤二、利用改进A星算法获得子母式飞行器中母平台及各个子机的飞行路径，所述飞行路径对应最低的路径代价；具体过程包括：

步骤二一、构建改进A星算法的启发项；

步骤二二、初始化三维栅格地图的栅格节点，设置起点及多个子机终点的栅格节点信息，初始化各个起点、终点搜索分支的开放列表和封闭列表；

步骤二三、对于飞行起点和多个子机目标点，采用正向搜索与反向搜索相结合的方式，将飞行起点或子机目标点自身作为起始节点、其他点作为终点，并以到其他点的距离之和为启发项，建立多条A星算法的搜索分支；通过依次扩展每个搜索分支的开放列表中路径代价最小的节点及其邻接节点进行搜索；

步骤二四、判断各个搜索分支的开放列表是否为空：若为非空，则将开放列表中路径代价值最小的键值对纳入封闭列表，并记录更新各个搜索分支的最新节点；

步骤二五、判断各个搜索分支的封闭列表中是否出现了重合节点：若出现了重合节点，则确定重合节点为一个分离点；以该分离点为起点节点、以搜索分支封闭列表中剩余未交汇的最新节点为目标节点，返回执行步骤二二；若未出现重合节点，则依次将各个搜索分支中当前最新节点的邻接节点及相应路径代价值作为键值对一同纳入开放列表中，返回执行步骤二四；直至目标节点数目为0，表示完成所有路径的搜索；

步骤二六、将各个搜索分支搜索得到的路径拼接，获得包含母平台与各个子机的分离点信息、母平台及各个子机的飞行路径；其中，母平台与各个子机的分离点为多个路径的交点。

进一步地，步骤二一中所述启发项的距离类型采用对角线距离。

进一步地，当子机数量为m时，所述路径代价的函数形式为：

其中，p

进一步地，步骤二三中搜索时选择两种策略之一进行搜索，一种策略对应的启发项为：以当前搜索分支节点到剩余搜索分支封闭列表中最新节点的距离之和；另一种策略对应的启发项为：以当前搜索分支到其他搜索分支各自起点的距离之和。

进一步地，步骤二四中若某个搜索分支的开放列表为空，则表示该搜索分支被障碍物围困，不存在可行路径。

根据本发明的另一方面，提供一种基于改进A星算法的空投飞行器路径规划系统，该系统包括：

三维栅格地图构建模块，其配置成构建子母式飞行器所飞行区域的三维栅格地图，并确定飞行起点和多个子机终点位置；

路径规划模块，其配置成利用改进A星算法获得子母式飞行器中母平台及各个子机的飞行路径，所述飞行路径对应最低的路径代价。

进一步地，所述路径规划模块中利用改进A星算法获得子母式飞行器中母平台及各个子机的飞行路径的具体过程包括：

步骤二一、构建改进A星算法的启发项；所述启发项的距离类型采用对角线距离；

步骤二二、初始化三维栅格地图的栅格节点，设置起点及多个子机终点的栅格节点信息，初始化各个起点、终点搜索分支的开放列表和封闭列表；

步骤二三、对于飞行起点和多个子机目标点，采用正向搜索与反向搜索相结合的方式，将飞行起点或子机目标点自身作为起始节点、其他点作为终点，并以到其他点的距离之和为启发项，建立多条A星算法的搜索分支；通过依次扩展每个搜索分支的开放列表中路径代价最小的节点及其邻接节点进行搜索；

步骤二四、判断各个搜索分支的开放列表是否为空：若为非空，则将开放列表中路径代价值最小的键值对纳入封闭列表，并记录更新各个搜索分支的最新节点；

步骤二五、判断各个搜索分支的封闭列表中是否出现了重合节点：若出现了重合节点，则确定重合节点为一个分离点；以该分离点为起点节点、以搜索分支封闭列表中剩余未交汇的最新节点为目标节点，返回执行步骤二二；若未出现重合节点，则依次将各个搜索分支中当前最新节点的邻接节点及相应路径代价值作为键值对一同纳入开放列表中，返回执行步骤二四；直至目标节点数目为0，表示完成所有路径的搜索；

步骤二六、将各个搜索分支搜索得到的路径拼接，获得包含母平台与各个子机的分离点信息、母平台及各个子机的飞行路径；其中，母平台与各个子机的分离点为多个路径的交点。

进一步地，所述路径规划模块中步骤二三中搜索时选择两种策略之一进行搜索，一种策略对应的启发项为：以当前搜索分支节点到剩余搜索分支封闭列表中最新节点的距离之和；另一种策略对应的启发项为：以当前搜索分支到其他搜索分支各自起点的距离之和。

进一步地，所述路径规划模块中步骤二四中若某个搜索分支的开放列表为空，则表示该搜索分支被障碍物围困，不存在可行路径。

本发明的有益技术效果是：

本发明针对所空投物体具有一定飞行或滑翔能力的空投飞行器使用中独特的路径规划问题，提出了一种基于改进A星算法的空投飞行器路径规划方法及系统；采用正向搜索与反向搜索相结合的方法，通过为每一个起点和目标点构建A星算法搜索分支，并将A星算法中的启发项改进为当前节点到其他搜索分支节点距离之和形式的方法，解决了空投飞行器路径规划中确定空投分离点的问题，实现了同时包含分离点以及子机、载机各自路径信息的空投飞行器路径规划，丰富了空投飞行器的路径规划；在上述改进的基础上，分别提出了两种搜索策略，即以本搜索分支节点到其他搜索分支closeList中的最新节点或者其他搜索分支的起点为启发项。

通过仿真实验发现，以其他搜索分支当前节点为目标时，完成路径搜索的时间更短、但获得的路径长度更长；而以其他搜索分支起点为目标时，完成路径搜索的时间更长、但获得的路径长度更短。在实际应用中，可根据不同的地图环境和算法需求(计算效率优先或路径最短优先)来选择不同搜素策略。

本发明针对空投飞行器需要同时给出分离点以及子机、载机各自路径信息的路径规划问题，在A星算法的基础上进行了改进优化，提出了能有效解决该问题的改进A星算法，在保证无人子机和载机避障效果的同时，有着较高的计算效率，具有较高的工程应用价值。

附图说明

本发明可以通过参考下文中结合附图所给出的描述而得到更好的理解，所述附图连同下面的详细说明一起包含在本说明书中并且形成本说明书的一部分，而且用来进一步举例说明本发明的优选实施例和解释本发明的原理和优点。

图1为本发明实施例基于改进A星算法的空投飞行器路径规划方法的流程图。

图2为本发明实施例中基于改进A星算法的具体算法流程图。

图3为本发明实施例中两种搜索策略的搜索路径俯视对比图。

图4为本发明实施例中两种搜索策略的搜索路径三维对比图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，在下文中将结合附图对本发明的示范性实施方式或实施例进行描述。显然，所描述的实施方式或实施例仅仅是本发明一部分的实施方式或实施例，而不是全部的。基于本发明中的实施方式或实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式或实施例，都应当属于本发明保护的范围。

传统路径规划算法专注于解决从单一起点到唯一确定终点的安全路径搜索问题。而对于空投飞行器的路径规划问题，如何在考虑路径最优情况下，针对多个目标终点，得到子机投放位置的同时确定载机与子机的飞行路径，是本发明待解决的核心问题。

本发明实施例提出一种基于改进A星算法的空投飞行器路径规划方法，该方法包括以下步骤：

步骤一、构建子母式飞行器所飞行区域的三维栅格地图，并确定飞行起点和多个子机终点位置；

步骤二、利用改进A星算法获得子母式飞行器中母平台及各个子机的飞行路径，所述飞行路径对应最低的路径代价；具体过程包括：

步骤二一、构建改进A星算法的启发项；所述启发项的距离类型采用对角线距离；

步骤二二、初始化三维栅格地图的栅格节点，设置起点及多个子机终点的栅格节点信息，初始化各个起点、终点搜索分支的开放列表和封闭列表；

步骤二三、对于飞行起点和多个子机目标点，采用正向搜索与反向搜索相结合的方式，将飞行起点或子机目标点自身作为起始节点、其他点作为终点，并以到其他点的距离之和为启发项，建立多条A星算法的搜索分支；通过依次扩展每个搜索分支的开放列表中路径代价最小的节点及其邻接节点进行搜索；

步骤二四、判断各个搜索分支的开放列表是否为空：若为非空，则将开放列表中路径代价值最小的键值对纳入封闭列表，并记录更新各个搜索分支的最新节点；

步骤二五、判断各个搜索分支的封闭列表中是否出现了重合节点：若出现了重合节点，则确定重合节点为一个分离点；以该分离点为起点节点、以搜索分支封闭列表中剩余未交汇的最新节点为目标节点，返回执行步骤二二；若未出现重合节点，则依次将各个搜索分支中当前最新节点的邻接节点及相应路径代价值作为键值对一同纳入开放列表中，返回执行步骤二四；直至目标节点数目为0，表示完成所有路径的搜索；

步骤二六、将各个搜索分支搜索得到的路径拼接，获得包含母平台与各个子机的分离点信息、母平台及各个子机的飞行路径；其中，母平台与各个子机的分离点为多个路径的交点。

本实施例中，优选地，路径代价的函数形式为：

F(n)＝g(n)+h(n)

其中，F(n)代表第n个节点的总代价函数值；g(n)表示第n个节点到起点节点的距离；h(n)表示启发项，根据两种搜索策略设计两类启发项值(可根据任务需求任选其一)：①当前节点到其他搜索分支最新节点的距离之和；②当前节点到其他分支搜索起点距离之和。

本实施例中，优选地，当子机数量为m时，所述路径代价的函数形式为：

其中，p

本实施例中，优选地，步骤二三中搜索时选择两种策略之一进行搜索，一种策略对应的启发项为：以当前搜索分支节点到剩余搜索分支封闭列表中最新节点的距离之和；另一种策略对应的启发项为：以当前搜索分支到其他搜索分支各自起点的距离之和。

本实施例中，优选地，步骤二四中若某个搜索分支的开放列表为空，则表示该搜索分支被障碍物围困，不存在可行路径。

在传统A星算法的基础上，本发明另一实施例提出了一种基于改进A星算法的空投飞行器路径规划方法，有效实现了对投放分离点和飞行轨迹的确定。如图1、2所示，该方法的具体步骤如下：

步骤一：选取合适栅格大小，构建障碍区域并获取三维栅格地图及障碍物信息，通过手动设定或提前预设方式选定空投飞行器的初始位置以及多个目标终点位置。

根据本发明实施例，地图环境采用预先生成方式给定，规划目标为同时生成分离点位置与载机及子机飞行路径。具体地，地图环境选用三维20m*20m*10m的空间，栅格大小为0.2m，障碍物由随机函数生成。空投飞行器的起点终点信息可以在ROS仿真平台中设定：设置空投飞行器初始位置为坐标原点p

步骤二：确定合适的启发项距离类型，初始化地图范围的栅格节点，设置起点及各终点的栅格节点信息，初始化各个起点、终点搜索分支的开放列表-openList和封闭列表-closeList，并分别将各个起点和终点的代价值和栅格节点作为键值对纳入各个搜索分支的openList中。

根据本发明实施例，启发项的距离类型一般有欧氏距离、曼哈顿距离、对角线距离等形式。其中欧氏距离计算的是起点到终点的最短直线距离，而在栅格地图中往往不能很好的对应到各个栅格节点处，导致启发项数值偏小，搜索过程中扩展更多节点、搜索效率降低；曼哈顿距离只考虑上下左右的邻接节点，更多用于城市网络等井状网格地图中；而对角线距离在栅格扩展时除了上下左右四个方向，还可以走对角线方向，能够更加贴合实际行进的路径、增加搜索效率，因此，本发明实施例中启发项的距离形式采用对角线距离。

由于后续常需要不断调用更少目标点的改进A星算法，应在执行搜索之前重新初始化三维栅格地图的信息，避免搜索中重用之前信息。在具体实现中，为避免搜索中信息互相覆盖，在仿真环境中给每一个起点或终点开始的搜索分支构建一个地图范围的栅格空间。

步骤三：进入改进A星算法搜索的循环主体，改进算法采用正向与反向搜索同时进行的方案，对于每一个路径端点(空投飞行器起点和子机与载机的各自终点)，将自身视为起点并将其他端点视为终点，建立多条A星算法搜索分支；启发项将考虑本搜索分支到其他搜索分支的距离之和，具体实现中可以选择以本搜索分支节点到剩余搜索分支closeList中最新节点的距离之和为启发项，或是本搜索分支到其他搜索分支各自起点的距离之和为启发项(这里的两种方案对应不同搜索策略)。通过依次扩展每个搜索分支的openList中代价函数最小的节点及其邻接节点进行搜索：

每次循环中都需要判断各个搜索分支的openList是否为空：由于A星算法的搜索方式是将openList中的一对值和节点取到closeList，并将该节点的所有邻接节点纳入openList，openList中节点对的数目应当是不断增加的，而openList为空说明该起点或终点的分支被障碍物围困、不存在可行路径，搜索失败，直接退出循环；如果所有搜索分支的openList均为非空，则分别将各个搜索分支openList中代价值最小的键值对纳入closeList，并记录更新各分支的最新节点。

在A星算法中，是通过在openList不断选取代价函数值最小的节点纳入closeList，并扩展相应的邻接节点到openList中进行搜索的，代价函数形式如式(1)所示：

F(n)＝g(n)+h(n) (1)

其中，F(n)代表第n个节点的总代价函数值，g(n)为第n个节点到起点节点的距离，h(n)为第n个节点到终点节点的启发函数值，一般取第n个节点到终点节点的距离，而针对空投飞行器路径规划，h(n)将改进为根据两种搜索策略设计的两类启发项值(可根据任务需求任选其一)：①当前节点到其他搜索分支最新节点的距离之和；②当前节点到其他分支搜索起点距离之和。以载机搭载m架子机为例，代价函数如式(2)所示：

其中，p

步骤四：在循环中判断各个搜索分支的closeList中是否出现了重合节点，分为以下两种情况分析：①当搜索分支的closeList中出现了重合节点时，即确定重合节点为一个分离点。随后以分离点为起点，剩余未交汇搜索分支closeList的最新节点为目标点，根据重合的搜索分支数调用目标点数目更少的改进A星算法；②如果各个搜索分支均未出现重合节点，则依次将各个搜索分支中当前最新节点的邻接节点及相应代价值作为键值对一同纳入openList中。

根据本发明实施例，以起点终点数目是三为例：如果三个closeList中节点的坐标出现了重合，则得到了分离点及分离点到各个起点终点完整的路径，路径搜索成功；而多数情况下，会先出现其中某两个搜索分支closeList中的节点坐标出现重合的情况，则按照以上两种搜索策略进行目标点数目减少一的改进A星算法搜索；而各个搜索分支的closeList中均未出现重合节点时，则依次将各个搜索分支中当前最新节点的邻接节点及相应代价值作为键值对一同纳入openList中。

对于更多目标数的搜索，可以采用同样的思路进行处理：即有某两个搜索分支closeList中的节点坐标出现重合，则调用目标数减少一的改进A星算法搜索；某三个重合则调用目标数减少二等等。需要注意的是一定数量节点重合时，少于该数量的节点重合条件同样满足，应当优先判断更多搜索分支重合，避免重复执行。

步骤五：不断重复步骤二、步骤三、步骤四流程，直至正向与反向搜索路径全部汇合，即视为路径规划成功。随后将每次搜索得到的路径拼接，即完成了包含分离点信息及子机、载机各自路径信息的空投飞行器路径规划。

根据本发明实施例，针对步骤三中提出的两种不同启发项的搜索策略，即以本搜索分支节点到其他搜索分支closeList中的最新节点或者其他搜索分支的起点的距离之和为启发项。

进一步通过实验验证本发明的技术效果。

以本搜索分支最新节点到其他搜索分支closeList中的最新节点的距离之和为启发项所获得的路径如图3中深色栅格点所示，其分离点如图3中浅色三角形所示，搜索获得的路径长度为27m，搜索耗时为45.826025ms；而以本搜索分支最新节点到其他搜索分支起点的距离之和为启发项所获得的路径如图3中浅色栅格点所示，其分离点如图3中深色五边形所示，搜索获得的路径长度为29.2m，搜索耗时为8.863251ms。

通过仿真实验所搜索出的路径、路径长度及搜索耗时可以看出，本发明提出的基于改进A星算法的空投飞行器路径规划方法，能够有效完成空投飞行器路径规划中空投分离位置与载机及子机飞行路径的确定任务，并且能够根据不同的地图环境和算法需求(计算效率优先或路径最短优先)，选择本搜索分支节点到其他搜索分支中的最新节点或其他搜索分支中起点的距离之和为启发项两种搜索策略，有较好的工程应用前景。

本发明另一实施例提出了一种基于改进A星算法的空投飞行器路径规划系统，该系统包括：

三维栅格地图构建模块，其配置成构建子母式飞行器所飞行区域的三维栅格地图，并确定飞行起点和多个子机终点位置；

路径规划模块，其配置成利用改进A星算法获得子母式飞行器中母平台及各个子机的飞行路径，所述飞行路径对应最低的路径代价；具体过程包括：

步骤二一、构建改进A星算法的启发项；所述启发项的距离类型采用对角线距离；

步骤二二、初始化三维栅格地图的栅格节点，设置起点及多个子机终点的栅格节点信息，初始化各个起点、终点搜索分支的开放列表和封闭列表；

步骤二三、对于飞行起点和多个子机目标点，采用正向搜索与反向搜索相结合的方式，将飞行起点或子机目标点自身作为起始节点、其他点作为终点，并以到其他点的距离之和为启发项，建立多条A星算法的搜索分支；通过依次扩展每个搜索分支的开放列表中路径代价最小的节点及其邻接节点进行搜索；

步骤二四、判断各个搜索分支的开放列表是否为空：若为非空，则将开放列表中路径代价值最小的键值对纳入封闭列表，并记录更新各个搜索分支的最新节点；

步骤二五、判断各个搜索分支的封闭列表中是否出现了重合节点：若出现了重合节点，则确定重合节点为一个分离点；以该分离点为起点节点、以搜索分支封闭列表中剩余未交汇的最新节点为目标节点，返回执行步骤二二；若未出现重合节点，则依次将各个搜索分支中当前最新节点的邻接节点及相应路径代价值作为键值对一同纳入开放列表中，返回执行步骤二四；直至目标节点数目为0，表示完成所有路径的搜索；

步骤二六、将各个搜索分支搜索得到的路径拼接，获得包含母平台与各个子机的分离点信息、母平台及各个子机的飞行路径；其中，母平台与各个子机的分离点为多个路径的交点。

本实施例中，可选地，所述路径规划模块中步骤二三中搜索时选择两种策略之一进行搜索，一种策略对应的启发项为：以当前搜索分支节点到剩余搜索分支封闭列表中最新节点的距离之和；另一种策略对应的启发项为：以当前搜索分支到其他搜索分支各自起点的距离之和。

本实施例中，可选地，所述路径规划模块中步骤二四中若某个搜索分支的开放列表为空，则表示该搜索分支被障碍物围困，不存在可行路径。

本实施例所述一种基于改进A星算法的空投飞行器路径规划系统的功能可以由前述一种基于改进A星算法的空投飞行器路径规划方法说明，因此本实施例未详述部分，可参见以上方法实施例，在此不再赘述。

尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的，本发明的范围由所附权利要求书限定。