本文主要是介绍关于ENU与LLA坐标系互相转换的python代码，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
关于ENU与LLA坐标系互相转换的python代码
import math# WGS84 ellipsoid parameters
WGS_PARAMS = {'a': 6378137,'f': 1 / 298.257223563,'b': 6378137 * (1 - 1 / 298.257223563),
}
wgs84_a = 6378137
wgs84_b = 6378137 * (1 - 1 / 298.257223563)
wgs84_f = 1 / 298.257223563
# wgs84_f = 1.0 / 298.257223563
pow_e_2 = wgs84_f * (2.0 - wgs84_f)
def deg2radian(deg):return (deg * math.pi) / 180.0
def radian2deg(rad):return (rad * 180.0) / math.pi
def wgs2ecef(wgs):lng_r = deg2radian(wgs['lng'])lat_r = deg2radian(wgs['lat'])a = WGS_PARAMS['a']b = WGS_PARAMS['b']N = a ** 2 / math.sqrt(a ** 2 * math.cos(lat_r) ** 2 + b ** 2 * math.sin(lat_r) ** 2)x = (N + wgs['alt']) * math.cos(lat_r) * math.cos(lng_r)y = (N + wgs['alt']) * math.cos(lat_r) * math.sin(lng_r)z = (N * (b / a) ** 2 + wgs['alt']) * math.sin(lat_r)return {'x': x, 'y': y, 'z': z}def ecef2wgs(x, y, z):a = WGS_PARAMS['a']b = WGS_PARAMS['b']e = math.sqrt(a ** 2 - b ** 2)r = math.sqrt(x ** 2 + y ** 2 + z ** 2)u = math.sqrt(0.5 * (r ** 2 - e ** 2) + 0.5 * math.sqrt((r ** 2 - e ** 2) ** 2 + 4 * e ** 2 * z ** 2))Q = math.hypot(x, y)huE = math.hypot(u, e)Beta = math.atan((huE / u) * (z / Q))eps = ((b * u - a * huE + e ** 2) * math.sin(Beta)) / ((a * huE * 1) / math.cos(Beta) - e ** 2 * math.cos(Beta))Beta += epslat = math.atan((a / b) * math.tan(Beta))lng = math.atan2(y, x)alt = math.hypot(z - b * math.sin(Beta), Q - a * math.cos(Beta))inside = (x ** 2 / a ** 2 + y ** 2 / a ** 2 + z ** 2 / b ** 2) < 1if inside:alt = -altlat = radian2deg(lat)lng = radian2deg(lng)return lat, lng, altdef enu2uvw(east, north, up, lng0, lat0):lng_r = deg2radian(lng0)lat_r = deg2radian(lat0)t = math.cos(lat_r) * up - math.sin(lat_r) * northw = math.sin(lat_r) * up + math.cos(lat_r) * northu = math.cos(lng_r) * t - math.sin(lng_r) * eastv = math.sin(lng_r) * t + math.cos(lng_r) * eastreturn {'u': u, 'v': v, 'w': w}def enu2ecef(enu, wgs0):ecef0 = wgs2ecef(wgs0)uvw = enu2uvw(enu['x'], enu['y'], enu['z'], wgs0['lng'], wgs0['lat'])return {'x': ecef0['x'] + uvw['u'],'y': ecef0['y'] + uvw['v'],'z': ecef0['z'] + uvw['w'],}def enu2wgs(enu, wgs0):ecef = enu2ecef(enu, wgs0)return ecef2wgs(ecef['x'], ecef['y'], ecef['z'])def pos2ecef(lon0, lat0, alt0):# print('pos2ecef', lon, lat, alt)lon = math.radians(float(lon0))lat = math.radians(float(lat0))alt = float(alt0)cosLon = math.cos(lon)cosLat = math.cos(lat)sinLon = math.sin(lon)sinLat = math.sin(lat)N = wgs84_a / math.sqrt(1.0 - pow_e_2 * sinLat * sinLat)ecef_x = (N + alt) * cosLat * cosLonecef_y = (N + alt) * cosLat * sinLonecef_z = (N * (1.0 - pow_e_2) + alt) * sinLatprint('pos2ecef', lon0, lat0, alt0, '->', ecef_x, ecef_y, ecef_z)return (ecef_x, ecef_y, ecef_z)def pos2enu(lon, lat, alt, ref_lon, ref_lat, ref_alt):# lon0, lat0, alt0 = p0_lon, p0_lat, p0_altecef_x1, ecef_y1, ecef_z1 = pos2ecef(ref_lon, ref_lat, ref_alt)ecef_x0, ecef_y0, ecef_z0 = pos2ecef(lon, lat, alt)# offset_x, offset_y, offset_z = ecef_x1-ecef_x0, ecef_y1-ecef_y0, ecef_z1-ecef_z0offset_x, offset_y, offset_z = ecef_x0 - ecef_x1, ecef_y0 - ecef_y1, ecef_z0 - ecef_z1# array_delta = np.array( [[offset_x], [offset_y], [offset_z]], dtype=np.float64)# print(array_delta.reshape(1,3))# lon0 = math.radians(lon)# lat0 = math.radians(lat)lon0 = math.radians(ref_lon)lat0 = math.radians(ref_lat)cosLon = math.cos(lon0)cosLat = math.cos(lat0)sinLon = math.sin(lon0)sinLat = math.sin(lat0)x = -1 * sinLon * offset_x + cosLon * offset_yy = -1 * sinLat * cosLon * offset_x - 1 * sinLat * sinLon * offset_y + cosLat * offset_zz = cosLat * cosLon * offset_x + cosLat * sinLon * offset_y + sinLat * offset_zreturn (x, y, z)# array_S1 = np.array([-1 * sinLon, cosLon, 0], dtype=np.float64)# array_S2 = np.array([-1 * sinLat * cosLon, -1 * sinLat *sinLon, cosLat], dtype=np.float64)# array_S3 = np.array([cosLat * cosLon, cosLat * sinLon, sinLat], dtype=np.float64)# array_S = np.array([array_S1, array_S2, array_S3], dtype=np.float64)# v = np.dot(array_S, array_delta)# # print(v.reshape(1,3))# # print(v[0][0], v[1][0], v[2][0])# return (v[0][0], v[1][0], v[2][0])# Example usage
enu = {'x': -16.47, 'y': 8.667, 'z': 0}
wgs0 = {'lng': 121.5442525290947, 'lat': 31.229345976263158, 'alt': 0}
result = enu2wgs(enu, wgs0)
print(result)p0_lon, p0_lat, p0_alt = 121.5442525290947, 31.229345976263158, 0
p1_lon, p1_lat, p1_alt = result[1], result[0], result[2]
v1 = pos2enu(p1_lon, p1_lat, p1_alt, p0_lon, p0_lat, p0_alt) # p0_lon, p0_lat, p0_alt为基准坐标
print(v1)这篇关于关于ENU与LLA坐标系互相转换的python代码的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
