宗意凱,蘇淑靖,高瑜宏

(中北大學,省部共建動態(tài)測試技術國家重點實驗室,山西太原 030051)

0 引言

姿態(tài)解算系統(tǒng)[1-2]廣泛應用于無人機、自主機器人[3]、航空航天等強烈需求自身姿態(tài)信息的自動化系統(tǒng)中。隨著近年來無人機[4-6]行業(yè)的發(fā)展,對微型航姿參考系統(tǒng)(AHRS)的精度要求進一步提高。受限于微機電系統(tǒng)(MEMS)的成本和尺寸限制,低成本慣性測量單元(IMU)方案存在較大的噪聲誤差和漂移誤差,使得僅用陀螺儀解算得到姿態(tài)角具有明顯的解算漂移[7-8]。針對低成本IMU傳感器的誤差問題,文獻[9]基于自回歸滑動平均系統(tǒng)設計了一套零滯后補償的測量方法并實現了一種自適應濾波方法以降低噪聲,但由于自協(xié)方差的求解不能保證無偏,因而對自協(xié)方差的反復迭代存在增大自協(xié)方差誤差風險。文獻[10]以樣本長度、模型參數、降低模型階數等方面為基礎設計了一種隨機誤差補償方法以優(yōu)化Kalman濾波器[11-12]輸出,但所建立的模型在預測誤差較大時去噪效果不明顯。目前主要使用互補濾波法、梯度下降法、卡爾曼濾波法實現姿態(tài)解算,其中互補濾波因其簡化的計算量而被廣泛用于嵌入式設備中。

互補濾波基于陀螺儀積分得到的角度,利用加速度計絕對靜態(tài)下重力指向的唯一性對陀螺儀解算得到的姿態(tài)角進行收斂補償。傳統(tǒng)互補濾波模型利用單種六軸慣性傳感器或九軸慣性傳感器進行姿態(tài)解算,受所選慣性傳感器噪聲特性、漂移特性的影響。例如,BMI088六軸慣性傳感器具有良好的零漂特性與溫漂特性,但傳感器數據存在較高噪聲;ICM20689六軸慣性傳感器具有較低的噪聲特性,但其卻存在較嚴重的零漂與溫漂問題。此外,受限于MEMS傳感器工藝限制,低成本IMU傳感器的數據輸出頻率通常不高于1 000 Hz,使得陀螺儀的積分誤差較大?；谝陨蟽牲c,本文設計的IMU載板由BMI088與ICM20689組成,基于TMS320F28379S微處理器實現數據的采集與傳輸。以互補濾波算法為基礎,發(fā)揮互補濾波傳感器噪聲頻段互補的優(yōu)勢的同時結合粒子濾波處理非線性問題的優(yōu)點,優(yōu)化多源IMU數據融合補償過程,從而更快、更準獲取測量系統(tǒng)的姿態(tài)信息。

1 數據預處理

因數據采集平臺與轉臺之間存在安裝誤差,需要對數據采集平臺獲取的六軸數據進行校準以消除坐標偏移誤差對姿態(tài)解算的影響。因數據采集系統(tǒng)與轉臺系統(tǒng)分屬2個獨立系統(tǒng),兩者采集所得數據存在時間軸不對齊問題,需對兩者時間數據進行對齊校正并裁剪多余數據以供結果分析。

1.1 傳感器參數校準

由于數據采集平臺坐標系與轉臺坐標系存在一定的安裝誤差,在解算姿態(tài)前需要計算校準參數以修正漂移誤差和旋轉誤差。

建立采集平臺加速度真實值與測量值關系表達式:

(1)

式中:Ature為采集平臺加速度計真實值;R為表征測量值向真實值旋轉的3×3矩陣;Scalex、Scaley、Scalez為表征真實加速度向量與測量加速度向量之間的縮放尺度;Amx、Amy、Amz為加速度計測量值;offsetx、offsety、offsetz為加速度計測量值的零漂。

對式(1)進行簡化得到:

(2)

式中:Coffx、Coffy、Coffz均為加速度計偏置。

將式(2)進一步轉換為齊次標準型后轉置帶入六面參數得:

(3)

利用最小二乘法求解式(3)方程:

(4)

式中:C為式(3)中的4×3待定校準系數矩陣;Am為式(3)中的6×4加速度計六面參數矩陣;G為式(3)中的6×3加速度計六面真值參數矩陣。

對靜置50 s的陀螺儀數據求平均得到陀螺儀零漂量以修正陀螺儀零漂偏置。

1.2 時間軸對齊

轉臺記錄得到的角速度數據與陀螺儀采集得到的角速度數據存在一致性,但由于慣性傳感器數據采集與轉臺數據采集由2個獨立系統(tǒng)分別實現,兩者之間存在采樣時間點不重合的問題。

慣性傳感器數據采集的時間由28379S定時器記錄,精確到1 μs,轉臺數據采集時間間隔為1 ms。因此首先使用分段三次樣條插值方法對慣性傳感器數據進行1 ms等間隔時間插值,生成獲取同為1 ms時間間隔的慣性傳感器數據序列。

基于MSE理論通過對2條曲線進行均方誤差計算以獲取兩者重合度,損失函數為

(5)

離散計算公式為

(6)

對滯后曲線逐次時移并計算均方誤差,均方誤差最小時的時移系數即數據滯后時間。即計算式(7)最小值:

(7)

假設f2(x)滯后于f1(x),當式(7)中均方誤差最小時,τ即滯后時間,計算得到滯后時間后對曲線進行時間軸裁剪以完成時間軸對齊。

2 改進型互補濾波算法

姿態(tài)解算即橫滾角φ、俯仰角θ、偏航角Ψ的求解,基于陀螺儀積分獲取得到的姿態(tài)角信息具有較好的高頻特性,但由于陀螺儀的數據采樣及解算過程為離散過程,在積分的過程中不能避免積分誤差。因而純陀螺儀解算得到的姿態(tài)角信息將會隨著時間推移產生較大的漂移。利用加速度計解算姿態(tài)角即利用重力加速度在運載體坐標系下的矢量來求解運載體的姿態(tài)信息,加速度計具有較好的低頻特性,且不隨時間產生漂移,但當運載體進行大幅度運動時,加速度計不能表征運載體的劇烈運動姿態(tài)?；谕勇輧x較好的高頻特性與加速度計較好的低頻特性,進行互補融合可以獲取兼顧高頻特性與低頻特性的姿態(tài)解算算法。

受低成本MEMS慣性傳感器限制,參與解算的傳感器數據頻率基本不高于1 000 Hz,且易受單種六軸傳感器設計本身的噪聲特性影響。為此,本文基于多源慣性傳感器優(yōu)化姿態(tài)解算算法,提高單位時間樣本量的同時,削弱單種傳感器芯片噪聲特性對姿態(tài)解算的影響。

盡管有大量研究表明基于線性高斯模型搭建的Kalman濾波器具有最小方差與最小誤差的最優(yōu)性,但在運載體的實際運動過程中,傳感器受外部環(huán)境影響,引入的噪聲信息并不完全符合線性高斯模型,因此本文依據粒子濾波理論[13-14]對互補濾波進行優(yōu)化處理,期望獲得相比傳統(tǒng)互補濾波更優(yōu)的姿態(tài)解算算法。算法結構如圖1所示。

圖1 姿態(tài)解算算法結構圖

2.1 同步傳感器互補濾波姿態(tài)四元數解算

定義四元數:

(8)

四元數q描述坐標系以[rxryrz]T為旋轉軸,旋轉α的過程。

基于上一次解算獲取得到的姿態(tài)四元數估計重力方向向量:

(9)

對加速度計測得的重力向量與姿態(tài)四元數估計所得的重力向量做叉乘得到誤差角的近似:

|am×a|=|am||a|sinθ

(10)

式中:am為加速度計測得的重力向量;a為姿態(tài)四元數估計所得的重力向量。

由于am與a均為單位向量,因此當運載體小角度運動時誤差近似為

e=|am×a|=sinθ≈θ

(11)

誤差角基于PI模型對陀螺儀測量所得角速度進行補償得到修正后的角速度:

(12)

得到補償后的三軸角速度后由四元數微分方程迭代更新四元數:

(13)

計算所得的四元數利用四元數定義可解算出三軸姿態(tài)角:

(14)

2.2 異步傳感器角速度補償優(yōu)化

理想情況下,加速度計數據與陀螺儀數據的采樣頻率一致且采樣同步,例如ICM20689六軸傳感器在1 000 Hz采樣率下加速度計與陀螺儀數據為同步采樣。但BMI088六軸傳感器加速度計最大采樣頻率為800 Hz,陀螺儀最大采樣頻率為1 000 Hz,加速度計數據與陀螺儀數據間存在較嚴重的異步現象,且兩者采樣時間間隔并不固定。為削弱BMI088加速度與陀螺儀異步采樣數據對解算精度的影響,需對式(12)的角速度補償公式進行優(yōu)化。

利用DSP C2000的高精度捕獲單元(eCAP)捕獲BMI088加速度計與陀螺儀的數據就緒信號,計算得到陀螺儀數據與加速度數據之間的采樣間隔時間τ。利用采樣間隔時間τ動態(tài)修正式(12)角速度補償公式中的比例項,優(yōu)化后的角速度補償公式為

(15)

式中:Tgyro為陀螺儀的采樣周期;τ為陀螺儀數據與加速度數據之間的采樣間隔時間;α為比例項下限系數。

當加速度數據采樣離陀螺儀數據時間較遠,則比例系數被相應減小,削弱加速度計數據對角度的修正。

2.3 多源姿態(tài)四元數融合

為融合多源傳感器計算所得的四元數,建立多源姿態(tài)四元數融合公式:

qfusion=KAqA+KBqB

(16)

式中:qfusion為融合所得四元數;qA為基于ICM20689加速度計數據與陀螺儀數據進行同步傳感器互補濾波姿態(tài)解算得到姿態(tài)四元數;qB為基于BMI088加速度計數據與陀螺儀數據進行異步傳感器互補濾波姿態(tài)解算得到姿態(tài)四元數。

為削弱噪聲特性對四元數解算的影響,利用信息熵對2種傳感器的噪聲進行評估,并基于評估調整2組四元數的融合權值KA、KB。

假定一段時間內的任一傳感器四元數解算結果為q1,q2,…,qk,計算相鄰時間的四元數偏差:

Δqi=|qi+1-qi|

(17)

得到k-1個偏差Δq1,Δq2,…,Δqk-1后對偏差數據進行量化編碼處理:

ei=[Δqi/u]

(18)

式中u為編碼步長。

利用k-1個量化編碼計算概率:

(19)

式中c(ej)為編碼ej出現的次數。

根據編碼概率計算四元數的信息熵:

(20)

式中:m為c(ej)所有取值可能的總數。

信息熵H(q)越大則數據不確定度越大,噪聲越大?；谛畔㈧赜嬎憬Y果對預融合權值KA′、KB′進行動態(tài)更新:

(21)

因此,融合四元數更傾向于當前數據,而根據他源四元數距當前的時間間隔動態(tài)削弱其融合權重。對四元數預融合系數歸一化后得到四元數融合系數:

(22)

2.4 粒子濾波

建立狀態(tài)方程與觀測方程:

(23)

式中:Xk為k時刻的狀態(tài)變量;Qk為k時刻的角度噪聲;Yk為k時刻的觀測值;Rk為k時刻的角速度噪聲。

由于四元數本質為四維超向量,直接進行四元數粒子濾波計算量非常大,因此本文以歐拉角為基礎構建粒子濾波模型:

(24)

(25)

生成n個粒子參與粒子濾波。由于粒子數量越多,計算量越大,因此為兼顧計算量,粒子數于100～500范圍內選取。假設粒子服從的概率密度為

(26)

式中:ω(i)為粒子權重;δ(x)為狄拉克函數。

假設初始時刻x0的概率密度為

(27)

生成初始粒子并初始化粒子權重:

(28)

構建預測步計算公式:

(29)

(30)

(31)

(32)

構建更新步計算公式:

(33)

基于更新得到的概率分布計算期望得到姿態(tài)角結果:

(34)

(35)

2.5 重采樣

在粒子濾波預測步更新過程中存在少數粒子占據極高權重,而多數粒子權重為0的現象。權重為0的粒子在后續(xù)粒子濾波過程中權重也將始終為0,該現象將使得實際參與粒子濾波的粒子數減少。為了削弱粒子退化引起的粒子濾波更新失效,當粒子群發(fā)生退化時引入重采樣步驟,按概率對粒子進行復制與淘汰,權重高的粒子更有可能被多次復制,從而保證整個粒子群活性粒子的總數基本不變。

由于重采樣會削弱粒子多樣性,因此僅對明顯退化的粒子群進行重采樣。重采樣觸發(fā)閾值設計為

(36)

(37)

重采樣序列示意圖如圖2所示。

圖2 重采樣序列示意圖

取n次[0,1]范圍內的隨機數,根據隨機數落點更新粒子:

(38)

并將所有更新后的粒子權重刷新為1/n:

(39)

3 實驗數據處理及結果分析

3.1 實驗平臺

本文實驗數據基于圖3(a)所示三軸位置速率搖擺溫控轉臺所得,數據采集平臺實物如圖3(b)所示。

(a)三軸位置速率搖擺轉臺

(b)數據采集平臺實物

轉臺常溫連續(xù)運行模式下三軸位置精度3″,回轉精度3″,垂直精度5″;內框角加速度最大值2 000 (°)/s2,中框角加速度最大值1 000 (°)s/2,精度滿足實驗要求。轉臺記錄三軸角度數據與角速度數據,數據間隔1 ms。

設定轉臺內框(橫滾軸)和中框(俯仰軸)為搖擺組態(tài)并記錄雙軸搖擺實驗下的角度與角速度。

3.2 數據采集平臺硬件簡述

數據采集平臺硬件如圖4所示。

圖4 數據采集平臺硬件框圖

基于TM320F28379S微處理器設計數據采集平臺。加速度計數據與陀螺儀數據基于SPI通信實時獲取。

3.3 傳感器參數校準處理及結果分析

基于傳感器參數校準方法對BMI088加速度計數據進行校準,校準前與校準后六面數據如表1、表2所示,校準結果顯示通過六面校準模型整定后六面數據的漂移誤差與旋轉誤差均得到有效抑制。

表1 BMI088加速度計校準前六面數據 m/s2

表2 BMI088加速度計校準后六面數據 m/s2

3.4 時間軸處理及結果分析

基于傳感器數據與轉臺數據時間軸對齊方法對傳感器數據與轉臺數據進行時間軸對齊與裁剪。時間軸對齊前與對齊后的角速度數據參考如圖5、圖6所示,結果顯示利用該方法實現的時間軸對齊能夠最大程度對齊2個獨立系統(tǒng)的時間軸。

圖5 時間軸對齊前角速度數據參考

圖6 時間軸對齊后角速度數據參考

3.5 姿態(tài)解算結果對比分析

本文以高精度轉臺記錄得到的三軸實際角度作為真實角度信息對解算所得姿態(tài)角進行評估。對比不同方法下姿態(tài)解算結果。

對僅使用BMI088解算姿態(tài)、僅使用ICM20689解算姿態(tài)、使用BMI088+ICM20689解算姿態(tài)、使用BMI088 +ICM20689基于粒子濾波優(yōu)化解算姿態(tài)進行測試對比。計算各姿態(tài)解算方法得到的俯仰角、橫滾角解算角曲線與轉臺記錄的實際姿態(tài)角曲線間的均方誤差;記錄23 s后無磁力計參與計算的解算漂移誤差角;計算解算角曲線與實際姿態(tài)角曲線的時間遲滯。利用上述3項指標對各姿態(tài)解算方法進行評估。

僅使用BMI088六軸傳感器解算所得姿態(tài)角如圖7～圖9所示,圖中深色曲線為轉臺記錄所得的實際角度曲線,淺色曲線為解算所得角度曲線。由對比結果可見,基于BMI088低零漂特性,在搖擺實驗下姿態(tài)角解算零漂較低,而由于未引入磁力計以修正偏航角漂移,姿態(tài)迭代23 s后產生了-3.051 2°的漂移。

僅使用ICM20689六軸傳感器解算所得姿態(tài)角曲線與實際角度曲線如圖10～圖12所示。由對比結果可見,由于ICM20689相對較差的零漂特性,在搖擺實驗下橫滾角解算表現出明顯的零漂現象,而由于未引入磁力計以修正偏航角漂移,姿態(tài)迭代23 s后產生了-3.795 1°的漂移。

使用BMI088+ICM20689多源傳感器組合基于粒子濾波優(yōu)化解算所得姿態(tài)角如圖13～圖15所示。由對比結果可見,利用2顆非同種IMU進行姿態(tài)解算,在搖擺實驗下解算結果得到明顯改善,解算角度曲線與實際角度擬合度明顯提升,而23 s后偏航角解算漂移也被抑制至-0.848 2°。

各方法解算所得姿態(tài)角與轉臺實際角度之間的均方誤差、偏航角解算漂移和解算時滯如表3所示,序號1為僅使用BMI088進行姿態(tài)解算結果;序號2為僅使用ICM20689進行姿態(tài)解算結果;序號3為使用BMI088和ICM20689進行姿態(tài)解算結果;序號4為使用BMI088和ICM20689并利用粒子濾波優(yōu)化實現的姿態(tài)解算結果。通過實驗數據對比可以看到,引入多源傳感器組合進行姿態(tài)角冗余計算明顯縮減了姿態(tài)解算的滯后時間并提升了俯仰角與橫滾角的解算精確度?；诹Ｗ訛V波優(yōu)化后的互補濾波姿態(tài)解算方法進一步提升了俯仰角與橫滾角的解算精確度。在未使用磁力計參與解算的條件下偏航角的解算存在漂移,但通過實驗數據可以發(fā)現引入多源IMU對偏航角的漂移有著明顯抑制作用。

圖7 純BMI088互補濾波俯仰角解算結果圖

圖8 純BMI088互補濾波橫滾角解算結果圖

圖9 純BMI088互補濾波偏航角解算結果圖

圖10 純ICM20689互補濾波俯仰角解算結果圖

圖11 純ICM20689互補濾波橫滾角解算結果圖

圖12 純ICM20689互補濾波偏航角解算結果圖

圖14 改進型互補濾波橫滾角解算結果圖

圖15 改進型互補濾波偏航角解算結果圖

表3 解算結果誤差及時延對比

4 結論

本文針對互補濾波姿態(tài)解算算法引入多源傳感器組合與粒子濾波方法以實現對姿態(tài)角解算的優(yōu)化?；诟呔绒D臺記錄所得角度數據對解算角進行驗證分析,結果表明該設計的改進型互補濾波方法對姿態(tài)解算的精度有較高的提升,對無磁力計參與解算下的偏航角偏移有著明顯抑制。在本設計基礎上可繼續(xù)引入與ICM20689對稱分布的ICM20602六軸傳感器參與冗余解算以消除ICM20689相對BMI088位置的中心偏移影響,引入磁力計數據以約束偏航角的解算漂移[15-16],獲得更優(yōu)的姿態(tài)解算方案。