结合人工势场法与A-star算法角度约束的航迹规划方法

结合人工势场法与a-star算法角度约束的航迹规划方法
技术领域
1.本发明涉及航迹规划技术领域，具体涉及结合人工势场法与a-star算法角度约束的航迹规划方法。


背景技术：

2.无人机航迹规划，是指为无人机规划出一条航迹，该航迹应在满足如无人飞行器到达时间、燃料消耗、威胁以及飞行区域等条件的基础上使规划出的航迹最优，或是达到一定的要求，以确保飞行任务的顺利完成。因为无人机应用环境复杂多变，且无人机自身性能存在约束，为了保证完成预定任务，无人机规划航迹应能最大程度上适应环境，避开威胁，提高其安全性，尽可能地减少航程与燃料消耗。
3.按照规划决策可以将路径规划算法分为现代智能算法以及传统经典算法。常用到的现代智能算法有a-star算法、粒子群算法、蚁群算法、遗传算法等，其中a-star算法更为简单高效，但随着搜索空间增大，a-star算法的计算量会呈指数增长，导致规划时间过长。常用的传统经典算法包括dijkstra算法、人工势场法、模拟退火算法，其中人工势场法算法简明、实时性好、规划速度快、易于实现，但该方法在相近的障碍物面前难以发现路径，在狭窄通道内存在摆动现象，且具有局部最优问题。除此之外，在实际的环境中进行航迹规划时，无人飞行器为了躲避威胁区域或者攻击掩体背后的目标，还需要满足从特定飞行方向接近目标的约束，即规划出的航迹要满足端点方向约束，从起点的特定方向出发，并从目标点的威胁规避角度进行攻击。目前在解决具有端点进入角度约束的问题时，一般通过在目标点周围增加高威胁代价区域的方式实现，这样做不仅具有很大的局限性，也增加了航迹规划的时间，降低了规划效率。
4.针对上述问题，本发明提出将人工势场法与改进a-star算法结合分别指导全局路径与局部路径：采用人工势场法进行全局路径规划，无人机的飞行方向由引力场控制，采用改进的a-star算法进行局部路径规划，避让大型障碍物，并且利用双向搜索模型解决端点进入角度的约束。本发明可以通过提高避障效果，缩短寻路时间来优化规划路径、提高规划效率，同时航迹还满足了威胁规避角度约束，更符合实际应用。


技术实现要素：

5.本发明的目的在于克服现有技术的不足，提供结合人工势场法与a-star算法角度约束的航迹规划方法，包括以下步骤：步骤1：根据地图建立航迹坐标系；步骤2：以无人机当前位置为起始点，以目标位置为终止点，获取起始点坐标、终止点坐标，判断地图中障碍物个数是否小于规划威胁阈值，若是，则采用双向搜索模型计算威胁规避角度，无人机沿威胁规避角度的航迹进入目标点，完成航迹规划，若否，则进入步骤3；步骤3：根据人工势场法建立引力场，无人机依据引力场向终止点移动，判断动作
过程中是否存在影响距离超过阈值的障碍物，若是，则进入步骤4，若否，则返回步骤2；步骤4：对地图进行栅格化，确定起始点和终止点，采用改进a-star算法生成航迹路径，无人机沿航迹路径向终止点移动，无人机完成航迹路径后返回步骤2。
6.优选的，所述步骤2中，采用双向搜索模型计算威胁规避角度时，具体包括有以下步骤。
7.步骤201：对双向搜索模型进行参数初始化；步骤202：以起始点指向终止点为正方向，从起始点沿正方向搜索第一航点，搜索路程为第一航迹，同时从终止点沿逆方向搜索第二航点，搜索路程为第二航迹，判断第一航迹、第二航迹是否交汇，若是，则进入步骤203，若否，则重复搜索过程；步骤203：将两段航迹连成具有威胁规避角度的闭合航迹。
8.优选的，所述步骤201中的双向搜索模型为：；为双向搜索模型；为改进后a-star算法的代价函数，为改进后的实际代价函数，为估算代价函数，、、为权重系数，；为角度偏差代价；价；表示沿正方向搜索的当前节点与沿逆方向搜索的当前节点速度方向的夹角，表示沿正方向搜索的当前节点的速度方向与航迹坐标系x轴正半轴的夹角。
9.引入角度偏差代价的主要目的为引导双向搜索尽快闭合，待拓展点的值越小，那么该拓展点被选择的可能性就越大。
10.优选的，所述规划威胁阈值设为三个。
11.优选的，所述人工势场法建立引力场的表达公式为：其中，为无人机所受引力场，为无人机当前坐标，为终止点坐标 ，为障碍物坐标，为引力系数，为障碍物的影响距离阈值；当无人机与障碍物之间的距离小于等于时，则无人机将停止移动，进行避让，否则，无人机将受到人工势场法的引力作用，不断向目标位置移动。
12.优选的，所述改进a-star算法的具体改进方式包括以下步骤。
13.步骤401：引入障碍物扩展策略；步骤402：修改a-star算法中的实际代价函数，生成a-star算法的改进代价函数；步骤403：设定实际代价函数、估算代价函数的权重；步骤404：利用通视性检查方法删除a-star算法生成的航迹路径中的无用点。
14.优选的，所述步骤402中，改进后的实际代价函数和改进后a-star算法的代价函数如下所示：改进后a-star算法的代价函数为：；式中，为改进后的实际代价函数，为估算代价函数，、、为权重系数，；为角度偏差代价；为角度偏差代价；表示沿正方向搜索的当前节点与沿逆方向搜索的当前节点速度方向的夹角，表示沿正方向搜索的当前节点的速度方向与航迹坐标系x轴正半轴的夹角。
15.地图栅格化后，设为无人机当前所在点的坐标，其中，改进后的实际代价函数可表示为：。
16.优选的，所述步骤403中，设定权重后的代价函数如下所示：优选的，所述步骤403中，设定权重后的代价函数如下所示：其中，为自变量，取值范围为。
17.本发明的有益效果是：（1）将人工势场法与改进a-star算法结合分别指导全局路径与局部路径：采用人工势场法进行全局路径规划，无人机的飞行方向由引力场控制，采用改进的a-star算法进行局部路径规划，避让大型障碍物，通过提高避障效果，缩短寻路时间来优化规划路径、提高规划效率。
18.（2）利用双向搜索模型解决了威胁规避角度的约束，更符合实际应用。
附图说明
19.图1显示为结合人工势场法与a-star算法角度约束的航迹规划方法的流程图；图2显示为结合人工势场法与a-star算法角度约束的航迹规划方法的双向搜索模型流程图；图3显示为结合人工势场法与a-star算法角度约束的航迹规划方法的双向搜索模型示意图；图4显示为结合人工势场法与a-star算法角度约束的航迹规划方法的双向搜索模型角度偏差示意图；
图5显示为结合人工势场法与a-star算法角度约束的航迹规划方法的改进的a-star算法流程图。
具体实施方式
20.下面结合本发明的附图1-附图5，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施。
21.在本发明的描述中，需要理解的是，术语“逆时针”、“顺时针”“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
22.以下结合实施例对本发明的特征和性能做进一步的详细描述。
23.结合人工势场法与a-star算法角度约束的航迹规划方法，如图1所示，包括以下步骤：步骤1：根据地图建立航迹坐标系；步骤2：以无人机当前位置为起始点，以目标位置为终止点，获取起始点坐标、终止点坐标，判断地图中障碍物个数是否小于规划威胁阈值，若是，则采用双向搜索模型计算威胁规避角度，无人机沿威胁规避角度的航迹进入目标点，完成航迹规划，若否，则进入步骤3；步骤3：根据人工势场法建立引力场，无人机依据引力场向终止点移动，判断动作过程中是否存在影响距离超过阈值的障碍物，若是，则进入步骤4，若否，则返回步骤2；步骤4：对地图进行栅格化，确定起始点和终止点，采用改进a-star算法生成航迹路径，无人机沿航迹路径向终止点移动，无人机完成航迹路径后返回步骤2。
24.进一步的，所述步骤2中，采用双向搜索模型计算威胁规避角度时，具体包括有以下步骤。如图2所示。
25.步骤201：对双向搜索模型进行参数初始化；步骤202：以起始点指向终止点为正方向，从起始点沿正方向搜索第一航点，搜索路程为第一航迹，同时从终止点沿逆方向搜索第二航点，搜索路程为第二航迹，判断第一航迹、第二航迹是否交汇，若是，则进入步骤203，若否，则重复搜索过程；如图3所示；步骤203：将两段航迹连成具有威胁规避角度的闭合航迹。
26.进一步的，所述步骤201中的双向搜索模型为：；为双向搜索模型；为改进后a-star算法的代价函数，为改进后的实际代价函数，为估算代价函数，、、为权重系数，；为角度偏差代价；
表示沿正方向搜索的当前节点与沿逆方向搜索的当前节点速度方向的夹角，表示沿正方向搜索的当前节点的速度方向与航迹坐标系x轴正半轴的夹角。如图4所示。
27.引入角度偏差代价的主要目的为引导双向搜索尽快闭合，待拓展点的值越小，那么该拓展点被选择的可能性就越大。
28.进一步的，本实施例中优选所述规划威胁阈值设为三个。
29.当地图中障碍物的数量小于三个时，双向搜索模型的规划效率最高，规划出的轨迹能够更加符合无人机在航行过程中的安全性和约束要求，且障碍物半径越小，双向搜索模型的规划效率越高。
30.进一步的，所述人工势场法建立引力场的表达公式为：其中，为无人机所受引力场，为无人机当前坐标，为终止点坐标 ，为障碍物坐标，为引力系数，为障碍物的影响距离阈值；当无人机与障碍物之间的距离小于等于时，则无人机将停止移动，进行避让，否则，无人机将受到人工势场法的引力作用，不断向目标位置移动。
31.进一步的，所述改进a-star算法的具体改进方式包括以下步骤。如图5所示。
32.步骤401：引入障碍物扩展策略；步骤402：修改a-star算法中的实际代价函数，生成a-star算法的改进代价函数；步骤403：设定实际代价函数、估算代价函数的权重；步骤404：利用通视性检查方法删除a-star算法生成的航迹路径中的无用点。
33.在步骤401中，所述障碍物扩展策略为：逐一检查所有栅格区域时，将处于障碍物所在区间的栅格的邻近8个栅格均设为障碍物扩展区域，并为其赋予障碍物栅格一样的属性。
34.在步骤402中，a-star算法的改进代价函数能够使得每个区间的搜索计算量降低，以减少寻路时间，提高算法的搜索效率。
35.在步骤404中，利用通视性检查方法删除a-star算法生成的航迹路径中的无用点时，具体包括以下步骤。
36.步骤4041：改进a-star算法生成航迹路径后，从航迹路径起始点作为当前节点，以起始点指向终止点为正方向，进入步骤4042；步骤4042：从当前节点开始，沿正方向判断航迹路径中任一节点与当前节点间相连的线段是否与障碍物相交，若否，则保留该节点，并删除该节点与当前节点之间的其余节点，并进入步骤4043，若是，则进入步骤4044；步骤4043：判断航迹路径中节点是否均检查完毕，若是，则停止检查，输出检查后的航迹路径，若否，则返回步骤4042；
步骤4044：保留该节点，并将该节点作为当前节点，返回步骤4042。
37.进一步的，所述步骤402中，改进后的实际代价函数和改进后a-star算法的代价函数如下所示：改进后a-star算法的代价函数为：其中，为改进后的实际代价函数，为估算代价函数，、、为权重系数，；为角度偏差代价；为角度偏差代价；表示沿正方向搜索的当前节点与沿逆方向搜索的当前节点速度方向的夹角，表示沿正方向搜索的当前节点的速度方向与航迹坐标系x轴正半轴的夹角。
38.地图栅格化后，设为无人机当前所在点的坐标，其中，改进后的实际代价函数可表示为：其中，通常采用欧氏距离计算得来：通常采用欧氏距离计算得来：是目标点坐标。
39.进一步的，所述步骤403中，设定权重后的代价函数如下所示：进一步的，所述步骤403中，设定权重后的代价函数如下所示：其中，为自变量，取值范围为。
40.的取值探索应该从1开始拓展，经过加权求平均最终取得合适值。