高偉，郭宇，程正生

1.哈爾濱工程大學(xué)自動化學(xué)院，黑龍江哈爾濱 150001 2.中船重工第707研究所，江西九江 332000

一種基于速度加比力匹配的傳遞對準時間延遲補償算法

高偉1，郭宇1，程正生2

1.哈爾濱工程大學(xué)自動化學(xué)院，黑龍江哈爾濱 150001 2.中船重工第707研究所，江西九江 332000

傳遞對準過程中，由于主慣導(dǎo)的傳輸延遲，子慣導(dǎo)解算信息與進行匹配的基準信息不能完全同步，有些情況下時間延遲影響較大。分析了時間延遲對速度加比力匹配傳遞對準算法的影響，并用擴增狀態(tài)量的方法對該算法時間延遲進行建模和補償。仿真結(jié)果表明，對準過程中當艦船處于勻速運動狀態(tài)，時間延遲幾乎不會影響對準性能；當艦船進行加速運動或S形機動運動時，時間延遲會嚴重影響對準性能，通過補償可以有效地減少時間延遲對于該算法的影響。

速度加比力匹配傳遞對準；時間延遲；慣導(dǎo)系統(tǒng)；卡爾曼濾波；補償算法

傳遞對準是在動態(tài)條件下，通過對載體主慣導(dǎo)（已經(jīng)對準）的輸出信息與安裝在載體上的子慣導(dǎo)（未對準）的輸出信息進行匹配，估算出子慣導(dǎo)的失準角，從而完成子慣導(dǎo)初始對準的一種方法［1-3］。

速度加比力匹配是艦船在低機動要求下針對平臺慣導(dǎo)系統(tǒng)常用的匹配方式，它通過比較主子慣導(dǎo)間的速度和比力輸出，利用卡爾曼濾波來估計子慣導(dǎo)誤差。該算法有對準時間短，對機動要求低，對準精度不受船體變形影響等優(yōu)點。對于平臺慣導(dǎo)系統(tǒng)，傳遞對準濾波算法的實現(xiàn)是在子慣導(dǎo)系統(tǒng)中進行的，需要將主慣導(dǎo)系統(tǒng)的一些相關(guān)信息傳輸?shù)阶討T導(dǎo)系統(tǒng)。由于時間延遲，子慣導(dǎo)接收到的導(dǎo)航信息跟主慣導(dǎo)傳遞時的導(dǎo)航信息存在一定誤差，若子慣導(dǎo)直接以接收到的信息作為基準進行傳遞對準，就會影響平臺誤差角的估計精度［4-6］。

文章采用平臺慣導(dǎo)速度加比力匹配傳遞對準，深入分析了艦船幾種典型運動狀態(tài)下，時間延遲對傳遞對準性能的影響，并利用擴增狀態(tài)量的方法對時間延遲進行補償。

1 速度加比力匹配傳遞對準模型

1.1 平臺慣導(dǎo)誤差分析

設(shè)平臺式慣導(dǎo)系統(tǒng)的平臺要求模擬的導(dǎo)航坐標系n為地理坐標系，實際建立的平臺坐標系為n′。由于計算誤差、誤差源影響及施矩誤差，n′系相對n系有偏差角φn。1）姿態(tài)誤差方程：

設(shè)陀螺的漂移為ε，刻度系數(shù)誤差陣為

式中ΔKGx、ΔKGy、ΔKGz為x、y、z軸的刻度系數(shù)誤差。所以平臺的實際角速度為

將式（2）帶入式（1），得

2）速度誤差方程：

式中：fn為比力在地理系上的投影，ω為慣性坐標系相對于地球坐標系的角速度在地理坐標系上的投影，ω為地球坐標系相對于地理坐標系在地理坐標系上的投影，v為載體相對于地球的速度，gn是重力加速度在地理系上的投影。

考慮加速度計測量誤差為?，刻度系數(shù)誤差陣為ΔKG＝diag（ΔKGx，ΔKGy，ΔKGz），并且考慮到實際平臺n′具有姿態(tài)誤差角φn，則實際加速度計的輸出比力為

用于計算有害加速度的實際角速度為

用于比力輸出和補償有害加速度的計算都有誤差，所以按力學(xué)編排方程確定的速度也有誤差，設(shè)速度誤差為ΔVne，則

將式（4）代入式（5）得到速度誤差方程：

1.2 卡爾曼濾波模型

1.2.1 狀態(tài)方程

濾波器狀態(tài)方程是建立在子慣導(dǎo)基礎(chǔ)上的，利用平臺慣導(dǎo)誤差方程，建立狀態(tài)方程為

式中：X為狀態(tài)變量，W為系統(tǒng)噪聲矩陣。

選取狀態(tài)量為

式中：φx、φy、φz是平臺誤差角，Δvx、Δvy是速度誤差，εx、εy、εz是陀螺漂移，?x、?y是加速度計零偏。整理平臺誤差方程得到狀態(tài)方程為

1.2.2 量測方程

采用主子慣導(dǎo)的速度和比力測量，量測方程為

取觀測量為

子慣導(dǎo)系統(tǒng)（SINS）在t時刻獲得的主慣導(dǎo)的速度和比力是主慣導(dǎo)在t－Δt時刻的數(shù)據(jù)。主慣導(dǎo)解算的速度差和比力差表示如下：

同理：

由于Δt選取的時間很短，默認為在延遲時間Δt內(nèi)，速度的變化率和比力的變化率在計算的周期內(nèi)近似不變，即

2 仿真分析

2.1 仿真條件

1）假設(shè)主慣導(dǎo)無誤差，桿臂效應(yīng)誤差已得到充分補償。

2）載體初始位置：經(jīng)度117°，緯度39°。載體初始姿態(tài)角（縱搖、橫搖、航向）分別為：0°、0°、0°；子慣導(dǎo)平臺初始誤差角為φx＝5′，φy＝5′，φz＝5′。

3）子慣導(dǎo)陀螺儀常值漂移均為0.01（°）／h，隨機漂移噪聲均為0.001（°）／h；加速度計常值零偏均為1×10－4g，隨機零偏噪聲均為1×10－5g，其中g(shù)為重力加速度。

4）狀態(tài)估計初始值均為0；初始方差陣P0參數(shù)根據(jù)上述慣性器件誤差進行設(shè)置，保證卡爾曼濾估計的最優(yōu)性。

2.2 仿真分析

結(jié)合艦船典型運動方式，仿真分析傳遞對準過程中艦船分別做勻速直線運動、勻加速直線運動、“S”形機動運動，基準信息時間延遲對“速度加比力”匹配對準性能的影響和補償后的對準性能。

2.2.1 機動方式1——勻速直線運動

載體以5 m／s的速度沿45°航向勻速直線運動。仿真結(jié)果如圖1所示。圖中實線條為時間延遲未補償?shù)膶市Ч?，點劃線為時間延遲補償后的對準效果。從仿真圖看，艦船勻速直航時，基準信息延遲對傳遞對準性能幾乎無影響。

圖1 勻速直線運動狀態(tài)傳遞對準效果

2.2.2 機動方式2——勻加速直線運動

載體以5 m／s的速度沿45°航向，從10 s開始以0.2 m／s的加速度做加速直線運動，加速時間為100 s，加速結(jié)束后做勻速直線運動。

仿真結(jié)果如圖2所示。從仿真圖看，時間延遲對于勻加速直線運動狀態(tài)的傳遞對準效果有明顯影響，在加速狀態(tài)下傳遞對準很難收斂，當艦船重新進入勻速狀態(tài)后，影響會慢慢變小。對時間延遲進行補償后可以看出，水平誤差角和方位誤差角可以很快收斂。

圖2 勻加速直線運動狀態(tài)傳遞對準效果

2.2.3 機動方式3——“S”形機動運動

載體機動方式為“S”形機動，最初以10 m／s的速度勻速直線運動，S形機動轉(zhuǎn)彎角速度為1（°）／h?？偟姆抡鏁r間為500 s，機動歷時200 s，航行曲線如圖3所示。

圖3 “S”形機動路線俯視圖

仿真結(jié)果如圖4所示。從仿真圖看，在“S”形機動狀態(tài)下，時間延遲對“速度＋比力”匹配影響較大，水平方位角可以估出，誤差在0.5′，進入勻速狀態(tài)后誤差慢慢變小，方位誤差角在做“S”形機動下無法估計出。對時間延遲進行補償后，水平誤差角和方位誤差角可以快速收斂。

圖4 “S”形機動傳遞對準效果

3 結(jié)束語

傳遞對準過程中艦船進行轉(zhuǎn)彎或機動運動時，基準信息延遲會導(dǎo)致平臺誤差角的估計誤差變大，使傳遞對準性能下降。本文通過擴增狀態(tài)量的方法，對速度加比力匹配傳遞對準的時間延遲進行了補償，并以艦船的3種典型運動方式進行了仿真。仿真結(jié)果表明該算法有效地減小了時間延遲對于速度加比力匹配傳遞對準的影響，驗證了本算法的有效性。

［1］陳剛，劉紅光.機動對速度匹配方法傳遞對準效果的影響［J］.中國慣性技術(shù)學(xué)報，2010，18（6）：653-659.

［2］CHEN Kai，ZHAO Gang，MENG Zhongjie，et al.Equiva-lent approaches to equations of traditional transfer alignment and rapid transfer alignment［J］.World Congress on Intel-lignent Control and Automation，2008，6：25-27.

［3］ZHANG Ludong，LIAN junxiang，WU Meiping，et al.Re-search on auto compensation technique of strapdown inertial navigation systems［C］／／International Asia Conference on Informatics in Control，Automation and Robotics.Bangkok，Thailand，2009：350-359.

［4］STANCIC R，GRAOVAC S.The integration of strap-down INS and GPS based on adaptive error damping［J］.Robotics and Autonomous Systems，2010，58（10）：1117-1129.

［5］徐翔.捷聯(lián)慣導(dǎo)系統(tǒng)傳遞對準中時間補償算法［J］.魚類技術(shù)，2011（4）：282-284.

［6］LIM Y C，LYOU J.An error compensation method for trans-fer alignment［C］／／Proceedings of IEEE Region 10 Inter-national Conference on Electrical and Electronic Technology.Singapore，2001：850-855.

［7］付夢印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航中的應(yīng)用［M］.北京：科學(xué)出版社，2003：35-36.

［8］韓海軍，陳家斌.艦船慣導(dǎo)系統(tǒng)傳遞對準技術(shù)［J］.火力與指揮控制，2004，29（6）：6-9.

［9］侯永利，萬彥輝.慣導(dǎo)系統(tǒng)傳遞對準技術(shù)綜述［J］.導(dǎo)航與控制，2005，4（2）：65-68.

［10］陳哲.捷聯(lián)慣導(dǎo)系統(tǒng)原理［M］.北京：宇航出版社，1986：1-9.

A time-delay compensation algorithm for velocity and force transfer alignment

GAO Wei1，GUO Yu1，CHENG Zhengsheng2
1.College of Automation，Harbin Engineering University，Harbin 150001，China
2.707 Research Institute of China Shipbuilding Industry Corporation，Jiujiang 332000，China

Because of the transfer time delay of master inertial navigation system（INS）during transfer alignment，the information computed by secondary inertial navigation system（SINS）does not synchronize completely with the basic matching information，and sometimes the effect of time-delay is relatively large.This paper analyses the time-delay effect on the velocity and force matching transfer alignment，and uses an extended state variable method to model and compensate the time-delay.The simulation results show that，in the case of constant velocity in align-ment，the time-delay’s effect on velocity and force matching transfer alignment can be ignored.In the case of varia-ble acceleration and swerves in alignment，the time-delay could affect the accuracy of alignment seriously，and through compensation the proposed algorithm can reduce the time-delay effect on the algorithm efficiently.

matching transfer alignment；time-delay；inertial navigation system；Kalman filter；compensation algo-rithm

U666.1

A

1009-671X（2014）05-053-56

10.3969／j.issn.1009-671X.201309017

2013-09-27.

日期：2014-09-24.

國家自然科學(xué)基金資助項目（51379042）.

高偉（1977-），男，教授，博士生導(dǎo)師；

郭宇（1989-），男，碩士研究生.

郭宇，E-mail：515040035＠qq.com.

http：／／www.cnki.net／kcms／doi／10.3969／j.issn.1009-671X.201309017.html