CN115778445A

CN115778445A - 一种机械臂末端激光指引验证方法

Info

Publication number: CN115778445A
Application number: CN202211561867.5A
Authority: CN
Inventors: 曹红洋; 龚肖
Original assignee: Tuodao Medical Technology Co Ltd
Current assignee: Tuodao Medical Technology Co Ltd
Priority date: 2022-12-07
Filing date: 2022-12-07
Publication date: 2023-03-14

Abstract

本发明公开了一种机械臂末端激光指引验证方法，包括步骤：(1)通过光学跟踪系统识别机械臂末端上示踪器位姿及用于表征激光发射器发射的激光位姿的探测器上示踪器位姿，进而计算得到激光发射器发射的激光在机械臂末端上示踪器对应坐标系下的位姿；(2)根据激光发射器发射的激光位姿通过右手定则建立激光坐标系，据此计算得到激光坐标系与机械臂末端上示踪器对应坐标系之间的变换关系；(3)根据步骤(2)中激光坐标系与机械臂末端上示踪器对应坐标系之间的变换关系进行激光指引验证。本发明可以对激光指引进行验证。

Description

一种机械臂末端激光指引验证方法

技术领域

本发明涉及机器人技术领域，尤其涉及一种机械臂末端激光指引验证方法。

背景技术

肺癌是对人生命和健康危险最大的癌症之一，肺部活检是肺部肿瘤诊断的金标准。肺部活检手术需要具有丰富临床经验的医生取出病灶进行病理分析。在肺部活检手术中，采用手术导航系统进行指引操作，手术导航系统主要原理就是术前的CT影像坐标系和术中的空间坐标系进行配准，从而实现将手术器械实时显示在术前的CT影像坐标空间。

肺部穿刺活检是属于有创检查，患者有一定的痛苦，因而需要局部麻醉来减轻患者的痛苦，麻醉在病灶越近，则麻醉效果越好。为了提高医生麻醉的精准性，需要在导航设备上安装激光灯，指示病灶在人体表面的位置和方向，为了激光有精确的指引病灶的作用，需要计算出激光坐标系和末端示踪器坐标系的精确关系，但是由于加工和安装的误差，需要我们对激光坐标系和末端示踪器坐标系进行进一步验证，从而对激光指引做指引验证。

发明内容

发明目的：针对上述不足，本发明提出一种机械臂末端激光指引验证方法，可以得到激光坐标系和末端示踪器坐标系之间的变换关系，进而对激光指引进行验证。

技术方案：

一种机械臂末端激光指引验证方法，包括步骤：

(1)通过光学跟踪系统识别机械臂末端上示踪器位姿及用于表征激光发射器发射的激光位姿的探测器上示踪器位姿，进而计算得到激光发射器发射的激光在机械臂末端上示踪器对应坐标系下的位姿；

(2)根据激光发射器发射的激光位姿通过右手定则建立激光坐标系，据此计算得到激光坐标系与机械臂末端上示踪器对应坐标系之间的变换关系；

(3)根据步骤(2)中激光坐标系与机械臂末端上示踪器对应坐标系之间的变换关系进行激光指引验证。

所述步骤(1)中，所述探测器通过其尖端指引激光点位置以表征激光发射器发射的激光位姿。

所述步骤(1)中，保持激光发射器位置不动并通过不同的平面得到在机械臂末端上示踪器对应坐标系下的不同激光点位置的坐标，以不同激光点构成直线即为激光发射器发射的激光，进而得到其位姿。以不同激光点构成直线为采用加权最小二乘法对各激光点进行拟合得到的直线。

所述建立激光坐标系具体包括：

1)以不同激光点构成直线作为激光坐标系的z轴；

2)选取所述z轴上一点作为原点，按照右手定则建立与所述机械臂末端上建立的末端坐标系对齐的激光坐标系；其中，所述末端坐标系以在机械臂末端上导航通道轴线作为z轴，所述激光坐标系与末端坐标系对齐为二者x轴或y轴夹角最小。

在得到激光坐标系的z轴后，判断机械臂末端坐标系的z轴向量与步骤1)中激光坐标系的z轴向量内积是否小于0；若小于，则对激光坐标系的z轴进行反向；若不小于，则不变，从而确定激光坐标系的z轴的正向。

所述建立激光坐标系具体为：选取所述z轴上一点作为原点，按照右手定则选取其中垂直于激光坐标系z轴且与机械臂末端坐标系的y轴或x轴之间夹角最小的一条直线方向作为激光坐标系对应的坐标轴，通过右手定则确定激光坐标系的另一坐标轴，进而得到激光坐标系。

选取其中垂直于激光坐标系z轴且与机械臂末端坐标系的y轴或x轴之间夹角最小的一条直线方向作为激光坐标系对应的坐标轴具体为：

随机选取垂直于激光坐标系z轴的一条直线方向作为候选y轴或x轴，并使其以1度为步长，绕激光坐标系z轴旋转360度，每次旋转后计算当前候选y轴或x轴所在向量与机械臂末端坐标系对应坐标轴所在向量之间的内积，若该内积小于0，则将其剔除；若该内积大于0，计算两向量的夹角余弦值；

得到其中夹角余弦值最大时对应的候选y轴或x轴即为最终激光坐标系的y轴或x轴。

所述机械臂末端上示踪器有多组，通过光学跟踪系统识别各示踪器位姿，并根据计算得到的其中一组示踪器对应坐标系与激光坐标系之间的变换关系计算得到其他组示踪器对应坐标系与激光坐标系之间的变换关系。

根据步骤(2)中激光坐标系与机械臂末端上示踪器对应坐标系之间的变换关系控制机械臂运动使得激光发射器发射的激光位姿发生改变，根据激光的实际位姿与目标位姿偏差是否在设定范围内对激光指引进行验证。

有益效果：本发明可以建立激光坐标系与末端示踪器之间的精确关系，可以得到激光坐标系和末端示踪器坐标系的变换关系，进而对激光指引进行验证。

附图说明

图1为本发明的机械臂末端激光指引验证的架构图；

图2为验证工装的结构示意图；

图3为本发明的机械臂末端激光指引验证方法的流程图。

其中，1.机械臂末端，11.发光二极管，12.激光灯；

2.探针，21.示踪球，22.尖点；

3.光学跟踪系统；

4.验证工装，41.工装示踪球，42.验证通道，43.验证通道平面圆。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

本发明的机械臂末端激光指引验证架构如图1所示，包括安装于机械臂末端1上的激光灯12、用于指向激光灯12发射的激光点的探针2、用于识别机械臂末端1上和探针2上安装的示踪器位姿的光学跟踪系统3及用于激光指引验证的验证工装4以及根据三维影像中的验证通道及验证工装上通道计算得到激光灯12发射的激光的目标位姿并据此控制机械臂运动的上位机。

机械臂末端如图1所示，在机械臂末端1上安装有若干组末端示踪器，现有技术中，示踪器通常包括反光球，以便于提升视觉导航精度，但是，反光球存在体积大、占用地方，而且在消毒使用过程中极容易损坏的缺点，因此，本发明的各组末端示踪器中的示踪球采用发光二极管11，每一组末端示踪器均包含至少三个共面不共线的发光二极管11，且各组末端示踪器所在平面不共面，具体地，本发明具体实施例中，末端示踪器有三组，每组包含有四个发光二极管11，分别位于机械臂末端的两侧面及上顶斜面处，如图1所示；具体地，定义三组末端示踪器A、B、S对应的发光二极管11分别为A1、A2、A3、A4、B1、B2、B3、B4、S1、S2、S3、S4(图中未示出)；其中，A1、A2、A3、A4共面安装，组成一个刚体，记为末端示踪器A，建立对应的末端示踪器坐标系C_A，B1、B2、B3、B4共面安装，组成一个刚体，记为末端示踪器B，建立对应的末端示踪器坐标系C_B，S1、S2、S3、S4共面安装，组成一个刚体，记为末端示踪器S，建立对应的末端示踪器坐标系C_S。

激光灯12安装于机械臂末端1的下端面上，用于发出激光以进行指引。在探针2上固定安装有用于示踪探针2位姿的探针示踪器，探针示踪器采用至少三个共面不共线的示踪球21，具体地，示踪球21为4个，均固定于探针2上；探针2的末端为其尖点22，通过其尖点22指向激光灯12发射的激光在相应平面上的激光点，尖点22相对于探针示踪器的相对位置固定，光学跟踪系统3识别探针示踪器的位置即可得到激光点的位置。光学跟踪系统3用于实时获取并识别机械臂末端1上的末端示踪器和探针2上的探针示踪器的位姿，并发送至上位机。验证工装4上安装有工装示踪器41、若干验证通道42及若干验证通道平面圆43，工装示踪器41可以采用探针2上的示踪器；工装示踪器41非对称地安装于验证工装4上，用于三维影像与光学跟踪系统之间的配准；验证通道42设置于验证工装4的不同平面上，且验证通道42的上表面有验证通道的平面圆43，用于验证激光点与工装表面的交点是否在平面圆内，若在平面圆内表示激光点在精度范围内，满足激光精度要求。

上位机，根据在不同的平面采集不同的激光点得到激光坐标系C₂的z轴，根据右手定则构建得到待选激光坐标系，并根据激光坐标系C₂与机械臂末端坐标系C₁进行对齐并更新激光坐标系得到最终的激光坐标系C₂，计算机械臂末端上末端示踪器所对应坐标系与最终得到的激光坐标系C₂之间的变换关系。激光指引阶段通过控制机械臂运动实现激光在病人体表合适的位置打出激光点指引医生操作，执行阶段通过控制机械臂运动实现导航通道与规划通道重合实现，因此将激光坐标系C₂与机械臂末端坐标系C₁进行对齐可使得激光指引阶段与执行阶段对应的机械臂运动的逆解选解策略都一致，便于两阶段使用共同的机械臂运动逆解选解策略，降低开发难度；

其中，计算机械臂末端上末端示踪器所对应坐标系与最终得到的激光坐标系C₂之间的变换关系是在保持机械臂位置不变使得其上激光灯位置不动，据此根据上述建立激光坐标系，得到该末端示踪器所对应坐标系与激光坐标系之间的变换关系，上位机根据扫描验证工装得到的三维影像上的示踪器进行配准得到三维影像与光学跟踪系统之间的变换关系，选择三维影像中一验证通道作为激光需要指引的目标位姿，进而根据三维影像与光学跟踪系统之间的变换关系及机械臂末端上某一末端示踪器所对应坐标系与激光坐标系之间的变换关系控制机械臂运动，使得其上的激光灯发出的激光到达前述得到的目标位姿，判断激光灯发射的激光能否打在验证工装上对应的验证通道平面圆内，进而来验证激光指引的精确性。

本发明的机械臂末端激光指引验证方法如图3所示，包括如下步骤：

(1)调整机械臂，使得其上某一组末端示踪器正对光学跟踪系统，机械臂末端上的激光灯发射激光至一平面上得到对应激光点，并通过探针的尖点指向激光点，使得探针尖点坐标和激光点坐标相同；

(2)通过光学跟踪系统识别该末端示踪器及探针示踪器，并分别得到探针示踪器及该末端示踪器所对应坐标系与光学跟踪系统坐标系之间的变换关系，记为T₀和T₁；同时根据探针设计参数计算得到尖点在光学跟踪系统的坐标p_t，进而计算得到激光点在该末端示踪器所对应坐标系下的坐标p_j＝T₁ ^-1*T₀*p_t；

本发明具体实施例中，选取其中末端示踪器B(即机械臂末端的上顶斜面处的末端示踪器)正对光学跟踪系统；

(3)构建激光坐标系C₂；

为了提供精准的激光指引，就需要依据激光灯发射的激光点建立对应的激光坐标系并计算得到激光坐标系相对于步骤(1)中末端示踪器对应坐标系的变换关系，具体如下：

(31)在步骤(1)的基础上保持激光灯位置不动并通过不同的平面得到不同的激光点，并根据步骤(2)计算得到各激光点在该末端示踪器所对应坐标系下的坐标，采用加权最小二乘法对各激光点进行拟合得到直线，以该直线作为激光坐标系的z轴；

在本步骤中，为了保证指引阶段与执行阶段对应的机械臂运动的逆解选解策略都一致，激光坐标系与机械臂末端坐标系之间各对应坐标轴之间夹角越小越能降低算法开发难度，也即需要对激光坐标系与机械臂末端坐标系进行对齐，因此还需要判断该激光坐标系z轴的正向，具体为根据实际操作过程中需要保证机械臂末端上导航通道与激光坐标系的z轴夹角为锐角，根据前述方式在机械臂末端上导航通道建立机械臂末端坐标系，其z轴为导航通道轴线，那么激光坐标系的z轴(即拟合得到的直线)向量为Z₁，机械臂末端坐标系的z轴向量为Z₂，判断两个向量的内积是否小于0，若小于，则对激光坐标系的z轴进行反向，若不小于，则不变，从而可以确定激光坐标系的z轴的正向；

其中，机械臂末端坐标系C₁是以机械臂末端上的导航通道轴线作为z轴，根据右手定则建立的坐标系；更具体地，机械臂末端坐标系C₁的各个轴可通过三坐标测量得出，其中以在机械臂末端上导航通道轴线作为z轴、以垂直于机械臂末端两侧面的直线作为x轴、以垂直于机械臂末端z轴和x轴所在平面的直线作为y轴、以导航通道的上表面与导航通道轴线的交点为原点，按照右手定则建立机械臂末端坐标系C₁。

(32)选取激光坐标系z轴上一点作为原点，按照右手定则建立与所述机械臂末端坐标系对齐的激光坐标系，从而得到步骤(1)的末端示踪器所对应坐标系与激光坐标系之间的变换关系T₁′；

本发明一实施例中，建立与所述机械臂末端坐标系对齐的激光坐标系可以采用如下方式：选取激光坐标系z轴上一点作为原点按照右手定则确定x轴、y轴，建立得到候选激光坐标系，再将候选激光坐标系绕着其z轴进行旋转并使其x轴或y轴与所述机械臂末端坐标系对应坐标轴夹角最小，从而得到最终激光坐标系。

在本发明另一实施例中，建立与所述机械臂末端坐标系对齐的激光坐标系还可以采用选取激光坐标系z轴上一点作为原点，按照右手定则选取其中垂直于激光坐标系z轴且与机械臂末端坐标系的y轴之间夹角最小的一条直线方向作为激光坐标系的y轴，通过右手定则确定激光坐标系的x轴，进而得到激光坐标系；

选取其中垂直于激光坐标系z轴且与机械臂末端坐标系的y轴之间夹角最小的一条直线方向作为激光坐标系的y轴具体为：随机选取垂直于激光坐标系z轴的一条直线方向作为候选y轴，并使其以1度为步长，绕激光坐标系z轴旋转360度，每次旋转后计算当前候选y轴所在向量与机械臂末端坐标系的y轴所在向量之间的内积，若该内积小于0，则说明当前候选y轴和机械臂末端坐标系的y轴成钝角，将其剔除；若该内积大于0，计算两向量的夹角余弦值；得到其中夹角余弦值最大也即两向量的夹角最小时对应的候选y轴即为最终激光坐标系的y轴。

除了上述得到激光坐标系的实施例，在本发明的其他实施例中，也可以在确定激光坐标系z轴后，先选取一条垂直于激光坐标系z轴的直线作为候选激光坐标系的x轴并进行激光坐标系与机械臂末端坐标系对齐计算确定最终激光坐标系的x轴(对齐计算方式参照上述y轴绕着激光坐标系z轴进行旋转计算过程)，并以得到的激光坐标系z轴、最终激光坐标系的x轴确定激光坐标系y轴，最后得到最终激光坐标系。

本发明中，在得到激光坐标系的x轴和y轴后，分别对其坐标轴进行向量归一化。

(4)计算其它末端示踪器所对应坐标系与激光坐标系之间的变换关系；

机械臂末端还有其它组末端示踪器，因此也需要确定在其它组末端示踪器对应的坐标系与激光坐标系之间的变换关系；

通过光学跟踪系统分别识别得到其他组末端示踪器的位姿，并得到其中一组末端示踪器(定义为K组)所对应坐标系与光学跟踪系统坐标系之间的变换关系T_K，并结合步骤(3)的末端示踪器所对应坐标系与激光坐标系之间的变换关系T₁′、光学跟踪系统坐标系与上述步骤(2)中得到的相应末端示踪器所对应坐标系之间的变换关系T₁，那么即可得到K组末端示踪器所对应坐标系与激光坐标系之间的变换关系T_K′＝T_K ^-1*T₁*T₁′；

(5)激光校准验证；

采用如图2的验证工装进行激光指引验证，验证工装上安装有工装示踪器、若干验证通道及若干验证通道平面圆，对该验证工装进行CT扫描得到其三维影像，通过识别三维影像中验证工装上的示踪器结合光学跟踪系统识别得到的工装示踪器位姿以及验证工装设计参数进行配准得到三维影像与光学跟踪系统之间的变换关系，选择三维影像中一验证通道作为激光需要指引的目标位姿，光学跟踪系统识别机械臂末端上其中一组末端示踪器，进而根据三维影像与光学跟踪系统之间的变换关系及该组末端示踪器所对应坐标系与激光坐标系之间的变换关系控制机械臂运动，使得其上的激光灯发射的激光运动到达其目标位姿，此时判断激光灯发射的激光能否打在验证工装上对应的验证通道上，进而来验证激光指引的精确性。

本发明通过探针指引激光灯发射的激光点，并根据不同平面上激光点位置得到激光点位姿构建激光坐标系，从而可以建立激光坐标系与末端示踪器之间的精确关系，据此对激光指引进行验证。

以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。

Claims

1.一种机械臂末端激光指引验证方法，其特征在于：包括步骤：

2.根据权利要求1所述的机械臂末端激光指引验证方法，其特征在于：所述步骤(1)中，所述探测器通过其尖端指引激光点位置以表征激光发射器发射的激光位姿。

3.根据权利要求2所述的机械臂末端激光指引验证方法，其特征在于：所述步骤(1)中，保持激光发射器位置不动并通过不同的平面得到在机械臂末端上示踪器对应坐标系下的不同激光点位置的坐标，以不同激光点构成直线即为激光发射器发射的激光，进而得到其位姿。

4.根据权利要求3所述的机械臂末端激光指引验证方法，其特征在于：以不同激光点构成直线为采用加权最小二乘法对各激光点进行拟合得到的直线。

5.根据权利要求3所述的机械臂末端激光指引验证方法，其特征在于：所述建立激光坐标系具体包括：

1)以不同激光点构成直线作为激光坐标系的z轴；

6.根据权利要求5所述的机械臂末端激光指引验证方法，其特征在于：在得到激光坐标系的z轴后，判断机械臂末端坐标系的z轴向量与步骤1)中激光坐标系的z轴向量内积是否小于0；若小于，则对激光坐标系的z轴进行反向；若不小于，则不变，从而确定激光坐标系的z轴的正向。

7.根据权利要求6所述的机械臂末端激光指引验证方法，其特征在于：所述建立激光坐标系具体为：选取所述z轴上一点作为原点，按照右手定则选取其中垂直于激光坐标系z轴且与机械臂末端坐标系的y轴或x轴之间夹角最小的一条直线方向作为激光坐标系对应的坐标轴，通过右手定则确定激光坐标系的另一坐标轴，进而得到激光坐标系。

8.根据权利要求6所述的机械臂末端激光指引验证方法，其特征在于：选取其中垂直于激光坐标系z轴且与机械臂末端坐标系的y轴或x轴之间夹角最小的一条直线方向作为激光坐标系对应的坐标轴具体为：

9.根据权利要求1所述的机械臂末端激光指引验证方法，其特征在于：所述机械臂末端上示踪器有多组，通过光学跟踪系统识别各示踪器位姿，并根据计算得到的其中一组示踪器对应坐标系与激光坐标系之间的变换关系计算得到其他组示踪器对应坐标系与激光坐标系之间的变换关系。

10.根据权利要求1～9任一所述的机械臂末端激光指引验证方法，其特征在于：根据步骤(2)中激光坐标系与机械臂末端上示踪器对应坐标系之间的变换关系控制机械臂运动使得激光发射器发射的激光位姿发生改变，根据激光的实际位姿与目标位姿偏差是否在设定范围内对激光指引进行验证。