坐标系转换之LLA、ECEF、ENU互转

1 名词解释
1.1 ECEF坐标系
也叫地心地固直角坐标系。其原点为地球的质心，x轴延伸通过本初子午线（0度经度）和赤道（0deglatitude）的交点。 z轴延伸通过的北极（即，与地球旋转轴重合）。 y轴完成右手坐标系，穿过赤道和90度经度。

1.2 ENU坐标系
也叫东北天坐标系，坐标系定义为： X轴指向东边，Y轴指向北边，Z轴指向天顶。

1.3 LLA坐标系
也就是也叫经纬高坐标系(经度(longitude)，纬度(latitude)和海拔(altitude)LLA坐标系)。也叫做全球地理坐标系、大地坐标系、WGS-84坐标系。

1.4 GCJ02坐标系
GCJ02坐标系，也叫做火星坐标系，是中国国家测绘局制定的一种地理坐标系，它的值与LLA比较接近，LLA与GCJ02坐标系本质上可看成相互之间的微小偏移。

 

2 坐标转换
在经纬高坐标系下，数据并不直观，因此我们更希望将坐标数值转换为常规的直角坐标系（笛卡尔坐标系）下的数据。一种很常见的思想是，假设地球为椭球，然后以地心为原点建立直角坐标系，然后将地球表面的经纬高数据直接转换为对应的直角坐标系数据，也就是常说的LLA转ECEF。虽然这时的ECEF坐标数值已经是直角坐标系下的数值，因为这时的数值很大，受限于程序语言数值计算范围与精度，ECEF的数值在后续数据处理中容易产生数值异常或精度不足的问题。由于我们处理的数据，往往基于地球某一个区域，而这个区域相对于地球来说本身较小，因此这时会在这个区域选择一个锚点，然后以这个锚点为中心建立对应的坐标系，这时以这个区域建立的坐标数值就会小很多，不会带来计算溢出等问题，这时的转换就是ECEF转ENU。ECEF转ENU坐标系本身是直角坐标系下的平移与旋转，因此这种转换本身不破坏数据间的相对关系。

为了后续表示方便，这里选取WGS-84坐标系下经纬度参数，地球基准椭球体的极扁率\(f=\)1/298.257223563，基准椭球体的长半径 \(a=\)6378137.0m，短半径\(b=a\sqrt{1-e^2}\)，其中\(e^2=f(2-f)\)。

2.1 LLA转ECEF
这里约定LLA的经度为\(\alpha\)，纬度为\(\beta\)，海拔为\(h\)。LLA转ECEF的坐标 \((x,y,z)\)为  
(1)
$$
\begin{aligned}
x &= (n+h)\cos \beta \cos \alpha
\\
y &= (n + h)\cos \beta \sin \alpha
\\
z &= (n(1-e^2)+h)\sin \beta
\end{aligned}
$$

上面公式中的\(n\)为如下值  
(2)
$$
n = \dfrac{a}{\sqrt{1-e^2\sin^2\beta}}
$$
2.2 ECEF转LLA
ECEF坐标\((x,y,z)\)转LLA的经度\(\alpha\)，纬度\(\beta\)，海拔\(h\)。  
ECEF转LLA，本质上是求解公式(1)的非线性方程组，通过观察(1)中的第1与第2个等式，明显可以得到经度的计算表达式，即  
(3)
$$
\alpha = \arctan(y, x)=\arctan(\dfrac{y}{x})
$$
接下来，还剩计算纬度与海拔。计算方法上，本质上还是求解非线性方程组。本文提供三种求解方法。

2.2.1 二分法求解

对(1)进行变形有  
(4)
$$
\begin{aligned}
x^2+y^2 &= (n+h)^2(1-\sin^2 \beta)
\\
z &= (n(1-e^2)+h)\sin \beta
\end{aligned}
$$
为了便于计算，这里设置如下中间量  
(5)
$$
\begin{aligned}
r &=\sqrt{x^2+y^2}
\\
t &= \sin \beta
\end{aligned}
$$
将(5)代入(4)消去\(h\)可以得到
(6)
$$
t=(\dfrac{z}{r}+\dfrac{a}{r}\dfrac{e^2t}{\sqrt{1-e^2t^2}})\sqrt{1-t^2}
$$
这里(6)看似一个不动点迭代式，可惜这个算式并不是对每个值都迭代收敛。进一步，我们设置如下形式  
(7)
$$
f(t)=(\dfrac{z}{r}+\dfrac{a}{r}\dfrac{e^2t}{\sqrt{1-e^2t^2}})\sqrt{1-t^2}-t
$$
从公式(7)我们可以发现，如下等式成立  
(8)
$$
\begin{cases}
f(1)\ \ \ =-1 < 0
\\
f(-1)=1 \ \ \ > 0
\end{cases}
$$
由于\(t\)在[-1,1]区间取值，公式(8)满足使用二分法求解的条件。因此，使用二分法可以求解出满足精度的\(t\)。最终在二分结束后，使用如下方式计算纬度与海拔  
(9)
$$
\begin{aligned}
\beta &=\arcsin(t)
\\
h&=\begin{cases}
\dfrac{r}{\sqrt{1-t^2}}-\dfrac{a}{\sqrt{1-e^2t^2}},\ |t|<0.1
\\
\\
\dfrac{z}{t}-\dfrac{a(1-e^2)}{\sqrt{1-e^2t^2}},\ |t| \geq 0.1
\end{cases}
\end{aligned}
$$

因此，使用二分法求解步骤为：使用(7)进行二分求解出\(t\)；使用(9)计算出纬度与海拔；使用(3)计算出经度。

二分法在求解出满意精度时，一般需要分割40次。

注意: 在(9)式计算海拔时，为了防止计算除以一个接近0的值，因此以0.1数值作为分段求解。 

2.2.2 不动点迭代求解
二分法虽然有指数级收敛速度，但是依然不够快，这里考虑不动点迭代算法。

为了构建更快的迭代收敛式，这里设置如下中间变量  
(10)
$$
s = (n+h)\sin\beta
$$
将(10)算式代入公式(4)的第1个方程有
(11)
$$
\sin\beta = \dfrac{s}{\sqrt{r^2+s^2}}
$$
将(11)代入公式(4)的第2个方程有  
(12)
$$
s=z+\dfrac{ae^2s}{\sqrt{r^2+s^2-e^2s^2}}
$$
上面表达式即为不动点迭代式，为了证明这个迭代式满足不动点收敛迭代，这里设  
(13)
$$
g(s)=z+\dfrac{ae^2s}{\sqrt{r^2+s^2-e^2s^2}}
$$
对于(13)我们有如下结果  
(14)
$$
\begin{aligned}
g(s_+)&=z+\dfrac{ae^2}{\sqrt{\dfrac{r^2}{s_+^2}+1-e^2}} <z+\dfrac{ae^2}{\sqrt{1-e^2}} \leq |z|+\dfrac{ae^2}{\sqrt{1-e^2}}<|z|+a
\\
g(s_{-}) &=z-\dfrac{ae^2}{\sqrt{\dfrac{r^2}{s_-^2}+1-e^2}} >z-\dfrac{ae^2}{\sqrt{1-e^2}} \geq -|z|-\dfrac{ae^2}{\sqrt{1-e^2}}>-|z|-a
\end{aligned}
$$
因为\(g(s)\)在区间\([-\infty, \infty]\)内的取值在\((-|z|-a, |z|+a)\)，说明(12)算式在区间 \([-\infty,\infty]\)存在不动点。

进一步  
(15)
$$
\begin{aligned}
g'(s)&=\dfrac{ar^2e^2}{(r^2+s^2-e^2s^2)^{1.5}}
\\
&=\dfrac{ae^2}{\sqrt{r^2+s^2(1-e^2)}}\dfrac{1}{1+(\dfrac{s}{r})^2(1-e^2)}
\\
&<\dfrac{ae^2}{\sqrt{r^2+s^2(1-e^2)}}
\\
&<\dfrac{ae^2}{r} \leq \dfrac{ae^2}{b}=\dfrac{e^2}{\sqrt{1-e^2}}
\\
&<\dfrac{1}{148.88}\end{aligned}
$$
因此有\(0<g'(s)<\dfrac{1}{148.88}\), 结合(14)的结果，说明(12)算式在区间 \([-\infty, \infty]\)存在不动点迭代，且迭代收敛到唯一值。从(15)可以发现迭代收敛速度较快。在实际迭代时，可以取 \(s=0\)或 \(s=z\)作为初值，然后迭代到满意精度。

最终，迭代完成后，可以使用如下方式获取纬度与海拔：  
(16)
$$
\begin{aligned}
\beta &= \arctan(s,r)=\arctan(\dfrac{s}{r})
\\
h&=\begin{cases}
\sqrt{r^2+s^2}-n,\ |\beta|<\dfrac{7\pi}{16}
\\
\\
\dfrac{z}{\sin\beta}-n(1-e^2),\ |\beta|\geq \dfrac{7\pi}{16}
\end{cases}
\end{aligned}
$$

因此，使用不动点迭代法求解步骤为：使用(12)迭代求解出\(s\)；使用(16)计算出纬度与海拔；使用(3)计算出经度。

不动点迭代法在求解出满意精度时，一般迭代4-7次。  

2.2.3 直接求解
使用不动点迭代速度已经很快，但如果还想确保每次计算调用时间可控，那就使用直接求解方法。

对公式(12)进行变形，可得到如下结果：  
(17)
$$
(1-e^2)s^4-2z(1-e^2)s^3+(r^2+z^2(1-e^2)-a^2e^4)s^2-2zr^2s+z^2r^2=0
$$

很明显，(17)是一个关于\(s\)的一元四次方程，由于一元四次方程有单独的求根公式，因此使用求根公式可以求出对应的解。

由于(12)式变到(17)式忽略了正负信息，因此对(17)求根，会存在多解的情况，这时可以将解代入(12)式进行验证，只有一个唯一解满足(12)式。因此最终求得\(s\)。由于(12)式有一定计算量，为了快速筛掉不满足条件的解，通过公式(1)与(10)可以发现，\(s\)与\(z\)的正负性完全一致，因此可以通过解的正负快速判断当前解是否一定是非解。

因此，使用直接法求解步骤为：对(17)式直接求解一元四次方程的解\(s\)；使用(16)计算出纬度与海拔；使用(3)计算出经度。

由于使用的直接法求解，求解耗时主要在解一元四次方程的根。但本质上，这种方法求解最稳定、速度也很快，最主要是可控。 

2.3 ECEF与ENU互转
ENU坐标，即将地心坐标经过平移旋转到地表指定点，让新坐标系的z轴指向天空，新坐标的y指向北极，新坐标x轴指向东方。ENU本质上需要一个参考锚点(这里称为Anchor点)，然后以这个锚点为原点创建的局部坐标系。假设Anchor的LLA坐标为\((\alpha_0,\beta_0,h_0)\)，Anchor的ECEF坐标为\((x_0,y_0,z_0)\)，一个点p的ECEF坐标为\((x,y,z)\)，p基于Anchor的ENU坐标为\((x_{enu},y_{enu},z_{enu})\)，则ECEF与ENU互转过程如下  
(18)  
$$
\begin{aligned}
R_\alpha &=\begin{bmatrix}
\cos(\dfrac{\pi}{2}+\alpha_0) &\sin(\dfrac{\pi}{2}+\alpha_0) &0
\\
-\sin(\dfrac{\pi}{2}+\alpha_0) & \cos(\dfrac{\pi}{2}+\alpha_0) & 0
\\
0 &0&1
\end{bmatrix}
\\
\\ R_\beta &=\begin{bmatrix}
1 &0 &0
\\
0 &\cos(\dfrac{\pi}{2}-\beta_0) &\sin(\dfrac{\pi}{2}-\beta_0)
\\
0 &-\sin(\dfrac{\pi}{2}-\beta_0) & \cos(\dfrac{\pi}{2}-\beta_0)
\end{bmatrix}\\ \\
S &=R_\beta R_\alpha=\begin{bmatrix}
-\sin\alpha_0 & \cos \alpha_0 & 0
\\
-\sin \beta_0 \cos \alpha_0 & -\sin\beta_0\sin\alpha_0 &\cos\beta_0
\\
\cos\beta_0\cos\alpha_0 & \cos\beta_0\sin\alpha_0 & \sin\beta_0
\end{bmatrix}
\\
\\
\begin{bmatrix}
x_{enu}
\\
y_{enu}
\\
z_{enu}
\end{bmatrix} &=S\begin{bmatrix}
x-x_0\\
y-y_0\\
z-z_0
\end{bmatrix}
\\
\\
\begin{bmatrix}
x
\\
y
\\
z
\end{bmatrix} &=S^T\begin{bmatrix}
x_{enu}\\
y_{enu}\\
z_{enu}
\end{bmatrix}+\begin{bmatrix}
x_0
\\
y_0
\\
z_0
\end{bmatrix}
\end{aligned}
$$

很明显上面的S矩阵就是一个旋转变换矩阵，

2.4 LLA与ENU互转
LLA与ENU互转，本质上可通过中间的ECEF坐标系进行转换。这里不再赘述。

 

3 不同锚点选择对数据间相对关系的影响

在数据处理过程中，难免会遇到对数据相对关系进行处理，比如两个点的空间距离，两个点在平面上的距离，两条线在空间的夹角，两个点在平面夹角…… 。

由于ECEF转ENU包含3D旋转操作，因此在不同锚点情况下构建的局部ENU坐标系，单独使用某一个轴（仅考虑转换后的x、y、z数据），或者某两个轴的数据(比如仅考虑所谓平面操作)，这时的处理会存在差异。比如以锚点A建立坐标系，这时处理这个坐标系下的平面数据(x-y)，其本质上来说锚点A坐标系下x与y的数据含有以锚点B坐标系下的(x-y-z)信息，因此如果以锚点B建立坐标系，这时如果也只处理平面数据(x-y), 在相同处理流程下，两套锚点得到的结果有所差异。

而对于3D空间数据，因为ECEF转ENU本质上是一个刚体变换(不带缩放的3D旋转+平移)，由于刚体变换不影响原始数据的相对关系，因此不同锚点的选取，并不影响结果。

因此最终有如下结论：

(1) 如果数据处理流程，包含单独针对某个轴的数据(x或y或z)的处理,  不同锚点的选取，对结果有所影响。

(2) 如果数据处理流程，包含单独针对平面(x-y或x-z或y-z)的处理,  不同锚点的选取，对结果有所影响。

(3) 如果数据处理流程，所有处理均在3D空间进行，不同锚点的选取，对结果没有影响。

 

4 坐标转换测试

目前MathSword软件提供了如下函数供坐标系数据转换: CoorECEFtoENU、CoorECEFtoLLA、CoorENUtoECEF、CoorENUtoLLA、CoorLLAtoECEF、CoorLLAtoENU