范紹里 王 磊,2 陸巍巍

(1.海軍航空工程學(xué)院 煙臺 264001)(2.91515部隊 三亞 572016)

?

參考地物的飛航導(dǎo)彈INS誤差估計及補償方法*

范紹里1王 磊1,2陸巍巍1

(1.海軍航空工程學(xué)院 煙臺 264001)(2.91515部隊 三亞 572016)

針對飛航導(dǎo)彈慣導(dǎo)系統(tǒng)(INS)單獨使用時存在位置和速度估計誤差發(fā)散的問題,從便于工程實現(xiàn)的角度,提出了一種基于對航路上已知地標(biāo)連續(xù)、被動觀測的INS誤差估計及補償方法。該誤差估計方法以INS誤差作為狀態(tài)量,以導(dǎo)彈相對于地標(biāo)的觀測序列與INS估計信息解算的偽觀測序列構(gòu)造觀測量,通過迭代求解,分兩個階段分別估計出INS水平通道和垂直通道誤差,并利用平均去噪的思想提高了估計精度。該誤差補償方法考慮了誤差估計耗時對補償精度的影響,采用實時補償?shù)姆绞浇档土司葥p失。仿真結(jié)果表明:誤差補償后,飛航導(dǎo)彈INS位置估計精度收斂到3m之內(nèi),速度估計精度收斂到0.5m/s之內(nèi),驗證了論文方法的有效性。

視覺導(dǎo)航; INS誤差估計; 迭代法; 飛航導(dǎo)彈; 觀測序列; 平均去噪

Class Number E927

1 引言

在飛航導(dǎo)彈導(dǎo)航領(lǐng)域,慣性導(dǎo)航是擁有絕對統(tǒng)治地位的導(dǎo)航方式。但單獨使用INS(Inertial Navigation System)長時間工作時,往往不能滿足導(dǎo)航精度要求,有必要對彈上INS解算的地理坐標(biāo)進行修正。

GPS/INS組合導(dǎo)航是對INS漂移誤差的一種有效補償方法,但戰(zhàn)爭時期GPS等衛(wèi)星導(dǎo)航信息將可能不可用[1]。彈載前視裝置(例如TV和紅外成像)是實現(xiàn)精確末制導(dǎo)所必須的,在中制導(dǎo)階段,它也可被用于對導(dǎo)彈的INS誤差進行修正[2]。

國內(nèi)外很多學(xué)者在飛行器視覺輔助導(dǎo)航方面進行了研究,取得了許多有用的成果[3～13]。與一般飛行器不同,飛航導(dǎo)彈視覺輔助導(dǎo)航有如下特點:飛航導(dǎo)彈在中制導(dǎo)階段是沿著規(guī)劃的航路飛行的,視覺導(dǎo)航不能改變其巡航路徑;視覺導(dǎo)航環(huán)境具有大規(guī)模、非結(jié)構(gòu)化、稀疏特征的特點,單次視覺導(dǎo)航過程中,飛行航路附近一般僅有一個已知固定地面目標(biāo)(簡稱地標(biāo))可供參考;僅支持單目視覺,且視場范圍有限。這樣,上述文獻中所提到的方法難以推廣應(yīng)用于飛航導(dǎo)彈的導(dǎo)航與定位。

針對導(dǎo)彈視覺輔助導(dǎo)航與定位的文獻比較少見。文獻[14]研究了使用導(dǎo)引頭跟蹤地物修正慣導(dǎo),利用擴展卡爾曼濾波對視線角速率和導(dǎo)彈的高度、速度信息進行信息融合,實現(xiàn)了地空導(dǎo)彈的慣導(dǎo)水平位置誤差修正,但誤差估計精度僅為80m,且高度和速度濾波結(jié)果不能收斂。文獻[15]在假定INS速度估計精確的前提下,提出了基于視線角、視線角速率及高度量測的UKF濾波INS位置誤差修正方法。而實際上INS速度估計是存在常值誤差的。文獻[16～17]研究了基于成像幀頻計算無人機的瞬時飛行速度。針對導(dǎo)彈進行視覺輔助速度估計的文獻未見報道。

針對上述問題,結(jié)合中制導(dǎo)段飛航導(dǎo)彈視覺輔助導(dǎo)航的特點,本文從易于工程實現(xiàn)的角度,提出了一種視覺輔助INS誤差估計和補償方法:僅依托彈載前視裝置對航路上單個已知地標(biāo)的連續(xù)、被動觀測,實現(xiàn)INS位置和速度誤差的估計,并進行誤差實時補償。仿真分析表明,該方法能有效解決飛航導(dǎo)彈INS單獨使用時位置和速度估計誤差發(fā)散的問題。

2 導(dǎo)航數(shù)學(xué)模型

導(dǎo)彈相對于地標(biāo)的可觀測距離有限,且受導(dǎo)引頭視場的限制可觀測時間也很短,因此,地球自轉(zhuǎn)和地球曲率對研究結(jié)果影響很小,故可以在局部范圍內(nèi)假設(shè)大地為平面。導(dǎo)彈與地標(biāo)間的相對關(guān)系如圖1所示。

圖1 導(dǎo)彈與地標(biāo)相對關(guān)系

Az=arctan(y/x)

(1)

(2)

實際應(yīng)用時需要具體考慮視線角所在象限。

(3)

(4)

3 INS誤差估計及補償方法

3.1 量測信息及處理

飛航導(dǎo)彈在中制導(dǎo)段飛行過程中,利用其前視裝置對進入視場的地標(biāo)進行連續(xù)、被動觀測,觀測過程中任意采樣時刻ti(i=1,2,…,n),都能夠獲得導(dǎo)彈相對于被測地標(biāo)的一組觀測信息:

(5)

另一方面,由于慣導(dǎo)在鉛垂方向加速度計的積分是發(fā)散的,飛航導(dǎo)彈采用INS和氣壓高度表組合導(dǎo)航,ti時刻導(dǎo)彈能夠估計得到其狀態(tài)信息

(6)

(7)

這樣,就得到了可以用于飛航導(dǎo)彈INS誤差修正的量測信息,如圖2所示,并將其實時存儲于彈載計算機。

圖2 量測信息示意圖

觀測結(jié)束后,利用INS估計信息構(gòu)造的偽觀測序列ZI(ti)與前視裝置獲得的實際觀測序列Zm(ti)構(gòu)造殘差,以殘差作為觀測量,INS誤差作為狀態(tài)量,建立量測方程。

tanAzI≈tanAz+a11δx+a12δy

(8a)

tanElI≈tanEl+a21δx+a22δy+a23δz

(8b)

(8c)

(8d)

又前視裝置觀測信息可以表示為

tanAzm≈tanAz+w1

(9a)

tanElm≈tanEl+w2

(9b)

(9c)

(9d)

式中,w1、w2、w3、w4分別為方位角、高度角、方位角速率和高度角速率量測噪聲。

將式(8)與式(9)相減,可得INS誤差估計量測方程:

(10)

由于INS誤差量有六個,而導(dǎo)彈相對于被測地標(biāo)的可觀測量只有四個,因此,式(10)無法全部估計出導(dǎo)彈的INS誤差量。考慮到INS垂直通道的估計是由氣壓高度表輔助實現(xiàn)的,盡管氣壓高度表受氣壓和溫度變化的影響存在常值量測誤差,但其估計精度較水平通道單獨依靠INS的估計精度要好很多。因此,可以暫不考慮垂直通道的估計誤差,估計出導(dǎo)彈INS水平通道誤差;在此基礎(chǔ)上,對INS水平通道數(shù)據(jù)進行更新,忽略水平通道的估計誤差,再進行垂直通道誤差估計。

3.2 水平通道誤差估計

(11)

式中,X1(ti)=[δx(ti),δy(ti),δve(ti),δvn(ti)]T;

ti時刻,式(11)中的四個觀測分量方程是相互獨立的,因此H1(ti)可逆,于是可得:

(12)

這樣,就可以估計出ti時刻飛航導(dǎo)彈INS東、北向位置及速度誤差。但是,由于量測噪聲W1(ti)的影響,使得誤差估計精度不高,不能滿足飛航導(dǎo)彈INS誤差補償?shù)木纫蟆?/p>

考慮到受前視裝置視場的限制,導(dǎo)彈對地標(biāo)的可觀測時間很短,所以可以合理化地認為INS速度誤差在觀測過程中保持不變,即

δve(ti)≈δve,δvn(ti)≈δvn,δvu(ti)≈δvu

(13)

于是,由ti與tn時刻導(dǎo)彈的相對位置關(guān)系,可以得到如下約束方程:

δx(tn)≈δx(ti)+δve·(n-i)T

(14a)

δy(tn)≈δy(ti)+δvn·(n-i)T

(14b)

將式(14)代入式(11),可以將觀測過程中任意采樣時刻ti的量測方程都表示成以tn時刻INS水平通道誤差為狀態(tài)量的形式

(15)

式中,X1(tn)=[δx(tn),δy(tn),δve,δvn]T,

這樣,就可以利用平均去噪的思想,將n次觀測得到的量測方程求和處理:

(16)

由式(16)可得:

(17)

又量測噪聲可以認為是高斯白噪聲,即

(18)

于是可得:

(19)

(20a)

(20b)

(20c)

(20d)

其中,“-”表示更新前,后文不再贅述。

3.3 垂直通道誤差估計

令δx=δy=δve=δvn=0,代入式(10)可得垂直通道誤差估計的量測方程:

(21)

考慮到氣壓高度表量測誤差隨飛行高度近似線性變化,而飛航導(dǎo)彈中制導(dǎo)段多為平飛,且觀測時間很短,可以認為觀測過程中氣壓高度表量測誤差近似不變,即

δz(ti)≈δz

(22)

于是可得,

X2(ti)=X2=[δz,δvu]T

(23)

將n次觀測得到的量測方程求和處理可得

(24)

由式(18)和式(24)可得

(25)

于是,可得到新的高度和天向速度估計值:

(26a)

(26b)

這樣,經(jīng)過兩階段的誤差估計,由式(19)和式(25)得到了tn時刻飛航導(dǎo)彈INS誤差的估計量:

(27)

3.4 誤差補償

(28a)

(28b)

(29)

4 迭代算法

Step1:INS水平通道誤差估計;

Step2:令δz=0,δvu=0,S(k)=0;

Step6:水平通道數(shù)據(jù)更新,即式(20);

Step9:垂直通道誤差估計;

Step10:令δx=δy=δve=δvn=0;

Step14:水平通道數(shù)據(jù)更新,即式(26);

水平通道誤差估計是垂直通道誤差估計的基礎(chǔ),其估計精度將直接影響垂直通道誤差估計的效果,且INS水平通道初始誤差一般較大,因此,有必要對INS水平通道誤差估計執(zhí)行內(nèi)環(huán)迭代。而考慮到INS垂直通道的估計是由氣壓高度表輔助實現(xiàn)的,初始誤差較小,所以,收斂快,外環(huán)迭代足以滿足精度要求。

5 仿真分析

表1 tn時刻INS誤差估計結(jié)果

由于存在速度誤差,tn時刻飛航導(dǎo)彈東、北向位置估計誤差真值已經(jīng)分別從初始時刻的500m和400m發(fā)散到643.76m和519.80m,由圖3可以看出,tn時刻INS誤差量的估計值能夠有效收斂到真實值,驗證了方法的正確性。

表2 誤差補償前后INS估計精度對比

從仿真結(jié)果可以看到,誤差補償后,飛航導(dǎo)彈INS位置估計精度收斂到1m之內(nèi),速度估計精度收斂到0.5m/s之內(nèi),驗證了方法的有效性。

為了進一步驗證本文方法的可用性,仿真研究在不同初始誤差條件下誤差補償?shù)男Ч?。定義誤差補償后INS的位置估計誤差精度因子σ1和速度估計誤差精度因子σ2如下:

首先,其它條件不變,初始位置誤差在δx∈(-700,700),δy∈(-700,700)(單位m)區(qū)間內(nèi)變化,研究σ1、σ2隨初始位置誤差的變化規(guī)律,如圖4(a)、(b)所示。

然后,其它條件不變,初始速度誤差在δve∈(-15,15),δvn∈(-15,15)(單位m/s)區(qū)間內(nèi)變化,研究σ1、σ2隨初始速度誤差的變化規(guī)律,如圖4(c)、(d)所示。

可以看出,σ1和σ2對于初始誤差的變化表現(xiàn)出較強的魯棒性。盡管初始誤差不同,誤差補償后,飛航導(dǎo)彈INS估計精度都顯著提高,位置估計精度達到3m,速度估計精度達到0.5m,充分說明了方法的工程應(yīng)用可行性。

6 結(jié)語

本文以新型飛航導(dǎo)彈為應(yīng)用背景,在GPS等衛(wèi)星導(dǎo)航信息不可用的情況下,提出了基于地標(biāo)被動觀測的飛航導(dǎo)彈INS誤差估計及補償方法,該方法具有如下特點:

1) 以INS誤差作為狀態(tài)量,以導(dǎo)彈相對于地標(biāo)的觀測序列與INS估計信息解算的偽觀測序列構(gòu)造觀測量,建立量測方程。

2) 將INS水平通道和垂直通道誤差分兩個階段進行估計,采用迭代求解的方法,實現(xiàn)了三維位置和速度誤差的有效估計。

3) 利用平均去噪的思想提高了誤差估計精度。

4) 無需增加導(dǎo)彈硬件或要求導(dǎo)彈進行機動,符合飛航導(dǎo)彈中制導(dǎo)階段的飛行特點,易于工程實現(xiàn)。

即使將來GPS等衛(wèi)星導(dǎo)航信息可用,該方法仍然可以作為一種輔助導(dǎo)航手段,用于進一步增強組合導(dǎo)航系統(tǒng)的可靠性。

[1] Dr. Mikel, M. Miller. Navigation in GPS Denied Environments: Feature-Aided Inertial Systems[R]. Air Force Research Laboratory, RTO-EN-SET-116, 2010.

[2] Mosier Daniel, Sundareshan Malur K. A multiple model algorithm for passive ranging and air-to-air missile guidance[C]//Drummond Oliver E. Signal and Data Processing of Small Targets. San Diego, CA, USA: The International Society for Optical Engineering,2001:222-233.

[3] M. K. Kaiser. Vision-Based Estimation for Guidance, Navigtion and Control of an Aerial Vehicle[J]. IEEE Transactions on Aerospace and Electronic Systems,2010,46(3):1064-1077.

[4] Wu A D, Johnson E N, Proctor A A. Vision-Aided Inertial Navigation for Flight Control[J]. Journal of Aerospace Computing, Information and Communication,2005,2(9):348-360.

[5] 張志勇,張靖,朱大勇,等.一種基于視覺成像的快速收斂的位姿測量算法及實驗研究[J].航空學(xué)報,2007,28(4):943-947.

[6] 徐超,范耀祖,沈曉蓉,等.一種無人機視覺導(dǎo)航方法及其濾波算法改進[J].北京航空航天大學(xué)學(xué)報,2010,36(8):1000-1004.

[7] Yoko Watanabe. Stochastically Optimized Monocular Vision Based Navigation and Guidance[D]. Georgia: PHD Thesis, Georgia Institute of Technology,2008.

[8] 王小剛,郭繼峰,崔乃剛.基于數(shù)據(jù)鏈的智能導(dǎo)彈協(xié)同定位方法[J].中國慣性技術(shù)學(xué)報,2009,17(3):319-323.

[9] Hongliang Ren, Kazanzides, P. Investigation of Attitude Tracking Using an Integrated Inertial and Magnetic Navigation System for Hand-Held Surgical Instruments[J]. IEEE Transactions on Mechatronics,2012,17(2):210-217.

[10] Martinelli, A. Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale, and Bias Determination[J]. IEEE Transactions on Robotics,2012,28(1):44-60.

[11] Lupton, T, Sukkarieh, S. Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions[J]. IEEE Transactions on Robotics,2012,28(1):61-76.

[12] Jun Zhang, Weisong Liu, Yirong Wu. Novel Technique for Vision-Based UAV Navigation[J]. IEEE Transactions on Aerospace and Electrionics Systems,2011,47(4):2731-2741.

[13] Jonathan Kelly, Gaurav S. Sukhatme. Visual-Inertial Sensor Fusion: Localization, Mapping and Sensor-to-Sensor Self-Calibration[J]. International Journal of Robotics Research,2011,30(1):56-79.

[14] 明寶印,尹健,高士英,等.利用成像導(dǎo)引頭修正空地導(dǎo)彈慣導(dǎo)誤差研究[J].彈箭與制導(dǎo)學(xué)報,2006,26(2):717-719.

[15] 盧曉東,董鵬,周軍,等.飛航導(dǎo)彈地標(biāo)被動觀測自主導(dǎo)航修正技術(shù)[J].飛行力學(xué),2009,27(6):83-86.

[16] Wang J L, Garratt M, Lambert A, et al. Integration of GPS/INS/Vision sensors to navigate unmanned aerial vehicle[C]//International Archives of the of Photogrammetry, Remote Sensing and Spatial Information Sciences,2008:963-969.

[17] 于起峰,李強,雷志輝,等.基于序列圖像的無人機自測速方法與試驗[J].航空學(xué)報,2009,30(8):1503-1507.

INS Error Estimation and Compensation Method Based on Passive Observation to Feature Target for Cruise Missile

FAN Shaoli1WANG Lei1,2LU Weiwei1

(1. Naval Aeronautical Engineering Academy, Yantai 264001)(2. No. 91515 Troops of PLA, Sanya 572016)

In absence of the other navigation system, cruise missile INS error will be cumulated continuously with the increase of endurance. Therefore, this article proposes a novel practical INS error estimation and compensation method based on continuous observation to feature target. With the observation sequence and pseudo observation sequence, the residual error sequence is constructed. And the error estimation equation is established by treating the residual error sequence as the observation. Then the INS level channel error and vertical channel error are estimated respectively by iterative algorithm. Means de-noising approach is used to improve the estimation accuracy. The INS error compensation method is a real time method. The simulation result shows that this method can reduce the INS position error to 3m and velocity error to 0.5m. So it could be applied to cruise missile autonomous navigation. Even if other navigation systems are available, this method could also be used to increase the reliability of the integrated navigation system.

vision navigation, INS error estimation, iterative algorithm, cruise missile, observation sequence, means de-noising approach

2015年3月7日,

2015年4月27日

范紹里,男,副教授,研究方向:飛行器測試與控制。王磊,男,碩士研究生,工程師,研究方向:導(dǎo)航制導(dǎo)與控制。陸巍巍,男,助教,研究方向:裝備綜合保障。

E927

10.3969/j.issn.1672-9730.2015.09.012