毛紅瑛，陳至坤，張瑞成

(華北理工大學(xué)電氣工程學(xué)院，河北唐山 063210)

0 引言

慣性傳感器和磁阻傳感器進(jìn)行姿態(tài)解算最早被應(yīng)用在導(dǎo)航領(lǐng)域中。近年來(lái)，慣性傳感器的發(fā)展速度日益加快，不斷向更小的體積，更輕的質(zhì)量、更便捷，以及更低的功耗方向發(fā)展。由于其價(jià)格低廉，也被大量的應(yīng)用于無(wú)人機(jī)，游戲機(jī)和虛擬現(xiàn)實(shí)等眾多領(lǐng)域中。

慣性傳感器姿態(tài)測(cè)量系統(tǒng)是由三部分組成，分別為三軸加速度計(jì)、三軸陀螺儀和三軸電子磁羅盤(pán)[1-2]，可以通過(guò)采集傳感器輸出的信息，利用慣性導(dǎo)航系統(tǒng)原理來(lái)測(cè)量運(yùn)動(dòng)載體的俯仰角、橫滾角和偏航角。目前常用的姿態(tài)測(cè)量方法有很多，本文基于慣性測(cè)量單元對(duì)姿態(tài)進(jìn)行求解。由于慣性測(cè)量單元有著不受氣候和空間條件的限制，同時(shí)適用于對(duì)測(cè)量精度和動(dòng)態(tài)性能較高的領(lǐng)域，因此與其他姿態(tài)測(cè)量方法相比有著較大的優(yōu)勢(shì)，但是由于選用的是低成本傳感器，陀螺儀和加速度計(jì)存在零偏和溫漂，三軸磁羅盤(pán)又容易受環(huán)境影響，因此長(zhǎng)時(shí)間工作必定會(huì)產(chǎn)生較大的累積誤差，針對(duì)MEMS慣性傳感器的誤差問(wèn)題，文獻(xiàn)[3]提出了一套零滯后補(bǔ)償?shù)臏y(cè)量方法來(lái)對(duì)自回歸滑動(dòng)平均系統(tǒng)的參數(shù)進(jìn)行估計(jì)，同時(shí)也提出了一種減小噪聲的算法來(lái)降低殘差自適應(yīng)濾波器的噪聲影響。文獻(xiàn)[4]中提出了一種隨機(jī)誤差建模補(bǔ)償方法，主要從確定建模樣本的長(zhǎng)度、求解模型參數(shù)、處理卡爾曼濾波器輸出結(jié)果和降低模型階數(shù)這些方面出發(fā)。由于每種傳感器都有自身的局限性，為了消除信息冗余，我們需要對(duì)多傳感信息融合進(jìn)行多層次多級(jí)別的信息自動(dòng)處理過(guò)程。在對(duì)姿態(tài)求解方面，最常用的方法有3種，分別是互補(bǔ)濾波法、卡爾曼濾波法和梯度下降法等，用不同的算法得到的解算效果也不盡相同。文獻(xiàn)[5]中介紹了自適應(yīng)互補(bǔ)濾波算法、梯度下降和互補(bǔ)濾波結(jié)合的這兩種算法，并分別從靜態(tài)和動(dòng)態(tài)兩方面對(duì)姿態(tài)解算的結(jié)果進(jìn)行比較。文獻(xiàn)[6]中設(shè)計(jì)了一種新的卡爾曼濾波方法，從測(cè)量方程出發(fā)，對(duì)其進(jìn)行處理。用一個(gè)線性測(cè)量方程表示，最后基于卡爾曼濾波方法對(duì)姿態(tài)進(jìn)行解算。文獻(xiàn)[7]中討論了2種姿態(tài)算法，梯度下降法和擴(kuò)展卡爾曼濾波算法來(lái)求解姿態(tài)，先利用梯度下降法對(duì)三軸加速度計(jì)和三軸磁羅盤(pán)進(jìn)行四元數(shù)的求解，通過(guò)陀螺儀可以求得一組姿態(tài)四元數(shù)，最后對(duì)兩者進(jìn)行融合，進(jìn)而取得最優(yōu)姿態(tài)。文獻(xiàn)[8]對(duì)Sage-Husa自適應(yīng)濾波算法進(jìn)行改進(jìn)，使算法結(jié)構(gòu)變得簡(jiǎn)單，通過(guò)遺忘因子對(duì)噪聲協(xié)方差進(jìn)行估計(jì)提高了速度和姿態(tài)角的解算效率和精度。文獻(xiàn)[9]介紹了Sage-Husa自適應(yīng)擴(kuò)展卡爾曼濾波算法，通過(guò)對(duì)測(cè)量噪聲的取值進(jìn)行改進(jìn)，提高了算法的魯棒性，對(duì)車輛的行駛狀態(tài)進(jìn)行了準(zhǔn)確的估計(jì)。

本文設(shè)計(jì)了一種IMU誤差模型，對(duì)慣性傳感器數(shù)據(jù)進(jìn)行靜態(tài)預(yù)處理的基礎(chǔ)上，通過(guò)構(gòu)建兩級(jí)噪聲方差陣的自適應(yīng)擴(kuò)展卡爾曼濾波融合方法降低了有害加速度的影響，提高了姿態(tài)角的解算精度。

1 姿態(tài)表示與解算過(guò)程

利用三軸加速度計(jì)來(lái)對(duì)載體的加速度進(jìn)行測(cè)量，三軸磁羅盤(pán)對(duì)磁場(chǎng)變化進(jìn)行測(cè)量，三軸陀螺儀對(duì)載體三個(gè)軸的角速率進(jìn)行測(cè)量。為了使求得的姿態(tài)角較為準(zhǔn)確，先對(duì)慣性器件的測(cè)得值進(jìn)行預(yù)處理，建立姿態(tài)角解算模型，最后使用自適應(yīng)擴(kuò)展卡爾曼算法進(jìn)行姿態(tài)角的計(jì)算。

在慣性導(dǎo)航系統(tǒng)中，為了更好的說(shuō)明一下載體的姿態(tài)，在此建立相應(yīng)的空間坐標(biāo)系，將載體坐標(biāo)系設(shè)為b系，地心慣性坐標(biāo)系設(shè)為i系，地理坐標(biāo)系設(shè)為n系，選取東北天為地理坐標(biāo)系，其中地理坐標(biāo)系的OnXn軸正方向指向正東方向，OnYn軸正方向指向正北方向，OnZn軸正方向垂直地面指向天空，并且符合右手定則原理，通常情況下，導(dǎo)航坐標(biāo)系就是地理坐標(biāo)系，姿態(tài)實(shí)際上就是一個(gè)旋轉(zhuǎn)過(guò)程，因此載體的姿態(tài)就是載體坐標(biāo)系相對(duì)于地理坐標(biāo)系東北天之間關(guān)系，通常使用四元數(shù)法、歐拉角法、方向余弦矩陣、旋轉(zhuǎn)矢量等方法對(duì)旋轉(zhuǎn)運(yùn)動(dòng)進(jìn)行描述，現(xiàn)規(guī)定偏航角用φ表示，是指載體坐標(biāo)系繞其Z軸旋轉(zhuǎn)方向的角度，范圍是0°～360°；俯仰角用θ表示，是指載體坐標(biāo)系繞其Y軸旋轉(zhuǎn)方向的角度，范圍從-90°～90°；橫滾角用γ表示，是指載體坐標(biāo)系繞其X軸旋轉(zhuǎn)方向的角度，范圍從-180°～180°。方向余弦矩陣表示的坐標(biāo)變換為

(1)

(2)

在慣性導(dǎo)航系統(tǒng)中，方向余弦矩陣法運(yùn)算量大，歐拉角法存在萬(wàn)向節(jié)死鎖的現(xiàn)象。為了更方便的描述剛體的運(yùn)動(dòng)，通常使用四元數(shù)法，由于四元數(shù)法具有最佳性能：不僅可以避免繞固定坐標(biāo)軸旋轉(zhuǎn)順序所造成的萬(wàn)向節(jié)死鎖現(xiàn)象，而且在精度高的同時(shí)還可以簡(jiǎn)化計(jì)算量，因此將載體坐標(biāo)系相對(duì)于地理坐標(biāo)系的旋轉(zhuǎn)變化通過(guò)四元數(shù)用符號(hào)q表示，具體形式為：

q=q0+q1i+q2j+q3k

(3)

(4)

2 傳感器數(shù)據(jù)的預(yù)處理

由于傳感器的測(cè)量結(jié)果受環(huán)境和其精度等因素的影響，其精度影響主要體現(xiàn)在安裝誤差、零偏誤差、刻度因數(shù)，同時(shí)在測(cè)量中會(huì)出現(xiàn)不可避免的噪聲和野值，若是直接進(jìn)行計(jì)算會(huì)出現(xiàn)較大的誤差，因此數(shù)據(jù)的預(yù)處理就顯得至關(guān)重要。

2.1 陀螺儀誤差模型

ARMA全稱為自回歸滑動(dòng)平均模型，是一種基于時(shí)間序列的分析方法，將MEMS陀螺儀隨機(jī)誤差用該方法進(jìn)行建模分析，首先要滿足平穩(wěn)正態(tài)、零均值的條件。然而通過(guò)采樣得到的隨機(jī)噪聲序列并不完全滿足，因此建模之前先對(duì)采集數(shù)據(jù)進(jìn)行去除偏移量和濾波處理。MEMS隨機(jī)誤差時(shí)序模型階次越高真實(shí)性就越高，計(jì)算難度和計(jì)算誤差也會(huì)隨著階數(shù)的升高而增大[10]。用p，q來(lái)表示模型階數(shù)，故ARMA(p,q)模型表示為：

x(t)=φ1x(t-1)+φ2x(t-2)+…+φpx(t-p)+
ε(t)-θ1ε(t-1)-θ2ε(t-2)-…-θqε(t-q)

(5)

式中：φ1,φ2,…,φp為自回歸系數(shù)；θ1,θ2,…,θq為移動(dòng)平均數(shù)；ε(t)為輸入的白噪聲。

綜合上述階次對(duì)模型的影響，通常將隨機(jī)誤差模型定在3階以內(nèi)，因此本文取p=2，q=1，在靜止?fàn)顟B(tài)下采集6 000個(gè)樣本數(shù)據(jù)，則ARMA(2,1)為

x(t)=φ1x(t-1)+φ2x(t-2)+

ε(t)-θ1ε(t-1)

(6)

根據(jù)公式：

(7)

式中：ρk為樣本自相關(guān)系數(shù)。

對(duì)樣本數(shù)據(jù)用最小二乘法進(jìn)行曲線擬合得到最終表達(dá)式：

ω(t)=-0.172ω(t-1)+0.135ω(t-2)-
0.571ε(t-1)+ε(t)

(8)

2.2 加速度誤差模型

利用最小二乘法對(duì)不同階的一元高階模型進(jìn)行擬合[11]，在靜止?fàn)顟B(tài)下采集6000個(gè)左右的加速度計(jì)誤差信號(hào)點(diǎn)，本文采用一元三階模型對(duì)加速度計(jì)誤差進(jìn)行補(bǔ)償，最終表達(dá)式如下所示：

a(t)=5.1×104a3(t-1)-530a2(t-1)+

1.59a(t-1)+0.01

(9)

3 擴(kuò)展卡爾曼濾波解算過(guò)程

卡爾曼濾波算法實(shí)際上是由預(yù)測(cè)和校正(或更新和修正)2部分組成。在預(yù)測(cè)過(guò)程中，當(dāng)前狀態(tài)的預(yù)測(cè)是通過(guò)濾波器對(duì)上一時(shí)刻的狀態(tài)估計(jì)求得。在校正過(guò)程中，為了獲得更準(zhǔn)確，接近真實(shí)值的新估計(jì)量，需要濾波器利用當(dāng)前時(shí)刻狀態(tài)的觀測(cè)量來(lái)修正預(yù)測(cè)階段獲得的預(yù)測(cè)量。

在實(shí)際應(yīng)用中，慣性導(dǎo)航系統(tǒng)是一個(gè)非線性系統(tǒng)[13]，因此此擴(kuò)展卡爾曼濾波算法的方程如下所示：

(10)

式中：xk為狀態(tài)向量；Zk為測(cè)量向量；Hk為三維常系數(shù)測(cè)量矩陣；B為已知的系統(tǒng)結(jié)構(gòu)參數(shù)；ωk-1為k-1時(shí)刻過(guò)程噪聲；Vk為k時(shí)刻測(cè)量噪聲。

根據(jù)以上建立的狀態(tài)方程和觀測(cè)方程，利用擴(kuò)展卡爾曼算法對(duì)狀態(tài)向量和協(xié)方差進(jìn)行預(yù)測(cè)，然后通過(guò)遞歸過(guò)程求得最優(yōu)四元數(shù)，最后對(duì)四元數(shù)進(jìn)行歸一化處理，計(jì)算姿態(tài)角。

根據(jù)陀螺儀在載體坐標(biāo)系下輸出的數(shù)據(jù)，和由初始姿態(tài)角計(jì)算得到初始四元數(shù)，列寫(xiě)如下四元數(shù)的微分方程[14]：

(11)

式中ωx,ωy,ωz為陀螺儀輸出的三軸角速率。

通過(guò)式(12)得到實(shí)時(shí)更新的四元數(shù)為

(12)

式中T為采樣周期。

為了得到較為準(zhǔn)確的姿態(tài)角，將姿態(tài)四元數(shù)q0(k)、q1(k)、q2(k)、q3(k)作為狀態(tài)量，列寫(xiě)狀態(tài)方程為

x(k) =[q0(k)q1(k)q2(k)q3(k)]T

=f(x(k-1),k-1)+w(k-1)

(13)

對(duì)函數(shù)f(x(k-1),k-1)求偏導(dǎo)得到的雅克比矩陣為A(x,k-1)，表示如下：

(14)

將加速度計(jì)的姿態(tài)解算方程作為測(cè)量方程，則觀測(cè)方程列寫(xiě)為：

(15)

式中：

(16)

對(duì)函數(shù)求h(x(k),k)求偏導(dǎo)得到的雅克比矩陣為

(17)

擴(kuò)展卡爾曼濾波算法的核心公式[15]如下：

(18)

式中：P(k|k-1)為x(k|k-1)預(yù)測(cè)誤差對(duì)應(yīng)的協(xié)方差；Q為系統(tǒng)噪聲協(xié)方差；R為測(cè)量噪聲協(xié)方差；Kk為卡爾曼增益；I為單位矩陣。

通過(guò)上述過(guò)程即可求得最優(yōu)估計(jì)四元數(shù)，對(duì)其進(jìn)行規(guī)范化處理得到：

(19)

再根據(jù)四元數(shù)轉(zhuǎn)化歐拉角的式(4)，得到最終的姿態(tài)角，實(shí)現(xiàn)姿態(tài)數(shù)據(jù)的融合。

4 自適應(yīng)擴(kuò)展卡爾曼濾波解算過(guò)程

從理論上說(shuō)，只有在準(zhǔn)確的知道系統(tǒng)的結(jié)構(gòu)參數(shù)和噪聲特性才能通過(guò)擴(kuò)展卡爾曼算法得到最優(yōu)估計(jì)值，由于在慣性導(dǎo)航系統(tǒng)中，這2個(gè)參數(shù)會(huì)存在誤差。因此通過(guò)自適應(yīng)算法對(duì)測(cè)量噪聲進(jìn)行處理，提高算法的精度。

首先需要降低有害加速度，第一級(jí)測(cè)量噪聲為

(20)

其中，通過(guò)試湊法確定權(quán)重因子ka，為k+1時(shí)刻的規(guī)范化加速度值‖a‖，當(dāng)僅有重力作用時(shí)，‖a‖=1，I為3×3的單位矩陣。

在使用慣性傳感器進(jìn)行測(cè)量時(shí)，測(cè)量噪聲并不是一成不變的，當(dāng)這種不確定性存在時(shí)，就可能會(huì)造成估計(jì)精度的下降，甚至有可能會(huì)發(fā)散[16]。而Sage-Husa自適應(yīng)卡爾曼濾波算法就是對(duì)運(yùn)動(dòng)過(guò)程中的載體的測(cè)量噪聲進(jìn)行較為準(zhǔn)確的估計(jì)，從而對(duì)協(xié)方差矩陣更新，使得姿態(tài)解算的精度更高。在這里使用指數(shù)漸消記憶自適應(yīng)卡爾曼濾波算法來(lái)實(shí)現(xiàn)測(cè)量噪聲的改變，計(jì)算第二級(jí)測(cè)量噪聲為

(21)

=ka(|‖a‖-1|)I+(1-βk)Rk-1+βk

(22)

因此，使用改進(jìn)后的量測(cè)噪聲，將其帶入式(18)所示的過(guò)程進(jìn)行解算，最終得到自適應(yīng)擴(kuò)展卡爾曼濾波算法，求得姿態(tài)角。

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

基于STM32F051K8微處理器系列芯片，選用ICM42605的三軸陀螺儀、三軸加速度計(jì)模塊搭建了實(shí)驗(yàn)平臺(tái)。將其安裝在無(wú)人機(jī)3p平臺(tái)進(jìn)行驗(yàn)證，將無(wú)人機(jī)輸出姿態(tài)角作為參考角度。整個(gè)實(shí)驗(yàn)放在遠(yuǎn)離磁場(chǎng)干擾的地方進(jìn)行。

5.1 數(shù)據(jù)預(yù)處理實(shí)驗(yàn)

將三軸陀螺儀固定好，然后對(duì)信號(hào)進(jìn)行除偏移和濾波處理，由式(8)的ARMA模型建立卡爾曼濾波器，將陀螺儀測(cè)量的實(shí)際漂移數(shù)據(jù)作為卡爾曼濾波器的狀態(tài)輸入，則有：

狀態(tài)方程x(k|k-1)=Ax(k|k-1)+Bwk-1

觀測(cè)方程yk=Ckyk+Vk

參數(shù)設(shè)置如下所示：

(23)

(24)

C=[1 0]

(25)

圖1 陀螺儀隨機(jī)漂移誤差

對(duì)于加速度計(jì)的處理與陀螺儀類似，根據(jù)式(9)誤差模型利用卡爾曼濾波進(jìn)行處理，得到濾波前后的誤差如圖2所示。同時(shí)陀螺儀和加速度計(jì)在濾波前后的數(shù)據(jù)對(duì)比如表1所示，對(duì)均值和標(biāo)準(zhǔn)差分別進(jìn)行說(shuō)明。

圖2 加速度計(jì)隨機(jī)漂移誤差

表1 濾波前后數(shù)據(jù)對(duì)比

由表1可以看出，陀螺儀的隨機(jī)漂移誤差均值由濾波前的0.008 62降到了0.0009 7，減小了一個(gè)數(shù)量級(jí)，同樣加速度計(jì)的隨機(jī)漂移誤差在均值和標(biāo)準(zhǔn)差方面也有明顯的降低。說(shuō)明了數(shù)據(jù)的預(yù)處理效果比較好，隨機(jī)漂移可以被有效抑制。

5.2 姿態(tài)解算實(shí)驗(yàn)

在預(yù)處理的基礎(chǔ)上，進(jìn)行擴(kuò)展卡爾曼濾波算法和自適應(yīng)擴(kuò)展卡爾曼濾波的姿態(tài)融合算法，得到最終的姿態(tài)角。設(shè)置采樣頻率為100 Hz，時(shí)間為3 min左右，驗(yàn)證算法的實(shí)際運(yùn)行效果。圖3和圖4分別為擴(kuò)展卡爾曼濾波算法和自適應(yīng)擴(kuò)展卡爾曼濾波的計(jì)算結(jié)果，表2為上述2種姿態(tài)解算與參考值相比產(chǎn)生的誤差數(shù)據(jù)分析。

表2 2種方法姿態(tài)誤差數(shù)據(jù)對(duì)比(°)

由圖3、圖4和表2的數(shù)據(jù)可以看出采用自適應(yīng)擴(kuò)展卡爾曼濾波算法得到的姿態(tài)角最大誤差都在3°左右，要比擴(kuò)展卡爾曼濾波算法的誤差小很多，因此，采用自適應(yīng)擴(kuò)展卡爾曼濾波算法可以得到較好的結(jié)果，提高了姿態(tài)角的解算精度。

(a)

(b)圖3 擴(kuò)展卡爾曼濾波算法結(jié)果對(duì)比

(a)

(b)圖4 自適應(yīng)擴(kuò)展卡爾曼濾波算法結(jié)果對(duì)比

6 結(jié)束語(yǔ)

本文將實(shí)驗(yàn)數(shù)據(jù)導(dǎo)入MATLAB中，并對(duì)結(jié)果進(jìn)行分析。主要針對(duì)慣性傳感器的系統(tǒng)誤差，易受環(huán)境干擾和陀螺儀的漂移等因素引起姿態(tài)發(fā)散的現(xiàn)象，在預(yù)處理的基礎(chǔ)上，采用對(duì)測(cè)量噪聲的兩級(jí)自適應(yīng)處理來(lái)提高計(jì)算精度。通過(guò)實(shí)驗(yàn)和仿真，將該算法與擴(kuò)展卡爾曼濾波進(jìn)行對(duì)比，結(jié)果表明，數(shù)據(jù)的預(yù)處理效果較好，基本上消除了加速度計(jì)和陀螺儀的干擾和野值，通過(guò)自適應(yīng)擴(kuò)展卡爾曼濾波算法的姿態(tài)解算，使得姿態(tài)角的精度進(jìn)一步提高，有效的解決了累計(jì)誤差的問(wèn)題，說(shuō)明該方法是一種高精度的姿態(tài)解算方法。