吴艺旋,张爱军,杨 萌

(南京理工大学机械工程学院,南京 210094)

0 引言

行人航位推算(pedestrian dead reckoning,PDR)是一种基于惯性导航的用于未知环境定位的行人定位方法,具有使用便携、成本低、不依赖基础设施等诸多优势,常用于消防救援、单兵作战等场景[1-2]。PDR算法包括步数检测、步长估计和航向估计3个步骤,通过上述步骤能够获取行人的实时定位。其中航向估计是PDR算法的最后一步,它的精度会直接影响最终定位结果的准确性,而单一依靠陀螺仪解算的航向角,由于存在漂移误差并不能维持长时间的高精度,通常采用非线性的卡尔曼滤波融合算法,通过磁力计和加速计来校准陀螺仪估计的航向角,从而提高行人航向角的精度[3-4]。

扩展卡尔曼滤波(extended Kalman filter,EKF)是一种最为常见的非线性滤波算法,然而在EKF的线性化转换过程中会产生模型误差累积[5-6]。随后提出的无迹卡尔曼滤波(unscented Kalman filter,UKF)[7]和容积卡尔曼滤波(cubature Kalman filter,CKF)[8]能够弥补EKF的缺陷,其中当滤波模型的维度超过3时,UKF会损失掉部分Sigma点对后验估计的统计特性,因此CKF的精度优于UKF。与此同时,CKF在递推过程中存在计算量大、数值不稳定、正定性丢失等问题[9],因此文献[10]提出了平方根容积卡尔曼滤波器(square root cubature Kalman filter,SRCKF),平方根滤波思想的引入确保了CKF过程中协方差矩阵的对称性和半正定性。

然而,在实际的行走过程中,传感器可能在行人行走状态突变等突发情况下采集到异常的数据,这些极端噪声会造成滤波精度下降甚至出现滤波发散的问题,H∞滤波器[11]能够最小化异常值对估计输出的影响,从而确保出现异常值时具有良好的鲁棒性。为了使H∞滤波器适应于非线性系统,文献[12]提出了H∞平方根容积卡尔曼滤波器(square-root cubatureH∞filter,SR-CHF)。

虽然SR-CHF提高了系统应对极端噪声的鲁棒性,但是对于外部噪声未知、动力学模型的不准确性等情况,其精度仍有待提高。为了解决这一问题,多种自适应滤波的方法被提出。Sage-Husa自适应滤波[13]可以实时校正过程噪声和测量噪声的统计特性,但很难给出准确的噪声特性,计算量大。文献[14]采用模型预测滤波器的概念建立动态模型误差估计器,将模型误差估计补偿到滤波器中。文献[15]使用Huber方法补偿过程不确定性。上述自适应方法都不能同时估计过程和测量噪声特性,而周东华等[16-17]提出的基于新息正交的渐消因子和多渐消因子法能够在不估计过程和测量误差的情况下抑制模型不确定性的影响,通过在预测协方差阵中引入渐消因子,保持对实际系统的强跟踪。

因此,MSTHSCKF算法通过将H∞滤波器、多渐消因子和SRCKF进行融合,能够提高滤波结果的稳定性、鲁棒性及其自适应能力。最后,通过对采集的实际行走数据进行验证,实验结果证明, MSTHSCKF在行人航向角估计中具有更好的鲁棒性、准确性和优越性,经由该算法滤波后的航向角更加接近真实值。

1 SRCKF

在行人航向解算的非线性系统中,其状态方程和测量方程表示如式(1)

(1)

其中,xk∈Rn,zk∈Rn;f(·)和h(·)分别为非线性状态转移函数和量测函数;wk和rk分别为独立同分布的状态和量测噪声,其协方差分别为Qk和Rk。

以下方程为平方根卡尔曼滤波的算法流程:

1)获取容积点

(2)

2)传播容积点

(3)

3)计算预测状态值

(4)

4)计算估计预测误差协方差矩阵的平方根

(5)

5)传播容积点,计算预测测量值

(6)

6)计算新息协方差矩阵的平方根

(7)

7)计算互协方差矩阵

(8)

(9)

8)计算增益

(10)

9)计算状态估计值

(11)

10)计算误差协方差的平方根系数

(12)

2 基于H∞和多渐消因子的SRCKF

2.1 H∞滤波

H∞滤波是卡尔曼滤波的特殊形式,它采用了博弈论的思想,能够在噪声最大时最小化输出状态误差。定义非线性滤波的代价函数如下[18]

(13)

(14)

从式(14)可以看出,H∞滤波需要线性化测量模型,文献[12]分别将H∞滤波融入了CKF和SRCKF,提出了不需要雅可比矩阵计算的H∞平方根容积卡尔曼滤波。SR-CHF在估计预测状态、预测误差协方差矩阵的平方根、预测测量值、新息协方差矩阵的平方根因子等步骤均与SRCKF一致。其求取SR-CHF误差协方差的平方根因子的过程如下[12]

(15)

其中,ΘJ表示J-酉矩阵;SR,k表示测量噪声协方差的平方根。

2.2 多渐消因子

针对系统模型的不确定性和状态突变等问题,为了提高滤波算法的滤波精度和跟踪性能,文献[16-17]提出了渐消因子自适应滤波器。渐消因子自适应滤波器的主要思想是修改预测误差协方差,使其满足

(16)

(17)

其中,ηk表示k时刻的残差,式(16)表示自适应滤波器的性能标准;式(17)表示新息序列不相关。通过在线调整预测协方差和增益矩阵,强制新息正交,以确保滤波器跟踪能力。使用多渐消因子修改预测协方差相对于单渐消因子来说,能够对多维状态进行修正,更好地改善整个系统平均性能。

Pk|k-1=λk(Pk|k-1-Qk)+Qk

(18)

其中,λk表示多渐消因子,它的计算过程如下所示。

根据先验经验,采用近似确定比例系数,即

λk(1)∶λk(2)∶…∶λk(n)=α1∶α2∶…∶αn

(19)

λk=diag[λk(1)λk(2)…λk(n)]

(20)

λ′k(i)=αiCk

(21)

(22)

(23)

(24)

(25)

(26)

其中,0<ρ≤1为遗忘因子;αi为弱化因子,它可以使得状态估计值更加平滑。由于Hk和Fk需要经过雅可比矩阵运算才能得到,为了使多渐消因子适用于SRCKF,可对Hk和Fk进行如下变换[20]

(27)

(28)

联立式(20)～式(28)可以求得多渐消因子λk,使用λk修正式(5)中新息协方差矩阵平方根,完成状态的预测。

(29)

2.3 MSTHSCKF

根据上述推导,建立了MSTHSCKF算法。基于式(1)的非线性系统,MSTHSCKF算法的详细计算过程(见图1)如下:

图1 MSTHSCKF流程图Fig.1 Flow chart of MSTHSCKF

1)初始化状态。

3)计算预测过渡误差协方差矩阵Pk|k-1。

(30)

5)计算预测自协方差。

(31)

6)根据式(20)～式(28)计算多渐消因子λk。

7)根据式(29)更新预测误差协方差矩阵平方根Sk|k-1。

10)计算增益Kk。

Kk=Pxz,k|k-1+(Re)-T/2(Re)-1/2

(32)

其中,Re=Pzz,k|k-1+Rk。

12)计算误差协方差矩阵的平方根因子Sk|k。

(33)

3 实验与结果分析

3.1 实验条件与数据采集

为了验证所提出的融合算法的性能,进行了室内行走实验,实验采用荷兰Xsens公司的MTi-30进行数据采集。MTi-30是一款高精度、高稳定性、具有动态快速响应等优势的姿态-航向参考系统,其内部集成了加速度计、磁力计和陀螺仪,可以输出每个传感器的原始数据,MTi-30内置各传感器性能指标如表1所示。

表1 MTi-30内置传感器性能指标Tab.1 Performances of MTi-30 built-in sensors

实验时将模块固定在行人脚背(见图2),连线另一端连接电脑,通过Xsens的上位机软件MT Manager实时采集传感器数据,采样频率为20 Hz,实验地点为一直形走廊,全长约60 m,宽约2 m,如图3所示,行人全程沿直线行走。

图2 传感器穿戴图Fig.2 Picture of sensor wearing

图3 实验环境Fig.3 Experimental site

3.2 实验结果分析

将采集的数据分别通过SRCKF、MSTSCKF、SR-CHF和MSTHSCKF进行传感器数据融合,获取行人运动时的航向角。将四元数以及陀螺仪漂移误差作为状态向量,将磁传感器的输出和加速度计的输出分别校准后作为观测向量[21]。在预测阶段,四元数通过陀螺仪的角速率积分进行预测;在更新阶段,使用当前四元数估计加速度计和磁力计的数值,并将其与加速度计和磁力计测试出来的原始数据进行对比,修正四元数的值。仿真过程的流程框图如图4所示。

图4 基于四元数的实验仿真流程图Fig.4 Experimental simulation flow chart based on quaternion

其中,初始四元数由静止状态下的加速度计和磁力计数值计算得到。

过程噪声协方差Q=diag[10-610-610-610-610-810-810-8],测量噪声协方差R=diag[10-110-110-110-810-810-8]。

为了直观地了解该算法的优越性,比较了经过不同滤波器滤波后的航向角与真实值的误差,其中图5、图6、图7分别代表经由不同滤波之后的航向角、航向角误差和航向角绝对值误差对比图。从图5可以看出,经由MSTHSCKF估计后的行人航向角围绕着真实值上下平缓地波动,最为接近真实值。从图6、图7可以看出,MSTHSCKF在大多数情况下的误差都保持在2°以内,SR-CHF和MSTSCKF大部分在3°以内,而SRCKF的误差突破了6°。表2给出了各滤波器求得的行人航向角的误差对比结果,通过表2能够更直观地看出,MSTHSCKF的误差绝对值的均值相较于SRCKF、SR-CHF和MSTSCKF分别下降了56.72%、21.54%和22.91%。MSTHSCKF的均方差相较于SCKF、SR-CHF和MSTHCSKF分别下降了30.62%、23.36%和22.52%。因此,可以得出,所提出的MSTHSCKF能够提供更为准确的行人航向估计。

图5 航向角解算结果Fig.5 Calculation result of heading angle

图6 航向角误差Fig.6 Error of heading angle

图7 航向角误差的绝对值Fig.7 Absolute value of error of heading angle

表2 行人航向角的滤波结果对比Tab.2 Comparison of filtering results of pedestrian heading angle

为了进一步验证MSTHSCKF的优越性,将经由不同滤波器滤波后得到的行人航向角应用于PDR算法中。在绘图时以行人起点为原点,行人行走方向为Y方向,与行走方向垂直的方向为X方向,以此构建行人在图3走廊上的二维行走轨迹,如图8所示,其中黑色轨迹表示参考路径。表3表示经由各滤波器获得的航向角所估计的行人轨迹在X方向上的位置误差,包括误差绝对值的最大值、最小值和均值。其中MSTHSCKF的最接近参考值,其平均误差在0.072 7 m,相较于SRCKF、SR-CHF和MSTSCKF分别下降了44.63%、24.97%和25.28%。同时MSTHSCKF对应的误差绝对值最大值和最小值也都比其他滤波器要小,这说明行人航向角估计误差确实很大程度地影响行人定位的精度,进一步验证了所提出的MSTHSCKF的抗差能力好,稳定性和可靠性更高。

图8 行人行走轨迹Fig.8 Pedestrian walking trajectories

表3 X方向的位移误差绝对值Tab.3 Absolute value of position error in Xaxis

4 结论

本文提出了一种基于H∞滤波和多渐消因子的平方根容积卡尔曼滤波算法MSTHSCKF,用于提高PDR算法的航向角估计精度,解决因传感器误差累积、噪声特性不确定、模型不确定等造成的行人航向角精度降低的问题,实验结果证明:

1)采用MSTHSCKF融合加速度计、陀螺仪和磁力计数据获得的航向角,解决了陀螺仪解算误差积累的问题。

2)MSTHSCKF融合滤波求得的航向角相较于SRCKF、SR-CHF和MSTSCKF,误差绝对值的均值分别下降了56.72%、21.54%和22.91%。均方根误差分别下降了30.62%、23.36%和22.52%,证明该算法能够提供精确稳定的航向角估计。

3)最后的行人定位实验进一步验证了该算法获得的行人航向角为后续的行人定位和导航提供了更高的精度。