一种基于激光雷达的距离与航向测量方法

技术领域

本发明涉及环境感知技术领域，具体地说是一种基于激光雷达的长走廊环境下对两侧墙面距离的测量，以及偏航角的计算。

背景技术

激光雷达作为一种主动测量传感器，能够准确获得场景的深度信息，并具有良好的抗干扰能力，在机器人导航、工程测量等领域具有较大的应用价值。在长走廊环境下，由于光线昏暗、环境纹理特征稀疏，基于视觉或激光的定位方法由于无法提取有效的环境特征信息，定位易失效或发生较大的漂移。

针对此等环境条件，提出一种轻量级的距离与偏航角测量方法，通过提供机器人到两侧墙的距离和自身的偏航角，保障机器人的自主导航。

发明内容

针对现有的技术在长走廊环境下易失效的不足，本发明提出一种基于激光雷达的距离与航向测量方法，实现机器人在长走廊环境下对两侧墙面距离的测量，以及机器人行进方向的偏航角测量，将检测结果数据输出给机器人控制系统可保障机器人在长走廊环境下的自主行进。

为实现上述目的，本发明采用的技术方案为：

一种基于激光雷达的距离与航向测量方法，该方法基于16线激光雷达实现机器人在长走廊环境下对两侧墙面距离的测量，以及机器人行进方向的偏航角测量；方法包括以下步骤：

步骤1：通过设在机器人本体上的激光雷达传感器扫描获得环境的三维点云数据；

步骤2：对获取的三维点云数据进行数据处理，具体操作包括：对点云进行直通滤波、统计滤波、欧式聚类、随机采样一致性分割，以及计算聚类点云簇的质心；

步骤3：根据得到的点云簇的质心和平面参数，计算激光雷达到走廊两侧墙面的距离以及机器人的偏航角。

所述激光雷达传感器安装在机器人的旋转云台上，所述云台水平方向旋转角度为-90度—90度；通过控制旋转云台旋转用于调整激光雷达传感器的360度横扫平面为水平面。

所述从扫描得到的三维点云数据提取测量所需的特征，包括以下步骤：

步骤2.1：根据激光雷达扫描的空间三维点距离，进行直通滤波操作，得到激光雷达区域内的数据；

步骤2.2：对直通滤波后的点云数据进行统计滤波，去除由噪声引起的离群点；

步骤2.3：对统计滤波处理后的数据进行欧式聚类，并对聚类得到的点云簇进行随机采样一致性分割，获取拟合平面的内点；

步骤2.4：对步骤2.3获得的点云内点数据进行第二次欧式聚类并计算聚类后点云簇的质心；

步骤2.5：对步骤2.4欧式聚类得到的点云簇进行平面分割，获得平面的参数信息。

所述根据激光雷达扫描的空间三维点距离，进行直通滤波操作，包括以下步骤：

获取的三维点云数据，遍历每个点云点，根据设定的候选区域坐标阈值范围剪除离群点，减少数据量。

所述候选区域坐标阈值范围为：Y轴位于(-1.0,1.0)米范围内、Z轴位于(-0.3,1.0)米范围内、X轴全部的数据。

所述对直通滤波后的点云数据进行统计滤波，去除由噪声引起的离群点包括以下过程：

首先，计算每个点到所有K邻域点的平均距离，这里K取50；其次，计算整个点集距离容器的平均值和样本标准差，并假设其服从高斯分布；将平均距离在标准范围之外的点作为外点进行剔除，得到去除噪声点的点云数据。

所述对统计滤波处理后的数据进行欧式聚类，并对聚类点云簇做随机采样一致性分割，获取拟合平面的内点，包括以下步骤：

对于空间中某三维点P，通过KDTree近邻搜索得到K个离P最近的点，将这些点中距离小于阈值的点聚类到集合Q中，当Q中点数不再增加且满足指定的数目要求时，完成聚类过程；否则重新选取P点，完成上述操作；

在获得的聚类点云簇上，根据随机采样一致性算法RANSAC，创建平面模型的随机采样一致性对象，根据多次迭代获取的平面模型测试簇内所有的其它点数据，得到满足模型的簇内点。

所述对获得的点云内点数据进一步进行欧式聚类，计算聚类点云簇的质心，并进一步对欧式聚类得到的点云簇进行平面分割，获得平面的参数信息，具体过程为：

根据欧式聚类得到的点云簇，计算点云簇的质心，计算方式为：

其中，r

通过随机采样一致性算法，拟合点云的平面模型，最终拟合的平面模型参数为以下形式：

ax+by+cz+d＝0

根据平面模型，获得平面的法向量为：

所述根据得到的点云簇的质心和平面参数，计算激光雷达到走廊两侧墙面的距离与机器人的偏航角，包括以下步骤：

a.假设拟合平面即为两侧墙面，则激光雷达到走廊两侧墙面的距离即为激光雷达坐标原点到拟合平面的距离，计算公式为：

b.偏航角为激光雷达Y轴正方向与拟合平面的夹角，即Y轴的方向向量n

θ＝|90°-

其中，偏航角的方向可根据所提取法向量的平面位于激光雷达的X轴正方向或负方向来分类讨论进行判断。

一种基于激光雷达的距离与航向测量系统，该装置基于16线激光雷达实现机器人在长走廊环境下对两侧墙面距离的测量，以及机器人行进方向的偏航角测量；系统包括：机器人的旋转云台、安装于其上的激光雷达传感器，以及嵌入式计算平台；所述嵌入式计算平台存储以下程序模块：数据采集模块、点云数据处理模块、距离与航向计算模块；所述嵌入式计算平台读取各个程序模块完成激光雷达的距离与航向测量；

所述数据采集模块，输出指令通过旋转云台控制激光雷达传感器的360度横扫平面为水平面，控制激光雷达传感器扫描获得环境的三维点云数据；

所述点云数据处理模块，对扫描的三维点云数据提取测量所需的特征信息，具体操作包括：对获得的三维点云进行直通滤波、统计滤波、欧式聚类、随机采样一致性分割，以及计算聚类点云簇的质心；

所述距离与航向计算模块，根据得到的点云簇的质心和平面参数，计算激光雷达到走廊两侧墙面的距离以及机器人的偏航角。

本发明具有以下效益及优点：

1.本发明解决了机器人在长走廊、光照条件差、环境纹理特征稀疏下定位漂移或丢失的问题，通过提供机器人本体到两侧墙面的距离以及自身的偏航角，保障机器人在长走廊环境下的自主导航。

2.本发明采用16线激光雷达传感器作为测量装置，抗干扰能力强，测量精度高，可以实现3cm的距离测量精度。

3.本发明将激光雷达装载与旋转云台，而非固定式安装，可以控制激光雷达的扫描角度，获得更丰富的点云数据。

4.本发明提出的方法更加轻量级，节约计算资源。

附图说明

图1为本发明的方法流程图；

图2为激光雷达传感器模型示意图；

图3为激光雷达坐标系相对拟合平面示意图一；

图4为激光雷达坐标系相对拟合平面示意图二；

图5为激光雷达坐标系相对拟合平面示意图三；

图6为激光雷达坐标系相对拟合平面示意图四；

图7(a)为实验走廊激光点云图；

图7(b)为本发明方法处理后的点云图。

具体实施方式

为使本发明的上述目的、特征能够更加明显易懂，下面将结合附图对本发明的具体实施进行详细的说明。非另有声明定义外，本文所使用的技术和科学术语属于本发明领域技术人员的通常理解。

如图1为本发明的方法流程图，基于激光雷达的距离与航向测量方法包括以下具体过程：

首先通过控制激光雷达云台的旋转，保证在对环境扫描时激光雷达的云台与机器人本体处于同一平行面，激光雷达传感器模型如图2所示。激光雷达使用RoboSense 16线激光雷达，水平测量360度，垂直测量-15—15度。激光雷达与工控机采用以太网通信，使用UDP协议传输三维测量数据。

通过RoboSense的ROS功能包完成对原始激光雷达数据的解析，建立本文所提算法的工作空间，在工作空间中创建ROS节点，获取激光雷达的三维点云数据。

在上述步骤中读取的点云数据包含离机器人本体较远的点云，以及大量的噪声点，为方便后面的应用操作，先要对点云进行滤波处理。

本发明采用两种滤波方法来对数据进行预处理，第一步先对数据信息直通滤波操作，剪除离机器人本体较远的点云，减少数据量，具体选取方式如下：

对获取的点云数据，遍历每个点云点，保留Y轴位于(-1.0,1.0)米范围内、Z轴位于(-0.3,1.0)米范围内、X轴方向全部的数据。选取机器人本体周围的数据点，更能准确的进行测量。

在直通滤波操作后，对数据进行统计滤波处理，去除由噪声引起的离群点，具体的过程为：

首先计算每个点到所有K邻域点的平均距离，这里K取50；其次，计算整个点集距离容器的平均值和样本标准差，并假设其服从高斯分布；将平均距离在标准范围之外的点作为外点进行剔除，便可得到去除噪声点的点云数据。

在对激光雷达点云数据进行两次滤波操作后，可获得良好的点云数据用于聚类和分割操作，首先对统计滤波处理后的数据进行欧式聚类，步骤如下：

对于空间中某三维点P，通过KDTree近邻搜索K个离P最近的点，将这些点中距离小于阈值的聚类到集合Q中，当Q中点数不再增加且满足指定的数目要求时，完成聚类过程；否则重新选取P点，完成上述操作。

在欧式聚类完成后，对点云进行随机采样一致性分割，获取满足平面模型的内点，此步操作后可进一步剔除离群点数据。具体操作方式为：

根据随机采样一致性算法(RANSAC，Random Sample Consensus)，创建平面模型的随机采样一致性对象，根据多次迭代获取的平面模型测试簇内所有的其它点的数据，得到满足模型的簇内点。

对获得的点云内点数据进一步进行欧式聚类，计算点云簇的质心，过程如下：

根据聚类得到的点云簇，计算点云簇的质心，计算方式为：

其中，r

对获得的点云内点数据进行欧式聚类，在对得到的点云簇进行平面分割，获得平面的参数信息，包括以下过程：

通过随机采样一致性算法，拟合点云的平面模型，最终拟合的平面模型参数为以下形式：

ax+by+cz+d＝0

其中，a、b、c、d为平面方程的参数；

根据平面模型，获得平面的法向量为：

最后，根据得到的点云簇的质心和平面参数，计算激光雷达到走廊两侧墙面的距离与机器人的偏航角，计算方式为：

到两侧墙面的距离即为激光雷达坐标原点到拟合平面的距离l，计算公式为：

偏航角为激光雷达Y轴正方向与拟合平面的夹角，即Y轴的方向向量n

θ＝|90°-

其中，

在偏航角的计算过程中，根据所依赖的平面和坐标系的朝向不同，可分为4种情况，如图3—6所示。在计算过程中，假设激光雷达坐标系的Y轴位于图中虚线的左侧时，偏航角取正；否则，雷达坐标系的Y轴位于图中虚线的右侧时，航向角取负。

在计算偏航角时，需要依赖平面的法向量

当p

设法向量

如图3所示，此时所依赖的平面位于激光雷达坐标系右侧，即p

当法向量

同理，如图4所示，此时所依赖的平面位于激光雷达坐标系左侧，即p

同理，如图5所示，此时所依赖的平面位于激光雷达坐标系右侧，即p

同理，如图6所示，此时所依赖的平面位于激光雷达坐标系左侧，即p

进一步地，本发明还提供一种基于激光雷达的距离与航向测量系统。

该系统基于16线激光雷达实现机器人在长走廊环境下对两侧墙面距离的测量，以及机器人行进方向的偏航角测量；系统包括：机器人的旋转云台、安装于其上的激光雷达传感器，以及嵌入式计算平台；所述嵌入式计算平台存储以下程序模块：数据采集模块、点云数据处理模块、距离与航向计算模块；所述嵌入式计算平台读取各个程序模块完成激光雷达的距离与航向测量。

所述数据采集模块，输出指令通过旋转云台控制激光雷达传感器的360度横扫平面为水平面，控制激光雷达传感器扫描获得环境的三维点云数据；所述点云数据处理模块，对扫描的三维点云数据提取测量所需的特征信息，具体操作包括：对获得的三维点云进行直通滤波、统计滤波、欧式聚类、随机采样一致性分割，以及计算聚类点云簇的质心；所述距离与航向计算模块，根据得到的点云簇的质心和平面参数，计算激光雷达到走廊两侧墙面的距离以及机器人的偏航角。其中，嵌入式计算平台选用的是研华MIO-5391单板电脑。

为验证本发明方法的有效性，利用RoboSense 16线激光雷达在走廊内扫描获得场景的三维点云数据，并实时返回距离两侧墙面的距离，及机器人的偏航角信息，数据刷新频率为30HZ。并利用所求解计算的数据，在走廊环境下进行机器人的自主导航测试，实测距离测量平均误差在0.026米左右，满足任务需求。如图7(a)、(b)为实验走廊环境的三维点云图及处理后的点云图，可见本发明方法解决了机器人在长走廊环境下自主导航对到两侧墙面距离和偏航角的需求，通过所提供的数据，实现特定场景下的自主导航。

按照上述实例，即可很好地实现本发明。需要说明的是，在本说明书的指导下本领域技术人员所做出的任何等同替代方法，或明显变形方式均在本发明的保护范围内。