9轴MEMS-IMU实时姿态估算算法

张金艺,徐德政,李若涵,陈兴秀,徐秦乐

(1．上海大学特种光纤与光接入网省部共建重点实验室,上海 200444;

2．上海大学微电子中心,上海 200444;

3．上海大学教育部新型显示与系统应用重点实验室,上海 200444)

9轴MEMS-IMU实时姿态估算算法

张金艺1,2,3,徐德政2,李若涵1,陈兴秀1,徐秦乐2

(1．上海大学特种光纤与光接入网省部共建重点实验室,上海 200444;

2．上海大学微电子中心,上海 200444;

3．上海大学教育部新型显示与系统应用重点实验室,上海 200444)

随着对微机电系统-惯性测量单元(micro-electro-mechanical system-inertial measurement unit,MEMS-IMU)在室内定位、动态追踪等应用领域中的需求日益迫切,使得具有高精度、低成本和实时性的MEMS-IMU模块设计成为研究热点.针对MEMS-IMU的核心技术——姿态估算进行研究,设计了一种基于四元数的9轴MEMS-IMU实时姿态估算算法.该算法运用分解四元数算法处理加速度和磁感应强度数据,计算出静态四元数;通过角速度与四元数的微分关系估算动态四元数;运用卡尔曼滤波融合动、静态四元数,进而实现实时姿态估算.针对分解四元数算法中存在的奇异值问题,提出了转轴补偿方法对其修正,以实现全姿态估算;考虑动态情况下的非线性加速度分量对姿态估算精度的影响,设计了R自适应卡尔曼滤波器,以进一步提高姿态估算算法的精度.验证结果表明,R自适应卡尔曼滤波器能够有效抑制加速度噪声,提高姿态估算精度;同时,转轴补偿-分解四元数算法能够准确估算奇异值点的姿态信息,并且计算时间仅为原“借角”补偿方法的50%左右,有效提高了整体算法的实时性.

微机电系统-惯性测量单元;姿态估算;分解四元数算法;奇异值补偿;卡尔曼滤波

微机电系统-惯性测量单元(micro-electro-mechanical system-inertial measurement unit, MEMS-IMU)实现了传感器信息获取、传输、处理的一体化集成,具备体积小、成本低、便于大批量生产等优点[1],因而已广泛应用于便携式智能设备中.近年来,随着手持式智能设备的蓬勃发展,催生了通过MEMS-IMU实现室内定位、动态追踪等复杂功能的应用需求,使得具有高精度、低成本和实时性的MEMS-IMU模块设计逐渐成为惯性技术的发展方向.而姿态估算作为MEMS-IMU的核心技术也越来越受到国内外专家学者的关注.

陀螺仪是目前最常用的姿态传感器,但是由于存在漂移、累积误差等缺陷而无法长期稳定地提供姿态信息.针对陀螺仪存在的问题,文献[2]利用倾角仪和磁力计对陀螺仪提供的姿态信息进行补偿,最先提出了互补卡尔曼滤波姿态估算的思想.文献[3]设计了基于欧拉角的互补卡尔曼滤波器融合9轴MEMS传感器数据实现姿态估算.然而由于该算法需要计算大量三角函数,并且在运算中易产生奇异值,因此基于四元数的卡尔曼滤波器更普遍地应用于姿态估算.文献[4]使用静态姿态估算算法将加速度和磁感应强度数据转化为四元数,然后运用基于四元数的卡尔曼滤波器融合传感器数据,得到最终的姿态估算结果.目前,常用的静态四元数估算算法有高斯牛顿算法和QUEST算法[5],这两种算法在本质上属于代数方法,需要建立精确的磁感应模型,以避免竖直方向的姿态估算受到影响;同时两种算法在计算中存在若干奇异值点,给实际应用带来很大的困扰[6].文献[7]给出一种分解四元数的方法,将四元数按欧拉角方向分解并合成,达到了较好的姿态估算效果.文献[8]在运算中运用半角公式,避免了三角函数的运算,提高了计算效率.美中不足的是该算法使用了三参数法推导四元数,因此也不可避免地引入了奇异值.文献[9]在此基础上设计了两段式扩展卡尔曼滤波器,分别使用加速度和磁感应数据估算竖直和水平方向的姿态,同样消除了磁偏量的非精确匹配对姿态估算的干扰.不过由于扩展卡尔曼滤波作为一种次优滤波,在线性化过程中存在误差,同时需要求解观测方程的雅可比矩阵,对于高维问题而言过于繁琐,故在性能上并不如四元数卡尔曼滤波方法[10].

综合分析上述研究成果,本研究使用分解四元数算法(factored quaternion algorithm, FQA)计算静态四元数;利用角速度与四元数的微分关系估算动态四元数;运用卡尔曼滤波融合动、静态四元数,进而实现实时姿态估算.针对分解四元数算法中存在的奇异值问题,提出转轴补偿方法来修正奇异值,从而实现全姿态估算;针对动态情况下的非重力加速度干扰姿态估算问题,设计了R自适应卡尔曼滤波器,以进一步提高姿态估算的精度.验证结果表明,R自适应卡尔曼滤波器能够有效抑制加速度噪声,提高姿态估算精度;同时,转轴补偿-分解四元数算法能够准确估算奇异值点的姿态,且计算时间仅为原“借角”补偿方法的50%左右,有效提高了整体算法的实时性.

1 四元数物理意义剖析

四元数Q=[qwqxqyqz],可用于表示载体在空间中的姿态,其一般表现形式为

式中,qw,qx,qy,qz为4个实数,i,j,k为四元数的3个虚系数单位的基.由此可知,四元数实际上由一个标量qw和一个矢量q构成.当四元数满足单位化条件,即+++=1时,四元数的物理意义就可以理解为刚体绕过定点的转轴n转过角度α,则式(1)中的四元数又可以表示为

如果此时再作一次转动变换,记作Q′,则两次转动可以等效为一个转动四元数

向量r经上述转动后得到向量r′,则r′和r的转换关系为

式(3)和(4)中,⊗表示四元数乘法.假设代表合成的四元数Qsyn=[q0q1q2q3],则对应的Q∗=[q0−q1−q2−q3].

2 转轴补偿-分解四元数算法

分解四元数算法[7-8]是一种只需单步计算的静态及准静态姿态估算算法.该算法利用四元数的物理意义,将载体的转动分解并合成以求得姿态四元数.但是由于使用三参数法推导四元数,从而不可避免地存在奇异值.下面将有针对性地提出转轴补偿方法来修正奇异值,以实现全姿态估算.

2.1 分解四元数算法

初始载体与北东地(north east down,NED)参考系重合,此时首先绕z轴转过航向角(yaw),记作ϕ;然后再绕y轴转过俯仰角(pitch),记作θ;最后绕x轴转过横滚角(roll),记作γ.利用上述转动对应的欧拉角可表示载体相对参考系的任意姿态,称为三参数法定姿.分解四元数算法就是应用三参数法,分别求解沿欧拉角方向的四元数,然后合成求得姿态四元数.

2.1.1 俯仰角方向的四元数

由于载体沿水平方向的旋转并不会影响垂直方向的姿态,因此首先求解俯仰角和横滚角的四元数.当载体初始静止时,测得的加速度a=[0 0 g]T.若载体绕y轴旋转θ角度,则加速度在y轴的分量ay仍为0,同时有将测得的加速度数据单位化,得到单位化后的则有

由于俯仰角θ∈[−π/2,π/2],则运用三角函数的半角关系可得

式中,sign(a)为取符函数,定义规则如下:当a>0时,sign(a)=1;反之,当a<0时, sign(a)=−1.参考式(2)可得表征俯仰角的四元数

2.1.2 横滚角方向的四元数

载体再绕x轴旋转γ角度,测得加速度x轴分量ax不变,同时ay和az分别为

则横滚角的余弦值和正弦值分别为

值得一提的是,式(10)中的cosθ出现在分母的位置,隐含有cos=0的限制条件.因此,当俯仰角θ=±π/2时,式(10)不再适用于求解横滚角,否则会引入奇异值.对于奇异值点的补偿方法将在第3节中讨论.由于横滚角γ∈(−π,π],参见式(7),则表征横滚角的四元数

2.1.3 航向角方向的四元数

假设航向角为φ,测得磁感应强度mb=[mbxmbymbz]T.为求得航向角φ,需先将其转换到水平平面上,利用式(4)得到转换后的等效磁感应强度

若忽略磁北方向和正北方向之间的磁偏角,并以BN代表地磁场的北向分量,则此时水平方向的磁感应强度与地磁场强度水平分量之间有如下关系:

为计算简便,可将水平方向的磁感应强度与地磁场强度作单位化处理,得到单位化水平方向磁场强度和则计算可得航向角的余弦值和正弦值分别为

由于φ∈(−π,π],参考式(7)中的半角公式,则航向角的旋转四元数

2.1.4 四元数的合成

由式(8),(11)和(15)分别求得横滚角、俯仰角和航向角的旋转四元数后,运用式(3)对3个四元数分量进行合成,得到姿态四元数

至此,分解四元数算法利用载体静止时的固有重力加速度以及地磁场强度作为参考,根据四元数的物理意义,通过分解-合成四元数的方法,消除了传统静态姿态估算算法中由磁感应模型非精确匹配造成的竖直方向姿态估算误差.运算过程中使用半角公式避免了三角函数运算,利用四元数运算提高了算法的计算效率.只是在运算过程中由于存在若干奇异值点,需要额外的补偿方法来修正奇异值.

2.2 转轴补偿方法对奇异值的修正

当俯仰角θ=±π/2时,使用式(10)计算横滚角会出现奇异值,这将导致分解四元数算法无法正确估算姿态.下面就奇异值的补偿方法展开讨论,提出采用转轴补偿方法来修正奇异值,通过转轴补偿-分解四元数算法实现对全姿态的估算.

文献[8]给出了一种补偿方法——借角法.借角法的基本思想是在cosθ<ε时(ε为接近0的正数),将俯仰角额外转过α角度,此时等效俯仰角为θ−α,同时将测得的原始加速度和磁感应数据也作相应四元数转化后,代入分解四元数算法进行计算,将这个过程称为“借角”.计算得到四元数后,再乘以额外转角的四元数qα,将求得的四元数转化回实际的姿态四元数,称这个过程为“还角”.借角法虽然可以估算奇异值点的姿态四元数,但是在估算奇异值点的姿态时增加了多次四元数乘法运算,使得计算量明显增加.

而本研究则从分析奇异值的产生原因入手,结合几何理解和物理意义,提出一种转换坐标轴的补偿方法,以下简称转轴法.转轴法可以在几乎不增加基础算法运算量的前提下实现奇异值点的姿态估算,从而提高整体算法的实时性.

在z-y-x的旋转顺序及层级定义下,当载体绕中间层级的y轴旋转至俯仰角为±π/2时,载体坐标系x轴与参考坐标系z轴重合,即第一层级与第三层级的转轴重合,这会导致旋转中第三个自由度旋转失效,即欧拉角姿态估算中常见的万向节锁(gimbal lock)问题.通过上述分析不难发现,可采用解万向节锁的方法结合四元数计算来消除奇异值.实际操作中若遇到万向节锁,一般会先将万向节旋转系统还原至初始位置,然后沿航向角转过π角度,再沿横滚角转过π角度,最后沿俯仰角转至另一侧的π/2角度.同理,当θ接近±π/2时,启用转轴补偿法修正分解四元数算法实现对奇异值点姿态的估算.第一步:将测得的加速度及磁感应强度数据转轴匹配为载体在另一侧水平面的数据;第二步:将俯仰角看作0◦,代入式(11)和(15)中计算横滚角和航向角对应的四元数qγ和qφ;第三步:将原俯仰角θ对应的四元数qθ代入式(16)中,得到合成的四元数.由此可见,转轴补偿方法中除数据匹配外,没有增加分解四元数算法的运算量,就完成了对奇异值的修正.图1给出了转轴补偿法中数据匹配的示例,当载体由位置1(载体水平正面向上)沿俯仰角方向转至位置2(载体竖立左侧正面、右侧反面)时,发生万向节锁.此时,应用转轴补偿方法,将位置2的惯性数据匹配至位置3(载体水平反面向上).易知,位置3中x轴数据应为位置2中z轴测得数据的相反数,且z轴数据应为位置2中x轴测得的数据.将位置2的惯性数据转轴匹配,然后代入分解四元数算法即可估算出奇异值点的姿态.

一般而言,改变载体的旋转顺序即改变转轴的层级关系,但这样并不能有效消除奇异值点,而仅仅是使奇异值点转移.而转轴补偿方法的实质是在奇异值出现时将旋转顺序由原来的z-y-x转变为z-x-y,但是由于四元数运算顺序没有改变,也就是转轴之间的层级关系并没有发生改变,因此不会引入其他奇异值点.

图1 转轴补偿方法的数据匹配示例Fig.1 An example of the data matching in axies-exchanged compensation method

3 R自适应卡尔曼滤波

传感器数据融合是指在系统中使用多种传感器,利用不同传感器的特性来克服其他传感器的不足,从而获得更准确的数据[11].本研究设计基于四元数的卡尔曼滤波器融合9轴MEMS传感器数据,利用四元数微分关系构建状态方程,将分解四元数算法提供的静态四元数作为观测修正值,极大减少了卡尔曼滤波器的计算量.针对动态条件下非重力加速度分量对姿态估算的干扰,本研究设计了R自适应卡尔曼滤波器来准确获取姿态四元数.

3.1 卡尔曼滤波器的设计

下面介绍基于四元数的卡尔曼滤波器的设计,令状态变量为四元数,即x=q,而测量变量由分解四元数算法提供,即z=qm.以A和H表示状态矩阵和输出矩阵,w为过程激励噪声, v为观测噪声,Q和R分别为A和H的协方差矩阵.则离散时间的卡尔曼滤波方程如下:

在惯性运动系统中,角速度ω与四元数q具有如下四元数微分关系:

对式(18)的四元数微分方程作线性化及离散化的近似处理,令qk−1与qk的采样更新时间间隔为h,则可得离散时间状态方程如下:

由于对四元数微分方程作线性离散的近似处理,省去了求解微分方程的繁琐运算.同时,由于分解四元数算法中隐含有四元数单位化,因而测量方程中并不需要额外的约束方程.整个卡尔曼滤波方程仅为4阶,且输出矩阵H为4×4阶单位阵,这样极大地减少了卡尔曼滤波算法的运算量,提高了姿态估算的实时性.

基于四元数的卡尔曼滤波算法流程如图2所示.初始利用分解四元数算法计算状态变量和误差协方差P0,通过时间更新方程计算先验估计值和先验误差协方差,由卡尔曼增益方程根据噪声协方差矩阵Q和R计算卡尔曼滤波增益Kk.将分解四元数算法求得的静态姿态四元数作为观测变量zk,代入卡尔曼滤波的测量更新方程中,运用卡尔曼增益调整先验估计值和新息zk−之间的权重,最终得到准确的姿态估算结果.

图2 基于四元数的卡尔曼滤波算法流程Fig.2 Flow of the quaternion-based Kalman filter

3.2 R的自适应调节

分解四元数算法作为一种静态及准静态姿态估算算法,是利用载体静止时所受作用力为重力加速度的原理进行姿态估算的.而当载体处于高速运动状态时会引入除重力外的非线性加速度分量,从而导致分解四元数算法对静态四元数的估算产生较大的偏差.这就造成观测量不仅失去了本身的校准功能,反而成为误差的来源.为避免这种情况,本研究设计了R自适应调节卡尔曼增益方法,增加动态四元数估算结果的权重,以消除非线性加速度噪声对整体姿态估算的影响.具体的方法为设置动、静态阈值,通过判断载体3轴合加速度与阈值间的关系,对离线标定的静态?√测量误差协方差矩阵?R作在线调节.

(1)当δa≤ asta时,表示载体处于静止状态,测量变量值准确可信,此时R可不作调整或略微调小,则

(2)当asta<δa ≤adyn且|δak−δak−1|≤ asta时,表示载体运动趋势不明显,则Rk=R.若|δak−δak−1|>asta,则物体有变加速度运动的趋势,但是此时的测量值仍有一定的参考价值,因此设置

(3)当δa>adyn时,表示载体处于高速运动状态,此时静态四元数计算得到的测量值完全失去参考修正价值,可令

运用上述规则在线调整测量误差协方差矩阵,并将调整后的Rk代入卡尔曼增益公式中,计算卡尔曼增益Kk.通过卡尔曼增益来调整动、静态四元数在卡尔曼滤波中所占的权重,从而获得准确的姿态估算结果.

4 验证系统构建与结果分析

下面通过云台旋转实验平台模拟载体的各种姿态,利用9轴MEMS-IMU采集惯性测量数据,采用Matlab软件实现转轴补偿-分解四元数算法和R自适应卡尔曼滤波算法,仿真得到姿态估算结果.

4.1 验证系统的构建

本验证系统由硬件和软件两部分组成.硬件部分由9轴MEMS-IMU模块和上位机组成,系统架构如图3所示,其中MEMS-IMU(见图4(a))主要由MEMS传感器和微处理器构成.陀螺仪为STMicroelectronics公司的L3G4200D,根据应用需要选用±500 dps量程,灵敏度为17.5 mdps/digit;加速度计为Freescale公司的MMA8452Q,选用±2 g量程,灵敏度约为0.001 g/count;磁力计为Freescale公司的MAG3110,量程为±1 000µT,灵敏度为0.1µT/bit.微处理器通过I2C总线采集MEMS传感器的原始惯性信息,运用阈值滤波、水平校准及零偏补偿等方法预处理原始数据,然后通过Uart通信方式以115 200 bit/s的速率将预处理后的惯性传感器数据传输至上位机.软件部分采用Matlab 2011b作为算法验证平台以实现转轴补偿-分解四元数算法和R自适应卡尔曼滤波算法,来分析姿态估算算法的性能.

图4(b)为旋转实验平台,将9轴MEMS-IMU固定于照相机云台上,借助云台可以实现航向角由−180◦∼180◦、横滚角由−180◦∼180◦以及俯仰角由0◦∼90◦的旋转测试.因此,云台旋转实验平台基本上可以达到测试要求.

图3 验证系统架构Fig.3 Architecture of verification system

图4 MEMS-IMU与旋转实验平台Fig.4 MEMS-IMU and rotation platform

4.2 验证结果与分析

虽然四元数算法在姿态估算过程中具有运算速度快、非奇异性等优势,但是四元数不能直观反映载体的姿态信息,因此需将实验结果由四元数转化为欧拉角后显示出来.根据人们的使用习惯,以角度制表示欧拉角,并将航向角由−180◦∼180◦的表示方式改为0◦∼360◦来显示. 4.2.1分解四元数算法的性能测试

(1)水平旋转实验.通过与高斯牛顿算法进行对比,验证采用分解四元数算法估算姿态的优势.将MEMS-IMU置于水平面上沿航向角正向旋转,在经预设点时作短暂停留,如此往复7圈后静止于预设点.图5分别给出了高斯牛顿算法和分解四元数算法对上述姿态变化的估算结果,横轴为采样点数,纵轴为姿态角的角度.两种算法对航向角的估算结果几乎一致,而分解四元数算法对俯仰角和横滚角的姿态估算更为准确.这是由于分解四元数算法中的磁感应数据仅用于估算水平方向上航向角姿态,并不参与垂直方向上俯仰角和横滚角的姿态估算,因此屏蔽了磁感应模型的干扰,使得误差较小.

图5 高斯牛顿算法与分解四元数算法对水平旋转实验的姿态估算结果对比Fig.5 Results comparison between Gauss-Newton algorithm and FQA in horizontal rotationexperiment

(2)奇异值点测试.由式(10)可知,当俯仰角达到90◦时,分解四元数算法会出现奇异值.因此设计如下奇异值点测试:将完成水平旋转测试的MEMS-IMU模块沿俯仰角方向作0◦∼90◦的旋转,并在俯仰角90◦处作短暂停留,然后恢复原状.采用分解四元数算法对上述运动姿态进行估算的结果如图6所示,图(a)中未修正的分解四元数算法在俯仰角90◦处出现了奇异值,表明算法对于航向角和横滚角的估算存在较大波动,完全偏离了实际值.这说明分解四元数算法无法准确估算奇异值点的姿态,需要配合补偿方法才能实现全姿态估算.图(b)则给出使用转轴补偿-分解四元数算法对奇异值点的估算结果,基本与实际情况一致.关于采用转轴补偿方法修正奇异值点的分析将于4.2.3节中详细讨论.

图6 分解四元数算法对奇异值点的姿态估算Fig.6 Attitude estimation of singularity from FQA with and without compensation

4.2.2 R自适应卡尔曼滤波器的仿真验证

由图5和6可知,使用分解四元数算法得到的姿态波形中存在明显的毛刺,这主要是由加速度计的测量噪声及其他非重力加速度分量造成的.下面将测试R自适应卡尔曼滤波融合动态数据对姿态估算结果的影响.依照式(20)∼(22)的R自适应调节规则改进卡尔曼滤波器,将通过分解四元数算法得到的姿态估算结果作为卡尔曼滤波的观测值输入,经滤波得到最终的姿态估算结果.相应的R自适应卡尔曼滤波对水平旋转实验的姿态估算结果如图7所示,与图5对比发现,经滤波后俯仰角和横滚角的波形明显更为平滑.这主要是由于R自适应卡尔曼滤波器有效地调节了卡尔曼增益,加大了动态姿态数据的权重,从而显著抑制了除重力加速度外的其他非线性加速度分量对姿态估算的影响,得到了较为准确的姿态估算结果.

图7 水平旋转实验经R自适应卡尔曼滤波后的姿态估算结果Fig.7 Attitude estimation of R-adapted Kalman filter in horizontal rotation experiment

4.2.3 奇异值转轴补偿方法的验证

图8 转轴补偿-分解四元数算法对奇异点的姿态估算结果(经卡尔曼滤波)Fig.8 Attitude estimation(after Kalman filter)of the FQA with axis-exchanged compensation method

(1)转轴补偿方法对奇异值修正效果的测试.图8给出了经卡尔曼滤波后的转轴补偿-分解四元数算法对奇异值点的估算结果.观察当俯仰角由0◦开始缓慢增大至接近90◦时,补偿标志位翻转为1,表明开启了转轴补偿方法来估算奇异值点的姿态,此时航向角和横滚角翻转180◦,表示整个载体处于翻面的状态.而当俯仰角迅速转离90◦时,补偿标志位随即归0,航向角和横滚角也立刻恢复原值.测试结果表明,转轴补偿方法能够准确估算奇异值点的姿态信息,从而弥补了分解四元数算法存在的不足.

(2)计算时间测试.运用Matlab软件中的运行时间统计功能粗略估计转轴补偿-分解四元数算法的计算时间.Matlab仿真平台所在的计算机处理器(CPU)为Intel i5-3317U,主频为1.7 GHz,安装内存(RAM)为8 GB.分别将文献[8]中的“借角”补偿方法以及本研究提出的转轴补偿方法代入分解四元数算法来估算奇异值点的姿态.图9为采用两种补偿方法计算奇异值点姿态所需时间的对比,对照图8可以发现,在采样点为150∼300的区间内,需要补偿修正分解四元数算法.而本研究提出的转轴补偿方法在此区间内的运算时间仅为“借角”补偿法的50%左右,这主要是因为借角法在借、还角的过程中,额外增加了多次四元数乘法运算,而本方法只是将惯性传感数据转换坐标轴匹配,几乎没有增加算法的运算量,因此在估算奇异值点的姿态时具有更高的运算效率.

图9 转轴补偿法与借角补偿法[8]在估算奇异点姿态时的运算时间对比Fig.9 Comparison of the efficiency between the axis-exchanged compensation method and the method in Ref.[8]

5 结束语

本研究设计了一种基于四元数的9轴MEMS-IMU实时姿态估算算法,运用转轴补偿方法修正分解四元数算法中存在的奇异值,实现了静态全姿态估算;设计了R自适应卡尔曼滤波器融合动、静态姿态,以抑制动态条件下非重力加速度对姿态估算的影响,实时且准确地估算出载体姿态.经验证表明,采用R自适应卡尔曼滤波器能够有效提高姿态估算精度.同时,转轴补偿-分解四元数算法能够准确估算奇异值点的姿态,并且计算时间仅为原“借角”补偿方法的50%左右,有效提高了整体算法的实时性.本算法能够实现实时、准确的全姿态估算,非常适用于行人航迹推算以及动态追踪等应用领域,具有较大的现实意义和应用价值.

[1]蔡春龙,刘翼,刘一薇.MEMS仪表惯性组合导航系统发展现状与趋势[J].中国惯性技术学报, 2009,17(5):562-567.

[2]Foxlin E.Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter[C]//Virtual Reality Annual International Symposium.1996:185-194.

[3]Zhang R,Hoeflinger F,Reindl L.Inertial sensor based indoor localization and monitoring system for emergency responders[J].IEEE Sensors Journal,2013,13(2):838-848.

[4]Lee J K,Park E J.Minimum-order Kalman filter with vector selector for accurate estimation of human body orientation[J].IEEE Transactions on Robotics,2009,25(5):1196-1201.

[5]Shuster M D.Filter QUEST or REQUEST[J].Journal of Guidance,Control,and Dynamics, 2009,32(2):643-645.

[6]Liu C,Liu F,Jiang Z,et al.New method for initial orientation estimation in dynamic state using MEMS magnetometers and accelerometers[C]//Modelling,Identification&Control(ICMIC). 2012:29-34.

[7]Conrado A.Implementation of a quaternion-based Kalman filter for human body motion tracking using MARG sensors[D].Monterey:Naval Postgraduate School,2004.

[8]Yun X P,Bachmann E R,McGhee R B.A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements[J].IEEE Transactions on Instrumentation and Measurement,2008,57(3):638-650.

[9]Sabatelli S,Galgani M,Fanucci L,et al.A double-stage Kalman filter for orientation tracking with an integrated processor in 9-D IMU[J].IEEE Transactions on Instrumentation and Measurement,2013,62(3):590-598.

[10]高显忠,侯中喜,王波,等.四元数卡尔曼滤波组合导航算法性能分析[J].控制理论与应用,2013, 30(2):171-177.

[11]Brigante C M N,Abbate N,Basile A,et al.Towards miniaturization of a MEMS-based wearable motion capture system[J].IEEE Transactions on Industrial Electronics,2011,58(8): 3234-3241.

9-axis MEMS-IMU real-time data fusion algorithm for attitude estimation

ZHANG Jin-yi1,2,3,XU De-zheng2,LI Ruo-han1, CHEN Xing-xiu1,XU Qin-le2
(1.Key Laboratory of Specialty Fiber Optics and Optical Access Networks,
Shanghai University,Shanghai 200444,China;
2.Microelectronic Research and Development Center,Shanghai University,Shanghai 200444,China;
3.Key Laboratory of Advanced Displays and System Application,Ministry of Education, Shanghai University,Shanghai 200444,China)

To meet urgent application demands in indoor location and motion tracking, studies on low-cost high-resolution and real-time micro-electro-mechanical system-inertial measurement unit(MEMS-IMU)have attracted much attention.This paper presents a quaternion-based data fusion algorithm for real-time attitude estimation,including factored quaternion algorithm(FQA)for static attitude estimation,and Kalman filtering fordata fusion.A singularity avoidance method,axis-exchanged compensation,is proposed to modify the FQA,allowing the algorithm to track at all attitudes.An R-adapted module is designed to adjust the Kalman gain,which effectively restrains noise due to dynamic nonlinear acceleration,and improves attitude estimation accuracy.Experimental results show that the R-adapted Kalman filter can accurately estimate attitudes in real-time.Additionally,FQA with an axis-exchanged method has good performance in estimating attitudes of singularity points,and the computational efficiency is higher than a previous method by 50%.

micro-electro-mechanical system-inertial measurement unit(MEMS-IMU); attitude estimation;factored quaternion algorithm(FQA);singularity compensation;Kalman filter(KF)

TP 212.9;TP 391;TN 966;TN 967

A

1007-2861(2015)05-0547-13

10.3969/j.issn.1007-2861.2014.01.037

2014-01-02

上海市教委重点学科建设资助项目(J50104);上海市科委基金资助项目(08706201000,08700741000)

张金艺(1965—),男,研究员,博士生导师,博士,研究方向为通信类SoC设计与室内无线定位技术. E-mail:zhangjinyi@staff.shu.edu.cn