3. 医疗机器人动力学建模
运动学回答“末端在哪里”，动力学回答“要施加多大力、电机输出多大扭矩”。医疗机器人对动力学的要求比工业机器人更苛刻：器械末端往往只有几毫米，却要在跳动的心脏、搏动的血管或柔软肝脏上保持稳定接触力；力控带宽不足会直接表现为组织穿刺过深或缝合撕裂。本节围绕两条主线展开——牛顿-欧拉与拉格朗日两种建模方法、摩擦与柔顺效应、组织交互力建模、参数辨识，以及仿真与实测的对比验证。
一、牛顿-欧拉与拉格朗日：两种等价路径
牛顿-欧拉法从单个连杆的力/力矩平衡出发，沿基座到末端做一次前向递推求速度与加速度，再从末端到基座做一次后向递推求关节力矩，得到形如 τ = M(q)q̈ + C(q,q̇)q̇ + G(q) 的模型。它的物理意义直观，计算复杂度为 O(n)，适合实时控制。拉格朗日法则从系统动能 K 与势能 P 出发，用 L = K − P，通过 d/dt(∂L/∂q̇) − ∂L/∂q = τ 得到同一组方程。它不需要逐杆分析内力，但需要处理大量偏导，符号推导繁琐，适合离线建模与理论分析。
两者结果完全等价，差别在工程实现。以 6 自由度串联机械臂为例，牛顿-欧拉递推在嵌入式控制器上单周期约几十微秒，而拉格朗日展开后的 M、C、G 矩阵若用符号表达式直接计算，单次求值可能达到毫秒级。因此实际力控系统通常用牛顿-欧拉做在线计算，用拉格朗日做模型结构验证。需要注意：无论哪种方法，都必须先明确坐标系与正负号约定。本节统一采用“关节角 q 以逆时针为正、末端接触力以压向组织为正、重力沿 −z 方向”的约定，全文与图示保持一致。
二、摩擦与柔顺：被忽略却决定力控精度
理想模型假设关节力矩等于指令力矩，但真实关节存在摩擦与传动柔顺。摩擦通常用 LuGre 或 Stribeck 模型描述：低速时静摩擦占主导，速度升高后过渡到库仑摩擦，再加上与速度成正比的黏性项。以某 6 轴机械臂第二关节为例，辨识得到 Fv ≈ 0.8 N·m·s/rad、Fc ≈ 1.2 N·m、Fs ≈ 0.6 N·m、vs ≈ 0.05 rad/s。若忽略摩擦，在 0.02 rad/s 的低速跟踪中，力矩误差可达 1.5 N·m 以上，末端力偏差超过 2 N，足以造成组织误伤。
柔顺效应来自谐波减速器、同步带和连杆弹性。可近似为关节弹簧：τ = K·Δq，其中 Δq 是电机侧与关节侧的角度差。K 越小，力控越“软”，但带宽下降。工程上常把柔顺与摩擦一起并入关节力矩补偿项，在控制周期内前馈补偿，使残余力矩误差控制在 0.1 N·m 量级。
三、组织交互力建模
组织不是刚体。穿刺、夹持、缝合三种操作对应不同接触模型。穿刺阶段可用 Hertz 接触：F = k·δ^n，δ 为压入深度，n 与接触几何有关，球形压头 n≈1.5，钝头器械 n≈1.1–1.3。夹持阶段需加入黏弹性：F = k·δ^n + c·δ̇，c 为阻尼系数，反映组织松弛。以猪肝为例，实验拟合得到 k ≈ 0.5 N/mm^n、n ≈ 1.2、c ≈ 0.08 N·s/mm。这些参数随组织类型、温度、灌注状态变化，必须现场辨识，不能直接套用文献值。
建模时还要区分“接触前”与“接触后”。接触前末端自由运动，动力学由机器人自身决定；接触后系统变为“机器人 + 组织”耦合，等效刚度下降，力控增益需相应调整。若仍用接触前的增益，会出现力超调与振荡。
四、动力学参数辨识
辨识的核心是把动力学方程写成关于参数向量的线性形式：τ = Y(q, q̇, q̈)·θ，其中 Y 是回归矩阵，θ 包含连杆质量、质心、惯量、摩擦系数等。采集 N 组激励轨迹数据，堆叠成 Y_stack·θ = τ_stack，用最小二乘或加权最小二乘求解 θ。激励轨迹应覆盖各关节运动范围，常用有限项傅里叶级数生成，避免共振频率。
实操中需注意三点：一是先做数据滤波，截止频率取关节带宽的 1/3–1/2，避免微分噪声放大；二是剔除静摩擦区间的数据，否则 θ 会被污染；三是用留出集验证，若预测力矩与实测力矩的均方根误差超过 5% 额定力矩，说明模型结构或激励不足。辨识完成后，把 θ 写回控制器前馈项，可显著降低跟踪误差。
五、仿真与实测对比
仿真与实测的偏差主要来自四类：模型简化（未建模柔顺、间隙）、参数误差（辨识不准）、传感噪声（力传感器零漂、编码器量化）、以及接触模型失配（组织非线性、滑动）。验证时不要只看末端轨迹，应同时对比关节力矩、末端接触力、以及接触力上升沿的响应时间。
一个可复现的验证流程是：在仿真中让器械以 2 mm/s 压入模拟组织，记录力-位移曲线；在实测中用相同速度、相同器械压入离体组织，记录同一曲线；把两条曲线叠在一起，观察三个指标——峰值力偏差、上升沿斜率偏差、卸载后的残余力。若峰值力偏差小于 10%、斜率偏差小于 15%、残余力小于 0.2 N，可认为模型可用于力控设计。若偏差超标，优先检查摩擦补偿与接触刚度参数，而不是直接调控制增益。