基于簡(jiǎn)化模型與自適應(yīng)濾波的車載SINS靜基座快速對(duì)準(zhǔn)

王 解，郭曉松

(1.解放軍電子工程學(xué)院608室，合肥 230011；2.火箭軍工程大學(xué)，兵器發(fā)射理論與技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，西安 710025)

基于簡(jiǎn)化模型與自適應(yīng)濾波的車載SINS靜基座快速對(duì)準(zhǔn)

王 解1，郭曉松2

(1.解放軍電子工程學(xué)院608室，合肥 230011；2.火箭軍工程大學(xué)，兵器發(fā)射理論與技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，西安 710025)

為了實(shí)現(xiàn)捷聯(lián)慣性導(dǎo)航系統(tǒng)(strap-down inertial navigation system，SINS)快速初始對(duì)準(zhǔn)，根據(jù)已有可觀測(cè)性分析結(jié)果，通過(guò)理論分析和計(jì)算得到了擴(kuò)展觀測(cè)量時(shí)初始對(duì)準(zhǔn)系統(tǒng)最優(yōu)可觀測(cè)狀態(tài)量組合，在此基礎(chǔ)上簡(jiǎn)化了對(duì)準(zhǔn)模型，建立了新的系統(tǒng)方程；針對(duì)載車發(fā)動(dòng)機(jī)啟動(dòng)或其他情況導(dǎo)致系統(tǒng)噪聲無(wú)法精確統(tǒng)計(jì)，提出了運(yùn)用基于強(qiáng)跟蹤濾波原理的自適應(yīng)卡爾曼濾波算法抑制濾波發(fā)散，加快收斂速度；仿真結(jié)果表明運(yùn)用簡(jiǎn)化模型和自適應(yīng)濾波在系統(tǒng)噪聲不匹配時(shí)具有更快的收斂速度和更高的對(duì)準(zhǔn)精度，車載實(shí)驗(yàn)結(jié)果也表明運(yùn)用簡(jiǎn)化模型和自適應(yīng)濾波可以實(shí)現(xiàn)快速對(duì)準(zhǔn)。

捷聯(lián)慣導(dǎo)；快速對(duì)準(zhǔn)；簡(jiǎn)化模型；自適應(yīng)濾波

0 引言

在不同的應(yīng)用場(chǎng)合中，對(duì)捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)速度要求越來(lái)越高。文獻(xiàn)[1-5]分別提出了不同方法來(lái)提高捷聯(lián)慣導(dǎo)的初始對(duì)準(zhǔn)速度，其中文獻(xiàn)[1]首先利用Kalman濾波估計(jì)出水平失準(zhǔn)角，然后通過(guò)一階濾波器估計(jì)方位失準(zhǔn)角；文獻(xiàn)[2-3]均通過(guò)擴(kuò)展觀測(cè)量，改善初始對(duì)準(zhǔn)系統(tǒng)可觀測(cè)性來(lái)提高初始對(duì)準(zhǔn)的速度；文獻(xiàn)[4-5]討論了增加陀螺輸出信息對(duì)系統(tǒng)可觀測(cè)性的影響，得出了相關(guān)結(jié)論。另一方面，針對(duì)Kalman濾波對(duì)噪聲統(tǒng)計(jì)特性過(guò)度依賴，對(duì)動(dòng)態(tài)模型敏感等問(wèn)題，各類自適應(yīng)濾波和魯棒濾波技術(shù)也被應(yīng)用于捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)中[6-8]，這些算法的應(yīng)用提高了濾波器的穩(wěn)定性，為在復(fù)雜環(huán)境中實(shí)現(xiàn)初始對(duì)準(zhǔn)提供了有效的估計(jì)方法。對(duì)于SINS靜基座初始對(duì)準(zhǔn)系統(tǒng)，系統(tǒng)噪聲可以根據(jù)慣性器件輸出的噪聲統(tǒng)計(jì)確定，但是隨著載車環(huán)境的變化，噪聲的統(tǒng)計(jì)特性也就會(huì)發(fā)生變化。對(duì)于車載系統(tǒng)來(lái)說(shuō)，對(duì)準(zhǔn)時(shí)載車發(fā)動(dòng)機(jī)是否工作對(duì)慣性器件的輸出噪聲就有很大的影響，而對(duì)準(zhǔn)時(shí)其他系統(tǒng)的工作也會(huì)產(chǎn)生不同程度的影響，這就導(dǎo)致系統(tǒng)噪聲難以通過(guò)統(tǒng)計(jì)數(shù)據(jù)進(jìn)行確定，因此，有必要通過(guò)自適應(yīng)濾波避免對(duì)準(zhǔn)發(fā)散，加快對(duì)準(zhǔn)的速度。本文在文獻(xiàn)[5]研究的基礎(chǔ)上，將陀螺信息擴(kuò)展為觀測(cè)量的同時(shí)，通過(guò)簡(jiǎn)化系統(tǒng)模型，使系統(tǒng)可觀測(cè)性最優(yōu)；針對(duì)車載環(huán)境的影響，利用低通濾波器加快對(duì)準(zhǔn)速度。

1 基于觀測(cè)量擴(kuò)展的捷聯(lián)慣導(dǎo)靜基座對(duì)準(zhǔn)模型

取東、北、天地理坐標(biāo)系為系統(tǒng)導(dǎo)航坐標(biāo)系，根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)在靜基座小失準(zhǔn)角下的姿態(tài)誤差方程和速度誤差方程建立初始對(duì)準(zhǔn)系統(tǒng)的狀態(tài)方程為：

式中，A為狀態(tài)矩陣；X為狀態(tài)量，包括初始失準(zhǔn)角、速度誤差、陀螺常值漂移和加速度計(jì)零偏；G為噪聲驅(qū)動(dòng)矩陣；W為慣性器件噪聲包括陀螺噪聲和加速度計(jì)噪聲，具體表達(dá)為：

根據(jù)文獻(xiàn)[5]的研究將陀螺信息擴(kuò)展為觀測(cè)量，建立觀測(cè)方程：

Z=HX+V

2 基于最佳可觀測(cè)子空間的模型簡(jiǎn)化

s1=δVE,s2=δVN,s3=δVU,s4=-gφN+▽E,

s5=gφE+▽N,s6=▽U,s7=wiesinLφE+εN,

s8=wiesinLφN-wiecosLφU-εE,

s9=wiecosLφE-εU

僅以速度誤差為觀測(cè)量的情況下，文獻(xiàn)[9]從提高姿態(tài)角誤差可觀測(cè)性的角度出發(fā)將▽E，▽N，εE3個(gè)狀態(tài)量確定為不可觀測(cè)量，并推導(dǎo)了由此產(chǎn)生的估計(jì)誤差。但是觀測(cè)量擴(kuò)展系統(tǒng)發(fā)生變化，盡管可觀測(cè)組合的個(gè)數(shù)不變，但是其最優(yōu)可觀測(cè)子空間仍需要通過(guò)相關(guān)分析確定。由于實(shí)際系統(tǒng)中高度通道有很強(qiáng)的不穩(wěn)定性，需要借助其他傳感器(如高度計(jì))獲取高度信息，天向速度誤差作為觀測(cè)量時(shí)，僅影響▽U的可觀測(cè)性，對(duì)其他狀態(tài)量的可觀測(cè)性基本沒有影響，所以首先將δVU，▽z從狀態(tài)空間中移除，這樣更符合實(shí)際情況。

首先定性分析將εx，εy，εz，▽x，▽y中哪3個(gè)狀態(tài)量作為不可觀測(cè)量時(shí)系統(tǒng)剩余狀態(tài)量可觀測(cè)性最優(yōu)且對(duì)準(zhǔn)精度最高。初步分析考慮以下兩個(gè)因素，并獲得分析結(jié)果：

1)要確保在去掉3個(gè)狀態(tài)量后可觀測(cè)矩陣的秩是7，即去掉3個(gè)不可觀測(cè)狀態(tài)量后系統(tǒng)的可觀測(cè)矩陣應(yīng)為滿秩，系統(tǒng)為完全可觀測(cè)。觀察式中的可觀測(cè)狀態(tài)組合中的s5、s7和s9，要使這三組向量保持無(wú)關(guān)，則▽N，εN和εU中至多可以去掉一個(gè)且必然要去掉一個(gè)。去掉εE和▽E可觀測(cè)量組合仍然是無(wú)關(guān)，所以可以考慮的組合有：εE，▽E和▽N、εE，▽E和

εN、εE，▽E和εU。

2)去掉狀態(tài)量則意味帶來(lái)誤差，需要考慮降低對(duì)準(zhǔn)誤差。因此明確式中狀態(tài)量的數(shù)量級(jí)：小失準(zhǔn)角中φ以分為單位，數(shù)量級(jí)為10-4；ε以度每小時(shí)為單位，數(shù)量級(jí)為10-6；▽以u(píng)g為單位，數(shù)量級(jí)是10-5。假設(shè)觀測(cè)量的各階導(dǎo)數(shù)均可精確獲得，根據(jù)各狀態(tài)的系數(shù)和數(shù)量級(jí)就能定性分析不同狀態(tài)量的可觀測(cè)性，也可以分析出大致估計(jì)精度。根據(jù)s4，s5對(duì)應(yīng)等式右邊項(xiàng)可知，φE，φN前有系數(shù)g，由于φ的數(shù)量級(jí)和▽相當(dāng)，所以可知φE，φN能更精確的被估計(jì)出來(lái)。而對(duì)s7，s9對(duì)應(yīng)的項(xiàng)分析可知φE的系數(shù)很小，若將εN或εU確定為不可觀測(cè)量，則使φE得不到很好的估計(jì)(估計(jì)誤差為εN/wiesinL或εU/wiecosL，若εN=0.01°/h，φE則的誤差將達(dá)到幾分)。

綜合以上兩點(diǎn)，可以確定以εE,▽E和▽N為不可觀測(cè)量既可以使其余狀態(tài)量完全可觀測(cè)，又可以得到最高精度的估計(jì)值，3個(gè)方向的失準(zhǔn)角估計(jì)誤差分別是：

基于奇異值分解的可觀測(cè)性分析可以定量分析狀態(tài)量的可觀測(cè)性[10]，下面分別計(jì)算將εE,▽E和▽N、εE,▽E和εN、εE,▽E和εU作為不可觀測(cè)狀態(tài)量時(shí)可觀測(cè)矩陣的奇異值大小。表1給出了各子空間的可觀測(cè)矩陣奇異值。

根據(jù)表中的數(shù)據(jù)分析可知，εE,▽E,▽N作為不可觀測(cè)狀態(tài)量時(shí)可觀測(cè)矩陣的奇異值要大于其他兩種情況，結(jié)合根據(jù)可觀測(cè)組合的分析，可以確定選擇εE,▽E,▽N為不可觀測(cè)狀態(tài)量時(shí)，系統(tǒng)可觀測(cè)性最好，對(duì)準(zhǔn)精度最優(yōu)。

表1 除去不同不可觀測(cè)狀態(tài)量時(shí)系統(tǒng)可觀測(cè)矩陣奇異值

根據(jù)上述的結(jié)論建立新的最優(yōu)可觀測(cè)狀態(tài)子空間和初始對(duì)準(zhǔn)濾波模型。

此時(shí)分析的系統(tǒng)狀態(tài)空間表達(dá)為：

其中：X=[φEφNφUδVEδVNεyεz]T；W=[wgxwgywgzwaxwaywaz]T；觀測(cè)量Z和觀測(cè)噪聲與簡(jiǎn)化前一致；G，H由系統(tǒng)誤差方程和觀測(cè)方程可得。

3 基于強(qiáng)跟蹤Kalman濾波的初始對(duì)準(zhǔn)仿真與實(shí)驗(yàn)

3.1 自適應(yīng)濾波在對(duì)準(zhǔn)中的應(yīng)用

對(duì)于SINS靜基座初始對(duì)準(zhǔn)系統(tǒng)，系統(tǒng)噪聲可以根據(jù)慣性器件輸出的噪聲統(tǒng)計(jì)確定，但是隨著載車環(huán)境的變化，噪聲的統(tǒng)計(jì)特性也就會(huì)發(fā)生變化。對(duì)于車載系統(tǒng)來(lái)說(shuō)，對(duì)準(zhǔn)時(shí)載車發(fā)動(dòng)機(jī)是否工作對(duì)慣性器件的輸出噪聲就有很大的影響，而對(duì)準(zhǔn)時(shí)其他系統(tǒng)的工作也會(huì)產(chǎn)生不同程度的影響，這就導(dǎo)致系統(tǒng)噪聲難以通過(guò)統(tǒng)計(jì)數(shù)據(jù)進(jìn)行確定，因此，有必要通過(guò)自適應(yīng)濾波避免對(duì)準(zhǔn)發(fā)散，加快對(duì)準(zhǔn)的速度。

強(qiáng)跟蹤自適應(yīng)濾波原理在于在線調(diào)整增益矩陣Kk+1，強(qiáng)迫量測(cè)預(yù)測(cè)殘差序列保持相互正交，使濾波器保持對(duì)實(shí)際系統(tǒng)狀態(tài)的跟蹤，即要求濾波器滿足以下兩個(gè)條件：

(1)

E(γ(k+1+j)γT(k+1))=0,k=0,1,2...;j=1,2,...

(2)

保持量測(cè)預(yù)測(cè)殘差序列相互正交的充分條件為

(3)

又因?yàn)闉V波增益矩陣計(jì)算為：

將上式代入式(3)可知

令：

(4)

則式(1)成立。

記：

其中β≥1為一個(gè)選定的弱化因子，目的是使?fàn)顟B(tài)量估計(jì)值更加平滑。

式(4)可以表達(dá)為λkMk=Nk，則次優(yōu)漸消因子λk近似求解為：

式中：0<ρ≤1為遺忘因子。

用式(2)代替求解Kalman狀態(tài)預(yù)測(cè)協(xié)方差陣即可得到基于強(qiáng)跟蹤濾波原理的自適應(yīng)Kalman濾波流程。

3.2 仿真與實(shí)驗(yàn)

首先通過(guò)仿真驗(yàn)證自適應(yīng)濾波在初始對(duì)準(zhǔn)中的效果，再通過(guò)實(shí)驗(yàn)檢驗(yàn)簡(jiǎn)化模型和自適應(yīng)濾波的實(shí)際效果。仿真條件如下：

Kalman濾波器的初始條件如下：

初始狀態(tài)估計(jì)值：X0=zeros[7,1]；初始方差陣：

噪聲方差陣：

Q=diag(0.1°/h,0.1°/h,0.1°/h,100ug,100ug,0,0)2；

觀測(cè)噪聲方差陣：

三次仿真數(shù)據(jù)中分別設(shè)置慣性器件的輸出噪聲方差為Qk，10Qk，100Qk，對(duì)比在噪聲不匹配時(shí)，自適應(yīng)Kalman濾波和常規(guī)Kalman濾波的對(duì)準(zhǔn)效果。

圖1、圖2和圖3分別對(duì)應(yīng)三次仿真的結(jié)果。根據(jù)圖示結(jié)果可知，在系統(tǒng)噪聲方差和濾波器設(shè)置的噪聲方差匹配時(shí)，自適應(yīng)濾波和常規(guī)Kalman濾波的效果基本一致，而在噪聲不匹配的情況下，自適應(yīng)Kalman濾波提高了方位失準(zhǔn)角的對(duì)準(zhǔn)精度，在慣性器件噪聲方差為10Rk和100Rk時(shí)，方位失準(zhǔn)角誤差分別由7.7′和6.5′減小到5.6′和3.5′，估計(jì)收斂的速度也略快于常規(guī)Kalman濾波，表明自適應(yīng)濾波的對(duì)準(zhǔn)效果要優(yōu)于常規(guī)Kalman濾波。

圖1 噪聲方差為Qk時(shí)的估計(jì)誤差曲線

圖2 噪聲方差為10 Qk時(shí)的估計(jì)誤差曲線

圖3 噪聲方差為100 Qk時(shí)的估計(jì)誤差曲線

實(shí)驗(yàn)：為了進(jìn)一步驗(yàn)證算法的實(shí)用性，運(yùn)用本文算法進(jìn)行了車載捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)實(shí)驗(yàn)，首先在前10分鐘載車保持靜止的情況下進(jìn)行高精度對(duì)準(zhǔn)，以此結(jié)果為基準(zhǔn)再設(shè)置5組不同的東、北、天失準(zhǔn)角，并在之后的不同時(shí)刻開始進(jìn)行再次對(duì)準(zhǔn)。10分鐘后啟動(dòng)發(fā)動(dòng)機(jī)保持怠速狀態(tài)，運(yùn)用自適應(yīng)Kalman進(jìn)行初始對(duì)準(zhǔn)，對(duì)準(zhǔn)是利用經(jīng)過(guò)低通濾波的實(shí)驗(yàn)數(shù)據(jù)在線下完成，本次實(shí)驗(yàn)采樣頻率為200 Hz，數(shù)據(jù)處理時(shí)首先經(jīng)過(guò)均值濾波輸出20 Hz數(shù)據(jù)，再使用截止頻率為0.1 Hz的IIR低通濾波器去噪。在發(fā)動(dòng)機(jī)轉(zhuǎn)動(dòng)之后陀螺噪聲及加速度計(jì)噪聲未知的情況下，設(shè)置濾波器參數(shù)中陀螺漂移噪聲和加速度計(jì)的隨機(jī)噪聲為0.1°/h和100 ug。表2為5組對(duì)準(zhǔn)結(jié)果，以載車完全靜止且無(wú)外界噪聲干擾時(shí)的對(duì)準(zhǔn)結(jié)果為基準(zhǔn)，其中東、北、天失準(zhǔn)角分別設(shè)置為0.1°、0.1°、0.5°，起始時(shí)刻為第800 s的對(duì)準(zhǔn)結(jié)果如圖4所示。

表2 不同起始時(shí)刻和失準(zhǔn)角兩種濾波方法的誤差對(duì)比

圖4 實(shí)驗(yàn)中兩種方法的估計(jì)誤差曲線

實(shí)驗(yàn)結(jié)果表明，在發(fā)動(dòng)機(jī)怠速啟動(dòng)后，自適應(yīng)Kalman濾波的收斂速度比常規(guī)Kalman濾波的收斂速度要快，若以靜止?fàn)顟B(tài)下對(duì)準(zhǔn)結(jié)果為基準(zhǔn)，自適應(yīng)Kalman濾波的方位失準(zhǔn)角對(duì)準(zhǔn)精度明顯高于常規(guī)Kalman濾波的對(duì)準(zhǔn)精度，而水平對(duì)準(zhǔn)的精度相當(dāng)。

4 結(jié)論

在擴(kuò)展觀測(cè)量的基礎(chǔ)上，本文根據(jù)可觀測(cè)性分析的結(jié)果，確定了靜基座初始對(duì)準(zhǔn)系統(tǒng)最優(yōu)可觀測(cè)狀態(tài)量子空間，并建立了簡(jiǎn)化后的模型，由于簡(jiǎn)化后的系統(tǒng)為完全可觀測(cè)，加快了狀態(tài)量的估計(jì)速度。在此基礎(chǔ)上研究了應(yīng)用強(qiáng)跟蹤自適應(yīng)濾波進(jìn)一步提高初始對(duì)準(zhǔn)速度，特別是在車載環(huán)境下效果更加明顯。研究表明，將陀螺信息作為觀測(cè)量時(shí)，陀螺噪聲太大將導(dǎo)致對(duì)準(zhǔn)失效，而本文實(shí)驗(yàn)表明當(dāng)載車發(fā)動(dòng)機(jī)啟動(dòng)而無(wú)其他干擾時(shí)，經(jīng)過(guò)數(shù)字濾波再運(yùn)用自適應(yīng)濾波方法，陀螺信息的引入仍然可以加速對(duì)準(zhǔn)。

[1] Fang J C，Wang D J.A fast initial alignment method for strapdown inertial navigation system stationary base[J].IEEE Transactions on Aerospace and Electronic Systems，1996，32(4)：1501-1505.

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

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

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

[5] 郭曉松，王 解，周召發(fā)，等.基于觀測(cè)量擴(kuò)展的捷聯(lián)慣導(dǎo)快速初始對(duì)準(zhǔn)方法[J].系統(tǒng)工程與電子技術(shù)，2014，36(7)：1392-1396.

[6] 程向紅，王 宇，楊文博.SINS初始對(duì)準(zhǔn)中的魯棒超球體無(wú)跡卡爾曼濾波方法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2012，20(5)：540-543.

[7] 蘇宛新.自適應(yīng)UKF濾波在SINS初始對(duì)準(zhǔn)中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2011，19(5)：533-536.

[8] 郝燕玲，牟宏偉.自適應(yīng)平方根中心差分卡爾曼濾波算法在捷聯(lián)慣性導(dǎo)航系統(tǒng)大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用[J].吉林大學(xué)學(xué)報(bào)(工學(xué)版)，2013，43(1)：261-266.

[9] Jiang Y F，Lin Y P.Error Estimation of INS Ground Alignment Through Observability Analysis[J].IEEE Trans.on Aerospace and Electronic System，1992，28(1)：92-96.

[10] 王新龍.慣導(dǎo)系統(tǒng)可觀測(cè)性及最佳可觀測(cè)子空間的定量研究[J].宇航學(xué)報(bào)，2006，27(3)：345-348.

Fast Alignment of Vehicle-based SINS Based on Simplified Model and Adaptive Filtering

Wang Jie1, Guo Xiaosong2

(1.No 608 Staff Room, Electronic Engineering Institute，Hefei 230011，China; 2.State Key Laboratory of Armament Launch Theory and Technology, Rocket Force University of Engineering, Xi’an 710025，China)

To realize fast initial alignment of SINS, according to the results of the observability analysis, state combinations with best observability are found by theoretical analysis and calculation, then the alignment model is simplified and new system function is proposed. As for system noise is unknown when engine is starting, an adaptive Kalman filtering (KF)algorithm based on strong tracking filter theory is proposed, which could restrain filtering divergence and speed up the convergence. The simulation results show that the adaptive algorithm has faster convergence speed and higher precision when the system noises mismatches. The vehicle-based experiment result also shows that fast alignment can be realized with the application of simplified model and adaptive Kalman filter.

SINS; fast initial alignment; simplified model; adaptive filtering

2016-11-19；

2017-02-27。

王 解(1989-)，男，助教，主要從事組合導(dǎo)航與導(dǎo)航對(duì)抗方向的研究。

郭曉松(1957-),男，教授，博士生導(dǎo)師，主要從事定位定向與基準(zhǔn)傳遞方向的研究。

1671-4598(2017)07-0190-04

10.16526/j.cnki.11-4762/tp.2017.07.047

V249.32

A