3. 医疗机械臂运动学与动力学建模

医疗机械臂的安全控制，本质上是在“知道末端在哪里、知道末端能多快、知道各关节要出多大力”这三件事上建立可信边界。运动学回答位置与姿态问题，雅可比矩阵回答速度与力的传递问题，动力学方程回答力矩与惯量问题。三者共同构成安全监控、碰撞检测、力限幅和轨迹校验的数学底座。本节以一台典型的6自由度协作医疗机械臂为对象，说明建模过程及其在安全控制中的直接用途。

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

正运动学解决的是“给定各关节角度，末端执行器在基坐标系中的位置和姿态是什么”。工程上通常采用改进DH参数法，为每个连杆定义四个参数：连杆长度 a、连杆扭角 α、关节偏置 d、关节角 θ。相邻坐标系之间的齐次变换矩阵为：

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

末端位姿：T_0_6 = T_1 · T_2 · T_3 · T_4 · T_5 · T_6

以一台教学用6自由度机械臂为例，假设其DH参数如下（示例参数，非实测）：关节1的 d=0.15 m，关节2的 a=0.30 m，关节3的 a=0.25 m，关节4至6主要决定姿态，末端工具长度 0.08 m。当给定一组关节角 q = [0°, 30°, -45°, 0°, 60°, 0°] 时，通过依次相乘六个齐次矩阵，可得到末端在基坐标系中的位置约为 (0.38, 0.12, 0.42) m，姿态用旋转矩阵或四元数表示。这个结果不是靠“感觉”得到的，而是每一步矩阵乘法可复现的。

在安全控制中，正运动学用于实时计算末端位置，并与预设的安全工作空间边界比较。如果末端位置超出允许区域，控制器立即触发减速或停止。此外，正运动学还用于碰撞检测：将机械臂各连杆简化为胶囊体或包围盒，通过正运动学得到每个连杆在空间中的实际位置，再与患者、手术床、医护人员等障碍物做距离计算。

二、逆运动学：从目标位姿反解关节角

逆运动学解决的是“给定末端目标位姿，各关节应该转到什么角度”。对于6自由度机械臂，逆解通常存在多组解，包括肩部左/右、肘部上/下、腕部翻转等构型。医疗场景中，必须从多组解中选择一组既满足关节限位、又远离奇异位形、且不会与患者发生碰撞的解。

逆运动学的求解方式主要有解析法和数值法。解析法速度快、可枚举全部解，适合结构满足Pieper条件的机械臂；数值法（如雅可比伪逆迭代、阻尼最小二乘）通用性强，但需要迭代，且可能收敛到不可用解。安全控制中，逆运动学不是“算出来就用”，而是要先做解的有效性筛选：

• 关节角是否在软限位范围内，并保留安全余量；
• 是否接近奇异位形，导致关节速度异常增大；
• 该构型下连杆是否与患者或设备干涉；
• 从当前位姿到目标位姿的关节空间路径是否连续、无跳变。

如果逆解不满足上述条件，控制器应拒绝该目标点或重新规划，而不是强行驱动。这是医疗机械臂与工业机械臂在安全策略上的重要区别。

三、雅可比矩阵：速度、力与奇异性的桥梁

雅可比矩阵 J(q) 描述关节速度与末端速度之间的线性映射：ẋ = J(q)·q̇。它同时建立了关节力矩与末端等效力之间的关系：τ = Jᵀ(q)·F。因此，雅可比矩阵是安全控制中“速度限幅”和“力限幅”的共同依据。

当机械臂接近奇异位形时，J(q) 降秩，其行列式趋近于零。此时即使末端速度很小，某些关节速度也可能趋于无穷大。安全控制器通常计算雅可比矩阵的最小奇异值 σ_min，并设定阈值。例如当 σ_min < 0.02 时，判定为接近奇异，限制末端最大速度或直接停止。另一种做法是采用阻尼最小二乘逆解，在奇异附近引入阻尼项，牺牲一点跟踪精度换取关节速度有界。

在力控制方面，当机械臂末端接触患者组织时，末端力 F 通过 Jᵀ 映射为各关节力矩。如果某个关节的力矩超过安全阈值，控制器可以判断为异常接触或碰撞，触发柔顺退让。因此，雅可比矩阵不仅是运动学量，也是安全力限幅的核心参数。

四、动力学方程：力矩、惯量与碰撞判断

动力学方程描述关节力矩与关节运动之间的关系。标准形式为：

M(q)·q̈ + C(q,q̇)·q̇ + G(q) = τ + τ_ext

其中 M(q) 是惯量矩阵，C(q,q̇) 包含科氏力和离心力项，G(q) 是重力项，τ 是关节驱动力矩，τ_ext 是外部接触力映射到关节的力矩。医疗机械臂在低速运动时，科氏力和离心力项相对较小，但重力项和惯量项不可忽略，尤其是当机械臂携带不同手术器械时，末端负载变化会直接改变 M(q) 和 G(q)。

动力学模型在安全控制中的核心用途是“力矩残差观测”。控制器根据当前关节角、角速度和角加速度，利用动力学方程计算理论驱动力矩 τ_model，再与实际测量力矩 τ_meas 比较，得到残差 r = τ_meas - τ_model。当残差超过阈值时，说明存在模型未解释的外部力，可能是碰撞、组织牵拉或器械卡滞。此时控制器可以触发柔顺退让或紧急停止。

需要注意的是，动力学模型不可能完全精确。摩擦、关节柔顺、线缆拖拽、负载参数误差都会造成残差。因此，实际系统中通常采用自适应阈值或带通滤波，避免把正常摩擦波动误判为碰撞。例如，在低速平稳段，残差阈值可设为 2 N·m；在加速段，阈值适当放宽到 4 N·m，并配合持续时间判断，只有残差持续超过阈值 50 ms 才触发停止。

五、三者在安全控制中的协同关系

正运动学提供“当前在哪里”，逆运动学提供“能不能去”，雅可比矩阵提供“速度与力如何传递”，动力学方程提供“需要多大力、是否遇到异常力”。四者不是孤立模块，而是安全控制流水线上的连续环节。一个典型的安全校验周期可以这样组织：

1. 读取当前关节角 q 和关节速度 q̇；
2. 用正运动学计算末端位姿，检查是否超出安全工作空间；
3. 用雅可比矩阵计算 σ_min，判断是否接近奇异，必要时限制速度；
4. 用动力学方程计算理论力矩，与实际力矩比较，判断是否有异常接触；
5. 若目标位姿来自规划器，用逆运动学求解并筛选有效构型；
6. 综合以上结果，输出正常运动、减速、停止或柔顺退让指令。

这套流程的实时性要求很高，通常需要在 1 ms 到 4 ms 的控制周期内完成。因此，正运动学和雅可比矩阵多采用解析式直接计算，动力学方程可采用递归牛顿-欧拉法提高效率，逆运动学则根据机械臂结构选择解析法或有限次迭代。

总结来说，运动学与动力学建模在医疗机械臂安全控制中承担的是“可计算的安全边界”角色。正运动学让边界可见，逆运动学让边界可达，雅可比矩阵让边界可控，动力学方程让边界可感。只有这四者协同工作，才能在手术、康复、辅助操作等场景中实现既灵活又可靠的安全保障。