基于GPS 的先验地图辅助视觉惯性定位方法

李磊磊，梁 琳，钟 傲，杨伊菲，张 哲，韩勇强

（北京理工大学 自动化学院，北京 100081）

精确定位是自主导航系统中至关重要的一部分，全球定位系统(Global Positioning System,GPS)采用绝对定位技术，具有精度高、使用范围广等特点，定位精度通常可达到10～15m[1]，并且没有累计误差。基于视觉传感器和惯性测量单元(Inertial Measurement Unit,IMU) 的同时定位与地图构建(Simultaneous Localization and Mapping，SLAM) 技术，采用相对定位方法进行地图构建，目前具有代表性的算法有ROVIO[2]、VI-DSO[3]、VINS-Mono[4]、ORB-SLAM3[5]等，但其无法得知实际航向、无法提供绝对位置，且随着系统的长时间运行会积累漂移。

Deng 等[6]将GPS 与基于激光雷达的SLAM 算法相结合，可实现室内、室外、隧道等场景下的高精度导航，Su等[7]提出了一种基于激光雷达的SLAM系统，由GPS/INS 集成导航系统作为辅助，提高了自动驾驶汽车实时地图和定位的鲁棒性，但激光雷达的使用使得以上两种方案成本较高。李洪[8]等利用机场跑道左右边线与中线，建立了线特征视觉位姿测量模型，解决了无人机着陆时卫星拒止环境所带来的问题，但只适用于特定环境。Kiss-Illés 等[9]提出了GPS-SLAM，使用图像帧的GPS 位置和IMU 角度信息来确认下一帧图像的位置，提高了系统定位精度，但图像处理时间也有所增加。Chen 等[10]基于优化的方案，提出一种GPS 测量值与ORB-SLAM 紧耦合的无人驾驶定位方法，可在室外大范围环境中实现无人驾驶汽车的自主定位，但在动态环境中的鲁棒性差。

针对上述问题，本文在先前工作[11]的基础上，提出一种基于GPS 的先验地图辅助视觉惯性定位方法。对GPS 的位置信息进行处理，得到每一帧图像采集点处相对于相机参考帧的相对坐标，并与关键帧信息同步来构建先验地图。建立先验图像帧与单目相机图像帧的关系，利用最小二乘法进行系统的位置姿态估计与优化。本文方法有效地提升了视觉惯性导航系统的定位精度，而且由于先验地图帧的相对位置坐标是由GPS 得到的真实地理坐标转换而来，在最终输出轨迹中可以找到某点对应的地理坐标，与先前工作只能得到相对轨迹估计相比，实现了真实地理坐标系下的轨迹估计。

1 系统框架

本文主要研究GPS 位置信息与关键帧数据融合生成先验地图的方法以及基于先验地图的重定位技术，整体算法流程如图1 所示。

图1 算法流程图Fig.1 Algorithm flow chart

单目视觉无法获得场景结构的真实深度信息，IMU 存在累积误差，会使大范围位姿估计出现较大偏差，单纯使用GPS 只能获取载体的位置，并不能获得载体的姿态。将单目视觉惯性同步定位与地图构建（Visual Inertial SLAM，VI-SLAM）方法的位姿估计与GPS 位置信息相融合，通过插值优化算法来获取每个关键帧所对应的真实地理坐标，进而得到其相对位置坐标，利用该数据进行位置预测，修正累积误差。

纯视觉初始化是通过运动恢复结构（Structure from Motion，SFM[12]）得到当前相机位姿与路标点的3D 坐标，具体过程如图2 所示，首先提取图像特征并跟踪，将图像第一帧作为参考，选取与当前帧特征点匹配较多的为参考帧，计算帧间相对位置姿态T，然后利用三角测量、PnP (Perspective-n-Point)等方法求取滑窗内每帧图像的位姿与每个特征点3D 坐标。

图2 纯视觉初始化Fig.2 Visual initialization

视觉惯性联合初始化部分，首先使不同频率的相机与IMU 采集到的信息同步，然后对两帧图像之间的IMU 数据积分处理。由于单目视觉只能估计出轨迹，而轨迹的实际大小是不确定的，这就需要IMU 为系统提供尺度信息。最后利用相机和IMU 之间的旋转约束来估计陀螺仪的零偏，利用平移约束来估计重力向量、速度以及初始尺度。

非线性位姿优化部分采用滑动窗口法，解算窗口内关键帧及相邻帧之间的位姿以进行局部优化，在关键帧数据库中存储特征点及其描述子信息、位置信息和姿态信息。当关键帧滑出滑窗时，会基于图像特征信息来匹配闭环帧，即当两帧相似度计算超过一定的阈值视为同一场景，计算出系统的累积偏移，再进行全局一致性位姿优化，将偏移添加到每一帧中，减小累积误差，更新关键帧数据库。先验地图的加入提高了关键帧信息中的定位精度，保证了估计结果的可靠性。

2 先验地图辅助的VI-SLAM

2.1 先验地图生成

为了便于表述，后文中统一用(·)c表示相机系，用(·)b表示IMU 系，(·)w表示世界坐标系。需要注意的是，构建先验地图时要考虑GPS 与IMU 系之间的平移关系。将系统的状态变量定义为：

其中，xi表示第i帧图像IMU 的状态，表示相机和IMU 的坐标变换，λk表示第k个特征点第一次被观测到时的深度。

首先系统将搭载单目相机与惯性传感器进行载体位姿估计，同时独立采集GPS 信息，将GPS 信息表示为：

其中，B表示纬度，L表示经度，h表示高度。利用式(3)，将GPS 的经纬度转化为高斯投影坐标(x,y)，即可得到数据采集点的位置坐标为(x,y,h)：

其中，L0表示中子午线的经度，N表示子午圈曲率半径，X表示子午线弧长，e表示椭球第一偏心率，e'表示椭球第二偏心率。

将图像数据与GPS 数据插值同步，利用采集时间可得到第k帧图像相对于初始帧的位置平移量，视觉惯性导航系统的位姿估计结果与GPS 数据对齐，如图3 所示，就可得知各帧图像对应的绝对地理位置及各帧图像之间的旋转和平移关系，以此来构建包含绝对位置的先验地图。

图3 数据同步示意图Fig.3 Illustration of data synchronization

如图4 所示，P1、P3为先验地图库中的关键帧，P2、P4、P5为采集到的新帧，当载体再次经过P1、P3时就可根据先验地图来优化所有帧的位姿，得到各帧的绝对位置。在构建先验地图时，为了节省空间，只保存特征点的信息，丢弃原始图像。先验地图中存储的信息为：

图4 先验地图优化技术示意图Fig.4 Illustration of optimization using priori map

其中，k表示关键帧的索引，l表示闭环帧的索引，表示k帧的位置的平移和旋转量，表示关键帧与闭环帧之间的相对平移和相对方向角，D(u,v,des)表示图像帧中特征点的像素坐标及其brief描述子[13]，ϕ作为地理坐标保存在先验地图中，便于在最终输出轨迹中查找某点的地理坐标。

2.2 位姿修正

首先将当前帧与先验地图匹配，系统基于词袋模型对当前帧与先验地图帧进行相似度计算，通过计算特征点的描述符之间的最小汉明距离来建立帧与帧之间的联系。为了减少误匹配，系统首先剔除异常点[14]，再剔除外点，此时若匹配候选帧与当前帧的匹配点依然超过系统设定的阈值时，则视为匹配成功，即找到了当前帧对应的先验地图帧，本文中称为闭环帧。

第二步根据闭环帧进行位姿修正，将当前滑动窗口内的位姿与闭环帧位姿对齐。在此过程中，将闭环帧的位姿视为常量，使用全部IMU 测量信息、局部视觉测量信息和检索到的特征联合优化，如图5 所示。设l帧为闭环帧，固定l帧的初始位姿，即当前窗口不对l帧的位姿进行优化，当闭环帧被成功匹配时，将其与关键帧的相对位姿和初始位姿添加到滑动窗口中，建立闭环帧l观测到的特征点重投影误差模型作为约束，与视觉重投影误差因子和IMU 残差因子共同优化，滑动窗口内的位姿估计会根据先验地图进行修正，得到最终带有GPS 地理坐标的位姿结果。式(5)为非线性优化目标函数，前三项分别为边缘化信息、IMU 残差与视觉残差，最后一项为引入GPS 先验信息后加入的误差项。

图5 重定位示意图Fig.5 Illustration of relocation

Huber 范数定义为：

由于IMU 的存在，系统中只有位置坐标(x,y,z)以及偏航角yaw不可观，为了校正这四个自由度的漂移，加入了顺序边和闭环边，如图6 所示，顺序边指每个关键帧与它前面的几个关键帧所构建的边，其测量值来自于视觉惯性里程计，闭环边指该关键帧与其闭环帧之间的连接所构成的边，其测量值来自闭环检测中PnP 求解的结果，两种边的测量方程均用式(7)表示：

图6 顺序边与闭环边Fig.6 Sequential and closed loop edges

3 实验验证

通过自主搭建实验平台完成陆地和低空的实验验证，采用PointGrey FL3-U3-32S2C-CS 单目相机和CHIOPT 8mm 镜头，IMU 的型号为MTI-G710，GPS数据以及真值由星网宇达XW-GNSS1061 高精度差分定位系统给出，在一台小型PC（Intel Core i7-5850EQ，16GB RAM）上计算。实验中首先搭载视觉传感器和惯性传感器进行位姿估计，同时独立记录GPS 数据，离线构建先验地图，后续则无需使用GPS 便实现真实地理坐标系下的轨迹估计，实验装置如图7 所示。

图7 实验装置Fig.7 Experiment equipment

3.1 地面车载实验

本次车载实验在北京理工大学中关村校区校园内进行，实验轨迹如图8 所示，以GPS 真值作为参考，分别比较了无先验地图、无GPS 的先验地图、有GPS的先验地图条件下的定位效果如图9-10 所示，对两次车载实验中x、y方向上位置估计结果分别进行误差分析，得到有、无先验地图两种情况下x、y方向误差随时间变化曲线如图11-14 所示。

图8 实验轨迹Fig.8 Experiment trajectory

图9 车载实验一轨迹图Fig.9 Illustration of the vehicle trajectory 1

图10 车载实验二轨迹图Fig.10 Illustration of the vehicle trajectory 2

图11 x 方向的位置误差对比图（车载实验一）Fig.11 Error comparison of x direction (vehicle 1)

图12 y 方向的位置误差对比图（车载实验一）Fig.12 Error comparison of y direction (vehicle 1)

图13 x 方向的位置误差对比图（车载实验二）Fig.13 Error comparison of x direction (vehicle 2)

图14 y 方向的位置误差对比图（车载实验二）Fig.14 Error comparison of y direction (vehicle 2)

对车载实验数据进行分析，得到两次实验平均定位误差如表1 所示。从表1 可以看出，未使用先验地图的定位精度远低于使用先验地图的定位精度，并且在系统运行过程中，GPS 先验地图能够提供绝对地理方位信息，从而对系统的航向误差及时修正。本文所提出包含GPS 信息的先验地图相比于无先验地图的定位方法，在x、y方向的平均定位误差分别降低了68.2%、75.1%。同时，与无GPS 的先验地图定位方法对比，在x、y方向的平均定位误差分别降低了34.9%、51.5%。

表1 车载实验平均定位误差分析Tab.1 Average location error analysis of vehicle experiment

在实验过程中，道路两旁树木的遮挡会使得GPS精度下降，如图9 中在x=100m附近真值轨迹曲线出现波动，与真实轨迹出现偏差，而在先验地图辅助下的VI-SLAM 能得到更接近实际的平滑轨迹，说明VI-SLAM 可以补偿GPS 的精度下降。为验证算法的有效性和普适性，在地面实验的基础上，进一步开展了低空飞行实验。

3.2 低空飞行实验

实验预设无人机飞行轨迹为4×400m的正方形，飞行高度为300m，为使实验中获得较丰富的图像特征，实验设备采用斜向下45 °来采集数据。需要注意的是，实际轨迹会由于受到环境的影响而与预设飞行轨迹出现偏差，以GPS 真值作为参考，我们分别比较了无先验地图、无GPS 的先验地图、有GPS 的先验地图条件下的定位效果，实验结果如图15-18 所示。

图15 飞行实验一轨迹图（x-y-z 三维图）Fig.15 Illustration of the flight trajectory 1（x-y-z）

图16 飞行实验一轨迹图（x-y 平面图）Fig.16 Illustration of the flight trajectory 1（x-y）

图17 飞行实验二轨迹图（x-y-z 三维图）Fig.17 Illustration of the flight trajectory 2（x-y-z）

图18 飞行实验二轨迹图（x-y 平面图）Fig.18 Illustration of the flight trajectory 2（x-y）

取无人机飞行一圈的位置估计结果，对两次飞行实验x、y、z三个方向上位置估计结果分别进行误差分析，如图19-24 所示。

图19 x 方向的位置误差对比图（飞行实验一）Fig.19 Error comparison of x direction (flight 1)

图20 y 方向的位置误差对比图（飞行实验一）Fig.20 Error comparison of y direction (flight 1)

图21 z 方向的位置误差对比图（飞行实验一）Fig.21 Error comparison of z direction (flight 1)

图22 x 方向的位置误差对比图（飞行实验二）Fig.22 Error comparison of x direction (flight 2)

图23 y 方向的位置误差对比图（飞行实验二）Fig.23 Error comparison of y direction (flight 2)

图24 z 方向的位置误差对比图（飞行实验二）Fig.24 Error comparison of z direction (flight 2)

对机载实验数据进行统计，得到两次实验平均误差如表2 所示。从表2 可以看出，本文所提出基于GPS的先验地图辅助视觉惯性定位方法，有效降低了视觉惯性导航系统的定位误差，相比于无先验地图的定位方法，三个方向的平均定位误差分别降低了79.4%、83.8%、46.4%；与无GPS 的先验地图定位方法对比，三个方向的平均定位误差分别降低了57.5%、52.0%、19.4%。

表2 飞行实验平均定位误差分析Tab.2 Average location error analysis of flight experiment

4 结论

针对视觉惯性导航系统在大范围环境中定位精度较低、航向缺失且无法提供绝对地理位置的问题，本文采用融合GPS 信息的先验地图辅助系统定位。离线处理得到每一帧图像采集点处的GPS 地理坐标，再求得相对于相机参考帧的相对坐标，并与关键帧信息同步来构建先验地图。利用先验地图重定位时，基于词袋模型建立先验图像帧与当前帧的联系，利用最小二乘法进行系统的位置姿态估计。实验表明，本文方法使得视觉惯性导航系统在地面和低空大范围环境中，能估计绝对位置并有效降低航向误差，从而提高定位精度。

本文的研究是在光照情况良好的大范围室外环境下开展，而全天候的视觉惯性导航系统在航空航天、国防军事等领域具有广阔应用前景，之后将在导航系统中融入红外视觉传感器或微光传感器，将其与惯性测量元件进行紧耦合，构建全天候的导航系统。