本发明涉及一种农用植保无人机避障喷施路径规划方法及无人机,属于无人机路线规划方法及控制方法领域。
背景技术:
植保无人机喷施作业的区域一般面积较大,而无人机体积小,反应灵活,所以一般植保无人机喷施的路线被规划成来回往复的形如“Π”的形状,其路线如图3所示,图3(a)是标准的矩形喷施区域,图3(b)是一个不规则的喷施区域,但是他们规划的路线都是来回往复的。这种来回往复的路线在没有障碍的情况下喷施作业的效率和喷洒的均匀度(重喷和漏喷)都特别好。但是在实际应用中这种无任何障碍的无人机喷洒作业几乎不存在。植保无人机在喷施作业中很有可能遇到零散分布的树木,信号塔,电线杆等障碍。这无可避免的会涉及到植保无人机的喷施过程的避障问题。
一般植保无人机在喷施作业中遇到的情况如图4所示(其中圆形代表障碍),在已经离线规划好的路线中分布着若干障碍,这些障碍的特点是比较小,也比较分散,在离线路线规划其中很难将其排除,此时就要求无人机在喷施作业的过程中还要进行障碍的躲避。
以无人机在喷施作业中只遇到一个障碍的情况为例,运用传统的避障算法所得到的避障路线如图5所示。由图5看出,无人机在避障的同时进行喷洒作业,因为无人机在躲避障碍的时候不能停止喷施作业,所以就会导致有些区域喷洒不到农药(如上面的漏喷区域),有些区域会在无人机回飞时喷洒两遍(如上面的重复喷洒区域)。此避障过程会导致大面积的重喷和漏喷,下面就无人机喷施作业中的避障问题展开讨论,并提出一种兼顾路径长短和多喷漏喷的方法。给出一种无人机在喷施作业中的避障策略,这种避障策略可以在路径长度增加不多的情况下,有效的减少多喷和漏喷的面积。图5是以单障碍为例,当遇到多个障碍时,传统的避障算法将会导致更多的重喷和漏喷的面积。
文献“《基于Dubins路径的无人机避障规划算法》,关震宇”提供了一种无人机避障规划方法,但其未排除不需要躲避的障碍,并且不能返回原飞行路线上,从而导致其不能在减少重喷漏喷面积的基础上选择最佳路径。
技术实现要素:
本发明的目的是为了解决现有技术中的避障算法未能兼顾减少路径长短以及减少重喷漏喷的缺点,而提出一种新的用于植保无人机的避障路径规划方法及无人机。
一种用于植保无人机的避障喷施路径长度确定方法,所述方法用于规划从起始点到终点的飞行路径,所述飞行路径包括M条喷施路径以及N条转弯路径,其特征在于,确定第一条喷施路径的方法包括:
步骤1)从第一条直线喷施路径起,将当前飞行路径的起点和终点连接,形成一条用于表示原飞行路线的直线段;
步骤2)判断所述直线段是否与障碍圆相交,若不相交,则按原路线直线飞行,若相交,则执行步骤3);所述障碍圆是用于表示障碍物位置和大小特征的模型;将与所述直线段相交的障碍圆称为目标障碍圆;
步骤3)比较无人机的最小转弯半径与所述障碍圆的半径大小;
若障碍圆的半径大于等于无人机的最小转弯半径,则执行步骤4);
若障碍圆的半径小于无人机的最小转弯半径,则生成两个所述障碍圆的内切圆,以内切圆代替所述障碍圆,然后执行步骤4);所述内切圆的圆心位于所述直线段的经过所述障碍圆圆心的垂线上,所述内切圆的半径等于所述最小转弯圆的半径;
步骤4)生成复数个与所述直线段与所述障碍圆同时相切的最小转弯圆;所述最小转弯圆的半径为无人机的最小转弯半径;
步骤5)根据所述直线段、所述目标障碍圆以及所述最小转弯圆确定复数个可选的飞行路径并计算路径的长度以及与所述的可选飞行路径对应的重喷漏喷面积,并将数据进行归一化处理;
步骤6)将所述的复数个可选路径作为初始种群,计算出适应值的大小;
步骤7)使用遗传算法的选择、交叉、变异操作,当满足迭代次数或者满足预定的适应值大小条件时,输出最佳的搜索路径。
步骤8)计算无人机从当前喷施路径的终点飞行到下一条喷施路径的起点所需经过的转弯路径的长度;
步骤9):重复执行步骤1)至步骤8),通过每一次执行得到的计算结果计算出总的飞行路径长度以及重喷漏喷面积。
本发明还包括一种无人机,包括:
路径存储装置,用于预先存储已规划完成的预定飞行路径信息;
障碍检测装置,用于实时检测周围的障碍物信息,并预测是否会与障碍物碰撞;
判断装置,用于在障碍检测模块预测会与障碍物碰撞时,发出避障信号;
避障喷施路径确定装置,用于在未接收到避障信号时,从所述路径存储模块中读取所 述预定飞行路径信息,并发送至飞行控制装置;还用于在接收到避障信号时,实现如权利要求7或8中任意一项装置所述的功能,并将所述最佳路径信息发送至飞行控制装置;
飞行控制装置,用于根据接收到的路径信息控制无人机进行飞行。
本发明的有益效果为:使用Dubins路径算法生成的避障路径相比较于现有技术的避障算法更符合无人机的飞行特性;使用起点至终点的连线来排除不需要躲避的障碍,在多障碍喷施路径选择时采用启发式的遗传算法搜索路径搜索最优路径时用时更少;在已知障碍的情况下,相比较现有技术可以大大减少重喷漏喷的面积,一般可以减少200%至400%。
附图说明
图1为本发明的用于植保无人机的避障路径规划方法的流程图;
图2为本发明的无人机的方框原理图;
图3(a)为无人机在无障碍的矩形区域进行喷施的路线示意图;
图3(b)为无人机在无障碍的不规则区域进行喷施的路线示意图;
图4(a)为无人机在有障碍的矩形区域进行喷施的路线示意图;
图4(b)为无人机在有障碍的不规则区域进行喷施的路线示意图;
图5为现有技术的避障算法的避障路线图;
图6为本发明的障碍圆及障碍圆区域半径的示意图;
图7为本发明的第一条飞行路径上具有多障碍圆的实施例的示意图;
图8为本发明的通过遗传算法和Dunbins路径最终确定的避障路径的示意图;
图9为当无人机的最小转弯半径小于喷施半径时的飞行路线示意图;
图10为使用本发明的方法得到的避障路线的一个实施例的示意图;
图11为本发明的障碍圆与最小转弯圆构成的三角形区域的示意图;
图12为本发明的无人机的避障流程图;
图13为本发明的当障碍圆半径小于最小转弯半径时的示意图;
图14为图13的局部放大图。
具体实施方式
具体实施方式一:本实施方式的用于植保无人机的避障路径规划方法,方法用于规划飞行路径,飞行路径包括至少一条直线喷施路径以及至少一条转弯路径,每一条直线喷施路径包括一个起点和一个终点,每一条转弯路径表示从一个直线喷施路径的终点飞行至下一个直线喷施路径的起点的路径,其特征在于,方法包括:
步骤1)从第一条直线喷施路径起,将当前飞行路径的起点和终点连接,形成一条用 于表示原飞行路线的直线段;
步骤2)判断直线段是否与障碍圆相交,若不相交,则按原路线直线飞行,若相交,则执行步骤3);障碍圆是用于表示障碍物位置和大小特征的模型;将与直线段相交的障碍圆称为目标障碍圆;
步骤3)比较无人机的最小转弯半径与障碍圆的半径大小;
若障碍圆的半径大于等于无人机的最小转弯半径,则执行步骤4);
若障碍圆的半径小于无人机的最小转弯半径,则生成两个障碍圆的内切圆,以内切圆代替障碍圆,然后执行步骤4);内切圆的圆心位于直线段的经过障碍圆圆心的垂线上,内切圆的半径等于最小转弯圆的半径;
步骤4)生成复数个与直线段与障碍圆同时相切的最小转弯圆;最小转弯圆的半径为无人机的最小转弯半径;
步骤5)根据直线段、目标障碍圆以及最小转弯圆确定复数个可选的飞行路径并计算路径的长度以及与可选飞行路径对应的重喷漏喷面积,并将数据进行归一化处理;
步骤6)将所述的复数个可选路径作为初始种群,计算出适应值的大小;
步骤7)使用遗传算法的选择、交叉、变异操作,当满足迭代次数或者满足预定的适应值大小条件时,输出最佳的搜索路径。
步骤8)计算无人机从当前喷施路径的终点飞行到下一条喷施路径的起点所需经过的转弯路径的长度;
步骤9):重复执行步骤1)至步骤8),通过每一次执行得到的计算结果计算出总的飞行路径长度以及重喷漏喷面积。
步骤3)考虑到障碍圆的半径小于无人机的最小转弯半径的情形,“小障碍”的表述意味着本发明主题的技术手段考虑到了障碍圆的半径较小的情形。
图13中出现了一个小于最小转弯圆的障碍(虚线圆为最小转弯圆),此时的处理方法是:过此小障碍圆的圆心,做与ST垂直的直线,以最小转弯圆的大小为大小,作此障碍圆的内切圆,内切圆的圆心落在之前做的垂线上,这样的内切圆有两个,分别以这两个内切圆的切点加入到有向图中,作为两个顶点,进行最佳路径的搜索。当障碍圆的半径小于最小转弯圆时,后续所有的计算中都使用内切圆来代替障碍圆,或者说将内切圆视为原障碍圆。
图14是图13的局部放大图,从图14中可以看出,障碍圆的内切圆可以做出两个,内切圆的圆心均位于障碍圆圆心与直线段所确定的垂线上。
具体实施方式二:本实施方式与具体实施方式一不同的是:
步骤5)中每条可选路径的飞行路径的长度通过如下公式计算:
其中N为单条路径上必须躲避的障碍总数,N1是障碍圆与最小转弯圆异侧的障碍圆个数,N2是障碍圆与最小转弯圆同侧的障碍圆个数,且满足N1+N2=N;其中b为直线段的长度;Rz为无人机的最小转弯半径;
当障碍圆的半径大于等于无人机的最小转弯半径时,Ld为障碍圆的圆心到直线段的距离,Ri为障碍圆的半径;
当障碍圆的半径小于无人机的最小转弯半径时,Ld为内切圆的圆心到直线段的距离,Ri代表最小转弯半径的大小;
障碍圆半径Ri满足公式Ri=Rd+Rl,其中Rd为障碍圆的中心到最边缘的距离,Rl为无人机需要与障碍物保持的安全距离。
其它步骤及参数与具体实施方式一相同。
具体实施方式三:本实施方式与具体实施方式一或二不同的是:
步骤5)中每条所述可选路径的重喷漏喷面积通过如下公式计算:
其它步骤及参数与具体实施方式一或二相同。
具体实施方式四:本实施方式与具体实施方式一至三之一不同的是:
步骤8)用于根据如下公式计算转弯路径的长度D转:
D转=2Rp-2Rz+πRz
其中Rp为无人机喷洒的半径。
其它步骤及参数与具体实施方式一至三之一相同。
具体实施方式五:本实施方式与具体实施方式一至四之一不同的是:
步骤9)中通过每一次执行得到的计算结果计算出总的飞行路径长度具体为:
其中,M为直线喷施路径的个数,K1为所有直线喷施路径中障碍圆与最小转弯圆异侧的障碍圆个数,K2为所有直线喷施路径中障碍圆与最小转弯圆同侧的障碍圆个数。
其它步骤及参数与具体实施方式一至四之一相同。
具体实施方式六:步骤9)中通过每一次执行得到的计算结果计算出总的重喷漏喷面积具体为:
其它步骤及参数与具体实施方式一至五之一相同。
具体实施方式七:遗传算法的染色体基因对应于顶点,顶点的确定方法为:
过每个障碍圆的圆心,作直线段的垂线,垂线与障碍圆形成2个交点;将所有交点与飞行路径的起点与终点的集合作为顶点的集合;
设K为障碍圆的个数,则顶点集中有2+2K个顶点;
染色体编码从第1位到第2k+2位分别表示选择原飞行路径的起点、第1个障碍圆左侧的路径、选择第1个障碍圆右侧的路径、选择第2个障碍圆左侧的路径……选择第k个圆的右侧路径、原飞行路径的终点。
即这2+2K个顶点其中的2表示起点和终点,2K表示K个障碍圆都有左侧的路径和右侧的路径。规定顶点的概念是为了将无人机避障问题转化为图论,以便进一步使用遗传算法解决该问题。也就是说将每个障碍圆的左侧路径和右侧路径分别都视为一个图论中的顶点,那么一组从起点经过每个障碍圆的左侧路径或右侧路径,再到达终点的过程就可以用从起始点到每个顶点,最后再到终点的一组有向图的路径来表示。因此上述的顶点确定方式还有很多种,本实施方式仅举出一个具体的例子,只要选取的顶点能够唯一对应一个障碍圆的左侧路径或右侧路径即可,本发明不做限制。
例如,在一个实施例中,染色体编码如下
其中,D后面第一位代表第几个障碍,第二位代表该障碍的左侧或右侧,左侧为1,右侧为2。例如,D12表示遇到第一个障碍时选取障碍圆右侧的曲线作为飞行轨迹,D31表示遇到第三个障碍时选取障碍圆左侧的曲线作为飞行轨迹。
因此,上述实施例的路径可以表示为“起点→D11→D22→D32→终点”,其染色体描述为(1,2,5,7,8)。
适应度函数为路径的长短和重喷漏喷面积归一化之后和的倒数,适应值越大则表示适应度越好。
其它步骤及参数与具体实施方式一至六之一相同。
具体实施方式八:本实施方式与具体实施方式七不同的是:
遗传算法的适宜度函数为:
其中Fitness(i)为第i条路径的适宜度,D(xi)和S(xi)分表代表路径长度归一化后的数据以及重喷漏喷面积归一化后的数据;β1、β2分别代表路径长度和重喷漏喷面积的权重;
路径长度和重喷漏喷面积采用的数据归一化函数为:
其中x为待处理的数据,x*为经过归一化处理后的数据。即将路径长度或重喷漏喷面积作为x带入上述公式,可以得到D(xi)或S(xi)。x*在这里就是D(xi)或S(xi)。
其它步骤及参数与具体实施方式七相同。
具体实施方式九:本实施方式提供一种无人机,如图2所示,包括:
路径存储装置11,用于预先存储已规划完成的预定飞行路径信息;
障碍检测装置12,用于实时检测周围的障碍物信息,并预测是否会与障碍物碰撞;
判断装置13,用于在障碍检测模块预测会与障碍物碰撞时,发出避障信号;
避障喷施路径确定装置14,用于在未接收到避障信号时,从路径存储模块中读取预定飞行路径信息,并发送至飞行控制装置;还用于在接收到避障信号时,实现如具体实施方式一至八中任意所述的方法所对应的功能,并将最佳路径信息发送至飞行控制装置;
飞行控制装置,用于根据接收到的路径信息控制无人机进行飞行。
<实施例>
本实施例主要涉及具体实施方式中出现公式的推导过程。
一、符号定义:
假设植保无人机在一块具有若干个障碍的矩形地面上进行喷施作业(其他情况也是相似),假设矩形的长为a,宽为b,无人机的起点命名为S,终点命名为T。无人机来回往复的路线一共M条。
植保无人机喷施作业时的相关参数为:植保无人机的喷洒半径为Rp,飞行速度V,植保无人机的最小转弯半径Rz,障碍圆的中心到最边缘的距离为Rd,植保无人机的与障碍物的安全飞行距离Rl,所以,障碍圆的障碍区域半径Ri=Rd+Rl。
部分定义如图6所示。
二、数学分析
取图4(a)中植保无人机来回往复路线上的第一条飞行路径为例,在该路线上当遇到多障碍时,处理方法如下:
如图7所示,该路线上一共有五个大小位置不一的障碍,要得到较优的避障路线一方面要考虑避障路径的长短,另一方面还要尽量减少重喷漏喷的面积。该问题的求解算法描述如下。
步骤一:过起点S终点P做一条直线段,此线段将障碍圆模型分为两类
1类、障碍圆模型与直线不相交,说明该障碍圆不在原规划的飞行路径上,即不影响之前规划的飞行路径,在规划路线时就不予考虑此类障碍;
2类、障碍圆模型与直线相交,即障碍圆在飞行路线上,阻挡正常飞行,规划路线时主要考虑此类障碍;
步骤二:若所有障碍属于1类障碍则按照原路线飞行,若出现障碍圆模型属于2类障碍则采用的是Dubins路径生成避障路线。方法是:首先确定障碍圆的位置和大小以及根据最小转弯半径生成最小转弯圆;然后,以最小转弯圆的大小为大小,做出同时与障碍圆和原路线ST相切的最小转弯圆,这样的最小转弯圆一共可以生成四个,最后生成可飞路径,由于上图中有三个必须多必的障碍,所以会生成八条可飞路径,逐一计算路径的长短即可选出最佳路径。
但是上述的只是三个障碍的情况,当障碍一旦增加若干,则可飞路径将会骤然增多,对于此类问题,本文采取的方法如下。
当遇到多障碍时问题转化为转化为多障碍环境下的避障问题,即找到一条从起点S到终点T的适合植保无人机喷施作业的飞行曲线路径,即路径S-(障碍圆1-障碍圆n)-T 的路径,其求解可以归纳如下
在已知无人机出发点S,初始速度向量V,终点T,终点速度向量也是V,最小转弯半径Rz,障碍圆模型Dk(k=0,1,2,3…n)的位置情况下,求解从点S到T,满足植保无人机飞行最小转弯半径约束的可避开障碍的安全飞行路径,即求取从起始点S到终点T满足最小转弯半径约束的有向曲线。
由Dubins路径的生成方法知,该曲线是一条与n(n=[1,k])个障碍圆相切的曲线,如果将起始圆和终点圆作为一个有向图的起点和终点,过障碍圆圆心Dk做直线且与飞行路线垂直,该直线与障碍圆有左右两个交点,以这两个交点作为图中的各个顶点,则可构建有向图G=(V,A),其中V为数量为2*k+2的非空顶点集,A为有向路径的集合。则本题转化为求有向路径(S,T)=(S,W,T)的问题,其中W为由该有向曲线遍历的各个点组成的集合,W中各个点的顺序表明了其被遍历的顺序。
运用遗传算法和Dubins路径的方法最终搜索到无人机喷施作业中避障路径如图8所示。
如前述的图5避障的过程,该算法的避障过程也会使植保无人机的喷施过程产生重喷和漏喷的情况,重喷面积是植保无人机在第二条回飞路径上回飞喷施造成的,漏喷面积是躲避障碍时无法喷施到造成的,下面就其多障碍情况下产生的重喷漏喷的面积给予计算和推导。
如图9所示,已知障碍圆Dk的圆心坐标为(Xdi,Ydi),障碍圆模型的半径Ri,生成的最小转弯圆的圆心坐标(Xzi,Yzi),最小转弯半径都为Rz。障碍圆的中心点偏离原路线的距离为Li,可构建如上图所示的三角形区域。
由单条路径推广到整片区域,需要考虑到路径之间的转弯问题,假设植保无人机满足Rp>=Rz的条件,则转弯时可以采取以下方法:植保无人机可以通过Dubins路径生成形如“LCLCL”的曲线,做180°掉头,直接飞行到下一条喷施路径,常见的植保无人机喷施半径大约是4-8米,此时要求植保无人机的最小转弯半径应小于此值。其具体的飞行如图10所示。
对于整片区域,综合运用以上方法得到的避障路线如图11所示。
本发明还可有其它多种实施例,在不背离本发明精神及其实质的情况下,本领域技术人员当可根据本发明作出各种相应的改变和变形,但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。