0 引言
自主移动机器人近年来在工业、农业和服务业中得到了广泛的应用，它应当具备的基本功能就是环境的感知与定位。激光雷达具有较高的测距精度和实时性，故普遍被用作自主移动机器人的环境感知传感器[1]。利用激光雷达进行环境特征的提取，关键在于如何从激光雷达扫描点的距离数据中提取出可用于移动机器人定位和构建地图所需的特征线段或特征点[2]。
在移动机器人领域，研究人员提出了多种利用激光雷达距离数据提取直线特征的算法，这些算法可以大致分为两类：序惯算法和递归算法。序惯方法有基于点间距离的分割(point-distance-based segmentation，PDBS)算法、连续边缘跟踪 (successive edge following，SEF)算法和直线跟踪(line tracking，LT)算法。递归方法有迭代适应点 (iterative end point fit，IEPF)算法[3]和分割-合并(split-merge,SM)算法[4]。除此以外，还有不依赖于局部信息的霍夫变换(Hough transform,HT)算法[5]。其中，序惯类算法多存在阈值选取困难、无法对相交直线进行分割等缺点；递归类算法对阈值较为敏感，存在过分割或欠分割的现象，而且运算量较大。HT算法不适合实时应用，且由于它采用投票的方式确定直线，提取的直线严重依赖于扫描点的空间密集程度。
满增光等[6]提出了一种分开-合并框架，先对数据集合以某种准则递归地分割，分割后再把这些集合以某种准则递归地合并。在分开阶段，应用 IEPF算法对数据集合递归分割；在合并阶段，同时采用端点拟合(end point fit,EPF)算法和总体最小二乘法两种直线拟合误差作为合并条件的参考量对数据集合递归合并。该方法较好地解决了IEPF算法固定阈值所带来的过分割或欠分割的问题，降低了误分割率，取得了较好的特征提取效果，但是计算量依然偏大，计算效率较低[7-10]。
综上所述，目前从激光雷达距离中提出直线特征的方法较多，但普遍采用迭代计算，依然缺少一种高效、简便、准确率高的算法。本文综合上述方法的优点，提出了一种利用斜率差进行分割和特征点提取的方法。
1 算法原理
本文方法以激光雷达作为圆心，利用相邻点的斜率差来区分断点、角点或独立点。如图1所示，从O点向直线L以等角度Δθ的间隔画直线，假设交点依次为P1，P2，…，O点到交点的长度依次为ρ1，ρ2，…，然后，由P1点向OP2作垂线相交于P′1点，则φ1为直线L与P1P′1的夹角，同理可得φ2、φ3。由几何关系可得
φ3=φ2+Δθ=φ1+2Δθ
因为
当Δθ很小时，则有
(1)
令第i点处的斜率ki为
(2)
所以
k3-k2≈k2-k1≈0
图1 扫描点示意图
Fig.1 Scanning point diagram
由此可得，当相邻两点处的ki值之差很小时，就认为这两点处于同一条直线上，否则该点为断点或独立点。如图2所示，由O点分别向直线L1和L2以等角度间隔θ作射线，与直线L1交于点P1,…,Pi-1,Pi，与直线L2交于点Pi+1,…,Pn-1,Pn，其中ρi表示O点至Pi点的距离，Pi和Pi+1分别为直线L1和L2的断点。
图2 断点示意图
Fig.2 Breakpoint diagram
如图3中实线所示，在断点处k值有明显的峰值。为了消除相邻两点ki值微小的差值，令
Δk(i)=ki-ki-1
(3)
图3 断点处Δk分布示意图
Fig.3 The distribution diagram of Δk at the break point
则Δk如图3中虚线所示，在直线L1的末点Pi和L2的起点Pi+1对应的Δk(i-1)和Δk(i)处峰值非常明显，且这两处峰值的符号相反。同理，如图4所示，直线L1和直线L2相交于点Pi，则Pi为两条直线的角点。分别按照式(2)和式(3)计算ki和Δk，如图5所示。在角点Pi处Δk(i)有较明显的峰值。
图4 角点示意图
Fig.4 Angular point diagram
图5 角点处Δk分布示意图
Fig.5 The distribution diagram of Δk at the angular point
某次实验中激光雷达部分扫描点数据如图6所示。按照式(3)计算的Δk分布情况如图7所示，虽然由于扫描的误差而导致Δk波动，但在角点53处有明显的峰值。
图6 实验中部分扫描点数据
Fig.6 Some scanning point data in the experiment
图7 角点处Δk分布
Fig.7 The distribution of Δk at the angular point
2 算法详细描述
激光雷达每扫描环境一次，返回一组有序二维激光雷达数据，将此数据预处理后得到点集
P={(θi,ρi),i=1,2,…,n}
式中，θi、ρi分别为扫描第i点时转过的角度和返回的距离。
本算法分割断点和角点的算法如下：
(1)根据点集P，利用前述原理计算相邻扫描点斜率的差值Δk，即
(4)
i=2,3,…,n-1
因为激光雷达具有环形扫描的特点，故可将第n点作为第1点的前一个扫描点，将第1点作为第n点的后一个扫描点，即
如果相邻的3个扫描点斜率的差值Δki-1、Δki和Δki+1均大于阈值kth，且|Δki-Δki-1|>2kth，|Δki+1-Δki|>2kth，则认为第i点是第一类孤立点(图8中第28点)。找出所有第一类孤立点，删除后更新点集P。
图8 第一类孤立点
Fig.8 The first kind of isolated point
(2)更新点集P后，再依次计算第i个扫描点的θi与相邻两扫描点的θi-1和θi+1的差值，如果|θi-θi-1|>θth，且|θi+1-θi|>θth(θth为阈值)，则认为第i点为第二类孤立点(图9中第257点)。找出所有第二类孤立点，删除后再次更新点集P。
图9 第二类孤立点
Fig.9 The second kind of isolated point
(3)将去除孤立点后的点集P重新利用式(4)计算相邻扫描点的斜率差值，进行断点和角点的判断。判断规则如下：①如果|θi+1-θi|>θth，则第i点和第i+1点均为断点，分别为前一条直线上的末点和后一条直线的起点；②如果Δki和Δki+1均大于阈值kth，且ΔkiΔki+1<0，则第i点和第i+1点均为断点，分别为前一条直线上的末点和后一条直线的起点；③如果|Δki|>αkth(0<α<1)， |Δki+1-Δki|>0， |Δki-Δki-1|>0，且 (Δki+1-Δki)(Δki-Δki-1)<0，则第i点为角点，即前一条直线的终点，同时也是后一条直线的起点，其中，α为角点阈值系数。
(4)根据点集P中各点的属性，从第1点至最后一点依次分割，并计算每条线段的始末点坐标和线段的斜率，得到线段集L。
(5)由于激光雷达工作的特点，有可能会从某条直线的中间部分开始扫描，所以线段集中的第一条和最后一条线段可能属于同一条线段。如果第一条线段的始末点均在最后一条线段所在的直线上，则将这两条线段合并。
3 实验及结果分析
实验时以某楼端侧梯形平台为测试对象，该平台几何尺寸使用卷尺测量后如图10所示。选用SLAMTEC公司的RPLIDAR A2型激光雷达，在测试平台随机选取10个点进行扫描。算法在MATLAB平台下实现。实验中各参数如下：θth=2.5°，kth=0.95，α=0.7。
图10 实验测试平台
Fig.10 Experimental test platform
实验中第1次激光雷达扫描数据按照本文算法进行分割和特征提取的结果如图11所示。其中，实心圆点表示激光雷达的扫描点，方框表示根据算法提取的角点或断点特征，直线表示根据算法提取的直线段特征，圆圈表示激光雷达位置。由图11可以发现，该算法在特征点和特征线段提取方面可以获得与手工提取相同的结果。其他9次实验也可以得到相同的结论。
图11 第1次实验结果
Fig.11 The result of first experiment
为了进一步验证本算法提取线段特征的准确性，计算提取线段特征的长度和相对角度，并与实际测量值进行了对比。相对角度是在提取的特征线段中，以BA线段为基准，其他线段与BA线段的角度差作为评判依据。由于激光雷达扫描的特点，删除了部分因遮挡或距离限制而无法完整提取的线段特征。实验结果见表1和表2，线段编号与图10相对应。其中，最大差值百分比依据提取值和测量值差值的绝对值与测量值的百分比进行计算。
表1 提取特征线段长度与测量值对比
Tab.1 The length of the feature line is compared with the measured value m
位置序号线段及长度NMJIGFFEBA6.143.804.222.565.8016.18———5.862—3.834.282.59—36.26—4.28—5.934—3.854.162.57—56.283.714.272.515.876—3.794.252.57—76.13———5.8086.15———5.859——4.202.53—10—3.794.232.58—测量平均值6.203.794.242.565.86最大差值百分比(%)2.282.371.421.952.24
表2 提取特征线段角度与测量值对比
Tab.2 The angle of the feature line is compared with the measured value (°)
位置序号线段夹角ANNMJIGFFE90303090360188.6430.44—90.23359.942—29.3130.1089.38359.39388.4730.1529.9389.52359.58487.5129.6830.0289.44359.70587.7529.8329.9589.83359.216—29.1529.9789.34359.15788.8430.56——360.00890.5630.45——358.549—29.7230.3989.62359.5510—29.3030.0489.37359.28测量平均值88.8329.8730.0589.64359.49最大差值百分比(%)2.772.831.300.730.41
由表1和表2中数据可以发现，相对差值均在3%以内。在长度方面，除第5次测量的差值稍大以外，其余9次测量的差值均小于2%。在角度方面，只有线段AN、NM的个别次测量相对差值超过了2%，而GF和FE两条线段的差值均在1%以内。
4 结论
在提取特征点时，本文算法不需要迭代，减小了计算工作量，而且对阈值不敏感，也减小了计算的难度。实验结果表明，本文算法在扫描点分割方面可以获得与手工分割相同的结果；与测量值对比，提取的特征线段也可获得较高的准确度。
[1] KLIMENTJEW D, ARLI M, ZHANG J.3D Scene Reconstruction Based on a Moving 2D Laser Range Finder for Service-robots[C]∥IEEE International Conference on Robotics & Biomimetics.Guilin, 2009：1-5.
[2] OGAZ M, SANDOVAL R, CHACON M.Data Processing from a Laser Range Finder Sensor for the Construction of Geometric Maps of an Indoor Environment[C]∥IEEE International Midwest Symposium on Circuits & Systems.Cancun, 2009：1-5.
[3] NGUYEN V, G
CHTER S, MARTINELLI A, et al.A Comparison of Line Extraction AlgorithmsUsing 2D Range Data for Indoor Mobile Robotics[J].Autonomous Robots, 2007, 23(2): 97-111.
[4] CHOI Y H, LEE T K, OH S Y.A Line Feature Based SLAM with Low Grade Range Sensors Using Geometric Constraints and Active Exploration for Mobile Robot[J].Autonomous Robots, 2008, 24(1): 13-27.
[5] FERNANDES L A F, OLIVEIRA M M.Real-time Line Detection through an Improved Hough Transform Voting Scheme[J].Pattern Recognition, 2008, 41: 299-314.
[6] 满增光, 叶文华, 楼佩煌, 等.基于分开-合并的激光雷达距离图像特征提取 [J].中国机械工程, 2011, 22(19): 2303-2306.
MAN Zengguang,YE Wenhua,LOU Peihuang.Feature Extraction Based on Split-merge in Range Image of LIDAR [J].China Mechanical Engineering, 2011, 22(19): 2303-2306.
[7] 曲丽萍, 王宏健, 边信黔.基于距离聚类的圆柱类实体路标特征提取算法[J].探测与控制学报, 2013, 35(1): 43-49.
QU Liping,WANG Hongjian,BIAN Xinqian.Feature Extraction of Circular Landmark Based Numerical Inference and Geometric Analysis[J].Journal of Detection & Control,2013, 35(1): 43-49.
[8] 薛金林, 张顺顺.基于激光雷达的农业机器人导航控制研究[J].农业机械学报, 2014, 45(9): 55-60.
XUE Jinlin,ZHANG Shunshun.Navigation of an Agricultural Robot Based on Laser Radar[J].Transaction of the Chinese Society for Agricultural Machinery, 2014, 45(9): 55-60.
[9] CADENA C, CARLONE L, CARRILLO H, et al.Past, Present, and Future of Simultaneous Localization and Mapping: toward the Robust-perception Age[J].IEEE Transactions on Robotics, 2016, 32(6): 1309-1332.
[10] 谢自强, 葛为民, 王肖锋, 等.发展型机器人实时特征提取方法研究[J].机器人, 2017, 39(2): 189-196.
XIE Ziqiang,GE Weimin,WANG Xiaofeng,et al.Real Time Feature Extraction Method of Developmental Robot[J].Robot, 2017, 39(2): 189-196.
