小型尾坐式飛行器自適應SRUKF姿態算法

武斌，陳鵬，胡永江，王長龍(軍械工程學院，石家莊050003)

小型尾坐式飛行器自適應SRUKF姿態算法

武斌，陳鵬*，胡永江，王長龍
(軍械工程學院，石家莊050003)

針對小型尾坐式飛行器姿態估計問題，設計了由陀螺、加速度計、磁強計組成的姿態測量系統。為了抑制MEMS陀螺漂移導致的姿態誤差，以四元數為狀態變量，以加速度計和磁強計的輸出作為觀測變量，建立了濾波模型。采用平方根無跡卡爾曼濾波(SRUKF)對傳感器信息進行融合，保證了濾波算法的數值穩定性。由于小型尾坐式飛行器抗干擾能力弱，引入自適應算法，解決了量測信息受到干擾時濾波精度下降的問題，提高了系統的魯棒性和可靠性。仿真結果表明，存在外界磁場干擾時，姿態誤差小于1°。通過實際飛行實驗，驗證了算法的可行性。

姿態估計;尾坐式飛行器;平方根UKF;自適應

小型尾坐式飛行器具有垂直起降、結構簡單等優點，精確的姿態測量是實現其穩定控制的基礎。小型尾坐式飛行器傾轉過程中需根據俯仰角調整拉重比［1］，這就要求俯仰角有高的精度;小型尾坐式飛行器飛行器導航系統抗干擾能力弱、系統模型誤差和噪聲的不確定性會導致濾波精度的降低甚至發散。因此，高精度和具有抗干擾能力的姿態測量算法對飛行器的穩定飛行顯得尤為重要。

張榮輝［2］等從數值解算方法的角度對四元數法姿態解算方法進行了研究，但沒有考慮傳感器自身誤差對姿態估計的影響。文獻［3］采用卡爾曼濾波對陀螺和加速度計信號進行融合，修正了陀螺儀的隨機漂移。這種方法要求系統模型是線性的，噪聲為高斯白噪聲，而實際復雜的環境很難滿足這種條件。粒子濾波［4-5］從理論上來說適用于任何非線性非高斯隨機系統，然而大量的粒子導致計算效率極大地降低，無法滿足系統實時性要求。

本文針對由MEMS加速度計、陀螺儀和磁強計構成的慣導系統，采用自適應SRUKF對姿態信息進行融合，減小了陀螺漂移帶來的誤差，增強了濾波算法的穩定性，提高了系統對模型和噪聲的自適應的能力。仿真和飛行實驗驗證了自適應SRUKF提高了姿態解算性能。

1 系統模型

1.1 系統硬件組成

本文采用三軸加速度計、三軸陀螺、三軸磁強計各一個分別測量飛行器的線加速度、角速度和磁場強度。采用Atmega 2560單片機完成數據的采集、處理和飛行器的控制。系統中機體坐標系定義為“前左上”，導航坐標系定義為“東北天”。傳感器的安裝軸與機體坐標系重合。

1.2 運動模型

以四元數為狀態變量，考慮到陀螺儀存在漂移，為了防止誤差積累，將狀態變量擴增為X=［q0q1q2q3ωxωyωz］T，其中q=［q0q1q2q3］T為姿態四元數，ω=［ωxωyωz］T為陀螺漂移，估計出陀螺漂移并對其進行抑制。四元數微分方程為［6］:

應用龍格-庫塔法將式(1)離散化，得:

其中，

其中，T為陀螺采樣周期。

機體系下陀螺漂移誤差方程為［7-8］:

由式(3)和式(5)可得，系統狀態方程為:

w(k)為系統噪聲，假定為零均值高斯白噪聲，其協方差為Q。

以加速度計和磁強計的輸出作為觀測值，則觀測方程為:

式中，［abmb］、［anmn］分別為機體系和導航系下加速度和磁場強度的值，Cbn［9］為四元數表示的姿態矩陣，v(k)為觀測噪聲，假定為零均值高斯白噪聲，其協方差為R。

2 自適應平方根UKF算法

(2)計算Sigma點

2.1 平方根UKF

無跡卡爾曼濾波(UKF)用一系列確定的采樣點對狀態的概率密度進行近似，經過UT變換，得到估計的均值和方差，對于高斯系統可以達到三階泰勒精度［10］。實際應用中，由于數值計算存在舍入誤差等因素，導致協方差矩陣負定，影響濾波算法的穩定性。平方根UKF(SRUKF)用協方差平方根代替協方差，可以保證濾波的數值穩定性。

根據本文建立的運動模型，SRUKF的具體步驟為［7，11］:

(1)初始狀態

其中，λ=α2(n+κ)-n，n為狀態維數，κ通常為0，α決定了Sigma點離散程度，本文取為0.01。

(3)時間更新

式中，qr{·}表示矩陣的QR分解，cholupdate{·}表示矩陣Cholesky分解的修正。Wmi，Wci分別為均值和協方差的權系數，β用來描述狀態變量的先驗分布信息，對于高斯分布，最佳值為β=2［12］。

(4)量測更新

χk|k-1是由Xk|k-1和PXk|k-1計算出的Sigma點。

(5)濾波更新

其中，εk=Zk-Zk|k-1為新息序列。

2.2 自適應SRUKF(ASRUKF)

為了獲得狀態向量的最優估計值，必須準確獲取噪聲的統計特性。實際應用中，由于模型誤差、量測信息受到干擾等因素使得噪聲不確定，導致濾波精度降低、不穩定。因此，將自適應算法引入SRUKF中，減小運動加速度和磁干擾等造成的姿態誤差。

根據極大似然準則，自適應量測噪聲方差陣滿足［13］:

式中，N為窗口數量，表示所用新息數量的多少。令N=1，得到量測噪聲測量值:

式中，b為可調節的參數，b越小，新息序列對量測噪聲估計值貢獻越大。當量測誤差較大時，調節b的大小，將式(27)中^Rk代入式(18)中，就可以實時估計量測噪聲方差，進而通過式(21)調整濾波增益，降低量測值的權重，使濾波算法對噪聲變化具有適應性，提高濾波精度。

3 仿真實驗

仿真實驗所用傳感器性能參數如表1所示。將系統安裝在轉臺上，模擬小型尾坐式飛行器水平與垂直狀態轉換的過渡飛行模式，初始姿態角為［φ，θ，ψ］=［2°，1°，45°］。采用JAVA語言編制了姿態顯示界面，如圖1所示，對傳感器數據和姿態信息進行實時顯示和存儲。

表1 傳感器性能參數

圖2為只使用陀螺測量值解算的四元數法誤差(圖中虛線)和采用SRUKF融合算法分別解算出的姿態角誤差(圖中實線)。

圖1 姿態顯示界面

圖2 純陀螺與融合算法解算的姿態角誤差

對圖2分析可知，只采用陀螺解算的姿態角誤差較大，這是由于陀螺的漂移造成的。融合算法抑制了陀螺漂移造成的姿態誤差，姿態角誤差小于0.5°，滾轉、俯仰、偏航角誤差標準差分別為0.13°、0.06°、0.09°。

為了驗證自適應算法在量測信息受到干擾時的有效性，在轉臺附近放置磁鐵模擬磁場干擾，初始姿態［φ，θ，ψ］=［0°，0°，-150°］。圖3和圖4分別為SRUKF(圖中虛線)和自適應SRUKF算法(圖中實線)求解的偏航角和誤差曲線。

圖3 偏航角變化曲線

圖4 偏航角誤差曲線

由圖3和圖4可見，當存在磁場干擾時，SRUKF解算誤差明顯增大，而自適應算法仍能保證姿態誤差小于1°。由此可見，自適應SRUKF算法能有效抑制干擾，提高了系統了魯棒性。

將系統安裝到飛行器樣機上，采用遙控方式對樣機進行控制，在-8℃、微風(風速小于3 m/s)條件下實現了樣機的起飛、傾轉和降落，圖5為飛行過程中某個時刻樣機的實際狀態。

圖5 飛行過程截圖

4 結論

本文以加速度計和磁強計的測量值補償陀螺漂移帶來的姿態誤差，推導了采用自適應平方根UKF對傳感器數據進行融合的姿態測量算法，有效的抑制了陀螺漂移導致的姿態累積誤差，提高了姿態解算精度。采用自適應算法，有效的改進了小型尾坐式飛行器易受干擾的不足，提高了系統的魯棒性。由于樣機處于初步研制階段，機體本身抗風能力較弱。因此，需要對機體進行改進，進而對本文提出的算法在更加惡劣環境下的可行性作進一步研究。

［1］Hugh Stone R，Peter Anderson，Colin Hutchison，et al.Flight Testing of the T-Wing Tail-Sitter Unmanned Air Vehicle［J］.Journal of Aircraft，2008，45(2):673-685.

［2］張榮輝，賈宏光，陳濤，等.基于四元數法的捷聯式慣性導航系統的姿態解算［J］.光學精密工程，2008，16(10):1963-1970.

［3］葉锃鋒，馮恩信.基于四元數和卡爾曼濾波的兩輪車姿態穩定方法［J］.傳感技術學報，2012，25(4):524-528.

［4］喬相偉，周衛東，吉宇人.基于四元數粒子濾波的飛行器姿態估計算法研究［J］.兵工學報，2012，33(9):1070-1075.

［5］張共愿，趙忠.粒子濾波及其在導航系統中的應用綜述［J］.中國慣性技術學報，2006，14(6):91-94.

［6］羅秋鳳，高振，牛妍.基于DSC與MEMS的微型測姿系統研究［J］.傳感技術學報，2013，26(6):861-864.

［7］曲藴杰，孫堯，莫宏偉.小型無人機組合導航系統的平方根UKF技術研究［J］.彈箭與制導學報，2009，29(1):91-94.

［8］Zhou Zhanxin，Gao Yanan，Chen Jiabin.Unscented Kalman filter for SINS Alignment［J］.Journal of Systems Engineering and Electronics，2007，18(2):327-333.

［9］Mohamed Okasha，Brett Newman.Switching Algorithm to Avoid Attitude Representation Singularity［C］//AIAA Atmospheric Flight Mechanics Conference.Chicago，2009:1-17.

［10］Crassidis J L.Survey of Nonlinear Attitude Estimation Methods［J］. Journal of Guidance，Control，and Dynamics，2007，30(1):12-28.

［11］趙鶴，王喆垚.基于UKF的MEMS傳感器姿態測量系統［J］.傳感技術學報，2011，24(5):642-646.

［12］趙琳.非線性系統濾波理論［M］.北京:國防工業出版社，2012.

［13］秦偉，苑偉政，常洪龍，等.基于自適應UKF算法的MEMS陀螺空中在線標定技術［J］.中國慣性技術學報，2011，19(2): 170-174

武斌(1990-)，男，江蘇南京人，碩士研究生在讀，研究方向為傳感器數據融合，wubin435@163.com;

陳鵬(1968-)，男，吉林通化人，博士，教授，碩士生導師，研究方向為設備智能檢測技術。

EEACC:7230 doi:10.3969/j.issn.1004－1699.2014.05.013

Attitude Algorithm of Small Tail-Sitter Aircraft Based on Adaptive SRUKF

WU Bin，CHEN Peng*，HU Yongjiang，WANG Changlong

(Ordnance Engineering College，Shijiazhuang 050003，China)

For attitude estimation of small tail-sitter aircraft，the attitude measurement system composed of gyroscope，accelerometer and magnetometer is designed.In order to prevent the attitude error caused by drift of MEMS gyroscope，the filter modelis established with quaternion as state variables and the output ofaccelerometer and magnetometer as measurement information.Square root kalman filter(SRUKF)is carried out on the sensor information fusion to guarantee the numerical stability of filtering algorithm.Due to the weak anti-interference ability of small tail-sitter aircraft，the adaptive algorithm is introduced，which solves the problem of the decline of the filtering precision when measurement information is interferenced and improves the robustness and reliability of the system.Simulation results show that attitude error is less than 1°under the condition of magnetic disturbance.Actual flight test verifies the feasibility of the algorithm.

attitude estimation;tail-sitter aircraft;SRUKF;adaptive

V249

A

1004－1699(2014)05－0633－04

10.3969/j.issn.1004－1699.2014.05.012

2014-03-21

2014-04-22