L4级全自动泊车系统Valet Charge详解二：地图篇

Valet Charge项目为适应室外和室内停车场设计了一个三层分级拓扑/尺度地图，主要用来定位和做动作规划。



上图为Valet Charge的导航框架图。停车场的服务器为每辆进出的车提供一个针对性的全局导航，为车辆指定停车位。服务器上包含一个提前制作好的停车场的路网地图。



机器人学中地图的表示方法有四种：几何特征地图、拓扑地图、栅格地图以及直接表征法（Appearance based Methods）。特征地图用有关的几何特征（如点、直线、面）表示环境。它一般通过如GPS、UWB以及摄像头配合稀疏（Sparse）方式的vSLAM算法产生，优点是相对数据存储量和运算量比较小，多见于最早的SLAM算法中。拓扑地图把室内环境表示为带结点和相关连接线的拓扑结构图，其中结点表示环境中的重要位置点（拐角、门、电梯、楼梯等），边表示结点间的连接关系，如走廊等。栅格地图则是把环境划分成一系列栅格，其中每一栅格给定一个可能值，表示该栅格被占据的概率。直接表征法省去了特征或栅格表示这一中间环节，直接用传感器读取的数据来构造机器人的位姿空间。每种方法各有自己的特点和适用范围，其中特征地图和栅格地图应用最普遍。

三层分级拓扑/尺度地图最上层为稀疏（Sparse）地图，可以看做几何特征和直接表征法的融合。是带3D路标（Landmark）车辆姿态图，用直接表征和几何特征编码。稀疏地图为下一步做图定义坐标系，同时也包括定位所需要的几何特征和直接表征。中间层为稠密（Dense）地图，是一个单层的静态场景结构的高度地图。这两层都是为车辆视觉定位而设计的。最底层是路网地图，这是抽象的用于全局规划和语义的地图。灰色线条代表边界，黄色线条是推荐的行驶轨迹，黑色矩阵框是停车位。

稀疏地图采用离线方式用vSLAM管线制造，这是一种基于拓扑/尺度地图的定位方法。从四个鱼眼相机搜集的图像序列，利用GPU运行SURF算法抽出每张图像的关键点和descriptors，这里的Descriptor，其实我们可以把它当做大家传统上理解的特征。Descriptor更接近局部特征或部分特征的意思。Speeded Up Robust Features（SURF，加速稳健特征），是一种稳健的局部特征点检测和描述算法。SURF要在性能和速度之间取一个平衡，既降低描述子的维度，又要保证足够的区分度。

特征点检测后开始进行匹配计算，常用的有暴力匹配(BruteForceMatcher)和最近邻匹配(FLANN)两种匹配方法。Valet Charge使用FLANN的变种KNN算法即K最近邻(k-Nearest Neighbor，KNN)分类算法，这是一个理论上比较成熟的方法，也是最简单的机器学习算法之一。该方法的思路是：如果一个样本在特征空间中的k个最相似(即特征空间中最邻近)的样本中的大多数属于某一个类别，则该样本也属于这个类别。最后使用Bundle Adjustment，中文是光束平差法，这是机器人视觉中最顶尖的方法，也是常见的SLAM算法，有时简称BA。就是利用非线性最小二乘法来求取相机位姿，三维点坐标。在仅给定相机内部矩阵的条件下，对四周物体进行高精度重建。

BundleAdjustment的优化目标是让重复投影误差降到最低。与利用non-linear mean square 解三角同，bundle adjustment 中所有的参数，RCX均为变量。N幅图则有N个姿态（Pose），X个点，这就会得到非常大的jacobbian Matrix.本质上，使用雅克比矩阵进行梯度下降搜索，同时迭代所有的q C X ，最终可以同时得到世界点坐标，相机姿态== SLAM。



Dense地图仍然由鱼眼相机完成，Valet Charge中苏黎世理工学院机器视觉与几何实验室将多鱼眼相机用到了极致。特别是多目视图的立体重建，多目立体视觉简称MVS，其实现方法简单分为四大类，一是基于VOXEL体素的方法，二是特征点扩展，三是基于表面演化，四是基于深度图像融合。第四种比较灵活，因此广泛使用，Valet Charge也使用了第四种和第一种。

首先抽出四个鱼眼相机中一个相机的图像，即左图，抽出其透视图，运行平面扫描立体匹配算法(Plane Sweep Stereo Matching)获得深度图像，就是中间这幅图像。其余三个鱼眼相机的同期三幅图像也用平面扫描立体匹配算法获得深度图像，融合后得到右图。

这样得到的密度图还是两层的，需要再处理，首先计算出深度图将其送入一个立体栅格图中。每一个网格的体素（VOXEL）存储信息包含栅格占有的似然值。然后原始高度图与VOXEL逐点匹配。最终得到一个全局凸面优化的单层高度图。



最底层的路网地图是一个离线地图，也就是先验地图，是语义与尺度合成的地图，储存在停车场的地图服务器上，主要由牛津大学机器人实验室完成。绿线代表车辆的行驶轨迹，还包括建议速度，蓝线代表可泊车空间。地图为无人车提供精确的导航。大多数离线地图一旦生成，升级都需要重新再做一遍，牛津大学的设计可以让地图自动升级，这就是牛津大学机器人实验室所推崇的Introspective Active Learning，主动内省学习法。 简单地说就是用稀疏高斯处理分类器加信息向量机Informative Vector Machine (IVM),进行机器学习。这是一种端到端的学习。



车辆每一次泊车出车都是一次升级，或者说内省学习的过程，最终泊车出车的速度都可以大大提高。



蓝色代表行人出现较少的区域，可以高速行驶，黄色代表行人出现较多的区域，需要低速行驶。这是多次学习得出的成果。



车辆在第一次泊车时，为保证可靠性，仍然需要车载的鱼眼相机检测停车位，这里是同提前制作的模板来检测停车位。左图为鱼眼相机合成的俯视图，中间这张图是用模板检测出来的停车位图，右边则是用谷歌卫星地图检测的停车位图。停车位模板一是基于形状，而是基于带颜色的停车线框。