2. AUV运动学基础与坐标系

要让AUV按航路点自主航行，飞控或任务计算机首先必须回答三个问题：我在哪里、我朝哪个方向、我以多快的速度往哪走。这三个问题分别对应位置、姿态和速度，而它们的数学描述都离不开坐标系。本节不涉及力和力矩，只讨论“几何与运动”层面的描述，也就是运动学。理解清楚坐标系及其转换关系，是后续表示航路点、设计航迹和判断到达条件的直接前提。

一、三个坐标系及其定义

工程上最常用的是地球坐标系、载体坐标系和航迹坐标系，三者分工明确。

地球坐标系（惯性/导航系）：记作 {E}，原点取在任务起点或局部地面某点，x 轴指向正东，y 轴指向正北，z 轴指向天顶，构成右手系。对于几十公里以内的近海任务，可以忽略地球曲率，把它当作局部水平直角坐标系使用。AUV的位置、航路点坐标、航迹几何都在这个系里描述。

载体坐标系（体系）：记作 {B}，原点取在AUV重心，x 轴指向艇艏，y 轴指向右舷，z 轴指向艇底。速度、加速度、角速度以及推进器、传感器安装位置通常在这个系里测量或定义。它与艇体固连，随艇一起转动。

航迹坐标系（速度系）：记作 {W}，原点同样在重心，但 x 轴指向AUV相对水（或地）的实际速度方向，z 轴在包含速度矢量的铅垂面内指向下方。它描述的是“实际往哪走”，而不是“艇艏朝哪”。当存在海流或侧滑时，航迹系与体系并不重合，这个差别在航路点跟踪中非常关键。

二、位置、姿态与速度的数学描述

在地球系中，位置用三维向量表示：

η₁ = [x, y, z]ᵀ   （单位：m）

姿态用三个欧拉角描述：艏向角 ψ（绕 z 轴，从北向东为正）、纵倾角 θ（绕 y 轴，抬艏为正）、横滚角 φ（绕 x 轴，右倾为正）。三者组成姿态向量 η₂ = [φ, θ, ψ]ᵀ。于是完整的位姿向量为 η = [η₁; η₂]，共六个自由度。

速度有两种描述方式：体系下的线速度 v = [u, v, w]ᵀ（纵荡、横荡、垂荡）和角速度 Ω = [p, q, r]ᵀ（横滚、纵倾、艏摇）。地球系下的速度则是 η₁ 的时间导数。两者通过旋转矩阵联系：

η₁̇ = R(φ, θ, ψ) · v
R = Rz(ψ)·Ry(θ)·Rx(φ)

展开后，R 的第三行只与 θ、φ 有关，这解释了为什么纵倾角会直接影响深度变化率。同理，欧拉角变化率与体系角速度之间通过一个与 φ、θ 相关的变换矩阵 T 联系：η₂̇ = T(φ, θ)·Ω。当 θ 接近 ±90° 时 T 出现奇异，因此实际系统中常改用四元数表示姿态，避免万向节锁死。

三、坐标系之间的转换关系

三个坐标系之间的转换可以串成一条链：地球系 → 载体坐标系 → 航迹坐标系。地球系到体系用旋转矩阵 R 完成；体系到航迹系则用两个角度描述：攻角 α（速度在艇体纵剖面内相对 x 轴的偏角）和侧滑角 β（速度相对纵剖面的偏角）。当 α、β 均为零时，航迹系与体系重合，艇艏方向即速度方向；存在侧滑时，航迹系的 x 轴会偏离艇艏，此时艏向角 ψ 与航迹方位角 χ 不再相等，二者之差就是漂角。

下图把三个坐标系、位姿向量和转换关系放在同一张图上，便于对照记忆。

四、从坐标系到航路点表示

有了上述约定，航路点就可以严格定义。一个航路点通常写成 P_i = (x_i, y_i, z_i, ψ_i)，前三个分量在地球系中给出期望到达位置，ψ_i 给出到达时的期望艏向（可选）。AUV当前位姿由导航系统输出 η，于是“是否到达”就转化为地球系中的距离判断：

d = sqrt((x - x_i)² + (y - y_i)² + (z - z_i)²)
到达条件：d < R_accept   （R_accept 为接受半径，示例取 5 m）

而“朝哪个方向走”则由期望航迹方位角给出：χ_des = atan2(y_i − y, x_i − x)，注意这里用的是地球系坐标差，且 atan2 的参数顺序要与“北为 y、东为 x”的约定匹配。控制器比较的是 χ_des 与实际航迹方位角 χ，而不是直接比较 ψ，否则在海流中会出现系统性偏航。

五、一个可手算的示例

设AUV当前位于地球系 (x, y, z) = (100, 200, −30) m，体系速度 v = [1.2, 0.1, −0.05]ᵀ m/s，姿态角 φ = 0°、θ = 3°、ψ = 45°。先算旋转矩阵第三行对应的深度变化率：

ż ≈ −sinθ·u + sinφ·cosθ·v + cosφ·cosθ·w
  ≈ −0.0523×1.2 + 0 + 0.9986×(−0.05)
  ≈ −0.0628 − 0.0499 ≈ −0.113 m/s

也就是说，尽管垂荡速度 w 只有 −0.05 m/s，纵倾 3° 使深度变化率被放大到约 −0.113 m/s。若下一个航路点深度为 −35 m，按此速率约需 44 s 到达，但期间 x、y 也在变化，因此实际到达判断必须用三维距离而非只看深度。这个例子说明：姿态角不是“附属信息”，它直接参与位置更新，忽略纵倾会导致深度估计偏差。

掌握以上内容后，航路点就不再是一串孤立数字，而是带有明确坐标基准和姿态约束的几何目标；航迹也可以被描述为地球系中的一条参数曲线，其切向即期望航迹方位角。后续的航迹跟踪与制导律，本质上都是在比较“期望的 χ、γ”与“实际的 χ、γ”，并据此生成体系下的期望速度或角速度指令。