LPC_经纬海拔转XYZ

LPCLocalization Planning and Control输入GNSS 输出LLH(lat, lon, h)WGS84纬度 φ经度 λ大地高 h输出ENU东、北、天 / NED北、东、地很多无人机 / IMU 用 NED你说的西北地很少见行业标准是 NED变换链路标准(LLH \xrightarrow{椭球公式} ECEF(X,Y,Z) \xrightarrow{平移旋转矩阵} ENU)基准点(P_0(\phi_0,\lambda_0,h_0))也就是 ENU 原点。注意ENU/NED 不是地图投影是站心切平面坐标变换LTP两套方向约定ENUE 东N 北U 天向上——Waymo、GTSAM、大多数自动驾驶定位后端NEDN 北E 东D 地向下——PX4、飞控、很多 IMU 器件坐标系NED 和 ENU 只是坐标轴重排 U 取反[NED][01010000−1][ENU] \begin{bmatrix} N \\ E \\ D \end{bmatrix} \begin{bmatrix} 0 1 0 \\ 1 0 0 \\ 0 0 -1 \end{bmatrix} \begin{bmatrix} E \\ N \\ U \end{bmatrix}​NED​​​010​100​00−1​​​ENU​​前置WGS84 椭球常数(\begin{aligned}a 6378137.0 \quad \text{半长轴(m)} \f 1/298.257223563 \quad \text{扁率} \e^2 2f-f^2 \quad \text{第一偏心率平方}\end{aligned})卯酉圈半径 (R_N \dfrac{a}{\sqrt{1-e2\sin2\phi}})a6378137.0半长轴(m)f1/298.257223563扁率e22f−f2第一偏心率平方 \begin{aligned} a 6378137.0 \quad \text{半长轴(m)} \\ f 1/298.257223563 \quad \text{扁率} \\ e^2 2f-f^2 \quad \text{第一偏心率平方} \end{aligned}afe2​6378137.0半长轴(m)1/298.257223563扁率2f−f2第一偏心率平方​卯酉圈半径RNa1−e2sin⁡2ϕ R_N \dfrac{a}{\sqrt{1-e^2\sin^2\phi}}RN​1−e2sin2ϕ​a​方法一标准完整变换工业 / 自动驾驶首选Waymo/GTSAM 底层实现步骤LLH → ECEF地心地固直角坐标求基准点 (P_0) 的 ECEF(X_0,Y_0,Z_0)矢量差(\Delta X X-X_0,\Delta YY-Y_0,\Delta ZZ-Z_0)ECEF → ENU 旋转矩阵(R_{enu} \begin{bmatrix}-\sin\lambda_0 \cos\lambda_0 0 \-\sin\phi_0\cos\lambda_0 -\sin\phi_0\sin\lambda_0 \cos\phi_0 \\cos\phi_0\cos\lambda_0 \cos\phi_0\sin\lambda_0 \sin\phi_0\end{bmatrix})(\begin{bmatrix}E\N\U\end{bmatrix} R_{enu}\begin{bmatrix}\Delta X \ \Delta Y \ \Delta Z\end{bmatrix})所有角度计算弧度逆变换ENU → ECEF → LLH方法二局部近似公式简易微分公式小范围 1km适合低成本小车不经过 ECEF直接用经纬度差换算东、北偏移(\begin{aligned}\Delta E R_N \cos\phi_0 \cdot (\lambda-\lambda_0) \\Delta N R_M \cdot (\phi-\phi_0) \\Delta U h - h_0\end{aligned})(R_M\dfrac{a(1-e2)}{(1-e2\sin2\phi_0){3/2}}) 子午圈半径⚠️ 缺点距离原点远了曲率误差明显不能用于高精度激光 IMU 融合下面代码优先实现【方法一标准 ECEF 中转版本】工程主用附带简易近似版本。Python 实现可直接运行WGS84 LLH - ENU - NEDimport math class Wgs84Ltp: # WGS84 椭球参数 A 6378137.0 F 1.0 / 298.257223563 E2 2 * F - F * F staticmethod def llh2ecef(lat_deg, lon_deg, h): LLH(度,度,m) - ECEF X,Y,Z (m) lat math.radians(lat_deg) lon math.radians(lon_deg) sin_lat math.sin(lat) cos_lat math.cos(lat) sin_lon math.sin(lon) cos_lon math.cos(lon) rn Wgs84Ltp.A / math.sqrt(1.0 - Wgs84Ltp.E2 * sin_lat * sin_lat) x (rn h) * cos_lat * cos_lon y (rn h) * cos_lat * sin_lon z (rn * (1 - Wgs84Ltp.E2) h) * sin_lat return x, y, z staticmethod def ecef2llh(x, y, z, max_iter10): ECEF - LLH (lat_deg, lon_deg, h) lon math.atan2(y, x) p math.hypot(x, y) lat math.atan2(z, p * (1 - Wgs84Ltp.E2)) for _ in range(max_iter): sin_lat math.sin(lat) rn Wgs84Ltp.A / math.sqrt(1.0 - Wgs84Ltp.E2 * sin_lat * sin_lat) h p / math.cos(lat) - rn lat math.atan2(z, p * (1 - Wgs84Ltp.E2 * rn / (rn h))) return math.degrees(lat), math.degrees(lon), h staticmethod def ecef2enu(x, y, z, x0, y0, z0, lat0_deg, lon0_deg): ECEF to ENU, origin P0(lat0,lon0,h0) lat0 math.radians(lat0_deg) lon0 math.radians(lon0_deg) dX x - x0 dY y - y0 dZ z - z0 s_lon0 math.sin(lon0) c_lon0 math.cos(lon0) s_lat0 math.sin(lat0) c_lat0 math.cos(lat0) e -s_lon0 * dX c_lon0 * dY n -s_lat0 * c_lon0 * dX - s_lat0 * s_lon0 * dY c_lat0 * dZ u c_lat0 * c_lon0 * dX c_lat0 * s_lon0 * dY s_lat0 * dZ return e, n, u staticmethod def enu2ecef(e, n, u, x0, y0, z0, lat0_deg, lon0_deg): ENU back to ECEF lat0 math.radians(lat0_deg) lon0 math.radians(lon0_deg) s_lon0 math.sin(lon0) c_lon0 math.cos(lon0) s_lat0 math.sin(lat0) c_lat0 math.cos(lat0) dx -s_lon0 * e - s_lat0 * c_lon0 * n c_lat0 * c_lon0 * u dy c_lon0 * e - s_lat0 * s_lon0 * n c_lat0 * s_lon0 * u dz c_lat0 * n s_lat0 * u return x0 dx, y0 dy, z0 dz staticmethod def llh2enu(lat, lon, h, lat0, lon0, h0): LLH - ENU 主入口 x0, y0, z0 Wgs84Ltp.llh2ecef(lat0, lon0, h0) x, y, z Wgs84Ltp.llh2ecef(lat, lon, h) return Wgs84Ltp.ecef2enu(x, y, z, x0, y0, z0, lat0, lon0) staticmethod def enu2llh(e, n, u, lat0, lon0, h0): ENU - LLH x0, y0, z0 Wgs84Ltp.llh2ecef(lat0, lon0, h0) x, y, z Wgs84Ltp.enu2ecef(e, n, u, x0, y0, z0, lat0, lon0) return Wgs84Ltp.ecef2llh(x, y, z) staticmethod def enu2ned(e, n, u): ENU转NED return n, e, -u staticmethod def ned2enu(n, e, d): NED转ENU return e, n, -d staticmethod def llh2enu_approx(lat, lon, h, lat0, lon0, h0): 【简易近似版本 方法二】适合小范围1km lat0_r math.radians(lat0) rn Wgs84Ltp.A / math.sqrt(1.0 - Wgs84Ltp.E2 * math.sin(lat0_r)**2) rm Wgs84Ltp.A * (1 - Wgs84Ltp.E2) / ((1 - Wgs84Ltp.E2 * math.sin(lat0_r)**2)**1.5) dlon math.radians(lon - lon0) dlat math.radians(lat - lat0) e rn * math.cos(lat0_r) * dlon n rm * dlat u h - h0 return e, n, u if __name__ __main__: # 测试样例 # 基准原点 P0 lat0 34.76 lon0 113.65 h0 100.0 # 目标点 lat 34.761 lon 113.652 h 105.0 e,n,u Wgs84Ltp.llh2enu(lat, lon, h, lat0, lon0, h0) print(fENU: E{e:.3f}, N{n:.3f}, U{u:.3f}) ned Wgs84Ltp.enu2ned(e,n,u) print(fNED: N{ned[0]:.3f}, E{ned[1]:.3f}, D{ned[2]:.3f}) # 逆变换验证 lat2, lon2, h2 Wgs84Ltp.enu2llh(e,n,u, lat0, lon0, h0) print(f逆算LLH: lat{lat2:.6f}, lon{lon2:.6f}, h{h2:.3f}) # 简易近似版本对比 e_ap, n_ap, u_ap Wgs84Ltp.llh2enu_approx(lat, lon, h, lat0, lon0, h0) print(f近似ENU: E{e_ap:.3f}, N{n_ap:.3f}, U{u_ap:.3f})C 实现Header cppROS/GTSAM 工程直接嵌入无第三方依赖wgs84_ltp.h#pragma once #include cmath struct ENU { double E, N, U; }; struct NED { double N, E, D; }; struct ECEF { double X, Y, Z; }; struct LLH { double lat; // deg double lon; // deg double h; // m }; class Wgs84Ltp { public: static constexpr double A 6378137.0; static constexpr double F 1.0 / 298.257223563; static constexpr double E2 2 * F - F * F; static ECEF llh2ecef(const LLH llh); static LLH ecef2llh(const ECEF ecef, int iter 10); static ENU ecef2enu(const ECEF p, const ECEF origin_ecef, double lat0_deg, double lon0_deg); static ECEF enu2ecef(const ENU enu, const ECEF origin_ecef, double lat0_deg, double lon0_deg); static ENU llh2enu(const LLH point, const LLH origin); static LLH enu2llh(const ENU enu, const LLH origin); static NED enu2ned(const ENU enu); static ENU ned2enu(const NED ned); // 简易微分近似版本 static ENU llh2enu_approx(const LLH point, const LLH origin); };wgs84_ltp.cpp#include wgs84_ltp.h ECEF Wgs84Ltp::llh2ecef(const LLH llh) { double lat llh.lat * M_PI / 180.0; double lon llh.lon * M_PI / 180.0; double slat sin(lat); double clat cos(lat); double slon sin(lon); double clon cos(lon); double rn A / sqrt(1.0 - E2 * slat * slat); ECEF out{}; out.X (rn llh.h) * clat * clon; out.Y (rn llh.h) * clat * slon; out.Z (rn * (1 - E2) llh.h) * slat; return out; } LLH Wgs84Ltp::ecef2llh(const ECEF ecef, int iter) { LLH out{}; out.lon atan2(ecef.Y, ecef.X); double p hypot(ecef.X, ecef.Y); double lat atan2(ecef.Z, p * (1 - E2)); double h 0.0; for(int i0; iiter; i){ double slat sin(lat); double rn A / sqrt(1.0 - E2 * slat * slat); h p / cos(lat) - rn; lat atan2(ecef.Z, p * (1 - E2 * rn/(rn h))); } out.lat lat * 180.0 / M_PI; out.lon out.lon * 180.0 / M_PI; out.h h; return out; } ENU Wgs84Ltp::ecef2enu(const ECEF p, const ECEF o, double lat0_deg, double lon0_deg) { double lat0 lat0_deg * M_PI / 180.0; double lon0 lon0_deg * M_PI / 180.0; double dX p.X - o.X; double dY p.Y - o.Y; double dZ p.Z - o.Z; double slon0 sin(lon0); double clon0 cos(lon0); double slat0 sin(lat0); double clat0 cos(lat0); ENU enu{}; enu.E -slon0 * dX clon0 * dY; enu.N -slat0*clon0*dX - slat0*slon0*dY clat0*dZ; enu.U clat0*clon0*dX clat0*slon0*dY slat0*dZ; return enu; } ECEF Wgs84Ltp::enu2ecef(const ENU enu, const ECEF o, double lat0_deg, double lon0_deg) { double lat0 lat0_deg * M_PI / 180.0; double lon0 lon0_deg * M_PI / 180.0; double slon0 sin(lon0); double clon0 cos(lon0); double slat0 sin(lat0); double clat0 cos(lat0); double dX -slon0 * enu.E - slat0*clon0*enu.N clat0*clon0*enu.U; double dY clon0 * enu.E - slat0*slon0*enu.N clat0*slon0*enu.U; double dZ clat0 * enu.N slat0 * enu.U; ECEF out{}; out.X o.X dX; out.Y o.Y dY; out.Z o.Z dZ; return out; } ENU Wgs84Ltp::llh2enu(const LLH point, const LLH origin) { ECEF o_ecef llh2ecef(origin); ECEF p_ecef llh2ecef(point); return ecef2enu(p_ecef, o_ecef, origin.lat, origin.lon); } LLH Wgs84Ltp::enu2llh(const ENU enu, const LLH origin) { ECEF o_ecef llh2ecef(origin); ECEF p_ecef enu2ecef(enu, o_ecef, origin.lat, origin.lon); return ecef2llh(p_ecef); } NED Wgs84Ltp::enu2ned(const ENU enu) { return {enu.N, enu.E, -enu.U}; } ENU Wgs84Ltp::ned2enu(const NED ned) { return {ned.E, ned.N, -ned.D}; } ENU Wgs84Ltp::llh2enu_approx(const LLH point, const LLH origin) { double lat0_r origin.lat * M_PI / 180.0; double slat0 sin(lat0_r); double rn A / sqrt(1.0 - E2 * slat0 * slat0); double rm A*(1-E2) / pow(1.0 - E2 * slat0*slat0, 1.5); double dlon_r (point.lon - origin.lon) * M_PI / 180.0; double dlat_r (point.lat - origin.lat) * M_PI / 180.0; ENU enu{}; enu.E rn * cos(lat0_r) * dlon_r; enu.N rm * dlat_r; enu.U point.h - origin.h; return enu; }main.cpp 测试#include iostream #include wgs84_ltp.h int main(){ LLH origin{34.76, 113.65, 100.0}; LLH pt{34.761, 113.652, 105.0}; ENU enu Wgs84Ltp::llh2enu(pt, origin); NED ned Wgs84Ltp::enu2ned(enu); std::cout ENU E: enu.E N: enu.N U: enu.U \n; std::cout NED N: ned.N E: ned.E D: ned.D \n; LLH back Wgs84Ltp::enu2llh(enu, origin); std::cout back lat: back.lat lon: back.lon h: back.h \n; return 0; }CMakeLists.txt编译cmake_minimum_required(VERSION 3.16) project(ltp_demo) set(CMAKE_CXX_STANDARD 17) add_executable(demo main.cpp wgs84_ltp.cpp)两种转换方法对比总结工程选型表格方案原理精度适用场景后端适配你前面 GTSAM / 联邦滤波【标准 ECEF 中转】LLH→ECEF→ENU严格椭球几何旋转矩阵变换全范围高精度自动驾驶、GNSS 后处理、激光 ICP、IMU 预积分✅首选Waymo 同款【局部微分近似】直接 Δlat Δlon 算 EN一阶泰勒切平面近似忽略高阶曲率项1km 很好远了漂移低成本单片机、简易小车可视化粗算❌ 不建议放进因子图优化残差补充工程实操要点和你之前定位链路打通原点选择策略固定原点高精地图原点整张地图一套 ENU缺点离原点几十公里曲率误差上来滑动原点sliding LTP以当前车辆 GNSS 位置动态刷新 P0Waymo/Cruise 滑动窗口定位后端方案永远保持车辆在原点附近曲率误差压到极小和 GTSAM 对接GTSAM 自带Pose3、Point3就是 ENU 空间把 GNSS、激光匹配、IMU 预积分全部放到 ENU 下构建因子坐标系转换顺序GNSS LLH → ENU后端优化域 → LLH → UTM/GK地图存储、可视化