CN109345574A - 基于语义点云配准的激光雷达三维建图方法 - Google Patents

基于语义点云配准的激光雷达三维建图方法 Download PDF

Info

Publication number
CN109345574A
CN109345574A CN201811006637.6A CN201811006637A CN109345574A CN 109345574 A CN109345574 A CN 109345574A CN 201811006637 A CN201811006637 A CN 201811006637A CN 109345574 A CN109345574 A CN 109345574A
Authority
CN
China
Prior art keywords
point cloud
point
laser radar
radar
fix system
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.)
Granted
Application number
CN201811006637.6A
Other languages
English (en)
Other versions
CN109345574B (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.)
Xidian University
Original Assignee
Xidian University
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 Xidian University filed Critical Xidian University
Priority to CN201811006637.6A priority Critical patent/CN109345574B/zh
Publication of CN109345574A publication Critical patent/CN109345574A/zh
Application granted granted Critical
Publication of CN109345574B publication Critical patent/CN109345574B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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
    • 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)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Processing Or Creating Images (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

一种基于语义点云配准的激光雷达三维建图方法，其实现步骤为：(1)获取待扫描物体的点云数据；(2)从采集的激光雷达点云数据中提取语义特征点；(3)根据三角形相似原理，对特征点进行基于语义的点云配准；(4)利用配准点云对构建三维点云图。本发明提取的特征点有极点、交点和顶点三种类别，配准过程中误匹配概率降低，匹配的复杂度降低，构建地图实时性提高，利用配准点云对计算激光雷达运动位姿的准确性提高，利用运动位姿纠正点云位置信息，投影到世界坐标系构建的地图更加精准，本发明仅利用激光雷达扫描数据完成在线地绘制高质量的三维点云图，具有精确误匹配所致误差的优点。

Description

基于语义点云配准的激光雷达三维建图方法
技术领域
本发明属于通信技术领域，更进一步涉及激光雷达测绘技术领域中的一种基于语义点云配准的激光雷达三维建图方法。本发明可以应用于移动机器人对地形的测绘和对三维环境的感知，针对已知空间点位置的相邻点云进行配准，从而构建完整的三维点云图。
背景技术
点云配准是激光雷达同步定位与三维建图应用中的关键技术。在点云配准过程中，如果激光雷达本身运动过快，则扫描期间自身造成的运动失真会导致配准错误，从而影响三维点云图的构建。此外，目前提出的点云配准方法复杂度过高，影响构建三维点云图的实时性。
江苏中科院智能科学技术应用研究院在其申请的专利文献“基于三维激光雷达的无人机多场景定位建图方法”(专利申请号：201810597661.5，申请公开号：108303710A)中公开了基于三维激光雷达的无人机多场景定位建图方法。该方法包括以下步骤：将三维激光雷达采集的每一帧点云数据进行提取边缘点和平面点，依据提取的边缘点特征集及平面点特征集采用迭代最近邻算法进行配准，即找到每一个待配准点距它最近的点构成配准点对，并求解相邻两帧的位姿变换矩阵。该方法只通过提取的边缘点和平面点进行配准，存在高概率的误匹配，并且该方法因为在配准时寻找的是距离待配准点最近的点，超出阈值便会存在误差，因此该方法存在的不足之处是，误匹配概率高，在计算激光雷达的运动位姿变换矩阵时存在偏差。
Greenspan M,Yurick M在其发表的论文“Approximate K-D Tree Search forEfficient ICP”(IEEE,2003:442-448)中提出了使用K-D树优化后的最近邻迭代配准方法。该方法在寻找待配准点的对应点时，选择的是K阶最近邻点而不是空间上的最近邻点，减少了点云配准过程中局部优化问题的出现。由于该方法在寻找对应的最近邻点步骤上需要遍历点云中的所有点，导致计算最优的变换矩阵复杂度为O(m3,n3)，对于百万数量级的三维点云来说，这种方法复杂度太高，因此该方法存在的不足之处是，点云配准过程中复杂度太高，对于大规模点云的配准效率难以达到实时三维建图。
发明内容
本发明的目的在于针对上述已有技术的不足，提出了一种基于语义点云配准的激光雷达三维建图方法。
实现本发明目的的思路是，利用激光雷达扫描获得的点云数据，使用余弦定理获得在雷达坐标系中带有坐标信息的点云，将点云投影到世界坐标系中建图；取点云中的极点、交点和顶点作为语义特征点，分别将连续两帧的语义特征点中的任意三点组成三角形，取任意两个三角形，利用三角形相似原理获得配准三角形对，并根据配准三角形对的对应点都满足语义特征点类别提取配准点云对，利用配准点云对计算连续两帧间激光雷达的运动位姿，通过运动位姿更新构建的三维点云图，完成建图。从采集的激光雷达点云数据中提取语义特征点，根据三角形相似原理，对特征点进行基于语义的点云配准，
本发明的具体步骤包括如下：
(1)获取激光雷达点云数据：
(1a)将激光雷达固定到电机控制的旋转平台上，遵循右手定则，以激光雷达当前所在的位置为雷达坐标系的原点，激光雷达的正前方为z轴建立雷达坐标系；
(1b)利用余弦公式，计算激光雷达扫描的每个点在雷达坐标系中每个轴的坐标值，输出带有坐标信息的点云；
(1c)以激光雷达第一帧扫描时雷达坐标系的原点作为世界坐标系的原点，将第一帧带有坐标信息的点云存入世界坐标系中，构建初始三维点云图；
(2)从采集的激光雷达点云数据中提取语义特征点：
(2a)利用点距离公式，计算点云中每个点与雷达坐标系中y轴正负方向所有与其相邻的距离中最近的正负两个点之间距离和的平均值，提取点云中任意两点间距离等于平均值的点，将提取的所有点组成规范点云；
(2b)沿着与雷达坐标系中z轴平行的方向，将规范点云中与z轴平行的每条线上的点组成一条水平线；
(2c)将每条水平线上使柱面方程成立的任意两个点组成一条线段，取每条线段的中心点为极点；将每条水平线上使平面垂直线段方程成立的任意两个点组成一条线段，取每条线段的起点和终点为顶点，取每条线段与其他水平线相交的交点为交点，将所有线段的极点、顶点、交点组成垂直特征点云；
(2d)沿着与雷达坐标系中y轴平行的方向，提取垂直特征点云中和y轴平行的每条线上的点，将每次提取的点组成垂直特征线；
(2e)将每条垂直特征线中极点、顶点、交点最多的一类作为该线的类别；
(2f)提取每条垂直特征线中的起点和终点，将每条垂直特征线的类别作为所提的起点和终点的类别，将所有的起点、终点组成语义特征点云；
(3)根据三角形相似原理，对特征点进行基于语义的点云配准：
(3a)将上一时刻的语义特征点云投影到当前时刻的雷达坐标系中，将投影后的语义特征点云中的任意三个点组成三角形，将上一时刻的所有三角形组成源点云三角形集合；
(3b)将当前时刻的语义特征点云中的任意三个点组成三角形，将当前时刻的所有三角形组成生长点云三角形集合；
(3c)从生长点云三角形集合中任意提取一个三角形，从源点云三角形集合中任意提取两个三角形；
(3d)分别计算每次提取的从生长点云三角形集合中提取的一个三角形的三个顶点与源点云三角形集合中提取的两个三角形的三个顶点的对应距离，将两个距离分别求和后相除，得到商值；
(3e)判断是否提取完生长点云三角形集合中的所有三角形，若是，则执行步骤(3f)；否则，执行步骤(3c)；
(3f)提取所有商值中大于等于阈值的商值，将每个商值对应的生长点云三角形集合中提取的一个三角形，及其源点云三角形集合中提取的两个三角形中对应距离小的三角形组成一个三角形对；
(3g)提取使每个三角形对的三个对应顶点类别都相同的每个对应顶点作为配准点云对；
(4)利用配准点云对构建三维点云图：
(4a)构建非线性优化目标函数如下：
f＝(zk-(R×xk+τ))T(zk-(R×xk+τ))
其中，f表示非线性优化目标函数，zk表示当前时刻第k个配准点云对在世界坐标系中的坐标值，R表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的旋转矩阵,xk表示上一时刻第k个配准点云对在雷达坐标系中的坐标值，τ表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的平移矩阵，T表示转置操作；
(4b)使用高斯牛顿公式，计算当前时刻激光雷达的运动位姿；
(4c)用上一时刻的雷达坐标系中点云中每一点的坐标值与更新后的旋转矩阵相乘，用其乘积与更新后的平移矩阵相加，得到当前时刻的点在世界坐标系中的坐标值，将当前时刻的所有点组成三维点云图。
本发明与现有技术相比具有如下优点：
第一，由于本发明从采集的激光雷达点云数据中提取语义特征点时，提取了极点、交点和顶点三种，克服了现有技术点云配准时，只提取边缘点和平面点进行配准，误匹配概率较大的问题，使得本发明点云配准方法可以更好的配准点云，提高了配准结果可靠性。
第二，由于本发明根据三角形相似原理，对特征点进行基于语义的点云配准时，仅使用三角形相似原理配准，克服了现有技术点云配准时，采集的样本点较多，计算的变换矩阵复杂度太高的问题，使得本发明在点云配准过程中配准复杂度低于现有方法，提高了配准的效率。
第三，由于本发明根据三角形相似原理，对特征点进行基于语义的点云配准时，利用语义特征点的类别作为配准点云的约束条件，克服了现有技术点云配准时，使用配准点对计算的变换矩阵受误差影响大的问题，使得本发明在点云配准过程中误差项影响小于现有方法，提高了配准结果的精确性。
附图说明
图1是本发明的流程图；
图2是本发明提取点云的语义特征点步骤的示意图；
图3是本发明仿真一的仿真图；
图4是本发明仿真二的仿真图。
具体实施方式
下面结合附图对本发明作进一步的描述。
参照图1，对本发明的具体步骤作进一步的描述。
步骤1，获取激光雷达点云数据。
将激光雷达固定到电机控制的旋转平台上，遵循右手定则，以激光雷达当前所在的位置为雷达坐标系的原点，激光雷达的正前方为z轴建立雷达坐标系。
利用余弦公式，计算激光雷达扫描的每个点在雷达坐标系中每个轴的坐标值，输出带有坐标信息的点云。
所述的余弦公式如下：
al＝bl×cosθl
其中，al表示点云中第l个点在雷达坐标系的三个轴中坐标值对应轴的坐标值，bl表示点云中第l个点到原点的距离，cos表示取余弦操作，θl表示点云中第l个点和原点的连线与雷达坐标系的三个轴中对应轴的夹角。
以激光雷达第一帧扫描时雷达坐标系的原点作为世界坐标系的原点，将第一帧带有坐标信息的点云存入世界坐标系中，构建初始三维点云图。
步骤2，从采集的激光雷达点云数据中提取语义特征点。
利用点距离公式，计算点云中每个点与雷达坐标系中y轴正负方向所有与其相邻的距离中最近的正负两个点之间距离和的平均值，提取点云中任意两点间距离等于平均值的点，将提取的所有点组成规范点云。
所述的点距离公式如下：
其中，hi表示点云中第i个点与在雷达坐标系中y轴正负方向上所有与其相邻的距离中最近的正负两个点之间距离和的平均值，(xi,yi,zi)表示点云中第i个点在雷达坐标系中的坐标值，(xi,yi+1,zi)、(xi,yi-1,zi)分别表示在雷达坐标系中y轴正负方向上所有与其相邻的距离中最近的两个点在雷达坐标系中的坐标值。
沿着与雷达坐标系中z轴平行的方向，将规范点云中与z轴平行的每条线上的点组成一条水平线。
将每条水平线上使柱面方程成立的任意两个点组成一条线段，取每条线段的中心点为极点；将每条水平线上使平面垂直线段方程成立的任意两个点组成一条线段，取每条线段的起点和终点为顶点，取每条线段与其他水平线相交的交点为交点，将所有线段的极点、顶点、交点组成垂直特征点云。
所述的柱面方程如下：
||(P-Q)×Ca||-r＝0
其中，||·||表示取距离操作，P表示水平线中第p个点在雷达坐标系中的坐标值，Q表示水平线中第q个点在雷达坐标系中的坐标值，Ca表示雷达坐标系中y轴方向的单位长度坐标值；r表示在雷达坐标系中水平线中第p个点与第q个点之间的距离。
所述的平面垂直线段方程如下：
其中，xm表示水平线中第m个点在雷达坐标系中x轴的坐标值，xn表示水平线中第n个点在雷达坐标系中x轴的坐标值，a表示水平线在雷达坐标系中x轴方向上的方向分量；ym表示水平线中第m个点在雷达坐标系中y轴的坐标值，yn表示水平线中第n个点在雷达坐标系中y轴的坐标值，b表示水平线在雷达坐标系中y轴方向上的方向分量；zm表示水平线中第m个点在雷达坐标系中z轴的坐标值，zn表示水平线中第n个点在雷达坐标系中z轴的坐标值，c表示水平线在雷达坐标系中z轴方向上的方向分量。
沿着与雷达坐标系中y轴平行的方向，提取垂直特征点云中和y轴平行的每条线上的点，将每次提取的点组成垂直特征线。
将每条垂直特征线中极点、顶点、交点最多的一类作为该线的类别。
提取每条垂直特征线中的起点和终点，将每条垂直特征线的类别作为所提的起点和终点的类别，将所有的起点、终点组成语义特征点云。
下面结合图2对本步骤做进一步的说明
图2(a)是本发明提取的语义特征点示意图，图2(b)是本发明的特征垂直线示意图。
图2(a)中最底端矩形框内部的点为极点；最底端矩形框内部整体形状如圆柱，使柱面方程成立的任意两个点，一个分布在圆柱轴上，另一个分布在圆柱体表面，将两点连成的线段取中点便是极点。
图2(a)中最右端矩形框内部的点为交点；使平面垂直线段方程成立的任意两个点都位于水平线上，取水平线的顶点与其他水平线相交的交点便是交点。
图2(a)中最左端矩形框内部的点为顶点，使平面垂直线段方程成立的任意两个点都位于水平线上，取水平线中与其他水平线不相交的顶点便是顶点。
图2(b)中的每一条特征垂直线都是沿着与雷达坐标系中y轴平行的方向，提取图2(a)中和y轴平行的每条线上的点，将每次提取的点组成垂直特征线。
步骤3，根据三角形相似原理，对特征点进行基于语义的点云配准。
(3.1)将上一时刻的语义特征点云投影到当前时刻的雷达坐标系中，将投影后的语义特征点云中的任意三个点组成三角形，将上一时刻的所有三角形组成源点云三角形集合。
(3.2)将当前时刻的语义特征点云中的任意三个点组成三角形，将当前时刻的所有三角形组成生长点云三角形集合。
(3.3)从生长点云三角形集合中任意提取一个三角形，从源点云三角形集合中任意提取两个三角形。
(3.4)分别计算每次提取的从生长点云三角形集合中提取的一个三角形的三个顶点与源点云三角形集合中提取的两个三角形的三个顶点的对应距离，将两个距离分别求和后相除，得到商值。
(3.5)判断是否提取完生长点云三角形集合中的所有三角形，若是，则执行步骤(3.6)；否则，执行步骤(3.3)。
(3.6)提取所有商值中大于等于阈值的商值，将每个商值对应的生长点云三角形集合中提取的一个三角形，及其源点云三角形集合中提取的两个三角形中对应距离小的三角形组成一个三角形对。所述的阈值是指在小于等于1的范围内取的一个任意值。
(3.7)提取使每个三角形对的三个对应顶点类别都相同的每个对应顶点作为配准点云对。
步骤4，利用配准点云对构建三维点云图。
构建非线性优化目标函数如下：
f＝(zk-(R×xk+τ))T(zk-(R×xk+τ))
其中，f表示非线性优化目标函数，zk表示当前时刻第k个配准点云对在世界坐标系中的坐标值，R表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的旋转矩阵,xk表示上一时刻第k个配准点云对在雷达坐标系中的坐标值，τ表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的平移矩阵，T表示转置操作。
所述的激光雷达运动位姿的旋转矩阵是指，在t-1时刻和t时刻对激光雷达运动旋转角度取正余弦操作后获得的3×3矩阵。
所述的激光雷达运动位姿的平移矩阵是指，在t-1时刻和t时刻对激光雷达运动平移量取转置操作后获得的3×1矩阵。
使用高斯牛顿公式，计算当前时刻激光雷达的运动位姿。
所述的高斯牛顿公式如下：
T′←[T-(JT×J)-1JT(zk-(R×xk+τ))]
其中，T′表示当前时刻更新后的激光雷达运动位姿，T表示当前时刻的激光雷达运动位姿，←表示更新操作，J表示第k个配准点云对的对应点误差对上一时刻点的导数，-1表示取逆操作。
用上一时刻的雷达坐标系中点云中每一点的坐标值与更新后的旋转矩阵相乘，用其乘积与更新后的平移矩阵相加，得到当前时刻的点在世界坐标系中的坐标值，将当前时刻的所有点组成三维点云图。
下面结合仿真实验对本发明的效果做进一步的说明
1.仿真条件：
本发明的仿真实验采用激光雷达Hokuyo UTM-30LX、电机Dynamixel MX-28分别通过接口USB连接到笔记本电脑上，在Ubuntu14.04的ROS上进行的。
2.仿真内容与结果分析：
本发明的仿真实验是在室内，对两种不同的场景分别进行三维建图仿真。
仿真实验1：
在长走廊场景下，使用激光雷达构建三维点云图，仿真如图3所示。
图3(a)是长走廊环境实景图，图3(b)是激光雷达扫描时某一帧的生长点云图，图3(c)是激光雷达扫描时当前帧的三维点云图，图3(d)是从后向前观察的激光雷达构建的三维点云图，图3(e)是激光雷达运动位姿图，箭头所指方向代表激光雷达坐标系中x轴方向。
由图3(d)可见，点云中突出部分与周围平整处为内凹的门的轮廓，最前方为激光雷达构建的长走廊的大门。使用三维点云图中长走廊的大门两边点云对应的点坐标相减得到大门的宽度作为测量值，长走廊实景图最前方大门的实际宽度作为实际值。
仿真实验2：
在旋转楼梯场景下，使用激光雷达构建三维点云图，仿真如图4所示。
图4(a)是旋转楼梯实景图，图4(b)是激光雷达扫描时某一帧的生长点云图，图4(c)是激光雷达扫描时当前帧的三维点云图，图4(d)是从后向前观察的三维点云图，图4(e)是激光雷达运动位姿图，箭头所指方向代表激光雷达坐标系中x轴方向。
由图4(d)可见，激光雷达构建的三维点云最底端为楼梯开始处，旋转向上到楼梯结束处。从开始到结束两处的点云对应点坐标取长度得到激光雷达构建的旋转楼梯的楼梯长度作为测量值，旋转楼梯实景图的楼梯实际长度作为实际值。
将仿真实验1和仿真实验2中的测量值与真实值进行比较，如表1所示。
表1.三维点云图中墙壁距离测量值与实际值对比表
测试场景 测量值(cm) 实际值(cm) 相对误差
大门 200.6 203.5 1.4％
走廊 183.2 186.3 1.6％
由表1所示的实验结果可以看出在室内环境中，构建的三维点云图的精度约为2％，说明本发明可以精确估计激光雷达的运动轨迹，并实时地构建高质量的三维点云图。

Claims (9)

1.一种基于语义点云配准的激光雷达三维建图方法，其特征在于，从采集的激光雷达点云数据中提取语义特征点，根据三角形相似原理，对特征点进行基于语义的点云配准，该方法的步骤包括如下：
(1)获取激光雷达点云数据：
(1a)将激光雷达固定到电机控制的旋转平台上，遵循右手定则，以激光雷达当前所在的位置为雷达坐标系的原点，激光雷达的正前方为z轴建立雷达坐标系；
(1b)利用余弦公式，计算激光雷达扫描的每个点在雷达坐标系中每个轴的坐标值，输出带有坐标信息的点云；
(1c)以激光雷达第一帧扫描时雷达坐标系的原点作为世界坐标系的原点，将第一帧带有坐标信息的点云存入世界坐标系中，构建初始三维点云图；
(2)从采集的激光雷达点云数据中提取语义特征点：
(2a)利用点距离公式，计算点云中每个点与雷达坐标系中y轴正负方向所有与其相邻的距离中最近的正负两个点之间距离和的平均值，提取点云中任意两点间距离等于平均值的点，将提取的所有点组成规范点云；
(2b)沿着与雷达坐标系中z轴平行的方向，将规范点云中与z轴平行的每条线上的点组成一条水平线；
(2c)将每条水平线上使柱面方程成立的任意两个点组成一条线段，取每条线段的中心点为极点；将每条水平线上使平面垂直线段方程成立的任意两个点组成一条线段，取每条线段的起点和终点为顶点，取每条线段与其他水平线相交的交点为交点，将所有线段的极点、顶点、交点组成垂直特征点云；
(2d)沿着与雷达坐标系中y轴平行的方向，提取垂直特征点云中和y轴平行的每条线上的点，将每次提取的点组成垂直特征线；
(2e)将每条垂直特征线中极点、顶点、交点最多的一类作为该线的类别；
(2f)提取每条垂直特征线中的起点和终点，将每条垂直特征线的类别作为所提的起点和终点的类别，将所有的起点、终点组成语义特征点云；
(3)根据三角形相似原理，对特征点进行基于语义的点云配准：
(3a)将上一时刻的语义特征点云投影到当前时刻的雷达坐标系中，将投影后的语义特征点云中的任意三个点组成三角形，将上一时刻的所有三角形组成源点云三角形集合；
(3b)将当前时刻的语义特征点云中的任意三个点组成三角形，将当前时刻的所有三角形组成生长点云三角形集合；
(3c)从生长点云三角形集合中任意提取一个三角形，从源点云三角形集合中任意提取两个三角形；
(3d)分别计算每次提取的从生长点云三角形集合中提取的一个三角形的三个顶点与源点云三角形集合中提取的两个三角形的三个顶点的对应距离，将两个距离分别求和后相除，得到商值；
(3e)判断是否提取完生长点云三角形集合中的所有三角形，若是，则执行步骤(3f)；否则，执行步骤(3c)；
(3f)提取所有商值中大于等于阈值的商值，将每个商值对应的生长点云三角形集合中提取的一个三角形，及其源点云三角形集合中提取的两个三角形中对应距离小的三角形组成一个三角形对；
(3g)提取使每个三角形对的三个对应顶点类别都相同的每个对应顶点作为配准点云对；
(4)利用配准点云对构建三维点云图：
(4a)构建非线性优化目标函数如下：
f＝(zk-(R×xk+τ))T(zk-(R×xk+τ))
其中，f表示非线性优化目标函数，zk表示当前时刻第k个配准点云对在世界坐标系中的坐标值，R表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的旋转矩阵,xk表示上一时刻第k个配准点云对在雷达坐标系中的坐标值，τ表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的平移矩阵，T表示转置操作；
(4b)使用高斯牛顿公式，计算当前时刻激光雷达的运动位姿；
(4c)用上一时刻的雷达坐标系中点云中每一点的坐标值与更新后的旋转矩阵相乘，用其乘积与更新后的平移矩阵相加，得到当前时刻的点在世界坐标系中的坐标值，将当前时刻的所有点组成三维点云图。
2.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(1b)中所述的余弦公式如下：
al＝bl×cosθl
其中，al表示点云中第l个点在雷达坐标系的三个轴中坐标值对应轴的坐标值，bl表示点云中第l个点到原点的距离，cos表示取余弦操作，θl表示点云中第l个点和原点的连线与雷达坐标系的三个轴中对应轴的夹角。
3.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(2a)中所述的点距离公式如下：
其中，hi表示点云中第i个点与在雷达坐标系中y轴正负方向上所有与其相邻的距离中最近的正负两个点之间距离和的平均值，(xi,yi,zi)表示点云中第i个点在雷达坐标系中的坐标值，(xi,yi+1,zi)、(xi,yi-1,zi)分别表示在雷达坐标系中y轴正负方向上所有与其相邻的距离中最近的两个点在雷达坐标系中的坐标值。
4.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(2c)中所述的柱面方程如下：
||(P-Q)×Ca||-r＝0
其中，||.||表示取距离操作，P表示水平线中第p个点在雷达坐标系中的坐标值，Q表示水平线中第q个点在雷达坐标系中的坐标值，Ca表示雷达坐标系中y轴方向的单位长度坐标值；r表示在雷达坐标系中水平线中第p个点与第q个点之间的距离。
5.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(2c)中所述的平面垂直线段方程如下：
其中，xm表示水平线中第m个点在雷达坐标系中x轴的坐标值，xn表示水平线中第n个点在雷达坐标系中x轴的坐标值，a表示水平线在雷达坐标系中x轴方向上的方向分量；ym表示水平线中第m个点在雷达坐标系中y轴的坐标值，yn表示水平线中第n个点在雷达坐标系中y轴的坐标值，b表示水平线在雷达坐标系中y轴方向上的方向分量；zm表示水平线中第m个点在雷达坐标系中z轴的坐标值，zn表示水平线中第n个点在雷达坐标系中z轴的坐标值，c表示水平线在雷达坐标系中z轴方向上的方向分量。
6.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(3f)中所述的阈值是指在小于等于1的范围内取的一个任意值。
7.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(4a)中所述的激光雷达运动位姿的旋转矩阵是指，在t-1时刻和t时刻对激光雷达运动旋转角度取正余弦操作后获得的3×3矩阵。
8.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(4a)中所述的激光雷达运动位姿的平移矩阵是指，在t-1时刻和t时刻对激光雷达运动平移量取转置操作后获得的3×1矩阵。
9.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(4b)中所述的高斯牛顿公式如下：
T′←[T-(JT×J)-1JT(zk-(R×xk+τ))]
其中，T′表示当前时刻更新后的激光雷达运动位姿，T表示当前时刻的激光雷达运动位姿，←表示更新操作，J表示第k个配准点云对的对应点误差对上一时刻点的导数，-1表示取逆操作。
CN201811006637.6A 2018-08-31 2018-08-31 基于语义点云配准的激光雷达三维建图方法 Active CN109345574B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811006637.6A CN109345574B (zh) 2018-08-31 2018-08-31 基于语义点云配准的激光雷达三维建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811006637.6A CN109345574B (zh) 2018-08-31 2018-08-31 基于语义点云配准的激光雷达三维建图方法

Publications (2)

Publication Number Publication Date
CN109345574A true CN109345574A (zh) 2019-02-15
CN109345574B CN109345574B (zh) 2020-10-09

Family

ID=65294022

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811006637.6A Active CN109345574B (zh) 2018-08-31 2018-08-31 基于语义点云配准的激光雷达三维建图方法

Country Status (1)

Country Link
CN (1) CN109345574B (zh)

Cited By (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110068824A (zh) * 2019-04-17 2019-07-30 北京地平线机器人技术研发有限公司 一种传感器位姿确定方法和装置
CN110162098A (zh) * 2019-07-03 2019-08-23 安徽理工大学 一种矿用无人机
CN110316376A (zh) * 2019-07-03 2019-10-11 安徽理工大学 一种用于探测矿井火灾的无人机
CN110333495A (zh) * 2019-07-03 2019-10-15 深圳市杉川机器人有限公司 利用激光slam在长走廊建图的方法、装置、***、存储介质
CN110533769A (zh) * 2019-08-20 2019-12-03 福建捷宇电脑科技有限公司 一种翻开书本图像的平整化方法及终端
CN111427373A (zh) * 2020-03-24 2020-07-17 上海商汤临港智能科技有限公司 一种位姿确定方法、装置、介质和设备
CN111580089A (zh) * 2020-05-11 2020-08-25 北京小狗智能机器人技术有限公司 一种定位方法和相关装置
CN111612829A (zh) * 2020-06-03 2020-09-01 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、***、终端和存储介质
CN111710040A (zh) * 2020-06-03 2020-09-25 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、***、终端和存储介质
CN111935642A (zh) * 2019-05-13 2020-11-13 北京地平线机器人技术研发有限公司 一种可移动设备的定位方法及装置
CN112130151A (zh) * 2020-10-16 2020-12-25 中国有色金属长沙勘察设计研究院有限公司 一种圆弧合成孔径地基雷达坐标投影快速计算方法
CN112347550A (zh) * 2020-12-07 2021-02-09 厦门大学 耦合式室内三维语义建图及建模方法
CN112348897A (zh) * 2020-11-30 2021-02-09 上海商汤临港智能科技有限公司 位姿确定方法及装置、电子设备、计算机可读存储介质
CN112578363A (zh) * 2019-09-30 2021-03-30 上海禾赛科技股份有限公司 激光雷达运动轨迹获取方法及装置、介质
CN113223062A (zh) * 2021-06-04 2021-08-06 武汉工控仪器仪表有限公司 一种基于角点特征点选取与快速描述子的点云配准方法
CN113551678A (zh) * 2020-04-09 2021-10-26 阿里巴巴集团控股有限公司 构建地图的方法、构建高精度地图的方法及移动设备
CN113819917A (zh) * 2021-09-16 2021-12-21 广西综合交通大数据研究院 自动驾驶路径规划方法、装置、设备及存储介质
CN114067068A (zh) * 2021-11-11 2022-02-18 武汉理工大学重庆研究院 一种环境建图方法、装置、设备及存储介质
CN114413881A (zh) * 2022-01-07 2022-04-29 中国第一汽车股份有限公司 高精矢量地图的构建方法、装置及存储介质
WO2022194110A1 (zh) * 2021-03-15 2022-09-22 华为技术有限公司 一种外参标定方法、装置、设备、服务器及车载计算设备
CN116358573A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 地图建立方法、装置、存储介质以及车辆
CN113551678B (zh) * 2020-04-09 2024-07-30 浙江菜鸟供应链管理有限公司 构建地图的方法、构建高精度地图的方法及移动设备

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102103202A (zh) * 2010-12-01 2011-06-22 武汉大学 一种融合影像的机载激光雷达数据半监督分类方法
CN103822616A (zh) * 2014-03-18 2014-05-28 武汉大学 一种图分割与地形起伏约束相结合的遥感影像匹配方法
US9183631B2 (en) * 2012-06-29 2015-11-10 Mitsubishi Electric Research Laboratories, Inc. Method for registering points and planes of 3D data in multiple coordinate systems
CN105427317A (zh) * 2015-11-25 2016-03-23 武汉大学 一种适用于多视角自动化配准多站地面激光点云数据的方法
CN105469103A (zh) * 2014-09-11 2016-04-06 清华大学 基于低质量grb-d数据的场景恢复方法及装置
CN107564012A (zh) * 2017-08-01 2018-01-09 中国科学院自动化研究所 面向未知环境的增强现实方法及装置
CN107992829A (zh) * 2017-12-05 2018-05-04 武汉中海庭数据技术有限公司 一种交通信号灯车道级控制关系提取方法及装置

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102103202A (zh) * 2010-12-01 2011-06-22 武汉大学 一种融合影像的机载激光雷达数据半监督分类方法
US9183631B2 (en) * 2012-06-29 2015-11-10 Mitsubishi Electric Research Laboratories, Inc. Method for registering points and planes of 3D data in multiple coordinate systems
CN103822616A (zh) * 2014-03-18 2014-05-28 武汉大学 一种图分割与地形起伏约束相结合的遥感影像匹配方法
CN105469103A (zh) * 2014-09-11 2016-04-06 清华大学 基于低质量grb-d数据的场景恢复方法及装置
CN105427317A (zh) * 2015-11-25 2016-03-23 武汉大学 一种适用于多视角自动化配准多站地面激光点云数据的方法
CN107564012A (zh) * 2017-08-01 2018-01-09 中国科学院自动化研究所 面向未知环境的增强现实方法及装置
CN107992829A (zh) * 2017-12-05 2018-05-04 武汉中海庭数据技术有限公司 一种交通信号灯车道级控制关系提取方法及装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
ZHANG J, SINGH S: "LOAM: lidar odometry and mapping in realtime", 《ROBOT SCI SYST CONF》 *

Cited By (32)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110068824B (zh) * 2019-04-17 2021-07-23 北京地平线机器人技术研发有限公司 一种传感器位姿确定方法和装置
CN110068824A (zh) * 2019-04-17 2019-07-30 北京地平线机器人技术研发有限公司 一种传感器位姿确定方法和装置
CN111935642A (zh) * 2019-05-13 2020-11-13 北京地平线机器人技术研发有限公司 一种可移动设备的定位方法及装置
CN110333495A (zh) * 2019-07-03 2019-10-15 深圳市杉川机器人有限公司 利用激光slam在长走廊建图的方法、装置、***、存储介质
CN110316376A (zh) * 2019-07-03 2019-10-11 安徽理工大学 一种用于探测矿井火灾的无人机
CN110162098A (zh) * 2019-07-03 2019-08-23 安徽理工大学 一种矿用无人机
CN110533769A (zh) * 2019-08-20 2019-12-03 福建捷宇电脑科技有限公司 一种翻开书本图像的平整化方法及终端
CN110533769B (zh) * 2019-08-20 2023-06-02 福建捷宇电脑科技有限公司 一种翻开书本图像的平整化方法及终端
CN112578363A (zh) * 2019-09-30 2021-03-30 上海禾赛科技股份有限公司 激光雷达运动轨迹获取方法及装置、介质
CN111427373A (zh) * 2020-03-24 2020-07-17 上海商汤临港智能科技有限公司 一种位姿确定方法、装置、介质和设备
CN111427373B (zh) * 2020-03-24 2023-11-24 上海商汤临港智能科技有限公司 一种位姿确定方法、装置、介质和设备
CN113551678B (zh) * 2020-04-09 2024-07-30 浙江菜鸟供应链管理有限公司 构建地图的方法、构建高精度地图的方法及移动设备
CN113551678A (zh) * 2020-04-09 2021-10-26 阿里巴巴集团控股有限公司 构建地图的方法、构建高精度地图的方法及移动设备
CN111580089A (zh) * 2020-05-11 2020-08-25 北京小狗智能机器人技术有限公司 一种定位方法和相关装置
CN111710040A (zh) * 2020-06-03 2020-09-25 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、***、终端和存储介质
CN111612829A (zh) * 2020-06-03 2020-09-01 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、***、终端和存储介质
CN111612829B (zh) * 2020-06-03 2024-04-09 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、***、终端和存储介质
CN111710040B (zh) * 2020-06-03 2024-04-09 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、***、终端和存储介质
CN112130151A (zh) * 2020-10-16 2020-12-25 中国有色金属长沙勘察设计研究院有限公司 一种圆弧合成孔径地基雷达坐标投影快速计算方法
CN112130151B (zh) * 2020-10-16 2022-07-08 中国有色金属长沙勘察设计研究院有限公司 一种圆弧合成孔径地基雷达坐标投影快速计算方法
CN112348897A (zh) * 2020-11-30 2021-02-09 上海商汤临港智能科技有限公司 位姿确定方法及装置、电子设备、计算机可读存储介质
CN112347550A (zh) * 2020-12-07 2021-02-09 厦门大学 耦合式室内三维语义建图及建模方法
CN112347550B (zh) * 2020-12-07 2022-07-15 厦门大学 耦合式室内三维语义建图及建模方法
WO2022194110A1 (zh) * 2021-03-15 2022-09-22 华为技术有限公司 一种外参标定方法、装置、设备、服务器及车载计算设备
CN113223062A (zh) * 2021-06-04 2021-08-06 武汉工控仪器仪表有限公司 一种基于角点特征点选取与快速描述子的点云配准方法
CN113223062B (zh) * 2021-06-04 2024-05-07 武汉工控仪器仪表有限公司 一种基于角点特征点选取与快速描述子的点云配准方法
CN113819917A (zh) * 2021-09-16 2021-12-21 广西综合交通大数据研究院 自动驾驶路径规划方法、装置、设备及存储介质
CN114067068A (zh) * 2021-11-11 2022-02-18 武汉理工大学重庆研究院 一种环境建图方法、装置、设备及存储介质
CN114413881B (zh) * 2022-01-07 2023-09-01 中国第一汽车股份有限公司 高精矢量地图的构建方法、装置及存储介质
CN114413881A (zh) * 2022-01-07 2022-04-29 中国第一汽车股份有限公司 高精矢量地图的构建方法、装置及存储介质
CN116358573A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 地图建立方法、装置、存储介质以及车辆
CN116358573B (zh) * 2023-05-31 2023-08-29 小米汽车科技有限公司 地图建立方法、装置、存储介质以及车辆

Also Published As

Publication number Publication date
CN109345574B (zh) 2020-10-09

Similar Documents

Publication Publication Date Title
CN109345574A (zh) 基于语义点云配准的激光雷达三维建图方法
CN111882593B (zh) 结合注意力机制及三维图卷积网络的点云配准模型及方法
CN104484648B (zh) 基于轮廓识别的机器人可变视角障碍物检测方法
CN105389569B (zh) 一种人体姿态估计方法
CN111486855A (zh) 一种具有物体导航点的室内二维语义栅格地图构建方法
CN112785643A (zh) 一种基于机器人平台的室内墙角二维语义地图构建方法
CN111060924B (zh) 一种slam与目标跟踪方法
CN107967463B (zh) 一种基于合成图像和深度学习的虚拟人脸识别方法
CN105636201B (zh) 基于稀疏信号指纹数据库的室内定位方法
CN111862213A (zh) 定位方法及装置、电子设备、计算机可读存储介质
CN101907459A (zh) 基于单目视频的实时三维刚体目标姿态估计与测距方法
CN110675453B (zh) 一种已知场景中运动目标的自定位方法
CN108229416A (zh) 基于语义分割技术的机器人slam方法
CN104517289A (zh) 一种基于混合摄像机的室内场景定位方法
CN107316328A (zh) 一种基于二维激光扫描仪角点特征的闭环检测方法
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN107153831A (zh) 智能终端的定位方法、***及智能终端
CN108680177A (zh) 基于啮齿类动物模型的同步定位与地图构建方法及装置
Shu et al. 3D point cloud-based indoor mobile robot in 6-DoF pose localization using a Wi-Fi-aided localization system
CN107463871A (zh) 一种基于角特征加权的点云匹配方法
US20240169584A1 (en) Neural network-based pose estimation and registration method and device for heterogeneous images, and medium
CN112925322B (zh) 一种长期场景下无人车的自主定位方法
CN109636897A (zh) 一种基于改进RGB-D SLAM的Octomap优化方法
Zhai et al. A³PNet: Antijam and Accurate Antenna Parameters Measuring Network for Mobile Communication Base Station Using UAV
Wada et al. Dataset Genreratoin for Semantic Segmentation from 3D Scanned Data Considering Domain Gap

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