CN115046540A - 一种点云地图构建方法、***、设备和存储介质 - Google Patents

一种点云地图构建方法、***、设备和存储介质 Download PDF

Info

Publication number
CN115046540A
CN115046540A CN202210579859.7A CN202210579859A CN115046540A CN 115046540 A CN115046540 A CN 115046540A CN 202210579859 A CN202210579859 A CN 202210579859A CN 115046540 A CN115046540 A CN 115046540A
Authority
CN
China
Prior art keywords
point cloud
wheel speed
data
representing
imu
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
CN202210579859.7A
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.)
New Drive Chongqing Intelligent Automobile Co ltd
Original Assignee
New Drive Chongqing Intelligent Automobile Co ltd
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 New Drive Chongqing Intelligent Automobile Co ltd filed Critical New Drive Chongqing Intelligent Automobile Co ltd
Priority to CN202210579859.7A priority Critical patent/CN115046540A/zh
Publication of CN115046540A publication Critical patent/CN115046540A/zh
Pending legal-status Critical Current

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/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/3848—Data obtained from both position sensors and additional sensors
    • 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
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • 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/40—Extraction of image or video features
    • 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/10004—Still image; Photographic image

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Processing Or Creating Images (AREA)

Abstract

本发明公开基于一种点云地图构建方法、***、设备和存储介质，S1：获取车辆的IMU、轮速计数据以构建里程计增量估计模型，再采用所述里程计增量估计模型对激光雷达数据进行预处理，得到第一数据；S2：对所述第一数据进行地面分割、拟合、特征提取，得到第一特征；S3：将所述第一特征与局部地图进行特征匹配，输出当前帧的位姿，并构建约束参数；S4：将约束参数加入因子图对当前帧的位姿进行优化，构建点云地图。通过构建里程计增量估计模型对多传感器数据进行处理，以及构建多传感器预积分约束，从而在地图建模过程中进行优化，提高输出地图的精度。

Description

一种点云地图构建方法、***、设备和存储介质
技术领域
本发明涉及点云地图构建技术领域，特别涉及公开一种点云地图构建方法、***、设备和存储介质。
背景技术
Slam(Simultaneous Localization andMapping，机器人同时定位与建图)在移动机器人、自动驾驶导航当中起着基础且重要的作用，它是自动驾驶车辆在未知环境下实现自主导航、建图最主要的技术，能够以一定的频率估计移动设备自身的运动状态，包括姿态、位置、速度，同时建立周围环境或稠密或稀疏的环境点三维坐标即所谓建图，它为移动机器人后续运动规划、控制提供必要的基础，而且感知模块所感知到的动态物体才能够有一个合理的位置信息。
通常它由以下几个部分组成：(1)数据预处理节点，负责接收传感器数据，对传感器数据做同步和对齐、去畸变等操作。(2)里程计节点，负责以较高的频率输出自身位姿。(3)后端优化节点，在更大的尺度范围内更新、优化状态变量，输出优化后的变量，里程计节点会利用最新得到的优化后的变量进行后续的变量估计。(4)建图节点，将所有移动设备所捕获到的环境特征点组成一张全局的静态地图，并且执行全局的状态变量的优化。(5)回环检测，如果检测到回环，会利用此信息进一步消除里程计的累计误差。
Slam技术在最近一些年获得了很大的发展，由最初的以视觉为主要传感器的方案，发展为以3d激光雷达为主要传感器的方案，以及近两年来备受关注的视觉、激光融合的方案，总体发展的方向是越来越多的融合不同的传感器，达到增强***在不同环境下的鲁棒性、实时性的目的，因为各种不同的传感器都有其优势和劣势，通过传感器融合***能够做到优势互补，有更强的环境适应性。
在矿山场景下，车辆往往会经历各种不同的环境和路况，有平坦的区域，也会经过非常颠簸且有一定坡度的区域，在这样的情况下，激光雷达的运动畸变会变得更加严重，因此其匹配的精度因而下降，也会经过长长的走廊使得激光雷达无法捕捉到足够的环境信息，使得控制精度降低。
发明内容
针对现有技术中Slam***在起伏路况输出点云地图的精度较低的问题，本发明提出一种点云地图构建方法、***、设备和存储介质，通过构建里程计增量估计模型对多传感器数据进行处理，以及构建多传感器预积分约束，从而在地图建模过程中进行优化，提高输出地图的精度。
为了实现上述目的，本发明提供以下技术方案：
一种点云地图构建方法，具体包括以下步骤：
S1：获取车辆的IMU、轮速计数据以构建里程计增量估计模型，再采用所述里程计增量估计模型对激光雷达数据进行预处理，得到第一数据；
S2：对所述第一数据进行地面分割、拟合、特征提取，得到第一特征；
S3：将所述第一特征与局部地图进行特征匹配，输出当前帧的位姿，并构建约束参数；
S4：将约束参数加入因子图对当前帧的位姿进行优化，构建点云地图。
优选地，还包括：
S5：构建误差传播模型对约束参数进行优化，并将优化后的约束参数加入因子图。
优选地，所述S5中，约束参数包括：激光里程计相对位姿约束、imu预积分、轮速计预积分、回环检测结果。
优选地，所述S1中，里程计增量估计模型的表达式为：
Figure BDA0003661914180000031
公式(1)中，cij为i时刻到j时刻之间车辆的运动轨迹，由轮速计获取；c是地面粗糙度；
Figure BDA0003661914180000032
表示车辆在航向上的角度，dθpitch分别为车辆在俯仰方向上的角度，由IMU获取。
优选地，所述轮速计预积分res为下式：
Figure BDA0003661914180000033
公式(2)中，αkk+1表示轮速计得到的位置预积分测量值，βkk+1表示轮速计得到的速度预积分测量值；RIw为世界坐标系w向imu坐标系的旋转矩阵；PWIk+1表示k+1时刻轮速计原点在世界坐标系中的位置，PWIk表示k时刻轮速计原点在世界坐标系中的位置，vWIk表示第k时刻轮速计在世界坐标系中的速度；vWIk+1表示第k+1时刻轮速计在世界坐标系中的速度。
优选地，所述误差传播模型为：
Figure BDA0003661914180000034
公式(3)中，ηp表示轮速计位置噪声，ηv表示轮速计速度噪声；
Figure BDA0003661914180000035
表示imu预积分的旋转参数；
Figure BDA0003661914180000036
表示imu预积分旋转参数的误差量；
Figure BDA0003661914180000037
表示第k时刻imu或者轮速计i时刻的位置向量；
Figure BDA0003661914180000038
表示第k时刻imu或者轮速计i时刻的速度向量；
Figure BDA0003661914180000039
表示k+1时刻的位置误差，
Figure BDA00036619141800000310
表示k+1时刻的速度误差，i表示误差对应的是车体坐标系i时刻。
优选地，所述预处理为对激光雷达数据进行去畸变：
激光雷达数据中每个点的坐标通过下式转换到点云i时刻的坐标系上：
Figure BDA0003661914180000041
公式(4)中，pi为去畸变后i时刻点云数据中点的坐标，pk是去畸变前点云数据中点的坐标，
Figure BDA0003661914180000042
表示i时刻第k个点云点对应的旋转量，t表示平移量。
本发明还提供一种点云地图构建***，包括：
传感器采集模块，用于采集传感器检测的数据，传感器包括激光雷达、IMU和轮速计；
数据预处理模块，用于构建里程计增量估计模型对采集的传感器数据进行预处理和特征提取，输出第一特征；
前端激光里程计模块，用于根据第一特征输出位置和姿态；
后端优化和建图模块，用于对传感器数据进行优化并根据位置和姿态构建点云地图。
本发明还提供一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述点云地图构建方法的步骤。
本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述点云地图构建方法的步骤。
综上所述，由于采用了上述技术方案，与现有技术相比，本发明至少具有以下有益效果：
1.将imu与轮速计数据相结合得到里程计增量估计模型，能够更好的拟合模拟矿山场景下凹凸不平路面的运动场景，进而能够更好的进行激光雷达的畸变去除，帮助激光雷达里程计更好地完成当前帧与全局地图的匹配，并且当点云畸变被去除后对于利用激光雷达所做得感知的准确性也有很大的好处，能够减少点云杂点以及点云分层的现象，对于感知中的障碍物检测有直接的好处；
2.由于轮速计的加入，构建了轮速计预积分误差因子，可以对状态变量做更好的约束；
3.在lio-sam模型基础加入了上述模块，使得lio-sam的表现更加鲁棒，尤其使其无gps信号以及结构信息较少的场景下，保持较好的运动估计能力。
附图说明：
图1为根据本发明示例性实施例的一种点云地图构建***示意图。
图2为根据本发明示例性实施例的一种点云地图构建方法示意图。
图3为根据本发明示例性实施例的车辆在车辆坐标系下轨迹示意图。
具体实施方式
下面结合实施例及具体实施方式对本发明作进一步的详细描述。但不应将此理解为本发明上述主题的范围仅限于以下的实施例，凡基于本发明内容所实现的技术均属于本发明的范围。
在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
如图1所示，本发明提供一种点云地图构建***，包括传感器采集模块，数据预处理模块、前端激光里程计模块、后端优化和建图模块、回环检测模块。
传感器采集模块的输出端与数据预处理模块的输入端连接，数据预处理模块的输出端与前端激光里程计模块的输入端连接，前端激光里程计模块的输出端与后端优化和建图模块的第一输入端连接，回环检测模块的输出端与后端优化和建图模块的第二输入端连接，后端优化和建图模块的输出端输出地图。
传感器采集模块，用于采集传感器检测的数据，传感器包括激光雷达、IMU(Inertial measurement unit，惯性测量单元)和轮速计；
数据预处理模块，用于构建里程计增量估计模型对采集的传感器数据去畸变，构建预积分约束参数(包括IMU预积分约束、轮速计预积分约束、激光里程计预积分约束)并进行优化，作为后端优化和建图模块的优化参数。
用于对采集的传感器数据进行预处理，包括同步、对齐、去畸变等操作；
前端激光里程计模块，用于检测输出自身的位置和姿态；
后端优化和建图模块，用于根据前端激光里程计模块输出的位置和姿态构建一张全局的点云地图，并且执行全局的状态变量的优化，前端激光里程计模块会利用最新得到的优化后的变量进行后续的变量估计；
回环检测模块，消除前端激光里程计模块的累计误差。
本实施例中，前端激光里程计与回环检测模块均采用lio-sam技术，lio-sam是一项优秀的激光惯性紧融合slam方案。
本实施例中，在现有slam***上增加了轮速计传感器，轮速计的加入能够对路况上下起伏抖动较严重的路面短时间的精确运动估计进而将估计的结果应用到点云去畸变的预处理过程中，另外，因轮速计传感器的加入引入了轮速计的预积分及其误差传播方法，然后可以将轮速计预积分量加入到后端优化中更好的约束状态变量。
本方案主要考虑的场景是在矿山这种经常碰到崎岖不平路段的情况，大多数算法将地面设想为一整个平面或者小的平面的组合，这限制了在复杂地形下的应用。本发明通过构建了一个三维曲线位姿增量模型，利用到了轮速计和imu测量数据，更加适应复杂地形的情况。
本实施例中，如图2所示，还提出一种点云地图构建方法，包括以下步骤：
S1：获取车辆上传感器数据，输入数据预处理模块中的里程计增量估计模型进行预处理，得到第一数据。
S1-1：构建里程计增量估计模型。
本实施例中，传感器包括激光雷达、IMU和轮速计，imu的采集频率＞轮速计的采集频率＞激光雷达的采集频率。
如3图所示，在轮速计的数据周期内，在i时刻建立参考坐标系(原点为Oi)，在j时刻时，cij为i时刻到j时刻之间车辆的运动轨迹，
Figure BDA0003661914180000071
表示j时刻车辆的X轴坐标，
Figure BDA0003661914180000072
表示j时刻车辆的y轴坐标，
Figure BDA0003661914180000073
表示j时刻车辆的z轴坐标。
那么这段时间的位移向量可以表示成如下：
Figure BDA0003661914180000074
公式(1)中，Lij表示车辆在极坐标下i时刻到j时刻的位移向量；Iij表示向量长度；
Figure BDA0003661914180000075
表示车辆在航向上的角度，dθpitch分别为车辆在俯仰方向上的角度，可通过imu的测量值积分获得；T表示转置矩阵。
因为车辆在轮速计一个周期内的角度变化是非常小的，可以通过一下公式近似计算Iij：
Figure BDA0003661914180000076
公式(2)中，cij为i时刻到j时刻之间车辆的运动距离，可通过轮速计获得；C是地面粗糙度，它是为了补偿地面小的起伏带来的误差，最小的地面粗糙度是1，表示地面平坦，在其它情况下，地面粗糙度可以根据不同的地面特性进行设置。
cij为一个标量表示车轮向前滚了多长的距离，这个数据可以直接由轮速计根据轮子转过的角度以及轮子的半径得到，这一点和车子在平面上行驶是一样的，在平面上行驶同样可以根据轮子转过的角度和半径算出一段时间内轮子滚了多少距离，不同点在于起伏路面轮子滚动的轨迹是曲线，而平面上轮子滚动轨迹为直线。
一般轮速计得到的数据只是一个一维的值，其含义是车轮滚动的距离或者是车的速度值，结合其它传感器数据，得到车辆平面行驶的线速度和转向角速度，在纵向z方向没有值，通常设置为0，这样的方法实质属于平面建模。
本实施例中，结合了轮速计和imu得到的位置坐标是一个三维向量，它在纵向z方向有值，因此可以构建曲线轨迹。
在以i时刻为原点的坐标系中车辆位置变化量的向量表达，即里程计增量估计模型的表达式如下：
Figure BDA0003661914180000081
公式(3)中，dpij表示笛卡尔坐标系坐标下i时刻到j时刻的位移向量，cij为i时刻到j时刻之间车辆的运动距离；c是地面粗糙度；
Figure BDA0003661914180000082
表示车辆在航向上的角度，dθpitch分别为车辆在俯仰方向上的角度。
S1-2：采用里程计增量估计模型对激光雷达数据进行预处理得到第一数据。
本实施例中，由于激光雷达在扫描过程中的运动导致了运动畸变，利用S1构建的运动增量模型结合线性插值可以知道两帧点云任意时刻激光雷达位姿进而进行去畸变的操作。
去畸变的方法为：
首先接收到激光雷达的第i帧点云数据，获取第i帧点云数据开始时刻时间戳和结束时刻时间戳；
在imu的数据缓存中找到比第i帧点云数据开始时刻时间戳早但最接近的第一imu数据，然后找到比点云结束时间戳晚但最接近的第二imu数据，就是说找到一系列的imu数据集合X＝{X1，X2，...，Xn}，Xn表示第n帧imu数据，且X1的时间戳比第i帧点云数据开始时刻时间戳早，Xn的时间戳比第i帧点云数据结束时刻时间戳晚，即要求这些imu数据集合恰好包含第i帧点云数据的时间(开始时刻时间戳和结束时刻时间戳)，因为imu数据频率远大于点云所以符合要求的数据是可以找到的。然后同理，也找到相应时间的轮速计数据集合Y＝{Y1，Y2，...，Ym}，Ym表示第m帧轮速计数据。
对于imu数据集合X，从第一个数据开始积分，然后依次会得到每个imu时刻对应的位置、速度、旋转角。对于点云中的每个点，读取它的时间戳信息，然后去imu旋转角数据中找到前后两帧的数据，对这两帧数据进行线性插值就得到了点云点对应的旋转量。
对于轮速计数据集合Y，同样从第一个数据进行积分，在每次积分的时候用到本发明所述运动增量模型，得到一个三维坐标，对于每个轮速计值都对应一个三维坐标，同样对于点云中的每个点，根据其时间戳信息，找到前后两个轮速计对应的三维坐标信息，对其进行线性插值，这样就得到了这个点对应的平移量。
每个点云中的点知道了其旋转量和平移量，就能将每个点的坐标通过下式转换到点云开始时刻的坐标系上：
Figure BDA0003661914180000091
公式(4)中，pi为去畸变后点云数据中点的坐标，pk是去畸变前点云数据中点的坐标，
Figure BDA0003661914180000092
表示第k个点云点对应的旋转量，t表示平移量。
S2：构建传感器预积分，并进行误差优化。
因为imu的采集频率和轮速计的采集数据频率要高于激光雷达传感器的采集频率，那么在两个连续的激光雷达数据帧之间可以构建预积分约束，后续会把预积分约束加入到位姿优化当中。由于IMU预积分的技术在很多方案中已经应用，本发明不再赘述。主要说明使用轮速计构建预积分，定义轮速计预积分为下式：
Figure BDA0003661914180000101
公式(4)中，αkk+1表示轮速计得到的位置预积分测量值，βkk+1表示轮速计得到的速度预积分测量值，作为约束参数加入到后端优化中。
RIw为世界坐标系w向imu坐标系的旋转矩阵，PWIk+1表示k+1时刻轮速计本体系原点在世界坐标系中的位置，vWIk表示第k时刻轮速计的速度在世界坐标系中的表达，为简单起见认为以轮速计坐标系与imu坐标系重合，严格来说轮速计和imu之间存在一个外参转换矩阵T，可借此矩阵将轮速计坐标系下的量由轮速计坐标系转换到imu坐标系，而imu坐标系通常认为就是车体坐标系。
在轮速计测量过程中，存在测量误差，包括前一个时刻的误差、当前时刻的测量噪声、以及旋转误差。因此需构建误差传播模型对误差进行优化：
Figure BDA0003661914180000102
公式(5)中，ηp表示轮速计位置噪声，ηv表示轮速计速度噪声；
Figure BDA0003661914180000103
表示imu预积分的旋转参数；
Figure BDA0003661914180000104
表示imu预积分旋转参数的误差量；
Figure BDA0003661914180000105
表示imu第k时刻或者轮速计i时刻的位置向量；
Figure BDA0003661914180000106
表示imu第k时刻或者轮速计i时刻的速度向量；
Figure BDA0003661914180000107
表示k+1时刻的位置误差，其中i表示这个误差是表示在i时刻的imu坐标系也就是i时刻车体坐标系上；
Figure BDA0003661914180000108
表示k+1时刻的速度误差。
上式表示预积分的时间段内，有很多时刻，对应一系列的轮速计值和imu值，因为轮速计和imu帧率远高于激光的帧率，所以两个激光数据之间存在很多不同时刻的轮速计和imu值，
i时刻表示预积分开始时刻；k表示整个预积分时间段内轮速计的第k个值，对应k时刻；这个式子的目的是当知道k时刻的误差时，如何得到k+1时刻的误差，进而由此递推公式得到在预积分结束时刻整个预积分的误差。
而k+1时刻的误差由三个组成部分，一个是k时刻本身带有的误差，一个是当轮速计测量得到第k+1个数据时的测量噪声，第三个是由imu数据得到的旋转信息带有的误差。
Figure BDA0003661914180000111
表示第k时刻imu或者轮速计在i时刻也就是积分开始时刻imu坐标系中的位置向量，
Figure BDA0003661914180000112
表示第k时刻imu或者轮速计在i时刻也就是积分开始时刻的imu坐标系中的速度向量，
Figure BDA0003661914180000113
表示k+1时刻的位置误差，其中i表示这个误差是表示在i时刻的imu坐标系也就是i时刻车体坐标系上。
Figure BDA0003661914180000114
表示k+1时刻的速度误差。
本实施例中，因为预积分作为一个因子图的约束项，要给其分配一个权重，这个权重由误差得来，误差大则权重小些相当于不太信任这次观测，误差小则权重大些更加信任本次观测，比如可以统计误差的最大最小值，然后对当前误差进行归一化，利用归一化的值作为权重值。也可以继续计算误差的方差，根据方差的倒数作为权重值，方差大说明该误差的不确定度比较大，置信度比较小。
S3：采用lego-loam算法对第一数据进行处理，得到第一特征。
采用lego-loam算法对第一数据进行处理包括地面分割、地面拟合、特征提取，这属于现有技术，不是本发明重点。
地面分割，为对雷达数据进行地面分割，目的在于去除掉点云中的地面点减少点云匹配中的干扰因素；地面拟合，用于计算地面内点和法向量；特征包括点云线特征和面特征提取。
S4：前端激光里程计模块将第一特征与局部地图进行特征匹配，匹配成功后输出当前帧的位姿，构建相对运动约束供后端优化使用。
S5：后端优化和建图模块利用因子图进行后端的优化，为了运行的有效性，构建了长度为n的滑动窗口，车辆状态变量作为node，滑动窗口n指局部地图由最近的n个关键帧(当前帧的位姿)构成，新到来的关键帧会与局部地图匹配；激光里程计、imu预积分、轮速计预积分、回环检测结果作为约束参数加入到因子图，从而输出精度更高的地图。
S6：回环检测模块采用scan-context算法进行回环检测，检测到回环后将回环检测结果加入因子图构成一个约束参数。
本发明还提供一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述实施例所述点云地图构建方法的步骤。
本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述实施例所述点云地图构建方法的步骤。
本发明的优点在于：
1.imu与轮速计相结合得到里程计增量估计模型，能够更好的拟合模拟矿山场景下凹凸不平路面的运动场景，进而能够更好的进行激光雷达的畸变去除，帮助激光雷达里程计更好地完成当前帧与全局地图的匹配，并且当点云畸变被去除后对于利用激光雷达所做得感知的准确性也有很大的好处，能够减少点云杂点以及点云分层的现象，对于感知中的障碍物检测有直接的好处；
2.由于轮速计的加入，构建了轮速计预积分误差因子，可以对状态变量做更好的约束；
3.在lio-sam模型基础加入了上述模块，使得lio-sam的表现更加鲁棒，尤其使其无gps信号以及结构信息较少的场景下，保持较好的运动估计能力。
本领域的普通技术人员可以理解，上述各实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。

Claims (10)

1.一种点云地图构建方法，其特征在于，具体包括以下步骤：
S1：获取车辆的IMU、轮速计数据以构建里程计增量估计模型，再采用所述里程计增量估计模型对激光雷达数据进行预处理，得到第一数据；
S2：对所述第一数据进行地面分割、拟合、特征提取，得到第一特征；
S3：将所述第一特征与局部地图进行特征匹配，输出当前帧的位姿，并构建约束参数；
S4：将约束参数加入因子图对当前帧的位姿进行优化，构建点云地图。
2.如权利要求1所述的点云地图构建方法，其特征在于，还包括：
S5：构建误差传播模型对约束参数进行优化，并将优化后的约束参数加入因子图。
3.权利要求2所述的点云地图构建方法，其特征在于，所述S5中，约束参数包括：激光里程计相对位姿约束、imu预积分、轮速计预积分、回环检测结果。
4.如权利要求1所述的点云地图构建方法，其特征在于，所述S1中，里程计增量估计模型的表达式为：
Figure FDA0003661914170000011
公式(1)中，cij为i时刻到j时刻之间车辆的运动轨迹，由轮速计获取；c是地面粗糙度；
Figure FDA0003661914170000012
表示车辆在航向上的角度，dθpitch分别为车辆在俯仰方向上的角度，由IMU获取。
5.如权利要求1所述的点云地图构建方法，其特征在于，所述轮速计预积分res为下式：
Figure FDA0003661914170000013
公式(2)中，αkk+1表示轮速计得到的位置预积分测量值，βkk+1表示轮速计得到的速度预积分测量值；RIw为世界坐标系w向imu坐标系的旋转矩阵；PWIk+1表示k+1时刻轮速计原点在世界坐标系中的位置，PWIk表示k时刻轮速计原点在世界坐标系中的位置，vWIk表示第k时刻轮速计在世界坐标系中的速度；vWIk+1表示第k+1时刻轮速计在世界坐标系中的速度。
6.如权利要求2所述的点云地图构建方法，其特征在于，所述误差传播模型为：
Figure FDA0003661914170000021
公式(3)中，ηp表示轮速计位置噪声，ηv表示轮速计速度噪声；
Figure FDA0003661914170000022
表示imu预积分的旋转参数；
Figure FDA0003661914170000023
表示imu预积分旋转参数的误差量；
Figure FDA0003661914170000024
表示第k时刻imu或者轮速计i时刻的位置向量；
Figure FDA0003661914170000025
表示第k时刻imu或者轮速计i时刻的速度向量；
Figure FDA0003661914170000026
表示k+1时刻的位置误差，
Figure FDA0003661914170000027
表示k+1时刻的速度误差，i表示误差对应的是车体坐标系i时刻。
7.如权利要求1所述的点云地图构建方法，其特征在于，所述预处理为对激光雷达数据进行去畸变：
激光雷达数据中每个点的坐标通过下式转换到点云i时刻的坐标系上：
Figure FDA0003661914170000028
公式(4)中，pi为去畸变后i时刻点云数据中点的坐标，pk是去畸变前点云数据中点的坐标，
Figure FDA0003661914170000029
表示i时刻第k个点云点对应的旋转量，t表示平移量。
8.基于权利要求1-7任一所述方法的一种点云地图构建***，其特征在于，包括：
传感器采集模块，用于采集传感器检测的数据，传感器包括激光雷达、IMU和轮速计；
数据预处理模块，用于构建里程计增量估计模型对采集的传感器数据进行预处理和特征提取，输出第一特征；
前端激光里程计模块，用于根据第一特征输出位置和姿态；
后端优化和建图模块，用于对传感器数据进行优化并根据位置和姿态构建点云地图。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述点云地图构建方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述点云地图构建方法的步骤。
CN202210579859.7A 2022-05-25 2022-05-25 一种点云地图构建方法、***、设备和存储介质 Pending CN115046540A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210579859.7A CN115046540A (zh) 2022-05-25 2022-05-25 一种点云地图构建方法、***、设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210579859.7A CN115046540A (zh) 2022-05-25 2022-05-25 一种点云地图构建方法、***、设备和存储介质

Publications (1)

Publication Number Publication Date
CN115046540A true CN115046540A (zh) 2022-09-13

Family

ID=83159716

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210579859.7A Pending CN115046540A (zh) 2022-05-25 2022-05-25 一种点云地图构建方法、***、设备和存储介质

Country Status (1)

Country Link
CN (1) CN115046540A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115435775A (zh) * 2022-09-23 2022-12-06 福州大学 基于拓展卡尔曼滤波的多传感器融合slam方法
CN115638787A (zh) * 2022-12-23 2023-01-24 安徽蔚来智驾科技有限公司 一种数字地图生成方法、计算机可读存储介质及电子设备
CN116758006A (zh) * 2023-05-18 2023-09-15 广州广检建设工程检测中心有限公司 脚手架质量检测方法及装置
CN117387639A (zh) * 2023-09-22 2024-01-12 成都睿芯行科技有限公司 一种基于激光salm的地图更新***及其方法

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115435775A (zh) * 2022-09-23 2022-12-06 福州大学 基于拓展卡尔曼滤波的多传感器融合slam方法
CN115638787A (zh) * 2022-12-23 2023-01-24 安徽蔚来智驾科技有限公司 一种数字地图生成方法、计算机可读存储介质及电子设备
CN116758006A (zh) * 2023-05-18 2023-09-15 广州广检建设工程检测中心有限公司 脚手架质量检测方法及装置
CN116758006B (zh) * 2023-05-18 2024-02-06 广州广检建设工程检测中心有限公司 脚手架质量检测方法及装置
CN117387639A (zh) * 2023-09-22 2024-01-12 成都睿芯行科技有限公司 一种基于激光salm的地图更新***及其方法

Similar Documents

Publication Publication Date Title
CN115046540A (zh) 一种点云地图构建方法、***、设备和存储介质
CN110906923B (zh) 车载多传感器紧耦合融合定位方法、***、存储介质及车辆
CN112083725B (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位***
CN113819914B (zh) 一种地图构建方法及装置
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN113781582A (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN109828588A (zh) 一种基于多传感器融合的机器人室内路径规划方法
CN111795686A (zh) 一种移动机器人定位与建图的方法
CN110186461B (zh) 一种基于重力梯度信息测距的协同导航方法
CN113819905B (zh) 一种基于多传感器融合的里程计方法及装置
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN113763548B (zh) 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及***
CN113503873B (zh) 一种多传感器融合的视觉定位方法
CN114674311B (zh) 一种室内定位与建图方法及***
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合***
CN115685292B (zh) 一种多源融合导航***的导航方法和装置
CN113466890A (zh) 基于关键特征提取的轻量化激光雷达惯性组合定位方法和***
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN115127543A (zh) 一种激光建图优化中异常边剔除方法及***
CN115183762A (zh) 一种机场仓库内外建图方法、***、电子设备及介质
CN115930977A (zh) 特征退化场景的定位方法、***、电子设备和可读存介质
CN110849349B (zh) 一种基于磁传感器与轮式里程计融合定位方法
CN115540854A (zh) 一种基于uwb辅助的主动定位方法、设备和介质
CN113450411B (zh) 一种基于方差分量估计理论的实时自身位姿计算方法
CN113701749A (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