地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)

目录

  • 一、大地坐标系现状简析
    • 1.1 我国
    • 1.2 美国
    • 1.3 日本
  • 二、经纬度坐标(L,B,H)与直角坐标系坐标(X,Y,Z)相互转换
    • 2.1 正解(L,B,H)——>(X,Y,Z)
      • 2.1.1 数学推导
      • 2.1.2 C++代码实现
      • 2.1.3 matlab代码实现
    • 2.2 反解(X,Y,Z)——>(L,B,H)
      • 2.2.1 数学推导
      • 2.2.2 C++代码实现
      • 2.2.3 matlab代码实现



一、大地坐标系现状简析

1.1 我国

  解放后在我国大陆地区曾先后使用了三种坐标系。其一,于1954年引入了苏联1942年普尔科沃坐标系(PULK0V01942),经局部平差建立了“1954年北京坐标系(BJ54)”,使用克拉索夫斯基(Krassowsky)椭球,原点位于前苏联普尔科沃;其二,在20世纪70年代末期,利用天文大地测量资料通过统一整体平差,建立了“1980年国家大地坐标系”,使用UGG75椭球,大地原点位于陕西省西安市北泾阳县永乐镇,又称“1980年西安大地坐标系”;其三,在1980年国家大地坐标系基础上建立“新1954年北京坐标系”,恢复使用了克拉索夫斯基椭球,有利于新坐标成果的使用和新、旧图的拼接。
  未来几年,我国将逐渐使用新一代大地坐标系“2000国家大地坐标系”(China Geodetic Coordinate System2000,CGCS2000)。其定义与协议地球参考系的定义一致,即原点是包括海洋和大气的整个地球质心,初始定向由1984.0时国际时间局定向给定,定向的时间演化使得地壳无整体旋转。

1.2 美国

  美国国防部以军事目的获取全球地面定位信息,相继建立了多个全球地心坐标系。美国国防部于1984年继WGS60、WGS66、WGS72之后建立起来第四个世界大地坐标系世界大地坐标系(World Geodetic System1984,WGS84)。WGS84规定其原点在地球质心、Z轴指向BH1984.0定义的协议地球极方向、X轴指向BH1984.0的零子午面和协议地球极赤道的交点,Y轴与X、Z轴构成右手地心地固ECEF直角坐标系。

1.3 日本

  日本在1918年之前,使用的是“日本东京1892年大地坐标系(JTD1892)”,又称“旧东京坐标系”,坐标原点在东京麻布旧东京天文台(旧海军观象台)子午仪中心点,采用1841年贝塞尔椭球,a=6377397.155米,1/f=299.1528。由于东京坐标系与WGS84坐标系的差异产生的问题日益突出。于是,2001年6月日本新测量法明确规定了其新的大地基准应该与WGS84保持一致,同时新建立的“日本大地基准2000(JGD2000)”取代了“东京坐标系”。东京坐标系通过中间坐标系Tokyo97与JGD2000大地坐标系(水平)相连,Tokyo97采用东京坐标系原点和贝塞尔参考椭球,轴向与1TRF94一致。东京坐标系坐标首先通过平差或网格内插变换至Tokyo97后,然后再变换至TRF94。这样便形成了JGD2000大地坐标系(水平),它采用GRS80椭球。

二、经纬度坐标(L,B,H)与直角坐标系坐标(X,Y,Z)相互转换

  以目前使用最为广泛的WGS84的经纬度坐标进行转换,是基于同一基准的大地坐标系转换。

2.1 正解(L,B,H)——>(X,Y,Z)

2.1.1 数学推导

地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第1张图片
  如上示意图,求大地坐标系中点P(L,B,H)在大地空间直角坐标系的坐标(X,Y,Z)分为三步:

  1. 首先,求得P点在其子午线平面坐标系O-xy内坐标(x,y)与纬度B的关系:

地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第2张图片

  1. 其次,求得子午线平面坐标(x,y)在大地空间直角坐标系坐标(X,Y,Z):
    在这里插入图片描述

  2. 最后,求得大地坐标系坐标P(L,B,H)在大地空间直角坐标系中的坐标(X,Y,Z):
    地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第3张图片

以上公式中:
a:长半轴,取 6378137m;
b:短半轴,取 6356752.3142451793m;
e^2:椭球偏心率
N:椭圆曲率半径
在这里插入图片描述

2.1.2 C++代码实现

原代码:lla2xyz.cpp

#include 
#include 
#include 
constexpr double DEG_TO_RAD_LOCAL = 3.1415926535897932 / 180.0;

void LLA2XYZ(double longitude, double latitude, double height, double& X, double& Y, double& Z)
{
    double lon = longitude * DEG_TO_RAD_LOCAL;      //经度
    double lat = latitude * DEG_TO_RAD_LOCAL;
    double hei = height;

    // variable
    double a = 6378137.0;       //地球赤道半径 ,单位是 m
    double b = 6356752.31424518;    //地球短半轴 ,单位是 m
    //double E = (a * a - b * b) / (a * a);   // E = e^2
    //std::cout << "E= "<< E << std::endl;
    //double N = a/(sqrt(1-E*sin(lat)*sin(lat)));
    double N = a/(sqrt(1-((a * a - b * b) / (a * a)) * sin(lat) * sin(lat)));
    //std::cout << "N= "<< N << std::endl;

    X =(N + hei ) * cos(lat) * cos(lon);
    Y =(N + hei ) * cos(lat) * sin(lon);
    Z =((b * b * N)/ (a * a) + hei) * sin(lat) ;
}

int main()
{
    
    double longitude = 116.17;
    double latitude = 40.22;
    double height = 36.77;
    double X;
    double Y;
    double Z;

    LLA2XYZ(longitude, latitude, height, X, Y, Z);
    
    std::cout<

查看计算结果:

g++ lla2xyz.cpp -o lla2xyz
./lla2xyz

在这里插入图片描述

2.1.3 matlab代码实现

原代码:LLA2XYZ.m

%% WGS84经纬度LLA转直角坐标系XYZ
% 东经正数,西经为负数
% 北纬为正数,南纬为负数
% 输入参数1:纬度;输入参数2:经度;输入参数3:高度
% 经纬度单位:度;高度单位:米
% 东经116.17° 北纬40.22° 高度36.77m
function [x,y,z]=LLA2XYZ(latitude,longitude,height)
a = 6378137.0;%单位m
b = 6356752.31424518;%单位m
E = (a * a - b * b) / (a * a);

COSLAT = cos(latitude * pi / 180);
SINLAT = sin(latitude * pi / 180);
COSLONG = cos(longitude * pi / 180);
SINLONG = sin(longitude * pi / 180);
N = a / (sqrt(1 - E * SINLAT * SINLAT));
NH = N + height;
x = NH * COSLAT * COSLONG;
y = NH * COSLAT * SINLONG;
z = (b * b * N / (a * a) + height) * SINLAT;
end

查看计算结果:

[x,y,z] = LLA2XYZ(40.22 , 116.17 , 36.77 )

地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第4张图片

2.2 反解(X,Y,Z)——>(L,B,H)

2.2.1 数学推导

  对于大地空间直角坐标(X,Y,Z)计算对应的大地坐标(L,B,H)通常也分三部分:求解大地经度、求解大地纬度和计算大地高。

  1. 大地经度的解算
    根据正解公式,可以得到:
    地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第5张图片

  2. 大地高的解算
    解得经度后:
    地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第6张图片

  3. 大地纬度解算
    大地纬度解算通常使用迭代方法和直接方法。
    地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第7张图片

迭代法公式:
地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第8张图片
直接法公式:
地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第9张图片

2.2.2 C++代码实现

原代码:xyz2lla.cpp

#include 
#include 
#include 
constexpr double DEG_TO_RAD_LOCAL = 3.1415926535897932 / 180.0;
constexpr double RAD_TO_DEG_LOCAL = 180.0 /3.1415926535897932;

void XYZ2LLA(  double X, double Y, double Z,double& longitude, double& latitude,double& altitude)
{
double a, b, c, d;
		double Longitude;// 经度
		double Latitude;// 纬度
		double Altitude;// 海拔高度
		double p, q;
		double N;
		a = 6378137.0;
		b = 6356752.31424518;
		c = sqrt(((a * a) - (b * b)) / (a * a));
		d = sqrt(((a * a) - (b * b)) / (b * b));
		p = sqrt((X * X) + (Y * Y));
		q = atan2((Z * a), (p * b));
		
		longitude = atan2(Y, X);
		N = a / sqrt(1 - ((c * c) * sin(latitude)* sin(latitude)));
		altitude = (p / cos(latitude)) - N;
		latitude = atan2((Z + (d * d) * b * pow(sin(q), 3)), (p - (c * c) * a * pow(cos(q), 3)));
		
		longitude = longitude * RAD_TO_DEG_LOCAL;
		latitude = latitude * RAD_TO_DEG_LOCAL;
}

int main()
{
    
    double longitude;
    double latitude ;
    double altitude ;
    double X=-2150931.511720;
    double Y=4377053.846931;
    double Z=4096692.121877;

    XYZ2LLA(X,Y,Z,longitude, latitude, altitude);
    
    std::cout<

查看计算结果:

g++ xyz2lla.cpp -o xyz2lla
./xyz2lla

在这里插入图片描述

2.2.3 matlab代码实现

原代码:XYZ2LLA.m

%% WGS84直角坐标系转经纬度
% 东经正数,西经为负数
% 北纬为正数,南纬为负数
% 输入参数1:纬度;输入参数2:经度;输入参数3:高度
% 经纬度单位:度;高度单位:米
function [latitude,longitude,height]=XYZ2LLA(x,y,z)
 
a = 6378137.0;
b = 6356752.31424518;
c = sqrt(((a * a) - (b * b)) / (a * a));
d = sqrt(((a * a) - (b * b)) / (b * b));
p = sqrt((x * x) + (y * y));
q = atan((z * a)/ (p * b));
longitude = atan(y/x);
latitude = atan((z + (d * d) * b * sin(q)^3)/(p - (c * c) * a * cos(q)^3));
N = a / sqrt(1 - ((c * c) * sin(latitude)^2));
height = (p / cos(latitude)) - N;
longitude = longitude * 180.0 / pi+180;
latitude = latitude * 180.0 / pi;
end

查看计算结果:

[latitude,longitude,height]=XYZ2LLA(-2150931.511720,4377053.846931,4096692.121877)

地心地固坐标系WGS84经纬度与笛卡尔直角坐标系相互转换的推导、理解与代码实现(C++和matlab)_第10张图片

你可能感兴趣的:(组合导航,matlab,c++,开发语言)