2. 机器人位姿基础

在工业机器人TCP标定与精度验证中，所有测量、计算和误差补偿最终都归结为一个问题：工具中心点相对于机器人基坐标系到底处于什么位置、什么姿态？如果位姿描述不统一，后续的标定方程、误差模型和验证结果都会失去意义。本节从最基础的位姿表示出发，逐步建立旋转矩阵、欧拉角、四元数与齐次变换矩阵之间的完整知识链，为后续TCP标定算法打下数学基础。

一、位置与姿态：位姿的两个组成部分

机器人末端或TCP的位姿（Pose）由两部分构成：位置和姿态。位置描述一个点在参考坐标系中的坐标，通常用三维列向量表示：

P = [x, y, z]^T   （单位：mm 或 m）

姿态描述一个刚体坐标系相对于参考坐标系的朝向，它不关心原点在哪里，只关心三个坐标轴的方向。位置有3个自由度，姿态也有3个自由度，因此空间刚体共有6个自由度。工业机器人通常也是6轴，正是为了在可达空间内同时确定位置和姿态。

二、旋转矩阵：姿态的代数表示

旋转矩阵是一个3×3正交矩阵，其列向量分别表示动坐标系三个轴在参考坐标系中的方向余弦。若动坐标系为{B}，参考坐标系为{A}，则旋转矩阵记为 R_AB，满足：

R^T R = I,  det(R) = +1
P_A = R_AB · P_B

旋转矩阵的优点是运算规则清晰、可直接用于坐标变换和复合旋转；缺点是9个元素中只有3个独立参数，存在6个约束，存储冗余，且数值迭代后容易失去正交性，需要重新正交化。

三、欧拉角：直观但需约定顺序

欧拉角用三个角度描述姿态，例如绕Z、Y、X轴依次旋转的ZYX角，或绕X、Y、Z轴依次旋转的XYZ角。工业机器人中常见的有：

• RPY角：绕X轴Roll、绕Y轴Pitch、绕Z轴Yaw，常用于描述末端姿态；
• ZYZ欧拉角：常用于关节型机器人运动学推导；
• 轴角：绕某一单位轴旋转一个角度，是四元数的几何来源。

欧拉角最大的问题是万向节死锁：当中间角为±90°时，第一次和第三次旋转轴重合，姿态自由度退化，解算出现奇异。此外，不同旋转顺序得到的欧拉角完全不同，工程中必须明确约定顺序，否则会出现“同一姿态、不同角度”的混乱。

四、四元数：紧凑且无奇异的姿态表示

四元数用一个实部和三个虚部表示旋转：

q = [w, x, y, z],  且  w² + x² + y² + z² = 1

单位四元数可以表示任意三维旋转，且不存在万向节死锁。它只有4个参数、1个约束，比旋转矩阵紧凑；插值平滑，适合姿态过渡和SLAM、视觉伺服等场景。工业机器人控制器内部常用四元数进行姿态插补，再转换为旋转矩阵参与运动学计算。

五、齐次变换矩阵：位置与姿态的统一载体

齐次变换矩阵把旋转和平移统一到一个4×4矩阵中：

T = [ R  P ]
    [ 0  1 ]

T_AB = [ R_AB  P_AB ]
       [  0     1   ]

其中R为3×3旋转矩阵，P为3×1位置向量。齐次变换的复合非常简洁：

T_AC = T_AB · T_BC
T_BA = inv(T_AB)

在TCP标定中，机器人末端法兰到基座的变换、法兰到TCP的变换、传感器坐标系到基座的变换，全部用齐次变换矩阵串联。标定问题本质上就是求解一个未知的齐次变换矩阵T_TCP，使多次测量下的TCP位置一致。

六、四种姿态表示对比与选用建议

七、案例：TCP标定中的位姿链

假设机器人基座坐标系为{B}，法兰坐标系为{F}，TCP坐标系为{T}。一次接触测量中，TCP点在世界坐标系下的位置为：

P_B = T_BF · T_FT · [0,0,0,1]^T

其中T_BF由机器人正运动学给出，T_FT是待标定的未知齐次变换。若让TCP以不同姿态接触同一固定点，理论上P_B应保持不变。实际中由于T_FT估计不准，P_B会分散。标定算法就是寻找T_FT使这些分散点尽量重合。这里旋转矩阵负责姿态传递，位置向量负责平移传递，齐次变换矩阵把两者串成一条可计算的链。

八、常见误区与复盘

• 误区一：把欧拉角顺序当成默认。不同品牌机器人对RPY的定义可能不同，必须查阅控制器手册确认旋转顺序和旋转轴。
• 误区二：旋转矩阵不正交仍继续使用。长时间迭代后应重新正交化，否则位置误差会被放大。
• 误区三：四元数直接线性插值。应使用球面线性插值SLERP，否则角速度不均匀。
• 误区四：齐次变换矩阵求逆用普通矩阵求逆。应利用其结构快速求逆，避免数值误差。