CN113791400B - 一种基于激光雷达的楼梯参数自主检测方法 - Google Patents

Abstract

本发明针对通过激光雷达对典型环境特征识别问题，提出了一种基于激光雷达的楼梯参数自主检测方法。本发明对室内环境三维点云中的楼梯点云对象进行了分析，针对其结构参数的多样性与三维激光点云获取时误差的随机性，提出了一种基于墙面检测及楼梯立面包装盒算法实现楼梯参数的测量。此外，本发明设计处理模型具有流程少，运算量小的特点，可以在工控机上达到实时检测的效果。

Description

一种基于激光雷达的楼梯参数自主检测方法

技术领域

本发明属于基于激光雷达识别典型环境特征领域，用于智能机器人对环境中的各种楼梯特征进行实时检测。

背景技术

随着信息化的普及与科技的发展，无人系统及智能机器人技术日趋成熟。在智能机器人领域，常用三维激光雷达来获取周围环境中的精确位置信息，具有分辨率高，抗干扰能力强，不受光线影响，便于集成等优点，所以基于激光雷达进行智能机器人周围环境典型特征检测对智能机器人技术的发展与完备具有积极意义。

在智能机器人失去了与人工配合的情况下，智能机器人面临的首要问题就是对于环境特征的辨识与定位。机器人对于环境的有效理解对于提升机器人的智能性具有重要作用，场景中典型的环境特征可以极大的提高机器人定位的精度与效率，也可以方便机器人提前制定对应特殊环境特征的策略，因而对于环境典型特征的识别显得尤为重要。

对于环境的特征识别具有相当长的研究历史，其中对于典型环境特征楼梯的识别如文献(Bansal M,Matei B,Southall B,et al.A LIDAR streaming architecture formobile robotics with application to 3D structure characterization[C]//IEEEInternational Conference on Robotics&Automation.IEEE,2011.)Mayank Bansal等人基于三维激光构建的环境点云，在三维空间中使用模版匹配的算法实现楼梯识别，该方法对于先验目标的测定较为精准，但是相关设定参数过多，具有较大局限性。文献(Theeravithayangkura C,Takubo T,Mae Y,et al.Stair recognition with laserrange scanning by limb mechanism robot“ASTERISK”[C]//Proceedings of IEEEInternational Conference on Robotics and Biomimetics(ROBIO’08).Bangkok,Thailand:IEEE,2009.)Chayooth等人对激光雷达扫描信息进行逐列提取，基于同平面内点云的分布特征，对于可能处于同一立面内的连续点云扫描线段进行归并，不同面内的点云进行分割，从而实现点云中立面结构的识别，并基于此参数建立阶梯模型，该方法的局限性在于当点云数据以非直面台阶采集时，连续线无法聚合，同时由于阶梯边缘位置依赖于所提取线段的端点，其对不规则面的识别效果较差。文献(Lu X,Manduchi R.Detection andlocalization of curbs and stairways using stereo vision[C]//Proceedings ofthe 2005IEEE International Conference on Robotics and Automation(ICRA).Barcelona,Spain:IEEE,2005.)X.Y.Lu等使用三维视觉中提取到的边缘估计点进行二维映射，将单目相机获得环境图像与之结合，可以较为准确的拟合出边缘位置，从而在一定程度上解决了基于二维图像的识别方法无法生成对应三维阶梯模型的问题，然而其从根本上解决边缘检测中的假阳性情况。上述技术诸基于先验环境模型，并且在一定参数下测得目标边缘线较为散乱且复杂，需要对误判边缘进行滤除，同时，在融合边缘线的过程中运算量较大，使得实时性差强人意。当点云数据以非直面台阶采集，即楼梯各个面不平整时，连续线无法聚合，同时由于阶梯边缘位置依赖于所提取线段的端点，其对不规则面的识别效果较差。

利用智能机器人装配的三维激光传感器获取环境数据，检测环境中的楼梯，对于采集现场点云数据中的完整楼梯可以识别、建模，并计算楼梯参数，具体包括楼梯阶梯高度、阶梯宽度、阶梯深度、楼梯阶梯数量等，以便智能机器人改变姿态通过楼梯。

发明内容

本发明针对通过激光雷达对典型环境特征识别问题，提出了一种激光雷达检测楼梯参数方法。本发明对室内外环境三维点云中的楼梯对象进行了分析，针对其结构参数的多样性与三维激光点云获取时误差的随机性，提出了一种基于墙面检测及楼梯立面包装盒算法，从而实现楼梯参数的测量。此外，本发明设计处理模型具有流程少、运算量小的特点，可以在工控机上达到实时检测的效果。

本发明的技术方案：

一种基于激光雷达的楼梯参数自主检测方法，步骤如下：

步骤一：激光点云数据预处理

(1)去除无效点：从激光传感器获得的点云可能包含几种测量误差，部分点对样本的反映不准确。其中一种情况是在一些点的坐标中存在为空的情况，无效点会对点云的后续处理带来干扰，应对点云进行搜索遍历剔除无效点；

(2)降采样：在获取场景的点云时，利用激光雷达得到的原始点云数据往往过于稠密且复杂，其中除了目标点云外还包括障碍物、噪声、遮挡等等，对后续处理极为不利；为了提高后续环节的运算速度与处理效率，必须对原始点云进行稀疏化，减少待处理点云的数量；同时对于环境中的噪声与障碍物进行去除，降低干扰，从而降低后续所有环节中遍历算法的时间复杂度，进一步提高效率。本方法中采用体素滤波进行点云的稀疏化，体素滤波是在空间内以一定参数模拟出连续的体素立方，在每个立方空间内对其中的点求均值，用来代替整体的状况；依据点云数据坐标，确定点云的最小外接长方体，其长宽高为l_x,l_y,l_z。同时设置体素栅格边长为cell，将X，Y，Z三轴均等划分为M，N，L份，则最小外接长方体被划分成M*N*L个体素小栅格。

(3)区域分割：在稀疏化的点云中，一方面存在被向下采样压缩的障碍物噪声等，另一方面待检测楼梯区域由于附近多墙体平面的存在使得检测环境复杂，后续基于墙面的分析方法难以使用，故需要提前对小的浮空点云进行剔除、大的点云进行分割；当前研究中主要方法为基于法线估计的区域生长以及基于欧氏距离判定的欧氏分割，由于当前环节区域生长在时间上的消耗过大，同时欧氏分割对于当前情况的适用性较好，本方法中采用欧氏分割对点云进行处理，实现点云的区域分割；

步骤二：楼梯点云样本处理

对于预处理后的楼梯点云样本进行处理，包括对楼梯潜在区域墙壁的检测，墙壁去除，将楼梯立面进行PCA主元分析，计算楼梯立面质心，楼梯立面包装盒参数估计等；

(1)楼梯潜在区域检测：本发明设计提出了基于墙面的楼梯检测方法，由于设计中场景信息采集传感器扫描位置的特殊性，导致扫描台阶水平面存在部分缺失，然而墙面却可以较为完整的扫描得到，故通过先定位墙体，后定位楼梯位置的方法完成对楼梯潜在区域的检测；

(2)墙壁检测：为了在复杂点云样本中获取最好的墙面拟合效果，本方法中采用RanSAC算法进行拟合，其鲁棒性较强，可适应周围噪声较多的环境，一般实现中该算法通过迭代原始数据点得到最优的拟合参数，数据点中可包含大量的噪点而不影响拟合效果，使其在本设计中的拟合效果明显优于最小二乘等其他方法，但由于其初始内群点选择的随机性，其时间复杂度一般不确定，且得到可信模型的概率与信度随迭代次数增加而增加，注意到一般扫描阶梯点云会存在铅锤墙面等大面积扁平直立结构，此时楼梯作为噪声对其进行RanSAC拟合分析，可得到较为理想的拟合效果，估算迭代次数m，用p表示一次迭代过程中从点云数据集内随机选取出的点为均平面内点的概率，用q表示每次从点云数据集中选取一个局内点的概率，如下式所示：q＝平面内点的数目/点云数据集的数目，用n来表示拟合平面模型最少需要的点云数据个数，可知有如下关系：

1-p＝(1-qⁿ)^m (2)

对上式进行取对数等变换，得到对于迭代次数m而言在迭代过程中的标准差为：

上式用于估计在整个拟合过程中所需要的迭代次数m，RANSAC算法通过反复选择数据中的一组随机子集来达成目标，被选取的子集就被假设为局内点，在本方法中我们定义迭代次数m最大M次(100)，预设拟合平面模型最少需要的点云数据个数n为N(1000)，预设初始用于拟合平面的局内点为a个(3)，用初始平面模型去测试所有的其它点云数据，如果某个点适用于估计的模型，认为它也是局内点，增加局内点的个数，不断进行迭代直到局内点个数>n或迭代次数>m。

(3)墙壁去除：当前方法对于点云中的墙面需要进行剔除，故需要对墙面进行检测，然后根据其法线方向判定楼梯侧，对另一侧点云进行删除，同时对楼梯侧进行取样，在前述步骤的描述中，已通过RanSAC方法完成了墙面拟合过程，得到了其拟合参数coff＝[[A₁，B₁，C₁，D₁]…[A_n，B_n，C_n，D_n]]，其中A，B，C，D，为拟合平面的系数，下标代表拟合平面的编号。对于拟合的墙面1对应参数coff[0]＝[A₁，B₁，C₁，D₁]而言，通过遍历其对应点云簇S₁＝[¹p₁，¹p₂，…¹p_n]，p的上标代表点云簇的序号，p的下标代表每个点云簇中点的序号，每个点p包含x，y，z三个坐标；S下标代表点云簇的序号。计算其到墙面距离如下：

以上式所得距离为依据，删除墙面中的点(包括墙面内部及距拟合墙面δ阈值距离内的点)：计算点云簇距离该平面法线同侧的距离均值为dis_p与反向距离均值为dis_n如下，其中k_p为点云个数p的右上标+代表与该平面法线同侧的点云，-代表与该平面法线异侧的点云：

仅保留dis_p和dis_n小于±0.6*l_{heig ht_max}的平面，(其中l_{heig ht_max}为默认最大楼梯宽度：3m)，该平面为楼梯所依附的墙面，若|dis_p|＞|dis_n|，则楼梯区域位于墙面法向正方向n＝(A，B，C)，反之则楼梯区域位于墙面法向反方向；

(4)PCA主元分析及楼梯立面包装盒：保留去除墙壁后楼梯立面点云，再次进行欧式聚类，得到每一片楼梯立面点云。对于任一个楼梯立面点云M_j(x，y，z)＝[^jr₁，^jr₂，…，^jr_n]，其中r为楼梯立面中的点，r上标表示楼梯立面的序号，r下标表示点集内点的序号，M为楼梯立面点集，M下标代表楼梯立面的序号。楼梯立面实际上并不为一个理想的平面，而是近似存在一定厚度(较小)的长方体，为了求解其所代表阶梯的宽轴向，可使用主元分析法PCA对其进行求解，先对台阶面点云进行去均值化，然后求解其协方差矩阵：

随后使用SVD分解：

Cov(X，Y，Z)＝U∑V^T (8)

得到奇异值矩阵左奇异矩阵计算U＝[pc₁，pc₂，pc₃]，计算主元pc₁方差贡献率：

其中，e1、e2、e3分别表示楼梯立面点云协方差矩阵的特征值；U表示协方差矩阵SVD分解的左奇异矩阵，pc₁，pc₂，pc₃代表分解楼梯立面点云后正交的三个主元方向；α₁为对应e1主元方向上的方差贡献率；用c₁、c₂、c₃分别表示每片楼梯立面点云的质心；

当α₁此值大于给定百分比阈值时，认定楼梯主元识别无误；如上方法所述利用PCA主元分析法获得点云的三个主方向，获取每一片楼梯立面点云的质心，计算每一片楼梯立面点云的协方差，从而获得每一片楼梯立面点云的协方差矩阵，求取协方差矩阵的特征值和特征向量，特征向量即为主方向；利用获得的每一片楼梯立面点云的主方向和质心，将每一片楼梯立面点云的输入点云转换至原点，且主方向与坐标系方向重合，建立变换到原点的每一片楼梯立面点云的包围盒；包装盒的宽度即为楼梯的宽度估计；质心连线是一条斜率与台阶一致，方向沿着台阶面中心的折线；将各个质心坐标相减则为楼梯踏面的长度和楼梯立面的深度。

本发明的有益效果：本方法针对与环境典型特征之一楼梯与检测的实际需求，提出了一种基于激光雷达的楼梯参数自主检测方法，通过点云拟合平面的基本思想，是三维点云较之于二维点云拟合边缘线独有的优势，配合引入楼梯点云数据处理统计误差，合理控制参数检测误差，具有精准、快速，实时性强等特点。

附图说明

图1激光雷达实物图。

图2楼梯采样视角。

图3楼梯原始稠密点云。

图4楼梯体素滤波点云数据。

图5楼梯欧式聚类分割点云数据。

图6楼梯取样示意图。

图7去除墙面后的楼梯点云。

图8包装盒示意图。

图9质心连线示意图。

图10为本发明的方法流程图。

具体实施方式

以下将结合技术方案和附图详细叙述本发明的具体实施方式：

步骤一：对环境中的楼梯样本进行采样得到点云数据

选用RS-LiDAR-M1(B3样件)对楼梯进行采样，该激光雷达采用MEMS固态激光雷达的方式，测量距离高达200米，水平测角120°(-60.0°～+60.0°)，垂直测角25°(-12.5°～+12.5°)，点云数据较为稠密，对于真实楼梯样本数据损失较少，便于后续点云处理；将激光雷达通过以太网与工控机相连，驱动激光雷达，对目标楼梯数据进行采样；楼梯采样视角如图2所示；

激光雷达实物图如图1所示，楼梯原始稠密点云如图3所示

步骤二：对点云文件进行预处理

(1)降采样：在获取场景的点云时，由三维激光搭配云台扫描得到的原始点云数据往往过于稠密且复杂，其中除了目标点云外还包括障碍物、噪声、遮挡等等，对后续处理极为不利；为了提高后续环节的运算速度与处理效率，必须对原始点云进行稀疏化，减少待处理点云的数量；同时对于环境中的噪声与障碍物进行去除，降低干扰，从而降低后续所有环节中遍历算法的时间复杂度，进一步提高效率；对于三维点云的常用滤波稀疏化的方法有四种，即条件滤波、半径滤波、统计滤波、体素滤波；本方法中采用体素滤波进行点云的稀疏化，体素滤波是在空间内以一定参数模拟出连续的体素立方，在每个立方空间内对其中的点求均值，用来代替整体的状况；对于原始点云而言其点云基数大，不适于基于点云周边点遍历的方法进行滤波；楼梯体素滤波点云数据如图4所示；使用素材的原点云经体素滤波后规模由4000000多个降至90000多个，待处理的容量缩小为原来的2.25％。观察处理效果可发现点云的细节几乎没有损耗，尤其是待处理的台阶部分特征保留完整。算法耗时约0.3s。

(2)区域分割：在稀疏化的点云中，一方面存在被向下采样压缩的障碍物噪声等，另一方面由于室内多墙体的存在使得环境复杂，后续基于墙面的分析方法难以使用，故需要提前对小的浮空点云进行剔除、大的点云进行分割；当前研究中主要方法为基于法线估计的区域生长以及基于欧氏距离判定的欧氏分割，由于当前环节区域生长在时间上的消耗过大，同时欧氏分割对于当前情况的适用性较好，本方法中采用欧氏分割对点云进行处理；楼梯欧式聚类分割点云数据如图5所示；可观察到，除了目标点云部分，其余墙壁与噪声点云均被剔除，进一步减少了后期处理的点云规模，极大减少了后期发现估计与边缘测绘等算法中的时间复杂度，细节保留完整，分割效果能满足后期处理需求。

步骤三：楼梯点云样本处理

对于预处理后的楼梯点云样本进行处理，楼梯潜在区域检测，墙壁检测，墙壁去除，将楼梯立面进行PCA主元分析，计算楼梯立面质心，楼梯立面包装盒参数估计等；

(1)墙壁检测：为了在复杂点云样本中获取最好的墙面拟合效果，本方法中采用RanSAC算法进行拟合，其鲁棒性较强，可适应周围噪声较多的环境，一般实现中该算法通过迭代原始数据点得到最优的拟合参数，数据点中可包含大量的噪点而不影响拟合效果，使其在本发明设计中的拟合效果明显优于最小二乘等其他方法；楼梯取样示意图如图6所示；

(2)墙壁去除：当前方法对于点云中的墙面立面需要进行剔除，故需要对墙面进行检测，然后根据其法线方向判定楼梯侧，对另一侧点云进行删除，同时对楼梯侧进行取样，一方面对于整体点云而言可以删除杂点，降低点云容量，另一方面对于后续PCA操作可以有效降低拟合时间，增加拟合精准度；去除墙面后的楼梯点云如图7所示；

(3)PCA主元分析及楼梯立面包装盒：利用PCA主元分析法获得点云的三个主方向，获取质心，计算协方差，获得协方差矩阵，求取协方差矩阵的特征值和特长向量，特征向量即为主方向；利用获得的主方向和质心，将输入点云转换至原点，且主方向与坐标系方向重回，建立变换到原点的点云的包围盒；包装盒示意图如图8所示；包装盒的宽度即为楼梯的宽度估计；质心连线是一条斜率与台阶基本一致，方向沿着台阶面中心的折线；质心连线示意图如图9所示；由于对阶梯立面点云进行欧氏聚类分割后其容器center_list顺序为乱序:

center_list＝{P1,…,P4,…,Pn,...} (10)

其中Pn为楼梯立面点云质心坐标，不仅相邻点不能保证顺序相连，而且其中还包括了部分干扰点云的质心信息，为了得到准确的质心连线，将质心按z坐标从低到高进行排序，排序后将各个质心的x坐标相减则为楼梯踏面的长度，将各个质心的z坐标相减楼梯立面的深度；

(4)楼梯参数估计及耗时：楼梯宽度约为112cm，立面深度约为15cm，楼梯踏面宽度约为31cm，耗时约0.41s。

Claims (1)

1.一种基于激光雷达的楼梯参数自主检测方法，步骤如下：

步骤一：激光点云数据预处理

(1)去除无效点：对点云进行搜索遍历剔除无效点；

(2)降采样：对原始点云进行稀疏化，减少待处理点云的数量；同时对于环境中的噪声与障碍物进行去除，降低干扰，从而降低后续所有环节中遍历算法的时间复杂度；采用体素滤波进行点云的稀疏化，体素滤波是在空间内以参数模拟出连续的体素立方，在每个立方空间内对其中的点求均值，用来代替整体的状况；依据点云数据坐标，确定点云的最小外接长方体，其长宽高为l_x,l_y,l_z；同时设置体素栅格边长为cell，将X，Y，Z三轴均等划分为M，N，L份，则最小外接长方体被划分成M*N*L个体素小栅格；

(3)区域分割：采用欧氏分割对点云进行处理，实现点云的区域分割；

步骤二：楼梯点云样本处理

(1)楼梯潜在区域检测：由于设计中场景信息采集传感器扫描位置的特殊性，导致扫描台阶水平面存在部分缺失，然而墙面却可以较为完整的扫描得到，故通过先定位墙体，后定位楼梯位置的方法完成对楼梯潜在区域的检测；

(2)墙壁检测：采用RanSAC算法进行拟合，该算法通过迭代原始数据点得到最优的拟合参数，估算迭代次数m，用p表示一次迭代过程中从点云数据集内随机选取出的点为均平面内点的概率，用q表示每次从点云数据集中选取一个局内点的概率，如下式所示：q＝平面内点的数目/点云数据集的数目，用n来表示拟合平面模型最少需要的点云数据个数，可知有如下关系：

1-p＝(1-qⁿ)^m(2)

对上式进行取对数变换，得到对于迭代次数m而言在迭代过程中的标准差为：

上式用于估计在整个拟合过程中所需要的迭代次数m，RANSAC算法通过反复选择数据中的一组随机子集来达成目标，被选取的子集就被设为局内点，定义迭代次数m最大M次，预设拟合平面模型最少需要的点云数据个数n为N，预设初始用于拟合平面的局内点为a个，用初始平面模型去测试所有的其它点云数据，如果某个点适用于估计的模型，认为它也是局内点，增加局内点的个数，不断进行迭代直到局内点个数>n或迭代次数>m；

(3)墙壁去除：根据墙面法线方向判定楼梯侧，对另一侧点云进行删除，同时对楼梯侧进行取样，已通过RanSAC方法完成了墙面拟合过程，得到了其拟合参数coff＝[[A₁,B₁,C₁,D₁]…[A_n,B_n,C_n,D_n]]，其中A,B,C,D,为拟合平面的系数，下标代表拟合平面的编号；对于拟合的墙面1对应参数coff[0]＝[A₁,B₁,C₁,D₁]而言，通过遍历其对应点云簇S₁＝[¹p₁,¹p₂,…¹p_n]，p的上标代表点云簇的序号，p的下标代表每个点云簇中点的序号，每个点p包含x，y，z三个坐标；S下标代表点云簇的序号；计算其到墙面距离如下：

以上式所得距离为依据，删除墙面中的点，包括墙面内部及距拟合墙面δ阈值距离内的点：计算点云簇距离该平面法线同侧的距离均值为dis_p与反向距离均值为dis_n如下，其中k_p为点云个数，p的右上标+代表与该平面法线同侧的点云，-代表与该平面法线异侧的点云：

仅保留dis_p和dis_n小于±0.6*l_{height_max}的平面，其中l_{height_max}为默认最大楼梯宽度，该平面为楼梯所依附的墙面，若|dis_p|>|dis_n|，则楼梯区域位于墙面法向正方向n＝(A,B,C)，反之则楼梯区域位于墙面法向反方向；

(4)PCA主元分析及楼梯立面包装盒：保留去除墙壁后楼梯立面点云，再次进行欧式聚类，得到每一片楼梯立面点云；对于任一个楼梯立面点云M_j(x,y,z)＝[^jr₁,^jr₂,…,^jr_n]，其中r为楼梯立面中的点，r上标表示楼梯立面的序号，r下标表示点集内点的序号，M为楼梯立面点集，M下标代表楼梯立面的序号；楼梯立面实际上并不为一个理想的平面，而是近似存在一定厚度的长方体，为了求解其所代表阶梯的宽轴向，可使用主元分析法PCA对其进行求解，先对楼梯立面点云进行去均值化，然后求解其协方差矩阵：

随后使用SVD分解：

Cov(X,Y,Z)＝U∑V^T(8)

得到奇异值矩阵左奇异矩阵计算U＝[pc₁,pc₂,pc₃]，计算主元pc₁方差贡献率：

其中，e1、e2、e3分别表示楼梯立面点云协方差矩阵的特征值；U表示协方差矩阵SVD分解的左奇异矩阵，pc₁,pc₂,pc₃代表分解楼梯立面点云后正交的三个主元方向；α₁为对应e1主元方向上的方差贡献率；用c₁、c₂、c₃分别表示每片楼梯立面点云的质心；

当α₁此值大于给定百分比阈值时，认定楼梯主元识别无误；如上方法利用PCA主元分析法获得点云的三个主方向，获取每一片楼梯立面点云的质心，计算每一片楼梯立面点云的协方差，从而获得每一片楼梯立面点云的协方差矩阵，求取协方差矩阵的特征值和特征向量，特征向量即为主方向；利用获得的每一片楼梯立面点云的主方向和质心，将每一片楼梯立面点云的输入点云转换至原点，且主方向与坐标系方向重合，建立变换到原点的每一片楼梯立面点云的包围盒；包装盒的宽度即为楼梯的宽度估计；质心连线是一条斜率与台阶一致，方向沿着台阶面中心的折线；将各个质心坐标相减则为楼梯踏面的长度和楼梯立面的深度。