[发明专利]面向斜坡区域的基于多线激光雷达的斜坡角度估计方法

说明书

本发明公开了一种面向斜坡区域的基于多线激光雷达的斜坡角度估计方法，包括：获取给定斜坡区域的原始激光雷达点云数据D_L；结合多线激光雷达坐标系O_LX_LY_LZ_L到机器人坐标系O_RX_RY_RZ_R的坐标变换关系，对D_L进行坐标变换和滤波处理，生成新的激光雷达点云数据D_S；基于D_S，利用PROSAC算法进行平面拟合，得到斜坡坡面方程；从D_S中选取3个到平面X_RO_RY_R的距离均不超过给定阈值d_Z的数据点，并利用该组数据点计算得到地面方程；依据所述斜坡坡面方程和所述地面方程计算得到斜坡角度的估计结果。本发明利用多线激光雷达获取斜坡区域的距离信息并基于斜坡坡面法向量实现斜坡角度的估计，提高了斜坡角度估计的质量，为机器人环境理解等方面的应用提供技术支持。

技术领域

本发明涉及机器人环境理解领域，特别是一种面向斜坡区域的基于多线激光雷达的斜坡角度估计方法。

背景技术

随着移动机器人和传感器技术的不断发展，其应用领域也在不断拓展。对于机器人所面向的室外环境来说，斜坡是普遍存在的。斜坡检测的质量将直接影响到机器人任务执行的性能。国内外研究人员在斜坡角度估计方面做出了一定的研究。在斜坡区域的信息采集方面，多利用立体相机或深度相机进行感知，但通过立体相机或深度相机获得的距离信息往往不够准确，这降低了斜坡角度估计的质量。在方法上多通过寻找斜坡纵向截面的方式来计算斜坡的角度，该方法比较直观，但由于斜坡位置和坡面朝向等因素的影响，纵向截面的确定比较困难，这对斜坡角度的估计准确度造成较大的影响。与纵向截面特征相比，斜坡坡面法向量这一全局特征能够更有效的反映斜坡。考虑到多线激光雷达能够获取更为准确的距离信息，有必要利用多线激光雷达获取斜坡区域的距离信息并基于斜坡坡面法向量实现斜坡角度的估计。

发明内容

为了解决现有技术中的上述问题，即为了解决现有技术对斜坡角度的估计准确度难以满足需求的问题，本发明提出了一种面向斜坡区域的基于多线激光雷达的斜坡角度估计方法，提高了斜坡角度估计的质量。

本发明提出的一种面向斜坡区域的基于多线激光雷达的斜坡角度估计方法，包括以下步骤：

步骤S1，获取给定斜坡区域的原始激光雷达点云数据D_L；

步骤S2，结合多线激光雷达坐标系O_LX_LY_LZ_L到机器人坐标系O_RX_RY_RZ_R的坐标变换关系，对D_L进行坐标变换和滤波处理，生成新的激光雷达点云数据D_S；

步骤S3，基于D_S，利用PROSAC算法进行平面拟合，得到斜坡坡面方程；

步骤S4，从D_S中选取3个到平面X_RO_RY_R的距离均不超过给定阈值d_Z的数据点，并利用该组数据点计算得到地面方程；

步骤S5，依据所述斜坡坡面方程和所述地面方程计算得到斜坡角度的估计结果。

优选地，步骤S3利用PROSAC算法得到斜坡坡面方程的具体方法为：

步骤S31，从新的激光雷达点云数据D_S中任选3个数据点，计算出一个平面；