改進(jìn)的粒子濾波在四旋翼姿態(tài)估計(jì)中的應(yīng)用

曲仕茹, 馬志強(qiáng)

(西北工業(yè)大學(xué) 自動(dòng)化學(xué)院, 陜西 西安 710072)

改進(jìn)的粒子濾波在四旋翼姿態(tài)估計(jì)中的應(yīng)用

曲仕茹, 馬志強(qiáng)

(西北工業(yè)大學(xué) 自動(dòng)化學(xué)院, 陜西 西安 710072)

針對(duì)四旋翼飛行器非線(xiàn)性模型的姿態(tài)估計(jì)問(wèn)題,提出了一種基于重要密度函數(shù)優(yōu)選的改進(jìn)粒子濾波(IDOPF)姿態(tài)估計(jì)算法。該算法通過(guò)擴(kuò)大重要密度函數(shù)的覆蓋范圍,主動(dòng)從眾多重要密度函數(shù)中選擇更優(yōu)初始化粒子群。結(jié)合動(dòng)力學(xué)模型和基于反步法控制器的相關(guān)控制數(shù)據(jù),進(jìn)行了IDOPF粒子濾波算法在四旋翼飛行器姿態(tài)估計(jì)中的仿真實(shí)驗(yàn)。與EKF算法相比,該算法具有更高的估計(jì)收斂速度和估計(jì)精度,避免了不穩(wěn)定濾波,改善了濾波效果,驗(yàn)證了IDOPF算法在四旋翼姿態(tài)估計(jì)的可行性和有效性。

粒子濾波; 擴(kuò)展卡爾曼濾波; 四旋翼姿態(tài)估計(jì)

0 引言

四旋翼飛行器因其結(jié)構(gòu)緊湊、啟動(dòng)條件少和實(shí)現(xiàn)成本低等特點(diǎn),成為近年來(lái)航空學(xué)術(shù)領(lǐng)域的研究熱點(diǎn)。四旋翼飛行器是一種可垂直起降、定點(diǎn)懸停的非共軸旋槳飛行器,通過(guò)調(diào)整四個(gè)獨(dú)立槳葉的轉(zhuǎn)速,實(shí)現(xiàn)飛行器姿態(tài)的控制。目前已有澳洲臥龍崗大學(xué)的Mckerrow對(duì)其進(jìn)行了動(dòng)力學(xué)建模,Bouabdallash等設(shè)計(jì)了Backstepping,LQ和PID等多種方法的增穩(wěn)反饋控制器[1-3]。為了使反饋控制器準(zhǔn)確有效地控制飛行器平穩(wěn)飛行,需要了解飛行器的姿態(tài)。由于受到各種噪聲的影響,一般傳感器的敏感值與真實(shí)值之間存在一定的偏差,為解決這類(lèi)問(wèn)題,常用擴(kuò)展卡爾曼濾波算法(EKF)及Sigma點(diǎn)卡爾曼濾波算法對(duì)觀測(cè)值進(jìn)行估計(jì)[4-5],達(dá)到減小偏差的目的。姿態(tài)估計(jì)中常用的EKF對(duì)四旋翼非線(xiàn)性系統(tǒng)進(jìn)行線(xiàn)性截?cái)?以實(shí)現(xiàn)非線(xiàn)性問(wèn)題線(xiàn)性化。由于EKF算法忽略了非線(xiàn)性函數(shù)的高階導(dǎo)數(shù)項(xiàng),在姿態(tài)估計(jì)中會(huì)出現(xiàn)一定的偏差,并且在局部線(xiàn)性化不成立或線(xiàn)性化數(shù)值結(jié)果不良的時(shí)候產(chǎn)生不穩(wěn)定的濾波。

本文提出了一種基于重要密度函數(shù)優(yōu)選的粒子濾波(IDOPF)算法進(jìn)行姿態(tài)估計(jì)。通過(guò)建立四旋翼飛行器姿態(tài)模型并進(jìn)行仿真,以驗(yàn)證方法的可行性和有效性。

1 粒子濾波基本理論

設(shè)非線(xiàn)性系統(tǒng)模型為:

(1)

式中,wk和vk分別為系統(tǒng)噪聲和測(cè)量噪聲,兩者相互獨(dú)立,一般認(rèn)為是白噪聲;uk為系統(tǒng)的控制輸入。求解后驗(yàn)概率分布p(xk|y1,k)是實(shí)現(xiàn)最優(yōu)非線(xiàn)性濾波的關(guān)鍵,其中y1,k為觀測(cè)向量。根據(jù)貝葉斯估計(jì)理論,在x0,k給定,則yk于k時(shí)刻與以前的測(cè)量相互獨(dú)立時(shí),獲得后驗(yàn)概率表達(dá)式為:

(2)

對(duì)于非線(xiàn)性、非高斯模型,消去式(2)中間參量和其他位置參量所涉及的計(jì)算是困難的,無(wú)法獲得完整的解析式來(lái)描述此概率密度函數(shù),而求取近似解需要計(jì)算多重積分,這些算法上的限制導(dǎo)致求解難以實(shí)現(xiàn)。

為解決上述問(wèn)題,引入用于貝葉斯計(jì)算的蒙特卡羅方法,該方法通過(guò)合理分散估計(jì)量有效地完成求和及積分運(yùn)算,經(jīng)過(guò)整合,成為本文描述的粒子濾波算法。粒子濾波又稱(chēng)序貫蒙特卡羅方法,是一種基于蒙特卡羅方法和遞推貝葉斯估計(jì)的統(tǒng)計(jì)濾波方法,理論上能夠估計(jì)任意形式的概率分布,可解決任意非線(xiàn)性系統(tǒng)的估計(jì)問(wèn)題。

由于一般情況中后驗(yàn)概率分布p(xk|y1,k)未知,需要引入重要采樣概率函數(shù)q(xk|y1,k),利用該函數(shù)進(jìn)行重要性采樣,由貝葉斯遞推公式推導(dǎo)可得權(quán)值函數(shù):

(3)

利用主要采樣概率函數(shù),結(jié)合式(2)能夠獲得粒子的權(quán)值,進(jìn)而獲得狀態(tài)的后驗(yàn)概率密度。然而利用該遞推公式獲得的權(quán)值會(huì)出現(xiàn)粒子退化現(xiàn)象,有效地改善方法是引入重采樣步驟或增加初始粒子數(shù)量。

2 四旋翼飛行器姿態(tài)模型

四旋翼飛行器是典型的欠驅(qū)動(dòng)系統(tǒng),通過(guò)4個(gè)旋轉(zhuǎn)槳葉調(diào)節(jié)六個(gè)自由度的姿態(tài)。研究中簡(jiǎn)化了動(dòng)力學(xué)模型,認(rèn)為四旋翼飛行器的運(yùn)動(dòng)為六自由度的剛體運(yùn)動(dòng),為圍繞三個(gè)軸向的轉(zhuǎn)動(dòng)(偏航、俯仰和滾轉(zhuǎn))和重心沿三個(gè)軸向的線(xiàn)運(yùn)動(dòng)(進(jìn)退、側(cè)飛和升降)。

機(jī)體運(yùn)動(dòng)選擇體坐標(biāo)系進(jìn)行描述,坐標(biāo)原點(diǎn)即為機(jī)體重心,規(guī)定機(jī)體俯仰抬頭時(shí)為正。結(jié)合文獻(xiàn)[6]對(duì)四旋翼飛行器的動(dòng)力學(xué)分析,可以獲得動(dòng)力學(xué)關(guān)系。體坐標(biāo)系下三軸轉(zhuǎn)動(dòng)的角速度和歐拉角速度的關(guān)系為:

(4)

機(jī)體坐標(biāo)系-地面坐標(biāo)系的旋轉(zhuǎn)矩陣為:

(5)

式中,cψ表示cosψ;sψ表示sinψ;其余類(lèi)似。

地坐標(biāo)系加速度與合外力關(guān)系為:

(6)

式中,Fx,Fy,Fz為體坐標(biāo)系下的合外力F在三個(gè)軸向上的分量;Vxyz為體坐標(biāo)系中的飛行速度方向;Vxy為體坐標(biāo)系Oxy平面內(nèi)的飛行速度方向;C為空氣摩擦系數(shù)。

本文的四旋翼飛行器研究限于室內(nèi)懸停及慢飛情況,姿態(tài)變化并不劇烈,所以作以下假設(shè):旋翼中心和機(jī)體質(zhì)心始終在一個(gè)水平面上;忽略空氣擾動(dòng)及空氣摩擦阻力;認(rèn)為歐拉角速度和體坐標(biāo)系角速度一致;不考慮旋翼?yè)]舞,認(rèn)為它只受到與其轉(zhuǎn)速的平方成正比的升力和反扭力矩作用;認(rèn)為旋翼質(zhì)量很輕,不考慮其轉(zhuǎn)動(dòng)慣量矩[7];為了提取數(shù)學(xué)模型,可以直接將旋翼轉(zhuǎn)速當(dāng)作系統(tǒng)動(dòng)力學(xué)模型的輸入。在上述假設(shè)的基礎(chǔ)上,獲得動(dòng)力學(xué)模型為:

(7)

(8)

式中,θ,φ,ψ分別為俯仰角、滾轉(zhuǎn)角和偏航角;Ui(i=1,2,3,4)為四個(gè)旋翼的運(yùn)動(dòng)控制量;Ω為四個(gè)旋翼的轉(zhuǎn)速;其他測(cè)量獲得的物理常量如表1所示。

表1 物理常量Table 1 Physical quantity

3 基于IDOPF的姿態(tài)估計(jì)算法

基本粒子波(PF)對(duì)重要密度函數(shù)的依賴(lài)性很強(qiáng),重要密度函數(shù)選擇不當(dāng)會(huì)直接導(dǎo)致粒子枯竭,從而濾波發(fā)散,失去估計(jì)能力。重要密度函數(shù)的選取難度與抽取目標(biāo)概率樣本相同,從量測(cè)值中獲得概率樣本的方法在大噪聲情況下受到限制。IDOPF是一種基于PF思想的改進(jìn)濾波算法,其基本思想是:擴(kuò)大重要密度函數(shù)的覆蓋范圍,主動(dòng)從眾多重要密度函數(shù)中選擇更優(yōu)的初始化粒子群。衡量粒子群優(yōu)劣的主要方法是考察粒子退化程度,通過(guò)統(tǒng)計(jì)大權(quán)值粒子重采樣的次數(shù)來(lái)反映該項(xiàng)指標(biāo)。一般情況下,粒子退化程度越嚴(yán)重,大權(quán)值粒子的重采樣次數(shù)越多;但當(dāng)初始化中心值偏離后驗(yàn)概率過(guò)多時(shí),會(huì)直接導(dǎo)致粒子枯竭,此時(shí)重采樣次數(shù)反而減少,所以?xún)?yōu)選需要在中心值與重采樣次數(shù)符合正比例關(guān)系的范圍內(nèi)進(jìn)行。為了達(dá)到粒子中心值的選取精度,可以采用迭代本算法。

本文以四旋翼飛行器的姿態(tài)估計(jì)為例,詳述IDOPF算法步驟。為了避免粒子濾波算法的狀態(tài)高維數(shù)引起的計(jì)算量過(guò)大、難收斂等問(wèn)題,設(shè)計(jì)中將姿態(tài)劃分為三個(gè)通道,每個(gè)通道依次進(jìn)行濾波計(jì)算。俯仰角θ通道IDOPF算法步驟為:

(1)給出俯仰角θ通道的狀態(tài)模型:

(9)

觀測(cè)方程式(9)是認(rèn)為歐拉角速度和體坐標(biāo)系角速度一致而近似得到的。其中:

(10)

(11)

(12)

其他通道的估計(jì)過(guò)程與上述過(guò)程類(lèi)似。

4 數(shù)值仿真

為了驗(yàn)證算法的有效性,本文基于文獻(xiàn)[6]提供的物理數(shù)據(jù),采用matlab/simulink設(shè)計(jì)了基于反步法的四旋翼飛行器姿態(tài)仿真控制器,獲得了可靠的控制數(shù)據(jù)及姿態(tài)角度信息,并采用IDOPF算法對(duì)含有噪聲的姿態(tài)信息進(jìn)行估計(jì)。仿真實(shí)驗(yàn)中,傳感器采樣周期為20 ms;三軸姿態(tài)角速度初始真實(shí)值為(1,1,1);角速度調(diào)整目標(biāo)值為(0,0,0);三軸姿態(tài)角初始真實(shí)值為(1,1,1);姿態(tài)角調(diào)整目標(biāo)值為(0,0,0);初始姿態(tài)方差為(1 rad)2;初始姿態(tài)速度方差為(0.1 rad/s)2;初始姿態(tài)速度誤差為0.3 rad/s;過(guò)程方差為(0.3 rad)2;粒子數(shù)為500;重要密度函數(shù)中心值精度為1。

仿真初始階段需要對(duì)重要密度函數(shù)進(jìn)行優(yōu)選,根據(jù)初始化粒子群的退化程度選擇更近似后驗(yàn)概率的重要密度函數(shù),結(jié)果如圖1所示。

圖1 粒子中心值分布與重采樣結(jié)果Fig.1 Center value distribution and resampling results of particles

中心值選取[1,15]中的整數(shù),經(jīng)過(guò)初始階段的重采樣,[1,10]區(qū)間的大權(quán)值粒子復(fù)制次數(shù)數(shù)值變化基本符合正比例關(guān)系,累計(jì)誤差也隨著重要密度函數(shù)的中心值偏離后驗(yàn)概率分布而增加;[11,15]區(qū)間的復(fù)制次數(shù)明顯偏低、且不滿(mǎn)足正比例關(guān)系,這是由于中心值偏離后驗(yàn)概率分布過(guò)大(偏離倍數(shù)為10倍),從而粒子枯竭而造成的。初始化粒子群用于俯仰角θ通道的濾波估計(jì)得到圖2所示的一組曲線(xiàn),重要密度函數(shù)中心值選取在[1,10]上的粒子群能夠?qū)崿F(xiàn)狀態(tài)的估計(jì)。

圖2 不同重要密度函數(shù)粒子群的估計(jì)曲線(xiàn)Fig.2 Estimation of particle swarm of different important density function

由圖1和圖2可知,隨著中心值偏離真實(shí)后驗(yàn)密度概率函數(shù)程度越大,IDOPF粒子退化程度越嚴(yán)重,狀態(tài)估計(jì)的收斂速度越慢、估計(jì)精度越低,粒子枯竭的樣本基本無(wú)法用于濾波估計(jì)。

經(jīng)過(guò)重要密度函數(shù)優(yōu)選的粒子群可用于后繼的狀態(tài)估計(jì)中,實(shí)驗(yàn)著重考察了IDOPF算法在四旋翼飛行器姿態(tài)估計(jì)中的效果,與同等條件下的擴(kuò)展卡爾曼濾波EKF算法進(jìn)行比對(duì),數(shù)值仿真結(jié)果如圖3、圖4所示。

通過(guò)圖3可以發(fā)現(xiàn),在初始有誤差情況下,EKF和IDOPF估計(jì)算法均能較快調(diào)整到觀測(cè)值附近。在測(cè)量值變化劇烈的區(qū)域中,IDOPF曲線(xiàn)相比EKF更平滑。由于受到噪聲的影響,EKF算法容易出現(xiàn)誤估計(jì)的情況,而IDOPF算法能夠穩(wěn)定地保持在觀測(cè)值附近。IDOPF算法和EKF都能根據(jù)觀測(cè)值對(duì)真實(shí)值進(jìn)行有效估計(jì),但當(dāng)局部線(xiàn)性化數(shù)值結(jié)果不良時(shí),EKF算法濾波并不穩(wěn)定。

圖3 角速度估計(jì)跟蹤曲線(xiàn)Fig.3 Tracking of angular rate estimation

圖4 算法估計(jì)誤差Fig.4 Algorithm estimation error

由圖4可以發(fā)現(xiàn),當(dāng)數(shù)值波動(dòng)較大時(shí),IDOPF算法估計(jì)誤差保持在±0.1 rad/s,而EKF算法的誤差保持在±0.3 rad/s;在數(shù)值波動(dòng)較小時(shí),兩種算法均保持在很低的誤差范圍內(nèi)。在誤差收斂速度方面,在0.5 s時(shí),IDOPF算法誤差收斂到±0.1 rad/s,EKF算法超過(guò)±0.1 rad/s,兩種算法均存在跟蹤誤差;相比較而言,IDOPF算法誤差小于EKF、波動(dòng)更小,估計(jì)曲線(xiàn)更接近真實(shí)姿態(tài)曲線(xiàn)。

5 結(jié)束語(yǔ)

本文提出將PF算法進(jìn)行改進(jìn)獲得IDOPF算法,并運(yùn)用到四旋翼飛行器姿態(tài)估計(jì)中,解決了由于噪聲造成的傳感器敏感值與真實(shí)值偏差導(dǎo)致姿態(tài)控制發(fā)散的問(wèn)題,同時(shí)也避免了由于重要密度函數(shù)選取不當(dāng)而造成的粒子枯竭及過(guò)早的粒子退化問(wèn)題。相比EKF算法,IDOPF有效提高了估計(jì)精度并降低了姿態(tài)估計(jì)收斂的時(shí)間,使得控制器響應(yīng)更迅速準(zhǔn)確。不足之處是相比于EKF算法,IDOPF需要初始化更多的粒子群,并且后繼的濾波過(guò)程計(jì)算量大,相同水平的硬件平臺(tái)IDOFP運(yùn)行速度較低。為解決上述問(wèn)題,可以考慮利用硬線(xiàn)邏輯電路實(shí)現(xiàn)該算法,從而提高應(yīng)用效率,并可將其運(yùn)用到各種自動(dòng)航行器的實(shí)時(shí)姿態(tài)估計(jì)中。

[1] Salih A L,Moghavvemi M,Mohamed H A F,et al.Modeling and PID controller design for a quad-rotor unmanned air vehicle[C]//2010 IEEE International Conference on Automation Quality and Testing Robotics (AQTR).2010:1-5.

[2] Bouabdallah S,Siegwart R.Backstepping and sliding-mode techniques applied to an indoor micro quad-rotor[C]//2005 IEEE International Conference on Robotics and Automation.Spain,2005:2259-2264.

[3] Mistler V,Benallegue A,M’Sirdi N K.Exact linearization and noninteracting control of a 4 rotors helicopter via dynamic feedback[C]//10th IEEE International Workshop on Robot and Human Interactive Communication. Paris,2001:586-593.

[4] Einicke G A,White L B.Robust extended Kalman filtering[J].IEEE Transactions on Signal Processing,1999,47(9):2596-2599.

[5] Van Der Merwe R,Wan E A,Julier S.Sigma-point Kalman filters for nonlinear estimation and sensor-fusion—applications to integrated navigation[R].AIAA-2004-5120,2004.

[6] Ryll M,Bulthoff H H,Giordano P R.Modeling and control of a quadrotor UAV with tilting propellers[C]//2012 IEEE International Conference on Robotics and Automation (ICRA).2012:4606-4613.

[7] 聶博文.微小型四旋翼無(wú)人直升機(jī)建模及控制方法研究[D].長(zhǎng)沙:國(guó)防科學(xué)技術(shù)大學(xué),2006.

Applicationoftheimprovedparticlefiltertoquad-rotoraircraft’sattitudeestimation

QU Shi-ru, MA Zhi-qiang

(College of Automation, Northwestern Polytechnical University, Xi’an 710072, China)

This paper presents an estimation method based on the important density function optimization particle filter (IDOPF) in order to solve the problem of quad-rotor aircraft’s attitude estimation. The IDOPF which depends on the important density function select severity according to expand the scope of important density estimation function, and the more optimization particles is acquired by important density functions. The experiments of simulation were carrying out on the control dataset of dynamic model and backstepping controller. Eventually, the results of experiments show that, compared with EKF, the IDOPF has higher estimation convergence speed and higher accuracy, which avoids the unstable filtering, improves the filtering effect and verifies that IDOPF is feasible and effective in the quad-rotor attitude estimation.

particle filter; EKF; quad-rotor aircraft’s attitude estimation

V249.1

A

1002-0853(2013)05-0458-04

2013-01-19;

2013-05-14; < class="emphasis_bold">網(wǎng)絡(luò)出版時(shí)間

時(shí)間:2013-08-21 16:13

航天科技創(chuàng)新基金資助(CASC201104)；航空科學(xué)基金資助(2012ZC53043)

曲仕茹(1963-)，女，山東青島人，教授，博士，研究方向?yàn)榻煌ㄟ\(yùn)輸規(guī)劃與管理；馬志強(qiáng)(1987-)，男，黑龍江哈爾濱人，碩士研究生，研究方向?yàn)闊o(wú)人飛行器姿態(tài)控制。

(編輯：李怡)