作者带你读论文 | 对SLAM感兴趣吗？不妨试试FP-Loc吧！（ICRA2022）

FP-Loc: Lightweight and Drift-free Floor Plan-assisted LiDAR Localization.

Ling Gao and Laurent Kneip.

Mobile Perception Lab, SIST, ShanghaiTech.

一种轻量级、无漂移，基于建筑图纸的激光雷达定位解决方案。

高翎，Laurent Kneip

移动感知实验室（MPL），信息科学与技术学院，上海科技大学

鲁棒性、准确性、高效性，是众多移动自主系统（例如智能车辆或机器人平台）所关注的核心指标。依此，在被动增量式传感器（例如多线激光雷达、传统RGB相机等）的数据支持下，即时定位与地图重建技术（SLAM算法）凭借其出众表现，一般会被视作在无法使用GPS系统来支持绝对定位的环境下至关重要的解决方案。SLAM算法无需环境信息的先验知识，便可在实现追踪位姿的同时构建出周围场域的环境地图信息。不过这样的假设在我们看来大可不必！

室内环境下，我们不难获得关于建筑环境信息的先验知识，比如通过专用的三维环境扫描工具来生成稠密的三维点云（例如MPL实验室的合作伙伴岱悟智能Stereye所研发的Polar扫描系统，便可轻松生成如下图所示的高质量稠密三维环境地图）。通过使用这类解决方案，我们不仅可以生成高质量的彩色三维环境点云，也可以由此进一步得到自动生成的建筑平面图纸或建筑信息模型（BIM）。考虑到未来不同设备之间的互联互通特性以及传输性能的进一步提升，我们有理由相信这类自动生成或者本身就具备的建筑信息可直接为算法所用。因此，与其考虑没有任何先验信息的SLAM算法，不如更多尝试设计一些有一定先验信息的定位算法。这样的趋势在自动驾驶社区兴盛已久，但在室内定位领域仍需进一步推动。

在这项工作中，我们首次提出了一种在室内环境下使用建筑图纸作为环境先验信息的三维激光雷达追踪方法。具体而言，我们的方法是在激光雷达输出的三维点云数据中，实时鲁棒估测天花板和地板两个平面的数学表达，并以此来求解传感器高度、滚转角、俯仰角的变化值。这些被求解的运动参数可以进一步被用于运动补偿，以帮助我们更好的在剩余三维点云数据中分割出竖直墙面的特征点集。这个代表墙面的特征点集最终会被投影至一个水平平面，并与建筑图纸实现配准对齐，以获得余下三个未解的运动参数。这样的设计可以保证三维激光雷达的绝对位姿被持续稳定地追踪。

流程图完整展示了我们提出的基于建筑图纸的室内定位算法细节（FP-Loc）。这个系统利用三维激光雷达的读数和建筑图纸的信息作为输入，通过两段式优化来生成一个六自由度的最优轨迹。第一段的单帧注册优化仅基于建筑图纸信息优化单个位姿，然后根据优化后的位姿和先前关键帧的位姿距离来判断是否构成新的关键帧。第二段优化如因子图所示，是一个多帧滑动窗口的优化问题。绿色的实心圆形节点代表着每一个关键帧的位姿信息；橙色的矩形框代表着建筑图纸信息（以近似最近邻域场、ANNF来表征衡量）；灰色的方格是基于关键帧的相对位姿与相对时间信息所估计的近似线、角速度，随时间正则化以保证整体运动平滑。

这项工作利用了一种为建筑图纸设计特别设计的数据结构，叫作近似最近邻域场（ANNF）。它可以高效地查找出建筑图纸中任意给定位置的最近邻几何元素（例如线段、曲线、圆等）。这种数据结构可以在定位任务开始前提前构建，并在任务中直接查表使用。这个数据结构显著地减少了匹配所需的计算时间开销，并使得最终的算法可在嵌入式硬件上顺畅运行。实验表明我们的算法结果优于传统的SLAM算法，并可以在一台常规性能的笔记本电脑上以五倍于实时的效率运行。实验同样证明了这项工作可以在具有挑战性的室内场所（例如具有重复外观的长走廊环境）实现精确的长期定位。

不妨再看一下展示我们实验结果的学术视频：

想要进一步了解？具体的学术论文可见于此链接。

https://arxiv.org/pdf/2203.00292.pdf

另外，MPL实验室近期发布了一个大规模环境数据集 ，由两台传统RGB相机，两台新型事件相机，一台深度相机，一台三维激光雷达，和一只惯性测量单元组成的传感器阵列采集而成。基于这项数据集所带来的便利，我们正在着手实施一项基于FP-Loc框架的拓展工作。我们计划实现所有上述提及的被动增量式传感器作为数据输入，并允许不同传感器模块之间动态切换；将环境信息的地图表达形式得以拓展，例如建筑图纸、稠密三维点云、或是基于网格的三维平面表征等等；同时保留先前定位工作中所具备的最优化、无漂移、高效轻量等特征。在实际生活中，环境信息的先验知识并不难获得，我们相信这项解决方案将会给学术界和工业界带来现实价值。如果您有兴趣资助我们或者与我们合作，请不要犹豫，速速联系我们！