一種空間曲線軌跡跟蹤的無人機自適應(yīng)導(dǎo)航控制算法

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

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

0 引言

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

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

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

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

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

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

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

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

1.1 空間八字曲線

在圖1中八字空間曲線有四個段組成，分別是東西方向的兩個小圓段(轉(zhuǎn)彎圓)，兩個大圓段(簡稱測地線，在xoy的平面投影為直線)，其交點為交叉點o，四個過渡點(測地線與小圓段的交點)分別是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)

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

(3)

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

(4)

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

(5)

(6)

空間八字曲線飛行上的點都是繞著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)航控制決定了無人機的橫向加速度和滾轉(zhuǎn)角的大小?？臻g曲線上點的位置向量r(φ)如圖1所示，公式如下。

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

(11)

(12)

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

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

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

(13)

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

(14)

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

(15)

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

(16)

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

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é)合運動學(xué)方程得出無人機在空間曲線上飛行過程的加速度如公式(20)所示，其中Kr和Kv為增益系數(shù)。

ap=a1+Krdl+KvVl

(20)

(21)

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

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

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

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

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

通過公式(21)和(22))可以發(fā)現(xiàn)無人機的加速度與增益系數(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è)計一個雙輸入單輸出的模糊控制系統(tǒng)，如圖3所示，以軌跡誤差(e)和軌跡誤差變化率(de)為輸入量，并且以常數(shù)系數(shù)k=k1,k2(u)為輸出量，根據(jù)變化的軌跡誤差來調(diào)整常數(shù)系數(shù)k，從而改變加速度的大小，減小固定翼無人機在空間曲線飛行的軌跡誤差，優(yōu)化曲線路徑。

圖3 增大k空間曲線路徑

圖4 橫向平面投影

表1 軌跡誤差根方差

2.1 選擇模糊變量

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

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

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

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

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

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

軌跡誤差e和軌跡誤差變化率de都有七個模糊子集為[-3，-2，-1，0，1，2，3]，用字母表示分別是NB(誤差負(fù)最大)，NM(誤差負(fù)中)，NS(誤差負(fù)小)，ZO(誤差為零)，PS(誤差正小)，PM(誤差正中)，PB(誤差正最大)，軌跡誤差e的三角形隸屬度函數(shù)如圖5所示。常數(shù)系數(shù)u有七個模糊子集為[-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]，這個中心所對應(yīng)的橫坐標(biāo)就是要求的輸出變量。如公式(23)所示，其中A(uj)是u在第j點隸屬度函數(shù)。

(23)

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

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

表2 模糊規(guī)則表

表中兩個輸入量分別是軌跡誤差e和軌跡誤差變化率de，當(dāng)軌跡誤差|e|越大，誤差變化率|de|大的時候，KL1增益系數(shù)就越大，當(dāng)軌跡誤差和誤差變化率基本為零時，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 仿真實驗

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

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

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

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

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

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

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

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

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

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

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

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

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

圖13 軌跡誤差對比

4 結(jié)束語

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