CN110893118B - 手术机器人***以及机械臂的运动控制方法 - Google Patents

Abstract

本发明提供一种手术机器人***以及机械臂的运动控制方法。所述手术机器人***包括控制模块，具有至少五个自由度的机械臂，以及设于机械臂末端的手术器械，所述机械臂至少包括五个关节，三个关节用于控制手术器械末端的位置，另两个关节用于控制手术器械通过主动不动点；所述控制模块用于根据预设的期望位置和主动不动点的位置获得手术器械的期望姿态，并根据机械臂逆运动学模型计算得到机械臂各个关节的期望绝对状态参数，进而用于根据机械臂各个关节的期望绝对状态参数，控制机械臂驱动手术器械通过主动不动点，并且手术器械末端运动至期望位置。以此从算法上实现了机械臂末端的不动点约束，简化了机械臂的结构，提高了机械臂各关节的配合能力。

Description

手术机器人***以及机械臂的运动控制方法

技术领域

本发明涉及医疗器械技术领域，特别涉及一种手术机器人***以及机械臂的运动控制方法。

背景技术

手术机器人凭借优越的性能、高精度的控制、直观的手术图像和良好的术后恢复等优势，越来越受到人们的关注，其使用范围也越来越大，关节手术机器人便是其中一种。在关节手术中(包括膝关节和髋关节的修复或置换手术)，需要对关节骨骼进行研磨和钻孔等操作，需要对手术器械进行精确控制，保证对关节骨骼修整的准确性，因此对手术执行者要求较高。以MAKOplasty为代表的关节手术机器人，以其准确的控制和定位精度、边界保护等功能，大大提高了关节手术操作精度、手术效率和术后恢复效果。

然而，发明人发现，当前关节机器人主要为辅助操作模式，在手术过程中末端器械的位置和姿态主要由医生控制，机器人辅助保证精度和安全，即，在此模式下，由操作者直接用手控制手术器械，同时由机器人提供辅助协调控制，实现末端器械的精确定位，并对相应组织进行修整。

因此，有必要开发一种能够实现全自动操作的手术机器人***，其能够在有限的操作空间下，自动精确地确定末端器械的期望位置和姿态。

发明内容

有鉴于此，本发明的目的在于提供一种手术机器人***以及机械臂的运动控制方法，用于通过控制模块确定末端器械的期望姿态，以此降低硬件配置和机械构件的成本，同时提高手术操作的灵活性，并还可方便地调整主动不动点的位置，以此使手术机器人适应不同的手术需要，扩展机械臂的工作能力。

因此，根据本发明的一个方面，提出一种手术机器人***，其包括控制模块，具有至少五个自由度的机械臂，以及挂载于机械臂末端的手术器械，所述机械臂至少包括五个关节；其中：

所述机械臂用于驱动所述手术器械围绕一主动不动点运动，所述机械臂的三个关节用于控制手术器械末端的位置，所述机械臂的另外两个关节用于控制所述手术器械通过所述主动不动点；

所述控制模块用于根据预设的期望位置和所述主动不动点的位置获得所述手术器械的期望姿态，并根据机械臂逆运动学模型计算得到机械臂各个关节的期望绝对状态参数；进而所述控制模块用于根据机械臂各个关节的期望绝对状态参数，控制所述机械臂驱动所述手术器械通过所述主动不动点，并且手术器械末端运动至期望位置。

此外，根据本发明的另一个方面，还提供了一种机械臂的运动控制方法，该机械臂至少具有五个自由度并至少包括五个关节，所述机械臂的三个关节用于控制手术器械末端的位置，所述机械臂的另外两个关节用于控制所述手术器械通过所述主动不动点，所述运动控制方法包括：

将一手术器械挂载于所述机械臂的末端；

一控制模块根据预设的期望位置和一主动不动点的位置获得所述手术器械的期望姿态，并根据机械臂逆运动学模型计算得到机械臂各个关节的期望绝对状态参数；

所述控制模块根据机械臂各个关节的期望绝对状态参数，控制机械臂驱动所述手术器械通过所述主动不动点，并且手术器械末端运动至期望位置。

进一步地，所述手术器械的期望姿态包括手术器械之期望坐标系的Z轴的期望姿态；所述手术器械之期望坐标系的Z轴的期望姿态的计算如下：

其中：n_te为手术器械的期望坐标系的Z轴的期望姿态在机械臂基坐标系下的表示；P_to为手术器械末端的期望位置在机械臂基坐标系下的表示；P_fo为主动不动点的位置在机械臂基坐标系下的表示。

进一步地，所述手术器械的期望姿态还包括任取的一手术器械之期望坐标系的Y轴的期望姿态，以及手术器械之期望坐标系的X轴的期望姿态，所述手术器械之期望坐标系的X轴的期望姿态根据右手定则、Y轴的期望姿态以及Z轴的期望姿态获得；或者，

所述手术器械的期望姿态还包括手术器械之期望坐标系的Y轴的期望姿态，以及任取的一手术器械之期望坐标系的X轴的期望姿态，所述手术器械之期望坐标系的Y轴的期望姿态根据右手定则、X轴的期望姿态以及Z轴的期望姿态获得。

进一步地，所述手术器械之期望坐标系的Y轴的期望姿态为：

p_te＝n_te×r_tc

所述手术器械之期望坐标系的X轴的期望姿态为:

r_te＝p_te×n_te

其中：p_te为手术器械之期望坐标系的Y轴的期望姿态在机械臂基坐标系下的表示；r_tc为手术器械的坐标系的X轴的当前姿态在机械臂基坐标系下的表示；r_te为手术器械之期望坐标系的X轴的期望姿态在机械臂基坐标系下的表示。

进一步地，所述机械臂还包括用于测量关节绝对状态参数的位置传感器，所述位置传感器与所述控制模块通信连接；

所述机械臂末端和/或手术器械上设有至少一不动点标志位；

当所述不动点标志位与所述主动不动点重合时，所述控制模块通过所述位置传感器获取机械臂各个关节的绝对状态参数，进而根据机械臂正运动学模型计算得到所述主动不动点的位置。

进一步地，所述主动不动点的位置在机械臂基坐标系下的表示如下：

P_f＝^bT₁×¹T₂ײT₃×…×^n-1T_n×ⁿP_f

其中:P_f为主动不动点的位置在机械臂基坐标系下的表示；^n-1T_n为关节n-1坐标系到关节n坐标系的转换矩阵；n为关节数量；ⁿP_f为主动不动点的位置在关节n坐标系下的表示；^bT₁为关节1坐标系到机械臂基坐标系的转换矩阵。

进一步地，所述手术机器人***还包括规划模块，与所述控制模块通信连接；所述规划模块用于提供由多个位置点组成的手术路径，所述控制模块用于控制所述手术器械末端沿所述手术路径运动至期望位置。

进一步地，所述控制模块先判断所述手术器械末端的当前位置点与期望位置点之间的距离是否超过预设的距离限值；

若超过，则所述控制模块在所述手术器械末端的当前位置点与期望位置点之间设置多个中间轨迹点，所述中间轨迹点之间的距离不大于预设的距离限值，然后，所述控制模块控制所述手术器械末端依次经过多个所述中间轨迹点，到达期望位置点；

若未超过，则所述控制模块直接控制所述手术器械末端运动至期望位置点。

进一步地，所述控制模块根据手术器械末端的当前位置点与期望位置点通过插值获得所述中间轨迹点的位置。

进一步地，所述中间轨迹点的位置的计算如下：

P_tk＝P_tc+k(P_te-P_tc)/m

其中：P_tk为中间轨迹点的位置；P_tc为手术器械末端的当前位置；P_te为手术器械末端的期望位置；m为中间轨迹点的个数，k＝1,2，…，m-1。

进一步地，所述控制模块根据手术器械末端的期望的笛卡尔速度，通过雅可比矩阵的逆矩阵，获得所述机械臂各个关节的期望速度；进而所述控制模块根据机械臂各个关节的期望绝对状态参数和期望速度，控制机械臂驱动所述手术器械通过所述主动不动点，并且手术器械末端运动至期望位置。

进一步地，所述期望绝对状态参数为转动关节的绝对转动角度，或移动关节的绝对位移。

进一步地，所述手术机器人***还包括模式选择模块，用于选择性地使手术机器人***处于多种工作模式中的一种，且多种工作模式中至少包括自动控制模式，在所述自动控制模式下，所述机械臂驱动手术器械受到主动不动点约束或其他姿态条件约束。

本发明提供的手术机器人***以及机械臂的运动控制方法具有如下有益效果：

第一、本发明的手术机器人***包括控制模块，所述控制模块可以根据预设的期望位置和主动不动点的位置获得手术器械的期望姿态，并根据机械臂逆运动学模型计算得到机械臂各个关节的期望绝对状态参数，进而控制模块根据机械臂各个关节的期望绝对状态参数，控制机械臂驱动手术器械通过主动不动点，并且手术器械末端运动至期望位置；以此从算法上实现了机械臂末端的不动点约束，致使机械臂上无需配置不动点机构，从而简化机械臂的结构，减小机械臂的体积，有助于机械臂的安装和使用，且不动点机构的消失还有利于提高机械臂各关节的配合能力，提升机械臂调整的灵活性；

第二、本发明的机械臂末端和/或手术器械上设有至少一个不动点标志位，当不动点标志位与主动不动点重合时，所述控制模块可通过位置传感器获取机械臂各个关节的绝对状态参数，进而根据机械臂正运动学模型得到主动不动点的位置，因此，可以快速、方便地根据不动点标志位的位置确定主动不动点的位置；并且，不动点标志位可以是多个，只要将不同的不动点标志位移动至期望的主动不动点处，即可实现不动点的调整，以此满足各种应用需求，扩展机械臂的工作能力；

第三、本发明的手术机器人***还包括规划模块，该规划模块可提供由多个位置点组成的手术路径，进而以手术路径上每个位置点作为期望位置，所述控制模块连续控制所述手术器械末端沿所述手术路径上的位置点运动，由此可以实现控制手术器械沿手术路径运动。进一步，所述控制模块还判断手术器械末端的当前位置点与期望位置点之间的距离超过预设的距离限值，以确保手术器械围绕主动不动点运动。更进一步，所述控制模块还通过控制关节的速度，来实现手术器械末端以期望的速度运动，以保证末端运行平稳，减少抖动现象发生。

附图说明

本发明的实施方法以及相关实施例的特征、性质和优势将通过结合下列附图进行描述，其中：

图1是本发明一个实施例提供的机械臂的结构示意图；

图2是本发明一个实施例提供的机械臂末端之手术器械围绕主动不动点运动的示意图；

图3是本发明一个实施例提供的机械臂末端之手术器械进入体内进行手术操作的示意图；

图4是本发明一个实施例提供的实现机械臂之运动控制的流程图；

图5是本发明优选实施例提供的实现机械臂之运动控制的流程图；

图6是本发明一个实施例提供的手术机器人***的工作流程图。

图中：

200-机械臂：1-第一转动关节；2-第一摆动关节；3-第二摆动关节；4-第二转动关节；5-第三摆动关节；6-手术器械；7-患者；8-组织器官；RC-不动点标志位；FP-主动不动点。

具体实施方式

如背景技术所述，发明人发现，现有的关节手术机器人在手术过程中不能实现全自动操作。发明人进一步研究发现，若要实现关节手术机器人的全自动操作，最重要的问题在于确定器械末端的期望位置和期望姿态。然而，器械末端的期望位置一般可由输入端的导航***直接提供(例如由操作者根据病患组织位置，手术类型，以及人体组织分布需要，设定手术路径)，因此，可通过导航***直接设定末端器械的期望位置(即机械臂关节上期望进行修整的位置)。但是，末端器械的期望姿态的确定比较困难，而且在确定末端器械的姿态时，需要保证关节手术机器人在运动过程中不会与组织发生干涉，引发安全问题，尤其在微创伤手术中，由于切口小，因此，器械末端的姿态更需要得到严格的控制。发明人还发现，除了用于关节手术的机器人外，对于工作在其他场景中的手术机器人同样存在着这些问题，也需要解决之。

为此，本发明实施例提出了一种手术机器人***及机械臂的运动控制方法，用于通过内部的控制模块实现机械臂的全自动操作，并将末端器械严格约束在术中主动不动点附近，而且根据手术应用需要，能够随时调整主动不动点的位置。

其中，所述手术机器人***包括控制模块，具有至少五个自由度的机械臂，以及挂载于机械臂末端的手术器械，所述机械臂至少包括五个关节；其中：

所述机械臂用于驱动所述手术器械围绕一主动不动点运动，所述机械臂的三个关节用于控制手术器械末端的位置，所述机械臂的另外两个关节用于控制所述手术器械通过所述主动不动点；所述控制模块用于根据预设的期望位置和所述主动不动点的位置获得所述手术器械的期望姿态，并根据机械臂逆运动学模型计算得到机械臂各个关节的期望绝对状态参数；所述控制模块进而用于根据机械臂各个关节的期望绝对状态参数，控制所述机械臂驱动所述手术器械通过所述主动不动点，并且手术器械末端运动至期望位置。这里关节的“绝对状态参数”，是指关节当前位置相对于初始化时关节位置的变化量。对于转动关节而言，“绝对状态参数”即为绝对转动角度，对于移动关节而言，“绝对状态参数”即为绝对位移。

需说明的是，本发明对于手术器械的具体类型不作特别的要求，可以是腹腔镜，也可以是其他手术器械，如镊子、开合器、剪刀、打孔器等。

下面将结合示意图对本发明进行更详细的描述，其中表示了本发明的优选实施例，应该理解本领域技术人员可以修改在此描述的本发明，而仍然实现本发明的有利效果。因此，下列描述应当被理解为对于本领域技术人员的广泛知道，而并不作为对本发明的限制。

为了清楚，不描述实际实施例的全部特征。在下列描述中，不详细描述公知的功能和结构，因为它们会使本发明由于不必要的细节而混乱。应当认为在任何实际实施例的开发中，必须做出大量实施细节以实现开发者的特定目标，例如按照有关***或有关商业的限制，由一个实施例改变为另一个实施例。另外，应当认为这种开发工作可能是复杂和耗费时间的，但是对于本领域技术人员来说仅仅是常规工作。

在下列段落中参照附图以举例方式更具体地描述本发明。根据下面说明和权利要求书，本发明的优点和特征将更清楚。需说明的是，附图均采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施例的目的。

以下列举的较优实施例，以清楚说明本发明的内容，应当明确的是，本发明的内容并不限制于以下实施例，其他通过本领域普通技术人员的常规技术手段的改进亦在本发明的思想范围之内。

下面参考附图1至附图3，对机械臂的结构做进一步详细的说明。

如图1所示，本发明实施例的手术机器人***包括一机械臂200，该机械臂200具有五个自由度，同时该机械臂200包括顺次连接的第一转动关节1、第一摆动关2节、第二摆动关节3、第二转动关节4和第三摆动关节5。所述第一转动关节1的轴线与所述第一摆动关节2的轴线垂直相交；所述第一摆动关节2的轴线与所述第二摆动关节3的轴线相平行；所述第二摆动关节3的轴线与所述第二转动关节4的轴线垂直相交；所述第三摆动关节5的轴线与所述第二摆动关节3的轴线相平行；所述第三转动关节的轴线与第二转动关节4的轴线相交设置。在本实施例中，所有关节均为转动关节，在一些替代性实施例中，机械臂200也可以包括移动关节。

此外，所述手术机器人***还包括一手术器械6，所述手术器械6挂载于机械臂200的末端，机械臂200具体通过第三摆动关节5连接手术器械6。于是，可通过机械臂200驱动手术器械6围绕主动不动点FP运动，在运动过程中，通过第一转动关节1、第一摆动关2节和第二摆动关节3控制手术器械6的空间位置，并通过第二转动关节4和第三摆动关节5控制手术器械6通过主动不动点FP。需明确的是，这里的主动不动点FP实际上是空间上的虚拟不动点，并不存在于机械臂或手术器械上，在实际手术操作时，该主动不动点FP需与患者身上的创口相重合，避免对患者造成二次伤害。

结合图2和图3来讲，实际手术时，首先将手术器械6通过患者7身上开设的创口***体内，将主动不动点FP设置为创口位置，之后，所述机械臂200通过其五个关节的相互配合，将手术器械6调整到所期望的位置和姿态，例如将手术器械6从姿态1调整到姿态2或姿态3，调整过程中，机械臂200驱动手术器械6始终围绕主动不动点FP运动，从而对组织器官8进行相关的手术操作。

另外，所述手术机器人***还包括一控制模块(未图示)，用于对机械臂200的运动进行控制。

以下结合图4和图5，对机械臂200的运动控制过程作进一步详细的说明。

首先如图4所示，在一个实施例中，所述机械臂200的运动控制过程包括：

所述控制模块获得主动不动点的位置401，并结合预设的期望位置402，获得手术器械的期望姿态403；

之后，所述控制模块根据手术器械的期望姿态403，利用机械臂逆运动学模型计算得到机械臂各转动关节的期望角度404；

然后，所述控制模块根据机械臂各转动关节的期望角度404，控制机械臂各转动关节运动405，以使机械臂200驱动手术器械6通过主动不动点FP，并且所述手术器械末端运动至期望位置406。

本实施例中，可通过不动点标志位RC的位置来获取所述主动不动点的位置401。所述不动点标志位RC既可以设置在机械臂的末端，也可以设置在手术器械6上，或者同时在机械臂的末端以及手术器械上设置不动点标志位RC。具体的使用方式为，当所述不动点标志位RC与所述主动不动点FP重合时，所述控制模块获取当前机械臂各个转动关节的绝对转动角度，进而根据机械臂正运动学模型计算得到所述主动不动点的位置401。较佳地，所述机械臂200还包括测量转动关节之转动角度的位置传感器，所述位置传感器与所述控制模块通信连接，于是，可通过所述位置传感器获取机械臂各个转动关节的绝对转动角度。

本发明实施例中，所述不动点标志位RC可以为多个，并选定其中一个不动点标志位RC在进入自动控制模式(如果有)，被配置为与主动不动点FP相重合，从而将该选定的不动点标志位RC的位置确定为主动不动点FP的位置。例如，当机械臂处于被动调整模式时，通过手动调整机械臂，可使其中一个不动点标志位RC移动到期望的主动不动点FP处，并将此时的不动点标志位RC的位置记录为主动不动点FP的位置。因此，当不动点标志位RC为多个时，将不同的不动点标志位移动到期望的主动不动点处，便可方便地更改主动不动点的位置，以满足各种应用需求。

本发明实施例中，所述主动不动点的位置在机械臂基坐标系下的表示如下：

P_f(x_f,y_f,z_f) (1-1)

其中：P_f为主动不动点在机械臂基坐标系下的笛卡尔位置；x_f为主动不动点在机械臂基坐标系之x轴上的坐标位置；y_f为主动不动点在机械臂基坐标系之y轴上的坐标位置；z_f为主动不动点在机械臂基坐标系之z轴上的坐标位置。

进一步地，所述主动不动点的位置在机械臂基坐标系下的表示如下：

P_f＝^bT₁×¹T₂ײT₃×…×^n-1T_n×ⁿP_f (1-2)

其中:P_f为主动不动点的位置在机械臂基坐标系下的表示；^n-1T_n为关节n-1坐标系到关节n坐标系的转换矩阵；n为关节数量；ⁿP_f为主动不动点的位置在关节n坐标系下的表示；^bT₁为关节1坐标系到机械臂基坐标系的转换矩阵。

此外，预设的期望位置即是手术器械末端的期望位置，由操作者自行设定，例如通过输入端的规划模块提供，从而得到手术器械6的期望位置。

进一步地，所述手术器械6的期望姿态具体包括手术器械之期望坐标系的Z轴的期望姿态(即沿手术器械6的轴线方向)。所述手术器械之期望坐标系的Z轴的期望姿态的计算如下：

其中：n_te为手术器械的期望坐标系的Z轴的期望姿态在机械臂基坐标系下的表示；P_to为手术器械末端的期望位置在机械臂基坐标系下的表示；P_f为主动不动点的位置在机械臂基坐标系下的表示。

然而，本发明对手术器械6的期望坐标系之X轴和Y轴的取向不做具体限制。所述手术器械的期望姿态还包括任取的一手术器械之期望坐标系的Y轴的期望姿态，以及手术器械之期望坐标系的X轴的期望姿态，所述手术器械之期望坐标系的X轴的期望姿态根据右手定则、Y轴的期望姿态以及Z轴的期望姿态获得。或者，所述手术器械的期望姿态还包括手术器械之期望坐标系的Y轴的期望姿态，以及任取的一手术器械之期望坐标系的X轴的期望姿态，所述手术器械之期望坐标系的Y轴的期望姿态根据右手定则、X轴的期望姿态以及Z轴的期望姿态获得。

在一个示范性的实施例中，所述手术器械之期望坐标系的Y轴的期望姿态为：

p_te＝n_te×r_tc (1-4)

其中：p_te为手术器械之期望坐标系的Y轴的期望姿态在机械臂基坐标系下的表示；r_tc为手术器械的坐标系的X轴的当前姿态在机械臂基坐标系下的表示；n_te为手术器械的期望坐标系的Z轴的期望姿态在机械臂基坐标系下的表示。

所述手术器械之期望坐标系的X轴的期望姿态为:

r_te＝p_te×n_te (1-5)

其中：r_te为手术器械之期望坐标系的X轴的期望姿态在机械臂基坐标系下的表示。按上述方法计算的期望姿态和当前姿态偏差较小，有利于后续计算。

由此，便可得到手术器械6的期望姿态(r_te,p_te,n_te),在此姿态下结合手术器械末端的期望位置P_to，便可确保手术器械6始终经过主动不动点FP。

在一个优选的实施例中，所述手术机器人***还优选包括规划模块，与控制模块通信连接。所述规划模块用于提供由多个位置点组成的手术路径，所述控制模块则用于控制所述手术器械末端沿所述手术路径遍历所有的位置点运动至手术路径的末端，例如到达目标组织器官。在此过程中，所述控制模块需要不断将手术器械末端的当前位置点的下一个位置点作为期望位置，按照上述方法控制手术器械运动。

进一步地，发明人发现，如果手术器械末端的当前位置点与期望位置点之间的移动距离过大，若按照第一种方式控制机械臂运动，则有可能降低不动点约束的精度。

因此，所述控制模块在计算得到手术器械的期望姿态之前，先判断所述手术器械末端的当前位置点与期望位置点之间的距离是否超过预设的距离限值；若超过，则所述控制模块在手术器械末端的当前位置点与期望位置点之间设置多个中间轨迹点，且任意两相邻所述中间轨迹点之间的距离不大于预设的距离限值；然后，按照图4的方式执行多次，使控制模块控制手术器械末端依次经过多个所述中间轨迹点，最终到达期望位置点。反之，若移动距离未超过预设的距离限值，则直接执行图4所示的方式，使所述控制模块直接控制所述手术器械末端运动至期望位置点。

本发明实施例中，中间轨迹点的数量与约束的移动距离偏差的大小有关，即，人为设定了移动距离的最大限值，每一次运动时不能超过该最大限值，如此，在最大限值范围内，便能有效的将手术器械6的运动约束在主动不动点FP附近，确保手术器械6通过主动不动点FP，且手术器械末端位于期望的位置。本发明对于设置中间轨迹点的具体方法没有特别的限制，本领域技术人员可以根据现有的插值法，包括但不限于样条插值法、拉格朗日插值法、牛顿插值法、埃尔米特插值法等获得中间轨迹点。

优选，实现中间轨迹点设置的方法包括：在手术器械末端的当前位置点和最终期望位置点之间等距***一系列中间轨迹点，将这些中间轨迹点作为新的期望位置点。所述中间轨迹点的位置的计算如下：

P_tk＝P_tc+k(P_te-P_tc)/m (1-6)

其中：P_tk为中间轨迹点的位置；P_tc为手术器械末端的当前位置；P_te为手术器械末端的期望位置；m为中间轨迹点的个数，具体数值根据当前位置点和最终期望位置点之间的距离以及预设的距离限值来确定，k＝1,2，…，m-1。

这样一来，在对机械臂的运动进行控制时，增加了手术器械末端移动距离的约束，进一步确保了手术器械末端沿规划路径运动过程中，手术器械始终约束在主动不动点附近。

接下去结合图5，更详细地描述机械臂沿手术路径运动的控制过程，具体包括：

所述控制模块根据预设的期望轨迹(即手术路径)获得所有的位置点501，并根据手术器械末端的当前位置，获得手术器械末端的期望位置点502，即手术器械末端需要移动到的目标位置点；

然后，所述控制模块判断手术器械末端的当前位置点和期望位置点之间的移动距离是否超过预设的距离限值503；若没有超过，则所述控制模块根据手术器械末端的期望位置以及主动不动点的位置，驱动手术器械通过主动不动点，而且手术器械末端运动至期望位置505，具体的过程可以参见图4的说明；接着，所述控制模块判断是否已经到达手术路径的终点506，即期望位置点为手术路径的末端点；如果已经到达终点，则结束手术器械调整508。反之，若手术器械末端的当前位置点和期望位置点之间的移动距离超过预设的距离限值，则所述控制模块在手术器械末端的当前位置点与期望位置点之间设置多个中间轨迹点504，然后，所述控制模块根据手术器械末端的期望位置以及主动不动点的位置，驱动手术器械通过主动不动点，而且手术器械末端运动至期望位置505’，具体的过程可以参见图4的说明。这里的期望位置可以是中间轨迹点的位置，也可以是期望位置点的位置。接着在步骤505’之后，所述控制模块会判断是否已经遍历所有中间轨迹点以及期望位置点507，即所述手术器械是否已经通过了所有的中间轨迹点，并到达期望位置点。进一步，判断手术器械末端是否到达规划路径的终点506，若否，则继续执行上述步骤，直至手术器械末端依次经过所有的位置点到达手术路径的终点，若是，则结束手术器械的调整508。

较佳地，所述控制模块还根据手术器械末端的期望的笛卡尔速度，通过雅克比矩阵的逆矩阵，获得机械臂各个转动关节的期望速度509，进而，所述控制模块根据机械臂各个转动关节的期望角度和期望速度，控制机械臂驱动所述手术器械通过所述主动不动点，并控制所述手术器械末端以期望的笛卡尔速度运动至期望位置，其中所述机械臂各个转动关节的期望速度的计算如下：

其中：

为机械臂各关节的期望速度；J为雅克比矩阵；v为预设的为手术器械末端的期望的笛卡尔速度。这样可增加底层速度控制，保证机械臂运动的平稳性，减少抖动现象发生。

进一步地，图6示出了本发明实施例的手术机器人***的工作方式，其具体如下：

所述手术机器人***还包括模式选择模块，用于选择手术机器人***的工作模式。在一个示范性实施例中，所述工作模式包括协从控制模式、自动控制模式、以及辅助调整模式。

启动手术机器人***700(即开机上电)，由步骤701完成机械臂的一系列初始化后，使手术机器人***进入步骤703的待机状态；在待机状态下，机械臂上各个电机处于抱闸状态，不可运动；进而，通过模式选择模块选择不同的工作模式，由待机状态可自由切换到步骤702的协从控制模式、步骤704的辅助调整模式或步骤705的自动控制模式；这里，只有当机械臂处于自动控制模式时，才能够启动机械臂的不动点约束控制，否则在其他状态下，机械臂的运动不会受到主动不动点的限制；例如，当机械臂处于辅助调整模式时，可通过机械臂上各机械臂关节处设置的力传感器测量操作人员施加在机械臂上的作用力，之后，控制器控制电机辅助驱动机械臂，使操作人员可以更加自由、便利的改变机械臂末端的位置和姿态，以便将机械臂上被选定的一个不动点标志位RC移动到期望的主动不动点FP位置；另外，当机械臂处于协从控制模式时，机械臂的工作方式与辅助调整模式下的类似，但是可根据手术机器人***的应用场景的不同，添加附加功能，如虚拟夹具、虚拟边界限制等。

更进一步，将手术机器人***切换至自动控制模式后，则通过步骤706来判断是否启用机械臂的不动点约束功能，若启动，则直接进入步骤707的不动点约束控制，若不启动，则进入步骤708的其他姿态控制。

进入步骤707后，便可按照前述提供的机械臂的运动控制流程实现机械臂的不动点约束，以此约束手术器械围绕主动不动点FP运动。而且，在不动点约束控制下，操作者可随时切换回待机状态703，以便重新调整主动不动点的位置，即在执行不动点约束时，通过步骤709来判断是否调整主动不动点的位置，从而确定是否重新进入待机状态，进而可由待机状态切换至辅助调整模式，以此调整主动不动点的位置。

最后，本发明较佳实施例如上所述，但不限于上述实施例公开的范围，例如本发明对于机械臂的具体构型不作具体限制，实际中，机械臂还可以是大于5个自由度，且关节的数量随之也可大于五个。此外，本发明实施例的控制模块和规划模块可集成在手术机器人***现有的设备中，也可以单独设置。上述实施例中，机械臂包括了5个转动关节。本领域技术人员应理解，机械臂也可以包括移动关节，该替代性实施例同样在本发明的保护范围内，实现相似的技术效果。

综上，本发明实施例提供的手术机器人***以及机械臂的运动控制方法具有如下有益效果：

第一、本发明实施例的手术机器人***包括一个控制模块，该控制模块可以根据预设的期望位置和主动不动点的位置获得手术器械的期望姿态，并根据机械臂逆运动学模型计算得到机械臂各个转动关节的期望绝对转动角度，进而控制模块根据机械臂各个转动关节的期望绝对转动角度，控制机械臂驱动手术器械通过主动不动点，并且手术器械末端运动至期望位置；以此从算法上实现了机械臂末端的不动点约束，致使机械臂上无需配置不动点机构，从而简化机械臂的结构，减小机械臂的体积，有助于机械臂的安装和使用，且不动点机构的消失还有利于提高机械臂各关节的配合能力，提升机械臂调整的灵活性；

第二、本发明实施例的机械臂末端和/或手术器械上设有至少一个不动点标志位，当不动点标志位与主动不动点重合时，所述控制模块可通过位置传感器获取机械臂各个转动关节的绝对转动角度，进而根据机械臂正运动学模型得到主动不动点的位置，因此，可以快速、方便地根据不动点标志位的位置确定主动不动点的位置；并且，不动点标志位可以是多个，只要将不同的不动点标志位移动至期望的主动不动点处，即可实现不动点的调整，以此满足各种应用需求，扩展机械臂的工作能力；

第三、本发明实施例的手术机器人***还包括一个规划模块，该规划模块可提供由多个位置点组成的手术路径，进而以手术路径上每个位置点作为期望位置，所述控制模块连续控制所述手术器械末端沿所述手术路径上的位置点运动，由此可以实现控制手术器械沿手术路径运动。进一步，控制模块还判断手术器械末端的当前位置点与期望位置点之间的距离超过预设的距离限值，以确保手术器械围绕主动不动点FP运动。更进一步，控制模块还通过控制关节的速度，来实现手术器械末端以期望的速度运动，以保证末端运行平稳，减少抖动现象发生。

上述描述仅是对本发明较佳实施例的描述，并非对本发明范围的任何限定，本发明领域的普通技术人员根据上述揭示内容做的任何变更、修饰，均属于权利要求书的保护范围。

Claims (21)

1.一种手术机器人***，包括控制模块，具有五个自由度的机械臂，以及挂载于机械臂末端的手术器械，所述机械臂包括五个关节；其中，

所述机械臂用于驱动所述手术器械围绕一主动不动点运动，所述机械臂的三个关节用于控制手术器械末端的位置，所述机械臂的另外两个关节用于控制所述手术器械通过所述主动不动点；所述机械臂末端和/或手术器械上设有多个不动点标志位；

当其中一个所述不动点标志位与所述主动不动点重合时，所述控制模块获取机械臂各个关节的绝对状态参数，进而根据机械臂正运动学模型计算得到所述主动不动点的位置；

所述控制模块用于根据预设的期望位置和所述主动不动点的位置获得所述手术器械的期望姿态，并根据机械臂逆运动学模型计算得到机械臂各个关节的期望绝对状态参数；进而所述控制模块用于根据机械臂各个关节的期望绝对状态参数，控制所述机械臂驱动所述手术器械通过所述主动不动点，并且手术器械末端运动至期望位置。

2.根据权利要求1所述的手术机器人***，其特征在于，所述手术器械的期望姿态包括手术器械之期望坐标系的Z轴的期望姿态；所述手术器械之期望坐标系的Z轴的期望姿态的计算如下：

其中：n _te 为手术器械的期望坐标系的Z轴的期望姿态在机械臂基坐标系下的表示；P _t0 为手术器械末端的期望位置在机械臂基坐标系下的表示；P _f0 为主动不动点的位置在机械臂基坐标系下的表示。

3.根据权利要求2所述的手术机器人***，其特征在于，所述手术器械的期望姿态还包括任取的一手术器械之期望坐标系的Y轴的期望姿态，以及手术器械之期望坐标系的X轴的期望姿态，所述手术器械之期望坐标系的X轴的期望姿态根据右手定则、Y轴的期望姿态以及Z轴的期望姿态获得；或者，

所述手术器械的期望姿态还包括手术器械之期望坐标系的Y轴的期望姿态，以及任取的一手术器械之期望坐标系的X轴的期望姿态，所述手术器械之期望坐标系的Y轴的期望姿态根据右手定则、X轴的期望姿态以及Z轴的期望姿态获得。

4.根据权利要求3所述的手术机器人***，其特征在于，所述手术器械之期望坐标系的Y轴的期望姿态为：

所述手术器械之期望坐标系的X轴的期望姿态为:

其中：p _te 为手术器械之期望坐标系的Y轴的期望姿态在机械臂基坐标系下的表示；r _tc 为手术器械的坐标系的X轴的当前姿态在机械臂基坐标系下的表示; r _te 为手术器械之期望坐标系的X轴的期望姿态在机械臂基坐标系下的表示。

5.根据权利要求1所述的手术机器人***，其特征在于，所述机械臂还包括用于测量关节绝对状态参数的位置传感器，所述位置传感器与所述控制模块通信连接；

当其中一个所述不动点标志位与所述主动不动点重合时，所述控制模块通过所述位置传感器获取机械臂各个关节的绝对状态参数，进而根据机械臂正运动学模型计算得到所述主动不动点的位置。

6.根据权利要求5所述的手术机器人***，其特征在于，所述主动不动点的位置在机械臂基坐标系下的表示如下：

其中:P _f 为主动不动点的位置在机械臂基坐标系下的表示；

为关节n-1坐标系到关节n坐标系的转换矩阵；n为关节数量；

为主动不动点的位置在关节n坐标系下的表示；

为关节1坐标系到机械臂基坐标系的转换矩阵。

7.根据权利要求1所述的手术机器人***，其特征在于，还包括规划模块，与所述控制模块通信连接；所述规划模块用于提供由多个位置点组成的手术路径，所述控制模块用于控制所述手术器械末端沿所述手术路径运动至期望位置。

8.根据权利要求7所述的手术机器人***，其特征在于，所述控制模块先判断所述手术器械末端的当前位置点与期望位置点之间的距离是否超过预设的距离限值；

若超过，则所述控制模块在所述手术器械末端的当前位置点与期望位置点之间设置多个中间轨迹点，所述中间轨迹点之间的距离不大于预设的距离限值，然后，所述控制模块控制所述手术器械末端依次经过多个所述中间轨迹点，到达期望位置点；

若未超过，则所述控制模块直接控制所述手术器械末端运动至期望位置点。

9.根据权利要求8所述的手术机器人***，其特征在于，所述控制模块根据手术器械末端的当前位置点与期望位置点通过插值获得所述中间轨迹点的位置。

10.根据权利要求9所述的手术机器人***，其特征在于，所述中间轨迹点的位置的计算如下：

其中：

为中间轨迹点的位置；

为手术器械末端的当前位置；

为手术器械末端的期望位置；m为中间轨迹点的个数，

。

11.根据权利要求1所述的手术机器人***，其特征在于，所述控制模块根据手术器械末端的期望的笛卡尔速度，通过雅可比矩阵的逆矩阵，获得所述机械臂各个关节的期望速度；进而所述控制模块根据机械臂各个关节的期望绝对状态参数和期望速度，控制机械臂驱动所述手术器械通过所述主动不动点，并且手术器械末端运动至期望位置。

12.根据权利要求1所述的手术机器人***，其特征在于，所述期望绝对状态参数为转动关节的绝对转动角度，或移动关节的绝对位移。

13.根据权利要求1所述的手术机器人***，其特征在于，还包括模式选择模块，用于选择性地使手术机器人***处于多种工作模式中的一种，且多种工作模式中至少包括自动控制模式，在所述自动控制模式下，所述机械臂驱动手术器械受到主动不动点约束。

14.一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序用于实现机械臂的运动控制，所述机械臂具有五个自由度并包括五个关节，所述机械臂的三个关节用于控制手术器械末端的位置，所述机械臂的另外两个关节用于控制所述手术器械通过主动不动点，所述手术器械挂载于所述机械臂的末端，所述机械臂末端和/或手术器械上设有多个不动点标志位，其特征在于，当所述计算机程序被处理器执行时执行以下的步骤：

当其中一个所述不动点标志位与所述主动不动点相重合时，获取机械臂各个关节的绝对状态参数并反馈至一控制模块，所述控制模块进而根据机械臂正运动学模型获计算得到所述主动不动点的位置；

所述控制模块根据预设的期望位置和所述主动不动点的位置获得所述手术器械的期望姿态，并根据机械臂逆运动学模型计算得到机械臂各个关节的期望绝对状态参数；

所述控制模块根据机械臂各个关节的期望绝对状态参数，控制机械臂驱动所述手术器械通过所述主动不动点，并且手术器械末端运动至期望位置。

15.根据权利要求14所述的计算机可读存储介质，其特征在于，所述手术器械的期望姿态包括手术器械之期望坐标系的Z轴的期望姿态；所述手术器械之期望坐标系的Z轴的期望姿态的计算如下：

其中：n _te 为手术器械的期望坐标系的Z轴的期望姿态在机械臂基坐标系下的表示；P _t0 为手术器械末端的期望位置在机械臂基坐标系下的表示；P _f0 为主动不动点的位置在机械臂基坐标系下的表示。

16.根据权利要求15所述的计算机可读存储介质，其特征在于，所述手术器械的期望姿态还包括任取的一手术器械之期望坐标系的Y轴的期望姿态，以及手术器械之期望坐标系的X轴的期望姿态，所述手术器械之期望坐标系的X轴的期望姿态根据右手定则、Y轴的期望姿态以及Z轴的期望姿态获得；或者，

所述手术器械的期望姿态还包括手术器械之期望坐标系的Y轴的期望姿态，以及任取的一手术器械之期望坐标系的X轴的期望姿态，所述手术器械之期望坐标系的Y轴的期望姿态根据右手定则、X轴的期望姿态以及Z轴的期望姿态获得。

17.根据权利要求16所述的计算机可读存储介质，其特征在于，所述手术器械之期望坐标系的Y轴的期望姿态为：

所述手术器械之期望坐标系的X轴的期望姿态为:

其中：p _te 为手术器械之期望坐标系的Y轴的期望姿态在机械臂基坐标系下的表示；r _tc 为手术器械的坐标系的X轴的当前姿态在机械臂基坐标系下的表示; r _te 为手术器械之期望坐标系的X轴的期望姿态在机械臂基坐标系下的表示。

18.根据权利要求14所述的计算机可读存储介质，其特征在于，当其中一个所述不动点标志位与所述主动不动点相重合时，一位置传感器获取机械臂各个关节的绝对状态参数并反馈至所述控制模块，所述控制模块进而根据机械臂正运动学模型获计算得到所述主动不动点的位置。

19.根据权利要求14所述的计算机可读存储介质，其特征在于，还包括：

一规划模块提供由多个位置点组成的手术路径，进而所述控制模块控制所述手术器械末端沿所述手术路径运动至期望位置。

20.根据权利要求19所述的计算机可读存储介质，其特征在于，所述控制模块先判断所述手术器械末端的当前位置点与期望位置点之间的距离是否超过预设的距离限值；

若超过，则所述控制模块在所述手术器械末端的当前位置点与期望位置点之间设置多个中间轨迹点，所述中间轨迹点之间的距离不大于预设的距离限值，然后，所述控制模块控制所述手术器械末端依次经过多个所述的中间轨迹点，到达期望位置点；

若未超过，则所述控制模块直接控制所述手术器械末端运动至期望位置点。

21.根据权利要求14所述的计算机可读存储介质，其特征在于，所述控制模块根据手术器械末端的期望的笛卡尔速度，通过雅可比矩阵的逆矩阵，获得所述机械臂各个关节的期望速度；进而，所述控制模块根据机械臂各个关节的期望绝对状态参数和期望速度，控制机械臂驱动所述手术器械通过所述主动不动点，并且手术器械末端运动至期望位置。

Images