自適應(yīng)誤差四元數(shù)無(wú)跡卡爾曼濾波四旋翼飛行器姿態(tài)解算方法

崔培林,周翟和,呂品,胡斌

(南京航空航天大學(xué)自動(dòng)化學(xué)院,211106,南京)

四旋翼飛行器(后文簡(jiǎn)稱飛行器)是控制科學(xué)和無(wú)人機(jī)領(lǐng)域內(nèi)備受關(guān)注的研究對(duì)象,飛行器姿態(tài)信息的測(cè)量精度直接影響了飛行器速度、位置的導(dǎo)航精度[1-2]。在小型飛行器中,常使用陀螺儀、加速度計(jì)以及磁強(qiáng)計(jì)作為姿態(tài)測(cè)量器件,陀螺儀存在累積誤差,而加速度計(jì)和磁強(qiáng)計(jì)受外界干擾較大,需要對(duì)其進(jìn)行有效的數(shù)據(jù)融合以保證獲得較為精確的姿態(tài)角[3-4]。

在姿態(tài)解算方法中,文獻(xiàn)[5-6]使用卡爾曼濾波(Kalman Filter,KF)算法進(jìn)行數(shù)據(jù)融合,該方法建模簡(jiǎn)單,實(shí)時(shí)性較好,但是忽略了非線性因素對(duì)濾波帶來(lái)的影響。文獻(xiàn)[7-8]使用擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)方法,這是一種關(guān)于噪聲均值和協(xié)方差的線性化方法,用這種方法對(duì)狀態(tài)方程和量測(cè)方程進(jìn)行線性化處理,必然會(huì)引入誤差。對(duì)于弱非線性,EKF可獲得較為準(zhǔn)確的結(jié)果,然而,對(duì)于強(qiáng)非線性,線性化模型的誤差可能導(dǎo)致濾波器性能下降甚至發(fā)散。文獻(xiàn)[9]提出了基于四元數(shù)無(wú)跡卡爾曼濾波(Unscented Kalman Filter,UKF)算法,該方法使用四元數(shù)法進(jìn)行姿態(tài)解算,保證了系統(tǒng)可以全姿態(tài)工作,具有很好的非線性姿態(tài)估計(jì)精度與濾波穩(wěn)定性,但是具有較大的計(jì)算量。文獻(xiàn)[10-11]中提出了誤差四元數(shù)卡爾曼濾波(Error Quaternion Kalman Filter,EQKF)方法,使用誤差四元數(shù)的方法構(gòu)建系統(tǒng)的狀態(tài)方程與量測(cè)方程,具有計(jì)算量小,響應(yīng)快的優(yōu)點(diǎn),但是也忽略了非線性因素的影響。

針對(duì)慣性傳感器在飛行器姿態(tài)解算中存在隨機(jī)漂移誤差和易受到非重力加速度影響的問(wèn)題,本文在以上研究的基礎(chǔ)上,提出了一種自適應(yīng)誤差四元數(shù)無(wú)跡卡爾曼濾波(Adaptive Error Quaternion Unscented Kalman Filter,AEQUKF)的飛行器姿態(tài)解算方法。選用誤差四元數(shù)和陀螺儀的關(guān)系,并結(jié)合陀螺儀漂移誤差模型構(gòu)建系統(tǒng)的狀態(tài)方程,利用加速度計(jì)和磁強(qiáng)計(jì)數(shù)據(jù)構(gòu)建系統(tǒng)的量測(cè)方程,最后選用誤差四元數(shù)無(wú)跡卡爾曼濾波(Error Quaternion Unscented Kalman Filter,EQUKF)框架實(shí)現(xiàn)數(shù)據(jù)融合,通過(guò)判斷系統(tǒng)是否存在非重力加速度,自適應(yīng)調(diào)整量測(cè)噪聲協(xié)方差矩陣,減小非重力加速度的影響。該方法抑制了傳感器隨機(jī)漂移誤差,自適應(yīng)補(bǔ)償了非重力加速度對(duì)姿態(tài)估計(jì)的影響,相比較EKF算法進(jìn)一步提高了姿態(tài)系統(tǒng)檢測(cè)的精度。實(shí)驗(yàn)表明,當(dāng)飛行器有非重力加速度時(shí),該算法比傳統(tǒng)算法具有更好的姿態(tài)解算精度。

1 誤差四元數(shù)無(wú)跡卡爾曼濾波建模

1.1 狀態(tài)方程的建立

陀螺儀輸出的信號(hào)與運(yùn)動(dòng)條件無(wú)關(guān),但是陀螺儀信號(hào)具有偏置誤差[12-13]。因此,陀螺儀測(cè)量模型為角速度、偏置誤差、白噪聲的總和,即

ωo=ω+εg+ng

(1)

式中:ωo表示陀螺儀的輸出;ω表示實(shí)際角速度;εg表示陀螺儀的偏置誤差;ng表示陀螺儀的零均值白噪聲。

若用?表示四元數(shù)乘法,則計(jì)算真實(shí)四元數(shù)的微分方程為

?ω

(2)

計(jì)算四元數(shù)的估計(jì)值的微分方程為

?ωo

(3)

(4)

式中

Qe=[qe1qe2qe3]T

(5)

其中qe1、qe2、qe3為誤差四元數(shù)的3個(gè)參數(shù)。

(6)

對(duì)式(6)兩邊求導(dǎo)可得

(7)

將式(1)(2)代入式(7)可得

(8)

將式(6)代入式(8)可得

(9)

(10)

進(jìn)一步地,將式(1)代入式(10)可得

(11)

對(duì)于3階向量,定義[p×]為

(12)

因?yàn)檎`差四元數(shù)第一項(xiàng)為常數(shù)項(xiàng)1,對(duì)其求導(dǎo)為常數(shù)0,對(duì)后面模型的搭建沒(méi)有影響。式(11)可簡(jiǎn)化為

0.5(εg+ng)+[Qe×](εg+ng)

(13)

又因?yàn)?/p>

[Qe×]ωo=-[ωo×]Qe

(14)

而Qe、εg和ng很小,所以[Qe×](εg+ng)為高階小項(xiàng),可以忽略。將式(14)代入式(13),忽略掉[Qe×]·(εg+ng)可得

(15)

若nεg為陀螺儀漂移零均值白噪聲,則陀螺儀漂移模型為

(16)

選取系統(tǒng)狀態(tài)量為

x=[qe1qe2qe3εgxεgyεgz]T

(17)

式中:εgx、εgy、εgz表示陀螺儀漂移三軸分量。

根據(jù)式(15)～(17),可得誤差四元數(shù)UKF的狀態(tài)方程為

xk=f(xk-1)+wk-1

(18)

式中:f(xk-1)為系統(tǒng)的狀態(tài)轉(zhuǎn)換方程;wk-1為系統(tǒng)過(guò)程噪聲。f(xk-1)和wk-1的具體公式為

(19)

(20)

式中T表示采樣時(shí)間。

系統(tǒng)協(xié)方差矩陣為

(21)

式中Qg和Qεg分別為陀螺儀噪聲協(xié)方差矩陣和陀螺儀漂移噪聲協(xié)方差矩陣,具體值由實(shí)驗(yàn)獲得。

1.2 觀測(cè)方程的建立

加速度輸出數(shù)據(jù)信號(hào)模型為重力、非重力加速度、零均值白噪聲的總和,磁強(qiáng)計(jì)輸出數(shù)據(jù)信號(hào)模型為地磁場(chǎng)和零均值白噪聲的總和,公式為

(22)

(23)

選取觀測(cè)量z=[fafm]T,根據(jù)式(18)(19),誤差四元數(shù)UKF的量測(cè)方程為

zk=h(xk)+vk

(24)

式中:h(xk)為系統(tǒng)的量測(cè)轉(zhuǎn)換方程;vk為系統(tǒng)量測(cè)噪聲。h(xk)和vk的具體公式為

(25)

(26)

由于ab和na互不相關(guān),所以量測(cè)噪聲協(xié)方差矩陣公式為

(27)

式中:Rb為非重力加速度協(xié)方差矩陣,具體值參考本文2.2節(jié);Ra和Rm分別為加速度計(jì)噪聲協(xié)方差矩陣和磁強(qiáng)計(jì)噪聲協(xié)方差矩陣,具體值由實(shí)驗(yàn)獲得。

2 自適應(yīng)誤差四元數(shù)無(wú)跡卡爾曼濾波算法

2.1 對(duì)稱采樣

無(wú)跡變換(Unscented Transformation,UT)的主要思想是使用近似概率分布來(lái)代替非線性函數(shù),在原先狀態(tài)分布中按照某一規(guī)則選取確定性的點(diǎn)集S(即Sigma點(diǎn)),使這些點(diǎn)集的均值和方差等于原狀態(tài)分布的均值和方差。通過(guò)非線性變換,加權(quán)計(jì)算得到變換后的均值和方差。

UT變換算法中采樣策略是關(guān)鍵部分,本文的采樣策略為對(duì)稱采樣[15],具體如下。

(1)Sigma點(diǎn)的產(chǎn)生

(28)

(2)權(quán)值計(jì)算

(29)

2.2 非重力加速度補(bǔ)償

當(dāng)飛行器在上升、下降、加速、減速等階段存在著較大的非重力加速度時(shí),加速度計(jì)不能將其區(qū)分出來(lái),由加速度計(jì)解算得到的姿態(tài)角度信息將受到非重力加速度的影響,姿態(tài)角包含較大誤差,將直接影響飛行器的飛行控制。本文通過(guò)自適應(yīng)調(diào)整協(xié)方差矩陣來(lái)減少非重力加速度帶來(lái)的影響。

判斷是否存在非重力加速度的公式為

|‖f‖-g|<δg

(30)

式中:‖f‖為加速度輸出數(shù)據(jù)的范數(shù);g為重力加速度;δg為閾值常數(shù),其取值和加速度計(jì)噪聲有關(guān)。

若上式滿足,則飛行器不存在非重力加速度,即

Rb=0

(31)

反之,則存在非重力加速度,即

(32)

式中:ρ為常數(shù),由多次實(shí)驗(yàn)獲得,用于調(diào)節(jié)濾波精度;ra為加速度計(jì)的殘差信息,公式為

ra=zk-h(xk)

(33)

2.3 自適應(yīng)誤差四元數(shù)無(wú)跡卡爾曼濾波算法

(34)

(35)

(2)狀態(tài)更新。根據(jù)對(duì)稱采樣策略生成Sigma點(diǎn),公式為

(36)

計(jì)算Sigma點(diǎn)的下一步預(yù)測(cè)值,對(duì)每個(gè)Sigma點(diǎn)非線性變換,公式為

χk+1,k=f(χk)

(37)

對(duì)變換后的Sigma點(diǎn)進(jìn)行加權(quán)得到下一步預(yù)測(cè)和協(xié)方差矩陣,公式為

(38)

(39)

根據(jù)非線性觀測(cè)方程對(duì)Sigma點(diǎn)集χk進(jìn)行非線性變換,公式為

zk+1,k=h(χk+1,k)

(40)

使用加權(quán)計(jì)算,得到預(yù)測(cè)的觀測(cè)值為

(41)

(3)非重力加速度補(bǔ)償。根據(jù)式(30)判斷飛行器系統(tǒng)是否存在非重力加速度。當(dāng)不存在非重力加速度時(shí),加速度輸出的數(shù)據(jù)為當(dāng)?shù)刂亓铀俣戎?非重力加速度協(xié)方差矩陣為式(31),代入本文算法求解姿態(tài)數(shù)據(jù);當(dāng)系統(tǒng)存在非重力加速度時(shí),殘差信息式(33)中包含了最新時(shí)刻的加速度計(jì)數(shù)據(jù),也保留了非重力加速度的信息,非重力加速度越大,殘差信息也就越大,相應(yīng)的非重力加速度協(xié)方差矩陣也就越大。利用式(32)調(diào)整協(xié)方差矩陣Rb,代入本文算法求解姿態(tài)數(shù)據(jù)。非重力加速度協(xié)方差矩陣是根據(jù)非重力加速度大小自適應(yīng)更新的,非重力加速度越大,補(bǔ)償效果越明顯。

(4)量測(cè)更新。協(xié)方差矩陣更新公式[19-20]為

(42)

量測(cè)輸出協(xié)方差矩陣更新公式為

(43)

卡爾曼增益矩陣更新公式為

(44)

狀態(tài)更新后,濾波值更新公式為

(45)

后驗(yàn)方差矩陣更新公式為

Pk+1=Pk+1,k-Kk+1Pzk+1Kk+1

(46)

3 仿真與實(shí)驗(yàn)驗(yàn)證

使用Matlab仿真平臺(tái)和飛行器平臺(tái)對(duì)提出的算法進(jìn)行驗(yàn)證。算法驗(yàn)證分為靜態(tài)實(shí)驗(yàn)驗(yàn)證和動(dòng)態(tài)非重力加速度仿真驗(yàn)證。靜態(tài)實(shí)驗(yàn)主要是驗(yàn)證改進(jìn)的算法具有良好的濾波效果,可以抑制陀螺漂移,獲得較高的姿態(tài)解算的精度。動(dòng)態(tài)非重力加速度仿真主要是為了驗(yàn)證飛行器在存在非重力加速度的情況下,該方法可以有效地減小非重力加速度影響,獲得更為準(zhǔn)確的濾波值。

具體實(shí)驗(yàn)和仿真參數(shù)如下:T=0.01 s,Ra=0.000 1I3,Rm=0.000 1I3,Qg=0.000 1I3,Qεg=0.000 001I3,P=0.000 1I6,ρ=10,δg=0.02。

3.1 靜態(tài)驗(yàn)證

利用飛行器平臺(tái)進(jìn)行靜態(tài)實(shí)驗(yàn)驗(yàn)證本文方法。飛行器實(shí)驗(yàn)平臺(tái)如圖1所示,姿態(tài)測(cè)量器件為MPU6050(三軸陀螺儀和三軸加速度計(jì))和HMC5883L(三軸磁強(qiáng)計(jì)),飛行器軸長(zhǎng)45 cm,數(shù)據(jù)通過(guò)UART通信,將傳感器數(shù)據(jù)發(fā)送到上位機(jī)。飛行器在靜止?fàn)顟B(tài)下的俯仰角、航向角和橫滾角均為0°,飛行器不存在非重力加速度。

圖1 飛行器實(shí)驗(yàn)平臺(tái)

圖2和表1給出了靜態(tài)實(shí)驗(yàn)中AEQUKF的解算結(jié)果,圖和表中“加速度計(jì)”、“陀螺儀”和“磁強(qiáng)計(jì)”表示該類傳感器數(shù)據(jù)經(jīng)過(guò)四元數(shù)解算方法直接解算得到的姿態(tài)數(shù)據(jù),可以看出:相較于加速度計(jì)和磁強(qiáng)計(jì)的解算結(jié)果,AEQUKF具有較高的姿態(tài)解算精度;相較于陀螺儀的解算結(jié)果,AEQUKF濾除了陀螺漂移帶來(lái)的影響。

(a)俯仰角誤差

(b)橫滾角誤差

(c)航向角誤差圖2 靜態(tài)條件下姿態(tài)解算誤差

解算方法姿態(tài)角解算誤差標(biāo)準(zhǔn)差/10-4(°)解算方法姿態(tài)角解算誤差標(biāo)準(zhǔn)差/10-4(°)AEQ-UKF俯仰角橫滾角航向角1.073.174.24加速度計(jì)俯仰角橫滾角航向角2733 陀螺儀俯仰角橫滾角航向角373935磁強(qiáng)計(jì)俯仰角橫滾角航向角30

3.2 動(dòng)態(tài)驗(yàn)證

飛行器在加速上升、減速下降等過(guò)程中存在較大的非重力加速度。為了驗(yàn)證提出的方法對(duì)非重力加速度有較好的抑制效果,利用Matlab平臺(tái)對(duì)算法進(jìn)行仿真驗(yàn)證。圖3為Matlab仿真中加速度計(jì)的輸出噪聲數(shù)據(jù),nx、ny、nz分別為加速度計(jì)在x、y、z方向的噪聲,可以看出加速度計(jì)噪聲部分在閾值范圍外,說(shuō)明仿真中存在非重力加速度影響。

(a)加速度計(jì)x方向噪聲

(b)加速度計(jì)y方向噪聲

(c)加速度計(jì)z方向噪聲圖3 動(dòng)態(tài)條件下加速度計(jì)的輸出噪聲

圖4和表2給出了動(dòng)態(tài)非重力加速度仿真中AEQUKF解算結(jié)果,可以看出:EQUKF受到非重力加速度影響,濾波精度比較低,而本文提出的AEQUKF可以有效地減小非重力加速度影響,獲得較高的姿態(tài)解算精度。

(a)俯仰角解算誤差

(b)橫滾角解算誤差

(c)航向角解算誤差圖4 動(dòng)態(tài)條件下姿態(tài)解算誤差

解算方法姿態(tài)角解算誤差標(biāo)準(zhǔn)差/10-3(°)俯仰角1.8AEQUKF橫滾角2.1航向角2.4俯仰角52EQUKF橫滾角62航向角67

4 總 結(jié)

通過(guò)研究飛行器姿態(tài)解算時(shí)存在非重力加速度影響的問(wèn)題,提出了AEQUKF飛行器姿態(tài)估計(jì)算法。在AEQUKF算法中,基于誤差四元數(shù)方法構(gòu)建UKF狀態(tài)方程和量測(cè)方程。當(dāng)檢測(cè)到存在非重力加速度時(shí),利用殘差信息自適應(yīng)調(diào)整協(xié)方差矩陣,減小非重力加速度的影響,獲得較為準(zhǔn)確的姿態(tài)解算值,有效地減小了陀螺儀漂移誤差帶來(lái)的影響。實(shí)驗(yàn)表明,AEQUKF算法對(duì)非重力加速度具有很好的補(bǔ)償作用,為飛行器提供了一個(gè)低成本、高精度的姿態(tài)檢測(cè)系統(tǒng)。