王曉東，馬 磊，韋宗毅

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

0 引 言

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

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

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

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

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

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

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

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

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

步驟3 濾波增益更新

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

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

步驟5 估計均方誤差Pk的實時更新

2 傳感器信號噪聲分析及抑制

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

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

2.1 傳感器噪聲分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2 加速度計可信性條件

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

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

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

4 實驗測試

4.1 硬件構(gòu)架

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

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

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

4.2 實驗及結(jié)果

圖2 四旋翼飛行器半實物仿真平臺

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語

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

該姿態(tài)控制系統(tǒng)以基于ARM Cortex M4內(nèi)核的嵌入式系統(tǒng)為硬件平臺，通過基于Sage－Husa方法的卡爾曼濾波器以及相應(yīng)的加速度計判別條件，提高姿態(tài)解算系統(tǒng)的自適應(yīng)性的同時也避免了傳統(tǒng)方法實時估計觀測噪聲而引入野值誤差。本方法提高了模型的精確程度以及姿態(tài)結(jié)果的精度和可靠性。該姿態(tài)解算及控制系統(tǒng)針對陀螺儀與加速度計固有特點，抑制了陀螺儀噪聲的隨機漂移的同時也降低了姿態(tài)結(jié)果的波動性。四旋翼半實物仿真平臺實際測試結(jié)果表明，該系統(tǒng)能夠?qū)崟r獲取可靠的姿態(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）. ［曲仕茹，馬志強.改進的粒子濾波在四旋翼姿態(tài)估計中的應(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è)，等.小型四旋翼飛行器動力學(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.