基于模型預(yù)測(cè)靜態(tài)規(guī)劃的自適應(yīng)軌跡跟蹤算法

王萌萌，張曙光

北京航空航天大學(xué) 交通科學(xué)與工程學(xué)院，北京 100083

軌跡跟蹤需要飛行器對(duì)位置矢量等狀態(tài)量按照預(yù)期方式實(shí)施跟蹤。傳統(tǒng)的方法多基于線性化模型進(jìn)行反饋控制[1-3]，但往往受限于模型本身的非線性與狀態(tài)間耦合程度。受到關(guān)注的方法還有反饋線性化[4-6]等其他方法，然而反饋線性化方法也受限于動(dòng)態(tài)逆的對(duì)合條件與零動(dòng)力學(xué)[7]。因而，自抗擾控制器在軌跡跟蹤領(lǐng)域得到廣泛關(guān)注[8-9]?；谧顑?yōu)控制方法的模型預(yù)測(cè)控制近年來(lái)被廣泛應(yīng)用于軌跡跟蹤問(wèn)題，例如文獻(xiàn)[10-11]提出了一種使用非線性模型預(yù)測(cè)控制的無(wú)人固定翼飛行器軌跡跟蹤算法。根據(jù)不同研究對(duì)象模型的復(fù)雜與非線性程度，模型預(yù)測(cè)控制要求一定的計(jì)算能力。

在各類基于模型解算生成指令的控制方法中，模型預(yù)測(cè)靜態(tài)規(guī)劃(Model Predictive Static Programming，MPSP)是由Padhi提出的基于模型的、解決一類帶有終端約束的最優(yōu)控制問(wèn)題的高效算法[12]。其從原理上結(jié)合了模型預(yù)測(cè)控制與近似動(dòng)態(tài)規(guī)劃的特點(diǎn)，目前已有許多學(xué)者進(jìn)行了擴(kuò)展與應(yīng)用[13-15]。MPSP算法最顯著的特點(diǎn)的是其通過(guò)引入一個(gè)靜態(tài)拉格朗日算子，將問(wèn)題轉(zhuǎn)換為一個(gè)靜態(tài)優(yōu)化問(wèn)題，得出解析的關(guān)于終端誤差的控制量修正量，這一特點(diǎn)使得MPSP算法具有較高的計(jì)算效率，具備在線應(yīng)用潛力。Kumar和Padhi文獻(xiàn)[16]中進(jìn)一步提出了MPSP算法的一種擴(kuò)展應(yīng)用，引入了過(guò)程狀態(tài)約束應(yīng)用于軌跡跟蹤問(wèn)題，被稱為跟蹤模型預(yù)測(cè)靜態(tài)規(guī)劃(Tracking Model Predictive Static Programming，T-MPSP)。文獻(xiàn)[17]中將T-MPSP應(yīng)用于無(wú)人機(jī)自主著陸制導(dǎo)。作為MPSP算法的一種擴(kuò)展應(yīng)用，T-MPSP同樣繼承了MPSP算法的高效性與基于模型的特性。與傳統(tǒng)的模型預(yù)測(cè)控制(Model Predictive Control，MPC)[18]相比，MPSP及T-MPSP中不存在數(shù)值優(yōu)化的過(guò)程，直接給出了解析解，因此在實(shí)時(shí)性方面有著顯著優(yōu)勢(shì)。

同時(shí)，T-MPSP對(duì)模型精度有明顯依賴性。在一些特殊情況下，飛行器的實(shí)際情況與算法中使用的標(biāo)稱模型可能存在較大偏差。例如，當(dāng)飛行器受損時(shí)，氣動(dòng)性能可能發(fā)生退化使得氣動(dòng)參數(shù)偏離標(biāo)稱參數(shù)。文獻(xiàn)[19]中指出了當(dāng)固定翼飛機(jī)機(jī)翼受損時(shí)的氣動(dòng)參數(shù)變化問(wèn)題。其他模型擾動(dòng)也廣泛存在，例如飛行器質(zhì)量等不確定性[20]。這也就意味著，基于模型的T-MPSP算法可能無(wú)法保證實(shí)際模型偏差下的跟蹤效果。

針對(duì)上述問(wèn)題本文進(jìn)行了改進(jìn)，引入?yún)?shù)估計(jì)思想抑制模型偏離影響。提出一種基于MPSP離散時(shí)間框架下的參數(shù)估計(jì)方法，將該參數(shù)估計(jì)方法與T-MPSP相結(jié)合，得到一種自適應(yīng)T-MPSP算法。

1 自適應(yīng)軌跡跟蹤算法

為了提高對(duì)模型偏差的適應(yīng)性，本文首先對(duì)標(biāo)稱模型的實(shí)際偏離進(jìn)行在線參數(shù)估計(jì)，并將模型修正引入到跟蹤環(huán)節(jié)中。因而，提出的自適應(yīng)軌跡跟蹤算法由2部分組成，分別是基于MPSP的參數(shù)估計(jì)與T-MPSP軌跡跟蹤。2部分均使用顯含待估參數(shù)的隨時(shí)間推進(jìn)離散系統(tǒng)動(dòng)力學(xué)方程：

xk+1=Fk(xk,uk,νk)

(1)

式中：x∈Rn為狀態(tài)矢量；u∈Rm為控制矢量；ν∈Rp為參數(shù)矢量；k=1,2,…,N-1，其中N為一個(gè)離散時(shí)間區(qū)間中的最終步。為了區(qū)分參數(shù)估計(jì)部分與軌跡跟蹤部分的時(shí)間區(qū)間，分別以Ne與Nt標(biāo)記相應(yīng)的離散化步數(shù)，下標(biāo)e對(duì)應(yīng)參數(shù)估計(jì)，下標(biāo)t對(duì)應(yīng)軌跡跟蹤。

1.1 基于MPSP的參數(shù)估計(jì)方法

根據(jù)本文所建立的控制思想，給出在MPSP框架下的參數(shù)估計(jì)算法。MPSP算法框架考慮一個(gè)過(guò)去的時(shí)間區(qū)間，對(duì)應(yīng)的離散后的時(shí)間節(jié)點(diǎn)為k=1,2,…,Ne-1。為了更新當(dāng)前參數(shù)，選擇xNe作為當(dāng)前時(shí)刻的狀態(tài)量。考慮到如下輸出方程：

(2)

式中：ye∈Rqe為參數(shù)估計(jì)中考慮的輸出向量。定義當(dāng)前時(shí)刻輸出向量測(cè)量值與預(yù)測(cè)值之間的預(yù)測(cè)誤差為

(3)

(4)

對(duì)式(1)進(jìn)行泰勒展開(kāi)，并考慮到小量近似，可以得到k+1 節(jié)點(diǎn)處的誤差表示為

(5)

由于參數(shù)估計(jì)算法所考慮的時(shí)間節(jié)點(diǎn)均為過(guò)去的時(shí)間節(jié)點(diǎn)，控制矢量已知且確定，所以不考慮控制量的變化，即duk=0，式(5)可簡(jiǎn)化為

(6)

同理在當(dāng)前時(shí)刻節(jié)點(diǎn)，即Ne節(jié)點(diǎn)有

(7)

代入式(4)后,可得

(8)

將式(6)中的dxk(k=1,2,…,Ne-1)不斷代入可得

(9)

式中：

(10)

(11)

認(rèn)為初始狀態(tài)不變，則dx1=0，可以化簡(jiǎn)得到

(12)

(13)

式中：上標(biāo)0表示計(jì)算過(guò)程中的中間變量。為了構(gòu)建一個(gè)完整的最優(yōu)控制問(wèn)題結(jié)構(gòu)，選取如下目標(biāo)函數(shù)：

(14)

(15)

式中：λ為靜態(tài)拉格朗日算子，通過(guò)靜態(tài)優(yōu)化理論求解取得最優(yōu)解時(shí)的必要條件，并假設(shè)dν在各步均相等，可以得到

(16)

(17)

通過(guò)式(16)與式(17)求解參數(shù)修正量dν可得

(18)

式中：

(19)

由此，給出參數(shù)的估計(jì)值為

v=νp+dν

(20)

式中：ν為更新后的參數(shù)值；νp為更新前的參數(shù)值。值得注意的是盡管在推導(dǎo)過(guò)程中做出了參數(shù)在估計(jì)區(qū)間內(nèi)時(shí)不變的假設(shè)，但是由于算法本身的快速更新特性，可以對(duì)時(shí)變參數(shù)進(jìn)行估計(jì)。由式(12)中關(guān)系可得，補(bǔ)償了參數(shù)修正量dν后，相應(yīng)的系統(tǒng)預(yù)測(cè)輸出誤差也得到了補(bǔ)償。從而在1.2節(jié)軌跡跟蹤算法應(yīng)用中，可以保證更好的跟蹤效果。

1.2 跟蹤模型預(yù)測(cè)靜態(tài)規(guī)劃

在1.1節(jié)得到模型修正量的基礎(chǔ)上，本節(jié)將模型修正引入跟蹤控制中。根據(jù)T-MPSP算法理論[16]，輸出方程為

yk=h(xk)

(21)

類似1.1節(jié)推導(dǎo)式(5)的數(shù)學(xué)過(guò)程，不考慮參數(shù)變化，即dvk=0，可得

(22)

注意到，經(jīng)過(guò)式(20)的修正后，式(22)的系統(tǒng)動(dòng)力學(xué)方程Fk(xk,uk,νk)已使用更新后的參數(shù)，實(shí)現(xiàn)了對(duì)T-MPSP算法僅應(yīng)用帶誤差的模型參數(shù)的改進(jìn)。

類似式(7)～式(11)推導(dǎo)，可以得到

(23)

(24)

(25)

將式(25)代入式(24)中可得

(26)

考慮到小量近似，即Δyk≈dyk, Δuk≈duk，則

(27)

將式(23)代入(27,)可得

(28)

至此，目標(biāo)函數(shù)被表示為了du的函數(shù)，于是通過(guò)最優(yōu)控制理論可以得到一系列必要條件可以用于求解控制量的修正量，即du，如式(29)所示：

(29)

求解式(29)可以得到如下方程：

(30)

式中：最左側(cè)系數(shù)矩陣對(duì)角線元素為Cij+Ri，其他元素為Cij。

其中：

i=1,2,…,Nt-1;j=1,2,…,Nt-1

求解式(30),可得到控制量的修正量為

(31)

于是更新后的控制量可以表示為

(32)

至此基于T-MPSP算法的控制量求解過(guò)程結(jié)束，注意由于T-MPSP算法多次使用了小量近似，所以可能需要迭代若干次才能滿足軌跡跟蹤的效果，仿真中一般在5次以內(nèi)能收斂。此外，由于解算的結(jié)果均是顯式的解析解，迭代所需的計(jì)算量較小，所用時(shí)間較理想。

如前述，本文將模型偏離參數(shù)引入動(dòng)力學(xué)方程Fk(xk,uk,νk)中，擴(kuò)展應(yīng)用T-MPSP算法，建立自適應(yīng)修正過(guò)程，從而保證了更好的軌跡跟蹤效果。

2 仿真模型

飛行器質(zhì)點(diǎn)模型為[21]

(33)

(34)

式中：ρ、S和C分別為大氣密度、參考面積與氣動(dòng)力系數(shù)。對(duì)于軌跡問(wèn)題，假定指令操縱響應(yīng)具有一階延遲，即

(35)

模型狀態(tài)矢量與控制矢量為

(36)

通過(guò)歐拉法離散模型后得到的系統(tǒng)動(dòng)力學(xué)方程為

(37)

式中：h為離散時(shí)間步長(zhǎng)。

根據(jù)第1節(jié)，存在兩組系統(tǒng)的輸出矢量分別用于參數(shù)估計(jì)與軌跡跟蹤：

(38)

(39)

3 參數(shù)估計(jì)與軌跡跟蹤仿真分析

為驗(yàn)證自適應(yīng)T-MPSP算法的有效性，應(yīng)用參考文獻(xiàn)[16]中的原始T-MPSP與本文提出的自適應(yīng)T-MPSP在相同的飛行狀態(tài)下執(zhí)行相同的軌跡跟蹤任務(wù)。飛行器的參數(shù)在表1中給出。選定待估計(jì)參數(shù)矢量為

以模擬出現(xiàn)損傷的情況，假定損傷發(fā)生于時(shí)t=15 s，且導(dǎo)致的參數(shù)偏差為ΔCLα=-0.5, Δm=-500 kg，并且考慮到測(cè)量噪聲，測(cè)量方程為

式中：σ為測(cè)量的白噪音，白噪音的幅值為縱向位移、橫側(cè)位移、高度均為3 m，航跡偏角、航跡傾角均為0.1°，速度為1 m/s。根據(jù)CLα和m的數(shù)值大小比例，選定

(40)

同樣根據(jù)狀態(tài)量數(shù)值大小，選取Qk為以[0.3 0.3 0.3 10 10 1]為元素的對(duì)角陣。此外，選定Rk為

(41)

如此選擇的原因在于希望使得控制量在一個(gè)預(yù)測(cè)周期前段修正較小避免突變。離散步長(zhǎng)為0.2 s。Ne與Nt均為76，對(duì)應(yīng)預(yù)測(cè)周期為75個(gè)時(shí)間節(jié)點(diǎn)，共計(jì)15 s。

表1 飛行器標(biāo)稱參數(shù)Table 1 Nominal parameters in simulation test

參考文獻(xiàn)[16]中T-MPSP與本文提出的自適應(yīng)T-MPSP在相同的飛行器參數(shù)下執(zhí)行圓周盤旋上升軌跡跟蹤任務(wù)的結(jié)果對(duì)比如圖 1所示?？梢钥闯觯倪M(jìn)后的自適應(yīng)T-MPSP跟蹤參考軌跡的效果顯著好于T-MPSP算法，實(shí)際飛行軌跡與參考軌跡接近，而T-MPSP算法生成的軌跡則與參考軌跡有一定偏差。自適應(yīng)T-MPSP中參數(shù)估計(jì)過(guò)程的結(jié)果如圖 2所示，一個(gè)周期后，參數(shù)值得到了較為準(zhǔn)確的估計(jì)。由于T-MPSP僅使用標(biāo)稱模型，沒(méi)有進(jìn)行參數(shù)估計(jì)，因此，圖 2 中用于比較的變量為受損參數(shù)的真實(shí)值與自適應(yīng)T-MPSP算法得出的估計(jì)值。

圖3進(jìn)一步給出參考文獻(xiàn)[16]中的原始T-MPSP和本文自適應(yīng)T-MPSP的航跡偏角、航跡傾角與速度曲線對(duì)比，可見(jiàn)，兩種跟蹤效果的差別更加明顯。由于預(yù)測(cè)模型中存在參數(shù)誤差，T-MPSP無(wú)法跟蹤參考軌跡參數(shù)，其狀態(tài)量在參考值附近振蕩。相反，自適應(yīng)T-MPSP算法由于及時(shí)修正了模型中的參數(shù)誤差，實(shí)現(xiàn)了較好的跟蹤效果。

迎角指令、滾轉(zhuǎn)角指令與推力等軌跡控制量變化見(jiàn)圖 4。與較快變化的狀態(tài)量(軌跡角、速度)體現(xiàn)出相同的現(xiàn)象，T-MPSP給出的控制指令不斷振蕩，自適應(yīng)T-MPSP在修正了由參數(shù)變化帶來(lái)的誤差之后，僅體現(xiàn)由測(cè)量誤差引起的每一個(gè)制導(dǎo)周期的小幅修正。

從仿真結(jié)果中可以看出，本文提出的自適應(yīng)T-MPSP算法實(shí)現(xiàn)了對(duì)參數(shù)的較準(zhǔn)確估計(jì)，同時(shí)由于對(duì)模型參數(shù)進(jìn)行及時(shí)修正，所生成的控制指令能實(shí)現(xiàn)更好的軌跡跟蹤效果。且本文提供的自適應(yīng)T-MPSP具備與MPSP/T-MPSP算法相似的收斂特性，迭代次數(shù)相當(dāng)。

表2 中給出了自適應(yīng)T-MPSP，包括參數(shù)估計(jì)與軌跡跟蹤的計(jì)算耗時(shí)。共進(jìn)行軌跡跟蹤任務(wù)(飛行器圓周盤旋上升)仿真測(cè)試100次，仿真均在Windows10(i7-6700，16 G內(nèi)存)的MATLAB R2016b環(huán)境下完成。每次仿真模擬飛行150 s，共測(cè)試10個(gè)周期，每個(gè)周期計(jì)算耗時(shí)1.5～2 s。由表2得出，該算法平均計(jì)算耗時(shí)短，而實(shí)際飛行時(shí)，機(jī)載計(jì)算采用的是運(yùn)行速度快得多的計(jì)算系統(tǒng)(計(jì)算速度至少快1個(gè)數(shù)量級(jí))。從而表明提供了解析解的自適應(yīng)T-MPSP具有實(shí)時(shí)應(yīng)用潛力。

表2 參數(shù)估計(jì)與軌跡跟蹤耗時(shí)Table 2 Time consumption of estimation and tracking

4 結(jié) 論

1) 在離散模型預(yù)測(cè)靜態(tài)規(guī)劃(MPSP)框架下建立了一種高效的實(shí)時(shí)參數(shù)估計(jì)算法，可有效準(zhǔn)確估計(jì)偏離標(biāo)稱模型的參數(shù)。由于使用當(dāng)前時(shí)刻的測(cè)量值，保證了參數(shù)能夠不斷地在線更新，從而對(duì)于時(shí)變參數(shù)也能進(jìn)行較好的估計(jì)。

2) 提出的結(jié)合參數(shù)估計(jì)的自適應(yīng)跟蹤模型預(yù)測(cè)靜態(tài)規(guī)劃算法，相較T-MPSP算法，能通過(guò)模型參數(shù)更新，更好保證軌跡跟蹤的效果，同時(shí)能避免誤差的積累導(dǎo)致算法發(fā)散。

3) 算法的參數(shù)估計(jì)與軌跡跟蹤兩個(gè)部分，均是在模型預(yù)測(cè)靜態(tài)規(guī)劃的基本框架下擴(kuò)展，其解析解特性保證了較高的計(jì)算效率。