戚文昊， 劉 寧， 蘇 中， 喬利康， 王靖驍

(1.北京信息科技大學(xué)高動態(tài)導(dǎo)航技術(shù)北京市重點實驗室，北京 100000；2.現(xiàn)代測控技術(shù)教育部重點實驗室，北京 100000)

0 引言

飛行器姿態(tài)解算對飛行器的導(dǎo)航與控制具有重要意義，基于IMU的姿態(tài)解算方法精度低、噪聲大，且無法準(zhǔn)確測量飛行器在偏航方向上的姿態(tài)變化，而采用多傳感器數(shù)據(jù)融合的方法能有效提高飛行器姿態(tài)解算精度[1]。目前，由陀螺儀、加速度計和磁強計組成的MARG傳感器系統(tǒng)受到廣泛關(guān)注，對MARG傳感器數(shù)據(jù)進(jìn)行互補融合，實現(xiàn)飛行器姿態(tài)參數(shù)的最佳估計，是當(dāng)前姿態(tài)解算的重要研究內(nèi)容[2]。

目前，飛行器姿態(tài)解算的數(shù)據(jù)融合方法主要有互補濾波算法、擴(kuò)展卡爾曼濾波(EKF)、無跡卡爾曼濾波(UKF)和容積卡爾曼濾波等[3]。其中，典型的互補濾波算法簡單可靠，對傳感模塊的精度要求較低，可以綜合多傳感器各自頻率響應(yīng)優(yōu)勢，以減小姿態(tài)測量和估計的偏差，但無法確定高通與低通濾波器截止頻率，因此精度不高[4]；EKF是應(yīng)用最為廣泛的一種非線性濾波方法，但存在線性化誤差、截斷誤差以及容易發(fā)散等問題[5-6]；在文獻(xiàn)[7-9]中，將UKF應(yīng)用于姿態(tài)解算，在收斂速度和估計精度中要高于EKF，但其計算量要比EKF大，并且它們的系統(tǒng)噪聲和觀測噪聲不易確定，解算的姿態(tài)角容易受到噪聲干擾；文獻(xiàn)[10]中提到的容積卡爾曼濾波雖然精度高于EKF，但對計算能力提出了更高的要求。

本文提出一種基于共軛梯度算法的MARG傳感器姿態(tài)解算方法，能夠在梯度下降算法的基礎(chǔ)上提高收斂速度，又可以避免卡爾曼濾波算法計算量大的缺點。利用加速度計和磁強計建立飛行器姿態(tài)誤差函數(shù)，采用共軛梯度方法求解誤差函數(shù)最小值，對飛行器姿態(tài)四元數(shù)進(jìn)行最優(yōu)估計，修正更新陀螺儀測量的飛行器姿態(tài)，最終將陀螺儀測量的飛行器姿態(tài)四元數(shù)和共軛梯度算法求解的姿態(tài)四元數(shù)進(jìn)行加權(quán)融合，解算出姿態(tài)角。

1 四元數(shù)姿態(tài)表示

1.1 坐標(biāo)系建立

上述方法在姿態(tài)解算中存在奇異性問題，本文采用四元數(shù)姿態(tài)表達(dá)式。在四元數(shù)姿態(tài)表達(dá)式中，一個坐標(biāo)系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考坐標(biāo)系中的方向矢量r進(jìn)行單次轉(zhuǎn)動角度α來實現(xiàn)。坐標(biāo)系轉(zhuǎn)換示意圖見圖1。

圖1 坐標(biāo)系轉(zhuǎn)換示意圖Fig.1 Diagram of coordinate system conversion

1.2 四元數(shù)姿態(tài)更新

陀螺儀在載體坐標(biāo)系下三軸的角速率輸出分別為ωx，ωy和ωz，轉(zhuǎn)換為四元數(shù)形式可以表示為bω=[0ωxωyωz]。

(1)

(2)

式中，Δt為采樣周期。

2 姿態(tài)解算方法

2.1 傳感器模型

(3)

(4)

(5)

2.2 共軛梯度算法

根據(jù)傳感器模型得到加速度計和磁強計各自的誤差函數(shù)，在共軛梯度算法中，為了尋找目標(biāo)誤差函數(shù)的極值，需要計算誤差函數(shù)fg,▽對應(yīng)的雅可比矩陣Jg,▽

(6)

最終可以采用共軛梯度法進(jìn)行姿態(tài)四元數(shù)更新，即

(7)

式中：λt-1為姿態(tài)更新的最優(yōu)步長；dt-1為姿態(tài)更新的搜索方向，算式如下[14]

(8)

式中,gt為目標(biāo)函數(shù)在t時刻的迭代梯度。為了使共軛梯度方法計算得到的姿態(tài)四元數(shù)接下來可以與陀螺儀估計的姿態(tài)四元數(shù)進(jìn)行加權(quán)融合，通過共軛梯度方法更新姿態(tài)四元數(shù)時每次采樣只迭代一次。

2.3 數(shù)據(jù)融合

(9)

式中，γt為融合加權(quán)系數(shù)。為了使共軛梯度算法的收斂速度與陀螺儀解算姿態(tài)的發(fā)散速度相同，即(1-γt)β=γtλt/Δt，則應(yīng)使姿態(tài)更新的步長λt盡可能大，γt可化簡為

(10)

(11)

圖2 共軛梯度算法流程圖Fig.2 Flow chart of conjugate gradient method

3 仿真試驗與結(jié)果分析

為了驗證所提姿態(tài)測量方法的有效性和準(zhǔn)確性，設(shè)計一種包含加速度計、陀螺儀、磁強計的MARG傳感器系統(tǒng)的測量裝置。測量裝置和試驗環(huán)境如圖3所示。

圖3 測量裝置和試驗環(huán)境Fig.3 Measuring device and test environment

3.1 靜態(tài)試驗

將測量裝置靜止放置于隔振臺上，靜止采集數(shù)據(jù)45 min，采用不同方法對測量裝置姿態(tài)角進(jìn)行解算，計算并比較不同算法在靜止?fàn)顟B(tài)下的姿態(tài)角偏移誤差。不同算法的姿態(tài)解算結(jié)果如圖4所示；計算并匯總靜態(tài)試驗下兩種方法的誤差如表1所示。

圖4 不同算法的姿態(tài)解算結(jié)果

表1 靜態(tài)試驗姿態(tài)解算誤差

由該組靜態(tài)試驗對比可以得到，靜態(tài)時共軛梯度算法的波動范圍和標(biāo)準(zhǔn)差均小于傳統(tǒng)互補濾波算法，說明共軛梯度算法相比傳統(tǒng)姿態(tài)解算方法具有更強的穩(wěn)定性。

3.2 動態(tài)試驗

將測量裝置固連于三軸轉(zhuǎn)臺中心，控制三軸轉(zhuǎn)臺圍繞測量裝置的x，y，z軸依次轉(zhuǎn)動至-80°，80°位置，重復(fù)兩次。分別采用共軛梯度法、互補濾波法、擴(kuò)展卡爾曼濾波(EKF)以及角速率積分方法對測試數(shù)據(jù)進(jìn)行姿態(tài)解算，姿態(tài)解算結(jié)果如圖5所示。為了便于區(qū)分，不同算法在縱向進(jìn)行了適當(dāng)偏移。

圖5 姿態(tài)解算結(jié)果

分別計算不同算法解算姿態(tài)角與真實值之間的誤差，得到橫滾角、俯仰角和偏航角的誤差對比，如圖6所示。

圖6 姿態(tài)解算誤差

由圖6可以得到，本文提出的共軛梯度算法可以有效抑制陀螺儀輸出姿態(tài)的發(fā)散問題，相比于互補濾波算法，具有更好的動態(tài)性能，收斂速度更快，解算精度與EKF相差不大，但能夠有效降低系統(tǒng)運算壓力。最后，計算并匯總3種算法的均方根誤差(RMSE)，如表2所示。

表2 動態(tài)試驗姿態(tài)解算RMSE

4 結(jié)論

本文提出了一種共軛梯度算法，用于解算MARG傳感器系統(tǒng)的姿態(tài)，利用加速度計和磁強計構(gòu)建誤差函數(shù)，對姿態(tài)四元數(shù)進(jìn)行補償和修正，并對多傳感器數(shù)據(jù)進(jìn)行加權(quán)融合，以提高姿態(tài)解算精度。試驗結(jié)果表明，相較于以IMU為基礎(chǔ)的姿態(tài)解算方法，本文提出的算法在靜態(tài)時具有良好的抑制噪聲和抑制漂移的能力，在動態(tài)環(huán)境下具有更好的性能及更高的解算精度。