2D激光SLAM中特征角点的提取方法

刘 朋，任工昌，何 舟

（陕西科技大学机电学院，西安710021）

2D激光即时定位与构图（Simultaneous local⁃ization and mapping，SLAM）技术由于其相对简单，发展时间长，技术较为成熟，目前被广泛应用于物流配送、家庭服务等类型的自主移动机器人中。该技术的基本方法可以概括为：机器人通过激光雷达对周围环境信息进行观测，同时利用机器人的控制信息（里程计）实现对机器人的即时定位与地图构建。因为里程计信息具有较大的累积误差，为了获得机器人轨迹的长期精确估计，需要将机器人所感知的环境信息与之前建立并实时更新的地图进行匹配。所以，匹配是SLAM中的一个关键问题。

在匹配算法中，观测信息量的大小直接影响着算法的精度和效率。如果能从激光雷达扫描数据中提取出较为精确的特征信息，将会大幅减少整个SLAM算法的运算量。在激光雷达数据中普遍存在断点、角点、线段和圆弧段等几类特征［1］。由于环境中存在障碍物，造成断点和线段特征会随着观测位置的变化而变化。弧线段特征在室内环境下不多见，且其相对复杂，探测比较困难。鉴于以上原因，本文以提取角点特征进行匹配，可以很好地满足SLAM算法的需要［2⁃3］。

目前，对于角点特征的提取，一般都是先提取线段特征，再计算两条线段的交点以获得角点特征。线段提取方法中，序惯类的基于点间距离的分割（Point⁃distance⁃based segmentation，PDBS）算法和连续边沿跟踪（Successive edge following，SEF）算法［4］，是两种最简单的线段类特征提取方法，但是没有考虑激光雷达以固定间隔进行扫描的特点，效果往往较差。直线跟踪（Line tracking，LT）算法［5］对阈值敏感，很容易错误地将应属于下一条线段上的点合并到当前线段上；递归类的迭代适应点（Iterative end point fit，IEPF）算法［6⁃7］和分割与合并（Split⁃merge，SM）算法［8⁃9］，也对噪声和阈值敏感，当采用固定阈值时很容易造成欠分割或过分割现象，而且运算量较大。Hough变换算法［10⁃11］特征精度较高，但计算量较大，难以保证局部地图构建的实时性。满增光等［12］提出了一种通过计算角点函数的方法直接提取角点特征，但该算法在进行角点特征滤波和角点函数计算时，计算量仍然偏大，并且存在从正反两个方向提取时得到不同结果的可能。

综上所述，目前对于从激光雷达扫描点集中提取特征的方法较多，但是依然缺少一种高效、简便和准确的算法。因此，本文根据激光雷达扫描的特点，提出了一种通过点集分割、线段合并和求取交点相结合的角点特征提取方法。该方法只需要一次计算出相邻点所构成的两条直线间的斜率差，即可以实现对点集的分割。与IEPF和SM等递归类算法相比，不需要进行迭代，在一定程度上降低了计算量。另外，该算法避免了由于扫描点之间的间隔造成提取角点误差较大的问题；同时，利用两点拟合直线代替最小二乘法，又可以进一步降低计算量。

1 算法原理

本文提出的算法包括3部分。首先使用激光雷达扫描获得的点集，通过计算斜率差对其进行初步分割。然后，计算分割后每条线段的斜率，对过分割的线段依据线段间的连接关系进行合并，并获得可能的角点位置。最后，对角点进行修正并计算角点坐标。算法总体框图如图1所示。

图1 算法总体框图Fig.1 Overall block diagram of algorithm

1.1 线段分割原理

如图2所示，从O点向直线l以等角度Δθ的间隔画直线，交点依次为P1、P2、P3、P4、…，O点到交点的长度依次为ρ1、ρ2、ρ3、ρ4、…，然后，由P1点向OP2做垂线相交于P1'点，P2点向OP3做垂线相交于P2'点，P3点向OP4做垂线相交于P3'点，则φ1为直线l与P1P1'的夹角，同理可得φ2、φ3。其中∠P1OP1'=∠P2OP2'=∠P3OP3'=Δθ。

图2 扫描点示意图Fig.2 Diagram of scanning points

由几何关系可得

φ3=φ2+Δθ=φ1+2⋅Δθ

因为

当Δθ很小时，令ki=tanφi，则有

所以

tanφ3-tanφ2≈tanφ2-tanφ1≈0

由此可得，当相邻两点处的ki值之差很小时，就认为该两点处于同一条直线上，否则该点为断点或独立点。

斜率差计算公式为

如图3所示，当点Pi和Pi+1分别为直线L1的末点和直线L2的起点时，Δk（i）和Δk（i+1）的值如图4所示，在直线L1的末点Pi和L2的起点Pi+1对应的Δk（i）和Δk（i+1）处峰值非常明显，且该两处峰值的符号相反。当|Δk（i）|>dkth，|Δk（i+1）|>dkth，且Δk（i）·Δk（i+1）<0时，第i点为前一条直线的末点，第i+1点为后一条直线的起点。其中dkth为线段分割阈值，该值与激光雷达的测量误差有关，可通过实验的方法选取，同时为了避免出现欠分割的情况，dkth的值在选取时应尽量偏小。

图3 断点示意图Fig.3 Breakpoint diagram

图4 断点处Δk分布示意图Fig.4 Distribution diagram ofΔk at the break point

如图5所示，当点Pi为直线L1和直线L2的交点时，则Pi为两条直线的角点，Δk（i）的值如图6所示，在角点Pi处Δk（i）有较明显的峰值。当|Δk（i）|>α·dkth，|Δk（i）|>|Δk（i-1）|，且|Δk（i）|>|Δk（i+1）|时，第i点为角点，即为前一条直线的末点，同时也是后一条直线的起点。其中α为角点线段分割阈值系数，该值也可通过实验的方法选取，一般取值为0.6。

图5 角点示意图Fig.5 Diagram of angular points

图6 角点处Δk分布示意图Fig.6 Distribution diagram ofΔk at the angular point

假设激光雷达扫描数据每帧有n个扫描点，本方法使用式（2）计算n-2次得到相邻两点的Δk，与相应的分割阈值进行比较即可获得较为精确的线段分割结果和线段连接关系。而如果使用IEPF算法，由于需要寻找与直线距离最大的点不断进行迭代，至少需要计算2n次以上点到直线的距离才可得到相同的分割结果。所以，与IEPF等迭代类算法相比，本算法具有计算简单、计算量小的优点，特别是当扫描点数或分割线段较多时，这种优势更加明显。

1.2 线段合并

经过初步分割后，得到的线段集L为

L={(Lbi，Lei，Lpi)，i=1，2，…，m}

式中：Lbi表示第i条线段的起点在点集P中对应的点数；Lei表示第i条线段的终点在点集P中对应的点数；Lpi表示第i条线段是否与第i+1条线段相连，Lpi为1表示两条线段由角点相连，否则为断点断开。

为了避免出现欠分割的情况，dkth的值一般选择可取范围的下限，所以依据Δk（i）值进行分割时可能会出现过分割的情况。计算每条线段Li的斜率kLi，如果相邻的两条直线相交且其夹角tanθ小于线段合并阈值θth时，即可将此两条直线进行合并。经过实验分析，θth一般取值为0.3，当θth大于0.4时会造成过多的误合并，而当θth小于0.2时合并效果不明显。

如果线段集中Li和Li+1两条直线相交，其斜率分别为kLi和kLi+1，如图7所示，则两条直线的夹角为

图7 线段夹角示意图Fig.7 Angular diagram of two intersecting lines

1.3 角点定位与特征提取

激光雷达在对环境进行扫描时，获得的是离散的扫描点。因此，从这些扫描点中提取出的角点特征位置与真实的物理角点位置之间存在一定的误差，特别是物理角点离激光雷达较远的情况，这种误差将会很大。为了减少提取出的角点特征位置与真实物理角点位置之间的差值，需要根据初步线段分割后获得的可能角点进行精确定位。

如图8所示，58～63点在一条直线上，64～70点在另一条直线上，“★”位置为该两条直线相交的真实角点位置。按照前述分割方法，64点处的Δk最大（图9），是两条线段相交的角点，很明显此位置与真实角点位置之间差值较大，所以不能直接使用线段分割时获得的可能角点位置直接作为角点特征。

图9 真实角点附近Δk分布Fig.9 Distribution ofΔk around the real corner point

出现此种情况的主要原因在于扫描点是离散的，而真实的角点出现在相邻两个扫描点之间。如图8所示，64点应在第2条直线上，但是分割后其作为第1条直线的末点，同时作为第2条直线起点，因此导致角点位置的差异。

进一步分析Δk发现，当Δk（i）符合角点特征，出现峰值时，如果|Δk（i）-Δk（i-1）|<|Δk（i）-Δk（i+1）|，则角点应在第i-1和第i点之间，相反，角点则在第i点和第i+1点之间。如图8、9所示，真实角点位置在第63点和第64点之间，第63点为前一条线段末点，第64点为后一条线段起点。分别拟合出两条直线，求出交点即为角点特征。

图8 真实角点与扫描点位置Fig.8 Position of the real corner point and scanning points

在进行直线拟合时，目前普遍采用的方法是最小二乘法，此方法的优点是拟合时考虑到了每一个点，尽量减小了由激光雷达扫描误差而引起的拟合误差，但该方法的缺点是运算量相对偏大。为了减小运算量，本算法已经剔除掉了不在一条直线上的点，在待拟合的点集中找出起点、末点和中间点，将点集分为前后两段，然后算出前后两部分点集坐标的平均值作为新点，最后以此两点来拟合直线，这样可以大幅减小直线拟合时的计算量。

2 算法详细描述

激光雷达每扫描环境一次，返回一组有序二维激光雷达数据，将此数据预处理后得到点集为

P={(θi，ρi)，i=1，2，…，n}

其中θi和ρi分别为扫描第i点时转过的角度和返回的距离。

步骤1 根据点集P，利用前述原理中式（2）计算相邻扫描点斜率的差值Δk，即

式中i=2，3，…，n-1。然后，根据Δki、Δki+1与dkth的关系对点集P进行分割，获取初步分割后的线段集L。其中

L={(Lbi，Lei，Lpi)，i=1，2，…，m}

步骤2 如果线段集中Lpi=1，则取出Lbi、Lei、Lbi+1、Lei+1点对应坐标，计算线段Li和Li+1的斜率kLi、kLi+1；然后依据式（3）计算tanθ，如果tanθ<θth则线段Li和Li+1合并，将线段Li中的末点Lei修正为Lei+1，并将线段Li+1从线段集L中删除。

步骤3 在合并后的线段集L中，当第i条线段的Lpi=1时，则在点集P中取出该线段末点Lei的对应点Pq及其前后两点Pq-1和Pq+1，并依据式（2）计算出相应的Δk（q-1）、Δk（q）、Δk（q+1）；如果|Δk（q）-Δk（q-1）|<|Δk（q）-Δk（q+1）|，则将线段Li的末点Lei修正为点Pq-1，否则将线段Li+1的起点Lbi+1修正为Pq+1。

3 实验与结果分析

实验数据来自Cartographer ROS提供的德意志博物馆Deutsches Museum的2D激光SLAM数据集［13］。算法使用Matlab2016b，计算机CPU为Intel Core i5⁃6300U，内存8G。实验中各参数为：θth=0.3，dkth=1.0，α=0.6。从数据集中选取了10帧数据进行角点特征提取和直线拟合。由于篇幅限制，图10和图11给出了第5帧和第10帧的点集数据和特征提取结果，其中圆点表示激光雷达扫描点，直线为拟合的线段，“*”位置表示提取的角点特征位置。

图10 第5帧点集数据和提取的角点特征Fig.10 Scanning point data and extracted corner features from frame 5

图11 第10帧点集数据和提取的角点特征Fig.11 Scanning point data and extracted corner features from frame 10

为了验证算法的正确性，从结果中挑选了一处具有明显角点特征的环境扫描数据进行分析，如图12所示。其中A、B、C为提取的角点位置，线段AB和BC为拟合的直线。由于移动机器人在不同位置提取的特征在激光雷达坐标系下具有不同的位置，但是两个特征的相对位置却保持不变。因此，为了衡量提取角点特征的精度，本文以两角点之间的距离和角点两直线的夹角作为评判依据，即线段AB和BC的长度及∠A、∠B和∠C的大小。

图12 提取角点位置示意图Fig.12 Location diagram of extracted corner features

如表1所示，根据10帧数据提取的结果，线段长度与均值之间的差值较大的是线段BC，差值范围为［-0.009，0.011］，提取的角度与均值之间差值较大的是∠A，差值范围为［-1.01，0.86］。由此可见，本文提出的角点特征提取算法具有较高的定位精度，可以满足自主机器人的建图与定位需要。

表1 角点特征提取结果Table 1 Results of extracting corner features

为了进一步验证本文算法在直线拟合方面的精度和计算效率，从上述10帧数据中选取了5帧，分别采用本文直线拟合方法和最小二乘法对分割后的线段进行拟合，获得角点B和B'点坐标，结果如表2所示。由此对比结果可得，本文算法计算的B点与最小二乘法计算的B'点之间的距离范围为［0.003，0.007］，所以使用本文算法可以得到与最小二乘法基本相同的结果。表3为本算法与使用最小二乘法进行拟合时计算效率的对比，通过对比发现，本算法在直线拟合时相较最小二乘法有较为明显的优势，单帧扫描数据提取特征角点的用时均小于10 ms。

表2 特征点提取结果对比Table 2 Comparison of the results extracting feature point between the two algorithms

表3 两种算法运算效率对比Table 3 Comparison of computational efficiency between the two algor ithms

4 结 论

本文提出了一种利用激光雷达扫描数据直接提取角点特征的算法。该算法使用激光雷达中获得的扫描点对应矢径长度和角度，计算相邻点的斜率差对点集进行初始分割。然后，计算分割后每部分点集对应线段的斜率，对过分割的点集进行合并。最后，通过计算相邻两直线的交点对角点特征进行定位和提取。在进行点集分割时，本算法只需要一次计算出相邻点的斜率差，与其他迭代类算法相比，减小了计算量。在对直线进行拟合求取角点时，已根据角点处斜率差的分布特点对角点进行了定位，实现对点集分割结果的修正，所以使用两点拟合直线代替传统的最小二乘法，在满足精度的前提下，也在一定程度上提高了计算效率。通过实验表明，本算法能够准确获得角点位置，并且精度较高，特别适用于使用嵌入式系统开发的自主机器人SLAM算法中。

本算法仅完成了角点特征的提取，如果需要可以在此基础上进一步提取线段、断点或其他特征。