[发明专利]一种消化内镜辅助介入机器人的控制系统与控制方法

权利要求书

1.一种消化内镜辅助介入机器人的控制系统，其特征在于，包括主端控制装置、手柄、脚踏板、底层控制器、电机驱动器、气压调节装置、光电耦合器、电磁阀、力反馈装置和限位保护装置；

所述力反馈装置，用于测量消化内镜介入时近端遇到的阻力和阻力矩；

所述气压调节装置，用于调节气路的气压，控制执行器对消化内镜的夹持力；

所述电磁阀，由光电耦合器控制其通断，用于控制执行器夹持机构的充气与放气，配合执行器驱动机构完成消化内镜输送；

所述底层控制器，用于接收主端控制装置命令处理后发送给所述电机驱动器、气压调节装置、光电耦合器，采样阻力与阻力矩信息传送给所述主端控制装置；

所述限位保护装置，用于保护机器人执行器与消化内镜操作部旋钮不受损坏；

所述主端控制装置，与底层控制器双向连接；

所述底层控制器，第一输出端连接所述电机驱动器，第二输出端连接所述气压调节装置的输入端，IO输出端连接所述光电耦合器的输入端，第一输入端连接所述力传感器的输出端，第二输入端连接所述扭矩传感器的输出端，第三输入端连接所述限位保护装置的输出端；

所述手柄，连接主端控制装置，用于控制胃镜的末端位姿；

所述脚踏板，连接主端控制装置，用于切换工作模式与控制送气、送水、抽吸功能。

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

3.根据权利要求1所述的一种消化内镜辅助介入机器人的控制系统，其特征在于，所述主端控制装置包括：

手柄信息提取与处理模块，用于提取手柄动作信息，并处理得到机器人运动指令；

脚踏板信息提取与处理模块，用于提取脚踏板动作信息，处理得到工作模式以及送气、送水与抽吸命令；

力反馈信息处理模块，用于去除力反馈装置测量阻力和阻力矩中零偏和重力的干扰，显示至人机交互界面模块，并根据预设比例映射到主端控制手柄；

人机交互界面模块，用于工作模式的显示设定与机器人状态的显示。

4.根据权利要求3所述的一种消化内镜辅助介入机器人的控制系统，其特征在于，所述人机交互界面模块包括：

力信息提示与报警模块，用于显示输送阻力与扭转阻力矩信息，并在所述阻力与阻力矩超过预设值时启动图像或声音提示报警；

消化内镜动态信息提示模块，用于提示机器人末端执行器的动态信息，包括对消化内镜的输送、旋转、弯曲以及送气、送水、抽吸操作；

消化内镜末端三维姿态显示模块，用于提示消化内镜末端的三维姿态信息，方便操作者进行操作；

工作模式显示设定模块，用于显示与设定控制模式。

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

6.一种消化内镜辅助介入机器人的控制方法，其特征在于，包括以下步骤：

主端控制装置与底层控制器参数初始化，设定工作模式：速度控制模式或位置控制模式；

启动定时器，读取手柄信息作为机器人执行器的控制命令；

当设定工作模式为速度控制模式时，计算手柄按键按下后的位移变化，对位移变化进行比例放大作为执行器的运行速度，发送命令至底层控制器；

当设定工作模式为位置控制模式时，根据建立的消化内镜的近远端对应关系模型，通过手柄的位置变化进行雅克比反变换计算机器人各关节变量的变化量，并发送控制命令至底层控制器；

主端控制装置通过发送的控制命令根据消化内镜近远端对应关系模型计算消化内镜末端三维姿态的变化；

消化内镜末端三维姿态变化显示至人机交互界面；

底层控制器对主端控制装置的命令进行转换，发送给电机驱动器、气压调节装置、光电耦合器；

电机驱动器驱动电机按照控制命令运动，气压调节装置调节气路气压，光电耦合器通过控制电磁阀通断控制执行器夹持机构配合驱动机构完成镜体的输送。