2. AUV六自由度运动学基础

上一节我们建立了AUV六自由度动力学的总体框架，本节聚焦其中的“运动学”部分。动力学回答“力如何产生加速度”，而运动学回答“速度如何积分成位置与姿态”。对AUV而言，运动学是导航、控制与仿真的公共底座：没有一套自洽的坐标系与姿态表示，后续的推力分配、路径跟踪和传感器融合都无从谈起。本节从坐标系定义出发，依次建立姿态角、速度表示，最终推导出六自由度运动学方程并给出变换矩阵的构建与验证方法。

一、惯性系与体坐标系

描述AUV运动至少需要两个坐标系。惯性系（也称地球固定系）用于描述AUV相对地面的绝对位置与姿态，工程上常取北-东-地（NED）右手系：X轴指北、Y轴指东、Z轴垂直向下，原点固定于水面某点。体坐标系固连于AUV本体，原点通常取在重心或浮心处，X轴指向艏部、Y轴指向右舷、Z轴指向底部，同样构成右手系。两套坐标系均为右手系，这一点必须在全文保持一致，否则后续叉乘与旋转矩阵的符号会整体翻转。

设惯性系下AUV位置为 η₁ = [x, y, z]ᵀ，姿态为 η₂ = [φ, θ, ψ]ᵀ（横滚、纵倾、艏向）；体坐标系下线速度为 v₁ = [u, v, w]ᵀ，角速度为 v₂ = [p, q, r]ᵀ。这十二个变量构成六自由度状态，其中位置与姿态各三自由度。需要强调：v₁ 与 v₂ 是相对体坐标系的量，而 η₁ 的导数才是惯性系下的绝对速度，二者通过旋转矩阵联系。

二、姿态角与四元数

欧拉角直观但存在奇异点。采用Z-Y-X顺序（先艏向ψ，再纵倾θ，最后横滚φ）时，旋转矩阵为：

R = Rz(ψ)·Ry(θ)·Rx(φ)
  = [ cψcθ,  cψsθsφ−sψcφ,  cψsθcφ+sψsφ
      sψcθ,  sψsθsφ+cψcφ,  sψsθcφ−cψsφ
      −sθ,   cθsφ,          cθcφ        ]

其中c、s分别代表cos与sin。当纵倾角θ接近±90°时，横滚与艏向耦合，出现万向节锁，姿态解算失效。AUV在垂直下潜或大纵倾机动时恰好会接近这一区域，因此工程实现中常以四元数作为内部姿态表示。四元数 q = [q₀, q₁, q₂, q₃]ᵀ 无奇异，其与欧拉角的转换关系为：

φ = atan2(2(q₀q₁+q₂q₃), 1−2(q₁²+q₂²))
θ = asin(2(q₀q₂−q₃q₁))
ψ = atan2(2(q₀q₃+q₁q₂), 1−2(q₂²+q₃²))

实际做法是：控制器内部用四元数积分姿态，仅在显示或与欧拉角接口交互时转换，避免奇异点附近的数值跳变。

三、速度与角速度的坐标变换

体坐标系速度到惯性系速度的变换为 η₁̇ = R·v₁，这是纯旋转关系。姿态角导数与体角速度的关系则不是简单旋转，而需通过欧拉角速率矩阵 T 联系：η₂̇ = T·v₂，其中

T = [1, sφ·tanθ, cφ·tanθ
     0, cφ,      −sφ
     0, sφ/cθ,   cφ/cθ ]

可见当θ→±90°时tanθ与1/cθ发散，这正是欧拉角表示奇异性的数学根源。若改用四元数，姿态更新为 q̇ = 0.5·q⊗[0, p, q, r]ᵀ，全程无奇异，代价是需周期性归一化以抑制数值漂移。

四、六自由度运动学方程

将上述两部分合并，得到完整的六自由度运动学方程：

[η₁̇]   [R   0] [v₁]
[η₂̇] = [0   T] [v₂]

若采用四元数表示姿态，则第二行替换为四元数微分方程，第一行仍为 η₁̇ = R(q)·v₁，其中R由四元数直接构造，无需经过欧拉角。这一形式在数值积分中更为稳健。实际仿真中，给定体速度与角速度序列，用四阶龙格-库塔法对上述方程积分，即可得到AUV的轨迹与姿态历程。

五、变换矩阵的构建与验证

构建R时，建议按“单轴旋转矩阵相乘”的方式逐项实现，而非直接抄写展开式，以减少符号错误。验证可从三个层面进行。第一，正交性检验：计算 RᵀR，与单位阵比较，最大元素偏差应小于1e-6。第二，逆变换检验：对任意向量，先经R变换再经Rᵀ变换，应还原原向量。第三，物理一致性检验：取ψ=90°、θ=φ=0，此时体坐标系X轴应指向惯性系Y轴（东），用R乘以[1,0,0]ᵀ，结果应为[0,1,0]ᵀ。若符号相反，说明旋转顺序或轴定义有误。

一个常见的实操错误是混淆“体到惯性”与“惯性到体”的变换方向。记住：η₁̇ = R·v₁ 是体速度投影到惯性系，因此R的列向量是体坐标轴在惯性系中的方向余弦。若需要把惯性系下的量转到体坐标系，应使用 Rᵀ 而非重新构造一个矩阵。把这一条写进代码注释，能避免大量调试时间。