3. 刚体变换与位姿表示

在医疗机器人导航中，我们关心的核心问题始终是同一个：某个工具、标记或解剖结构，相对于某个参考坐标系，处于什么位置、什么姿态？位置是三维平移，姿态是三维旋转，二者合起来称为位姿（pose）。刚体变换就是描述位姿的数学工具——它保证物体在变换前后形状不变，只发生平移和旋转，这正是手术器械、患者解剖、影像坐标系之间关系的基本假设。

一、旋转矩阵：姿态的“坐标投影表”

旋转矩阵 R 是一个 3×3 正交矩阵，满足 RᵀR = I 且 det(R) = +1。它的每一列可以理解为“动坐标系三个轴在参考坐标系中的方向余弦”。例如，若工具坐标系绕参考系 Z 轴旋转角度 θ，则：

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

旋转矩阵的优点是无奇异、可直接复合：连续旋转只需矩阵相乘 R = R₁R₂，且能直接作用于向量 p' = R·p。缺点是 9 个元素中只有 3 个自由度，存在冗余约束，人眼难以直观读出“绕哪个轴转了多少度”，且多次相乘会累积浮点误差，需要定期用 SVD 或 Gram-Schmidt 正交化修正。

二、欧拉角：最直观也最易踩坑的姿态描述

欧拉角用三个角度描述姿态，常见约定有 ZYX（偏航-俯仰-滚转，即 yaw-pitch-roll）、XYZ、ZXZ 等。约定不同，同一组角度对应的姿态完全不同，工程中必须在接口文档中明确写出旋转顺序和内旋/外旋。以医疗机器人常用的 ZYX 内旋为例，姿态由绕 Z 转 yaw、绕新 Y 转 pitch、绕新 X 转 roll 复合而成。

欧拉角最大的陷阱是万向节死锁（gimbal lock）：当 pitch = ±90° 时，第一次和第三次旋转轴重合，丢失一个自由度，姿态解算会出现跳变。因此欧拉角适合人机交互显示和简单姿态设定，不适合作为内部连续运算和插值的表示。

三、四元数：姿态运算与插值的首选

单位四元数 q = (w, x, y, z)，满足 w²+x²+y²+z²=1，用 4 个数紧凑表示三维旋转，无奇异。绕单位轴 n 旋转角度 θ 的四元数为：

q = ( cos(θ/2),  n·sin(θ/2) )

四元数乘法对应旋转复合，共轭对应逆旋转，旋转向量时用 q ⊗ p ⊗ q*。它最大的工程价值在于球面线性插值（SLERP）：给定两个姿态 q₀、q₁，插值公式为

q(t) = sin((1-t)Ω)/sinΩ · q₀ + sin(tΩ)/sinΩ · q₁
其中 Ω = arccos(q₀·q₁)

SLERP 保证角速度均匀、路径最短，是机械臂关节轨迹、光学跟踪器姿态平滑、AR 导航箭头朝向过渡的标准做法。注意插值前要检查 q₀·q₁ 的符号，若为负需先取 -q₁，否则会绕远路旋转。

四、齐次变换：把旋转和平移装进一个矩阵

齐次变换矩阵 T 是 4×4 矩阵，把旋转 R、平移 t 和透视缩放统一表达：

T = [ R  t
      0  1 ]

点 p 的变换写作 p' = T·[p;1]。齐次变换的复合就是矩阵连乘，例如从影像坐标系到机器人基座坐标系的链路可写为：

T_base_image = T_base_robot · T_robot_tracker · T_tracker_image

这正是导航系统“坐标链”的核心。每个 T 都来自一次标定或跟踪测量，链路上任何一环误差都会向后传播。

五、位姿插值与误差传播

导航中经常需要在两个已标定位姿之间生成平滑过渡，例如机械臂从“术前规划入点”移动到“目标点”。位置用线性插值即可，姿态必须用 SLERP 或四元数归一化线性插值（NLERP）。若直接对欧拉角线性插值，在接近死锁区会出现姿态抖动甚至翻转。

误差传播是导航精度的关键。假设坐标链 T = T₁T₂T₃，每个环节带有小扰动 δξᵢ（6 维位姿扰动），则末端误差近似为：

δξ ≈ J₁δξ₁ + J₂δξ₂ + J₃δξ₃

其中 Jᵢ 是伴随矩阵（Adjoint），把上游误差映射到末端坐标系。一个直观结论是：旋转误差会随力臂放大为位置误差。例如跟踪器姿态误差 0.5°（约 0.0087 rad），若工具尖端距跟踪器 200 mm，则引入的位置误差约为 200 × 0.0087 ≈ 1.7 mm——这已经接近很多颅脑手术的精度阈值。因此工程上要尽量缩短“误差力臂”，把跟踪标记靠近工具尖端安装。

掌握这四种表示及其转换，是后续手眼标定、点云配准、光学跟踪和机械臂控制的基础。选择哪种表示，取决于当前任务是“给人看”“给算法算”还是“给控制器插值”——没有万能表示，只有与任务匹配的表示。