基于零加速修正的室内惯性定位算法

苏志刚，赵世伟，郝敬堂

(1.中国民航大学 智能信号与图像处理天津市重点实验室，天津 300300；2.中国民航大学 中欧航空工程师学院，天津 300300)

0 引 言

借助GPS定位功能，移动互联网可以提供基于地理定位的信息或娱乐服务，即基于位置服务(location based services，LBS)[1]，其被广泛应用于各种场景中。在无GPS信号的环境中，如室内、洞穴等，GPS信号受遮蔽影响，无法提供相应的位置服务[2]。因此，需要不依赖于GPS定位功能的定位技术。

近年来，随着智能手机功能逐渐完善，智能手机内置的微传感器使得不依赖于GPS的室内定位成为可能。基于WIFI定位技术利用提前部署的WIFI热点进行定位[3,4]，此技术过分依赖热点的分布情况且存有盲区。基于蓝牙定位技术同样需要在室内部署网关，并且传播距离有限、实时性和稳定性较差[5]。利用惯性传感器进行定位的技术主要有行人航位推算(pedestrian dead reckoning，PDR)技术和捷联惯性导航(strapdown inertial navigation，SIN)技术两类[6]。如何确定行人的步长是PDR技术的核心问题。学者通过分析行人步行特点，给出不同的步长估计方案[7-9]。PDR技术精度通常为0.3%-10%，由于PDR技术过于依赖步长估计，具有较大的局限性。SIN技术则是基于惯性数据信号积分的方法，其难点在于如何有效地消除累积误差[10]。Wang Yusheng等通过分析行人步态特点，将脚触地时的零速信息作为卡尔曼滤波的状态量补偿惯性导航的偏差来解决定位系统中累积误差问题[11]；Zeng Shijie等没有增加额外的传感器，提出利用智能手机的加速度峰值确定步数，并对每步的状态进行卡尔曼滤波，从而抑制累积误差引起的定位误差发散[12]；R.Kannan等提出一种基于训练的自适应阈值校准算法，该算法利用自适应阈值校准来纠正惯性传感器的错误从而达到消除误差的目的[13]；Chen Zhenghua等在利用智能手机惯性传感器进行人员定位的基础上融合iBeacons距离测量来校正累积误差[14]。

本文在未增加额外器件的前提下，通过对实时采集到的数据信号进行零加速检测，并直接在零加速状态对定位系统进行状态估计与修正，从而在不增加其它额外算法的基础上完成人员室内定位。室内外实验结果表明该算法可以有效地解决无GPS信号区域的人员定位问题，并且在实用的基础上具有较低的实现复杂度。

1 算法框架

利用智能手机内置的惯性传感器进行定位时，须考虑如图1所示的3个坐标系。以起始点为参考点的东北天(ENU)坐标系定义的地理坐标系(n系)，以手机中心为参考点建立平行于手机宽边、长边及垂直手机面的右手螺旋坐标系ObXbYbZb作为载体坐标系(b系)，以手机中心为参考点，平行于地理坐标系各坐标轴建立惯性坐标系(i系)ObXiYiZi。 载体坐标系与惯性坐标系是共原点的，因此两者间只存在旋转操作，而惯性坐标系与地理坐标系对应轴之间是平行的，因此两者间存在着平移操作。载体坐标系与惯性坐标系间的旋转角度用于解算平台的姿态，惯性坐标系与地理坐标系之间的平移用于重构平台的轨迹。

图1 相关坐标系间关系

定位算法框架如图2所示，由数据采集、零加速检测、零加速姿态估计和位置解算4个模块构成[15]。利用智能手机的内置传感器，主要包括三轴加速度计、三轴陀螺仪和三轴磁力计等，实时地进行数据采集。由于各传感器内置于手机内，因此，各传感器输出的数据是载体坐标系形式。零加速检测模块是利用加速度及角速度是否低于设定门限来判定平台是否处于零加速状态。若检测出平台处于零加速状态，将通过零加速估计模块地对平台的姿态进行估计，并将估计出的姿态信息送达位置解算模块用于姿态修正。若零加速检测模块给出平台处于非零加速状态，则位置解算模块不进行平台姿态修正，只常规地进行位置解算。

图2 算法框架

2 零加速检测

零加速检测是对零速检测的拓展。根据行人步态特点，利用额外传感器获得脚触地的零速信息[11]。如果仅利用智能手机的内置传感器无法有效获得平台的零速信息。然而，在人行走过程中存在着动平衡状态，平动加速度与转动加速度均为零，即零加速状态，此时平台处于相对稳定的状态，可通过对平台的姿态估计实现对累积误差的修正。

智能手机内置的三轴加速度计测量的是平台的平动加速度与重力加速度的矢量合成。在零加速状态时，由于无平动加速度，三轴加速度计反映的是重力加速度。同时，平台不发生转动，使得三轴角速度计的输出应为零。因此，平台零加速状态可利用惯性传感器输出的三轴加速度和三轴角速度进行判决。

加速度模值检测(acceleration magnitude detector，AMD)是利用加速度进行门限检测

(1)

角速度模值检测(angular rate magnitude detector，ARMD) 是利用三轴陀螺仪输出的角速度进行检测

(2)

如前所述，零加速状态时，平动加速度和转动角速度均为零，所以零加速检测可表述为

ZAD(t)=D1(t)×D2(t)

(3)

根据实验数据分别加速度模值、角速度模值和零加速检测，结果如图3所示。图3中曲线取1时表示有效检测状态。由图可见，每种检测方案均可以获得相应的有效检测结果，但ZAD的结果更能够准确地反映零加速状态，且检测出的有效状态的数量相对较少。

图3 零加速状态检测结果

3 零加速姿态估计

(4)

其中，I为三阶单位矩阵，Κ(t) 为该单位向量r(t) 的斜对称矩阵

(5)

(6)

向量gb(t) 和nb(t) 均是向量gi和ni绕单位向量r(t) 逆时针旋转θ(t) 角获得的，因此单位向量r(t) 在重力加速度矢量gb(t) 和gi的对称面上，该平面的法向量为

(7)

同理，磁分量矢量nb(t) 和ni的对称面包含向量r(t)。 相应平面的法向量可表示为

(8)

向量r(t) 同时处于式(7)和式(8)所确定的两平面上，即向量r(t) 位于两平面的交线上，该交线的方向向量为

rl(t)=m1(t)×m2(t)

(9)

由此可得方向向量rl(t) 在惯性坐标系各轴向的分量

rlxt=gyt(nzt-hz)-(gzt+g)(nyt-hy)

(10)

rlyt=(gzt+g)(nxt-hx)-gxt(nzt-hz)

(11)

rlzt=gxt(nyt-hy)-gyt(nxt-hx)

(12)

对方向向量rl(t) 进行归一化计算

(13)

由式(13)即可得分量rxt，ryt，rzt。

在零加速状态下，两坐标系的重力加速度向量间关系为

(14)

将式(4)、式(5)、式(6)代入式(14)整理得

-gxt/g=[1-cosθ(t)]rxtrzt+rytsinθ(t)

(15)

-gyt/g=[1-cosθ(t)]rytrzt-rxtsinθ(t)

(16)

(17)

由式(17)可得

(18)

将式(18)代入式(16)并整理可得

(19)

由式(18)和式(19)可获得角度θ(t) 的值。

4 位置解算

在平台运动过程中，三轴加速度计输出的载体坐标系下的加速度向量ab(t)需要转换为地理坐标系下的加速度向量an(t)。 由于地理坐标系与惯性坐标系下的加速度向量相等，即an(t)=ai(t)， 所以

(20)

显然，地理坐标系下的加速度an(t) 是平台的平动加速度，已经消除了重力加速度的影响。

根据平动加速度an(t)， 可将速度更新方程表示为

vn(t+dt)=vn(t)+an(t)×dt

(21)

式中：vn(t) 和vn(t+dt) 分别为速度的当前值与更新值。当零加速状态时，式(21)可转化为

vn(t+dt)=vn(t)

(22)

由载体坐标系到惯性坐标系的姿态转换矩阵的更新为

(23)

式中：I为单位矩阵，Β(t) 为斜对称矩阵，用于定义转动方向

(24)

其中，dt为采样时间间隔，ωxt、ωyt和ωzt分别为时刻t的3个轴向角速度计分量。在零加速状态时，Β(t) 为3×3的零矩阵，即，此时式(23)简化为

(25)

位置向量的更新方程为

pn(t+dt)=pn(t)+vn(t)×dt

(26)

其中，pn(t) 为当前时刻的位置向量，pn(t+dt) 为位置向量的更新值。

5 时间及空间复杂度分析

本文算法涉及的时间消耗主要发生于零加速状态检测、姿态矩阵估计和位置解算3个方面。对于实验中获得的m个样本所能够检测到的零加速状态有w个。显然，w≤m。 当w

T1=(1+w)α+(β+γ)m

(27)

式中：α为估计姿态矩阵所消耗的时间，β和γ分别为单次零加速状态检测和位置解算所消耗的时间。由于初始化时也需要对姿态矩阵进行估计，因此姿态矩阵的估计次数为 (1+w)。

将w=m代入式(27)获得算法的时间复杂度峰值

T2=α+(α+β+γ)m

(28)

关于算法的空间复杂度也可用相同的方式进行分析。算法的空间占用主要涉及原始数据及中间变量的存贮，主要有姿态矩阵、零加速状态、速度和位置等，因此，相应的空间复杂度为

S=μ+(μ+ν+τ+ρ)m

(29)

式中：μ为姿态矩阵消耗的空间，ν、τ和ρ分别为单次零加速检测、速度和位置瞬时值所消耗的空间。

文献[12]与文献[14]均是利用智能手机的内置传感器进行定位的方法，因此，本文算法与上述两种算法的复杂度比较见表1。

表1 算法复杂度对比

文献[12]中提出的算法在行人运动过程中首先需要对行人的零速状态进行检测，当检测到零速状态时，将此时的传感器输出作为状态量代入卡尔曼滤波算法中来缓解累积误差，提高导航精度；而本文提出的算法在检测行人零加速状态的基础上，只需要重新估计此状态下的平台姿态，无需辅助其它额外算法即可实现累计误差的消除。由表1可见，本文算法在时间复杂度和空间复杂度上明显优于文献[12]算法。表1还显示，文献[14]算法略优于本文算法。文献[14]算法无需进行零速状态检测，但它需要提前部署的ibeacons网关实现对行人位置距离校准。通常部署的ibeacons网关较少，因此校准的次数较少，消耗的时间和空间资源相对而言就比较少。然而，文献[14]算法增大了对环境要求，提高了实现成本。

6 实验结果分析

实验平台采用iPhone 6 plus智能手机，其内置的三轴加速度计和三轴角速度计是Invensense公司的MPU-6700集成的，三轴磁力计采用的是AKM半导体公司的AK8963C。利用手机端Matlab通过IP与电脑端Matlab互联，实现传感器数据的采集与处理。数据采样率设置为100 Hz。零加速检测中的加速度检测阈值为th1min=9.2和th1max=10.4， 角速度检测阈值为th2max=0.2。

为检验定位算法的有效性，首先进行户外定位实验，利用GPS定位数据检验本文算法的有效性。在户外手持智能手机进行了“Z”字形运动。将零加速修正的轨迹、未进行零加速修正的轨迹和GPS轨迹相比较，如图4所示。由图4可见，未进行零加速修正的轨迹很快就发散，导致定位远偏离实际位置。当采用了零加速修正，相应的轨迹能够较好地保持在真实轨迹附近。

图4 户外实验结果

对于室内实验，实验人员沿着室内固定轨迹行走，采用零加速修正与否的对比实验结果如图5所示。由图5可见，零加速修正方法可以有效地避免轨迹发散现象。

图5 室内实验结果

7 结束语

基于智能手机内置传感器进行惯性定位时，由于累积误差导致平台轨迹重现时存在轨迹发散问题。本文针为此提出一种基于零加速度检测与姿态修正的算法，利用检测出的零加速状态实现对平台姿态的重新估计与校正，克服惯性传感器累积误差持续增长问题，在未增加额外器件基础上，实现对人员的自主惯性定位。理论分析与实验结果表明该算法具有较低的时间与空间复杂度，且可以不借助GPS定位信息实现对人员位置的确定。