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

运动学建模是医疗机器人从“机械结构”走向“可编程运动”的桥梁。无论是腹腔镜持镜臂、骨科钻孔机器人，还是穿刺针定位机构，控制系统最终都要回答两个问题：给定各关节角度，末端执行器在患者坐标系中的位姿是什么；反过来，给定病灶靶点与进针方向，各关节应转到什么角度。前者是正运动学，后者是逆运动学，而DH参数、雅可比矩阵、奇异位形与工作空间分析则构成支撑这两类计算的完整工具链。

一、坐标系约定与DH参数

在医疗机器人中，常用基座坐标系{B}、末端工具坐标系{T}和患者/影像坐标系{P}。本节统一采用右手坐标系，关节角以逆时针为正，长度单位为毫米，角度单位为度。描述相邻连杆关系最经典的方法是标准DH（Denavit-Hartenberg）参数法，每个连杆用四个参数刻画：连杆长度a_i、连杆扭角α_i、关节偏距d_i、关节角θ_i。相邻两坐标系之间的齐次变换为：

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

展开为4×4矩阵：
[ cθ  -sθcα   sθsα   a·cθ ]
[ sθ   cθcα  -cθsα   a·sθ ]
[ 0    sα     cα     d    ]
[ 0    0      0      1    ]

以一个简化的三自由度穿刺定位臂为例：关节1为绕竖直轴的旋转关节，关节2为俯仰关节，关节3为沿针轴方向的移动关节。设a₁=0、α₁=90°、d₁=120 mm；a₂=200 mm、α₂=0、d₂=0；a₃=0、α₃=0、d₃为可变进针深度。这些数值属于教学示例，实际设备需以厂家标定结果为准。

二、正运动学：从关节角到末端位姿

正运动学通过连乘各连杆变换得到末端位姿：T_0^3 = T_1·T_2·T_3。若给定θ₁=30°、θ₂=45°、d₃=80 mm，可依次计算T₁、T₂、T₃并相乘，得到末端位置与姿态。实际工程中通常用Python的NumPy或ROS的tf2完成矩阵运算，避免手工累积误差。正运动学的价值在于：它是碰撞检测、可视化仿真和误差分析的基础，也是验证逆解是否正确的“金标准”。

三、逆运动学：从靶点到关节角

逆运动学求解的是已知T₀³求θ₁、θ₂、d₃。对于上述三自由度结构，可采用解析法：由末端位置(x,y,z)先求θ₁=atan2(y,x)，再在竖直平面内用几何关系求θ₂和d₃。解析法速度快、可枚举多解，适合实时控制；当结构复杂或存在冗余自由度时，则采用雅可比伪逆迭代等数值法。医疗场景中还需加入关节限位、避障和入针方向约束，因此逆解往往不是唯一解，而是带约束的可行解集合。

四、雅可比矩阵：速度与力的传递

雅可比矩阵J(q)描述关节速度到末端速度的线性映射：ẋ = J(q)·q̇，其中ẋ为末端六维速度（线速度+角速度），q̇为关节速度。对上述三自由度臂，J为6×3矩阵，前三行对应线速度，后三行对应角速度。雅可比矩阵的转置还联系着静力关系：τ = JT·F，即末端受到穿刺阻力F时，各关节需要输出的力矩τ。医疗机器人中，雅可比条件数常用于衡量当前位形下的运动灵巧度，条件数过大意味着接近奇异，控制精度会下降。

五、奇异位形与工作空间

奇异位形是雅可比矩阵降秩的位形，此时det(J)=0，末端在某些方向上失去运动能力，逆解可能无解或趋于无穷大。对穿刺臂而言，当θ₂接近0°或180°、连杆共线时即进入奇异区。工程上通常采取三种措施：在轨迹规划中避开奇异区、在奇异附近限制关节速度、或采用阻尼最小二乘伪逆（DLS）平滑过渡。

工作空间分为可达工作空间和灵巧工作空间。可达工作空间是末端能到达的所有位置集合，灵巧工作空间是末端能以任意姿态到达的位置集合。对医疗机器人，工作空间必须覆盖病灶区域并留有余量，同时不能与患者体表、手术床或影像设备发生干涉。实际分析时，可在关节限位范围内进行蒙特卡洛采样，统计末端点云分布，再结合患者CT/MR影像确定安全边界。

六、从模型到临床的检查要点

• 坐标系一致性：基座、工具、患者影像坐标系定义是否统一，旋转方向与正负号是否与图示一致。
• DH参数来源：参数应来自厂家标定或实测，不可直接套用近似值用于临床。
• 逆解约束：是否加入关节限位、速度限幅、避障和入针方向约束。
• 奇异判断：是否实时计算雅可比条件数或行列式，并设置降速或停止阈值。
• 工作空间验证：是否用采样点云与患者影像配准，确认靶点位于灵巧工作空间内。

运动学建模不是一次性计算，而是贯穿术前规划、术中导航和术后复盘的持续过程。只有把正逆解、雅可比、奇异与工作空间作为一个整体来理解和验证，医疗机器人才能在真实解剖环境中安全、准确地完成定位与操作。