王天陽，章天楊，陶學(xué)恒，葉華國

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

0 引言

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

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

1 無人機(jī)運(yùn)動(dòng)學(xué)模型

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

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

(1)

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

(2)

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

圖1 無人機(jī)坐標(biāo)系轉(zhuǎn)換

(3)

(4)

(5)

1.2 無人機(jī)的質(zhì)心運(yùn)動(dòng)學(xué)方程

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

(6)

其中：hI為慣性坐標(biāo)系中的動(dòng)量矩矢量；LI為在慣性坐標(biāo)系中作用于剛體的外力矩。

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

(7)

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

(8)

將式(8)展開成分量形式并做簡(jiǎn)化可得到無人機(jī)常用的動(dòng)力學(xué)方程式(9)：

(9)

其中：LX、LY、LZ為機(jī)體坐標(biāo)系中的外力矩在X、Y、Z三個(gè)坐標(biāo)軸方向的分量；IX、IY、IZ為機(jī)體坐標(biāo)系慣量矩陣在X、Y、Z三個(gè)坐標(biāo)軸方向的分量。

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

(10)

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

2 植保無人機(jī)的目標(biāo)路徑

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

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

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

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

3 路徑跟蹤控制器

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

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

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

在MATLAB/Simulink軟件中對(duì)兩種路徑跟蹤控制器的跟蹤效果進(jìn)行對(duì)比仿真，結(jié)果如圖5所示。由圖5可以看到：模糊PID控制器的跟蹤效果要優(yōu)于傳統(tǒng)PID控制器的跟蹤效果。圖6為跟蹤目標(biāo)路徑時(shí)，兩種跟蹤控制器的航跡角度和速度變化情況。由圖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)速度三個(gè)方面來看，模糊PID控制器能夠提高無人機(jī)飛行線路的控制效果。

4 結(jié)論

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

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

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