CN112070770A - 一种高精度三维地图与二维栅格地图同步构建方法 - Google Patents
一种高精度三维地图与二维栅格地图同步构建方法 Download PDFInfo
- Publication number
- CN112070770A CN112070770A CN202010686222.9A CN202010686222A CN112070770A CN 112070770 A CN112070770 A CN 112070770A CN 202010686222 A CN202010686222 A CN 202010686222A CN 112070770 A CN112070770 A CN 112070770A
- Authority
- CN
- China
- Prior art keywords
- point
- point cloud
- dimensional
- coordinate system
- 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.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/11—Region-based segmentation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/23—Clustering techniques
-
- 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
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/13—Edge detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/136—Segmentation; Edge detection involving thresholding
-
- 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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Data Mining & Analysis (AREA)
- Software Systems (AREA)
- Geometry (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Biology (AREA)
- Bioinformatics & Computational Biology (AREA)
- Artificial Intelligence (AREA)
- Remote Sensing (AREA)
- Life Sciences & Earth Sciences (AREA)
- Computer Graphics (AREA)
- Image Analysis (AREA)
Abstract
本发明涉及机器人领域,公开了一种高精度三维地图与二维栅格地图同步构建方法,包括以下步骤:有序存储三维激光雷达扫描到的点云数据;分割地面点与非地面点并对非地面点采取聚类处理;在地面点与聚类后点云中提取高度、边缘、平面特征;利用边缘、平面特征点云配准,建立三维点云地图;对世界坐标系中点云进行高度特征提取,建立二维栅格地图。本发明提出了一种实时三维建图方法,并且基于导航机器人高度特征同步构建二维栅格地图,可直接运用于机器人的导航和定位场景中。
Description
技术领域
本发明涉及机器人领域,具体地,涉及一种高精度三维地图与二维栅格地图同步构建方法。
背景技术
随着机器人技术水平的不断提升,机器人逐渐在各行业表现出重要的用途。移动机器人主要的任务是在未知复杂环境中,使用自身携带的传感器感知周围环境,通过传感器对环境的采样建立环境地图,并实现自身定位和导航。因此建立环境地图是移动机器人实现自主导航行动的前提,地图一方面可以帮助机器人配合自身的传感器进行实时精确定位,同时也用于后续导航。
专利号CN201710598881.5可以使用一个一线激光雷达配合一个带有编码器的电机,建立出高精度三维地图。但是这种技术对电机精度要求非常高,所以提高了技术的门槛。另外,该技术所用的算法对计算机的性能要求较高,导致其绘制地图的时间大打折扣。最后,该技术也不能直接生成二维栅格地图。
专利号CN201310363428.8提出的一种栅格地图创建方法也只是基于X-Y二维平面坐标系建立的地图,并未考虑到实际中导航机器人的三维高度,在路径规划后展开后续行动中,很有可能会碰撞到机器人底盘或其他位置。其次,单一的二维栅格地图包含的信息量及其有限,并不能很好的被人类直观的理解。
发明内容
本发明的目的是提供一种高精度三维地图与二维栅格地图同步构建方法,该方法构建的三维地图与二维栅格地图包含信息量丰富,且可直接用于后续导航,在三维点云处理过程中运用了分割聚类,不仅提高了点云特征提取与配准的效率与精度,还降低了计算复杂度,在生成二维栅格地图中也更加全面的考虑到了机器人的三维高度。
为了实现上述目的,本发明提供了一种高精度三维地图与二维栅格地图同步构建方法,包括以下步骤:
建立坐标系以及空间格网,所述坐标系包括世界坐标系以及激光雷达坐标系;
通过三维激光雷达进行实时扫描,获得带有三维坐标和距离信息的点云数据Pt,Pt={p1,p2,…,pi,…,pn},其中pi为所述点云数据Pt中第i个点;
设置虚拟图像,把当前时刻扫描到的所述点云数据Pt有序投影到相应的所述虚拟图像中;
对点云数据Pt进行分割聚类,输出地面点云Gt与聚类点云no_GSt:
分割地面点云Gt与非地面点云no_Gt,并且对代表地面的地面点云Gt予以指定标签标记;
对所述非地面点云no_Gt进行聚类处理,表示同一对象的点云形成一个点簇,给予唯一的标签视为一类,舍弃点云数量小于影响阈值的类别,输出聚类点云no_GSt;
对Gt与no_GSt进行高度、边缘、平面特征提取;
优选地,所述建立坐标系以及空间格网包括:
建立以三维激光雷达为中心的激光雷达坐标系,在三维激光雷达进行扫描时的最初位置建立世界坐标系,各轴方向与所述激光雷达坐标系相同;
基于所述世界坐标系建立空间格网,根据所需栅格地图分辨率,设置小立方体。
优选地,所述分割地面点云Gt与非地面点云no_Gt,并且对代表地面的地面点云Gt予以指定标签标记包括:
根据公式(1)判断所述虚拟图像第i行j列像素表示的点是否为地面点,
其中,Ptr[j+i×k],表示所述虚拟图像第i行j列的像素,则Ptr[j+i×k].x、Ptr[j+i×k].y、Ptr[j+i×k].z表示该点在激光雷达坐标系下的三维坐标,其中,k表示总列数;
dz=Ptr[j+(i+1)×k].z-Ptr[j+i×k].z,
dx=Ptr[j+(i+1)×k].x-Ptr[j+i×k].x,
dy=Ptr[j+(i+1)×k].y-Ptr[j+i×k].y,
判断angle是否小于地面分割阈值:
若是,则该像素表示的点与对应下一行的点,即第i+1行j列像素表示的点,均视为地面点,且予以指定标签标记;
若否,则视为非地面点,不予标记。
优选地,所述三维激光雷达水平方向测量角度范围为0°到360°,垂直方向测量角度范围为-15°到15°,其水平角分辨率为0.2°,垂直角分辨率为2°。
优选地,所述对所述非地面点云no_Gt进行聚类处理,表示同一对象的点云形成一个点簇,给予唯一的标签视为一类包括:
遍历no_Gt中每一个点,对未聚类处理的点进行广度优先搜索,得到在图像中当前点的上下左右四个邻点;
根据公式(2)计算A、B两点的角度β的值,
其中:dmax=max{A.intensity,B.intensity},
dmin=min{A.intensity,B.intensity},
A.intensity,B.intensity分别表示A点、B点到所述三维激光雷达传感器的欧式距离,α表示两激光束的夹角,A为当前点,B为四个邻点中的任意一个邻点;
判断角度β是否大于预设的聚类角度阈值;
在判断所述角度β大于所述聚类角度阈值的情况下,确定A、B两点表示同一对象;
在A、B两点表示同一对象的情况下,把A、B两点放入同一点簇中以表示同一对象,并给予唯一的标签标记。
优选地,所述对Gt与no_GSt进行高度、边缘、平面特征提取包括:
遍历no_GSt中所有点,在Gt中使用K近邻搜索算法找到与当前点在X-Y平面上距离最近的地面点,计算当前点与所述最近的地面点在Z方向的高度差dz;
判断所述高度差是否位于预设的高度范围[Hmin,Hmax];
在判断所述高度差位于所述高度范围[Hmin,Hmax]的情况下,对当前遍历到的点给予高度特征标记。
优选地,所述对Gt与no_GSt进行高度、边缘、平面特征提取包括:
遍历所述虚拟图像中的每一个点,以当前点为中心点,在中心点所在行中提取中心点左右相邻的连续点组成点集S,根据公式(3)计算所述虚拟图像中的每一个粗糙度rij,
边缘、平面特征提取:把表示0°-360°范围的虚拟图像平均分为6个分别表示60°的子图像,根据所述子图像的粗糙度值rij对每一行的点进行排序;
在子图像中的每一行中提取具有最小粗糙度且属于地面点云Gt的点作为平面特征点,以形成如公式(4)所示的集合,
{P∈Gt|min{P.rij,j∈[colmin,colmax]},i∈[rowmin,rowmax]} (4)
其中P.rij表示点P的粗糙度,min{P.rij,j∈[colmin,colmax]}表示子图像中第i行具有最小粗糙度的点,colmin、colmax分别表示子图像列的最小值、最大值,rowmin、rowmax分别表示子图像行的最小值、最大值;
在子图像中的每一行中提取具有最大粗糙度且属于聚类点云no_GSt的点作为边缘特征点,以形成如公式(5)所示的集合,
{P∈no_GSt|max{P.rij,j∈[colmin,colmax]},i∈[rowmin,rowmax]} (4)
其中P.rij表示点P的粗糙度,max{P.rij,j∈[colmin,colmax]}表示子图像中第i行具有最大粗糙度的点,colmin、colmax分别表示子图像列的最小值、最大值,rowmin、rowmax分别表示子图像行的最小值、最大值。
在当前时刻地面点云Gt中提取到的平面特征点与上一时刻地面点云Gt-1中提取的平面特征点,在当前时刻聚类点云no_GSt中提取的边缘特征点与上一时刻聚类点云no_GSt-1中提取的边缘特征点,使用ICP算法进行点云配准,得到所述三维激光雷达从上一时刻到当前时刻运动的位移变换矩阵T与旋转变换矩阵R;
以第一次扫描为起点利用相邻两次扫描间所述三维激光雷达的位移变换矩阵T与旋转变换矩阵R,得到相对世界坐标系,当前时刻激光雷达的位移变换矩阵与旋转变换矩阵如公式(6)所示,
建立三维地图:将世界坐标系中的点云数据Pt与扫描前的三维地图上的点云Qt-1再次进行点云配准优化,输出点云并加入到扫描前的世界坐标系中点云Qt-1中,得到当前时刻世界坐标系下的三维地图点云Qt,即当前时刻建立的三维地图。
将所述空间格网投影在X-Y二维栅格平面上,根据X-Y二维栅格中的某个栅格在Z方向上是否存在已经标记的小立方体得到当前时刻的局部二维栅格地图,累加每一时刻的局部二维栅格地图,得到当前时刻世界坐标系下的全局二维栅格地图。
所述根据X-Y二维栅格中的某个栅格在Z方向上是否存在已经标记的小立方体得到当前时刻的局部二维栅格地图包括:
如果某个栅格在Z方向上对应的格网中有一个或多个小立方体被标记为占据,则将该栅格标记为占据;如果某个栅格在Z方向上对应的所有小立方体均被标记为未占据,则将栅格标记为未占据。
本发明同步构建三维点云地图与二维栅格地图,不仅包含三维点云地图信息量丰富、易被用户直观理解等优点,还包含二维栅格地图的优点,可以直接运用于后续路径规划中。
本发明在构建地图过程中,应用了分割聚类并且舍弃含有较少点云数量的类别,提高了点云特征提取与配准的处理效率与精度,降低了计算复杂度,实时性能更好。
本发明最终生成的二维栅格地图考虑到了导航机器人的三维高度,比一般的二维栅格地图更加可靠,在后续规划行动中也更加安全。
本发明的其它特征和优点将在随后的具体实施方式部分予以详细说明。
附图说明
附图是用来提供对本发明的进一步理解,并且构成说明书的一部分,与下面的具体实施方式一起用于解释本发明,但并不构成对本发明的限制。在附图中:
图1示出了本发明的一实施方式的高精度三维地图与二维栅格地图同步构建方法的流程图;
图2示出了本发明的一实施方式的表示两个对象的点云聚类示意图;
图3示出了本发明的一实施方式的两点聚类判断示意图;
图4示出了本发明的一实施方式的激光雷达坐标系示意图。
具体实施方式
以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是,此处所描述的具体实施方式仅用于说明和解释本发明,并不用于限制本发明。
图1示出了本发明的一实施方式的高精度三维地图与二维栅格地图同步构建方法的流程图,请参照图1,本发明提出的一种高精度三维地图与二维栅格地图同步构建方法的实现步骤如下:
建立坐标系以及空间格网,所述坐标系包括世界坐标系以及激光雷达坐标系。
在导航机器人上安装固定好16线三维激光雷达,并建立以激光雷达为中心的激光雷达坐标系,如图4所示,我们以Y轴表示正前方,以X轴表示正右方,并遵循右手定则。在激光雷达进行扫描时的最初位置建立世界坐标系,各轴方向与激光雷达坐标系相同。基于世界坐标系建立空间格网,根据所需栅格地图分辨率,设置每格为一定边长的小立方体。在本实施例中小立方体的边长为0.1米。
通过三维激光雷达进行实时扫描,获得带有三维坐标和距离信息的点云Pt,可令其表示为Pt={p1,p2,…,pi,…,pn},其中pi为点云Pt中第i个点,包含有点pi在激光雷达坐标系下的三维坐标与点pi到激光雷达传感器的欧式距离;
设置虚拟图像,把当前时刻扫描到的所述点云数据Pt有序投影到相应的所述虚拟图像中。16线激光雷达水平方向测量角度范围为0-360°,垂直方向测量角度范围为30°(+15°到-15°)。其水平角分辨率为0.2°,垂直角分辨率为2°,则设置图像的行数为16,列数为360÷0.2=1800,这样Pt中每一个点pi都可以用图像中唯一像素表示,直接明确了领域关系,计算效率更高。激光雷达从低到高的16线激光束依次投影到图像0到15行,从Y轴正方向为起点逆时针360°返回的激光点依次投影到图像0到1799列。定义一个点云指针Ptr,用点云指针来表示虚拟图像有序存储点云Pt,即Ptr[j+i×k]表示图像第i行j列的像素,其中k表示总列数,有上述可知,本实施例中的总列数为k=1800。则Ptr[j+i×1800].x、Ptr[j+i×1800].y、Ptr[j+i×1800].z表示该点在激光雷达坐标系下的三维坐标,Ptr[j+i×1800].intensity表示该点到激光雷达传感器的欧式距离。
对点云数据Pt进行分割聚类,输出地面点云Gt与聚类点云no_GSt。
分割地面点云Gt与非地面点云no_Gt,并且对代表地面的地面点云予以指定标签标记。通过公式(1)判断虚拟图像第i行j列像素表示的点是否为地面点:
其中dz=Ptr[j+(i+1)×1800].z-Ptr[j+i×1800].z,
dx=Ptr[j+(i+1)×1800].x-Ptr[j+i×1800].x,
dy=Ptr[j+(i+1)×1800].y-Ptr[j+i×1800].y,
当angle小于地面分割阈值时,优选的10°,则该像素表示的点与对应下一行的点,即第i+1行j列像素表示的点,均视为地面点,反之视为非地面点,且对地面点予以特定标签标记。
对非地面点云no_Gt进行聚类处理,表示同一对象的点云形成一个点簇,给予唯一的标签视为一类,舍弃点云数量小于影响阈值的类别,输出聚类点云no_GSt。这里的影响阈值为根据当前环境得来的,如果机器人在室外嘈杂的环境中工作,小物体例如树叶可能会形成琐碎和不可靠的特征,因为同一片树叶可能在连续两次扫描中不会被看到。为了使用分割的点云进行快速可靠的特征提取,省略了点云数量小于影响阈值的类别,在本实施例中,影响阈值优选为30。
遍历no_Gt中每一个点,对未聚类处理的点进行广度优先搜索,得到在图像中当前点的上下左右四个邻点,判断当前点A与其邻点B是否表示为同一对象。如图2和图3所示,A、B表示两返回的激光点,OA、OB表示相应的激光束,通过公式(2)来计算A、B两点的角度β值:
其中dmax=‖OA‖=A.intensity,dmin=‖OB‖=B.intensity。
正如前述,A.intensity,B.intensity分别表示A、B点到激光雷达传感器的欧式距离,其中α表示两激光束的夹角,当考虑左右邻关系时则α=0.2°,当考虑上下邻关系时则α=2°。
当角度β大于聚类角度阈值,优选的60°,则A、B两点表示同一对象。当前点与其邻点表示同一对象时,把当前点与该邻点放入同一点簇中,以此表示同一对象并给予唯一的标签标记。
对Gt与no_GSt进行高度、边缘、平面特征提取。
高度特征提取:遍历no_GSt中所有点,在Gt中使用K近邻搜索算法找到与当前点在X-Y平面上距离最近的地面点。也就是计算当前点与最近的地面点的平面距离d,其中,dx2为所述地面点和所述当前点在X方向上的距离的平方,dy2为所述地面点和所述当前点在Y方向上的距离的平方,根据平面距离d的大小找出最近的地面点。
计算当前点与地面点在Z方向的高度差dz。考虑导航机器人高度,评估得到可能产生碰撞的高度范围[Hmin,Hmax],若满足如下不等式Hmin≤dz≤Hmax,则对no_GSt中当前遍历到的点给予高度特征标记。
定义粗糙度rij:遍历虚拟图像中的每一个点,以当前点为中心,即为中心点,在中心点所在行中提取中心点左右相邻的连续点组成点集S,以此定义点的粗糙度rij,并通过公式(3)计算图像中的粗糙度rij,
边缘、平面特征提取:把表示0°-360°范围的虚拟图像平均分为6个分别表示60°的子图像,根据所述子图像的粗糙度值rij对每一行的点进行排序;
在子图像中的每一行中提取具有最小粗糙度且属于地面点云Gt的点作为平面特征点,以形成如公式(4)所示的集合,
{P∈Gt|min{P.rij,j∈[colmin,colmax]},i∈[rowmin,rowmax]} (4)
其中P.rij表示点P的粗糙度,min{P.rij,j∈[colmin,colmax]}表示子图像中第i行具有最小粗糙度的点,colmin、colmax分别表示子图像列的最小值、最大值,rowmin、rowmax分别表示子图像行的最小值、最大值;
在子图像中的每一行中提取具有最大粗糙度且属于聚类点云no_GSt的点作为边缘特征点,以形成如公式(5)所示的集合,
{P∈no_GSt|max{P.rij,j∈[colmin,colmax]},i∈[rowmin,rowmax]} (5)
其中P.rij表示点P的粗糙度,max{P.rij,j∈[colmin,colmax]}表示子图像中第i行具有最大粗糙度的点,colmin、colmax分别表示子图像列的最小值、最大值,rowmin、rowmax分别表示子图像行的最小值、最大值。
具体的,把虚拟图像平均分为6个16行300列子图像,根据子图像的粗糙度值对每一行的点进行排序。
在子图像中每一行中提取具有最小粗糙度且属于地面点云Gt的点作为平面特征点。以第一幅子图像为例,平面特征点满足如下集合:
{P∈Gt|min{P.rij,j∈[0,299]},i∈[0,15]}
其中P.rij表示点P的粗糙度,min{P.rij,j∈[0,299]}表示子图像中第i行具有最小粗糙度的点。
在子图像中每一行中提取具有最大粗糙度且属于聚类点云no_GSt的点作为边缘特征点。以第一幅子图像为例,边缘特征点满足如下集合:
其中P.rij表示点P的粗糙度,max{P.rij,j∈[0,299]}表示子图像中第i行具有最大粗糙度的点。
利用边缘、平面特征点云配准,建立三维地图。
以相同标签为候选集找到当前扫描点云与上一时刻扫描点云的对应特征点集,即在当前时刻地面点云Gt中提取到的平面特征点与上一时刻地面点云Gt-1中提取的平面特征点,在当前时刻聚类点云no_GSt中提取的边缘特征点与上一时刻聚类点云no_GSt-1中提取的边缘特征点。结合各自标签并使用ICP算法对不同时刻的特征点进行点云配准,计算得到两次连续扫描之间的激光雷达的位移变换矩阵T与旋转变换矩阵R。
以第一次扫描为起点利用相邻两次扫描之间激光雷达的位移变换矩阵T与旋转变换矩阵R,可以得到相对世界坐标系,当前时刻激光雷达的位移变换矩阵与旋转变换矩阵如公式(6)所示:
建立三维地图:将世界坐标系中的点云数据Pt与扫描前的三维地图上的点云Qt-1再次进行点云配准优化,输出点云并加入到扫描前的世界坐标系中点云Qt-1中,得到当前时刻世界坐标系下的三维地图点云Qt,即三维地图。其中Qt-1为累加的从初始时刻到t-1时刻的扫面点云数据,即t-1时刻建立的三维点云地图,Qt为累加的从初始时刻到t时刻的扫描点云数据,即t时刻建立的三维点云地图。
利用最初建立的空间格网,如果某个小立方体对应的位置处具有点云中的一个或多个点,则可以将该小立方体标记为已占据;相反,如果小立方体对应的位置处不具备任何点,则将该小立方体标记为未占据,由此实现对空间格网中的每个小立方体进行标记。
将世界坐标系下三维空间格网投影在X-Y二维栅格平面上,如果X-Y二维栅格中的某个栅格在Z方向上对应的格网中有一个或多个小立方体被标记为占据,则将该栅格标记为占据;相反若栅格在Z方向上对应的所有小立方体均被标记为未占据,则将栅格标记为未占据。这样得到了当前时刻局部二维栅格地图,累加每一时刻的局部二维栅格地图,得到当前时刻世界坐标系下的全局二维栅格地图。
本发明的三维地图与二维栅格地图包含信息量丰富,且可直接用于后续导航。在三维点云处理过程中运用了分割聚类,不仅提高了点云特征提取与配准的效率与精度,还降低了计算复杂度。在生成二维栅格地图中也更加全面的考虑到了机器人的三维高度。
以上结合附图详细描述了本发明的优选实施方式,但是,本发明并不限于上述实施方式中的具体细节,在本发明的技术构思范围内,可以对本发明的技术方案进行多种简单变型,这些简单变型均属于本发明的保护范围。另外需要说明的是,在上述具体实施方式中所描述的各个具体技术特征,在不矛盾的情况下,可以通过任何合适的方式进行组合,为了避免不必要的重复,本发明对各种可能的组合方式不再另行说明。
此外,本发明的各种不同的实施方式之间也可以进行任意组合,只要其不违背本发明的思想,其同样应当视为本发明所公开的内容。
Claims (10)
1.一种高精度三维地图与二维栅格地图同步构建方法,其特征在于,包括以下步骤:
建立坐标系以及空间格网,所述坐标系包括世界坐标系以及激光雷达坐标系;
通过三维激光雷达进行实时扫描,获得带有三维坐标和距离信息的点云数据Pt,Pt={p1,p2,…,pi,…,pn},其中pi为所述点云数据Pt中第i个点;
设置虚拟图像,把当前时刻扫描到的所述点云数据Pt有序投影到相应的所述虚拟图像中;
对点云数据Pt进行分割聚类,输出地面点云Gt与聚类点云no_GSt:
分割地面点云Gt与非地面点云no_Gt,并且对代表地面的地面点云Gt予以指定标签标记;
对所述非地面点云no_Gt进行聚类处理,表示同一对象的点云形成一个点簇,给予唯一的标签视为一类,舍弃点云数量小于影响阈值的类别,输出聚类点云no_GSt;
对Gt与no_GSt进行高度、边缘、平面特征提取;
2.根据权利要求1所述的高精度三维地图与二维栅格地图同步构建方法,其特征在于,所述建立坐标系以及空间格网包括:
建立以三维激光雷达为中心的激光雷达坐标系,在三维激光雷达进行扫描时的最初位置建立世界坐标系,各轴方向与所述激光雷达坐标系相同;
基于所述世界坐标系建立空间格网,根据所需栅格地图分辨率,设置小立方体。
3.根据权利要求1所述的高精度三维地图与二维栅格地图同步构建方法,其特征在于,所述分割地面点云Gt与非地面点云no_Gt,并且对代表地面的地面点云Gt予以指定标签标记包括:
根据公式(1)判断所述虚拟图像第i行j列像素表示的点是否为地面点,
其中,Ptr[j+i×k],表示所述虚拟图像第i行j列的像素,则Ptr[j+i×k].x、Ptr[j+i×k].y、Ptr[j+i×k].z表示该点在激光雷达坐标系下的三维坐标,其中,k表示总列数;
dz=Ptr[j+(i+1)×k].z-Ptr[j+i×k].z,
dx=Ptr[j+(i+1)×k].x-Ptr[j+i×k].x,
dy=Ptr[j+(i+1)×k].y-Ptr[j+i×k].y,
判断angle是否小于地面分割阈值:
若是,则该像素表示的点与对应下一行的点,即第i+1行j列像素表示的点,均视为地面点,且予以指定标签标记;
若否,则视为非地面点,不予标记。
4.根据权利要求1所述的高精度三维地图与二维栅格地图同步构建方法,其特征在于,所述三维激光雷达水平方向测量角度范围为O°到360°,垂直方向测量角度范围为-15°到15°,其水平角分辨率为0.2°,垂直角分辨率为2°。
5.根据权利要求1所述的高精度三维地图与二维栅格地图同步构建方法,其特征在于,所述对所述非地面点云no_Gt进行聚类处理,表示同一对象的点云形成一个点簇,给予唯一的标签视为一类包括:
遍历no_Gt中每一个点,对未聚类处理的点进行广度优先搜索,得到在图像中当前点的上下左右四个邻点;
根据公式(2)计算A、B两点的角度β的值,
其中:dmax=max{A.intensity,B.intensity},
dmin=min{A.intensity,B.intensity},
A.intensity,B.intensity分别表示A点、B点到所述三维激光雷达传感器的欧式距离,α表示两激光束的夹角,A为当前点,B为四个邻点中的任意一个邻点;
判断角度β是否大于预设的聚类角度阈值;
在判断所述角度β大于所述聚类角度阈值的情况下,确定A、B两点表示同一对象;
在A、B两点表示同一对象的情况下,把A、B两点放入同一点簇中以表示同一对象,并给予唯一的标签标记。
6.根据权利要求1所述的高精度三维地图与二维栅格地图同步构建方法,其特征在于,所述对Gt与no_GSt进行高度、边缘、平面特征提取包括:
遍历no_GSt中所有点,在Gt中使用K近邻搜索算法找到与当前点在X-Y平面上距离最近的地面点,计算当前点与所述最近的地面点在Z方向的高度差dz;
判断所述高度差是否位于预设的高度范围[Hmin,Hmax];
在判断所述高度差位于所述高度范围[Hmin,Hmax]的情况下,对当前遍历到的点给予高度特征标记。
7.根据权利要求1所述的高精度三维地图与二维栅格地图同步构建方法,其特征在于,所述对Gt与no_GSt进行高度、边缘、平面特征提取包括:
遍历所述虚拟图像中的每一个点,以当前点为中心点,在中心点所在行中提取中心点左右相邻的连续点组成点集S,根据公式(3)计算所述虚拟图像中的每一个粗糙度rij,
边缘、平面特征提取:把表示0°-360°范围的虚拟图像平均分为6个分别表示60°的子图像,根据所述子图像的粗糙度值rij对每一行的点进行排序;
在子图像中的每一行中提取具有最小粗糙度且属于地面点云Gt的点作为平面特征点,以形成如公式(4)所示的集合,
{P∈Gt|min{P.rij,j∈[colmin,colmax]},i∈[rowmin,rowmax]} (4)
其中,P.rij表示点P的粗糙度,min{P.rij,j∈[colmin,colmax]}表示子图像中第i行具有最小粗糙度的点,colmin、colmax分别表示子图像列的最小值、最大值,rowmin、rowmax分别表示子图像行的最小值、最大值;
在子图像中的每一行中提取具有最大粗糙度且属于聚类点云no_GSt的点作为边缘特征点,以形成如公式(5)所示的集合,
{P∈no_GSt|max{P.rij,j∈[colmin,colmax]},i∈[rowmin,rowmax]} (5)
其中P.rij表示点P的粗糙度,max{P.rij,j∈[colmin,colmax]}表示子图像中第i行具有最大粗糙度的点,colmin、colmax分别表示子图像列的最小值、最大值,rowmin、rowmax分别表示子图像行的最小值、最大值。
在当前时刻地面点云Gt中提取到的平面特征点与上一时刻地面点云Gt-1中提取的平面特征点,在当前时刻聚类点云no_GSt中提取的边缘特征点与上一时刻聚类点云no_GSt-1中提取的边缘特征点,使用ICP算法进行点云配准,得到所述三维激光雷达从上一时刻到当前时刻运动的位移变换矩阵T与旋转变换矩阵R;
以第一次扫描为起点利用相邻两次扫描间所述三维激光雷达的位移变换矩阵T与旋转变换矩阵R,得到相对世界坐标系,当前时刻激光雷达的位移变换矩阵与旋转变换矩阵如公式(6)所示,
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010686222.9A CN112070770B (zh) | 2020-07-16 | 2020-07-16 | 一种高精度三维地图与二维栅格地图同步构建方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010686222.9A CN112070770B (zh) | 2020-07-16 | 2020-07-16 | 一种高精度三维地图与二维栅格地图同步构建方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112070770A true CN112070770A (zh) | 2020-12-11 |
CN112070770B CN112070770B (zh) | 2022-11-01 |
Family
ID=73657387
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010686222.9A Active CN112070770B (zh) | 2020-07-16 | 2020-07-16 | 一种高精度三维地图与二维栅格地图同步构建方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112070770B (zh) |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112767490A (zh) * | 2021-01-29 | 2021-05-07 | 福州大学 | 一种基于激光雷达的室外三维同步定位与建图方法 |
CN113393423A (zh) * | 2021-05-18 | 2021-09-14 | 深圳拓邦股份有限公司 | 一种基于点云的悬崖检测方法、装置及移动机器人 |
CN113640822A (zh) * | 2021-07-07 | 2021-11-12 | 华南理工大学 | 一种基于非地图元素过滤的高精度地图构建方法 |
CN113848944A (zh) * | 2021-10-18 | 2021-12-28 | 云鲸智能科技(东莞)有限公司 | 一种地图构建方法、装置、机器人及存储介质 |
CN114693862A (zh) * | 2020-12-29 | 2022-07-01 | 北京万集科技股份有限公司 | 三维点云数据模型重建方法、目标重识别方法及装置 |
CN114750147A (zh) * | 2022-03-10 | 2022-07-15 | 深圳甲壳虫智能有限公司 | 机器人的空间位姿确定方法、装置和机器人 |
CN114817426A (zh) * | 2021-01-28 | 2022-07-29 | 中强光电股份有限公司 | 地图建构装置及方法 |
CN116226298A (zh) * | 2023-05-08 | 2023-06-06 | 上海维智卓新信息科技有限公司 | 一种地图质量的自动化评估方法 |
CN117146829A (zh) * | 2023-10-30 | 2023-12-01 | 江苏云幕智造科技有限公司 | 基于双目与三维点云的多姿态人形机器人环境导航方法 |
CN117274527A (zh) * | 2023-08-24 | 2023-12-22 | 东方电气集团科学技术研究院有限公司 | 一种发电机设备三维可视化模型数据集构建方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140350839A1 (en) * | 2013-05-23 | 2014-11-27 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
CN106204705A (zh) * | 2016-07-05 | 2016-12-07 | 长安大学 | 一种基于多线激光雷达的3d点云分割方法 |
US20180232947A1 (en) * | 2017-02-11 | 2018-08-16 | Vayavision, Ltd. | Method and system for generating multidimensional maps of a scene using a plurality of sensors of various types |
CN108932736A (zh) * | 2018-05-30 | 2018-12-04 | 南昌大学 | 二维激光雷达点云数据处理方法以及动态机器人位姿校准方法 |
CN109443369A (zh) * | 2018-08-20 | 2019-03-08 | 北京主线科技有限公司 | 利用激光雷达和视觉传感器融合构建动静态栅格地图的方法 |
CN110221603A (zh) * | 2019-05-13 | 2019-09-10 | 浙江大学 | 一种基于激光雷达多帧点云融合的远距离障碍物检测方法 |
-
2020
- 2020-07-16 CN CN202010686222.9A patent/CN112070770B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140350839A1 (en) * | 2013-05-23 | 2014-11-27 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
CN106204705A (zh) * | 2016-07-05 | 2016-12-07 | 长安大学 | 一种基于多线激光雷达的3d点云分割方法 |
US20180232947A1 (en) * | 2017-02-11 | 2018-08-16 | Vayavision, Ltd. | Method and system for generating multidimensional maps of a scene using a plurality of sensors of various types |
CN108932736A (zh) * | 2018-05-30 | 2018-12-04 | 南昌大学 | 二维激光雷达点云数据处理方法以及动态机器人位姿校准方法 |
CN109443369A (zh) * | 2018-08-20 | 2019-03-08 | 北京主线科技有限公司 | 利用激光雷达和视觉传感器融合构建动静态栅格地图的方法 |
CN110221603A (zh) * | 2019-05-13 | 2019-09-10 | 浙江大学 | 一种基于激光雷达多帧点云融合的远距离障碍物检测方法 |
Non-Patent Citations (2)
Title |
---|
LI YI等: ""SyncSpecCNN: Synchronized Spectral CNN for 3D Shape Segmentation"", 《2017 IEEE CONFERENCE ON COMPUTER VISION AND PATTERN RECOGNITION (CVPR)》 * |
赵凯等: ""基于均值高程图的城市环境三维LiDAR点云地面分割方法"", 《军事交通学院学报》 * |
Cited By (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114693862A (zh) * | 2020-12-29 | 2022-07-01 | 北京万集科技股份有限公司 | 三维点云数据模型重建方法、目标重识别方法及装置 |
CN114817426A (zh) * | 2021-01-28 | 2022-07-29 | 中强光电股份有限公司 | 地图建构装置及方法 |
CN112767490A (zh) * | 2021-01-29 | 2021-05-07 | 福州大学 | 一种基于激光雷达的室外三维同步定位与建图方法 |
CN112767490B (zh) * | 2021-01-29 | 2022-06-21 | 福州大学 | 一种基于激光雷达的室外三维同步定位与建图方法 |
CN113393423A (zh) * | 2021-05-18 | 2021-09-14 | 深圳拓邦股份有限公司 | 一种基于点云的悬崖检测方法、装置及移动机器人 |
CN113640822A (zh) * | 2021-07-07 | 2021-11-12 | 华南理工大学 | 一种基于非地图元素过滤的高精度地图构建方法 |
CN113640822B (zh) * | 2021-07-07 | 2023-08-18 | 华南理工大学 | 一种基于非地图元素过滤的高精度地图构建方法 |
CN113848944A (zh) * | 2021-10-18 | 2021-12-28 | 云鲸智能科技(东莞)有限公司 | 一种地图构建方法、装置、机器人及存储介质 |
CN114750147A (zh) * | 2022-03-10 | 2022-07-15 | 深圳甲壳虫智能有限公司 | 机器人的空间位姿确定方法、装置和机器人 |
CN114750147B (zh) * | 2022-03-10 | 2023-11-24 | 深圳甲壳虫智能有限公司 | 机器人的空间位姿确定方法、装置和机器人 |
CN116226298A (zh) * | 2023-05-08 | 2023-06-06 | 上海维智卓新信息科技有限公司 | 一种地图质量的自动化评估方法 |
CN117274527A (zh) * | 2023-08-24 | 2023-12-22 | 东方电气集团科学技术研究院有限公司 | 一种发电机设备三维可视化模型数据集构建方法 |
CN117274527B (zh) * | 2023-08-24 | 2024-06-11 | 东方电气集团科学技术研究院有限公司 | 一种发电机设备三维可视化模型数据集构建方法 |
CN117146829A (zh) * | 2023-10-30 | 2023-12-01 | 江苏云幕智造科技有限公司 | 基于双目与三维点云的多姿态人形机器人环境导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN112070770B (zh) | 2022-11-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112070770B (zh) | 一种高精度三维地图与二维栅格地图同步构建方法 | |
CN111486855B (zh) | 一种具有物体导航点的室内二维语义栅格地图构建方法 | |
CN111563442B (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及*** | |
CN111429574B (zh) | 基于三维点云和视觉融合的移动机器人定位方法和*** | |
CN109345588B (zh) | 一种基于Tag的六自由度姿态估计方法 | |
CN111882612B (zh) | 一种基于三维激光检测车道线的车辆多尺度定位方法 | |
CN110853075B (zh) | 一种基于稠密点云与合成视图的视觉跟踪定位方法 | |
Lenac et al. | Fast planar surface 3D SLAM using LIDAR | |
CN109993793B (zh) | 视觉定位方法及装置 | |
CN109947097B (zh) | 一种基于视觉和激光融合的机器人定位方法及导航应用 | |
CN113865580B (zh) | 构建地图的方法、装置、电子设备及计算机可读存储介质 | |
CN112785643A (zh) | 一种基于机器人平台的室内墙角二维语义地图构建方法 | |
CN113409410A (zh) | 一种基于3d激光雷达的多特征融合igv定位与建图方法 | |
CN107917710B (zh) | 一种基于单线激光的室内实时定位与三维地图构建方法 | |
CN111523545B (zh) | 一种结合深度信息的物品查找方法 | |
CN104040590A (zh) | 用于估计物体的姿态的方法 | |
CN112799096B (zh) | 基于低成本车载二维激光雷达的地图构建方法 | |
CN109033989A (zh) | 基于三维点云的目标识别方法、装置及存储介质 | |
CN111968177A (zh) | 一种基于固定摄像头视觉的移动机器人定位方法 | |
CN114004869A (zh) | 一种基于3d点云配准的定位方法 | |
CN114088081B (zh) | 一种基于多段联合优化的用于精确定位的地图构建方法 | |
Zhao et al. | Cbhe: Corner-based building height estimation for complex street scene images | |
CN116977628A (zh) | 一种应用于动态环境的基于多模态语义框架的slam方法及*** | |
Feng et al. | Marker-assisted structure from motion for 3D environment modeling and object pose estimation | |
CN116679307A (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 | ||
CB02 | Change of applicant information | ||
CB02 | Change of applicant information |
Address after: No. 397, Tongcheng South Road, Baohe District, Hefei City, Anhui Province 230061 Applicant after: Super high voltage branch of State Grid Anhui Electric Power Co.,Ltd. Address before: No. 397, Tongcheng South Road, Baohe District, Hefei City, Anhui Province 230061 Applicant before: STATE GRID ANHUI POWER SUPPLY COMPANY OVERHAUL BRANCH |
|
GR01 | Patent grant | ||
GR01 | Patent grant |