本发明属于室内三维环境地图技术领域，涉及一种基于单线激光的室内实时定位与三维地图构建方法，可用于室内环境中的实时定位服务并满足对室内三维环境地图的构建需求。
背景技术：
随着计算机技术、自动化技术、人工智能技术等技术的快速发展，以及人民生活水平的逐步提高，对室内服务机器人的需求正在不断地增长。区别于传统的工业机器人，服务机器人需要应对更加复杂多变的工作环境。因此自动化与智能化是目前室内服务机器人所必须的两项特性，自主智能的服务机器人可以有效地在复杂未知的环境中工作。为了实现自主智能的需求，机器人必须需要有能够感觉三维环境，并确定自己在环境中的位置的能力。传统的获取环境数据的传感器主要有视觉传感器以及激光传感器两种。视觉传感器具有较低的造价，并且使用起来较为方便，典型的有单目相机(如工业相机)，双目相机，深度相机(如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°，
当两条相邻候选直线段的方向向量的夹角小于阈值且首尾点之间的距离也小于阈值时，将这两条候选直线段合并为一条，并重新计算合并后的直线段的拟合参数；
步骤1.5.2：重复1.5.1的操作，直到没有可以再合并的候选直线段为止；计算合并后的直线段的长度，剔除长度小于100mm的直线段，将剩下的直线段作为最终提取得到的当前扫描线上的直线段，并按顺序存储。
步骤2，对步骤1中获得的线段集合提取平面。具体步骤如下：
步骤2.1：通过扫描线位姿值，将对应扫描线上的二维直线段转换为三维直线段；如果该扫描线是静止采集的，则它的位姿可由扫描设备提供的电机旋转角度直接得到。如果不是静止采集的，则可以通过步骤5-步骤6中的方法得到扫描线的位姿估计值。当对一个sweep内的所有扫描线完成三维直线段的提取操作后，可获得一组分布在整个扫描环境中的三维直线段集合，记为
步骤2.2：任选l中的一条直线段
平面方程描述为：
x·n＝d,
其中，x为平面上的点，n为平面的法向量，d是一个常数；将
步骤2.3：在pl周围搜索属于pl的直线段，对于pl周围的一条直线段l，如果其满足以下条件时，则认为l属于平面pl：
其中，xi为直线段l上的点；
将直线段l加入到平面pl中，并重新计算平面pl的拟合参数；
步骤2.4：重复步骤2.3的操作，不断拓展平面pl，直到在平面pl的周围不再有能够合并到平面pl中的直线段为止，那么此刻就得到了一个完整地平面pl；
步骤2.5：对于
步骤3，对于第一个sweep，我们利用提取出的平面信息，进行主方向提取并抽稀。主要步骤包括：
步骤3.1：初始化三个正交主方向d＝{d1,d2,d3}＝{{1,0,0},{0,1,0},{0,0,1}}，计算每个平面的法向量np，并单位化；
步骤3.2：将每个平面与三个主方向分别进行叉乘，ni＝np×di，选取ni最小时所对应的主方向作为与该面匹配的主方向dp；
步骤3.3：利用匹配好的法向量对，借助ceres进行主方向优化，进而提取最优主方向；
所述借助ceres进行主方向优化，具体包括以下子步骤：
步骤3.3.1：将d＝{d1,d2,d3}的变换矩阵r＝{rotatex,rotatey,rotatez}作为ceres的优化项；
步骤3.3.2：对每一平面计算匹配后的向量对的叉乘，s＝np×dp，||s||作为ceres优化的残差项；
步骤3.3.3：通过不断迭代优化r，获取优化误差小于预设值时对应的主方向的旋转矩阵，将此时的r变换为3*3旋转矩阵r,进而优化后的主方向
步骤3.4：对结构地图点云进行均匀抽稀。
步骤4，构建初始结构地图。基本步骤如下：
步骤4.1：对于每个平面，确定与该面最近的主方向，并计算两向量夹角：
如果|θ|≤预设阈值，可取15°，则进行平面方向优化；
所述进行平面方向优化，具体包括以下子步骤：
步骤4.1.1：将点云c平移到以重心为原点坐标系中，构成点云cg；
步骤4.1.2：利用旋转轴s＝np×dp和旋转角θ求旋转矩阵r；
步骤4.1.3：将点云cg旋转变换为c'g＝rcg；
步骤4.1.4：将点云c'g变换回世界坐标系；
步骤4.2：判断结构地图是否已经被初始化。如果尚未初始化平面地图，则将优化后的平面直接加入到结构地图中，以完成对结构地图的初始化。如果结构地图已经被初始化了，那么判断在结构地图中有无与优化过后的平面相似的平面；
若有相似平面则进行合并，合并条件是平面向量相等，且平面距离d＜预设阈值，本实施例取0.1m；
其中xp,yp,zp是一个面的重心，a,b,c,d是另一个面的平面方程系数；
合并过程包括以下子步骤：
步骤4.2.1：计算合并后的重心点pg；
其中pg1,pg2是两平面重心点，size1,size2代表两平面点数；
步骤4.2.2：将点云按照平面发现方向平移至重心点；
其中t1,t2表示两平面点云平移量；
如果无相似平面，则将平面上的点投影到平面方程的绝对平面上，将平面加入结构地图；
步骤4.3：循环步骤4.1-步骤4.2，直到没有面进行优化，则构成结构地图。
步骤5，单条scan与结构地图匹配，提取需优化点对。
基本步骤如下：
步骤5.1：确定当前scan的初始位姿；
其中
步骤5.2：提取匹配点；将scan的直线段进行降采样，在结构地图中搜寻降采样点的匹配点构成匹配点对。
步骤6，优化对应点间的距离d。
其中，是第个扫描点的三维坐标；将每条scan的位姿作为优化项，对应点间的距离作为残差项，借助ceres进行优化，当两次优化位姿之差，则优化结束，得到每条扫描线的优化位姿。
步骤7，迭代循环步骤5-6，直到一个sweep点云优化结束，则将点云添加到结构地图中，进行结构地图生长。具体步骤如下：
步骤7.1：对新sweep依据步骤1-步骤2进行面提取；
步骤7.2：对于每一个提取面pi在结构地图中寻找候选面；候选面条件为两个面的夹角小于角度阈值tθ，可取15°，且两面的距离小于距离阈值td，可取100mm；
其中xp,yp,zp是一个面的重心，a,b,c,d是另一个面的平面方程系数；nnew表示提取面的法向量，nstruct表示结构地图中候选面的向量。
若没有候选面，则将此面上的点投影到平面方程的绝对平面上，并将平面加入结构地图；若有候选面，则进行步骤7.3；
步骤7.3：通过将候选面建立k-d树搜索，确定合并面pmerge；
具体包括以下子步骤：
步骤7.3.1：将候选面点云建立k-d树，并为候选面点云建立所属面的索引关系；
步骤7.3.2：对pi中构成面的每条线段取两端点和中点作为搜索点；
步骤7.3.3：在k-d树中搜索最临近点，并记录临近点的所属平面信息，最终将最邻近点数最多的平面作为合并面pmerge；
步骤7.4：进行pi的方向优化，使其与pmerge方向一致，将两面合并，并进行结构地图生长。
步骤8，循环迭代步骤5-7，直至点云数据处理完毕；最终结构地图如图2所示，三维场景地图与运动轨迹叠加的结果示意图如图3所示。
在硬件设备上，本发明将一个二维激光测距仪固定到一个稳定的高精度的手持式旋转平台上。旋转平台可以带动二维激光测距仪以连续旋转和摇头旋转两种模式进行旋转，从而实现对三维环境数据的获取。对于所获取的三维环境数据，本发明设计了一种准确的结构地图构建算法以及一种鲁棒的扫描线匹配算法，实现了在室内环境中的实时定位以及三维环境地图构建的功能。
本发明提供了一种基于结构信息的准确的结构地图构建算法以及一个鲁棒的扫描线匹配算法，实现了对传感器的实时位姿轨迹以及对室内三维环境的准确构图。为室内智能服务机器人的发展提供了有效的技术基础，具有快速准确，实用性较强，可以应用与三维建模、服务机器人等多种场景。
应当理解的是，本说明书未详细阐述的部分均属于现有技术。
应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。