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

随笔- 1525 文章- 2 评论- 66 阅读- 128万

坐标系转换- 大地坐标系到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度的经度范围，每个区内的坐标均基于横轴墨卡托投影，如下图所示：

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

#!/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