基于開關擴展卡爾曼濾波的姿態估計?

姚若晨

基于開關擴展卡爾曼濾波的姿態估計?

姚若晨

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

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

動中通；姿態估計；卡爾曼濾波

1 引言

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

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

2 坐標系的建立

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

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

地理坐標系t：以車體的旋轉中心為原點，x軸指向當地的正東方向，y軸指向當地的正北方向，z軸垂直于x軸、y軸且構成右手直角坐標系，這個坐標系也就是慣性器件測量的絕對參考系。

圖1 坐標系關系示意圖Fig.1 The relationship of coordinate system

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

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

3 姿態估計和零偏估計

3.1無航向角姿態更新算法

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

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

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

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

對˙Ctb=Ct

bΩb×兩邊取轉置，得：

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

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

3.2系統方程建立

3.2.1狀態方程

令

則

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

當積分周期t比較短時，狀態轉移矩陣Fk，k-1，上式即為SEKF狀態方程。

3.2.2測量方程

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

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

令

即

上式即為SEKF測量方程。

3.3SEKF算法

設一離散非線性系統滿足

式中，Xk為狀態向量，f（·）為非線性狀態轉移矩陣函數，Yk為觀測向量，h（·）為非線性測量矩陣函數，Wk～N（0，Q）為系統噪聲序列，Vk～N（0，R）為測量噪聲序列。EKF算法步驟如下。

（1）求雅克比陣

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

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

（4）計算增益矩陣H

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

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

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

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

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

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

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

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

得

4 實驗數據處理及分析

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

圖3 實驗裝置Fig.3 Experimental equipment

實驗中，行車圍繞環形操場進行運動，包含了轉彎和加減速運動。圖4為陀螺和加速度計估計姿態角，圖5為EKF和SEKF估計姿態角，圖6為SEKF估計姿態角誤差，圖7為陀螺零偏估計結果。

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

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

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

圖5 EKF和SEKF估計姿態角Fig.5 Estimated angle of EKF and SEKF

圖6 SEKF估計姿態角誤差Fig.6 Estimated angle error of SEKF

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

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

由XW-ADU7612給出的參考姿態角能夠得到實驗中陀螺的零偏均值，將所估計的零偏均值與參考零偏均值進行比較能夠得到所估計陀螺零偏的誤差。實驗中，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 結論

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

［1］李耐和.“動中通”衛星通信技術［J］.現代軍事，2007（7）：60-63.

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

［2］朱豐超.動中通低成本測控系統關鍵技術研究［D］.西安：第二炮兵工程學院，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］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導航原理［M］.西安：西北工業大學出版社，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］張天光，王秀萍，王麗霞.捷聯慣性導航技術［M］.2版.北京：國防工業出版社，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］陸光華，彭學愚，張林讓.隨機信號處理［M］.西安：西安電子科技大學出版社，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］沈曉衛，姚敏立，李琳琳，等.寬帶移動衛星通信低成本組合姿態確定算法［J］.西安交通大學學報，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］高亞楠，陳家斌.慣性導航系統中的Kalman濾波技術［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］黃旭，王常虹，伊國興，等.利用磁強計及微機械加速度計和陀螺的姿態估計擴展卡爾曼濾波器［J］.中國慣性技術學報，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］鄒衛軍，薄煜明.差分線性化EKF濾波方法研究［J］.計算機工程與應用，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