3. 欧拉角表示姿态

任务目标

本案例实现动平台姿态的ZYX欧拉角表示法，并编写将欧拉角（绕Z轴偏航ψ、绕Y轴俯仰θ、绕X轴滚转φ）转换为旋转矩阵的函数。该旋转矩阵用于后续Stewart平台正运动学中描述动平台相对于基座的姿态。

ZYX欧拉角定义

ZYX欧拉角采用外旋（绕固定坐标系旋转）方式，旋转顺序为：先绕Z轴旋转ψ（偏航），再绕Y轴旋转θ（俯仰），最后绕X轴旋转φ（滚转）。对应的旋转矩阵为：

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

其中各基本旋转矩阵为：

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φ]

Python实现

import numpy as np

def euler_zyx_to_rotation_matrix(psi, theta, phi):
    """
    将ZYX欧拉角转换为旋转矩阵（外旋，绕固定轴）
    
    参数:
        psi   : 偏航角，绕Z轴旋转（弧度）
        theta : 俯仰角，绕Y轴旋转（弧度）
        phi   : 滚转角，绕X轴旋转（弧度）
    
    返回:
        3x3 numpy数组，表示旋转矩阵R = Rz(psi) @ Ry(theta) @ Rx(phi)
    """
    cz, sz = np.cos(psi), np.sin(psi)
    cy, sy = np.cos(theta), np.sin(theta)
    cx, sx = np.cos(phi), np.sin(phi)
    
    Rz = np.array([[cz, -sz, 0],
                   [sz,  cz, 0],
                   [0,   0,  1]])
    
    Ry = np.array([[cy,  0, sy],
                   [0,   1,  0],
                   [-sy, 0, cy]])
    
    Rx = np.array([[1,  0,   0],
                   [0,  cx, -sx],
                   [0,  sx,  cx]])
    
    return Rz @ Ry @ Rx

# 最小验证示例
if __name__ == "__main__":
    # 测试1: 零角度应得到单位矩阵
    R_zero = euler_zyx_to_rotation_matrix(0, 0, 0)
    print("零角度结果:\n", R_zero)
    print("是否为单位矩阵:", np.allclose(R_zero, np.eye(3)))
    
    # 测试2: 仅绕Z轴旋转90度(π/2)
    R_z90 = euler_zyx_to_rotation_matrix(np.pi/2, 0, 0)
    expected_z = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
    print("\n仅偏航90°结果:\n", R_z90)
    print("与预期一致:", np.allclose(R_z90, expected_z))
    
    # 测试3: 验证旋转矩阵正交性 R^T @ R = I
    R_test = euler_zyx_to_rotation_matrix(0.3, -0.5, 0.8)
    ortho_check = R_test.T @ R_test
    print("\n正交性检查 R^T·R:\n", ortho_check)
    print("是否为单位矩阵:", np.allclose(ortho_check, np.eye(3)))

运行与验证

运行方式：将上述代码保存为 euler_zyx.py，在终端执行 python euler_zyx.py。

验证方法：

• 零角度输入应输出单位矩阵，验证函数基本正确性；
• 仅偏航90°时，结果应与绕Z轴旋转90°的标准矩阵一致；
• 任意角度下，旋转矩阵应满足正交性（R^TR = I），行列式为1。

预期输出：三个测试均打印 True，且输出的矩阵数值与上述理论值一致。

关键说明

• 本函数采用外旋（固定轴）定义，与内旋（绕自身轴）的ZYX欧拉角矩阵形式不同，使用时需明确约定；
• 角度单位均为弧度，若输入角度为度需先转换；
• 该旋转矩阵将基座坐标映射到动平台坐标，即 p_platform = R @ p_base，后续正运动学中用于计算动平台铰点位置。