摘 要:低成本捷聯(lián)慣導(dǎo)系統(tǒng)的研制和開(kāi)發(fā)已成為導(dǎo)航領(lǐng)域的主要發(fā)展趨勢(shì)之一，在選定了低成本的慣性器件后，系統(tǒng)預(yù)期的性能指標(biāo)和最后樣機(jī)的性能測(cè)試結(jié)果之間往往存在著一定的差距，如何降低開(kāi)發(fā)成本并縮短開(kāi)發(fā)時(shí)間，就成了低成本捷聯(lián)慣導(dǎo)系統(tǒng)研制中的一個(gè)亟待解決的問(wèn)題。這里基于Matlab設(shè)計(jì)了一個(gè)仿真系統(tǒng)，將捷聯(lián)慣導(dǎo)的軟件和硬件模塊化，該系統(tǒng)不僅可以完成對(duì)系統(tǒng)性能的預(yù)測(cè)和評(píng)估，也可完成對(duì)算法和器件選擇的優(yōu)化。

關(guān)鍵詞:Matlab; 陀螺; 捷聯(lián)慣導(dǎo)系統(tǒng); 仿真系統(tǒng)

中圖分類(lèi)號(hào):TP274 文獻(xiàn)標(biāo)識(shí)碼:A

文章編號(hào):1004-373X(2010)12-0078-03

Simulation System of Low Cost Strapdown Inertial Navigation System Based on Matlab

LIU Zhi-ping， SHAO Xiao-bing

(College of Computer， Xi’an Technological University， Xi’an 710032，China)

Abstract:The research and development of low cost strapdown inertial navigation system has been the tendency in the field of navigation. The discrepancy between the expected performance and the test data was inevitable with the usage of inertial sensors. How to reduce the cost and shorten the development period is a hot issue in the development of low cost strapdown inertial navigation system. A simulation system based on MATLAB was designed and constructed， the software and hardware of strapdown inertial navigation were modularized， the optimization of various algorithms and selection of sensors should be more convenient and feasible.

Keywords:Matlab; gyro; strapdown inertial navigation; simulation system

0 引 言

捷聯(lián)慣導(dǎo)系統(tǒng)由于體積小、成本低、可靠性好而大量應(yīng)用在航空、航天、航海、兵器等領(lǐng)域[1-3]。它繼承了慣性導(dǎo)航系統(tǒng)的自主性、隱蔽性、實(shí)時(shí)性和無(wú)輻射的優(yōu)點(diǎn)，隨著慣性器件技術(shù)的不斷發(fā)展，激光陀螺、光纖陀螺、硅微陀螺及硅微加速度計(jì)的出現(xiàn)，捷聯(lián)慣導(dǎo)系統(tǒng)的組成形式越來(lái)越多。但無(wú)一例外的是，捷聯(lián)慣導(dǎo)朝著高精度、低成本、高可靠性的方向發(fā)展，其應(yīng)用領(lǐng)域早已超出了軍事應(yīng)用的范疇，在民用的車(chē)輛和船舶、礦山的開(kāi)采、海洋打撈上都可以看到捷聯(lián)慣導(dǎo)的應(yīng)用例證[4-10]。

捷聯(lián)慣導(dǎo)系統(tǒng)和其他任何產(chǎn)品一樣，都存在著研制和開(kāi)發(fā)、樣機(jī)制作、修改、定型、投產(chǎn)的過(guò)程，為了減少慣導(dǎo)產(chǎn)品研發(fā)的費(fèi)用并縮短研發(fā)的時(shí)間，人們一直在尋找一種簡(jiǎn)單可行的方法，以盡可能避免在方案論證階段和樣機(jī)研制階段的循環(huán)往復(fù)，計(jì)算機(jī)仿真就是經(jīng)常采用的方式之一。本文正是在這一技術(shù)需求的前提下，利用Matlab的強(qiáng)大仿真功能，設(shè)計(jì)了捷聯(lián)慣導(dǎo)系統(tǒng)的仿真系統(tǒng)。該系統(tǒng)包括3大模塊:靜態(tài)誤差補(bǔ)償模塊;導(dǎo)航解算模塊(包括姿態(tài)、速度、位置的解算算法、動(dòng)態(tài)誤差補(bǔ)償);輸出數(shù)據(jù)的動(dòng)態(tài)演示模塊。

1 靜態(tài)誤差補(bǔ)償

所謂的靜態(tài)誤差補(bǔ)償就是指陀螺和加速度計(jì)的各種誤差標(biāo)定系數(shù)的確定。陀螺的誤差標(biāo)定系數(shù)一般包括安裝偏差角、力矩器刻度系數(shù)、陀螺的零漂、陀螺隨外界角振動(dòng)和線(xiàn)振動(dòng)引起的漂移、陀螺隨外界磁場(chǎng)的變化引起的漂移、陀螺隨溫度變化引起的漂移。加速度計(jì)的誤差標(biāo)定系數(shù)包括安裝偏差角、力矩器刻度系數(shù)、加速度計(jì)的零偏、加速度計(jì)隨外界的磁場(chǎng)、振動(dòng)、溫度的變化而產(chǎn)生的漂移。在這些誤差通過(guò)誤差模型被標(biāo)定后，還需要考慮加速度計(jì)和陀螺之間的耦合影響。因此，在第一部分，主要完成各種靜態(tài)誤差的誤差建模和Simulink建模，亦即獎(jiǎng)?wù)`差的數(shù)學(xué)模型變?yōu)镸atlab下的Simulink模型。陀螺的靜態(tài)誤差的Simulink模型中所包含的參數(shù)是通過(guò)Matlab中的m文件給出。加速度計(jì)的靜態(tài)誤差的建模和陀螺的靜態(tài)誤差的建?；疽恢拢?yàn)橥勇莺图铀俣扔?jì)之間的數(shù)據(jù)需要交叉補(bǔ)償，所以，這部分的Simulink建模原理圖見(jiàn)圖1。

靜態(tài)誤差補(bǔ)償?shù)臄?shù)學(xué)模型為:

ωr=ωm-δω= 1-Sx -Mxy -Mxy

-Myx 1-Sy -Myz

-Mzx -Mzy 1-Sz ωm- Gxx Gxy Gxz

Gyx Gyy Gyz

Gzx Gzy Gzz am- G2x amx amz

G2y amy amz

G2z amz amy -

T gδT- H gH- ε g0- ε gr (1)

ar=am-δa= 1-Kx -Lxy -Lxy

-Lyx 1-Ky -Lyz

-Lzx -Lzy 1-Kz am-

Gaxx Gaxy Gaxz

Gayx Gayy Gayz

Gazx Gazy Gazz ωm

- T aδT- H aH- ε a0- ε ar (2)

式中:Sij為刻度系數(shù);Mij為安裝偏差系數(shù);Gij為陀螺對(duì)線(xiàn)加速度的敏感系數(shù); T g為陀螺對(duì)溫度的敏感系數(shù)矩陣; H g為陀螺對(duì)磁場(chǎng)的敏感系數(shù)矩陣; ε g0為陀螺的常值漂移; ε gr為陀螺的隨機(jī)漂移;Ki為加速度計(jì)的刻度系數(shù)誤差;Lij為加速度計(jì)的安裝誤差系數(shù); G aij為加速度計(jì)對(duì)角速率的敏感系數(shù); T a為加速度計(jì)對(duì)溫度的敏感系數(shù); H a為加速度計(jì)對(duì)磁場(chǎng)的敏感系數(shù); ε g0為加速度計(jì)的零偏。在本文中，陀螺的隨機(jī)漂移和加速度計(jì)的零偏均被假定為一階高斯-馬爾科夫過(guò)程，具體的參數(shù)值可以設(shè)定，亦可通過(guò)實(shí)驗(yàn)室標(biāo)定后在Matlab下的m文件里設(shè)定。

圖1 靜態(tài)誤差補(bǔ)償原理圖

2 導(dǎo)航解算

導(dǎo)航解算包括姿態(tài)解算、位置解算和速度解算、系統(tǒng)級(jí)的誤差補(bǔ)償，這些誤差主要包括桿臂效應(yīng)、圓錐誤差、劃船誤差和渦卷誤差[4]。

姿態(tài)解算包含了3種方法的解算模塊:方向余弦矩陣、四元數(shù)和等效旋轉(zhuǎn)矢量。這里只給出筆者改進(jìn)的方向余弦矩陣算法，以及等效旋轉(zhuǎn)矢量算法的三子樣、四子樣算法的具體計(jì)算公式和仿真模塊圖。

姿態(tài)變換矩陣的微分形式為:

Lb= R Lb Ω bLb=- Ω LbL R Lb (3)

式中: R Lb是從彈體系到導(dǎo)航系的姿態(tài)變換矩陣; Ω bLb為彈體系相對(duì)導(dǎo)航系的角速度在彈體系上的投影的反對(duì)稱(chēng)矩陣。在姿態(tài)計(jì)算頻率較高、精度要求較高時(shí)，這里在文獻(xiàn)[3]的基礎(chǔ)上提出了如下的改進(jìn)型:

Φ(t，t0)=e∫tt0-ΩLbLdte- Ω LbLΔT

I- Ω LbLΔT+( Ω LbLΔT)22!-

( Ω LbLΔT)33!+( Ω LbLΔT)44!-

Ω LbLΔT= 0 Δθz -Δθy

-Δθz 0 Δθx

Δθy -Δθx 0 (4)

R Lb≈ 1-b(Δθ2y+Δθ2z) (a+c)Δθz+bΔθxΔθy bΔθxΔθz-(a+c)Δθy

bΔθxΔθy-(a+c)Δθz 1-b(Δθ2x+Δθ2z) bΔθyΔθz+(a+c)Δθx

bΔθxΔθz+(a+c)Δθy bΔθyΔθz-(a+c)Δθx 1-b(Δθ2x+Δθ2y)R Lb0 (5)

Δψ2=Δθ2x+Δθ2y+Δθ2z，a=1-Δθ2x+Δθ2y+Δθ2z6，b=12-Δθ2x+Δθ2y+Δθ2z24，c=Δψ4120 (6)

因?yàn)樵谧藨B(tài)計(jì)算中存在著嵌套的問(wèn)題所以對(duì)原來(lái)的算法進(jìn)行改進(jìn)，為了避免這個(gè)問(wèn)題，必須增大數(shù)據(jù)采樣速率和姿態(tài)計(jì)算速率。

姿態(tài)計(jì)算需要輸入的信息是陀螺的輸出、彈體的對(duì)地速度、彈體的位置。

等效旋轉(zhuǎn)矢量三子樣算法的思路和原理為在一個(gè)采樣周期內(nèi)等間隔取三個(gè)數(shù)據(jù)，并且假設(shè)此時(shí)的ωbLb按照二次曲線(xiàn)規(guī)律變化:

ωbLb(tk-1+τ)=a+bτ+cτ2 (7)

由此得到3個(gè)采樣:

θ1=∫h/30(a+bτ+cτ2)dτ，θ2=∫2h/3h/3(a+bτ+cτ2)dτ，

θ3=∫h2h/3(a+bτ+cτ2)dτ (8)

最后得到:

φ (tk-1+h)=θ1+θ2+θ3+3380(θ1θ3)+

5780θ2#8226;(θ1-θ3) (9)

式中: φ 為旋轉(zhuǎn)矢量。在求出 φ 后，四元數(shù)的更新過(guò)程為:

q =[cos(| φ |/2)，( φ /| φ |)sin(| φ |/2)]T=

[cos(| φ |/2)，( φ x/| φ |)sin(| φ |/2)，( φ y/| φ |)

sin(| φ |/2)，( φ z/| φ |)sin(| φ |/2)]T(10)

式中:| φ |為旋轉(zhuǎn)矢量的幅值。

等效旋轉(zhuǎn)矢量四子樣算法就是在1個(gè)采樣周期內(nèi)選取4個(gè)采樣點(diǎn):

ω=aτ+bτ2+cτ3，θ1=∫h/40ωdτ，

θ2=∫2h/4h/4ωdτ，θ3=∫3h/42h/4ωdτ，

θ4=∫h3h/4ωdτ(11)

對(duì)角增量表達(dá)式進(jìn)行切畢雪夫正交多項(xiàng)式擬合，得到:

φ (T+h)=θ+52105θ1θ2+

212315θ1θ3+112315θ1θ4+

184315θ2θ3+212315θ2θ4+

52105θ3θ4(12)

不論是三子樣還是四子樣，在得到了旋轉(zhuǎn)使矢量后，均按照式(10)進(jìn)行姿態(tài)計(jì)算。因此，姿態(tài)計(jì)算模塊的仿真原理圖可以統(tǒng)一化為圖2，只不過(guò)具體的計(jì)算公式不同。

圖2 姿態(tài)計(jì)算模塊

導(dǎo)航解算的數(shù)學(xué)公式如下:

X L=(rL，VL， R Lb)T

L= D -1VL

R Lbfb-(2 Ω Lie+ Ω LeL)VL+gL

R Lb Ω bib- Ω LiL R Lb(13)式中:

D -1= 0 1RM+h 0

1(RN+h)cos ψ 0 0

0 0 1，

RN=Re1-ε2sin2ψ，RM=Re(1-ε2)(1-ε2sin2ψ)3/2，

Re=6 378 137， f=298.257 223 563，

e=1/f， ε2=e(2-e)

式中:RN為卯酉圈的曲率半徑;RM為子午圈的曲率半徑;Re為橢球的長(zhǎng)半軸;f為地球的扁率。

式(13)中第二個(gè)方程右邊的第三行就是姿態(tài)計(jì)算。姿態(tài)計(jì)算、速度解算和位置解算在捷聯(lián)慣導(dǎo)中是作為一個(gè)整體進(jìn)行解算的。具體的原理圖如圖3所示。

圖3 SINS原理圖

基于該原理圖，速度解算和位置解算是需要知道地球引力場(chǎng)中重力加速度的模型，其具體的數(shù)學(xué)模型為:

ge=0

gn=7.546 619 578 9×10-10hsin(2ψ)

gu=(c00+c10sin2ψ+c20sin4ψ)+

(c01+c11sin2ψ+c21sin4ψ)h+

(c02+c12sin2φ+c22sin4ψ)h2

c00=9.780 326 582 929 618

c10=5.197 841 463 945×10-2

c20=-1.188 523 953 28×10-4

c01=-2.868 506 65×10-7

c11=4.105 897 709 9×10-10

c21=-9.247 990 22×10-13

c02=6.210 810 570 69×10-15

c12=-1.745 620 225 6×10-17

c22=-1.181 473 426×10-19(14)

式中:h的單位為m，ψ的單位為(°)，ci0的單位為m/s2。

圖4就是在Matlab 下進(jìn)行速度和位置解算的模塊圖，它包括3個(gè)獨(dú)立的模塊，這3個(gè)模塊又包括各自的子模塊，這些子模塊在此略去。

圖4 位置和速度解算模塊圖

該處的動(dòng)態(tài)誤差補(bǔ)償主要包括圓錐誤差、劃船誤差、渦卷誤差、桿臂效應(yīng)的補(bǔ)償。這些誤差的原理已在文獻(xiàn)[4]中有了較為深入的研究。限于篇幅，此處略去這些誤差補(bǔ)償?shù)姆抡嬖韴D。

3 輸出數(shù)據(jù)的演示

本文提出的仿真模型輸出的數(shù)據(jù)包括位置、對(duì)地速度、姿態(tài)、時(shí)間信息。這些信息的輸出有2種形式:一種是直接利用示波器演示，1臺(tái)示波器既可以只顯示1組數(shù)據(jù)，也可同時(shí)顯示多組數(shù)據(jù);另一種形式是把輸出數(shù)據(jù)放在工作空間中，通過(guò)編寫(xiě)m文件實(shí)現(xiàn)數(shù)據(jù)的顯示，這樣顯示的數(shù)據(jù)可以帶有標(biāo)題、坐標(biāo)軸格式設(shè)置、數(shù)據(jù)的單位等信息。

4 結(jié) 語(yǔ)

本文基于Matlab仿真環(huán)境，提出了低成本捷聯(lián)慣導(dǎo)系統(tǒng)的仿真模型，具體包括了元器件的誤差補(bǔ)償模型、姿態(tài)的解算模型、位置的解算模型、速度的解算模型、動(dòng)態(tài)誤差的補(bǔ)償模型、數(shù)據(jù)輸出等環(huán)節(jié)。這個(gè)模型的建立，為慣性器件的選擇、導(dǎo)航性能的評(píng)估、算法的優(yōu)化提供了參考平臺(tái)。

參考文獻(xiàn)

[1]祝彬，鄭娟.美國(guó)慣性導(dǎo)航與制導(dǎo)技術(shù)的新進(jìn)展[J].中國(guó)航 天，2008(1):43-45.

[2]張炎華，王立瑞，戰(zhàn)興群，等.慣性導(dǎo)航技術(shù)的新進(jìn)展及發(fā)展趨勢(shì)[ J] .中國(guó)造船，2008(10):134-144.

[3]BIEZAD D J.Integrated navigation and guidance systems[ M] . [ S.l.] : AIAA education series， 1999.

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

[5]陳永冰，鐘斌.慣性導(dǎo)航原理[M].北京:國(guó)防工業(yè)出版社，2007.

[6]黃麗，屈衛(wèi)東.車(chē)載INS/GPS組合導(dǎo)航系統(tǒng)建模與仿真[ J] .控制工程，2008(9):32-35.

[7]陶冶，房建成，楊超，等.基于低成本MEMS器件的MUAV導(dǎo)航與控制系統(tǒng)的設(shè)計(jì)[ J] .航空學(xué)報(bào)，2008(4):966-972.

[8]王希彬，趙國(guó)榮，高青偉.艦載機(jī)慣導(dǎo)系統(tǒng)快速傳遞對(duì)準(zhǔn)方法建模與仿真[ J] .飛行力學(xué)，2009(2):63-66.

[9]王東升，艾光彬，呂善民，等.艦載平臺(tái)式慣導(dǎo)系統(tǒng)的傳遞對(duì)準(zhǔn)[ J] .中國(guó)慣性技術(shù)學(xué)報(bào)，2009(1):24-27.

[10]韋凈凈，王艷東.INS/SMNS組合導(dǎo)航信息融合算法[ J] .北京航空航天大學(xué)學(xué)報(bào)，2009(3):292-295.