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

摘要

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

主权项

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