基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法

摘要

本发明公开的基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法，属于无人机航迹规划领域。本发明首先，将SAS算法嵌入到修复式Anytime构架中；然后，通过引入自适应变步长策略缓解基本修复式Anytime稀疏A*算法对规划区域和障碍尺度敏感的问题，并在航迹迭代优化过程中引入存储空间约束策略和双排序准则，自适应地管理节点存储空间和使用不同的代价进行排序，以提高初始可行航迹的生成速度和减少搜索收敛至最优航迹的迭代次数，实现无人机动态可行航迹的快速生成并在允许时间内不断提高航迹最优性。本发明能够实现无人机动态可行航迹的快速生成，并在允许时间内不断提高航迹最优性，具有效率高和鲁棒性强的优点。

说明书

技术领域

本发明涉及一种基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法，属于无人机航迹规划技术领域。

背景技术

在复杂多变的现实环境中，往往存在着突发、移动威胁和机动目标，无人机需要快速对动态环境作出反应，在允许时间范围内即时规划出新的可行航迹以保证无人机飞行安全，同时应尽可能提高航迹最优性以减小无人机消耗和提高任务效能。传统航迹规划算法主要有图搜索算法和智能优化算法等。其中，以稀疏A*算法为代表的启发式图搜索算法因其效率高和鲁棒性强等优点，在工程上应用最为广泛。但上述算法在复杂环境下难以保证在规划时间限度内规划出可行航迹，并且无法充分利用充裕的时间进行航迹优化。为了在给定时间限度内规划出航程较短的可行航迹，需要定制研究Anytime规划算法。

Anytime算法架构可以在规划开始后很短的时间内找到可行航迹，进而在限定的规划时间内不断改善航迹最优性，实现了规划时效性和结果最优性之间的合理权衡，可以有效处理动态航迹规划问题。Anytime算法主要包括权重、窗口和修复式等架构方式，并且这些架构方式多与A*启发式搜索算法相结合。其中，Likhachev等提出的修复式架构相较于其他架构具有更好的计算效率和鲁棒性。该架构通过多次执行加权A*算法，并逐次减小启发项权重以获得更好的规划结果。在每一次迭代过程中，Anytime Repairing A*(ARA*)能够利用以往的扩展信息提高后续搜索效率。

虽然修复式架构具有相对较好的计算性能，但该架构需要依赖很小的启发项权重才能获得最优解，该特点降低了算法节点扩展的效率。并且修复式Anytime构架在与稀疏A*算法结合时存在着计算效率对规划区域和障碍尺度强烈敏感的问题，该问题导致算法对初始可行航迹求解效率和鲁棒性的下降。另外，现有的Anytime A*算法主要针对机器人和机械臂规划问题，无法直接应用于无人机航迹规划，亟待开发面向无人机应用的Anytime航迹规划算法。

发明内容

本发明公开的基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法，要解决的技术问题是实现无人机动态可行航迹的快速生成，并在允许时间内不断提高航迹最优性，具有效率高和鲁棒性强的优点。

本发明公开的基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法，针对动态环境下改善无人机航迹规划算法时效性、可行性和最优性的需求，将Anytime思想与稀疏A*搜索(SAS)相结合提出一种改进修复式Anytime稀疏A*搜索算法(EnhancedAnytime Repairing Sparse A*Search，EAR-SAS)，实现可行航迹的快速生成，并在允许时间内不断提高航迹最优性。首先，将SAS算法嵌入到修复式Anytime构架中。然后，通过引入自适应变步长策略缓解基本修复式Anytime稀疏A*算法(Basic Anytime RepairingSparse A*Search，BAR-SAS)对规划区域和障碍尺度敏感的问题，并在航迹迭代优化过程中引入存储空间约束策略和双排序准则，自适应地管理节点存储空间和使用不同的代价进行排序，以提高初始可行航迹的生成速度和减少搜索收敛至最优航迹的迭代次数，实现无人机动态可行航迹的快速生成并在允许时间内不断提高航迹最优性。

本发明公开的基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法，包括如下步骤：

步骤1：获取无人机飞行环境设定，并获取无人机飞行性能参数。

步骤2：构建航迹规划求解模型。

步骤2具体实现方法如下：

步骤2.1：表示无人机航迹。

将无人机航迹表示为一组节点序列{s_start,s₁,...,s_n-1,G}，其中s_start为起始点，G为目标点，s₁,...,s_n-1为中间待规划的航迹节点。航迹节点的具体形式为一个结构体，其中包含自身位置信息、代价信息、父节点信息，航迹节点具体表示如公式(1)所示。

s＝{x,y,z,x_f,y_f,z_f,g(s),h(s),key(s),ID,ID_f}(1)

其中x,y,z表示该节点的位置坐标，x_f，y_f，z_f表示父节点位置坐标，g(s)表示由起始点到达当前节点的实际代价，h(s)表示当前节点到达目标点的估计代价。key(s)为节点代价，为了解决基本修复式Anytime架构算法收敛时依赖低启发项权重而导致的节点扩展效率较低的问题，加快算法寻找最优解的速度，设计如公式(2)所示的节点代价的双排序准则

其中ε₁，ε₂为启发项权重系数，并且有ε₁＞ε₂≥1。ID为该节点的ID序号，ID_f为父节点的ID序号。

步骤2.2：建立无人机航线规划问题模型的过程中需要考虑无人机约束。

步骤2.2中无人机约束包括以下五个方面：(1)最大转弯角约束；(2)最大爬升/俯冲角约束；(3)最小航段长度约束；(4)最小相对高度约束；(5)禁飞区约束。

步骤3：设置EAR-SAS算法参数，包括权重系数ε₁、ε₂，权重缩减系数Δε，初始扩展步长l，步长缩减值Δl，设置目标点实际代价g(s_goal)＝∞。初始化OPEN表和CLOSED表，将起始节点放入OPEN表，CLOSED表置空。

步骤4：判断启发项权重ε₁是否小于1或计算时间是否超出规划时限。若是，则输出当前航迹，完成无人机动态航迹规划，否则转步骤5；

步骤5：执行EAR-SAS算法，进行无人机动态航迹规划。

步骤5具体实现方法如下：

步骤5.1：判断目标点实际代价g(s_goal)是否大于OPEN表中所有节点key(s,1)的最小值，若是则转步骤4，否则转步骤5.2。

步骤5.2：为了加快算法寻找最优解的速度避免基本修复式框架中的低启发偏好问题，根据步骤2.1中的双排序准则进行节点排序。具体实现方法如下。若g(s)<dis(s,s_goal)/2，其中dis(s,s_goal)表示起始点至终点的欧式距离，将OPEN表中key(s,2)值最小的节点作为当前扩展节点，否则将key(s,1)值最小的节点作为当前扩展节点。将当前扩展节点移出OPEN表并放入CLOSED表中。

步骤5.3：对当前扩展节点进行扩展。EAR-SAS算法将航迹约束条件加入到节点扩展的过程当中。

步骤5.3的具体实现方法如下。

步骤5.3.1：将最小航迹长度、最大转弯角和最大爬升/下滑角约束条件加入到节点扩展的过程当中。根据最小航迹长度、最大转弯角和最大爬升/下滑角约束条件，将EAR-SAS算法中扩展节点限制在一个由四棱锥和球面所包围的区域内。定义一个区域内扩展的扇面为M个，每一个扇面内均匀的扩展N个扇区，则EAR-SAS算法单次扩展节点数为N×M个。

步骤5.3.2：将飞行最小相对高度、禁飞区约束条件加入到节点扩展的过程当中。EAR-SAS算法通过限制节点扩展的方式来满足飞行航迹飞行最小相对高度、禁飞区约束条件。在搜索过程中，若任意一个给定的节点飞行高度超出给定限制范围内，或落入禁飞区，均视为无效节点，不再扩展。

步骤5.4：计算各扩展子节点s’的代价值key(s’)。

步骤5.5：判断各扩展子节点是否已经位于OPEN表或CLOSED表中，若是，转步骤5.6；若否，则将该子节点放入OPEN表中，转步骤5.7。

步骤5.6：若某子节点s’∈OPEN表中节点且g(s’)＞g(s)+c(s,s’)，c(s,s’)表示父节点s到s’的实际代价，则更新OPEN表中s’的key值及其父节点。若s’∈CLOSED且g(s’)＞g(s)+c(s,s’)，则将s’放入INCONS表中。

步骤5.7：为了加快算法搜索到最优解的速度，因此在迭代搜索最优解的过程中加入了OPEN表节点存储空间限制策略，具体实现方法如下：判断OPEN表中节点数量是否大于存储界限，若是，删除key(s,2)值最大的节点，直到OPEN节点数量小于等于存储界限。

步骤5.8：判断s_goal是否已被扩展，若否则不进行操作，返回步骤5.1；若是且当前扩展目标节点代价*g(s_goal)<g(s_goal)，则更新目标节点信息，转步骤6。

步骤6：由目标节点s_goal回溯获得完整航迹。

步骤7：EAR-SAS算法权重参数ε₁、ε₂及扩展步长l更新。

ε₁＝ε₁-Δε，若ε₂＞1，则ε₂＝ε₂-Δε。若步骤5.3中的扩展步长l大于设定的最小扩展步长l_min，则线性减小该扩展步长l。

步骤8：将INCONS表中的节点移入OPEN表中，并根据ε₁，ε₂更新OPEN表中节点的key值。返回步骤4，直至判断启发项权重ε₁小于1或计算时间超出规划时限，则输出当前航迹，完成无人机动态航迹规划。

有益效果：

1、本发明公开的基于改进修复式Anytime稀疏A*算法，将Anytime修复式架构与稀疏A*算法相结合，并针对修复式架构中的缺陷和无人机航迹规划的特点提出改进策略，能够高效解决无人机动态航迹规划问题，快速规划出可行航迹，并随规划时间的增加不断提高航迹质量。

2、本发明公开的公开的基于改进修复式Anytime稀疏A*算法，针对修复式Anytime架构中对规划区域和障碍尺度敏感的问题，通过自适应扩展步长策略提高算法规划动态可行航迹的效率。

3、本发明公开公开的基于改进修复式Anytime稀疏A*算法，针对修复式Anytime架构收敛依赖较低的启发项权重的问题，通过双排序准则和存储空间限制策略提高算法规划动态最优航迹的效率。

4、本发明公开公开的基于改进修复式Anytime稀疏A*算法，由于相较于基本修复式Anytime架构具有更高的规划效率，因此对于无人机动态航迹规划问题具有更强的鲁棒性。

附图说明

图1为本发明所述的基于改进修复式Anytime稀疏A*算法无人机动态航迹规划方法流程图；

图2为EAR-SAS算法节点扩展示意图；

图3为具体实施方式案例一的航迹结果示意图；

图4为具体实施方式案例一的可行航迹规划时间对比箱线图；

图5为具体实施方式案例一的最优航迹规划时间对比箱线图；

图6为具体实施方式案例一的航迹相对代价对比箱线图；

图7为具体实施方式案例二的动态环境示意图；

图8为具体实施方式案例二中EAR-SAS动态航迹规划过程；图8(a)为EAR-SAS算法0s时刻规划的航迹；图8(b)为EAR-SAS算法100s时刻规划的航迹；图8(c)为EAR-SAS算法200s时刻规划的航迹；图8(d)为EAR-SAS算法350s时刻规划的航迹；

图9为具体实施方式案例二中BAR-SAS动态航迹规划过程。图9(a)为BAR-SAS算法0s时刻规划的航迹；图9(b)为BAR-SAS算法100s时刻规划的航迹；图9(c)为BAR-SAS算法200s时刻规划的航迹；图9(d)为BAR-SAS算法350s时刻规划的航迹；

具体实施方式

为了更好的说明本发明的目的与优点，下面通过无人机航迹规划实例，结合附图与表格对本发明做进一步说明，并通过与BAR-SAS算法的蒙特卡洛航迹规划仿真试验结果进行比较，进而说明本发明在计算效率和鲁棒性上的优势。

实施例1：

为了验证所提出的改进策略的有效性，分别使用BAR-SAS算法和本发明提出EAR-SAS算法求解无人机航迹规划问题。仿真硬件为Intel Core i3-2110CPU3.10GHz 4G内存，仿真环境为MATLAB。随机设置100组起始点和目标点，设置每次规划时限为15s，记录每次规划的可行航迹规划时间，最优航迹规划时间以及最终输出的航迹相对代价。

本实施例公开的基于改进修复式Anytime稀疏A*算法，具体实现步骤如下：

步骤1获取无人机飞行环境设定以及无人机飞行性能参数。

在本例中，设置飞行区域为为60km×60km的方形区域，在规划区域左上角和右下角分别随机生成规划起始点和目标点，无人机最小转弯半径为1500m，最大飞行高度1000m，最大转弯角90°。在规划环境内存在的威胁信息如表1所示。

表1威胁信息表

威胁序号威胁坐标(m)威胁半径(m)137000 280005000223000 150004500325000 240004000433000 150004000527000 350004500612000 450004300745000 120004500820000 460004000916500 360004000

步骤2构建无人机航迹规划问题模型。

步骤2.1无人机航迹表示：将无人机航迹表示为一组节点序列{s_start,s₁,...,s_n-1,G}，其中s_start为起始点，G为目标点，s₁,...,s_n-1为中间待规划的航迹节点。航迹节点的具体形式为一个结构体，其中包含了自身位置信息、代价信息、父节点信息等，具体如式(1)所示。

步骤2.2在无人机航迹规划过程中构建考虑无人机机动能力、地形、威胁等约束模型。

步骤3设置EAR-SAS算法参数：ε₁＝3，ε₂＝1.2，Δε＝0.1，初始扩展步长取为最大威胁半径，即l₀＝5000m，Δl＝500m，最小扩展步长为无人机最小转弯半径，l_min＝1500m。设置g(s_goal)＝∞。初始化OPEN表和CLOSED表，将起始节点放入OPEN表，CLOSED表置空。规划时限为20s。扩展过程中横向扩展节点个数M＝7，纵向扩展节点个数N＝3。

步骤4判断启发项权重ε₁是否小于1或计算时间是否大于20s。若是，则输出当前航迹，否则转步骤5；

步骤5.1判断g(s_goal)是否大于OPEN表中所有节点key(s,1)的最小值，若是则转步骤，否则转步骤5.2。

步骤5.2若g(s)<dis(s,s_goal)/2，其中dis(s,s_goal)表示起始点至终点的欧式距离，将OPEN表中key(s,2)值最小的节点作为当前扩展节点，否则将key(s,1)值最小的节点作为当前扩展节点。将当前扩展节点移出OPEN表并放入CLOSED表中。

步骤5.3对当前扩展节点进行扩展。得到M×N个子节点。

步骤5.4计算各扩展子节点s’的代价值key(s’)。

步骤5.5判断各扩展出的子节点是否已经位于OPEN表或CLOSED表中，若否，则将该子节点放入OPEN表中。转步骤5.7。

步骤5.6若某子节点s’∈OPEN表中节点且g(s’)＞g(s)+c(s,s’)，c(s,s’)表示父节点s到s’的实际代价，则更新OPEN表中s’的key值及其父节点。若s’∈CLOSED且g(s’)＞g(s)+c(s,s’)，则将s’放入INCONS表中。

步骤5.7判断OPEN表中节点数量是否大于存储界限，若是，删除key(s,2)值最大的节点，直到OPEN节点数量小于等于存储界限。

步骤5.8判断s_goal是否已被扩展，并且若当前扩展的*g(s_goal)<g(s_goal)，则更新g(s_goal)。返回步骤5.1。

步骤6由s_goal回溯获得完整航迹。

步骤7线性减小ε₁，ε₁＝ε₁-Δε，若ε₂＞1，同时线性减小ε₂，ε₁＝ε₂-Δε。线性减小步骤5.3中的扩展步长，l＝l-Δl。

步骤8将INCONS表中的节点移入OPEN表中，并根据ε₁，ε₂更新OPEN表中节点的key值。返回步骤4。

步骤9记录规划路径，初始可行航迹规划时间，最优航迹规划时间以及最终得到的航迹相对代价，航迹相对代价等于航迹长度除以起始点到目标点的欧式距离。

步骤10是否完成100次的随机起始点终点的航迹规划，若是退出，否则重新生成一组新的起始点和目标点。转步骤1。

利用本实施例所述的基于改进Anytime稀疏A*算法无人机动态航迹规划方法，在进行无人机航迹规划时可以在极短的时间内规划出可行航迹，并能随着规划时间的增长改善规划航迹的质量，在达到规划时限时输出当前找到的最优航迹。为了说明本实施例中改进策略的优势，使用BAR-SAS算法在上述相同的想定下执行100次航迹规划，仿真试验对比结果如图3、图4和图5所示。仿真试验的具体数据如表1所示

表2仿真试验数据

算法平均可行航迹规划时间平均最优航迹规划时平均航迹代价EAR-SAS0.0641s3.4774s1.067BAR-SAS0.3628s6.4958s1.082

由仿真试验可知，在可行航迹规划时间方面，EAR-SAS算法相比于BAR-SAS算法效率提升了近6倍，最优航迹规划时间则是BAR-SAS算法的一半，并且在计算效率大幅提升的同时，EAR-SAS在航迹质量上也具有一定的优势。仿真结果说明：在一定的规划时限内，EAR-SAS算法能够在保证规划航迹最优性的同时，在更短的时间内规划出可行航迹，更快地收敛到当前存在的最优航迹。

实施例2：

为了验证本发明提出的算法能有效处理动态环境下的无人机航迹规划问题，在三维战场环境中设置移动威胁和运动目标进行实时无人机航迹规划仿真测试。并且为了验证动态规划算法的有效性，并且考虑到威胁和目标的连续机动过程，采用滚动动态规划策略，每间隔10s调用EAR-SAS算法完成当前点到运动目标点的动态航迹重规划，并更新当前航迹。仿真硬件为Intel Core i3-2110CPU 3.10GHz 4G内存，仿真环境为MATLAB。

本实施例公开的基于改进修复式Anytime稀疏A*算法，具体实现步骤如下：

步骤1获取无人机飞行环境设定以及无人机飞行性能参数。

在本例中，设置飞行区域为为60km×60km的方形区域，在规划区域左上角和右下角分别随机生成规划起始点和目标点，无人机飞行速度为100m/s，最小转弯半径为1500m，最大飞行高度1000m，最大转弯角90°。在规划环境内存在的威胁信息如表5所示。动态战场环境中，移动威胁与运动目标均以15m/s的速度作匀速运动，目标运动方向随机，移动威胁沿与x轴成夹角45°方向作直线折返移动。

表3威胁信息表

威胁序号威胁类型威胁坐标(m)威胁半径(m)1静态威胁12000,4500043002移动威胁27000,3500045003静态威胁25000,2400040004静态威胁23000,1500045005移动威胁37000,2500050006移动威胁45000,120004500

步骤2构建无人机航迹规划问题模型。其具体方式与案例一步骤二相同。

步骤3设置仿真步长h＝1s，使用EAR-SAS算法规划起始点到目标初始点的航迹(EAR-SAS航迹规划详细流程与案例一中步骤3-8相同)，规划时限为20s，规划完成后开始飞行仿真。

步骤4每个仿真步长下根据无人机飞行速度、威胁移动速度以及运动目标移动速度更新无人机位置和态势环境。

步骤5判断飞行是否飞行至目标点，若是退出仿真，否则继续下一步骤。

步骤6判断仿真时长是否是10s的倍数，若是则根据当前态势调用EAR-SAS算法进行动态航迹规划，规划时限为0.2s，规划起点为无人当前位置，终点为运动目标当前位置。否则转步骤四。

步骤7根据规划结果更新当前无人机航迹。若动态规划失败则继续按原航路飞行。

整个仿真飞行过程持续约680s，当无人机到达目标点5公里范围(5km为无人机侦查范围，当目标进入无人机侦察范围内则转入自动导引阶段)内不再进行规划，因此仿真过程中共调用63次EAR-SAS算法进行动态航迹规划，并全部在规定时限内找到可行较优航迹，无人机飞抵运动目标的过程中成功规避所有威胁。使用BAR-SAS算法替换上诉步骤中的EAR-SAS，进行同样的动态航迹规划仿真试验，整个过程共进行63次动态规划，由于在飞行起始阶段距离目标点较远，算法需要扩展较多的节点，因此在该阶段的20次动态规划均无法在0.2s的时限内找到可行航迹，后续的43次(仿真飞行210s后)动态规划都成功找到可行航迹。并且由于起始阶段出现的规划失败，导致无人机飞抵目标的过程进入了威胁覆盖范围，进入时长为12s，严重降低了成功完成任务的概率。其对比结果如表4所示。仿真试验结果验证了本发明提出的EAR-SAS算法能够满足动态环境下的无人机航迹规划需求，并且相较于BAR-SAS算法具有更强的鲁棒性。两种方法的动态航迹规划仿真过程分别如图8和图9所示。

表4动态航迹规划仿真结果

算法仿真飞行时间动态规划总次数动态规划成功次数进入威胁时长EAR-SAS682s63630sBAR-SAS685s634341s

根据前述的无人机航迹规划实例仿真结果与分析可见，本实施例所述的基于自适应修复式Anytime稀疏A*算法的无人机动态航迹规划方法在航迹规划过程中能够迅速找到可行解并随着规划时间的增加不断优化当前结果，并在限定的规划时刻输出当前找到的最优航迹，具有快速完成无人机动态航迹规划的能力，因此本实施例具有很强的工程实用性，并且能够实现预期的发明目的。

以上的具体描述，是对发明的目的、技术方案和有益效果的进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施实例，仅用于解释本发明，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。