低成本慣導(dǎo)系統(tǒng)預(yù)處理和姿態(tài)解算

毛紅瑛，陳至坤，張瑞成

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

0 引言

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

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

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

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

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

在慣性導(dǎo)航系統(tǒng)中，為了更好的說明一下載體的姿態(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)實際上就是一個旋轉(zhuǎn)過程，因此載體的姿態(tài)就是載體坐標(biāo)系相對于地理坐標(biāo)系東北天之間關(guān)系，通常使用四元數(shù)法、歐拉角法、方向余弦矩陣、旋轉(zhuǎn)矢量等方法對旋轉(zhuǎ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)中，方向余弦矩陣法運算量大，歐拉角法存在萬向節(jié)死鎖的現(xiàn)象。為了更方便的描述剛體的運動，通常使用四元數(shù)法，由于四元數(shù)法具有最佳性能：不僅可以避免繞固定坐標(biāo)軸旋轉(zhuǎn)順序所造成的萬向節(jié)死鎖現(xiàn)象，而且在精度高的同時還可以簡化計算量，因此將載體坐標(biāo)系相對于地理坐標(biāo)系的旋轉(zhuǎn)變化通過四元數(shù)用符號q表示，具體形式為：

q=q0+q1i+q2j+q3k

(3)

(4)

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

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

2.1 陀螺儀誤差模型

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

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

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

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

(6)

根據(jù)公式：

(7)

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

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

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

(8)

2.2 加速度誤差模型

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

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

1.59a(t-1)+0.01

(9)

3 擴展卡爾曼濾波解算過程

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

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

(10)

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

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

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

(11)

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

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

(12)

式中T為采樣周期。

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

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

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

(13)

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

(14)

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

(15)

式中：

(16)

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

(17)

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

(18)

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

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

(19)

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

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

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

首先需要降低有害加速度，第一級測量噪聲為

(20)

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

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

(21)

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

(22)

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

5 實驗與結(jié)果分析

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

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

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

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

觀測方程yk=Ckyk+Vk

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

(23)

(24)

C=[1 0]

(25)

圖1 陀螺儀隨機漂移誤差

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

圖2 加速度計隨機漂移誤差

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

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

5.2 姿態(tài)解算實驗

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

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

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

(a)

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

(a)

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

6 結(jié)束語

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