EarthModel 接口文档¶
简介¶
简述：本文件提供了基于MAV坐标系约定的大地模型与坐标转换工具类，支撑无人机仿真场景下的各类坐标系统一转换计算。
在无人机仿真与飞控开发过程中，不同应用场景通常使用不同的坐标定义，包括WGS84经纬度坐标、ENU东北天坐标、NED北东地坐标等多种体系，坐标转换的准确性直接影响无人机定位、导航与控制算法的仿真效果。本模块作为RflySimSDK控制工具链的基础支撑模块，适配PX4飞控的MAV坐标系规范，可满足仿真环境中坐标转换、大地位置计算等核心需求，适用于无人机路径规划、状态估计、异机坐标对齐等各类开发仿真场景。
快速开始¶
最简可用示例，复制后修改最少配置即可运行。
from RflySimSDK.ctrl import EarthModel
# 初始化地球坐标转换模型
earth = EarthModel()
# 定义 home 点经纬度高（单位：弧度，高度单位：米），示例为北京大致坐标
lat0 = 39.9 * 3.1415926 / 180 # 纬度转弧度
lon0 = 116.3 * 3.1415926 / 180 # 经度转弧度
h0 = 50 # 原点高度，单位米
# 待转换目标点经纬度高
lat = 39.901 * 3.1415926 / 180
lon = 116.301 * 3.1415926 / 180
h = 100
# 第一步：经纬度高(LLA)转换为地心地固坐标(ECEF)
x_ecef, y_ecef, z_ecef = earth.lla2ecef(lat, lon, h)
print(f"目标点ECEF坐标：X={x_ecef:.2f} Y={y_ecef:.2f} Z={z_ecef:.2f} 米")
# 第二步：ECEF坐标转换为相对于原点的ENU东北天坐标
x_enu, y_enu, z_enu = earth.ecef2enu(x_ecef, y_ecef, z_ecef, lat0, lon0, h0)
print(f"目标点相对于原点的ENU坐标：东={x_enu:.2f} 北={y_enu:.2f} 天={z_enu:.2f} 米")
# 反向转换：ENU转ECEF验证结果
x_ecef_rev, y_ecef_rev, z_ecef_rev = earth.enu2ecef(x_enu, y_enu, z_enu, lat0, lon0, h0)
print(f"反向转换回的ECEF坐标：X={x_ecef_rev:.2f} Y={y_ecef_rev:.2f} Z={z_ecef_rev:.2f} 米")
环境与依赖¶
- Python 环境：
>= 3.8.10 - 依赖库：
copy、cv2、math、numpy、os、socket、struct、sys、threading、time - 前置准备：调用此接口前，需确保已正确导入RflySimSDK模块并完成基础环境配置。
核心接口说明¶
该模块 EarthModel.py 包含了配置变量、辅助函数及核心业务类。
全局常量与枚举定义¶
本节列出模块中所有可直接引用的全局常量和枚举定义。
独立常量¶
无
枚举定义¶
Coordinate¶
参考MAV_FRAME
| 名称 | 类型 | 值 | 说明 |
|---|---|---|---|
LOCAL_NED |
int |
1 |
- |
GLOBAL_INT |
int |
5 |
- |
NED_BODY |
int |
8 |
- |
全局/独立函数¶
无
EarthModel 类¶
提供地球坐标系之间的相互转换功能，支持经纬度高度、地心地固坐标系、东北天坐标系、北东地坐标系之间的坐标变换，常用于无人机仿真中的坐标定位与导航计算场景。
__init__()¶
功能说明：初始化 EarthModel 类实例
参数列表 (Args)：
无参数 返回值 (Returns)：
EarthModel实例对象
异常 (Raises)：
- 无
lla2ecef(lat, lon, h)¶
功能说明：将经纬度高度坐标（LLA，WGS84坐标系）转换为地心地固坐标（ECEF） 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
lat |
float |
是 | - | 纬度，单位为度 |
lon |
float |
是 | - | 经度，单位为度 |
h |
float |
是 | - | 高度，单位为米 |
返回值 (Returns)：
tuple[float, float, float]: 地心地固坐标系下的 (x, y, z) 坐标，单位为米
异常 (Raises)：
- 无
ecef2enu(x, y, z, lat0, lon0, h0)¶
功能说明：将地心地固坐标（ECEF）转换为以参考点为原点的东北天坐标（ENU） 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
x |
float |
是 | - | 地心地固坐标系下的X坐标，单位为米 |
y |
float |
是 | - | 地心地固坐标系下的Y坐标，单位为米 |
z |
float |
是 | - | 地心地固坐标系下的Z坐标，单位为米 |
lat0 |
float |
是 | - | 参考点纬度，单位为度 |
lon0 |
float |
是 | - | 参考点经度，单位为度 |
h0 |
float |
是 | - | 参考点高度，单位为米 |
返回值 (Returns)：
tuple[float, float, float]: 东北天坐标系下的 (东, 北, 天) 坐标，单位为米
异常 (Raises)：
- 无
enu2ecef(xEast, yNorth, zUp, lat0, lon0, h0)¶
功能说明：将以参考点为原点的东北天坐标（ENU）转换为地心地固坐标（ECEF） 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
xEast |
float |
是 | - | 东北天坐标系下的东向坐标，单位为米 |
yNorth |
float |
是 | - | 东北天坐标系下的北向坐标，单位为米 |
zUp |
float |
是 | - | 东北天坐标系下的天向坐标，单位为米 |
lat0 |
float |
是 | - | 参考点纬度，单位为度 |
lon0 |
float |
是 | - | 参考点经度，单位为度 |
h0 |
float |
是 | - | 参考点高度，单位为米 |
返回值 (Returns)：
tuple[float, float, float]: 地心地固坐标系下的 (x, y, z) 坐标，单位为米
异常 (Raises)：
- 无
ecef2lla(x, y, z)¶
功能说明：将地心地固坐标（ECEF）转换为经纬度高度坐标（LLA，WGS84坐标系） 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
x |
float |
是 | - | 地心地固坐标系下的X坐标，单位为米 |
y |
float |
是 | - | 地心地固坐标系下的Y坐标，单位为米 |
z |
float |
是 | - | 地心地固坐标系下的Z坐标，单位为米 |
返回值 (Returns)：
tuple[float, float, float]: 经纬度高度坐标 (纬度, 经度, 高度)，纬度经度单位为度，高度单位为米
异常 (Raises)：
- 无
lla2enu(lat, lon, h, lat_ref, lon_ref, h_ref)¶
功能说明：直接将经纬度高度坐标（LLA）转换为以参考点为原点的东北天坐标（ENU） 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
lat |
float |
是 | - | 目标点纬度，单位为度 |
lon |
float |
是 | - | 目标点经度，单位为度 |
h |
float |
是 | - | 目标点高度，单位为米 |
lat_ref |
float |
是 | - | 参考点纬度，单位为度 |
lon_ref |
float |
是 | - | 参考点经度，单位为度 |
h_ref |
float |
是 | - | 参考点高度，单位为米 |
返回值 (Returns)：
tuple[float, float, float]: 东北天坐标系下的 (东, 北, 天) 坐标，单位为米
异常 (Raises)：
- 无
enu2lla(xEast, yNorth, zUp, lat_ref, lon_ref, h_ref)¶
功能说明：直接将以参考点为原点的东北天坐标（ENU）转换为经纬度高度坐标（LLA） 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
xEast |
float |
是 | - | 东北天坐标系下的东向坐标，单位为米 |
yNorth |
float |
是 | - | 东北天坐标系下的北向坐标，单位为米 |
zUp |
float |
是 | - | 东北天坐标系下的天向坐标，单位为米 |
lat_ref |
float |
是 | - | 参考点纬度，单位为度 |
lon_ref |
float |
是 | - | 参考点经度，单位为度 |
h_ref |
float |
是 | - | 参考点高度，单位为米 |
返回值 (Returns)：
tuple[float, float, float]: 经纬度高度坐标 (纬度, 经度, 高度)，纬度经度单位为度，高度单位为米
异常 (Raises)：
- 无
lla2ned(lla, lla0)¶
功能说明：将经纬度高度坐标（LLA）转换为以参考点为原点的北东地坐标（NED） 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
lla |
list[float]/tuple[float] |
是 | - | 目标点经纬度高度，格式为 [纬度, 经度, 高度]，纬度经度单位为度，高度单位为米 |
lla0 |
list[float]/tuple[float] |
是 | - | 参考点经纬度高度，格式为 [纬度, 经度, 高度]，纬度经度单位为度，高度单位为米 |
返回值 (Returns)：
list[float]: 北东地坐标系下的 [北, 东, 地] 坐标，单位为米
异常 (Raises)：
- 无
ned2lla(ned, lla0)¶
功能说明：将以参考点为原点的北东地坐标（NED）转换为经纬度高度坐标（LLA） 参数列表 (Args)：
| 参数名 | 类型 | 是否必填 | 默认值 | 说明 |
|---|---|---|---|---|
ned |
list[float]/tuple[float] |
是 | - | 目标点北东地坐标，格式为 [北, 东, 地]，单位为米 |
lla0 |
list[float]/tuple[float] |
是 | - | 参考点经纬度高度，格式为 [纬度, 经度, 高度]，纬度经度单位为度，高度单位为米 |
返回值 (Returns)：
list[float]: 经纬度高度坐标 [纬度, 经度, 高度]，纬度经度单位为度，高度单位为米
异常 (Raises)：
- 无
示例：
from RflySimSDK.ctrl import EarthModel
# 初始化地球模型
em = EarthModel()
# 参考点经纬度高度（北京某点）
ref_lat, ref_lon, ref_h = 39.9087, 116.4021, 50
# 目标点经纬度高度
tar_lat, tar_lon, tar_h = 39.9097, 116.4031, 100
# LLA转ENU坐标
enu = em.lla2enu(tar_lat, tar_lon, tar_h, ref_lat, ref_lon, ref_h)
print(f"ENU坐标: {enu}")
# ENU转回LLA坐标
lla = em.enu2lla(enu[0], enu[1], enu[2], ref_lat, ref_lon, ref_h)
print(f"LLA坐标: {lla}")
进阶用法示例¶
展示复杂组合场景（如多类协作、异步控制、批量操作）
本示例展示批量转换多架无人机的位置坐标，实现多机任务规划场景下的不同坐标系批量转换处理：首先导入RflySimSDK的坐标转换模块，同时读取预先存储的多架无人机的GPS经纬度高度数组，批量将LLA坐标转换为相对于起飞点的ENU东北天坐标，再将转换后的ENU坐标反向转换回LLA坐标验证转换精度，满足多机协同任务中坐标统一计算的需求。
import numpy as np
from RflySimSDK.ctrl.EarthModel import EarthModel
# 初始化地球模型参数
em = EarthModel()
# 模拟5架无人机的LLA经纬度高度测量值 (纬度, 经度, 高度)
multi_uav_lla = np.array([
[31.2304, 121.4737, 100],
[31.2310, 121.4745, 120],
[31.2298, 121.4729, 90],
[31.2308, 121.4752, 110],
[31.2295, 121.4720, 85]
])
# 设定第一架无人机为原点，批量转换所有无人机LLA坐标到ENU坐标
origin_lla = multi_uav_lla[0]
multi_uav_enu = em.lla2enu(multi_uav_lla, origin_lla)
# 反向转换验证转换精度，得到还原后的LLA坐标
restore_lla = em.enu2lla(multi_uav_enu, origin_lla)
# 输出转换误差
print(f"LLA坐标还原最大误差: {np.max(np.abs(multi_uav_lla - restore_lla)):.8f}")
注意事项与避坑指南¶
- 坐标单位规范：输入LLA坐标时，纬度、经度的单位为度，不可直接输入弧度值，否则会产生上千公里的坐标偏移。
- 数组输入维度要求：批量转换坐标时，输入数组需要保持每一行对应一组坐标，若为一维单组坐标会自动补维度处理，自定义批量输入时需要注意维度对齐。
- 原点参数必填要求：LLA与ENU、NED坐标系互相转换时，必须传入原点坐标参数，不设置原点会触发参数缺失错误。
- 椭球参数默认说明：EarthModel默认采用WGS84椭球参数，若任务需要使用其他椭球模型，需要重新初始化修改对应参数后再进行转换。
更新日志¶
2024-06-18: fix：API格式调整2024-06-13: fix:更新例程索引2024-06-12: fix:更新接口注释2024-06-12: fix：更新earthmodel.py的接口注释2024-06-11: fix:更新接口注释2023-11-09: feat: 增加云兼容接口2023-10-24: feat: 修复部分bug2023-10-23: fix: 从PX4MavCtrlV4.py中剔除掉UE4控制相关代码，增加EarthModel.py文件