基于智能手机的车辆多源融合导航方法

纪新春，魏东岩，袁 洪，周德云

（1.西北工业大学电子信息学院，西安 710114；2.中国科学院空天信息创新研究院，北京 100094）

车辆导航应用要求导航系统能够实时提供精确、稳定和连续的车辆位置、速度及航向信息。考虑到成本问题，大众车辆导航通常选取微机电系统（Micro Electro Mechanical System，MEMS）-捷联惯性导航系统（Strap-down Inertial Navigation System，SINS）与全球导航卫星系统（Global Navigation Satellite System，GNSS）进行组合定位。然而，在城市环境中，车辆易受楼宇、隧道、高架桥等遮挡，GNSS 定位常出现精度降低甚至完全中断的问题。此时，低成本微机电惯性测量单元（MEMS-based Inertial Measurement Unit，MIMU）的漂移误差会使MIMU-SINS/GNSS 组合导航系统的性能在短时间内迅速恶化[1,2]。

为了解决该问题，车辆多源融合导航技术近年来得到了广泛研究。除GNSS、MIMU 外，常用的车辆导航源还包括里程计、车辆运动约束、磁力计、视觉传感器、数字地图等[3-8]。基于多源融合的定位算法能够充分利用各导航源的特点，实现量测信息的优势互补，构成多冗余、高可靠的车辆导航系统，进而提高城市复杂环境中车辆导航的性能。

此外，随着应用的普及，智能手机导航已逐渐取代专用导航终端而成为大众车辆导航的主流方式。目前绝大多数的智能手机均内置有GNSS 模块、MIMU、磁力计等导航传感器以及高性能处理器，具有强大的感知能力、运算能力与通信交互能力[9]。基于此，本文提出了一种基于智能手机的车辆多源融合导航方法，在MIMU-SINS/GNSS 组合的基础上：通过蓝牙接口引入车载诊断系统（On-Board Diagnostic，OBD）中的轮速信息，与横向、垂向零速构成完整性运动约束，提高姿态估计精度及短时GNSS 失锁后的定位性能；对于深度城市峡谷、地下车库等应用场景，采用基于里程信息辅助的地磁匹配定位算法，提供绝对位置校准，解决长时间GNSS 不可用情况下MIMU-SINS的误差发散问题。选取典型城市环境进行跑车测试，结果表明所提出方法能够显著提高智能手机的导航性能，满足城市环境车辆导航应用需求。

1 车辆多源融合导航方案

针对城市环境车辆导航应用，在常规MIMU-SINS与GNSS 组合导航的基础上，增加OBD 轮速、车辆运动约束以及地磁匹配等观测信息，构成车辆多源融合导航系统，算法流程如图1所示。

图1 车辆多源融合导航流程框图Fig.1 The diagram of vehicle multi-source fusion navigation

图1中，多源信息融合过程采用序贯滤波方法，以保证计算实时性，并满足各导航源即时接入、退出的要求。其中，基于加速度计、陀螺仪测量值实时判断零速状态，当检测到车辆处于零速状态时进行陀螺零偏计算、补偿，同时采用零速、零角速约束，否则采用横向、垂向零速约束；在GNSS 长期不可用的情况，利用浅层地壳及地表建筑物磁化后形成的地磁异常场特征进行匹配定位，进而为MIMU-SINS 提供绝对位置校准；OBD轮速除了提供前向速度与横向、垂向零速构成完整性运动约束外，积分后获得的行驶里程为地磁匹配过程中的磁特征测量值空间刻度化处理提供尺度基准，提高匹配解算的效率和精度。

2 车辆智能手机MIMU-SINS

2.1 车辆SINS 模型

导航坐标系（n系）选择东北天坐标系，载体坐标系（b系）选为右前上坐标系。对于SINS 解算，车辆的姿态、速度和位置变化率可以通过式(1)计算得出：

式中，表示b系相对于n系的姿态矩阵，vn为n系下东北天速度矢量，p为n系下经纬高位置矢量，为b系相对于惯性系（i系）的角速率，由陀螺仪测得，表示n系相对于i系的旋转角速度，由地球自转和车辆地速引起的n系旋转形成，fb为加速度计测量的比力矢量，gn为重力加速度矢量，Mpv为车辆速度至位置的转换矩阵：

式中，L表示纬度，h为高度，RM、RN分别为当地子午圈和卯酉圈的曲率半径。

当初始参数已知，通过加速度计和陀螺仪可以获取fb和，根据式（1）计算姿态、速度和位置的变化率后，通过积分即可获得车辆当前的姿态、速度和位置信息。

2.2 智能手机MIMU 安装矩阵投影

SINS 解算过程要求MIMU 测量坐标系（m系）与b系重合，而实际由于智能手机摆放的随意性m系与b系之间通常存在差异，即安装偏差角。对于低成本传感器，由于MIMU 误差特性不稳定且GNSS 观测信息质量较差，信息融合过程将安装偏差角作为独立的状态量很难获得稳定的估计结果。因而，本文采取MIMU 安装矩阵投影的方法来消除手机摆放姿态的影响，处理过程包括初始零速检测、水平投影与安装偏差角补偿。

在车辆初始静止时，依据加速度计的输出fm计算m系与当地水平面夹角，然后将fm、统一投影至当地水平面。采用加速度、角速率方差检测法进行车辆零速状态检测：

式中，Nw为滑动检测窗口长度，、分别为窗口内的加速度、角速率均值，Tf、Tω为检测门限，若k时刻方差小于阈值，则k时刻为零速时刻，否则为非零速时刻。

当检测出车辆零速状态时，得到m系与当地水平面坐标系（t系）夹角为：

式中，Δθ为俯仰方向夹角，Δγ为横滚方向夹角，基于Δθ、Δγ构建旋转矩阵即可将MIMU 测量值转换至当地水平面。

通常情况，车辆初始静止时b系与当地水平面不完全重合，且水平投影后的MIMU 航向与b系航向不重合。当MIMU-SINS/GNSS 组合模式滤波稳定后，t系下载体速度可表示为：

式中，αφ、αθ分别为水平投影后航向、俯仰安装偏差角，当车辆行驶速度不为零时，可由、计算得出。

综上，得到智能手机MIMU 的安装矩阵为：

式中，Cist即为初始安装矩阵，C1、C2、C3及C4分别为Δγ、Δθ、αθ及αφ扩展形成的姿态转换矩阵。在完成安装矩阵Cist投影后，MIMU 测量坐标系与车辆载体坐标系保持一致。

3 车辆运动约束与地磁匹配

3.1 运动约束

车辆在道路上正常行驶时，通常不发生侧滑和跳跃，可假设车辆在b系下x轴和z轴方向的速度为零[4,5]。根据实际情况，可将x轴和z轴方向的速度考虑为零均值高斯白噪声，同时结合OBD 轮速提供的前向（y轴）速度构成车辆完整性运动约束，即：

式中，vOBD为OBD 轮速信息。

对于零速状态，采用零速校正（zero-velocity update，ZUPT）方法抑制导航误差发散。由于零速状态的车辆真实速度为零，MIMU-SINS 的速度输出即为速度误差，将该速度误差作为量测值可对MIMU-SINS 的各项误差进行辨识和补偿。此外，零速状态航向角误差具有不可观测性，仅利用ZUPT 无法对航向角误差进行精准的估计。因而，引入零积分航向角速率（zero integrated heading rate，ZIHR）修正方法[6]，将相邻时刻航向角差值作为量测值，减小陀螺漂移引起的航向角误差，提高航向保持精度。

式中，φk为k时刻车辆载体航向角。

3.2 地磁匹配

现代城市环境中，地表钢筋混凝土建筑、供电线路等磁性物质进一步丰富了地磁特征信息，且具有高空间分辨率，是有效的匹配导航信息源[10]。地磁匹配导航分为离线建库与在线匹配两个阶段，由于车辆行驶速度不尽相同，会造成两阶段的磁测量数据无法空间对齐，导致无法利用磁场空间分布特征进行匹配定位。现有文献通常采用动态时间归整（Dynamic Time Warping，DTW）算法解决该问题，但该算法计算复杂，不能满足智能手机车辆导航的实时性要求。本文则利用里程信息来实现在线测量数据与离线基准库数据的空间对齐，图2所示为磁测量值等里程间隔空间转换过程。

图2 磁测量值时间序列变换至等里程间隔空间序列Fig.2 Transformation of the magnetic measurements from time sequence to spatial sequence

变换前磁测量值与时间、行驶里程与时间的对应关系如下：

式中，（M,T）为磁测量值时间序列，（mt,n,tn）为tn时刻的磁测量值，（D,T）为里程测量值时间序列，（dt,n,tn）为tn时刻里程测量值，可由OBD 轮速积分计算得出。

对里程序列（D,T）进行等里程间隔的采样，并根据采样点所对应的测量时刻读取（M,T）中相应的磁测量值，完成磁测量值的空间刻度化处理：

式中：d为里程采样间隔；kL为里程采样格点，（M,D）为磁测量值等里程间隔空间序列。

离线建库与在线匹配过程采用相同的里程间隔d，即可实现二者空间尺度对齐。考虑到单点匹配易产生误匹配，在行驶里程上取距离为Dw=w⋅d长度的实测地磁数据与基准库地磁数据进行滑动窗口匹配。其中，w表示窗口长度，匹配过程如图3所示。

图3 地磁匹配定位示意图Fig.3 The diagram of geomagnetic matching positioning

式中，与分别为滑动窗口内实测磁场数据与基准库磁场数据的均值，在搜索区间kL∈（w,KL）内，相关系数峰值所对应的基准库位置即为匹配位置。

4 多源融合模型

4.1 状态方程

多源融合滤波模型基于MIMU-SINS 误差方程建立，选取n系中的位置误差、速度误差、姿态误差、加速度计常值偏差和陀螺仪常值零偏构成状态矢量：

式中，δφn,δ vn,δPn分别为姿态、速度及位置误差，为加速度计常值零偏，为陀螺仪常值零偏。

4.2 量测方程

4.2.1 GNSS 信息

当GNSS 信息有效时，采用GNSS 定位、测速结果校正MIMU-SINS 解算误差，GNSS 位置观测量为：

式中，LI,λI,hI、δ LI,δλI,δhI分别为MIMU-SINS 的纬度、经度、高度及相应误差，LG,λG,hG为GNSS 纬度、经度、高度，观测矩阵，为GNSS 位置观测噪声。

GNSS 速度观测量为：

4.2.2 地磁匹配结果

相比开阔空间，在城市峡谷、地下车库等GNSS信号遮蔽环境中，地磁特征更加丰富。当车辆行驶至地磁基准数据库可用区域时，采用地磁匹配位置结果校正MIMU-SINS 解算误差，观测方程为：

式中，LMag、λMag、hMag为纬度、经度、高度匹配结果，观测矩阵为匹配位置观测噪声。

4.2.3 车辆运动约束

当车辆处于运动状态时，车辆载体的横向、垂向虚拟零速与OBD 轮速信息构成完成性运动约束，观测矢量为：

完成MIMU 安装矩阵投影后，认为已消除安装偏差角影响，对式(17)求全微分可得：

式中，（vn×）为三维矢量vn的反对称矩阵。

当处于运动状态时，车辆运动约束观测方程为：

当车辆处于静止状态时，为了提高航向角的修正精度，同时进行ZUPT 和ZIHR 修正，观测矢量对于MIMU-SINS 解算，航向角变化率表示为：

式中，θ为俯仰角，γ为横滚角，车辆静止时中仅包含地球自转项。

仅考虑航向角φ与陀螺零偏的影响，对式(20)求偏微分可得：

静止状态下车辆航向角φk=φk-1，则可计算相邻时刻航向角差值为：

式中，TI为MEMS-SINS 解算周期。

静止状态下，车辆运动约束观测方程为：

式中，

为观测矩阵，V5为静止状态下车辆东向、北向、天向速度和航向角观测噪声。

4.3 多源信息融合

车辆行驶环境与状态复杂多变，使GNSS、地磁匹配及运动约束等导航源的可用性并不稳定，需要信息融合算法满足即插即用要求。本文采用序贯滤波[12]方法进行多源融合解算，算法结构如图4所示。

图4 序贯滤波信息融合算法结构图Fig.4 The diagram of sequential-filtering fusion algorithm

在序贯滤波信息融合过程中，需要对各导航源信息的有效性进行判断，包括GNSS 定位、测速有效性、地磁匹配有效性及零速状态，并依据有效性检测结果在同一周期内顺序滤波处理。状态更新过程如下：

式中，F为状态转移矩阵，W为系统噪声矩阵，K为增益矩阵，i=2…N为导航源序号。对于第k个滤波

5 试验验证

5.1 试验场景

试验区域为中科院北京新技术基地周边区域，路线长度约8.4 km，包括高楼遮挡、地下车库、高速路等典型城市环境。试验设备包括：别克昂科威车辆提供OBD 轮速信息（经标定后积分里程精度约1%）；某品牌智能手机作为车辆多源融合导航软件运行平台，采集各传感器数据并进行信息融合处理；NovAtel-100C 组合导航系统用于离线建立地磁基准库时提供高精度空间坐标，实时融合定位过程则作为基准比对源。试验场景及设备如图5所示。

图5 试验场景及设备图Fig.5 Experiment route and equipment

5.2 结果分析

为了提高车辆运动约束的性能，需要对智能手机随意摆放所导致的安装偏差角进行估计补偿。如图6（上）所示，组合导航的输出为MIMU 测量坐标系下的姿态结果，滤波稳定后恒定的姿态误差即为载体系与测量坐标系间的安装偏差角，航向偏差角约为28.9 °，俯仰偏差角约为0.5 °（水平投影处理后）。开阔环境在GNSS/MIMU 组合模式下，采用式（6）的计算方法，对航向、俯仰安装偏差角进行估计。分析图6可知，俯仰安装偏差角估计结果约为0 °，航向安装偏差角则稳定在约29.3 °，与实际安装偏差角值基本一致，投影处理后即可实现MIMU 测量坐标系与车辆载体坐标系的统一。

图6 安装偏差角估计结果Fig.6 Estimation results of the mounting angle

图7所示为测试过程GNSS 定位结果。分析可见：在普通道路及高速等开阔环境，GNSS 具有良好的定位性能，水平、高程定位误差均优于5.0 m；然而，当车辆驶入中科院新技术园区后，由于受到建筑物遮挡，GNSS 定位结果出现明显波动，水平、高程定位误差最大分别达到49.3 m 和18.7 m，且在两处地下车库中分别存在约215 m、286 m 长的不定位，严重限制了车辆导航的可用性。

图7 GNSS 原始定位结果分析Fig.7 GNSS original positioning results

图8所示为典型的OBD 轮速积分里程误差与磁场特征空间刻度化情况。图8中可见，采用基于里程信息辅助的磁场特征空间刻度化处理能够实现在线测量数据与基准库数据的空间尺度统一，从而避免了类似DTW 中大量的尺度伸缩运算，可显著提高匹配解算的实时性。

图9所示为测试过程地磁匹配定位结果。由于采用滑动窗口匹配，需要采集一定窗口长度的磁特征数据后启动匹配解算，在初始阶段无定位结果输出。同时，为了降低成本，离线建库过程仅采集了单条车道的磁特征信息。

如图9中所示：对于高速等多车道路段，测试路线与建库路线车道不一致会引入额外的匹配误差，水平定位误差约为3 m；对于中科院新技术园区及地下车库等单车道路段，则不存在该问题，匹配精度更高，水平定位误差约为1.5 m。整个测试过程平均匹配定位解算处理时延低于0.1 s，能够满足MIMU-SINS 误差修正的实时性需求。

图9 地磁匹配定位结果分析Fig.9 Geomagnetic matching positioning results

图10对比分析了智能手机不同导航源组合模式下的定位性能。图10中可见，GNSS/MIMU/OBD 轮速组合模式能够有效抑制GNSS 显著跳点，且可在短时GNSS 信号遮蔽场景中提供连续、稳定的定位功能。然而，由于MIMU、OBD 轮速均存在误差累积的问题，导致车辆驶入中科院新技术园区后仍存在明显的定位性能下降。可以预见，对于长时GNSS 信号遮蔽环境，GNSS/MIMU/OBD 轮速组合模式定位结果将逐渐发散。GNSS/MIMU/OBD 轮速/地磁匹配组合模式则进一步引入地磁匹配绝对定位信息，能够有效解决导航结果发散的问题，且不受应用场景限制。测试过程中该模式水平定位误差优于2.4 m，高程定位误差优于0.7 m，显著提高了车辆导航性能。

图10 多源融合导航定位结果Fig.10 Multi-source fusion positioning results

图11、图12分别为智能手机GNSS/MIMU/OBD轮速/地磁匹配组合模式测速误差与航向角误差，分析可见：初始对准完成后，三轴测速误差优于0.5 m/s，航向角误差可限制在±2 °内，很好地满足了目前大众车辆导航应用的需求。

图11 多源融合导航测速结果Fig.11 Multi-source fusion velocity results

图12 多源融合导航航向结果Fig.12 Multi-source fusion heading results

6 结 论

智能手机导航已经成为目前应用最广泛的大众车辆导航方案。本文提出了一种车辆多源融合导航方法，利用智能手机集成的GNSS、MIMU、磁力计等导航传感器，以及通过蓝牙端口获得的OBD 轮速信息，实现复杂城市环境车辆连续、稳定的导航功能。测试结果表明，所提出的多源融合导航方法能够很好地满足大众车辆导航应用需求。此外，现阶段GNSS 仍为最有效的车辆绝对位置、速度信息源，在实际应用过程中，仅需对城市峡谷、复杂立交高架、隧道及大型地下车库等“热点”区域事先构建地磁基准库，利用地磁匹配绝对定位信息，解决GNSS 长时间中断所导致的导航结果发散问题。