基于MIMU和磁力计的姿态更新算法研究*

2015-05-09田增山

传感技术学报 2015年1期

关键词：陀螺仪加速度计陀螺

米刚,田增山,金悦,李泽,周牧

(重庆邮电大学移动通信技术重庆市重点实验室,重庆 400065)

基于MIMU和磁力计的姿态更新算法研究*

米刚,田增山*,金悦,李泽,周牧

(重庆邮电大学移动通信技术重庆市重点实验室,重庆 400065)

针对传统人体姿态解算算法中存在MEMS陀螺误差发散快的问题,提出一种基于微惯性测量单元(MIMU)及磁力计信息融合的姿态解算算法。该算法利用互补滤波结合PI调节控制完成陀螺零偏校正,然后在加速度计和磁强计的辅助校正下,通过EKF(Expand Kalman Filter)滤波器更新四元数法实现陀螺姿态解算。本算法采用MPU9150传感器模块完成测试实验,实验中对比分析了单独扩展卡尔曼滤波算法与本算法的滤波效果。实验结果表明,本算法能够有效地抑制陀螺的发散,实现稳定地输出高精度姿态数据。

传感器;姿态解算;陀螺校正;互补滤波;PI调节;EKF

人体姿态的准确解算是实现行人航迹推算系统的基础,其在行人定位系统的研究中起着很重要的作用。随着MEMS(Micro-Electro-Mechanical Systems)及计算机技术的快速发展,由陀螺、加速度计组成的微惯性测量单元MIMU(Miniature Inertial Measuringunit),辅以微磁力计,组成了航向姿态参考系统AHRS(Attitude and Heading Reference System),其可实现载体的全姿态参数测量。在AHRS中,加速计同时对重力加速度和运动加速度敏感,并且磁传感器易受铁磁性物质干扰,所以利用加速度计和磁传感器的测量值解算姿态角在载体运动情况下易产生较大误差。同时MEMS陀螺也存在零偏问题,短时间内陀螺偏移也会引起较大的姿态误差。

常用的解决上述问题的方法是采用互补滤波和卡尔曼滤波器。互补滤波在定姿算法中用于滤除加速度计存在的线加速度,以及磁力计受到干扰等因素引起的测量信息中的高频噪声,同时还可以消除陀螺零偏及其他误差造成的积分低频噪声漂移,使两种来源的姿态信息互补,得到准确的姿态输出[1-2]。卡尔曼滤波则利用来自卫星定位系统、加速度计和磁力计等传感器的数据来补偿陀螺偏差引起的姿态误差。Rehbinder H等人利用陀螺仪和加速度传感器信号进行融合[3],从而修正陀螺仪的偏差,该方法仅能减少水平面倾角的漂移,无法修正偏航角误差且易受运动加速度等的干扰。Elder M Hemerly[4]等人利用GPS修正加速度信息,从而利用补偿后的加速度结合陀螺仪和磁力计完成姿态的更新过程,但是其对GPS的性能和可用性提出了要求。Brigante C M N等人设计了基于MEMS的小型化可穿戴运动捕捉系统[5],提出采用扩展卡尔曼滤波器对陀螺仪、加速度传感器、磁阻传感器3种姿态数据进行融合,获得了精度较高的人体姿态。但受限于MEMS器件的精度问题,需要卡尔曼滤波器在融合过程中对误差信号进行精确建模并建立稳定的非线性观测方程。Madgwick S[6-7]采用梯度下降算法对加速度计和磁强计测得的数据进行处理,并将其通过互补滤波器与陀螺仪测得的结果融合,修正陀螺偏移,但梯度下降算法本身计算比较复杂,同时互补滤波适用的精度有限。

针对上述众多问题,本文设计了一种基于微惯性测量单元(MIMU)及磁力计信息融合的姿态解算算法。利用小型集成IMU采集人体运动信息,求取磁力计和加速度计的观测值,并将其作为PI调节器[6]的误差输入,同时结合互补滤波算法修正陀螺仪角速度输出,将修正后的陀螺仪角速度数据与加速度计、磁力计数据通过EKF(Extend Kalman Filter)滤波器处理,实现姿态精确解算。

1 算法总体结构

本文将加速度计和磁力计计算的误差向量作为PI调节的输入,结合互补滤波算法校正陀螺仪的角速度输出,然后通过EKF滤波器进行姿态角求解。姿态解算前,传感器数据均经过中值滤波去噪处理。本文设计的行人姿态解算算法流程如图1所示。

图1 系统算法框图

姿态更新系统总体分为三部分,第1部分利用起始姿态角求取初始四元数,以便给EKF的起始状态赋初值;第2部分利用互补滤波器与PI控制对角速度进行校正,同时将校正后的角速度值送入EKF;第3部分EKF对四元数进行更新,将更新后的四元数求取最终的姿态角。

2 利用起始姿态角计算初始四元数

通常将惯性测量单元中陀螺仪、加速度计和磁强计的测量数据定义到载体坐标系b,取载体的重心为载体坐标系原点,3个轴分别与载体的纵轴、横轴和竖轴相互重合,分别定义为右前上(XYZ)。为继续后面的计算,载体坐标系要转换到导航坐标系n(东北天ENU)。一般载体坐标系向导航坐标系的转换可以通过四元数法或Euler角法实现,由于四元数法可以避免Euler角的奇异问题,应用更加广泛,本文使用四元数法进行坐标系转换[8]。

2.1 陀螺仪计算姿态角[8-9]

根据四元数理论,姿态角计算公式为

(1)

式中γ为翻滚角,θ为俯仰角,ψ为航向角。

2.2 利用加速度计、磁强计计算初始四元数

单独的三轴加速度计只能确定横滚角、俯仰角,与磁强计组合后可确定偏航角。当载体在导航坐标系中静止,并且载体坐标系和导航坐标系重合时,加速度计和磁力计的测量输出为:

an=[0 0g]mn=[0bnbu]

(2)

式中:g是重力加速度,bn是地磁场在ENU坐标系中的北向分量,bu是地磁场在ENU坐标系中的垂直分量。

初始静止情况下的导航坐标系加速度为:

(3)

对上式作矢量展开,得到初始水平姿态角(翻滚角和俯仰角):

(4)

同样可得初始静止情况下的导航坐标系磁力计为:

(5)

磁航向角的初值估计可通过下式获得

(6)

根据获取的起始姿态角代入式(7)计算得到初始四元数为

(7)

3 互补滤波器和PI控制算法设计

由于陀螺长时间工作会引起漂移、再加上机械振动引起的加速度计误差和传感器周围磁干扰的作用,基于MEMS惯性器件测量值解算出来的姿态会存在漂移,为抑制陀螺的漂移,通过互补滤波器和PI控制来调节误差。

3.1 互补滤波和PI控制原理

由于陀螺短时精度高,长时间工作会引起漂移,而对于加速度计和磁力计,它们短时间内精度没有陀螺仪高,但长时间内却能保持稳定。同时从频域上看陀螺显示高通特性,加速度计与磁力计显示低通特性,因此它们在频域上可以相互补充,从而实现高精度的姿态测量。基于此本文将通过磁罗盘和四元数法分别计算得到的姿态角之间的差值作为修正值,用其校正陀螺仪的角速度输出。互补滤波器的原理如下[11-12]:

假设经过校正后得到的姿态角度为θ,则在s域上有如下等式成立

(8)

式中A(s)=K/(S+K)为低通滤波器,B(s)=S/(S+K)为高通滤波器,且A(s)+B(s)=1,wm表示陀螺仪测量值,θref为磁力计结合旋转矩阵解算出来的姿态角。

式(8)求拉普拉斯反变换,可得其在时域中的表达式为:

(9)

θref-θ即为通过磁罗盘算法和四元数法分别计算得到的姿态角度的误差量,将其作为陀螺修正值,以校正陀螺仪的角速度输出,θref-θ的值通过PI调节器来控制[13]。

PI调节器的模型为:

(10)

其中,e(t)为t时刻系统的误差,u(t)为t时刻PI调节器的输出。通过调节Kp和Ki两个参数就可以获得较好的误差估计量。

3.2 向量误差校正

对于陀螺误差的校正,误差量的计算是一个难点,误差量的计算往往是基于梯度下降法[14]计算的,但是梯度下降法比较复杂并且计算量大,为降低复杂度,这里使用一种基于向量叉乘的算法来计算误差量。

两者矢量之间的叉乘公式如下:

(11)

(12)

理论情况下,Δψ和Δθ,γ值应恒为0,但由于测量、数值截断、坐标对准、非正交化等误差的存在,此式不总为零。将上述两种向量进行叉乘,向量积的长度为两向量夹角的正弦,并不完全与两组向量的误差角度成正比但在误差较小时,误差角度与其正弦近似相等,因此

(13)

(14)

空间误差角度θ投影到3个轴上形成误差矢量,所以叉乘的结果可以校正陀螺所产生的漂移,获得的总误差矢量为:

Δ=Δθ,γ+Δψ

(15)

根据式(9)、式(10)、式(15)可得:

(16)

4 基于EKF的四元数更新算法

4.1 状态量选取及状态方程的建立

取四元数的4个参数作为状态变量,根据捷联惯导系统的四元数理论[9],状态方程为:

X(k)=Φ(k,k-1)X(k-1)+Γ(k-1)W(k-1)

(17)

其中:

(18)

(19)

其中W(k)近似为白噪声序列,Γ(k)为噪声驱动阵。

4.2 观测量的选取及观测方程的建立

选取观测量为:

Z=[axayazmxmymz]T

(20)

其中ax、ay、az为加速度计在载体坐标系中测量出的归一化后的数据,mx、my、mz为三轴磁力计在载体坐标系中测量出的归一化后的数据。

基于EKF的观测方程为:

Z(k)=H(k)X(k)+V(k)

(21)

其中:

(22)

5 实验验证及结果分析

本文采用MPU9150九轴惯性传感器测量单元进行测试,MPU9150由MPU6050传感器(其中包括三轴加速度计和三轴陀螺仪)与HMC5883L三轴磁力计集成,数据通过I2C总线进行读取。利用本文设计的滤波算法对传感器输出的原始数据进行处理来获得载体准确的姿态信息。在实际使用中,传感器因为存在一定的固定误差,需要对其进行提前校正。MPU9150模块如图2所示。

图2 MPU9150模块

5.1 静止状态下测试

本文静止状态下的测试地点为室内过道中,行人手持传感器模块静止2min,然后通过本文设计的姿态解算算法在MATLAB上对MPU9150传感器数据进行处理得到其静止下的姿态图,如图3～图5所示。

5.2 运动状态下测试

本文运动状态下的测试地点选在室内,行人手持传感器模块行走一个方形,时间为2min,通过本文设计的姿态解算算法对MPU9150传感器数据进行处理得到其行走状态下的姿态图,如图6～图8所示。

图3 静止时有PI控制和无PI控制下的航向角

图4 静止时有PI控制和无PI控制下的航向角

图5 静止时有PI控制和无PI控制下的翻滚角

图6 运动时有PI控制和无PI控制下的航向角

图7 运动时有PI控制和无PI控制下的俯仰角

图8 运动时有PI控制和无PI控制下的翻滚角

传感器按50Hz/s更新数据,分别在静止和运动状态下进行了2min左右的多次测试,其数据统计特性如表1所示(分析时剔除了俯仰角和横滚角在转弯处的较大偏差值)。

表1 单独EKF与EKF+PI姿态数据统计对比表

结论:通过图3～图5可知以及表1可知,在静止状态下,加入PI控制后可以有效抑制载体3个姿态角的漂移,并能很好的保持稳定;通过图6～图8以及表1可知,在运动状态下,PI控制对航向角起到很好的调整作用,但是在转弯处,PI控制对俯仰角和横滚角的调整则会短暂失效。

6 总结

本文针对传统人体姿态解算算法中存在的MEMS陀螺误差发散快问题,提出了一种基于MIMU及磁力计信息融合的姿态解算算法。该算法利用互补滤波,同时结合PI调节控制完成陀螺零偏校正,然后在加速度计和磁力计的辅助校正下,通过EKF滤波器实现陀螺姿态解算。通过仿真结果表明,本算法能够实现稳定地输出高精度姿态数据,在对航向角有较高要求的行人导航等应用中具有实际应用价值。

[1]Mahony R,Hamel T,Pflimlin J M.Nonlinear Complementary Filters on the Special Orthogonal Group[J].Automatic Control,IEEE Transactions on,2008,53(5):1203-1218

[2]Euston M,Coote P,Mahony R,et al.A Complementary Filter for Attitude Estimation of a Fixed-Wing Uav[C]//Intelligent Robots and Systems,2008.IROS 2008.IEEE/RSJ International Conference on.IEEE,2008:340-345

[3]Rehbinder H,Hu X.Drift-Free Attitude Estimation for Accelerated Rigid Bodies[J].Automatica,2004,40(4):653-659

[4]Elder M Hemerly,Benedito C O Maciel,Anderson de P Milhan,et al.Attitude and Heading Reference System with Acceleration Compensation[J].Aircraft Engineering and Aerospace Technology,2012,84(2):87-93

[5]Brigante C M N,Abbate N,Basile A,et al.Towards Miniaturization of a MEMS-Based Wearable Motion Capture System[J].Industrial Electronics,IEEE Transactions on,2011,58(8):3234-3241

[6]Madgwick S.An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays[J].Report x-io and University of Bristol(UK),2010

[7]Madgwick S O H,Harrison A J L,Vaidyanathan R.Estimation of IMU and MARG Orientation Using a Gradient Descent Algorithm[C]//Rehabilitation Robotics(ICORR),2011 IEEE International Conference on.IEEE,2011:1-7

[8]刘兴川,张盛,李丽哲,等.基于四元数的MARG传感器姿态测量算法[J].清华大学学报(自然科学版),2012(5):627-631

[9]贾瑞才.基于四元数EKF的低成本MEMS姿态估计算法[J].传感技术学报,2014,27(1):90-95

[10]秦永元.惯性导航[M].北京:科学出版社,2006

[11]Mahony R,Hamel T,Pflimlin J M.Complementary Filter Design on the Special Orthogonal Group SO(3)[C]//Proc of the IEEE Conference on Decision and Control and European Control Conference,2005:1477-1484

[12]Robert Mahony,Tarek Hamel,Pascal Morin,et al.Nonlinear Complementary Filters on the Special Linear Group[J].International Journal of Control,2012,85(10):1557-1573.

[13]董恒,王辉,黄科元.永磁同步电动机驱动系统数字PI调节器参数设计[J].电气传动,2009(1):7-10

[14]朱文杰,王广龙,高凤岐,等.基于MIMU和磁强计的在线实时定姿方法[J].传感技术学报,2013,26(4):536-540.

MIMU Update Algorithm Based on the Posture and Magnetometer*

MIGang,TIANZengshan*,JINYue,LIZe,ZHOUMu

(Chongqing Key Lab of Mobile Communications Technology,Chongqing University of Posts and Telecommunications,Chongqing 400065,China)

Since the conventional human attitude algorithms suffer from the fast divergence of MEMS gyro errors,this paper proposes a novel attitude algorithm based on the information fusion of micro inertial measurement unit(MIMU)and magnetometers.We first rely on the filter combined with PI regulation to conduct gyro zero slant correction,and then in the condition of accelerometer and magnetometer auxiliary correction,realize the gyro attitude algorithm by using the extended Kalman filter(EKF)to update quaternion.By selecting the MPU9150 sensor module,the extensive experiments are provided to compare the performance of the conventional single EKF and the proposed algorithm in filtering.The results demonstrate that the proposed algorithm can significantly restrain the divergence of MEMS gyro errors,and consequently output the stable and highly-accurate attitude data.

sensor;attitude algorithm;gyro calibration;complementary filter;PI regulation;EKF