4. 坐标系与运动学基础

AUV 的自主航行，本质上是在不断回答三个问题：我在哪里、我朝哪个方向、我下一步往哪走。要回答这三个问题，必须先建立一套统一的“语言”——坐标系。本节从惯性坐标系与载体坐标系出发，讲清姿态角的定义、坐标变换的数学形式，并最终落到 AUV 六自由度运动学方程上，为后续的导航、控制与路径跟踪打基础。

一、惯性坐标系：描述“绝对”位置与航向

惯性坐标系（也称世界坐标系、大地坐标系）是静止不动的参考基准。在 AUV 工程中，常用北-东-地（NED）坐标系：原点取在任务起始点或某已知基准点，X 轴指向正北，Y 轴指向正东，Z 轴垂直向下指向地心。这样定义的好处是：Z 轴向下与 AUV 通常的下潜方向一致，深度值取正，符合直觉。

惯性坐标系用来描述 AUV 的位置（x, y, z）和航向角，也是路径规划、地图构建和任务坐标的统一参照。需要注意，NED 是右手系：X 北、Y 东、Z 下，三者满足右手定则。若团队习惯使用 ENU（东-北-天），则 Z 轴向上、深度取负，全文必须统一，不能混用。

二、载体坐标系：描述“自身”运动

载体坐标系（体坐标系）固连在 AUV 上，随载体一起运动。通常定义：原点取在 AUV 重心（或浮心与重心之间的参考点），X 轴指向艇艏（前进方向），Y 轴指向右舷，Z 轴指向艇底（向下）。这样，AUV 的前进、横移、下潜就分别对应 X、Y、Z 轴的正方向。

载体坐标系的价值在于：AUV 的推力、阻力、传感器安装位置、螺旋桨推力方向，都是在载体自身框架下描述的。例如“主推产生沿 X 轴正向的推力”“垂直推进器产生沿 Z 轴负向的推力（上浮）”，这些描述与 AUV 当前朝向无关，非常直观。

三、姿态角定义：横滚、俯仰、艏向

载体坐标系相对惯性坐标系的转动，用三个欧拉角描述，按 Z-Y-X 顺序（先绕 Z 轴转艏向，再绕 Y 轴转俯仰，最后绕 X 轴转横滚）：

• 艏向角 ψ（yaw）：绕 Z 轴旋转，载体 X 轴在水平面内相对正北的夹角，右转为正，范围通常取 (-180°, 180°]。
• 俯仰角 θ（pitch）：绕 Y 轴旋转，艇艏相对水平面的抬升角，抬头为正，范围 (-90°, 90°)。
• 横滚角 φ（roll）：绕 X 轴旋转，绕艇艏轴的滚转角，右倾为正，范围 (-180°, 180°]。

这三个角就是 AUV 的姿态。实际工程中，姿态由 IMU（惯性测量单元）测量，输出通常是四元数或欧拉角。需要提醒的是：欧拉角在俯仰角接近 ±90° 时会出现万向节死锁，因此内部姿态解算常用四元数，只在人机界面和日志中转换为欧拉角显示。

四、坐标变换：两套坐标系之间的桥梁

坐标变换解决“同一个量在两套坐标系下如何换算”的问题。核心是旋转矩阵 R，它把载体坐标系下的向量转换到惯性坐标系：

R = Rz(ψ) · Ry(θ) · Rx(φ)

其中：
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]

R 是正交矩阵，满足 R⁻¹ = Rᵀ，所以从惯性系到载体系的变换就是 Rᵀ。举例：AUV 艏向 ψ=90°（正东），俯仰、横滚为 0，载体系下测得前进速度 u=1 m/s，则惯性系速度为 R·[1,0,0]ᵀ = [0,1,0]ᵀ，即正东方向 1 m/s，与直觉一致。这个例子说明：速度大小不变，方向随姿态旋转。

五、六自由度运动学方程

AUV 的六自由度包括三个平动（x, y, z）和三个转动（φ, θ, ψ）。运动学方程描述的是“速度与位置/姿态变化率”的关系，不涉及力和质量（那是动力学的事）。其标准形式为：

位置导数：
ẋ = u·cosθ·cosψ + v·(sinφ·sinθ·cosψ - cosφ·sinψ) + w·(cosφ·sinθ·cosψ + sinφ·sinψ)
ẏ = u·cosθ·sinψ + v·(sinφ·sinθ·sinψ + cosφ·cosψ) + w·(cosφ·sinθ·sinψ - sinφ·cosψ)
ż = -u·sinθ + v·sinφ·cosθ + w·cosφ·cosθ

姿态导数：
φ̇ = p + q·sinφ·tanθ + r·cosφ·tanθ
θ̇ = q·cosφ - r·sinφ
ψ̇ = q·sinφ/cosθ + r·cosφ/cosθ

式中 u、v、w 是载体坐标系下的线速度（纵荡、横荡、垂荡），p、q、r 是载体坐标系下的角速度（横滚角速度、俯仰角速度、艏向角速度）。这组方程是 AUV 导航解算的核心：IMU 和 DVL 给出 u、v、w、p、q、r，通过积分即可递推出位置和姿态。

六、工程实现中的几个关键点

第一，角度单位统一。数学推导用弧度，但传感器和界面常用度，代码中务必在入口处统一转换，避免“转了 90 却当成 90 弧度”的低级错误。第二，积分方法。位置和姿态的递推常用一阶欧拉积分，但采样周期较大时误差累积明显，工程上可用二阶龙格-库塔或直接对四元数积分。第三，零速修正。AUV 在悬停或坐底时，DVL 可能输出零速，此时应冻结位置积分，防止漂移。第四，坐标系一致性检查。仿真、实船、日志三处的坐标系定义必须一致，建议在代码中把 NED 与 ENU 的转换封装成独立函数，并在启动时打印当前约定。

把上面这些内容串起来看：惯性坐标系给出“绝对”参照，载体坐标系描述“自身”运动，姿态角连接两者，旋转矩阵完成换算，六自由度运动学方程则把速度积分成位姿。这五件事构成 AUV 导航解算的完整链条。后续学习导航滤波、路径跟踪和姿态控制时，都会反复回到这套坐标系与方程上。建议在动手写代码前，先用本节的教学示例手算一遍，确认符号和方向无误，再进入仿真验证。