2. 水下定位导航中的坐标系体系
水下定位导航的所有计算，最终都要落到“某个点在某个坐标系里怎么表示”这个问题上。同一台水下机器人、同一个声学信标，在不同坐标系中的坐标数值完全不同；如果坐标系定义、轴向约定或转换方向搞错，定位结果可能偏差几十米甚至方向完全相反。因此，在讨论任何定位算法之前，必须先建立一套清晰的坐标系体系，并明确它们之间的层级关系。
一、地球坐标系：全局基准
地球坐标系是全局的、不随载体运动的基准，用于描述载体在地球上的绝对位置。工程中最常用的是地心地固坐标系（ECEF）和大地坐标系。
ECEF（Earth-Centered, Earth-Fixed）：原点位于地球质心，Z 轴指向协议地球北极，X 轴指向本初子午线与赤道的交点，Y 轴按右手定则确定。ECEF 是直角坐标系，单位通常为米，任意一点用 (X, Y, Z) 表示。它的优点是三点确定一个直角坐标，做矢量运算、坐标旋转非常方便；缺点是数值不直观，人无法从 (X, Y, Z) 直接读出“在哪个海域”。
大地坐标系（Geodetic）：用经度 λ、纬度 φ、大地高 h 描述位置，基于 WGS-84 等参考椭球。它直观、便于与海图、GPS 输出对接，但经纬度不是等距量，直接做距离和角度运算需要先转换为局部平面坐标。实际系统中，GNSS 接收机输出经纬度，而导航解算多在 ECEF 或局部坐标系中完成，因此两者之间的正反算（经纬度↔ECEF）是必备的基础运算。
需要强调：ECEF 与大地坐标系描述的是同一个物理点，只是表达方式不同，二者之间是确定性的双向转换关系，不涉及任何近似假设（除椭球参数选择外）。
二、当地水平坐标系：局部作业基准
水下作业范围通常只有几百米到几公里，用 ECEF 做局部相对定位会带来大数值相减的精度损失，也不便于理解“东、北、上”。因此引入当地水平坐标系，原点取在某个参考点（如布放点、船载 GNSS 天线、水面浮标），坐标轴与当地水平面和对地垂线对齐。
常用的两种约定是 ENU 和 NED：
- ENU（East-North-Up）：X 轴指东，Y 轴指北，Z 轴指天（垂直向上）。这是测绘、机器人领域常见的约定，Z 向上为正，深度为负值。
- NED（North-East-Down）：X 轴指北，Y 轴指东，Z 轴指地（垂直向下）。这是航空航天与海洋工程中广泛使用的约定，深度为正值，与“下潜深度”这一物理量方向一致。
两者都是右手系，只是轴的排列和 Z 方向不同，可以通过固定旋转矩阵互相转换。选择哪一种没有对错，关键是全系统统一：如果导航解算用 NED，而显示界面按 ENU 解释，深度符号就会反号，操作员看到的“上浮”实际是“下潜”。在课程后续的坐标转换中，我们统一采用 NED 作为当地水平坐标系约定，深度向下为正。
三、载体坐标系与传感器坐标系
载体坐标系（Body Frame）：固连于水下机器人本体，原点通常取在载体重心或导航基准点，轴向按载体结构定义。常见约定为 X 轴指向载体艏向（前），Y 轴指向右舷，Z 轴指向下，构成右手系（前-右-下，FRD）。载体的姿态（横滚、俯仰、航向）就是载体坐标系相对当地水平坐标系的旋转关系。载体坐标系随载体运动而运动，因此它描述的是“相对载体自身”的量，例如前向速度、侧向速度、艏向角速度。
传感器坐标系（Sensor Frame）：每个传感器都有自己的安装位置和安装朝向。例如 DVL 的波束平面、IMU 的敏感轴、USBL 基阵的阵列平面、磁力计的敏感轴，都可能与载体坐标系存在安装偏差（安装角误差和安装位置偏移）。传感器原始观测值首先是在传感器坐标系中表达的，必须经过“传感器→载体”的外参标定转换，才能进入载体坐标系参与导航解算。
这里有一个容易被忽视的实践要点：传感器坐标系到载体坐标系的转换包含两部分——旋转（安装角）和平移（安装位置）。旋转影响方向量（速度、加速度、波束方向），平移影响位置量（尤其是载体旋转时，杠杆臂会引入附加速度）。只标定旋转而忽略杠杆臂，在转向机动时会产生可观测的速度误差。
四、层级关系与转换链
上述坐标系并非平级并列，而是构成一条从全局到局部、从局部到本体的层级链。下面的图示给出了各坐标系之间的层级与转换关系，以及每一级转换所依赖的关键参数。
从图中可以看出，转换链的方向是“全局→局部→本体→传感器”。实际导航解算中，数据流往往是反向的：传感器先给出本体坐标系下的观测（如 DVL 测得的前向/侧向速度），经姿态旋转到当地水平系，再积分得到局部位置，最后按需转换到大地坐标或 ECEF 输出。理解这条链，就能明白每一个转换环节需要什么参数、误差会从哪里引入。
五、一个具体的转换示例
假设某水下机器人布放点的大地坐标为东经 120.0000°、北纬 30.0000°、大地高 0 m，以该点为原点建立 NED 当地水平系。机器人当前在 NED 系中的位置为 (120.0 m, 80.0 m, 15.0 m)，即北向 120 m、东向 80 m、深度 15 m。若要把该位置换算到 ECEF，需要先将参考点经纬度转换为 ECEF 原点坐标，再按参考点纬度构造旋转矩阵，把 NED 位移旋转到 ECEF 轴向并叠加。这一过程在代码中通常封装为两个函数：geodetic_to_ecef() 与 ned_to_ecef()，前者只依赖椭球参数，后者还依赖参考点纬度与经度。
反过来，若已知某信标的 ECEF 坐标，要得到它在 NED 系中的位置，则先减去参考点 ECEF 坐标得到相对矢量，再乘以旋转矩阵的转置。这里必须注意：旋转矩阵是正交矩阵，其逆等于转置，但平移部分不能“取负了事”，必须先旋转再平移，顺序错了结果就错。
六、约定统一是工程底线
坐标系体系本身并不复杂，真正的风险在于混用。工程上建议在系统设计阶段就固定一张“坐标系约定表”，明确每个坐标系的轴向、单位、正方向、原点定义，以及每一级转换所用的参数来源。例如：深度在 NED 中向下为正，在 ENU 中向上为正；航向角从北向东为正，还是从北向西为正；姿态旋转顺序是 Z-Y-X 还是 X-Y-Z。这些约定一旦写入接口文档和代码注释，就能避免大量隐蔽错误。
综上，地球坐标系提供全局绝对基准，当地水平坐标系提供局部作业基准，载体坐标系描述本体运动，传感器坐标系描述原始观测。四者构成一条层级清晰的转换链，每一级转换都对应明确的物理参数。后续的声学定位解算、航位推算与组合导航，都是在这条链上展开的。