重力辅助和模拟零速修正的手机航向修正方法

曾庆化，曾世杰，刘建业，陈锐志，孟 骞，王敬贤

（1. 南京航空航天大学 导航研究中心，南京 210016；2. 卫星通信与导航协同创新中心，南京 210016；3. 武汉大学测绘遥感信息工程国家重点实验室，武汉 430079）

基于位置的服务（Location Based Service, LBS）可以根据人们不同需求，在任何时间、地点，确定用户实际地理位置，为用户提供与位置相关的信息与服务[1-2]。随着智能手机的普及和功能的不断完善，基于手机的行人导航定位技术成为导航领域中的一个重要的新兴分支，可实时确定并监测行人的位置与人体状态，从而有效提高抢险搜救人员、特定功能行动的快速反应能力与任务执行效率，具有广阔的应用前景。行人手机导航已成为当前导航领域研究热点之一，国内外各高校、研究所都在此领域进行了深入的研究。2017年，美国商务部国家标准技术研究院（National Institute of Standards and Technology, NIST）举办了名为“PerfLoc”的大奖赛，为参赛者提供NIST的智能手机数据信息，便于参赛者挖掘智能手机在室内定位导航应用中的潜力。

室外环境下的行人导航研究以卫星导航或者是GNSS/INS组合导航为主，近些年取得了较好的成果[3]。而室内环境下导航研究，针对行人导航技术的研究主要集中于以下两类：一类是基站式定位，主要基于室内UWB、ZigBee、无线网络、WiFi以及蓝牙等无线射频设备[4]；一类是惯性为主的传感器定位方式，主要基于人体脚部，腿部、腰部、肩部等部位安装惯性传感器[5]，通过观测上述部位的运动特征，研究零速修正（Zero Velocity Update, ZUPT）、步态识别等方法，实现基于惯性传感器的行人导航。上述的两类方法各有优劣，前者需要事先在室内布置诸如WiFi路由器、信标基站等作为导航信息源，增加了成本，适用环境受局限，后者需要在人体上添置额外的传感器设备，其性能精度受传感器误差影响较大。

除了单独使用上述两种导航定位方法外，组合导航也是室内行人导航领域中常用的方式，如INS/UWB、INS/WiFi、INS/磁场匹配（Magnetic Matching, MM）[6-8]等。德国航空航天中心通信与导航研究所的研究人员提出了一种判断步伐、姿态和步长的方法，优化了行人惯性导航的结果[9]；西班牙自动化与机器人中心的研究人员提出了使用磁力计、陀螺仪和加速度计配合扩展卡尔曼滤波的方式为行人航迹推算（Pedestrian Dead Reckoning, PDR）算法获得更精确的航向信息[10]。上述的组合导航算法需要惯性传感器以外的传感器配合，且在陌生环境和建筑中，其导航效果会大打折扣。

近年来基于智能手机的行人导航定位技术发展迅速。南京航空航天大学的导航研究中心的研究人员基于智能手机设计并实现了室内外的无缝导航系统[11]；爱丁堡大学的研究人员提出使用手机作为导航设备和WiFi指纹识别系统相结合的组合导航算法HiMLoc[12]；德国卡尔斯鲁厄理工学院的研究人员提出混合定位概念，使用手机和UWB设备搭建了组合导航平台[13]。在使用手机进行纯惯性导航定位的研究中，法国GEOLOC实验室的研究人员设计了一种杂交滤波器用于标定通常PDR方法中的行人步长[14]。韩国高等科学技术研究所的研究人员提出并搭建了一种基于手机的SmartPDR导航系统，能利用手机完成PDR导航解算，但是该系统要求手机的使用方式保持固定，不能有太大的移动幅度[15]，以减少不同手机手持方式对导航定位解算的影响。

将手机保持在一个固定的握持姿态，不仅给行人增加了使用困难，也限制了手机导航的适用性，降低了用户体验，因此迫切需要一种能自动判别手机姿态，并根据姿态变化自动修正航向的算法，同时需要提高航向补偿性能，以提高纯惯性手机导航的适用性和定位精度。为此，提出了一种新的航向补偿算法，通过对MEMS惯性器件的输出信息采集分析，判断手机的使用方式和当前姿态，并对行人使用手机导航时的航向误差进行补偿。

1 行人航迹推算算法原理

自主性更强的惯性定位技术在无GNSS信号的区域具有巨大的发展潜力。传统的捷联惯导系统（Strapdown Inertial Navigation System, SINS）通过对加速度计和陀螺仪输出进行分析处理，得到导航坐标系下的位置、速度和航向三维信息。但由于手机内 MEMS惯性器件精度低、稳定性差，其航向和定位结果容易迅速发散。而传统行人PDR导航方法是通过在某一时刻对行人行走的步数、步长和方向进行测量和统计，推算出下一时刻行人行走轨迹和位置等信息，所以比SINS具有更高的定位精度，一般应用于低成本下的导航定位研究。

传统PDR包含三个重要内容[16]：1）步伐检测；2）步长估计；3）航向估计。

式(1)为PDR算法的递推公式，E和N分别为东向和北向位置的坐标，Sk、qk分别为tk-1到tk时刻行人从位置(Ek-1,Nk-1)到位置(Ek,Nk)的行走距离和方位角。

由式(1)可得行人在tk时刻东向和北向位置信息。

2 模拟零速修正算法

PDR方法的整体航位推算结果受低精度手机陀螺仪影响较大。足部或者车辆载体中应用的 ZUPT算法是一种简便、高效的惯性导航误差补偿方法，可有效减缓惯性导航误差积累，提高惯性导航的定位精度[17]，已经在车载导航和行人足部导航中获得应用。然而，在手机行人导航中，当行人手持手机时，手机一般位于行人腰部以上的位置，不包含明显的零速时刻，很难将传统的ZUPT算法应用进来。因此，本文提出了一种在行人脚部落地时刻进行卡尔曼滤波的模拟零速修正（Simulated Zero-Velocity Update,S-ZUPT）方法。

为了提高步伐检测算法的准确性，对三轴加速度计输出的信号进行如下预处理：

其中，A为加速度峰值阈值，tmin和tmax为时间间隔的区间值。行人的行走频率一般在2 Hz到4 Hz之间，因此选择的时间间隔为

对步伐检测算法进行多次实验结果如表1所示，其中，单次实验的准确率基本能达到98%以上，平均准确率为98.975%。

图1所示为编号为1的实验结果图，图中的三角形表示探测出的峰值点，一个三角形代表一步。

本文提出的S-ZUPT算法利用人体运动特点构建步伐观测模型，采用卡尔曼滤波器估计姿态和传感器误差，达到抑制航向角度误差发散的目的。设置滤波系统的系统方程和观测方程为：

表1 步伐检测实验结果Tab.1 Experiment results of step detection

图1 步伐探测结果图Fig.1 Step detection results

式中，F(t)为系统状态矩阵，H(t)为系统观测矩阵，G(t)为系统噪音驱动矩阵，w(t)为系统噪音，v(t)为观测噪音。通过解算可得离散情况下k+1时刻的系统状态估计方程和系统观测方程。

系统状态估计方程如下：

其中，时刻k为步伐检测算法判断出的步伐时刻，分别为该时刻下的横滚角、俯仰角和航向角以及手机陀螺仪三个轴输出的角速度值。

将行人航位推算系统获得的水平姿态角度和航向角度以及实时陀螺输出角速度值作为系统的观测量可以获得k+1时的观测方程如下：

受限于行人行走的特点，航向角度输出周期具有非一致性，在分析了行人步伐的周期特点基础上，选择了时变离散的航向角输出周期，即：

经由S-ZUPT算法滤波后的姿态角信息和角速度信息即为每个步伐时刻的最优估计值。

3 基于重力的手机姿态判断和航向补偿方法

在日常生活的应用中，手机常处于多种状态，一般可以分为竖屏摆放、横屏摆放以及通话模式等，如图2所示。本文将手机使用的方式大致分为三种方式：竖屏姿态、横屏姿态、通话姿态。

图2 手机持握方式示意图Fig.2 Ways to use smartphone

当使用手机的姿态从图2提及的姿态中相互切换时，手机都会有较大的位置和姿态变动，并且手机内部传感器一并记录了输出姿态变换过程中的加速度、角速度等信息。假如姿态切换过程中的角速度进入了导航系统，这对依赖MEMS惯性器件的手机导航而言无疑是有害的，会误导导航系统认为是行人的航向发生了变化，从而给出错误的导航定位结果。在手机行人导航时，仅使用S-ZUPT算法依然无法补偿由于手机使用方式改变造成的姿态角变化，因此本文根据行人对手机使用方式以及传感器输出数据分析，在实现S-ZUPT算法的基础上，提出了一种重力辅助航向补偿（Heading Compensation with Gravity Assisted,HCGA）方法对手机的航向进行补偿。

HCGA方法分为两个部分，分别为重力球区域姿态判断法和航向补偿过程。

手机重力计探测到的重力数据具有以下特点：

式中，为重力加速度合矢量，分别为手机重力计输出的各轴重力加速度矢量。

若将采集到的重力计数据投影到三维坐标系中并且建立一个半径大小为当地重力加速度的球面，便得到了如图3所示的球面点云图。图中的I、II、III区域为分别竖屏姿态、横屏姿态、通话姿态下重力数据在三维坐标系中的投影图。从图中能够发现不同姿态下手机重力计输出数据在球面上的投影区域有明显的区别，即通过计算重力计数据在球面坐标系下的方位角和仰角便可区分手机当前的姿态。

通过公式(10)得到三轴重力数据在球面坐标系下的方位角和仰角：

图3 球面点云图Fig.3 Spherical point cloud

为了方便计算，本文中实验以右手使用手机进行实验。在实际使用时，由于坐标轴的对称性，使用左手进行实验时只需将判断的区域进行对称变换即可。

式(11)展示了不同姿态下方位角和仰角的范围，其中，J为姿态标识，1、2、3分别表示竖屏姿态、横屏姿态、通话姿态三种姿态。

基于对行人使用手机方式同手机姿态之间关系的研究，本文提出了基于手机姿态对航向进行补偿的方法。本方法具体过程分为以下4步：

1）获取手机MEMS器件的数据，并判断步伐时刻；

2）实时监控两次步伐之间航向的改变值；

3）根据航向改变值大小决定是否进行前后两次步伐的手机姿态判断；

4）对比前后两次的手机姿态，根据对比结果对航向进行修正。

4 实验与分析

为了对本文提出的基于重力辅助和模拟零速修正的手机无约束航向修正方法进行验证，在南京航空航天大学自动化学院楼群进行了室内行走实验。本次实验所用设备为ONEPLUS A3000手机，设置采集传感器信号的频率为 50 Hz，携带方式为右手手持，竖屏模式下的手机头部朝向行人行走方向。

实验在自动化学院一号楼5楼走廊进行。行走矩形路线45 m´53 m，在行走时依次使用了图2中的三种手机使用模式，并采用传统PDR算法、HCGA算法、S-ZUPT算法和S-ZUPT+HCGA算法对采集数据进行解算。其中，HCGA算法对于手机姿态的判断结果如图4所示，HCGA算法成功判断出了三次姿态变化的时刻，并且姿态判断的准确率为100%。

图4 手机使用模式判断图Fig.4 Judgment of smartphone using-mode

图5所示为行走轨迹复现图，其中，绿色实线代表实际的行走轨迹，蓝色下三角和红色上三角分别代表为未添加HCGA的PDR算法和S-ZUPT算法解算结果，蓝色点线和红色虚线代表PDR+HCGA和S-ZUPT+HCGA算法解算结果。

从图5可以看出，未添加HCGA的解算结果在第一次改变手机姿态后，行走轨迹就出现了较大的偏差，航向偏移接近 60°。在三次姿态改变后，行走轨迹已经完全偏移实际行走路线。对辅助了HCGA算法的两组结果而言，行走轨迹基本贴近实际路线。说明本文提出的HCGA算法能够有效判断手机姿态变化，并能对由于手机姿态改变而造成的航向变化进行补偿。

图5 轨迹对比图Fig.5 Walking experiments comparison

此外，从图5中第一次改变姿态之前的路线结果就能明显看出S-ZUPT算法在对误差抑制的效果上要优于传统PDR算法。由图5可知，在同样辅助了HCGA算法的情况下，S-ZUPT算法最后的定位结果为（−1.674, 1.543），与起始点之间的距离为2.28 m，对于196 m的行走距离而言误差为1.2%。相较于PDR算法的（−8.161, 3.047）最后定位结果，精度提高了74%。且从整体的复现图中，S-ZUPT+HCGA算法的导航路线图和实际行走路线图更为贴近，说明本文设计的S-ZUPT+HCGA算法在仅使用 MEMS器件采集到的导航传感器信息时，能有效提高导航精度，抑制惯性导航的误差发散，并且能对手机姿态进行判断，补偿因手机姿态改变造成的航向误差。

图6所示为使用四种算法得到的航向值曲线。其中，灰色点虚线代表行走过程中实验人员转弯时刻，而淡蓝色虚线代表手机姿态发生变化的时刻。从图6可以明显看出：在行人行走转弯时，HCGA算法不影响航向的变化，但当手机姿态发生变化时，未使用HCGA辅助的算法会认为是行人行走进行了转弯，从而输出错误的航向；而辅助HCGA算法后，系统通过对使用姿态的判断进行了航向的补偿，保持了航向，且S-ZUPT算法得到的航向结果与传统PDR算法的航向结果相比，对误差的抑制效果更明显。

5 总 结

用手机进行行人导航时，手机的姿态由于正常使用而发生改变，传统导航算法易受姿态改变的影响，给出错误的导航结果。本文采用基于模拟零速修正的PDR系统进行导航，以卡尔曼滤波算法对陀螺仪输出的角速度进行滤波处理，以抑制惯导误差随时间发散。同时，在每次探测到步伐时，对手机姿态进行判断，并根据判断的结果进行航向的补偿。

从实验结果中可以看出，本文提出的算法在200 m的矩形行走实验中，定位误差为 1.63%，能够有效减少误差积累，抑制航向发散，提高导航精度，使得导航复现路线更加贴合真实行走轨迹；结合HCGA算法后能够动态补偿由于手机姿态改变导致的航向误差，提高了手机导航的精度。

目前本文专门针对三种手机使用模式进行判断，且采用经验阈值分析方法获得统计数据。后继将引入机器学习算法，增加相关的判断模型，提高手机使用模式的分析效率，增强用户体验。