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

目录

• 1. 概述
• 2. 原理
  • 2.1. 平移
  • 2.2. 旋转
  • 2.3. 总结
• 3. 实现
• 4. 参考

1. 概述

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

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

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

2. 原理

令选取的站心点为P，其大地经纬度坐标为\(（B_p，L_p，H_p）\)，对应的地心地固坐标系为\(（X_p，Y_p，Z_p）\)。地心地固坐标系简称为ECEF，站心坐标系简称为ENU。

2.1. 平移

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

\[T =
\begin{bmatrix}
1&0&0&X_p\\
0&1&0&Y_p\\
0&0&1&Z_p\\
0&0&0&1\\
\end{bmatrix}
\]

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

\[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}
\]

2.2. 旋转

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

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

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

\[R_x =
\begin{bmatrix}
1&0&0&0\\
0&cosθ&-sinθ&0\\
0&sinθ&cosθ&0\\
0&0&0&1\\
\end{bmatrix}
\]

绕Z轴旋转矩阵为：

\[R_z =
\begin{bmatrix}
cosθ&-sinθ&0&0\\
sinθ&cosθ&0&0\\
0&0&1&0\\
0&0&0&1\\
\end{bmatrix}
\]

从ENU转换到ECEF的旋转矩阵为：

\[R = {R_z(\frac{pi}{2}+L)}\cdot{R_x(\frac{pi}{2}-B)}
\tag{1}
\]

根据三角函数公式：

\[sin(π/2+α)=cosα\\
sin(π/2-α)=cosα\\
cos(π/2+α)=-sinα\\
cos(π/2-α)=sinα\\
\]

有：

\[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}
\]

\[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}
\]

将(2)、(3)带入(1)中，则有：

\[R =
\begin{bmatrix}
-sinL&-sinBcosL&cosBcosL&0\\
cosL&-sinBsinL&cosBsinL&0\\
0&cosB&sinB&0\\
0&0&0&1\\
\end{bmatrix}
\tag{4}
\]

而从ECEF转换到ENU的旋转矩阵为：

\[R^{-1} = {R_x(-(\frac{pi}{2}-B))} \cdot {R_z(-(\frac{pi}{2}+L))}
\tag{5}
\]

旋转矩阵是正交矩阵，根据正交矩阵的性质:正交矩阵的逆矩阵等于其转置矩阵，那么可直接得：

\[R^{-1} =
\begin{bmatrix}
-sinL&cosL&0&0\\
-sinBcosL&-sinBsinL&cosB&0\\
cosBcosL&cosBsinL&sinB&0\\
0&0&0&1\\
\end{bmatrix}
\tag{6}
\]

2.3. 总结

将上述公式展开，可得从ENU转换到ECEF的图形变换矩阵为：

\[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}
\]

而从ECEF转换到ENU的图形变换矩阵为：

\[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}
\]

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. 参考

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