机器人在自主导航过程中，对当前环境的点云地图进行运算处理，根据分析结果实施路径规划。但点云地图会由于传感器误差、光照、阴影等外在因素，导致存在大量的冗余信息，需要通过滤波消除不利影响^[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的点云拼接

ORB-SLAM2利用RGB图像信息和对应的深度信息进行点云拼接，获取环境特征的地图形式。定义序列 $X = \left\{ {{x_1},{x_2}, \cdots ,{x_n}} \right\}$ 来表示三维空间点的集合，其中 ${{\boldsymbol{x}}_1} = {\left[ {x\;\;y\;\;z} \right]^{\rm{T}}}$ 表示相机在三维空间中的坐标。则三维空间中的坐标点映射到二维图像平面的像素坐标 ${\left[ {u\;\;v} \right]^{\rm{T}}}$ 用相机成像模型来描述：

$d\left[ \begin{array}{l} u \\ v \\ 1 \\ \end{array} \right] = {\boldsymbol{K}} \cdot \left( {{\boldsymbol{R}} \cdot \left[ \begin{array}{l} x \\ y \\ z \\ \end{array} \right] + {\boldsymbol{t}}} \right)$

(1)

式中： ${\boldsymbol{K}}$ 是相机的内参数矩阵； ${\boldsymbol{R}}$ 和 ${\boldsymbol{t}}$ 分别为相机的旋转矩阵和平移向量； $d$ 是像素值的深度。

相机坐标 ${\left[ {x\;\;y\;\;z} \right]^{\rm{T}}}$ 可以根据其在三维空间的坐标到二维像素坐标的反推计算：

$\left[ \begin{array}{l} x \\ y \\ z \\ \end{array} \right] = \left[ \begin{array}{l} (u - {c_x}) \cdot z/{f_x} \\ (v - {c_y}) \cdot z/{f_y} \\ d \\ \end{array} \right]$

(2)

其中， ${c_x}$ 、 ${c_y}$ 和 ${f_x}$ 、 ${f_y}$ 分别为相机内参中的光心和焦距，是相机的固有参数。

式(2)将当前帧的RGB图像、深度图像，关键帧对应的相机位姿信息变换到世界坐标，创建当前帧的点云地图。本文采用迭代最近点(iterated closest points, ICP)^[13-14]算法将不同坐标系下的点云对准到同一世界坐标系，实现局部点云地图的拼接。算法如图1所示。

ICP首先将待拼接的2片点云或其中1片点云进行采样，然后根据点到点、点到投影、点到面这3种方法确定对应点集，一般采用最小二乘法迭代求解2片点云间的最优坐标变换，完成点云的拼接。

2 路径规划中的点云数据处理 2.1 点云模型中的滤波方法

机器人路径规划一般用直通滤波分割点云的范围，删除指定场景外的点。深度相机获取的点云数据量过大，需要经过降采样处理以减少点的数量，提高处理效率，路径规划算法常用体素滤波进行数据压缩，节省存储空间。由于设备精度误差和环境物体间的视野相互遮挡，深度相机输出的原始点云数据存在大量离群噪声点，路径规划算法常用统计滤波去除离群点噪声，保留物体表面上的点。

统计滤波首先通过 $K$ 个最近邻域搜索功能计算点到其邻域的平均距离。计算如式(3)所示。

$ {{\rm{outlier}}_i} = \left\{ {\begin{array}{*{20}{l}} {{\rm{true}},\;\;\;\;\mu \pm \alpha \cdot \sigma \geqslant l}\\ {{\rm{false}},\;{\text{其他}} } \end{array}} \right. $

(3)

式中： $\mu $ 代表估计的平均距离； $\sigma $ 代表标准差； $\alpha $ 为标准差倍数阈值。如果某点的计算值 $\mu \pm \alpha \cdot \sigma $ 不小于点云中所有点与邻域的距离 $l$ ，则被认为是异常点，可将其剔除。故离群点的去除效果由阈值 $K$ 和 $\alpha $ 决定。

2.2 障碍物点云分割及其坡度计算

机器人越障需把斜坡面和地平面分割出来，计算两个平面之间的夹角，如图2所示。经典的点云分割算法分为基于聚类的分割算法和基于模型的分割算法^[15]，基于模型的分割算法采用随机采样一致性(random sampling consensus, RANSAC)原理，删除范围以外的点，得到指定范围内的点所组成的子集，从而拟合线或面。

当两个点云平面即斜坡面和地平面被分割以后，如果以两个方程分别表示斜坡面： ${A_1}x + $ $ {B_1}y + {C_1}z + {D_1} = 0$ 和地平面： ${A_2}x + {B_2}y + {C_2}z + {D_2} = 0$ ，则两个平面的夹角 $\theta $ 用式(4)求取：

$ \theta = \left\langle {{{{\boldsymbol{n}}_1}} , {{{\boldsymbol{n}}_2}} } \right\rangle = \arccos \frac{{\left| {{A_1}{A_2} + {B_1}{B_2} + {C_1}{C_2}} \right|}}{{\sqrt {A_1^2 + B_1^2 + C_1^2} \cdot \sqrt {A_2^2 + B_2^2 + C_2^2} }} $

(4)

2.3 去噪方法的性能评估

为了评估统计滤波方法在不同设置下的准确性，定义假阳性率(FPR)和假阴性率(FNR)来表示两类不同的误差^[16]，FPR为被误检为噪声点的正常点个数在总正常点云数中的比例，FNR为被误检为正常点的噪声点个数在总噪声点云数中的比例。

3 实验结果与分析 3.1 数据集下的异常点剔除分析及参数选择

斯坦福大学的Table Scene数据集如图3所示，数据量共460400个点，将带有噪声的点云(4719点)标为红色区域，统计滤波算法在不同参数下的异常点删除对比如图4所示。不同参数的滤波结果如表1所示。

从图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所示。

机器人以0.2 m/s的速度运行，如果斜坡角度小于20°，机器人继续执行先前规划的路径；如果坡度大于20°，那么机器人会停下来重新规划新的路径。两处斜坡如图8所示。由Kinect-2.0所拍摄的画面及通过ORB-SLAM2所构建的局部环境点云如图9所示，可发现点云拼接的局部地图包括斜坡、蓝色卷闸门以及大量离散分布的噪声点。

3.2.2 直通滤波

在机器人左右两侧和前方进行直通滤波之后，得到的点云地图如图10所示，可以看到滤波较好地去除了无关信息，保留了机器人视角范围内的有效信息。

3.2.3 体素滤波

直通滤波处理后，点云仍然存在许多视野重叠点，为提高算法速度，体素滤波降采样处理后的点云如图11所示，点云变得稀疏但是仍保持着原来的形状特征，计算数据量对比如表2所示。

从表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处斜坡表面的内点，去除了离群的噪声外点。

3.2.5 点云分割及其平面夹角计算

经过直通、体素和统计滤波处理以后，算法将图12障碍物点云分割为两个平面，其效果如图13所示。

分割出的斜坡面和地平面的方程系数如表3所示，将每一个平面方程的前3个系数 ${A_1}$ 、 ${B_1}$ 、 ${C_1}$ 和 ${A_{\rm{2}}}$ 、 ${B_{\rm{2}}}$ 、 ${C_2}$ 代入式(4)可求得第1处斜坡坡度大小约为32°，第2处斜坡坡度大小约为10°。

3.2.6 机器人路径规划过程

机器人检测到前方有斜坡，算法经过式(4)计算后判断是否可以通过。由于第一个斜坡夹角为32°，Turtlebot机器人无法通过，只能左转运动到位姿D重新规划路径，第2处斜坡夹角约10°，机器人可直行，经过位姿E、F、G到达坡上终点，整个过程如图14所示，机器人运动时线速度和角速度变化如图15所示。

从图15可以看出，机器人在位姿A处检测到第1处斜坡，停止前进对坡度进行计算，计算出来的夹角32°大于机器人越障的阈值20°，故无法通过，机器人原地旋转变为位姿B；在经过位姿C、位姿D的过程中，机器人同时有线速度和角速度的变化，在位姿E处检测到第2处斜坡，同样停止前进对坡度进行计算，计算出来的夹角10°小于系统设定的阈值，机器人继续前进，经过F点和G点到达设定的终点。

4 结束语

本文对户外的机器人运动环境建立了三维点云地图，通过对点云地图进行直通滤波筛选出目标数据，再通过体素滤波降采样，然后使用统计滤波对目标点云中的噪声点进行删除，最后通过分割后的两个平面夹角，判断机器人是否可以通行。下一步计划在前端视觉里程计中，根据图像的灰度信息来计算相机运动，减少对环境纹理的依赖，实现更鲁棒的三维点云地图处理。