第16卷第4期 智能系统学报 Vol.16 No.4 2021年7月 CAAI Transactions on Intelligent Systems Jul.2021 D0:10.11992/tis.202007040 网络出版地址：https:/ns.cnki.net/kcms/detail/23.1538.tp.20210409.0908.002.html 自主移动机器人路径规划中的点云噪声处理 王新杰，张莹2，张东波2，王玉'，杨知桥 (1.湘潭大学自动化与电子信息学院，湖南湘潭411105,2.湖南大学机器人视觉感知与控制技术国家工程实 验室，湖南长沙410082) 摘要：机器人在自主导航过程中，图像处理算法通过对路径中障碍物的三维点云平面夹角进行分析，实时计 算障碍物的高度，判断是否可以通行。而RGB-D相机输出的原始点云数据存在大量噪声，严重影响分割的准 确性，需要进行滤波处理。本文在Table Scene数据集中分析比较了统计滤波中不同参数的去噪效果，得到了最 佳参数K=20,a=2。移动机器人通过ORB-SLAM2算法在户外环境下构建点云地图，然后分别进行直通滤波、 体素滤波、统计滤波和平面分割，计算斜坡夹角，实施运动规划。实验结果表明，在Table Scene数据集中获得 的最优统计滤波参数能适用于户外环境，机器人能根据运算结果自动进行路径规划，完成指定任务。 关键词：点云地图：直通滤波；体素滤波；统计滤波：点云分割；移动机器人：运动规划：RGBD感知 中图分类号：TP242文献标志码：A 文章编号：1673-4785(2021)04-0699-08 中文引用格式：王新杰，张莹，张东波，等.自主移动机器人路径规划中的点云噪声处理J1.智能系统学报，2021,16(4)： 699-706. 英文引用格式：VANG Xinjie,ZHANG Ying,ZHANG Dongbo,etal.Point cloud noise processing in path planning of autonomous mobile robot[J].CAAI transactions on intelligent systems,2021,16(4):699-706. Point cloud noise processing in path planning of autonomous mobile robot WANG Xinjie',ZHANG Ying'2,ZHANG Dongbo2,WANG Yu',YANG Zhiqiao (1.College of Automation and Electronic Information,Xiangtan University,Xiangtan 411105,China;2.National Engineering Labor- atory of Robot Vision Perception and Control Technology,Changsha 410082,China) Abstract:In the process of robots'autonomous navigation,the image processing algorithm analyzes the 3D point cloud plane angles of obstacles in the path and calculates their height in real time,thus determining whether the robots can pass or not.However,a large amount of noise is present in the original point cloud data output by the RGB-D camera, thereby seriously affecting the accuracy of segmentation;such noise thus needs to be filtered.This paper first obtains the best parameters(K=20 and a=2)by analyzing and comparing the denoising effects of different parameters in statistical filtering in the Table Scene dataset.The mobile robot constructs a point cloud map in an outdoor environment through the ORB-SLAM2 algorithm and then performs passthrough filtering,voxel filtering,statistical filtering,and plane seg- mentation,calculating the slope angle and implementing motion planning.Experimental results show that the optimal statistical filter parameters obtained in the Table Scene dataset can be applied to outdoor environments and that robots can automatically plan their paths according to the calculation results and complete their specified tasks. Keywords:point cloud map;pass through filtering;voxel filtering;statistical filtering;point cloud segmentation;mo- bile robot;motion planning:RGB-D perception 收稿日期：2020-07-24.网络出版日期：2021-04-09 机器人在自主导航过程中，对当前环境的点 基金项目：国家自然科学基金项目(61175075)：国家自然科学 云地图进行运算处理，根据分析结果实施路径规 基金区域创新发展联合基金项目U19A2083):湖南 省战略性新兴产业科技攻关与重大成果转化项目 划。但点云地图会由于传感器误差、光照、阴影 2019GK4007). 通信作者：张莹.E-mail:hangying@xtu.edu.cn 等外在因素，导致存在大量的冗余信息，需要通
DOI: 10.11992/tis.202007040 网络出版地址: https://kns.cnki.net/kcms/detail/23.1538.tp.20210409.0908.002.html 自主移动机器人路径规划中的点云噪声处理 王新杰1 ，张莹1,2，张东波1,2，王玉1 ，杨知桥1 （1. 湘潭大学 自动化与电子信息学院，湖南 湘潭 411105; 2. 湖南大学 机器人视觉感知与控制技术国家工程实 验室，湖南 长沙 410082） 摘 要：机器人在自主导航过程中，图像处理算法通过对路径中障碍物的三维点云平面夹角进行分析，实时计 算障碍物的高度，判断是否可以通行。而 RGB-D 相机输出的原始点云数据存在大量噪声，严重影响分割的准 确性，需要进行滤波处理。本文在 Table Scene 数据集中分析比较了统计滤波中不同参数的去噪效果，得到了最 佳参数 K=20，α=2。移动机器人通过 ORB-SLAM2 算法在户外环境下构建点云地图，然后分别进行直通滤波、 体素滤波、统计滤波和平面分割，计算斜坡夹角，实施运动规划。实验结果表明，在 Table Scene 数据集中获得 的最优统计滤波参数能适用于户外环境，机器人能根据运算结果自动进行路径规划，完成指定任务。 关键词：点云地图；直通滤波；体素滤波；统计滤波；点云分割；移动机器人；运动规划；RGB-D 感知 中图分类号：TP242 文献标志码：A 文章编号：1673−4785(2021)04−0699−08 中文引用格式：王新杰, 张莹, 张东波, 等. 自主移动机器人路径规划中的点云噪声处理 [J]. 智能系统学报, 2021, 16(4): 699–706. 英文引用格式：WANG Xinjie, ZHANG Ying, ZHANG Dongbo, et al. Point cloud noise processing in path planning of autonomous mobile robot[J]. CAAI transactions on intelligent systems, 2021, 16(4): 699–706. Point cloud noise processing in path planning of autonomous mobile robot WANG Xinjie1 ，ZHANG Ying1,2 ，ZHANG Dongbo1,2 ，WANG Yu1 ，YANG Zhiqiao1 (1. College of Automation and Electronic Information, Xiangtan University, Xiangtan 411105, China; 2. National Engineering Laboratory of Robot Vision Perception and Control Technology, Changsha 410082, China) Abstract: In the process of robots’ autonomous navigation, the image processing algorithm analyzes the 3D point cloud plane angles of obstacles in the path and calculates their height in real time, thus determining whether the robots can pass or not. However, a large amount of noise is present in the original point cloud data output by the RGB-D camera, thereby seriously affecting the accuracy of segmentation; such noise thus needs to be filtered. This paper first obtains the best parameters (K=20 and α=2) by analyzing and comparing the denoising effects of different parameters in statistical filtering in the Table Scene dataset. The mobile robot constructs a point cloud map in an outdoor environment through the ORB-SLAM2 algorithm and then performs passthrough filtering, voxel filtering, statistical filtering, and plane segmentation, calculating the slope angle and implementing motion planning. Experimental results show that the optimal statistical filter parameters obtained in the Table Scene dataset can be applied to outdoor environments and that robots can automatically plan their paths according to the calculation results and complete their specified tasks. Keywords: point cloud map; pass through filtering; voxel filtering; statistical filtering; point cloud segmentation; mobile robot; motion planning; RGB-D perception 机器人在自主导航过程中，对当前环境的点 云地图进行运算处理，根据分析结果实施路径规 划。但点云地图会由于传感器误差、光照、阴影 等外在因素，导致存在大量的冗余信息，需要通 收稿日期：2020−07−24. 网络出版日期：2021−04−09. 基金项目：国家自然科学基金项目 (61175075)；国家自然科学 基金区域创新发展联合基金项目 (U19A2083)；湖南 省战略性新兴产业科技攻关与重大成果转化项目 (2019GK4007). 通信作者：张莹. E-mail：zhangying@xtu.edu.cn. 第 16 卷第 4 期 智 能 系 统 学 报 Vol.16 No.4 2021 年 7 月 CAAI Transactions on Intelligent Systems Jul. 2021
·700· 智能系统学报 第16卷 过滤波消除不利影响」 前帧的点云地图。本文采用送代最近点(iterated 点云的噪声分为小振幅噪声和离群点噪声)。 closest points,ICP)3算法将不同坐标系下的点 小振幅噪声体现为物体表面随机分布的点，为了 云对准到同一世界坐标系，实现局部点云地图的 减少小振幅噪声点，Alexa等使用移动最小二 拼接。算法如图1所示。 乘(moving least squares,MLS)算法生成平滑表面； 对原始点云数据 确定初始 去除错误 求解局部点云间 Chen等定义隐式移动最小二乘(implicit moving 进行采样 对应点集 对应点对 的坐标变换 least squares,.IMLS)方法构建插值或逼近隐式曲 图1基于ICP的三维点云拼接 面；Zhang等提出加权最小二乘法和层次聚类 Fig.1 Three-dimensional point cloud registration based on 算法以加快MLS方法；但是这些算法对大数量的 ICP 离群点和平滑表面敏感。而离群点噪声表现为远 ICP首先将待拼接的2片点云或其中1片点 离真实物体表面的点或点簇”。离群点去噪算法 云进行采样，然后根据点到点、点到投影、点到面 分为基于统计的方法和基于几何的方法。基于统 这3种方法确定对应点集，一般采用最小二乘法 计的方法将噪声数据拟合为标准概率分布，根据 迭代求解2片点云间的最优坐标变换，完成点云 噪声点云与正常点云的概率分布不同进行去噪。 的拼接。 基于几何的方法根据点到相邻点的距离或局部密 度中心的距离与阈值的比较，判断是否是噪声点9 2路径规划中的点云数据处理 基于图像的ORB-SLAM2IO在ORB-SLAM山 2.1点云模型中的滤波方法 的基础上增加了双目和RGB-D模式，可以直接获 机器人路径规划一般用直通滤波分割点云的 取图像的深度信息，但算法只能生成稀疏的特征 范围，删除指定场景外的点。深度相机获取的点 点云地图，不能准确表达环境的结构和纹理信息。 云数据量过大，需要经过降采样处理以减少点的 同时基于图像的三维重建技术比基于激光传感器 数量，提高处理效率，路径规划算法常用体素滤 生成的点云数据具有更高幅值和更多的噪声)， 波进行数据压缩，节省存储空间。由于设备精度 需要进一步研究。本文在ORB-SLAM2的基础上， 误差和环境物体间的视野相互遮挡，深度相机输 研究机器人路径规划中的点云噪声数据处理，并 出的原始点云数据存在大量离群噪声点，路径规 将处理后的结果用于机器人越障或避障的决策。 划算法常用统计滤波去除离群点噪声，保留物体 基于ORB-SLAM2的点云拼接 表面上的点。 统计滤波首先通过K个最近邻域搜索功能 ORB-SLAM2利用RGB图像信息和对应的深 计算点到其邻域的平均距离。计算如式(3)所示。 度信息进行点云拼接，获取环境特征的地图形 true,u±a·σ≥l 式。定义序列X={x,,…,x}来表示三维空间点 outlier;= false,其他 (3) 的集合，其中x=xy表示相机在三维空间中 式中：4代表估计的平均距离；σ代表标准差；a为 的坐标。则三维空间中的坐标点映射到二维图像 标准差倍数阈值。如果某点的计算值μ±α·σ不 平面的像素坐标[用相机成像模型来描述： 小于点云中所有点与邻域的距离1，则被认为是 卧 异常点，可将其剔除。故离群点的去除效果由阈 (1) 1 值K和a决定。 式中：K是相机的内参数矩阵；R和t分别为相机 2.2障碍物点云分割及其坡度计算 的旋转矩阵和平移向量；d是像素值的深度。 机器人越障需把斜坡面和地平面分割出来， 相机坐标[xy'可以根据其在三维空间的 计算两个平面之间的夹角，如图2所示。经典的 坐标到二维像素坐标的反推计算： 点云分割算法分为基于聚类的分割算法和基于模 型的分割算法吲，基于模型的分割算法采用随机 (u-c)./f (v-c)/f (2) 采样一致性(random sampling consensus, RANSAC)原理，删除范围以外的点，得到指定范 其中，cx、c,和东、分别为相机内参中的光心和 围内的点所组成的子集，从而拟合线或面。 焦距，是相机的固有参数。 当两个点云平面即斜坡面和地平面被分割以 式(2)将当前帧的RGB图像、深度图像，关键 后，如果以两个方程分别表示斜坡面：Ax+ 帧对应的相机位姿信息变换到世界坐标，创建当 By+C1z+D1=0和地平面：A2x+B2y+C2z+D2=0
过滤波消除不利影响[1-2]。 点云的噪声分为小振幅噪声和离群点噪声[3]。 小振幅噪声体现为物体表面随机分布的点，为了 减少小振幅噪声点，Alexa 等 [4] 使用移动最小二 乘 (moving least squares, MLS) 算法生成平滑表面； Chen 等 [5] 定义隐式移动最小二乘 (implicit moving least squares, IMLS) 方法构建插值或逼近隐式曲 面；Zhang 等 [6] 提出加权最小二乘法和层次聚类 算法以加快 MLS 方法；但是这些算法对大数量的 离群点和平滑表面敏感。而离群点噪声表现为远 离真实物体表面的点或点簇[7]。离群点去噪算法 分为基于统计的方法和基于几何的方法。基于统 计的方法将噪声数据拟合为标准概率分布，根据 噪声点云与正常点云的概率分布不同进行去噪[8]。 基于几何的方法根据点到相邻点的距离或局部密 度中心的距离与阈值的比较，判断是否是噪声点[9]。 基于图像的 ORB-SLAM2[10] 在 ORB-SLAM[11] 的基础上增加了双目和 RGB-D 模式，可以直接获 取图像的深度信息，但算法只能生成稀疏的特征 点云地图，不能准确表达环境的结构和纹理信息。 同时基于图像的三维重建技术比基于激光传感器 生成的点云数据具有更高幅值和更多的噪声[12] ， 需要进一步研究。本文在 ORB-SLAM2 的基础上， 研究机器人路径规划中的点云噪声数据处理，并 将处理后的结果用于机器人越障或避障的决策。 1 基于 ORB-SLAM2 的点云拼接 X = {x1, x2,··· , xn} x1 = [ x y z]T [u v] T ORB-SLAM2 利用 RGB 图像信息和对应的深 度信息进行点云拼接，获取环境特征的地图形 式。定义序列 来表示三维空间点 的集合，其中 表示相机在三维空间中 的坐标。则三维空间中的坐标点映射到二维图像 平面的像素坐标 用相机成像模型来描述： d u v 1 = K · R· x y z + t (1) K R t d 式中： 是相机的内参数矩阵； 和 分别为相机 的旋转矩阵和平移向量； 是像素值的深度。 [ x y z]T 相机坐标 可以根据其在三维空间的 坐标到二维像素坐标的反推计算： x y z = (u−cx)·z/ fx (v−cy)·z/ fy d (2) cx cy fx f 其中， 、 和 、 y 分别为相机内参中的光心和 焦距，是相机的固有参数。 式 (2) 将当前帧的 RGB 图像、深度图像，关键 帧对应的相机位姿信息变换到世界坐标，创建当 前帧的点云地图。本文采用迭代最近点 (iterated closest points, ICP)[13-14]算法将不同坐标系下的点 云对准到同一世界坐标系，实现局部点云地图的 拼接。算法如图 1 所示。 对原始点云数据 进行采样 确定初始 对应点集 去除错误 对应点对 求解局部点云间 的坐标变换 图 1 基于 ICP 的三维点云拼接 Fig. 1 Three-dimensional point cloud registration based on ICP ICP 首先将待拼接的 2 片点云或其中 1 片点 云进行采样，然后根据点到点、点到投影、点到面 这 3 种方法确定对应点集，一般采用最小二乘法 迭代求解 2 片点云间的最优坐标变换，完成点云 的拼接。 2 路径规划中的点云数据处理 2.1 点云模型中的滤波方法 机器人路径规划一般用直通滤波分割点云的 范围，删除指定场景外的点。深度相机获取的点 云数据量过大，需要经过降采样处理以减少点的 数量，提高处理效率，路径规划算法常用体素滤 波进行数据压缩，节省存储空间。由于设备精度 误差和环境物体间的视野相互遮挡，深度相机输 出的原始点云数据存在大量离群噪声点，路径规 划算法常用统计滤波去除离群点噪声，保留物体 表面上的点。 统计滤波首先通过 K 个最近邻域搜索功能 计算点到其邻域的平均距离。计算如式 (3) 所示。 outlieri = { true, µ±α·σ ⩾ l false, 其他 (3) µ σ α µ±α·σ l K α 式中： 代表估计的平均距离； 代表标准差； 为 标准差倍数阈值。如果某点的计算值 不 小于点云中所有点与邻域的距离 ，则被认为是 异常点，可将其剔除。故离群点的去除效果由阈 值 和 决定。 2.2 障碍物点云分割及其坡度计算 机器人越障需把斜坡面和地平面分割出来， 计算两个平面之间的夹角，如图 2 所示。经典的 点云分割算法分为基于聚类的分割算法和基于模 型的分割算法[15] ，基于模型的分割算法采用随机 采样一致 性 (random sampling consensus, RANSAC) 原理，删除范围以外的点，得到指定范 围内的点所组成的子集，从而拟合线或面。 A1 x+ B1y+C1z+ D1 = 0 A2 x+ B2y+C2z+ D2 = 0 当两个点云平面即斜坡面和地平面被分割以 后，如果以两个方程分别表示斜坡面： 和地平面： ， ·700· 智 能 系 统 学 报 第 16 卷
第4期 王新杰，等：自主移动机器人路径规划中的点云噪声处理 ·701· 则两个平面的夹角0用式(4)求取： 点)标为红色区域，统计滤波算法在不同参数下 0=(m1,z〉=arccos AiA2+BiB2+CiC2l (4) 的异常点删除对比如图4所示。不同参数的滤波 VA+B+C.VA好+B码+C☒ 结果如表1所示。 把点云分割为 斜坡面和地平面 并求取夹角 根据图像获取点云 采集图像 图2障碍物平面分割 Fig.2 Obstacle plane segmentation (a)带有噪声的点云数据集 2.3去噪方法的性能评估 为了评估统计滤波方法在不同设置下的准确 性，定义假阳性率(FPR)和假阴性率(FNR)来表示 两类不同的误差I6,FPR为被误检为噪声点的正 常点个数在总正常点云数中的比例，FNR为被误检 为正常点的噪声点个数在总噪声点云数中的比例。 3实验结果与分析 3.1数据集下的异常点剔除分析及参数选择 b)手动分类后的测试数据集（红色点为异常点） 斯坦福大学的Table Scene数据集如图3所示， 图3 Table Scene点云数据集 数据量共460400个点，将带有噪声的点云(4719 Fig.3 Table Scene point cloud dataset A=100 a=2 图4不同参数下统计滤波的异常点删除效果 Fig.4 Outlier removal effect of statistical filtering under different parameter 表1统计滤波在不同参数下的滤波结果 Table 1 Results of statistical filtering under different parameters K 算法内点 算法外点 FPR/ FNR/% 时间s 20 0 248347 212053 45.50 4493.60 10 20 0.25 370469 89931 18.70 1905.72 0 20 0.50 422854 37546 7.20 795.63 10 20 1 452077 8323 0.79 176.37 10 20 2 456450 3950 -0.17 83.70 10 20 457452 2948 -0.39 62.47 10
则两个平面的夹角 θ 用式 (4) 求取： θ = ⟨n1 , n2⟩ = arccos |A1A2 + B1B2 +C1C2| √ A 2 1 + B 2 1 +C 2 1 · √ A 2 2 + B 2 2 +C 2 2 (4) 把点云分割为 斜坡面和地平面 并求取夹角 根据图像获取点云 采集图像 图 2 障碍物平面分割 Fig. 2 Obstacle plane segmentation 2.3 去噪方法的性能评估 为了评估统计滤波方法在不同设置下的准确 性，定义假阳性率 (FPR) 和假阴性率 (FNR) 来表示 两类不同的误差[16] ，FPR 为被误检为噪声点的正 常点个数在总正常点云数中的比例，FNR 为被误检 为正常点的噪声点个数在总噪声点云数中的比例。 3 实验结果与分析 3.1 数据集下的异常点剔除分析及参数选择 斯坦福大学的 Table Scene 数据集如图 3 所示， 数据量共 460 400 个点，将带有噪声的点云 (4 719 点) 标为红色区域，统计滤波算法在不同参数下 的异常点删除对比如图 4 所示。不同参数的滤波 结果如表 1 所示。 (a) 带有噪声的点云数据集 (b) 手动分类后的测试数据集 (红色点为异常点) 图 3 Table Scene 点云数据集 Fig. 3 Table Scene point cloud dataset 原始 点云 K=20 α=0.5 K=20 α=0 K=20 α=1 K=20 α=2 K=20 α=3 K=50 α=2 K=100 α=2 图 4 不同参数下统计滤波的异常点删除效果 Fig. 4 Outlier removal effect of statistical filtering under different parameter 表 1 统计滤波在不同参数下的滤波结果 Table 1 Results of statistical filtering under different parameters K α 算法内点 算法外点 FPR/% FNR/% 时间/s 20 0 248347 212053 45.50 4493.60 10 20 0.25 370469 89931 18.70 1905.72 10 20 0.50 422854 37546 7.20 795.63 10 20 1 452077 8323 0.79 176.37 10 20 2 456450 3950 −0.17 83.70 10 20 3 457452 2948 −0.39 62.47 10 第 4 期 王新杰，等：自主移动机器人路径规划中的点云噪声处理 ·701·
·702· 智能系统学报 第16卷 续表1 K a 算法内点 算法外点 FPR/% FNR/% 时间/s 20 458050 2350 -0.52 49.80 10 50 2 456527 3873 -0.19 82.07 20 100 456413 3987 -0.16 84.49 35 200 2 455860 4540 -0.04 96.21 50 从图4可以看出，当K=20,a分别为0和0.5 机器人以0.2m/s的速度运行，如果斜坡角度 时，滤波删除了物体表面上的点；当K=20,aα=1 小于20°，机器人继续执行先前规划的路径：如果 时，滤波删除了桌面后方边缘处桌面内的点，出现 坡度大于20°，那么机器人会停下来重新规划新 了孔洞；当K=20,=3时，在桌面与地面之间出现 的路径。两处斜坡如图8所示。由Kinect-.2.0所 了没有被删除的离群点；当a=2,K分别为20、50、 拍摄的画面及通过ORB-SLAM2所构建的局部环 100时，滤波效果基本相当。根据表1的处理时 境点云如图9所示，可发现点云拼接的局部地图 间，选取K=20,a=2作为统计滤波的计算参数。 包括斜坡、蓝色卷闸门以及大量离散分布的噪声点。 3.2实际环境下的点云处理 3.2.2直通滤波 3.2.1机器人在路径规划中的点云处理 在机器人左右两侧和前方进行直通滤波之 为了验证滤波算法是否可行，选取室外环境 后，得到的点云地图如图10所示，可以看到滤波 中的两处斜坡作为研究对象，如图5所示。利用 较好地去除了无关信息，保留了机器人视角范围 Kinect-.2.0相机采集RGB图像和深度图像，通过 内的有效信息。 ORB-SLAM2算法求取关键帧，从而进行点云拼 接，构建三维点云地图，Turtlebot机器人如图6所 示，障碍物判断流程如图7所示。 Kinect-2.0 图5室外环境示意图 图6 Turtlebot机器人 Fig.5 Outdoor environment diagram Fig.6 Turtlebot robot RGB图像 ORB-SLAM2 直通滤波 平面分割 计算夹角 关键帧位姿 体素滤波 夹角0≤20° 深度图像 点云拼接 统计滤波 规划新的路径 机器人停止前进 继续先前的规划 机器人越过斜坡 图7机器人路径规划中的点云处理示意 Fig.7 Point cloud processing in robot path planning
从图 4 可以看出，当 K=20，α 分别为 0 和 0.5 时，滤波删除了物体表面上的点；当 K=20，α=1 时，滤波删除了桌面后方边缘处桌面内的点，出现 了孔洞；当 K=20，α=3 时，在桌面与地面之间出现 了没有被删除的离群点；当 α=2，K 分别为 20、50、 100 时，滤波效果基本相当。根据表 1 的处理时 间，选取 K=20，α=2 作为统计滤波的计算参数。 3.2 实际环境下的点云处理 3.2.1 机器人在路径规划中的点云处理 为了验证滤波算法是否可行，选取室外环境 中的两处斜坡作为研究对象，如图 5 所示。利用 Kinect-2.0 相机采集 RGB 图像和深度图像，通过 ORB-SLAM2 算法求取关键帧，从而进行点云拼 接，构建三维点云地图，Turtlebot 机器人如图 6 所 示，障碍物判断流程如图 7 所示。 图 5 室外环境示意图 Fig. 5 Outdoor environment diagram 机器人以 0.2 m/s 的速度运行，如果斜坡角度 小于 20°，机器人继续执行先前规划的路径；如果 坡度大于 20°，那么机器人会停下来重新规划新 的路径。两处斜坡如图 8 所示。由 Kinect-2.0 所 拍摄的画面及通过 ORB-SLAM2 所构建的局部环 境点云如图 9 所示，可发现点云拼接的局部地图 包括斜坡、蓝色卷闸门以及大量离散分布的噪声点。 3.2.2 直通滤波 在机器人左右两侧和前方进行直通滤波之 后，得到的点云地图如图 10 所示，可以看到滤波 较好地去除了无关信息，保留了机器人视角范围 内的有效信息。 Kinect-2.0 图 6 Turtlebot 机器人 Fig. 6 Turtlebot robot RGB 图像 深度图像 ORB-SLAM2 关键帧位姿 点云拼接 规划新的路径 继续先前的规划 直通滤波 体素滤波 统计滤波 机器人停止前进 机器人越过斜坡 N Y 平面分割 计算夹角 夹角 θ≤20° 图 7 机器人路径规划中的点云处理示意 Fig. 7 Point cloud processing in robot path planning 续表 1 K α 算法内点 算法外点 FPR/% FNR/% 时间/s 20 4 458050 2350 −0.52 49.80 10 50 2 456527 3873 −0.19 82.07 20 100 2 456413 3987 −0.16 84.49 35 200 2 455860 4540 −0.04 96.21 50 ·702· 智 能 系 统 学 报 第 16 卷
第4期 王新杰，等：自主移动机器人路径规划中的点云噪声处理 ·703· (a)第1处斜坡 (a)第1处斜坡 b)第2处斜坡 图10直通滤波后的点云地图 Fig.10 Point cloud map after pass through filtering 3.2.3体素滤波 直通滤波处理后，点云仍然存在许多视野重 (b)第2处斜坡 叠点，为提高算法速度，体素滤波降采样处理后 的点云如图11所示，点云变得稀疏但是仍保持着 图8室外越障环境 Fig.8 Outdoor obstacle crossing environment 原来的形状特征，计算数据量对比如表2所示。 (a)第1处斜坡 (a)第1处斜坡的RGB图(b)第2处斜坡的RGB图 像（上）和深度图像（下）像（上）和深度图像（下） (c)第1处斜坡点云图 (d第2处斜坡点云图 b)第2处斜坡 图9两处斜坡的点云地图 图11体素降采样后的点云地图 Fig.9 Point cloud maps of two slopes Fig.11 Point cloud map after voxel down sampling 表2体素滤波前后的点云地图数据对比 Table 2 Comparison of point cloud map data before and after voxel filtering 环境类型 地图类型 点云数个 存储大小MB 第1处斜坡 降采样前的点云地图 3568538 57.1 降采样后的点云地图 54583 0.8774 降采样前的点云地图 第2处斜坡 1246009 19.9 降采样后的点云地图 122322 2.0
(a) 第 1 处斜坡 (b) 第 2 处斜坡 图 8 室外越障环境 Fig. 8 Outdoor obstacle crossing environment (a) 第 1 处斜坡的 RGB 图 像 (上) 和深度图像 (下) (b) 第 2 处斜坡的 RGB 图 像 (上) 和深度图像 (下) (c) 第 1 处斜坡点云图 (d) 第 2 处斜坡点云图 图 9 两处斜坡的点云地图 Fig. 9 Point cloud maps of two slopes (a) 第 1 处斜坡 (b) 第 2 处斜坡 图 10 直通滤波后的点云地图 Fig. 10 Point cloud map after pass through filtering 3.2.3 体素滤波 直通滤波处理后，点云仍然存在许多视野重 叠点，为提高算法速度，体素滤波降采样处理后 的点云如图 11 所示，点云变得稀疏但是仍保持着 原来的形状特征，计算数据量对比如表 2 所示。 (a) 第 1 处斜坡 (b) 第 2 处斜坡 图 11 体素降采样后的点云地图 Fig. 11 Point cloud map after voxel down sampling 表 2 体素滤波前后的点云地图数据对比 Table 2 Comparison of point cloud map data before and after voxel filtering 环境类型 地图类型 点云数/个 存储大小/MB 第1处斜坡 降采样前的点云地图 3568 538 57.1 降采样后的点云地图 54 583 0.8774 第2处斜坡 降采样前的点云地图 1246 009 19.9 降采样后的点云地图 122 322 2.0 第 4 期 王新杰，等：自主移动机器人路径规划中的点云噪声处理 ·703·
·704· 智能系统学报 第16卷 从表2可得，经过体素降采样后，第1处斜坡 3.2.5点云分割及其平面夹角计算 环境地图中的点云数减少98.47%，存储空间减少 经过直通、体素和统计滤波处理以后，算法 了98.46%：第2处斜坡环境地图中的点云数减少 将图12障碍物点云分割为两个平面，其效果如 90.18%,存储空间减少了89.95%。 图13所示。 3.2.4统计滤波 分割出的斜坡面和地平面的方程系数如表3 从图11可以看出，经过直通滤波和体素滤波 所示，将每一个平面方程的前3个系数A1、B、C1 处理之后的两处斜坡点云，仍然存在远离点云平 和A2、B2、C2代入式(4)可求得第1处斜坡坡度大 面的异常点，尤其是第2处斜坡出现的噪声点要 小约为32°，第2处斜坡坡度大小约为10°。 比第1处斜坡更多（红色框标出部分），可能的原 因是机器人在第2处斜坡处的相机获得的信息更 复杂，深度图像范围越大，噪声越多，严重影响平 面分割的准确度，需要进行异常点的删除。采用 3.1节得到的K=20,a=2作为统计滤波输入参数， 滤波效果如图12所示，基本保留了第1处斜坡和 第2处斜坡表面的内点，去除了离群的噪声外点。 (a)第1处斜坡的分割结果 (a)第1处斜坡 (b)第2处斜坡 (b)第2处斜坡的分割结果 图12统计滤波剔别除离群点后的点云地图 图13分割后的斜坡面和地平面点云 Fig.12 Point cloud map with outliers removed by statistic- Fig.13 Point cloud of slope and ground plane after seg- al filtering mentation 表3分割出的各平面方程系数值 Table 3 Coefficients of each plane equation separated out 类型 方程各项 第1处障碍平面系数 第2处障碍平面系数 A -0.5628770 -0.1150810 -0.0675281 -0.0904616 斜坡面 C 0.8237770 0.9933560 D 1.1172500 0.4170340 A2 -0.03869850 -0.0626566 B2 0.00429857 0.0770747 地平面 C2 0.99924200 0.9980350 D2 0.32343200 0.3597780
从表 2 可得，经过体素降采样后，第 1 处斜坡 环境地图中的点云数减少 98.47%，存储空间减少 了 98.46%；第 2 处斜坡环境地图中的点云数减少 90.18%，存储空间减少了 89.95%。 3.2.4 统计滤波 从图 11 可以看出，经过直通滤波和体素滤波 处理之后的两处斜坡点云，仍然存在远离点云平 面的异常点，尤其是第 2 处斜坡出现的噪声点要 比第 1 处斜坡更多 (红色框标出部分)，可能的原 因是机器人在第 2 处斜坡处的相机获得的信息更 复杂，深度图像范围越大，噪声越多，严重影响平 面分割的准确度，需要进行异常点的删除。采用 3.1 节得到的 K=20，α=2 作为统计滤波输入参数， 滤波效果如图 12 所示，基本保留了第 1 处斜坡和 第 2 处斜坡表面的内点，去除了离群的噪声外点。 (a) 第 1 处斜坡 (b) 第 2 处斜坡 图 12 统计滤波剔除离群点后的点云地图 Fig. 12 Point cloud map with outliers removed by statistical filtering 3.2.5 点云分割及其平面夹角计算 经过直通、体素和统计滤波处理以后，算法 将图 12 障碍物点云分割为两个平面，其效果如 图 13 所示。 A1 B1 C1 A2 B2 C2 分割出的斜坡面和地平面的方程系数如表 3 所示，将每一个平面方程的前 3 个系数 、 、 和 、 、 代入式 (4) 可求得第 1 处斜坡坡度大 小约为 32°，第 2 处斜坡坡度大小约为 10°。 (a) 第 1 处斜坡的分割结果 (b) 第 2 处斜坡的分割结果 图 13 分割后的斜坡面和地平面点云 Fig. 13 Point cloud of slope and ground plane after segmentation 表 3 分割出的各平面方程系数值 Table 3 Coefficients of each plane equation separated out 类型 方程各项 第1处障碍平面系数 第2处障碍平面系数 斜坡面 A1 −0.562877 0 −0.115 0810 B1 −0.067528 1 −0.090 4616 C1 0.823777 0 0.993 3560 D1 1.117250 0 0.417 0340 地平面 A2 −0.038698 50 −0.062 6566 B2 0.004298 57 0.077 0747 C2 0.999242 00 0.998 0350 D2 0.323432 00 0.359 7780 ·704· 智 能 系 统 学 报 第 16 卷
第4期 王新杰，等：自主移动机器人路径规划中的点云噪声处理 ·705· 3.2.6机器人路径规划过程 到位姿D重新规划路径，第2处斜坡夹角约10° 机器人检测到前方有斜坡，算法经过式(4)计 机器人可直行，经过位姿E、F、G到达坡上终点， 算后判断是否可以通过。由于第一个斜坡夹角 整个过程如图14所示，机器人运动时线速度和角 为32°，Turtlebot机器人无法通过，只能左转运动 速度变化如图15所示。 图14机器人路径规划线路 Fig.14 Robot path planning 1.5 行。下一步计划在前端视觉里程计中，根据图像 -V .0 的灰度信息来计算相机运动，减少对环境纹理的 0.5 依赖，实现更鲁棒的三维点云地图处理。 参考文献： -0.5 -1.0 点 E F G点 []叶一飞，王建中基于点云的复杂环境下楼梯区域识别 0 20 40 60 80 100120 [).电子测量与仪器学报，2020,34(4)：124-133. t/s YE Yifei,WANG Jianzhong.Stair area identification in 图15驱动轮线速度()和角速度()变化曲线 complex environment based on point cloud[J].Journal of Fig.15 Variation curve of linear velocity and angular electronic measurement and instrumentation,2020,34(4): velocity (W)of driving wheel 124-133 从图15可以看出，机器人在位姿A处检测到 [2]李茂月，马康盛，王飞，等.基于结构光在机测量的叶片 第1处斜坡，停止前进对坡度进行计算，计算出来 点云预处理方法研究[.仪器仪表学报，2020,41(8)： 的夹角32°大于机器人越障的阈值20°，故无法通 55-66 过，机器人原地旋转变为位姿B:在经过位姿C、 LI Maoyue,MA Kangsheng,WANG Fei,et al.Research 位姿D的过程中，机器人同时有线速度和角速度 on the preprocessing method of blade point cloud based on 的变化，在位姿E处检测到第2处斜坡，同样停止 structured light on-machine measurement[J].Chinese 前进对坡度进行计算，计算出来的夹角10°小于 journal of scientific instrument,2020,41(8):55-66 系统设定的阈值，机器人继续前进，经过F点和 [3]CHEN Siyuan,TRUONG-HONG L,O'KEEFFE E,et al. G点到达设定的终点。 Outlier detection of point clouds generating from low-cost UAVs for bridge inspection[Cl//Proceedings of the 6th In- 4结束语 ternational Symposium on Life-Cycle Civil Engineering. Ghent,Belgium,2018. 本文对户外的机器人运动环境建立了三维点 [4]ALEXA M,BEHR J,COHEN-OR D,et al.Computing and 云地图，通过对点云地图进行直通滤波筛选出目 rendering point set surfaces[J].IEEE transactions on visu- 标数据，再通过体素滤波降采样，然后使用统计 alization and computer graphics,2003,9(1):3-15. 滤波对目标点云中的噪声点进行删除，最后通过 [5]CHEN Shenen,RICE C.BOYLE C,et al.Small-format 分割后的两个平面夹角，判断机器人是否可以通 aerial photography for highway-bridge monitoring[J]
3.2.6 机器人路径规划过程 机器人检测到前方有斜坡，算法经过式 (4) 计 算后判断是否可以通过。由于第一个斜坡夹角 为 32°，Turtlebot 机器人无法通过，只能左转运动 到位姿 D 重新规划路径，第 2 处斜坡夹角约 10°， 机器人可直行，经过位姿 E、F、G 到达坡上终点， 整个过程如图 14 所示，机器人运动时线速度和角 速度变化如图 15 所示。 起点 终点 位姿 A 位姿 B 位姿 C 位姿 D 位姿 E 位姿 F 位姿 G 图 14 机器人路径规划线路 Fig. 14 Robot path planning 起 点 终 点 位 姿 A 位 姿 B 位 姿 C 位 姿 D 位 姿 E 位 姿 F 位 姿 G 0 20 40 60 80 100 120 1.0 1.5 0.5 0 −0.5 −1.0 V W t/s V/(m·s−1 ); W/(rad·s−1 ) 图 15 驱动轮线速度 (V) 和角速度 (W) 变化曲线 Fig. 15 Variation curve of linear velocity (V) and angular velocity (W) of driving wheel 从图 15 可以看出，机器人在位姿 A 处检测到 第 1 处斜坡，停止前进对坡度进行计算，计算出来 的夹角 32°大于机器人越障的阈值 20°，故无法通 过，机器人原地旋转变为位姿 B；在经过位姿 C、 位姿 D 的过程中，机器人同时有线速度和角速度 的变化，在位姿 E 处检测到第 2 处斜坡，同样停止 前进对坡度进行计算，计算出来的夹角 10°小于 系统设定的阈值，机器人继续前进，经过 F 点和 G 点到达设定的终点。 4 结束语 本文对户外的机器人运动环境建立了三维点 云地图，通过对点云地图进行直通滤波筛选出目 标数据，再通过体素滤波降采样，然后使用统计 滤波对目标点云中的噪声点进行删除，最后通过 分割后的两个平面夹角，判断机器人是否可以通 行。下一步计划在前端视觉里程计中，根据图像 的灰度信息来计算相机运动，减少对环境纹理的 依赖，实现更鲁棒的三维点云地图处理。 参考文献： 叶一飞, 王建中. 基于点云的复杂环境下楼梯区域识别 [J]. 电子测量与仪器学报, 2020, 34(4): 124–133. YE Yifei, WANG Jianzhong. Stair area identification in complex environment based on point cloud[J]. Journal of electronic measurement and instrumentation, 2020, 34(4): 124–133. [1] 李茂月, 马康盛, 王飞, 等. 基于结构光在机测量的叶片 点云预处理方法研究 [J]. 仪器仪表学报, 2020, 41(8): 55–66. LI Maoyue, MA Kangsheng, WANG Fei, et al. Research on the preprocessing method of blade point cloud based on structured light on-machine measurement[J]. Chinese journal of scientific instrument, 2020, 41(8): 55–66. [2] CHEN Siyuan, TRUONG-HONG L, O'KEEFFE E, et al. Outlier detection of point clouds generating from low-cost UAVs for bridge inspection[C]//Proceedings of the 6th International Symposium on Life-Cycle Civil Engineering. Ghent, Belgium, 2018. [3] ALEXA M, BEHR J, COHEN-OR D, et al. Computing and rendering point set surfaces[J]. IEEE transactions on visualization and computer graphics, 2003, 9(1): 3–15. [4] CHEN Shenen, RICE C, BOYLE C, et al. Small-format aerial photography for highway-bridge monitoring[J]. [5] 第 4 期 王新杰，等：自主移动机器人路径规划中的点云噪声处理 ·705·
·706· 智能系统学报 第16卷 Journal of performance of constructed facilities,2011, automatica sinica,2015,41(2):285-294. 25(2):105-112 [14]王任栋，徐友春，齐尧，等.一种鲁棒的城市复杂动态场 [6]ZHANG Yingjie,GE Liling.Improved moving least 景点云配准方法).机器人，2018,40(3)：257-265. squares algorithm for directed projecting onto point WANG Rendong,XU Youchun,QI Yao,et al.A robust clouds[J].Measurement,2011,44(10):2008-2019. point cloud registration method in urban dynamic envir- [7]BERGER M,TAGLIASACCHI A,SEVERSKY L,et al. onment[J.Robot,.2018,40(3:257-265. State of the art in surface reconstruction from point [15]LIU Dingning,DING Qiong.Point cloud processing sys- clouds[C]//Eurographics 2014-State of the Art Reports. tem development based on PCL and Qt[J].International Strasbourg,France,2014:161-185. journal of engineering management,2019,3(1):33-39. [8]NURUNNABI A,WEST G,BELTON D.Outlier detec- [16]TRUONG-HONG L,LAEFER D F.Quantitative evalu- tion and robust normal-curvature estimation in mobile laser ation strategies for urban 3D model generation from re- scanning 3D point cloud data[J].Pattern recognition,2015, mote sensing data[J].Computers graphics,2015,49: 48(4):14041419. 82-91. [9]ZAMAN F,WONG Y P,NG B Y.Density-based denois- 作者简介： ing of point cloud [M]//IBRAHIM H,IQBAL S,TEOH S 王新杰，硕士研究生，主要研究方 S,et al.9th International Conference on Robotic,Vision, 向为模式识别与图像处理、视觉导航。 Signal Processing and Power Applications.Singapore: Springer,,2017:287-295. [10]MUR-ARTAL R,TARDOS J D.ORB-SLAM2:an open- source SLAM system for monocular,stereo,and RGB-D cameras[J].IEEE transactions on robotics,2017,33(5): 1255-1262 张莹，副教授，湖南省自动化学会 [11]MUR-ARTAL R,MONTIEL J MM,TARDOS J D. 理事，主要研究方向为机器人控制、视 ORB-SLAM:a versatile and accurate monocular SLAM 觉导航。主持和参与国家自然科学基 金项目、湖南省科技厅项目、湖南省教 system[J].IEEE transactions on robotics,2015,31(5): 育厅项目等多项。发表学术论文 1147-1163. 30余篇。 [12]WOLFF K,KIM C.ZIMMER H,et al.Point cloud noise and outlier removal for image-based 3D reconstruction[Cl/ Proceedings of 2016 Fourth International Conference on 张东波，教授，主要研究方向为模 式识别与图像处理、深度学习。湖南 3D Vision.Stanford,CA,USA,2016:118-127. 省自动化学会副理事长、湖南省青年 [13]祝继华，周顾，王晓春，等.基于图像配准的栅格地图拼 骨干教师。近5年，承担和参与国家 接方法[).自动化学报，2015,41(2)：285-294 级、省部级课题10余项。发表学术论 ZHU Jihua,ZHOU Yi,WANG Xiaochun,et al.Grid Map 文100余篇，获授权发明专利7项。 merging approach based on image registration[]].Acta
Journal of performance of constructed facilities, 2011, 25(2): 105–112. ZHANG Yingjie, GE Liling. Improved moving least squares algorithm for directed projecting onto point clouds[J]. Measurement, 2011, 44(10): 2008–2019. [6] BERGER M, TAGLIASACCHI A, SEVERSKY L, et al. State of the art in surface reconstruction from point clouds[C]//Eurographics 2014-State of the Art Reports. Strasbourg, France, 2014: 161−185. [7] NURUNNABI A, WEST G, BELTON D. Outlier detection and robust normal-curvature estimation in mobile laser scanning 3D point cloud data[J]. Pattern recognition, 2015, 48(4): 1404–1419. [8] ZAMAN F, WONG Y P, NG B Y. Density-based denoising of point cloud[M]//IBRAHIM H, IQBAL S, TEOH S S, et al. 9th International Conference on Robotic, Vision, Signal Processing and Power Applications. Singapore: Springer, 2017: 287−295. [9] MUR-ARTAL R, TARDÓS J D. ORB-SLAM2: an opensource SLAM system for monocular, stereo, and RGB-D cameras[J]. IEEE transactions on robotics, 2017, 33(5): 1255–1262. [10] MUR-ARTAL R, MONTIEL J M M, TARDÓS J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE transactions on robotics, 2015, 31(5): 1147–1163. [11] WOLFF K, KIM C, ZIMMER H, et al. Point cloud noise and outlier removal for image-based 3D reconstruction[C]// Proceedings of 2016 Fourth International Conference on 3D Vision. Stanford, CA, USA, 2016: 118−127. [12] 祝继华, 周颐, 王晓春, 等. 基于图像配准的栅格地图拼 接方法 [J]. 自动化学报, 2015, 41(2): 285–294. ZHU Jihua, ZHOU Yi, WANG Xiaochun, et al. Grid Map merging approach based on image registration[J]. Acta [13] automatica sinica, 2015, 41(2): 285–294. 王任栋, 徐友春, 齐尧, 等. 一种鲁棒的城市复杂动态场 景点云配准方法 [J]. 机器人, 2018, 40(3): 257–265. WANG Rendong, XU Youchun, QI Yao, et al. A robust point cloud registration method in urban dynamic environment[J]. Robot, 2018, 40(3): 257–265. [14] LIU Dingning, DING Qiong. Point cloud processing system development based on PCL and Qt[J]. International journal of engineering management, 2019, 3(1): 33–39. [15] TRUONG-HONG L, LAEFER D F. Quantitative evaluation strategies for urban 3D model generation from remote sensing data[J]. Computers & graphics, 2015, 49: 82–91. [16] 作者简介： 王新杰，硕士研究生，主要研究方 向为模式识别与图像处理、视觉导航。 张莹，副教授，湖南省自动化学会 理事，主要研究方向为机器人控制、视 觉导航。主持和参与国家自然科学基 金项目、湖南省科技厅项目、湖南省教 育厅项目等多项。发表学术论 文 30 余篇。 张东波，教授，主要研究方向为模 式识别与图像处理、深度学习。湖南 省自动化学会副理事长、湖南省青年 骨干教师。近 5 年，承担和参与国家 级、省部级课题 10 余项。发表学术论 文 100 余篇，获授权发明专利 7 项。 ·706· 智 能 系 统 学 报 第 16 卷