基于开关扩展卡尔曼滤波的姿态估计✴

姚若晨

基于开关扩展卡尔曼滤波的姿态估计✴

姚若晨

（北京理工大学信息与电子学院，北京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