Apollo进阶课程[3]——Apollo定位技术

无人车车辆自定位技术入门与应用

技术入门

相对某个坐标系，需要知道无人车的位置和姿态。
位置和姿态分别有三个自由度：

• 位置：XYZ
• 姿态：三个方向的旋转，航向（Yaw）、俯仰（Pitch）、横滚（Roll），一般用欧拉角表示。
  
  如果本地坐标系已经定义好，现在有一个车上的坐标系，它相对于本地坐标系的变化（即姿态的变化），就可用三个角度来表示，也就是本地坐标系的三个轴和相对坐标系的这三个轴之间的夹角。
• 
  速度、加速度和角速度等信息用于路径规划及车辆控制。加速度和角速度是相对于车体本身的，告知车辆当时瞬时的加速度和角速度，对人的乘坐体验非常重要。控制模块根据这些信息做一些控制上的优化，让人的体感更好。
• 还需要对位置和姿态加上一个置信度，表示这次输出的定位结果好不好。用置信度来衡量是否相信这个位置的准确性。

对于自动驾驶车定位指标的要求：

为什么无人车需要精准定位系统？
答：环境感知很难。感知和决策没有达到人的地步，需要定位和地图来帮助。相当于视力很差、智商不高的系统，我们需要用定位告诉系统他的位置和地图，这样对于感知就简单一些。

定位的三个方法：

• 基于电子信号的定位：最有特点的就是GNSS。它可以得知现在大概的位置。
• 航迹的推算：最有特点的就是IMU。航迹推算就是根据上一时刻位置和姿态，叠加一些测量信息可以知道现在的位置和姿态。IMU包含了加速度计和陀螺仪。加速度计会输出加速度的信息，它还包含重力加速度。
• 环境特征的匹配：环境特征匹配相对较多，比如LiDAR的匹配：在点云定位的地图，将LIDAR的数据和已建好的地图做匹配，计算自己的位置。通过摄像头，知道了一些车道线，红绿灯的标志，然后确定自身的位置。
  

差分技术：

• 物理差分
• 距离差分
  • 伪距离差分：利用多个卫星发射信号到基站与接收站（车上的）[RTK原理]。精度不会特别的高，大概在米级的这么一个量级
  • 载波相位差分：要估计一个载波相位的一个整周，精确基本上是在厘米级（小于5厘米）。

激光定位：

• 使用方式：预先制作一个地图（3D的Voxel地图、点云地图、2D的概率地图），将实时的激光点云与地图做匹配。
  2D概率地图：把所有点云数据铺到一块，压缩成了一个2D。给他分成很多小格子，每个格子里面存储了颜色信息，位置（位置上面可能只存Z的这个维度，因为2D地图有XY），包括它的概率，概率可能和所含有的点源的数量和统计后的分布相关。
  

视觉定位：受光照环境变化影响大。

惯性导航：基于IMU。

• 加速度计提供瞬时的加速度，陀螺仪提供瞬时角速度。加速度计不仅提供加速度，还测量重力加速度，所以用的时候需要先把重力加速度剔除掉。
• 工作过程：首先是由角速度通过积分后得到一个姿态，再加上之前的姿态得到现在的姿态，把它应用到加速度上，最后加速度积分得到速度，再得到最后的位置。缺点是误差随时间累积。
  

多传感器融合定位：
卡尔曼滤波器接收惯性导航输出的递推，作为它的时间更新，保证滤波器往前走，能够有高频的输出。接收GPS、激光点云定位，视觉定位的输出去做低频的状态更新。

基础知识

三维几何变换

左手坐标系&右手坐标系（大部分使用）
旋转：




平移
刚体位置与朝向：

刚体是种有限尺寸。可以忽形变的固体。自然界不存在完美的刚体，但物体通常可以假定为完美刚体。
刚体坐标系：
为了确定刚体的位置和朝向，需要在刚体内取一点P为原点建立刚体坐标系。
刚体位置：
刚体的位置可以作刚体原点P相对参考坐标系原点O所做的平移，可以使用三维平移向量表示。

定位常用坐标系

地心惯性坐标系(ECI)：
如图中红色0. XYZ坐标系所示，地心惯性坐标系(i系)的原点位于地球原点，Z轴沿地轴方向指向北极，X轴和Y轴位于赤道平面内，与Z轴满足右手法则，并且X轴和Y轴分别指向两个恒星。
在研究载体在地球表面附近的运动时,通常可以将其近似为惯性坐标系，如惯性传感器( IMU ) 中的陀螺仪和加速度计的输出均可以认为是在该参考坐标系下的量测输出。
特点

• X轴和Y轴指向两颗恒星
• 不随地球的自转而转动
• 可以作为地球附近传感器输出的惯性坐标系

地心地固坐标系( ECEF )
如图中绿色0-XYZ坐标系所示，地心地固坐标系(e系)的原点位于地球原点，Z轴沿地轴方向指向北极，X轴在赤道平面与格林威治子午面的交线上，Y轴在赤道平面与X轴Z轴满足右手法则。常用的如WGS84坐标系系统。
特点：

• 与地球固连在一起，随地球起转动
• 假使起始时刻i系和e系重合，二者旋转关系只与时间有关
• 地球表面的任点可以使用确定的坐标系(x,y,z)表示

坐标转换：

当地水平坐标系：
如图中蓝色0 XYZ坐标系所示，当地水平坐标系(系)的原点位于载体所在的地球表面，X轴和Y轴在当地水平面内，分别指向东向和北向，Z轴垂直向上,与X轴Y轴满足右手法则。该坐标系也是机器人领域通常所说的世界坐标系(w系).在导航解算过程中通常也把该坐标系选取为导航坐标系(n系)， 也称为"东-北天( E-N-U )"坐标系，与之相对应的”北东地(N-E-D )"坐标系等。
特点：

• 与地球固连在一起，随地球一起转动
• 与e系的旋转关系只与载体所在经度入、纬度中有关

坐标转换：
通用横轴墨卡托投影( UTM投影)：
UTM投影全称为“通用横轴墨卡托投影”UNIVERSAL TRANSVERSE MERCATOR PROJECTION , 是-种"等角横轴割圆柱投影”， 椭圆柱割地球于南纬80度、北纬84度两条等高圈，投影后两条相割的经线上没有变形，而中央经线上长度比0.9996.该投影方法按经度分为60个带，每带6度，从西经180度起算。如下图所示为全球的UTM投影,北京所在的区域在第50个投影带上。每个投影带上的一个坐标 (x,y,Z)可以唯一地表示地球上一个点,高度使用WGS84坐标系统中的大地高度。
特点：

• 坐标(x，y)加投影带号才能唯表示地球表面一点
• 坐标与经纬度或者ECEF坐标系有确定的转换关系

它是平时定位所输出的坐标系。

车体坐标系：

IMU坐标系：
图所示为IMU (惯性测量单元)坐标系，IMU坐标系的坐标原点在陀螺仪和加速度计的坐标原点，XYVZ三个轴方向，分别与陀螺仪和加速度计的对应轴向平行。在解算惯性导航系统( SINS )中IMU与车体固连，因此在不考虑安装误差角的情况下，载体坐标系也即为IMU坐标系。
特点：

• 与载体固连在一起，不考虑安装偏角与载体坐标系重合
• 与n系的旋转关系可以表征载体的当前姿志信息

相机坐标系：
如图所示其中0点为摄像机光心(投影中心) ,Xc轴和Yc轴与成像平面坐标系的x轴和y轴平行，Zc轴为摄像机的光轴，和图像平面垂直。由点0与Xc、 Yc、 Zc轴组成的直角坐标系称为像机的坐标系。
如何将相机坐标系中的点坐标转换到世界坐标系?
通常IMU坐标系的原点在世界坐标系的位置是已知的，通过IMU坐标系到相机坐标系的外参，以及IMU坐标系的姿态，可以得到相机坐标系到世界坐标系的转换。

激光雷达坐标系：
如图所示为激光雷达坐标系以及俯视图,从图中可以出激光雷达坐标系的坐标原点位于多线束中点旋转轴的交点处，Z轴沿轴线向上，X和V轴如俯视图所示其测量的点坐标是在激光雷达坐标系下的三维坐标，转换到世界坐标系，同相机坐标系下点转换到世界坐标系类似,通过IMU坐标系到激光雷达坐标系的外参,以及IMU坐标系的姿态，可以得到激光雷达坐标系到世界坐标系的转换。

坐标系之间的关系：

百度无人车定位技术

**GNSS技术：**单点定位，没有用基站

• 最著名的是GPS。GPS最早是由美国建立的，有24颗GPS卫星，两个频率的波段。GPS具有定位、测速、授时等功能。
• 原理：测距，有三颗卫星就可以交会两点，舍弃外部的空间点就可以得到自己测绘这个点。但是一般情况下，由于钟差，一般需要四颗卫星去做误差剔除。
• 

载波定位技术：

• 与基于TOA的伪距不同，载波由接收机内部环路锁相环给出的不足整周部分。伪距噪声0.5m~3m ,载波噪声1/100周，在2mm。常用的载波定位技术主要包括RTK (目前无人车在用)和PPP技术。都是为了确定载波的整周数，消除这些噪声，然后达到更精准的定位精度。
• RTK基本原理：20km基线/ 22000km地卫距视经重叠误差强相关
  • 优势：基本5秒内就可以提供cm级定位精度
  • 劣势：建基站、双向链路(4kpbs )
  • 发展趋势：终端低成本化、数据协议5G标准化等
• PPP基本原理：各项误差被单独分离后终端校正。可以简单理解为一个很强的单点，它有很多种基础基站的建设。这些基站通过卫星数据，把这些误差都在基站做分离处理，再传递给卫星。卫星已经做了误差的消除，再去对车端进行定位，得到一个非常高精度的定位信息。
  • 优势：全球分布均匀站100+，单项广播
  • 劣势：建立15+min的收敛时间+接收GEO广播所需授权
  • 发展趋势：LEO-GNSS增强
• 两种技术消除误差的方式：
  

GNSS的作用

• GPS授时
• 制作HD Map
• RTK在线定位

GNSS挑战：

• 可靠性，很容易受到电磁环境干扰
• 受到差环境的巨大影响

激光点云定位算法框架

图像对齐模块主要是用于航向角的优化。点云定位里面会输出四个维度的信息，XYZ和Yaw（航向角）。首先做航向角的优化，第二部分做XY优化用到了SSD-HF。输出的Z是从定位地图获取的。定位地图是一种数据的的存储方式。
地图格式：反射值（颜色）+高度值

基于Histogram Filter的激光点云定位：
SSD：平方差总和

航向角优化：
通过LK算法的框架，输入颜色、高度信息进行优化。
反射值与高度值的自适应融合：
根据分布计算权重，然后加和求出融合后的直方图。

视觉定位

输出也是XYZ和Yaw，即位置和朝向。视觉定位通过摄像头识别图像中具有语义信息的稳定特征并与地图做匹配，得到位置和朝向信息。

流程：

主要包含三个部分，一是3D特征地图的离线的生成，第二是图像特征的检测，最后是数据的整合输出。
特征匹配：
两条线段的匹配方式：将一条投影到另一条上，计算重合度以及线段之间端点的距离。

捷连惯性导航及组合导航技术

初始条件：已知初始速度、位置、姿态
输入数据：惯性测量元件（陀螺仪、加速度计）测量得到的载体相对于惯性空间角运动和线运动参数
结算方法：捷连惯性导航解算、IMU数据积分
输出结果：实时载体速度、位置、姿态
优点：

• 自主性、隐蔽性
• 三维速度、位置、姿态信息
• 输出频率高
• 短时间精度高

缺点：误差随时间积累

对准方式：
高等级IMU

• 精度等级:陀螺仪零偏<1°/h，加速度计零偏<1mg
• 特点:能够敏感到地球自转角速度( 15°/h )
• 原理:重力矢量与地球自转矢量不共线、Kalman滤波速度匹配精对准
• 典型IMU :光纤和激光的IMU

低等级IMU

• 精度等级:陀螺仪零偏> 1°/h，加速度计零偏> 1mg.
• 特点:无法敏感到地球自转角速度( 15°/h)、测量噪声大、存在随机开机零偏等误差。
• 原理:加速度计水平对准、航向角对准(外部辅助，磁强计、gps速度等，优化方式——双天线或车跑起来把速度方向当作Yaw方向 )、Kalman滤波速度匹配精对准。
• 典型IMU : MEMS以及消费级IMU

解算：即递推

组合导航系统：
系统组成：两种或两种以上非相似的导航系统(如全球卫星导航系统( GNSS )、SINS等)
必要条件：可以对同一信息进行测量，如GNSS、SINS均可以计算出载体的位置速度信息
融合方法：以SINS的误差方程作为Kalman滤波的状态方程，GNSS等其他系统与SINS的误差作为观测量进行SINS的误差估计，并对SINS进行校正
优点：

• 协同:超越单一子系统的性能,弥补各个单一子 系统的缺点
• 优势互补:充分发挥各个子系统的优势
• 余度:提高系统的稳定性