CN112754616B - 超声定位穿刺系统和存储介质 - Google Patents

Abstract

本申请涉及一种超声定位穿刺系统和存储介质。该超声定位穿刺系统包括计算机设备、第一串联机械臂和搭载在第一串联机械臂末端的超声探测装置，计算机设备分别与第一串联机械臂和超声探测装置电性连接，计算机设备包括存储器和处理器，存储器中存储有计算机程序，处理器在运行计算机程序时执行下列步骤：在超声探测装置投射的超声探测平面内定位靶点的第一位置信息；获取超声探测平面的第一姿态信息以及第一串联机械臂的第二姿态信息；根据第一姿态信息和第二姿态信息，确定超声定位穿刺系统的参考坐标系与超声探测平面所在坐标系的位置转换关系；根据位置转换关系和第一位置信息，确定靶点在参考坐标系内的第二位置信息。通过本申请，提高了穿刺手术的精度。

Description

超声定位穿刺系统和存储介质

技术领域

本申请涉及机械臂控制领域，特别是涉及超声定位穿刺系统和存储介质。

背景技术

传统的穿刺手术是医生在超声、CT、MRI等图像扫描设备的引导下，由医生手动将小型手术器械(如活检穿刺针等)送入患者体内，对病变部位进行检测或治疗，医生通过病灶附近的二维或三维扫描图像判断合适的入针点和入针方向，然后凭借经验手动完成穿刺操作。

计算机辅助导航技术和机器人技术被引入到穿刺手术中。具有手术导航系统的穿刺机器人工作原理如下：首先由计算机将扫描的二维图像进行三维合成形成病灶附近的三维图像，然后由医生通过三维图像判断靶点位置和合适的入针路径并输入导航系统，再由导航系统计算出机器人操作臂的当前状态和目标点并规划出一条轨迹，最后操作臂按照规划的轨迹完成穿刺定位，再通过进针机构或医生手动完成进针，避免了纯手动操作带来的误差。然而，现有的导航技术通常通过光学定位对穿刺针和医学影像设备进行定位和配准，在医学影像的辅助下进行穿刺，但光学定位和配准的过程中存在较大的误差，并且在操作的过程中不能使光学小球被遮挡，限制了手术的实施。

发明内容

本申请实施例提供了一种超声定位穿刺系统和存储介质，直接在超声影像中获取病灶坐标，而不需要通过光学定位等配准基准间接配准，以解决现有的医学影像导航下穿刺手术的定位和配准技术也存在误差的问题。

第一方面，本申请实施例提供了一种超声定位穿刺系统，所述超声定位穿刺系统包括计算机设备、第一串联机械臂和搭载在所述第一串联机械臂末端的超声探测装置，所述计算机设备分别与所述第一串联机械臂和所述超声探测装置电性连接，所述计算机设备包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器在运行所述计算机程序时执行下列步骤：在所述超声探测装置投射的超声探测平面内定位靶点的第一位置信息；获取所述超声探测平面的第一姿态信息以及所述第一串联机械臂的第二姿态信息；根据所述第一姿态信息和所述第二姿态信息，确定所述超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系；根据所述位置转换关系和所述第一位置信息，确定所述靶点在所述参考坐标系内的第二位置信息。

在其中的一些实施例中，在所述超声探测装置投射的超声探测平面内定位靶点的第一位置信息包括：获取由所述超声探测装置采集的超声图像，在所述超声图像中定位所述靶点的位置信息；根据所述靶点在所述超声图像中的位置信息以及所述超声探测装置的标定参数，确定所述靶点在所述超声探测装置投射的超声探测平面内的所述第一位置信息。

在其中的一些实施例中，所述超声探测装置与所述第一串联机械臂的末端可转动地连接，所述超声探测平面形成于所述超声探测装置的末端端点并自所述超声探测装置的末端端点沿扇面延伸；所述超声探测平面跟随所述超声探测装置旋转，所述超声探测平面的旋转轴经过所述末端端点；所述第一姿态信息包括所述超声探测平面的旋转角度。

在其中的一些实施例中，所述第一位置信息包括所述靶点相对于所述末端端点的距离，以及所述靶点和所述末端端点的连接线与所述旋转轴的夹角。

在其中的一些实施例中，所述第一串联机械臂包括多个关节，多个所述关节包括：旋转关节和/或移动关节，所述第二姿态信息包括每个所述关节的运动量信息。

在其中的一些实施例中，根据所述第一姿态信息和所述第二姿态信息，确定所述超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系包括：在所述第一串联机械臂处于初始状态时，建立所述第一串联机械臂的参考坐标系和所述第一串联机械臂的各关节的关节坐标系，以及在所述超声探测装置处于初始状态时，以所述超声探测装置的末端端点为坐标原点建立探头坐标系；获取所述第一串联机械臂的参考坐标系和所述第一串联机械臂的各关节的关节坐标系的DH参数，并根据所述DH参数，确定所述第一串联机械臂的参考坐标系到所述第一串联机械臂的末端关节的关节坐标系之间的第一位置转换关系；获取所述探头坐标系的坐标原点在所述末端关节的关节坐标系内的位置信息，以及所述探头坐标系相对于所述末端关节的关节坐标系的姿态角，并根据该位置信息、姿态角以及所述第一位置转换关系，确定所述参考坐标系与所述探头坐标系之间的第二位置转换关系；获取所述超声探测平面的旋转角度，并根据所述旋转角度确定所述超声探测平面所在坐标系与所述探头坐标系之间的第三位置转换关系；根据所述第二位置转换关系和所述第三位置转换关系，确定所述超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系。

在其中的一些实施例中，所述第一串联机械臂包括旋转关节和移动关节，旋转关节的关节坐标系的Z轴沿旋转轴设置，移动关节的关节坐标系的Z轴沿移动方向设置，参考坐标系与各关节的关节坐标系同为左手系或右手系，且当旋转关节与在前的移动关节连接时，旋转关节的关节坐标系的原点与该在前的移动关节的关节坐标系的原点重合。

在其中的一些实施例中，所述探头坐标系的X、Y、Z轴的方向与所述末端关节的关节坐标系的X、Y、Z轴的方向相同，且所述探头坐标系的Z轴和所述末端关节的关节坐标系的Z轴共线。

在其中的一些实施例中，所述第一串联机械臂的末端关节为并联运动平台，所述并联运动平台包括静平台、动平台和多个设置在所述静平台与所述动平台之间的伸缩组件，所述超声探测装置设于所述动平台。

在其中的一些实施例中，所述超声定位穿刺系统还包括第二串联机械臂和搭载在所述第二串联机械臂末端的穿刺器械；所述计算机设备还分别与所述第二串联机械臂电性连接，所述处理器在运行所述计算机程序时还执行下列步骤：将所述靶点在所述参考坐标系内的第二位置信息作为目标穿刺点，控制所述第二串联机械臂的穿刺器械对所述目标穿刺点进行穿刺。

第二方面，本申请实施例提供了一种存储介质，所述存储介质中存储有计算机程序，所述计算机程序被设置为运行时执行下列步骤：

在所述超声探测装置投射的超声探测平面内定位靶点的第一位置信息；

获取所述超声探测平面的第一姿态信息以及所述第一串联机械臂的第二姿态信息；

根据所述第一姿态信息和所述第二姿态信息，确定所述超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系；

根据所述位置转换关系和所述第一位置信息，确定所述靶点在所述参考坐标系内的第二位置信息。

相比于相关技术，本申请实施例提供的超声定位穿刺系统和存储介质，解决了，现有的医学影像导航下穿刺手术的定位和配准技术也存在误差的问题，提高了穿刺手术的精度。

本申请的一个或多个实施例的细节在以下附图和描述中提出，以使本申请的其他特征、目的和优点更加简明易懂。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1是本申请实施例提供的超声定位穿刺系统的结构框图。

图2是本申请实施例提供的超声定位穿刺系统的工作方法的流程图。

图3是本申请实施例的多关节机械臂的坐标转换方法的流程图。

图4是本申请优选实施例提供的多关节机械臂的结构示意图。

图5是本申请优选实施例提供的多关节机械臂的机械坐标系和关节坐标系的示意图。

图6是本申请优选实施例的超声探测装置的探头坐标系与Stewart计算坐标系的示意图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行描述和说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请提供的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。此外，还可以理解的是，虽然这种开发过程中所作出的努力可能是复杂并且冗长的，然而对于与本申请公开的内容相关的本领域的普通技术人员而言，在本申请揭露的技术内容的基础上进行的一些设计，制造或者生产等变更只是常规的技术手段，不应当理解为本申请公开的内容不充分。

在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域普通技术人员显式地和隐式地理解的是，本申请所描述的实施例在不冲突的情况下，可以与其它实施例相结合。

除非另作定义，本申请所涉及的技术术语或者科学术语应当为本申请所属技术领域内具有一般技能的人士所理解的通常意义。本申请所涉及的“一”、“一个”、“一种”、“该”等类似词语并不表示数量限制，可表示单数或复数。本申请所涉及的术语“包括”、“包含”、“具有”以及它们任何变形，意图在于覆盖不排他的包含；例如包含了一系列步骤或模块(单元)的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可以还包括没有列出的步骤或单元，或可以还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本申请所涉及的“连接”、“相连”、“耦接”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电气的连接，不管是直接的还是间接的。本申请所涉及的“多个”是指大于或者等于两个。“和/或”描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。本申请所涉及的术语“第一”、“第二”、“第三”等仅仅是区别类似的对象，不代表针对对象的特定排序。

本实施例提供了一种超声定位穿刺系统，图1是本申请实施例提供的超声定位穿刺系统的结构框图，如图1所示，该超声定位穿刺系统包括计算机设备101、第一串联机械臂102和搭载在第一串联机械臂102末端的超声探测装置103，计算机设备101分别与第一串联机械臂102和超声探测装置103电性连接，计算机设备101包括存储器1011和处理器1012，存储器1011中存储有计算机程序10111。

图2是本申请实施例提供的超声定位穿刺系统的工作方法的流程图，如图2所示，处理器1012在运行计算机程序时执行下列步骤S201至步骤S204。

步骤S201，在超声探测装置投射的超声探测平面内定位靶点的第一位置信息。

超声探测装置103投射的超声探测平面通常呈扇面状，扇面的扇心即为超声发生的起点位置，在扇心位置发出超声并接收物质反射的回波来探测该超声探测平面内的目标位置。

由于超声探测平面只能够探测到一个扇面的信息，而待穿刺的靶点位置位于人体内的一个空间位置内，因此若要探测到准确的靶点位置，搭载超声探测装置103的第一串联机械臂102需要将超声探测装置103移动到靶点附近位置，在移动到靶点附近位置时，第一串联机械臂102的各关节的运动量会发生变化，在本申请实施例中这些关节运动量的集合称为第二姿态信息。在其中的一些实施例中，第一串联机械臂包括多个关节，多个关节包括：旋转关节和/或移动关节，第二姿态信息包括每个关节的运动量信息。

在其中的一些实施例中，超声探测装置与第一串联机械臂102的末端可转动地连接，超声探测平面形成于超声探测装置的末端端点并自超声探测装置的末端端点沿扇面延伸；超声探测平面跟随超声探测装置旋转，超声探测平面的旋转轴经过末端端点；第一姿态信息包括超声探测平面的旋转角度。

当超声探测装置103被移动到靶点附近位置之后，超声探测平面朝向靶点方向照射，为了在超声探测平面中显示岀靶点，在本申请实施例中沿扇面的对称轴旋转超声探测装置103，超声探测平面随之旋转，从而实现圆台形或圆锥形空间区域内的靶点探测，每个超声探测平面的超声图像都会实时显示在电子屏幕上。一旦在电子屏幕上观察到与靶点特征相符的图像，则可以该靶点在超声图像内的位置确定靶点在真实的超声探测平面内的位置，即为第一位置信息。

例如，确定靶点在超声探测平面内的位置，通过实时获取由超声探测装置采集的超声图像，在超声图像中定位靶点的位置信息；然后根据靶点在超声图像中的位置信息以及超声探测装置的标定参数，确定靶点在超声探测装置投射的超声探测平面内的第一位置信息。超声探测装置的标定参数包括但不限于以下至少之一：阵元数、曲率半径、线阵宽度、查扫视野(FOV)等。

第一位置信息可以采用直角坐标或极坐标的方式表示。例如，在采用极坐标方式表示的情况下，第一位置信息包括靶点相对于末端端点的距离，以及靶点和末端端点的连接线与旋转轴的夹角。

步骤S202，获取超声探测平面的第一姿态信息以及第一串联机械臂的第二姿态信息。

步骤S203，根据第一姿态信息和第二姿态信息，确定超声定位穿刺系统的参考坐标系与超声探测平面所在坐标系的位置转换关系。

步骤S204，根据位置转换关系和第一位置信息，确定靶点在参考坐标系内的第二位置信息。

在超声探测平面的第一姿态信息和第一串联机械臂的第二姿态信息确定的情况下，则超声探测平面上的任意一点与参考坐标系的转换关系就是确定的，因此，根据第一姿态信息和第二姿态信息，可以确定超声定位穿刺系统的参考坐标系与超声探测平面所在坐标系的位置转换关系，再根据该位置转换关系和第一位置信息就能够确定靶点在参考坐标系内的第二位置信息，实现靶点在参考坐标系下的定位。

需要说明的是，在上述流程中或者附图的流程图中示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。例如，步骤S201也可以在步骤S202或步骤S203之后执行。

在其中的一些实施例中，根据第一姿态信息和第二姿态信息，确定超声定位穿刺系统的参考坐标系与超声探测平面所在坐标系的位置转换关系包括如下步骤：

步骤S203-1，在第一串联机械臂处于初始状态时，建立第一串联机械臂的参考坐标系和第一串联机械臂的各关节的关节坐标系，以及在超声探测装置处于初始状态时，以超声探测装置的末端端点为坐标原点建立探头坐标系。

步骤S203-2，获取第一串联机械臂的参考坐标系和第一串联机械臂的各关节的关节坐标系的DH参数，并根据DH参数，确定第一串联机械臂的参考坐标系到第一串联机械臂的末端关节的关节坐标系之间的第一位置转换关系。

步骤S203-3，获取探头坐标系的坐标原点在末端关节的关节坐标系内的位置信息，以及探头坐标系相对于末端关节的关节坐标系的姿态角，并根据该位置信息、姿态角以及第一位置转换关系，确定参考坐标系与探头坐标系之间的第二位置转换关系。

步骤S203-4，获取超声探测平面的旋转角度，并根据旋转角度确定超声探测平面所在坐标系与探头坐标系之间的第三位置转换关系。

步骤S203-5，根据第二位置转换关系和第三位置转换关系，确定超声定位穿刺系统的参考坐标系与超声探测平面所在坐标系的位置转换关系。

上述的步骤S203-2旨在确定第一串联机械臂的参考坐标系到第一串联机械臂的末端关节的关节坐标系之间的第一位置转换关系。

针对相关技术中机械臂的控制复杂的问题，在本实施例提供了一种多关节机械臂的坐标转换方法，应用于包括旋转关节和移动关节的机械臂，这些旋转关节和移动关节依次串联连接。为了实现机械臂在空间中多个自由度的运动，旋转关节和移动关节的数量可以根据需要的自由度和运动范围进行设置，即旋转关节和移动关节的数量都可以是一个或者多个。

图3是本申请实施例的多关节机械臂的坐标转换方法的流程图，如图3所示，该流程包括如下步骤S301至步骤S303。

步骤S301，在机械臂处于初始状态时，建立机械臂的机械坐标系和机械臂的各关节的关节坐标系。

机械臂的控制通常基于DH参数进行坐标系的转换，DH参数包括传统的DH参数和改进的DH参数。相连接的两个关节为相邻关节。相邻两个关节的关节坐标系的变换通常采用DH参数或改进的DH参数来表示。以DH参数为例，相邻两个关节坐标系通过绕Z轴旋转θ、平移d后，再绕X轴旋转α、平移a，就能够相互重合，上述的θ、d、α和a即为DH参数。由此可知，如果DH参数越简洁，则相邻两个关节坐标系的转换越简单。

为了简化DH参数，在本实施例中，旋转关节的关节坐标系的Z轴沿旋转轴设置，移动关节的关节坐标系的Z轴沿移动方向设置，机械坐标系与各关节的关节坐标系同为左手系或右手系，且当旋转关节之前的关节为移动关节时，旋转关节的关节坐标系的原点与该移动关节的关节坐标系的原点重合。

上述的机械坐标系又可以称为参考坐标系或者世界坐标系，该机械坐标系通常设置于机械臂的底座中心，并且位于多关节臂的第一个关节的正下方，这样可以使得机械坐标系与第一个关节的转换关系尽可能地简单。

在串联机械臂中，在大多数情况下旋转关节在接收到转角为0或2π的指令时，可能不需要区分二者，而是根据预先的设定保持不旋转或者以设定方向旋转2π。但在一些情况下，需要对这两个转角进行区分。在需要对0或2π转角进行区分的情形下，旋转关节的DH参数中Z轴的旋转角度不为0或2π，以避免混淆0或2π的旋转角度。

步骤S302，获取机械臂的机械坐标系和机械臂的各关节的关节坐标系的DH参数，并根据DH参数，确定机械臂的机械坐标系到机械臂的末端关节的关节坐标系之间的转换关系。

在建立各关节的关节坐标系之后，就可以依次获得相邻关节之间的DH参数，并根据DH参数，确定机械臂的机械坐标系到机械臂的末端关节的关节坐标系之间的转换关系。

其中，根据DH法则，第i-1关节的坐标到第i关节的坐标的齐次变换被构造为具有两个旋转和两个变换的序列，采用矩阵可表示如下：

其中，i＝2，3，4，…，n；n为机械臂的旋转关节和移动关节的总数量。机械臂的第1个关节的DH参数是表示的该第一个关节与机械坐标系之间的坐标系的转换，记为

则机械坐标系与第1个关节的关节坐标系的转换关系：

可见，该变换关系与第i-1关节的坐标到第i关节的坐标的齐次变换形式完全相同。

在得到

与

之后，从第0个坐标系(机械坐标系)到第N关节的关节坐标系的转换矩阵可表示为：

其中，第N个关节为末端关节。

步骤S303，根据转换关系进行机械坐标系与末端关节的关节坐标系之间的坐标转换。

上述步骤S302中确定的

表示的就是机械坐标系与末端关节的关节坐标系之间的坐标转换关系，根据该转换关系就能够实现机械坐标系与末端关节的关节坐标系之间的坐标转换。

下面通过优选实施例对本申请实施例进行描述和说明。

图4是本申请优选实施例提供的多关节机械臂的结构示意图，如图4所示的多关节机械臂依次包括移动关节1、旋转关节2、移动关节3、旋转关节4、旋转关节5、移动关节6、旋转关节7、移动关节8、旋转关节9和移动关节10，此外，该多关节机械臂还包括与移动关节1固连的基座11。

该优选实施例的多关节机械臂的坐标转换方法包括步骤1至步骤3，分别对应于图3中的步骤S301至步骤S303。

步骤1，依照世界坐标系的规则在机械臂的基座处建立机械坐标系，以及各关节的关节坐标系。

图5是本申请优选实施例提供的多关节机械臂的机械坐标系和关节坐标系的示意图。

参考图5，机械坐标系F₀-X₀Y₀Z₀坐标系原点F₀固连于机械臂基座处，Z₀轴由F₀指向移动关节1，Y₀轴由基座F₀点指向机械臂，X₀轴指向符合右手坐标系。

移动关节1的关节坐标系L₁-X₁Y₁Z₁的原点L₁固连在移动关节₁上，各坐标轴的指向和机械坐标系的对应轴指向相同。

旋转关节2的关节坐标系R₂-X₂Y₂Z₂的原点R₂固连在旋转关节2上且与L₁重合，Z₂指向和Z₁轴指向相同，X₂轴和Y₂轴的指向分别与X₁轴和Y₁轴指向相反。

移动关节3的关节坐标系L₃-X₃Y₃Z₃原点L₃固连在移动关节3上，Z₃轴由L₁点指向L₃点，X₃轴和Y₃轴分别与X₂轴和Z₂轴指向相同。

旋转关节4的关节坐标系R₄-X₄Y₄Z₄原点R₄固连在旋转关节4上且与L₃点重合(图5中为清晰标明移动关节3的关节坐标系和旋转关节4的关节坐标系将L₃和R₄分开标出，下同)，Z₄轴指向和Y₃轴指向相反，初始时，X₄轴和Y₄轴指向分别与X₃轴和Z₃轴指向相反。

旋转关节5的关节坐标系R₅-X₅Y₅Z₅原点R₅固连在旋转关节5上，Z₅轴方向和Z₄轴方向相同，初始时，X₅轴和Y₅轴指向分别与X₄轴和Y₄轴指向相反。

移动关节6的关节坐标系L₆-X₆Y₆Z₆原点L₆固连在移动关节6上，Z₆轴由R₅点指向L₆点，X₆轴和Y₆轴指向分别与X₅轴和Z₅轴指向相同。

旋转关节7的关节坐标系R₇-X₇Y₇Z₇原点R7固连在旋转关节7上且和L₆点重合，初始时，Z₇轴和Y₇轴的指向分别与Y₆轴和X₆轴的指向相反，X₇轴和Z₆轴的指向相同。

移动关节8的关节坐标系L₈-X₈Y₈Z₈的原点L₈固连在移动关节8上，Z₈轴由L₈点指向R₇点，X₈和Y₈轴的指向分别与X₇和Y₇轴的指向相同。

旋转关节9的关节坐标系R₉-X₉Y₉Z₉的原点R₉点固连在旋转关节9上且和L₈点重合，Z₉轴的方向与Y₈轴的方向相反，初始时，X₉轴的方向与Z₈轴的方向相反，Y₉轴的方向与X8轴的方向相同。

移动关节10的关节坐标系L₁₀-X₁₀Y₁₀Z₁₀的原点L₁₀固连在移动关节10上，Z₁₀轴由R₉点指向L₁₀点，X₁₀轴和Y₁₀轴的指向分别与X₉轴和Z₉轴的指向相同。

F₀R₂的长度为l₁，R₂R₄的长度为l₂，R₄R₅的长度为l₃，R₅R₇的长度为l₄，R₇R₉的长度为l₅，R₉L₁₀的长度为1₆，点L₁、R₂、L₃、R₄、R₅、L₆和R₇位于同一水平面上。

步骤2，获取DH参数，并计算机械坐标系到多关节机械臂的末端关节的转换关系。

DH参数如表1所示。在表1中，为了避免混淆旋转关节的旋转角度0和2π，在每个旋转关节的Z轴旋转角θ上分别增加了一个偏转角度，例如旋转关节2的Z轴旋转角度为π+θ₂，其中θ₂即为偏转角度。

表1图5所示的多关节机械臂的DH参数表

在图4所示的多关节机械臂的末端关节上还可以搭载并联平台，并联平台包括静平台、动平台和多个设置在静平台与动平台之间的伸缩组件，并联平台的计算坐标系与多关节机械臂的末端关节的关节坐标系完全重合，超声探测装置设于动平台。

本实施例的并联平台可以实现多个自由度的运动，以具有六自由度的Stewart并联平台为例，Stewart并联平台包括静平台、动平台和多个设置在静平台与动平台之间的伸缩组件，可以实现空间上六个自由度的运动，分别为沿X轴位移、沿Y轴位移、沿Z轴位移、绕X轴转动、绕Y轴转动和绕Z轴转动。Stewart并联平台由6根伸缩组件支撑，与采用串联的悬臂梁结构的被动臂相比刚度大，结构稳定，并且由于刚度大，并联结构较串联结构在相同的自重或体积下，有较高的承载能力。采用串联的悬臂梁结构的被动臂末端的误差是各个关节误差的积累和放大，因而误差大、精度低，而并联平台则没有那样的误差积累和放大关系，微动精度高，更适宜执行高精度的手术操作。此外，在位置求解上，并联平台的逆解非常容易，容易根据坐标位置求得并联平台各伸缩组件的运动姿态。

正是基于并联平台逆解非常容易的特点，在上述实施例中，可以通过在静平台上建立从用户坐标系，将主操作手的位移量映射到从用户坐标系内，再通过从用户坐标系与计算坐标系的转换从而得到目标位置在计算坐标系内的位置信息，根据该位置信息通过逆解就能够很容易地得到并联平台各伸缩组件的运动姿态，相对于相关技术中通过正运动学求解并联平台末端的位置信息，上述方式大大降低了运算复杂度，提高了控制效率，节约了运算资源。

解算Stewart计算坐标系到机械坐标系的转换矩阵，即移动关节坐标系L₁₀-X₁₀Y₁₀Z₁₀到机械坐标系F₀-X₀Y₀Z₀的转换矩阵与逆矩阵：

根据机器人正运动学可知两相邻关节之间的转换矩阵为

从第m关节(包含m关节的自由度)到第n关节的转换矩阵可表示为：

可解算得单条机械臂从基座到Stewart平台静平台的转换矩降

即机械坐标系到Stewart计算坐标系的转换矩阵，命名为T_{trans_mach_st}，而Stewart计算坐标系到机械坐标系的转换矩阵则为逆矩阵

步骤3，根据转换矩阵和转换矩阵的逆矩阵进行机械坐标系与Stewart计算坐标系的坐标转换。

为了简化探头坐标系与参考坐标系之间的转换关系，在其中的一些实施例中，探头坐标系的X、Y、Z轴的方向与末端关节的关节坐标系的X、Y、Z轴的方向相同，且探头坐标系的Z轴和末端关节的关节坐标系的Z轴共线。在第一串联机械臂的末端关节为并联平台的情况下，探头坐标系的X、Y、Z轴的方向与动平台坐标系的X、Y、Z轴的方向相同，且探头坐标系的Z轴和动平台坐标系的Z轴共线。

下面通过优选实施例对本申请实施例的探头坐标系以及探头坐标系与参考坐标系的转换关系进行描述和说明。

图6是本申请优选实施例的超声探测装置的探头坐标系与Stewart计算坐标系的示意图。在超声探测装置与Stewart动平台之间设有一个旋转电机，电机的旋转角度记为θ_m。超声探测装置的末端点位于Stewart动平台坐标系的Z轴上，无论θ_m取何值，超声探测装置的末端点相对Stewart动平台的位置都是不变。

在超声探测装置末端建立探头坐标系，探头坐标系的原点与探头末端点重合，且忽略Stewart动平台与超声探测装置之间的旋转电机的运动，默认探头坐标系的XYZ轴始终与Stewart动平台坐标系的XYZ轴平行。也就是将Stewart动平台与超声探测装置之间的电机的旋转量θ_m视为超声探测平面相对探头坐标系的运动。

需要说明的是，探头坐标系的建立方式并不是唯一的，并不一定要将探头相对于Stewart动平台的转动θ_m转化为超声探测平面相对于探头坐标系的转动。

在通过主从映射方法控制Stewart并联平台的情况下，超声探头的运动可以由主操作手控制，例如通过混联主从映射算法将主操作手在竖直方向上的运动量映射到并联平台，将主操作手的水平平面内的运动量映射到串联机械臂。通过混联主从映射算法可得超声探测装置末端点在Stewart计算坐标系的位置C_{coord_det_st}(三维列向量)，和探头坐标系相对于Stewart计算坐标系的姿态角

(其中

为固定值)，即可解算得探头坐标系到Stewart计算坐标系的转换矩阵T_{trans_det_st}。

其中R_{coord_det_st}为旋转矩阵，用姿态角

表示为：

操作者可以通过主操作手操控末端安装有超声探测装置的机械臂，使穿刺目标靶点位于超声探测平面(声束平面)上。超声探测平面为经过超声探测装置末端点且垂直于探头坐标系XY平面的一个平面，在超声探测平面内的任意一点可测得其相对于探头坐标系的距离l_c和与Z轴的夹角θ_c。

通过l_c，θ_c，θ_m这三个已知量，可知穿刺目标靶点在探头坐标系下的坐标Sst(Xst，Yst，Zst)，记为C_{coord_tar_det}。

C_{coord_tar_det}＝[l_csinθ_ccosθ_m l_csinθ_csinθ_m l_ccosθ_c 1]^T。

通过坐标转换可得目标靶点在机械坐标系下的坐标C_{coord_tar_mach}：

C_{coord_tar_mach}＝T_{trans_mach_st}T_{trans_st_det}C_{coord_tar_det}。

在其中的一些实施例中，超声定位穿刺系统还包括第二串联机械臂和搭载在第二串联机械臂末端的穿刺器械；计算机设备还分别与第二串联机械臂电性连接，处理器在运行计算机程序时还执行下列步骤：将靶点在参考坐标系内的第二位置信息作为目标穿刺点，控制第二串联机械臂的穿刺器械对目标穿刺点进行穿刺。

通过坐标转换可得目标靶点在穿刺机械臂Stewart计算坐标系下的坐标C_{coord_tar_stl}：

C_{coord_tar_stl}＝T_{trans_stl_mach}C_{coord_tar_mach}。

其中T_{trans_stl_mach}为穿刺机械臂Stewart计算坐标系到计算坐标系的转换矩阵。

已知目标靶点在穿刺机械臂Stewart计算坐标系下的坐标以及预设的穿刺路径和穿刺速度，即可通过Stewart并联平台的逆运动学解算穿刺机械臂Stewart平台的关节运动量，用穿刺机械臂进行精准的穿刺。

通过上述具有两个机械臂的系统，第一串联机械臂的末端搭载超声探测装置，第二串联机械臂的末端搭载穿刺器械，穿刺消融手术的执行由两个机械臂协作完成，第一串联机械臂持超声探头，在人体表面扫描寻找病灶，并定位穿刺目标靶点，可以实现靶点的精准定位。第二串联机械臂持消融针，对目标靶点进行穿刺消融，可以实现对靶点的精准穿刺。整个定位和穿刺的过程几乎对人为操作的依赖性较小，更加安全，并且上述靶点定位和穿刺方法兼容现有的具有多机械臂且机械臂末端设置有并联平台的机器人，不需要独立开发复杂的系统。

另外，结合上述实施例中的方法，本申请实施例还提供一种计算机可读存储介质来实现。该计算机可读存储介质上存储有计算机程序；计算机程序被设置为运行时执行下列步骤：

S1，在超声探测装置投射的超声探测平面内定位靶点的第一位置信息。

S2，获取超声探测平面的第一姿态信息以及第一串联机械臂的第二姿态信息。

S3，根据第一姿态信息和第二姿态信息，确定超声定位穿刺系统的参考坐标系与超声探测平面所在坐标系的位置转换关系。

S4，根据位置转换关系和第一位置信息，确定靶点在参考坐标系内的第二位置信息。

在其中一些实施例中，在超声探测装置投射的超声探测平面内定位靶点的第一位置信息包括：获取由超声探测装置采集的超声图像，在超声图像中定位靶点的位置信息；根据靶点在超声图像中的位置信息以及超声探测装置的标定参数，确定靶点在超声探测装置投射的超声探测平面内的第一位置信息。

在其中一些实施例中，第一位置信息包括靶点相对于末端端点的距离，以及靶点和末端端点的连接线与旋转轴的夹角。

在其中一些实施例中，根据第一姿态信息和第二姿态信息，确定超声定位穿刺系统的参考坐标系与超声探测平面所在坐标系的位置转换关系包括：在第一串联机械臂处于初始状态时，建立第一串联机械臂的参考坐标系和第一串联机械臂的各关节的关节坐标系，以及在超声探测装置处于初始状态时，以超声探测装置的末端端点为坐标原点建立探头坐标系；获取第一串联机械臂的参考坐标系和第一串联机械臂的各关节的关节坐标系的DH参数，并根据DH参数，确定第一串联机械臂的参考坐标系到第一串联机械臂的末端关节的关节坐标系之间的第一位置转换关系；获取探头坐标系的坐标原点在末端关节的关节坐标系内的位置信息，以及探头坐标系相对于末端关节的关节坐标系的姿态角，并根据该位置信息、姿态角以及第一位置转换关系，确定参考坐标系与探头坐标系之间的第二位置转换关系；获取超声探测平面的旋转角度，并根据旋转角度确定超声探测平面所在坐标系与探头坐标系之间的第三位置转换关系；根据第二位置转换关系和第三位置转换关系，确定超声定位穿刺系统的参考坐标系与超声探测平面所在坐标系的位置转换关系。

在其中一些实施例中，第一串联机械臂包括旋转关节和移动关节，旋转关节的关节坐标系的Z轴沿旋转轴设置，移动关节的关节坐标系的Z轴沿移动方向设置，参考坐标系与各关节的关节坐标系同为左手系或右手系，且当旋转关节与在前的移动关节连接时，旋转关节的关节坐标系的原点与该在前的移动关节的关节坐标系的原点重合。

在其中一些实施例中，探头坐标系的X、Y、Z轴的方向与末端关节的关节坐标系的X、Y、Z轴的方向相同，且探头坐标系的Z轴和末端关节的关节坐标系的Z轴共线。

在其中一些实施例中，计算机程序还被设置为运行时执行下列步骤：将靶点在参考坐标系内的第二位置信息作为目标穿刺点，控制第二串联机械臂的穿刺器械对目标穿刺点进行穿刺。

需要说明的是，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。

本领域的技术人员应该明白，以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种超声定位穿刺系统，其特征在于，所述超声定位穿刺系统包括计算机设备、第一串联机械臂和搭载在所述第一串联机械臂末端的超声探测装置，所述计算机设备分别与所述第一串联机械臂和所述超声探测装置电性连接，所述计算机设备包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器在运行所述计算机程序时执行下列步骤：

在所述超声探测装置投射的超声探测平面内定位靶点的第一位置信息；

获取所述超声探测平面的第一姿态信息以及所述第一串联机械臂的第二姿态信息；

根据所述第一姿态信息和所述第二姿态信息，确定所述超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系；

根据所述位置转换关系和所述第一位置信息，确定所述靶点在所述参考坐标系内的第二位置信息；

其中，根据所述第一姿态信息和所述第二姿态信息，确定所述超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系包括：

在所述第一串联机械臂处于初始状态时，建立所述第一串联机械臂的参考坐标系和所述第一串联机械臂的各关节的关节坐标系，以及在所述超声探测装置处于初始状态时，以所述超声探测装置的末端端点为坐标原点建立探头坐标系；

获取所述第一串联机械臂的参考坐标系和所述第一串联机械臂的各关节的关节坐标系的DH参数，并根据所述DH参数，确定所述第一串联机械臂的参考坐标系到所述第一串联机械臂的末端关节的关节坐标系之间的第一位置转换关系；

获取所述探头坐标系的坐标原点在所述末端关节的关节坐标系内的位置信息，以及所述探头坐标系相对于所述末端关节的关节坐标系的姿态角，并根据该位置信息、姿态角以及所述第一位置转换关系，确定所述参考坐标系与所述探头坐标系之间的第二位置转换关系；

获取所述超声探测平面的旋转角度，并根据所述旋转角度确定所述超声探测平面所在坐标系与所述探头坐标系之间的第三位置转换关系；

根据所述第二位置转换关系和所述第三位置转换关系，确定所述超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系。

2.根据权利要求1所述的超声定位穿刺系统，其特征在于，在所述超声探测装置投射的超声探测平面内定位靶点的第一位置信息包括：

获取由所述超声探测装置采集的超声图像，在所述超声图像中定位所述靶点的位置信息；

根据所述靶点在所述超声图像中的位置信息以及所述超声探测装置的标定参数，确定所述靶点在所述超声探测装置投射的超声探测平面内的所述第一位置信息。

3.根据权利要求1所述的超声定位穿刺系统，其特征在于，所述超声探测装置与所述第一串联机械臂的末端可转动地连接，所述超声探测平面形成于所述超声探测装置的末端端点并自所述超声探测装置的末端端点沿扇面延伸；所述超声探测平面跟随所述超声探测装置旋转，所述超声探测平面的旋转轴经过所述末端端点；所述第一姿态信息包括所述超声探测平面的旋转角度。

4.根据权利要求3所述的超声定位穿刺系统，其特征在于，所述第一位置信息包括所述靶点相对于所述末端端点的距离，以及所述靶点和所述末端端点的连接线与所述旋转轴的夹角。

5.根据权利要求1所述的超声定位穿刺系统，其特征在于，所述第一串联机械臂包括多个关节，多个所述关节包括：旋转关节和/或移动关节，所述第二姿态信息包括每个所述关节的运动量信息。

6.根据权利要求1所述的超声定位穿刺系统，其特征在于，所述第一串联机械臂包括旋转关节和移动关节，旋转关节的关节坐标系的Z轴沿旋转轴设置，移动关节的关节坐标系的Z轴沿移动方向设置，参考坐标系与各关节的关节坐标系同为左手系或右手系，且当旋转关节与在前的移动关节连接时，旋转关节的关节坐标系的原点与该在前的移动关节的关节坐标系的原点重合。

7.根据权利要求1所述的超声定位穿刺系统，其特征在于，所述探头坐标系的X、Y、Z轴的方向与所述末端关节的关节坐标系的X、Y、Z轴的方向相同，且所述探头坐标系的Z轴和所述末端关节的关节坐标系的Z轴共线。

8.根据权利要求1所述的超声定位穿刺系统，其特征在于，所述第一串联机械臂的末端关节为并联运动平台，所述并联运动平台包括静平台、动平台和多个设置在所述静平台与所述动平台之间的伸缩组件，所述超声探测装置设于所述动平台。

9.根据权利要求1所述的超声定位穿刺系统，其特征在于，所述超声定位穿刺系统还包括第二串联机械臂和搭载在所述第二串联机械臂末端的穿刺器械；所述计算机设备还分别与所述第二串联机械臂电性连接，所述处理器在运行所述计算机程序时还执行下列步骤：

将所述靶点在所述参考坐标系内的第二位置信息作为目标穿刺点，控制所述第二串联机械臂的穿刺器械对所述目标穿刺点进行穿刺。

10.一种存储介质，所述存储介质中存储有计算机程序，其特征在于，所述计算机程序被设置为运行时执行下列步骤：

在超声探测装置投射的超声探测平面内定位靶点的第一位置信息；

获取所述超声探测平面的第一姿态信息以及第一串联机械臂的第二姿态信息；

根据所述第一姿态信息和所述第二姿态信息，确定超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系；

根据所述位置转换关系和所述第一位置信息，确定所述靶点在所述参考坐标系内的第二位置信息；

其中，根据所述第一姿态信息和所述第二姿态信息，确定所述超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系包括：

在所述第一串联机械臂处于初始状态时，建立所述第一串联机械臂的参考坐标系和所述第一串联机械臂的各关节的关节坐标系，以及在所述超声探测装置处于初始状态时，以所述超声探测装置的末端端点为坐标原点建立探头坐标系；

获取所述第一串联机械臂的参考坐标系和所述第一串联机械臂的各关节的关节坐标系的DH参数，并根据所述DH参数，确定所述第一串联机械臂的参考坐标系到所述第一串联机械臂的末端关节的关节坐标系之间的第一位置转换关系；

获取所述探头坐标系的坐标原点在所述末端关节的关节坐标系内的位置信息，以及所述探头坐标系相对于所述末端关节的关节坐标系的姿态角，并根据该位置信息、姿态角以及所述第一位置转换关系，确定所述参考坐标系与所述探头坐标系之间的第二位置转换关系；

获取所述超声探测平面的旋转角度，并根据所述旋转角度确定所述超声探测平面所在坐标系与所述探头坐标系之间的第三位置转换关系；

根据所述第二位置转换关系和所述第三位置转换关系，确定所述超声定位穿刺系统的参考坐标系与所述超声探测平面所在坐标系的位置转换关系。

Images