載體機動和磁干擾環(huán)境下的航姿算法研究

李乃星， 曹培培， 劉圣起， 于 歡

(上海無線電設(shè)備研究所，上海201109)

0 引言

基于低成本MEMS(Micro-Electro-Mechanical System)陀螺儀、加速度計以及微磁強計的航姿系統(tǒng)是一種自主式的載體姿態(tài)測量系統(tǒng)，具有體積小、功耗低、成本低的優(yōu)勢，廣泛應(yīng)用于小型飛行器、智能機器人以及人體的運動分析。在該類應(yīng)用中存在載體運動機動性強和運動環(huán)境磁干擾較大的情況，低成本的MEMS傳感器精度低、漂移大且具有高噪聲，在載體運動機動性強時加速度計產(chǎn)生的擾動加速度將會過大，而磁強計易受到環(huán)境磁干擾而使其數(shù)據(jù)精度下降甚至完全不可用，以上問題均將導(dǎo)致姿態(tài)信息不可靠[1-3]。

針對以上問題，目前常用的解決方案主要有以下幾類：將航姿系統(tǒng)和衛(wèi)星系統(tǒng)組合使載體保持較高的姿態(tài)精度，但這需要額外增加衛(wèi)星導(dǎo)航模塊，且衛(wèi)星模塊在信號遮擋環(huán)境中無法工作;而基于卡爾曼濾波及其改進版本的姿態(tài)求解方案模型多較為復(fù)雜，運算量較大;基于互補濾波器的姿態(tài)解算方法實現(xiàn)簡單但無法針對小的擾動進行在線的信息補償，故該算法的精度也會相應(yīng)下降;梯度下降法求解姿態(tài)的方案只有簡單的加法和乘法，已被證實是一種高效的姿態(tài)求解方案，但該算法無法對傳感器數(shù)據(jù)的誤差進行在線補償，在傳感器數(shù)據(jù)誤差較大時姿態(tài)解算精度會大幅下降，具有一定的局限性?？柭鼮V波求解姿態(tài)算法可對數(shù)據(jù)誤差進行在線補償，但其姿態(tài)解算模型復(fù)雜，運算量大，不易于實現(xiàn)[4-6]。因此，綜合各方法特點，設(shè)計具有抗機動性擾動功能的新型航姿算法則變得具有一定意義。

1 姿態(tài)解算方案

針對上述問題，本課題提出利用自適應(yīng)卡爾曼濾波對傳感器測量值進行預(yù)處理，利用梯度下降法完成對處理后測量值的姿態(tài)解算與姿態(tài)融合的姿態(tài)解算方案，使航姿系統(tǒng)具有了抗機動性擾動能力，方案流程圖如下所示。

圖1 方案總體流程圖

A HRS系統(tǒng)的姿態(tài)解算過程依賴于加速度計等傳感器提供的信息，為提升算法實用性，文章采用只有加法和乘法運算的四元數(shù)法進行姿態(tài)描述。繞導(dǎo)航坐標系的三個坐標軸x、y、z 軸旋轉(zhuǎn)的三個角分別為橫滾角θ，俯仰角γ航向角Ψ。文章采用二階龍格庫塔法利用陀螺提供的角速度數(shù)據(jù)進行四元數(shù)微分運算，計算公式如下所示。

式中：q 為四元數(shù)模型;T 是數(shù)據(jù)更新周期;Ωb為角速度轉(zhuǎn)置矩陣，三軸姿態(tài)角可利用上述四元數(shù)進行描述，針對四元數(shù)進行更新即可完成對姿態(tài)角的更新[7]。

綜合考慮低成本MEMS 傳感器的特性和算法的實用性，文章采用了梯度下降法將陀螺儀、加速度計和磁強計的傳感器數(shù)據(jù)進行融合來提供載體最終的姿態(tài)結(jié)果，計算公式如下。

式中：q 為四元數(shù)矩陣;β 為在梯度下降方向上更新的步長;f 表示基于不同傳感器解算出的姿態(tài)角差值;J 為多元向量f 的導(dǎo)數(shù)。根據(jù)如上所示的梯度下降融合算法，既可以將MEMS陀螺儀、加速度計和磁強計的傳感器信息進行融合[8-11]，從而實時的得到更加準確的姿態(tài)信息。

2 關(guān)鍵技術(shù)實現(xiàn)

2.1 卡爾曼濾波化簡

引卡爾曼濾波算法能從隨機的干擾當(dāng)中利用量測值信息估計出所需的最優(yōu)信息。傳統(tǒng)卡爾曼濾波的五個遞推公式如下所示。

基于此方法的原理，本課題將傳統(tǒng)的卡爾曼濾波器模型中的五個基本公式精簡為如下所示的四個公式。

式中：P 為狀態(tài)量的協(xié)方差矩陣;Q 為狀態(tài)轉(zhuǎn)移協(xié)方差矩陣;即觀測量協(xié)方差矩陣;R 為測量值噪聲協(xié)方差矩陣;Kg 為卡爾曼濾波增益;I 為單位矩陣;X 為濾波的輸入和輸出量;Z 為實際測量值。

由于濾波的狀態(tài)量與觀測量均為加速度和磁強數(shù)據(jù)，傳統(tǒng)卡爾曼模型中的系數(shù)矩陣A 設(shè)置為單位陣，系數(shù)矩陣B 設(shè)置為零矩陣，則濾波的狀態(tài)一步預(yù)測過程等式兩側(cè)相等，即可省略。故濾波更新過程可直接先對協(xié)方差P(k|k-1)進行更新，矩陣Q 為白噪聲。利用上述公式的關(guān)系即可實現(xiàn)對協(xié)方差P 和卡爾曼增益Kg 的更新，同時，本課題將觀測量的更新過程與估計值的導(dǎo)入過程合為一步，得到數(shù)據(jù)最終估計值X(k)[12]。

文章提出的簡化濾波器模型在不降低運算精度的前提下，精簡了運算步驟。且該模型中應(yīng)用的矩陣均為對角矩陣，一定程度上減小了濾波過程的運算量，提升了文章提出的姿態(tài)解算方案的實用性。

2.2 傳感器誤差補償

上述公式中R(k)為加速度和磁強實測數(shù)據(jù)對應(yīng)的協(xié)方差的值，該值隨實測數(shù)據(jù)的變化而變化，文章根據(jù)兩種傳感器輸出的數(shù)據(jù)特點，定義了R(k)的更新方式，根據(jù)其特點提出了針對兩種傳感器輸出數(shù)據(jù)統(tǒng)計量，并根據(jù)該統(tǒng)計量對傳感器輸出的數(shù)據(jù)進行篩選，從而去除因機動性擾動而產(chǎn)生的誤差過大的傳感器輸出的數(shù)據(jù)，提升了航姿系統(tǒng)的機動性與抗磁干擾能力。

2.2.1 加速度誤差協(xié)方差

根據(jù)MEMS加速度計輸出數(shù)據(jù)的特點，可以將加速度輸出模型簡化如下。

式中：g為重力加速度;am為傳感器實際的運動加速度;ea為MEMS加速度計器件輸出數(shù)據(jù)所對應(yīng)的誤差，可以認為是白噪聲[13]。

當(dāng)載體以一定的運動加速度做相對穩(wěn)定的機動性運動時，可以認為相鄰時刻的運動加速度變化在一定的范圍之內(nèi)，可將運動加速度am更新的數(shù)學(xué)模型定義如下。

式中：Ca為量綱常數(shù)，其取值在0 到1 之間;am，k和am，k+1分別為k時刻和下一時刻的運動加速度的值;Δa 為運動加速度在相鄰時間內(nèi)的變化[14]。

此加速度模型可應(yīng)用在卡爾曼濾波器的數(shù)學(xué)模型中，即由上一時刻濾波后得到的運動加速度的最優(yōu)估計值來反映出下一時刻的運動加速度的先驗值，該估計過程如下。

因存在重力加速度，加速度計無法直接測量運動加速度。而在高頻的數(shù)據(jù)采集過程中，相鄰時刻的加速度測量值也可認為在一個相同的領(lǐng)域內(nèi)變化，故傳感器實際輸出的加速度數(shù)據(jù)ai，k可以準確的反映出運動加速度am，k的變化情況。故可將公式中的am，k信息利用ai，k來代替，得到加速度測量值與估計值之間的關(guān)系式如下。

2.2.2 加速度誤差觀測量

狀態(tài)方程只包含加速度信息，故協(xié)方差矩陣Ra，k為對角陣，且各元素均為非負值。本課題針對加速度數(shù)據(jù)的協(xié)方差矩陣Ra，k的特點設(shè)置了統(tǒng)計量La，k，令該統(tǒng)計量的值為協(xié)方差矩陣對角線元素的相加的和(為避免對角陣元素相乘結(jié)果抵消的現(xiàn)象，故不采取對角線元素相乘的方法)，當(dāng)載體進行穩(wěn)定的機動性運動時，La，k將穩(wěn)定在一定閾值內(nèi)。設(shè)載體在開始穩(wěn)定運動前幾秒的統(tǒng)

2.2.3 磁強誤差協(xié)方差

與MEMS加速度計的數(shù)學(xué)模型類似，電子磁強計輸出數(shù)據(jù)的數(shù)學(xué)模型可以定義如下。

式中：h 為當(dāng)?shù)氐卮艌鲈谒矫嫱队暗膮⒖枷蛄?em為磁強計器件的誤差噪聲，可認為是白噪聲。磁強計輸出數(shù)據(jù)的更新方程如下所示。

式中：Cm為磁強計數(shù)據(jù)更新過程所對應(yīng)的量綱常數(shù);Δm 為相鄰時間內(nèi)磁強向量在各軸投影值的變化。磁強計測量值的最優(yōu)估計值和其下一時刻先驗值的關(guān)系如下。

根據(jù)前文的分析可以得到磁強計輸出的測量數(shù)據(jù)的協(xié)方差Rm，k的表達式如下。

2.2.4 磁強誤差觀測量

3 實驗測試與結(jié)果分析

為驗證算法的可行性，課題針對設(shè)計的航姿系統(tǒng)進行了實物實現(xiàn)，并設(shè)計了機動性實驗?zāi)M磁干擾環(huán)境和強機動運動環(huán)境。由于互補濾波姿態(tài)解算算法在姿態(tài)解算應(yīng)用中已被證實是一種高效的姿態(tài)求解方案，故此實驗也將解算結(jié)果和互補濾波姿態(tài)解算算法的解算結(jié)果進行了對比，從而得出算法解算效果的評估。

系統(tǒng)硬件實現(xiàn)應(yīng)用的傳感器為MPU6050和HMC5883磁強計，MPU6050模塊中含有三軸陀螺儀和三軸加速度計，各傳感器數(shù)據(jù)通過STM32F103 開發(fā)板采集，數(shù)據(jù)輸出頻率為50 Hz。姿態(tài)解算參考值來自Inertial Labs公司的AHRS-Ⅱ航姿系統(tǒng)。

實驗器件安裝如圖2所示，AHRS-Ⅱ三軸精度均在0.1°左右。為保證測試結(jié)果的有效性，AHRS-Ⅱ和本文的航姿解算硬件模塊通過PVC透明底板固連，并利用AHRS-Ⅱ?qū)嶒炗脗鞲衅鬟M行了標定。

為模擬航姿系統(tǒng)在有強磁干擾的環(huán)境下進行機動運動的情況，本課題針對機動性實驗做了如下設(shè)計。在航姿系統(tǒng)運動過程中，將帶有強磁性的器件靠近運動載體以模擬磁干擾環(huán)境，同時將載體進行高頻高速的隨機運動，使其發(fā)生較大較快的姿態(tài)變化，運動加速度峰值為1 g。實驗時常約為2 400 s，將采集到的數(shù)據(jù)通過MATLAB進行分析，得到了加速度計誤差統(tǒng)計量La，t和磁強計誤差統(tǒng)計量Lm，t的變化如圖3所示。

圖3 誤差統(tǒng)計量

由上圖可知，在(800～900)s等時刻航姿系統(tǒng)做機動運動，在(1 100～1 300)s時載體進行強機動運動，故此時加速度誤差統(tǒng)計量La，k驟增。在(1 400～2 000)s之間磁強計受到較大的磁干擾，導(dǎo)致磁強數(shù)據(jù)變得不可用。

姿態(tài)解算結(jié)果如圖4所示(星點線為AHRS-Ⅱ輸出的參考值，實線為本文算法的解算結(jié)果，虛線為互補濾波算法的解算結(jié)果)。

將本文算法和互補濾波算法的姿態(tài)結(jié)果分別和AHRS-Ⅱ提供的姿態(tài)參考值進行做差對比，結(jié)果統(tǒng)計如表1所示。

表1 姿態(tài)結(jié)果誤差對比

實驗結(jié)果表明，載體在磁干擾較強的環(huán)境下進行強機動運動時，與互補濾波姿態(tài)解算法相比，本課題設(shè)計的姿態(tài)解算方案提供的橫滾角均方誤差減小了9%，俯仰角均方誤差減小了10%，航向角均方誤差減小了38%，使航姿系統(tǒng)輸出姿態(tài)的精度得到了提升。

圖4 姿態(tài)角變化示意圖

4 結(jié)論

文章針對載體在磁干擾較多的環(huán)境下進行長時間的強機動運動的情況，結(jié)合AHRS航姿系統(tǒng)小型化、低成本的工程實現(xiàn)要求，設(shè)計了一種姿態(tài)解算算法。該方法利用卡爾曼濾波器對加速度計和磁強計傳感器信息進行預(yù)處理，建立四元數(shù)模型描述姿態(tài)，利用梯度下降法對多種傳感器信息進行融合與解算，以得到最后的姿態(tài)信息。實驗結(jié)果表明，此算法能利用自身信息對數(shù)據(jù)進行處理，提升載體在磁干擾較強環(huán)境下的強機動運動時的姿態(tài)精度，且此算法簡化了卡爾曼濾波器模型，結(jié)合梯度下降法使得算法的運算量更小，對姿態(tài)確定的相關(guān)工程應(yīng)用具有一定意義。