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

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

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

1 引言

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

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

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

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

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

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

2.1 慣性儀表誤差模型

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

（1）陀螺漂移誤差模型

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

式中：Q和 R分別為系統(tǒng)噪聲方差陣和量測噪聲方差陣，只要給定初值，便可利用上式遞推得到各狀態(tài)的最優(yōu)估計(jì)并進(jìn)行實(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)動包括：平直加速、平直勻速行駛、平直減速、停車這五個狀態(tài)。發(fā)射車每隔一定時間T重復(fù)這一運(yùn)動過程。在停車位置進(jìn)行卡爾曼濾波的零速修正，得到狀態(tài)的最優(yōu)估計(jì)并進(jìn)行閉環(huán)實(shí)時補(bǔ)償，停車時間即為卡爾曼濾波迭代時間τ。

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

國外資料表明，零速修正的停車間隔一般為3～5分鐘[5]。為了考察停車間隔時間T對零速修正性能的影響，本文設(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公里，仿真時間均在1小時左右，在停車位置濾波迭代40s進(jìn)行修正。

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

?

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

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

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

! 結(jié)論

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

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

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

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

[3]楊大燁，謝天懷，胡寶余.捷聯(lián)慣導(dǎo)系統(tǒng)初始姿態(tài)和儀表誤差標(biāo)定迭代算法研究[J].中國慣性技術(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].中國慣性技術(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)的靜基座快速精對準(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.