4. 坐标变换与齐次变换工程实现

在多轴运动平台中，控制器最终要回答的问题只有一个：工具中心点（TCP）此刻在工件坐标系中的位姿是什么，以及下一时刻应该是什么。位姿包含位置（3 个平移量）和姿态（3 个旋转自由度），而姿态的表达方式直接决定了插补、标定和逆解的数值行为。本节讨论旋转矩阵、齐次变换矩阵、欧拉角与四元数四种表达方式在平台位姿中的选择依据，以及代码实现中的数值稳定性问题。

一、旋转矩阵与齐次变换矩阵

旋转矩阵 R 是 3×3 正交矩阵，满足 RᵀR = I 且 det(R) = +1。它把同一向量在“旋转后的坐标系”与“原坐标系”之间做映射。约定：本文所有旋转矩阵均表示将坐标系 B 中的向量变换到坐标系 A，记作 R_AB，即 p_A = R_AB · p_B。绕 X、Y、Z 轴旋转角 θ 的基本矩阵为：

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

旋转矩阵的优点是可直接做向量乘法、可复合、无奇异；缺点是 9 个元素只有 3 个自由度，冗余度高，多次连乘后正交性会因浮点误差逐渐退化。因此工程上必须周期性做正交化（如 Gram-Schmidt 或 SVD 投影）。

齐次变换矩阵把旋转和平移统一为 4×4 形式：

T_AB = [[R_AB, t_AB],
        [0, 0, 0, 1]]

其中 t_AB 是 B 原点在 A 中的位置。齐次形式的最大价值在于变换链可以连乘：T_AC = T_AB · T_BC，逆变换为 T_BA = [[R_ABᵀ, -R_ABᵀ·t_AB],[0,1]]，无需矩阵求逆。多轴平台的运动学正解本质上就是一条齐次变换链：基座 → 各关节 → 末端法兰 → 工具。

二、欧拉角与四元数的选择

欧拉角用三个角描述姿态，直观、便于人工示教，但存在两个工程陷阱：一是万向节死锁（如 ZYZ 欧拉角在中间角为 0 或 π 时，首末两角不可分辨）；二是约定歧义——同样是 (α, β, γ)，XYZ 内旋、XYZ 外旋、ZYX 内旋会得到完全不同的姿态。因此工程文档必须写明旋转顺序与内旋/外旋约定，代码中建议用枚举固定，例如 EulerOrder::ZYX_INTRINSIC。

四元数 q = (w, x, y, z) 用 4 个数表达姿态，单位四元数满足 w² + x² + y² + z² = 1。它的优势是：无奇异、插补平滑（SLERP）、复合只需四元数乘法、数值误差可通过归一化简单修正。缺点是符号二义性（q 与 -q 表示同一姿态），在插补前必须做符号对齐。

工程建议：内部计算用四元数或旋转矩阵，人机交互用欧拉角，存储与传输用四元数或齐次矩阵。三者之间需要可靠的转换函数，且转换后必须校验。

三、数值稳定性问题与代码实现

姿态运算的误差主要来自三处：连乘累积、三角函数精度、以及归一化缺失。以下是一段可运行的 Python 示例，演示四元数归一化、旋转矩阵正交化与欧拉角转换的稳定性处理：

import numpy as np

def quat_normalize(q):
    n = np.linalg.norm(q)
    if n < 1e-12:
        raise ValueError("degenerate quaternion")
    return q / n

def quat_to_matrix(q):
    w, x, y, z = quat_normalize(q)
    return np.array([
        [1-2*(y*y+z*z), 2*(x*y-w*z),   2*(x*z+w*y)],
        [2*(x*y+w*z),   1-2*(x*x+z*z), 2*(y*z-w*x)],
        [2*(x*z-w*y),   2*(y*z+w*x),   1-2*(x*x+y*y)]])

def orthonormalize(R):
    # SVD 投影到最近的正交矩阵
    U, _, Vt = np.linalg.svd(R)
    Rn = U @ Vt
    if np.linalg.det(Rn) < 0:
        U[:, -1] *= -1
        Rn = U @ Vt
    return Rn

def matrix_to_euler_zyx(R):
    # 内旋 Z-Y-X，返回 (yaw, pitch, roll)
    R = orthonormalize(R)
    sy = -R[2, 0]
    sy = np.clip(sy, -1.0, 1.0)          # 防浮点越界
    pitch = np.arcsin(sy)
    if abs(sy) > 0.99999:                 # 接近死锁
        yaw = np.arctan2(-R[0, 1], R[1, 1])
        roll = 0.0
    else:
        yaw = np.arctan2(R[1, 0], R[0, 0])
        roll = np.arctan2(R[2, 1], R[2, 2])
    return yaw, pitch, roll

关键点有三：其一，np.clip 防止 arcsin 因浮点误差收到略大于 1 的输入而返回 NaN；其二，接近死锁时显式退化处理，把 roll 置零并只解出 yaw，避免角度跳变；其三，orthonormalize 用 SVD 把漂移的旋转矩阵拉回 SO(3)，比简单 Gram-Schmidt 更稳健。

四、变换链与验证

多轴平台的正解可写成一条链：T_base_tool = T_base_1 · T_1_2 · … · T_n_tool。每一步的旋转部分都应保持单位正交。工程上建议在每次正解后做一次残差检查：‖RᵀR − I‖_F < 1e-6，若超限则触发正交化。姿态插补时，先对相邻四元数做符号对齐（若点积为负则取反），再 SLERP，可避免末端姿态“绕远路”。

把上述约定落到代码里，建议封装一个 Pose 类，内部以四元数 + 平移向量存储，对外提供 to_matrix()、to_euler()、compose()、inverse() 四个方法，并在 compose() 后自动归一化。这样既保留了四元数的数值优势，又能在需要时无损转成齐次矩阵参与变换链计算。对于平台标定，最终交付的通常是 T_base_tool 的 4×4 矩阵或 6 维位姿向量 (x, y, z, rx, ry, rz)，其中旋转部分用 ZYX 内旋欧拉角表示，便于与示教器读数对照。