一种主从式手术机器人控制方法技术

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

A new master-slave surgical robot control method

The invention discloses a method for controlling the master-slave surgical robot model, including: S1 calculate the force and torque between the connecting rod and the joint iterative equation; S2 calculation of gravity compensation coefficient of each link, and the gravity of the joint force and torque of each joint is mapped to S3; after inverse dynamics calculated each joint the desired torque, and according to the feedback information of the current actual force feedback, the introduction of the inner / outer loop control structure control; S4 calculation of force and torque between the connecting rod and the joint iterative equation; S5 to obtain real-time feedback from the execution side, into the inner / outer loop control structure control. The present invention completed the main hand from theory and mapping relationship between hand kinematics and dynamics modeling and master-slave manipulator, using the gravity compensation and the inner / outer loop control strategy, using the modular program structure to complete the control algorithm, and efficient implementation of the master-slave teleoperation robot control system.

【技术实现步骤摘要】
一种新型主从式手术机器人控制方法
本专利技术涉及一种遥操作机器人控制领域，特别涉及主从式遥操作手术机器人建模与控制技术，属于人机交互

技术介绍
主从式遥操作手术机器人控制系统通常由一个主手操控台和若干个从手执行臂组成。从手执行臂安装于手术台旁边，在其末端可以安装内窥镜以及各种手术器械，通过微小创口到达病人体内病灶处。医生只需操作主手操控端，即可控制从手末端器械完成各种手术操作，为外科医生提供了传统手术的操作环境，可以协助医生完成更精细的手术动作，减少手术时由于疲劳产生的误操作或手部震颤造成的损伤。同时，因损伤小、愈合快为患者带来更理想的手术结果而得到广泛的应用。国外已有主从式手术机器人控制系统，但尚不具备动力学控制功能，且存在价格昂贵与不便于维护的问题。
技术实现思路
有鉴于此，本专利技术的目的是提供一种新型主从式遥操作手术机器人控制方法，并通过力反馈技术提高手术安全性以及可靠性，让医生在实施遥操作手术时感受到远端执行机构与组织间的相互作用力。本专利技术的目的是通过这样的技术方案实现的，一种新型主从式手术机器人控制方法，包括以下步骤：S1、获取主手机器人各关节的位置信息和速度信息，利用构造法计算主手机器人的雅可比矩阵，经过主手机器人正运动学计算得到各关节在笛卡尔空间的位置信息和速度信息，并经过主手机器人正动力学计算得到各连杆与关节之间的力和力矩迭代方程；S2、根据主手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得的各关节在笛卡尔空间的位置信息及力和力矩的关系信息计算各连杆的重力补偿系数，并将重力对各关节的力和力矩作用映射至各关节；S3、根据末端受力情况，以及重力对各关节的作用，经过逆动力学计算得到各关节的期望力矩，并根据实时反馈的当前实际力反馈信息，引入内环/外环控制架构进行控制；S4、根据主从手机器人比例及坐标映射关系得到从手机器人末端在笛卡尔空间的位置信息及速度信息，再经过从手机器人运动学位置逆解及基于逆雅可比矩阵变换得到从手机器人各关节的位置信息和速度信息，并计算各连杆与关节之间的力和力矩迭代方程；S5、根据从手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得位置及力和力矩的信息对从手机器人进行重力补偿，实时获取从手执行端的力反馈信息，引入内环/外环控制架构进行控制。进一步，在所述步骤S1与S4中，主手机器人与从手机器人的雅克比矩阵计算的构造法，上半部分下半部分其中，zi-1为第i个关节在基坐标系中表示的三维向量，on为末端执行器坐标原点在基坐标系中表示的三维向量，oi-1为第i个关节坐标原点在基坐标系中表示的三维向量，为雅可比矩阵的上半部分，为雅可比矩阵的下半部分。进一步，在所述步骤S1与S4中，主手机器人与从手机器人各连杆与关节之间的力迭代方程力矩迭代方程其中I为连体坐标系中的惯性张量，fi是由连杆i-1施加在连杆i上的力，为坐标系i+1到坐标系i的旋转矩阵，fi+1是由连杆i+1施加在连杆i上的力，mi为连杆i的质量，ac,i连杆i质心的加速度，gi重力引起的加速度在坐标系i中的表示，τi连杆i-1施加到连杆i的力矩，从坐标系i的原点到连杆i质心的向量，坐标系i相对于基坐标系的角加速度，ωi坐标系i相对于基坐标系的角速度，τi+1连杆i+1施加到连杆i的力矩。进一步，在所述步骤S3中，各连杆的重力向量为g(q)＝[g1(q),...,gn(q)]T,重力力矩对各关节的作用为力与力矩方程及各重力向量分量求得，g(q)重力引起的加速度在各坐标系中的向量表示，gn(q)为g(q)的分量，[]T为矩阵的转置，g(k)为总势能P对关节k的位移qk的偏导数。进一步，在所述步骤S3与S5中，内环控制方程为M(q)为惯量矩阵，aq表示待输入的加速度项，为离心项与科里奥项的表示，q为关节位置，为关节速度。进一步，在所述步骤S3与S5中，外环控制方程为其中，K0和K1是对角矩阵，其对角元素分别由位置增益和速度增益组成。为参考加速度，K0与K1为增益矩阵，为位置增益，为加速度增益。进一步，在所述步骤S4中，通过以下方法获得主从手机器人比例及坐标映射关系，(1)首先通过进行主手机器人正运动学求解，得到主手机器人相对于主手机器人的基坐标系的位置变化为Δdmb＝Tmain(Δθ1,...,Δθ6)；θ1～θ6为关节位置，Δθ1～Δθ6为关节位置变化，为坐标系1到坐标系0的齐次变化矩阵，为坐标系2到坐标系1的齐次变化矩阵，为坐标系3到坐标系2的齐次变化矩阵，为坐标系4到坐标系3的齐次变化矩阵，为坐标系5到坐标系4的齐次变化矩阵，为坐标系6到坐标系5的齐次变化矩阵。(2)然后通过Δdmu＝Tbu*Δdmb关系式，将(1)中所述的Δdmb从主手机器人的基坐标系转换到大地坐标系中，m表示主手，b表示大地坐标系，Δdmu为主手机器人在大地坐标系中的位置变化，Tbu为主手机器人的基坐标系相对于大地坐标系的变换矩阵。(3)再通过主手机器人与从手器械臂间的比例变换关系Δdsu＝k*Δdmu，根据(2)中所述的Δdmu得到从手器械臂相对于大地坐标系的位置变化Δdsu，其中k为比例因子。(4)再通过Δdsd＝Tu-d*Δdsu关系式，根据Tu-d和(3)中所述的Δdsu，将从手器械臂相对于大地坐标系的位置变化转换为从手器械臂相对于显示器坐标系的位置变化Δdsd，其中Tu-d为从大地坐标系到显示器坐标系的变换矩阵。(5)通过Δdsc＝Δdsd关系式，根据(4)中所述的Δdsd，得到从手器械臂相对于从手持镜臂的位置变化Δdsc。(6)通过从手器械臂正运动学关系求得从手器械臂末端相对于持镜臂基坐标系(从手基坐标系)的变换矩阵；最后通过Δdsb＝TslaveΔdsc关系式，根据(5)中所述的Δdsc得到从手器械臂相对于从手执行端基坐标系的运动；其中，θ1～θ6为关节位置，为坐标系1到坐标系0的齐次变化矩阵，为坐标系2到坐标系1的齐次变化矩阵，为坐标系3到坐标系2的齐次变化矩阵，为坐标系4到坐标系3的齐次变化矩阵，为坐标系5到坐标系4的齐次变化矩阵，为坐标系6到坐标系5的齐次变化矩阵。由于采用了上述技术方案，本专利技术具有如下的优点：本专利技术完成了主手与从手的运动学和动力学建模及主从手之间映射关系的理论推导，采用了重力补偿和内环/外环控制策略，采用模块化的程序架构完成了控制算法实现，高效的实现了主从式遥操作手术机器人控制系统，在控制的实时性、高速和高精度等方面具备较高水平。附图说明为了使本专利技术的目的、技术方案和优点更加清楚，下面将结合附图对本专利技术作进一步的详细描述，其中：图1为本专利技术遥操作手术机器人的控制系统结构示意图；图2为本专利技术遥操作手术机器人的主手结构示意图；图3为本专利技术遥操作手术机器人的从手程序框架图；图4为本专利技术遥操作手术机器人的控制系统程序框架图；图5为本专利技术遥操作手术机器人的控制系统建模方法流程图。具体实施方式以下将结合附图，对本专利技术的优选实施例进行详细的描述；应当理解，优选实施例仅为了说明本专利技术，而不是为了限制本专利技术的保护范围。本实施例提供一种新型主从式遥操作手术机器人控制系统及方法，并通过力反馈技术提高手术安全性以及可靠性，让医生在实施遥操作手术时感受到远端执行机构与组织间的相互作用力。一种新型主从式遥操作手术机器人控制本文档来自技高网...

【技术保护点】
一种新型主从式手术机器人控制方法，其特征在于：包括以下步骤：S1、获取主手机器人各关节的位置信息和速度信息，利用构造法计算主手机器人的雅可比矩阵，经过主手机器人正运动学计算得到各关节在笛卡尔空间的位置信息和速度信息，并经过主手机器人正动力学计算得到各连杆与关节之间的力和力矩迭代方程；S2、根据主手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得的各关节在笛卡尔空间的位置信息及力和力矩的关系信息计算各连杆的重力补偿系数，并将重力对各关节的力和力矩作用映射至各关节；S3、根据末端受力情况，以及重力对各关节的作用，经过逆动力学计算得到各关节的期望力矩，并根据实时反馈的当前实际力反馈信息，引入内环/外环控制架构进行控制；S4、根据主从手机器人比例及坐标映射关系得到从手机器人末端在笛卡尔空间的位置信息及速度信息，再经过从手机器人运动学位置逆解及基于逆雅可比矩阵变换得到从手机器人各关节的位置信息和速度信息，并计算各连杆与关节之间的力和力矩迭代方程；S5、根据从手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得位置及力和力矩的信息对从手机器人进行重力补偿，实时获取从手执行端的力反馈信息，引入内环/外环控制架构进行控制。...

【技术特征摘要】
1.一种新型主从式手术机器人控制方法，其特征在于：包括以下步骤：S1、获取主手机器人各关节的位置信息和速度信息，利用构造法计算主手机器人的雅可比矩阵，经过主手机器人正运动学计算得到各关节在笛卡尔空间的位置信息和速度信息，并经过主手机器人正动力学计算得到各连杆与关节之间的力和力矩迭代方程；S2、根据主手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得的各关节在笛卡尔空间的位置信息及力和力矩的关系信息计算各连杆的重力补偿系数，并将重力对各关节的力和力矩作用映射至各关节；S3、根据末端受力情况，以及重力对各关节的作用，经过逆动力学计算得到各关节的期望力矩，并根据实时反馈的当前实际力反馈信息，引入内环/外环控制架构进行控制；S4、根据主从手机器人比例及坐标映射关系得到从手机器人末端在笛卡尔空间的位置信息及速度信息，再经过从手机器人运动学位置逆解及基于逆雅可比矩阵变换得到从手机器人各关节的位置信息和速度信息，并计算各连杆与关节之间的力和力矩迭代方程；S5、根据从手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得位置及力和力矩的信息对从手机器人进行重力补偿，实时获取从手执行端的力反馈信息，引入内环/外环控制架构进行控制。2.根据权利要求1所述的一种新型主从式手术机器人控制方法，其特征在于：在所述步骤S1与S4中，主手机器人与从手机器人的雅克比矩阵计算的构造法，上半部分下半部分其中，zi-1为第i个关节在基坐标系中表示的三维向量，on为末端执行器坐标原点在基坐标系中表示的三维向量，oi-1为第i个关节坐标原点在基坐标系中表示的三维向量，为雅可比矩阵的上半部分，为雅可比矩阵的下半部分。3.根据权利要求1所述的一种新型主从式手术机器人控制方法，其特征在于：在所述步骤S1与S4中，主手机器人与从手机器人各连杆与关节之间的力迭代方程力矩迭代方程其中I为连体坐标系中的惯性张量，fi是由连杆i-1施加在连杆i上的力，为坐标系i+1到坐标系i的旋转矩阵，fi+1是由连杆i+1施加在连杆i上的力，mi为连杆i的质量，ac,i连杆i质心的加速度，gi重力引起的加速度在坐标系i中的表示，τi连杆i-1施加到连杆i的力矩，从坐标系i的原点到连杆i质心的向量，坐标系i相对于基坐标系的角加速度，ωi坐标系i相对于基坐标系的角速度，τi+1连杆i+1施加到连杆i的力矩。4.根据权利要求1所述的一种新型主从式手术机器人控制方法，其特征在于：在所述步骤S3中，各连杆的重力向量为g(q)＝[g1(q),...,gn(q)]T,重力力矩对各关节的作用为力与力矩方程及各重力向量分量求得，g(q)重力引起的加速度在各坐标系中的向量表示，gn(q)为g(q)的分量，[]T为矩...

【专利技术属性】
技术研发人员：向洋，熊亮，谢毅，傅舰艇，王黎，张敏锐，
申请(专利权)人：中国科学院重庆绿色智能技术研究院，
类型：发明
国别省市：重庆,50