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

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

0 引言

基于低成本MEMS(Micro-Electro-Mechanical System)陀螺儀、加速度計(jì)以及微磁強(qiáng)計(jì)的航姿系統(tǒng)是一種自主式的載體姿態(tài)測量系統(tǒng)，具有體積小、功耗低、成本低的優(yōu)勢，廣泛應(yīng)用于小型飛行器、智能機(jī)器人以及人體的運(yùn)動(dòng)分析。在該類應(yīng)用中存在載體運(yùn)動(dòng)機(jī)動(dòng)性強(qiáng)和運(yùn)動(dòng)環(huán)境磁干擾較大的情況，低成本的MEMS傳感器精度低、漂移大且具有高噪聲，在載體運(yùn)動(dòng)機(jī)動(dòng)性強(qiáng)時(shí)加速度計(jì)產(chǎn)生的擾動(dòng)加速度將會過大，而磁強(qiáng)計(jì)易受到環(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)境中無法工作;而基于卡爾曼濾波及其改進(jìn)版本的姿態(tài)求解方案模型多較為復(fù)雜，運(yùn)算量較大;基于互補(bǔ)濾波器的姿態(tài)解算方法實(shí)現(xiàn)簡單但無法針對小的擾動(dòng)進(jìn)行在線的信息補(bǔ)償，故該算法的精度也會相應(yīng)下降;梯度下降法求解姿態(tài)的方案只有簡單的加法和乘法，已被證實(shí)是一種高效的姿態(tài)求解方案，但該算法無法對傳感器數(shù)據(jù)的誤差進(jìn)行在線補(bǔ)償，在傳感器數(shù)據(jù)誤差較大時(shí)姿態(tài)解算精度會大幅下降，具有一定的局限性。卡爾曼濾波求解姿態(tài)算法可對數(shù)據(jù)誤差進(jìn)行在線補(bǔ)償，但其姿態(tài)解算模型復(fù)雜，運(yùn)算量大，不易于實(shí)現(xiàn)[4-6]。因此，綜合各方法特點(diǎn)，設(shè)計(jì)具有抗機(jī)動(dòng)性擾動(dòng)功能的新型航姿算法則變得具有一定意義。

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

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

圖1 方案總體流程圖

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

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

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

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

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

2.1 卡爾曼濾波化簡

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

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

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

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

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

2.2 傳感器誤差補(bǔ)償

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

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

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

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

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

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

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

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

2.2.2 加速度誤差觀測量

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

2.2.3 磁強(qiáng)誤差協(xié)方差

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

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

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

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

2.2.4 磁強(qiáng)誤差觀測量

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

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

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

實(shí)驗(yàn)器件安裝如圖2所示，AHRS-Ⅱ三軸精度均在0.1°左右。為保證測試結(jié)果的有效性，AHRS-Ⅱ和本文的航姿解算硬件模塊通過PVC透明底板固連，并利用AHRS-Ⅱ?qū)?shí)驗(yàn)用傳感器進(jìn)行了標(biāo)定。

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

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

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

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

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

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

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

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

4 結(jié)論

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