LLA、ENU、ECEF坐标系

最近在做规划控制的坐标系转换时，发现之前遗留的坐标系问题没有解决，因此单独整理一下。

1.左右手坐标系

对于单位坐标系，一般分为左手坐标系和右手坐标系，他们的区别在于三个坐标轴的组合。

1.1右手坐标系

在三维坐标系中，Z轴的正轴方向是根据右手定则确定的，右手定则也决定三维空间中任一坐标轴的正旋转方向。要标注X、Y和Z轴的正轴方向，就将右手背对着屏幕放置，拇指即指向X轴的正方向，伸出食指和中指，如下图所示，食指指向Y轴的正方向，中指所指示的方向即是Z轴的正方向。

1.2左手坐标系

伸出左手，让拇指和食指成“L”型，大拇指向右，食指向上。其余的手指指向前方，这样就建立了一个左手坐标系。拇指、食指和其余手指分别代表X、Y和Z轴的正方向。

2.坐标系简介

2.1 ECEF坐标系

也叫地心地固坐标系（Earth-Centered,Earth-Fixed，简称ECEF）简称地心坐标系，是一种以地心为原点的地固坐标系（也称地球坐标系），是一种笛卡儿坐标系。原点 O (0,0,0)为地球质心，Z 轴与地轴平行指向北极点，X轴指向本初子午线与赤道的交点，Y轴垂直于XOZ平面(即东经90度与赤道的交点)构成右手坐标系。

2.2 WGS-84坐标

也叫经纬高坐标系（LLA坐标系）、全球地理坐标系、大地坐标系。可以说是最为广泛应用的一个地球坐标系，能够给出一点的大地纬度、大地经度和大地高程而更加直观地告诉我们该点在地球中地位置。
一句话就是把前面提到地ECEF坐标系用在GPS中，就是WGS-84坐标系。

2.3 ENU

也叫站心坐标系、站点坐标系、东-北-天坐标系，英文名称是local Cartesian coordinates coordinate system，以用户所在位置P为坐标原点。坐标定义为：X轴指向东，Y轴指向北，Z轴指向天顶。
ENU局部坐标系采用三维直角坐标系来描述地球表面，实际应用较为困难，因此一般使用简化后的二维投影坐标系来描述。在众多二维投影坐标系中，统一横轴墨卡托（The Universal Transverse Mercator ，UTM）坐标系是一种应用较为广泛的一种。UTM坐标系统使用基于网格的方法表示坐标，他将地球分为60个经纬区，每个区包含6度的经度范围，每个区内的坐标均基于横轴墨卡托投影。

3 坐标系间的转换

参数	WGS-84
基于椭球体的长半径a	6378137 m
基于椭球体的极扁率f	1/298.257222101
地球自转角速度We	7.292115E-5 rad/s
地球引力常数	3.986004418E14 m3/s2
光速	2.99792458E8 m/s

3.1LLA坐标系转ECEF坐标系

LLA坐标系下的(lon,lat,alt)转换为ECEF坐标系下点(X,Y,Z)：
$$\{\begin{array}{c}X=(N+alt)cont(lat)cos(lon)\\ Y=(N+alt)cos(lat)sin(lon)\\ Z=(N(1-f{)}^2+alt)sin(lat))\end{array}$$

其中，f为极扁率，N为基准椭球体的曲率半径
$$N=\frac{a}{\sqrt{1-f(2-f)\ast si{n}^2(lat)}}$$

3.2ECEF坐标系转LLA坐标系

ECEF坐标系下点(X,Y,Z)转换为LLA坐标系下的(lon,lat,alt)
$$\begin{gathered} lon=arctan(\frac{y}{x}) \\ alt=\frac{p}{cos(lat)-N} \\ lat=arctan[\frac{z}{p}{\left(1-e^2\frac{N}{N+alt}\right)}^{-1}] \\ p=\sqrt{x^2+y^2} \end{gathered}$$
一开始lon是未知的，可以假设为0，经过计策迭代之后就能收敛。

3.3ECEF坐标系转ENU坐标系

用户所在坐标原点$P_0=(x_0,y_0,z_0)$，计算点$P=(x,y,z)$，在以点$P_0$为坐标原点的ENU坐标系位置(e,n,u)，这里需要用到LLA坐标系的数据，$P_0$的LLA坐标点为$LL{A}_0=(lo{n}_0,la{t}_0,al{t}_0)$

$$\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]-\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]$$
$$\left[\begin{array}{c}e\\ n\\ u\end{array}\right]=S\cdot \left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]=S\cdot \left[\begin{array}{ccc}-sin(lo{n}_0)& cos(lo{n}_0)& 0\\ -sin(la{t}_0)cos(lo{n}_0)& -sin(la{t}_0)sin(lo{n}_0)& cos(la{t}_0)\\ cos(la{t}_0)cos(lo{n}_0)& cos(la{t}_0)sin(lo{n}_0)& sin(la{t}_0)\end{array}\right]=S\cdot \left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]$$

3.4ENU坐标系转ECEF坐标系

S为单位正交矩阵
$$S^{-1}=S^T$$
反之
$$\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]=S^{-1}\cdot \left[\begin{array}{c}e\\ n\\ u\end{array}\right]=S^T\cdot \left[\begin{array}{c}e\\ n\\ u\end{array}\right]$$

3.5LLA坐标系转ENU坐标系

上述可以看到，从LLA坐标系转换到enu坐标系有较多计算量，在考虑地球偏心率ee很小的前提下，可以做一定的近似公式计算
$$\left[\begin{array}{c}{\Delta }_e\\ {\Delta }_n\\ {\Delta }_u\end{array}\right]=\left[\begin{array}{ccc}a\cdot cos(lat)\cdot \Delta lon& 0& 0\\ 0& a\cdot lat& 0\\ 0& 0& \Delta alt\end{array}\right]$$

4全局坐标系和车身坐标系

转载自 https://zhuanlan.zhihu.com/p/404773542

旋转矩阵

这张图可以很好地解释旋转矩阵的用法，这里有两个坐标系V和B，P点在这个空间中是一个固定的点，在不同坐标系下的表达是不一样的，比如在B坐标系里面，坐标是

加上平移