激光SLAM 自主导航的线段特征提取混合算法研究

桑 迪，周 军，皇攀凌，李 蕾

（1.山东大学机械工程学院，山东 济南 250061；2.山东大学高效洁净机械制造教育部重点实验室，山东 济南 250061；3.齐鲁工业大学（山东省科学院），机械与汽车工程学院，山东 济南 250353）

1 引言

自主导航作为一种新的导航技术已经广泛应用于移动机器人行业，移动机器人通过所携带的激光、视觉、里程计等传感器，在移动过程中完成构建地图与定位。SLAM（simultaneous localization and mapping）为自主导航的关键技术，也称为CML（Concurrent Mapping and Localization），即时定位与地图构建，或并发建图与定位[1-2]。SLAM 描述的是机器人在陌生的环境中通过自身携带的传感器，根据前一时刻的位姿以及里程计数据估计现在时刻的位姿，再通过激光视觉传感器所获得的数据进行修正，不断构建增量式地图，同时对原始的地图进行更新[3]。基于激光雷达SLAM自主导航中，环境对激光雷达的影响较小，具有较高的定位导航精度，缺点是传感器所获得的环境的信息少[4-8]。对原始数据的特征提取处理对建图的精度以及机器人在环境中的定位有着重要的影响[9-10]。

国内外学者针对线段特征提取的方法进行了深入的研究：文献[11]针对室内环境相对简单的结构特征，基于Hough 变换进行特征提取，充分利用有限的测量数据，获得较好的线段特征。但是霍夫变换特征提取算法的最大难点就在于对线性参数的量化确定。文献[12]针对D&C 算法在明显呈凹形分布的区域中，通过检测两条直线的斜率差值是否小于某一阈值，有效解决了凹形区域对激光雷达D&C 特征提取算法失效的情况，但是没有考虑到噪声的影响，在直线拟合的时候会造成拟合误差大。文献[13]针对直接从激光雷达扫描数据不精确、不稳定的提取特征点的问题，提出一种基于分割-合并的线段特征提取方法，其先采用IEPF 的方法判断是不是分割点，然后通过分割的两个线段之间的角度来判断这个分割点是不是分割，这样减少了线段的过度分割的问题。文献[14]在构建占用栅格地图时，基于的Hough 进行局部雷达进行特征提取，构建新的障碍物边界或将先前提取的边界的延伸，该方法适用于由线段组成的办公室环境地图的构建。

针对特征地图的构建问题：首先建立激光雷达的数学模型，然后分析PDBS、IEPF 分割算法的优缺点，提出了一种混合方法将PDBS 与IEPF 方法相结合，PDBS 和IEPF 的阈值设定通过激光雷达的系统误差等进行选择，提高区域分割的性能，最后通过对激光雷达的一组数据进行处理，比较特征提取与实际的环境特征的相似程度验证本论文所提混合方法的有效性。

2 激光雷达数据模型

激光雷达是SLAM 自主导航时构建地图及定位获取数据的来源，为了便于数据的处理首先建立激光雷达的数据模型，通过坐标的变换将传感器所获得的极坐标系下的数据转换为直角坐标系，为了在区域分割时提取线段特征，采用线性最小二乘法进行拟合。

2.1 激光雷达

激光雷达的原理是一种基于时间飞行原理的技术（Time of Flight，TOF），通过激光发射器发出脉冲波发出和接收时间计算出激光雷达到被测物体之间的距离s：

2.2 传感器数据的模型

选用单线激光雷达LMS111-10100，激光雷达的具体参数，如表1 所示。

表1 LMS111-10100 参数Tab.1 LMS111-10100 Parameters

激光雷达的输出是一系列的测得的距离的信息，每两个相邻的光束之间的角度是角度分辨率，如图1 所示。激光雷达的数据是按照逆时针排序从起始角度到终止角度，其中α 表示两个相邻激光束之间的角度，即激光雷达的角度分辨率，用αs表示测量开始的角度。在此研究中的角度选用激光雷达的分辨率选用0.5°，测量范围为270°，测量起始角度为-45°，终止角度为225°，共采集到541 组数据。

图1 传感器测距模型Fig.1 Sensor Ranging Model

为了便于数据处理，每一个激光束的极坐标系转换为直角坐标系，所测得的距离相对于传感器的直角坐标系分别用表示xi，yi求解方法为：

3 特征提取算法对比与改进

对传感器建立模型之后对数据进行处理后，为了将扫描的线段形状特征提取出来，首先将之前的数据进行区域分割，区域分割的目的是找出角点和断点，将测得的点分割为不同的区域，再拟合出特征线段，本节主要是通过分析PDBS、IEPF 这两种常用的区域分割方法优缺点，提出一种混合的算法。

3.1 PDBS 算法

PDBS（point distance based methods）线段分割的方法，是根据激光雷达两个相邻点之间在直角坐标下的距离di与设定的阈值ε相比，如果两个相邻测量点之间的距离di大于阈值ε，则认为两个点属于不同的区域。将激光雷达获得的一组数据从第一个点依次计算两个相邻点之间的距离，通过与设定的阈值ε 相比较，将所测得的点分割为不同的区域后进行最小二乘法拟合提取线段。

PDBS 方法中阈值ε 的设定对线段的分割具有重要的影响，阈值设定太小的时容易出现过分割的现象，阈值设定过大的时候又容易出现分割不足的现象，从激光雷达中获得的连续的几个测量值，如图2（a）所示。设定的不同阈值分割相同的区域时的影响，如图2（b）、图2（c）所示。Pi与Pj之间距离的值大于阈值ε，区域被分为两个区域，在拟合时拟合出两个线段，如图2（b）所示；由于距离的值小于阈值，区域未被分割，在拟合时，线段拟合为一条线段，如图2（c）所示。这对于在构建地图时线段特征的数量有较大的影响。PDBS 算法在角点处的会出现分割失败的情况。从现实环境墙角处所得出的数据，如图3（a）所示；提取出的正确的线段，如图3（b）所示；但是在利用PDBS 方法对这一区域进行阈值分割时，由于并没有提取出角点，会将此处的所有点划分在一个区域内，在对此区域进行最小二乘法进行数据拟合时，拟合出现错误，如图3（c）所示。

图2 PDBS 过分割与欠分割问题Fig.2 PDBS Over Segmentation and Under Segmentation

图3 PDBS 在角点处拟合失败Fig.3 PDBS Fitting Failure at Corner

3.2 IEPF 方法

IEPF（iterative end point fit）的原理步骤如下：

（1）从传感器提取出的数据，原始数据转化为直角坐标系，用黑色点从P1到Pn的坐标点，如图4（a）所示。

（2）将数据中的第一个点P1和最后一个点Pn连接形成一条直线L1，然后计算从P2点到倒数Pn-1点之间所有点到L1的距离，并找出距离最远的Pj点，与设定的阈值相比如果超过设定的阈值ε 相比，如果最远距离小于设定的阈值则认为是一条直线，不再划分；如果最远距离大于ε 再将所有的点分为两部分从P1到Pj点及Pj+1点到Pn点两部分，再按上面的步骤对以步骤操作，直到不能再划分为止，如图4（b）、图4（c）所示。

图4 IPEF 分割过程Fig.4 IPEF Algorithm Segmentation Process

IEPF 算法能够在线段的交点处将线段的角点提取出来，避免PDBS 在两个线段的交点处的特征提取错误的问题。但是IEPF 方法无法判断一个点是否是断点，如图5 所示。Pi与Pj是断点，如图5（a）所示；正确分割的情况，如图5（b）所示。将区域分为两个部分，在拟合时拟合出两条线段；但是实际上IEPF 无法根据两个相邻点之间的距离判断Pi与Pj是否是断点，在拟合时仍被认为是同一直线上的点，如图5（c）所示。拟合出的是一条直线。

图5 IEPF 分割问题Fig.5 IEPF Segmentation Problem

3.3 算法改进

根据PDBS 方法及IEPF 方法的优缺点提出了一种改进的区域分割的方法：首先用PDBS 方法对区域进行分割，然后利用IEPF 算出角点，用角点对数据再次分割，这样就避免了PDBS 方法在拟合的时角点处拟合失败的问题。研究选用SICK-LMS111-10100 激光雷达对空间进行数据的采集。阈值的选择，查阅机关传感器的误差的相关数据，如表2 所示。根据系统误差和统计的误差，同时在本实验中，选用的测量场景为10m 以内的测量范围，在这里选用PDBS 的阈值选用阈值为35mm。同时IEPF 的阈值设定为35mm。为了便于数据处理，首先定义重要的数据，如表3 所示。

表2 传感器的误差Tab.2 Sensor Errors

表3 参数定义Tab.3 Definition of Parameters

具体步骤如下：

（1）将原始数据的极坐标转变为直角坐标系下，极坐标中的第i 个测量点的极坐标的值为：

（3）两个点之间的最小的距离与thd_pdbs 之间的关系，dis_pdbs 大于thd_pdbs 时，将断点提取出来，存放到数组con 中；如果dis_pdbs 小于thd_pdbs 时，继续2、3 步，直到所有的断点都提取出来；

（4）如果con 数组中有数据与前后数据在的下标相差一个时，认为这是一个噪声点，删除；

（5）将拟合直线：将x_axis、y_axis 相邻下标分别为div_sub（i_con）和div_sub（i_con+1）的两个点拟合成直线，在此处，在步骤3 处的噪音点所在区域不做直线拟合，计算出k、b；

（6）依次计算点到直线之间的距离，求出最大的值dis_max_iepf，并记录他的下标值i_con；

（7）计算dis_max_iepf 与thd_iepf 的关系：当dis_max_iepf的值大于thd_iepf 时，将dis_max_iepf 的下标i_con 存入数组div_sub，并将数组重新排序；跳到步骤5；当dis_max_iepf 的值小于thd_iepf 时，记录下这个区间，后期检测直接跳过这个区间；

（8）当div_sub（i_con）的值超过541 时停止计算。

4 仿真实验

在实际环境中搭建一个由线段的组成的环境，并通过激光雷达的一组数据保存为文本文档，通过MATLAB 对原始的数据进行分析。

现实环境，在走廊的放置桌子，通过激光雷达收集现场的数据，如图6 所示。

激光雷达的一次扫描541 个点，通过串口助手将数据读出，并将其存为文本文件，以便于后期的分析，将原始的数据转换为直角坐标系并在MATLAB 中的图像，如图7 所示。对原始的数据利用PDBS 方法处理，在使用PDBS 方法拟合的时候在两个线段的交点处，算法中无法将两个线段分开拟合，造成拟合出的直线与实际的环境相差很大，如图8 所示。对原始图像的数据处理利用IEPF 方法提取，通过对两个线段之间的交点进行处理，线段的拟合效果提高，但是IEPF 在拟合中，存在噪音或者出现断点时，会出现拟合的直线与实际环境相差较大，如图9 所示。

图7 原始数据图像Fig.7 Original Data Image

图8 PDBS 方法特征提取Fig.8 PDBS Algorithm Feature Extraction

图9 IEPF 方法特征提取Fig.9 IEPF Algorithm Feature Extraction

对原始的数据进行数据处理利用PDBS 与IEPF 方法进行处理，在数据的处理过程中，首先通过PDBS 提取出断点，然后再通过IEPF 提取出角点，拟合出的线段较单独使用PDBS、IEPF 线段拟效果明显，验证了算法的有效性，如图10 所示。

图10 PDBS 与IEPF 结合特征提取Fig.10 PDBS Algorithm Combined with IEPF Algorithm for Feature Extraction

5 结论

针对基于激光SLAM 自主导航建图时特征线段提取方法进行研究，通过对PDBS 和IEPF 线段特征提取的方法进行比较，对PDBS 对两个线段的交点处的拟合失效的问题进行分析，同是结合IEPF 的方法在断点处的拟合失败问题，将两个算法进行互补，首先利用PDBS 方法找出断点，然后再利用IEPF 方法找出角点，最后将数据拟合，对比单独使用PDBS 和IEPF 的方法，混合特征线段的提取与实际环境更接进，验证了混合算法的有效性。