CN110441791A - 一种基于前倾2d激光雷达的地面障碍物检测方法 - Google Patents

Abstract

本发明公开一种基于前倾2D激光雷达的地面障碍物检测方法，包括以下步骤：定义障碍物类别；获取机器人当前所处环境的三维点云信息；基于获取的三维点云信息，分割聚类障碍物点云信息；基于定义的障碍物类别，对获取的每一障碍物点云信息进行分类。本发明根据障碍物的倾斜角度将其分为可行障碍物、不可行障碍物，从而可检测到斜坡、减速带等障碍物，检测精度高，且只需imu、里程计及激光雷达即可实现，成本低廉，泛用性强。

Description

一种基于前倾2D激光雷达的地面障碍物检测方法

技术领域

本发明涉及地面障碍物检测技术领域，尤其涉及一种基于前倾2D激光雷达的地面障碍物检测方法。

背景技术

近些年，随着人工智能浪潮的兴起，移动机器人得到了迅猛发展，特别是室内移动机器人已经发展得较为成熟，涌现了很多方案提供商和开源方案。而室外移动机器人发展较为缓慢，主要原因在于室外环境的复杂性，其中就包括地面的复杂性。不能有效的处理好地面的复杂性问题，而是简单地选择以水平地面来代替复杂地面，将会导致机器人在遇到减速带或斜坡时不能有效减速，从而造成倾倒危险，遇到较厚的砖块或台阶等应该避开的障碍物不能有效避开，从而造成轮子卡住的问题。这些问题严重影响了室外移动机器人运行的稳定性。

为了解决这些问题，当前主要的方法是通过深度学习来识别图片中是否含有减速带、砖块、台阶等物体，该方法有以下不足之处：

1)环境适应性较差，晚上几乎无法使用；

2)方法精准度不高，图像缺乏尺度信息，因此不能准确定位地面障碍物的位置；

3)成本高昂，需要为图像学习配备性能较高的显卡。

另一种方法是通过单线激光雷达进行检测，能够有效的避免基于图像的方法带来的适应性差、尺度和成本高的问题。但该方法都是基于单线数据根据线段分割的方法确定地面点和非地面点，并且将非地面点作为地面障碍物。该方法主要的不足之处就在于无法有效的区分非地面点类型，非地面点包括减速带、斜坡等可行点，台阶、砖块等不可行点。因此，该方法会将斜坡和减速带认为是障碍物，这将引起识别的不准确性，从而造成整个移动机器人导航的不准确性。

综上可知，所述地面障碍物检测方法，实际中存在不便的问题，所以有必要加以改进。

发明内容

本发明的目的是提供一种基于前倾2D激光雷达的地面障碍物检测方法，该方法根据障碍物的倾斜角度将其分为可行障碍物、不可行障碍物，从而可检测到斜坡、减速带等障碍物，检测精度高，且只需imu、里程计及激光雷达即可实现，成本低廉，泛用性强。

为实现上述目的，采用以下技术方案：

一种基于前倾2D激光雷达的地面障碍物检测方法，包括以下步骤：

S1：定义障碍物类别；

S2：获取机器人当前所处环境的三维点云信息；

S3：基于S2获取的三维点云信息，分割聚类得到障碍物点云信息；

S4：基于S1定义的障碍物类别，对S3获取的每一障碍物点云信息进行分类。

进一步地，所述S1步骤中的定义障碍物类别包括以下步骤：

S11：预设障碍物面向机器人一侧的倾斜角度阈值；

S12：将倾斜角度小于或等于倾斜角度阈值的障碍物定义为可行障碍物，将倾斜角度大于倾斜角度阈值的障碍物定义为不可行障碍物。

进一步地，所述S2步骤中的获取机器人当前所处环境的三维点云信息包括以下步骤：

S21：通过imu和里程计获取机器人在起始坐标系下的局部运动轨迹信息；

S22：通过激光雷达采集机器人所处环境的每一单帧的三维点云信息；

S23：基于S21获取的局部运动轨迹信息，对S22采集的每一单帧三维点云信息进行矫正；

S24：将S23获取的矫正后的每一单帧三维点云信息累加合成机器人当前所处环境的三维点云信息。

进一步地，所述S21步骤中的获取机器人在起始坐标系下的局部运动轨迹信息包括以下步骤：

S211：将机器人启动时所处的位姿作为起始坐标系；

S212：通过imu采集机器人运动过程中的每一时刻的姿态角度信息；

S213：通过里程计采集机器人运动过程中的每一时刻的速度信息；

S214：基于S212采集的姿态角度信息、以及S213获取的速度信息，获取机器人运动过程中每一时刻的位姿信息；

S215：将S214获取的机器人每一时刻的位姿信息累加合成局部运动轨迹信息。

进一步地，所述S23步骤中的对采集的每一单帧三维点云信息进行矫正包括以下步骤：

S231：根据激光雷达每一单帧采集三维点云信息的时间戳，及每一单帧接收到的激光点的数量，得到每一单帧内每一激光点接收时的时间戳；

S232：基于S231得到的每一激光点接收时的时间戳及S21获取的机器人在每一单帧采集时间内的位姿信息，推算得到每一激光点接收时对应机器人的位姿信息；

S233：基于S232得到的每一激光点接收时对应机器人的位姿信息，将每一激光点接收时的点信息同步至激光雷达单帧输出的点云信息内。

进一步地，所述S3步骤中的分割聚类障碍物点云信息包括以下步骤：

S31：基于体素滤波法对S2获取的三维点云信息进行降采样处理；

S32：基于直通滤波法在降采样处理后的三维点云信息中滤除地面点云信息；

S33：基于欧几里得聚类法分割聚类得到障碍物点云信息。

进一步地，所述S31步骤中的体素滤波法对三维点云信息降采样处理包括以下步骤：

S311：将三维点云信息进行体素化；

S312：计算每一体素内的所有点的质心坐标的平均值，并将该平均值作为对应体素的质心坐标；

所述S32步骤中的直通滤波法滤除地面点云信息包括以下步骤：

S321：预设地面高度阈值；

S322：将低于地面高度阈值的点云归类为在地面采集的点云，并滤除，将高于地面高度阈值的点云保留；

所述S33步骤中的欧几里得聚类法分割聚类得到障碍物点云信息包括以下步骤：

S331：预设第一距离阈值；

S332：将S32保留后的点之间的距离与第一距离阈值比较，将小于第一距离阈值的点归类为障碍物点云。

进一步地，所述S4步骤中的对每一障碍物点云信息进行分类包括以下步骤：

S41：基于区域增长算法对每一障碍物点云进行平面分割；

S42：基于平面拟合算法对每一分割平面进行拟合，同时计算拟合后的分割平面的倾斜角度；

S43：将拟合后的每一分割平面的倾斜角度与倾斜角度阈值进行比较，若其中一分割平面的倾斜角度大于倾斜角度阈值，则将该障碍物点云归类为不可行障碍物，若每一分割平面的倾斜角度均小于或等于倾斜角度阈值，则将该障碍物点云归类为可行障碍物。

进一步地，所述S41步骤中的区域增长算法进行平面分割包括以下步骤：

S411：预设第一角度阈值；

S412：计算每一障碍物点云所有点的法向量，并以其中若干点作为参考点，计算每一参考点相邻的所有的点与该参考点之间法向量之间的夹角，将夹角小于第一角度阈值的相邻点归类为同一平面，进而实现对障碍物点云的平面分割。

进一步地，所述S42步骤中的平面拟合算法拟合分割平面及计算分割平面的倾斜角度包括以下步骤：

S421：预设一个平面作为参考平面；

S422：计算每一分割平面内所有点到参考平面的距离差的平方和，根据最小二乘法计算每一分割平面的最优表达式，进而实现对分割平面的拟合；

S423：计算拟合后的分割平面的法向量，进而获得拟合后分割平面的倾斜角度。

采用上述方案，本发明的有益效果是：

1)根据障碍物的倾斜角度将其分为可行障碍物、不可行障碍物，从而可检测到斜坡、减速带等障碍物，检测区域大、检测精度高、实现机器人的精确导航；

2)只需imu、里程计及激光雷达即可实现地面障碍物检测，成本低廉，泛用性强；

3)方法简单高效，并可矫正激光雷达单帧采集三维点云信息产生的畸变，提高检测精度。

附图说明

图1为本发明的流程性框图；

图2为本发明的获取机器人当前所处环境的点云信息的流程性框图；

图3为本发明的获取机器人局部运动轨迹信息的流程性框图；

图4为本发明的对每一单帧三维点云信息进行矫正的流程性框图；

图5为本发明的分割聚类障碍物点云信息的流程性框图；

图6为本发明的对每一障碍物点云信息进行分类的流程性框图；

图7为本发明的平面拟合算法的流程性框图；

图8为本发明一实施例中验证该方法的实物图；

图9为本发明一实施例中获得的三维点云信息图；

图10为对图9进行分割聚类障碍物的信息图；

图11为对图10进行障碍物分类的信息图；

其中，附图标识说明：

1—减速带； 2—斜坡；

3—地面。

具体实施方式

以下结合附图和具体实施例，对本发明进行详细说明。

参照图1至7所示，本发明提供一种基于前倾2D激光雷达的地面障碍物检测方法，包括以下步骤：

S1：定义障碍物类别；

S2：获取机器人当前所处环境的三维点云信息；

S3：基于S2获取的三维点云信息，分割聚类得到障碍物点云信息；

S4：基于S1定义的障碍物类别，对S3获取的每一障碍物点云信息进行分类。

其中，所述S1步骤中的定义障碍物类别包括以下步骤：

S11：预设障碍物面向机器人一侧的倾斜角度阈值；

S12：将倾斜角度小于或等于倾斜角度阈值的障碍物定义为可行障碍物，将倾斜角度大于倾斜角度阈值的障碍物定义为不可行障碍物。

所述S2步骤中的获取机器人当前所处环境的三维点云信息包括以下步骤：

S21：通过imu和里程计获取机器人在起始坐标系下的局部运动轨迹信息；

S22：通过激光雷达采集机器人所处环境的每一单帧的三维点云信息；

S23：基于S21获取的局部运动轨迹信息，对S22采集的每一单帧三维点云信息进行矫正；

S24：将S23获取的矫正后的每一单帧三维点云信息累加合成机器人当前所处环境的三维点云信息。

所述S21步骤中的获取机器人在起始坐标系下的局部运动轨迹信息包括以下步骤：

S211：将机器人启动时所处的位姿作为起始坐标系；

S212：通过imu采集机器人运动过程中的每一时刻的姿态角度信息；

S213：通过里程计采集机器人运动过程中的每一时刻的速度信息；

S214：基于S212采集的姿态角度信息、以及S213获取的速度信息，获取机器人运动过程中每一时刻的位姿信息；

S215：将S214获取的机器人每一时刻的位姿信息累加合成局部运动轨迹信息。

所述S23步骤中的对采集的每一单帧三维点云信息进行矫正包括以下步骤：

S231：根据激光雷达每一单帧采集三维点云信息的时间戳，及每一单帧接收到的激光点的数量，得到每一单帧内每一激光点接收时的时间戳；

S232：基于S231得到的每一激光点接收时的时间戳及S21获取的机器人在每一单帧采集时间内的位姿信息，推算得到每一激光点接收时对应机器人的位姿信息；

S233：基于S232得到的每一激光点接收时对应机器人的位姿信息，将每一激光点接收时的点信息同步至激光雷达单帧输出的点云信息内。

所述S3步骤中的分割聚类障碍物点云信息包括以下步骤：

S31：基于体素滤波法对S2获取的三维点云信息进行降采样处理；

S32：基于直通滤波法在降采样处理后的三维点云信息中滤除地面点云信息；

S33：基于欧几里得聚类法分割聚类得到障碍物点云信息。

所述S31步骤中的体素滤波法对三维点云信息降采样处理包括以下步骤：

S311：将三维点云信息进行体素化；

S312：计算每一体素内的所有点的质心坐标的平均值，并将该平均值作为对应体素的质心坐标；

所述S32步骤中的直通滤波法滤除地面点云信息包括以下步骤：

S321：预设地面高度阈值；

S322：将低于地面高度阈值的点云归类为在地面采集的点云，并滤除，将高于地面高度阈值的点云保留；

所述S33步骤中的欧几里得聚类法分割聚类得到障碍物点云信息包括以下步骤：

S331：预设第一距离阈值；

S332：将S32保留后的点之间的距离与第一距离阈值比较，将小于第一距离阈值的点归类为障碍物点云。

所述S4步骤中的对每一障碍物点云信息进行分类包括以下步骤：

S41：基于区域增长算法对每一障碍物点云进行平面分割；

S42：基于平面拟合算法对每一分割平面进行拟合，同时计算拟合后的分割平面的倾斜角度；

S43：将拟合后的每一分割平面的倾斜角度与倾斜角度阈值进行比较，若其中一分割平面的倾斜角度大于倾斜角度阈值，则将该障碍物点云归类为不可行障碍物，若每一分割平面的倾斜角度均小于或等于倾斜角度阈值，则将该障碍物点云归类为可行障碍物。

所述S41步骤中的区域增长算法进行平面分割包括以下步骤：

S411：预设第一角度阈值；

S412：计算每一障碍物点云所有点的法向量，并以其中若干点作为参考点，计算每一参考点相邻的所有的点与该参考点之间法向量之间的夹角，将夹角小于第一角度阈值的相邻点归类为同一平面，进而实现对障碍物点云的平面分割。

所述S42步骤中的平面拟合算法拟合分割平面及计算分割平面的倾斜角度包括以下步骤：

S421：预设一个平面作为参考平面；

S422：计算每一分割平面内所有点到参考平面的距离差的平方和，根据最小二乘法计算每一分割平面的最优表达式，进而实现对分割平面的拟合；

S423：计算拟合后的分割平面的法向量，进而获得拟合后分割平面的倾斜角度。

本发明工作原理：

定义障碍物类别：对于室外移动机器人，为了能够保证在不同路面采取不同移动策略，需要对地面进行归一化分类。室外地面主要包括水平地面、斜坡、减速带、台阶、砖块以及各种零散的杂物，而对于机器人来说，只在乎哪些地面区域可直接正常通过，哪些障碍物可以减速压过去，哪些障碍物必须避开。因此可以将地面分为两类，正常行走路面和障碍物路面，障碍物路面又可分为两类，将能减速碾压过去的障碍物称为可行障碍物，将不能碾压必须绕开的障碍物称为不可行障碍物。对于一个地面障碍物，判断机器人能否减速通过的一个重要标准是该障碍物朝向机器人的一面的倾斜角大小，例如减速带、斜坡等地面障碍物的倾斜角较小，机器人能够直接碾压，只是需要减速，而台阶、砖块等地面障碍物的倾斜角较大，机器人不能直接碾压，否则会出现轮子卡住的问题。因此，基于地面障碍物表面倾斜角的大小(比较障碍物倾斜角度与倾斜角度阈值的大小)，将小于或等于倾斜角度阈值的障碍物定义为可直接碾压的障碍物，即可行障碍物；而将大于倾斜角度阈值的障碍物定义为必须绕开的障碍物，即不可行障碍物。

获取机器人当前所处环境的三维点云信息：

1)获取机器人局部运动轨迹信息：

定义机器人启动时所处的位姿为起始坐标系，后面计算的轨迹都是相对于该坐标系，原因是因为此时的imu(惯性测量装置)和里程计的信息都为初始状态，可直接利用imu(采集机器人姿态角度变化信息)和里程计(机器人速度信息)推算机器人位姿而不需要补偿。设置imu和里程计的发布频率相同，这样能保证两者数据的同步。

机器人的位姿信息有5个变量，实时位置信息(以起始坐标系为参考的，当前所处位置的x、y坐标，这个信息采集不到，需要计算得到)，在起始坐标系为参考的实时偏转角度信息，以及x、y两个方向的速度信息(以此计算机器人的位置信息)。首先，通过里程计获取机器人的实时运行速度信息(v_x,v_y)，imu获得机器人实时的偏转角θ_t(姿态角度信息)，然后利用如下公式，得到机器人的实时位置信息：

式中，(x_t-1,y_t-1,θ_t-1)分别为机器人当前时刻的和上一时刻的位置信息及偏转角度信息。

例如，当机器人启动行走，imu和里程计第一次采集数据后，需要计算得到当前机器人所处的位置(以起始坐标系为参考的x、y坐标)，此时，机器人上一时刻的位置在起始位置，即(x_t-1,y_t-1,θ_t-1)＝(0,0,0)，此时，imu和里程计采集到了机器人的速度信息及角度信息，利用位移与速度、时间及偏转角度的关系，

x_t＝v_x×Δt×cosθ_t-1+v_y×Δt×sinθ_t-1

y_t＝v_x×Δt×sinθ_t-1+v_y×Δt×cosθ_t-1

即可计算得到当前机器人所处的位置信息及偏转角度信息(x_t,y_t,θ_t)；以此类推，机器人继续行走，imu和里程计第二次采集相关数据后，将第一次(x_t,y_t,θ_t)的信息带入上述公式，计算机器人第二次行走后所处的位置信息及偏转角度信息，重复上述步骤，就可以增量式的更新机器人当前的位姿信息，进而有效的模拟出机器人在起点出发之后形成的局部运动轨迹信息T_raj，

T_raj＝((x₁,y₁,θ₁)，(x₂,y₂,θ₂)，……)。

2)对激光雷达采集的每一单帧三维点云信息进行矫正：

由于imu和里程计的更新频率都是50Hz，故其周期为0.02s，故在0.1秒内imu和里程计可采集机器人运动过程中的5个位姿信息(上述的位置信息及偏转角度信息)，而激光雷达的更新频率一般在10Hz左右，其周期为0.1s，故在0.1s内激光雷达只能采集机器人周围环境的单帧三维点云信息(即一次信息)，即在相同时间内(0.1s)，imu和里程计采集了5次的机器人的位姿信息，而激光雷达只采集了周围环境的一次单帧的点云信息，如果将该单帧三维点云信息不经过矫正处理，那就默认该单帧三维点云信息是机器人在一个固定位置处采集的，这与0.1s内机器人获得的5个位姿信息是不匹配的，机器人在0.1s内是移动的，因此，需要通过这个5个位姿信息对单帧三维点云信息进行矫正(激光雷达每一单帧都会接收很多激光点，激光点数量是已知的，每一激光点都会采集一环境信息(未知)，通过激光点数量，计算每一激光点接收的时间戳，然后依据5个位姿信息，利用插值法推算出每一激光点采集的点云信息，最后将每一激光点采集的点云信息同步到单帧三维点云信息获取时的时刻，从而完成对单帧三维点云信息的矫正)。具体地：

首先依据下式，计算单帧内每一激光点接收时的时间戳：

式中，t_c为要计算的每一激光点接收的时间戳，t_p为获取单帧三维点云信息的时间戳，n_p为每一单帧接收的激光点数量，i为要计算的激光点在所有激光点序列中的索引。由此，可计算得到单帧内每一激光点的接收时时间戳。

然后利用下式，确定激光点接收时时间戳所处的相邻位姿信息获取的时间间隔内：

t_c∈(t_i,t_i+1)(i＝1,2,3,4)

其中，t_i为获取5个位姿信息时的时间戳，5个位姿信息，故有四个位姿时间间隔，将激光点分类至对应的时间间隔内。

然后，通过插值法推算每一激光点接收时，机器人的位姿信息：

式中，(x_c,y_c,θ_c)为利用插值法推算得到的每一激光点获取时的机器人的位姿信息，(x_i,y_i,θ_i)为5个已知位姿信息中的第i个位姿信息，利用上式就可以推算出每一激光点获取时机器人的位姿信息，然后依据每一机器人的位姿信息将对应激光点获取的点信息(周围环境信息)同步到同一时刻(即激光雷达输出点云的时刻)，满足单帧点云在固定坐标系下获取的条件，减小因机器人位姿移动而造成的点云精度降低的影响。其计算方法如下：

式中，(p_cx,p_cy,p_cz)为矫正后的激光点获取时的对应环境信息坐标(三维空间坐标，x、y、z)，(p_x,p_、y,p_z)为矫正前的激光点获取时的对应环境信息坐标，(x_g,y_g,θ_g)为最后一个激光点插值获取的机器人位姿信息，利用上式，即可将每一激光点获取的点信息同步至激光雷达输出点云的时刻，进而实现对每一单帧点云信息的矫正，得到矫正后的单帧三维点云信息：

((p_1x,p_1y,p_1z)，(p_2x,p_2y,p_2z)，……)。

3)将获取的单帧三维点云信息累加合成三维点云信息：

为了能够通过点云信息识别出机器人周边的障碍物，需要将激光雷达每一单帧采集的三维点云信息累加合成整体的三维点云信息。由于机器人只接触其周边的地面障碍物信息，因此只需构建局部三维点云信息(可根据时间、环境自定义边界范围)即可，能够有效的降低处理时间。具体的：

将上述获取的矫正后的单帧三维点云信息及该信息获取时对应的机器人的位姿信息，作为一个数据帧(P,T)，其中，

P＝{(p_1x,p_1y,p_1z),(p_2x,p_2y,p_2z)……}

T＝(x_g,y_g,θ_g)

为了能实时获取机器人周围的三维环境信息，需要将机器人当前所处位置时的数据帧以及之前一定时间内(预设值，比如10s以内)的获取数据帧累加才能形成稠密的点云信息，之后才能进行地面识别。其累加方法如下：

假设机器人当前所处位置获取的数据帧为(P_t,T_t)，那么，其之前一定时间内获取的数据帧分别为(P_t-1,T_t-1)，(P_t-2,T_t-2)，(P_t-3,T_t-3)……，现以将数据帧(p_t-1,T_t-1)累加到数据帧(P_t,T_t)为例，利用下式进行说明，具体地

其中，

(p_cix,p_ciy,p_ciz)为数据帧(p_t-1,T_t-1)累加到当前数据帧(P_t,T_t)中的第i(i是该帧内激光点的序列索引)个激光点的点信息；

(p_ix,p_iy,p_iz)为数据帧(p_t-1,T_t-1)中的第i个激光点的点信息；

(x_t,y_t,θ_t)为当前数据帧(P_t,T_t)获取时机器人的位姿信息；

(x_t-1,y_t-1,θ_t-1)为(P_t-1,T_t-1)数据帧获取时的机器人的位姿信息；

利用上述方法，即可遍历(P_t-1,T_t-1)中的每一个激光点的信息，进而有效地将数据帧(p_t-1,T_t-1)的每一激光点信息累加到当前帧上。而依据该方法，可依次将其余时刻获取的数据帧层层叠加至当前数据帧内，进而形成机器人当前周围环境的三维点云信息。

分割聚类障碍物点云信息：

为了能够区分三维点云信息中有哪些物体，需要对点云信息进行分割。具体的：

首先由于获取的三维点云信息的密度较大，使用体素滤波法对三维点云信息进行降采样处理，以提高点云处理的实时性，其原理是将三维点云信息体素化，计算每一体素内的所有点质心坐标的平均值，并将该平均值作为对应体素的质心坐标，实现了将多个点转换为一个点，从而实现了点云数量的降低。具体地，

首先获取三维点云信息中的边界信息，

(x_max,y_max,z_max),(x_min,y_min,z_min)

即点云信息中的坐标的最大值及最小值，然后根据下式，计算三维点云信息体素化后的数量，

式中，(n_x,n_y,n_z)分别表示x,y,z方向上的体素数量，r_es为体素的分辨率，一般设置为0.01米。

每一体素内包含的n个点信息((x₁,y₁,z₁),(x₂,y₂,z₂)…(x_n,y_n,z_n))，然后利用下式计算其平均值将其合成一个点(x,y,z)，

即对应体素的质心坐标，通过上式，可计算每一体素的质心坐标，然后以该质心坐标代替体素内的n个点信息，降低点云数量。

为了能够有效的将点云信息归类，需要将地面点云滤除，滤除的地面点云所在的区域即为可正常行走区域。采用直通滤波法在降采样处理后的三维点云信息中滤除地面点云信息；其原理是预设地面高度阈值，将低于地面高度阈值的点云归类为在地面采集的点云，并滤除，将高于地面高度阈值的点云保留(由于激光雷达相对于地面的位置信息是已知的，所以可以通过设置地面高度阈值将地面采集的点云滤除)。具体地，按如下公式对点云是否位于地面进行分类，

p∈G(p_z<t_thresh)

p∈G_n(p_z＞t_thresh)

式中，G,G_n分别为地面采集的点云信息集合，非地面采集的点云信息集合(高于地面)，p是点云中单个点，p_z是单个点的z轴坐标，t_thresh为设定的地面高度阈值(根据环境预设定)，由此，将每个点的z轴坐标与t_thresh比较，进而实现将地面点云滤除。

最后，再采用欧几里得聚类方法对点云进行聚类即可获得分割出的聚类物体，即障碍物点云，其原理是预设第一距离阈值及点云数量阈值，将点云之间的距离与第一距离阈值比较，将小于第一距离阈值的点云归为一类，归类为障碍物点云。以此就能有效的将地面上的点云进行分开，每个障碍物点云生成一个单独的聚类对象。具体地，

先取点云中的一个点p(p_x,p_y,p_z)的坐标，利用下式，计算相邻点到该点的距离，

式中，p_i(p_ix,p_iy,p_iz)为点云中的第i个点，d_i为第i个点到点p的距离。如果d_i小于给定的第一距离阈值，则p_i与p为同一个聚类，否则为不同聚类。通过此方法可以有效的获得基于p点的聚类。剔除基于p点的聚类，在余下的点中上述过程，获得基于其它点的聚类，反复此过程，最终将点云分割成多个独立的聚类。

对每一障碍物点云信息进行分类：

在获取了多个障碍物点云后，需要对障碍物点云进行分类，确定出可行和不可行地障碍物，具体的：

首先利用区域增长算法对每个障碍物点云进行平面分割，其原理是预设第一角度阈值，计算每一障碍物点云所有点的法向量，并以其中若干点作为参考点，计算每一参考点相邻的所有的点与该参考点之间法向量之间的夹角，将夹角小于第一角度阈值的相邻点归类为同一平面，进而实现对障碍物点云的平面分割。近距离的法向量差异较小的点归为一个区域，于是就可以将一个对象分割成多个独立的平面。具体地，

依据上述计算相邻点距离的方法，获取距离每个点最近的三个点，然后根据三个点的坐标，利用如下方法解算出三个点拟合的平面，

假设平面公式为，

ax+by+cz+1＝0

于是有：

ax₁+by₁+cz₁+1＝0

ax₂+by₂+cz₂+1＝0

ax₃+by₃+cz₃+1＝0

基于上述三个方程即可解得平面公式，于是可得平面法向量为(a,b,c)，将此法向量作为该点的法向量。然后通过同样的方法遍历求出每个点的法向量。

然后，以其中若干法向量为参考，利用下式计算相邻点之间法向量的夹角，

式中，(a₁,b₁,c₁)，(a₂,b₂,c₂)分别为相邻的两个点的法向量，θ为其之间的夹角。设定第一角度阈值为θ_thresh，当θ>θ_thresh时，将相邻两点认为是不同区域，反之认为相邻两点认为是相同区域。依据此方法，遍历点云中的每个点，从而能够有效的将聚类点云分成不同的区域(不同的分割平面，但该平面的具体表达式未知，需要拟合出其表达式，然后算出其倾斜角度)。

然后通过平面拟合算法获得拟合的平面表达式，其原理是首先给定一个平面估计值(参考面)，计算每一区域内所有点云到参考平面的距离差的平方和，最后再利用最小二乘法计算出最优的平面表达式(即将点云拟合于一平面内)，如下：

假设，

ax+by+cz+d＝0

根据区域内点的坐标可得如下方程组：

根据最小二乘法解算即可获得最优的拟合平面法向量(a,b,c)，再根据以下公式：

即可获得分割平面的倾斜角。

将每一障碍物点云的每一分割平面的倾斜角度与倾斜角度阈值进行比较，若其中一分割平面的倾斜角度大于倾斜角度阈值，则将该障碍物点云归类为不可行障碍物，若每一分割平面的倾斜角度均小于或等于倾斜角度阈值，则将该障碍物点云归类为可行障碍物，进而实现了对地面障碍物的识别。

为验证本发明方法的有效性，建立如图8所示一实施例的模型图，该模型包含减速带1，两个斜坡2及地面3；如图9所示，为采用本方法获得三维点云信息图，从图中可以清楚的看到，减速带1、两个斜坡2及地面3的轮廓；如图10所示，为将地面3信息去除，可以很清楚得到上述障碍物的点云信息；图11中的，斜坡2表面涂有黑色的印记，是对障碍物进行分类后的结果，表面颜色为黑色，代表倾斜角度大于倾斜角度阈值，归为不可行障碍物。

以上仅为本发明的较佳实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，包括以下步骤：

S1：定义障碍物类别；

S2：获取机器人当前所处环境的三维点云信息；

S3：基于S2获取的三维点云信息，分割聚类得到障碍物点云信息；

S4：基于S1定义的障碍物类别，对S3获取的每一障碍物点云信息进行分类。

2.根据权利要求1所述的基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，所述S1步骤中的定义障碍物类别包括以下步骤：

S11：预设障碍物面向机器人一侧的倾斜角度阈值；

S12：将倾斜角度小于或等于倾斜角度阈值的障碍物定义为可行障碍物，将倾斜角度大于倾斜角度阈值的障碍物定义为不可行障碍物。

3.根据权利要求2所述的基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，所述S2步骤中的获取机器人当前所处环境的三维点云信息包括以下步骤：

S21：通过imu和里程计获取机器人在起始坐标系下的局部运动轨迹信息；

S22：通过激光雷达采集机器人所处环境的每一单帧的三维点云信息；

S23：基于S21获取的局部运动轨迹信息，对S22采集的每一单帧三维点云信息进行矫正；

S24：将S23获取的矫正后的每一单帧三维点云信息累加合成机器人当前所处环境的三维点云信息。

4.根据权利要求3所述的基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，所述S21步骤中的获取机器人在起始坐标系下的局部运动轨迹信息包括以下步骤：

S211：将机器人启动时所处的位姿作为起始坐标系；

S212：通过imu采集机器人运动过程中的每一时刻的姿态角度信息；

S213：通过里程计采集机器人运动过程中的每一时刻的速度信息；

S214：基于S212采集的姿态角度信息、以及S213获取的速度信息，获取机器人运动过程中每一时刻的位姿信息；

S215：将S214获取的机器人每一时刻的位姿信息累加合成局部运动轨迹信息。

5.根据权利要求4所述的基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，所述S23步骤中的对采集的每一单帧三维点云信息进行矫正包括以下步骤：

S231：根据激光雷达每一单帧采集三维点云信息的时间戳，及每一单帧接收到的激光点的数量，得到每一单帧内每一激光点接收时的时间戳；

S232：基于S231得到的每一激光点接收时的时间戳及S21获取的机器人在每一单帧采集时间内的位姿信息，推算得到每一激光点接收时对应机器人的位姿信息；

S233：基于S232得到的每一激光点接收时对应机器人的位姿信息，将每一激光点接收时的点信息同步至激光雷达单帧输出的点云信息内。

6.根据权利要求5所述的基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，所述S3步骤中的分割聚类障碍物点云信息包括以下步骤：

S31：基于体素滤波法对S2获取的三维点云信息进行降采样处理；

S32：基于直通滤波法在降采样处理后的三维点云信息中滤除地面点云信息；

S33：基于欧几里得聚类法分割聚类得到障碍物点云信息。

7.根据权利要求6所述的基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，所述S31步骤中的体素滤波法对三维点云信息降采样处理包括以下步骤：

S311：将三维点云信息进行体素化；

S312：计算每一体素内的所有点的质心坐标的平均值，并将该平均值作为对应体素的质心坐标；

所述S32步骤中的直通滤波法滤除地面点云信息包括以下步骤：

S321：预设地面高度阈值；

S322：将低于地面高度阈值的点云归类为在地面采集的点云，并滤除，将高于地面高度阈值的点云保留；

所述S33步骤中的欧几里得聚类法分割聚类得到障碍物点云信息包括以下步骤：

S331：预设第一距离阈值；

S332：将S32保留后的点之间的距离与第一距离阈值比较，将小于第一距离阈值的点归类为障碍物点云。

8.根据权利要求7所述的基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，所述S4步骤中的对每一障碍物点云信息进行分类包括以下步骤：

S41：基于区域增长算法对每一障碍物点云进行平面分割；

S42：基于平面拟合算法对每一分割平面进行拟合，同时计算拟合后的分割平面的倾斜角度；

S43：将拟合后的每一分割平面的倾斜角度与倾斜角度阈值进行比较，若其中一分割平面的倾斜角度大于倾斜角度阈值，则将该障碍物点云归类为不可行障碍物，若每一分割平面的倾斜角度均小于或等于倾斜角度阈值，则将该障碍物点云归类为可行障碍物。

9.根据权利要求8所述的基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，所述S41步骤中的区域增长算法进行平面分割包括以下步骤：

S411：预设第一角度阈值；

S412：计算每一障碍物点云所有点的法向量，并以其中若干点作为参考点，计算每一参考点相邻的所有的点与该参考点之间法向量之间的夹角，将夹角小于第一角度阈值的相邻点归类为同一平面，进而实现对障碍物点云的平面分割。

10.根据权利要求9所述的基于前倾2D激光雷达的地面障碍物检测方法，其特征在于，所述S42步骤中的平面拟合算法拟合分割平面及计算分割平面的倾斜角度包括以下步骤：

S421：预设一个平面作为参考平面；

S422：计算每一分割平面内所有点到参考平面的距离差的平方和，根据最小二乘法计算每一分割平面的最优表达式，进而实现对分割平面的拟合；

S423：计算拟合后的分割平面的法向量，进而获得拟合后分割平面的倾斜角度。