基于降階卡爾曼濾波算法在組合導(dǎo)航上的應(yīng)用

胡 彬，楊新民，王勝紅

（1.南京理工大學(xué) 江蘇 南京 210094；2.淮海工業(yè)集團(tuán) 山西 長治 046000）

基于降階卡爾曼濾波算法在組合導(dǎo)航上的應(yīng)用

胡 彬1，楊新民1，王勝紅2

（1.南京理工大學(xué) 江蘇 南京 210094；2.淮海工業(yè)集團(tuán) 山西 長治 046000）

本文在簡單介紹了卡爾曼濾波基本原理基礎(chǔ)上，基于分段線性系統(tǒng)理論以及SOM方法對(duì)系統(tǒng)狀態(tài)可觀性和可觀度的分析，采用降階卡爾曼濾波算法，合理剔除不可觀或觀測(cè)度較低的狀態(tài)因子，對(duì)高階系統(tǒng)進(jìn)行降階設(shè)計(jì)。采用Visual Studio 2010語言開發(fā)環(huán)境，對(duì)常規(guī)卡爾曼濾波和降階卡爾曼濾波進(jìn)行了算法仿真，通過對(duì)前后誤差波形的分析比較，結(jié)果表明降階后的濾波系統(tǒng)繼承了傳統(tǒng)卡爾曼濾波高精度的優(yōu)點(diǎn)，同時(shí)，算法階數(shù)的降低，顯著減少了導(dǎo)航計(jì)算機(jī)的計(jì)算負(fù)擔(dān)，實(shí)時(shí)性得到了顯著增強(qiáng)，更易于數(shù)字化實(shí)現(xiàn)，具有重要實(shí)際意義。

慣性導(dǎo)航；組合導(dǎo)航；卡爾曼濾波；數(shù)字仿真

工程中實(shí)性是個(gè)必須考慮的問題，尤其是在組合導(dǎo)航系統(tǒng)中，要求系統(tǒng)具有高精度、高實(shí)時(shí)性的機(jī)動(dòng)能力。在導(dǎo)航計(jì)算中，應(yīng)用卡爾曼濾波算法對(duì)數(shù)據(jù)處理時(shí)，由于采用數(shù)學(xué)迭代計(jì)算，計(jì)算時(shí)間由狀態(tài)維數(shù)n決定，每一步迭代的計(jì)算量都與n3正比[1]。顯然21階誤差狀態(tài)給導(dǎo)航計(jì)算機(jī)帶來了很大的計(jì)算負(fù)擔(dān)，工程實(shí)現(xiàn)困難，難以滿足組合導(dǎo)航高實(shí)時(shí)性的要求，在設(shè)計(jì)組合導(dǎo)航計(jì)算機(jī)速度和復(fù)雜度方面都不允許。

由于SINS/GPS系統(tǒng)卡爾曼濾波的初始誤差協(xié)方差很容易滿足正定性條件，而且系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣又是滿秩的，因此系統(tǒng)完全可控，濾波的有效性就取決于系統(tǒng)的可觀測(cè)行性。關(guān)于降階濾波模型的研究，前人給出了很多研究方法，例如Model方法，Lyapunow函數(shù)方法，Perturbation方法等等[2]。文獻(xiàn)[2]中提出了一種時(shí)變動(dòng)態(tài)系統(tǒng)可觀測(cè)性矩陣的奇異值分解分析方法，文獻(xiàn)[3]又進(jìn)行了改進(jìn)，本文是應(yīng)用分段線性系統(tǒng)可觀性理論和SOM分析法[4]，對(duì)系統(tǒng)狀態(tài)可觀測(cè)性和可觀測(cè)度的分析，拋開不可觀測(cè)或觀測(cè)度較低的系統(tǒng)狀態(tài)因子，從而得到一個(gè)只有12階的狀態(tài)變量降階模型。采用Visual Studio 2010語言開發(fā)環(huán)境設(shè)計(jì)編寫了常規(guī)卡爾曼濾波和降階濾波的組合導(dǎo)航程序，模擬設(shè)計(jì)飛行軌跡，最后通過計(jì)算機(jī)仿真驗(yàn)證了降階后的模型仍能滿足組合導(dǎo)航高精度的要求，并且實(shí)時(shí)性得到了顯著的增強(qiáng)。

1 組合導(dǎo)航濾波結(jié)構(gòu)和濾波算法

1.1 常規(guī)卡爾曼濾波算法

卡爾曼濾波是一種線性最小方差估計(jì)，它能夠從一系列不完全且包含噪聲的量測(cè)值中估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)，用于實(shí)時(shí)融合動(dòng)態(tài)的低層次冗余多傳感器數(shù)據(jù)，是實(shí)現(xiàn)組合導(dǎo)航系統(tǒng)的核心技術(shù)，由于采用數(shù)學(xué)迭代的運(yùn)算方式，非常適合計(jì)算機(jī)數(shù)據(jù)運(yùn)算。主要有如下特點(diǎn)[5-6]：

1）卡爾曼濾波的估計(jì)準(zhǔn)則是估計(jì)的均方誤差最小，即

2）卡爾曼濾波是無偏估計(jì)，即估計(jì)誤差的均值為零。

3）卡爾曼濾波具有連續(xù)型和離散型兩類算法。離散型算法可直接在數(shù)字計(jì)算機(jī)上實(shí)現(xiàn)，在計(jì)算上一般采取遞推形式，離散系統(tǒng)的數(shù)學(xué)模型可以用下式表示:

其中，X是n維系統(tǒng)狀態(tài)向量；Z為m維量測(cè)向量；φ為n×n維系統(tǒng)矩陣；Γ為n×r維系統(tǒng)噪聲矩陣；H為m×n維量測(cè)矩陣；卡爾曼濾波要求{Wk}和{Vk}是互不相關(guān)且均值為零的白噪聲序列[7]。

離散型卡爾曼濾波方程組如下：

1.2 降解卡爾曼濾波算法

在設(shè)計(jì)組合導(dǎo)航系統(tǒng)卡爾曼濾波器的過程中，必須保證濾波模型的可控性和可觀性，這樣設(shè)計(jì)出的濾波器才是一致穩(wěn)定的，濾波結(jié)果的可靠性才能得到保證[8]。文獻(xiàn)[2-3]總結(jié)并給出了組合導(dǎo)航系統(tǒng)同樣可以采用分段線性系統(tǒng)理論以及采用SOM分析方法進(jìn)行系統(tǒng)的可觀測(cè)性分析的結(jié)論，并給出了一種適合于分析全程系統(tǒng)狀態(tài)的可觀測(cè)度方法。下面對(duì)基于奇異值的分解的系統(tǒng)狀態(tài)可觀測(cè)度分析方法做簡要闡述。

定理：對(duì)于任意一個(gè)m×n階矩陣A，若Rank（A）=r＜max（m，n），則存在兩個(gè)正交矩陣 Um×m和Vn×n，使

其中，σ1≥σ2≥…≥σr≥0稱為矩陣 A 的奇異值，V2和V1分別是 N（A）和 N（A）┴的標(biāo)準(zhǔn)正交基，N（A）為矩陣 A 的零空間，N（A）┴為 N（A）的正交補(bǔ)空間[9-10]。

對(duì)于 SOM 矩陣，將可觀測(cè)矩陣 Qs（r）i=[Q1Q2…Q]T進(jìn)行奇異值分解：

其中：

U=[u1，u2，…，um]，V=[v1，v2，…，vm]均為正交矩陣。

上式表明，Yj為 X0在[σ1v1，σ2v2，…，σrvr]張成的子空間上的投影變換，所以以需要r個(gè)測(cè)量值才能唯一的確定 Xj0，如果 σr＞0，則表明 Rank（Qs（r）j）=n，即系統(tǒng)是隨機(jī)可觀測(cè)的[11]，此時(shí)：

當(dāng)存在 σi+1=σi+2=…=σr=0 時(shí) Rank（Qs（r）j）＜n，即系統(tǒng)是不完全隨機(jī)可觀測(cè)的，Xj0的某些狀態(tài)是不能從觀測(cè)量Yj中估計(jì)出來的。此時(shí)，可利用奇異值分解法構(gòu)造出可觀測(cè)子空間及不可觀測(cè)子空間的標(biāo)準(zhǔn)正交基，并由此可以分析出哪些狀態(tài)是可以估計(jì)的，哪些狀態(tài)是不可以估計(jì)的[12]。

由上式可知，Xj0是由r個(gè)列向量相加得到的。對(duì)于Xj0的某一分量，各向量中對(duì)應(yīng)分量的絕對(duì)值最大的那個(gè)分量，包含了最多的的信息，對(duì)于的貢獻(xiàn)最大，而此列向量中包含相應(yīng)的因子，此奇異值σi越大，其倒數(shù)越小，則測(cè)量值σi對(duì)的變換越敏感，也就是說，的變換可以由測(cè)量值yi中更明顯的反映出來，所以，次奇異值在一定程序上反映了狀態(tài)分量的可觀測(cè)程序。奇異值σi越大，的可觀性越好[13]。

由于計(jì)算中存在誤差，用奇異值分解法計(jì)算的奇異值時(shí)，理論上為零的奇異值可能不為零，而是一個(gè)很小的數(shù)。此時(shí)需根據(jù)計(jì)算機(jī)的精度選取一個(gè)界限值ζ，對(duì)于小于ζ的奇異值均認(rèn)為是零奇異值，大于ζ的便認(rèn)為是非零的奇異值。非零奇異值的個(gè)數(shù)就是可觀測(cè)矩陣的秩。由以上敘述可知，基于奇異值的系統(tǒng)可觀測(cè)性分析方法的優(yōu)點(diǎn)是，在確定系統(tǒng)可觀測(cè)性的同時(shí)，也可以確定系統(tǒng)可觀測(cè)狀態(tài)的可觀測(cè)度[14]。

2 數(shù)字仿真及分析

2.1 飛行軌跡仿真的設(shè)定

為了驗(yàn)證組合導(dǎo)航系統(tǒng)性能，設(shè)計(jì)了一組飛行軌跡：設(shè)計(jì)飛機(jī)的初始位置為：北緯32.05°，東經(jīng)118.766 7°，高度 0m；飛機(jī)的初始速度為 0m/s，航向120°，飛行任務(wù)有上升、加速、下降、左拐、右拐、靜止等，飛行時(shí)間為3 700s。具體飛行軌跡如圖1所示。

圖1 飛行軌跡圖

2.2 常規(guī)卡爾曼濾波仿真分析

采用位置、速度組合模式，常規(guī)卡爾曼濾波仿真誤差曲線如圖2、圖3、圖4所示。

圖2 常規(guī)卡爾曼濾波組合的位置誤差曲線

圖3 常規(guī)卡爾曼濾波組合的速度誤差曲線

圖4 常規(guī)卡爾曼濾波組合的姿態(tài)角誤差曲線

2.3 降解卡爾曼濾波仿真分析

基于分段線性系統(tǒng)理論及采用SOM分析方法對(duì)松耦合SINS/GPS系統(tǒng)進(jìn)行分段，求得系統(tǒng)可觀測(cè)性矩陣的秩為11，系統(tǒng)存在4個(gè)完全不可觀測(cè)狀態(tài)。15階組合導(dǎo)航濾波器中4個(gè)完全不可觀測(cè)的系統(tǒng)狀態(tài)分別是航向角誤差φz、y軸上的陀螺隨機(jī)漂移εby，加速度計(jì)x軸上和z軸上的零位誤差由于航向角為導(dǎo)航參數(shù)，因此不能剔除，故將其余3個(gè)不可觀測(cè)的狀態(tài)變量得到12階的簡化濾波方案，濾波效果如圖5～圖7所示。

圖5 降階卡爾曼濾波組合的位置誤差曲線

圖6 降階卡爾曼濾波組合的速度誤差曲線

圖7 降階卡爾曼濾波組合的姿態(tài)誤差曲線

3 結(jié)論

由圖2～4與圖5～7對(duì)比看出：簡化的濾波方案位置和速度誤差與原濾波算法相當(dāng)，姿態(tài)誤差比原濾波方案濾波效果稍差。實(shí)質(zhì)上降階設(shè)計(jì)總是一種次優(yōu)設(shè)計(jì)[15]，但由于精度波動(dòng)的幅度在可以接受的范圍，完全滿足組合導(dǎo)航的精度要求。同時(shí)數(shù)學(xué)模型階數(shù)的降低，使卡爾曼濾波計(jì)算時(shí)間減少了大約20％，簡化的濾波方案大大減少了矩陣迭代給導(dǎo)航計(jì)算機(jī)的計(jì)算量，提高了組合導(dǎo)航系統(tǒng)實(shí)時(shí)性，因此簡化的濾波方案具有較好的理論和實(shí)用價(jià)值。

[1]秦永元，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].3版.西安：西北工業(yè)大學(xué)出版社，2014.

[2]胡小毛，劉飛.MINS/GPS全組合系統(tǒng)可觀測(cè)性分析[J].中國慣性技術(shù)學(xué)報(bào)，2011，19（1）:1-4.

[3]周衛(wèi)東，蔡佳楠.GPS/SINS超緊組合導(dǎo)航系統(tǒng)可觀測(cè)性分析[J].北京航空航天大報(bào)，2013，39（9）:1-6.

[4]嚴(yán)濤，王躍鋼，楊波.SINS/DR組合導(dǎo)航系統(tǒng)可觀測(cè)性研究[J].現(xiàn)代防御技術(shù)，2012，40（3）:1-5.

[5]羅建軍，馬衛(wèi)華.組合導(dǎo)航原理及應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，2012.

[6]王新龍，李亞峰，紀(jì)新春.SINS/GPS組合導(dǎo)航技術(shù)[M].北京：北京航空航天大學(xué)出版社，2014.

[7]雷浩然.彈載SINS/GNSS組合導(dǎo)航系統(tǒng)研究[D].南京：南京理工大學(xué)，2014.

[8]朱莉.旋轉(zhuǎn)式捷聯(lián)慣導(dǎo)系統(tǒng)晃動(dòng)基座自對(duì)準(zhǔn)方法研究[D].北京：北京工業(yè)大學(xué)，2015.

[9]張賢達(dá).矩陣分析與應(yīng)用[M].北京：清華大學(xué)出版社，2013.

[10]廖安平.矩陣論[M].北京：清華大學(xué)出版社，2013.

[11]楊建峰.捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)可觀測(cè)性分析.[J]航天控制，2014，32（1）:1-4.

[12]高葉葉，李華濱.基于觀測(cè)方程的捷聯(lián)系統(tǒng)工具誤差可觀測(cè)性分析[J].航天控制，2014，32（3）:1-5.

[13]馬朋，張福斌.基于條件數(shù)的多自主水下航行器協(xié)同定位系統(tǒng)可觀測(cè)度分析[J].兵工學(xué)報(bào)，2015:36（1）:1-6.

[14]吳松羽，王可東.低精度INS/GPS組合導(dǎo)航大方位失準(zhǔn)角初始對(duì)準(zhǔn)方案[J].全球定位系統(tǒng)，2014：39（6）:1-5.

[15]袁克非.組合導(dǎo)航系統(tǒng)多源信息融合關(guān)鍵技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2012.

The application of reduced order Kalman filter algorithm in integrated navigation

HU Bin1，YANG Xin-min1，WANG Sheng-hong2
（1.Nanjing University of Science and Technology，Nanjing 210094，China；2.Huai Industrial Group，Changzhi 046000，China）

This paper briefly introduce the basic principle of Kalman filter and analyze the observability and the degree of observability of system based on the piecewise linear system theory and the SOM method.Using the reduced order Kalman filter algorithm to reasonably eliminate the state factors which are not observable or low observable and make a reduced-order design for the high-end system.Using the reduced order Kalman filter algorithm to reasonably eliminate the state factors which are not observable or low observable and make a reduced-order design for the high-end system.At the same time，the reduction of the degree， which has the important practical significance， significantly reduces the computational burden of navigation computer and enhances the real-time so that the algorithm can be more easily realized digitally.

inertial navigation； integrated navigation； Kalman filter； digital simulation

TN961

：A

：1674－6236（2017）15-0098-04

2016-06-07稿件編號(hào)：201606058

胡 彬（1990—），男，江蘇連云港人，碩士。研究方向：組合導(dǎo)航算法。