CN114521962B

CN114521962B - 手术机器人轨迹跟踪方法、装置、机器人及存储介质

Info

Publication number: CN114521962B
Application number: CN202210432786.9A
Authority: CN
Inventors: 钱坤; 黄志俊; 刘金勇; 陈鹏
Original assignee: Lancet Robotics Co Ltd
Current assignee: Lancet Robotics Co Ltd
Priority date: 2022-04-24
Filing date: 2022-04-24
Publication date: 2022-12-16
Anticipated expiration: 2042-04-24
Also published as: CN114521962A

Abstract

本发明实施例公开了手术机器人轨迹跟踪方法、装置、机器人及存储介质，该方法包括：记录在拖动示教过程中手术机器人各个关节的姿态数据；获取手术机器人末端工具在世界坐标系中的坐标，根据患者口腔中的光学定位器，将末端工具在世界坐标系中的坐标转换成口腔图像坐标系中的坐标；当患者位置发生变动时，记录在位置发生变动前手术机器人末端在图像坐标系中的历史坐标，以及在患者位置变动后，手术机器人末端在图像坐标系中的当前坐标，计算历史坐标到当前坐标之间的转换矩阵；根据转换矩阵，更新姿态数据，并根据更新后的姿态数据进行手术操作。使得机器人可以适应患者位置的变化，不会过于死板僵硬。

Description

手术机器人轨迹跟踪方法、装置、机器人及存储介质

技术领域

本发明涉及手术机器人控制领域，尤其涉及一种手术机器人轨迹跟踪方法、装置、机器人及存储介质。

背景技术

在种植牙手术过程中，医生需要逐级备孔，多次打钻植入位置以及进行种植牙植入。在此过程中，机械臂需要多次移动到患者口腔内部，现有的机械臂在经过示教或者术前规划后，只能对当前一种手术场景进行应对，当机械臂脱离了手术区，患者的光学定位器脱离了口腔时，可能会产生患者位置的变动，使得之前示教的数据无法应用于变动位置后的患者，为此需要重新示教，加大了手术的繁琐步骤，延长了手术时间，无论给患者还是医生都带来了极大的不便。

发明内容

有鉴于此，本申请提供一种手术机器人轨迹跟踪方法，包括：

记录在拖动示教过程中手术机器人各个关节的姿态数据；

获取所述手术机器人的末端工具在世界坐标系中的坐标，根据患者口腔中的光学定位器，将所述末端工具在世界坐标系中的坐标转换成口腔图像坐标系中的坐标；

当患者位置发生变动时，获取在位置发生变动前所述手术机器人的末端工具在所述口腔图像坐标系中的历史坐标，以及在患者位置变动后，所述手术机器人末端在所述口腔图像坐标系中的当前坐标，计算所述历史坐标到当前坐标之间的转换矩阵；

根据所述转换矩阵，更新所述姿态数据，并根据更新后的姿态数据进行手术操作。

进一步的，所述记录在所述拖动示教过程中所述手术机器人各个关节产生的姿态数据后，还包括：

将所述姿态数据转换成所述手术机器人末端工具在笛卡尔坐标系中的坐标点云数据，其中，所述笛卡尔坐标系以所述手术机器人的基座为原点建立；

根据所述转换矩阵，更新所述姿态数据包括：

根据所述转换矩阵，更新所述空间坐标点云，得到更新后的空间坐标点云，所述更新后的空间坐标点云作为更新后的姿态数据以进行所述手术操作。

进一步的，将所述姿态数据转换成所述手术机器人末端工具在笛卡尔坐标系中的坐标点云数据包括：

将所述手术机器人各个关节的运动数据转化成所述笛卡尔坐标系的空间坐标数据，并计算各个关节之间的平移变化矩阵；

根据各个关节之间的所述平移变化矩阵计算得到所述手术机器人末端工具的坐标点云数据。

进一步的，所述更新后的坐标点云计算公式为：

式中

为更新后的坐标点云数据，

为更新前的点云数据，

为所述转换矩阵。

进一步的，根据患者口腔中的光学定位器，将所述末端工具在世界坐标系中的位置转换成图像坐标系中的位置包括：

通过所述摄像头拍摄患者口腔中的光学定位器，确定所述患者在所述世界坐标系的位置；

计算所述世界坐标系转换到所述口腔图像坐标系的转换矩阵；

根据所述转换矩阵，将位于世界坐标系的所述末端工具的位置坐标转换到所述口腔图像坐标系下。

进一步的，所述转换矩阵计算公式为：

式中

为所述转换矩阵，

为所述手术机器人末端工具在图像坐标系下的历史坐标的逆矩阵，

为所述手术机器人末端在所述口腔图像坐标系中的当前坐标。

进一步的，所述记录在拖动示教过程中手术机器人各个关节的姿态数据包括：

以预设时间为周期，周期性记录各个关节的姿态数据。

进一步的，本申请实施例还提供一种手术机器人轨迹跟踪装置，包括：

记录模块，用于记录在拖动示教过程中手术机器人各个关节的姿态数据；

转换模块，用于获取所述手术机器人的末端工具在世界坐标系中的坐标，根据患者口腔中的光学定位器，将所述末端工具在世界坐标系中的坐标转换成口腔图像坐标系中的坐标；

更新模块，用于当患者位置发生变动时，记录在位置发生变动前所述手术机器人的末端工具在所述口腔图像坐标系中的历史坐标，以及在患者位置变动后，所述手术机器人末端在所述口腔图像坐标系中的当前坐标，计算所述历史坐标到当前坐标之间的转换矩阵；

执行模块，用于根据所述转换矩阵，更新所述姿态数据，并根据更新后的姿态数据进行手术操作。

进一步的，本申请还提供一种手术机器人，包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行上述实施例中任一项所述的手术机器人轨迹跟踪方法。

进一步的，本申请还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行上述实施例中任一项所述的手术机器人轨迹跟踪方法。

本发明实施例公开了一种手术机器人轨迹跟踪方法、装置、机器人及存储介质，该方法包括：记录在拖动示教过程中手术机器人各个关节的姿态数据；获取手术机器人末端工具在世界坐标系中的坐标，根据患者口腔中的光学定位器，将末端工具在世界坐标系中的坐标转换成口腔图像坐标系中的坐标；当患者位置发生变动时，记录在位置发生变动前手术机器人末端在图像坐标系中的历史坐标，以及在患者位置变动后，手术机器人末端在图像坐标系中的当前坐标，计算历史坐标到当前坐标之间的转换矩阵；根据转换矩阵，更新姿态数据，并根据更新后的姿态数据进行手术操作。使得机器人可以适应患者位置的变化，不会过于死板僵硬，使得手术机器人在进行了一次示教之后，可以在一次学习后，即便机器人脱离了手术区，再次进入时，也能通过已经学习的轨迹做出调整，找到进入口腔的方法。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。

图1示出了本申请一种手术机器人手术场景示意图；

图2示出了本申请一种手术机器人轨迹跟踪方法流程图；

图3示出了本申请又一种手术机器人轨迹跟踪方法流程图；

图4示出了本申请一种手术机器人轨迹跟踪装置的结构示意图。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

如图1所示，为本申请牙科手术的场景，本申请的技术方案应用于牙科手术的手术机器人，本申请中，该牙科手术系统包括牙科手术机器人以及一个可以监控整个手术场景的NDI（网络接口）设备100，该NDI设备100可以是一个双目摄像头，由该NDI设备的图像为基础的坐标系为世界坐标系。

在牙科手术前，患者通常会做一次CT扫描来扫描牙齿区域的图像，获得的图像在上位机中作为术前规划的依据，以该扫描图为基础的坐标系为口腔图像坐标系。

手术机器人200一般固定在机器作业台上，通过接收上位机的命令进行手术操作，根据在上述的口腔图像坐标系里的术前规划，以运动到手术区域300进行手术。其中以手术机器人为基础的坐标系为机器人坐标系。

上述三个坐标系之间可以相互转换，即对于同一个点，在不同坐标系下会有不同的坐标体现，这些坐标之间可以通过转换矩阵进行转换，如何计算转换矩阵以及如何转换可以使用最近点搜索法等算法，更具体的计算步骤和过程不在本申请的技术方案介绍之列，在此不做详细解释。

接下来以具体实施例介绍本申请的技术方案。

实施例1

如图2所示为本申请一种手术机器人轨迹跟踪方法流程示意图，该方法包括的步骤如下：

步骤S110，记录在拖动示教过程中手术机器人各个关节的姿态数据。

首先需要对手术机器人进行拖动示教，拖动示教的目的是教会机器人如何寻找路径从患者的口腔外部进入患者的口腔内部，并找到对应需要进行手术的牙齿。为此，机器人在示教过程中，会先记录下在过程中自己各个关节的姿态数据，这些姿态数据可以是关节角度、角速度等体现关节姿态的物理数据。

具体而言，获取这些关节数据时可以先预设一个周期时间，然后按照该周期时间周期性获取数据。

以六轴机械臂为例，设周期时间为4毫秒，则每4ms采集一次6个关节的数据，记录为1组数据，假设拖动时间为20s，机械臂记录下5000组6轴关节的数据形成关节的姿态数据。

步骤S120，获取所述手术机器人的末端工具在世界坐标系中的坐标，根据患者口腔中的光学定位器，将所述末端工具在世界坐标系中的坐标转换成口腔图像坐标系中的坐标。

本申请的技术方案在进行牙科手术时，由摄像机在外部拍摄整个手术过程，以监控到机械臂的位置以及患者的位置，同时患者口腔中设置有光学定位器，由此，摄像机可以同时获取到机械臂末端工具的世界坐标以及患者口腔世界坐标。

术前规划是在口腔图像坐标系上进行的，所以可以通过世界坐标系作为中转，将机械臂的坐标转换成口腔图像坐标系下的坐标，以此来确保控制机械臂的精确准确的移动。

具体而言，可以先通过所述摄像头拍摄患者口腔中的光学定位器，确定所述患者在所述世界坐标系的位置；然后再计算所述世界坐标系转换到所述口腔图像坐标系的转换矩阵；最后根据所述转换矩阵，将位于世界坐标系的所述末端工具的位置坐标转换到所述口腔图像坐标系下。

步骤S130，当患者位置发生变动时，获取在位置发生变动前所述手术机器人的末端工具在所述口腔图像坐标系中的历史坐标，以及在患者位置变动后，所述手术机器人末端在所述口腔图像坐标系中的当前坐标，计算所述历史坐标到当前坐标之间的转换矩阵。

当患者位置变动，使得患者口腔与手术机器人的相对位置发生变化，因此需要计算在患者位置变动后，手术机器人从旧口腔图像坐标系到新口腔图像坐标系的转换关系。因为手术机器人是按照手术开始时拖动示教的路径进行工作的，因此其复位后，手术机器人末端工具的坐标，在世界坐标系里是没有变化的，只在口腔图像坐标系里发生了变化，因此可以根据手术机器人末端在所述口腔图像坐标系中的历史坐标和当前坐标来求取转换关系。

手术中，患者的位置可能发生较大的变动，如机械臂做完一次打磨，需要更换钻头，机械臂会离开患者口腔，此时处于手术的中场休息阶段，患者可能调整姿势以减缓手术的不适，但是患者的位置变更，机械臂复位的位置却不会变更，因此在第一次示教后所记载的姿态数据就不适用于患者的新位置。

对于机械臂来讲，每次更换完钻头后进行复位，所到达的位置是确定的，但是相对于口腔图像坐标系来讲，因为患者位置的变化，所以会有不同，因此机械臂会在同一个位置，拥有两个不同的口腔图像坐标系下的坐标。

患者口腔的位置可以通过患者口腔内安放的光学定位器进行确定，该光学定位器可以是反光片或者反光球，便于被摄像头拍摄识别，因为摄像头是不会移动的，因此通过基于摄像头的世界坐标系可以准确地对患者位置变化后的位置进行定位。

针对这两个坐标，可以求出一个转换矩阵，该转换矩阵的计算公式如下：

式中

为所述转换矩阵，

步骤S140，根据所述转换矩阵，更新所述姿态数据，并根据更新后的姿态数据进行手术操作。

得到转换矩阵后，就可以对姿态数据进行更新，这样更新后的姿态数据是能适应当前患者的位置的，具体而言，该更新后的姿态数据计算公式为：

式中

为更新后的姿态数据数据，

为更新前的姿态数据，

为所述转换矩阵。

例如，在一次手术过程中，手术机器人完成了第一阶段的钻孔工作，需要更换末端的工具，从而离开了患者的口腔，此时处于手术的中场休息阶段，患者可能因此调整一下坐姿，例如整体向躺椅上方平移了一小段距离，而对于机器人来讲，其更换完工具后，复位的位置依旧是第一阶段手术开始时的位置，此时手术机器人要是还按照第一次拖动示教时所获得的数据进行工作，势必无法正常进入口腔。

此时，摄像头通过拍摄到用户口腔内的光学定位器，确定了用户口腔的位置，进而确定了用户口腔和机器人的相对位置，确认了患者位置的变动，由此可以得到手术机器人末端工具在用户位置变动前，位于口腔图像坐标系下的坐标，以及在用户位置变动后，手术机器人末端工具位于口腔图像坐标系下的坐标。两个坐标数值不同，但是在世界坐标系下为同一个点，所以可以通过这个点求出一个转换矩阵，该转换矩阵体现了患者从之前的位置转变成现在的位置，两个图像坐标系之间的变换，进而可以根据这个转换矩阵，对机器人的所学习到的姿态数据进行更新，使得机器人学习到在新的位置上，应该怎样安全准确的进入用户的口腔。

本申请的技术方案，在记录和学习了拖动示教后的数据后，还会根据患者实时的位置，对这些拖动示教后的数据进行调整，使得患者的口腔位置发生变化后，机器人也可以通过相应的变化，将旧数据更新成符合现在患者位置的新数据，使得机器人在一次拖动示教后，面对不同的工作场景，都可以自适应调整，减少了医护人员的工作负担。

实施例2

本实施例提供了另一种手术机器人轨迹跟踪方法，如图3所示，具体步骤如下：

步骤S210，记录在拖动示教过程中手术机器人各个关节的姿态数据。

本步骤和实施例1的步骤S110类似，这里不再赘述。

步骤S220，将所述姿态数据转换成所述手术机器人末端工具在笛卡尔坐标系中的坐标点云数据，其中，所述笛卡尔坐标系以所述手术机器人的基座为原点建立。

姿态数据一般都是各个关节角的角度和角速度，为了能更好的运用示教结果，需要根据动力学反推这些数据来得到手术机器人末端工具在笛卡尔坐标系中的坐标数据，即前述提到的手术机器人坐标系。

具体的，首先要将所述手术机器人各个关节的运动数据转化成所述笛卡尔坐标系的空间坐标数据，并计算各个关节之间的平移变化矩阵；根据各个关节之间的所述平移变化矩阵计算得到所述手术机器人末端工具的坐标点云数据。

步骤S230，获取所述手术机器人的末端工具在世界坐标系中的坐标，根据患者口腔中的光学定位器，将所述末端工具在世界坐标系中的坐标转换成口腔图像坐标系中的坐标。

本步骤和实施例1的步骤S120类似，这里不再赘述。

步骤S240，当患者位置发生变动时，获取在位置发生变动前所述手术机器人的末端工具在所述口腔图像坐标系中的历史坐标，以及在患者位置变动后，所述手术机器人末端在所述口腔图像坐标系中的当前坐标，计算所述历史坐标到当前坐标之间的转换矩阵。

本步骤和实施例1的步骤S130类似，这里不再赘述。

步骤S250，根据所述转换矩阵更新所述空间坐标点云，得到更新后的空间坐标点云，所述更新后的空间坐标点云作为更新后的姿态数据以进行所述手术操作。

得到转换矩阵后，就可以对姿态数据进行更新，这样更新后的姿态数据是能适应当前患者的位置的，具体而言，该更新后的坐标点云计算公式为：

式中

为更新后的坐标点云数据，

为更新前的点云数据，

为所述转换矩阵。

相较于实施例1的步骤S140，本步骤以手术机器人末端工具的点云坐标来进行具体的手术执行依据，因为点与点之间是按照直线的方式移动，这样可以更好的达到避障以及使得手术机器人姿态更为顺从的效果。

实施例3

如图4所示，本申请实施例还提供一种手术机器人轨迹跟踪装置，包括：

记录模块10，用于记录在拖动示教过程中手术机器人各个关节的姿态数据；

转换模块20，用于获取所述手术机器人末端工具在世界坐标系中的坐标，根据患者口腔中的光学定位器，将所述末端工具在世界坐标系中的坐标转换成口腔图像坐标系中的坐标；

更新模块30，用于当患者位置发生变动时，记录在位置发生变动前所述手术机器人末端在所述口腔图像坐标系中的历史坐标，以及在患者位置变动后，所述手术机器人末端在所述口腔图像坐标系中的当前坐标，计算所述历史坐标到当前坐标之间的转换矩阵；

执行模块40，用于根据所述转换矩阵，更新所述姿态数据，并根据更新后的姿态数据进行手术操作。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是智能手机、个人计算机、服务器、或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

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

Claims

1.一种手术机器人轨迹跟踪方法，其特征在于，包括：

记录在拖动示教过程中手术机器人各个关节的姿态数据；

将所述姿态数据转换成所述手术机器人末端工具在笛卡尔坐标系中的空间坐标点云数据，其中，所述笛卡尔坐标系以所述手术机器人的基座为原点建立；

根据所述转换矩阵，更新所述空间坐标点云，得到更新后的空间坐标点云，所述更新后的空间坐标点云作为更新后的姿态数据，用于对手术机器人进行控制。

2.根据权利要求1所述的手术机器人轨迹跟踪方法，其特征在于，将所述姿态数据转换成所述手术机器人末端工具在笛卡尔坐标系中的坐标点云数据包括：

根据各个关节之间的所述平移变化矩阵，计算得到所述手术机器人末端工具的坐标点云数据。

3.根据权利要求1所述的手术机器人轨迹跟踪方法，其特征在于，所述更新后的坐标点云的计算公式为：

P_cloud＝P_{cloud_old}·T_pre2now；

式中P_cloud为更新后的坐标点云数据，P_{cloud_old}为更新前的点云数据，T_pre2now为所述转换矩阵。

4.根据权利要求1所述的手术机器人轨迹跟踪方法，其特征在于，根据患者口腔中的光学定位器，将所述末端工具在世界坐标系中的位置转换成图像坐标系中的位置包括：

通过摄像装置拍摄患者口腔中的光学定位器，确定所述患者在所述世界坐标系的位置；

5.根据权利要求1所述的手术机器人轨迹跟踪方法，其特征在于，所述转换矩阵的计算公式为：

T_pre2now＝P_F ^-1·PN_{Now_F}

式中，T_pre2now为所述转换矩阵，P_F ^-1为所述手术机器人的末端工具在图像坐标系下的历史坐标的逆矩阵，P_{Now_F}为所述手术机器人末端在所述口腔图像坐标系中的当前坐标。

6.根据权利要求1所述的手术机器人轨迹跟踪方法，其特征在于，所述记录在拖动示教过程中手术机器人各个关节的姿态数据包括：

以预设时间为周期，周期性记录各个关节的姿态数据。

7.一种手术机器人轨迹跟踪装置，其特征在于，包括：

记录模块，用于记录在拖动示教过程中手术机器人各个关节的姿态数据；将所述姿态数据转换成所述手术机器人末端工具在笛卡尔坐标系中的空间坐标点云数据，其中，所述笛卡尔坐标系以所述手术机器人的基座为原点建立；

执行模块，用于根据所述转换矩阵，更新所述空间坐标点云，得到更新后的空间坐标点云，所述更新后的空间坐标点云作为更新后的姿态数据，用于对手术机器人进行控制。

8.一种手术机器人，其特征在于，包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行权利要求1至6中任一项所述的手术机器人轨迹跟踪方法。

9.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上运行时执行权利要求1至6中任一项所述的手术机器人轨迹跟踪方法。