[自动驾驶算法][从0开始轨迹预测]：二、自动驾驶系统中常用的坐标系及相应的转换关系

写在前面：上一篇文章中，我们介绍了坐标转换的基础知识，接下来，我们将介绍由汽车的传感器到全局定位会涉及到的若干个坐标系统。

下图是自动驾驶系统中常见的坐标系统：

​ 一般来说，典型的定位和建图场景情况，不同坐标系之间的关系如下：

​ 环境传感器数据（如激光雷达，相机等）都是在其自身坐标系下测量得到的数据，包括状态测量传感器（如 IMU，轮速计），其测量得到的也是基于本身状态（虽然一般来说考虑 IMU 和编码器固连，可以认为这些传感器的状态和汽车本身的状态一致），因此需要将这些测量数据通过外参转化到车体坐标系下。

​ 传感器数据统一到车体坐标系后，通过不同的感知融合方式和全局测量数据相比（在导航坐标系下或世界坐标系下）可以获取车辆在参考坐标系下的状态（定位），并且将传感器测量到的环境数据更新至参考坐标系中（建图），完成整体过程。在有绝对位置测量的情况下（经纬度），为了方便计算，一般先将其转换到投影坐标系下，且同样通过外参传递到车体坐标系下，和其他传感器数据进行融合，对融合完成后的位置同样通过相反过程，最后获得地理坐标系下的经纬度数据。

​ 在没有绝对测量值时可以将导航坐标系作为参考坐标系。而在有绝对测量值时，导航坐标系一般用来作为一个工具来计算周围特征之间的距离、角度等，相比于投影坐标系更加方便计算。

1. 传感器坐标系

​ 车载传感器都有其自身的坐标系，按照其测量原理可以分为相对测量（测量环境信息）和绝对测量（测量自身信息）。相对测量的传感器如相机、激光雷达，他们是通过测量环境信息来进行间接定位，而绝对测量如 IMU、轮速计、GPS 则是通过直接测量自身的运动状态（速度、位置、姿态等）来进行定位。对于相对测量的传感器而言，其测量值通常是基于其自身传感器；而绝对测量的测量结果一般则是自身坐标系相对于其他坐标系（局部坐标系或世界坐标系等）的结果。下面对常见的测量传感器进行归纳。

1.1 相机坐标系统

摄像机/摄像头以其低廉的价格、丰富的图像信息，成为自动驾驶中最受人们关注的传感器之一。摄像头的作用是把三维世界中的形状、颜色信息，压缩到一张二维图像上。

基于摄像头的感知算法则是从二维图像中提取并还原三维世界中的元素和信息，如车道线、车辆、行人、异性障碍物等，并计算他们与自己的相对位置。

1) 相机相关基础知识

单反相机参数之ISO/焦距篇

景深概念与计算

光轴：一条垂直穿过理想透镜中心的光线。

焦点：与光轴平行的光线射入凸透镜时，理想的凸镜应该是所有的光线会聚在透镜后面一点上，这个会聚所有光线的一点，就叫做焦点。

焦距：一般指镜片的中心到焦点为止的光轴上的距离，入射平行光线（或其延长线）与出射会聚光线（或其延长线）相交，就能确定折射主面，这个想象的平面与镜头光轴相交处就是主点。改变物体在相机中的成像大小。

光圈：光圈对照片的影响主要有两大项，一是镜头的进光量，二是相片的景深。光圈越大（f 值越小），景深越浅（前景/背景看上去就像化掉一样）；光圈越小（f 值越大），景深越深（前景/背景看上去比较清楚)。镜头焦距确定了画面的取景范围，而光圈主要影响画面虚实。

景深： 是指在一幅图像中清晰的范围，景深的深浅主要取决于以下元素：镜头的光圈大小、镜头的焦距长短、摄影主体的远近、感光元件的大小

2) 相机各坐标系

图像/像素坐标系

电脑上存储的照片或图像，一般以图像的左上角为原点，像素坐标系$O_{uv}$，以向右为$x-u$正方向，向下为$y-v$正方向，以“像素”为单位。$u,v$代表像素的行数和列数，无实际的物理单位。

相机坐标系

由于图像坐标系向右为$x$，向下为$y$，所以相机坐标系以镜头主光轴中心为原点，向右为x正方向，向下为y正方向，向前（景深方向）为z正方向。这样，x，y方向与图像/像素坐标系的方向吻合，z方向即为景深。

像平面坐标系

为了能够定量描述三维空间到二维图像的映射关系，图形学里引入了像平面坐标系。它是相机坐标系的一个平移，中心仍在摄像机主光轴上，距离光轴中心的距离等于摄像机的焦距$f$ ，根据小孔成像原理，相机会在光轴中心后方的底片上成一个缩小的倒像，是真正的像平面$X_f^{\prime }{O}_f^{\prime }{Y}_f^{\prime }$。但是为了分析和计算方便，我们会在光轴中心前方设立一个虚拟像平面。虚拟像平面上的成像为正像，大小与真实倒像相同$X_f{O}_f{Y}_f$。

3) 相机各坐标系之间的转换

像平面坐标系到像素坐标系的转换（平移缩放变换）

假设$(u_0,v_0)$代表像平面原点（焦点）在u-v坐标系下的坐标，dx和dy分别表示每个像素在像平面系下横轴x和纵轴y的物理尺寸；已知p点在像平面系下的坐标为$(x,y)$，那么$\frac{x}{dx}$单位就是像素了。

$$\begin{gathered} u=\frac{x}{dx}+u_0 \\ v=\frac{y}{dy}+v_0 \\ \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right] \end{gathered}$$

相机坐标系转像平面坐标系（透视变换）：

对于成像物体上的点$P(X,Y)$，其在像平面上对应点$p(x,y)$，根据相似三角形原理，它们之间的坐标对应关系有：

$$\begin{gathered} \frac{f}{Z}=\frac{x}{X}=\frac{y}{Y} \\ x=f\frac{X}{Z},y=f\frac{Y}{Z} \\ \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right] \end{gathered}$$

相机坐标系到像素坐标系

由前面两小节的推导，我们已经知道由相机坐标系到像平面坐标系，再由像平面坐标系到像素坐标系的转换，则相机坐标系到像素坐标系的转换为：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$

$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{cccc}\frac{f}{dx}& 0& u_0& 0\\ 0& \frac{f}{dy}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{cccc}{f}_x& 0& c_x& 0\\ 0& f_y& c_y& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\frac{1}{Z}\mathbf{K}\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$

其中$\mathbf{K}$​为相机的内参矩阵，由fx 和 fy：焦距，用像素表示。fx 是 x 轴方向的焦距，fy 是 y 轴方向的焦距。cx 和 cy：是图像的主点坐标，也就是图像中心的像素坐标。

世界坐标系到像素坐标系（旋转平移变换）

见上一篇坐标变换～

1.2 激光雷达坐标系

激光雷达可以分为 2D 和 3D，每一帧信息为一帧点云。点云中的每个点可以表示为极坐标（距离和角度）或者笛卡尔坐标（x, y, z）形式，该坐标系是在激光雷达坐标系下。激光雷达坐标系方向因具体型号而异，一般来说平放的情况下，Z 轴和激光雷达旋转轴重合指向上，XY 平面和地面平行，以下为 Ouster OS-1-64 的坐标系示意图：

1.3 IMU坐标系

和相机、激光雷达不同，IMU，GPS和轮速计等不采集环境信息，而是通过不同原理直接对传感器本身的状态进行测量。IMU 的测量原理可以参考我之前写的博客：IMU 测量模型。

IMU 加速度计和陀螺仪的测量值分别为 IMU 坐标系相对于惯性系的加速度和角速度。惯性系的选择随使用场景和精度要求不同可以有不同选择，对于小范围短时间内的定位可以近似认为局部坐标系为惯性系，这样解算载体状态较为简单，对于大范围长时间且要求精度高的场合可以选择世界坐标系（如下文提到的 ECEF 坐标系）为惯性系，此时解算载体状态需要考虑地球自转速度、载体所在高度等信息，计算相对较为复杂。

1.4 GPS

GPS 自身没有坐标系，它测量的是载体在世界坐标系（WGS84）下的坐标值。由经度和纬度组成。

1.5 轮速计

轮速计通常没有自身的坐标系，单个编码器的测量值为该轮的转速。通过两个轮子上编码器结合查分模型可以得到两轮连线中心的切向速度。如下图所示：

2. 车体坐标系

​ 车体坐标系用来描述车辆周围的物体和本车之间的相对位置关系。目前学术界和工业界有几种比较常用的车体坐标系定义方式。分别是ISO国际标准定义，SAE（Society of Automotive Engineers）汽车工程协会定义，和基于惯性测量单元IMU的坐标定义。

​ 在车辆动力学分析中，ISO定义的车体坐标系较为常见。SAE定义的车体坐标系与航空航天领域常用的集体坐标系相一致。基于IMU定义的车体坐标系，则在IMU的相关应用中较为常见。无论使用哪一种坐标系定义，都可以完成对车身位姿的描述，以及确定周围物体和本车间的相对位置关系。

3. 导航坐标系

自动驾驶中里程计会追踪汽车在连续时刻中的运动，从而引出导航坐标系的概念。导航坐标系一般固定在汽车起始时刻的位置。在能够对齐重力（有 IMU 等惯性传感器）的情况下，方向一般有两种，分别为 ENU（东-北-天）或者 NED （北-东-地），如果不能够对齐重力，则方向为汽车坐标系的初始姿态。里程计测量出来的汽车的位姿为在导航坐标系下的位姿。相对于世界坐标些而言，可以将其视为一个局部坐标系。

在小范围下，通过固定一个初始点 （tie point），通过计算经纬度之间的距离可以作为局部的笛卡尔坐标系使用。但是由于这种方式忽略了地球本身的曲率，因此只适用于较小范围，地球曲率可以忽略的区域的计算（通常小于 4km），但超过一定范围后变形会非常明显。此时如果想要控制物体结构的变形，可以考虑使用下面提到的针对地球球面提出的投影坐标系。

4. 世界坐标系

实际情况中，自动驾驶汽车可能会多次经过同一区域进行定位和建图。导航坐标系的原点每次都不一致，对于相同的场景，导航坐标系表现的结果也是不一致的。因此需要有全局测量获得汽车在地球上的绝对位置，从而引出世界坐标系的概念。世界坐标系通常囊括整个地球范围，且原点固定在地球某处（地心），按其形式有地心坐标系和笛卡尔坐标系。由于地球本身是一个（椭）球体，因此从地心坐标系转换到笛卡尔坐标系需要遵循某种投影方式，中间会涉及到失真和变形的问题。

4.1 地理坐标系（GCS）

1. 地心地固坐标系（ECEF）

地心地固坐标系（Earth-Centered,Earth-Fixed，ECEF）原点为地球质心，x 轴指向本初子午线与赤道的交点, y 轴垂直于xOz平面(即东经90度与赤道的交点), z 轴指向北极点，它和导航坐标系（以 ENU 为例）的关系如下图所示：

2. 大地坐标系（LLA）

由于地球表面本身是个椭球面，因此用 ECEF 来描述地理位置不太直观。因此除了用笛卡尔坐标系描述以外，还可以用大地坐标系来描述某个点在地球上的位置。从上图中可以看出，要定位一个点在地球上的位置，有两种参数化方法，一种是使用 ECEF 系下的 $x,y,z$ 坐标来描述，另一种是使用大地坐标系下的$\lambda ,\varphi ,h$来描述，这种方法就是我们常用的经纬度（和高度）描述法。目前使用的标准为 WGS-84。

4.2 投影坐标系

由于地球表面是曲面，对于 ECEF 和 LLA 坐标系，都不方便计算两个点沿地球表面的距离，因此在可视化以及建图定位时，常需要将地球投影到一个 2D 平面上，因此引入投影坐标系的概念。这里介绍两种投影坐标系。

1. 通用横轴墨卡托投影 UTM

UTM 投影的原理可以参考：聊聊GIS中的坐标系|再版。UTM 坐标将地球分为 60 个经度区和 20 个纬度区。在 UTM 系下，地球上的每个点都可以通过四个参数确定位置：经度带号、维度带号、该点距该经度区中心子午线的投影距离以及该点距赤道的投影的距离。如下图所示：

2. 网络墨卡托投影 Web Mercator

Web Mercator 投影的原理可以参考：Web Mercator 公开的小秘密。Web Mercator 能够将经纬度投影为两个 X/Y 坐标。

3. UTM和Web Mercator的对比

UTM 投影的思路是按经度进行分带，以若干条经线为中心左右 3° 或 6°进行投影。由于是横轴投影，因此可以保证中央的经线不会发生变形，离中央经线越远的地方变形程度越高。且纬度越高，横向的变形程度越大。

Web Mercator 为纵向投影，在南极和北极等高纬度有一部分无法被投影。可以按不同缩放比例，将地图投影到若干个瓦片，相比于 UTM 投影，其坐标是连续的，原点为本初子午线和赤道的交点，向东向北为正，向西向南为负。投影过程中纵向和横向都会发生变形，在高纬度地区尤其明显（对比格林兰岛和非洲的面积），但由于变形比例类似，因此可以保持物体的形状的发生不变。