Apollo自动驾驶入门课（3）——定位

定位是让无人车知道自身确切位置的方法，定位对于无人驾驶车的重要性不仅体现在找出车辆自身的大致方位，更在于以厘米级的标准将车感信息与高精地图信息进行比较来精准确定车辆位置。

1.定位

最常用的定位方法是将汽车传感器所看到的内容与地图上所显示的内容进行比较。车辆传感器可以测量车辆与静态障碍物之间的距离， 这些距离和静态障碍物的方向是在车辆自身的坐标系中测量的。
在车辆自身的坐标系中，车辆的前进方向始终向前，坐标系正方向始终与车头保持一致，但不一定与地图坐标系保持一致。车辆坐标系与地图坐标系均取决于手机导航系统中的设置。
在地图上找到地标后，将传感器的地标观测值与地标在地图上的位置进行匹配，需要将地标的测量值从车辆坐标系转换到地图坐标系。

2.定位方法

GNSS RTK

自身所处的位置可以通过自身与不在同一条直线上的3个静态参考物的相对距离确定，这称为三角测量。
这就是GPS的工作原理，只是以卫星为参考系。三颗卫星和另一颗定位高度的卫星，4颗卫星就可以自定位。这类系统的通用名称是全球导航卫星系统或GNSS，GPS是使用最广泛的GNSS系统。
GPS分为3部分。第一部分是卫星，在任何特定时间大约有 30 颗 GPS 卫星在外层空间运行，它们各自距离地球表面约 2 万公里。第二部分由世界各地的控制站组成，控制站用于监视和控制卫星，其主要目的是让系统保持运行并验证 GPS 广播信号的精确度。第三部分是 GPS 接收器，GPS 接收器存在于手机、电脑、汽车、船只以及许多其他设备中。
GPS 接收器实际上并不直接探测自身与卫星之间的距离，它首先测量信号的飞行时间即信号从卫星传播到GPS接收器所需的时间，通过将光速乘以这个飞行时间，来计算离卫星的距离。由于光速的值很大，即使是少量的时间误差也会在距离计算中造成巨大的误差。因此每颗卫星都配备了高精度原子钟。
为进一步减小误差，使用实时运动定位（RTK）。RTK 涉及在地面上建立几个基站，每个基站都知道自己精确的“地面实况”位置，但是每个基站也通过 GPS 测量自己的位置。已知的“地面实况”位置与通过 GPS 测量的位置之间的偏差，为 GPS 测量结果中的误差。然后将这个误差传递给其他 GPS 接收器以供其调整自身的位置计算。在 RTK 的帮助下，GPS 可以将定位误差限制在 10 厘米以内。
但仍存在高楼或其他障碍物可能遮挡GPS信号的问题，另一个缺点是，GPS的更新频率很低，大约为10Hz。而无人驾驶车处于快速移动时，需要更频繁地更新位置

惯性导航

已知汽车的初速度、加速度、行驶时间、初始位置，就可以通过计算得到任意时刻后汽车的速度和位置。
三轴加速计提供瞬时加速度，三轴加速计有三种不同类型，采用的方法也不同。加速度计提供的不仅是加速度，还测量了重力加速度，所以用的时候需要先把重力加速度剔除掉。加速度计根据车辆的坐标系记录测量结果，还需要陀螺仪传感器将测量值转换为全局坐标系测量值。
三轴陀螺仪提供瞬时角速度，三个外部平衡环一直在旋转，但三轴陀螺仪中的旋转轴始终固定在世界坐标系中。在坐标系中的位置是通过测量旋转轴和三个外部平衡环的相对位置来计算的。

加速度计和陀螺仪是惯性测量单元（IMU）的主要组件，IMU 的一个重要特征是可以以高频率更新，可达 1000 Hz，所以 IMU 可以提供接近实时的位置信息。但缺点在于其运动误差随时间增加而增加，所以只能依靠惯性测量单元在很短的时间范围内进行定位。
如果结合 GPS 和 IMU 来定位汽车，一方面 IMU 弥补了 GPS 更新频率较低的缺陷，另一方面 GPS 纠正了 IMU 的运动误差。但是 GPS 和 IMU 系统的结合也不能完全解决定位问题，如果车辆在山间行驶或城市峡谷中或在地下隧道中行驶，可能长时间没有 GPS 更新，使得定位失败。

补充进阶课内容：
IMU的工作过程：首先是角速度通过积分后得到一个姿态，并把它应用到加速度上，对加速度积分得到速度，再得到最后的位置。
优点：

• 可以得到6自由度信息；
• 短时精度高；
  十来万级别的设备，可以坚持五六百毫秒。
• 输出频率高、无延迟；
  基本上都是200赫兹以上，这样有助于同步。因为它有精准的时间戳，它检测的数据传输过来之后可以精准知道它的时间戳，有精准的时间戳就可以给出精准的位姿，即它在全局坐标系下有自己的位置。
• 无外部依赖；

缺点：

• 误差随时间积累；

激光雷达定位

利用激光雷达，可以通过点云匹配来对汽车进行定位。该方法将来自激光雷达传感器的检测数据与预先存在的高精度地图连续匹配，通过这种比较可获知汽车在高精度地图上的全球位置和行驶方向。匹配点云的方法有多种：

• 迭代最近点（ICP） 对于第一次扫描中的每个点，需要找到另一次扫描中最接近的匹配点，最终会收到许多匹配点对， 把每个点的距离误差相加，然后计算平均距离误差。当对两次点云扫描进行匹配，目标是通过点云旋转和平移来最大限度地降低这一平均距离误差，就可以在传感器扫描和地图之间找到匹配，将通过传感器扫描到的车辆位置转换为全球地图上的位置并计算出在地图上的精确位置。
• 滤波算法可消除冗余信息并在地图上找到最可能的车辆位置。Apollo 使用了直方图滤波算法，该方法有时也被称为误差平方和算法（ SSD）。为了应用直方图滤波，将通过传感器扫描的点云滑过地图上的每个位置，在每个位置上计算扫描的点与高精度地图上的对应点之间的误差或距离，然后对误差的平方求和，求得的和越小扫描结果与地图之间的匹配越好。在事例中，蓝色表示较好，红色较差，绿色表示一般。
• 卡尔曼滤波用于根据过去的状态和新的传感器测量结果预测当前的状态。卡尔曼滤波使用了预测更新周期，首先根据之前的状态以及对移动距离和方向的来估计新位置。运动估计并不完美，所以需要通过使用传感器测量位置并加以纠正。一旦传感器测量了新位置，便可以使用概率规则将不完美的测量结果与现有位置匹配起来。只要需要对车辆进行定位，先预测新位置，然后用传感器测量位置，将会永远遵循这个预测更新周期。
  LiDAR定位的主要优势在于稳健性，只要从高精地图开始并且存在有效的传感器，就始终能够进行定位。主要缺点在于难以构建高精地图并使其保持最新，事实上几乎不可能让地图完全保持最新，因为几乎每个地图均包含瞬态元素，汽车、行人、停放的汽车、垃圾等。

视觉定位

图像是要收集的最简单的数据类型，摄像头便宜且种类繁多、易于使用。通过图像实现精确定位却非常困难，实际上摄像头图像通常与来自其他传感器的数据相结合以准确定位车辆，将摄像头数据与地图和 GPS 数据相结合，比单独使用摄像头图像进行定位的效果更好。
假设一辆车正在路上行驶，感知到右边有树，但是地图显示道路右侧有几棵树有很多不同的点位置，但这样可以排除右边没有树的点。继续前进发现右边又出现一棵树，这样就可以排除右边只有一颗树的点。继续这个过程，通过观察结果、概率和地图来确定最可能的位置，这个过程称为粒子滤波，即用粒子或点来估计最可能的位置。
另外，可以使用相同的粒子滤波原理对车道线进行拍照，使用拍摄的图像来确定车辆在道路中的位置：
视觉定位的优点在于图像数据较容易获得，缺点在于缺乏三维信息和对三维地图的依赖。

Apollo定位

Apollo使用基于GPS、IMU、激光雷达的多传感器融合定位系统，这种方法利用了不同传感器的互补优势，也提高了稳定性和准确性。
Apollo定位模块依赖于IMU、GPS、激光雷达、雷达、高精地图，这些传感器同时支持 GNSS 定位和LiDAR 定位，GNSS 定位输出位置和速度信息，LiDAR 定位输出位置和行进方向信息。融合框架通过卡尔曼滤波将这些输出结合在一起。
卡尔曼滤波建立在两步预测测量周期之上，在Apollo中，惯性导航解决方案用于卡尔曼滤波的预测步骤，GNSS 和 LiDAR 定位用于卡尔曼滤波的测量结果更新步骤。