基于Sage－Husa方法的旋翼姿態(tài)控制

王曉東，馬 磊，韋宗毅

（西南交通大學(xué) 電氣工程學(xué)院，四川 成都610031）

0 引 言

實(shí)時(shí)獲取精確的姿態(tài)信息是實(shí)現(xiàn)旋翼飛行器自主飛行的首要前提。用于姿態(tài)解算的陀螺儀傳感器更新頻率越高越有助于提高姿態(tài)解算精度，但高頻數(shù)據(jù)更新也會(huì)帶來兩個(gè)問題：①隨機(jī)噪聲影響更顯著；②計(jì)算復(fù)雜度呈指數(shù)增長，難以滿足實(shí)時(shí)性。文獻(xiàn) ［1］通過一種增益可調(diào)的互補(bǔ)濾波器融合陀螺儀及加速度計(jì)姿態(tài)角度信息，實(shí)時(shí)性好但模型辨識(shí)度低；文獻(xiàn) ［2］通過卡爾曼濾波器解算當(dāng)前飛行姿態(tài)，但系統(tǒng)傳遞噪聲難以事先估算；文獻(xiàn) ［3］使用粒子濾波算法實(shí)現(xiàn)陀螺儀與加速度計(jì)姿態(tài)融合，算法復(fù)雜度大，難以滿足實(shí)時(shí)性要求；文獻(xiàn) ［4］通過基于四元數(shù)的自適應(yīng)卡爾曼濾波器獲得姿態(tài)角度，模型線性化過程引入誤差，影響姿態(tài)解算精度。

本文首先設(shè)計(jì)噪聲濾波模型以抑制傳感器隨機(jī)噪聲，之后通過自適應(yīng)信息融合濾波算法獲得姿態(tài)角度。建立ARMA （auto－regressive and moving average model）模型將有色噪聲白化，并通過基于ARMA 模型的卡爾曼濾波器抑制陀螺儀噪聲。之后，為獲得更精確的姿態(tài)角度，建立卡爾曼濾波器融合兩者傳感器信息。為提高系統(tǒng)的自適應(yīng)性，采用Sage－Husa自適應(yīng)方法優(yōu)化融合濾波算法；為彌補(bǔ)Sage－Husa方法容錯(cuò)性差的缺陷，本文提出一種加速度計(jì)可信性判別條件，避免Sage－Husa方法將觀測噪聲帶入姿態(tài)解算系統(tǒng)。

本文通過以上方法提高了姿態(tài)解算系統(tǒng)的模型精確度，并將該系統(tǒng)搭載在四旋翼半實(shí)物仿真平臺(tái)上，以驗(yàn)證該系統(tǒng)的實(shí)際效果。

1 卡爾曼濾波器及本文中的應(yīng)用

本文工作中陀螺儀信號(hào)降噪、姿態(tài)解算使用了卡爾曼濾波器。通過建立ARMA 模型并增廣卡爾曼濾波器系統(tǒng)狀態(tài)方程，估計(jì)當(dāng)前信號(hào)的同時(shí)通過時(shí)間序列分析估計(jì)當(dāng)前噪聲序列，從而將有色噪聲白化；由于陀螺儀與加速度計(jì)的信息融合模型具有時(shí)變性大、野值出現(xiàn)頻率高等特點(diǎn)，本文將Sage－Husa方法應(yīng)用于姿態(tài)融合的卡爾曼濾波器以增強(qiáng)姿態(tài)解算系統(tǒng)的自適應(yīng)性，并提出一種新的加速度信息可信性條件以增強(qiáng)系統(tǒng)容錯(cuò)性。

卡爾曼濾波的基礎(chǔ)算法如式 （1）～ （5）所示［5］。

步驟1 狀態(tài)一步預(yù)測

步驟2 一步預(yù)測均方誤差

這里Qk代表系統(tǒng)噪聲的方差矩陣。

步驟3 濾波增益更新

這里Rk代表觀測噪聲的方差矩陣。

步驟4 狀態(tài)估計(jì)

步驟5 估計(jì)均方誤差Pk的實(shí)時(shí)更新

2 傳感器信號(hào)噪聲分析及抑制

選用Analog Devices 公司的ADIS16405 傳感器，該IMU 內(nèi)置了三軸陀螺儀與三軸加速度計(jì)。數(shù)據(jù)更新頻率為819.2Hz，陀螺儀測量精度為0.0125°／sec，加速度計(jì)測量范圍是±8g，分辨率為3.33mg。

對傳感器原始數(shù)據(jù)降噪處理，通過四元數(shù)法及受力分析將角速度和加速度值轉(zhuǎn)換為相應(yīng)的姿態(tài)角度。使用單個(gè)傳感器獲得的姿態(tài)可靠性較低，其中陀螺儀輸出角速度信息，積分得到的姿態(tài)角信息存在累積性偏差，導(dǎo)致解算角度偏離真實(shí)角越來越大；加速度計(jì)對環(huán)境噪聲敏感，高頻噪聲較大。為此，設(shè)置信息融合濾波器求解傳感器的俯仰角和橫滾角姿態(tài)。為確保信息融合濾波效果，該融合濾波器需要有針對性地抑制陀螺儀積分噪聲，并根據(jù)加速度計(jì)所包含運(yùn)動(dòng)加速度的多少適時(shí)調(diào)整加速度計(jì)可信性條件。

2.1 傳感器噪聲分析

通過分析得知陀螺儀噪聲為平穩(wěn)的高斯有色噪聲，加速度計(jì)噪聲為平穩(wěn)的非高斯有色噪聲。

對于平穩(wěn)、高斯分布的陀螺儀噪聲，使用卡爾曼濾波器抑制噪聲，由于陀螺儀噪聲屬于有色噪聲，在低頻段功率密度較大，建立ARMA 模型將陀螺儀有色噪聲白化，并采用基于ARMA 模型的卡爾曼濾波器抑制陀螺儀噪聲。對加速度計(jì)噪聲使用低通濾波器抑制噪聲。

2.2 基于ARMA模型的卡爾曼濾波器陀螺儀降噪

針對陀螺儀噪聲，進(jìn)行自相關(guān)與偏自相關(guān)檢驗(yàn)后，利用時(shí)間序列分析法建立 （5，3）階的ARMA 模型［6］，如式（6）所示

式中：｛wt｝表示有色噪聲序列，等式左邊的序列亦可稱為AR 模型，描述該函數(shù)的偏自相關(guān)特性；｛vt｝表示與｛wt｝相對應(yīng)的均值為0、標(biāo)準(zhǔn)差為σ的白噪聲序列，等式右邊的序列稱為MA 模型，描述該時(shí)間序列的自相關(guān)特性。

使用卡爾曼濾波器抑制該類隨機(jī)噪聲，并增廣該濾波器的狀態(tài)方程，將得到的ARMA 噪聲模型 （式 （6））也作為狀態(tài)方程的一部分，獲得系統(tǒng)狀態(tài)方程如式 （7）所示

式中：x（k）——陀螺儀輸出信號(hào)的估計(jì)， Wk＝…，代表陀螺儀的AR 噪聲序列，vk表示均值為0 標(biāo)準(zhǔn)差為σ的MA 模型噪聲序列；Γk＝表示噪聲驅(qū)動(dòng)矩陣；Πk，k－1表征與Wk對應(yīng)的序列參數(shù)傳遞矩陣。

狀態(tài)增廣后的量測方程為

式中：Zk表征當(dāng)前觀測結(jié)果，Hk＝［1］，ξk 序列表征均值為0標(biāo)準(zhǔn)差為σ′的噪聲序列。

根據(jù)式 （1）～式 （5）算法實(shí)現(xiàn)卡爾曼濾波算法。經(jīng)過降噪濾波器后，將陀螺儀噪聲方差降低為原始噪聲的3.09%，實(shí)際飛行過程中濾波效果如圖1 所示。在保證有效信號(hào)不丟失的同時(shí)，有效抑制了原始數(shù)據(jù)的波動(dòng)性，提高了數(shù)據(jù)準(zhǔn)確性。

圖1 陀螺儀濾波前后數(shù)據(jù)對比

3 姿態(tài)信息融合濾波器設(shè)計(jì)

3.1 基于Sage－Husa方法的卡爾曼濾波

獲得降噪后的三軸角速度信息后，通過四元數(shù)法更新姿態(tài)角度信息。為了便于姿態(tài)控制將四元數(shù)姿態(tài)角度最終轉(zhuǎn)換為歐拉角［7］，將姿態(tài)信息表示為 “東－北－天”導(dǎo)航坐標(biāo)系下的航向角φ，俯仰角θ，橫滾角ρ信息。相應(yīng)地，采用基于一個(gè)參考點(diǎn)確定加速度計(jì)各個(gè)軸角度的方法，獲得加速度計(jì)相應(yīng)的俯仰角和橫滾角信息。

設(shè)計(jì)信息融合濾波器，利用陀螺儀對角速度的精確測量以及加速度計(jì)低通信號(hào)有效性以獲得更精確的姿態(tài)結(jié)果。加速度計(jì)俯仰角、橫滾角分別為θacc、ρa(bǔ)cc ，陀螺儀獲得的俯仰角和橫滾角分別為θgyro、ρgyro。兩者傳感器獲得的姿態(tài)值之間有如下關(guān)系式

其中，ωx與ωy分別代表陀螺儀X 軸、Y 軸檢測到的零偏隨機(jī)噪聲的積分信息，該隨機(jī)噪聲的均值不為零，數(shù)據(jù)波動(dòng)性較??；abias、a′bias表示加速度計(jì)在兩軸上的零均值噪聲；gbias、g′bias表示陀螺儀在兩軸上的零均值噪聲；θvx、θvy表示加速度計(jì)X 軸、Y 軸檢測到的線運(yùn)動(dòng)加速度分量的作用力。

當(dāng)運(yùn)載體近似靜止或勻速運(yùn)動(dòng)時(shí)，vx→0、vy→0，可忽略不計(jì)。根據(jù)式 （9）可獲得用于信息融合的卡爾曼濾波器系統(tǒng)方程

其中，τbias與τ′bias表示均值為零的高斯噪聲序列，分別作為ωx、ωy零偏隨機(jī)噪聲的一階馬爾科夫過程，表示狀態(tài)傳遞矩陣。

在卡爾曼濾波算法實(shí)時(shí)更新式 （1）～ （5）的同時(shí)，鑒于加速度計(jì)噪聲屬于非高斯信號(hào)，導(dǎo)致融合濾波器姿態(tài)控制系統(tǒng)的觀測噪聲方差具有時(shí)變性，需要增強(qiáng)融合濾波器的自適應(yīng)性，采用帶觀測噪聲時(shí)變估值器的簡化Sage－Husa自適應(yīng)算法［8］優(yōu)化信息融合濾波器。在式 （1）～（5）的基礎(chǔ)上，增添了觀測噪聲實(shí)時(shí)估計(jì)方程，如式 （11）所示，通過Sage－Husa方法實(shí)時(shí)更新系統(tǒng)的觀測噪聲

式中：ε＝（I－HkKk）YYT（I－HkKk）T表示當(dāng)前估計(jì)獲得的觀測噪聲協(xié)方差矩陣，dk表示衰減因子，Hk表征觀測矩陣、Kk表征濾波增益、Y ＝Zk－Hk表征當(dāng)前殘差。

3.2 加速度計(jì)可信性條件

為避免實(shí)時(shí)觀測過程中野值影響Sage－Husa方法估計(jì)結(jié)果的準(zhǔn)確性，增強(qiáng)系統(tǒng)容錯(cuò)性，提出一種新的加速度計(jì)可信性條件，不僅考慮載體當(dāng)前所受總加速度大小的時(shí)候，還估量當(dāng)前總加速度與重力間的相對方向。由于本文主要研究的是旋翼懸停狀態(tài)時(shí)的姿態(tài)控制問題，當(dāng)Z 軸加速度方向與重力方向相同時(shí)加速度姿態(tài)角度更為可信，若加速度計(jì)輸出信息包含過多的運(yùn)動(dòng)加速度信息時(shí)，不使用信息融合濾波更新姿態(tài)，僅依靠陀螺儀角速度信息。

判別加速度計(jì)信息是否可信的閾值條件ζ定義如下

綜上所述，當(dāng)進(jìn)行姿態(tài)更新時(shí)，首先進(jìn)行加速度計(jì)可信性判斷，若加速度計(jì)可信則通過基于自適應(yīng)卡爾曼的信息融合濾波器獲得姿態(tài)結(jié)果，將其作為最終的姿態(tài)解算結(jié)果；若加速度計(jì)不可信則將陀螺儀姿態(tài)作為最終姿態(tài)結(jié)果。

4 實(shí)驗(yàn)測試

4.1 硬件構(gòu)架

采用基于ARM Cortex M4內(nèi)核的STM32F417作為控制芯片。自制四旋翼半實(shí)物仿真平臺(tái)，由四旋翼飛行器機(jī)架、電子調(diào)速器、電機(jī)及萬向節(jié)軸承底座組成。萬向節(jié)與軸承使得四旋翼能夠在水平方向360°水平旋轉(zhuǎn)，在俯仰及橫滾方向±90°旋轉(zhuǎn)。仿真平臺(tái)如圖2所示。

姿態(tài)解算和控制系統(tǒng)按照功能劃分為傳感器ADIS16405、遙控器、STM32主控芯片、四旋翼飛行器半實(shí)物仿真平臺(tái)、存儲(chǔ)模塊、上位機(jī)等6個(gè)模塊。硬件結(jié)構(gòu)框架如圖3所示。

為了驗(yàn)證姿態(tài)確定系統(tǒng)的可行性，在得到四旋翼的三維姿態(tài)后，建立四旋翼飛行器動(dòng)力學(xué)模型［1，10］，并控制四旋翼飛行器的姿態(tài)［11］。

4.2 實(shí)驗(yàn)及結(jié)果

圖2 四旋翼飛行器半實(shí)物仿真平臺(tái)

圖3 硬件結(jié)構(gòu)框架

首先進(jìn)行姿態(tài)融合濾波測試。采集四旋翼飛行器實(shí)際飛行中垂直起降及懸停的原始數(shù)據(jù)，通過SDcard分別存儲(chǔ)單獨(dú)使用陀螺儀、單獨(dú)使用加速度計(jì)、姿態(tài)融合濾波器的輸出結(jié)果。

在ARM 芯片上通過設(shè)置相應(yīng)的浮點(diǎn)運(yùn)算單元 （FPU）實(shí)現(xiàn)正確高效的浮點(diǎn)運(yùn)算。ADIS16405傳感器采樣周期為1.22ms。通過設(shè)置相應(yīng)的外部中斷及定時(shí)器中斷管理程序進(jìn)程，縮減程序執(zhí)行時(shí)間。實(shí)際測試結(jié)果顯示，三軸陀螺儀通過3個(gè)6維卡爾曼濾波器濾波，三軸加速度計(jì)通過3個(gè)2階卡爾曼濾波器濾波，降噪模塊共耗時(shí)363.9μs；信息融合濾波模塊包含可信性判別及2個(gè)2階卡爾曼濾波器共耗時(shí)244.83μs。平均進(jìn)行一次姿態(tài)確定及控制程序的執(zhí)行周期為711.3μs，遠(yuǎn)小于姿態(tài)控制周期。

根據(jù)實(shí)際測試數(shù)據(jù)繪制改進(jìn)算法與常規(guī)融合濾波算法對比如圖4所示，半實(shí)物仿真平臺(tái)上旋翼飛行器保持平衡狀態(tài)，21s時(shí)人為給予機(jī)架－10°偏角。第1小圖與第2小圖是使用本文算法獲得的姿態(tài)結(jié)果與常規(guī)融合算法比較，第3小圖變量幅值為1時(shí)表示未進(jìn)行姿態(tài)融合濾波，僅依靠陀螺儀數(shù)據(jù)更新姿態(tài)。

圖4 改進(jìn)算法與常規(guī)融合濾波算法比較

由圖4可以看出，在13s及18s附近，未使用自適應(yīng)算法獲得的姿態(tài)解算結(jié)果存在輕微晃動(dòng)，如圖5所示是統(tǒng)計(jì)10s～21s各秒內(nèi)姿態(tài)角度方差后作出的對比，由方差對比可知改進(jìn)的融合算法姿態(tài)解算精度效果遠(yuǎn)好于常規(guī)融合濾波算法；由21s人為干擾后姿態(tài)解算結(jié)果對比，改進(jìn)的融合算法對姿態(tài)變化更敏感，對運(yùn)動(dòng)過程中噪聲的抑制性更強(qiáng)。

圖5 平穩(wěn)姿態(tài)方差對比

通過以上分析得到結(jié)論：應(yīng)用Sage－Husa方法增強(qiáng)姿態(tài)解算系統(tǒng)自適應(yīng)性，并使用加速度計(jì)可信性條件后，姿態(tài)解算系統(tǒng)對當(dāng)前環(huán)境的自適應(yīng)性更高，能夠獲得更精確的姿態(tài)結(jié)果。

姿態(tài)控制采用PID 算法，通過半實(shí)物仿真平臺(tái)檢驗(yàn)實(shí)際控制效果。在上電前給予四旋翼飛行器俯仰角和橫滾角一定的初始偏角，記錄上電后四旋翼飛行器恢復(fù)俯仰角0°、橫滾角0°所需的時(shí)間；在平衡狀態(tài)下，多次給予飛行器幅度不同的人為擾動(dòng)，記錄飛行器由擾動(dòng)后姿態(tài)到恢復(fù)平衡位置所需的時(shí)間。俯仰角及橫滾角實(shí)測數(shù)據(jù)如圖6所示。

人為干擾后，四旋翼飛行器的姿態(tài)角度回復(fù)到平衡位置的時(shí)間最快為0.39s，最慢為0.88s。由以上實(shí)驗(yàn)驗(yàn)證了姿態(tài)控制系統(tǒng)的優(yōu)越性，該系統(tǒng)反應(yīng)迅速、抗干擾性強(qiáng)。

5 結(jié)束語

圖6 半實(shí)物仿真平臺(tái)實(shí)測數(shù)據(jù)

該姿態(tài)控制系統(tǒng)以基于ARM Cortex M4內(nèi)核的嵌入式系統(tǒng)為硬件平臺(tái)，通過基于Sage－Husa方法的卡爾曼濾波器以及相應(yīng)的加速度計(jì)判別條件，提高姿態(tài)解算系統(tǒng)的自適應(yīng)性的同時(shí)也避免了傳統(tǒng)方法實(shí)時(shí)估計(jì)觀測噪聲而引入野值誤差。本方法提高了模型的精確程度以及姿態(tài)結(jié)果的精度和可靠性。該姿態(tài)解算及控制系統(tǒng)針對陀螺儀與加速度計(jì)固有特點(diǎn)，抑制了陀螺儀噪聲的隨機(jī)漂移的同時(shí)也降低了姿態(tài)結(jié)果的波動(dòng)性。四旋翼半實(shí)物仿真平臺(tái)實(shí)際測試結(jié)果表明，該系統(tǒng)能夠?qū)崟r(shí)獲取可靠的姿態(tài)信息，滿足四旋翼飛行器系統(tǒng)的姿態(tài)控制精度需求。

［1］Mahony R，Kumar V，Corke P.Multirotor aerial vehicles：Modeling，estimation and control of quadrotor ［J］.Robotics＆Automation Magazine.IEEE，2012，19 （3）：20－32.

［2］Tomic T，Schmid K，Lutz P，et al.Toward a fully autonomous UAV：Research platform for indoor and outdoor urban search and rescue ［J］.Robotics ＆ Automation Magazine.IEEE，2012，19 （3）：46－56.

［3］QU Shiru，MA Zhiqiang.Application of the improved particle filter to quad－rotor aircraft’s attitude estimation［J］.Flight Dynamics，2013，31 （5）：458－461 （in Chinese）. ［曲仕茹，馬志強(qiáng).改進(jìn)的粒子濾波在四旋翼姿態(tài)估計(jì)中的應(yīng)用 ［J］.飛行力學(xué)，2013，31 （5）：458－461.］

［4］Suh YS.Orientation estimation using aquaternion－based indirect Kalman filter with adaptive estimation of external acceleration ［J］.IEEE Transactions on Instrumentation and Measurement，2010，59 （12）：3296－3305.

［5］Aanonsen SI，Nvdal G，Oliver DS，et al.The ensemble Kalman filter in reservoir engineering－a review ［J］.Spe Journal，2009，14 （3）：393－412.

［6］Erdem E，Shi J.ARMA based approaches for forecasting the tuple of wind speed and direction ［J］.Applied Energy，2011，88 （4）：1405－1414.

［7］Do KD，Paxman J.Global tracking control of quadrotor VTOL aircraft in three dimensional space ［C］／／3rd Australian Control Conference，2013：26－33.

［8］Zheng Z，Shirong L，Botao Z.An improved Sage－Husa adaptive filtering algorithm ［C］／／31st Chinese Control Conference，2012：5113－5117.

［9］Bouabdallah S.Design and control of quadrotors with application to autonomous flying ［J］.Ecole Polytechnique Federale de Lausanne，2007.

［10］HAN Zhifeng，LI Rongbing，LIU Jianye，et al.Model optimization of small quad－rotor in the case of center of mass offset［J］.Control Engineering of China，2013，20 （S1）：158－162.（in Chinese）.［韓志鳳，李榮冰，劉建業(yè)，等.小型四旋翼飛行器動(dòng)力學(xué)模型優(yōu)化 ［J］.控制工程，2013，20（S1）：158－162.］

［11］Liu H，Bai Y，Lu G，et al.Robust tracking control of a quadrotor helicopter ［J］.Journal of Intelligent ＆ Robotic Systems，2013：1－14.