大型植保無人機航行路徑跟蹤控制研究

王天陽，章天楊，陶學恒，葉華國

(1.大連工業(yè)大學 機械工程與自動化學院，遼寧 大連 116034；2.安徽工程大學 機械與汽車工程學院，安徽 蕪湖 241000)

0 引言

隨著智能控制技術(shù)的快速發(fā)展和農(nóng)業(yè)現(xiàn)代化進程的不斷深入，無人機技術(shù)從軍事領(lǐng)域向民用領(lǐng)域推廣應(yīng)用，主要體現(xiàn)在土地資源和災(zāi)害監(jiān)測、火災(zāi)預警、地圖測繪、物流快運和植被保護等方面。就植被保護而言，大多數(shù)采用的是小型四旋翼無人機在特定的小區(qū)域范圍內(nèi)進行作業(yè)，由于考慮到無人機功率小、農(nóng)藥攜帶量少，通常需要進行無人機航行路徑規(guī)劃，以提高作業(yè)效率。關(guān)于植保無人機的路徑規(guī)劃問題國內(nèi)外已經(jīng)展開了許多的研究工作，文獻[1-3]描述了植保無人機的關(guān)鍵技術(shù)并展望了植保無人機未來的發(fā)展趨勢。文獻[4，5]用蟻群算法為無人機進行規(guī)劃路徑，節(jié)省了計算時間，得到了一條代價較小的無人機航跡。文獻[6，7]基于遺傳算法和改進粒子群算法，實現(xiàn)了無人機多作業(yè)區(qū)域航線的全局規(guī)劃和多植保無人機協(xié)同路徑規(guī)劃。文獻[8-10]介紹了植保無人機避障和多無人機最小能量消耗路徑規(guī)劃算法研究。

對于大面積、比較寬闊的植被或者農(nóng)作物保護任務(wù)，小型旋翼無人機的作業(yè)時間和載藥量就無法滿足要求，需要選用長航時、大載重量的大型固定翼無人機。文獻[11-13]介紹了固定翼無人機用于軍事領(lǐng)域的任務(wù)規(guī)劃和跟隨導航技術(shù)。本文正是基于能量和作業(yè)效率，研究大面積植保環(huán)境下固定翼植保無人機的航行路線規(guī)劃問題。根據(jù)文獻[14]的制導理論和DUBINS曲線[15，16]規(guī)劃出無人機目標路徑后，運用經(jīng)典PID控制算法并基于模糊控制理論設(shè)計航跡跟蹤控制器，在MATLAB/Simulink平臺上作進一步仿真優(yōu)化，以驗證路徑控制算法的有效性。

1 無人機運動學模型

1.1 無人機常用坐標系之間的轉(zhuǎn)換[17]

參考前述相關(guān)文獻，確定無人機運動學方程首先要明確各坐標軸間的關(guān)系。無人機的運動姿態(tài)就是與其固聯(lián)的體坐標系相對于某參考坐標系的方位關(guān)系，基本的姿態(tài)參數(shù)是這兩個坐標系之間的方向余弦矩陣。無人機運動學模型的建立即是在慣性坐標系、機體坐標系、航跡坐標系以及氣流坐標系等4個坐標系中描述無人機的運動參數(shù)?？紤]兩個坐標系間的轉(zhuǎn)換時，設(shè)坐標系A(chǔ)和坐標系B的3個正交軸的單位矢量分別為{a1,a2,a3}、{b1,b2,b3}，坐標系B的單位矢量可由坐標系A(chǔ)的單位軸矢量表示，即：

(1)

坐標系A(chǔ)到坐標系B的方向余弦矩陣為：

(2)

無人機坐標系轉(zhuǎn)換如圖1所示。假設(shè)任意兩個坐標系A(chǔ)到坐標系B僅在X軸、Y軸、Z軸的某一方向軸上存在旋轉(zhuǎn)角度θ時，對應(yīng)的坐標轉(zhuǎn)換矩陣如式(3)～式(5)所示：

圖1 無人機坐標系轉(zhuǎn)換

(3)

(4)

(5)

1.2 無人機的質(zhì)心運動學方程

無人機的質(zhì)心運行學方程是研究其飛行性能、飛行軌跡以及位置隨時間變化規(guī)律的基礎(chǔ)。研究無人機的質(zhì)心運動時通常假定無人機是剛體，可用動量矩定理來描述，即：

(6)

其中：hI為慣性坐標系中的動量矩矢量；LI為在慣性坐標系中作用于剛體的外力矩。

根據(jù)動量矩定理和坐標轉(zhuǎn)換矩陣可得到式(7)：

(7)

將式(7)簡化后可得式(8)：

(8)

將式(8)展開成分量形式并做簡化可得到無人機常用的動力學方程式(9)：

(9)

其中：LX、LY、LZ為機體坐標系中的外力矩在X、Y、Z三個坐標軸方向的分量；IX、IY、IZ為機體坐標系慣量矩陣在X、Y、Z三個坐標軸方向的分量。

要描述無人機相對于地面的位置變化規(guī)律和姿態(tài)變化規(guī)律，得到無人機的飛行軌跡，還需要建立無人機的質(zhì)心運動學方程：

(10)

其中：v為質(zhì)心運動速度；α、β分別為地面坐標系和航跡坐標系轉(zhuǎn)換時地面坐標系繞Z軸和Y軸的旋轉(zhuǎn)角。

2 植保無人機的目標路徑

研究植保無人機的航跡規(guī)劃問題時，首先要分析無人機的約束條件，包括禁飛區(qū)、威脅區(qū)、地形障礙等地理限制因素，還包括姿態(tài)角、最小轉(zhuǎn)彎半徑等無人機的自身飛行限制條件。對于大型固定翼植保無人機，假設(shè)其植保范圍為3 000 m×3 000 m的正方形區(qū)域，可以規(guī)劃該植保無人機的目標路徑，如圖2所示。

圖2 植保無人機的作業(yè)路徑

但是，圖2所示的目標路徑不適合作為固定翼無人機的飛行路線，在該路徑的直角轉(zhuǎn)彎部分固定翼無人機不能夠像旋翼無人機那樣懸?；蛘邔崿F(xiàn)點對點的飛行，需要一段平滑的圓弧路徑過渡，且該圓弧應(yīng)當滿足無人機的轉(zhuǎn)彎半徑約束。因此必須基于DUBINS曲線的相關(guān)原理對圖2所示路徑進行優(yōu)化。若平面上的兩點滿足間距L大于兩倍的無人機最小轉(zhuǎn)彎半徑r(即L>2r)條件，就必然存在DUBINS路徑，它是一條能夠滿足無人機航向和轉(zhuǎn)彎半徑的圓弧-直線-圓弧路徑。經(jīng)過優(yōu)化后的植保無人機目標路徑如圖3所示。

圖3 優(yōu)化后的植保無人機目標路徑

3 路徑跟蹤控制器

為了使無人機能夠按照目標路徑飛行，需要設(shè)計無人機的路徑跟蹤控制器。路徑跟蹤控制器由制導律和控制律組成，其中制導律為控制律提供無人機的速度指令，控制律按照指令控制無人機飛行，實現(xiàn)內(nèi)外環(huán)控制。基于線性制導律設(shè)計出傳統(tǒng)PID控制器,實現(xiàn)無人機跟蹤目標路徑飛行控制。為提高跟蹤控制精度，采用模糊控制理論[18-20]對PID控制器進行優(yōu)化。

根據(jù)模糊控制理論將PID控制器中的偏差以及偏差變化率與PID控制器的比例、積分、微分參數(shù)聯(lián)系起來，建立相關(guān)的模糊規(guī)則，實現(xiàn)三個參數(shù)的自適應(yīng)調(diào)節(jié)，提高無人機的跟蹤控制精度。無人機的模糊PID控制器如圖4所示。

圖4 植保無人機模糊PID路徑跟蹤控制器

在MATLAB/Simulink軟件中對兩種路徑跟蹤控制器的跟蹤效果進行對比仿真，結(jié)果如圖5所示。由圖5可以看到：模糊PID控制器的跟蹤效果要優(yōu)于傳統(tǒng)PID控制器的跟蹤效果。圖6為跟蹤目標路徑時，兩種跟蹤控制器的航跡角度和速度變化情況。由圖6可以看出：模糊PID路徑跟蹤控制器的跟蹤響應(yīng)速度和穩(wěn)定性明顯優(yōu)于傳統(tǒng)PID控制器。

圖5 兩種控制器航跡跟蹤控制效果

圖6 兩種控制器跟蹤航跡角度和速度變化情況

圖7為兩種路徑跟蹤控制器的跟蹤誤差仿真結(jié)果，圖8為x方向和y方向上的跟蹤誤差。由圖7和圖8可以看出：模糊PID控制器的跟蹤誤差明顯小于普通PID控制器的跟蹤誤差。

圖7 兩種控制器跟蹤誤差 圖8 x向和y向上的跟蹤誤差

綜上所述，從控制精度、穩(wěn)定性以及響應(yīng)速度三個方面來看，模糊PID控制器能夠提高無人機飛行線路的控制效果。

4 結(jié)論

(1) 針對大范圍作業(yè)的植被保護機器人，考慮到作業(yè)效率和載藥量，選用固定翼無人機更為適宜。但必須考慮固定翼無人機的轉(zhuǎn)彎半徑和航向約束問題，可以基于DUBINS曲線相關(guān)理論對目標路徑作出規(guī)劃。

(2) MATLAB軟件仿真結(jié)果表明，模糊PID控制器對固定翼無人機的航行路徑跟蹤效果更佳，與目標路徑的誤差更小，其中曲線段跟蹤誤差為0.01 m量級,直線段跟蹤誤差為0.001 m量級。

(3) 模糊PID控制器在航跡角度誤差、速度變化、航行方向誤差等方面均明顯優(yōu)于PID控制器，控制精度高、穩(wěn)定性好。