基于GNSS与视觉融合的山地果园无人机航迹控制

王 东 范叶满 薛金儒 袁 端 沈楷程 张海辉,3

(1.西北农林科技大学机械与电子工程学院， 陕西杨凌 712100； 2.农业农村部农业物联网重点实验室， 陕西杨凌 712100；3.陕西省农业信息感知与智能服务重点实验室， 陕西杨凌 712100)

0 引言

无人机技术[1-2]已经被广泛应用于农情监测、农业植保等领域[3-11]。相关研究表明，无人机的飞行航迹、高度、速度等飞行参数对植保效果影响显著[12-17]。

飞行航迹的控制是无人机航空作业的关键问题。由于人工控制无人机飞行航迹精度较差[18]，为了提高无人机植保作业的效果，降低人为控制的误差，无人机航迹自主控制技术是近年来无人机航空植保作业的研究热点。FAICAL等[19]提出了结合作物特征和无人机结构特征的微调设置以控制航迹；袁玉敏[20]设计的基于GPS(全球定位系统)和GPRS(通用分组无线服务技术)混合的定位算法，提高了农业植保无人机的定位精度；卢璐等[21]基于实时动态差分技术(Real-time kinematic，RTK)的北斗卫星导航系统优化了植保无人机飞控系统，大幅度提高了作业航迹的精度。此外，也有将全球导航卫星系统(Global navigation satellite system，GNSS)与惯性导航、视觉导航等技术相融合进行无人机航迹控制的研究[22-25]，但现有研究均是针对平原下大田块作业开展的。

我国丘陵山地约占国土面积的70%。同平原相比，丘陵山地不仅地形起伏多变，且田块碎小、形状各异。丘陵山地多以经济林果为主栽对象，果树沿坡地等高线种植，果树行多为曲线，同大田作物相比覆盖率较低，因此，对航迹控制精度要求更高。在GNSS导航过程中，如果以果树行首尾位置的经纬度为定位点导航，无人机以两定位点之间的直线飞行，则会错过其中不在直线航迹上的果树，无法实现植保作业的果树遍历飞行要求。相反，如果以单株果树为定位点，则定位点过密，同时受GNSS系统刷新频率限制，以及无人机在飞行过程中受到的速度、侧风等因素的干扰，极容易错过当前目标点，导致无人机需要反复移动以到达目标点。因此，该方式极易浪费作业时间和能量，降低作业效率，同时对无人机飞行轨迹控制算法的要求过高，在控制率以及控制精度上具有较高的挑战性。

苹果是我国北方地区的主要经济林作物，仅陕甘宁地区的种植面积就已超过100万hm2。本文以自然场景下的山地苹果园为研究对象，采用RTK-GNSS导航进行作业行间切换引导，使用机器视觉技术计算无人机与作业行中心线的偏航角，进而结合PID控制算法调整无人机作业航迹，以实现山地果园无人机植保作业航迹的高精度控制。

1 控制系统设计

1.1 总体设计

基于GNSS与视觉融合的山地果园无人机植保航迹控制系统主要由无人机飞行平台和地面控制站两部分组成。其中无人机飞行平台中，除飞行器外，搭载有GNSS移动站、内环飞控模块、电子罗盘模块、数传模块、云台、RGB相机、无线视频发射模块和电源；地面控制站包括GNSS基站、飞行控制模块、便携式计算机、无线视频接收模块以及视频采集模块。其系统整体结构如图1所示。

图1 控制系统整体结构图Fig.1 Overall structure diagram of control system

整个控制系统是在无人机内环飞控实现其自身的稳定以及控制其俯仰、偏航、横滚、升降等动作的基础上运行的。GNSS移动站同时接收基站和卫星信号，实时解算定位信息，并通过数传模块2发送至地面控制站的飞行控制模块。飞行控制模块接收到无人机定位信息后计算并发送相应的控制指令给无人机飞行平台，实现无人机的GNSS导航。RGB相机采集视频信息并通过无线视频发射模块实时发送，经视频采集模块传输至便携式计算机。由便携式计算机对图像进行处理以得到作业果树行趋势线及偏航角，将偏航角信息发送给飞行控制模块，飞行控制模块计算并发出控制指令实现无人机的视觉导航。作业时由GNSS导航进行果园作业行间切换，视觉导航进行行内无人机航迹控制，从而实现无人机山地果园作业时的航迹控制。

1.2 无人机飞行平台及地面控制站的搭建

本文使用四旋翼飞行器作为载体搭建无人机飞行平台。搭建的四旋翼飞行器包括：朗宇X4108s-kv380型电机、好盈Platinum-30A-Pro OPTO型电子调速器、1555型碳纤螺旋桨、Tarot 650型机架、DJI NAZA型内环飞控、FS-R6B型控制信号接收机，对角电机距离为650 mm。

在无人机上搭建主要部件，并通过有机配合，组成无人机飞行平台。主要部件是：RGB运动相机(HERO3+型，GoPro，美国)、TS351型无线视频发射模块(深圳市创兴科电子科技有限公司)、T-2D型两轴云台(温州飞越航模有限公司)、AS62-T20型无线数传模块(成都泽耀科技有限公司)和SCM345型电子罗盘(无锡迈科传感科技有限公司)。搭建的无人机飞行平台实物如图2所示。

图2 无人机飞行平台实物图Fig.2 Photo of UAV flight platform 1.控制信号接收机 2.内环飞控 3.数传模块2 4.相机 5.云台 6.无线视频发射模块 7.数传模块1 8.电子罗盘 9.GNSS模块 10.GNSS天线

地面控制站完成飞行平台定位、航向、视频等信息的收集，对图像进行处理解算得到偏航角信息，进而采用PID控制算法完成无人机的飞行控制。主要部件有：以STM32F103微处理器为主控制器的飞行控制模块(内嵌PID控制算法)，RC305无线视频接收模块(深圳市创兴科电子科技有限公司)，视频采集模块为AV信号的USB视频采集卡，以及便携式计算机(型号G7，处理器i7 8750H，显卡GTX1060，内存16 GB，戴尔)。

其中飞行控制模块由FS-T6型遥控器(深圳市富斯遥控模型技术有限公司)改装完成，模块由STM32微处理器驱动数模转换TLC5615芯片产生4路模拟电压信号，经遥控器转换后完成4个控制通道(偏航、俯仰、横滚、升降)动作的产生，从而实现对无人机飞行动作的稳定控制。搭建完成的地面控制站实物如图3所示。

图3 地面控制站实物图Fig.3 Photo of ground control station 1.计算机 2.无线视频接收模块 3.视频采集卡 4.STM32微处理器 5.FS-T6型遥控器 6.TLC5615芯片 7.GNSS基站

1.3 RTK-GNSS定位装置设计

为保证定位精度，本系统采用RTK-GNSS装置进行经纬度信息获取，基于瑞士U-blox公司的NEO-M8P-2芯片设计该装置。该芯片可兼容北斗、GPS、Galileo以及GLONASS 4种定位系统信号，并可通过配置成为基站或移动站使用。

系统中，配置基站的输出格式为RTCM3.2，输出频率为1 Hz。移动站输出格式为NMEA0183协议，主要使用经纬度信息为无人机提供定位及导航，其信息输出频率为1 Hz。

1.4 控制系统的工作流程

系统作业前将果树行起点和终点的经纬度坐标及作业航向参数输入至控制系统。系统作业航迹控制过程由GNSS导航和视觉导航控制两个阶段组成。无人机从起飞点至作业行起始点的飞行过程由GNSS导航控制，到达作业行起始点后由视觉导航控制无人机沿果树行的中线飞行。当检测到作业行终点时，再由GNSS导航至下一行起点，重复上述工作直至整个作业完成。系统控制流程如图4所示。

图4 系统控制流程图Fig.4 Flow chart of system control

2 主要参数确定及算法设计

2.1 主要参数确定

2.1.1相机倾角

当无人机在果树行上空飞行时，若能实时得到当前航向与果树行趋势线之间的偏航角，即可以控制无人机按偏航角进行转向操作，从而保证无人机按果树行的位置飞行。由于在果树行上空飞行时相机视野内近距离树冠面积占比较大，远距离果树行趋势线更明显，控制目标位置应为下一个控制指令发出时所在位置，而图像边缘区域畸变较大。因此，图像中最远距离应足够大，且控制目标点不宜距图像边缘较近。综合考虑相机参数和飞行参数，选择图像垂直宽度的1/4位置作为偏航角的获取目标位置。

图5 视野及目标点示意图Fig.5 Diagram of visual field and target point

无人机在果树行上飞行时其飞行高度可由内环飞控保持稳定，当RGB相机与垂直方向角度一定时，即可得到目标位置与无人机当前位置的水平距离。在无人机飞行时，其视野与目标位置间的关系如图5所示。图中，L为无人机与目标果树的水平相对距离，m。H为无人机与果树冠层间的相对高度，m。θ为相机轴线与重垂线的夹角，(°)，即相机倾角。β为相机垂直方向的视场角，(°)。h为果树平均高度，m。P为当前相机在果树冠层高度上的水平视野宽度，m。

由图5相机自身参数关系及三角函数关系可得

(1)

(2)

为了保证图5所示的对应关系成立，相机倾角应满足

(3)

2.1.2相机视场角

由于GoPro HERO3+相机的镜头带有超广角，其视频畸变比较严重。为了尽可能减小视频畸变带来的影响，采用720p下的Medium模式进行实时拍摄，此时图像畸变最小。因无线视频传输模块会对传输的视频进行压缩处理，在Python软件中实时读取到的图像会有黑边，需要基于Python软件对实时读取的视频进行实际视场角度计算。图6是用相机获取的AFT-MCT-OV430型视觉标定板(维视数字图像技术有限公司)图像。根据该图像，可求得相机垂直方向的视场角β=76°，相机焦距f=8.1 mm，水平方向上图像的总像素数D=640像素。

根据式(3)可得，相机倾角应满足38°≤θ<52°。

图6 视觉标定板图像Fig.6 Image of visual calibration board

由于一般植保作业时要求无人机距离果树冠层的高度为1～3 m[26]，为了获取较好的导航视野及合适的目标间距，由式(1)、(2)可得不同飞行高度下相机倾角θ与无人机到目标果树的水平相对距离L及相机在果树冠层高度上的水平视野宽度P之间的实际关系。无人机的飞行高度越高、相机的倾角越大，则无人机与目标果树的水平相对距离L就越大，相机在果树冠层高度上的水平视野宽度P也就越大。为了使L适合无人机转向的控制周期，且保证无人机在视觉导航时视野中的果树冠层面积所占比例较大，最终选择飞行高度为2 m，相机倾角为46°。此时，L=1.0 m，P=18.7 m。

2.2 主要算法设计

2.2.1GNSS导航方法

图7 果树行提取结果Fig.7 Results of tree row extraction

GNSS导航控制中，GNSS基准站通过数传模块1将定位基准信息发送至机载GNSS模块。机载模块解算获得无人机定位信息，通过电子罗盘模块读取实时航向信息，并将位置信息和航向信息通过机载数传模块2发送至地面飞控模块。地面飞控模块将之与果树行作业起点位置进行比较，将比较结果作为PID控制算法的输入，计算得到速度、航向等控制量，实现无人机的实时控制。重复上述控制过程，直至无人机飞行至作业果树行起点位置后，调整无人机作业航向并切换至视觉导航控制。

2.2.2视觉导航方法

2.2.2.1作业行提取方法

遥控无人机在山地苹果园上方飞行，使用GoPro HERO3+相机实时获取视频信息，并以PAL格式由无线视频传输模块上传到计算机端实时存储为图像，由于实时拍摄时连续帧图像间的区别不大，故计算机以jpg格式每秒存储一帧图像。由于视频采集卡转换的限制，最终存储的图像分辨率为640像素×480像素，实测图像实时传输延时约为200 ms。无人机飞行时，无人机距树冠高度约2 m，相机倾角为46°，镜头垂直方向中线与航向保持一致。从获取的图像中随机选取600幅用于果树行提取算法中参数的确定。图7a是任意选取的3幅图像。

通常在RGB颜色空间上使用过绿特征(2G-R-B)[27-28]对绿色果树目标进行识别，其中R、G、B为视频图像中红、绿、蓝颜色的分量。图7a的过绿特征提取结果如图7b所示。由于无人机拍摄到的图像为高空俯视图像，图像中不仅有果树，还有地表杂草、非作业果树行等干扰，使得基于过绿特征二值化后的图像存在较多的粘连区域，导致果树行的分割效果差，无法满足航迹控制要求。

有研究表明，最优线性组合识别方法可对视频进行快速处理[29-30]，因此，本文基于线性组合方法进行果树行提取，该线性组合式为

y=aR+bG+cB

(4)

式中a——R分量系数b——G分量系数

c——B分量系数

y——线性组合的结果

为获取果树行信息，需要将y映射至0～255范围内，处理为灰度图后以127为阈值进行二值化处理。将果树行区域设置为白色，其他区域设置为黑色。其映射关系式为

(5)

式中y*——图像映射结果

ymin——线性组合处理结果的最小值

ymax——线性组合处理结果的最大值

在参数选取试验中，发现B分量对果树目标行的识别效果影响不大。为了减小程序运算量，确定B分量系数c=0。通过对600幅图像的R和G分量进行分析，确定最佳参数为a=1.95，b=2.00。图7a进行最优线性组合识别后处理结果如图7c所示。图中，白色部分为识别后的果树区域。可见图中存在多行果树，虽然仍有部分区域存在粘连现象，但从图7c可以看出，水平方向中心线附近的待作业果树行分割效果较好，果树行趋势明显。

以上果树行识别算法是基于Python3.5平台，利用OpenCV库和NumPy库文件编程实现的，其算法流程如图8所示。

图8 果树行提取算法Fig.8 Flow chart of fruit tree row extraction algorithm

视觉导航时，无人机已由GNSS导航引导至待作业果树行正上方且航向已调整至作业方向，默认图像中心区域为待作业行。为了提取当前作业行，以图像垂直中线为起始位置，分别向左右像素位置进行遍历，直到遇见黑色的果树行边缘或者图像边界位置时终止，然后将每像素行左右识别到的端点取其中心位置作为该像素行的果树中心位置，其结果如图7d中绿色点所示。获取中心位置后，为了获取与实际果树行趋势线一致的拟合线，采用最小二乘法对图像上所有绿色点进行拟合得到表示实际的果树行的趋势线，其结果如图7d中蓝色曲线所示。从图中可以看出，在距无人机较近区域拟合得到的趋势线与实际果树行趋势线基本一致。

2.2.2.2偏航角获取算法设计

为了从图像中获得当前的偏航角信息，以图像1/4处水平线与拟合线的交点为当前目标点，即可保证目标点实际位置与无人机距离较近，且选取位置的图像中果树区域占比较大。通过将目标点像素坐标与垂直中线像素坐标作差，即可获得目标点距离中线的像素差。

设图像目标点的像素位置与垂直中线的像素差为d，根据相机关系可得

(6)

式中X——相机成像传感器水平方向长度，mm

δ——无人机偏航角，(°)

参数X由相机传感器尺寸决定，本文使用相机X=8.83 mm。

当目标点在中线左边时，此时像素差为负值，无人机需要左转δ；相反当目标点在中线右边时，此时像素差为正值，无人机需要右转δ。

将图7d中目标点距离中线的像素差代入式(6)，即可得到无人机的偏航角。

2.2.2.3视觉导航的控制流程

视觉导航控制中，由云台搭载RGB相机以确保无人机飞行时图像中心线与航向保持一致，以及RGB相机倾角固定不变。RGB相机拍摄视频并通过无线视频传输模块传送至地面接收模块。接收模块将接收到的视频信息经视频采集模块传输至便携式计算机中，运用图像处理算法将接收到的图像进行分割得到作业行区域，在图像中求取各像素行中作业行区域的中点坐标并拟合得到果树行趋势线，该曲线为作业行分布趋势。将作业行趋势线与图像中线比较计算无人机偏航角，并将偏航角传送给地面飞行控制模块。飞控模块采用PID控制算法计算得到偏航控制量，实现对无人机作业中航向的调整。重复该过程直到检测到该作业行结束后切换至GNSS导航控制。

3 控制系统性能测试与验证

3.1 GNSS装置定位精度测量

在对GNSS装置的定位精度进行测量时，将移动站天线放置在距离地面1 m的水平纸板上，记录其初始位置的经纬度数据。然后将该点作为原点，依次移动天线位置使其距离原点位置为3、5、10、20 cm，记录天线分别位于原点上、下、左、右4个方向不同位置的经纬度数据，每个位置记录10个数据。根据测量得到的经纬度数据和实际的距离，计算GNSS装置的定位误差，结果如表1所示。

表1 GNSS的定位误差Tab.1 Position errors of GNSS cm

由定位测试误差可知，3 cm和5 cm间距时误差基本一致，最大为2.57 cm。在10 cm和20 cm间距时，误差明显减小，最大为1.21 cm。表明装置定位精度为厘米级，可满足无人机GNSS导航时精确定位的需求。

3.2 图像处理速度测试

植保无人机的飞行速度一般小于3 m/s，本文选取飞行速度为2 m/s。使用本文提出的偏航角计算方法对视频进行处理，统计连续600幅图像的处理速度，其中处理速度最快为15.05 f/s，最慢为2.35 f/s，平均处理速度为6.53 f/s，且70%以上处理速度都集中于3～5 f/s之间。因此，视觉导航控制率确定为2次/s，可确保每次控制都可完成偏航角的计算，从而保证航迹控制的准确性。

3.3 导航控制性能测试

试验地点位于甘肃省平凉市静宁县山地苹果园(35.16°N、105.77°E、海拔1 608 m)。该地区果园的地形为台阶状，果树沿等高线单行种植。以该地区任意3块不同形状、不同高度差的苹果园为对象，对本文提出的GNSS与视觉融合导航试验方法进行验证。试验时间为2018年10月4—6日10:00—14:00。天气晴，光照充足，阵风，无固定风速风向。

为了获取各试验地点三维地形模型，使用大疆Phantom 3 Standard型无人机对果园进行航拍获取影像，并基于Agisoft PhotoScan软件(Agisoft，俄罗斯)进行三维地形建模，最终获取高分辨率的正射影像。将正射影像导入ArcGIS软件(Esri，美国)，获取每个作业行起点的经纬度及航向角度，将作业行起点的信息预先导入控制系统，并用GNSS系统实时记录无人机的位置信息，作为飞行轨迹。

图9a为其中一块山地苹果园的俯视图。该图中共有3行果树。无人机飞行时，以右上角为飞行起点，左下角为终止点。从右上角到左下角共有48棵果树。图中绿色点为手动标注的果树冠层中心点，用于对导航路径误差的计算。获取ArcGIS软件中冠层中心点的经纬度信息，计算冠层中心点到导航路径的投影距离，即导航时无人机偏离果树的误差。定义航向右侧误差为正，左侧误差为负。则遍历48棵果树时的导航误差如图9b所示。

图9 导航试验结果Fig.9 Results of navigation test

由图9a可以看出，除个别点外，无人机飞行轨迹较平稳。飞行中的误差绝对值最大处发生在第2行的果树上。其原因可能是由于第2行果树冠层茂密，覆盖率高，提取果树行时近场位置所占区域过大，计算输出偏航角过大，导致出现过控制；也可能是由于飞行过程中侧风对无人机的影响导致。通过对3块果园的导航误差进行统计，得出基于本文提出的GNSS与视觉融合的山地果园无人机航迹控制系统的误差范围为-47～42 cm，平均误差为-9 cm。由于苹果树的冠层直径为300 cm左右，因此，本文提出的航迹控制系统和方法的精度较好，能够满足无人机果树植保作业的要求。

4 结论

(1)设计了GNSS与视觉融合的山地果园无人机航迹控制系统，该系统由无人机飞行平台和地面控制站两部分组成。该无人机飞行平台以四旋翼无人机为载体，整个平台飞行稳定，控制性能良好。设计并搭建了以NEO-M8P-2芯片为核心的RTK-GNSS定位装置，试验结果表明，定位精度为厘米级，定位精度高。

(2)设计了视觉导航时偏航角获取算法。采用线性组合方法提取果树行信息，基于最小二乘法对果树行进行拟合，得到果树行趋势线，并通过果树趋势线获取了视觉导航偏航角。

(3)山地苹果园实地试验表明，设计的航迹控制系统可完成作业行间的GNSS导航控制及作业行中的视觉导航控制。在自然条件下，航迹控制系统的误差范围为-47～42 cm，平均误差为-9 cm。系统控制精度较高，可满足无人机山地果园植保作业时精准控制要求。