CN114952806B

CN114952806B - 约束运动控制方法、装置、系统和电子设备

Info

Publication number: CN114952806B
Application number: CN202210687904.0A
Authority: CN
Inventors: 赵进奇; 丁磊; 姚庭; 王超; 史琦亮; 高加超
Original assignee: Faoyiwei Suzhou Robot System Co ltd
Current assignee: Faoyiwei Suzhou Robot System Co ltd
Priority date: 2022-06-16
Filing date: 2022-06-16
Publication date: 2023-10-03
Anticipated expiration: 2042-06-16
Also published as: CN114952806A

Abstract

本申请提供一种约束运动控制方法、装置、系统和电子设备，针对配置有工具的机器人，获得工具所在的工具坐标系相对于机器人末端所在的末端坐标系的变换矩阵，并标定远心不动点，获得目标位姿信息。根据目标位姿信息及变换矩阵得到工具的末端位姿，根据工具的末端位姿中的末端位置信息及标定的远心不动点对变换矩阵进行更新。最后基于更新后的变换矩阵得到机器人各关节的角度位置信息，进而控制实体机器人带动配置的实体工具进行远心不动点约束的运动。本方案基于工具末端、机器人末端、远心不动点之间的相对关系实现约束控制，无需配套的机械结构，减少了维护成本并且降低对手术环境污染的可能性，并且还可避免机械结构件之间连接时产生的误差。

Description

约束运动控制方法、装置、系统和电子设备

技术领域

本申请涉及控制技术领域，具体而言，涉及一种约束运动控制方法、装置、系统和电子设备。

背景技术

在微创手术过程中，手术工具需以切口为中心，绕切口进行远心不动点运动，具体操作如自轴旋转、内外伸缩、偏摆、俯仰等。目前绝大多数远心不动点约束主要是通过手术工具及其配套物理装置组装形成的机械结构所具有的结构特性来实现的。主要有平行四边形杆件式结构、等中心式结构、圆构回转式结构、球铰连接式结构、云台式结构、平行腕转式结构或者钢带传动式结构等。

通过机械结构的方式实现机器人远心不动点运动相对简单，但存在机械结构件组装维护复杂的问题。并且，结构零件之间需要润滑，较容易污染手术环境。结构件连接不可避免产生间隙，容易放大误差，造成重复定位精度不准。而且由于固定的机械结构限制，使得末端工具运动范围上具有一定的局限性，无法在所有运动方向上进行远心不动点约束下的运动。

发明内容

本申请的目的包括，例如，提供了一种约束运动控制方法、装置、系统和电子设备，其能够在低维护成本且低污染风险的情况下实现约束控制。

本申请的实施例可以这样实现：

第一方面，本申请提供一种约束运动控制方法，所述方法包括：

针对配置有工具的机器人，获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵；

标定得到远心不动点，并获得目标位姿信息；

根据所述目标位姿信息及所述变换矩阵得到所述工具的末端位姿；

根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新；

根据更新后的变换矩阵得到机器人各关节的角度位置信息，并基于所述角度位置信息控制实体机器人带动配置的实体工具进行远心不动点约束的运动。

在可选的实施方式中，所述方法还包括预先配置工具信息的步骤，该步骤包括以下之一：

从预先构建的多个工具模型中选择至少一个工具模型，获得选择的至少一个工具模型的工具信息，基于所述工具信息在所述机器人的末端进行工具配置；或

获取以参数设置方式产生的工具信息，基于所述工具信息在所述机器人的末端进行工具配置。

在可选的实施方式中，所述获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵的步骤，包括：

在所述机器人处于多种不同姿态下，控制所述工具的末端与空间中一设置的坐标点重合，并记录各种姿态下所述机器人末端的空间笛卡尔坐标数据；

根据多种不同姿态下所述机器人末端的空间笛卡尔坐标数据计算得到所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的空间笛卡尔坐标，以完成所述工具坐标系的标定；

根据已完成标定的工具坐标系，获得所述工具坐标系相对于所述末端坐标系的变换矩阵。

在可选的实施方式中，所述标定得到远心不动点的步骤，包括：

在所述机器人处于多种不同姿态下，控制所述工具的末端与设置的远心不动点重合，并记录各种姿态下所述工具的末端的空间笛卡尔坐标数据；

根据多种姿态下所述工具的末端的空间笛卡尔坐标数据计算得到所述远心不动点相对于所述机器人的底座所在的基坐标系下的空间笛卡尔坐标，以完成所述远心不动点的标定。

在可选的实施方式中，所述获得目标位姿信息的步骤，包括：

获取摇杆被控制下所产生的偏移量；

在当前位姿信息基础上叠加所述偏移量得到目标位姿信息。

在可选的实施方式中，所述变换矩阵包括多个类型的位姿变换向量和多个方向上的位移信息；

所述根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新的步骤，包括：

根据所述工具的末端位姿中的末端位置信息及标定的远心不动点的坐标计算得到新的位姿变换向量和新的位移信息；

基于所述新的位姿变换向量和新的位移信息对所述变换矩阵进行更新。

在可选的实施方式中，所述根据所述工具的末端位姿中的末端位置信息及标定的远心不动点的坐标计算得到新的位姿变换向量和新的位移信息的步骤，包括：

计算所述工具的末端位姿中的末端位置信息和标定的远心不动点的坐标中同一方向上的坐标值的差值，结合多个方向上的差值计算得到新的位移信息；

根据多个方向上的差值构成差值向量，并根据所述差值向量和所述新的位移信息计算得到新的位姿变换向量。

第二方面，本申请提供一种约束运动控制装置，所述装置包括：

变换矩阵获得模块，用于针对配置有工具的机器人，获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵；

标定模块，用于标定得到远心不动点，并获得目标位姿信息；

末端位姿获得模块，用于根据所述目标位姿信息及所述变换矩阵得到所述工具的末端位姿；

更新模块，用于根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新；

控制模块，用于根据更新后的变换矩阵得到机器人各关节的角度位置信息，并基于所述角度位置信息控制实体机器人带动配置的实体工具进行远心不动点约束的运动。

第三方面，本申请提供一种电子设备，包括一个或多个存储介质和一个或多个与存储介质通信的处理器，一个或多个存储介质存储有处理器可执行的机器可执行指令，当电子设备运行时，处理器执行所述机器可执行指令，以执行前述实施方式中任意一项所述的方法步骤。

第四方面，本申请提供一种约束运动控制系统，包括控制设备、实体机器人、实体工具以及电子设备；

所述电子设备用于，针对配置有工具的机器人，获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵，标定得到远心不动点，并获得目标位姿信息，根据所述目标位姿信息及所述变换矩阵得到所述工具的末端位姿，根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新，根据更新后的变换矩阵得到机器人各关节的角度位置信息；

所述电子设备还用于，将所述角度位置信息发送至所述控制设备；

所述控制设备用于，根据接收到的角度位置信息控制所述实体机器人和实体工具；

所述实体机器人用于，在所述控制设备的控制下带动配置的实体工具进行远心不动点约束的运动。

本申请实施例的有益效果包括，例如：

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本申请实施例提供的约束运动控制系统的结构框图；

图2为本申请实施例提供的约束运动控制方法的流程图；

图3为图2中步骤S101包含的子步骤的流程图；

图4为图2中步骤S102包含的子步骤的流程图；

图5为图2中步骤S102包含的子步骤的另一流程图；

图6为图2中步骤S104包含的子步骤的流程图；

图7为图6中步骤S1041包含的子步骤的流程图；

图8为本申请实施例提供的电子设备的结构框图；

图9为本申请实施例提供的约束运动控制装置的功能模块框图。

图标：110-存储介质；120-处理器；130-约束运动控制装置；131-变换矩阵获得模块；132-标定模块；133-末端位姿获得模块；134-更新模块；135-控制模块；140-通信接口。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

需要说明的是，在不冲突的情况下，本申请的实施例中的特征可以相互结合。

请参阅图1，本申请实施例提供一种约束运动控制系统，该约束运动控制系统包括控制设备、实体机器人、实体工具以及电子设备。其中，实体机器人包括底座以及设置在底座上的机械臂，可以是如六臂机器人、四臂机器人等。实体工具可以是外科手术中所需要使用的，用于实现微创手术等的工具。实体工具可设置在实体机器人的末端。

电子设备可以是如上位机，例如个人计算机、笔记本电脑、服务器等。电子设备可提供人机交互功能，如可提供人机交互应用系统，从而供用户进行信息、参数配置等。此外，电子设备还可提供运动控制系统，以实现控制过程中信息、数据等的计算、控制信息的下发等。

控制设备可以分别连接实体机器人和电子设备，例如通过无线或有线的方式进行连接。控制设备可以基于电子设备所下发的控制指令对实体机器人进行控制，同时对实体机器人上配置的实体工具进行控制，从而实现约束控制。

请参阅图2，为本申请实施例提供的约束运动控制方法的流程图，该控制方法有关的流程所定义的方法步骤可以由上述电子设备所实现。下面将对图2所述的具体流程进行详细阐述。

S101，针对配置有工具的机器人，获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵。

S102，标定得到远心不动点，并获得目标位姿信息。

S103，根据所述目标位姿信息及所述变换矩阵得到所述工具的末端位姿。

S104，根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新。

S105，根据更新后的变换矩阵得到机器人各关节的角度位置信息，并基于所述角度位置信息控制实体机器人带动配置的实体工具进行远心不动点约束的运动。

本实施例中，可以利用电子设备以构建与实体机器人对应的机器人模型，并配置与实体工具对应的工具信息。从而在电子设备上对配置有工具的机器人(机器人模型)实现控制信息等的运算，从而可以基于最终运算得到的控制信息实现对实体机器人和实体工具的控制。

本实施例中，在以下描述中，机器人、工具用于指代基于电子设备所构建的模型类型的对象，而实体机器人、实体工具用于指代在实际应用场景下，可实际用于实现微创手术操作的对象。

本实施例中，首先在机器人上进行工具的配置，可将工具配置在机器人包含的机械臂的末端。机器人通过机械臂可以带动末端上配置的工具进行运动。

在三维空间内，机器人的末端所在的坐标系可为末端坐标系，而工具所在的坐标系可为工具坐标系。在后续实现控制时，主要可以获得对于机器人的控制信息。而最终是需要利用工具来进行微创手术，因此，最终是需要获得对于工具的具体的控制信息。因此，需要将对机器人的控制信息转换为对于工具的控制信息。

因此，本实施例中，为了获得机器人末端的坐标信息与工具的坐标信息之间的关系，首先可获得工具所在的工具坐标系相对于机器人末端所在的末端坐标系的变换矩阵。

本实施例提供的约束运动控制方法目的是对工具相对于微创口进行操作进行约束控制。该微创口即为远心不动点，本实施例通过前期的运动规划以达到对远心不动点的约束运动控制。

本实施例中，首先可进行远心不动点的标定，从而以寻求到远心不动点的坐标信息与机器人的坐标信息之间的相对关系。在此基础上，可以获得目标位姿信息，该目标位姿信息可以是针对机器人末端的，也可以是针对机器人底座的。该目标位姿信息可以基于操作人员的操作信息所获得，表明的是机器人到达目标点时的位姿信息。

而基于上述获得的工具坐标系相对于末端坐标系的变换矩阵，结合得到的目标位姿信息可以获得工具的末端位姿。

由于上述获得的变换矩阵是在标定情况下获得的，若控制机器人运动至目标点同时控制工具，则工具与机器人末端之间的坐标系相对关系可能发生改变。

因此，本实施例中，可根据工具的末端位姿中的末端位置信息及标定的远心不动点对上述获得的变换矩阵进行更新。最后，在更新的变换矩阵下，可以通过逆运动学计算，得到机器人各关节的角度位置，进而可以确定配置在关节末端的工具的角度位置。

最后可以基于得到的各关节的角度位置，通过控制设备相应地控制实体机器人并带动配置的实体工具进行远心不动点约束的运动。

本实施例提供的约束运动控制方案，基于工具末端、机器人末端、远心不动点之间的相关关系实现约束控制，无需如现有技术需要通过配套的机械结构来实现约束控制。可以减少维护成本，并且降低由于需要对机械结构进行润滑等导致的对手术环境污染的可能性。此外，还可避免机械结构件之间连接时所产生的误差。

由上述可知，首先需要在机器人基础上进行工具信息的配置，本实施例中，在一种可能的实现方式中，可以基于电子设备预先构建多个工具模型，每个工具模型具有具体的模型结构，也具有相应的工具信息。

在配置工具信息时，可以从预先构建的多个工具模型中选择至少一个工具模型，获得选择的至少一个工具模型的工具信息，基于工具信息在机器人末端进行工具配置。

本实施例中，对于如微创手术操作，可能需要用到的工具不止一个，因此，而机器人为多臂机器人，因此，可以根据需求选择一个或多个工具模型配置在机器人的末端。

此外，在另一种可能的实现方式中，若未进行工具模型的构建。在进行工具配置时，可以获取以参数设置方式产生的工具信息，基于工具信息在机器人的末端进行工具配置。

本实施例中，操作人员还可以直接基于电子设备提供的交互功能，在电子设备的设置界面上进行参数设置，例如，包括所需配置的工具的工具重量、质心等参数的设置。基于所设置的工具信息，则可以在机器人末端配置以参数信息来表征的虚拟工具。

在完成工具的配置后，请参阅图3，本实施例中，在上述步骤S101中获得工具坐标系相对于末端坐标系的变换矩阵的步骤可以通过以下方式实现：

S1011，在所述机器人处于多种不同姿态下，控制所述工具的末端与空间中一设置的坐标点重合，并记录各种姿态下所述机器人末端的空间笛卡尔坐标数据。

S1012，根据多种不同姿态下所述机器人末端的空间笛卡尔坐标数据计算得到所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的空间笛卡尔坐标，以完成所述工具坐标系的标定。

S1013，根据已完成标定的工具坐标系，获得所述工具坐标系相对于所述末端坐标系的变换矩阵。

本实施例中，所述的处于多种不同姿态主要是指设置机器人末端处于不同的姿态。姿态可用于表征对象在空间里的朝向，刚体的姿态可以用一个3×3的矩阵来表示。

其中，空间中一设置的坐标点可以是空间中任意一确定的坐标点，可在不同姿态下控制工具的末端与该设置的坐标点重合。如此，可以得到多个不同姿态下机器人末端的空间笛卡尔坐标数据。

本实施例中，可以通过运动控制系统基于机器人末端的空间笛卡尔坐标数据计算得到工具坐标系相对于末端坐标系下的空间笛卡尔坐标，从而实现工具坐标系的标定。所谓的工具坐标系的标定，即为获得工具坐标系在末端坐标系下的表征。

在工具坐标系标定的情况下，基于工具坐标系在末端坐标系下的表征，则可以得到工具坐标系相对于末端坐标系的变换矩阵变换矩阵/>可表征为如下形式：

其中：

式中，n、o、a分别表示不同类型的位姿变换向量，其中，n_x、o_x、a_x表示在x方向上的位姿变换向量，n_y、o_y、a_y表示在y方向上的位姿变换向量，n_z、o_z、a_z表示在z方向上的位姿变换向量，p_x表示在x方向上的位移信息，p_y表示在y方向上的位移信息，p_z表示在z方向上的位移信息。

此外，本实施例中，在另一种可能的实现方式中，也可以通过手动输入方式输入工具坐标系相对于机器人末端所在的末端坐标系的空间笛卡尔坐标。基于该手动输入获得的空间笛卡尔坐标从而计算得到工具坐标系相对于末端坐标系的变换矩阵。

在上述基础上，本实施例中，可利用交互应用系统配置RCM(remote center ofmotion，远程运动中心)。请参阅图4，上述步骤S102中标定远心不动点的步骤可以通过以下方式实现：

S1021，在所述机器人处于多种不同姿态下，控制所述工具的末端与设置的远心不动点重合，并记录各种姿态下所述工具的末端的空间笛卡尔坐标数据。

S1022，根据多种姿态下所述工具的末端的空间笛卡尔坐标数据计算得到所述远心不动点相对于所述机器人的底座所在的基坐标系下的空间笛卡尔坐标，以完成所述远心不动点的标定。

本实施例中，在三维空间内设置一远心不动点，该远心不动点的确定可以根据实际真实场景下，需要进行微创操作的伤口的位置进行相应设置。也即，该远心不动点即表征需要进行操作的位置点。

可在机器人处于多种不同姿态下，例如三种不同姿态下，将工具的末端与该远心不动点重合。在重合的状态下记录下工具的末端的空间笛卡尔坐标数据。如此，可以获得工具的末端的三个空间笛卡尔坐标数据。

由上述可知，机器人包括底座以及设置在底座上的机械臂。因此，机器人还具有基于底座所建立的基坐标系。由于底座的基坐标系相对固定，因此，对于远心不动点的标定可以以基坐标系为相对对象。

可以通过运动控制系统，基于获得的工具的末端的空间笛卡尔坐标数据得到远心不动点相对于基坐标系下的空间笛卡尔坐标。并且，可以通知运动控制系统开启RCM功能，以便于后续在对机器人运动规划时使用当前已标定的远心不动点的空间笛卡尔坐标。

此外，本实施例中，在另一种可能的实现方式中，也可以通过手动输入的方式输入远心不动点相对于基坐标系的空间笛卡尔坐标。基于该手动输入的空间笛卡尔坐标，以完成远心不动点的标定。

本实施例中，在电子设备的交互应用系统还提供有摇杆，该摇杆可相应对机器人实现控制。操作人员，例如医生在需要对机器人进行控制时，则可以通过操作该摇杆来实现。请参阅图5，本实施例中，在上述步骤S102中获得目标位姿信息的步骤可以通过以下方式实现：

S1023，获取摇杆被控制下所产生的偏移量。

S1024，在当前位姿信息基础上叠加所述偏移量得到目标位姿信息。

本实施例中，对于摇杆的控制可以是在三维空间内的控制，例如，可以是x轴的正、负方向，y轴的正、负方向，或者是z轴的正、负方向控制。当然，实际应用过程中，也可以实现摇杆在三维空间内的任意方向的操控。操作人员对摇杆进行操作，则基于该操作行为可以获得对于机器人的控制偏移量。该偏移量包括方向以及大小。

本实施例中，通过设置摇杆并对摇杆进行操作的方式，可以便于实际应用场景下操作人员的操作。因为在实际应用场景下，操作人员可能并不知道具体的操作数值，而是需要一边基于控制的结果一边相应调整操作。因此，本实施例中，无需操作人员输入具体的数值，只需基于摇杆即可实现便捷的操作。

在当前位姿信息基础上叠加产生的偏移量则可以得到目标位姿信息，记为其中，当前位姿信息可以是指机器人的末端的当前位姿信息，因此，得到的目标位姿信息相应地为机器人的末端的目标位姿信息。

由于最终是需要利用配置的工具对远心不动点对应的操作位置进行操作，因此，需要获得工具末端的位姿信息。本实施例中，可以基于机器人的目标位姿信息并结合上述获得的变换矩阵得到工具的末端位姿。可选地，可以按照以下公式计算得到工具的末端位姿

在工具末端所需重合的位置点不断变化的情况下，例如由上述的空间中一设置的坐标点变换为远心不动点的情况下，则工具坐标系相对于机器人的末端坐标系的变换矩阵也可能发生变化。因此，本实施例中，需要对变换矩阵进行更新。

请参阅图6，本实施例中，上述步骤S104中对变换矩阵进行更新的步骤可以通过以下方式实现：

S1041，根据所述工具的末端位姿中的末端位置信息及标定的远心不动点的坐标计算得到新的位姿变换向量和新的位移信息。

S1042，基于所述新的位姿变换向量和新的位移信息对所述变换矩阵进行更新。

位姿用于描述某个对象在指定坐标系下的位置和姿态，也即，位姿包含位置信息和姿态信息。根据获得的工具的末端位姿可以获得其中的末端位置信息，标记为(xe,ye,ze)。上述标定得到的远心不动点的坐标可标记为(xr,yr,zr)。基于末端位置信息和远心不动点的坐标可以计算得到新的位姿变换向量和新的位移信息。

请参阅图7，本实施例中，可以通过以下方式计算得到新的位姿变换向量和新的位移信息：

S10411，计算所述工具的末端位姿中的末端位置信息和标定的远心不动点的坐标中同一方向上的坐标值的差值，结合多个方向上的差值计算得到新的位移信息。

S10412，根据多个方向上的差值构成差值向量，并根据所述差值向量和所述新的位移信息计算得到新的位姿变换向量。

由上述可知，变换矩阵中包括多个方向的位移信息，如p_x、p_y和p_z，还包括多个类型的姿变换向量。在计算位姿变换向量和位移信息时，可以计算得到位移信息p_z和位姿变换向量a，也可以根据实际工具形态，计算得到p_x和n，或者是p_y和o。

本实施例中，以计算得到位移信息pz和位姿变换向量a为例。可以通过以下公式计算得到新的位置信息pz和位姿变换向量a：

在此基础上，可以保持n不变，按照公式o＝a×n，进而得到新的位姿变换向量o。

基于上述计算得到的结果得到更新后的变换矩阵，可标记为

在得到更新后的变换矩阵的基础上，可以基于工具的末端位姿以及该新的变换矩阵进行逆运动学计算，得到机器人的末端的位姿信息。进而可以得到机器人的各关节的角度信息。从而基于该角度信息生成驱动指令，通过控制设备来控制实体机器人运动，同时带动配置的实体工具围绕远心不动点进行约束运动控制。

本实施例提供的约束运动控制方案中，只需将工具配置在机器人末端，而无需为工具安装配套的用于实现远心不动点约束的机械结构。具有方便安装和维护的优点，同时减少了机械结构的维护，降低了对手术环境污染的可能性。

通过本实施例提供的约束运动控制方案，避免了因机械结构件之间连接时所产生的误差导致的重复定位精度不准的问题。在该运动控制方案下，机器人可以达到±0.02mm的重复定位精度，避免累计误差导致的远心不动点非轴线平移运动所造成增大患者伤口撕裂可能性的风险。

通过该技术方案使得在机器人运动范围空间内，围绕远心不动点可在笛卡尔坐标下的任意方向进行远心不动点运动，避免了固定的机械结构所造成的运动范围受限。

请参阅图8，为本申请实施例提供的电子设备的示例性组件示意图，该电子设备可以是如个人计算机、笔记本电脑、智能手机、服务器等。该电子设备中可配置有交互应用系统和运动控制系统等，该电子设备可包括存储介质110、处理器120、约束运动控制装置130及通信接口140。本实施例中，存储介质110与处理器120均位于电子设备中且二者分离设置。然而，应当理解的是，存储介质110也可以是独立于电子设备之外，且可以由处理器120通过总线接口来访问。可替换地，存储介质110也可以集成到处理器120中，例如，可以是高速缓存和/或通用寄存器。

约束运动控制装置130可以理解为上述电子设备，或电子设备的处理器120，也可以理解为独立于上述电子设备或处理器120之外的在电子设备控制下实现上述约束运动控制方法的软件功能模块。

如图9所示，上述约束运动控制装置130可以包括变换矩阵获得模块131、标定模块132、末端位姿获得模块133、更新模块134和控制模块135。下面分别对该约束运动控制装置130的各个功能模块的功能进行详细阐述。

变换矩阵获得模块131，用于针对配置有工具的机器人，获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵；

可以理解，该变换矩阵获得模块131可以用于执行上述步骤S101，关于该变换矩阵获得模块131的详细实现方式可以参照上述对步骤S101有关的内容。

标定模块132，用于标定得到远心不动点，并获得目标位姿信息；

可以理解，该标定模块132可以用于执行上述步骤S102，关于该标定模块132的详细实现方式可以参照上述对步骤S102有关的内容。

末端位姿获得模块133，用于根据所述目标位姿信息及所述变换矩阵得到所述工具的末端位姿；

可以理解，该末端位姿获得模块133可以用于执行上述步骤S103，关于该末端位姿获得模块133的详细实现方式可以参照上述对步骤S103有关的内容。

更新模块134，用于根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新；

可以理解，该更新模块134可以用于执行上述步骤S104，关于该更新模块134的详细实现方式可以参照上述对步骤S104有关的内容。

控制模块135，用于根据更新后的变换矩阵得到机器人各关节的角度位置信息，并基于所述角度位置信息控制实体机器人带动配置的实体工具进行远心不动点约束的运动。

可以理解，该控制模块135可以用于执行上述步骤S105，关于该控制模块135的详细实现方式可以参照上述对步骤S105有关的内容。

在一种可能的实施方式中，所述约束运动控制装置130还包括配置模块，该配置模块可以用于：

在一种可能的实施方式中，上述变换矩阵获得模块131可以用于：

在一种可能的实施方式中，上述标定模块132可以用于：

在一种可能的实施方式中，上述标定模块132还可以用于：

获取摇杆被控制下所产生的偏移量；

在当前位姿信息基础上叠加所述偏移量得到目标位姿信息。

在一种可能的实施方式中，所述变换矩阵包括多个类型的位姿变换向量和多个方向上的位移信息，上述更新模块134可以用于：

在一种可能的实施方式中，上述更新模块134可以用于：

关于装置中的各模块的处理流程、以及各模块之间的交互流程的描述可以参照上述方法实施例中的相关说明，这里不再详述。

进一步地，本申请实施例还提供一种计算机可读存储介质，计算机可读存储介质存储有机器可执行指令，机器可执行指令被执行时实现上述实施例提供的约束运动控制方法。

具体地，该计算机可读存储介质能够为通用的存储介质，如移动磁盘、硬盘等，该计算机可读存储介质上的计算机程序被运行时，能够执行上述约束运动控制方法。关于计算机可读存储介质中的及其可执行指令被运行时，所涉及的过程，可以参照上述方法实施例中的相关说明，这里不再详述。

本申请上述实施例提供的约束运动控制系统中，所述电子设备用于，针对配置有工具的机器人，获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵，标定得到远心不动点，并获得目标位姿信息，根据所述目标位姿信息及所述变换矩阵得到所述工具的末端位姿，根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新，根据更新后的变换矩阵得到机器人各关节的角度位置信息。

所述电子设备还用于，将所述角度位置信息发送至所述控制设备。

所述控制设备用于，根据接收到的角度位置信息控制所述实体机器人和实体工具。

本实施例提供的约束运动控制系统中各设备的处理流程以及各设备之间的交互流程的描述可以参照上述方法实施例中的相关说明，这里不再详述。

综上所述，本申请实施例提供的约束运动控制方法、装置、系统和电子设备，针对配置有工具的机器人，获得工具所在的工具坐标系相对于机器人末端所在的末端坐标系的变换矩阵，并标定远心不动点，获得目标位姿信息。根据目标位姿信息及变换矩阵得到工具的末端位姿，根据工具的末端位姿中的末端位置信息及标定的远心不动点对变换矩阵进行更新。最后基于更新后的变换矩阵得到机器人各关节的角度位置信息，进而控制实体机器人带动配置的实体工具进行远心不动点约束的运动。本方案基于工具末端、机器人末端、远心不动点之间的相对关系实现约束控制，无需配套的机械结构，减少了维护成本并且降低对手术环境污染的可能性，并且还可避免机械结构件之间连接时产生的误差。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。

Claims

1.一种约束运动控制方法，其特征在于，所述方法包括：

针对配置有工具的机器人，获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵，所述变换矩阵为在所述工具坐标系标定的情况下，所述工具坐标系在所述末端坐标系下的表征；

标定得到远心不动点，并获得目标位姿信息，所述目标位姿信息为由机器人的末端的当前位姿信息叠加产生的偏移量所获得的机器人的末端的目标位姿信息；

根据更新后的变换矩阵得到机器人各关节的角度位置信息，并基于所述角度位置信息控制实体机器人带动配置的实体工具进行远心不动点约束的运动，其中，所述机器人各关节的角度位置信息为基于工具的末端位姿及更新后的变换矩阵进行逆运动学计算得到机器人的末端的位姿信息后，根据机器人的末端的位姿信息所得到；

所述标定得到远心不动点的步骤，包括：

在所述机器人处于多种不同姿态下，控制所述工具的末端与设置的远心不动点重合，并记录各种姿态下所述工具的末端的空间笛卡尔坐标数据，根据多种姿态下所述工具的末端的空间笛卡尔坐标数据计算得到所述远心不动点相对于所述机器人的底座所在的基坐标系下的空间笛卡尔坐标，以完成所述远心不动点的标定；

所述变换矩阵包括多个类型的位姿变换向量和多个方向上的位移信息，所述根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新的步骤，包括：

根据所述工具的末端位姿中的末端位置信息及标定的远心不动点的坐标计算得到新的位姿变换向量和新的位移信息，基于所述新的位姿变换向量和新的位移信息对所述变换矩阵进行更新；

其中，计算得到新的位姿变换向量和新的位移信息的步骤，包括：

计算所述工具的末端位姿中的末端位置信息和标定的远心不动点的坐标中同一方向上的坐标值的差值，结合多个方向上的差值计算得到新的位移信息；根据多个方向上的差值构成差值向量，并根据所述差值向量和所述新的位移信息计算得到新的位姿变换向量。

2.根据权利要求1所述的约束运动控制方法，其特征在于，所述方法还包括预先配置工具信息的步骤，该步骤包括以下之一：

3.根据权利要求1所述的约束运动控制方法，其特征在于，所述获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵的步骤，包括：

4.根据权利要求1所述的约束运动控制方法，其特征在于，所述获得目标位姿信息的步骤，包括：

获取摇杆被控制下所产生的偏移量；

在当前位姿信息基础上叠加所述偏移量得到目标位姿信息。

5.一种约束运动控制装置，其特征在于，所述装置包括：

变换矩阵获得模块，用于针对配置有工具的机器人，获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵，所述变换矩阵为在所述工具坐标系标定的情况下，所述工具坐标系在所述末端坐标系下的表征；

标定模块，用于标定得到远心不动点，并获得目标位姿信息，所述目标位姿信息为由机器人的末端的当前位姿信息叠加产生的偏移量所获得的机器人的末端的目标位姿信息；

控制模块，用于根据更新后的变换矩阵得到机器人各关节的角度位置信息，并基于所述角度位置信息控制实体机器人带动配置的实体工具进行远心不动点约束的运动，其中，所述机器人各关节的角度位置信息为基于工具的末端位姿及更新后的变换矩阵进行逆运动学计算得到机器人的末端的位姿信息后，根据机器人的末端的位姿信息所得到；

所述标定模块，用于在所述机器人处于多种不同姿态下，控制所述工具的末端与设置的远心不动点重合，并记录各种姿态下所述工具的末端的空间笛卡尔坐标数据，根据多种姿态下所述工具的末端的空间笛卡尔坐标数据计算得到所述远心不动点相对于所述机器人的底座所在的基坐标系下的空间笛卡尔坐标，以完成所述远心不动点的标定；

所述变换矩阵包括多个类型的位姿变换向量和多个方向上的位移信息，所述更新模块，用于根据所述工具的末端位姿中的末端位置信息及标定的远心不动点的坐标计算得到新的位姿变换向量和新的位移信息，基于所述新的位姿变换向量和新的位移信息对所述变换矩阵进行更新；

所述更新模块，用于计算所述工具的末端位姿中的末端位置信息和标定的远心不动点的坐标中同一方向上的坐标值的差值，结合多个方向上的差值计算得到新的位移信息；根据多个方向上的差值构成差值向量，并根据所述差值向量和所述新的位移信息计算得到新的位姿变换向量。

6.一种电子设备，其特征在于，包括一个或多个存储介质和一个或多个与存储介质通信的处理器，一个或多个存储介质存储有处理器可执行的机器可执行指令，当电子设备运行时，处理器执行所述机器可执行指令，以执行权利要求1-4中任意一项所述的方法。

7.一种约束运动控制系统，其特征在于，包括控制设备、实体机器人、实体工具以及电子设备；

所述电子设备用于，针对配置有工具的机器人，获得所述工具所在的工具坐标系相对于所述机器人末端所在的末端坐标系的变换矩阵，所述变换矩阵为在所述工具坐标系标定的情况下，所述工具坐标系在所述末端坐标系下的表征，标定得到远心不动点，并获得目标位姿信息，根据所述目标位姿信息及所述变换矩阵得到所述工具的末端位姿，所述目标位姿信息为由机器人的末端的当前位姿信息叠加产生的偏移量所获得的机器人的末端的目标位姿信息，根据所述工具的末端位姿中的末端位置信息及标定的远心不动点对所述变换矩阵进行更新，根据更新后的变换矩阵得到机器人各关节的角度位置信息，其中，所述机器人各关节的角度位置信息为基于工具的末端位姿及更新后的变换矩阵进行逆运动学计算得到机器人的末端的位姿信息后，根据机器人的末端的位姿信息所得到；

所述实体机器人用于，在所述控制设备的控制下带动配置的实体工具进行远心不动点约束的运动；

其中，所述电子设备用于通过以下方式标定得到远心不动点：

所述变换矩阵包括多个类型的位姿变换向量和多个方向上的位移信息，所述电子设备用于通过以下方式对所述变换矩阵进行更新：

所述电子设备用于计算所述工具的末端位姿中的末端位置信息和标定的远心不动点的坐标中同一方向上的坐标值的差值，结合多个方向上的差值计算得到新的位移信息；根据多个方向上的差值构成差值向量，并根据所述差值向量和所述新的位移信息计算得到新的位姿变换向量。