论文《FLAME:Feature-Likeilhood Based Mapping and Localization for Autonomous Vehicles》阅读总结

摘要

准确的车辆定位是自动驾驶导航最关键也是最基本的任务。虽然基于密集3D点云的地图可以进行准确地定位，但是在城市规模的环境中使用时，它们会带来巨大的存储负担和传输负担。 在本文中，我们提出了一种用于LiDAR地图的高度压缩表示，以及一种针对车载LiDAR扫描的高效且强大的实时对齐算法(alignment algorithm)。所提出的建图框架（我们称为特征似然获取地图仿真（Feature Likelihood Acquisition Map Emulation) FLAME））需要的空间不到原始3D点云地图的0.1％。 本质上，FLAME通过特征似然函数(feature likelihood functions)模拟原始地图。特别的，FLAME对平面，杆和路缘(curb)特征进行建模。这三个要素类在车辆道路之间是长期稳定的，易于区分(distinct)且常见的。 通过特征检测从LiDAR扫描中提取多类特征点。 提出了一种新的基于多类的点到分布对齐方法(multiclass-based point-to-distribution alignment method)，以找到多类特征点和FLAME图之间的关联和对齐。实验结果表明，所提出的框架可以达到与基于3D点云的定位相同的精度水平（小于10cm）。

框架

第一条线路是：由高质量的点云图 —> 特征向量图 —> FLAME图，只保留了路缘特征、线杆特征、墙面特征，并抽象为特征似然域。完成了从1G到730KB的数据压缩。
第二条线路是：从车载激光雷达获取的点云帧/点云扫描上，提取上述三类特征点。
最后，将扫描中的特征点 与 FLAME图中的对应类别的特征似然域进行匹配，获取车辆在地图中的2D姿态。

线路一：生成FLAME Map

预处理

对高质量的点云地图处理：
首先，移除地面像素并将地面以上的像素组织到单元（cell）中。使用Cloth Simulation Filter 从点云中提取地平面以上的点。 接下来，将离地点光栅化（rasterizing）为X-Y平面上的小单元，分辨率为r×r。
可以使用vector：$c_i=\left\{P_{ci},N_{ci} \right\}$ci​={Pci​,Nci​} 表示该cell ，其中$P_{ci}$Pci​表示位于该cell内的点集，并且$N_{ci}$Nci​是点数。

生成特征向量图

所有地标特征（路缘、线杆、墙面）都是在地平面上方的垂直特征，即，与其他cell相比，包含地标特征的cell将包含更多点。因此，使用阈值$N_t$Nt​(该值是原始点云密度和栅格化分辨率的函数)，剔除所有点数低于该值的cell。使用保留下来的cell的$x$x、$y$y坐标即可形成特征向量图。
此时，平面特征和路缘特征可以描述为2D模型上一组连接的线段。
对特征向量图使用随机样本共识（RANSAC）或霍夫变换来提取表示墙面的连接线段。而对分割的地面点使用相同的方法，可以提取路缘石特征。至于线杆，由于在城市和郊区环境中大多数线杆要素的半径小于20cm，可以将其近似建模为2D点。应用2D点聚类并计算聚类的中心可以从点特征图中提取基于点的线杆特征。
提取对的三个特征类别构成了2D特征向量图。

生成FLAME Map

为了有效地执行特征匹配，需要将上一节中构建的特征向量图转换为本节中所述的以正态分布表示的特征似然图（feature likelihood map (FLAME map) represented with normal distributions）。
每个正态分布似然域（normal distribution likelihood field）的参数计算如下。

墙面特征：

墙面特征在特征向量图中已经建模为连接的线段，我们将连接的线段分割为长度为$d$d的线段。使用正态分布$\left\{{\mu}_{i},cov_{i} \right\}${μi​,covi​}建模其中的每条线段$l_i$li​，并作为似然域（likelihood field）（${\mu}_{i}$μi​为正态分布的均值，$cov_i$covi​为协方差矩阵）。假设沿着这条长度为$d$d的线段，正态分布$N\left(\boldsymbol{\mu}_{i}, \operatorname{cov}_{i}\right)$N(μi​,covi​)有一个$99 \%$99%置信度的椭圆（99% confidence ellipse），即${\mu}_{i}$μi​为置信椭圆的中心，线段方向即为主轴方向，且由该正态分布生成的点中，将有99％在该椭圆内。椭圆的长短轴计算如下：
$l_{a i}=2 \sqrt{s \cdot \lambda_{1}}=2\left(\frac{d}{2}+\varepsilon_{\text {line}}\right)$lai​=2s⋅λ1​​=2(2d​+εline​)
$l_{b i}=2 \sqrt{s \cdot \lambda_{2}}=2\left(\frac{\frac{d}{2}+\varepsilon_{\text {line}}}{10}\right)$lbi​=2s⋅λ2​​=2(102d​+εline​​)
$s$s是自由度为2的99％卡方似然；
$\lambda_{1}$λ1​和$\lambda_{2}$λ2​分别为协方差矩阵$cov_i$covi​的最大最小特征值。
$\varepsilon_{\text {line}}$εline​为不确定因子。
因此，可以从上面两式计算$\lambda_{1}$λ1​和$\lambda_{2}$λ2​。置信椭圆的长轴和短轴的正方向与分别与$\lambda_{1}$λ1​和$\lambda_{2}$λ2​的特征向量的方向相同。 因此，我们可以根据线段方向建立单位特征向量$v_1$v1​和$v_2$v2​。 如图2所示，$v_1$v1​指向与该线段相同的方向，$v_2$v2​垂直于$v_1$v1​。 因此，对于线段$l_i$li​，其相应的似然场（即正态分布$N\left(\boldsymbol{\mu}_{i}, \operatorname{cov}_{i}\right)$N(μi​,covi​)）可以计算如下：
$\boldsymbol{\mu}_{i}=\frac{\left(\boldsymbol{p}_{l i 1}+\boldsymbol{p}_{l i 2}\right)}{2}$μi​=2(pli1​+pli2​)​
$\operatorname{cov}_{l_{i}}=\boldsymbol{V} \cdot \boldsymbol{D} \cdot \boldsymbol{V}^{-1}$covli​​=V⋅D⋅V−1
$\boldsymbol{V}=\left[\begin{array}{ll} \boldsymbol{v}_{1} & \boldsymbol{v}_{2} \end{array}\right], \boldsymbol{D}=\left[\begin{array}{cc} \lambda_{1} & 0 \\ 0 & \lambda_{2} \end{array}\right]$V=[v1​​v2​​],D=[λ1​0​0λ2​​]
$\boldsymbol{p}_{l i 1}$pli1​、$\boldsymbol{p}_{l i 2}$pli2​分别为线段$l_i$li​的两个端点坐标。$V$V是正交特征向量矩阵（orthonormal eigenvector matrix）。

路缘特征：

对于路缘特征，我们使用相同的处理方法，因为路缘特征在特征向量图中也表示为连接的线段。 唯一的不同是，我们使用了较大的不确定因子。 之所以如此，是因为与墙壁，围墙和建筑物的外墙相比，路缘石要低很多，而且通常不严格垂直于地面，这会导致更大的不确定性。

线杆特征：

线杆特征在特征向量图中是一个点。其特征似然域用正态分布$N\left(\boldsymbol{\mu}_{\text {pole}_{j}}, cov_{pole_{j}}\right)$N(μpolej​​,covpolej​​)表示，$\boldsymbol{\mu}_{\text {pole}_{j}}$μpolej​​是线杆特征${pole}_{j}$polej​的坐标， $cov_{pole_{j}}$covpolej​​计算如下：
$\operatorname{cov}_{p o l e_{j}}=\frac{r^{2}}{s} \cdot I$covpolej​​=sr2​⋅I
$r$r表示置信圆的半径
$s$s是具有2个自由度的99％卡方似然，
$I$I是2×2单位矩阵。
如果提供了线杆半径，则可以将公式中的$r$r更改为$r_{p o l e_{j}}+\varepsilon_{p o l e}$rpolej​​+εpole​，其中前者线杆半径，而后者是不确定性因子。

第二条线路：对扫描进行特征检测

目前，环境的密集3D点云已转换为FLAME地图。 我们的目标是使用实时LiDAR扫描进行定位。但是，我们不建议将3D LiDAR扫描点直接匹配到FLAME地图，而是建议先将这些扫描转换成特征图，然后将它们对齐以进行定位。这样的优点是改进了关联，减少了歧义，并减少了计算量，同时保持了较高的准确性。
在LiDAR扫描中检测特征不同于在密集的高质量环境点云中进行特征检测，这主要是单帧雷达扫描会因为随着距离增加而分辨率降低。
我们选择了可以在各种分辨率（包括低采样分辨率）下运行的特征检测器。

预处理

对雷达扫描点求取表面法向，并将所有点分为：地面点、陡峭点（steep）、其他。这些法线为每个像素提供了陡峭度量；接近垂直（12.5度以内）的那些被标记为陡峭像素（steep pixel），用于收集我们的线杆和墙面特征的证据（或者说是候选像素）。 可以使用任何适当的地面检测方法标记地面像素。

线杆特征检测

我们通过首先将LiDAR扫描划分为大小为1m的规则单元阵列（regular array of cells）来搜索线杆特征。在每个cell中，我们收集有关线杆及其位置的证据。
将线杆建模为垂直圆柱体，在线杆表面上分布有LiDAR像素，并且法线矢量指向极点中心。线杆参数$\theta_{\text {pole}}=\left(x_{j}, y_{j}, r_{j}\right)$θpole​=(xj​,yj​,rj​)，表示线杆中心位置和半径。
假设某cell包含线杆：一个LIDAR像素（位置$x_i$xi​，法向$n_i$ni​），可以预测线杆中心位置。这种预测$P\left(\boldsymbol{x}_{i}, \boldsymbol{n}_{i} | T_{\text {pole}}, \theta_{\text {pole}}\right)$P(xi​,ni​∣Tpole​,θpole​)可以建模为以线杆中心为中心的高斯模型。

图：用于线杆检测的示例，其中线杆半径为$r_j$rj​。一个cell以10cm的间隔分成均匀的网格。每个陡峭点（黑点）预测沿其法线的方向偏移$r_j$rj​，以预测线杆中心。将预测值插值到网格中，然后霍夫变换通过搜索多个半径来找到最大似然中心。

如果cell内无线杆，作为网格中心上的均匀密度，模型$P\left(\boldsymbol{x}_{i}, \boldsymbol{n}_{i} | T_{\text {nonpole}}\right)$P(xi​,ni​∣Tnonpole​)等于$\frac{1}{n_{g}}$ng​1​，其中$n_g$ng​是cell中的网格数量。
假设像素采样独立，我们可以使用概率霍夫变换求解线杆与非线杆的最大似然比 ，这将在线杆位置和半径上最大化以下表达式：
$\operatorname{MLR}_{\text {pole}}\left(\theta_{\text {pole}}\right)=\arg \max _{\theta_{\text {pole}}} n_{g}^{N} \prod_{i=1}^{N_{s}} P\left(x_{i}, n_{i} | T_{\text {pole}}, \theta_{\text {pole}}\right)$MLRpole​(θpole​)=argθpole​max​ngN​i=1∏Ns​​P(xi​,ni​∣Tpole​,θpole​)
$N_s$Ns​是陡峭像素数量，$N$N是非地面像素数量，结果是每个cell的线杆估计以cell内存在线杆的似然比。区别地确定该似然比的阈值（不同地方阈值不同）。

墙面特征检测

墙面特征，如果线杆特征，是借助 在LiDAR扫描上的一个cell阵列（an array of cells）检测的。在每个cell中，一个墙面特征被建模为一组在水平方向上拥有相同法向的陡峭像素。 非平面特征被建模为具有统一的法线方向。再次使用概率霍夫变换求解墙与非墙的最大似然比，以及墙的朝向，在[23]中有更多详细信息。（D.D.Morris “Obstacles and foliage discrimination using lidar”）

路缘检测

路缘是另一类常见且重要的特征，可通过移动车辆轻松观察到。 基于路缘特征的几何形状和拓扑，它们可以在横向方向上提供可靠的约束。本文提出了一种轻量级的路缘石检测方法，该方法可用于任何分辨率的LiDAR。
我们使用16线LiDAR进行实验，LIDAR传感器测量形成的16线点云（这16线点云呈现同心圆分布），我们将其称为环。此外，LiDAR安装在车辆顶部并与地面平行。如果高度是固定的，则对于空的平坦地面，可以确定每个连续环之间的距离。而比这个距离更近的连续环，其上的点是路缘的候选者。
第i个环半径为：
$r_{i}^{\text {regular}}=\frac{h}{\tan \theta_{i}}$riregular​=tanθi​h​
$h$h是雷达高，$\theta_i$θi​是雷达射线角度，如图

环间距：
$\Delta r_{i, i+1}^{r e g u l a r}=r_{i+1}-r_{i}, 0<i<\mathrm{n}-1$Δri,i+1regular​=ri+1​−ri​,0<i<n−1
因为存在其他因素也影响连续环之间的距离（如，非平坦地面，雷达未完全平行于地面），为了能够容纳这些，我们定义一个间隔：
$\begin{aligned} d_{i, i+1}=&\left[a \Delta r_{i, i+1}^{r e g u l a r}, b \Delta r_{i, i+1}^{r e g u l a r}\right] \\ & 0.5<a<b<1 \end{aligned}$di,i+1​=​[aΔri,i+1regular​,bΔri,i+1regular​]0.5<a<b<1​
其中$a$a和$b$b是调整范围界限的参数。如果$\Delta r_{i, i+1}$Δri,i+1​落在上述间隔中，那么该点将作为路缘的候选点（当环$i$i上的点落到路缘上时，因为路缘相较于地面略高一点，那么环$i$i上路缘点将会向内偏移，$d_{i,i+1}$di,i+1​会比实际值放大？？？怎么和公式矛盾？）
(这一部分没看明白是如何划分网格的）为避免将倾斜区域误分类为路缘石，我们制定了以下策略。路缘通常在横向（$y$y方向）上具有较大的高度变化，而在纵向（$x$x方向）上具有较低的高度变化。 为了对此进行量化，将LiDAR扫描放置在$n×m$n×m的单元格中，其中$n$n是环的数量，每行$i$i存储一个环。每个cell网格$c_{ij}$cij​存储着落在$c_{ij}$cij​的点的位置的平均值$(x_{ij},y_{ij},z_{ij})$(xij​,yij​,zij​)。因此，梯度的计算如下：
$g_{i, j}=\frac{Z_{i, j+1}-Z_{i, j-1}}{\operatorname{dist}_{x y}\left(c_{i, j+1}, c_{i, j-1}\right)}$gi,j​=distxy​(ci,j+1​,ci,j−1​)Zi,j+1​−Zi,j−1​​
其中$z_{ij}$zij​是cell网格中第$i$i行第$j$j列的元素$z$z值。距离$dist_{xy}(a,b)$distxy​(a,b)表示了在$xy$xy平面$a$a和$b$b的欧氏距离。对于网格$c_{ij}$cij​，我们计算$g_{ij}$gij​并使用阈值$t_g$tg​将其列为路缘候选点。最后，基于上一小节中描述的地面分割，只有同时分类为地面和路缘的点才被分类为路缘点。

配准与定位

本节提出了一种定位方法，利用上述三种特征类型——线杆，墙面和路缘。与NDT类似，它可以优化与每个特征的位置密度相关的得分函数，而不需要显式关联[32]，我们通过汇总附近同一类特征的密度，为当前扫描的每个检测到的要素建立分数，并对其进行优化。
车辆在2D空间的位姿定义为$\boldsymbol{X}_{t}=\left(x_{t}, y_{t}, \theta_{t}\right)^{T}$Xt​=(xt​,yt​,θt​)T。
$z^{pole}_t$ztpole​，$z^{planar}_t$ztplanar​，$z^{curb}_t$ztcurb​分别指代$t$t时刻，雷达扫描中，被探测的线杆、墙面和路缘特征。
FLAME Map包含$N4$N4个似然域，$m_i$mi​包含类别名称、均值$\boldsymbol{\mu}_{i}$μi​、协方差矩阵$cov_i$covi​。
$v^m_t$vtm​和$\theta^m_t$θtm​分别是里程计和IMU在运动预测中获得的汽车速度和朝向。


这一行是运动预测，$T$T是采样周期。运动模型只用于给迭代对齐提供初始值。

以上是对齐迭代过程，我们以一个线杆特征为例。
第八行，对所有的经过变换后的线杆特征点 $T\left(\widehat{\boldsymbol{X}}_{t}, \boldsymbol{p}_{i}^{p o l e}\right)$T(Xt​,pipole​)，通过找到FLAME Map中最近的三个线杆特征似然域，作为其匹配。其中函数$T(\boldsymbol{X}, \boldsymbol{p})$T(X,p)是通过位姿$X$X变换点$p$p，$\widehat{\boldsymbol{X}}_{t}$Xt​是迭代中的位姿，仍然包含误差，所以，由$T\left(\widehat{\boldsymbol{X}}_{t}, \boldsymbol{p}_{i}^{p o l e}\right)$T(Xt​,pipole​)找到的最近似然域作为匹配似然域，不一定正确。但是，在最近的三个似然域中有一个是正确匹配的概率就非常高。因此，我们的策略是找到所有潜在的匹配，并结合他们的分数。如果匹配找到不足三个，那么就仅用一个或两个。如果一个匹配都找不到，则放弃该点。（这个策略也使得系统适应于动态环境）
10~11行的计算方法如下（如何结合三个最近匹配的分数）：

$\begin{aligned} &s_{i}^{c}\left(\widehat{\boldsymbol{X}}_{t}\right)=\sum_{j=1}^{3} \exp \left(-\frac{1}{2}\left(T\left(\widehat{\boldsymbol{X}}_{t}, \boldsymbol{p}_{i}^{c}\right)-\boldsymbol{\mu}_{j}\right)^{T} \cdot \operatorname{cov}_{j}^{-1}\right. \cdot \left.\left(T\left(\bar{X}_{t}, \boldsymbol{p}_{i}^{c}\right)-\boldsymbol{\mu}_{j}\right)\right) \end{aligned}$​sic​(Xt​)=j=1∑3​exp(−21​(T(Xt​,pic​)−μj​)T⋅covj−1​⋅(T(Xˉt​,pic​)−μj​))​
在提出的框架中，匹配仅在同一个类型中进行，会大大减少错误匹配。负分数（negative score），梯度向量和Hessian矩阵是根据它们的类别分别计算的，但都使用车辆姿态$\widehat{\boldsymbol{X}}_{t}$Xt​进行参数化。使用牛顿法最大化总得分函数（第17〜20行）。
为了降低系统的复杂性，提出的体系结构仅关注二维的三参数姿态估计。 对于车辆停留在地面上的大多数驾驶环境而言，这已足够。

结论

我们引入了一个新的建图和定位框架，该框架基于不同的特征类别和相应的特征似然函数。 提出的FLAME地图实现了稳健的定位，尽管表示FLAME地图所需的空间比传统3D点云地图小了几个数量级。 此外，提出了一种新的特征到特征对准算法，以将LiDAR扫描中检测到的特征对准FLAME图。 当特征密集时，使用类内关联可提高对齐的鲁棒性。 实验结果表明，该方法可以达到厘米级的定位精度，并且对未在地图中建模的真实环境中的静态和动态变化具有鲁棒性。