python 点云处理稀疏_基于VSLAM的3D稀疏点云到2D栅格图的实现方法与流程

本发明属于计算机视觉和移动机器人导航领域，涉及一种基于视觉slam生成的3d稀疏点云到2d栅格图的实现方法。

背景技术：

：同时定位与地图构建(slam)是机器人自主导航的一个重要研究领域。它是机器人搭载特定传感器，在没有环境先验信息情况下移动，构建环境地图，同时估计自己运动的过程。slam技术在室内移动机器人、自动驾驶、增强现实(ar)等领域应用明显。室内移动机器人的出现，为物流运输、仓储盘点、港口运输、室内救援、以及商超服务机器人等行业注入了新动力。目前市场上比较成熟的同时定位与地图构建方案是基于lidar的激光slam，比如比较受大众深知的科沃斯扫地机器人，图森未来、百度等的自动驾驶汽车。然而，多线激光价格昂贵；单线激光雷达探测距离受限，且缺乏语义信息。而对于大多数市场，需要性价比高而又稳定的产品。技术实现要素：本发明针对室内移动机器人定位和导航需要二维地图、使用激光雷达成本高等问题，提出了一种基于视觉slam生成的3d稀疏点云转换为2d栅格导航图的方法。本发明一种基于vslam的3d稀疏点云到2d栅格图的实现方法，包含以下步骤：步骤1、稀疏点云生成通过视觉slam算法生成稀疏3d点云图。在机器人上安装双目摄像头，获取室内环境信息；通过kalibr标定方法校准图像，实现图像的去畸变操作；然后提取图像的harris角点，利用光流法跟踪相邻帧；通过sfm进行纯视觉估计滑窗内所有帧的位姿及3d点逆深度，求解初始化参数；初始化成功后进行后端滑窗优化求解滑窗内所有帧的pvq、bias；进行闭环检测，当检测成功后进行重定位，并对整个相机轨迹进行全局图优化。最后保存生成的3d点云图。步骤2、点云数据优化首先加载步骤1中保存的3d点云图，然后对点云进行基于颜色的视觉分割，将其划分为平面区域和高度区域，平面区域用于2d栅格图生成，高度区域用于障碍物确立。步骤3、获取图像关键帧和地图点修改slam程序的启动文件，将每个关键帧添加到地图中。通过ros包创建两个节点，mynteyepub检测何时执行循环闭包，同时发布每个关键帧的3d姿态以及每个关键帧中所有可见地图点，供mynteyesub订阅。步骤4、地图映射当接收到所有关键帧后，移除y坐标，将相机位姿和所有可见地图点投影到xz平面上，存入字典结构中。每次处理一个关键帧，对每个关键帧中可见的所有地图点，使用bresenham的直线绘制算法将射线从相机位置投射到所有可见点；为射线上的每个点增加一个访问计数器，并为对应于映射点位置的端点增加一个占用计数器。访问和占用计数器存储与xz映射平面同比例的二维矩阵，矩阵每一行列的数值代表单元格位置。计算完所有关键帧后，栅格图中的每个单元格的未被占用概率计算公式：其中，occupied(i，j)和visit(i，j)分别代表某一空闲单元格被占用和访问的次数，pfree(i，j)代表该单元格未被占用的概率。通过设定两个阈值将单元格分为三种情况：(1)如果pfree(i，j)大于某一空闲阈值free_threshold,则视为空闲单元；(2)如果pfree(i，j)小于某一占用阈值occupied_threshold,则视为占用单元；(3)否则，视为未知单元；步骤5、优化映射单元在步骤4的3d到2d地图映射过程中，采用全局计数器计数接收到的所有关键帧。然而，在实际映射过程中，会出现多个点在2d图中共线的情况；而bresenham的直线绘制算法会递增访问计数器计数次数，降低该线上所有点的占用率，使得已经被占用的单元格被替换为空闲单元格，产生误判。基于此，本发明采用局部计数器和全局计数器结合的方式，针对局部特征点数大于设定阈值的地方，使用局部计数器接收关键帧，然后通过与运算，与全局计数器计数的所有关键帧求和，实现计数优化，提高了映射质量。步骤6、障碍物检测采用坡度阈值实现了障碍物检测和映射。为更好的区分障碍物，使用两个不同的阈值：对视觉分割后的平面区域，采用15°阈值；对于高度区域，采用80°阈值。为消除虚假障碍，在检测算法中附加两个约束条件：(1)只考虑距离映射平面一定高度内的点；(2)对估计点所在平面的斜率进行阈值处理，对每个点施加垂直度约束。在包含该点的垂直线上，所有像素的数量需大于设定的阈值。坡度阈值可能造成错误的消除真实障碍，而被错误消除的障碍大多位于空闲单元格和未知单元格之间的边界。基于此，采用canny边界检测算法找寻位于空闲单元格和未知单元格之间的边界，并设定某一固定阈值，然后将概率图中检测到的这些边缘对应的像素全部设置为已占用；得到用于导航和避障的2d栅格图。通过这两种阈值，能较好的识别出障碍物，提高导航地图精度。步骤7、自主导航：通过以上步骤，我们得到了可用于导航和避障的2d栅格图。本发明在完成视觉slam建图的基础上进行导航，采用dijkstra的全局规划算法和dwa的局部路径规划算法，完成机器人的室内自主运动。其中，(1)全局路径规划为按照机器人预先记录的环境地图并结合机器人当前位姿以及任务目标点的位置，在地图上找到前往目标点最快捷的路径；(2)局部路径规划为驶向终点过程中，实时根据环境在原来全局规划的基础上进行调整。本发明相当于现有技术的有益效果为：1、目前，市场上较为流行的室内定位方式有wi-fi、蓝牙、rfid、uwb、红外技术、超声波等，然而这些定位方式存在很多问题，比如定位精度低、易受障碍物影响、运动时抖动较大等。本发明采用视觉slam实现室内精准定位，克服了媒介等影响。2、提出了视觉slam的定位方案，克服了激光slam定位方式中lidar造价昂贵、地图缺乏语义信息等问题。3、目前市面上有3d稠密点云转2d栅格图的案例，但稠密点云的生成对平台要求高，通常需要结合cuda加速图像处理。本方案对稀疏点云进行转换，解决了稠密点云生成过程中的高能耗、高延时问题，提高了运行效率。4、采用基于颜色的视觉分割算法，并采用全局计数器结合局部计数器方式，实现更精准快捷的地图点映射。同时通过阈值算法实现障碍物检测，最终获得语义地图。附图说明图1为系统整体框架；图2为滑窗优化示意图；图3为3d稀疏点云到2d栅格地图转换流程图；图4为bresenham直线绘制算法流程图。具体实施方式下面结合附图和具体实施方式对本发明作进一步的说明。表1为系统平台参数。用于实施的硬件环境是jetsontx2，本发明在linux系统下进行，实验手段包括数据集测试和实地测试，实地测试使用mynteye进行数据采集，在实验楼道以及楼顶阳台完成不同场景不同光照条件的性能评估。参数实施条件系统硬件平台jetsontx2视觉传感器mynteyes1030运行环境ubuntu16.04编程语言c++、python测试环境实验楼(30*20m2)表1如图1所示，本发明基于视觉slam生成的3d稀疏点云到2d栅格图的实现方法，包括3d点云图生成，2d栅格图转换，路径规划与自主导航三部分。以下对这三部分中核心步骤进行详细说明。1、3d点云图生成：a)为获得较好的鲁棒性，我们采用视觉和惯导融合的slam算法，机器人搭载mynteye相机，该相机内置六轴imu，并实现相机与imu的硬件同步。先通过联合标定工具kalibr离线标定相机和imu，同步时间戳，然后再用mynteye相机采集环境信息。b)目前市面上常用的视觉里程计方法包括特征点法、直接法和光流法，本发明采用光流法估计像素在不同时刻图像中的运动。后端使用滑窗法求解位置、速度、旋转、bias等，图2所示为滑窗优化示意图。c)最后将相机获取的位姿点通过slam算法转换为点云信息，利用投影和正交定理将点云生成深度图像，并通过pcl保存点云数据，供后面的地图转换。2、3d点云图到2d栅格图的转换：a)如图3所示为3d点云到2d栅格地图转换流程。3d点云图转2d栅格图的方法有很多，常见的有使用ros提供的octomap完成栅格地图的生成与转换，而octomap希望点云数据尽可能的多，对于稀疏地图的转换，地图点间的关联度小，容易造成误匹配，地图转换精度低，因此更适用于准稠密和稠密地图的转换。另一种比较常用的方法是使用pointcloud_to_laserscan的ros包转换点云数据到模拟lidar扫描，然后使用结果数据作为gmapping算法的输入，从lidar扫描生成2d栅格图。然而这种方法可能会在地图中引入额外的不确定性，因为映射算法总是假设lidar数据有噪声，而视觉生成的点云只包含可靠点。本发明通过ros创建发布稀疏点云图的关键帧姿态和所有地图点，通过视觉分割算法和bresenham算法将点云数据映射到栅格图中的空闲单元格，并经过斜率阈值和高斯平滑等算法的优化处理，最后生成可用于导航和避障的2d栅格图。b)在点云位姿和地图点的映射过程中，采用bresenham的直线绘制算法将射线从相机位置投射到所有可见点。图4为bresenham算法流程图，bresenham直线算法用于描绘由两点所决定的直线，计算出一条线段在n维光栅上最接近的点。c)对于生成的2d栅格图，本发明中采集实验楼道内真实路况图，并通过homography矩阵对齐两地图，然后使用准确度测量和完整性评分进行精度评估。完整性＝地图中已知单元格数量/地图中单元格总数准确度＝地图中已知正确的单元格数量/地图中已知单元格总数其中，精确度测量表明生成的地图与实际路况图的相似度；完整性评分表示该算法生成真实地图的概率，无论地图是否正确。3、路径规划：路径规划在机器人的自主无碰运动，物流管理的车辆资源配置等领域具有广泛应用。市场上常用的路径规划算法有：dijkstra算法、a*搜索算法、模拟退火算法、蚁群算法、遗传算法、粒子群算法、floyd算法、fallback算法等。根据对环境信息的把握程度可把路径规划划分为基于先验信息的全局路径规划和基于传感器信息的局部路径规划。全局规划采用dijkstra算法,用于计算一个节点到其它节点的最短路径，遵循广度优先搜索思想，即以起始点为中心向外层层扩展，直到扩展到最终点为止；局部路径规划采用dwa算法，其原理是在速度空间(v,w)中采样多组速度，并模拟所有速度在一定时间内的运动轨迹，最后通过评价函数对所有轨迹打分，选择最优速度回传给机器人，实现局部最优路径规划。以上所述仅是本发明的优选实施方式，应当指出，对于本

技术领域：

的普通技术人员，在不脱离本发明构思的前提下，还可以做出若干改进和润色，这些改进和润色也应视为本发明保护范围内。当前第1页1 2 3