UTM坐标与GPS经纬度(WGS84)的相互转换

一、UTM介绍

统一横轴墨卡托投影系统（Universal Transverse Mercator，UTM）

参考：

https://www.youtube.com/watch?v=LcVlx4Gur7I

https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system

GPS经纬度坐标转UTM坐标(c++)_Bobsweetie的博客-CSDN博客_c++ 经纬度转utm

UTM坐标与时区说明

二、python转换

GPS经纬度（WGS84、大地坐标和UTM坐标相互转换

#!/usr/bin/env python3
#coding=utf-8

import utm

# 经纬度转UTM
lat=37.065
lon=128.543
# from_latlon(latitude, longitude, force_zone_number=None, force_zone_letter=None)
utm_ = utm.from_latlon(lat, lon)
utm_x = utm_[0]
utm_y = utm_[1]
utm_zone = utm_[2]
utm_band = utm_[3]
print("utm_x: %s, utm_y: %s, utm_zone: %s, utm_band: %s" % (utm_x, utm_y, utm_zone, utm_band))

# UTM转经纬度
lat, lon = utm.to_latlon(utm_x, utm_y, utm_zone, utm_band)
print("lat: %s, lon: %s" % (lat, lon))

输出：

utm_x: 459372.45701569295, utm_y: 4102180.728899499, utm_zone: 52, utm_band: S
lat: 37.06499999994699, lon: 128.54299999005866 

三、C++转换

经纬度转UTM

void LonLat2UTM(double longitude, double latitude, double& UTME, double& UTMN)
{
	double lat = latitude;
	double lon = longitude;

	double kD2R = PI / 180.0;
	double ZoneNumber = floor((lon - 1.5) / 3.0) + 1;
	double L0 = ZoneNumber * 3.0;

	double a = 6378137.0;
	double F = 298.257223563;
	double f = 1 / F;
	double b = a * (1 - f);
	double ee = (a * a - b * b) / (a * a);
	double e2 = (a * a - b * b) / (b * b);
	double n = (a - b) / (a + b); 
	double n2 = (n * n); 
	double n3 = (n2 * n); 
	double n4 = (n2 * n2); 
	double n5 = (n4 * n);
	double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2.0;
	double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32.0;
	double gm = 15 * n2 / 16 - 15 * n4 / 32;
	double dt = -35 * n3 / 48 + 105 * n5 / 256;
	double ep = 315 * n4 / 512;
	double B = lat * kD2R;
	double L = lon * kD2R;
	L0 = L0 * kD2R;
	double l = L - L0; 
	double cl = (cos(B) * l); 
	double cl2 = (cl * cl); 
	double cl3 = (cl2 * cl); 
	double cl4 = (cl2 * cl2); 
	double cl5 = (cl4 * cl); 
	double cl6 = (cl5 * cl); 
	double cl7 = (cl6 * cl); 
	double cl8 = (cl4 * cl4);
	double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
	double t = tan(B); 
	double t2 = (t * t); 
	double t4 = (t2 * t2); 
	double t6 = (t4 * t2);
	double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
	double yt = e2 * cos(B) * cos(B);
	double N = lB;
	N = N + t * Nn * cl2 / 2;
	N = N + t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
	N = N + t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
	N = N + t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
	double E = Nn * cl;
	E = E + Nn * cl3 * (1 - t2 + yt) / 6;
	E = E + Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
	E = E + Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
	E = E + 500000;
	N = 0.9996 * N;
	E = 0.9996 * (E - 500000.0) + 500000.0;

	UTME = E;
	UTMN = N;
}

UTM转经纬度

四、验证

转换结果可以利用软件：UTM Coordinate Converter进行验证

---

自动驾驶地图定位中的坐标变换：

GPS/RTK获得经度lon、纬度lat、高程height、偏航角yaw、俯仰角pitch、横滚角roll，将lon,lat转化为utm坐标，（utm_x, utm_y, height）就可以作为GPS此时在世界坐标系下位置，再把yaw,pitch,roll转化为旋转矩阵，就可以得到GPS在世界坐标系下的姿态q，合起来就得到了GPS在世界坐标系下的位姿，通过标定可得到GPS到车身base_link坐标系的变换，这样就能得到当前车身base_link在世界坐标系下的位姿，再由标定可得到lidar到车身base_link坐标系的变换，这样就能把lidar坐标系下的点云变换到世界坐标系下，也就是高精地图中点云的坐标。

由于地图太大，需要对其进行方形网格分块进行存储和查询，每一块的id可通过resolution, col(utm_x/resolution), row(utm_y/resolution), utm_zone, utm_band进行编码

同时，将GPS得到的经纬度转换为火星坐标系，就通过一些地理信息可视化库在地图上可视化轨迹，如通过轨迹坐标生成kml文件，就可以在谷歌地球上显示轨迹

---

 高程系统