复杂环境智能手机RTK/DR车载组合定位方法

王付有 高成发 甘露 张瑞成 王剑超

0 引言

车辆导航是智慧交通、智能车快速发展中不可或缺的一项，关系着快速寻路、规避拥堵、提高交通运转率等切实需求[1].随着硬件传感器的普及，车辆定位的方法也从单GNSS定位，扩展到GNSS接收机、惯性导航系统[2]、激光雷达[3]、里程计[4]等设备的融合定位.而对于用户而言，手机作为出行必不可少的设备，在大多数情况下承载着导航定位的任务.

现如今实时动态RTK定位已经在部分机型实现，且能够在开阔的环境下发挥稳定作用，而在复杂环境中，定位精度及稳定性均难以保证[5].智能手机内部集成的惯性传感器为航位推算(Dead Reckoning,DR)提供了可能，而实际应用中，低成本的惯导设备数据精度较低，短时间的积分运算即带来较大的速度、姿态、位置误差.此时RTK、航位推算的松组合结果会跟随RTK结果变化，难以带来精度上的有效提升.

影响航位推算过程精度的因素主要是航向角.航位推算过程中，通过角速度、加速度积分更新的航向角误差积累较快，有必要进行航向角的实时修正.航向角的修正一般围绕着车辆运动状态和场景信息进行，有零速修正[6]、道路线约束[7]、直线检测[8]、地图匹配[9]等方法.常规的地图匹配是将定位点纠正到道路当中，对地图精度要求较高.目前可获取的开源地图OSM(OpenStreetMap)数据在点位精度上较低，但几何精度较好[10]，具备修正航向角精度水平，因此本文采用了基于OSM地图匹配的航向角修正方法.首先，根据RTK/DR融合定位结果、航向角、点投影依次筛选，得到候选匹配道路线，再结合车辆运动状态、垂线距离、路线长度选择出最符合的行驶道路，最后使用该道路的方向来纠正航向角.在OSM地图数据的辅助下，航位推算的航向角与车辆的行驶方向更加一致.

智能手机RTK为抗差Kalman滤波浮点解，解算后与航位推算结果进行松组合，最后使用标准Kalman滤波对速度和位置进行估计.此外，本文增加了松组合的开关，结合RTK抗差Kalman解算待估参数的协方差阵Pk、车辆运动方向、RTK定位航向角变化量综合判断是否进行量测更新，以此减小RTK的严重漂移对松组合结果的影响.

1 关键算法

针对手机GNSS定位易受环境影响、手机内部传感器精度低等问题，本文提出一种RTK/DR组合定位方法，在RTK/DR松组合的基础上，设计了地图匹配算法，充分发挥OSM数据在航向角修正和松组合开关中的作用，进一步提升智能手机在复杂环境的定位精度.

算法流程主要有：

1)手机RTK定位；

2)航位推算；

3)航向角修正；

4)RTK/DR松组合.

1.1 手机RTK定位

智能手机通过网络连接校内基站，实时接收并解码基站观测数据，与手机原始观测值依次星间差分、站间差分，组成双差模型，并使用抗差Kalman滤波对位置、速度、加速度、双差模糊度进行估计[11-12].手机实时动态RTK难以固定模糊度，模型中使用浮点解进行解算.双差观测方程如下：

(1)

(2)

式中：1代表流动站，2代表基准站，s为其他卫星，r为参考卫星.在短基线下，双差模型大幅削弱了电离层延迟、对流层延迟的影响，消除了卫星钟差及测站接收机钟差.

手机采用的抗差方案[13]如下：

(3)

(4)

(5)

1.2 航位推算

航位推算是一种相对定位技术，基本原理为已知起算点姿态、速度、坐标，根据传感器采集到的信息去计算下一时刻载体的姿态、速度和位置，循环往复来完成一段时间的导航任务[14-15].

航位推算包括起算点初始化、速度更新、航向角更新、位置更新四个过程.起算点初始化可利用的信息有限，只能通过RTK定位进行赋值，具体公式为

(6)

速度更新方式与式(6)一致，其准确度与航向角的精度密切相关.

航向角更新为姿态更新的一部分，采用智能手机100 Hz采集的惯导传感器数据进行解算.惯导姿态更新算法参考严恭敏教授开源PSINS工具箱代码，获取到手机采集的加速度、角速度、重力加速度后，进行四元数更新.将四元数转换为姿态矩阵，最后计算出欧拉角，如下所示:

(7)

(8)

式中：q0至q3为手机四元数；φ，θ，γ分别为航向角、俯仰角和侧滚角.

位置更新基本公式为

(9)

式中：vn，ve，vu分别为北向、东向、天向的速度；dt为RTK定位时间间隔；Rm为子午圈主曲率半径；Rn为卯酉圈主曲率半径.

1.3 航向角修正

航向角修正策略就是通过地图匹配获取当前行驶路线，使用路线的方向来纠正.地图采用开源的OSM数据，其质量经学者分析，位置精度不高，但几何精度较好，具备修正航向角的能力.

地图匹配通常有点到点、点到线、线到线的传统方法[16]，也有基于隐马尔可夫[17]的概率方法，考虑到定位的实效性本文采用点到线的匹配方法.基本思路为：根据上个历元的融合定位结果，筛选出周围50 m以内的道路线段；根据此前航向角，筛选掉相差超过20°的线段，再去除点投影之外的线段，即可得到候选的路段；最后根据点到线最短垂线距离找到符合的线段，使用该线段的方向修正航向角.由于OSM地图位置精度有偏移，因此存在匹配到相邻车道的情况，而本文只使用道路的方向信息，所以平行的道路线段方向不影响对航向角的修正.

OSM地图的道路在转弯时，突变的角度与车辆平滑的角度变化相矛盾，因此航向角修正更适合在直线路段进行.本文实时记录惯导姿态更新后的航向角，当航向角连续2 s变化量超过3°，即可认为车辆处于转弯状态，从而停止航向角的修正，修正的具体效果将在实验部分给出.

1.4 RTK/DR松组合

RTK、DR松组合采用基本Kalman滤波，对手机的位置和速度进行估计，其状态方程和量测方程为

(10)

状态方程表达的是上一时刻的状态到当前状态的一个转变，状态转移矩阵Φk/k-1描述了状态的转换过程，其构成为

(11)

式中：dt为惯导数据采样时间间隔，Bk为纬度.量测方程中Zk为RTK定位得到的三维坐标和速度.

状态更新和量测更新按照式(12)进行：

(12)

式中：Rk为观测信息的方差矩阵，对应位置和速度的噪声，本文使用RTK抗差Kalman滤波后的协方差阵Pk来对Rk赋值；Hk为松组合的系数矩阵.

在道路两边均是高楼的复杂环境下，RTK定位结果会出现跳动或者漂移，给松组合引入较大的误差，也给后续的航位推算带来错误的起算点.针对这种情况，本文增加了松组合开关，结合RTK抗差Kalman滤波后的协方差阵Pk、车辆运动方向、RTK定位航向角变化量综合判断是否进行松组合中的量测更新.将Pk的位置方差值从WGS84坐标转为东北天坐标系，并计算平面的误差ep，作为判断定位质量的一项指标.当ep大于2、车辆运动方向呈现直线、RTK定位的航向角变化量大于10°，本文即认为RTK存在漂移或者跳动，此时松组合仅进行状态更新，而不进行量测更新.

2 实验及结果分析

2.1 实验方案

本次实验场地为东南大学九龙湖校区，车辆按照图1的路线行驶，起点和终点均为开阔的白色点，共行驶3圈.设备安装如图2所示：木板的两端各固定一个测地型接收机，用于计算手机的真实参考坐标；采用的手机为华为P30与P40，通过支架固定在木板上.实验过程中，手机预先安装团队开发的定位软件，实时接收校内基站观测数据，并进行RTK定位，同时存储手机的加速度、角速度、磁强度、重力加速度、GNSS原始观测值信息.RTK定位频率1 Hz，惯性传感器数据采样频率设置为100 Hz，事后进行RTK、航位推算的组合解算.

图1 车辆行驶路线Fig.1 Vehicle route

图2 设备安装位置Fig.2 Equipment placement

2.2 航向角修正结果分析

图3为航向角修正前后的航位推算轨迹，对应着表1中的P40-2，横坐标加上118.8°为实际的经度，黑色为测地型接收机解算的基准轨迹，红色为仅进行初始对准的航位推算轨迹，蓝色为经航向角修正的航位推算轨迹.图3显示，纯航位推算的航向角误差逐渐积累，偏离黑色的基准轨迹，而航向角修正后，航位推算的轨迹与基准轨迹更加平行一致.结合表1和图3，可以说明地图数据的引入能够有效控制航向角误差累积.

表1 航向角误差统计

图3 航位推算轨迹对比(横坐标加118.800°为实际经度)Fig.3 Comparison of dead reckoning trajectories

2.3 定位误差统计

方向修正后，航位推算在位置上仍与基准有偏差，需要与RTK组合进一步提升精度.图4给出了RTK、航位推算组合后的定位轨迹对比，对应着P40手机的第2圈，横坐标加上118.8°为实际的经度.黑色代表测地型接收机的基准坐标，红色代表手机RTK实时定位结果，蓝色代表RTK、航位推算组合定位结果.可以看到，在轨迹图上方，红色的RTK轨迹由于两侧高楼影响，定位出现了漂移，此时算法检测到RTK的异常，仅进行松组合的状态更新，而不进行量测更新，从而得到与基准更符合的定位轨迹.

图4 松组合定位轨迹(横坐标加118.800°为实际经度)Fig.4 Loose combination positioning trajectory

本文按照手机进行分组，统计了P30、P40手机3圈的定位结果，如图5所示.可以看到，部分时间段，RTK存在着超过5 m定位误差，而同时间段的RTK、航位推算组合定位性能明显提升，有效抑制了大幅跳动的误差点.

表2统计了2种定位方式下的均方根误差，每台手机3组结果.数据显示:有5组实验RTK、航位推算组合定位平面精度优于RTK；P30的第2组实验中N方向精度有所改善，E方向精度不佳，平面精度2种定位方式相当；6组实验中，组合定位精度最好达到1.512 7 m，最差2.052 7 m；相比于仅使用RTK定位，组合定位精度提升幅度最大的为P40-2实验，达到19.4%.结合图4、图5及表2，可以得到结论：航位推算的加入能够在复杂环境下平滑轨迹、降低侧向漂移.

图5 手机定位误差Fig.5 Mobile phone positioning errors

表2 定位结果均方根误差统计

3 结束语

在智能手机车载导航场景下，针对手机RTK在复杂环境下易发生漂移或跳动、手机惯性导航航向角易发散等问题，本文提出一种智能手机RTK/DR组合定位方法.利用OSM地图数据，经地图匹配修正惯导姿态更新的航向角，结合RTK抗差Kalman解算中的协方差阵、车辆运动方向、RTK定位航向角变化量设置了松组合的开关，最后采用标准kalman滤波估计松组合结果.

本文采用了P30、P40两台智能手机，对比了航向角修正前后的航位推算、组合定位与手机RTK定位的精度.从实验结果来看，OSM数据能够有效削弱航向角的发散，大幅提升方向精度，使纯航位推算能够获得与测地型接收机平行一致的轨迹.组合定位结果在大部分实验中获得了优于RTK的定位结果.轨迹图表明：算法能够有效识别RTK误差较大的路段，使得航位推算能够按照更准确的方向进行状态更新.6组实验结果中，组合定位相比于仅使用RTK定位，精度最高提升达19.4%.

本文的航位推算过程中，虽然在方向上彻底摆脱了对RTK的依赖，但在速度上使用的是RTK的速度投影，对RTK大幅跳变的漏判会导致航位推算的结果偏离实际轨迹.在速度上摆脱RTK，以及更准确的RTK跳变判断将挖掘出航位推算更大的潜力.