基于ROS与三维点云图像的室内物体精准定位

于 洋，朴 燕，倪 焱，佀同岭

(长春理工大学 电子信息工程学院，吉林 长春 130022)

1 引 言

由于GPS卫星定位的小环境局限性和室内环境的复杂性，室内物体定位技术便有了大展拳脚的机会。室内小型激光雷达与KINECT深度相机的出现和发展使得我们可以轻而易举地获得物体高清RGB图像、室内场景点云和三维特征点云图像信息，为能够实现物体更好的定位提供了技术手段。

使用三维点云图像数据来描述物体特征简单直观，而且数据获取方便。它主要是通过三维扫描获取设备，记录被检测物体表面上大量高度密集的点的三维坐标、反射率等信息，从而描述物体特征。

基于ROS的移动平台[1]由于其开源性近年发展迅速。许多知名机器人开源库，如TF坐标转换、3D点云处理驱动、SLAM等都是基于ROS开发的。

本研究以基于ROS的移动平台为基础，在其上面搭载了RPLIDAR A2激光雷达与KINECT深度相机，通过SLAM算法对室内地图进行实时构建，通过RPLIDAR A2激光雷达构建的二维地图与KINECT获得的物体的三维点云进行复合坐标定位来实现物体的精准定位。

2 定位系统整体设计

2.1 软硬件平台的搭建

为了完成室内物品的精准定位研究，在实验室自主研发的基于ROS的智能数据采集移动底盘的基础上放置了RPLIDAR A2激光雷达与KINECT相机。其硬件系统构成如图1所示。

图1 硬件系统构成图Fig.1 Hardware system diagram

这里重点介绍一下负责全局地图构建的RPLIDAR A2激光雷达与负责三维点云采集的KINECT深度相机。RPLIDAR A2具有高速激光测距采样能力，可对在米半径范围内物体进行激光测距扫描，并产生点云地图信息[2]。这些点云信息可以对室内全局进行地图构建。由于我们的研究地点为室内环境，该环境的特征是整体面积偏小，但是局部物品堆放复杂，要求物体定位信息精准。ROLIDAR A2的工作性能指标刚好满足我们的要求。

三维扫描设备可以快速精确地获得物体表面的形状和空间拓普信息。微软研发的KINECT深度相机采集到的物体三维点云信息更为精确且很难受到光照、纹理等条件影响，不但外观小巧方便装载，而且成本也不高[3]。因此本研究采用KINECT来获得物体的三维特征点云信息。

在构建了硬件平台后，想要完成本文研究还需要软件平台的支持。本研究主要采用了基于LINUX的Ubuntu16.04系统与开源机器人操作系统ROS(Robot Operating System)。ROS有很多优点，它是开源的，提供底层驱动程序管理、共用功能的执行、程序间的消息传递、程序发行包管理等，它也提供一些工具程序和库用于获取、建立、编写和运行多机整合的程序[4]。

2.2 融合算法实现流程

本文通过Gmapping算法结合了SLAMTEC发布的关于RPLIDAR激光雷达的ROS功能包rplidar_ros获取室内地图，然后使用KINCET深度相机结合orb_slam包来获取物体三维特征点云图像，最后通过点云图像与室内地图对物体进行复合坐标定位，从而实现室内物体的精准定位。其具体算法实现流程图如图2所示。

图2 算法实现流程图Fig.2 Algorithm realization flow chart

3 复合坐标的精准定位

图3 复合坐标精准定位流程图Fig.3 Flow chart for precise positioning of composite coordinates

我们知道二维坐标可以锁定平面内物体的位置，三维坐标可以表示空间内物体的位置，而本文采用的是四维复合坐标的方式来进行物体的精准定位，定位精度高。其完成复合四维坐标精准定位流程图如图3所示。

为完成本定位研究，主要研究内容为：室内地图构建、三维点云图像的获取、三维坐标信息计算与复合坐标系定位等步骤。

3.1 Gmapping算法选取与室内地图构建

室内物体精准定位的关键点在于获取物体基于某一坐标系的三维坐标。多室内的地图构建正是获取物体位置的关键一环。选取建图算法就更是重中之重。

Gmapping算法是基于RBPF的粒子滤波算法[5]，它在RBPF算法上做了两个重要的改进：为了减小粒子数Gmapping算法提出了改进提议分布，为了减少重采样的次数Gmapping算法提出了选择性重采样[6]。

这两个改进对构建室内地图来说影响重大。首先说一下改进提议分布。改进提议分布主要由两部分组成，一部分有计算出数据的提议分布，另一部分是权重计算。以图4为例，图中虚线概率分布是里程计采样的高斯分布，实线的概率分布也是使用激光进行观测后获得状态的高斯分布。由图可知观测提供的信息的准确度(方差小)相比控制的准确度要高很多。

图4 概率分布图Fig.4 Probability map

如果通过从提议分布中采样得到下一时刻移动平台的位姿，那么提议分布与目标分布越接近的话所使用的粒子越少。如果粒子是直接从目标分布采样的话只需要一个粒子就可以获得移动平台的位姿估计了。因此Gmapping算法要做的是改进提议分布，如果只从里程计中采样的，粒子的权重迭代公式见式(1)。

(1)

激光的分布相比里程计分布更接近真正的目标分布，因此如果可以把激光的信息融入到提议分布中的话那样提议分布就会更接近目标分布。因此本文采用RPLIDAR A2激光雷达来获取激光信息，分布计算公式变成式(2)，而粒子的权重公式计算公式变成了式(3)。

(2)

(3)

目标分布在大多数情况下只有一个峰值。为了简化计算量在峰值附近采K个值来模拟出提议分布。Gmapping算法使用高斯函数来构建提议分布，因此有了K个数据后就可以模拟出一个高斯函数作为提议分布:

.

(4)

Gmapping在计算权重时就是计算模拟出的提议分布和目标分布的不同。而这种不同体现在我们是由有限的采样点模拟出目标分布，因此权重的计算公式为:

.

(5)

至此Gmapping改进提议分布完成。接下来是重采样部分。这部分来说相对比较简单，Gmmaping算法通过设定一个阈值，当粒子的权重变化大于我们设定的阈值时就会执行重采样，这样减少了采样的次数，也就减缓了粒子退化，从而提高地图精度。

考虑到Gmapping算法在构建小场景地图所需计算量较小，而且精度较高，因此本研究采用Gmmaping算法对地图进行构建。

本文的实验地点为实验室。理论上，构图激光粒子数目越多，构建出的地图越准确。但实际操作中由于激光粒子的数目增多，计算机负载骤升，构图噪声也会更加明显。图5为构图粒子数对构图的影响。在构图激光粒子数目P由30增加到150的过程中，生成的二维地图精度符合似正态分布。通过试验比对，构图粒子选择为150。在选择最优粒子数后，Gmapping的Linear Update和Angular Update参数对构图效果也会产生影响。最终通过比对我们选择了Angular Update参数为0.2 ，且Linear Update参数为0.2 时地图构建效果最好。对比效果如图6所示。

图5 构图粒子数对构图的影响Fig.5 Influence of composition number on composition

图6 Linear Update和Angular Update对构图的影响Fig.6 Influence of Linear Update and Angular Update on composition

最终本文选择激光粒子数P=150，Linear Update与Angular Update都为0.2进行构图和后续实验。

3.2 二维坐标的标定

二维坐标的标定是三维外部坐标系φ建立的基础。通过在虚拟机Rviz软件中可以加载已经构建成功的室内2D地图。取构建好的地图左下角为二维坐标系原点，室内长宽分别为二维坐标系x，y轴。首先需要测量室内的长宽，然后对Rviz软件栅格等比例缩放来获得单位栅格长宽大小。用式(6)可以算出任意一点的坐标，其中P点为移动平台在Rviz中的实时显示的点。

.

(6)

室内2D地图的坐标标定至此完成，其效果如图7所示。

图7 任一点的二维标定Fig.7 Two dimensional calibration of any point

3.3 物体三维特征点云图像获取

本文用KINECT深度相机获取待定位物体点云图像。在PC端Rviz仿真软件点选待移动目标点，此时移动底盘将自主移动避障前往目标点。从高清摄像头中观测到待定位物体之后，我们让数据采集底盘在待定位物体周围反复移动，从不同的视觉获取特征点云从而获得物体三维点云图像。

图8 高清图像与特征点云Fig.8 High definition image and feature point cloud

图9 物体三维点云Fig.9 Object 3D point cloud

图7中，左侧图像是摄像头采集的原始图像，含有红绿特征点的黑白图像。对比两图，在左图中选取待定位物体之后，在待定位物体附近让数据采集底盘多次来回移动以保证采集足够多的特征点。最后在本地虚拟机中打开Rviz便可以看到采集到的物体三维点云图像了。如图9所示，其中蓝色方框为移动底盘行驶轨迹，红色点云为采集到的物体三维点云图像。

3.4 复合坐标精准定位

首先进行四维定位坐标系的构建。首先由构建地图获得的二维坐标结合空间信息组合成三维外部坐标系φ，然后通过对不同室内进行分类Ⅰ、Ⅱ、Ⅲ,结合外部坐标系构成四维定位坐标系。

四维坐标系构建完成后，为了获取到待定位物体的三维信息，要对获得物体三维特征点云图像进行滤波、聚类、坐标变换等过程。

滤波这里我们采取双边滤波器[7]对点云进行滤波。双边滤波器的好处是可以做到边缘保存，它是基于空间分布的高斯滤波函数，所以在点云边缘附近，离得较远的像素不会太多影响到边缘上的像素值[8]。

由KINECT视觉传感器的工作原理可知，本文获得的点云数据坐标都是相对于KINECT视觉传感器得到的，KINECT的摄像头坐标系和我们所定义的外部的坐标系不同。因此我们根据特征点云进行参数优化的方法，建立了坐标变换矩阵(7)。

(7)

其中表示旋转的为3乘3矩阵Ri，表示平移的为三维向量ti。我们在φ中选组坐标已知的M个点，坐标记为pk,然后测量这些点在φ中的坐标Q，根据变换关系就可以计算出他们在φ中的坐标TiQk。最后计算出变换矩阵为：

.

(8)

为了计算两个点之间的距离误差，构造了函数E。在实际中取三维欧几里得距离平方求得。

E(P,Q)=(Px-Qx)2+(Py-Qy)2+
(Pz-Qz)2

，

(9)

其中：P表示P对应于KINECT的三维坐标，Ti为坐标变换矩阵，求和过程相当于将所有计算后的点云坐标合并，其坐标值是基于我们定义的外部坐标系。

上述坐标变换校准过程的关键点是在于要有准确的坐标值P。这里基于KINECT三维坐标值的点云数据通过读取每一个像素点的三维数据获得。

Pi中除了含有待定位物体的点云信息之外，还有很多不需要的点的信息。由于待定位物体只会聚类在一定范围之内，因此我们需要将不需要点的信息去除[10]。本文结合高清摄像头采集的图像与三维点云信息对比，考虑待检测物体对应点云密集的关系，分割点云数据、由于每个物体表面产生的点云数据距离很近。因此我们可以计算相邻点的距离，通过距离的大小值对点云进行聚类，从而得到只有待测物体的点云P。这些点云信息最终用于待定位物体的位置坐标计算。考虑到本文获得的点云是在物体表面均匀采样得到的，因此点云数据基于KINECT三维坐标值如下：

.

(10)

基于优化理论，为了减少计算误差，特征点的数目需要足够多。位置选取应具有代表性[11]。把基于KINECT的三维坐标通过式(11)经过仿射矩阵获得物体基于外部坐标系φ的坐标。

.

(11)

再结合第一维坐标，获得物体的四维坐标数据，达到精准定位的效果。

4 实验结果

这里截取了两张典型实验的效果图，多室多物体精准定位实验结果如图10所示，实测两物体室内的位置平均测量误差只有4.2 cm。

之后本文试验了其他3种室内物体定位系统：超宽带定位系统、超声波与空外线定位系统、基于蓝牙角度测量的定位系统对相同室内环境下的大物体瓶子与小物体玩偶进行定位。对比其他3种常见的室内定位方法，本文方法精度更高，效果更好。其定位结果对比如表1所示。

(a)Ⅰ室瓶子四维坐标定位(a) Four-dimensional coordinate location of bottle in room Ⅰ

(b)Ⅱ室玩偶四维坐标标定(b)Four-dimensional coordinate location of doll in room Ⅱ图10 定位实验结果图Fig.10 Location experiment result diagram

表1 定位效果对比Tab.1 Comparison of positioning results

由实验结果可以看到，本文提出的定位系统对室内物体定位更加精准，尤其是在更困难的小物体的定位精度上更高。

5 结 论

本文提出了一种融合基于ROS的SLAM算法和三维特征点云的物体精准定位研究系统，克服了ROS SLAM只能描述二维信息，三维点云只能描述物体独立的三维信息等缺点。通过实验结果来看，其定位精度比起常见的超声波与红外线定位系统提高6.7%，比基于蓝牙角度测量的定位系统定位精度提高20%，比超宽带定位系统提高72%，对室内物体的精准定位效果显著。