CN114972668A - 基于高度信息的激光slam方法和系统 - Google Patents

基于高度信息的激光slam方法和系统 Download PDF

Info

Publication number
CN114972668A
CN114972668A CN202210599426.8A CN202210599426A CN114972668A CN 114972668 A CN114972668 A CN 114972668A CN 202210599426 A CN202210599426 A CN 202210599426A CN 114972668 A CN114972668 A CN 114972668A
Authority
CN
China
Prior art keywords
point cloud
data
robot
current frame
cloud data
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
CN202210599426.8A
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.)
Shenzhen Graduate School Harbin Institute of Technology
Original Assignee
Shenzhen Graduate School Harbin Institute of Technology
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 Shenzhen Graduate School Harbin Institute of Technology filed Critical Shenzhen Graduate School Harbin Institute of Technology
Priority to CN202210599426.8A priority Critical patent/CN114972668A/zh
Publication of CN114972668A publication Critical patent/CN114972668A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/20—Editing of 3D images, e.g. changing shapes or colours, aligning objects or positioning parts
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/80—Geometric correction
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2200/00—Indexing scheme for image data processing or generation, in general
    • G06T2200/32—Indexing scheme for image data processing or generation, in general involving image mosaicing
    • 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/20—Special algorithmic details
    • G06T2207/20212—Image combination
    • G06T2207/20221—Image fusion; Image merging
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2219/00—Indexing scheme for manipulating 3D models or images for computer graphics
    • G06T2219/20—Indexing scheme for editing of 3D models
    • G06T2219/2004—Aligning objects, relative positioning of parts

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Software Systems (AREA)
  • Geometry (AREA)
  • Computer Graphics (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Hardware Design (AREA)
  • Architecture (AREA)
  • Remote Sensing (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及基于高度信息的激光SLAM方法和系统。其中的方法包括：获取三维激光雷达数据，进行运动畸变和动态点滤除处理，得到第一点云数据；获取机器人IMU数据，进行预积分处理，得到第一位姿数据；对第一点云数据提取特征，得到特征点云，并记录特征点云的高度，并根据特征点云和特征点云的高度，得到激光雷达里程计数据；基于因子图，对第一位姿数据和激光雷达里程计数据进行融合，得到第二位姿数据作为机器人位姿；根据第二位姿数据确定对应的点云数据，拼接生成三维点云地图。本发明能够使机器人在陌生的集伤点场景下实现高精度的自身定位和高精度建图。

Description

基于高度信息的激光SLAM方法和系统
技术领域
本发明涉及一种基于高度信息的激光SLAM方法和系统，属于机器人技术领域。
背景技术
机器人定位和建图技术是移动机器人的关键技术，许多重要的应用场景，例如电力检查、室外安保巡逻、地震检伤等，都需要机器人能够在陌生的环境中自主定位和建图。
例如在地震集伤点检伤时，集伤点一般选择较为开阔的区域，如广场、篮球场、足球场等。这是在地震灾难发生后，由于救援人员未能第一时间到达地震现场开展救援工作，而大多数救援方式为自救和互救；因此，为了避免伤员再次受伤将救出的伤员安置到相对开阔的区域。但是，传统的激光SLAM(Simultaneous Localization and Mapping，即时定位与建图)方法在地震集伤点场景中不能稳定地建图和定位。
发明内容
本发明提供基于高度信息的激光SLAM方法和系统，旨在至少解决现有技术中存在的技术问题之一。
本发明的技术方案一方面涉及一种基于高度信息的激光SLAM方法，包括以下步骤：
S1、获取三维激光雷达数据，进行运动畸变和动态点滤除处理，得到第一点云数据；
S2、获取机器人IMU数据，进行预积分处理，得到第一位姿数据；
S3、对所述第一点云数据提取特征，得到特征点云，并记录所述特征点云的高度；
S4、根据所述特征点云和所述特征点云的高度，得到激光雷达里程计数据；
S5、基于因子图，对所述第一位姿数据和所述激光雷达里程计数据进行融合，得到第二位姿数据作为机器人位姿；
S6、根据所述第二位姿数据确定对应的点云数据，拼接生成三维点云地图。
进一步地，所述步骤S1包括：
S11、对于所述三维激光雷达数据，通过对机器人进行运动补偿，获得第二点云数据；
S12、获取当前帧所述第二点云数据与局部地图的深度图像数据的差值，根据所述差值确定当前帧所述第二点云数据是否为动态点；所述局部地图为机器人行进过程中根据当前帧之前的第一时间内的所述第一点云数据生成的地图；
S13、删除确定为动态点的所述第二点云数据，得到所述第一点云数据。
进一步地，所述步骤S12包括：若所述差值大于第一阀值，则当前所述第二点云数据为动态点。
进一步地，所述特征点云包括角点特征点云；所述步骤S3包括：计算当前帧中的所述第一点云数据中每个的点云的曲率；将曲率大于第一曲率阈值的点云作为所述角点特征点云。
进一步地，所述步骤S3包括：从当前帧中的所述第一点云数据中获取地面点云数据；对所述地面点云数据进行平面拟合，得到当前帧的地面平面方程。
进一步地，所述步骤S4包括：检测出当前帧的所述第一点云数据中的角点特征点云；并获取所述角点特征点云对应的地图特征点；获取所述地图特征点在局部地图中对应的边线特征的两个坐标点，得到通过这两个坐标点的第一直线；所述局部地图为机器人行进过程中根据当前帧之前的所述第一点云数据生成的地图；
将所述地图特征点与所述第一直线的距离作为优化函数，以缩短距离获取机器人的真实位姿。
进一步地，所述步骤S4包括：以当前帧的地面平面方程为特征，与局部地图的地面平面方程匹配；所述局部地图为机器人行进过程中根据当前帧之前的所述第一点云数据生成的地图；获取当前帧的地面平面方程与匹配的地面平面方程的法向量角度差，作为优化函数。
进一步地，所述步骤S4包括：获取当前帧的所述第一点云数据的特征点云与局部地图的高度匹配差，作为优化函数；所述局部地图为机器人行进过程中根据当前帧之前的所述第一点云数据生成的地图。
进一步地，所述步骤S5包括：将所述第一位姿作为当前的机器人位姿；基于因子图，通过所述激光雷达里程计数据对当前的机器人位姿进行更新，得到所述第二位姿作为当前的机器人位姿；所述因子图的因子包括所述第一位姿数据、所述激光雷达里程计数据和所述特征点云的高度。
本发明的技术方案还涉及一种计算机可读存储介质，其上储存有计算机程序指令，所述计算机程序指令被处理器执行时实施上述的方法。
本发明的技术方案还涉及一种基于高度信息的激光SLAM的系统，所述系统包括：
机器人，设有惯性测量单元和激光雷达装置；其中，所述惯性测量单元用于获取机器IMU数据，所述激光雷达装置用于获取三维激光雷达数据；和，
与所述机器人连接的计算机装置，所述计算机装置包括处理器及存储介质，所述处理器用于执行存储介质中存储的指令序列，以执行上述的基于高度信息的激光SLAM方法。
本发明的有益效果如下。
本发明所提出的基于高度信息的激光SLAM方法和系统，通过激光雷达而非视觉传感器来获取点云数据，减弱了对环境光照的要求。另外，本发明与IMU(InertialMeasurement Unit，惯性测量单元)传感器融合，并附加环境中的高度信息进行匹配定位，相较于传统的激光SLAM方法针对静态环境和空间特征点较多的场景，更适用于将伤员集中于较为宽阔平整的地面上而缺乏特征点，同时有动态的救援人员的集伤点场景，能够使机器人在陌生的集伤点场景下实现高精度的自身定位和高精度建图。基于此，机器人能够自主高效且安全的实现对伤员的检伤分类，进而在有限的医疗资源下最大化的救治伤员。
附图说明
图1是根据本发明的基于高度信息的激光SLAM方法的基本流程图。
图2是根据本发明的实施例对数据的处理示意图。
图3是根据本发明的实施例中的点云畸变的示意图。
图4是根据本发明的实施例中的地图特征点至第一直线的距离示意图。
图5是根据本发明的实施例中的平面法向量夹角示意图。
图6是根据本发明的实施例中的融合数据的因子示意图。
图7是根据本发明的实施例中的生成的三维点云地图的示例。
图8是根据本发明的基于高度信息的激光SLAM系统的基本框图。
具体实施方式
以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。
需要说明的是，如无特殊说明，当某一特征被称为“固定”、“连接”在另一个特征，它可以直接固定、连接在另一个特征上，也可以间接地固定、连接在另一个特征上。本文所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。此外，除非另有定义，本文所使用的所有的技术和科学术语与本技术领域的技术人员通常理解的含义相同。本文说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本发明。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。
参照图1，在一些实施例中，根据本发明的方法包括以下步骤：
S1、获取三维激光雷达数据，进行运动畸变和动态点滤除处理，得到第一点云数据；
S2、获取机器人IMU数据，进行预积分处理，得到第一位姿数据；
S3、对第一点云数据提取特征，得到特征点云，并记录特征点云的高度；
S4、根据特征点云和特征点云的高度，得到激光雷达里程计数据；
S5、基于因子图，对第一位姿数据和所述激光雷达里程计数据进行融合，得到第二位姿数据作为机器人位姿；
S6、根据第二位姿数据确定对应的点云数据，拼接生成三维点云地图。
本发明的一些实施例中，在例如集伤点等场景，搭载有惯性测量单元和激光雷达装置的机器人在行进过程中，参照图2，通过惯性测量单元获取机器人自身的IMU数据，通过激光雷达装置获取场景空间中的三维激光雷达数据。之后，对IMU数据处理得到IMU里程计数据(相当于第一位姿)，并对激光雷达数据处理得到激光雷达里程计数据。再以因子图方式，将IMU里程计数据与激光雷达数据进行融合，得到第二位姿数据并作为当前机器人的位姿。然后，根据当前机器人的位姿以及获取的对应的点云数据，更新三维点云地图。
即，本发明的实施例中，三维点云地图是在机器人的行进过程中即时更新的。以当前帧为例，局部地图表示：机器人行进过程中，根据在当前帧之前的第一时间内的点云数据生成的地图。其中，第一时间可自定义配置，例如，可以是以距离当前帧之前的时间长度的形式，也可以是以配置起始时间(例如：机器人开始采集三维激光雷达数据时)，而设结束时间为当前帧之前的那一帧的形式。在本实施例中，局部地图一般可以理解为设置一个区域，如果当前帧的位姿与第一帧的位姿之间的距离(x，y平面)大于设定的一个阈值，那么当前帧与第一帧之间所有的帧组合为一个局部地图，当前帧之后的帧属于下一个局部地图。局部地图一般是以距离划分。
下面，将详细地描述本发明上述步骤的具体实施方式。
步骤S1的具体实施方式
本发明的实施例中，激光点数据不是瞬时获得的，激光测量时伴随着机器人的运动。在一个数据采集周期内，当激光雷达运动到不同位置时，机器人以为在同一个位置测量出来的激光点数据，因此会导致出现畸变，参照图3。
为去除点云畸变，本发明的一些实施例中，例如，通过获取当前帧点云的开始时间和结束时间，并假设在该时间间隔内机器人为匀速运动，通过对机器人的运动补偿进而对获取第二点云数据作为真实的点云数据。应理解地，对机器人的运动补偿的方法并不限于此。
另外，去除畸变后的第二点云数据中仍包含动态的救援人员。若不去除动态点，而是直接对其执行以下步骤S3以产生角点特征点，再用角点特征点进行匹配，可能会导致一定的误差进而影响定位结果。因此，本发明的实施例中，还包括对去畸变后的点云进行动态点滤除的步骤。
在一些实施例中，例如采用基于RangeImage方法进行动态点滤除。具体地，对当前帧的第二点云数据和局部地图分别进行深度图像数据计算，得到对应的深度图像数据(RangeImage)；获取这两者的差值Diff，并与配置的第一阈值λ比较，若大于λ则确定该第二点云数据对应的点云为动态点。参照如下公式：
Diff＝Range(current)-Range(map)
Figure BDA0003669391660000051
步骤S2的具体实施方式
由于图像的运动模糊、快速运动、纯旋转的一系列问题，导致仅一个激光雷达很难满足实际场景的应用需求，而IMU直接可得到运动主体自身的角速度、加速度的测量数据，从而对运动进行约束，可实现快速的机器人运动的定位和主体纯旋转的处理，通过对IMU测量量的积分操作，能够获得机器人的位姿信息，从而进一步提高SLAM方法的可靠性。
IMU每个时刻可实现自身主体的线加速度a和角速度w的测量，则其测量模型:
Figure BDA0003669391660000052
Figure BDA0003669391660000053
式中，下标t表示在IMU本体坐标系下，并受到加速度偏置ba、陀螺仪偏置bw和附加噪声na、nw的影响。线加速度是重力加速度和物体加速度的合矢量，上标^代表是IMU的测量值。
Figure BDA0003669391660000054
表示世界坐标系到IMU坐标系的变换。
此外，附加噪声na、nw满足高斯分布，如下。
Figure BDA0003669391660000055
步骤S3的具体实施方式
对去畸变的第一点云数据进行处理，得到当前帧对应的角点特征点云和地面点云；并记录角点特征点对应的高度。
基于运动补偿之后的第一点云数据，需要进行特征提取来获取当前机器人的位姿。在本发明的一些实施例中，考虑到实时性需求，例如通过计算当前帧的第一点云数据中每个点的曲率，将曲率大于一定阈值的点作为场景环境中的特征点，得到角点特征点云。
此外，可以通过例如采用RANSAC(Random Sample Consensus)算法，对当前帧的地面点云进行拟合，获得地面的平面方程：
AX+BY+CZ+D＝0
式中，(A,B,C)代表该平面的法向量；D表示当前坐标系原点到平面的距离。即A，B，C，D为地面的平面方程的平面参数。
角点特征点对应的高度的获取方法，例如，如下：
对于S1步骤去畸变的第一点云数据，建立其对应的高度地图并保存。然后计算出角点特征点云的高度信息并保存。在本发明的一些实施例中，考虑到实时性的要求，采用最简单的方法将角点特征点云P的z值作为其对应的高度elevation。
elevation＝PZ
步骤S4的具体实施方式
根据特征点云和特征点云的高度，得到激光雷达里程计数据；
为了能够获取当前帧的第一点云数据对应的机器人位姿，对于上述第一点云数据的角点特征点，寻找其在局部地图中对应的地图特征点。理论上角点特征点与其对应的地图特征点之间的距离应为0。但是实际应用中这两者之间的距离存在一定的偏差。为了获取机器人的真实位姿，可以将两者之间的距离作为优化函数。
在本发明的一些实施例中，考虑到以最快的方式缩短距离，选择角点特征点到第一直线的距离作为优化函数；其中第一直线为通过以下两个坐标点的直线，这两个点为角点特征点在地图中对应的边线特征的两个坐标点：
Figure BDA0003669391660000061
式中xi表示当前帧的第一点云数据中检测出的第i个角点特征点；xj和xl分别表示第i个角点特征点在地图中对应的边线特征的两个坐标点。角点特征点到第一直线的距离d如图4所示。
另外，在一些实施例中，考虑到集伤点环境相对较为开阔的地面，当机器人检测到较少的特征点时会发生较大的位姿估计偏差，因此，针对该特点，将步骤S3拟合得到的平面作为特征。
即，将当前帧的地面方程与局部地图的地面方程匹配，考虑到地面相对平坦机器人的位姿在Z方向的偏差较小可忽略，因此将两者之间的法向量的角度差(参照图5)作为优化函数：
Figure BDA0003669391660000062
式中，a和b分别表示两个平面的法向量，θe为法向量的夹角。
针对特征点云的高度信息，采用当前帧的第一点云数据中的特征点云与局部地图的高度匹配差作为其优化函数：
eh＝||Pz-(Pz)map||
式中，eh为高度匹配差，P为特征点云的高度，(Pz)map为特征点云在局部地图中匹配的点的高度。
步骤S5的具体实施方式
在本发明的一些实施例中，所述步骤5包括：
S51、将步骤S2记录的IMU预积分结果，即第一位姿数据作为当前帧的机器人位姿(相当于令当前帧机器人的位姿为第一位姿数据)；
S52、通过步骤S4的激光雷达里程计数据对上述机器人位姿进行更新，得到第二位姿作为当前帧机器人的最终位姿。激光雷达里程计数据以及上述当前帧的机器人位姿，这两者的结合方式通过因子图的形式进行融合，参照图6。图6的因子图中包括三个因子：MIU因子、雷达里程计因子和高度因子，分别对应于第一位姿数据、激光雷达里程计数据和特征点云的高度。
步骤S6的具体实施方式
在本发明的一些实施例中，例如，根据机器人的第二位姿数据从保存的点云数据中获取对应的三维激光雷达点云数据，进而将不同时刻下机器人位姿对应的点云数据进行拼接得到场景的三维点云地图。
在本发明的另一些实施例中，也可以是，例如，根据当前帧的第二位姿数据和当前帧获取到的第一点云数据，在局部地图的基础上更新，以生成当前场景完整的三维点云地图。本发明的实施例生成的三给点云地图的示例，如图7所示。地图的更新操作可以按预设的频率(例如按一定的时间周期)或者按预设的规则进行，例如，若确定机器人行进距离大于某长度，则触发地图更新。
参照图8，在一些实施例中，根据本发明的一种基于高度信息的激光SLAM的系统，所包括：机器人100和计算机装置200。机器人100，设置有惯性测量单元和激光雷达装置；其中，惯性测量单元用于获取机器IMU数据，激光雷达装置用于获取三维激光雷达数据。计算机装置200，与机器人连接，计算机装置200包括处理器及存储介质，处理器用于执行存储介质中存储的指令序列，以执行上述的基于高度信息的激光SLAM方法。应理解的是，计算机装置200可以内置于机器人内部，可以是例如可编程逻辑芯片等；也可以是与机器人通信连接的计算机装置，例如电脑，平板，智能手机等。
因此，在本发明提出的方案中，通过搭载有惯性测量单元和激光雷达的机器人，相较于传统的激光SLAM方法，可有效降低因集伤点中将伤员集中于较为宽阔平整的地面上而缺乏特征点导致的精度下降，同时排除集伤点动态的救援人员的数据干扰，能够使机器人在陌生的集伤点场景下实现高精度的自身定位和高精度建图。基于此，机器人能够自主高效且安全的实现对伤员的检伤分类，进而在有限的医疗资源下最大化的救治伤员。
应当认识到，本发明所有实施例中的方法步骤可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。
此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。
进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还可以包括计算机本身。
计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。
以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。

Claims (10)

1.一种基于高度信息的激光SLAM方法，包括以下步骤：
S1、获取三维激光雷达数据，进行运动畸变和动态点滤除处理，得到第一点云数据；
S2、获取机器人IMU数据，进行预积分处理，得到第一位姿数据；
S3、对所述第一点云数据提取特征，得到特征点云，并记录所述特征点云的高度；
S4、根据所述特征点云和所述特征点云的高度，得到激光雷达里程计数据；
S5、基于因子图，对所述第一位姿数据和所述激光雷达里程计数据进行融合，得到第二位姿数据作为机器人位姿；
S6、根据所述第二位姿数据确定对应的点云数据，拼接生成三维点云地图。
2.根据权利要求1所述的方法，其中，所述步骤S1包括：
S11、对于所述三维激光雷达数据，通过对机器人进行运动补偿，获得第二点云数据；
S12、获取当前帧的所述第二点云数据与局部地图的深度图像数据的差值，根据所述差值确定当前帧的所述第二点云数据是否为动态点；所述局部地图为机器人行进过程中根据当前帧之前的第一时间内所述第一点云数据生成的地图；
S13、删除确定为动态点的所述第二点云数据，得到所述第一点云数据。
3.根据权利要求1所述的方法，其中，所述特征点云包括角点特征点云；所述步骤S3包括：
计算当前帧中的所述第一点云数据中每个的点云的曲率；
将曲率大于第一曲率阈值的点云作为所述角点特征点云。
4.根据权利要求1或3所述的方法，所述步骤S3包括：
从当前帧中的所述第一点云数据中获取地面点云数据；
对所述地面点云数据进行平面拟合，得到当前帧的地面平面方程。
5.根据权利要求3所述的方法，其中，所述步骤S4包括：
检测出当前帧的所述第一点云数据中的角点特征点云；并获取所述角点特征点云对应的地图特征点；
获取所述地图特征点在局部地图中对应的边线特征的两个坐标点，得到通过这两个坐标点的第一直线；所述局部地图为机器人行进过程中根据当前帧之前的第一时间内的所述第一点云数据生成的地图；
将所述地图特征点与所述第一直线的距离作为优化函数，以缩短距离获取机器人的真实位姿，
其中第一直线为通过以下两个坐标点的直线，这两个点为角点特征点在地图中对应的边线特征的两个坐标点：
Figure FDA0003669391650000021
式中xi表示当前帧的第一点云数据中检测出的第i个角点特征点，xj和xl分别表示第i个角点特征点在地图中对应的边线特征的两个坐标点。
6.根据权利要求4所述的方法，其中，所述步骤S4包括：
以当前帧的地面平面方程为特征，与局部地图的地面平面方程匹配；所述局部地图为机器人行进过程中根据当前帧之前的第一时间内的所述第一点云数据生成的地图；
获取当前帧的地面平面方程与匹配的地面平面方程的法向量角度差，作为优化函数。
7.根据权利要求1所述的方法，其中，所述步骤S4包括：获取当前帧的所述第一点云数据的特征点云与局部地图的高度匹配差，作为优化函数；所述局部地图为机器人行进过程中根据当前帧之前的所述第一点云数据生成的地图。
8.根据权利要求1所述的方法，其中，所述步骤S5包括：
将所述第一位姿作为当前的机器人位姿；
基于因子图，通过所述激光雷达里程计数据对当前的机器人位姿进行更新，得到所述第二位姿作为当前的机器人位姿；所述因子图的因子包括所述第一位姿数据、所述激光雷达里程计数据和所述特征点云的高度。
9.一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施如权利要求1至8中任一项所述的方法。
10.一种基于高度信息的激光SLAM系统，其特征在于，所述系统包括：
机器人，设有惯性测量单元和激光雷达装置；其中，所述惯性测量单元用于获取机器IMU数据，所述激光雷达装置用于获取三维激光雷达数据；和，
与所述机器人连接的计算机装置，所述计算机装置包括处理器及存储介质，所述处理器用于执行存储介质中存储的指令序列，以执行如权利要求1至8中任一项所述的方法。
CN202210599426.8A 2022-05-30 2022-05-30 基于高度信息的激光slam方法和系统 Pending CN114972668A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210599426.8A CN114972668A (zh) 2022-05-30 2022-05-30 基于高度信息的激光slam方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210599426.8A CN114972668A (zh) 2022-05-30 2022-05-30 基于高度信息的激光slam方法和系统

Publications (1)

Publication Number Publication Date
CN114972668A true CN114972668A (zh) 2022-08-30

Family

ID=82957913

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210599426.8A Pending CN114972668A (zh) 2022-05-30 2022-05-30 基于高度信息的激光slam方法和系统

Country Status (1)

Country Link
CN (1) CN114972668A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115655262A (zh) * 2022-12-26 2023-01-31 广东省科学院智能制造研究所 基于深度学习感知的多层级语义地图构建方法和装置
CN117451033A (zh) * 2023-12-21 2024-01-26 广东石油化工学院 一种同步定位与地图构建方法、装置、终端及介质

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115655262A (zh) * 2022-12-26 2023-01-31 广东省科学院智能制造研究所 基于深度学习感知的多层级语义地图构建方法和装置
CN117451033A (zh) * 2023-12-21 2024-01-26 广东石油化工学院 一种同步定位与地图构建方法、装置、终端及介质
CN117451033B (zh) * 2023-12-21 2024-05-14 广东石油化工学院 一种同步定位与地图构建方法、装置、终端及介质

Similar Documents

Publication Publication Date Title
CN109087359B (zh) 位姿确定方法、位姿确定装置、介质和计算设备
CN109084746B (zh) 用于具有辅助传感器的自主平台引导系统的单目模式
CN107990899B (zh) 一种基于slam的定位方法和系统
CN107888828B (zh) 空间定位方法及装置、电子设备、以及存储介质
US11313684B2 (en) Collaborative navigation and mapping
CN109461208B (zh) 三维地图处理方法、装置、介质和计算设备
Lupton et al. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions
US10133278B2 (en) Apparatus of controlling movement of mobile robot mounted with wide angle camera and method thereof
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
KR101776622B1 (ko) 다이렉트 트래킹을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
CN109885080B (zh) 自主控制系统及自主控制方法
KR101776621B1 (ko) 에지 기반 재조정을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법
EP2917754B1 (en) Image processing method, particularly used in a vision-based localization of a device
Langelaan State estimation for autonomous flight in cluttered environments
CN107748569B (zh) 用于无人机的运动控制方法、装置及无人机系统
US20220051031A1 (en) Moving object tracking method and apparatus
CN114972668A (zh) 基于高度信息的激光slam方法和系统
CN112639502A (zh) 机器人位姿估计
CN111197984A (zh) 一种基于环境约束下的视觉-惯性运动估计方法
KR101985344B1 (ko) 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
WO2021036587A1 (zh) 面向电力巡检场景的定位方法及系统
Tamjidi et al. 6-DOF pose estimation of a portable navigation aid for the visually impaired
CN112506200A (zh) 机器人定位方法、装置、机器人及存储介质
US9163943B1 (en) Systems and methods for using magnetic field readings to refine device location estimates

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