基于降維CKF和平滑的SINS/OD動基座對準(zhǔn)

黃湘遠(yuǎn), 湯霞清, 武　萌

(裝甲兵工程學(xué)院控制工程系, 北京 100072)

?

基于降維CKF和平滑的SINS/OD動基座對準(zhǔn)

黃湘遠(yuǎn), 湯霞清, 武萌

(裝甲兵工程學(xué)院控制工程系, 北京 100072)

為了提高捷聯(lián)慣導(dǎo)(strapdowninertialnavigationsystem,SINS)/里程計(jì)(odometer,OD)動基座對準(zhǔn)精度,使對準(zhǔn)過程中具備一定精度的位置導(dǎo)航能力,提出了一種動基座非線性對準(zhǔn)方法。首先對非線性系統(tǒng)進(jìn)行簡化并應(yīng)用降維容積卡爾曼濾波(reduceddimensioncubatureKalmanfilter,RD-CKF)進(jìn)行非線性對準(zhǔn),保證非線性對準(zhǔn)過程中能夠進(jìn)行位置導(dǎo)航,對準(zhǔn)結(jié)束時保證失準(zhǔn)角為小角度。然后非線性R-T-S平滑至初始時刻進(jìn)行校正,利用卡爾曼濾波再次對準(zhǔn),獲得對準(zhǔn)結(jié)束時刻高精度的姿態(tài)和位置導(dǎo)航。進(jìn)行了實(shí)車實(shí)驗(yàn),實(shí)驗(yàn)結(jié)果表明了方案的有效性。

容積卡爾曼濾波; 降維; 初始對準(zhǔn);R-T-S平滑

0　引　言

捷聯(lián)慣性導(dǎo)航(strapdowninertialnavigationsystem,SINS)/里程計(jì)(odometer,OD)的組合導(dǎo)航已經(jīng)成為陸用導(dǎo)航主要方式之一[1-2]。

晃動基座或動基座下SINS的初始對準(zhǔn)常用慣性系對準(zhǔn)方案[3-4],動基座對準(zhǔn)中必須引入外參考速度和位置。由于里程計(jì)不能提供絕對的速度和位置信息,文獻(xiàn)[5]提出一種動基座慣性系對準(zhǔn)方案,忽略對準(zhǔn)過程中的位置變化,長時間后位置誤差較大。為了保證對準(zhǔn)結(jié)束后位置精度,文獻(xiàn)[6]設(shè)計(jì)了逆向?qū)Ш剿惴?回溯到原點(diǎn)后進(jìn)行再次導(dǎo)航,文獻(xiàn)[7]基于同樣的思路設(shè)計(jì)了SINS/多普勒測速儀(Dopplervelocitylog，DVL)組合導(dǎo)航的動基座對準(zhǔn)方案。為了加快對準(zhǔn)速度,文獻(xiàn)[8]提出了基于狀態(tài)估計(jì)的卡爾曼濾波逆向?qū)Ш剿惴?文獻(xiàn)[9-10]提出了嚴(yán)格逆向過程的羅經(jīng)回溯對準(zhǔn)和循環(huán)解算方法,為了充分激發(fā)誤差,一次解算時間段不能太短。相關(guān)文獻(xiàn)表明逆向思路的導(dǎo)航處理或狀態(tài)估計(jì)可加快對準(zhǔn)速度和解決對準(zhǔn)結(jié)束后位置導(dǎo)航問題,都沒有考慮對準(zhǔn)過程中的位置導(dǎo)航。為了使對準(zhǔn)過程中具有一定精度的位置導(dǎo)航能力,可使用SINS/OD非線性對準(zhǔn)方案。

容積卡爾曼濾波(cubatureKalmanfilter,CKF)是一種優(yōu)秀的非線性濾波器,精度高，數(shù)值穩(wěn)定性強(qiáng)[11-12]。為了減小非線性對準(zhǔn)中CKF的計(jì)算量,文獻(xiàn)[13]設(shè)計(jì)了靜基座下大方位失準(zhǔn)角的降維CKF對準(zhǔn)方案。本文基于SINS/OD非線性對準(zhǔn)模型,首先對系統(tǒng)狀態(tài)進(jìn)行降維,推導(dǎo)了適合該模型的降維CKF算法。對準(zhǔn)結(jié)束后方位精度較高,位置精度一般。為了提高對準(zhǔn)結(jié)束后的位置精度,引入逆向處理。逆向處理過程也可通過非線性R-T-S平滑進(jìn)行狀態(tài)估計(jì)[14-15]來實(shí)現(xiàn)。

本文首先進(jìn)行非線性對準(zhǔn),對準(zhǔn)結(jié)束時利用R-T-S平滑和初始姿態(tài)進(jìn)行修正,然后利用存儲數(shù)據(jù)再次進(jìn)行線性對準(zhǔn)。最后進(jìn)行了實(shí)車實(shí)驗(yàn),結(jié)果表明，再次對準(zhǔn)之后,方位精度得到一定提高,位置精度得到了較大的改善,該方案同時實(shí)現(xiàn)了高精度對準(zhǔn)和位置導(dǎo)航。

1　SINS/OD非線性模型及簡化

1.1SINS誤差模型

首先定義相關(guān)坐標(biāo)系,記慣性系為i系,地球系為e系,導(dǎo)航系n系為東北天坐標(biāo)系，計(jì)算平臺系為p系，載體系b系為右前上坐標(biāo)系。

文獻(xiàn)[16]給出了基于歐拉角誤差模型的SINS姿態(tài)、速度和位置誤差模型,本文將其改寫成

(1)

式中,A′、A″、B′、B″、C′和C″均為3×3階矩陣,非零項(xiàng)為

該模型要求緯度誤差δL為小量,為了保證大失準(zhǔn)角下δL為小量,通過位置、速度間歇性閉環(huán)反饋來修正SINS速度位置誤差。

1.2OD誤差模型

設(shè)OD坐標(biāo)系為m系,y軸沿車體縱軸向前,x軸沿車體橫軸向右,z軸構(gòu)成右手系。

假設(shè)OD在ti-1～ti時刻內(nèi)輸出的脈沖數(shù)為Ni,則OD測得速度大小vD為

vD=KDNi/TD

(2)

式中,KD為OD標(biāo)度因數(shù);TD=ti-ti-1。

(3)

(4)

(5)

其中

1.3SINS/OD對準(zhǔn)系統(tǒng)模型

(6)

(7)

系統(tǒng)噪聲量w為

(8)

(9)

由式(1)、式(6)和式(9)構(gòu)成SINS/OD初始對準(zhǔn)的非線性系統(tǒng),方程為

(10)

式中,f(x)和h(x)為非線性函數(shù);系統(tǒng)噪聲w～N(0,Q),測量噪聲v～N(0,R), Q、R為噪聲方差陣。

(11)

非線性系統(tǒng)退化為線性系統(tǒng),可利用卡爾曼濾波完成對準(zhǔn)。

1.4系統(tǒng)降維及狀態(tài)分離框架

其次,SINS/OD動基座對準(zhǔn)中,系統(tǒng)導(dǎo)航誤差主要由大失準(zhǔn)角φ導(dǎo)致,ξD引起的導(dǎo)航誤差相對較小。因此,為了減小非線性濾波的計(jì)算量,可先不考慮ξD,將SINS/OD非線性對準(zhǔn)系統(tǒng)進(jìn)行降維。此時,系統(tǒng)狀態(tài)量變?yōu)?/p>

(12)

系統(tǒng)觀測方程變成

(13)

(14)

式中,F(α)為狀態(tài)轉(zhuǎn)移矩陣;g(α)、h(α)為非線性函數(shù);H為線性矩陣。

2　基于降維CKF的非線性對準(zhǔn)

考慮如式(15)所示的特殊非線性系統(tǒng):

(15)

式中,αk為n維變量xk的前m個元素;βk為xk的后l=n-m個元素;系統(tǒng)噪聲wk和觀測噪聲vk獨(dú)立,wk～N(0,Qk)和vk～N(0,Rk)。

為了降低非線性濾波的計(jì)算量,基于“只采樣非線性部分”的思路,引入降維CKF。針對系統(tǒng)式(15),設(shè)計(jì)對應(yīng)的降維采樣和濾波方式。

2.1時間更新

(16)

式中,Pξ為Px的前m行、m列子矩陣;Sx為Px的Cholesky分解陣；Sξ為Sx的前m行、m列子矩陣。且

步驟 1采樣

步驟 2狀態(tài)傳播

計(jì)算Fi=F(ξi,k-1),gi=g(ξi,k-1),Θi=Θ(ξi,k-1),Φi=Φ(ξi,k-1),Ψi=Ψ(ξi,k-1),Ωi=Ω(ξi,k-1)。

(17)

2.2量測更新

(18)

式中,Pξ為Px前m行、m列子矩陣,Pη為Px后l行、l列子矩陣,Pξη為Px的前m行、后l列子矩陣;Sx為Px的Cholesky分解陣,Sξ為Sx的前m行、m列子矩陣,Sη為Sx的后l行、l列子矩陣,Sηξ為Sx的后l行、前m列子矩陣。

證明可借鑒文獻(xiàn)[13]的證明思路,按照相同步驟來證明定理2。為了節(jié)略篇幅,僅列出主要證明步驟,忽略相關(guān)細(xì)節(jié)。

其次,求解Py。對Px做Cholesky分解

將Px和Sx改寫成

其中

舍棄繁瑣的推導(dǎo)步驟,有

則

其中

舍棄中間步驟,得

從而,可以得到

證畢

步驟 1重采樣

步驟 2觀測傳播

計(jì)算Ti=T(ξi,k|k-1),i=(ξi,k|k-1),hi=h(ξi,k|k-1)。

(19)

步驟 4狀態(tài)更新

(20)

2.3非線性對準(zhǔn)仿真

圖1　車輛行駛軌跡圖Fig.1　Vehicle driving trajectory

分別設(shè)置兩組初始失準(zhǔn)角[1°,1°,10°]和[3°,3°,20°],使用降維CKF進(jìn)行對準(zhǔn)實(shí)驗(yàn),仿真時間700 s。由于水平對準(zhǔn)速度快、精度高,只分析方位對準(zhǔn)情況。圖2給出了兩組實(shí)驗(yàn)中方位對準(zhǔn)結(jié)果,圖3給出了對準(zhǔn)過程中的水平位置誤差。

圖2　方位對準(zhǔn)誤差Fig.2　Azimuth alignment error

圖3　水平位置誤差Fig.3　Horizontal position error

3　非線性R-T-S平滑

非線性對準(zhǔn)結(jié)束時,在此基礎(chǔ)上進(jìn)行SINS/OD組合導(dǎo)航,將帶來較大的初始位置誤差。應(yīng)急情況下,可直接使用此對準(zhǔn)結(jié)果,導(dǎo)航過程中出現(xiàn)其他參考信息時可進(jìn)行校正。正常工作中,當(dāng)計(jì)算速度較快或有多個處理器時,為了減小位置誤差,可借用數(shù)據(jù)存儲和循環(huán)解算的思想,利用逆向?qū)Ш交驙顟B(tài)平滑對導(dǎo)航信息進(jìn)行再處理。為了減小計(jì)算量,應(yīng)用狀態(tài)平滑。

平滑分為R-T-S平滑[17-18]和TF平滑[19],前者計(jì)算量小,原理簡單,應(yīng)用廣泛。非線性R-T-S平滑在前向非線性濾波的基礎(chǔ)上進(jìn)行后向遞推,獲得此前任意時刻狀態(tài)估計(jì)，估計(jì)過程如下:

步驟 2后向遞推,按照k=N,N-1,…,1的順序進(jìn)行遞推

(21)

經(jīng)過后向遞推后,獲得各時刻的狀態(tài)平滑估計(jì),由于后向遞推利用的觀測信息多于前向?yàn)V波,估計(jì)精度優(yōu)于濾波。

圖4給出了上述仿真實(shí)驗(yàn)中第二組實(shí)驗(yàn)方位對準(zhǔn)的平滑估計(jì)結(jié)果,可以看出整個平滑過程中,方位失準(zhǔn)角為小角度。將SINS/OD非線性對準(zhǔn)結(jié)果進(jìn)行后向平滑估計(jì),獲得初始時刻失準(zhǔn)角估計(jì);此時對系統(tǒng)進(jìn)行修正,可獲得小誤差的SINS初始姿態(tài),可將非線性誤差模型退化為線性模型。

圖4　方位平滑估計(jì)誤差Fig.4　Azimuth alignment smoothing estimation error

4　實(shí)驗(yàn)驗(yàn)證

4.1對準(zhǔn)算法設(shè)置

根據(jù)車輛實(shí)際情況,設(shè)計(jì)如下對準(zhǔn)方案:

(1) 粗對準(zhǔn)

為了在較短時間里獲得較好估計(jì)效果,非線性對準(zhǔn)要求對初始方位失準(zhǔn)角進(jìn)行一定的控制,失準(zhǔn)角過大,對準(zhǔn)時間變長,無法收斂。在車輛發(fā)動下,采集30s左右的數(shù)據(jù),進(jìn)行慣性系粗對準(zhǔn),獲得初始姿態(tài)。

(2) 降維非線性對準(zhǔn)

車輛開始行駛,利用降維CKF進(jìn)行SINS/OD非線性對準(zhǔn),并存儲相關(guān)數(shù)據(jù)(SINS、OD、濾波數(shù)據(jù)等)。機(jī)動一段時間后,完成對準(zhǔn)。

(3) 非線性R-T-S平滑

基于存儲的濾波數(shù)據(jù)進(jìn)行非線性R-T-S平滑,初始時刻進(jìn)行校正,獲得初始姿態(tài)。

(4) 再次對準(zhǔn)

基于存儲的SINS和OD數(shù)據(jù),使用小角度線性誤差模型和卡爾曼濾波,再次進(jìn)行對準(zhǔn)。

4.2跑車實(shí)驗(yàn)

實(shí)驗(yàn)室將某型光纖陀螺SINS安裝在某戰(zhàn)車上,使用戰(zhàn)車自身安裝OD的輸出信號。光纖陀螺的零偏穩(wěn)定性不超過0.02 (°)/h,加速度計(jì)的偏值重復(fù)性小于0.05mg。首先需對SINS和OD的安裝偏差角、OD標(biāo)定系數(shù)進(jìn)行初始標(biāo)定,其中OD輸出脈沖當(dāng)量為 0.011 2米/脈沖。

表1　方位對準(zhǔn)結(jié)果

表2　經(jīng)度誤差

表3　緯度誤差

非線性對準(zhǔn)結(jié)束后,方位精度為0.224°,經(jīng)度誤差為129.87 m,緯度誤差為143.54 m,對準(zhǔn)過程中能夠提供相當(dāng)精度的位置導(dǎo)航。經(jīng)過R-T-S平滑后和再次線性對準(zhǔn)后,方位精度減為0.1°,經(jīng)度誤差為10.91 m,緯度誤差為10.01 m,對準(zhǔn)精度和位置精度大幅提高。再次對準(zhǔn)大大減小了后續(xù)SINS/OD組合導(dǎo)航的初始位置誤差,繼而提高整個導(dǎo)航過程的位置誤差。

5　結(jié)　論

為了實(shí)現(xiàn)SINS/OD動基座對準(zhǔn)中的位置導(dǎo)航,設(shè)計(jì)了“首次降維非線性對準(zhǔn)、非線性后向平滑、再次線性對準(zhǔn)”的動基座對準(zhǔn)方案。該方案對準(zhǔn)過程中具備位置導(dǎo)航能力,對準(zhǔn)結(jié)束后獲得高精度的姿態(tài)精度和位置精度。該方案嚴(yán)格來說是一種離線方案,而實(shí)際應(yīng)用需使用實(shí)時方案。為了實(shí)時實(shí)現(xiàn)該方案,一種方案是設(shè)計(jì)雙DSP結(jié)構(gòu)。DSP1用來處理前向過程,DSP2用來處理后向平滑和再次對準(zhǔn)。當(dāng)前向非線性對準(zhǔn)收斂后,啟動DSP2進(jìn)行平滑和再次對準(zhǔn)。由于DSP處理速度較快,DSP2處理時刻最終會趕上DSP1。此時,利用DSP2信息對DSP1進(jìn)行校正。實(shí)際過程中,還需進(jìn)行進(jìn)一步研究。

[1] Wu Y X, Wu M P, Hu X P, et al. Self-calibration for land navigation using inertial sensors and odometer: observability analysis[C]∥Proc.oftheAIAAConferenceofGuidance,NavigationandControl, 2009: 1-10.

[2] Georgy J, Karamat T,Umar I, et al. Enhanced MEMS-IMU / odometer/GPS integration using mixture particle filter[J].GPSSolutions, 2011, 15(3):239-252.

[3] Silson P M G. Coarse alignment of a ship’s strapdown inertial attitude reference system using velocity loci[J].IEEETrans.onInstrumentationandMeasurement, 2011, 60(6): 1930-1941.

[4] Gu D Q, El-Sheimy N, Hassan T, et al. Coarse alignment for marine SINS using gravity in the inertial frame as a reference[C]∥Proc.oftheIEEEPosition,LocationandNavigationSymposium, 2008:961-965.

[5] Yan G M, Qin Y Y, Wei Y X, et al. New initial alignment algorithm for SINS on moving based[J].SystemsEngineeringandElectronics, 2009, 31(3):634-637. (嚴(yán)恭敏,秦永元,衛(wèi)育新,等.一種適用于SINS動基座初始對準(zhǔn)的新算法[J].系統(tǒng)工程與電子技術(shù),2009, 31(3):634-637.)

[6] Yan G M, Yan W S, Xu D M. On reverse navigation algorithm and its application to SINS gyro-compass in-movement alignment[C]∥Proc.ofthe27thChineseControlConference, 2008: 724-729.(嚴(yán)恭敏,嚴(yán)衛(wèi)生,徐德民. 逆向?qū)Ш剿惴捌湓诮萋?lián)羅經(jīng)動基座初始對準(zhǔn)中的應(yīng)用[C]∥第27屆中國控制會議論文集,2008: 724-729.)

[7] Li W L, Wu W Q, Wang J L, et al. A novel backtracking navigation scheme for autonomous underwater vehicles[J].Measurement, 2014, 47: 496-504.

[8] Gao W,Ben Y Y,Zhang X,et al. Rapid fine strapdown INS alignment method under marine mooring condition[J].IEEETrans.onAerospaceandElectronicSystem, 2011, 47(4): 2887-2896.

[9] Liu X X, Xu X S, Wang L H, et al. A fast compass alignment method for SINS based on saved data and repeated navigation solution[J].Measurement, 2013, 46(10):3836-3846.

[10] Li J S, Xu J N, He H Y, et al. Backtracking scheme for alignment with strict reverse process based on compass[J].SystemsEngineeringandElectronics, 2014, 36(2): 348-353.(李京書, 許江寧, 何泓洋,等. 嚴(yán)格逆向過程的羅經(jīng)回溯對準(zhǔn)方法[J].系統(tǒng)工程與電子技術(shù), 2014, 36(2):348-353.)

[11] Arasaratnam I, Haykin S. Cubature Kalman filters[J].IEEETrans.onAutomaticControl, 2009, 54(6): 1254-1269.

[12] Wang S Y, Feng J C, Tse C K. Spherical simplex-radial cubature Kalman filter[J].IEEESignalProcessingLetters, 2014, 21(1): 43-46.

[13] Qian H M, Ge L, Huang W, et al. Reduced dimension CKF algorithm and its application in SINS initial alignment[J].SystemsEngineeringandElectronics, 2013,35(7): 1492-1497.(錢華明,葛磊,黃蔚,等. 降維CKF算法及其在SINS初始對準(zhǔn)中的應(yīng)用[J].系統(tǒng)工程與電子技術(shù), 2013, 35(7):1492-1497.)

[14] Arasaratnam I, Haykin S. Cubature Kalman smoothers[J].Automatica, 2011, 47(10): 2245-2250.

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

[16] Yan G M, Yan W S, Xu D M. A SINS nonlinear error model reflecting better characteristics of SINS errors[J].JournalofNorthwesternPolytechnicalUniversity, 2009, 27(4): 511-516. (嚴(yán)恭敏,嚴(yán)衛(wèi)生,徐德民. 基于歐拉平臺誤差角的 SINS 非線性誤差模型研究[J].西北工業(yè)大學(xué)學(xué)報(bào), 2009, 27(4): 511-516.)

[17] Wang X X, Pan Q, Liang Y, et al. Application of unscented transformation for nonlinear state smoothing[J].ActaAutomaticaSinica, 2012, 38(7):1107-1112.

[18] Jia B, Xin M. Rauch-Tung-Striebel high-degree cubature Kalman smoother[C]∥Proc.oftheAmericanControlConference, 2013: 2472-2477.

[19] Ma Y H, Fang J C, Li J L. Accurate estimation of lever arm in SINS/GPS integration by smoothing methods[J].Measurement, 2014, 48: 119-127.

ResearchonmovingbaseinitialalignmentofSINS/ODwithreduceddimensionCKFandsmoother

HUANGXiang-yuan,TANGXia-qing,WUMeng

(Department of Control Engineering, Academy of Armored Force Engineering, Beijing 100072, China)

Inordertoimprovethealignmentprecisionofstrapdowninertialnavigationsystem(SINS) /odometer(OD),andtomakeithavethepositionnavigationcapabilitywithacertainprecision,anonlinearalignmentmethodonthemovingbaseisproposed.Firstly,thenonlinearsystemissimplified,andthealignmentiscarriedoutbyusingthereduceddimensioncubatureKalmanfilter(RD-CKF),forthatpositionnavigationcanbeperformedinthenonlinearalignment,andensuringthatthemisalignmentanglesaresmallattheendofthealignment.Then,thenonlinearR-T-Ssmootherisusedtotheinitialtimeforattitudecalibration,andatwicealignmentismadebymeansofKalmanfiltertorealizehighprecisionattitudeandpositionnavigationattheendofthealignment.Theexperimentiscarriedoutinarealvehicletodemonstratetheeffectivenessoftheproposedmethod.

cubatureKalmanfilter(CKF);reduceddimension;initialalignment;R-T-Ssmoother

2015-07-16；

2015-11-17；網(wǎng)絡(luò)優(yōu)先出版日期：2016-02-18。

軍隊(duì)計(jì)劃項(xiàng)目資助課題

U666.1

ADOI:10.3969/j.issn.1001-506X.2016.09.24

黃湘遠(yuǎn)(1988-),男,博士研究生,主要研究方向?yàn)榉蔷€性濾波、慣性導(dǎo)航。

E-mail:huangxiangyuan.623@163.com

湯霞清(1965-),男,教授,博士,主要研究方向?yàn)閼T性導(dǎo)航、戰(zhàn)車火控。

E-mail:tangxiaqing_001@163.com

武萌(1981-),女,講師,博士研究生,主要研究方向?yàn)閷?dǎo)航、制導(dǎo)與控制。

E-mail:wumeng0326@126.com

網(wǎng)絡(luò)優(yōu)先出版地址：http://www.cnki.net/kcms/detail/11.2422.TN.20160218.1211.008.html