1.1.1 坐标系.md

坐标系

在自动驾驶和机器人的多传感器融合定位算法中，想要获得鲁棒的、连续的、精确的全局定位结果，通常会充分利用各个传感器和模型，比如 GNSS/IMU/轮速编码器/载体模型/camera/LiDAR/高精地图，这其中会涉及到两大类坐标系，一个是常见的全局坐标系，也可以叫做地理坐标系或导航坐标系，算法最终的定位结果、GNSS/高精地图的输入都与此相关，另一个是局部坐标系，用于表示载体自身或者其上各个传感器的位置和朝向，IMU/轮速编码器/载体模型/camera/LiDAR的输入数据都在各自的局部坐标下。

一、ECEF 坐标系

图1. ECEF坐标系

ECEF（Earth-Centered Earth-Fixed ）地心地固坐标系，ECEF坐标系与地球固联，且随着地球转动。图中O即为坐标原点，位置在地球质心。X轴指向本初子午线（0度经度）和赤道（0deglatitude）的交点。Z轴通过原点指向北极。Y轴与X、Z轴构成右手坐标系。地球附近空间的点都可以在此坐标系下表示为 (x, y, z)。图中 (a, b) 分别为大地椭球的长轴和短轴，对于大地椭球不同的几何形状定义会导致不同的 ECEF 系，WGS84 就是最常见的一种 ECEF 系。

在 ECEF 坐标系下的点 $(x,y,z)$ 也可以用经纬高 $(\lambda, \phi, h)$ 来表示：

$$ \begin{align} x &=(N+h)cos\phi \cdot cos\lambda \\ y &=(N+h)cosϕ \cdot sinλ \\ z &=[N(1−e^2)+h]sinϕ \\ \end{align} $$

• e：椭球偏心率， $e^2= \frac{a^2-b^2}{a^2}$ ， a：长半径，b：短半径；
• N ：基准椭球体卯酉圈曲率半径， $N= \frac{a}{\sqrt{ 1-e^2 sin^2\phi }}$ ；

反过来已知 ECEF 值，也能求解经纬高，但由于 h 的计算式含有待求解的 φ ，所以我们只能借助迭代法来逼近求解 φ 和 h 的值，通常收敛得很快，经过 3~4 次迭代就可以结束计算。

$$ \begin{align} \lambda &= arctan(\frac{y}{x} ) \\ h &= \frac{p}{cos\phi } - N \\ \phi &= arctan[\frac{z}{p} (1-e^2 \frac{N}{N+h})^{-1}] \\ p &= \sqrt{x^2+y^2} \end{align} $$

二、WGS-84

WGS-84 也叫经纬高坐标系(经度(longitude)，纬度(latitude)和高度(altitude)LLA坐标系), 只是分为笛卡尔坐标(ECEF)表示或者经纬高表示而已。可以理解为对基准椭球体（与地球几何最吻合的椭球体，中心与地球质心重合，短轴与协议地球(CTP)自转轴一致，表面与大地水准面之间的高度差的平方和最小）进行明确的定义。

美国国防局下属的国防制图局指定的世界大地坐标系，经过多次的修改和完善，1984 年版的大地坐标系已经是一个小但精确的协议地心直角坐标系，其定义的基准椭球面与大地水准面在全球范围内的差异（即 ℎ 值）在 -100m 到 + 75m 之间。最初使用 GRS80 参考椭球体，但在出版后，在以后的版本中进行了一些小的改进。

基准大地参数	值 [单位]
基准椭球体的长半径 a	6378137.0 [m]
基准椭球体的短半径 b	6356752.3142 [m]
基准椭球体的极扁率 f	1/298.257223563

有些国家和地区采用本地的⼀个坐标系统，与之相应的本地地图上的经度、纬度可能与 GPS 接收机定位结果给出的 WGS-84 大地坐标系中的经度、纬度不相⼀致。这种差异主要是本地地图所基于的基准椭球体可能只是对所在国家或局部地区的地球球体的逼近，而 WGS-84 是对整个地球球体的逼近。

比如 ETRS89 是欧盟推荐的欧洲大地测量参考坐标系，其重点是欧亚板块作为一个静态平面，不会受到大陆漂移的影响，它是欧洲唯一用于测绘目的的大地测量基准。它对欧洲的作用与北美的 NAD-83 相同，NAD-83 是北美板块作为一个整体是静态的，在美国、加拿大和墨西哥用于测绘和测量的基准面。

图2. 经纬高坐标系

日常导航中常用的 GNSS（Global Navigation Satellite System）定位结果都是基于此坐标系输出的三维坐标 $(\phi, \lambda, h)$ ，也就是常说的 纬度、精度、高度。

其中：

• 大地纬度是过用户点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。

• 大地经度是过用户点P的子午面与本初子午线之间的夹角。经度值在-180°到+180°之间。

• 大地高度h是过用户点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。

三、东北天坐标系（ENU）

站心坐标系，即东北天坐标系，可以直接的理解为地心地固直角坐标系通过旋转和平移，把坐标原点平移到了选中的球面 base 点，xyz 坐标轴分别指向该点的东边、北边、天顶。

图3. 东北天坐标系

在 ECEF 坐标系中一个点的坐标，或者是一个观测的向量，可以通过平移和旋转变换成站心坐标系坐标。当变换到了站心坐标系下时，观测点的坐标的三轴分量和角度有了更贴合实际应用的物理意义。

$$ \begin{bmatrix} e \\ n \\ u \\ \end{bmatrix} = S \cdot \begin{bmatrix} x \\ y \\ z \\ \end{bmatrix} $$

$$ \begin{bmatrix} x \\ y \\ z \\ \end{bmatrix} = S^{-1} \cdot \begin{bmatrix} e \\ n \\ u \\ \end{bmatrix} $$

$$ S = \begin{bmatrix} -sin\lambda & cos\lambda & 0 \\ -sin\phi cos\lambda & -sin\phi sin\lambda & cos\phi \\ cos\phi cos\lambda & cos\phi sin\lambda & sin\phi \\ \end{bmatrix} $$

$(\lambda, \phi)$ 为 base 点的经纬度，S 本质上是描述 ECEF 先绕 ECEF 的 z 轴旋转 $\lambda+90°$ ，然后再绕新的 x 轴旋转 $90°-\phi$ 的旋转矩阵。

而一个物体 $p$ 在 ECEF 坐标下的 pose 为 $(R,t)$ ，它在 base 点 $b$ 的 ENU 坐标下的 pose 计算过程如下：

首先计算 base 点所在的 ECEF 坐标 $(x_0, y_0, z_0)$ ，

$$ \begin{align} x_0 &=(N+h)cos\phi \cdot cos\lambda \\ y_0 &=(N+h)cosϕ \cdot sinλ \\ z_0 &= [\frac{a}{\sqrt{ 1-e^2 sin^2\phi }}(1−e^2)+h]sinϕ \\ \end{align} $$

则 ECEF 下的观测向量 $\overrightarrow{B P} = (x- x_0, y- y_0 -y, z -z_0 )$

ENU 下的坐标

$$ t'=\begin{bmatrix} x' \\ y' \\ z' \\ \end{bmatrix} = S \cdot \begin{bmatrix} x- x_0 \\ y- y_0 \\ z- z _0 \\ \end{bmatrix} $$

旋转 $R' = S*R$

得到在 ENU 下的 pose， $(R', t')$

四、UTM 坐标系

（一）墨卡托投影（Mercator Projection）

图4. 墨卡托投影

墨卡托投影是将地球表面绘制成矩形世界地图的正轴等角圆柱投影，在该投影中线型比例尺在图中任意一点周围都保持不变，从而可以保持大陆轮廓投影后的角度和形状不变（即等角）。

假设地球被围在一中空的圆柱里，其基准纬线与圆柱相切（赤道）接触，然后再假想地球中心有一盏灯，把球面上的图形，沿着光线投影到圆柱体上，再把圆柱体展开，这就是一幅选定基准纬线上的“墨卡托投影”绘制出的地图。

墨卡托投影没有角度变形，由每一点向各方向的长度比相等，它的经纬线都是平行直线，且相交成直角，经线间隔相等，纬线间隔从基准纬线处向两极逐渐增大。墨卡托投影的地图上长度和面积变形明显，但基准纬线处无变形，从基准纬线处向两极变形逐渐增大，但因为它具有各个方向均等扩大的特性，保持了方向和相互位置关系的正确.

由于墨卡托地图的线性比例随着纬度的增加而增加，因此它会使远离赤道的地理对象的尺寸失真，使面积产生变形（即沿着圆柱轴越远，失真越大）。在北纬或南纬大于70°的纬度上，墨卡托投影实际上是不可用的，因为两极的线性比例变得无限大。因此，墨卡托地图永远无法完整显示极地区域。

（二）UTM （通用横轴墨卡托投影）

UTM是横轴等角割圆柱投影，可以标记地球南纬80°至北纬84°之间的所有位置。也就是圆柱底面直径两端构成的两条轴线分别与南纬80°纬线和北纬84°纬线相割。

图5. UTM

UTM 经度区范围为1到60，其中58个区的东西跨度为 6°。纬度范围从 80°S 到 84°N 之间的所有区域。一共有 20个UTM 纬度区，每个区的南北跨度为 8°，使用字母 C 到 X 标识（其中没有字母 I 和 O），它们覆盖了部分南极和北极区。

图6. UTM

由于是横轴墨卡托投影的缘故，之前提到，沿着圆柱轴越远，失真越大（但你看上图左右方向又是没有失真的），所以 UTM 在经度方向上进行分区，每个分区的投影带用不同的中央子午线来进行变形的修正（越靠近圆柱相切部分失真越小），你可以理解为想要投影经线 0° 附近的区域，我们就把横轴割圆柱的横截面转到和 0° 经线平面重合，想要投影东经 90° 附近的区域，我们就把横轴割圆柱的横截面旋转到和东经 90° 的平面重合，来降低每个分区的失真。

所以可以看出，UTM 坐标系中，不同分区之间的坐标是不连续的！跨区时需要处理坐标跳变的问题。

五、ECI 坐标系

ECI（Earth-centered inertial）地心惯性系，原点与 ECEF 一样 ，Z 轴朝向也与 ECEF 一样，沿地轴方向朝向北极， X轴和Y轴位于赤道平面内，与Z轴满足右手法则，并且 X 轴指向某个位置相对稳定恒星，比如 GNSS 算法内部会用指向太阳的 ECI 系。也就是说不随着地球的自转而发生变化。它是一个朝向非常固定的坐标系，可以作为地球附近传感器输出的惯性坐标系。比如 IMU 测量得到的加速度，角速度都是相对于这个坐标系的，因此当在 ECEF 下进行位姿递推时，是需要考虑地球自转效应的。

六、Frenet坐标系

我们通常使用笛卡尔坐标系描述物体的位置，但笛卡尔坐标系对车辆来说并不是最佳选择。即使给出了车辆位置(x,y)，如果我们不知道道路在哪，就很难知道车辆行驶了多远也很难知道车辆是否偏离了道路中心。

图7. 笛卡尔坐标系

笛卡尔坐标系的替代解决方案为Frenet坐标系。Frenet坐标系描述了汽车相对于道路的位置。在Frenet框架中，s代表沿道路的距离，也被称为纵坐标。d表示与纵向线的位移，也被称为横坐标。在道路的每个点上，横轴和纵轴都是垂直的。纵坐标表示道路中的行驶距离，横坐标表示汽车偏离中心线的距离。

图8. Frenet坐标系