2. 数学基础
在机械臂视觉定位工程中，坐标变换不是“调一个函数”那么简单。相机看到的是像素坐标，机械臂执行的是关节角度，而目标物体存在于三维世界坐标系中。要把这三者串起来，必须依赖一套统一的数学语言：向量描述位置与方向，矩阵描述线性变换，齐次坐标统一旋转与平移，旋转矩阵、欧拉角、四元数则分别从不同角度表达姿态。理解它们的表示方法与相互转换，是后续手眼标定、位姿求解和轨迹规划的基础。
一、核心原理：从向量到齐次坐标
向量是机器人学中最基本的量。三维位置向量 p = [x, y, z]ᵀ 表示点在参考系中的坐标；单位向量表示方向；两个向量的点积反映夹角，叉积生成垂直于二者的方向，常用于构建坐标系。矩阵则用于描述基变换：若 R 是 3×3 旋转矩阵，则 p' = R·p 表示同一点在旋转后坐标系中的表达。
问题在于，平移不能写成 3×3 矩阵乘法。于是引入齐次坐标：把三维点写成 [x, y, z, 1]ᵀ，方向向量写成 [x, y, z, 0]ᵀ。这样，旋转与平移可以合并为一个 4×4 齐次变换矩阵：
T = [ R t ]
[ 0 1 ]
p_h' = T · p_h
其中 R 为 3×3 旋转矩阵，t 为 3×1 平移向量。齐次坐标的工程价值在于：多次变换可以连续相乘，T_total = T_1 · T_2 · … · T_n，从而把“相机→末端→基座→世界”的链路统一成矩阵链。这也是 ROS 中 tf 树、URDF 关节变换和手眼标定方程的共同数学底座。
二、姿态表示：旋转矩阵、欧拉角与四元数
旋转矩阵 R 是 3×3 正交矩阵，满足 RᵀR = I 且 det(R) = 1。它无奇异性、便于复合，但用 9 个参数表达 3 个自由度，存在冗余，且数值积分后需重新正交化。欧拉角用三个角（如绕 Z-Y-X 的 yaw-pitch-roll）描述姿态，直观易读，但存在万向节死锁：当中间角为 ±90° 时，另外两个轴重合，失去一个自由度。四元数用 q = [w, x, y, z] 四个数表示旋转，单位四元数满足 ||q|| = 1，无死锁、插值平滑，适合姿态估计与 SLAM，但不够直观，且 q 与 -q 表示同一旋转。
工程选择建议：内部计算与优化优先用旋转矩阵或四元数；人机交互与参数配置用欧拉角；姿态插值与滤波用四元数；涉及多传感器融合时，四元数可避免欧拉角死锁带来的数值不稳定。
三、方法流程：位姿变换的完整链路
以“相机识别目标→机械臂抓取”为例，典型流程如下：
- 相机内参标定，得到像素坐标到相机坐标的映射；
- 通过深度或 PnP 求解，得到目标在相机坐标系下的位姿
T_cam_obj； - 手眼标定得到相机到末端的变换
T_ee_cam； - 由机器人正运动学得到末端到基座的变换
T_base_ee； - 计算目标在基座坐标系下的位姿：
T_base_obj = T_base_ee · T_ee_cam · T_cam_obj； - 逆运动学求解关节角，驱动机械臂到达目标位姿。
这条链路中，任何一步的旋转表示不一致，都会导致姿态误差。例如相机输出四元数，而机器人控制器要求欧拉角，就必须做正确转换，并注意旋转顺序与内旋/外旋定义。
四、案例与场景：手眼标定中的姿态转换
假设某六轴机械臂进行 eye-in-hand 手眼标定。相机固定在末端法兰上，标定板固定在工作台。采集多组位姿后，求解方程 T_ee_cam · T_cam_obj = T_base_ee⁻¹ · T_base_obj。若相机 SDK 输出四元数 q_cam，而机器人控制器使用 Z-Y-X 欧拉角，则需先转旋转矩阵，再转欧拉角：
import numpy as np
from scipy.spatial.transform import Rotation as R
q = [0.707, 0.0, 0.707, 0.0] # w, x, y, z
rot = R.from_quat(q)
euler_zyx = rot.as_euler('zyx', degrees=True)
print(euler_zyx) # [yaw, pitch, roll]若忽略旋转顺序，把 zyx 写成 xyz，抓取姿态可能整体偏转 90°，导致夹爪无法对准目标。这类错误在调试日志中常表现为“位置正确、姿态随机”。
五、常见误区与复盘清单
| 误区 | 表现 | 修正方法 |
|---|---|---|
| 欧拉角顺序混淆 | 姿态整体偏转，位置正确 | 确认内旋/外旋与轴序，统一用旋转矩阵中转 |
| 四元数未归一化 | 旋转矩阵出现缩放或畸变 | 使用前执行 q / ||q|| |
| 齐次坐标方向向量误写为 1 | 平移被错误叠加到方向 | 方向向量第四维写 0，点写 1 |
| 旋转矩阵未正交化 | 多次迭代后姿态漂移 | 定期 SVD 正交化或改用四元数 |
| 坐标系方向定义不一致 | 左右手系混用，叉积方向相反 | 统一采用右手系，明确 Z 轴朝向 |
排障提示：当机械臂“看得见但抓不准”时，优先检查三件事：旋转顺序是否一致、四元数是否归一化、齐次变换链的乘法顺序是否从基座到目标依次右乘。多数姿态异常并非算法错误，而是表示方法混用。
记忆口诀：位置用向量，变换用矩阵；旋转平移合并靠齐次；内部计算用矩阵或四元数，人机交互用欧拉角；链式变换从基座到目标依次相乘，方向向量第四维写 0。
掌握这些数学表示，并不是为了推导公式，而是为了在工程中快速判断：当前数据属于哪种表示、是否需要转换、转换后是否保持右手系与单位长度。只有表示统一，后续的视觉定位、手眼标定和轨迹规划才能稳定收敛。