张永耀,周凌柯,黄海舟,蔡紫烨,李 胜

(南京理工大学自动化学院,南京 210094)

0 引言

随着智能手机等便携式移动终端的快速发展,相关的基于位置的服务成为人们关注的重点[1]。其中,惯性测量单元(IMU)具有尺寸小、自主性高等特点,成为行人导航领域研究的核心,在消防救援、单兵作战、商场等室内定位中具有广阔的应用前景。

基于惯导的定位技术通过积分运算或者航迹推算的方法得到运载体当前的位置、姿态等信息,但是随着时间的积累,通过积分计算出来的误差不断累积,只能在短时间内得到准确的位置信息。对此,许多学者进行了改进。Foxlin[2]将零速更新(zero velocity update,ZUPT)和扩展卡尔曼滤波器(extended Kalman filter,EKF)结合起来,提出了惯性导航系统(inertial navigation system,INS)+EKF+ZUPT的行人导航框架,对速度误差进行了修正;Rajagopal[3]在INS+EKF+ZUPT 框架中融入零角速度更新算法(zero angular rate update,ZARU)对陀螺仪的误差进行补偿;Jimenez等[4]使用地磁修正算法和启发式航向漂移修正算法(heuristic drift elimination,HDE)对航向角误差进行修正。

Jimenez提出的算法提供了航向角作为额外的观测量,但是如何正确获取航向信息十分重要。在实际应用中,地磁信息很容易受到干扰,对于磁力计测量的数据,如果直接使用,会引入额外的误差,因此需要对原始磁场信息进行校正。Afzal等[5]提出了稳定状态磁场(quasi-static magnetic field,QSF)的概念,并根据磁场强度构建检验量,对磁场信息进行检验;刘少兵等[6]针对QSF检验容易出现误判的情况,利用磁场强度、磁力计解算的航向角的差值、磁场强度变化量构造检验量,但是该方法只适用于直线行走时的磁场检验,在曲线行走时无法判断。针对行人室内定位中存在航向角漂移的问题,Borenstein等[7]提出了HDE算法,基于人行走一般沿着走廊等直线行走这一思想,将建筑物的方向设定为主方向,通过主方向来约束航向角;Jimenez等[8]则对HDE 算法进行改进,只有检测到行人正在直行时才会执行HDE 算法,从而减小行人在非主方向上行走时HDE的过度修正;针对HDE 算法在非主导方向上容易错误校正的缺点,Zhu等[9]提出一种实时扩充主方向数量的HDE 算法,使得航向修正在非预设主方向上时仍然能够正常执行;针对HDE 方法中判断直行需要设定阈值的问题,Muhammad等[10]提出了一种无阈值的转弯检测技术;邓平等[11]根据航向角变化的时域特征对航向角进行分类,确定行人的直行状态,并设定16个主方向,进行航向角的修正。

在分析以上算法的基础上,本文针对长时间的惯性导航存在航向失真从而导致定位误差大的问题,提出了一种融合IMU 和磁力计的行人导航算法,获取准确的航向信息。根据行人运动过程中步幅航向的变化,将行人的行走分为直线行走和曲线行走两部分,分别采用不同的方法对航向角信息进行修正。当检测到行人沿曲线行走时,使用地磁信息辅助导航,针对传统的QSF方法在磁场受到持续稳定干扰时容易出现误判的情况,设计了一种基于磁场强度、地磁倾角、INS与磁力计解算的航向角之差这三个特征值的广义似然比检验算法,进行稳定状态磁场的检验,获取纯净磁场从而计算得到航向信息;当检测到行人沿直线行走时,针对传统的HDE算法需要提前预知实验环境信息并人为设定主方向的缺点,提出了一种自适应航向角漂移修正(adaptive heuristic drift elimination,a HDE)算法,在直行时实时建立主方向,对行人的航向误差进行修正,提高行人导航定位的精度。

1 基于IMU 和磁力计的行人导航算法模块设计

本文提出的导航算法结构如图1所示。

图1 基于IMU 和磁力计的行人导航算法Fig.1 Pedestrian navigation algorithm based on IMU and magnetometer

图中,abk和wbk表示k时刻加速度计和陀螺仪在载体坐标系下的采样结果;Cnb,k/k-1表示k时刻的方向余弦阵;Δα,Δγ和Δφ分别表示行人的俯仰角、横滚角和航向角误差;b和n分别表示载体坐标系和导航坐标系。

建立15维的状态矢量模型Xk=[δΦkδvkδPkεbkΔbk]T,分别表示k时刻行人的姿态误差、速度误差、位置误差、陀螺仪零偏误差和加速度计零偏误差,选择导航坐标系为东北天坐标系。将捷联惯导算法的更新结果作为卡尔曼滤波的预测量,其余约束算法提供误差值的虚拟观测量,进行卡尔曼滤波融合后对行人的位置和姿态进行估计。

当行人行走时,从足部接触地面到抬起足部这一段时间,称为零速状态,本文所有的量测更新都是基于零速状态进行的。根据传感器采集的加速度计和陀螺仪数据,判断行人是否处于零速状态,如果行人处于零速状态,则由不同的约束算法分别得到行人的姿态角(横滚角、俯仰角、航向角)和速度虚拟观测量及误差,将误差输入到卡尔曼滤波器对行人的导航误差进行修正,得到更为准确的误差信息。算法流程如图2所示。

图2 算法流程Fig.2 Algorithm flow

2 算法模块实现

2.1 零速检测算法

零速检测算法用于检测行人的零速状态,在算法中具有很重要的作用。零速检测的准确程度直接关系到INS对导航误差的修正,本文中采用加速度模值、角速度模值和加速度移动方差三个方法[12-13]共同检测确定行人的零速区间。假设k时刻三个方法得到的检测结果分别为T1,T2和T3,则最终的检测结果如式(1)所示

零速检测算法输出的结果为0和1的运动静止序列,考虑到行人行走时运动和静止状态在时间上是连续的,并且持续一段时间。但是由于实际运动过程中存在传感器抖动等外部因素的影响,算法得到的零速区间并不连续。本文对于零速检测结果做二进制均值滤波处理,消除类似图3 中186～188 s处的伪静止区间。

图3 二进制均值滤波前后的零速检测结果对比Fig.3 Comparison of zero-speed detection results before and after binary mean filtering

从图3中可以看出,经过处理后,算法得到的结果在时间上是连续的,符合行人运动的规律。

2.2 速度修正算法和陀螺仪误差修正

行人处于零速状态时,只受重力加速度的影响,相应的速度和角速度理论值应该为零,但是测量得到的速度和角速度的实际值并不为零。此时将速度误差和陀螺仪零偏误差作为卡尔曼滤波的观测量,即ZUPT 和ZARU 算法。误差计算如式(2)和式(3)所示

2.3 俯仰角和横滚角的误差修正

假设加速度计的采样为(abx,aby,abz),则俯仰角和横滚角的计算如式(4)和式(5)所示

当行人处于零速状态时,只受到重力加速度的影响,此时导航坐标系下的加速度数据为[0,0,g]T,假设为载体坐标系到导航坐标系的方向余弦阵,此时的加速度表示如式(6)所示

根据零速状态的横滚角和俯仰角信息对姿态误差进行修正,将此时计算得到的横滚角和俯仰角与捷联惯导更新的估计值作差,输入到卡尔曼滤波器对误差进行修正,观测量的计算如式(7)和式(8)所示

2.4 改进的稳定状态磁场检验算法

磁力计可以测量得到环境中的磁场信息,但是地磁场非常微弱,很容易受到外界的干扰,因此使用磁力计解算航向角之前必须进行校准。本文采取椭球拟合[14-15]的方法对原始磁场数据进行补偿。

2.4.1 地磁场与航向角

地磁场的分布随时间和空间的变化非常缓慢[16],在一个小范围区域内,地磁偏角、磁倾角和磁场强度可以视为常值。地磁场的模型如图4所示。

图4 地球磁场与航向角模型图Fig.4 Model diagram of the Earth's magnetic field and heading angle

其中B表示地磁场;BH为水平分量;Bx,By,Bz为地磁场在北东地三个方向上的投影。磁场幅值和地磁倾角ψI的计算如式(9)和式(10)所示

航向角的计算如式(11)所示,其中磁偏角ψD在一定区域内是不变的。

2.4.2 改进的稳定状态磁场检验算法

在实际环境中,地磁信息很容易受到干扰,如何在凌乱的磁场信息中提取可用信息成为融合磁力计的关键。由于行人运动是小范围的低速运动,地磁场的磁场强度和磁倾角都是近似不变的。因此,本文设计了一种基于磁场强度、地磁倾角、IMU与磁力计解算的航向角之差的广义似然比检验算法,对稳定状态磁场的信息进行获取。

1)检验量的获取

a)根据式(9)可以计算得到磁场强度。

b)传统的磁倾角的求取如式(10)所示,但是该求取过程需要使用方向余弦阵Cnb,k/k-1将载体坐标系下的数据转换到导航坐标系下,而方向余弦阵由估算的欧拉角得到,使用估算的数据会引入额外的误差。进一步研究发现在静止状态时,磁倾角的大小与加速度计和磁力计数据的乘积有关,相关证明如下:

当行人静止时,载体坐标系下的加速度计和磁力计的数据分别为ab和mb,假设导航坐标系下的加速度和磁场分别为an和mn,则两者的关系可以表示为

已知在一定的范围内,重力加速度g和磁场强度均可视为常值,因此结论得证。

c)当磁场处于稳定时,地磁信息得到的航向角和惯导更新得到的航向角理论上是相等的。因此使用两个航向角的差值作为检验量,完成对稳定状态磁场的检验。

2)基于广义似然比的稳定状态磁场检验算法

行人运动第k步的观测模型如式(13)所示。

其中,sk=分别表示相邻两步之间的磁倾角变化、磁场强度变化和当前时刻IMU 和磁力计得到的航向角的差值;rk为量测噪声。

可以将稳定状态磁场的检验转换为二元假设问题。

其中,H1表示稳定状态磁场;Ωn= {ζ∈w:n≤ζ≤n+w-1},则概率密度函数的表示如式(15)所示

构建广义似然比检验量如式(16)所示

最终得到的广义似然比检验结果如式(17)所示

其中,w为滑动窗口长度;γ'Z为检验门限阈值;和为噪声方差。

如果式(17)成立,则磁场处于稳定状态,表示此时的地磁信息可以用来计算得到航向角信息,当行人处于无主方向进行约束时,可以根据地磁信息计算航向角并通过卡尔曼滤波器对行人航向进行修正。

2.5 自适应航向角漂移修正算法

通过提取磁场数据中的可用信息,可以对航向角漂移进行抑制。但是当环境干扰很大不存在稳定状态磁场时,使用磁力计的数据会带来额外的偏差,需要其他的方法来获取正确的航向信息。

传统的HDE 算法,需要先验知识预知建筑物以及环境的主方向,在陌生环境中无法工作。本文对HDE算法进行改进,对直行和转弯行走的检测严格限制,提出了一种实时建立主方向的a HDE 算法,通过相邻四步的步幅航向确定行人是否直行,直行的情况下实时建立主方向,根据建立的主方向库完成对后续航向角误差的修正,该方法不需要提前预知建筑物或者环境的主方向,可以根据行人的行走状态,实时建立主方向库,在后续直行时对航向角误差进行约束。

1)步幅航向

当IMU 固定于行人右脚时,定义右脚离开地面、接触地面到再次离开地面为一个复步,步幅航向为连续两步之间的角度[17]。定义零速点为零速区间的初始时刻,在零速点计算步幅航向。设第k个零速点的采样序列为s(k),此时东北方向位移分别为x(s(k))和y(s(k)),则第k步的步幅航向θs(k)如式(18)

则第k步的步长可以表示如式(19)

设定步长的阈值tth为0.3 m,若SSL(k)＜tth,则认为此时的零速点为伪零速点,应该舍去,进一步提高零速检测的准确度。

2)严格的直行转弯检测算法

使用a HDE 算法的前提是行人处于直行状态,因此正确检测出直行或者转弯是关键。行人行走可以分为直线行走和转弯两种,无论是小曲率半径行走还是大曲率半径行走都可以在有限步内完成。本文基于历史时刻的步幅航向提出了一种严格的直行和转弯检测方法。

假设此时为第k步,相邻四步的步幅航向为θs(j),j∈(k-3,k),当行人处于直行状态时,步幅航向应该保持一致,利用历史三步的步幅航向的差值进行判断。

如果Δφ＜tth1,则表示步幅航向变化不明显,行人处于直线行走或者沿大曲率半径曲线行走阶段。此时单纯利用步幅航向差值无法区分,分析发现,当行人沿直线行走时,步幅航向在一个常值附近波动,当行人沿曲线行走时,相邻几步的步幅航向有相同的变化趋势,因此进一步对直行和转弯进行限制。

首先,判断行人是否处于转弯状态。

其中,tth1为步幅航向角度检验的阈值。当T5=0时,表示行人处于转弯状态。定义相邻两步的步幅航向差值为Δθs,计算如下

当转弯时,相邻几步的步幅航向有相同的变化趋势,因此,做以下判断

设定直行标识符为SSLP(k),则有

只有以上条件同时满足时,认为行人处于直行状态。

3)a HDE算法流程

在实际的环境中存在着许多不规则路径,为了让HDE方法在这些路径上生效,本文对HDE 算法进行改进,改进后的方法可以实时建立主方向来约束航向角。a HDE算法流程如图5所示。

图5 aHDE算法流程图Fig.5 Flowchart of the aHDE algorithm

首先对行人的运动状态进行检测,当检测到行人当前状态为直行时,则遍历主方向数据库,如果主方向数量为0,直接建立新的主方向θnew,新的主方向θnew计算如式(25)所示

其中,k表示当前行人处于第k步,即文中所定义的第k个采样点;θs(j)表示第j步时行人的步幅航向;θnew表示新的主方向,由当前步幅航向和历史三步步幅航向计算得到。

如果主方向的数量不为0,则在数据库中搜索与当前步幅航向θs(k)距离最近的主方向θb1(k)。如果满足条件,使用主方向θb1(k)对航向角漂移进行约束。

如果不满足条件,则搜索与θnew最近的主方向θb2(k),满足条件时将θnew作为新的主方向添加入库。这样可以保证建立的主方向不会重复,否则如果在相同的路径上重复多次,解算得到的航向角略有差异,实时建立的主方向会有多条,增加匹配的难度。

直行状态下,航向角的观测量如式(26)所示

为了减少行人在非主方向上直行造成的误修正,使用自适应量测噪声,观测量δφ(k)的量测噪声标准差如式(27)所示

其中,σeHDE和β1的设置与传感器性能相关。当前时刻的步幅航向与主方向越接近,算法的修正作用越明显。

2.6 融合滤波更新

卡尔曼滤波的状态方程与量测方程分别为

其中,Xk为系统的状态变量,具体实现在前文已经提及;Φk/k-1为系统的状态转移矩阵;Qk-1为系统白噪声;Zk为观测量;Hk为量测矩阵;Wk表示传感器数据的噪声。具体的更新过程如文献[18]和[19]所示。

本文所涉及的量测值如式(30)和式(31)所示

则对应的量测矩阵的设置如式(32)所示。

其中,I3×3为三行三列的单位矩阵,03×3为三行三列的全零矩阵。

3 实验与结果

为了验证本文算法在不同环境中的有效性,分别设计室外实验和室内实验两部分,在学校内部采集行人运动信息,实验设计行人运动的起点位置和终点位置重合,便于对导航结果进行评价。室外实验行人携带GPS设备获取运动的参考轨迹,分别采用IEZ、IEZ+ZARU、IEZ+ZARU+QSF和本文的算法对实验人员的导航信息进行求取,从轨迹的重合程度、终点误差、终点定位误差和平均误差四个维度对本文算法的优越性进行分析;室内实验,由于GPS信号受到遮挡而无法使用,采用激光测距仪得到走廊两侧的墙壁信息,分析室内外磁场的差异,得知室内磁场由于叠加了建筑物的异常磁而不可用,因此室内实验本文算法结果仅与有速度修正的零速更新算法ZUPT 相比较,从轨迹是否穿墙、终点误差和终点定位误差三个维度进行比较,说明本文算法可以提高行人的定位精度。

实验所使用的传感器型号为MTI-300,采样频率为100 Hz,实验时传感器固定于实验人员右脚。传感器的参数如表1所示。

表1 MTI-300性能参数Tab.1 MTI-300 performance parameters

3.1 室外实验

实验地点为自动化学院篮球场,实验人员从起点出发,绕篮球场周围道路行走,终点位置与起点位置相同,使用GPS得到行人运动的参考轨迹。实验路径长约660 m,包含8条直行路径。参考轨迹如图6所示。

图6 室外复杂环境参考轨迹图Fig.6 Outdoor complex environment reference trajectory diagram

分别使用IEZ、IEZ+ZARU、IEZ+ZARU+QSF和本文的算法对实验人员的导航信息进行求取,得到的轨迹如图7所示。

图7 室外复杂环境实验结果图Fig.7 Experimental results of outdoor complex environment

不同方法得到的终点误差、终点定位误差和平均误差如表2所示。

表2 室外复杂路径下定位误差Tab.2 Positioning error in outdoor complex path

本文算法实时建立的8个主方向与理论上的8个主方向对比如表3所示。

表3 实时建立主方向与理论主方向对比Tab.3 Comparison of the main direction established in real time with the main direction of theory

根据表2和表3的数据可知,行人航向对行人定位误差的影响很大,提高行人航向的准确度可以有效提升行人定位精度。本文提出的算法在终点误差和定位误差方面优于其他的方法,可以对行人的航向角误差进行有效修正,得到的行人轨迹与参考轨迹重合度高,更加符合真实轨迹。在660 m 的实验中,本文算法得到终点误差为4.545 4 m,实验平均误差为1.747 3 m,相比较没有航向角修正的ZUPT,ZARU 算法和单独使用磁力计数据进行修正的QSF 算法,本文方法的定位误差分别降低了91.02%,82.93%和61.39%,并且可以实时建立的主方向完成对航向角的修正,定位精度符合行人定位的需求。

3.2 室内定位实验

实验地点为自动化学院院办楼,实验人员从起点出发,绕教学楼走廊行走两圈,终点位置与起点位置相同,使用激光测距仪得到建筑物的轮廓信息,实验路径长约320 m。传感器的安装及实验环境如图8所示。

图8 传感器安装及室内实验环境Fig.8 Sensor installation and indoor experimental environment

通过对室内磁力计采集得到的数据进行分析,发现室内磁场比较紊乱,实验中发现存在的稳定状态磁场数据较少。磁力计采集得到的室内数据和室外数据经过校正后的磁场强度对比如图9(a)和(b)所示。

图9 室内外实验磁场强度对比图Fig.9 Comparison of indoor and outdoor experimental magnetic field intensity

从图9可以看出,室外磁场在运动的过程中变化较小,基本保持在正常范围0.6～0.85 Gs,此时磁力计数据是可用的;对于室内实验得到的数据分析,初始时行人处于静止状态,磁场变化不明显,之后运动时由于室内的磁场叠加了建筑物的异常磁,在同一空间内分布不均匀,磁场强度变化剧烈,此时环境中的磁场数据几乎不可用。本文提出的算法主要使用a HDE 算法完成对航向角漂移误差的修正,所以仅使用零速修正方法和本文提出的算法分别对实验人员的轨迹进行获取,得到的结果如图10(a)和(b)所示。

图10 室内环境两种算法实验结果图Fig.10 Experimental results of two algorithms in indoor environment

从图10中可以看出,本文算法可以对航向角进行约束,相比于只对速度误差进行修正的ZUPT 方法,本文的算法结果将行人轨迹约束在两个墙面之间,减少了轨迹穿墙现象的发生。通过计算本文算法得到的终点误差为0.543 6 m,终点定位误差为总路程的0.17%,而ZUPT 方法的误差为1.533 4 m,本文算法与之相比定位误差降低了64.56%,定位精度符合行人室内定位的需求。

4 结论

本文基于IMU 和磁力计这两种传感器进行实验,提出了改进的稳定状态磁场检验方法和自适应航向角漂移修正算法,使用两种方法协同完成行人航向角的估计和修正。解决了行人定位无航向角修正和避免了单一使用磁力计由于磁场失真严重无法进行航向角修正的问题。通过实验得出以下结论:

1)本文根据行人运动过程中步幅航向的变化,将行人的行走分为直线行走和曲线行走两部分,分别采用改进的基于广义似然比的稳定状态磁场检验算法和自适应航向角漂移修正(a HDE)算法得到行人的航向信息,而且a HDE 方法不需要提前预知实验环境的主方向,相比较传统的HDE 算法,在陌生环境中也可以应用,应用局限更小。

2)室内外的多次实验结果说明,本文提出的算法提高了行人定位的精度,在室外和室内终点定位误差分别为4.545 4 m 和0.543 6 m,分别占总路径的0.69%和0.17%。并且本文的算法结果相比较没有航向角修正的ZUPT,ZARU 算法和单独使用磁力计数据进行修正的QSF算法,定位误差分别降低了91.02%,82.93%和61.39%。