CN111584061A - 基于无模型自适应算法的多爪针自动穿刺控制装置及控制方法 - Google Patents

Abstract

本发明提供了一种基于无模型自适应算法的多爪针自动穿刺装置及控制方法，用于控制多爪针运动。本发明的多爪针自动穿刺装置，包括控制模块和执行机构；控制模块解析上位机传输的路径跟踪控制指令，生成电机控制量输出；执行机构包括传动驱动模块和多爪针体，根据电机控制量，驱动多爪针体运动。本发明的控制方法从传感器获取主针和所有子针的针尖位置、电机位置，从上位机的指令中获取期望的针尖位置，基于紧格式动态性线性化数据模型的MFAC控制方法计算输出电机控制量，控制多爪针的穿刺动作，跟踪目标路径。本发明改进了穿刺装置行进的不确定性，降低传统控制方法对模型的依赖性，提高穿刺装置对复杂环境的适应性。

Description

基于无模型自适应算法的多爪针自动穿刺控制装置及控制 方法

技术领域

本发明涉及软组织穿刺器械技术领域，特别涉及一种基于无模型自适应(Model-Free Adaptive Control，MFAC)算法的多爪针自动穿刺控制装置及控制方法。

背景技术

软组织穿刺器械是一种重要的临床手术器械，其优点是手术创伤小、操作方便、实用性强，手术费用低等，广泛应用于临床诊断、治疗、肿瘤化疗与放疗、局部麻醉等领域。

目前，穿刺手术通常是医疗人员基于医疗影像的引导下进行经皮穿刺，将穿刺针穿入目标部位，进行相应的手术操作。但传统穿刺针是刚性的，每次只能定位一个靶点，因此穿刺路径较为单一，多次活检需要进行反复穿刺会引起患者较大不适。另外实际手术中受医疗人员体力、经验等因素的影响制约穿刺针难以精准操控。为解决上述问题，对穿刺器械的结构优化和自动控制在医疗辅助设备的应用开展进一步深入研究以实现精准定位，最大程度提高穿刺质量，减少病人痛苦。

发明内容

本发明的目的是针对穿刺针的精准操控需求，提出一种基于数据驱动算法的实现自动控制的多爪针自动穿刺控制装置，并开发了对应的控制方法，以克服穿刺过程的环境干扰，提高穿刺定位精度。

本发明提供了一种基于无模型自适应算法的多爪针自动穿刺控制装置，包括控制模块和执行机构。其中，控制模块包括微处理单元和电机控制单元；微处理单元用于解析上位机传输来的路径跟踪控制指令，生成电机控制量，输出给电机控制单元；电机控制单元对电机控制量进行放大，输出给执行机构；所述的执行机构包括传动驱动模块和多爪针体；传动驱动模块包括传动机构和驱动电机，多爪针体通过传动机构与驱动电机连接，驱动电机根据电机控制量，驱动多爪针体运动；多爪针体包括刚性主针和柔性子针束，子针束在非工作状态嵌套在主针内，在工作状态时，主针的驱动电机推动主针打开导向缝，子针束的驱动电机控制各柔性子针伸缩，使子针呈发散状作用于靶点位置。

本发明提供的一种基于无模型自适应算法的多爪针自动穿刺的控制方法，包括如下：

步骤1，多爪针自动穿刺控制装置从传感器获取主针和所有子针的针尖位置、电机位置，从上位机的指令中获取期望的针尖位置，并输出电机控制量；

其中，将电机控制量表示为输入物理量u(k)，u(k)包括k时刻的主针、各子针束的驱动电机的位置控制量；将k时刻的主针和所有子针的针尖位置表示为输出物理量y(k)；将历史的输出物理量和输入物理量与下一时刻的输出物理量，用多输入多输出的非线性离散时间系统表示；

步骤2，将步骤1的非线性离散时间系统转化为紧格式动态性线性化数据模型；

步骤3，为了保证获得期望的输出物理量，设计控制输入准则函数来获取控制律；控制律如下：

其中，u(k-1)为k-1时刻的输入物理量；权重因子λ>0；步长因子ρ∈(0,1]；y*(k+1)为期望的k+1时刻的输出物理量；Φ_c(k)为非线性离散时间系统在k时刻的伪雅克比矩阵；

为矩阵Φ_c(k)的转置；

步骤4，估计伪雅克比矩阵Φ_c(k)；

步骤5，根据伪雅克比矩阵的估计值

获取k时刻的控制律：

其中，

是矩阵

的转置；

计算出的u(k)就是控制模块在k时刻输出的电机控制量。

相对于现有技术，本发明的优点和积极效果在于：

(1)本发明实现了一种多爪针自动穿刺控制装置及控制方法，目前多爪针是手动控制的，大大影响穿刺精度，本发明实现了多爪针的自动控制，通过控制电机和齿轮实现多爪针主针和各子针的前进和伸缩。

(2)本发明对多爪针的自动穿刺控制方法，避免了多爪针在组织内运动模型较难建立的问题，对多爪针的电机驱动无需建模，降低了对模型的依赖性，建立了基于自动穿刺辅助装置，精确控制自动多爪针完成软组织穿刺，改进了人工穿刺的不确定性，提高了对复杂环境的适应性。

附图说明

图1为本发明的自动穿刺系统的组成结构原理图；

图2为本发明的多爪针体的结构示意图；

图3为本发明的自动控制多爪针的系统原理图；

图4为本发明的基于MFAC自动多爪针控制方法的示意图。

具体实施方式

下面结合附图和实例对本发明的技术方案作进一步的详细说明。

本发明设计了一种具有自动控制多爪针动作的穿刺执行机构，基于紧格式动态性线性化(compace form dynamic linearization，CFDL)数据模型的MFAC控制方法控制穿刺动作，跟踪目标路径，本发明改进了穿刺装置行进的不确定性，进而减少多次穿刺可能造成的损伤，降低传统控制方法对模型的依赖性，提高穿刺装置对复杂环境的适应性。

本发明的多爪针自动穿刺系统，如图1所示，整体包括：规划模块、控制模块、传感模块、传动驱动模块、多爪针体以及上位机。其中，上位机的功能主要包括有人机交互、控制模块的参数配置、接收传感模块的设备状态并展示、穿刺针与组织的形态展示等。规划模块包括术前预规划单元及术中规划单元；术前预规划模块用于获取术前的人体内部环境信息，获取障碍、靶点信息，初步规划穿刺路径；术中规划单元是在穿刺过程中，由于人体组织变化或穿刺出现较大偏差，对预规划路径进行修正重规划。规划模块可设置在上位机或者其他主机上，本发明的重点在于根据规划路径来控制多爪针运动，对多爪针的路径规划可采用现有的路径规划技术实现。规划模块将规划路径根据预设的通讯协议转译为一系列路径跟踪控制指令，发送给控制模块，指导多爪针按规划路径逐步精准穿刺到达靶点位置。

控制模块为一嵌入式模块，集成有微处理单元和电机控制单元。电机控制单元主要包含驱动功放电路。微处理单元在接收到关于规划路径的跟踪控制指令后，根据控制逻辑生成电机控制量，并将电压和电流量经由驱动功放电路放大，输出相应的PWM(脉冲宽度调制)的电压或电流量信号给传动驱动模块的电机，以驱动多爪针体运动。

传动驱动模块和多爪针体被划分为执行机构。传动驱动模块包括传动机构和驱动电机等。多爪针体通过传动机构与驱动电机连接，驱动电机通过传动机构驱动多爪针体执行相应动作。

传感模块用于实时采集针-组织状态及传动驱动模块中驱动电机的运动状态信息，并将信息传输给控制模块以便于后续多爪针体的路径调整。

如图2所示，多爪针体包括刚性主针及多个柔性子针，图2所示实例中，柔性子针分为左子针束1和右子针束2。主针由外部保护鞘3及内部导向针4组成，主针用于粗定位及保护子针。子针为内部弯曲细长结构，用于到达不同目标位置的精定位。左、右子针束1,2各由两支结构相同的预弯穿刺针组成，用于扩大施针范围，实际应用时可根据具体所要作用的部位大小或靶点数量进行子针数量的调整。多爪针体有三个可控自由度，即主针导向针4、左子针束1、右子针束2的进给运动，如图2所示的，子针伸缩运动a、主针导向针的伸缩运动b、基于主穿刺系统的主针旋转运动c。整个多爪针体作为自动穿刺系统的操纵末端。

对于图2所示的多爪针体，传动驱动模块包括主针导向针4的进给电机及传动机构，左子针束1的进给电机及传动结构，右子针束2的进给电机及传动机构。子针束1,2非工作状态时嵌套在主针内，当主针到达指定位置后，内部导向针4伸出，此时导向针头部4与保护鞘3分离形成了子针导向缝，子针经由该导向缝伸出，由子针的驱动电机分别控制子针外伸与收回。在子针束工作结束后，收回子针，关闭导向缝，主针退出，穿刺结束。由于主针的刚性较大，因此可准确到达粗定位靶点，而子针的柔性可用于调节由于环境复杂变化导致的定位不准，多爪子针可一次提供更多的穿刺靶点，降低穿刺损伤。

进一步对本发明实现多爪针的自动控制进行说明，如图3所示，电机驱动板卡设置在控制模块中，属于电机控制单元，图中的控制器为控制模块的微处理单元，控制器在提取到上位机发来的路径信息后，计算生成电机控制量传输给电机驱动板卡，电机驱动板卡从通信指令中提取出伺服驱动电机ID、位置参数以及转速参数等数据，提供功率放大功能，发送相应的用于电机控制的PWM电压或电流信号给驱动电机。多爪针主针和子针尾部有推拉杆，电机通过齿轮组，通过拉线反复拉动推拉杆来实现对应机构的伸缩运动。图3中的电机1,2,3对应驱动多爪针体左、右子针束和主针运动。

传感器1,2,3是检测电机1,2,3的工作状态并发送给控制模块，多爪针在运动时候针-组织形态可通过超声波来获得。

本发明目的是令多爪针自动穿刺装置跟随规划路径运动，由于人体环境复杂，多爪针在组织内运动模型较难建立，而多爪针自动穿刺控制装置中有多个输入和多个输出状态，因此采用多输入多输出无模型自适应控制(MIMO-MFAC)算法来解决该问题。MIMO-MFAC的基本思想是在闭环被控系统的当前工作点处建立一个虚拟的动态线性化模型来代替一般的离散时间非线性系统，并且引入伪雅克比矩阵(pseudo jacobian matrix,PJM)概念，仅使用系统的输入和输出数据在线估计PJM，进而实现无模型自适应控制。

如图4所示，本发明基于MFAC算法的多爪针自动穿刺控制方法的流程是：由规划模块提供多爪针目标路径，经传感系统提供多爪针系统运行状态，并由MIMO-MFAC控制器对该装置运行状态进行调节，计算得出控制命令，并作用到对应驱动电机，驱动多爪针体运动，最终实现针组织之间的相互运动，达到准确穿刺的目的。

多爪针自动穿刺控制装置的控制模块中微处理单元根据传感器获得三个电机的当前位置，结合路径目的位置，包括左、右子针束1,2和主针的导向针4的针尖位置，来生成电机的位置控制量。将电机控制器建模，设输入物理量用序列u(k)＝[p₁,p₂,p₃]表示，p₁,p₂,p₃分别为三个电机的位置控制量；输出物理量用序列y(k)＝[x_li,y_li,z_li,…,x_rj,y_rj,z_rj,…,x_c,y_c,z_c]表示，包括所有子针和主针的针尖位置坐标；(x_li,y_li,z_li)表示第i个左子针的针尖位置，(x_rj,y_rj,z_rj)表示第i个右子针的针尖位置，本发明实施例中每个子针束有两个子针为例，即i＝1,2；j＝1,2；(x_c,y_c,z_c)表示主针导向针的针尖位置。k表示k时刻。

为方便硬件实现，将上述的输入物理量和输出物理量的关系，用MIMO非线性离散时间系统表示如下：

y(k+1)＝f[y(k)，…，y(k-n_y)，u(k)，…，u(k-n_u)] (1)

其中，y(k+1)表示k+1时刻的输出物理量，y(k)表示k时刻的输出物理量，y(k-n_y)表示k-n_y时刻的输出物理量，u(k)表示k时刻的输入物理量，u(k-n_u)表示k-n_u时刻的输出物理量；n_y、n_u为两个未知整数，分别代表输出和输入的不同时刻；f(-)为未知的非线性函数算子，代表微处理单元中控制逻辑的数学模型。公式(1)表示下一时刻的输出物理量与历史输入物理量和历史输出物理量的关系。

由CFDL数据模型定理可知，当系统满足偏导数连续及Lipschitz条件假设时，非线性系统(1)可以等价地表述为以下CFDL数据模型：

Δy(k+1)＝Φ_c(k)Δu(k) (2)

其中，Δy(k+1)表示k+1时刻输出物理量较上一时刻输出的物理量的增减值，Δu(k)表示k时刻输入物理量较上一时刻输入物理量的增减值；Φ_c(k)为系统在k时刻的伪雅克比矩阵，如下所示：

其中，R^p×q表示维度为p×q的实数集合；q等于输入序列的维数，p等于输出序列的维数。本发明实施例中，输入序列的维数为3，输出序列的维数为15。伪雅各比矩阵是一个数学意义上的概念，并不能用解析式表达出来，但其数值可以通过相邻时刻的数据估计出来，具体矩阵Φ_c(k)的估计计算如式(6)所示。关于伪雅各比矩阵的定义可参考侯忠生的《无模型自适应控制——理论与应用》2013年6月出版的第5.2.1节中的记载。

公式(2)是非线性系统(1)的一种等价的动态线性化表示。它是一个用于控制器设计的、具有简单增量形式的线性时变数据模型，该模型只与当前时刻为止之前的I/O数据相关，这与机理模型和其他线性化方法得到的模型都有本质不同。

为节省控制系统执行能量、过大控制量产生的可能损害，同时保证期望输出信号的跟踪性能，因此，考虑如下控制输入准则函数J(u(k))：

J(u(k))＝||y^*(k+1)-y(k+1)||²+λ||u(k)-u(k-1)||² (3)

其中，λ>0是一个权重因子，用于限制控制输入量的变化，y^*(k+1)为系统的期望输出物理量，从上位机发来的指令中获得，y(k+1)表示实际的输出物理量，可由超声波传感器对针—组织的检测得到。||x||表示求取矩阵x的2范数。

将式(2)带入公式(3)中，并对u(k)求导，并令导数为0，并简化计算公式则可得到如下控制律：

其中，ρ∈(0,1]是步长因子，它的加入目的是使控制算法更具一般性，并可用于调节稳定性和收敛性；上角标T表示转置，如

代表矩阵Φ_c(k)的转置。

下面说明伪雅克比矩阵Φ_c(k)的估计方法。

对于满足上述假设的非线性系统(1)可由带有时变PJM参数中

的动态线性化数据模型(2)来表示，基于控制输入准则函数(3)的极小化，可设计出控制算法(4)，为实现控制算法(4)，则需要已知PJM的值，由于系统的数学模型未知，因此PJM是时变参数矩阵，其精确真实值很难获取，因此，需要设计利用受控系统的输入输出数据来估计PJM的值。

由于PJM估计值是由系统采样数据得到的，而该数据的变化是随机的，容易对PJM估计产生较大的影响，因一方面要降低输出偏差，另一方面要降低参数估计对个别数据的敏感度，提出如下PJM估计准则函数

其中，μ是权重因子，μ>0，用于调节PJM变化过大。

是Φ_c(k)的估计值。

对式(5)关于Φ_c(k)求极值，可得PJM的估计算法为：

其中，η∈(0,2]是加入的步长因子，目的是令PJM估计算法具有更强的灵活性和一般性。

下面说明紧格式的MFAC控制器设计。

综合前面所得到的PJM估计算法(6)及控制律(4)，设计针对MIMO非线性CFDL-MFAC控制方案如下：

如果

或‖Δu(k-1)‖≤ε或

则

其中，i＝1,2,…,p；

如果

或

则

其中，i＝1,2,…,p；j＝1,2,…,q；

则控制律如下：

其中，ε,ε₁,ε₂都是正数；

是设置的初值；sign(x)代表符号函数，当x大于0时，sign(x)＝1，否则sign(x)＝-1。

控制模块的微处理单元根据上述过程计算

按照控制律u输出驱动电机的位置控制量。

本发明的上面控制器的设置，将非线性、不确定干扰的穿刺过程系统，采用无模型自适应控制，只需要历史的输入输出数据即可进行控制，无需建模，并且实现了精确的自动控制多爪针穿刺。

以上应用具体实例对本发明所提供的一种基于自动多爪针的肿瘤消融系统进行了详细介绍。本发明领域的普通技术人员根据上述内容所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于无模型自适应算法的多爪针自动穿刺控制装置，其特征在于，包括控制模块和执行机构；

所述的控制模块包括微处理单元和电机控制单元；微处理单元用于解析上位机传输来的路径跟踪控制指令，生成电机控制量，输出给电机控制单元；电机控制单元对电机控制量进行放大，输出给执行机构；

所述的执行机构包括传动驱动模块和多抓针体；传动驱动模块包括传动机构和驱动电机，多抓针体通过传动机构与驱动电机连接，驱动电机根据电机控制量，驱动多抓针体运动；多抓针体包括刚性主针和柔性子针束，主针和各柔性子针束都连接单独的驱动电机；子针束在非工作状态嵌套在主针内，在工作状态时，主针的驱动电机推动主针打开导向缝，子针束的驱动电机驱动各柔性子针伸缩，使子针呈发散状作用于靶点位置；

所述的控制模块的微处理单元根据历史的主针、各柔性子针的针尖位置以及历史的电机位置，来输出电机位置控制量。

2.根据权利要求1所述的装置，其特征在于，所述的多爪针自动穿刺控制装置，通过传感器获取驱动电机的位置和多抓针体的针尖位置。

3.根据权利要求1所述的装置，其特征在于，所述的微处理单元对电机位置控制量的控制律如下：

其中，u(k)表示k时刻的电机控制量，包括主针、各柔性子针束的驱动电机的位置控制量；u(k-1)表示k-1时刻的电机控制量；权重因子λ>0；步长因子ρ∈(0,1]；y(k)表示实际的k时刻的多抓针体的针尖位置，包括所有子针和主针的针尖位置，由超声波探测获得；y^*(k+1)为期望的k+1时刻的多抓针体的针尖位置，从上位机传输来的指令获得；Φ_c(k)为k时刻的伪雅克比矩阵，

是Φ_c(k)的估计值，

为矩阵

的转置。

4.根据权利要求3所述的装置，其特征在于，所述的微处理单元估计伪雅克比矩阵

如下：

设计伪雅克比矩阵Φ_c(k)的估计准则函数

如下：

其中，Δy(k)表示k时刻的输出物理量较上一时刻的输出物理量的增减值；Δu(k-1)表示k-1时刻的输入物理量较上一时刻的输入物理量的增减值；μ是设置的权重因子；

对式(2)关于Φ_c(k)求极值，得到估计值

如下：

如果

或‖Δu(k-1)‖≤ε或

则

其中，i＝1,2,…,p；

如果

或

则

其中,i＝1,2,…,p；j＝1,2,…,q；

其中，ε,ε₁,ε₂都是正数；

为矩阵

中的元素；

是设置的初值；sign(x)代表符号函数，当x大于0时，sign(x)＝1，否则sign(x)＝-1。

5.一种基于无模型自适应算法的多爪针自动穿刺的控制方法，其特征在于，包括如下步骤：

步骤1，多爪针自动穿刺控制装置从传感器获取主针和所有子针的针尖位置、电机位置，从上位机的指令中获取期望的针尖位置，将输出电机控制量；

其中，将电机控制量表示为输入物理量u(k)，u(k)包括k时刻的主针、各子针束的驱动电机的位置控制量；将k时刻的主针和所有子针的针尖位置表示为输出物理量y(k)；将历史的输出物理量和输入物理量与下一时刻的输出物理量，用多输入多输出的非线性离散时间系统表示；

步骤2，将步骤1的非线性离散时间系统转化为紧格式动态性线性化数据模型，表示如下：

Δy(k+1)＝Φ_c(k)Δu(k) (4)

其中，Δy(k+1)表示k+1时刻输出物理量较上一时刻输出物理量的增减值，Δu(k)表示k时刻输入物理量较上一时刻输入物理量的增减值；Φ_c(k)为非线性离散时间系统在k时刻的伪雅克比矩阵；

步骤3，为了保证获得期望的输出物理量，根据如下控制输入准则函数J(u(k))来获取控制律；

J(u(k))＝||y^*(k+1)-y(k+1)||²+λ||u(k)-u(k-1)||² (5)

其中，权重因子λ>0；y^*(k+1)为期望的k+1时刻的输出物理量；y(k+1)为实际的k+1时刻的输出物理量；u(k-1)为k-1时刻的输入物理量；

将式(4)带入式(5)中，并对u(k)求导，令导数为0，得到如下控制律：

其中，步长因子ρ∈(0,1]；

为矩阵Φ_c(k)的转置；

步骤4，估计伪雅克比矩阵Φ_c(k)，估计准则函数

如下：

其中，Δy(k)表示k时刻的输出物理量较上一时刻的输出物理量的增减值；Δu(k-1)表示k-1时刻的输入物理量较上一时刻的输入物理量的增减值；μ是设置的权重因子；

是Φ_c(k)的估计值；

对式(7)关于Φ_c(k)求极值，得到估计值

如下：

如果

或‖Δu(k-1)‖≤ε或

则

其中，i＝1,2,…,p；

如果

或

则

其中,i＝1,2,…,p；j＝1,2,…,q；

其中，ε,ε₁,ε₂都是正数；

为矩阵

中的元素；

是设置的初值；sign(x)代表符号函数，当x大于0时，sign(x)＝1，否则sign(x)＝-1；

步骤5，根据伪雅克比矩阵的估计值

获取k时刻的控制律：

其中，

是矩阵

的转置；计算出的u(k)就是控制模块在k时刻输出的电机控制量。

Images