基于手机内置传感器的车辆组合定位方法

旷 俭，葛雯斐，张 全，窦 智，唐爱鹏，张小兵，牛小骥

(1. 武汉大学卫星导航定位技术研究中心，武汉430079；2. 高德软件技术有限公司，北京100020)

随着交通的发展和人们出行方式的改变，车辆定位成为一个热门的研究问题。与此同时，技术和设备的不断发展更新使得越来越多车载传感器进入市场。在很多自动驾驶或高精度车载定位系统中，会搭载多种车载传感器如GNSS 接收机、里程计[1]、相机[2]、激光雷达[3]等，利用这些传感器，车辆能够获取较高精度的位置，进一步感知环境并结合决策、规划模块进行车辆的控制[4]等。但是，上述设备因安装复杂、价格高昂、破坏车体外观等多种原因，在实际生活中应用并不是很普遍。对于大部分用户而言，在驾车过程中往往单独依靠车内用户自身的智能手机来完成车辆定位的工作。

随着智能手机技术的发展，智能手机内置的传感器种类越来越丰富，目前常见的智能手机传感器包含：GNSS 接收机、加速度计、陀螺仪、磁力计、温度传感器、蓝牙、WIFI 等等[5]。不过由于成本、体积等限制，目前手机内置传感器虽然种类丰富，但是各种传感器的精度都比较低，如何利用这些低精度传感器进行导航定位成为研究热点。目前很多利用智能手机进行定位的研究，多是应用在室内场景，文献[6]利用智能手机传感器进行人体行为识别；文献[7]利用手机磁力计等进行定位；文献[8][9]利用手机的WIFI 传感器进行室内定位。这些方法由于其自身的局限性，都无法在车载定位中应用。

在很多实际车载场景中，用户使用智能手机进行车辆定位往往会结合地图软件。在结合地图软件进行车辆定位的过程中，地图软件的定位算法较为依赖卫星定位结果，虽然会加入地图匹配、道路约束等[10]功能来提升用户的定位体验，但是在实际使用中，当车辆行驶到高架、隧道等信号遮挡场景时，定位软件常常会出现“卫星导航信号弱”的提示，无法为用户提供可靠的定位结果。同时，结合地图进行定位，定位结果依赖地图数据，没有很好地挖掘手机内置传感器进行车载组合定位的性能。因此，本文研究只依靠手机内置传感器而不使用其它辅助信息和设备进行车辆定位问题，挖掘低端传感器的性能，具有很大的研究价值和实用意义。

1 算法流程及关键技术

针对手机在车辆运动过程中易发生晃动、手机内置传感器精度低、GNSS 信号频繁中断等问题，本文以GNSS/INS 组合定位为基础算法，围绕挖掘车辆运动约束信息和传感器的观测信息，并利用数据后处理的优势，使用平滑算法进一步提升系统的定位能力。

图1 给出了基于智能手机内置传感器的车辆组合定位算法流程，主要包含四个部分：1）GNSS/INS 组合定位算法（图中红色虚线框部分），2）车辆运动模型约束（图中蓝色虚线框部分），3）磁航向约束（图中紫色部分），4）反向平滑算法。

图1 基于智能手机内置传感器的车辆组合定位算法流程Fig.1 Flowchart of the vehicular integrated navigation algorithm based on smart phone built-in sensors

1.1 GNSS/INS 组合定位算法

GNSS 能够全天候地提供高精度的定位服务。然而，因信号被遮挡、反射等原因，GNSS 定位对导航环境非常敏感。INS 不受导航环境的影响，但是其定位性能极度依赖于传感器精度，例如智能手机内置的MEMS IMU，其定位误差会在几秒内累计数米。GNSS/INS 组合定位能够发挥二者的优势，GNSS 位置能够有效地控制INS 的误差累积漂移；INS 能够填补两次GNSS 位置之间的动态信息，且在GNSS 定位不可用时，仍然能够短时间内提供高精定位服务。简单地说，GNSS/INS 组合定位是车辆定位场景下最基本且最骨干的算法。

A. 惯导机械编排

惯导机械编排是最经典的捷联惯导算法，基本原理为已知当前位置、速度和姿态的条件下，通过积分角速率和比力获得用户的当前位置、速度和姿态。由于手机内置的MEMS IMU 性能低（比如陀螺仪稳定性差和噪声大等），微小的误差改正项无法提升导航性能，所以本文的惯导机械编排算法中忽略地球自转角速度、车辆运动所造成的牵连角速度及速度划桨效应的影响。简化后惯导机械编排算法具体如下：

式(1)～(4)中，rn=［B L h］T为导航坐标系下（即n 系）的位置，包含纬度、经度和大地高程；vn=［vn ve vd］T为n 系下的速度，包含北向、东向和垂向；为传感器坐标系（即b 系）到n 系的方向余弦转换矩阵；Rm和Rn分别为地球子午圈和卯酉圈曲率半径；gn= ［ 0 0 -g］T为重力向量，其中为地球重力加速度；和bf分别为三轴加速度计观测值和加速度计零位偏置；和bg分别为三轴陀螺仪观测值和陀螺仪零位偏置；Δt k=t k-tk-1为第k- 1个和第k个观测历元的时间间隔，（·）× 为向量的反对称矩阵， I3为3×3 的单位矩阵。

B. 滤波设计

卡尔曼滤波是最常用的数据融合算法之一，通过融合不同种类的观测值，从而获得更准确地状态估计。为了降低非线性化带来的误差，本文采用扩展卡尔曼滤波，且滤波状态为导航误差状态。

滤波的系统状态方程可通过对式(1)进行微分获得，具体可参考文献[11]。另外，为了充分发挥车辆运动约束模型的性能，手机坐标系与车辆坐标系的角度差异（即安装角）以及传感器测量中心到车辆坐标系中心的杆臂也作为待估计参量。因此，20 维的误差状态向量总结如下：

式(5)中，δrn、δvn和φ分别为n 系下位置误差、速度误差和姿态误差，δbg、δba分别为陀螺零偏误差和加速度计零偏误差，δα为俯仰、航向安装角误差，δlb为IMU 测量中心到车辆坐标系(v 系)中心（即后轮中心）的杆臂误差。

导航误差状态及协方差矩阵预测更新方式如下：

式(6)～(8)中，δ x为误差状态向量，P为系统状态协方差矩阵，Q为系统噪声矩阵，系统状态转移矩阵Φ如下：

当观测值（这里具体包括GNSS 定位结果、来自车辆载体的速度信息和磁航向约束）可用时，则构建观测方程：

式(10)中，δ zk为预测值与观测值的差异（对应于导航误差状态向量），Hk为设计矩阵，n为观测值噪声。同时，采用基本卡尔曼滤波更新方法对导航误差状态量及协方差矩阵进行观测更新：

式(11)～(13)中，Kk为卡尔曼滤波增益矩阵；Hk为设计矩阵；Rk为观测信息先验方差矩阵，δzk为系统预测值与观测值的差异。最后，将估计的误差状态量反馈到导航状态中，并对误差状态量进行置零处理。

以下对本文方案中用到的每种观测修正进行具体描述。

C. GNSS 位置修正

GNSS 位置的主要作用是控制系统的绝对定位精度，同时估计传感器误差。对于智能手机终端，GNSS 天线与IMU 测量中心的距离较近（比如几个厘米），且单点定位模式下GNSS 位置精度较低（比如米级水平），可认为GNSS 天线与IMU 的测量中心重叠。因此，GNSS 位置观测方程可简化为：

式(14)中，和分别为n 系下惯导推算的位置和GNSS 位置，nr为位置量测误差。另外，由于GNSS 信号易受到外界环境的干扰，GNSS 位置的理论标准差并不能准确地反映其真实误差水平，所以为了维持滤波器的稳定，必须对GNSS 位置的可靠性进行检验[12]。本文采用单尾卡方检验的方法：

设检验统计量为

式(16)中，σ0为单尾标准差，检验拒绝域为α为给定置信区间(报警率)。需要说明的是，卡方检验方法只适用于存在少量GNSS 位置跳变的场景；当存在大量GNSS 位置粗差或者固定偏差时，需要使用更复杂的抗差和自适应滤波方法[13-14]。

1.2 车辆运动模型约束

在城市复杂环境中，GNSS 信号被遮挡数秒甚至于数分钟是非常普遍的现象，而依靠智能手机内置传感器将无法维持系统的定位精度。此时，充分挖掘车辆的运动信息，构建相应的约束观测，是一种能够有效地提高系统定位性能的典型做法。本文将车辆运动状态简单分为静止和运动，并使用IMU 原始数据构建判断函数对其进行区分[15]。

A. 静止状态

零速修正技术（Zero Velocity Update Technology，ZUPT）是控制速度误差累积的有效手段[16-17]。当检测到车辆处于静止状态时，可认为车辆的速度为零。此时构造零速度观测值，更新系统的导航状态。

式(17)中，为捷联惯导算法推算的速度，nv为速度量测噪声。

同时，在静止状态中，车辆的航向应该保持不变，所有航向误差都可以认为是由传感器误差引起的。本文通过存储静止时段的初始时刻航向角，构建虚拟的航向角观测值，从而达到有效控制航向角误差累积的目的，称为零航向角速率修正（Zero Integrated Heading Rate，ZIHR）[18]。航向角观测方程表述如下：

式(18)中，

为惯导机械编排推算的航向角，为存储的静止状态第一个历元对应的航向角，为方向余弦矩阵的第1 行第1 列元素，nφ为航向角的量测噪声。

B. 运动状态

对于普通用户而言，当检测到车辆在运动时，车辆只会向前或向后移动，极少情况下出现跳跃和侧向漂移现象。基于这样的客观事实，可假设车辆坐标系（即v 系）下的侧向和垂向速度一直为零。然而，车辆的前向速度仍然无法准确获得。本文采用有监督学习方式训练车辆前向速度预测模型[19]，车辆前向速度预测模型如图2 所示。该模型使用IMU原始数据，可以预测车辆的前向速度，误差可控制在0.5 m/s 以内。

图2 车辆前向速度预测模型结构示意图Fig.2 Architecture of the vehicle speed prediction model

由此，我们可以获得v 系下速度观测值:

式(19)中，vpredict为车辆前向速度预测模型输出值。相较于侧向和垂向速度为零的假设，获取前向速度的模型训练需要耗费较大的成本（比如训练数据采集和模型训练周期长等）。同时，由于智能手机是通过支架固定在车辆头部挡风玻璃处，对于不同用户场景手机的晃动程度差异较大，以至于训练好的模型不具有好的普适性，这是亟待解决的问题。

同时，惯导机械编排推算的速度在v 系下投影可表述为：

因此，对应的速度观测方程可表述为[20]：

实际车载应用过程中，由于智能手机存在随意拆卸和重新安装等现象，造成了安装角和杆臂参数不固定的问题。此时，传统的直接设定或预先标定方法不具备实施条件。而本文采用在线估计方式能够自动标定上述参数（如式(21)所示），从而使这一车辆运动约束算法具有更强的适用性。

1.3 磁场航向约束

车辆外壳主要由铁磁性物质组成，这严重限制了磁力计的使用。幸运的是，车辆外壳造成的磁干扰的主体可等效于磁力计零偏。若能够准确地标定和扣除磁力计零偏，基于磁力计观测值计算的航向角至少能够准确反映真实的航向角变化。因此，本文使用相对磁航向角变化量构建航向角观测值，具体的观测方程表述如下：

式(22)中，ψins为惯导机械编排推算的航向角，ψmag为磁航向角，k为历元编号，k0为判定为准静态磁场所对应的历元编号。准静态磁场判定方法和磁航向角计算方法可参考文献[21]。

1.4 反向平滑算法

实时定位阶段，卡尔曼滤波只能利用当前时刻以及历史时段的量测信息来估计当前状态。而对于非实时定位应用，事后数据处理不使用未来时间点的观测量优化当前状态就显得数据利用率不够充分。反向平滑算法的本质是利用了过去、当前以及未来时刻的所有测量数据来估计当前系统的状态，最终获得比卡尔曼滤波更高精度的状态估计。

本文采用RTS 平滑算法[22]，用于进一步提升定位性能。具体方法为：

式(23)～(25)中，k=N- 1,N- 2 …0，N为观测信息的总数，Ak为平滑增益矩阵。

2 实验数据处理与结果分析

2.1 实验数据

实验数据来源于室内定位与室内导航（IPIN2020）比赛的“基于智能手机的车辆定位”赛题[23]。该赛题共提供4 组数据，其中训练数据3 组，测试数据1 组。测试数据时长大约1 小时，测试路线分为静态初始校准阶段(约5 分钟)、开放环境阶段(约20 分钟)、受遮挡环境阶段(约25 分钟，GNSS 信号被周围的建筑物或树木衰减或遮挡，定位结果频繁中断)，以及无GNSS 信号阶段(约10 分钟)。智能手机型号为HUAWEI mate 20，通过支架安装在车辆前端，安装位置和方式如图3 所示。数据采集包括陀螺仪、加速度计、磁力计、GNSS（单点定位模式）等传感器。

图3 智能手机的安装位置和安装方式Fig.3 Installation location and orientation of smartphone

同时，位置参考结果由DGNSS/FOG-INS 系统提供，输出频率为1 Hz，位置精度约20 cm。

2.2 初始化设置

本文采用动对准的方式对系统状态进行初始化设置，包括位置、速度和姿态。当GNSS 位置满足以下两个条件：1）相邻两个GNSS 位置的质量高（根据GNSS 芯片输出的位置标准差判断可靠性），2）相邻两个GNSS 位置时间间隔为1 s，且距离大于5 m，则对系统状态进行初始化赋值，具体如下：

式(26)中，Bgnss,k、Lgnss,k和hgnss,k分别为纬度、经度和大地高，vnorth,k、veast,k和vdown,k分别为北方向、东方向和垂直指向地心方向速度，φ、θ和ψ分别为横滚、俯仰和航向角；为加速度计三轴观测值，k-1 和k为历元编号。

本文采用一阶高斯-马尔科夫过程描述陀螺仪零偏和加速度计零偏的变化。其中，对应的IMU 性能参数（陀螺零偏标准差、角度随机游走、加速度计零偏标准差、速度随机游走）使用3 组训练数据确定。基本原则是设置不同长度的GNSS 信号中断，调整IMU 性能参数，使GNSS 中断时段中INS 的位置漂移误差最小。

2.3 数据处理结果

为叙述方便，本文以GPS 周内秒时刻19800 s 作为起始时刻（即时间 0 点），同时以北向位置4437147 m 为北向零位置。GNSS 定位的质量和中断情况如图4 所示。在车辆运动过程中，手机GNSS 定位结果误差大部分在10 m 以内，但是GNSS 定位结果并不连续。在1900 s 到3900 s 之内，车辆共经历19 次GNSS 信号中断，其中1 分钟以上中断有8 次；最长的一次中断发生在2900 s-3500 s 之间，手机在2931 s 后丧失 GNSS 信号，共持续 427 s；在3358 s-3364 s 之间，共获取到6 个历元的GPS 信号，后续又经历持续95 s 的GPS 信号丢失。需要注意的是，尽管GNSS 信号存在频繁中断的现象，但是信号中断时段的初始和结束时刻不存在明显的位置跳变现象（即不存在GNSS 粗差），因此相对于真实的城市复杂场景，该测试数据仍然是比较干净的。

图4 GNSS 定位质量和中断情况Fig.4 GNSS positioning quality and signal outages

图5 测试期间车辆运动轨迹Fig.5 Vehicle trajectory in the test

测试数据对应的车辆运动轨迹如图5 所示。图中黄色点表示GNSS 定位结果，蓝色表示参考轨迹；子图（即左下角放大部分）为一个封闭机场。图中红色标识为最长的1 次GNSS 信号中断发生时车辆的运动轨迹，在这段接近9 分钟的时间内，车辆进入停车场内并在其中往返多次，这期间手机仅在停车场入口处接收到6 s 的GNSS 信号，其余时间GNSS 定位失效。

本文共评估了四种定位方案：(a)GNSS/INS 组合定位；(b)在方案(a)的基础上，发挥离线数据处理模式的优势，使用RTS 平滑算法提升定位精度；(c)在方案(a)的基础上，使用本文设计的观测修正信息，包括ZUPT、ZIRH、前向速度预测模型、车辆运动约束以及磁航向角约束；(d)在方案(c)的基础上，使用RTS 算法进一步提升定位精度。

图6 给出了四种定位方案的解算结果，红色线为参考轨迹，蓝色线为定位方案解算结果。子图(a)～(d)分别对应四种定位方案。从轨迹形状来看，从方案(a)到方案(d),定位性能依次变好。图7 给出了四种定位方案的位置误差，蓝色、红色和黄色线对应北方向、东方向和高程方向的位置误差。可以看出，系统的绝对定位精度由GNSS 位置决定，而四种方案的差异主要体现在GNSS 信号中断时系统推算的定位性能差异。结合图6 和图7，可以得到以下结论：

对于方案(a)而言，当GNSS 信号处于中断状态时，退化到纯INS 的位置推算，此时系统的定位精度完全依赖于传感器的机械性能，所以位置误差发散快速，以至于最大位置误差达到了几千米。

相较于方案(a)，方案(b)将位置误差降低到140 m以内，定位性能提升大于10倍。原因是平滑算法能够使用未来的观测信息来修正当前导航状态，有助于更加充分地利用观测数据。因此，在非实时定位应用场景，平滑算法应该作为基本算法配置，用于进一步提升定位性能。

方案(c)达到了比方案(b)更好的性能，最大位置误差小于90 米，性能提升接近1 倍。表明平滑滤波的性能提升是有上限的，即使在未完全发挥观测值作用的情况下，观测信息的增加能够更有效地提升系统定位性能。另外，方案(c)是一种实时定位算法结构，其定位性能也能一定程度地反映基于智能手机内置传感器车辆组合定位方案的实时定位性能。

在方案(c)的基础上，方案(d)的定位性能得到了进一步的提升，达到了定位误差小于40 米。此时，位置误差具体表现为：估计的航向逐渐偏离真实航向以及估计的往返距离比真实往返距离要短。造成该现象的原因是基于磁力计的相对航向约束仍然无法限制航向漂移，车辆前向速度预测模型不够准确等。

图8 给出了四种定位方案的平面位置误差累积密度函数。

图6 四种定位方案的解算结果Fig.6 Results of the four positioning schemes

图7 四种定位方案的位置误差Fig.7 Position error of the four positioning schemes

图8 四种定位方案的平面位置误差累积密度函数Fig.8 The plane position error CDF of the four positioning schemes

表1 统计了四种定位方案的平面位置误差的75%、95%和均方根。四种定位方案的平面位置误差的75%分别为131.52 m、24.21 m、16.76 m、7.09 m，均方根分别为669.86 m、42.90 m、17.44 m、11.34 m。在第七届IPIN2020 室内定位比赛中，以定位误差的75%概率值为评价指标，因此赛事举办方宣布本文方案的定位精度为7.09 m。然而，相较于均方根，75%的概率偏乐观，没有兼顾在最恶劣的定位场景，该指标很难全面反映系统的整体定位性能。本文以RMS作为统计指标，可以看到本文方案能够提供误差为11.34 m 的定位服务。

表1 四种定位方案的平面位置误差均方根、75%和95%（单位：米）Tab.1 RMS, 75% and 95% of the plane position errors of the four positioning schemes (Unit: m)

3 结 论

针对基于智能手机的车辆定位场景的典型问题，包括车辆运动过程中手机易发生晃动、手机内置惯性传感器精度低、GNSS 信号频繁中断等情况，本文设计了一种基于智能手机内置传感器的多源融合定位方案。

利用简化的捷联惯导算法处理IMU 数据，获取由纯惯导推算的导航状态；采用误差状态卡尔曼滤波算法进行多种传感器修正信息的融合，并且在状态量中加入安装角、杆臂等增广误差项，实时估计并补偿这些参数，缓解手机安装不稳定造成的影响。在有GNSS 信号时，利用卡方检验评估GNSS 定位结果的可靠性并进行GNSS 位置更新；同时使用车辆运动约束和前向速度预测模型构造速度观测，修正导航状态；而当判定车辆处于静止状态时，进行零速修正和零航向角速度率修正；另外，还根据磁力计数据计算航向变化量进行航向约束。这些修正在GNSS 中断期间发挥重要作用，维持滤波器的稳定，输出连续可靠的定位结果。最后，发挥数据后处理的优势，进一步利用反向平滑算法对融合算法正向滤波结果进行优化。本文算法参加第七届室内定位与室内导航国际会议（IPIN2020）-室内定位比赛的赛题六“基于智能手机的车辆定位”，基于公开数据集的测试结果表明，本文算法能够有效地融合不同传感器和车辆运动约束信息，即使在卫星信号受到干扰或者遮挡的情况下，也能够在短时间内（例如400 s）输出稳定、可靠的定位结果，最终以定位精度优于7 m（75%概率）的成绩获得该赛题的冠军。

本文设计的多源融合算法仍然有待进一步完善，例如在GNSS 信号中断时进一步减缓航向角的发散以及提升前向速度的估计精度。除此之外，还可以考虑发挥智能手机本身的通信能力和车联网基础设施的作用，利用相邻车辆之间的环境信号（例如磁场特征或短时间内的GNSS 信号指纹）相关性实现协同定位，进一步提升每辆车的定位性能。