一种障碍物检测方法及系统与流程

本发明涉及智能检测领域，特别是涉及一种障碍物检测方法及系统。

背景技术：

现代机器人技术及无人驾驶技术发展迅速，随着技术的不断成熟，可自主导航及路径规划的机器人出现在人们的生活中。在享受机器人技术及无人驾驶技术带来的便利、提高生产效率的同时，机器人与机器人之间、机器人与人之间、机器人与建筑物之间等碰撞事故时有发生，如何避免机器人的碰撞事故成为了人们重点关注的问题。因此，如何在机器人自主行动过程中去对所处环境中的各类障碍物进行检测，成为解决碰撞事故的关键。常用障碍物检测系统是基于单/双目摄像头、毫米波雷达等传感器。

其中单目摄像头检测，是先通过图像匹配进行目标识别(各种障碍等)，再通过目标在图像中的大小去估算目标距离，需要建立并不断维护一个庞大的样本特征数据库，保证这个数据库包含待识别目标的全部特征数据。而双目摄像头检测，是通过对两幅图像视差的计算，直接对前方景物进行距离测量。无论单/双目摄像头都受环境影响较大，如环境的光暗状况、阴影、温湿度等的干扰，图像会存在不同程度的失真，另外计算量大，对计算单元的性能及算法要求较高。相比于单/双目摄像头，毫米波雷达具有更高的精度及分辨率，探测范围也更加广泛，但其探测距离受到频段损耗的直接制约，也无法感知行人，并且对周边所有障碍物无法进行精准的建模。目前也存在一些多传感器融合的检测系统，但其同样面临着计算量大、受环境制约等问题。

技术实现要素：

本发明的目的是提供一种障碍物检测方法及系统，以解决传统检测方法及系统中计算量大且受环境影响大的问题。

为实现上述目的，本发明提供了如下方案：

一种障碍物检测方法，所述方法包括：

获取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。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。