简介
在自动驾驶中，由汽车的传感器到全局定位会涉及到若干个坐标系统，下面简单进行整理。
传感器坐标系
每个传感器都有其自身的坐标系，按照其测量原理可以分为相对测量（测量环境信息）和绝对测量（测量自身信息）。相对测量的传感器如相机，激光雷达是通过测量环境信息来进行间接定位，而绝对测量如 IMU，轮速计，GPS 则是通过直接测量自身的运动状态（速度、位置、姿态等）来进行定位。对于相对测量的传感器而言，其测量值通常是在其自身传感器下；而绝对测量的测量结果一般则是自身坐标系相对于其他坐标系（局部坐标系或世界坐标系等）。下面对常见的测量传感器进行归纳。
相机坐标系
相机作为环境传感器可以将环境信息根据投影原理反映到图片上。图片上的每个点通过反投影可以投影至相机的归一化平面上，相机坐标系如下图所示：
相机坐标系的方向一般为：X –> 右，Y –> 下，Z –> 前
激光雷达坐标系
激光雷达可以分为 2D 和 3D，每一帧信息为一帧点云。点云中的每个点可以表示为极坐标（距离和角度）或者笛卡尔坐标（x, y, z）形式，该坐标系是在激光雷达坐标系下。激光雷达坐标系方向因具体型号而异，一般来说平放的情况下，Z 轴和激光雷达旋转轴重合指向上，XY 平面和地面平行，以下为 Ouster OS-1-64 的坐标系示意图：
IMU 坐标系
和相机、激光雷达不同，IMU，GPS和轮速计等不采集环境信息，而是通过不同原理直接对传感器本身的状态进行测量。
IMU 的测量原理可以参考我之前写的博客：IMU 测量模型。IMU 可以分为 6 轴和 9 轴，6 轴 IMU 包含加速度计和陀螺仪。IMU 本身定义有一个坐标系，如下图所示：
IMU 加速度计和陀螺仪的测量值分别为 IMU 坐标系相对于惯性系的加速度和角速度。惯性系的选择随使用场景和精度要求不同可以有不同选择，对于小范围短时间内的定位可以近似认为局部坐标系为惯性系，这样解算载体状态较为简单，对于大范围长时间且要求精度高的场合可以选择世界坐标系（如下文提到的 ECEF 坐标系）为惯性系，此时解算载体状态需要考虑地球自转速度、载体所在高度等信息，计算相对较为复杂。
GPS
GPS 自身没有坐标系，它测量的是载体在世界坐标系（WGS84）下的坐标值。由经度和纬度组成。
轮速计
轮速计通常没有自身的坐标系，单个编码器的测量值为该轮的转速。通过两个轮子上编码器结合查分模型可以得到两轮连线中心的切向速度。如下图所示：
汽车坐标系
汽车坐标系为人为定义的固定在汽车的某一点上的坐标系。作为汽车系统本身的参考坐标系和各个传感器坐标系通过外参相联系。汽车坐标系方向一般为：FLU (前-左-上)，其位置随不同场景不同，以 Complex Urban Dataset 为例，其汽车坐标系定在后轮轴中心在地面高度的点，如下所示。在一些传感器较少的场合，也可以以 IMU、GPS 或者某个传感器坐标系为汽车坐标系。
导航坐标系
自动驾驶中里程计会追踪汽车在连续时刻中的运动，从而引出导航坐标系的概念。导航坐标系一般固定在汽车起始时刻的位置。在能够对齐重力（有 IMU 等惯性传感器）的情况下，方向一般有两种，分别为 ENU（东-北-天）或者 NED （北-东-地），如果不能够对齐重力，则方向为汽车坐标系的初始姿态。里程计测量出来的汽车的位姿为在导航坐标系下的位姿。相对于世界坐标些而言，可以将其视为一个局部坐标系。
在小范围下，通过固定一个初始点 （tie point），通过计算经纬度之间的距离可以作为局部的笛卡尔坐标系系使用。但是由于这种方式忽略了地球本身的曲率，因此只适用于较小范围，地球曲率可以忽略的区域的计算（通常小于 4km），但超过一定范围后变形会非常明显。此时如果想要控制物体结构的变形，可以考虑使用下面提到的针对地球球面提出的投影坐标系。
世界坐标系
实际情况中，无人汽车可能会多次经过同一区域进行定位和建图。导航坐标系的原点每次都不一致，显然不符合需求。因此需要有全局测量获得汽车在地球上的绝对位置，从而引出世界坐标系的概念。世界坐标系通常囊括整个地球范围，且原点固定在地球某处（地心），按其形式有地心坐标系和迪卡尔坐标系。由于地球本身是一个（椭）球体，因此从地心坐标系转换到迪卡尔坐标系需要遵循某种投影方式，中间会涉及到失真和变形的问题。
地理坐标系（GCS）
地心地固坐标系（ECEF）
地心地固坐标系（Earth-Centered,Earth-Fixed，ECEF）原点为地球质心，x 轴指向本初子午线与赤道的交点, y 轴垂直于xOz平面(即东经90度与赤道的交点), z 轴指向北极点，它和导航坐标系（以 ENU 为例）的关系如下图所示：
大地坐标系（LLA）
由于地球表面本身是个椭球面，因此用 ECEF 来描述地理位置不太直观。因此除了用笛卡尔坐标系描述以外，还可以用大地坐标系来描述某个点在地球上的位置。从上图中可以看出，要定位一个点在地球上的位置，有两种参数化方法，一种是使用 ECEF 系下的 $x, y, z$ 坐标来描述，另一种是使用大地坐标系下的 $\lambda, \phi, h$ 来描述，这种方法就是我们常用的经纬度（和高度）描述法。目前使用的标准为 WGS-84。从 WGS-84 转到 ECEF 的公式为：
$$
\begin{aligned}
x &= (N(\phi) + h)\cos{\phi}\cos{\lambda}\\
y &= (N(\phi) + h)\cos{\phi}\sin{\lambda}\\
z &= (\frac{b^2}{a^2}N(\phi) + h)\sin{\phi}\\
N(\phi) &= \frac{a^2}{\sqrt{a^2\cos{^2\phi} + b^2\sin{^2\phi}}} = \frac{a}{\sqrt{1 - e^2\sin{^2\phi}}}
\end{aligned}
$$
其中，$a$ 和 $b$ 分别为地球长轴和短轴的半径。
在计算两个经纬度表示的点的距离时，由于考虑到地球表面是一个曲面的原因，计算的距离并不是两点最短距离，而是两点间沿地球表面最短距离（Great-circle distance），计算公式为：
$$
\begin{aligned}
a &= \sin{^2(\Delta\phi/2)} + \cos{\phi}_1\cos{\phi}_2\sin{^2(\Delta\lambda/2)}\\
c &= 2\arctan(\sqrt{a}/\sqrt{(1- a)})\\
d &= Rc
\end{aligned}
$$
其中，$R = 6371000$ 为地球半径，$\phi, \lambda$ 分别为两点的纬度和经度。要对精度要求不高的情况下，公式可以简化为：
$$
\begin{aligned}
x &= \Delta\lambda\cos{\phi_m}\\
y &= \Delta\phi\\
d &= R\sqrt{x^2 + y^2}
\end{aligned}
$$
投影坐标系
由于地球表面是曲面，对于 ECEF 和 LLA 坐标系，都不方便计算两个点沿地球表面的距离，因此在可视化以及建图定位时，常需要将地球投影到一个 2D 平面上，因此引入投影坐标系的概念。这里介绍两种投影坐标系。
通用横轴墨卡托投影 UTM
UTM 投影的原理可以参考：聊聊GIS中的坐标系|再版。UTM 坐标将地球分为 60 个经度区和 20 个纬度区。如下图所示：
在 UTM 系下，地球上的每个点都可以通过四个参数确定位置：经度带号、维度带号、该点距该经度区中心子午线的投影距离以及该点距赤道的投影的距离。
网络墨卡托投影 Web Mercator
Web Mercator 投影的原理可以参考：Web Mercator 公开的小秘密。Web Mercator 能够将经纬度投影为两个 X/Y 坐标。计算关系为：
$$
\begin{aligned}
x &= r\text{Longitude}\\
y &= r\ln{(\tan(\frac{\pi}{4} + \frac{\text{Latitude}}{2}))}
\end{aligned}
$$
反投影关系为：
$$
\begin{aligned}
\text{Longitude} &= \frac{x}{r}\\
\text{Latitude} &= 2\arctan{(\exp{(y/r)})} - \frac{\pi}{4}
\end{aligned}
$$
UTM 和 Web Mercator 的对比
UTM 投影的思路是按经度进行分带，以若干条经线为中心左右 3° 或 6°进行投影。由于是横轴投影，因此可以保证中央的经线不会发生变形，离中央经线越远的地方变形程度越高。且纬度越高，横向的变形程度越大。
Web Mercator 为纵向投影，在南极和北极等高纬度有一部分无法被投影。可以按不同缩放比例，将地图投影到若干个瓦片，相比于 UTM 投影，其坐标是连续的，原点为本初子午线和赤道的交点，向东向北为正，向西向南为负。投影过程中纵向和横向都会发生变形，在高纬度地区尤其明显（对比格林兰岛和非洲的面积），但由于变形比例类似，因此可以保持物体的形状的发生不变。
传感器数据之间的流动关系
一般来说，典型的定位和建图场景情况，不同坐标系之间的关系如下：
传感器数据（如激光雷达，相机等）获得都是在其自身坐标系下的测量数据，包括状态测量传感器（如 IMU，轮速计）测量的也是基于本身的状态（虽然一般来说考虑 IMU 和编码器固连，可以认为这些传感器的状态和汽车本身的状态一致），因此需要将这些测量数据通过外参转化为汽车坐标系下的测量数据。
传感器数据统一到汽车坐标系之后，通过不同的融合方式和全局测量数据相比（在导航坐标系下或世界坐标系下）可以获取汽车在参考坐标系下的状态（定位），并且将传感器测量到的环境数据更新至参考坐标系中（建图），完成整体过程。在有绝对位置测量的情况下（经纬度），为了方便计算，一般先将其转换到投影坐标系下，且同样通过外参传递到汽车坐标系下，和其他传感器数据进行融合，对融合完成后的位置同样通过相反过程，最后获得地理坐标系下的经纬度数据。
在没有绝对测量值时可以将导航坐标系作为参考坐标系。而在有绝对测量值时，导航坐标系一般用来作为一个工具来计算周围特征之间的距离、角度等，相比于投影坐标系更加方便计算。
