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

(空軍工程大學工程學院自動控制工程系，西安 710038)

0 引言

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

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

角速度計輸出方程［2］為

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

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

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

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

2 加速度計配置方案

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

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

原點在載體質心處，yb軸沿載體縱軸并指向載體彈頭，xb軸沿尾翼并與yb軸垂直，zb軸與xb，yb軸構成右手坐標系，加速度計安裝位置如圖1所示，箭頭所指方向為加速度計的敏感方向。

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

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

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

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

載體角速度的計算值選取式(8)開方得到的數值，并利用式(7)確定開方后的符號，即(以x方向為例)

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

式中，T為計算周期。

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

3 加速度計/磁強計的濾波方程

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

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

根據國際地磁場模型，可以得出當地地磁場強度。將此磁場強度和三軸磁強計測量值的差值作為觀測值，就建立起了磁場強度的差值和四元數誤差的關系，對誤差四元數進行反饋校正，從而實現(xiàn)動態(tài)條件下的姿態(tài)確定。

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

式中:ωm是載體坐標系相對于地理坐標系的轉動角速度;q是測量的姿態(tài)四元數;b是加速度計的零偏誤差;wg是加速度計的測量噪聲;wm是加速度計的零偏誤差。

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

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

對式(18)進行展開可得系統(tǒng)觀測方程

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

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

4 仿真結果

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

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

圖2 無磁強計輔助GFSINS的俯仰角誤差Fig.2 Pitch error of GFSINS without magnetometer aiding

圖3 無磁強計輔助GFSINS的橫滾角誤差Fig.3 Roll error of GFSINS without magnetometer aiding

圖4 磁強計輔助GFSINS的俯仰角誤差Fig.4 Pitch error of GFSINS with magnetometer aiding

圖5 磁強計輔助GFSINS的橫滾角誤差Fig.5 Roll error of GFSINS with magnetometer aiding

5 結論

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

［1］ 史震，于秀萍.無陀螺捷聯(lián)式慣性導航系統(tǒng)［M］.哈爾濱:哈爾濱工程大學出版社，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］ 趙國榮，陳穆清.一種用于九加速度計GFSINS的姿態(tài)角速度輔助算法［J］.系統(tǒng)仿真學報，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］ 楊曉東，王煒.地磁導航原理［M］.北京:國防工業(yè)出版社，2009.

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

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

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

［10］ 田菁，吳美平，胡小平.三軸磁強計姿態(tài)確定［J］.國防科技大學學報，2001(5):17-21.

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

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