1. 概述

我在《大地经纬度坐标与地心地固坐标的的转换》这篇文章中已经论述了地心坐标系的概念。我们知道，基于地心坐标系的坐标都是很大的值，这样的值是不太方便进行空间计算的，所以很多时候可以选取一个站心点，将这个很大的值变换成一个较小的值。以图形学的观点来看，地心坐标可以看作是世界坐标，站心坐标可以看作局部坐标。

站心坐标系以一个站心点为坐标原点，当把坐标系定义为X轴指东、Y轴指北，Z轴指天，就是ENU（东北天）站心坐标系。这样，从地心地固坐标系转换成的站心坐标系，就会成为一个符合常人对地理位置认知的局部坐标系。同时，只要站心点位置选的合理（通常可选取地理表达区域的中心点），表达的地理坐标都会是很小的值，非常便于空间计算。

注意站心天向(法向量)与赤道面相交不一定会经过球心

2. 原理

令选取的站心点为P，其大地经纬度坐标为（Bp，Lp，Hp）（B_p，L_p，H_p）（Bp，Lp，Hp），对应的地心地固坐标系为（Xp，Yp，Zp）（X_p，Y_p，Z_p）（Xp，Yp，Zp）。地心地固坐标系简称为ECEF，站心坐标系简称为ENU。

2.1. 平移

通过第一节的图可以看出，ENU要转换到ECEF，一个很明显的图形操作是平移变换，将站心移动到地心。根据站心点P在地心坐标系下的坐标（Xp，Yp，Zp）（X_p，Y_p，Z_p）（Xp，Yp，Zp），可以很容易推出ENU转到ECEF的平移矩阵：

T=[100Xp010Yp001Zp0001]T = \begin{bmatrix} 1&0&0&X_p\\ 0&1&0&Y_p\\ 0&0&1&Z_p\\ 0&0&0&1\\ \end{bmatrix} T=⎣⎢⎢⎡100001000010XpYpZp1⎦⎥⎥⎤

反推之，ECEF转换到ENU的平移矩阵就是T的逆矩阵：

T−1=[100−Xp010−Yp001−Zp0001]T^{-1} = \begin{bmatrix} 1&0&0&-X_p\\ 0&1&0&-Y_p\\ 0&0&1&-Z_p\\ 0&0&0&1\\ \end{bmatrix} T−1=⎣⎢⎢⎡100001000010−Xp−Yp−Zp1⎦⎥⎥⎤

2.2. 旋转

另外一个需要进行的图形变换是旋转变换，其旋转变换矩阵根据P点所在的经度L和纬度B确定。这个旋转变换有点难以理解，需要一定的空间想象能力，但是可以直接给出如下结论:

当从ENU转换到ECEF时，需要先旋转再平移，旋转是先绕X轴旋转(pi2−B)(\frac{pi}{2}-B)(2pi−B)，再绕Z轴旋转(pi2+L)(\frac{pi}{2}+L)(2pi+L)
当从ECEF转换到ENU时，需要先平移再旋转，旋转是先绕Z轴旋转−(pi2+L)-(\frac{pi}{2}+L)−(2pi+L)，再绕X轴旋转−(pi2−B)-(\frac{pi}{2}-B)−(2pi−B)

根据我在《WebGL简易教程(五)：图形变换(模型、视图、投影变换)》提到的旋转变换，绕X轴旋转矩阵为：

Rx=[10000cosθ−sinθ00sinθcosθ00001]R_x = \begin{bmatrix} 1&0&0&0\\ 0&cosθ&-sinθ&0\\ 0&sinθ&cosθ&0\\ 0&0&0&1\\ \end{bmatrix} Rx=⎣⎢⎢⎡10000cosθsinθ00−sinθcosθ00001⎦⎥⎥⎤

绕Z轴旋转矩阵为：

Rz=[cosθ−sinθ00sinθcosθ0000100001]R_z = \begin{bmatrix} cosθ&-sinθ&0&0\\ sinθ&cosθ&0&0\\ 0&0&1&0\\ 0&0&0&1\\ \end{bmatrix} Rz=⎣⎢⎢⎡cosθsinθ00−sinθcosθ0000100001⎦⎥⎥⎤

从ENU转换到ECEF的旋转矩阵为：
R=Rz(pi2+L)⋅Rx(pi2−B)(1)R = {R_z(\frac{pi}{2}+L)}\cdot{R_x(\frac{pi}{2}-B)} \tag{1} R=Rz(2pi+L)⋅Rx(2pi−B)(1)

根据三角函数公式：
sin(π/2+α)=cosαsin(π/2−α)=cosαcos(π/2+α)=−sinαcos(π/2−α)=sinαsin(π/2+α)=cosα\\ sin(π/2-α)=cosα\\ cos(π/2+α)=-sinα\\ cos(π/2-α)=sinα\\ sin(π/2+α)=cosαsin(π/2−α)=cosαcos(π/2+α)=−sinαcos(π/2−α)=sinα

有：
Rz(pi2+L)=[−sinL−cosL00cosL−sinL0000100001](2)R_z(\frac{pi}{2}+L) = \begin{bmatrix} -sinL&-cosL&0&0\\ cosL&-sinL&0&0\\ 0&0&1&0\\ 0&0&0&1\\ \end{bmatrix} \tag{2} Rz(2pi+L)=⎣⎢⎢⎡−sinLcosL00−cosL−sinL0000100001⎦⎥⎥⎤(2)

Rx(pi2−B)=[10000sinB−cosB00cosBsinB00001](3)R_x(\frac{pi}{2}-B) = \begin{bmatrix} 1&0&0&0\\ 0&sinB&-cosB&0\\ 0&cosB&sinB&0\\ 0&0&0&1\\ \end{bmatrix} \tag{3} Rx(2pi−B)=⎣⎢⎢⎡10000sinBcosB00−cosBsinB00001⎦⎥⎥⎤(3)

将(2)、(3)带入(1)中，则有：
R=[−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001](4)R = \begin{bmatrix} -sinL&-sinBcosL&cosBcosL&0\\ cosL&-sinBsinL&cosBsinL&0\\ 0&cosB&sinB&0\\ 0&0&0&1\\ \end{bmatrix} \tag{4} R=⎣⎢⎢⎡−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001⎦⎥⎥⎤(4)

而从ECEF转换到ENU的旋转矩阵为：
R−1=Rx(−(pi2−B))⋅Rz(−(pi2+L))(5)R^{-1} = {R_x(-(\frac{pi}{2}-B))} \cdot {R_z(-(\frac{pi}{2}+L))} \tag{5} R−1=Rx(−(2pi−B))⋅Rz(−(2pi+L))(5)

旋转矩阵是正交矩阵，根据正交矩阵的性质:正交矩阵的逆矩阵等于其转置矩阵，那么可直接得：
R−1=[−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001](6)R^{-1} = \begin{bmatrix} -sinL&cosL&0&0\\ -sinBcosL&-sinBsinL&cosB&0\\ cosBcosL&cosBsinL&sinB&0\\ 0&0&0&1\\ \end{bmatrix} \tag{6} R−1=⎣⎢⎢⎡−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001⎦⎥⎥⎤(6)

2.3. 总结

将上述公式展开，可得从ENU转换到ECEF的图形变换矩阵为：
M=T⋅R=[100Xp010Yp001Zp0001][−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001]M = T \cdot R = \begin{bmatrix} 1&0&0&X_p\\ 0&1&0&Y_p\\ 0&0&1&Z_p\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} -sinL&-sinBcosL&cosBcosL&0\\ cosL&-sinBsinL&cosBsinL&0\\ 0&cosB&sinB&0\\ 0&0&0&1\\ \end{bmatrix} M=T⋅R=⎣⎢⎢⎡100001000010XpYpZp1⎦⎥⎥⎤⎣⎢⎢⎡−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001⎦⎥⎥⎤

而从ECEF转换到ENU的图形变换矩阵为：
M−1=R−1∗T−1=[−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001][100−Xp010−Yp001−Zp0001]M^{-1} = R^{-1} * T^{-1} = \begin{bmatrix} -sinL&cosL&0&0\\ -sinBcosL&-sinBsinL&cosB&0\\ cosBcosL&cosBsinL&sinB&0\\ 0&0&0&1\\ \end{bmatrix} \begin{bmatrix} 1&0&0&-X_p\\ 0&1&0&-Y_p\\ 0&0&1&-Z_p\\ 0&0&0&1\\ \end{bmatrix} M−1=R−1∗T−1=⎣⎢⎢⎡−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001⎦⎥⎥⎤⎣⎢⎢⎡100001000010−Xp−Yp−Zp1⎦⎥⎥⎤

3. 实现

接下来用代码实现这个坐标转换，选取一个站心点，以这个站心点为原点，获取某个点在这个站心坐标系下的坐标：

#include <iostream>
#include <eigen3/Eigen/Eigen>#include <osgEarth/GeoData>using namespace std;const double epsilon = 0.000000000000001;
const double pi = 3.14159265358979323846;
const double d2r = pi / 180;
const double r2d = 180 / pi;const double a = 6378137.0;       //椭球长半轴
const double f_inverse = 298.257223563;            //扁率倒数
const double b = a - a / f_inverse;
//const double b = 6356752.314245;         //椭球短半轴const double e = sqrt(a * a - b * b) / a;void Blh2Xyz(double &x, double &y, double &z)
{double L = x * d2r;double B = y * d2r;double H = z;double N = a / sqrt(1 - e * e * sin(B) * sin(B));x = (N + H) * cos(B) * cos(L);y = (N + H) * cos(B) * sin(L);z = (N * (1 - e * e) + H) * sin(B);
}void Xyz2Blh(double &x, double &y, double &z)
{double tmpX =  x;double temY = y ;double temZ = z;double curB = 0;double N = 0; double calB = atan2(temZ, sqrt(tmpX * tmpX + temY * temY)); int counter = 0;while (abs(curB - calB) * r2d > epsilon  && counter < 25){curB = calB;N = a / sqrt(1 - e * e * sin(curB) * sin(curB));calB = atan2(temZ + N * e * e * sin(curB), sqrt(tmpX * tmpX + temY * temY));counter++;  }      x = atan2(temY, tmpX) * r2d;y = curB * r2d;z = temZ / sin(curB) - N * (1 - e * e);
}void TestBLH2XYZ()
{//double x = 113.6;
//double y = 38.8;
//double z = 100;
//
//printf("原大地经纬度坐标：%.10lf\t%.10lf\t%.10lf\n", x, y, z);
//Blh2Xyz(x, y, z);//printf("地心地固直角坐标：%.10lf\t%.10lf\t%.10lf\n", x, y, z);
//Xyz2Blh(x, y, z);
//printf("转回大地经纬度坐标：%.10lf\t%.10lf\t%.10lf\n", x, y, z);double x = -2318400.6045575836;double y = 4562004.801366804;double z = 3794303.054150639;//116.9395751953      36.7399177551printf("地心地固直角坐标：%.10lf\t%.10lf\t%.10lf\n", x, y, z);Xyz2Blh(x, y, z);printf("转回大地经纬度坐标：%.10lf\t%.10lf\t%.10lf\n", x, y, z);
}void CalEcef2Enu(Eigen::Vector3d& topocentricOrigin, Eigen::Matrix4d& resultMat)
{double rzAngle = -(topocentricOrigin.x() * d2r + pi / 2);Eigen::AngleAxisd rzAngleAxis(rzAngle, Eigen::Vector3d(0, 0, 1));Eigen::Matrix3d rZ = rzAngleAxis.matrix();double rxAngle = -(pi / 2 - topocentricOrigin.y() * d2r);Eigen::AngleAxisd rxAngleAxis(rxAngle, Eigen::Vector3d(1, 0, 0));Eigen::Matrix3d rX = rxAngleAxis.matrix();Eigen::Matrix4d rotation;rotation.setIdentity();rotation.block<3, 3>(0, 0) = (rX * rZ);//cout << rotation << endl;double tx = topocentricOrigin.x();double ty = topocentricOrigin.y();double tz = topocentricOrigin.z();Blh2Xyz(tx, ty, tz);Eigen::Matrix4d translation;translation.setIdentity();translation(0, 3) = -tx;translation(1, 3) = -ty;translation(2, 3) = -tz;resultMat = rotation * translation;
}void CalEnu2Ecef(Eigen::Vector3d& topocentricOrigin, Eigen::Matrix4d& resultMat)
{double rzAngle = (topocentricOrigin.x() * d2r + pi / 2);Eigen::AngleAxisd rzAngleAxis(rzAngle, Eigen::Vector3d(0, 0, 1));Eigen::Matrix3d rZ = rzAngleAxis.matrix();double rxAngle = (pi / 2 - topocentricOrigin.y() * d2r);Eigen::AngleAxisd rxAngleAxis(rxAngle, Eigen::Vector3d(1, 0, 0));Eigen::Matrix3d rX = rxAngleAxis.matrix();Eigen::Matrix4d rotation;rotation.setIdentity();rotation.block<3, 3>(0, 0) = (rZ * rX);//cout << rotation << endl;double tx = topocentricOrigin.x();double ty = topocentricOrigin.y();double tz = topocentricOrigin.z();Blh2Xyz(tx, ty, tz);Eigen::Matrix4d translation;translation.setIdentity();translation(0, 3) = tx;translation(1, 3) = ty;translation(2, 3) = tz;resultMat = translation * rotation;
}void TestXYZ2ENU()
{double L = 116.9395751953;double B = 36.7399177551;double H = 0;cout << fixed << endl;Eigen::Vector3d topocentricOrigin(L, B, H);Eigen::Matrix4d wolrd2localMatrix;CalEcef2Enu(topocentricOrigin, wolrd2localMatrix);   cout << "地心转站心矩阵：" << endl;cout << wolrd2localMatrix << endl<<endl;cout << "站心转地心矩阵：" << endl;Eigen::Matrix4d local2WolrdMatrix;CalEnu2Ecef(topocentricOrigin, local2WolrdMatrix);cout << local2WolrdMatrix << endl;double x = 117;double y = 37;double z = 10.3;Blh2Xyz(x, y, z);cout << "ECEF坐标（世界坐标）：";Eigen::Vector4d xyz(x, y, z, 1);cout << xyz << endl;cout << "ENU坐标（局部坐标）：";Eigen::Vector4d enu = wolrd2localMatrix * xyz;cout << enu << endl;
}void TestOE()
{double L = 116.9395751953;double B = 36.7399177551;double H = 0;osgEarth::SpatialReference *spatialReference = osgEarth::SpatialReference::create("epsg:4326");osgEarth::GeoPoint centerPoint(spatialReference, L, B, H);osg::Matrixd worldToLocal;centerPoint.createWorldToLocal(worldToLocal);cout << fixed << endl;cout << "地心转站心矩阵：" << endl;for (int i = 0; i < 4; i++){for (int j = 0; j < 4; j++){printf("%lf\t", worldToLocal.ptr()[j * 4 + i]);}cout << endl;}cout << endl;osg::Matrixd localToWorld;centerPoint.createLocalToWorld(localToWorld);cout << "站心转地心矩阵：" << endl;for (int i = 0; i < 4; i++){for (int j = 0; j < 4; j++){printf("%lf\t", localToWorld.ptr()[j * 4 + i]);}cout << endl;}cout << endl;double x = 117;double y = 37;double z = 10.3;osgEarth::GeoPoint geoPoint(spatialReference, x, y, z);cout << "ECEF坐标（世界坐标）：";osg::Vec3d out_world;geoPoint.toWorld(out_world);cout << out_world.x() <<'\t'<< out_world.y() << '\t' << out_world.z() << endl;cout << "ENU坐标（局部坐标）：";osg::Vec3d localCoord = worldToLocal.preMult(out_world);cout << localCoord.x() << '\t' << localCoord.y() << '\t' << localCoord.z() << endl;
}int main()
{//TestBLH2XYZ();cout << "使用Eigen进行转换实现：" << endl;TestXYZ2ENU();cout <<"---------------------------------------"<< endl;cout << "通过OsgEarth进行验证：" << endl;TestOE();
}

这个示例先用Eigen矩阵库，计算了坐标转换需要的矩阵和转换结果；然后通过osgEarth进行了验证，两者的结果基本一致。运行结果如下：

4. 参考

站心坐标系和WGS-84地心地固坐标系相互转换矩阵
Transformations between ECEF and ENU coordinates
GPS经纬度坐标WGS84到东北天坐标系ENU的转换
三维旋转矩阵；东北天坐标系(ENU)；地心地固坐标系(ECEF)；大地坐标系(Geodetic)；经纬度对应圆弧距离

地心地固坐标系(ECEF)与站心坐标系(ENU)的转换相关推荐

三维旋转矩阵；东北天坐标系(ENU)；地心地固坐标系(ECEF)；大地坐标系(Geodetic)；经纬度对应圆弧距离
关注即可了解更多相关知识. 欢迎转发.收藏.友善交流. 文章目录旋转矩阵三角恒等式 Trigonometric identities 二维旋转矩阵三维旋转矩阵 Euler Rotations m ...
经纬度坐标系转东北天_大地坐标系(WGS-84)、地心地固坐标系(ECEF)与东北天坐标系(ENU)的相互转换C语言代码分享...
//ECEF ---> WGS84 //pcg为WGS-84坐标系结构体指针,pcc为ECEF坐标系结构体指针 void ECEFToWGS(PWGS pcg, PECEF pcc) { dou ...
GPS经纬度坐标WGS84到东北天坐标系ENU的转换
GPS经纬度坐标WGS84到东北天坐标系ENU的转换常用坐标系介绍地理坐标系 (Geographic Coordinate System, GCS) 地心地固坐标系 (ECEF) 当地东.北.上 ...
导航坐标系：地心惯性坐标系、地心地固坐标系、当地水平坐标系、载体/机体坐标系
导航中的几种常用坐标系地心惯性坐标系(ECI) 地心地固坐标系(ECEF) 当地水平坐标系(LLF).东北天坐标系ENU 地平坐标系载体/机体坐标系机动目标跟踪/室内定位/导航/优化技术探讨:W ...
ecef与enu的转换
1. 概述基于地心坐标系的坐标都是很大的值,这样的值是不太方便进行空间计算的,所以很多时候可以选取一个站心点,将这个很大的值变换成一个较小的值.以图形学的观点来看,地心坐标可以看作是世界坐标,站心坐 ...
Matlab gui大地坐标系-地心地固坐标系-站心坐标系坐标变换
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言一.坐标变换原理 1.大地坐标系到地心地固坐标系 2.地心地固坐标系到大地坐标系 3.地心地固坐标系到站心 ...
java 实现EME2000(国家大地坐标系)转ECEF坐标系(地心地固坐标系)
EME2000 = J2000 俗称国家大地坐标系 ECEF坐标系称为地心地固坐标系先上代码.传入经度.维度.以及高度.经过的年月日时分秒 private List<Double> E ...
站心坐标系和WGS-84地心地固坐标系相互转换矩阵
其中λ为经度,φ为纬度
1.1 坐标系转换关系
文章目录前言一.地球坐标系 1. 地心地固坐标系ECEF 2. 大地坐标系(LLA) 3. 坐标转换二.站心坐标系 1 东北天坐标系(ENU) 前言在雷达组网.多传感器融合等应用场景中,首先需 ...

地心地固坐标系(ECEF)与站心坐标系(ENU)的转换

文章目录