地心地固坐标系(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. 参考

分类: 大地测量学 , 地理信息系统原理

标签: 图形变换 , 站心坐标系 , 地心坐标系 , 矩阵变换

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

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

1. 概述

2. 原理

2.1. 平移

2.2. 旋转

2.3. 总结

3. 实现

4. 参考

热门文章

最新文章

相关电子书

热门

活动广场

任务中心

开发者评测

高校计划

乘风者计划

训练营

阿里云MVP

话题

直播

下载

镜像站

技术资料

插件

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

1. 概述

2. 原理

2.1. 平移

2.2. 旋转

2.3. 总结

3. 实现

4. 参考

热门文章

最新文章

相关电子书