无人驾驶导航定位技术初探（开山篇）

写在前面
本文是基于全球卫星导航体统（GNSS）结合惯导(IMU）在无人驾驶中的应用。计划写成一个系列，力求通俗易懂，尽量少用公式，大白话解释问题。撰写过程中参考学习了一系列的书籍、论文、博客文章，对前人的工作表示感谢。
概述：
定位的目的是让自动驾驶汽车找到自身确切位置的方法，可以说，定位导航技术是整个自动驾驶技术的核心！在日常生活中，我们一直使用谷歌、百度、高德or其他地图 来确定自己的位置，其实质就是使用了卫星导航技术，手机终端接收卫星导航电文进行解算，计算出你此刻的位置。但卫星导航定位精度误差在10米左右，这对于自动驾驶是致命的，完全不能满足自动驾驶的需求。如果驾驶的周围环境复杂，过桥洞，接收不了导航信号，遮蔽严重，如高楼、山脉，会产生多径效应，导航的精度可能会更差，50米开外了。所以无人驾驶系统不能依赖卫星导航系统，因此需要借助其他方式方法来提高车辆在地图上的位置精度，这就要配合高精地图，融合惯导和视觉等技术了，所以，无人驾驶系统中的导航定位分系统是技术交叉很厉害的一部分。
一、导航定位技术
什么是导航定位？导航定位就是利用电、磁、光、力学等科学原理与方法，通过测量与空中飞机、海上舰船、大洋里的潜艇、陆地上的车辆、人流等运动物体每时每刻位置有关的参数，从而实现对运动体的定位，并正确地从出发点沿着预定的路线，安全、准确、经济地引导到目的地，这种技术就叫导航技术（百度百科解释）。曾经听一个报告，一位大专家说什么是导航？他说这是一个哲学问题：“我从哪里来？将要到何方？”，作为一个俗人，我不能给出这么富有诗意的表达，我的理解导航定位就是：定位是导航的第一步，导航是定位的一个连续过程，导航涉及路径规划和决策引导。所以，能不能导航，关键看定位怎么样，核心指标就是定位精度。导航技术分为无线电导航、卫星导航、天文导航、惯性导航、地形辅助导航等，这里主要谈卫星导航和惯性导航以及激光雷达、视觉导航。
二、卫星导航
1、定位原理
需要说明的是卫星导航技术依然是无人驾驶导航定位的主要手段，简单说一下全球导航卫星系统（GNSS），GNSS包括美国的GPS系统，中国北斗（BDS），俄罗斯的格洛纳斯（GLONASS），还有不争气的欧盟伽利略（GALILEO），主流使用美国的GPS，其实现在的终端都是多模多星座，用你不经意之间就使用了北斗系统。卫星导航原理及其简单，就是“三球交汇原理”，用户测量出已知位置的卫星到用户接收机之间的距离，在时间高精度同步的前提下，理论上接收机观测到三颗以上卫星即可实现定位（其实需要大于等于4颗）。原理图如下，关于卫星导航技术不过多的叙述，有时间专门写一篇介绍北斗系统的文章。

2、RTK技术
RTK（Real - time kinematic，实时动态）载波相位差分技术，是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标，RTK能够提高卫星导航的定位精度达到厘米级。基本原理是十分简单，在一定的区域范围内，两个卫星导航终端接收机都距离不远，那么两者的信号将具有几乎相同的误差，如果能精确地计算出第一个接收机的误差，我们就可以对第二个接收机的结果进行修正。这样做大大提高了导航地位的精度。目前，也是无人驾驶的通用技术之一。

三、惯性导航
惯性导航系统是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统。其工作环境不仅包括空中、地面，还可以在水下。惯导的基本工作原理是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、偏航角和位置等信息。百度百科解释
在驾驶中想要知道车的当前位置，用速度乘以时间即可，但是这是在车是匀速且走直线的前提下。很显然，实际情况要复杂得多。这个时候就用到了高中物理姿势"加速度"！对加速度做积分，得到了速度，再次积分，得到了位置，道理就这么简单！但是问题来了，这“加速度”如何得到？为了测量加速度，就要使用“三轴加速度计”传感器。它可以精确测量加速度。速度解决了，还有方向问题，车不是一直走直线的，所以单有加速度计不足以计算车辆的位置。这个时候就需要一个叫“陀螺仪角速度器”出场了！用陀螺仪能测量当前的角速度。解决了车辆行驶的方向的问题。加速度计和陀螺仪合起来就是是 IMU（惯性测量单元）一个解决速度，一个解决方向。IMU 频率为1000Hz，所以 IMU 可以提供接近实时的位置信息。而卫星导航的频率大约10Hz,实时性差很多，说直白一点，开跑车的话，容易“丢星”。但是IMU也是有缺点的，会产生误差积累，运动误差会随时间增加而增加，所以IMU只能短时定位。现在通用办法是结合 GNSS 和 IMU 来进行来汽车定位，一方面，IMU 弥补了GNSS更新频率较低的缺陷；另一方面，GNSS反过来纠正了 IMU 的运动误差。这种结合就是“组合导航”。
四、LiDAR 定位
LiDAR也就是激光雷达。是利用卫星导航信号和IMU机载激光扫描。其所测得的数据为数字表面模型的离散点表示，数据中含有空间三维信息和激光强度信息。应用分类技术在这些原始数字表面模型中移除建筑物、人造物、覆盖植物等测点，即可获得数字高程模型(Digital Elevation Model, DEM)，并同时得到地面覆盖物的高度 。百度百科解释
LiDAR会在车辆行驶的过程中不断收集点云来了解周围的环境。就可以利用这些环境信息来定位。这里我们可以把这个问题用一个简化的概率问题来表示：已知t0时刻的卫星导航（GPS/BDS）信息，t0时刻的点云信息，以及t1时刻无人车可能所在的三个位置：P1、P2和P3（这里为了简化问题，假设无人车会在这三个位置中的某一个）。求t1时刻车在这三点的概率。根据贝叶斯法则，无人车的定位问题可以简化为如下概率公式：

右侧第一项表示给定当前位置，观测到点云信息的概率分布。其计算方式一般分局部估计和全局估计两种。局部估计较简单的做法就是通过当前时刻点云和上一时刻点云的匹配，借助几何推导，可以估计出无人车在当前位置的可能性。全局估计就是利用当前时刻的点云和上面提到过的高清地图做匹配，可以得到当前车相对地图上某一位置的可能性。在实际中一般会两种定位方法结合使用。右侧第二项表示对当前位置预测的概率分布，这里可以简单的用卫星导航（GPS/BDS）系统给出的位置信息作为预测。通过计算P1、P2和P3这三个点的后验概率，就可以估算出无人车在哪一个位置的可能性最高。通过对两个概率分布的相乘，可以很大程度上提高无人车定位的准确度。
五、视觉定位
基于视觉的定位算法有两大分类：一种是基于拓扑与地标的算法，另一种是基于几何的视觉里程计算法。
基于拓扑与地标的算法：把所有的地标组成一个拓扑图，然后当无人车监测到某个地标时，便可以大致推断出自己所在的位置。基于拓扑与地标的算法相对于基于几何的方法容易，但是要求预先建立精准的拓扑图，比如将每个路口的标志物做成地标。
基于几何的视觉里程计算法：该方法计算比较复杂，但是不需要预先建立精准的拓扑图，这种算法可以在定位的同时扩展地图。视觉里程计算法主要分为单目以及双目两种，纯单目的算法的问题是无法推算出观察到的物体的大小，所以使用者必须假设或者推算出一个初步的大小，或者通过与其它传感器（如陀螺仪）的结合去进行准确的定位。双目的视觉里程计算法通过左右图三角剖分（Triangulation）计算出特征点的深度，然后从深度信息中推算出物体的大小。

图 中， Ｑ 是待测距点（某匹配点），其到相机的垂直距离为 Ｒ，在左右相机上形成的像点分别是 Ｑ１ 和 Ｑ２ 。 ｂ 是双目相机的基线长度，ｆ是相机焦距， X1,X2 是 Ｑ 点在左右两幅图像上像点的位置差， 又称为视差。利用相似三角形原理， 可得相机到 Ｑ 点的距离为

双目视觉里程计的优势在于，能够精确的估计运动轨迹，且具有确切的物理单位。在单目视觉里程计中，你仅仅只知道物体在x/y方向上移动了1个单位，而双目视觉里程计则可明确知道是移动了1cm。但是，对于距离很远的物体，双目系统则退化成为单目系统，所以，究竟使用哪种视觉里程计方法，取决于我们的应用场景。通过视觉里程计算法，无人车可以实时推算出自己的位置，进行自主导航。但是纯视觉定位计算的一个很大问题是算法本身对光线相当敏感。在不同的光线条件下，同样的场景不能被识别。特别在光线较弱时，图像会有很多噪点，极大地影响了特征点的质量。在反光的路面，这种算法也很容易失效。这也是影响视觉里程计算法在无人驾驶场景普及的一个主要原因。
至此，把无人驾驶系统中的导航定位分系统进行了一个简单的梳理，后续将对差分RTK技术，激光雷达，视觉定位进行展开讨论。