CN111973279B - 一种血管介入手术机器人的主从位置自适应跟踪控制方法 - Google Patents

Abstract

本发明涉及一种血管介入手术机器人的主从位置自适应跟踪控制方法，首先，建立了主手端操作机构动力学模型和从手端递送机构动力学模型，然后将递送机构轴向运动模型的线性部分和非线性部分分开考虑，求解了滑模控制的等效控制量和鲁棒控制量，在滑模控制的基础上，针对不确定上下界的扰动，设计了自使用滑模控制方法，与现有技术相比，本发明能明显的减小系统的抖动和控制偏差，提高了控制效果，从而提高主从位置跟踪的性能。

Description

一种血管介入手术机器人的主从位置自适应跟踪控制方法

技术领域

本发明涉及手术机器人控制领域，尤其是涉及一种血管介入手术机器人的主 从位置自适应跟踪控制方法。

背景技术

现在综合性院校或相关血管介入手术医疗机械手研究机构，在建立血管介入 手术机器人主手端操作机构和从手端递送机构的动力学模型时，忽略了非线性的扰 动，如滚动轴承的摩擦力、滚珠丝杠的振动等。

血管介入手术机器人主从控制器是从手端递送机构追踪主手端操作机构获取 医生的操作运动的指令，来完成导管/导丝的递送。在其控制过程中实时接收移动 指的位移反馈信息根据误差自动调整控制指令，其控制输出的好坏直接决定了递送 精度。

采用经典或现代控制理论设计的主从控制算法，需要建立被控对象的准确的 数学模型，在此基础上合理的选择控制策略。然后导丝递送过程中由于导管/导丝 为柔性体而且是无限自由度的，递送过程中其形状是实时变化的，这种变形能会对 递送机构造成扰动；其递送过程中与血管壁产生摩擦，同时血流也会对导管/导丝 的造成扰动，这就会造成对递送机构造成非线性、不确定的扰动。所以很难通过简 单的方法建立准确的数学模型，这就导致了经典或现代控制方法不适用于血管介入 手术机器人主从控制。

导管/导丝为柔性体是无限自由度的，递送过程中其形状是实时变化的，这种 变形能会对递送机构造成扰动；其递送过程中与血管壁产生摩擦，同时血流也会对 导管/导丝的造成扰动，这就会造成对递送机构造成非线性、不确定的扰动。因此 递送机构会受到许多不确定的、非线性的外界扰动，会对系统的稳定性和性能品质 产生影响，这就需要一种能够抗外界干扰鲁棒性强的控制算法。滑模控制算法通过 开关控制量的切换使系统沿滑模面运动，所以其与外界的扰动无关，有很强的鲁棒 性。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种血管介入手 术机器人的主从位置自适应跟踪控制方法。

本发明的目的可以通过以下技术方案来实现：

一种血管介入手术机器人的主从位置自适应跟踪控制方法，包括以下步骤：

1)根据主手端操作机构的结构，构建主手端操作机构的轴向运动动力学模型 和旋转运动动力学模型；

2)根据从手端递送机构的结构，构建考虑不确定扰动的轴向运动动力学模型 以及从手端递送机构的旋转运动动力学模型；

3)确定自适应滑模控制律，并构建自适应滑模控制器，结合主手端操作机构 的滑动套筒动作产生的旋捻位移指令和推进位移指令进行从手端递送机构的自适 应跟踪控制。

所述的步骤1)中，轴向运动动力学模型为施加在滑动套筒上的轴向推力F_m和 递送机构滑动套筒轴向位移x_m之间的关系模型，其表达式为：

其中，F_m为施加在滑动套筒上的轴向推力，m_m为滑动套筒质量，J_mx1为同步 带轮转动惯量，J_mx2为磁粉制动器输入轴转动惯量，J_mx3为导轮转动惯量，J_mx4为 弹性联轴器转动惯量，J_c为编码器转动惯量，x_m为递送机构滑动套筒轴向位移，μ_m为滑动套筒与操作轴间滚动摩擦力系数，f_n为紧定螺钉的压力，r_m为同步带轮半 径，T_mxr为磁粉制动器反馈力矩。

所述的步骤1)中，旋转运动动力学模型为主手操作滑动套筒的旋转力矩M_m和 主手操作旋转角度θ_mθ之间的关系模型，其表达式为：

其中，M_m为主手操作滑动套筒的旋转力矩，J_mθ1为滑动套筒转动惯量，J_mθ2为 旋转轴转动惯量，J_mθ3为同步带主动轮转动惯量，i_mθ为同步带传动比，J_mθ4为同步 带从动轮转动惯量，J_c为编码器转动惯量，θ_mθ为主手操作旋转角度。

所述的步骤2)中，轴向运动动力学模型为进给电机驱动力矩T_sx和轴向推进位 移x_s之间的关系模型，其表达式为：

其中，T_sx为进给电机驱动力矩，p为滚珠丝杠导程，J_s为伺服电机转动惯量， J_sx1为弹性联轴器转动惯量，J_sx2为滚珠丝杠的转动惯量，m_t为移动指质量，η₁为 滚珠丝杠正效率，x_s为轴向推进位移，μ_v为滑块直线导轨的黏性摩擦系数，B_s为 电机阻尼系数，μ_c为滑块直线导轨的库伦摩擦系数，Δ为不确定扰动项。

所述的步骤2)中，旋转运动动力学模型为旋转伺服电机的驱动力矩T_sθ与导管 /导丝旋转角度之间的关系模型，其表达式为：

其中，T_sθ为旋转伺服电机的驱动力矩，J_s为伺服电机转动惯量，J_sθ1为弹性联 轴器转动惯量，J_sθ2为主动带轮转动惯量，i_sθ1为主动带轮和从动带轮之间传动比， i_sθ2为从动带轮和夹持爪之间传动比，J_sθ3为所有从动带轮转动惯量，J_sθ4为夹持爪 转动惯量，θ_sθ1为主动带轮旋转角度，θ_sθ2为从动带轮旋转角度，B_s为电机阻尼系 数。

所述的步骤3)中，自适应滑模控制律表现为PID控制规律和加速度、速度前 馈的复合控制律的形式，其控制函数表达式为：

k_acc＝A₁

k_vel＝k₁

k_p＝-k₁c-μsgn(σ(t))

k_i＝-μsgn(σ(t))

k_d＝(B₁-A₁·c-k₁)

T_smc＝i_smc×k_t

其中，x_d为初始位移，k₁为控制律参数，且k₁＞0，c为常数，μ为正数，k_acc、 k_vel、k_p、k_i、k_d均为中间参量，σ(t)为滑模面函数，sgn(·)为信号函数，i_smc为电 机电流，k_t为电机转矩常数。

与现有技术相比，本发明具有以下优点：

滑模控制算法通过开关控制量的切换使系统沿滑模面运动，所以其与外界的扰动无关，有很强的鲁棒性。因此发明申请设计了一种自适应滑模控制器用于主从位 置跟踪控制。对于导丝阻力等不确定因素的扰动，滑模控制具有很强的抗干扰能力。 故本申请采用不依靠系统精确建模的滑模控制作主从控制算法，其快速响应、滑动 段完全鲁棒性的特点非常适合血管介入手术机器人主从控制，能够通过改变系统的 结构，实现较好的主从跟踪效果。

附图说明

图1为主手端操作机构轴向运动简化模型。

图2为主手端操作机构旋转运动简化模型。

图3为从手轴向运动简化模型。

图4为从手旋转运动简化模型。

图5为主从位置跟踪的自适应滑模控制结构。

图6为递送机构轴向运动自适应滑模控制模型。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

本发明提供一种血管介入手术机器人的主从位置自适应跟踪控制方法，通过不依靠系统精确建模的滑模控制作主从控制方法，快速响应、滑动段完全鲁棒性的特 点非常适合血管介入手术机器人主从控制，能够通过改变系统的结构实现较好的主 从跟踪效果。

血管介入手术的外界环境比较复杂，存在许多非线性和不确定的影响因素，为 了建立合理的主手端操作机构动力学模型和从手端递送机构动力学模型，需要做合 理的近似处理，忽略部分非线性和不确定因素。为此做一下假设条件：

(1)主从机构模型为线性、确定系统；

(2)忽略传动过程中的不确定性因素，如轴承的摩擦力、带的变形势能、带的 振动干扰；

(3)导管/导丝、同步带的质量比较小，忽略其质量和惯性的影响；

(4)假设导管/导丝在血管内运动时与血管壁的摩擦力比较小，忽略摩擦阻力和血流扰动的影响。

1、主手端操作机构动力学模型

1.1操作机构轴向运动动力学模型

主手端操作机构轴向运动的结构简图如图1所示，其力位置信号采集和反馈原 理为：操作机构采用同步带传动形式，医生操作滑动套筒进行轴向移动，滑动套筒 内部通过同步带与同步带轮和导轮连接，同步带轮一端与磁粉制动器输入轴固定， 另一端通过弹性联轴器与编码器固定，所以操作机构可以通过磁粉制动器施加阻力 来建立力觉临场感，同时可以通过编码器采集轴向运动位置信息。

忽略同步带传动过程中的变形、振动、运动惯量等因素对运动的影响，建立主 手端操作机构动力学模型。滑动套筒沿操作轴圆柱表面运动，其摩擦力大小与紧定 螺钉压力成线性关系，其动力学模型为：

f_m＝μ_mf_n

建立带传动动力学方程为：

带传动的滑动套筒直线位移和带轮转角的运动关系为：

x_m＝θ_mxr_m

联立方程(5-1)～(5-4)得到递送机构的人手施加在滑动套筒上的轴向推力和轴向 推进位移之间的关系式为：

其中，F_m为施加在滑动套筒上的轴向推力，m_m为滑动套筒质量，J_mx1为同步 带轮转动惯量，J_mx2为磁粉制动器输入轴转动惯量，J_mx3为导轮转动惯量，J_mx4为 弹性联轴器转动惯量，J_c为编码器转动惯量，x_m为递送机构滑动套筒轴向位移，μ_m为滑动套筒与操作轴间滚动摩擦力系数，f_n为紧定螺钉的压力，r_m为同步带轮半 径，T_mxr为磁粉制动器反馈力矩。

1.2操作机构旋转运动动力学模型

操作机构实现旋转运动的机构结构原理如图2所示，医生施加旋转力矩在滑动 套筒上使其旋转，旋转套筒与操作轴不能相对转动，操作套筒一端与主动同步带轮 相对固定，主动带轮通过同步带与从动带轮相连，从动带轮与编码器输入轴固连， 所以滑动套筒可以经过传动带动编码器旋转，编码器可以采集人手操作滑动套筒的 位置信息。

忽略同步带运动过程中的变形、振动和轴承摩擦力等不确定因素递的影响，建 立主手端操作机构旋转运动的动力学模型。首先建立滑动套筒和操作轴的动力学方 程为：

同步带轮动力学方程：

主动带轮和从动带轮之间的运动关系为：

θ_mθ＝i_mθθ'_mθ

联立方程(5-6)～(5-8)得到递送机构旋转运动的人手操作滑动套筒的旋转力矩和 旋转角度之间的关系为：

其中，M_m为主手操作滑动套筒的旋转力矩，J_mθ1为滑动套筒转动惯量，J_mθ2为 旋转轴转动惯量，J_mθ3为同步带主动轮转动惯量，i_mθ为同步带传动比，J_mθ4为同步 带从动轮转动惯量，J_c为编码器转动惯量，θ_mθ为主手操作旋转角度。

2从手端递送机构动力学模型

2.1从手轴向运动动力学模型

从手端递送机构实现轴向运动的结构主要由：轴承、滚珠丝杠、丝母、移动指、 直线导轨、弹性联轴器、电机等组成，其简化模型如图3所示。伺服电机通过弹性 联轴器与滚珠丝杠直接相连，与滚珠丝杠啮合的丝母，与移动指相连，可以通过滚 珠丝杠和丝母配合将电机的旋转运动，转化为移动指的直线运动。移动指在直线导 轨的约束下，能够减轻摩擦力，实现光滑、平稳、高精度的直线运动，从而实现导 管/导丝的轴向递给运动。因为手术过程中，移动指受到的导管/导丝的阻力很小， 因此可以忽略滚珠丝杠和弹性联轴器的变形引起的扭转弹性势能。因为移动指的重 力由直线导轨承受，同时滚珠丝杠选用的是一端支撑一端固定的固定形式，因此滚 珠丝杠高速运转过程中升温带来的丝杠的伸长不会影响轴承的正常运转，同时滚动 轴承本身的转动阻力非常小，因此滚动轴承的擦力可以忽略不计。

根据从手端递送机构轴向运动的传递过程，结合力学、运动原理，依次建立相 关零部件的运动学方程，忽略中间传动中的摩擦的影响。递送机构提供轴向递送动 力的电机选用的是瑞士Maxon无刷电机，其动力学方程为：

建立弹性联轴器的动力学方程为：

建立滚珠丝杠的动力学方程为：

滚珠丝杠的扭矩与推力的关系可表示为：

滚珠丝杠的轴向负荷为移动指的阻力，其受力为：

F_f为夹持爪与导管间的摩擦力，由库伦摩擦力和黏性摩擦力构成：

移动指递进位移与滚珠丝杠角位移之间的关系为：

F_e是导管/导丝施加在夹持爪上的轴向阻力，此阻力主要是由于导管/导丝递进过程中与弯曲的血管壁之间的摩擦造成的，此外还包括外人体内血流的扰动力。因 为导管/导丝的刚性比较小，很容易弯曲，所以这种阻力很小，此外由于血管内形 状复杂，这种柔性体的力在递送过程中是实时变化的，很难建立精确的力学方程， 因此将F_e忽略。

联立方程(5-10)～(5-16)可得递送机构的进给电机驱动力矩和轴向位移之间的关 系模型为：

其中，T_sx为进给电机驱动力矩，p为滚珠丝杠导程，J_s为伺服电机转动惯量， J_sx1为弹性联轴器转动惯量，J_sx2为滚珠丝杠的转动惯量，m_t为移动指质量，η₁为 滚珠丝杠正效率，x_s为轴向推进位移，μ_v为滑块直线导轨的黏性摩擦系数，B_s为 电机阻尼系数，μ_c为滑块直线导轨的库伦摩擦系数，Δ为不确定扰动项。

2.2从手旋转运动动力学模型

从手端递送机构实现旋转运动的结构主要由：夹持爪、从动轮、主动轮、弹性 联轴器、电机等组成，其简化模型如图4所示。伺服电机通过弹性联轴器与主动带 轮相连，主动带轮、从动带轮、夹持爪转动轴通过带相连，主动带轮通过带驱动从 从动带轮、夹持爪转动，夹持爪加紧导丝，进而实现导丝的旋捻操作。

同理，根据力学、运动学原理结合旋转运动的具体传递过程建立递送机构的动 力学方程。建立电机的动力学方程为：

建立弹性联轴器的动力学方程为：

主动带轮的动力学方程为：

从动带轮的动力学方程为：

夹持爪的动力学方程为：

带传动的主动轮和从动轮间的传动比为：

θ_sθ＝i_sθ1θ_sθ1

带传动的从动轮和夹持爪间的传动比为：

θ_sθ1＝i_sθ2θ_sθ2

由式子(5-23)和式(5-24)可得带传动的主动轮和夹持爪间的传动比为：

θ_sθ＝i_sθ1i_sθ2θ_sθ2

T_sθr为导管/导丝施加在旋转指夹持爪上的阻力矩，阻力矩主要是由于导管/导 丝递进过程中与弯曲的血管壁之间的摩擦造成的，此还包括外人体内血流的扰动 力。因为其随着导丝的旋转具有时变性且很小，因此将其忽略。

联立方程可以得到从手端递送机构旋转伺服电机的驱动力矩和导管/导丝旋转角度之间的关系为：

其中，T_sθ为旋转伺服电机的驱动力矩，J_s为伺服电机转动惯量，J_sθ1为弹性联 轴器转动惯量，J_sθ2为主动带轮转动惯量，i_sθ1为主动带轮和从动带轮之间传动比， i_sθ2为从动带轮和夹持爪之间传动比，J_sθ3为所有从动带轮转动惯量，J_sθ4为夹持爪 转动惯量，θ_sθ1为主动带轮旋转角度，θ_sθ2为从动带轮旋转角度，B_s为电机阻尼系 数。

基于主从系统的自适应滑模控制器设计

在建立血管介入手术机器人主手端操作机构和从手端递送机构的动力学模型时，忽略了非线性的扰动，如滚动轴承的摩擦力、滚珠丝杠的振动等。因为导管/ 导丝为柔性体是无限自由度的，递送过程中其形状是实时变化的，这种变形能会对 递送机构造成扰动；其递送过程中与血管壁产生摩擦，同时血流也会对导管/导丝 的造成扰动，这就会造成对递送机构造成非线性、不确定的扰动。因此递送机构会 受到许多不确定的、非线性的外界扰动，会对系统的稳定性和性能品质产生影响， 而PID控制中并未考虑这种扰动，这就需要一种能够抗外界干扰鲁棒性强的控制 算法。滑模控制算法通过开关控制量的切换使系统沿滑模面运动，所以其与外界的 扰动无关，有很强的鲁棒性。因此本发明设计了一种自适应滑模控制器用于主从位 置跟踪控制。

操作机构采集医生的操作信息传给从手端，从手端通过跟踪主手端的位置信息来完全对导管/导丝的介入操作，这就是血管微创介入手术机器人主从控制系统的 原理，其控制框图如图5所示。

由于建立模型时不精确，未考虑导管/导丝的扰动，现将递送机构轴向运动的 动力学模型表示为：

式中，

Δ为不确定扰动项。

由式上式可以看出该模型为一个二阶系统，且为一个单输入控制量，所以滑模 面函数可选用一阶的形式，如下式：

将上式整理得：

对上式求导，并根据式(5-35)可以得到滑模面函数的导数：

根据指数趋近律，可以得到滑模控制律为：

式中，k₁＞0，k₂＞q。

为了验证系统的稳定性，定义李雅普诺夫方程如下：

对李雅普诺夫方程求导得：

满足李雅普诺夫定理，因此滑模控制律可以在有限的时间内使系统状态误差进入滑模面并沿滑模面收敛到0。因为外界的扰动和参数不确定的Δ的上界未知，引 入自适应参数校正律对k₂进行评估，即：

式中，μ＞0，经过计算得到了自适应滑模控制规律(ASMC)为：

不考虑控制律的参数校正，上式整理得：

式中，kacc＝A₁，kvel＝k₁，k_p＝-k₁c-μsgn(σ(t))，k_i＝-μsgn(σ(t))， k_d＝(B₁-A₁·c-k₁)，

由上式可 以看出，基于递送机构轴向递送运动动力学模型的自适应滑模控制律表现为PID 控制规律和加速度、速度前馈的复合控制律的形式，图6为其控制结构图。

T_smc＝i_smc×k_t

其中，i_smc为电机电流，k_t为电机转矩常数。

基于主从系统的自适应滑模控制仿真

本发明针对血管介入手术机器人主从位置跟踪的特点设计了自适应滑模控制器，为了验证其有效性，利用MATLAB/Simulink建立了控制器的仿真系统，因为 机器人系统的轴向运动和旋转运动动力学方程都为二阶系统其仿真原理相同，此处 以轴向递进位置跟踪为例进行仿真验证。

递送机构轴向递进运动的动力学方程考虑不确定的因素的扰动，其开环传递函数为：

式中，

通过查阅手册和计算可以得到参数的具体值，见表1。

表1系统参数值

根据试验参数调节过程，本发明取c＝1000，k_s＝150，μ＝3，则可以计算出： kaa＝1.87×10^-4，kvel＝106，k_p＝150000，k_i＝3000，k_d＝394。

本发明主要设计了主从位置跟踪的自适应滑模制方法。首先，建立了主手端操 作机构动力学模型和从手端递送机构动力学模型。然后将递送机构轴向运动模型的 线性部分和非线性部分分开考虑，求解了滑模控制的等效控制量和鲁棒控制量；在 滑模控制的基础上，针对不确定上下界的扰动，设计了自使用滑模控制算法。利用 Matlab/Simulink进行了仿真，检验控制算法在递送机构模型的有效性。仿真结果标 明，对比滑模控制算法自适应滑模控制算法，能明显的减小系统的抖动和控制偏差， 提高了控制效果，从而提高主从位置跟踪的性能。

Claims (5)

1.一种血管介入手术机器人的主从位置自适应跟踪控制方法，其特征在于，包括以下步骤：

1)根据主手端操作机构的结构，构建主手端操作机构的轴向运动动力学模型和旋转运动动力学模型，轴向运动动力学模型为施加在滑动套筒上的轴向推力F_m和递送机构滑动套筒轴向位移x_m之间的关系模型，其表达式为：

其中，F_m为施加在滑动套筒上的轴向推力，m_m为滑动套筒质量，J_mx1为同步带轮转动惯量，J_mx2为磁粉制动器输入轴转动惯量，J_mx3为导轮转动惯量，J_mx4为弹性联轴器转动惯量，J_c为编码器转动惯量，x_m为递送机构滑动套筒轴向位移，μ_m为滑动套筒与操作轴间滚动摩擦力系数，f_n为紧定螺钉的压力，r_m为同步带轮半径，T_mxr为磁粉制动器反馈力矩；

2)根据从手端递送机构的结构，构建考虑不确定扰动的轴向运动动力学模型以及从手端递送机构的旋转运动动力学模型；

3)确定自适应滑模控制律，并构建自适应滑模控制器，结合主手端操作机构的滑动套筒动作产生的旋捻位移指令和推进位移指令进行从手端递送机构的自适应跟踪控制。

2.根据权利要求1所述的一种血管介入手术机器人的主从位置自适应跟踪控制方法，其特征在于，所述的步骤1)中，旋转运动动力学模型为主手操作滑动套筒的旋转力矩M_m和主手操作旋转角度θ_mθ之间的关系模型，其表达式为：

其中，M_m为主手操作滑动套筒的旋转力矩，J_mθ1为滑动套筒转动惯量，J_mθ2为旋转轴转动惯量，J_mθ3为同步带主动轮转动惯量，i_mθ为同步带传动比，J_mθ4为同步带从动轮转动惯量，J_c为编码器转动惯量，θ_mθ为主手操作旋转角度。

3.根据权利要求1所述的一种血管介入手术机器人的主从位置自适应跟踪控制方法，其特征在于，所述的步骤2)中，轴向运动动力学模型为进给电机驱动力矩T_sx和轴向推进位移x_s之间的关系模型，其表达式为：

其中，T_sx为进给电机驱动力矩，p为滚珠丝杠导程，J_s为伺服电机转动惯量，J_sx1为弹性联轴器转动惯量，J_sx2为滚珠丝杠的转动惯量，m_t为移动指质量，η₁为滚珠丝杠正效率，x_s为轴向推进位移，μ_v为滑块直线导轨的黏性摩擦系数，B_s为电机阻尼系数，μ_c为滑块直线导轨的库伦摩擦系数，Δ为不确定扰动项。

4.根据权利要求1所述的一种血管介入手术机器人的主从位置自适应跟踪控制方法，其特征在于，所述的步骤2)中，旋转运动动力学模型为旋转伺服电机的驱动力矩T_sθ与导管/导丝旋转角度之间的关系模型，其表达式为：

其中，T_sθ为旋转伺服电机的驱动力矩，J_s为伺服电机转动惯量，J_sθ1为弹性联轴器转动惯量，J_sθ2为主动带轮转动惯量，i_sθ1为主动带轮和从动带轮之间传动比，i_sθ2为从动带轮和夹持爪之间传动比，J_sθ3为所有从动带轮转动惯量，J_sθ4为夹持爪转动惯量，θ_sθ1为主动带轮旋转角度，θ_sθ2为从动带轮旋转角度，B_s为电机阻尼系数。

5.根据权利要求3所述的一种血管介入手术机器人的主从位置自适应跟踪控制方法，其特征在于，所述的步骤3)中，自适应滑模控制律表现为PID控制规律和加速度、速度前馈的复合控制律的形式，其控制函数表达式为：

k_acc＝A₁

k_vel＝k₁

k_p＝-k₁c-μ sgn(σ(t))

k_i＝-μ sgn(σ(t))

k_d＝(B₁-A₁·c-k₁)

T_smc＝i_smc×k_t

其中，x_d为初始位移，k₁为控制律参数，且k₁＞0，c为常数，μ为正数，k_acc、k_vel、k_p、k_i、k_d均为中间参量，σ(t)为滑模面函数，sgn(·)为信号函数，i_smc为电机电流，k_t为电机转矩常数。

Images