本发明属于环境感知领域,特别是涉及一种基于多帧点云序列拼接的楼梯检测方法。
背景技术:
1、可通行区域检测是无人车在未知环境中自主探索中必需的基础功能之一。可通行区域信息是全局探索规划与用于避障和跟踪全局路径的局部路径规划所必需的重要前提。室内多为结构性空间,地面较为平整,但同时空间又具有狭窄和复杂的特性,对可通行区域检测精度具有较高的要求。对于在室内立体空间中需要跨越楼层的无人车来说,需要将楼梯空间也纳入可通行区域检测的范围。楼梯具有明显的几何结构特征,楼梯检测算法需要通过提取激光雷达扫描的点云特征,精确检测出楼梯的位置、朝向以及台阶的高度、宽度、深度等几何信息。因此,设计了一种基于多帧点云序列拼接检测楼梯的方法,能够利用拼接连续多帧点云用于楼梯检测并构建高完整度的楼梯模型。
技术实现思路
1、本发明的目的是提供一种基于多帧点云序列拼接的楼梯检测方法,以解决上述现有技术存在的问题。
2、为实现上述目的,本发明提供了一种基于多帧点云序列拼接的楼梯检测方法,包括:
3、获取原始点云,通过区域生长的方法进行点云平面分割,对分割后的点云进行筛选,提取得到楼梯平面的点云;
4、根据楼梯平面的几何特征对楼梯平面的点云进行筛选,获得符合楼梯标准的平面并拼接得到楼梯图形;
5、对所有连续相邻的两个台阶的的楼梯图形进行初始化,对成功初始化后的楼梯图形进行拓展,每一次拓展都以一定的半径搜索符合楼梯标准的平面点云,若连续拓展两个台阶没有搜索到符合楼梯标准的平面,则拓展停止,完成单帧原始点云检测,将检测出的平面点云拼接至下一帧的原始点云,进行下一帧的检测;
6、构建评价函数,根据评价函数计算出每一帧的评价值,若截止当前帧的楼梯模型大于上一帧的楼梯模型的评价值,则进行替代,并将符合楼梯标准的平面点云拼接至原始点云,直至停止运动。
7、可选的,平面的获取过程包括:采用八叉树对所述原始点云进行降采样处理,选定降采样后的点云中的一个点作为生长的种子点,将种子点作为平面中心,使用种子点领域中所有的点对切平面进行局部拟合,根据拟合结果计算得到点云的法向量,基于法向量与种子点领域的点集合获得拟合平面;在拟合平面中,寻找种子点的knn点,若所述knn点不属于任何区域,则建立列表存储,并计算种子点与列表中每个knn点的平面距离,若平面距离不超过预设值,则将此knn点归入当前拟合平面,并作为新的种子点,继续扩展拟合平面,若超出预设值,则作为新平面的种子点进行生长,直至分割获得所有平面。
8、可选的,筛选的过程包括:设置楼梯标准,楼梯标准包括楼梯角度与楼梯几何尺寸;
9、使用pca法计算每一个平面的特征向量,通过特征向量与世界坐标系z轴单位向量计算每个平面与世界坐标系z轴的夹角,与楼梯角度预设值进行比较,对所有平面进行几何角度的筛选;
10、计算筛选后个平面的特征向量与世界坐标系x轴的角度,根据夹角构建旋转矩阵,在x轴的正方向对齐点云,筛选出符合踏板和立板结构的、符合几何尺寸预设值的平面。
11、可选的,将符合几何角度预设值但几何尺寸小于几何尺寸预设值的平面点云拼接至下一帧的原始点云中,并对拼接后的点云进行降采样处理。
12、可选的,初始化包括楼梯立板初始化、楼梯踏板初始化;进行拓展时均对楼梯图形进行多次初始化。
13、可选的,拓展的过程包括:根据每一个初始化结果中的元素构建一个空的楼梯图形,对成功初始化后的楼梯图形按照一个台阶为单位沿楼梯延伸方向进行拓展,每一次拓展都以一定的半径搜索符合楼梯标准的立板平面点云和踏板平面点云,若搜索到则将对应的立板或踏板平面点云添加到楼梯图形中,其中,楼梯延伸方向包括向上与向下。
14、可选的,基于立板评价函数、踏板评价函数、立板平面数量、踏板数量构建所述评价函数,所述评价函数表达式为:sstair=sr·nr+st·nt
15、式中,nr和nt分别为检测到的楼梯模型立板和踏板平面的数量,sr为立板评价函数,st为踏板评价函数。
16、可选的,所述立板评价函数为立板台阶深度评价值、立板台阶高度评价值以及立板楼梯朝向评价值三部分加和;
17、所述踏板评价函数为踏板台阶深度评价值和踏板台阶高度评价值两部分加和。
18、本发明的技术效果为:
19、本发明的技术方案使用多帧点云序列拼接的方式能够在提高楼梯检测的精度和完整度,并且能够在楼梯结构部分被遮挡的情况下保持检测楼梯的能力,能够提高使用激光雷达对楼梯检测的精度和稳定性。
1.一种基于多帧点云序列拼接的楼梯检测方法,其特征在于,包括以下步骤:
2.根据权利要求1所述的基于多帧点云序列拼接的楼梯检测方法,其特征在于,
3.根据权利要求1所述的基于多帧点云序列拼接的楼梯检测方法,其特征在于,
4.根据权利要求3所述的基于多帧点云序列拼接的楼梯检测方法,其特征在于,
5.根据权利要求1所述的基于多帧点云序列拼接的楼梯检测方法,其特征在于,
6.根据权利要求1所述的基于多帧点云序列拼接的楼梯检测方法,其特征在于,
7.根据权利要求1所述的基于多帧点云序列拼接的楼梯检测方法,其特征在于,
8.根据权利要求7所述的基于多帧点云序列拼接的楼梯检测方法,其特征在于,