鄭積仕，孟凡茹，賴宏圖，陳興武

(福建工程學(xué)院 信息科學(xué)與工程學(xué)院，福州 350118)

0 引言

無人機(jī)可以根據(jù)預(yù)先設(shè)定的航線自主飛行，廣泛的應(yīng)用于軍事巡邏、資源勘探、地形勘測等，隨著無人機(jī)應(yīng)用越來越廣泛[1-2]，飛行任務(wù)越來越復(fù)雜，如目標(biāo)跟蹤、自主著陸[3]、編隊(duì)飛行[4]、空中加油[5]和利用空氣動力學(xué)進(jìn)行發(fā)電[6]等，這也對無人機(jī)空間軌跡跟蹤控制算法提出更高的要求。

目前，三維軌跡導(dǎo)航控制算法是研究的熱點(diǎn)[7]，其中，文獻(xiàn)[8]中提出了利用特殊正交群SO(3)求解姿態(tài)問題的三維路徑跟蹤算法，文獻(xiàn)[9]中基于Lyapunov理論設(shè)計(jì)三維路徑跟蹤導(dǎo)航算法，通過控制目標(biāo)路徑航點(diǎn)的速度來避免最接近無人機(jī)投影點(diǎn)作為參考點(diǎn)所引起的奇異性，并且適用于平滑條件一般的三維曲線。文獻(xiàn)[10-11]中構(gòu)建一個(gè)矢量場，使無人機(jī)沿著矢量場收斂到期望路徑，并且在風(fēng)的干擾下也有很好的跟蹤性能。其中矢量場算法中矢量場組合中有死區(qū)，并且無人機(jī)每次到達(dá)段的末端，矢量場會改變[12]。文獻(xiàn)[13-14]中提出的L1-Navigation非線性導(dǎo)航算法應(yīng)用較為廣泛[15]。L1-Navigation導(dǎo)航算法在預(yù)期軌跡上選擇一個(gè)參考點(diǎn)，參考點(diǎn)與無人機(jī)之間的距離為L1,并且通過參考點(diǎn)產(chǎn)生一個(gè)橫向加速度指令，控制無人機(jī)飛行，在直線路徑跟蹤上采用PD控制，沿著曲線軌跡跟蹤加入了超前量，并且橫向加速度指令中使用慣性速度，在由外部干擾的情況下速度可以自適應(yīng)變化[13]。目前L1-Navigation導(dǎo)航算法已經(jīng)在Ardupilot開源軟件中被應(yīng)用，但L1-Navigation導(dǎo)航算法存在參數(shù)固定、自適應(yīng)能力不強(qiáng)等缺點(diǎn)。

本文以空間八字曲線導(dǎo)航為例，空間八字空間曲線是通過兩個(gè)半圓以及兩個(gè)測地線構(gòu)成，通過空間曲線上的目標(biāo)航點(diǎn)以三維坐標(biāo)的形式表示，無人機(jī)當(dāng)前位置的目標(biāo)航點(diǎn)坐標(biāo)乘以旋轉(zhuǎn)矩陣表示下個(gè)目標(biāo)航點(diǎn)坐標(biāo)。將導(dǎo)航控制分為橫向、速度高度控制，通過兩個(gè)固定增益系數(shù)來計(jì)算加速度，由于增益系數(shù)不變，在固定翼無人機(jī)實(shí)際飛行時(shí)，受到外界干擾無法自適應(yīng)變化，導(dǎo)致無人機(jī)會產(chǎn)生一定的軌跡誤差。

軌跡誤差是導(dǎo)航控制的重要指標(biāo)，本文采用自適應(yīng)的方法優(yōu)化控制性能，減少軌跡誤差。模糊控制結(jié)合人的經(jīng)驗(yàn)，無需構(gòu)建精確的控制模型，適用于導(dǎo)航控制性能的優(yōu)化上。本文以計(jì)算橫向加速度的增益系數(shù)為輸出量，根據(jù)變化的軌跡誤差來調(diào)整常數(shù)系數(shù)，從而改變加速度的大小，優(yōu)化固定翼無人機(jī)在空間曲線飛行的軌跡誤差，提高跟蹤性能和控制的自適應(yīng)性。

本文的結(jié)構(gòu)安排如下：第二部分構(gòu)建空間曲線模型，以空間八字曲線為例。第三部構(gòu)建模糊自適應(yīng)控制器，以軌跡誤差和軌跡誤差變化率作為模糊控制輸入量，計(jì)算橫向加速度的增益系數(shù)常數(shù)作為輸出量，優(yōu)化控制效果。第四章對提出的優(yōu)化方案進(jìn)行模擬飛行測試，經(jīng)軟件在環(huán)仿真測試，驗(yàn)證了本文方法的有效性。最后是本文的總結(jié)。

1 空間曲線飛行導(dǎo)航控制

本文以空間八字曲線為例，固定翼無人機(jī)繞著空間八字曲線周期飛行利用空氣動力學(xué)將風(fēng)能轉(zhuǎn)換成電能進(jìn)行發(fā)電[6]，固定翼無人機(jī)在空中飛行發(fā)電相比于傳統(tǒng)發(fā)電機(jī)大大降低了安裝塔架的成本[16]。在半球上的周期八字曲線如圖1所示。

圖1 在球上的八字曲線周期飛行路徑

1.1 空間八字曲線

在圖1中八字空間曲線有四個(gè)段組成，分別是東西方向的兩個(gè)小圓段(轉(zhuǎn)彎圓)，兩個(gè)大圓段(簡稱測地線，在xoy的平面投影為直線)，其交點(diǎn)為交叉點(diǎn)o，四個(gè)過渡點(diǎn)(測地線與小圓段的交點(diǎn))分別是t1、t2、t3、t4。轉(zhuǎn)彎圓的半徑Rc=Rsinθρ，θt轉(zhuǎn)彎圓的圓心與Z軸的夾角。在NED坐標(biāo)下垂直空間曲線的路徑平面的單位法向量erc如公式(1)所示[6]。指向東西(σe=±1)轉(zhuǎn)彎圓的圓心的單位法向量如公式(2)所示。

(1)

球心到轉(zhuǎn)彎圓圓心的距離D=Rcosθρ，并且球心到轉(zhuǎn)彎圓的圓心的位置向量rc=Derc。

(2)

兩個(gè)測地線相交的夾角為χo(0<χo≤π)，并且東方向兩個(gè)測地線與轉(zhuǎn)彎圓相交的過渡點(diǎn)t1、t2的位置向量如公式(3)所示：

(3)

東北、西北、東南、西南四個(gè)方向的過渡點(diǎn)的單位切向量如公式(4)所示。

(4)

其中:(σn,σe)=(+,+),(+,-),(-,+),(-,-)。

(5)

(6)

空間八字曲線飛行上的點(diǎn)都是繞著y軸旋轉(zhuǎn)-θ角，繞著z軸旋轉(zhuǎn)ψ角，結(jié)合以上公式，可以推出空間曲線的旋轉(zhuǎn)矩陣R(θ,ψ)=(eθ,eψ,-er)，其中在NED坐標(biāo)系下單位向量如公式(7)～(9)所示。

(7)

(8)

(9)

e1,e2為橫向平面子空間的基向量。

(10)

1.2 導(dǎo)航控制

導(dǎo)航控制分為(NE空間)橫向平面的導(dǎo)航控制和速度高度控制[16]。導(dǎo)航控制決定了無人機(jī)的橫向加速度和滾轉(zhuǎn)角的大小?？臻g曲線上點(diǎn)的位置向量r(φ)如圖1所示，公式如下。

R(cosθρerc+sinθρ(cosφeθc+σsinφeψc))

(11)

(12)

其中空間曲線上點(diǎn)的位置向量分為(NE空間)橫向平面位置向量和高度位置向量，如公式(13)～(14)所示。

r1(φ)=(Rcosθρsinθce1+

sinθρ(cosφcosθce1+σsinφe2))

(13)

z(φ)=-R(cosθρsinθc-cosφcosθρsinθc)

(14)

從而可以得出曲線上一點(diǎn)的橫向速度v1(φ)：

(15)

則無人機(jī)的向心加速度為a1(φ)，其中k1(φ)為曲線的曲率[6]。

(16)

無人機(jī)在飛行過程中，實(shí)際飛行路徑與預(yù)期軌跡存在一定的軌跡誤差，在圖1中無人機(jī)的位置向量為ra，無人機(jī)的橫向平面投影點(diǎn)為ve，預(yù)期軌跡上距離無人機(jī)最近點(diǎn)的位置向量rp，如公式(17)所示，其中enp為最近點(diǎn)的外部法向量。

rp=Derc+Rsinθρenp

(17)

圖2 八字飛行路徑橫向平面投影

三維空間中軌跡誤差向量為rpa=ra-rp，軌跡誤差在橫向方向的投影向量rpal=rpa-rpa,3e3，其中rpa,3e3為縱軸方向，橫向投影的軌跡誤差如公式(18)所示:

dl=sgn(rpalenpl)|rpal|

(18)

速度向量在橫向方向的投影以及速度矢量在法線方向的分量如公式(19)所示。

vpal=va-va,3e3，Vl=vpalenpl

(19)

結(jié)合運(yùn)動學(xué)方程得出無人機(jī)在空間曲線上飛行過程的加速度如公式(20)所示，其中Kr和Kv為增益系數(shù)。

ap=a1+Krdl+KvVl

(20)

(21)

本文的導(dǎo)航控制方法已經(jīng)成功在ardupilot中實(shí)現(xiàn)，代碼中將無人機(jī)在跟蹤空間曲線飛行時(shí)加速度分為兩種情況：

1)無人機(jī)在在空間曲線外飛行或進(jìn)行跟蹤直線飛行，軌跡誤差向量橫向投影dl大于零a=as。

2) 無人機(jī)在曲線內(nèi)飛行，軌跡誤差向量橫向投影dl小于零a=ap。

2 三維曲線跟蹤的自適應(yīng)導(dǎo)航控制

無人機(jī)在空中飛行可能會遭受外部環(huán)境的干擾(風(fēng)的干擾)，無人機(jī)受風(fēng)的干擾軌跡誤差增大，所以須考慮環(huán)境干擾下的自適應(yīng)性以及跟蹤性能。

通過公式(21)和(22))可以發(fā)現(xiàn)無人機(jī)的加速度與增益系數(shù)Kr和Kv，L1-Navigation導(dǎo)航中的增益系數(shù)KL1有關(guān)：

(22)

其中：T是周期，周期不改變Kr也固定不變。通過改變常數(shù)系數(shù)k1與k2，增益系數(shù)Kv和KL1隨之改變。根據(jù)圖3、圖4與表1所示，通過更改常數(shù)系數(shù)k1,k2，空間八字曲線軌跡變化，軌跡誤差也隨之改變。所以本文設(shè)計(jì)一個(gè)雙輸入單輸出的模糊控制系統(tǒng)，如圖3所示，以軌跡誤差(e)和軌跡誤差變化率(de)為輸入量，并且以常數(shù)系數(shù)k=k1,k2(u)為輸出量，根據(jù)變化的軌跡誤差來調(diào)整常數(shù)系數(shù)k，從而改變加速度的大小，減小固定翼無人機(jī)在空間曲線飛行的軌跡誤差，優(yōu)化曲線路徑。

圖3 增大k空間曲線路徑

圖4 橫向平面投影

表1 軌跡誤差根方差

2.1 選擇模糊變量

經(jīng)多次仿真測試，本文中軌跡誤差e的物理論域?yàn)閇-200，200](單位m)，模糊論域?yàn)閇-3，3]。軌跡誤差變化率的物理論域?yàn)閇-200，200](單位m)，模糊論域?yàn)閇-3，3]，其中軌跡誤差、軌跡誤差變化率的量化因子Ke=0.015，Kde=0.015。

常數(shù)系數(shù)k的物理論域u有兩種情況：

1)當(dāng)無人機(jī)在空間八字曲線內(nèi)飛行：

常數(shù)系數(shù)k1(u)的物理論域?yàn)閇1，4]，模糊論域?yàn)閇-3，3]，常數(shù)系數(shù)k1的比例因子Ku=0.833。

2)當(dāng)無人機(jī)在空間曲線外飛行、跟蹤航點(diǎn)：

常數(shù)系數(shù)k2(u)的物理論域?yàn)閇4，8]，模糊論域?yàn)閇-3，3]，常數(shù)系數(shù)k1的比例因子Ku=0.333。

軌跡誤差e和軌跡誤差變化率de都有七個(gè)模糊子集為[-3，-2，-1，0，1，2，3]，用字母表示分別是NB(誤差負(fù)最大)，NM(誤差負(fù)中)，NS(誤差負(fù)小)，ZO(誤差為零)，PS(誤差正小)，PM(誤差正中)，PB(誤差正最大)，軌跡誤差e的三角形隸屬度函數(shù)如圖5所示。常數(shù)系數(shù)u有七個(gè)模糊子集為[-3，-2，-1，0，1，2，3]，用字母表示分別是NB(系數(shù)最小)，NM(系數(shù)越小)，NS(系數(shù)小)，ZO(系數(shù)適中)，PS(系數(shù)大)，PM(系數(shù)越大)，PB(系數(shù)最大)。

圖5 軌跡誤差e三角形隸屬度函數(shù)

在跟蹤預(yù)期軌跡的過程中，與預(yù)期軌跡之間出現(xiàn)誤差，需要快速響應(yīng)調(diào)整誤差，所以模糊變量隸屬函數(shù)采用三角形隸屬函數(shù)，輸出變量的去模糊化采用面積中心法，面積中心法是隸屬度函數(shù)曲線與橫坐標(biāo)包圍面積的中心[17]，這個(gè)中心所對應(yīng)的橫坐標(biāo)就是要求的輸出變量。如公式(23)所示，其中A(uj)是u在第j點(diǎn)隸屬度函數(shù)。

(23)

2.2 模糊規(guī)則的設(shè)定

模糊規(guī)則是模糊系統(tǒng)重要的組成部分[18]，本文有49條模糊規(guī)則，是經(jīng)過多次實(shí)驗(yàn)和經(jīng)驗(yàn)所得，如表2所示。

表2 模糊規(guī)則表

表中兩個(gè)輸入量分別是軌跡誤差e和軌跡誤差變化率de，當(dāng)軌跡誤差|e|越大，誤差變化率|de|大的時(shí)候，KL1增益系數(shù)就越大，當(dāng)軌跡誤差和誤差變化率基本為零時(shí)，KL1增益系數(shù)適中，模糊規(guī)則例如下：

If e is NM and de is Z0 then u is PM;

If e is Z0 and de is PM then u is PM;

….

If e is PS and de is NS then u is Z0;

If e is PM and de is PS then u is PM.

3 仿真實(shí)驗(yàn)

為了驗(yàn)證本文的方法的有效性，本文基于開源自動駕駛儀Ardupilot平臺進(jìn)行SITL(Software in the Loop)仿真，Ardupilot是全球應(yīng)用最廣泛的開源平臺之一[19],SITL可以在沒有硬件的情況下進(jìn)行模擬飛行，可以實(shí)時(shí)修改飛行導(dǎo)航代碼，進(jìn)行編譯模擬飛行，避免無人機(jī)在真實(shí)環(huán)境中損壞。本文使用小型固定翼無人機(jī)3.10版本固件，無人機(jī)的飛行數(shù)據(jù)來自飛行模擬器中的動力學(xué)模型[20]。本文利用飛行數(shù)據(jù)閃存日志(logs)中的數(shù)據(jù)在Matlab中進(jìn)行圖像顯現(xiàn)，仿真實(shí)驗(yàn)飛行設(shè)定無人機(jī)飛行速度為22m/s，飛行實(shí)驗(yàn)主要有兩個(gè)測試：無干擾條件下的空間曲線軌跡跟蹤仿真測試，干擾條件下空間曲線跟蹤仿真測試(固定翼無人機(jī)跟蹤空間八字曲線代碼已經(jīng)上傳到github中[21])。

3.1 無干擾條件下空間曲線軌跡跟蹤仿真

為了驗(yàn)證使用模糊控制可以改進(jìn)空間曲線飛行的軌跡誤差和優(yōu)化空間曲線路徑，本文將為空間曲線飛行軌跡和使用模糊控制改進(jìn)的空間曲線飛行軌跡進(jìn)行對比，圖6為無人機(jī)跟蹤空間八字曲線模擬飛行軌跡，如圖7和圖8所示。

圖7 無干擾條件下空間曲線飛行軌跡

無人機(jī)在八字曲線內(nèi)飛行，軌跡誤差增大時(shí)，常數(shù)系數(shù)k1增大修正軌跡誤差；若無人機(jī)在八字曲線外飛行跟蹤目標(biāo)航點(diǎn)，當(dāng)軌跡誤差增大時(shí)，常數(shù)系數(shù)k2增大，修正軌跡誤差。如圖6為無人機(jī)模擬飛行，圖7為空間曲線軌跡，圖8為八字曲線軌跡的橫向投影。從表3中可以看出使用模糊控制改進(jìn)的導(dǎo)航算法跟蹤八字曲線軌跡相比于原導(dǎo)航算法的曲線軌跡估計(jì)誤差均方根小，曲線更加規(guī)則化，并且更接近預(yù)期軌跡。

3.2 干擾條件下空間曲線跟蹤仿真

為了驗(yàn)證改進(jìn)空間曲線飛行算法的實(shí)用性，在有風(fēng)干擾的條件下進(jìn)行仿真測試，風(fēng)速10 m/s，風(fēng)向西風(fēng)。圖9為風(fēng)干擾下三維曲線軌跡，圖10為風(fēng)干擾下橫向平面投影。

圖9 風(fēng)干擾下三維曲線軌跡

圖10 風(fēng)干擾下橫向平面投影

無人機(jī)在常風(fēng)的干擾下跟蹤八字曲線軌跡，如圖11所示，曲線軌跡往風(fēng)向側(cè)偏移，其中原始導(dǎo)航算法下，無人機(jī)跟蹤的八字曲線軌跡相比于使用模糊控制改進(jìn)的導(dǎo)航算法偏離的比較大。

如圖12所示，利用模糊控制改進(jìn)的導(dǎo)航算法在風(fēng)干擾的情況下可以自適應(yīng)的修正飛行軌跡，使無人機(jī)沿著設(shè)定軌跡飛行。如圖13軌跡誤差變化對比，利用模糊控制改進(jìn)的導(dǎo)航控制算法相比與原始導(dǎo)航算法，軌跡誤差變化比較平緩。從圖12、13中對比說明添加模糊控制有很好的自適應(yīng)性，根據(jù)軌跡誤差的變化自適應(yīng)的調(diào)整常數(shù)系數(shù)k，減小軌跡誤差，從而說明有良好的跟蹤效果和魯棒性。

圖11 三種情況下飛行軌跡對比

圖12 三種情況下飛行軌跡橫向平面投影對比

圖13 軌跡誤差對比

4 結(jié)束語

本文基于模糊控制改進(jìn)的L1-navigation導(dǎo)航算法，在空間八字曲線的導(dǎo)航算法中添加模糊控制，以軌跡誤差和軌跡誤差變化率為輸入量，以計(jì)算加速度的增益系數(shù)常數(shù)為輸出，改變加速度的大小，優(yōu)化軌跡誤差。在Ardupilot的SITL仿真飛行結(jié)果表明加入模糊控制的導(dǎo)航算法性能優(yōu)于原始導(dǎo)航算法，提高了空間曲線的跟蹤性能，特別是在干擾的情況下(如風(fēng))，具有很好的自適應(yīng)性和跟蹤性能。本文的方法適用于空間曲線導(dǎo)航控制。