基于開關(guān)擴展卡爾曼濾波的姿態(tài)估計?

姚若晨

基于開關(guān)擴展卡爾曼濾波的姿態(tài)估計?

姚若晨

（北京理工大學(xué)信息與電子學(xué)院，北京100081）

針對低成本動中通系統(tǒng)中的姿態(tài)估計問題，提出一種開關(guān)擴展卡爾曼濾波算法。以無航向角的姿態(tài)更新算法為基礎(chǔ)，根據(jù)微機械陀螺和加速度計分別建立系統(tǒng)狀態(tài)方程和測量方程。針對機動加速度的影響，設(shè)計了三維開關(guān)擴展卡爾曼濾波方程，對載體姿態(tài)角和陀螺零偏進行實時估計。實驗結(jié)果表明，該算法能夠準確估計載體姿態(tài)和陀螺零偏，姿態(tài)角估計誤差小于0.5°，俯仰角和橫滾角估計誤差的方差分別為0.130 1°和0.140 5°，兩軸陀螺零偏誤差均值均小于（2×10-4）°／s，能夠滿足動中通的應(yīng)用要求。

動中通；姿態(tài)估計；卡爾曼濾波

1 引言

動中通（SatCom on-the-move，SOTM）是基于固定通信頻段的運動中的衛(wèi)星通信［1］，近年來，動中通在惡劣通信環(huán)境（聯(lián)合作戰(zhàn)、搶險救災(zāi)等）中作用突出，成為國內(nèi)外研究的熱點。為了保證在運動中天線波束時刻對準衛(wèi)星，需對載體航向和姿態(tài)進行精確估計。當(dāng)前航姿估計按成本高低可分為高成本和低成本兩類。前者是基于高精度姿態(tài)航向參考系統(tǒng)（Attitude and Heading Reference Systems，AHRS）提供的載體的航向和姿態(tài)角，雖然能夠提供準確的信息，但成本過高，嚴重制約了動中通的推廣；后者是使用低成本的陀螺和傾角儀（或加速度計）進行信息融合，估計載體姿態(tài)［2］。由于陀螺存在零偏，積分后存在漂移。傳統(tǒng)方法采用加速度計等輔助傳感器，借助擴展卡爾曼濾波器（EKF）和互補濾波器融合兩者信息，提高姿態(tài)估計精度，但加速度計易受機動加速度的干擾，傳統(tǒng)融合方法無法解決。

本文提出了一種開關(guān)擴展卡爾曼算法進行姿態(tài)估計。其主要思想是當(dāng)載體受到機動加速度作用時僅依靠陀螺進行姿態(tài)估計，當(dāng)載體不受機動加速度作用時通過融合陀螺和加速度計信息估計載體姿態(tài)。通過三維開關(guān)擴展卡爾曼濾波（Switched Extended Kalman Filter，SEKF）方程融合了陀螺和加速度計信息，實現(xiàn)了載體姿態(tài)估計。最后利用實測數(shù)據(jù)進行了仿真驗證。

2 坐標系的建立

由于天線的航向角、俯仰角和極化角都是以地理坐標系為參考的，所以動中通測控系統(tǒng)忽略了車體平動因素，認為地理系就是天線波束需要穩(wěn)定的慣性參考系。如圖1所示，建立載體坐標系和地理坐標系［3］如下。

載體坐標系b：以車體的旋轉(zhuǎn)中心為原點，x軸與車體的橫軸重合，指向車體前進方向的右側(cè)；y軸與車體的縱軸重合，指向車體前進方向；z軸與車體的垂直軸重合，指向車體上方。

地理坐標系t：以車體的旋轉(zhuǎn)中心為原點，x軸指向當(dāng)?shù)氐恼龞|方向，y軸指向當(dāng)?shù)氐恼狈较?，z軸垂直于x軸、y軸且構(gòu)成右手直角坐標系，這個坐標系也就是慣性器件測量的絕對參考系。

圖1 坐標系關(guān)系示意圖Fig.1 The relationship of coordinate system

令載體的航向角為ψ，俯仰角為θ，橫滾角為φ，按照ψ→θ→φ的旋轉(zhuǎn)順序，即可得地理坐標系到載體坐標系的旋轉(zhuǎn)矩陣［3］：

其中，S代表正弦運算、C代表余弦運算。

3 姿態(tài)估計和零偏估計

3.1無航向角姿態(tài)更新算法

對于動中通系統(tǒng)，通信衛(wèi)星相對于地球是靜止的，搜索到衛(wèi)星便可以得到載體航向角，初始捕獲衛(wèi)星后不斷進行跟蹤，可以不斷得到航向角度的更新，所以在慣性器件的姿態(tài)更新過程中可以不涉及航向角。無航向角的姿態(tài)更新算法如下。

由于Cbt為正交陣，故Ctb=（Cbt）T，即

式中，C31、C32、C33為由陀螺積分所得俯仰角和橫滾角運算后所得值［4-5］。Ctb可以通過如下公式進行遞推計算［6-9］：

式中，ωx、ωy、ωz分別為三軸陀螺測量值。

對˙Ctb=Ct

bΩb×兩邊取轉(zhuǎn)置，得：

由Ctb第三行元素C31、C32、C33可求出θ、φ。取的第三行，可得僅包含θ、φ的遞推公式為

式中，C31=-cosθsinφ，C32=sinθ，C33=cosθcosφ。由此可通過解算C31、C32、C33求出θ、φ。

3.2系統(tǒng)方程建立

3.2.1狀態(tài)方程

令

則

上式經(jīng)積分即可得到陀螺測量的角度遞推公式：

當(dāng)積分周期t比較短時，狀態(tài)轉(zhuǎn)移矩陣Fk，k-1，上式即為SEKF狀態(tài)方程。

3.2.2測量方程

由加速度計估計的俯仰角θ、橫滾角φ也可以得到C31、C32、C33：

取測量向量Yk=［Ca31Ca32Ca33］T，式中Ca31、Ca32、Ca33為由加速度計估計的俯仰角、橫滾角運算后所得值。

令

即

上式即為SEKF測量方程。

3.3SEKF算法

設(shè)一離散非線性系統(tǒng)滿足

式中，Xk為狀態(tài)向量，f（·）為非線性狀態(tài)轉(zhuǎn)移矩陣函數(shù)，Yk為觀測向量，h（·）為非線性測量矩陣函數(shù)，Wk～N（0，Q）為系統(tǒng)噪聲序列，Vk～N（0，R）為測量噪聲序列。EKF算法步驟如下。

（1）求雅克比陣

（2）計算一步預(yù)測估計值Xk，k-1

（3）計算未考慮噪聲均方誤差陣Pk，k-1

（4）計算增益矩陣H

（5）計算濾波估計值Xk，k

（6）計算濾波器的均方誤差陣Pk，k

EKF算法的原理是將狀態(tài)方程和量測方程中的非線性部分用Taylor級數(shù)展開并略去非線性項，之后再按照卡爾曼濾波的方法進行運算［10-12］。當(dāng)載體受到機動加速度作用時，由加速度計得到的測量值完全不可信，此時只能通過狀態(tài)方程來進行估計。因此，設(shè)計了開關(guān)擴展卡爾曼濾波算法來處理此問題。SEKF算法的狀態(tài)方程和測量方程模型與EKF算法相同，SEKF算法和EKF算法的區(qū)別在于SEKF算法在判斷測量值不準確時，令增益矩陣H=0，僅依靠狀態(tài)方程進行估計。SEKF算法中的開關(guān)判斷條件需要根據(jù)實際情況來選取，門限值的設(shè)定需要經(jīng)過多次實驗來確定。其算法的流程圖如圖2所示。

圖2 SEKF算法流程圖Fig.2 The folwchart of SEKF algorithm

考慮到加速度計往往受到向心和徑向兩種非重力加速度的影響，這里載體受非重力加速度的判斷條件為

其中，ax、ay、az分別代表三軸加速度計輸出值；ωz為z軸陀螺輸出值；a、b為兩個門限值，需要根據(jù)實際情況設(shè)定。條件1用于判斷載體轉(zhuǎn)彎時x軸加速度計受到向心加速度的影響，條件2用于判斷載體受到線加速度的影響。當(dāng)機動加速度發(fā)生時，SEKF切換到僅依靠陀螺積分得到載體姿態(tài)角，抑制非重力加速度的影響。

經(jīng)過濾波后，能夠得到C31、C32、C33的估計值由此能夠求出載體俯仰角和橫滾角的估計值＾θ、＾φ：

得到載體姿態(tài)角估計值ψ、θ、φ后，以載體姿態(tài)角估計值為參考，對三軸陀螺零偏進行估計。由歐拉角公式

得

4 實驗數(shù)據(jù)處理及分析

為驗證姿態(tài)估計算法，將XW-ADU7612和XW -ADU5220同軸安裝于汽車頂端進行行車實驗。XW-ADU7612是由星網(wǎng)宇達科技公司研制的光纖航姿系統(tǒng)，可以給出被測對象精確的姿態(tài)角（方位、俯仰和橫滾角），其方位角精度為0.1°，俯仰角和橫滾角的精度靜態(tài)時達0.05°，動態(tài)時可達0.1°。XW -ADU5220作為低成本慣性測量單元，以100 Hz輸出三軸陀螺和三軸加速度計數(shù)據(jù)。實驗中以光纖航姿系統(tǒng)XW-ADU7612給出的姿態(tài)角為參考，將微機械慣性測量單元XW-ADU5220輸出的三軸陀螺和三軸加速度計數(shù)據(jù)進行融合，將得到載體姿態(tài)角與參考值進行比較。

圖3 實驗裝置Fig.3 Experimental equipment

實驗中，行車圍繞環(huán)形操場進行運動，包含了轉(zhuǎn)彎和加減速運動。圖4為陀螺和加速度計估計姿態(tài)角，圖5為EKF和SEKF估計姿態(tài)角，圖6為SEKF估計姿態(tài)角誤差，圖7為陀螺零偏估計結(jié)果。

從圖4中可以看出，陀螺零偏導(dǎo)致陀螺輸出角不斷積累誤差，陀螺誤差角近似為線性增長，加速度計受到非重力加速度影響時，會導(dǎo)致輸出角產(chǎn)生較大誤差，兩者均不能單獨使用來進行航姿估計。

圖4 陀螺和加速度計估計姿態(tài)角Fig.4 Estimated angle of gyros and accelerometer

從圖5和圖6中可以看出，EKF算法在載體受到機動加速度作用時不能準確估計載體姿態(tài)，誤差角最大值為4°；SEKF算法能夠準確估計載體姿態(tài)，姿態(tài)角的估計誤差最大值均小于0.5°。兩種算法結(jié)果比較如表1所示。

圖5 EKF和SEKF估計姿態(tài)角Fig.5 Estimated angle of EKF and SEKF

圖6 SEKF估計姿態(tài)角誤差Fig.6 Estimated angle error of SEKF

圖7 陀螺零偏估計結(jié)果Fig.7 Estimated result of gyros′bias

表1 EKF與SEKF誤差角結(jié)果Table 1 The error angle results of EKF and SEKF

由XW-ADU7612給出的參考姿態(tài)角能夠得到實驗中陀螺的零偏均值，將所估計的零偏均值與參考零偏均值進行比較能夠得到所估計陀螺零偏的誤差。實驗中，x軸陀螺零偏為-0.081 8°／s，y軸陀螺零偏為0.052 9°／s。兩軸陀螺零偏誤差均值為x軸（5.04×10-5）°／s、y軸（-3.97×10-5）°／s，均小于（2×10-4）°／s，證明該算法能夠準確估計陀螺零偏。

5 結(jié)論

本文通過開關(guān)擴展卡爾曼姿態(tài)估計算法融合微機械陀螺和加速度計信息，避免了非重力加速度對姿態(tài)估計的影響，得到了較為精確的姿態(tài)角和兩軸陀螺零偏。實驗中，姿態(tài)角的估計誤差控制在0.5°以內(nèi)，俯仰角和橫滾角估計誤差的方差分別為0.130 1°和0.140 5°，兩軸陀螺零偏誤差均值均小于（2×10-4）°／s，能夠滿足動中通的應(yīng)用要求。下一步應(yīng)對開關(guān)判斷條件進行研究，使開關(guān)判斷條件能夠更加準確地判斷載體運動狀態(tài)。

［1］李耐和.“動中通”衛(wèi)星通信技術(shù)［J］.現(xiàn)代軍事，2007（7）：60-63.

LI Nai-he.Satellite communication technology of SOTM［J］. Modern Military，2007（7）：60-63.（in Chinese）

［2］朱豐超.動中通低成本測控系統(tǒng)關(guān)鍵技術(shù)研究［D］.西安：第二炮兵工程學(xué)院，2010：26-54.

ZHU Feng-chao.Research on Key Techniques of Low Cost M＆C System for SOTM［D］.Xi′an：The Second Artillery Engineering College，2010：26-54.（in Chinese）

［3］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安：西北工業(yè)大學(xué)出版社，2007：24-33.

QIN Yong-yuan，ZHANG Hong-yue，WANG Shu-hua. Kalman filter and rinciple of compounding navigation［M］. Xi′an：Northwestern Polytechnical University Press，2007：24 -33.（in Chinese）

［4］Maruthi R A，James T H，Gnana R K.Rigid body attitude control with inclinometer and low-cost gyro measurements［J］.Science Direct Systems＆Control Letters，2003，49（2）：151-159.

［5］Zhou Hongren.Kumar K S P.A current statistical model and adaptive algorithm for estimating maneuvering targets［J］.Journal of Guidance，Control and Dynamics，1984，7（5）：596-602.

［6］張?zhí)旃?，王秀萍，王麗?捷聯(lián)慣性導(dǎo)航技術(shù)［M］.2版.北京：國防工業(yè)出版社，2007：212-229.

ZHANG Tian-guang，W ANG Xiu-ping，W ANG Li-xia. Strapdown Inertial Navigation Technology［M］.2nd ed.Beijing：National Defense Industry Press，2007：212-229.（in Chinese）

［7］陸光華，彭學(xué)愚，張林讓.隨機信號處理［M］.西安：西安電子科技大學(xué)出版社，2002：48-57.

LU Guang-hua，PENG Xue-yu，ZHANG Lin-rang. Random signal processing［M］.Xi′an：Xidian University Press，2002：48-57.（in Chinese）

［8］沈曉衛(wèi)，姚敏立，李琳琳，等.寬帶移動衛(wèi)星通信低成本組合姿態(tài)確定算法［J］.西安交通大學(xué)學(xué)報，2011，45（6）：1-5.

SHEN Xiao-wei，YAO Min-li，LI Lin-lin，et al.Low cost hybrid attitude determination for broadband mobile satellite communication［J］.Xi′an：Journal of Xi′an Jiaotong U-niversity，2011，45（6）：1-5.（in Chinese）

［9］高亞楠，陳家斌.慣性導(dǎo)航系統(tǒng)中的Kalman濾波技術(shù)［J］.火力指揮與控制，2005，31（1）：1-4.

GAO Ya-nan，CHEN Jia-bin.Kalman Filter Algorithm sapplied to INS［J］.Fire Control and Command Control，2005，31（1）：1-4.（in Chinese）

［10］黃旭，王常虹，伊國興，等.利用磁強計及微機械加速度計和陀螺的姿態(tài)估計擴展卡爾曼濾波器［J］.中國慣性技術(shù)學(xué)報，2005，13（2）：27-30，34.

HUANG Xu，WANG Chang-hong，YI Guo-xing，et al. Extended Kalman Filter for IMU Attitude Estimation Using Magnetometer，MEMS Accelerometer and Gyroscope［J］. Journal of China Inertia Technology，2005，13（2）：27-30，34.（in Chinese）

［11］鄒衛(wèi)軍，薄煜明.差分線性化EKF濾波方法研究［J］.計算機工程與應(yīng)用，2009，45（9）：64-66.

ZOU Wei-jun，BO Yu-ming.Research of EKF based on differential linearization［J］.Computer Engineering and Applications，2009，45（9）：64-66.（in Chinese）

［12］Grenon G，An P E，Smith S M，et al.Enhancement of the inertial navigation system for the morpheus autonomous underwater vehicles［J］.IEEE Journal of Oceanic Engineering，2001，26（4）：548-560.

Attitude Estimation Based on Switched Extended Kalman Filter

YAO Ruo-chen
（School of Information and Electronics，Beijing Institute of Technology，Beijing 100081，China）

An attitude estimation method using Switched Extended Kalman Filter（SEKF）is proposed for low cost SatCom on-the-move（SOTM）.Based on no-heading attitude updating algorithm，the state function and the measure function are estibalished respectively by gyros and accelerometers.For the effect of movement acceleration，three dimensions SEKF equation is designed for estimation of attitude and gyros′bias.The results of experiment indicate that the method can precisely estimate attitude and gyros′bias，which satisfies the demand of SOTM.Estimated error of attitude is less than 0.5°，and variance of pitch and roll is 0.1301°and 0.1405°，respectively.The estimated mean error of gyros′bias is less than（2×10-4）°／s.

satCom on-the-move（SOTM）；attitude estimation；Kalman filter

TN92

A

10.3969／j.issn.1001-893x.2012.05.014

姚若晨（1991—），男，山西稷山人，主要研究方向為通信與信號處理。

1001-893X（2012）05-0674-06

2011-12-02；

2012-02-20

YAO Ruo-chen was born in Jishan，Shanxi Province，in 1991.His research concerns communication and signal processing.

Email：yrc514725949＠126.com