CN113093787A - 一种基于速度场的无人机轨迹规划方法 - Google Patents

Abstract

本发明涉及一种基于速度场的无人机轨迹规划方法，所得初始路径是在A‑star算法的基础上，通过构造距离矩阵、速度矩阵，以及设计速度函数、起终点约束最终实现的。A‑Star算法是一种静态路网中求解最短路最有效的直接搜索方法，该算法综合了最良优先搜索和Dijkstra算法的优点。有益效果如下：兼具A‑Star算法的优点，而且在进行启发式搜索提高算法效率的同时，可以保证找到一条最优路径；通过构造的距离矩阵、速度矩阵，可以确保无人机在障碍物间确定一条适当远离障碍物的路径，使得固定翼飞行器能更加有效、安全地避开禁飞区；通过设计的起终点约束，规划出的航迹更易于后期的轨迹跟踪，满足实际轨迹规划需求。

Description

一种基于速度场的无人机轨迹规划方法

技术领域

本发明属于无人机军事应用技术领域，涉及一种基于速度场的无人机轨迹规划方法。

背景技术

随着无人机技术的不断完善与发展，无人机相关应用在军事和民用生活中发挥着越来越重要的影响。在现代战争方面，高级先进的信息技术是现代战争最重要最突出的特点。与以往战争相比，先进的现代战争不仅注重武器的打击效果，同时更加注重军事人员的生命安全。而无人机在没有人员配备的条件下对指定目标进行打击，从根本上保障了人员的安全，杜绝了伤亡的造成。

在使用过程中，无人机配备了智能设备，能够实时模拟出飞行的环境，确定自身位置，控制自身飞行状态，探测障碍物，并依据载入的轨迹规划算法计算规避路径，从而成功避开障碍物顺利完成任务。无人机轨迹规划的主要内容是在地形环境约束、无人机性能约束以及其他众多威胁约束下，规划出从起始点到目标点的最优安全航迹。通常情况下最优轨迹是指有效躲避外在威胁的最短路径。规划安全轨迹是无人机从起飞点到达目标点的重要保障，也是无人机完成任务的重要前提。轨迹规划作为无人机自主飞行控制研究的重要内容之一，一直都是人们研究的热点。

科学合理地为无人机集群设计轨迹规划算法，对于发挥无人机作战潜能和优势、充分利用无人机资源以及实现预定的作战任务起着关键的作用，成为实现无人机高效作战的首要问题。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于速度场的无人机轨迹规划方法。

技术方案

一种基于速度场的无人机轨迹规划方法，其特征在于步骤如下：

步骤1：将无人机感知设备得到的环境信息栅格化，得到防空区域，且含障碍物区域的信息；将栅格图整体缩小或者放大等价转换成地图矩阵；

步骤2：在地图矩阵上设定起点的信息为(x_S，y_S，v_S，θ_S)，终点的信息为(x_E，y_E，v_E，θ_E)，其中x，y表示该点的横纵坐标，v，θ表示该点的速度与偏航角；

以起点为例，在起点航向两侧构造转弯圆，并视为障碍物区域，转弯圆的圆心连线垂直于航向；

转弯圆的半径以及两侧转弯圆的圆心坐标计算如下：

其中，g为重力加速度，

表示坡度。θ_lef，θ_rig分别表示作左右转弯圆的圆心与起点连线与x轴正向的夹角，x_Slef，y_Slef分别表示左侧圆心的横纵坐标，同理x_Srig，y_Srig表示右侧圆的圆心横纵坐标；

在起点和终点上，再设计三角形障碍物区域，其中两个顶点的坐标分别为两侧转弯圆的圆心，另一个顶点坐标为：

其中，trai_Sx，trai_Sy，trai_Ex，trai_Ey分别表示在起点、终点所构造的三角形的另一个顶点的横纵坐标；

步骤3：根据地图矩阵和步骤2的起终点约束设计，执行如下步骤：

1、构造相同维数的距离矩阵：计算图中各点到最近障碍物的欧氏距离，记录在距离矩阵中；

2、构造相同维数的速度矩阵：利用距离矩阵中的数据，设计速度函数如下：

式中V_MAX表示无人机的最大飞行速度，dis表示该点距离最近障碍物的欧氏距离，ACC表示地图的缩小倍数，e表示自然常数；

步骤4：采用A-star算法在所构造的速度矩阵中寻路；

步骤5：对于A-star算法在速度矩阵中搜索到连接起点和终点的一系列离散点，去掉位于同一线段上的中间点；

步骤6：在安全距离之下，规划出的轨迹点距离最近障碍物的距离需大于安全距离，步骤为：

步骤a、从起点至终点，遍历任意相连三点，记录三点之间欧氏距离的最小值，视为本次处理路径点的标准值；

步骤b、记录所有满足上述标准值的相邻三顶点，连接三点中的首尾点，得到一系列的线段；

步骤c、判断上述线段上的任意一点在距离矩阵中的值是否大于安全距离，若均大于安全距离，则去掉该相邻三点的中间点，否则标记对应的三点，其距离值不可被设为新的标准值；

重复执行步骤a～步骤c,直至任意相邻三点距离值均不被设为标准值，结束。

有益效果

本发明提出的一种基于速度场的无人机轨迹规划方法，所得初始路径是在A-star算法的基础上，通过构造距离矩阵、速度矩阵，以及设计速度函数、起终点约束最终实现的。A-Star算法是一种静态路网中求解最短路最有效的直接搜索方法，该算法综合了最良优先搜索和Dijkstra算法的优点。因此，本方法的有益效果如下：

(1)兼具A-Star算法的优点，而且在进行启发式搜索提高算法效率的同时，可以保证找到一条最优路径(基于评估函数)；

(2)除此之外，通过构造的距离矩阵、速度矩阵，可以确保无人机在障碍物间确定一条适当远离障碍物的路径，使得固定翼飞行器能更加有效、安全地避开禁飞区；

(3)通过设计的起终点约束，规划出的航迹更易于后期的轨迹跟踪，满足实际轨迹规划需求。

附图说明

图1地图矩阵数据的图示。

图2距离矩阵数据的图示。

图3速度矩阵数据的图示。

图4起终点约束设计图示。

图5初始路径规划图示。

图6预处理轨迹点图示。

图7基于安全距离进一步处理轨迹点示意图。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

1前期准备工作

基于无人机携带的各种感知设备、地面站、遥感卫星所传输的信息，将环境栅格化，得到防空区域(障碍物区域)的信息。将栅格图整体缩小或者放大等价转换成地图矩阵，放缩倍数可视情况自行调节，如附图说明图1，描述了一个实例化的二维场景的地图矩阵，实际场景为3km*5km的作战区域，整体缩小20倍，构造150*250维的地图矩阵，图中红色区域表示为提前探测到的障碍物。

2起终点约束设计

传统的航迹规划算法均未考虑起终点的航向约束，导致规划出的路径可能与初始航向或终点航向相悖，不仅造成后端的轨迹优化困难，还可能导致无法进行实际的轨迹跟踪，降低了轨迹规划的有效性。在此算法中，设计起终点约束如下：

假定在二维平面上，起点的信息为(x_S，y_S，v_S，θ_S)，终点的信息为(x_E，y_E，v_E，θ_E)，其中x，y表示该点的横纵坐标，v，θ表示该点的速度与偏航角。以起点为例，在起点航向两侧构造转弯圆，并视为障碍物区域，如附图说明图4所示。转弯圆的圆心连线垂直于航向。转弯圆的半径以及两侧转弯圆的圆心坐标计算公式如下：

其中，g为重力加速度，

表示坡度。θ_lef，θ_rig分别表示作左右转弯圆的圆心与起点连线与x轴正向的夹角，x_slef，y_Stef分别表示左侧圆心的横纵坐标，同理x_Srig，y_Srig表示右侧圆的圆心横纵坐标。注：上述角度均已标准化，即θ∈[0,2π)。

对于起点和终点，再设计三角形障碍物区域，其中两个顶点的坐标分别为两侧转弯圆的圆心，另一个顶点坐标为：

其中，trai_Sx,trai_Sy,trai_Ex,trai_Ey分别表示在起点、终点所构造的三角形的另一个顶点的横纵坐标。

3构造距离矩阵、速度矩阵

基于3.1所构造的地图矩阵以及3.2起终点约束设计，执行如下步骤：

(1)第一步，构造相同维数的距离矩阵：计算图中各点到最近障碍物的欧氏距离，记录在距离矩阵中，如附图说明图2所示，即为上述场景的距离矩阵数据图示；

(2)第二步，构造相同维数的速度矩阵：利用距离矩阵中的数据，设计速度函数如下：

上式中V_MAX表示无人机的最大飞行速度，dis表示该点距离最近障碍物的欧氏距离，ACC表示地图的缩小倍数，e表示自然常数。利用所设计的速度函数，计算各点的速度，记录在速度矩阵中，如附图说明图3所示即为上述场景的速度矩阵数据图示。

4 A-star算法寻路

利用A-star算法在所构造的速度矩阵中寻路。A-star算法为典型的启发式搜索算法。启发式搜索就是在状态空间中的搜索，首先对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无谓的搜索路径，提高了效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。

启发中的估价是用估价函数表示的，如：f(n)＝g(n)+h(n)，其中f(n)是节点n的估价函数，g(n)是在状态空间中从初始节点到n节点的实际代价，h(n)是从n到目标节点最佳路径的估计代价。

在本算法中，g(n)与h(n)数值差异过大，会导致启发式函数效果不明显，轨迹规划效果差。设计g(n)为在速度矩阵上点n到终点的欧式距离，h(n)为从起点到点n的各点速度值累计和。如附图说明图5所示，在速度矩阵中搜索到连接起点和终点的一系列离散路径点。

5路径点预处理

A-star算法在速度矩阵中搜索到连接起点和终点的一系列离散点，大多数点位于同一条线段上。作如下操作：

去掉位于同一线段上的中间点，不仅大大减少了路径点的个数，而且有利于后期的路径优化以及轨迹跟踪。对比附图说明图5、6可得，该处理大大减少了路径点。

6基于安全距离进一步处理路径点

本次仿真实验所用型号无人机后期轨迹跟踪误差约为50米，视为安全距离，为了保障无人机的安全，最终规划出的轨迹点距离最近障碍物的距离需大于安全距离。为了进一步减少搜索到的路径点，同时保证安全，作如下操作：

(1)第一步，从起点至终点，遍历任意相连三点，记录三点之间欧氏距离的最小值，视为本次处理路径点的标准值；

(2)第二步，记录所有满足上述标准值的相邻三顶点，连接三点中的首尾点，得到一系列的线段；

(3)第三步，判断上述线段上的任意一点在距离矩阵中的值是否大于安全距离，若均大于安全距离，则去掉该相邻三点的中间点，否则标记对应的三点，其距离值不可被设为新的标准值；

(4)重复执行(1)、(2)、(3)直至任意相邻三点距离值均不可被设为标准值，结束。

最终结果，参照附图说明图7所示，对比图6、7可得进一步减少了路径点个数，便于后期轨迹优化与跟踪。

Claims (1)

1.一种基于速度场的无人机轨迹规划方法，其特征在于步骤如下：

步骤1：将无人机感知设备得到的环境信息栅格化，得到防空区域，且含障碍物区域的信息；将栅格图整体缩小或者放大等价转换成地图矩阵；

步骤2：在地图矩阵上设定起点的信息为(x_S,y_S,v_S,θ_S)，终点的信息为(x_E,y_E,v_E,θ_E)，其中x，y表示该点的横纵坐标，v，θ表示该点的速度与偏航角；

以起点为例，在起点航向两侧构造转弯圆，并视为障碍物区域，转弯圆的圆心连线垂直于航向；

转弯圆的半径以及两侧转弯圆的圆心坐标计算如下：

其中，g为重力加速度，

表示坡度。θ_lef,θ_rig分别表示作左右转弯圆的圆心与起点连线与x轴正向的夹角，x_Slef,y_Slef分别表示左侧圆心的横纵坐标，同理x_Srig,y_Srig表示右侧圆的圆心横纵坐标；

在起点和终点上，再设计三角形障碍物区域，其中两个顶点的坐标分别为两侧转弯圆的圆心，另一个顶点坐标为：

其中，trai_Sx,trai_Sy,trai_Ex,trai_Ey分别表示在起点、终点所构造的三角形的另一个顶点的横纵坐标；

步骤3：根据地图矩阵和步骤2的起终点约束设计，执行如下步骤：

1、构造相同维数的距离矩阵：计算图中各点到最近障碍物的欧氏距离，记录在距离矩阵中；

2、构造相同维数的速度矩阵：利用距离矩阵中的数据，设计速度函数如下：

式中V_MAX表示无人机的最大飞行速度，dis表示该点距离最近障碍物的欧氏距离，ACC表示地图的缩小倍数，e表示自然常数；

步骤4：采用A-star算法在所构造的速度矩阵中寻路；

步骤5：对于A-star算法在速度矩阵中搜索到连接起点和终点的一系列离散点，去掉位于同一线段上的中间点；

步骤6：在安全距离之下，规划出的轨迹点距离最近障碍物的距离需大于安全距离，步骤为：

步骤a、从起点至终点，遍历任意相连三点，记录三点之间欧氏距离的最小值，视为本次处理路径点的标准值；

步骤b、记录所有满足上述标准值的相邻三顶点，连接三点中的首尾点，得到一系列的线段；

步骤c、判断上述线段上的任意一点在距离矩阵中的值是否大于安全距离，若均大于安全距离，则去掉该相邻三点的中间点，否则标记对应的三点，其距离值不可被设为新的标准值；

重复执行步骤a～步骤c,直至任意相邻三点距离值均不被设为标准值，结束。

Images