【ROS系列】坐标系转换介绍和对齐

一、坐标系简介

本篇文章介绍：ECEF、ENU、UTM、WGS-84坐标系（LLA)

1.1、ECEF坐标系

ECEF坐标系也叫地心地固直角坐标系。

原点：地球的质心，

x轴：原点延伸通过本初子午线（0度经度）和赤道（0维度）的交点。

z轴：原点延伸通过的北极，也就是理想地球旋转轴。

y轴：Z-->X 完成右手定则，穿过赤道和90度经度。如下图所示：

 

1.2、 WGS-84坐标系（LLA)

WGS-84坐标系也叫经纬高坐标系(经度(longitude)，纬度(latitude)和高度(altitude)LLA坐标系)，全球地理坐标系、大地坐标系。

特点：最为广泛应用的一个地球坐标系，它给出一点的大地纬度、大地经度和大地高程。更加直观地告诉我们该点在地球中的位置，故又被称作经纬高坐标系。

WGS-84坐标系：

X轴：指向BIH(国际时间服务机构)1984.0定义的零子午面(Greenwich)和协议地球极(CTP)赤道的交点。

Z轴：指向CTP方向。

Y轴：与X、Z轴构成右手坐标系。
其中：
（1）大地纬度是过点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。

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

（3）大地高度h是过点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。
图示可以参照上图，清楚描述ECEF 和 WGS-84的关系。WGS-84和GPS关系参照下图：

1.3、 东北天坐标系（ENU）

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

坐标系定义为：

X轴：指向东边

Y轴：指向北边

Z轴：指向天顶

ENU局部坐标系采用三维直角坐标系来描述地球表面，实际应用较为困难，因此一般使用简化后的二维投影坐标系来描述。

二维投影坐标系中 统一横轴墨卡托（The Universal Transverse Mercator ，UTM）坐标系是一种应用较为广泛的一种。

UTM 坐标系统使用基于网格的方法表示坐标，它将地球分为 60 个经度区，每个区包含6度的经度范围，每个区内的坐标均基于横轴墨卡托投影，如下图所示：

将上图切分后展平得到下图：

DMAP: UTM Grid Zones of the World

根据中国utm分区表，确定所在地区的utm分区，最常用的对照以下表：

等会坐标转换就可以使用这个图上的分区。

二、坐标转换

2.1、工具介绍

主要使用pyproj工具进行坐标的转换，文档地址：入门 — pyproj 2.1.1 文档

2.2、坐标之间相互转换

知识前置，我们最常见的的是gps坐标拿到经纬度和海拔信息。gps使用的坐标系为WGS，其编号（EPSG）EPSG：4326 ，这个参数后续要用到

在使用UTM坐标系转化时要弄清处数据在utm分区表中哪一个分区，假设当前经纬信息为【121.398926，31.559815 】可以根据分区表得到北半球51分区，即51N

网站EPSG.io: Coordinate Systems Worldwide 上查询utm分区对应的utm坐标系编码，输入 51N 进行查询,找到对应WGS84 转换前坐标系。得到EPSG：32651

转换原理

（1） 经纬度坐标系转ECEF

 

 （2）ECEF坐标系转LLA坐标系

start：lon是未知的，假设为0，经过几次迭代之后就能收敛

update公式:  alt=(p/cos(lat))-N

（3）ECEF坐标系转ENU坐标系

用户所在坐标原点0=(0,0,0),计算点=(,,)在以点0为坐标原点的ENU坐标系位置(e,n,u)这里需要用到LLA坐标系的数据，0的LLA坐标点为0=(0,0,0)

（4）ENU坐标系转ECEF坐标系

上面介绍S为单位正交阵

 （5）经纬度LLA坐标系直接转ENU坐标系

上述可以看到，从LLA坐标系转换到enu坐标系有较多计算量，在考虑地球偏心率很小的前提下，可以做一定的近似公式计算

代码实现

（1） 经纬度转UTM坐标

    # 经纬度到UTM世界平面坐标系
    def wgs2utm_51N(lat,lon):
        crs = pyproj.CRS.from_epsg(4326)
        crs_cs = pyproj.CRS.from_epsg(32651)
        transformer = Transformer.from_crs(crs, crs_cs)
        x, y = transformer.transform(lat, lon)

        return [x, y]

（2）UTM转经纬坐标

    # 经纬度到UTM世界平面坐标系
    def utm_51N2wgs(x,y):
        transformer = Transformer.from_crs("epsg:32651", "epsg:4326")
        lat, lon = transformer.transform(x, y)
        return [lat, lon]

（3）经纬海拔转 地心地固坐标

    # 经纬度到地心地固(earth-central, earth-fixed)坐标系
    def wgs2ecef(lon,lat,alt):
        ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
        lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
        x, y, z = pyproj.transform(lla, ecef, lon, lat, alt,
                                   radians=False)

        return np.array([x, y, z])

（4）地心地固转enu坐标

  # 用户所在坐标原点Pb=(x0,y0,z0),计算点P=(x,y,z)在以点P0为坐标原点的坐标系位置(e,n,u),
    # 这里需要用到LLA的数据,P0的LLA坐标点为LLAo=(lon0,lat0,alt0)
    # 地心地固到东北天坐标系
    def ecef2enu(self, point0, point):
        # 转换为弧度
        lat_r = math.radians(self.lat)
        lon_r = math.radians(self.lon)
        line1 = np.array([-math.sin(lon_r), math.cos(lon_r), 0])
        line2 = np.array([-math.sin(lat_r) * math.cos(lon_r),
                          -math.sin(lat_r) * math.sin(lon_r),
                          math.cos(lat_r)])
        line3 = np.array([math.cos(lat_r) * math.cos(lon_r),
                          math.cos(lat_r) * math.sin(lon_r),
                          math.sin(lat_r)])
        trans = np.vstack((line1, line2, line3))
        difference = point - point0
        [e, n, u] = np.matmul(trans, np.array(difference).T).T

        return [e, n, u]

（5）enu转地心地固坐标

    def enu2ecef(self, point):
        # 转换为弧度
        lat_r = math.radians(self.lat)
        lon_r = math.radians(self.lon)
        line1 = np.array([-math.sin(lon_r), math.cos(lon_r), 0])
        line2 = np.array([-math.sin(lat_r) * math.cos(lon_r),
                          -math.sin(lat_r) * math.sin(lon_r),
                          math.cos(lat_r)])
        line3 = np.array([math.cos(lat_r) * math.cos(lon_r),
                          math.cos(lat_r) * math.sin(lon_r),
                          math.sin(lat_r)])
        trans = np.vstack((line1, line2, line3))
        [B, L, H] = np.matmul(trans.T, point)

        return [B, L, H]

（6）enu转imu坐标

    # enu空间中的点在惯导坐标系下的坐标,俯仰角绕东轴(x)旋转,横滚角绕北轴(y)旋转,
    # 航向角绕天轴(z)旋转,这里可以认为简化,只考虑偏航角的情况下
    # def enu2imu(self, imu_enu, pitch, roll, yaw):
    def enu2imu(self, imu_enu, yaw, points_enu):
        Rz = np.array([[math.cos(yaw), -math.sin(yaw), 0],
                       [math.sin(yaw), math.cos(yaw), 0],
                       [0, 0, 1]])
        T = np.array(-imu_enu)
        trans = np.hstack((Rz, T))
        imu_xyz = np.matmul(trans, points_enu)

        return imu_xyz