CN110243375A - 一种同时构建二维地图和三维地图的方法 - Google Patents

Abstract

本发明实施例公开了一种同时构建二维地图和三维地图的方法，通过一安装有车轮编码器、激光扫描仪和Kinect传感器的移动机器人来实施，包括：获得激光扫描仪的输出数据，构建室内环境的二维地图；该移动机器人在构建二维地图过程中，获得该移动机器人的位姿及位姿变换矩阵；在该移动机器人构建二维地图的同时，利用该Kinect传感器获取室内环境的二维图像，将二维图像转换成三维点云图像；利用机器人的位姿及位姿变换矩阵替换Kinect位姿及位姿变换矩阵；根据机器人的位姿及位姿变换矩阵，完成相邻点云图像拼接，同时构建出室内环境的二维地图和三维地图。

Description

一种同时构建二维地图和三维地图的方法

技术领域

本发明涉及室内环境地图构建领域，尤其是涉及一种基于二维地图下融合Rao－Blackw ellised粒子滤波器和特征点法构建三维地图的方法。

背景技术

随着计算机技术，卫星定位系统以及互联网技术的飞速发展，基于室内环境构图以及定位技术在现实生活中得到了大量应用。当前，室内环境的地图构建问题已经变成了一个热门领域，虽然基于激光雷达构建二维地图的方法已经相对成熟和完整，但却无法获取物体的真实三维形状，只能得到一个平面，使得定位效果不佳。

目前，在室内三维地图构建方法中比较流行的是利用特征点来估计相机运动(特征点法)和根据图像的像素灰度信息来计算相机运动(直接法)，但是以上方法都会受到光照、亮度，以及环境中重复区域的影响，出现错误的图像匹配，耗时且准确性较差。

发明内容

本发明实施例所要解决的技术问题在于，提供一种同时构建二维地图和三维地图的方法。该方法可解决移动机器人在室内导航过程中定位精度不高，误差大等问题。

为了解决上述技术问题，本发明实施例提供了一种同时构建二维地图和三维地图的方法，该方法以具有车轮编码器、激光扫描仪和Kinect传感器的移动机器人为载体实施，包括以下步骤：

S1、获得激光扫描仪的输出数据，构建室内环境的二维地图；

S2、该移动机器人在构建二维地图过程中，获得该移动机器人的位姿及位姿变换矩阵；

S3、在该移动机器人构建二维地图的同时，利用该Kinect传感器获取室内环境的二维图像，将二维图像转换成三维点云图像；

S4、利用机器人的位姿及位姿变换矩阵替换Kinect位姿及位姿变换矩阵；

S5、根据机器人的位姿及位姿变换矩阵，完成相邻点云图像拼接，同时构建出室内环境的二维地图和三维地图。

进一步地，所述步骤S2具体包括：

S21、在该移动机器人构建二维地图过程中，通过Rao－B lackw ellised粒子滤波器方法，获取该移动机器人在不同时刻的位姿；

S22、根据李群、李代数和罗德里格斯公式，计算获得该移动机器人的位姿及位姿变换矩阵。

更进一步地，所述步骤S3具体包括：

S31、利用该Kinect传感器实时获得室内环境的二维图像，该二维图像包括彩色图像和深度图像，根据该Kinect传感器的内参矩阵和ORB算法计算该二维图像的特征点，完成相邻两个时刻的二维图像的关键点提取和得到特征描述子，然后计算该相邻两个时刻的二维图像间的匹配关系；

S32、根据步骤S31中所得的匹配关系，计算出该Kinect传感器的旋转矩阵和平移向量；

S33、根据所述步骤S22，计算出该Kinect传感器的位姿及位姿变换矩阵。

更进一步地，所述步骤S4具体包括：

S41、根据所述步骤S21、S22和S33，，将该移动机器人的位姿及位姿变换替换Kinect传感器的位姿及位姿变换矩阵。

更进一步地，所述步骤S5具体包括：

S51、根据所述步骤S31，利用该Kinect传感器的内参矩阵对环境的二维图像完成二维图像到三维点云图像的变换；

S52、通过VoxelGrid滤波器对步骤S51所得三维点云图像进行降采样滤波处理；

S53、利用所得该移动机器人的位姿及位姿变换，完成所得相邻三维点云图像的拼接；

S54、构建二维地图结束时，完成对各帧三维点云图像的拼接，实现室内环境三维场景的构建。

更进一步地，所述激光扫描仪的扫描范围为360°，最大测量距离为6米，Kinect传感器的水平和垂直观测角度分别是57.5°和43.5°，工作距离为0.8－4米。

实施本发明实施例，具有如下有益效果：本发明的工作只需一个移动机器人就可以完成，所用的实验设备成本低，环境适用性强，本发明利用激光扫描仪的输出数据获得室内环境的二维地图，通过Kinect传感器获得室内环境的二维图像，以及移动机器人的位姿变换矩阵来完成三维点云图像的拼接，构建出精确的室内环境的三维地图。在该移动机器人构建二维地图过程中，通过Rao－B lackw ellised粒子滤波器方法，获取该移动机器人在不同时刻的位姿；将该移动机器人的位姿及位姿变换矩阵替换Kinect传感器的位姿及位姿变换矩阵进行，提高了移动机器人运动中的定位精度，避免出现图像拼接的错误匹配，克服了在重复区域中图像匹配混乱的现象。

附图说明

图1是本发明构建三维地图的方法的流程图；

图2是移动机器人在建筑物内得到的楼层的二维地图；

图3是基于二维地图构建的室内环境三维地图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述。

本发明实施例的实验环境为某实验楼的走廊，办公室和实验室的室内环境。在本实施例中，移动机器人探测楼层的公开区域。

本实施例中使用TurtleBot移动机器人，在该移动机器人上安装有车轮编码器，激光扫描仪和Kinect传感器，在本实施例中，该激光扫描仪为Rplidar激光雷达，该Rplidar激光雷达的扫描范围为360°，最大测量距离为6米。所采用的实施系统为Ubuntu系统，并搭建ROS系统平台和Arduino开发板，采用Opencv开源软件，利用C++来实现的。为了实现本发明的实时性和稳定性，利用游戏手柄控制移动机器人来构建室内环境的二维地图。

可以理解的，在其他实施例中，该激光扫描仪除Rplidar激光雷达外，也可以为sick或声呐传感器中的一种。

请参阅图1，本发明实施例提供的一种同时构建二维地图和三维地图的方法，包括如下步骤：

S1、获得该激光扫描仪的输出数据，构建室内环境的二维地图；

二维地图构建过程：该移动机器人在移动过程中，为了能实现实时定位与地图构建，在本实施例中采用了基于Rao－B lackw ellized粒子滤波器的FastSLAM方法，SLAM(Simultaneous Localization And Mapping，同步定位与地图创建)是指移动机器人在未知环境中构建地图，并同时使用该地图进行定位与导航。本发明将SLAM问题，分解为移动机器人姿态和路标在地图的位置两个递归算法。该移动机器人采用激光扫描仪获得数据并结合粒子滤波来估计该移动机器人的路径位姿；FastSLAM方法使用EKF(Extended Kalm anFilter，扩展卡尔曼滤波)来进行路标评估，估计环境特征位置。

EKF使用运动模型和测量模型来获得准确的位置关系。运动模型基于上一时刻的状态和控制系统，去估计下一时刻位置状态；测量模型根据传感器测量获得数据以及运动模型得到的状态，两个数据更新获得移动机器人准确的位置。从而根据路径位姿和环境特征，构建出室内环境二维地图并实时定位。利用上述方法步骤，得到室内环境的二维地图见图3所示。

S2、该移动机器人在构建二维地图过程中，获得该移动机器人的位姿及位姿变换矩阵；

该移动机器人利用上构建二维地图过程中(从走廊的一端到另一端)，并在在运动过程中当移动机器人移动到不同的位置点(位置1、位置2、位置3、….、位置N)使用Rao－Blackw ellised粒子滤波器来估计并获取该移动机器人在不同时刻不同位置的位姿。根据不同时刻的位姿，得到该移动机器人的旋转和平移矩阵，根据李群、李代数和罗德里格斯公式获取不同时刻的机器人的位姿及位姿变换矩阵，用于点云地图的拼接。

S3、在该移动机器人构建二维地图的同时，利用该Kinect传感器获取室内环境的二维图像，将二维图像转换成三维点云图像；

Kinect传感器获取的室内环境的二维图像，彩色图像和深度图像；利用该Kinect传感器的内参矩阵和ORB算法计算出该二维图像的特征点，完成相邻两幅二维图像的特征点提取和描述子的计算，同时根据描述子和特征点可以得出图像之间的匹配关系；根据图像匹配关系，利用Opencv库函数求解出Kinect传感器的旋转矩阵和平移向量；其次根据李群、李代数和罗德里格斯公式，计算获得该移动机器人的位姿变换矩阵，并获得Kinect传感器的位姿及位姿变换矩阵。

S4、利用利用机器人的位姿及位姿变换矩阵替换Kinect位姿及位姿变换矩阵；

根据上述步骤S21、S22和S33，利用移动机器人的位姿以及位姿变换矩阵替换Kinect传感器的位姿以及位姿变换矩阵。

S5、根据机器人的位姿及位姿变换矩阵，完成相邻点云图像拼接，同时构建出室内环境的二维地图和三维地图。

根据所述步骤S32，利用Kinect传感器的内参矩阵将所得室内环境的二维图像图像变换为三维点云图像；利用VoxelGrid滤波器对构建的各帧的三维点云图像进行降采样滤波处理，减少冗余的点云以及减少图像所占的空间内存；利用所得该移动机器人的位姿及位姿变换，完成所得相邻三维点云图像的拼接；见图3，最终得到实验环境中室内环境的全局三维点云地图。

上述本发明的基于二维地图下融合Rao－B lackw ellised粒子滤波器和特征点法构建三维地图的方法，利用二维地图，采用Rao－B lackw ellised粒子滤波器，提高了移动机器人运动中的定位精度，避免出现图像拼接的错误匹配，克服了在重复区域中图像匹配混乱的现象；利用该Kinect传感器实时获得室内环境的二维图像，根据该Kinect传感器的内参矩阵和ORB算法计算该二维图像的特征点，完成相邻两个时刻的二维图像的关键点提取和得到特征描述子，然后计算该相邻两个时刻二维图像间的匹配关系；计算出该Kinect传感器的旋转矩阵和平移向量；根据李群、李代数和罗德里格斯公式，获得该Kinect传感器的位姿及位姿变换矩阵；然后将该移动机器人的位姿及位姿变换替换Kinect传感器的位姿及位姿变换矩阵；利用该Kinect传感器的内参矩阵对环境的二维图像完成二维图像到三维点云图像的变换；通过VoxelGrid滤波器对所得三维点云图像进行降采样滤波；利用所得该移动机器人的位姿及位姿变换，完成所得相邻三维点云图像的拼接；构建二维地图结束时，完成对各帧三维点云图像的拼接，从而实现室内环境的三维场景构建。

以上该实施例仅表达了本发明的优选实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (6)

1.一种同时构建二维地图和三维地图的方法，其特征在于，所述方法以具有车轮编码器、激光扫描仪和Kinect传感器的移动机器人为载体实施，具体包括以下步骤：

S1、获得该激光扫描仪的输出数据，构建室内环境的二维地图；

S2、所述移动机器人在构建二维地图过程中，获得所述移动机器人的位姿及位姿变换矩阵；

S3、在所述移动机器人构建二维地图的同时，利用所述Kinect传感器获取室内环境的二维图像，将二维图像转换成三维点云图像；

S4、利用所述移动机器人的位姿及位姿变换矩阵替换Kinect位姿及位姿变换矩阵；

S5、根据所述移动机器人的位姿及位姿变换矩阵，完成相邻点云图像拼接，同时构建出室内环境的二维地图和三维地图。

2.根据权利要求1所述的同时构建二维地图和三维地图的方法，其特征在于，所述步骤S2具体包括：

S21、在所述移动机器人构建二维地图过程中，通过Rao－Blackwellised粒子滤波器方法，获取所述移动机器人在不同时刻的位姿；

S22、根据李群、李代数和罗德里格斯公式，计算获得所述移动机器人的位姿及位姿变换矩阵。

3.根据权利要求2所述的同时构建二维地图和三维地图的方法，其特征在于，所述步骤S3具体包括：

S31、利用所述Kinect传感器实时获得室内环境的二维图像，该二维图像包括彩色图像和深度图像，根据所述Kinect传感器的内参矩阵和ORB算法计算该二维图像的特征点，完成相邻两个时刻的二维图像的关键点提取和得到特征描述子，然后计算所述相邻两个时刻的二维图像间的匹配关系；

S32、根据步骤S31中所得的匹配关系，计算出所述Kinect传感器的旋转矩阵和平移向量；

S33、根据所述步骤S22，计算出所述Kinect传感器的位姿及位姿变换矩阵。

4.根据权利要求3所述的同时构建二维地图和三维地图的方法，其特征在于，所述步骤S4具体包括：

S41、根据所述步骤S21、S22和S33，，将所述移动机器人的位姿及位姿变换替换Kinect传感器的位姿及位姿变换矩阵。

5.根据权利要求4所述的同时构建二维地图和三维地图的方法，其特征在于，所述步骤S5具体包括：

S51、根据所述步骤S31，利用所述Kinect传感器的内参矩阵对环境的二维图像完成二维图像到三维点云图像的变换；

S52、通过VoxelGrid滤波器对步骤S51所得三维点云图像进行降采样滤波处理；

S53、利用所得所述移动机器人的位姿及位姿变换，完成所得相邻三维点云图像的拼接；

S54、构建二维地图结束时，完成对各帧三维点云图像的拼接，实现室内环境三维场景的构建。

6.根据权利要求1所述的同时构建二维地图和三维地图的方法，其特征在于，所述激光扫描仪的扫描范围为360°，最大测量距离为6米；所述Kinect传感器的水平和垂直观测角度分别为57.5°和43.5°，工作距离为0.8－4米。