基于降維CKF和平滑的SINS/OD動基座對準

黃湘遠, 湯霞清, 武　萌

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

?

基于降維CKF和平滑的SINS/OD動基座對準

黃湘遠, 湯霞清, 武萌

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

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

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

0　引　言

捷聯慣性導航(strapdowninertialnavigationsystem,SINS)/里程計(odometer,OD)的組合導航已經成為陸用導航主要方式之一[1-2]。

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

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

本文首先進行非線性對準,對準結束時利用R-T-S平滑和初始姿態進行修正,然后利用存儲數據再次進行線性對準。最后進行了實車實驗,結果表明，再次對準之后,方位精度得到一定提高,位置精度得到了較大的改善,該方案同時實現了高精度對準和位置導航。

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

1.1SINS誤差模型

首先定義相關坐標系,記慣性系為i系,地球系為e系,導航系n系為東北天坐標系，計算平臺系為p系，載體系b系為右前上坐標系。

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

(1)

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

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

1.2OD誤差模型

設OD坐標系為m系,y軸沿車體縱軸向前,x軸沿車體橫軸向右,z軸構成右手系。

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

vD=KDNi/TD

(2)

式中,KD為OD標度因數;TD=ti-ti-1。

(3)

(4)

(5)

其中

1.3SINS/OD對準系統模型

(6)

(7)

系統噪聲量w為

(8)

(9)

由式(1)、式(6)和式(9)構成SINS/OD初始對準的非線性系統,方程為

(10)

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

(11)

非線性系統退化為線性系統,可利用卡爾曼濾波完成對準。

1.4系統降維及狀態分離框架

其次,SINS/OD動基座對準中,系統導航誤差主要由大失準角φ導致,ξD引起的導航誤差相對較小。因此,為了減小非線性濾波的計算量,可先不考慮ξD,將SINS/OD非線性對準系統進行降維。此時,系統狀態量變為

(12)

系統觀測方程變成

(13)

(14)

式中,F(α)為狀態轉移矩陣;g(α)、h(α)為非線性函數;H為線性矩陣。

2　基于降維CKF的非線性對準

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

(15)

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

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

2.1時間更新

(16)

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

步驟 1采樣

步驟 2狀態傳播

計算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列子矩陣。

證明可借鑒文獻[13]的證明思路,按照相同步驟來證明定理2。為了節略篇幅,僅列出主要證明步驟,忽略相關細節。

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

將Px和Sx改寫成

其中

舍棄繁瑣的推導步驟,有

則

其中

舍棄中間步驟,得

從而,可以得到

證畢

步驟 1重采樣

步驟 2觀測傳播

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

(19)

步驟 4狀態更新

(20)

2.3非線性對準仿真

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

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

圖2　方位對準誤差Fig.2　Azimuth alignment error

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

3　非線性R-T-S平滑

非線性對準結束時,在此基礎上進行SINS/OD組合導航,將帶來較大的初始位置誤差。應急情況下,可直接使用此對準結果,導航過程中出現其他參考信息時可進行校正。正常工作中,當計算速度較快或有多個處理器時,為了減小位置誤差,可借用數據存儲和循環解算的思想,利用逆向導航或狀態平滑對導航信息進行再處理。為了減小計算量,應用狀態平滑。

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

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

(21)

經過后向遞推后,獲得各時刻的狀態平滑估計,由于后向遞推利用的觀測信息多于前向濾波,估計精度優于濾波。

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

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

4　實驗驗證

4.1對準算法設置

根據車輛實際情況,設計如下對準方案:

(1) 粗對準

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

(2) 降維非線性對準

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

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

基于存儲的濾波數據進行非線性R-T-S平滑,初始時刻進行校正,獲得初始姿態。

(4) 再次對準

基于存儲的SINS和OD數據,使用小角度線性誤差模型和卡爾曼濾波,再次進行對準。

4.2跑車實驗

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

表1　方位對準結果

表2　經度誤差

表3　緯度誤差

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

5　結　論

為了實現SINS/OD動基座對準中的位置導航,設計了“首次降維非線性對準、非線性后向平滑、再次線性對準”的動基座對準方案。該方案對準過程中具備位置導航能力,對準結束后獲得高精度的姿態精度和位置精度。該方案嚴格來說是一種離線方案,而實際應用需使用實時方案。為了實時實現該方案,一種方案是設計雙DSP結構。DSP1用來處理前向過程,DSP2用來處理后向平滑和再次對準。當前向非線性對準收斂后,啟動DSP2進行平滑和再次對準。由于DSP處理速度較快,DSP2處理時刻最終會趕上DSP1。此時,利用DSP2信息對DSP1進行校正。實際過程中,還需進行進一步研究。

[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. (嚴恭敏,秦永元,衛育新,等.一種適用于SINS動基座初始對準的新算法[J].系統工程與電子技術,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.(嚴恭敏,嚴衛生,徐德民. 逆向導航算法及其在捷聯羅經動基座初始對準中的應用[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.(李京書, 許江寧, 何泓洋,等. 嚴格逆向過程的羅經回溯對準方法[J].系統工程與電子技術, 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初始對準中的應用[J].系統工程與電子技術, 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. (嚴恭敏,嚴衛生,徐德民. 基于歐拉平臺誤差角的 SINS 非線性誤差模型研究[J].西北工業大學學報, 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；網絡優先出版日期：2016-02-18。

軍隊計劃項目資助課題

U666.1

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

黃湘遠(1988-),男,博士研究生,主要研究方向為非線性濾波、慣性導航。

E-mail:huangxiangyuan.623@163.com

湯霞清(1965-),男,教授,博士,主要研究方向為慣性導航、戰車火控。

E-mail:tangxiaqing_001@163.com

武萌(1981-),女,講師,博士研究生,主要研究方向為導航、制導與控制。

E-mail:wumeng0326@126.com

網絡優先出版地址：http://www.cnki.net/kcms/detail/11.2422.TN.20160218.1211.008.html