【论文精读】AVP-Loc: Surround View Localization and Relocalization Based on HD VectorMap for AVP
【论文精读】AVP-Loc: Surround View Localization and Relocalization Based on HD VectorMap for Automated Valet Parking
论文出处
IROS2021
摘要
定位是自动代客泊车的关键前提，汽车需要在无gps信息的停车场自动导航，传统的视觉定位方法通常会构建一个特征地图，并将其用于后边的定位。然而，特征地图对于光照、纹理和观察视角不够鲁棒。为了处理这个问题，我们需要一个更加稳定的地图。本篇文章，我们直接使用停车场的高清矢量地图进行定位，这种矢量表现形式很稳定但也在数据关联中带来了挑战。为此，我们提出了一种新的数据关联方法来匹配环视图像和高清矢量图，此外，我们还提出了一种利用矢量图中独特的道路标志组合来进行封闭形式重定位的策略，实验表明，该方法能够在多层停车场中实现厘米级的定位精度。
贡献
- 一种基于语义分割的环视图与高清矢量图相匹配的异构数据关联方法
- 一种利用分布在矢量图之间的独特道路标志组合的封闭形式重定位策略
- 一个基于全环绕视野的在无单独定位层停车场下实现厘米级定位精度的定位系统
高清矢量图：矢量图由一组语义交通元素的矢量化三维形状组成，如停车线、车道线、箭头和减速带。
方法
定位
A.BEV的语义分割
我们利用逆透视变换从四个环视鱼眼摄像头中构建一个BEV图像，BEV图像的像素 [ u , v ] T \mathbf{[u, v]}^{T} [u,v]T和它在车辆坐标系中的位置 [ X , Y , 0 ] T \mathbf{[X, Y, 0]}^{T} [X,Y,0]T的关系如下：
其中 w , h w,h w,h 是图像的宽和高， s s s 是像素比例，BEV中的每个像素在车辆坐标中占据一个 2 c m 2 \mathbf{2cm}^{2} 2cm2 的空间。我们使用 640 × 872 640×872 640×872 图像大小，在真实世界中覆盖的 ROI 为 12.8 × 17.44 m 2 12.8 × 17.44\mathbf{m}^{2} 12.8×17.44m2。
为了关联图像和矢量图之间的异构数据，我们训练了一个卷积神经网络，将BEV图像转换为语义分割图，我们在这里使用的语义类是停车场中不同类型的道路标记，例如，车道线、停车线、箭头、文本、减速带、斑马条、仪表板段。在标签定义中，我们区分了水平和竖直的停车线w.r.t.和停车场的入口方向，这种区分使我们在横向和纵向方向上有更好的位置约束，我们使用 Pyramid scene parsing network 来训练这个网络。
B.矢量图匹配
我们现在将BEV图像的语义分割图与高清矢量图进行匹配。与AVP-SLAM中将帧中每一个特征与对应的特征地图相匹配的方法相反，我们用BEV语义分割图中的语义特征点与每个矢量段相匹配。
直接使用ICP很慢，还会带来歧义，而且直接的ICP很慢。因为前景语义特征集肯包含多达10万个点，索引很慢，而且BEV图中40cm宽的减速带在矢量图中被表示为一个线段，BEV图中箭头是一个连接区域，但矢量图只存储其边界。
在匹配前对BEV分割图像进行预处理。细线降采样，离散道路标记转换成轮廓线；减速带投射到共线点上，每个绿点代表一个与特征点匹配的矢量段。
一种类匹配策略：
- 对于地图中以多边形表示的离散道路标线（如斑马带，箭头），我们将多边形中的每个段与语义实例的轮廓像素进行匹配。
- 对于地图中以多条线段表示的连续细道路标线（例如，车道线、停车线），我们对这些类中的语义像素进行一个数量级的降采样，并将每个矢量段与降采样集进行匹配。
- 对于地图中以多条段线或一条线段表示的连续的粗道路标线（例如，减速带），我们将其轮廓像素投影到轮廓集的主PCA轴上，并将每个矢量段与这个投影集相匹配。
不同匹配策略的目的是为不同的语义类型提高一个更好的匹配模型，并加快匹配过程。
- 将矢量形状分成一组一米的分段，并将这些分段的中点与像素相匹配，使空间能够被均匀地覆盖。
- 最多有一个像素可以与一个给定的一米的线段相匹配。
- 只有具有相同语义标签的段和像素可以相互匹配。
C.ESKF状态估计
我们使用ESKF进行状态估计，旨在估计真实的IMU数据，用来替代世界坐标系下的位姿，速度，自身坐标系到世界坐标系下的四元数、加速度和陀螺仪偏差。
ESKF的更新使用了两种信息来源：
1. 矢量图的匹配
给定像素坐标系和世界坐标系中点到段的匹配 ( p , c i c i + 1 ) (p, cici+1) (p,cici+1) ，在世界坐标系下矢量图的观测模型对点到线的距离施加惩罚：
其中 x W \mathbf{x}^{W} xW 和 x V \mathbf{x}^{V} xV 是世界坐标系和车辆坐标系下的匹配点。 T V I \mathbf{T}_{V}^{I} TVI 表示车辆系到 IMU系的变换，车辆坐标系下的 x V \mathbf{x}^{V} xV 是使用等式1从输入像素 ( p u , p v ) (\mathbf{p}_{u},\mathbf{p}_{v}) (pu,pv) 计算出来的，所有点到段的匹配通过等式3的叠加来形成一个残余向量。
2. 轮速计
给定一个车辆系下记录的轮速 v V \mathbf{v}^{V} vV，并在相同的时间戳下插入一个IMU的角速度 ω \omega ω，要求在IMU状态下，车速观测模型的速度分量与车轮编码器的测量结果一致。
等式的两边都表示IMU坐标系下车辆系原点的速度，右侧来自车轮轮速的读数，左侧是由IMU状态向量计算出来的，并施加了杠杆臂补偿，其中 t I V \mathbf{t}_{I}^{V} tIV 应解释为车辆系在IMU系下的起始位置。
重定位
A.独特的道路标记选择
我们用一个3元组 ( p i , θ i , c i ) (\mathbf{p}_{i},\mathbf{\theta}_{i},\mathbf{c}_{i}) (pi,θi,ci) 来表示每个离散的道路标记实例，组内元素分别表示各自的质心位置，PCA计算出的方向和类标签。每个离散的道路标记都给出一个索引 i ∈ N i\in N i∈N。我们用一组道路标记索引来表示一个地标 α \alpha α。如果在半径 R 内没有其他类似的地标，则认为地标 α \alpha α 在半径 R 上是独特的。如果存在一个刚体变换，使两个3元组对齐，则这两个地标相似，但是这个定义需要求解刚体变换，导致了不必要的计算。为了便于相似性检测，我们将刚性变换检测转换为“空间特征”检测。在这个检测中，地标 α \alpha α 和地标 β \beta β 是相似的，当且仅当
其中 γ \gamma γ 是 β \beta β 排列中的一个元素。 n n n 是每个地标中元素指数的数量。 α i , γ i \mathbf{\alpha}_{i}, \mathbf{\gamma}_{i} αi,γi 分别表示每个地标中的第 i 个元素索引。 τ p = 0.2 m \mathbf{\tau}_{p} = 0.2m τp=0.2m, τ θ = 5 ° \mathbf{\tau}_{\theta} = 5° τθ=5° 为相似性阈值。等式6到9表示每个地标内的相对距离和方向（即空间特征）是否相似。
我们选择减速带、箭头和仪表段作为基本的离散道路标记进行组合。仪表板段不能单独使用，但可以参与增强一个组合，以获得更好的独特性。在实践中，我们为一个地标分配至少一个和最多三个离散的标记，每个地标也与它独特的半径相关联。注意，虽然等式6-9的相似性实验涉及到排列，但不会导致任何组合爆炸问题，因为一个地标最多包含三个实例，并且只需要排列具有相同标签的实例。在实践中，一个现实世界的地标中，最多有两个实例共享相同的标签，这只会导致两个排列结果。
B.漂移检测
只有在检测到漂移时，才会触发重定位模式。
在正确的结果中，每个从BEV图像中提取的语义特征点在矢量图中都有一个相匹配的矢量段。当发生漂移时，许多语义特征点无法找到它们所匹配的向量，这种特征点被称为孤立特征点。选择孤立特征点的比值作为漂移检测度量。如果孤立比在连续几帧都低于预定义阈值，则认为发生了漂移。
一帧的孤立比 r r r 被定义为：
其中 S S S 是语义特征点的集合， L L L 是参与矢量图匹配的线段的集合。 d ( s , l ) d(s,l) d(s,l) 表示点到线的距离， T m \mathbf{T}_{m} Tm 是匹配距离的阈值。为了节省算力，我们将 S 设置为定位中矢量图匹配所描述的特征点降采样集。为了加速求解 d ( s , l ) d(s,l) d(s,l)，只保留实际找到的最接近的匹配线段。我们为这些段的中间点创建了一个八叉树索引，这样 m i n l ∈ L d ( s , l ) \mathbf{min}_{l}\in\mathbf{L}^{d(s,l)} minl∈Ld(s,l) 可以通过一个最近邻搜索计算。
C.封闭式的重定位初始化
重定位是通过将局部地标候选物与矢量图中的对应物进行匹配来实现的。
-
局部地标检测
从定位的矢量图匹配中我们已经得到了一个BEV分割的离散元素轮廓，其中每个轮廓代表离散标记的一个实例。我们在局部坐标系中使用 PCA 计算每个实例的3元组表示 ( p i , θ i , c i ) (\mathbf{p}_{i},\mathbf{\theta}_{i},\mathbf{c}_{i}) (pi,θi,ci)。在此之后，我们通过自由组合3元组形成一个地标候选集，并去除候选集中非标准大小的实例。 -
局部-全局地标匹配
给定一个局部地标候选集 L s \mathbf{L}_{s} Ls 和全局地标候选集 L M \mathbf{L}_{M} LM, 返回一个正确的匹配。对于一个局部地标候选集 l s \mathbf{l}_{s} ls，我们要求地图返回与 l s \mathbf{l}_{s} ls 共享相同“标签特征”的地标。一个地标的标签特征被定义为带有三个标签 {减速带、箭头、仪表板段} 的频率直方图。返回的地标集合列表 { L 1 , . . . , L n } \{\mathbf{L}_{1},...,\mathbf{L}_{n}\} {L1,...,Ln}, 它们都共享相同的标签特征且集合中的每一个元素 L i ∈ { L 1 , . . . , L n } \mathbf{L}_{i}\in\{\mathbf{L}_1,...,\mathbf{L}_{n}\} Li∈{L1,...,Ln} 都有自己的空间特征。显然，列表中最多有一个集合将匹配 L s \mathbf{L}_{s} Ls 空间特征的查询。匹配的集合被表示为 L ∗ \mathbf{L}_{*} L∗，需要确定 L ∗ \mathbf{L}_{*} L∗ 中的哪个地标是与 L s \mathbf{L}_{s} Ls 真正匹配的。如果 L ∗ \mathbf{L}_{*} L∗ 只包含一个地标，则完成了匹配。否则，我们就利用距离信息来区分相同的地标。
为了解决跟踪丢失，状态估计器输出不可信的问题，我们维护一个单独的卡尔曼滤波器，只使用IMU和轮速计的跟踪里程计，当没有漂移时，滤波器将定期重置，使其协方差保持较小且有界，当漂移发生时，滤波器为我们提供了一个来自其均值和协方差的不确定性椭圆。
为了解决匹配过程没有区分不同楼层停车场相似地标的问题，我们维护一个z轴估计器来判断我们处于哪个楼层。包括两种解决方案：
1.在漂移发生之前使用Z轴估计器，通过与矢量图的车道拓扑进行大致匹配，漂移发生时里程计轨迹xy方向的分量可以用来确定车辆是上楼还是下楼。
2.使用一个气压计，可以给出一个1米精度的z轴估计器。
-
封闭式重定位初始化
现在我们得到了两组匹配的三元组，记作 { p α i , θ α i , c α i } \{\mathbf{p}_{\alpha i},\mathbf{\theta}_{\alpha i}, \mathbf{c}_{\alpha i}\} {pαi,θαi,cαi} 和 { p β i , θ β i , c β i } \{\mathbf{p}_{\beta i},\mathbf{\theta}_{\beta i}, \mathbf{c}_{\beta i}\} {pβi,θβi,cβi}，我们计算一个三维刚体变换来对齐两个三元组。由于只需要估计4个自由度，一个3元组就足以计算变换：
对于包含多个实例的地标，我们为每个实例计算一个单独的 R，t，并将它们的均值作为最终重新初始化的姿态。该精度足以满足以下的非线性细化。
为了解决每个实例主要方向上存在一个符号歧义的问题，我们采用之前介绍的单独的卡尔曼滤波器。该解决方案是基于在滤波器中的姿态漂移小于90°的假设。在这个假设下，我们可以将每个主方向转换为世界坐标系，如果方向点与矢量图中的对应方向相反，我们可以将其翻转。在实践中，单独的过滤器将定期重置，以保持该假设永远成立。 -
非线性细化
使用最后一步粗略的重新初始化姿态，然后使用等式3中的点段对齐成本对其进行细化，这是一个可以用高斯牛顿方法来解决的非线性最小二乘问题，我们使用 Ceres 求解器来实现优化。
实验
A.测试环境
测试车辆平台包括4个鱼眼摄像头，1个IMU，1个轮速计。图像流和语义分割都在10Hz下运行。IMU和轮速编码器分别运行在100Hz和50Hz下。该算法可以在机载的Xavier计算机上运行。
高清地图是基于激光雷达的方法创建的。
B.定位评估
采用两个室内停车场序列来评估定位器的性能，停车场包括地下B2，B3，B4三层。第一个序列是一个多楼层序列，它从B4层的一个停车位开始，经过了B3层，最后返回了B4层的起点。第二个序列是内楼层序列，它从B4层的一个停车位开始，来回转两圈，最后返回同一个停车位。由于实验中使用的停车场不能使用GPS，我们选择了三种方法来评估定位精度。
1.将BEV轨迹和基于NDT算法的激光雷达的定位器估计的轨迹进行比较，使用后者作为伪真值，由于NDT表示和矢量图都是从同一个底层点云创建而来的，比较起来比较合理。平均平移误差在10cm以内，大多数的旋转误差都在3°以内。 该实验表明，我们的环绕视图定位器可以实现激光雷达定位器的精度，同时使用更小的矢量图。
2.我们利用估计的姿态将HD矢量图投影到BEV图像上，并评估地图投影与图像内容之间对应关系的距离。视觉上可区分的特征，如停车位角点，箭头角点被选择作为评价条件。当没有足够的角特征时，我们选择任意的车道线点，并回退到点到线的距离度量。由于BEV图像与物理世界之间的尺度差异是已知的，因此在BEV图像上的距离测量可以作为一个合理的评价指标。但是由于接近BEV图像边界时，相机的外参误差被放大，该实验倾向于输出一个悲观的带偏见的评价。
3.我们将车停在一个停车位多次并评估车和停车线之间的距离，比较定位器输出的距离和用磁带测量的距离。本实验中，该定位器达到了 2.23 厘米的精度，由于车辆周围的两条停车线为横向定位提供了非常强有力的线索，这个实验倾向于输出一个乐观的有偏见的评价。
算法和 AVP-SLAM 在准确性和地图尺寸上的比较。AVP-SLAM 的地图大小是从内楼层序列轨迹中累积起来的语义特征图计算而来，矢量图的尺寸对应该楼层序列的所有向量元素，比特征图小一个数量级。
消融实验：仅使用IMU和轮速计得到的里程计轨迹。
没有矢量图的约束下，里程计很快发生漂移。
C.重定位评估
为了评估重定位的性能，我们在一个好的姿态周围模拟受扰动的姿态，并测试了被干扰的姿态如何被拉回原来的姿态。
算法和AVP_SLAM在重定位性能上比较。
AVP_SLAM也利用了语义分割，它通过使用 ICP 对齐两个子地图来执行回环检测（重定位）。子地图是在三维图像中对附近分割结果的累积，由于基于子地图的重定位使用了ICP，因此它在很大程度上依赖于初始姿态的接近程度，如果初值不好就会陷到局部最小值里。本算法的重定位使用减速带和箭头组合作为地标，由于重新初始化是在封闭形式下完成的，因此无论噪声如何扰动，解都会收敛到相同的最小值。
点评
相对于AVP-SLAM的语义+ICP，本篇论文是语义+新的匹配策略，在和AVP-SLAM的对比实验中，可以看到地图尺寸有所减小，但定位精度变化不大（2.36cm-2.23cm，5.23cm-5.20cm）。由于ICP匹配严重依赖于初值，初值不好很容易陷入局部最小里，本篇论文提出的封闭形式重定位方法可以做到无论噪声如何扰动，解都会收敛到相同的最小值，对应到地图上就是可以回到原来定位的姿态。
论文链接
AVP-Loc: Surround View Localization and Relocalization Based on HD Vector Map for Automated Valet Parking | IEEE Conference Publication | IEEE Xplore