基于卡尔曼滤波优化航向的PDR算法

朱军桃,林知宇,李海林,任招财,陈荣生,兰荣添,代程远

(桂林理工大学,广西 桂林 541004)

随着智能设备的发展,位置信息在现实生产生活中的应用越来越广泛,提高了诸多领域的生产生活效率。在室外位置信息的获取中,GNSS提供了较高精度的位置信息,使得智能设备广泛使用的传感器在定位时达到米级至亚米级的定位精度,可以应对绝大部分对于室外位置信息的需求。在室内环境中,由于建筑物本身的遮挡及多路径效应等因素的影响,GNSS信号存在较大的衰减,同时多路径导致定位的误差较大,造成了在室内环境中GNSS信号可用性大大降低,无法提供满足室内使用精度的定位与导航服务需求。

为了解决上述需求,目前的室内定位[1]广泛采用Wi-Fi[2-3]、地磁场指纹[4-5]、超宽带技术(ultra wide band,UWB)[6-7]、计算机视觉[8]、惯性传感器[9-11]、蓝牙[12]等方法进行实现。Wi-Fi、UWB需要借助信号发射装置并配合接收装置使用,提供室内定位信号,应用过程中需要提前准备相关设备;同时Wi-Fi、地磁场指纹、计算机视觉等室内定位方案需要前期采集数据,尤其在大范围应用场景下需要准备的前期工作较多。

行人航位推算(pedestrian dead reckoning, PDR)算法作为一种当前相对成熟、易于实现的室内定位算法,可以仅使用智能设备的地磁场传感器、惯性传感器信息,在不借助外界有源信号源的情况下,基于初始位置对设备位置进行位置推算与定位,在室内定位应用中可以协助其他传感器信息对数据进行修正进而提高定位精度。PDR算法主要包括步数估计、步长估计及航向估计,其中,航向估计可以使用惯性传感器与地磁传感器实现。但室内定位环境中磁场环境复杂,会受到电子设备、金属制品等的干扰,造成地磁场信息偏差。受传感器性能的限制,智能设备传感器本身存在一定的观测误差,同时惯性导航误差会随时间累积,因此在结合惯性设备的使用过程中,位置信息的误差会随时间而增大,进而对PDR算法中的航向估计造成影响,即影响位置推算结果。PDR算法可与Wi-Fi[13]、UWB[14]、计算机视觉等室内定位方法相融合,从而显著提高精度及算法的稳定性,减少PDR算法行人轨迹中“穿墙”现象的发生。

针对地磁信息在室内定位中受到复杂电磁环境的影响,以及智能手机传感器性能的限制所导致的数据精度误差,PDR算法中航向信息会产生角度误差,本文使用卡尔曼滤波算法,结合陀螺仪与加速度传感器信息,对智能设备在PDR算法中的角度数据进行优化,判断智能设备的航向角,并通过多个传感器的数据计算智能设备转向的角度,结合优化算法,对航向数据的计算进行优化,以期能提高PDR算法角度信息的可靠性,进而增加PDR算法的定位精度。

1 PDR算法

PDR算法是一种利用设备本身所集成的传感器,自主测量与计算定位信息的惯性定位方法。通过设备集成的惯性传感器与磁力传感器,得到距离、角度信息,并结合上一时刻的坐标数据,推算当前时刻坐标信息。PDR算法的原理如图1所示,公式如下

图1 PDR推算位置原理

(1)

式中,(xi,yi)为设备推算出的在当前坐标系下的位置坐标;(x0,y0)为初始状态下设备在当前坐标系下的位置坐标,可以通过上一时刻PDR算法推算的位置信息得到,也可以通过Wi-Fi定位、NFC标签等其他方式得到;n为上一时刻到当前时刻所走过的步数;l为上一时刻到当前时刻走过的n步中每一步对应的步长。通过对应时刻走过的步数与对应的步长信息得到航向角信息。

智能手机各轴的信息如图2所示。X轴正向为握持手机情况下的正右方向;Y轴正向为握持手机情况下屏幕的上方方向,即机身指向正前方;Z轴正向垂直于屏幕,屏幕正面方向为正向。

图2 智能手机各轴示意

1.1 步数检测

根据运动状态的不同,步数检测可以依据智能手机手持前进状态、位于口袋状态、导航状态等,以及惯性传感器数据,采用Y轴加速度的波峰、波谷轴检测及合成加速度检测的方法进行。其中,Y轴加速度波峰、波谷检测法适用于手机手持前进状态、位于口袋状态的步数检测;合成加速度适用于手持手机导航状态。

如图3所示,行人手持手机前进时,手机随着手臂摆动,可以近似为绕着肩膀、以手臂为半径的圆周运动。在行人前进方向上,每向前行走一步,手机的运动轨迹可以近似为前进-后退-前进或后退-前进-后退。此时Y轴加速度输出的加速度信息类似正弦波,正弦波的波峰-波峰或者波谷-波谷可近似对应实际行走的一步。因此可以利用波峰或波谷检测步数,利用相邻的波峰或波谷信息,得到每一步对应的加速度数据,并以此计算步长信息。本文将采用波峰检测法对数据进行检测。

图3 手持手机前进Y轴数据

1.2 步长计算

当前常用的步长模型包括固定步长、线性及非线性模型。线性步长的步频模型[15]公式为

S=af+b

(2)

式中,f为步频;a、b为模型系数,可通过试验拟合得到。

考虑人的行走步长与身高、体重等因素有一定关联性,固定步长算法在使用中存在一定的局限性,对于不同的人可能产生的误差大小并不相同;同时由于每一步的步长并不完全一致,且在转弯等情况下,步长相比直行有大幅改变,存在一定的随机性。因此,基于人的身高、体重等信息计算得到的线性步长会因步长的随机性产生一定的误差。

本文使用基于行人加速度信息计算步长的非线性步长,即利用智能手机的加速度计得到的步长信息。常用的非线性步长为Weinberg方法[16],公式为

(3)

式中,S为数据计算得到的单步步长信息;k为非线性步长模型系数,利用多组试验结果采用最小二乘法得到;a为智能设备在Y轴上对应该单步范围内的加速度信息,amax为最大加速度,amin为最小加速度。经测试,Weinberg方法在不同场景下定位精度更高,因此本文采用Weinberg方法。

1.3 航向信息

PDR算法中的航向信息,是利用地磁场信息并结合加速度计信息计算当前设备的姿态,通过计算俯仰、偏航角修正磁场数据的信息,得到智能设备在当前姿态下Y轴正向对应平面指向的航向信息。

通过加速度计数据计算得到设备的俯仰角θ与偏航角γ,公式如下

θ=arcsin(Gy/g)

(4)

γ=arctan(-Gx/Gz)

(5)

式中,G为设备加速度计测得各轴上的加速度信息,与加速度各轴正向相同的为正,相反的为负,单位为m/s;g为当地重力加速度。通过俯仰角与偏航角计算得到设备当前的航向角φm为

(6)

式中,M为设备磁力传感器测得各轴上的磁场信息。通过航向角φm,并结合磁偏角修正量信息md,计算得到当前设备Y轴正向指向的方位角信息φn为

φn=φm+md

(7)

式中,磁偏角修正量西偏为负,东偏为正,可通过公开信息查询得到当地磁偏角。陀螺仪是智能设备中检测角速度的传感器。检测特定时间段的角速度,并利用角速度信息与时间进行积分,得到设备在一个时间段内绕各轴的旋转角度。陀螺仪受到外界电磁信号的干扰较小,可以短时间内获得相对高精度的角度信息。但陀螺仪在没有外界数据对其角度信息进行修正的情况下,会产生误差累积。具体表现为,在长时间的观测中,陀螺仪会出现特定方向上的误差,即零偏误差,需要使用前对陀螺仪进行测量得到误差信息并加以消除。根据Android设备的参考文档,陀螺仪获得设备围绕各个角度旋转的角度公式为

r=dt·ω

(8)

式中,dt为陀螺仪记录的当前时刻与上一时刻的时间差;ω为当前时刻与上一时刻之间的角速度;r为积分得到的dt时间内设备绕对应轴的旋转角。

利用地磁传感器的航线信息与陀螺仪的角度信息合成的模型公式为

φk+1=φk±ω·dt+wk·dt

(9)

式中,φk+1为合成后角度;φk为上一时刻角度,由地磁场数据计算得到;ω为上一时刻到当前时刻的角速度,由陀螺仪计算数据计算得到;wk为角速度积分的增量。

2 卡尔曼滤波

卡尔曼滤波是一种在最小均方根误差准则下得出的作为一种状态最优估计的方法,在无人机、机器人等姿态解算中被广泛应用,在室内定位中也有很大的应用潜力[17]。卡尔曼滤波可以根据上一个时刻的系统状态,推测下一时刻的系统真实状态。本文将卡尔曼滤波应用于融合磁场、加速度、陀螺仪传感器的角度信息计算,以提高航向角精度。

先根据上一时刻的数据进行预测状态估计,公式为

(10)

利用上一时刻的协方差求先验估计协方差矩阵,公式为

(11)

计算卡尔曼增益,公式为

(12)

用更新后的参数进行后验估计,公式为

(13)

更新后验估计协方差矩阵,公式为

(14)

利用卡尔曼滤波优化航向的PDR算法具体步骤如图4所示。

图4 卡尔曼滤波优化航向PDR算法步骤

由智能设备的加速度传感器、地磁传感器、陀螺仪分别得到加速度、地磁、陀螺仪的角度数据,利用加速度数据得到步长、步数信息,并计算设备当前的俯仰角、横滚角,结合地磁传感器得到地磁数据,计算得到当前设备的航向角信息。利用陀螺仪计算得到对应时间内绕设备各轴旋转的角度信息,利用卡尔曼滤波将其与地磁场得到的方位角信息进行融合,得到融合后航向角信息。利用融合后的航向角信息与步长、步数信息,并将上一时刻的坐标作为计算的初始坐标,对当前坐标进行推算,计算得到当前时刻的坐标。

3 试验结果分析

3.1 试验设置

使用小米10手机作为试验器材,手机的惯性传感器、地磁传感器与陀螺仪均设置为50 Hz的采样频率对数据进行采样。试验开始前,智能设备进行“8”字形运动,使磁力传感器获取各个角度的磁场信息,并静置一段时间同时记录陀螺仪数据,得到陀螺仪的零偏误差,在数据处理中消除零偏误差。

在确保试验路线无行人与物品遮挡与干扰的情况下,采用单手持智能手机方式前进,手机随手臂自然摆动,在同一参考行进路线上,进行20组试验。

3.2 试验分析

将利用加速度计修正的地磁信息作为航向参考依据的PDR算法,与利用卡尔曼滤波对陀螺仪与加速度计进行修正的地磁信息作为航向参考依据的PDR算法作对比。两种方法使用相同试验采集的加速度、地磁、陀螺仪传感器得到的数据进行对比。试验路线图及其中一组数据如图5所示。

图5 试验路线及其结果

由图5可以看出,在经过第2次转向后,未经优化的PDR算法得到的角度数据,与设计路线产生了一定的夹角,导致最终的定位误差,且误差会随着距离与时间的增加而增大;而优化后的PDR算法角度数据更接近于设计路线,误差随距离与时间而增大的趋势较小,即优化后的角度信息更接近于真实情况,定位信息更准确。20次试验的误差分布如图6所示。

图6 20次试验误差分布

20次试验PDR算法得到的坐标与设计线路的误差分布区间见表1。

表1 不同方法误差分布 m

由表1可知,20次试验中,经过卡尔曼滤波融合的航向信息相比未经卡尔曼滤波处理的数据,主要分布在(1,2]m误差区间内,占55%;[0,0.5]m及(0.5,1]m误差区间的结果占35%;>2 m的误差仅占10%。未经滤波的数据,仅有15%的数据在[0,2]m的误差区间内,75%的数据误差大于2 m。同一次试验中,使用卡尔曼滤波对航向数据融合的PDR算法得到的结果误差,均小于仅使用地磁得到航向信息的PDR算法得到的结果,即使用卡尔曼滤波修正航向后的数据精度更高。

使用不同方法获得航向信息的试验误差结果见表2。

表2 不同方法的误差 m

由表2可知,使用加速度对俯仰、偏航角进行修正的地磁信息为航向的PDR算法,其平均误差为2.93 m,最大误差为4.21 m,中误差为3.07 m;而利用卡尔曼滤波使用陀螺仪与加速度对俯仰、偏航角进行修正的地磁信息进行融合得到的航向信息的PDR算法,精度有明显提升,平均误差为1.36 m,最大误差为2.22 m,中误差为1.36 m。利用卡尔曼滤波优化航向的PDR算法相对于仅使用地磁的PDR算法,平均误差减小了1.63 m,最大误差减小了1.99 m,中误差减小了1.71 m。

同时,在20组试验数据计算得到的路线中,由于部分数据的行进路线靠近附近行人与电子设备,造成磁场传感器采集数据产生一定波动,对航向角指示产生了偏差,在未经过卡尔曼滤波处理的情况下,行进方向会出现偏差,影响最终的定位精度;在使用卡尔曼滤波对航向进行优化的情况下,上述干扰仍然对最终定位精度产生了一定的影响。

4 结 语

智能手机地磁传感器测量精度相对有限,且室内定位中的室内电磁环境复杂,影响地磁场的测量精度。PDR算法使用惯性与地磁场信息,在惯性导航中误差会不断累积。本文使用卡尔曼滤波,结合惯性传感器加速度信息,对地磁场传感器数据的精度进行改进。经试验验证,优化后最终平均误差由2.93 m提升至1.36 m,大部分结果误差在2 m范围内。

但该算法仍有较大的改进空间,可以考虑使用深度学习等方法进行优化;同时可利用Wi-Fi、UWB、计算机视觉、NFC标签等有源数据获取的位置信息与PDR算法的定位信息进行比较与融合,提升定位精度。