本发明公开了一种基于两个双目视觉系统的手术器械无遮挡定位方法，包括以下步骤：(1)把A安装于机械臂前端，与机械臂一起运动；(2)把B固定于机械臂支架中部，并在A上固定一标记物，用于实现B对A的定位；(3)获取手术器械在A坐标系下的位置，并根据A在B中的位置确定手术器械在世界坐标系下的位置；(4)当有障碍物在手术器械前方造成遮挡时，A在机械臂控制下绕开障碍物，获取手术器械在新的A坐标系下的位置；(5)B再次对运动后的A定位，并通过空间坐标变换即可实现对手术器械的无遮挡定位。具有有效的解决了手术过程中标记物易被遮挡的问题和降低了对机械臂运动精度的要求等优点。
1.一种基于两个双目视觉系统的手术器械无遮挡定位方法，基于双目视觉系统，所述双目视觉系统包括两个摄像头和支架，所述支架用于固定摄像头的位置；所述的两个双目视觉系统包括，双目视觉系统A和双目视觉系统B，以下用A表示双目视觉系统A，用B表示双目视觉系统B；所述基于两个双目视觉系统的手术器械无遮挡定位方法包括以下步骤：(1)把A安装于机械臂前端，并建立A坐标系和世界坐标系；
(2)把B固定于机械臂支架中部的合适位置上，并建立B坐标系；
(3)获取手术器械在A坐标系下的位置，以及A在B坐标系下的位置；
(4)有障碍物遮挡时，A在机械臂控制下在B的有效视场内绕开障碍物；
(5)运动后，再次获取手术器械在A坐标系下的位置，以及A在B坐标系下的位置；
(6)根据空间坐标变换得到手术器械在世界坐标系下的位置；
所述的步骤(2)中，合适位置是指，把B尽可能地放到能保证A在B的有效视场内绕开障碍物运动的位置上，即先预判A可能的移动范围，再确定B的位置，确保B的有效视场能尽可能大的覆盖A的移动范围；所述的有效视场，指在双目视觉系统的两个摄像机均能拍摄到的区域中，实现对物体精确定位的区域。
2.根据权利要求1所述的基于两个双目视觉系统的手术器械无遮挡定位方法，其特征在于，所述的步骤(3)中，通过在手术器械上加入标记点以及在A下方加入标记物，来获取手术器械在A坐标系下的位置以及A在B坐标系下的位置。
3.根据权利要求2所述的基于两个双目视觉系统的手术器械无遮挡定位方法，其特征在于所述的在A下方加入标记物，其标记物为一正四面体，当A中两摄像机的光轴与地面水平时，此正四面体呈倒立状态，即有1个与地面平行的上表面及3个斜向下的侧面；此正四面体的4个顶点上各有1个标记点，此正四面体的3个侧棱上分别有1个、2个、3个标记点，所述标记点不包括顶点，这些标记点分别对3个侧棱二等分、三等分、四等分；从而B可以根据所拍到的正四面体标记物上的标记点排列来确定A的位置和方向。
4.根据权利要求1所述的基于两个双目视觉系统的手术器械无遮挡定位方法，其特征在于，所述的步骤(4)中，所述的有效视场指：在两个摄像机均能拍到的区域中，能够实现对手术器械精确定位的区域；所述的A在机械臂控制下在B的有效视场内绕开障碍物，其原理如下：机械臂后端的舵机M1，用于控制整个机械臂绕固定支架左右旋转，舵机M2和M3联合运动用于控制机械臂上下前后运动，我们默认M1用于控制A的左右位置，M2用于控制A的上下位置，M3用于控制A的前后位置，舵机M4用于控制A的上下方向，M5用于控制A的左右方向；当有障碍物遮挡时，M1、M2根据障碍物位置转动从而控制A的位置，M3、M4、M5根据手术器械位置转动从而控制A的方向和距离。
5.根据权利要求4所述的基于两个双目视觉系统的手术器械无遮挡定位方法，所述的A在机械臂控制下在B的有效视场内绕开障碍物，其控制过程包括如下步骤：(1)根据新拍的图像上障碍物位置控制M1、M2上下左右旋转，使得障碍物离开A的有效视场；
(2)转动后，根据手术器械在A坐标系下的位置，确定M3、M4、M5的转动方式，确保手术器械不离开A的有效视场；
(3)一直重复步骤(1)至(2)，直到A绕开障碍物获取手术器械位置为止，同时，整个过程中A下方的标记物必须在B的有效视场内。
6.根据权利要求1所述的一种基于两个双目视觉系统的手术器械无遮挡定位方法，其特征在于，所述的步骤(6)中，所述的根据空间坐标变换得到手术器械在世界坐标系下的位置，具体指：B根据A下方的正四面体标记物得到A相对于其初始化状态下的旋转平移矩阵，初始状态下A坐标系与世界坐标系重合，从而可以对位置改变后的A所获取的手术器械的位置进行旋转平移变换，得到其在世界坐标系下的位置。
一种基于两个双目视觉系统的手术器械无遮挡定位方法
技术领域
[0001] 本发明涉及一种手术导航技术，特别涉及一种基于两个双目视觉系统的手术器械无遮挡定位方法。
背景技术
[0002] 手术导航中的光学定位设备一般是由两个定焦摄像机固定在一个支架上，形成一个固定的有效视场，手术器械在摄像机的有效视场范围内运动即可被精确定位到。但目前的光学定位技术有两个主要的缺点：1)光学成像设备受摄像机有效视场限制，使得手术必须在摄像机的有效视场范围内完成。2)手术中光线容易被遮挡。医生只能调整成像设备或者手术器械到一个合理的位置来完成定位，给医生带来了很大的不便。这两个缺点严重地影响了光学定位技术的使用。目前，对于摄像机有效视场小的问题，一般采用广角镜头获取更大的视场来解决。而对于术中光线容易被遮挡的问题，仍没有人提出有效的解决办法。
[0003] 机械臂是目前在机械人技术领域中得到最广泛实际应用的自动化机械装置，在工业制造、医学治疗、娱乐服务、军事、半导体制造以及太空探索等领域都能见到它的身影。尽管它们的形态各有不同，但它们都有一个共同的特点，就是能够接受指令，精确地定位到三维(或二维)空间上的某一点进行作业。一般机械臂的价格主要受其运动的精度和负重能力所影响。精度高、负重能力强的机械臂价格昂贵；相反的，精度低、负重能力弱的机械臂价格相对便宜。
发明内容
[0004] 本发明的目的在于克服现有技术中光学定位技术在术中光线容易被遮挡的的缺点与不足，提供一种基于两个双目视觉系统的手术器械无遮挡定位方法，该手术器械无遮挡定位方法有效解决手术过程中标记物易被遮挡的问题，同时降低了对机械臂运动精度的要求，减少了机械臂成本，在手术导航领域有重要的应用价值。
[0005] 本发明的目的通过下述技术方案实现：一种基于两个双目视觉系统的手术器械无遮挡定位方法，所述双目视觉系统包括两个摄像头以及固定它们位置的支架组成；所述的两个双目视觉系统包括，双目视觉系统A(下面简称为A)和双目视觉系统B(下面简称为B)；所述的手术器械无遮挡定位方法，其原理如下：使用具有5个以上自由度的机械臂控制A运动，使A能绕开障碍物，获取手术器械在A坐标系下的位置；把B固定于机械臂支架中部，用于定位A的位置，最后通过空间坐标变换实现对手术器械的无遮挡定位。该基于两个双目视觉系统的手术器械无遮挡定位方法可以包括以下步骤：
[0006] (1)把双目视觉系统A(下面简称为A)安装于机械臂前端，并建立A坐标系和世界坐标系；
[0007] (2)把双目视觉系统B(下面简称为B)固定于机械臂支架中部的合适位置上，并建立B坐标系；
[0008] (3)获取手术器械在A坐标系下的位置，以及A在B坐标系下的位置；
[0009] (4)有障碍物遮挡时，A在机械臂控制下在B的有效视场内绕开障碍物；
[0010] (5)运动后，再次获取手术器械在A坐标系下的位置，以及A在B坐标系下的位置；
[0011] (6)根据空间坐标变换，得到手术器械在世界坐标系下的位置；
[0012] 从而实现对手术器械的无遮挡定位。
[0013] 具体的，所述的步骤(2)中，合适位置是指，把B尽可能地放到能保证A在B的有效视场内绕开障碍物运动的位置上，即先预判A可能的移动范围，再确定B的位置，确保B的有效视场能尽可能大的覆盖A的移动范围；所述的有效视场，指在双目视觉系统的两个摄像机均能拍摄到的区域中，能够实现对物体精确定位的区域。
[0014] 具体的，所述的步骤(3)是通过在手术器械上加入标记点以及在A下方加入标记物，来获取手术器械在A坐标系下的位置以及A在B坐标系下的位置；所述的在A下方加入标记物，其标记物为一正四面体，当A中两摄像机的光轴与地面水平时，此正四面体呈倒立状态，即有1个与地面平行的上表面及3个斜向下的侧面；此正四面体的4个顶点上各有1个标记点，此正四面体的3个侧棱上(不包括顶点)分别有1个、2个、3个标记点，这些标记点分别对3个侧棱二等分、三等分、四等分；从而B可以根据所拍到的正四面体标记物上的标记点的数量和排列方式来确定A的位置和方向。
[0015] 具体的，所述的步骤(4)中，A在机械臂控制下在B的有效视场内绕开障碍物，其原理如下：机械臂后端的舵机M1，用于控制整个机械臂绕固定支架左右旋转，舵机M2和M3联合运动用于控制机械臂上下前后运动，我们默认M1用于控制A的左右位置，M2用于控制A的上下位置，M3用于控制A的前后位置，舵机M4用于控制A的上下方向，M5用于控制A的左右方向；当有障碍物遮挡时，M1、M2根据障碍物位置转动从而控制A的位置，M3、M4、M5根据手术器械位置转动从而控制A的方向和距离。其控制流程如下：(1)根据新拍的图像上障碍物位置控制M1、M2上下左右转动，使得障碍物离开A的有效视场；(2)转动后，根据手术器械在A坐标系下的位置，确定M3、M4、M5的转动方式，确保手术器械不离开A的有效视场；(3)一直重复步骤(1)-(2)，从而实现A绕开障碍物获取手术器械位置，同时，整个过程中A下方的标记物必须在B的有效视场内。
[0016] 具体的，所述的步骤(6)中，所述的根据空间坐标变换得到手术器械在世界坐标系下的位置，具体是指，B根据A下方的正四面体标记物得到A相对于其初始化状态下的旋转平移矩阵，从而可以对位置改变后的A所获取的手术器械的位置进行旋转平移变换，得到其在世界坐标系下的位置。所述的世界坐标系与初始状态下的A坐标系重合。
[0017] 本发明的工作原理：本发明使用了两个双目视觉系统A和B，使用机械臂搭载双目视觉系统A,使用双目视觉系统B定位双目视觉系统A，从而实现对手术器械的无遮挡定位。
[0018] 本发明相对于现有技术具有如下的优点及效果：
[0019] 1、使用机械臂搭载双目视觉系统运动，有效解决了光学定位技术中光线易被遮挡的问题。
[0020] 2、使用第二套双目视觉系统定位第一套双目视觉系统，而不是依靠机械臂自身舵机的位置关系来定位第一套双目视觉系统，降低了对机械臂运动精度的要求，减少了机械臂控制的成本，同时也避免了对多自由度机械臂进行复杂的坐标变换运算。
附图说明
[0021] 图1为两个双目视觉系统和机械臂的结构示意图。
[0022] 图2为双目视觉系统自动绕开障碍物的控制流程图。
具体实施方式
[0023] 下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
[0024] 实施例
[0025] 如图1所示，一种基于两个双目视觉系统的手术器械无遮挡定位方法通过如下步骤实现：
[0026] (1)把双目视觉系统A(下面简称A)固定于机械臂前端，随机械臂运动。双目视觉系统由两个近红外摄像机以及固定他们位置的支架组成，初始化机械臂位置，使A的两摄像机光轴与地面水平，两摄像机的连线中心在机械臂舵机M5的转轴上，如图1所示。以A的两摄像机的连线中心为原点，以两摄像机连线方向为X轴，以左摄像机指向右摄像机的方向作为X轴方向，以竖直向上为Z轴建立直角坐标系作为A的坐标系。以此时的A坐标系作为世界坐标系。
[0027] (2)确定双目视觉系统B(下面简称B)的位置和方向，使得B的有效视场最大限度地覆盖A的运动范围。以B的两摄像机的连线中心为原点，以两摄像机连线方向为X轴，以左摄像机指向右摄像机的方向作为X轴方向，以垂直两摄像机光轴所构成的平面的直线为Z轴，以指向摄像机上方的方向为Z轴方向建立直角坐标系作为B的坐标系。在A下方固定一个正四面体标记物，当A运动时，正四面体标记物跟随A一起运动，因此，B可以通过跟踪A下方标记物来确定A的位置和方向。正四面体标记物的设计如图1所示，其4个顶点上各有1个标记点，而3个侧棱上(不包括顶点)分别有1个、2个、3个标记点，这些标记点分别对3个侧棱二等分、三等分、四等分。从而B可以根据所拍到的正四面体标记物上的标记点的数量和排列方式来确定正四面体标记物的四个面的位置和方向，确定其相对于初始状态的旋转平移矩阵。
[0028] (3)记录初始状态下(此时，A坐标系与世界坐标系重合)正四面体标记物的标记点在B坐标系下的坐标，从而确定正四面体标记物在初始状态下在B坐标系下的位置和方向。当手术器械进入A的有效视场后，通过A获取手术器械在A坐标系下的位置。
[0029] (4)当有遮挡物时，A在机械臂控制下在B的有效视场内绕开障碍物，具体通过以下方案实现：如图1所示，机械臂的M1和M5舵机可以实现左右旋转，M2、M3、M4舵机可以实现上下旋转。舵机M1、M2根据障碍物位置转动，从而控制A的左右上下位置；舵机M3、M4、M5根据手术器械位置转动，从而控制A的前后位置以及上下左右方向。具体控制步骤如图2所示：(1)根据A获取的图像上障碍物的位置，控制舵机M1左右转动来实现A的左右运动，控制舵机M2上下转动来实现A的上下运动，使得障碍物离开A的有效视场；(2)在M1、M2转动后，手术器械在A坐标系下的坐标会发生相应的改变。因此，再次根据A获取的最新图像，得到手术器械的位置，根据手术器械的位置，控制M3、M4、M5转动。具体的说，当手术器械位置离A距离过近或过远时，控制舵机M3上下转动来实现A的前后运动；当手术器械位置偏上或偏下时，控制舵机M4上下转动来改变A的上下方向；当手术器械位置偏左或偏右时，控制舵机M5左右转动来改变A的左右方向；(3)由于步骤(2)过后，并不能完全确保A能绕开障碍物，同时，M3、M4的转动除了影响A的前后位置和上下方向外，也会影响到A的上下位置，所以需一直循环步骤(1)-(2)，从而实现A自动绕开障碍物获取手术器械位置。为保证实时的效果，本方案使用的双目视觉系统采集帧数达60帧/秒。此外，在上述运动过程中，B一直获取A下方的正四面体标记物的位置，当标记物将要离开B的有效视场时，先让机械臂停止运动，系统再通过坐标计算，判断是否存在能让A在B的有效视场内绕开障碍物遮挡的位置，存在则让机械臂直接运动到该位置，不存在则复位机械臂。
[0030] (5)机械臂每次运动后，B均根据A下方的正四面体标记物的位置和方向，计算其相对于初始状态下的旋转平移矩阵。由于正四面体标记物相对于A静止，因此，其旋转平移矩阵即为A相对于世界坐标系的旋转平移矩阵。通过对手术器械在A坐标系下的坐标进行相应的旋转平移变换，即可实现在世界坐标系下对手术器械的定位。
[0031] 上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
