用于微创手术机器人的主从跟踪控制方法技术

本发明专利技术涉及一种用于微创手术机器人的主从跟踪控制方法，其解决现有微创手术机器人系统主从跟踪控制方法实时性差、准确性差、精度低的技术问题，其将异构映射模式与同构映射模式相结合的简化运动学建模方法，主从操作手定位部分采用异构映射模式，主从操作手姿态部分采用同构映射模式。本发明专利技术广泛用于微创手术机器人。

【技术实现步骤摘要】
用于微创手术机器人的主从跟踪控制方法
本专利技术涉及微创手术机器人
，具体而言，涉及一种用于微创手术机器人的主从跟踪控制方法。
技术介绍
参考授权公告号为CN109091237B，名称为微创手术器械辅助系统的中国专利技术专利以及授权公告号为CN109091238B、名称为分体式微创手术器械辅助系统的中国专利技术专利，以及授权公告号为CN210872029U、名称为医生操作台的技术专利，以及，微创手术器机器人被设计为在内窥镜下，由医生在手术室内操作，控制手术器械做手术的设备系统。医生坐在医生操作台前，通过观看3D影像显示器，操作医生机械臂，医生可如同平常手术般，利用患者机械臂精准的控制手术器械执行各种手术动作。参考公开号为CN105286999A、CN105286989A的专利技术专利申请，手术器械可以执行不同功能，包括夹持、切除、切割、缝合、吻合等。手术器械具有不同的配置，其包括执行末端，腕节、器械杆、器械盒等。现有微创手术机器人系统，医生通过手持多自由度的机械式运动输入装置来输出动作指令，通过主从运动映射算法，可以将采集到的动作之指令转换为机械臂的关节运动信息，从机械臂跟随主机械臂的运动，然而目前的算法实时性差、准确性差、精度低。
技术实现思路
本专利技术就是为了解决现有微创手术机器人系统主从跟踪控制方法实时性差、准确性差、精度低的技术问题，提供了一种改善时性差、准确性差、精度的用于微创手术机器人的主从跟踪控制方法。本专利技术提供的用于微创手术机器人的主从跟踪控制方法，包括以下步骤：针对主操作臂用D-H法建立其D-H参数表，D-H参数表包括M1、M2、M3三个关节的连杆长度ai，连杆偏移量di，连杆转角αMi和关节角θMi，根据空间变换的原理得到两个相邻关节之间的齐次变换矩阵如下公式：根据上式建立各个相邻关节之间的变换，然后根据下述公式求主操作臂末端坐标系相对于基坐标系的齐次变换矩阵从齐次变换矩阵即可得到主操作臂末端相对于基坐标系的位置；将写成下面这种形式：然后令：PM即为主操作臂的手腕的位置坐标；在主操作臂末端位置点建立相应的动态坐标系，根据以下公式计算出手腕三个关节M4、M5、M6的转动角度值θMi，表示手腕4的姿态；θMi＝AMi/μMi其中，AMi表示该关节编码器数值的变化量，μMi为该关节编码器精度、减速器减速比值与机械结构传动比值的积；从操作臂的关节S1、关节S2、关节L3，手术器械末端腕部的关节S4、关节S5、关节S6，针对从操作臂端采用D-H参数法建立运动学模型，依据齐次变换矩阵计算公式求得从操作臂的末端在基坐标系中的齐次变换矩阵根据以下公式计算出手术器械腕部三个关节的转动角度值θSi：θSi＝ASi/μSi将主操作臂的位置基坐标系映射到从操作臂描述的基坐标系中，映射后得到从操作臂位置的齐次变换矩阵如下：其中，为主操作臂基坐标系到从操作臂基坐标系的转换矩阵，为主从映射的比例因子；写成下面这种形式：从操作臂位置坐标为根据从机械臂构型按照D-H参数法推出从操作臂关节S1、关节S2、关节L3的齐次变换矩阵：其中，进一步建立如下关系式：psx＝xspsy＝yspsz＝zs根据这三个方程可以求出关节S1、关节S2、关节L3的未知参数θs1、θs2、ls3；从操作臂器械末端腕部三关节的期望角度值计算公式如下：θs4＝θM4θs5＝θM5θs6＝θM6其中，θM4、θM5、θM6分别表示主操作手腕部三个关节的实际转动角度值；然后利用运动控制器给从操作臂输出相应的数值，实现主从跟随。本专利技术的有益效果是：本专利技术公开一种异构映射模式与同构映射模式相结合的简化运动学建模方法。主从操作手定位部分采用异构映射模式，主从操作手姿态部分采用同构映射模式。简化了整个运动学计算过程的复杂程度，提高了主从跟踪运动控制系统的实时性、准确性、精度。本专利技术进一步的特征和方面，将在以下参考附图的具体实施方式的描述中，得以清楚地记载。附图说明图1是微创手术机器人的主操作臂的结构示意图；图2是图1所示结构中手腕的结构示意图；图3是图2所示手腕的结构图；图4是主操作臂位置调整的构型图；图5是从操作臂处于折叠状态的示意图；图6是从操作臂处于展开状态的示意图；图7是从操作臂处于展开状态的示意图；图8是从操作臂的构型图；图9是手术器械的构型图；图10是手术器械的机构示意图；图11是手腕的构型图。图中符号说明：1.基座，2.第一臂杆，3.第二臂杆，4.手腕，4-1.固定连杆，4-2.第一L型连杆，4-3.第二L型连杆，4-4.手柄，4-5.开合座，4-6.第一编码器，4-7.第二编码器，4-8.第三编码器；5.转动连接座。201.从端基座，202.连杆座，203.从端连杆I，204.从端连杆II，205.器械安装装置，206.器械升降座，207.手术器械，208.电机，209.电机。具体实施方式本专利技术应用的主操作臂是授权公告号为CN210872029U，名称为医生操作台的技术专利中的医生机械臂。应用的从操作臂是专利号为201711314221.6，名称为微创手术主操作臂的机械臂。如图1所示的主操作臂，第一臂杆2的后端与基座1通过第一关节连接，第一臂杆2能够在水平面上转动，转动连接座5与第一臂杆2的前端通过第二关节连接，转动连接座5能够在水平面上转动，第二臂杆3与转动连接座5通过第三关节连接，第二臂杆3能够在垂直面上转动。手术过程中，医生用手握住手腕4进行操作，主操作臂各关节分别绕第一关节的关节轴线一14、第二关节的关节轴线二15、第三关节的关节轴线三16转动，其中，关节轴线三16与重力方向垂直，关节轴线一14、关节轴线二15与地面垂直。如图2和3所示，手腕4包括固定连杆4-1、第一L型连杆4-2、第二L型连杆4-3、手柄4-4、开合座4-5、第一编码器4-6、第二编码器4-7、第三编码器4-8。手腕4设有关节M4、关节M5、关节M6。如图4所示的主操作臂位置调整构型图，其中M1、M2、M3三个关节是图1所示结构图中的第一关节、第二关节、第三关节。如图3和11，M4、M5、M6三个关节为姿态调整关节，这6个关节的自由度分别决定主操作臂的末端在笛卡尔坐标系中的位置和姿态。根据上述主操作臂构型，本专利技术可以采用D-H法进行主操作臂的运动学分析，建立其D-H参数表，D-H参数表包括M1、M2、M3三个关节的连杆长度ai，连杆偏移量di，连杆转角αMi和关节角θMi。比如图1中，a1表示关节M1的轴线和关节M2的轴线之间的距离，a2表示关节M2的轴线和关节M3的轴线之间的距离，a3表示关节M3的轴线与手腕4之间的距离。本文档来自技高网...

【技术保护点】
1.一种用于微创手术机器人的主从跟踪控制方法，其特征在于，包括以下步骤：/n针对主操作臂用D-H法建立其D-H参数表，D-H参数表包括M1、M2、M3三个关节的连杆长度a

【技术特征摘要】
1.一种用于微创手术机器人的主从跟踪控制方法，其特征在于，包括以下步骤：
针对主操作臂用D-H法建立其D-H参数表，D-H参数表包括M1、M2、M3三个关节的连杆长度ai，连杆偏移量di，连杆转角αMi和关节角θMi，根据空间变换的原理得到两个相邻关节之间的齐次变换矩阵如下公式：



根据上式建立各个相邻关节之间的变换，然后根据下述公式求主操作臂末端坐标系相对于基坐标系的齐次变换矩阵



从齐次变换矩阵即可得到主操作臂末端相对于基坐标系的位置；
将写成下面这种形式：



然后令：



PM即为主操作臂的手腕的位置坐标；
在主操作臂末端位置点建立相应的动态坐标系，根据以下公式计算出手腕三个关节M4、M5、M6的转动角度值θMi，表示手腕4的姿态；
θMi＝AMi/μMi
其中，AMi表示该关节编码器数值的变化量，μMi为该关节编码器精度、减速器减速比值与机械结构传动比值的积；
从操作臂的关节S1、关节S2、关节L3，手术器械末端腕部的关节S4、关节S5、关节S6，针对从操作臂端采用D-H参数法...

【专利技术属性】
技术研发人员：姜英俊，王炳强，王淑林，孙之建，
申请(专利权)人：山东威高手术机器人有限公司，
类型：发明
国别省市：山东;37