基于高度融合的植保無人機仿地飛行方法研究

吳開華 孫學超 張競成 陳豐農

(杭州電子科技大學生命信息與儀器工程學院， 杭州 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)