基于高度融合的植保无人机仿地飞行方法研究

吴开华 孙学超 张竞成 陈丰农

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

0 引言

植保无人机[1-4]作业时，为达到较好的喷洒效果，使农药更均匀地附着在农作物叶片的表面，应使无人机在农作物顶部保持稳定的高度，所谓仿地飞行即距离农作物的高度不随地形地势的变化而改变。如果仅靠无人机飞手采用手动定高飞行进行植保作业，由于受到视距限制难以实现仿地飞行，因此研究无人机仿地飞行方法对植保作业具有重要意义。

目前关于仿地飞行的研究中，文献[5]提出基于差分GPS(DGPS)高度实现高度跟随，但无法控制无人机与农作物的相对高度，只能适应较为平坦的地形。文献[6]提出基于自适应S滤波的高度信息融合技术，将气压计、加速度计和DGPS高度融合提高精度，但气压计误差会随着高度的降低而增大。文献[7]提出基于双激光传感器实现地形匹配的方法，但由于激光传感器测得的是点距离，疏密度不同的农作物易造成测量结果波动大。文献[8]提出基于超声波传感器实现无人机高度控制的方法，但螺旋桨机翼高速旋转对超声波有影响，而且超声波测距在植被表面误差较大。文献[9]提出基于无人机航测获取地形信息的方法，但在植保作业中需要额外增加航测的时间和成本。

毫米波雷达传感器具有低空精确测高的特点[10]，故本文提出运用3个毫米波雷达传感器实时测量无人机与地面的相对高度以及与前方地形斜向距离的方法，根据前置毫米波雷达判断前方地形坡度，根据坡度选择相应的高度融合算法，再与模糊PID控制算法相结合，实现高精度仿地飞行。

1 仿地飞行原理

仿地飞行的高度检测原理如图1所示。基于3个雷达传感器实现仿地飞行，其中对地毫米波雷达(简称对地雷达)用于获取无人机相对于地面的飞行高度H1，前置毫米波雷达(简称前置雷达)用于获取无人机前进时无人机与前方地形的斜向距离H2，后置毫米波雷达(简称后置雷达)用于获取无人机后退时无人机与后方地形的斜向距离H3。H1与H2、H1与H3的夹角是已经设定好的两个雷达传感器之间的夹角α。通过获取飞行控制模块的俯仰角[11]的正负，可以判断无人机处于前进还是后退状态，从而确定使用前置雷达或是后置雷达。

图1 仿地飞行原理图Fig.1 Schematic diagram of terrain following

以下假设无人机处于前进状态，根据三角形余弦公式可得对地雷达和地形的夹角为

(1)

地形坡度为θ，则θ=90°-β。当无人机处于平地时，θ=0°；当无人机遇到上升坡时，如图1中飞行状态1所示，θ>0°；当无人机遇到下降坡时，如图1中飞行状态2所示，θ<0°；融合高度为

(2)

式中θ0——坡度阈值

hDGPS——DGPS高度

定义飞行状态处于小坡度飞行时为水平模式H-Mode，处于抬升飞行时为抬升模式R-Mode，处于下降飞行时为下降模式D-Mode。当-θ0≤θ≤θ0，处于H-Mode，为减小高度误差，采用hDGPS和H1卡尔曼滤波后的高度实现仿地飞行；当θ>θ0或θ<-θ0时，坡度起伏较大，飞机处于R-Mode或D-Mode，为提前对地形变化做出响应，采用多雷达高度融合提前采取抬升动作或下降动作，以提高响应速度，使得无人机和作物保持稳定的高度。

图2 坡度阈值设置原理图Fig.2 Schematic diagram of setting of slope threshold

坡度阈值设置原理如图2所示，为了保证坡度起伏较大情况时无人机抬升或下降的距离和坡地起伏距离一致，根据植保场景设置的无人机水平速度vx和垂向速度vy，求得坡度阈值为

(3)

式中Hy——纵向调整距离

Hx——横向飞行距离

t——飞行时间

2 仿地飞行算法

2.1 基于卡尔曼滤波的高度信息融合

当-θ0≤θ≤θ0时，采用卡尔曼滤波算法。卡尔曼滤波[12]是一种递推线性最小方差的最优估计方法，滤波过程中根据前一个高度的估计值和最近一个高度估计信号的当前值，即可用状态方程和递推方法进行估计。卡尔曼滤波后可以得到无人机高度的无偏估计值和测距传感器的常值偏差估计值，通过常值偏差估计值可以校正测距传感器的实际测量值，从而提高小坡度场景下仿地飞行的精度。

传感器在检测过程中含有一些噪声。已知真实高度和真实加速度，对传感器测量值进行补偿，建立对地雷达高度、DGPS高度和垂向加速度af的量测方程

H1=h+ε1

(4)

hDGPS=h+ε2

(5)

af=ag+ε3

(6)

式中h——真实高度

ε1——对地雷达测量噪声

ε2——DGPS测量噪声

ag——真实垂向加速度

ε3——加速度计测量噪声

将高度h、垂直方向的速度vg和加速度ag作为状态变量，为了不影响高度信号估计，运用了噪声淹没[13]技术，将加速度信息用一个方差较大的随机信号代替，可以使滤波时加速度的预测信息淹没而不影响高度信息的估计值，式中用η表示。则系统状态X=[hvgag]T，测高系统状态方程为

(7)

由之前对雷达、DGPS和加速度计的测量分析，建立系统量测方程

(8)

由系统状态方程和系统量测方程，离散化后可以得到离散卡尔曼滤波的状态方程和离散卡尔曼滤波的量测方程

Xk=φk,k-1Xk-1+Γk-1Wk-1

(9)

Zk=HkXk+Vk

(10)

式中Xk——状态向量序列

Zk——量测序列

Wk-1、Vk——均值为零的高斯白噪声

φk,k-1——一步状态转移矩阵

Γk-1——系统噪声驱动矩阵

Hk——量测矩阵

卡尔曼滤波分为预测和更新两个过程：预测过程中采用上一时刻对系统状态的估计值，对当前时刻系统状态进行估计；更新过程中采用当前时刻系统状态的观测值，优化在预测过程获取的系统状态估计值，最后可以获得一个更准确的系统状态估计值。

预测过程

(11)

更新过程

(12)

融合高度

H0=k

(13)

Pk/k-1——一步预测均方误差

Kk——滤波增益

Pk——估计均方误差

Qk-1——非负正定矩阵

2.2 基于多雷达的高度信息融合

当θ<-θ0或θ>θ0时，采用多雷达高度信息融合算法。地形变化幅度大时，无人机如果不提前进行地形判断并调整高度，会由于响应滞后造成无人机仿地飞行误差增加。多雷达高度信息融合可以通过预判提前调整高度，极大地提高高度响应速度。

(14)

其中

式中wi——权重系数

(15)

对地雷达检测距离通过滑动平均滤波和限幅滤波后得到检测距离hground,前置雷达检测距离通过滑动平均滤波和限幅滤波后得到检测距离hpre,计算前方地形坡度θ，当θ>θ0，说明前置雷达检测到前方地面有上升坡，属于R-Mode，需要执行抬升动作；当θ<-θ0，说明前置雷达检测到前方地面有下降坡，属于D-Mode，需要执行下降动作，则抬升距离为

R=D=|hground-hprecosα|f1f2

(16)

式中D——下降距离f1——载药系数

f2——坡度系数

无人机载药量越小f1越大，可以根据无人机的载药量调整响应的灵敏度。当作业场景的坡度θ较大时，为提高高度响应速度，增大f2，当作业场景的坡度较小时，过早地采取抬升或者下降动作会导致跟随精度下降，减小f2采用梯度响应来提高响应的精度和飞行的稳定性。则融合高度

(17)

3 模糊PID高度控制算法

植保无人机在农药喷洒过程中，质量不断减小，电池电量不断下降，当外界有较大干扰如风力风向变化，使用传统的PID[14-18]进行高度调节误差可能增加，影响仿地飞行的稳定性。

图3 模糊PID高度控制原理图Fig.3 Schematic diagram of fuzzy PID height control

(18)

将偏差值累加到初始值即可得到最后的PID输出参数，通过对PID的修正，可以对植保无人机系统实现更加稳定的高度控制。

4 仿真分析与试飞实验

4.1 毫米波雷达选型

采用湖南纳雷科技有限公司的NRA-24系列毫米波雷达，表1为NRA-24的主要参数，图4为NRA-24的实物图。

表1 NRA-24毫米波雷达参数Tab.1 NRA-24 millimeter wave radar parameters

图4 NRA-24实物图Fig.4 NRA-24 physical maps

4.2 模糊PID高度控制实验

通过建立模糊PID模型，确定模糊规则和隶属度函数，通过Matlab的fuzzy工具箱将模糊规则和隶属度函数导入生成.fis文件，利用Simulink中的fuzzy controller模块将此文件与传统PID模块组合生成模糊PID控制系统。建立Simulink仿真，仿真参数为：植保无人机空载质量15 kg，载药量为10 kg，喷洒速度为30 g/s,传统PID控制参数为KP0=2,KI0=0.03,KD0=0.5。

喷洒一段时间后，无人机质量、电量等参数发生变化，此时给定无人机15 cm的阶跃响应，测试给定阶跃响应后1.5 s内的高度控制效果，图5为给定阶跃响应后模糊PID与PID的控制效果图，由图可知传统PID不能很好地适应，超调量较大，模糊PID控制相比于PID控制响应快、超调量小、稳定性高，能更好地对植保无人机进行高度控制。

图5 模糊PID和PID控制效果图Fig.5 PID and fuzzy PID control effect diagram

4.3 小坡度场景下卡尔曼滤波高度信息融合仿真分析与实地试飞

为了验证小坡度场景下卡尔曼滤波算法的有效性，首先在Matlab中建立无人机在低空的仿真实验。无人机动态飞行时毫米波雷达传感器误差在±10 cm，则ε1～N(0,100)，DGPS误差在±10 cm，则ε2～N(0,100)，因实际加速度计测量误差较小，方差取为0.01，则ε3～N(0,0.01)，η～N(0,100)。设无人机真实高度为：h=200+10sin(0.1t)，则无人机的真实垂向速度为v=cos(0.1t)，真实垂向加速度为a=-0.1sin(0.1t)。设仿真时间T=200 s，仿真结果如图6、7所示。

图6 融合高度仿真曲线Fig.6 Fusion height simulation curves

图7 卡尔曼滤波融合高度误差曲线Fig.7 Error curve of Kalman filtering fusion

图6为DGPS高度和对地雷达高度卡尔曼滤波融合后的高度和设定飞行高度曲线，图7为融合高度和设定仿真飞行高度的误差曲线,由图可知融合后的高度和实际高度误差在±1.5 cm内。图8为实地飞行场景，将无人机定高在150 cm的高度飞行。图9为一组设定150 cm实地飞行时的融合高度误差曲线，由图可知在150 cm飞行时该组数据最大误差为4 cm。

图8 实地飞行场景Fig.8 Field flight scene

图9 实地飞行误差曲线Fig.9 Field flight error curve

分别将无人机在100、150、200 cm高度仿真和实地试飞50组，卡尔曼滤波融合高度经过模糊PID控制误差均在±4 cm内。传感器自身测距误差、农作物冠层高度不一致以及飞行时旋翼的风场下压造成植物高度变化导致毫米波雷达检测存在误差，但与DGPS或者对地雷达单一传感器检测相比，融合高度精度更高，由此可知设计的滤波器满足无人机在低空小坡度场景飞行的要求。

4.4 大坡度场景下多雷达高度信息融合实地试飞

为了验证大坡度场景下多雷达高度信息融合算法的有效性，设计实验场景为如图10所示的上升坡和下降坡坡度相同的坡地，坡度分别设置为15°、25°、35°，坡地垂直高度为1 m，图11为35°时坡地的具体参数。

图10 实验场景Fig.10 Experimental scene

图11 坡度参数示意图Fig.11 Diagram of slope parameters

图12为35°坡度、飞行高度为150 cm时采用DGPS高度定高飞行下前置雷达和对地雷达的高度数据曲线。由于DGPS定高飞行时，DGPS高度为当前高度与起飞点高度的差值即海拔高度。当地面有如图10所示的坡地时，DGPS无法反馈距离地面的相对高度，因此在坡地上方无法保证仿地飞行的精度,图中对地雷达距离坡地最近为60 cm，最大误差为90 cm。

图12 DGPS定高飞行雷达高度数据Fig.12 Height of radar using DGPS height

图13为35°坡度、飞行高度为150 cm时采用多雷达高度融合前置雷达和对地雷达的高度数据曲线。由图中可以看出，前置雷达可以提前检测到前方地形变化，当无人机飞行靠近坡地即到达区域A时，前置雷达检测到前方地形为上升坡，由式(17)可得，融合高度H0=hground-R,把抬升信号反馈给无人机控制模块触发抬升动作，当无人机处于坡地上方时即图中区域B时，前置雷达提前检测到前方地形为下降坡，融合高度H0=hground+D，把下降信号反馈给无人机控制模块触发下降动作，模糊PID控制器根据高度误差与高度误差的导数修正PID参数，进行高度控制调节。由数据可知，爬坡阶段对地雷达距离坡地最大为182 cm,最大误差为32 cm,下坡阶段对地雷达距离坡地最小为112 cm，最大误差为38 cm。

图13 多雷达高度信息融合实验结果Fig.13 Experimental results of multi radar height information fusion

分别在坡度15°、25°、35°测试5组数据，设定飞行高度为150 cm，飞行速度为3 m/s，每组测试10次，表2为其中一组实验数据，表3为不同坡度下融合高度最大误差。

表2 不同坡度下实验数据Tab.2 Experimental data of sloping field at different slopes cm

表3 不同坡度时最大误差Tab.3 Maximum error of different slopes

根据表3可知，随着地形坡度的增加无人机高度最大误差也相应的增加，主要是因为坡度增加后无人机提前抬升或下降的控制量也越大，但由于无人机控制耦合现象[19]的存在，高度调节存在一定的延时性，因此坡度越大跟随地形的难度也越大。高度调节的控制量增大导致无人机俯仰角增加，前置雷达受俯仰角影响测距误差相应增大，造成了融合高度误差的增加。多雷达高度信息融合高度经过模糊PID控制后爬坡与下坡阶段误差在±40 cm内，相比于DGPS定高误差更小，可以满足大坡度仿地飞行的精度要求。

5 结束语

小坡度场景下仿真与试飞结果表明，采用卡尔曼滤波算法可以得到比单个传感器更高精度的高度；大坡度场景下试飞实验表明，可以通过前置雷达的高度变化触发无人机的提前抬升或下降。通过仿真和试飞验证了模糊PID算法可以提高无人机系统的鲁棒性和稳定性。但本算法需要根据特定场景设置坡度阈值，仍然会受到地理条件和飞行高度的影响，下一步研究的重点是如何根据地理条件[20]和飞行高度采用自适应算法设置坡度阈值。本文提出的算法成本低、精度高，对植保无人机仿地飞行具有较大的应用价值。

1 薛新宇,兰玉彬. 美国农业航空技术现状和发展趋势分析[J/OL]. 农业机械学报,2013,44(5):194-201. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20130534&journal_id=jcsam. DOI:10.6041/j.issn.1000-1298.2013.05.034.

XUE Xinyu, LAN Yubin. Agricultural aviation applications in USA[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2013, 44(5):194-201.(in Chinese)

2 张东彦,兰玉彬,陈立平，等. 中国农业航空施药技术研究进展与展望[J/OL]. 农业机械学报,2014,45(10):53-59.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20141009&journal_id=jcsam.DOI:10.6041/j.issn.1000-1298.2014.10.009.

ZHANG Dongyan, LAN Yubin, CHEN Liping, et al. Current status and future trends of agricultural aerial apraying technology in China[J/OL]. Transactions of the Chinese Society for Agricultural Machinery,2014, 45(10):53-59. (in Chinese)

3 娄尚易,薛新宇,顾伟，等. 农用植保无人机的研究现状及趋势[J]. 农机化研究, 2017, 39(12):1-6.

LOU Shangyi, XUE Xinyu, GU Wei, et al. Current status and trends of agricultural plant protection unmanned aerial vehicle[J]. Journal of Agricultural Mechanization Research, 2017, 39(12):1-6. (in Chinese)

4 王大帅,张俊雄,李伟，等. 植保无人机动态变量施药系统设计与试验[J/OL]. 农业机械学报, 2017, 48(5):86-93. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20170510&journal_id=jcsam. DOI：10.6041/j.issn.1000-1298.2017.05.010.

WANG Dashuai,ZHANG Junxiong,LI Wei, et al.Design and test of dynamic variable spraying system of plant protection UAV[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2017, 48(5):86-93. (in Chinese)

5 胡永红. 数据融合方法在小型飞行器高度定位中的应用[J]. 计算机测量与控制, 2006, 14(10):1371-1373.

HU Yonghong. Application of data fusion approach on altitude location for small aircraft[J]. Computer Measurement & Control, 2006, 14(10):1371-1373. (in Chinese)

6 马旭,程咏梅,郝帅. 面向无人机高度融合估计的自适应S滤波方法[J]. 中国惯性技术学报, 2013, 21(5):604-608.

MA Xu, CHENG Yongmei, HAO Shuai. Adaptive S filtering method for UAV height fusion estimation[J]. Journal of Chinese Inertial Technology, 2013, 21(5):604-608. (in Chinese)

7 文恬,高嵩,邹海春. 基于激光测距的无人机地形匹配飞行方法研究[J]. 计算机测量与控制, 2015, 23(9): 3209-3212.

WEN Tian, GAO Song, ZOU Haichun. Based on laser ranging method research of UAV terrain matching flight[J]. Computer Measurement & Control, 2015, 23(9):3209-3212. (in Chinese)

8 刘博,常佶. 用于小型无人机的超声波低空测高系统实验研究[J]. 宇航计测技术, 2010, 30(3):74-78.

LIU Bo,CHANG Ji. Experiment research of ultrasonic low-altitude measurement system for using SUVA[J]. Journal of Astronautic Metrology & Measurement, 2010, 30(3):74-78. (in Chinese)

9 张惠均. 无人机航测带状地形图的试验及分析[J]. 测绘科学, 2013, 38(3):100-101.

ZHANG Huijun. Test and analysis of aerial surveying zonal topographic map using unmanned aerial vehicle[J]. Science of Surveying & Mapping, 2013, 38(3):100-101. (in Chinese)

10 MORGAN C, JAROSZEWSKI S, MOUNTCASTLE P. Terrain height estimation using GMTI radar[C]∥Proceedings of the 2004 IEEE Radar Conference, 2004:378-381.

11 彭孝东,张铁民,李继宇，等. 基于传感器校正与融合的农用小型无人机姿态估计算法[J]. 自动化学报, 2015, 41(4):854-860.

PENG Xiaodong, ZHANG Tiemin, LI Jiyu, et al. Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration[J]. Acta Automatica Sinica, 2015, 41(4):854-860. (in Chinese)

12 秦永元,张洪钺,汪叔华. 卡尔曼滤波与组合导航原理[M]. 西安：西北工业大学出版社, 2015:33-57.

13 ZHOU Guoqing, SCOTT R. UAV-based multi-sensor data fusion processing[J]. International Journal of Image & Data Fusion, 2010, 1(3):283-291.

14 廖懿华,张铁民,兰玉彬. 农田信息采集用多旋翼无人机姿态稳定控制系统设计与试验[J]. 农业工程学报, 2017, 33(3):88-98.

LIAO Yihua, ZHANG Tiemin, LAN Yubin. Design and test of attitude stabilization control system of multi-rotor unmanned aerial vehicle applied in farmland information acquisition[J]. Transactions of the CSAE, 2017, 33(3):88-98. (in Chinese)

15 PROUTY R W. Helicopter performance, stability, and control[M]. Malabar, FL:Krieger Publishing Company,2003.

16 SUFENDI, TRILAKSONO B R, NASUTION S H, et al. Design and implementation of hardware-in-the-loop-simulation for UAV using pid control method[C]∥2013 3rd International Conference on Instrumentation, Communications, Information Technology, and Biomedical Engineering, 2013:124-130.

17 龙文,陈洋,吴怀宇,等. 变载荷下六旋翼无人机高度控制[J]. 传感技术学报, 2017,30(7):1125-1130.

LONG Wen, CHEN Yang, WU Huaiyu,et al. Altitude control of six rotor UAVs with variable load[J]. Chinese Journal of Sensors and Actuators, 2017, 30(7):1125-1130. (in Chinese)

18 刘浩蓬,龙长江,万鹏,等. 植保四轴飞行器的模糊PID控制[J]. 农业工程学报, 2015,31(1):71-77.

LIU Haopeng, LONG Changjiang, WAN Peng, et al. Fuzzy self-adjusting proportion integration differentiation for eppo quadrocopter[J]. Transactions of the CSAE, 2015,31(1):71-77. (in Chinese)

19 付伟, 孙春贞. 小型高速无人机控制耦合分析及补偿控制[J]. 太赫兹科学与电子信息学报, 2014, 12(1):57-61.

FU Wei, SUN Chunzhen. Control coupling analysis and compensation control of small high speed UAV[J]. Journal of Terahertz Science and Electronic Information Technology, 2014, 12(1):57-61. (in Chinese)

20 陈加栋,邱白晶,管贤平,等. 基于二维激光传感器无人直升机作业边界探测[J]. 农机化研究, 2017,39(7):10-15.

CHEN Jiadong, QIU Baijing, GUAN Xianping,et al. Detection of boundary of unmanned helicopter based on 2D laser sensor[J]. Journal of Agricultural Mechanization Research, 2017, 39(7):10-15. (in Chinese)