CN112353492A - 一种绳索驱动微器械外力间接检测模型的设计方法 - Google Patents

Abstract

本发明公开的一种绳索驱动微器械外力间接检测模型的设计方法，包括以下步骤：建立绳索驱动微器械系统完整动力学模型；以微器械系统完整动力学模型为基础，建立无外力情况下微器械位置估计器及其参数变化模型；对无外力情况下微器械位置估计器模型进行分段辨识，建立无外力情况下微器械位置估计器动态参数库；建立无外力情况下微器械位置估计器的参数自主选择模型；将无外力情况下微器械位置估计器参数自主选择模型的输出与有外力情况下微器械系统完整动力学模型输出的位置估计值做差，将差值看作外力引起的扰动建立绳索驱动微器械外力间接检测模型。本发明设计方法得到的检测模型大大提高绳索驱动微器械外力检测的精确度。

Description

一种绳索驱动微器械外力间接检测模型的设计方法

技术领域

本发明属于微创手术机器人技术领域，具体涉及一种绳索驱动微器械外力间接检测模型的设计方法。

背景技术

微器械的外力检测能力是实现手术机器人力感知和力反馈的基础，有助于让医生感知和施加正确的手术操作力，增加临场感。现有的手术机器人力检测能力主要分为直接检测和间接检测两种，考虑到手术微器械的消毒方式和耗材属性的影响，直接检测的方式很难在实际手术中应用，而现有的间接检测方式多半由驱动电流或者系统模型来间接估计，由于电流对应的分辨率及迟滞效应等因素的影响，外力检测精度不高，尤其是在系统启动和停止阶段具有较大误差和迟滞效应。通过模型估计的方法通常忽略间隙、摩擦等非线性因素的影响，对模型进行简化或者线性化处理导致绳索驱动微器械外力检测精度不高。因此，展开绳索驱动微器械精确的外力间接检测方法的研究至关重要。

发明内容

本发明的目的在于提供一种绳索驱动微器械外力间接检测模型的设计方法，设计的检测模型能够实现对绳索驱动微器械的外力间接检测。

本发明所采用的技术方案是：一种绳索驱动微器械外力间接检测模型的设计方法，包括以下步骤：

步骤1、建立绳索驱动微器械系统完整动力学模型；

步骤2、以微器械系统完整动力学模型为基础，建立无外力情况下微器械位置估计器及其参数变化模型；

步骤3、对无外力情况下微器械位置估计器模型进行分段辨识，建立无外力情况下微器械位置估计器动态参数库；

步骤4、建立无外力情况下微器械位置估计器的参数自主选择模型；

步骤5、将无外力情况下微器械位置估计器参数自主选择模型的输出与有外力情况下微器械系统完整动力学模型输出的位置估计值做差，将差值看作外力引起的扰动建立绳索驱动微器械外力间接检测模型。

本发明的特点还在于，

步骤1具体包括以下步骤：

步骤1.1、建立正、反向钢丝绳索动力学模型：

正向钢丝绳索动力学模型：

F_s1＝K_s1(y₁-x₁)+F_s10 (4)

式(4)中，K_s1是正向驱动钢丝绳索的弹性系数，y₁是正向驱动钢丝绳索与关节连接端的位移，x₁是正向驱动钢丝绳和滑块连接端的位移，F_s10是正向驱动钢丝绳索的初始张力；

反向钢丝绳索动力学模型：

F_s2＝K_s2(y₂-x₂)+F_s20 (3)

式(3)中，K_s2是反向驱动钢丝绳索的弹性系数，y₂是反向驱动钢丝绳索与关节连接端的位移，x₂是反向驱动钢丝绳和滑块连接端的位移，F_s20是反向驱动钢丝绳索的初始张力；

公式(3)和公式(4)中，F_s10＝F_s20＝F_k0＝F₀，其中F_k0是弹簧的初始张力，F₀是系统的初始张力；

步骤1.2、建立操作钳关节的动力学模型：

式(5)中，τ_e，J_e和r依次是操作钳关节驱动力矩，操作钳关节的等效转动惯量和操作钳关节的等效驱动半径；

依次是操作钳关节转角θ的角速度和角加速度，τ_ef是操作钳关节的摩擦力矩，由库伦摩擦力矩τ_efc和粘滞摩擦力矩τ_efv两部分组成，f_efc和f_efv依次为库伦摩擦力和粘滞摩擦力系数；

步骤1.3、由操作钳关节转角θ和关节两端绳索位移y₁，y₂的关系式

y₁＝y₂＝rθ (6)

得微器械系统完整动力学模型如式(8)所示：

式(8)中，K为弹性系数，K_s1＝K_s2＝K。

步骤2具体包括以下步骤：

步骤2.1、当微器械操作钳施加的外力为零时，即F_ext＝0，得到微器械位置估计器模型如下：

步骤2.2、将步骤2.1得到的微器械位置估计器模型描述为如公式(9)状态空间的形式：

公式(9)中，y为关节转角和角速度的输出，

令x＝x₁+x₂，

β＝2r_e，

将公式(10)变形得：

公式(11)中，参数R、Q和P都是与关节转角θ相关的时变参数；

步骤2.3、将式(9)描述为典型线性参数变化的二阶状态空间模型，如式(12)所示：

步骤3具体包括以下步骤：

步骤3.1、根据公式(12)得到x和关节转角θ之间的关系如式(13)所示：

步骤3.2、将全局的辨识实验过程按照时间序列离散成多个局部实验过程，将每个局部实验过程看作线性时不变系统进行参数辨识，将每个时间段内的线性时不变系统模型改为公式(14)所示的待辨识模型：

公式(14)中，Y为关节转角输出值，W为测量值，X为辨识值；

步骤3.3、通过加权最小二乘法进行辨识实验，辨识数据获取时，微器械末端转角通过安装在末端的微型磁编码器采集并通过滤波得到θ，并通过光栅检测位移x₁与x₂，同时通过微分得到

由此得到测量值W，并通过多次实验，获得测量的Y_i(i＝1,2,…,m)，W_i(i＝1,2,…,n)；因为待辨识量的最可能值是使各项实际观测值和计算值之间差的平方和最小，即使得

最小，由此得到模型参数R、Q和P；

步骤3.4、通过对每个局部实验过程进行参数辨识，得到多组辨识值X及关节角度值Y，并最终得到整个辨识实验过程的微器械位置估计器动态参数库。

步骤4具体包括：

步骤4.1、对公式(9)所示的无外力情况下微器械位置估计器的整个辨识实验过程采用加权最小二乘法得到位置估计器的参数，建立无外力情况下位置估计器初始数学模型，将位置估计器模型改写成以下待辨识形式：

Y＝WX+ρ (20)

公式(20)中：ρ为误差项，利用最小二乘法得到位置估计器模型的待辨识矩阵X中参数，建立无外力情况下位置估计器初始数学模型，并由此根据初始的位置估计器得到微器械末端位置的初始估计值；

步骤4.2、将末端位置的初始估计值θ和模型参数R、Q和P进行匹配，逐次将位置的初始估计值θ与参数库中角度值进行差值处理，为位置估计器选取差值最小的动态参数，并通过此参数自主选择方法，建立无外力情况下微器械位置估计器的参数自主选择模型。

步骤5具体包括：

将无外力情况下位置估计器参数自主选择模型的输出作为无外力情况下的位置估计值，结合有外力情况下微器械位置估计器模型的输出，即由公式(8)通过辨识实验获得的位置估计值，得到两个位置估计器模型输出的差值ΔP，将位置差值ΔP看作外力引起的扰动，建立如下外力与位置差值ΔP之间的关系：

最后，结合实验辨识的方式得到式(22)中的参数矩阵K、B和M。

本发明的有益效果是：本发明一种绳索驱动微器械外力间接检测模型的设计方法，将双位置估计器模型输出的差值看作外力所引起的扰动，并分析外力与位置差值之间的关系，可以大大提高绳索驱动微器械外力检测的精确度。

附图说明

图1是本发明一种绳索驱动微器械外力间接检测模型的设计方法基于的微器械腕部结构示意图；

图2是本发明一种绳索驱动微器械外力间接检测模型的设计方法流程图；

图3是本发明一种绳索驱动微器械外力间接检测模型的设计方法中位置估计器动态参数库设计原理图；

图4是本发明一种绳索驱动微器械外力间接检测模型的设计方法中位置估计器参数自主选择方法的结构示意图；

图5是本发明一种绳索驱动微器械外力间接检测模型的设计方法步骤5原理图。

具体实施方式

下面结合附图以及具体实施方式对本发明进行详细说明。

本发明提供了一种绳索驱动微器械外力间接检测模型的设计方法，如图2所示，包括以下步骤：

步骤1、绳索驱动微器械系统完整动力学建模；

步骤2、以微器械完整动力学模型为基础，建立无外力情况下位置估计器模型及位置估计器参数变化模型；

步骤3、对无外力情况下微器械位置估计器模型进行分段辨识，建立无外力情况下微器械位置估计器动态参数库；

步骤4、建立无外力情况下微器械位置估计器的参数自主选择模型；

步骤5、基于双位置估计器模型设计绳索驱动微器械外力间接检测方法。

步骤1的具体过程如下：

基于局部分析到整体集成的思想，首先考虑微器械的工作空间要求，设计行之有效的弹性绳索驱动机构。在考虑微器械关节布局和耦合作用的基础上，对绳索驱动微器械的各个部件建模，特别考虑到微器械的刚度特性、系统非线性摩擦等因素的影响，采取局部模型分解和整体重构的方法分析各部分之间模型的关系，最后结合各个部件模型，建立绳索驱动微器械系统完整的动力学模型。

如图1所示，四自由度微器械腕部具有三个关节，包含一个俯仰关节，两个偏摆关节(操作钳A和操作钳B)。每个关节的驱动力矩由关节两端钢丝绳索张力差决定，其中开合自由度和偏摆自由度的驱动力矩由操作钳A和操作钳B关节的驱动力矩共同决定，整个腕部及其驱动部件作为一个整体由一个旋转关节来驱动。

当单关节原理样机系统处于初始状态，末端转角处于零位，没有外力施加在操作钳上，弹性钢丝绳索处于静止状态，正向驱动钢丝绳索的初始张力F_s10和反向驱动端钢丝绳索的初始张力F_s20以及弹簧的初始张力F_k0的关系如式(1)所示，其中F₀是系统的初始张力。

F_s10＝F_s20＝F_k0＝F₀ (1)

反向驱动钢丝绳索的动力学模型通常被看做是一个质量弹簧阻尼系统，则钢丝绳索的动力学模型可以表示为式(3)。其中K_s2是反向驱动钢丝绳索的弹性系数，y₂是反向驱动钢丝绳索与关节连接端的位移，x₂是反向驱动钢丝绳和滑块连接端的位移。

和

分别是钢丝绳两端的速度，B_s2是反向驱动钢丝绳索的阻尼系数。

和

分别是钢丝绳两端的加速度，m₂是反向驱动端钢丝绳索的质量。

由于钢丝绳索的质量很小，这就导致反向驱动钢丝绳索的阻尼系数B_s2也偏小，而绳索的移动速度也比较小，所以上述模型的惯性项

和阻尼项

可以忽略不计。简化后的反向钢丝绳索动力学模型如式(3)所示。

F_s2＝K_s2(y₂-x₂)+F_s20 (3)

同理可得正向钢丝绳索动力学模型如式(4)所示。

F_s1＝K_s1(y₁-x₁)+F_s10 (4)

其中K_s1是正向驱动钢丝绳索的弹性系数，y₁是正向驱动钢丝绳索与关节连接端的位移，x₁是正向驱动钢丝绳和滑块连接端的位移。

单关节原理样机的操作钳关节的动力学模型如式(5)所示，其中：θ，τ_e，J_e和r分别是操作钳关节转角，操作钳关节驱动力矩，关节的等效转动惯量和操作钳关节的等效驱动半径。

分别是操作钳关节转角的角速度和角加速度，τ_ef是操作钳关节的摩擦力矩，可以看做由库伦摩擦力矩τ_efc和粘滞摩擦力矩τ_efv两部分组成，f_efc和f_efv分别为库伦摩擦力和粘滞摩擦力系数。

微器械关节转角θ和微器械关节两端绳索位移y₁，y₂的关系如下所示。

y₁＝y₂＝rθ (6)

由于正向驱动钢丝绳索和反向驱动钢丝绳索的材料和长度都相同，所以它们的弹性系数相等。

K_s1＝K_s2＝K (7)

结合公式(1)到(7)我们可以得到微器械关节的动力学模型如式(8)所示。

步骤2的具体过程如下：

在深入分析分析微器械完整动力学基础上，采用全局分离与全局重构的方法构建钢丝绳索位移和微器械末端位置的关系，设计无外力情况下微器械位置估计器模型。此外考虑到间隙，摩擦等其他未建模因素的影响，建立无外力情况下位置估计器参数变化模型。

当微器械操作钳施加的外力为零时(F_ext＝0)，可以得到位置估计器模型如下表达式。

该模型实际上是一个非线性模型，可以看作一个线性参数变化(LPV)模型，模型可以描述为以下状态空间的形式。

公式(9)中，y为关节转角和角速度的输出，

令x＝x₁+x₂，

β＝2r_e，

将公式(10)变形得：

公式(11)中，参数R、Q和P都是与关节转角θ相关的时变参数；

可以将式(9)描述为典型线性参数变化的二阶状态空间模型，如式(11)所示：

步骤3的具体过程如下：

根据公式(12)可以得到x和微器械关节转角θ之间的关系如式(13)所示。

参数R、Q和P都是与关节转角相关的时变参数，但是该线性参数变化模型在一小段时间段内可以看作线性时不变系统，此时参数R、Q和P可以看做常数。将全局的辨识实验过程按照时间序列离散成很多个局部实验过程，每个局部的实验过程可以近似的看作线性时不变(LTI)系统，整个辨识实验过程(包含微器械整个工作空间)可以被认为是一个准线性参数变化过程，结合加权最小二乘法对每个线性时不变(LTI)系统进行参数辨识。

将每个时间段内的LTI模型改为式(14)所示的待辨识模型：

公式(14)中，Y为关节转角输出值，W为测量值，X为辨识值；

通过加权最小二乘法进行辨识实验，辨识数据获取时，微器械末端转角通过安装在末端的微型磁编码器采集并通过滤波得到θ，并通过光栅检测位移x₁与x₂，同时通过微分得到

由此得到W，并通过多次实验，获得测量的Y_i(i＝1,2,…,m)，W_i(i＝1,2,…,n)。因为待辨识量的最可能值是使各项实际观测值和计算值之间差的平方和最小，即使得

最小，由此得到模型参数R、Q和P。

通过对每个局部的实验过程进行参数辨识，得到多组辨识值X及关节角度值Y，并最终得到整个辨识实验过程的微器械位置估计器动态参数库。微器械位置估计器动态参数库设计原理框图如图3所示。

步骤4的具体过程如下：

对公式(9)所示的无外力情况下微器械位置估计器的整个辨识实验过程采用加权最小二乘法得到位置估计器的参数，建立无外力情况下位置估计器初始数学模型。

将位置估计器模型改写成以下待辨识形式：

Y＝WX+ρ (20)

其中：ρ为误差项，其他矩阵中参数和数学变形可以经过传感器得到，利用最小二乘法得到位置估计器模型的待辨识矩阵X中参数。待辨识矩阵X中参数确定后，可以建立无外力情况下位置估计器初始数学模型，并由此可以根据初始的位置估计器得到微器械末端位置的初始估计值。

最后，将末端位置的初始估计值θ和模型参数R、Q和P进行匹配，逐次将位置的初始估计值θ与参数库中角度值进行差值处理，为位置估计器快速选取合适的动态参数，原理框图如图4所示，并通过此参数自主选择方法，建立无外力情况下微器械位置估计器的参数自主选择模型。

步骤5具体如下：

将无外力情况下位置估计器参数自主选择模型的输出当作无外力情况下的位置估计值，结合有外力情况下微器械位置估计器模型的输出，即由公式(8)通过辨识实验获得的位置估计值，得到两个位置估计器模型输出的差值ΔP。将位置差值ΔP看作外力引起的扰动，建立如下外力与误差ΔP之间关系：

最后，结合实验辨识的方式得到式(22)中的参数矩阵K、B和M。基于双位置估计器模型的微器械外力检测方法原理框图如图5所示。

Claims (6)

1.一种绳索驱动微器械外力间接检测模型的设计方法，其特征在于，包括以下步骤：

步骤1、建立绳索驱动微器械系统完整动力学模型；

步骤2、以微器械系统完整动力学模型为基础，建立无外力情况下微器械位置估计器及其参数变化模型；

步骤3、对无外力情况下微器械位置估计器模型进行分段辨识，建立无外力情况下微器械位置估计器动态参数库；

步骤4、建立无外力情况下微器械位置估计器的参数自主选择模型；

步骤5、将无外力情况下微器械位置估计器参数自主选择模型的输出与有外力情况下微器械系统完整动力学模型输出的位置估计值做差，将差值看作外力引起的扰动建立绳索驱动微器械外力间接检测模型。

2.如权利要求1所述的一种绳索驱动微器械外力间接检测模型的设计方法，其特征在于，所述步骤1具体包括以下步骤：

步骤1.1、建立正、反向钢丝绳索动力学模型：

正向钢丝绳索动力学模型：

F_s1＝K_s1(y₁-x₁)+F_s10 (4)

式(4)中，K_s1是正向驱动钢丝绳索的弹性系数，y₁是正向驱动钢丝绳索与关节连接端的位移，x₁是正向驱动钢丝绳和滑块连接端的位移，F_s10是正向驱动钢丝绳索的初始张力；

反向钢丝绳索动力学模型：

F_s2＝K_s2(y₂-x₂)+F_s20 (3)

式(3)中，K_s2是反向驱动钢丝绳索的弹性系数，y₂是反向驱动钢丝绳索与关节连接端的位移，x₂是反向驱动钢丝绳和滑块连接端的位移，F_s20是反向驱动钢丝绳索的初始张力；

公式(3)和公式(4)中，F_s10＝F_s20＝F_k0＝F₀，其中F_k0是弹簧的初始张力，F₀是系统的初始张力；

步骤1.2、建立操作钳关节的动力学模型：

式(5)中，τ_e，J_e和r依次是操作钳关节驱动力矩，操作钳关节的等效转动惯量和操作钳关节的等效驱动半径；

依次是操作钳关节转角θ的角速度和角加速度，τ_ef是操作钳关节的摩擦力矩，由库伦摩擦力矩τ_efc和粘滞摩擦力矩τ_efv两部分组成，f_efc和f_efv依次为库伦摩擦力和粘滞摩擦力系数；

步骤1.3、由操作钳关节转角θ和关节两端绳索位移y₁，y₂的关系式

y₁＝y₂＝rθ (6)

得微器械系统完整动力学模型如式(8)所示：

式(8)中，K为弹性系数，K_s1＝K_s2＝K。

3.如权利要求2所述的一种绳索驱动微器械外力间接检测模型的设计方法，其特征在于，所述步骤2具体包括以下步骤：

步骤2.1、当微器械操作钳施加的外力为零时，即F_ext＝0，得到微器械位置估计器模型如下：

步骤2.2、将步骤2.1得到的微器械位置估计器模型描述为如公式(9)状态空间的形式：

公式(9)中，y为关节转角和角速度的输出，

令x＝x₁+x₂，

将公式(10)变形得：

公式(11)中，参数R、Q和P都是与关节转角θ相关的时变参数；

步骤2.3、将式(9)描述为典型线性参数变化的二阶状态空间模型，如式(12)所示：

4.如权利要求3所述的一种绳索驱动微器械外力间接检测模型的设计方法，其特征在于，所述步骤3具体包括以下步骤：

步骤3.1、根据公式(12)得到x和关节转角θ之间的关系如式(13)所示：

步骤3.2、将全局的辨识实验过程按照时间序列离散成多个局部实验过程，将每个局部实验过程看作线性时不变系统进行参数辨识，将每个时间段内的线性时不变系统模型改为公式(14)所示的待辨识模型：

公式(14)中，Y为关节转角输出值，W为测量值，X为辨识值；

步骤3.3、通过加权最小二乘法进行辨识实验，辨识数据获取时，微器械末端转角通过安装在末端的微型磁编码器采集并通过滤波得到θ，并通过光栅检测位移x₁与x₂，同时通过微分得到

由此得到测量值W，并通过多次实验，获得测量的Y_i(i＝1,2,…,m)，W_i(i＝1,2,…,n)；因为待辨识量的最可能值是使各项实际观测值和计算值之间差的平方和最小，即使得

最小，由此得到模型参数R、Q和P；

步骤3.4、通过对每个局部实验过程进行参数辨识，得到多组辨识值X及关节角度值Y，并最终得到整个辨识实验过程的微器械位置估计器动态参数库。

5.如权利要求4所述的一种绳索驱动微器械外力间接检测模型的设计方法，其特征在于，所述步骤4具体包括：

步骤4.1、对公式(9)所示的无外力情况下微器械位置估计器的整个辨识实验过程采用加权最小二乘法得到位置估计器的参数，建立无外力情况下位置估计器初始数学模型，将位置估计器模型改写成以下待辨识形式：

Y＝WX+ρ (20)

公式(20)中：ρ为误差项，利用最小二乘法得到位置估计器模型的待辨识矩阵X中参数，建立无外力情况下位置估计器初始数学模型，并由此根据初始的位置估计器得到微器械末端位置的初始估计值；

步骤4.2、将末端位置的初始估计值θ和模型参数R、Q和P进行匹配，逐次将位置的初始估计值θ与参数库中角度值进行差值处理，为位置估计器选取差值最小的动态参数，并通过此参数自主选择方法，建立无外力情况下微器械位置估计器的参数自主选择模型。

6.如权利要求5所述的一种绳索驱动微器械外力间接检测模型的设计方法，其特征在于，所述步骤5具体包括：

将无外力情况下位置估计器参数自主选择模型的输出作为无外力情况下的位置估计值，结合有外力情况下微器械位置估计器模型的输出，即由公式(8)通过辨识实验获得的位置估计值，得到两个位置估计器模型输出的差值ΔP，将位置差值ΔP看作外力引起的扰动，建立如下外力与位置差值ΔP之间的关系：

最后，结合实验辨识的方式得到式(22)中的参数矩阵K、B和M。

Images