CN110673107B - 基于多线激光雷达的路沿检测方法及装置 - Google Patents

Abstract

本发明提供了一种基于多线激光雷达的路沿检测方法，包括：对每个路沿点的第一位置信息进行坐标转换，得到第二坐标系下，每个路沿点的第二位置信息；获取当前帧的激光点云信息；根据路沿点的第二位置信息和当前帧的激光点云信息，确定原始路沿点云信息；根据激光雷达的探头信息，对原始路沿点云信息中的路沿点进行聚类，得到聚类路沿点云信息；根据聚类点云信息中的路沿点的中心点坐标和路沿点的第二位置信息，对聚类路沿点云信息进行过滤，得到过滤的路沿点云信息；计算过滤的路沿点云信息的局部特征；根据局部特征，对过滤的路沿点云信息进行处理，得到目标路沿点；对多个目标路沿点进行拟合，得到路沿信息。该方法对环境要求第、鲁棒性高。

Description

基于多线激光雷达的路沿检测方法及装置

技术领域

本发明涉及数据处理技术领域，尤其涉及一种基于多线激光雷达的路沿 检测方法及装置。

背景技术

近年来智能驾驶技术的飞速发展，环境感知是智能驾驶系统中的重要组 成部分，道路信息提取是环境感知的重要研究内容，路沿是一种典型的道路 信息，检测路沿可以辅助智能驾驶系统实现多种复杂的功能和任务。

为了实现路沿检测的功能，研究者提出了多种解决方案，根据使用传感 器的不同，可以将解决方案分为两类，基于摄像头的路沿检测和基于激光雷 达的路沿检测。

在《智能车辆视觉导航中道路检测算法的研究》中，刘化胜等人提出了 基于摄像头的路沿检测算法，但是基于摄像头的路沿检测算法，容易受到天 气、光照、裂缝、水迹等因素的影响。

在《基于3D激光雷达的实时道路边沿检测算法》中，刘梓提出了基于激 光雷达的路沿检测算法，该算法没有考虑道路中存在的障碍物，抗干扰能力 较差。Rho提出了基于Hough变换的路沿检测算法，通过提取线段的断点，提 取出路沿信息，该方法要求地面平整，算法鲁棒性较差。

现有基于视觉信息和多线激光雷达信息的路沿检测算法，对路面环境要 求较高，鲁棒性较差，不能适应路面上存在障碍物的情况。而且计算复杂度 较高，对系统性能要求较高，很多算法不能满足实时性的要求。

发明内容

本发明实施例的目的是提供一种基于多线激光雷达的路沿检测方法，以 解决现有技术中的路沿检测方法，容易受到天气、光照、裂缝、水迹等因素 的影响、鲁棒性较差、计算复杂度较高，不能满足实时性的要求的问题。

为解决上述问题，本发明提供了一种基于多线激光雷达的路沿检测方法， 所述方法包括：

根据车辆的位置信息，获取第一坐标系下，预设的地图中的多个路沿点 中每个路沿点的第一位置信息；

对每个路沿点的第一位置信息进行坐标转换，得到第二坐标系下，每个 路沿点的第二位置信息；

获取当前帧的激光点云信息；

根据所述路沿点的第二位置信息和所述当前帧的激光点云信息，确定原 始路沿点云信息；

根据激光雷达的探头信息，对所述原始路沿点云信息中的路沿点进行聚 类，得到聚类路沿点云信息；

分别计算所述聚类点云信息中的各路沿点的中心点坐标；

根据所述聚类点云信息中的路沿点的中心点坐标和所述路沿点的第二位 置信息，对所述聚类路沿点云信息进行过滤，得到过滤的路沿点云信息；

计算所述过滤的路沿点云信息的局部特征；

根据所述局部特征，对所述过滤的路沿点云信息进行处理，得到目标路 沿点；

对多个目标路沿点进行拟合，得到路沿信息。

在一种可能的实现方式中，所述对每个路沿点的第一位置信息进行坐标 转换，得到第二坐标系下，每个路沿点的第二位置信息，具体包括：

利用公式

计算路沿点的第二位置信息；

其中，

和

分别为第一坐标系下的旋转矩阵和和平移矩阵；C_M为第 一坐标系下，车辆的位置信息；U_M为第一坐标系下，路沿点的第一位置信息；

为第二坐标系下的路沿点的第二位置信息。

在一种可能的实现方式中，所述根据所述路沿点的第二位置信息和所述 当前帧的激光点云信息，确定原始路沿点云信息具体包括：

根据所述路沿点的第二位置信息和所述当前帧的激光点云信息的重合区 域，提取与所述路沿点的距离在第一预设范围内的激光点云信息，得到原始 激光点云信息；其中，所述原始激光点云信息包含n₀个点

p_i＝(x_i，y_i，z_i，r_i)，(x_i，y_i，z_i)为点的空间坐标，r_i为当前点的激光探头信息。

在一种可能的实现方式中，所述局部特征包括径向梯度，所述计算所述 过滤的路沿点云信息的局部特征，具体包括：

利用公式

计算径向梯度；

其中，

为径向梯度，

和

分别为点云三维空间坐标x和y分量， k＝{2，2，2，2，2，0，-2，-2，-2，-2，-2}。在一种可能的实现方式中，所述根据所 述局部特征，对所述过滤的路沿点云信息进行处理，得到目标路沿点，具体 包括：

当所述过滤的路沿点云信息中的路沿点云的径向梯度大于预设的径向梯 度阈值时，保留所述路沿点云；

对保留下来的全部路沿点云依据高度进行排序，并取高度中位点；

以所述高度中位点为中心，选取与中位点的距离在第二预设范围的路沿 点云作为目标路沿点。

在一种可能的实现方式中，所述对多个目标路沿点进行拟合，得到路沿 信息，具体包括：

通过随机抽样一致算法，对所述目标路沿点进行过滤；

通过最小二乘法，对过滤后的目标路沿点进行拟合，得到路沿信息。

第二方面，本发明提供了一种基于多线激光雷达的路沿检测装置，所述 装置包括：

获取单元，所述获取单元用于根据车辆的位置信息，获取第一坐标系下， 预设的地图中的多个路沿点中每个路沿点的第一位置信息；

坐标转换单元，所述坐标转换单元用于对每个路沿点的第一位置信息进 行坐标转换，得到第二坐标系下，每个路沿点的第二位置信息；

所述获取单元还用于，获取当前帧的激光点云信息；

确定单元，所述确定单元用于根据所述路沿点的第二位置信息和所述当 前帧的激光点云信息，确定原始路沿点云信息；

聚类单元，所述聚类单元用于根据激光雷达的探头信息，对所述原始路 沿点云信息中的路沿点进行聚类，得到聚类路沿点云信息；

计算单元，所述计算单元用于分别计算所述聚类点云信息中的各路沿点 的中心点坐标；

过滤单元，所述过滤单元用于根据所述聚类点云信息中的路沿点的中心 点坐标和所述路沿点的第二位置信息，对所述聚类路沿点云信息进行过滤， 得到过滤的路沿点云信息；

所述计算单元还用于，计算所述过滤的路沿点云信息的局部特征；

处理单元，所述处理单元用于根据所述局部特征，对所述过滤的路沿点 云信息进行处理，得到目标路沿点；

拟合单元，所述拟合单元用于对多个目标路沿点进行拟合，得到路沿信 息。

第三方面，本发明提供了一种设备，包括存储器和处理器，所述存储器 用于存储程序，所述处理器用于执行第一方面任一所述的方法。

第四方面，本发明提供了一种包含指令的计算机程序产品，当所述计算 机程序产品在计算机上运行时，使得所述计算机执行如第一方面任一所述的 方法。

第五方面，本发明提供了一种计算机可读存储介质，所述计算机可读存 储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如第一方 面任一所述的方法。

应用本发明实施例提供的基于多线激光雷达的路沿检测方法及装置，能 够准确提取出路沿几何信息，且对环境要求较低，鲁棒性高，计算复杂度低， 能够满足实时性要求，提取出的路沿信息，可以辅助智能驾驶系统完成复杂 的任务，例如局部轨迹推算、贴边清扫等。

附图说明

图1为本发明实施例一提供基于多线激光雷达的路沿检测方法流程图；

图2为多线激光雷达线束分布后视图；

图3为多线激光雷达投影到地面的俯视图；

图4为利用路沿点的第二位置信息缩小路沿点云检测范围的示意图；

图5A为多线激光雷达投影到路沿点的后视图；

图5B为多线激光雷达投影到路沿点的后视图；

图6为本发明实施例二提供的基于多线激光雷达的路沿检测装置结构示 意图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是， 此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另 外还需要说明的是，为便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特 征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

图1为本发明实施例一提供基于多线激光雷达的路沿检测方法流程图。 该方法可以应用在安装有多线激光雷达的无人驾驶车辆或者机器人中，下面 以将该方法应用在无人驾驶车辆中为例进行说明。当将该方法应用在无人驾 驶车辆中时，该方法的执行主体为车辆的处理器。如图1所示，该方法包括 以下步骤：

步骤101，根据车辆的位置信息，获取第一坐标系下，预设的地图中的多 个路沿点中每个路沿点的第一位置信息。

具体的，第一坐标系为地图坐标系，车辆上安装有多线激光雷达和其它 传感器，比如，车辆上安装有全球定位系统(Global Positioning System， GPS)，可以通过GPS，得到车辆在地图坐标系下的位置信息。在预设的地图 中，会标注地图坐标系下路沿的位置和角度。路沿中每个路沿点都具有第一 位置信息。

由此，通过车辆中智能驾驶系统的处理器，可以依据预设的高精地图的 路沿点的第一位置信息和当前车辆的位置信息，计算出路沿点在车辆坐标系 下的第二位置信息，通过第二位置信息，可以缩小路沿检测的检测范围，提 高检测的精度和鲁棒性。

步骤102，对每个路沿点的第一位置信息进行坐标转换，得到第二坐标系 下，每个路沿点的第二位置信息。

具体的，可以利用公式

计算路沿点的第二位置信息；

其中，

和

分别为第一坐标系下的旋转矩阵和和平移矩阵；C_M为第 一坐标系下，车辆的位置信息；U_M为第一坐标系下，路沿点的第一位置信息；

为第二坐标系下的路沿点的第二位置信息。

其中，第二位置信息中可以是路沿点的空间三维坐标。

步骤103，获取当前帧的激光点云信息。

具体的，参见图2，多线激光雷达具有垂直排布的多个激光探头，激光探 头之间在垂直方向上角度均匀分布，激光探头采用飞行时间(time of fly， ToF)技术测量距离信息,激光雷达在运行过程中绕垂直轴高速旋转，旋转时 每个探头按照一定顺序高频测量环境信息。雷达旋转一圈，即可采集一圈周 围的环境数据，周围的环境信息以离散稀疏的三维空间坐标的形式提供，称 为原始激光点云信息。

如图3所示，多线激光雷达点云投射到平面上的点云，从俯视角度观察， 理论上是一圈完整的圆。

对于原始激光点云信息，可以根据多线激光雷达在车辆的安装位置，将 激光雷达坐标系下的原始激光点云信息转换为车辆坐标系下的激光点云信息。

步骤104，根据路沿点的第二位置信息和当前帧的激光点云信息，确定原 始路沿点云信息。

具体的，参见图4，根据路沿点的第二位置信息和当前帧的激光点云信息 的重合区域，提取与路沿点的距离在第一预设范围内的激光点云信息，得到 原始激光点云信息；其中，原始激光点云信息包含n₀个点

p_i＝(x_i，y_i，z_i，r_i)，(x_i，y_i，z_i)为点的空间坐标，r_i为当前点的激光探头信息，例 如16线激光雷达包含16根激光头，r_i＝{0，1，2，3，4，5，6，7，8，9，10，11，12，13，14，15}。 提取的原始激光点云信息的数量n₀远远小于激光点云信息的数量N。由此，通 过地图，缩小了激光点云信息的计算范围，提高了计算速度。

步骤105，根据激光雷达的探头信息，对原始路沿点云信息中的路沿点进 行聚类，得到聚类路沿点云信息。

步骤106，分别计算聚类点云信息中的路沿点的中心点坐标。

步骤107，根据聚类点云信息中的路沿点的中心点坐标和路沿点的第二位 置信息，对聚类路沿点云信息进行过滤，得到过滤的路沿点云信息。

具体的，根据原始路沿点云信息的r_i信息，将r_i值相同的路沿点云信息聚 类在一起，对聚类后的路沿点云信息分别计算中心点坐标，如果中心点坐标 与地图中路沿线段的距离太远，即不在预设的第二距离范围内时，则在路沿 点中删除该聚类后的路沿点云信息。由此，能够过滤大部分障碍物点云和被 障碍物遮挡的点云。

步骤108，计算过滤的路沿点云信息的局部特征。

步骤109，根据局部特征，对过滤的路沿点云信息进行处理，得到目标路 沿点。

具体的，参见图5A和5B，道路旁的路沿，高度高于路面，激光雷达投射 在高度不同的地面和路沿时，如图5A和5B所示，相邻点云的径向距离存在 跳变，且路沿上点云的高度高于地面上的点云。由此，可以利用点云的局部 特征，比如，径向距离梯度或者高度梯度，提取出投影在路沿边缘上的点云。

利用公式

计算径向梯度；

其中，

为径向梯度，

和

分别为点云三维空间坐标x和y分量， k＝{2，2，2，2，2，0，-2，-2，-2，-2，-2}。

之后，当过滤的路沿点云信息中的路沿点云的径向梯度大于预设的径向 梯度阈值时，保留路沿点云；

对保留下来的全部路沿点云依据高度进行排序，并取高度中位点；

以高度中位点为中心，选取与中位点的距离在第二预设范围的路沿点云 作为目标路沿点。

具体的，设定径向梯度阈值G_thresh，如果径向梯度

则保留 点云，否则舍弃点云。记录保存下来的点云

对保存下来的点

依据高度进行排序，选取高度中位点

以中位 点p_m为中心，选取中位点附近的点作为目标路沿点

步骤110，对多个目标路沿点进行拟合，得到路沿曲线。

具体的,对于目标路沿点

可以采用随机抽样一致(Random SampleConsensus，RANSAC)算法进行曲线拟合。

具体的，对RANSAC算法进行说明：

(1)随机选择一些原始数据组合，作为曲线拟合的原始点。选取的原始 数据，每个点的激光探头信息r_i都不一致。

(2)从组合中抽取一组，作为原始数据，建立曲线模型；

(3)用其它数据验证模型，计算其它点到曲线的距离；

(4)提取到曲线距离小于阈值的点，设置为内点，计算内点的数量和原 始点集；

(5)循环第二步至第四步，直到所有组合都被处理过；

(6)选出内点数最多的原始点集作为最佳拟合点集，并根据原始点集对 应的内点，通过最小二乘法拟合路沿曲线。

将路沿直线方程投影到x-y平面生成二维直线方程，根据二维直线方程 计算路沿直线方程与车辆坐标系y轴的交点p_y，路沿方向n，为以点向式表示 路沿信息(p_y，n)。

其中，在进行RANSAC曲线拟合采样点时，考虑每个点的先验信息，点的 激光探头信息r_i。采样点组合中的所有点，r_i必须不相同。该限制条件保证了 进行RANSAC拟合时，每个点之间的距离最大，拟合出来的直线更准确。根据 内点数选取最佳采样点，提取该最佳采样点的所有内点，利用这些内点采用 最小二乘法计算路沿的直线方程。

应用本发明实施例提供的基于多线激光雷达的路沿检测方法，能够准确 提取出路沿几何信息，且对环境要求较低，鲁棒性高，计算复杂度低，能够 满足实时性要求，提取出的路沿信息，可以辅助智能驾驶系统完成复杂的任 务，例如局部轨迹推算、贴边清扫等。

图6为本发明实施例二提供的基于多线激光雷达的路沿检测装置结构示 意图。该装置应用在实施例一中的基于多线激光雷达的路沿检测方法中，如 图6所示，该基于多线激光雷达的路沿检测装置600包括：获取单元601，坐 标转换单元602，确定单元603，聚类单元604，计算单元605，过滤单元606， 处理单元607和拟合单元608。

获取单元601用于根据车辆的位置信息，获取第一坐标系下，预设的地 图中的多个路沿点中每个路沿点的第一位置信息。

坐标转换单元602用于对每个路沿点的第一位置信息进行坐标转换，得 到第二坐标系下，每个路沿点的第二位置信息。

获取单元601还用于，获取当前帧的激光点云信息。

确定单元603用于根据路沿点的第二位置信息和当前帧的激光点云信息， 确定原始路沿点云信息。

聚类单元604用于根据激光雷达的探头信息，对原始路沿点云信息中的 路沿点进行聚类，得到聚类路沿点云信息。

计算单元605用于分别计算聚类点云信息中的各路沿点的中心点坐标。

过滤单元606用于根据聚类点云信息中的路沿点的中心点坐标和路沿点 的第二位置信息，对聚类路沿点云信息进行过滤，得到过滤的路沿点云信息。

计算单元605还用于，计算过滤的路沿点云信息的局部特征。

处理单元607用于根据局部特征，对过滤的路沿点云信息进行处理，得 到目标路沿点。

拟合单元608用于对多个目标路沿点进行拟合，得到路沿信息。

每个单元的具体作用以及该基于多线激光雷达的路沿检测装置的技术效 果与实施例一中的类似，此处不再赘述。

本发明实施例三提供了一种设备，包括存储器和处理器，存储器用于存 储程序，存储器可通过总线与处理器连接。存储器可以是非易失存储器，例 如硬盘驱动器和闪存，存储器中存储有软件程序和设备驱动程序。软件程序 能够执行本发明实施例提供的上述方法的各种功能；设备驱动程序可以是网 络和接口驱动程序。处理器用于执行软件程序，该软件程序被执行时，能够 实现本发明实施例提供的方法。

本发明实施例四提供了一种包含指令的计算机程序产品，当计算机程序 产品在计算机上运行时，使得计算机执行本发明实施例一提供的方法。

本发明实施例五提供了一种计算机可读存储介质，计算机可读存储介质 上存储有计算机程序，计算机程序被处理器执行时实现本发明实施例一提供 的方法。

专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的 各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来 实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能 一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来 执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每 个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为 超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理 器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器 (RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、 寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式 的存储介质中。

以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进 一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不 用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修 改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于多线激光雷达的路沿检测方法，其特征在于，所述方法包括：

根据车辆的位置信息，获取第一坐标系下，预设的地图中的多个路沿点中每个路沿点的第一位置信息；

对每个路沿点的第一位置信息进行坐标转换，得到第二坐标系下，每个路沿点的第二位置信息；

获取当前帧的激光点云信息；

根据所述路沿点的第二位置信息和所述当前帧的激光点云信息，确定原始路沿点云信息；

根据激光雷达的探头信息，对所述原始路沿点云信息中的路沿点进行聚类，得到聚类路沿点云信息；

分别计算所述聚类路沿点云信息中的各路沿点的中心点坐标；

根据所述聚类路沿点云信息中的路沿点的中心点坐标和所述路沿点的第二位置信息，对所述聚类路沿点云信息进行过滤，得到过滤的路沿点云信息；

计算所述过滤的路沿点云信息的局部特征；

根据所述局部特征，对所述过滤的路沿点云信息进行处理，得到目标路沿点；

对多个目标路沿点进行拟合，得到路沿信息。

2.根据权利要求1所述的方法，其特征在于，所述对每个路沿点的第一位置信息进行坐标转换，得到第二坐标系下，每个路沿点的第二位置信息，具体包括：

利用公式

计算路沿点的第二位置信息；

其中，

平移矩阵；

为第一坐标系下，车辆的位置信息；

为第一坐标系下，路沿点的第一位置信息；

为第二坐标系下的路沿点的第二位置信息。

3.根据权利要求1所述的方法，其特征在于，所述根据所述路沿点的第二位置信息和所述当前帧的激光点云信息，确定原始路沿点云信息具体包括：

根据所述路沿点的第二位置信息和所述当前帧的激光点云信息的重合区域，提取与所述路沿点的距离在第一预设范围内的激光点云信息，得到原始激光点云信息；其中，所述原始激光点云信息包含

个点

，

，

为点的空间坐标，

为当前点的激光探头信息。

4.根据权利要求1所述的方法，其特征在于，所述局部特征包括径向梯度，所述计算所述过滤的路沿点云信息的局部特征，具体包括：

计算径向梯度；

其中，

，

。

5.根据权利要求4所述的方法，其特征在于，所述根据所述局部特征，对所述过滤的路沿点云信息进行处理，得到目标路沿点，具体包括：

当所述过滤的路沿点云信息中的路沿点云的径向梯度大于预设的径向梯度阈值时，保留所述路沿点云；

对保留下来的全部路沿点云依据高度进行排序，并取高度中位点；

以所述高度中位点为中心，选取与中位点的距离在第二预设范围的路沿点云作为目标路沿点。

6.根据权利要求1所述的方法，其特征在于，所述对多个目标路沿点进行拟合，得到路沿信息，具体包括：

通过随机抽样一致算法，对所述目标路沿点进行过滤；

通过最小二乘法，对过滤后的目标路沿点进行拟合，得到路沿信息。

7.一种基于多线激光雷达的路沿检测装置，其特征在于，所述装置包括：

获取单元，所述获取单元用于根据车辆的位置信息，获取第一坐标系下，预设的地图中的多个路沿点中每个路沿点的第一位置信息；

坐标转换单元，所述坐标转换单元用于对每个路沿点的第一位置信息进行坐标转换，得到第二坐标系下，每个路沿点的第二位置信息；

所述获取单元还用于，获取当前帧的激光点云信息；

确定单元，所述确定单元用于根据所述路沿点的第二位置信息和所述当前帧的激光点云信息，确定原始路沿点云信息；

聚类单元，所述聚类单元用于根据激光雷达的探头信息，对所述原始路沿点云信息中的路沿点进行聚类，得到聚类路沿点云信息；

计算单元，所述计算单元用于分别计算所述聚类路沿点云信息中的各路沿点的中心点坐标；

过滤单元，所述过滤单元用于根据所述聚类路沿点云信息中的路沿点的中心点坐标和所述路沿点的第二位置信息，对所述聚类路沿点云信息进行过滤，得到过滤的路沿点云信息；

所述计算单元还用于，计算所述过滤的路沿点云信息的局部特征；

处理单元，所述处理单元用于根据所述局部特征，对所述过滤的路沿点云信息进行处理，得到目标路沿点；

拟合单元，所述拟合单元用于对多个目标路沿点进行拟合，得到路沿信息。

8.一种设备，其特征在于，所述设备包括存储器和处理器，所述存储器用于存储程序，所述处理器用于执行权利要求1-6任一所述的方法。

9.一种包含指令的计算机程序产品，其特征在于，当所述计算机程序产品在计算机上运行时，使得所述计算机执行权利要求1-6任一所述的方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-6任一所述的方法。

Images