趙彥明，秦永元，嚴(yán)恭敏

(西北工業(yè)大學(xué)自動化學(xué)院，陜西西安 710129)

0 引言

隨著微機(jī)械和微細(xì)加工技術(shù)的發(fā)展，各種MEMS傳感器日趨成熟，利用微慣性傳感器設(shè)計微型航向姿態(tài)參照系統(tǒng)(micro attitude heading reference system，AHRS)成為現(xiàn)實(shí)。自主式微航姿系統(tǒng)(AHRS)具有集成度高、體積小、質(zhì)量輕、成本低、功耗低、結(jié)構(gòu)簡單、可靠性高等優(yōu)點(diǎn)，所以在很多使用場合被采用，在飛機(jī)上常被用作備份的應(yīng)急測姿設(shè)備。微型AHRS一般由三軸MEMS陀螺、三軸MEMS加速度計、三軸MEMS磁強(qiáng)計和信息處理電路組成[1]。其中，MEMS陀螺敏感載體運(yùn)動角速度，通過求解姿態(tài)微分方程來更新姿態(tài)角，但是MEMS陀螺存在嚴(yán)重的零點(diǎn)漂移和隨機(jī)誤差，在姿態(tài)解算中會產(chǎn)生累積誤差，難以滿足應(yīng)用的精度要求[2]。為了實(shí)時補(bǔ)償陀螺漂移和累積誤差造成的姿態(tài)誤差，微型姿態(tài)測量系統(tǒng)常同時采用MEMS加速度計和微型磁傳感器的測量值，利用信息融合算法，進(jìn)行傳感器信息融合，構(gòu)建組合姿態(tài)測量系統(tǒng)，抑制姿態(tài)誤差角的發(fā)散[3]。

目前有大量文獻(xiàn)討論利用MEMS慣性傳感器實(shí)現(xiàn)姿態(tài)解算的問題，主要分為兩類：使用隨機(jī)線性估計方法，如卡爾曼濾波算法等；使用確定性的互補(bǔ)濾波器算法。

本文對AHRS信息融合算法，包括直接卡爾曼濾波法、間接卡爾曼濾波法和互補(bǔ)濾波器法，進(jìn)行了算法推導(dǎo)和比較分析，并且應(yīng)用設(shè)計的AHRS系統(tǒng)進(jìn)行了半物理仿真實(shí)驗(yàn)驗(yàn)證，為工程應(yīng)用中AHRS算法選擇提供選擇依據(jù)。

1 AHRS系統(tǒng)算法原理

設(shè)導(dǎo)航坐標(biāo)系(n系)為“東北天”地理坐標(biāo)系(g系)，載體坐標(biāo)系(b系)為“右前上”坐標(biāo)系。AHRS的姿態(tài)更新可以采用四元數(shù)形式進(jìn)行更新，姿態(tài)四元數(shù)微分方程如下[2]：

(1)

低成本AHRS系統(tǒng)常作為機(jī)載應(yīng)急備份導(dǎo)航系統(tǒng)。由于其MEMS陀螺漂移大，姿態(tài)精度較低，需要利用飛機(jī)勻速直線飛行時的加速度計輸出計算俯仰角和橫滾角，結(jié)合磁強(qiáng)計輸出計算航向角，實(shí)時修正四元數(shù)更新結(jié)果。

(2)

(3)

(4)

考慮磁差和羅差的影響，磁強(qiáng)計輸出的磁航向角必須補(bǔ)償當(dāng)?shù)氐拇牌铅痢?/p>

2 AHRS信息融合方法

2.1 直接卡爾曼濾波法

根據(jù)四元數(shù)微分方程和陀螺相關(guān)漂移微分方程，建立EKF狀態(tài)方程：

(5)

其中，

(6)

(7)

(8)

將加速度計、磁強(qiáng)計測量值作為觀測量。勻速直線飛行條件下，構(gòu)造量測方程分別如下：

(9)

系統(tǒng)量測方程為

(10)

其中

(11)

式(5)和式(10)構(gòu)成狀態(tài)空間模型呈非線性，用EKF進(jìn)行處理。

2.2 間接卡爾曼濾波法

間接卡爾曼濾波法構(gòu)建AHRS系統(tǒng)，以載體的姿態(tài)誤差參數(shù)，比如姿態(tài)誤差四元數(shù)或平臺失準(zhǔn)角作為被估計量，以相對穩(wěn)定的加速度計、磁強(qiáng)計測量數(shù)據(jù)為觀測量，應(yīng)用卡爾曼濾波算法進(jìn)行濾波估計，而后進(jìn)行誤差校正[6-7]。由于平臺失準(zhǔn)角誤差模型具有線性形式，可以直接采用卡爾曼濾波基本方程進(jìn)行濾波融合，算法簡單。選擇AHRS系統(tǒng)的平臺失準(zhǔn)角φ和陀螺漂移εr為濾波狀態(tài)，即狀態(tài)向量為：X=[φEφNφUεrxεryεrz]T。構(gòu)建濾波狀態(tài)方程：

(12)

式中：F(t)為系統(tǒng)矩陣；G(t)為噪聲分配矩陣；w(t)為系統(tǒng)驅(qū)動噪聲。

(13)

(14)

(15)

=HfX+Vf

(16)

其中，

(17)

(18)

(19)

(20)

(21)

2.3 互補(bǔ)濾波器法

除了卡爾曼濾波算法之外，互補(bǔ)濾波器也常被用于低成本AHRS系統(tǒng)姿態(tài)濾波，其具有算法簡單，計算量小，實(shí)時性高，魯棒性好的優(yōu)點(diǎn)，尤其適用于處理器計算資源有限的情況[8]。互補(bǔ)濾波器利用各種傳感器量測來相互補(bǔ)充從而獲得更好的姿態(tài)解算[9-11]?；パa(bǔ)濾波器算法原理如下：采用陀螺積分得到的載體姿態(tài)和已知的地球重力矢量，解算出地球重力矢量在載體系下的投影和加速度計測量出的地球重力矢量的矢量乘積結(jié)果作為水平姿態(tài)角的誤差表征數(shù)值，并且采用比例積分跟蹤算法進(jìn)行誤差跟蹤反饋，實(shí)現(xiàn)了準(zhǔn)確的水平姿態(tài)角跟蹤測量。利用陀螺積分得到的姿態(tài)和已知的地球磁場信息，解算地磁場矢量在載體系下的投影與三軸磁強(qiáng)計測量的地磁場矢量叉乘結(jié)果作為航向誤差角的誤差表征數(shù)值，并且采用比例積分跟蹤算法進(jìn)行誤差跟蹤反饋,實(shí)現(xiàn)了準(zhǔn)確的航向跟蹤測量[12]。

(22)

(23)

(24)

(3)計算角速度誤差向量

(25)

(4)將δω代入比例積分控制環(huán)節(jié)，得到載體角速度等效修正量，角速率的總補(bǔ)償量為

(26)

式中：Kp、Ki分別為比例、積分系數(shù)，根據(jù)工程經(jīng)驗(yàn)取值為Kp=0.3、Ki=0.01。

(27)

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)說明

利用實(shí)驗(yàn)室研發(fā)的自主式AHRS系統(tǒng)開展實(shí)驗(yàn)，AHRS系統(tǒng)包括姿態(tài)測量模塊，Micro SD卡數(shù)據(jù)采集和存儲模塊，電源模塊。姿態(tài)測量模塊采用九自由度MIMU ADIS16405和STM32F103 ARM處理器搭建。AHRS系統(tǒng)實(shí)物如圖1所示。MIMU數(shù)據(jù)輸出頻率100 Hz，陀螺零偏穩(wěn)定性26°/h，加速度計零偏0.2 mg，磁強(qiáng)計零偏誤差4 mGuass。AHRS系統(tǒng)通過過渡板安裝在三軸轉(zhuǎn)臺上，如圖2所示。初始時刻，系統(tǒng)的姿態(tài)角歸0，即X、Y、Z軸依次沿東向、真北方向，天向?？刂妻D(zhuǎn)臺依次轉(zhuǎn)到以下測試點(diǎn)，記錄各靜態(tài)測試點(diǎn)的傳感器數(shù)據(jù)：(1)偏航角測試的測試點(diǎn)為：0°、30°、60°、90°、120°、150°、180°、210°、240°、270°、300°、330°、360°；(2)橫滾角測試的測試點(diǎn)為：0°、30°、60°、90°、120°、150°、±180°、-150°、-120°、-90°、-60°、-30°、0°；(3)俯仰角測試的測試點(diǎn)為：0°、-10°、-20°、-30°、-40°、-50°、-60°、-70°、-80°、-90°、0°、10°、20°、30°、40°、50°、60°、70°、80°、90°、0°。

圖1 AHRS系統(tǒng)實(shí)物圖

圖2 AHRS系統(tǒng)三軸轉(zhuǎn)臺實(shí)驗(yàn)

從所記錄的MIMU數(shù)據(jù)中選取穩(wěn)定狀態(tài)的數(shù)據(jù)(總時間長6 000 s)。利用實(shí)驗(yàn)過程中實(shí)時采集的MIMU角速率、加速度和地磁場測量數(shù)據(jù)進(jìn)行數(shù)據(jù)融合算法測試，即分別用直接卡爾曼濾波算法、間接卡爾曼濾波算法、互補(bǔ)濾波器算法處理實(shí)測數(shù)據(jù)，并且對結(jié)果進(jìn)行分析比較。

3.2 實(shí)驗(yàn)結(jié)果與分析

實(shí)驗(yàn)結(jié)果如圖3～圖5和表1所示。其中，圖3為采用直接KF濾波法計算的姿態(tài)誤差曲線，圖4為采用間接KF濾波法計算的姿態(tài)誤差曲線，圖5為采用互補(bǔ)濾波器法計算的姿態(tài)誤差曲線。每幅圖均包含3個子圖，分別為俯仰、橫滾及航向角誤差曲線圖。表1為3種算法的姿態(tài)誤差統(tǒng)計結(jié)果。

圖3 采用直接KF濾波法的姿態(tài)誤差曲線

圖4 采用間接KF濾波法的姿態(tài)誤差曲線

圖5 采用互補(bǔ)濾波器法的姿態(tài)誤差曲線

(1)表1統(tǒng)計結(jié)果顯示，在3種方法之中，間接卡爾曼濾波法精度較高，互補(bǔ)濾波器最低，直接卡爾曼濾波法精度居于兩者之間。但是3種算法誤差處于同一量級，相差不大。

(2)數(shù)據(jù)離線處理的過程中，用Matlab工具統(tǒng)計各算法的時間開銷分別為直接卡爾曼濾波90.264 211 s、間接卡爾曼濾波法183.947 936 s、互補(bǔ)濾波器方法40.540 979 s。間接卡爾曼濾波法時間開銷最大，計算量最大，互補(bǔ)濾波器時間開銷最小，計算量最小。直接卡爾曼濾波法雖然狀態(tài)維數(shù)比間接卡爾曼濾波多一維，但是由于省去了SINS姿態(tài)解算的步驟，計算量小于間接卡爾曼濾波法。

表1 姿態(tài)誤差統(tǒng)計結(jié)果 (°)

(3)KF濾波需要知道精確的濾波模型和濾波參數(shù)，如果參數(shù)設(shè)置不當(dāng)，或者模型誤差大，可能出現(xiàn)濾波發(fā)散?；パa(bǔ)濾波法利用比例積分控制，使姿態(tài)和航向趨于穩(wěn)定，誤差不會累積發(fā)散，魯棒性好。

(4)和KF算法相比，互補(bǔ)濾波器算法簡單、運(yùn)算量小，實(shí)時性更好。若AHRS硬件平臺采用計算能力有限的單片機(jī)，采用互補(bǔ)濾波器更合適。

4 結(jié)束語

本文對AHRS系統(tǒng)數(shù)據(jù)融合算法進(jìn)行研究，首先對工程中廣泛應(yīng)用AHRS算法，包括直接卡爾曼濾波法、間接卡爾曼濾波法、互補(bǔ)濾波器法，進(jìn)行了詳細(xì)介紹，最后利用所設(shè)計的AHRS系統(tǒng)進(jìn)行三軸轉(zhuǎn)臺測試，利用采集的MIMU數(shù)據(jù)對以上3種算法進(jìn)行試驗(yàn)驗(yàn)證。根據(jù)試驗(yàn)結(jié)果，對3種算法的精度、計算量和實(shí)時性進(jìn)行分析比較。和KF算法相比，互補(bǔ)濾波器算法簡單、運(yùn)算量小，實(shí)時性更好，適用于單片機(jī)的AHRS硬件平臺。