GPS经纬度坐标WGS84到东北天坐标系ENU的转换

常用坐标系介绍

地理坐标系 (Geographic Coordinate System, GCS)

可以说是最为广泛应用的一个地球坐标系，它给出一点的大地纬度、大地经度和大地高程而更加直观地告诉我们该点在地球中的位置，故又被称作纬经高坐标系。WGS-84坐标系的X轴指向BIH(国际时间服务机构)1984.0定义的零子午面(Greenwich)和协议地球极(CTP)赤道的交点。Z轴指向CTP方向。Y轴与X、Z轴构成右手坐标系。

一句话解释就是：把前面提到的ECEF坐标系用在GPS中，就是WGS-84坐标系。

其中：
（1）：大地纬度是过用户点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。
（2）：大地经度是过用户点P的子午面与本初子午线之间的夹角。经度值在-180°到+180°之间。
（3）：大地高度h是过用户点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。

参考资料:地理信息系统中常用坐标系

地心地固坐标系 (ECEF)

在GPS中使用的被称为地球中心，地球固定(ECEF)。ECEF使用三维XYZ坐标(以米为单位)来描述GPS用户或卫星。“地球”这个词来自于坐标轴的原点(0,0,0)位于质心处(通过多年的跟踪卫星确定轨迹)。“地球固定”这个词意味着坐标轴相对于地球是固定的(也就是说，它们随地球旋转)。其 x 轴延伸通过本初子午线（经度 0 度）和赤道（0度纬度）。z 轴延伸穿过真正的北极（即与地球自转轴重合）。y 轴完成右手坐标系，穿过赤道和 90 度经度，如图 1所示

图1 ECEF直角坐标系

当地东、北、上 (ENU) 坐标

站心坐标系也叫东北天坐标系 ENU，用于表示以观察者为中心的其他物体
的运动规律。以站心为坐标系原点，z 轴与椭球法线重合，向上为正，y 与椭球
短半轴重合（北向），x 轴与椭球长半轴重合（东向）

基坐标相互转化

地理坐标系到地心地固坐标系 (GCS to ECEF)

这里因为地球近似为一个椭球,所以沿着椭球表面的法线方向不在 O e O_{e} Oe​与 M M M的连线方向,首先我们假设它的椭球表面的法线方向的反向延长线交 Z Z Z轴与于点 M ′ M{'} M′,设 ∣ P Q ∣ = N , ∣ P M ∣ = h |PQ|= N,|PM| = h ∣PQ∣=N,∣PM∣=h, 在 X E C E F − E C E F − E C E F 在X_{ECEF}-_{ECEF}-_{ECEF} 在XECEF​−ECEF​−ECEF​基坐标系下,根据几何关系,M的坐标为
( ( N + h ) cos ⁡ ( φ ) cos ⁡ ( λ ) , ( N + h ) cos ⁡ ( φ ) sin ⁡ ( λ ) , ( N + h ) sin ⁡ ( φ ) ) − O Q ) \left((N+h)\cos \left(\varphi\right) \cos (\lambda), (N+h) \cos \left(\varphi\right) \sin (\lambda), (N+h) \sin \left(\varphi\right)\right)-OQ) ((N+h)cos(φ)cos(λ),(N+h)cos(φ)sin(λ),(N+h)sin(φ))−OQ)

然后我们将 M M M点所在的子午圈椭圆投影到一个二维的平面上,如下图所示,(gps的高度h不是与地心在一条直线上)

首先设p点所在的子午圈的方程设为
x 2 R e 2 + z 2 R p 2 = 1 ① \tag*{①}\frac{x^{2}}{R_{e}^{2}}+\frac{z^{2}}{R_{p}^{2}}=1 Re2​x2​+Rp2​z2​=1① 椭圆的扁率定义为
f = R e − R p R e f=\frac{R_{e}-R_{p}}{R_{e}} f=Re​Re​−Rp​​ 椭圆的离心率定义为
e = R e 2 − R p 2 R e ② e=\tag*{②}\frac{\sqrt{R_{e}^{2}-R_{p}^{2}}}{R_{e}} e=Re​Re2​−Rp2​ ​​②由②式可得   R p = R e 1 − e 2   ③ \tag*{③}\ R_{p}=Re \sqrt{1-e^{2}}\,  Rp​=Re1−e2 ​③
将椭圆方程①两边同时对 x   x\, x求导,并结合③式,可得 2 x R e 2 + 2 z   d z / d x R e 2 ( 1 − e 2 ) = 0 \frac{2 x}{R_{e}^{2}}+\frac{2 z \mathrm{~d} z / \mathrm{d} x}{R_{e}^{2}\left(1-e^{2}\right)}=0 Re2​2x​+Re2​(1−e2)2z dz/dx​=0移项整理得 d z   d x = − ( 1 − e 2 ) x z \frac{\mathrm{d} z}{\mathrm{~d} x}=-\left(1-e^{2}\right) \frac{x}{z}  dxdz​=−(1−e2)zx​式中 d z   d x \frac{\mathrm{d} z}{\mathrm{~d} x}  dxdz​表示椭圆在P点的切线的斜率,显然切线PE ⊥   \bot \, ⊥法线PQ,所以 K P E K P Q = − 1   K_{PE} K_{PQ}=-1\, KPE​KPQ​=−1,因为 P Q PQ PQ的斜率为 tan ⁡ φ   \tan \varphi \, tanφ,则
d z   d x tan ⁡ φ = − ( 1 − e 2 ) x z tan ⁡ φ = − 1   \frac{\mathrm{d} z}{\mathrm{~d} x} \tan \varphi =-\left(1-e^{2}\right) \frac{x}{z} \tan \varphi =-1\,  dxdz​tanφ=−(1−e2)zx​tanφ=−1
整理可得 z = x ( 1 − e 2 ) tan ⁡ φ ④ \tag*{④}z=x\left(1-e^{2}\right) \tan \varphi z=x(1−e2)tanφ④
结合③④带入椭圆方程①中,可得到以地理纬度 φ   \varphi \, φ的椭圆的参数方程 x = R e 1 − e 2 sin ⁡ 2 φ cos ⁡ φ z = R e ( 1 − e 2 ) 1 − e 2 sin ⁡ 2 φ sin ⁡ φ } ⑤ \tag*{⑤}\left.\begin{array}{l} x=\frac{R_{e}}{\sqrt{1-e^{2} \sin ^{2} \varphi}} \cos \varphi \\ z=\frac{R_{e}\left(1-e^{2}\right)}{\sqrt{1-e^{2} \sin ^{2} \varphi}} \sin \varphi \end{array}\right\} x=1−e2sin2φ ​Re​​cosφz=1−e2sin2φ ​Re​(1−e2)​sinφ​⎭⎬⎫​⑤
根据子午圈投影的图可得 x = N c o s φ ⑥ \tag*{⑥} x =N cos \varphi x=Ncosφ⑥ 将⑤中的 x   x\, x带入⑥式中 N = R e 1 − e 2 sin ⁡ 2 φ N =\frac{R_{e}}{\sqrt{1-e^{2} \sin ^{2} \varphi}} N=1−e2sin2φ ​Re​​
因此参数方程可以简写为 x = N cos ⁡ φ z = N ( 1 − e 2 ) sin ⁡ φ } ⑥ \tag*{⑥}\left.\begin{array}{l} x=N\cos \varphi \\ z=N\left(1-e^{2}\right) \sin \varphi \end{array}\right\} x=Ncosφz=N(1−e2)sinφ​}⑥
通过上面的椭圆参数方程⑥,结合投影到 2 D 平 面 上 的 Z 轴 与 E C E F 的 Z 轴 2D平面上的Z轴与ECEF的Z轴 2D平面上的Z轴与ECEF的Z轴是同一个可得 M M M点的坐标系 z M = N ( 1 − e 2 ) sin ⁡ φ + h s i n φ = ( z_{M}=N\left(1-e^{2}\right) \sin \varphi + hsin \varphi =( zM​=N(1−e2)sinφ+hsinφ=(,结合之前所说的{ECEF坐标系下$P$的坐标系为$$\left((N+h)\cos \left(\phi \right)\cos (\lambda ),(N+h)\cos \left(\phi \right)\sin (\lambda ),(N+h)\sin \left(\phi \right)\right)-OQ)$$
可以得出
x M = ( N + h ) c o s ( φ ) c o s ( λ ) x_{M} = (N+h)cos(φ)cos(λ) xM​=(N+h)cos(φ)cos(λ)
y M = ( N + h ) c o s ( φ ) s i n ( λ ) y_{M} = (N+h)cos(φ)sin(λ) yM​=(N+h)cos(φ)sin(λ)
z M = ( b 2 a 2 N + h ) sin ⁡ φ z_{M} =\left(\frac{b^{2}}{a^{2}} N+h\right) \sin \varphi zM​=(a2b2​N+h)sinφ

附上各参数的含义
φ = l a t i t u d e \varphi = latitude φ=latitude
λ = l o n g i t u d e \lambda = longitude λ=longitude
h = h e i g h t a b o v e e l l i p s o i d ( m e t e r s ) h = height above ellipsoid (meters) h=heightaboveellipsoid(meters)
N = R a d i u s o f C u r v a t u r e ( m e t e r s ) , d e f i n e d a s : a 1 − e 2 sin ⁡ 2 φ N = Radius of Curvature (meters), defined as:\frac{a}{\sqrt{1-e^{2} \sin ^{2} \varphi}} N=RadiusofCurvature(meters),definedas:1−e2sin2φ ​a​,这里的a就是我们上面推导公式的 R e R_{e} Re​,详细的参考 ⑥ ⑥ ⑥下面的推导.

WGS84 Parameters
a = 6378137 a = 6378137 a=6378137(椭圆的长半轴)这里表示赤道椭圆的长半轴
b = a ( 1 − f ) = 6356752.31424518 b = a (1 -f ) = 6356752.31424518 b=a(1−f)=6356752.31424518
f = 1 298.257223563   f =\frac{1}{298.257223563}\, f=298.2572235631​
e = a 2 − b 2 a 2 e=\frac{\sqrt{a^{2}-b^{2}}}{a^{2}} e=a2a2−b2 ​​详细含义参照②

我们把 M M M点放大,来说说gps的高度

高度 h   , h \,, h,gps , 的 的 的altitude,(从上面图中可知gps的测量高度是GPS天线与参考椭球面的垂直距离,WGS84椭球面在世界范围内近似大地水准面),地球的长半轴 a a a与短半轴 b b b是已知的,然后地理纬度 φ   \varphi \, φ也是已知的,所以M点在 E C E F ECEF ECEF坐标系下已知.

地心地固坐标系到东北天、站心坐标系 ECEF to ENU

这里采用几何意义的方式来讲这个转化关系,这里为了直观,我们在p点画ENU坐标系
因为 E a s t ( x L o c a l ) East(x Local) East(xLocal)的方向与M点所在的子午线(P点子午线)垂直,所以我们首先把绕着 Z E C E F Z_{ECEF} ZECEF​旋转至 X E C E F X_{ECEF} XECEF​与 E a s t ( x L o c a l ) East(x Local) East(xLocal)的方向一致① E C E F 坐 标 系 ECEF坐标系 ECEF坐标系绕着 Z E C E F Z_{ECEF} ZECEF​逆时针旋转 ( π 2 + λ ) (\frac{π}{2} + \lambda) (2π​+λ),然后绕着 X E C E F X_{ECEF} XECEF​旋转至 Z E C E F Z_{ECEF} ZECEF​与 U p ( Z L o c a l ) Up(Z Local) Up(ZLocal)方向一致②绕着 X E C E F X_{ECEF} XECEF​逆时针旋转 ( π 2 − φ ) (\frac{π}{2} - \varphi) (2π​−φ)③平移的部分就是之前LLA转到ECEF坐标系下的 M M M的坐标
R z e c e f [ ( π 2 + λ ) ] = ( cos ⁡ [ ( π 2 + λ ) ] sin ⁡ [ ( π 2 + λ ) ] 0 − sin ⁡ [ ( π 2 + λ ) ] cos ⁡ [ ( π 2 + λ ) ] 0 0 0 1 ) R_{z_{ecef}}[(\frac{π}{2} + \lambda)]=\left(\begin{array}{ccc}\cos [(\frac{π}{2} + \lambda)] & \sin [(\frac{π}{2} + \lambda)] & 0 \\ -\sin [(\frac{π}{2} + \lambda)] & \cos [(\frac{π}{2} + \lambda)] & 0 \\ 0 & 0 & 1 \end{array}\right) Rzecef​​[(2π​+λ)]=⎝⎛​cos[(2π​+λ)]−sin[(2π​+λ)]0​sin[(2π​+λ)]cos[(2π​+λ)]0​001​⎠⎞​

R x e c e f [ ( π 2 − φ ) ] = [ 1 0 0 0 cos ⁡ [ ( π 2 − φ ) ] sin ⁡ [ ( π 2 − φ ) ] 0 − sin ⁡ [ ( π 2 − φ ) ] cos ⁡ [ ( π 2 − φ ) ] ] R_{x_{ecef}}[(\frac{π}{2} - \varphi)]=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos [(\frac{π}{2} - \varphi)] & \sin [(\frac{π}{2} - \varphi)] \\ 0 & -\sin [(\frac{π}{2} - \varphi)] & \cos [(\frac{π}{2} - \varphi)] \end{array}\right] Rxecef​​[(2π​−φ)]=⎣⎡​100​0cos[(2π​−φ)]−sin[(2π​−φ)]​0sin[(2π​−φ)]cos[(2π​−φ)]​⎦⎤​

R x e c e f [ ( π 2 − φ ) ]   R z e c e f [ ( π 2 + λ ) ] = ( − sin ⁡ λ cos ⁡ λ 0 − cos ⁡ λ sin ⁡ φ − sin ⁡ λ sin ⁡ φ cos ⁡ φ cos ⁡ λ cos ⁡ φ sin ⁡ λ cos ⁡ φ sin ⁡ φ ) R_{x_{ecef}}[(\frac{π}{2} - \varphi)] \, R_{z_{ecef}}[(\frac{π}{2} + \lambda)] = \left(\begin{array}{ccc} -\sin \lambda & \cos \lambda & 0 \\ -\cos \lambda \sin \varphi & -\sin \lambda \sin \varphi & \cos \varphi \\ \cos \lambda \cos \varphi & \sin \lambda \cos \varphi & \sin \varphi \end{array}\right) Rxecef​​[(2π​−φ)]Rzecef​​[(2π​+λ)]=⎝⎛​−sinλ−cosλsinφcosλcosφ​cosλ−sinλsinφsinλcosφ​0cosφsinφ​⎠⎞​

在车辆运动中,我们一般车辆启动假设gps天线的初始位置在 { X r , Y r , Z r } \left\{X_{r}, Y_{r}, Z_{r}\right\} {Xr​,Yr​,Zr​},车辆运动的位置为 { X p , Y p , Z p } \left\{X_{p}, Y_{p}, Z_{p}\right\} {Xp​,Yp​,Zp​},则基于启动为原点的ENU的坐标为:
[ x y z ] = [ − sin ⁡ λ cos ⁡ λ 0 − cos ⁡ λ sin ⁡ φ − sin ⁡ λ sin ⁡ φ cos ⁡ φ cos ⁡ λ cos ⁡ φ sin ⁡ λ cos ⁡ φ sin ⁡ φ ] [ X p − X r Y p − Y r Z p − Z r ] \left[\begin{array}{l} x \\ y \\ z \end{array}\right]=\left[\begin{array}{ccc} -\sin \lambda & \cos \lambda & 0 \\ -\cos \lambda \sin \varphi & -\sin \lambda \sin \varphi & \cos \varphi \\ \cos \lambda \cos \varphi & \sin \lambda \cos \varphi & \sin \varphi \end{array}\right]\left[\begin{array}{c} X_{p}-X_{r} \\ Y_{p}-Y_{r} \\ Z_{p}-Z_{r} \end{array}\right] ⎣⎡​xyz​⎦⎤​=⎣⎡​−sinλ−cosλsinφcosλcosφ​cosλ−sinλsinφsinλcosφ​0cosφsinφ​⎦⎤​⎣⎡​Xp​−Xr​Yp​−Yr​Zp​−Zr​​⎦⎤​
这里讲的也很好

地理坐标系到东北天、站心坐标系(GCS to ENU)

GCS和ENU之间的坐标系转换通过先转换为ECEF实现。
代码有时间再放上来吧