2. 医疗机器人运动学建模

运动学建模是医疗机器人仿真与验证的“坐标系地基”。无论是腹腔镜器械末端在患者体内的位姿推算，还是内窥镜视角随关节运动的实时变化，最终都要落到同一套数学语言上：用刚体变换描述连杆之间的相对位姿，用DH参数把机械结构抽象成可计算的关节链，再通过正、逆运动学在关节空间与任务空间之间来回映射。本节围绕这条主线，结合手术器械与内窥镜的典型构型，说明建模时应如何定义坐标系、如何选取参数、如何用URDF/SDF把模型交给仿真器。

刚体变换：位姿的统一表达

刚体变换描述两个坐标系之间的相对位置与姿态，通常写成齐次变换矩阵 T = [R p; 0 1]，其中 R 是3×3旋转矩阵，p 是3×1平移向量。旋转部分可用绕固定轴或绕动轴的基本旋转复合得到，例如绕Z轴转θ的旋转矩阵为：

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

在医疗机器人中，约定必须先行明确：本文统一采用“父坐标系到子坐标系”的变换方向，即 T_parent_child 表示子坐标系在父坐标系中的位姿；旋转正方向按右手定则，绕轴逆时针为正。若某处改用“子到父”的逆变换，必须显式说明，否则后续DH参数与逆解会整体错位。平移向量 p 的物理含义是子坐标系原点在父坐标系中的坐标，这一点在定义器械尖端、内窥镜镜头中心等关键点时尤其重要。

DH参数：把机械结构变成可计算链

标准DH参数用四个量描述相邻连杆：连杆长度 a_i、连杆扭角 α_i、关节偏置 d_i、关节角 θ_i。对转动关节，θ_i 是变量；对移动关节，d_i 是变量。相邻变换可写为：

T_i = Rz(θ_i) · Tz(d_i) · Tx(a_i) · Rx(α_i)

以腹腔镜持镜臂为例，若采用6自由度串联结构，前三个关节决定镜头位置，后三个关节决定镜头姿态。假设一组教学示例参数（非实测，仅用于说明建模过程）：

把上表代入 T_i 并依次相乘，即可得到末端镜头坐标系相对基座的位姿。实际建模时，d_4 常对应器械插入深度，θ_4 对应器械自转，这两个量在手术操作中直接决定视野方向与器械朝向，必须与器械实际装配尺寸一致。

正逆运动学：从关节到末端，再从末端回关节

正运动学是“给定关节角，求末端位姿”，计算直接、无歧义，适合仿真中实时更新器械与内窥镜位置。逆运动学是“给定期望位姿，求关节角”，在医疗机器人中更关键，因为主手操作、路径规划、虚拟夹具都依赖逆解。逆解通常存在多解、无解或奇异问题：多解时需按“关节行程余量最大”“远离奇异位形”“避免器械与组织碰撞”等准则选解；无解时说明目标位姿超出工作空间，应提示操作者或触发安全限位。

以2自由度平面为例，若末端目标为 (x, y)，连杆长 L1、L2，则：

cosθ2 = (x² + y² - L1² - L2²) / (2·L1·L2)
θ2 = ±acos(cosθ2)
θ1 = atan2(y, x) - atan2(L2·sinθ2, L1 + L2·cosθ2)

两个θ2对应“肘上/肘下”两种构型。医疗场景中，若器械已进入体腔，通常优先选择不改变器械插入方向的解，避免末端在体内大幅摆动。

手术器械与内窥镜运动链建模

手术器械常见构型包括直杆器械、腕式器械和连续体器械。直杆器械可视为“插入+自转+偏转”的简化链；腕式器械在末端增加俯仰/偏航关节，自由度更高但逆解更复杂；连续体器械则需用分段常曲率模型描述，不能直接套用刚体DH链。内窥镜运动链通常包含镜体插入、旋转和镜头弯曲，建模时要区分“镜体坐标系”与“镜头坐标系”，因为视野方向由镜头坐标系决定，而插入深度由镜体坐标系沿轴位移决定。

一个实用的做法是：先建立基座→持镜臂→镜体→镜头的完整链，再在镜头坐标系上附加一个“视野锥”可视化节点，用于在仿真中检查视野是否覆盖目标组织。器械链同理，在器械尖端附加工具中心点（TCP），所有路径点和碰撞检测都基于TCP进行。

URDF/SDF描述：把模型交给仿真器

URDF用XML描述连杆、关节、惯性、碰撞体和视觉体，适合ROS生态；SDF在Gazebo中更常用，支持更丰富的物理与传感器配置。一个典型的URDF关节片段如下：

<joint name="joint2" type="revolute">
  <parent link="link1"/>
  <child link="link2"/>
  <origin xyz="0.3 0 0" rpy="0 0 0"/>
  <axis xyz="0 0 1"/>
  <limit lower="-1.57" upper="1.57" effort="50" velocity="1.0"/>
</joint>

其中 origin 对应DH变换中的平移与旋转，axis 定义关节转轴，limit 必须与真实器械行程一致。若URDF中的关节限位比实际宽松，仿真中器械可能“穿模”进入组织；若过紧，则逆解可行域被错误压缩。建议把DH参数表与URDF逐项对照，确保 a_i、d_i、α_i 与 origin 一致。

典型构型案例与验证思路

以“持镜臂+腹腔镜”为例，验证可分三步：第一步，给一组关节角，用正运动学算出镜头位姿，与URDF在仿真器中读出的位姿对比，误差应小于0.1mm/0.1°；第二步，给定目标视野中心，用逆解求关节角，再正解回代，检查末端位置误差；第三步，沿典型手术路径采样若干位姿，检查是否出现奇异、限位或碰撞。若某段路径逆解频繁无解，通常说明工作空间不足或DH参数与实际装配不符，应优先核对 d_4 与 a_2 等关键尺寸。