[发明专利]一种基于激光雷达的楼梯参数自主检测方法

权利要求书

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主元分析法获得点云的三个主方向，获取每一片楼梯立面点云的质心，计算每一片楼梯立面点云的协方差，从而获得每一片楼梯立面点云的协方差矩阵，求取协方差矩阵的特征值和特征向量，特征向量即为主方向；利用获得的每一片楼梯立面点云的主方向和质心，将每一片楼梯立面点云的输入点云转换至原点，且主方向与坐标系方向重合，建立变换到原点的每一片楼梯立面点云的包围盒；包装盒的宽度即为楼梯的宽度估计；质心连线是一条斜率与台阶一致，方向沿着台阶面中心的折线；将各个质心坐标相减则为楼梯踏面的长度和楼梯立面的深度。