基于MEMS傳感器的慣導系統預處理和姿態解算

劉 維,王明杭,朱志宇

(江蘇科技大學電子與信息學院,江蘇 鎮江 212003)

隨著MEMS傳感器技術的飛快發展,MEMS器件以其成本低、尺寸小、功耗低等優勢逐漸被人們用到IMU領域,如在無人機、航姿參考系統、水下機器人、單兵導航等領域都有較為廣泛的應用[1],故本文探討的基于MEMS傳感器的慣導系統預處理和姿態解算是有重大意義的。

捷聯慣性導航系統(SINS)是不依靠外界信號,也不會向外界輻射信號的一種自主導航系統[2]。但是MEMS陀螺儀存在常值漂移,加速度具有零偏,而磁力計容易受到周圍環境的干擾,工作較長時間后這些器件會產生很大的誤差,造成姿態角的解算精度下降[3]。針對慣性器件誤差問題,文獻[4]等,解決的方法是建立IMU的誤差模型,使用卡爾曼濾波等算法求出誤差模型參數,以達到誤差補償目的,但建立準確的誤差模型和濾波時選對參數,存在計算量大的問題為了改進姿態角解算精度,文獻[5]設計出一種互補濾波的姿態融合算法,動態試驗下,滾動角誤差小于2.3°,俯仰角誤差小于3.1°,航向角誤差小于4°。精度不是太高。文獻[6]是直接對姿態角進行補償,首先推導并建立姿態角的誤差方程,使用EKF等算法求出誤差參數,以達到誤差補償目的,此方法姿態解算的精度高,滿足測姿要求,但是推導的姿態角的誤差方程比較繁瑣,而且狀態方程非線性,使用EKF需要計算雅可比矩陣,計算量加大,不利于實際使用。

本文設計一種基于優化自適應無跡卡爾曼濾波的姿態解算方法,在MEMS慣性傳感器經過預處理的基礎上,利用優化AUKF高精度特性,消除噪聲干擾提升姿態角解算的精度,進行了靜態預處理實驗和姿態解算跑車實驗,分別驗證了預處理的效果和姿態解算的性能。

1 SINS姿態角的解算過程

SINS的姿態角解算的具體過程如圖1所示,SINS是采用三軸加速度計、陀螺儀和磁力計分別測量載體的加速度、角速率和地磁場變化信息。由于MEMS傳感器測得值易受到誤差的干擾,造成姿態解算精度不高。為了得到較準確的姿態角信息,所以先對慣性器件測得值進行預處理,之后建立姿態角解算模型,再使用優化AUKF算法進行姿態角計算。

圖1 SINS姿態角的解算過程

2 傳感器數據的預處理

圖2 MEMS傳感器預處理過程

2.1 陀螺儀誤差模型

使用時間序列分析法對MEMS陀螺儀隨機誤差進行分析,建模之前先對采集數據進行去除偏移量和濾波處理后,將平穩、正態分布的隨機誤差用ARMA(p,q)模型表示[7]:

(1)

式中:xt為時間序列;φp是自回歸系數;θq滑動平均系數;εt為殘差,p,q為ARMA模型階次數[7],一般情況下,陀螺儀的溫漂誤差模型階數都不是太高,通常小于或等于三階,取p=2,q=1,采集100個陀螺儀隨機誤差信號點(Z軸),使用最小二乘法進行曲線擬合,得到其表達式:

xt=0.761 8xt-1+0.064 56xt-2-εt-0.977 4εt-1

(2)

2.2 加速度計誤差模型

采用一元高階多項式模型對MEMS加速度計信號進行建模補償[8],利用最小二乘法對不同階的模型參數擬合。首先采集靜止狀態下的100個加速度計誤差信號點(X軸),最小二乘法擬合的不同階模型參數如下:

表1 MEMS加速度計模型參數

設計一元高階模型時,通過調節模型的階次,對比補償前后加速度計的誤差,發現階次越高補償效果越好,四階以上的補償效果近似,故采用一元四階模型對加速度計誤差進行補償,具體模型如下:

(3)

3 基于優化AUKF的姿態解算

3.1 姿態角的提取

根據參考文獻[9]的姿態角微分方程可以使用下式表示:

(4)

(5)

式中:m=[mx,my,mz]T是磁力計的測量值。f=[fx,fy,fz]T是加速度信號。把式(4)求出的姿態信息對上式求得姿態角進行補償即可。

3.2 姿態角解算過程

為得到三個姿態角準確解算結果,選用三個姿態角作為狀態變量X=[θφψ],建立濾波狀態方程為:

(6)

式中:[wθwφwψ]T為系統的過程激勵噪聲向量。通過姿態算法解算出來的姿態角為Zk=[θkφkψk],可以反映出姿態角的變化可是實時性較低,故當作觀測量來更新系統的狀態,則觀測方程為:

(7)

式中:vk=[vθkvφkvψk]T是觀測噪聲向量。

4 優化AUKF算法

4.1 普通UKF存在的問題

第一,自適應問題。UKF相比較于EKF有較高精度,但是在實踐過程中,當UKF濾波器在選取初始值時,若初始值存在誤差干擾時,肯定會影響最優的結果[10-11]。第二,魯棒性差。在實際的工作中,在噪聲的驅使或干擾下,會造成濾波器的精度和穩定性下降。第三,對系統模型的結構問題,當系統模型存在誤差較大時,濾波器通過狀態最優估計消除噪聲和使用自適應因子對協方差進行縮放,但是效果不是很理想。

4.2 自適應因子的選取

影響自適應無跡卡爾曼濾波器性能的關鍵因素是自適應因子的選取,自適應因子ak與系統的協方差矩陣和殘差有關,初值為1,它的取值范圍是0≤ak≤1。如果ak取值合理,則它能夠平衡系統模型預測信息與測量信息之間的權比。ak按下式構造:

(8)

4.3 狀態噪聲自適應處理

當系統的模型誤差和外部噪聲比較大時,若只是依靠上面的自適應因子進行衰減,得到的最優狀態估計值并不是很理想,為得到準確的狀態值,需要對狀態噪聲做進一步的自適應處理。若觀測方程為線性的,假設在加性噪聲的干擾下,在濾波的過程中,狀態的協方差可以表示為:

(9)

(10)

在最優估計狀況下,dk為期望值0的高斯白噪聲[12],對兩邊取方差得:

(11)

根據上式引入縮放因子為:

(12)

(13)

5 實驗結果與分析

為了驗證預處理方法的效果和姿態解算的結果,本文進行了實驗。實驗使用基于某型號的MEMS微傳感器的捷聯慣導系統,高精度的光纖慣導(GIF6536A)并把它輸出姿態角當作真值,在室外遠離磁干擾的地方進行下面實驗。

5.1 預處理實驗

采集固定好的MEMS陀螺儀信號,以5 Hz連續采集5 min,對其輸出信號進行去除偏移量和濾波處理,處理后的角速度信號正態檢驗如圖3所示。

圖3 陀螺儀數據處理

圖3為靜態時陀螺儀的Z軸輸出,可以看出處理后的信號是一個平穩、正態分布的隨機序列。再由式(2)的ARMA模型,建立Kalman濾波器,參數設置如下:

式中:Φk,k-1是tk-1到tk時刻的轉移矩陣,Γ是系統噪聲矩陣,Q是過程噪聲協方差。得到的Kalman濾波前后的陀螺儀Z軸靜態隨機誤差如圖4和表2所示。

圖4 靜態實驗結果

實驗數據濾波前濾波后加速度計均值0.000 240.000 21信號/g標準差0.001 100.000 31陀螺儀信號均值-0.001 84-0.000 87/(°/s)標準差0.007 900.001 80

同樣根據式(3)描述的加速度誤差模型,進行Kalman濾波,濾波前后的加速度計信號誤差如圖5和表2所示。

圖5 加速度計的信號預處理

根據圖4和圖5可以很清楚的看到,經過Kalman濾波后的陀螺儀和加速度計誤差信號的變化幅度明顯較小。通過表2的MEMS傳感器誤差信號濾波前后的均值和標準差對比,也可以看出Kalman濾波后的信號分散程度明顯變小,其中濾波后的信號標準差比濾波前信號小了一個數量級,進一步說明預處理效果較好。

5.2 姿態解算跑車實驗

為驗證姿態角解算的精度,進行跑車實驗,本實驗以高精度的光纖慣導的姿態數據為基準數據(真值)。將實驗系統與高精度的慣導并排安裝在實驗車上,開動試驗車,兩套實驗系統同時測量試驗車的姿態信息,從開始到試驗車停止進行10 min左右的時間,車載實驗實物圖如圖6所示。把系統解算的姿態角與高精度慣導的姿態角做差得到動態實驗的姿態角誤差結果見圖7～圖9所示,表3為三種不同的濾波算法產生的姿態誤差數據分析。

圖6 車載實驗實物圖

圖7 滾動角誤差

圖8 俯仰角誤差

圖9 航向角誤差

實驗數據KFUKF優化AUKF滾動角誤差<2.7<2.4<2.3/(°)RMSE1.4521.3271.258俯仰角誤差<3.2<3<3/(°)RMSE1.8721.571.403航向角誤差<4.5<3.7<2.9/(°)RMSE3.5663.2983.205

從圖7～圖9和表3可以得到,動態實驗下采用優化AUKF解算得到的滾動角誤差小于2.3°,俯仰角誤差小于3°,航向角誤差小于2.9°,相比KF和UKF產生的姿態角誤差減小很多。同時計算動態實驗下姿態角的均方根誤差(RMSE),采用優化AUKF算法得到的滾動角、俯仰角和航向角的RMSE分別為 1.029°、1.493°和2.762°,通過表3中的數據和圖7～圖9,可以看出采用優化AUKF算法相對于KF、UKF可以得到較高的姿態角解算精度,效果明顯。

6 結論

本文設計一種基于優化自適應無跡卡爾曼濾波的姿態解算方法,主要針對慣性器件系統誤差、環境干擾大和陀螺漂移引起姿態發散等場景,在預處理的基礎上,利用優化AUKF的高精度特性完成姿態解算。從實驗結果效果看:預處理效果明顯,基本消除了噪聲干擾和信號野點值,通過優化AUKF的姿態解算,姿態角精度進一步提高,該方法實現簡單和有較強的實用性,是一種高精度的姿態解算方法。