📚 课程目录第2章水下导航坐标系与运动学基础

2. 水下导航坐标系与运动学基础

在DVL与惯导组合导航中，几乎所有“数据对不上”的问题，追根溯源都能归结为坐标系没对齐。DVL测的是载体相对海底的速度，惯导解算的是载体在导航系下的姿态、速度和位置，两者要融合，就必须先统一到同一个数学框架里。本节把四个常用坐标系、它们之间的转换关系，以及水下载体的运动学方程讲清楚，为后续的数据融合打下基础。

四个坐标系：各司其职

惯性坐标系（i系）是牛顿力学成立的基准，原点取在地心，坐标轴指向恒星（如J2000）。它不随地球自转，是陀螺测量角运动的绝对参考。实际工程中我们很少直接在i系里算，但它是所有推导的起点。

地球坐标系（e系）与地球固连，随地球一起自转，原点在地心，z轴沿自转轴指向北极，x轴指向本初子午线与赤道交点。它用来描述载体相对地球的位置，经度、纬度、高度都定义在e系框架下。

导航坐标系（n系）是惯导解算的工作坐标系，通常取“东北天”（ENU）或“北东地”（NED）。以ENU为例：x轴指东、y轴指北、z轴指天，原点取在载体所在点的地球表面。n系随载体移动而平移，但坐标轴方向相对当地水平面保持不变，因此姿态角（航向、俯仰、横滚）都是相对n系定义的。

载体坐标系（b系）与载体固连，原点取在惯导的测量中心，x轴指向载体右舷、y轴指向艏向、z轴指向载体上方（右-前-上，RFU）。DVL的波束方向、IMU的加速度计和陀螺安装方向，都是在b系下描述的。

约定说明：本课程统一采用ENU导航系与RFU载体系。航向角以n系y轴（北）为基准，顺时针为正；俯仰角绕b系x轴，抬头为正；横滚角绕b系y轴，右倾为正。全文与图示保持一致，切换参照系时会明确说明。

坐标系之间的转换关系

四个坐标系构成一条清晰的转换链：i系经地球自转得到e系，e系经当地经纬度得到n系，n系经载体姿态得到b系。核心工具是方向余弦矩阵（DCM）。

从n系到b系的转换由三个姿态角依次旋转实现。以ENU和RFU约定，若航向角为ψ、俯仰角为θ、横滚角为φ，则旋转顺序通常取“先绕z轴转ψ，再绕x轴转θ，最后绕y轴转φ”，得到矩阵 C_n^b。反过来，C_bⁿ = (C_n^b)^T，正交矩阵的转置就是逆。这个矩阵是惯导姿态解算的直接输出，也是把DVL的b系速度投影到n系的关键。

从e系到n系的转换只依赖当地纬度L和经度λ，与载体姿态无关，记为 C_eⁿ。它把地球固连的位置增量换算到当地水平坐标，惯导的位置更新和地球自转补偿都要用到它。

实际计算中，我们不会每次都从i系一路乘下来，而是维护一个从b系到n系的姿态矩阵，用陀螺输出的角增量实时更新它，这就是惯导姿态解算的本质。

水下载体运动学方程

惯导的核心是一组运动学微分方程。设载体在n系下的速度为 vⁿ，姿态矩阵为 C_bⁿ，位置用纬度L、经度λ、高度h表示，则基本方程可写为：

姿态：  Ċ_b^n = C_b^n · [ω_ib^b ×]
速度：  v̇^n = C_b^n · f^b − (2ω_ie^n + ω_en^n) × v^n + g^n
位置：  L̇ = v_N / (R_M + h)
        λ̇ = v_E / ((R_N + h)·cos L)
        ḣ = v_U

逐项看：f^b 是加速度计测得的比力，经 C_bⁿ 投影到n系；2ω_ieⁿ×vⁿ 是哥氏加速度修正，ω_enⁿ×vⁿ 是载体在地球曲面运动引起的向心加速度修正；gⁿ 是重力矢量。这三项修正对水下低速载体看似很小，但长时间积分会累积成可观的漂移，不能省略。

姿态更新用陀螺角增量 ω_ib^b 构造反对称矩阵，通过四元数或旋转矢量法递推。工程上常用四元数避免万向锁，每步更新后再归一化并转回DCM。

一个可复现的数值示例

假设某水下潜器在平静水域定深直航，DVL在b系测得速度 v^b = [0.02, 1.20, 0.01] m/s（右、前、上），惯导输出姿态为航向ψ=30°、俯仰θ=2°、横滚φ=1°。我们把它投影到n系，检验融合前的数据对齐是否正确。

按ENU与RFU约定构造 C_bⁿ，取小角度近似（θ、φ均小于3°，可先忽略二阶项），则航向旋转主导：

v_E ≈ v_x·cosψ + v_y·sinψ = 0.02×0.866 + 1.20×0.5 ≈ 0.617 m/s
v_N ≈ −v_x·sinψ + v_y·cosψ = −0.02×0.5 + 1.20×0.866 ≈ 1.029 m/s
v_U ≈ v_z + v_y·θ ≈ 0.01 + 1.20×0.035 ≈ 0.052 m/s

结果 vⁿ ≈ [0.617, 1.029, 0.052] m/s。可以看到，艏向1.20 m/s的速度在n系下分解为东向0.617、北向1.029，合速度约1.20 m/s，与输入一致，说明投影正确。若此时惯导解算的n系速度与这组值相差较大，就应优先排查姿态矩阵的旋转顺序或正负号约定，而不是急着调滤波器参数。

常见错误：把DVL的b系速度直接与惯导的n系速度相减。两者不在同一坐标系，差值没有物理意义，融合结果必然发散。务必先经 C_bⁿ 投影，并确认DVL安装角（如波束相对载体的偏置）已在标定中补偿。

为数据融合做好准备

把上面的内容串起来，DVL与INS融合的数学准备其实就三件事：第一，明确n系与b系的约定并全文统一；第二，维护好 C_bⁿ 的实时更新，它是连接两个传感器的桥梁；第三，理解速度方程中各项修正的物理含义，知道哪些量在什么航速、什么时长下不可忽略。做到这三点，后续无论是松组合还是紧组合，量测方程里的每一项都能对应到具体的物理对象，调试时也有据可查。