今天，你算法了没？
关注：九三智能控，每天学点AI算法
定位（Location）是让无人驾驶汽车知道自身确切位置的技术，这是一个有趣且富有挑战的任务，对于无人驾驶汽车来说非常重要。
假设你在驾驶一辆车时彻底迷路了，你不知道自己实际在哪个地方。假设你有一张全球的高精度地图，定位的任务就是确定你的汽车在这张高精度地图上的位置。
在我们的日常生活中，通常使用手机的GPS来确定自己的位置。但GPS对于无人驾驶汽车来说不够精确。大多时候，GPS的精度在1至3米之间，在一些情况下，比如被高楼、山脉围绕或位于峡谷内时，可能只有10米，甚至50米，这对于无人驾驶汽车来说太不精确了。因此 ，我们必须找到另一种方法来更准确定位车辆在地图上的位置。
最常用的方法是， 将汽车传感器所看到的内容与地图上所显示的内容进行比较。车辆传感器可以测量车辆与静态障碍物之间的距离，我们可以在车辆自身坐标系中测量这些距离以及这些静态障碍物的方向。
在车辆坐标系中，汽车的前进方向始终向前。当汽车左转或右转时，坐标系与汽车一起旋转，以使车辆的前进方向在坐标系中继续向前。随着车辆转弯，车辆自身的坐标系必然与地图坐标系不一致。车辆坐标系和地图坐标系可能取决于导航系统中的设置。
在地图上也可能找到车辆传感器所检测到的地标。为估计车辆在地图上的位置，我们将传感器的地标观测值与这些地标在地图上的位置进行匹配。无人驾驶汽车软系统必须将传感器的测量值从车辆坐标系转换为地图坐标系，反之亦然。执行这类转换是解决定位问题的关键步骤。
实现无人驾驶汽车定位有很多方法，接下来将探讨几种常见的定位方法，如GNSS RTK、惯性导航、LiDAR定位和视觉定位。
1. GNSS RTK
如果在野外迷路，你会怎么做？假如你看到自己离一棵树75米远，那么现在你比刚才更清楚自己可能处于什么位置。但你仍不知道自己的确切位置。
然后，你看到了一个离自己64米远的房子，你对自己的位置更确定了一步，即你位于两个圆的其中一个交点上。
现在你又看到了第三个路标，即离自己55远的路灯。通过这三个地标，你终于可以确定自己的确切位置了。
如果你有一张精确的地图，里面标注了这些地标的位置，你就可以利用上面的方法来确定自己的确切位置了，这就是三角测量法。
上面介绍了二维空间中的定位方法，那么如何在地球表面进行三维定位呢？其实，我们可以利用卫星，通过计算我们与卫星之间的距离来实现定位，这就是GPS的工作原理。
GPS即全球定位系统，这是一种由美国开发，并在全球范围内运营的卫星导航系统，这类系统通用名称为GNSS（全球导航卫星系统）。
GPS分为三个部分，第一部分是卫星。在任何特定时间，大约有30颗GPS卫星在外层空间运行。它们各自距离地球表面约2万公里。
该系统的第二部分由世界各地的控制站组成，控制站用于监视和控制卫星。其主要目的是让系统保持运行，并验证GPS广播信号的精确度。
最后一部分是GPS接收器，其广泛存在于手机、电脑、汽车、船只以及许多其他设备中。如果周围没有高楼等障碍物，并且天气良好，那么无论你身在何处，GPS接收器每次应至少检测到四颗GPS卫星。
GPS接收器实际并不直接测量你与卫星之间的距离。它首先测量信号的传输时间，通过将这个传输时间乘以光速来计算卫星的距离。由于光速非常之大，即使少量的时间误偏差也会造成巨大的误差。因此，每颗卫星都配备了高精度的原子钟。
为了进一步减小误差，还可以使用实时定位技术，即RTK。RTK需要在地面上建立几个基站，每个基站都知道自己精确的“地面实际”位置。同时，每个基站也通过GPS测量自己的位置。基站的实际位置与GPS测量位置之间的偏差，就是GPS实际的测量误差。
然后将这个误差传递给其他GPS接收器，以供其调整自身的位置计算。采用RTK技术后，GPS可以将定位误差限制在10厘米以内。但是，当存在高楼或其他障碍物时，仍可能阻挡GPS信号，从而降低定位精度。
GPS的另一个缺点在于它的更新频率很低，大约为10Hz（每秒更新10次）。由于无人驾驶汽车在快速移动，因此需要频繁的更新位置信息，这时GPS就难以满足使用要求了。
2. 惯性导航
假设一辆汽车正以恒定速度直线行驶，如果我们知道汽车的初始位置、速度和行驶时长，那么我们可以很容易知道车辆在任意时刻的位置。同样的，根据初始位置、速度和加速度，依然可以确定汽车的实时位置，这也是惯性导航的基本原理。
对于自动驾驶汽车，加速度可以用三轴加速度计来测量。但仅使用加速度计还不足以计算我们的位置和速度。加速度计是在车辆坐标系记录中进行测量，我们需要知道如何将该测量值转换到全局坐标系。
这种转换可以通过陀螺仪实现。三轴陀螺仪的三个外部平衡环一直在旋转，但其旋转轴始终固定在全局坐标系中。车辆位置可以通过测量旋转轴和三个外部平衡环的相对位置来计算。
加速度计和陀螺仪是惯性测量单元（或IMU）的主要组件。IMU的一个重要特征在于它以高频率更新（其频率可达1000Hz），所以IMU可以提供接近实时的位置信息。其缺点是，IMU的运动误差会随时间而增加，我们只能依靠它在很短的时间内进行定位。
一种更有效的方法是同时结合GPS和IMU来定位汽车。一方面IMU弥补了GPS更新频率低的缺陷，另一方面GPS纠正了IMU的运动误差。
但是，即使将二者结合使用，也不能完全解决自动驾驶的定位问题。比如，我们在山中或者地下隧道中行驶时，可能会长时间没有GPS更新，这会让整个定位系统面临失效风险。
3. LiDAR定位
利用LiDAR（激光雷达），我们可以通过点云匹配来对汽车进行定位。该方法将来自激光雷达传感器的检测数据，与预先存在的高精度地图连续匹配。通过这种比较，可以获知车辆在高精度地图上的全球位置和行驶方向。
匹配点云常用的一种方法是迭代最近点（或CIP）。假设想对两次点云扫描进行匹配，对于第一次扫描中的每个点，我们需要找到另一次扫描中最接近的匹配点，最终会收集到许多匹配点对。我们把没对点之间的距离误差相加，然后计算平均距离误差，再通过点云的旋转和平移来最大限度的降低这一平均距离误差。一旦最大限度的降低了点云之间的误差，就可以在传感器扫描和地图之间找到匹配。然后，就可以将LiDAR扫描到的车辆位置转换到全局地图上，并计算车在地图上的精确位置。
滤波算法是另一种LiDAR定位方法，该方法可以消除冗余信息，并在地图上找到最可能的车辆位置。Apollo使用了直方图滤波算法，该方法也被称为误差平方和算法（或SSD）。
为了应用直方图滤波，我们将LiDAR扫描的点云滑过地图上的每个位置。在每个位置，计算扫描的点与高精度地图上的对应点之间的误差或距离。然后对误差的平方求和，和值越小，扫描结果与地图匹配的越好。
下图中，红色点表示匹配较好的点，蓝色点则匹配的较差。
第三种LiDAR定位方法是卡尔曼滤波。卡尔曼滤波是一种算法，用于根据过去状态和新的传感器测量结果预测当前的状态。具体来说，卡尔曼滤波使用了预测—更新循环。
首先我们根据之前的状态以及对移动距离和方向估计，来预测新的位置。由于运动估计可能存在偏差，所以需要通过所以LiDAR测量实际的位置并加以纠正。一旦我们测量了新的位置，便可以使用概率方法将传感器测量结果与现在实际的位置预测结合起来，并不断的重复这个预测-更新循环。
LiDAR定位的优势主要在于稳健性，从高精度地图开始，只要存在有效的传感器，我们就能始终进行定位。其缺点在于难以构建高精度地图，并且保持其更新。事实上，几乎不可能使地图完全保持最新，因为几乎每个地图均包含瞬态元素：汽车、行人等等。下次路过时，这些元素几乎都会消失，地图在不断发生变化。
4. 视觉定位
对于自动驾驶汽车，图像是收集起来最简单的数据类型。摄像头便宜且种类繁多，而且易于使用。
我们能够使用图像来定位汽车吗？事实上，单纯通过图像来实现车辆精确定位非常困难。所以摄像头图像通常与高精度地图、GPS的数据相结合，从而实现更好的定位效果。
假设一辆车正在路上行驶，它感知到右边有一棵树。但是地图显示许多位置的车道右侧都有树，我们如何知道车辆看到的是哪棵树呢？
我们可以用概率来解决这个问题。想象一下，我们位于道路上许多不同点中的任意一处。我们假设从一些点可以看到右侧的树，而另一些点则看不到。显然，我们位于右侧存在树的地方，从而可以排除在地图上车道右侧没有种树的哪些点。
然后，让车辆继续向前开，并观察周边的世界，可以发现有一个位置在车辆右侧有成排的两棵树，那么我们处于该位置的概率是最大的。
我们可以通过观察结果、概率和地图来确定最可能的位置。因为我们使用粒子或点来估计最可能的位置，因此该过程又被称作粒子滤波。
树木在许多道路上其实是比较稀少，于是一种更常用的方法是利用车道线进行定位。首先使用粒子滤波原理对车道线进行拍照，然后使用图像来确定车辆在道路中的位置，再将道路图像与地图进行比较，从而匹配出最可能的位置。
视觉定位的优点在于图像很容易获取，且成本很低；缺点则是缺乏三维信息和对三维地图的利用。
5. Apollo定位策略
百度Apollo使用基于GPS、IMU和LiDAR的多传感器融合定位系统，利用了不同传感器的互补优势，提高了定位的稳定性和准确性。
Apollo定位模块依赖于IMU、GPS、LiDAR 、雷达和高精度地图，这些传感器同时支持GNSS定位和LiDAR定位。GNSS定位输出位置和速度信息，LiDAR 定位输出位置和行进方向信息。融合框架通过卡尔曼滤波将这些输出结合在一起。
卡尔曼滤波建立在两步预测测量周期之上。在Apollo中，惯性导航解决方案用卡尔曼滤波的预测步骤，GNSS和LiDAR定位用于卡尔曼滤波的测量结果更新步骤。
6. 项目实例
下面的示例使以C 为基础构建粒子滤波器并对汽车进行定位，项目以地图和一些初始定位信息开始，类似于GPS提供的功能。在每个时间步中，粒子滤波器都会接收观测和运动数据。
本内容来自Udacity X Apollo 无人驾驶课程。 链接： http://apollo.auto/devcenter/courselist_cn.html?target=3
