CN112711027B - 一种基于激光雷达点云数据的隧道内横向定位方法 - Google Patents

Abstract

本发明公开了一种基于激光雷达点云数据的隧道内横向定位方法。首先通过激光雷达对环境进行扫描，获取环境点云图，再剔除掉一些坏点与非平面点，通过对点云进行计算求得距离激光雷达最近的平面点的距离，此距离是隧道的一侧墙面与激光雷达之间的距离；然后计算水平旋角相差180°的另一侧扫描点云与激光雷达的距离，即可实现高精度的隧道内横向定位。同时利用隧道宽度恒定的特点，可防止极端环境下，大型障碍物遮挡激光雷达视角后导致无法横向定位的情况。应用本发明的测量方法实现的隧道内横向定位精度可达到厘米级精度，完全能够满足定位需求。

Description

一种基于激光雷达点云数据的隧道内横向定位方法

技术领域

本发明涉及激光雷达定位方法，尤其涉及一种基于激光雷达点云数据的隧道内横向定位方法。

背景技术

激光雷达定位技术于20世纪90年代开始应用于AGV(Automated Guided Vehicle)导航系统，早期的激光雷达导航系统需要在AGV行驶的路径周围安装精确的反射板。后经技术的发展，激光雷达技术逐渐完善，形成当前的一个体积小、易于安装与携带的激光雷达传感器。

激光雷达在工作时不受有源干扰的影响，且以激光为测量手段，所以有数据精度高、实时性好、数据稳定等特点。基于激光雷达的定位技术，在过去的几十年中，研究人员提出了很多基于激光传感器的2D SLAM建图系统，如GMapping、Hector SLAM和2DCartographer等。基于重建好的地图，定位系统在环境中的位姿成为纯定位。后又有研究人员提出了基于滤波的算法如AMCL、NDT-MCL和Kalman Filter等，不过这都局限于前提是基于二维地图，无法获取环境的三维信息，在复杂的道路环境下无法实现精确定位。

经过现代激光SLAM技术的发展，3D激光SLAM技术成为目前最先进的SLAM技术之一，主流的3D激光SLAM算法有基于关键点的LOAM、以及其改进版LeGO_LOAM和A_LOAM、还有LIO等主流算法，但是在目前的隧道场景下，这类算法都受到了极大的挑战，因为隧道环境是特征严重缺失的环境，在该环境下算法内提取的特征无法进行正确的匹配，导致定位信息的缺失，无法在隧道中得到高精度定位。

发明内容

发明目的：针对以上问题，本发明提出一种基于激光雷达点云数据的隧道内横向定位方法，利用激光雷达的高精度测量获取车辆在隧道内的横向定位信息，为隧道内车辆定位提供了高精度的横向定位信息。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于激光雷达点云数据的隧道内横向定位方法，具体方法如下：

(1)获取激光雷达测量得到的隧道点云数据，并根据曲率将激光雷达点划分为平面点和非平面点，并剔除掉非平面点；

所述点云数据的前一帧点云数据，其中，每一帧点云数据是由所述激光雷达按照扫描角度范围完成一个周期的扫描得到的；

所述每一帧点云数据包括所述激光雷达在对应的扫描角度下测量得到的与障碍物的距离以及激光雷达点；

(2)设置激光雷达的最大仰角为θ，从步骤(1)中选择仰角为θ的激光雷达得到的点云数据进行计算，得到激光雷达在仰角为θ，不同水平旋角下测量得到的与障碍物的距离的水平值，并从中选择数值最小的m个，同时将其对应的点云数据构成集合{D}；

其中，计算公式：

d^γ＝l_θ·cosθ

式中，θ为激光雷达的最大仰角；γ为激光雷达的水平旋角；d^γ为激光雷达在仰角为θ、水平旋角为γ的角度下测量得到的与障碍物的距离的水平值；l_θ为激光雷达在仰角为θ、水平旋角为γ的角度下测量得到的与障碍物的距离；{D}为d^γ值最小的m个激光雷达的点云数据集合；m为集合{D}的长度；

(3)为防止因路面的不平整因素或激光雷达的侧倾导致激光雷达偏移，从步骤(1)中选择仰角为α，α<θ的激光雷达得到的点云数据；

(4)对于步骤(2)所述集合{D}中的每一个激光雷达点，均从步骤(3)中选择与其相同水平旋角的点云数据进行计算，得到激光雷达在该水平旋角下测量得到的与障碍物的距离的水平值d^l；计算公式：

式中，α为激光雷达的仰角，且α<θ；a为仰角为θ，水平旋角为γ的激光雷达点与仰角为α，水平旋角为γ的激光雷达点的距离；l_α为激光雷达在仰角为α、水平旋角为γ的角度下测量得到的与障碍物的距离；d^l为激光雷达在水平旋角γ下测量得到的与障碍物的距离的水平值；

(5)根据下列公式对步骤(4)所述参数d^l进行计算，取最小值作为激光雷达与障碍物之间的确定距离值；其中，计算公式：

d＝min_ld^l

式中，d为激光雷达与障碍物的确定距离值；

(6)根据步骤(5)所述参数d的所对应的激光雷达的水平旋角确定障碍物的位置；参数d所对应的激光雷达的水平旋角用表示；若水平旋角/>在0°-180°范围内，则表示障碍物位于激光雷达的右侧；若水平旋角/>在-180°-0°范围内，则表示障碍物位于激光雷达的左侧；

(7)从步骤(1)中选择激光雷达在仰角为θ、水平旋角为的角度下测量得到的与障碍物的距离l_θ1，以及激光雷达在仰角为α，α<θ、水平旋角为/>的角度下测量得到的与障碍物的距离l_α1，通过下列公式计算得到激光雷达与障碍物另一侧的距离值；所述另一侧为步骤(6)确定得到的障碍物的位置的相对侧；

其中，计算公式：

式中，α为激光雷达的仰角，且α<θ；a′为仰角为θ，水平旋角为的激光雷达点与仰角为α，水平旋角为/>的激光雷达点的距离；l_α1为激光雷达在仰角为α、水平旋角为的角度下测量得到的与障碍物的距离；d^l′为激光雷达在水平旋角/> 下测量得到的与障碍物的距离的水平值；

(8)分别判断激光雷达与障碍物左右两侧的距离值是否发生突变，并将未发生突变的距离值作为激光雷达与隧道墙壁的距离完成隧道内横向定位，具体包括：

若左右两侧的距离值均未发生突变，则表示激光雷达与障碍物左右两侧的距离值为激光雷达与隧道两侧墙壁的距离值，由此确定隧道内的横向定位；

若仅有左侧的距离值发生突变，则将未发生突变的右侧的距离值作为激光雷达与隧道右侧墙壁的距离；并根据隧道横向宽度值一定，得到激光雷达与隧道左侧墙壁的距离，由此完成隧道内对车辆的横向定位；

若仅有右侧的距离值发生突变，则将未发生突变的左侧的距离值作为激光雷达与隧道左侧墙壁的距离；并根据隧道横向宽度值一定，得到激光雷达与隧道左侧墙壁的距离，由此完成隧道内对车辆的横向定位；

进一步的，激光雷达与障碍物左右两侧的距离值是否发生突变的判断方法；

设置阈值，将步骤(5)与步骤(7)得到的激光雷达与障碍物的距离值分别与前一帧点云数据处理得到的相应的距离值相比较；若绝对差值大于阈值，则表示发生突变；否则表示没有发生突变；

进一步的，步骤(1)所述曲率计算公式：

式中，c为激光雷达点的曲率；S为同一激光束获得的连续五个点的点集合；|S|表示集合的S长度；表示在雷达坐标系下，第k帧中的第i个点的坐标；/>表示在雷达坐标系下，第k帧中的第j个点的坐标。

在一种优选方案中，激光雷达的最大仰角为15度。

有益效果：本发明提出了一种基于激光雷达点云数据的隧道内横向定位方法，通过对激光点云的分类，提取出面特征点，计算出激光雷达与隧道两侧墙壁的距离，从而提供精确的横向定位信息。在环境特征极度缺失的隧道环境下，通过对激光雷达点云进行的处理，不需要进行大量的计算，即可快速获得高精度的隧道内的横向定位信息，从而完成精确的隧道内定位。

附图说明

图1是本发明的技术方案流程图；

图2是防止遮挡的测距原理图；

图3是防止激光雷达摆放不平整的测距原理图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

本发明所述的一种基于激光雷达点云数据的隧道内横向定位方法，具体方法如下：

(1)获取激光雷达测量得到的隧道点云数据，并根据曲率将激光雷达点划分为平面点和非平面点，并剔除掉非平面点；

所述点云数据的前一帧点云数据，其中，每一帧点云数据是由所述激光雷达按照扫描角度范围完成一个周期的扫描得到的；

所述每一帧点云数据包括所述激光雷达在对应的扫描角度下测量得到的与障碍物的距离以及激光雷达点；

(2)设置激光雷达的最大仰角为θ，从步骤(1)中选择仰角为θ的激光雷达得到的点云数据进行计算，得到激光雷达在仰角为θ，不同水平旋角下测量得到的与障碍物的距离的水平值，并从中选择数值最小的m个，同时将其对应的点云数据构成集合{D}；

其中，计算公式：

d^γ＝l_θ·cosθ

式中，θ为激光雷达的最大仰角；γ为激光雷达的水平旋角；d^γ为激光雷达在仰角为θ、水平旋角为γ的角度下测量得到的与障碍物的距离的水平值；l_θ为激光雷达在仰角为θ、水平旋角为γ的角度下测量得到的与障碍物的距离；{D}为d^γ值最小的m个激光雷达的点云数据集合；m为集合{D}的长度；

(3)为防止因路面的不平整因素或激光雷达的侧倾导致激光雷达偏移，从步骤(1)中选择仰角为α，α<θ的激光雷达得到的点云数据；

(4)对于步骤(2)所述集合{D}中的每一个激光雷达点，均从步骤(3)中选择与其相同水平旋角的点云数据进行计算，得到激光雷达在该水平旋角下测量得到的与障碍物的距离的水平值d^l；计算公式：

式中，α为激光雷达的仰角，且α<θ；a为仰角为θ，水平旋角为γ的激光雷达点与仰角为α，水平旋角为γ的激光雷达点的距离；l_α为激光雷达在仰角为α、水平旋角为γ的角度下测量得到的与障碍物的距离；d^l为激光雷达在水平旋角γ下测量得到的与障碍物的距离的水平值；

(5)根据下列公式对步骤(4)所述参数d^l进行计算，取最小值作为激光雷达与障碍物之间的确定距离值；其中，计算公式：

d＝min_ld^l

式中，d为激光雷达与障碍物的确定距离值；

(6)根据步骤(5)所述参数d的所对应的激光雷达的水平旋角确定障碍物的位置；参数d所对应的激光雷达的水平旋角用表示；若水平旋角/>在0°-180°范围内，则表示障碍物位于激光雷达的右侧；若水平旋角/>在-180°-0°范围内，则表示障碍物位于激光雷达的左侧；

(7)从步骤(1)中选择激光雷达在仰角为θ、水平旋角为的角度下测量得到的与障碍物的距离l_θ1，以及激光雷达在仰角为α，α<θ、水平旋角为/>的角度下测量得到的与障碍物的距离l_α1，通过下列公式计算得到激光雷达与障碍物另一侧的距离值；所述另一侧为步骤(6)确定得到的障碍物的位置的相对侧；

其中，计算公式：

式中，α为激光雷达的仰角，且α<θ；a′为仰角为θ，水平旋角为的激光雷达点与仰角为α，水平旋角为/>的激光雷达点的距离；l_α1为激光雷达在仰角为α、水平旋角为的角度下测量得到的与障碍物的距离；d^l′为激光雷达在水平旋角/> 下测量得到的与障碍物的距离的水平值；

(8)分别判断激光雷达与障碍物左右两侧的距离值是否发生突变，并将未发生突变的距离值作为激光雷达与隧道墙壁的距离完成隧道内横向定位，具体包括：

若左右两侧的距离值均未发生突变，则表示激光雷达与障碍物左右两侧的距离值为激光雷达与隧道两侧墙壁的距离值，由此确定隧道内的横向定位；

若仅有左侧的距离值发生突变，则将未发生突变的右侧的距离值作为激光雷达与隧道右侧墙壁的距离；并根据隧道横向宽度值一定，得到激光雷达与隧道左侧墙壁的距离，由此完成隧道内对车辆的横向定位；

若仅有右侧的距离值发生突变，则将未发生突变的左侧的距离值作为激光雷达与隧道左侧墙壁的距离；并根据隧道横向宽度值一定，得到激光雷达与隧道左侧墙壁的距离，由此完成隧道内对车辆的横向定位；

进一步的，激光雷达与障碍物左右两侧的距离值是否发生突变的判断方法；

设置阈值，将步骤(5)与步骤(7)得到的激光雷达与障碍物的距离值分别与前一帧点云数据处理得到的相应的距离值相比较；若绝对差值大于阈值，则表示发生突变；否则表示没有发生突变；

进一步的，步骤(1)所述曲率计算公式：

式中，c为激光雷达点的曲率；S为同一激光束获得的连续五个点的点集合；|S|表示集合的S长度；表示在雷达坐标系下，第k帧中的第i个点的坐标；/>表示在雷达坐标系下，第k帧中的第j个点的坐标。

下面根据实际环境下的实验对本发明的技术方案验证效果与精度。首先为模拟隧道下的环境，实验环境设置在走廊，激光雷达放置在水平地面上进行测量得到算法结果值；根据钢卷尺得到实测结果值。实测结果与算法算法结果仅相差厘米级，同时算法的数据的波动也仅在1厘米之内。多次测量的结果对比如下表1所示：

表1测距结果对比

从表1中可看出，在本次的测量中，算法的测距结果在厘米级精度，并且波动范围在1厘米以内，则得到隧道内精度为厘米级的横向定位结果。

在某市内某隧道路段进行了隧道内车辆横向定位的测距实验，并根据实际经验验证算法的动态性与实用性。测试结果得到车辆距离左侧墙壁的距离是5.74米左右，距离右侧墙壁距离为6.21米左右，车辆靠左侧车道行驶，隧道总宽度为11.96米左右。再根据经验得知，隧道宽度大约在12米左右，得到激光雷达到隧道两侧墙壁的距离的精度符合要求。而车辆在行驶的过程中到两侧墙壁的距离不可能一成不变，所以车辆到两侧墙壁的距离在一定范围内波动，同时计算得到的隧道宽度结果相对稳定的，波动也在厘米级范围。

Claims (5)

1.一种基于激光雷达点云数据的隧道内横向定位方法，其特征在于，具体方法如下：

(1)获取激光雷达测量得到的隧道点云数据，并根据曲率将激光雷达点划分为平面点和非平面点，并剔除掉非平面点；

每一帧点云数据是由所述激光雷达按照扫描角度范围完成一个周期的扫描得到的；

(2)设置激光雷达的最大仰角为θ，从步骤(1)中选择仰角为θ的激光雷达得到的点云数据进行计算，得到激光雷达在仰角为θ，不同水平旋角下测量得到的与障碍物的距离的水平值，并从中选择数值最小的m个，同时将其对应的点云数据构成集合{D}；

其中，计算公式：

d^γ＝l_θ·cosθ

式中，θ为激光雷达的最大仰角；γ为激光雷达的水平旋角；d^γ为激光雷达在仰角为θ、水平旋角为γ的角度下测量得到的与障碍物的距离的水平值；l_θ为激光雷达在仰角为θ、水平旋角为γ的角度下测量得到的与障碍物的距离；

(3)为防止因路面的不平整因素或激光雷达的侧倾导致激光雷达偏移，从步骤(1)中选择仰角为α的激光雷达得到的点云数据，α＜θ；

(4)对于步骤(2)所述集合{D}中的每一个激光雷达点，均从步骤(3)中选择相同水平旋角的点云数据进行计算，得到激光雷达在该水平旋角下测量得到的与障碍物的距离的水平值d^l；计算公式：

式中，α为激光雷达的仰角，且α＜θ；a为仰角为θ、水平旋角为γ的激光雷达点与仰角为α、水平旋角为γ的激光雷达点的距离；l_α为激光雷达在仰角为α、水平旋角为γ的角度下测量得到的与障碍物的距离；d^l为激光雷达在水平旋角γ下测量得到的与障碍物的距离的水平值；

(5)根据下列公式从步骤(4)参数d^l中，选择最小值作为激光雷达与障碍物之间的确定距离值；其中，计算公式：

d＝min_ld^l

式中，d为激光雷达与障碍物的确定距离值；

(6)根据步骤(5)所述参数d所对应的激光雷达的水平旋角确定障碍物的位置；参数d所对应的激光雷达的水平旋角用φ表示；若水平旋角φ在0°-180°范围内，则表示障碍物位于激光雷达的右侧；若水平旋角φ在-180°-0°范围内，则表示障碍物位于激光雷达的左侧；

(7)从步骤(1)中选择激光雷达在仰角为θ、水平旋角为φ+π的角度下测量得到的与障碍物的距离l_θ1，以及激光雷达在仰角为α、水平旋角为φ+π的角度下测量得到的与障碍物的距离l_α1，α＜θ，通过下列公式计算得到激光雷达与另一侧的障碍物的距离值；所述另一侧为步骤(7)确定得到的障碍物位置的相对侧；

其中，计算公式：

式中，α为激光雷达的仰角，且α＜θ；a′为仰角为θ、水平旋角为的激光雷达点与仰角为α，水平旋角为φ+π的激光雷达点的距离；l_α1为激光雷达在仰角为α、水平旋角为/>的角度下测量得到的与障碍物的距离；d^l′为激光雷达在水平旋角φ+π下测量得到的与障碍物的距离的水平值；

(8)分别判断激光雷达与障碍物左右两侧的距离值是否发生突变，并根据未发生突变的距离值作为激光雷达与隧道墙壁的距离完成隧道内横向定位。

2.根据权利要求1所述的一种基于激光雷达点云数据的隧道内横向定位方法，其特征在于，步骤(1)所述曲率的计算公式为：

式中，c为激光雷达点的曲率；S为同一激光束获得的连续五个点的点集合；|S|表示集合的S长度；表示在雷达坐标系下，第k帧中的第i个点的坐标；/>表示在雷达坐标系下，第k帧中的第j个点的坐标。

3.根据权利要求1所述的一种基于激光雷达点云数据的隧道内横向定位方法，其特征在于，所述每一帧点云数据包括所述激光雷达在对应的扫描角度下测量得到的与障碍物的距离、以及激光雷达点。

4.根据权利要求1所述的一种基于激光雷达点云数据的隧道内横向定位方法，其特征在于，步骤(8)所述方法具体包括：

若左右两侧的距离值均未发生突变，则表示激光雷达与障碍物左右两侧的距离值为激光雷达与隧道两侧墙壁的距离值，由此确定隧道内的横向定位；

若仅有左侧的距离值发生突变，则将未发生突变的右侧的距离值作为激光雷达与隧道右侧墙壁的距离；并根据隧道横向宽度值一定，得到激光雷达与隧道左侧墙壁的距离，由此完成隧道内对车辆的横向定位；

若仅有右侧的距离值发生突变，则将未发生突变的左侧的距离值作为激光雷达与隧道左侧墙壁的距离；并根据隧道横向宽度值一定，得到激光雷达与隧道右侧墙壁的距离，由此完成隧道内对车辆的横向定位。

5.根据权利要求4所述的一种基于激光雷达点云数据的隧道内横向定位方法，其特征在于，步骤(8)激光雷达与障碍物左右两侧的距离值是否发生突变的判断方法：

设置阈值，将步骤(5)与步骤(7)得到的激光雷达与障碍物的距离值分别与前一帧点云数据处理得到的相应的距离值比较，若绝对差值大于阈值，则表示发生突变；否则表示没有发生突变。