3. 坐标转换数学基础

水下定位的所有坐标转换，最终都落在两类数学对象上：向量描述“点在哪里、方向朝哪”，矩阵描述“如何把一个坐标系里的量搬到另一个坐标系”。本节不追求数学上的完备证明，而是把后续推导中真正会用到的运算规则、矩阵性质与参数关系讲清楚，使读者在看到旋转矩阵、方向余弦矩阵、齐次变换时，能判断它由什么构造、满足什么约束、对应哪种物理转动。

一、向量运算：坐标转换的“原子操作”

在三维空间中，一个点或一个方向用列向量表示，例如水声应答器在载体坐标系下的位置 p^b = [x, y, z]ᵀ。上标 b 表示该向量表达在载体坐标系（body）中，这一约定必须贯穿全文：同一个物理量在不同坐标系下数值不同，但物理对象不变。

转换中高频使用的向量运算有三类。其一是点积 a·b = |a||b|cosθ，它给出两向量夹角，是方向余弦矩阵元素的直接来源；其二是叉积 a×b，结果垂直于 a、b 所在平面，用于构造旋转轴、判断手性；其三是模长与单位化 a/|a|，把斜距向量变成单位方向向量。举例来说，若测得应答器相对载体的斜距 r = 120.0 m，方向单位向量 u^b = [0.6, 0.0, 0.8]ᵀ，则位置向量为 p^b = r·u^b = [72.0, 0.0, 96.0]ᵀ m。这一步是“距离+方向→坐标”的最小闭环，也是后续所有定位解算的起点。

二、旋转矩阵：性质决定了它能做什么

设 R 为 3×3 矩阵，把载体坐标系下的向量变换到导航坐标系（n系）：p^n = R_b^n · p^b。真正可用的旋转矩阵必须同时满足三条性质：

• 正交性：RᵀR = I，即 R⁻¹ = Rᵀ。逆变换不需要求逆，只需转置，这在实时解算中非常关键。
• 行列式为 +1：det(R) = +1。它保证变换是纯旋转而非镜像；若算出 −1，说明矩阵构造中出现了轴向或手性错误。
• 保长度、保夹角：|Rp| = |p|，(Ra)·(Rb) = a·b。因此旋转不改变斜距和方位夹角，只改变表达坐标系。

工程上常用这两条做自检：解算出的姿态矩阵若 RᵀR 与单位阵偏差超过 1e-6，或 det(R) 明显偏离 1，就应怀疑数值累积误差或构造错误，需要重新正交化。

三、旋转矩阵的构造：绕单轴转动与连乘顺序

绕 x、y、z 轴转动角 φ、θ、ψ 的基本旋转矩阵分别为：

Rx(φ) = [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]
Ry(θ) = [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
Rz(ψ) = [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]

复杂姿态由基本旋转连乘得到，而连乘顺序不可交换：Rz(ψ)Ry(θ)Rx(φ) ≠ Rx(φ)Ry(θ)Rz(ψ)。因此必须明确约定“先绕哪个轴转”。水下导航中常用 Z-Y-X 顺序（先偏航 ψ、再俯仰 θ、最后横滚 φ），对应 R_b^n = Rz(ψ)·Ry(θ)·Rx(φ)。若把顺序写反，即使三个角数值相同，得到的姿态也会明显不同——这是实际调试中最常见的错误来源之一。

四、齐次坐标：把旋转和平移合成一次乘法

纯旋转矩阵无法表达平移。齐次坐标通过升维解决：把三维点 p = [x, y, z]ᵀ 写成 [x, y, z, 1]ᵀ，变换矩阵扩展为 4×4：

T = [ R  t ]      T · [p; 1] = [R·p + t; 1]
    [ 0  1 ]

其中 t 为平移向量。这样“旋转+平移”被压缩为一次矩阵乘法，多级坐标链（如应答器→载体→导航系）可直接连乘 T_total = T_1·T_2·…。需要注意：齐次变换的连乘顺序同样不可交换，且逆变换为 T⁻¹ = [Rᵀ, −Rᵀt; 0, 1]，利用正交性避免了显式求逆。

五、欧拉角与方向余弦矩阵的关系

方向余弦矩阵（DCM）就是旋转矩阵的另一种叫法，其元素 R_ij 等于新坐标系第 i 轴与旧坐标系第 j 轴夹角的余弦。欧拉角是它的三参数表示，DCM 是九元素表示，两者一一对应但各有取舍：欧拉角直观、便于人机交互，但存在万向节锁（俯仰角接近 ±90° 时偏航与横滚耦合）；DCM 无奇异、便于连乘，但需维护正交性。

以 Z-Y-X 顺序为例，展开后可直接从矩阵元素反解欧拉角：θ = −arcsin(R_31)，ψ = atan2(R_21, R_11)，φ = atan2(R_32, R_33)。实际解算中，当 |R_31| 接近 1 时即进入奇异区，应改用四元数或做特殊处理，而不是强行反解。

六、把数学落到工程自检上

把上述内容串成一次可执行的自检流程：给定一组姿态角 ψ=30°、θ=5°、φ=−2°（示例假设值），按 Z-Y-X 顺序构造 R_b^n，先验证 RᵀR 与单位阵的最大偏差是否小于 1e-6、det(R) 是否为 +1；再用反解公式回算三个角，与输入比对，偏差应在 1e-9 量级（双精度）。若回算不一致，优先检查连乘顺序与角度单位（弧度/度）；若正交性超差，检查是否在迭代中直接累乘了未正交化的矩阵。完成这一步，后续的坐标链连乘、斜距投影与位置解算才有可靠的数学底座。