基于改进直线特征提取算法的室内移动机器人地图构建

,,,

(西南交通大学 信息科学与技术学院,成都 611756)

0 概述

自主移动机器人在未知环境中移动,需要根据位置估计和地图进行自身定位,同时在自身定位的基础上建造增量式地图,实现机器人自主定位和导航。这一过程被称为即时定位与地图构建(Simultaneous Localization And Mapping,SLAM)。在室内通常采用特征地图,因其需要更少的内存,能够更高效全面地提供关于环境的信息。被提取的特征通常是几何特征,如角和线段,在室内这些特征是很丰富的,可以对障碍物进行表示,并用于建图和定位。因此,特征地图被广泛地研究并用在SLAM中。

从激光雷达扫描室内环境所得数据中提取直线特征的算法主要有分裂合并法[1-2]、增量法[3]、直线回归法[4]、霍夫变换(HT)[5-8]、随机抽样一致性算法(RANSAC)[9-10]、期望最大化算法(EM)等[11-12]。激光雷达对环境的扫描数据具有极角的排列顺序,而RANSAC、HT和EM等算法由于自身的算法特性,不会利用扫描数据点的排列顺序,试图虚假地跨顺序拟合直线,造成拟合所得直线的假阳性及算法的时间复杂度增高。分裂合并法、直线回归法和增量法则采用线性回归方法从扫描数据中拟合直线,但是当扫描数据被大量噪声干扰时,噪声点参与直线拟合,造成拟合所得直线的精确性下降。因此,从受到噪声干扰的大规模离散数据点中剔除噪声点,精准地提取直线特征,构建精确地图,实现移动机器人在不同时刻不同位置对同一目标的观测值相对一致显得十分重要。文献[13]讨论了在SLAM过程中建图噪声去除问题的解决方案。文献[14]采用无向图,使用能量函数最小化的方法从2D激光扫描数据中提取直线特征,对噪声的抑制具有较好的效果。文献[15]使用在运动连续性和空间连续性上的物理约束来确定噪声点,但在构建确定性地图时,计算复杂度较高。文献[16]运用图像处理方法,把原始激光扫描数据转换为二值图像,每一个扫描点对应一个像素点,对图像进行腐蚀运算,该算法在移除噪声点的过程中会把真实点移除,虽采用膨胀算法进行补救,但仍会丢失一些真实点,同时存在转换误差,且实时性差。文献[17]提出一种自适应的图像线段检测器,该方法不需要参数调整,可以获得较精确的提取结果,但是其提取直线特征速度较慢,达不到机器人实时精准建图的要求。

文献[18]提出相似三角形法则(STCSM)对每条真实直线上的噪声点服从的随机特性进行量化。然后利用统计方法寻找出真实直线的最优方向,以最大程度剔除噪声点。该方法结合分裂算法,能快速检测线段,满足机器人实时建图。但在确定真实直线最优方向的过程中会误剔除数据点,导致数据点信息利用率不高造成直线提取精度不稳定。本文针对上述问题提出改进型相似三角形去噪算法。通过雷达在不同位置对障碍物环境进行扫描,从获得的扫描数据中提取直线特征。

1 直线特征提取算法

本文直线特征提取算法分为下面的5个步骤:数据预处理,数据分裂[2],改进相似三角形法则去噪,去噪数据再次分裂,最小二乘直线拟合。激光扫描数据点在激光雷达自身的扫描坐标系表示为为极坐标形式P(α,r)。数据预处理时计算第i个点Pi(i=2,3,…,m-1,其中m为激光数据点的总数)与前后相邻点Pi-1和Pi+1之间的极径差值分别为k1和k2。若k1和k2符号同号,并且其绝对值均大于阈值dc,则该点作为噪声点。然后对预处理后的数据进行分裂,得到分裂点集,对每2个相邻分裂点之间的所有数据点采用改进相似三角形算法去噪。最后对去噪后的数据点重新进行分裂,并进行最小二乘直线拟合,计算直线参数和拟合误差。

算法直线特征提取算法

输入原始激光扫描数据,参数dc、dth、dr、bin、ε、ds

输出所有线段所在直线的参数列表S

1.数据预处理(dc),输出数据集Data1

2.数据分裂(dth,Data1),输出分裂点集合M1

3.foreach M1中每两相邻分裂点间的数据点 do

4.改进型相似三角形法则去噪(dr,bin,ε)

5.end

6.输出数据集Data2

7.数据分裂(dth,Data2),输出分裂点集合M2

8.foreach M2中每两相邻分裂点间的数据点do

9.最小二乘直线拟合(ds)

10.end

11.输出直线参数列表S

1.1 相似三角形法则去噪

由于在相邻2个分裂点之间的大部分离散点在误差允许范围内属于目标直线,同时离散点相对于真实目标直线左右波动的距离关系可以认为服从零均值的正态分布,因此假设目标直线已知,构建一条与目标直线相交于点O的参考直线lr,同时2条直线之间的夹角为γ。令某2个相邻分裂点间的m个离散点构成集合{Pi|i=1, 2,…,m},则离散点Pi与Oi、O构成RtΔPiOiO,其中Oi是Pi到直线lr的垂线的垂足。此时所有离散点到目标直线距离关系可被间接量化为某一分布X～M(μ,σ2),且该分布的众数为γ,其中X={Xi|Xi=∠PiOOi}。理论上所有属于目标直线的离散点Pi,其对应的Xi会集中分布于X的众数γ。相似三角形去噪算法的执行步骤如下[18]:

步骤1在点集U={Pi|i=1,2,…,m;i≠m}中,以离散点Pi作为基准参考点,以点Pi和点Pm两点拟合直线li′,并以离散点Pi为旋转中心,把直线li′顺时针旋转角度θ(即γ=θ,5°<θ<10°)得到参考直线li,如图1所示。

图1 相似三角形法则去噪示意图

步骤2对集合W={Pj|j=1,2,…,m-1;j≠i}中的点进行遍历。计算每一个点Pj与参考点Pi的距离dj1以及点Pj与参考直线li的距离dj2,求比值cij=dj2/dj1。当遍历完W中所有点后,得到数据集Q={cij|j=1,2,…,m-1;j≠i}。

步骤3求数据集Q的众数。在数据集Q的最大值和最小值之间进行S等分,得到S个区间间隔。统计在S个区间中的最大频数ni-max以及该频数对应的区间平均值ci-max。如果ni-max>nmax,那么令nmax=ni-max、cmax=ci-max、num=i,进入步骤4;否则,直接进入步骤4。其中,众数nmax表示对集合U中每一个点Pi执行算法步骤1～步骤4后经比较保存的最大区间频数,cmax表示nmax对应区间的平均正弦比例值,num表示nmax对应的基准参考点Pi在原始数据点中的排列序号。

步骤4当点集U中所有点都作为参考点,并执行算法步骤1～步骤3后,进入步骤5;否则把下一个离散点Pi+1代替Pi作为基准参考点并跳转至步骤1。

步骤6计算Pk(k=1,2,…,m)到直线l′的距离θi,若dk大于阈值dr,则把点Pk记为噪声点。算法结束。

相似三角形去噪算法的时间复杂度为O(m2),算法可以归纳为寻求函数J最优的过程:

(1)

其中:

(2)

其中,δ为ci-max对应区间的长度。

相邻分裂点间的所有离散点到目标直线的距离关系可被间接地量化为某一分布X～M(μ,σ2),当进行正弦函数映射后,Y=sin(X),Y与X的分布不再一致,在步骤3中求出的ci-max所对应的角度arcsin(ci-max)与X的众数值不严格相等。当X较小时,Y=sin(X)≈X,此时Y的分布可以近似为X的分布,角度arcsin(ci-max)则可作为X的众数值。由图1可知,集合{Pj|j=1,2, …,m-1;j≠i}中的任意点Pj,其对应的arcsin(d2/d1)在θ附近波动,即arcsin(d2/d1)≈θ,而arcsin(d2/d1)正是随机变量X的一个样本点。所以,取5°<θ<10°,就能使Y的分布与X的分布近似一致。

参数S的取值将影响γ的精确性,S由式(3)给出:

S=[max(cij)-min(cij)]/bin

(3)

其中,cij∈Q,bin是各区间间隔值,其实际取值视需要的精度而定,通常较小。

1.2 改进相似三角形去噪法则

当集合U={Pi|i=1,2,…,m;i≠m}中的点Pi作为参考点时构建参考直线,并且相似三角形去噪算法步骤3被执行完成后,可以得到数据Q={cij|j=1,2,…,m-1;j≠i}。假设cij的分布直方图对应的轮廓线如图2所示。

图2 cij分布直方图轮廓线

在图2中,ci-max表示当以Pi作为参考点时,数据集Q的众数,ni-max表示ci-max对应的频数,cih对应图3中的Ph点。

图3 参考点Pi邻域内的离散点分布

假设图3中直线li′是目标直线,左右两边的虚线与li′的距离均为dr。由相似三角形去噪算法步骤6可知,离散点到目标直线的距离是否大于阈值dr决定该点是否是噪声点。因此,图3中2条虚线间区域内的点都不该视为噪声点,在此区域内离散点的位置信息需被利用起来确定最终的真实目标直线。图3中点Ph与参考点Pi的距离dh1很小,Ph对应正弦值cih≫sinθ。据相似三角形去噪法则原理,cih不会被包含在用于确定目标直线的离散数据点集合;但Ph处于阈值区域内,应为确定目标直线做出贡献。由上述分析可知,在参考点Pi的某一空间邻域内会出现较多属于阈值区域内的Ph点,忽略这些点的信息将导致求得的目标直线参数不准确。本文针对这一问题提出改进算法,在相似三角形去噪法则的步骤3之后对ni-max进行更新。

算法ni-max更新

输入基准参考点Pi,及对应的分裂区间点集U、参考直线li、数据集Q、ci-max和ni-max

输出ni-max的更新值

1. 求φ=arcsin(ci-max)

2. 根据φ、li求出直线lm

3. foreach Pj∈U do

4. if cij∉Q

5. 计算Pj到直线lm的距离dj-m

6. if dj-m>dr

7. ni-max++

8. end

9. end

10. end

11. 输出ni-max

阈值dr的大小随着原始探测数据的线性一致性增大而减小。当环境中障碍物表面粗糙程度差异较大时,可对U中的点进行二维PCA处理,dr可自适应取值为在次分量方向上数据投影的方差;否则可取dr为定值。

1.3 直线拟合及特征提取

当所有相邻分裂点构成的线段区间上的离散点经过相似三角形法则去噪后,对去噪后的所有点重新执行分裂算法,得到精确的分裂点集。按照分裂点的顺序,对相邻2个分裂点之间所有非噪声点的数目进行统计,若点数小于阈值ds,则舍弃该线段区间,否则利用最小二乘法拟合直线。拟合所得直线l表示为:

d=rcos(α-θ)

(4)

其中,d为原点到直线l的垂线长,θ为垂线与极轴的逆时针方向的夹角,(α,r)为直线l上的点P(α,r)。

θ和d由式(5)、式(6)给出:

(5)

(6)

(7)

F=

(8)

(9)

其中,N为参与拟合直线l的离散点个数,F为Jacobian矩阵,C为参与拟合直线l的N个激光测距点参数的协方差矩阵。

2 实验结果与分析

本文使用RPLIDAR360°激光测距仪对12 m×6 m的实验环境进行扫描。雷达有效测距半径为6 m,设置其扫描间隔为0.25°,得到693个扫描点。分裂算法的分裂阈值dth为70 mm,相似三角形去噪过程中的统计区间间隔设为sin 0.5°,距离阈值dr为定值5 mm,再次分裂并拟合直线步骤中的点数阈值ds为5。测量极角的方差忽略不计,测量距离的方差与测量距离的关系如图4所示。

图4 测量距离方差与测量距离的关系

2.1 算法精度

测试环境如图5(a)所示,实验中激光雷达的测量中心所在位置为原点O,沿其测量极坐标系的极轴方向建立x轴,相应建立y轴。采用改进相似三角形去噪算法,最终拟合直线的结果如图5(b)所示,得到7条直线,与真实障碍物环境中的直线相吻合。由于改进相似三角形去噪法则能最大程度剔除噪声点及利用有效点确定真实直线所在方向,因此没有线段合并的过程。各条拟合所得直线的参数如表1所示。表1中皮尔逊系数表示根据如图5(b)所示的离散点拟合所得各直线的线性程度。点数比例表示去噪后参与拟合各条直线的离散点数在线段区间上的原始数据点数中所占比例。拟合所得各线段的点数比例最低为45.07%,平均比例为52.25%。由相似三角形去噪算法改进原理可知,算法去噪建立在噪声点服从正态分布的随机特性基础上,能避免大量的局部大面积去噪。因此,去噪后的数据点能准确描述真实直线的信息。相关系数最小值为0.999 972,拟合各条直线的离散点线性相关性均趋近1。由此表明本文算法保证在不丢失原始数据绝大部分信息的前提下,利用线性程度较高的数据点拟合得到直线参数。同时,实验所得直线参数精度也较高,直线li参数di的方差的最高数量级为10-1mm2,参数θi的方差的最高数量级为10-5rad2,针对该激光扫描器的特性而言,算法满足对环境的精确建模。

图5 采用改进相似三角形法则提取的环境直线

直线θ/radd/mmσ2d/mm2σdθ/(mm·rad-1)σ2θ/rad2皮尔逊系数点数比例/%L10.276444.96705´1031.20427´10-11.91303´10-44.37536´10-60.99999445.07L20.593222.80229´1039.93612´10-31.58685´10-45.65798´10-60.99999155.38L30.276394.96759´1031.27231´10-12.07484´10-35.67538´10-50.99999251.78L41.930532.88392´1035.24792´10-35.83036´10-51.80895´10-60.99999954.43L50.816253.53996´1031.38402´10-2-1.26114´10-41.80798´10-60.99999854.00L61.928335.00153´1035.40386´10-2-1.48725´10-57.78181´10-80.99997355.06L72.912284.65371´1034.33849´10-23.25620´10-52.02008´10-80.99997250.00

2.2 算法准确性

把激光雷达的测量中心分别平移到如图6(a)所示标号的位置(xj,yj)处对环境进行扫描,j=1,2,…,14。雷达在(x1,y1)处的测量坐标系作为全局不变坐标系。因此,直线li(θi,di),i=1,2…,7,在平移前后的雷达极坐标系中的参数(θi1,di1)、(θij,dij)满足下式:

Δθi=|θij-θi1|

(10)

(11)

其中,Δθi称为平移不变性参数,理论上Δθi=0。由于直线li的准确参数(θi,di)无法预知,因此无法对激光雷达平移前后的直线参数进行式(11)的检验。但由式(5)、式(6)可知,参数di的准确性依赖于θi,直线极角的准确性在一定程度上能保证极径的准确性。因此,对平移前后的直线参数进行式(10)的检验即可评价算法的准确性。

激光雷达在位置(xi,yi)处扫描环境获得一帧数据时,分别采用传统分裂合并法、STCSM算法和本文改进算法进行直线提取。当雷达在所有标号位置处对环境扫描结束,采用每一种算法提取完成直线后,均可得直线极角的集合R={θij|j=1,2,…,14},分别对3种方法所得集合R的元素进行方差分析,结果如图6(b)～图6(h)所示。从图6(b)～图6(h)可以看出,采用本文改进算法在不同测量位置提取的各条直线参数的方差最小,最大数量级为10-6rad2,即在机器人移动过程中保持的直线平移不变性要优于采用传统分裂合并法和STCSM算法。随着雷达平移,障碍物轮廓线L1、L2和L3被激光扫描得到的点逐渐减少,同时激光雷达在位置11～位置14处时,线段L1不在雷达0°～180°的视野内,因此图6(b)～图6(d)中所示直线极角参数的方差相较于其余直线参数偏大。图6(e)～图6(h)表明,在不同位置提取的线段L4～L7角度参数的波动幅度很小,平移不变性参数Δθi≈0。

西格沃特等[2]的研究结果表明:就正确性而言,增量法表现最好,其假阳性约6%;分裂合并法其次,其假阳性约10%;同时分裂合并法的执行速度是所有算法里最快的,但是该算法的精度却不是最高的。RANSAC、HT、EM算法较于分裂合并法其提取直线的速度非常慢,假阳性较高,但是它们能够产生比其他算法更精确的直线,其原因在于它们有能力去除异常点和噪声大的有效点。综上分析,本文提出的基于改进相似三角形去噪算法有能力去除异常点或噪声大的有效点有效提高了提取直线的精度。

图6 雷达在不同位置对环境扫描并采用不同方法提取直线结果及对比

3 结束语

在相似三角形去噪法则剔除噪声点过程中,噪声点相对于真实直线的空间位置所呈现的分布进行量化存在非线性量化误差,进而会剔除部分有效点。本文提出的改进相似三角形去噪算法降低了误剔除有效点的数量。在12 m×6 m的障碍物环境中进行实验,得到在0°～180°的扫描范围内693个点。直线参数极径di的方差数量级在10-1mm2以下,极角θi方差的数量级最高为10-5rad2。激光雷达在不同位置对环境扫描并提取直线的角度参数的方差数量级最大为10-6rad2。实验结果表明,本文提出的改进算法在直线提取的准确性和精度方面都优于传统分裂合并法、增量法、直线回归法、RANSAC、HT和EM算法,对室内机器人精准鲁棒地实时地图构建、路径规划、全局定位具有重要意义。

[1] PAVLIDIS T,HOROWITZ S L.Segmentation of Plane Curves[J].IEEE Transactions on Computers,1974,23(8):860-870.

[2] NGUYEN V,MARTINELLI A,TOMATIS N,et al.A Comparison of Line Extraction Algorithms Using 2D Laser Rangefinder for Indoor Mobile Robotics[C]//Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems.Washington D.C.,USA:IEEE Press,2005:1929-1934.

[3] AHMAD H,NAMERIKAWA T.Robot Localization and Mapping Problem with Unknown Noise Charac-teristics[C]//Proceedings of IEEE International Conference on Control Applications.Washington D.C.,USA:IEEE Press,2010:1275-1280.

[4] ARRAS K O,SIEGWART R Y.Feature Extraction and Scene Interpretation for Map-based Navigation and Map Building[C]//Proceedings of MRXI’97.[S.1.]:SPIE Press,1997:42-53.

[5] WANG C F.Fast Line Extraction Algorithm Based on Improved Hough Transformation[J].Advanced Materials Research,2014(926-930):3612-3615.

[6] XU Zezhong,SHIN B,KLETTE R.Accurate and Robust Line Segment Extraction Using Minimum Entropy with Hough Transform[J].IEEE Transactions on Image Processing,2015,24(3):813-822.

[7] 王竞雪,朱 庆,张云生,等.叠置分区辅助的相位编组直线提取算法[J].测绘学报,2015,44(7):768-774,790.

[8] 张 帆,高云龙,黄先锋,等.基于球面投影的单站地面激光点云直线段提取方法[J].测绘学报,2015,44(6):655-662.

[9] CANAZ S,KARSLI F,GUNEROGLU A,et al.Automatic Boundary Extraction of Inland Water Bodies Using LiDAR Data[J].Ocean and Coastal Management,2014,118(1):158-166.

[10] FISCHLER M A,BOLLES R C.Random Sample Consensus:A Paradigm for Model Fitting with Application to Image Analysis and Automated Cartography[J].Communications of the ACM,1981,24(6):381-395.

[11] NGUYEN V,GACHTER S,MARTINLLI A,et al.A Comparison of Line Extraction Algorithms Using 2D Range Data for Indoor Mobile Robotics[J].Autonomous Robots,2007,23(2):97-111.

[12] PFISTER S,ROUMELIOTIS S L,Burdick J W.Weighted Line Fitting Algorithms for Mobile Robot Map Building and Efficient Data Representation[C]//Proceedings of IEEE International Conference on Robotics and Automation.Washington D.C.,USA:IEEE Press,2003:1304-1311.

[13] RAO A,MULLANE J,WIJERUPAGE W S,et al.SLAM with Adaptive Noise Tuning for the Marine Environment[C]//Proceedings of OCEANS’11.Washington D.C.,USA:IEEE Press,2011:1-6.

[14] LI Xinzhao,LIU Yuehu,NIU Zhenning,et al.A Line Segments Extraction Based Undirected Graph from 2D Laser Scans[C]//Proceedings of the 17th IEEE International Workshop on Multimedia Signal Processing.Washington D.C.,USA:IEEE Press,2015:1-6.

[15] YE Cang,BORENSTEIN J.A Novel Filter for Terrain Mapping with Laser Rangefinders[J].IEEE Transactions on Robotics,2004,20(5):913-921.

[16] RAVANKAR A A,HOSHINO Y,RAVANKAR A,et al.Algorithms and a Frame Work for Indoor Robot Mapping in a Noisy Environment Using Clustering in Spatial and Hough Domains[J].International Journal of Advanced Robotic Systems,2015,12(3):57-62.

[17] GROMPONE V G,RAFAEL,JAKUBOWICZ J,et al.LSD:A Fast Line Segment Detector with a False Detection Control [J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2008,32(4):722-732.

[18] JIAN Ming,ZHANG Cuifang,YAN Fei,Tet al.A Global Line Extraction Algorithm for Indoor Robot Mapping Based on Noise Eliminating via Similar Triangles Rule[C]//Proceedings of the 35th IEEE China Control Conference.Washington D.C.,USA:IEEE Press,2016:6133-6138.