简介Matlab经纬度、东北天与地心直角坐标系转换代码是一套面向测绘、导航与定位领域的实用型坐标换算工具。适用于飞行器、车辆、舰船等运动平台的东北天坐标表示以及卫星定位结果与地心、站心坐标系之间的变换同时适合相关专业学生对照学习坐标框架原理。压缩包整体仅3KB包含6个文件其中5个m脚本分别实现经纬度转东北天、经纬度转地心直角坐标、地心直角坐标转东北天、地心直角坐标转经纬度四类操作另有1个txt说明辅助理解调用方式无第三方依赖下载后即可加入Matlab路径运行。函数命名直观、接口简洁逻辑清晰且运行高效既可作为独立工具快速解决坐标换算问题也能嵌入到组合导航、无人机定位等项目中复用。目前已有224人学习下载对于需要校验转换算法或构建坐标变换模块的工程师和研究者是一份值得收藏的小型代码包。1. 为什么导航定位总绕不开经纬度到东北天的转换做组合导航、无人机起降引导或者车辆高精度定位时最常遇到的一个问题就是GPS 给的是 WGS-84 经纬度和椭球高而你的控制算法、惯导解算或者里程计积分用的是东北天ENU直角坐标。经纬度是球面上的角度量直接拿去做差分、滤波、路径规划会遇到两个麻烦一是角度单位在赤道和两极对应的物理距离不一样二是姿态解算和航迹推算天然工作在直角坐标里。把经纬度转到东北天坐标系本质上是先在椭球面上做一次大地坐标到地心地固ECEF坐标的换算再把参考点处的地球切平面拉直得到一个以“东-北-天”为轴的局部笛卡尔坐标系。这个转换不复杂但到处是坑长半轴用错、参考点高程没设、旋转矩阵方向弄反都会让几百米外目标偏出去几十米。这篇文章按我的工程习惯把从公式到 MATLAB 代码、再到参数标定和验证的完整链路写清楚。2. 东北天坐标系定义与 WGS-84 椭球参数2.1 从经纬高到 ECEF 的数学基础在动手写代码之前先把坐标系之间的关系理清楚。WGS-84 坐标系下一个点的位置用大地纬度 B、大地经度 L 和椭球高 H 表示。这里的纬度是过该点的椭球法线与赤道面的夹角经度是起始子午面到该点子午面的夹角椭球高是沿法线方向到椭球面的距离。注意它不是海拔高度海拔高度是到大地水准面的距离两者之间隔着高程异常普通工程可以忽略精密工程必须在数据源头约定好。ECEF 坐标系的原点在地心X 轴指向本初子午线与赤道的交点Y 轴按右手定则指向东经 90 度方向Z 轴指向北极。从经纬高转 ECEF 的公式是教科书级的标准形式核心是先算出椭球卯酉圈曲率半径 NN a / sqrt(1 - e^2 * sin^2(B)) X (N H) * cos(B) * cos(L) Y (N H) * cos(B) * sin(L) Z (N * (1 - e^2) H) * sin(B)其中 a 是椭球长半轴e 是第一偏心率。这里最容易错的地方是忘记把角度从度换成弧度以及把纬度当经度代入。MATLAB 的三角函数默认接受弧度所以deg2rad这一步省不得。WGS-84 椭球的参数应该硬编码成常量而不是每次重新计算。长半轴 a 6378137.0 米扁率 f 1/298.257223563第一偏心率 e^2 f * (2 - f)约等于 6.69437999014e-3。有的老代码里用的是克拉索夫斯基椭球a 6378245那个是北京 54 坐标系的东西和 GPS 的 WGS-84 混用会引入百米级误差。建议代码里加一行注释说明参数来源避免半年后自己回来改代码时还要重新查资料。2.2 站心坐标系与坐标轴取向约定ECEF 是全局坐标系而东北天是局部坐标系。所谓站心坐标系就是把参考点当作“站在地面上的人”的局部坐标系X 轴指向东EY 轴指向北NZ 轴指向天顶U。这个坐标系的特点是距离参考点越远地球曲率的影响越大所以它只适合描述参考点附近的相对位置。一般工程经验是几十公里范围内用东北天坐标做差分和路径规划误差可以接受上千公里的场景必须回到 ECEF 或者用大地线解算。东北天和北东地NED是两个容易混淆的坐标系。NED 是 Z 轴向下指向地心常用于飞行器机体坐标系和惯性导航ENU 是 Z 轴向上常用于地面雷达站和测量学。同一个向量在 ENU 和 NED 里的坐标X/Y 互换且 Z 取反。这个细节在对接不同厂商的 SDK 时特别容易出事我在项目里就碰到过惯导输出的是 NED 而定位模块输出的是 ENU结果融合出来的轨迹在垂直方向上是反的排查了大半天才发现是坐标系约定不一致。建议代码里统一用 ENU接口处做显式转换不要依赖调用方的“默认约定”。2.3 旋转矩阵的推导与方向校验从 ECEF 转到东北天不需要先转到经纬度再转东北天直接用参考点的经纬度构造旋转矩阵即可。设参考点纬度为 B0、经度为 L0从 ECEF 到 ENU 的旋转矩阵 R 为R [ -sin(L0) cos(L0) 0 -sin(B0)*cos(L0) -sin(B0)*sin(L0) cos(B0) cos(B0)*cos(L0) cos(B0)*sin(L0) sin(B0) ]这个矩阵的每一行分别对应东、北、天方向的基向量在 ECEF 坐标系中的表达。比如东方向向量是(-sin(L0), cos(L0), 0)它在赤道面上垂直于参考点的经线方向。北方向向量是(-sin(B0)*cos(L0), -sin(B0)*sin(L0), cos(B0))它在子午面内垂直于法线。天方向就是参考点的法线方向。怎么验证矩阵对不对两个办法。第一个是看 ECEF 参考点本身转出来的东北天坐标应该是 (0,0,0)因为旋转矩阵把 ECEF 的原点平移量抵消后参考点在自身站心系里的坐标当然为零。第二个是取一个特殊点如果待转换点位于参考点正东方 1 公里处那么它的 ENU 坐标应该是 (1000, 0, 0) 左右北向分量接近零。这个校验方法后面写测试用例时会直接用到。3. MATLAB 实现函数封装与批量转换3.1 核心转换函数输入输出设计与防呆我一般把转换封装成两个函数一个处理单个点一个处理点集。MATLAB 的向量化特性使得批量转换非常方便没必要在主代码里写 for 循环遍历每一个点。先看单个点的版本function [E, N, U] geodetic2enu(lat, lon, alt, lat0, lon0, alt0) % GEODETIC2ENU 将 WGS-84 经纬高转换为东北天坐标 % 输入: % lat, lon, alt - 待转换点的纬度(度)、经度(度)、椭球高(米) % lat0, lon0, alt0 - 参考点(站心原点)的纬度、经度、椭球高 % 输出: % E, N, U - 东向、北向、天向坐标(米) % WGS-84 椭球参数 a 6378137.0; f 1/298.257223563; e2 f * (2 - f); % 角度转弧度 lat_r deg2rad(lat); lon_r deg2rad(lon); lat0_r deg2rad(lat0); lon0_r deg2rad(lon0); % 待转换点到 ECEF N_phi a / sqrt(1 - e2 * sin(lat_r)^2); X (N_phi alt) * cos(lat_r) * cos(lon_r); Y (N_phi alt) * cos(lat_r) * sin(lon_r); Z (N_phi * (1 - e2) alt) * sin(lat_r); % 参考点到 ECEF N_phi0 a / sqrt(1 - e2 * sin(lat0_r)^2); X0 (N_phi0 alt0) * cos(lat0_r) * cos(lon0_r); Y0 (N_phi0 alt0) * cos(lat0_r) * sin(lon0_r); Z0 (N_phi0 * (1 - e2) alt0) * sin(lat0_r); % ECEF 差值 dX X - X0; dY Y - Y0; dZ Z - Z0; % 旋转矩阵(ECEF - ENU) E -sin(lon0_r) * dX cos(lon0_r) * dY; N -sin(lat0_r) * cos(lon0_r) * dX - sin(lat0_r) * sin(lon0_r) * dY cos(lat0_r) * dZ; U cos(lat0_r) * cos(lon0_r) * dX cos(lat0_r) * sin(lon0_r) * dY sin(lat0_r) * dZ; end这个函数把经纬高到 ECEF、再经旋转到 ENU 的过程一步做完。逻辑上主要分三段先算待转换点的 ECEF再算参考点的 ECEF最后做坐标差并旋转。注意事项有几点前两个 ECEF 的代码段可以提取成一个子函数避免重复我这里保持了平铺写法是为了让每一步都直观可读最基础的防呆检查是纬度范围在 -90 到 90 度、经度在 -180 到 180 度之间超出范围应该直接报错不要静默处理否则算出来的坐标没有意义。3.2 批量转换点集用向量化告别 for 循环实际数据处理中一组轨迹动辄上万行逐点调用上面那个函数会慢得没法忍。MATLAB 的向量化写法是把纬度、经度、高度全部传成列向量让三角函数和四则运算一次性处理整个数组。下面的函数通过是否存在行向量输入来判断是否需要广播同时用height参数统一海拔单位function enu geodetic2enu_vector(lat, lon, alt, lat0, lon0, alt0) % GEODETIC2ENU_VECTOR 批量转换经纬高到东北天坐标 % 输入为列向量, 输出为 Nx3 矩阵, 三列分别为 E, N, U % 确保输入为列向量 lat lat(:); lon lon(:); alt alt(:); % 参数校验 assert(length(lat) length(lon) length(lat) length(alt), ... 纬度、经度、高度必须等长); a 6378137.0; f 1/298.257223563; % 参考点参数 lat0_r deg2rad(lat0); lon0_r deg2rad(lon0); N0 a / sqrt(1 - (f*(2-f)) * sin(lat0_r)^2); X0 (N0 alt0) * cos(lat0_r) * cos(lon0_r); Y0 (N0 alt0) * cos(lat0_r) * sin(lon0_r); Z0 (N0 * (1 - f*(2-f)) alt0) * sin(lat0_r); % 批量转 ECEF lat_r deg2rad(lat); lon_r deg2rad(lon); N_phi a ./ sqrt(1 - (f*(2-f)) * sin(lat_r).^2); X (N_phi alt) .* cos(lat_r) .* cos(lon_r); Y (N_phi alt) .* cos(lat_r) .* sin(lon_r); Z (N_phi .* (1 - f*(2-f)) alt) .* sin(lat_r); % 向量化旋转矩阵计算 dX X - X0; dY Y - Y0; dZ Z - Z0; E -sin(lon0_r) .* dX cos(lon0_r) .* dY; N -sin(lat0_r) .* cos(lon0_r) .* dX - sin(lat0_r) .* sin(lon0_r) .* dY cos(lat0_r) .* dZ; U cos(lat0_r) .* cos(lon0_r) .* dX cos(lat0_r) .* sin(lon0_r) .* dY sin(lat0_r) .* dZ; enu [E, N, U]; end注意这里用./和.*的地方是 MATLAB 数组运算的语法细节。sqrt和sin这类函数天然支持数组输入不会破坏向量化。X - X0用到的是 MATLAB 的标量扩展机制X0 是常数而 X 是向量结果仍是逐元素相减。这段代码在 10 万行轨迹输入时性能比 for 循环快两个数量级以上对于处理车载或无人机采集的原始定位数据够用了。3.3 置一个测试用例验证转换正确性代码写出来不验证等于白写。这里提供一个可重复的测试方法给定一个已知的参考点和待转换点手算或者用可靠在线工具得到东北天坐标再和函数输出做比较。用以下测试数据参数参考点待转换点纬度30.0°N30.001°N经度120.0°E120.0°E椭球高0 m0 m纬度差 0.001 度约等于 111 米。因为只有纬度变化预期结果是北向偏移约 111 米东向为零。运行转换函数后用norm检查误差。若误差超过 0.1 米优先检查椭球参数和矩阵是否抄错。用这个简单用例可以快速定位问题——如果连纯纬度变化都算不对那旋转矩阵的方向十有八九是反了。4. 实际工程中的参数配置与常见误差来源4.1 参考点选择对转换精度的影响东北天坐标系是局部坐标系它的精度和适用范围直接取决于参考点的选取。参考点选得越接近实际作业区域转换误差越小。道理很好理解东北天是参考点处地球切平面的近似离参考点越远切平面和真实椭球面的偏离越大。这种偏离带来的误差并不是线性的粗略估计时纬度方向每 100 公里偏离约 1 公里但实际取决于方向和高差。工程上选择参考点的通行做法是取作业区域中心而不是取起点。比如无人机巡检一条 10 公里的输电线路把参考点选在线路几何中心两端误差对称分布整体精度比选在起点高一倍。另外如果数据是流动站历史轨迹参考点的椭球高也要选对。有些实现把 alt0 设为零这就等于假设参考点在海平面上若实际地形海拔是 1000 米天向坐标会整体偏移 1000 米东向和北向坐标也会因为法线方向轻微变化而产生分米级误差。4.2 精度损失、单位混用与弧度陷阱精度问题要从两个方面看一是经纬度数据的原始精度二是转换过程中的数值稳定性。GPS 单点定位的经纬度小数位通常只到 6 位对应约 0.1 米分辨率RTK 可以到 9 位以上。如果输入数据的精度本身只有米级转换代码再精确也没有意义。在 MATLAB 里默认 double 精度处理 6378137 米级别的 ECEF 坐标和 1 米级的差值混合运算时会有约 1e-9 的相对误差也就是亚微米级对任何工程应用都足够。最容易翻车的还是单位混用。经纬度输入一定要明确是十进制度还是度分秒两者差一个 60 倍的系数。另一个坑是把度分秒字符串解析成小数时分和秒的进制是 60 而不是 100。这类 bug 的特点是转换结果看起来大致合理但误差大到离谱尤其在纬度方向表现明显因为纬度 1 秒对应约 30 米一个解析错误就能让轨迹整体漂出几公里。4.3 和 Python 高德坐标转换的差异对比热词里有个搜索是“python 将gps经纬度转换为高德经纬度”做的其实是坐标系基准转换出发点是不同的。GPS 用的是 WGS-84高德地图用的是 GCJ-02火星坐标这属于地球椭球体偏移问题和东北天转换解决的是两个层次的问题。GCJ-02 加了一个非线性偏移是为了让国内地图坐标和真实经纬度错开而东北天转换是数学上的刚性旋转和平移不改变椭球基准。两者可以串联使用先把 WGS-84 经纬度转成 GCJ-02 用于地图显示再把 GCJ-02 或者原始 WGS-84 转成 ECEF/ENU 用于导航解算。严禁混用否则地图上的位置和局部坐标会互相矛盾。4.4 MATLAB 版本兼容性与运行效率考量MATLAB 的代码大体上各个版本通用但有几个细节需要注意。R2016b 之前和之后的隐式扩展行为不同老版本里X - X0如果 X 是向量而 X0 是标量没问题但如果是两个不同形状的数组就会报错。建议写代码时用显式的.*和./老版本跑不了就加一行X0 repmat(X0, size(X))做兼容。此外在频繁调用的转换场景里把函数转成codegen支持的格式可以生成 C 代码用于嵌入式平台这部分迁移时要把assert改成运行时错误处理因为生成代码里assert行为会有差异。还有一个效率优化技巧如果参考点固定不变预计算该点的sin(lat0_r)、cos(lat0_r)、sin(lon0_r)、cos(lon0_r)以及 ECEF 坐标存成结构体或全局变量避免每帧重复计算。对于实时性要求高的场景比如无人机飞控里每秒执行 50 次的坐标转换这个优化能减少约 40% 的运算量。推荐把参数打包成 struct字段含义单位ref_ecef参考点 ECEF 坐标米sin_lat0 / cos_lat0参考点纬度三角函数值-sin_lon0 / cos_lon0参考点经度三角函数值-这样转换函数本体只需要做减法加矩阵乘没有任何三角函数调用速度和稳定性都会好很多。5. 逆变换与实时系统里的应用技巧5.1 东北天到经纬度的反向回算很多场景需要逆变换控制算法计算出目标点在东北天系下的坐标要回推它的经纬度用于上报或者驱动云台指向。逆变换的思路是先用旋转矩阵的转置把 ENU 转回 ECEF 差值再加回参考点的 ECEF然后用迭代法求经纬度。因为 R 是正交矩阵R就是它的逆。ECEF 转经纬度的迭代公式通常两步收敛适合实时系统function [lat, lon, alt] enu2geodetic(E, N, U, lat0, lon0, alt0) % ENU2GEODETIC 东北天坐标转 WGS-84 经纬高 a 6378137.0; f 1/298.257223563; e2 f * (2 - f); lat0_r deg2rad(lat0); lon0_r deg2rad(lon0); N0 a / sqrt(1 - e2 * sin(lat0_r)^2); X0 (N0 alt0) * cos(lat0_r) * cos(lon0_r); Y0 (N0 alt0) * cos(lat0_r) * sin(lon0_r); Z0 (N0 * (1 - e2) alt0) * sin(lat0_r); % ENU 到 ECEF 差值的转置旋转 dX -sin(lon0_r) * E - sin(lat0_r) * cos(lon0_r) * N cos(lat0_r) * cos(lon0_r) * U; dY cos(lon0_r) * E - sin(lat0_r) * sin(lon0_r) * N cos(lat0_r) * sin(lon0_r) * U; dZ cos(lat0_r) * N sin(lat0_r) * U; X X0 dX; Y Y0 dY; Z Z0 dZ; % ECEF 到经纬高的迭代解算 lon atan2(Y, X); p sqrt(X^2 Y^2); lat atan2(Z, p * (1 - e2)); for k 1:5 N_phi a / sqrt(1 - e2 * sin(lat)^2); alt p / cos(lat) - N_phi; lat atan2(Z, p * (1 - e2 * N_phi / (N_phi alt))); end lat rad2deg(lat); lon rad2deg(lon); end这个逆变换的经纬度输出可以直接和原始 GPS 数据做环路测试先正转再逆转比较输入输出差异。整个过程有一点必须格外注意当目标点在参考点正下方或者极区附近时cos(B)趋近于零p也趋近于零迭代公式会退化。工程上这种情况建议直接报错或返回 NaN而不是给一个错误坐标。5.2 在 Simulink 和 C 代码生成环境中的落地方式把转换代码集成到 Simulink 模型时不要直接用 MATLAB Function 块因为每次仿真都要解释执行效率很差。推荐的做法是把转换函数做成 MATLAB Function 后开启代码生成或者直接用 Interpreted MATLAB Function 块在验证阶段替代最终在代码生成阶段换成 S-Function。另外要注意 Simulink 的 MATLAB Function 块默认输入是 double 类型如果上游是定点数需要在函数入口做一次double()转换并明确数据精度。代码生成环境下assert不能被正确处理。在 C 代码生成时建议把参数校验改成返回状态标志function [E, N, U, valid] geodetic2enu_c(lat, lon, alt, ref) % 代码生成版本, 通过 valid 标志报告输入是否合法 valid (lat -90 lat 90 lon -180 lon 180); if ~valid E NaN; N NaN; U NaN; return; end % 转换逻辑与之前一致 end这样在嵌入式环境中调用方可以自己决定是丢弃该点还是保持上一帧状态不会因为一个异常数据把整个控制循环打崩。实时性方面单个点的转换在当代嵌入式 CPU 上的耗时大约是几百纳秒到一两微秒因为全部是加减乘除和开方没有超越函数的连续调用完全满足无人机或自动驾驶 100 Hz 甚至 1000 Hz 的控制频率需求。5.3 批量轨迹数据后处理与可视化技巧外业采集的数据经常要后处理成轨迹图转换完成后直接plot3(E, N, U)就能获得正确的相对位置关系。这里有个实用技巧作图前先把 ENU 坐标减去起始点坐标把轨迹平移到原点附近一方面避免坐标数值过大导致浮点精度问题另一方面图和车辆的局部运动直观对应。如果轨迹里混有异常跳点可以在转换后做一个简单的滤波d diff([E, N, U]); speed vecnorm(d, 2, 2) / dt; valid speed max_speed; % max_speed 根据载体类型设定这套过滤逻辑是坐标转换之外的数据质量控制能防止个别错误定位点污染整条轨迹的展示和分析。对于多趟重复采集的数据以同一参考点转换后可以直接做误差统计比如计算往返轨迹的重合度这在测绘无人机质检和车道级地图采集里是标配操作。本文还有配套的精品资源点击获取