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

权利要求书

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所述的一种体内导入装置的控制系统，其特征在于，所述控制方法包括启动模式、前进控制模式和后退控制模式，

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

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

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