基于多源IMU和粒子濾波優(yōu)化的姿態(tài)融合算法

宗意凱,蘇淑靖,高瑜宏

(中北大學(xué),省部共建動(dòng)態(tài)測(cè)試技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室,山西太原 030051)

0 引言

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

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

1 數(shù)據(jù)預(yù)處理

因數(shù)據(jù)采集平臺(tái)與轉(zhuǎn)臺(tái)之間存在安裝誤差,需要對(duì)數(shù)據(jù)采集平臺(tái)獲取的六軸數(shù)據(jù)進(jìn)行校準(zhǔn)以消除坐標(biāo)偏移誤差對(duì)姿態(tài)解算的影響。因數(shù)據(jù)采集系統(tǒng)與轉(zhuǎn)臺(tái)系統(tǒng)分屬2個(gè)獨(dú)立系統(tǒng),兩者采集所得數(shù)據(jù)存在時(shí)間軸不對(duì)齊問(wèn)題,需對(duì)兩者時(shí)間數(shù)據(jù)進(jìn)行對(duì)齊校正并裁剪多余數(shù)據(jù)以供結(jié)果分析。

1.1 傳感器參數(shù)校準(zhǔn)

由于數(shù)據(jù)采集平臺(tái)坐標(biāo)系與轉(zhuǎn)臺(tái)坐標(biāo)系存在一定的安裝誤差,在解算姿態(tài)前需要計(jì)算校準(zhǔn)參數(shù)以修正漂移誤差和旋轉(zhuǎn)誤差。

建立采集平臺(tái)加速度真實(shí)值與測(cè)量值關(guān)系表達(dá)式:

(1)

式中:Ature為采集平臺(tái)加速度計(jì)真實(shí)值;R為表征測(cè)量值向真實(shí)值旋轉(zhuǎn)的3×3矩陣;Scalex、Scaley、Scalez為表征真實(shí)加速度向量與測(cè)量加速度向量之間的縮放尺度;Amx、Amy、Amz為加速度計(jì)測(cè)量值;offsetx、offsety、offsetz為加速度計(jì)測(cè)量值的零漂。

對(duì)式(1)進(jìn)行簡(jiǎn)化得到:

(2)

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

將式(2)進(jìn)一步轉(zhuǎn)換為齊次標(biāo)準(zhǔn)型后轉(zhuǎn)置帶入六面參數(shù)得:

(3)

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

(4)

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

對(duì)靜置50 s的陀螺儀數(shù)據(jù)求平均得到陀螺儀零漂量以修正陀螺儀零漂偏置。

1.2 時(shí)間軸對(duì)齊

轉(zhuǎn)臺(tái)記錄得到的角速度數(shù)據(jù)與陀螺儀采集得到的角速度數(shù)據(jù)存在一致性,但由于慣性傳感器數(shù)據(jù)采集與轉(zhuǎn)臺(tái)數(shù)據(jù)采集由2個(gè)獨(dú)立系統(tǒng)分別實(shí)現(xiàn),兩者之間存在采樣時(shí)間點(diǎn)不重合的問(wèn)題。

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

基于MSE理論通過(guò)對(duì)2條曲線進(jìn)行均方誤差計(jì)算以獲取兩者重合度,損失函數(shù)為

(5)

離散計(jì)算公式為

(6)

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

(7)

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

2 改進(jìn)型互補(bǔ)濾波算法

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

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

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

圖1 姿態(tài)解算算法結(jié)構(gòu)圖

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

定義四元數(shù):

(8)

四元數(shù)q描述坐標(biāo)系以[rxryrz]T為旋轉(zhuǎn)軸,旋轉(zhuǎn)α的過(guò)程。

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

(9)

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

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

(10)

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

由于am與a均為單位向量,因此當(dāng)運(yùn)載體小角度運(yùn)動(dòng)時(shí)誤差近似為

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

(11)

誤差角基于PI模型對(duì)陀螺儀測(cè)量所得角速度進(jìn)行補(bǔ)償?shù)玫叫拚蟮慕撬俣?

(12)

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

(13)

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

(14)

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

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

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

(15)

式中:Tgyro為陀螺儀的采樣周期;τ為陀螺儀數(shù)據(jù)與加速度數(shù)據(jù)之間的采樣間隔時(shí)間;α為比例項(xiàng)下限系數(shù)。

當(dāng)加速度數(shù)據(jù)采樣離陀螺儀數(shù)據(jù)時(shí)間較遠(yuǎn),則比例系數(shù)被相應(yīng)減小,削弱加速度計(jì)數(shù)據(jù)對(duì)角度的修正。

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

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

qfusion=KAqA+KBqB

(16)

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

為削弱噪聲特性對(duì)四元數(shù)解算的影響,利用信息熵對(duì)2種傳感器的噪聲進(jìn)行評(píng)估,并基于評(píng)估調(diào)整2組四元數(shù)的融合權(quán)值KA、KB。

假定一段時(shí)間內(nèi)的任一傳感器四元數(shù)解算結(jié)果為q1,q2,…,qk,計(jì)算相鄰時(shí)間的四元數(shù)偏差:

Δqi=|qi+1-qi|

(17)

得到k-1個(gè)偏差Δq1,Δq2,…,Δqk-1后對(duì)偏差數(shù)據(jù)進(jìn)行量化編碼處理:

ei=[Δqi/u]

(18)

式中u為編碼步長(zhǎng)。

利用k-1個(gè)量化編碼計(jì)算概率:

(19)

式中c(ej)為編碼ej出現(xiàn)的次數(shù)。

根據(jù)編碼概率計(jì)算四元數(shù)的信息熵:

(20)

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

信息熵H(q)越大則數(shù)據(jù)不確定度越大,噪聲越大?；谛畔㈧赜?jì)算結(jié)果對(duì)預(yù)融合權(quán)值KA′、KB′進(jìn)行動(dòng)態(tài)更新:

(21)

因此,融合四元數(shù)更傾向于當(dāng)前數(shù)據(jù),而根據(jù)他源四元數(shù)距當(dāng)前的時(shí)間間隔動(dòng)態(tài)削弱其融合權(quán)重。對(duì)四元數(shù)預(yù)融合系數(shù)歸一化后得到四元數(shù)融合系數(shù):

(22)

2.4 粒子濾波

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

(23)

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

由于四元數(shù)本質(zhì)為四維超向量,直接進(jìn)行四元數(shù)粒子濾波計(jì)算量非常大,因此本文以歐拉角為基礎(chǔ)構(gòu)建粒子濾波模型:

(24)

(25)

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

(26)

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

假設(shè)初始時(shí)刻x0的概率密度為

(27)

生成初始粒子并初始化粒子權(quán)重:

(28)

構(gòu)建預(yù)測(cè)步計(jì)算公式:

(29)

(30)

(31)

(32)

構(gòu)建更新步計(jì)算公式:

(33)

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

(34)

(35)

2.5 重采樣

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

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

(36)

(37)

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

圖2 重采樣序列示意圖

取n次[0,1]范圍內(nèi)的隨機(jī)數(shù),根據(jù)隨機(jī)數(shù)落點(diǎn)更新粒子:

(38)

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

(39)

3 實(shí)驗(yàn)數(shù)據(jù)處理及結(jié)果分析

3.1 實(shí)驗(yàn)平臺(tái)

本文實(shí)驗(yàn)數(shù)據(jù)基于圖3(a)所示三軸位置速率搖擺溫控轉(zhuǎn)臺(tái)所得,數(shù)據(jù)采集平臺(tái)實(shí)物如圖3(b)所示。

(a)三軸位置速率搖擺轉(zhuǎn)臺(tái)

(b)數(shù)據(jù)采集平臺(tái)實(shí)物

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

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

3.2 數(shù)據(jù)采集平臺(tái)硬件簡(jiǎn)述

數(shù)據(jù)采集平臺(tái)硬件如圖4所示。

圖4 數(shù)據(jù)采集平臺(tái)硬件框圖

基于TM320F28379S微處理器設(shè)計(jì)數(shù)據(jù)采集平臺(tái)。加速度計(jì)數(shù)據(jù)與陀螺儀數(shù)據(jù)基于SPI通信實(shí)時(shí)獲取。

3.3 傳感器參數(shù)校準(zhǔn)處理及結(jié)果分析

基于傳感器參數(shù)校準(zhǔn)方法對(duì)BMI088加速度計(jì)數(shù)據(jù)進(jìn)行校準(zhǔn),校準(zhǔn)前與校準(zhǔn)后六面數(shù)據(jù)如表1、表2所示,校準(zhǔn)結(jié)果顯示通過(guò)六面校準(zhǔn)模型整定后六面數(shù)據(jù)的漂移誤差與旋轉(zhuǎn)誤差均得到有效抑制。

表1 BMI088加速度計(jì)校準(zhǔn)前六面數(shù)據(jù) m/s2

表2 BMI088加速度計(jì)校準(zhǔn)后六面數(shù)據(jù) m/s2

3.4 時(shí)間軸處理及結(jié)果分析

基于傳感器數(shù)據(jù)與轉(zhuǎn)臺(tái)數(shù)據(jù)時(shí)間軸對(duì)齊方法對(duì)傳感器數(shù)據(jù)與轉(zhuǎn)臺(tái)數(shù)據(jù)進(jìn)行時(shí)間軸對(duì)齊與裁剪。時(shí)間軸對(duì)齊前與對(duì)齊后的角速度數(shù)據(jù)參考如圖5、圖6所示,結(jié)果顯示利用該方法實(shí)現(xiàn)的時(shí)間軸對(duì)齊能夠最大程度對(duì)齊2個(gè)獨(dú)立系統(tǒng)的時(shí)間軸。

圖5 時(shí)間軸對(duì)齊前角速度數(shù)據(jù)參考

圖6 時(shí)間軸對(duì)齊后角速度數(shù)據(jù)參考

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

本文以高精度轉(zhuǎn)臺(tái)記錄得到的三軸實(shí)際角度作為真實(shí)角度信息對(duì)解算所得姿態(tài)角進(jìn)行評(píng)估。對(duì)比不同方法下姿態(tài)解算結(jié)果。

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

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

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

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

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

圖7 純BMI088互補(bǔ)濾波俯仰角解算結(jié)果圖

圖8 純BMI088互補(bǔ)濾波橫滾角解算結(jié)果圖

圖9 純BMI088互補(bǔ)濾波偏航角解算結(jié)果圖

圖10 純ICM20689互補(bǔ)濾波俯仰角解算結(jié)果圖

圖11 純ICM20689互補(bǔ)濾波橫滾角解算結(jié)果圖

圖12 純ICM20689互補(bǔ)濾波偏航角解算結(jié)果圖

圖14 改進(jìn)型互補(bǔ)濾波橫滾角解算結(jié)果圖

圖15 改進(jìn)型互補(bǔ)濾波偏航角解算結(jié)果圖

表3 解算結(jié)果誤差及時(shí)延對(duì)比

4 結(jié)論

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