Qin, Tong, Yuxin Zheng, Tongqing Chen, Yilun Chen, and Qing Su. “A Light-Weight Semantic Map for Visual Localization towards Autonomous Driving.” In 2021 IEEE International Conference on Robotics and Automation (ICRA), 11248–54. Xi’an, China: IEEE, 2021. https://doi.org/10.1109/ICRA48506.2021.9561663.
1 Introduction
本文的贡献：
- 提出一个新颖的面向无人驾驶的轻量级定位架构，该架构包括车载制图、云端地图维护，以及用户终端定位；
- 提出一个新颖的想法：让具有丰富传感器的车辆来辅助低成本量产汽车，具体途径为让具有丰富传感器的汽车每天收集数据并自动更新地图；
- 利用真实场景实验验证了该系统的可行性。
3 System Overview
系统架构如Fig. 2所示，该系统包含三个部分：
- 车载制图：汽车装备有前向摄像头、RTK-GPS，以及一些基础导航传感器（IMU、轮速记等）；利用分割网络从前向摄像头获取的图像中提取语义信息，然后基于优化后的汽车位姿将语义信息投影至世界坐标系，由此车辆本地可构建一个局部的语义地图，然后被上传至云端服务器。
- 云端制图：将众多局部地图融合为一个全局地图，然后该全局地图经边界提取进行压缩，最终，压缩后的全局地图被分发至终端用户。
- 终端定位：终端用户为量产汽车，装备有低成本传感器，如相机、GPS、IMU 及轮速记等；与车载制图步骤一样，终端用户从本地的车载摄像头获取的图像中提取语义信息；终端用户将下载的语义地图进行解码，然后根据语义特征匹配进行定位。
4 On-Vehicle Mapping
4.1 Image Segmentation
利用CNN 网络进行语义分割，分类结果中使用地面、车道线、停止线、道路标志信息进行语义制图，语义语义分割结果如Fig. 3所示：
4.2 Inverse Perspective Transformation
获取到语义信息之后，在汽车坐标系下，将语义像素点从图像平面逆投影至地平面，该过程也被称为 Inverse Perspective Mapping（IPM）。相机内参和相机到车辆中心的外部转换矩阵已经提前标定好了。由于观测噪声，越远的地方误差越大，所以作者只选取Region Of Interest（ROI）区域的像素进行IPM，如Fig. 3（a）所示。
假设地面是个平面，语义像素点投影至地平面（z 等于0）的IPM 过程如下所示：
其中，$\pi_c()$ 是相机的畸变+投影模型，$\pi_c()^{-1}$ 是逆投影过程；$[\mathbf{R}_c, \mathbf{t}_c]$ 是每个相机相对于汽车中心的外参矩阵；$[u, v]$ 表示图像坐标系中的像素坐标；$[x^v, y^v]$ 表示特征在汽车中心坐标系下的位置；$\lambda$ 为尺度参数；$()_{col:i}$ 表示选取矩阵的第 i 列。IPM 的过程如Fig. 3（c）所示。
4.3 Pose Graph Optimization
为了构建地图，需要获取车辆精确的位姿信息。为了减小在GNSS 拒止环境下里程计的漂移，作者使用位姿图进行优化，如Fig. 5所示：
位姿图优化过程可建模为：
其中，$\mathbf{s}$ 为汽车位姿信息（位置和朝向信息）；$\mathbf{r}_o$ 表示里程计因子的残差；$\hat{\mathbf{m}}_{i-1, i}^o$ 表示里程计观测信息，其中包含相邻两个状态间的位置变化 $\delta \hat{\mathbf{p}}_{i-1, i}$ 和朝向变化 $\delta \hat{\mathbf{q}}_{i-1, i}$ ；$\mathbf{r}_g$ 表示里程计因子的残差；$\mathcal{G}$ 为GNSS 良好观测区域的状态组；$\hat{\mathbf{m}}_{i}^g$ 表示GNSS 观测信息，即全局坐标系下的位置信息 $\hat{\mathbf{p}}_i$ 。
残差因子的定义如下所示：
其中，$\mathbf{q}$ 为四元数表示的朝向信息；$\mathbf{R}(\mathbf{q})$ 表示将四元数转换为旋转矩阵；$[]_{xyz}$ 表示取四元数的前三个元素，近似等于流形上的误差扰动。
4.4 Local Mapping
基于位姿图优化获取的位姿，将语义特征从汽车坐标系转换至全局坐标系：
对于语义特征，每一个像素点代表了真实世界中的一小块区域，随着汽车移动，该区域会被观测到多次；然而，由于分割噪声的存在，该区域可能会被识别为不同的种类，为了减少噪声的影响，作者使用统计数据来滤除噪声：作者将地图分为 $0.1 \times 0.1 \times 0.1 m$ 的网格，每个网格包含位置、语义标签以及每种标签的计数信息，随着观测的增多，每个网格内不同标签的计数逐渐变化，最终选取计数最多的标签作为该网格的语义类别，作者通过这种统计策略来增强鲁棒性。全局制图的结果如Fig. 6（a）所示：
5 On-Cloud Mapping
5.1 Map Merging / Updating
为了节省带宽，只将局部地图的占据网格信息上传至云端，网格的分辨率同样是 $0.1 \times 0.1 \times 0.1 m$ ，值得注意的是，局部地图的网格被加到全局地图上时，会将标签的计数信息同时加上去，并根据计数信息来更新当前网格的语义标签。更新过程如Fig. 9所示：
5.2 Map Compression
为了节省带宽，需要进一步压缩语义地图。作者使用边界提取方法来进一步压缩语义地图：
- 首先，产生语义地图的顶视图，每个像素代表一个网格；
- 然后，提取每个语义群的边界；
- 最终，保存边界点然后分发至各个终端车辆。
该过程如Fig. 6所示。
6 User-End Localization
终端代表的是量产汽车，装备了低成本传感器。
6.1 Map Decompression
终端汽车将边界点地图解压缩为语义地图：在边界内填充相同语义标签的像素。
6.2 ICP Localization
与制图过程类似，终端汽车通过CNN 网络提取前视摄像头捕捉图片中的语义信息，并投影至汽车坐标系中，然后当前汽车的位姿通过语义特征匹配进行获取。这里，作者使用ICP 方法来进行位姿估计：
其中，$\mathbf{q, p}$ 分别是当前帧的四元数和位置参数；$\mathcal{S}$ 表示当前帧中的语义特征点；$[x_k^v, y_k^v, 0]$ 表示语义特征在汽车坐标系下的坐标；$[x_k^w, y_k^w, 0]$ 表示世界坐标系下地图中最近的点坐标。
最终，作者采用EKF 架构将里程计和视觉定位结果进行融合，得到鲁棒、顺滑的轨迹。
7 Experimental Results
在定位精度测试中，作者使用RTK GPS 作为真值，对于自动驾驶任务，只解算 $[x, y, yaw]$ 信息，作者将本文方法与基于Lidar 的方法进行比较，结果如下所示：
