[发明专利]一种基于单线激光的室内实时定位与三维地图构建方法

权利要求书

1.一种基于单线激光的室内实时定位与三维地图构建方法，其特征在于，包括以下步骤：

步骤1：获取二维扫描线上的原始点云数据，在二维扫描线中提取直线段，并将完整覆盖到一个三维空间的直线段组成一个集合，记为Sweep；

其中所述在二维扫描线中提取直线段，具体实现包括以下子步骤：

步骤1.1：从二维扫描线上的一个起始点开始，对其之后的N个扫描点进行最小二乘直线拟合，并计算拟合误差；

拟合误差的表达形式如下所示：

其中，a,b是直线拟合参数，(x_i,y_i)是第i个扫描点的二维坐标,j为当前直线段在当前二维扫描线上的起始点的序号，N为预设值；

步骤1.2：当拟合误差ε_l＜T_l时，认为得到了一条初始直线段；否则，就跳过当前的起始点，从其下一个点开始继续重复步骤1.1的操作，直到找到了一条初始直线段为止；其中T_l是直线拟合误差的阈值；

步骤1.3：获得一条初始直线段之后，对该条初始直线段进行拓展；具体包括以下子步骤：

步骤1.3.1：从当前初始直线段之后的一个点P_i开始，计算点P_i到当前初始直线段的距离L，其中i＞j+N，j表示当前初始直线段起始点在二维扫描线中的点序号；如果L＜T_d，则该点属于当前初始直线段，将点P_i加入到当前初始直线段中以实现对当前初始直线段的扩展，同时重新对该初始直线段进行拟合，更新当前初始直线段的拟合参数；否则，认为当前初始直线段已经不能继续拓展，当前初始直线段的拓展操作结束；其中T_d为点到直线距离的最大阈值；

步骤1.3.2：如果当前初始直线段被成功地拓展，则继续重复步骤1.3.1的操作，直到当前初始直线段不能继续拓展为止；

步骤1.4：在完成一条初始直线段的拓展之后，将该条初始直线段加入到候选直线段集合中，并从当前初始直线段的下一个点开始重复步骤1.1-步骤1.3的操作，直到无法再从当前二维扫描线上提取出新的直线段为止；

步骤1.5：对于当前二维扫描线上所有的候选直线段，对其进行合并以及删除的操作，以得到最终的直线段；具体包括以下子步骤：

步骤1.5.1：从第i条候选直线段开始，判断与其相邻的第i+1条候选直线段是否能与其合并，判断条件如下：

其中，v_i表示第i条直线段的方向向量，θ(v_i,v_i+1)表示两条直线段方向向量的夹角，T_θ为阈值，表示第i条候选直线段的最后一个点，表示第i+1条候选直线段的第一个点，表示两点之间的距离，T_d′为阈值；

当两条相邻候选直线段的方向向量的夹角小于阈值且首尾点之间的距离也小于阈值时，将这两条候选直线段合并为一条，并重新计算合并后的直线段的拟合参数；

步骤1.5.2：重复1.5.1的操作，直到没有可以再合并的候选直线段为止；计算合并后的直线段的长度，剔除长度小于预设阈值的直线段，将剩下的直线段作为最终提取得到的当前二维扫描线上的直线段，并按顺序存储；

步骤2：对步骤1中获得的直线段集合提取平面；

步骤3：对于第一个Sweep，利用提取出的平面信息，进行主方向提取并抽稀；

步骤4：构建初始结构地图；

步骤5：单条二维扫描线与结构地图匹配，提取需优化点对；

步骤6：优化对应点间的距离d″，计算二维扫描线的位姿估计值；

步骤7：循环迭代步骤5-步骤6，直到一个Sweep中的所有二维扫描线优化结束，则将点云添加到结构地图中，进行结构地图生长；

步骤8：循环迭代步骤5-步骤7，直至点云数据处理完毕。