基于Sage－Husa方法的旋翼姿態控制

王曉東，馬 磊，韋宗毅

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

0 引 言

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

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

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

1 卡爾曼濾波器及本文中的應用

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

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

步驟1 狀態一步預測

步驟2 一步預測均方誤差

這里Qk代表系統噪聲的方差矩陣。

步驟3 濾波增益更新

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

步驟4 狀態估計

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

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

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

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

2.1 傳感器噪聲分析

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

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

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

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

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

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

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

狀態增廣后的量測方程為

式中：Zk表征當前觀測結果，Hk＝［1］，ξk 序列表征均值為0標準差為σ′的噪聲序列。

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

圖1 陀螺儀濾波前后數據對比

3 姿態信息融合濾波器設計

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

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

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

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

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

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

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

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

3.2 加速度計可信性條件

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

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

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

4 實驗測試

4.1 硬件構架

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

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

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

4.2 實驗及結果

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

圖3 硬件結構框架

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

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

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

圖4 改進算法與常規融合濾波算法比較

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

圖5 平穩姿態方差對比

通過以上分析得到結論：應用Sage－Husa方法增強姿態解算系統自適應性，并使用加速度計可信性條件后，姿態解算系統對當前環境的自適應性更高，能夠獲得更精確的姿態結果。

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

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

5 結束語

圖6 半實物仿真平臺實測數據

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

［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）. ［曲仕茹，馬志強.改進的粒子濾波在四旋翼姿態估計中的應用 ［J］.飛行力學，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）.［韓志鳳，李榮冰，劉建業，等.小型四旋翼飛行器動力學模型優化 ［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.