手术机械臂精准定位方法、装置、控制终端及存储介质与流程

1.本发明涉及视觉精准定位领域，尤其涉及一种手术机械臂精准定位方法、装置、控制终端及存储介质。


背景技术：

2.在诸如口腔种植机器人手术过程中，机器人手术系统不论是在种植上颌缺损牙位，还是下颌缺损牙位的手术过程中，术中姿态变化不大，在有限的空间中，即便机械臂或者患者的位置发生偏移，机械臂与患者和外置视觉传感器之间的相对位置也不会有多大改变，为此传统的精准定位方式反而更加繁琐，并且在上述情况下，反而会不够精确。


技术实现要素：

3.第一方面，本技术提供一种手术机械臂精准定位方法，包括：当机械臂在规划的手术区域内运动时，获取设置在所述机械臂上的标记点在机械臂基座坐标系下的物理坐标点云；获取所述标记点在视觉空间坐标系下的视觉坐标点云；根据所述视觉坐标点云和所述物理坐标点云，获得所述视觉空间坐标系到基座坐标系的第一转换矩阵；根据所述第一转换矩阵将基座坐标系和所述视觉空间坐标系进行精准定位，以获得所述机械臂末端工具坐标系。
4.进一步的，所述第一转换矩阵包括旋转参数和平移参数；所述根据所述视觉坐标点云和所述物理坐标点云，获得所述视觉空间坐标系到所述机械臂的基座坐标系的第一转换矩阵包括：获取所述物理坐标点云与所述视觉坐标点云之间的最小旋转参数；根据所述最小旋转参数，获得所述平移参数；将所述最小旋转参数作为所述旋转参数，并结合所述平移参数得到所述第一转换矩阵。
5.进一步的，获取所述物理坐标点云与所述视觉坐标点云之间的最小旋转参数包括：分别获取所述物理坐标点云的物理质心，以及所述视觉坐标点云的视觉质心，建立所述物理质心和所述视觉质心的转换方程；分别计算所述视觉质心与每个所述视觉坐标点云的视觉质心偏差值，以及所述物理质心与每个所述物理坐标点云的物理质心偏差值；根据所述视觉质心偏差值、所述物理质心偏差值和所述转换方程，计算所述视觉坐标点云与所述物理坐标点云的总误差值；将所述总误差值最小时的旋转参数作为所述最小旋转参数。
6.进一步的，根据所述第一转换矩阵将基座坐标系和所述视觉空间坐标系进行精准定位，以获得所述机械臂末端工具坐标系包括：
获取所述机械臂的末端工具的工装数据，并通过所述工装数据计算所述末端工具与所述机械臂的末端法兰的偏移量；根据所述标记点在所述机械臂上的位置信息，以及所述标记点的工装信息，确定所述标记点相对所述末端法兰的偏移量；根据dh坐标系变换，获得所述机械臂的基座到所述末端法兰的第二转换矩阵；根据所述第二转换矩阵和所述第一转换矩阵，得到所述视觉空间坐标系到所述机械臂的末端法兰的第三转换矩阵；根据所述标记点相对所述末端法兰的偏移量和所述末端工具与所述末端法兰的偏移量，得到所述标记点到所述末端工具的第四转换矩阵；根据所述第三转换矩阵及所述第四转换矩阵得到所述机械臂的末端工具坐标系。
7.进一步的，所述获得所述机械臂的基座到所述末端法兰的第二转换矩阵包括：根据机器人正运动学，通过所述机械臂各个相邻电机之间的dh转换矩阵，得到所述第二转换矩阵。
8.进一步的，所述物理质心和所述视觉质心的转换方程表达式为：式中，为所述物理质心，为所述视觉质心，平移变量，表示对进行旋转变换。
9.进一步的，所述标记点为设置在所述机械臂上固定位置的反光球或定位块。
10.第二方面，本技术还提供一种固定姿态手术臂精准定位装置，包括：点云获取模块，用于当机械臂在规划的手术区域内运动时，获取设置在所述机械臂上的标记点在机械臂基座坐标系下的物理坐标点云；获取所述标记点在视觉空间坐标系下的视觉坐标点云；计算模块，用于根据所述视觉坐标点云和所述物理坐标点云，获得所述视觉空间坐标系到基座坐标系的第一转换矩阵；精准定位模块，用于根据所述第一转换矩阵将基座坐标系和所述视觉空间坐标系进行精准定位，以获得机械臂末端工具坐标系。
11.第三方面，本技术还提供一种控制终端，包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行所述的手术机械臂精准定位方法。
12.第四方面，本技术还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行所述的手术机械臂精准定位方法。
13.本发明实施例公开了一种手术机械臂精准定位方法、装置、控制终端及存储介质，包括：当机械臂在规划的手术区域内运动时，获取设置在所述机械臂上的标记点在机械臂基座坐标系下的物理坐标点云；获取所述标记点在视觉空间坐标系下的视觉坐标点云；根据所述视觉坐标点云和所述物理坐标点云，获得所述视觉空间坐标系到基座坐标系的第一转换矩阵；根据所述第一转换矩阵将基座坐标系和所述视觉空间坐标系进行精准定位，以获得所述机械臂末端工具坐标系。使得精准定位过程快速方便，并且针对小范围机械臂的移动，其精准定位精度更高。适合在小区域内的手术操作。
附图说明
14.为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。
15.图1示出了本技术实施例一种手术场景示意图；图2示出了本技术实施例一种手术机械臂精准定位方法流程示意图；图3示出了本技术实施例第一转换矩阵获取流程示意图；图4示出了本技术实施例一种手术机械臂精准定位装置结构示意图。
具体实施方式
16.下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
17.通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
18.在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
19.此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
20.除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。
21.本技术的技术方案应用于手术机械臂的精准定位，且针对在局部小范围空间的精准定位，通过在机械臂上配置可以给双目视觉传感器捕捉到的标记点，分别获取机械臂基座坐标系下的物理点云和视觉空间坐标系下的视觉点云，这两个点云坐标是同样的坐标点在不同坐标系下的坐标值，以此来计算得到双目视觉传感器到手术机械臂的转换矩阵，从而实现对手术机械臂的精准定位，来得到手术机械臂末端工具的坐标系。
22.如图1所示，为本技术的手术场景示意图。
23.该手术场景中包括一个双目视觉传感器100和手术机械臂200，该双目视觉传感器100的视觉范围囊括手术机械臂200和接下来将要进行手术的区域。
24.手术机械臂200的末端处安装有手术用的末端工具220，且在机械臂的末端附近，安装有一个标记点210。该标记点210可以是一个反光球或者定制的标记装置，用于让双目视觉传感器100通过获取该标记点210的坐标，以确定手术机械臂200的位置。该双目视觉传
感器100可以是双目摄像机，通过将拍摄到的图像数据传输给控制终端，使得控制终端可以控制手术机械臂200进行精准的手术，或者在手术前，对机械臂进行精准定位。
25.通过该双目视觉传感器100获取的标记点210的坐标，末端工具220的工装数据，标记点210所在位置的位置数据，来对该手术机械臂200进行精准定位，得到需要的末端工具坐标系。
26.接下来以具体实施例说明本技术的技术方案。
27.实施例1如图2所示，本技术的技术方案包括以下步骤：步骤s100，当机械臂在规划的手术区域内运动时，获取设置在所述机械臂上的标记点在机械臂基座坐标系下的物理坐标点云；获取所述标记点在视觉空间坐标系下的视觉坐标点云。
28.开始精准定位时，机械臂在手术区域内运动，需要说明的是，该手术区域为局部较小的区域，由术前规划时决定，手术机械臂经过频繁的运动，可以产生多个标记点所在的空间位置。其中，标记点的位置是已知的，设定在机械臂固定的位置，例如最后一节机械臂上中间之类的，由此，机械臂自身可以获得该标记点在机械臂基座坐标系下的物理坐标，所有的多个标记点坐标组成物理坐标点云。
29.同样的，还要获取这些点在视觉空间坐标系下的坐标，可以通过双目视觉传感器对机械臂拍摄，机械臂每运动一次，就记录下所拍摄到的标记点位置，可以得到标记点在视觉空间坐标系下的视觉坐标，从而得到视觉坐标点云。为了方便表述，视觉坐标点云记为，物理坐标点云记为。可以理解的是，这两个点云实际上是不同坐标系下，对同一个点云的不同描述，因此，这两个点云之间存在如下转换关系：其中，为物理坐标点云到视觉坐标点云的转换矩阵，也是上述机械臂基座坐标系和视觉空间坐标系的转换矩阵，即后续步骤求解的第一转换矩阵。
30.其中，这些点云不都在一条直线上，即其中至少三个点不共线，以尽可能的产生在空间上的差异。
31.步骤s200，根据所述视觉坐标点云和所述物理坐标点云，获得所述视觉空间坐标系到基座坐标系的第一转换矩阵。
32.如图3所示，本步骤包括以下步骤：步骤s210，获取所述物理坐标点云与所述视觉坐标点云之间的最小旋转参数。
33.可以理解的是，将上述第一转换矩阵t展开可得形式的矩阵，其中，r*为旋转参数，r0为平移参数，求出旋转参数和平移参数，就可以得到第一旋转矩阵，为此，需要先获取最小旋转参数。
34.分别获取所述物理坐标点云的物理质心，以及所述视觉坐标点云的视觉质心，建立所述物理质心和所述视觉质心的转换方程。
35.其中转换方程表达式为：
式中，为所述物理质心，为所述视觉质心，平移变量，表示对进行旋转变换，也相当于上述的旋转参数r*。
36.然后再分别计算所述视觉质心与每个所述视觉坐标点云的视觉质心偏差值，以及所述物理质心与每个所述物理坐标点云的物理质心偏差值。
37.设视觉坐标实际点与视觉质心偏差量为，视觉坐标实际点为，实际物理点与物理质心偏差量为实际物理点为，则存在如下关系式：因为为质心，所以在坐标系中，点云中其他点到该质心的偏差值为0，所以存在以下关系式：同理，也存在如下关系式。同理，也存在如下关系式。
38.由此，和上述的转换方程结合，可得如下关系式：即对于上述的偏差量之间的转换关系和质心之间的转换关系是相同的，从而可以通过偏差值来代表质心。
39.然后，根据所述视觉质心偏差值、所述物理质心偏差值和所述转换方程，计算所述视觉坐标点云与所述物理坐标点云的总误差值。
40.误差值关系式如下：进而得到总误差值的关系式：其中，为误差值，表示在旋转参数r*和平移参数r0下，视觉质心和物理质心的理论值的误差。如果旋转参数r*和平移参数r0完全符合，则理论上来说，该误差值是0，然而点云中有多个点，可能存在旋转参数r*和平移参数r0符合第i个点之间的装换关系，然是不符合第i+1个点的转换关系，使得误差值不为0。
41.将所述总误差值最小时的旋转参数作为所述最小旋转参数。
42.以旋转参数r*和平移参数r0为变量，计算总误差值最小时，旋转参数r*和平移参数r0的取值认为是最优解。
43.展开上述的总误差值的关系式可得：
其中，第二项和第三项为固定值，因此总误差值的大小和第一项有关，计算第一项的最小值，从而得到上述的最小旋转参数r*。也就是说，找到了使得两个点云之间，转换误差最小的旋转参数，使得整体的转换结果最优。
44.步骤s220，根据所述最小旋转参数，获得所述平移参数。
45.得到最小旋转参数后，根据上述公式可得，从而得到平移参数。
46.步骤s230，将所述最小旋转参数作为所述旋转参数，并结合所述平移参数得到所述第一转换矩阵。
47.计算得到了旋转参数和平移参数后，就得到了视觉空间坐标系到机械臂基座坐标系的旋转和平移关系，也就得到的上述的第一转换矩阵。
48.步骤s300，根据所述第一转换矩阵将基座坐标系和所述视觉空间坐标系进行精准定位，以获得所述机械臂末端工具坐标系。
49.得到第一转换矩阵，就确立了机械臂和双目视觉传感器之间的关系，使得机械臂基座坐标系和双目视觉空间坐标系下的坐标可以相互转换，但是为了精准的控制机械臂工作，还是需要知道机械臂末端工具坐标系，来把握机械臂末端工具的位置。
50.可以根据dh转换，获得所述机械臂的基座到所述末端法兰的第二转换矩阵。根据机器人正运动方程以及dh转换矩阵，可以得到机械臂的基座坐标系到机械臂的末端法兰的转换关系，例如一个6轴机械臂，则存在：式中a1至a6表示机器人各轴间的dh转换矩阵，b代表机械臂的基座的坐标系，e代表机械臂的末端法兰坐标系，则是机械臂的基座到机械臂的末端法兰的转换关系，也就是所述的第二转换矩阵。
51.由此根据所述第二转换矩阵和所述第一转换矩阵，得到所述双目视觉传感器到所述机械臂的末端法兰的第三转换矩阵。由此，可以得到机械臂末端法兰的坐标系。
52.然而在实际的操作过程中，而对于双目视觉传感器来讲，其拍摄到的图像中，对于机械臂的掌控是依靠在机械臂上配置的标记点。根据所述标记点在所述机械臂上的位置信息，以及所述标记点的工装信息，确定所述标记点相对所述末端法兰的偏移量。
53.同时，获取所述机械臂的末端工具的工装数据，并通过所述工装数据计算所述末端工具与所述机械臂的末端法兰的偏移量。
54.这样，通过上述两个偏移量，可以得到所述标记点到所述末端工具的第四转换矩阵。
55.由此，在双目视觉传感器实际拍摄到标记点的位置后，可以通过第三转换矩阵和第四转换矩阵，计算得到机械臂末端工具的位置，并建立起视觉空间坐标系和末端工具坐标系之间的转换关系，从而得到机械臂末端工具坐标系。
56.具体而言，存在如下关系式：
式中，为所述标记点的坐标，为所述标记点坐标的转置矩阵，为所述第二转换矩阵，为所述第一转换矩阵，为所述标记点相对所述末端法兰的偏移量，为所述标记点相对所述末端法兰的偏移量的转置矩阵。
57.其中，第三转换矩阵就是第一转换矩阵和第二转换矩阵相乘计算得出，即：其中，为第三转换矩阵，由此，从上述双目视觉传感器中所拍摄到的图像坐标可以通过该第三转换矩阵，转换到机械臂末端坐标系下的坐标。
58.第四转换矩阵是标记点到末端工具的转换矩阵，因为标记点到末端法兰的偏移量和末端工具到末端法兰的偏移量都是可以测量得到的，且两者都是针对末端法兰的偏移量，因此可以通过对标记点的工装数据以及末端工具的工装数据，计算得到对应的第四转换矩阵。
59.第四转换矩阵的表达式为：式中，为所述末端工具与所述机械臂末端法兰的偏移量，为所述标记点相对所述机械臂的法兰的偏移量，为所述第四转换矩阵。
60.这里的第四转换矩阵是用于得到末端工具尖端的坐标的。
61.得到上述四个转换矩阵后，就完成了对手术机械臂的精准定位，在手术过程中，双目摄像机拍摄机械臂，通过标记点，获取手术机械臂的坐标，此时获得的坐标在视觉坐标系下，通过第一转换矩阵和第二转换矩阵得到的第三转换矩阵，可以得到在机械臂末端坐标系下，该标记点的坐标，再通过第四转换矩阵，可以计算得到末端工具的空间坐标，从而可以建立末端工具坐标系，实现精准定位。
62.本发明实施例公开了一种手术机械臂精准定位方法、装置、控制终端及存储介质，包括：当机械臂在规划的手术区域内运动时，获取设置在所述机械臂上的标记点在机械臂基座坐标系下的物理坐标点云；获取所述标记点在视觉空间坐标系下的视觉坐标点云；根据所述视觉坐标点云和所述物理坐标点云，获得所述视觉空间坐标系到基座坐标系的第一转换矩阵；根据所述第一转换矩阵将基座坐标系和所述视觉空间坐标系进行精准定位，以获得所述机械臂末端工具坐标系。使得精准定位过程快速方便，并且针对小范围机械臂的移动，其精准定位精度更高。更适用于局部小区域的手术场景。
63.实施例2本技术还提供一种固定姿态手术臂精准定位装置，如图4所述包括：点云获取模块10，用于当机械臂在规划的手术区域内运动时，获取设置在所述机械臂上的标记点在机械臂基座坐标系下的物理坐标点云；获取所述标记点在视觉空间坐标系下的视觉坐标点云；计算模块20，用于根据所述视觉坐标点云和所述物理坐标点云，获得所述视觉空间坐标系到基座坐标系的第一转换矩阵；精准定位模块30，用于根据所述第一转换矩阵将基座坐标系和所述视觉空间坐标
系进行精准定位，以获得所述机械臂末端工具坐标系。
64.本技术还提供一种控制终端，包括处理器和存储器，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行所述的手术机械臂精准定位方法。
65.本技术还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行所述的手术机械臂精准定位方法。
66.在本技术所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
67.另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
68.所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是智能手机、个人计算机、服务器、或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器（rom，read-only memory）、随机存取存储器（ram，random access memory）、磁碟或者光盘等各种可以存储程序代码的介质。
69.以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。