CN112577499B - 一种vslam特征地图尺度恢复方法及*** - Google Patents
一种vslam特征地图尺度恢复方法及*** Download PDFInfo
- Publication number
- CN112577499B CN112577499B CN202011301722.2A CN202011301722A CN112577499B CN 112577499 B CN112577499 B CN 112577499B CN 202011301722 A CN202011301722 A CN 202011301722A CN 112577499 B CN112577499 B CN 112577499B
- Authority
- CN
- China
- Prior art keywords
- vslam
- track
- positioning
- scale
- positioning track
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/50—Context or environment of the image
- G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Multimedia (AREA)
- Theoretical Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种VSLAM特征地图尺度恢复方法,在安装有摄像头、激光雷达和组合惯性导航***的车辆在指定区域内行驶的情况下,包括步骤:获取摄像头采集的图像数据、激光雷达采集的点云数据以及组合惯性导航***采集的高精度地图数据;对三者相对位置进行标定,以将采集的数据坐标统一;采用VSLAM方法由图像数据生成无尺度的Mvslam和Tvslam;采用LSLAM方法由点云数据生成具有相对尺度的Mlslam和Tlslam;将Tlslam与高精度地图数据Tcombine融合,得绝对尺度定位轨迹Tall;采用Tall对Tvslam进行粗标定可得S1与RT1,将粗标定的定位轨迹Tvslam进行精标定,得到S2与RT2;采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图Mvslam进行缩放和转换,得到具有绝对尺度的VSLAM特征地图,其中S=S1·S2、RT=RT1·RT2。
Description
技术领域
本发明涉及一种地图生成方法和***,尤其涉及基于视觉的地图生成方法和***。
背景技术
近年来,随着无人驾驶技术的快速发展,无人车运用在日常生活中的可能性也越来越大。而车辆定位是自动驾驶领域的核心技术之一,对于停车楼、地库等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方法生成无尺度的特征地图Mvslam和定位轨迹Tvslam;基于所述点云数据采用LSLAM方法生成具有相对尺度的特征地图Mlslam和定位轨迹Tlslam;
400:将定位轨迹Tlslam与高精度地图数据Tcombine融合,得到绝对尺度定位轨迹Tall;
500:采用绝对尺度定位轨迹Tall对定位轨迹Tvslam进行粗标定,得到第一缩放系数S1与第一变换矩阵RT1;
600:对经过粗标定的定位轨迹Tvslam进行精标定,得到第二缩放系数S2与第二变换矩阵RT2;
700:采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图Mvslam进行缩放和转换,得到具有绝对尺度的VSLAM特征地图,其中S=S1·S2、RT=RT1·RT2。
在本发明中,通过利用安装有摄像头、激光雷达和组合惯性导航***的数据采集车在指定区域(如停车楼和停车库)按一定路线人工驾驶,采集数据。安装在车辆上的传感器(摄像头、激光雷达和组合惯性导航***)采集到图像数据、点云数据及高精度GPS定位轨迹数据后,采用VLSAM及LSLAM方法分别进行建图及定位。
将LSLAM得到的定位轨迹Tlslam与组合惯性导航***得到的高精度地图数据Tcombine融合,可以赋予Tlslam以绝对尺度(如经纬度)信息,得到绝对尺度定位轨迹Tall。最后利用绝对尺度定位轨迹Tall依次对定位轨迹Tvslam进行粗标定及精标定,即可得到恢复尺度的VSLAM特征地图,该特征地图最终可存储于本地或云端,并应用于自动驾驶车辆定位。
此外,需要说明的是,在本发明所述的VSLAM特征地图尺度恢复方法中,在上述步骤700中,“S=S1·S2、RT=RT1·RT2”中的“·”表示矩阵相乘。
进一步地,在本发明所述的VSLAM特征地图尺度恢复方法中,所述摄像头为单目摄像头或环视摄像头。
进一步地,在本发明所述的VSLAM特征地图尺度恢复方法中,所述步骤400进一步包括:
对于定位信号好的区域,采用高精度地图数据Tcombine作为具有绝对尺度的第一定位轨迹;
对于定位信号不好的区域,将定位轨迹Tlslam中的各轨迹点与高精度地图数据Tcombine中的相应轨迹点进行匹配,得到定位轨迹Tlslam到高精度地图数据Tcombine的转换矩阵;采用转换矩阵对定位轨迹Tlslam进行变换,得到具有绝对尺度的第二定位轨迹;
将第一定位轨迹和第二定位轨迹组合,得到绝对尺度定位轨迹Tall。
进一步地,在本发明所述的VSLAM特征地图尺度恢复方法中,采用迭代最近邻算法将定位轨迹Tlslam中的各轨迹点与高精度地图数据Tcombine中的相应轨迹点进行匹配。
进一步地,在本发明所述的VSLAM特征地图尺度恢复方法中,所述步骤500进一步包括:
501:将定位轨迹Tvslam与绝对尺度定位轨迹Tall在同一3D坐标系下可视化;
502:将定位轨迹Tvslam缩放至与绝对尺度定位轨迹Tall同一数量级;
503:将缩放后的定位轨迹Tvslam旋转、平移至与绝对尺度定位轨迹Tall大致重合,得到新定位轨迹T’vslam以及第一缩放系数S1与第一变换矩阵RT1。
进一步地,在本发明所述的VSLAM特征地图尺度恢复方法中,所述步骤600进一步包括:
601:令当前定位轨迹Tcur=T’vslam。,当前缩放系数Scur=S1;
602:令最佳缩放系数Sbest=Scur,采用梯度下降法进行Scur迭代;
603:令当前目标函数值Jcur=Jpre,Jpre表示前一时刻目标函数值,采用迭代最近邻算法将当前定位轨迹Tcur和绝对尺度定位轨迹Tall进行一次匹配,得到当前变换矩阵,然后更新当前定位轨迹Tcur;
604:基于当前定位轨迹Tcur和绝对尺度定位轨迹Tall计算当前目标函数值Jcur,并将当前目标函数值Jcur与前一时刻目标函数值Jpre的差值与设定的期望目标函数迭代差值ΔJ进行比较:若差值小于ΔJ,则将与当前定位轨迹Tcur对应的当前变换矩阵作为第二变换矩阵RT2输出,并进行步骤605;否则,则转回步骤603;
605:将最佳缩放系数Sbest与当前缩放系数Scur的差值与设定的期望缩放系数迭代差值ΔS进行比较:若差值小于ΔS,则将当前缩放系数Scur作为第二缩放系数S2输出;否则,则转回步骤602。
相应地,本发明的另一目的在于提供一种VSLAM特征地图尺度恢复***,该VSLAM特征地图尺度恢复***可以用于实施本发明上述的VSLAM特征地图尺度恢复方法。
为了实现上述目的,本发明提出了一种VSLAM特征地图尺度恢复***,其包括:
安装有摄像头、激光雷达和组合惯性导航***的车辆,所述车辆在指定区域内行驶;所述摄像头采集并输出图像数据,所述激光雷达采集并输出点云数据,所述组合惯性导航***采集并输出高精度地图数据;
预处理模块,其对摄像头、激光雷达和组合惯性导航***的相对位置进行标定,以将所述图像数据、点云数据和高精度地图数据的坐标统一;
VSLAM模块,其基于所述图像数据生成无尺度的特征地图Mvslam和定位轨迹Tvslam;
LSLAM模块,其基于所述点云数据生成具有相对尺度的特征地图Mlslam和定位轨迹Tlslam;
数据融合模块,其将定位轨迹Tlslam与高精度地图数据Tcombine融合,得到绝对尺度定位轨迹Tall;
粗标定模块,其采用绝对尺度定位轨迹Tall对定位轨迹Tvslam进行粗标定,得到第一缩放系数S1与第一变换矩阵RT1;
精标定模块,其经过粗标定的定位轨迹Tvslam进行精标定,得到第二缩放系数S2与第二变换矩阵RT2;
VSLAM特征地图尺度恢复模块,其采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图Mvslam进行缩放和转换,得到具有绝对尺度的VSLAM特征地图,其中S=S1·S2、RT=RT1·RT2。
进一步地,在本发明所述的VSLAM特征地图尺度恢复***中,所述摄像头为单目摄像头,其视场角FOV>90°,其采样帧率>20Hz;或者所述摄像头为环视摄像头,其视场角FOV>120°,其采样帧率>20Hz。
进一步地,在本发明所述的VSLAM特征地图尺度恢复***中,所述激光雷达设置在车辆顶部,其线数>16线,角分辨率<1°,采样频率>20Hz。
进一步地,在本发明所述的VSLAM特征地图尺度恢复***中,所述数据融合模块被设置为执行下述步骤:
对于定位信号好的区域,采用高精度地图数据Tcombine作为具有绝对尺度的第一定位轨迹;
对于定位信号不好的区域,将定位轨迹Tlslam中的各轨迹点与高精度地图数据Tcombine中的相应轨迹点进行匹配,得到定位轨迹Tlslam到高精度地图数据Tcombine的转换矩阵;采用转换矩阵对定位轨迹Tlslam进行变换,得到具有绝对尺度的第二定位轨迹;
将第一定位轨迹和第二定位轨迹组合,得到绝对尺度定位轨迹Tall。
本发明所述的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方法生成无尺度的特征地图Mvslam和定位轨迹Tvslam;基于所述点云数据采用LSLAM方法生成具有相对尺度的特征地图Mlslam和定位轨迹Tlslam;
400:将定位轨迹Tlslam与高精度地图数据Tcombine融合,得到绝对尺度定位轨迹Tall;
500:采用绝对尺度定位轨迹Tall对定位轨迹Tvslam进行粗标定,得到第一缩放系数S1与第一变换矩阵RT1;
600:对经过粗标定的定位轨迹Tvslam进行精标定,得到第二缩放系数S2与第二变换矩阵RT2;
700:采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图Mvslam进行缩放和转换,得到具有绝对尺度的VSLAM特征地图,其中S=S1·S2、RT=RT1·RT2。上述“·”表示矩阵相乘。
由此可见,本发明所述的VSLAM特征地图尺度恢复方法通过利用安装有摄像头、激光雷达和组合惯性导航***的数据采集车在指定区域(如停车楼和停车库)按一定路线人工驾驶,并采集数据。安装在车辆上的传感器(摄像头、激光雷达和组合惯性导航***)采集到图像数据、点云数据及高精度GPS定位轨迹数据后,可以采用VLSAM及LSLAM方法分别进行建图及定位。
相应地,将LSLAM得到的定位轨迹Tlslam与组合惯性导航***得到的高精度地图数据Tcombine融合,可以赋予Tlslam以绝对尺度(如经纬度)信息,得到绝对尺度定位轨迹Tall。最后利用绝对尺度定位轨迹Tall依次对定位轨迹Tvslam进行粗标定及精标定,即可得到恢复尺度的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内拍摄的图像序列为It1-t2,该图像序列需要根据摄像头内参数进行图像矫正;相应地,激光雷达在所需要的时间段t1-t2内扫描得到的激光点云序列Pt1-t2;组合惯性导航***在所需要的时间段t1-t2内输出的坐标转换到车辆后轴中点后的定位轨迹T t1-t2。
在本实施方式中,本发明所述的VSLAM特征地图尺度恢复***中的VSLAM模块,可以基于图像数据It1-t2生成无尺度的特征地图Mvslam和时间段t1-t2内车辆行驶过的无尺度信息的定位轨迹Tvslam。需要说明的是,VSLAM有许多现有的成熟算法,例如:ORB-SLAM、LSD-SLAM、MonoSLAM、PTAM和SVO,用户可以根据实际需要选用合适的算法作为VSLAM模块模型。
相应地,在本实施方式中,本发明所述的VSLAM特征地图尺度恢复***中的LSLAM模块可以基于点云数据Pt1-t2,生成具有相对尺度的特征地图Mlslam和时间段t1-t2内车辆行驶过的具有相对尺度信息的定位轨迹Tlslam。需要说明的是,LSLAM有许多现有的成熟算法,例如:Cartographer和Loam,用户可以根据实际需要选用合适的算法作为LSLAM模块模型。
同样的,在本实施方式中,本发明所述的VSLAM特征地图尺度恢复***中的数据融合模块可以将LSLAM模块得到的定位轨迹Tlslam与组合惯性导航***得到的高精度地图数据Tcombine融合,从而得到绝对尺度定位轨迹Tall。
但需要注意的是,在本实施方式中,上述融合得到绝对尺度定位轨迹Tall的步骤可以进一步包括以下操作:
对于室外GPS定位信号好的区域,优先选择信任组合惯性导航***得到的高精度地图数据Tcombine作为具有绝对尺度的第一定位轨迹;
而对于室内停车楼或地库这些定位信号不好的区域,优先选择信任激光雷达的定位轨迹Tlslam。因组合惯性导航***和激光雷达同时运行,通过查找邻近时间戳,定位轨迹Tlslam中的每一个轨迹点都可以和高精度地图数据Tcombine中的相应轨迹点进行匹配。因此,只需对室外部分定位轨迹Tlslam到高精度地图数据Tcombine的轨迹点进行迭代最近邻算法(ICP)匹配,即可得到定位轨迹Tlslam到高精度地图数据Tcombine的转换矩阵。采用转换矩阵对定位轨迹Tlslam进行变换,得到具有绝对尺度的第二定位轨迹;
对于同时包含室内和室外的区域,室内部分选择激光雷达下的绝对定位轨迹Tall,室外部分选择组合惯导的绝对定位轨迹Tall,组合后即得到完整绝对尺度定位轨迹Tall。
此外,需要说明的是,在本发明中,采用VSLAM模块生成的特征地图Mvslam与定位轨迹Tvslam无尺度,或者说非真实尺度,与绝对尺度定位轨迹Tall相差可能非常大,若直接进行精标定,进行迭代最近邻算法(ICP)迭代时较难收敛到期望精度,因此在本实施方式中可以优选的先进行粗标定,具体步骤如图3所示。
图3示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下进行粗标定的流程图。
结合参考图1可知,在本发明所述VSLAM特征地图尺度恢复方法的步骤500中,需要采用绝对尺度定位轨迹Tall对定位轨迹Tvslam进行粗标定,以得到第一缩放系数S1与第一变换矩阵RT1。
如图3所示,在本实施方式中,本发明所述的步骤500可以进一步包括如下步骤:
501:将定位轨迹Tvslam与绝对尺度定位轨迹Tall在同一3D坐标系下可视化;
502:将定位轨迹Tvslam缩放至与绝对尺度定位轨迹Tall同一数量级;
503:将缩放后的定位轨迹Tvslam旋转、平移至与绝对尺度定位轨迹Tall大致重合,得到新定位轨迹T’vslam以及第一缩放系数S1与第一变换矩阵RT1。
图4示意性地显示了本发明所述的VSLAM特征地图尺度恢复方法在一种实施方式下进行精标定的流程图。
相应地,在粗标定基础上,精标定流程如图4所示。在本发明所述VSLAM特征地图尺度恢复方法的步骤600中,需要对经过粗标定的定位轨迹Tvslam进行精标定,以得到第二缩放系数S2与第二变换矩阵RT2。
如图4所示,在本实施方式中,本发明所述VSLAM特征地图尺度恢复方法的步骤600可以进一步包括如下步骤:
601:令当前定位轨迹Tcur=T’vslam。,当前缩放系数Scur=S1;
在上述步骤601中,可以先初始化最佳缩放系数Sbest、前一时刻目标函数值Jpre、期望缩放系数迭代差值ΔS和期望目标函数迭代差值ΔJ。
602:令最佳缩放系数Sbest=Scur,采用梯度下降法进行Scur迭代;
603:令当前目标函数值Jcur=Jpre,Jpre表示前一时刻目标函数值,采用迭代最近邻算法将当前定位轨迹Tcur和绝对尺度定位轨迹Tall进行一次匹配,得到当前变换矩阵,然后更新当前定位轨迹Tcur;
604:基于当前定位轨迹Tcur和绝对尺度定位轨迹Tall计算当前目标函数值Jcur,并将当前目标函数值Jcur与前一时刻目标函数值Jpre的差值与设定的期望目标函数迭代差值ΔJ进行比较:若差值小于ΔJ,则将与当前定位轨迹Tcur对应的当前变换矩阵作为第二变换矩阵RT2输出,并进行步骤605;否则,则转回步骤603;
605:将最佳缩放系数Sbest与当前缩放系数Scur的差值与设定的期望缩放系数迭代差值ΔS进行比较:若差值小于ΔS,则将当前缩放系数Scur作为第二缩放系数S2输出;否则,则转回步骤602。
经过上述操作步骤后,可以得到第二缩放系数S2与第二变换矩阵RT2,由于无尺度的特征地图Mvslam和定位轨迹Tvslam同时采集,尺度相同,只需将Mvslam利用S=S1·S2、RT=RT1·RT2进行缩放和变换后即可恢复特征地图尺度,最终得到具有绝对尺度的VSLAM特征地图Mvslam-real。
综上所述可以看出,本发明所述的VSLAM特征地图尺度恢复方法通过使用组合导航和LSLAM定位结果恢复了采用单目摄像头定位的VSLAM特征地图的尺度,解决了单目摄像头定位没有实际尺寸的缺陷。在本发明中,激光雷达和组合惯性导航***只需在建图时被使用,在最终定位应用时只需单目摄像头即可,可以大大降低自动驾驶定位模块的成本,加速该功能量产。
此外,本发明所述的VSLAM特征地图尺度恢复方法通过与高精度的定位结果进行标定可以使VSLAM具有比较高的定位精度,同时在标定过程中也可以获知VSLAM特征地图是否准确以及误差较大的区域。
相应地,本发明所述的VSLAM特征地图尺度恢复***可以用于实施本发明上述方法,其同样具有上述的优点以及有益效果。
需要说明的是,本发明保护范围中现有技术部分并不局限于本申请文件所给出的实施例,所有不与本发明的方案相矛盾的现有技术,包括但不局限于在先专利文献、在先公开出版物,在先公开使用等等,都可纳入本发明的保护范围。
此外,本案中各技术特征的组合方式并不限本案权利要求中所记载的组合方式或是具体实施例所记载的组合方式,本案记载的所有技术特征可以以任何方式进行自由组合或结合,除非相互之间产生矛盾。
还需要注意的是,以上所列举的实施例仅为本发明具体实施例。显然本发明不局限于以上实施例,随之做出的类似变化或变形是本领域技术人员能从本发明公开的内容直接得出或者很容易便联想到的,均应属于本发明的保护范围。
Claims (8)
1.一种VSLAM特征地图尺度恢复方法,其特征在于,在安装有摄像头、激光雷达和组合惯性导航***的车辆在指定区域内行驶的情况下,包括步骤:
100:获取摄像头采集的图像数据、激光雷达采集的点云数据以及组合惯性导航***采集的高精度地图数据;
200:对摄像头、激光雷达和组合惯性导航***的相对位置进行标定,以将所述图像数据、点云数据和高精度地图数据的坐标统一;
300:基于所述图像数据采用VSLAM方法生成无尺度的特征地图Mvslam和定位轨迹Tvslam;基于所述点云数据采用LSLAM方法生成具有相对尺度的特征地图Mlslam和定位轨迹Tlslam;
400:将定位轨迹Tlslam与高精度地图数据Tcombine融合,得到绝对尺度定位轨迹Tall;其中,对于定位信号好的区域,采用高精度地图数据Tcombine作为具有绝对尺度的第一定位轨迹;对于定位信号不好的区域,将定位轨迹Tlslam中的各轨迹点与高精度地图数据Tcombine中的相应轨迹点进行匹配,得到定位轨迹Tlslam到高精度地图数据Tcombine的转换矩阵;采用转换矩阵对定位轨迹Tlslam进行变换,得到具有绝对尺度的第二定位轨迹;将第一定位轨迹和第二定位轨迹组合,得到绝对尺度定位轨迹Tall;
500:采用绝对尺度定位轨迹Tall对定位轨迹Tvslam进行粗标定,得到第一缩放系数S1与第一变换矩阵RT1;
600:对经过粗标定的定位轨迹Tvslam进行精标定,得到第二缩放系数S2与第二变换矩阵RT2;
700:采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图Mvslam进行缩放和转换,得到具有绝对尺度的VSLAM特征地图,其中S=S1·S2、RT=RT1·RT2。
2.如权利要求1所述的VSLAM特征地图尺度恢复方法,其特征在于,所述摄像头为单目摄像头或环视摄像头。
3.如权利要求1所述的VSLAM特征地图尺度恢复方法,其特征在于,采用迭代最近邻算法将定位轨迹Tlslam中的各轨迹点与高精度地图数据Tcombine中的相应轨迹点进行匹配。
4.如权利要求1所述的VSLAM特征地图尺度恢复方法,其特征在于,步骤500进一步包括:
501:将定位轨迹Tvslam与绝对尺度定位轨迹Tall在同一3D坐标系下可视化;
502:将定位轨迹Tvslam缩放至与绝对尺度定位轨迹Tall同一数量级;
503:将缩放后的定位轨迹Tvslam旋转、平移至与绝对尺度定位轨迹Tall大致重合,得到新定位轨迹T’vslam以及第一缩放系数S1与第一变换矩阵RT1。
5.如权利要求4所述的VSLAM特征地图尺度恢复方法,其特征在于,步骤600进一步包括:
601:令当前定位轨迹Tcur=T’vslam,当前缩放系数Scur=S1;
602:令最佳缩放系数Sbest=Scur,采用梯度下降法进行Scur迭代;
603:令当前目标函数值Jcur=Jpre,Jpre表示前一时刻目标函数值,采用迭代最近邻算法将当前定位轨迹Tcur和绝对尺度定位轨迹Tall进行一次匹配,得到当前变换矩阵,然后更新当前定位轨迹Tcur;
604:基于当前定位轨迹Tcur和绝对尺度定位轨迹Tall计算当前目标函数值Jcur,并将当前目标函数值Jcur与前一时刻目标函数值Jpre的差值与设定的期望目标函数迭代差值ΔJ进行比较:若差值小于ΔJ,则将与当前定位轨迹Tcur对应的当前变换矩阵作为第二变换矩阵RT2输出,并进行步骤605;否则,则转回步骤603;
605:将最佳缩放系数Sbest与当前缩放系数Scur的差值与设定的期望缩放系数迭代差值ΔS进行比较:若差值小于ΔS,则将当前缩放系数Scur作为第二缩放系数S2输出;否则,则转回步骤602。
6.一种VSLAM特征地图尺度恢复***,其特征在于,其包括:
安装有摄像头、激光雷达和组合惯性导航***的车辆,所述车辆在指定区域内行驶;所述摄像头采集并输出图像数据,所述激光雷达采集并输出点云数据,所述组合惯性导航***采集并输出高精度地图数据;
预处理模块,其对摄像头、激光雷达和组合惯性导航***的相对位置进行标定,以将所述图像数据、点云数据和高精度地图数据的坐标统一;
VSLAM模块,其基于所述图像数据生成无尺度的特征地图Mvslam和定位轨迹Tvslam;
LSLAM模块,其基于所述点云数据生成具有相对尺度的特征地图Mlslam和定位轨迹Tlslam;
数据融合模块,其将定位轨迹Tlslam与高精度地图数据Tcombine融合,得到绝对尺度定位轨迹Tall;
粗标定模块,其采用绝对尺度定位轨迹Tall对定位轨迹Tvslam进行粗标定,得到第一缩放系数S1与第一变换矩阵RT1;
精标定模块,其经过粗标定的定位轨迹Tvslam进行精标定,得到第二缩放系数S2与第二变换矩阵RT2;
VSLAM特征地图尺度恢复模块,其采用最终缩放系数S和最终变换矩阵RT对无尺度的特征地图Mvslam进行缩放和转换,得到具有绝对尺度的VSLAM特征地图,其中S=S1·S2、RT=RT1·RT2;
其中所述数据融合模块被设置为执行下述步骤:
对于定位信号好的区域,采用高精度地图数据Tcombine作为具有绝对尺度的第一定位轨迹;
对于定位信号不好的区域,将定位轨迹Tlslam中的各轨迹点与高精度地图数据Tcombine中的相应轨迹点进行匹配,得到定位轨迹Tlslam到高精度地图数据Tcombine的转换矩阵;采用转换矩阵对定位轨迹Tlslam进行变换,得到具有绝对尺度的第二定位轨迹;
将第一定位轨迹和第二定位轨迹组合,得到绝对尺度定位轨迹Tall。
7.如权利要求6所述的VSLAM特征地图尺度恢复***,其特征在于,所述摄像头为单目摄像头,其视场角FOV>90°,其采样帧率>20Hz;或者所述摄像头为环视摄像头,其视场角FOV>120°,其采样帧率>20Hz。
8.如权利要求6所述的VSLAM特征地图尺度恢复***,其特征在于,所述激光雷达设置在车辆顶部,其线数>16线,角分辨率<1°,采样频率>20Hz。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011301722.2A CN112577499B (zh) | 2020-11-19 | 2020-11-19 | 一种vslam特征地图尺度恢复方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011301722.2A CN112577499B (zh) | 2020-11-19 | 2020-11-19 | 一种vslam特征地图尺度恢复方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112577499A CN112577499A (zh) | 2021-03-30 |
CN112577499B true CN112577499B (zh) | 2022-10-11 |
Family
ID=75122838
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011301722.2A Active CN112577499B (zh) | 2020-11-19 | 2020-11-19 | 一种vslam特征地图尺度恢复方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112577499B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113484843A (zh) * | 2021-06-02 | 2021-10-08 | 福瑞泰克智能***有限公司 | 一种激光雷达与组合导航间外参数的确定方法及装置 |
CN117409071B (zh) * | 2023-12-12 | 2024-03-01 | 知行汽车科技(苏州)股份有限公司 | 单目vslam的空间尺度恢复方法、装置、设备及介质 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108801276A (zh) * | 2018-07-23 | 2018-11-13 | 奇瑞汽车股份有限公司 | 高精度地图生成方法及装置 |
CN109341706A (zh) * | 2018-10-17 | 2019-02-15 | 张亮 | 一种面向无人驾驶汽车的多特征融合地图的制作方法 |
CN109920011A (zh) * | 2019-05-16 | 2019-06-21 | 长沙智能驾驶研究院有限公司 | 激光雷达与双目摄像头的外参标定方法、装置及设备 |
CN111090084A (zh) * | 2018-10-24 | 2020-05-01 | 舜宇光学(浙江)研究院有限公司 | 多激光雷达外参标定方法、标定装置、标定***和电子设备 |
CN111089579A (zh) * | 2018-10-22 | 2020-05-01 | 北京地平线机器人技术研发有限公司 | 异构双目slam方法、装置及电子设备 |
CN111462029A (zh) * | 2020-03-27 | 2020-07-28 | 北京百度网讯科技有限公司 | 视觉点云与高精地图融合方法、装置和电子设备 |
CN111522043A (zh) * | 2020-04-30 | 2020-08-11 | 北京联合大学 | 一种无人车激光雷达快速重新匹配定位方法 |
CN111696155A (zh) * | 2020-06-08 | 2020-09-22 | 华南理工大学 | 一种基于单目视觉的多传感融合机器人定位方法 |
CN111750853A (zh) * | 2020-06-24 | 2020-10-09 | 国汽(北京)智能网联汽车研究院有限公司 | 一种地图建立方法、装置及存储介质 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10223806B1 (en) * | 2017-08-23 | 2019-03-05 | TuSimple | System and method for centimeter precision localization using camera-based submap and LiDAR-based global map |
US11340632B2 (en) * | 2018-03-27 | 2022-05-24 | Uatc, Llc | Georeferenced trajectory estimation system |
CN111323027A (zh) * | 2018-12-17 | 2020-06-23 | 兰州大学 | 一种基于激光雷达与环视相机融合制作高精度地图方法及装置 |
-
2020
- 2020-11-19 CN CN202011301722.2A patent/CN112577499B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108801276A (zh) * | 2018-07-23 | 2018-11-13 | 奇瑞汽车股份有限公司 | 高精度地图生成方法及装置 |
CN109341706A (zh) * | 2018-10-17 | 2019-02-15 | 张亮 | 一种面向无人驾驶汽车的多特征融合地图的制作方法 |
CN111089579A (zh) * | 2018-10-22 | 2020-05-01 | 北京地平线机器人技术研发有限公司 | 异构双目slam方法、装置及电子设备 |
CN111090084A (zh) * | 2018-10-24 | 2020-05-01 | 舜宇光学(浙江)研究院有限公司 | 多激光雷达外参标定方法、标定装置、标定***和电子设备 |
CN109920011A (zh) * | 2019-05-16 | 2019-06-21 | 长沙智能驾驶研究院有限公司 | 激光雷达与双目摄像头的外参标定方法、装置及设备 |
CN111462029A (zh) * | 2020-03-27 | 2020-07-28 | 北京百度网讯科技有限公司 | 视觉点云与高精地图融合方法、装置和电子设备 |
CN111522043A (zh) * | 2020-04-30 | 2020-08-11 | 北京联合大学 | 一种无人车激光雷达快速重新匹配定位方法 |
CN111696155A (zh) * | 2020-06-08 | 2020-09-22 | 华南理工大学 | 一种基于单目视觉的多传感融合机器人定位方法 |
CN111750853A (zh) * | 2020-06-24 | 2020-10-09 | 国汽(北京)智能网联汽车研究院有限公司 | 一种地图建立方法、装置及存储介质 |
Non-Patent Citations (2)
Title |
---|
Qingqu Wang.On SLAM Based on Monocular Vision and Lidar Fusion System *.《2018 IEEE CSAA Guidance, Navigation and Control Conference》.2020,全文. * |
薛 杨,等.基准地图测绘下的视觉导航算法.《兵工自动化》.2019,第38卷(第10期),全文. * |
Also Published As
Publication number | Publication date |
---|---|
CN112577499A (zh) | 2021-03-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109934920B (zh) | 基于低成本设备的高精度三维点云地图构建方法 | |
CN111522043B (zh) | 一种无人车激光雷达快速重新匹配定位方法 | |
CN109696663B (zh) | 一种车载三维激光雷达标定方法和*** | |
CN107505644B (zh) | 基于车载多传感器融合的三维高精度地图生成***及方法 | |
CN110033489B (zh) | 一种车辆定位准确性的评估方法、装置及设备 | |
CN107167826B (zh) | 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位***及方法 | |
CN111882612A (zh) | 一种基于三维激光检测车道线的车辆多尺度定位方法 | |
CN111487642A (zh) | 基于三维激光和双目视觉的变电站巡检机器人定位导航***及方法 | |
CN112577499B (zh) | 一种vslam特征地图尺度恢复方法及*** | |
CN109596121B (zh) | 一种机动站自动目标检测与空间定位方法 | |
CN110187375A (zh) | 一种基于slam定位结果提高定位精度的方法及装置 | |
CN111815765B (zh) | 一种基于异构数据融合的图像三维重建方法 | |
CN103885455B (zh) | 跟踪测量机器人 | |
CN110827403B (zh) | 一种矿山三维点云地图的构建方法及装置 | |
CN113220818B (zh) | 一种停车场自动建图与高精度定位方法 | |
CN112465732A (zh) | 一种车载激光点云与序列全景影像的配准方法 | |
CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
CN110986888A (zh) | 一种航空摄影一体化方法 | |
CN112179338A (zh) | 一种基于视觉和惯导融合的低空无人机自身定位方法 | |
CN114485654A (zh) | 一种基于高精地图的多传感器融合定位方法及装置 | |
CN116758234A (zh) | 一种基于多点云数据融合的山地地形建模方法 | |
CN115574816A (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN113405555B (zh) | 一种自动驾驶的定位传感方法、***及装置 | |
WO2020113425A1 (en) | Systems and methods for constructing high-definition map |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |