📚 课程目录第2章姿态描述与坐标变换仿真

2. 姿态描述与坐标变换仿真

在姿态控制软件中，姿态描述方式的选择直接影响数值稳定性、计算效率和可视化效果。本节围绕四种常用姿态表示——方向余弦矩阵（DCM）、欧拉角、四元数和旋转矢量——讨论它们在软件中的数据结构、相互转换关系，以及如何通过可视化手段验证转换的正确性。

一、四种姿态表示的软件数据结构

方向余弦矩阵用 3×3 正交矩阵描述本体系相对参考系的方位，软件中通常以二维数组或 9 元素一维数组存储。其优点是物理意义直观、可直接用于矢量坐标变换，缺点是存在 6 个冗余约束，长时间积分会因舍入误差失去正交性，需要定期正交化。

欧拉角用三个角度描述依次绕坐标轴的旋转，常见顺序有 3-1-2、3-2-1 等。软件中一般用长度为 3 的数组存储，并额外记录旋转顺序。欧拉角最直观，但存在万向节死锁：当中间角接近 ±90° 时，另两个角耦合，解算出现奇异。

四元数用四个参数（标量部分加矢量部分）描述姿态，软件中常用长度为 4 的数组，且需保持单位模约束。四元数无奇异、运算量小，适合姿态递推和插值，是星上软件最常用的表示。

旋转矢量用三维矢量表示绕某轴旋转的角度，模长即旋转角。它适合描述小角度增量旋转和姿态误差，在姿态确定与滤波中常作为状态量。

二、转换关系与实现要点

四种表示之间可以相互转换，但转换路径的选择会影响数值精度和计算量。软件中常见的转换包括：

欧拉角 → DCM：按旋转顺序依次构造基本旋转矩阵并相乘。以 3-1-2 顺序为例，C = C₂(θ) · C₁(φ) · C₃(ψ)，其中 C₁、C₂、C₃ 分别为绕 x、y、z 轴的基本旋转矩阵。
DCM → 欧拉角：从矩阵元素反解角度。以 3-1-2 顺序为例，θ = arcsin(C₂₃)，φ = arctan2(−C₂₁, C₂₂)，ψ = arctan2(−C₁₃, C₃₃)。当 |C₂₃| → 1 时进入死锁区，需特殊处理。
四元数 → DCM：利用单位四元数的二次型公式直接构造矩阵，避免三角函数运算。
DCM → 四元数：通过矩阵迹和对称/反对称分量提取四元数，需注意符号选择和归一化。
旋转矢量 → 四元数：q₀ = cos(θ/2)，qᵥ = e · sin(θ/2)，其中 θ = |Φ|，e = Φ/|Φ|。小角度时可用泰勒展开近似。

实现提示：所有转换函数应统一约定旋转方向（如“本体系相对参考系”还是“参考系相对本体系”），并在函数注释中明确。四元数转换后必须重新归一化，DCM 转换后建议做一次正交化处理。

三、可视化验证方法

转换正确性不能只靠代码审查，必须通过可视化手段验证。推荐以下三种验证方式：

往返一致性检查：将姿态从一种表示转到另一种再转回来，比较原始值与返回值。例如四元数 q → DCM → 四元数 q′，检查 |q·q′| 是否接近 1。若偏差超过 1e-10，说明转换公式或符号约定有误。
三维姿态可视化：用三轴坐标系或航天器模型显示姿态，直观判断旋转方向是否正确。例如给定欧拉角 (30°, 45°, 60°)，观察本体系 x 轴是否指向预期方向。
轨迹对比：对同一姿态序列分别用四元数和 DCM 递推，绘制两者随时间的变化曲线，检查是否一致。若 DCM 逐渐偏离，说明需要正交化。

四、教学示例：四元数与欧拉角互转验证

下面给出一个可运行的 Python 示例，演示四元数与 3-1-2 欧拉角的互转及往返验证。示例参数为合理假设，非实测数据。

import numpy as np

def euler312_to_quat(phi, theta, psi):
    """3-1-2 欧拉角转四元数"""
    c1, s1 = np.cos(phi/2), np.sin(phi/2)
    c2, s2 = np.cos(theta/2), np.sin(theta/2)
    c3, s3 = np.cos(psi/2), np.sin(psi/2)
    q0 = c1*c2*c3 - s1*s2*s3
    q1 = s1*c2*c3 + c1*s2*s3
    q2 = c1*s2*c3 - s1*c2*s3
    q3 = c1*c2*s3 + s1*s2*c3
    return np.array([q0, q1, q2, q3])

def quat_to_euler312(q):
    """四元数转 3-1-2 欧拉角"""
    q0, q1, q2, q3 = q / np.linalg.norm(q)
    theta = np.arcsin(2*(q0*q2 - q1*q3))
    phi = np.arctan2(2*(q0*q1 + q2*q3), 1 - 2*(q1**2 + q2**2))
    psi = np.arctan2(2*(q0*q3 + q1*q2), 1 - 2*(q2**2 + q3**2))
    return np.array([phi, theta, psi])

# 示例：给定欧拉角
phi, theta, psi = np.radians([30, 45, 60])
q = euler312_to_quat(phi, theta, psi)
euler_back = quat_to_euler312(q)
print("原始欧拉角(deg):", np.degrees([phi, theta, psi]))
print("往返欧拉角(deg):", np.degrees(euler_back))
print("最大角度误差(rad):", np.max(np.abs(euler_back - [phi, theta, psi])))

运行后应看到往返欧拉角与原始值一致，最大角度误差在 1e-15 量级（浮点精度极限）。若误差明显偏大，应检查四元数是否归一化、欧拉角顺序是否匹配。

常见错误：欧拉角转四元数时旋转顺序写错，导致往返结果不一致；四元数转欧拉角时未处理 arcsin 的定义域，当 |2(q₀q₂−q₁q₃)| > 1 时出现 NaN。建议在 arcsin 前做截断处理。

五、工程实践建议

在星上软件中，姿态表示的选择需权衡存储、计算和精度。四元数适合作为主递推量，欧拉角适合人机交互和地面显示，DCM 适合坐标变换和敏感器数据处理，旋转矢量适合滤波和误差描述。软件架构上建议设计统一的姿态类，内部以四元数存储，对外提供各表示的转换接口，并在每次转换后执行一致性自检。

可视化验证应集成到仿真测试流程中，每次修改转换函数后自动运行往返测试和三维显示，确保姿态描述与坐标变换的正确性。