坐标系与变换：从WGS84到齐次矩阵

坐标系这个东西，说白了就是给空间里的点一个「身份证」。做轨迹分析这些年，我见过太多因为坐标系搞混而翻车的案例。有一次，一个同事把WGS84的经纬度直接当笛卡尔坐标算距离，结果算出来的偏差能绕地球半圈——嗯，这故事我后面细说。

一、三大常用坐标系

做轨迹分析，你绕不开这三个坐标系。我个人的习惯是，拿到数据第一件事，先确认它用的是哪个坐标系。

1. WGS84（世界大地坐标系）

这是GPS用的坐标系。它用经纬度+海拔来描述位置。说白了，就是在地球这个椭球上画格子。

  • 经度（Longitude）：-180° 到 180°，东西方向
  • 纬度（Latitude）：-90° 到 90°，南北方向
  • 海拔（Altitude）：相对于参考椭球面的高度
⚠️ 我曾经踩过的坑：WGS84的经纬度不是平面坐标！直接用经纬度算欧氏距离，在赤道附近误差还能忍，到了高纬度地区，那误差能让你怀疑人生。我有个项目在北极圈附近做轨迹分析，一开始没做投影转换，结果轨迹点全飘了。

2. ENU（东北天坐标系）

ENU是一种局部坐标系。你想想看，你在某个位置站定，东方向是X轴，北方向是Y轴，天方向是Z轴。这就是ENU。

  • 适合小范围（几十公里内）的轨迹分析
  • 原点通常选在某个参考点（比如基站、起点）
  • 单位是米，计算距离和方向非常直观

我建议做无人机轨迹分析时，优先用ENU。因为无人机飞控输出的往往是ENU坐标，省去一次转换。

3. 笛卡尔坐标系（直角坐标系）

这个大家最熟悉。X、Y、Z三个轴互相垂直。在轨迹分析中，笛卡尔坐标系通常用于：

  • 机器人SLAM中的局部地图
  • 三维重建的点云数据
  • 惯性导航系统的积分计算

但要注意：笛卡尔坐标系假设空间是平的。地球曲率在长距离下不能忽略。

二、坐标转换方法

坐标转换，说白了就是给点「换身份证」。我把它分成两类：

1. WGS84 ↔ ENU 转换

这是轨迹分析中最常见的转换。步骤很简单：

  1. 先把WGS84（经纬高）转成地心地固坐标系（ECEF）
  2. 再从ECEF转到以参考点为原点的ENU

代码实现如下（Python）：

import math

def wgs84_to_enu(lat, lon, alt, ref_lat, ref_lon, ref_alt):
    # 1. WGS84转ECEF
    a = 6378137.0  # 地球长半轴
    e2 = 0.00669437999014  # 第一偏心率平方
    
    lat_rad = math.radians(lat)
    lon_rad = math.radians(lon)
    ref_lat_rad = math.radians(ref_lat)
    ref_lon_rad = math.radians(ref_lon)
    
    N = a / math.sqrt(1 - e2 * math.sin(lat_rad)**2)
    x = (N + alt) * math.cos(lat_rad) * math.cos(lon_rad)
    y = (N + alt) * math.cos(lat_rad) * math.sin(lon_rad)
    z = (N * (1 - e2) + alt) * math.sin(lat_rad)
    
    # 参考点同样转ECEF
    N_ref = a / math.sqrt(1 - e2 * math.sin(ref_lat_rad)**2)
    x_ref = (N_ref + ref_alt) * math.cos(ref_lat_rad) * math.cos(ref_lon_rad)
    y_ref = (N_ref + ref_alt) * math.cos(ref_lat_rad) * math.sin(ref_lon_rad)
    z_ref = (N_ref * (1 - e2) + ref_alt) * math.sin(ref_lat_rad)
    
    # 2. ECEF转ENU
    dx = x - x_ref
    dy = y - y_ref
    dz = z - z_ref
    
    e = -math.sin(ref_lon_rad) * dx + math.cos(ref_lon_rad) * dy
    n = (-math.sin(ref_lat_rad) * math.cos(ref_lon_rad) * dx 
         - math.sin(ref_lat_rad) * math.sin(ref_lon_rad) * dy 
         + math.cos(ref_lat_rad) * dz)
    u = (math.cos(ref_lat_rad) * math.cos(ref_lon_rad) * dx 
         + math.cos(ref_lat_rad) * math.sin(ref_lon_rad) * dy 
         + math.sin(ref_lat_rad) * dz)
    
    return e, n, u
💡 我的经验：参考点的选择很关键。我一般选轨迹的起点或者区域中心。如果参考点选得太远，ENU的「平面假设」误差会变大。

2. 笛卡尔坐标系之间的变换

两个笛卡尔坐标系之间的变换，无非就是旋转+平移。用齐次变换矩阵可以统一表达。

三、齐次变换矩阵

齐次变换矩阵，说白了就是把旋转和平移塞到一个4x4的矩阵里。为什么用4维？因为3x3矩阵只能做旋转，加一个维度才能表达平移。

结构如下：

| R   t |
| 0   1 |

其中R是3x3旋转矩阵，t是3x1平移向量。

举个例子，绕Z轴旋转θ角度，再平移(dx, dy, dz)：

| cosθ  -sinθ  0  dx |
| sinθ   cosθ  0  dy |
| 0      0     1  dz |
| 0      0     0  1  |

使用齐次矩阵的好处是：多个变换可以连乘。比如从坐标系A到B，再到C，只需要把矩阵乘起来：

T_A_to_C = T_B_to_C * T_A_to_B

🔑 核心要点：齐次变换矩阵是轨迹分析中「串联」不同传感器数据的桥梁。我在做多传感器融合时，IMU、GPS、激光雷达各自有自己的坐标系，全靠齐次矩阵把它们对齐到同一个参考系。

四、知识体系总览

下面这张图，是我梳理的坐标系与变换的知识结构。你想想看，从全局到局部，从球面到平面，每一步都有对应的数学工具。

坐标系与变换知识体系 WGS84 经纬高（全球） ENU 东北天（局部） 笛卡尔 XYZ（平面） 坐标转换方法 WGS84 ↔ ECEF ↔ ENU 笛卡尔 ↔ 笛卡尔（旋转+平移） 齐次变换矩阵（4×4） 旋转矩阵 R + 平移向量 t 应用：轨迹对齐 · 多传感器融合 · 地图构建 —— 资深算法工程师整理

五、避坑指南与个人经验

⚠️ 我曾经犯过的错：

  • 有一次做车载轨迹分析，直接把WGS84的经纬度当成笛卡尔坐标做卡尔曼滤波。结果滤波后的轨迹直接飞到了大西洋。后来才发现，经纬度是角度，不是长度！必须先投影到平面。
  • 还有一次，齐次变换矩阵的乘法顺序搞反了。记住：矩阵乘法不满足交换律。先旋转后平移，和先平移后旋转，结果完全不同。

💡 我的建议：

  • 做轨迹分析前，先画一个坐标系关系图。把每个传感器的坐标系、参考系、目标系都标清楚。
  • 写代码时，每个坐标值都带上单位（米/度/弧度），减少低级错误。
  • 验证转换是否正确：找一个已知点，手动算一遍，跟代码结果对比。

嗯，坐标系与变换这部分，说白了就是轨迹分析的「地基」。地基没打好，上面的算法再漂亮也是空中楼阁。我见过太多项目，最后排查问题发现是坐标系搞错了——这种错误最冤，也最浪费时间。

可可精品资料库

← 上一章 📖 返回目录 下一章 →