UKF濾波算法在兩點(diǎn)邊值問題求解中的應(yīng)用

藏潔，劉升剛

(1.北京空間飛行器總體設(shè)計(jì)部，北京 100094；2.北京航空航天大學(xué) 宇航學(xué)院，北京 100191)

0 引言

在最優(yōu)化問題的間接法求解算法中，根據(jù)最優(yōu)點(diǎn)的必要性條件，原最優(yōu)化問題轉(zhuǎn)化為一個(gè)兩點(diǎn)邊值問題(two point boundary value problem,TPBVP)，兩點(diǎn)邊值問題的解就是原問題的最優(yōu)解[1]。通常兩點(diǎn)邊值問題所描述的是一個(gè)有限維動(dòng)力系統(tǒng)，在其有限的單維自變量區(qū)間的2個(gè)端點(diǎn)上存在與系統(tǒng)維數(shù)相同個(gè)數(shù)的若干約束，要求求解滿足上述約束的系統(tǒng)狀態(tài)的時(shí)間軌跡。

約束是分別施加于2個(gè)端點(diǎn)，故這2個(gè)端點(diǎn)的狀態(tài)都存在一定的自由度。兩點(diǎn)邊值問題求解的關(guān)鍵就是尋找2個(gè)端點(diǎn)狀態(tài)之間精確的定量關(guān)系，或者具有足夠精度的近似關(guān)系。不過除了十分簡(jiǎn)單的情況，一般來說從原最優(yōu)化問題轉(zhuǎn)化得到的兩點(diǎn)邊值問題所對(duì)應(yīng)的系統(tǒng)都是比較復(fù)雜的非線性系統(tǒng)，自變量區(qū)間2個(gè)端點(diǎn)處的系統(tǒng)狀態(tài)之間的關(guān)系都是具有高敏度、高非線性特性的，且很難得到解析表達(dá)式?，F(xiàn)在主流的兩點(diǎn)邊值問題求解算法都是數(shù)值求解算法，基本可分為2類:打靶法(shooting method)和轉(zhuǎn)錄/配置法(transcription/collocation)[2]。

打靶法的設(shè)計(jì)變量是考慮始端約束條件下，始端自由狀態(tài)變量，通過數(shù)值方法計(jì)算得到終端系統(tǒng)狀態(tài)變量以及終端狀態(tài)變量相對(duì)始端狀態(tài)變量的雅可比矩陣，然后根據(jù)終端狀態(tài)約束和雅可比矩陣計(jì)算始端狀態(tài)的修正量，由此迭代重復(fù)進(jìn)行上述計(jì)算，直至終端約束滿足，得到兩點(diǎn)邊值問題的精確解[3-5]。打靶法中終端狀態(tài)和始端狀態(tài)之間定量關(guān)系是對(duì)原系統(tǒng)的一階線性近似，對(duì)于高敏度高非線性特性的系統(tǒng)模型，要求打靶迭代的狀態(tài)初始猜測(cè)值具有較高的精度，否則難以收斂到精確解。為了降低系統(tǒng)敏度對(duì)打靶法的影響，提高打靶法的魯棒性，在此基礎(chǔ)上發(fā)展了多重打靶法[6]，將系統(tǒng)狀態(tài)軌跡分為若干段，相鄰段之間需要滿足連續(xù)性約束條件[7]，每一段內(nèi)采用打靶法求解。多重打靶法降低了模型敏度，提高了魯棒性，但本質(zhì)上并沒有解決打靶法對(duì)狀態(tài)初值敏感的問題，并且引入額外的設(shè)計(jì)變量和約束。

轉(zhuǎn)錄/配置法[8-9]將自變量在取值區(qū)間上離散化，以所有離散節(jié)點(diǎn)所對(duì)應(yīng)的系統(tǒng)狀態(tài)變量為設(shè)計(jì)變量，節(jié)點(diǎn)之間的狀態(tài)變量通過多項(xiàng)式插值得到。在自變量區(qū)間上選擇配置點(diǎn)，要求在配置點(diǎn)上滿足系統(tǒng)的動(dòng)力學(xué)方程，這實(shí)際上是有限維的等式約束，由此將動(dòng)力系統(tǒng)的動(dòng)力學(xué)方程約束轉(zhuǎn)化為代數(shù)方程約束，同時(shí)考慮原問題的邊值約束，將兩點(diǎn)邊值問題轉(zhuǎn)化為一個(gè)非線性規(guī)劃問題[7,10]。為了保證在配置點(diǎn)上的導(dǎo)數(shù)計(jì)算精度，一般采取Hermite-Simpson插值方法[11]計(jì)算系統(tǒng)狀態(tài)變量的導(dǎo)數(shù)。與打靶法相比，打靶法在求解過程中始終保證滿足系統(tǒng)動(dòng)力學(xué)方程，而轉(zhuǎn)錄/配置法在求解過程中不能保證滿足系統(tǒng)動(dòng)力學(xué)方程，一般不需要進(jìn)行大計(jì)算量的數(shù)值積分，但是其設(shè)計(jì)變量維數(shù)大大增加，并且同樣存在對(duì)狀態(tài)初值敏感的問題。

針對(duì)目前兩點(diǎn)邊值問題求解算法對(duì)初值十分敏感、難以收斂的問題，本文提出基于UKF(unscented Kalman filter)濾波算法的新的兩點(diǎn)邊值問題求解方法。其基本思路是將原動(dòng)力系統(tǒng)隨機(jī)化，假設(shè)系統(tǒng)狀態(tài)變量都滿足高斯分布，將原兩點(diǎn)邊值問題轉(zhuǎn)換為一個(gè)最優(yōu)參數(shù)估計(jì)問題。采用Unscented變換，可以根據(jù)系統(tǒng)始端狀態(tài)變量的概率分布，得到經(jīng)過非線性變換后系統(tǒng)終端狀態(tài)變量概率分布的二階以上精度近似，由此得到系統(tǒng)始端狀態(tài)變量和終端狀態(tài)變量之間二階以上精度的定量關(guān)系。采用UKF濾波算法，可以遞推修正系統(tǒng)始端狀態(tài)，直至濾波收斂，得到原兩點(diǎn)邊值問題的解。由于此方法對(duì)系統(tǒng)的近似描述具有二階以上精度，直觀上可以確定其收斂域要遠(yuǎn)大于上述僅僅具有一階精度的傳統(tǒng)求解方法，對(duì)狀態(tài)初值敏感的程度大大降低。

1 Kalman濾波和Unscented Kalman濾波算法簡(jiǎn)介

目前，估計(jì)理論主要有三大類方法:最小二乘估計(jì)、極大似然估計(jì)和貝葉斯估計(jì)。從最小二乘法和貝葉斯估計(jì)出發(fā)，在一定假設(shè)條件下，都可以推導(dǎo)出Kalman濾波。從最小二乘法理論理解，Kalman濾波本質(zhì)上是線性系統(tǒng)的線性最小方差估計(jì)；從貝葉斯估計(jì)理論理解，Kalman濾波本質(zhì)上是高斯線性系統(tǒng)的序列Bayes估計(jì)[12-13]。通常貝葉斯估計(jì)需要相關(guān)變量完整的概率分布描述，在高斯分布的假設(shè)下，只需要變量的均值、方差和協(xié)方差即可。最小二乘估計(jì)(最小方差估計(jì))只需要變量的均值、方差和協(xié)方差，但精確的最小方差估計(jì)求解比較困難，以線性最小方差為目標(biāo)，可以采用Hilbert空間投影定理直接求解。

考慮在狀態(tài)空間中描述的一個(gè)離散系統(tǒng)，

(1)

(2)

若式(1)是線性系統(tǒng)的情況，上述遞推線性最小方差估計(jì)的實(shí)現(xiàn)(很容易計(jì)算隨機(jī)變量經(jīng)線性變換后的均值和協(xié)方差)就是Kalman filter(KF)。對(duì)于式(1)是非線性系統(tǒng)的情況，產(chǎn)生的困難是準(zhǔn)確計(jì)算隨機(jī)變量經(jīng)非線性變換后的均值和協(xié)方差。Extended Kalman filter(EKF)是將非線性系統(tǒng)近似為一階線性系統(tǒng)，但精度較低。Unscented Kalman filter(UKF)由Julier首先提出[14]，是在Kalman filter基礎(chǔ)上增加了UT變換(unscented transformation)，專門處理非線性系統(tǒng)的最優(yōu)估計(jì)問題。

UT變換可以得到隨機(jī)變量經(jīng)非線性變換后二階精度以上的均值和協(xié)方差，基本思想是近似非線性函數(shù)的概率分布比近似非線性函數(shù)本身更容易，它采取一定規(guī)則的Sigma采樣點(diǎn)集合S來表征隨機(jī)變量的基本統(tǒng)計(jì)特征，然后對(duì)所有Sigma點(diǎn)進(jìn)行非線性變換，通過加權(quán)計(jì)算非線性變換后的隨機(jī)變量的基本統(tǒng)計(jì)特征。這種方法不依賴于非線性函數(shù)的具體形式，關(guān)鍵在于采樣策略，即Sigma點(diǎn)的個(gè)數(shù)，分布和相應(yīng)的權(quán)值。

(3)

(4)

將所有的Sigma點(diǎn)進(jìn)行非線性變換G處理，

(5)

(6)

上面公式中一些常數(shù)的含義如下：

(2)常量κ≥0，以保證方差矩陣的半正定性，一般取為0或者3-n。

(3)β是與x的先驗(yàn)分布相關(guān)的常量，對(duì)于高斯分布，β=2是最優(yōu)的。對(duì)于非高斯分布，這個(gè)參數(shù)還可以用來控制誤差。

(7)

不考慮系統(tǒng)噪聲，經(jīng)式(1)非線性變換，

(8)

xk經(jīng)過(1)作用后的均值和方差為

(9)

(10)

(11)

不考慮觀測(cè)噪聲，經(jīng)式(1)非線性變換，

(12)

根據(jù)UT變換，

(13)

(14)

而xk+1和yk+1的協(xié)方差矩陣為

(15)

對(duì)于線性最小方差估計(jì)，

(16)

(17)

KF和UKF本質(zhì)上都是高斯系統(tǒng)的線性最小方差估計(jì)(只需要隨機(jī)變量概率密度分布的一階矩和二階矩即可計(jì)算最優(yōu)估計(jì))，而KF是線性系統(tǒng)的精確實(shí)現(xiàn)，UKF是非線性系統(tǒng)二階精度以上的實(shí)現(xiàn)。濾波過程的核心是計(jì)算相關(guān)變量的方差矩陣，只有正確計(jì)算方差矩陣才能保證濾波的收斂性和無偏性。系統(tǒng)狀態(tài)變量的方差分為2部分，之前時(shí)刻變量方差的傳遞和當(dāng)前系統(tǒng)噪聲的影響。之前時(shí)刻的狀態(tài)變量方差體現(xiàn)了對(duì)狀態(tài)變量先驗(yàn)知識(shí)的了解，系統(tǒng)的噪聲體現(xiàn)了對(duì)模型先驗(yàn)知識(shí)的了解。在實(shí)際應(yīng)用中，系統(tǒng)模型是不可能完全精確建模，為了彌補(bǔ)模型和真實(shí)情況的偏差，必須給定合適的系統(tǒng)噪聲，以體現(xiàn)這部分的影響，否則濾波器沒有足夠的增益對(duì)狀態(tài)變量進(jìn)行更新修正，導(dǎo)致濾波發(fā)散，具體的體現(xiàn)可能就是狀態(tài)變量的方差很小，但濾波結(jié)果遠(yuǎn)偏離實(shí)際值。適當(dāng)增加系統(tǒng)噪聲的方差，可以增加濾波器的穩(wěn)定性[15]，降低對(duì)狀態(tài)初值的敏感性，但是這會(huì)降低模型的預(yù)測(cè)精度，增加濾波結(jié)果的誤差。從另一角度來理解此結(jié)論，Kalman濾波發(fā)散是由于模型的偏差使得只能在短時(shí)間內(nèi)保證預(yù)測(cè)正確性，而Kalman濾波算法是根據(jù)全部的測(cè)量預(yù)測(cè)結(jié)果來估計(jì)當(dāng)前的狀態(tài)。增加系統(tǒng)噪聲方差，使得在濾波過程中，更早的觀測(cè)數(shù)據(jù)會(huì)更快地被舍棄掉，而給最近的觀測(cè)數(shù)據(jù)賦予更大的權(quán)值，由此可以避免濾波發(fā)散。

2 UPE算法簡(jiǎn)介及其在兩點(diǎn)邊值問題求解中的應(yīng)用

UPE，即UKF parameter estimation。假設(shè)存在非線性映射，

yk=G(xk,w),k=1,2,…,∞,

(18)

式中：(xk,yk)為已知的映射對(duì)序列；w為一組未知的參數(shù)，參數(shù)估計(jì)即根據(jù)上述非線性映射和映射對(duì)序列，估計(jì)未知的參數(shù)w。將非線性映射進(jìn)行如下變換：

(19)

這2種方法思路都是在濾波初段設(shè)置較大的系統(tǒng)噪聲方差，保證濾波收斂，在濾波后段使得系統(tǒng)噪聲方差減小，保證濾波精度。算法流程如下：

(1)初始化

(20)

(2)狀態(tài)更新

對(duì)于k∈{1,2,…,∞}狀態(tài)更新和Sigma點(diǎn)計(jì)算

(21)

Yk|k-1=G(xk,Wk|k-1),

(22)

(3)觀測(cè)更新

(23)

兩點(diǎn)邊值問題也可以看作一個(gè)參數(shù)估計(jì)問題。存在如下動(dòng)力系統(tǒng)，

(24)

式中：x為n維狀態(tài)變量；u為給定驅(qū)動(dòng)，在時(shí)間區(qū)間t∈[t0,tf]上分析系統(tǒng)響應(yīng)。時(shí)間區(qū)間2個(gè)端點(diǎn)的狀態(tài)變量記為x0=x(t0)和xf=x(tf)，當(dāng)給定x0即可根據(jù)式(24)確定xf，xf可記為x0的函數(shù)xf=g(x0)。假設(shè)對(duì)x0存在k個(gè)約束，對(duì)xf存在n-k個(gè)約束，形式為

(25)

上述即為常用的兩點(diǎn)邊值問題完整描述。由式(25)可以消除x0的k個(gè)自由度，假設(shè)余下的n-k個(gè)自由度記為w，x0可記為w的函數(shù)x0=h(x)。式(25)的終端約束可以變換為

0=G(w)=Gf(g(h(w))).

(26)

式(26)可看作一個(gè)參數(shù)估計(jì)問題，和式(18)相比，xk隱含在w中，yk恒等于0，可以采用UPE算法得到兩點(diǎn)邊值問題的解。

3 算例

考慮高超聲速飛行器上升段飛行過程，動(dòng)力為固體火箭發(fā)動(dòng)機(jī)，采用縱向平面內(nèi)動(dòng)力學(xué)方程，不考慮地球自轉(zhuǎn)和弧度，忽略控制系統(tǒng)延遲，根據(jù)瞬時(shí)平衡假設(shè)，歸一化的質(zhì)心動(dòng)力學(xué)方程為

(27)

(28)

氣動(dòng)力為

(29)

(30)

Cx=(a2M2+a1M+a0)+(b2M2+b1M+b0)α2，

(31)

Cy=(c2M2+c1M+c0)+(d2M2+d1M+d0)α,

(32)

式中：M為馬赫數(shù)；α為攻角；ai，bi，ci，di為常系數(shù)。

初始邊界為

(33)

終端邊界為

(34)

優(yōu)化指標(biāo)為

(35)

(36)

協(xié)態(tài)方程為

(37)

式(37)右端下標(biāo)代表對(duì)應(yīng)偏導(dǎo)數(shù)，具體形式略。最優(yōu)控制α*使得沿最優(yōu)軌跡哈密爾頓函數(shù)最小，滿足必要條件，

(38)

將式(29)～(30)代入式(38)，得

(39)

式(39)可能存在多解，需要確定使得式(36)最小的值。將式(39)再次對(duì)α求偏導(dǎo)

Hαα=k1sinα+k2cosα+k3=

(40)

式中：k1，k2，k3對(duì)應(yīng)相應(yīng)的系數(shù)。

如果式(40)無解，那么式(39)只有一個(gè)解，如果式(40)有解，將其記為

(41)

式中:β定義如下：

(42)

橫截條件為

(43)

組成了兩點(diǎn)邊值問題的描述。算例初末狀態(tài)見表1。

表1 初末軌道要素和計(jì)算參數(shù)Table 1 Elementary and final orbital elements and calculation parameters

圖1 考慮不同大氣影響下狀態(tài)變量時(shí)間歷程Fig.1 Considering the time history of state variables under different atmospheric influences

圖2 考慮不同大氣影響下協(xié)態(tài)變量時(shí)間歷程Fig.2 Considering the time history of co-state variables under different atmospheric influences

圖3 考慮不同大氣影響下控制變量時(shí)間歷程Fig.3 Considering the time history of control variables under different atmospheric influences

根據(jù)仿真結(jié)果，所有的邊界條件和最優(yōu)必要條件都滿足。對(duì)于本文給定末端條件的顯著特點(diǎn)是，有大氣影響時(shí)末速最大的彈道并不是直接達(dá)到給定的15 km高度，而是盡快達(dá)到較高的高度，然后下滑拉平。從物理意義來看，在具有足夠的爬高能力時(shí)，快速達(dá)到較高的高度，可以顯著減少氣動(dòng)阻力損失，增大最終末狀態(tài)的能量。

4 結(jié)束語

UPE是基于UKF的參數(shù)估計(jì)方法，適用于非線性模型的參數(shù)估計(jì)問題，具有較大的收斂域。兩點(diǎn)邊值問題可以轉(zhuǎn)化為一個(gè)非線性參數(shù)估計(jì)問題，因此可以采用UPE進(jìn)行求解。與傳統(tǒng)的兩點(diǎn)邊值問題求解算法相比，其收斂域更大，對(duì)初值精度要求不高，因此大大降低了兩點(diǎn)邊值問題的求解難度。

兩點(diǎn)邊值問題在諸多科學(xué)問題中都有涉及，特別是間接法最優(yōu)化問題中，最終將最優(yōu)化問題轉(zhuǎn)化為一個(gè)兩點(diǎn)邊值問題來求解，這在航空航天器軌跡優(yōu)化與最優(yōu)制導(dǎo)中應(yīng)用廣泛。本文給出的算例，驗(yàn)證了UPE算法求解兩點(diǎn)邊值問題應(yīng)用在軌跡優(yōu)化中的可行性和優(yōu)越性，但對(duì)于大氣層內(nèi)的軌跡優(yōu)化問題，仍存在一些不足之處需要改進(jìn)。

后續(xù)研究方向主要分為3個(gè)方向：①UKF濾波的方差初值和根據(jù)濾波情況自適應(yīng)調(diào)整系統(tǒng)方差的算法；②UPE算法在直接法最優(yōu)化問題中的應(yīng)用；③粒子濾波算法在兩點(diǎn)邊值問題中的應(yīng)用。