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

王曉海, 孟秀云, 李傳旭

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

0 引 言

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

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

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

1 無人機側向航跡跟蹤模型建立

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

(1)

式中,an為無人機的側向過載,忽略無人機的側滑角影響,則an與無人機滾轉角γ的關系為

an=gtanγ

(2)

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

(3)

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

(4)

(5)

2 側向航跡跟蹤控制器設計

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

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

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

(6)

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

(7)

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

(8)

(9)

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

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

對于一個單輸入單輸出線性系統(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)的預測值為

(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)變量在未來時刻的預測值為

(18)

(19)

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

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

Y=Fxu(k)+ΘΔU

(20)

式中,

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

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

(21)

式中,Q=diag(q1,q2,…,qNP)為誤差加權矩陣;R=diag(r1,r2,…,rNC)為控制增量加權矩陣。聯(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)的預測值為

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)變量相關的增益系數(shù),Kr表示為與原系統(tǒng)中輸出變量相關的增益系數(shù)。狀態(tài)擴展MPC的框圖如圖2所示。

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

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

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

圖3 對比實驗仿真結果

從圖3中可以看出,當控制時域相同時,狀態(tài)擴展MPC比常規(guī)的MPC更快進入穩(wěn)態(tài)。由于控制時域對MPC算法的計算量有著較大的影響,故縮短控制時域對于降低計算量具有較大的意義。從仿真結果中可以推測出,當二者響應速度相同時,狀態(tài)擴展MPC所需的控制時域會小于常規(guī)MPC,狀態(tài)擴展MPC的運算速度具有一定的先進性。

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

2.3.1 粒子更新方式

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

(26)

(27)

(28)

2.3.2 粒子的評價

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

fcost=ω1f1+ω2f2

(29)

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

(30)

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

標準化后的與控制量相關的代價f2定義為

(31)

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

(32)

粒子的代價函數(shù)值越低,粒子的適應度越高,粒子越符合優(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判斷是否達到終止條件,若達到則結束,輸出全局最優(yōu)粒子的位置,若未達到則轉到步驟3。

2.3.4 MPC參數(shù)設計

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

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

(33)

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

2.4 ESO設計

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

(34)

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

(35)

(36)

(37)

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

(38)

3 仿真結果與分析

圖4 仿真結果分析

從仿真結果中可以看出,本文設計的控制器能夠對指令進行較好的跟蹤,響應無超調,無靜差。

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

(39)

圖5 加入干擾后的仿真結果

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

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

(40)

式中,其中k為期望航跡的斜率;b為期望航跡的截距。假設無人機初始位置的(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)。仿真結果如圖6所示。可以看出,無人機在航跡段切換時具有一定的預見性,這是因為MPC的輸入是一個指令序列,而不是一個單獨的指令。仿真結果表明本文設計的無人機側向航跡跟蹤控制器可以較好地對折線航跡進行跟蹤。

圖6 折線航跡跟蹤效果

4 結 論

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