[发明专利]一种主从式手术机器人控制方法

说明书

本发明公开了一种主从式手术机器人控制方法，包括：S1计算得到各连杆与关节之间的力和力矩迭代方程；S2计算各连杆的重力补偿系数，并将重力对各关节的力和力矩作用映射至各关节；S3经过逆动力学计算得到各关节的期望力矩，并根据实时反馈的当前实际力反馈信息，引入内环/外环控制架构进行控制；S4计算各连杆与关节之间的力和力矩迭代方程；S5实时获取从手执行端的力反馈信息，引入内环/外环控制架构进行控制。本发明完成了主手与从手的运动学和动力学建模及主从手之间映射关系的理论推导，采用了重力补偿和内环/外环控制策略，采用模块化的程序架构完成了控制算法实现，高效的实现了主从式遥操作手术机器人控制系统。

技术领域

本发明涉及一种遥操作机器人控制领域，特别涉及主从式遥操作手术机器人建模与控制技术，属于人机交互技术领域。

背景技术

主从式遥操作手术机器人控制系统通常由一个主手操控台和若干个从手执行臂组成。从手执行臂安装于手术台旁边，在其末端可以安装内窥镜以及各种手术器械，通过微小创口到达病人体内病灶处。医生只需操作主手操控端，即可控制从手末端器械完成各种手术操作，为外科医生提供了传统手术的操作环境，可以协助医生完成更精细的手术动作，减少手术时由于疲劳产生的误操作或手部震颤造成的损伤。同时，因损伤小、愈合快为患者带来更理想的手术结果而得到广泛的应用。国外已有主从式手术机器人控制系统，但尚不具备动力学控制功能，且存在价格昂贵与不便于维护的问题。

发明内容

有鉴于此，本发明的目的是提供一种新型主从式遥操作手术机器人控制方法，并通过力反馈技术提高手术安全性以及可靠性，让医生在实施遥操作手术时感受到远端执行机构与组织间的相互作用力。

本发明的目的是通过这样的技术方案实现的，一种新型主从式手术机器人控制方法，包括以下步骤：

S1、获取主手机器人各关节的位置信息和速度信息，利用构造法计算主手机器人的雅可比矩阵，经过主手机器人正运动学计算得到各关节在笛卡尔空间的位置信息和速度信息，并经过主手机器人正动力学计算得到各连杆与关节之间的力和力矩迭代方程；

S2、根据主手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得的各关节在笛卡尔空间的位置信息及力和力矩的关系信息计算各连杆的重力补偿系数，并将重力对各关节的力和力矩作用映射至各关节；

S3、根据末端受力情况，以及重力对各关节的作用，经过逆动力学计算得到各关节的期望力矩，并根据实时反馈的当前实际力反馈信息，引入内环/外环控制架构进行控制；

S4、根据主从手机器人比例及坐标映射关系得到从手机器人末端在笛卡尔空间的位置信息及速度信息，再经过从手机器人运动学位置逆解及基于逆雅可比矩阵变换得到从手机器人各关节的位置信息和速度信息，并计算各连杆与关节之间的力和力矩迭代方程；

S5、根据从手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得位置及力和力矩的信息对从手机器人进行重力补偿，实时获取从手执行端的力反馈信息，引入内环/外环控制架构进行控制。

进一步，在所述步骤S1与S4中，主手机器人与从手机器人的雅克比矩阵计算的构造法，上半部分下半部分其中，z_i-1为第i个关节在基坐标系中表示的三维向量，o_n为末端执行器坐标原点在基坐标系中表示的三维向量，o_i-1为第i个关节坐标原点在基坐标系中表示的三维向量，为雅可比矩阵的上半部分，为雅可比矩阵的下半部分。

进一步，在所述步骤S1与S4中，主手机器人与从手机器人各连杆与关节之间的力迭代方程