王偉鵬 楊 博 李 方

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

?

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

王偉鵬1楊 博1李 方2

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

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

角速率；速度增量；四階龍格-庫塔法；劃槳效應

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

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

1 新型SINS算法

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

1.1 姿態(tài)更新

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

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

(1)

(2)

1.2 速度更新和補償

取地理坐標系為導航坐標系，速度方程[9-10]為

(3)

在[tk,tk+1]對上式進行積分，得tk+1時刻載體在導航坐標系內(nèi)的速度為

(4)

(5)

(6)

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

(7)

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

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

(8)

(9)

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

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

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

(10)

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

(11)

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

(12)

(13)

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

2 仿真驗證

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

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

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

圖1 姿態(tài)誤差

圖2 速度誤差

3 結(jié)論

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

仿真結(jié)果表明, 文中采用的導航算法可行，具有較好的精度，滿足實際系統(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)慣導系統(tǒng)的圓錐誤差補償算法研究[J].南京航空航天大學學報，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)慣導系統(tǒng)劃船效應補償算法研究[J].北京航空航天大學學報，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].航空制造技術，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] 王蘊會，趙忠.捷聯(lián)慣導算法設計和仿真[D].西安：西北工業(yè)大學， 2009.

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

[10] 秦永元.慣性導航[M].北京：科學出版社，2006.

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

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

[13] 顏慶津.數(shù)值分析(第三版)[M].北京：北京航空航天大學出版社，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-)，男，山西汾陽人，碩士研究生，主要研究方向為導航制導與控制；楊 博(1963-)，女，北京人，副教授，主要研究方向為導航制導與控制；李 方(1982-)，男，西安人，講師，主要研究方向為武器系統(tǒng)和制導控制。

V324.2+3

A

1006-3242(2012)03-0003-04