264
机械设计与制造
Machinery Design&Manufacture
第5期
2021年5月
激光SLAM自主导航的线段特征提取混合算法研究幽默小故事
桑迪，周军，皇攀凌，李蕾
(1.山东大学机械工程学院，山东济南250061；2.山东大学高效洁净机械制造教育部重点实验室，山东济南250061；
3•齐鲁工业大学(山东省科学院)，机械与汽车工程学院，山东济南250353)
摘要：移动机器人在未知环境中自主导航时定位需要精确的地图，同时为了构建精确的地图必须确定机器人的位姿，同时定位与地图构建(SLAM)问题便产生。基于激光雷达SLAM自主导航算法是研究的
方向之一，激光雷达具有测距精度高、测距远等优点，但是由于激光雷达的数据量少、传感器本身噪声的影响，容易导致机器人构建地图精度低、定位误差大等问题。针对激光雷达的数据测得的距离信息进行线段特征提取算法，通过将常用PDBS、IEPF算法优缺点进行综合，提出了一种混合特征提取算法。最后通过对SICK的LMS111-10100激光雷达获取环境中的一组数据进行处理，分别对PDBS、IEPF算法及混合算法提取效果与实际环境对比，验证了特征提取混合算法的有效性。
关键词：移动机器人;SLAM；激光雷达；PDBS、IPEF算法；线段特征提取
中图分类号:TH16；TP242.2文献标识码:A文章编号：1001-3997(2021)05-0264-05
Line Segment Feature Extraction Algorithm for Lar SLAM Autonomous Navigation
SANG Di,ZHOU Jun,HUANG Pan-ling,LI Lei
(1.School of Mechanical Engineering,Shandong University,Shandong Ji'nan250061,China;2.Key Laboratory of High Efficiency and Clean Mechanical Manufacture,Shandong University,Ministry of Education,Shandong Ji'nan250061,China;
3.School of Mechanical and Automotive Engineering,Qilu University of Technology(Shandong Academy of Sciences),Shandong Ji'nan250353,China)
Abstract：With the development of key technologies such as mapping,positioning and navigation of mobile robots,mobile robots have been”idely udfor autonomous navigation in various complex indoor and workshop environments.In autonomous navigation,robot localization in unfamiliar environments requires accurate maps.In order to build accurate maps,it is necessary to know the position and po of the robot,that is,simultaneous localization and mapping(SLAM).SLAM navigation algorithm bad on radar is one of the rearch directions.Radar has the advantages of high ranging accuracy and long distance.However,due to the less data of radar and the influence of nsor noi,the robot makes mistakes in the process of mapping and positioning.This paper improves the line gment feature extraction algorithm for the range information measured by radar data,and synthesizes the advantages and disadvantages qfthe commonly ud PDBS and IEPF algorithms,and propos a hybrid feature extraction algorithm.A t of data in real environment is acquired and procesd through the data of LMS111-10100nsor of SICK.The effectiveness of PDBS,IEPF and hybrid algorithm is verified by comparing with the actual environment.
Key Words:Mobile Robot；SLAM；Radar；PDBS;IPEF Algorithm；Line Segment Feature Extraction
1引言
自主导航作为一种新的导航技术已经广泛应用于移动机器人行业，移动机器人通过所携带的激光、视觉、里程计等传感器，在移动过程中完成构建地图与定位。SLAM(simultaneouslocaliz-ation and mapping)为自主导航的关键技术，也称为CML(Concurrent Mapping and Localization)，即时定位与地图构建，或并发建图与定位|1-役SLAM描述的是机器人在陌生的环境中通过自身携带的传感器，根据前一时刻的位姿以及里程计数据估计现在时刻的位姿，再通过激光视觉传感器所获得的数据进行修正，不断构建增量式地图，同时对原始的地图进行更新|3。基于激光雷达SLAM 自主导航中，环境对激光雷达的影响较小，具有较高的定位导航精度，缺点是传感器所获得的环境的信息少冋。对原始数据的特
来稿日期：2020-07-07
基金项目：山东省重点研发计划项目(2017CXGC0903)；山东省重点研发计划项目(2017CXGC0215)；
山东省重点研发计划项目(2018CXGC0908)；山东省重点研发计划(2017CXGC0810)；山东省农机装备研发创新计^((2017YF047)作者简介：桑迪,(1989-)，男，山东聊城人，硕士研究生，主要研究方向：基于激光SLAM的自主导航AGV；
周军,(1975-)，男，四川广安人，博士研究生，博士生导师，教授，主要研究方向：机械制造及自动化
、智能控制系统等
第5期 桑 迪等：激光SLAM 自主导航的线段特征提取混合算法研究 265
征提取处理对建图的精度以及机器人在环境中的定位有着重要 的影响I 。
国内外学者针对线段特征提取的方法进行了深入的研究:
文献W 针对室内环境相对简单的结构特征，基于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 :
s =c x
(t ；t 2)
(1)
式中：s —激光雷达到物之间距离；一光速;t 1—脉冲发送时间;
t 2—脉冲接收时间
2.2传感器数据的模型
选用单线激光雷达LMS111-10100，激光雷达的具体参数,
如表 1 所示。
表 1 LMS111-10100 参数
Tab.1 LMS111-10100 Parameters 参数
值范围20m
扫描角度
Max:270⑷可调)角度分辨率
0.25°/0.5°(可调)
扫描频率25/50Hz
激光光斑尺寸
8mm
激光雷达的输岀是一系列的测得的距离的信息，每两个相
邻的光束之间的角度是角度分辨率，如图1所示。激光雷达的数
据是按照逆时针排序从起始角度到终止角度，其中琢表示两个相 邻激光束之间的角度，即激光雷达的角度分辨率，用琢，表示测量 开始的角度。在此研究中的角度选用激光雷达的分辨率选用
0.5。，测量范围为270。，测量起始角度为-45。，终止角度为225。， 共采集到541组数据。
起始角度
图1传感器测距模型Fig.1 Sensor Ranging Model
为了便于数据处理，每一个激光束的极坐标系转换为直角
坐标系，所测得的距离相对于传感器的直角坐标系分别用表示
x ,i 求解方法为：
兹=(O.5i-45)x 备
道德观
(3)
180
fx. =d . cos0I
■
■
⑷
[yi =d - sinQ
式中:i —第i 束激光;；—第i 束激光的角度;d —第i 激光测得的
距离。
两个相邻测量点之间的距离用d i 表示:
d
=
姨(x »1-
x i )2+(y»1
—y)式中：下标，一第.束激光；下标,+1—第.+1束激光。
所有的测量点的x .和y .坐标集合用X 和Y 表示:
|x=[x 0，1 , •••，540 ]嗓 y =[y 0 y ，— ,y 540
]
此研究中直线的拟合方法采用最小二乘法拟合。
设直线的方程为:
y .二 k .x +b j
式中:k —第j 条拟合直线斜率；-第 j 拟合直线截距。
采用最小二乘法拟合得岀：
k = n 移x .y , — 移x . 移y ,‘ n 移x . -( 移x .)
b = 移x .移y . 一移x .移x .y .
n 移x . -( 移x .)
方程可表示为:
n 移x .y . — 移x .移y . 移x . 移y . — 移x . 移x .y y . =k.x+b . =—— .J ~— .一2. x --
点到直线之间的距离d i 表示为:
恥厂1+" ( 5)
( 6)
( 7)
( 8)
( 9)
( 10)
n 移x . -( 移x .)(11)
d i
=
3策+1 特征提取算法对比与改进
对传感器建立模型之后对数据进行处理后，为了将扫描的
No.5May.2021
266机械设计与制造
线段形状特征提取岀来，首先将之前的数据进行区域分割，区域
分割的目的是找岀角点和断点，将测得的点分割为不同的区域，
再拟合岀特征线段，本节主要是通过分析PDBS 、IEPF 这两种常 用的区域分割方法优缺点，提岀一种混合的算法。
3.1 PDBS 算法
PDBS (point distance bad methods )线段分割的方法，是根据
激光雷达两个相邻点之间在直角坐标下的距离d .与设定的阈值着 相比，如果两个相邻测量点之间的距离d .大于阈值着，则认为两个
点属于不同的区域。将激光雷达获得的一组数据从第一个点依次 计算两个相邻点之间的距离，通过与设定的阈值着相比较，将所
测得的点分割为不同的区域后进行最小二乘法拟合提取线段。
PDBS 方法中阈值着的设定对线段的分割具有重要的影响，
阈值设定太小的时容易岀现过分割的现象，阈值设定过大的时候
又容易岀现分割不足的现象，从激光雷达中获得的连续的几个测
量值，如图2(a)所示。设定的不同阈值分割相同的区域时的影 响，如图2(b)、图2(c )所示。P 与P j 之间距离的值大于阈值着，区
域被分为两个区域，在拟合时拟合岀两个线段，如图2(b)所示；
由于距离的值小于阈值，区域未被分割，在拟合时，线段拟合为一 条线段，如图2(c)所示。这对于在构建地图时线段特征的数量有
图2 PDBS 过分割与欠分割问题
较大的影响。
P i P
• •• • •• •
d .
S . S j
d
&(a)
(b)
(c)
Fig.2 PDBS Over Segmentation and Under Segmentation
周浦花海门票
PDBS 算法在角点处的会岀现分割失败的情况。从现实环境 墙角处所得岀的数据，如图3(a)所示；提取岀的正确的线段，如
图3(b)所示；但是在利用PDBS 方法对这一区域进行阈值分割 时，由于并没有提取岀角点，会将此处的所有点划分在一个区域
内，在对此区域进行最小二乘法进行数据拟合时，拟合岀现错误,
如图3(c)所示。
L
(a )
(b ) (c )
图3 PDBS 在角点处拟合失败Fig.3 PDBS Fitting Failure at Corner
3.2 IEPF 方法
IEPF ( iterative end pointfit)的原理步骤如下：
(1) 从传感器提取岀的数据，原始数据转化为直角坐标系，
用黑色点从P 到P ”的坐标点，如图4(a)所示。
(2) 将数据中的第一个点P 1和最后一个点P ”连接形成一条 直线L 1，然后计算从巴点到倒数P ”-1点之间所有点到L 1的距离， 并找岀距离最远的P 点，与设定的阈值相比如果超过设定的阈
值着相比，如果最远距离小于设定的阈值则认为是一条直线，不
再划分；如果最远距离大于着再将所有的点分为两部分从P 1到
P 点及P +1点到P ”点两部分，再按上面的步骤对以步骤操作，直
到不能再划分为止，如图4(b)、图4(c )所示。
Fig.4 IPEF Algorithm Segmentation Process
IEPF 算法能够在线段的交点处将线段的角点提取岀来，避
免PDBS 在两个线段的交点处的特征提取错误的问题。但是
IEPF 方法无法判断一个点是否是断点，如图5所示。P .与P j 是断
点，如图5(a)所示；正确分割的情况，如图5(b)所示。将区域分为
两个部分，在拟合时拟合岀两条线段；但是实际上IEPF 无法根据
两个相邻点之间的距离判断P i 与P 是否是断点，在拟合时仍被 认为是同一直线上的点，如图5(c)所示。拟合岀的是一条直线。
P,
t
L —
P j
(a )
(b )
(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
测量距离
系统误差统计误差(1〜10)m ±25mm ±6mm (10~20)m ±35mm ±8mm (20~30)m
±50mm
±14mm
表3参数定义
Tab.3 Definition of Parameters
参数
下标表示说明
x_axis i_xy_axis
x 坐标数组及下标
y_axis i_ xy_axis y 坐标数组及下标div_sub i_div_sub
分割点数组及下标con i 角点数组及下标
thd _pdbs -PDBS 阈值thd _iepf
-IEPF 阈值dis_pdbs
-两个点之间的距离dis_max_iepf
-点到直线之间最大距离
k -拟合直线的斜率b
-
拟合直线的截距
No.5
May.2021
机械设计与制造267
( 12)
( 13)
( 14)
具体步骤如下：
(1)将原始数据的极坐标转变为直角坐标系下，极坐标中的 第i 个测量点的极坐标的值为：
Q=(0.5i -45)X
f x _axis (i _x _axis ) =d ; cosQ
1 y _axis (i _y _axis ) =d ; sinQ
所有点的集合的表示为：
■ x _axis -[x _axis ] ,_axis 2，…，,_曲$54]]
1 y _axis - [ y _axis 〔 , y _axis 2，…，y _axis 54]]
(2 )计算两个相邻测量点之间的距离：
dis _pdbs =姨/(x i +1 -x ; ) +(y -+1 -y -)
(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 所示。血压高如何降
图6环境数据的提取
Fig.6 Extraction of Environmental Data
激光雷达的一次扫描541个点，通过串口助手将数据读岀,
并将其存为文本文件，以便于后期的分析，将原始的数据转换为
直角坐标系并在MATLAB 中的图像，如图7所示。
图7原始数据图像
Fig.7 Original Data Image
对原始的数据利用PDBS 方法处理，在使用PDBS 方法拟合
的时候在两个线段的交点处，算法中无法将两个线段分开拟合,
造成拟合岀的直线与实际的环境相差很大，如图8所示。
图8 PDBS 方法特征提取
Fig.8 PDBS Algorithm Feature Extraction
对原始图像的数据处理利用IEPF 方法提取，通过对两个线
段之间的交点进行处理，线段的拟合效果提高，但是IEPF 在拟合
中，存在噪音或者岀现断点时，会岀现拟合的直线与实际环境相
豹成语差较大，如图9所示。
图9 IEPF 方法特征提取
Fig.9 IEPF Algorithm Feature Extraction
对原始的数据进行数据处理利用PDBS 与IEPF 方法进行
处理，在数据的处理过程中，首先通过PDBS 提取岀断点，然后再
通过IEPF 提取岀角点，拟合岀的线段较单独使用PDBS 、IEPF 线 段拟效果明显，验证了算法的有效性，如图10所示
。
No.5
May.2021 268机械设计与制造
图10PDBS与IEPF结合特征提取
Fig.10PDBS Algorithm Combined with IEPF
Algorithm for Feature Extraction
装修电工5结论
针对基于激光SLAM自主导航建图时特征线段提取方法进行研究，通过对PDBS和IEPF线段特征提取的方法进行比较，对PDBS对两个线段的交点处的拟合失效的问题进行分析，同是结合IEPF的方法在断点处的拟合失败问题，将两个算法进行互补，首先利用PDBS方法找岀断点，然后再利用IEPF方法找岀角点，最后将数据拟合，对比单独使用PDBS和IEPF的方法，混合特征线段的提取与实际环境更接进，验证了混合算法的有效性。
参考文献
[1]刘欢，王健，李金凤.未知环境下机器人避障设计研究J].机械设计与
制造,2013(10)：236-238.
(Liu Huan,Wang Jian,Li Jin—feng.Design of robot obstacle avoidance in unknown environment J[.Mechanical Design and M a nufac t uring,2013
(10)：236-23&)
[2]谢丰隆，韩建海，李向攀.一种快速的机器人固定视觉标定方法J].机
械设计与制造,2018(11)：237-240.
(Xie Feng—long,Han Jian—hai,Li Xiang—pan.A fast fixed vision calibration method for robots[J[.Mechanical Design and Manufacturing,2018
(11)：237-240.)
[3]Connette C PMeister O,Martin Hagele.Decomposition of line gments
into corner and statistical grown line features in an EKF—SLAM framework[C].//IEEE/RSJ International Conference on Intelligent Robots&Sy—stems.IEEE，2007.
[4]楼晓春.基于扩展Kalman滤波器算法的移动机器人同步定位与建图
[j].中国机械工程,2011,22(24)：2927-2930.
(Lou Xiao—chun.Simultaneous localization and mapping algorithm of a mobile robot bad on extended kalman filter J[.China Mechanical Engineering,2011,22(24)：2927-2930.)
[5]赵季.基于激光测距仪的同步定位与建图研究[D].武汉：武汉科技大学,
2008.
(Zhao Ji.Lar—bad simultaneous localization and mapping]D].Wuhan：Wuhan University of Science and Technology,2008.)
[6]Rouveure R,Checchin P,Faure P.Simultaneous localization and map bui
lding using radar nsor in extensive outdoor environment:first results [j].Lancet,2007,349(9054)：811-811.
[7]Leonard J J Newman P M,Rikoski R J.Towards Robust Data Association
and Feature Modeling for Concurrent Mapping and Localization[M].// Robotics Rearch.Springer Berlin Heidelberg,2003：9—12.
[8]Sweetland C.Method,Design and Implementation of a Multiur Indoor
Localization System with Concurrent Fault Detection]C].//Mobiquitous，14,International Conference on Mobile and Ubiquitous Systems:Computing,Networking and Services,2014：66—82.
[9]Tardos J D.Robust mapping and localization in indoor environments us
ing sonar data J[international Journal ofRobotics Rearch,2002,21(4)：311-330.
[10]Guivant J,Nebot E,Durrant—Whyte H.Simultaneous Localization and
Map Building Using Natural Features in Outdorr Environments[M].// European Journal of Inorganic Chemistry,2000:2992-3000.
[11]魏杨.基于激光雷达的即时定位与制图技术[D].哈尔滨：哈尔滨工业
大学,2017.
(Wei Yang.Simultaneous localization and mapping technology bad on lar lida[D[.Harbin:Harbin Institute of Technology,2017.)
[12]廖自威.激光雷达/微惯性室内自主建图与导航技术研究[D].南京：
南京航空航天大学,016.
(Liao Z i—wei.R e s earch on autonomous mapping and navigation technology in indoor environment bad on lidar and MEMS inertial components D[.Nanjing:Nanjing University of Aeronautics and Astronautics, 2016.)
[13]满增光.基于激光雷达的室内AGV地图创建与定位方法研究[D].南
京：南京航空航天大学,2014.
(Man Zeng—guangRearch on mapping and localization of indoor AGV bad on LADAR[D[.Nanjing：Nanjing University of Aeronautics and Astronautics，2014.)
[14]Lorenzo J M P Vazquez—Martin R,Nunez P.A hough-bad method for
concurrent mapping and localization in indoor environments C].//Robotics,Automation and Mechatronics,2004IEEE Conference on.IEEE, 2004(2)：840-845.
（上接第263页）
(Zhong Gong—wang.German packaging machinery automation,high efficiency,one machine multipurpo,complete rvice[j].Chine Journal of Packaging Industry,2006(9):21-22.)
[4]李军，李小兵，赵桢.硬币分类、整理、包装机械系统研究与设计[J].机
械设计与制造,2018(2).
(Li Jun,Li Xiao—bing,Zhao Zhen.Rearch and design on coin sorting, sorting,packaging systems J[.Machinery Design&Manufacture,2018
(2).)
[5]俞斌.新型细支礼品盒卷烟条透包装机的研制j.内燃机与配件,018
(10).
(Yu Bin.Development of a new type of fine gift box tobacco filling machine J[.Internal Combustion Engine&Parts,2018(10).)
[6]高德.包装机械设计[M].北京：化学工业出版社,2005(8).
(Gao De.Packaging Machinery Design^M[.Beijing Chemical Industry Press,2005(8).)[7]雷文斌.我国包装机械行业发展现状研究J].中国包装工业,2015(8).
(Lei Wen—bin.Rearch on the current situation of packaging machinery industry in China J].China Packaging Industry,2015(8).)
[8]张建中.蒙特卡洛方法(I)J].数学的实践与认识,1974(1)：30-42.
(Zhang Jian—zhong.Monte carlo method(I)J[.Mathematics in Practice and Theory,1974(1):30-42.)
[9]纪玉杰，杨强，孙志礼.应用C语言编写ADAMS用户自定义函数的研
究[J].机械设计与制造,2006(1):101-103.
(ji Yu—jie,Yang Qiang,Sun Zhi—li.The rearch on writing ur-written subroutine of ADAMS in C programming language[j].Machinery Design &Manufacture,2006(1)：101-103.)
[10]孙志礼，陈良玉.实用机械可靠性设计理论与方法[M].北京：科学出
版社,2003(7).
(Sun Zhi—li,Chen Liang—yu.Theory and Method of Practical Mechanical Reliability Design^M].Beijing,Science Press,2003(7).
)
