黎遠松，梁金明

（四川理工學(xué)院計算機學(xué)院，四川 自貢 643000）

基于ILOPF的四旋翼姿態(tài)估計算法設(shè)計與實現(xiàn)*

黎遠松，梁金明

（四川理工學(xué)院計算機學(xué)院，四川 自貢 643000）

針對四旋翼飛行器姿態(tài)角速率的測量伴隨隨機噪聲，直接進行姿態(tài)信息融合的結(jié)果波動劇烈的問題，提出了一種結(jié)合EKF的改進線性優(yōu)化粒子濾波算法。該方法將EKF融合到重要密度函數(shù)中，改進了線性優(yōu)化過程的結(jié)合策略，考慮算法的實時性，對其進行了相應(yīng)的簡化；實現(xiàn)了仿真條件下四旋翼飛行器姿態(tài)角速率的估計，統(tǒng)計結(jié)果顯示，相比EKPF算法，ILOPF的濾波效率提高了20%。在飛行樣機的飛行過程中使用ILOPF，表明了其有效性。

粒子濾波，姿態(tài)估計，EKF，四旋翼飛行器

0 引言

四旋翼飛行器是典型的多耦合、非線性欠驅(qū)動系統(tǒng)，近年來，因其飛行條件低，物理易實現(xiàn)等特點，成為控制領(lǐng)域研究的新熱點。多國研究機構(gòu)為其進行數(shù)學(xué)建模、控制器設(shè)計和飛行樣機制作［1］。為了實現(xiàn)飛行器的有效控制，需要準確描述飛行器的飛行姿態(tài)。加速度在動態(tài)過程中由于受到高頻振動的影響，無法單獨解算出空間的姿態(tài)角，因此，四旋翼飛行器的姿態(tài)信息由三軸加速度和三軸角速率經(jīng)由信息融合算法獲得，陀螺儀在測量過程中由于受到擾動和自身量測噪聲影響，角速率數(shù)據(jù)會產(chǎn)生高頻波動，直接使用這樣的數(shù)據(jù)進行融合獲得的姿態(tài)信息無法用于控制系統(tǒng)。常用的抗干擾方法有卡爾曼濾波算法、四元數(shù)融合技術(shù)及其他機械抗抖動措施，其中卡爾曼濾波器或互補濾波器不能很好地對四旋翼飛行器的非線性環(huán)節(jié)進行估計，四元數(shù)算法能解決姿態(tài)角解算中出現(xiàn)奇點的問題，不過不能很好地抵抗噪聲，機械措施設(shè)計難度大，實現(xiàn)成本高［2-3］。本文結(jié)合擴展卡爾曼濾波算法和粒子濾波算法，提出一種適用于四旋翼飛行器角速率估計的簡化線性優(yōu)化粒子濾波算法，該方法比擴展卡爾曼濾波算法更適用于非線性模型，而且簡化了粒子濾波過程，提高了算法效率，符合四旋翼飛行器飛行的實時性要求，在數(shù)據(jù)進行融合前，進行快速的濾波處理，抑制擾動。

簡化線性優(yōu)化粒子濾波算法的思想是：解決基本粒子濾波算法中重要性權(quán)值偏差較大的問題，采用高頻率的重采樣策略，這種方法在工程應(yīng)用中被證明是有效的［4］；解決高頻率重采樣策略帶來的粒子退化和多樣性匱乏問題，引入線性優(yōu)化策略，提高粒子的多樣性；為了滿足實時性要求，對線性優(yōu)化算法進行了簡化。

本文首先建立了四旋翼飛行器姿態(tài)的基本數(shù)學(xué)模型，接著研究了四旋翼飛行器姿態(tài)角速率估計的改進粒子濾波算法，然后在仿真條件下對文中算法進行試驗和評價，最后通過樣機飛行數(shù)據(jù)證明了本文算法的有效性。

1 飛行器模型描述

四旋翼飛行器的機體坐標原點選為機體中心，飛行器姿態(tài)使用歐拉角描述，取機頭朝向為x軸方向，機體中心指向機體右側(cè)為y軸方向，機體中心垂直水平面指向地心為z軸方向，機體坐標系的選擇符合右手定則，繞x軸轉(zhuǎn)動與水平面形成的夾角為滾轉(zhuǎn)角，繞y軸轉(zhuǎn)動與水平面形成的夾角為俯仰角，繞z軸轉(zhuǎn)動與過z軸的水平垂面的夾角為偏航角，分別用φ，θ，ψ表示，角度的正負根據(jù)右手定則判斷。地面坐標系選擇和機體坐標系相同的表示軸系。

結(jié)合Paul Pounds等人對四旋翼飛行器的動力學(xué)分析［5］，可以獲得如下的動力學(xué)關(guān)系：

體坐標系下三軸轉(zhuǎn)動的角速率同歐拉角速率的關(guān)系：

其中,θ，φ，ψ分別為三軸的角度，U為4個旋翼的運動控制量，Ω為4個旋翼的轉(zhuǎn)速，其他為測量獲得的物理常量見表1。

根據(jù)前述知識，可以根據(jù)該四旋翼飛行器模型建立姿態(tài)穩(wěn)定控制器。為了說明問題，本文結(jié)合基于反步法的姿態(tài)穩(wěn)定控制器討論本文中的濾波算法在姿態(tài)估計中的性能。

2 改進線性優(yōu)化粒子濾波算法

粒子濾波經(jīng)過經(jīng)典的重采樣過程后，粒子的多樣性降低［6］，線性優(yōu)化方法將候選粒子劃分為拋棄組與復(fù)制組，重采樣過程完成后，將拋棄組和復(fù)制組的粒子進行線性組合，形成新的粒子集，進入下一次估計。這種方法的優(yōu)點在于，使用新的粒子代替單一重采樣后的粒子，降低了粒子集中粒子的重復(fù)率，而且用這種方法產(chǎn)生的粒子分布更接近真實的后驗概率分布。本文提出的改進線性優(yōu)化粒子濾波算法在上述思想基礎(chǔ)上進行了適當調(diào)整，針對四旋翼飛行器的姿態(tài)角速率估計問題，以降低算法復(fù)雜度和提高實時性為目的，另外，該方法還能有效提高粒子集的多樣性。

2.1 EKF算法

為了設(shè)計姿態(tài)角速率的粒子濾波算法，需要找到能夠比較準確反映姿態(tài)角速率概率分布的重要密度函數(shù)。由于四旋翼飛行器系統(tǒng)是非線性的，經(jīng)典卡爾曼濾波算法對其失效，Buay等人針對非線性系統(tǒng)，提出的EKF算法是一種次優(yōu)濾波方法，其截取非線性系統(tǒng)的泰勒展開式的低階項，忽略高階項，將非線性問題線性化［7］。將EKF作用到四旋翼飛行器姿態(tài)角速率估計中，得到如圖1所示的狀態(tài)估計曲線，從曲線中可以看出，絕大部分時間里，EKF都比較有效地估計了角速率的狀態(tài)，局部有所偏差是由于線性化效果不理想導(dǎo)致的［7］。從而得出如下結(jié)論，可以通過使用EKF算法獲得較為接近真實后驗概率分布后驗概率密度函數(shù)。

在本文算法中，可以用EKF對粒子集進行更新，將最后得到的近似后驗密度作為重要性密度函數(shù)，即

由此產(chǎn)生新的粒子集，進行權(quán)值更新后對粒子集進行重采樣，就是所謂的EKPF算法，在此基礎(chǔ)上結(jié)合改進的線性優(yōu)化策略，保證運行速率的同時，能夠提高算法的精度。

2.2 改進組合方式

經(jīng)典的線性優(yōu)化粒子濾波算法的主要思想是：在需要重復(fù)采集某個采樣點時，通過將采樣點和被拋棄的采樣點進行合適的線性組合而產(chǎn)生一個新的采樣點，線性組合的方式為

式中，xn是通過組合方式產(chǎn)生的新采樣點，xa為被重復(fù)選擇的采樣點；xs是被拋棄的采樣點，L=（Nw）-1/m，N為粒子個數(shù)，m為采樣空間維度，w為任意采樣點鄰域空間內(nèi)采樣點的分布概率，劃分粒子為拋棄組和復(fù)制組的閾值為：

參考上述線性優(yōu)化過程，提出一種增大隨機度的改進組合方式，主要思想是：在某一組估計進行中，依區(qū)分閾值ωThr將粒子集劃分為復(fù)制組和拋棄組，并分別歸一化權(quán)值，采用輪盤賭方法對歸一化權(quán)值后的復(fù)制組進行重采樣，并得出本次估計的結(jié)果，采用輪盤賭方法分別取復(fù)制組和拋棄組中的一個采樣點，依照式（8）組成一個新的采樣點

其中，randn為隨機數(shù)，用此方法產(chǎn)生N個采樣點作為下一次估計使用的粒子集［8］。

2.3 改進的線性優(yōu)化粒子濾波算法流程

Step1：初始化粒子集，根據(jù)需要估計的狀態(tài)的值域范圍，隨機生成N個采樣點，組成粒子集，本例中，合適的值域范圍是［0,1］；

Step2：參考EKPF獲得更新后的粒子集，及相應(yīng)的重要性權(quán)值；

Step3：對粒子集進行權(quán)值歸一化，并將其分別劃分為復(fù)制組和拋棄組，并分別歸一化；

Step4：每次估計均依照改進組合方式過程進行重采樣步驟和生成下一組采樣點；

Step5：更新狀態(tài)并返回Step2。

3 數(shù)值實驗

3.1 仿真實驗

仿真算例為上文提到的四旋翼飛行器模型，結(jié)合反步法非線性控制系統(tǒng)，獲得完整的仿真姿態(tài)數(shù)據(jù)，分別采用EKPF和本文提出的改進粒子濾波算法對四旋翼飛行器的角速率(p，q，r)進行估計。

仿真過程中，傳感器采樣周期為20 ms，過程噪聲方差Q為0.1(rad/s)2，量測噪聲方差為0.1(rad/s)2，下頁圖2所示為使用不同濾波算法對從非平穩(wěn)姿態(tài)調(diào)整到平穩(wěn)姿態(tài)過程中的角速率的估計，是四旋翼飛行器姿態(tài)調(diào)整的重要，并且會反復(fù)出現(xiàn)的環(huán)節(jié)，具備很強的典型性。為了分析算法性能，給出兩種算法的均方根誤差曲線，均方誤差公式為

圖3 和圖4分別為滾轉(zhuǎn)和俯仰通道的均方根誤差曲線，偏航角通道的情況同他們類似，從圖3和圖4中容易發(fā)現(xiàn)，兩種算法隨著粒子數(shù)量的增加，算法的均方根誤差減小，本例中，在粒子數(shù)為50時，算法的均方根誤差基本穩(wěn)定，相比EKF算法，ILOPF和EKPF的均方根誤差更小，而本文的改進算法比EKPF有更小的均方根誤差，精度更高。

為了考察本例中兩種算法的濾波效率，兩種算法的粒子數(shù)分別取為100、200和500，運行50次，統(tǒng)計其平均數(shù)據(jù)，感興趣的數(shù)據(jù)為濾波效率、均方根誤差和狀態(tài)估計的平均時間，其中，濾波效率計算為式（10）所示，其中N為算法使用的粒子數(shù)目。

結(jié)果如表2所示。從表2中的數(shù)據(jù)可以看出，ILOPF算法和EKPF算法都保有比較穩(wěn)定的AMSE值，過程噪聲增大時，ILOPF算法的AMSE有所降低，說明該算法對噪聲的抑制效果比較明顯。相同條件下ILOPF算法的AMSE普遍低于EKPF算法，而且更低于EKF算法，說明ILOPF算法的波動小，精度高，從濾波效率觀察，容易發(fā)現(xiàn)，ILOPF算法的效率比EKPF算法高出20%。研究表2還能發(fā)現(xiàn)，ILOPF算法和EKPF算法無法通過增加粒子數(shù)目提高濾波效率，所以，針對不同的算例，選擇合適的粒子數(shù)也對算法的執(zhí)行效果有一定影響，結(jié)合估計時間考慮，兩種算法提高估計精度是通過提高算法中使用粒子數(shù)和犧牲運算時間獲得的，而在粒子數(shù)目并非巨大（一般小于500）時，ILOPF算法的各項性能指標都優(yōu)于EKPF算法，所以在進行一般的狀態(tài)估計時，優(yōu)先考慮使用ILOPF算法，而且出于實時性和效率的考慮，建議算法中的粒子數(shù)不多于200為宜。

3.2 實驗數(shù)據(jù)

為了進一步驗證算法的有效性，將ILOPF算法應(yīng)用到實驗用四旋翼飛行器樣機上，該樣機使用X650機架，使用STM32F103VET6為主控芯片，使用MPU6050和HMC5883L相結(jié)合制作的AHRS，傳感器采樣周期為20 ms，將ILOPF算法融合到航向姿態(tài)解算過程，能夠?qū)崿F(xiàn)飛行器的穩(wěn)定飛行，飛行器記錄儀記錄的部分飛行數(shù)據(jù)繪制成如圖5和圖6曲線。圖5曲線為0 s～25 s內(nèi)的飛行姿態(tài)統(tǒng)計，在這25s內(nèi)，飛行器姿態(tài)從平穩(wěn)到滾擺并附帶一定的偏航角度，最終在25 s左右只進行俯仰操作，從圖中容易發(fā)現(xiàn)，在由于從MUP6050中獲得的角速率信息波動比較大，直接用這種信息進行姿態(tài)融合，獲得的角度存在很大波動，這種無規(guī)律的抖動對控制器的影響很大，控制器使用這樣的信息進行輸出，機體抖動明顯，容易引發(fā)系統(tǒng)的高頻特性，十分容易傾覆，因此，在進行信息融合前，需要使用濾波算法對角速率進行估計，以平滑并削弱這種波動，從圖中曲線容易看出，ILOPF算法有效削弱了量測過程中產(chǎn)生的噪聲，姿態(tài)曲線比濾波前更加平滑。圖6為圖5過程中16 s～20 s的細節(jié)，從中能夠更加清晰地觀察到上述情況，在俯仰通道，姿態(tài)為趨于0 rad的穩(wěn)定狀態(tài)，而由于量測噪聲影響，直接融合獲得的數(shù)據(jù)有接近0.05 rad的波動，而經(jīng)過估計后的數(shù)據(jù)波動不會超過0.02 rad，有效抑制了噪聲，觀察其他通道，同樣能夠獲得類似的結(jié)果。

4 結(jié)束語

本文根據(jù)四旋翼飛行器模型的特點，提出了一種改進的線性優(yōu)化粒子濾波方法。設(shè)計的濾波方法過程簡單，解決了四旋翼飛行器姿態(tài)角速度估計的問題。通過仿真實驗，驗證了算法對姿態(tài)角速率有很好的估計效果，而且均方根誤差保持在很低的水平，粒子選擇合適的情況下，濾波效率比EKPF算法提高了20%，而且具備很強的實時性。飛行樣機記錄的飛行數(shù)據(jù)表明該算法切實可行，可用于一般四旋翼飛行器的姿態(tài)角濾波估計。下一步擬結(jié)合集群優(yōu)化算法進一步改進本算法。

［1］Samir B，Andre N，Roland S.PID vs LQ Control Techniques Applied to an Indoor Micro Quadrotor[C]//IEEE，2004：2451-2456.

［2］王帥,魏國.卡爾曼濾波在四旋翼飛行器姿態(tài)測量中的應(yīng)用[J].兵工自動化，2011，30（1）:73-80.

［3］馬敏，吳海超.基于四元數(shù)自補償四旋翼飛行器姿態(tài)解算[J].制造業(yè)自動化，2013，35（23）:18-21.

［4］Crisan D,Doucet A.A Survey of Convergence Results on Particle Filtering Methods for Practitioners[J].Signal Processing, IEEE Transactions on，2002，50（3）：736-746.

［5］Pounds P，Mahony R，Corke P.Modelling and Control of a Large Quadrotor Robot[J].Control Engineering Practice，2010，18（7）：691-699.

［6］Cheng S Y,Zhang J Y.Review on Particle Filters[J].Journal of Astronautics，2008，29（4）：1099-1111.

［7］Garry A E，Langford B W.Robust Extended Kalman Filtering [J].Signal Processing，IEEE Transactions on，1999，47（9）：2596-2599.

［8］Chen J，Yan P，Zhang J Y.Research on Weight Optimal Combination Particle Filter Algorith[J].Computer Engineering and Applications，2009，45（24）：33-35.

［9］Liu Z L，Liu T，Cao J.Fusion Tracking Algorithm Based on Particle Filter[J].Science Technology and Engineering，2009（7）：1751-1753.

［10］Wang L，Xia H P.Application in Target Tracking Based on Tabu Search Particle Filter Algorithm[J].Science Technology and Engineering，2013（6）：1630-1634.

Design and Implementation of ILOPF in Attitude Estimation of Quadrotor

LI Yuan-song，LIANG Jin-ming
（School of Computer，Sichuan University of Science and Engineering，Zigong 643000，China）

For the problem of the volatile fusion result of quadrotor caused by the random noise occurring in the measurement of attitude angular rate.This paper presents an improved linear optimization particle filter with EKF.Firstly,EKF into important density function is integrated; secondly,the combination strategy of linear optimization process is improved,and as the algorithm is real-time,it is simplified according.In the simulation,the algorithm achieves the estimation of attitude angular rate.Statistic shows that compared on EKPF,filtering efficiency of ILOPF increase by 20%. Physics flight with ILOPF demonstrates its effectiveness.

PF，attitude estimation，EKF，quadrotor

TP391

A

1002-0640（2015）09-0167-05

2014-08-15

2014-09-20

四川省教育廳科研基金(13ZA0125)；四川省高校重點實驗室開放基金(2014WZY05)；企業(yè)信息化與物聯(lián)網(wǎng)測控技術(shù)四川省高校重點實驗室項目（2014WZY03）；軟件工程專業(yè)綜合改革基金資助項目（B12201002）

黎遠松（1970- ），男，重慶開縣人，碩士，副教授。研究方向：智能算法設(shè)計等。