CN112336297B - 体内导入装置的控制方法、系统和计算机可读存储介质 - Google Patents

Abstract

本发明涉及体内导入装置的控制方法、系统和计算机可读存储介质，装置包括导管、推送机构、绳驱动模块、绳固定模块和控制器；导管由多个节导管串联集成，每个节导管的管壁均设有骨架弹簧和导丝，导管一端的节导管中的导丝连接绳驱动模块，视为主动导管，其余导丝连接绳固定模块；方法包括：1)获取导管末端的推送速度，进而控制推送机构；2)获取导管末端的目标姿态，再基于最接近导管始端的主动导管的始端位置，根据主动导管的节变换矩阵，获取每个主动导管的旋转角度和弯曲角度，计算主动导管中各个导丝的长度变化量，从而控制对应的绳驱动模块。与现有技术相比，本发明实现了对导管的自动控制，在保证控制精确的同时，使得操作更加方便。

Description

体内导入装置的控制方法、系统和计算机可读存储介质

技术领域

本发明涉及体内导入技术领域，尤其是涉及体内导入装置的控制方法、系统和计算机可读存储介质。

背景技术

支气管肺癌(简称肺癌)是世界范围内发病率及死亡率最高的恶性肿瘤，近年发病率及死亡率均呈逐年上升趋势。手术仍是目前肺癌最有效的治疗方法，但是由于早期肺癌临床表现无特异性，大多数患者确诊肺癌时已错过手术时机，而目前的化疗和放疗整体效果并不理想。早期诊断和手术是提高肺癌患者生存的关键，所以如何能在早期准确识别和诊断肺癌的技术就至关重要。

目前肺癌早期筛查常用的方法是经皮肺穿刺活检或针吸，以获取病灶样本进行活检，该方法准确率较高，但是往往受限于病灶的位置以及大小，较小的外周肺结节等不适用，并且有可能引发气胸等并发症。故体内穿刺成为了关注的重点。同时，常规支气管镜检查仅能见到支气管腔内的病灶，对其进行组织及细胞学检查，对支气管腔外病灶无法探及，仅能依靠盲式活检、刷检来诊断，取样往往不尽如人意。常规支气管镜也受限于其设备大小，无法到达较细的气管部位。

中国专利CN201910231360.5披露了一种改进型支气管镜，解决了现有的支气管镜在进行角度调节的时候，因现有调节模块在使用的时候容易出现滑动，不能很好的规定手指位置，这样在进行操作的时候不是很方便的问题。但其仍属于传统支气管镜范围，没有太大改动，当进行支气管镜镜检时仍存在操作不方便、易使患者支气管受损且难以进入更小的支气管分支的问题。

发明内容

本发明的目的就是为了克服上述现有技术存在操作不方便、易使患者支气管受损且难以进入更小的支气管分支的缺陷而提供一种控制方便的体内导入装置的控制方法、系统和计算机可读存储介质。

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

一种体内导入装置的控制方法，所述体内导入装置包括导管、推送机构、绳驱动模块、绳固定模块和控制器；

所述推送机构用于带动所述导管前进和后退；

所述导管由多个节导管串联集成，每个节导管的内部设有沿节导管纵向分布的通孔，管壁设有沿节导管纵向分布的骨架弹簧和导丝，所述导管中远离所述推送机构的一端的一个或多个节导管中的导丝一一对应地连接所述绳驱动模块，所述导管中其余节导管中的导丝一一对应地连接所述绳固定模块；所述绳驱动模块和推送机构均连接所述控制器；

所述绳驱动模块包括电机驱动机构，该电机驱动机构的输出端连接所述导丝，用于缠绕和释放导丝；

将所述导管远离所述推送机构的一端视为末端，接近所述推送机构的一端视为始端，将连接有所述绳驱动模块的节导管视为主动导管；

所述控制方法包括：

推送步骤：获取所述导管末端的推送速度，根据该推送速度控制所述推送机构；

导丝驱动步骤：获取所述导管末端的目标姿态，基于该导管末端的目标姿态以及最接近所述导管始端的所述主动导管的始端位置，根据所述主动导管的节变换矩阵，获取每个所述主动导管的旋转角度和弯曲角度，从而计算每个所述主动导管中各个导丝的长度变化量，根据各个导丝的长度变化量控制对应的所述绳驱动模块。

进一步地，每个所述主动导管的旋转角度和弯曲角度的计算过程具体为，

构建所述导管末端的姿态相对于最接近所述导管始端的主动导管的始端姿态的整体姿态变换矩阵，根据最接近所述导管始端的主动导管当前的始端姿态和所述导管末端的目标姿态，对所述整体姿态变换矩阵进行求解，得到每个所述主动导管的旋转角度和弯曲角度。

进一步地，相邻的两个所述节导管间设有连接件，所述整体姿态变换矩阵的表达式为：

式中，n为从最接近导管尾端方向起主动导管的序号，

为第n个主动导管尾端相对于始端的位姿变换矩阵，c为cos函数的简写，α_n为第n个主动导管的旋转角度，β_n为第n个主动导管的弯曲角度，s为sin函数的简写，L_n为第n个主动导管的长度，d为连接件的长度。

进一步地，所述节导管的管壁沿同一圆周均匀设有三个所述导丝，所述主动导管中各个导丝的长度变化量的计算表达式为：

式中，n为从最接近导管尾端方向起主动导管的序号，Δl_n1为第n个主动导管中第1个导丝的长度变化量，Δl_n2为第n个主动导管中第2个导丝的长度变化量，Δl_n3为第n个主动导管中第3个导丝的长度变化量，r为主动导管中导丝所在圆周的半径，α_n为第n个主动导管的旋转角度，β_n为第n个主动导管的弯曲角度。

进一步地，根据各个导丝的长度变化量，通过PID控制对应的所述绳驱动模块。

进一步地，所述控制方法包括启动模式、前进控制模式和后退控制模式，

所述启动模式具体为，获取启动信号，控制所述体内导入装置整体运行；

所述前进控制模式具体为，获取前进控制信号，调整所述推送机构为驱动导管前进的第一转动方向，并执行所述推送步骤和导丝驱动步骤；

所述后退控制模式具体为，获取后退控制信号，调整所述推送机构为驱动导管后退的第二转动方向，并执行所述推送步骤和导丝驱动步骤；

进一步地，所述控制方法还包括暂停控制模式：获取暂停控制信号，控制所述推送机构和绳驱动模块停止动作。

进一步地，所述控制方法还包括紧急断电控制模式：获取紧急断电控制信号，控制所述体内导入装置整体断电。

本发明还提供一种体内导入装置的控制系统，包括存储器和处理器，所述存储器存储有计算机程序，处理器调用所述计算机程序执行如上所述的方法的步骤。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质上储存有计算机程序，所述计算机程序被处理器执行如上所述的方法。

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

(1)本发明采用多个节导管串联集成导管，各节导管的管壁设有骨架弹簧和导丝，通过控制导丝的长度实现对导管末端姿态的控制，通过推送机构控制导管整体的前进和后退；该方案对患者支气管壁损坏较小，对导管的可控性强、控制精度高、可进入更小支气管分支；

本发明对导丝的驱动可根据设定的导管末端的目标姿态以及主动导管整体的始端位置，对各主动导管的节变换矩阵进行求解，获取每个主动导管的旋转角度和弯曲角度，进而求得每个导丝的长度变化量，实现对导管的自动控制，在保证控制精确的同时，使得操作更加方便。

(2)本发明考虑到越导管尾端的主动导管的旋转角度和弯曲角度变化，会影响到其它主动导管的变化，因此从尾端的主动导管开始将各主动导管的位姿变换矩阵依次相乘，得到整体姿态变换矩阵，保证了计算结果的准确性。

(3)本发明考虑到实际操作时对各情景的控制需求，在控制方法中建立了启动模式、前进控制模式、后退控制模式、暂停控制模式和紧急断电控制模式，提高了控制的便利性和安全性。

附图说明

图1为本发明实施例中体内导入控制装置的整体结构示意图；

图2为本发明实施例中绳驱动模块的结构示意图；

图3为本发明实施例中绳张紧机构的结构示意图；

图4为本发明实施例中导管的内部结构示意图；

图5为本发明实施例中导管的整体结构示意图；

图6为本发明实施例中导管的工作状态示意图；

图7为本发明实施例中推送机构的结构示意图；

图8为本发明实施例中导管内导丝弯曲角度示意图；

图9为本发明实施例中单节导管弯曲角度示意图；

图10为本发明实施例中单节导管的受力分析图，其中，(a)为单节导管受力分析示意图，由对称性可知，合力P的作用线在导管两端的连线上。末端弯曲角度为β₀，连线转角为β₀/2；(b)为对称处理后，取一段对称部分分析示意图，在其末端建立坐标系XYZO以便于分析其弯曲变形情况，导管任一位置弯曲角度为β，弧长为s，末端在坐标为(x_a,y_a)；

图11为单节导管末端坐标变换过程示意图；

图12为本发明实施例中控制方法的流程示意图；

图中，1、导管，101、导丝，102、骨架弹簧，103、管壁，104、鞘，105、通孔，2、推送机构，201、第二底座，202、被动偏心轮，203、主动摩擦轮，204、第二电机，3、绳驱动模块，4、控制器，5、手柄，501、启动按钮，502、后退模式按钮，503、前进模式按钮，504、暂停按钮，505、可旋转摇杆，506、前后可动摇杆，507、紧急断电按钮，6、绳张紧机构，601、调整头，602、调整固定块，603、调整活动块，604、调整杆，605、底板，606、可动板，607、第一压块，608、第二压块，609、第一可动轮，610、第二可动轮，611、第一固定轮，612、第二固定轮，613、第三固定轮，7、电机驱动机构，701、第一电机，702、电机座，8、导管支架，9、第一底座，10、镜体，11、灯源，12、摄像头，13、活检针。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例1

本实施例提供一种体内导入装置的控制方法，下面对体内导入装置及其控制方法分别进行描述。

一、体内导入装置

体内导入装置包括导管1、推送机构2、绳驱动模块3、绳固定模块和控制器4；

推送机构2包括主动摩擦单元和被动摩擦单元，主动摩擦单元连接有驱动单元，主动摩擦单元和被动摩擦单元共同碾压导管1，驱动单元通过驱动主动摩擦单元从而带动导管1前进和后退；

导管1由多个节导管串联集成，每个节导管的内部均设有沿节导管纵向分布的通孔105，管壁103均设有沿节导管纵向分布的骨架弹簧102和导丝101，导管1远离推送机构的一端的一个或多个节导管中的导丝101一一对应地连接绳驱动模块3，导管中其余节导管中的导丝101一一对应地连接绳固定模块；绳驱动模块3和推送机构2均连接控制器4；本实施例中，相邻两个节导管间还连接有连接件，该连接件可以为橡胶材料的连接套。

绳驱动模块3包括电机驱动机构7，该电机驱动机构7的输出端连接导丝101，用于缠绕和释放导丝101。

每个节导管的管壁103中沿同一圆周均匀设有三个骨架弹簧102和导丝101。

作为一种优选的实施方式，绳驱动模块3和绳固定模块均还设有绳张紧机构6，导丝101经过绳张紧机构6，连接电机驱动机构7的输出端；

绳张紧机构6包括固定轮、可动轮、可动轮驱动单元和底板605，底板605分别固定连接固定轮和可动轮驱动单元，可动轮驱动单元的输出端连接可动轮，用于带动可动轮水平移动，可动轮与固定轮相配合，用于抵接导丝101，并控制导丝101的指向。

作为一种优选的实施方式，可动轮驱动单元包括调整头601、调整固定块602、调整活动块603、调整杆604、可动板606、第一压块607和第二压块608；

调整头601连接调整杆604，并受调整固定块602固定，调整固定块602受底板605固定；调整杆604还连接调整活动块603，用于带动调整活动块603移动；可动板606分别连接调整活动块603和可动轮，底板605设有供可动板606水平移动的导轨，第一压块607和第二压块608均连接底板605，分别位于可动板606的两侧，并设有限制可动板606上下位移的凹槽。

作为一种优选的实施方式，电机驱动机构7包括第一电机701和电机座702，绳驱动模块3还包括第一底座9，电机座702分别连接第一电机701和第一底座9，第一电机701的输出端连接导丝101，第一电机701受控制器4控制。

作为一种优选的实施方式，主动摩擦单元包括主动摩擦轮203，被动摩擦单元包括被动偏心轮202，驱动单元包括第二电机204，推送机构2还包括支撑整个推送机构2的第二底座201，主动摩擦轮203和被动偏心轮202共同碾压导管1，第二电机204连接主动摩擦轮203，用于带动主动摩擦轮203转动，第二电机204连接控制器4。

作为一种优选的实施方式，通孔105内用于放置灯源11、摄像头12或活检针13。

作为一种优选的实施方式，骨架弹簧102为圆柱螺旋弹簧。

作为一种优选的实施方式，节导管的管壁103采用硅胶材质。

作为一种优选的实施方式，导管1分为内层和外层，内层为集成的多个节导管，外层为鞘104。

作为一种优选的实施方式，导管1远离推送机构2的一端的三个节导管中的导丝101均连接绳驱动模块3。

作为一种优选的实施方式，控制器4还连接有手柄5。

将上述优选的实施方式进行组合可以得到一种最优的实施方式，下面对该最优的实施方式的工作原理进行描述。

本实施例中控制器采用微信处理机，通过微信处理机控制各个第一电机701，实现导管的旋转、弯曲运动，控制第二电机204实现导管的推拉运动；微型处理机后端连接手柄，进而实现推动手柄摇杆控制导管运动，且二者运动坐标系统一。手柄上分别设有控制导管上下左右和前后运动的摇杆，摇杆拨动方向与导管头部运动方向一致。

借鉴血管介入手术领域所用导管，本导管采用多节式软体结构，管壁为硅胶材质，中间有通孔，便于支气管镜或活检钳的伸入伸出。该导管外层为鞘，起到密封和保护作用。如图4所示，导管管壁中心圈每隔60°穿孔，便于穿过导丝和骨架弹簧，骨架弹簧为圆柱螺旋弹簧，且这两者间隔穿入。值得注意的是，如图5所示，三根骨架弹簧从导管头部延伸至尾部。导管的驱动方式采用绳驱动，即第一节导管穿入三根导丝，之后每增加一节导管增加三根导丝，所有导丝都延伸至导管尾部进行张紧，但除前三节导管9根导丝外的导丝只张紧，不采用电机旋转控制。

导管的弯曲、旋转运动通过电机旋转带动绑在电机上的导丝旋转实现，而导管的伸缩运动通过安装于绳驱动模块3前端的推送机构2实现，如图1所示，推送机构2由两个摩擦轮和电机构成。电机控制摩擦轮的正转和反转，从而实现导管的伸缩运动。旋转用电机和推送用电机都由微型处理机控制实现，其后端连接手柄，从而实现手柄控制导管运动的目的。

下面详细说明各部分的具体实现方法：

如图2所示，绳驱动模块3包括导丝101，绳张紧机构6，第一电机701、电机座702，导管受导管支架8固定，第一电机701的输出端连接有转轴。导丝绕在转轴上，从而电机旋转可带动导丝旋转，而如图3，绳张紧装置包括调整头601、调整固定块602、调整活动块603、调整杆604、底板605、可动板606、第一压块607、第二压块608、第一可动轮609、第二可动轮610、第一固定轮611、第二固定轮612和第三固定轮613。工作时，通过旋转调整头601，可使调整活动块603和调整杆604移动，从而使可动板606以及其上第一可动轮609和第二可动轮610移动，达到张紧绳的目的，第一可动轮609和第二可动轮610越靠近第一固定轮611、第二固定轮612和第三固定轮613，则绳的张紧效果越好，具体操作时，可综合绳的强度与所需张紧程度调节第一可动轮609和第二可动轮610，以达到最佳张紧效果。

如图4所示，导管内部结构为中央通孔105，导管圆周中心线上以60°为间隔均匀排布导丝101、骨架弹簧102，图4所示为第一个节导管，其后每增加一节导管增加三根导丝，如图5。且每根导丝都延伸至导管尾部并单独连接一套绳张紧装置，前三节导管上的导丝还需连接至电机，以进行旋转控制。

导管1的工作过程如图6所示，先在导管内伸入镜体10，镜体10包括灯源11和摄像头12。当导管1到达病灶附近，则退回镜体10，再向管内通孔插入活检针13，完成活检后，再退出活检针13，随后缩回导管1。

如图7所示为导管的推送机构2，由第二底座201、被动偏心轮202、主动摩擦轮203和第二电机204组成。导管被夹持在两个摩擦轮之间，通过导管与摩擦轮表面间产生的摩擦力，完成导管的推拉动作。第二电机204连接微型处理机，通过手柄5发出前进后退信号给微型处理器，再由微型处理器控制第二电机204的正反转。当第二电机204正转时，带动主动摩擦轮203正转，实现导管1的前进；反之，第二电机204反转实现导管1的后退。

二、控制方法

如图12所示，控制方法包括：

推送步骤：获取导管末端的推送速度，根据该推送速度控制推送机构；

导丝驱动步骤：获取导管末端的目标姿态，基于该导管末端的目标姿态以及最接近导管始端的主动导管的始端位置，根据主动导管的节变换矩阵，获取每个主动导管的旋转角度和弯曲角度，从而计算每个主动导管中各个导丝的长度变化量，根据各个导丝的长度变化量控制对应的绳驱动模块。

每个主动导管的旋转角度和弯曲角度的计算过程具体为，

构建导管末端的姿态相对于最接近导管始端的主动导管的始端姿态的整体姿态变换矩阵，根据最接近导管始端的主动导管当前的始端姿态和导管末端的目标姿态，对整体姿态变换矩阵进行求解，得到每个主动导管的旋转角度和弯曲角度。

相邻的两个节导管间设有连接件，整体姿态变换矩阵的表达式为：

式中，n为从最接近导管尾端方向起主动导管的序号，

为第n个主动导管尾端相对于始端的位姿变换矩阵，c为cos函数的简写，α_n为第n个主动导管的旋转角度，β_n为第n个主动导管的弯曲角度，s为sin函数的简写，L_n为第n个主动导管的长度，d为连接件的长度。

节导管的管壁沿同一圆周均匀设有三个导丝，主动导管中各个导丝的长度变化量的计算表达式为：

式中，n为从最接近导管尾端方向起主动导管的序号，Δl_n1为第n个主动导管中第1个导丝的长度变化量，Δl_n2为第n个主动导管中第2个导丝的长度变化量，Δl_n3为第n个主动导管中第3个导丝的长度变化量，r为主动导管中导丝所在圆周的半径，α_n为第n个主动导管的旋转角度，β_n为第n个主动导管的弯曲角度。

优选地，根据各个导丝的长度变化量，通过PID控制对应的绳驱动模块。

控制方法包括启动模式、前进控制模式、后退控制模式、暂停控制模式和紧急断电控制模式，

启动模式具体为，获取启动信号，控制体内导入装置整体运行；

前进控制模式具体为，获取前进控制信号，调整推送机构为驱动导管前进的第一转动方向，并执行推送步骤和导丝驱动步骤；

后退控制模式具体为，获取后退控制信号，调整推送机构为驱动导管后退的第二转动方向，并执行推送步骤和导丝驱动步骤；

暂停控制模式：获取暂停控制信号，控制推送机构和绳驱动模块停止动作。

紧急断电控制模式：获取紧急断电控制信号，控制体内导入装置整体断电。

下面对整体姿态变换矩阵和导丝长度变化量计算表达式的推导过程分别进行描述。

1、整体姿态变换矩阵

导管弯曲变形的主要取决于骨架弹簧的弯曲变形，本发明采用的是小升角(α<8-10°)圆柱螺旋弹簧，为了便于这里理论建模分析，将其等效为等长的细长干。其等效抗弯刚度(EI_d)为：

式中，E为材料的弹性模量，I为圆柱螺旋弹簧簧丝横截面的惯性矩，μ为材料的泊松比。

如图8所示，三根驱动绳上的驱动力分别为P₁,P₂,P₃。通过在驱动绳上施加不同的载荷来使得骨架弹簧弯曲变形，从而导管的姿态，位置也随之改变。作用在骨架弹簧上的合力为P，合力矩为M，由导管结构的对称性，以及载荷施加的方式可知，骨架弹簧所受载荷也具有对称性，因此只要取对称结构的对称部分进行分析，即可得到整个骨架弹簧的受力变形分析结果，前面已将导管骨架等效为细长杆。

如图10，如果将弯曲变形曲线假设为圆弧，则弯曲角度β₀为：

式中，L₀为单节等效细长杆长度，R_c为弯曲圆弧对应的弯曲半径。

驱动绳上的驱动力为P_i，驱动绳所在圆弧半径为r，则圆弧弯曲下单根驱动绳作用下对导管受力分析可得：

利用上两式易求求得各弯曲角度下，导管骨架弹簧的等效细长杆的弯曲圆弧方程以及绳上的驱动力。

如图8所示，单节导管的长度为l₀，在主动导管始端面建立坐标系O₁X₁Y₁Z₁，在末端面建立坐标系O₅X₅Y₅Z₅，Z轴垂直于端面，X轴在端面平面里，朝向第一个绳孔，导管转动与弯曲角度分别为α₀，β₀。

在手术中，导管先由推送机构推送一段距离L，然后转动与弯曲，O₁O₅为两点间的直线距离，对其进行系列坐标变换。

由于导管的两节结构相同，有相同的自由度，因此首先分析单节导管的运动学，在此基础上推导多节导管的运动学。如图9所示，单节介入主动导管的长度为l₀，在主动导管始端面建立坐标系O₁X₁Y₁Z₁，在末端面建立坐标系O₅X₅Y₅Z₅，Z轴垂直于端面，X轴在端面平面里，朝向第一个绳孔，导管转动与弯曲角度分别为α₀,β₀。

在手术中，导管先由推送机构推送一段距离L，然后转动与弯曲，O₁O₅为两点间的直线距离，具体变换如图11所示。

1.基坐标系O₀X₀Y₀Z₀沿Z₀平移L到坐标系O₁X₁Y₁Z₁，变换记为

2.坐标系O₁X₁Y₁Z₁沿矢量O₁O₅移动到O₂X₂Y₂Z₂，变换记为

3.坐标系O₂X₂Y₂Z₂绕z₂轴顺时针转动α₀角到坐标系O₃X₃Y₃Z₃，变换记为

4.坐标系O₃X₃Y₃Z₃绕x₃轴顺时针转动β₀角到坐标系O₄X₄Y₄Z₄，变换记为

5.坐标系O₄X₄Y₄Z₄绕z₄轴逆时针转动α₀角到坐标系O₅X₅Y₅Z₅，变换记为

在实际操作中首末端面之间并没有相对的扭转，所以这里需要进行第5步的变化沿Z轴反向旋转α₀角消除之前变化过程中产生的扭转。

由位姿变换的相关知识可知，绕x轴，y轴和z轴旋转的θ角的旋转矩阵分别为：

式中，sθ是sinθ的简写，cθ是cosθ的简写。因此上述5个变换可表示如下：

单节介入主动导管末端面相对于首端面的位姿姿态变换矩阵为

单节导管末端面经过推送、旋转、弯曲动作后在基座标系O₁X₁Y₁Z₁中的位姿为

可得末端点坐标与导管长度l₀，送进距离L，单节介入主动导管的旋转角度α₀，弯曲角度β₀的关系：

在设计时，为了满足治疗的需求，头部采用三节导管串联使得导管机器人更加灵活，可达工作空间更大。

单节介入主动导管有两个弯曲自由度，整段导管有一个送进自由度，因此三节导管串联时，共有六个转动自由度加上一个整体的送进自由度共七个自由度，第一节绳驱动主动导管的长度为L₁，旋转角度为α₁，弯曲角度为β₁，则第二节绳驱动导管为L₂，α₂，β₂，第三节为L₃，α₃，β₃，相邻导管间的连接件长度为d。

对于多节导管可以将其中每一节导管作为一个整体，按照单节的理论求得其末端相对始端的变换矩阵，再将各节变换矩阵相乘即可得到多节导管末端相对始端的变换矩阵。这里将各导管间的连接件处理成一段送进距离，从而可以直接利用单节导管的矩阵变换形式。

式中，第一节绳驱动导管变换矩阵为

第二节为

第三节为

那么三节串联绳驱动导管的运动学正解的变换矩阵为：

本发明支持增加导管节数，这里给出n节导管分析。动导管长L_n，旋转角度为α_n，弯曲角度为β_n。第n节导管的姿态转换矩阵形式如下：

那么n节串联时的绳驱动介入主动导管的姿态变换矩阵形式为：

2、导丝长度变化量计算表达式

如图10所示，关节空间的旋转与弯曲角度α_n，β_n是通过三根绳长的变化来控制的。当单节导管旋转角α₀，弯曲角β₀时，各段绳长的变化为：

Δl_j＝l₀-l_j

式中，Δl_j表示第j根绳的绳长变化，l_j表示第j根绳的绳长，下面以第1根绳为例推导绳长变化与导管关节角度的关系：

Δl₁＝l₀-l₁＝R₁β₀-R₁β₀＝O₁D₂β₀-A₁D₁β₀

＝O₁D₂β₀-MD₂β₀＝O₁Mβ₀＝rβ₀cos(α₀-π/2)

式中r(2.5mm)表示绳孔所在圆周的半径，M,N为绳孔圆心在O₁D₂上的垂足。三根驱动绳均匀分布在导管圆周上，两两夹角为

同理可得：

Δl₂＝l₀-l₂＝rcos(α₀-π/2+2π/3)β₀

＝rβ₀cos(α₀+π/6)

Δl₃＝l₀-l₃＝rcos(α₀-π/2+4π/3)β₀

＝rβ₀cos(α₀+5π/6)

由于第二节导管的驱动绳是从第一节导管中穿过的，所以第一节导管的运动势必影响第二节导管中驱动绳的绳长变化，第二节导管单独运动时则不会影响第一节导管中绳长的变化，因此这里只要计算出第一节导管运动时影响第二节导管的耦合量，即可实现运动上的解耦。

第二节导管单独运动时，绳长变化可由上式求得。

第一节导管的旋转、弯曲角度分别为α₁β₁，第二节导管的旋转、弯曲角度分别为α₂β₂。则第二节导管的绳长变化为第一节的耦合影响与自身变化的叠加。第一节导管中有6根均布圆周的绳穿过，两两夹角为

第二节导管绳长变化计算如下：

以此类推，可以得到第n(n＞1)节导管的绳长变化表达形式：

由此我们可在已知要使头端第一节达到的形态的旋转角α₁以及弯曲度β₁、当前导管状态即可计算相应Δl_ij，即相应电机M_ij的移动量，则据PID控制：

即可求得下一时刻M_ij的速度V_ij-next等控制信息，以上运算均由STM32进行处理并将控制信息发送给相应电机，实现绳驱动的精准闭环控制。

本实施例还对推送机构的推送力和推送速度进行了分析，具体如下：

推送力分析：

推送机构在推送导管时，主动摩擦轮两侧的支撑滚动轴承也会对其产生滚动摩擦力矩，可以表示为下式：

式中，μ为滚动轴承的摩擦系数，μ＝0.0015；P为轴承负荷；F_r为轴承径向力，F_r≈6N；F_a为轴承轴向力；d为轴承内径，d＝4mm；

因此，负载所需要的转矩M＝2M_A＝0.054N·mm。

本实施例为安全考虑，选取安全系数K＝2，电机及轴承传动效率η＝η₁η₂η₃＝0.945，则电机所需要输送的转矩可以计算得到，如式

推送速度分析：

推送介入导管的速度指的是主动摩擦轮的线速度，假设设计导管推送的线速度最大值为v_max＝60mm/s，主动摩擦轮的最大角速度为ω_max。推送介入导管时，主动摩擦轮的最大转速n_max可以计算得到，表示为式：

式中，v_max为导管推送最大速度[mm/s]；D为主动摩擦轮的凹槽直径[mm]。

工作过程描述：

医师术前了解病患位置并进行路径规划后，将镜体顺导管中心通孔伸入并固定于前端卡口106，再将导管顺套管伸入患者气管中，便可控制导管运动。当导管头部到达病灶，医生可撤出镜体，再沿该通孔放入活检针，取部分目标病灶后撤出活检针，放松已张紧的各条导丝，再沿原路缩回导管，完成活检手术。

本发明还提供一种体内导入装置的控制系统，包括存储器和处理器，所述存储器存储有计算机程序，处理器调用所述计算机程序执行如上所述的体内导入装置的控制方法。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质上储存有计算机程序，所述计算机程序被处理器执行如上所述的体内导入装置的控制方法。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (6)

1.一种体内导入装置的控制系统，其特征在于，包括存储器和处理器，所述存储器存储有计算机程序，处理器调用所述计算机程序执行一种体内导入装置的控制方法，所述体内导入装置包括导管(1)、推送机构(2)、绳驱动模块(3)、绳固定模块和控制器(4)；

所述推送机构(2)用于带动所述导管(1)前进和后退；

所述导管(1)由多个节导管串联集成，每个节导管的内部设有沿节导管纵向分布的通孔(105)，每个所述节导管的管壁(103)中沿同一圆周均匀设有多个骨架弹簧(102)和导丝(101)；所述导管(1)中远离所述推送机构的一端的一个或多个节导管中的导丝(101)一一对应地连接所述绳驱动模块(3)，所述导管(1)中其余节导管中的导丝(101)一一对应地连接所述绳固定模块；所述绳驱动模块(3)和推送机构(2)均连接所述控制器(4)；

将所述导管远离所述推送机构的一端视为末端，接近所述推送机构的一端视为始端，将连接有所述绳驱动模块的节导管视为主动导管；

所述控制方法包括：

推送步骤：获取所述导管末端的推送速度，根据该推送速度控制所述推送机构；

导丝驱动步骤：获取所述导管末端的目标姿态，基于该导管末端的目标姿态以及最接近所述导管始端的所述主动导管的始端位置，根据所述主动导管的节变换矩阵，获取每个所述主动导管的旋转角度和弯曲角度，从而计算每个所述主动导管中各个导丝的长度变化量，根据各个导丝的长度变化量控制对应的所述绳驱动模块；

每个所述主动导管的旋转角度和弯曲角度的计算过程具体为，

构建所述导管末端的姿态相对于最接近所述导管始端的主动导管的始端姿态的整体姿态变换矩阵，根据最接近所述导管始端的主动导管当前的始端姿态和所述导管末端的目标姿态，对所述整体姿态变换矩阵进行求解，得到每个所述主动导管的旋转角度和弯曲角度；

将骨架弹簧等效为细长杆，将导管弯曲变形曲线假设为圆弧，则主动导管的弯曲角度β₀为：

式中，L₀为单节等效细长杆长度，R_c为弯曲圆弧对应的弯曲半径；

相邻的两个所述节导管间设有连接件，所述整体姿态变换矩阵的表达式为：

式中，n为从最接近导管尾端方向起主动导管的序号，

为第n个主动导管尾端相对于始端的位姿变换矩阵，c为cos函数的简写，α_n为第n个主动导管的旋转角度，β_n为第n个主动导管的弯曲角度，s为sin函数的简写，L_n为第n个主动导管的长度，d为连接件的长度。

2.根据权利要求1所述的一种体内导入装置的控制系统，其特征在于，所述节导管的管壁沿同一圆周均匀设有三个所述导丝，所述主动导管中各个导丝的长度变化量的计算表达式为：

式中，n为从最接近导管尾端方向起主动导管的序号，Δl_n1为第n个主动导管中第1个导丝的长度变化量，Δl_n2为第n个主动导管中第2个导丝的长度变化量，Δl_n3为第n个主动导管中第3个导丝的长度变化量，r为主动导管中导丝所在圆周的半径，α_n为第n个主动导管的旋转角度，β_n为第n个主动导管的弯曲角度。

3.根据权利要求1所述的一种体内导入装置的控制系统，其特征在于，根据各个导丝的长度变化量，通过PID控制对应的所述绳驱动模块。

4.根据权利要求1所述的一种体内导入装置的控制系统，其特征在于，所述控制方法包括启动模式、前进控制模式和后退控制模式，

所述启动模式具体为，获取启动信号，控制所述体内导入装置整体运行；

所述前进控制模式具体为，获取前进控制信号，调整所述推送机构为驱动导管前进的第一转动方向，并执行所述推送步骤和导丝驱动步骤；

所述后退控制模式具体为，获取后退控制信号，调整所述推送机构为驱动导管后退的第二转动方向，并执行所述推送步骤和导丝驱动步骤。

5.根据权利要求4所述的一种体内导入装置的控制系统，其特征在于，所述控制方法还包括暂停控制模式：获取暂停控制信号，控制所述推送机构和绳驱动模块停止动作。

6.根据权利要求5所述的一种体内导入装置的控制系统，其特征在于，所述控制方法还包括紧急断电控制模式：获取紧急断电控制信号，控制所述体内导入装置整体断电。

Images