GPS经纬度坐标与XY坐标相互转换的python程序

前言

室外定位常用的是GPS，故编队队形、设定轨迹都是基于GPS经纬度坐标。而在仿真中我们通常会在XY坐标系下进行，所以需要一组GPS经纬度坐标与XY坐标相互转换的函数。
查阅很多资料，但没有找到我想要的简单python程序，于是自己按照需求编写一个。

一、说明

参考px4官方代码：PX4中GPS与XY坐标系转换函数。可以查看其中的MapProjection::project和MapProjection::reproject函数。
GPS与XY坐标系转关系是：基于一个参考GPS经纬度坐标，计算其与当前GPS经纬度坐标的偏差，这个偏差就是XY坐标。
使用时请注意，以下坐标转换函数中输入输出变量的单位为：

• GPS经纬度的单位是 °
• XY的单位是 m

此外，X轴正方向为北，Y轴正方向为东。

二、函数

1.import 和 常数

放在文件最开始：

import math
import numpy as np
CONSTANTS_RADIUS_OF_EARTH = 6371000.     # meters (m)

2.GPS经纬度转XY坐标

输入目标经纬度坐标和参考经纬度坐标，得到相对XY坐标。
代码如下：

def GPStoXY(self, lat, lon, ref_lat, ref_lon):
        # input GPS and Reference GPS in degrees
        # output XY in meters (m) X:North Y:East
        lat_rad = math.radians(lat)
        lon_rad = math.radians(lon)
        ref_lat_rad = math.radians(ref_lat)
        ref_lon_rad = math.radians(ref_lon)

        sin_lat = math.sin(lat_rad)
        cos_lat = math.cos(lat_rad)
        ref_sin_lat = math.sin(ref_lat_rad)
        ref_cos_lat = math.cos(ref_lat_rad)

        cos_d_lon = math.cos(lon_rad - ref_lon_rad)

        arg = np.clip(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon, -1.0, 1.0)
        c = math.acos(arg)

        k = 1.0
        if abs(c) > 0:
            k = (c / math.sin(c))

        x = float(k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * self.CONSTANTS_RADIUS_OF_EARTH)
        y = float(k * cos_lat * math.sin(lon_rad - ref_lon_rad) * self.CONSTANTS_RADIUS_OF_EARTH)

        return x, y

3.XY坐标转GPS经纬度

输入相对XY坐标和参考经纬度坐标，得到目标经纬度坐标。
代码如下：

def XYtoGPS(self,x, y, ref_lat, ref_lon):
        x_rad = float(x) / self.CONSTANTS_RADIUS_OF_EARTH
        y_rad = float(y) / self.CONSTANTS_RADIUS_OF_EARTH
        c = math.sqrt(x_rad * x_rad + y_rad * y_rad)

        ref_lat_rad = math.radians(ref_lat)
        ref_lon_rad = math.radians(ref_lon)

        ref_sin_lat = math.sin(ref_lat_rad)
        ref_cos_lat = math.cos(ref_lat_rad)

        if abs(c) > 0:
            sin_c = math.sin(c)
            cos_c = math.cos(c)

            lat_rad = math.asin(cos_c * ref_sin_lat + (x_rad * sin_c * ref_cos_lat) / c)
            lon_rad = (ref_lon_rad + math.atan2(y_rad * sin_c, c * ref_cos_lat * cos_c - x_rad * ref_sin_lat * sin_c))

            lat = math.degrees(lat_rad)
            lon = math.degrees(lon_rad)

        else:
            lat = math.degrees(ref_lat)
            lon = math.degrees(ref_lon)

        return lat, lon

总结

本文提供了GPS经纬度和XY坐标相互转换的函数，并且可以保证较高的精度。
并且提供了完整的程序以及调用样例：

GPS经纬度坐标与XY坐标相互转换的python程序