王海涌，徐 源，王偉東，劉佳琪

（1.北京航空航天大學(xué)宇航學(xué)院，北京 100191; 2.北京航天長征飛行器研究所，北京 100076）

地磁導(dǎo)航具有自主性、無積累誤差的優(yōu)點，與捷聯(lián)慣導(dǎo)組合依然保持導(dǎo)航自主性[1-4]。但不同高度區(qū)域的磁場環(huán)境復(fù)雜，需在應(yīng)用區(qū)域建立精確的地磁場模型。

利用加速度計、磁強計獲得重力矢量和地磁矢量修正姿態(tài)誤差的方法受到廣泛關(guān)注[5-11]，但在載體晃動或機(jī)動條件下不具適用性。文獻(xiàn)[12]則面向動態(tài)工況環(huán)境，將晃動情形下的有害加速度作為高頻噪聲，通過互補濾波加以補償。但其算法中的對姿態(tài)誤差方程的過分近似和簡化處理不合理，引入了額外誤差，難以有效提高導(dǎo)航精度。

高動態(tài)條件下飛行器導(dǎo)航系統(tǒng)分析和處理技術(shù)一直是核心技術(shù)和難點，從工程角度考量，機(jī)動工況在整個任務(wù)過程中的時間占比畢竟很低，絕大部分時段都是非機(jī)動工況，如懸停、勻速直航，一枚全導(dǎo)式導(dǎo)彈的中途巡航段大都是非機(jī)動工況。本文針對中低精度自主導(dǎo)航需求，提出了一種基于非機(jī)動窗口捕捉的姿態(tài)誤差在線修正方法。基于加速度計輸出值判定飛行狀態(tài)，捕捉非機(jī)動窗口，利用地平系下地磁矢量以及重力矢量通過新型濾波方法修正三軸姿態(tài)角，以期獲得較好的工程實用效果。

1 地磁場模型

地磁場是一個矢量場。比較著名的地磁場模型是每隔5年公布一次的IGRF(International Geomagnetic Reference Field)[4]。地磁場強度勢函數(shù)V表示為:

式中:Re為地球半徑，r為地心距，Lon為當(dāng)?shù)氐乩斫?jīng)度，Lat為當(dāng)?shù)氐乩砭暥?，N為IGRF 模型的截止階數(shù)，本文取N=10；和為高斯系數(shù)，由IGRF 模型提供；(cosLat)為施密特準(zhǔn)歸一化n次m階Legendre函數(shù)。

地磁場強度B在導(dǎo)航系(以東北天地理系為導(dǎo)航系)n 系中的矢量為Bn，在地球坐標(biāo)系e 系中的矢量為Be，在本體系下的矢量B（b由三軸磁強計測量獲得），假定三軸磁強計坐標(biāo)系與載體坐標(biāo)系重合，則有如下關(guān)系:

式中:Be通過磁場勢函數(shù)解算得到[11]。為捷聯(lián)矩陣，地球系到導(dǎo)航系的轉(zhuǎn)換矩陣可以表示為:

2 捷聯(lián)慣導(dǎo)/重力/地磁組合系統(tǒng)模型

2.1 系統(tǒng)狀態(tài)方程

以平臺失準(zhǔn)角φx,φy,φz（記為列向量φ）和三軸陀螺漂移εx,εy,εz（記為列向量ε）為狀態(tài)量，建立狀態(tài)方程:

式中為捷聯(lián)矩陣的轉(zhuǎn)置，ωe，ωre為陀螺隨機(jī)噪聲。

2.2 量測量構(gòu)建

2.2.1 重力矢量計算水平失準(zhǔn)角

加速度計測量值為比力fb。地理系下慣導(dǎo)的比力方程為:

gn為所在高度處的重力加速度，后兩項稱為有害加速度。當(dāng)載體懸?；騽蛩龠\動時，，此時式(5)可近似為:

式中，φ×為由φx,φy,φz張成的反對稱矩陣:

將式(7)帶入式(6)得:

非機(jī)動狀態(tài)時，有fn'=-gn=［0 0g］T。重力矢量作為參考只可估計二維水平失準(zhǔn)角φx,φy，則式(9)變?yōu)?

2.2.2 基于地磁場水平分量的航向角計算

地磁場矢量B的模為地磁場強度B，B在導(dǎo)航系下分量描述如圖1所示:

圖1 地理坐標(biāo)系下地磁要素Fig.1 Geomagnetic elements in geographical coordinate system

OENU為導(dǎo)航系，地磁矢量B在水平面OEN上的投影H為水平分量，其模為H。D為磁偏角，B與水平面的夾角I稱為磁傾角。

載體的俯仰角、橫滾角記為φ、γ，由312 轉(zhuǎn)序可得捷聯(lián)矩陣:

建立地平坐標(biāo)系h，與本體系b間的轉(zhuǎn)移矩陣為:

則可求得B在h系下的矢量坐標(biāo):

由矢量關(guān)系式:

則可求得磁航向角主值ψm:

ψm的真值需根據(jù)、進(jìn)一步判斷。那么載體的航向角觀測量為:

式(13)中的γ、φ現(xiàn)有方法中有二種獲得方式:一是將陀螺積分的姿態(tài)帶入；二是直接利用重力矢量計算[6-12]，如式(18):

本方法將水平失準(zhǔn)角與航向失準(zhǔn)角先后濾波，先對水平姿態(tài)角濾波，再帶入到式(13)～(16)求解航向角觀測量，可以降低誤差干擾提高定姿精度。

2.3 信息融合

采用輸出反饋卡爾曼濾波方式，將水平失準(zhǔn)角與航向失準(zhǔn)角分開濾波，其原理圖如圖2所示。

圖2 組合導(dǎo)航系統(tǒng)原理圖Fig.2 Principle figure of integrated navigation system

2.3.1 水平失準(zhǔn)角濾波器

水平失準(zhǔn)角濾波器的狀態(tài)量為X1=[φx φy εx εy εz]T，狀態(tài)方程為:

式中W1=[ωex ωeyωrex ωreyωrez]T為陀螺儀噪聲。

式中R1為水平姿態(tài)量測噪聲協(xié)方差陣。

2.3.2 航向失準(zhǔn)角濾波器的設(shè)計

根據(jù)平臺失準(zhǔn)角與姿態(tài)誤差角關(guān)系:

濾波后的水平姿態(tài)可認(rèn)為誤差角為0，根據(jù)式(20)此時航向失準(zhǔn)角與航向誤差角相等，則。以φz和本體系下Z軸陀螺漂移εz為狀態(tài)量的系統(tǒng)模型為:

式中R2為航向量測噪聲。

2.3.3 非機(jī)動窗口捕捉策略

即便在非機(jī)動工況下，加速度計輸出值的模|fb|也會因器件噪聲及載體晃動等原因圍繞重力加速度g在一個區(qū)間呈波動狀態(tài)。加速度計比力隨機(jī)噪聲標(biāo)準(zhǔn)差σax、σay、σaz通過標(biāo)定測試或者在線獲得，令。建立判別指標(biāo)α，公式如下:

根據(jù)α在線實測值進(jìn)行工況判別方法如下:

(1)非機(jī)動飛行:α≤1 0σa，此時判定載體處于非機(jī)動飛行狀態(tài)，式(22)中的R1=[σayσax]T。

(2)低加速度飛行:10σa≤α＜ 30σa；設(shè)計比例系數(shù)px、py，可以設(shè)計為α函數(shù)。式(22)中的R1變?yōu)槿缦滦问?R1=[py(α)σaypx(α)σax]T，即量測噪聲協(xié)方差陣隨α而改變。

(3)大機(jī)動飛行或失重狀態(tài):α≥ 30σa，本方法已不再適用，濾波器停止工作。

上述方法中的系數(shù)10、30、px(α)和py(α)只是設(shè)計階段的一個例子。隨著工程背景或在線實際情況的不同應(yīng)做相應(yīng)調(diào)整。

3 計算機(jī)仿真試驗

3.1 仿真條件設(shè)置

將飛行仿真軌跡設(shè)計為非機(jī)動情形(勻速飛行段)和機(jī)動情形(起飛爬升段、機(jī)動飛行段)的組合，以驗證本方法能否捕捉非機(jī)動窗口；利用勻速飛行段導(dǎo)航數(shù)據(jù)評估本方法的陀螺漂移補償?shù)淖藨B(tài)精度。仿真時間為330 s，捷聯(lián)慣導(dǎo)全程工作，125 s 后濾波器根據(jù)判斷條件捕捉非機(jī)動窗口進(jìn)行濾波，300 s 后濾波器停止工作。仿真軌跡如圖3所示:

圖3 設(shè)計的仿真飛行軌跡Fig.3 Designed simulation flight trajectory

仿真環(huán)境參數(shù)如下:

(1)出發(fā)點北緯34 °，東經(jīng)108 °，海拔高度100 m，仿真過程中最大飛行高度海拔2.73 km。

(2)初始航向角90 °，俯仰角0 °，橫滾角0 °；航向失準(zhǔn)角1 °，俯仰、橫滾失準(zhǔn)角0.3 °。

(3)采樣周期:陀螺、加速度計采樣周期0.01 s，磁力計采樣周期0.1 s，慣導(dǎo)解算周期0.01 s，卡爾曼濾波周期0.1 s。

(4)器件參數(shù):陀螺常值漂移6 °/h，隨機(jī)噪聲標(biāo)準(zhǔn)差為0.002 °/s；加速度計常值偏移為10 mg，隨機(jī)噪聲標(biāo)準(zhǔn)差2 mg；磁強計噪聲100 nT。

3.2 結(jié)果分析

組合導(dǎo)航系統(tǒng)的姿態(tài)角誤差如圖4～6，速度誤差如圖7所示。

圖4 俯仰角誤差Fig.4 Pitch angle error

圖5 橫滾角誤差Fig.5 Roll angleerror

圖6 航向角誤差Fig.6 Heading angle error

圖7 速度誤差Fig.7 Velocity error

在初始飛行的125s，只有慣導(dǎo)工作，姿態(tài)誤差迅速發(fā)散，導(dǎo)致速度誤差也迅速發(fā)散。俯仰角、橫滾角、航向角誤差分別達(dá)到了1.7°、1.4°、1.76°，速度誤差分別累積到了14.7 m/s、11.2m/s、1.48m/s。125s后載體變?yōu)閯蛩亠w行，圖示的姿態(tài)誤差迅速收斂并保持穩(wěn)定，表明本方法成功捕捉到非機(jī)動工況窗口，濾波器的工作條件被觸發(fā)并開始工作，在175s時，俯仰角誤差為0.036′，橫滾角誤差2.46′，航向角誤差1.08′。東向、北向速度誤差也得到快速、有效的抑制。

在175s到187s之間載體做連續(xù)機(jī)動飛行，機(jī)動導(dǎo)致加速度計輸出遠(yuǎn)大于重力加速度，濾波器停止工作，姿態(tài)誤差又開始發(fā)散。187s后，飛行器繼續(xù)做勻速運動，濾波器得以重啟，姿態(tài)誤差重新收斂。300s后人為關(guān)閉濾波器工作，捷聯(lián)慣導(dǎo)姿態(tài)誤差重新發(fā)散。

觀察圖7中125～300s時間段幾乎平直的東向和北向速度誤差，發(fā)散抑制程度顯著，正是由于濾波提升了數(shù)學(xué)平臺精度。由于捷聯(lián)慣導(dǎo)高度通道發(fā)散，天向速度誤差自身無法抑制，需借助其它方式補償。

此外，本方法還能有效估計出載體系下的陀螺漂移，如圖8所示。

圖8 陀螺漂移估計值Fig.8 Estimated value of gyro drift

為驗證加速度計噪聲對姿態(tài)精度的影響，分別選取加速度計隨機(jī)噪聲標(biāo)準(zhǔn)差為5mg、20mg、40mg、80 mg作為仿真條件開展仿真，同時，揀選文獻(xiàn)[12]中互補濾波這一現(xiàn)有典型方法進(jìn)行復(fù)現(xiàn)，對于姿態(tài)結(jié)果做比較。針對飛行過程中的非機(jī)動工況時段，本方法的姿態(tài)角標(biāo)準(zhǔn)差列于表1。文獻(xiàn)[12]中的互補濾波方法計算的姿態(tài)角標(biāo)準(zhǔn)差列于表2。

表1 不同加速度計噪聲下本文方法姿態(tài)標(biāo)準(zhǔn)差Tab.1 Attitude error Std of thismethodunder different accelerometer noise

表2 不同加速度計噪聲下的互補濾波算法的姿態(tài)標(biāo)準(zhǔn)差Tab.2 Attitude error stdof thecomplementary filtering methodunder differentaccelerometer noise

對比表1、表2，隨著加速度計噪聲水平的增加，本文所提方法對于組合系統(tǒng)姿態(tài)精度降低效果顯著，當(dāng)隨機(jī)噪聲強度增加至80 mg時，本文方法姿態(tài)精度優(yōu)于8′，與現(xiàn)有互補濾波[12]相比優(yōu)勢明顯，姿態(tài)精度提高約50%。此外，本文方法算法中還具有觀測噪聲方差R1隨著加速度計輸出的自適應(yīng)調(diào)節(jié)機(jī)制，也有助于提高濾波精度。

4 基于轉(zhuǎn)臺的物理試驗

為驗證算法實用性，利用ADIS16488九軸MEMS慣組進(jìn)行轉(zhuǎn)臺試驗并分析結(jié)果，IMU噪聲特性見表3。

表3 IMU噪聲特性Tab.3 IMU noise specifications

試驗現(xiàn)場裝置如圖9。

圖9 MEMS慣組和轉(zhuǎn)臺試驗裝置Fig.9 MEMS IMU and the turntable device

4.1 靜態(tài)試驗

將IMU 緊固在轉(zhuǎn)臺內(nèi)框臺面，調(diào)整其本體三軸與轉(zhuǎn)臺XYZ三軸重合。靜態(tài)采集數(shù)據(jù)150s 靜態(tài)并分析，觀察對比有無本文濾波方法的性能差異。

4.2 動態(tài)試驗

控制轉(zhuǎn)臺以70°/s的角速率繞Z軸(天向)在0～110°范圍內(nèi)做往復(fù)運動，采集60s數(shù)據(jù)。在0°和110°時會因角速度方向變化有電機(jī)啟、停，是一種機(jī)動狀態(tài)，可驗證本文算法的非機(jī)動窗口捕捉性能。

4.3 結(jié)果分析

試驗結(jié)果如圖10～12。由圖10～11可知，靜態(tài)條件下，不做漂移補償任由陀螺積分計算的姿態(tài)角在150s時刻分別產(chǎn)生了高達(dá)21.15°、25.52°、63.77°的漂移誤差。在基于轉(zhuǎn)臺的機(jī)動動態(tài)測試中，陀螺積分方法在60s過程中分別產(chǎn)生了最大值為4.953°、4.375°、51.28°的三軸最大姿態(tài)角誤差，圖11中已用O標(biāo)出。

圖10 有無本文濾波算法的靜態(tài)數(shù)據(jù)三軸姿態(tài)角誤差對比Fig.10 Attitude angle error comparison of the static sampling data with vs.without the algorithm of this paper

圖11 轉(zhuǎn)臺動態(tài)運動下兩種方法姿態(tài)角對比Fig.11 Attitude comparison with vs.without the algorithm of this paper under turntable maneuvering motion

試驗結(jié)果表明，本文方法在靜、動態(tài)測試中均能有效修正姿態(tài)誤差。然后，復(fù)現(xiàn)現(xiàn)有典型的互補濾波算法[12]得到的測試結(jié)果如圖12所示。

圖12 動態(tài)條件下本算法與典型的互補濾波方法誤差對比Fig.12 Attitude error comparison between the typical complementary filtering method and the method of this paper under turntable dynamic motion

本方法與現(xiàn)有的互補濾波[12]算法相比，考慮了加速度計噪聲以及載體機(jī)動對姿態(tài)誤差的影響，并根據(jù)加速度計輸出值將載體分為三種機(jī)動情形，并自適應(yīng)調(diào)整式(22)中的R1。分析圖12，在動態(tài)測試中，轉(zhuǎn)臺啟、停產(chǎn)生的機(jī)動會影響加速度計輸出值，進(jìn)而產(chǎn)生姿態(tài)修正誤差，本文所提方法在計算過程中能夠識別出此機(jī)動，并將其判定為低加速度機(jī)動過程，進(jìn)一步減小機(jī)動帶來的姿態(tài)誤差，圖中□標(biāo)志為本方法判定出的機(jī)動時刻。而現(xiàn)有互補濾波方法明顯受此機(jī)動影響，在過程中產(chǎn)生較大姿態(tài)誤差，抗機(jī)動性能不如本文方法。

5 結(jié) 論

針對低成本中低精度慣性自主導(dǎo)航，全航段或航時中非機(jī)動窗口的占比往往都很大。根據(jù)非機(jī)動窗口下的加速度計比力等于重力的現(xiàn)象，構(gòu)建了基于加速度計、磁強計的新型濾波方法。建立了加速度計比力值判定指標(biāo)確定飛行中的非機(jī)動窗口，通過重力矢量先行修正水平姿態(tài)角，然后利用已修正的水平姿態(tài)角以及磁強計輸出值再行修正航向角，濾波效果顯著提升。給出了計算機(jī)仿真結(jié)果，在陀螺漂移6 °/h，磁力計精度100 nT器件參數(shù)仿真條件下，姿態(tài)精度優(yōu)于3 ′，速度誤差發(fā)散程度也得到抑制，同時進(jìn)行了加速度計精度對該方法有效性的仿真，加速度計隨機(jī)噪聲強度為80 mg時，該方法姿態(tài)精度優(yōu)于8 ′，與現(xiàn)有的互補濾波[13]方法相比姿態(tài)精度提高50%。利用MEMS慣組ADIS16488開展了轉(zhuǎn)臺試驗，試驗數(shù)據(jù)處理結(jié)果表明，該方法運行正確，可有效修正姿態(tài)誤差，比現(xiàn)有互補濾波[12]方法更具抗機(jī)動性。該方法完全自主，精度較高，可有效提升中等精度慣性器件的導(dǎo)航性能，具有工程應(yīng)用價值。