加速度計(jì)/磁強(qiáng)計(jì)無(wú)陀螺捷聯(lián)慣導(dǎo)姿態(tài)解算研究

渠 晉， 宋艷君， 臧 潔， 程洪炳

(空軍工程大學(xué)工程學(xué)院自動(dòng)控制工程系，西安 710038)

0 引言

無(wú)陀螺捷聯(lián)慣導(dǎo)系統(tǒng)(Gyro-Free Strap-Down Inertial Navigation Systems，GFSINS)與普通捷聯(lián)慣導(dǎo)系統(tǒng)相比具有低成本、低功耗、可靠性高、壽命時(shí)間長(zhǎng)等優(yōu)點(diǎn)，但由于在GFSINS中，載體的角速度是通過加速度計(jì)的比力方程解算得到，而目前加速度計(jì)測(cè)量誤差較大，造成姿態(tài)角計(jì)算值的誤差積累速度較快［1］。近年來(lái)，多傳感器數(shù)據(jù)融合技術(shù)發(fā)展迅速，在各個(gè)領(lǐng)域都得到了廣泛的應(yīng)用，本文利用了GFSINS和三軸磁強(qiáng)計(jì)構(gòu)成組合導(dǎo)航系統(tǒng)，以磁強(qiáng)計(jì)的測(cè)量值結(jié)合國(guó)際地磁場(chǎng)模型修正GFSINS的姿態(tài)角誤差，抑制誤差的積累，提高了姿態(tài)角的解算精度。

1 GFSINS姿態(tài)角解算

角速度計(jì)輸出方程［2］為

其中:i為慣性系;b為載體系;ri為加速度計(jì)安裝位置矢量;θi為敏感方向矢量;fb為投影到載體系的比力;為載體角速度為載體角加速度;Ω為載體角速度的反對(duì)稱矩陣［2－3］。

因此得到固連在載體上的N個(gè)加速度計(jì)輸出的矩陣形式為

令Q為J的左逆矩陣，則有

由式(3)可計(jì)算得到載體相對(duì)慣性空間的角速度在載體系3個(gè)軸的分量，進(jìn)而可以確定載體姿態(tài)角。

2 加速度計(jì)配置方案

目前常用的配置方案有六加速度計(jì)和九加速度計(jì)，相比于六加速度計(jì)，九加速度計(jì)配置方案系統(tǒng)可靠性更高，而且可以利用冗余信息提高角速度解算精度。本文采用如圖1所示的九加速度計(jì)配置方案［4］。

圖1 加速度計(jì)配置方案Fig.1 Configuration of the accelerometer

原點(diǎn)在載體質(zhì)心處，yb軸沿載體縱軸并指向載體彈頭，xb軸沿尾翼并與yb軸垂直，zb軸與xb，yb軸構(gòu)成右手坐標(biāo)系，加速度計(jì)安裝位置如圖1所示，箭頭所指方向?yàn)榧铀俣扔?jì)的敏感方向。

加速度計(jì)的位置矢量r和敏感方向矢量θ為

式中，l為非原心處加速度計(jì)與原心之間的距離。由公式(2)得加速度計(jì)的輸出方程為

對(duì)照組采取常規(guī)治療，根據(jù)實(shí)際病情予以對(duì)癥干預(yù)，使用鈣離子拮抗劑、β受體阻滯劑、硝酸酯類藥物等，阿司匹林口服劑量為100 mg/次，1次/d，曲美他嗪20 mg/次，3次/d，單硝酸異山梨酯注射液劑量為40 mg/次，1次/d，靜脈注射，將其置于生理鹽水中使用，阿托伐他汀20 mg/次，1次/d，治療時(shí)間為兩周。

其中:ωx，ωy，ωz為載體角速度在載體系各軸上的分量;在載體系各軸上的分量;ωxωy，ωxωz，ωyωz為冗余信息。由式(6)解得

載體角速度的計(jì)算值選取式(8)開方得到的數(shù)值，并利用式(7)確定開方后的符號(hào)，即(以x方向?yàn)槔?

由式(9)得到了角速度，經(jīng)過積分即可獲得姿態(tài)角(以俯仰角φ為例)

式中，T為計(jì)算周期。

由于加速度計(jì)存在測(cè)量誤差，通過積分得到姿態(tài)角的誤差隨時(shí)間快速積累，不利于姿態(tài)角的精確解算。

3 加速度計(jì)/磁強(qiáng)計(jì)的濾波方程

無(wú)陀螺慣導(dǎo)系統(tǒng)姿態(tài)角的數(shù)值積分中存在累積誤差等因素，這使得計(jì)算得到的姿態(tài)角誤差不斷積累。磁強(qiáng)計(jì)具有抗搖動(dòng)與抗震性，在現(xiàn)代導(dǎo)航系統(tǒng)中常結(jié)合加速度計(jì)來(lái)提供輔助的航向信息，其提供的航向精度高且穩(wěn)定，因而廣泛地應(yīng)用于無(wú)人機(jī)、航空、航天自主導(dǎo)航領(lǐng)域。本文在系統(tǒng)中增加了三軸磁強(qiáng)計(jì)，通過測(cè)量地磁場(chǎng)在載體系3個(gè)軸向的分量，修正姿態(tài)角。本文采用了國(guó)際地磁場(chǎng)模型［6］，假設(shè)地球是一個(gè)半徑為RE的標(biāo)準(zhǔn)球體。采用當(dāng)?shù)氐乩碜鴺?biāo)系來(lái)描述磁場(chǎng)強(qiáng)度，則可得到沿坐標(biāo)系3個(gè)方向的磁場(chǎng)分量為［6－7］

式中:r為地心距;RE為地球平均半徑;λ為經(jīng)度;θ為余緯;為n次m階締合勒讓德函數(shù);為基本磁場(chǎng)的高斯系數(shù)。

根據(jù)國(guó)際地磁場(chǎng)模型，可以得出當(dāng)?shù)氐卮艌?chǎng)強(qiáng)度。將此磁場(chǎng)強(qiáng)度和三軸磁強(qiáng)計(jì)測(cè)量值的差值作為觀測(cè)值，就建立起了磁場(chǎng)強(qiáng)度的差值和四元數(shù)誤差的關(guān)系，對(duì)誤差四元數(shù)進(jìn)行反饋校正，從而實(shí)現(xiàn)動(dòng)態(tài)條件下的姿態(tài)確定。

系統(tǒng)姿態(tài)誤差方程［8－9］為

式中:ωm是載體坐標(biāo)系相對(duì)于地理坐標(biāo)系的轉(zhuǎn)動(dòng)角速度;q是測(cè)量的姿態(tài)四元數(shù);b是加速度計(jì)的零偏誤差;wg是加速度計(jì)的測(cè)量噪聲;wm是加速度計(jì)的零偏誤差。

取狀態(tài)變量X=［Δq b］T，則系統(tǒng)狀態(tài)方程為

式中:Δq=［Δq0Δq1Δq2Δq3］T;b=［bxbybz］T;w(t)=［wgwm］T。

對(duì)式(18)進(jìn)行展開可得系統(tǒng)觀測(cè)方程

由式(17)、式(19)通過Kalman濾波得到誤差四元數(shù)的估計(jì)值，則真實(shí)的四元數(shù)為誤差四元數(shù)與測(cè)量的四元數(shù) q 的和［4］，即

根據(jù)載體四元數(shù)即可計(jì)算得到載體姿態(tài)角。

4 仿真結(jié)果

利用某型導(dǎo)彈的彈道數(shù)據(jù)，分別在有磁強(qiáng)計(jì)輔助和無(wú)磁強(qiáng)計(jì)輔助的情況下進(jìn)行仿真實(shí)驗(yàn)，配置方案的參數(shù)設(shè)計(jì)為:加速度計(jì)的精度為10－6g;磁強(qiáng)計(jì)的精度為 ±0.5°;加速度計(jì)安裝位置 l為 0.1 m，仿真步長(zhǎng)為1 s，仿真時(shí)間為350 s。初始位置設(shè)計(jì)為北緯40°、東經(jīng)120°、海拔高度500 m;導(dǎo)彈初始速度為500 m/s，初始俯仰角45°，航向角30°。仿真結(jié)果如圖2～圖5所示。

從仿真結(jié)果可以看出，無(wú)磁強(qiáng)計(jì)輔助GFSINS的姿態(tài)角度誤差在很短的時(shí)間內(nèi)就急劇發(fā)散，而磁強(qiáng)計(jì)輔助無(wú)陀螺系統(tǒng)的解算結(jié)果相對(duì)比較穩(wěn)定。對(duì)有無(wú)磁強(qiáng)計(jì)輔助解算出的姿態(tài)角誤差進(jìn)行統(tǒng)計(jì)分析發(fā)現(xiàn)，在仿真時(shí)間段內(nèi)，無(wú)磁強(qiáng)計(jì)輔助GFSINS的俯仰角解算誤差均值為0.0932，方差為0.0115，橫滾角解算誤差均值為 －0.0966，方差為0.0116;磁強(qiáng)計(jì)輔助 GFSINS 的俯仰角解算誤差為0.0011，方差為0.0021，橫滾角解算誤差為0.0089，方差為0.0041。這表明磁強(qiáng)計(jì)輔助GFSINS可以明顯提升姿態(tài)解算精度。

圖2 無(wú)磁強(qiáng)計(jì)輔助GFSINS的俯仰角誤差Fig.2 Pitch error of GFSINS without magnetometer aiding

圖3 無(wú)磁強(qiáng)計(jì)輔助GFSINS的橫滾角誤差Fig.3 Roll error of GFSINS without magnetometer aiding

圖4 磁強(qiáng)計(jì)輔助GFSINS的俯仰角誤差Fig.4 Pitch error of GFSINS with magnetometer aiding

圖5 磁強(qiáng)計(jì)輔助GFSINS的橫滾角誤差Fig.5 Roll error of GFSINS with magnetometer aiding

5 結(jié)論

在GFSINS中，構(gòu)造了一種改進(jìn)的九加速度計(jì)配置方案，該配置能直接得到載體的角加速度項(xiàng)和角速度乘積項(xiàng)。由于加速度計(jì)測(cè)量誤差的存在，角速度誤差隨時(shí)間快速積累，所得的姿態(tài)角誤差值也出現(xiàn)強(qiáng)烈的離散現(xiàn)象。將磁強(qiáng)計(jì)用于輔助GFSINS姿態(tài)解算中，用得到的地磁場(chǎng)載體系軸向分量和當(dāng)?shù)貒?guó)際地磁場(chǎng)模型3個(gè)軸向地磁場(chǎng)分量的差值修正姿態(tài)誤差角，有效避免了加速度計(jì)的測(cè)量誤差引起的積分誤差積累，提高了GFSINS姿態(tài)解算的精度。

［1］ 史震，于秀萍.無(wú)陀螺捷聯(lián)式慣性導(dǎo)航系統(tǒng)［M］.哈爾濱:哈爾濱工程大學(xué)出版社，2007.

［2］ TAN C W，PARK S，MOSTOV K，et al.Dsign of gyroscope-free navigation systems［C］//2001 IEEE Intelligent Transportation Systems Conference，August 2001:25-29.

［3］ EGZIABHER D G.A gyro-free quaternion-based attitude determination system suitable for implementation using low cost sensors［C］//2000 IEE Position Location and Navigation Symposium，2000:185-192.

［4］ 趙國(guó)榮，陳穆清.一種用于九加速度計(jì)GFSINS的姿態(tài)角速度輔助算法［J］.系統(tǒng)仿真學(xué)報(bào)，2007，19(14):3350-3353.

［5］ HUANG Lin，JING Wuxing.Attitude-independent geomagnetic navigation using onboard complete three-axis magnetometer calibration［C］//2008 IEEE Aerospace Conference，2008:1-7.

［6］ 楊曉東，王煒.地磁導(dǎo)航原理［M］.北京:國(guó)防工業(yè)出版社，2009.

［7］ 董昆，周軍，葛致磊.基于地磁場(chǎng)的新型導(dǎo)航方法研究［J］.火力與指揮控制，2009，34(3):153-155.

［8］ 楊功流，李士心，姜朝宇.地磁輔助慣性導(dǎo)航系統(tǒng)的數(shù)據(jù)融合算法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2007，5(1):46-49.

［9］ 穆華，任治新，胡小平，等.船用慣性/地磁導(dǎo)航系統(tǒng)信息融合策略與性能［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2007，15(3):322-326.

［10］ 田菁，吳美平，胡小平.三軸磁強(qiáng)計(jì)姿態(tài)確定［J］.國(guó)防科技大學(xué)學(xué)報(bào)，2001(5):17-21.

［11］ 韋娟，寧方立.誤差四元數(shù)及其在航天器姿態(tài)控制中的應(yīng)用［J］.飛行力學(xué)，2006，24(2):60-62.

［12］ 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安:西北工業(yè)大學(xué)出版社，1998.