LPC：Localization 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）

两套方向约定：

• ENU：E 东，N 北，U 天（向上）——Waymo、GTSAM、大多数自动驾驶定位后端
• NED：N 北，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-e^2\sin2\phi}})

a=6378137.0半长轴(m)f=1/298.257223563扁率e2=2f−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第一偏心率平方​

卯酉圈半径：
RN=a1−e2sin⁡2ϕ R_N = \dfrac{a}{\sqrt{1-e^2\sin^2\phi}}RN​=1−e2sin2ϕ​a​

方法一：标准完整变换（工业 / 自动驾驶首选，Waymo/GTSAM 底层实现）

步骤：

1. LLH → ECEF（地心地固直角坐标）
2. 求基准点 (P_0) 的 ECEF：(X_0,Y_0,Z_0)
3. 矢量差：(\Delta X = X-X_0,\Delta Y=Y-Y_0,\Delta Z=Z-Z_0)
4. 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-e^2)}{(1-e2\sin^2\phi_0){3/2}}) 子午圈半径
⚠️ 缺点：距离原点远了曲率误差明显，不能用于高精度激光 + IMU 融合

下面代码优先实现【方法一：标准 ECEF 中转版本】（工程主用），附带简易近似版本。

Python 实现（可直接运行，WGS84 LLH <-> ENU <-> NED）

import 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_iter=10): """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(f"ENU: E={e:.3f}, N={n:.3f}, U={u:.3f}") ned = Wgs84Ltp.enu2ned(e,n,u) print(f"NED: 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 + cpp，ROS/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 i=0; i<iter; 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)

两种转换方法对比总结（工程选型）

表格

补充工程实操要点（和你之前定位链路打通）

1. 原点选择策略
   • 固定原点：高精地图原点，整张地图一套 ENU；缺点：离原点几十公里曲率误差上来
   • 滑动原点（sliding LTP）：以当前车辆 GNSS 位置动态刷新 P0，Waymo/Cruise 滑动窗口定位后端方案，永远保持车辆在原点附近，曲率误差压到极小
2. 和 GTSAM 对接：GTSAM 自带Pose3、Point3就是 ENU 空间；把 GNSS、激光匹配、IMU 预积分全部放到 ENU 下构建因子
3. 坐标系转换顺序：

GNSS LLH → ENU（后端优化域） → LLH → UTM/GK（地图存储、可视化）