一种消化内镜辅助介入机器人的控制系统与控制方法.pdf

本发明提供了一种消化内镜辅助介入机器人的控制方法与系统，属于医疗机器人领域。所述方法包括：提取操作者操作主端手柄的动作信息进行处理，根据设定的位置控制与速度控制工作模式得到机器人的运动指令；根据安装在执行器端的力反馈装置测量输送阻力与扭转阻力矩信息，经零偏与重力干扰去除后显示在人机交互界面进行提示，反馈至主端控制手柄提供给操作者力感知当阻力与阻力矩超出设定值，上位机报警，并通过局部自主调节输送力与力矩至安全范围，并；还包括提取脚踏板的信息进行处理，进行工作模式的切换与送气、送水、抽吸功能。所述控制系统包括主端控制装置、底层控制器、力反馈装置、限位保护装置、电机驱动器、气压调节装置、光电耦合器与电磁阀。采用本发明提高了消化内镜辅助介入机器人的安全性。

1.  一种消化内镜辅助介入机器人的控制系统，其特征在于，包括主端控制装置、手柄、脚踏板、底层控制器、电机驱动器、气压调节装置、光电耦合器、电磁阀、力反馈装置和限位保护装置；
所述力反馈装置，用于测量消化内镜介入时近端遇到的阻力和阻力矩；
所述气压调节装置，用于调节气路的气压，控制执行器对消化内镜的夹持力；
所述电磁阀，由光电耦合器控制其通断，用于控制执行器夹持机构的充气与放气，配合执行器驱动机构完成消化内镜输送；
所述底层控制器，用于接收主端控制装置命令处理后发送给所述电机驱动器、气压调节装置、光电耦合器，采样阻力与阻力矩信息传送给所述主端控制装置；
所述限位保护装置，用于保护机器人执行器与消化内镜操作部旋钮不受损坏；
所述主端控制装置，与底层控制器双向连接；
所述底层控制器，第一输出端连接所述电机驱动器，第二输出端连接所述气压调节装置的输入端，IO输出端连接所述光电耦合器的输入端，第一输入端连接所述力传感器的输出端，第二输入端连接所述扭矩传感器的输出端，第三输入端连接所述限位保护装置的输出端；
所述手柄，连接主端控制装置，用于控制胃镜的末端位姿；
所述脚踏板，连接主端控制装置，用于切换工作模式与控制送气、送水、抽吸功能。

2.  根据权利要求1所述的一种消化内镜辅助介入机器人的控制系统，其特征在于，所述力反馈装置包括安装在机器人执行器端的阻力测量装置和阻力矩测量装置。

3.  根据权利要求1所述的一种消化内镜辅助介入机器人的控制系统，其特征在于，所述主端控制装置包括：
手柄信息提取与处理模块，用于提取手柄动作信息，并处理得到机器人运动指令；
脚踏板信息提取与处理模块，用于提取脚踏板动作信息，处理得到工作模式以及送气、送水与抽吸命令；
力反馈信息处理模块，用于去除力反馈装置测量阻力和阻力矩中零偏和重力的干扰，显示至人机交互界面模块，并根据预设比例映射到主端控制手柄；
人机交互界面模块，用于工作模式的显示设定与机器人状态的显示。

4.  根据权利要求3所述的一种消化内镜辅助介入机器人的控制系统，其特征在于，所述人机交互界面模块包括：
力信息提示与报警模块，用于显示输送阻力与扭转阻力矩信息，并在所述阻力与阻力矩超过预设值时启动图像或声音提示报警；
消化内镜动态信息提示模块，用于提示机器人末端执行器的动态信息，包括对消化内镜的输送、旋转、弯曲以及送气、送水、抽吸操作；
消化内镜末端三维姿态显示模块，用于提示消化内镜末端的三维姿态信息，方便操作者进行操作；
工作模式显示设定模块，用于显示与设定控制模式。

5.  根据权利要求1所述的一种消化内镜辅助介入机器人的控制系统，其特征在于，所述工作模式的修改方式包括人机交互界面设定修改与脚踏板设定修改方式。

6.  一种消化内镜辅助介入机器人的控制方法，其特征在于，包括以下步骤：
主端控制装置与底层控制器参数初始化，设定工作模式：速度控制模式或位置控制模式；
启动定时器，读取手柄信息作为机器人执行器的控制命令；
当设定工作模式为速度控制模式时，计算手柄按键按下后的位移变化，对位移变化进行比例放大作为执行器的运行速度，发送命令至底层控制器；
当设定工作模式为位置控制模式时，根据建立的消化内镜的近远端对应关 系模型，通过手柄的位置变化进行雅克比反变换计算机器人各关节变量的变化量，并发送控制命令至底层控制器；
主端控制装置通过发送的控制命令根据消化内镜近远端对应关系模型计算消化内镜末端三维姿态的变化；
消化内镜末端三维姿态变化显示至人机交互界面；
底层控制器对主端控制装置的命令进行转换，发送给电机驱动器、气压调节装置、光电耦合器；
电机驱动器驱动电机按照控制命令运动，气压调节装置调节气路气压，光电耦合器通过控制电磁阀通断控制执行器夹持机构配合驱动机构完成镜体的输送。

7.  根据权利要求6所述的一种消化内镜辅助介入机器人的控制方法，其特征在于，所述计算消化内镜末端三维姿态的变化包括以下步骤：
消化内镜末端三维姿态是实时更新的，在控制指令发出的同时，消化内镜末端的三维姿态显示模块会根据消化内镜近远端对应关系模型计算出内镜末端的位置与姿态变化，与当前位姿比较并进行更新。

8.  根据权利要求6所述的一种消化内镜辅助介入机器人的控制方法，其特征在于，所述启动定时器时，还读取力反馈装置返回的阻力与阻力矩信息，执行如下操作：
主端控制装置力反馈信息处理模块对采集到得阻力信息与阻力矩信息进行处理，去除零偏与重力干扰；
力信息经处理后反馈至人机界面显示，并根据阻力与阻力矩值的大小进行显示，当阻力或阻力矩值大于预设警告阈值时，人机界面出现画面提示并伴随有声音提示，此时末端执行器具有一定的自主性，通过调节自身位姿使阻力或阻力矩恢复至安全范围内；
力信息经处理后，经预设比例放大后，映射至手柄给操作者提供力感知。

9.  根据权利要求6所述的一种消化内镜辅助介入机器人的控制方法，其特征在 于，所述启动定时器时，还读取脚踏板的信息，根据脚踏板的状态实现工作模式切换与送气、送水、抽吸功能。

一种消化内镜辅助介入机器人的控制系统与控制方法
技术领域
本发明属于医疗设备技术领域，公开了一种消化内镜辅助介入机器人的控制装置，用于柔性内镜介入辅助机器人的控制。
背景技术
据世界卫生组织统计，全球有70%左右的人患有不同程度的胃肠道疾病。我国有近8000万的胃肠道疾病患者，且随着老龄化人口增长、生活节奏加快、社会应激因素的增加等，胃肠肿瘤、胃肠道出血、胃肠功能紊乱等疾病发病率增加，社会需求巨大。消化内镜检查已成为消化道疾病诊断与治疗的常规手段，具有损伤小、痛苦少、并发症少、恢复快等优点。国内县级及以上医院98%均已开展消化内镜检查，但医生的操作和诊断水平存在较大的差异，而且病人数量远远多于所需要的内镜检查医生；此外，消化道疾病内镜检查诊治具有高污染的风险特点。医生在诊断过程中长期接触病人的唾液、胃肠液、分泌物及污染的血液等，职业风险大，致使工作人员成为易受感染和辐射等的高危人群，并且通过医护人员可能在患者中广泛传播。随着内镜下治疗项目越来越多，感染性疾病患者的内镜检查与治疗，内镜结合X射线等操作增加，受射线照射的机会大为增加。一方面辐射造成全身或局部损害，如致癌、遗传、造血器官、皮肤等损害；另一方面，开展此类手术需要置铅屏、穿铅衣、戴铅帽、脖套，必要时带防护镜，使得医护人员的操作环境昂贵和体力消耗巨大。
利用机器人的高精度、稳定性、灵活性辅助医生进行消化内镜的操作，提高消化内镜操作的水平、效率和安全性，减轻医护人员的人力工作，提高职业防护水平；缓解患者数量众多与训练有素医生严重不足的矛盾。本发明提出的控制系统与控制方法可以通过机器人辅助医生更高精度的完成内镜介入诊断与治疗，并提供力感知，提高操作的安全性。
发明内容
鉴于以上问题，本发明提出一种能够适用于消化内镜辅助介入机器人的控 制系统与控制方法，提高消化内镜辅助机器人介入操作的安全性。
本发明为实现上述目的所采用的技术方案是：一种消化内镜辅助介入机器人的控制系统，包括主端控制装置、手柄、脚踏板、底层控制器、电机驱动器、气压调节装置、光电耦合器、电磁阀、力反馈装置和限位保护装置；
所述力反馈装置，用于测量消化内镜介入时近端遇到的阻力和阻力矩；
所述气压调节装置，用于调节气路的气压，控制执行器对消化内镜的夹持力；
所述电磁阀，由光电耦合器控制其通断，用于控制执行器夹持机构的充气与放气，配合执行器驱动机构完成消化内镜输送；
所述底层控制器，用于接收主端控制装置命令处理后发送给所述电机驱动器、气压调节装置、光电耦合器，采样阻力与阻力矩信息传送给所述主端控制装置；
所述限位保护装置，用于保护机器人执行器与消化内镜操作部旋钮不受损坏；
所述主端控制装置，与底层控制器双向连接；
所述底层控制器，第一输出端连接所述电机驱动器，第二输出端连接所述气压调节装置的输入端，IO输出端连接所述光电耦合器的输入端，第一输入端连接所述力传感器的输出端，第二输入端连接所述扭矩传感器的输出端，第三输入端连接所述限位保护装置的输出端；
所述手柄，连接主端控制装置，用于控制胃镜的末端位姿；
所述脚踏板，连接主端控制装置，用于切换工作模式与控制送气、送水、抽吸功能。
所述力反馈装置包括安装在机器人执行器端的阻力测量装置和阻力矩测量装置。
所述主端控制装置包括：
手柄信息提取与处理模块，用于提取手柄动作信息，并处理得到机器人运 动指令；
脚踏板信息提取与处理模块，用于提取脚踏板动作信息，处理得到工作模式以及送气、送水与抽吸命令；
力反馈信息处理模块，用于去除力反馈装置测量阻力和阻力矩中零偏和重力的干扰，显示至人机交互界面模块，并根据预设比例映射到主端控制手柄；
人机交互界面模块，用于工作模式的显示设定与机器人状态的显示。
所述人机交互界面模块包括：
力信息提示与报警模块，用于显示输送阻力与扭转阻力矩信息，并在所述阻力与阻力矩超过预设值时启动图像或声音提示报警；
消化内镜动态信息提示模块，用于提示机器人末端执行器的动态信息，包括对消化内镜的输送、旋转、弯曲以及送气、送水、抽吸操作；
消化内镜末端三维姿态显示模块，用于提示消化内镜末端的三维姿态信息，方便操作者进行操作；
工作模式显示设定模块，用于显示与设定控制模式。
所述工作模式的修改方式包括人机交互界面修改与脚踏板修改方式。
一种消化内镜辅助介入机器人的控制方法，包括以下步骤：
主端控制装置与底层控制器参数初始化，设定工作模式：速度控制模式或位置控制模式；
启动定时器，读取手柄信息作为机器人执行器的控制命令；
当设定工作模式为速度控制模式时，计算手柄按键按下后的位移变化，对位移变化进行比例放大作为执行器的运行速度，发送命令至底层控制器；
当设定工作模式为位置控制模式时，根据建立的消化内镜的近远端对应关系模型，通过手柄的位置变化进行雅克比反变换计算机器人各关节变量的变化量，并发送控制命令至底层控制器；
主端控制装置通过发送的控制命令根据消化内镜近远端对应关系模型计算消化内镜末端三维姿态的变化；
消化内镜末端三维姿态变化显示至人机交互界面；
底层控制器对主端控制装置的命令进行转换，发送给电机驱动器、气压调节装置、光电耦合器；
电机驱动器驱动电机按照控制命令运动，气压调节装置调节气路气压，光电耦合器通过控制电磁阀通断控制执行器夹持机构配合驱动机构完成镜体的输送。
所述计算消化内镜末端三维姿态的变化包括以下步骤：
消化内镜末端三维姿态是实时更新的，在控制指令发出的同时，消化内镜末端的三维姿态显示模块会根据消化内镜近远端对应关系模型计算出内镜末端的位置与姿态变化，与当前位姿比较并进行更新。
所述启动定时器时，还读取力反馈装置返回的阻力与阻力矩信息，执行如下操作：
主端控制装置力反馈信息处理模块对采集到得阻力信息与阻力矩信息进行处理，去除零偏与重力干扰；
力信息经处理后反馈至人机界面显示，并根据阻力与阻力矩值的大小进行显示，当阻力或阻力矩值大于预设警告阈值时，人机界面出现画面提示并伴随有声音提示，此时末端执行器具有一定的自主性，回复至其上一状态，直至阻力或阻力矩恢复至安全范围内；
力信息经处理后，经预设比例放大后，映射至手柄给操作者提供力感知。
所述启动定时器时，还读取脚踏板的信息，根据脚踏板的状态实现工作模式切换与送气、送水、抽吸功能。
本发明具有以下优点及有益效果：
1.本发明控制消化内镜辅助机器人的输送器与操作器实现消化内镜的前进、后退、旋转、末端弯曲、送气、送水功能；
2.在控制过程中，采集力传感器与扭矩传感器的信息，给予主手力反馈；当力信息超出安全阈值时，通过局部自主调节输送力至安全范围；
3.计算机人机界面显示消化内镜状态信息、力与扭矩信息并有报警功能；
4.辅助医生完成消化内镜在人体内的介入操作，降低操作难度，提高操作的安全性，保障医护人员的身心健康。
附图说明
图1是控制系统总体框图；
图2是人机界面示意图；
图3是本发明的流程示意图。
具体实施方式
下面结合附图及实施例对本发明做进一步的详细说明。
如图1所示，一种消化内镜辅助介入机器人系统，包括主端控制装置1、手柄2、底层控制器3、电机驱动器4、气压调节装置5、电磁阀6、力反馈装置7、限位保护装置8、脚踏板9和光电耦合器10；所述力反馈装置7包括安装在机器人执行器端的阻力测量装置701和阻力矩测量装置702，用于测量消化内镜介入时近端遇到的阻力和阻力矩，所述气压调节装置5用于调节气路的气压，控制执行器对消化内镜的夹持力以及对消化内镜操作部按钮的压力，所述电磁阀6由光电耦合器10控制其通断，用于控制执行器夹持机构的充气与放气，配合执行器驱动机构完成消化内镜输送，所述电磁阀6还用于控制执行器按钮操作机构的运动，完成送气、送水、抽吸功能，所述底层控制器3用于接收主端控制装置命令处理后发送给电机驱动器4、气压调节装置5、光电耦合器10，采样阻力与阻力矩信息传送给主端控制装置1，所述限位保护装置用于保护机器人执行器与消化内镜操作部旋钮不受损坏；所述手柄2与所述主端控制装置1双向连接，所述脚踏板9与所述主端控制装置1双向连接，所述主端控制装置1与底层控制器2双向连接，所述底层控制器3的第一输出端连接电机驱动器4，所述底层控制器3的第二输出端连接气压调节装置5的输入端，所述底层控制器3的IO输出端连接光电耦合器106的输入端，所述底层控制器3的第一输入端连接所述阻力测量装置701的输出端，所述底层控制器3的第二输入端连接 所述阻力矩测量装置702的输出端，所述底层控制器3的第三输入端连接所述限位保护装置8的输出端；
所述主端控制装置1包括手柄信息提取与处理模块101、力反馈信息处理模块102、人机交互界面模块103、脚踏板信息提取与处理模块104，所述手柄信息提取与处理模块101用于提取手柄动作信息，并处理得到机器人运动指令；力反馈信息处理模块102用于去除力反馈装置测量阻力和阻力矩中零偏和重力的干扰，显示至人机交互界面模块，并根据预设比例映射到主端控制手柄2；所述脚踏板信息提取与处理模块104用于提取脚踏板信息，并处理得到运动模式切换与送气、送水、抽吸指令；
所述人机交互界面模块103用于工作模式的显示设定与机器人状态的显示，如图2所示，所述人机交互模块103包括力信息提示与报警模块1031、消化内镜末端三维姿态显示模块1032、消化内镜动态信息显示模块1033和工作模式显示设定模块1034，所述力信息提示与报警模块1031包括阻力显示与阻力矩显示，以同心圆的方式给予操作者直观显示，同心圆的半径与力信息的大小成正比关系，根据临床实验预设安全与警告两个阈值，分别对应内圆和外圆，当阻力或阻力矩小于安全阈值时，力信息显示呈绿色，当阻力或阻力矩在安全阈值与警告阈值之间时，力信息显示呈黄色，当阻力或阻力矩信息大于警告阈值时，力信息显示呈红色，并伴有声音提示；所述消化内镜末端三维姿态显示模块1032根据主端控制装置发送的指令计算消化内镜末端姿态并使用三维绘图在人机界面实时显示；所述消化内镜动态信息显示模块1033显示执行器操作消化内镜的各个动作，包括前进、后退、上弯、下弯、左弯、右弯、顺时针旋转、逆时针旋转、送气、送水、抽吸，当执行器执行上述动作之一或更多时，相应的动作会闪烁，提示操作者正在进行的操作；所述工作模式显示设定模块1034，用于显示与设定控制模式。
本实施例中，底层控制器采用Delta Tau公司的Clipper型多轴运动控制卡。Clipper多轴运动控制卡CPU为80MHz的DSP5603，具有256K*24的用户SRAM， 使用100Mbps的Ethernet与上位机通讯，主板具有4通道轴接口电路，可扩充至12轴，适用于多轴机器人的控制；其可配置附件支持多路AD采样与IO。
如图3所示，一种消化内镜辅助介入机器人的控制方法，包括以下步骤：
步骤一，主端控制装置与底层控制器参数初始化，设定工作模式，工作模式可在介入操作中根据要求实时更改；
步骤二，启动定时器，在定时响应函数里有两类信息需要处理，分别为读取手柄信息作为机器人执行器的控制命令，读取力反馈装置返回的阻力与阻力矩信息，读取脚踏板信息作为模式切换与送气、送水、抽吸命令；
步骤三，工作模式选择；当设定工作模式为速度控制模式时执行步骤四，当设定工作模式为位置控制模式时执行步骤五；
步骤四，计算手柄按键按下后的位移变化，对位移变化进行比例放大作为执行器的运行速度，发送命令至底层控制器
步骤五，根据建立的消化内镜的近远端对应关系模型，通过手柄的位置变化进行雅克比反变换计算机器人各关节变量的变化量，并发送控制命令至底层控制器；
步骤六，主端控制装置通过发送的控制命令根据消化内镜近远端对应关系模型计算消化内镜末端三维姿态的变化；
步骤七，消化内镜末端三维姿态变化显示至人机交互界面；
步骤八，底层控制器对主端控制装置的命令进行转换，发送给电机驱动器、气压调节装置、光电耦合器；
步骤九，电机驱动器驱动电机按照控制命令运动，气压调节装置调节气路气压，光电耦合器通过控制电磁阀通断控制执行器夹持机构配合驱动机构完成镜体的输送；
步骤十，在步骤二中，主端控制装置定时采集力信息与扭矩信息，处理如步骤十一到步骤十三，与步骤三到九是并行的；
步骤十一，主端控制装置力反馈信息处理模块对采集到得阻力信息与阻力 矩信息进行处理，去除零偏与重力干扰；
步骤十二，力信息经处理后反馈至人机界面显示，并根据阻力与阻力矩值的大小进行显示，当阻力或阻力矩值大于预设警告阈值时，人机界面出现画面提示并伴随有声音提示，此时末端执行器具有一定的自主性，通过调节自身位姿使阻力或阻力矩恢复至安全范围内；
步骤十三，力信息经处理后，经预设比例放大后，映射至手柄给操作者提供力感知。
步骤十四，判断相应脚踏板是否按下，如按下进行工作模式切换，否则进入步骤十五；
步骤十五，判断相应脚踏板是否按下，如按下进行送气、送水或抽吸操作，否则返回步骤二；
所述消化内镜的近远端对应关系模型与雅克比反变换参照论文“Ott,L.,etal.,Robotic Assistance to Flexible Endoscopy by Physiological-Motion Tracking.IEEE Transactions on Robotics,2011.27(2):p.346-359.”。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，对于熟悉该技术的人员在不脱离本发明构思的前提下，还可以做出若干变形与改进，这些都属于本发明的保护范围。