農(nóng)用無人機移動補給平臺自主降落算法與試驗

祖林祿 侯加林 陳 民 王 紅 蘇 斐

(1.山東農(nóng)業(yè)大學機械與電子工程學院，泰安 271018; 2.山東農(nóng)業(yè)大學山東省園藝機械與裝備重點實驗室，泰安 271018；3.山東農(nóng)業(yè)大學山東省農(nóng)業(yè)裝備智能化工程實驗室，泰安 271018)

0 引言

近年來，農(nóng)業(yè)航空技術快速發(fā)展，電動多旋翼農(nóng)用無人機(簡稱農(nóng)用無人機)由于具有可垂直起降、空中懸停、操作維護簡便等優(yōu)點，在農(nóng)業(yè)作業(yè)中受到廣泛關注[1]。但續(xù)航能力和噴灑物補給方法是制約農(nóng)用無人機工作時間和工作效率的重要因素[2]。

農(nóng)用無人機移動補給平臺的關鍵技術之一是無人機能夠自主降落到運動的補給平臺上。農(nóng)用無人機降落過程涉及運動目標識別、運動軌跡預估、跟蹤控制器設計等多項技術[3]，對降落算法要求極高，包括目標識別算法快速、準確，軌跡預估算法實時、穩(wěn)定，以及跟蹤控制器響應速度快、跟蹤誤差小等。在應對運動速度較快的目標時，若采用單一PID控制，則系統(tǒng)響應速度較慢[3]。為此，本文提出一種基于模糊邏輯和PID分段控制的算法，并進行仿真試驗和現(xiàn)場試驗，以實現(xiàn)農(nóng)用無人機對運動目標的穩(wěn)定跟蹤和在移動補給平臺上的降落。

1 目標跟蹤軌跡擬合

1.1 問題描述

農(nóng)用無人機空基平臺的目標檢測跟蹤不僅要求算法能適應運動背景、實時性高，還要求算法對環(huán)境變化、目標尺度變化、目標速度變化等方面具有很好的魯棒性。背景減除法、幀差法、光流法[3]是靜態(tài)背景下的運動目標檢測算法，無法滿足本文要求。MeanShift算法[4]因算法簡單、收斂條件寬松而具有較好的實時性，但是無法適應目標尺度變化、運動速度較快等情況。Camshift算法[5]雖然實時性較好，但難以適應復雜背景下的目標跟蹤。

農(nóng)用無人機對補給平臺的跟蹤屬于復雜運動背景下的快速跟蹤，而且兩者都處于非線性、非高斯類型的機動狀態(tài)，此時對目標進行位置跟蹤屬于非線性系統(tǒng)狀態(tài)估計問題[6]，所以上述5種算法均不能滿足此類要求。本文提出一種解決策略：采用粒子濾波跟蹤算法并配合軌跡擬合算法完成對運動目標的位置跟蹤，將擬合出的預估軌跡作為期望路徑輸入農(nóng)用無人機飛行控制器。

1.2 粒子濾波算法

粒子濾波是一種基于蒙特卡洛方法和遞推貝葉斯估計的統(tǒng)計濾波方法。通過因子采樣法對狀態(tài)量進行更新，根據(jù)t-1時刻的所有粒子(即樣本)的概率重新進行采樣，高概率粒子被采樣次數(shù)多，低概率粒子則可能被舍棄，得到t時刻的粒子。t時刻的所有粒子對應的測量值為t時刻的粒子賦予新的概率，用于t+1時刻粒子的生成[7]。粒子濾波算法歸納如下：

(i=1,2,…,Ns)

(1)

并歸一化

(2)

則可獲得時刻t位置參數(shù)x的最小均方估計為

(3)

(5)時刻t=t+1，轉到步驟(2)。

粒子濾波適用于非高斯背景的非線性隨機系統(tǒng)，對本文所研究的對非線性、非高斯類型的運動目標跟蹤，能夠使跟蹤精度逼近最優(yōu)估計，有助于提升識別跟蹤算法的準確率。

1.3 軌跡預測

1.3.1軌跡擬合函數(shù)的選擇

所選的擬合函數(shù)應能準確描述一段時間內(nèi)的運動特性，因此選取可表征大部分目標運動軌跡的二次函數(shù)作為擬合函數(shù)[8]。

1.3.2擬合函數(shù)參數(shù)求解

(1)設二次函數(shù)表達式為

=a0+a1x+a2x2

(4)

此擬合函數(shù)共有3個未知參數(shù)，選用最小二乘法，通過最小化試驗數(shù)據(jù)誤差的平方和估計擬合函數(shù)的未知參數(shù)。

(2)誤差平方和為

(5)

(3)由多元函數(shù)求極值條件得(式(5)中E→0)

(6)

(4)將式(6)轉換成矩陣形式，并簡化得

(7)

可得

EA=Y

(8)

A=(XTX)-1XTY

(9)

XTX為奇異矩陣，所以A=[a0a1a2]T存在唯一最優(yōu)解。為保證跟蹤效果，采用15組最新數(shù)據(jù)實時更新擬合函數(shù)的參數(shù)。

1.4 目標跟蹤流程

為保證農(nóng)用無人機準確快速地跟蹤運動目標，需要把粒子濾波跟蹤算法輸出的目標位置坐標利用擬合函數(shù)進行運動軌跡擬合，以實現(xiàn)當目標因遮擋而暫時性消失時保證農(nóng)用無人機按照擬合軌跡飛行，大大提升了農(nóng)用無人機在非線性隨機環(huán)境中的跟蹤能力[9]。跟蹤算法具體實現(xiàn)流程如圖1所示。

圖1 跟蹤算法具體實現(xiàn)流程

2 模糊邏輯PID分段控制算法設計

2.1 PID控制器

PID控制器因原理簡單、理論完善、功能強大得到廣泛應用，包括比例控制器、積分控制器、微分控制器[10]。比例控制器中輸出與誤差成比例關系；積分控制器可加速系統(tǒng)趨近設定值，并消除穩(wěn)態(tài)誤差；微分控制器可提升整定時間及系統(tǒng)穩(wěn)定性[11]。本文采用串級PID控制算法，以農(nóng)用無人機與運動目標的位置偏差為外環(huán)輸入，以農(nóng)用無人機接近運動目標的速度偏差為內(nèi)環(huán)輸入，確保農(nóng)用無人機能快速、準確、穩(wěn)定跟蹤運動目標。若定義evk為速度偏差，uvk為串級PID內(nèi)環(huán)輸出，則串級PID控制器內(nèi)環(huán)可表達為

(10)

式中kPv、kIv、kDv——比例、積分、微分系數(shù)

2.2 模糊邏輯控制器

2.2.1結構設計

2.2.2模糊邏輯控制規(guī)則設計

模糊控制器設計中選擇輸入語言變量的詞集為{正大(PB)、正中(PM)、正小(PS)、零(Z)、負小(NS)、負中(NM)、負大(NB)}，選擇輸出語言變量的詞集為{正大(PB)、正中(PM)、正小(PS)、零(Z)、負小(NS)、負中(NM)、負大(NB)}。本文選用三角形和梯形混合型隸屬度函數(shù)、7級輸入變量、7級輸出變量。圖2a、2b為模糊邏輯控制器的輸入變量隸屬度函數(shù)及其分布，圖2c為輸出變量隸屬度函數(shù)及其分布。

2.2.3去模糊化

(11)

2.3 模糊邏輯PID分段控制系統(tǒng)結構

經(jīng)典PID控制器雖然控制精度較高，但其響應速度慢、超調量大。模糊控制具有響應速度快、超調量小的優(yōu)點，卻存在導致控制精度降低的問題。為滿足農(nóng)用無人機追蹤降落過程中響應速度快、超調量小的控制目標，本文提出一種改進策略：以誤差輸入量e與設定閾值的比較結果為轉換依據(jù)，進行PID控制器和模糊邏輯控制器的分段控制，且此分段控制系統(tǒng)不影響PID控制器與模糊邏輯控制器的獨立性。

圖2 隸屬度函數(shù)及其分布

本文設計的分段控制系統(tǒng)結構如圖3所示。在農(nóng)用無人機與運動目標相距較遠時，距離誤差e大于設定閾值，此階段由模糊邏輯控制器對農(nóng)用無人機實施控制。因其響應速度快的優(yōu)點可以使農(nóng)用無人機快速接近移動目標，避免出現(xiàn)因跟蹤速度慢而丟失跟蹤目標的問題。當距離誤差e小于設定閾值時，由PID控制器對農(nóng)用無人機實施控制。其控制精度高的優(yōu)點可滿足農(nóng)用無人機在降落過程中實現(xiàn)對移動平臺的精確鎖定，防止因農(nóng)用無人機與移動平臺間存在位置偏差而導致農(nóng)用無人機難以成功降落到移動平臺上。

圖3 分段控制系統(tǒng)結構圖

3 系統(tǒng)整體功能實現(xiàn)

3.1 農(nóng)用無人機動力學模型構建

四旋翼農(nóng)用無人機的運動包括3個軸向線運動和繞3個軸向角運動[14]。為準確描述農(nóng)用無人機在三維空間內(nèi)的姿態(tài)、位置、速度等信息，需要定義2個三維笛卡爾直角坐標系。如圖4所示，全局坐標系為Oe，采用NED(North(北)-East(東)-Down(下))坐標表示方法[15]。機體坐標系為Ob，坐標原點取機體的質心。機體坐標系的Xb-Yb-Zb方向定義為機身的前-右-下。在機體坐標系下，農(nóng)用無人機繞Zb軸運動為偏航(yaw)運動，繞Yb軸運動為俯仰(pitch)運動，繞Xb軸運動為橫滾(roll)運動。

圖4 坐標圖

假設農(nóng)用無人機在全局坐標系Oe中的速度為V，則V=[VXVYVZ]、重力加速度為g、農(nóng)用無人機質量為m；理想狀況下所有電機產(chǎn)生的總升力為fA；農(nóng)用無人機的轉動慣量為J；基向量e=[0 0 1]T；R為機體坐標系相對于全局坐標系的旋轉矩陣[16]；農(nóng)用無人機相對于全局坐標系的角速度為ω=[ωXωYωZ]T，可以用矩陣表達為

(12)

農(nóng)用無人機在全局坐標系中位置可用P=[pXpYpZ]T表示。農(nóng)用無人機動力學方程可表示為[16]

(13)

3.2 圖像坐標系的變換

為方便求出農(nóng)用無人機與所追蹤運動目標的位置偏差，需要將該運動目標在攝像頭中的三維坐標變換到全局坐標系中。如圖4所示，全局坐標系為Oe，攝像機坐標系為Oc，成像平面坐標系為Oi，f為攝像機焦距，T為空間變換的平移矢量，表示全局坐標系原點在攝像機坐標系中的坐標。假設(xc,yc,zc,1)、(xe,ye,ze,1)分別為空間內(nèi)任一點在攝像機坐標系和全局坐標系下的齊次坐標，則兩坐標系間的變換關系為

(14)

其中，s為任意非零參數(shù)，三維坐標系旋轉參數(shù)存儲在3×3正交單位矩陣Rr中，O=(0,0,0)為零矢量，M為攝像機外部參數(shù)矩陣，決定了攝像機光軸在全局坐標系中的坐標，可將全局坐標系下的坐標映射到攝像機坐標系中。

3.3 不同坐標系下誤差的計算

把農(nóng)用無人機通過GPS和氣壓計等傳感器得到的在大地坐標系下的空間位置坐標變換到全局坐標系Oe下得到機體坐標PN=(xN,yN,zN)，若攝像機獲取并轉換得到的運動目標在全局坐標系Oe中的坐標為x(t)=(x′t,y′t,z′t)，可知在全局坐標系Oe下農(nóng)用無人機與運動目標的距離誤差為ep=(xN-x′t,yN-y′t,zN-z′t)。把濾波后的陀螺儀傳感器、加速度傳感器和地磁羅盤傳感器數(shù)據(jù)進行實時解算求出橫滾角φ、俯仰角θ及偏航角φ。全局坐標系Oe變換到機體坐標系Ob變換矩陣[17]為

(15)

可得在機體坐標系Ob下用于輸入模糊邏輯PID混合控制器的誤差ee=(xe,ye,ze)。其中xe、ye經(jīng)過模糊邏輯PID混合控制算法處理后的結果可分別用于控制農(nóng)用無人機的橫滾、俯仰運動，以完成對運動目標的跟蹤，ze為機體與運動目標高度誤差，在機體下降到移動補給平臺的過程中消除[18]。

3.4 自主降落流程

以字母H為主要標志的降落平臺結構垂直對稱、清晰且便于識別，H的開口方向可用于校正農(nóng)用無人機降落方向。但是此降落平臺圖像特征過于簡單，農(nóng)用無人機在運動過程中難以把此圖像精確鎖定在正下方并實施降落[3]。本文設計一種帶有輔助識別區(qū)的降落平臺，如圖5所示，平臺圖案分為上下兩部分：上部為降落區(qū)，以H圖案為主要標志；下部為輔助識別區(qū)，以二維碼圖案為主要標志。下部輔助識別區(qū)的黑白色塊圖案顏色對比鮮明，能夠增加運動平臺的顏色可識別特征，有助于農(nóng)用無人機對降落平臺的精確鎖定[19]。此外，農(nóng)用無人機可利用光流算法檢測黑白色塊圖像中光點與暗點的移動來計算運動平臺相對于農(nóng)用無人機的移動速度，并把此相對速度作為農(nóng)用無人機降落過程中的參考變量，進一步提升降落階段的穩(wěn)定性。

圖5 以字母H為主要標志的降落平臺

農(nóng)用無人機在移動補給平臺上自主降落可分解為2個階段：第1階段，模糊邏輯算法主導控制下的追蹤階段；第2階段，PID算法主導控制下的降落階段。跟蹤降落過程的流程如圖6所示。其中xe為x軸方向誤差，ye為y軸方向誤差[20]。

圖6 跟蹤降落過程流程圖

4 試驗結果

為更好地驗證模糊邏輯PID分段控制算法的優(yōu)越性，設置模糊邏輯控制、PID控制為對照試驗，并分別進行軟件仿真試驗及現(xiàn)場試驗。

4.1 Matlab仿真試驗

在Simulink中搭建仿真試驗平臺，包括期望位置模塊(用于輸入擬合函數(shù)擬合預測的期望路徑)、當前位置模塊(表示農(nóng)用無人機當前運動狀態(tài)和空間位置)、模糊邏輯PID分段控制器模塊(處理輸入的距離偏差并按照相應的控制規(guī)則轉換為控制量輸出)、電機模塊(在輸出控制量的控制下改變轉速以完成農(nóng)用無人機的橫滾、俯仰、偏航運動)和機體模型模塊(農(nóng)用無人機動力學模型)。

3種算法對階躍信號和正弦信號的跟蹤仿真結果如圖7所示。為衡量3種算法的控制效果，對階躍信號跟蹤時控制過程上升時間、超調量、調節(jié)時間和穩(wěn)態(tài)誤差指標進行求解，結果如表1所示。可得模糊邏輯PID分段控制算法比PID算法超調量低12%、調節(jié)時間縮短2 s；相比于模糊邏輯算法穩(wěn)態(tài)誤差低0.12 cm。對正弦信號跟蹤時，應用皮爾遜相關系數(shù)衡量農(nóng)用無人機實際運動軌跡和期望的正弦軌跡的擬合程度，模糊邏輯算法作用時相關系數(shù)為0.970，PID算法作用時相關系數(shù)為0.981，模糊邏輯PID分段控制算法作用時的相關系數(shù)最高，為0.998[21]。

圖7 農(nóng)用無人機對階躍信號和正弦信號的跟蹤結果

表1 3種控制算法控制效果對比

4.2 現(xiàn)場試驗

現(xiàn)場試驗采用的平臺技術參數(shù)為：農(nóng)用無人機軸距330 mm、1 400 kV直流無刷電機、8寸碳纖維槳、好盈40 A無刷電調、領航者飛控、格式11.1(3S)鋰電池、Ublox-NEO-M8N GPS模塊、LenaCV視覺攝像頭、NVIDA Jetson TX2圖像處理器。試驗在山東農(nóng)業(yè)大學體育場完成，如圖8所示。將控制算法分別設置為模糊邏輯算法、PID算法、模糊邏輯PID分段算法各進行了10次測試，試驗結果如表2所示。由表2可知，模糊邏輯PID分段控制算法與PID算法相比，平均響應時間由1.1 s減少到0.6 s左右，響應速度提升約45.8%，且降落時振蕩情況較輕；與模糊邏輯算法相比，跟蹤誤差小10 cm左右，降低約72%；在降落成功率方面，分段控制算法比PID算法高40%，比模糊邏輯算法高70%；10次測試降落誤差最大為7.2 cm。

圖8 現(xiàn)場試驗圖

表2 現(xiàn)場試驗結果

5 結論

(1)分段控制算法在補給平臺突然加速、急轉彎等運動狀態(tài)突變情況下的響應速度比單一PID控制器提升45.8%；在農(nóng)用無人機跟蹤車載降落平臺過程中的超調量比單一PID控制器控制低12%。

(2)分段控制算法在控制農(nóng)用無人機對車載降落平臺進行跟蹤時的跟蹤誤差比單一模糊邏輯控制算法降低72%。

(3)針對單一H圖案降落平臺圖像特征過于簡單的問題，提出一種帶有輔助識別區(qū)的降落平臺，配合模糊邏輯PID分段控制算法可使農(nóng)用無人機在垂直降落過程中的誤差縮小至7.2 cm內(nèi)。