摘要
此篇是对Map-Based Precision Vehicle Localization in Urban Environments[4]工作的改进，在精度、地图更新、对环境改变和动态障碍物的鲁棒性方面都有所提升。具体而言，环境没有被建模为一个固定的反射强度网格，而是被建模成概率网格，每个网格都独立表达为一个对反射强度的高斯分布。这样就可以优先使用贝叶斯推断加权最可能是stationary的地图部分，从而减少不确定度和灾难性错误。进一步地，通过使用离线SLAM来对齐同一环境的多个passes（在时间上可能相差几天甚至几个月），可以构建一个越来越鲁棒的用于定位的地图。
II 概率地图
我们对建图的最终目标是获得一个对观测环境的网格化表达，这些网格同时保存了在那个位置上观测到的平均反射强度和这些值的方差。我们通过三步来获得这样一张图：首先，我们对所有轨迹进行后处理让重叠区域都对齐。然后，我们标定每一个laser beam的反射强度让beams都有相同的响应曲线。最后，我们将标定后的激光返回值从对齐的轨迹上投影到一个高分辨率的概率地图上。
A 使用GraphSLAM的地图对齐
给定一个或多个包含GPS，惯导，激光数据的logfile，我们希望refine这些轨迹从而将重叠区域对齐。具体而言，如果logfiles中存在空间上很近单时间上分隔的section，这些section中的vehicle pose必须进行校正从而让两个section合理地对齐。这个任务在[4]中就已经实现。简要地说，GraphSLAM用于优化一个目标函数，这个目标函数中，相邻的vehicle pose由惯导和odometry数据连接，vehicle pose连接到它们的全局估计位置，从logfile中匹配上的section由它们之间计算出来的对齐偏移量连接。
这一次的实现中，匹配的poses之间的对齐偏移量通过对来自两个section中的一簇5个相邻的360度laser scan进行ICP进行计算，x,y,z,yaw,pitch, roll全都优化。一旦一系列匹配计算好，GraphSLAM最小化目标函数从而轨迹得到更新。
B 激光标定
在生成地图之前，对不同的laser beam进行标定使得它们对具有同样亮度的物体具有相似的响应是很重要的。对于良好标定的激光雷达，这一步省略是没有影响的。但是从标定很差的激光雷达生成的概率地图有两个缺点：其一，每个网格的强度均值将严重依赖与那个激光束恰好打到它；其二，计算出来的强度方差将显著比实际的大。实际上，我们发现每次使用时都重新标定是不必要的，不过使用出厂标定毫无疑问也是不够的。因此我们只标定一次，得到的标定表将用于后续建图和定位。
我们不是对每一个laser beam计算一个单一的参数（虽然与未标定的数据相比可能也有提升），而是对每个beam计算一个完整的响应曲线。这样我们就有对每个beam有一个从观测的强度到输出的强度的完整映射函数。
我们没有使用一个固定的标定目标，而是提出一种无监督的标定方法，可以在任意环境开一圈的时候执行。为了计算每个beam的标定响应函数，当车辆穿过一系列poses时，我们投影来自logfile的激光测量，对于每一个地图网格，保存强度值和hit到该网格的 beam相应的ID。对于每个beam j观测到的8-bit强度值a，beam j对于观测强度a的响应值简单地通过 beam j返回强度为a的所有网格中的其它beam的强度平局值来得到。
具体而言，令T为一系列观测\(\{z_1, ..., z_n\}\)，其中z_i为包含beam ID，range测量，强度测量和地图cell ID的4元组\(<b_i, r_i, a_i, c_i>\)。那么我们可以得到
其中N和M为地图的维度。则观测强度为a的beam j的标定输出c(a,j)可以在single pass计算得到：
也就是说，当beam j观测到强度a时，标定的输出为所有beam j观测到强度为a的地图网格内其它beam强度读数的条件期望。
这个等式对所有的64x256个j和a的组合。因此一个标定文件时一个64乘256的强度映射函数；没有被直接观测到的值可以直接插值得到。只需要计算一次并保存到一个查询表。该方法对标定环境没要求，动态障碍的存在也影响不大。
C 地图创建
给定标定后的laser和一个或多个轨迹对齐后的logfile，就可以生成一个高精地图了。为了构建一个概率地图，每个地图网格不仅保存所有强度的均值，还保存它们的方差。像[13]那样显式地通过多次photographs来估计实际的反射率对我们的方法是没必要的，只需要简单地考虑观测到的强度方差就能实现我们的建图和定位目标。
生成概率地图的算法是直观的。随着车辆穿过它的一系列位姿，激光点投影到一个x, y正交平面，每个地图网格表达了15cm x 15cm的地面块。每个网格都维护一个intermediate values以在每次有新的观测时更新它的强度值和协方差。
III. 在线定位
一旦我们建好环境的地图，就可以用于实时定位。我们表达用一个2维的直方图滤波器来表达可能的x和y的offsets的似然分布。与往常一样，滤波器由两部分组成：运动更新，在我们基于运动的估计中降低置信度；以及观测更新，在我们基于数据的估计中增加置信度。
A. 运动更新
我们的GPS/IMU系统以200Hz提供惯性更新和全局位置估计。通过积分惯性更新我们维护一个“smooth coordinate”系统，它对GPS位姿跳动具有不变性，但是不可避免地会随时间漂移。幸运的是，由于smooth坐标系通过积分速度进行更新，它相对于真实全局坐标系统的offset可以以一个高斯噪声的随机游走精确地建模。显然，恢复两个坐标系之间的offset等效于知道我们的真实全局位置，所以这个offset是我们努力要估计的。
这样，我们滤波器中的运动模型非常简单。我们只需要建模smooth和global之间的drift，而不用建模车辆自车运动的不确定性。车辆运动模型实际上并不是被忽略了，而是在紧耦合的GPS/IMU系统内部被精确地使用，从而最小化smooth和global之间漂离的速度。由于smooth的漂移被建模为零均值高斯噪声，运动模型更新每一个网格的概率如下：
其中\(\overline P(x,y)\)为运动更新后的车位于网格(x,y)中的后验概率，\(\eta\)为归一化因子，\(\sigma\)为描述smooth系统漂移速率的参数。
B. 观测更新
直方图滤波器中的第二个部分是观测更新，使用实时接收到的laser scan来refine车辆的位置估计。
我们处理实时laser scan的方式于之前描述的建图过程是相同的。也就是说，以与我们地图完全相同的方式通过积累的传感器数据构建一个rolling grid（增量式构建？），而不是把每个laser return都作为网格的观测。这个方法让我们可以直接比较从传感器数据构建的cells和来自地图的cells，并且避免了过渡赋权给那些有很多return的网格（例如树和大的动态物体）。
如果记z为传感器数据，m为地图，x和y为可能的相对GPS pose（？）的偏移，则由贝叶斯法则：
我们可以将GPS/IMU pose的不确定度近似为方差大小为\(\sigma_{GPS}^2\)的高斯，从而就可以简单估计P(x, y)为GPS高斯和运动更新之后的后验置信度的乘积：
为了计算给定offset (x,y)和地图m下传感器数据z的概率，我们累乘所有网格的概率，这一概率由观测到的传感器数据网格的平均强度和对应的地图网格的平均强度以及它们的协方差给出。这个值然后施加一个指数幂\(\alpha < 1\)，以考虑数据不是完全不独立的事实。比如说由于系统标定误差或者多帧测量到的环境中小幅结构改变。
我们称地图和传感器数据中的标准差二维数组分别为\(m_\sigma\)和\(z_\sigma\)。对平均强度值也是用同样的记号，则有：
为了对局部过时地图实现鲁棒性，我们进一步为地图和传感器数据的强度值impose一个极小的融合的标准差？这对环境改变有点用。出于计算考虑我们严格限制对\(P(x, y | z, m)\)的计算限制在GPS估计的几米内。然而这个搜索半径很快就需要增加如果是用不那么精确的GPS系统。
C. 最可能的估计
直接选Max可能会有抖动的风险。另一个选择是使用后验分布的质心，这可以提升一致性，但是可能偏离中心太多。作为一个折衷，我们使用具有variation的质心。即，我们对\(P(x, y)\)加一个指数幂\(\alpha > 1\)，如下：
