CN114074330B - 一种机器人控制方法、系统、机器人及存储介质 - Google Patents

Abstract

本申请公开了一种机器人控制方法、系统、机器人及存储介质，所述方法包括：采集机器人工作时的多组RCM标定数据，并根据RCM标定数据得到基坐标相对信息；根据基坐标相对信息和床体初始位姿数据建立机器人的运动学模型；根据机器人的运动学模型确定操作器械末端和第二标记物的初始相对位姿；采集第二标记物与标记物检测装置的当前相对位姿，并根据第二标记物与标记物检测装置的当前相对位姿控制机械臂围绕RCM点运动，以使操作器械末端和第二标记物的相对位姿与初始相对位姿相同。本申请能够高效且精准度地控制机械臂跟随床体运动。

Description

一种机器人控制方法、系统、机器人及存储介质

技术领域

本申请涉及机器人控制技术领域，特别涉及一种机器人控制方法、系统、机器人及存储介质。

背景技术

随着科技的进步，机器人技术逐渐成熟并被广泛应用。机器人通常包括主操作台及从操作设备，主操作台包括手柄，操作人员通过操作手柄向从操作设备发送控制命令；从操作设备包括机械臂，机械臂远端具有操作臂，操作臂具有末端器械。由于机器人通常具有多个机械臂，在操作过程中可以根据分工不同在不同的机械臂上安装不同的操作器械，如手术刀、手术剪、内窥镜、喷头、电焊等。

为了提高机器人使用过程的安全性、精准度和稳定性，本领域中通常将机械臂的操作对象放置于床体上并进行固定，以使机械臂对床体上的操作对象进行精准的操作。在实际操作过程中，需要通过调整床的姿态已实现对操作对象进行更方便的操作的情况，本领域常规的操作是通过手动调节机械臂位姿，以使机械臂能够与被操作对象进行再次的定位，该方式存在操作耗时长并且精准度低的缺点。

因此，如何高效且精准度地控制机械臂跟随床体运动是本领域技术人员目前需要解决的技术问题。

发明内容

本申请的目的是提供一种机器人控制方法、系统、机器人及存储介质，能够高效且精准度地控制机械臂跟随床体运动。

为解决上述技术问题，本申请提供一种机器人控制方法，所述机器人包括机械臂和台车，所述机械臂的操作器械末端围绕远心不动点（即RCM点）工作，所述台车设置有标记物检测装置，所述机器人控制方法包括：

在所述操作器械末端安装第一标记物，采集所述机器人工作时的多组RCM标定数据，并根据所述RCM标定数据得到基坐标相对信息；其中，所述RCM标定数据包括所述第一标记物和所述标记物检测装置的相对位姿，所述基坐标相对信息为所述标记物检测装置与所述机械臂基坐标系的相对位置或相对位姿；

根据所述基坐标相对信息和床体初始位姿数据建立所述机器人的运动学模型；其中，所述床体初始位姿数据为床体上第二标记物与所述标记物检测装置的相对位姿；

根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物的初始相对位姿；

采集所述第二标记物与所述标记物检测装置的当前相对位姿，并根据所述第二标记物与所述标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，以使所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同。

可选的，所述机器人还包括伸缩臂，所述机械臂安装于所述伸缩臂的一端；

相应的，根据所述RCM标定数据得到基坐标相对信息，包括：

根据所述RCM标定数据和所述伸缩臂的运动数据计算所述基坐标相对信息。

可选的，在采集所述第二标记物与所述标记物检测装置的当前相对位姿之前，还包括：

查询所述机器人的当前跟随模式；

若所述当前跟随模式为床体跟随模式，则进入采集所述第二标记物与所述标记物检测装置的当前相对位姿的步骤；

若所述当前跟随模式为机械臂跟随模式，则控制所述床体跟随所述机械臂运动。

可选的，控制所述床体跟随所述机械臂运动，包括：

采集所述操作器械末端与所述标记物检测装置的当前相对位姿，并根据所述操作器械末端与所述标记物检测装置的当前相对位姿控制所述床体运动，以使所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同。

可选的，根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物的初始相对位姿，包括：

根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物在所述机械臂基坐标系内的初始相对位姿；

或，根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物在所述标记物检测装置的坐标系内的初始相对位姿。

可选的，根据所述第二标记物与所述标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，包括：

根据所述第二标记物与所述标记物检测装置的当前相对位姿确定所述第二标记物的位姿变化量；

根据所述位姿变化量确定所述操作器械末端的目标位姿；其中，当所述操作器械末端处于目标位姿时，所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同；

根据所述操作器械末端的当前位姿和目标位姿规划移动路径，并按照所述移动路径控制所述机械臂围绕RCM点运动，以使所述操作器械末端变换至所述目标位姿。

可选的，所述标记物检测装置为光学跟踪仪，所述第一标记物和所述第二标记物为光学标记物。

本申请还提供了一种机器人控制系统，所述机器人包括机械臂和台车，所述机械臂的操作器械末端围绕RCM点工作，所述台车设置有标记物检测装置，所述机器人控制系统包括：

标定模块，用于在所述操作器械末端安装第一标记物，采集所述机器人工作时的多组RCM标定数据，并根据所述RCM标定数据得到基坐标相对信息；其中，所述RCM标定数据包括所述第一标记物和所述标记物检测装置的相对位姿，所述基坐标相对信息为所述标记物检测装置与所述机械臂基坐标系的相对位置或相对位姿；

模型构建模块，用于根据所述基坐标相对信息和床体初始位姿数据建立所述机器人的运动学模型；其中，所述床体初始位姿数据为床体上第二标记物与所述标记物检测装置的相对位姿；

初始相对位姿确定模块，用于根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物的初始相对位姿；

床体跟随模块，用于采集所述第二标记物与所述标记物检测装置的当前相对位姿，并根据所述第二标记物与所述标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，以使所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同。

本申请还提供了一种存储介质，其上存储有计算机程序，所述计算机程序执行时实现上述机器人控制方法执行的步骤。

本申请还提供了一种机器人，包括机械臂、台车、存储器和处理器，所述机械臂的操作器械末端围绕RCM点工作，所述台车设置有标记物检测装置，所述存储器中存储有计算机程序，所述处理器调用所述存储器中的计算机程序时实现上述机器人控制方法的步骤。

本申请提供了一种机器人控制方法，所述机器人包括机械臂和台车，所述机械臂的操作器械末端围绕RCM点工作，所述台车设置有标记物检测装置，所述机器人控制方法包括：在所述操作器械末端安装第一标记物，采集所述机器人工作时的多组RCM标定数据，并根据所述RCM标定数据得到基坐标相对信息；其中，所述RCM标定数据包括所述第一标记物和所述标记物检测装置的相对位姿，所述基坐标相对信息为所述标记物检测装置与所述机械臂基坐标系的相对位置或相对位姿；根据所述基坐标相对信息和床体初始位姿数据建立所述机器人的运动学模型；其中，所述床体初始位姿数据为床体上第二标记物与所述标记物检测装置的相对位姿；根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物的初始相对位姿；采集所述第二标记物与所述标记物检测装置的当前相对位姿，并根据所述第二标记物与所述标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，以使所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同。

本申请提供的机器人包括机械臂和台车，台车上设置有标记物检测装置，标记物检测装置能够检测设置于机械臂操作器械末端的第一标记物以及设置于床体的第二标记物。本申请采集机器人工作时第一标记物和标记物检测装置的相对位姿可以确定基坐标相对信息，即：标记物检测装置与所述机械臂基坐标系的相对位置或相对位姿。本申请根据基坐标相对信息和床体初始位姿数据建立所述机器人的运动学模型，进而基于运动学模型确定操作器械末端和第二标记物的初始相对位姿。本申请可以实时采集第二标记物与标记物检测装置的当前相对位姿，根据该当前相对位姿可以确定床体与机械臂之间的相对位姿变化。因此本申请根据第二标记物与标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，以使操作器械末端和第二标记物的相对位姿保持为初始相对位姿，实现机械臂对床体的跟随控制。本申请的机械人控制过程无需人工参与，能够高效且精准度地控制机械臂跟随床体运动。本申请同时还提供了一种机器人控制系统、一种存储介质和一种机器人，具有上述有益效果，在此不再赘述。

附图说明

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

图1为本申请实施例所提供的一种机器人控制方法的流程图；

图2为本申请实施例所提供的一种光学跟踪仪NDI和机械臂UR统一坐标系后的示意图；

图3为本申请实施例所提供的一种机器人控制系统的结构示意图。

具体实施方式

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

下面请参见图1，图1为本申请实施例所提供的一种机器人控制方法的流程图。

具体步骤可以包括：

S101：在所述操作器械末端安装第一标记物，采集所述机器人工作时的多组RCM标定数据，并根据所述RCM标定数据得到基坐标相对信息；

其中，本实施例可以应用于手术机器人、喷漆机器人、零件装配机器人等机器人，该机器人包括机械臂和台车，机械臂的末端安装有操作器械，操作器械可以对设置于床体上的被操作对象进行操作。在机械臂工作过程中，围绕RCM（Remote Center of Motion，远程运动中心）点运动。机械臂的操作器械末端（即RCM点）设置有第一标记物，所述台车设置有标记物检测装置，标记物检测装置可以检测第一标记物相对于自身的相对位姿或相对位置。

本步骤可以控制机器人的机械臂移动，以便采集多组RCM标定数据。上述RCM标定数据为操作器械末端的第一标记物相对与标记物检测装置的相对位姿，通过上述RCM标定数据可以计算得到基坐标相对信息。上述基坐标相对信息为标记物检测装置与机械臂基坐标系的相对位置或相对位姿。其中，本实施例中的第一标记物可以仅在标定时使用，标定后就可以不对第一标记物的位姿进行检测。标定完成之后，通过监测床体上标记物变化，根据建立的运动学模型，即可得到器械末端期望的位姿。

作为一种可行的实施方式，上述机器人还可以包括伸缩臂，机械臂安装于所述伸缩臂的一端，机械臂可以随伸缩臂进行水平方向运动或竖直方向运动。相应的，本步骤可以根据所述RCM标定数据和所述伸缩臂的运动数据计算所述基坐标相对信息。

S102：根据基坐标相对信息和床体初始位姿数据建立机器人的运动学模型；

其中，在本步骤之前在可以存在利用标记物检测装置获取床体初始位姿数据的操作，上述床体初始位姿数据为床体上第二标记物与所述标记物检测装置的相对位姿。本实施例可以根据基坐标相对信息和床体初始位姿数据建立机器人的运动学模型。上述运动学模型包括机器人、安装于机器人台车任意位置的标记物检测装置、床体、安装于床体任意位置的第二标记物。本实施例还可以根据需要增加虚拟的人体模型（RCM点），还包括RCM与机械臂（UR）的基坐标的相对位姿关系、NDI与机械臂(UR)的基坐标的相对位置关系、NDI与RCM点的相对位姿关系、NDI与Mark1的相对位姿关系、RCM点与Mark1的相对位姿关系。上述运动学模型可以包括机器人所在场地内的各装置的3D模型，且各3D模型之间的相对位姿关系与真实场景相同。

作为一种可行的实施方式，标记物检测装置可以为光学跟踪仪、照相机或其他传感器，相应的第一标记物和第二标记物为光学标记物。

S103：根据机器人的运动学模型确定操作器械末端和第二标记物的初始相对位姿；

其中，在得到机器人的运动学模型之后，本实施例可以利用该模型得到操作器械末端和第二标记物的初始相对位姿。RCM点为机械臂的操作器械插入操作对象内部的开孔位置。为了避免床体运动带来的开孔位置变化，本实施例确定操作器械末端和第二标记物的初始相对位姿，该初始相对位姿为操作器械末端和第二标记物需要始终保持的相对位姿。

作为一种可行的实施方式，本实施例可以在多个坐标系内确定初始相对位姿，至少可以通过以下两种方式确定初始相对位姿：

方式1：根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物在所述机械臂基坐标系内的初始相对位姿；

方式2：根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物在所述标记物检测装置的坐标系内的初始相对位姿。

本实施例可以通过上述方式一和方式二实现坐标相对信息统一，即：标记物检测装置与机械臂基坐标、床上标记物与操作器械末端全部能统一到同一坐标系内。利用标定的统一坐标系，在机器人工作时只需要采集床上标记物的数据，即可实现机械臂跟随床运动。进一步，本实施例也可以在操作器械末端安装标记物，只需要检测器械末端的标记物，实现床体跟随机械臂运动。

S104：采集第二标记物与标记物检测装置的当前相对位姿，并根据第二标记物与标记物检测装置的当前相对位姿控制机械臂围绕RCM点运动，以使操作器械末端和第二标记物的相对位姿与初始相对位姿相同。

其中，在确定操作器械末端和第二标记物的初始相对位姿之后，本实施例可以实时采集第二标记物与标记物检测装置的当前相对位姿。若当前相对位姿发生变化，本实施例可以将初始相对位姿为参照、根据第二标记物与标记物检测装置的当前相对位姿控制机械臂围绕RCM点运动，以使操作器械末端和第二标记物的相对位姿与初始相对位姿相同。

本实施例提供的机器人包括机械臂和台车，台车上设置有标记物检测装置，标记物检测装置能够检测设置于机械臂操作器械末端的第一标记物以及设置于床体的第二标记物。本实施例采集机器人工作时第一标记物和标记物检测装置的相对位姿可以确定基坐标相对信息，即：标记物检测装置与所述机械臂基坐标系的相对位置或相对位姿。本实施例根据基坐标相对信息和床体初始位姿数据建立所述机器人的运动学模型，进而基于运动学模型确定操作器械末端和第二标记物的初始相对位姿。本实施例可以实时采集第二标记物与标记物检测装置的当前相对位姿，根据该当前相对位姿可以确定床体与机械臂之间的相对位姿变化。因此本实施例根据第二标记物与标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，以使操作器械末端和第二标记物的相对位姿保持为初始相对位姿，实现机械臂对床体的跟随控制。本实施例的机械人控制过程无需人工参与，能够高效且精准度地控制机械臂跟随床体运动。

图1对应的实施例提供了因床体位姿变化，控制机械臂跟随床体运动的方案。当机器臂因震动后人员误操作发生位姿改变时，可以控制床体跟随机械臂运动。因此，在S104采集所述第二标记物与所述标记物检测装置的当前相对位姿之前，还可以查询所述机器人的当前跟随模式，上述当前跟随模式可以包括床体跟随模式和机械臂跟随模式。在床体跟随模式下，机械臂的RCM点跟随床体远动；在机械臂跟随模式下，床体跟随机械臂的RCM点运动。若所述当前跟随模式为床体跟随模式，则进入S104采集所述第二标记物与所述标记物检测装置的当前相对位姿的步骤；若所述当前跟随模式为机械臂跟随模式，则控制所述床体跟随所述机械臂运动。

作为一种可行的实施方式，本实施例可以通过以下方式控制床体跟随所述机械臂运动：采集所述操作器械末端与所述标记物检测装置的当前相对位姿，并根据所述操作器械末端与所述标记物检测装置的当前相对位姿控制所述床体运动，以使所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同。

作为对于图1对应实施例的进一步介绍，S104中控制所述机械臂围绕RCM点运动的过程可以包括以下过程：根据所述第二标记物与所述标记物检测装置的当前相对位姿确定所述第二标记物的位姿变化量；根据所述位姿变化量确定所述操作器械末端的目标位姿；根据所述操作器械末端的当前位姿和目标位姿规划移动路径，并按照所述移动路径控制所述机械臂围绕RCM点运动，以使所述操作器械末端变换至所述目标位姿。当所述操作器械末端处于目标位姿时，所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同。

下面通过在实际应用中的手术机器人的实施例说明上文描述的流程。

在腹腔镜微创手术中，机械臂控制微创手术器械通过穿刺器进入病人体内进行手术，当医生需要通过调整手术床改变患者体位从而方便进行手术操作时，患者体内的手术器械需要随着手术床的姿态而进行同步调整。本领域通常采用运动学控制和传感测量相结合的方式，通过测量手术床运动过程中姿态变化，利用正运动学和逆运动学计算机械臂的关节变化，从而实现机械臂跟随床体的运动，避免在调整手术床时，手术器械对患者造成伤害。该技术方案中床体和机械臂的初始定位精度，直接影响跟随运动的精度。

为了提高机械臂的床体跟随精度，本申请提供一种多机械臂联合运动的控制方案，能够使机械臂跟随床体具有更精准的起始位置，提高跟随运动的精度。本实施例中在床体上设置光学标记物Mark1，在手术器械末端上设置光学标记物Mark2，机器人的台车上设置有检测光学标记物的光学跟踪仪NDI，本实施例可以包括以下步骤：

步骤1：采集多组机械臂上器械末端移动时光学标记物Mark2相对与光学跟踪仪（NDI）的相对位姿变化，并计算得到光学跟踪仪NDI与机械臂UR的基坐标的相对位姿。

其中，本实施例中的手术机器人可以包括手术台车、安装于手术台车上的机械臂，以及用于连接机械臂与器械的滑台，所述滑台安装于机械臂的末端，所述器械安装于所述滑台上。所述机械臂安装于手术台车的位置即为机械臂的基坐标位置，所述机械臂的末端相对于机械臂的基坐标的相对位姿通过机械臂的关节参数即可计算得到，安装于滑台上的器械末端与机械臂的末端的相对位姿通过电机参数也可以计算得到，从而得到器械末端相对机械臂的基坐标的位姿。

本实施例可以在机器人台车任意位置按照光学跟踪仪NDI，还可以在安装机械臂上的滑台上的器械末端(RCM点)的安装位置安装光学标记物Mark2，通过光学跟踪仪NDI采集多组数据，可以不围绕RCM点移动机械臂（即机械臂可以不受限制的运动，在不影响NDI采集Mark2数据的情况下），通过光学跟踪仪NDI采集光学标记物Mark2随着机械臂移动的姿态变化的相对姿态变化数据。机械臂UR的基坐标相对光学跟踪仪NDI是固定的，因此可以计算出机械臂UR的基坐标与光学跟踪仪NDI的相对位置，从而标定机械臂UR的基坐标。

本实施例中安装于滑动上的器械指“戳卡”，器械末端是指手术器械通过戳卡进入人体内的位置，即RCM点位置。如果仅仅是标记光学跟踪仪NDI与机械臂的基坐标的相对位置，可以不需要使用RCM点相对机械臂的基坐标和光学跟踪仪NDI的相对位姿的关系，通过采集一次Mark2的相对位姿数据即可完成。通过本步骤可以统一光学跟踪仪NDI与机械臂UR基坐标的坐标系。请参见图2，图2为本申请实施例所提供的一种光学跟踪仪NDI和机械臂UR统一坐标系后的示意图，图2中所示的坐标系为机械臂UR的基坐标系，RCM1为移动前RCM的位置，RCM1’为移动后RCM的位置，MARK1为移动前床体标记点的位置，MARK1’为移动后床体标记点的位置。

其中，手术机器人至少存在机械臂UR基坐标固定不变和机械臂UR基坐标可移动的两种结构。若机械臂的基坐标不固定，即手术机器人还包括伸缩臂，所述机械臂安装在所述伸缩臂一端上，伸缩臂可在手术机器人台车上水平/垂直方向移动。本步骤可以获取用于控制伸缩臂水平/竖直方向移动的伸缩臂的移动数据，通过计算得到机械臂移动后的相对机械臂UR的基坐标移动前的相对位置变化，通过转换公式得到移动后的机械臂UR基坐标的位置。从而实现通过光学跟踪仪NDI监测机械臂UR的基坐标变化，实现不管机械臂基坐标是否变化，均可实现床对机械臂的跟随。

机械臂UR的基坐标移动前后的转化公式为如下：

以上公式中

为机械臂基坐标系在竖直滑台和水平滑台在初始化位姿坐标系相对于竖直滑台和水平滑台运动后的机械臂基坐标系相对位姿，

为水平滑台相对于初始位置的量程值，单位为mm；

为竖直滑台相对于初始位置的量程值，单位为mm。

步骤2：根据步骤1标定的光学跟踪仪NDI与机械臂UR的相对位置/位姿建立运动学模型，通过运动学模型计算确定初始的床体Mark1和器械末端（RCM）Mark2的初始相对位姿。

本步骤根据标定出的光学跟踪仪NDI与机械臂UR的基坐标的相对位置，建立运动学模型。上述运动学模型包括机器人、安装于机器人台车任意位置的光学跟踪仪NDI、手术床、安装于手术床任意位置的光学标记物Mark1，还可以根据需要增加虚拟的人体模型RCM点，还包括RCM与机械臂UR的基坐标的相对位姿关系、光学跟踪仪NDI与机械臂UR的基坐标的相对位置关系、NDI与RCM点的相对位姿关系、光学跟踪仪NDI与Mark1的相对位姿关系、RCM点与Mark1的相对位姿关系。通过光学跟踪仪NDI监测Mark1的起始姿态数据（即床起始姿态），得到Mark1与光学跟踪仪NDI的起始相对位姿，从而在运动学模型中，通过计算得到Mark1与RCM的初始相对位姿（即床体与器械末端的初始相对位姿）。所述安装于机械臂上的滑台上的手术器械的安装位置的光学标记物与器械安装效果等同（即光学标记物Mark2相对机械臂的基坐标位姿，与器械末端相对机械臂的基坐标位姿相同）。本实施例采用该方式标定Mark1与RCM点的初始相对位姿，无论RCM点在那个位姿，都可以准确的标定出Mark1与RCM点的初始相对位姿。

步骤3：根据步骤1和步骤2中得到的任一项数据，实时采集光学跟踪仪NDI相对Mark1的相对位姿，计算得到Mark1实时姿态变化后，RCM点需要达到的目标位姿RCM1（即RCM1相对于光学跟踪仪NDI的相对位姿）。

作为一种可行的实施方式，本实施例可以根据步骤1标定出的光学跟踪仪NDI相对于机械臂UR的基坐标的相对位置，通过实时采集获取Mark1移动时相对于光学跟踪仪NDI的实时相对姿态，通过计算获取Mark1与机械臂UR基坐标的实时相对位姿，从而计算得到RCM需要保持RCM与Mark2相对位姿不变的时随Mark2得到的目标位姿RCM1。

获取Mark1与机械臂UR基坐标的相对位姿的计算公式为：

光学跟踪仪NDI1相对UR基坐标系相对位姿（通过步骤1已标定出），符号为

；MARK1点到UR基坐标系相对位姿的符号为

；光学跟踪仪NDI1与MARK1的相对位姿（实时获取）的符号为

。

作为另一种可行的实施方式，根据在步骤2中已确定RCM点与Mark1的初始相对位姿关系，同时床-机械臂整个随动过程中RCM点与Mark1的相对位姿关系一致保持不变，实时采集NDI相对Mark1的相对位姿，RCM随Mark1位姿变化的实时位姿变化相对NDI的相对位姿，从而得到Mark1移动后对应的RCM1新的位姿。

通过Mark1与NDI实时相对位姿数据，计算得到目标位姿RCM1相对于Mark1移动后的相对位姿，公式为：

。

其中，

表示新的RCM1位姿相对于新的Mark1的相对位姿（在步骤2中，已确定RCM点与Mark1的初始相对位姿关系，在整个过程中，需要保持二者的相对位姿不变）；

表示机械臂UR的基坐标移动后相对于Mark1的相对位姿；

表示机械臂UR的基坐标移动后相对于新的RCM1位姿的相对位姿。

本步骤还可以你想运用：实时采集NDI相对RCM(即在器械末端安装Mark)位姿变化后的相对位姿，计算得到RCM位姿变化后，床上的Mark1需要达到的目标位姿。

本实施例计算得到RCM相对机械臂(UR)的基坐标的相对位姿，从而计算得到Mark1随RCM移动需要达到的目标位姿，标定RCM和Mark1的初始相对位姿关系。

步骤4：根据RCM的实时目标位姿RCM1，生成控制机械臂围绕RCM点运动的实时控制指令。

本步骤根据步骤3获得RCM目标位姿RCM1，通过机械臂各关数据和滑台电机数据，得到控制机械臂围绕RCM点运动的实时控制指令。

本实施例建立“手术床与机械臂”3D模型及统一的坐标系，根据数据采集装置“光学跟踪仪与光学标记物组成”的姿态变化，使得安装于机械臂滑台上手术器械的末端与光学标记物的姿态同等变化，从而实现机械臂围绕远心不动点(RCM点) 跟随床体一起运动。通过确定光学跟踪仪（NDI）坐标系和UR机械臂坐标系，通过转换将两个坐标系统一到一个坐标系中并完成3D建模。利用光学跟踪仪(NDI)实时采集光学标记物（Mark1）的实时姿态变化数据，通过3D模型实时测算并模拟安装于机械臂滑台上手术器械末端的姿态变化，生成控制手术器械末端围绕RCM点运动的机械臂移动控制指令，以实现机械臂跟随床体上光学标记物(Mark1)的姿态变化实时围绕RCM点运动。

本实施例标定NDI与机械臂基坐标的相对位置，机械臂可以不受约束的任意移动。在床随动过程中，机械臂UR和RCM点可能不止一个，避免多次标定，以便于医生还可以在手术过程中，增加RCM点（手术开孔），仍能准确的确定出Mark1与多个RCM点的相对位姿关系，从而实现控制多条机械臂能同时跟随床的姿态变化而同时围绕多个RCM点运动，不会受其他因素影响。比如还可以在每个器械末端安装光学标记物Mark，同时通过NDI实时监测每条机械臂上Mark2与床上Mark1的姿态变化，从而确定多个Mark2与Mark1的相对位姿，从而确定多个RCM点与Mark1的位姿。但该方案容易因为存在多个机械臂同时运动而遮挡多个Mark2中的任意一个及以上Mark2，影响NDI采集数据，从而影响床随动的正常运行。本方案光学标记物是随意安装于床体上，通过标定获得初始的相对位置及数据，相较于现有技术，不需要通过复杂的摆位过程实现精确的初始摆位。本方案的RCM点的起始坐标位置相对床体是任意的，通过标定获得初始的相对位置及数据，相较于现有技术，不需要人体姿态与床之间有非常高的精度适配。本方案通过标定获得的相对位置精度比传统的测量更高，则机械臂跟随床体的运动控制精度越高。

请参见图3，图3为本申请实施例所提供的一种机器人控制系统的结构示意图；机器人包括机械臂和台车，所述机械臂的操作器械末端围绕RCM点工作，所述台车设置有标记物检测装置，所述机器人控制系统包括：

标定模块301，用于在所述操作器械末端安装第一标记物，采集所述机器人工作时的多组RCM标定数据，并根据所述RCM标定数据得到基坐标相对信息；其中，所述RCM标定数据包括所述第一标记物和所述标记物检测装置的相对位姿，所述基坐标相对信息为所述标记物检测装置与所述机械臂基坐标系的相对位置或相对位姿；

模型构建模块302，用于根据所述基坐标相对信息和床体初始位姿数据建立所述机器人的运动学模型；其中，所述床体初始位姿数据为床体上第二标记物与所述标记物检测装置的相对位姿；

初始相对位姿确定模块303，用于根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物的初始相对位姿；

床体跟随模块304，用于采集所述第二标记物与所述标记物检测装置的当前相对位姿，并根据所述第二标记物与所述标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，以使所操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同。

本实施例提供的机器人包括机械臂和台车，台车上设置有标记物检测装置，标记物检测装置能够检测设置于机械臂操作器械末端的第一标记物以及设置于床体的第二标记物。本实施例采集机器人工作时第一标记物和标记物检测装置的相对位姿可以确定基坐标相对信息，即：标记物检测装置与所述机械臂基坐标系的相对位置或相对位姿。本实施例根据基坐标相对信息和床体初始位姿数据建立所述机器人的运动学模型，进而基于运动学模型确定操作器械末端和第二标记物的初始相对位姿。本实施例可以实时采集第二标记物与标记物检测装置的当前相对位姿，根据该当前相对位姿可以确定床体与机械臂之间的相对位姿变化。因此本实施例根据第二标记物与标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，以使操作器械末端和第二标记物的相对位姿保持为初始相对位姿，实现机械臂对床体的跟随控制。本实施例的机械人控制过程无需人工参与，能够高效且精准度地控制机械臂跟随床体运动。

进一步的，所述机器人还包括伸缩臂，所述机械臂安装于所述伸缩臂的一端；

相应的，标定模块301用于根据所述RCM标定数据和所述伸缩臂的运动数据计算所述基坐标相对信息。

进一步的，还包括：

模式查询模块，用于在采集所述第二标记物与所述标记物检测装置的当前相对位姿之前，查询所述机器人的当前跟随模式；

第一模式选择模块，用于若所述当前跟随模式为床体跟随模式，则进入采集所述第二标记物与所述标记物检测装置的当前相对位姿的步骤；

第二模式选择模块，用于若所述当前跟随模式为机械臂跟随模式，则控制所述床体跟随所述机械臂运动。

进一步的，第二模式选择模块，用于采集所述操作器械末端与所述标记物检测装置的当前相对位姿，并根据所述操作器械末端与所述标记物检测装置的当前相对位姿控制所述床体运动，以使所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同。

进一步的，初始相对位姿确定模块303包括：

第一确定单元，用于根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物在所述机械臂基坐标系内的初始相对位姿；

或，第二确定单元，用于根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物在所述标记物检测装置的坐标系内的初始相对位姿。

进一步的，床体跟随模块304用于根据所述第二标记物与所述标记物检测装置的当前相对位姿确定所述第二标记物的位姿变化量；还用于根据所述位姿变化量确定所述操作器械末端的目标位姿；其中，当所述操作器械末端处于目标位姿时，所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同；还用于根据所述操作器械末端的当前位姿和目标位姿规划移动路径，并按照所述移动路径控制所述机械臂围绕RCM点运动，以使所述操作器械末端变换至所述目标位姿。

进一步的，所述标记物检测装置为光学跟踪仪，所述第一标记物和所述第二标记物为光学标记物。

由于系统部分的实施例与方法部分的实施例相互对应，因此系统部分的实施例请参见方法部分的实施例的描述，这里暂不赘述。

本申请还提供了一种存储介质，其上存有计算机程序，该计算机程序被执行时可以实现上述实施例所提供的步骤。该存储介质可以包括：U盘、移动硬盘、只读存储器（Read-Only Memory ，ROM）、随机存取存储器（Random Access Memory ，RAM）、磁碟或者光盘等各种可以存储程序代码的介质。

本申请还提供了一种机器人，包括机械臂、台车、存储器和处理器，所述机械臂的操作器械末端围绕RCM点工作，所述台车设置有标记物检测装置，所述存储器中存储有计算机程序，所述处理器调用所述存储器中的计算机程序时实现上述机器人控制方法的步骤。

说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以对本申请进行若干改进和修饰，这些改进和修饰也落入本申请权利要求的保护范围内。

还需要说明的是，在本说明书中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的状况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

Claims (9)

1.一种机器人控制方法，其特征在于，所述机器人包括机械臂和台车，所述机械臂的操作器械末端围绕RCM点工作，所述台车设置有标记物检测装置，所述机器人控制方法包括：

在所述操作器械末端安装第一标记物，采集所述机器人工作时的多组RCM标定数据，并根据所述RCM标定数据得到基坐标相对信息；其中，所述RCM标定数据包括所述第一标记物和所述标记物检测装置的相对位姿，所述基坐标相对信息为所述标记物检测装置与所述机械臂基坐标系的相对位置或相对位姿；

根据所述基坐标相对信息和床体初始位姿数据建立所述机器人的运动学模型；其中，所述床体初始位姿数据为床体上第二标记物与所述标记物检测装置的相对位姿；其中，所述机器人的运动学模型包括机器人所在场地内的各装置的3D模型，以及所述3D模型之间的相对位姿关系；

根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物的初始相对位姿；

采集所述第二标记物与所述标记物检测装置的当前相对位姿，并根据所述第二标记物与所述标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，以使所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同；

其中，在采集所述第二标记物与所述标记物检测装置的当前相对位姿之前，还包括：

判断所述机器人的当前跟随模式；

若所述当前跟随模式为床体跟随模式，则进入采集所述第二标记物与所述标记物检测装置的当前相对位姿的步骤；

若所述当前跟随模式为机械臂跟随模式，则控制所述床体跟随所述机械臂运动。

2.根据权利要求1所述机器人控制方法，其特征在于，所述机器人还包括伸缩臂，所述机械臂安装于所述伸缩臂的一端；

相应的，根据所述RCM标定数据得到基坐标相对信息，包括：

根据所述RCM标定数据和所述伸缩臂的运动数据计算所述基坐标相对信息。

3.根据权利要求1所述机器人控制方法，其特征在于，控制所述床体跟随所述机械臂运动，包括：

采集所述操作器械末端与所述标记物检测装置的当前相对位姿，并根据所述操作器械末端与所述标记物检测装置的当前相对位姿控制所述床体运动，以使所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同。

4.根据权利要求1所述机器人控制方法，其特征在于，根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物的初始相对位姿，包括：

根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物在所述机械臂基坐标系内的初始相对位姿；

或，根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物在所述标记物检测装置的坐标系内的初始相对位姿。

5.根据权利要求1所述机器人控制方法，其特征在于，根据所述第二标记物与所述标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，包括：

根据所述第二标记物与所述标记物检测装置的当前相对位姿确定所述第二标记物的位姿变化量；

根据所述位姿变化量确定所述操作器械末端的目标位姿；其中，当所述操作器械末端处于目标位姿时，所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同；

根据所述操作器械末端的当前位姿和目标位姿规划移动路径，并按照所述移动路径控制所述机械臂围绕RCM点运动，以使所述操作器械末端变换至所述目标位姿。

6.根据权利要求1所述机器人控制方法，其特征在于，所述标记物检测装置为光学跟踪仪，所述第一标记物和所述第二标记物为光学标记物。

7.一种机器人控制系统，其特征在于，所述机器人包括机械臂和台车，所述机械臂的操作器械末端围绕RCM点工作，所述台车设置有标记物检测装置，所述机器人控制系统包括：

标定模块，用于在所述操作器械末端安装第一标记物，采集所述机器人工作时的多组RCM标定数据，并根据所述RCM标定数据得到基坐标相对信息；其中，所述RCM标定数据包括所述第一标记物和所述标记物检测装置的相对位姿，所述基坐标相对信息为所述标记物检测装置与所述机械臂基坐标系的相对位置或相对位姿；

模型构建模块，用于根据所述基坐标相对信息和床体初始位姿数据建立所述机器人的运动学模型；其中，所述床体初始位姿数据为床体上第二标记物与所述标记物检测装置的相对位姿；其中，所述机器人的运动学模型包括机器人所在场地内的各装置的3D模型，以及所述3D模型之间的相对位姿关系；

初始相对位姿确定模块，用于根据所述机器人的运动学模型确定所述操作器械末端和所述第二标记物的初始相对位姿；

床体跟随模块，用于采集所述第二标记物与所述标记物检测装置的当前相对位姿，并根据所述第二标记物与所述标记物检测装置的当前相对位姿控制所述机械臂围绕RCM点运动，以使所述操作器械末端和所述第二标记物的相对位姿与所述初始相对位姿相同；

模式查询模块，用于在采集所述第二标记物与所述标记物检测装置的当前相对位姿之前，查询所述机器人的当前跟随模式；

第一模式选择模块，用于若所述当前跟随模式为床体跟随模式，则进入采集所述第二标记物与所述标记物检测装置的当前相对位姿的步骤；

第二模式选择模块，用于若所述当前跟随模式为机械臂跟随模式，则控制所述床体跟随所述机械臂运动。

8.一种机器人，其特征在于，包括机械臂、台车、存储器和处理器，所述机械臂的操作器械末端围绕RCM点工作，所述台车设置有标记物检测装置，所述存储器中存储有计算机程序，所述处理器调用所述存储器中的计算机程序时实现如权利要求1至6任一项所述机器人控制方法的步骤。

9.一种存储介质，其特征在于，所述存储介质中存储有计算机可执行指令，所述计算机可执行指令被处理器加载并执行时，实现如权利要求1至6任一项所述机器人控制方法的步骤。

Images