一種基于速度加比力匹配的傳遞對(duì)準(zhǔn)時(shí)間延遲補(bǔ)償算法

高偉，郭宇，程正生

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

一種基于速度加比力匹配的傳遞對(duì)準(zhǔn)時(shí)間延遲補(bǔ)償算法

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

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

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

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

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

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

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

1 速度加比力匹配傳遞對(duì)準(zhǔn)模型

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

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

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

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

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

2）速度誤差方程：

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

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

用于計(jì)算有害加速度的實(shí)際角速度為

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

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

1.2 卡爾曼濾波模型

1.2.1 狀態(tài)方程

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

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

選取狀態(tài)量為

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

1.2.2 量測(cè)方程

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

取觀測(cè)量為

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

同理：

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

2 仿真分析

2.1 仿真條件

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

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

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

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

2.2 仿真分析

結(jié)合艦船典型運(yùn)動(dòng)方式，仿真分析傳遞對(duì)準(zhǔn)過(guò)程中艦船分別做勻速直線運(yùn)動(dòng)、勻加速直線運(yùn)動(dòng)、“S”形機(jī)動(dòng)運(yùn)動(dòng)，基準(zhǔn)信息時(shí)間延遲對(duì)“速度加比力”匹配對(duì)準(zhǔn)性能的影響和補(bǔ)償后的對(duì)準(zhǔn)性能。

2.2.1 機(jī)動(dòng)方式1——?jiǎng)蛩僦本€運(yùn)動(dòng)

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

圖1 勻速直線運(yùn)動(dòng)狀態(tài)傳遞對(duì)準(zhǔn)效果

2.2.2 機(jī)動(dòng)方式2——?jiǎng)蚣铀僦本€運(yùn)動(dòng)

載體以5 m／s的速度沿45°航向，從10 s開(kāi)始以0.2 m／s的加速度做加速直線運(yùn)動(dòng)，加速時(shí)間為100 s，加速結(jié)束后做勻速直線運(yùn)動(dòng)。

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

圖2 勻加速直線運(yùn)動(dòng)狀態(tài)傳遞對(duì)準(zhǔn)效果

2.2.3 機(jī)動(dòng)方式3——“S”形機(jī)動(dòng)運(yùn)動(dòng)

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

圖3 “S”形機(jī)動(dòng)路線俯視圖

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

圖4 “S”形機(jī)動(dòng)傳遞對(duì)準(zhǔn)效果

3 結(jié)束語(yǔ)

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

［1］陳剛，劉紅光.機(jī)動(dòng)對(duì)速度匹配方法傳遞對(duì)準(zhǔn)效果的影響［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，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)傳遞對(duì)準(zhǔn)中時(shí)間補(bǔ)償算法［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］付夢(mèng)印，鄧志紅，張繼偉.Kalman濾波理論及其在導(dǎo)航中的應(yīng)用［M］.北京：科學(xué)出版社，2003：35-36.

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

［9］侯永利，萬(wàn)彥輝.慣導(dǎo)系統(tǒng)傳遞對(duì)準(zhǔn)技術(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.

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

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

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

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

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