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

权利要求书

1.一种面向全地形车的楼梯自动检测与攀爬方法，其特征在于，包括以下步骤：

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

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

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

2.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，所述步骤1中，基于激光雷达获取单帧或多帧点云数据，将激光雷达一次采集的所有点云数据记作：

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

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

P_processed＝Pre_processing(P_ori) (2)式中Pre_processing(·)为对点云数据进行预处理过程。

3.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，采用基于KD-tree优化的DBSCAN算法对预处理后的点云进行分割，P_objs是一组点云团簇，每个点云团簇代表真实环境中的一个对象：

P_objs＝{P_i|i＝1，…，N_c}＝DBSCAN(P_processed) (3)

上式中N_c为点云团簇的个数，P_i 表示第i个点云团簇，将分割后的每一个点云团簇拟合成一个平面，并提取拟合优度和平面坡度特征，根据所述特征判断所述点云团簇是否为楼梯。

4.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，若某点云团簇为楼梯，则点云团簇内的点分布于倾斜面上，汇聚成多条相互平行的线，所述具有平行关系的线与线之间由小圆弧连接，考虑到激光雷达水平安装的扫描特性，直线分布的点对应楼梯台阶的垂直面，小圆弧对应楼梯台阶的水平面，将某个团簇中的点拟合到一个平面中，通过平面坡度和拟合优度来判断该簇描述的对象是否是楼梯。

5.根据权利要求1所述的面向全地形车的楼梯自动检测与攀爬方法，其特征在于，对于楼梯的点簇P_i，簇中的点分布在坡度为s_i的斜面α_i上，斜面α_i的描述方程如下：

上式中均表示平面α_i的参数，由此得到斜面α_i的坡度s_i为：

楼梯斜面的坡度s_i∈[s_min，s_max]，与此同时，合成斜面α_i对点簇P_i的拟合优度应满足g_i∈[g_min，g_max]。