坐标系转换- 大地坐标系到GPS坐标系

 

世界大地测量系统(World Geodetic System, WGS)，比如WGS84，是一种地理坐标系统，用于全球定位系统(GPS)。 

 ENU（东、北、天）坐标系就是分别以原点正东方、正北方和垂直地表正上方作为XYZ三轴正方向的坐标系，ENU坐标系又称为站心坐标系或站点坐标系；

 NED（北、东、地）坐标系就是分别以原点正北方、正东方和正下方作为XYZ三轴正方向的坐标系，NED坐标系ENU坐标系本质上是一体两面的。

 

 

1.1 ECEF坐标系

 

  也叫地心地固直角坐标系。其原点为地球的质心，x轴延伸通过本初子午线（0度经度）和赤道（0deglatitude）的交点。 z轴延伸通过的北极（即，与地球旋转轴重合）。 y轴完成右手坐标系，穿过赤道和90度经度。

 

 ENU坐标系和NED坐标系如图1所示： 

 

 

1.2 WGS-84坐标
  也就是也叫经纬高坐标系(经度(longitude)，纬度(latitude)和高度(altitude)LLA坐标系)。，全球地理坐标系、大地坐标系。可以说是最为广泛应用的一个地球坐标系，它给出一点的大地纬度、大地经度和大地高程而更加直观地告诉我们该点在地球中的位置，故又被称作纬经高坐标系。WGS-84坐标系的X轴指向BIH(国际时间服务机构)1984.0定义的零子午面(Greenwich)和协议地球极(CTP)赤道的交点。Z轴指向CTP方向。Y轴与X、Z轴构成右手坐标系。

  一句话解释就是：把前面提到的ECEF坐标系用在GPS中，就是WGS-84坐标系。
其中：
（1）：大地纬度是过用户点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。

（2）：大地经度是过用户点P的子午面与本初子午线之间的夹角。经度值在-180°到+180°之间。

（3）：大地高度h是过用户点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。

 

 

 

 

1.3 东北天坐标系（ENU）
  也叫站心坐标系以用户所在位置P为坐标原点。

  坐标系定义为： X轴：指向东边 Y轴：指向北边 Z轴：指向天顶

  ENU局部坐标系采用三维直角坐标系来描述地球表面，实际应用较为困难，因此一般使用简化后的二维投影坐标系来描述。在众多二维投影坐标系中，统一横轴墨卡托（The Universal Transverse Mercator ，UTM）坐标系是一种应用较为广泛的一种。UTM 坐标系统使用基于网格的方法表示坐标，它将地球分为 60 个经度区，每个区包含6度的经度范围，每个区内的坐标均基于横轴墨卡托投影，如下图所示：

 

#!/usr/bin/python
# -*- coding: UTF-8 -*-



'''


// 北东天->gps
void world2GPS(double r_lat, double r_lon, double x_in_NED, double y_in_NED, double& t_lon, double& t_lat) {
    double y_rad = x_in_NED / C_EARTH;
    t_lat = (r_lat + RAD2DEG(y_rad));
    double x_rad = y_in_NED / C_EARTH / cos(DEG2RAD(t_lat));
    t_lon = r_lon + RAD2DEG(x_rad);
}

void trans_NED2GPS(std::map<std::string, cv::Point3d>& all_gps_data) {
    std::map<std::string, cv::Point3d>::iterator it;
    double init_lat = 25.44993764;  // 纬度
    double init_lon = 112.71335009; // 经度
    // std::ofstream out("../data/gps_ccctrans.txt", std::ios::app);
    for (it = all_gps_data.begin(); it != all_gps_data.end(); it++) {
        double lat, lon;
        // std::cout<<"x:"<<(it->second).x<<std::endl;
        // std::cout<<"y:"<<(it->second).y<<std::endl;
        world2GPS(init_lat, init_lon, (it->second).x, (it->second).y, lon, lat);
        it->second.x = lat;
        it->second.y = lon;
        // std::cout<<"lat:"<<lat<<std::endl;
        // std::cout<<"lon:"<<lon<<std::endl<<std::endl;
        // out<<it->first<<" "<<std::setprecision(12)<<lat<<" "<<lon<<" "<<(it->second).z<<std::endl;
    }
    // out.close();
}


'''

import pyproj

#初始化GPS位置
#111.492682  24.413449  351
gps_int=[111.492682,24.413449,351]


def ecef2lla(x,y,z):
    '''

    x = 652954.1006

    y = 0

    z = -4167647.7937

    '''

    #ecef转化为经纬高

    ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')

    lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')

    lon, lat, alt = pyproj.transform(ecef, lla, x, y, z, radians=False)#radians否用弧度返回值

    print ('纬度：',lat)

    print ('经度：',lon)

    print ('高度：',alt)

    return lat,lon,alt



import math as mt
'''以下为WGS84椭球参数'''
a = 6378137.0         # 参考椭球的长半轴, 单位 m
b = 6356752.31414     # 参考椭球的短半轴, 单位 m
 
'''以下为三角函数调用'''
a = 6378137.0         # 参考椭球的长半轴, 单位 m
f=1 / 298.257223563     # 参考椭球的扁率
b = 6356752.31414     # 参考椭球的短半轴, 单位 m
'''以下为三角函数调用'''
sqrt = mt.sqrt
sin = mt.sin
cos = mt.cos
atan = mt.atan
atan2=mt.atan2
fabs=math.fabs
'''弧度转角度'''
def rad2angle(r):
    """
    该函数可以实现弧度到角度的转换.
    :param r:  弧度
    :return:  a, 对应的角度
    """
    a = r*180.0/mt.pi
    return a
 
'''角度转换弧度函数'''
def angle2rad(a):
    """
    该函数可以实现角度到弧度的转换.
    :param a:  角度
    :return:  r, 对应的弧度
    """
    r = a*mt.pi/180.0
    return r
 
def XYZ2BLH(X, Y, Z):
    """
    该函数实现把某点在参心空间直角坐标系下的坐标（X, Y, Z)转为大地坐标（B, L, H).
    :param X:  X方向坐标，单位 m
    :param Y:  Y方向坐标, 单位 m
    :param Z:  Z方向坐标, 单位 m
    :param a: 地球长半轴，即赤道半径，单位 m
    :param b: 地球短半轴，即大地坐标系原点到两级的距离, 单位 m
    :return:  B, L, H, 大地纬度、经度、海拔高度 (m)
    """
    R = sqrt(X * X + Y * Y)
    B0 = atan2(Z, R)
 
    while 1:
        N = a/ sqrt(1.0 - f * (2 - f) * sin(B0) * sin(B0))
        B_ = atan2(Z + N * f* (2 - f) * sin(B0), R)
        if (fabs(B_ - B0) < 1.0e-7):
            break
        B0 = B_
    L_ = atan2(Y, X)
    H= R / cos(B_) - N
    B=rad2angle(B_)
    L=rad2angle(L_)
    return B,L,H
 
 
 
'''该函数把某点的大地坐标(B, L, H)转换为空间直角坐标（X, Y, Z)'''
def BLH2XYZ(B, L, H):
    """
     该函数把某点的大地坐标(B, L, H)转换为空间直角坐标（X, Y, Z).
    :param B:  大地纬度, 角度制, 规定南纬为负，范围为[-90, 90]
    :param L:  大地经度, 角度制, 规定西经为负, 范围为[-180, 180]
    :param H:  海拔，大地高, 单位 m
    :param a:  地球长半轴，即赤道半径，单位 m
    :param b:  地球短半轴，即大地坐标系原点到两级的距离, 单位 m
    :return:   X, Y, Z, 空间直角坐标, 单位 m
    """
 
    B = angle2rad(B)    # 角度转为弧度
    L = angle2rad(L)    # 角度转为弧度
 
    e = sqrt((a**2-b**2)/(a**2))     # 参考椭球的第一偏心率
    N = a/sqrt(1-e*e*sin(B)*sin(B))  # 卯酉圈半径, 单位 m
 
    X =(N+H)*cos(B)*cos(L)
    Y = (N+H)*cos(B)*sin(L)
    Z = (N*(1-e*e)+H)*sin(B)
    return X, Y, Z    # 返回空间直角坐标(X, Y, Z)
 
'''以下函数为大地坐标转站心地平坐标'''
def XYZ2NEU(X,Y,Z,X1,Y1,Z1):
    """
    该函数实现把某点在参心空间直角坐标系下的坐标（X, Y, Z)转为站心地平坐标(E,N,U).
     X,Y,Z 定位结果的XYZ值
     X1,Y1,Z1 参考坐标的XYZ值
    :return:  N, E ,U, 东方向，北方向，天顶方向(m)
    """
    dx=X-X1
    dy=Y-Y1
    dz=Z-Z1
    B,L,H=XYZ2BLH(X1,Y1,Z1)
    B=angle2rad(B)
    L=angle2rad(L)
    N = -sin(B)*cos(L)*dx-sin(B)*sin(L)*dy+cos(B)*dz
    E = -sin(L)*dx+cos(L)*dy
    U = cos(B)*cos(L)*dx+cos(B)*sin(L)*dy+sin(B)*dz
    H=sqrt(pow(N,2)+pow(E,2))#平面方向高程
    error=sqrt(pow(dx,2)+pow(dy,2)+pow(dz,2))
    return H,U,error