2. 坐标变换基础

任务目标

本案例实现从Stewart平台基座坐标系{B}到动平台坐标系{P}的齐次变换矩阵T，该矩阵由3×3旋转矩阵R与3×1平移向量t构成。同时验证旋转矩阵的正交性（RTR = I且det(R)=1），确保其属于SO(3)群。

核心实现

import numpy as np

def homogeneous_transform(roll, pitch, yaw, tx, ty, tz):
    """
    构建从基座到动平台的齐次变换矩阵
    参数: 欧拉角(rad, 绕固定轴ZYX顺序), 平移量
    返回: 4x4齐次变换矩阵T
    """
    cr, sr = np.cos(roll), np.sin(roll)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)

    # 固定轴ZYX旋转矩阵 R = Rz(yaw) @ Ry(pitch) @ Rx(roll)
    R = np.array([
        [cy*cp,  cy*sp*sr - sy*cr,  cy*sp*cr + sy*sr],
        [sy*cp,  sy*sp*sr + cy*cr,  sy*sp*cr - cy*sr],
        [-sp,    cp*sr,             cp*cr]
    ])

    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = [tx, ty, tz]
    return T

def verify_orthogonality(R):
    """验证旋转矩阵正交性: R^T R = I 且 det(R) = 1"""
    I_check = R.T @ R
    is_orthogonal = np.allclose(I_check, np.eye(3), atol=1e-10)
    det = np.linalg.det(R)
    is_proper = np.isclose(det, 1.0, atol=1e-10)
    return is_orthogonal, is_proper, det

# 示例：动平台相对基座偏航30°，平移(0.1, 0, 0.5)米
if __name__ == "__main__":
    roll, pitch, yaw = 0.0, 0.0, np.deg2rad(30)
    T = homogeneous_transform(roll, pitch, yaw, 0.1, 0.0, 0.5)
    print("齐次变换矩阵 T:")
    print(np.round(T, 6))

    R = T[:3, :3]
    ortho, proper, det = verify_orthogonality(R)
    print(f"\n正交性验证: R^T R = I -> {ortho}")
    print(f"行列式验证: det(R) = {det:.10f} -> 等于1: {proper}")

    # 验证平移向量正确提取
    t = T[:3, 3]
    print(f"平移向量 t = {np.round(t, 6)}")

运行与验证

环境依赖：仅需NumPy（pip install numpy）。

运行方式：将代码保存为transform_basis.py，执行python transform_basis.py。

预期输出：程序打印4×4齐次矩阵，其中左上3×3为旋转矩阵，右上3×1为平移向量。验证输出应为：R^T R = I -> True，det(R) = 1.0000000000 -> 等于1: True。旋转矩阵数值与ZYX欧拉角公式一致（例如R[0,0]=cos(30°)=0.866025）。

关键说明

• 采用固定轴ZYX欧拉角约定（先绕X轴滚转，再绕Y轴俯仰，最后绕Z轴偏航），该约定与Stewart平台常用姿态描述一致。
• 正交性验证使用np.allclose与np.isclose，容差1e-10，避免浮点误差导致误判。
• 齐次矩阵的逆T-1可直接通过np.linalg.inv(T)获得，用于后续案例中从动平台坐标反算基座坐标。

上图展示了基座坐标系{B}与动平台坐标系{P}之间的空间关系：动平台先绕基座Z轴旋转30°，再沿基座X轴平移0.1m、Z轴平移0.5m。齐次变换矩阵T完整编码了这一旋转与平移的组合，使得任意在动平台坐标系中表示的点pP可通过pB = T @ [pP; 1]转换到基座坐标系。