地心地固坐标系(ECEF)与站心坐标系(ENU)的转换
目录
1. 概述
我在《大地经纬度坐标与地心地固坐标的的转换》这篇文章中已经论述了地心坐标系的概念。我们知道，基于地心坐标系的坐标都是很大的值，这样的值是不太方便进行空间计算的，所以很多时候可以选取一个站心点，将这个很大的值变换成一个较小的值。以图形学的观点来看，地心坐标可以看作是世界坐标，站心坐标可以看作局部坐标。
站心坐标系以一个站心点为坐标原点，当把坐标系定义为X轴指东、Y轴指北，Z轴指天，就是ENU（东北天）站心坐标系。这样，从地心地固坐标系转换成的站心坐标系，就会成为一个符合常人对地理位置认知的局部坐标系。同时，只要站心点位置选的合理（通常可选取地理表达区域的中心点），表达的地理坐标都会是很小的值，非常便于空间计算。
注意站心天向(法向量)与赤道面相交不一定会经过球心
2. 原理
令选取的站心点为P，其大地经纬度坐标为（Bp，Lp，Hp）（Bp，Lp，Hp），对应的地心地固坐标系为（Xp，Yp，Zp）（Xp，Yp，Zp）。地心地固坐标系简称为ECEF，站心坐标系简称为ENU。
2.1. 平移
通过第一节的图可以看出，ENU要转换到ECEF，一个很明显的图形操作是平移变换，将站心移动到地心。根据站心点P在地心坐标系下的坐标（Xp，Yp，Zp）（Xp，Yp，Zp），可以很容易推出ENU转到ECEF的平移矩阵：
T=⎡⎢ ⎢ ⎢ ⎢⎣100Xp010Yp001Zp0001⎤⎥ ⎥ ⎥ ⎥⎦T=[100Xp010Yp001Zp0001]
反推之，ECEF转换到ENU的平移矩阵就是T的逆矩阵：
T−1=⎡⎢ ⎢ ⎢ ⎢⎣100−Xp010−Yp001−Zp0001⎤⎥ ⎥ ⎥ ⎥⎦T−1=[100−Xp010−Yp001−Zp0001]
2.2. 旋转
另外一个需要进行的图形变换是旋转变换，其旋转变换矩阵根据P点所在的经度L和纬度B确定。这个旋转变换有点难以理解，需要一定的空间想象能力，但是可以直接给出如下结论:
- 当从ENU转换到ECEF时，需要先旋转再平移，旋转是先绕X轴旋转(pi2−B)(pi2−B)，再绕Z轴旋转(pi2+L)(pi2+L)
- 当从ECEF转换到ENU时，需要先平移再旋转，旋转是先绕Z轴旋转−(pi2+L)−(pi2+L)，再绕X轴旋转−(pi2−B)−(pi2−B)
根据我在《WebGL简易教程(五)：图形变换(模型、视图、投影变换)》提到的旋转变换，绕X轴旋转矩阵为：
Rx=⎡⎢ ⎢ ⎢⎣10000cosθ−sinθ00sinθcosθ00001⎤⎥ ⎥ ⎥⎦Rx=[10000cosθ−sinθ00sinθcosθ00001]
绕Z轴旋转矩阵为：
Rz=⎡⎢ ⎢ ⎢⎣cosθ−sinθ00sinθcosθ0000100001⎤⎥ ⎥ ⎥⎦Rz=[cosθ−sinθ00sinθcosθ0000100001]
从ENU转换到ECEF的旋转矩阵为：
R=Rz(pi2+L)⋅Rx(pi2−B)(1)(1)R=Rz(pi2+L)⋅Rx(pi2−B)
根据三角函数公式：
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)(2)Rz(pi2+L)=[−sinL−cosL00cosL−sinL0000100001]
Rx(pi2−B)=⎡⎢ ⎢ ⎢⎣10000sinB−cosB00cosBsinB00001⎤⎥ ⎥ ⎥⎦(3)(3)Rx(pi2−B)=[10000sinB−cosB00cosBsinB00001]
将(2)、(3)带入(1)中，则有：
R=⎡⎢ ⎢ ⎢⎣−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001⎤⎥ ⎥ ⎥⎦(4)(4)R=[−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001]
而从ECEF转换到ENU的旋转矩阵为：
R−1=Rx(−(pi2−B))⋅Rz(−(pi2+L))(5)(5)R−1=Rx(−(pi2−B))⋅Rz(−(pi2+L))
旋转矩阵是正交矩阵，根据正交矩阵的性质:正交矩阵的逆矩阵等于其转置矩阵，那么可直接得：
R−1=⎡⎢ ⎢ ⎢⎣−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001⎤⎥ ⎥ ⎥⎦(6)(6)R−1=[−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001]
2.3. 总结
将上述公式展开，可得从ENU转换到ECEF的图形变换矩阵为：
M=T⋅R=⎡⎢ ⎢ ⎢ ⎢⎣100Xp010Yp001Zp0001⎤⎥ ⎥ ⎥ ⎥⎦⎡⎢ ⎢ ⎢⎣−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001⎤⎥ ⎥ ⎥⎦M=T⋅R=[100Xp010Yp001Zp0001][−sinL−sinBcosLcosBcosL0cosL−sinBsinLcosBsinL00cosBsinB00001]
而从ECEF转换到ENU的图形变换矩阵为：
M−1=R−1∗T−1=⎡⎢ ⎢ ⎢⎣−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001⎤⎥ ⎥ ⎥⎦⎡⎢ ⎢ ⎢ ⎢⎣100−Xp010−Yp001−Zp0001⎤⎥ ⎥ ⎥ ⎥⎦M−1=R−1∗T−1=[−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001][100−Xp010−Yp001−Zp0001]
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.7399177551 printf("地心地固直角坐标：%.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)；经纬度对应圆弧距离
