低成本二维激光传感器室内移动建图方法研究

刘重男,杨 洋,卢清心,张 艺,汪 沛

(北京林业大学 理学院,北京 100083)

0 引 言

近年来,随着计算机技术和自动化技术的发展,机器人越来越多地被应用于工业生产[1]、物流[2]、家政服务[3]等领域。工作中的机器人需要自我定位,感知周围环境,并构建环境地图,以辅助导航和规划路径。因此机器人在未知环境中的自动控制和定位成为当前研究的热点问题。SLAM(Simultaneous Localization and Mapping)技术通过搭载多种传感器,可以在对机器人定位的同时完成环境地图的构建,从而为后续的机器人导航和路径规划提供重要的基础[4]。

SLAM技术可以基于不同种类的传感器实现,比如:视觉传感器、毫米波传感器和激光传感器。基于视觉的SLAM技术,由于需要从二维图像反演三维信息,运算量较大、算法较复杂[5]。基于毫米波雷达的SLAM技术,当前的成本较高,技术还不成熟[6]。基于激光传感器的SLAM技术具有获取三维信息直接、运算量小的特点,在光照影响不大时能够保证较好的精度。目前激光传感器在三维感知和自动驾驶等应用领域已具有较大的优势[7]。对于室内SLAM技术,由于室内环境比较复杂,传感器与室内障碍物距离一般较近,对于精度和实时响应要求高,因此与其他传感器相比,二维激光传感器以其成本低、易集成的优点,在室内SLAM产品中占据更多的市场[8]。

当前基于二维激光雷达的室内SLAM方法主要有两大类。第一类是基于粒子滤波的算法,主要通过概率的方法,将SLAM中定位与建图两个过程分开,先定位再建图[9]。2007年Griseti等在Rao-Blackwellized Particle Filters(RBPF)粒子滤波器的基础上提出了GMapping算法,通过改进提议分布(Proposal Distribution)和引入自适应重采样这两个机制提高了执行效率[10]。这类方法的优点是计算量小,在构建小场景地图时精度更高;缺点是算法依赖于里程计,构建大场景时会产生地图错位[11-12]。第二类是基于图优化的算法,通过计算帧间预测值与观测值的误差,利用非线性最小二乘作为误差函数,求解误差函数的最小值,来找到每帧的最优位姿(Pose),使地图预测值与观测值的误差最小[13-14]。2016年谷歌公司提出的Cartograppher算法,通过引入回环检测,实现地图的全局约束和局部约束[15]。这类方法的优点是对激光雷达硬件条件要求低;缺点是计算量大、运算时间长[16]。

目前主流的基于视觉的室内SLAM方法主要有两大类。第一类是基于图像像素点的算法[17];第二类是基于图像特征点的算法[18]。

该文采用低成本二维激光雷达获取数据,并基于图优化算法进行帧间数据配准,再结合Density-Based Spatial Clustering of Applications with Noise(DBSCAN)算法和K-Nearest Neighbors(KNN)算法绘制地图。基于激光雷达的硬件特性,使用DBSCAN算法预处理点云数据,先采用KNN算法与均方根误差对点云聚类,再利用最小线性二乘法拟合点云数据,实现对室内环境的制图。

1 试验设备及数据采集

1.1 硬件设备平台

该文采用“塔克机器人”作为试验平台采集数据,其硬件构成包括:A1M8型激光雷达、树莓派4B、STM32和四个JGA37直流减速电机,实物图如图1所示。用树莓派4B,搭载Raspbian系统,安装Kinetic Kame版本的ROS系统。运动控制器选择STM32 F103作为主控制单元。

图1 试验流程

二维激光雷达选用思岚公司开发的RPLIDAR A1M8传感器,成本低于500元。该传感器采用激光三角测距技术[19]。该传感器可以360度全方位获取二维点云数据,最远测量距离为12米,在扫描频率为5.5 Hz时360度采样可获取1 450个采样点。

1.2 试验数据采集

2020年10月至2021年4月之间,项目组选择北京林业大学校内四个不同室内场景,采用移动平台展开了多次数据采集试验。场景一位于新食堂四层北侧楼道内,楼道内存在直角拐弯的地形特征;场景二位于新食堂四层东侧楼道内,楼道内存在长侧面的边界;场景三位于学研中心A座14层南侧楼道内,楼道内存在多个短侧面与玻璃装饰;场景四位于基础楼一层西侧楼道,楼道内结构整齐,侧面长度均匀。

试验中,项目组通过笔记本电脑遥控小车平台运动,同时控制小车搭载的激光雷达传感器采集点云数据,从而持续获取室内结构信息,并将数据存储在树莓派4B中,原始点云数据的格式为.bag文件。

2 方法设计

该文提出的室内移动建图方法可以分为三个阶段,分别是:帧间匹配及降噪、聚类分割和自动制图,流程如图1所示。

2.1 帧间匹配及降噪

通过激光传感器采集,获得单帧的点云数据,而室内自主建图需要将单帧点云数据进行帧间的融合匹配,即通过一组平移和旋转参数,使相邻的两帧点云数据能够最大程度的重合,从而将连续扫描的多帧激光点云在同一个坐标系中叠加显示[20]。

由于传感器测量误差、数据处理精度、平台运动时的震动等干扰都会增加点云数据的噪声,从而增加测量误差,降低测量精度[21]。因此,需要对点云降噪以降低数据误差,提高处理速度和结果精度。该文采用两次降噪处理,第一次对每帧的细节处进行降噪处理,第二次从全局的角度出发,对帧间匹配后的点云数据进行降噪处理,这样分别从整体和局部两个方向降低了误差。

帧间配准及降噪的流程如下所示:

(1)激光雷达采集单帧数据,并将极坐标系转换为直角坐标系;

(2)进行第一次DBSCAN降噪处理。

定义每帧点云数据,距离激光雷达中心点直线距离5米以内的点为有效数据。激光雷达在扫描时,由于传感器的误差和平台运动带来的颠簸,造成每帧的数据点存在误差。同时,为了降低后续在配准时,由于单帧中的异常点对全局地图所产生的误差,对坐标转换后的每帧数据进行第一次DBSCAN降噪处理。

DBSCAN是一种基于密度的聚类算法,这是一种很典型的密度聚类算法,既可以适用于凸样本集,也可以适用于非凸样本集。DBSCAN算法的显著优点是聚类速度快且能够有效处理噪声点和发现任意形状的空间聚类。该算法利用基于密度的聚类的概念,要求聚类空间中的一定区域内所包含对象(点或其他空间对象)的数目不小于某一给定的阈值。过滤低密度区域,发现稠密度样本点。在该类别任意样本周围不远处一定有同类别的样本存在。

滤波流程如下:对点云集合中的每个点,在其半径邻域内搜索,若该点的邻域内包含的点少于设定的最小样本点数,则标记为异常点并滤除。

(3)采用Cartographer的配准思想,利用非线性优化的方法,实现单帧扫描点云产生的局部地图与全局地图间的转变。

局部地图通过每帧点云数据扫描产生,在直接匹配子局部地图时,误差会慢慢累计。因此采用稀疏姿态调整(Sparse Pose Adjustment)的思想优化局部地图的位姿,这个过程可以视为一个非线性最小二乘问题。其中由全局地图下的局部地图位姿和扫描位姿在相对位姿的约束下与协方差矩阵关联估算特性,如文献[22]中提出的方法,计算并保存其最小参差时的位姿为最优解。

为了更好地完成精准点间匹配,在局部地图中框出一个搜索窗口,通过选择窗口的移动角度步长和移动坐标步长得到一个有限的搜索集合,遍历集合中的每个元素,改变窗口的角度与坐标位置,求得相邻帧间点云的最佳位姿。采用分支定界方法(Branch and Bound Approach),提高窗口搜索的效率,其基本原理见文献[23]。

(4)将每帧点云数据,按照相对于全局地图的位置和角度信息转换后,进行第二次DBSCAN降噪处理。

将激光雷达获得的极坐标数据转换到笛卡尔坐标系下,然后根据帧间匹配得到每帧的位姿信息,由每帧的局部地图构建全局地图。坐标转换公式如(1)所示:

Do=ρ×(cosθsinθ)

(1)

(2)

其中,ρ和θ分别为通过传感器获得的极坐标信息,Do为笛卡尔坐标系下的原始坐标,px、py和pθ分别为帧间匹配获得的位姿信息,Dt为笛卡尔坐标系下的真实坐标。

因为配准中存在着误差,为降低对后续聚类结果的影响,当帧间配准完成后,对全局地图进行第二次DBSCAN降噪处理。对全局中每个点,按照设定的半径邻域与最小样本点数逐点迭代,标记并滤除掉不符合要求的点。两次降噪结束后,将全局地图转换为正射影像图方便对结果进行观察。

2.2 聚类分割

为绘制室内墙面的环境地图,需要先按照线性关系对原始点云数据进行分类。该文采用基于KNN原理的方法对点云数据进行聚类分析,将聚类点云通过线性最小二乘法拟合成线段,并利用RMSE值判断拟合效果。RMSE是用来衡量观测值与真实值之间的误差,一般指拟合后线段数据与原始点云数据之间方差的平方根[24],RMSE越小,代表拟合效果越好,其计算公式如下所示:

(3)

其中,m是观测过程中使用的数据总数,f(xi)是拟合值,yi是原始值。

聚类拟合方法流程如图2所示。

图2 基于KNN的聚类算法流程

聚类具体流程如下所示:

(1)判断点云数据集合中是否为空,若为空则聚类结束,否则随机选择未被分类的点作为聚类中心点Pc,并定义为一个类别。

(2)对Pc点采用KNN处理,KNN(K-Nearest Neighbor)是采用测量不同特征值之间的距离方法进行分类,优点有以下几点:思想简单、理论成熟,既可以用来做分类也可以用来做回归;可用于非线性分类;训练时间复杂度比支持向量机之类的算法低;和朴素贝叶斯之类的算法比,精度高、对异常值不敏感且无数据输入假定。由于KNN方法主要靠周围有限的邻近的样本,而不是靠判别类域的方法来确定所属的类别,因此对于类域交叉或重叠较多的待分类样本集来说,KNN方法较其他方法更为适合。且KNN方法比较适用于样本容量较大的类域的自动分类,而那些样本容量较小的类域采用KNN方法比较容易产生误分类的情况。

该文选择N个近邻。根据不同试验场景中,各线段点云数量和稀疏程度不同,选择了不同的近邻点数量N,范围在50～200之间。

(3)用线性最小二乘拟合方法对近邻点Pn进行拟合。

(4)线段拟合后,计算拟合线段的RMSE值。

(5)比较RMSE值与阈值,若小于阈值则跳转到步骤6继续聚类,否则跳转到步骤1。根据不同试验场景,整体物理结构和算法运行速度选择不同的RMSE阈值,范围在0.17～0.2之间。

(6)保存近邻点Pn与Pc为同一个类别。

(7)计算近邻点Pn中每个点与Pc的直线距离。

(8)选择Pn与Pc距离最大的点,作为新的聚类中心点Pc,跳转至步骤2。

2.3 自动制图

制图阶段的原则是,符合实际物理环境结构。因此,该文的标准是要求相邻线段首尾相连,线段间不允许出现断开、交叉等现象。具体的操作步骤如下所示:

(1)根据聚类分割的结果,以不同类别中点云数据的数量为标准再进行筛选。

该文规定同一类别的点云数量,应至少大于2N个点。即每一个类别至少经过两次完整的KNN搜索,认为在同一类别中若小于设定的点云数量则代表搜索的点云过少,拟合后形成的线段过短,即不能明显地还原环境信息的线段特征。

(2)采用线性最小二乘方法,对筛选后剩下的类别分类别拟合线段并绘制于图中。

该文的数据获取于建筑楼宇中,墙面线性度和平整度均较好。因此,选择线性最小二乘法拟合点云数据。如下式所示,若需要拟合的数据点为(xi,yi),即找到函数f(xi)使得ei的平方和最小:

(4)

f(xi)=kxi+b

(5)

(3)将线段分为两类对应楼道的两侧,并对每侧线段重新排序。

试验场景为室内楼道,其两侧近似平行,另外两侧则呈向外开放式。因此,将线段按照其坐标点信息划分为两类,然后根据实际场景的物理顺序结构排列。

(4)计算每侧相邻线段的交点,并将交点设置为每条线段新的起始或终止坐标点后重新绘制所有线段于图中。

由于点云数据在聚类拟合时存在误差,导致拟合后的线段没有形成首尾相连的二维环境图。且已知在同一平面内,两条不平行的直线之间必存在唯一的交点,因此可以求出每侧相邻线段的交点坐标,实现环境地图的完整性。

3 试验结果

3.1 融合点云降噪结果

试验采集的四个不同场景原始点云数据,经过融合及降噪处理后,点云数据质量得到改善。图3对比了降噪前和降噪后的点云数据,整体降噪效果明显。在图3中,用框线标出了重点对比区域。显然,经过融合降噪处理,数据因为传感器精度、采集条件、平台和环境的稳定性等导致噪声点云被大量滤除。

图3 四个场景原始点云及降噪结果

同时在降噪后的数据中存在数据的损失。部分边缘数据或者状态不好的数据,在融合降噪的过程中被滤除掉,如图3(g)和(h)左侧所示。由于设备会在室内空间沿路径一直采集数据,这部分滤除的边缘数据将在后续的数据采集和处理中被生成,因而并不会影响室内建图的完整性。因此,图中保留点云的整体轮廓还是较好地还原了实际场景的物理环境信息。

3.2 点云聚类分割结果

试验中四个不同场景的点云数据经降噪处理后,分别使用文中算法进行聚类,将不同类别的数据点集拟合为长直线段,并用不同颜色标识出来。聚类后的边框线如图5所示,与图4相比地图轮廓更加集中规整,可以更好地展示环境的边界特征,但是在相邻的不同类别线段接触处,存在着空隙或交叉,与实际环境情况不符,因此需要对结果进行进一步处理。

图4 四个场景聚类结果

图5 四个场景制图结果

3.3 自动制图结果

根据聚类拟合后的线段,分别计算四个场景中两两相邻线段的起始与终止点坐标,接下来根据每个类别线段的斜率与截距制图,处理结果如图5所示。可以直观观察到整齐的地图轮廓,且不同线段的交界处较好地衔接在一起,得到较准确的环境地图。

3.4 数据误差分析

对四个试验场景自动制图后绘制的线段,按照阿拉伯数字以顺时针的顺序标号;对四个试验场景楼道间的位置,按照英文小写字母自左往右的顺序标号。图6展示了场景一的各线段标号。分别记录四种场景在室内自主建图中获得的数据,并与实际数据相比较,计算出楼道长度平均绝对误差为3.69厘米,楼道宽度平均绝对误差为2.39厘米。

图6 场景1线段长度和楼道宽度分析

4 分析与讨论

平台选择搭载低成本二维激光传感器,具有精度较高、速度快、价格低等优点,试验获得的制图结果基本反映了试验区域的室内空间环境。但是在试验中也发现了一些问题和可以改进的空间。

在硬件方面,设备和环境的影响都会增加测量误差,降低制图精度。二维激光雷达在运动过程中由于每帧采集的数据数量定,在面对较复杂的地形时,有限的点云数据会导致制图后的地图精度降低。如表1中场景1的线段9、线段15和线段16,其本身长度小于50厘米,试验平台在经过时无法获得充足的环境纹理信息,导致拟合后的相对误差值偏大。这个问题也可以在后期通过增加平台在该区域的驻留时间,从而提高激光传感器的扫描密度,改善测量精度。同时,如表2所示,该方法对楼道宽度的平均误差较小,均在1%左右。

表1 场景1和场景3线段实测值与拟合值及误差

在软件方面,算法策略会影响系统的制图精度和实时性。试验选择室内长直楼道区域,环境中多为墙面的线性结构,因此算法中采用线段拟合。当算法在遇到有弧度的拐角或凸起凹陷等不规则曲线边缘时,拟合的精度会有较大的下降,甚至在局部区域出现拟合错误。

现在,激光雷达与视觉传感器相结合的多传感器融合的SLAM方案是未来的发展趋势,能够兼顾弱纹理环境中的特征获取和结构信息测量,保障数据采集的精度。而在软件算法上,室内制图也将与深度学习融合成新的SLAM技术,从而改善前端帧间匹配和后端闭环检测[25]。在未来工作中,也将围绕这些方面开展相应的研究工作。

5 结束语

设计了搭载低成本二维激光雷达的自主移动室内建图平台,并通过扫描试验场景获取点云数据完成建图。试验中提出了通过预滤波、多帧数据融合、聚类分割和自动制图等一系列处理快速实现室内建图。对比四个试验场景结构信息,制图结果能够较好地反映场景实际结构,说明该方法具有一定的有效性和可行性。在未来的研究工作中,将进一步提升算法的鲁棒性和精度,提升算法对室内较复杂场景的建图能力。