全自动植保无人机含障不规则区域的航迹规划算法

金泽选， 吴开华， 王　朔， 王文杰， 孙学超

(杭州电子科技大学生命信息与仪器工程学院，浙江杭州 310018)

近年来，国内外对植保无人机(简称植保机)已经开展广泛研究，具有自主作业功能的全自动植保机[1]越来越受到研究者的重视。我国部分农田是比较零散、形状不规则的，其中有些地块中存在障碍物。为实现植保机的全自动作业，需要研究覆盖航迹规划技术[2]，覆盖航迹规划可以避开障碍物，规划出一条能够遍历某一区域的最优飞行路径。目前对于存在障碍物的不规则农田航迹规划问题的研究相对较少，因此迫切需要解决植保机对于含障不规则区域的航迹规划问题，以大幅度增加全自动植保机可作业的地块。

对于植保机区域覆盖的航迹规划问题，目前的主要方法是采用“Z”字形扫描线方式，即植保机沿直线飞行，在区域边界处转向，然后朝反方向沿平行直线飞行，如此反复，逐行遍历待扫描区域。这种方法较适用于规则的矩形作业区域，但对于不规则形状的多边形作业区域来说，无论沿何种方向进行作业，作业航线无法保证与所有的边界都平行，因此均会出现多余覆盖的现象，须对“Z”字形扫描算法进行优化，使其满足不规则区域航迹规划的要求。

针对植保机的避障问题，目前主要有2种解决方法，第一种方法是通过在无人机平台增加避障传感器，根据检测到的障碍物信息进行实时避障，张跃东等研究了一种基于单目视觉的障碍物深度提取算法，可以有效发现并区分不同深度的障碍物[3]；程虹霞等通过对超声波及红外传感器的数据融合，研究了多传感器信息融合技术在无人机平台自主避障行为中的应用[4]；王和平等通过分析红外成像和毫米波雷达测距技术，研发了基于红外与雷达一体化吊舱的无人机避障技术[5]。另一种方法是事先将障碍物信息标记出来，并在航迹规划时将其规避。张逊逊等提出一种基于改进人工势场的避障控制方法[6]，将障碍物划分为低矮型和高杆型，并制定不同的避障策略。将无人机与障碍物的相对运动速度引入人工势场中，给出基于改进人工势场的避障控制算法，无论从避障路径还是避障时间上看均优于传统人工势场避障。关震宇等研究了一种基于Dubins路径的无人机避障规划算法，通过采用遗传算法，结合无人机的飞行性能和最小转弯半径，在已知障碍物空间位置前提下实现无人机避障，并将其推广到多障碍环境下，使之能解决飞行器多障碍避障航迹规划问题[7]。

在无人机平台增加传感器来进行避障的方法主要适用于不确定的或未知环境的区域。对于地理信息已知的农田区域可通过将边界信息和障碍物信息事先标记出来的方式进行避障。文献[6]将障碍物分为低矮型和高杆型，分别采用爬升和绕飞的方式进行避障，但是没有涉及障碍物的大小。文献[7]虽然是事先标记障碍物进行航迹规划，但主要研究从起始点到目标点避开所有障碍物的最优飞行路线问题，没有涉及区域的全覆盖问题。本研究以植保机含障不规则区域覆盖航迹规划问题为研究背景，首先对“Z”字形扫描进行优化，在扫描线方式的基础上，通过栅格法[8]对工作区域进行划分，使该算法满足对不规则区域的全覆盖。然后根据障碍物的大小进行分类，将其分为点状障碍物(如树、电线杆等)和区域障碍物(如房屋、树林等)，对不同的障碍物设计不同的避障算法，在优化全局航迹规划的基础上通过插入航点的方式进行局部规划，从而达到绕过障碍物继续飞行的目的。

1　含障不规则区域航迹规划算法研究

1.1　不规则区域全局航迹规划算法

对于不含障碍物的不规则区域，需要研究一种航迹规划算法，使其可以根据任意的作业方向，或者在未指定作业方向的情况下，给出某一推荐的作业方向和航线，尽可能地减小多余覆盖面积和总飞行距离，解决不规则区域的全覆盖问题。由于在植保机喷药作业过程中，各条作业航线是相互平行的，相邻平行航线的距离均为植保机的喷药宽度W，采用不规则区域航迹规划算法，可快速得到植保机的作业航线。图1为植保机作业航迹规划示意。

如图1所示，植保机对不规则区域全局航迹规划算法如下：(1)计算航迹规划的起始点和起始边。定义实际作业中飞机起飞时所在的位置为起飞点，根据边界多边形各顶点距飞机起飞点的距离计算起始点。选择距飞机起飞点最近的顶点作为起始点。在飞机的飞行过程中，为尽量减少转弯次数，从而提高作业效率，应使飞机沿地块较长的边飞行，因此本研究选择经过起始点2条边界线中较长的边界线作为起始边。假设起始点为A1(x1，y1)，起始边为LA1A2，可求得起始边的表达式为(y-y2)(x1-x2)=(y1-y2)(x-x2)。

(2)计算距起始边最远的点。计算边界多边形除顶点A1、A2外其余各顶点到起始边的距离，假设其余各顶点到起始边的距离分别为d1，d2，…，dm。获得距离的最大值dmax=max[d1，d2，…，dm]。图1中A4(x4，y4)到起始边A1A2的距离最大。过点A4作起始边的平行线即为最远边LA4-A1A2，最远边的表达式为(y-y4)(x2-x1)=(x-x4)(y2-y1)。

(3)计算区域内各条航线。将最远边和起始边之间的距离按喷幅W进行分割，分得的每块小区域为植保机作业的宽度。进而可求出区域内的所有航线。所有航线均平行，除了最后一条航线，每条航线的距离都是定值W。通过计算可求得分割的航线数，以及第i条航线Li方程的斜率和截距：

dis_cut=(int)dmax/W；

(1)

kLi=kL(A1A2)；

(2)

bLi=(bL(A4-A1A2)-bL(A1A2))×i/dis_cut+bL(A1A2)。

(3)

式中：dis_cut为分割的条数；kL(A1A2)为起始边LA1A2的斜率；kLi为第i条航线的斜率；bL(A4-A1A2)为最远边LA4-A1A2的截距；bL(A1A2)为起始边LA1A2的截距；bLi为第i条航线的截距。

由方程(2)(3)得航线Li的表达式为yi=kLi×xi+bLi。

(4)计算每条航线与边界线的交点。每条航线都会和边界线产生2个交点如D1、D2。这些交点就是植保机航迹规划出的航点。

(5)对航点进行“Z”字形扫描。对已经求出的全部航点，通过“Z”字形排序，确定航点先后顺序。

通过以上过程可将区域内全部航点求出，然后对航点排序得到上传植保机的航迹。该算法实现了对不规则区域的全覆盖航迹规划，是研究含障区域航迹规划的基础。

1.2　含障区域局部航迹规划算法

对于不含障碍物的区域，植保机根据全局航迹规划算法，生成飞行航线，实现区域的全覆盖。而对于含障碍物区域，障碍物仅可能影响其中某一条或几条航线，其余航线不受障碍物影响，那些被障碍物影响的航线，可通过插入航点的方式进行避障。

1.2.1点状障碍物局部航迹规划算法点状障碍物的特点是面积较小，实际影响的航线条数有限，仅能影响其中一条或几条航线，对点状障碍物影响的航线可以通过插入4个航点的方式避过障碍物。如图2所示，设障碍物Z点坐标为(xZ，yZ)，受影响的航线为LMN，过Z点向航线LMN作垂线，交点为Z′(xZ′，yZ′)，其中航线上2个航点M、N的坐标都是已知的，分别为M(xM，yM)、N(xN，yN)。现需要插入避障点A、B、C、D，使植保机沿航线LMN飞行时，当飞到障碍物Z点附近时，沿A—B—C—D路径飞行，避过障碍物后，再回到航线LMN上。

障碍点与避障线的距离分别如下：点Z到直线AB的距离为d1；点Z到直线BC的距离为d2；点Z到直线CD的距离为d3；长度d1、d2、d3需要通过对飞机的飞行速度、减速所需时间及转弯的机动性等因素的综合分析来确定。

根据向量运算可求得4个避障点的坐标如下：

A[(xM×d1-xZ′×d1+xZ′×dZ′M)/dZ′M，(yM×d1-yZ′×d1+yZ′×dZ′M)/dZ′M]；

(4)

B{[(xZ′-xZ)×(d2-dZZ′)+xA×dZZ′]/dZZ′，[(yZ′-yZ)×(d2-dZZ′)+yA×dZZ′]/dZZ′}；

(5)

C{[(xZ′-xZ)×(d2-dZZ′)+xD×dZZ′]/dZZ′，[(yZ′-yZ)×(d2-dZZ′)+yD×dZZ′]/dZZ′}；

(6)

D{[(xZ′-xA)×d3+xZ′×d1]/d1，[(yZ′-yA)×d3+yZ′×d1]/d1}。

(7)

式中：dZ′M为线段Z′M的长度；dZZ′为线段ZZ′的长度。

通过上面的算法可求出4个避障点的坐标。将4个避障点按A、B、C、D的顺序插入原航迹中，可实现含点状障碍物区域的完整绕飞功能。

令人不解的是，那支骑兵，还有骑兵后面的马车，此刻却没有动静。老太医满腹狐疑，正要与秦铁崖谈及此事，马蹄声碎，烟尘腾起，骑兵已分为两支纵队，护卫着马车走来。马车停下，轿帘掀起，下来一位官员。

1.2.2区域障碍物局部航迹规划算法区域障碍物的特点是面积比较大，实际影响的航线比较多，不能像点状障碍物那样通过插入4个航点的方式进行避障。为解决含区域障碍物的避障问题，需要改变插入航点的策略，通过将区域障碍物边界向外平移的方式进行避障。

图3为区域障碍物避障示意图。设区域障碍物为多边形H1I1G1K1L1(简称H1)，将该多边形每条边均向外平移距离d得到多边形H2I2G2K2L2(简称H2)，要求距离d必须保证飞机沿多边形H2边界飞行时不会碰到障碍物。

植保机对区域障碍物局部航迹规划算法如下：(1)采集区域障碍物多边形H1的坐标。得到多边形H1中各点的坐标分别为H1(xH1，yH1)、I1(xI1，yI1)、G1(xG1，yG1)、K1(xK1，yK1)、L1(xL1，yL1)。(2)计算平移后多边形H2的坐标。得到平移后的多边形H2中各点的坐标分别为H2(xH2，yH2)、I2(xI2，yI2)、G2(xG2，yG2)、K2(xK2，yK2)、L2(xL2，yL2) 。(3)确定多边形H2影响原有航线的条数。通过对不规则区域的全局航迹规划可计算所有的飞行航线，计算每条航线与多边形H2存在交点的数量。若航线与多边形H2没有交点，说明这条航线不受障碍物的影响；若航线与多边形H2有1个交点，说明这条航线恰好通过多边形H2的某个顶点，由于飞行沿多边形H2飞行时不会碰到障碍物，所以这条航线也不受障碍物的影响；若航线与多边形A2B2C2D2E2有2个交点，说明植保机沿该航线飞行时，需要穿过障碍物区域，必须采集避障动作，使植保机可以绕过障碍物继续飞行。

现以航线LMN为例，多边形H2航线LMN存在2个交点，分别为P、Q。通过几何知识可求得直线LMN、直线LL2H2、直线LL2K2的方程分别如下：

(y-yN)(xM-xN)=(yM-yN)(x-xN)；

(8)

(y-yH2)(xL2-xH2)=(yL2-yH2)(x-xH2)；

(9)

(y-yL2)(xK2-xL2)=(yK2-yL2)(x-xL2)。

(10)

联立方程(8)(9)(10)可求出交点P、Q的坐标分别为(xp，yp)、(xQ，yQ)。

(4)确定插入原有航迹的航点，使植保机沿多边形H2边界飞行以达到避障的效果。植保机要在航线PQ段进行避障，有2条避障路线，分别为M—P—L2—Q—N和M—P—H2—I2—G2—K2—Q—N。选择插入航点数量较少的路线作为实际避障路线，在2条路线插入航点数量一致的情况下，选择避障路线距离较短的路线作为避障路线。因此在航线LMN中需要插入的航点为点P、L2、Q。

(5)重复(3)和(4)的操作，将被区域障碍物影响的航线全部找到，并通过插入航点的方式进行避障。

以上过程可求出穿过区域障碍物的所有航线，对穿过区域障碍物的航线，使其沿平移后的多边形边界飞行，可解决存在区域障碍物地块的全覆盖航迹规划问题。

2　仿真和田间试验

2.1　仿真

通过APM SITL仿真模拟器可以在没有任何硬件配合的情况下在室内方便地搭建出一个运行在电脑上的完全模拟真实飞行的仿真环境。因此可以利用APM SITL仿真模拟器对含障不规则区域航迹规划算法进行验证。

2.2　田间试验

在实际作业中，为满足植保机的全自动飞行，要求飞行精度偏差控制在0.5 m以内，而且在避障过程中不能撞上障碍物。为了验证含障不规则区域航迹规划算法的实际应用效果，需要进行田间试验计算实际飞行航迹和理论规划航迹的最大偏差。试验在1块包含点状障碍物和区域障碍物的田地进行，通过手持GPS测得田地边界点的坐标、点状障碍物坐标以及区域障碍物边界坐标。根据上文所述航迹规划算法求出规划好的航迹点，上传飞控，记录植保机的飞行情况。

进行10组飞行试验，每组飞行10次，通过机载GPS实时记录植保机的飞行航迹，并实时下传到地面站。图6为作业地块示意，其中矩形区域为实际作业的区域，图7为实际飞行航迹图，其中直线为上传飞控的规划航迹，曲线为植保机飞行的实际航迹。

从图7可以看出， 植保机在飞行时的实际航迹与上传的规划航迹是有一定偏差的，且拐弯处和避障处的偏差要明显大于沿航线直线飞行的偏差。记录第1组每次飞行试验的规划航迹和实际飞行航迹的最大偏差，结果见表1。

表1　规划航迹和实际飞行航迹的最大偏差

从表1可以看出，在共计10次飞行中，虽然规划航迹和实际飞行航迹均存在偏差，但每次飞行的最大偏差均小于 0.4 m，而且很好地规避了障碍物，满足了植保机全自动作业的要求。

3　结论

本研究对植保机在实际作业中对区域的覆盖航迹规划问题进行了研究，重点研究并完成了含障不规则区域的航迹规划算法设计，该算法将全局规划和局部规划相结合，通过APM SITL仿真试验，从理论上证明了算法的可行性。在所进行的田间试验中，理论航迹和实际飞行航迹最大偏差均小于0.4 m，而且很好地规避了障碍物，满足实际作业要求，可为无人机全自动飞行提供参考。

参考文献：

[1]蒋智超，刘朝宇. 浅谈植保无人机发展现状及趋势[J]. 新疆农机化，2016(2)：30-31，42.

[2]陈海，王新民，焦裕松，等. 一种凸多边形区域的无人机覆盖航迹规划算法[J]. 航空学报，2010(9)：1802-1808.

[3]张跃东，李丽，刘晓波，等. 基于单目视觉的无人机障碍探测算法研究[J]. 激光与红外，2009，39(6)：673-676.

[4]程虹霞，骆云志，朱松柏，等. 多传感器信息融合技术在无人机平台避障中的应用[J]. 兵工自动化，2010，29(6)：89-91.

[5]王和平，汪俊，申争光. 基于红外与雷达一体化吊舱的直升机安全避障技术[J]. 科技创新导报，2014(29)：56-59.

[6]张逊逊，许宏科，朱旭. 低空低速植保无人直升机避障控制系统设计[J]. 农业工程学报，2016，32(2)：43-50.

[7]关震宇，杨东晓，李杰，等. 基于Dubins路径的无人机避障规划算法[J]. 北京理工大学学报，2014，34(6)：570-575.

[8]徐博，陈立平，谭彧，等. 基于无人机航向的不规则区域作业航线规划算法与验证[J]. 农业工程学报，2015，31(23)：173-178.