融合A＊算法与动态窗口法的无人机路径规划研究

袁 塘，李廷元

（中国民用航空飞行学院计算机学院，广汉 618307）

0 引言

“十四五”时期，交通运输部将无人机列为未来发展的重要领域，未来将会加速无人机相关技术研发和应用。这几年无人机技术的发展越来越成熟，无人机越来越多地融入到服务和救援任务等领域，提高了工作效率。作为无人机的一项关键技术，路径规划旨在为无人机寻找最短或最优路径，以便利用最少的能量和资源，更加精准、高效地完成更多的任务。

无人机路径规划的目标是生成连接起始状态和目标状态同时满足所需约束的飞行路径，路径的优化、路径的完整性、搜索效率和实现鲁棒性是无人机路径规划的关键障碍。目前常用的搜索算法主要有基于启发式的路径搜索算法、基于采样的搜索算法、势场法、生物启发式算法以及单一算法的不断改进和多种算法的融合。A*算法是当前路径规划算法中使用的最多的算法之一，于1968年提出。该算法具有灵活性强、计算速度快、路径搜索效率高等优点，广泛地用于无人机领域。李晓辉等提出的改良A*算法解决了在有多边形禁飞区情况下的路径规划问题。姜秋月等提出改进的基于栅格法的联合双向搜寻策略的A*算法，提高了路径规划过程中节点的搜寻效率，减少了节点搜索个数，降低了运行时间。Chen J等提出了向量叉积优化的A*算法，搜索范围变得更小，提高了航迹点的搜索效率。上述研究基本上都对A*算法做了一部分的改进，寻路效率都有所提高，但无法避免空间中突如其来的障碍物，未解决路径不平滑的问题，本文将针对这两方面的缺陷做一定的研究。

计算从起飞点到达目的地的安全路径，是无人机能够完成任务所必须要做的工作，正确选择路径点则是无人机路径规划中的重要环节。因此，设计合适的算法对于无人机的路径规划显然非常重要。把A*算法与动态窗口算法融合在一起，在确保无人机飞行的全局路径最优的基础上实现实时动态规划路径，融合算法的好处既提高了算法的效率和路径的平滑性，同时也让算法拥有了良好的局部规划的效果。

1 无人机的运动建模

建立无人机的动力学模型，是路径规划研究的前提基础部分。无人机的位置变化控制转化成速度控制，避障问题转变成空间中的运动约束问题，这样可以根据运动约束条件选择局部最优的路径。动态窗口算法在规划路径时先确定无人机的速度的范围，将无人机模型带入该速度范围，进行轨迹的模拟，使用评价函数对模拟出来的轨迹打分的方式，分数高的轨迹则为最终的最优路径。

1.1 无人机的运动模型

图1 无人机运动模型坐标系

在时刻无人机的线速度表示为，角速度表示为，无人机的速度方向与整体坐标系中轴的夹角为。经过Δ时刻，无人机的位移增量为：

由此可得+1时刻的无人机在整体坐标系中的坐标位置为：

+1时刻无人机速度与整体坐标系轴正向的夹角为：

1.2 速度采样

在假定了无人机的轨迹运动模型之后，通过无人机的速度推算出对应的轨迹，要求采样很多速度，在无人机的速度(,)空间中，有非常多组速度，需要根据无人机自身和障碍物限制将采样速度保持在一定的区间内，即在一个动态窗口内。

（1）受自身飞行的最大速度、最小速度的限制范围：

式中（max）和（min）分别代表无人机所能达到的最大线速度和最小线速度，（max）和（min）分别代表无人机能达到的最大角速度和最小角速度。

（2）无人机受最大加速度、最大减速度的限制范围：

全面预算管理通过预算编制把高校医院预算目标具体化和量化，全部分解落实到各部门、各科室、各环节中去，建立责任中心和责任追究机制，将各个岗位、各个职工的权、责、利进行有机结合，激发全体员工发挥主观能动性，调动全员参与管理的积极性，有利于提高工作效率和管理水平。

式中（max）和（min）分别代表时刻无人机的最大和最小线加速度，（min）和（max）分别代表时刻无人机的最大和最小角加速度。

（3）周边障碍物的限制，确保无人机飞行安全，不能让无人机与障碍物相撞，无人机需要和障碍物保持一定的间隔，在无人机可用的最大减速度情况下，无人机的速度就必须保持在一个可控制的范围内：

公式当中的（，）表示无人机到障碍物的最小间隔。

1.3 评价函数

在上述所得到的采样速度组中，有一部分的轨迹是可用于无人机飞行的，所以可以采用综合评价目标函数的形式，为每一条可行的轨迹信息进行综合评价打分，通过评价函数选择下一时刻中满足约束的最优速度，最优速度用最大（，）表示，评价指标包括方位角、安全距离和速度，评价函数为：

式中、、为权值参数；（，ω）评价的是无人机在当前设定的采样速度下，到达模拟轨迹终点时无人机的朝向和目的地方向的角度差，角度越小，评价得分越高；（，ω）表示无人机在当前行进的轨迹上，与周围的障碍物之间的距离大小，距离越大，评价得分越高；无人机的线速度和角速度用（，ω）表示，速度越大，评价得分越高。

1.4 归一化处理

为防止评价指标中某一项占比过高，在优化之前还需要的步骤是对评价函数的各项做归一化处理：

公式当中的为动态窗口内的轨迹，为当前需要进行评价所对应的轨迹。

2 融合算法路径规划

2.1 A*算法

作为一种采用启发式搜索方法的A*算法，搜索出来的路径必须是在静态状态空间中的，A*算法搜索过程相对其他算法比较直观清晰，它是属于遍历的确定性搜索方式，所以是解决地图环境预知的情况下全局路径搜索问题十分经典的一种算法。A*算法通过下列的公式（14）来计算每个节点所对应的优先级。

公式里面的（）是节点的估价相关函数，选择下一个节点遍历时，需要选取一个综合利用优先级程度最高的节点；（）是在状态空间中重要节点距离起点的代价；（）表示的是估计成本,是A*算法的启发函数。对于网格形式的地图，曼哈顿距离、对角距离和欧几里得距离是启发函数的可以采用的计算距离。本文采用对角距离作为A*算法的启发函数。

2.2 动态窗口算法

动态窗口方法（dynamic window approach，DWA）是应用比较广泛的一种算法，主要用于局部路径规划。DWA通过选择无人机的速度，不断向前搜索使快速到达目标位置，对于空间中存在的对无人机有威胁的障碍物也能够及时的躲避。将无人机的动态特性加入到该方法中，能够找到无人机飞行过程可使用的最佳速度，并且能够把搜索的空间缩小到在动态限制下能采用的速度，为无人机飞行提供保障。

DWA算法原理是在速度空间内采样无人机的线速度和角速度，并按照无人机的运动学模型预测后续飞行的轨迹，对存在多条预测的轨迹评分，最终得到平滑、安全、具有稳定性的最优局部路径。DWA算法的流程如图2所示。

图2 DWA算法流程

2.3 融合算法

A*算法可以得到全局路径规划中的最优路径，在静态工作环境中能很好地完成全局路径规划能力。DWA算法路径规划缺少全局指引，只有目的地一个方向指引，在障碍物较多的环境中容易使无人机的路径规划陷入局部最优，导致路径规划失败。

DWA的优势在于局部的路径规划，检测窗口滚动前进，有非常不错的避障效果，获取局部的最优路径。本文通过采用全局与局部的相结合的方法，将A*与动态窗口的优势融合在一起，确保给无人机规划的路径最优。融合算法流程如图3所示。

图3 融合算法流程

3 仿真实验分析

3.1 仿真环境描述

确保融合算法的可靠性和有效性，使用Matlab R2016a仿真工具为实验平台进行仿真实验。动态窗口法的相关参数设置为：起始的坐标点为（0，0），目标点所在位置为（18，18），初始方位角为π/2，初始线速度为0 m/s，初始角速度0 rad/s，最大线速度1 m/s，最大角速度设置为20 rad/s，最大线加速度设置为为0.2 m/s，最大角加速度设置为50 rad/s，线速度的分辨率为0.01 m/s，角速度的分辨率为1 rad/s，时间分辨率为0.1 s，预测周期间隔为3 s。

3.2 仿真结果分析

从实验结果可以看出，A*算法能够在地图上找到较短的路径，但是路径不平滑，转弯方式是折线转弯，在实际应用中容易导致无人机在障碍物的拐角处与障碍物发生碰撞。动态窗口算法能确保无人机有效的避开障碍物，而且所规划出来的路径与周围的障碍能有一个最小的间隔，不会发生碰撞的情况，但该算法运行时间较长、所规划路径不是最短路径。对比DWA算法与融合算法的线速度图，可以看出融合算法能让无人机的速度保持好的稳定性。融合算法将两者有效的结合，弥补了各自的缺点，在确保全局最优路径的状态下，能够有效地避开障碍物，并保持路径的平滑性，确保无人机在工作飞行过程能够一直保持良好的姿态。

图4 A＊算法路径规划

图5 DWA算法路径规划

图6 A＊与DWA融合算法路径规划

图7 DWA算法线速度

图8 融合算法线速度

表1 算法性能指标对比

4 结语

本文把A*算法和动态窗口算法融合在一起的无人机路径规划的方法，将二者的优势聚集在一起，在确保全局最优可行路径的前提下，进行局部避障，保证了路径的平滑稳定性。仿真实验结果表明，该融合算法切实有效，能够满足无人机的运动约束，可以合理地规划出优良路径，使无人机安全到达目标位置，表明了该融合算法在无人机路径规划中具有可行性。