基于卡爾曼濾波器的零速修正技術(shù)在導(dǎo)彈發(fā)射車中的應(yīng)用研究

淮陰師范學(xué)院（淮安 223001） 楊裕翠 俞阿龍

南京航空航天大學(xué)（南京 210016） 祝燕華

1 引言

現(xiàn)代戰(zhàn)爭(zhēng)中，為了提高導(dǎo)彈武器系統(tǒng)的生存能力和戰(zhàn)斗能力，采用隨機(jī)點(diǎn)快速發(fā)射（機(jī)動(dòng)發(fā)射）是一項(xiàng)重要的措施。傳統(tǒng)的定點(diǎn)發(fā)射方式，需預(yù)先在發(fā)射場(chǎng)坪標(biāo)定出大地坐標(biāo)，這既不適用于任意點(diǎn)發(fā)射，又不利于縮短反應(yīng)時(shí)間；而機(jī)動(dòng)發(fā)射帶來的問題是如何能快速、精確地測(cè)定發(fā)射點(diǎn)的地理坐標(biāo)位置。

目前，用于發(fā)射車定位的技術(shù)主要有慣性定位和衛(wèi)星定位技術(shù)，本文所研究的問題是針對(duì)慣性定位而言的。慣性定位系統(tǒng)能實(shí)時(shí)自主地提供載體的姿態(tài)、速度和位置信息，但是它的不足之處在于慣性器件引起的定位誤差隨時(shí)間積累，因而長(zhǎng)時(shí)間獨(dú)立工作后的定位信息的誤差大[1,2]。

本文提出了將零速修正技術(shù)應(yīng)用在導(dǎo)彈發(fā)射車車載捷聯(lián)慣導(dǎo)系統(tǒng)中，采用卡爾曼濾波對(duì)慣導(dǎo)誤差進(jìn)行實(shí)時(shí)計(jì)算并補(bǔ)償，提高了慣導(dǎo)的定位精度，可以實(shí)現(xiàn)隨機(jī)點(diǎn)的快速定位。

2 基于卡爾曼濾波的零速修正算法

零速修正（ZUPT）技術(shù)是利用載體停車時(shí)慣性系統(tǒng)的速度輸出作為系統(tǒng)速度誤差的觀測(cè)量，進(jìn)而對(duì)其它各項(xiàng)誤差實(shí)現(xiàn)校正的技術(shù)。具體的實(shí)現(xiàn)過程為，慣導(dǎo)系統(tǒng)啟動(dòng)并經(jīng)初始對(duì)準(zhǔn)后，從一個(gè)已知坐標(biāo)點(diǎn)出發(fā)，每隔一定時(shí)間停車進(jìn)行零速修正，到達(dá)待測(cè)點(diǎn)后，在短時(shí)間內(nèi)（如1分鐘）即可獲得該點(diǎn)的坐標(biāo)數(shù)據(jù)。這種方法除需要一個(gè)起始坐標(biāo)點(diǎn)外，途中不需要任何外部信息，因此將其應(yīng)用在導(dǎo)彈發(fā)射車上，對(duì)于現(xiàn)代戰(zhàn)爭(zhēng)環(huán)境下的應(yīng)用具有十分明顯的優(yōu)勢(shì)。

采用零速修正進(jìn)行誤差估計(jì)的算法有很多[2]，如曲線擬合、最大似然估計(jì)和卡爾曼濾波等。曲線擬合和最大似然估計(jì)方法由于模型簡(jiǎn)單且對(duì)時(shí)間間隔的依賴性大，所以估值精度較低；而卡爾曼濾波是線性最小方差的無偏估計(jì)，估值精度較高，并且從誤差特性考慮。本文針對(duì)某國(guó)防項(xiàng)目的實(shí)際研究需要，采用基于實(shí)時(shí)卡爾曼濾波的零速修正誤差估值算法，分析零速修正時(shí)不同時(shí)間間隔對(duì)系統(tǒng)性能和精度的影響。

2.1 慣性儀表誤差模型

慣性儀表誤差主要包括安裝誤差、刻度系數(shù)誤差和隨機(jī)誤差，認(rèn)為非隨機(jī)性誤差已通過多位置量測(cè)等方法進(jìn)行校正，這里僅考慮隨機(jī)誤差。

（1）陀螺漂移誤差模型

式中，!b為隨機(jī)常數(shù)；!r為一階馬爾可夫過程；"g為白噪聲。

假定三個(gè)軸向的陀螺漂移誤差模型相同，均為

式中，Tg為馬氏過程的相關(guān)時(shí)間，"r為馬氏過程的白噪聲。

（2）加速度計(jì)誤差模型

考慮其為一階馬爾可夫過程，且假定三個(gè)軸向加速度計(jì)的誤差模型相同，均為

式中，Ta為馬氏過程的相關(guān)時(shí)間，ωa為馬氏過程的白噪聲。

2.2 基于卡爾曼濾波器的ZUPT模型

基于卡爾曼濾波器的ZUPT模型的基本原理如圖所示：

發(fā)射車車載捷聯(lián)慣導(dǎo)系統(tǒng)的誤差模型可用下列狀態(tài)方程描述：

上式中，狀態(tài)向量可分為兩個(gè)向量來考慮

其中，X1為慣導(dǎo)系統(tǒng)的九個(gè)導(dǎo)航誤差狀態(tài)向量，X2為九個(gè)傳感器誤差的增廣狀態(tài)向量，如下所示：

T（t）為18階的系統(tǒng)矩陣，G（t）為18×9的系統(tǒng)噪聲系數(shù)矩陣，W（t）表示系統(tǒng)噪聲。

在導(dǎo)彈發(fā)射車停車時(shí)，真實(shí)的速度輸出在東、北、天方向的分量都應(yīng)該為零，那么慣導(dǎo)的速度輸出必然就是速度誤差。所以，以慣導(dǎo)系統(tǒng)的速度輸出作為觀測(cè)量，建立下面的觀測(cè)方程：

其中，V（t）表示量測(cè)噪聲，H值應(yīng)為：

于是經(jīng)離散化后，卡爾曼濾波器有下列方程組

式中：Q和 R分別為系統(tǒng)噪聲方差陣和量測(cè)噪聲方差陣，只要給定初值，便可利用上式遞推得到各狀態(tài)的最優(yōu)估計(jì)并進(jìn)行實(shí)時(shí)的補(bǔ)償[3,4]。

3 仿真分析

3.1 仿真流程圖及其初始條件

導(dǎo)彈發(fā)射車捷聯(lián)慣導(dǎo)系統(tǒng)零速修正仿真流程圖給定如下：

導(dǎo)彈發(fā)射車純捷聯(lián)慣導(dǎo)誤差參數(shù)如表1所示：

3.2 系統(tǒng)仿真和結(jié)果

導(dǎo)彈發(fā)射車初始位置為東經(jīng)118.0°，北緯 32.0°，海 拔 高 度1000.0米，初始姿態(tài)水平，初始航向角為0.0°。

導(dǎo)彈發(fā)射車的運(yùn)動(dòng)包括：平直加速、平直勻速行駛、平直減速、停車這五個(gè)狀態(tài)。發(fā)射車每隔一定時(shí)間T重復(fù)這一運(yùn)動(dòng)過程。在停車位置進(jìn)行卡爾曼濾波的零速修正，得到狀態(tài)的最優(yōu)估計(jì)并進(jìn)行閉環(huán)實(shí)時(shí)補(bǔ)償，停車時(shí)間即為卡爾曼濾波迭代時(shí)間τ。

要想從根本上提高民辦高校實(shí)驗(yàn)建設(shè)和教學(xué)管理水平，既滿足教學(xué)需求，又能做到為學(xué)生負(fù)責(zé)，為國(guó)家提供自主創(chuàng)新的人才。需要民辦高校不斷深化教育教學(xué)改革，對(duì)實(shí)驗(yàn)教學(xué)管理提出更高的要求，并在此基礎(chǔ)上不斷培養(yǎng)高素質(zhì)、高水平的技術(shù)應(yīng)用型人才。但是，現(xiàn)在的民辦高校還不能更好的適應(yīng)社會(huì)的發(fā)展，實(shí)驗(yàn)室的管理水平還不夠科學(xué)規(guī)范，實(shí)驗(yàn)室的利用率沒有充分挖掘，不能更好的服務(wù)于學(xué)生。要提高教學(xué)質(zhì)量，需要進(jìn)一步解決實(shí)驗(yàn)教學(xué)中存在的不足。

國(guó)外資料表明，零速修正的停車間隔一般為3～5分鐘[5]。為了考察停車間隔時(shí)間T對(duì)零速修正性能的影響，本文設(shè)計(jì)了三條航跡進(jìn)行仿真比較，這三條航跡中發(fā)射車所經(jīng)過的路徑是完全相同的，則車輛的行駛里程也必然相同，不同點(diǎn)在于停車間隔T，具體說明如下：

＊航跡1：停車間隔T=300s，車輛行駛過程中的加速度為5.00m/s/s，最大速度為25.0m/s；

＊航跡2：停車間隔T=500s，車輛行駛過程中的加速度為4.96m/s/s，最大速度為20.6m/s；

＊航跡3：停車間隔T=900s，車輛行駛過程中的加速度為4.12m/s/s，最大速度為24.8m/s；

行駛里程共73.75公里，仿真時(shí)間均在1小時(shí)左右，在停車位置濾波迭代40s進(jìn)行修正。

為了考察以及比較零速修正的性能，我們給出整個(gè)仿真過程導(dǎo)彈發(fā)射車?yán)硐虢?jīng)緯度位置曲線（曲線1）、導(dǎo)彈發(fā)射車上純捷聯(lián)慣導(dǎo)解算得到的經(jīng)緯度位置曲線及其誤差曲線（曲線5），以及三條不同停車間隔的SINS+ZUPT解算得到的經(jīng)緯度位置曲線及其誤差曲線（T=300s時(shí)，曲線2；T=500s時(shí)，曲線 3；T=900s時(shí)，曲線 4），如圖 3、4和 5所示：上面的曲線圖不僅直觀地反映了零速修正技術(shù)大大降低了發(fā)射車的位置誤差，提高了捷聯(lián)慣導(dǎo)的定位精度，而且說明了零速修正的有效性會(huì)隨著停車間隔時(shí)間的增長(zhǎng)而降低。

?

由于導(dǎo)彈發(fā)射車零速修正的目的在于實(shí)現(xiàn)導(dǎo)彈的隨機(jī)點(diǎn)快速發(fā)射，即要能以較高的精度給出發(fā)射車到達(dá)發(fā)射點(diǎn)時(shí)刻的位置，為此給出四種情況下發(fā)射車到達(dá)終點(diǎn)時(shí)刻的定位數(shù)據(jù)，如表2所示：

分析表格中的數(shù)據(jù)可知，停車間隔T=300s時(shí)終點(diǎn)經(jīng)度、緯度誤差都在500米左右，相對(duì)純捷聯(lián)而言，精度提高了100倍，隨著T的增加，精度降低，到T=900s時(shí)終點(diǎn)經(jīng)度、緯度誤差已達(dá)2000米以上，零速修正的效果已經(jīng)不是很顯著。以本文仿真所給出的IMU精度來講，停車間隔在10分鐘以內(nèi)SINS+ZUPT的定位精度是比較高的。

一方面，對(duì)于導(dǎo)彈隨機(jī)發(fā)射點(diǎn)的定位精度來說，顯然停車間隔T越短越好；另一方面，對(duì)于發(fā)射車發(fā)動(dòng)機(jī)而言，頻繁加速減速停車會(huì)增重其負(fù)擔(dān)，停車間隔T又不易太短。所以，在工程實(shí)踐中，應(yīng)同時(shí)從這兩個(gè)方面考慮，并結(jié)合IMU的精度，選擇合適的停車間隔時(shí)間。

! 結(jié)論

本文提出了將零速修正技術(shù)應(yīng)用在導(dǎo)彈發(fā)射車車載捷聯(lián)慣導(dǎo)系統(tǒng)中，并采用卡爾曼濾波方法對(duì)慣導(dǎo)誤差進(jìn)行實(shí)時(shí)計(jì)算并補(bǔ)償，經(jīng)過多次的仿真驗(yàn)證，均表明卡爾曼濾波的零速修正其有效性隨著停車間隔時(shí)間的增長(zhǎng)而降低，從原理上分析也可知，一方面是由于慣導(dǎo)系統(tǒng)位置誤差快速增長(zhǎng)；另一方面，慣導(dǎo)速度誤差與位置誤差變化的相關(guān)性隨著停車間隔T的增大而降低。因此在工程實(shí)踐中應(yīng)綜合考慮定位精度要求和導(dǎo)彈發(fā)射車行車的需求，選擇合理的停車間隔。

綜上，本文提出的卡爾曼濾波的零速修正能在較大程度上減小慣導(dǎo)的定位誤差，解決了導(dǎo)彈武器系統(tǒng)發(fā)射車的隨機(jī)點(diǎn)快速發(fā)射問題，而且通過對(duì)零速修正間隔時(shí)間不同對(duì)慣導(dǎo)性能的影響比較，在工程實(shí)踐中具有重要的參考價(jià)值。

[1]李輝，王子濱.國(guó)外GPS/INS復(fù)合制導(dǎo)技術(shù)的發(fā)展[J].現(xiàn)代防御技術(shù)，2004，32（2）：42 ～ 46.

[2]高鐘毓，王進(jìn)，董景新，趙長(zhǎng)德.慣性測(cè)量系統(tǒng)零速修正的幾種估計(jì)方法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，1995，3（2）：24 ～ 29.

[3]楊大燁，謝天懷，胡寶余.捷聯(lián)慣導(dǎo)系統(tǒng)初始姿態(tài)和儀表誤差標(biāo)定迭代算法研究[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2002，10（3）：15 ～ 20.

[4]Xiaoying Kong.INS algorithm using quaternion model for low cost IMU.Robotics and Autonomous Systems,2004,46（4）:221 ～246.

[5]Hu Shan,Luo Jian－Jun,Yuan Jian－ping.Development of simulation system of INS/DS integrated system.Journal of System Simulation,2004,16（9）:57～60.

[6]馬小艷，富立，王玲玲.不等間隔角速率輸出的捷聯(lián)航姿算法研究[J].中國(guó)慣性技術(shù)學(xué)報(bào),2006,14（3）:18～22.

[7]Bortz J E.A new mathematical formulation for strapdown inertial navigation[J].IEEE Transactions on Aerospace and Electronic Systems,1971,7（1）:61～66.

[8]祝燕華，劉建業(yè)，錢偉行，賴際舟.低成本捷聯(lián)慣導(dǎo)系統(tǒng)的靜基座快速精對(duì)準(zhǔn)方法[J].上海交通大學(xué)學(xué)報(bào),2008,32（05）：32 ～37.

[9]Miller R B.A new strap － down attitude algorithm[J].Journal of Guidance,Control,and Dynamics,19183,6（4）:287～291.