改進的粒子濾波在四旋翼姿態估計中的應用

曲仕茹, 馬志強

(西北工業大學 自動化學院, 陜西 西安 710072)

改進的粒子濾波在四旋翼姿態估計中的應用

曲仕茹, 馬志強

(西北工業大學 自動化學院, 陜西 西安 710072)

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

粒子濾波; 擴展卡爾曼濾波; 四旋翼姿態估計

0 引言

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

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

1 粒子濾波基本理論

設非線性系統模型為:

(1)

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

(2)

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

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

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

(3)

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

2 四旋翼飛行器姿態模型

四旋翼飛行器是典型的欠驅動系統,通過4個旋轉槳葉調節六個自由度的姿態。研究中簡化了動力學模型,認為四旋翼飛行器的運動為六自由度的剛體運動,為圍繞三個軸向的轉動(偏航、俯仰和滾轉)和重心沿三個軸向的線運動(進退、側飛和升降)。

機體運動選擇體坐標系進行描述,坐標原點即為機體重心,規定機體俯仰抬頭時為正。結合文獻[6]對四旋翼飛行器的動力學分析,可以獲得動力學關系。體坐標系下三軸轉動的角速度和歐拉角速度的關系為:

(4)

機體坐標系-地面坐標系的旋轉矩陣為:

(5)

式中,cψ表示cosψ;sψ表示sinψ;其余類似。

地坐標系加速度與合外力關系為:

(6)

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

本文的四旋翼飛行器研究限于室內懸停及慢飛情況,姿態變化并不劇烈,所以作以下假設:旋翼中心和機體質心始終在一個水平面上;忽略空氣擾動及空氣摩擦阻力;認為歐拉角速度和體坐標系角速度一致;不考慮旋翼揮舞,認為它只受到與其轉速的平方成正比的升力和反扭力矩作用;認為旋翼質量很輕,不考慮其轉動慣量矩[7];為了提取數學模型,可以直接將旋翼轉速當作系統動力學模型的輸入。在上述假設的基礎上,獲得動力學模型為:

(7)

(8)

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

表1 物理常量Table 1 Physical quantity

3 基于IDOPF的姿態估計算法

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

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

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

(9)

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

(10)

(11)

(12)

其他通道的估計過程與上述過程類似。

4 數值仿真

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

仿真初始階段需要對重要密度函數進行優選,根據初始化粒子群的退化程度選擇更近似后驗概率的重要密度函數,結果如圖1所示。

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

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

圖2 不同重要密度函數粒子群的估計曲線Fig.2 Estimation of particle swarm of different important density function

由圖1和圖2可知,隨著中心值偏離真實后驗密度概率函數程度越大,IDOPF粒子退化程度越嚴重,狀態估計的收斂速度越慢、估計精度越低,粒子枯竭的樣本基本無法用于濾波估計。

經過重要密度函數優選的粒子群可用于后繼的狀態估計中,實驗著重考察了IDOPF算法在四旋翼飛行器姿態估計中的效果,與同等條件下的擴展卡爾曼濾波EKF算法進行比對,數值仿真結果如圖3、圖4所示。

通過圖3可以發現,在初始有誤差情況下,EKF和IDOPF估計算法均能較快調整到觀測值附近。在測量值變化劇烈的區域中,IDOPF曲線相比EKF更平滑。由于受到噪聲的影響,EKF算法容易出現誤估計的情況,而IDOPF算法能夠穩定地保持在觀測值附近。IDOPF算法和EKF都能根據觀測值對真實值進行有效估計,但當局部線性化數值結果不良時,EKF算法濾波并不穩定。

圖3 角速度估計跟蹤曲線Fig.3 Tracking of angular rate estimation

圖4 算法估計誤差Fig.4 Algorithm estimation error

由圖4可以發現,當數值波動較大時,IDOPF算法估計誤差保持在±0.1 rad/s,而EKF算法的誤差保持在±0.3 rad/s;在數值波動較小時,兩種算法均保持在很低的誤差范圍內。在誤差收斂速度方面,在0.5 s時,IDOPF算法誤差收斂到±0.1 rad/s,EKF算法超過±0.1 rad/s,兩種算法均存在跟蹤誤差;相比較而言,IDOPF算法誤差小于EKF、波動更小,估計曲線更接近真實姿態曲線。

5 結束語

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

[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] 聶博文.微小型四旋翼無人直升機建模及控制方法研究[D].長沙:國防科學技術大學,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">網絡出版時間

時間:2013-08-21 16:13

航天科技創新基金資助(CASC201104)；航空科學基金資助(2012ZC53043)

曲仕茹(1963-)，女，山東青島人，教授，博士，研究方向為交通運輸規劃與管理；馬志強(1987-)，男，黑龍江哈爾濱人，碩士研究生，研究方向為無人飛行器姿態控制。

(編輯：李怡)