CN114452004B - 一种手术机器人末端位置和姿态的控制方法 - Google Patents

Abstract

本发明公开了一种手术机器人末端位置和姿态的控制方法，包括：步骤S1：建立远心点，拖动手术机器人使其末端移动至所述远心点的上方；所述远心点为供手术器械的末端穿过目标位置转动所围绕的虚拟不动点；步骤S2：将所述手术器械连接于所述手术机器人的器械座；步骤S3：控制所述手术器械穿过远心点；步骤S4：获取所述手术器械的末端的期望笛卡尔速度，计算所述机器人的各轴的目标速度，根据所述目标速度控制所述机器人的各轴运动。该控制方法能够精确控制手术机器人末端位姿，提高手术的精确性安全性。

Description

一种手术机器人末端位置和姿态的控制方法

技术领域

本发明涉及手术机器人领域，特别涉及一种手术机器人末端位置和姿态的控制方法。

背景技术

微创手术机器人可以减轻医生在手术过程中的体力劳动，同时达到精准手术的目的，使患者微创伤、失血少、术后感染少、术后恢复快。微创手术机器人系统通常使用主从式控制模式：操作者在对主手进行操作时，手部运动会带动主手随之运动，主手关节处传感器可以测量运动信息，通过主从控制算法将主手的运动映射到从手，从手各关节被动运动，带动手术器械实现相应运动。手术机器人的末端用来装载手术器械，其位置和姿态决定了手术操作的精确性。

因此，如何控制手术机器人末端的位置和姿态，提高手术精确和安全性成为本领域技术人员需要解决的技术问题。

发明内容

本发明的目的是提供一种手术机器人末端位置和姿态的控制方法，该控制方法能够精确控制手术机器人末端位姿，提高手术的精确性安全性。

为实现上述目的，本发明提供一种手术机器人末端位置和姿态的控制方法，包括：

步骤S1：建立远心点，拖动手术机器人使其末端移动至所述远心点的上方；所述远心点为供手术器械的末端穿过目标位置转动时所围绕的虚拟不动点；

步骤S2：将所述手术器械连接于所述手术机器人的器械座；

步骤S3：控制所述手术器械穿过远心点；

步骤S4：获取所述手术器械的末端的期望笛卡尔速度，计算所述手术机器人的各轴的目标速度，根据所述目标速度控制所述手术机器人的各轴运动。

可选地，所述手术机器人包括至少五个运动轴的第一机械臂、连接所述第一机械臂的直线引导部，所述直线引导部用以带动所述器械座做直线运动，还包括连接所述直线引导部用以驱动所述手术器械动作的第二机械臂。

可选地，所述步骤S1具体包括：

在所述目标位置开口，将戳卡插入所述开口；

拖动全部所述运动轴，使所述直线引导部平行所述戳卡、且所述器械座设于所述戳卡的正上方；

连接戳卡座与所述戳卡，获取所述远心点的位置。

可选地，所述连接戳卡座与所述戳卡，获取所述远心点的位置的步骤具体为：

根据全部所述运动轴处设置的编码器获取全部所述运动轴的实际角度或位移，根据全部所述实际角度或位移，计算所述远心点的位置。

可选地，所述连接戳卡座与所述戳卡，获取所述远心点的位置的步骤具体为：

根据所述戳卡座位置获取所述戳卡位置，由所述戳卡位置和所述远心点相对所述戳卡座的固定位置偏移得到所述远心点的位置。

可选地，所述步骤S3具体包括：

通过所述直线引导部控制所述手术器械直线运动穿过所述戳卡，借助设于所述直线引导部的位移传感器检测的位移控制所述手术器械的直线运动距离。

可选地，所述步骤S4进一步包括：

保持所述手术机器人与所述远心点重合的重合点不动，以所述重合点为球心驱动所述手术器械的转动，获取全部所述运动轴的驱动电机的第一约束角范围；

由所述目标速度，计算全部所述运动轴的驱动电机的第二约束角范围；

比对获取所述第一约束角范围和所述第二约束角范围的对应重合范围，根据所述对应重合范围控制全部所述驱动电机运动。

可选地，所述步骤S4具体为：

根据计算出手术器械坐标系的六个映射轴的第一输出关节速度；

根据计算出基坐标系下全部所述运动轴的第二输入关节速度，N等于所述运动轴的个数且N大于等于5；

其中：j^-1为六个所述映射轴的构型对应的第一雅克比矩阵的逆矩阵，v为六个所述映射轴得到输入笛卡尔速度，表示所述第一输出关节速度的六维向量；

J^-1为反映全部所述运动轴的构型对应的第二雅克比矩阵的逆矩阵， 表示所述第二输入关节速度的六维向量；

六个所述映射轴包括所述重合点处的两个虚拟轴、所述直线引导部对应的直线轴和所述第二机械臂的末端的三个所述运动轴；

v_x表示所述重合点的x方向速度，v_y表示所述重合点y方向速度，v_z表示所述重合点z方向速度，ω_x表示重合点的绕x方向旋转速度，ω_y表示重合点的绕y方向旋转速度，ω_z表示重合点的绕z方向旋转速度，和/>分别表示两个虚拟轴的第一关节输出速度。

可选地，所述步骤S4之后还包括：

手术完成或中止时控制所述直线引导部拉出所述手术器械，解除所述戳卡座与所述戳卡的连接，解除全部所述运动轴的限制。

相对于上述背景技术，本发明所提供的手术机器人末端位置和姿态的控制方法通过建立远心点使得手术器械在患者体内动作时围绕一虚拟不动点，通过控制手术器械绕远心点伸缩、旋转的速度即可控制手术器械末端的位置，通过控制靠近手术器械末端的运动轴即可控制手术器械末端的姿态。在远心点建立后，将手术器械与手术机器人的器械座连接，控制手术器械穿过远心点伸入患者体内，由需要完成的手术动作获取手术器械末端的期望笛卡尔速度，通过期望笛卡尔速度的逆运动学求解手术机器人各运动轴的目标速度，然后控制手术机器人的各轴按目标速度运动，实现手术机器人末端也即手术器械末端的位姿的精确控制。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为发明实施例所提供的手术机器人的示意图；

图2为图1中器械坐标系下六个映射轴的示意图；

图3为图1中基坐标系下运动轴的示意图；

图4为本发明实施例所提供的手术机器人末端位置和姿态的控制方法的流程图。

其中：

1-运动轴、2-直线引导部、3-器械座、4-手术器械、5-远心点。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

微创外科手术是指通过小切口把细长的内窥镜和微小的手术器械4植入病人体内进行的手术。外科医生借助视觉检测装置可以在手术的同时观测手术器械4的位置及手术过程。相对于传统的开放性外科手术，它具有创口小、可减轻患者痛苦、术后恢复快等优点。

手术机器人系统则能够进行微创外科手术的机器人系统，主要包括主操作手也即主手机器人、从操作手也即从手机器人和控制系统。主手机器人由医生控制产生位姿信号，该位姿信号被采集处理后得到控制从手机器人的位姿信号，进而通过从手机器人实现手术操作。本发明的目的是公开一种手术机器人(从手机器人)末端位置和姿态控制方法，从而提高手术精确度和安全性。

需要说明的是，本发明中所提供的手术机器人控制方法，不仅适用于目标物为假人时的教学形式的模拟手术，还适用于目标物为真人时的真实手术。当进行真实手术时，目标物为人体，目标位置对应于患者创口。

为了使本技术领域的技术人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。

请参考图1至图4，图1为发明实施例所提供的手术机器人的示意图，图2为图1中器械坐标系下六个映射轴的示意图，图3为图1中基坐标系下运动轴的示意图，图4为本发明实施例所提供的手术机器人末端位置和姿态的控制方法的流程图。

本发明的所提供手术机器人末端位置和姿态的控制方法可以应用于如图1至图3所示手术机器人；该手术机器人包括第一机械臂和第二机械臂，第一机械臂采用具有至少5个运动轴1的机器人，本发明以六轴机器人为例进行说明，并将其称为通用机器人，并将以通用机器人的第一个运动轴1处建立的坐标系称为基坐标系；第一机械臂的末端连接直线引导部2，第二机械臂能够沿直线引导部2直线运动且第二机械臂用来控制手术器械4动作，如第二机械臂包括两个控制手术器械4开合的电机(相当于一个自由度)。

将由第一机械臂动作产生的手术器械4在远心点5转动的两个虚拟轴、直线引导部2的直线轴和第二机械臂用来控制手术器械4动作的三个运动轴1看作一个拥有六个自由度的器械机器人，手术器械4坐标系是以远心点5为基点建立的坐标系。本发明是基于手术器械4坐标系下期望笛卡尔速度和各运动轴1的第二关节输出速度也即目标速度转换实现对手术机器人末端的位置和姿态的控制的。

本发明所提供的手术机器人末端位置和姿态的控制方法包括如下四个步骤：

步骤S1：建立远心点5，拖动手术机器人使其末端移动至所述远心点的上方；所述远心点5为供手术器械4的末端穿过目标位置转动时所围绕的虚拟不动点；

步骤S2：将所述手术器械4连接于所述手术机器人的器械座3；

步骤S3：控制所述手术器械4穿过远心点5；

步骤S4：获取所述手术器械4的末端的期望笛卡尔速度，计算所述手术机器人的各轴的目标速度，根据所述目标速度控制所述手术机器人的各轴运动。

其中，步骤S1的作用是建立远心点5，确定手术器械4围绕旋转过程中与远心点5重合的重合点，也即与病人创口重合，使得手术器械4的末端绕该重合点做伸缩旋转动作，提高控制精度。具体操作为，对患者手术部位进行微创开口，开口后将腹腔镜插入微创开口处，拖动将手术机器人的各运动轴1，使得通用机器人末端的直线引导部2平行于戳卡、用来安装手术器械4的器械座3位于戳卡的正上方。

之后将戳卡与戳卡座连接，当传感器检测到戳卡与戳卡器连接之后，传感器将戳卡和戳卡座连接到位信息发送给手术机器人的控制系统，通过控制器对全部运动轴1设置的驱动电机的运动范围进行约束。

然后将手术器械4安装在手术机器人的器械座3，器械座3与直线引导部2的直线滑块的固接，当直线滑块运动时，带动器械座3及手术器械4作直线运动，用来控制器械俯仰、滚转、偏航和开合动作的电机能够与器械座3一起随同直线滑块运动。

上述将戳卡和戳卡座连接的具体操作可以为：拖动各运动轴1运动时，由设置在各运动轴1处编码器测得的全部运动轴1的实际角度或位移，拖动器械根据手术机器人的构型参数计算器械座3的位置。

在戳卡座和戳卡连接时，通过戳卡座的位置和戳卡相对戳卡座的固定偏移距离计算远心点5的位置，一般来说，远心点5设置在戳卡的正中央，通过戳卡座的位置偏移戳卡半径的距离，下沉预设深度后即可获取远心点5的位置。

步骤S3：获取远心点5的位置后，通过直线引导部2的直线滑块带动器械座3和手术器械4下放，从而使手术器械4穿过远心点5。下放长度可由器械座3位置、器械座3与戳卡或远心点5之间间距和手术器械4的长度以及手术器械4越过远心点5伸入患者体内的长度确定。由手术器械4越过远心点5伸入患者体内的长度作为手术器械4末端的旋转半径，手术器械4与远心点5重合的点作为手术器械4的重合点，重合点在手术器械4的末端旋转过程中保持不动，根据患者的手术位置，手术器械4伸入患者体内的长度不同，远心点5为虚拟的不动点。

步骤S4：在手术操作中，需要保持重合点不动，通过如上所述的六轴机器人的六个运动轴1转动保持重合点不动而手术器械4的末端运动。此时全部运动轴1需要输入一定约束条件来约束运动轴1的运动。

上述约束条件的获取具体采用：保持重合点不动，按照重合点不动的约束条件计算全部运动轴1的驱动电机第一约束角范围；然后根据手术器械4末端的手术空间范围，通过逆向运动学运算得到全部运动轴1的驱动电机第二约束角范围(此时无需保持重合点不动)，然后比对获取第一约束角范围和第二约束角范围的对应重合范围，上述对应重合范围同时满足手术器械4的重合点不动和手术器械4末端按照目标速度运动。通过向控制系统输入对应重合范围，控制全部运动轴1的驱动电机按照对应重合范围转动，满足对手术器械4末端的位置和姿态的精确控制。

具体来说，计算获取对应重合范围下各驱动电机的目标速度的步骤具体如下：

第一步，在手术器械4坐标系下运算；

获取手术器械4末端的期望笛卡尔速度v，此期望笛卡尔速度包括笛卡尔平移速度和笛卡尔旋转速度，换句话说v为一个六维向量；然后获取器械机器人构型也即六个映射轴的构型对应的雅克比矩阵的逆矩阵j^-1，j^-1与器械机器人的结构参数有关，在器械机器人的相关参数确认时，j^-1在运算时为已知。六个映射轴包括重合点处的两个虚拟轴、直线引导部2的直线轴和第二机械臂控制手术器械4运动的三个运动轴1；

根据：计算出手术器械坐标系的六个映射轴的第一输出关节速度/> 表示第一输出关节速度的六维向量；由于手术器械4坐标系是以远心点5为基点建立的坐标系，/>和/>分别表示两个虚拟轴的第一关节输出速度，/>表示直线引导部2或直线轴的第一关节输出速度；/>和/>分别表示用来控制手术器械4姿态的第二机械臂或器械机器人的三个运动轴1在手术器械坐标系下的第一关节输出速度。

第二步，在基坐标系下运算；

根据计算出基坐标系下全部运动轴1的第二输入关节速度，其中N等于第一机械臂或通用机器人全部运动轴1的个数且N大于等于5。

其中，表示第二输入关节速度的六维向量，J^-1为反映全部运动轴1构型对应的第二雅克比矩阵的逆矩阵，当多轴机器人的结构参数确认时，J^-1在运算时为已知；

v_x表示重合点的x方向速度，v_y表示重合点y方向速度，v_z表示重合点z方向速度，ω_x表示重合点的绕x方向旋转速度，ω_y表示重合点的绕y方向旋转速度，ω_z表示重合点的绕z方向旋转速度，/>和/>分别表示两个虚拟轴的第一关节输出速度，在第一步计算中已经求得，且由于重合点不动，仅发生手术器械4末端转动，故v_x、v_y、v_z以及ω_x均取零。

通过上述运算得到第一机械臂的各运动轴1在基坐标系下各运动轴1的第二关节输入速度，控制系统按照第二关节输入速度控制在对应重合范围内控制各驱动电机的转速即可。

当完成手术或者其它原因造成手术中止时，通过直线引导部2将手术器械4从戳卡拔出即可，分离戳卡和戳卡座，进而控制系统解除对各运动轴1的限制。

需要说明的是，在本说明书中，诸如第一和第二之类的关系术语仅仅用来将一个实体与另外几个实体区分开来，而不一定要求或者暗示这些实体之间存在任何这种实际的关系或者顺序。

以上对本发明所提供的手术机器人末端位置和姿态的控制方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (10)

1.一种手术机器人，其特征在于，包括第一机械臂和器械机器人，所述器械机器人包括直线引导部和第二机械臂，所述直线引导部分别与所述第一机械臂和所述第二机械臂连接，所述器械机器人具有映射轴，所述映射轴包括虚拟轴、所述第二机械臂的运动轴和所述直线引导部的直线轴，所述直线引导部的直线轴和所述第一机械臂的运动轴用于控制手术器械末端的位置，所述第二机械臂的运动轴用于控制所述手术器械末端的姿态；

其中，所述手术机器人在进行手术器械末端位置和姿态的控制的过程中，实现的步骤包括：

步骤S1：建立远心点，拖动所述手术机器人使所述手术机器人末端移动至所述远心点的上方；所述远心点为供所述手术器械的末端穿过目标位置转动时所围绕的虚拟不动点；所述虚拟轴是由所述第一机械臂动作产生的手术器械在所述远心点转动的轴；

步骤S2：将所述手术器械连接于所述手术机器人的器械座；

步骤S3：控制所述手术器械穿过所述远心点；

步骤S4：获取所述手术器械的末端的期望笛卡尔速度，对所述期望笛卡尔速度进行逆向映射得到所述虚拟轴、所述第二机械臂的运动轴和所述直线轴的第一输出关节速度，对所述虚拟轴的第一输出关节速度进行逆向映射得到所述第一机械臂的运动轴的第二输入关节速度，根据所述第一机械臂的运动轴的第二输入关节速度和所述直线轴的第一输出关节速度控制所述手术器械末端的位置，根据所述第二机械臂的运动轴的第一输出关节速度控制所述手术器械末端的姿态。

2.根据权利要求1所述的手术机器人，其特征在于，所述第一机械臂的运动轴用于控制所述手术器械末端绕所述远心点旋转运动，所述器械机器人的直线引导部的直线轴用于控制所述手术器械末端穿过所述远心点直线运动，所述第二机械臂的运动轴用于控制所述手术器械末端为俯仰、滚转、偏航和开合中的一个姿态，所述第一机械臂包括至少五个运动轴。

3.根据权利要求1所述的手术机器人，其特征在于，所述直线引导部包括直线滑块，所述直线滑块可沿所述直线轴运动，所述手术器械与所述器械座连接，所述器械座与所述直线滑块连接，当所述直线滑块运动时，通过所述器械座带动所述手术器械作直线运动。

4.根据权利要求1所述的手术机器人，其特征在于，所述步骤S1具体包括：

在所述目标位置开口，将戳卡插入所述开口；

拖动所述第一机械臂全部所述运动轴，使所述直线引导部平行所述戳卡、且所述器械座设于所述戳卡的正上方；

连接戳卡座与所述戳卡，获取所述远心点的位置。

5.根据权利要求4所述的手术机器人，其特征在于，所述连接戳卡座与所述戳卡，获取所述远心点的位置的步骤具体为：

根据所述第一机械臂全部所述运动轴处设置的编码器获取所述第一机械臂全部所述运动轴的实际角度或位移，根据全部所述实际角度或位移，计算所述远心点的位置。

6.根据权利要求4所述的手术机器人，其特征在于，所述连接戳卡座与所述戳卡，获取所述远心点的位置的步骤具体为：

根据所述戳卡相对所述戳卡座的固定偏移距离、戳卡座位置，计算所述远心点的位置。

7.根据权利要求5或6所述的手术机器人，其特征在于，所述步骤S3具体包括：

通过所述直线引导部的直线滑块控制所述手术器械直线运动穿过所述戳卡，借助设于所述直线引导部的位移传感器检测的位移控制所述手术器械的直线运动距离。

8.根据权利要求7所述的手术机器人，其特征在于，所述步骤S4进一步包括：

保持所述手术机器人与所述远心点重合的重合点不动，以所述重合点为球心驱动所述手术器械的转动，获取所述第一机械臂全部所述运动轴的驱动电机的第一约束角范围；

根据所述手术器械末端的手术空间范围，通过逆向运动学运算得到所述第二机械臂全部运动轴的驱动电机第二约束角范围；

比对获取所述第一约束角范围和所述第二约束角范围的对应重合范围，根据所述对应重合范围控制全部所述驱动电机运动。

9.根据权利要求1所述的手术机器人，其特征在于，所述步骤S4具体为：

根据计算出手术器械坐标系的六个映射轴的第一输出关节速度；

根据计算出基坐标系下所述第一机械臂全部所述运动轴的第二输入关节速度，N等于所述运动轴的个数且N大于等于5；

其中：为六个所述映射轴的构型对应的第一雅克比矩阵的逆矩阵，/>为六个所述映射轴得到输入笛卡尔速度，/>表示所述第一输出关节速度的六维向量；

为反映全部所述运动轴的构型对应的第二雅克比矩阵的逆矩阵，， />表示所述第二输入关节速度的N维向量；

六个所述映射轴包括重合点处的两个虚拟轴、所述直线引导部对应的直线轴和所述第二机械臂的末端的三个所述运动轴；其中，所述重合点为所述手术机器人与所述远心点重合的点；

表示所述重合点的x方向速度，/>表示所述重合点y方向速度，/>表示所述重合点z方向速度，/>表示重合点的绕x方向旋转速度，/>表示重合点的绕y方向旋转速度，/>表示重合点的绕z方向旋转速度，/>和 />分别表示两个虚拟轴的第一关节输出速度。

10.根据权利要求9所述的手术机器人，其特征在于，所述步骤S4之后还包括：

手术完成或中止时沿所述直线引导部拉出所述手术器械，解除戳卡座与戳卡的连接，解除全部所述运动轴的限制。