一种仿地飞行实时避障方法、系统、设备和介质与流程

本发明属于无人系统导航，具体涉及一种仿地飞行实时避障方法、系统、设备和介质。

背景技术：

1、大型无人机在执行仿地飞行任务时，优秀的规划路线使得飞行时间减短、飞行功耗减低。目前无人机仿地飞行路径规划大致有在避障规划路径基础上匹配dem或借助机载传感器计算相对高度并作出调整，这些方法在平原和丘陵地区适用，在高差较大的山区难以应用。一方面，航线高差变化剧烈时，无人机调整高度的时间长，耗能高；另一方面，规划航线穿过难以逾越的高山时，无人机将陷入困境。即使可以穿越，是采取穿越还是绕行需要定量的评价。

2、现有专利cn115268504b提出了一种大型无人机仿地飞行路径规划方法。该方法基于所述大型无人机仿地飞行的起点和终点，获取所述大型无人机仿地飞行的路径点集；将所述路径点集映射到二维空间平面进行平滑处理，得到大型无人机仿地飞行的最短路径点集；基于所述最短路径点集，得到大型无人机仿地飞行的路径规划。但此方法为全局路径规划方法，在复杂高原地带，仿地飞行时会出现局部山峰点，无人机存在实时监测与避障的风险。

技术实现思路

1、本发明提出一种仿地飞行实时避障方法、系统、设备和介质，以解决现有技术中，仿地飞行时出现的局部山峰点不能及时避开的问题。

2、为达上述目的，本发明提出技术方案如下：

3、一种仿地飞行实时避障方法，包括如下步骤：

4、步骤1，导入任务区域数据、无人机数据和目标航迹点数据；

5、步骤2，基于步骤1中的任务区域数据、无人机数据和目标航迹点数据，对任务区域进行直线离散化和区域离散化，获得任务区域离散点的高程信息，作为数据点；

6、步骤3，通过聚类算法根据高程信息对数据点进行判断，由此确定危险区域，并获得最高点经纬度数据；

7、步骤4，基于危险区域进行无人机路径规划。

8、优选的，所述步骤1中的任务区域数据为任务区域的地形高程数据；

9、所述无人机数据为无人机的位置信息；

10、所述目标航迹点数据为目标航迹点的经纬度信息。

11、优选的，所述步骤2中直线离散化具体步骤为：

12、步骤2.1.1，求解无人机此时位置与目标航迹点的距离：

13、

14、

15、d＝r*ε

16、其中δlat表示无人机此时位置与目标航迹点两个坐标点的纬度差，δlon表示两个坐标点的经度差，lat1表示无人机的纬度，lat2表示目标航迹点的纬度，r表示地球半径，d表示两个坐标点的距离，ε和为中间值，其中，ε为两个坐标点的直线距离，为两个坐标点分别与地球球心连线的夹角；

17、步骤2.1.2，设置的离散精度l，根据离散精度求出两坐标点距离之间最终分解成多少个离散点：

18、

19、其中d表示两个坐标点的距离，l其中表示离散精度，该离散精度l的取值与任务区域有关；η表示离散点个数；

20、步骤2.1.3，计算无人机此时位置与目标航迹点之间的方位角，计算公式如下：

21、α＝sin(lon2_rad-lon1_rad)*cos(lat2_rad)

22、φ＝cos(lat1_rad)*sin(lat2_rad)

23、γ＝sin(lat1_rad)*cos(lat2_rad)*cos(lon2_rad-lon1_rad)

24、β＝φ-γ

25、θ＝atan2(α,β)

26、其中α和β为中间值，θ表示两个坐标点的方位角，lat1_rad和lon1_rad分别表示目标航迹点的纬度和经度，lat2_rad表示当前离散点的纬度，lon2_rad表示当前离散点的经度，ф和γ为中间值；

27、步骤2.1.4，计算每个离散点到无人机起始点的角距离：

28、

29、其中表示每个离散点到达起始点的角距离，di为离散点i到起始点的距离，i＝1，2，3，4…η；

30、步骤2.1.5，根据起始点经纬度坐标、角距离、方位角计算出下一个离散点的经纬度坐标：

31、

32、latn_rad＝asin(sin(lat(n-2)_rad))*cos(δ)+cos(lat(n-2_rad)*sin(δ)*cos(θ)

33、ф＝cos(δ)-sin(lat(n-2)_rad)*sin(latn_rad)

34、γ＝atan2(sin(θ)*sin(δ)*cos(lat(n-2)_rad),ф)

35、latn_rad＝lon(n-2)_rad+γ

36、其中δ表示弧长，lat(n-2)_rad表示离散点n的纬度，lon(n-2)_rad表示离散点n的经度；

37、n＝3,4,5…η；

38、步骤2.1.6，完成直线离散化。

39、优选的，所述步骤2中的区域离散化具体步骤为：

40、步骤2.2，基于直线离散化的结果，进行区域离散化，计算到每个离散点时，转换离散方位角，基于离散精度将左右区域进行分割离散化；

41、步骤2.3，循环计算，直至所有离散点都计算完成，形成局部离散点矩阵；

42、步骤2.4，读取离散点矩阵中每个离散点的高程信息，作为数据点。

43、优选的，所述步骤3具体为：

44、步骤3.1，将比无人机此时更高的数据点作为危险高程点；

45、步骤3.2，计算所有数据点之间的相对距离；

46、步骤3.3，设置点半径和最少点，基于危险高程点与危险高程点以外的数据点之间的相对距离确定危险区域，并获得最高点经纬度数据。

47、优选的，步骤3.3的具体步骤为：

48、设置点半径，针对每个危险高程点判断其点半径内是否存在其他危险高程点，若存在，则该危险高程点的半径范围内为危险区域；

49、设置最少点阈值，针对每个危险高程点判断其点半径内的数据点数量，将点半径内的数据点数量超过最少点阈值的危险高程点作为核心点，通过迭代算法，逐个判断每个危险高程点是否为核心点；

50、判断危险高程点为核心点之后，判断该危险高程点的点半径内其他危险高程点是否为核心点，将不是核心点的危险高程点作为噪音点；

51、将核心点及其点半径内的数据点作为一个集合，将噪音点及其点半径内的数据点作为一个集合，获得若干集合；

52、找到每一个集合中的最高点，形成最高点集合，保存作为最高点经纬度数据。

53、优选的，所述步骤4中规划路径的条件包括：

54、优先避开危险区域；

55、基于最高点经纬度数据，判断能否从高处飞过危险高程点，若能则从高处飞过，若不能则绕开。

56、一种用于实施权利要求一种仿地飞行实时避障方法的仿地飞行实时避障系统，包括信息导入模块、离散化模块、聚类算法模块和路径规划模块；

57、所述信息导入模块用于导入任务区域的数据，无人机的数据和目标航迹点的数据；

58、所述离散化模块用于根据信息导入模块中的信息进行直线离散化和区域离散化，形成无人机飞行区域，并读取对应的高程信息；

59、所述聚类算法模块用于根据高程信息对数据点进行区分，得到危险区域数据；

60、所述路径规划模块用于基于危险区域数据进行无人机的避障路径规划。

61、一种电子设备，包括存储器和处理器；

62、存储器，用于存储计算机程序；

63、处理器，用于执行所述计算机程序，所述计算机程序被处理器执行时实现一种仿地飞行实时避障方法的步骤。

64、一种计算机可读存储介质，

65、所述存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现所述的一种仿地飞行实时避障方法的步骤。

66、本发明的有益之处在于：

67、本发明提出对无人机的任务区域进行直线离散化和区域离散化，实现将任务区域的局部高峰点一并进行数据化，之后获取高程信息，基于高程信息得到危险区域数据，基于此进行任务区域地避障路线进行规划，实现无人机在任务区域的避障飞行。

68、本发明获取飞行路径与区域的高程信息，并基于局部距离计算局部山峰点和危险高程点信息，实现对任务区域的实时监测。