张守建，李欣然，王逸石，徐 博

城市环境下视觉惯性里程计辅助PPP定位

张守建，李欣然，王逸石，徐 博

（武汉大学 测绘学院，武汉 430079）

针对复杂城市环境下，卫星信号容易受到遮蔽，精密单点定位（PPP）技术无法实现高精度连续定位，而视觉惯性里程计（VIO）可以提供连续的相对位置和速度，但其误差随时间累积，无法长时间独立提供高精度导航等问题，提出半紧组合VIO辅助PPP（VIO-PPP）的定位方法：设计了VIO和PPP 2个滤波器；在VIO滤波器中，使用多状态约束下的卡尔曼滤波器来处理获取的视觉图像和惯性测量单元（IMU）观测数据，以预测位置和速度，并用PPP滤波器来修正预测值；同时根据定位精度因子和卫星的数量，在不同的环境中对PPP和VIO采取不同的加权策略；然后搭建全球卫星导航系统（GNSS）、惯性和视觉多源融合数据采集平台，对各传感器完成参数标定和时间同步，形成一套完整的城市环境下的高精度定位系统；最后在户外复杂环境中进行多组实验，比较VIO、PPP和VIO-PPP不同定位模式的定位结果。实验结果表明，多源融合导航系统可以显著提高导航性能，与PPP相比，VIO-PPP模型的精度能够提高40%以上，平面精度和高程精度分别可达到0.2和0.3 m。

多源融合平台；参数标定；车载导航；半紧组合；精密单点定位（PPP）；视觉惯性里程计（VIO）

0 引言

当前，高精度车道级导航已经成为自动驾驶中的关键技术。而在城市中，由于环境复杂，任何单一技术都无法长时间提供连续可靠的导航定位服务，因此集成多种传感器的多源融合导航成为未来导航领域的发展趋势[1-7]。

高精度全球卫星导航系统（global navigation satellite system，GNSS）定位模式主要有实时动态（real-time kinematic，RTK）载波相位差分定位技术和精密单点定位（precise point position，PPP）技术。与需要密集的地面基站的RTK技术相比，精密单点定位技术仅使用单一GNSS接收机就可以提供全球高精度定位服务[8-10]，因而得到了国内外的广泛关注。然而，在城市地区，卫星信号经常被阻断，导致PPP出现频繁初始化的问题，其定位精度和效率受到严重影响。文献[11]在复杂城市环境中，使用PPP-RTK技术的定位精度仅为10 m左右。同样，文献[12]在城市环境下基于单频PPP的三维均方根（root mean square，RMS）精度为1.17 m。仅靠单一GNSS难以在城市环境中提供稳定可靠的车道级导航服务。

惯性导航系统（inertial navigation system，INS）不依赖于外部信号，具备自主工作能力。然而，其定位误差会随着时间的推移而累积，因此在一段时间后很难提供准确的位置和速度。

惯性测量单元（inertial measurement unit, IMU）通常可根据其性能分为4个级别，分别是战略级、导航级、战术级和微机械级。目前市面上的IMU设备定位精度从每小时数十公里到数米不等。性能优异的IMU误差发散较慢、定位精度高，但受限于高昂的价格而难以推广。科学技术的发展催生了低成本的微机电系统（micro-electro-mechanical system，MEMS）IMU。目前MEMS IMU得到了广泛的应用。文献[12-17]通过将其与GNSS、视觉等导航技术结合，弥补了单一MEMS IMU误差飘移快、定位精度低的缺陷，为高精度的连续导航提供了新的方案。

为了在复杂城市环境下提供连续可靠的定位服务，搭建包含惯性、视觉和GNSS等多源传感器的移动观测平台，设计开发包括视觉惯性里程计（visual inertial odometry，VIO）滤波器和PPP滤波器在内的多频多系统PPP与VIO的半紧组合系统；为了保证定位结果的连续性，稳健的定权策略至关重要，因此还提出一种根据定位精度衰减因子（position dilution of precision，PDOP）和卫星数量的综合加权策略；最后，利用自主搭建的多传感器平台在城市环境下采集多源观测数据，对系统的精度进行全面评估。

1 VIO-PPP组合模型

如图1所示为半紧组合VIO-PPP系统的结构。为了简化算法并提高系统的稳健性，将系统分为 2个滤波器，即GNSS滤波器和VIO滤波器。VIO滤波器使用多状态约束下的卡尔曼滤波器（multi-state constraint Kalman filter，MSCKF）处理视觉图像和IMU观测数据。首先通过惯性导航系统机械编排整合状态变量，然后从视觉图像中提取并跟踪特征点，使用紧组合的方法对惯性与视觉观测数据进行融合。由于VIO的定位误差会随着时间的推移而累积，因此当最新的状态变量的时间戳与当前的GNSS历元同步时，PPP的结果可用来更新滤波器中的状态变量以限制误差累积。

图1 半紧组合VIO辅助PPP系统设计路线

在GNSS滤波器中，本文使用多频率和多系统的GNSS伪距和载波相位观测值和精密产品进行PPP解算，以获得定位结果。在计算过程中，VIO的预测定位结果被用于辅助PPP滤波器计算卫星到接收机的几何距离，同时参与进行半紧组合滤波，以提高PPP解算精度。

1.1 PPP观测模型

将卫星轨道、钟差和伪距码偏差代入到精密单点定位模型中，可以得到用户端多频多系统PPP的线性化观测方程为

PPP模型的详细公式可参考文献[10]。

1.2 视觉惯性里程计组合模型

误差状态可以表示为

将整个状态的协方差表示为

完整的误差传递方程可以表示为

当收到新的图像时，应增广状态协方差矩阵为

通过式（8）、式（9）和式（15），预测和更新步骤以扩展卡尔曼滤波的形式进行，完整公式可参见文献[18]。

1.3 半紧组合VIO辅助PPP模型

PPP的解算结果包含系（地心地固坐标系，即以地球为中心且与地球固联的坐标系，Earth-centered Earth-fixed，ECEF）中的纬度、经度和高程坐标。而VIO的解算结果是载体相对于起始系（世界坐标系，与重力方向一致的起始VIO框架）的状态，其中有4个不可估计的状态，即全球位置坐标和航向。为了整合2个不同坐标框架下的结果，以第一个历元的坐标为起点，将PPP解的结果转换为到系（当地导航坐标系，即以载体为中心，指向东-北-天方向的坐标系，east-north-up，ENU）下，且随后的PPP定位结果均被转换到第一个系下。在VIO的初始化过程中，位置积分的原点被设置为全局初始位置，因此系和系之间的转换只是关于基于PPP轨迹计算的航向角的旋转排列。通过上述初始化方法，即可以在系下融合PPP和VIO观测值。

在短时间内，VIO的定位精度高，因此将其作为初始值，取代PPP处理中标准单点定位的结果；同时，为了将VIO的预测位置融合到PPP滤波器中，我们在PPP观测模型中加入位置约束

位置约束的协方差由文献[19]中的方法确定。

2 实验与结果分析

车载实验在武汉大学校内进行，路线场景包括林荫小道和高楼建筑等复杂环境。本文所使用的多源融合导航硬件平台（如图2所示）中搭载了北京米文动力科技有限公司的Apex Xavier II产品作为控制单元，其内部集成了英伟达智能控制平台Jetson AGX Xavier。GNSS接收机和IMU采用的是Inertial Labs（惯性实验室）公司的INS-B型号GNSS/INS组合接收机，IMU的采样频率为100 Hz，其主要性能参数如表1所示。相机模块采用森云智能SG-AR0143型号摄像头，图像数据采样频率为10 Hz。同时，基准站设置于武汉大学测绘学院楼顶，用于实验后使用Inertial Explorer（惯性浏览器）软件获得RTK/INS的双向平滑紧组合定位结果。该结果的整体定位精度为厘米级，并在接下来的评估中作为参考值使用。

图2 GNSS/INS/视觉多源融合导航数据采集平台

表1 IMU主要性能参数

本次实验采集道路环境如图3所示。

图3 车载实验采集环境

图4 实验期间卫星数与PDOP值

2.1 多传感器参数标定与时间同步

本文的多传感器数据采集平台安装了包括GNSS接收机、IMU、视觉传感器等在内的多种传感器，因此需要事先标定出IMU与GNSS天线、视觉传感器的空间几何关系，以便后续解算，保证导航定位精度。关于视觉传感器的使用，其内外参数需要标定。相机的内参包括焦距、主点位置、成像畸变系数等。此外，由于本文使用的相机镜头为鱼眼镜头，其内参数还包括一个镜头弯曲系数。相机的外参是相机相对IMU的相对位姿。实验过程中，视觉传感器的内参数及其与IMU之间的外参均采用开源标定工具包Kalibr进行标定[20]。

此外，多传感器时间同步会很大程度地影响算法实现。GNSS绝对时间通常可达到纳秒级，可靠性强。我们可通过硬件时间同步和软件时间同步等将GNSS绝对时间传递给其他传感器。本文设计的多源融合导航平台中，IMU与GNSS已进行时间同步，但视觉传感器采集图像过程相对独立，采集到的图像数据并无时标信息，因此需要使用GNSS绝对时间传递给图像数据并打上时间戳，以便在融合解算过程中判断图像数据的采集时间。本文采取的时间同步策略为，首先由GNSS接收机发送秒脉冲及美国国家海洋电子协会（National Marine Electronics Association，NMEA）协议信号给控制单元，控制单元结合内部高稳晶振完成外部协调世界时（coordinated universal time，UTC）授时，建立自身高精度的时间基准，并通过发送脉冲信号来触发相机采样，同时设备会记录该脉冲的产生时间。传感器完成采样后，设备会将记录的时间与本次相机传回的图像做关联，作为该图像的时间戳，完成同步。

2.2 结果分析

为了验证半紧组合的VIO-PPP算法的有效性和可靠性，本文设计了3种不同的方案，包括VIO、PPP和VIO-PPP，来处理实验观测数据。其中PPP结果为GPS+BDS+Galileo三系统的双频非组合浮点解，其中GPS系统选用的信号频率为L1和L2，BDS系统选用的信号频率为B1I和B3I，GAL系统使用的信号频率为E1和E5a。并且实验使用Inertial Explorer软件解算得到的RTK/INS的双向平滑紧组合定位结果作为参考值。

2.2.1 INS/视觉里程计紧组合定位性能分析

在全程一共约25 min的观测数据中，选择其中视觉数据纹理清晰、路上车辆和行人干扰较少的约700 s时长数据进行INS/视觉里程计紧组合解算，同时该段时间内车辆的运动状态丰富，有利于进行实验验证。

选取GPS周内秒25200～25900 s时间段的观测数据，解算得到INS/视觉紧组合模式下的姿态误差序列和定位误差序列，如图5所示。由图可知，INS/视觉紧组合解算模式下，车辆俯仰角（pitch）和横滚角（roll）在700 s内仍能保持较好的稳定性，而最能明显反映城市环境下车辆行驶状态的航向角（yaw）出现了明显随时间发散的情况，行驶700 s后航向角误差已超过10°，同时INS/视觉里程计紧组合解算定位误差累积在N、E 2个方向上均达到50 m以上，U方向精度略高，但也超过20 s。可见单纯基于航位推算模式的INS/视觉里程计组合定位方式难以在城市环境中提供长时间稳定、可靠的导航服务，因此利用GNSS技术提供的绝对位置来限制INS/视觉里程计系统的误差累积是很有必要的。

图5 INS/视觉推算定姿、定位误差

如表2所示为INS/视觉紧组合模式下的位置误差和姿态误差精度定量统计情况，分别列出了位置和姿态误差的RMS值以及最大发散误差。

总结上述视觉惯性里程计推算结果可知，在视觉传感器辅助INS的情况下，定位定姿误差会随时间积累而逐渐增大，呈现出发散性，难以满足车道级别导航的要求。

表2 INS/视觉定位定姿误差统计

2.2.2 PPP/INS/视觉半紧组合定位性能分析

利用双频数据进行GPS+BDS+Galileo三系统动态PPP和PPP/INS/视觉半紧组合2种模式下的定位解算。

如图6所示分别为单PPP和VIO-PPP组合2种模式下的定位误差序列。由于实验过程的环境十分复杂，单纯GNSS的定位结果较差，尽管引入了高精度的相位观测值，但是误差序列依然呈现出显著的发散，遮挡严重的环境下，粗差和多路径效应导致多次出现收敛后再次发散的情况，PPP动态浮点解的条件下，N、E、U 3个方向的位置误差均存在1～2 m的浮动。而在VIO-PPP半紧组合的融合定位模式下，整个时间段内的系统差和定位精度得到了大幅改善，N、E、U 3个方向的位置误差均控制在1 m以内，且变化更为平滑，没有大量跳跃现象。该组实验说明，在复杂环境中，惯性数据和视觉信息的引入对动态PPP定位的精度有着显著的提升，对单一GNSS定位方式受周围环境影响产生的多路径效应有明显的改善。

如表3所示为单独PPP浮点解和VIO-PPP组合2种模式下的N、E、U方向定位精度统计情况。可以看出，VIO-PPP组合条件下在各方向上的精度均相比单纯PPP模式有显著提升。由于信号被遮挡，PPP定位的结果在N、U方向上仅能达到米级的精度，E方向上的定位精度能达到0.5 m以内；而在利用惯性和视觉手段辅助PPP进行粗差探测、多路径误差建模等预处理环节的情况下，可以显著改进定位精度而达到分米级。结合表2可知，借助GNSS测量信息的误差不随时间累积的特性，可以成功抑制VIO解算结果精度随时间发散的情况。同图5中的VIO解算结果相比，VIO-PPP半紧组合的位姿精度和稳定性均有明显提升。

3 结束语

本文搭建了多源融合数据采集平台，对使用的传感器进行参数标定与时间同步；开发了半紧组合VIO-PPP模型，包含一个PPP滤波器和一个VIO滤波器。GNSS数据和VIO数据被集成到MSCKF模型中，该模型使用VIO观测数据预测位置和速度，并用PPP滤波器修正预测值，为PDOP和卫星数量设定了阈值，以便对PPP和VIO采取不同的加权策略。然后在户外复杂环境环境中采集多传感器数据，利用半紧组合VIO-PPP组合导航系统进行解算，验证其导航性能，并分别与VIO和PPP解算结果进行比较。其中单一VIO模式的精度在约700 s的时间段内航向角误差累积超过10°，定位误差也发散至数十米，难以满足车辆导航的精度要求。而在复杂城市环境中，动态PPP的三维定位有效值也仅有1.98 m，无法满足车道级导航的要求。同时集成了VIO和GNSS的半紧组合VIO-PPP模式可以在城市环境中提供连续准确的位置，东、北、天3个方向的精度分别为0.22、0.12和0.18 m，与PPP相比，分别提高了85%、76%和85%。这表明，使用多传感器系统可以极大程度上弥补单一导航手段带来的缺陷，有效限制INS/视觉传感器的误差累积，提升卫星信号被严重遮挡情况下的定位精度与稳定性，实现高精度连续定位，并且目前的精度已经可以初步满足车辆在复杂城市环境中的车道级导航，为自驾车辆等技术的发展提供参考。同时为了使系统更加可靠，提高定位精度，未来我们可能会考虑其他传感器，如激光雷达（light detection and ranging，LiDAR）。此外，预计使用GNSS PPP固定解，性能将得到进一步提高。

[1] LIAO J C, LI X X, WANG X B, et al. Enhancing navigation performance through visual-inertial odometry in GNSS-degraded environment[J]. GPS Solutions, 2021, 25(2): 1-18.

[2] LI X, LI X X, HUANG J X, et al. Improving PPP-RTK in urban environment by tightly coupled integration of gnss and ins[J]. Journal of Geodesy, 2021, 95(12):1-18.

[3] 朱锋. GNSS/SINS/视觉多传感器融合的精密定位定姿方法与关键技术[D]. 武汉大学, 2019.

[4] 黄声享, 李冠青, 张文, 等. 港珠澳大桥沉管隧道施工控制网布设研究[J/OL]. 武汉大学学报•信息科学版: 1-9[2022-10-28]. https://doi.org/10.13203/j.whugis20170196.

[5] REID T, HOUTS S E, CAMMARATA R, et al. Localization requirements for autonomous vehicles[J]. SAE International Journal of Connected and Automated Vehicles, 2019, 2(3): 173-190.

[6] 张乐添, 赵冬青, 贾晓雪, 等. 面向动态环境的双目/惯性SLAM系统[J/OL]. 导航定位学报: 1-7[2022-11-03]. http://kns.cnki.net/kcms/detail/10.1096.p.20220902.1438.002.html.

[7] BOCK Y, MELGAR D. Physical applications of GPS geodesy: A review[J]. Reports on Progress in Physics, 2016, 79(10): 106801.

[8] BLEWITT G. An automatic editing algorithm for GPS data[J]. Geophysical Research Letters, 1990, 17(3): 199-202. DOI:10.1029/GL017i003p00199.

[9] KOUBA J, HÉROUX P. Precise point positioning using IGS orbit and clock products[J]. GPS Solutions, 2001, 5: 12-28.

[10] 简濠骏, 王逸石, 张元泰, 等. 北斗卫星广播星历精度评估与单点定位优化模型[J]. 大地测量与地球动力学, 2022, 42(3): 291-297. DOI:10.14075/j.jgg.2022.03.014.

[11] LI X, LI X X, LI S Y, et al. Centimeter-Accurate vehicle navigation in urban environments with a tightly integrated PPP-RTK/MEMS/vision system[J]. GPS Solution, 2022, 26:124.

[12] GU S F, DAI C Q, FANG W T, et al. Multi-GNSS PPP/INS tightly coupled integration with atmospheric augmentation and its application in urban vehicle navigation[J]. Journal of Geodesy, 2021, 95(6): 1-15.

[13] RABBOU M A, EL-RABBANY A. Tightly coupled integration of GPS precise point positioning and MEMS-based inertial systems[J]. GPS Solution, 2017, 19(4): 601-609.

[14] LI M Y, MOURIKIS A I. High-Precision,consistent EKF-based visual-inertial odometry[J]. Int J Robot Res, 2013, 32: 690-711.

[15] BLOESCH M, OMARI S, HUTTER M, et al. Robust visual inertial odometry using a direct EKF-based approach[C]// IEEE/RSJ International Conference on Intelligent Robots and Systems. Hamburg, Germany, 2015. DOI: 10.1109/IROS.2015.7353389.

[16] LEUTENEGGER S, LYNEN S, BOSSE M, et al. Keyframe based visual-inertial odometry using nonlinear optimization[J]. Int J Robot Res, 2015, 34: 314-334.

[17] GAO Z Z, ZHANG H P, GE M R, et al. Tightly coupled integration of multi-GNSS PPP and MEMS inertial measurement unit data[J]. GPS Solutions, 2017, 21(2): 377-391.

[18] SUN K, MOHTA K, PFROMMER B, et al. Robust stereo visual inertial odometry for fast autonomous flight[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 965-972.

[19] LI X X, WANG X B, LIAO J C, et al. Semi-Tightly coupled integration of multi-GNSS PPP and S-VINS for precise positioning in GNSS-challenged environments[J]. Satellite Navigation, 2021, 2(1): 1-14.

[20] 苏景岚. 车载视觉/INS/GNSS多传感器融合定位定姿算法研究[D]. 武汉大学, 2019.

Visual inertial odometry-aided PPP positioning in urban environments

ZHANG Shoujian, LI Xinran, WANG Yishi, XU Bo

（School of Geodesy and Geomatics, Wuhan University, Wuhan 430079, China）

Aiming at the problems that in the complex urban environment, precise point positioning (PPP) cannot achieve high precision continuous positioning because satellite signal is easily blocked by trees, buildings and tunnels, meanwhile visual inertial odometry (VIO) can provide continuous relative position and velocity, but its time-increasing errors make it difficult to provide highly accurate positioning results independently after a long time, the paper proposed a positioning method of semi-tightly coupled VIO aided PPP (VIO-PPP) system: two Kalman filters including VIO filter and PPP filter were designed; in the VIO filter, the multi-state constraint Kalman filter (MSCKF) model was used to process the acquired visual images and inertial measurement unit (IMU) observations to predict the position and velocity, and the predicted values were corrected with the PPP filter; then based on the position dilution of precision (PDOP) and the number of satellites, different weighting strategies for the PPP and the VIO were adopted in different environments; in addition, a multi-sensor data acquisition platform was built with global navigation satellite system (GNSS), inertial sensors and cameras, and the parameter calibration and the time synchronization were performed for each sensor, for forming a complete set of high-precision positioning system in the urban environment; finally, multiple sets of experiments were conducted in a complex outdoor environment to compare the positioning results of different positioning modes of VIO, PPP and VIO-PPP. Results showed that a multi-source fusion navigation system integrating multiple sensors could significantly improve the performance of navigation and positioning, and the VIO-PPP model would bring more than 40% accuracy improvement compared with PPP, with 0.2 m accuracy in plane and 0.3 m accuracy in elevation.

multi-source fusion platform platform; parameter calibration; in-vehicle navigation; semi-coupled integration; precise point positioning (PPP); visual inertial odometry (VIO)

张守建, 李欣然, 王逸石, 等. 城市环境下视觉惯性里程计辅助PPP定位[J]. 导航定位学报, 2023, 11(6): 34-41.（ZHANG Shoujian, LI Xinran, WANG Yishi, et al. Visual inertial odometry-aided PPP positioning in urban environments[J]. Journal of Navigation and Positioning, 2023, 11(6): 34-41.）DOI:10.16547/j.cnki.10-1096.20230605.

P228

A

2095-4999(2023)06-0034-08

2023-11-08

国家重点研发计划（2021YFB2501100）

张守建（1980—），男，山东临沂人，博士，副教授，研究方向为多源融合导航、低轨卫星精密定轨等。