0%

Long-term map maintenance pipeline for autonomous vehicles:阅读笔记

前言

这是我阅读 Long-term map maintenance pipeline for autonomous vehicles 过程中记录的一些笔记。这篇论文主要是探索了怎么保持自动驾驶中特征地图的自动更新,提出了一个完整的流程来对特征地图根据每次行驶过程中采集的结果进行离线更新。

需要解决的问题

通常无人驾驶汽车在上路前通常需要预先建立地图,在行驶时根据建好的地图进行定位。目前业界还没有一个统一的标准如何制作和维护地图。由于存储以及传输速度等原因,很多人会选择采用特征地图。这类地图通常比较轻量,主要由一系列类型的特征组成,包括:立杆、交通灯、路标等等。要使用这类地图有个很大问题是:地图的有效性。由于环境的经常性变化,一周前建好的地图可能这周就失效了,因此需要持续性的对地图进行更新。

目前来说更新地图有两个思路:

  1. 往地图中不停加入新侦测到的特征
  2. 除了加入新特征以外,还包括去除地图中的无效特征

目前有很多方法可以在给定某些条件完成添加新特征和去除旧特征的其中一步,但鲜有方法能够不依靠定位算法独立实现,意味着这写算法都需要从定位算法获取数据匹配(匹配特征点)结果以及汽车的位姿。

这篇论文主要提供了一套能同时解决以上两种问题的方案。主要提出了一种基于层级结构的建图方法来存储检测以及匹配到的特征。主要包括三层:

  1. 先验地图层:这一层基于特征的用于定位的地图,我们会更新特征的属性并且去除无效的特征
  2. 新特征层:通用的定位系统通常包含两个模块:数据匹配(计算观测数据和已知地图的匹配)和状态估计(计算当前汽车的状态,主要是位姿以及速度)。其中数据匹配模块可以通过调整获得三种信息(成功匹配的特征、地图上有但没有匹配到的特征,观测中有但在地图中找不到匹配的特征),其中最后一个信息会被存储到新特征层来进行评估,决定它是否是有价值的新特征从而被加入到先验地图中。
  3. 传感器模型层:是 2D 栅格结构并且以汽车坐标系为中心。这个模型主要描述在汽车周围特征提取模块能够提取到不同特征的概率。

文章中提出的方法使用了不同的算法以及数据结构来存储、获取以及更新每一层数据。并且和定位算法相独立,只需要获取当前机器人位姿、匹配到的特征、匹配不到地图特征以及观测特征即可。

相关研究

这里作者主要列举了一些有关检测地图变化并在特征地图中进行增删特征的工作,分别有:

以增加特征来反应地图更新的方法:

  • Churchill & Newman 提出了一种叫 plastic maps 的地图来做长期导航。主要思路是:在每次定位失败时将当前的视觉经验添加进地图中作为额外的特征,这样接下来重新到这个地方的时候车可以依靠不止一次的经验来进行定位。
  • MacTavish: 和上一条类似也是在视觉定位中回收多次视觉经验,存储在一个层特征中,定位的时候选取最合适的经验来进行匹配

去除失效特征的:

  • Biber & Duckett:用多个离散时间片断来表示环境,对于每一类语义特征设定一个失效时间。
  • Rosen:给予特征的持续性采用一个概率来描述特征的有效时间
  • Hochadofer & Schlegel : 将覆盖相同观测区域的特征进行分组,每一组中根据各个特征的不确定度来确定它的大概信息量,并进行排名,将分数低的特征去除。

同时增加有效特征和去除无效特征:

  • Konolige & Bowman:根据 FramSLAM 提出一种基于视角的地图结构。这种方法可以在添加新特征的同时去除视角。主要是通过计算每个视角匹配次数的百分比然后去除最不经常使用(LRU)的视角。
  • Egger : 使用 3D surfel 来表示环境,使用 C-SLAM 算法来积分出新的节点以此代替已经改变的节点。

地图维护方法

这一部分主要介绍作者提出的用来更新基于特征的地图的解决方案,主要手段是去除已经消失的元素和添加新检测到的特征。流程总共涉及三个地图层。先验地图层表示之前建好的用来定位的地图,这一层主要以数据匹配算法获取的信息来进行更新。通过对这一层的维护,我们希望检测哪些特征已经消失以对其进行删除;新特征层表示了一个我们存储以及更新所有还没与现有地图元素进行关联的特征。另外,在另一个并行的处理中,当前观测还用来和这一层作比较,以此来识别重复观察的结果;传感器模型层和一个存储汽车周围激光雷达观测到的特征的栅格地图关联。

上述三个地图层中,先验和新特征层是用来分别从该层保持或者去除特征的,而最终这两层会通过优化合并成一个更新后的地图。

传感器模型层

如上文介绍,这一层主要表现传感器如何从周围环境感知到特征(的概率)。其主要由一个以车坐标系为中心的 2D 栅格。文中的试验中采用的 60mx60m 的栅格地图,因为他们所有的 VLP-16 基本上可以感知到车子周围 30 m 为半径的立杆和拐角。每次有一个特征的匹配或者丢失发生的时候,该位置对应的栅格值会进行更新。更新过程根据贝叶斯铝箔,每个栅格值表示该位置能观察到特征的对数概率值,更新方式如下:

更新的时候,我们将成功匹配或者丢失更新的值($l_c(v|z_t)$)分别为:0.7 和 -0.4

先验地图层

这一层是由一个定位地图组成。定位地图构建的方式是计算每个特征的可见性。每个特征的可见性由两个向量定义,这两个向量中的每个位置对应一个 0 ~ 360度 以内离散化的角度值。第一个向量包含了在一个特定角度下该特征能被观测到的最大距离;第二个向量表示在该方向下特征能被观测到的概率。向量的尺寸取决于角度离散化的分辨率。

上述两个向量中,其中的范围向量更新的条件是:有一个特征被匹配到了,并且检测到的特征的范围比该方向原先的值大。满足这个条件是范围向量该方向的值会更新为当前值。还有一种情况下范围向量的值会更新,需要满足三个条件:特征没被匹配到;汽车离特征的距离比范围向量对应角度的范围要小,并且汽车和特征中间没有任何阻碍物。此时汽车应该可以观测到特征但却没有,因此需要范围向量中对应角度的值。(怎么更新?)

第二个向量表示特征在某个方向下的可见性,具体表现为能被观测到的可能性。更上述类似,每个值更新有两种情况:被匹配到了;或者没有阻挡但却没被匹配上,更新的公式是:

其中,$l_{ct}$ 表示在当前时间传感器模型层的对应栅格为对数概率值。

数据匹配算法输出成功的匹配和没有被匹配的特征,但没有考虑特征是否被阻挡了。因为城市环境中有很多动态物品,例如行人、汽车等等。这些动态物品可能部分或者完全挡住用来定位的特征,并且阻挡效果可能是短期也可能是长期的。为了检查,没被匹配上的特征是否被阻挡,作者提出了以下方法:

以汽车的坐标系为中心维护一个 2.5D 的栅格地图。最初这个周围环境被分成 rxr m 和 nxn 个栅格。然后将点云数据转换到这个坐标系下并且投影到栅格地图上,每个栅格里面存着在当前位置的 3D 点中 z 轴的最大和最小值。然后我们计算其差值,如果高度差超过一个阈值,我们会考虑它作为一个障碍物。

当我们在栅格地图中获得了环境中的障碍物之后,我们进行 2D 激光苏的投影。所有激光束的原点相对障碍物栅格地图是固定的,并且是汽车坐标系的中心点。激光束投影算法生成一个类似 2D 激光扫描的结果,再每一个离散化角度上产生一个范围值。然后我们知道每一个特征离汽车的距离,因此通过这两个值(特征离汽车的距离,汽车在该方向上激光束投影的范围值)我们可以判断该特征是否被遮挡:在汽车坐标系中的特征坐标可以从直角坐标转为极坐标并且和激光束投影产生的结果比较。

通过确认障碍物,可以对地图特征的可见性由一个更准确的更新。算法 1 总结了在一个特定汽车位姿下更新可见性向量的过程。

通过不断更新可见性向量,我们可以从中选择那些已经消失的特征从地图中去掉。可见性通过下列式子进行评价:

其中,

在每一次生成行驶数据集时,计算每个特征的可见性区域差异。如果某一个特征的可见性下降则表示该特征不能被观察到。如果可见性下降的幅度超过一定阈值,则将它从地图中去除。在试验中,阈值设定为 12%。

新特征层

这一层存储了没和先验地图中特征匹配上的观测到新特征。这类特征的出现原因可能是环境发生了结构化变化或者它们之前被阻挡。在作者的试验中,在第一个月时他们将特征提取器高度最小值和建立地图过程的参数设为一样。在完成了地图的建立之后,他们放宽了特征检测算法的约束,因此可以提取到很多之前没被提取到的特征(因为在范围外)。并且原先检测到的特征也可以在更远的距离被检测到。

通常来说这些特征会被抛弃,因为它们没有和现有特征匹配上。在这篇论文,作者提出将这些特征存储在一个新特征地图中以存储这些最近的信息。当地位算法初始化,检测到的特征和先验地图中的特征没有匹配上是,将这些特征转换为全局坐标并存储到特征地图层中。

这些新的观测会用来和新特征层的特征已经比较,这篇论文里面作者使用了基于 ICP 的数据匹配已经直角坐标集群算法。这些信息需要用来评估每个特征的参数,并且最后建立一个图结构来优化新特征的位置。作者每周进行一次新特征层的所有特征进行一次直角坐标集群。为防当数据匹配算法失效时,在同一个标准差小于15cm的集群中的特征会被连接在一起。

以下作者使用两个预测器来评估的特征的质量从而判断它们是否被添加到定位地图中。

首先在每一个时间点,先计算特征以及汽车位子的几何关系。然后计算两个参数:

  • 在能够检测到特征情况下能够行进的距离:监视观测到某个特征点的位姿集合。如果某个特征能够在很长距离也被观测到,则提高它的重要性,因为这会提高他在定位算法中的左右(能够对更多的位姿进行更新)。

  • Concentration ratio(集中比例) = $\frac{max(df_n)}{\sum df_n}$:表现了特征的分布。对每个特征,先计算两个参数:跟它相距最远的特征距离,以及一定范围内跟其他特征的距离里,最后求两个参数的比例。这个比例越低,该特征所处位置的特征密度越高。假如比例接近 1,则这特征分布很稀疏。我们更看重分布稀疏的特征(因为在该区域比较独特)。

在新特征层中,作者挑选了距离汽车 1 米之外的比较能观测到的特征,对其进行构造图,节点为特征位置,边为机器人的位姿。然后对这个图进行优化。对优化后的特征,计算其集中比例,去除比例低于 0.4 的特征并将剩余的特征并入先验图层中。

地图评估方法

在这一部分中,作者利用三个组件来对上述提出长期建图方法进行分析和评估。分别是:

  • 数据库:数据包含了同一辆车在长时间进行重复路径生成的数据,并且包括一部分经过了结构性变化的区域;
  • 一个最初版本的定位地图:作为最初的基准
  • 定位算法:能够基于已有的传感器数据和先验地图计算出车子的全局位姿

以下一一进行介绍

数据库

  • 时间以及地址:18 个月,每周一次,在悉尼大学采集,数据库包括不同天气、光照、新建建筑物以及不同的行人和交通流量
  • 传感器(具体型号见论文):16 线激光雷达、照相机、IMU、四个轮速计、电位器(计算前轮转角)、GNSS

最初的特征地图

收集了数据库之后,作者使用了一个基于图的 SLAM 算法以及经过标签的点云来制作最初的特征地图。

  • 标记点云

这里作者主要是先用 CNN 对照相机结果进行语义识别,并将识别出来的信息从照片转换到点云(照相机和激光雷达的外参经过标定)。在转换的过程中,需要筛选出对相机不可见的点云。

  • 特征提取

有了标签化的点云之后,通过对其进行集合形状分析进行分类:在一定高度范围内的圆柱体分类为立杆;有垂直相交的平面中间的交线识别为转角。在城市环境中,直线通常是由建筑物的墙反映产生。另外在最初地图的构建中,特征识别的参数设置得比较严格,为了保证的最初提取的特征是静止的,以提高会换检测的成功率。圆柱的要求为:高于 1.8 m,直径小于 0.3 m;转角高于 1.6 m。

下图展示了点云以及从中构建的特征,每个特征从一组垂直排列的 3D 点投影到汽车平面形成一个 2D 点。每个特征包括:(x, y) 车坐标系下的坐标、高度、语义标签、特征类型、几何属性(立杆:直径、转角:角度)

有了特征之后作者使用了一种基于图的 SLAM 来构造图。其中:使用 ICP 进行前后帧点云特征的匹配;GNSS 用来计算相邻汽车位姿在全局坐标系的变换,并将其应用到特征的局部位置中,此外,GNSS 还用来提供汽车位姿之间的松约束;光学照片用来标记立杆,通过最近邻算法找到标记出来的立杆和地图特征之间的关联。获取了这些关系之后用来构造图以进行优化。

定位算法

大致流程如下:

  • 当汽车速度超过阈值(实验中用的是 3 m/s)时,用前后两帧 GNSS 来估计汽车的方向,
  • 利用 UKF 集中 GNSS, IMU, 和轮速计来估计汽车在全局坐标系中的位置
  • 从先验地图中,利用估计的车子位姿获取 40 m 范围内的特征,然后将其和当前观测到的特征进行比较。每当成功找到一对匹配关系时,UKF 利用额外 ICP 匹配算法中获得的额外观测信息对汽车位姿进行更新
  • 上一步中的 ICP 匹配算法除了提供位姿以外,还提供了没匹配上的特征(观测得到的)。主要是以两个向量的形式:第一个向量包括和地图特征匹配上的观测特征;第二个向量包括没匹配上的观测特征。这些特征都包括了:位置、特征类型(立杆或者转角)、语义标签、高度和几何属性。我们需要这些信息按照前一部分介绍的流程来对地图特征进行更新。

在某些地区有可能提取的特征很少、并且不正确的匹配关系有可能会对结果有很大的影响。考虑到我们预先知道车子会在哪些区域行驶,我们可以认为设定某些区域为合理行驶区域,在论文中作者设定可行驶区域是包含车道的区域,这些区域不属于建筑物或者公园、足球场等等。如果估计出来的位姿不在合法行驶区域内的话则重新初始化 UKF 来重新对位姿进行估计。

实验结果

作者的实验流程如下:

  • 第一周数据用来构建先验地图,获取了 405 个立杆和拐角特征
  • 从第二周开始进行上述的地图维护流程,在这段实验过程中,为了能够更多检测到新特征,降低特征检测算法的限制条件
  • 接下来每三周进行一次这样的流程,每次都会进行新特征和去除特征点的记录,如下表所示。

表中还记录每次行驶过程 UKF 重置的次数,用来间接表示定位的可靠性。在 2, 5, 8 周有发生两次或多次重置,这些重置基本发生在检测到的特征数量较小的区域,由于这些区域没有一个很强的特征模式,所以数据匹配可能会出错,并且里程计的误差也会积累;在第 11 周之后,通过降低高度限制获得了足够数量的特征,因此提高了定位质量,在此之后就没有出现滤波的重置了。

作者还用了另一个参数来衡量建图质量:滤波更新过程中程度较大的位置修正的数量。作者的思路是:当汽车没办法在一段时间内匹配到特征,里程计的误差会积累。因此,在很长一段没有匹配到特征之后一旦出现一个匹配的特征,此时位姿的估计就会产生不连续性(有较大的修正)。同时,不正确的数据匹配、以及特征位置不正确也可能导致类似行为。在试验中,作者数了大修正的数量来展示对某一版本地图匹配观测的能力。他们将大修正定义为很大的侧向平移,这类平移不会从非完整约束中产生。下图展示了一部分汽车轨迹的不连续性。

下图展示某区域特征更新后的结果,在这张图中作者将所有检测到的特征覆盖在该区域的光学照片上,可以看到最初该区域的特征较少,定位很大程度依赖里程计。随着时间推进,一部分特征从照片底部发生飘逸,这是由于汽车的行驶方向以及里程计的累计误差。从 11 周后,有一部分新特征被检测(降低了条件),定位算法能够匹配到更多的观测,因此定位质量有提升。定位质量提高之后,特征的位置准确度也有明显提高(和图片正确位置重合)

结论

作者提取了一种分层结构的方法来进行长时间的地图维护。接下来作者准备将语义标签也融合进流程中用来作者一个新的变量来区分静态和动态物品。