CN112577499B - 一种vslam特征地图尺度恢复方法及系统 - Google Patents

Abstract

本发明公开了一种VSLAM特征地图尺度恢复方法，在安装有摄像头、激光雷达和组合惯性导航系统的车辆在指定区域内行驶的情况下，包括步骤：获取摄像头采集的图像数据、激光雷达采集的点云数据以及组合惯性导航系统采集的高精度地图数据；对三者相对位置进行标定，以将采集的数据坐标统一；采用VSLAM方法由图像数据生成无尺度的M_vslam和T_vslam；采用LSLAM方法由点云数据生成具有相对尺度的M_lslam和T_lslam；将T_lslam与高精度地图数据T_combine融合，得绝对尺度定位轨迹T_all；采用T_all对T_vslam进行粗标定可得S₁与RT₁，将粗标定的定位轨迹T_vslam进行精标定，得到S₂与RT₂；采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图M_vslam进行缩放和转换，得到具有绝对尺度的VSLAM特征地图，其中S＝S₁·S₂、RT＝RT₁·RT₂。

Description

一种VSLAM特征地图尺度恢复方法及系统

技术领域

本发明涉及一种地图生成方法和系统，尤其涉及基于视觉的地图生成方法和系统。

背景技术

近年来，随着无人驾驶技术的快速发展，无人车运用在日常生活中的可能性也越来越大。而车辆定位是自动驾驶领域的核心技术之一，对于停车楼、地库等GPS信号弱或消失的区域，SLAM(Simultaneous Localization and Mapping，即时定位与地图构建，或并发建图与定位)技术是最主流的定位解决方案之一。

SLAM技术可以有效建立指定区域的特征地图，再通过地图特征匹配即可实现自动驾驶车辆定位。SLAM主要分为基于视觉的SLAM(VSLAM)和基于激光雷达的SLAM(LSLAM)。其中，VSLAM(Visual SLAM)是基于视觉的即时定位与地图构建，LSLAM(Lidar SLAM)是基于激光雷达的即时定位与地图构建。

在实际应用过程中，由于激光雷达的成本较高，很难实现商用，而单目摄像头成本低廉且容易量产，因此采用单目VSLAM技术进行即时定位是目前自动驾驶定位中的主流方案。

然而，由于单目摄像头难以测距，因此现有技术中的采用单目摄像头进行VSLAM定位的方法的主要缺陷是无法获得实际尺度信息，即无法知道自车实际行驶的距离。

基于此，针对现有技术中的缺陷，本发明期望获得一种新型单目VSLAM特征地图尺度恢复方法。该方法可以利用组合导航和LSLAM定位结果恢复采用单目摄像头定位的VSLAM特征地图的尺度，从而解决了单目摄像头定位没有实际尺寸的缺陷。由于最终的定位地图是基于视觉特征生成的，激光雷达和组合惯性导航系统只需在建图时被使用，在最终定位应用时只需单目摄像头即可，可以大大降低自动驾驶定位模块的成本，加速该功能量产。

发明内容

本发明的目的之一在于提供一种VSLAM特征地图尺度恢复方法，该VSLAM特征地图尺度恢复方法可以利用组合导航和LSLAM定位结果恢复采用了单目摄像头定位的VSLAM特征地图的尺度，从而解决现有技术中的单目摄像头定位没有实际尺寸的缺陷。由于最终的定位地图是基于视觉特征生成的，激光雷达和组合惯性导航系统只需在建图时被使用，在最终定位应用时只需单目摄像头即可，可以大大降低自动驾驶定位模块的成本，加速该功能量产。

为了实现上述目的，本发明提出了一种VSLAM特征地图尺度恢复方法，其在安装有摄像头、激光雷达和组合惯性导航系统的车辆在指定区域内行驶的情况下，包括步骤：

100：获取摄像头采集的图像数据、激光雷达采集的点云数据以及组合惯性导航系统采集的高精度地图数据；

200：对摄像头、激光雷达和组合惯性导航系统的相对位置进行标定，以将所述图像数据、点云数据和高精度地图数据的坐标统一；

300：基于所述图像数据采用VSLAM方法生成无尺度的特征地图M_vslam和定位轨迹T_vslam；基于所述点云数据采用LSLAM方法生成具有相对尺度的特征地图M_lslam和定位轨迹T_lslam；

400：将定位轨迹T_lslam与高精度地图数据T_combine融合，得到绝对尺度定位轨迹T_all；

500：采用绝对尺度定位轨迹T_all对定位轨迹T_vslam进行粗标定，得到第一缩放系数S₁与第一变换矩阵RT₁；

600：对经过粗标定的定位轨迹T_vslam进行精标定，得到第二缩放系数S₂与第二变换矩阵RT₂；

700：采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图M_vslam进行缩放和转换，得到具有绝对尺度的VSLAM特征地图，其中S＝S₁·S₂、RT＝RT₁·RT₂。

在本发明中，通过利用安装有摄像头、激光雷达和组合惯性导航系统的数据采集车在指定区域(如停车楼和停车库)按一定路线人工驾驶，采集数据。安装在车辆上的传感器(摄像头、激光雷达和组合惯性导航系统)采集到图像数据、点云数据及高精度GPS定位轨迹数据后，采用VLSAM及LSLAM方法分别进行建图及定位。

将LSLAM得到的定位轨迹T_lslam与组合惯性导航系统得到的高精度地图数据T_combine融合，可以赋予T_lslam以绝对尺度(如经纬度)信息，得到绝对尺度定位轨迹T_all。最后利用绝对尺度定位轨迹T_all依次对定位轨迹T_vslam进行粗标定及精标定，即可得到恢复尺度的VSLAM特征地图，该特征地图最终可存储于本地或云端，并应用于自动驾驶车辆定位。

此外，需要说明的是，在本发明所述的VSLAM特征地图尺度恢复方法中，在上述步骤700中，“S＝S₁·S₂、RT＝RT₁·RT₂”中的“·”表示矩阵相乘。

进一步地，在本发明所述的VSLAM特征地图尺度恢复方法中，所述摄像头为单目摄像头或环视摄像头。

进一步地，在本发明所述的VSLAM特征地图尺度恢复方法中，所述步骤400进一步包括：

对于定位信号好的区域，采用高精度地图数据T_combine作为具有绝对尺度的第一定位轨迹；

对于定位信号不好的区域，将定位轨迹T_lslam中的各轨迹点与高精度地图数据T_combine中的相应轨迹点进行匹配，得到定位轨迹T_lslam到高精度地图数据T_combine的转换矩阵；采用转换矩阵对定位轨迹T_lslam进行变换，得到具有绝对尺度的第二定位轨迹；

将第一定位轨迹和第二定位轨迹组合，得到绝对尺度定位轨迹T_all。

进一步地，在本发明所述的VSLAM特征地图尺度恢复方法中，采用迭代最近邻算法将定位轨迹T_lslam中的各轨迹点与高精度地图数据T_combine中的相应轨迹点进行匹配。

进一步地，在本发明所述的VSLAM特征地图尺度恢复方法中，所述步骤500进一步包括：

501：将定位轨迹T_vslam与绝对尺度定位轨迹T_all在同一3D坐标系下可视化；

502：将定位轨迹T_vslam缩放至与绝对尺度定位轨迹T_all同一数量级；

503：将缩放后的定位轨迹T_vslam旋转、平移至与绝对尺度定位轨迹T_all大致重合，得到新定位轨迹T’_vslam以及第一缩放系数S₁与第一变换矩阵RT₁。

进一步地，在本发明所述的VSLAM特征地图尺度恢复方法中，所述步骤600进一步包括：

601：令当前定位轨迹T_cur＝T’_vslam。，当前缩放系数S_cur＝S₁；

602：令最佳缩放系数S_best＝S_cur，采用梯度下降法进行S_cur迭代；

603：令当前目标函数值J_cur＝J_pre，J_pre表示前一时刻目标函数值，采用迭代最近邻算法将当前定位轨迹T_cur和绝对尺度定位轨迹T_all进行一次匹配，得到当前变换矩阵，然后更新当前定位轨迹T_cur；

604：基于当前定位轨迹T_cur和绝对尺度定位轨迹T_all计算当前目标函数值J_cur，并将当前目标函数值J_cur与前一时刻目标函数值J_pre的差值与设定的期望目标函数迭代差值ΔJ进行比较：若差值小于ΔJ，则将与当前定位轨迹T_cur对应的当前变换矩阵作为第二变换矩阵RT₂输出，并进行步骤605；否则，则转回步骤603；

605：将最佳缩放系数S_best与当前缩放系数S_cur的差值与设定的期望缩放系数迭代差值ΔS进行比较：若差值小于ΔS，则将当前缩放系数S_cur作为第二缩放系数S₂输出；否则，则转回步骤602。

相应地，本发明的另一目的在于提供一种VSLAM特征地图尺度恢复系统，该VSLAM特征地图尺度恢复系统可以用于实施本发明上述的VSLAM特征地图尺度恢复方法。

为了实现上述目的，本发明提出了一种VSLAM特征地图尺度恢复系统，其包括：

安装有摄像头、激光雷达和组合惯性导航系统的车辆，所述车辆在指定区域内行驶；所述摄像头采集并输出图像数据，所述激光雷达采集并输出点云数据，所述组合惯性导航系统采集并输出高精度地图数据；

预处理模块，其对摄像头、激光雷达和组合惯性导航系统的相对位置进行标定，以将所述图像数据、点云数据和高精度地图数据的坐标统一；

VSLAM模块，其基于所述图像数据生成无尺度的特征地图M_vslam和定位轨迹T_vslam；

LSLAM模块，其基于所述点云数据生成具有相对尺度的特征地图M_lslam和定位轨迹T_lslam；

数据融合模块，其将定位轨迹T_lslam与高精度地图数据T_combine融合，得到绝对尺度定位轨迹T_all；

粗标定模块，其采用绝对尺度定位轨迹T_all对定位轨迹T_vslam进行粗标定，得到第一缩放系数S₁与第一变换矩阵RT₁；

精标定模块，其经过粗标定的定位轨迹T_vslam进行精标定，得到第二缩放系数S₂与第二变换矩阵RT₂；

VSLAM特征地图尺度恢复模块，其采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图M_vslam进行缩放和转换，得到具有绝对尺度的VSLAM特征地图，其中S＝S₁·S₂、RT＝RT₁·RT₂。

进一步地，在本发明所述的VSLAM特征地图尺度恢复系统中，所述摄像头为单目摄像头，其视场角FOV>90°，其采样帧率>20Hz；或者所述摄像头为环视摄像头，其视场角FOV>120°，其采样帧率>20Hz。

进一步地，在本发明所述的VSLAM特征地图尺度恢复系统中，所述激光雷达设置在车辆顶部，其线数>16线，角分辨率<1°，采样频率>20Hz。

进一步地，在本发明所述的VSLAM特征地图尺度恢复系统中，所述数据融合模块被设置为执行下述步骤：

对于定位信号好的区域，采用高精度地图数据T_combine作为具有绝对尺度的第一定位轨迹；

对于定位信号不好的区域，将定位轨迹T_lslam中的各轨迹点与高精度地图数据T_combine中的相应轨迹点进行匹配，得到定位轨迹T_lslam到高精度地图数据T_combine的转换矩阵；采用转换矩阵对定位轨迹T_lslam进行变换，得到具有绝对尺度的第二定位轨迹；

将第一定位轨迹和第二定位轨迹组合，得到绝对尺度定位轨迹T_all。

本发明所述的VSLAM特征地图尺度恢复方法及系统相较于现有技术具有如下所述的优点和有益效果：

本发明所述的VSLAM特征地图尺度恢复方法通过使用组合导航和LSLAM定位结果，恢复了采用单目摄像头定位的VSLAM特征地图的尺度，解决了现有技术中采用单目摄像头定位没有实际尺寸的缺陷。在本发明中，激光雷达和组合惯性导航系统只需在建图时被使用，在最终定位应用时只需单目摄像头即可，可以大大降低自动驾驶定位模块的成本，加速该功能量产。

此外，本发明所述的VSLAM特征地图尺度恢复方法通过与高精度的定位结果进行标定可以使VSLAM具有比较高的定位精度，同时在标定过程中也可以获知VSLAM特征地图是否准确以及误差较大的区域。

相应地，本发明所述的VSLAM特征地图尺度恢复系统可以用于实施本发明上述方法，其同样具有上述的优点以及有益效果。

附图说明

图1示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下的步骤流程图。

图2示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下各个传感器在车辆上的布置位置。

图3示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下的步骤500中进行粗标定的流程图

图4示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下的步骤600中进行精标定的流程图。

具体实施方式

下面将结合说明书附图和具体的实施例对本发明所述的VSLAM特征地图尺度恢复方法及系统做进一步的解释和说明，然而该解释和说明并不对本发明的技术方案构成不当限定。

在本发明中，本发明公开了一种自动驾驶局部路径规划系统，该自动驾驶局部路径规划系统可以用于实施本发明的自动驾驶局部路径规划方法，具体步骤流程如图1所示。

图1示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下的步骤流程图。

结合参考图1可以看出，在本实施方式中，当安装有摄像头、激光雷达和组合惯性导航系统的车辆在指定区域内行驶时，本发明所述的VSLAM特征地图尺度恢复方法可以包括如下步骤：

100：获取摄像头采集的图像数据、激光雷达采集的点云数据以及组合惯性导航系统采集的高精度地图数据；

200：对摄像头、激光雷达和组合惯性导航系统的相对位置进行标定，以将所述图像数据、点云数据和高精度地图数据的坐标统一；

300：基于所述图像数据采用VSLAM方法生成无尺度的特征地图M_vslam和定位轨迹T_vslam；基于所述点云数据采用LSLAM方法生成具有相对尺度的特征地图M_lslam和定位轨迹T_lslam；

400：将定位轨迹T_lslam与高精度地图数据T_combine融合，得到绝对尺度定位轨迹T_all；

500：采用绝对尺度定位轨迹T_all对定位轨迹T_vslam进行粗标定，得到第一缩放系数S₁与第一变换矩阵RT₁；

600：对经过粗标定的定位轨迹T_vslam进行精标定，得到第二缩放系数S₂与第二变换矩阵RT₂；

700：采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图M_vslam进行缩放和转换，得到具有绝对尺度的VSLAM特征地图，其中S＝S₁·S₂、RT＝RT₁·RT₂。上述“·”表示矩阵相乘。

由此可见，本发明所述的VSLAM特征地图尺度恢复方法通过利用安装有摄像头、激光雷达和组合惯性导航系统的数据采集车在指定区域(如停车楼和停车库)按一定路线人工驾驶，并采集数据。安装在车辆上的传感器(摄像头、激光雷达和组合惯性导航系统)采集到图像数据、点云数据及高精度GPS定位轨迹数据后，可以采用VLSAM及LSLAM方法分别进行建图及定位。

相应地，将LSLAM得到的定位轨迹T_lslam与组合惯性导航系统得到的高精度地图数据T_combine融合，可以赋予T_lslam以绝对尺度(如经纬度)信息，得到绝对尺度定位轨迹T_all。最后利用绝对尺度定位轨迹T_all依次对定位轨迹T_vslam进行粗标定及精标定，即可得到恢复尺度的VSLAM特征地图，该特征地图最终可存储于本地或云端，并可以有效应用于自动驾驶车辆定位。

需要说明的是，在本实施方式中，本发明所述的VSLAM特征地图尺度恢复系统可以用于实施上述步骤100-700，其可以包括：安装有摄像头、激光雷达和组合惯性导航系统的车辆、预处理模块、VSLAM模块、LSLAM模块、数据融合模块、粗标定模块、精标定模块和VSLAM特征地图尺度恢复模块。

其中，车辆在指定区域内行驶时，摄像头可以采集并输出图像数据，激光雷达可以采集并输出点云数据，组合惯性导航系统可以采集并输出高精度地图数据。

在本发明中，摄像头一般可以采用单目摄像头或者环视摄像头，在使用摄像头之前需要对摄像头进行内参数和外参数标定。在本实施方式中，采用的摄像头可以为单目摄像头，且可以优选摄像头视场角(FOV)>90°，采样帧率>20Hz，支持高动态光照渲染(HDR)。而在某些实施方式中当采用环视摄像头采集图像数据时，可使用前后左右4个环视摄像头拼接后的图像或前环视摄像头图像，并优选的要求摄像头视场角(FOV)>120°，采样帧率>20Hz，支持高动态光照渲染(HDR)。

相应地，在本发明中，激光雷达一般可以采用顶置激光雷达，且在本实施方式中，可以优选地要求激光雷达线数>16线，角分辨率<1°，采样频率>20Hz。其中，在使用激光雷达之前需要对激光雷达进行外参数标定，建议进行内参数标定。

此外，在本发明中，在本实施方式中，还可以优选的要求组合惯性导航系统开启载波相位差分技术(RTK)定位，并控制采样频率>50Hz，室外定位精度<0.1m，室内行驶1000m定位精度<2m。其中，使用组合惯性导航系统之前需要对组合惯性导航系统进行外参数标定。

需要特别说明的是，在本实施方式中，本发明中的组合惯性导航系统可以融合载波相位差分技术(RTK)和惯性导航系统(INS)实现高精度高频率定位。其中，定位使用载波相位差分技术后可以得到高精度厘米级的定位结果，但频率不高；而惯性导航系统频率高但误差随时间推移会不断变大。二者融合后则可以优势互补，尤其在GPS定位信号良好的区域可以得到高精度和高频率的绝对尺度定位结果。

图2示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下各个传感器在车辆上的布置位置。

如图2所示，在本实施方式中，本发明所述的VSLAM特征地图尺度恢复方法所采用的摄像头为单目摄像头，且其设置在车辆前挡风玻璃上方内部；激光雷达采用顶置激光雷达，设置于车辆顶部，可以进行360°扫描；组合惯性导航系统布置位置无特殊要求，可置于后备箱固定。

在本发明所述的VSLAM特征地图尺度恢复方法中，针对需要构建VSLAM特征地图的区域，需要先在GPS定位信号良好的室外开启系统以完成初始化，从而获得精准绝对定位及传感器数据同步。之后，按指定路线在区域内人工驾驶并采集所需传感器数据。为了保证建图及定位精度，在摄像头、激光雷达和组合惯性导航系统这些传感器的数据采集过程中车辆行驶速度应稳定在较低行驶速度，例如10km/h。

需要注意的是，如图2所示，本发明中的摄像头、激光雷达和组合惯性导航系统均安装于车辆的不同位置，因此，为了保证不同传感器得到的数据坐标的一致性，需要进行坐标统一。

在本实施方式中，可以以车辆后轴中心作为车辆坐标系原点，采用右手坐标系，并规定X轴正方形指向车辆正前方。在本发明所述的VSLAM特征地图尺度恢复系统中，系统中的预处理模块在对摄像头、激光雷达和组合惯性导航系统的外参数进行标定后，可以分别得到单目摄像头、激光雷达和组合惯性导航系统相对于车辆后轴中心点的变换矩阵，以将图像数据、点云数据和高精度地图数据的坐标统一。

相应地，如图1所示，在本实施方式中，摄像头采用的是单目摄像头，且在所需要的时间段t1-t2内拍摄的图像序列为I_t1-t2，该图像序列需要根据摄像头内参数进行图像矫正；相应地，激光雷达在所需要的时间段t1-t2内扫描得到的激光点云序列P_t1-t2；组合惯性导航系统在所需要的时间段t1-t2内输出的坐标转换到车辆后轴中点后的定位轨迹T _t1-t2。

在本实施方式中，本发明所述的VSLAM特征地图尺度恢复系统中的VSLAM模块，可以基于图像数据I_t1-t2生成无尺度的特征地图M_vslam和时间段t1-t2内车辆行驶过的无尺度信息的定位轨迹T_vslam。需要说明的是，VSLAM有许多现有的成熟算法，例如：ORB-SLAM、LSD-SLAM、MonoSLAM、PTAM和SVO，用户可以根据实际需要选用合适的算法作为VSLAM模块模型。

相应地，在本实施方式中，本发明所述的VSLAM特征地图尺度恢复系统中的LSLAM模块可以基于点云数据P_t1-t2，生成具有相对尺度的特征地图M_lslam和时间段t1-t2内车辆行驶过的具有相对尺度信息的定位轨迹T_lslam。需要说明的是，LSLAM有许多现有的成熟算法，例如：Cartographer和Loam，用户可以根据实际需要选用合适的算法作为LSLAM模块模型。

同样的，在本实施方式中，本发明所述的VSLAM特征地图尺度恢复系统中的数据融合模块可以将LSLAM模块得到的定位轨迹T_lslam与组合惯性导航系统得到的高精度地图数据T_combine融合，从而得到绝对尺度定位轨迹T_all。

但需要注意的是，在本实施方式中，上述融合得到绝对尺度定位轨迹T_all的步骤可以进一步包括以下操作：

对于室外GPS定位信号好的区域，优先选择信任组合惯性导航系统得到的高精度地图数据T_combine作为具有绝对尺度的第一定位轨迹；

而对于室内停车楼或地库这些定位信号不好的区域，优先选择信任激光雷达的定位轨迹T_lslam。因组合惯性导航系统和激光雷达同时运行，通过查找邻近时间戳，定位轨迹T_lslam中的每一个轨迹点都可以和高精度地图数据T_combine中的相应轨迹点进行匹配。因此，只需对室外部分定位轨迹T_lslam到高精度地图数据T_combine的轨迹点进行迭代最近邻算法(ICP)匹配，即可得到定位轨迹T_lslam到高精度地图数据T_combine的转换矩阵。采用转换矩阵对定位轨迹T_lslam进行变换，得到具有绝对尺度的第二定位轨迹；

对于同时包含室内和室外的区域，室内部分选择激光雷达下的绝对定位轨迹T_all，室外部分选择组合惯导的绝对定位轨迹T_all，组合后即得到完整绝对尺度定位轨迹T_all。

此外，需要说明的是，在本发明中，采用VSLAM模块生成的特征地图M_vslam与定位轨迹T_vslam无尺度，或者说非真实尺度，与绝对尺度定位轨迹T_all相差可能非常大，若直接进行精标定，进行迭代最近邻算法(ICP)迭代时较难收敛到期望精度，因此在本实施方式中可以优选的先进行粗标定，具体步骤如图3所示。

图3示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下进行粗标定的流程图。

结合参考图1可知，在本发明所述VSLAM特征地图尺度恢复方法的步骤500中，需要采用绝对尺度定位轨迹T_all对定位轨迹T_vslam进行粗标定，以得到第一缩放系数S₁与第一变换矩阵RT₁。

如图3所示，在本实施方式中，本发明所述的步骤500可以进一步包括如下步骤：

501：将定位轨迹T_vslam与绝对尺度定位轨迹T_all在同一3D坐标系下可视化；

502：将定位轨迹T_vslam缩放至与绝对尺度定位轨迹T_all同一数量级；

503：将缩放后的定位轨迹T_vslam旋转、平移至与绝对尺度定位轨迹T_all大致重合，得到新定位轨迹T’_vslam以及第一缩放系数S₁与第一变换矩阵RT₁。

图4示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下进行精标定的流程图。

相应地，在粗标定基础上，精标定流程如图4所示。在本发明所述VSLAM特征地图尺度恢复方法的步骤600中，需要对经过粗标定的定位轨迹T_vslam进行精标定，以得到第二缩放系数S₂与第二变换矩阵RT₂。

如图4所示，在本实施方式中，本发明所述VSLAM特征地图尺度恢复方法的步骤600可以进一步包括如下步骤：

601：令当前定位轨迹T_cur＝T’_vslam。，当前缩放系数S_cur＝S₁；

在上述步骤601中，可以先初始化最佳缩放系数S_best、前一时刻目标函数值J_pre、期望缩放系数迭代差值ΔS和期望目标函数迭代差值ΔJ。

602：令最佳缩放系数S_best＝S_cur，采用梯度下降法进行S_cur迭代；

603：令当前目标函数值J_cur＝J_pre，J_pre表示前一时刻目标函数值，采用迭代最近邻算法将当前定位轨迹T_cur和绝对尺度定位轨迹T_all进行一次匹配，得到当前变换矩阵，然后更新当前定位轨迹T_cur；

604：基于当前定位轨迹T_cur和绝对尺度定位轨迹T_all计算当前目标函数值J_cur，并将当前目标函数值J_cur与前一时刻目标函数值J_pre的差值与设定的期望目标函数迭代差值ΔJ进行比较：若差值小于ΔJ，则将与当前定位轨迹T_cur对应的当前变换矩阵作为第二变换矩阵RT₂输出，并进行步骤605；否则，则转回步骤603；

605：将最佳缩放系数S_best与当前缩放系数S_cur的差值与设定的期望缩放系数迭代差值ΔS进行比较：若差值小于ΔS，则将当前缩放系数S_cur作为第二缩放系数S₂输出；否则，则转回步骤602。

经过上述操作步骤后，可以得到第二缩放系数S₂与第二变换矩阵RT₂，由于无尺度的特征地图M_vslam和定位轨迹T_vslam同时采集，尺度相同，只需将M_vslam利用S＝S₁·S₂、RT＝RT₁·RT₂进行缩放和变换后即可恢复特征地图尺度，最终得到具有绝对尺度的VSLAM特征地图M_vslam-real。

综上所述可以看出，本发明所述的VSLAM特征地图尺度恢复方法通过使用组合导航和LSLAM定位结果恢复了采用单目摄像头定位的VSLAM特征地图的尺度，解决了单目摄像头定位没有实际尺寸的缺陷。在本发明中，激光雷达和组合惯性导航系统只需在建图时被使用，在最终定位应用时只需单目摄像头即可，可以大大降低自动驾驶定位模块的成本，加速该功能量产。

此外，本发明所述的VSLAM特征地图尺度恢复方法通过与高精度的定位结果进行标定可以使VSLAM具有比较高的定位精度，同时在标定过程中也可以获知VSLAM特征地图是否准确以及误差较大的区域。

相应地，本发明所述的VSLAM特征地图尺度恢复系统可以用于实施本发明上述方法，其同样具有上述的优点以及有益效果。

需要说明的是，本发明保护范围中现有技术部分并不局限于本申请文件所给出的实施例，所有不与本发明的方案相矛盾的现有技术，包括但不局限于在先专利文献、在先公开出版物，在先公开使用等等，都可纳入本发明的保护范围。

此外，本案中各技术特征的组合方式并不限本案权利要求中所记载的组合方式或是具体实施例所记载的组合方式，本案记载的所有技术特征可以以任何方式进行自由组合或结合，除非相互之间产生矛盾。

还需要注意的是，以上所列举的实施例仅为本发明具体实施例。显然本发明不局限于以上实施例，随之做出的类似变化或变形是本领域技术人员能从本发明公开的内容直接得出或者很容易便联想到的，均应属于本发明的保护范围。

Claims (8)

1.一种VSLAM特征地图尺度恢复方法，其特征在于，在安装有摄像头、激光雷达和组合惯性导航系统的车辆在指定区域内行驶的情况下，包括步骤：

100：获取摄像头采集的图像数据、激光雷达采集的点云数据以及组合惯性导航系统采集的高精度地图数据；

200：对摄像头、激光雷达和组合惯性导航系统的相对位置进行标定，以将所述图像数据、点云数据和高精度地图数据的坐标统一；

300：基于所述图像数据采用VSLAM方法生成无尺度的特征地图M_vslam和定位轨迹T_vslam；基于所述点云数据采用LSLAM方法生成具有相对尺度的特征地图M_lslam和定位轨迹T_lslam；

400：将定位轨迹T_lslam与高精度地图数据T_combine融合，得到绝对尺度定位轨迹T_all；其中，对于定位信号好的区域，采用高精度地图数据T_combine作为具有绝对尺度的第一定位轨迹；对于定位信号不好的区域，将定位轨迹T_lslam中的各轨迹点与高精度地图数据T_combine中的相应轨迹点进行匹配，得到定位轨迹T_lslam到高精度地图数据T_combine的转换矩阵；采用转换矩阵对定位轨迹T_lslam进行变换，得到具有绝对尺度的第二定位轨迹；将第一定位轨迹和第二定位轨迹组合，得到绝对尺度定位轨迹T_all；

500：采用绝对尺度定位轨迹T_all对定位轨迹T_vslam进行粗标定，得到第一缩放系数S₁与第一变换矩阵RT₁；

600：对经过粗标定的定位轨迹T_vslam进行精标定，得到第二缩放系数S₂与第二变换矩阵RT₂；

700：采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图M_vslam进行缩放和转换，得到具有绝对尺度的VSLAM特征地图，其中S＝S₁·S₂、RT＝RT₁·RT₂。

2.如权利要求1所述的VSLAM特征地图尺度恢复方法，其特征在于，所述摄像头为单目摄像头或环视摄像头。

3.如权利要求1所述的VSLAM特征地图尺度恢复方法，其特征在于，采用迭代最近邻算法将定位轨迹T_lslam中的各轨迹点与高精度地图数据T_combine中的相应轨迹点进行匹配。

4.如权利要求1所述的VSLAM特征地图尺度恢复方法，其特征在于，步骤500进一步包括：

501：将定位轨迹T_vslam与绝对尺度定位轨迹T_all在同一3D坐标系下可视化；

502：将定位轨迹T_vslam缩放至与绝对尺度定位轨迹T_all同一数量级；

503：将缩放后的定位轨迹T_vslam旋转、平移至与绝对尺度定位轨迹T_all大致重合，得到新定位轨迹T’_vslam以及第一缩放系数S₁与第一变换矩阵RT₁。

5.如权利要求4所述的VSLAM特征地图尺度恢复方法，其特征在于，步骤600进一步包括：

601：令当前定位轨迹T_cur＝T’_vslam，当前缩放系数S_cur＝S₁；

602：令最佳缩放系数S_best＝S_cur，采用梯度下降法进行S_cur迭代；

603：令当前目标函数值J_cur＝J_pre，J_pre表示前一时刻目标函数值，采用迭代最近邻算法将当前定位轨迹T_cur和绝对尺度定位轨迹T_all进行一次匹配，得到当前变换矩阵，然后更新当前定位轨迹T_cur；

604：基于当前定位轨迹T_cur和绝对尺度定位轨迹T_all计算当前目标函数值J_cur，并将当前目标函数值J_cur与前一时刻目标函数值J_pre的差值与设定的期望目标函数迭代差值ΔJ进行比较：若差值小于ΔJ，则将与当前定位轨迹T_cur对应的当前变换矩阵作为第二变换矩阵RT₂输出，并进行步骤605；否则，则转回步骤603；

605：将最佳缩放系数S_best与当前缩放系数S_cur的差值与设定的期望缩放系数迭代差值ΔS进行比较：若差值小于ΔS，则将当前缩放系数S_cur作为第二缩放系数S₂输出；否则，则转回步骤602。

6.一种VSLAM特征地图尺度恢复系统，其特征在于，其包括：

安装有摄像头、激光雷达和组合惯性导航系统的车辆，所述车辆在指定区域内行驶；所述摄像头采集并输出图像数据，所述激光雷达采集并输出点云数据，所述组合惯性导航系统采集并输出高精度地图数据；

预处理模块，其对摄像头、激光雷达和组合惯性导航系统的相对位置进行标定，以将所述图像数据、点云数据和高精度地图数据的坐标统一；

VSLAM模块，其基于所述图像数据生成无尺度的特征地图M_vslam和定位轨迹T_vslam；

LSLAM模块，其基于所述点云数据生成具有相对尺度的特征地图M_lslam和定位轨迹T_lslam；

数据融合模块，其将定位轨迹T_lslam与高精度地图数据T_combine融合，得到绝对尺度定位轨迹T_all；

粗标定模块，其采用绝对尺度定位轨迹T_all对定位轨迹T_vslam进行粗标定，得到第一缩放系数S₁与第一变换矩阵RT₁；

精标定模块，其经过粗标定的定位轨迹T_vslam进行精标定，得到第二缩放系数S₂与第二变换矩阵RT₂；

VSLAM特征地图尺度恢复模块，其采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图M_vslam进行缩放和转换，得到具有绝对尺度的VSLAM特征地图，其中S＝S₁·S₂、RT＝RT₁·RT₂;

其中所述数据融合模块被设置为执行下述步骤：

对于定位信号好的区域，采用高精度地图数据T_combine作为具有绝对尺度的第一定位轨迹；

对于定位信号不好的区域，将定位轨迹T_lslam中的各轨迹点与高精度地图数据T_combine中的相应轨迹点进行匹配，得到定位轨迹T_lslam到高精度地图数据T_combine的转换矩阵；采用转换矩阵对定位轨迹T_lslam进行变换，得到具有绝对尺度的第二定位轨迹；

将第一定位轨迹和第二定位轨迹组合，得到绝对尺度定位轨迹T_all。

7.如权利要求6所述的VSLAM特征地图尺度恢复系统，其特征在于，所述摄像头为单目摄像头，其视场角FOV>90°，其采样帧率>20Hz；或者所述摄像头为环视摄像头，其视场角FOV>120°，其采样帧率>20Hz。

8.如权利要求6所述的VSLAM特征地图尺度恢复系统，其特征在于，所述激光雷达设置在车辆顶部，其线数>16线，角分辨率<1°，采样频率>20Hz。

Images