CN109541630A - 一种适用于建筑物室内平面2d slam测绘的方法 - Google Patents

Abstract

本发明公开了一种适用于建筑物室内平面2D SLAM测绘的方法，该方法包括以下步骤：首先，激光雷达所产生的高精度点云，运用本发明改进的图优化算法，提取扫描关键帧，并对其优化之后，插入到子图当中；其次，优化后的扫描帧会存放在后台，以备用作闭环检测；再者，通过全局优化，缩小SLAM过程中的累计误差；最后，调用存放在后台的扫描帧进行闭环检测，当实现闭环时，可以得到精确的二维的平面图。本发明提供的测绘方法相比传统的的室内测绘方法更加精确，更加简便，同时激光雷达所提供的地图可以用于机器人导航。该方法所获得能够全面和精确构造室内平面环境，对测绘学科的发展有重要意义。

Description

一种适用于建筑物室内平面2D SLAM测绘的方法

技术领域

本发明涉及机器人SLAM和测图技术，尤其涉及一种适用于建筑物室内平面2DSLAM测绘的方法。

背景技术

传统的测量建筑物室内平面图的步骤，大致如下：

1、收集建筑设计施工的图纸，划分工作区间；

2、准备相应的测量仪器，如测距仪、盒尺和配套的绘图图纸、铅笔以及相机；

3、测量得到相关的数据计算两次或者多次，计算平均值；

4、运用计算机辅助设计工具CAD来绘制平面图，计算相关数据。

在整个测绘的过程中，一者，流程比较繁琐，用到的工具比较多，操作过程也不方便，整个流程周期时间长，不是很可取；二者，虽然用到了测距仪和相机等精密传感器，但是测量过程始终会有人为参与，这是传统测绘方法所无法避免的，因为测量结果包含了人对环境的主观感知，这会导致测得的地图精度不高。

发明内容

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种适用于建筑物室内平面2D SLAM测绘的方法。

本发明解决其技术问题所采用的技术方案是：一种适用于建筑物室内平面2DSLAM测绘的方法，包括以下步骤：

1)数据采集：使用激光雷达扫描并采集建筑物室内的环境数据，激光雷达采集的数据为ASCII格式的端点帧数据；端点帧数据集合即为高精度点云数据；

2)数据处理：

2.1)对激光雷达采集的端点帧数据进行位姿变换提取，获得相应的点集；

其中，T_ζ表示扫描到的当前雷达坐标系中的位姿ζ到世界坐标系中的子图的变换，p表示扫描到的端点，激光雷达的位姿为ζ，且ζ＝(ζ_x,ζ_y,ζ_θ)^T，这三个参数为相对世界坐标系的二维平面坐标ζ_x,ζ_y及其方位角ζ_θ；

2.2)对点集进行优化，选取满足在子图中的概率最大的点集，其寻优公式如下：

其中，M_smooth函数使用的是双三次插值法函数，p＝{h_k},k＝1,2,…,k，描述的是扫描的端点；

2.3)建立网格地图，将多帧数据经过位姿转换后插入到子图，基于已有的激光扫描估计点集在子图中的最佳位置；

对于每个网格点，定义一个相应的像素，每当一个新的子图被插入进概率网格，一组hit网格点和不相交的miss网格点概率就被计算，同时，当网点没有被观测到的话，会根据他们所在的集合是hit还是miss赋予一个概率值，若已被观测到，就会更新网格hit与miss的概率odds(p)和网格的分辨率M_new(x)；

2.4)将生成的子图进行拼接，为了减小拼接过程中的累积误差，每隔设定时间，就会调用ceres库计算一次，SPA计算公式如下：

其中，代表子图的位姿，表示扫描帧的位姿，∑_ij表示的是子图与扫描帧的协方差矩阵，ζ_ij表示子图所对应的扫描帧的位姿，E代表残差函数，ρ函数是Huber损失函数；

2.5)闭环检测：根据步骤2.2)中优化后的扫描帧及与之对应的子图帧，进行闭环检测；

若闭环检测成功，则转入步骤3)，否则转入步骤2.2)

3)将所有子图拼接的结果作为最后的全局地图。

本发明产生的有益效果是：

1、本发明方法降低了传统测绘过程中对人工测量获取数据的依赖性，实现全自主的、无人为干预的测绘方式，可应用于大规模场景的室内测绘，同时，在SLAM制图的过程中不需要担心由于激光雷达出现晃动的情况而导致全局地图重叠的问题，具有较好的鲁棒性。

2、使用本发明方法仅仅依靠单一的传感器(激光雷达)，在没有GPS和IMU模块的协助下，依然能够获得较高的地图精度，此方法使用于刚竣工的大型建筑物的室内测图以及地下隧道和地下矿场的测绘，测绘精度高，商用价值巨大。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的方法流程图；

图2是本发明实施例的硬件连接示意图；

图3是本发明实施例的SLAM示意图；

图4是本发明实施例的全局地图示意图；

图5是本发明实施例的大规模全局地图示意图；

图6是本发明实施例的Hector SLAM全局地图示意图；

图7是本发明实施例的Hector SLAM大规模地图示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1所示，本发明提供一种适用于室内平面2D SLAM测绘的方法，包括如下步骤：

步骤S1：激光雷达的启动与数据的传输：

步骤S11：激光雷达的启动：激光雷达连接好之后，移动电源正常供电，5V电源给无线路由器供电，12V电源给激光雷达供电，激光雷达开始正常工作；如图2；

步骤S12：激光雷达数据传输：激光雷达开始扫描，且扫描所得数据经由路由器通过TCP/IP的方式实时传输到PC端，PC端ping通激光雷达后接收数据；

步骤S2：PC端上位机数据的接收与处理：

步骤S21：PC端上位机数据的接收：配置好cartographer包，以及相应的launch启动文件，启用ROS节点，以及RViz软件，一切软件环境就绪；

步骤S22：PC端接受经过无线传输的激光雷达数据，经过比较多束激光雷达的扫描数据，获取扫描之后的endpoint frame端点帧数据，多帧数据之间形成扫描位姿，将位姿通过转换，插入到子图submap中,每个子图就对应整个区域中的一小块地图。

其中，激光雷达端点帧转换的公式如下：

其中，T_ζ表示扫描到的当前位姿ζ到子图的变换，p表示扫描到的端点endpoint，激光雷达的位姿为ζ，且ζ＝(ζ_x,ζ_y,ζ_θ)^T，由于是在2D平面的SLAM测绘，故可用三个参数描述它的位姿，这三个参数为相对外部坐标系的二维平面坐标ζ_x,ζ_y及其方位角ζ_θ；

步骤S23：非线性优化，如图3中扫描帧位姿ceres优化：在插入扫描帧到子图之前，扫描的位姿会用cere-solver非线性优化库进行一个局部的优化,也就是将位姿的优化问题作为一个非线性最小二乘问题，公式如下：

其中M_smooth函数使用的是双三次插值法，比网格分辨率的优化具有更高的准确性，且H＝{h_k},k＝1,2,…,k，描述的是扫描的端点；

步骤S24：数据存放于后端以备用：经过上一步骤优化之后，将扫描位姿及其对应的子图帧放入后台中，用以检测地图的闭环问题。

步骤S25：网格地图的更新：子图是以概率网格的形式构造的，对于每个网格点，定义一个相应的像素，每当一个新的扫描图被插入进概率网格，一组hit网格点和不相交的miss网格点概率就被计算，同时，当网点没有被观测到的话，会根据他们所在的集合是hit还是miss赋予一个概率值，若已被观测到，就会更新hit与miss的概率odds(p)和网格的分辨率M_new(x)；

其中，用以点x的更新hit的概率公式和网格的大小公式分别如下：

M_new(x)＝clamp(odds^-1(odds(M_old(x))·odds(p_hit)))

odds表示的是hit在网格中被观测的概率，概率越大，其对应的子图区域在地图上越明亮；

M_new(x)表示已观测到的网格点的概率值在原有的M_old(x)的基础上的更新；

步骤S3：激光雷达的水平移动与全局优化：

步骤S31：激光雷达的移动：测绘人员手持激光雷达，绕特定的室内环境进行水平移动，系统状态图3所示，激光雷达的移动运用到了机器人学的graph SLAM的概念，有5个机器人位姿和2个地图特征m₁、m₂机器人(也就是实验中的激光雷达)，图中的实线连接的是相邻机器人的位姿，虚线连接与在此位姿观察到的特征，graph SLAM中的每个连接都是一个非线性二次方约束，其全局优化公式如SPA所示。

步骤S32：全局优化：为了获得质感更强，细节更精确的地图，测绘人员需尽量走到室内环境的角落买区域，以防出现遮挡，而导致地图不完整，同时测绘人员在移动的过程中，激光雷达与路标建立了更多的约束关系，可以还原环境的更多细节；再者，随着激光雷达的移动，看到多子图的不断生成，以及不断拼接过程。为此，为了减小拼接过程中的累积误差，每隔几秒，就会调用ceres库计算一次，SPA计算公式如下：

其中代表子图的位姿，表示扫描帧的位姿，∑_ij表示的是子图与扫描帧的协方差矩阵，ζ_ij表示子图所对应的扫描帧的位姿，E代表残差函数，ρ函数是Huber损失函数，为了减小外部因素的干扰，使全局优化达到最好的效果；优化的直接效果是调整子图，防止图歪斜。

步骤S4：闭环检测与全局地图的生成：

步骤S41：闭环检测，如图1所示：存放在后台的扫描帧以及与之对应的子图帧用于地图的闭环检测，如果闭环检测成功，则生成全局一致地图，若闭环检测失败，继续返回优化过程，直至检测成功，获取全局地图。

步骤S42：生成的全局地图如图4所示，并地图在其相应的地点取景，可以看出，该算法效果相比其他测绘方法的优越性；生成的更大规模的地图如图5所示，如图所示，地图中的小汽车的轮廓，以及墙面和阀门很明显的显示了出来，可见该测绘算法更能完美的还原现实中的细节，体现出来该算法图形建模的准确性。

传统的Hector SLAM方法对比，如图6所示，当地图规模较小时，由于缺少优化环节，地图明显出现了歪曲，而且，在地图拼接的过程中，也出现了较大的误差，相比之下，本文的图优化算法，能够很好的对室内环境进行建模，误差相比之下要小很多，如图4；当环境规模变大时，由于hector算法本身的限制，无法根据地图的大小调节网格的尺度，导致建图超出了边界，实验无法继续进行，如图7,与此同时，本发明的图优化算法，能根据地图尺寸大小，调整网格的区域，如图5,且在大范围的环境下也能够得到很好的效果。

本发明区别于传统的室内测绘的方式，不需要复杂的外部硬件和测量工具，能不在人为干预测绘的基础上，利用机器人学的graph SLAM和传统的2D激光雷达便能达到一个较高精度的室内地图。也就是说，用户只需一台笔记本电脑和一个激光雷达，即可实现室内测绘。用户使用起来安全可靠，轻便易行。进一步的，本发明仅仅依靠单一的传感器(激光雷达)，在没有GPS和IMU模块的协助下，依然能够获得较高的地图精度，此方法使用于刚竣工的大型建筑物的室内测图以及地下隧道和地下矿场的测绘，商用价值巨大。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (4)

1.一种适用于建筑物室内平面2D SLAM测绘的方法，其特征在于，包括以下步骤：

1)数据采集：使用激光雷达扫描并采集建筑物室内的环境数据，即端点帧数据；

2)数据处理：

2.1)对激光雷达采集的端点帧数据进行位姿变换提取，获得用于形成子图的点集；

其中，T_ζ表示扫描到的当前雷达坐标系中的位姿ζ到世界坐标系中的子图的变换，p表示扫描到的端点，激光雷达的位姿为ζ，且ζ＝(ζ_x,ζ_y,ζ_θ)^T，这三个参数为相对世界坐标系的二维平面坐标ζ_x,ζ_y及其方位角ζ_θ；

2.2)对点集进行优化，选取满足在子图中的概率最大的点集，其寻优公式如下：

其中，M_smooth函数使用的是双三次插值法函数，p＝{h_k},k＝1,2,…,k，描述的是扫描的端点；

2.3)建立网格地图，将多帧数据经过位姿转换后插入到子图，基于已有的激光扫描估计其在子图中的最佳位置；

对于每个网格点，定义一个相应的像素，每当一个新的子图被插入进概率网格，一组hit网格点和不相交的miss网格点概率就被计算，同时，当网点没有被观测到的话，会根据他们所在的集合是hit还是miss赋予一个概率值，若已被观测到，就会更新网格hit与miss的概率odds(p)和网格的分辨率M_new(x)；

2.4)将生成的子图进行拼接，为了减小拼接过程中的累积误差，每隔设定时间，调用ceres库计算一次，SPA计算公式如下：

其中，代表子图的位姿，表示扫描帧的位姿，∑_ij表示的是子图与扫描帧的协方差矩阵，ζ_ij表示子图所对应的扫描帧的位姿，E代表残差函数，ρ函数是Huber损失函数；

2.5)闭环检测：根据步骤2.2)中优化后的扫描帧及与之对应的子图帧，进行闭环检测；

若闭环检测成功，则转入步骤3)，否则转入步骤2.2)

3)将所有子图拼接的结果作为最后的全局地图。

2.根据权利要求1所述的适用于建筑物室内平面2D SLAM测绘的方法，其特征在于，所述步骤1)中激光雷达采集的数据为ASCII格式的端点帧数据。

3.根据权利要求1所述的适用于建筑物室内平面2D SLAM测绘的方法，其特征在于，所述步骤2.4)中将生成的子图进行拼接，为了减小拼接过程中的累积误差进行如下优化：每隔设定时间周期，调用ceres库计算一次，SPA计算公式如下：

其中，代表子图的位姿，表示扫描帧的位姿，∑_ij表示的是子图与扫描帧的协方差矩阵，ζ_ij表示子图所对应的扫描帧的位姿，E代表残差函数，ρ函数是Huber损失函数。

4.根据权利要求3所述的适用于建筑物室内平面2D SLAM测绘的方法，其特征在于，所述设定时间周期为1至5秒。