CN112545652B - 微创手术机器人柔性丝传动手术器械高精度离线控制方法 - Google Patents

Abstract

微创手术机器人柔性丝传动手术器械高精度离线控制方法，涉及医疗机器人领域。为解决现有的微创手术机器人柔性丝传动手术器械控制方法控制精度有限，操作粗糙，使用不便的问题。采用简单便捷的方式进行手术器械的离线辨识，并且补偿准确度高。具体地说，本发明事先测量多把器械的滞回曲线，利用多项式拟合和极大似然估计得到该器械通用的滞回模型，再在实际使用之前用FCN网络对电机匀速往返运动时地电流数据进行辨识，准确地提取出回差大小以调整滞回模型的幅值，进而实现对手术器械的细腻控制。使用本发明时，只需要提前让致动电机匀速往返运动若干次即可，十分方便，且补偿细腻准确度高，使用方便。本发明适用于医疗机器人领域。

Description

微创手术机器人柔性丝传动手术器械高精度离线控制方法

技术领域

本发明涉及医疗机器人领域，具体涉及一种微创手术机器人柔性丝传动手术器械高精度离线控制方法。

背景技术

近年来，腔镜微创手术机器人的应用提高了手术效果，减轻了患者病痛。医生通过主操作手控制伸入患者腔体内的细长手术器械来实现各种手术操作。然而，由于紧凑设计，手术器械通常使用远距离致动柔性丝传动，进而导致致动端与输出端之间的运动存在迟滞现象，影响外科医生的精细操作。

目前业界主要通过“辨识-补偿”的思路来解决该问题。补偿方面，有直接补偿和基于模型补偿两种方式。直接补偿一般通过在器械回差阶段前馈位置信号来实现，此方法简单但比较粗糙，位置控制细腻度较差。而基于模型补偿测试按照器械滞回模型或者动力学模型来补偿回差，准确度较高。而辨识方面，有在线辨识与离线辨识两种方式。在线辨识一般通过卡尔曼滤波或者支持向量分类等智能算法借助实际使用时采集到的电机编码器读数和电机电流来判断当前是否处于滞回状态，借此辨识结果指导补偿算法进行回差补偿。这种方式无需事先辨识，便于使用，但由于实际使用中的负载变化不可预知，该类方法的辨识准确度仍不能满足要求。而离线辨识主要通过对手术器械的运动或者动力学特性进行建模，在实际使用之前事先利用复杂的参考轨迹来辨识模型或者使用额外的三维位置测量仪器(比如NDI光学定位系统)测量模型，进而对辨识好的这一把器械进行前馈补偿。这种方式虽然辨识准确度较高，但需要对每一把器械提前进行辨识，使用不方便。此外，由于加工装配误差的影响，每一把器械滞回曲线的形状和幅值略有不同，如果在实际使用中只使用对某一把器械测量的模型来进行补偿，会产生较大的误差。

总之，现有的微创手术机器人柔性丝传动手术器械控制方法控制精度有限，操作粗糙，使用不便的问题。

发明内容

本发明为解决现有的微创手术机器人柔性丝传动手术器械控制方法控制精度有限，操作粗糙，使用不便的问题，而提出一种微创手术机器人柔性丝传动手术器械高精度离线控制方法。

本发明的一种微创手术机器人柔性丝传动手术器械高精度离线控制方法，其具体步骤如下：

步骤一、微创手术机器人系统开始主从控制，在每个控制周期循环判断致动电机是否反向运动；

步骤二、致动电机反向运动后，判断反向运动后的电机转向，如果正向转动则令电机目标位置q_d

q_d＝q_d+ΔS₁ (1)

利用电机目标位置q_d来补偿机械间隙回差；

如果反向转动则令电机目标位置q_d

q_d＝q_d-ΔS₁ (2)

利用电机目标位置q_d来补偿机械间隙回差，式(1)和式(2)中ΔS₁是该手术器械的机械间隙回差；

步骤三、使用该手术器械的柔性丝形变回差ΔS₂和手术器械通用滞回模型f(x；μ_ω) (3a)得到反归一化的电机补偿曲线：

f^denorm(x^denorm；μ_ω)＝f(x/ΔS₂；μ_ω) (3)

式(3)中x是滞回模型的自变量，μ_ω是滞回模型的参数，x^denorm是归一化后的自变量；

步骤四、在开始柔性丝形变回差补偿之前记录电机初始位置

步骤五、电机目标位置：

通过令电机目标位置来补偿柔性丝形变回差；

步骤六、在每个控制周期循环判断柔性丝变形回差是否补偿结束，如果结束则通过令电机目标位置q_d＝q_d+f^denorm(1)来固定补偿值；

步骤七、跳回步骤一，不断重复；

进一步的，所述的步骤二中的手术器械的机械间隙回差ΔS₁的计算方法如下：

步骤二一、在主控控制开始之前，令致动电机以速度v匀速往返运动，幅值大小为A_m，采样周期为T_s，采集致动电机匀速往返运动时的电流数据；

步骤二二、从电流数据中截取长度为L的电流数据，并作为一维FCN神经网络的输入；

步骤二三、一维FCN神经网络利用长度为L的电流数据计算机械间隙周期大小C₁；

步骤二四、计算手术器械的机械间隙回差

ΔS₁＝C₁·T_s·v (5)，

式中C₁是步骤二三中的机械间隙周期大小，T_s是步骤二一中的采样周期，v是步骤二一中的致动电机速度；

进一步的，所述的步骤三中的手术器械的柔性丝形变回差ΔS₂的计算方案如下：在主控控制开始之前，令致动电机以速度v匀速往返运动，幅值大小为A_m，采样周期为T_s，采集致动电机匀速往返运动时的电流数据；从电流数据中截取长度为L的电流数据，并作为一维FCN神经网络的输入；一维FCN神经网络利用长度为L的电流数据计算柔性丝形变周期大小C₂；计算手术器械的柔性丝形变回差

ΔS₂＝C₂·T_s·v (6)，

式中C₂是柔性丝形变周期大小，T_s是步骤二一中的采样周期，v是步骤二一中的致动电机速度；

进一步的，所述的步骤三中的手术器械通用滞回模型f(x；μ_ω)的计算方法中，M是大于等于5的整数，K是大于等于2的整数，具体流程如下：

步骤三一、使用三维光学定位设备采集M个手术器械的滞回曲线；

步骤三二、从滞回曲线中分别提取柔性丝形变过程的滞回曲线

q_out＝ξ_i(q_in)(i∈{1,2,...,M}) (7)，

共M条，式(7)中q_out是手术器械末端位置，q_in是致动电机的位置；

步骤三三、使用线性缩放，将柔性丝形变过程的滞回曲线的横轴范围归一化到区间 [0,1]，得到归一化的柔性丝形变过程的滞回曲线

式中x是归一化滞回曲线的自变量，ξ_i是步骤三二中的滞回曲线，q_in是致动电机位置；

步骤三四、建立K次多项式函数

式中x是多项式函数的自变量，

是多项式函数的参数，并利用最小二乘法来拟合归一化的柔性丝形变过程的滞回曲线

得到M组多项式函数的参数ω_i(i∈{1,2,...,M})；

步骤三五、用参数θ＝{μ_ω,∑_ω}的高斯分布为步骤三四中的M组多项式函数的参数ω_i(i∈{1,2,...,M})建模，即ω～p(ω；θ)＝N(ω|θ)，式中θ是高斯分布的参数，μ_ω是高斯分布的均值，Σ_ω是高斯分布的方差；

步骤三六、利用极大似然估计计算θ＝{μ_ω,∑_ω}，即

式中M是手术器械的个数，ω_i是步骤三四中的M组多项式函数的参数；

步骤三七、令ω＝μ_ω，带入多项式函数f(x；ω)得到手术器械通用滞回模型f(x；μ_ω)；

进一步的，所述的电流数据中截取长度为L的电流数据中至少包含电机往返运动一个周期长度的数据。

本发明与现有技术相比具有以下有益效果：

本发明克服了现有技术的缺点，采用简单便捷的方式进行手术器械的离线辨识，并且补偿准确度高。具体地说，本发明事先测量多把器械的滞回曲线，利用多项式拟合和极大似然估计得到该器械通用的滞回模型，再在实际使用之前用FCN网络对电机匀速往返运动时地电流数据进行辨识，准确地提取出回差大小以调整滞回模型的幅值，进而实现对手术器械的细腻控制。使用本发明时，只需要提前让致动电机匀速往返运动若干次即可，十分方便，且补偿细腻准确度高，从而提高控制精度，使用方便。

附图说明

图1是本发明所述的一种微创手术机器人柔性丝传动手术器械高精度离线控制方法的流程图；

图2是本发明中离线辨识手术器械回差的流程图；

图3是本发明中获取手术器械通用滞回模型的流程图。

具体实施方式

具体实施方式一：结合图1说明本实施方式，本实施方式所述的一种微创手术机器人柔性丝传动手术器械高精度离线控制方法，其具体步骤如下：

步骤一、微创手术机器人系统开始主从控制，在每个控制周期循环判断致动电机是否反向运动；

步骤二、致动电机反向运动后，判断反向运动后的电机转向，如果正向转动则令电机目标位置q_d

q_d＝q_d+ΔS₁ (1)

利用电机目标位置q_d来补偿机械间隙回差；

如果反向转动则令电机目标位置q_d

q_d＝q_d-ΔS₁ (2)

利用电机目标位置q_d来补偿机械间隙回差，式(1)和式(2)中ΔS₁是该手术器械的机械间隙回差；

步骤三、使用该手术器械的柔性丝形变回差ΔS₂和手术器械通用滞回模型f(x；μ_ω) (3a)得到反归一化的电机补偿曲线：

f^denorm(x^denorm；μ_ω)＝f(x/ΔS₂；μ_ω) (3)

式(3)中x是滞回模型的自变量，μ_ω是滞回模型的参数，x^denorm是归一化后的自变量；

步骤四、在开始柔性丝形变回差补偿之前记录电机初始位置

步骤五、电机目标位置：

通过令电机目标位置来补偿柔性丝形变回差；

步骤六、在每个控制周期循环判断柔性丝变形回差是否补偿结束，如果结束则通过令电机目标位置q_d＝q_d+f^denorm(1)来固定补偿值；

步骤七、跳回步骤一，不断重复；

本具体实施方式，采用简单便捷的方式进行手术器械的离线辨识，并且补偿准确度高。具体地说，本发明事先测量多把器械的滞回曲线，利用多项式拟合和极大似然估计得到该器械通用的滞回模型，再在实际使用之前用FCN网络对电机匀速往返运动时地电流数据进行辨识，准确地提取出回差大小以调整滞回模型的幅值，进而实现对手术器械的细腻控制。使用本发明时，只需要提前让致动电机匀速往返运动若干次即可，十分方便，且补偿细腻准确度高，从而提高控制精度，使用方便。

具体实施方式二：结合图2说明本实施方式，本实施方式是对具体实施方式一所述的控制方法的进一步的限定，本实施方式所述的一种微创手术机器人柔性丝传动手术器械高精度离线控制方法，所述的步骤二中的手术器械的机械间隙回差ΔS₁的计算方法如下：

步骤二一、在主控控制开始之前，令致动电机以速度v匀速往返运动，幅值大小为A_m，采样周期为T_s，采集致动电机匀速往返运动时的电流数据；

步骤二二、从电流数据中截取长度为L的电流数据，并作为一维FCN神经网络的输入；

步骤二三、一维FCN神经网络利用长度为L的电流数据计算机械间隙周期大小C₁；

步骤二四、计算手术器械的机械间隙回差

ΔS₁＝C₁·T_s·v (5)，

式中C₁是步骤二三中的机械间隙周期大小，T_s是步骤二一中的采样周期，v是步骤二一中的致动电机速度；

本具体实施方式，利用图2中记载的离线辨识手术器械回差的算法计算手术器械的机械间隙回差ΔS₁；采用一唯FCN网络对电机匀速往返运动时地电流数据进行辨识，准确地提取出回差大小以调整滞回模型的幅值，进而实现对手术器械的细腻控制。

具体实施方式三：结合图2说明本实施方式，本实施方式是对具体实施方式一所述的控制方法的进一步的限定，本实施方式所述的一种微创手术机器人柔性丝传动手术器械高精度离线控制方法，所述的步骤三中的手术器械的柔性丝形变回差ΔS₂的计算方案如下：在主控控制开始之前，令致动电机以速度v匀速往返运动，幅值大小为A_m，采样周期为T_s，采集致动电机匀速往返运动时的电流数据；从电流数据中截取长度为L的电流数据，并作为一维FCN神经网络的输入；一维FCN神经网络利用长度为L的电流数据计算柔性丝形变周期大小C₂；计算手术器械的柔性丝形变回差

ΔS₂＝C₂·T_s·v (6)，

式中C₂是柔性丝形变周期大小，T_s是步骤二一中的采样周期，v是步骤二一中的致动电机速度。

具体实施方式四：结合图3说明本实施方式，本实施方式是对具体实施方式三所述的控制方法的进一步的限定，本实施方式所述的一种微创手术机器人柔性丝传动手术器械高精度离线控制方法，所述的步骤三中的手术器械通用滞回模型f(x；μ_ω)的计算方法中， M是大于等于5的整数，K是大于等于2的整数，具体流程如下：

步骤三一、使用三维光学定位设备采集M个手术器械的滞回曲线；

步骤三二、从滞回曲线中分别提取柔性丝形变过程的滞回曲线

q_out＝ξ_i(q_in)(i∈{1,2,...,M}) (7)，

共M条，式(7)中q_out是手术器械末端位置，q_in是致动电机的位置；

步骤三三、使用线性缩放，将柔性丝形变过程的滞回曲线的横轴范围归一化到区间 [0,1]，得到归一化的柔性丝形变过程的滞回曲线

式中x是归一化滞回曲线的自变量，ξ_i是步骤三二中的滞回曲线，q_in是致动电机位置；

步骤三四、建立K次多项式函数

式中x是多项式函数的自变量，

是多项式函数的参数，并利用最小二乘法来拟合归一化的柔性丝形变过程的滞回曲线

得到M组多项式函数的参数ω_i(i∈{1,2,...,M})；

步骤三五、用参数θ＝{μ_ω,∑_ω}的高斯分布为步骤三四中的M组多项式函数的参数ω_i(i∈{1,2,...,M})建模，即ω～p(ω；θ)＝N(ω|θ)，式中θ是高斯分布的参数，μ_ω是高斯分布的均值，∑_ω是高斯分布的方差；

步骤三六、利用极大似然估计计算0＝{μ_ω,∑_ω}，即

式中M是手术器械的个数，ω_i是步骤三四中的M组多项式函数的参数；

步骤三七、令ω＝μ_ω，带入多项式函数f(x；ω)得到手术器械通用滞回模型f(x；μ_ω)；

本具体实施方式，引入了一维FCN神经网络，才能仅使用电机匀速往返运动时的电流数据来辨识手术器械的机械间隙回差和柔性丝形变回差；正是因为使用极大似然估计分析了M个手术器械的滞回曲线并计算得到手术器械通用滞回模型，才能提高手术器械控制的准确度。

具体实施方式五：结合图3说明本实施方式，本实施方式是对具体实施方式二所述的控制方法的进一步的限定，本实施方式所述的一种微创手术机器人柔性丝传动手术器械高精度离线控制方法，所述的电流数据中截取长度为L的电流数据中至少包含电机往返运动一个周期长度的数据。

Claims (5)

1.微创手术机器人柔性丝传动手术器械高精度离线控制方法，其特征在于：其具体步骤如下：

步骤一、微创手术机器人系统开始主从控制，在每个控制周期循环判断致动电机是否反向运动；

步骤二、致动电机反向运动后，判断反向运动后的电机转向，如果正向转动则令电机目标位置q_d

q_d＝q_d+ΔS₁ (1)

利用电机目标位置q_d来补偿机械间隙回差；

如果反向转动则令电机目标位置q_d

q_d＝q_d-ΔS₁ (2)

利用电机目标位置q_d来补偿机械间隙回差，式(1)和式(2)中ΔS₁是该手术器械的机械间隙回差；

步骤三、使用该手术器械的柔性丝形变回差ΔS₂和手术器械通用滞回模型f(x；μ_ω)(3a)得到反归一化的电机补偿曲线：

f^denorm(x^denorm；μ_ω)＝f(x/ΔS₂；μ_ω) (3)

式(3)中x是滞回模型的自变量，μ_ω是滞回模型的参数，x^denorm是归一化后的自变量；

步骤四、在开始柔性丝形变回差补偿之前记录电机初始位置

步骤五、电机目标位置：

通过令电机目标位置来补偿柔性丝形变回差；

步骤六、在每个控制周期循环判断柔性丝变形回差是否补偿结束，如果结束则通过令电机目标位置q_d＝q_d+f^denorm(1)来固定补偿值；

步骤七、跳回步骤一，不断重复。

2.根据权利要求1所述的微创手术机器人柔性丝传动手术器械高精度离线控制方法，其特征在于：所述的步骤二中的手术器械的机械间隙回差ΔS₁的计算方法如下：

步骤二一、在主控控制开始之前，令致动电机以速度v匀速往返运动，幅值大小为A_m，采样周期为T_s，采集致动电机匀速往返运动时的电流数据；

步骤二二、从电流数据中截取长度为L的电流数据，并作为一维FCN神经网络的输入；

步骤二三、一维FCN神经网络利用长度为L的电流数据计算机械间隙周期大小C₁；

步骤二四、计算手术器械的机械间隙回差

ΔS₁＝C₁·T_s·v (5)，

式中C₁是步骤二三中的机械间隙周期大小，T_s是步骤二一中的采样周期，v是步骤二一中的致动电机速度。

3.根据权利要求1所述的微创手术机器人柔性丝传动手术器械高精度离线控制方法，其特征在于：所述的步骤三中的手术器械的柔性丝形变回差ΔS₂的计算方案如下：在主控控制开始之前，令致动电机以速度v匀速往返运动，幅值大小为A_m，采样周期为T_s，采集致动电机匀速往返运动时的电流数据；从电流数据中截取长度为L的电流数据，并作为一维FCN神经网络的输入；一维FCN神经网络利用长度为L的电流数据计算柔性丝形变周期大小C₂；计算手术器械的柔性丝形变回差

ΔS₂＝C₂·T_s·v (6)，

式中C₂是柔性丝形变周期大小，T_s是步骤二一中的采样周期，v是步骤二一中的致动电机速度。

4.根据权利要求3所述的微创手术机器人柔性丝传动手术器械高精度离线控制方法，其特征在于：所述的步骤三中的手术器械通用滞回模型f(x；μ_ω)的计算方法中，M是大于等于5的整数，K是大于等于2的整数，具体流程如下：

步骤三一、使用三维光学定位设备采集M个手术器械的滞回曲线；

步骤三二、从滞回曲线中分别提取柔性丝形变过程的滞回曲线

q_out＝ξ_i(q_in)(i∈{1,2,...,M}) (7)，

共M条，式(7)中q_out是手术器械末端位置，q_in是致动电机的位置；

步骤三三、使用线性缩放，将柔性丝形变过程的滞回曲线的横轴范围归一化到区间[0,1]，得到归一化的柔性丝形变过程的滞回曲线

ξ_i ^norm(x)＝ξ_i(q_in/max(q_in))，i∈{1,2,...,M} (8)，

式中x是归一化滞回曲线的自变量，ξ_i是步骤三二中的滞回曲线，q_in是致动电机位置；

步骤三四、建立K次多项式函数

式中x是多项式函数的自变量，

是多项式函数的参数，并利用最小二乘法来拟合归一化的柔性丝形变过程的滞回曲线

得到M组多项式函数的参数ω_i(i∈{1,2,...,M})；

步骤三五、用参数θ＝{μ_ω,∑_ω}的高斯分布为步骤三四中的M组多项式函数的参数ω_i(i∈{1,2,...,M})建模，即ω～ p(ω；θ)＝N(ω|θ)，式中θ是高斯分布的参数，μ_ω是高斯分布的均值，Σ_ω是高斯分布的方差；

步骤三六、利用极大似然估计计算θ＝{μ_ω,∑_ω}，即

式中M是手术器械的个数，ω_i是步骤三四中的M组多项式函数的参数；

步骤三七、令ω＝μ_ω，带入多项式函数f(x；ω)得到手术器械通用滞回模型f(x；μ_ω)。

5.根据权利要求2所述的微创手术机器人柔性丝传动手术器械高精度离线控制方法，其特征在于：所述的电流数据中截取长度为L的电流数据中至少包含电机往返运动一个周期长度的数据。

Images