靳 攀, 林 娜

(1.鄭州航空工業(yè)管理學(xué)院信息管理中心，鄭州 450046； 2. 河南工業(yè)貿(mào)易職業(yè)學(xué)院信息工程系，鄭州 450000)

基于陀螺信息的SINS快速抗干擾初始對(duì)準(zhǔn)

靳 攀1, 林 娜2

(1.鄭州航空工業(yè)管理學(xué)院信息管理中心，鄭州 450046； 2. 河南工業(yè)貿(mào)易職業(yè)學(xué)院信息工程系，鄭州 450000)

為了解決存在干擾或較大噪聲的情況下,利用陀螺信息實(shí)現(xiàn)捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)快速初始對(duì)準(zhǔn)，首先引入了帶高斯噪聲等式約束的卡爾曼濾波算法(QECKF)，建立了基于QECKF算法的SINS初始對(duì)準(zhǔn)模型;然后，從車載捷聯(lián)慣導(dǎo)系統(tǒng)的實(shí)際特征出發(fā)，提出首先使用IIR低通濾波器降噪，再通過模糊判據(jù)結(jié)合QECKF算法和卡爾曼濾波(KF)兩種濾波方法，該方案實(shí)現(xiàn)了陀螺信息的最優(yōu)化應(yīng)用以達(dá)到快速對(duì)準(zhǔn)的目的;最后，試驗(yàn)表明在系統(tǒng)受發(fā)動(dòng)機(jī)和人員操作的擾動(dòng)下，該算法相比于常規(guī)KF估計(jì)將速度提高了近25%，解決了在噪聲較大時(shí)基于擴(kuò)展觀測(cè)量的方法估計(jì)發(fā)散問題。

捷聯(lián)慣導(dǎo)系統(tǒng)； 初始對(duì)準(zhǔn)； 抗干擾； 陀螺信息； 等式約束

0 引言

如何實(shí)現(xiàn)捷聯(lián)慣性導(dǎo)航系統(tǒng)快速抗干擾，一直是慣導(dǎo)領(lǐng)域研究的難點(diǎn)。文獻(xiàn)[1-3]分別提出了不同的抗干擾粗對(duì)準(zhǔn)算法，有必要在此基礎(chǔ)上實(shí)現(xiàn)快速抗干擾精對(duì)準(zhǔn)以完成干擾狀態(tài)下的初始對(duì)準(zhǔn)。為了實(shí)現(xiàn)捷聯(lián)慣導(dǎo)的快速初始對(duì)準(zhǔn)，近年來，文獻(xiàn)[4-7]提出了利用陀螺信息以擴(kuò)展觀測(cè)量的濾波方法，使濾波估計(jì)收斂速度加快，該方法很大程度上縮短了捷聯(lián)慣導(dǎo)靜基座初始對(duì)準(zhǔn)的時(shí)間。根據(jù)文獻(xiàn)[8]的研究，在參數(shù)估計(jì)中利用約束信息的方法還可以有兩種：一是通過約束降低狀態(tài)方程的維數(shù)；二是將估計(jì)結(jié)果投影至約束空間或面內(nèi)。根據(jù)捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)模型可知，由陀螺輸出信息建立的約束是變化的，所以并不能由此建立狀態(tài)方程，而且根據(jù)可觀測(cè)分析可知，引入陀螺信息后也不能降低狀態(tài)方程的維數(shù)。而利用陀螺信息建立約束使估計(jì)結(jié)果符合約束是可行的，所以本文以此為切入點(diǎn)，利用已知條件判斷陀螺信息的可利用性，再根據(jù)判斷結(jié)果確定是否將結(jié)果投影至約束空間，這充分利用了陀螺信息加快初始對(duì)準(zhǔn)的速度。

在介紹帶高斯噪聲等式約束的卡爾曼濾波(Quasi-Equality Constrained Kalman Filter,QECKF)算法并建立基于QECKF算法的捷聯(lián)慣導(dǎo)系統(tǒng)(Strap-down Initial Navigation System, SINS)對(duì)準(zhǔn)模型后，通過分析在載車發(fā)動(dòng)機(jī)怠速轉(zhuǎn)動(dòng)且人員正常操作情況下陀螺輸出信號(hào)的特征，得出了車載情況下可以利用陀螺信息的結(jié)論；又根據(jù)陀螺輸出在地理坐標(biāo)系投影為常值，利用數(shù)字低通濾波器進(jìn)行濾波的無延遲問題，對(duì)計(jì)算地理坐標(biāo)系下的陀螺輸出濾波，確定以此為帶等式約束Kalman濾波器的約束量；再根據(jù)系統(tǒng)原理，提出了避免估計(jì)發(fā)散而設(shè)計(jì)的自適應(yīng)判據(jù)，使得在陀螺輸出經(jīng)濾波仍含有低頻干擾的情況下通過Kalman濾波估計(jì)失準(zhǔn)角；最后通過試驗(yàn)驗(yàn)證本文對(duì)準(zhǔn)方法的有效性。

1 QECKF及系統(tǒng)建模

1.1 QECKF

TEIXEIRA等學(xué)者[9-10]首先推導(dǎo)了基于Kalman濾波的帶等式約束的線性系統(tǒng)狀態(tài)最優(yōu)估計(jì)算法(Equality Constrainted KF,ECKF)，并給出了估計(jì)步驟，在此基礎(chǔ)上，文獻(xiàn)[11-12]對(duì)此算法進(jìn)行了發(fā)展和完善。由于利用了物理模型中的等式約束，理論上可以增強(qiáng)系統(tǒng)的可觀測(cè)性，提高估計(jì)的精度和速度。

離散線性系統(tǒng)可用狀態(tài)方程和量測(cè)方程表示為

(1)

式中：xk，zk分別為狀態(tài)向量和觀測(cè)向量；φk,k-1，Γk,k-1，Hk分別為狀態(tài)轉(zhuǎn)移矩陣、噪聲輸入矩陣和量測(cè)矩陣；wk，vk分別為系統(tǒng)噪聲和量測(cè)噪聲，均為零均值白噪聲。利用觀測(cè)向量修正一步預(yù)測(cè)值，進(jìn)而獲得狀態(tài)最優(yōu)估計(jì)值的標(biāo)準(zhǔn)Kalman濾波遞推過程由預(yù)測(cè)與校正構(gòu)成。其滿足目標(biāo)函數(shù)J(xk)最大[9]，即

J(xk)=ρ(xk|z1,…,zk)

(2)

也就是后驗(yàn)概率密度最大。

預(yù)測(cè)部分為

(3)

(4)

(5)

(6)

(7)

利用量測(cè)信息校正部分為

(8)

(9)

(10)

現(xiàn)假設(shè)系統(tǒng)狀態(tài)量滿足約束

Dk-1xk=dk-1

(11)

式中，Dk-1，dk-1分別為已知的s×n維矩陣和s維向量，rank(Dk-1)=s

(12)

(13)

同樣地，令

(14)

為了使目標(biāo)函數(shù)J(xk)最大，則投影修正為

(15)

(16)

以上為ECKF的投影修正部分，文獻(xiàn)[9]給出了其成立的具體證明。

。

(17)

(18)

對(duì)比于擴(kuò)展觀測(cè)量KF濾波方法，通過獨(dú)立的投影修正使得估計(jì)值滿足系統(tǒng)的物理原理具有明顯的優(yōu)越性。在約束等式不再成立時(shí)，轉(zhuǎn)而使用常規(guī)KF濾波后估計(jì)仍然是最優(yōu)的，并且是連續(xù)的，而且通過根據(jù)系統(tǒng)實(shí)際選取最優(yōu)的噪聲方差，使得等式約束對(duì)估計(jì)的影響可調(diào)。因此考慮將QECKF應(yīng)用于基于陀螺信息的SINS初始對(duì)準(zhǔn)。

1.2 基于QECFK的SINS初始對(duì)準(zhǔn)模型

假設(shè)經(jīng)過粗對(duì)準(zhǔn)之后，姿態(tài)失準(zhǔn)角為小角度，取東、北、天地理坐標(biāo)系為系統(tǒng)導(dǎo)航坐標(biāo)系，捷聯(lián)慣導(dǎo)靜基座的姿態(tài)誤差方程和速度誤差方程分別為

(19)

(20)

式中：φ為失準(zhǔn)角；ε為陀螺常值漂移誤差；δv為速度誤差；f為比力；▽為加速度計(jì)常值漂移誤差；wie為地球自轉(zhuǎn)角速度。根據(jù)姿態(tài)和速度誤差方程建立系統(tǒng)狀態(tài)方程為

(21)

以速度作為系統(tǒng)觀測(cè)量時(shí)，由于系統(tǒng)處于靜基座上，故導(dǎo)航解算所得的速度即為速度誤差，對(duì)應(yīng)的觀測(cè)方程為

z=Hx+v

(22)

=

(23)

(24)

由式(21)、式(22)、式(24)構(gòu)成的系統(tǒng)可以應(yīng)用QECKF算法進(jìn)行激光陀螺捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)。

2 QECKF/KF初始對(duì)準(zhǔn)研究

2.1 信號(hào)特征分析

在載車發(fā)動(dòng)機(jī)怠速轉(zhuǎn)動(dòng)，人員正常車上操作狀態(tài)下，采集車載陀螺輸出信號(hào)后進(jìn)行頻譜分析，其中，采樣頻率為200 Hz，得到x，y，z三軸陀螺原始信號(hào)頻譜如圖1所示，其中，w(f)為經(jīng)過濾波之后的輸出。頻譜分析結(jié)果顯示,陀螺信號(hào)中含有較多的高頻噪聲，1 Hz以下的噪聲很小。

圖1 陀螺輸出信號(hào)的頻譜Fig.1 Frequency spectrum of gyro output signal

圖2 濾波后的陀螺投影信號(hào)Fig.2 The projected gyro signal after filtering

2.2 對(duì)準(zhǔn)方案設(shè)計(jì)

根據(jù)初始對(duì)準(zhǔn)的原理和以上對(duì)車載慣組輸出信號(hào)的特征分析后，確定如圖3的對(duì)準(zhǔn)方案，方案中，根據(jù)能否從陀螺輸出值中獲得地球自轉(zhuǎn)信息，結(jié)合使用QECKF和KF兩種不同的算法，加快初始對(duì)準(zhǔn)的速度。

圖3 基于QECKF/KF算法的抗干擾初始對(duì)準(zhǔn)方案Fig.3 Scheme of anti-disturbance initial alignment based on QECKF/KF

wf

(25)

wm由σg，φM，ε確定，實(shí)際應(yīng)用中可根據(jù)經(jīng)驗(yàn)設(shè)置。僅當(dāng)式(25)成立時(shí)，可使用QECKF估計(jì)算法，否則使用KF算法。

3 試驗(yàn)結(jié)果

為了驗(yàn)證所提快速抗干擾方法的有效性，進(jìn)行了車載捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)試驗(yàn)。試驗(yàn)采取實(shí)際平臺(tái)采集數(shù)據(jù)再線下數(shù)據(jù)處理的方式，試驗(yàn)共進(jìn)行了20 min，前10 min載車處于靜止?fàn)顟B(tài)，10 min后啟動(dòng)發(fā)動(dòng)機(jī)，人員正常上下車操作，圖1和圖2分別為后10 min陀螺輸出頻譜圖和濾波后輸出值曲線，在250～300 s之間是人為施加的劇烈晃動(dòng)，以檢驗(yàn)算法效果。利用前10 min的數(shù)據(jù)進(jìn)行初始對(duì)準(zhǔn)的結(jié)果作為真實(shí)姿態(tài)參考，設(shè)置東、北、天失準(zhǔn)角分別為6′，6′，10′，分別利用常規(guī)KF濾波估計(jì)和QECKF/KF估計(jì)方案進(jìn)行對(duì)準(zhǔn)，以估計(jì)輸出減去設(shè)置的失準(zhǔn)角為誤差，得到失準(zhǔn)角誤差曲線如圖4和圖5所示。

圖4 試驗(yàn)失準(zhǔn)角估計(jì)誤差Fig.4 The error of misalignment angles’ estimation

圖5 方位失準(zhǔn)角估計(jì)誤差Fig.5 The error of azimuth misalignment angle’s estimation

由圖4、圖5可知，兩種方法精度相當(dāng)，而采用QECKF/KF算法時(shí)，方位失準(zhǔn)角估計(jì)收斂顯然要快，在150 s內(nèi)完成收斂，而常規(guī)KF算法在200 s之后才完成收斂。取150～200 s之間兩種方法方位估計(jì)值誤差計(jì)算平均值和方差，基于KF和基于QECKF/KF算法估計(jì)誤差平均值分別為-1.481 3′，-1.353 1′，方差分別為5.028 3″，11.408 7″，表明新方法可以加快對(duì)準(zhǔn)的速度，從此次試驗(yàn)來看，該方法將方位對(duì)準(zhǔn)的時(shí)間縮短了近1/4，而水平失準(zhǔn)角對(duì)準(zhǔn)的速度和精度是一致的，這符合可觀測(cè)性分析結(jié)論[5-6]。

4 總結(jié)

為了將基于陀螺信息的捷聯(lián)慣導(dǎo)快速初始對(duì)準(zhǔn)方法應(yīng)用于受干擾基座，引入了帶高斯噪聲等式約束的卡爾曼濾波算法，通過分析車載陀螺在干擾狀態(tài)下的輸出特性，研究設(shè)計(jì)了一種抗干擾快速初始對(duì)準(zhǔn)方案。方案通過IIR低通濾波獲取陀螺輸出中的地球自轉(zhuǎn)信息，再通過自適應(yīng)判據(jù)綜合KF和QECKF完成初始對(duì)準(zhǔn)，試驗(yàn)表明該方法的效果明顯，是將基于陀螺信息的快速初始對(duì)準(zhǔn)方法應(yīng)用于擾動(dòng)基座的有效途徑，具有重要的應(yīng)用和研究?jī)r(jià)值。將基于晃動(dòng)基座的粗對(duì)準(zhǔn)方法和本文所提的方法結(jié)合完成整個(gè)對(duì)準(zhǔn),將大大提高SINS抗干擾初始對(duì)準(zhǔn)的速度。

[1] 嚴(yán)恭敏,白亮,翁浚,等．基于頻域分離算子的SINS抗晃動(dòng)干擾初始對(duì)準(zhǔn)算法[J]．宇航學(xué)報(bào),2011,32(7)：1486-1490.

[2] 孫楓,夏健鐘,高偉．基于2D-HMM/KF的捷聯(lián)慣導(dǎo)系泊對(duì)準(zhǔn)方法[J]．華中科技大學(xué)學(xué)報(bào)：自然科學(xué)版,2014,42(1)：110-114.

[3] 黃鳳榮,高峰,付中澤,等．基于小波濾波的激光陀螺SINS晃動(dòng)基座初始對(duì)準(zhǔn)[J]．中國慣性技術(shù)學(xué)報(bào),2014,22(2)：157-160.

[4] 王立冬,蔡玲,魯軍.車載捷聯(lián)慣導(dǎo)系統(tǒng)靜止條件下的初始對(duì)準(zhǔn)方法研究[J].中國慣性技術(shù)學(xué)報(bào),2004,12(2)：1-5.

[5] 熊劍,劉建業(yè),賴際舟,等.一種陀螺量測(cè)信息輔助的快速初始對(duì)準(zhǔn)方法[J].宇航學(xué)報(bào),2009,30(4)：1454-1459.

[6] 高偉熙,繆玲娟,倪茂林.一種引入陀螺角速度信息的快速對(duì)準(zhǔn)方法[J].宇航學(xué)報(bào),2010,31(6)：1596-1601.

[7] ACHARYA A,SADHO S,GHOSHAL T K.Improved self-alignment scheme for SINS using augmented measurement[J].Aerospace Science and Technology,2011,15(2)：125-128.

[8] SHEN L F,LIN Y．Quasi-equality constrained risk-sensitive filtering for nonlinear discrete-time systems[J]．Control Theory,2012,10(2):229-235．

[9] TEIXEIRA B O S,CHANDRASEKAR J,TORRES L A B,et al.State estimation for equality-constrained linear systems[C]//Proceedings of the 46th IEEE Conference on Decision and Control,2007:6220-6225.

[10] SIMON D,CHIA T.Kalman filtering with state equality constraints[J].IEEE Transactions on Aerospace and Electronic Systems,2002,38(1):128-136.

[11] SIRCOULOMB V,HOBLOS G,CHAFOUK H,et al．State estimation under nonlinear state inequality constraints:a tracking application[C]//The 16th Mediterranean Conference on Control and Automation Congress Centre, 2008：1669-1674.

[12] TEIXEIRA B O S,CHANDRASEKAR J,TORRES L A B,et al.Unscented filtering for equality-constrained nonlinear systems[C]//American Control Conference, 2008：39-44.

Fast and Anti-Disturbance Initial Alignment of SINS Based on Gyros Information

JIN Pan1, LIN Na2

(1.Information Management Center,Zhengzhou University of Aeronautics,Zhengzhou 450046,China; 2.Department of Information Engineering,Henan Industry and Trade Vocational College,Zhengzhou 450000,China)

In order to achieve fast initial alignment of Strap-down Initial Navigation System (SINS) with gyros information under circumstance of disturbance and strong noise,Quasi-Equality Constrained Kalman Filter (QECKF) was introduced,based on which the model of SINS initial alignment was established.According to the characteristics of the SINS onboard land vehicles,it was proposed to use IIR low-pass filter at first for noise reduction,then the filtering method of QECKF together with KF.The scheme made full use of the gyro information,and could realize fast initial alignment.Result of the test under disturbance of engine and operators showed that the speed of the algorithm was increased by 25% compared with the normal KF.The algorithm solved the problem of estimation divergence of the normal method.

SINS; initial alignment; anti-disturbance; gyros information; equality-constrained

靳攀,林娜.基于陀螺信息的SINS快速抗干擾初始對(duì)準(zhǔn)[J].電光與控制，2017，24(9)：95-99.JIN P,LIN N.Fast and anti-disturbance initial alignment of SINS based on gyros information[J].Electronics Optics & Control,2017,24(9):95-99.

2016-10-08

2017-07-18

河南省教育廳重點(diǎn)項(xiàng)目(14A520073)

靳 攀(1980 —),男，河南鄭州人，碩士，講師，研究方向?yàn)橛?jì)算機(jī)控制及軟件設(shè)計(jì)。

V249.32

A

10.3969/j.issn.1671-637X.2017.09.020