3. 医疗机器人运动学与视觉坐标系建模

在医疗机器人视觉导航中，真正决定“机器人能不能把器械准确送到病灶”的，往往不是单目或双目相机本身的分辨率，而是从影像到患者、从患者到机器人、从机器人到工具末端这一整条坐标变换链是否被正确建立与维护。本节围绕位姿表示、手眼关系、工具坐标系、患者坐标系与影像坐标系之间的变换链展开，并给出可落地的工程实现方式。

一、位姿表示：旋转与平移的统一描述

刚体位姿由旋转矩阵 R（3×3，正交且行列式为 +1）与平移向量 t（3×1）组成，齐次变换矩阵写作：

T = [ R  t ]
    [ 0  1 ]   (4×4)

点变换：p_B = T_AB · p_A
其中 T_AB 表示“把 A 坐标系下的点映射到 B 坐标系”

工程中要特别强调下标约定：T_AB 表示从 A 到 B 的变换，其逆为 T_BA = T_AB⁻¹。旋转部分常用四元数或旋转向量（轴角）存储，避免万向锁并便于插值；但进入变换链计算前统一转为 4×4 齐次矩阵，减少符号混乱。医疗场景中还需注意单位：平移通常用毫米（mm），角度用弧度，避免 mm 与 m 混用导致数量级错误。

二、手眼关系：eye-in-hand 与 eye-to-hand

手眼标定解决的是相机坐标系与机器人末端（或基座）之间的固定变换。常见两种构型：

• eye-in-hand：相机固定在末端法兰上，相机随工具运动，标定得到 T_cam_tool（相机→工具）；
• eye-to-hand：相机固定在外部支架上，标定得到 T_cam_base（相机→机器人基座）。

经典求解形式为 AX = XB：A 为相邻两次末端位姿的相对运动，B 为对应相机观测到的标定板相对运动，X 即待求手眼矩阵。工程上通常采集 15～25 组姿态差异明显的位姿，用 Tsai-Lenz 或 Park-Martin 方法求初值，再以重投影误差做非线性优化。判断标定质量时，重投影误差建议控制在 0.3～0.8 像素，平移残差小于 1 mm 量级（示例阈值，需按实际相机与工作距离调整）。

三、工具坐标系与工具中心点（TCP）

工具坐标系定义在器械末端，其原点即工具中心点 TCP。对穿刺针、内窥镜、超声探头等不同工具，TCP 位置与轴向不同。四点法标定 TCP 时，让工具尖端以 4 种以上姿态触碰同一固定参考点，求解 T_flange_tool。工具轴向（如针尖方向）通常取工具坐标系的 +Z，用于后续沿轴进针或视线对齐。

四、患者坐标系与影像坐标系

影像坐标系来自 CT/MR 或术中 C 臂，其原点与体素网格相关；患者坐标系则建立在患者身体或手术床上，常用解剖标志点或注册标记（fiducial）定义。二者通过影像-患者配准建立 T_img_patient。配准方式包括：

• 点对配准：至少 3 个非共线标记点，用 SVD 求刚体变换；
• 面配准：采集患者体表点云与影像分割表面做 ICP；
• 术中影像配准：C 臂正侧位与术前 CT 做 2D-3D 配准。

配准后常用靶点配准误差（TRE）评估，示例目标为 TRE < 2 mm；若标记点分布过于集中，即使 FRE 很小，TRE 也可能在远端放大，因此标记点应尽量包围手术区域。

五、完整变换链与工程实现

把上述环节串起来，从影像中的靶点 p_img 到机器人基座下的目标位姿，变换链为：

p_base = T_base_patient · T_patient_img · p_img
T_base_tool = T_base_patient · T_patient_img · T_img_tool
其中 T_img_tool 由影像规划给出工具在影像中的目标位姿

若为 eye-in-hand 构型，还需把相机观测纳入：T_base_cam = T_base_flange · T_flange_cam，再由 T_cam_tool 得到工具位姿。工程实现上建议维护一棵“变换树”，每个节点是一个坐标系，边是 4×4 矩阵，查询任意两坐标系变换时沿树路径连乘，避免手工推导出错。

实际部署时，建议把每个变换矩阵连同时间戳、来源（标定/配准/机器人正解）一起记录。机器人运动过程中，T_base_flange 由正运动学实时更新，而 T_flange_tool、T_cam_flange、T_patient_img 相对稳定，只需在标定或配准后更新。这样既能保证实时性，又能在出现偏差时快速定位是哪一段变换出了问题。

综上，医疗机器人视觉导航的坐标建模本质是维护一条从影像到工具的、可验证的变换链。位姿表示保证数学一致，手眼标定与工具标定保证末端精度，患者与影像配准保证解剖对应，工程实现则通过变换树、时间戳与端到端验证把误差控制在可接受范围内。