彭 漢，劉增力

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

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

彭 漢，劉增力

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

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

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

0 引言

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

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

1 卡爾曼濾波理論

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

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

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

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

如果已知kt時(shí)刻的測(cè)量值為kZ，則kX 的估計(jì)值 ?kX 按照下列方程求解：

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

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

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

濾波增益方程：

一步預(yù)測(cè)方差：

估計(jì)均方誤差方程：

或

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中：

4 仿真實(shí)驗(yàn)

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

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

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

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

5 結(jié)論

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

[1] 陳明輝. SINS誤差特性及組合對(duì)準(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] 程向紅, 萬(wàn)德鈞. 捷聯(lián)慣導(dǎo)系統(tǒng)的課觀測(cè)性和可觀測(cè)度研究. 東南大學(xué)學(xué)報(bào), 1997, 27(6): 6-11.

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

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

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

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

[9] 陳哲. 捷聯(lián)慣導(dǎo)系統(tǒng)原理. 北京: 國(guó)防工業(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)對(duì)準(zhǔn)中的應(yīng)用[J]. 軟件，2017，38（12）：143-147

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

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