[发明专利]一种面向全地形车及其自动检测楼梯与攀爬方法

说明书

本发明公开了一种面向全地形车及其自动检测楼梯与攀爬方法，方法具体为：对获取的单帧或多帧点云数据降采样以及过滤地面点处理，进行分割得到不同的点云团簇并将其拟合成一个平面，并提取拟合优度和平面坡度特征，判断所述点云团簇是否为楼梯，从获取代表楼梯的点云团簇提取出表征楼梯结构的平行线，并根据平行线获取楼梯与全地形车的相对位置关系，需要攀爬通过楼梯时，将车辆航向调整到与楼梯垂直；根据获取车辆实时车辆三轴姿态数据，判断全地形车辆在楼梯上的状态，根据横滚角引导车辆攀爬楼梯；本发明对楼梯不是由台阶平面检测出来的，而是由坡度和平行边缘几何特征来检测，本发明中方法可以从单帧激光雷达数据中实时检测和定位楼梯。

技术领域

本发明属于自动驾驶和机器人领域，具体涉及一种面向全地形车及其自动检测楼梯与攀爬方法。

背景技术

近年来，自动驾驶和机器人技术得到了广泛应用，然而，在L5级别的自动驾驶技术成熟之前，将这些技术应用于特定场景中的一些轮式机器人，显得更加重要和有意义，全地形车辆 (ATV)广泛应用于城市搜救，ATV工作的非结构化、复杂场景中通常包含许多特殊的障碍，如楼梯；因此，自动检测、定位和通过楼梯是一种十分重要的能力。机器人遥控爬楼梯已经被证明是低效和不可靠的，因此，许多关于机器人自主爬楼梯检测和导航的研究已经展开。为了能在有楼梯的环境中自主安全移动，ATV必须:(1)找到楼梯并定位；(2)判断是否需要走楼梯；(3)如有需要，安全快速爬上楼梯，但是，目前的大多数方法只能提供部分的解决方案。比如有的只关注楼梯检测，有的只关注控制方面，有些方法探索了楼梯检测与导航两方面的研究，但仅限于室内环境或其他特定场景。

发明内容

为了解决现有技术的问题，本发明提供一种面向全地形车及其自动检测楼梯与攀爬方法，基于激光雷达采集关于环境的单帧或多帧点云数据，获取基于点云数据中点云团簇的几何特征，如坡度和平行线等，识别和定位楼梯，基于ATV位于楼梯上时的横滚角和偏航角的耦合关系，根据实时获取的三轴姿态，控制并稳定车辆姿态，从而进行楼梯攀爬，结果证明了本发明算法的鲁棒性和准确性，并证明了该方法是安全可行的。

为了达到上述目的，本发明采用的技术方案为：一种面向全地形车的楼梯自动检测与攀爬方法，包括以下步骤：

步骤1，楼梯自动检测：获取单帧或多帧点云数据，对所述单帧或多帧点云数据进行降采样以及过滤地面点处理，再进行分割得到不同的点云团簇，将所述一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据所述拟合优度和平面坡度特征判断所述点云团簇是否代表楼梯，获取代表楼梯的点云团簇；

步骤2，定位楼梯并调整车辆航向与楼梯对齐：从步骤1所获取代表楼梯的点云团簇中提取出表征楼梯结构的平行线，并根据所述平行线获取楼梯与全地形车的相对位置关系，当车辆与楼梯的距离小于设定阈值，且需要攀爬通过楼梯时，将车辆航向调整到与楼梯横向垂直；

步骤3，自动攀爬楼梯：获取车辆实时车辆三轴姿态数据，根据所述实时车辆三轴姿态数据，判断全地形车辆在楼梯上的状态，根据位姿状态数据中横滚角的大小判断车辆朝向是否与楼梯横向保持垂直，并据此引导车辆以垂直方向攀爬楼梯；根据车辆俯仰角大小判断车辆是否完成楼梯攀爬，如果俯仰角大于设定阈值，说明没有完成攀爬任务，则需要继续攀爬；如果俯仰角小于阈值，则说明攀爬任务已经完成，停止攀爬动作。

所述步骤1中，基于激光雷达获取单帧或多帧点云数据，将激光雷达一次采集的所有点云数据记作：

P_ori＝{p_i＝(x_i，y_i，z_i)|i＝1，…，N} (8)

上式中N为点数，经过降噪、范围过滤和地面滤波以及降采样预处理后的点云数据表示为：