基于5階降維平方根-容積卡爾曼濾波的動基座對準應用研究

黃湘遠,湯霞清,武萌,吳偉勝(裝甲兵工程學院控制工程系,北京100072)

?

基于5階降維平方根-容積卡爾曼濾波的動基座對準應用研究

黃湘遠,湯霞清,武萌,吳偉勝
(裝甲兵工程學院控制工程系,北京100072)

摘要:為提高動基座下捷聯慣導系統的對準精度、數值穩定性和減小計算量,將5階容積卡爾曼濾波(CKF)、降維算法、多次離散和平方根(SR)濾波結合起來,形成5階降維SR-CKF非線性對準方案。為減小5階CKF的計算量,建立非線性-線性分離的系統模型,引入降維算法;為提高1階龍格-庫塔法的逼近精度,設計多次離散和時間更新的濾波框架;為提高數值穩定性,推導了5階降維SR-CKF;比較常規3階SR-CKF、5階CKF和5階降維SR-CKF的各項特性。實車動基座對準實驗結果表明:該方案對準精度高、數值穩定性強、計算量小,滿足應用需要。

關鍵詞:兵器科學與技術;容積卡爾曼濾波;降維;平方根濾波;多次離散

0　引言

由于準備時間縮短、使用環境苛刻、應用功能拓展等各方面需求,捷聯慣導系統(SINS)動基座非線性對準受到了越來越多的關注。

容積卡爾曼濾波[1](CKF)計算量小,數值穩定性強,精度較高,一經提出獲得了大量應用[2-3]。為了提高CKF的濾波精度,出現了正交采樣5階CKF[4-5]和非正交簡化采樣5階CKF[6]。系統維數較大時,5階算法計算量急劇增加,數值穩定性變差。

為了減小非線性濾波的計算量,文獻[7-8]分別使用3階降維CKF和邊緣采樣無跡卡爾曼濾波(UKF),只對非線性部分進行采樣,文獻[9-10]設計了容積卡爾曼濾波-卡爾曼(CKF-Kalman)和擴展CKF-Kalman組合濾波方案,將非線性部分和線性部分分開處理。本文將降維濾波引入到正交采樣5階CKF,形成5階降維CKF.實際應用中,需對非線性系統進行離散,常用4階龍格-庫塔離散算法;降維方案中,只能進行1階離散,為了避免濾波精度降低,設計了一種多次離散的方式。

SINS/北斗2代衛星導航系統(BD2)非線性對準中,常規3階CKF和5階降維CKF是絕對數值穩定的,5階CKF不是數值穩定的。絕對數值穩定并不代表濾波器一定穩定,受計算機長度和算法精度的限制,方差陣Pk也可能失去正定,導致Cholesky分解失敗,濾波失去穩定。增強濾波穩定性的方式有平方根(SR)濾波[11-12],奇異值分解(SVD)代替Cholesky分解[13]等。SR算法不需進行Cholesky分解,數值穩定性最好。本文推導了5階降維SRCKF,有效提高了非線性對準的穩定性。

為了提高SINS的動基座對準精度、數值穩定性和降低計算量,本文將5階CKF、降維濾波、多次離散和SR濾波結合起來,推導了5階降維SR-CKF,進行了實驗驗證。結果表明該方案大失準角下對準精度高,數值穩定性強,計算量小,綜合性能優于常規3階CKF,滿足應用需要,具有重要的工程應用價值。

1　歐拉角誤差的非線性對準模型

1.1大失準角下非線性對準模型

記地心慣性坐標系為i系;地球系為e系;導航系n系為東北天(OENU)坐標系;計算平臺系為p系;載體系b系為右前上(Oχyz)坐標系。

n系到b系的旋轉角度為航向角ψ、俯仰角θ和橫滾角γ,姿態矩陣為Cnb.n系到p系的旋轉角度為失準角ΦU、ΦE和ΦN,記Φ= (ΦE,ΦN,ΦU)T.載體速度vn= (υE,υN,υU)T,速度誤差δvn= (δυE,δυN,δυU)T,緯度L、經度λ和高度h,位置誤差δL,δλ,δh.陀螺測量誤差δωbib,加速度計測量誤差δfb.

大失準角下,基于歐拉角誤差的姿態、速度和位置誤差[14]為

式中:Cpn、Cω-1、δωnie、δωnen等見文獻[14]。模型要求緯度誤差δL為小量。

陸用導航一般忽略高度通道δh和δυU.對準過程中,需對陀螺和加速度計誤差進行估計和補償,可將陀螺誤差δωbib近似為常值零漂εb加白噪聲wbg,加速度計誤差δfb近似為常值零偏Δb加白噪聲wba, 即

將Δb和εb擴充為系統狀態,狀態χ、系統噪聲w和觀測量z分別取為

式中:vnI、LI、λI等為SINS的解算值;vnB、LB、λB等為BD2的測量值。

大失準角下初始對準的非線性系統為

式中:f(χ)和G可由(1)式推導;H = [04×3,I4×4, 04×5];v為測量噪聲。

1.2非線性-線性分離模型

為簡化(4)式,靜基座下可不進行位置更新,動基座下可通過外界輔助信息(如BD2提供位置、速度)形成阻尼誤差模型[14],缺點是要求BD2連續穩定。

SINS/ BD2組合導航或動基座對準中,位置和速度可觀測度高,短時間內能夠有效估計。為了保證位置誤差δL作為小量,可通過位置、速度間歇性閉環反饋來抑制速度和位置誤差的快速發散。誤差模型(1)式可改寫為

(5)式中,Cω-1、Cpn和Cpb為與失準角Φ有關的矩陣,A1、A2、B1、B2、C1和C2均與Φ無關。非線性系統中非線性因素由Φ引起,與其他變量無關,可將狀態量χ分解成非線性部分α和線性部分β,即χ= [αT,βT]T,α=Φ,β= [δvT,δpT, (Δb)T, (εb)T]T.

從而可將非線性系統(4)式改寫為如下非線性-線性分離的框架:

式中:F(α)為與α相關的狀態轉移矩陣;g(α)為與α相關的多維非線性函數。

常規系統將所有狀態均定義為非線性狀態,其維數為12,非線性-線性分離系統中非線性狀態只有失準角Φ,維數為3,線性狀態維數為9.

2　多次離散的降維濾波

為了降低濾波計算量,非線性-線性分離系統中可使用降維CKF[7]。CKF等高斯非線性濾波使用如下的Guass-Bayes最優濾波框架[15-16]:

2.13階降維CKF

考慮如下非線性離散系統:

式中:αk-1為χk-1的前m個元素;系統噪聲wk～N(wk;0,Qk);觀測噪聲vk～N(vk;0,Rk).

定理[7]已知n維隨機變量χ～N(χ;,Pχ),變量α為χ的前m個分量,即α= [χ1,…,χm]T,則因變量y = F(α)χ+ g(α)的期望,方差Py只與隨機變量α相關,值為

式中:Pα為Pχ的前m行、m列子矩陣;Rn為積分區域;Sχ、Sα分別為Pχ和Pα的Cholesky分解陣,且

根據定理和3階CKF,可推導如下3階降維CKF算法:

1)時間更新。

步驟1 容積采樣。對Pk-1進行Cholesky分解,即Pk-1= Sk-1STk-1;取為k-1的前m個變量, Sα為Sk-1的前m行、m列子矩陣。令i =1,…,2m, 且

步驟2 計算容積傳播點:

2)量測更新。

步驟5 狀態更新:

3階降維CKF從χk-1的統計特性(k-1,Pk-1)中提取α的統計特性(,Pα),進行2m次采樣并經過相關變換求得k|k-1和Pk|k-1.相比于3階CKF 的2n采樣,減少了2(n-m)個,減輕了計算負擔。

2.2連續系統的多次離散

實際應用中需對連續系統進行離散,常用4階龍格庫塔算法。然而, (6)式經4階離散無法得到離散形式(8)式,難以直接應用降維CKF.經1階龍格庫塔離散能得到該形式,但離散精度會降低,導致濾波性能下降,需減小濾波周期才能保證濾波精度。然而濾波周期受到系統觀測周期的限制,不一定能夠滿足濾波要求。為了解決該問題,可使用多次離散的方案。

將濾波周期T分成nt個濾波子周期Δt,在子周期k + jΔt時刻上利用k + (j-1)Δt對系統進行1階離散和時間更新,獲得狀態預測k + jΔt.當獲得新觀測值zk +1時,進行量測更新,獲得k + 1時刻狀態估計k +1.具體算法如下:

步驟1 k + jΔt時,進行1階龍格-庫塔離散, 即

步驟3 如果j＜nt,令k + jΔt=k + jΔt|k + (j-1)Δt和方差陣Pk + jΔt= Pk + jΔt|k + (j-1)Δt;如果j = nt,利用觀測量zk +1進行量測更新,獲得估計值k +1和Pk +1.

3　5階降維SR-CKF算法

式中:f(·)為非線性函數。

令χ= ry,yTy = 1,r∈[0,∞),積分式I(f)經spherical-radial變換可分離為spherical積分S(r)和radial積分R兩部分,為

式中:Un為n維單位球面;σ(·)為Un上的元素。

3.1兩種5階CKF性能比較

文獻[17]總結了S(r)的多種5階多項式逼近形式,文獻[4,6]采用不同形式構建了不同的5階spherical規則,采用計算量最小的5階radial規則,形成5階正交采樣CKF和5階非正交簡化采樣CKF兩種形式。

數值穩定性上,通過計算積分計算的穩定因子I[18],表明常規非線性對準中,由于非線性狀態維數n =12,兩種5階CKF均不是絕對數值穩定的,方差陣Pk易失去正定性,精度有限甚至導致濾波失敗。非線性-線性分離對準中,非線性狀態維數m = 3,兩種5階CKF均是絕對數值穩定的。

計算量上,時間更新中,5階正交采樣CKF需采樣2n2+1次,5階非正交簡化采樣CKF需采樣n2+ 3n +4次。常規非線性對準中,后者計算量較小;非線性-線性分離對準中,前者計算量較小。

數值穩定性和計算量分析表明,非線性-線性分離對準中,5階正交采樣CKF具有一定優勢。

3.25階降維CKF

1)時間更新。

步驟1 容積采樣。對Pk-1進行Cholesky分解,Pk-1= Sk-1STk-1;取為k-1的前m個變量,Sα為Sk-1的前m行、m列子矩陣。令i =1,…,2m, j = 1,…,2m(m-1),且

式中:[e]i為集合的第i個列向量,{e}mt =1= {[1,0,…,0]T,[0,1,…,0]T,…, [0, 0,…, 1 ]T; [ s ]j為集合,的第j列向量,= {(ek±el) /2;k,l =1,…,m;k＜l}.

步驟2 計算容積傳播點:

2)量測更新。

由于觀測方程為線性方程,量測更新與降維3階CKF相同。

3.35階降維SR-CKF

5階降維SR-CKF中,需進行方差陣Pk的Cholesky分解,計算量大,易出錯,數據處理精度受計算機字長限制。SR濾波利用矩陣QR分解代替Cholesky分解,降低計算機字長對精度的影響,提高濾波的數值穩定性。

結合SR-UKF[18]、3階SR-CKF[1]以及線性觀測SR-CKF[12],在5階降維CKF的基礎上可推導5階降維SR-CKF,具體如下:

1)時間更新。

步驟1 容積采樣,與5階降維CKF相同。

步驟2 計算容積傳播點:

式中:Θ(α) = F(α)Sχ

式中:S = qr(A)為[Q,R]= QR(AT),S = RT,QR(·)為矩陣QR分解[1]。S = cholupdate(S,u,±υ)為平方根矩陣S的一次Cholesky分解更新[18]。 2)量測更新。

步驟5 狀態更新:

3.4算法性能分析

常規非線性對準利用4階龍格-庫塔對12維非線性系統進行離散,使用3階SR-CKF和5階CKF 在1次濾波周期內進行1次時間更新和1次量測更新。

本文采用非線性-線性分離對準中,1次濾波周期內,使用1階龍格-庫塔進行4次系統離散,使用5階降維SR-CKF進行4次時間更新和1次量測更新。

計算量上,常規3階SR-CKF需進行1次矩陣QR分解和2n = 24次采樣,每次采樣進行4次(共96次)非線性函數計算;5階CKF需進行1次矩陣SVD分解和2n2+ 1 = 289次采樣,每次采樣進行4次(共1 156次)非線性函數計算;5階降維SRCKF需進行4次QR分解和4次Cholupdate更新,進行4(2m2+ 1) = 40次采樣,每次采樣進行1次(共40次)非線性函數計算。由于矩陣QR分解和Cholupdate更新的計算量大于非線性函數計算,計算量上5階降維SR-CKF相對3階SR-CKF具有一定的劣勢,相對于5階CKF計算量大大降低。

濾波精度上,常規3階SR-CKF的逼近精度為3階,5階降維SR-CKF為5階精度,后者濾波精度高于前者。

4　實驗驗證

實驗室將某型光纖陀螺SINS安裝在某戰車上,行駛過程中使用高精度BD2導航芯片提供速度、位置參考信號。陀螺零偏穩定性小于0.02°/ h,加速度計偏值重復性小于5×10-5g,BD2的位置精度為10 m,速度精度為0.1 m/ s.靜基座下初始對準10 min,開始行駛并進行SINS/ BD2組合導航,行駛過程包括加速、勻速、轉彎、上下坡、顛簸路面等。

使用開始跑車時的1 000 s數據進行動基座對準,分別進行常規3階SR-CKF非線性對準、5階CKF非線性對準和5階降維SR-CKF非線性-線性分離對準。實驗過程中由于無法獲得準確的俯仰、橫滾和方位角,使用SINS/ BD2組合導航結果作為參考姿態。

實驗步驟:靜基座下完成初始對準,系統開始進入SINS/ BD2組合導航,實驗過程中存儲所有數據。車輛開始行進時,在參考姿態的基礎上設置4組不同的失準角進行離線對準實驗,失準角分別為Φ1= (1°,1°,1°)、Φ2= (1°,1°,10°)、Φ3= (10°,10°, 30°)和Φ4= (15°,15°,50°)。每次跑車實驗分別進行4個非線性對準實驗,每個對準實驗分別使用3階SR-CKF、5階CKF、5階降維SR-CKF等3種方法進行對準。總共進行6次跑車實驗,將同一對準算法和同一失準角下的6次實驗的方位角均方根誤差作為該算法在該失準角下對準精度評價指標。

由于5階CKF數值穩定性較差,采用協方差矩陣的SVD分解代替常規的Cholesky分解[13]。

圖1～圖4為6次動基座對準中方位對準均方根誤差曲線,表1給出了對準結束時方位對準的均方根誤差。均方根誤差越大,對準精度越差。圖1表明小失準角下3種算法的對準結果相當;圖2表明方位失準角為10°時,三者的對準結果沒有太大區別。圖3和圖4為方位角較大時的對準結果,此時三者有了一定的區別。

圖1　失準角1下方位角均方根誤差Fig.1 Root mean square error of azimuth angle at misalignment angle 1

圖2　失準角2下方位角均方根誤差Fig.2 Root mean square error of azimuth angle at misalignment angle 2

圖3　失準角3下方位角均方根誤差Fig.3 Root mean square error of azimuth angle atmisalignment angle 3

圖4　失準角4下方位角均方根誤差Fig.4 Root mean square error of azimuth angle at misalignment angle 4

表1　不同失準角下方位對準均方根誤差Tab.1 Root mean square errors of azimuth angle at misalignment angles　(°)

對準速度上,3階SR-CKF和兩種5階CKF算法相比,誤差曲線收斂速度趨勢大致相似,嚴格意義上來說5階CKF并不能加快對準速度,這是因為對準速度由系統可觀測決定,高階濾波算法并不能改變系統的可觀測性。當失準角較大時,但5階CKF能夠使誤差收斂到同一范圍內的速度快于3階SRCKF,從某種意義上講5階CKF加快了濾波速度。

對準精度上,基于3階SR-CKF和5階CKF對準均采用基于高階離散的濾波框架,5階CKF誤差較小表明5階CKF的濾波精度高于3階SR-CKF,由表1可知失準角越大優勢越明顯。5階CKF和5階降維SR-CKF算法采用不同的濾波框架,二者誤差曲線類似,表明5階降維SR-CKF多次離散方案和5階CKF高階離散方案精度相當。

濾波計算量和數值穩定性前文做了比較詳細的分析。綜上,如果能夠將初始失準角控制10°左右,建議選擇常規3階SR-CKF完成對準;當失準角較大時,應使用5階降維SR-CKF.

圖1～圖4表明,隨著失準角增大,3種算法的對準結果變差。這是因為系統非線性程度隨著失準角增大而變強,龍格-庫塔離散的逼近精度和非線性濾波的估計精度會逐漸降低。因此條件允許下,應盡量保證失準角足夠小。當無法確定失準角的大致范圍時,建議選擇5階降維SR-CKF.

5　結論

為了提高動基座下SINS的對準精度、數值穩定性和降低計算量,本文推導了非線性-線性分離的系統結構;引入了降維CKF,為了避免1階龍格-庫塔離散造成濾波精度降低而設計了多次離散濾波;為了提高濾波精度和數值穩定性,推導了適合于動基座對準的5階降維SR-CKF;對常規3階SR-CKF高階離散、5階CKF高階離散和5階降維SR-CKF多次離散對準進行了綜合比較和實驗驗證。5階降維SR-CKF多次離散方案對準精度高,數值穩定性強,計算量較小,對失準角大小具有較強的魯棒性。

參考文獻(References)

[1]Arasaratnam I, Haykin S.Cubature Kalman filters [ J].IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[2]Ge Q b, Xu D X, Wen C L.Cubature information filters with correlated noises and their applications in decentralized fusion [J].Signal Processing, 2014, 94: 434-444.

[3]Gadsden S A, Al-Shabi M, Arasaratnam I, et al.Combined cubature Kalman and smooth variable structure filtering: a robust nonlinear estimation strategy [J].Signal Process, 2014, 96:290-299.

[4]Jia B, Xin M, Cheng Y.High-degree cubature Kalman filter [J].Automatica, 2013, 49(2): 510-518.

[5]Jia B, Xin M.Rauch-Tung-Striebel high-degree cubature Kalman smoother [ C]∥Proceddings of American Control Conference.Washington, DC, US:the American Automatic Control Council, 2013:2472-2477.

[6]Wang S Y, Feng J C, Tse C K.Spherical simplex-radial cubature Kalman filter [ J].IEEE Signal Processing Letters, 2014, 21(1): 43-46.

[7]錢華明,葛磊,黃蔚,等.降維CKF算法及其在SINS初始對準中的應用[J].系統工程與電子技術, 2013, 35(7):1492-1497.QIAN Hua-ming, GE Lei, HUANG Wei, et al.Reduced dimension CKF algorithm and its application in SINS initial alignment [J].Systems Engineering and Electronics, 2013, 35(7): 1492-1497.(in Chinese)

[8]李方能,許江寧,亓洪標.基于邊緣采樣UKF濾波的捷聯慣導初始對準方法[J].中國慣性技術學報, 2014, 22(5): 612-618.LI Fang-neng, XU Jiang-ning, QI Hong-biao.Initial alignment of strapdown inertial navigation system based on marginalized unscented Kalman filter[J].Journal of Chinese Inertial Technology, 2014, 22(5):612-618.(in Chinese)

[9]孫楓,唐李軍.Cubature卡爾曼濾波-卡爾曼濾波算法[J].控制與決策, 2012, 27(10): 1561-1565.SUN Feng, TANG Li-jun.Cubature Kalman filter-Kalman filter algorithm [J].Control and Decision, 2012, 27 (10): 1561-1565.(in Chinese)

[10]趙曦晶,劉光斌,汪立新,等.擴展容積卡爾曼濾波-卡爾曼濾波組合算法[J].紅外與激光工程, 2014,43(2): 647-653.ZHAO Xi-jing, LIU Guang-bin, WANG Li-xin, et al.Augmented cubature Kalman filter/ Kalman filter integrated algorithm [J].Infrared and Laser Engineering, 2014, 43(2): 647-653.(in Chinese)

[11]Chandra K P B, Gu D W, Postlethwaite I.Square root cubature information filter[J].IEEE Sensors Journal, 2013, 13(2): 750-758.

[12]Wang S Y, Feng J C, Tse C K.Novel cubature Kalman filtering for systems involving nonlinear states and linear measurements [J].International Journal of Electronics and Communications, 2015, 69: 314-320.

[13]張秋昭,張書畢,劉志平,等.基于奇異值分解的魯棒容積卡爾曼濾波及其在組合導航中的應用[J].控制與決策,2014, 29(2): 341-346 ZHANG Qiu-zhao, ZHANG Shu-bi, LIU Zhi-ping, et al.Robust cubature Kalman filter based on SVD and its application to integrated navigation[J].Control and Decision, 2014,29(2): 341-346.(in Chinese)

[14]嚴恭敏,嚴衛生,徐德民.基于歐拉平臺誤差角的SINS非線性誤差模型研究[J].西北工業大學學報, 2009,27(4): 511-516.YAN Gong-min, YAN Wei-sheng, XU De-min.A SINS nonlinear error model reflecting better characteristics of SINS errors[J].Journal of Northwestern Polytechnical University, 2009, 27(4): 511-516.(in Chinese)

[15]Ito K F, Xiong K Q.Gaussian filter for nonlinear filtering problem [ J].IEEE Transactions on Automatic Control, 2000, 45(5):910-927.

[16]Julier S J, Uhlmann J K, Durrant-Whyte H F.Unscented filtering and nonlinear estimation [ J].Proceedings of the IEEE Aerospace and Electronic Systems, 2004, 92(3):401-422.

[17]Lu J, Darmofal D L.Higher-dimensional integration with Gaussian weight for applications in probabilistic design [ J].SIAM Journal on Scientific Computing, 2004, 26(2):613-624.

[18]Wu Y X, Hu D W, Wu M P, et al.A numerical-integration perspective on Gaussian filters [J].IEEE Transactions on Signal Processing, 2006, 54(8): 2910-2921.

Research on Initial Alignment of Moving Base with 5th-degree Dimensionality Reduction SR-CKF

HUANG Xiang-yuan, TANG Xia-qing, WU Meng, WU Wei-sheng
(Department of Control Engineering, Academy of Armored Force Engineering, Beijing 100072, China)

Abstract:In order to achieve higher alignment precision, stronger numerical stability and lower computational cost for nonlinear alignment of strapdown inertial navigation system (SINS) on moving base, a scheme of 5th-degree dimensionality reduction SR-CKF nonlinear alignment is proposed,which combines 5th-degree cubature Kalman filter (CKF), dimensionality reduction algorithm, multiple discretization, and square root(SR) filter.A nonlinear-linear separation system model is established, and the dimensionality reduction algorithm is introduced to reduce the calculated amount.A multiple discretization and time update filter framework is designed to improve the approximation accuracy.The 5th-degree dimensionality reduction SR-CKF is deduced to improve the numerical stability.The features of the conventional 3rd-degree SR-CKF, 5th-degree CKF and the proposed algorithm are compared.The experimental results show that the proposed method has a high alignment precision, strong numerical stability and little calculated amount, which meets the application requirements.

Key words:ordnance science and technology; cubature Kalman filter; dimensionality reduction; square root filter; multiple discretization

作者簡介:黃湘遠(1988—),男,博士研究生。E-mail: huangxiangyuan.623@163.com;湯霞清(1965—),男,教授,博士生導師。E-mail: tangxiaqing_001@163.com

基金項目:軍隊計劃項目(51309030106)

收稿日期:2015-06-02

DOI:10.3969/ j.issn.1000-1093.2016.02.004

中圖分類號:U666.1

文獻標志碼:A

文章編號:1000-1093(2016)02-0219-07