Wang, Huayou, Changliang Xue, Yanxing Zhou, Feng Wen, and Hongbo Zhang. “Visual Semantic Localization Based on HD Map for Autonomous Vehicles in Urban Scenarios.” In 2021 IEEE International Conference on Robotics and Automation (ICRA), 11255–61. Xi’an, China: IEEE, 2021. https://doi.org/10.1109/ICRA48506.2021.9561459.
Abstract
本文提出了一种考虑局部结构一致性、全局模式一致性以及时间一致性的DA 方法；同时，使用了一种滑动窗口因子图优化框架来融合里程计和数据关联信息。
1 Introduction
本文的贡献：
- 提出一种利用轻量级HD地图（不需要知道地图特征的高精度绝对高度信息）和视觉语义特征的精确、鲁棒的定位算法；
- 提出了一种考虑局部结构一致性、全局模式一致性以及时间一致性的DA 方法；
- 提出了一种视觉语义测量和里程计测量紧耦合的因子图优化框架；
- 进行了仿真和真实场景的实验来验证DA的效果和定位的精度。
3 System Overview
定位问题可表示为最大后验（MAP）估计问题：
上式的MAP可分为两步：DA以及基于DA的位姿估计问题：
因此，定位框架可分为四个组件：传感器和地图、探测、关联和优化，如Fig. 2所示：
- 系统的传感器包括：单目相机，IMU，两个里程计以及一个GNSS接收机。相机用来探测语义特征，IMU 与里程计用来提供局部相对运动估计，GNSS 接收机可提供一个当前位姿的粗略估计，用于系统的初始化。
- 探测层实现对图片中道路标志，杆状物，交通灯和指示牌进行探测。
- 关联层将图片中提取到的语义特征与HD 地图进行关联，该步骤可细分为5步：
- 1、在先验位姿附近产生候选项，将地图特征基于每个采样位置映射到图片中；
- 2、基于局部结构一致性进行粗数据关联，来寻找一个近似最优的采样位置；
- 3、利用考虑匹配数量、匹配相似度以及局部结构相似度的最优数据关联方法来实现最优的全局匹配；
- 4、进行连续帧间的特征跟踪；
- 5、使用temporal smoothing 来获取时间一致性的DA。
- 在优化层，利用位姿图优化来融合DA 和里程计测量的位姿估计。
4 Methodology
4.1 Semantic Features and Detection
考虑到容易获取、频繁出现、高效存储，以及天气、光照、视角和外观不变性，作者使用YOLOV3 来提取道路标志，杆状物，交通灯和指示牌作为语义提取特征：
- 指示牌：包含类别、表示探测置信度的得分，以及一个bbox；
- 杆状物：包含类别，表示探测置信度的得分，以及两个顶点；
- 道路标志：采样点。
4.2 Semantic Data Association with HD map
DA 是视觉定位系统中最具挑战性的问题之一。
根据前文所述，本文提出的DA 包含五个步骤，分别叙述如下：
4.2.1 Step 1
在里程计获取的先验相对位姿周围生成数个可能的位姿估计，根据每个位姿估计来将地图特征映射到图片平面：
其中 $P^m_i$ 表示第 i 个地图特征的位置。
4.2.2 Step 2
基于局部结构一致性进行粗数据关联，来寻找近似最优的可能位姿，以消除由较大先验位姿误差造成的误匹配。局部结构一致性指的是保持感知到的特征和相应的地图映射特征的横向位置分布保持一致。
首先，将感知到的和映射过来的特征根据横向位置以降序排列；然后计算每个感知特征 $s_t$ 和每个映射特征 $r_k$ 的相似度：
此处可参考文章(Bowman 等, 2017)，上式的前两项分类与得分相似度可通过感知结果的离线学习来获取；最后一项需考虑不同的语义种类，对于指示牌，该项包含位置与尺寸相似项：
杆状物的似然包含位置、朝向以及重叠的相似项。
如果一个感知特征的最大相似度得分大于一个阈值，且局部特征被保留了，那么这就认为是一对成功的数据关联；对于每个可能的位姿计算相应的损失函数C，包含匹配数量 $N_m$ 和匹配误差 $e_{ii’}$ ：
其中 $e_{ii’}$ 被定义为特征 $i, i’$ 之间的横向距离，如Fig. 3所示：
最终，C 值最大的位姿作为近似最优位姿。
4.2.3 Step 3
基于上步得到的近似最优位姿，考虑匹配数量、匹配相似性和局部结构相似性来实现全局匹配，这被建模为一个多阶图匹配问题：
其中，N 和 M 分别表示感知到的和映射的特征数量；$N_e$ 表示两组特征的边的数量；$x_{ii’}$ 表示感知特征i 是否与映射特征 $i’$ 匹配；$s_{ii’}$ 表示感知特征i 与映射特征 $i’$ 的相似度，由式（4）计算得到；$s_{ij, i’j’}$ 表示边 $e_{ij}, e_{i^{‘}j^{‘}}$ （如Fig. 3所示）之间的相似度：
4.2.4 Step 4：Feature tracking
本步骤建立连续帧间的特征关联，因为感知到的特征是静态的且保持局部结构，作者将该步骤建模为类似等式（7）的多阶图匹配问题。
4.2.5 Step 5：Temporal smoothing
本步骤构建连续帧中感知到的特征与地图特征之间的最优一致性匹配，当前帧的匹配正确性也许会与滑动窗口内的早先匹配结果不同，进一步地，如果当前帧发生误匹配，可以基于早先的匹配与跟踪进行矫正。
时间顺滑temporal smoothing 指的是：根据匹配权重 $D_{1:T}$ 和匹配置信度 $c_{t,i}$ 来获取与地图特征 $x^l$ 对应的感知特征 $s_i$ ：
其中，$I(s_i, D_t)$ 表示地图特征 $x^l$ 是否与感知特征 $s_i$ 相匹配；匹配置信度 $c_{t,i}$ 是根据特征和局部结构相似性来给定的：
若最优感知特征的累计置信度要远大于第二优，则该最优感知特征被视为地图特征 $x^l$ 的正确匹配，否则，地图特征 $x^l$ 被认为具有不确定匹配，且每个感知特征的匹配概率可以得到。本步骤可以区分确定匹配与不确定匹配，可以解决因奇异性造成的误匹配问题。
4.3 Pose Graph Optimization
等式（2）表示的位姿估计可以表示为先验概率与似然的乘积：
基于里程计测量 $z_{i,i+1}^o$ 和匹配特征对 $z_i^l$ ，作者构建了一个滑动窗口非线性最小二乘估计器来估计最近的 T 位姿，相较于常用的滤波器方法，优化方法可以解决非同步和延迟测量，并在相同计算资源下获得更高的准确度，优化目标表示为：
其中，每个误差项及其对应的信息矩阵（协方差矩阵的逆矩阵）被视为一个因子，每个状态变量被视为一个节点，因此，定位问题可被表示为Fig. 4所示的因子图：
误差项包含里程计误差 $e^o$ ，语义测量误差 $e^l$ ，以及地图误差 $e_j^m$ 。
其中，$[.]_0$ 表示向量中的第一个元素，此处的测量误差只考虑横向误差，来消除高度误差的影响和对于地图特征准确绝对高度的需求，如Fig. 5所示。
其中，$m_j$ 是第j 个地图特征的位置。
非线性优化问题可通过迭代进行求解，本文使用滑动窗口法在保证解算精度的前提下提高了计算效率。窗口之外的旧状态直接被舍弃，本文不使用边际化方法，因为边际化会累积线性误差，使得系统矩阵稠密化并造成死锁。虽然边际化方法可以使用旧数据来约束位姿，但使用地图特征作为先验可以充分约束汽车位姿。
