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

1基于四元素的扩展卡尔曼滤波航姿参考系统算法设计刘维亭1，白杨2（江苏科技大学江苏镇江212003）摘要：为了优化飞行姿态信息，设计一种适用于捷联式航姿参考系统的九态扩展卡尔曼滤波算法，得到最优估计。为了验证此EKF算法的可行性和滤波效果，采用AHRS300惯性测量单元的数据进行MATLAB滤波算法仿真，把感器采集数据平滑计算后，建立精确的MATLAB算法模型作为参考。把两种结果作差比较，计算姿态角误差。从仿真结果可以看出，所设计的算法能够获得较好的姿态角精度，证明此扩展卡尔曼滤波性能良好。关键词：姿态解算；扩展卡尔曼滤波器；惯性传感器中图分类号：TJ765DesignofExtendedKalmanFilteringforAHRSAlgorithmBasedonFourElementsLIUWei-ting1,BAIyang2(JiangsuUniversityofScienceandTechnologyJiangsuZhenjiang212003,China)Abstract:inordertooptimizetheflightinformation,thispaperdesignsasuitableforStrapdownAttitudeHeadingReferenceSystemfortheninestateextendedKalmanfilteringalgorithm,gettingthebestestimatesuitable.InordertoverifythefeasibilityofthisEKFalgorithmandfilteringeffect,usedtheAHRS300inertialmeasurementunitdatafortheMATLABfilteringalgorithmsimulationandthesmoothcomputationsensordatatoestablishMATLABattitudealgorithmmodelasreference.Thepapercomparswiththetwosolutiontotheresultsofdifferenceofwhich,anddrawstheerrortable.thesimulationresultsseethatthedesignedEKFalgorithmcanachievebetterprecision,whichhasagoodfilteringperformance.Keywords:Attitudesolution;ExtendedKalmanfilter;Inertialsensor0引言航姿参考系统起源于飞行器相关技术，随着成本的器件成本的不断降低，被广泛的应用于机动车辆与无人机，近年来随着MEMS传感器得到越来越多的重视与应用，大大降低生产成本。在AHRS中，包括基于MEMS的三轴陀螺仪，加速度计和磁强计,数据融合的方法有很多，目前常用的航姿参考系统（AHRS）内部采用的多传感器数据融合进行的航姿解算单元为卡尔曼滤波器。本文设计一种九态扩展卡尔曼滤波算法，取加速度和磁传感器的测量值与估计值之差作为观测量，进行误差校正，可很好的收敛姿态角，提高姿态角精度，适用于捷联式航姿参考系统。1姿态角与姿态矩阵的关系欧拉角和捷联矩阵都能表征姿态，在不同的应用和场合中，相应有不同的姿态表示方法最适宜运用。在这些姿态表示方法中，四元数法由于其出众的数学特性，以及计算效率，最适合用于惯性导航系统的姿2态确定。从载体坐标系到导航坐标系的变换矩阵nbC用欧拉角（横滚角γ，俯仰角θ，航向角ψ）表示如下：cosγcosψ-sinγsinθsinψ-cosθsinψsinγcosψ+cosγsinθsinψcosγsinψ+sinγsinθcosψcosθcosψsinγsinψ-cosγsinθcosψ-sinγcosθsinθcosγcosθnbC（1）在已知方向余弦矩阵ijCnbC的情况下，可以计算姿态角如下：滚转角：3233γ=arctan2(C,C)（2）俯仰角：22313233θ=arctan2(-C,C+C)（3）航向角：2111φ=arctan2(C,C)（4）利用四元素法求解后，得到从载体坐标系到导航坐标系捷联矩阵为：222222222222a+b-c-d2(bc+ad)2(bd-ac)2(bc-ad)a-b+c-d2(ab+cd)2(bd+ac)2(cd-ab)a-b-c+dabC（5）其中a表示载体坐标系，b表示导航坐标系。2四元数微分方程求解计算四元数微分方程方法有求解方法有毕卡逼近法、四阶-龙格库塔法等方法，本文选用毕卡逼近的方法来求解方程。采用四元数得到即时修正的微分方程如式，每个采样周期内可根据陀螺测量的角速度值bibω对姿态进行更新：1231322313210-w-w-ww0w-w1w-w0w2ww-w0qq（6）可通过对式积分得到四元数q，进一步进行四元数的归一化。捷联式惯性导航系统将根据陀螺的测量量更新姿态，由于-bbbnbibin（7）而Txyzbbbbibw是陀螺的测量量，binω是地球表观运动角速度，binω由式计算得出：bbninninwCw（8）其中，bnC是捷联姿态矩阵的转置阵，ninw由下式计算得出：ieie(λ+ω)cosf-f-(λ+ω)sinfninω（9）这里取eNvλ==0cosf(R+h)，nMvf=R+h=0，iew取57.292115146710rad/s,由此可得bnbw的计算结果，设T123bnbw，12bbnwqQq（10）这里：123132313210-w-w-ww0w-w=w-w0bbnwQ（11）利用毕卡逼近法得k-1时刻与k时刻四元数的递推关系式：3kk-1sin(w)q(t)(cos(w)IW)q(t)w（12）最后将每个时刻的q(tk)进行归一化：22221234qqqqkkq(t)q(t)（13）3九态扩展卡尔曼滤波算法设计3.1EKF状态方程本文采用的EKF数据融合算法选用九态扩展卡尔曼滤波，滤波方式为采样间接式滤波器，误差状态方程的δx选取如下：δpδx=δbδa（14）其中TNEDεεεδρ是姿态角误差向量；Txyzbbbδb是陀螺零偏误差向量，Txyzδaδaδaδa是载体系中的机动加速度误差向量，则状态方程可写为：δx(t)F(t)δx(t)W(t)（15）其中状态矩阵为：nbinna-wC0F(t)00000F（16）系统驱动矩阵为：ρgαWW(t)WW（17）W(t)为均值为零的高斯白噪声，且(-)EtTw(t)w(τ)Q(t)，δ是单位脉冲函数。3.2EKF观测方程Z(t)H(t)δxv(t)（18）其中bbδfZ(t)=δm，δf是加速度计在载体系测得的加速度值bf与估计加速度值ˆbf之差；δm是磁阻传感器在载体系内测得的三轴地磁场bm与估计地磁场ˆbm之差；其中v(t)为均值为零的高斯白噪声，且Eδ(t-τ)Tv(t)v(τ)R(t)。离散化及滤波算法流程如下状态转移矩阵为：k,k-1kΦIFΔT（19）滤波器增益为：-T-T-1k+1k+1k+1k+1K+1k+1k+1KPH(HPHR)（20）k+1时刻的误差量为：+k+1k+1k+1δxKZ（21）所设计的扩展卡尔曼滤波估计后实时修正了机动加速度、捷联矩阵和陀螺零偏。4.仿真测试结果AHRS300的采样频率为100Hz，采集多组静态和动态下的传感器数据并保存，静态数据取其中一组，4仿真时间设为100s。为了验证算法动态精度，动态实验实验仿真设为大约500s左右，图像和误差统计表单位为度，仿真结果如图1、2、3、4所示,：图1静态试验仿真下的姿态角图2静态试验仿真下的姿态角误差表1静态实验仿真下的姿态角误差统计（单位为度）姿态角最大误差最小误差平均误差均方误差滚转角0.0041-0.0040-0.000790.000827俯仰角0.0064-0.0036-0.000540.000857航向角0.00048900.0003290.00079图3动态试验仿真下的姿态角图4动态实验仿真下的姿态角误差表2动态实验仿真下的姿态角误差统计（单位为度）姿态角最大误差最小误差平均误差均方误差滚转角0.0103-0.01660.0000030.0017俯仰角0.0100-0.0001-0.000540.0023航向角0.0033-0.0000890.000480.000385仿真结果分析通过AHRS300传感器数据采集，并进行了静态实验和动态实验，并用MATLAB精确模型算法进行误差对比，从静态实验图1、2的结果可以看出，静态试验姿态角最大误差不超过0.01°，平均误差也不超过0.001°，均方误差也在0.001°附近，通过静态实验姿态角误差表1，得出所设计的EKF算法静态精度很好，适用于低姿态系统。动态实验中，取较长仿真时间，从动态实验图3、4和姿态角误差表2看出，姿态角的5最大误差在0.01°附近，平均误差不超过0.001°，均方误差也不超过0.01°，此算法可较好的收敛姿态角，可用于高动态环境中。6结论本文设计出一种新的EKF算法，滤波性能较好，把姿态角误差向量，陀螺零偏误差向量，机动加速度误差向量作为状态量，改进了原有的卡尔曼滤波算法，能够得到较高的姿态角精度。从各组实验仿真结果可以看出，姿态角的误差精度皆不超过01.0，证明此算法能较好地优化姿态角精度，可适用于高低动态环境中。随着器件成本的不断降低，它可适用于捷联式航姿参考系统的无人机、机动车辆等。但由于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]LeeS,LeeT,ParkS.FlightTestResultsofUAVAutomaticControlUsingaSingle-AntennaGPSReceiver[C].IAAGuidance,Navigation,andControlConferenceandExhibit,(Austin,TX),2003.[12]ChoA,KimJ,LeeS.FullyAutomaticTaxiing,TakeoffandLandingofaUAVonlywithaSingle-AntennaGPSReceiver[C].AIAAInfotech@Aerospace2007ConferenceandExhibit,2007.刘维亭，男，1966年1月出生，博士研究生学历，博士学位，教授，主要研究方向为控制理论与控制工程；智能控制；船舶电气。白杨，