基于四元素的扩展卡尔曼滤波航姿参考系统算法设计 - 图文

基于四元素的扩展卡尔曼滤波航姿

参考系统算法设计

刘维亭1，白杨2

（江苏科技大学 江苏镇江 212003）

摘 要：为了优化飞行姿态信息，设计一种适用于捷联式航姿参考系统的九态扩展卡尔曼滤波算法，得到最优估计。为了验证此EKF算法的可行性和滤波效果，采用AHRS300惯性测量单元的数据进行MATLAB滤波算法仿真，把感器采集数据平滑计算后，建立精确的MATLAB算法模型作为参考。把两种结果作差比较，计算姿态角误差。从仿真结果可以看出，所设计的算法能够获得较好的姿态角精度，证明此扩展卡尔曼滤波性能良好。

关键词：姿态解算；扩展卡尔曼滤波器；惯性传感器 中图分类号：TJ765

Design of Extended Kalman Filtering for AHRS Algorithm

Based on Four Elements

LIU Wei-ting1, BAI yang2

(Jiangsu University of Science and Technology Jiangsu Zhenjiang 212003, China)

Abstract: in order to optimize the flight information, this paper designs a suitable for Strapdown Attitude Heading Reference System for the nine state extended Kalman filtering algorithm, getting the best estimate suitable. In order to verify the feasibility of this EKF algorithm and filtering effect, used the AHRS300 inertial measurement unit data for the MATLAB filtering algorithm simulation and the smooth computation sensor data to establish MATLAB attitude algorithm model as reference. The paper compars with the two solution to the results of difference of which, and draws the error table. the simulation results see that the designed EKF algorithm can achieve better precision, which has a good filtering performance. Key words: Attitude solution; Extended Kalman filter; Inertial sensor

0 引言

航姿参考系统起源于飞行器相关技术，随着成本的器件成本的不断降低，被广泛的应用于机动车辆与无人机，近年来随着MEMS传感器得到越来越多的重视与应用，大大降低生产成本。在AHRS中，包括基于MEMS的三轴陀螺仪，加速度计和磁强计,数据融合的方法有很多，目前常用的航姿参考系统（AHRS）内部采用的多传感器数据融合进行的航姿解算单元为卡尔曼滤波器。本文设计一种九态扩展卡尔曼滤波算法，取加速度和磁传感器的测量值与估计值之差作为观测量，进行误差校正，可很好的收敛姿态角，提高姿态角精度，适用于捷联式航姿参考系统。 1 姿态角与姿态矩阵的关系

欧拉角和捷联矩阵都能表征姿态，在不同的应用和场合中，相应有不同的姿态表示方法最适宜运用。在这些姿态表示方法中，四元数法由于其出众的数学特性，以及计算效率，最适合用于惯性导航系统的姿

1

态确定。

从载体坐标系到导航坐标系的变换矩阵Cb用欧拉角（横滚角γ，俯仰角θ，航向角ψ）表示如下： ?cosγcosψ-sinγsinθsinψ-cosθsinψsinγcosψ+cosγsinθsinψ?nCb??cosγsinψ+sinγsinθcosψcosθcosψsinγsinψ-cosγsinθcosψ? （1）

??-sinγcosθsinθcosγcosθ??n在已知方向余弦矩阵Cb???Cij??的情况下，可以计算姿态角如下：

n滚转角：γ=arctan2(C32,C33) （2） 俯仰角：θ=arctan2(-C31,C322+C332) （3） 航向角：φ=arctan2(C21,C11) （4） 利用四元素法求解后，得到从载体坐标系到导航坐标系捷联矩阵为：

?a2+b2-c2-d22(bc+ad)2(bd-ac)???aCb??2(bc-ad)a2-b2+c2-d22(ab+cd)? （5）

?2(bd+ac)2(cd-ab)a2-b2-c2+d2???其中a表示载体坐标系，b表示导航坐标系。

2 四元数微分方程求解

计算四元数微分方程方法有求解方法有毕卡逼近法、四阶-龙格库塔法等方法，本文选用毕卡逼近的方

b法来求解方程。采用四元数得到即时修正的微分方程如式，每个采样周期内可根据陀螺测量的角速度值ωib对姿态进行更新：

?0-w1-w2-w3?1?w10w3-w2??q???q2?w2-w30w1? （6） ??w3w2-w10??可通过对式积分得到四元数q，进一步进行四元数的归一化。

捷联式惯性导航系统将根据陀螺的测量量更新姿态，由于 bbb （7） wnb?wib-winbxyzbb而wib???wbwbwb??是陀螺的测量量，ωin是地球表观运动角速度，ωin由式计算得出： bbnwin?Cnwin （8）

bn其中，Cn是捷联姿态矩阵的转置阵，win由下式计算得出：

T?+ω)cosf??(λie??n?ωin??-f? （9） ??-(λ??+ωie)sinf??=这里取λve?=vn=0，w取7.2921151467?10?5rad/s,由此可得wb的计算结果，设=0，fnbie

cosf(RN+h)RM+hbbn1Tb??Qwq （10） wnb??w1w2w3?，q2这里：

?0-w1-w2-w3??w0w-w?3Qw=?1? （11）

w-w0w31??2??w3w2-w10??利用毕卡逼近法得k-1时刻与k时刻四元数的递推关系式：

bbn2

q(tk)?(cos(w)I?sin(w)wW)q(tk-1) （12）

最后将每个时刻的q(tk)进行归一化：

q(tk)?q(tk)222q12?q2?q3?q4 （13）

3 九态扩展卡尔曼滤波算法设计 3.1 EKF状态方程

本文采用的EKF数据融合算法选用九态扩展卡尔曼滤波，滤波方式为采样间接式滤波器，误差状态方程的δx选取如下：

?δp?δx=?δb? （14）

???δa?其中δρ??εNεEεD?是姿态角误差向量；δb??δa???bxbybz??是陀螺零偏误差向量，?δaxδayδaz??是载体系中的机动加速度误差向量，则状态方程可写为：

?x(t)?F(t)δx(t)?W(t) （15） δ其中状态矩阵为：

nb?-?win?Cn0?????F(t)??000? （16）

?00Fa???系统驱动矩阵为：

TTT?Wρ?W(t)??Wg? （17） ????Wα??TW(t)为均值为零的高斯白噪声，且E??w(t)w(τ)???Q(t)?(t-?)，δ是单位脉冲函数。

3.2 EKF观测方程

Z(t)?H(t)δx?v(t) （18）

?δfb??b与估计加速度值f?b之差；δm是磁阻其中Z(t)=?b?，δf是加速度计在载体系测得的加速度值f?δm??b与估计地磁场m?b之差；其中v(t)为均值为零的高斯白噪声，且传感器在载体系内测得的三轴地磁场mTE??v(t)v(τ)???R(t)δ(t-τ)。

离散化及滤波算法流程如下

状态转移矩阵为：

Φk,k-1?I?FkΔT （19） 滤波器增益为：

-T-T-1Kk+1?Pk+1Hk+1(Hk+1PK+1Hk+1?Rk+1) （20）

k+1时刻的误差量为：

+δxk+1?Kk+1Zk+1 （21）

所设计的扩展卡尔曼滤波估计后实时修正了机动加速度、捷联矩阵和陀螺零偏。

4. 仿真测试结果

AHRS300的采样频率为100Hz，采集多组静态和动态下的传感器数据并保存，静态数据取其中一组，

3

仿真时间设为100s。为了验证算法动态精度，动态实验实验仿真设为大约500s左右，图像和误差统计表单位为度，仿真结果如图1、2、3、4所示,：

图1 静态试验仿真下的姿态角 图2 静态试验仿真下的姿态角误差

表1 静态实验仿真下的姿态角误差统计（单位为度）

姿态角 最大误差 最小误差 平均误差 均方误差 滚转角 俯仰角 航向角 0.0041 0.0064 0.000489 -0.0040 -0.0036 0 -0.00079 -0.00054 0.000329 0.000827 0.000857 0.00079

图3 动态试验仿真下的姿态角 图4 动态实验仿真下的姿态角误差

表2 动态实验仿真下的姿态角误差统计（单位为度）

姿态角 滚转角 俯仰角 航向角 最大误差 最小误差 0.0103 0.0100 0.0033 -0.0166 -0.0001 平均误差 均方误差 0.000003 -0.00054 0.0017 0.0023 0.00038

-0.000089 0.00048

5 仿真结果分析

通过AHRS300传感器数据采集，并进行了静态实验和动态实验，并用MATLAB精确模型算法进行误差对比，从静态实验图1、2的结果可以看出，静态试验姿态角最大误差不超过0.01°，平均误差也不超过0.001°，均方误差也在0.001°附近，通过静态实验姿态角误差表1，得出所设计的EKF算法静态精度很好，适用于低姿态系统。动态实验中，取较长仿真时间，从动态实验图3、4和姿态角误差表2看出，姿态角的

4

最大误差在0.01°附近，平均误差不超过0.001°，均方误差也不超过0.01°，此算法可较好的收敛姿态角，可用于高动态环境中。 6 结论

本文设计出一种新的EKF算法，滤波性能较好，把姿态角误差向量，陀螺零偏误差向量，机动加速度误差向量作为状态量，改进了原有的卡尔曼滤波算法，能够得到较高的姿态角精度。从各组实验仿真结果可以看出，姿态角的误差精度皆不超过0.01，证明此算法能较好地优化姿态角精度，可适用于高低动态环境中。随着器件成本的不断降低，它可适用于捷联式航姿参考系统的无人机、机动车辆等。但由于EKF的计算量较大，计算时间可能较长，若计算失误可能导致滤波发散，计算效率上有待提高和改进。

参考文献：

[1] 陈永冰, 钟斌. 惯性导航原理[M].北京: 国防工业出版社, 2007: 7-10.

[2] 张帅勇. 基于MIMU的捷联惯性/GPS组合系统研究[D]. 南京理工大学, 2008.

[3] 秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理. 西安: 西北工业大学出版社, 1998 [4] 谭红力. 航弹族低成本捷联惯导系统关键技术研究[D]. 国防科学技术大学, 2007. [5] 肖乾. 多传感器组合导航系统信息融合技术研究[D]. 哈尔滨工程大学, 2005. [6] 袁政. 无人机航姿参考系统开发及信息融合算法研究[D]. 中南大学, 2012.

[7] 姜朋. 基于MEMS-IMU的捷联式惯性导航系统技术与实现研究[D]. 哈尔滨工程大学, 2012. [8] 丁君. 基于微惯性传感器的姿态算法研究[D]. 上海交通大学, 2013.

[9] 陈宇捷. 基于MEMS的微小型嵌入式航姿参考系统研究[D]. 上海交通大学, 2009. [10] 杨澜. 基于多源信息融合的车辆航姿估计技术研究[D]. 长安大学, 2013.

[11] Lee S, Lee T, Park S. Flight Test Results of UAV Automatic Control Using a Single-Antenna GPS Receiver[C]. IAA Guidance, Navigation, and Control Conference and Exhibit, (Austin, TX), 2003.

[12] Cho A, Kim J, Lee S. Fully Automatic Taxiing, Takeoff and Landing of a UAV only with a Single-Antenna GPS Receiver[C]. AIAA Infotech@ Aero space 2007 Conference and Exhibit, 2007.

?

刘维亭，男，1966年1月出生，博士研究生学历，博士学位，教授，主要研究方向为控制理论与控制工程；智能控制；船舶电气。

白杨，女，1988年7月出生，硕士在读，研究方向为电力电子与电力传动。 邮编：212000 联系方式：15862933933 Email: 785831843@qq.com

5