用于柠檬果园的植保无人机自主避障轨迹规划方法、存储介质及计算机设备

本发明涉及无人机技术，具体涉及一种用于柠檬果园的植保无人机自主避障轨迹规划方法、存储介质及计算机设备。

背景技术：

1、柠檬是具有很高营养和药用价值的绿色水果，到了21世纪后，随着中国人民的生活水平提高，对柠檬的需求量快速增加，这也促使了中国柠檬种植的快速发展。由于人均耕地面积少，中国南方地区的柠檬果树多种植在山地丘陵地带，且果树种植普遍因势利导，采用非结构化种植方式。受地形条件和种植条件限制，地面喷雾机具难以在丘陵山地果园内无障碍通行。目前柠檬果园的植保作业仍以人工喷药为主，工作量繁重，植保作业的强度很大，需要投入大量的人力物力。亟需解决因种植地域非结构化问题，无人机作业过程中的路径避障问题。一般来说，植保无人机喷洒作业时，路径规划通常是离线进行的，通过地面站地图进行路径点的选择，生成一条“π”形状的路径。然而，果园中通常会有些障碍物，如电线杆、路灯杆、高度不一致的树木等。这都会对植保无人机执行路径任务时造成安全威胁。当前大部分关于植保无人机路径规划算法主要集中于对传统路径搜索算法的改进优化，这些研究在算法效率上取得了提升。然而，它们忽视了后端优化，特别是在轨迹光滑性、安全性和动力学可行性上的优化。这会导致轨迹质量不佳以及存在不必要的转弯，难以保证植保无人机的飞行稳定性。

技术实现思路

1、本发明的第一目的是为了克服以上现有技术存在的不足，提供了一种用于柠檬果园的植保无人机自主避障轨迹规划方法。此用于柠檬果园的植保无人机自主避障轨迹规划方法克服了传统方法的缺陷，具有高效、准确和更智能等优点。

2、本发明的第二目的是提供了一种存储介质。

3、本发明的第三目的是提供了一种计算机设备。

4、本发明的目的通过以下的技术方案实现：本用于柠檬果园的植保无人机自主避障轨迹规划方法，包括以下步骤：

5、s1、基于果园的作业范围，确定无人机每个作业路径点及各个作业路径点之间的连接顺序，连接各个作业路径点以生成全局路径；

6、s2、将全局路径拟合成全局b样条轨迹；

7、s3、构建路径引导优化函数，减少b样条轨迹的控制点与几何引导路径采集点的距离，使b样条轨迹贴合几何引导路径，令全局b样条轨迹优化成预热轨迹；

8、s4、构建代价函数，以对预热轨迹进行优化，生成满足要求的自主规避轨迹。

9、在步骤s2和s3之间还存在以下步骤：

10、基于障碍地图进行全局b样条轨迹可行性检测，当检测到预定的轨迹会穿过障碍物时，则在局部地图范围内采用a*算法寻找一条初步的无碰撞路径。

11、步骤s1中，各个作业路径点的确定包括以下步骤：

12、在植保无人机上搭载云台和相机获取柠檬果园的图像，其中云台安装角度为45°～60°之间，云台俯仰角度为75°～90°之间，根据获取图像确定每个路径点的位置。

13、步骤s2中全局b样条轨迹的拟合过程如下：

14、s21、令用于拟合的全局b样条轨迹的控制点的个数为2l，其中1为全局路径的长度，每个控制点的间隔距离为0.5m；

15、s22、令无人机在柠檬果园中每个无人机路径点为di，则全局b样条轨迹的表达式为：

16、

17、其中，n为控制点的数量，i为自然数，pb为全局b样条轨迹的阶数，ti为节点向量，qi为控制点。

18、在步骤s2和s3还包括以下步骤：

19、无人机执行全局b样条轨迹，并基于障碍物地图进行轨迹可行性检测，如果检测到预定轨迹将会穿过障碍物，则在局部地图范围内采用a*算法寻找一条初步的无碰撞路径。

20、判断预定轨迹是否会穿过障碍物的步骤为：

21、定义障碍物上的点为ob(x1,y1,z1)，定义ob到全局b样条轨迹上距离最近点为τi(x2,y2,z2)，ob到τi的距离di为：

22、

23、如果di＜dthr1，其中dthr1为植保无人机的1.5倍轴距dwb，则认为轨迹穿过障碍物。

24、步骤s3包括以下具体步骤：

25、s31、构建轨迹光滑性代价函数fs：

26、

27、其中，line(qi-1qi+1)是全局b样条轨迹控制点qi-1与qi+1连接成的直线，上式表示最小化qi到直线line(qi-1qi+1)之间的距离；

28、s32、令b样条上每个控制点qi在引导路径上都分配一个关联点gi，gi在引导路径上均匀采样，qi与di之间的欧式距离总和为fd，即fd是惩罚引导路径和全局b样条轨迹之间的距离函数：

29、

30、s33、构建路径引导优化函数为：

31、ftotal1＝λ1sfs+λ1dfd；

32、其中，fs是轨迹光滑性代价函数，fd是惩罚引导路径和全局b样条轨迹之间的距离函数；

33、s34、找到qinew的坐标值使得ftotal1达到最小值，确定预热轨迹的表达式为：

34、

35、其中，

36、步骤s4包括以下具体步骤：

37、s41、构建碰撞代价函数fc：

38、

39、

40、其中，d(qi)是避障轨迹的控制点qi到最近柠檬果园障碍物之间的距离，lc(d(qi))是关于d(qi)的分段函数，dthr2是定义的控制点到最近柠檬果园障碍物最小距离阈值；

41、s42、构建无人机速度fv和加速度的代价函数fa：

42、

43、

44、

45、其中，vu、au是代表植保无人机一维的速度和加速度。vmax、amax分别是植保无人机一维的线性最大速度和加速度；

46、s43、构建关于预热轨迹的安全性、光滑性、动力学可行性的代价函数为：

47、ftotal2＝λ2sfs+λ2cfc+λ2d(fv+fa)；

48、其中，fs和fc是轨迹光滑性代价函数和碰撞代价函数，fv和fa是关于无人机速度和加速度的代价函数，λ2s、λ2c和λ2d分别为这三个代价函数的权重；

49、s42、找到q'inew的坐标值使得ftotal2达到最小值，以确定优化后的自主规避轨迹为：

50、

51、存储介质，存储有程序，所述程序被处理器执行时，实现第一目的所述的用于柠檬果园的植保无人机自主避障轨迹规划方法。

52、计算机设备，包括处理器以及用于存储处理器可执行程序的存储器，所述处理器执行存储器存储的程序时，实现第一目的所述的用于柠檬果园的植保无人机自主避障轨迹规划方法。

53、本发明相对于现有技术具有如下的优点：

54、本发明的一种用于柠檬果园的植保无人机自主避障轨迹规划方法保障了植保无人机在柠檬果园安全作业。通过确认全局路径点的位置，生成全局轨迹，并使用b样条曲线对全局轨迹进行拟合，得到易于优化的轨迹，提高了轨迹可优化性，提高轨迹优化效率。根据b样条轨迹性质设计优化函数，使轨迹满足安全性、光滑性、动力学可行性要求。植保无人机执行该轨迹时，安全性和飞行稳定性都会得到保障。