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

摘要

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

权利要求

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参数法建立运动学模型，依据齐次变换矩阵计算公式求得从操作臂的末端在基坐标系中的齐次变换矩阵 根据以下公式计算出手术器械腕部三个关节的转动角度值θSi：θSi＝ASi/μSi

将主操作臂的位置基坐标系映射到从操作臂描述的基坐标系中，映射后得到从操作臂位置的齐次变换矩阵如下：其中， 为主操作臂基坐标系到从操作臂基坐标系的转换矩阵， 为主从映射的比例因子；

写成下面这种形式：

从操作臂位置坐标为 根据从机械臂构型按照D‑H参数法推出从操作臂关节S1、关节S2、关节L3的齐次变换矩阵：其中，进一步建立如下关系式：

psx＝xs

psy＝ys

psz＝zs

根据这三个方程可以求出关节S1、关节S2、关节L3的未知参数θs1、θs2、ls3；

从操作臂器械末端腕部三关节的期望角度值计算公式如下：θs4＝θM4

θs5＝θM5

θs6＝θM6

其中，θM4、θM5、θM6分别表示主操作手腕部三个关节的实际转动角度值；

然后利用运动控制器给从操作臂输出相应的数值，实现主从跟随。

说明书

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

技术领域

[0001] 本发明涉及微创手术机器人技术领域，具体而言，涉及一种用于微创手术机器人的主从跟踪控制方法。

背景技术

[0002] 参考授权公告号为CN109091237B，名称为微创手术器械辅助系统的中国发明专利以及授权公告号为CN109091238B、名称为分体式微创手术器械辅助系统的中国发明专利，以及授权公告号为CN210872029U、名称为医生操作台的实用新型专利，以及，微创手术器机器人被设计为在内窥镜下，由医生在手术室内操作，控制手术器械做手术的设备系统。医生坐在医生操作台前，通过观看3D影像显示器，操作医生机械臂，医生可如同平常手术般，利用患者机械臂精准的控制手术器械执行各种手术动作。

[0003] 参考公开号为CN105286999A、CN105286989A的发明专利申请，手术器械可以执行不同功能，包括夹持、切除、切割、缝合、吻合等。手术器械具有不同的配置，其包括执行末端，腕节、器械杆、器械盒等。

[0004] 现有微创手术机器人系统，医生通过手持多自由度的机械式运动输入装置来输出动作指令，通过主从运动映射算法，可以将采集到的动作之指令转换为机械臂的关节运动信息，从机械臂跟随主机械臂的运动，然而目前的算法实时性差、准确性差、精度低。

发明内容

[0005] 本发明就是为了解决现有微创手术机器人系统主从跟踪控制方法实时性差、准确性差、精度低的技术问题，提供了一种改善时性差、准确性差、精度的用于微创手术机器人的主从跟踪控制方法。

[0006] 本发明提供的用于微创手术机器人的主从跟踪控制方法，包括以下步骤：

[0007] 针对主操作臂用D‑H法建立其D‑H参数表，D‑H参数表包括M1、M2、M3三个关节的连杆长度ai，连杆偏移量di，连杆转角αMi和关节角θMi，根据空间变换的原理得到两个相邻关节之间的齐次变换矩阵如下公式：

[0008]

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

[0010]

[0011] 从齐次变换矩阵即可得到主操作臂末端相对于基坐标系的位置；

[0012] 将 写成下面这种形式：

[0013]

[0014] 然后令：

[0015]

[0016] PM即为主操作臂的手腕的位置坐标；

[0017] 在主操作臂末端位置点建立相应的动态坐标系，根据以下公式计算出手腕三个关节M4、M5、M6的转动角度值θMi，表示手腕4的姿态；

[0018] θMi＝AMi/μMi

[0019] 其中，AMi表示该关节编码器数值的变化量，μMi为该关节编码器精度、减速器减速比值与机械结构传动比值的积；

[0020] 从操作臂的关节S1、关节S2、关节L3，手术器械末端腕部的关节S4、关节S5、关节S6，针对从操作臂端采用D‑H参数法建立运动学模型，依据齐次变换矩阵计算公式求得从操作臂的末端在基坐标系中的齐次变换矩阵 根据以下公式计算出手术器械腕部三个关节的转动角度值θSi：

[0021] θSi＝ASi/μSi

[0022] 将主操作臂的位置基坐标系映射到从操作臂描述的基坐标系中，映射后得到从操作臂位置的齐次变换矩阵如下：

[0023]

[0024] 其中， 为主操作臂基坐标系到从操作臂基坐标系的转换矩阵， 为主从映射的比例因子；

[0025] 写成下面这种形式：

[0026]

[0027] 从操作臂位置坐标为 根据从机械臂构型按照D‑H参数法推出从操作臂关节S1、关节S2、关节L3的齐次变换矩阵：

[0028]

[0029] 其中，进一步建立如下关系式：

[0030] psx＝xs

[0031] psy＝ys

[0032] psz＝zs

[0033] 根据这三个方程可以求出关节S1、关节S2、关节L3的未知参数θs1、θs2、ls3；

[0034] 从操作臂器械末端腕部三关节的期望角度值计算公式如下：

[0035] θs4＝θM4

[0036] θs5＝θM5

[0037] θs6＝θM6

[0038] 其中，θM4、θM5、θM6分别表示主操作手腕部三个关节的实际转动角度值；

[0039] 然后利用运动控制器给从操作臂输出相应的数值，实现主从跟随。

[0040] 本发明的有益效果是：本发明公开一种异构映射模式与同构映射模式相结合的简化运动学建模方法。主从操作手定位部分采用异构映射模式，主从操作手姿态部分采用同构映射模式。简化了整个运动学计算过程的复杂程度，提高了主从跟踪运动控制系统的实时性、准确性、精度。

[0041] 本发明进一步的特征和方面，将在以下参考附图的具体实施方式的描述中，得以清楚地记载。

附图说明

[0042] 图1是微创手术机器人的主操作臂的结构示意图；

[0043] 图2是图1所示结构中手腕的结构示意图；

[0044] 图3是图2所示手腕的结构图；

[0045] 图4是主操作臂位置调整的构型图；

[0046] 图5是从操作臂处于折叠状态的示意图；

[0047] 图6是从操作臂处于展开状态的示意图；

[0048] 图7是从操作臂处于展开状态的示意图；

[0049] 图8是从操作臂的构型图；

[0050] 图9是手术器械的构型图；

[0051] 图10是手术器械的机构示意图；

[0052] 图11是手腕的构型图。

[0053] 图中符号说明：

[0054] 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.电机。

具体实施方式

[0055] 本发明应用的主操作臂是授权公告号为CN210872029U，名称为医生操作台的实用新型专利中的医生机械臂。应用的从操作臂是专利号为201711314221.6，名称为微创手术主操作臂的机械臂。

[0056] 如图1所示的主操作臂，第一臂杆2的后端与基座1通过第一关节连接，第一臂杆2能够在水平面上转动，转动连接座5与第一臂杆2的前端通过第二关节连接，转动连接座5能够在水平面上转动，第二臂杆3与转动连接座5通过第三关节连接，第二臂杆3能够在垂直面上转动。

[0057] 手术过程中，医生用手握住手腕4进行操作，主操作臂各关节分别绕第一关节的关节轴线一14、第二关节的关节轴线二15、第三关节的关节轴线三16转动，其中，关节轴线三16与重力方向垂直，关节轴线一14、关节轴线二15与地面垂直。

[0058] 如图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。

[0059] 如图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之间的距离。连杆偏移量是指两相邻公垂线沿轴方向的距离，连杆偏移量di均是0。已知各关节的参数，根据空间变换的原理得到两个相邻关节之间的齐次变换矩阵如下所示。

[0060]

[0061] 根据上式建立各个相邻关节之间的变换，然后控制器根据下述公式可求主操作臂末端坐标系(即手腕4坐标系)相对于基坐标系的齐次变换矩阵 i取值1、2、3，从齐次变换矩阵即可得到主操作臂末端相对于基坐标系的位置。

[0062]

[0063] 还可以写成下面这种形式：

[0064]

[0065] 然后令：

[0066]

[0067] PM即为主操作臂的手腕4的位置坐标。

[0068] 在主操作臂末端位置点建立相应的动态坐标系，根据以下公式计算出手腕4三个关节M4、M5、M6的转动角度值θMi，表示手腕4的姿态。

[0069] θMi＝AMi/μMi

[0070] 其中，AMi表示该关节编码器数值的变化量，μMi为该关节编码器精度、减速器减速比值与机械结构传动比值的积。参考图3，θM4为关节M4的转动角度值，对应的编码器是第一编码器4‑6，θM5为关节M5的转动角度值，对应的编码器是第二编码器4‑7，θM6为关节M6的转动角度值，对应的编码器是第三编码器4‑8。

[0071] 如图5‑7所示，从操作臂包括从端基座201、连杆座202、从端连杆I203、从端连杆II204、器械升降座206、器械安装装置205。连杆座202旋转地安装在从端基座201的一端，其旋转轴线与重力方向一致。手术器械207可安装在器械安装装置205上。

[0072] 从操作臂的构型如附图8所示，关节S1为使连杆座202、从端连杆I203、从端连杆II204和器械升降座206整体横向移动的旋转关节，关节S2是平行四边形绕轴线俯仰旋转关节(使从端连杆I203、从端连杆II204、器械升降座206伸展或折叠)，关节L3是器械安装装置205沿着器械升降座206移动的关节，关节S1、关节S2确定了远端不动点(图7中的P点)的位置，在远端不动点轴线上下移动的关节L3决定了P点的位置。如图9和10所示，手术器械采用“自传‑偏转‑末端自传”式腕部自由度布局，手术器械末端腕部的关节S4、关节S5、关节S6决定了手术器械末端的姿态。同理，从操作臂端也采用D‑H参数法建立从手的运动学模型，依据上述齐次变换矩阵计算公式求得从操作臂的末端在基坐标系中的齐次变换矩阵 从齐次变换矩阵中可以得到从操作臂手术器械末端在基坐标系中的位置。同样可以根据以下公式计算出手术器械腕部三个关节的转动角度值θSi，分别是θS4、θS5、θS6，表示从操作手器械末端腕部的姿态。

[0073] θSi＝ASi/μSi

[0074] 其中ASi表示该关节编码器数值的变化量，μSi为该关节编码器精度、减速器减速比值与机械结构传动比值的积。

[0075] 根据设计的主操作臂与从操作臂的构型，手术器械及其末端同构式主手的腕部三个关节轴线交于一点，可实现运动学分析中的位置与姿态解耦。结合结构特点，本发明建立位姿分离、末端同构的主从映射。在位置运动映射中，需将主操作臂的位置基坐标系映射到从操作臂描述的基坐标系中，映射后得到从操作臂位置的齐次变换矩阵如下：

[0076]

[0077] 其中， 为主操作臂基坐标系到从操作臂基坐标系的转换矩阵， 为主从映射的比例因子，将主操作手的运动按照一定的比例缩小后映射为操作手的运动。 还可以写成下面这种形式：

[0078]

[0079] 从操作臂位置坐标为 根据从机械臂构型按照D‑H参数法推出从操作臂关节S1、关节S2、关节L3的齐次变换矩阵：

[0080]

[0081] 其中，矩阵中的各元素均是关节1至关节3参数(θs1、θs2、ls3)的表达式。根据以上描述建立如下关系式：

[0082] psx＝xs

[0083] psy＝ys

[0084] psz＝zs

[0085] 根据这三个方程可以求出关节S1、关节S2、关节L3的未知参数θs1、θs2、ls3。

[0086] 而器械与主手末端的姿态映射采用“一对一”映射方式实现。因此从操作臂器械末端腕部三关节的期望角度值计算公式如下：

[0087] θs4＝θM4

[0088] θs5＝θM5

[0089] θs6＝θM6

[0090] 其中，θM4、θM5、θM6分别表示主操作手腕部三个关节的实际转动角度值。然后利用运动控制器给从操作臂输出相应的数值，达到主从跟随的目的。本发明根据主从操作臂构型，即位姿分离、末端同构的思路提供了一种微创手术机器人简化运动学模型的建模方法，简化了整个运动学计算过程的复杂程度，提高了运动控制系统的实时性。

[0091] 以上所述仅对本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。