CN111142116A - 一种基于三维激光的道路检测与建模方法 - Google Patents

Abstract

本发明公开了一种基于三维激光的道路检测与建模方法，包括以下步骤：获取激光点云数据；过滤单帧激光点云数据；提取单帧路面点云数据；绘制二维栅格地图；重复上述步骤直到信息采集结束；建模：根据二维栅格的状态进行道路建模。本发明克服了光照变化的影响，提高了道路检测与建模精度，使其能够遇障碍物自主绕行，从而提高机器人的导航精度。

Description

一种基于三维激光的道路检测与建模方法

技术领域

本发明涉及巡检机器人领域，具体涉及一种基于三维激光的道路检测与建模方法。

背景技术

近年来，随着科技的发展，变电站巡检机器人已经成为变电站设备巡检的重要辅助手段，巡检机器人利用传感器设备对变电站的道路环境信息进行采集，从而规划出巡检路径成为变电站巡检机器人实现自主巡检的关键问题。目前国内外已有的主流路面自动检测设备虽然已经实现了自动化的采集功能，但多数采用二维图像采集、检测技术对路面进行检测识别，不但容易受到雨雪、光照等天气条件的影响，而且检测结果并不生动直观。采用二维图像检测技术，一方面路面作为三维立体的客观实体，二维图像并不能完整的表现出路面的所有信息，这直接导致了识别检测精度降低；另一方面由于路面检测的数字图像处理技术往往达不到路面检测的精度要求，还是需要人工辅助来提高精确度，费时费力的缺点依旧存在。

针对以上问题，采用激光传感器采集变电站的环境信息，并建立环境地图，机器人根据建立的地图和自己在地图中的位置，进行巡检路径规划，然而由于外界环境的变化如道路两侧长草，巡检机器人的定位精度就会受到影响，而变电站的道路较窄，宽度在1.2～2.5m左右，机器人存在跌落到道路之外的情况。并且当机器人遇到障碍物时，不能够准确规划路线，自主绕行。

发明内容

为了解决上述问题，本发明提出了一种基于三维激光的道路检测与建模方法，包括以下步骤：

获取激光点云数据：搭载三维激光传感器的机器人进行道路信息采集，获取单帧激光点云数据；

过滤单帧激光点云数据为：基于领域点高度将单帧激光点云数据进行过滤，得到单帧路面待选点云数据；

提取单帧路面点云数据为：统计所有的单帧路面待选点云数据的曲率，过滤掉曲率和反射率发生明显变化的点，得到单帧路面点云数据；

绘制二维栅格地图：绘制二维栅格形成二维栅格地图，单帧路面点云数据映射到二维栅格中，并根据映射情况更新二维栅格的状态；

重复上述步骤直到信息采集结束；

建模：根据二维栅格的状态进行道路建模。

进一步地，过滤单帧激光点云数据具体为：

以激光传感器为原点O，机器人的行进方向为Y轴，与Y轴垂直的方向为X轴，Z轴向上，建立激光扫描坐标系，则其中的某一激光点p的坐标为p(x，y，z)；取与p点在同一扫描线的左右相邻点，p点前一扫描线和后一扫描线上离p点最近的点以及它们各自的左右相邻两点构成3×3点阵，计算该点阵的z值卷积和∑Z，对∑Z设定阈值Th，选取在∑Z在阈值Th范围内的点，得到单帧路面待选点云数据。

进一步地，点p(x，y，z)的坐标值x，y，z的取值如下：

其中，S为激光点p到原点0的距离，α为激光点p的水平方向角度值，θ为激光点p的竖直方向角度值。

卷积和∑Z的计算公式如下：

其中，Z₀为p点在激光扫描坐标系的z值，Z₁～Z₈为p点的相邻点在基于机器人的激光扫描坐标系中的z值；

进一步地，阈值Th的范围为

其中h为激光传感器的安装高度，β＝θ₁，θ₁为机器人所遇到的地面斜坡的倾角，地面斜坡的倾角由机器人采集得到。

进一步地，提取路面点云数据具体为：

计算路面待选点云数据中的某一帧扫描线的中间点P_m，以P_m为中心分别向两侧进行遍历，计算所有点的曲率，找到曲率变化最大的两点P_l、P_r，将P_l与P_r之间的点云数据按照邻接关系存入集合C；

统计集合C中的路面待选点云数据中所有扫描线上各点对应的反射率，过滤掉反射率发生明显变化的点，保留连续且具有近似反射率的点的点云数据，得到路面点云数据。

进一步地，提取路面点云数据具体为：

计算路面待选点云数据中所有扫描线上各点对应的反射率，过滤掉反射率发生明显变化的点，保留连续且具有近似反射率的点的点云数据,存入集合B；

计算集合B中的某一帧扫描线的中间点P_m，以P_m为中心分别向两侧进行遍历，计算所有点的曲率，找到曲率变化最大的两点P_l、P_r，将P_l与P_r之间的点云数据按照邻接关系存入集合B’，得到路面点云数据。

进一步地，绘制二维栅格地图具体为：

所述二维栅格的状态为二维栅格的置信度，各个栅格的置信度设置为P(x_i,y_i)＝0，各个栅格的置信度的初始值为0；

计算二维栅格在全局坐标系中的坐标G_i(x_i,y_i)，将路面点云数据中的各点的坐标通过坐标转换模块转换为各点在全局坐标系中的坐标p'(x',y',z')，计算各点在地图栅格中的位置；根据是否有激光点落入各个栅格更新各个栅格的置信度，若多个路面点对应同一地图栅格位置，仅随机取其中一个路面点对应的地图栅格在地图坐标系中的坐标；

进一步地，根据二维状态进行道路建模具体为：

将置信度大于阈值P_T的栅格标记为灰色，进而标记出变电站内的道路在栅格地图上的位置，道路建模完成。

进一步地，更新各个栅格相应的置信度的具体公式为：

当第一帧点云数据落入二维栅格时：

当第二帧点云数据落入二维栅格时：

当第n帧点云数据落入栅格时，

其中，

s和t的取值根据地面环境的稀疏程度进行调节。

进一步地，阈值P_T的取值范围为P_T∈(0.6，0.8)。

与现有技术相比，本发明具有以下有益效果：

1、本发明能够准确提取变电站的道路信息，有效防止机器人跌落的道路之外，并能够为机器人的智能避障提供路线支持，提高了机器人的栅格导航精度，增强了机器人对环境的适应性。

2、本发明克服了光照变化的影响，提高了道路检测与建模精度，使其能够遇障碍物自主绕行，从而提高机器人的导航精度。

附图说明

图1为单帧激光数据处理流程图；

图2为三维激光点云数据映射示意图；

图3为基于机器人的激光扫描坐标系图；

图4为领域的3×3点阵图；

图5为阈值示意图；

图6为变电站道路建模流程；

图7为变电站道路建模示意图；

图8为激光扫描示意图。

具体实施方式

下面结合附图，对本发明提出的基于三维激光传感器的道路检测与建模算法进行详细描述，具体实施步骤如下：

一、单帧激光数据处理流程

机器人搭载三维激光传感器在变电站内进行道路信息采集，三维激光传感器的型号为VLP-16，垂直视场为-15.0°至+15.0°，安装高度为h，h∈(40cm,60cm)，由于在道路信息采集的过程中，单帧激光虽然能同时发射16线激光，但16线的激光大概只有4至5线的激光能投射到地面上，故需过滤掉没有投射到地面的激光点云数据，如图7所示，过滤之后剩下垂直视场为-15.0°、-13.0°、-11.0°、-9.0°和-7.0°的5线激光，之后通过曲率与反射率相结合的检测算法检测出道路信息，最后采取概率栅格的方式对获取的道路数据进行建模，结合图1，具体步骤如下：

(1)基于领域点高度的单帧激光点云过滤

以激光传感器为原点，机器人的行进方向为y轴，与y轴垂直的方向为x轴，z轴向上，建立激光扫描坐标系，如图2所示，用S来表示三维空间中的某一激光点p到原点O的距离，α、θ为该激光的水平方向角度值和竖直方向角度值，点p(x，y，z)的坐标值x，y，z的取值如下所示。

如图3所示。利用相邻地面点的高程z值相近的特点，使用3×3点阵(见图4)的z值运算来实现地面点的探测，具体算法为：

将单帧激光点云数据中的任一激光点设为本点，取与本点在同一扫描线的左右相邻点，本点前一扫描线和后一扫描线上离本点最近的点以及它们各自的左右相邻两点构成3×3点阵(如图4所示)，计算该点阵的z值卷积和∑Z，计算公式如下：

其中，Z₀为本点在激光扫描坐标系的z值，Z₁～Z₈为本点的相邻点在基于机器人的激光扫描坐标系中的z值。

对于∑Z设定阈值Th，∑Z在阈值范围内时，将该点阵中的所有点均标记为地面点。结合图5，当机器人遇到斜坡时，点P被投射到斜坡面上，P^z是点P投影到机器人激光传感器z轴上的投影点，θ₁是斜坡的倾斜角，θ₁由机器人中的倾角仪采集得到，阈值Th的计算方法如下：

当机器人上坡时，

故OP^z＝h-PP′tanβ，其中β＝θ₁，h为激光传感器的安装高度，阈值Th的下限值为

同理当机器人下坡时，可得阈值Th的下限值为

即

时，将该点阵中的所有点均标记为地面点，遍历当前帧中所有的激光点云数据，标记出当前帧中所有的地面待选点，作为下一步路面点云数据提取的数据输入。

(2)单帧路面点云数据提取

变电站的路宽大约为1.2～2.5m左右水泥路，两侧长有杂草或为土地，为了正确检测出道路信息，需要提取出投射在道路表面的激光点云数据，计算某一扫描线上各点的曲率，检测出该扫描线上曲率变化最大的两点P_l、P_r，保留P_l与P_r之间的激光点云，将P_l与P_r之间的点云数据存入集合C。由于不同材质的物体对应的激光反射率不同，根据该特性统计集合C中各点的反射率，进而提取出最终的路面点云数据。

①基于曲率的单帧路面点云数据提取

在某一扫描线上确定该扫描线的中间点P_m，确定P_m的方法为：比较各点到z轴的距离，距离最小的点，即|z_m+h|最小的点定为P_m，以P_m为中心分别向两侧进行遍历，计算所有点的曲率，找到曲率变化最大的两点P_l、P_r。

假设P_m(x_m，y_m)，P₁(x₁，y₁)，P₂(x₂，y₂)为连续的三个激光数据点，三点构建的圆心(x₀，y₀)的圆：

x₀＝(a-b+c)/d，y₀＝(e-f+g)/-d

式中：a＝(x_m+x₂)(x₂-x_m)(y₃-y₂)

b＝(x₂+x₃)(x₃-x₂)(y₂-y_m)

c＝(y_m-y₃)(y₂-y_m)(y₃-y₂)

d＝2[(x₂-x_m)(y₃-y₂)-(x₃-x₂)(y₂-y_m)]

e＝(y_m+y₂)(y₂-y_m)(x₃-x₂)

f＝(y₂+y₃)(y₃-y₂)(x₂-x_m)

g＝(x₁-x₃)(x₂-x₁)(x₃-x₂)

P_m(x_m，y_m)，P₁(x₁，y₁)，P₂(x₂，y₂)三点的曲率分别为：

由于基于扫描线的数据是按照邻接关系存储的，通过上述方法，以P_m为中心分别向两侧进行遍历，计算各点的曲率，同时比较相邻两点曲率的变化程度，以P_m为中心向右各点的曲率为(c_r1，c_r2，c_r3…c_rn)；

当|c_a-c_b|＜ε_c，(c_a，c_b为相邻两点的曲率，ε_c∈(0.01，0.1))时，保留各曲率所对应的点，当不符合该情况的点出现时，将该点定为P_r，停止曲率比较。同理以P_m为中心向左遍历确定P_l，保留P_m与P_l之间的点，将P_l与P_r之间的点云数据按照邻接关系存入集合C。

②基于反射率的单帧路面点云数据提取

激光反射率是扫描目标对三维激光传感器激光束的反射回波功率，包含了目标表面的特征信息，一般情况下，同类型的物体具有近似的反射率特征。根据不同材质具有不同反射率特征这个特点，对集合C中的单帧路面点云数据进行进一步检测。统计所有扫描线上各点对应的反射率特征，过滤掉反射率发生明显变化的点，保留连续且具有近似反射率的点，即各连续点满足|r_a-r_b|＜ε_r，(r_a，r_b为相邻两点对应的反射率，ε_r∈(0.01，0.1))时，将各点标记为变电站的最终路面点，获得单帧路面点云数据。

由于基于曲率提取单帧路面点云数据是基于点云数据的几何特征，而基于反射率提取单帧路面点云数据是基于点云数据的物理特征，两者是互相不干扰的提取，所以上述对单帧路面点云数据的提取也可以先基于反射率提取再基于曲率提取，对应的提取公式不变。

(3)单帧激光数据的变电站道路建模

利用概率栅格对变电站道路进行建模，首先在变电站所在的道路平面上绘制二维栅格，并求得每一栅格在全局坐标系中的坐标G_i(x_i，y_i)，设各个栅格的置信度为P_n(x_i，y_i)，n为自然数，表示当前的点云数据为第n帧的点云数据，初始置信度设为0。将单帧激光点云数据中的路面点云，通过坐标转换模块，计算当前单帧路面点云数据的各点在全局坐标系中的坐标p′(x′，y′，z′)，不考虑当前单帧路面点云数据的各点的Z轴坐标，将各点垂直映射到相对应的地图栅格位置，(即计算每个路面点在地图栅格中的位置)。若其中有重复落在同一地图栅格位置的多个路面点，仅随机取其中一个路面点对应的地图栅格在地图坐标系中的坐标。根据映射情况更新地图栅格的状态，即根据是否有路面点云落入栅格更新相应栅格的置信度。

重复上述的获取激光点云数据、过滤单帧激光点云数据和提取单帧路面点云数据步骤，不断更新地图栅格的状态，直到信息采集结束。

更新各个栅格的具体规则为：

第一帧激光点云数据落入二维平面栅格时，更新各个栅格相应的置信度：

当第二帧激光数据落到二维栅格平面时，根据以下公式更新各个栅格的置信度：

当第n帧激光点云数据落入二维平面栅格时，各个栅格的置信度为：

其中，各个栅格的状态更新系数为：

s和t的取值根据地面环境的稀疏程度进行调节。

由上述公式可知，最后，对于路面点云占据的栅格的置信度将趋近于1，未被路面点云占据的栅格的置信度将趋近于0。

二、对变电站内的路面进行道路建模

结合图6，机器人在变电站内对道路信息采集结束后，根据最终得到的各个栅格的的置信度，将置信度大于阈值P_T的栅格标记为灰色，P_T∈(0.6，0.8)，进而标记出变电站内的道路在栅格地图上的位置，道路建模完成，变电站建模示意图如图7所示。

本发明克服了光照变化的影响，提高了道路检测与建模精度，使其能够遇障碍物自主绕行，从而提高机器人的导航精度。

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

Claims (10)

1.一种基于三维激光的道路检测与建模方法，其特征在于，包括以下步骤：

获取激光点云数据：搭载三维激光传感器的机器人进行道路信息采集，获取单帧激光点云数据；

过滤单帧激光点云数据：基于领域点高度将单帧激光点云数据进行过滤，得到单帧路面待选点云数据；

提取单帧路面点云数据：统计所有的单帧路面待选点云数据的曲率，过滤掉曲率和反射率发生明显变化的点，得到单帧路面点云数据；

绘制二维栅格地图：绘制二维栅格形成二维栅格地图，单帧路面点云数据映射到二维栅格中，并根据映射情况更新二维栅格的状态；

重复上述步骤直到信息采集结束；

建模：根据二维栅格的状态进行道路建模。

2.根据权利要求1所述的基于三维激光的道路检测与建模方法，其特征在于，所述过滤单帧激光点云数据具体为：

以激光传感器为原点O，机器人的行进方向为Y轴，与Y轴垂直的方向为X轴，Z轴向上，建立激光扫描坐标系，则其中的某一激光点p的坐标为p(x,y,z)；取与p点在同一扫描线的左右相邻点，p点前一扫描线和后一扫描线上离p点最近的点以及它们各自的左右相邻两点构成3×3点阵，计算该点阵的z值卷积和∑Z，对∑Z设定阈值Th，选取在∑Z在阈值Th范围内的点，得到单帧路面待选点云数据。

3.根据权利要求2所述的基于三维激光的道路检测与建模方法，其特征在于，所述点p(x,y,z)的坐标值x,y,z的取值如下：

其中，S为激光点p到原点O的距离，α为激光点p的水平方向角度值，θ为激光点p的竖直方向角度值；

卷积和∑Z的计算公式如下：

其中，Z₀为p点在激光扫描坐标系的z值，Z₁～Z₈为p点的相邻点在基于机器人的激光扫描坐标系中的z值。

4.根据权利要求2所述的基于三维激光的道路检测与建模方法，其特征在于，所述阈值Th的范围为

其中h为激光传感器的安装高度,β＝θ₁,θ₁为机器人所遇到的地面斜坡的倾角，地面斜坡的倾角由机器人采集得到。

5.根据权利要求2所述的基于三维激光的道路检测与建模方法，其特征在于，所述提取路面点云数据具体为：

计算路面待选点云数据中的某一帧扫描线的中间点P_m，以P_m为中心分别向两侧进行遍历，计算所有点的曲率，找到曲率变化最大的两点P_l、P_r，将P_l与P_r之间的点云数据按照邻接关系存入集合C；

统计集合C中的路面待选点云数据中所有扫描线上各点对应的反射率，过滤掉反射率发生明显变化的点，保留连续且具有近似反射率的点的点云数据，得到路面点云数据。

6.根据权利要求2所述的基于三维激光的道路检测与建模方法，其特征在于，所述提取路面点云数据具体为：

计算路面待选点云数据中所有扫描线上各点对应的反射率，过滤掉反射率发生明显变化的点，保留连续且具有近似反射率的点的点云数据,存入集合B；

计算集合B中的某一帧扫描线的中间点P_m，以P_m为中心分别向两侧进行遍历，计算所有点的曲率，找到曲率变化最大的两点P_l、P_r，将P_l与P_r之间的点云数据按照邻接关系存入集合B’，得到路面点云数据。

7.根据权利要求5或6所述的基于三维激光的道路检测与建模方法，其特征在于，所述绘制二维栅格地图具体为：

所述二维栅格的状态为二维栅格的置信度，各个栅格的置信度设置为P(x_i,y_i)＝0，各个栅格的置信度的初始值为0；

计算二维栅格在全局坐标系中的坐标G_i(x_i,y_i)，将路面点云数据中的各点的坐标通过坐标转换模块转换为各点在全局坐标系中的坐标p'(x',y',z')，计算各点在地图栅格中的位置；根据是否有激光点落入各个栅格更新各个栅格的置信度，若多个路面点对应同一地图栅格位置，仅随机取其中一个路面点对应的地图栅格在地图坐标系中的坐标。

8.根据权利要求7所述的基于三维激光的道路检测与建模方法，其特征在于，所述根据二维状态进行道路建模具体为：

将置信度大于阈值P_T的栅格标记为灰色，进而标记出变电站内的道路在栅格地图上的位置，道路建模完成。

9.根据权利要求7所述的基于三维激光的道路检测与建模方法，其特征在于，所述更新各个栅格相应的置信度的具体公式为：

当第一帧点云数据落入二维栅格时：

当第二帧点云数据落入二维栅格时：

当第n帧点云数据落入栅格时，

其中，

s和t的取值根据地面环境的稀疏程度进行调节。

10.根据权利要求9所述的基于三维激光的道路检测与建模方法，其特征在于，阈值P_T的取值范围为P_T∈(0.6，0.8)。

Images