CN116594029A - 一种基于2d-3d匹配网络的地理定位方法 - Google Patents

一种基于2d-3d匹配网络的地理定位方法 Download PDF

Info

Publication number
CN116594029A
CN116594029A CN202310562749.4A CN202310562749A CN116594029A CN 116594029 A CN116594029 A CN 116594029A CN 202310562749 A CN202310562749 A CN 202310562749A CN 116594029 A CN116594029 A CN 116594029A
Authority
CN
China
Prior art keywords
point
frame
laser radar
map
matching
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
CN202310562749.4A
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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202310562749.4A priority Critical patent/CN116594029A/zh
Publication of CN116594029A publication Critical patent/CN116594029A/zh
Pending legal-status Critical Current

Links

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
    • 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
    • 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
    • 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
    • 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/3852—Data derived from aerial or satellite images
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • 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
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/045—Combinations of networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/82—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using neural networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/10—Terrestrial scenes
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/60—Type of objects
    • G06V20/64—Three-dimensional objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]
    • 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)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Evolutionary Computation (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Computing Systems (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Electromagnetism (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Biomedical Technology (AREA)
  • Medical Informatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Databases & Information Systems (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Data Mining & Analysis (AREA)
  • Molecular Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Processing Or Creating Images (AREA)

Abstract

本发明涉及SLAM与深度学习领域，提出了一种基于2D‑3D匹配网络的地理定位方法，包括以下步骤：S1、对输入的激光雷达数据使用激光雷达里程计进行跟踪；S2、根据当前位置数据截取局部卫星地图；S3、将矫正后的激光雷达点云和所述局部卫星地图作为网络输入，通过2D‑3D匹配网络得到激光雷达点和卫星图片上的二维像素点的匹配关系；S4、根据匹配关系计算激光雷达帧相对地图的位姿变换关系；S5、结合激光雷达里程计的跟踪结果与基于卫星地图的配准结果进行位姿图优化，得到优化后的位姿变换结果；S6、根据所述优化后的位姿变换结果对每帧激光雷达点云进行拼接得到点云地图。本发明在GPS初始化以后，仅需要点云、卫星图即可实现长时间实时地理定位，且能保持较高的定位精度。

Description

一种基于2D-3D匹配网络的地理定位方法
技术领域
本发明涉及SLAM与深度学习领域，具体是涉及一种基于2D-3D匹配网络的地理定位方法。
背景技术
长时间准确进行定位是自动驾驶的关键技术。人们对精确定位的追求并未随着GPS的出现而减弱，反而对长时间稳定、准确地定位有了更多的要求。虽然基于各种传感器设计的里程计可以提供相对准确的位姿估计，但不可避免地会受到累计漂移的影响。因此，车辆定位技术通常依赖于全球导航卫星系统来估计绝对位姿。然而，在高楼林立的城市环境中，商业GPS信号通常因受到遮挡而无法使用。这种情况下，基于先验地图的定位已经成为解决定位问题的可行方案，它可以弥补GPS信号的局限性，并提供更高精度的6自由度姿态估计。大多数基于先验地图的方法需要预先对环境进行建图，然后进行定位。有不少研究建议使用开源地图用于机器人定位，例如地理信息系统(GIS)和卫星图像，这些现有的先验地图不仅覆盖范围更广，同时也易于随时获取，但是现有的地理定位方法存在在GPS受到遮挡等情况信号不良时不能准确定位的问题。
特征关联方法一般可分为图像特征匹配、3D点云配准以及图像与点云之间的关联。传统的图像间的特征匹配通常可分为角点提取、特征计算和特征匹配等环节。一般来说，需要预先使用检测器预先提取图像中对光照、尺度不敏感的特征，然后对不同图像提取出来的特征点进行关联以形成匹配。近年来，随着研究的不断推进，无检测器的特征匹配方法引起了人们的广泛兴趣，也陆续有新的方法被提出，仅输入图片即可输出图片中的特征供后续匹配使用。经典的点云之间的匹配方法有ICP(Iterative Closest Point)，通过迭代的方法计算点云拼合的最优位姿来进行点云间的配准。而随着基于深度学习的点云配准方法的提出和发展，也让两帧点云即便具有较小的重叠区域也能正确配准成为了可能。
发明内容
针对上述问题，本发明基于同步建图与定位技术(Simultaneous Localizationand Mapping,SLAM)方法与深度学习方法，设计了一个基于激光点云与图像的2D-3D匹配网络，并构建了一个基于车载激光雷达点云与卫星地图的地理定位系统，解决了现有GPS方案容易在受到遮挡等信号不良情况下不能准确定位的问题，在该系统初始化完成后不依赖于GPS信号也能长时间保持较高的定位精度。
为了实现上述目的，本发明提供一种基于2D-3D匹配网络的地理定位方法，包括以下步骤：
S1、对输入的激光雷达数据使用激光雷达里程计进行跟踪；
S2、根据当前位置数据截取局部卫星地图；
S3、将矫正后的激光雷达点云和所述局部卫星地图作为网络输入，通过2D-3D匹配网络得到激光雷达点和卫星图片上的二维像素点的匹配关系；
S4、根据所述激光雷达点和卫星图片上的二维像素点的匹配关系计算激光雷达帧相对地图的位姿变换关系；
S5、结合激光雷达里程计的跟踪结果与基于卫星地图的配准结果进行位姿图优化，得到优化后的位姿变换结果；
S6、根据所述优化后的位姿变换结果对每帧激光雷达点云进行拼接得到点云地图。
优选的，所述步骤S1具体包括以下步骤：
S11、对输入对激光雷达点进行预处理，根据曲率提取边角点和平面点；
S12、前端根据前后帧提取的点构建点线约束和点面约束求解帧间位姿变换；
S13、后端根据前端结果寻找当前帧地图中的栅格索引，然后构造用来优化当前帧的局部地图，构造点线约束和点面约束进行优化更新里程计与地图之间的位姿变换。
优选的，所述根据曲率提取边角点和平面点时其中的每一个点的曲率计算公式为：
其中S为计算曲率时，在与i相同扫描线上，取i点周围的点的集合，为第k帧下i点的坐标，/>为第k帧下j点的坐标。
优选的，所述步骤S12具体为：
由成功匹配的边角点对计算残差，其公式为：
其中点i为第k+1帧中的边角点，点j和点l为第k帧中与点i匹配的两个边角点，为第k+1帧中点i在第k帧激光雷达坐标系下的坐标，/>和/>为点j和点l在第k帧激光雷达坐标系下的坐标，几何意义为点i到点j和点l构成的直线的距离；
由成功匹配的平面点对计算残差，其公式为：
其中点i为第k+1帧中的平面点，点j、点l和点m为第k帧中与点i匹配的三个平面点，为第k+1帧中点i在第k帧激光雷达坐标系下的坐标，
和/>为点j、点l和点m在第k帧激光雷达坐标系下的坐标，几何意义为点i到由点j、点l和点m构成的平面的距离。
优选的，所述步骤S2具体包括以下步骤：
S21、判断是否完成系统初始化，若未完成系统初始化，则取当前GPS位置为初始位置，并拉取一定大小的卫星地图；
S22、初始化完成且系统正常运行时，使用优化后的位姿在卫星地图上截取局部卫星图片作为2D-3D匹配网络的部分输入。
优选的，所述步骤S3具体包括以下步骤：
S31、使用特征金字塔网络(FPN)提取2D图像的粗和细两个层次的特征，其中提取的2D图像的粗特征图为原图大小的1/16，提取的2D图像的细特征图为原图大小的1/8；
S32、对点云进行四次下采样，采用KPConv主干来提取点云的多尺度局部特征得到粗特征和细特征，并使用Local Pool进行Z轴方向的特征聚合；
S33、经过网络提取的粗特征后，使用粗级Transformer模块来融合2D点和3D点的位置以及它们自身所包含的其他上下文线索来增强特征独特性；
S34、使用经过增强的粗特征用点匹配模块进行初始匹配选择；
S35、使用细阶段匹配模块细化粗匹配的结果。
优选的，所述步骤S4具体包括以下步骤：
S41、随机选择三对激光雷达和卫星图片的匹配点通过重投影误差计算激光雷达帧到卫星地图的位姿变换；
S42、根据计算的位姿变换验证所有匹配对，计算模型内点率；
S43、若模型内点率不足，则重复S41、S42的步骤，否则利用所得模型的内点构造最小二乘问题并求解，最终的激光雷达帧到卫星地图的位姿变换。
优选的，所述步骤S5具体包括以下步骤：
S51、利用里程计跟踪结果，构建位姿图优化问题，通过激光雷达帧的相对位姿与基于全局位姿计算的相对位姿来计算位姿图中顶点之间的边的误差；
S52、采用先验边的形式，融合激光雷达帧基于卫星地图的定位结果，对位姿图进行优化。
优选的，所述步骤S6具体为利用所述优化后的位姿变换结果，将激光雷达点云帧变换到地图点云坐标系，拼接得到更新后的点云地图。
与现有技术相比，本发明的有益效果是：
本发明提供的一种基于2D-3D匹配网络的地理定位方法，在使用GPS初始化完成以后，仅需要点云、卫星图即可实现长时间实时地理定位，不依赖于外部GPS信号仍能保持较高的定位准确度。
附图说明
图1为本发明的方法流程图；
图2为本发明局部卫星图片选取示意图；
图3为本发明所构建的2D-3D匹配网络示意图；
图4为本发明Local pool操作示意图；
图5为本发明匹配网络输出的匹配效果图；
图6为本发明激光雷达帧和局部卫星图片配准效果图；
图7为本发明位姿图结构示意图；
图8为本发明输出点云地图效果图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
2D-3D匹配通常可以理解为图像中的二维特征与点云中的三维空间点进行关联，是一种跨模态的匹配。通常有两种实现该匹配的方式，一种是将点云通过相机内外参变化为图像坐标，然后通过二维特征匹配的方式进行匹配；另一种是通过三角测量或深度估计方法恢复图像像素的空间坐标，然后使用点云配准的方法进行匹配关联。2D-3D匹配网络即通过深度学习的方式，来实现图像与点云的特征关联。而本方法即提出了一种端到端的方法实现卫星图像和激光雷达点云之间的特征关联，并将其用于在GPS等全球卫星定位系统信号受到阻挡的情况下，仍能让系统保持准确的定位，为解决GPS不可用的情况下实现准确定位提供了新的思路与解决方案。
针对现有技术中存在的问题与不足本发明提出一种基于2D-3D匹配网络的地理定位方法，可以结合激光SLAM算法，实现在卫星地图中的实时定位。
本发明提出的一种基于2D-3D匹配网络的地理定位方法，如图1所示，包括以下步骤：
S1、对输入的激光雷达数据使用激光雷达里程计进行跟踪；
S2、根据当前位置数据截取局部卫星地图；
S3、将矫正后的激光雷达点云和所述局部卫星地图作为网络输入，通过2D-3D匹配网络得到激光雷达点和卫星图片上的二维像素点的匹配关系；
S4、根据所述激光雷达点和卫星图片上的二维像素点的匹配关系计算激光雷达帧相对地图的位姿变换关系；
S5、结合激光雷达里程计的跟踪结果与基于卫星地图的配准结果进行位姿图优化，得到优化后的位姿变换结果(定位结果)；
S6、根据所述优化后的位姿变换结果对每帧激光雷达点云进行拼接得到激光雷达点云地图。
下面对每一步骤进行详细的说明介绍。
步骤S1、对输入的激光雷达数据使用激光雷达里程计进行跟踪。
对于输入的激光雷达帧，本发明通过里程计技术对每一帧激光雷达进行跟踪得到局部的相对定位结果，再后续利用全局匹配定位的结果对其进行矫正。
具体的，主要包括以下步骤：
S11、对输入对激光雷达点进行预处理，根据曲率提取边角点和平面点，对点云提取边角点和平面点时，以点的曲率作为依据，其中每一个点的曲率计算公式为：
其中S为计算曲率时，在与i相同扫描线上，取i点周围的点的集合，为第k帧下i点的坐标，/>为第k帧下j点的坐标。
S12、前端根据前后帧提取的点构建点线约束和点面约束求解帧间位姿变换，由成功匹配的边角点对计算残差的公式为：
其中点i为第k+1帧中的边角点，点j和点l为第k帧中与点i匹配的两个边角点。为第k+1帧中点i在第k帧激光雷达坐标系下的坐标，/>和/>为点j和点l在第k帧激光雷达坐标系下的坐标，几何意义为点i到点j和点l构成的直线的距离。
由成功匹配的平面点对计算残差的公式为：
其中点i为第k+1帧中的平面点，点j、点l和点m为第k帧中与点i匹配的三个平面点。为第k+1帧中点i在第k帧激光雷达坐标系下的坐标，/>和/>为点j、点l和点m在第k帧激光雷达坐标系下的坐标。残差的几何意义为，点i到由点j、点l和点m构成的平面的距离。
S13、后端根据前端结果寻找当前帧地图中的栅格索引，然后构造用来优化当前帧的局部地图，构造点线约束和点面约束进行优化更新里程计与地图之间的位姿变换。
步骤S2、根据当前位置数据截取局部卫星地图。
本发明使用KITTI数据集作为数据支撑。KITTI数据集是由德国卡尔斯鲁厄理工学院和丰田工业大学芝加哥分校联合赞助的用于自动驾驶领域研究的数据集。作者收集了长达6个小时的真实交通环境，数据集由经过校正和同步的图像、激光雷达扫描、高精度的GPS信息和IMU加速信息等多种模态的信息组成。初始GPS坐标可以从KITTI数据集中直接获得。本发明根据数据集的GPS信息事先从谷歌网站上加载了数据集所在城市的全局卫星地图，后续则在全局地图中根据位置截取局部地图(如图2所示)。
具体的，主要包括以下步骤：
S21、判断是否完成系统初始化，若未完成系统初始化，则从数据集中获取当前GPS位置为初始位置，并截取一定大小的卫星地图；
S22、初始化完成且系统正常运行时，使用优化后的位姿在卫星地图上截取局部卫星图片作为2D-3D匹配网络的部分输入。
步骤S3、将矫正后的激光雷达点云和所述局部卫星地图作为网络输入，通过2D-3D匹配网络得到激光雷达点和卫星图片上的二维像素点的匹配关系。
本发明提出了一个两阶段的无角点检测器的2D-3D匹配网络(如图3所示)，网络可生成图像I和激光雷达点云P之间密集的对应关系。
具体的，主要包括以下步骤：
S31、使用特征金字塔网络(FPN)提取2D图像I的粗和细两个层次的特征,其中提取的2D图像的粗特征图为原图大小的1/16，提取的2D图像的细特征图/>为原图大小的1/8；
S32、对从里程计获取的去畸变的输入点云P进行四次下采样，从而得到四个具有不同分辨率的点云Pk(k＝1，2，3，4)，采用KPConv主干来提取点云的多尺度局部特征得到粗特征和细特征/>并使用Local Pool进行Z轴方向的特征聚合，如图所示4，在特征提取时对每一层点云输出，本发明将点云特征向固定分辨率的2D网格进行采样，之后再用maxpool对网格特征进行池化然后反向采样回3D；
S33、经过网络提取的粗特征和/>后，使用粗级Transformer模块来融合2D点和3D点的位置以及它们自身所包含的其他上下文线索来增强特征独特性。本发明在已经下采样的特征图上进行匹配，而不是原始图像和点云，因为配准可以通过匹配子集之间更粗的对应关系来固定。原始分辨率的特征往往过于密集，使用这些特征进行匹配是低效和费时的。在低分辨率的特征上进行匹配，会使输入到Transformer模块的数据明显减少，从而大幅提高了匹配效率也保持了精度。
注意层通常接收查询Q，键K，和值V作为输入，完成类似信息检索的工作。注意力通过计算查询Q和键K之间的相似度来确定权重，然后通过softmax层将相似度值V转换为一组权重。最后，根据这组权重和相应的值的乘积，通过加权求和得到注意力值。
Attention(Q,K,V)＝softmax(QKT)V
注意力可以广泛地解释为重要性权重的向量。为了预测一个元素，注意力向量被用来估计它与其他元素的关系，而这些值的加权和被用来作为目标的近似值。当输入查询Q和值V都是图像或点云特征(或/>)时，这种注意机制被称为自我注意。当输入的查询Q和值V分别是图像和点云特征(/>和/>)时，它被称为交叉注意。
为了将位置信息集成到地物中，最简单有效的方法是将位置嵌入向量直接添加或串联到地物中。为了一致地嵌入二维像素位置和三维点云，只嵌入点云的x和y位置，并通过zoomLevel恢复卫星地图的真实比例q(m/pix)。pixelCoordinate＝worldCoordinate×2zoomLevel
这么做的主要原因是，卫星图像是通过对地面的空中卫星测量获得的，忽略了地表物体的高度信息。LiDAR点云中具有相同水平坐标(x，y)的点应该对应于卫星图像上的同一个点，并保持沿z轴的特征不变性。
S34、使用经过增强的粗特征用点匹配模块进行初始匹配选择，在进行粗略级别的匹配时，一个2D特征可能会匹配到多个3D点特征。因此，本发明在提取匹配对时仅对点云在图像上的匹配分数进行softmax操作以得到每个激光雷达点在图像中每个图像块点匹配概率。
首先，本发明对Transformer模块输出的和/>特征进行归一化处理，并计算出分数矩阵/>
由于并非所有的三维点都能在二维图像上找到，本发明通过增加一个新的行，并填充可学习变量a将分数S扩展为然后，通过temperature为t的softmax得到匹配的置信矩阵P,
最后，选择P每列上的最大值，并且只有当匹配的最大置信度高于预设的阈值thc时才选择匹配。粗略级别的匹配预测表示为：
Mc＝{(ic,jc)IP(ic,jc)＝max(P(.,jc)),P(ic,jc)>thc)
S35、使用细阶段匹配模块细化粗匹配的结果。匹配直接在2D图像和3D点云的低分辨率特征上进行匹配，所得到的匹配对的精度还不足以用于一些类似定位的下游任务，为此本发明使用细阶段匹配模块对其进行进一步处理细化粗匹配的结果，对于每个粗略的匹配(ic，jc)，从图像和点云中抽取精细特征和/>准确地说，在二维图像中首先计算出ic在特征图/>上的相应位置(uc，vc)，然后在该位置裁剪一个大小为w×w的窗口。对于三维点云，根据jc在欧氏距离上的位置，从P3中抽取一个局部点云/>然后从/>中获取局部点云特征。
与粗级Transformer类似，本发明使用Transformer获得特征(和/>)来计算图像上的精确点位置。值得注意的是，这部分只使用主干学习的局部点特征，没有任何位置嵌入。事实上，在通过粗略的匹配解决了全局的模糊性之后，点级匹配主要取决于点之间的接近程度。
最后，从和图像块中的所有点中选择中心点jf的特征，计算匹配概率，得到热图。通过计算概率分布的期望值，得到像素位置if作为匹配位置。在对粗略匹配得到的每个点进行处理后，收集匹配结果，形成最终的全局密集点对应关系Mf＝(if，jf)。最终得到的匹配效果如图5所示。
步骤S4、根据所述激光雷达点和卫星图片上的二维像素点的匹配关系计算激光雷达帧相对地图的位姿变换关系。
具体的，主要包括以下步骤：
S41、随机选择三对激光雷达和卫星图片的匹配点通过重投影误差计算激光雷达帧到卫星地图的位姿变换。具体的误差公式如下：
其中，R、t为激光雷达点云中心到图像中心的位置变换关系，p和uv为三维激光雷达点与像素点坐标的匹配点，π(·)为投影变换关系，对于输入的点[x,y,z]，其变换公式为：
n([x,y,z])＝[x/scale+H/2,y/scale+W/2]
其中，scale为卫星图片的真实尺度信息，H和W分别为卫星图片的高和宽，这个变换关系将三维点映射到二维到卫星图片中，并且值得注意到是由于卫星图片可视作是正摄的，所以直接忽略了z轴。
S42、根据计算的位姿变换验证所有匹配对，计算模型内点率；
S43、如果模型内点率不足，那么重复S41、S42的步骤，否则利用所得模型的内点构造最小二乘问题并求解，最终的激光雷达帧到卫星地图的位姿变换(配准得到的结果如图6所示)。
步骤S5、结合激光雷达里程计的跟踪结果与基于卫星地图的配准结果进行位姿图优化，得到优化后的位姿变换结果(定位结果)。
具体的，主要包括以下步骤：
S51、利用里程计跟踪结果，构建位姿图优化问题，通过激光雷达帧的相对位姿与基于全局位姿计算的相对位姿来计算位姿图中顶点之间的边的误差；
S52、采用先验边的形式，融合激光雷达帧基于卫星地图的定位结果，对位姿图进行优化。
本发明使用位姿图优化的方法融合里程计的结果和基于卫星地图进行重定位的结果，并可以与里程计无缝耦合。里程计输出的位姿记为2D-3D匹配网络输出的配准结果记为/>本发明使用里程计输出/>和配准结果/>构建位姿图优化问题(结构如图7所示)，以获得更精确的位姿/>其中，里程计误差边被定义为：
为了融合配准得到的全局定位，本发明设计了特殊的误差边，其定义如下：
其中，和/>为/>的平移和旋转分量，/>和/>为/>的平移和旋转分量，R|z表示区旋转矩阵R的z轴旋转分量，即取yaw角。
步骤S6、根据所述优化后的位姿变换结果对每帧激光雷达点云进行拼接得到点云地图。
本发明使用优化后的位姿，将点云变换到地图坐标系中。这样，新的点云和原地图中的点云组成一张新的雷达点云地图，实现了地图的更新，整体点云输出效果如图8所示。
虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。

Claims (9)

1.一种基于2D-3D匹配网络的地理定位方法，其特征在于，包括以下步骤：
S1、对输入的激光雷达数据使用激光雷达里程计进行跟踪；
S2、根据当前位置数据截取局部卫星地图；
S3、将矫正后的激光雷达点云和所述局部卫星地图作为网络输入，通过2D-3D匹配网络得到激光雷达点和卫星图片上的二维像素点的匹配关系；
S4、根据所述激光雷达点和卫星图片上的二维像素点的匹配关系计算激光雷达帧相对地图的位姿变换关系；
S5、结合激光雷达里程计的跟踪结果与基于卫星地图的配准结果进行位姿图优化，得到优化后的位姿变换结果；
S6、根据所述优化后的位姿变换结果对每帧激光雷达点云进行拼接得到点云地图。
2.根据权利要求1所述的一种基于2D-3D匹配网络的地理定位方法，其特征在于，所述步骤S1具体包括以下步骤：
S11、对输入对激光雷达点进行预处理，根据曲率提取边角点和平面点；
S12、前端根据前后帧提取的点构建点线约束和点面约束求解帧间位姿变换；
S13、后端根据前端结果寻找当前帧地图中的栅格索引，然后构造用来优化当前帧的局部地图，构造点线约束和点面约束进行优化更新里程计与地图之间的位姿变换。
3.根据权利要求2所述的一种基于2D-3D匹配网络的地理定位方法，其特征在于，所述根据曲率提取边角点和平面点时其中的每一个点的曲率计算公式为：
其中S为计算曲率时，在与i相同扫描线上，取i点周围的点的集合，为第k帧下i点的坐标，/>为第k帧下j点的坐标。
4.根据权利要求2所述的一种基于2D-3D匹配网络的地理定位方法，其特征在于，所述步骤S12具体为：
由成功匹配的边角点对计算残差，其公式为：
其中点i为第k+1帧中的边角点，点j和点l为第k帧中与点i匹配的两个边角点，为第k+1帧中点i在第k帧激光雷达坐标系下的坐标，/>和/>为点j和点l在第k帧激光雷达坐标系下的坐标，几何意义为点i到点j和点l构成的直线的距离；
由成功匹配的平面点对计算残差，其公式为：
其中点i为第k+1帧中的平面点，点j、点l和点m为第k帧中与点i匹配的三个平面点，为第k+1帧中点i在第k帧激光雷达坐标系下的坐标，/>和/>为点j、点l和点m在第k帧激光雷达坐标系下的坐标，几何意义为点i到由点j、点l和点m构成的平面的距离。
5.根据权利要求1所述的一种基于2D-3D匹配网络的地理定位方法，其特征在于，所述步骤S2具体包括以下步骤：
S21、判断是否完成系统初始化，若未完成系统初始化，则取当前GPS位置为初始位置，并拉取一定大小的卫星地图；
S22、初始化完成且系统正常运行时，使用优化后的位姿在卫星地图上截取局部卫星图片作为2D-3D匹配网络的部分输入。
6.根据权利要求1所述的一种基于2D-3D匹配网络的地理定位方法，其特征在于，所述步骤S3具体包括以下步骤：
S31、使用特征金字塔网络(FPN)提取2D图像的粗和细两个层次的特征，其中提取的2D图像的粗特征图为原图大小的1/16，提取的2D图像的细特征图为原图大小的1/8；
S32、对点云进行四次下采样，采用KPConv主干来提取点云的多尺度局部特征得到粗特征和细特征，并使用Local Pool进行Z轴方向的特征聚合；
S33、经过网络提取的粗特征后，使用粗级Transformer模块来融合2D点和3D点的位置以及它们自身所包含的其他上下文线索来增强特征独特性；
S34、使用经过增强的粗特征用点匹配模块进行初始匹配选择；
S35、使用细阶段匹配模块细化粗匹配的结果。
7.根据权利要求1所述的一种基于2D-3D匹配网络的地理定位方法，其特征在于，所述步骤S4具体包括以下步骤：
S41、随机选择三对激光雷达和卫星图片的匹配点通过重投影误差计算激光雷达帧到卫星地图的位姿变换；
S42、根据计算的位姿变换验证所有匹配对，计算模型内点率；
S43、若模型内点率不足，则重复S41、S42的步骤，否则利用所得模型的内点构造最小二乘问题并求解，最终的激光雷达帧到卫星地图的位姿变换。
8.根据权利要求1所述的一种基于2D-3D匹配网络的地理定位方法，其特征在于，所述步骤S5具体包括以下步骤：
S51、利用里程计跟踪结果，构建位姿图优化问题，通过激光雷达帧的相对位姿与基于全局位姿计算的相对位姿来计算位姿图中顶点之间的边的误差；
S52、采用先验边的形式，融合激光雷达帧基于卫星地图的定位结果，对位姿图进行优化。
9.根据权利要求1所述的一种基于2D-3D匹配网络的地理定位方法，其特征在于，所述步骤S6具体为利用所述优化后的位姿变换结果，将点云帧变换到地图点云坐标系，拼接得到更新后的激光雷达点云地图。
CN202310562749.4A 2023-05-18 2023-05-18 一种基于2d-3d匹配网络的地理定位方法 Pending CN116594029A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310562749.4A CN116594029A (zh) 2023-05-18 2023-05-18 一种基于2d-3d匹配网络的地理定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310562749.4A CN116594029A (zh) 2023-05-18 2023-05-18 一种基于2d-3d匹配网络的地理定位方法

Publications (1)

Publication Number Publication Date
CN116594029A true CN116594029A (zh) 2023-08-15

Family

ID=87589285

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310562749.4A Pending CN116594029A (zh) 2023-05-18 2023-05-18 一种基于2d-3d匹配网络的地理定位方法

Country Status (1)

Country Link
CN (1) CN116594029A (zh)

Similar Documents

Publication Publication Date Title
US9760996B2 (en) Non-rigid registration for large-scale space-time 3D point cloud alignment
Sim et al. Integrated position estimation using aerial image sequences
US9989969B2 (en) Visual localization within LIDAR maps
US8427505B2 (en) Geospatial modeling system for images and related methods
AU2007355942B2 (en) Arrangement and method for providing a three dimensional map representation of an area
US5576964A (en) System and method for relating a passive sensor to a geographic environment
CN105352509B (zh) 地理信息时空约束下的无人机运动目标跟踪与定位方法
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法
Qu et al. Landmark based localization in urban environment
CN111830953A (zh) 车辆自定位方法、装置及系统
US11430199B2 (en) Feature recognition assisted super-resolution method
CN108364279B (zh) 确定静止轨道遥感卫星指向偏差的方法
Jende et al. A fully automatic approach to register mobile mapping and airborne imagery to support the correction of platform trajectories in GNSS-denied urban areas
Dawood et al. Harris, SIFT and SURF features comparison for vehicle localization based on virtual 3D model and camera
CN114359476A (zh) 一种用于城市峡谷环境导航的动态3d城市模型构建方法
GB2527997A (en) Registration of SAR images by mutual information
JP2002532770A (ja) 映像に関連してカメラポーズを決定する方法及びシステム
Fervers et al. Continuous self-localization on aerial images using visual and lidar sensors
Khoshelham et al. Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry
Haala et al. A multi-sensor system for positioning in urban environments
Chellappa et al. On the positioning of multisensor imagery for exploitation and target recognition
CN113838129B (zh) 一种获得位姿信息的方法、装置以及系统
CN116594029A (zh) 一种基于2d-3d匹配网络的地理定位方法
Shukla et al. Automatic geolocation of targets tracked by aerial imaging platforms using satellite imagery
Jende et al. Fully automatic feature-based registration of mobile mapping and aerial nadir images for enabling the adjustment of mobile platform locations in GNSS-denied urban environments

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