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

小型尾坐式飛行器自適應(yīng)SRUKF姿態(tài)算法

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

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

姿態(tài)估計(jì);尾坐式飛行器;平方根UKF;自適應(yīng)

小型尾坐式飛行器具有垂直起降、結(jié)構(gòu)簡(jiǎn)單等優(yōu)點(diǎn)，精確的姿態(tài)測(cè)量是實(shí)現(xiàn)其穩(wěn)定控制的基礎(chǔ)。小型尾坐式飛行器傾轉(zhuǎn)過(guò)程中需根據(jù)俯仰角調(diào)整拉重比［1］，這就要求俯仰角有高的精度;小型尾坐式飛行器飛行器導(dǎo)航系統(tǒng)抗干擾能力弱、系統(tǒng)模型誤差和噪聲的不確定性會(huì)導(dǎo)致濾波精度的降低甚至發(fā)散。因此，高精度和具有抗干擾能力的姿態(tài)測(cè)量算法對(duì)飛行器的穩(wěn)定飛行顯得尤為重要。

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

本文針對(duì)由MEMS加速度計(jì)、陀螺儀和磁強(qiáng)計(jì)構(gòu)成的慣導(dǎo)系統(tǒng)，采用自適應(yīng)SRUKF對(duì)姿態(tài)信息進(jìn)行融合，減小了陀螺漂移帶來(lái)的誤差，增強(qiáng)了濾波算法的穩(wěn)定性，提高了系統(tǒng)對(duì)模型和噪聲的自適應(yīng)的能力。仿真和飛行實(shí)驗(yàn)驗(yàn)證了自適應(yīng)SRUKF提高了姿態(tài)解算性能。

1 系統(tǒng)模型

1.1 系統(tǒng)硬件組成

本文采用三軸加速度計(jì)、三軸陀螺、三軸磁強(qiáng)計(jì)各一個(gè)分別測(cè)量飛行器的線加速度、角速度和磁場(chǎng)強(qiáng)度。采用Atmega 2560單片機(jī)完成數(shù)據(jù)的采集、處理和飛行器的控制。系統(tǒng)中機(jī)體坐標(biāo)系定義為“前左上”，導(dǎo)航坐標(biāo)系定義為“東北天”。傳感器的安裝軸與機(jī)體坐標(biāo)系重合。

1.2 運(yùn)動(dòng)模型

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

應(yīng)用龍格-庫(kù)塔法將式(1)離散化，得:

其中，

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

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

由式(3)和式(5)可得，系統(tǒng)狀態(tài)方程為:

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

以加速度計(jì)和磁強(qiáng)計(jì)的輸出作為觀測(cè)值，則觀測(cè)方程為:

式中，［abmb］、［anmn］分別為機(jī)體系和導(dǎo)航系下加速度和磁場(chǎng)強(qiáng)度的值，Cbn［9］為四元數(shù)表示的姿態(tài)矩陣，v(k)為觀測(cè)噪聲，假定為零均值高斯白噪聲，其協(xié)方差為R。

2 自適應(yīng)平方根UKF算法

(2)計(jì)算Sigma點(diǎn)

2.1 平方根UKF

無(wú)跡卡爾曼濾波(UKF)用一系列確定的采樣點(diǎn)對(duì)狀態(tài)的概率密度進(jìn)行近似，經(jīng)過(guò)UT變換，得到估計(jì)的均值和方差，對(duì)于高斯系統(tǒng)可以達(dá)到三階泰勒精度［10］。實(shí)際應(yīng)用中，由于數(shù)值計(jì)算存在舍入誤差等因素，導(dǎo)致協(xié)方差矩陣負(fù)定，影響濾波算法的穩(wěn)定性。平方根UKF(SRUKF)用協(xié)方差平方根代替協(xié)方差，可以保證濾波的數(shù)值穩(wěn)定性。

根據(jù)本文建立的運(yùn)動(dòng)模型，SRUKF的具體步驟為［7，11］:

(1)初始狀態(tài)

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

(3)時(shí)間更新

式中，qr{·}表示矩陣的QR分解，cholupdate{·}表示矩陣Cholesky分解的修正。Wmi，Wci分別為均值和協(xié)方差的權(quán)系數(shù)，β用來(lái)描述狀態(tài)變量的先驗(yàn)分布信息，對(duì)于高斯分布，最佳值為β=2［12］。

(4)量測(cè)更新

χk|k-1是由Xk|k-1和PXk|k-1計(jì)算出的Sigma點(diǎn)。

(5)濾波更新

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

2.2 自適應(yīng)SRUKF(ASRUKF)

為了獲得狀態(tài)向量的最優(yōu)估計(jì)值，必須準(zhǔn)確獲取噪聲的統(tǒng)計(jì)特性。實(shí)際應(yīng)用中，由于模型誤差、量測(cè)信息受到干擾等因素使得噪聲不確定，導(dǎo)致濾波精度降低、不穩(wěn)定。因此，將自適應(yīng)算法引入SRUKF中，減小運(yùn)動(dòng)加速度和磁干擾等造成的姿態(tài)誤差。

根據(jù)極大似然準(zhǔn)則，自適應(yīng)量測(cè)噪聲方差陣滿足［13］:

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

式中，b為可調(diào)節(jié)的參數(shù)，b越小，新息序列對(duì)量測(cè)噪聲估計(jì)值貢獻(xiàn)越大。當(dāng)量測(cè)誤差較大時(shí)，調(diào)節(jié)b的大小，將式(27)中^Rk代入式(18)中，就可以實(shí)時(shí)估計(jì)量測(cè)噪聲方差，進(jìn)而通過(guò)式(21)調(diào)整濾波增益，降低量測(cè)值的權(quán)重，使濾波算法對(duì)噪聲變化具有適應(yīng)性，提高濾波精度。

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

仿真實(shí)驗(yàn)所用傳感器性能參數(shù)如表1所示。將系統(tǒng)安裝在轉(zhuǎn)臺(tái)上，模擬小型尾坐式飛行器水平與垂直狀態(tài)轉(zhuǎn)換的過(guò)渡飛行模式，初始姿態(tài)角為［φ，θ，ψ］=［2°，1°，45°］。采用JAVA語(yǔ)言編制了姿態(tài)顯示界面，如圖1所示，對(duì)傳感器數(shù)據(jù)和姿態(tài)信息進(jìn)行實(shí)時(shí)顯示和存儲(chǔ)。

表1 傳感器性能參數(shù)

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

圖1 姿態(tài)顯示界面

圖2 純陀螺與融合算法解算的姿態(tài)角誤差

對(duì)圖2分析可知，只采用陀螺解算的姿態(tài)角誤差較大，這是由于陀螺的漂移造成的。融合算法抑制了陀螺漂移造成的姿態(tài)誤差，姿態(tài)角誤差小于0.5°，滾轉(zhuǎn)、俯仰、偏航角誤差標(biāo)準(zhǔn)差分別為0.13°、0.06°、0.09°。

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

圖3 偏航角變化曲線

圖4 偏航角誤差曲線

由圖3和圖4可見(jiàn)，當(dāng)存在磁場(chǎng)干擾時(shí)，SRUKF解算誤差明顯增大，而自適應(yīng)算法仍能保證姿態(tài)誤差小于1°。由此可見(jiàn)，自適應(yīng)SRUKF算法能有效抑制干擾，提高了系統(tǒng)了魯棒性。

將系統(tǒng)安裝到飛行器樣機(jī)上，采用遙控方式對(duì)樣機(jī)進(jìn)行控制，在-8℃、微風(fēng)(風(fēng)速小于3 m/s)條件下實(shí)現(xiàn)了樣機(jī)的起飛、傾轉(zhuǎn)和降落，圖5為飛行過(guò)程中某個(gè)時(shí)刻樣機(jī)的實(shí)際狀態(tài)。

圖5 飛行過(guò)程截圖

4 結(jié)論

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

［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］張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算［J］.光學(xué)精密工程，2008，16(10):1963-1970.

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

［4］喬相偉，周衛(wèi)東，吉宇人.基于四元數(shù)粒子濾波的飛行器姿態(tài)估計(jì)算法研究［J］.兵工學(xué)報(bào)，2012，33(9):1070-1075.

［5］張共愿，趙忠.粒子濾波及其在導(dǎo)航系統(tǒng)中的應(yīng)用綜述［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2006，14(6):91-94.

［6］羅秋鳳，高振，牛妍.基于DSC與MEMS的微型測(cè)姿系統(tǒng)研究［J］.傳感技術(shù)學(xué)報(bào)，2013，26(6):861-864.

［7］曲藴杰，孫堯，莫宏偉.小型無(wú)人機(jī)組合導(dǎo)航系統(tǒng)的平方根UKF技術(shù)研究［J］.彈箭與制導(dǎo)學(xué)報(bào)，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傳感器姿態(tài)測(cè)量系統(tǒng)［J］.傳感技術(shù)學(xué)報(bào)，2011，24(5):642-646.

［12］趙琳.非線性系統(tǒng)濾波理論［M］.北京:國(guó)防工業(yè)出版社，2012.

［13］秦偉，苑偉政，常洪龍，等.基于自適應(yīng)UKF算法的MEMS陀螺空中在線標(biāo)定技術(shù)［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2011，19(2): 170-174

武斌(1990-)，男，江蘇南京人，碩士研究生在讀，研究方向?yàn)閭鞲衅鲾?shù)據(jù)融合，wubin435@163.com;

陳鵬(1968-)，男，吉林通化人，博士，教授，碩士生導(dǎo)師，研究方向?yàn)樵O(shè)備智能檢測(cè)技術(shù)。

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