基于共軛梯度的MARG傳感器姿態(tài)解算方法

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

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

0 引言

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

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

本文提出一種基于共軛梯度算法的MARG傳感器姿態(tài)解算方法，能夠在梯度下降算法的基礎(chǔ)上提高收斂速度，又可以避免卡爾曼濾波算法計(jì)算量大的缺點(diǎn)。利用加速度計(jì)和磁強(qiáng)計(jì)建立飛行器姿態(tài)誤差函數(shù)，采用共軛梯度方法求解誤差函數(shù)最小值，對(duì)飛行器姿態(tài)四元數(shù)進(jìn)行最優(yōu)估計(jì)，修正更新陀螺儀測量的飛行器姿態(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á)式中，一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考坐標(biāo)系中的方向矢量r進(jìn)行單次轉(zhuǎn)動(dòng)角度α來實(shí)現(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ù)傳感器模型得到加速度計(jì)和磁強(qiáng)計(jì)各自的誤差函數(shù)，在共軛梯度算法中，為了尋找目標(biāo)誤差函數(shù)的極值，需要計(jì)算誤差函數(shù)fg,▽對(duì)應(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時(shí)刻的迭代梯度。為了使共軛梯度方法計(jì)算得到的姿態(tài)四元數(shù)接下來可以與陀螺儀估計(jì)的姿態(tài)四元數(shù)進(jìn)行加權(quán)融合，通過共軛梯度方法更新姿態(tài)四元數(shù)時(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 仿真試驗(yàn)與結(jié)果分析

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

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

3.1 靜態(tài)試驗(yàn)

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

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

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

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

3.2 動(dòng)態(tài)試驗(yàn)

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

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

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

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

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

表2 動(dòng)態(tài)試驗(yàn)姿態(tài)解算RMSE

4 結(jié)論

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