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

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

Info

Publication number
CN109345574B
CN109345574B CN201811006637.6A CN201811006637A CN109345574B CN 109345574 B CN109345574 B CN 109345574B CN 201811006637 A CN201811006637 A CN 201811006637A CN 109345574 B CN109345574 B CN 109345574B
Authority
CN
China
Prior art keywords
point cloud
point
coordinate system
radar
points
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
CN201811006637.6A
Other languages
English (en)
Other versions
CN109345574A (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

Images

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

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轴正负方向所有与其相邻的距离中最近的正负两个点之间距离和的平均值，提取点云中任意两点间距离等于平均值的点，将提取的所有点组成规范点云。
所述的点距离公式如下：
Figure BDA0001784121200000051
其中，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个点之间的距离。
所述的平面垂直线段方程如下：
Figure BDA0001784121200000061
其中，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＝(sk-(R×uk+τ))T(sk-(R×uk+τ))
其中，f表示非线性优化目标函数，sk表示当前时刻第k个配准点云对在世界坐标系中z轴的坐标值，R表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的旋转矩阵,uk表示上一时刻第k个配准点云对在雷达坐标系中x轴的坐标值，τ表示当前时刻从雷达坐标系到世界坐标系转换在的激光雷达运动位姿的平移矩阵，T表示转置操作；
(4b)使用高斯牛顿公式，计算当前时刻激光雷达的运动位姿；
(4c)用上一时刻的雷达坐标系中点云中每一点的坐标值与更新后的旋转矩阵相乘，用其乘积与更新后的平移矩阵相加，得到当前时刻的点在世界坐标系中的坐标值，将当前时刻的所有点组成三维点云图。
2.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(1b)中所述的余弦公式如下：
al＝bl×cosθl
其中，al表示点云中第l个点在雷达坐标系的三个轴中坐标值对应轴的坐标值，bl表示点云中第l个点到原点的距离，cos表示取余弦操作，θl表示点云中第l个点和原点的连线与雷达坐标系的三个轴中对应轴的夹角。
3.根据权利要求1所述的基于语义点云配准的激光雷达三维建图方法，其特征在于，步骤(2a)中所述的点距离公式如下：
Figure FDA0002615901180000031
其中，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)中所述的平面垂直线段方程如下：
Figure FDA0002615901180000041
其中，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(sk-(R×uk+τ))]
其中，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 CN109345574A (zh) 2019-02-15
CN109345574B true 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)

Families Citing this family (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110068824B (zh) * 2019-04-17 2021-07-23 北京地平线机器人技术研发有限公司 一种传感器位姿确定方法和装置
CN111935642B (zh) * 2019-05-13 2022-10-21 北京地平线机器人技术研发有限公司 一种可移动设备的定位方法及装置
CN110316376A (zh) * 2019-07-03 2019-10-11 安徽理工大学 一种用于探测矿井火灾的无人机
CN110333495A (zh) * 2019-07-03 2019-10-15 深圳市杉川机器人有限公司 利用激光slam在长走廊建图的方法、装置、系统、存储介质
CN110162098A (zh) * 2019-07-03 2019-08-23 安徽理工大学 一种矿用无人机
CN110533769B (zh) * 2019-08-20 2023-06-02 福建捷宇电脑科技有限公司 一种翻开书本图像的平整化方法及终端
CN112578363B (zh) * 2019-09-30 2022-08-19 上海禾赛科技有限公司 激光雷达运动轨迹获取方法及装置、介质
CN111427373B (zh) * 2020-03-24 2023-11-24 上海商汤临港智能科技有限公司 一种位姿确定方法、装置、介质和设备
CN113551678A (zh) * 2020-04-09 2021-10-26 阿里巴巴集团控股有限公司 构建地图的方法、构建高精度地图的方法及移动设备
CN111580089A (zh) * 2020-05-11 2020-08-25 北京小狗智能机器人技术有限公司 一种定位方法和相关装置
CN111612829B (zh) * 2020-06-03 2024-04-09 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、系统、终端和存储介质
CN111710040B (zh) * 2020-06-03 2024-04-09 纵目科技（上海）股份有限公司 一种高精度地图的构建方法、系统、终端和存储介质
CN112130151B (zh) * 2020-10-16 2022-07-08 中国有色金属长沙勘察设计研究院有限公司 一种圆弧合成孔径地基雷达坐标投影快速计算方法
CN112348897A (zh) * 2020-11-30 2021-02-09 上海商汤临港智能科技有限公司 位姿确定方法及装置、电子设备、计算机可读存储介质
CN112347550B (zh) * 2020-12-07 2022-07-15 厦门大学 耦合式室内三维语义建图及建模方法
CN115082562A (zh) * 2021-03-15 2022-09-20 华为技术有限公司 一种外参标定方法、装置、设备、服务器及车载计算设备
CN114413881B (zh) * 2022-01-07 2023-09-01 中国第一汽车股份有限公司 高精矢量地图的构建方法、装置及存储介质
CN116358573B (zh) * 2023-05-31 2023-08-29 小米汽车科技有限公司 地图建立方法、装置、存储介质以及车辆

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
LOAM: lidar odometry and mapping in realtime;Zhang J, Singh S;《Robot Sci Syst Conf》;20140731;第1-9页 *

Also Published As

Publication number Publication date
CN109345574A (zh) 2019-02-15

Similar Documents

Publication Publication Date Title
CN109345574B (zh) 基于语义点云配准的激光雷达三维建图方法
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN110966991B (zh) 一种无控制点下的单幅无人机影像定位方法
Fraundorfer et al. Visual odometry: Part ii: Matching, robustness, optimization, and applications
Lin et al. A fast, complete, point cloud based loop closure for LiDAR odometry and mapping
CN108229416B (zh) 基于语义分割技术的机器人slam方法
CN108225327B (zh) 一种顶标地图的构建与定位方法
CN111028292B (zh) 一种亚像素级图像匹配导航定位方法
Gong et al. A Frustum-based probabilistic framework for 3D object detection by fusion of LiDAR and camera data
CN111046843B (zh) 一种智能驾驶环境下的单目测距方法
CN108830933B (zh) 一种电力杆塔塔身重建方法、系统、介质及设备
WO2021021862A1 (en) Mapping and localization system for autonomous vehicles
Qian et al. Robust visual-lidar simultaneous localization and mapping system for UAV
Quintana et al. Door detection in 3D colored laser scans for autonomous indoor navigation
Wei et al. Automatic coarse registration of point clouds using plane contour shape descriptor and topological graph voting
CN107316328A (zh) 一种基于二维激光扫描仪角点特征的闭环检测方法
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
Lu et al. A lightweight real-time 3D LiDAR SLAM for autonomous vehicles in large-scale urban environment
CN113721254A (zh) 一种基于道路指纹空间关联矩阵的车辆定位方法
CN113239072A (zh) 一种终端设备定位方法及其相关设备
Zhang et al. Lidar odometry and mapping based on two-stage feature extraction
Wong et al. Monocular localization within sparse voxel maps
CN113554705B (zh) 一种变化场景下的激光雷达鲁棒定位方法
CN111275748B (zh) 动态环境下基于激光雷达的点云配准方法
CN114677435A (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