📚 课程目录第2章 ROV姿态与航向的数学描述

2. ROV姿态与航向的数学描述

要让控制器“知道”ROV现在是什么姿态、朝哪个方向，必须先把姿态这件事用数学语言说清楚。ROV在三维空间中的运动可以拆成六个自由度：沿三个轴的平移（纵荡、横荡、垂荡）和绕三个轴的旋转（横滚、俯仰、偏航）。本节只讨论旋转部分，也就是姿态角与航向的数学描述，它是后续姿态控制、航向保持和深度控制算法的共同基础。

一、姿态角的定义与参照约定

工程上最常用的是欧拉角表示法，用三个角度描述机体相对参考系的旋转。为避免符号混乱，本节统一采用如下约定，后续章节和图示保持一致：

参考系（大地系）：X轴指向北，Y轴指向东，Z轴垂直向下（NED，北东地）。这是水下航行器常用的右手坐标系。
机体系：X轴指向ROV艏向（前进方向），Y轴指向右舷，Z轴指向下方。机体系固连在ROV本体上，随ROV一起转动。
横滚角 roll（φ）：绕机体X轴旋转，右舷下沉为正。
俯仰角 pitch（θ）：绕机体Y轴旋转，艏部抬起为正。
偏航角 yaw（ψ）：绕机体Z轴旋转，从北向顺时针（即向右转）为正，取值范围通常取 −180°～+180° 或 0°～360°。

这里要特别强调：偏航角就是航向角的数学表达。ROV的“航向”本质上就是机体X轴在水平面上的投影与北向之间的夹角，与偏航角ψ是同一个量。实际使用时，航向常以0°～360°表示（0°为正北，90°为正东），而控制算法内部多用−180°～+180°，两者只差一个角度归一化处理。

二、坐标系转换与旋转矩阵

姿态的本质是“机体系到大地系的坐标变换”。若已知某点在机体系下的坐标，要换算到大地系，需要依次绕三个轴旋转。按Z-Y-X顺序（先偏航、再俯仰、最后横滚）旋转，单次旋转矩阵分别为：

Rz(ψ) = [ cosψ  -sinψ  0 ]
        [ sinψ   cosψ  0 ]
        [   0      0    1 ]

Ry(θ) = [  cosθ  0  sinθ ]
        [   0    1    0   ]
        [-sinθ  0  cosθ ]

Rx(φ) = [ 1   0      0    ]
        [ 0  cosφ  -sinφ  ]
        [ 0  sinφ   cosφ  ]

完整的旋转矩阵为三者相乘：R = Rz(ψ)·Ry(θ)·Rx(φ)。这个矩阵把机体系向量变换到大地系。反过来，大地系到机体系的变换就是R的转置（因为旋转矩阵是正交矩阵，R⁻¹ = Rᵀ）。

举个教学示例（数值为假设，仅用于说明计算过程）：设ROV当前横滚φ=5°、俯仰θ=−3°、偏航ψ=30°。若机体前方有一个传感器，其机体系坐标为(1, 0, 0)，要判断它在大地系中的指向，只需用R乘以该向量。由于该向量只有X分量，结果就是R的第一列，即(cosψ·cosθ, sinψ·cosθ, −sinθ) ≈ (0.865, 0.499, 0.052)。这说明传感器指向大致为北偏东30°、略微向上，与直觉一致。这类计算在传感器安装位置换算、推力分配和视觉目标定位中会反复用到。

三、欧拉角的局限与工程处理

欧拉角直观、易读，但存在两个工程上必须注意的问题。第一是万向节死锁：当俯仰角接近±90°时，横滚和偏航的旋转轴趋于重合，姿态描述出现奇异，角度解不再唯一。ROV正常作业时俯仰角一般不超过±30°，很少触及该奇异点，但在大角度机动或翻滚回收时仍需警惕。第二是角度周期性：航向从179°变到−179°，数值上跳变358°，实际只转了2°。控制器若直接对角度做差，会产生巨大的虚假误差。工程上的标准做法是做角度归一化，把误差折算到−180°～+180°区间：

def wrap_angle(a):
    while a > 180:  a -= 360
    while a < -180: a += 360
    return a

# 航向误差计算示例
err = wrap_angle(target_heading - current_heading)

若需要完全避开奇异问题，可改用四元数描述姿态，但四元数不直观，通常只在姿态解算内部使用，对外显示和控制目标仍用欧拉角。对入门阶段的ROV控制而言，掌握欧拉角加角度归一化已足够。

四、从姿态角到控制量的衔接

姿态角最终要服务于控制。ROV上通常装有IMU（惯性测量单元），它直接输出横滚、俯仰和航向角，或输出角速度由控制器积分得到角度。控制器读取当前姿态角，与目标姿态角比较，得到误差，再经PID等算法输出力矩指令，分配给推进器。这里的关键是：误差必须在同一参照系下计算。若目标航向用0°～360°表示，当前航向用−180°～180°表示，必须先统一再相减，否则控制器会在跨越0°时产生剧烈抖动。

下面的图示把本节的核心关系串起来：大地系与机体系的对应、三个姿态角的旋转轴、旋转矩阵的合成顺序，以及姿态角如何进入控制回路。

把这张图看懂，本节的知识就串起来了：左边是“姿态是什么”，中间是“姿态怎么算”，右边是“姿态怎么用”。后续的航向保持、定深控制和姿态稳定算法，都是在这套数学描述之上展开的。实际调试时，建议先在静止状态下核对IMU输出的横滚、俯仰是否与水平仪一致，再手动转动ROV观察航向变化方向是否符合“右转为正”的约定，确认符号无误后再接入控制器，可以避免大量因参照系不一致导致的反复排查。