武汉大学开源RoadLib：基于道路标识的增量式SLAM！

引言

端到端的自动驾驶系统目前已经成为行业的一个趋势。通过将感知、规划、控制等模块整合到统一的端到端网络体系下，一定程度上可以克服单一模块的信息损失，有望用数据驱动的方式解决传统算法在corner case方面遇到的挑战。

在这样的背景下，基于特定特征的轻量级道路地图依然有其应用价值 。通过低成本终端或者众包的方式对地图进行构建和维护，可以在用户端作为额外的信息源，增强其高精度定位或者超视距感知的能力，从而提升系统的稳健性和高效性。

在此介绍RoadLib，一个开源的基于道路标识的增量式建图与定位系统 ，通过对单目图像中的道路标识（实线、虚线、指示标识、停车线等）进行实例级建模并充分考虑其空间不确定性，高效构建大尺度范围内具备一致性的轻量级语义地图，并支持多地图融合和高精度的地图匹配定位。本开源项目主要基于本团队之前的工作[1]；在此基础上，本项目增强了 道路标识几何建模、增量式建图、地理注册 方面的功能，提高了其实用性。该系统具有以下的特性：

• 通过二维语义分割与逆透视变换（IPM）提取三维道路标识并进行矢量化的实例级建模。

• 考虑道路标识实例的空间不确定性进行概率融合，实现一致性的局部地图构建。

• 使用绝对位置对局部地图进行地理注册，并支持多地图的概率融合。

• 使用滑动窗口和迭代的实例级匹配进行用户端的地图定位。

尽管在轻量级道路地图方面已经有很多发表的工作[2-5]，但是目前缺少比较完整的开源解决方案。本项目提供了一个开源的轻量级道路建图与定位系统，支持了 感知-建图-定位 的pipeline和一些基本的算法实现，希望可以给大家的研究提供一个参考。区别于端到端的在线矢量地图生成方法[6-7]，本系统聚焦于以定位为导向的道路特征地图，并提供了相对更加完整的流程。

工作实现

算法说明

算法部分的主要原理和框架可以参考我们发表的论文[1]；同时，我们在在原有算法上进行了一定的改进，在此加以说明。

算法部分主要包括 道路标识提取与建模 、 局部增量式建图 、 地图管理 与 地图匹配定位 等模块。

道路标识提取与建模

首先，使用二维语义分割网络，生成车载相机的 道路标识语义分割图像 。在此基础上，使用逆透视变换（IPM）将语义图像转换到鸟瞰视角。IPM的使用基于已知的 相机-地面几何 （相机高度、相机与地面的夹角），需要加以预先标定[8-10]。

注意到，由于语义分割以及IPM条件的限制，鸟瞰图上的要素其实并不完全精确，而是具有一定的空间不确定性。造成 空间不确定性/几何误差 的因素包括：1）图像本身/语义分割造成的像素误差；2）相机俯仰角造成的IPM误差；2）相机距离地面高度造成的IPM误差。针对这些因素，我们假定了典型情况下的误差量级，并通过误差传播得到鸟瞰图上任意位置的空间不确定度。此外，针对于车辆颠簸、道路坡度显著变化等情况，算法会采取IMU姿态高频补偿、坡度变化检测与粗差剔除等方式，尽可能获取精确的空间几何要素。

基于变换后的鸟瞰视角语义图像，对不同类别的道路标识进行像素聚类，并对聚类后的像素块进一步参数化。根据其语义类别，我们将道路标识特征分为 块状特征（虚线、箭头等） 与 线状特征（实线、停车线等） ：对于块状特征，我们将其建模为一个参考点，一个主方向，一个包围框，以及四根框线距离的不确定度；对于线状特征，我们将其建模为多段线，以及多段线端点的不确定度。至此，语义像素图转化为了一系列离散的轻量级道路特征实例，大大减小了数据量，便于后续的处理。

局部增量式建图

基于载体的相对位姿估计（由VIO、轮式里程计等获取），我们可以将提取出的道路特征实例根据位姿投影到局部地图坐标系中。在此过程中，对于道路标识的多帧观测将会在空间中叠加起来。为了保持地图要素的一致性并提高其几何精度，我们 将道路标识实例进行聚类并加以融合 。

具体来说，对 块状特征 而言，使用线框的交集检查对其进行聚类，并进行基于不确定性线框集合的优化。需要特别注意的是，语义分割的结果受到物体或者视野遮挡的影响，可能会导致不完整的块状特征观测，在融合优化的过程中需要考虑这部分错误信息并加以剔除。

对于 线状特征 而言，使用邻近点的绝对距离、垂向距离以及局部方向对多帧的线状特征进行聚类，根据聚类的空间范围对融合后线状特征的端点进行扩展，并最小化多帧观测到的三维采样点到线状特征的距离，得到优化后的新线状特征。为了维持线状特征的基本形态并抵抗粗差观测的影响，我们在上述的优化中对线状特征施加一个曲线平滑性的约束。

地图管理

局部建图基于里程计位姿进行构建，其结果存在不可避免的漂移，并且没有绝对位置信息，无法进行多地图融合或提供给用户端使用。为了满足实用需求，需要对地图进行地理注册， 纠正地图的偏移并将其放置到绝对坐标系下 。

地理注册的过程需要使用到高精度的GNSS信息，但是对于实时性没有很高的要求。基于间断可用的高精度GNSS位置，我们可以将其与车辆里程计提供的相对位姿构建全局位姿图，优化得到全局一致的相对高精度的绝对位姿。利用这个无漂移的绝对位姿信息，我们可以在局部地图中对每一个位姿关联的道路特征进行纠正，并将其自然地转换到绝对坐标系下。

完成地理注册后，局部构建的地图到了统一的地理坐标系下；我们也可以将 多个时段、多个载体 采集的地图进行基于不确定性的融合，融合的过程与局部增量式建图中的聚类/融合方式基本是一致的，但在聚类条件方面略有差别。

地图匹配定位

一旦建立了轻量级的道路特征地图之后，用户可以从云端获取地图并用它来进行匹配定位。

基于道路特征地图的匹配定位分两步， 第一步是建立正确的匹配关系 。在此，初始位置的确定和初始匹配的建立是难点，也较为重要。（ 在此次开源的项目中，我们没有考虑这部分内容，直接用初始时刻的近似位姿进行初始化。 ）一种可行的方案是，由GNSS来提供粗略的初始位置与航向，并结合空间分布模式搜索来实现初始定位。具体来说，在10m左右的初始定位误差下，观测到箭头这种分布相对分散的特征，基本上就可以实现亚米级的初始定位，如下图所示。在位置初值较好的情况下，对用户观测到的特征进行空间最邻近搜索，综合考虑线状特征方向/距离、块状特征尺寸/方向/类别等方面的因素，实现实例级的数据关联。

一旦建立了匹配关系之后，我们使用 滑动窗口的因子图进行用户端的位姿优化 。因子图中主要包含里程计因子和地图匹配因子。其中，里程计因子提供相对位姿约束； 地图匹配因子统一构建为点到线的距离 ——对于线状特征而言，优化用户端特征采样点到地图中线状特征局部线段的距离；对于块状特征而言，优化用户端观测到的包围框采样点到地图中块状特征四条框线的距离。总的来说，车道线实线通常提供横向的位置约束，而车道线虚线、停车线、箭头等道路标识提供纵向的位置约束。

考虑到道路特征检测以及匹配过程中可能存在的粗差，我们维护一个较长的滑动窗口（如10秒）联合多帧的观测进行位姿估计，并使用Cauchy鲁棒核对地图匹配因子进行处理；在优化过程中，会迭代地更新道路特征的数据关联，以实现较为精确和鲁棒的位姿估计。

数据采集

在数据采集方面，我们搭建了自己的数据自动化采集平台，通过GNSS接收机产生的秒脉冲（PPS）触发/同步各种传感器的数据，储存下来的数据记录打上GPS时间系统的时间戳；采集平台可集成GNSS、相机、激光雷达、惯导、毫米波雷达等多种传感器，适用于小车、乘用车等多种平台。

在参考真值方面，我们会在数据采集开始前在开阔环境架设参考基站，用于进行差分GNSS处理；在数据采集完成后，我们使用商用GNSS/INS后处理软件Inertial Explorer（IE）对GNSS与战术级惯导数据进行双向平滑的PPK/INS紧组合解，从而获取相对精确可靠的高频参考位姿。

数据标注与训练

本系统的感知模块依赖于道路标识的二维语义分割；目前开源数据集中，带有道路标识语义标注的数据较少，具有代表性的是Apolloscape。

为了实现武汉市区典型道路场景的建图，我们在多时段、不同终端采集的车载图像中，选取600张使用labelme进行了语义标注。在语义分割网络方面，我们使用了MMSeg提供的SegFormer实现。（如果对于实时性有更高的要求，可以考虑使用PP-LiteSeg等轻量级网络，效果都能满足需求）。

本仓库提供了SegFormer的模型文件和运行脚本；如果需要获取数据集进行自行训练，可以联系我的邮箱（[email protected]）。

效果展示

本系统的建图模块运行效果如下图所示。

定位模块运行效果如下图所示。

将地图匹配定位的结果与单点定位（SPP）进行对比，结果如下图所示。可以看到，通过融合VIO提供的相对位姿以及地图匹配信息，可以实现稳定的分米级定位。

开源仓库

RoadLib的仓库链接如下：https://github.com/GREAT-WHU/RoadLib

欢迎大家去提issue！

视频展示

我们在Bilibili上传了关于本开源项目的预览视频，传送门如下：

https://www.bilibili.com/video/BV1bp42117N1

https://www.bilibili.com/video/BV1dS421d71n

团队实验室介绍

武汉大学GREAT（GNSS+ REsearch, Application and Teaching）团队 长期从事卫星精密定位定轨与多源融合导航方面的研究与教学工作。团队成员有 李星星教授 ，李昕、袁勇强、张柯柯、郑玉新、冯绍权、廖健驰等青年教师，以及三十余名博士和硕士研究生。

主要研究方向及成果包括：1）以PPP-RTK为代表的实时精密定位，可生成并提供多频多系统实时精密轨道、实时钟差、实时UPD以及实时精密大气产品，支持星地一体化增强的快速精密定位；2）基于因子图和滤波的GNSS、视觉、激光、惯性以及高精地图等多源信息紧融合算法，以及相应的软硬件一体化终端设备；3）低轨导航增强以及在观测值层面的多种空间大地测量技术（GNSS/SLR/VLBI）联合解算。

近年来，团队在国际大地测量与地球科学、智能交通、机器人等领域权威期刊发表高水平 SCI 论文 170 篇，Google Scholar 总被引 8300余次。入选爱思唯尔中国高被引学者和全球2%顶尖科学家榜单。承担国际合作、国家级、省部级以及产学研合作项目三十余项，相关成果获得广泛的应用。先后荣获湖北省自然科学奖一等奖、教育部科技进步奖一等奖、国际大地测量和地球物理学联合会（IUGG）青年科学家奖等奖项。

目前，团队已经开源GNSS高精度定位（PPP/RTK）算法（ GREAT-PVT ）、基于连续时间的多传感器时空标定（ iKalibr ）、道路标识建图与定位（ RoadLib ）、深度光流与多传感器融合建图（ DBA-Fusion ）等代码项目，欢迎大家关注我们的仓库！

参考文献

[1] Y. Zhou, X. Li, S. Li, and X. Wang, “Visual mapping and localization system based on compact instance-level road markings with spatial uncertainty,” IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 10802–10809, 2022.

[2] T. Qin, Y. Zheng, T. Chen, Y. Chen, and Q. Su, “A Light-Weight Semantic Map for Visual Localization towards Autonomous Driving,” in 2021 IEEE International Conference on Robotics and Automation (ICRA), May 2021, pp. 11248–11254. doi: 10.1109/ICRA48506.2021.9561663.

[3] T. Qin, T. Chen, Y. Chen, and Q. Su, “AVP-SLAM: Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct. 2020, pp. 5939–5945. doi: 10.1109/IROS45743.2020.9340939.

[4] J. Jeong, Y. Cho, and A. Kim, “Road-SLAM : Road marking based SLAM with lane-level accuracy,” in 2017 IEEE Intelligent Vehicles Symposium (IV), Jun. 2017, pp. 1736–1473. doi: 10.1109/IVS.2017.7995958.

[5] Z. Qiao, Z. Yu, H. Yin, and S. Shen, “Online monocular lane mapping using catmull-rom spline,” in 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE, 2023, pp. 7179–7186.

[6] Y. Liu, T. Yuan, Y. Wang, Y. Wang, and H. Zhao, “Vectormapnet: End-to-end vectorized hd map learning,” in International Conference on Machine Learning, PMLR, 2023, pp. 22352–22369. Accessed: Oct. 04, 2024.

[7] B. Liao et al., “Maptr: Structured modeling and learning for online vectorized hd map construction,” arXiv preprint arXiv:2208.14437.

[8] Y.-L. Chang, L.-Y. Hsu, and O. T.-C. Chen, “Auto-calibration around view monitoring system,” in Proc. IEEE 78th Veh. Technol. Conf. (VTC Fall), Las Vegas, NV, USA, Sep. 2013, pp. 1–5.

[9] B. Li, X. Du, Y. Hu, H. Yu, and W. Zhang, “Online camera-to-ground calibration for autonomous driving,” 2023, arXiv:2303.17137.

[10] Y. Zhou, X. Li, S. Li, X. Wang, and Z. Shen, “Ground-VIO: Monocular visual-inertial odometry with online calibration of camera-ground geometric parameters,” IEEE Transactions on Intelligent Transportation Systems, 2024