黎遠(yuǎn)松，梁金明

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

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

黎遠(yuǎn)松，梁金明

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

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

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

0 引言

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

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

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

1 飛行器模型描述

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

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

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

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

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

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

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

2.1 EKF算法

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

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

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

2.2 改進(jìn)組合方式

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

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

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

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

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

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

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

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

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

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

3 數(shù)值實(shí)驗(yàn)

3.1 仿真實(shí)驗(yàn)

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

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

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

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

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

3.2 實(shí)驗(yàn)數(shù)據(jù)

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

4 結(jié)束語

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

［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].兵工自動(dòng)化，2011，30（1）:73-80.

［3］馬敏，吳海超.基于四元數(shù)自補(bǔ)償四旋翼飛行器姿態(tài)解算[J].制造業(yè)自動(dòng)化，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)；四川省高校重點(diǎn)實(shí)驗(yàn)室開放基金(2014WZY05)；企業(yè)信息化與物聯(lián)網(wǎng)測控技術(shù)四川省高校重點(diǎn)實(shí)驗(yàn)室項(xiàng)目（2014WZY03）；軟件工程專業(yè)綜合改革基金資助項(xiàng)目（B12201002）

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