基于无逆Kalman 滤波器的姿态估计算法①

何百岳 张文安

(浙江工业大学信息工程学院 杭州310023)

0 引言

姿态估计在生产生活中得到了广泛的应用,例如医疗健康[1-3]、人机交互[4]等等。近年来,随着磁-惯性传感器(magnetic/inertial measurement unit,MIMU)的迅速发展,这种易于穿戴、价格较低的传感器得到了研究人员的广泛关注[5]。

但是磁-惯性传感器的应用仍然存在一定的困难。例如,陀螺仪积分会带来姿态的漂移[6],加速度计容易受到外部线性加速的干扰[7-8],磁力计易受到外部磁场的干扰[9-10]。基于Kalman 滤波器的多传感器融合算法能实现多传感器信息的融合、信息互补,提高估计精度。文献[11]引入了乘积扩展Kalman 滤波器(multiplicative extended Kalman filter,MEKF),在滤波过程中估计偏差来提高精度。文献[12]在扩展Kalman 滤波器的基础上,增加了隐马尔可夫模型(hidden Markov model,HMM)分辨状态,并采取了自适应方法调整滤波参数,提高了估计精度。文献[13]在Kalman 滤波的框架下,将加速度量测和磁力计量测分解,降低了磁场干扰对重力方向姿态角的影响。基于阈值的方法将陀螺仪量测作为过程方程,姿态解算得到的四元数作为观测方程,实现了信息的融合[14-15]。这些方法已能达到不错的估计精度,但是对于现实的应用,实时性能也很重要。基于标准Kalman 滤波器(standard Kalman filter,SKF)的方法需要对新息矩阵求逆。求逆的运算复杂度是O(n3)[16],这给系统带来了较大的计算负担。特别地,嵌入式设备更是难以在进行这种运算的同时确保实时性能。文献[17]针对主对角占优的矩阵,提出了应用近似逆来代替精确逆值计算的无逆Kalman 滤波(inverse free Kalman filter,IFKF)算法。这种算法将求逆运算替代为一个复杂度为O(n2)的运算。姿态估计应用中,由于磁-惯性传感器的特性,测量噪声协方差矩阵通常被假设为项值较大的对角阵。这导致了新息协方差矩阵自然地成为一个对角占优矩阵,从而保证了泰勒级数展开的收敛性。因此,无逆Kalman 滤波器适用于基于磁-惯性传感器的人体姿态估计应用。

在各种运动情况下,单一的姿态估计策略不能反映复杂的人体运动情况,最终导致计算资源的浪费和精度的下降。在行人导航应用中,零速度更新(zero velocity update,ZUPT)是一种基于步态的分类估计策略。这种策略能有效提高室内导航的精度[18-19]。对其他运动模式识别,例如对慢走、坐姿、乘电梯的识别,也能有效提高导航精度[20]。但是在姿态估计中此类的讨论仍然较少,这值得进一步研究。针对以上问题,本文提出将运动情况分成稳态和动态两种情况分别采用不同的估计策略。

根据上述讨论,精度和计算效率都是姿态估计的重要指标。本文从引入无逆Kalman 滤波器、设计无反三角函数判定条件和设计双策略方法3 个方面,提出了一种准确、计算效率较高的姿态估计方法。

1 问题描述

姿态估计主要研究肢体坐标系相对参考坐标系的旋转运动情况,即估计人体坐标系相对参考坐标系的空间转动。正如前文所述,虽然欧拉角能较为直观地描述运动姿态,但是会存在万向节死锁问题。另外,四元数法计算更为高效,所以本文采用旋转四元数来描述任意的人体运动姿态。姿态估计的目标是要找到一个可以将在肢体坐标系下的任意向量x旋转到参考坐标系下的旋转四元数。这个旋转关系可以由以下方程表示:

在姿态估计领域,一个典型的体感网络系统需要同时处理多个磁力-惯性传感器的数据。进一步地,姿态估计往往是人体姿态估计和运动跟踪的一个组成部分,姿态估计值很可能是其他应用的输入,如手臂运动轨迹重建就需要姿态估计数据。如果姿态估计的计算效率过低,则会降低整个系统的实时性能。鉴于以上原因,这一类系统亟需一种姿态估计准确而且计算复杂度较低的方法。为此,本文提出了一种计算效率较高同时又不影响估计精度的姿态估计算法。

2 惯性传感器信号分析和处理

加速计和磁力计的量测信号可以分成以下3 部分:

其中,g是重力加速度,m是地球磁场强度,bA是由于人体运动而被引入的线性加速度干扰,bM是环境中的铁磁物质产生的磁场干扰,nA和nM分别是传感器量测产生的传感器噪声,都可被假设为零均值的高斯白噪声。

陀螺仪的量测方程如下:

其中,ω是角速度,nG是测量的噪声,可被认为是均值为0 的高斯白噪声。

姿态估计的精度依赖于传感器测量的准确程度,然而,传感器的量测会受到线性加速度和外部磁场干扰的影响。根据惯性传感器的特性,很多研究工作都采用了基于阈值的方法[14-15]来降低干扰的影响。鉴于这种方法在工程应用上的便利性和有效性,本文算法以其为基本框架,并对其进行修改以提高计算效率。特别地,文献[14]在考虑磁场干扰时,需要引入反三角函数运算,这对设备的计算能力提出了较高的要求。为避免复杂的反三角函数运算,本文设计了无反三角函数运算的磁场噪声评价指标,并将其称为严重干扰拒绝方法(severe disturbance reject method,SDR)以示区别。

2.1 基于加速度计的严重干扰拒绝

加速度计测量的是传感器的加速度大小和方向。其中,在人体运动的范围内,地球表面的重力加速度可被近似认为是大小和方向都不随时间改变的矢量。那么,只要知道参考坐标系下重力加速度和最终姿态的重力加速度,就能获得当前姿态和竖直方向下的夹角。然而,在姿态运动的情况下,加速度计的量测值是地球重力加速度和运动加速度的矢量和。这个矢量和通常和重力加速度的方向是不相同的。那么,就认为运动带来的线性加速度是一个干扰。一旦这个干扰过大,则加速度计的测量会被认为是不可靠的。这时,预测的加速度,即,会比直接量测出来的加速度更可靠。因此,观测向量为

其中,g是参考坐标系下的重力加速度(在传感器完全静止的情况下测得),是时间更新得到的预测四元数,计算公式如下文的式(7)所示,εA1和εA2是由多次实验得出的运动加速度干扰检测阈值。

2.2 基于磁力计的严重干扰拒绝

基于磁场强度来判定航向角是一种很常用的方法,只要知道参考坐标系下地球磁场强度和最终姿态的传感器坐标系下的磁场强度,就能获得当前姿态在水平方向的朝向。此方法只考虑了匀强磁场下的导航问题,但是,室内场景下会存在铁磁物体以及电线,这些物品会产生额外的磁场,导致匀强磁场假设不能成立。鉴于以上问题,本文设定了2 个判定条件来检测磁场强度是否存在严重干扰的情况。通常,在无干扰的环境下,传感器只能检测到地磁场。那么,可以通过检测磁场强度的大小来判定是否存在较大的干扰,一旦磁场强度变化过大,则认为这一时刻的磁场测量是不准确的。此外,很多文献都引入了磁倾角作为判断指标。磁倾角指磁场强度和重力加速度之间的夹角,在磁场强度不变的情况下,磁倾角不会有较大的变化。更重要的是,相对于磁场强度模值,磁倾角对磁场变化更敏感,因此磁倾角更能用于指导判定是否出现严重干扰的情况。文献[10]列举了一个磁场强度变化而磁场强度幅值不变的例子,这种情况下,引入磁倾角作为判定条件很有必要。

综上所述,磁场的观测向量可以写成如下形式:

2.3 无反三角运算的磁倾角判定规则

在判定是否存在较大磁场干扰时,判定条件需要计算每个时刻的磁倾角。反三角函数的计算会给嵌入式系统带来较大的计算负担。而本文通过化简,剔除了每一时刻的反三角函数估计运算,新的判定条件可以写为

其中,mt是传感器坐标系下的地磁场量测,gt是参考坐标系下的重力加速度,是前文中获得的可靠的重力观测。特别地,不随时间变化,可以在初始时刻被确定。

由于判定条件式(6)不需要任何的反三角函数运算,计算复杂度相比于原来的判定条件式(5)会减少,从而减少了算法的执行时间。

3 动态状态下的姿态估计策略

在动态的情况下,MIMU 中单一的传感器不足以提供准确的姿态信息,比如说陀螺仪的估计会受到积分漂移的影响,加速度计对重力的测量会受到运动产生的线性加速度的干扰,以及磁力计受到外部磁场的干扰导致无法测量出地磁场的磁场强度。鉴于以上问题,利用基于最优估计理论的Kalman 滤波器进行多传感器信息融合,实现信息的互补是很有必要的。

动态状态下的姿态估计策略主要有以下3 个步骤组成。首先,通过四元数运动学获得离散的状态更新方程。其次,用严重干扰拒绝器获得更为可靠的陀螺仪和加速度计量测值。将可靠的量测值用最优两向量四元数估计方法(optimal two-observation quaternion estimation method,O2OQ 估计算法)计算得到四元数量测,作为量测值。最后,设计相应的IFKF 滤波器,将角速度量测和旋转四元数量测融合,得到最终的姿态估计结果。

3.1 基于陀螺仪的姿态更新

基于四元数的离散时间状态更新过程方程如下[21]:

其中,qt代表第t时刻从参考坐标系到肢体坐标系的旋转四元数,Δt是一个相对短的时间间隔,对于本系统是10 ms。Y(ωt) 可以由下式表示:

其中,ωt是三轴陀螺仪的角速度量测值,[ω×]是反对称矩阵的表示方法。

3.2 基于加速度计和磁力计确定姿态

研究姿态解算最著名的问题就叫做Wahba 问题[22]。本文在考虑计算效率和估计精度2 个指标后,选择两向量最优四元数估计方法来解算姿态,具体步骤参见文献[22]。为保证解算出的姿态的准确性,本文使用了前述的严重干扰拒绝(SDR)方法来获得t时刻的重力加速度观测值和磁场强度观测值。

3.3 无逆Kalman 滤波器设计

系统的状态量设定为姿态四元数qt,观测量是qO2OQ,t,由O2OQ 算法计算获得。整个动态系统可表示为式(9)。

过程噪声协方差Qt-1表示为

对于本文研究的动态系统式(9)来说,标准Kalman 滤波器所示如下:

由式(11)可得,标准Kalman 滤波器的估计过程需要对新息协方差矩阵求矩阵逆,这一运算过程会给整个系统带来较大的计算负担,从而降低系统的实时性。为避免其产生的不良影响,本系统引入了无逆的Kalman 滤波算法。在人体姿态估计的应用中,量测噪声一般都会被设定成对角线较大的矩阵,这导致了新息协方差矩阵是一个对角占优的矩阵。对角占优这个特性可以保证矩阵在一阶泰勒展开后,只取泰勒展开式的低阶项也能保证收敛性。换句话说,使用新息矩阵一阶泰勒展开的低阶项的逆矩阵和来近似代替矩阵的精确逆值不会引起较大的误差。

其中,Cii=Pii +Rii,P和R分别代表自协方差矩阵和量测协方差矩阵。

除此之外,无逆Kalman 滤波的形式式(12)中,每一行互不相关,这个特性适合于多个处理器的并行运算,能够进一步加快运算速度。

最后,状态四元数需要被标准化表示旋转。

其中,分子部分是无逆Kalman 滤波器对姿态的估计值,分母中‖·‖运算表示求向量的模值。

根据文献[17],本文研究的动态系统式(9)是完全一致可观的,(θt,) 是完全一致能控的,而且初始时刻的状态协方差P0是半正定的或者正定的,所以是有界的。由此可知,本文设计的基于无逆Kalman 滤波器的姿态估计算法是收敛的。

4 稳态状态下的姿态估计策略

人体运动情况变化复杂,针对特定的情况应该因地制宜地采用一些更加方便灵活的估计策略。针对人体在较长时间几乎不动的情况下,例如手臂放在桌上休息,或者躺在床上,本文设计了一种特殊的估计策略。实际情况下,稳定状态可能会经常出现,并且持续较长的时间,单独研究它的特性很有意义。

4.1 基于加速度计的稳定状态检测

在稳定的状态下,加速度计只能检测到重力加速度,因此,每个轴检测到的加速度分量和上一时刻会是相同的。这样,根据相邻时刻每一轴上检测到加速度的差值可以判定系统是否处于稳态。

其中,ax(t),ay(t)和az(t)表示了传感器坐标下,X轴,Y轴和Z轴在时刻t的量测。

4.2 基于陀螺仪的稳定状态检测

只要检测传感器的量测是否小于一个阈值,就能判定传感器是否处于稳定状态。判定公式如下:

其中,ωx(t)、ωy(t)和ωz(t)表示传感器坐标下,X轴、Y轴和Z轴在时刻t的原始量测,thgro,X、thgro,Y和thgro,Z分别是判定稳定的X轴、Y轴、Z轴的阈值,是多次实验得出的经验参数。

4.3 基于时间阈值的稳定状态检测

陀螺仪和加速度传感器对于较小的运动都拥有较高的灵敏性,但是一些特殊情况的出现可能会导致估计策略的频繁切换,这均会降低系统的稳定性。因此,在判定稳定的时候,不但要考虑当前时刻是否稳定,也要结合过去一段时间的状态进行分析评判,为此判定稳态时间周期被引入。判定稳态时间周期要求:在整个时间周期内,所有时刻都满足加速度计和陀螺仪的判定条件,系统才被认为进入了稳定状态。判定条件可以写成以下形式:

其中,count是计数时刻数,thinterval是设定的时间阈值。

4.4 基于时间阈值的稳定状态检测

在稳定状态下,每一时刻的估计值都与上一时刻相等,这一策略能够避免较为复杂的姿态估计过程,既能减少系统估计的运算量,又能减少系统中由于计算产生的能量损耗。

姿态的维持和协方差的预测想法来源于文献[23],具体的递推方程如下所示。

综上所述,本文提出的算法的伪代码如算法1所示。

5 实验和讨论

实验使用了Xsens 公司生产的MTi-300 惯性传感器,包括1 个三轴加速度计,1 个三轴陀螺仪和1个三轴磁力计,磁力-惯性传感器的采样频率是100 Hz。实验场景如图1 所示,其中8 个OptiTrack摄像头环绕在实验人员周围,同时以100 Hz 的频率获得前臂刚体的姿态信息。

图1 实验场景

5.1 实验1 慢速且无磁场干扰

实验者被要求在实验室的中间较宽阔的地方做运动,这样可以避免各种铁磁设备对实验的干扰。同时,为了避免较大的线性加速度噪声,测试人员被要求手臂旋转运动尽可能缓慢。如前文所述,Opti-Track 系统能够计算获得精度极高的姿态值,因此OptiTrack 系统的量测值可以作为人体前臂的标准值,用于评价估计值的精确度。图2 展示了标准Kalman 滤波器和无逆Kalman 滤波器的估计结果,这两个滤波器的估计值都非常接近OptiTrack 的量测。为了便于读者区分,图3 展示了标准Kalman 滤波器和无逆Kalman 滤波器姿态估计值的差值,其中由于标准Kalman 滤波算法是更为精准的计算方法,因此标准Kalman 滤波器的估计值是基准值。从图3可以清晰地看出,基于无逆Kalman 滤波器的姿态估值与基于标准Kalman 滤波器的姿态估值之间的差异非常小,比方向四元数要小很多个数量级。因此,新引入的无逆Kalman 滤波器并不会明显地影响姿态估计的精度。本文为了方便展示精度,提出了均方根误差(root mean square errors,RMSE)这一指标。它计算每一步估计值和真实值(OptiTrack 系统的量测值)直接误差的均值,因此,RMSE 能够指示姿态估计的精确度。表1 展示了基于标准Kalman和基于无逆Kalman 的姿态估计结果,同时也比较了包含SDR 和不包含SDR 的估计结果的RMSE。从表1 结果发现,在保留四位小数的情况下,无逆Kalman 滤波器并不会降低精度。而且由于干扰较小,SDR 对性能的提升并不明显。表2 展示了计算时间耗损。算法时间耗损描述了相同的实验数据经过多次运算使用时间的平均值,本文的时间耗损是10次运算的平均时间。因此它能够在统计意义上描述运算的复杂程度。显然,无逆Kalman 滤波器的计算时间要少很多,几乎是标准Kalman 滤波器计算时间的70%。实验结果显示,动态的情况下,无逆Kalman 滤波器具备更高的计算效率。

表1 估计结果对比表

表2 时间耗损比较(实验1)

图2 姿态估计结果图

图3 估计差值结果图

5.2 实验2 快速运动且存在磁场干扰

和实验1 相比,实验2 是一个更为复杂的场景。实验人员被要求将手臂以较快速度旋转,同时也靠近磁铁,以造成一定的磁场干扰。从图4 可以看出整个实验大部分都是在动态策略下运行,虚线分别表示实验中设定的上下阈值。另外,在执行过程中还存在几段磁场干扰。从图5 可以看出,包含严重干扰拒绝(SDR)的标准Kalman 滤波估计结果和无逆Kalman 滤波估计结果和OptiTrack 估计都很接近。图6 展示了标准Kalman 滤波器和无逆Kalman滤波器的差值。同样地,文中以标准Kalman 滤波器的姿态估计值作为基准值,无逆Kalman 滤波器的姿态估值与其差异非常小,因此,可以认为这两种方法有相接近的估计精度。与之相对的是,表3 展示了算法的时间耗损。其中,使用无逆Kalman 滤波器的估计方法后,计算时间只相当于原来计算时间的67%。进一步来说,本文使用的是单线程的处理方法。一旦转换成多线程的方法,运算时间能够进一步地减少。另外,本文还关心严重干扰拒绝的效果。图7 比较了是否包含严重干扰拒绝的无逆Kalman滤波器估计的姿态四元数。显然,含有严重干扰拒绝的算法能够获得更高的估计精度。除了图片之外,表4 能够更为直观地展示精度的高低。标准Kalman 滤波器和无逆Kalman 滤波器在RMSE 的指标上表现非常一致,但是一旦没有包含严重干扰拒绝方法,姿态估计的精度会出现大幅下滑。结合实验1 来看,无逆Kalman 滤波算法能够在提高运算效率的同时不影响估计的精度,而严重干扰拒绝能够较大程度地缓解时间较短的外部干扰。

表3 时间耗损对比表(实验2)

表4 估计结果对比表

图4 噪声干扰图

图5 姿态估计结果图

图6 估计差值结果图

图7 姿态估计结果图

5.3 实验3 稳态情况和切换时刻的姿态估计性能

实验3 的运动过程描述如下:实验人员被要求先快速旋转手臂到某一角度。然后放在桌子上保持手臂不动,这时另外一名实验人员拿磁铁不断在周围晃动,引入磁场干扰。最后再快速运动回到原始位置。而图8 能够较好地反映出这样的实验场景。这样的运动情景会存在多次的动态和稳态的状态切换,而且长时间的磁场扰动,使得文献[14,15]提出的基于阈值的方法失效,大量的磁力计量测数据被丢弃,导致姿态漂移。但是本文提出的稳态策略则主要用于解决这一问题。图9 展示了双策略较标准Kalman 滤波方法更接近真实值,这是因为磁场干扰幅值大而且时间长,大量的量测数据被拒绝丢弃。使用陀螺仪积分的姿态估计值不能够得到矫正,因此会产生一定程度上的漂移。图10 展示了累积误差的图像。累积误差指整个运动时间段内,估计值和真实值之间的绝对误差总和,指示了估计器的精度。另外,从表5 可以看出双策略的方法能够达到比较高的精度,这种方法精度略高于标准Kalman 估计方法。表6 显示双策略方法比原来的基于标准Kalman 的方法耗时明显缩短。因此,双策略方法确实是一种有效维持精度且运算快速的方法。

图8 传感器量测模值

图9 姿态估计结果图

图10 累积误差图

表5 估计结果对比表

表6 时间耗损比较(实验3)

6 结论

本文针对现有基于标准Kalman 滤波器的姿态估计算法复杂度较高的问题,应用无逆Kalman 滤波器,设计了基于无逆Kalman 滤波器的姿态估计方法。无论是干扰较小的实验1 还是干扰较大的实验2,这种方法都能够获得和基于标准Kalman 滤波器估计方法相近的计算精度,同时计算时间大幅下降。这说明了本文提出算法的高效性和高精确度。进一步地,实验3 证明,在稳态的情况下,双策略确实能够降低计算量,同时避免了姿态的漂移,能够获得更高的精度。

综上所述,实验结果证明了本文提出的方法的有效性。