CN110689622B - 一种基于点云分割匹配闭环校正的同步定位与构图方法 - Google Patents

一种基于点云分割匹配闭环校正的同步定位与构图方法 Download PDF

Info

Publication number
CN110689622B
CN110689622B CN201910604487.7A CN201910604487A CN110689622B CN 110689622 B CN110689622 B CN 110689622B CN 201910604487 A CN201910604487 A CN 201910604487A CN 110689622 B CN110689622 B CN 110689622B
Authority
CN
China
Prior art keywords
point cloud
pose
point
algorithm
map
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
Application number
CN201910604487.7A
Other languages
English (en)
Other versions
CN110689622A (zh
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.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN201910604487.7A priority Critical patent/CN110689622B/zh
Publication of CN110689622A publication Critical patent/CN110689622A/zh
Application granted granted Critical
Publication of CN110689622B publication Critical patent/CN110689622B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/003—Navigation within 3D models or images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • 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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Graphics (AREA)
  • Computer Hardware Design (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于点云分割匹配闭环校正的同步定位与构图算法，属于机器人自主导航和计算机图形学技术领域。本发明所述算法通过对从三维点云中提取的特征点进行帧间匹配得到机器人的相对位姿变换，同时将求得的位姿以图的形式保存在后端，再将点云基于此位姿记录下来形成地图；再利用点云分割和描述算法将点云片段化处理后保存，利用随机森林算法对点云片段进行匹配，形成闭环约束条件；最后通过图优化算法对历史位姿和地图进行修正，实现机器人同步定位与构图。本发明所述方法在保证局部定位精度的同时，对历史位姿和地图进行了保存和修正，有效地减小了室外长距离环境下的累积误差，进而实现了全局一致性良好的机器人同步定位与构图。

Description

一种基于点云分割匹配闭环校正的同步定位与构图方法
技术领域
本发明属于机器人自主导航和计算机图形学技术领域，具体涉及一种基于点云分割匹配闭环校正的同步定位与构图方法。
背景技术
21世纪以来，随着计算机科学的迅猛发展，人类的生活和工作方式正发生着翻天覆地的变化。人工智能的出现为人们从繁重的体力劳动甚至是复杂的脑力劳动中解放提供了巨大的可能。机器人作为协助和替代人类工作的工具，拥有自主导航的能力是实现其智能化的第一步。同步定位与构图SLAM技术能使机器人同时完成构建环境地图和对自身位置的估计，是实现自主导航的先决条件，也是目前机器人领域和人工智能领域备受关注的热点问题。
激光雷达是SLAM算法常用的用来获取环境信息的传感器。三维激光雷达可以采集到海量具有准确角度和距离信息的点，被称为点云，以此来准确的反映真实环境的几何信息。其建图直观，测距精度高且不容易受到光照变化的影响，在室外大型环境的应用中是必不可少的传感器，因此涌现了众多基于其的SLAM算法。但因为三维激光雷达一次扫描可以获得包含上百万个点的点云，且点云的信息单一、无序，缺乏环境的纹理信息，对高效存储、回环闭合检测，即对重复场景的判定，以及SLAM算法的实时性提出了挑战。着眼于三维激光SLAM的应用前景和技术难点，点云分割匹配技术通过将点云片段化处理，为实现回环闭合检测、解决其数据压缩和实时性的需求提供了切实可行的方向。以往的基于三维激光点云的机器人SLAM研究工作主要有以下几种方法：
(1)Zhang等提出了激光里程计与建图LOAM算法，通过将二维激光旋转得到三维点云，并对其提取特征，根据特征点、线和面之间的约束关系来估计机器人位姿和建图。另外，其通过降频多级运算满足了基于三维点云的SLAM算法实时性的需求。
(2)Deschaud提出了一种扫描到模型的匹配方法，通过最小化从当前点到隐式移动最小二乘IMLS表示的表面的距离来完成姿态估计。虽然这种方法精度较高，但是它不是实时在线的SLAM算法。
(3)Serafin等提出了一种基于点邻域特征的SLAM方法，其使用主成分分析PCA描述点与点周围的特征。这种方法对于稀疏点云有较好的效果。
(4)Droeschel等提出了一种基于有效局部映射和分层优化后端的高效连续时间SLAM方法。其考虑了有限的历史位姿对当前位姿估计的影响，提高了机器人局部定位的精度。
三维激光雷达通过对周围环境多线程圆周式的扫描，精确的感知和获取环境的几何信息，形成三维激光点云。以上方法通过对点云的有效处理实现了对机器人的定位以及环境地图的构建，但依然存在很多局限，主要表现在：
(1)缺少对历史位姿和地图的保存修正，轨迹和地图全局一致性较差。以上方法大多基于马尔可夫假设，仅仅利用前一时刻或者相邻几个时刻的位姿来估计机器人当前位姿。由于车轮打滑、传感器测量等带来的误差，上述方法必然会造成误差的累积，并且缺少相应的全局优化模块对历史位姿和地图进行保存和修正。尤其是在室外较大型的长距离环境下运行时，误差随着机器人运行轨迹的增长而累积，导致得到的轨迹与真实轨迹、建立的地图与真实地图在SLAM运行的后期阶段出现较大偏移，从而导致得到的轨迹和地图全局一致性较差。
(2)缺少回环闭合检测，即对重复场景的判定。当机器人观测到重复场景时，应该具有回环闭合检测环节来实现对历史已观测到的重复场景的判定，以此来消除SLAM算法在长距离环境运行下产生的累积误差。由于三维激光点云数据量大，结构特征单一，导致其存储和基于特征的匹配较为困难，所以目前基于三维激光的SLAM算法大多注重局部的匹配精度而缺少回环闭合检测环节。
发明内容
本发明的目的是解决现有的SLAM算法在室外长距离环境下运行时，误差会随距离增加而积累，并且缺少回环闭合检测来对历史位姿和地图进行存储和修正的问题，提供了一种基于点云分割匹配闭环校正的同步定位与构图方法。
本发明所提出的技术问题是这样解决的：
一种基于点云分割匹配闭环校正的同步定位与构图方法，包括以下步骤：
步骤1.利用多线程三维激光雷达获取环境的三维激光点云数据：点云数据通过机器人操作系统ROS以消息的形式传输给计算机；使用开源的点云处理函数库PCL将其表示为无序的散列坐标点；再根据雷达的扫描方式和时间戳对其进行排序；
步骤2.提取特征点：根据点周围局部曲率将当前时刻扫描到的点云区分为角点和平面点；任一点的曲率由该点周围的点与该点形成的和向量的模表示；同时根据雷达视角与局部点云的相对关系剔除断点和孤立点；
步骤3.基于特征点的帧间匹配：利用kd-tree最近邻查找算法在上一时刻tk-1的点云Pk-1中寻找距离当前时刻tk的特征点pi最近的特征点，形成特征线与特征面；利用L-M算法求解关于pi到特征线和特征面的距离的最小二乘问题，从而得到相邻时刻的机器人位姿变换矩阵Tk，进而递推式地求得机器人的位姿；
步骤4.特征点与地图的匹配：利用与步骤3相同的方法，将当前时刻tk提取的特征点pi与已建立好的地图中的特征点云进行匹配，从而对机器人当前时刻的位姿进行校正；根据校正后的位姿将当前时刻的点云从雷达坐标系转换到世界坐标系即为建立的地图；
步骤5.点云分割：同时，将tk时刻获取到的点云数据通过体素分割和聚类的方法，去除地面点云并分割出地上物体的点云片段；
步骤6.利用特征描述子对点云片段进行描述：利用基于点云片段的三维结构张量矩阵的特征值和特征向量，以及点云片段的形状直方图的描述子来描述点云片段的线性、平面性、分散度等特征，并将其作为点云片段的唯一标识进行存储；
步骤7.基于特征描述子的点云片段匹配：通过kd-tree在特征空间中寻找当前点云片段的候选匹配，利用训练好的随机森林分类器为候选匹配打分，再根据设立的阈值和随机抽样一致性检验来确定正确的点云片段配对；根据点云片段的质心计算点云片段间的相对变换关系，以此作为优化的约束条件传入后端图优化步骤；
步骤8.利用图优化对全局位姿和地图进行修正：根据步骤3、步骤4以及步骤7计算得到的不同时刻的机器人位姿和位姿之间的变换关系构建位姿因子图，其中图中节点表示位姿，边表示位姿之间的约束；通过L-M优化算法优化节点状态，使之满足所有的约束，进而实现对全局的位姿和基于位姿的地图进行修正。
步骤3的具体步骤为：
步骤3-1.将tk-1时刻的点云Pk-1根据求得的位姿变换矩阵Tk-1重投影到tk时刻，按照kd-tree的数据结构存储；
步骤3-2.将tk时刻获得的点云Pk根据步骤2的方法提取出角点Ek和平面点Hk，其中Pk＝{pi,i＝1...N}，Ek＝{εi,i＝1...N1}，Hk＝{δi,i＝1...N2}，N是激光单次扫描得到点云总数；设迭代次数为1，初始值
Figure GDA0002999042190000031
步骤3-3.根据最近邻搜索算法在建立好的kd-tree中搜索εi的最近的两个点形成特征线，搜索δi的最近的三个点形成特征面，根据点到线和点到面的距离公式计算角点到特征线的距离dε，平面点到特征面的距离dδ；
步骤3-4.根据一系列的距离即可得到一组非线性方程，用矩阵表示为
Figure GDA0002999042190000032
其中n为迭代次数；令距离向量d趋于0则可得到一个关于
Figure GDA0002999042190000033
的非线性最小二乘问题，则方程关于变量
Figure GDA0002999042190000034
求导即可得到雅可比矩阵
Figure GDA0002999042190000035
根据L-M算法则
Figure GDA0002999042190000036
迭代次数加一；
步骤3-5.判断当前误差e是否达到预设精度要求，或者迭代次数达到指定次数；如达到要求则进行下一步骤；如未达到要求，返回至步骤3-3；
本发明的有益效果是：
本发明对现有的基于三维激光雷达的机器人SLAM算法进行了改进，提出了一种基于点云分割匹配闭环校正的同步定位与构图方法。
(1)对全局位姿和地图进行存储和修正，得到全局一致性良好的位姿轨迹和地图。通过将位姿序列以图的形式建立在后端模块，并且利用图优化算法对全局位姿图进行优化和修正，再根据优化后的位姿轨迹来修正地图，有效地利用了历史全部的位姿和位姿约束信息来对全局位姿序列进行存储和修正，从而能够保证得到的位姿轨迹与地图全局一致性良好。同时，位姿图是基于贝叶斯网络与因子图的稀疏结构，能够满足点云数据的高效存储与SLAM算法实时性的需求。
(2)实现基于点云分割匹配的回环闭合检测，有效地消除了累积误差。通过对点云数据分割和基于特征描述子的描述，使其能够更加高效的存储；进而通过对特征描述子的匹配策略，实现了对历史重复观测到的点云数据的判定。将重复观测到的点云数据作为约束条件送入后端的全局图优化模块，对相关联的全局位姿序列与地图进行修正，进而有效地减小了SLAM算法在室外长距离环境中运行的累积误差。
附图说明
图1为本发明所述基于点云分割匹配闭环校正的同步定位与构图方法流程图；
图2为本发明所述方法得到的轨迹和点云地图的结果图；
图3为本发明中通过点云分割算法得到的点云片段结果图；
图4为本发明中通过点云片段匹配算法得到的点云配准结果图；
图5为无回环闭合检测的传统SLAM算法得到的全局位姿轨迹对比图；
图6为本发明实施实例SLAM算法得到的全局位姿轨迹对比图；
图7为无回环闭合检测的传统SLAM算法得到的全局位姿绝对误差曲线图；
图8为本发明实施实例得到的全局位姿轨迹与真实轨迹的绝对误差曲线图；
图9为无回环闭合检测的传统SLAM算法得到的位姿轨迹每米角度漂移曲线图；
图10为本发明实施实例得到的位姿轨迹每米角度漂移曲线图；
具体实施方式
下面结合附图和实施例对本发明进行进一步的说明。
本实施实例提供一种基于点云分割匹配闭环校正的同步定位与构图方法。本发明实施例采用Velodyne HDL-16E三维激光雷达和KITTI室外长距离城市街道数据集进行算法验证。此外移动机器人还搭载了里程计编码器、惯性测量单元IMU等传感器，以及装有ROS系统的工控机来实现各传感器与机器人间的通信。三维激光雷达的水平扫描范围为360度，俯仰扫描范围为30度，扫描距离为100米；工控机的核心处理器为i5-4460，四线程，3.20GHz，4G运行内存。本实例算法在室外长距离城市街道环境数据集下进行测试，生成的轨迹总长度为2161.48米。本发明选取得到的前2700个位姿与真实位姿轨迹进行了多次实验对比，最终验证了本发明提出的基于点云分割匹配闭环校正的同步定位与构图算法相比现有的一些基于三位激光的同步定位与构图算法在局部精度和全局一致性上有明显的改进效果。算法流程图如图1所示，包括以下步骤：
步骤1.利用多线程三维激光雷达获取环境的三维激光点云数据：点云数据通过ROS以消息的形式传输给计算机；使用开源的PCL函数库将其表示为无序的散列坐标点；再根据雷达圆周式分层的扫描方式，对散列的点云坐标点进行分层排序，同时对于每一层的坐标点，按照其时间戳的大小进行排序；
步骤2.提取特征点：根据点周围局部曲率将当前时刻扫描到的点云区分为角点和平面点；任一点的曲率由该点周围的点与该点形成的和向量的模表示为
Figure GDA0002999042190000051
同时根据雷达视角与局部点云的相对关系剔除断点和孤立点，其中断点为被遮挡物边缘的点和处在与雷达视角近似平行的平面上的点；此外，特征点周围的点云不再进行特征提取；
步骤3.基于特征点的帧间匹配：利用kd-tree最近邻查找算法在上一时刻tk-1的点云Pk-1中寻找距离当前时刻tk的特征点pi最近的特征点，形成特征线与特征面；利用L-M算法求解关于pi到特征线和特征面的距离的最小二乘问题，从而得到相邻时刻的机器人位姿变换矩阵Tk，一系列的位姿变换矩阵累乘即为当前时刻机器人的位姿；
步骤4.特征点与地图的匹配：利用与步骤3相同的方法，将当前时刻tk提取的特征点pi与已建立好的地图中的特征点云进行匹配，从而对机器人当前时刻的位姿进行校正；根据校正后的位姿将当前时刻的点云从雷达坐标系转换到世界坐标系即为建立的地图；其中位姿序列将作为图优化位姿图的节点存储在后端形成全局位姿轨迹；得到的轨迹和点云地图的效果图如图2所示；
步骤5.点云分割：同时，将tk时刻获取到的点云数据通过体素分割和聚类的方法，去除地面点云并分割出地上物体的点云片段；通过点云分割算法得到的点云片段效果图如图3所示；
步骤6.利用特征描述子对点云片段进行描述：利用基于点云片段的三维结构张量矩阵的特征值和特征向量，以及点云片段的形状函数直方图的描述子来描述点云片段的线性、平面性、分散度等特征，并将其作为点云片段的唯一标识进行存储；
其中点云片段的三维结构张量矩阵
Figure GDA0002999042190000052
中的元素描述了点云中的点在三个方向上的变化程度，其特征值λ1、λ2、λ3和特征向量则描述了点云片段的局部特征，包括线性特征Lλ＝e1-e2/e1，平面型特征Pλ＝e2-e3/e1，分散度Sλ＝e3/e1，各向异性Aλ＝e1-e3/e1，全方差
Figure GDA0002999042190000053
矩阵熵Eλ＝-e1ln(e1)-e2ln(e2)-e3ln(e3，曲率变化Cλ＝e3/e1+e2+e3，其中ei为特征值归一化后的结果；形状函数直方图则描述了点云的全局特征，其中D2表示随机选择的点对之间的距离，A3表示随机选择的三个点形成的两条线之间的角度，D3表示随机选择的三个点形成的三角形面积；最终这些特征将用一个特征向量fi统一表示；
步骤7.基于特征描述子的点云片段匹配：通过kd-tree在特征空间中寻找当前点云片段的候选匹配，利用训练好的随机森林分类器为候选匹配打分，再根据设立的阈值和随机抽样一致性检验来确定正确的点云片段配对；根据点云片段的质心计算点云片段间的相对变换关系，以此作为优化的约束条件传入后端图优化步骤；点云片段匹配算法得到的点云配准结果图如图4所示；
步骤8.利用图优化对全局位姿和地图进行修正：根据步骤3、步骤4以及步骤7计算得到的不同时刻的机器人位姿和位姿之间的变换关系构建位姿因子图，其中图中节点表示位姿，边表示位姿之间的约束；通过L-M优化算法优化节点状态，使之满足所有的约束，进而实现对全局的位姿和基于位姿的地图进行修正。
步骤3的具体步骤为：
步骤3-1.将tk-1时刻的点云Pk-1根据求得的位姿变换矩阵Tk-1重投影到tk时刻，按照kd-tree的数据结构存储；
步骤3-2.将tk时刻获得的点云Pk根据步骤2的方法提取出角点Ek和平面点Hk，其中Pk＝{pi,i＝1...N}，Ek＝{εi,i＝1...N1}，Hk＝{δi,i＝1...N2}，N是激光单次扫描得到点云总数；设迭代次数为1，初始值
Figure GDA0002999042190000061
步骤3-3.根据最近邻搜索算法在建立好的kd-tree中搜索εi的最近的两个点形成特征线，搜索δi的最近的三个点形成特征面，根据点到线和点到面的距离公式计算角点到特征线的距离dε，平面点到特征面的距离dδ；
步骤3-4.根据一系列的距离即可得到一组非线性方程，用矩阵表示为
Figure GDA0002999042190000062
其中n为迭代次数；令距离向量d趋于0则可得到一个关于
Figure GDA0002999042190000063
的非线性最小二乘问题，则方程关于变量
Figure GDA0002999042190000064
求导即可得到雅可比矩阵
Figure GDA0002999042190000065
根据L-M算法则
Figure GDA0002999042190000066
迭代次数加一；
步骤3-5.判断当前误差e是否达到预设精度要求，或者迭代次数达到指定次数；如达到要求则进行下一步骤；如未达到要求，返回至步骤3-3；
图5和图6给出了传统的无回环闭合检测和全局后端优化的SLAM算法以及本实施实例算法得到的前2700个位姿形成的轨迹与真实位姿轨迹进行的实验对比。采用无回环闭合检测的SLAM算法得到的全局位姿轨迹对比图如图5所示，以及本实施实例的具有回环闭合检测模块和全局优化后端的SLAM算法得到的全局位姿轨迹对比图如图6所示。通过图5与图6的对比，表明本发明提出的基于点云分割匹配闭环校正的同步定位与构图算法形成的全局位姿轨迹在全局一致性上有更好的表现。
下表给出了传统的无回环闭合检测和全局后端优化的SLAM算法以及本实施实例算法得到的前2700个位姿形成的轨迹与真实位姿轨迹进行的详细误差数据对比。其中绝对误差为相同时间戳位姿的绝对误差值，
Figure GDA0002999042190000067
用来描述全局位姿的全局一致性；相对误差值则用每米的角度漂移来表示，通过在位姿轨迹上间隔1米的选取位姿序列，对比位姿的角度偏差来得到，用来描述局部的位姿精度。无回环闭合检测的传统SLAM算法得到的全局位姿绝对误差曲线如图7所示；本发明实施实例得到的全局位姿轨迹与真实轨迹的绝对误差曲线如图8所示；无回环闭合检测的传统SLAM算法得到的位姿轨迹每米角度漂移曲线如图9所示；本发明实施实例得到的位姿轨迹每米角度漂移曲线如图10所示。
Figure GDA0002999042190000071
通过上述实验对比结果可以看出，本实施实例的基于点云分割匹配闭环校正的SLAM方法得到的位姿轨迹与真实的轨迹的误差最大值为4.47米，均值为1.30米，均方差为1.61米，而无回环闭合检测的传统SLAM算法与真实轨迹的偏差较大，尤其是在出现回环型线路时，由于其无法对已观测到的重复场景进行判断，所以无法修正历史位姿轨迹，偏差尤为明显。通过图7与图8的对比可以表明本实例实施算法较传统SLAM算法在轨迹的全局一致性上有明显的改进。同时本实施实例SLAM算法得到的位姿轨迹与真实的轨迹的每米角度漂移均值为1.23deg，均方差为2.78deg，较传统的SLAM算法有所改进。通过图9与图10对比，图10所示局部漂移误差曲线较稳定，无图9所示较多的离群值，可以表明本实例实施算法较传统SLAM算法的局部漂移误差有所改进，保证了局部定位的精度。实验表明，本发明算法定位的位置误差不超过2％，重复定位精度不超过0.06米。通过图5与图6的效果对比，本实施实例的位姿轨迹与真实轨迹较为贴合，无图5所示具有明显偏离，更能表明本发明提出的基于点云分割匹配闭环校正的同步定位与构图算法形成的全局位姿轨迹在全局一致性和局部定位精度上有更好的表现。
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (2)

1.一种基于点云分割匹配闭环校正的同步定位与构图方法，其特征在于，包括以下步骤：
步骤1.利用多线程三维激光雷达获取环境的三维激光点云数据：点云数据通过机器人操作系统ROS以消息的形式传输给计算机；使用开源的点云处理函数库PCL将其表示为无序的散列坐标点；再根据雷达的扫描方式和时间戳对其进行排序；
步骤2.提取特征点：根据点周围的局部曲率将当前时刻扫描到的点云区分为角点和平面点；任一点的曲率由该点周围的点与该点形成的和向量的模表示；同时根据雷达视角与局部点云的相对关系剔除断点和孤立点；
步骤3.基于特征点的帧间匹配：利用kd-tree最近邻查找算法在上一时刻tk-1的点云Pk-1中寻找距离当前时刻tk的特征点pi最近的特征点，形成特征线与特征面；利用L-M算法求解关于pi到特征线和特征面的距离的最小二乘问题，从而得到相邻时刻的机器人位姿变换矩阵Tk，进而递推式地求得机器人的位姿；
步骤4.特征点与地图的匹配：利用与步骤3相同的方法，将当前时刻tk提取的特征点pi与已建立好的地图中的特征点云进行匹配，从而对机器人当前时刻的位姿进行校正；根据校正后的位姿将当前时刻的点云从雷达坐标系转换到世界坐标系即为建立的地图；
步骤5.点云分割：同时，将tk时刻获取到的点云数据通过体素分割和聚类的方法，去除地面点云并分割出地上物体的点云片段；
步骤6.利用特征描述子对点云片段进行描述：利用基于点云片段的三维结构张量矩阵的特征值和特征向量，以及点云片段的形状直方图的描述子来描述点云片段的线性、平面性、分散度等特征，并将其作为点云片段的唯一标识进行存储；
步骤7.基于特征描述子的点云片段匹配：通过kd-tree在特征空间中寻找当前点云片段的候选匹配，利用训练好的随机森林分类器为候选匹配打分，再根据设立的阈值和随机抽样一致性检验来确定正确的点云片段配对；根据点云片段的质心计算点云片段间的相对变换关系，以此作为优化的约束条件传入后端图优化步骤；
步骤8.利用图优化对全局位姿和地图进行修正：根据步骤3、步骤4以及步骤7计算得到的不同时刻的机器人位姿和位姿之间的变换关系构建位姿因子图，其中图中节点表示位姿，边表示位姿之间的约束；通过L-M优化算法优化节点状态，使之满足所有的约束，进而实现对全局的位姿和基于位姿的地图进行修正。
2.根据权利要求1所述的基于点云分割匹配闭环校正的同步定位与构图方法，其特征在于步骤3包括：
步骤3-1.将tk-1时刻的点云Pk-1根据求得的位姿变换矩阵Tk-1重投影到tk时刻，按照kd-tree的数据结构存储；
步骤3-2.将tk时刻获得的点云Pk根据步骤2的方法提取出角点Ek和平面点Hk，其中Pk＝{pi,i＝1...N}，Ek＝{εi,i＝1...N1}，Hk＝{δi,i＝1...N2}，N是激光单次扫描得到点云总数；设迭代次数为1，初始值
Figure FDA0003147774900000021
步骤3-3.根据最近邻搜索算法在建立好的kd-tree中搜索εi的最近的两个点形成特征线，搜索δi的最近的三个点形成特征面，根据点到线和点到面的距离公式计算角点到特征线的距离dε，平面点到特征面的距离dδ；
步骤3-4.根据一系列的距离即可得到一组非线性方程，用矩阵表示为
Figure FDA0003147774900000022
其中n为迭代次数；令距离向量d趋于0则可得到一个关于
Figure FDA0003147774900000023
的非线性最小二乘问题，则方程关于变量
Figure FDA0003147774900000024
求导即可得到雅可比矩阵
Figure FDA0003147774900000025
根据L-M算法则
Figure FDA0003147774900000026
迭代次数加一；
步骤3-5.判断当前误差e是否达到预设精度要求，或者迭代次数达到指定次数；如达到要求则进行下一步骤；如未达到要求，返回至步骤3-3。
CN201910604487.7A 2019-07-05 2019-07-05 一种基于点云分割匹配闭环校正的同步定位与构图方法 Active CN110689622B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910604487.7A CN110689622B (zh) 2019-07-05 2019-07-05 一种基于点云分割匹配闭环校正的同步定位与构图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910604487.7A CN110689622B (zh) 2019-07-05 2019-07-05 一种基于点云分割匹配闭环校正的同步定位与构图方法

Publications (2)

Publication Number Publication Date
CN110689622A CN110689622A (zh) 2020-01-14
CN110689622B true CN110689622B (zh) 2021-08-27

Family

ID=69108298

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910604487.7A Active CN110689622B (zh) 2019-07-05 2019-07-05 一种基于点云分割匹配闭环校正的同步定位与构图方法

Country Status (1)

Country Link
CN (1) CN110689622B (zh)

Families Citing this family (28)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111402332B (zh) * 2020-03-10 2023-08-18 兰剑智能科技股份有限公司 基于slam的agv复合建图与导航定位方法及系统
CN111736540B (zh) * 2020-03-20 2021-10-15 北京京东乾石科技有限公司 货物分拣控制方法、装置、电子设备及存储介质
CN111461981B (zh) * 2020-03-30 2023-09-01 北京百度网讯科技有限公司 点云拼接算法的误差估计方法和装置
CN113552586B (zh) * 2020-04-08 2024-04-05 杭州萤石软件有限公司 一种移动机器人的定位方法和移动机器人
CN111540005B (zh) * 2020-04-21 2022-10-18 南京理工大学 基于二维栅格地图的回环检测方法
CN111708047B (zh) * 2020-06-16 2023-02-28 浙江华睿科技股份有限公司 机器人定位评估方法、机器人及计算机存储介质
CN111784835B (zh) * 2020-06-28 2024-04-12 北京百度网讯科技有限公司 制图方法、装置、电子设备及可读存储介质
GB202010245D0 (en) * 2020-07-03 2020-08-19 Five Ai Ltd Lidar Mapping
CN111862162B (zh) * 2020-07-31 2021-06-11 湖北亿咖通科技有限公司 回环检测方法及系统、可读存储介质、电子设备
CN111736137B (zh) * 2020-08-06 2020-11-27 广州汽车集团股份有限公司 LiDAR外部参数标定方法、系统、计算机设备及可读存储介质
CN112214019B (zh) * 2020-09-21 2023-05-23 国网浙江省电力有限公司 一种无人巡检设备无盲区智能反馈控制系统、方法、终端
CN112365537B (zh) * 2020-10-13 2023-06-27 天津大学 一种基于三维点云对齐的主动相机重定位方法
CN112184906B (zh) * 2020-10-22 2023-10-03 北京爱笔科技有限公司 一种三维模型的构建方法及装置
CN112348893B (zh) * 2020-10-30 2021-11-19 珠海一微半导体股份有限公司 一种局部点云地图构建方法及视觉机器人
CN113761091B (zh) * 2020-11-27 2024-04-05 北京京东乾石科技有限公司 闭环检测方法、装置、电子设备、系统和存储介质
CN112581519B (zh) * 2020-12-21 2022-03-22 中广核工程有限公司 一种放射性废物包识别定位方法与装置
CN112325873B (zh) * 2021-01-04 2021-04-06 炬星科技（深圳）有限公司 一种环境地图自主更新方法、设备及计算机可读存储介质
CN112927269A (zh) * 2021-03-26 2021-06-08 深圳市无限动力发展有限公司 基于环境语义的地图构建方法、装置和计算机设备
CN113340295B (zh) * 2021-06-16 2021-12-21 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN113340296B (zh) * 2021-06-21 2024-04-09 上海仙工智能科技有限公司 一种自动更新移动机器人地图的方法及装置
CN113503883B (zh) * 2021-06-22 2022-07-19 北京三快在线科技有限公司 采集用于构建地图的数据的方法、存储介质及电子设备
CN113379915B (zh) * 2021-07-05 2022-12-23 广东工业大学 一种基于点云融合的行车场景构建方法
CN113674411B (zh) * 2021-07-29 2024-06-07 浙江大华技术股份有限公司 基于位姿图调整的建图方法及相关设备
CN113624239B (zh) * 2021-08-11 2024-05-31 火种源码(中山)科技有限公司 基于层级可开关稀疏位姿图优化的激光建图方法及装置
CN114136316A (zh) * 2021-12-01 2022-03-04 珠海一微半导体股份有限公司 基于点云特征点的惯导误差消除方法、芯片及机器人
CN114371716A (zh) * 2022-01-20 2022-04-19 红骐科技(杭州)有限公司 一种用于消防机器人的自动驾驶巡检方法
CN114353807B (zh) * 2022-03-21 2022-08-12 沈阳吕尚科技有限公司 一种机器人的定位方法及定位装置
CN114842084B (zh) * 2022-07-04 2022-09-30 矿冶科技集团有限公司 一种地图构建方法、装置及移动探测设备

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106033653A (zh) * 2015-03-12 2016-10-19 襄阳翠鸟视图科技有限公司 一种基于地面激光扫描的三维室内地图制作方法
CN108320329A (zh) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 一种基于3d激光的3d地图创建方法
EP3474230A1 (en) * 2017-10-18 2019-04-24 Tata Consultancy Services Limited Systems and methods for edge points based monocular visual slam
CN109782902A (zh) * 2018-12-17 2019-05-21 中国科学院深圳先进技术研究院 一种操作提示方法及眼镜

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108460779B (zh) * 2018-02-12 2021-09-24 浙江大学 一种动态环境下的移动机器人图像视觉定位方法
CN109522832B (zh) * 2018-11-06 2021-10-26 浙江工业大学 基于点云片段匹配约束和轨迹漂移优化的回环检测方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106033653A (zh) * 2015-03-12 2016-10-19 襄阳翠鸟视图科技有限公司 一种基于地面激光扫描的三维室内地图制作方法
EP3474230A1 (en) * 2017-10-18 2019-04-24 Tata Consultancy Services Limited Systems and methods for edge points based monocular visual slam
CN108320329A (zh) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 一种基于3d激光的3d地图创建方法
CN109782902A (zh) * 2018-12-17 2019-05-21 中国科学院深圳先进技术研究院 一种操作提示方法及眼镜

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LOAM: Lidar Odometry and Mapping in Real-time;J. Zhang等;《Proceedings of Robotics: Science and Systems》;20141230;2-9 *
Segmatch: Segment based place recognition in 3d point clouds;R. Dubé等;《roceedings of IEEE International Conference on Robotics and Automation》;20171231;5266-5272 *
基于点线综合特征的双目视觉SLAM方法;谢晓佳;《中国优秀硕士学位论文全文数据库信息科技辑》;20170815(第8期);I138-405 *

Also Published As

Publication number Publication date
CN110689622A (zh) 2020-01-14

Similar Documents

Publication Publication Date Title
CN110689622B (zh) 一种基于点云分割匹配闭环校正的同步定位与构图方法
CN109186606B (zh) 一种基于slam和图像信息的机器人构图及导航方法
Cieslewski et al. Point cloud descriptors for place recognition using sparse visual information
WO2023184968A1 (zh) 一种基于点线面特征的结构化场景视觉slam方法
US10288425B2 (en) Generation of map data
CN107862735B (zh) 一种基于结构信息的rgbd三维场景重建方法
CN107917710B (zh) 一种基于单线激光的室内实时定位与三维地图构建方法
Ji et al. Lloam: Lidar odometry and mapping with loop-closure detection based correction
CN104040590A (zh) 用于估计物体的姿态的方法
CN105139379A (zh) 基于分类分层的机载Lidar点云建筑物顶面渐进提取方法
Proença et al. Fast cylinder and plane extraction from depth cameras for visual odometry
CN110570474B (zh) 一种深度相机的位姿估计方法及系统
CN116449384A (zh) 基于固态激光雷达的雷达惯性紧耦合定位建图方法
Shu et al. 3D point cloud-based indoor mobile robot in 6-DoF pose localization using a Wi-Fi-aided localization system
Zhang et al. Accurate real-time SLAM based on two-step registration and multimodal loop detection
Ma et al. RoLM: Radar on LiDAR map localization
Zhu Binocular vision-slam using improved sift algorithm
Zhang et al. Point-plane SLAM based on line-based plane segmentation approach
Wang et al. Monocular visual-inertial localization in a point cloud map using feature-to-distribution registration
CN114782689A (zh) 一种基于多帧数据融合的点云平面分割方法
CN111239761B (zh) 一种用于室内实时建立二维地图的方法
Dai et al. An intensity-enhanced LiDAR SLAM for unstructured environments
Huang et al. Research on slam improvement method based on cartographer
Chen et al. Multi-robot point cloud map fusion algorithm based on visual SLAM
Dong et al. R-LIOM: reflectivity-aware LiDAR-inertial odometry and mapping

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