地心地固坐标系(ECEF)与站心坐标系(ENU)的转换
目錄1. 概述2. 原理2.1. 平移2.2. 旋轉(zhuǎn)2.3. 總結(jié)3. 實現(xiàn)4. 參考
1. 概述
我在《大地經(jīng)緯度坐標(biāo)與地心地固坐標(biāo)的的轉(zhuǎn)換》這篇文章中已經(jīng)論述了地心坐標(biāo)系的概念。我們知道，基于地心坐標(biāo)系的坐標(biāo)都是很大的值，這樣的值是不太方便進行空間計算的，所以很多時候可以選取一個站心點，將這個很大的值變換成一個較小的值。以圖形學(xué)的觀點來看，地心坐標(biāo)可以看作是世界坐標(biāo)，站心坐標(biāo)可以看作局部坐標(biāo)。
站心坐標(biāo)系以一個站心點為坐標(biāo)原點，當(dāng)把坐標(biāo)系定義為X軸指東、Y軸指北，Z軸指天，就是ENU（東北天）站心坐標(biāo)系。這樣，從地心地固坐標(biāo)系轉(zhuǎn)換成的站心坐標(biāo)系，就會成為一個符合常人對地理位置認(rèn)知的局部坐標(biāo)系。同時，只要站心點位置選的合理（通常可選取地理表達(dá)區(qū)域的中心點），表達(dá)的地理坐標(biāo)都會是很小的值，非常便于空間計算。
注意站心天向(法向量)與赤道面相交不一定會經(jīng)過球心
2. 原理
令選取的站心點為P，其大地經(jīng)緯度坐標(biāo)為(（B_p，L_p，H_p）)，對應(yīng)的地心地固坐標(biāo)系為(（X_p，Y_p，Z_p）)。地心地固坐標(biāo)系簡稱為ECEF，站心坐標(biāo)系簡稱為ENU。
2.1. 平移
通過第一節(jié)的圖可以看出，ENU要轉(zhuǎn)換到ECEF，一個很明顯的圖形操作是平移變換，將站心移動到地心。根據(jù)站心點P在地心坐標(biāo)系下的坐標(biāo)(（X_p，Y_p，Z_p）)，可以很容易推出ENU轉(zhuǎn)到ECEF的平移矩陣：
[T =
egin{bmatrix}
1&0&0&X_p\
0&1&0&Y_p\
0&0&1&Z_p\
0&0&0&1\
end{bmatrix}
]
反推之，ECEF轉(zhuǎn)換到ENU的平移矩陣就是T的逆矩陣：
[T^{-1} =
egin{bmatrix}
1&0&0&-X_p\
0&1&0&-Y_p\
0&0&1&-Z_p\
0&0&0&1\
end{bmatrix}
]
2.2. 旋轉(zhuǎn)
另外一個需要進行的圖形變換是旋轉(zhuǎn)變換，其旋轉(zhuǎn)變換矩陣根據(jù)P點所在的經(jīng)度L和緯度B確定。這個旋轉(zhuǎn)變換有點難以理解，需要一定的空間想象能力，但是可以直接給出如下結(jié)論:
當(dāng)從ENU轉(zhuǎn)換到ECEF時，需要先旋轉(zhuǎn)再平移，旋轉(zhuǎn)是先繞X軸旋轉(zhuǎn)((frac{pi}{2}-B))，再繞Z軸旋轉(zhuǎn)((frac{pi}{2}+L))
當(dāng)從ECEF轉(zhuǎn)換到ENU時，需要先平移再旋轉(zhuǎn)，旋轉(zhuǎn)是先繞Z軸旋轉(zhuǎn)(-(frac{pi}{2}+L))，再繞X軸旋轉(zhuǎn)(-(frac{pi}{2}-B))
根據(jù)我在《WebGL簡易教程(五)：圖形變換(模型、視圖、投影變換)》提到的旋轉(zhuǎn)變換，繞X軸旋轉(zhuǎn)矩陣為：
[R_x =
egin{bmatrix}
1&0&0&0\
0&cosθ&-sinθ&0\
0&sinθ&cosθ&0\
0&0&0&1\
end{bmatrix}
]
繞Z軸旋轉(zhuǎn)矩陣為：
[R_z =
egin{bmatrix}
cosθ&-sinθ&0&0\
sinθ&cosθ&0&0\
0&0&1&0\
0&0&0&1\
end{bmatrix}
]
從ENU轉(zhuǎn)換到ECEF的旋轉(zhuǎn)矩陣為：
[R = {R_z(frac{pi}{2}+L)}cdot{R_x(frac{pi}{2}-B)}
ag{1}
]
根據(jù)三角函數(shù)公式：
[sin(π/2+α)=cosα\
sin(π/2-α)=cosα\
cos(π/2+α)=-sinα\
cos(π/2-α)=sinα\
]
有：
[R_z(frac{pi}{2}+L) =
egin{bmatrix}
-sinL&-cosL&0&0\
cosL&-sinL&0&0\
0&0&1&0\
0&0&0&1\
end{bmatrix}
ag{2}
]
[R_x(frac{pi}{2}-B) =
egin{bmatrix}
1&0&0&0\
0&sinB&-cosB&0\
0&cosB&sinB&0\
0&0&0&1\
end{bmatrix}
ag{3}
]
將(2)、(3)帶入(1)中，則有：
[R =
egin{bmatrix}
-sinL&-sinBcosL&cosBcosL&0\
cosL&-sinBsinL&cosBsinL&0\
0&cosB&sinB&0\
0&0&0&1\
end{bmatrix}
ag{4}
]
而從ECEF轉(zhuǎn)換到ENU的旋轉(zhuǎn)矩陣為：
[R^{-1} = {R_x(-(frac{pi}{2}-B))} cdot {R_z(-(frac{pi}{2}+L))}
ag{5}
]
旋轉(zhuǎn)矩陣是正交矩陣，根據(jù)正交矩陣的性質(zhì):正交矩陣的逆矩陣等于其轉(zhuǎn)置矩陣，那么可直接得：
[R^{-1} =
egin{bmatrix}
-sinL&cosL&0&0\
-sinBcosL&-sinBsinL&cosB&0\
cosBcosL&cosBsinL&sinB&0\
0&0&0&1\
end{bmatrix}
ag{6}
]
2.3. 總結(jié)
將上述公式展開，可得從ENU轉(zhuǎn)換到ECEF的圖形變換矩陣為：
[M = T cdot R =
egin{bmatrix}
1&0&0&X_p\
0&1&0&Y_p\
0&0&1&Z_p\
0&0&0&1\
end{bmatrix}
egin{bmatrix}
-sinL&-sinBcosL&cosBcosL&0\
cosL&-sinBsinL&cosBsinL&0\
0&cosB&sinB&0\
0&0&0&1\
end{bmatrix}
]
而從ECEF轉(zhuǎn)換到ENU的圖形變換矩陣為：
[M^{-1} = R^{-1} * T^{-1} =
egin{bmatrix}
-sinL&cosL&0&0\
-sinBcosL&-sinBsinL&cosB&0\
cosBcosL&cosBsinL&sinB&0\
0&0&0&1\
end{bmatrix}
egin{bmatrix}
1&0&0&-X_p\
0&1&0&-Y_p\
0&0&1&-Z_p\
0&0&0&1\
end{bmatrix}
]
3. 實現(xiàn)
接下來用代碼實現(xiàn)這個坐標(biāo)轉(zhuǎn)換，選取一個站心點，以這個站心點為原點，獲取某個點在這個站心坐標(biāo)系下的坐標(biāo)：
#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; //扁率倒數(shù)
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("原大地經(jīng)緯度坐標(biāo)：%.10lf %.10lf %.10lf
", x, y, z);
//Blh2Xyz(x, y, z);
//printf("地心地固直角坐標(biāo)：%.10lf %.10lf %.10lf
", x, y, z);
//Xyz2Blh(x, y, z);
//printf("轉(zhuǎn)回大地經(jīng)緯度坐標(biāo)：%.10lf %.10lf %.10lf
", x, y, z);
double x = -2318400.6045575836;
double y = 4562004.801366804;
double z = 3794303.054150639;
//116.9395751953 36.7399177551
printf("地心地固直角坐標(biāo)：%.10lf %.10lf %.10lf
", x, y, z);
Xyz2Blh(x, y, z);
printf("轉(zhuǎn)回大地經(jīng)緯度坐標(biāo)：%.10lf %.10lf %.10lf
", 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 << "地心轉(zhuǎn)站心矩陣：" << endl;
cout << wolrd2localMatrix << endl<<endl;
cout << "站心轉(zhuǎn)地心矩陣：" << 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坐標(biāo)（世界坐標(biāo)）：";
Eigen::Vector4d xyz(x, y, z, 1);
cout << xyz << endl;
cout << "ENU坐標(biāo)（局部坐標(biāo)）：";
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 << "地心轉(zhuǎn)站心矩陣：" << endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
printf("%lf ", worldToLocal.ptr()[j * 4 + i]);
}
cout << endl;
}
cout << endl;
osg::Matrixd localToWorld;
centerPoint.createLocalToWorld(localToWorld);
cout << "站心轉(zhuǎn)地心矩陣：" << endl;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
printf("%lf ", 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坐標(biāo)（世界坐標(biāo)）：";
osg::Vec3d out_world;
geoPoint.toWorld(out_world);
cout << out_world.x() <<' '<< out_world.y() << ' ' << out_world.z() << endl;
cout << "ENU坐標(biāo)（局部坐標(biāo)）：";
osg::Vec3d localCoord = worldToLocal.preMult(out_world);
cout << localCoord.x() << ' ' << localCoord.y() << ' ' << localCoord.z() << endl;
}
int main()
{
//TestBLH2XYZ();
cout << "使用Eigen進行轉(zhuǎn)換實現(xiàn)：" << endl;
TestXYZ2ENU();
cout <<"---------------------------------------"<< endl;
cout << "通過OsgEarth進行驗證：" << endl;
TestOE();
}
這個示例先用Eigen矩陣庫，計算了坐標(biāo)轉(zhuǎn)換需要的矩陣和轉(zhuǎn)換結(jié)果；然后通過osgEarth進行了驗證，兩者的結(jié)果基本一致。運行結(jié)果如下：
4. 參考
站心坐標(biāo)系和WGS-84地心地固坐標(biāo)系相互轉(zhuǎn)換矩陣
Transformations between ECEF and ENU coordinates
GPS經(jīng)緯度坐標(biāo)WGS84到東北天坐標(biāo)系ENU的轉(zhuǎn)換
三維旋轉(zhuǎn)矩陣；東北天坐標(biāo)系(ENU)；地心地固坐標(biāo)系(ECEF)；大地坐標(biāo)系(Geodetic)；經(jīng)緯度對應(yīng)圓弧距離
總結(jié)
以上是生活随笔為你收集整理的地心地固坐标系(ECEF)与站心坐标系(ENU)的转换的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： postgresql兴建用户_Postg
- 下一篇： 在vue中使用西瓜视频播放器