一种激光雷达点云数据障碍检测算法的制作方法

本发明涉及一种激光雷达点云数据障碍检测算法。

背景技术：

激光雷达作为一种主动传感器，具有对物体的感知信息来源于自身，受外界环境影响很小，在深度信息的获取上，其可靠性和精确性要高于被动传感器的特点，因而其被广泛应用于环境感知系统。

无人车为高速移动机器人,实时性要求极高。而三维激光雷达的原始数据量过于庞大,如直接在原始数据上进行后续处理,难以达到实时性的要求。故需要一种高效快速的激光雷达点云数据处理算法。

基于栅格地图的表示是目前最常用的一种三维数据的表示方法，这一类方法对于每个栅格都只是保存了部分数据信息。使得需要处理的数据量变小。

栅格障碍检测虽然处理数据的效率比较高，但主要缺点有:由于多个障碍物之间遮挡,断裂以及远处栅格因缺失部分三维点云而出现漏检的问题。

技术实现要素：

针对以上现有技术问题，一种新的激光雷达点云数据障碍检测算法，保证障碍物检测准确性的同时实现高速处理。

具体技术方案如下：

一种激光雷达点云数据障碍检测算法，其步骤为：

(1)获取原始数据并解析；

(2)栅格地图投射算法；

(3)一级栅格信息提取；

(4)二级栅格建立及信息提取；

(5)障碍物判定：采用分块邻域膨胀历便算法；

(6)得出障碍物坐标；

进一步地：障碍物判定分块领域膨胀历便算法的具体步骤为：

(1)建立一级栅格；

(2)将一级栅格投射到二级栅格；

(3)二级栅格进行八邻域膨胀；

(4)计算膨胀后二级格栅的最小高度；

(5)遍及一级栅格，计算一级栅格与所属膨胀二级栅格的高度差；

(6)超过高度阈值的一级栅格属性设为障碍物；

(7)障碍物栅格计算直角坐标系坐标；

进一步地，获取原始数据并解析中：通过实时的数据包，获得多维激光雷达原始数据，每个数据包都包含有每一激光束返回的距离信息和角度信息，定义激光雷达旋转一周输出的数据为一帧数据，经过对原始数据的解析，每个激光雷达点数据包含三维空间位置坐标、垂直角度、水平角度、强度、以及激光线束；

进一步地，一级栅格信息提取包括：建立一级栅格地图grid[(m+1),(n+1)]，栅格大小为g，将直角坐标系下的三维点投射到(m,n)栅格平面上，每个栅格都只是保存了最大高度zmax，最小高度zmin，计算三维点的栅格坐标(i，j)数据信息；

进一步地，计算三维点的栅格坐标(i，j)数据信息的示例性投射算法为：i＝x/g+sign(x)/2+m/2；j＝y/g+sign(y)/2+n/3，此算法表示装载激光雷达的移动载体处于一级栅格中的位置为x方向的中心，y方向的1/3处；

进一步地，统计落在一级栅格内的雷达三维点,对于最小高度zmin超过2.5m的障碍点,视为悬挂障碍物进行剔除。

进一步地，二级栅格建立及信息提取包括：根据一级栅格建立二级栅格地图secgrid[m/3,n/3]，横坐标ii[0,m/3],纵坐标jj[0,n/3]，设置高度阀值t1，遍历一级栅格，对一级栅格所属的二级栅格进行八邻域膨胀，计算膨胀后二级栅格中的最小高度；

进一步地，计算一级栅格的最大高度zmax^i，j及所属膨胀二级栅格最小高度templow之间高度差，若zmax^i，j－templow≥t1，则一级栅格属性设为障碍物；

进一步地，栅格属性为障碍物的栅格转化为直角坐标系坐标的算法为：

floatx＝(i-m/2)*g–sign(i-m/2)*g/2

floaty＝(j-n/3)*g–sign(j-n/3)*g/2。

本发明与现有技术相比，具有如下优点和效果：

通过获取激光点云数据，建立两级栅格地图，首先对悬挂物进行剔除再通过对二级栅格进行八邻域膨胀，遍历一级栅格，分区计算高度差的算法对障碍物进行判定。本发明既保留了栅格法快速稳定的特点，又解决了多个障碍物之间遮挡,断裂以及远处栅格因缺失部分三维点云而出现漏检的问题。

附图说明

图1为本发明处理流程示意图

图2为分块邻域膨胀遍历算法处理流程示意图

图3为八邻域膨胀模板

图4为普通栅格算法得出的障碍物示意图(白色1为障碍物属性)

图5为本发明算法得出的障碍物示意图(白色1为障碍物属性)

具体实施方式

以下结合实施例，对本发明进行进一步的详细说明。

通过实时的udp数据包，获得多维激光雷达原始数据。每个数据包都包含有每一激光束返回的距离信息和角度信息。我们定义激光雷达旋转一周输出的数据为一帧数据。

经过对原始数据的解析，每个激光雷达点数据包含三维空间位置坐标(x,y,z)，垂直角度verangle，水平角度horangle，强度intensity，以及激光线束id。

建立一级栅格地图grid[(m+1),(n+1)]，栅格大小为g。将直角坐标系下的三维点投射到(m,n)栅格平面上。每个栅格都只是保存了最大高度zmax，最小高度zmin。计算三维点的栅格坐标(i，j)(0≤i≤m；0≤j≤n)数据信息，使得需要处理的数据量变小。示例性投射算法：i＝x/g+sign(x)/2+m/2；j＝y/g+sign(y)/2+n/3。

此算法表示装载激光雷达的移动载体处于一级栅格中的位置为x方向的中心，y方向的1/3处。

统计落在一级栅格内的雷达三维点(x,y,z),对于最小高度zmin超过2.5m的障碍点,视为悬挂障碍物进行剔除。

根据一级栅格建立二级栅格地图secgrid[m/3,n/3]，横坐标ii[0,m/3],纵坐标jj[0,n/3]。

设置高度阀值t1，遍历一级栅格，对一级栅格所属的二级栅格进行八邻域膨胀，计算膨胀后二级栅格中的最小高度。计算一级栅格的最大高度zmaxi，j及所属膨胀二级栅格最小高度templow之间高度差。若zmaxi，j-templow≥t1，则一级栅格属性设为障碍物。

分块邻域膨胀遍历算法伪代码为

把栅格属性为障碍物的栅格转化为直角坐标系坐标(floatx，floaty)。算法为：

floatx＝(i-m/2)*g–sign(i-m/2)*g/2

floaty＝(j-n/3)*g–sign(j-n/3)*g/2

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种改进，或未经改进直接应用于其它场合的，均在本发明的保护范围之内。