基于MPC的無人機航跡跟蹤控制器設(shè)計

王曉海, 孟秀云, 李傳旭

(北京理工大學宇航學院, 北京 100081)

0 引 言

近年來,無人機在軍事領(lǐng)域和民用領(lǐng)域中的應(yīng)用越來越廣泛,其中固定翼無人機以其航程長、飛行速度快、載重量大等優(yōu)點引起了人們的重視。固定翼無人機主要采用傾斜轉(zhuǎn)彎(bank-to-turn, BTT)的控制方式,目前主要的BTT航跡跟蹤方法有基于比例積分微分(proportional integral derivative,PID)控制的跟蹤算法、非線性制導算法、向量場算法及純追蹤與視線角(pure pursuit and line of sight, PLOS)組合制導算法等。文獻[1]使用PID控制方法與微分幾何制導算法對無人機航跡跟蹤方法進行了設(shè)計與驗證;文獻[2]將非線性制導算法進行了改進,引入了一種自適應(yīng)引導長度,提高了算法的跟蹤性能;文獻[3-4]提出了向量場算法,并使用Lyapunov方法對算法的穩(wěn)定性進行了證明;文獻[5]對PLOS算法的抗干擾能力進行了討論。

模型預(yù)測控制(model predictive control, MPC)是20世紀70年代后期提出的一類新型計算機控制算法,是一種屬于基于模型預(yù)測的啟發(fā)式控制算法[6]。模型預(yù)測控制器具有設(shè)計簡單、易于實現(xiàn)等優(yōu)點,一經(jīng)問世,便在電力、化工等復雜工業(yè)過程控制中取得了巨大成功。近年來,MPC在飛行器控制問題中得到了大量應(yīng)用。文獻[7]對MPC中用于預(yù)測的模型進行泰勒展開,得到近似的線性模型,并對線性化后的模型應(yīng)用了預(yù)測控制。文獻[8-10]將目標的運動考慮到預(yù)測模型中,用MPC實現(xiàn)了無人飛行器對運動目標的跟蹤與著陸。

本文針對固定翼無人機BTT的側(cè)向航跡跟蹤問題,采用動態(tài)逆方法將模型線性化,基于線性化后的模型,設(shè)計基于狀態(tài)擴展的雙反饋模型預(yù)測控制器,并使用量子粒子群優(yōu)化(quantum particle swarm optimization, QPSO)算法對控制器參數(shù)進行優(yōu)化,為進一步提高控制器的抗干擾性能,引入擴張狀態(tài)觀測器(extended states observer, ESO)對未知干擾進行觀測,并在控制律中進行補償。最后,結(jié)合實際工程應(yīng)用對本文設(shè)計的控制器性能進行了仿真分析。

1 無人機側(cè)向航跡跟蹤模型建立

可推導出無人機側(cè)向航跡跟蹤模型為

(1)

式中,an為無人機的側(cè)向過載,忽略無人機的側(cè)滑角影響,則an與無人機滾轉(zhuǎn)角γ的關(guān)系為

an=gtanγ

(2)

由滾轉(zhuǎn)舵偏角δx到滾轉(zhuǎn)角運動的傳遞函數(shù)[11]為

(3)

式中,Kx為無人機滾轉(zhuǎn)運動傳遞系數(shù)；Tx為無人機滾轉(zhuǎn)運動時間常數(shù)。由式(1)～式(3)可得完整的無人機側(cè)向航跡跟蹤模型為

(4)

(5)

2 側(cè)向航跡跟蹤控制器設(shè)計

2.1 動態(tài)逆線性化

考慮式(4)所示的單輸入單輸出非線性系統(tǒng)

定義 1系統(tǒng)的相對階。當以下條件滿足時,式(4)所描述的非線性系統(tǒng)在x=x(t0)的鄰域內(nèi)存在相對階r:

(6)

若系統(tǒng)在x=x(t0)處的相對階與狀態(tài)空間的維數(shù)相等,則通過坐標變換

(7)

可得原系統(tǒng)的狀態(tài)變量經(jīng)坐標變換后的坐標zi=φi(x),經(jīng)坐標變換后的系統(tǒng)的狀態(tài)空間方程為

(8)

(9)

2.2 狀態(tài)擴展模型預(yù)測控制

預(yù)測控制中的預(yù)測模型可分為參數(shù)化模型和輸入/輸出模型。參數(shù)化模型指根據(jù)系統(tǒng)運作原理進行建模得到的模型,如參數(shù)化狀態(tài)空間模型等。輸入/輸出模型指對系統(tǒng)的輸入與輸出之間的關(guān)系進行處理得到的模型,如階躍響應(yīng)模型、脈沖響應(yīng)模型、Wiener模型[12]、Hammmerstein模型[13-15]、Volterra模型[16],以及神經(jīng)網(wǎng)絡(luò)模型[17-19]與模糊模型等智能模型[20-21]。使用參數(shù)化模型的優(yōu)點是可以較為容易地對系統(tǒng)進行設(shè)計和穩(wěn)定性分析;缺點是在實際情況中,有時系統(tǒng)較為復雜,往往很難以較高的準確度建立參數(shù)化模型。輸入/輸出模型的優(yōu)點是不需要詳細了解系統(tǒng)內(nèi)部運作機理,對于難以獲得參數(shù)化模型的系統(tǒng)較為適用;其缺點在于需要對噪聲等干擾因素進行建模,需要進行參數(shù)辨識和模型校驗,過程相比于使用參數(shù)化模型來說更為繁瑣,并且有的模型(如階躍響應(yīng)模型等)只適用于描述穩(wěn)定的系統(tǒng)。無人機的參數(shù)化動力學模型可以較為容易地根據(jù)氣動數(shù)據(jù)計算得出,故本文采用參數(shù)化狀態(tài)空間模型進行設(shè)計。

對于一個單輸入單輸出線性系統(tǒng)

(10)

對其進行離散化可得

(11)

將式(11)的第一式的兩相鄰時刻的狀態(tài)作差,可得

x(k+1)-x(k)=Ad(x(k)-x(k-1))+Bd(u(k)-u(k-1))

(12)

x(k+1)=x(k)+AdΔx(k)+BdΔu(k)

(13)

則基于第k步系統(tǒng)狀態(tài),對k時刻之后i步系統(tǒng)狀態(tài)的預(yù)測值為

(14)

式中,NC為控制時域。

對于系統(tǒng)的輸出y(k),有

y(k+1)=y(k)+CdAdΔx(k)+CdBdΔu(k)

(15)

將y(k+1)擴展為一個新的狀態(tài)變量,擴展后的狀態(tài)空間模型為

xu(k+1)=Auxu(k)+BuΔu(k)

(16)

(17)

由擴展后的狀態(tài)空間模型可得狀態(tài)變量在未來時刻的預(yù)測值為

(18)

(19)

式中,NP為預(yù)測時域,則系統(tǒng)在未來NP個時刻的輸出預(yù)測可以表示為

1922年5月前后，中共湘區(qū)委員會成立（湘區(qū)轄湖南及江西萍鄉(xiāng)），毛澤東任書記，委員有何叔衡、易禮容、李立三、郭亮、劉少奇等，區(qū)委機關(guān)仍設(shè)于此。

Y=Fxu(k)+ΘΔU

(20)

式中,

MPC以滾動優(yōu)化確定控制策略。在每個時刻k,要計算出未來NC個時刻的控制增量,使系統(tǒng)在未來NP個時刻的預(yù)測輸出值逼近給定的期望值。將優(yōu)化性能指標式定義為

J=(Yref-Y)TQ(Yref-Y)+ΔUTRΔU

(21)

式中,Q=diag(q1,q2,…,qNP)為誤差加權(quán)矩陣;R=diag(r1,r2,…,rNC)為控制增量加權(quán)矩陣。聯(lián)立式(20)與式(21),得

J=[Yref-Fxu(k)]TQ[Yref-Fxu(k)]-2ΔUTΘTQ[Yref-Fxu(k)]+ΔUT(ΘTQΘ+R)ΔU

(22)

ΔU=(ΘTQΘ+R)-1ΘTQ[Yref-Fxu(k)]

(23)

在滾動優(yōu)化的策略下,只取ΔU中的第一個元素作為實際加入到系統(tǒng)輸入的控制增量,下一采樣時刻時刻加入系統(tǒng)輸入的控制增量則根據(jù)下一采樣時刻的系統(tǒng)狀態(tài)重新計算。式(23)可改寫為

ΔU=(ΘTQΘ+R)-1ΘTQYref-(ΘTQΘ+R)-1ΘTQFxu(k)

(24)

下一時刻控制變量的增量可以看作是由兩個部分組成,第一個部分是(ΘTQΘ+R)-1ΘTQYref的第一個元素,記為Kryref(k),第二個部分是(ΘTQΘ+R)-1ΘTQFxu(k)的第一個元素,記為Ksxu(k),則下一時刻系統(tǒng)狀態(tài)的預(yù)測值為

xu(k+1)=Axu(k)+BΔu(k)=(A-BKs)xu(k)+BKryref(k)

(25)

在狀態(tài)擴展MPC中,Ks的最后一列與Kr相等,將Ks寫為Ks=[Kx,Kr],而狀態(tài)變量xu(k)=[Δx(k)T,y(k)]T,故Kx表示與原系統(tǒng)狀態(tài)變量相關(guān)的增益系數(shù),Kr表示為與原系統(tǒng)中輸出變量相關(guān)的增益系數(shù)。狀態(tài)擴展MPC的框圖如圖2所示。

圖2 狀態(tài)擴展MPC框圖

從圖2中可以看出,狀態(tài)擴展MPC的形式為狀態(tài)與輸出雙反饋的結(jié)構(gòu)。而在常規(guī)的MPC中,由于沒有在狀態(tài)變量中擴展輸出變量,故其結(jié)構(gòu)上只有狀態(tài)反饋而無輸出反饋。相比于常規(guī)的MPC,由于加入了輸出反饋,將輸出擴展為新的狀態(tài)變量的MPC的輸出變量的控制趨勢會更快。

以第2節(jié)所述的模型作為被控模型,取采樣周期為0.1 s,預(yù)測時域為300,指令為常值1,對比實驗結(jié)果如圖3所示。

圖3 對比實驗仿真結(jié)果

從圖3中可以看出,當控制時域相同時,狀態(tài)擴展MPC比常規(guī)的MPC更快進入穩(wěn)態(tài)。由于控制時域?qū)PC算法的計算量有著較大的影響,故縮短控制時域?qū)τ诮档陀嬎懔烤哂休^大的意義。從仿真結(jié)果中可以推測出,當二者響應(yīng)速度相同時,狀態(tài)擴展MPC所需的控制時域會小于常規(guī)MPC,狀態(tài)擴展MPC的運算速度具有一定的先進性。

2.3 基于QPSO的MPC參數(shù)優(yōu)化

2.3.1 粒子更新方式

近年來,粒子群算法由于其建模簡單、易于實現(xiàn)等優(yōu)點在優(yōu)化問題中得到了廣泛的應(yīng)用。粒子群算法是一種群智能算法,其基本理論來源于對鳥群覓食活動特征的觀察和仿真實驗分析結(jié)果[22]。本文采用QPSO對MPC控制器參數(shù)進行優(yōu)化。QPSO中粒子位置更新方式如式(26)～式(28)所示

(26)

(27)

(28)

2.3.2 粒子的評價

粒子的評價方式?jīng)Q定了粒子群算法優(yōu)化的方向,在實際工程應(yīng)用中,往往不希望舵機偏轉(zhuǎn)角度過大。故本文中對MPC航跡跟蹤控制器參數(shù)進行優(yōu)化的目標有兩個,一個是使系統(tǒng)具有較好的動態(tài)特性,另一個是使控制輸入量較小。取粒子代價函數(shù)為

fcost=ω1f1+ω2f2

(29)

式中,f1表示與系統(tǒng)動態(tài)特性相關(guān)的代價;f2表示與控制量相關(guān)的代價;ω1和ω2為權(quán)值,為使權(quán)值更具有效性,需對f1與f2進行標準化處理。

(30)

式中,e(t)是系統(tǒng)響應(yīng)與期望值之差；te為優(yōu)化過程中所取的最大仿真時間；emax為優(yōu)化過程中人為設(shè)置的最大誤差限值,當|e(t)|>|emax|時,取|e(t)|=|emax|,emax不宜取得過小。

標準化后的與控制量相關(guān)的代價f2定義為

(31)

式中,u(t)為控制輸入量；umax為實際系統(tǒng)中控制輸入的限幅值。則粒子的代價函數(shù)為

(32)

粒子的代價函數(shù)值越低,粒子的適應(yīng)度越高,粒子越符合優(yōu)化目標的要求。

表1 標準化處理前后代價值對比

2.3.3 QPSO算法流程

QPSO算法的流程如下:

步驟 1確定優(yōu)化維數(shù)以及每一維的上下界,確定粒子評價方式,確定迭代終止條件,本文中終止條件為迭代到最大迭代次數(shù);

步驟 2初始化粒子種群;

步驟 3根據(jù)式(32)計算種群中每個粒子的代價值,記錄當前代最優(yōu)粒子pO與全局最優(yōu)粒子pGO的位置;

步驟 4根據(jù)式(26)～式(28)所描述的方法進行粒子更新;

步驟 5判斷是否達到終止條件,若達到則結(jié)束,輸出全局最優(yōu)粒子的位置,若未達到則轉(zhuǎn)到步驟3。

2.3.4 MPC參數(shù)設(shè)計

MPC控制器需要設(shè)計的參數(shù)包括采樣時間T、預(yù)測時域NP、控制時域NC、誤差加權(quán)矩陣Q和控制增量加權(quán)矩陣R。在MPC實際應(yīng)用中,要求采樣時間T滿足Shannon采樣定理?？刂茣r域NC表示了所需要得到的控制增量改變的次數(shù),由于優(yōu)化是針對預(yù)測時域NP內(nèi)的系統(tǒng)狀態(tài)進行的,故要求NC≤NP,NC的實際含義為用NC個優(yōu)化變量實現(xiàn)NP個點的輸出優(yōu)化,NP確定時,NC越小,控制的機動性越弱;NC越大,控制的機動性越強,但系統(tǒng)魯棒性越差[6]。誤差加權(quán)系數(shù)qi(i=1,2，…,P)通常取為同一個值q,控制增量加權(quán)系數(shù)ri(i=1,2，…,M)也取為同一個值r,q取得較大而r取得較小時,系統(tǒng)的響應(yīng)較快,同時控制量變化也較大;相反,當q取得較小而r取得較大時,系統(tǒng)響應(yīng)較慢,同時控制量變化較小。

可以看出,MPC需要設(shè)計的參數(shù)較多,且參數(shù)之間存在一定的相互影響,若對簡單地對所有參數(shù)優(yōu)化范圍進行設(shè)置,則參數(shù)在取值范圍內(nèi)的變化規(guī)律不具有實際意義,且優(yōu)化結(jié)果會較差,故本文對設(shè)計參數(shù)進行一定的修改與簡化。對于控制時域NC,相較于自身的取值,其與NP的比值更加具有參考意義,故本文在優(yōu)化時將其取值范圍用(0,1]來簡化代替;誤差加權(quán)系數(shù)q和控制增量加權(quán)系數(shù)r的取值是相對的,故可取r為一固定值,只改變q。對于q,在優(yōu)化時,若使其簡單地在一個線性范圍內(nèi)變化,則會使取值尺度失衡,即當q=0.5r時,q的取值為0.5,與q=r時q的取值相差為0.5,而當q=2r時,q的取值為2,與q=r時q的取值相差為1,也即粒子朝兩個方向運動時移動的距離與產(chǎn)生的效果不均等,若初始粒子種群是在全優(yōu)化空間內(nèi)隨機產(chǎn)生的,則更多地粒子會處于q>r的位置上,不利于算法尋優(yōu),故采用如下方法對q進行優(yōu)化,首先定義一變量qitm,取值范圍為(0,2),q與qitm的關(guān)系為

(33)

這樣,初始粒子在q>r和q

2.4 ESO設(shè)計

固定翼無人機航程跨度較大,飛行過程中模型參數(shù)不確定性較高,且可能存在其他未知干擾。為進一步提高系統(tǒng)的魯棒性,對無人機側(cè)向航跡跟蹤系統(tǒng)引入干擾觀測器。將系統(tǒng)參數(shù)不確定性與未知干擾合并看作系統(tǒng)的總干擾,無人機側(cè)向航跡跟蹤系統(tǒng)模型可以表示為

(34)

則系統(tǒng)的總干擾為

(35)

(36)

(37)

式中,ωo為觀測器的觀測帶寬。將觀測的總干擾代入到控制律中,實現(xiàn)對干擾的補償,則基于ESO的控制律為

(38)

3 仿真結(jié)果與分析

圖4 仿真結(jié)果分析

從仿真結(jié)果中可以看出,本文設(shè)計的控制器能夠?qū)χ噶钸M行較好的跟蹤,響應(yīng)無超調(diào),無靜差。

為驗證系統(tǒng)的抗干擾能力,現(xiàn)將干擾加入到系統(tǒng)中。考慮無人機氣動參數(shù)的不確定性和飛行過程中飛行速度變化、測量元件的誤差以及執(zhí)行機構(gòu)的響應(yīng)過程等干擾因素,在數(shù)學仿真中加入干擾,動力系數(shù)拉偏為1.2,對于飛行速度的干擾施加形式為在Vm上加入一個幅值為20 m/s的正弦擾動,即Vm=120+20 sin(0.5t),單位為m/s,在仿真系統(tǒng)中的滾轉(zhuǎn)角測量值上加入一個幅值為±0.5°的均勻分布誤差,對于執(zhí)行機構(gòu)的響應(yīng)過程這一干擾因素,本文中的側(cè)向航跡跟蹤系統(tǒng)的執(zhí)行機構(gòu)為無人機的滾轉(zhuǎn)舵,將舵機等效為一個二階環(huán)節(jié),如式(39)所示。

(39)

圖5 加入干擾后的仿真結(jié)果

可以看出,在無人機速度存在一定變化且測量元件與執(zhí)行機構(gòu)存在一定誤差的情況下,本文設(shè)計的控制器的響應(yīng)依然較為平滑且無超調(diào),在存在常值干擾力矩時,控制器能夠消除干擾力矩的影響,使系統(tǒng)響應(yīng)無靜差。

在實際情況中,期望航跡往往不是一條一成不變的直線,而是由多個連續(xù)折線段組成的。當需要無人機對折線航跡進行跟蹤時,可以選取水平面內(nèi)某一直線為基準線,以基準線為x軸,基準線所對應(yīng)的z坐標為0,MPC中的側(cè)向航跡偏差指令序列zΔei(i=1,2,…,NP)為

(40)

式中,其中k為期望航跡的斜率;b為期望航跡的截距。假設(shè)無人機初始位置的(x,z)坐標為(0 m,2 000 m),第一段期望航跡與基準線平行,距離基準線2 000 m,長度為500 m,起點為(0 m,2 000 m),終點為(500 m,2 000 m),第二段期望航跡為z=-x+2 500,起點為(500 m,2 000 m),終點為(2 500 m,0 m),第三段期望航跡與基準線重合,起始位置坐標為(2 500 m,0 m)。仿真結(jié)果如圖6所示?？梢钥闯?無人機在航跡段切換時具有一定的預(yù)見性,這是因為MPC的輸入是一個指令序列,而不是一個單獨的指令。仿真結(jié)果表明本文設(shè)計的無人機側(cè)向航跡跟蹤控制器可以較好地對折線航跡進行跟蹤。

圖6 折線航跡跟蹤效果

4 結(jié) 論

本文針對固定翼無人機BTT的轉(zhuǎn)彎控制問題,首先建立了固定翼無人機BTT側(cè)向航跡跟蹤運動模型,使用動態(tài)逆方法對建立的模型進行線性化,針對線性化后的模型設(shè)計了基于狀態(tài)擴展的雙反饋模型預(yù)測控制器。之后,利用QPSO對預(yù)測控制的參數(shù)進行優(yōu)化,并引入ESO對未知干擾進行觀測,進一步提高系統(tǒng)的魯棒性。最后,結(jié)合實際工程應(yīng)用,對控制器進行數(shù)學仿真,并對控制器的響應(yīng)性能與抗干擾能力進行了分析。仿真結(jié)果表明,本文設(shè)計的基于狀態(tài)擴展雙反饋模型預(yù)測控制的無人機側(cè)向航跡跟蹤控制器能夠在系統(tǒng)存在模型不確定性與受到動態(tài)干擾時對期望航跡進行準確、穩(wěn)定的跟蹤。