将占据栅格地图转换为基于姿态图的地图：基于BIM的2D LiDAR定位在变化和动态环境中实现终身室内导航的鲁棒性。
摘要：
许多研究都依赖于事实上的标准自适应蒙特卡罗定位（AMCL）方法，以在从建筑信息模型（BIM模型）提取的占用栅格地图（OGM）中定位机器人。然而，大多数这些研究都假设BIM模型准确地代表了真实世界，而这很少是真实的情况。参考BIM模型与现实世界（Scan-BIM偏差）之间的差异不仅是由于家具或杂物的存在，还由于在设计阶段创建的任何模型中存在的按计划和按实际建造的偏差。这些扫描-BIM偏差可能会严重影响AMCL的准确性。本文提出了一种开源方法，可以从BIM模型中生成适当的姿态图（Pose Graph）地图，以实现在变化和动态环境中的稳健的2D-LiDAR定位。首先，2D OGMs是从复杂的BIM模型自动生成的。这些OGM仅代表结构元素，允许室内自主机器人导航。然后，一种有效的技术将这些2D OGM转换为基于姿态图的地图，从而实现更精确的机器人姿态跟踪。最后，我们利用不同的地图表示结合最先进的算法，实现准确、鲁棒的定位。此外，我们在三个模拟场景中提供了各种最先进的定位算法的定量比较，这些场景具有不同程度的扫描-BIM偏差和动态代理。更准确地说，我们比较了两种粒子滤波（PF）算法：自适应蒙特卡罗定位（AMCL）和通用蒙特卡罗定位（GMCL）；以及两种基于图形的定位（GBL）方法：Google的Cartographer和SLAM工具箱，解决全局定位和姿态跟踪问题。我们发现在一个真实的办公环境中（在中等级别的Scan-BIM偏差下），AMCL的平移RMSE增加了四倍（从空环境中的8.5厘米增加到真实环境中的33.7厘米）。相反，基于位姿图的算法表现出与粒子滤波（PF）算法相比的优越性，在实际环境中实现了7.2厘米的均方根误差（RMSE）。大量实验表明，所提出的方法在变化和动态环境中，无论是使用设计好的BIM模型还是稀疏OGM，都能够实现稳健的定位，比传统的AMCL在精度和稳健性方面表现更好。
1、介绍
在室内无法使用GPS的环境中，准确的定位系统对于成功部署自主移动机器人至关重要。
室内定位问题已经通过应用多种技术得到解决。有些技术依赖于已知的地标位置，例如AprilTags或文本提示，而其他技术则依赖于必须在建筑物上战略性安装的传感器，例如Beacons或WiFi接入点。然而，在大多数情况下，特定地标的确切位置事先是不知道的。此外，增加额外的传感器会增加导航系统的成本。
一个BIM模型，对于大多数当前的建筑来说都是可用的，可以用作LiDAR定位的参考地图。
此外，模型的附加语义信息可以被利用来创建高级自动化机器人任务，例如物体检查（Kim＆Peavy 2022）或绘画（Kim等人2021），这同时依赖于准确的定位系统。
扫描-BIM偏差是使用BIM模型或楼层平面图作为2D-LiDAR定位参考地图的核心问题。这些偏差可能是由于模型中不存在的家具或杂物、按计划和实际建造的变化以及环境中的动态或“准静态”变化引起的。为了解决这个挑战，我们提出了一个系统，可以从BIM模型创建OGM，并允许它们在基于姿态图的地图中自动转换。这些地图被利用于在室内GPS失效的环境中快速、内存高效和准确的定位，从而实现更安全的自主导航。
本文的主要贡献如下：
一种从复杂的多层BIM模型中提取OGMs的方法，可以实现机器人路径规划和在室内无GPS信号的环境下自主导航。
一种高效的开源方法，用于将这些2D OGM转换为基于姿态图的地图，以实现精确的2D-LiDAR定位。
在三个精心设计的模拟场景中，对各种最先进的2D LiDAR定位算法进行了广泛的定量比较，这些场景具有不同水平的Scan-BIM偏差，并且有无动态代理。
本文的剩余部分组织如下。第2节介绍了LiDAR定位的问题表述以及基于粒子滤波和基于图的定位策略的主要原理。第3节描述了基于BIM的LiDAR定位的先前工作。第4节介绍了我们从BIM模型生成OGMs的方法，从OGMs生成姿态图的地图，以及提出利用这些地图进行鲁棒定位的方法。第5节介绍了实验设置，接着在第6节中呈现了结果和分析。最后，第7节总结了我们的工作。
2、理论背景
在介绍当前最先进的方法之前，本文提供了一个简要介绍，介绍了本研究中使用的两种主要定位算法的理论基础。
2.1 定位问题
在本文中，我们解决了机器人姿态跟踪和全局定位问题，即在给定3D BIM模型作为先验地图的情况下，分别考虑具有近似初始姿态和没有近似初始姿态的情况，并假设机器人采用2D-LiDAR传感器。BIM模型在实际环境中省略了相当多的信息。
在二维问题中，机器人在时间??的姿态被定义为在地图坐标系下的位置和方向?? = [?, ?, ?] ⊤。我们的目标是根据测量值??和地图?，估计出最有可能的机器人姿态?∗?。形式上，目标是计算：
两种广泛使用的方法旨在计算这个估计值，它们分别是PF算法和GBL算法。
2.2粒子滤波算法
PF算法，也称为MCL方法，是一种概率方法，它使用一组归一化加权的粒子来表示姿态估计。
随后，算法会迭代地重复三个步骤：运动更新、重要性加权和粒子重采样。
为了更详细地解释每个步骤，读者可以参考Thrun等人的论文（2005年）。在本文中，我们实现了AMCL（Pfaff等人，2006年）和GMCL（Alshikh Khalil＆Hatem，2021年），并在不同程度的扫描-建模偏差下进行了测试。
2.3 基于图优化的算法
基于图形的定位方法，也称为基于优化的定位方法，使用姿态图数据进行姿态估计。这些姿态图包含与节点相关联的环境地标（可以表示为子地图），这些节点是观察到地标的姿态。
此外，节点之间还受到空间约束的限制。
滑动窗口方法不仅考虑最近的测量值，而且考虑一组测量值来计算当前的姿态。在假设测量值服从正态分布且独立同分布的情况下，可以将公式1表示为加权最小二乘问题。
这个问题通常使用Levenberg-Marquart算法进行迭代求解。在本文中，我们实现了Cartographer（Hess等人，2016年）和SLAM Toolbox（Macenski和Jambrecic，2021年）作为GBL算法。
虽然粒子滤波算法更易于实现并且可以表示非高斯分布，但基于图的定位算法除了是确定性的之外，还可以处理延迟测量并保持最近的姿态历史记录。Wilbers等人（2019）提供了更详尽的定性比较。
3、相关研究
具有三维几何信息的BIM模型可以用作先验地图，以在室内无GPS信号的环境中准确定位机器人并实现自主导航。
本节将概述最先进的方法，这些方法使用先前的建筑信息，即BIM模型或楼层平面图，来找到正确的机器人位置和方向。
Follini等人（2020）表明，通过应用标准的AMCL算法，可以检索机器人参考系统和从BIM模型中提取的地图之间的转换矩阵。
Prieto等人（2020年）、Karimi等人（2021年）、Kim等人（2021年）和Kim＆Peavy（2022年）使用相同的算法来定位一个轮式机器人在从BIM模型生成的OGM中。
这些方法的主要区别在于它们如何从BIM模型创建OGM。Follini等人采用与水平平面相交的元素的顶点，并使用Open CASCADE查看器生成具有相应分辨率和地图原点信息的pgm格式的OGM。而Prieto等人则使用Industry Foundation Classes（IFC）文件中空间的几何形状以及每个开口的位置和大小。
Karimi等人（2020）开发了一个建筑信息机器人系统（BIRS），使得可以从BIM模型生成和语义传输拓扑和度量地图到机器人操作系统（ROS）。该工具在（Karimi等人，2021）中进一步开发，具有最优路径规划器，集成了用于施工评估的关键组件。Kim等人（2021）实现了一种将IFC文件转换为ROS兼容的仿真定义格式（SDF）世界文件的方法，适用于机器人任务规划。他们评估了他们的方法，用于室内墙面涂漆。
后来，为了加入动态对象并进行门检查，Kim＆Peavy（2022）提出了一种将IFC模型转换为Universal Robot Description Format（URDF）建筑世界的技术。一旦获得了URDF模型，他们使用PgmMap创建器（Yang 2018）来创建一个OGM。
Hendrikx等人（2021）提出了一种方法，该方法不使用OGM，而是使用从IFC文件中提取的机器人特定的世界模型表示进行2D-LiDAR定位。在他们基于因子图的定位方法中，系统查询其周围的语义对象，并在它们与激光测量之间创建数据关联。虽然他们证明了该方法可以跟踪机器人的姿态，但没有进行定量评估。
Boniardi等人（2017）使用基于CAD的建筑平面图进行2D LiDAR定位，而不是使用BIM模型。在他们的定位系统中，他们实现了广义ICP（GICP）用于扫描匹配，并结合姿态图同步定位与建图（SLAM）系统。稍后在（Boniardi等人，2019）中，他们提出了一个改进的流程，用于在动态环境中进行长期定位。
Zimmerman等人（2022）使用从切片TLS点云获得的OGM，结合人类可读的本地化提示来辅助定位。他们基于文本检测的定位技术可以检测已知的房间号码，因此可以在具有结构变化的对称环境中稳健地处理定位。
虽然已经出现了几种方法旨在从BIM模型创建OGM，但它们都没有处理具有多个楼层和倾斜地板的复杂非凸模型。
此外，大多数提出的技术都基于一个强烈的假设，即BIM模型非常精确地表示了建筑物的实际当前状态，忽略了由于杂物、家具、按计划与实际建设差异、长期运营变化或动态代理的存在可能导致的Scan-BIM偏差。
4、方法论
如图1所示，我们的方法可以分为三个主要步骤：第一步：使用IfcConvert和OpenCV从IFC文件创建OGM。第二步：利用图像处理、覆盖路径规划和射线投射自动生成基于姿态图的地图。第三步：使用粒子滤波算法和基于图的定位系统进行稳健的定位。
4.1 OGM从IFC中生成
为了从复杂的多层IFC模型中创建适合机器人定位和导航的2D OGMs，使用了IfcOpenSchell（Krijnen 2015）的IfcConvert工具和图像处理技术。IfcConvert允许创建一个SVG格式的2D地图，其中包含IFC模型中在所需高度处穿过平面的所需元素。
在我们的情况下，非永久性实体，如空间、窗户和门，通过忽略相应的实体名称，被排除在生成的2D OGMs之外。这种排除对于过滤建筑物的结构信息是必要的，从而使进一步的自主导航只关注想要探索的房间。
除了在OGMs中拥有永久结构之外，为了实现全球定位和后续正确的姿态图生成，区分室外（未知）和室内（可导航）空间在OGMs中非常重要。这种区分可以通过创建第二个2D地图来自动化完成，该地图包含IFC文件中的所有实体（例如门和窗户）。
根据SVG图像中的轮廓，对室外（灰色）、室内（白色）和障碍物（黑色）进行最终分离。OpenCV允许根据轮廓的层次结构进行处理，即根据它们是否在另一个轮廓内部（子轮廓）或外部（父轮廓）进行处理。
最终生成的文件被转换为.pgm格式，连同其属性（分辨率和原点）一起保存在一个.yaml文件中，然后可以作为先前的环境信息加载到机器人系统中，从而实现机器人的定位、路径规划和自主导航。
对于多层建筑，可以采用类似的方法。对于不重叠的楼层，如果它们之间的相对位置已知，则可以将不同的OGM合并为一个。为了保持这种空间关系，在获取OGM时，可以在其周围包含高度等于最大建筑高度的参考辅助元素。通过这些额外的元素，所有OGM将具有相同的尺寸，从而可以进行合并。
使用IfcConvert创建2D OGM（占用网格图）相对简单，当所需截面是水平的（与XY平面平行）时。然而，如果模型具有斜坡或略微倾斜的地板，则在生成占用网格图之前必须将模型旋转。幸运的是，IfcConvert还允许在给定旋转向量计算出的四元数的所需角度内旋转模型。
4.2 OGM2PGBM OGM到位姿图的转换
从BIM模型自动生成适用于GBL方法的数据意味着在模型中模拟整个可导航空间中的顺序激光数据，并使用相应的里程计数据。为此，使用先前生成的2D OGMs。
应用Lee等人（1994）提出的骨架方法，可以在平滑的轨迹中连接所有房间。随后，在骨架的膨胀版本内可行驶区域上应用Wavefront覆盖路径规划器（Zelinsky等人，1993），以找到模拟激光的路标。然后，使用射线投射算法，不需要实时仿真引擎（如Gazebo），模拟激光传感器数据和里程计数据，按照前一步骤找到的路标进行模拟。最后，轨迹构建器合并这些传感器数据，创建一个基于姿态图的精确地图，序列化为Cartographer的.pbstream文件或SLAM Toolbox的.posegraph文件。
这个流水线可以高效地自动生成基于姿态图的地图，使用2D OGMs作为输入。由于我们的OGM2PGBM工作流不需要使用Gazebo进行数据模拟，因此它比基于Gazebo的流水线更快速和更易于移植，可以在隔离的环境中执行。此外，由于该技术不考虑完整的3D模型，而只考虑2D OGMs，因此非常高效。此外，它可以从任何给定的OGMs中使用，除了可以从BIM模型（使用前一节中介绍的方法生成）生成外，还可以从楼层平面图或先前扫描的地图中生成。由于每个扫描的位置在模拟中已经准确知道，因此不需要进行图形优化。
4.3 鲁邦的定位
一旦从BIM模型生成了所需的不同地图表示（OGM和姿态图基础地图），它们可以用于在不断变化的环境中进行稳健的定位。
我们建议利用GMCL的自适应PF，仅在SER区域中扩散粒子，从而高效地解决全局定位问题。正如后面所示（第6节），能够表示非高斯分布的PF算法可以比基于图的算法更快地解决全局定位问题。
一旦找到一个协方差小于0.05的估计姿态，GMCL的节点就会停止，并且可以启动GBL算法。
例如，为了准确跟踪机器人的姿态，可以在GMCL收敛时使用生成的.pbstream地图通过start\_traj服务启动Cartographer。类似地，SLAM工具箱可以使用先前的.posegraph地图启动，并设置初始姿态。这些方法在第4.2节中有所介绍。
56
7、结论
在这篇论文中，除了提出从BIM模型创建OGMs并将其转换为基于姿态图的地图以实现鲁棒定位的方法之外，我们还对不同水平的Scan-BIM偏差下的各种最先进的2D-LiDAR定位算法进行了广泛比较，包括有无动态代理的情况。
我们发现，在姿态跟踪问题上，GBL算法优于PF算法。如果地图具有非常低（或可以忽略不计）的Scan-BIM偏差，SLAM Toolbox可以实现最佳性能。相反，如果地图具有中等水平的Scan-BIM偏差（例如，由于大型家具或计划和实际建设之间的差异，如实际办公楼），则Cartographer是表现最佳的方法。
然而，在环境变化程度过高的情况下（例如第三种情况），虽然误差相对较高，但SLAM工具箱仍然是测试过的定位算法中最佳的选择。粒子滤波算法只考虑最近的观测结果来更新当前姿态的置信度，这使它们具有处理高度模糊的情况（例如第三种情况）的一定鲁棒性。然而，这也会导致在Scan-BIM偏差程度中等的情况下（例如第二种情况）出现较高的不准确性。另一方面，利用最近一段时间的观测历史，全局位姿估计算法可以更好地处理这种真实世界的情况，并能更准确地跟踪机器人的姿态。
然而，相比于全局贝叶斯定位算法（GBL），基于图的蒙特卡罗定位算法（GMCL）在全局定位问题上表现更好。一般来说，我们建议在真实环境中进行基于BIM（或平面图）的2D LiDAR姿态跟踪时使用GBL算法以获得更高的精度，而在全局定位时使用GMCL算法。为了促进正确实现基于图的定位算法，我们提供了一种开源方法，可以从任何OGM（占据网格地图）创建准确的姿态图。此外，我们还提供了一种从复杂的多层BIM模型创建OGM的方法，这可以进一步用于路径规划和自主导航。最先进的SLAM技术已经从使用粒子滤波器转向使用基于图的优化方法；根据我们的实验，我们可以得出结论，对于大多数定位系统来说，这将是类似地有利的。
8、将来工作
根据实验结果和相关研究的动机，我们认为以下是有前途的未来研究方向：
考虑不仅使用2D-LiDAR信息，而且使用3D-LiDAR传感器数据是一个有前途的方向，可以可靠地处理重要的Scan-BIM偏差，部分由Blum等人（2020）和Moura等人（2021）展示。
融合多个传感器模式，如IMU、RGB-D相机和LiDAR传感器，将增加定位方法的鲁棒性，以处理快速角度运动和陈旧的场景，如Lin＆Zhang（2021）和Xu等人（2022）所示。
为了实现更大的鲁棒性，从BIM模型中提取详细信息，如房间编号标签、门和窗户的位置，可以支持解决全局定位问题，即使在对称环境中也是如此，由Zimmerman等人（2022）和Haque等人（2020）完成。
10、参考
Alshikh Khalil, M. A. & Hatem, I. (2021) GMCL As a Pro- posed Replacement to AMCL in ROS for Mobile Robots Localization in Known-Based 2D Environments. Blum, H., Stiefel, J., Cadena, C., Siegwart, R. & Gawel, A. (2020) Precise Robot Localization in Architectural 3D Plans. Boniardi, F., Caselitz, T., Kummerle, R. & Burgard, W. (2017) Robust LiDAR-based localization in architectural floor plans. In: 2017 IEEE/RSJ International Conference on In- telligent Robots and Systems (IROS). IEEE, pp. 3318– 3324. Boniardi, F., Caselitz, T., Kümmerle, R. & Burgard, W. (2019) A pose graph-based localization system for long-term navi- gation in CAD floor plans. Robotics and Autonomous Sys- tems 112, 84–97. Follini, C., Magnago, V. & Freitag, K. et al. (2020) BIM-Inte- grated Collaborative Robotics for Application in Building Construction and Maintenance. Robotics 10 (1), 2. Grupp, M. (2017) evo: Python package for the evaluation of odometry and SLAM. https://github.com/MichaelGrupp/evo. Haque, A., Elsaharti, A., Elderini, T., Elsaharty, M. A. & Neu- bert, J. (2020) UAV Autonomous Localization using Macro-Features Matching with a CAD Model. Sensors (Basel, Switzerland) 20 (3). Hendrikx, R. W. M., Pauwels, P., Torta, E., Bruyninckx, H. J. & van de Molengraft, M. J. G. (2021) Connecting Seman- tic Building Information Models and Robotics: An applica- tion to 2D LiDAR-based localization. In: 2021 IEEE Inter- national Conference on Robotics and Automation (ICRA). IEEE, pp. 11654–11660. Hess, W., Kohler, D., Rapp, H. & Andor, D. (2016) Real-time loop closure in 2D LIDAR SLAM. In: 2016 IEEE Interna- tional Conference on Robotics and Automation (ICRA). IEEE, pp. 1271–1278. Karimi, S., Braga, R. G., Iordanova, I. & St-Onge, D. (2021) Semantic Navigation Using Building Information on Con- struction Sites. Karimi, S., Iordanova, I. & St-Onge, D. (2020) An ontology- based approach to data exchanges for robot navigation on construction sites. Kim, K. & Peavy, M. (2022) BIM-based semantic building world modeling for robot task planning and execution in built environments. Automation in Construction 138, 104247. Kim, S., Peavy, M., Huang, P.-C. & Kim, K. (2021) Develop- ment of BIM-integrated construction robot task planning and simulation system. Automation in Construction 127, 103720. Krijnen, T. (2015) IfcOpenShell. https://github.com/IfcOpenShell/IfcOpenShell. Lee, T. C., Kashyap, R. L. & Chu, C. N. (1994) Building Skel- eton Models via 3-D Medial Surface Axis Thinning Algo- rithms. CVGIP: Graphical Models and Image Processing 56 (6), 462–478. Lin, J. & Zhang, F. (2021) R3LIVE: A Robust, Real-time, RGB- colored, LiDAR-Inertial-Visual tightly-coupled state Esti- mation and mapping package. Macenski, S. & Jambrecic, I. (2021) SLAM Toolbox: SLAM for the dynamic world. Journal of Open Source Software 6 (61), 2783. Moura, M. S., Rizzo, C. & Serrano, D. (2021) BIM-based Lo- calization and Mapping for Mobile Robots in Construction. In: 2021 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC). IEEE, pp. 12– 18. Pfaff, P., Burgard, W. & Fox, D. (2006) Robust Monte-Carlo Localization Using Adaptive Likelihood Models. In: Chris- tensen, H. I. (ed.) European Robotics Symposium 2006. Springer-Verlag, Berlin/Heidelberg, pp. 181–194. Prieto, S. A., Garcia de Soto, B. & Adan, A. (2020) A Metho- dology to Monitor Construction Progress Using Auto- nomous Robots. In: "Osumi, H. (ed.) Proceedings of the 37th International Symposium on Automation and Robotics in Construction (ISARC). International Association for Au- tomation and Robotics in Construction (IAARC). Thrun, S., Burgard, W. & Fox, D. (2005) Probabilistic robot- ics. MIT Press, Cambridge Mass. Wilbers, D., Merfels, C. & Stachniss, C. (2019) A Comparison of Particle Filter and Graph-Based Optimization for Local- ization with Landmarks in Automated Vehicles. In: 2019 Third IEEE International Conference on Robotic Compu- ting (IRC). IEEE, pp. 220–225. Xu, W., Cai, Y., He, D., Lin, J. & Zhang, F. (2022) FAST- LIO2: Fast Direct LiDAR-Inertial Odometry. IEEE Trans- actions on Robotics, 1–21. Yang, H. (2018) GitHub - PgmMapcreator: Create pgm map from Gazebo World File for ROS Localization., https://github.com/hyfan1116/pgmmapcreator. Zelinsky, A., Jarvis, R. A., Byrne, J. C. & Yuta, S. (eds.) (1993) Planning paths of complete coverage of an unstruc- tured environment by a mobile robot. Citeseer. Zimmerman, N., Wiesmann, L., Guadagnino, T., Läbe, T., Behley, J. & Stachniss, C. (2022) Robust Onboard Locali- zation in Changing Environments Exploiting Text Spot- ting. http://arxiv.org/pdf/2203.12647v1.
- based Environments Localization Graph-based Navigationbased environments localization graph-based graph-based 基因组graph-based基因pan-genome environments lightning-fast connectivity environments experience qnx understanding environments virtual localization high-strength environments operations reliable explanation-aware environments explanation experience localization simultaneous real-time slamesh