基于四元数EKF算法的小型无人机姿态估计

宋 宇,翁新武,郭昕刚

(长春工业大学 计算机科学与工程学院,长春 130012)



基于四元数EKF算法的小型无人机姿态估计

宋 宇,翁新武,郭昕刚

(长春工业大学 计算机科学与工程学院,长春 130012)

针对小型无人机设计的姿态测量系统,提出一种用于小型无人机姿态估计的四元数扩展Kalman滤波算法.该算法通过建立四元数姿态运动模型和航姿传感器测量模型,构建了以四元数和陀螺仪随机漂移为状态向量、以加速度计测量值和磁阻仪解算的航向角为观测向量的扩展Kalman滤波器,并设计了自适应测量噪声协方差矩阵修正法.实验结果表明,该算法不但解决了微机电系统惯性器件用于载体姿态测量时精度低、易发散、易被干扰的问题,而且显著减小了陀螺仪随机漂移对姿态估计的影响,有效提高了姿态估计的精度.

四元数；扩展Kalman滤波；姿态估计；微机电系统；小型无人机

实时、稳定、精准的姿态信息是无人机实现自主飞行和执行各种任务的基础[1].受载荷和尺寸的限制,小型无人机(small UAV)通常不适合搭载高性能、体积大、价格高的传统姿态测量系统.微机电系统(micro electro mechanical systems,MEMS)传感器和嵌入式处理器由于其体积小、重量轻、功耗低等优点,在小型无人机惯性导航上应用广泛.但MEMS惯性器件用于姿态估计时存在如下问题：1)陀螺仪存在漂移,单独使用陀螺仪估计无人机姿态存在积累误差；2)加速度计受线性加速度和振动影响较大,单独利用加速度计估计载体姿态角,会出现较大误差,动态时可信度较低；3)磁阻仪易受周围环境(如高压线、铁矿厂等)所产生的磁场干扰,从而影响航向角的输出.由于此类误差是随机的,无法预先消除,因此不能单独通过上述方法获取准确的姿态角,必须采用合理的数据融合方法以补偿各类误差带来的影响,才能获得稳定可靠的姿态信息[2-3].

描述飞行器姿态常用的方法有Euler角法、四元数法、方向余弦法和等效旋转矢量法等.Euler角法描述简单、无冗余参数,但存在奇异值,计算量大；方向余弦法可全姿态工作,但存在9个参数,求解计算量大；等效旋转矢量法可消除计算的方向余弦矩阵或四元数中的不可交换性误差,但存在大量参数冗余；四元数法因为计算量小、非奇异性、无超越函数和可全姿态工作等优点得到广泛应用.文献[4]提出了应用于便携式移动机器人的四元数姿态解算方法,但未考虑传感器的测量误差会影响姿态估计;文献[5]将MEMS传感器数据通过四元数互补滤波器实现三自由度的姿态测量,有效减小了测量误差,提高了测量精度;文献[6-7]采用基于四元数的Kalman滤波器(Kalman filter,KF),进一步提高了姿态测量精度.但在实际应用中,飞行器姿态测量系统一般都是非线性系统,则基于系统线性Gauss分布的Kalman滤波器不能给出最优解,而扩展Kalman滤波器(extended Kalman filter,EKF)利用Taylor级数展开的方法将非线性系统线性化,进而采用Kalman滤波的基本步骤进行状态估计更新[8].

本文在总结、分析常见姿态测量方法的基础上,提出一种基于四元数扩展Kalman滤波器的小型无人机姿态测量方案.通过建立四元数姿态描述模型和MARG(magnetic,angular rate,and gravity)传感器测量模型,构建了以四元数、陀螺仪零漂误差为状态向量、以加速度计和磁阻仪解算的航向角为观测向量的扩展Kalman滤波器,为减小载体机动时和外界磁干扰对姿态测量的影响,设计了自适应的测量噪声协方差矩阵,提高了姿态测量精度,实现了三自由度的姿态测量.最后通过实验对上述算法进行了有效评估.

1 飞行器姿态运动模型

1.1参考坐标系

根据捷联惯性导航原理[9],飞行器的姿态角就是载体坐标系b相对于导航坐标系n的方位关系.本文定义载体坐标系为OXbYbZb,该坐标系与机体固联,原点选为飞行器质心,Xb轴沿机体横轴指向右,Yb轴沿机体纵轴指向前,Zb轴垂直指向机体上方,且构成右手坐标系.定义地理坐标系OXnYnZn作为导航坐标系,该坐标系的原点在飞行器的质心,Xn,Yn和Zn轴分别从原点出发指向东、北和天向,简称ENU系.

1.2坐标系间的转换

(1)

为坐标转换矩阵,也称为姿态矩阵.

(2)

由式(1),(2)可得载体的姿态角为

(3)

其中滚转角φ和航向角ψ需要对主值进行相位判断[10].

1.3四元数姿态运动模型

(4)

式中:ωx表示绕滚转轴角速度;ωy表示绕俯仰轴角速度;ωz表示绕航向轴角速度.

采用一阶Runge-Kutta法求解四元数微分方程[12],可得其离散时间模型为

其中:T表示系统采样时间间隔;q(k-1)表示k-1时刻四元数.设置初始四元数q(0),利用式(5)可递推出第k时刻四元数q(k)的取值,进而更新姿态矩阵.

2 MARG传感器误差分析与校正

MARG传感器由一个三轴MEMS陀螺仪、一个三轴MEMS加速度计和一个三轴MEMS磁阻仪组成,直接固联在飞行器上,各传感器内部量测坐标轴相互正交,且与载体坐标系重合,因此各传感器输出为三维矢量,对应的输出为角速度ωb、加速度ab、磁场向量mb.

2.1误差分析

使用MARG传感器求解飞行器姿态时主要存在以下几种误差：1)传感器的安装误差和标度误差；2)陀螺仪的漂移和加速度计的零位误差；3)初始条件误差,包括导航参数和姿态角的初始误差；4)计算误差,主要包括量化误差、用四元数求解姿态角和滤波算法的计算误差；5)载体机动导致的动态误差.其中前两项由MARG传感器导致,而由于MEMS传感器精度较低,其误差还包括测量噪声.MARG传感器输出误差将影响姿态的精度,所以有必要对其进行误差校准.

2.2误差校正

由于飞行器在运动过程中其姿态并非一直保持水平,因此还需对磁场测量值进行倾斜补偿,得到水平磁场分布,磁场的水平分量为

由式(6),解算得到航向角ψ=arctan(myn/mxn).

3 扩展Kalman滤波器的设计

3.1系统结构

图1 系统结构Fig.1 Block diagram of the system

姿态测量系统结构如图1所示.为保证系统输出的姿态角可靠、稳定,本文将陀螺仪、加速度计和磁阻仪测量值通过扩展Kalman滤波器进行多传感器信息融合,求取姿态角、更新姿态矩阵,并设置自适应的测量噪声协方差矩阵,提高姿态测量精度.

3.2系统模型

考虑非线性离散系统：

其中:x(k)∈n表示k时刻系统的状态矢量;z(k)∈m表示k时刻系统的观测矢量；f(·)表示实的n维矢量函数,h(·)表示实的m维矢量函数,f(·)和h(·)对其自变量都是非线性的；w(k)∈n和v(k)∈m是协方差分别为Q(k)和R(k)的过程噪声矢量.

3.2.1 状态方程 设系统状态量为

(7)

其中:q0(k),q1(k),q2(k),q3(k)为姿态四元数；bωx(k),bωy(k),bωz(k)分别为绕滚转轴、俯仰轴和航向轴的陀螺仪随机漂移向量.则系统的状态方程为

对f(x(k-1),k-1)求取Jacobi矩阵：

(9)

当飞行器静止和匀速运动时,存在如下关系：

(10)

系统的观测方程为

对h(x(k),k)求取Jacobi矩阵,可得系统量测矩阵为

3.3扩展Kalman滤波方程

根据扩展Kalman滤波理论,通过时间更新和量测更新的迭代求得系统状态向量的最优估计,系统的扩展Kalman滤波递推过程如下：

1)初始估计.计算状态初始值x(0),设定过程噪声协方差Q(k)、量测噪声协方差R(k)和误差方差矩阵P(0).

2)时间更新.计算状态转移函数的Jacobi矩阵：

(12)

从而k时刻状态向量估计值为

更新状态误差协方差阵为

3.4扩展Kalman滤波修正

ca和cm为设定的权重因子.当载体线性加速度和外界磁干扰越大,相应的协方差也越大,此时状态增益矩阵将减小,以减缓Kalman预测校准.

4 实 验

本文采用的惯性测量单元(IMU)包括三轴加速度计、三轴陀螺仪(MPU-6050)、三轴磁阻仪(HMC5583L)和一个高性能FPGA微处理器(EP2C35F672C6).用微处理器对MARG传感器测量的原始数据以1 000 Hz的采样频率进行采样,在未滤波的情况下将原始数据通过串口发送至PC机,在PC机上利用MATLAB设计的扩展Kalman滤波器对数据进行处理,以获取准确的姿态信息,最后将实验结果用曲线描绘的方式显示,并对解算结果进行比较分析.

为验证本文姿态解算算法的有效性,分别进行3组实验:1)IMU置于水平桌面静止不动,如图2所示；2)IMU做无规律运动,且有外界磁干扰,如图3所示；3)IMU做无规律运动,有外界磁干扰,且增加高频振动,如图4所示.每组实验都设计3种姿态解算法：利用加速度计和磁阻仪测量值的姿态解算法(G-M)(图中黑色实线)；只采用陀螺仪的四元数姿态解算法(AR-Q)(图中蓝色双线)；以及利用四元数EKF滤波将陀螺仪、加速度计和磁阻仪测量数据融合的姿态解算法(Q-EKF)(图中红色虚线).通过3组实验对比3种算法在不同情况下的性能.

图2 静态时仿真结果Fig.2 Simulation results at static state

图3 动态时仿真结果Fig.3 Simulation results at movement state

图4 振动干扰时仿真结果Fig.4 Simulation results under vibration disturbance

由图2可见,静态时没有其他线性加速度及干扰磁场,G-M算法能准确获得载体的姿态信息；AR-Q算法会产生姿态角发散问题；而Q-EKF算法能有效地修正陀螺仪的随机漂移误差,解决了角度误差累积的问题,准确地获取了载体的姿态,且俯仰角和横滚角都在0°附近,误差控制在0.2°内.由图3可见,动态时由于存在线性加速度及干扰磁场的影响,G-M算法获得的载体姿态信息有时误差会较大；AR-Q算法虽然不受线性加速度及干扰磁场的影响,但仍存在姿态角发散问题；而Q-EKF算法能有效减小载体线性加速度和干扰磁场的影响,且能有效地修正陀螺仪的随机漂移误差,动态性能较好.由图4可见,当载体附加有高频振动时,G-M算法得到姿态角有明显的毛刺和突变分量,可能会导致载体的震荡；AR-Q算法姿态角发散问题较严重；而Q-EKF算法有效地消除了干扰噪声和突变分量,实现了姿态角的正确估计.

综上所述,本文提出了一种适用于小型无人机姿态估计的四元数扩展Kalman滤波算法,将三轴陀螺仪、三轴加速度计和三轴磁阻仪的测量数据通过扩展Kalman滤波器进行信息融合,修正了陀螺仪的随机漂移误差,解决了角度误差累积问题,能够准确获取载体的姿态.并设计了自适应量测噪声协方差矩阵修正法,能有效抑制线性加速度和干扰磁场对姿态估计的影响,提高了姿态测量精度.实验表明,本文提出的算法在静态、动态时都能提供实时、稳定、精准的姿态信息.

[1] 房建成,张霄.小型无人机自动驾驶仪技术 [J].中国惯性技术学报,2007,15(6):658-663.(FANG Jiancheng,ZHANG Xiao.Development of Autopilot Technology of Small UAV [J].Journal of Chinese Inertial Technology,2007,15(6):658-663.)

[2] Tayebi A,McGilvray S,Roberts A,et al.Attitude Estimation and Stabilization of a Rigid Body Using Low-Cost Sensors [C]//Proceeding of the 46th IEEE Conference on Decision and Control.Berlin:Springer,2007:6424-6429.

[3] 秦勇,臧希喆,王晓宇,等.基于MEMS惯性传感器的机器人姿态检测系统的研究 [J].传感技术学报,2007,20(2):298-301.(QIN Yong,ZANG Xizhe,WANG Xiaoyu,et al.Research of Attitude Estimation System Using MEMS Inertial Sensors [J].Chinese Journal of Sensors and Actuators,2007,20(2):298-301.)

[4] 张思奇,赵忆文.便携式移动机器人姿态解算方法 [J].微计算机信息,2007,23(10-2):188-190.(ZHANG Siqi,ZHAO Yiwen.Attitude Algorithm of Portable Mobile Robot [J].Microcomputer Information,2007,23(10-2):188-190.)

[5] YUN Xiaoping,Lizarraga M,Bachmann E,et al.An Improved Quaternion-Based Kalman Filter for Real-Time Tracking of Rigid Body Orientation [C]//Proceedings of the IEEE International Conference on Intelligent Robots and Systems.Piscataway,NJ:IEEE,2003:1074-1079.

[6] Harms H,Amft O,Winkler R,et al.ETHOS:Miniature Orientation Sensor for Wearable Human Motion Analysis [C]//Proceedings of IEEE Sensors Conference.Piscataway,NJ:IEEE,2010:1037-1042.

[7] Yun X,Bachmann E,McGhee R.A Simplified Quaternion Based Algorithm for Orientation Estimation from Earth Gravity and Magnetic Field Measurements [J].IEEE Trans Instrum Meas,2008,57(3):638-650.

[8] 王松,田波,战榆莉,等.基于修正EKF的微小型飞行器姿态估计 [J].高技术通讯,2011,21(6):612-618.(WANG Song,TIAN Bo,ZHAN Yuli,et al.Flight Attitude Estimation for MAVs Based on Amended EKF [J].Chinese High Technology Leters,2011,21(6):612-618.)

[9] 陈哲.捷联惯导系统原理 [M].北京:宇航出版社,1996:82-85.(CHEN Zhe.The Principle of Strap Down Inertial Navigation System [M].Beijing:Astronautics Press,1996:82-85.)

[10] 秦永元.惯性导航 [M].北京:科学出版社,2006:297.(QIN Yongyuan.Inertial Navigation [M].Beijing:Science Press,2006:297.)

[11] Oshman Y,Carmi A.Attitude Estimation from Vector Observations Using Genetic Algorithm-Embedded Quaternion Particle Filter [J].Journal of Guidance,Control,and Dynamics,2006,29(4):879-891.

[12] 王鹏,吕显瑞,张伸煦.求解随机微分方程的三级半隐式随机龙格库塔方法 [J].吉林大学学报:理学版,2008,46(2):219-223.(WANG Peng,LÜ Xianrui,ZHANG Shenxu.Three-Stage Semi-implicit Stochastic Runge-Kutta Methods for Stochastic Differential Equations [J].Journal of Jilin University:Science Edition,2008,46(2):219-223.)

[13] 刘兴川,张盛,李丽哲,等.基于四元数的MARG传感器姿态测量算法 [J].清华大学学报:自然科学版,2012,52(5):627-631.(LIU Xingchuan,ZHANG Sheng,LI Lizhe,et al.Quaternion-Based Algorithm for Orientation Estimation from MARG Sensors [J].Tsinghua Univ:Sci &Tech,2012,52(5):627-631.)

[14] Suh Y S.Orientation Estimation Using a Quaternion-Based Indirect Kalman Filter with Adaptive Estimation of External Acceleration [J].IEEE Trans Instrumentation and Measurement,2010,59(12):3296-3305.

[15] Bird J,Arden D.Indoor Navigation with Foot-Mounted Strap Down Inertial Navigation and Magnetic Sensors [J].IEEE Wireless Communcation,2011,18(2):28-35.

(责任编辑：韩 啸)

SmallUAVAttitudeEstimationBasedontheAlgorithmofQuaternionExtendedKalmanFilter

SONG Yu,WENG Xinwu,GUO Xingang

(CollegeofComputerScienceandEngineering,ChangchunUniversityofTechnology,Changchun130012,China)

A new algorithm of quaternion extended Kalman filter was adopted to estimate the attitude of small UAV.After the establishment of the models of quaternion attitude movement and magnetic,angular rate,and gravity sensor measurement,a new designed extended Kalman filter was given with the quaternion and random drifting of gyro being the static vectors and the accelerometer measurements and the heading solved by the magnetometer being the observational vectors.What’s more,an innovation amendment method based on an adaptive approach to construct the measurement noise covariance matrix was designed.Experimental results show that the algorithm not only solves the problems of micro electro mechanical systems inertial sensors used for attitude measurement to show low accuracy and to be easy to diverge and to disturb,but also significantly reduces the effect of random drifting of gyro scope on attitude estimation,and proves to be effective at improving the accuracy of attitude estimation.

quaternion;extended Kalman filter;attitude estimation;micro electro mechanical systems (MEMS);small UAV

10.13413/j.cnki.jdxblxb.2015.03.31

2014-09-17.

宋 宇(1969—),男,汉族,硕士,副教授,从事嵌入式系统设计的研究,E-mail：songyu@mail.ccut.edu.cn.

教育部“春晖计划”项目(批准号:Z2011139).

TP39

：A

：1671-5489(2015)03-0511-08