一种基于单线激光的室内实时定位与三维地图构建方法技术

本发明专利技术公开了一种基于单线激光的室内实时定位与三维地图构建方法，通过将单线激光测距仪固定到一个旋转装置上，克服了单线激光测距仪只能获取平面2D数据的缺陷，实现了对室内3D环境数据获取。对于所获取的3D扫描数据，本发明专利技术通过一个准确的结构地图构建算法以及一个鲁棒的扫描线匹配算法，实现了对激光扫描线的六自由度位姿估计，从而得到了传感器的运动轨迹，并结合激光扫描数据构建出了所扫描的环境的三维地图。本发明专利技术并实现了一种准确的室内实时定位与地图构建方法。

An indoor real-time positioning and 3D map building method based on single line laser

The invention discloses an indoor real-time positioning based on single line laser and 3D map building method, the single laser rangefinder is fixed to a rotating device, overcomes the defects of single laser rangefinder can obtain plane 2D data, the 3D data acquisition of indoor environment. For the 3D scan data obtained by the invention, an accurate structural map building algorithm and scan line of a robust matching algorithm, to achieve the six DOF of the laser scanning line estimation, thus obtained the motion sensor, and combined with the laser scanning data to construct a three-dimensional map of the scanned the environment. The invention and implementation of an accurate method of real time indoor location and map building.

【技术实现步骤摘要】
一种基于单线激光的室内实时定位与三维地图构建方法
本专利技术属于室内三维环境地图
，涉及一种基于单线激光的室内实时定位与三维地图构建方法，可用于室内环境中的实时定位服务并满足对室内三维环境地图的构建需求。
技术介绍
随着计算机技术、自动化技术、人工智能技术等技术的快速发展，以及人民生活水平的逐步提高，对室内服务机器人的需求正在不断地增长。区别于传统的工业机器人，服务机器人需要应对更加复杂多变的工作环境。因此自动化与智能化是目前室内服务机器人所必须的两项特性，自主智能的服务机器人可以有效地在复杂未知的环境中工作。为了实现自主智能的需求，机器人必须需要有能够感觉三维环境，并确定自己在环境中的位置的能力。传统的获取环境数据的传感器主要有视觉传感器以及激光传感器两种。视觉传感器具有较低的造价，并且使用起来较为方便，典型的有单目相机(如工业相机)，双目相机，深度相机(如RGB-D相机)等。但是，这类视觉传感器的视场角都普遍偏小，并且对环境光照的变化都十分敏感。而相比视觉传感器，激光传感器的视场角较大，并且对环境光照的变化不是十分敏感。此外，激光传感器具有较高的测距精度，有利于构建高精度的环境地图。激光传感器又可以细分为2D激光传感器与3D激光传感器。3D激光传感器可以直接获取环境的三维扫描数据，因此被广泛应用于室外环境中的导航技术。但是3D激光传感器的造价高昂，不适用于在室内环境中工作的服务机器人。相比之下，2D激光传感器的造价较低，也拥有与3D激光传感器相当的测距精度与视场角。然而，由于2D激光传感器只能获取平面二维扫描数据，因此往往只被用于估计传感器的平面运动轨迹以及构建室内平面地图，目前开源的hectorslam以及gmapping就是其中的典型算法。但是，对于室内服务机器人，仅仅确定其平面位姿以及构建平面环境地图是远远不够的。
技术实现思路
为了能够确定机器人的六自由度位姿，并构建室内三维环境地图，同时也为了降低传感器的成本，本专利技术提供了一种基于2D激光传感器的室内实时定位与三维环境地图构建方法。本专利技术所采用的技术方案是：一种基于单线激光的室内实时定位与三维地图构建方法，其特征在于，包括以下步骤：步骤1：获取二维扫描线(Scan)上的原始点云数据，在二维扫描线(Scan)中提取直线段，并将完整覆盖到一个三维空间的扫描线组成一个集合，称之为Sweep；步骤2：对步骤1中获得的直线段集合提取平面；步骤3：对于第一个Sweep，利用提取出的平面信息，进行主方向提取并抽稀；步骤4：构建初始结构地图；步骤5：单条Scan与结构地图匹配，提取需优化点对；步骤6：优化对应点间的距离d，计算Scan的位姿估计值；步骤7：迭代循环步骤5-步骤6，直到一个Sweep中的所有Scan优化结束，则将点云添加到结构地图中，进行结构地图生长；步骤8：循环迭代步骤5-步骤7，直至点云数据处理完毕。和现有的技术相比，本专利技术具有的有益成果在于：使用了一种较低成本的激光扫描策略，通过一个准确的结构地图构建算法以及一个鲁棒的扫描线匹配算法实现了对传感器的实时位姿估计以及对室内三维环境的准确构图。为室内智能服务机器人的发展提供了有效的技术手段。附图说明图1是本专利技术实施例的流程图；图2是本专利技术实施例中结构地图的构建结果示意图；图3是本专利技术实施例中三维场景地图与运动轨迹叠加的结果示意图。具体实施方法为了便于本领域普通技术人员理解和实施本专利技术，下面结合附图及实施例对本专利技术作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本专利技术，并不用于限定本专利技术。请见图1，本专利技术提供的一种基于单线激光的室内实时定位与三维地图构建方法，包括以下步骤：步骤1.1：从扫描线上的一个起始点开始，对其之后的10个扫描点(包含起始点)扫描点进行最小二乘直线拟合，并计算拟合误差；拟合误差的表达形式如下所示：其中，a,b是直线拟合参数，(xi,yi)是第i个扫描点的二维坐标,j为当前直线段在当前扫描线上的起始点的序号，N为预设值，本实施例取10；步骤1.2：当拟合误差εl＜Tl时，认为得到了一条初始直线段；否则，就跳过当前的起始点，从其下一个点开始继续重复步骤1.1的操作，直到找到了一条初始线段为止；其中Tl是直线拟合误差的阈值，可取50mm；步骤1.3：获得一条初始直线段之后，对该条初始直线段进行拓展；具体包括以下子步骤：步骤1.3.1：从当前初始直线段之后的一个点Pi开始，计算点Pi到当前直线段的距离d，其中i＞j+N，j表示当前直线段起始点在扫描线中的点序号，N可取10；如果d＜Td，则该点属于当前直线段，将点Pi加入到当前直线段中以实现对直线段的扩展，同时重新对该直线段进行拟合，更新直线段的拟合参数；否则，认为当前直线段已经不能继续拓展，直线段的拓展操作结束；其中Td为点到直线距离的最大阈值，可取50mm；步骤1.3.2：如果当前直线段被成功地拓展，则继续重复步骤1.3.1的操作，直到直线段不能继续拓展为止；步骤1.4：在完成一条直线段的拓展之后，将该条直线段加入到候选直线段集合中，并从当前直线段的下一个点开始重复步骤1.1-步骤1.3的操作，直到无法再从当前扫描线上提取出新的直线段为止；步骤1.5：对于当前扫描线上所有的候选直线段，对其进行合并以及删除的操作，以得到最终的直线段；具体包括以下子步骤：步骤1.5.1：从第i条候选直线段开始，判断与其相邻的第i+1条候选直线段是否能与其合并，判断条件如下：其中，vi表示第i条直线段的方向向量，θ(vi,vi+1)表示两条直线段方向向量的夹角，Tθ为阈值，可取1°，表示第i条候选直线段的最后一个点，表示第i+1条候选直线段的第一个点，表示两点之间的距离，Td为阈值，可取200mm；当两条相邻候选直线段的方向向量的夹角小于阈值且首尾点之间的距离也小于阈值时，将这两条候选直线段合并为一条，并重新计算合并后的直线段的拟合参数；步骤1.5.2：重复1.5.1的操作，直到没有可以再合并的候选直线段为止；计算合并后的直线段的长度，剔除长度小于100mm的直线段，将剩下的直线段作为最终提取得到的当前扫描线上的直线段，并按顺序存储。步骤2，对步骤1中获得的线段集合提取平面。具体步骤如下：步骤2.1：通过扫描线位姿值，将对应扫描线上的二维直线段转换为三维直线段；如果该扫描线是静止采集的，则它的位姿可由扫描设备提供的电机旋转角度直接得到。如果不是静止采集的，则可以通过步骤5-步骤6中的方法得到扫描线的位姿估计值。当对一个Sweep内的所有扫描线完成三维直线段的提取操作后，可获得一组分布在整个扫描环境中的三维直线段集合，记为其中，表示第i条扫描线上的第j条直线段；步骤2.2：任选L中的一条直线段在所属的扫描线Si的相邻扫描线Si+1,Si-1上，寻找与共面的直线段；平面方程描述为：x·n＝d,其中，x为平面上的点，n为平面的法向量，d是一个常数；将与和它相邻扫描线上的直线段根据上述的平面方程进行最小二乘平面拟合，如果拟合误差小于预设值，那么认为找到了一个初始平面Pl。否则，换一条直线段，重复步骤2.2的操作，直到找到一个初始平面为止；步骤2.3：在Pl周围搜索属于Pl的直线段，对于Pl周围的一条直线段l，如果其满足以下条本文档来自技高网...

【技术保护点】
一种基于单线激光的室内实时定位与三维地图构建方法，其特征在于，包括以下步骤：步骤1：获取二维扫描线Scan上的原始点云数据，在二维扫描线Scan中提取直线段，并将完整覆盖到一个三维空间的扫描线组成一个集合，记为Sweep；步骤2：对步骤1中获得的直线段集合提取平面；步骤3：对于第一个Sweep，利用提取出的平面信息，进行主方向提取并抽稀；步骤4：构建初始结构地图；步骤5：单条Scan与结构地图匹配，提取需优化点对；步骤6：优化对应点间的距离d，计算Scan的位姿估计值；步骤7：迭代循环步骤5‑步骤6，直到一个Sweep中的所有Scan优化结束，则将点云添加到结构地图中，进行结构地图生长；步骤8：循环迭代步骤5‑步骤7，直至点云数据处理完毕。

【技术特征摘要】
1.一种基于单线激光的室内实时定位与三维地图构建方法，其特征在于，包括以下步骤：步骤1：获取二维扫描线Scan上的原始点云数据，在二维扫描线Scan中提取直线段，并将完整覆盖到一个三维空间的扫描线组成一个集合，记为Sweep；步骤2：对步骤1中获得的直线段集合提取平面；步骤3：对于第一个Sweep，利用提取出的平面信息，进行主方向提取并抽稀；步骤4：构建初始结构地图；步骤5：单条Scan与结构地图匹配，提取需优化点对；步骤6：优化对应点间的距离d，计算Scan的位姿估计值；步骤7：迭代循环步骤5-步骤6，直到一个Sweep中的所有Scan优化结束，则将点云添加到结构地图中，进行结构地图生长；步骤8：循环迭代步骤5-步骤7，直至点云数据处理完毕。2.根据权利要求1所述的基于单线激光的室内实时定位与三维地图构建方法，其特征在于，步骤1中所述在二维扫描线Scan中提取直线段，具体实现包括以下子步骤：步骤1.1：从扫描线上的一个起始点开始，对其之后的N个扫描点进行最小二乘直线拟合，并计算拟合误差；拟合误差的表达形式如下所示：其中，a,b是直线拟合参数，(xi,yi)是第i个扫描点的二维坐标,j为当前直线段在当前扫描线上的起始点的序号，N为预设值；步骤1.2：当拟合误差εl＜Tl时，认为得到了一条初始直线段；否则，就跳过当前的起始点，从其下一个点开始继续重复步骤1.1的操作，直到找到了一条初始线段为止；其中Tl是直线拟合误差的阈值；步骤1.3：获得一条初始直线段之后，对该条初始直线段进行拓展；具体包括以下子步骤：步骤1.3.1：从当前初始直线段之后的一个点Pi开始，计算点Pi到当前直线段的距离d，其中i＞j+N，j表示当前直线段起始点在扫描线中的点序号；如果d＜Td，则该点属于当前直线段，将点Pi加入到当前直线段中以实现对直线段的扩展，同时重新对该直线段进行拟合，更新直线段的拟合参数；否则，认为当前直线段已经不能继续拓展，直线段的拓展操作结束；其中Td为点到直线距离的最大阈值；步骤1.3.2：如果当前直线段被成功地拓展，则继续重复步骤1.3.1的操作，直到直线段不能继续拓展为止；步骤1.4：在完成一条直线段的拓展之后，将该条直线段加入到候选直线段集合中，并从当前直线段的下一个点开始重复步骤1.1-步骤1.3的操作，直到无法再从当前扫描线上提取出新的直线段为止；步骤1.5：对于当前扫描线上所有的候选直线段，对其进行合并以及删除的操作，以得到最终的直线段；具体包括以下子步骤：步骤1.5.1：从第i条候选直线段开始，判断与其相邻的第i+1条候选直线段是否能与其合并，判断条件如下：其中，vi表示第i条直线段的方向向量，θ(vi,vi+1)表示两条直线段方向向量的夹角，Tθ为阈值，表示第i条候选直线段的最后一个点，表示第i+1条候选直线段的第一个点，表示两点之间的距离，Td为阈值；当两条相邻候选直线段的方向向量的夹角小于阈值且首尾点之间的距离也小于阈值时，将这两条候选直线段合并为一条，并重新计算合并后的直线段的拟合参数；步骤1.5.2：重复1.5.1的操作，直到没有可以再合并的候选直线段为止；计算合并后的直线段的长度，剔除长度小于预设阈值的直线段，将剩下的直线段作为最终提取得到的当前扫描线上的直线段，并按顺序存储。3.根据权利要求1所述的基于单线激光的室内实时定位与三维地图构建方法，其特征在于，步骤2的具体实现包括以下子步骤：步骤2.1：通过扫描线位姿值，将对应扫描线上的二维直线段转换为三维直线段；判断该扫描线是否是静止采集的；如果该扫描线是静止采集的，则它的位姿由扫描设备提供的电机旋转角度直接得到；如果不是静止采集的，则通过步骤5-步骤6中方法得到扫描线的位姿估计值；当对一个Sweep内的所有扫描线完成三维直线段的提取操作后，可获得一组分布在整个扫描环境中的三维直线段集合，记为其中，表示第i条扫描线上的第j条直线段；步骤2.2：任选L中的一条直线段在所属的扫描线Si的相邻扫描线Si+1,Si-1上，寻找与共面的直线段；平面方程描述为：x·n＝d,其中，x为平面上的点，n为平面的法向量，d是一个常数；将与和它相邻扫描线上的直线段根据上述的平面方程进行最小二乘平面拟合，如果拟合误差小于预设值，则认为找到了一个初始平面Pl；否则，换一条直线段，重复步骤2.2的操作，直到找到一个初始平面为止；步骤2.3：在Pl周围搜索属于Pl的直线段，对于Pl周围的一条直线段l，如果其满足以下条件时，则认为l属于平面Pl：其中，xi为直线段l上的点；将直线段l加入到平面Pl中，并重新计算平面Pl的拟合参数；步骤2.4：重复步骤2.3的操作，不断拓展平面Pl，直到在平面Pl的周围不再有能够合并到平面Pl中的直线段为止，那么此刻就得到了一个完整地平面Pl；步骤2.5：对于中剩下的直线段重复步骤2.2-步骤2.4的操作，直到不再有新的平面可以被提取到为止。4.根据权利要求1所述的基于单线激光的室内实时定位与三维地图构建方法，其特征在于，步骤3的具体实现包括以下子步骤：步骤3.1：初始化三个正交主方向D＝{d1,d2,d3}＝{{1,0,0},{0,1,0},{0,0,1}}...

【专利技术属性】
技术研发人员：姚剑，谈彬，罗磊，李礼，
申请(专利权)人：武汉大学，
类型：发明
国别省市：湖北,42