地心地固座標系(ECEF)與站心座標系(ENU)的轉換

charlee44發表於2021-10-08

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確定。這個旋轉變換有點難以理解，需要一定的空間想象能力，但是可以直接給出如下結論:

當從ENU轉換到ECEF時，需要先旋轉再平移，旋轉是先繞X軸旋轉\((\frac{pi}{2}-B)\)，再繞Z軸旋轉\((\frac{pi}{2}+L)\)
當從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進行了驗證，兩者的結果基本一致。執行結果如下：
imglink2

4. 參考

ArcGIS地圖投影與座標系轉換的方法
2023-09-28
地圖
Qt - 座標系及轉換
2024-03-07
QT
GIS中的座標系定義與轉換 (轉)
2008-05-28
OpenGL座標系與幾何變換
2015-12-28
gcoord: 轉換WGS84、GCJ02、BD09座標，轉換百度高德地圖座標系
2018-02-24
GC地圖
OpenGL ES on iOS --- 座標系統與矩陣轉換
2017-12-14
iOS矩陣
C++ opencv的圓轉矩形，極座標轉笛卡爾座標系
2020-10-29
C++OpenCV
ogre世界座標魚螢幕座標相互轉換
2014-05-08
Python pytorch 座標系變換與維度轉換
2024-04-22
PythonPyTorch
座標系的生成
2024-09-22
Go版本的各座標系互相轉換的工具
2018-03-19
Go
利用齊次座標進行二維座標轉換
2021-12-17
形象化理解笛卡爾座標系和極座標系
2018-09-23
SVG 座標系統
2018-11-30
SVG
iOS中地圖經緯度座標轉換
2018-01-03
iOS地圖
世界座標系到攝像機座標系的矩陣變換推導過程
2024-11-10
矩陣
座標系定義和相互轉換演算法
2020-12-13
演算法
MathNet Ray3D座標系下轉換
2021-01-04
3D
canvas座標系原點
2018-09-10
Canvas
canvas 座標系原點
2018-09-10
Canvas
海康相機畫素座標(px,py)到sdk ptz 座標轉換最後到onvif ptz座標
2024-11-08
三維座標系的旋轉矩陣
2017-05-31
矩陣
ROS TF ：使用 TF 設定機器人釋出座標變換使用座標變換將感測器資料轉換為機器人座標系下
2020-11-24
ROS機器人
pose座標變換
2024-10-17
三維空間座標系變換-旋轉矩陣
2019-10-30
矩陣
地圖投影系列介紹（二）_ 地理座標系
2013-04-22
地圖
vue 實現高德座標轉GPS座標
2022-04-20
Vue
GPS座標轉換為BIM
2024-04-07
火星座標和正常座標之間的轉換
2014-04-08
WebGL座標系的小祕密
2019-09-06
Web
Threejs的三維座標系
2024-11-26
JS
iOS地球座標、火星座標和百度座標之間轉換(Swift3.0)
2017-01-13
iOSSwift
高德座標打點（點為正常的WGS84地球座標系，常見於 GPS 裝置，Google 地圖等國際標準的座標體系）偏移，調整偏移量
2024-06-03
Go地圖
OpenGL 座標系統詳解
2023-11-07
Excel轉百度座標系（AngularJS）
2019-02-08
ExcelAngularJS
ArcEngine下投影座標和經緯度座標的相互轉換
2011-04-14
機器人工具座標系標定原理
2020-11-05
機器人
百度座標轉換API使用
2016-05-05
API