王 解，郭曉松

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

基于簡化模型與自適應濾波的車載SINS靜基座快速對準

王 解1，郭曉松2

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

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

捷聯(lián)慣導；快速對準；簡化模型；自適應濾波

0 引言

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

1 基于觀測量擴展的捷聯(lián)慣導靜基座對準模型

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

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

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

Z=HX+V

2 基于最佳可觀測子空間的模型簡化

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

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

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

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

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

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

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

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

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

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

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

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

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

3 基于強跟蹤Kalman濾波的初始對準仿真與實驗

3.1 自適應濾波在對準中的應用

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

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

(1)

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

(2)

保持量測預測殘差序列相互正交的充分條件為

(3)

又因為濾波增益矩陣計算為：

將上式代入式(3)可知

令：

(4)

則式(1)成立。

記：

其中β≥1為一個選定的弱化因子，目的是使狀態(tài)量估計值更加平滑。

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

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

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

3.2 仿真與實驗

首先通過仿真驗證自適應濾波在初始對準中的效果，再通過實驗檢驗簡化模型和自適應濾波的實際效果。仿真條件如下：

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

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

噪聲方差陣：

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

觀測噪聲方差陣：

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

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

圖1 噪聲方差為Qk時的估計誤差曲線

圖2 噪聲方差為10 Qk時的估計誤差曲線

圖3 噪聲方差為100 Qk時的估計誤差曲線

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

表2 不同起始時刻和失準角兩種濾波方法的誤差對比

圖4 實驗中兩種方法的估計誤差曲線

實驗結(jié)果表明，在發(fā)動機怠速啟動后，自適應Kalman濾波的收斂速度比常規(guī)Kalman濾波的收斂速度要快，若以靜止狀態(tài)下對準結(jié)果為基準，自適應Kalman濾波的方位失準角對準精度明顯高于常規(guī)Kalman濾波的對準精度，而水平對準的精度相當。

4 結(jié)論

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

[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)慣導系統(tǒng)靜止條件下的初始對準方法研究[J]. 中國慣性技術(shù)學報，2004，12(2)：1-5.

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

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

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

[6] 程向紅，王 宇，楊文博.SINS初始對準中的魯棒超球體無跡卡爾曼濾波方法[J].中國慣性技術(shù)學報，2012，20(5)：540-543.

[7] 蘇宛新.自適應UKF濾波在SINS初始對準中的應用[J].中國慣性技術(shù)學報，2011，19(5)：533-536.

[8] 郝燕玲，牟宏偉.自適應平方根中心差分卡爾曼濾波算法在捷聯(lián)慣性導航系統(tǒng)大方位失準角初始對準中的應用[J].吉林大學學報(工學版)，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] 王新龍.慣導系統(tǒng)可觀測性及最佳可觀測子空間的定量研究[J].宇航學報，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-)，男，助教，主要從事組合導航與導航對抗方向的研究。

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

1671-4598(2017)07-0190-04

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

V249.32

A