CN116839600A - 一种基于轻量化点云地图的视觉测绘导航定位方法 - Google Patents

Abstract

本发明公开了一种基于轻量化点云地图的视觉测绘导航定位方法，包括步骤如下：S1，测绘车采集自身地理位置和环境图像信息并进行时间同步；S2，构建地理坐标系下的视觉点云地图；S3，通过提取定位关键帧，轻量化点云地图；S4，对相似定位关键帧进行融合处理，剔除冗余信息；S5，基于增强后的定位关键帧，构建视觉基准库；S6，采集车辆所处环境图像信息并生成局部点云，与点云地图进行匹配；S7，初匹配成功后，启动视觉里程计并赋予其初始位姿，从此帧开始持续推算车辆位姿；S8，将点云地图匹配定位结果和视觉里程计航位推算结果进行滤波融合，输出车辆位姿。本发明能避免匹配失败导致的定位失败问题，为车辆提供长航时定位。

Description

一种基于轻量化点云地图的视觉测绘导航定位方法

技术领域

本发明涉及地图测绘和导航定位领域，尤其涉及一种基于轻量化点云地图的视觉测绘导航定位方法。

背景技术

传统的车辆自主导航主要应用惯性/卫星组合导航技术，但在卫星信号拒止情况下，惯性器件的漂移会给车辆导航带来极大的挑战。随着计算机硬件的迭代升级和数据处理技术的发展，引入数字地图匹配环境路标成为无卫星环境下的有效定位方式。

基于视觉的数字地图匹配方式主要分为基于图像的全景地图匹配和基于描述子的点云地图匹配。全景地图包含环境整体图像信息，不容易受到光照、季节变换等环境因素带来的影响，但是定位精度较低，且无法获得车辆准确的位姿。点云地图包含环境中的视觉特征点及其描述子，综合运用RANSAC(Random Sample Consensus)估值法、卡方检测和光束平差法可以有效剔除伪点，获取车辆的最优位姿估计。现阶段，用于匹配的数字地图往往数据量大，在大范围场景下对存储空间的要求较高。同时，在行驶过程中无法保证任一时刻均可以进行有效匹配，无法满足长时间的行驶定位需求。

发明内容

发明目的：本发明的目的是提供一种能有效地避免匹配失败导致的定位失败问题，为车辆提供大范围、长航时、高精度定位的基于轻量化点云地图的视觉测绘导航定位方法。

技术方案：本发明的视觉测绘导航定位方法，包括步骤如下：

S1，测绘车采集自身地理位置信息和环境图像信息并进行时间同步；

S2，构建地理坐标系下的视觉点云地图；

S3，通过提取定位关键帧，轻量化点云地图；

S4，对相似定位关键帧进行融合，筛选有用信息并对定位关键帧进行增强处理，剔除冗余信息；

S5，基于增强后的定位关键帧，构建视觉基准库；

S6，采集车辆所处环境图像信息并生成局部点云，与点云地图进行匹配；

S7，初匹配成功后，将输出位姿赋予视觉里程计作为初始位姿，从此帧开始视觉里程计获取相机采集图像并持续推算车辆位姿；

S8，采用滤波融合法，将点云地图匹配定位结果和视觉里程计航位推算结果进行结合，输出车辆位姿。

进一步，步骤S1中，所述地理位置信息包括采集时刻车辆所处的经度、纬度、高度以及该数据的采集时间；则图像采集时刻t_f，车辆的地理位置表达式如下：

其中，距离t_f时刻前后最近的两个地理位置采集时刻分别为t_l和t_l+1，P_g(t_l+1)为t_l+1时刻的地理位置，P_g(t_l)为t_l时刻的地理位置；

所述环境图像信息包括采集的图像及采集时间。

进一步，步骤S2中，构建地理坐标系下的视觉点云地图的实现步骤如下：

S21，通过光束平差法同时优化视觉帧位姿和路标空间点位置，整体的重投影代价函数如下：

其中，u_ij表示第i个路标空间点在第j帧下的像素坐标，K为相机的内参，表示第j帧的位姿，P_i表示第i个路标空间点在世界坐标系下的位置，z_i表示该路标空间点的深度，|| ||表示范数；

S22，将地理位置信息转化到视觉世界坐标系下，从地理位置信息中引入第一误差项e_j：

其中，O_wj表示第j帧光心在世界坐标系下的位置，P_gj表示车辆定位装置在地理坐标系下的位置，为/>的平移分量；/>为地理坐标系到视觉世界坐标系的转换矩阵，表达式如下：

其中，由相机与定位装置安装的位置和角度差计算求得，/>由初始欧拉角计算求得；

S23，设计逆外参强约束方法，引入第二误差项e'：

其中，为第j帧的外参，/>为/>的逆，[]^∨表示从四维矩阵到六维向量的变换，ln()表示自然对数；

S24，构建一个统一的代价函数C，此代价函数将所有的误差项进行整合，表达式如下：

其中，ρ_ij、ρ_j和ρ'表示鲁棒核函数，Ω_ij、Ω_j和Ω'分别表示重投影误差、地理位置误差和逆外参误差相关联的协方差矩阵，T表示转置矩阵；

S25，建图完成后，经转换成地理坐标系下的视觉点云地图。

进一步，步骤S3中，提取定位关键帧的步骤如下：

S31，提取候选图像帧；

S32，对于候选图像帧，取其15帧范围以内的关键帧，并选择能观测到路标空间路标点数目最多的关键帧作为定位关键帧。

进一步，提取候选图像帧的筛选准则需满足如下任一条件：

A1，当车辆处于长时间直线行驶状态时，SLAM方法对车辆位置的估计值和定位装置的定位结果之间的位置差超过了空间位置差的阈值Th_D；

A2，当车辆处于转弯状态时，SLAM方法对航向角的估计值与定位装置推算的航向角的角度差超过了空间角度差的阈值Th_A；

A3，当车辆不满足A1或A2中的阈值判断条件，且距离上次筛选图像帧后，累积行驶距离超过了距离阈值Th_L。

进一步，步骤S4中，对相似定位关键帧进行融合的实现步骤如下：

S41，对于筛选出的定位关键帧，进行词袋检测，若有相似度较高的定位关键帧，且满足其与最相近的两例相似关键帧位置偏差在阈值范围之内，或航向角偏差在阈值范围之内，则将此定位关键帧标记为冗余关键帧；

S42，对于两例相似关键帧，分别计算与冗余关键帧共同观测到的路标空间点数目，选择共同观测较多的标记为待融合关键帧；

S43，设冗余关键帧位姿为其可观测路标空间点在此关键帧下观测位置为P_r＝(X_r,Y_r,Z_r),r＝0,1,…,Num_r，其中Num_r表示冗余关键帧可观测的路标空间点数量；待融合关键帧位姿为/>其可观测路标空间点在待融合关键帧下观测位置为P_fuse＝(X_fuse,Y_fuse,Z_fuse),fuse＝0,1,…,Num_fuse，其中Num_fuse表示待融合关键帧可观测的路标空间点数量，在成像平面的像素坐标为P_uvfuse＝(u_fuse,v_fuse)；

将P_r依次投影到待融合关键帧内，其在成像平面内坐标如下：

其中，z_r表示路标空间点的深度；

在设定阈值th范围内搜索与路标空间点相似的关键点；

若有成功匹配的像素点，且该像素点没有对应的路标空间点，则在待融合关键帧中添加对此路标空间点的观测；若成功匹配的像素点有对应的路标空间点，则选择观测数目较多的路标空间点。

进一步，步骤S5中，所述视觉基准库包含筛选、增强处理后的定位关键帧集合{KFs}及其所能观察到的地图点集合{Mps}，对于能被不同定位关键帧观测到的地图点，仅保存一次。

进一步，步骤S6中，与点云地图进行匹配的实现步骤如下：

S61，根据车辆所处的大致位置，加载该位置周围区域性预先构建的轻量化点云地图；

S62，计算图像帧词袋向量，遍历所有的候选定位关键帧进行快速匹配，同时利用匹配结果估算采集图像帧位姿；

S63，估算成功后，将候选关键帧所观测的路标空间点投影至当前帧，采用BA算法优化，经卡方检测去除外点；

S64，若内点的数目多于阈值，则输出当前帧有效位姿；否则，舍弃此候选关键帧；

若所有的候选关键帧遍历结束仍没有输出有效位姿，则判定匹配失败。

进一步，步骤S8中，将点云地图匹配定位结果和视觉里程计航位推算结果进行结合的实现步骤如下：

S81，自初匹配成功后，视觉里程计不断根据帧间匹配关系计算位姿增量，每一时刻均输出车辆位姿推算结果；

S82，每一时刻输入的图像均进行点云地图匹配，若匹配定位失败，在前一次融合定位结果基础上依据里程计输出递推计算车辆定位结果；

若匹配定位成功，依据各自置信度融合对视觉里程计航位推算结果和点云地图匹配定位结果进行滤波融合，计算车辆定位结果；视觉里程计航位推算的实现过程如下：

将k-1时刻和k时刻相机采集的两帧图像输入视觉里程计，视觉里程计输出两帧间的增量运动估计，定义增量运动估计为δ_k,k-1∈R⁶：

其中，ρ∈R³表示平移分量，φ∈R³表示旋转分量，R表示实数集；

视觉里程计通过k-1时刻的车辆状态x_k-1∈R⁶和增量运动估计δ_k,k-1来预测车辆在k时刻的状态x_k，表达式如下：

x_k＝ln(exp(x_k-1 ^∧)exp(δ_k,k-1 ^∧))^∨

根据误差传播定律计算协方差矩阵：

其中，exp()表示自然指数函数，()^{^}表示从六维向量到四维矩阵的变换。

本发明与现有技术相比，其显著效果如下：

1、本发明设计了一种轻量化点云地图的自动化测绘方案，通过车载传感器自动采集车辆位置及环境图像信息，能自动完成地图构建，减少测绘过程所需人力物力，提高数字地图构建效率；

2、本发明设计了新的误差项形式，通过在点云地图构建过程中融入高精度地理位置信息，能够有效约束点云地理累积误差，提高地图构建精度；

3、本发明设计了逆外参强约束方法，赋予不同误差项自适应权重，通过逆外参固有属性紧密耦合各类误差项，能够较好地平衡代价函数内各误差项影响，获得较好的优化效果；

4、本发明设计了相似关键帧判定及融合方法，通过词袋检测联合阈值判断准则筛选相似关键帧，对其观测路标空间点进行分类融合，能够轻量化视觉点云地图并增强有益信息，减少视觉点云地图的冗余数据量，进一步提高地图构建精度；

5、本发明的融合滤波算法融合了点云匹配定位结果和视觉里程计航位推算结果，不间断输出车辆位置信息，能有效地避免了匹配失败导致的定位失败问题，能够为车辆提供长航时、高精度定位。

附图说明

图1为本发明的系统框架图；

图2为全局优化因子图；

图3为冗余关键帧示意图；

图4为匹配点无路标空间点索引的相似关键帧融合示意图；

图5为匹配点存在路标空间点索引的相似关键帧融合示意图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

下面结合说明书附图和具体实施方式对本发明做进一步详细描述。

如图1所示，为本发明的系统框架图，其本质是设计一种基于轻量化点云地图的视觉测绘导航定位方法，该方法分为测绘阶段和导航阶段：

在测绘阶段，通过测绘车所携带的高精度定位装置和图像采集装置同步获取车辆所处的地理位置和环境图像信息，利用高精度地理位置信息消除视觉点云地图累积误差并将其转换至地理坐标系下，自动生成地理坐标系下的高精度视觉点云地图并进行轻量化筛选和融合增强处理，构建视觉基准库。

在定位阶段，采集实时图像生成局部点云，与测绘阶段生成的轻量化点云地图匹配，首次匹配成功后启动视觉里程计并赋予其初始位姿，通过滤波算法融合匹配定位结果和视觉里程计航位推算结果，为车辆提供大范围、长航时、高精度定位。

下面进行详细说明，首先是测绘阶段，步骤如下：

步骤1，测绘车采集自身地理位置信息和环境图像信息并进行时间同步；

测绘车地理位置信息来自于车载高精度定位装置，例如双天线RTK(可获得厘米级定位结果并计算欧拉角)，包括采集时刻车辆所处的经度、纬度、高度以及该数据的采集时间。环境图像信息来自于车辆前置双目相机，包括采集的图像及采集时间。由于定位装置和图像采集设备的初始化时间不同，采集频率不同，本发明采用业内常用的邻域插值法进行时间同步。假设某一图像采集时间为t_f，距离t_f前后最近的2个地理位置采集时刻分别为t_l和t_l+1，则图像采集时刻t_f，车辆的地理位置如式(1)所示：

其中，P_g(t_l+1)为t_l+1时刻的地理位置，P_g(t_l)为t_l时刻的地理位置。

步骤2，构建地理坐标系下的视觉点云地图；

经典SLAM(Simultaneous localization and mapping，同步定位与建图)方法包括前端里程计、后端(非线性优化)、回环检测和建图四大模块，可用于构建视觉点云地图，其以初始帧的相机坐标系作为世界坐标系，路标空间点的世界坐标系位姿记为T_wi,i＝0,1,2,…n，n为路标空间点的总数，包含旋转分量R_wi和平移分量t_wi。车载高精度定位装置获取车辆纬经高数据，可转化为以初始位置为起点的东北天坐标数据，路标空间点的地理坐标系位姿记为T_gi,i＝0,1,2,…n，包含旋转分量R_gi和平移分量t_gi。世界坐标系与东北天坐标系之间通过机体系(b系)进行关联，其中可由相机与定位装置安装的位置和角度差计算求得，/>由初始欧拉角计算求得，因此可建立视觉世界坐标系与东北天地理坐标系之间的关联：/>

本发明将地理位置信息转化到视觉世界坐标系下，在传统光束平差法的基础上加入新的误差项，约束SLAM方法的累积误差，建图完成后经转换成地理坐标系下的视觉点云地图。传统光束平差法可同时优化视觉帧位姿和路标空间点位置，整体的重投影代价函数e_ij如式(2)所示：

其中，u_ij表示第i个路标空间点在第j帧下的像素坐标，K为相机的内参，表示第j帧的位姿，P_i表示第i个路标空间点在世界坐标系下的位置，z_i表示该路标空间点的深度，||||表示范数。

在此基础上，本发明从地理位置信息中引入新的第一误差项e_j，如式(3)所示：

其中，O_wj表示第j帧光心在世界坐标系下的位置，P_gj表示车辆定位装置在地理坐标系下的位置，为地理坐标系到视觉世界坐标系的转换矩阵，/>为/>的平移分量。

如式(2)所示，重投影误差约束如式(3)所示，地理位置信息约束/>即/>的平移分量，赋予重投影误差的权重太高会降低地理信息的作用，赋予第一误差项的权重太高会降低点云中路标空间点的位置精度。为了平衡两者并获得最佳优化效果，本发明设计逆外参强约束方法，对于待优化的帧，其外参为/>求取逆为/>引入第二误差项e'如式(4)所示，

其中[]^∨表示从四维矩阵到六维向量的变换，ln()表示自然对数。

本发明设计构建一个统一的代价函数，此函数将所有的误差项进行整合，如式(5)所示：

其中，ρ_ij、ρ_j和ρ'表示鲁棒核函数，Ω_ij、Ω_j和Ω'分别表示重投影误差、地理位置误差和逆外参误差相关联的协方差矩阵，T表示转置矩阵，Ω_ij的比重为1，Ω_j的比重为重投影误差边的数目的十分之一，Ω'的比重为重投影误差边的数目，在保证逆外参强约束的同时能够有效平衡重投影和地理位置信息带来的影响。

全局优化因子图如图2所示，其中各个顶点表示优化变量，各条边表示误差项，可以清晰地看出本步骤所要优化的4类变量及各变量之间的误差构成关系。

步骤3，轻量化点云地图，提取定位关键帧；

在经典SLAM算法中，关键帧可以降低信息冗余度，减少计算机资源的损耗，保证系统的平稳运行。建图过程中，关键帧的筛选主要由不同帧之间的内点数、前序关键帧插入的时刻以及局部建图线程的工作状态共同决定，关键帧的筛选研究较为成熟，可沿用业内已有的判断准则。

定位关键帧，顾名思义指用于定位的关键帧。本发明在关键帧筛选的基础上，提出定位关键帧筛选准则，对原始点云地图进行轻量化处理。首先提取候选图像帧，筛选准则需满足如下任一条件：

1)当车辆处于长时间直线行驶状态时，SLAM方法对车辆位置的估计值和定位装置的定位结果之间的位置差超过了空间位置差的阈值Th_D；

2)当车辆处于转弯状态时，SLAM方法对航向角的估计值与定位装置推算的航向角的角度差超过了空间角度差的阈值Th_A；

3)当车辆不满足1)或2)中的阈值判断条件，且距离上次筛选图像帧后，累积行驶距离超过了距离阈值Th_L。

对于候选图像帧，取其15帧范围以内的关键帧，并选择能观测到路标空间点数目最多的关键帧作为定位关键帧。

需要特别说明的是，条件1)和条件2)所说的偏差，在筛选候选图像帧后归零，防止累积误差导致后续图像帧均满足筛选条件，影响轻量化结果。

需要特别说明的是，回环检测能够有效抑制累积误差，在完成回环检测后，需要重新对差值条件进行设置。

步骤4，融合增强相似定位关键帧；

在测绘过程中，测绘车不可避免会经过重复路段，由于前置环境不同，筛选出的定位关键帧会显得冗余，如图3所示。

对于筛选出的定位关键帧，首先进行词袋检测，若有相似度较高的定位关键帧，且满足其与最相近的两例相似关键帧位置偏差在阈值范围之内，或航向角偏差在阈值范围之内，则将此定位关键帧标记为冗余关键帧。对于两例相似关键帧，分别计算与冗余关键帧共同观测到的路标空间点数目，选择共同观测较多的标记为待融合关键帧。定位关键帧的融合主要是融合其内路标空间点，假设冗余关键帧位姿为其可观测路标空间点在此关键帧下观测位置为P_r＝(X_r,Y_r,Z_r),r＝0,1,…,Num_r，其中Num_r表示冗余关键帧可观测的路标空间点数量；待融合关键帧位姿为/>其可观测路标空间点在待融合关键帧下观测位置为P_fuse＝(X_fuse,Y_fuse,Z_fuse),fuse＝0,1,…,Num_fuse，其中Num_fuse表示待融合关键帧可观测的路标空间点数量，在成像平面的像素坐标为P_uvfuse＝(u_fuse,v_fuse)。

将P_r依次投影到待融合关键帧内，其在成像平面内坐标如下：

其中，z_r表示路标空间点的深度。

设定阈值th，在此范围内搜索与路标空间点P_r相似的关键点。如图4所示，若投影点P_uvr有成功匹配的像素点P′_uvr，且P′_uvr没有对应的路标空间点，则在待融合关键帧中添加P′_uvr对路标空间点P_r的观测。如图5所示，若P′_uvr存在对应的路标空间点P′_r，则选择P_r与P′_r之间能被较多关键帧观测的作为P_uvr和P′_uvr所对应的路标空间点。

步骤5，构建视觉基准库；

视觉基准库包含筛选、增强处理后的定位关键帧集合{KFs}及其所能观察到的地图点集合{Mps}，对于能被不同定位关键帧观测到的地图点，仅保存一次。

步骤6，采集车辆所处环境图像信息并生成局部点云，与点云地图进行匹配；

在定位阶段，环境图像信息依然来自于车辆前置双目相机，包括采集的图像以及采集时间。在定位的初始阶段，本发明假设已知车辆所处的大致位置，并成功加载该位置周围一定范围内，预先构建的轻量化点云地图。

对于前置双目相机采集的图像帧，首先提取其内特征点并计算对应描述子，接着计算图像帧词袋向量并从定位关键帧库中选取候选定位关键帧。遍历所有的候选定位关键帧，通过词袋节点进行快速匹配，同时利用匹配结果估算采集图像帧位姿。估算成功后将候选关键帧所观测的路标空间点投影至当前帧，利用BA(Bundle Adjustment光束法平差)算法进行优化，经卡方检测去除外点；若内点的数目多于阈值(例如50)，则输出当前帧有效位姿；否则，舍弃此候选关键帧。若所有的候选关键帧遍历结束仍没有输出有效位姿，则判定匹配失败。

步骤7，初匹配成功后，将输出位姿赋予视觉里程计作为初始位姿，从此帧开始视觉里程计获取相机采集图像并持续推算车辆位姿；

视觉里程计通常被称为使用单个或多个相机逐步估计车辆自我运动的问题，初匹配成功后将匹配输出位姿赋予视觉里程计作为初始位姿，从此帧开始视觉里程计获取相机采集图像；经特征提取、特征匹配、帧间位姿恢复和位姿优化等步骤后，可获得帧间差值，递推式推算车辆位姿。

步骤8，采用滤波融合法，将点云地图匹配定位结果和视觉里程计航位推算结果进行结合，输出车辆位姿；

视觉里程计作为一种典型的航位推算方法，相邻两帧之间的增量计算误差较小，但随着车辆行驶距离的增加，所产生的累积误差会导致定位结果漂移。点云地图匹配定位算法的全局定位结果能克服增量位姿漂移，同时在跟踪丢失的情况下能够恢复车辆位姿，但误匹配的存在会导致灾难性的定位偏差。本发明使用一种滤波融合方法，综合考虑视觉里程计的短时精确性和点云地图匹配定位算法的全局一致性，两者相辅相成，为车辆提供长航时、高精度定位。

本发明采用经典非线性卡尔曼滤波方法，自初匹配成功后，视觉里程计不断根据帧间匹配关系计算位姿增量，每一时刻均输出车辆位姿推算结果。同时，每一时刻均进行点云地图匹配定位，若匹配定位失败，在前一次融合定位结果基础上依据里程计输出递推计算车辆定位结果；若匹配定位成功，依据各自置信度融合视觉里程计航位推算结果和点云地图匹配定位结果计算车辆定位结果。通过滤波融合方法，一方面能够抑制视觉里程计累积误差，另一方面能够减少误匹配发生的概率，接下来着重分析视觉里程计航位推算结果和点云地图匹配定位结果的不确定性。

分别将k-1时刻和k时刻相机采集的两帧图像输入视觉里程计，视觉里程计输出两帧间的增量运动估计，定义这个增量运动估计为δ_k,k-1∈R⁶，具体如式(7)所示：

其中ρ∈R³表示平移分量，φ∈R³表示旋转分量，R表示实数集。

视觉里程计通过k-1时刻的车辆状态x_k-1∈R⁶和增量运动估计δ_k,k-1来预测车辆在k时刻的状态x_k，状态的不确定性由6*6的协方差矩阵表示。

选取视觉里程计输出两帧间的增量运动估计作为样本，对于随机抽取的大量样本{X₁,X₂,…,X_n}，均值和协方差∑_X由式(8)和式(9)进行估计：

其中，N表示样本数量。

对于相邻待匹配的两帧图像，本发明从中随机采样8组帧间特征匹配关系，代入RANSAC算法进行计算，内点数超过阈值的帧间位姿估计(即两帧间的增量运动估计)被保留在候选集合中。从候选中随机抽取大量样本，可获得大量有效的估计以计算增量的均值和协方差。点云匹配定位算法的不确定性计算与帧间匹配相同，不再过多描述。

视觉里程计的误差随着位姿推算不断传播，k时刻的车辆状态x_k取决于k-1时刻车辆状态x_k-1和增量运动估计δ_k,k-1，即

x_k＝ln(exp(x_k-1 ^{^})exp(δ_k,k-1 ^{^}))^∨ (10)

根据误差传播定律计算协方差矩阵：

其中，exp()表示自然指数函数，()^表示从六维向量到四维矩阵的变换。

在点云地图匹配成功时，依据各自置信度融合视觉里程计航位推算定位结果和点云地图匹配定位结果计算车辆定位结果，其中协方差矩阵数值小的定位结果将获得较大的融合权重。

Claims (10)

1.一种基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，包括步骤如下：

S1，测绘车采集自身地理位置信息和环境图像信息并进行时间同步；

S2，构建地理坐标系下的视觉点云地图；

S3，通过提取定位关键帧，轻量化点云地图；

S4，对相似定位关键帧进行融合，筛选有用信息并对定位关键帧进行增强处理，剔除冗余信息；

S5，基于增强后的定位关键帧，构建视觉基准库；

S6，采集车辆所处环境图像信息并生成局部点云，与点云地图进行匹配；

S7，初匹配成功后，将输出位姿赋予视觉里程计作为初始位姿，从此帧开始视觉里程计获取相机采集图像并持续推算车辆位姿；

S8，采用滤波融合法，将点云地图匹配定位结果和视觉里程计航位推算结果进行结合，输出车辆位姿。

2.根据权利要求1所述基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，步骤S1中，所述地理位置信息包括采集时刻车辆所处的经度、纬度、高度以及该数据的采集时间；则图像采集时刻t_f，车辆的地理位置表达式如下：

其中，距离t_f时刻前后最近的两个地理位置采集时刻分别为t_l和t_l+1，P_g(t_l+1)为t_l+1时刻的地理位置，P_g(t_l)为t_l时刻的地理位置；

所述环境图像信息包括采集的图像及采集时间。

3.根据权利要求1所述基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，步骤S2中，构建地理坐标系下的视觉点云地图的实现步骤如下：

S21，通过光束平差法同时优化视觉帧位姿和路标空间点位置，整体的重投影代价函数如下：

其中，u_ij表示第i个路标空间点在第j帧下的像素坐标，K为相机的内参，表示第j帧的位姿，P_i表示第i个路标空间点在世界坐标系下的位置，z_i表示该路标空间点的深度，||||表示范数；

S22，将地理位置信息转化到视觉世界坐标系下，从地理位置信息中引入第一误差项e_j：

其中，O_wj表示第j帧光心在世界坐标系下的位置，P_gj表示车辆定位装置在地理坐标系下的位置，为/>的平移分量；/>为地理坐标系到视觉世界坐标系的转换矩阵，表达式如下：

其中，由相机与定位装置安装的位置和角度差计算求得，/>由初始欧拉角计算求得；

S23，设计逆外参强约束方法，引入第二误差项e'：

其中，为第j帧的外参，/>为/>的逆，[]^∨表示从四维矩阵到六维向量的变换，ln()表示自然对数；

S24，构建一个统一的代价函数C，此代价函数将所有的误差项进行整合，表达式如下：

其中，ρ_ij、ρ_j和ρ'表示鲁棒核函数，Ω_ij、Ω_j和Ω'分别表示重投影误差、地理位置误差和逆外参误差相关联的协方差矩阵，T表示转置矩阵；

S25，建图完成后，经转换成地理坐标系下的视觉点云地图。

4.根据权利要求1所述基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，步骤S3中，提取定位关键帧的步骤如下：

S31，提取候选图像帧；

S32，对于候选图像帧，取其15帧范围以内的关键帧，并选择能观测到路标空间点数目最多的关键帧作为定位关键帧。

5.根据权利要求4所述基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，提取候选图像帧的筛选准则需满足如下任一条件：

A1，当车辆处于长时间直线行驶状态时，SLAM方法对车辆位置的估计值和定位装置的定位结果之间的位置差超过了空间位置差的阈值Th_D；

A2，当车辆处于转弯状态时，SLAM方法对航向角的估计值与定位装置推算的航向角的角度差超过了空间角度差的阈值Th_A；

A3，当车辆不满足A1或A2中的阈值判断条件，且距离上次筛选图像帧后，累积行驶距离超过了距离阈值Th_L。

6.根据权利要求1所述基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，步骤S4中，对相似定位关键帧进行融合的实现步骤如下：

S41，对于筛选出的定位关键帧，进行词袋检测，若有相似度较高的定位关键帧，且满足其与最相近的两例相似关键帧位置偏差在阈值范围之内，或航向角偏差在阈值范围之内，则将此定位关键帧标记为冗余关键帧；

S42，对于两例相似关键帧，分别计算与冗余关键帧共同观测到的路标空间点数目，选择共同观测较多的标记为待融合关键帧；

S43，设冗余关键帧位姿为其可观测路标空间点在此关键帧下观测位置为P_r＝(X_r,Y_r,Z_r),r＝0,1,…,Num_r，其中Num_r表示冗余关键帧可观测的路标空间点数量；待融合关键帧位姿为/>其可观测路标空间点在待融合关键帧下观测位置为P_fuse＝(X_fuse,Y_fuse,Z_fuse),fuse＝0,1,…,Num_fuse，其中Num_fuse表示待融合关键帧可观测的路标空间点数量，在成像平面的像素坐标为P_uvfuse＝(u_fuse,v_fuse)；

将P_r依次投影到待融合关键帧内，其在成像平面内坐标如下：

其中，z_r表示路标空间点的深度；

在设定阈值th范围内搜索与路标空间点相似的关键点；

若有成功匹配的像素点，且该像素点没有对应的路标空间点，则在待融合关键帧中添加对此路标空间点的观测；若成功匹配的像素点有对应的路标空间点，则选择观测数目较多的路标空间点。

7.根据权利要求5所述基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，步骤S5中，所述视觉基准库包含筛选、增强处理后的定位关键帧集合{KFs}及其所能观察到的地图点集合{Mps}，对于能被不同定位关键帧观测到的地图点，仅保存一次。

8.根据权利要求5所述基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，步骤S6中，与点云地图进行匹配的实现步骤如下：

S61，根据车辆所处的大致位置，加载该位置周围区域性预先构建的轻量化点云地图；

S62，计算图像帧词袋向量，遍历所有的候选定位关键帧进行快速匹配，同时利用匹配结果估算采集图像帧位姿；

S63，估算成功后，将候选关键帧所观测的路标空间点投影至当前帧，采用BA算法优化，经卡方检测去除外点；

S64，若内点的数目多于阈值，则输出当前帧有效位姿；否则，舍弃此候选关键帧；

若所有的候选关键帧遍历结束仍没有输出有效位姿，则判定匹配失败。

9.根据权利要求1所述基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，步骤S8中，将点云地图匹配定位结果和视觉里程计航位推算结果进行结合的实现步骤如下：

S81，自初匹配成功后，视觉里程计不断根据帧间匹配关系计算位姿增量，每一时刻均输出车辆位姿推算结果；

S82，每一时刻输入的图像均进行点云地图匹配，若匹配定位失败，在前一次融合定位结果基础上依据里程计输出递推计算车辆定位结果；

若匹配定位成功，依据各自置信度对视觉里程计航位推算结果和点云地图匹配定位结果进行滤波融合，计算车辆定位结果。

10.根据权利要求9所述基于轻量化点云地图的视觉测绘导航定位方法，其特征在于，所述视觉里程计航位推算的实现过程如下：

将k-1时刻和k时刻相机采集的两帧图像输入视觉里程计，视觉里程计输出两帧间的增量运动估计，定义增量运动估计为δ_k,k-1∈R⁶：

其中，ρ∈R³表示平移分量，φ∈R³表示旋转分量，R表示实数集；

视觉里程计通过k-1时刻的车辆状态x_k-1∈R⁶和增量运动估计δ_k,k-1来预测车辆在k时刻的状态x_k，表达式如下：

x_k＝ln(exp(x_k-1 ^∧)exp(δ_k,k-1 ^∧))^∨

根据误差传播定律计算协方差矩阵：

其中，exp()表示自然指数函数，()^∧表示从六维向量到四维矩阵的变换。