ICRA2019 Best Paper候选 Variational End-to-End Navigation and Localization翻译

摘要：

深度学习彻底变革了直接从原始感知数据学习「端到端」自动车辆控制的能力。虽然最近在处理导航指令形式的扩展方面取得了一些进步，但这些研究还无法捕捉机器人所有可能动作的完整分布，也无法推断出机器人在环境中的定位。

在本文中，研究者扩展了端到端驾驶网络，使其能够仅基于粗糙GPS信息，实现点到点导航并获得当前位置的概率分布。他们定义了一个新的变分网络，该网络能够根据环境的原始相机数据和拓扑路线图进行学习，以预测可能的控制指令的完整概率分布，以及能够在地图内指定路线上导航的确定性控制指令。

此外，受人类驾驶员可以进行粗略定位的启发，研究者根据地图和观察到的视觉道路拓扑之间的对应关系，制定了如何使用其模型来定位机器人的方案。研究者在真实驾驶数据上评估了该算法，并将点到点导航算法集成到一个全尺寸自动驾驶汽车上，以实现实时性能。另外，他们还在一组新的道路和交叉路口上评估了其定位算法，并展示了该模型在没有任何 GPS 先验的情况下也具备粗略定位的能力。

绪论：

即使观察环境严重受限，人类驾驶员也具有对其驾驶环境的高层级结构进行推理的天生能力。 他们利用这种能力将高层级驾驶指令与具体的控制命令联系起来，并且即使没有具体的定位也能更好地定位自己。 受这些能力的启发，我们开发了一种学习引擎，使机器人车辆能够学习如何在端到端自动驾驶系统中使用地图。

粗粒度地图因其地图范围及抽象特性为我们提供了对环境的更高层次理解。 这使得通过长期目标[1]在分层架构内推理低级控制、定位、防止漂移和闭环成为可能。 与其他场景识别和闭环工作不同，我们关注于使车辆学习如何将不同场景外观与结构，甚至是未知的细粒度几何特征的交叉点与道路的模式与地图中的粗粒度几何特征进行匹配。

虽然端到端驱动[2]由于其易扩展性和强适应性而具有前景，但它因其模仿学习的本质，处理长期计划的能力有限[3]，[4]。 最近的一些方法将地图作为输入[5]，[6]以捕捉长期行动结构，但他们忽略了不确定性地图带给我们的位置的不确定性和长期计划的不确定性。

在本文中，我们通过开发一种新的模型来消除这些局限性，该模型将导航信息与原始传感数据整合到单个端到端变分网络中，并以保留不确定性推理的方式这样做。 这不仅使系统能够完全利用人类的感知结果和导航数据学习复杂环境，而且还能理解何时定位或映射不正确，并因此校正位姿（参见图1）。

我们的模型处理粗粒度，未指定路径的路线图以及前向摄像机图像，以产生机器人可能在该瞬间执行的不同低级转向命令的概率估计。 此外，如果同一地图的包含路径版本也作为输入提供，我们的模型能够输出确定性转向控制信号以沿着给定路线导航。
本文的主要贡献如下：

1.设计一种新颖的变化端到端控制网络，结合原始传感数据与包含和不包含路线地图，实现复杂驾驶环境中的导航和定位。
2.使用我们训练好的端到端网络建立定位算法，以推断给定的道路拓扑，并通过提取地图与视觉道路外观之间的联系来推断机器人的位姿。
3.在具有挑战性的真实世界数据集上评估我们的算法，即使在GPS信息严重受限的情况下，也可以演示转向控制导航并改进姿势定位。

本文的其余部分结构如下：我们在第 II 节中总结了相关工作。 在第 III 节中建立了用于后验位姿估计的算法。在第IV节中描述我们的实验设置，数据集和结果，并在第V节中提供结束语。

模型：

网络的输入是下面图片中绿色框内的部分及道路的图片，输出是转弯角θ_s的分布。利用高斯混合模型将该分布拆分为3个分布，对应道路中的3个方向。惩罚权重误差的方差以抑制额外路线。当输入带路线地图后，会从3个中输出最优转角分布。带路线和不带路线网络方程分别可写为：

其中θ_p是车的位置与航向角。M_U和M_R分别是带路径和不带路径地图，根据θ_p的结果从原地图中裁剪获得。f_S和f_D则是网络的输出结果。θ_s是根据路径最终确定的转向角。本文的所有舵机控制结果都通过转角结果给出，因为这样可以不考虑路面摩擦与控制参数等因素。最后，GMM输出的第i路径的参数由（φi，μi，σi）的值表示。网络的总体架构如下图所示。

每一个相机裁剪后的图片与裁剪后的地图都经过单独的卷积通道，与[2]相似。地图经过的卷积层较少主要由于2个原因：地图特征较少；希望避免平移不变性对位置估计产生的影响。卷积结果通过全连接层产生舵机指令的分布参数（f_S）。第二个任务则是将带路径地图通过卷积加入网络后端，以输出单一控制结果（f_D）。这个网络能使我们处理带路径与不带路径地图，提供定位和驾驶员意图信息，以及根据高级导航指令（即转弯指令）进行驾驶。

模型的损失函数被我们定义为：

其中φ_S与各转角分布标准差相关，我们使用了log σi的二次项：

L是根据具有参数{(φi，μi，σi)}和P(θs|θp; I;M)的GMM的转向命令的负对数似然。其中

A.通过端到端网络定位

基于地图与从车辆看到的道路拓扑之间的关系，模型的条件结构提供关于车辆姿态的后验置信度的更新。例如，对于车载视觉输入I，我们希望获取车辆在地图不同位置的概率分布P(θp | I;M)。虽然我们的网络仅获得了P(θs | θp; I;M)，但我们可以通过θs和θp的边缘概率分布估计车体位姿。对于给定的先验概率分布P(θp)，我们能够获得其后验概率分布方程：

该方程由全概率公式与贝叶斯公式推导而来。如此后验概率分布可通过θs和θp的边缘概率分布进行估计。然而基于两个随机变量的边缘概率分布通常难以求取。以下两种情况下θp的边缘概率分布可以简单求取：a)当位姿由于先前的观察而高度确定时，如在线定位的情况； b)在离散的道路网络上对位姿进行采样，如在地图匹配算法中所做的那样。算法1展示了更新后验置信度的方法。简而言之，该算法在所有转向角样本上计算特定位姿和图像/地图能够解释该转向角的概率，以及估计分区函数和归一化分布所需的附加循环。 我们注意到相同的算法可以在小修改后用于地图匹配框架[28]，[29]。

结果：

在本节中，我们将演示使用我们的方法在具有丰富驾驶环境的训练和测试数据集上获得的结果。 我们首先介绍我们的系统设置和数据集，然后展示我们使用的输入图片，带路径和不带路径地图，以及最后的输出结果。 最后，我们演示了我们的方法如何根据地图和图像输入之间的一致性来减少位姿不确定性。

A.系统设置

我们在2015款丰田普锐斯V上评估我们的系统，该系统配备了自动线控驱动功能[30]。 此外，我们还专门针对这项工作对传感器和计算平台进行了一些改进。 三个Leopard成像LI-AR0231-GMSL相机[31]，能够捕获大约30Hz的1080p RGB图像，被用作本研究的视觉数据源。 我们以不同的偏航角度将三个摄像机安装在车辆的前部：一个面向前，另外两个在车辆的左/右旋转以捕获更大的FOV。 使用OXTS RT3000 GPS [32]以及Xsense MTi 100系列IMU [33]捕获粗粒度全局定位。我们使用横摆率γ[rad / sec]和车辆速度v [m / sec]来计算人类驾驶过程中的曲率（或转向半径）θ_s=γ/v。 最后，所有的传感器处理都是在NVIDIA Drive PX2上完成的[34]。

为了建立道路网络，我们从行驶区域的开放街道地图（OSM）收集边缘信息。 我们获得了有向拓扑图G(V; E)，其中vi∈V表示道路网络上的交叉点，ei∈E表示两个交叉点之间的单个定向道路。 每个边缘的权重w(ei)根据其最大圆长度来定义，但我们稍微修改了权重以匹配道路状况，甚至实时交通延误。 离线地图匹配的问题涉及从一组含噪声有序位姿{θp}到相应的一组遍历的路段{ei}（即所采用的路线）。 我们将地图匹配算法通过离线预处理步骤实现，如[29]中所述。

地图生成过程中的一个问题是处理地图的不明确部分。 不明确的部分被定义为不能将驾驶路线描述为单个简单曲线的部分（例如两次经过同一路口）。 为了有效地处理大规模驱动序列，并避免地图的模糊部分，我们将路径分解为非模糊子路径，并为每个子路径生成带路径地图，形成一组地图图表。 例如，在车辆在同一交叉路口上行多次但在不同方向上行驶的情况下，我们应该将路线分成不同的段，使得在所呈现的路线中没有自交叉。 最后，我们通过在黑色画布上绘制所有边来获得无路径地图。 通过将{ei}绘制为红色通路并获得有路径地图。

使用生成地图和来自三个摄像机的原始感知图像数据，我们使用在郊区区域采集的25公里的驾驶数据训练我们的模型（在TensorFlow [35]中实施），其具有各种不同类型的转弯，交叉路口，环形交叉路口以及其他动态障碍（车辆和行人）。 我们在一组完全不同的路段和交叉路口上测试和评估我们的结果，这些路段和路口未用于训练网络。

B.根据导航输入进行驾驶

我们展示了网络计算给定无路径地图的转向控制的连续概率分布以及在有路径地图上获得确定性导航结果的能力。为了使用导航输入进行驾驶，我们将无路径和有路径地图都提供给网络并计算f_D）。在图3中，我们展示了网络的输入和系统转向角的参数分布。有路径和无路径地图中的道路均以白色显示有路径映射中，所需路径以红色绘制。为了生成图中所示的轨迹，我们将瞬时控制曲率弧形投影到图像帧中。也即我们将车辆将要执行的转向命令所对应的的路径投影到了图片中。摄像机图像中的绿色矩形表示实际上实际馈送到网络的感兴趣区域（ROI）。我们根据这些ROI进行裁剪，因此不向网络提供额外的非必要数据，这些数据不应对其控制决策做出贡献（例如，地平线以上的像素不会影响转向）。

我们展示了模型在从简单的车道跟随到更丰富的情况（例如转弯和交叉点）的各种不同的驾驶场景（从左到右）的输出（参见图3）。 在车道跟随（左）的情况下，网络能够识别GMM仅需要单个高斯模式用于控制，而多个高斯模式被应用于岔路口，转弯和交叉点。 在同时提供有路径地图的情况下，网络能够从多个高斯分布中选择正确的控制命令以导航到路线。 我们还展示了对更丰富类型的交叉点的概括，例如环形交叉口（右），它从未作为训练数据的一部分包括在内。 此外，我们将我们提出的端到端导航堆栈整合到我们的自动驾驶汽车[30]上，通过一条跨越大约1公里（包含总共9个交叉路口）的从未见过的测试轨道来实现实时性能（15Hz）。

为了定量评估我们的网络，我们计算了整个测试集的混合估计精度（参见图4）。 具体而言，对于一系列转向控制分布上的z分数，我们计算测试集内真实（人）控制输出在预测范围内的样本数。 为了更加定性地理解变分模型的空间精度，我们还在测试集上可视化了GPS点的热图（参见图4B），其中颜色代表预测分布中真实控制值所对应的概率密度。 我们观察到概率密度在交叉点之前减小，因为GMM的模式自然地展开以覆盖更多数量的可能路径。

C.减少定位不确定度

我们使用算法1演示了如何使用我们的模型根据观察到的行驶方向来定位车辆。我们在研究了如何减少位姿不确定度，并可视化了能够提供提供更好位姿的区域。
对于这个实验，我们从GPS获得的姿势开始，并假设位姿的初始误差具有一些不确定性（空间位置，航向上的高斯或两者）。我们通过算法1计算后验概率，并查看先验和后验分布的不确定性度量或总熵。如果后验分布的不确定性低于先前分布的不确定性，我们可以得出结论，在看到所提供的视觉输入（即相机图像）之后，我们的学习模型能够增加其定位置信度。在图5中，我们展示了训练（A）和测试（B）数据集。请注意，这两个数据集中的道路和交叉点完全不相交，模型从未使用测试集的道路/交叉路口进行训练。

对于测试集，我们在地图上覆盖各个GPS点，并根据我们的算法是减少（蓝色）还是增加（橙色）后验不确定性对每个点着色。在考虑不确定性降低时，重要的是要注意哪些自由度（例如空间位姿与航向角）在道路网络的哪些区域能更好地定位。出于这个原因，我们在（1）空间方差，（2）角度方差，（3）整体姿势方差和（4）整体熵减少（参见图5）中分别可视化了不确定性减少热图。虽然在直线行驶和更复杂的区域（转弯和交叉点）处都能很好地校正航向角，但是空间自由度仅能在含有丰富信息的地图区域中得到较好校正，而在线性路段中则难以校正。这与预相符，类似于计算机视觉中的孔径问题[36]  - 线性道路中的几何信息不足以实现3自由度定位。

如果我们关注交叉点之前的区域（大约20米之前），我们通常会看到空间不确定性（2m的先验不确定性）在交叉点之前减小，这是有道理的，因为在我们通过路口后前向视觉输入不能捕捉车辆后方的交叉路口。在交叉路口附近看，我们平均减少0.31 nats。对于初始不确定度为0.8弧度（45度）的角度不确定性，，我们实现了0.2弧度（11度）的标准偏差的减小。

我们量化了图6中交叉点周围的后验不确定性降低程度。具体来说，对于图5中的每个不确定度（空间，角度等），我们在图6中将相应的不确定度减少数值表示为先验不确定性的函数。注意，我们实现了各种先验不确定度值的航向和空间不确定性的减少。此外，对于所有先验的不确定性值，交叉区域的平均改进值总是正的，这表明在使用我们的算法之后平均定位效果不会恶化。

D.粗粒度定位

我们接下来评估我们的模型在没有任何先验位置信息情况下区分不同位置的能力。例如，假设您处于没有GPS的位置，但仍希望根据您的视觉环境进行粗略定位（类似于被绑架的机器人问题）。我们希望建立地图与视觉道路区域之间的对应关系，用于粗粒度地点识别。

在图7中，我们演示了如何根据地图和相机图像解释出的转向方向识别和消除一小部分位置的歧义。我们的结果表明，我们可以很容易地区分不同道路拓扑或道路几何形状的位置，这种方式应该不受区域的外观或环境条件影响。此外，网络努力消除各种位姿歧义的情况是可以理解的。例如，当试图确定从环境4拍摄的图像是哪个地图时，网络选择地图A和D，它们两个都具有即将到来的左转和右转。同样，当试图确定环境5的位置时，地图B和E的概率最高。即使道路不包含任何直接转弯，它在左侧包含一个大型车道，类似于可能的左转弯（因此，证明选择地图B是合理的）。然而，网络仍然能够将这五种情况中的每一种正确地对应到正确的地图位置（即，由不确定矩阵的深色对角线标记）。

V.结论

在本文中，我们开发了一种新的变分模型，用于将粗粒度路线图与原始感知数据结合起来，直接学习自主车辆的转向控制。 我们根据有路径地图，不同可能控制命令的概率估计，以及基于地图的定位校正和位置识别来展示控制的确定性预测。 我们使用我们的学习网络制定具体的位姿估计算法，以推断机器人在环境中的定位，并证明我们产生的姿势的不确定性更小（更大的置信度）。
在未来，我们还打算将我们的定位算法集成到我们的全尺寸自动驾驶车辆上的在线离散道路地图匹配中，并提供与人类驾驶员相比更强大的位姿估计。