CN113358112B - 一种地图构建方法及一种激光惯性里程计 - Google Patents
一种地图构建方法及一种激光惯性里程计 Download PDFInfo
- Publication number
- CN113358112B CN113358112B CN202110618535.5A CN202110618535A CN113358112B CN 113358112 B CN113358112 B CN 113358112B CN 202110618535 A CN202110618535 A CN 202110618535A CN 113358112 B CN113358112 B CN 113358112B
- Authority
- CN
- China
- Prior art keywords
- information
- map
- laser
- estimated
- dynamic object
- 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
- 238000010276 construction Methods 0.000 title claims abstract description 22
- 238000005457 optimization Methods 0.000 claims abstract description 31
- 238000001514 detection method Methods 0.000 claims abstract description 25
- 239000011159 matrix material Substances 0.000 claims abstract description 23
- 238000005259 measurement Methods 0.000 claims abstract description 13
- 230000003993 interaction Effects 0.000 claims abstract description 4
- 239000000523 sample Substances 0.000 claims abstract description 4
- 238000000034 method Methods 0.000 claims description 46
- 230000033001 locomotion Effects 0.000 claims description 23
- 230000008569 process Effects 0.000 claims description 15
- 238000013507 mapping Methods 0.000 claims description 14
- 230000011218 segmentation Effects 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000007613 environmental effect Effects 0.000 claims description 5
- 238000013527 convolutional neural network Methods 0.000 claims description 4
- 230000010354 integration Effects 0.000 description 8
- 230000006870 function Effects 0.000 description 6
- 230000004927 fusion Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 230000004807 localization Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000003915 air pollution Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 238000004898 kneading Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/50—Systems of measurement based on relative movement of target
- G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明属于自动驾驶领域，具体涉及一种地图构建方法及一种激光惯性里程计，地图构建方法包括传感器信息获取步骤、动态物体去除步骤、里程计数据获取步骤、信息比对步骤、回环检测步骤、非线性优化步骤及地图绘制步骤；激光惯性里程计包括数据交互的点阵扫描装置和惯性测量装置；所述惯性测量装置与所述点阵扫描装置的扫描探头在空间中相互固定设置。本发明的一种地图构建方法及一种激光惯性里程计对环境的适应性和鲁棒性强，构建的地图精度高。
Description
技术领域
本发明属于自动驾驶领域，具体涉及一种地图构建方法及一种激光惯性里程计。
背景技术
自动驾驶技术是近年的热点话题，在缓解交通拥堵、提高道路安全性、减少空气污染等领域，自动驾驶将会带来颠覆性的改变。由于自动驾驶面对的往往是非可控环境，其关键难点之一在于环境的不可预知性，如何准确、快速、有效地获取和掌握环境信息并准确定位是其它模块正常运行的先决条件和重要保证。
在室外场景下常常可以借助全球卫星导航***(Global Navigation SatelliteSystem,GNSS)进行绝对定位，然而其信号常常会因为高楼、桥梁、隧道等遮挡从而造成定位精度不稳定，甚至丢失位置信息的情况。
为了保证定位的稳定性，一种可行的方法是预先对环境建立地图，再实时地根据传感器数据在已有地图中确定位置和姿态。这种方法能够有效地避免卫星信号遮挡的问题，在多种环境下均能稳定地工作，因此越来越多地被应用到自动驾驶中。此时，预先建立的环境地图的精度和质量就显得尤为重要，其直接影响了后续定位的准确性。
为了构建适合于定位的点云地图，常常采用同时定位与建图(SimultaneousLocalization and Mapping,SLAM)的方法。SLAM是一种定位与建图紧耦合的问题，经典的SLAM框架主要可以分为传感器信息获取、前端(里程计)、后端(非线性优化)、回环检测以及建图等部分。
对于建图前端而言，视觉里程计在室外会受到光照变化、天气、运动模糊等影响，其准确性和鲁棒性不高。激光里程计在一定程度上抗环境干扰，然而现有技术往往直接匹配相邻激光帧，或引入IMU提供匹配初值，形成松耦合的激光惯性里程计，精度相对不高。此外IMU往往有一定的零偏，不经估计校准而直接使用也会引入误差。
对于建图后端而言，基于滤波器的优化方法存储量增长过快以及全局优化性能不佳等缺点。基于确定性图优化的方法，如BA和位姿图，在每次添加新节点时都会对整个图进行优化，当节点数和边数较多时会消耗大量时间，且存在着不少重复计算，效率较低。
对于现有的较为完整的建图技术方案，他们往往针对确定的传感器形式进行设计，难以扩展，不利与多传感器融合以提高建图精度。另外，这些方案未考虑到环境中的动态障碍，如车辆、行人等，直接应用于建图不仅会引入前端里程计匹配误差，而且会在地图中造成拖影，影响地图的质量。
发明内容
有鉴于此，本发明提出一种高精度高质量的点云地图构建方法，对环境的适应性和鲁棒性强，构建的地图精度高。
为了达到上述技术目的，本发明所采用的具体技术方案为：
一种地图构建方法，包括：
传感器信息获取，用于通过点阵扫描装置测量所在地图构建装置在运动过程中的位置信息、速度信息、姿态信息和周围环境信息；
动态物体去除，用于利用卷积神经网络的点云分割算法去除所述周围环境信息中的动态物，获得预估环境信息；
里程计数据获取，用于基于惯性测量装置获得地图构建装置的在运动过程中的速度和姿态；
信息比对，比对所述传感器信息获取所得信息和所述里程计数据获取所得信息，生成预估位置信息、预估速度信息和预估姿态信息；
回环检测，用于在所述地图构建装置到达循环检测地点时，获得的预估环境信息，比对上一次所述地图构建装置到所述循环检测地点所获得的预估环境信息，获得偏差校正信息；
非线性优化及地图绘制，基于因子图优化框架，将预估位置信息、预估速度信息、预估姿态信息、预估环境信息和偏差校正信息代入世界地图中，依据所述偏差校正信息对两次到达所述循环检测地点之间的累积偏差进行非线性优化，绘制地图；
其中，所述点阵扫描装置的相邻扫描位置之间存在至少三个相同的扫描点。
进一步的，所述点阵扫描装置为3D激光雷达。
进一步的，所述惯性测量装置为惯性测量单元(Inertial Measurement Unit,IMU)。
进一步的，所述点云分割算法为cnn_seg。
进一步的，所述非线性优化依据GTSAM优化库。
进一步的，所述动态物体去除的具体方法包括以下步骤：
S101:将激光雷达的激光帧输入点云分割的网络中，输出所述激光帧的所有扫描点的标签类别信息；
S102：在所述激光帧中索引并去除所有扫描点的标签类别信息中，属于动态物体类别的扫描点，得到动态物体去除激光帧。
进一步的，所述动态物体去除的具体方法还包括，
S103：进一步索引并去除所述动态物体去除激光帧中属于动态物体类别的扫描点。
进一步的，所述IMU获得的地图构建装置在运动过程中的位置、速度和姿态通过以下公式计算：
其中：v为速度，p为位置，R为姿态旋转矩阵，g为重力加速度常量。
进一步的，还包括：
IMU位置和姿态校准，通过去除所述IMU的噪声校准所述IMU获得的所述地图构建装置在运动过程中的速度和姿态。
进一步的，所述信息比对的具体方法为：
S201:从所述激光帧中提取点特征和面特征，代入位置信息和姿态信息后与所述世界地图融合，生成局部线特征地图和/或局部面特征地图；
S202：将所有局部线特征地图和/或局部面特征地图根据所述IMU获得的地图构建装置的在运动过程中的位置、速度和姿态串合之后形成单帧地图；
S203：利用GTSAM优化库中的calculateEstimate函数对IMU零偏进行估计，生成的估计值为预偏移值，所述预偏移值用于作为所述激光雷达的下一个激光帧的单帧地图的空间偏移量。
同时，本发明还提出一种激光惯性里程计，包括数据交互的点阵扫描装置和惯性测量装置；所述惯性测量装置与所述点阵扫描装置的扫描探头在空间中相互固定设置。
采用上述技术方案，本发明能够带来以下有益效果：
1、本发明实现了一种高精度高质量的点云地图构建方法，对环境的适应性和鲁棒性强，构建的地图精度高；
2、本发明采用因子图作为非线性优化后端，整体结构上为便于实现多传感器融合而设计，可扩展性强。同时各传感器可以优势互补，进一步提高建图精度和环境的适应性；
3、本发明具有动态物体去除模块，能够有效地避免动态物体在地图中造成的拖影现象，建图质量高。同时该模块能使前端匹配过程中免受动态物体的干扰，提高了前端里程计的精度。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1为本发明具体实施方式中一种地图构建方法的整体流程示意图；
图2为本发明具体实施方式中非线性优化的流程示意图。
具体实施方式
下面结合附图对本发明实施例进行详细描述。
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本发明，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。
还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践所述方面。
在本发明的一个实施例中，提出一种地图构建方法，包括：
传感器信息获取，用于通过点阵扫描装置测量所在地图构建装置在运动过程中的位置信息、速度信息、姿态信息和周围环境信息；
动态物体去除，用于利用卷积神经网络的点云分割算法去除周围环境信息中的动态物，获得预估环境信息；
里程计数据获取，用于基于惯性测量装置获得地图构建装置的在运动过程中的里程计位置信息、里程计速度信息和里程计姿态信息；
信息比对，比对传感器信息获取所得信息和里程计数据获取所得信息，生成预估位置信息、预估速度信息和预估姿态信息；
回环检测，用于在所述地图构建装置到达循环检测地点时，获得的预估环境信息，比对上一次所述地图构建装置到所述循环检测地点所获得的预估环境信息，获得偏差校正信息；
非线性优化及地图绘制，基于因子图优化框架，将预估位置信息、预估速度信息、预估姿态信息、预估环境信息和偏差校正信息代入世界地图中，依据所述偏差校正信息对两次到达所述循环检测地点之间的累积偏差进行非线性优化，绘制地图；
其中，点阵扫描装置的相邻扫描位置之间存在至少三个相同的扫描点。
在本实施例中，点阵扫描装置为激光雷达，激光雷达为激光探测及测距***的简称，由发射机、天线、接收机、跟踪架及信息处理等部分组成，其原理与传统雷达类似，区别在于将传统雷达的“雷达波”替换为点阵激光，通过接收机接收反射回来的点阵激光汇总后即可探测周围环境，本实施例的激光雷达不限定于特定的激光线束数量，能够得到周围环境信息即可。通过设置固定的扫描频率，在运动过程中多次记录并对比前后扫描结果，就能够得到地图构建装置在运动过程中的位置信息、速度信息、姿态信息和周围环境信息；
在本实施例中，惯性测量装置为IMU(惯性传感器)，本实施例的IMU还应包含AHRS(Attitude and Heading Reference System，航姿测量***)、GNSS(Global NavigationSatellite System，全球导航卫星***)等能输出加速度和角速度信息的传感器。
在本实施例中，点云分割算法为cnn_seg等一类具有输入激光帧点云、输出各点类别标签功能的网络。非线性优化依据GTSAM优化库。
在本实施例中，动态物体去除的具体方法包括以下步骤：
S101:将激光雷达的激光帧输入点云分割的网络中，输出激光帧的所有扫描点的标签类别信息；
S102：在激光帧中索引并去除所有扫描点的标签类别信息中，属于动态物体类别的扫描点，得到动态物体去除激光帧。
在本实施例中，动态物体去除的具体方法还包括，
S103：进一步索引并去除动态物体去除激光帧中属于动态物体类别的扫描点。
在本实施例中，IMU获得的地图构建装置在运动过程中的位置、速度和姿态通过以下公式计算：
其中：v为速度，p为位置，R为姿态旋转矩阵，g为重力加速度常量。
本实施例还包括：
IMU位置和姿态校准，通过去除IMU的噪声校准IMU获得的地图构建装置在运动过程中的速度和姿态。
在本实施例中，信息比对的具体方法为：
S201:从激光帧中提取点特征和面特征，代入位置信息和姿态信息后与世界地图融合，生成局部线特征地图和/或局部面特征地图；
S202：将所有局部线特征地图和/或局部面特征地图根据IMU获得的地图构建装置的在运动过程中的位置、速度和姿态串合之后形成单帧地图；
S203：利用calculateEstimate函数对IMU零偏进行估计，生成的估计值为预偏移值，预偏移值用于作为激光雷达的下一个激光帧的单帧地图的空间偏移量。
本实施例还提出一种激光惯性里程计，包括数据交互的点阵扫描装置和惯性测量装置；惯性测量装置与点阵扫描装置的扫描探头在空间中相互固定设置。
以下为结合激光惯性里程计的高精度高质量的点云地图构建的具体方法：
高精度高质量的点云地图构建方法包括四部分：动态物体去除、里程计前端处理(传感器信息获取)、非线性优化后端处理(非线性优化)、回环检测和地图构件。本实施例总体流程示意图如图1所示，传感器信息首先输入动态物体去除模块，输出的不含动态物体的激光帧数据及其他传感器数据作为后续模块的输入。前端模块实时给出节点间的位姿关系，后端模块对带有误差和噪声的节点间位姿关系进行非线性优化。同时，回环检测模块根据传感器信息和已有的节点关系判断是否回到先前到过的地方，并向后端添加相应的约束。最后，后端输出优化后的节点位姿信息，并结合激光帧数据整合为地图。
以下进行分步描述：
1)动态物体去除
环境中往往存在大量的动态物体，如行人、车辆等，若直接对传感器采集到的数据进行建图，则地图中会存在拖影从而影响质量，同时动态物体也会影响前端匹配的准确性。因此，需要先进行处理。
动态物体去除模块对激光雷达所获得的激光帧去除动态物体所代表的点，主要由基于卷积神经网络的点云分割算法实现，如cnn_seg等。点云分割的网络输入为激光雷达所获得的激光帧，输出为该帧中所有点所属的标签类别信息。在得到标签类别信息后，索引其中属于动态物体类别的点，并在激光雷达所获得的激光帧中直接去除，此时的结果作为动态物体去除模块的输出结果。
由于点云分割算法往往存在一定的漏检率，动态物体去除模块对后端优化后用于构建地图的激光帧还会进行一次检测和去除操作，以确保地图的质量。
2)里程计前端处理
里程计前端模块用于给出当前传感器的位姿信息，其定位精度很大程度上决定了最终建图的精度。本实施例提出一种IMU和激光雷达紧耦合的激光惯性里程计方法，能够给出准确的位姿信息。
IMU可以直接给出自身的加速度值、角速度值，通过积分可以得到一段时间Δt内自身位置、速度、姿态的变化情况。为了避免更新估计IMU零偏后重新计算所有积分值，本实施例采用预积分的方法确定自身位置、速度、姿态，用公式表示为
其中v为速度，p为位置，R为姿态旋转矩阵，g为重力加速度常量。
由于IMU的数据往往带有一定的噪声，在经过高频率的积分后会被放大，因此预积分得到的节点间相对的位置和姿态值需要进一步校准。本实施例将IMU预积分后得到的位姿Ti作为激光帧匹配的初始值，激光帧匹配的最终结果，即相邻节点之间的位姿关系，作为里程计前端的输出。
激光帧匹配采用基于特征和局部地图融合的方法。特征提取基于点的粗糙度，设pi为待计算粗糙度的一点，集合S为与pi在同一个激光通道(即ring值相同)的相邻点的集合，其中各有一半的点在pi的两侧，则其粗糙度可通过公式计算：
通过设置阈值cl和ch，可以把点分为线特征(c>ch)和面特征(c<cl)。对最近n个节点的激光帧分别提取线面特征，并利用节点的位姿信息{Ti-n,Ti-n+1,…,Ti-1}把特征点变换到统一的世界坐标系W下作融合，可分别得到局部融合的线特征地图和面特征地图。
得到局部融合的线特征地图和面特征地图后，可利用IMU预积分后得到的位置和姿态作为初始值进行激光帧的匹配。首先对当前激光帧分别提取线、面特征，然后利用IMU预积分后得到的位姿Ti把线、面特征变换到世界坐标系W下。对于第k个线特征，寻找其在局部融合的线特征地图中与该特征点最近的两点，记为
对于第k个面特征，寻找其在局部融合的面特征地图中与该特征点最近的三点，记为
接下来确定线、面特征的距离误差值：对于第k个线特征，距离误差
为该特征点到
确定的直线的距离；对于第k个面特征，距离误差
为该特征点到
确定的平面的距离。最后优化以下函数
即可得到里程计前端输出的节点位姿信息Ti。
在激光帧匹配得到节点位姿信息Ti后，利用GTSAM优化库的ISAM2类中的calculateEstimate函数可实现对IMU零偏的估计，估计的结果将作为下次IMU预积分时使用的偏移值。
3)非线性优化后端处理
本实施例采用因子图作为非线性优化后端，具体实现时采用了GTSAM开源优化库，其示意图如图2所示。其中l为路标，x为位姿节点，z为观测数据，位姿节点x1在运动方程f的作用下产生了下一个位姿节点x2。运动方程f对位置节点x的约束有前述IMU预积分产生的位姿约束和激光帧匹配所产生的位姿约束，以及下述的回环检测约束。路标点l和观测z则可以是GPS或者RTK给出的定位结果。注意到运动方程f，路标点l和观测z可以是多样的，不局限于所举例子，例如运动方程f还可以是轮速里程计(wheel odometry)给出的约束，路标点l和观测z还可以是通过相机观察环境提取到的特征点和相应的对应关系约束。可见本实施例能够方便有效地实现多传感器融合。
4)回环检测
本实施例里程计前端部分实现的是相对定位，即下一位点xi+1的位姿变换约束Ti+1是相对于当前节点xi的。随着时间的推移不可避免地会引入累积误差，因此本实施例引入回环检测模块，其旨在通过传感器信息和已有的节点关系判断是否回到先前到过的地方，并向后端添加相应的约束以消除累积误差。
本实施例的回环检测模块基于距离检测和激光帧匹配。对于新的状态节点xi+1，首先通过欧氏距离‖·‖2，在已有状态状态节点{x1,x2,…,xi}中寻找距离最小的节点，设为xl。然后，对找到的节点融合其前后m个节点{xl-m,…,xl,…,xl+m}的激光帧，得到局部融合的线特征地图和面特征地图。融合方法同(2)里程计前端“激光帧匹配采用的基于特征和局部地图融合的方法”。
得到局部融合的线特征地图和面特征地图后，利用(2)里程计前端“激光帧的匹配”方法可计算出新的状态节点xi+1与回环节点xl的相对变换Tl,i+1。该相对变换Tl,i+1可作为运动方程f的约束添加到(3)非线性优化后端的因子图中，进行统一优化从而减小全局的累积误差。
5)地图构建
待全部传感器数据处理完毕、(3)非线性优化后端对所有节点做出优化后，即可得到各个节点{x1,x2,…,xn}的位姿变换信息{T1,T2,…,Tn}。此时属于各个节点{x1,x2,…,xn}的激光帧还会再经过(1)动态物体去除模块进行动态物体检测与去除。然后，根据{T1,T2,…,Tn}即可把各节点的激光帧变换到统一的世界坐标系W下，各激光帧直接叠加即得到本实施例最终构建的关于环境的高精度高质量点云地图。
如图1所示，本实施例的传感器信息首先输入动态物体去除模块，输出的不含动态物体的激光帧数据及其他传感器数据作为后续模块的输入。前端模块实时给出节点间的位姿关系，后端模块对带有误差和噪声的节点间位姿关系进行非线性优化。同时，回环检测模块根据传感器信息和已有的节点关系判断是否回到先前到过的地方，并向后端添加相应的约束。最后，后端输出优化后的节点位姿信息，并结合激光帧数据整合为地图。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。
Claims (7)
1.一种地图构建方法，其特征在于，包括：
传感器信息获取，用于通过点阵扫描装置测量所在地图构建装置在运动过程中的位置信息、速度信息、姿态信息和周围环境信息；
动态物体去除，用于利用卷积神经网络的点云分割算法去除所述周围环境信息中的动态物，获得预估环境信息；
里程计数据获取，用于基于惯性测量装置获得地图构建装置的在运动过程中的速度和姿态；
信息比对，比对所述传感器信息获取所得信息和所述里程计数据获取所得信息，生成预估位置信息、预估速度信息和预估姿态信息；
回环检测，用于在所述地图构建装置到达循环检测地点时，获得的预估环境信息，比对上一次所述地图构建装置到所述循环检测地点所获得的预估环境信息，获得偏差校正信息；
非线性优化及地图绘制，基于因子图优化框架，将预估位置信息、预估速度信息、预估姿态信息、预估环境信息和偏差校正信息代入世界地图中，依据所述偏差校正信息对两次到达所述循环检测地点之间的累积偏差进行非线性优化，绘制地图；
其中，所述点阵扫描装置的相邻扫描位置之间存在至少三个相同的扫描点；
所述点云分割算法为cnn_seg；所述非线性优化依据GTSAM优化库；
所述惯性测量装置为惯性测量单元，即IMU；
所述信息比对的具体方法为：
S201:从激光帧中提取点特征和面特征，代入位置信息和姿态信息后与所述世界地图融合，生成局部线特征地图和/或局部面特征地图；
S202：将所有局部线特征地图和/或局部面特征地图根据所述IMU获得的地图构建装置的在运动过程中的位置、速度和姿态串合之后形成单帧地图；
S203：利用GTSAM优化库中的calculateEstimate函数对IMU零偏进行估计，生成的估计值为预偏移值，所述预偏移值用于作为激光雷达的下一个激光帧的单帧地图的空间偏移量。
2.根据权利要求1所述的地图构建方法，其特征在于：所述点阵扫描装置为3D激光雷达。
3.根据权利要求2所述的地图构建方法，其特征在于，所述动态物体去除的具体方法包括以下步骤：
S101:将激光雷达的激光帧输入点云分割的网络中，输出所述激光帧的所有扫描点的标签类别信息；
S102：在所述激光帧中索引并去除所有扫描点的标签类别信息中，属于动态物体类别的扫描点，得到动态物体去除激光帧。
4.根据权利要求3所述的地图构建方法，其特征在于，所述动态物体去除的具体方法还包括
S103：进一步索引并去除所述动态物体去除激光帧中属于动态物体类别的扫描点。
5.根据权利要求1所述的地图构建方法，其特征在于，所述IMU获得的地图构建装置在运动过程中的位置、速度和姿态通过以下公式计算：
其中：v为速度，p为位置，R为姿态旋转矩阵，g为重力加速度常量。
6.根据权利要求1所述的地图构建方法，其特征在于，所述地图构建方法还包括：
IMU位置和姿态校准，通过去除所述IMU的噪声校准所述IMU获得的所述地图构建装置在运动过程中的速度和姿态。
7.根据权利要求1-6之任一项所述的地图构建方法的一种激光惯性里程计，其特征在于：包括数据交互的点阵扫描装置和惯性测量装置；所述惯性测量装置与所述点阵扫描装置的扫描探头在空间中相互固定设置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110618535.5A CN113358112B (zh) | 2021-06-03 | 2021-06-03 | 一种地图构建方法及一种激光惯性里程计 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110618535.5A CN113358112B (zh) | 2021-06-03 | 2021-06-03 | 一种地图构建方法及一种激光惯性里程计 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113358112A CN113358112A (zh) | 2021-09-07 |
| CN113358112B true CN113358112B (zh) | 2023-01-17 |
Family
ID=77531894
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110618535.5A Active CN113358112B (zh) | 2021-06-03 | 2021-06-03 | 一种地图构建方法及一种激光惯性里程计 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113358112B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113850864B (zh) * | 2021-09-14 | 2024-04-12 | 中南大学 | 一种面向室外移动机器人的gnss/lidar回环检测方法 |
| CN114018284B (zh) * | 2021-10-13 | 2024-01-23 | 上海师范大学 | 一种基于视觉的轮速里程计校正方法 |
| CN114170320B (zh) * | 2021-10-29 | 2022-10-28 | 广西大学 | 一种基于多传感器融合的打桩机自动定位和工况自适应方法 |
| CN115493579A (zh) * | 2022-09-02 | 2022-12-20 | 松灵机器人(深圳)有限公司 | 定位修正方法、装置、割草机器人以及存储介质 |
| CN115200588B (zh) * | 2022-09-14 | 2023-01-06 | 煤炭科学研究总院有限公司 | 移动机器人的slam自主导航方法及其装置 |
| CN115326068B (zh) * | 2022-10-17 | 2023-01-24 | 北京理工大学 | 激光雷达-惯性测量单元融合里程计设计方法及*** |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106570454B (zh) * | 2016-10-10 | 2019-06-11 | 同济大学 | 基于移动激光扫描的行人交通参数提取方法 |
| IL250382B (en) * | 2017-01-31 | 2021-01-31 | Arbe Robotics Ltd | A radar-based system and method for real-time simultaneous positioning and mapping |
| CN110261870B (zh) * | 2019-04-15 | 2021-04-06 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN111105495A (zh) * | 2019-11-26 | 2020-05-05 | 四川阿泰因机器人智能装备有限公司 | 一种融合视觉语义信息的激光雷达建图方法及*** |
| CN111583136B (zh) * | 2020-04-25 | 2023-05-23 | 华南理工大学 | 一种救援场景下自主移动平台同时定位与建图方法 |
| CN111929699B (zh) * | 2020-07-21 | 2023-05-09 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及*** |
-
2021
- 2021-06-03 CN CN202110618535.5A patent/CN113358112B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113358112A (zh) | 2021-09-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113358112B (zh) | 一种地图构建方法及一种激光惯性里程计 | |
| CN110160542B (zh) | 车道线的定位方法和装置、存储介质、电子装置 | |
| CN110412635B (zh) | 一种环境信标支持下的gnss/sins/视觉紧组合方法 | |
| CN109341706B (zh) | 一种面向无人驾驶汽车的多特征融合地图的制作方法 | |
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
| CN111652179A (zh) | 基于点线特征融合激光的语义高精地图构建与定位方法 | |
| CN112162297B (zh) | 一种剔除激光点云地图中动态障碍伪迹的方法 | |
| JP2007333385A (ja) | 不動物位置記録装置 | |
| JP4984659B2 (ja) | 自車両位置推定装置 | |
| CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
| CN112859051A (zh) | 激光雷达点云运动畸变的矫正方法 | |
| CN114323033B (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
| CN110187375A (zh) | 一种基于slam定位结果提高定位精度的方法及装置 | |
| CN114612348B (zh) | 激光点云运动畸变校正方法、装置、电子设备及存储介质 | |
| CN110515110B (zh) | 数据评估的方法、装置、设备和计算机可读存储介质 | |
| CN114565674B (zh) | 自动驾驶车辆城市结构化场景纯视觉定位方法及装置 | |
| CN114623823B (zh) | 一种融合里程计的uwb多模态定位***、方法及装置 | |
| CN116452763A (zh) | 一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法 | |
| CN114119886A (zh) | 高精地图点云重建方法、装置、车辆、设备和存储介质 | |
| CN115127543A (zh) | 一种激光建图优化中异常边剔除方法及*** | |
| CN115930977A (zh) | 特征退化场景的定位方法、***、电子设备和可读存介质 | |
| CN115574816A (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN113838129B (zh) | 一种获得位姿信息的方法、装置以及*** | |
| Nguyen et al. | Mcd: Diverse large-scale multi-campus dataset for robot perception | |
| CN117388830A (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| PE01 | Entry into force of the registration of the contract for pledge of patent right | ||
| PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A Map Construction Method and a Laser Inertial Odometer Effective date of registration: 20230727 Granted publication date: 20230117 Pledgee: Haidian Beijing science and technology enterprise financing Company limited by guarantee Pledgor: Beijing Chaoxing Future Technology Co.,Ltd. Registration number: Y2023110000311 |
|
| PC01 | Cancellation of the registration of the contract for pledge of patent right | ||
| PC01 | Cancellation of the registration of the contract for pledge of patent right |
Granted publication date: 20230117 Pledgee: Haidian Beijing science and technology enterprise financing Company limited by guarantee Pledgor: Beijing Chaoxing Future Technology Co.,Ltd. Registration number: Y2023110000311 |