彭 漢，劉增力

（昆明理工大學(xué)信息工程與自動化學(xué)院，云南 昆明 650500）

卡爾曼濾波技術(shù)在捷聯(lián)慣性導(dǎo)航系統(tǒng)對準(zhǔn)中的應(yīng)用

彭 漢，劉增力

（昆明理工大學(xué)信息工程與自動化學(xué)院，云南 昆明 650500）

本文首先闡述了卡爾曼濾波技術(shù)的簡單理論，然后建立了捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對準(zhǔn)的卡爾曼濾波模型, 并對靜基座下的卡爾曼濾波初始對準(zhǔn)精度進行分析，最后進行了卡爾曼濾波仿真。仿真結(jié)果表明,此方法算法簡單,能有效縮短初始對準(zhǔn)時間,對準(zhǔn)穩(wěn)態(tài)精度較高,是一種可靠的初始對準(zhǔn)方案。

捷聯(lián)慣性導(dǎo)航系統(tǒng)；初始對準(zhǔn)；卡爾曼濾波

0 引言

慣性導(dǎo)航是一種自主導(dǎo)航方法，它是通過測量運載體本身的加速度來完成導(dǎo)航任務(wù)的。根據(jù)牛頓慣性原理，利用慣性元器件（陀螺儀、加速度計）測量出運載體的加速度，經(jīng)過積分和運算，可得出速度和位置信息以供導(dǎo)航使用[1]。

所謂初始對準(zhǔn)，就是在慣性導(dǎo)航系統(tǒng)尚未正式進入導(dǎo)航工作狀態(tài)前，建立導(dǎo)航狀態(tài)所必須的初始條件。20世紀(jì) 60年代初，卡爾曼濾波理論就開始應(yīng)用到慣性導(dǎo)航系統(tǒng)中，從而為慣性導(dǎo)航系統(tǒng)的初始對準(zhǔn)、速度阻尼和位置點重置提供了一種統(tǒng)一的方法，1965年，L.D.Brock[2]在其論文中利用卡爾曼濾波算法處理平臺慣性導(dǎo)航系統(tǒng)的初始對準(zhǔn)和標(biāo)定問題。1967年，J.F.Bellantoni[3]將卡爾曼濾波用在捷聯(lián)慣性導(dǎo)航系統(tǒng)的初始對準(zhǔn)和標(biāo)定中。當(dāng)失準(zhǔn)角為小角度時，SINS的誤差方程可近似為線性方程，可以通過兩位置對準(zhǔn)、多位置對準(zhǔn)和卡爾曼濾波器實現(xiàn)初始對準(zhǔn)。目前，基于線性方程的初始對準(zhǔn)技術(shù)已經(jīng)比較成熟。

1 卡爾曼濾波理論

卡爾曼濾波屬于最優(yōu)估計理論，是一種遞推線性最小方差估計法[4]。設(shè)離散系統(tǒng)狀態(tài)方程和測量方程分別為

其中 Xk為k時刻的n維狀態(tài)矢量，即被估計的狀態(tài)矢量；Zk為k時刻的m維量測矢量；為k-1時刻的r維系統(tǒng)噪聲；為n×r階系統(tǒng)噪聲矩陣，表征由 k -1到k時刻的各系統(tǒng)噪聲分別影響k時刻各個狀態(tài)的程度；Hk為k時刻的m×n階測量矩陣； Vk為k時刻的m維量測噪聲序列。

卡爾曼濾波要求 Wk-1和 Vk為互不相關(guān)的零均值白噪聲序列[5]，且

其中kQ和kR是系統(tǒng)噪聲的方差陣和量測噪聲的方差陣，該矩陣為已知的非負(fù)正定陣。kjδ是Kroneckerδ 函數(shù)，其統(tǒng)計特性為：

如果已知kt時刻的測量值為kZ，則kX 的估計值 ?kX 按照下列方程求解：

狀態(tài)一步預(yù)測方程：

在公式（6）中，作為狀態(tài) Xk-1的卡爾曼濾波估值，可以將 X ?k-1認(rèn)為是利用 k -1以前時刻的量測值計算而得到的；而 X ?k/k-1則認(rèn)為是用 X ?k-1計算得到的對 Xk的一步預(yù)測。

狀態(tài)估計值計算方程：

濾波增益方程：

一步預(yù)測方差：

估計均方誤差方程：

或

上式為離散型卡爾曼濾波基本方程。只要給定初值 X ?0和P0,據(jù)tk時刻的量測量Zk，就可以遞推出tk時刻的狀態(tài)估計

2 捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對準(zhǔn)濾波模型的建立

根據(jù)系統(tǒng)的誤差傳播方程可知[6]：陀螺和加速度計的誤差均包含隨機常值誤差、一階馬爾科夫過程和白噪聲誤差這三種分量。馬爾可夫過程的相關(guān)時間一般大于1小時，所以這種誤差可近似視為隨機常數(shù)，且與隨機常值漂移相比小12～個數(shù)量級。所以，陀螺和加速度計誤差模型可簡化為[8]：

在實際工程運用中，由于捷聯(lián)慣導(dǎo)系統(tǒng)的慣性器件（陀螺儀和加速度計）是直接固連在載體上，因此其誤差也必然是在載體坐標(biāo)系b系中得到的。而在本模型中，IMU誤差則是在導(dǎo)航坐標(biāo)系n中，因此需要將b系下的量轉(zhuǎn)換到n系中：

由于本文的應(yīng)用對象為艦船導(dǎo)航系統(tǒng)，因而可以將垂直通道對系統(tǒng)的影響忽略，并將陀螺儀和加速度計誤差擴充為狀態(tài)量。此時，系統(tǒng)的狀態(tài)方程可以寫成：

W( t)為系統(tǒng)噪聲：

在公式（16）中， ax、 ay為加速度計在b系下的噪聲， ωx、 ωy、 ωz為陀螺在b系下的噪聲。加速度計和陀螺的噪聲均為呈正態(tài)分布的白噪聲，其均值為0、方差為 ()Q t。其中：

取水平速度誤差為觀測量，系統(tǒng)觀測方程為：

通常情況下，實際應(yīng)用中的傳感器誤差的可觀測性是相當(dāng)?shù)偷?，甚至很多情況是不可觀測的，在這種情況下，如果將濾波模型的維數(shù)增大，不但無法估計傳感器的誤差，反而會導(dǎo)致系統(tǒng)初始對準(zhǔn)精度的降低。為了解決這個問題，可以把陀螺的常值漂移和加速度計的常值零偏不作為狀態(tài)變量，使得濾波模型的維度從十維降到五維，以減少計算復(fù)雜度，同時可以令濾波估計變得更加穩(wěn)定。這五維狀態(tài)矢量為：

五維狀態(tài)方程為：

3 靜基座卡爾曼濾波初始對準(zhǔn)精度分析

靜基座或準(zhǔn)靜基座的卡爾曼濾波對準(zhǔn)特點如下[7]：

（a）零速匹配（取 0v= ）

在捷聯(lián)慣導(dǎo)系統(tǒng)初始對準(zhǔn)技術(shù)中，卡爾曼濾波技術(shù)是一種運用外界的測量信息來估計系統(tǒng)誤差項的初始對準(zhǔn)方法，在整個初始對準(zhǔn)過程中，都需要利用其他的測量元器件測量外界測量信息，而這些外界測量信息的測量精度與產(chǎn)生的誤差都會對濾波器的狀態(tài)估量產(chǎn)生相應(yīng)的影響。在靜基座或準(zhǔn)靜基座的條件下，可以直接令外界測量信息為零速度，以此避免速度測量誤差對濾波器的影響，使得系統(tǒng)在對準(zhǔn)的速度和精度上都有一定程度的提高。

（b）狀態(tài)估計矢量降維

在靜基座條件下，默認(rèn)載體的初始位置不變，那么初始對準(zhǔn)的載體位置是不需要進行修正的。因此可以將載體位置誤差從狀態(tài)估計矢量中去除，降低卡爾曼濾波器的維數(shù)，同時降低計算量以減少計算負(fù)擔(dān)。

（c）器件誤差在地理坐標(biāo)系上不變

在靜基座條件下，從濾波器的角度來看，可以把陀螺的常值漂移和加速度計的常值零偏狀態(tài)直接設(shè)置在地理坐標(biāo)系上。然后直接使用數(shù)字平臺轉(zhuǎn)化到載體坐標(biāo)系上即可。這樣就可以按照平臺慣性導(dǎo)航的分析方式分析捷聯(lián)慣性導(dǎo)航的對準(zhǔn)精度，簡化精度分析過程。

下面建立靜基座情況下初始對準(zhǔn)卡爾曼濾波器。

由慣性導(dǎo)航系統(tǒng)的誤差方程，在靜基座條件下，系統(tǒng)的真實速度為0，該誤差方程可以被簡化為：

根據(jù)靜基座的特點，一方面，靜基座條件下載體坐標(biāo)系和地理坐標(biāo)系不存在相對運動，器件誤差在地理坐標(biāo)系上也為常值，因此直接將地理坐標(biāo)系上的器件誤差設(shè)置為狀態(tài)變量可以方便精度分析可；另一方面，由于位置誤差無需估計和補償，可將位置誤差從狀態(tài)估計矢量中去除。因此，可將系統(tǒng)的狀態(tài)矢量設(shè)置為：

考慮到慣性元器件的噪聲和在近似過程中的模型誤差，可將系統(tǒng)誤差狀態(tài)矢量表示為：

由靜基座對準(zhǔn)得特點[9]，當(dāng)選取零速度作為量測信息時，此時的導(dǎo)航系統(tǒng)中，實際速度只有系統(tǒng)本身的振動或晃動。所以如果以零速為外測信息，那么測量誤差可以寫為：

捷聯(lián)慣性導(dǎo)航所計算出得速度誤差可以表示為：

其中 δ vE、 δvN分別為慣性導(dǎo)航計算出來得速度無差量。

將慣性導(dǎo)航速度方程與零速度方程作差即可得：

其中：

4 仿真實驗

在靜基座初始條件下進行計算機仿真。設(shè)置初始參數(shù)：

狀態(tài)矢量 X ( 0) = 0 ；陀螺常值漂移 = 0 .02°/h ，隨機漂移 = 0 .01°/h ；加速度計常值零偏隨機零偏取濾波周期 T = 0 .1s。

經(jīng)過卡爾曼濾波后，得到如下結(jié)果：

圖1 仿真結(jié)果Fig.1 Simulation result

5 結(jié)論

經(jīng)過上面的分析，進行計算機仿真實驗后，由輸出結(jié)果可以看出，在靜基座初始條件下，經(jīng)過卡爾曼濾波器處理后的速度誤差在相當(dāng)短的時間內(nèi)就收斂于0并保持穩(wěn)定。因此卡爾曼濾波技術(shù)在靜基座下的捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對準(zhǔn)中，能夠起到提高系統(tǒng)對準(zhǔn)精度的作用。

[1] 陳明輝. SINS誤差特性及組合對準(zhǔn)的方法研究[D]. 哈爾濱工程大學(xué), 2008.

[2] Brock L D. Application of statistical estimation to navigation systems. MIT, 1965

[3] Bellantoni J F, Dodge K W. A Square Root Formulation of the Kalman-Schmidt Filter. Journal of AIAA, 1967, 5(7).

[4] 程向紅, 萬德鈞. 捷聯(lián)慣導(dǎo)系統(tǒng)的課觀測性和可觀測度研究. 東南大學(xué)學(xué)報, 1997, 27(6): 6-11.

[5] 雷曙光, 張柳, 林列書. 卡爾曼濾波在水下慣性導(dǎo)航系統(tǒng)中的研究及實現(xiàn)[J]. 艦船科學(xué)技術(shù), 2015, 37(07).

[6] 李瑩, 楊新, 李波. 捷聯(lián)航姿系統(tǒng)航行狀態(tài)下的組合對準(zhǔn)算法研究[J]. 艦船電子工程, 2015, 35(02).

[7] 張義. 艦船捷聯(lián)慣性系統(tǒng)初始對準(zhǔn)技術(shù)研究[D]. 哈爾濱工程大學(xué), 2012.

[8] 胡再剛. 基于H∝的傳遞對準(zhǔn)方法研究[D]. 哈爾濱工程大學(xué), 2007: 14-15.

[9] 陳哲. 捷聯(lián)慣導(dǎo)系統(tǒng)原理. 北京: 國防工業(yè)出版社, 1986.

Application of Calman Filtering Technique in Alignment of Strapdown Inertial Navigation System

PENG Han, LIU Zeng-li
(Department of Communication Engineering, KunMing University of Science and Technology Kunming 650500, China)

This paper describes the simple theory of Calman filtering technology, and then established the Calman filter model of initial alignment of strapdown inertial navigation system, and Calman filter to initial static base alignment accuracy is analyzed, finally the simulation of Calman filter. The simulation results show that the algorithm is simple and can effectively shorten the initial alignment time, and the alignment steady state precision is high. It is a reliable initial alignment scheme.

Strapdown inertial navigation system (SINS); Initial alignment; Calman filtering

TN966

A

10.3969/j.issn.1003-6970.2017.12.027

本文著錄格式：彭漢，劉增力. 卡爾曼濾波技術(shù)在捷聯(lián)慣性導(dǎo)航系統(tǒng)對準(zhǔn)中的應(yīng)用[J]. 軟件，2017，38（12）：143-147

彭漢(1991-)，男，碩士研究生，主要研究方向：現(xiàn)代數(shù)字信號處理。

劉增力，男，教授，碩士研究生導(dǎo)師，主要研究方向：現(xiàn)代數(shù)字信號處理。