CN109631855A - 基于orb-slam的高精度车辆定位方法 - Google Patents

Abstract

本发明公开了一种基于ORB‑SLAM的高精度车辆定位方法，主要解决当前ORB‑SLAM经典定位算法定位结果精度不高的问题。其实现步骤为：选取标定板对双目相机进行标定，并对摄取到的图像进行立体校正；检测校正图像中的ORB特征点并完成特征点提取；利用双目稀疏特征匹配方法将提取到的特征点进行匹配，再利用相邻帧特征跟踪的方法获取当前相机位姿信息，构建局部地图；对建立的局部地图进行闭环检测与全局优化，以完成视觉地图的建立，并保存视觉地图；根据图像中匹配特征点数量选择车辆定位方案，并通过读取视觉地图确定出目标车辆的最终位置。本发明提高了车辆定位精度和鲁棒性，可用于无人驾驶汽车人工智能管理及安全处置。

Description

基于ORB-SLAM的高精度车辆定位方法

技术领域

本发明属于图像处理技术领域，特别涉及一种车辆定位方法，可用于无人驾驶汽车人工智能管理及安全处置。

背景技术

车辆定位技术是无人驾驶汽车研究中的关键技术，传统的定位方案，例如载波相位差分技术与磁导航技术，部署成本高且使用范围受限。同时定位与建图技术SLAM根据传感器信息，计算自身位姿，同时构建地图，获得高精度定位结果，成为目前车辆定位技术的研究热点。同时定位与建图技术SLAM可以描述为：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边逐步描绘出此环境完全的地图，同时一边决定机器人应该往哪个方向行进。当前这种同时定位与建图技术视觉SLAM主要方法分为三种：单目视觉的同时定位与建图技术SLAM、双目视觉的同时定位与建图技术SLAM和红绿蓝RGB的同时定位与建图技术DSLAM。其中：

单目视觉的同时定位与建图技术SLAM，是单目相机发展成熟、应用灵活、简单、成本低。但是，单目视觉的同时定位与建图技术SLAM在每个时刻只能获取一张图像，且只能依靠获得的图像数据计算环境物体的方向信息，无法直接获得可靠的深度信息，因此，地图初始创建及特征点的深度恢复都比较困难。此外，尺度不确定性是单目视觉的同时定位与建图技术SLAM的主要特点，一方面它是主要的误差源之一，另一方面尺度不确定性使得单目视觉的同时定位与建图技术SLAM能够在大尺度和小尺度环境之间进行自由转换。

双目视觉的同时定位与建图技术SLAM，是利用外极线几何约束的原理去匹配左右两个相机的特征，从而能够在当前帧速率的条件下直接提取完整的特征数据，因而应用比较广泛，它直接解决了系统地图特征的初始化问题。但是系统设计比较复杂，系统成本比较高，且它的视角范围受到限制，不能够获取远处的场景，从而只能在有限的尺度范围内进行可靠的测量，从而缺乏灵活性。最早研究双目视觉的同时定位与建图技术SLAM方案的是Paz等人，基于条件独立分割和扩展的同时定位与建图技术SLAM，其显著特点是能够在大场景中运行，更重要的是，这是第一个使用近特征点和远特征点的双目视觉的同时定位与建图技术SLAM系统。目前大多数双目系统都是基于特征匹配和局部BA优化的方式，来获得尺度。Strasdat等人采用在一个输出窗口的关键帧和位姿的BA联合优化算法，在全局不一致性的情况下，通过限制窗口的大小的方式，实现了约束了时间的复杂程度的目的。

红绿蓝RGB的同时定位与建图技术DSLAM，是深度相机在获得彩色图像的同时获得深度图像，从而方便获得深度信息，且能够获得稠密的地图，但是成本高，体积大，有效探测距离短，从而受应用环境的限制。

总体来看，单目视觉的同时定位与建图技术SLAM没有深度信息是其致命缺点，初始化缓慢，需要一系列的帧才可以估计位置，并且随着时间的延长会产生尺度漂移，所以很难在无人驾驶车上应用。红绿蓝RGB的同时定位与建图技术DSLAM虽然可以获得深度信息，但是有效探测距离太短，只适合室内场景，在户外大场景下很难应用。双目视觉的同时定位与建图技术SLAM可以直接获得深度信息，虽然系统设计比较复杂，计算量比较大，但是都可以通过改进算法不断优化，有效探测距离可以能满足室外大场景。从长远发展来看，双目视觉的同时定位与建图技术SLAM是未来发展的趋势。

双目视觉的同时定位与建图技术SLAM中，将特征提取检测与匹配算法ORB与同时定位与建图技术SLAM相结合的算法ORB-SLAM是一种较为成熟的算法，但是该算法由于没有实现地图保存与读取的功能，因而不能在预先加载地图的情况下对无人驾驶汽车实现全局定位，即无法在无人驾驶汽车行驶过程中获得准确的位置信息。

发明内容

本发明的目的在于针对上述已有技术的不足，提出一种基于ORB-SLAM的高精度车辆定位算法，以获得高精度的车辆定位结果，实现在无人驾驶汽车行驶过程中使用准确的位置信息。

为实现上述目的，本发明的技术方案包括如下：

(1)选取标定板，进行双目摄像机标定，检测图像角点，通过标定实验选择标定效果最好的一次，并调用图像校正函数进行图像校正；

(2)在完成相机标定和图像校正后，提取均匀分布的特征点检测角点FAST,得到旋转多尺度角点ORB特征描述子并计算FAST角点的方向；

(3)在得到图像中旋转多尺度角点ORB特征描述子之后，进行双目稀疏特征的匹配,令匹配到特征点的个数为M，M＝0，1，2，…，并在匹配完成后追踪车辆运动过程中的相邻帧特征，确定当前相机位姿；

(4)建立局部地图：

根据追踪运动车辆所确定的相机位姿中所包含的关键帧，建立共视图，在下一刻的运动中以同样的方法获得新的关键帧并插入到共视图中，更新共视图；

通过对来自共视图中相连关键帧的旋转多尺度角点ORB特征进行三角运算创建新的地图点，同时对共视图进行优化并剔除多余的关键帧以完成局部地图的建立；

(5)对建立的局部地图进行闭环检测与全局优化，实现双目视觉ORB-SLAM视觉地图的建立；

(6)在跟踪运动车辆的过程中，对ORB-SLAM视觉地图进行实时保存与读取，并利用基于SLAM视觉地图的定位算法进行跟踪车辆的实时定位。

本发明与现有技术相比具有以下优点：

1.本发明由于使用的传感器包括激光雷达和视觉传感器，其中双目视觉传感器相比单目视觉传感器，可以获得深度信息，相比激光雷达，成本较低而且环境信息丰富。

2.本发明所涉及到的双目SLAM技术中，使用较为成熟的ORB-SLAM算法框架，并针对该算法没有实现地图保存与读取的功能，不能在预先加载地图的情况下进行全局定位的不足，在ORB-SLAM建立好环境地图后，对ORB-SLAM视觉地图进行实时保存与读取，使ORB-SLAM更加适用于车辆定位。

3.本发明由于利用基于SLAM视觉地图的定位算法进行跟踪车辆的实时定位，当系统匹配到特征点较少时，能快速、及时地在附近的关键帧中计算出一个最优位姿，且具有更高的鲁棒性，提高了定位精度。

附图说明

图1是本发明的实现流程图；

图2是用本发明中双目视觉系统对室外复杂背景视差测试的效果图；

图3是用本发明对目标车辆定位的行驶轨迹与真实行驶轨迹的精度对比图；

图4是用传统ORB-SLAM算法和本发明对目标车辆定位的绝对位置误差APE曲线对比图。

具体实施方式

以下结合附图对本发明的实施率和效果做进一步详细说明。

参照图1，本实例的具体实现步骤如下：

步骤1，选取标定板并进行标定。

选取型号为Pattern2.0的标定板，材质为KT板，8*6图案阵列，单元格尺寸为28*28mm，平行度误差小于0.05mm，水平线与基准边平行度误差小于30秒；

标定板选取好之后，再设置棋盘角点数为8×6，并选取摄像机标定函数的标志位与检测次数，检测次数设定为20次；

完成上述准备后，用OpenCV标定函数检测48个图像角点之后，再计算标定误差，重复多次标定实验，选取标定效果最好的一次；

在标定流程中，要保证两台摄像机在同一场景，对同一标定物同时进行图像采集，以避免在立体标定过程中出现无法标定的错误。

步骤2，图像立体校正。

标定完成后调用cvStereoRectify图像校正函数进行图像校正，使得左、右图像中同一事物呈现平行位置关系。

步骤3，检测ORB特征以提取FAST角点。

ORB特征是具有256位特征描述子的旋转多尺度FAST角点，具有良好的旋转不变性和鲁棒性，并且计算和匹配速度快，每张图像的特征提取时间远少于33毫秒，远超过一些流行的方法，这使得双目视觉的同时定位与建图技术SLAM系统可以在没有GPU加速的情况下实时运行；

通过双目相机获取需要处理的图像，对每张图像不断进行高斯模糊和向下采样，得到宽高均为原图像1/2的不同分辨率的图像，用这些不同分辨率的图像构成n层高斯金字塔，n≥8；

获取8层高斯金字塔，把每层图像分成网格，在网格中选取一点P，以P为圆心画一个半径为3pixel的圆，圆周上如果有连续12个像素点的灰度值比P点的灰度值大或者小，则认为P为FAST角点，并进行提取；

为了确保角点的均匀分布，每格提取至少5个角点；当原图像分辨率为512×384～752×480时，共提取1000个角点；

当分辨率高于512×384～752×480时，共提取2000个角点；

所提取到的角点中均包含ORB特征描述子，但不包含FAST角点方向。

步骤4，计算FAST角点方向。

由于在步骤3中所获取的FAST角点不包含角点方向，则需要通过进行如下计算确定角点方向：

(4a)定义矩重心m_pq：

m_pq＝∑_x,y∈rx^py^qI(x,y)，

其中，I(x,y)为图像灰度表达式，r为角点范围半径，p,q为坐标轴，(x,y)为选取角点O的坐标；

(4b)通过矩重心计算角点以r为半径范围内的质心C：

其中，m₁₀为p轴边界点的矩重心，m₀₁为q轴边界点的矩重心，m₀₀为坐标轴原点的矩重心；

(4c)将角点O坐标到质心C所形成的向量角为θ作为选取角点O的方向，其计算公式如下：

步骤5，双目稀疏特征匹配。

(5a)输入步骤3中校正后的双目图像，之后提取左右图像的旋转多尺度角点ORB特征角点；

(5b)将左右两图像以方格图划分，在左图标定一个旋转多尺度角点ORB特征点(u_L,v_L)，将右图像中的水平坐标定义为u_R，由这三个坐标共同定义立体关键点：x_s＝(u_L,v_L,u_R)，在“极线约束”的条件下沿着极线，在右图中的[u_R,u_R+dmax]的区域内寻找同名点，其中u_L为左图中的水平座标，v_L为极线位置，dmax为划分方格的宽度；

(5c)计算标定特征点和所寻找到的同名点之间的绝对位置误差，选取绝对误差最小的点，作为对应匹配点，令匹配到的特征点个数为M，M＝0，1，2，…；

(5d)对于每一对匹配的ORB特征角点，进行深度计算得到两点的空间位置关系：

其中，f_x是水平焦距，b是基线即两个摄像头之间的距离，d是两点之间的空间深度距离。

步骤6，相邻帧特征跟踪。

在匹配完成后，追踪车辆运动过程中的相邻帧特征，确定当前相机位姿，具体实施如下：

(6a)对于匀速运动车辆的追踪，根据上一帧的之前帧计算上一帧的运动速度，利用上一帧的位姿与速度估计当前帧的位姿，如果在跟踪过程目标丢失，则执行(6c)；

(6b)对于非匀速运动车辆的追踪，首先与最近一个关键帧匹配，假设当前帧和参考关键帧位姿矩阵相同，计算当前帧的词袋模型BoW，用词袋模型来加速匹配，并设定初始位姿为上一帧的位姿，再根据位姿和词袋模型BoW词典寻找特征匹配，利用匹配的特征优化位姿；如果在跟踪过程目标丢失，则执行(6c)；

(6c)计算当前帧的词袋模型BoW，并在关键帧数据库中搜索、计算与每个关键帧中的地图点关联的ORB特征点的对应关系，并对每个关键帧执行交替RANSAC迭代，尝试使用PnP算法找到当前位姿。

步骤7，根据追踪运动车辆所确定的相机位姿中所包含的关键帧信息进行局部地图的构建。

具体实施如下：

(7a)根据追踪运动车辆所确定的相机位姿中包含的关键帧，建立共视图，共视图中，每个节点都是一个关键帧，如果两个关键帧之间可以观察到至少有15个相同的地图点，关键帧之间就存在一个边缘，对于边缘上相同点的数量赋予权重η；

(7b)构建共视图生成树，简化共视图以生成稳定共视图网络。

(7b1)从第一个关键帧开始，连接边缘数量最少的共视图的子图像，连续地构建一个生成树，生成树包含η_min＝100的较高视图相关性的共视图边缘和地图点闭环回路边缘的子集；

(7b2)插入新的关键帧，并将这些关键帧连接到树中具有最多相同点的关键帧上，通过筛选策略删除无关关键帧，并根据被删除关键帧所在位置更新其他关键帧的连接；

所述筛选策略为：跟踪线程必须在25％的预测可见帧中找到这个地图点；若地图点创建时关联了多个关键帧，则至少被三个关键帧观测到；剔除余下地图点未被至少三个关键帧观测到的地图点，使地图包含非常少的异常值；

(7b3)根据(7b1)中构建的生成树来完成共视图简化，保留所有的关键帧，通过简化后共视图中共视边缘数量的减少，做到保持关键帧的连通性和共视图网络的稳定性；

(7c)在下一刻的运动中通过步骤6中再次获得新的关键帧并插入到共视图中，更新共视图；

(7d)对来自共视图中相连关键帧的旋转多尺度角点ORB特征进行三角运算，以创建新的地图点；

(7d1)测出双目相机间距x，以及双目相机与特征点的夹脚α和β，并标定焦距y及目标点投射到双目相机的相机坐标系下的坐标点A和B；

(7d2)基于三角形稳定准则，由A、B、x、y、α、β这些参数，利用三角形相似原理计算得到特征点的空间坐标，将此特征点设置为新的地图点；

(7e)将新的地图点插入到共视图中；

(7f)对共视图进行优化并剔除多余的关键帧，以完成局部地图的建立。

步骤8，对局部地图进行闭环检测与全局优化以建立视觉地图。

(8a)对闭环候选关键帧进行检测：

(8a1)计算关键帧K_i的词袋模型BoW和它在共视图中的相邻关键帧，并且设置保留的最低分为s_min；

(8a2)利用mvInvertedFile函数在关键帧数据库中搜索，丢弃所有分数低于s_min的关键帧；

(8a3)在共视图中连续检测三个一致的相联关键帧，确定出一个闭环候选帧；

(8b)对关键帧进行相似性变换计算，建立地图点闭环：

(8b1)计算与当前关键帧中地图点关联的ORB特征点与闭环候选关键帧之间的对应关系，从而为每个候选关键帧建立3D到3D的对应关系；

(8b2)根据每个候选关键帧之间的3D到3D对应关系，对每个候选帧进行随机抽样一直RANSAC迭代算法迭代，并使用Horn的方法找到一个具有足够内点的相似性转换S_il，对它进行一次优化，并执行更多对应关系的引导式搜索；

(8b3)在引导式搜索完成后，对相似性变换S_il进行二次优化，若经过二次优化的相似性变换S_il仍具有足够的内点，则地图点闭环建立；

(8b4)在地图点闭环建立后，对重复的地图点进行闭环融合，以建立新的连接地图点闭环边缘，从而构建一个稳定的网络以获得定位结果；

(8c)在一个单独的线程中执行全局BA优化，从而允许系统继续创建地图点和检测地图点闭环；若在优化运行时检测到新地图点闭环，则中止优化并继续进行检测地图点闭环，直到地图点闭环完建立成后，将再次启动全局的BA优化；

(8d)将全局BA优化完成后的关键帧和地图点子集与未被优化的关键帧和地图点这四者进行融合，使得未优化的位姿变换为最佳位姿，完成视觉地图的构建。

步骤9，视觉地图的保存与读取。

(9a)视觉地图的保存：

(9a1)在相机标定文件的最后加入保存地图的标签，并且写上地图文件名；

(9a2)在ORB-SLAM的代码中，加入一个bool型变量“do_save_map”用来判断是否需要保存地图，当返回值为false时，不启动保存地图的功能；当返回值为true时，启动保存地图的功能并且创建地图文件；

(9b)视觉地图的读取及定位：

(9b1)输入正确的地图名称，系统自动加载地图；

(9b2)读取地图，并进入视觉地图定位模式，等待目标车辆行驶过程中道路图像输入；

(9b3)输入第一帧图像，系统进行图像定位，找到这一帧在地图中的位置，并开始跟踪目标车辆，实时返回目标车辆在地图中的位置。

步骤10，基于视觉地图对目标车辆进行定位。

(10a)设定匹配特征点数量阈值T为150，将步骤5中获得匹配到的特征点数M与该阈值T进行比较，若M≥T，则利用步骤6进行定位，否则，执行(10b)；

(10b)获取与当前帧具有最佳共视的N个关键帧，N≤0，1，2,…,150；计算所获取帧的词袋模型BoW向量个数K，并对计算后的词袋模型BoW向量进行数量匹配；

(10c)设置匹配数量阈值P为5，剔除K≤P的关键帧，K＝0，1，2，…；再利用PnP算法优化求解余下的关键帧的位姿，根据确定的目标车辆位姿读取目标车辆在视觉地图中的位置，完成车辆定位。

本发明的效果可以通过以下试验进一步说明：

1、试验条件：

采用Linux操作系统，选择长期维护的、相对稳定的Ubuntu16.04版本；

选用OpenCV计算机视觉库，为了获得最新的库，选择2017年发布的OpenCV3.2.0版本；

选用Pangolin开发库，用于实现地图的实时显示功能；

平台使用HNY-CV-002可变基线双目视觉模块，具备定基线双目摄像头的所有功能。该平台最大的优势是基线可调整，也就是可根据拍摄目标的远近来调整双目的间距，可以更有效的进行三维识别、测量，可提高测量精度。

采用KITTI数据集来对系统定位精度进行测试，KITTI数据集包含城市和高速公路环境下从汽车行驶过程中记录的双目数据，该数据集序列为00，01，02，03，04，05，06，07，08，09，10，其包含所需测试的地图点闭环。

2、试验内容与结果：

试验一：为了检测双目立体视觉系统鲁棒性，以不同着装的人为目标，在室外不同环境、不同摄像头角度下用本发明进行复杂背景视差试验，试验结果如图2。其中图2(a)图2(c)图2(e)图2(g)为双目相机摄取到的双目图像，图2(b)图2(d)图2(f)图2(h)为双目图像的视差图；从图2可见，在室外环境下，面对不同复杂的背景和目标，仍可以得到比较精确的视差图，说明本发明的鲁棒性较好，达到预期要求。

试验二：利用KITTI数据集进行建图过程中的精度评价：

利用KITTI数据集中00、01、05、07、08序列对本发明进行建图测试，测试结果如图3所示，其中图3(a)为00序列的测试结果，图3(b)为01序列的测试结果，图3(c)为05序列的测试结果，图3(d)为07序列的测试结果，图3(e)为08序列的测试结果，这些结果展示了估计目标车辆行驶轨迹的一些例子；通过使用两种不同的度量标准，绝对平移RMSE误差，以及平均相对平移坐标和旋转误差，表明本发明所使用的ORB-SLAM系统在大多数序列中优于双目LSD-SLAM，并且通常实现低于1％的相对误差。

试验三：利用KITTI数据集进行地图定位精度评价：

采用KITTI数据集07序列对ORB-SLAM经典定位算法的定位结果和本发明的定位结果进行误差计算，结果如所示表1所示；

表1：KITTI的07序列各算法运行结果

算法名	最大误差	误差均值	最小误差	误差均方根
					本发明	0.926	0.389	0.073	0.422
ORB-SLAM定位算法	1.818	0.480	0.098	0.588
					ORB-SLAM无地图定位算法	4.279	1.331	0.576	1.529

从表1可见在误差均值和误差均方根值这两个重要指标中，本发明相对经典算法明显是最小的；

再对定位结果进行绝对位置误差APE计算，并将计算结果进行可视化处理，结果如图4所示，其中曲线1为本发明计算结果，曲线2为ORB-SLAM经典算法计算结果，曲线3为ORB-SLAM经典定位算法在无地图情况下的计算结果。从图4可见，本发明的定位效果明显优于其它算法的定位效果。

从以上的试验结果可以看出，在定位算法评价时，使用KITTI公用数据集对本发明和经典视觉定位算法进行测试，通过对测试结果进行对比，验证了本发明具有较高的定位精度。

Claims (9)

1.一种基于ORB-SLAM的高精度车辆定位方法，其特征在于，包括如下：

(1)选取标定板，进行双目摄像机标定，检测图像角点，通过标定实验选择标定效果最好的一次，并调用图像校正函数进行图像校正；

(2)在完成相机标定和图像校正后，提取均匀分布的特征点检测角点FAST,得到旋转多尺度角点ORB特征描述子并计算FAST角点的方向；

(3)在得到图像中旋转多尺度角点ORB特征描述子之后，进行双目稀疏特征的匹配，令匹配到特征点的个数为M，并在匹配完成后追踪车辆运动过程中的相邻帧特征，确定当前相机位姿，M＝0，1，2，…；

(4)建立局部地图：

根据追踪运动车辆所确定的相机位姿中所包含的关键帧，建立共视图，在下一刻的运动中以同样的方法获得新的关键帧并插入到共视图中，更新共视图；

通过对来自共视图中相连关键帧的旋转多尺度角点ORB特征进行三角运算创建新的地图点，同时对共视图进行优化并剔除多余的关键帧以完成局部地图的建立；

(5)对建立的局部地图进行闭环检测与全局优化，实现双目视觉ORB-SLAM视觉地图的建立；

(6)在跟踪运动车辆的过程中，对ORB-SLAM视觉地图进行实时保存与读取，并利用基于SLAM视觉地图的定位算法进行跟踪车辆的实时定位。

2.根据权利要求书1所述的方法，其中所述步骤(2)中提取均匀分布的特征点检测角点FAST，其实现如下：

(2a)对一张图像不断进行高斯模糊和向下采样，得到宽高均为原图像1/2的不同分辨率的图像，用这些不同分辨率的图像构成n层高斯金字塔，n≥8；

(2b)从(2a)得到高斯金字塔中获取8层高斯金字塔，把每层图像分成网格，在网格中选取一点P，以P为圆心画一个半径为3pixel的圆，圆周上如果有连续12个像素点的灰度值比P点的灰度值大或者小，则认为P为FAST角点，并进行提取；

(2c)为了确保角点的均匀分布，每格提取至少5个角点；当原图像分辨率为512×384～752×480时，共提取1000个角点；当分辨率高于512×384～752×480时，共提取2000个角点；所得到的角点中均包含ORB特征描述子但不包含FAST角点方向。

3.根据权利要求书1所述的方法，其中所述步骤(2)中计算FAST角点的方向，其实现如下：

(2d)定义如下矩重心m_pq：

m_pq＝∑_x,y∈rx^py^qI(x,y)，

其中，I(x,y)为图像灰度表达式，r为角点范围半径，p,q为坐标轴，(x,y)为选取角点O的坐标；

(2e)通过矩重心计算角点以r为半径范围内的质心C：

其中,m₁₀为p轴边界点的矩重心，m₀₁为q轴边界点的矩重心，m₀₀为坐标轴原点的矩重心；

(2f)将角点O坐标到质心C所形成的向量角为θ作为选取角点O的方向，其计算公式如下：

4.根据权利要求书1所述的方法，其中(3)中的双目稀疏匹配，其实现如下：

(3a)输入校正后的双目图像，并提取左右图像的旋转多尺度角点ORB特征角点；

(3b)将左右两图像以方格图划分，在左图标定一个旋转多尺度角点ORB特征点(u_L,v_L)，u_L为左图中的水平座标，v_L为极线位置；将右图像中的水平坐标定义为u_R，由三个坐标共同定义立体关键点：x_s＝(u_L,v_L,u_R)，在“极线约束”的条件下沿着极线，在右图中的[u_R,u_R+dmax]的区域内寻找同名点，其中dmax为划分方格的宽度；

(3c)计算标定特征点和所寻找到的同名点之间的绝对位置误差，选取绝对误差最小的点，作为对应匹配点；

(3d)对于每一对匹配的ORB特征角点，进行深度计算得到两点的空间位置关系：

其中f_x是水平焦距，b是基线即两个摄像头之间的距离，d是两点之间的空间深度距离。

5.根据权利要求书1所述的方法，其中(3)中追踪车辆运动过程中的相邻帧特征，其实现如下：

(3e)对于匀速运动车辆的追踪，根据上一帧的之前帧计算上一帧的运动速度，利用上一帧的位姿与速度估计当前帧的位姿，如果在跟踪过程目标丢失，则执行(3c)；

(3f)对于非匀速运动车辆的追踪，首先与最近一个关键帧匹配，假设当前帧和参考关键帧位姿矩阵相同，计算当前帧的词袋模型BoW，用词袋模型来加速匹配，并设定初始位姿为上一帧的位姿，再根据位姿和词袋模型BoW词典寻找特征匹配，利用匹配的特征优化位姿；如果在跟踪过程目标丢失，则执行(3c)；

(3g)计算当前帧的词袋模型BoW，并在关键帧数据库中搜索、计算与每个关键帧中的地图点关联的ORB特征点的对应关系，并对每个关键帧执行交替RANSAC迭代，尝试使用PnP算法找到当前位姿。

6.根据权利要求书1所述的方法，其中(4)中对来自共视图中相连关键帧的旋转多尺度角点ORB特征进行三角运算，其实现如下：

(4a)测出双目相机间距x，以及双目相机与特征点的夹脚α和β，并标定焦距y及目标点投射到双目相机的相机坐标系下的坐标点A和B；

(4b)基于三角形稳定准则，由A、B、x、y、α、β这些参数，利用三角形相似原理计算得到特征点的空间坐标，将此特征点设置为新的地图点，并将其坐标插入到新的共视图中。

7.根据权利要求书1所述的方法，其中(4)中优化并剔除局部地图中多余的关键帧，是先对当前处理的关键帧和与当前帧相关连的关键帧中的地图点进行观测，并对被观测的地图点进行BA优化；再剔除掉在观测过程中所含90％的地图点至少在其他三个关键帧中可被观测到的关键帧，以避免在相同环境中车辆运动时关键帧的无限增加。

8.根据权利要求书1所述的方法，其中(5)中局部地图闭环检测与全局优化，其实现如下：

(5a)对闭环候选关键帧进行检测：

(5a1)计算关键帧K_i的词袋模型BoW和它在共视图中的相邻关键帧，并且设置保留的最低分为s_min。

(5a2)利用mvInvertedFile函数在关键帧数据库中搜索，丢弃所有分数低于s_min的关键帧；

(5a3)在共视图中连续检测三个一致的相联关键帧，确定出一个闭环候选帧；

(5b)对关键帧进行相似性变换计算，建立地图点闭环：

(5b1)计算与当前关键帧中地图点关联的ORB特征点与闭环候选关键帧之间的对应关系，从而为每个候选关键帧建立3D到3D的对应关系；

(5b2)根据每个候选关键帧之间的3D到3D对应关系，对每个候选帧进行RANSAC迭代，并使用Horn的方法找到一个具有足够内点的相似性转换S_il，对它进行一次优化，并执行更多对应关系的引导式搜索；

(5b3)在引导式搜索完成后，对相似性变换S_il进行二次优化，若经过二次优化的相似性变换S_il仍具有足够的内点，则地图点闭环建立；

(5b4)在地图点闭环建立后，对重复的地图点进行闭环融合，以建立新的连接地图点闭环的边缘，从而构建一个稳定的网络以获得定位结果；

(5c)在一个单独的线程中执行全局BA优化，从而允许系统继续创建地图点和检测地图点闭环；如果在优化运行时检测到新地图点闭环，则中止优化并继续进行检测地图点闭环，直到地图点闭环完建立成后，将再次启动全局的BA优化；

(5d)当全局BA优化完成后，将优化后的关键帧和地图点子集和与未被优化的关键帧和地图点融合，使得未优化的位姿变换为最佳位姿。

9.根据权利要求书1所述的方法，其中(6)中利用基于SLAM视觉地图的定位算法进行跟踪车辆的实时定位，其实现如下：

(6a)设定匹配特征点数量阈值T为150，将(3)中获得匹配到的特征点数M与该阈值T进行比较，若M≥T，则利用(3)中相邻帧追踪方法进行定位，否则，执行(6b)；

(6b)获取与当前帧具有最佳共视的N个关键帧，N≤0，1，2,…,150；计算所获取帧的词袋模型BoW向量个数K，并对计算后的词袋模型BoW向量进行数量匹配；设置匹配数量阈值P为5，剔除K≤P的关键帧，K＝0，1，2，…；再利用PnP算法优化求解余下的关键帧的位姿，完成车辆定位。