王付有 高成发 甘露 张瑞成 王剑超
车辆导航是智慧交通、智能车快速发展中不可或缺的一项，关系着快速寻路、规避拥堵、提高交通运转率等切实需求[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的严重漂移对松组合结果的影响.
针对手机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.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 定位结果均方根误差统计
在智能手机车载导航场景下，针对手机RTK在复杂环境下易发生漂移或跳动、手机惯性导航航向角易发散等问题，本文提出一种智能手机RTK/DR组合定位方法.利用OSM地图数据，经地图匹配修正惯导姿态更新的航向角，结合RTK抗差Kalman解算中的协方差阵、车辆运动方向、RTK定位航向角变化量设置了松组合的开关，最后采用标准kalman滤波估计松组合结果.
本文采用了P30、P40两台智能手机，对比了航向角修正前后的航位推算、组合定位与手机RTK定位的精度.从实验结果来看，OSM数据能够有效削弱航向角的发散，大幅提升方向精度，使纯航位推算能够获得与测地型接收机平行一致的轨迹.组合定位结果在大部分实验中获得了优于RTK的定位结果.轨迹图表明：算法能够有效识别RTK误差较大的路段，使得航位推算能够按照更准确的方向进行状态更新.6组实验结果中，组合定位相比于仅使用RTK定位，精度最高提升达19.4%.
本文的航位推算过程中，虽然在方向上彻底摆脱了对RTK的依赖，但在速度上使用的是RTK的速度投影，对RTK大幅跳变的漏判会导致航位推算的结果偏离实际轨迹.在速度上摆脱RTK，以及更准确的RTK跳变判断将挖掘出航位推算更大的潜力.
猜你喜欢航向修正轨迹风浪干扰条件下舰船航向保持非线性控制系统舰船科学技术(2022年10期)2022-06-17Some new thoughts of definitions of terms of sedimentary facies: Based on Miall"s paper(1985)Journal of Palaeogeography(2022年1期)2022-03-25修正这一天快乐语文(2021年35期)2022-01-18知坐标，明航向新世纪智能(高一语文)(2021年3期)2021-07-16轨迹读友·少年文学(清雅版)(2020年4期)2020-08-24轨迹读友·少年文学(清雅版)(2020年3期)2020-07-24考虑几何限制的航向道模式设计民用飞机设计与研究(2019年4期)2019-05-21轨迹现代装饰(2018年5期)2018-05-26软件修正摄影之友(影像视觉)(2017年1期)2017-07-18进化的轨迹（一）——进化，无尽的适应中国三峡(2017年2期)2017-06-09
