CN120245007B

CN120245007B - 一种手术机器人术中患者侧力估计方法

Info

Publication number: CN120245007B
Application number: CN202510734752.9A
Authority: CN
Inventors: 吴新宝; 赵春鹏; 曹奇勇; 朱罡
Original assignee: Beijing Jishuitan Hospital Affiliated To Capital Medical University
Current assignee: Beijing Jishuitan Hospital Affiliated To Capital Medical University
Priority date: 2025-06-04
Filing date: 2025-06-04
Publication date: 2025-08-26
Anticipated expiration: 2045-06-04
Also published as: LU602211B1; CN120245007A

Abstract

本发明涉及手术机器人控制技术领域，公开了一种手术机器人术中患者侧力估计方法，包括：构建手术机器人患者侧机械臂在自由空间的动力学模型；将患者侧机械臂通过套管插入患者腹部体壁，采集关节数据，同时通过动力学模型预测当前时刻各关节的自由空间扭矩；将采集的各关节的位置、速度数据和自由空间扭矩作为输入数据，并将每一时刻各关节的实测扭矩与自由空间扭矩的残差作为标签数据，分别为每个关节单独训练一个神经网络模型；基于当前各关节的实测扭矩、自由空间扭矩以及预测的各关节额外扭矩，计算末端执行器与患者组织之间的接触力。本发明能够准确估计手术机器人末端执行器与组织之间相互作用力，提高手术的安全性和精确性。

Description

一种手术机器人术中患者侧力估计方法

技术领域

本发明涉及手术机器人控制技术领域，更具体地，涉及一种手术机器人术中患者侧力估计方法。

背景技术

在现代医疗技术领域，手术机器人因其能够提高手术的精确性和安全性而得到了广泛应用。这些机器人能够协助外科医生执行复杂的手术操作，尤其是在微创手术中，其优势更为明显。然而，现有手术机器人在触觉反馈方面存在显著的技术挑战，限制了手术的直觉性和安全性。

手术机器人（如达芬奇系统）在辅助微创手术中，医生需要精确感知末端执行器（包括手术工具）与患者组织（如腹腔内身体组织）间的相互作用力（如切割、缝合时的阻力），以提供触觉反馈，从而提升手术安全性和精准度。现有技术中，手术机器人的触觉反馈系统主要依赖于末端执行器力传感器或基于模型的动力学计算。力传感器方法虽然能够直接测量器械与组织之间的相互作用力，但存在成本高昂、难以灭菌以及在狭窄手术空间中难以集成等问题。基于模型的动力学计算方法通过建立机器人的动力学模型来估计相互作用力，但这些模型通常需要大量的实验数据进行校准，并且对机器人结构和环境变化较为敏感，难以适应不同的手术条件和器械。

此外，现有的患者侧力估计算法在处理机器人与套管之间的相互作用力时存在不足。套管是手术机器人进入患者体内的必要通道，通常在腹部手术中，套管通过一个小切口插入患者体壁，为手术器械提供进入体内的通道。在手术过程中，患者侧机械臂通过套管插入患者腹腔或其他手术部位，其末端安装的手术工具进行手术操作，但套管与患者侧机械臂之间接触产生的摩擦力和其他相互作用力会对末端执行器与组织之间的相互作用力估计产生显著影响，如套管摩擦和阻力作用于器械轴，通过机械臂的关节传递，最终影响末端执行器的运动精度和力反馈。现有技术难以准确补偿这些额外的力矩，导致末端手术工具与组织间的触觉反馈的准确性下降，进而影响医生手术的精确性和安全性。

发明内容

本发明的目的是提出一种手术机器人术中患者侧力估计方法，实现准确估计手术机器人末端执行器与组织之间相互作用力，提高手术的安全性和精确性，并为外科医生提供更可靠的触觉反馈。

为实现上述目的，本发明提出了一种手术机器人术中患者侧力估计方法，包括：

构建手术机器人患者侧机械臂在自由空间的动力学模型，所述动力学模型用于根据关节的位置、速度、加速度预测关节的自由空间扭矩；

将患者侧机械臂通过套管插入患者腹部体壁，在该过程中采集包含患者侧机械臂与套管交互的关节数据，包括各关节的位置、速度及关节实测扭矩；同时，通过所述动力学模型预测当前时刻各关节的自由空间扭矩；

将采集的各关节的位置、速度数据和各关节的自由空间扭矩作为输入数据，并将每一时刻各关节的实测扭矩与自由空间扭矩的残差作为标签数据，形成训练数据集，利用所述训练数据集分别为每个关节单独训练一个神经网络模型，以预测患者侧机械臂与套管交互引起的各关节额外扭矩；

在手术过程中末端执行器与患者组织接触时，基于当前各关节的实测扭矩、动力学模型预测的各关节的自由空间扭矩以及神经网络模型预测的各关节额外扭矩，计算末端执行器与患者组织之间的接触力。

可选地，所述构建手术机器人患者侧机械臂的动力学模型包括：

基于改进型DH参数法建立患者侧机械臂的运动学模型，定义每个关节的坐标系并使用齐次变换矩阵表示相邻关节间的位姿关系；

基于所述运动学模型，采用欧拉-拉格朗日方程构建包含连杆惯性、重力、关节摩擦及弹簧刚度的动力学模型；

通过结合优化的激励轨迹对所述动力学模型进行动态参数识别。

可选地，所述通过结合优化的激励轨迹对所述动力学模型进行动态参数识别包括：

基于傅里叶级数生成能够充分激发机器人动态特性的周期性激励轨迹，并基于关节位置、速度和加速度上下限的约束条件，采用凸优化方法优化激励轨迹的参数，以确保机器人在安全范围内运动；

控制患者侧机械臂在自由空间运行优化的激励轨迹，采集关节的位置、速度、加速度和关节实测扭矩数据；

基于采集的关节位置、速度、加速度构造回归矩阵，回归矩阵的每一行对应一个包含关节位置、速度和加速度数据的采样点；

对回归矩阵进行 QR 分解，得到基参数回归矩阵；

使用基参数回归矩阵和关节实测扭矩数据，利用加权最小二乘法求解基参数；所述基参数为动态参数的线性组合，其为描述机械臂动力学的最小参数集；

其中，所述动态参数包括连杆长度、连杆质量、连杆质心位置、连杆惯性张量、关节摩擦系数以及弹簧刚度。

可选地，所述利用加权最小二乘法求解基参数的目标函数为：

其中，为基参数回归矩阵，δ _b为基参数向量，为第i个采样点的关节实测扭矩，m为采样点数量，为权重系数，用于平衡各关节量级差异，。

可选地，所述神经网络模型为每个关节独立训练的多层感知机，所述多层感知机包括输入层、隐藏层和输出层，所述隐藏层包括两个全链接层，激活函数为ReLU。

可选地，所述神经网络模型的输入数据具体包括：历史时间窗口内的关节位置序列和速度序列，以及当前时刻动力学模型预测的当前关节的自由空间扭矩；

所述神经网络模型的输出为当前关节的补偿扭矩。

可选地，所述神经网络模型训练时，采用均方误差作为损失函数，并找到使损失函数最小化的模型参数作为训练目标。

可选地，各关节的实测扭矩与自由空间扭矩的残差计算公式为：

其中，为各关节实测扭矩的向量，包括每个关节电机实际测量的扭矩，为各关节自由空间扭矩的向量，包括动力学模型预测的每个关节在自由空间运动时的扭矩，，为基参数回归矩阵，为基参数向量。

可选地，计算末端执行器与患者组织之间的接触力的方法包括：

将实时采集的各关节的位置、速度、加速度输入所述动力学模型，通过所述动力学模型预测各关节的自由空间扭矩；

将实时采集的关节的位置、速度数据以及动力学模型预测的自由空间扭矩输入所述神经网络模型并输出预测的关节额外扭矩；

将神经网络模型输出的关节额外扭矩对关节自由空间扭矩进行补偿；

基于当前各关节的实测扭矩和补偿后的关节扭矩，结合空间雅可比矩阵的逆矩阵，计算末端执行器的笛卡尔力。

可选地，计算末端执行器的笛卡尔力的公式为：

其中，为末端执行器与患者组织的接触力向量，J为当前关节位置下的空间雅可比矩阵，为当前时刻的各关节实测扭矩向量，为动力学模型预测的各关节自由空间扭矩向量，为神经网络模型输出的补偿扭矩向量。

本发明的有益效果在于：

本发明首先构建手术机器人患者侧机械臂在自由空间（无接触）的动力学模型，利用动力学模型预测关节的自由空间扭矩，然后将患者侧机械臂通过套管插入患者体内并采集插入过程中的关节数据，包括关节位置、速度和关节实测扭矩，结合机械臂与套管交互作用时采集的关节数据和动力学模型计算出的自由空间关节扭矩数据，利用神经网络模型学习套管与患者侧机械臂相互作用产生的额外力矩，以对动力学模型预测的扭矩进行补偿，然后基于当前各关节的实测扭矩、动力学模型预测的各关节的自由空间扭矩以及神经网络模型预测的各关节额外扭矩，完成末端执行器与患者组织之间的接触力的估算，从而实现准确估计手术机器人末端执行器与组织之间相互作用力，为手术机器人提供精确的触觉反馈，提高手术的安全性和精确性。同时本发明通过结合动力学模型和神经网络学习的方法，在保证力估计精度的同时，相较于纯学习方法，降低了对训练数据覆盖范围的要求，提高了对不同手术场景和操作条件的适应能力。

进一步，本发明通过优化激励轨迹和QR分解提取基参数的方式完成动态参数辨识，提升了模型鲁棒性；采用分关节训练小规模神经网络模型，仅需少量术中数据实时补偿套管摩擦，实现了轻量化神经网络补偿；基于空间雅可比矩阵，将关节扭矩高效转换为末端笛卡尔力，实现了实时力映射，保证了术中触觉反馈的实时性，提高了医生手术的直觉性和安全性。

本发明的方法具有其它的特性和优点，这些特性和优点从并入本文中的附图和随后的具体实施方式中将是显而易见的，或者将在并入本文中的附图和随后的具体实施方式中进行详细陈述，这些附图和具体实施方式共同用于解释本发明的特定原理。

附图说明

通过结合附图对本发明示例性实施例进行更详细的描述，本发明的上述以及其它目的、特征和优势将变得更加明显，在本发明示例性实施例中，相同的参考标号通常代表相同部件。

图1示出了根据本发明的一种手术机器人术中患者侧力估计方法的流程图。

具体实施方式

下面将参照附图更详细地描述本发明。虽然附图中显示了本发明的优选实施例，然而应该理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了使本发明更加透彻和完整，并且能够将本发明的范围完整地传达给本领域的技术人员。

实施例

如图1所示，本实施例提供一种手术机器人术中患者侧力估计方法，包括：

S1：构建手术机器人患者侧机械臂在自由空间的动力学模型，所述动力学模型用于根据关节的位置、速度、加速度预测关节的自由空间扭矩；

本步骤具体包括：

S101：基于改进型DH参数法建立患者侧机械臂的运动学模型，定义每个关节的坐标系并使用齐次变换矩阵表示相邻关节间的位姿关系；

具体地，运动学模型描述机器人各关节位置与末端执行器位姿（位置和姿态）之间的关系，不涉及力和质量。其核心是坐标变换和雅可比矩阵。基于患者侧机械臂的结构特点，定义每个关节的坐标系。通常使用Denavit-Hartenberg（DH）参数来完成，本方法采用改进型DH参数可更好地适应患者侧机械臂的特定结构。

在一个示例中，利用改进型DH参数法，将每个关节i的坐标系通过以下四个参数定义：

相邻关节i−1到i坐标系的齐次变换矩阵为：

其中，Rot(z,θ _i)为当前关节绕前一关节z轴旋转角θ_i的旋转矩阵，Trans(z,d _i)为当前关节沿前一关节z轴平移距离d_i的平移矩阵，Trans(x,a _i)为连杆沿当前关节x轴平移距离a_i的平移矩阵，Rot(x,α _i)为连杆绕当前关节x轴旋转角α _i的旋转矩阵。

展开后的矩阵形式为：

然后基于齐次变换矩阵进行机械臂运动学链的构建，通过逐层计算齐次变换矩阵，最终得到末端执行器的精确位姿。例如，从基座（关节0）到末端执行器（关节n）的整体变换矩阵为：

从矩阵中提取平移部分得到末端位置（如第四列），从矩阵中提取旋转矩阵（如前三列）得到末端姿态。

而后，通过雅可比矩阵描述关节速度与末端速度的映射关系：

其中，为末端线速度和角速度向量，为空间雅可比矩阵，由各关节的运动旋量组成，为关节位置向量，为关节速度（角速度）向量。

改进型DH法更适用于串联机器人，参数定义更直观，计算误差更小。通过改进型DH参数和齐次变换矩阵，确保末端位姿计算的高精度。

S102：基于所述运动学模型，采用欧拉-拉格朗日方程构建包含连杆惯性、重力、关节摩擦及弹簧刚度的动力学模型；

具体地，本步骤建立机器人自身动态的精确模型（不含外部干扰）。动力学模型描述关节扭矩与运动（关节位置、关节速度、关节加速度）之间的关系，涉及惯性、重力、摩擦等物理效应。本方法使用欧拉-拉格朗日方程，推导包含惯性、重力、摩擦、弹簧力的扭矩方程。

欧拉-拉格朗日方程为：

其中，L为拉格朗日量，K为系统总动能，P为系统总势能。

动力学方程为：

展开后得到非线性动力学方程为：

其中，为关节扭矩（关节电机扭矩），为关节位置向量，为关节速度向量，为关节加速度向量，为惯性矩阵，描述机器人的质量分布和惯性特性，为科氏力矩阵，包含科氏力和离心力项；为重力项，由重力势能梯度计算得到，为摩擦项，通常建模为粘性摩擦和库仑摩擦的组合。

上述非线性动力学方程可转换为线性参数化形式：

其中，δ为动态参数，为回归矩阵，每行对应一个时间点的动力学方程，每列对应一个动态参数。

转换方法为：首先定义动态参数δ，包含所有影响动力学的物理量，动态参数δ包括连杆质量、质心位置、惯性张量、摩擦系数等。然后将非线性动力学方程中的每个非线性项展开为动态参数的线性组合，并将展开后的每一项系数提取为回归矩阵W的列，构造回归矩阵W。

将动力学方程重新组织为动态参数δ的线性组合，便于后续的参数辨识。

S103：通过结合优化的激励轨迹对所述动力学模型进行动态参数识别，具体如下：

S1031:基于傅里叶级数生成能够充分激发机器人动态特性的周期性激励轨迹，并基于关节位置、速度和加速度上下限的约束条件，采用凸优化方法优化激励轨迹的参数，以确保机器人在安全范围内运动；

具体地，采用傅里叶级数生成能够充分激发机器人动态特性的周期性轨迹，优选，目标是最小化回归矩阵W的条件数，数学表达式为：

其中，表示j关节坐标随时间的变化，即关节的位置随时间变化的函数；表示j关节坐标的偏移量或初始位置，是轨迹的基准起始点；表示第k 次谐波的正弦项幅值系数，决定了该谐波正弦部分对轨迹的贡献大小；表示第k 次谐波的余弦项幅值系数，决定了该谐波余弦部分对轨迹的贡献大小；k表示谐波的次数；ω表示基础频率，决定了轨迹的周期性变化速率；t为时间变量，表示轨迹随时间的变化；nH为谐波总次数。

其中，基础频率ω和谐波总次数nH需优化选择，优选基频为0.18Hz，谐波次数为6。

然后，基于关节位置、速度和加速度上下限的约束条件，采用凸优化方法（如采用凸优化工具CVXPY包结合SCS求解器）优化激励轨迹的幅值系数和，以确保机器人在安全范围内运动。

S1032:控制患者侧机械臂在自由空间（无外部接触）运行优化的激励轨迹，采集关节的位置、速度、加速度和关节实测扭矩数据；

其中，关节的位置、速度、加速度由机器人内置的编码器或其他位置/速度传感器测量得到。关节实测扭矩数据由机器人关节处的扭矩传感器测量得到。这些数据通过机器人操作系统进行采集和传输，用于后续的动力学参数识别和力矩估计过程。本步骤通过激励轨迹驱动机械臂运动，记录每个时刻的各关节位置、速度、加速度和实测扭矩。

S1033:基于采集的关节位置、速度、加速度构造回归矩阵，回归矩阵的每一行对应一个包含关节位置、速度和加速度数据的采样点；

具体地，构造回归矩阵W，基于动力学方程，每行对应一个采样点，元素由关节位置、速度、加速度计算得到，描述各动态参数对扭矩的贡献。

S1034:对回归矩阵进行QR分解，得到基参数回归矩阵；

具体地，对回归矩阵W进行列主元QR分解：

其中，P为置换矩阵，Q为正交矩阵，R为上三角矩阵的非零对角线元素指示独立列。

选择基参数，选取前b个线性无关列，构成基参数回归矩阵，其中，P _b是置换矩阵。基参数δ _b为原始参数 δ 的线性组合，满足，是描述系统的最小参数集，其中T表示转置。

通过QR分解剔除冗余参数，减少辨识复杂度。

S1035:使用基参数回归矩阵和关节实测扭矩数据，利用加权最小二乘法求解基参数；所述基参数为动态参数的线性组合，其为描述机械臂动力学的最小参数集；

其中，所述利用加权最小二乘法求解基参数的目标函数为：

通过最小二乘法拟合动态参数δ _b，使得模型预测扭矩尽可能接近实测扭矩，其中。

上述动力学建模过程为本领域现有，具体细节不再赘述。

通过本步骤S1精确分离机器人自身动态扭矩，为后续补偿提供基准。

S2：将患者侧机械臂通过套管插入患者腹部体壁，在该过程中采集包含患者侧机械臂与套管交互的关节数据，包括各关节的位置、速度及关节实测扭矩；同时，通过所述动力学模型预测当前时刻各关节的自由空间扭矩；

具体地，患者侧机械臂在插入套管并穿过腹部体模时，实时采集的关节状态和扭矩数据。插入过程中，仅患者侧机械臂与套管接触，而末端执行器并不与患者体内的组织接触，从而保证采集的关节状态和扭矩数据只包含患者侧机械臂与套环相互作用下产生的数据，将患者侧机械臂通过套管插入患者腹部体壁这个过程通常为4分钟左右（约48,000数据点），优选采样率为200Hz（窗口大小5），可生成约9,600个训练样本。在采集上述数据的同时，通过所述动力学模型根据当前时刻采集的各关节实测扭矩、位置、速度和加速度实时预测出当前时刻各关节的自由空间扭矩，用于后续神经网络模型的训练。

S3：将采集的各关节的位置、速度数据和各关节的自由空间扭矩作为输入数据，并将每一时刻各关节的实测扭矩与自由空间扭矩的残差作为标签数据，形成训练数据集，利用所述训练数据集分别为每个关节单独训练一个神经网络模型，以预测患者侧机械臂与套管交互引起的各关节额外扭矩；

具体地，本步骤中，采用动力学模型计算自由空间预测扭矩，关节的实测扭矩与自由空间扭矩的残差扭矩计算公式为：

其中，为关节实测扭矩的向量，包括每个关节电机实际测量的扭矩，为各关节自由空间扭矩的向量，包括动力学模型预测的每个关节在自由空间运动时的扭矩，，为基参数回归矩阵，为基参数向量。

所述神经网络模型为每个关节独立训练的多层感知机（MLP），所述多层感知机包括输入层、隐藏层和输出层，所述隐藏层包括两个全链接层，激活函数为ReLU。

所述神经网络模型的输入数据具体包括：历史时间窗口（如窗口大小为5）内的关节位置序列和速度序列，以及当前时刻动力学模型预测的当前关节的自由空间扭矩；所述神经网络模型的输出为预测的当前关节的补偿扭矩。

所述神经网络模型训练时，采用均方误差作为损失函数：，并找到使损失函数最小化的模型参数作为训练目标。

通过为每个关节单独训练一个神经网络模型，学习患者特异性套管摩擦模式，预测患者侧机械臂与套管交互引起的各关节额外扭矩，以对动力学模型预测的自由空间关节扭矩进行补偿，从而动态修正动力学模型预测误差。

S4：在手术过程中末端执行器与患者组织接触时，基于当前各关节的实测扭矩、动力学模型预测的各关节的自由空间扭矩以及神经网络模型预测的各关节额外扭矩，计算末端执行器与患者组织之间的接触力。

本步骤中，计算末端执行器与患者组织之间的接触力的方法包括：

其中，计算末端执行器的笛卡尔力的公式为：

其中，为当前时刻末端执行器与患者组织的接触力向量（包括XYZ方向的力和力矩），J为当前关节位置下的空间雅可比矩阵，为当前时刻的各关节实测扭矩向量，为动力学模型预测的各关节自由空间扭矩向量，为神经网络模型输出的补偿扭矩向量。

需要说明的是，实测扭矩为实际手术动作下各关节当前的真实扭矩，包含了维持基本姿态所需的扭矩（对应无外部干扰下维持姿态所需的扭矩，即关节自由空间扭矩）、抵抗套管摩擦等产生的套管交互扭矩（对应神经网络模型预测的补偿扭矩）以及末端执行器与患者组织接触时相互作用的产生扭矩，因此即为末端执行器与患者组织相互作用所产生的扭矩, 通过空间雅可比矩阵的逆矩阵即可将其转换为笛卡尔空间的力。

通过计算出的末端执行器与组织之间的准确的接触力提供术中触觉反馈，从而提高医生手术的直觉性和安全性。

本方法中采用的小规模神经网络模型的训练时间通常小于20秒，因此可支持术中快速部署。本方法采集的训练数据基于特定患者采集，训练后的神经网络模型可适用于对特定患者手术过程中的力矩补偿。由于不同患者的生理结构、组织硬度、体壁厚度等存在差异，这些差异会影响套管与机械臂之间的相互作用力。因此，针对不同患者，可在每次手术前，进行简短的数据采集过程（如4分钟左右），收集当前患者的特定数据，利用采集的数据对神经网络模型进行快速重新训练，以适应当前患者和手术条件，在手术过程中，使用重新训练后的模型实时估计机械臂与套管之间相互作用力引起的额外关节扭矩并对自由空间关节扭矩补偿，进而提高末端执行器力估计的准确性。

以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。

Claims

1.一种手术机器人术中患者侧力估计方法，其特征在于，包括：

2.根据权利要求1所述的手术机器人术中患者侧力估计方法，其特征在于，所述构建手术机器人患者侧机械臂的动力学模型包括：

3.根据权利要求2所述的手术机器人术中患者侧力估计方法，其特征在于，所述通过结合优化的激励轨迹对所述动力学模型进行动态参数识别包括：

基于采集的关节位置、速度、加速度数据构造回归矩阵，回归矩阵的每一行对应一个包含关节位置、速度和加速度数据的采样点；

对回归矩阵进行 QR 分解，得到基参数回归矩阵；

4.根据权利要求3所述的手术机器人术中患者侧力估计方法，其特征在于，所述利用加权最小二乘法求解基参数的目标函数为：

，

5.根据权利要求1所述的手术机器人术中患者侧力估计方法，其特征在于，所述神经网络模型为每个关节独立训练的多层感知机，所述多层感知机包括输入层、隐藏层和输出层，所述隐藏层包括两个全链接层，激活函数为ReLU。

6.根据权利要求5所述的手术机器人术中患者侧力估计方法，其特征在于，所述神经网络模型的输入数据具体包括：历史时间窗口内的关节位置序列和速度序列，以及当前时刻动力学模型预测的当前关节的自由空间扭矩；

所述神经网络模型的输出为当前关节的补偿扭矩。

7.根据权利要求6所述的手术机器人术中患者侧力估计方法，其特征在于，所述神经网络模型训练时，采用均方误差作为损失函数，并找到使损失函数最小化的模型参数作为训练目标。

8.根据权利要求4所述的手术机器人术中患者侧力估计方法，其特征在于，各关节的实测扭矩与自由空间扭矩的残差计算公式为：

，

9.根据权利要求8所述的手术机器人术中患者侧力估计方法，其特征在于，计算末端执行器与患者组织之间的接触力的方法包括：

10.根据权利要求9所述的手术机器人术中患者侧力估计方法，其特征在于，计算末端执行器的笛卡尔力的公式为：

，

其中，为末端执行器与患者组织的接触力向量，J为当前关节位置下的空间雅可比矩阵，T表示转置，为当前时刻的各关节实测扭矩向量，为动力学模型预测的各关节自由空间扭矩向量，为神经网络模型输出的补偿扭矩向量。