王偉鵬 楊 博 李 方

1．北京航空航天大學(xué)宇航學(xué)院，北京 1001912.空軍工程大學(xué)工程學(xué)院，西安 710038

?

基于角速率和速度增量的捷聯(lián)慣性導(dǎo)航算法研究

王偉鵬1楊 博1李 方2

1．北京航空航天大學(xué)宇航學(xué)院，北京 1001912.空軍工程大學(xué)工程學(xué)院，西安 710038

捷聯(lián)慣性導(dǎo)航算法一般是基于慣性器件輸出為角速率、比力或角增量、速度增量進(jìn)行設(shè)計(jì)的，不能直接應(yīng)用于陀螺輸出為角速率、加速度計(jì)輸出為速度增量的捷聯(lián)慣性導(dǎo)航系統(tǒng)。為了解決此問題和滿足精度要求，重新設(shè)計(jì)了一套捷聯(lián)慣性導(dǎo)航算法：姿態(tài)更新算法采用了經(jīng)典的四階龍格-庫塔法，推導(dǎo)出了一種新的速度更新算法，該算法可以有效補(bǔ)償速度計(jì)算中的劃槳效應(yīng)誤差。仿真結(jié)果表明，該種速度更新算法仿真速度快、精度高，具有一定的工程實(shí)用價(jià)值。

角速率；速度增量；四階龍格-庫塔法；劃槳效應(yīng)

捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strap-down Inertial Navigation System，簡稱SINS)廣泛應(yīng)用在飛機(jī)、艦船、坦克、導(dǎo)彈等海、陸、空、天慣性導(dǎo)航與制導(dǎo)領(lǐng)域。為了提高系統(tǒng)的精度，滿足各種載體的導(dǎo)航與制導(dǎo)要求，出現(xiàn)了很多捷聯(lián)算法研究[1-6]。

由于陀螺儀輸出有角速率和角增量兩種方式，如微機(jī)械陀螺輸出為角速率或角增量方式、激光陀螺輸出為角增量方式、光纖陀螺輸出為角速率方式等；加速度計(jì)輸出有比力和速度增量兩種方式。因此，SINS的解算有以下組合方式[7]： 1)角速率和比力； 2)角增量和速度增量； 3)角增量和比力； 4)角速率和速度增量。在實(shí)際慣性導(dǎo)航系統(tǒng)中，因工作環(huán)境、技術(shù)指標(biāo)及成本等方面的限制，需要選擇不同等級(jí)的慣性器件，使得常規(guī)的角速率與比力、角增量與速度增量輸出方式為主的SINS導(dǎo)航算法不能滿足實(shí)際系統(tǒng)的設(shè)計(jì)需求。因此，有必要研究其它不同輸出方式的SINS算法，以適應(yīng)慣性器件多元化的發(fā)展[7]。對(duì)于慣性器件輸出為角速率和速度增量組合方式的SINS算法，目前尚未有文獻(xiàn)對(duì)其報(bào)道，若通過角速率提取角增量后再利用基于增量信號(hào)SINS算法解算會(huì)帶來較大誤差。本文針對(duì)這種組合方式的SINS算法進(jìn)行了速度補(bǔ)償算法方面的研究，鑒于角速率信號(hào)能直接連續(xù)得到，本文從算法精度和計(jì)算量方面綜合考慮，選用四階龍格-庫塔法[8]進(jìn)行姿態(tài)更新，詳細(xì)推導(dǎo)出了一種新型速度更新算法，該算法可以補(bǔ)償速度解算中的劃槳效應(yīng)[9]，并進(jìn)行了仿真驗(yàn)證，為后續(xù)工程化實(shí)現(xiàn)提供了一定的理論依據(jù)。

1 新型SINS算法

設(shè)i為慣性坐標(biāo)系，e為地球固聯(lián)坐標(biāo)系，n為導(dǎo)航坐標(biāo)系(文中取東北天地理坐標(biāo)系)，b為載體坐標(biāo)系，T為系統(tǒng)采樣周期。

1.1 姿態(tài)更新

通常姿態(tài)更新算法有龍格-庫塔法(Range-Kutta)和旋轉(zhuǎn)矢量法[5]。龍格-庫塔法直接利用角速率計(jì)算載體的姿態(tài)四元數(shù)；旋轉(zhuǎn)矢量法先利用角增量計(jì)算旋轉(zhuǎn)矢量, 再通過旋轉(zhuǎn)矢量計(jì)算載體的姿態(tài)四元數(shù)。因此，對(duì)陀螺儀以角速率方式輸出的指北方位捷聯(lián)慣導(dǎo)系統(tǒng)，姿態(tài)更新算法采用龍格-庫塔法計(jì)算載體的姿態(tài)四元數(shù)較為合理。

設(shè)q為四元數(shù)，則四元數(shù)微分方程為[7]

(1)

(2)

1.2 速度更新和補(bǔ)償

取地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，速度方程[9-10]為

(3)

在[tk,tk+1]對(duì)上式進(jìn)行積分，得tk+1時(shí)刻載體在導(dǎo)航坐標(biāo)系內(nèi)的速度為

(4)

(5)

(6)

式中，Δθ(t)為角增量，ΔV(t)為速度增量，ΔVrotk為速度的旋轉(zhuǎn)效應(yīng)補(bǔ)償項(xiàng)，該項(xiàng)由運(yùn)動(dòng)載體的線運(yùn)動(dòng)方向在空間旋轉(zhuǎn)引起；ΔVsculk為速度的劃槳效應(yīng)補(bǔ)償項(xiàng)，該項(xiàng)由運(yùn)動(dòng)載體沿縱軸作線振動(dòng)和沿橫軸作同頻同相的角振動(dòng)引起。對(duì)于以角速率和速度增量的方式為輸入的捷聯(lián)慣導(dǎo)系統(tǒng)，速度計(jì)算時(shí)必須考慮旋轉(zhuǎn)效應(yīng)和劃槳效應(yīng)補(bǔ)償項(xiàng)，則不能直接應(yīng)用式(6)，需重新推導(dǎo)速度的劃槳效應(yīng)補(bǔ)償項(xiàng)。

(7)

其中，擬合系數(shù)分別為a，b，c，A，B，C。

在t∈[tk,tk+1]內(nèi)，對(duì)(7)式積分則

(8)

(9)

在ΔT，2ΔT和3ΔT3個(gè)時(shí)間段內(nèi)，速度增量的三子樣采樣值分別為

=AΔT+3BΔT2+7CΔT3

=AΔT+5BΔT2+19CΔT3

(10)

聯(lián)立式(9)和(10)，求解得擬合系數(shù)

(11)

將式(7)和(8)分別代入(6)式計(jì)算得

(12)

(13)

式中ωk(1)、ωk(2)和ωk(3)分別為tk+ΔT、tk+2ΔT和tk+13時(shí)刻的角速率采樣值；Vk(1)、Vk(2)和Vk(3)分別為[tk,tk+1]內(nèi)速度增量的3個(gè)等間隔采樣值。

2 仿真驗(yàn)證

整個(gè)系統(tǒng)仿真分為兩步進(jìn)行，首先設(shè)計(jì)軌跡發(fā)生器[11]模擬出一條載體的航跡，除了輸出載體的姿態(tài)、速度和位置等導(dǎo)航參數(shù)，還要輸出陀螺儀和加速度計(jì)的采樣值，作為SINS系統(tǒng)解算的輸入值。然后以陀螺的角速率、加速度計(jì)的速度增量作為仿真輸入，將系統(tǒng)仿真的結(jié)果和模擬出的載體航跡相比較，檢驗(yàn)算法的正確性和補(bǔ)償效果。

假設(shè)載體初始位置為北緯34°14.763′，東經(jīng)108°54.579′，高度380m，初始速度為0，初始俯仰角為90°，航向角、橫滾角為隨機(jī)角度。在1500s的仿真時(shí)間內(nèi)，前126s為上升階段，第126s～129s為退出爬升、改平階段，第129s～900s為勻速直線平飛階段(巡航段)，巡航段飛行速度(沿縱向)為4.0Ma。

姿態(tài)誤差、速度誤差仿真結(jié)果分別如圖1和圖2所示。從仿真結(jié)果中可以看出，在導(dǎo)航飛行全過程中由于算法導(dǎo)致的姿態(tài)誤差在0.001角分量級(jí)，速度誤差在0.01m/s量級(jí)。

圖1 姿態(tài)誤差

圖2 速度誤差

3 結(jié)論

本文提出了一種慣性器件輸出以角速率和速度增量為組合方式的SINS算法，采用經(jīng)典的四階龍格-庫塔法進(jìn)行姿態(tài)更新，并推導(dǎo)出了一種新的速度更新算法，有效補(bǔ)償速度計(jì)算中的劃槳效應(yīng)誤差項(xiàng)。完成了對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)的數(shù)字全仿真，從軌跡發(fā)生器、捷聯(lián)解算等方面很好的模擬了捷聯(lián)慣導(dǎo)系統(tǒng)的工作過程。

仿真結(jié)果表明, 文中采用的導(dǎo)航算法可行，具有較好的精度，滿足實(shí)際系統(tǒng)的要求。

[1] Brotz J E.A New Mathematical Formulation for Strapdown Inertial Navigation [J].IEEE Trans.on Aerospace and Electronic System, 1971,7(1):61-66.

[2] Paul G.Savage.Strapdown Inertial Navigation Integration Algorithm Design part1: Attitude Algorithms[J].Journal of Guidance, Control, and Dynamics,1998,21(1):19-28.

[3] Paul G.Savage.Strapdown Inertial Navigation Integration Algorithm Design part2: Velocity and Position Algorithms[J].Journal of Guidance, Control, and Dynamics,1998,21(2):208-229.

[4] Paul G.Savage.A Unified Mathematical Framework for Strapdown Algorithm Design [J].Journal of Guidance, Control, and Dynamics,2006,29(2):237-249.

[5] 劉危，解旭輝，李圣怡.捷聯(lián)慣導(dǎo)系統(tǒng)的圓錐誤差補(bǔ)償算法研究[J].南京航空航天大學(xué)學(xué)報(bào)，2004，36(3)：348-352.(LIU Wei,XIE Xu-hui,LI Sheng-yi.Coning Algorithm for Strap-Down Inertial Navigation System[J].Journal of Nanjing University of Aeronautics & Astronautics,2004,36(3):348-352.)

[6] 耿延睿，王養(yǎng)柱，崔中興.捷聯(lián)慣導(dǎo)系統(tǒng)劃船效應(yīng)補(bǔ)償算法研究[J].北京航空航天大學(xué)學(xué)報(bào)，2001，27(6)：694-697.(GENG Yan-rui,WANG Yang-zhu,CUI Zhong-xing.Optimal Design for Sculling algorithms of SINS[J].Journal of Beijing University of Aeronautics and Astronautics,2001,27(6):694-697.)

[7] 秦永元.國際慣性器件發(fā)展現(xiàn)狀和趨勢[J].航空制造技術(shù)，2008，(9)：68-69.(QIN Yong-yuan.Current Status and Development Trend of International Inertial Instrument[J].Aeronautical Manufacturing Technology,2008,(9):68-69.)

[8] 王蘊(yùn)會(huì)，趙忠.捷聯(lián)慣導(dǎo)算法設(shè)計(jì)和仿真[D].西安：西北工業(yè)大學(xué)， 2009.

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

[10] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2006.

[11] 張 磊，汪 渤.高精度游移方位捷聯(lián)慣導(dǎo)系統(tǒng)的數(shù)字仿真[J].微計(jì)算機(jī)信息(測控自動(dòng)化)，2008，22:238-239.(ZHANG Lei,WANG Bo.Digital Simulation of Highly Accurate Free-Orientation SINS[J].Control& Automation,2008,22:238-239.)

[12] 張宗麟.慣性導(dǎo)航與組合導(dǎo)航[M].北京：航空工業(yè)出版社，2000.

[13] 顏慶津.數(shù)值分析(第三版)[M].北京：北京航空航天大學(xué)出版社，2006.

The New Algorithm for Strap-down Inertial Navigation System

WANG Weipeng1YANG Bo1LI Fang2

1.School of Astronautics,Beijing University of Aeronautics and Astronautics,Beijing 100191,China2.Engineering College,Air Force Engineering University,Xi’an 710038,China

Theangularrate,specificforceorangularincrementandvelocityincrementareoftenusedinthealgorithmsofstrap-downinertialsystem.Thealgorithmcannotbeappliedtostrap-downinertialsystemwhoseoutputsignalsareangularrateandvelocityincrement.Inordertosolvetheproblemandmeetprecisionrequirements,asetofnewnavigationalgorithmsaredeveloped.Thefour-orderRange-Kuttamethodisadoptedinattitudeupdate.Anewvelocityupdatealgorithmisdeducedwhichcancompensatescullingeffect.Theobtainedsimulationresultsshowtheeffectivenessofthisapproach.

Angularvelocity;Velocityincrement;Four-orderRange-Kuttamethod;Scullingeffect

2011-12-01

王偉鵬(1982-)，男，山西汾陽人，碩士研究生，主要研究方向?yàn)閷?dǎo)航制導(dǎo)與控制；楊 博(1963-)，女，北京人，副教授，主要研究方向?yàn)閷?dǎo)航制導(dǎo)與控制；李 方(1982-)，男，西安人，講師，主要研究方向?yàn)槲淦飨到y(tǒng)和制導(dǎo)控制。

V324.2+3

A

1006-3242(2012)03-0003-04