黃湘遠,湯霞清,武 萌
(裝甲兵工程學院控制工程系,北京100072)
5階CKF在捷聯(lián)慣導非線性對準中的應用研究
黃湘遠,湯霞清,武 萌
(裝甲兵工程學院控制工程系,北京100072)
為了提高動基座下車載捷聯(lián)慣導系統(tǒng)(strapdown inertial navigation system,SINS)的初始對準精度和縮短對準時間,在不進行粗對準的前提下,利用5階容積卡爾曼濾波(cubature Kalman filter,CKF)完成非線性對準。針對3階CKF濾波精度不高、5階高斯厄密特濾波器計算量過大的問題,基于多項式逼近的思想詳細推導了5階球面徑向容積規(guī)則,繼而提出了5階CKF并分析了該算法的濾波精度、采樣量和數(shù)值穩(wěn)定性,利用奇異值分解代替Cholesky分解來增強濾波穩(wěn)定性。實驗結(jié)果表明,該方法能夠有效完成大失準角下的非線性對準,精度高于3階CKF。
容積卡爾曼濾波;球面徑向容積規(guī)則;初始對準;動基座
隨著慣性導航技術(shù)的進步及應用環(huán)境的拓展,具備復雜環(huán)境下快速高精度的初始對準能力是捷聯(lián)慣導系統(tǒng)(strapdown inertial navigation system,SINS)的基本需求之一。此時,線性初始對準方案受到了非常大的限制,基于大失準角模型的非線性對準方案受到越來越多的關注。
高斯非線性濾波器是非線性濾波的重要組成部分,其最優(yōu)濾波框架需利用狀態(tài)后驗分布的一、二階矩來獲得當前時刻的最優(yōu)狀態(tài)估計。由于無法獲得狀態(tài)后驗理論分布,只能通過狀態(tài)的先驗信息和近似算法來估計,從而構(gòu)成了一些次優(yōu)濾波器,如擴展卡爾曼濾波(extended Kalman filter,EKF)、高斯厄密特濾波(Gauss-Hermite quadrature filter,GHQF)、無跡卡爾曼濾波(unscented Kalman filter, UKF)、中心差分卡爾曼濾波(central difference Kalman filter, CDKF)、容積卡爾曼濾波(cubature Kalman filter,CKF)等[1-6]。
EKF使用Taylor展開對非線性函數(shù)進行線性截斷,精度較低,容易發(fā)散;GHQF通過GH變換來逼近非線性積分,精度高,穩(wěn)定性強,不過采樣點多,計算量大,難以應用于高維濾波問題;UKF和CDKF采用Sigma點來計算均值與協(xié)方差,精度至少可達二階[6];CKF基于球面徑向容積規(guī)則(spherical radial cubature rule,SRCR)將非線性積分進行分解和逼近,能夠達到3階精度[4];CKF一經(jīng)提出,就獲得了大量應用[78]。為了提高CKF的濾波性能,從數(shù)值穩(wěn)定性[4,9]、計算復雜度[1011]、濾波精度[5,12-13]等角度出發(fā),出現(xiàn)了多種改進算法。
為了提高動基座下SINS的對準精度,從精度和計算量上綜合考慮,基于3階CKF推導了5階SRCR和CKF,進行了性能分析和改進。對準結(jié)果表明,該方法能夠完成任意失準角下的非線性對準,濾波精度高、穩(wěn)定性強、計算量相對較小,具有重要的工程應用價值。
導航坐標系n系為東北天(O-ENU)坐標系;載體坐標系b系為右前上(O-x yz)坐標系。系統(tǒng)航向角、俯仰角θ和橫滾角γ,姿態(tài)矩陣為Cbn。水平失準角ΦE和ΦN及方位失準角ΦU,記為為速度誤差,L,λ為緯度和經(jīng)度。陀螺Δ誤差為常值零漂εb加白噪聲ωbg,加速度計誤差為常值零偏b加白噪聲ωba。
準靜基座下或者依賴GPS/BD提供速度位置信息的阻尼SINS導航模式下,大失準角下非線性系統(tǒng)模型[14]為
式中
狀態(tài)向量取
噪聲向量取
式中,H=[02×3,I2×2,02×5];v為測量噪聲。
設離散非線性系統(tǒng)為
式中,xk和zk分別為k時刻系統(tǒng)的狀態(tài)量和觀測量;f(·)和h(·)為已知任意函數(shù);wk和vk分別為系統(tǒng)噪聲和觀測噪聲,wk~N(wk;0,Qk)和vk~N(vk;0,Rk),且互相獨立。
文獻[1]基于高斯噪聲假設和Bayes濾波估計原理推導了上述非線性系統(tǒng)的最優(yōu)濾波框架,為
式中,^xk|k-1,^zk|k-1,Pxx,Pzz和Pxz參考文獻[1,4]。
這些值均為如下形式的多維積分:
式中,f(·)為非線性函數(shù);Rn為積分區(qū)域。
該框架將非線性濾波問題轉(zhuǎn)化成非線性函數(shù)與高斯概率密度乘積的多維積分求解問題,求解積分式是非線性濾波的關鍵問題。一般情況下無法得到上述積分的解析值,需要求取數(shù)值近似解。不同的濾波算法對該積分值的求取思路和方法各不相同,典型算法有EKF、UKF、CDKF、CKF、GHQF等,各算法基本區(qū)別如下:
(1)基本原理:EKF基于Taylor展開和線性截斷, UKF使用UT變換,CDKF采用中心差分和多項式插值來避免導數(shù)計算,CKF使用3階SRCR逼近,GHQF利用GH變換逐層逼近。
(2)計算難點:EKF需計算繁瑣的雅可比陣,其他算法需進行誤差協(xié)方差陣Cholesky分解。
(3)采樣點數(shù):設系統(tǒng)維數(shù)為n,一次濾波過程中UKF需采樣2n+1次,CDKF為2n+1次,CKF為2n次,GHQF的采樣次數(shù)根據(jù)濾波精度而變,實現(xiàn)2m-1階精度需進行mn次采樣,計算量大,極易陷入維數(shù)災難。
(4)濾波精度:EKF為一階精度,UKF和CDKF達到二階精度,CKF達到3階精度,GHQF的精度根據(jù)需要設置。
(5)數(shù)值穩(wěn)定性:GHQF和CKF積分公式絕對數(shù)值穩(wěn)定,UKF和CDKF積分公式非絕對穩(wěn)定,表明前兩者的濾波穩(wěn)定性強于后兩者。
綜上,CKF為數(shù)值穩(wěn)定性強,濾波精度較高,計算量小的非線性濾波算法。為了進一步提高濾波精度,可將3階SRCR提升到5階,構(gòu)建計算量遠小于5階GHQF的5階CKF。
式中,Un為n維單位超球面;σ(·)為Un上的元素。積分式I(f)可分離為spherical積分S(r)和radial積分R兩部分,其中
3.1 5階球面徑向容積規(guī)則
令變量x=r y,其中yTy=1,r∈[0,∞),則全局范圍內(nèi)非線性函數(shù)積分I(f)在spherical-radial坐標系可寫成
討論積分式I(f)的數(shù)值解逼近精度時,一般使用簡單多項式函數(shù)來逼近和分析??紤]多項式f(y)=yd11yd22…均為非負整數(shù),次數(shù)為此時S(r)和
R轉(zhuǎn)化為
理論證明,當S(r)和R的逼近精度達到m階時,I(f)的逼近達到m階精度。3階CKF使用3階球面徑向容積規(guī)則,I(f)的逼近精度達到3階,為了提高CKF的濾波精度,在此基礎上推導5階逼近算法。
3.1.1 spherical規(guī)則
S(r)的m階逼近算法要求式(7)對所有|d|≤m的多項式應嚴格成立。
S(r)的計算可利用如下公式:
式(10)表明,當|d|為奇數(shù)時,{di}中必然存在奇數(shù),此時S=0。當|d|為偶數(shù)時,S才可能不為零,此時需計算S。并不是所有的偶數(shù)階多項式都需計算S;對于某多項式f(y)而言,對階數(shù){di}的排列順序進行簡單的置換形成的多項式,積分值S保持不變。當m=5時,需計算S值的典型形式有4種,分別為
兩種采樣方式均能使S(r)的逼近精度達到5階,本質(zhì)上沒有太大區(qū)別。本文采取文獻[5]中的采樣方式,此時可用如下逼近方式求解spherical積分:
式中,[e]i、[s]i分別為[e]n、[s]n的第i列。
將|d|≤5時各典型形式代入式(11),得:
3.1.2 radial規(guī)則
radial積分R=∫∞
0S(r)rn-1e-r2d r的m階逼近算法要求式(8)對所有不超過m階的單項式S(r)=ri應嚴格成立。
當i為奇數(shù)時,由于I(f)的對稱性可知,ωr,i的任意取值不會影響算法的逼近效果,因此只需考慮i為偶數(shù)時ωr,i的選取問題。此時,積分R的真實值可利用廣義Gauss-Laguerre積分來計算,也意味著R的Nr采樣逼近規(guī)則能夠達到2(2Nr-1)+1階精度[45]。
解此方程組,得
該方程組擁有無窮多個可能解,理論上方程組的任意解不會影響算法的逼近精度,但會影響算法的數(shù)值穩(wěn)定性。為了獲取最優(yōu)解,可利用廣義Gauss-Laguerre積分規(guī)則來求解該方程組。此時,r21和r22的最優(yōu)值為多項式方程Ln2/2-1(x)=0的解[8],其中
求解多項式方程Ln2/2-1(x)=0,有
為了獲得R的5階逼近,要求式(8)滿足如下方程組:
3.1.3 非線性函數(shù)積分
假設隨機變量x~N(x;^x,Px),則任意函數(shù)f(x)的統(tǒng)計特性E f為
將其代入式(14),可以得到
式中,Sx為Px的Cholesky分解陣。
將5階spherical規(guī)則和radial規(guī)則代入式(16),可得Ef的5階精度的逼近方式:
圖1表示R2空間中標準正態(tài)分布下3階和5階SRCR的采樣點和計算權(quán)值。3階SRCR采集某圓與坐標軸的交點,各采樣點計算權(quán)值相同;5階SRCR采集兩圓與坐標軸的交點及其中點,計算權(quán)值不盡相同,對非線性函數(shù)的逼近更精確。由于采樣點的計算權(quán)值相對較小,減小了采樣個體對積分逼近的重要性,對奇異點具有抑制作用。5階SRCR采樣點較多,需16次,計算量較大,而3階SRCR僅采樣4次。
圖1 SRCR采樣點及權(quán)重
3.2 5階CKF算法
將5階SRCR和Guass-Bayes濾波框架結(jié)合起來,得到5階CKF,具體如下。
3.2.1 時間更新
(2)計算容積點
3.3 CKF算法性能分析
濾波精度:5階CKF能夠達到5階濾波精度,高于3階CKF,與5階GHQF精度相當。
采樣點數(shù):3階CKF的采樣點數(shù)2n個,5階CKF相對較多,為4n2個,系統(tǒng)維數(shù)較大時,算法計算量較大。相對于5階GHQF所需的3n個采樣點,計算量依然可以接受。
數(shù)值穩(wěn)定性:任何濾波方法都由積分公式導出,為了衡量積分公式的數(shù)值穩(wěn)定性,定義穩(wěn)定因子I[16]為I=∑|ωi|(19)式中,ωi為各采樣點的權(quán)值。
當I>1時,積分公式是數(shù)值不穩(wěn)定的,會引入大量的截斷誤差,可能導致誤差協(xié)方差陣失去正定性,對非線性濾波器來說是致命的。CKF中I=1,說明其積分計算是絕對數(shù)值穩(wěn)定的,意味著CKF的濾波穩(wěn)定性強;UKF中,當n>3時,I=(2n-3)/3,說明UKF在處理高維問題時精度較差甚至濾波失敗;5階CKF中,當n>5時,I=3(n-2)/ (n+2),在處理高維問題時可能導致濾波失敗。非線性對準模型中,n=10,此時5階CKF不是絕對穩(wěn)定的,當初始協(xié)方差陣設置不當時,Pk失去正定性的可能性大于CKF,表明5階CKF的穩(wěn)定性弱于CKF。
3.4 5階CKF算法的改進措施
CKF算法進行多次濾波之后,由于計算舍入、模型失準等原因,可能導致Pk失去正定性,使得濾波發(fā)散。文獻[4]為了解決3階CKF的濾波穩(wěn)定問題,推導了Square-root CKF算法。由于需對采樣點的權(quán)值進行平方根運算,而高維問題中5階CKF的采樣權(quán)值出現(xiàn)了較多的負數(shù),Squareroot思想難以直接遷移過來。為了增強5階CKF的穩(wěn)定性,可利用協(xié)方差矩陣Pk的奇異值分解代替Cholesky分解[17]。由于奇異值分解的數(shù)值計算魯棒性較強,有利于提高算法的數(shù)值穩(wěn)定性。由于奇異值分解的計算量大于Cholesky分解,因此算法優(yōu)先選擇Cholesky分解,當分解失敗時使用奇異值分解。
4.1 搖擺基座下初始對準仿真
假設陀螺的常值漂移為0.02(°)/h,隨機漂移0.01(°)/h,加速度計的常值零偏0.1 m g,隨機零偏0.05 m g。緯度L= 39.84°,經(jīng)度λ=116.12°。戰(zhàn)車在某顛簸路面行駛,其姿態(tài)[18]為
對于車載捷聯(lián)慣導來說,水平姿態(tài)一般不超過10°,從而能夠獲得相對較小的初始水平失準角。設初始失準角為Φ=(10°,10°,40°)T,分別使用3階和5階CKF算法進行非線性對準仿真,仿真時間700 s。由于對準過程中水平姿態(tài)估計與速度誤差直接相關,可觀測性強,對準速度快,精度高,故不做重點分析。
圖2給出了200 s后兩種算法的方位失準角誤差估計曲線。濾波速度上,圖2表明兩種算法的估計速度相當,均需300 s以上的時間,說明高階逼近算法不能增加系統(tǒng)的可觀測性,對加快濾波速度無能為力。
圖2 搖擺基座下方位失準角誤差估計曲線
濾波精度上,圖2說明5階CKF略高于3階CKF。為了定量分析濾波精度,表1給出了對準結(jié)束前10 s對準結(jié)果平均值,結(jié)果表明:水平對準誤差ΦE和ΦN相對于方位對準誤差ΦU是小量,這與初始對準極限精度公式是一致的;5階CKF的方位對準精度提高了略0.03°,表明了算法的有效性。
表1 對準誤差比較
計算量上,3階CKF每次濾波過程中采樣20次,5階CKF需采樣400次,5階GHQF需采樣59 049次。表明5階CKF相對3階CKF采樣計算量較大,相對5階GHQF較小。在某些需要高精度的場合中5階CKF具有重要意義。為此,需結(jié)合具體應用,在計算量和對準精度之間權(quán)衡利弊,選擇合適的方案。
4.2 戰(zhàn)車行進過程中的初始對準實驗
現(xiàn)代戰(zhàn)爭的突然性和快速機動性要求戰(zhàn)車慣導系統(tǒng)能夠在行進過程中快速完成高精度對準。此時,粗對準需較長時間或者難以實現(xiàn),可考慮大失準角下的非線性對準問題。
實驗室采用高精度光纖陀螺SINS,安裝在某步兵戰(zhàn)車上,戰(zhàn)車行進過程中使用高精度GPS/BD導航芯片提供速度、位置參考信號。實驗過程中,戰(zhàn)車的行進路線如圖3所示,時間為2 h,行進中有上下坡、顛簸路面、轉(zhuǎn)彎、停車等多種環(huán)境。任意選取4段900 s的數(shù)據(jù)進行對比分析,實驗過程中由于粗對準需要較長時間,系統(tǒng)不進行粗對準,直接進行非線性精對準。由于無法提供慣導的真實姿態(tài)作為參考,使用SINS/GPS組合導航結(jié)果作為參考姿態(tài)。行進過程中,GPS/BD信號良好。
圖3 戰(zhàn)車行進中的位置曲線
圖4反應了4次動基座方位對準的均方根誤差曲線,可以看出動基座下兩種濾波的方位失準角收斂速度相當,約400 s左右均能達到0.2°左右,約600 s后誤差曲線趨于穩(wěn)定,且5階CKF的精度高于3階CKF。表2給出了對準結(jié)束前10 s對準結(jié)果平均值,發(fā)現(xiàn)方位對準精度由上3階CKF的0.141 9°提高到了0.110 7°。說明,將5階CKF應用到車輛行進時慣導系統(tǒng)的初始對準過程中,短時間內(nèi)能夠獲得和CKF相當?shù)木?一段時間后能夠取得更好的結(jié)果。
圖4 動基座方位對準均方根誤差曲線
表2 動基座對準均方根誤差
為了減少非線性濾波的計算量,最有效的方法是降低系統(tǒng)維數(shù)。其中3階CKF算法計算量為O(n),5階CKF算法的計算量為O(n2),表明隨著系統(tǒng)維數(shù)的減小,5階算法的計算量減小量更為顯著,對縮短濾波計算時間更有意義。靜基座下非線性對準中,可將狀態(tài)向量x分為線性部分xl和非線性部分xn,利用CKF-Kalman混合濾波進行處理,有效降低計算量[19]。5階CKF也可類似處理,形成5階CKF-Kalman混合濾波器,計算量降低效果更為明顯,不過動基座下非線性對準還得進行進一步的研究。
基于非線性濾波的非線性對準問題是當前研究的熱點。CKF利用球面容積徑向規(guī)則對多維非線性積分進行逼近,擺脫了對非線性模型的依賴,數(shù)值穩(wěn)定性強,計算量較小,在非線性對準中具有自身的優(yōu)點。然而,其3階濾波精度導致初始失準角偏大時對準精度不夠高。高階濾波算法意味著大的計算量,基于5階球面容積徑向規(guī)則的高階CKF能夠兼顧濾波精度和計算復雜度,在非線性對準中具有重要的應用價值。
[1]Ito K F,Xiong K Q.Gaussian filter for nonlinear filtering problem[J].IEEE Trans.on Automatic Control,2000,45(5):910-927.
[2]Julier S J,Uhlmann J K,Durrant-Whyte H F.Unscented filtering and nonlinear estimation[J].Proc.of the IEEE Aerospace and Electronic Systems,2004,92(3):401-422.
[3]Arasaratnam I,Haykin S,Hurd T R.Cubature Kalman filtering for continuous-discrete systems:theory and simulations[J]. IEEE Trans.on Signal Processing,2010,58(10):4977-4993. [4]Arasaratnam I,Haykin S.Cubature Kalman filters[J].IEEE Trans.on Automatic Control,2009,54(6):1254-1269.
[5]Jia B,Xin M,Cheng Y.High-degree cubature Kalman filter[J]. Automatica,2013,49:510-518.
[6]Zhang Z Y,Hao Y L,Zhang X.Performance evaluation of sigma point Kalman filter for SINSinitial alignment[J].Journal of Chinese Inertial Technology,2010,18(6):639-644.(張召友,郝燕玲,張鑫.Sigma點Kalman濾波在慣性導航初始對準中的性能評估[J].中國慣性技術(shù)學報,2010,18(6):639-644.)
[7]Sun F,Tang L J.Initial alignment of large azimuth misalignment angle in SINS based on CKF[J].Chinese Journal of Scientific Instrument,2012,33(2):327-333.(孫楓,唐李軍.基于CKF的SINS大方位失準角初始對準[J].儀器儀表學報,2012, 33(2):327-333.)
[8]Pesonen H,Piche R.Cubature-based Kalman filters for positioning[C]∥Proc.of the 7th Workshop on Positioning,Navigation and Communication,2010:45-49.
[9]Tang X J,Wei J L,Chen K.Square-root adaptive cubature Kalman filter with application to spacecraft attitude estimation[C]∥Proc.of the 15th International Conference on Information Fusion, 2012:1406-1412.
[10]Dahmani M,Meche A,Keche M,et al.Reduced cubature Kalman filtering applied to target tracking[C]∥Proc.of the 2nd International Conference on Control,Instrumentation and Automation,2011:1097- 1101.
[11]Qian H M,Ge L,Huang W,et al.Reduced dimension CKF algorithm and its application in SINS initial alignment[J].Systems Engineering and Electronics,2013,35(7):1492-1497. (錢華明,葛磊,黃蔚,等.降維CKF算法及其在SINS初始對準中的應用[J].系統(tǒng)工程與電子技術(shù),2013,35(7):1492-1497.)
[12]Lin X G,Xu S S,Xie Y H.Multi-sensor hybrid fusion algorithm based on adaptive square-root cubature Kalman filter[J]. Journal of Marine Science and Application,2013,12:106-111.
[13]Pei H L,Sanjeev A,Tharaka A,et al.A Gaussian-sum based cubature Kalman filter for bearings-only tracking[J].IEEE Trans.on Aerospace and Electronic Systems,2013,49(2): 1161-1176.
[14]Yan G M,Yan W S,Xu D M.Application of simplified UKF in SINS initial alignment for large misalignment angles[J]. Journal of Chinese Inertial Technology,2008,16(3):253 265. (嚴恭敏,嚴衛(wèi)生,徐德民.簡化UKF濾波在SINS大失準角初始對準中的應用[J].中國慣性技術(shù)學報,2008,16(3):253-265.)
[15]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.
[16]Wu Y X,Hu D W,Wu M P,et al.A numerical-integration perspective on Gaussian filters[J].IEEE Trans.on Signal Processing,2006,54(8):2910-2921.
[17]Zhang Q Z,Zhang S B,Liu Z P,et al.Robust cubature Kalman filter based on SVDand its application to integrated navigation[J]. Control and Decision,2014,29(2):341-346.(張秋昭,張書畢,劉志平,等.基于奇異值分解的魯棒容積卡爾曼濾波及其在組合導航中的應用[J].控制與決策,2014,29(2):341-346.)
[18]Wang Y G,Yang J S,Yu Y,et al.On the move alignment for SINS based on odometer aiding[J].Systems Engineering and Electronics,2013,35(5):1060-1063.(王躍鋼,楊家勝,蔚躍,等.基于里程計輔助的SINS動基座初始對準方法[J].系統(tǒng)工程與電子技術(shù),2013,35(5):1060-1063.)
[19]Zhao X J,Liu G B,Wang L X,et al.Augmented cubature Kalman filter/Kalman filter integrated algorithm[J].Infrared and Laser Engineering,2014,43(2):647-653.(趙曦晶,劉光斌,汪立新,等.擴展容積卡爾曼濾波-卡爾曼濾波組合算法[J].紅外與激光工程,2014,43(2):647-653.)
Application of 5th-degree CKF in SINS nonlinear initial alignment
HUANG Xiang-yuan,TANG Xia-qing,WU Meng
(Department of Control Engineering,Academy of Armored Force Engineering,Beijing 100072,China)
In order to improve alignment precision and reduce time for the initial alignment of strapdown inertial navigation system(SINS)under the moving base,a new method that nonlinear alignment with 5th-degree cubature Kalman filter(CKF)without coarse alignment is proposed.Because of lower accuracy of 3rd-degree CKF and too much calculation work of the 5th-degree Gauss-Hermite quadrature filter,a new sampling rule named 5th-degree spherical radial cubature rule based on polynomials approximation and a new CKF with 5thdegree accuracy from this rule are deduced.The filter accuracy,sampling points,numerical stability are analyzed.The singular value decomposition algorithm is used to replace Cholesky decomposition for stronger stability.The experiment results demonstrate that the method is able to complete nonlinear alignment on big misalignment angles and has higher estimation accuracy than 3rd-degree CKF.
cubature Kalman filter(CKF);spherical radial cubature rule;initial alignment;moving base
U 666.1
A
10.3969/j.issn.1001-506X.2015.03.25
黃湘遠(1988-),男,博士研究生,主要研究方向為非線性濾波、慣性導航。
E-mail:huangxiangyuan.623@163.com
湯霞清(1965),男,教授,博士,主要研究方向為慣性導航、戰(zhàn)車火控。
E-mail:tangxiaqing_001@163.com
武 萌(1981-),女,講師,博士研究生,主要研究方向為導航、制導與控制。
E-mail:wumeng0326@126.com
網(wǎng)址:www.sys-ele.com
1001-506X(2015)03-0633-06
2014 05 14;
2014 10 16;網(wǎng)絡優(yōu)先出版日期:2014 11 05。
網(wǎng)絡優(yōu)先出版地址:http://w ww.cnki.net/kcms/detail/11.2422.TN.20141105.1500.001.html
軍隊計劃項目資助課題