CN107169986B - 一种障碍物检测方法及系统 - Google Patents
一种障碍物检测方法及系统 Download PDFInfo
- Publication number
- CN107169986B CN107169986B CN201710368864.2A CN201710368864A CN107169986B CN 107169986 B CN107169986 B CN 107169986B CN 201710368864 A CN201710368864 A CN 201710368864A CN 107169986 B CN107169986 B CN 107169986B
- Authority
- CN
- China
- Prior art keywords
- region
- point cloud
- plane
- cloud data
- point
- 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.)
- Expired - Fee Related
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
-
- 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
-
- 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/30—Subject of image; Context of image processing
- G06T2207/30241—Trajectory
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开一种障碍物检测方法及系统。所述方法包括:获取N线激光雷达扫描得到的点云数据;将点云数据划分为Q个区域,包括第1区域、第2区域至第Q区域;依次对第1至第Q区域的点云数据进行平面拟合,对应得到第1平面、第2平面至第Q平面;依次获取第1至第Q区域的点云数据中的障碍物的坐标;对所述障碍物的坐标数据进行体素化处理,得到动态障碍物的坐标集合和静态障碍物的坐标集合。采用本发明检测方法及系统,实现了三维点云数据的大规模压缩,大大降低了数据处理量;还进一步把环境划分为三部分:路面区域(可行区域)、静态障碍物区域、动态障碍物区域,为机器人的有效行为决策提供了更好的环境信息。
Description
技术领域
本发明涉及智能检测领域,特别是涉及一种障碍物检测方法及系统。
背景技术
现代机器人技术及无人驾驶技术发展迅速,随着技术的不断成熟,可自主导航及路径规划的机器人出现在人们的生活中。在享受机器人技术及无人驾驶技术带来的便利、提高生产效率的同时,机器人与机器人之间、机器人与人之间、机器人与建筑物之间等碰撞事故时有发生,如何避免机器人的碰撞事故成为了人们重点关注的问题。因此,如何在机器人自主行动过程中去对所处环境中的各类障碍物进行检测,成为解决碰撞事故的关键。常用障碍物检测系统是基于单/双目摄像头、毫米波雷达等传感器。
其中单目摄像头检测,是先通过图像匹配进行目标识别(各种障碍等),再通过目标在图像中的大小去估算目标距离,需要建立并不断维护一个庞大的样本特征数据库,保证这个数据库包含待识别目标的全部特征数据。而双目摄像头检测,是通过对两幅图像视差的计算,直接对前方景物进行距离测量。无论单/双目摄像头都受环境影响较大,如环境的光暗状况、阴影、温湿度等的干扰,图像会存在不同程度的失真,另外计算量大,对计算单元的性能及算法要求较高。相比于单/双目摄像头,毫米波雷达具有更高的精度及分辨率,探测范围也更加广泛,但其探测距离受到频段损耗的直接制约,也无法感知行人,并且对周边所有障碍物无法进行精准的建模。目前也存在一些多传感器融合的检测系统,但其同样面临着计算量大、受环境制约等问题。
发明内容
本发明的目的是提供一种障碍物检测方法及系统,以解决传统检测方法及系统中计算量大且受环境影响大的问题。
为实现上述目的,本发明提供了如下方案:
一种障碍物检测方法,所述方法包括:
获取N线激光雷达扫描得到的点云数据,所述点云数据竖直方向坐标不同、水平面上的投影为N条平行直线;
将所述点云数据划分为Q个区域,所述Q个区域包括第1区域、第2区域至第Q区域;
依次对所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据进行平面拟合,对应得到第1平面、第2平面至第Q平面;
依次获取所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据中的障碍物的坐标;
对所述障碍物的坐标数据进行体素化处理,得到动态障碍物的坐标集合和静态障碍物的坐标集合。
可选的,所述将所述点云数据划分为Q个区域,具体包括:
将所述水平面上投影的N条直线中每连续m条直线对应的点云数据划分为同一区域,依次得到第1区域、第2区域至第Q区域;所述第1区域的点云数据包括所述N条直线中第1至m条直线对应的点云数据,所述第2区域的点云数据包括所述N条直线中第m至2m-1条直线对应的点云数据,所述第Q区域的点云数据包括所述N条直线中第1+(Q-1)(m-1)至1+Q(m-1)条直线对应的点云数据。
可选的,所述依次对所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据进行平面拟合,对应得到第1平面、第2平面至第Q平面,具体包括:
利用四分位差法构造门函数压缩数据,确定第i区域点云数据的门上限和门下限;其中i=1,2,……,Q;
根据所述第i区域点云数据的门上限和所述门下限确定所述第i区域点云数据更新后的区域,所述第i区域点云数据更新后的区域为所述上门限和所述下门限之间的点云数据区域;
根据所述第i区域点云数据更新后的区域,利用随机抽样一致性算法进行平面拟合,得到第i区域的初始平面;
当i=1时,将所述第i区域的初始平面确定为第i平面;
当i>1时,判断第i区域的初始平面是否有效,得到第一判断结果;
当所述第一判断结果表示是时,确定所述第i区域的初始平面为第i平面;
当所述第一判断结果表示否时,判断第i+1区域的初始平面是否有效,得到第二判断结果;
当所述第二判断结果表示是时,确定所述第i+1区域的初始平面为第i平面。
可选的,所述利用四分位差法构造门函数压缩数据,确定第i区域点云数据的门上限和门下限,具体包括:
确定所述第i区域点云数据的四分位差IQR=Q75%-Q25%;其中IQR表示所述第i区域中竖直方向坐标小于z75%且大于z25%的点云数据区域,其中竖直方向坐标z75%=z1+Δz×75%,z25%=z1+Δz×25%,z1为所述第i区域中点云数据中竖直方向坐标最小值,Δz为所述第i区域中点云数据中数值方向坐标最大值与最小值的差值;
确定所述第i区域点云数据的门上限为Qmax=Q75%;
确定所述第i区域点云数据的门下限为Qmin=Q25%-0.5×(IQR)。
可选的,所述根据所述第i区域点云数据更新后的区域,利用随机抽样一致性算法进行平面拟合,得到第i区域的初始平面,具体包括:
在所述第i区域点云数据更新后的区域,利用三点的随机抽样一致性算法进行平面拟合,得到初始拟合后的平面;
判断所述初始拟合后的平面内点数量是否大于设定阈值,得到第三判断结果;
当所述第三判断结果表示是时,将所述初始拟合后的平面确定为第i区域的初始平面;
当所述第三判断结果表示否时,返回利用三点的随机抽样一致性算法进行平面拟合的步骤。
可选的,所述当i>1时,判断第i区域的初始平面是否有效,得到第一判断结果,具体包括:
利用公式计算所述第i区域的初始平面与第i-1区域的初始平面之间的角度差δΨi,其中为所述第i区域的初始平面的单位法向量,为所述第i-1区域的初始平面的单位法向量;
计算所述第i区域的初始平面与第i-1区域的初始平面之间的高度差δZi;
判断是否角度差δΨi小于角度设定阈值且高度差δZi小于高度设定阈值,得到第四判断结果;
当所述第四判断结果表示是时,确定所述第i区域的初始平面有效;
当所述第四判断结果表示否时,确定所述第i区域的初始平面无效。
可选的,所述依次获取所述第1区域点云数据、所述第2区域点云数据至所述第Q区域点云数据中的障碍物的坐标,具体包括:
利用公式遍历计算第i区域内第i平面外的任一点p到第i平面的距离d;其中p0为所述第i平面上任一点,为所述第i区域内对应第i平面的单位法向量;
判断所述距离d是否大于距离设定阈值,得到第五判断结果;
当所述第五判断结果表示是时,确定所述点p是障碍物点;
获取所述点p的坐标;
当所述第五判断结果表示否时,确定所述点p不是障碍物点。
可选的,所述对所述障碍物的坐标数据进行体素化处理,得到动态障碍物的坐标集合和静态障碍物的坐标集合,具体包括:
对第一时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第一体素化处理,其中,为向下取整函数,X1为任一障碍物点p1的坐标,v为体素的大小,为所述障碍物点p1体素化后得到的对应体素的位置坐标,对所述障碍物点进行第一体素化处理后,计算对应体素内包含的点个数,得到所述障碍物点p1的占用值;
对第二时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第二体素化处理,其中所第二时刻为所述第一时刻所述N线激光雷达扫描的后一次扫描时刻,X2为任一障碍物点p2的坐标,为所述障碍物点p2体素化后得到的对应体素的位置坐标,对所述障碍物点进行第二体素化处理后,计算对应体素内包含的点个数,得到所述障碍物点p2的占用值;
在所述第一体素化处理得到的体素和所述第二体素化处理得到的体素范围内,判断以所述为中心、以设定体素阈值为半径组成的球体范围内的体素的占用值是否与所述的占用值相等,得到第六判断结果;
当所述第六判断结果表示是时,确定所述障碍物点p2为动态障碍物;
当所述第六判断结果表示否时,确定所述障碍物点p2为静态障碍物;
获得动态障碍物的坐标集合和静态障碍物的坐标集合。
一种障碍物检测系统,所述系统包括:
点云数据获取模块,用于获取N线激光雷达扫描得到的点云数据,所述点云数据竖直方向坐标不同、水平面上的投影为N条平行直线;
区域划分模块,用于将所述点云数据划分为Q个区域,所述Q个区域包括第1区域、第2区域至第Q区域;
平面拟合模块,用于依次对所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据进行平面拟合,对应得到第1平面、第2平面至第Q平面;
障碍物坐标获取模块,用于依次获取所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据中的障碍物的坐标;
体素化处理模块,用于对所述障碍物的坐标数据进行体素化处理,得到动态障碍物的坐标集合和静态障碍物的坐标集合。
可选的,所述体素化处理模块具体包括:
第一体素化处理单元,用于对第一时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第一体素化处理,其中,为向下取整函数,X1为任一障碍物点p1的坐标,v为体素的大小,为所述障碍物点p1体素化后得到的对应体素的位置坐标,对所述障碍物点进行第一体素化处理后,计算对应体素内包含的点个数,得到所述障碍物点p1的占用值;
第二体素化处理单元,用于对第二时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第二体素化处理,其中所第二时刻为所述第一时刻所述N线激光雷达扫描的后一次扫描时刻,X2为任一障碍物点p2的坐标,为所述障碍物点p2体素化后得到的对应体素的位置坐标,对所述障碍物点进行第二体素化处理后,计算对应体素内包含的点个数,得到所述障碍物点p2的占用值;
占用值判断单元,用于在所述第一体素化处理得到的体素和所述第二体素化处理得到的体素范围内,判断以所述为中心、以设定体素阈值为半径组成的球体范围内的体素的占用值是否与所述的占用值相等,得到第六判断结果;
动态障碍物确定单元,用于当所述第六判断结果表示是时,确定所述障碍物点p2为动态障碍物;
静态障碍物确定单元,用于当所述第六判断结果表示否时,确定所述障碍物点p2为静态障碍物;
坐标集合获得单元,用于获得动态障碍物的坐标集合和静态障碍物的坐标集合。
根据本发明提供的具体实施例,本发明公开了以下技术效果:以三维激光雷达为传感器,通过分段平面拟合、路面分离、点云体素化几个步骤,实现了三维点云数据的大规模压缩,大大降低了数据处理量,并且不受环境限制;不仅可以检测出环境中的障碍物,还进一步把环境划分为三部分:路面区域(可行区域)、静态障碍物区域、动态障碍物区域,为机器人的有效行为决策提供了更好的环境信息。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明障碍物检测方法流程图;
图2为本发明障碍物检测系统结构图;
图3为本发明障碍物检测方法中激光雷达扫描示意图;
图4为本发明障碍物检测方法中拟合平面示意图;
图5为本发明障碍物检测方法中k时刻障碍物体素化处理后示意图;
图6为本发明障碍物检测方法中k+1时刻障碍物体素化处理后示意图;
图7为本发明障碍物检测方法中k+1时刻障碍物体素化处理后与k时刻结合后的示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本发明作进一步详细的说明。
图1为本发明障碍物检测方法流程图。如图1所示,所述方法包括:
步骤101:获取点云数据。获取N线激光雷达扫描得到的点云数据,所述点云数据竖直方向坐标不同、水平面上的投影为N条平行直线。具体可以采用十六线激光雷达,也可以用三十二线激光雷达,激光雷达扫描示意图具体参见图3。
步骤102:划分点云数据。将所述点云数据划分为Q个区域,所述Q个区域包括第1区域、第2区域至第Q区域。具体的,将所述水平面上投影的N条直线中每连续m条直线对应的点云数据划分为同一区域,依次得到第1区域、第2区域至第Q区域;所述第1区域的点云数据包括所述N条直线中第1至m条直线对应的点云数据,所述第2区域的点云数据包括所述N条直线中第m至2m-1条直线对应的点云数据,所述第Q区域的点云数据包括所述N条直线中第1+(Q-1)(m-1)至1+Q(m-1)条直线对应的点云数据。例如,采用十六线激光雷达扫描得到的点云数据,则获得16条扫描线点云数据,可以将每3条扫描线的点云数据划为一个区域,1-4条线的点云数据为参考区域,4-6条线的点云数据为第一区域,6-8条线的点云数据为第二区域,8-10条线的点云数据为第三区域……依次构成7个区域。
步骤103:进行平面拟合。依次对所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据进行平面拟合,对应得到第1平面、第2平面至第Q平面。对于每一区域的点云数据进行平面拟合具体步骤为:
(1)使用四分位差法构造门函数压缩数据并更新区域;
确定所述第i区域点云数据的四分位差IQR=Q75%-Q25%;其中IQR表示所述第i区域中竖直方向坐标小于z75%且大于z25%的点云数据区域,其中竖直方向坐标z75%=z1+Δz×75%,z25%=z1+Δz×25%,z1为所述第i区域中点云数据中竖直方向坐标最小值,Δz为所述第i区域中点云数据中数值方向坐标最大值与最小值的差值;
确定所述第i区域点云数据的门上限为Qmax=Q75%;
确定所述第i区域点云数据的门下限为Qmin=Q25%-0.5×(IQR);
保留Z∈(Qmin,Qmax)的点为更新后的区域。
(2)针对更新后的区域使用随机抽样一致算法从区域内随机抽取三个点构造初始平面,当初始平面内点数量足够多的时候(即初始平面内点数量大于设定阈值时),初始平面拟合成功并表示为:
aix+biy+ciz+di=0。
(3)依次构造所有区域内的平面,并验证其有效性。
验证构造的初始平面是否有效的方法为:计算相邻平面的角度差及高度差,即利用公式计算第i区域的初始平面与第i-1区域的初始平面之间的角度差δΨi,其中为所述第i区域的初始平面的单位法向量,为所述第i-1区域的初始平面的单位法向量;
利用公式δZi=|Zi-Zi-1|计算第i区域的初始平面与第i-1区域的初始平面之间的高度差δZi;其中
当两者均满足阈值要求,则该平面有效;否则该平面无效,将其舍弃。例如将第2区域的初始平面舍弃后,第3区域拟合的初始平面的相邻平面则为第1区域的初始平面。通过平面拟合获得每个区域的代表平面,作为参考路面。
步骤104:获得障碍物坐标。依次获取所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据中的障碍物的坐标。采用计算各区域中平面外的点到平面的距离,将远离平面的点确定为障碍物点,将接近平面的点确定为路面点。具体的,利用公式遍历计算第i区域内第i平面外的任一点p到第i平面的距离d;其中p0为所述第i平面上任一点,为所述第i区域内对应第i平面的单位法向量;然后判断所述距离d是否大于距离设定阈值,如果是,确定所述点p是障碍物点,并获取所述点p的坐标;
步骤105:对障碍物坐标进行体素化处理。将所有非路面点即障碍物点进行体素化并根据每个体素内包含点的个数计算各自的占用值,将这一时刻数据作为模板储存在数据存储模块内。使用同样的方法处理下一时刻数据,得到若干个体素及占用值,将其映射到上一时刻得到的模板数据中进行对比处理。由于在任何时刻,同一障碍在体素内呈现相同的占用值,结合两组数据遍历每一个体素进行判断,若在该体素的相邻体素中具有与其相等的占用值,则该体素代表动态障碍物;若在该体素的相邻体素中没有与其相等的占用值,则该体素代表静态障碍物。具体体素化示意图参见图5。体素化处理主要作用于非路面点即障碍物点,使其以体素的形式表现出来,计算体素内所含点的个数,记作体素的占用值。对比相邻体素之间的占用值变化,从而检测动、静态障碍物得到动态障碍物的坐标集合和静态障碍物的坐标集合。具体流程如下:
对第一时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第一体素化处理,其中,为向下取整函数,X1为任一障碍物点p1的坐标,v为体素的大小,为所述障碍物点p1体素化后得到的对应体素的位置坐标,对所有障碍物点进行第一体素化处理后,计算各体素内包含的点个数,记作其占用值;
对第二时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第二体素化处理,其中所第二时刻为所述第一时刻所述N线激光雷达扫描的后一次扫描时刻,X2为任一障碍物点p2的坐标,为所述障碍物点p2体素化后得到的对应体素的位置坐标,对所有障碍物点进行第二体素化处理后,计算各体素内包含的点个数,记作其占用值;
在所述第一体素化处理得到的体素和所述第二体素化处理得到的体素范围内,判断以体素为中心、以设定体素阈值为半径组成的球体范围内的体素的占用值是否与所述体素的占用值相等;
如果是,确定所述障碍物点p2为动态障碍物;
如果否,确定所述障碍物点p2为静态障碍物。
步骤106:得到动态障碍物坐标集合和静态障碍物坐标集合。根据步骤105确定的动态障碍物和静态障碍物,依次获得相应的坐标,构成动态障碍物坐标集合和静态障碍物坐标集合,并结合步骤103获得的平面,指示机器人合理规划路线,避免碰撞。
图2为本发明障碍物检测系统结构图。如图2所示,所述系统包括:
点云数据获取模块201,用于获取N线激光雷达扫描得到的点云数据,所述点云数据竖直方向坐标不同、水平面上的投影为N条平行直线;
区域划分模块202,用于将所述点云数据划分为Q个区域,所述Q个区域包括第1区域、第2区域至第Q区域;
平面拟合模块203,用于依次对所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据进行平面拟合,对应得到第1平面、第2平面至第Q平面;
障碍物坐标获取模块204,用于依次获取所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据中的障碍物的坐标;
体素化处理模块205,用于对所述障碍物的坐标数据进行体素化处理,得到动态障碍物的坐标集合和静态障碍物的坐标集合。
所述系统的各模块具体工作过程参见图1中相应各步骤的说明。
图3为本发明障碍物检测方法中激光雷达扫描示意图。以十六线激光雷达为例,将十六线激光雷达与处理计算机连接,各子系统经过初始化开始正常运行,十六线激光雷达通过DP83848网络模块以UDP为传输协议,向处理计算机传输扫描得到的稠密点云数据。
如图3所示,十六线三维激光雷达,共有十六个扫描平面,每两个平面扫描之间的夹角Δα为2°,在0到λ0的范围内,雷达数据密集且精确度高,该区域作为参考区域。雷达被固定在距离地面高h处,有:
α0=arctan(λ0/h)
扫描区域被分割成λk-1个,每个区域的长度λk由下式给出:
λk=h·tan(α0+k·η·Δα),(k:1,2,......,N),{K:1,…,N}
其中,η取3,表示每三个扫描平面构成一个区域。
图4为本发明障碍物检测方法中拟合平面示意图。如图4所示,利用四分位差法构造门函数压缩数据,方法为:
四分位差IQR=Q75%-Q25%;其中IQR为第i区域中竖直方向坐标小于z75%且大于z25%的点云数据区域,其中竖直方向坐标z75%=z1+Δz×75%,z25%=z1+Δz×25%,z1为所述第i区域中点云数据中竖直方向坐标最小值,Δz为所述第i区域中点云数据中数值方向坐标最大值与最小值的差值;
确定第i区域点云数据的门上限为Qmax=Q75%;
确定第i区域点云数据的门下限为Qmin=Q25%-0.5×(IQR)
根据所述第i区域点云数据的门上限和所述门下限,保留Z∈(Qmin,Qmax)的点为第i区域点云数据更新后的区域;
根据所述第i区域点云数据更新后的区域,利用三点的随机抽样一致性算法进行平面拟合,得到第i区域的初始平面aix+biy+ciz+di=0;
为验证构造的初始平面是否有效,计算相邻平面的角度差及高度差,即利用公式计算第i区域的初始平面与第i-1区域的初始平面之间的角度差δΨi,其中为所述第i区域的初始平面的单位法向量,为所述第i-1区域的初始平面的单位法向量;
利用公式δZi=|Zi-Zi-1|计算第i区域的初始平面与第i-1区域的初始平面之间的高度差δZi,
当两者均满足阈值要求,则该平面有效;否则该平面无效,将其舍弃。
图5为本发明障碍物检测方法中k时刻体素化处理示意图。如图5所示,在动、静态障碍物检测的过程中,认为随着时间的改变及数据的积累,动态障碍会占据不同的体素并具有相同的占用值,静态障碍物占据相同的体素。因此构造模板匹配机制。
对第k时刻激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行体素化处理,其中,为向下取整函数,X1为任一障碍物点p1的坐标,v为体素的大小,v=0.1,为所述障碍物点p1体素化后得到的对应体素的位置坐标,对所有障碍物点进行体素化处理后,;计算障碍物所占体素的占用值,假设探测到三个障碍物O1、O2、O3,占据了三个体素并具有不同的占用值,如图5所示,将其作为模板。
图6为本发明障碍物检测方法中k+1时刻障碍物体素化处理后示意图。如图6所示,对第k+1时刻激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行体素化处理,其中所第k+1时刻为所述第k时刻所述N线激光雷达扫描的后一次扫描时刻,X2为任一障碍物点p2的坐标,为所述障碍物点p2体素化后得到的对应体素的位置坐标,对所有障碍物点进行体素化处理后;计算障碍物所占体素的占用值,假设探测到三个障碍物O1’、O2、O3,占据了三个体素并具有不同的占用值,如图6所示.
图7为本发明障碍物检测方法中k+1时刻障碍物体素化处理后与k时刻结合后的示意图。将k+1时刻体素化后数据与k时刻数据结合,假设O1发生了运动,到达了O1’所在体素但其占用值不变,O2、O3未发生运动,还占据着相同的体素,如图7所示。在第k时刻中,分别以O1’、O2、O3为中心体素,与其相邻体素(设定阈值范围内的体素)占用值作对比,若具有相同的占用值则该障碍为动态障碍并从模板中剔除,所有对比完成后更新模板,依次往下迭代。即:在第k时刻,与O1’相邻并具有占用值的体素为O1及O3,O1’与O1具有相同的占用值,与O3相比具有不同的占用值,故O1为运动障碍物,将其从模板中剔除。更新后的模板即为图6。
本说明书中各个实施例采用递进的方式描述,每个实施例重点说明的都是与其他实施例的不同之处,各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言,由于其与实施例公开的方法相对应,所以描述的比较简单,相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处。综上所述,本说明书内容不应理解为对本发明的限制。
Claims (9)
1.一种障碍物检测方法,其特征在于,所述方法包括:
获取N线激光雷达扫描得到的点云数据,所述点云数据竖直方向坐标不同、水平面上的投影为N条平行直线;
将所述点云数据划分为Q个区域,所述Q个区域包括第1区域、第2区域至第Q区域;
依次对所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据进行平面拟合,对应得到第1平面、第2平面至第Q平面;
依次获取所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据中的障碍物的坐标;
对所述障碍物的坐标数据进行体素化处理,得到动态障碍物的坐标集合和静态障碍物的坐标集合;
所述依次对所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据进行平面拟合,对应得到第1平面、第2平面至第Q平面,具体包括:
利用四分位差法构造门函数压缩数据,确定第i区域点云数据的门上限和门下限;其中i=1,2,……,Q;
根据所述第i区域点云数据的门上限和所述门下限确定所述第i区域点云数据更新后的区域,所述第i区域点云数据更新后的区域为所述门上限和所述门下限之间的点云数据区域;
根据所述第i区域点云数据更新后的区域,利用随机抽样一致性算法进行平面拟合,得到第i区域的初始平面;
当i=1时,将所述第i区域的初始平面确定为第i平面;
当i>1时,判断第i区域的初始平面是否有效,得到第一判断结果;
当所述第一判断结果表示是时,确定所述第i区域的初始平面为第i平面;
当所述第一判断结果表示否时,判断第i+1区域的初始平面是否有效,得到第二判断结果;
当所述第二判断结果表示是时,确定所述第i+1区域的初始平面为第i平面。
2.根据权利要求1所述的方法,其特征在于,所述将所述点云数据划分为Q个区域,具体包括:
将所述水平面上投影的N条直线中每连续m条直线对应的点云数据划分为同一区域,依次得到第1区域、第2区域至第Q区域;所述第1区域的点云数据包括所述N条直线中第1至m条直线对应的点云数据,所述第2区域的点云数据包括所述N条直线中第m至2m-1条直线对应的点云数据,所述第Q区域的点云数据包括所述N条直线中第1+(Q-1)(m-1)至1+Q(m-1)条直线对应的点云数据。
3.根据权利要求1所述的方法,其特征在于,所述利用四分位差法构造门函数压缩数据,确定第i区域点云数据的门上限和门下限,具体包括:
确定所述第i区域点云数据的四分位差IQR=Q75%-Q25%;其中IQR表示所述第i区域中竖直方向坐标小于z75%且大于z25%的点云数据区域,其中竖直方向坐标z75%=z1+Δz×75%,z25%=z1+Δz×25%,z1为所述第i区域中点云数据中竖直方向坐标最小值,Δz为所述第i区域中点云数据中数值方向坐标最大值与最小值的差值;
确定所述第i区域点云数据的门上限为Qmax=Q75%;
确定所述第i区域点云数据的门下限为Qmin=Q25%-0.5×(IQR)。
4.根据权利要求1所述的方法,其特征在于,所述根据所述第i区域点云数据更新后的区域,利用随机抽样一致性算法进行平面拟合,得到第i区域的初始平面,具体包括:
在所述第i区域点云数据更新后的区域,利用三点的随机抽样一致性算法进行平面拟合,得到初始拟合后的平面;
判断所述初始拟合后的平面内点数量是否大于设定阈值,得到第三判断结果;
当所述第三判断结果表示是时,将所述初始拟合后的平面确定为第i区域的初始平面;
当所述第三判断结果表示否时,返回利用三点的随机抽样一致性算法进行平面拟合的步骤。
5.根据权利要求1所述的方法,其特征在于,所述当i>1时,判断第i区域的初始平面是否有效,得到第一判断结果,具体包括:
利用公式计算所述第i区域的初始平面与第i-1区域的初始平面之间的角度差δΨi,其中为所述第i区域的初始平面的单位法向量,为所述第i-1区域的初始平面的单位法向量;
计算所述第i区域的初始平面与第i-1区域的初始平面之间的高度差δZi;
判断是否角度差δΨi小于角度设定阈值且高度差δZi小于高度设定阈值,得到第四判断结果;
当所述第四判断结果表示是时,确定所述第i区域的初始平面有效;
当所述第四判断结果表示否时,确定所述第i区域的初始平面无效。
6.根据权利要求1所述的方法,其特征在于,所述依次获取所述第1区域点云数据、所述第2区域点云数据至所述第Q区域点云数据中的障碍物的坐标,具体包括:
利用公式遍历计算第i区域内第i平面外的任一点p到第i平面的距离d;其中p0为所述第i平面上任一点,为所述第i区域内对应第i平面的单位法向量;
判断所述距离d是否大于距离设定阈值,得到第五判断结果;
当所述第五判断结果表示是时,确定所述点p是障碍物点;
获取所述点p的坐标;
当所述第五判断结果表示否时,确定所述点p不是障碍物点。
7.根据权利要求1所述的方法,其特征在于,所述对所述障碍物的坐标数据进行体素化处理,得到动态障碍物的坐标集合和静态障碍物的坐标集合,具体包括:
对第一时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第一体素化处理,其中,为向下取整函数,X1为任一障碍物点p1的坐标,v为体素的大小,为所述障碍物点p1体素化后得到的对应体素的位置坐标,对所述障碍物点进行第一体素化处理后,计算对应体素内包含的点个数,得到所述障碍物点p1的占用值;
对第二时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第二体素化处理,其中所第二时刻为所述第一时刻所述N线激光雷达扫描的后一次扫描时刻,X2为任一障碍物点p2的坐标,为所述障碍物点p2体素化后得到的对应体素的位置坐标,对所述障碍物点进行第二体素化处理后,计算对应体素内包含的点个数,得到所述障碍物点p2的占用值;
在所述第一体素化处理得到的体素和所述第二体素化处理得到的体素范围内,判断以所述为中心、以设定体素阈值为半径组成的球体范围内的体素的占用值是否与所述的占用值相等,得到第六判断结果;
当所述第六判断结果表示是时,确定所述障碍物点p2为动态障碍物;
当所述第六判断结果表示否时,确定所述障碍物点p2为静态障碍物;
获得动态障碍物的坐标集合和静态障碍物的坐标集合。
8.一种障碍物检测系统,其特征在于,所述系统包括:
点云数据获取模块,用于获取N线激光雷达扫描得到的点云数据,所述点云数据竖直方向坐标不同、水平面上的投影为N条平行直线;
区域划分模块,用于将所述点云数据划分为Q个区域,所述Q个区域包括第1区域、第2区域至第Q区域;
平面拟合模块,用于依次对所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据进行平面拟合,对应得到第1平面、第2平面至第Q平面;具体包括:利用四分位差法构造门函数压缩数据,确定第i区域点云数据的门上限和门下限;其中i=1,2,……,Q;根据所述第i区域点云数据的门上限和所述门下限确定所述第i区域点云数据更新后的区域,所述第i区域点云数据更新后的区域为所述门上限和所述门下限之间的点云数据区域;根据所述第i区域点云数据更新后的区域,利用随机抽样一致性算法进行平面拟合,得到第i区域的初始平面;当i=1时,将所述第i区域的初始平面确定为第i平面;当i>1时,判断第i区域的初始平面是否有效,得到第一判断结果;当所述第一判断结果表示是时,确定所述第i区域的初始平面为第i平面;当所述第一判断结果表示否时,判断第i+1区域的初始平面是否有效,得到第二判断结果;当所述第二判断结果表示是时,确定所述第i+1区域的初始平面为第i平面;
障碍物坐标获取模块,用于依次获取所述第1区域的点云数据、所述第2区域的点云数据至所述第Q区域的点云数据中的障碍物的坐标;
体素化处理模块,用于对所述障碍物的坐标数据进行体素化处理,得到动态障碍物的坐标集合和静态障碍物的坐标集合。
9.根据权利要求8所述的系统,其特征在于,所述体素化处理模块具体包括:
第一体素化处理单元,用于对第一时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第一体素化处理,其中,为向下取整函数,X1为任一障碍物点p1的坐标,v为体素的大小,为所述障碍物点p1体素化后得到的对应体素的位置坐标,对所述障碍物点进行第一体素化处理后,计算对应体素内包含的点个数,得到所述障碍物点p1的占用值;
第二体素化处理单元,用于对第二时刻所述N线激光雷达扫描后得到的障碍物点,利用公式对障碍物点进行第二体素化处理,其中所第二时刻为所述第一时刻所述N线激光雷达扫描的后一次扫描时刻,X2为任一障碍物点p2的坐标,为所述障碍物点p2体素化后得到的对应体素的位置坐标,对所述障碍物点进行第二体素化处理后,计算对应体素内包含的点个数,得到所述障碍物点p2的占用值;
占用值判断单元,用于在所述第一体素化处理得到的体素和所述第二体素化处理得到的体素范围内,判断以所述为中心、以设定体素阈值为半径组成的球体范围内的体素的占用值是否与所述的占用值相等,得到第六判断结果;
动态障碍物确定单元,用于当所述第六判断结果表示是时,确定所述障碍物点p2为动态障碍物;
静态障碍物确定单元,用于当所述第六判断结果表示否时,确定所述障碍物点p2为静态障碍物;
坐标集合获得单元,用于获得动态障碍物的坐标集合和静态障碍物的坐标集合。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710368864.2A CN107169986B (zh) | 2017-05-23 | 2017-05-23 | 一种障碍物检测方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710368864.2A CN107169986B (zh) | 2017-05-23 | 2017-05-23 | 一种障碍物检测方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107169986A CN107169986A (zh) | 2017-09-15 |
CN107169986B true CN107169986B (zh) | 2019-09-17 |
Family
ID=59820468
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710368864.2A Expired - Fee Related CN107169986B (zh) | 2017-05-23 | 2017-05-23 | 一种障碍物检测方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107169986B (zh) |
Families Citing this family (25)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107808123B (zh) * | 2017-09-30 | 2021-11-05 | 杭州迦智科技有限公司 | 图像可行域检测方法、电子设备、存储介质、检测系统 |
TWI651686B (zh) * | 2017-11-30 | 2019-02-21 | 國家中山科學研究院 | 一種光學雷達行人偵測方法 |
CN108152831B (zh) * | 2017-12-06 | 2020-02-07 | 中国农业大学 | 一种激光雷达障碍物识别方法及系统 |
CN108648219B (zh) * | 2018-04-08 | 2022-02-22 | 浙江大承机器人科技有限公司 | 一种基于双目的障碍物与可行区域检测方法 |
CN108549087B (zh) * | 2018-04-16 | 2021-10-08 | 北京瑞途科技有限公司 | 一种基于激光雷达的在线检测方法 |
CN109074490B (zh) * | 2018-07-06 | 2023-01-31 | 达闼机器人股份有限公司 | 通路检测方法、相关装置及计算机可读存储介质 |
CN109188382B (zh) * | 2018-07-27 | 2020-11-06 | 惠州华阳通用电子有限公司 | 一种基于毫米波雷达的目标识别方法 |
CN109141364B (zh) * | 2018-08-01 | 2020-11-03 | 北京进化者机器人科技有限公司 | 障碍物检测方法、系统及机器人 |
CN109144097B (zh) * | 2018-08-15 | 2021-04-06 | 广州极飞科技有限公司 | 障碍物或地面识别及飞行控制方法、装置、设备及介质 |
CN110857859B (zh) * | 2018-08-23 | 2022-02-08 | 杭州海康机器人技术有限公司 | 障碍物检测方法及装置 |
CN109212540A (zh) | 2018-09-12 | 2019-01-15 | 百度在线网络技术(北京)有限公司 | 基于激光雷达系统的测距方法、装置及可读存储介质 |
CN110909569B (zh) * | 2018-09-17 | 2022-09-23 | 深圳市优必选科技有限公司 | 路况信息识别方法及终端设备 |
CN109214348A (zh) * | 2018-09-19 | 2019-01-15 | 北京极智嘉科技有限公司 | 一种障碍物检测方法、装置、设备及存储介质 |
CN109934141B (zh) * | 2019-03-01 | 2021-05-04 | 北京百度网讯科技有限公司 | 用于标注数据的方法和装置 |
CN109934164B (zh) * | 2019-03-12 | 2020-12-01 | 杭州飞步科技有限公司 | 基于轨迹安全度的数据处理方法和装置 |
CN110032188A (zh) * | 2019-04-10 | 2019-07-19 | 湖南汽车工程职业学院 | 一种基于无人驾驶观光电动车的自主避障方法 |
CN110216664B (zh) * | 2019-04-30 | 2020-12-22 | 北京云迹科技有限公司 | 基于点云数据的障碍物识别方法及装置 |
CN110516564A (zh) * | 2019-08-06 | 2019-11-29 | 深兰科技(上海)有限公司 | 路面检测方法和装置 |
CN112445208A (zh) * | 2019-08-15 | 2021-03-05 | 纳恩博(北京)科技有限公司 | 机器人、行驶路径的确定方法、装置及存储介质 |
CN110928301B (zh) * | 2019-11-19 | 2023-06-30 | 北京小米智能科技有限公司 | 一种检测微小障碍的方法、装置及介质 |
CN112180343A (zh) * | 2020-09-30 | 2021-01-05 | 东软睿驰汽车技术(沈阳)有限公司 | 激光点云数据处理方法、装置、设备及无人驾驶系统 |
CN113138396B (zh) * | 2021-06-21 | 2021-09-07 | 上海伯镭智能科技有限公司 | 一种基于激光雷达的灰尘和障碍物检测方法 |
CN114367988B (zh) * | 2022-02-11 | 2023-11-17 | 煤炭科学研究总院有限公司 | 一种煤矿自主巡检平台上的机械臂运动规划方法和装置 |
CN114779209A (zh) * | 2022-03-28 | 2022-07-22 | 西安电子科技大学 | 一种激光雷达点云体素化方法及装置 |
CN116994457B (zh) * | 2023-09-26 | 2023-12-29 | 深圳海星智驾科技有限公司 | 车辆防碰撞的检测方法、装置、计算机设备及存储介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101975951A (zh) * | 2010-06-09 | 2011-02-16 | 北京理工大学 | 一种融合距离和图像信息的野外环境障碍检测方法 |
CN103955920A (zh) * | 2014-04-14 | 2014-07-30 | 桂林电子科技大学 | 基于三维点云分割的双目视觉障碍物检测方法 |
CN106199558A (zh) * | 2016-08-18 | 2016-12-07 | 宁波傲视智绘光电科技有限公司 | 障碍物快速检测方法 |
CN106650640A (zh) * | 2016-12-05 | 2017-05-10 | 浙江大学 | 一种基于激光雷达点云局部结构特征的负障碍物检测方法 |
-
2017
- 2017-05-23 CN CN201710368864.2A patent/CN107169986B/zh not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101975951A (zh) * | 2010-06-09 | 2011-02-16 | 北京理工大学 | 一种融合距离和图像信息的野外环境障碍检测方法 |
CN103955920A (zh) * | 2014-04-14 | 2014-07-30 | 桂林电子科技大学 | 基于三维点云分割的双目视觉障碍物检测方法 |
CN106199558A (zh) * | 2016-08-18 | 2016-12-07 | 宁波傲视智绘光电科技有限公司 | 障碍物快速检测方法 |
CN106650640A (zh) * | 2016-12-05 | 2017-05-10 | 浙江大学 | 一种基于激光雷达点云局部结构特征的负障碍物检测方法 |
Also Published As
Publication number | Publication date |
---|---|
CN107169986A (zh) | 2017-09-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107169986B (zh) | 一种障碍物检测方法及系统 | |
CN106092104B (zh) | 一种室内机器人的重定位方法及装置 | |
Zhou et al. | StructSLAM: Visual SLAM with building structure lines | |
CN110807350B (zh) | 用于面向扫描匹配的视觉slam的系统和方法 | |
CN111486855A (zh) | 一种具有物体导航点的室内二维语义栅格地图构建方法 | |
JP5588812B2 (ja) | 画像処理装置及びそれを用いた撮像装置 | |
CN108647646A (zh) | 基于低线束雷达的低矮障碍物的优化检测方法及装置 | |
JP4100239B2 (ja) | 障害物検出装置と同装置を用いた自律移動ロボット、障害物検出方法、及び障害物検出プログラム | |
CN107850448A (zh) | 用于生成及使用定位参考数据的方法及系统 | |
CN113781582A (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
CN112346463B (zh) | 一种基于速度采样的无人车路径规划方法 | |
Zhao et al. | Reconstruction of textured urban 3D model by fusing ground-based laser range and CCD images | |
CN109282808A (zh) | 用于桥梁三维巡航检测的无人机与多传感器融合定位方法 | |
CN108958232A (zh) | 一种基于深度视觉的移动扫地机器人slam装置及算法 | |
KR101207535B1 (ko) | 이동 로봇의 이미지 기반 동시적 위치 인식 및 지도 작성 방법 | |
CN115388902B (zh) | 室内定位方法和系统、ar室内定位导航方法和系统 | |
Mueller et al. | GIS-based topological robot localization through LIDAR crossroad detection | |
CN109711321A (zh) | 一种结构自适应的宽基线影像视角不变直线特征匹配方法 | |
Zhang et al. | Loop closure through vanishing points in a line-based monocular SLAM | |
CN115908539A (zh) | 一种目标体积自动测量方法和装置、存储介质 | |
Yang et al. | Vision system of mobile robot combining binocular and depth cameras | |
Zhu et al. | Indoor multi-robot cooperative mapping based on geometric features | |
CN111462241A (zh) | 一种基于单目视觉的目标定位方法 | |
JP7145770B2 (ja) | 車間距離測定装置、誤差モデル生成装置および学習モデル生成装置とこれらの方法およびプログラム | |
CN103744110B (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 | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20190917 |