CN114063127A - 一种多焦距视觉slam与gps融合的方法及存储介质 - Google Patents

一种多焦距视觉slam与gps融合的方法及存储介质 Download PDF

Info

Publication number
CN114063127A
CN114063127A CN202111355852.9A CN202111355852A CN114063127A CN 114063127 A CN114063127 A CN 114063127A CN 202111355852 A CN202111355852 A CN 202111355852A CN 114063127 A CN114063127 A CN 114063127A
Authority
CN
China
Prior art keywords
focal
gps
image
camera
slam
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.)
Pending
Application number
CN202111355852.9A
Other languages
English (en)
Inventor
冯明驰
邓程木
李成南
刘景林
王鑫
孙博望
岑明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing University of Post and Telecommunications
Original Assignee
Chongqing University of Post and Telecommunications
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Chongqing University of Post and Telecommunications filed Critical Chongqing University of Post and Telecommunications
Priority to CN202111355852.9A priority Critical patent/CN114063127A/zh
Publication of CN114063127A publication Critical patent/CN114063127A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Studio Devices (AREA)

Abstract

本发明请求保护一种多焦距视觉SLAM与GPS融合的方法，所属领域为自动驾驶及智能汽车环境地图构建和定位领域。包括以下步骤：1.安装多焦距相机和GPS模块，短焦距和长焦距相机水平放置。2.多焦距相机之间的标定，同时采集获取多焦距图像和GPS数据。3.根据多焦距相机焦距的差异，分别基于图像金字塔提取图像特征点，进行特征匹配。4.采集到的GPS定位数据坐标转换到多焦距视觉SLAM世界坐标系下。5.转换后的GPS数据和多焦距视觉SLAM数据进行时间戳对齐。6.时间戳对齐后的GPS数据和SLAM图像数据构建图优化模型优化位姿。7.由SLAM生成的运行轨迹在地图上匹配对应的地图道路轨迹。

Description

一种多焦距视觉SLAM与GPS融合的方法及存储介质
技术领域
本发明属于自动驾驶及智能汽车环境地图构建和定位领域，具体涉及一种多焦距视觉SLAM与GPS融合及实现方法。
背景技术
同时定位与建图(SLAM)是近二十年来计算机视觉和机器人领域的研究热点，它能实时构建未知环境地图，并定位自身位置。视觉SLAM因其分辨率高、采集环境图像信息丰富、简易便携、硬件成本低、定位精度高且无需环境先验信息等优势成为SLAM研究的热门领域。现有的双目视觉SLAM为了获得良好的视觉地图构建和定位精度，一般都使用两个相同的短焦距相机组合，其能获得广阔的图像视野，但在汽车驾驶领域，更需要利用相机对周围环境感知，仅使用短焦距相机不能获得远距离场景信息，不能对远距离场景的对象检测和定位。因此需要结合长焦距相机对远距离环境感知。多焦距的相机图像能同时获取广阔的图像视野与远距离的信息，且对转弯场景具有更强的稳定性。
全球定位系统(GPS)是当前行车定位不可或缺的技术，在智能驾驶定位中也担负着相当重要的职责。GPS系统包括太空中的32颗GPS卫星，地面上1个主控站、3个数据注入站和5个监测站及作为用户端的GPS接收机。最少只需其中3颗卫星，就能迅速确定用户端在地球上所处的位置及海拔高度。现在民用GPS可以达到十米左右的定位精度。GPS系统使用低频讯号，纵使气候不佳仍能保持相当的讯号穿透性。GPS定位系统是利用卫星基本三角定位原理、GPS接收装置以量测无线电信号的传输时间来量测距离。卫星距离测量存在着卫星钟与播延迟导致的误差等问题，通常利用差分技术来消除或者降低这些误差，让GPS达到更高的精度。然而无人车是在复杂的动态环境中行驶，尤其在大城市，GPS多路径反射的问题会很明显，这样得到的GPS定位信息很容易就有几米的误差。
经过检索，最接近的现有技术中，201910849671.8，同时定位建图方法及装置。其中，方法包括：对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；根据各帧双目图像的三维路标点云，获取亲和度矩阵；根据所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体；根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；其中，亲和度，为两个路标属于同一刚体的对数概率。上述发明实施例提供的同时定位建图方法及装置，根据亲和度进行层次聚类，能更准确地检测出场景中的运动物体并对运动进行建模和位姿估计，并且计算量更小，从而能提高同时定位建图的精度和速度。
本发明采用多焦距相机获取图像，同时兼顾了广视野与更远距离信息，所提供的SLAM具有更强的稳定性，同时在地图上进行道路轨迹准确定位。
发明内容
本发明旨在解决以上现有技术的问题。提出了一种多焦距视觉SLAM与GPS融合的方法。本发明的技术方案如下：
一种多焦距视觉SLAM与GPS融合的方法，其包括以下步骤：
步骤1.安装多焦距相机和GPS模块，多焦距视觉SLAM由短焦距相机和长焦距相机组成，短焦距和长焦距相机水平放置；步骤2.多焦距相机之间进行标定，同时采集多焦距图像和GPS数据，获取多焦距图像数据和GPS数据；步骤3.根据多焦距相机焦距的差异，分别基于图像金字塔提取图像特征点，进行特征匹配；步骤4.对采集到的GPS定位数据进行坐标转换到多焦距视觉SLAM世界坐标系下；步骤5.对转换后的GPS数据和多焦距视觉SLAM数据进行时间戳对齐；步骤6.对时间戳对齐后的GPS数据和SLAM图像数据构建图优化模型优化位姿；步骤7.由SLAM生成的运行轨迹在地图上匹配到对应的地图道路轨迹。
进一步的，所述步骤2多焦距相机之间进行标定，同时采集多焦距图像和GPS数据，获取多焦距图像数据和GPS数据，具体包括：
步骤21，根据短焦距相机和长焦距相机安装位置，完成多焦距相机标定；
步骤22，实现短焦距相机和长焦距相机的信息同步，在时间上，要求短焦距相机和长焦距相机同步拍摄；在空间上，短焦距相机和长焦距相机都安装在车顶前方，短焦距相机位于车顶前方左侧，长焦距相机位于车顶前方右侧；
步骤23，相机的图像采集频率保持恒定，采集过程中相机设置自动曝光；采集多焦距图像数据和GPS数据。
进一步的，所述步骤3根据多焦距相机焦距的差异，分别基于图像金字塔提取图像特征点，进行特征匹配具体包括：
步骤31，对图像构建图像金字塔，根据短焦距相机和长焦距相机的焦距差异，确定图像金字塔下采样的缩放因子s；
步骤32，根据多焦距图像差异，只选择短焦距和长焦距图像金字塔中具有相同图像信息大小的图层提取ORB特征点，如短焦距图像金字塔顶上ldis层不提取特征点，长焦距相机图像金字塔底下ldis层不提取特征点；
步骤33，获取需要提取特征点的对应层所需要提取的ORB特征点数量；
步骤34，使用归一化互相关算法确定在短焦距相机图像中与长焦距相机图像相同的部分区域，称为ROI区域；
步骤35，对短焦距相机图像金字塔某层的ROI区域内使用四叉树算法增加提取ORB特征点的数量，减少ROI区域外ORB特征点提取的数量；
步骤36，对提取的特征点进行特征匹配，并筛选删除误匹配。
进一步的，所述步骤33中，获取需要提取特征点的对应层所需要提取的ORB特征点数量为：
Figure BDA0003357048020000031
其中，s表示图像金字塔缩放比例，ldis表示相同大小图像特征在短焦距图像金字塔和长焦距图像金字塔中的固定层次差、n表示图像金字塔缩放层数，N表示总共需要提取的ORB特征点数量，α表示所需要提取ORB特征点的图层，Nα表示图像金字塔中图层α提取的ORB特征点数量。
进一步的，所述步骤34的归一化互相关算法具体表示为：
Figure BDA0003357048020000041
选择ρ(x,y)最大值对应的区域为ROI短焦距相机图像的部分区域。其中S为短焦距相机图像，m、n分别表示长焦距相机图像长和宽，i、j分别表示图像像素坐标点，X、Y分别表示短焦距相机图像中和长焦距相机图像相同大小的区域块，g为长焦距相机图像，
Figure BDA0003357048020000042
为短焦距相机图像平均灰度，
Figure BDA0003357048020000043
长焦距相机图像平均灰度。
进一步的，所述步骤4对采集到的GPS定位数据进行坐标转换到多焦距视觉SLAM世界坐标系下，具体包括：
步骤41，根据多焦距视觉SLAM世界坐标系以SLAM第一帧建立的，在整个多焦距视觉SLAM过程不发生改变，求出所述多焦距视觉SLAM第一帧到初始帧的平移向量在所述SLAM世界坐标系和所述GPS坐标系的坐标；该平移矢量在SLAM世界坐标系描述：
tw＝Rcw*(posc,init-posc,first)
tw表示世界坐标系，Rcw表示相机坐标系到世界坐标系的转换，posc,init表示初始化时相机位姿，posc,first表示相机第一帧位姿。
在GPS坐标系的描述为：
tGPS＝posGPS,init-posGPS,first
tGPS表示GPS坐标系，posGPS,init表示GPS初始化时位姿，posGPS,first表示GPS第一帧位姿。
根据两个坐标系的坐标求出所述GPS坐标系与所述多焦距双目SLAM世界坐标系之间的旋转矩阵：
RGPSw*tGPS＝tw
RGPSw表示GPS坐标系到相机世界坐标系的旋转矩阵。
进一步的，所述步骤6对时间戳对齐后的GPS数据和SLAM图像数据构建图优化模型优化位姿，具体包括：
步骤61，k时刻，在位置xk处，传感器进行一次观测，得到数据zk，观测方程为：
zk＝h(xk)
由于观测总是存在误差，定义误差：
ek＝zk-h(xk)
步骤62，构建全局位姿图，需要求解的位姿作为优化图的顶点，观测值作为边，构建目标函数，观测值包括相机对路标点的观测以及GPS数据：
Figure BDA0003357048020000051
其中ek表示误差，包括图像重投影误差以及GPS定位数据误差项；Ωk表示信息矩阵，是协方差矩阵的逆，是一个对称矩阵。
步骤63，运用图优化器部署出上述全局位姿图的顶点和边，求解出最优的位姿。
进一步的，所述步骤7由SLAM生成的运行轨迹在地图上匹配到对应的地图道路轨迹中，具体包括：
步骤71，建立路网有向图，获取道路的节点ID、坐标信息及属性信息,从而建立路网的有向图G(V,E),其中V的元素为道路端点,E的元素为道路路段；
步骤72，生成网格索引，使用网格索引，将整个地图分成M×N个网格，通过包含分析算法，预先计算每个网格中相交的路段，获取轨迹点的候选路段，网格索引与路段数据通过路段ID相关联；
步骤73，对轨迹点设置半径为R的缓冲区,基于网格索引确定轨迹点所处的网格，并查询以该网格为中心的3×3的网格内距该轨迹点最近的K条路段作为候选路段；
步骤74，计算发射概率，发射概率即在某种隐含状态下得到某种观测值的概率，考虑距离因素；
步骤75，计算传递概率；
步骤76，使用维特比算法(每一段选出最优路径删除其他路径，一直迭代到终点)求出最优路径。
进一步的，所述计算发射概率，发射概率即在某种隐含状态下得到某种观测值的概率，考虑距离因素，具体公式为：
Figure BDA0003357048020000061
其中，σ表示获取轨迹数据误差的标准差，pi表示轨迹点，
Figure BDA0003357048020000062
表示第k个点对应的候选路段，
Figure BDA0003357048020000063
表示从轨迹点到候选路段的距离。
步骤81，传递概率基于道路一般是两点连线的直线最近距离，前后路段间的路网距离越小，传递概率越大：
Figure BDA0003357048020000064
其中，
Figure BDA0003357048020000065
和
Figure BDA0003357048020000066
分别为第t+1个轨迹点和第t个轨迹点对应的候选路段，a是设定的系数，D代表候选路段间的实际路网距离；
一种存储介质，其上存储有计算机程序，其该计算机程序被处理器执行时实现任一项所述的多焦距视觉SLAM与GPS融合的方法。
本发明的优点及有益效果如下：
本发明采用的多焦距相机实现视觉视觉SLAM，同时融合了GPS信息，提高SLAM的定位精度；同时将SLAM形成的轨迹与地图道路进行匹配，得到在地图上的准确定位，具体包含以下方面：
1.多焦距视觉SLAM。之前的立体视觉SLAM大多采用相同焦距相机获取图像，特征提取匹配来实现，相同焦距相机的局限性在于两个同焦距相机仅仅是为了实现计算特征点深度，获取图像深度信息的目的。而本发明采用的多焦距相机由短焦相机和长焦相机组合而成，短焦距相机能够获取较宽的视野，而长焦距相机能够获取更多远距离信息，两个相机不仅是为了获取图像深度信息，还能得到更多想要利用的信息，如相机左右方向更宽的视野，相机远距离更多的特征信息。本发明根据多焦距相机焦距的差异，改进图像特征匹配的方式，根据短焦距相机和长焦距相机的焦距差异，确定图像金字塔下采样的缩放因子s，分别基于图像金字塔提取图像特征点，对不同长短焦图像金字塔层进行特征匹配，从而完成了长短焦图像特征匹配。
2.多焦距图像融合GPS定位数据。通过对GPS数据进行坐标转换，再将坐标转换后的GPS数据与视觉SLAM图像位姿数据进行时间戳对齐，构建传感器融合模型和位姿图优化模型。运用了视觉相机和GPS定位数据完成同时定位与地图构建，提高了SLAM的定位与建图精度。
3.SLAM轨迹与地图匹配。用上述生成的SLAM轨迹与路网地图进行轨迹匹配，降低GPS的定位误差，使得在地图上正确显示当前所处位置。
附图说明
图1是本发明提供优选实施例多焦距视觉SLAM与GPS融合的框架图。
图2是多焦距相机与GPS接收模块安装位置示意图。
图3是GPS坐标转换到SLAM世界坐标系的示意图。
图4是图优化示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
本发明解决上述技术问题的技术方案是：
步骤1.安装多焦距相机和GPS模块，多焦距视觉SLAM由短焦距相机和长焦距相机组成，短焦距和长焦距相机水平放置。
步骤2.多焦距相机之间的标定，同时采集多焦距图像和GPS数据，获取多焦距图像数据和GPS数据。
步骤3.根据多焦距相机焦距的差异，分别基于图像金字塔提取图像特征点，进行特征匹配。
步骤4.对采集到的GPS定位数据进行坐标转换到多焦距视觉SLAM世界坐标系下。
步骤5.对转换后的GPS数据和多焦距视觉SLAM数据进行时间戳对齐。
步骤6.对时间对齐后的GPS数据和SLAM图像数据构建图优化模型优化位姿。
步骤7.由SLAM生成的运行轨迹在地图上匹配到对应的地图道路轨迹。
作为本实施例一种可能的实现方式，所述步骤1安装多焦距相机和GPS模块，多焦距视觉SLAM由短焦距相机和长焦距相机组成，短焦距和长焦距相机水平放置如图2所示，包括以下步骤：
步骤11，安装多焦距相机的位置，多焦距相机由短焦距相机和长焦距相机组成，短焦距和长焦距相机水平放置在车顶前方。
步骤12，GPS定位数据获取模块安装在车顶中间。
作为本实施例一种可能的实现方式，所述步骤2包括以下步骤：
步骤21，根据短焦距相机和长焦距相机安装位置，完成多焦距相机标定。
步骤22，实现短焦距相机和长焦距相机的信息同步，在时间上，要求短焦距相机和长焦距相机同步拍摄；在空间上，短焦距相机和长焦距相机都安装在车顶前方，短焦距相机位于车顶前方左侧，短焦距相机和长焦距相机位于车顶前方右侧。
步骤23，相机的图像采集频率应保持恒定，一般为10Hz左右，采集过程中相机设置自动曝光。采集多焦距图像数据和GPS数据。
作为本实施例一种可能的实现方式，所述步骤3包括以下步骤：
步骤31，对图像构建图像金字塔，根据短焦距相机和长焦距相机的焦距差异，确定图像金字塔下采样的缩放因子s。
步骤32，根据多焦距图像差异，只选择短焦距和长焦距图像金字塔中具有相同图像信息大小的图层提取ORB特征点，如短焦距图像金字塔顶上ldis层不提取特征点，长焦距相机图像金字塔底下ldis层不提取特征点。
步骤33，需要提取特征点的对应层所需要提取的ORB特征点数量为：
Figure BDA0003357048020000091
其中，s表示图像金字塔缩放比例，ldis表示相同大小图像特征在短焦距图像金字塔和长焦距图像金字塔中的固定层次差、n表示图像金字塔缩放层数，N表示总共需要提取的ORB特征点数量，α表示所需要提取ORB特征点的图层，Nα表示图像金字塔中图层α提取的ORB特征点数量。
步骤34，使用归一化互相关算法确定在短焦距相机图像中与长焦距相机图像相同的部分区域，称为ROI区域。其中算法表示为：
Figure BDA0003357048020000092
选择ρ(x,y)最大值对应的区域为ROI短焦距相机图像的部分区域。其中S为短焦距相机图像，m、n分别表示长焦距相机图像长和宽，i、j分别表示图像像素坐标点，X、Y分别表示短焦距相机图像中和长焦距相机图像相同大小的区域块，g为长焦距相机图像，
Figure BDA0003357048020000093
为短焦距相机图像平均灰度，
Figure BDA0003357048020000094
长焦距相机图像平均灰度。
步骤35，对短焦距相机图像金字塔某层的ROI区域内使用四叉树算法增加提取ORB特征点的数量，减少ROI区域外ORB特征点提取的数量。
步骤36，对提取的特征点进行特征匹配，并筛选删除误匹配。
作为本实施例一种可能的实现方式，所述步骤4对采集到的GPS定位数据进行坐标转换到多焦距视觉SLAM世界坐标系下如图3所示，包括以下步骤：
步骤41，根据所述多焦距视觉SLAM世界坐标系是所述多焦距SLAM的第一帧，在整个多焦距SLAM过程不发生改变，求出所述多焦距SLAM第一帧到初始帧的平移向量在所述SLAM世界坐标系和所述GPS坐标系的坐标。该平移矢量在SLAM世界坐标系描述：
tw＝Rcw*(posc,init-posc,first)
tw表示世界坐标系，Rcw表示相机坐标系到世界坐标系的转换，posc,init表示初始化时相机位姿，posc,first表示相机第一帧位姿。
在GPS坐标系的描述为：
tGPS＝posGPS,init-posGPS,first
tGPS表示GPS坐标系，posGPS,init表示GPS初始化时位姿，posGPS,first表示GPS第一帧位姿。
根据两个坐标系的坐标求出所述GPS坐标系与所述多焦距双目SLAM世界坐标系之间的旋转矩阵：
RGPSw*tGPS＝tw
RGPSw表示GPS坐标系到相机世界坐标系的旋转矩阵。
作为本实施例一种可能的实现方式，所述步骤5包括以下步骤：
步骤51，不同传感器的数据输出频率是不一致的，民用车载GPS接收机输出频率为50Hz左右，相机输出频率为10Hz，要融合传感器数据，需要对不同频率的数据进行对齐。对齐相机数据和GPS数据，以时间戳为基准，在相机输出图片时获取这一时刻的GPS定位数据，如果该时刻没有GPS数据，就选取距该时刻最近的GPS数据作为此时的数据。
作为本实施例一种可能的实现方式，所述步骤6如图4所示，包括以下步骤：
步骤61，k时刻，在位置xk处，传感器进行一次观测，得到了数据zk，观测方程为：
zk＝h(xk)
由于观测总是存在误差，定义误差：
ek＝zk-h(xk)
步骤62，构建全局位姿图，需要求解的位姿作为优化图的顶点，观测值(包括相机对路标点的观测以及GPS数据)作为边，构建目标函数：
Figure BDA0003357048020000111
其中ek表示误差，包括图像重投影误差以及GPS定位数据误差项；Ωk表示信息矩阵，是协方差矩阵的逆，是一个对称矩阵。
步骤63，运用g2o图优化器求解出位姿。
作为本实施例一种可能的实现方式，所述步骤7包括以下步骤：
步骤71，建立路网有向图，路网数据一般为shapefile格式,通过读取shp文件和dbf文件,获取道路的节点ID、坐标信息及属性信息,从而建立路网的有向图G(V,E),其中V的元素为道路端点,E的元素为道路路段。
步骤72，生成网格索引，地图匹配过程中需要获取轨迹点的候选路段。由于简单地遍历整个道路网络耗费时间较长,使用网格索引,将整个地图分成M×N个网格,通过包含分析算法,预先计算每个网格中包含(相交)的路段，将整个地图划分为1000×1000个的网格。网格索引与路段数据通过路段ID相关联。
步骤73，对轨迹点设置半径为R的缓冲区,基于网格索引确定轨迹点所处的网格,并查询以该网格为中心的3×3的网格内距该轨迹点最近的K条路段作为候选路段。
步骤74，发射概率计算，发射概率即在某种隐含状态下得到某种观测值的概率，考虑距离因素，公式为：
Figure BDA0003357048020000121
其中，σ表示获取轨迹数据误差的标准差，pi表示轨迹点，
Figure BDA0003357048020000122
表示第k个点对应的候选路段，
Figure BDA0003357048020000123
表示从轨迹点到候选路段的距离。
步骤75，传递概率基于道路一般是两点连线的直线最近距离，前后路段间的路网距离越小，传递概率越大：
Figure BDA0003357048020000124
其中，
Figure BDA0003357048020000125
和
Figure BDA0003357048020000126
分别为第t+1个轨迹点和第t个轨迹点对应的候选路段，a是设定的系数，D代表候选路段间的实际路网距离。
步骤76，使用维特比算法求出最优路径。
上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (10)

1.一种多焦距视觉SLAM与GPS融合的方法，其特征在于，包括以下步骤：
步骤1.安装多焦距相机和GPS模块，多焦距视觉SLAM由短焦距相机和长焦距相机组成，短焦距和长焦距相机水平放置；步骤2.多焦距相机之间进行标定，同时采集多焦距图像和GPS数据，获取多焦距图像数据和GPS数据；步骤3.根据多焦距相机焦距的差异，分别基于图像金字塔提取图像特征点，进行特征匹配；步骤4.对采集到的GPS定位数据进行坐标转换到多焦距视觉SLAM世界坐标系下；步骤5.对转换后的GPS数据和多焦距视觉SLAM数据进行时间戳对齐；步骤6.对时间戳对齐后的GPS数据和SLAM图像数据构建图优化模型优化位姿；步骤7.由SLAM生成的运行轨迹在地图上匹配到对应的地图道路轨迹。
2.根据权利要求1所述的一种多焦距视觉SLAM与GPS融合的方法，其特征在于，所述步骤2多焦距相机之间进行标定，同时采集多焦距图像和GPS数据，获取多焦距图像数据和GPS数据，具体包括：
步骤21，根据短焦距相机和长焦距相机安装位置，完成多焦距相机标定；
步骤22，实现短焦距相机和长焦距相机的信息同步，在时间上，要求短焦距相机和长焦距相机同步拍摄；在空间上，短焦距相机和长焦距相机都安装在车顶前方，短焦距相机位于车顶前方左侧，长焦距相机位于车顶前方右侧；
步骤23，相机的图像采集频率保持恒定，采集过程中相机设置自动曝光；采集多焦距图像数据和GPS数据。
3.根据权利要求1所述的一种多焦距视觉SLAM与GPS融合的方法，其特征在于，所述步骤3根据多焦距相机焦距的差异，分别基于图像金字塔提取图像特征点，进行特征匹配具体包括：
步骤31，对图像构建图像金字塔，根据短焦距相机和长焦距相机的焦距差异，确定图像金字塔下采样的缩放因子s；
步骤32，根据多焦距图像差异，只选择短焦距和长焦距图像金字塔中具有相同图像信息大小的图层提取ORB特征点，如短焦距图像金字塔顶上ldis层不提取特征点，长焦距相机图像金字塔底下ldis层不提取特征点；
步骤33，获取需要提取特征点的对应层所需要提取的ORB特征点数量；
步骤34，使用归一化互相关算法确定在短焦距相机图像中与长焦距相机图像相同的部分区域，称为ROI区域；
步骤35，对短焦距相机图像金字塔某层的ROI区域内使用四叉树算法增加提取ORB特征点的数量，减少ROI区域外ORB特征点提取的数量；
步骤36，对提取的特征点进行特征匹配，并筛选删除误匹配。
4.根据权利要求3所述的一种多焦距视觉SLAM与GPS融合的方法，其特征在于，所述步骤33中，获取需要提取特征点的对应层所需要提取的ORB特征点数量为：
Figure FDA0003357048010000021
其中，s表示图像金字塔缩放比例，ldis表示相同大小图像特征在短焦距图像金字塔和长焦距图像金字塔中的固定层次差、n表示图像金字塔缩放层数，N表示总共需要提取的ORB特征点数量，α表示所需要提取ORB特征点的图层，Nα表示图像金字塔中图层α提取的ORB特征点数量。
5.根据权利要求4所述的一种多焦距视觉SLAM与GPS融合的方法，其特征在于，所述步骤34的归一化互相关算法具体表示为：
Figure FDA0003357048010000022
选择ρ(x,y)最大值对应的区域为ROI短焦距相机图像的部分区域。其中S为短焦距相机图像，m、n分别表示长焦距相机图像长和宽，i、j分别表示图像像素坐标点，X、Y分别表示短焦距相机图像中和长焦距相机图像相同大小的区域块，g为长焦距相机图像，S为短焦距相机图像平均灰度，g长焦距相机图像平均灰度。
6.根据权利要求5所述的一种多焦距视觉SLAM与GPS融合的方法，其特征在于，所述步骤4对采集到的GPS定位数据进行坐标转换到多焦距视觉SLAM世界坐标系下，具体包括：
步骤41，根据多焦距视觉SLAM世界坐标系以SLAM第一帧建立的，在整个多焦距视觉SLAM过程不发生改变，求出所述多焦距视觉SLAM第一帧到初始帧的平移向量在所述SLAM世界坐标系和所述GPS坐标系的坐标；该平移矢量在SLAM世界坐标系描述：
tw＝Rcw*(posc,init-posc,first)
tw表示世界坐标系，Rcw表示相机坐标系到世界坐标系的转换，posc,init表示初始化时相机位姿，posc,first表示相机第一帧位姿；
在GPS坐标系的描述为：
tGPS＝posGPS,init-posGPS,first
tGPS表示GPS坐标系，posGPS,init表示GPS初始化时位姿，posGPS,first表示GPS第一帧位姿；
根据两个坐标系的坐标求出所述GPS坐标系与所述多焦距双目SLAM世界坐标系之间的旋转矩阵：
RGPSw*tGPS＝tw
RGPSw表示GPS坐标系到相机世界坐标系的旋转矩阵。
7.根据权利要求6所述的一种多焦距视觉SLAM与GPS融合的方法，其特征在于，所述步骤6对时间戳对齐后的GPS数据和SLAM图像数据构建图优化模型优化位姿，具体包括：
步骤61，k时刻，在位置xk处，传感器进行一次观测，得到数据zk，观测方程为：
zk＝h(xk)
由于观测总是存在误差，定义误差：
ek＝zk-h(xk)
步骤62，构建全局位姿图，需要求解的位姿作为优化图的顶点，观测值作为边，构建目标函数，观测值包括相机对路标点的观测以及GPS数据：
Figure FDA0003357048010000041
其中ek表示误差，包括图像重投影误差以及GPS定位数据误差项；Ωk表示信息矩阵，是协方差矩阵的逆，是一个对称矩阵；
步骤63，运用图优化器部署出上述全局位姿图的顶点和边，求解出最优的位姿。
8.根据权利要求7所述的一种多焦距视觉SLAM与GPS融合的方法，其特征在于，所述步骤7由SLAM生成的运行轨迹在地图上匹配到对应的地图道路轨迹中，具体包括：
步骤71，建立路网有向图，获取道路的节点ID、坐标信息及属性信息,从而建立路网的有向图G(V,E),其中V的元素为道路端点,E的元素为道路路段；
步骤72，生成网格索引，使用网格索引，将整个地图分成M×N个网格，通过包含分析算法，预先计算每个网格中相交的路段，获取轨迹点的候选路段，网格索引与路段数据通过路段ID相关联；
步骤73，对轨迹点设置半径为R的缓冲区,基于网格索引确定轨迹点所处的网格，并查询以该网格为中心的3×3的网格内距该轨迹点最近的K条路段作为候选路段；
步骤74，计算发射概率，发射概率即在某种隐含状态下得到某种观测值的概率，考虑距离因素；
步骤75，计算传递概率；
步骤76，使用维特比算法(每一段选出最优路径删除其他路径，一直迭代到终点)求出最优路径。
9.根据权利要求8所述的一种多焦距视觉SLAM与GPS融合的方法，其特征在于，所述计算发射概率，发射概率即在某种隐含状态下得到某种观测值的概率，考虑距离因素，具体公式为：
Figure FDA0003357048010000051
其中，σ表示获取轨迹数据误差的标准差，pi表示轨迹点，
Figure FDA0003357048010000052
表示第k个点对应的候选路段，
Figure FDA0003357048010000053
表示从轨迹点到候选路段的距离；
步骤81，传递概率基于道路一般是两点连线的直线最近距离，前后路段间的路网距离越小，传递概率越大：
Figure FDA0003357048010000054
其中，
Figure FDA0003357048010000055
和
Figure FDA0003357048010000056
分别为第t+1个轨迹点和第t个轨迹点对应的候选路段，a是设定的系数，D代表候选路段间的实际路网距离。
10.一种存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现如权利要求1至9任一项所述的多焦距视觉SLAM与GPS融合的方法。
CN202111355852.9A 2021-11-16 2021-11-16 一种多焦距视觉slam与gps融合的方法及存储介质 Pending CN114063127A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111355852.9A CN114063127A (zh) 2021-11-16 2021-11-16 一种多焦距视觉slam与gps融合的方法及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111355852.9A CN114063127A (zh) 2021-11-16 2021-11-16 一种多焦距视觉slam与gps融合的方法及存储介质

Publications (1)

Publication Number Publication Date
CN114063127A true CN114063127A (zh) 2022-02-18

Family

ID=80273432

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111355852.9A Pending CN114063127A (zh) 2021-11-16 2021-11-16 一种多焦距视觉slam与gps融合的方法及存储介质

Country Status (1)

Country Link
CN (1) CN114063127A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114979485A (zh) * 2022-05-25 2022-08-30 江苏集萃智能光电系统研究所有限公司 一种分布式多目相机触发数不一致纠偏方法
CN114995651A (zh) * 2022-06-15 2022-09-02 Oppo广东移动通信有限公司 时空对齐方法、装置、电子设备及存储介质

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114979485A (zh) * 2022-05-25 2022-08-30 江苏集萃智能光电系统研究所有限公司 一种分布式多目相机触发数不一致纠偏方法
CN114979485B (zh) * 2022-05-25 2024-05-07 江苏集萃智能光电系统研究所有限公司 一种分布式多目相机触发数不一致纠偏方法
CN114995651A (zh) * 2022-06-15 2022-09-02 Oppo广东移动通信有限公司 时空对齐方法、装置、电子设备及存储介质

Similar Documents

Publication Publication Date Title
CN108801274B (zh) 一种融合双目视觉和差分卫星定位的地标地图生成方法
CN111275750B (zh) 基于多传感器融合的室内空间全景图像生成方法
WO2020006667A1 (en) Vehicle navigation system using pose estimation based on point cloud
CN106461402A (zh) 用于确定相对于数字地图的位置的方法及系统
CN110617821B (zh) 定位方法、装置及存储介质
CN107850445A (zh) 用于生成及使用定位参考数据的方法及系统
CN110859044A (zh) 自然场景中的集成传感器校准
KR100529401B1 (ko) 합성 개구 레이더 영상을 이용한 수치표고자료 제작 장치및 그 방법
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN114063127A (zh) 一种多焦距视觉slam与gps融合的方法及存储介质
CN112465732A (zh) 一种车载激光点云与序列全景影像的配准方法
KR101442703B1 (ko) Gps 단말기 및 gps 위치 보정 방법
CN110986888A (zh) 一种航空摄影一体化方法
CN104729482A (zh) 一种基于飞艇的地面微小目标侦测系统及方法
CN103777196B (zh) 基于地理信息的地面目标距离单站测量方法及其测量系统
CN110927765B (zh) 激光雷达与卫星导航融合的目标在线定位方法
CN115166721B (zh) 路侧感知设备中雷达与gnss信息标定融合方法及装置
CN113009533A (zh) 基于视觉slam的车辆定位方法、设备及云服务器
CN113838129A (zh) 一种获得位姿信息的方法、装置以及系统
De Agostino et al. GIMPHI: a new integration approach for early impact assessment
Alsubaie et al. The feasibility of 3D point cloud generation from smartphones
CN113733091B (zh) 一种移动机器人室外高精度自主导航系统
Lee et al. Semantic 3D Map Change Detection and Update based on Smartphone Visual Positioning System
CN117848302B (zh) 一种实时地形智能测绘方法及系统
CN116385686B (zh) 基于不规则倾斜摄影的实景三维模型重建方法及系统

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