CN110051436B - 自动化协同工作组件及其在手术器械中的应用 - Google Patents

Abstract

一种自动化协同工作组件，包括第一运动臂、第二运动臂、工作部件和影像获取装置。影像获取装置设置于第二运动臂的一端，受第二运动臂的控制而运动，对目标实施光学定位和跟踪，目标遮挡的识别和避开。分别对第一运动臂和第二运动臂的位置、姿态和运动速度进行数值化，建立第一运动臂和第二运动臂的关联关系，并在计算机控制下实施协同运动。本发明提供的组件，用于图像引导手术器械，可以协同完成手术过程，避免碰撞，同时解决手术过程中人体、医疗器械和组织器官对跟踪目标的遮挡导致的手术中断，使得手术得以持续，保证手术的连贯性。

Description

自动化协同工作组件及其在手术器械中的应用

技术领域

本发明涉及一种能自动化运行的机械装置，尤其涉及一种组件，在计算机的控制下，各个部件之间能够互相协同，自动化完成各项任务，以及其在手术器械中的应用。

背景技术

基于光学定位的手术机器人***，如：法国MedTech公司的ROSA Spine***，美国Stryker公司的MAKO***，以及天智航公司的“天玑”***都使用光学定位实现对手术部位和手术工具(或者机械臂)的跟踪。这些***仅配置一个机械臂，用于抓持手术工具，光学定位***与安装机械臂的主机体分离，被另外安装在一个移动支架上。这种***的弊端在于，机械臂进行较大范围的自动运动时会存在风险，可能会碰撞周边环境物体，造成损伤。另外，这种***需要时刻保持手术部位和手术工具同时被光学定位跟踪***识别跟踪，才能确保手术的顺利进行。同时，识别两个移动物体需要定位跟踪***具有更大的识别跟踪范围，范围越大，光学定位跟踪***的定位精度越低。另外光学定位跟踪***基于摄像机摄像原理，存在拍摄光路遮挡情况，当手术部位或手术工具中的任何一个被遮挡时，都会造成手术中断。此外，由于两个被跟踪目标(如：手术部位与手术工具)都可能产生移动，两者同时保持跟踪时，操作者需要专门为跟踪摄像头让出拍摄的光路；还需要时刻保持手术工具朝向定位跟踪***。由于医生操作手术器械时，其运动自由度(如：移动、旋转和翻转等)很大，容易造成手术器械被遮挡，失去追踪，给医生实施手术带来诸多不便，降低了医生设备操作上的体验。

发明内容

本发明的一个目的在于提供一种组件，在计算机控制下各个组件之间能自动化协同工作，完成各项工作任务。

本发明的另一个目的在于提供一种组件，在计算机控制下各个组件能实施自动化协同，完成手术操作。

本发明的再一个目的在于提供一种组件，用于图像引导手术器械，解决手术过程中人体、医疗器械和组织器官对跟踪目标的遮挡导致的手术中断，使得手术得以持续，保证手术的连贯性。

一种组件，包括：

第一运动臂，其包括若干第一支臂和至少1个第一运动关节；

第二运动臂，其包括若干第二支臂和至少1个第二运动关节；

工作部件，其设置于第一运动臂的一端，受第一运动臂的控制而运动；

影像获取装置，其设置于第二运动臂的一端，受第二运动臂的控制而运动，对目标实施光学定位和跟踪，目标遮挡的识别和避开；

分别对第一运动臂和第二运动臂的位置、姿态和运动速度进行数值化，将第一运动臂的位置信息和姿态信息与第二运动臂的位置信息和姿态信息建立关联关系，实现第一运动臂坐标系和第二运动臂坐标系之间的转换，并在计算机控制下实施协同运动。

本发明提供的影像获取装置包括若干摄像头，按规则排列组成。

本发明的组件，实施协同运动的方法如下：

影像获取装置实时获取被跟踪目标的位置和姿态，计算机通过第一运动臂上各个第一关节的旋转角度实时获知工作部件(如：手术中使用的器械)的位置和姿态，根据第一运动臂与第二运动臂之间的关联关系，完成被跟踪目标在影像获取装置坐标系下的位置及姿态与在第一运动臂坐标系下的位置与姿态之间的转换，使第一运动臂获取被跟踪目标在第一运动臂坐标系下的位置与姿态，进而获得预定的工作路径的具***置与方向，驱动第一运动臂在被跟踪目标附近按预定的路径完成工作任务。

计算机实时获知工作部件(如：手术中使用的器械)的位置信息与姿态信息，以及第一运动臂的位置信息和姿态信息；

此时，设置在第二运动臂上的影像获取装置实时调整获取影像的方向，朝向第一运动臂及工作部件，并保持对第一运动臂以及工作部件进行跟踪；

接着，及时获得工作部件及第一运动臂所处的周边环境的三维计算机场景，即获得周边环境物体表面点的三维信息，得到点云，并进行网格化；

之后，根据第一运动臂上各个第一运动关节位置及周边环境物体表面点的三维信息，计算第一运动臂及工作部件与周边环境物体的最小距离，预判第一运动臂或工作部件与周边环境物体是否会发生碰撞；

当存在发生碰撞的可能时，则停止第一运动臂的移动。

在执行工作任务时，第二运动臂及影像获取装置还能解决手术过程中人体、医疗器械和组织器官对被跟踪目标的遮挡导致的手术中断，使得手术得以持续，保证手术的连贯性。

在手术时，影像获取装置对手术部位的跟踪通过跟踪与手术部位刚性连接的标志物实现。标志物又称目标，它包含若干个标志点，标志点具有特定的形状和颜色，对特定的光具有强烈的反射能力，可以被摄像头高对比度拍摄并通过算法高效识别。通过识别标志点，并运用双目定位方式，可以判断目标的位置和姿态。通过影像获取装置保持对目标的跟踪，同时实时获取视野场景中物体的表面三维位置信息。若影像获取装置因被遮挡，或因手术部位的移动造成对目标失去跟踪，则通过获取视野场景中物体的表面三维信息，自动计算出一个新的跟踪位置和方向；第二运动臂驱动影像获取装置达到新的位置和方向，再度实现对手术部位的实时跟踪。

本发明提供的一种避开遮挡、保持对目标进行跟踪的方法，包括如下步骤：

1.对影像获取装置进行标定，获得各摄像头的内部参数(如：焦距)和外部参数(如：位置和方向)。

2.设定影像获取装置中各摄像头按设定的频率同步曝光拍摄，对于每一次曝光重复以下各步骤：

首先，识别目标之标志点，一个目标预设1个以上的标志点，识别并确定每个摄像头所获取的影像中目标各标志点的中心位置；同时也确定各标志点中心的三维空间位置，并通过这些中心位置计算出一个目标的中心位置。

当任意摄像头所获取的影像中，识别出的目标的标志点数量少于预定数量时，判定有遮挡发生。

计算影像获取装置视场范围内所有物体表面的三维位置信息，得到用点云表示的物体表面，并将这些点云网格化表示，得到物体的几何边界，再按以下步骤获得影像获取装置的一个新位置和方向：

1)在影像获取装置和目标中心位置之间的距离上建立一个球面或者平面区域，亦或是任意选定的一个规则空间的三维区域，作为查找区域，查找区域的范围由第二运动臂能够到达的范围决定；

2)在查找区域上获得若干采样点(如：通过网格化的方式，将网格交叉点作为采样点)；

3)将影像获取装置的中心位置依次设定在这些采样点上，并将影像获取装置的视场方向设定为采样点与目标中心位置之间的矢量；

4)在每个采样点，根据摄像头的内部参数和外部参数，并沿视场方向旋转影像获取装置，在计算机中虚拟连续曝光拍摄，直到找到一个意向位置，使得各摄像头同时获取到目标的所有标志点；

最后，第二运动臂将影像获取装置移动旋转到计算得到的意向位置。

若在计算得到的意向位置上依然存在遮挡，则继续寻找，直到各个摄像头获取目标预设的全部标志点。

第二运动臂可以实现若干个自由度的运动，保证影像获取装置到达新的位置和方向(如：6自由度机械臂，x，y和z方向的移动和旋转)。

第二运动臂带有编程接口和电机，可以接收命令，并主动运动到命令指示的位置和姿态。

本发明提供的方法，适合在图像引导手术中应用，解决手术过程中人体、医疗器械和组织器官对跟踪目标的遮挡导致的手术中断，使得手术得以持续，保证手术的连贯性。

考虑到摄像机的拍摄范围大小不同，寻找避开遮挡的位置和方向需要更大范围的获取目标周围物体的信息。可以对影像获取装置进行特殊配置，如使用两个窄角度摄像头专门获取目标标志点的实时空间位置，用于定位跟踪，同时判断遮挡；再使用另外两个大角度摄像机获取目标周边的情况，计算新的影像获取装置位置和方向，避开遮挡。

影像获取装置具备点云获取模式，可以无接触获取手术部位表面点云信息，与CT影像重建得到的手术部位表面进行面配准，获取实际手术部位与CT影像之间的坐标转换关系，实现更高的手术精度。面配准目前已有公认成熟算法。图像引导手术中常用的面配准取点方法为使用专门的手术工具在手术部位划动取点(如在皮肤上取点)，光学无接触配准，相对接触式配准，可以避免手术部位表面形变，获得更高精度。

本发明技术方案实现的有益效果：

本发明提供的组件，能自动寻找工作部件(如：手术器械)的行进路径，同时避免发生触碰，保证安全操作。

本发明提供的组件，能使影像获取机构避开遮挡，使目标被持续追踪，保证自动化实施协同工作，如：手术的持续性，不中断。

通过获得运动臂关节转动参数实现对工作部件的跟踪，不需要依赖光学定位跟踪***，使手术不会因手术工具失去跟踪而中断，减少可能发生的遮挡。

本发明提供的组件，应用于图像引导手术器械，不需要同时跟踪手术部位和手术工具，仅需要跟踪手术部位，减少光学定位跟踪***的跟踪范围，提升精度，也降低了成本。

本发明提供的组件，能有效保证光学定位跟踪***与手术部位的距离在最佳精度范围内，提升精度。手术过程中可以实现光学定位跟踪***自动避开拍摄光路遮挡，方便医生操作，获得更好的手术体验。

本发明提供的组件，应用于图像引导手术器械，能无接触获得手术部位表面点云信息，获得更高精度的原始数据，以及配准矩阵，提升***精度

附图说明

图1为摄像头的3D重构一实施例的示意图；

图2为本发明组件一实施例的示意图；

图3为本发明组件另一实施例的示意图；

图4为本发明组件依据跟踪目标周边物体三维位置点云数据进行网格化一实施例的示意图；

图5为本发明一个与患者刚性连接的机构一实施例的示意图；

图6为依据跟踪目标周边物体三维位置信息进行网格化，并寻找新的跟踪位置与方向一实施例的示意图；

图7为用点云表示的物体表面一实施例的示意图；

图8为将图7所示的点云进行网格化所得的形态的一实施例示意图；

图9为本发明组件作为器械应用于手术的一实施例示意图。

具体实施方式

以下结合附图详细描述本发明的技术方案。本发明实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围中。

本实施例中，影像获取装置装载了不同的软件模块，即光学定位跟踪***和视觉监控避障***。它们可以各基于一组摄像头或基于共同的一组摄像头(一组包含两个摄像头)。如图1，A和B为经过标定(标定可以获得摄像头的焦距等内部参数，以及位置和方向等外部参数)的两个摄像头的中心位置，每个摄像头对应一个成像平面；x点为c点在平面1的像，x’为c点在平面2的像，从A点生成一条线经过x点，从B点生成一条线经过x’点，两条线的交叉点为c的位置，若在A、B所在的位置建立一个坐标系，就可以定义c的三维空间位置，实时连续对c进行拍摄，就可以实现定位跟踪***对c的跟踪。另外，若使用算法(如SIFT算法)在平面1和平面2中找到多个特征点(如物体表面的特征点)的对应关系，则可以得到摄像机拍摄场景中各物体表面上大量点的位置信息，获得一个点云(如图7)，将点云网格化(如图8)，可以还原物体形状，得到一个三维计算机场景。视觉监控避障***利用上述获得的三维场景信息，判断可能的阻挡或遮挡。因定位跟踪***与视觉避障***各自要求的精度或拍摄范围不同(比如视觉监控避障***要求的精度相对要低，但拍摄范围要求更大)，也可以使用两组，每组各两个摄像头分别实现。

运动臂是参数化***，它的姿态可以通过每一个运动关节的运动角度计算得到。通过将影像获取装置安装在一个运动臂的一端，能实现对影像获取装置位置的参数化设置与获取，实时获得影像获取装置的位置和方向。将手术器械通过夹具安装在第一运动臂末端法兰，手术器械与第一运动臂末端法兰的位置关系在机械结构设计时即已获得。影像获取装置与第二运动臂之间的坐标关系，第一运动臂与第二运动臂之间的坐标变换关系都可以通过在机械结构设计时设定的尺寸参数获得。因此，最终可以获得手术器械与影像获取装置之间的坐标变换关系，亦即通过以上的结构设计参数和运动关节运动信息，实现对手术工具位置和姿态的实时追踪。

又因运动臂是一个参数化***，两个运动臂的位置和姿态完全由各运动关节的运动角度确定，计算机可以精确获知任意时刻任一运动臂及安装在运动臂上的影像获取装置与手术工具的位置信息与姿态信息，这使两个臂实现自动化协同工作。

在进行手术动作时，影像获取装置获得手术部位被跟踪目标的姿态与位置，被跟踪目标与手术部位刚性连接，进而获得手术路径在手术部位的位置与方向。通过第一运动臂与第二运动臂之间的位置与姿态关联关系，将影像获取装置坐标系下手术部位的手术路径位置与方向，转化为第一运动臂坐标系下的位置与方向，第一运动臂驱动手术器械到达预定的位置与方向，并执行手术。手术过程中，***通过运动臂的各关节参数实时获知手术器械的位置与方向，实时反馈手术进程(如：钻孔深度)并判断是否有偏离等。

在进行非手术的动作时，第一运动臂获得一个目标位置，并驱动自身将手术器械移动到目标位置，计算机按如下步骤实现避免碰撞：

在计算机控制下，通过第一运动臂上各个第一关节的旋转角度实时获知手术器械的位置，以及第一运动臂的位置和姿态信息；

此时，设置在第二运动臂上的影像获取装置实施调整获取影像的方向，朝向第一运动臂及手术器械，并保持对第一运动臂以及手术器械进行跟踪；

接着，及时获得手术器械及第一运动臂所处的周边环境的三维计算机场景，即获得周边环境物体表面点的三维信息，得到点云，并进行网格化；

之后，根据第一运动臂上各个第一关节位置及周边环境物体表面点的三维信息，计算第一运动臂及手术器械与周边环境物体的最小距离，预判第一运动臂或手术器械与周边环境物体是否会发生碰撞；

当存在发生碰撞的可能时，则停止第一运动臂的移动。

在手术进行过程中，本实施例通过视觉监控避障***软件模块提供一种避开遮挡，保持对目标进行跟踪的方法，包括：

1.对影像获取装置进行标定，获得各摄像头的内部(如：焦距)和外部参数(如：位置和方向)

2.设定影像获取装置中各摄像头按一定频率同步曝光拍摄，对于每一次曝光重复以下各步骤：

首先，识别目标之标志点，一个目标预设1个以上的标志点，识别并确定每个摄像头所获取的影像中目标各标志点的中心位置；同时也确定各标志点中心的三维空间位置，并通过这些中心位置计算出一个目标的中心位置，记为DC。

当任意摄像头所获取的影像中，识别出的目标的标志点数量少于预定数量时，判定有遮挡发生(此时可能无法获取目标中心位置DC，可以用前一帧曝光拍摄获取的中心位置为当前帧目标的中心位置DC，并继续后续的步骤)。

计算当前影像获取装置视野场景内所有物体表面的三维位置信息，得到用点云表示的物体表面(如图7)，并将这些点云网格化表示(如图8)，得到物体的几何边界，按以下方法获得一个新的摄像机位置和方向：

1)在目标中心DC相距的一个位置，建立一个球面或者平面区域，亦或是任意选定的一个规则空间三维区域，这个区域记为CS，CS的范围由运动臂能够到达的范围决定。

2)在CS上获得若干采样点。

3)将影像获取装置的中心位置依次设定在这些采样点，并将影像获取装置的视场方向设定为采样点与目标中心DC之间的矢量。

4)在每个采样点，根据摄像头的内部和外部参数在计算机中虚拟连续曝光拍摄，直到找到一个位置，可以保证各摄像头同时拍摄到目标的所有标志点，过程中止。

若计算得到的方向跟位置依然存在遮挡，则重复以上过程，直到获得一个不被遮挡的位置为止。

考虑到摄像机的拍摄范围大小不同，寻找避开遮挡的位置和方向需要更大范围的获取目标周围物体的信息。可以使用两个窄角度摄像头专门获取目标标志点的实时空间位置，用于光学定位跟踪，同时判断遮挡；再使用另外两个大角度摄像机获取目标周边的情况，依据上文提到的算法计算新的摄像机位置和方向，避开遮挡，实现视觉监控避障。这里需要对两组摄像头之间的坐标系关系进行计算。

如图2，选用两个Universal Robot公司的UR3机械臂，UR3具有6个自由度。第二运动臂100的末端法兰安装了一个摄像头组合300，它包含两个摄像头(如：微软LifeCamStudio)，构成影像获取装置，从它获得的影像被传输到计算机，用于光学定位跟踪与视觉监控避障；第一运动臂200的末端法兰安装了手术器械400(譬如：骨钻)。

另一种可能的实施方式如图3，在第二运动臂100的末端法兰安装第一摄像头500和第二摄像头510，构成影像获取装置，第一摄像头500用于光学定位跟踪，第二摄像头510用于视觉监控避障，第一摄像头500和第二摄像头510也可以成90度安装。这种实施方式的一个优点在于，针对光学定位和视觉监控各自的要求不同，可以选择不同性能与参数的摄像头。在结构设计上保证第一摄像头500和第二摄像头510的安装位置的唯一性，保证第一摄像头500和第二摄像头510的拍摄方向一致，第一摄像头500和第二摄像头510之间的坐标变换关系可以通过一次平移实现，设该平移矩阵为M，则通过M可以实现第一摄像头500和第二摄像头510之间的视场目标变换。譬如：通过M，将第二摄像头510拍摄得到的位置坐标信息或点云数据信息转化为第一摄像头500下的位置坐标信息或点云数据信息，实现了两者数据的统一，这方便了后续的视觉监控避障。

在图2中，第二运动臂100与影像获取装置300之间、第一运动臂200与手术器械400之间，以及第二运动臂100与第一运动臂200之间的位置关系(坐标变换关系)可以通过机械结构安装时的尺寸测量获得。另外，因为第二运动臂100和第一运动臂200的位置完全由各运动关节的运动角度决定，这些角度可以通过运动臂返回的内部参数获得。因此，影像获取装置300与第一运动臂200及手术器械400之间的位置关系(即坐标系变换关系)可以实时通过计算获得，并不需要在手术器械400处安装任何用于光学定位跟踪的识别标志和标志点。

图9为本发明组件作为器械应用于手术的一实施例示意图。如图9所示，在执行手术任务时，被跟踪目标610与手术部位900刚性连接，被跟踪目标610的移动反映手术部位900的移动。手术部位900上含有手术路径910(如：钻孔方向)，获得手术路径910的一般方法是通过在CT影像上进行手术计划，在CT影像上标记手术路径，然后通过在CT影像上取若干个点，同时在手术部位取对应的若干个点，进行点配准，实现CT影像坐标系与手术部位坐标系的统一，最终实现将基于CT坐标系的手术路径转移到实际手术部位。影像获取机构300实时获取目标610的位置和姿态，计算机实时计算出手术路径的位置与姿态，这里的位置与姿态都以影像获取机构300下的坐标系描述。通过第二运动臂100与第一运动臂200之间的关联关系，计算机将手术路径910在影像获取机构300坐标系下的位置和姿态转化为第一运动臂200坐标系下的位置和姿态，并驱动第一运动臂200按照计划的手术路径执行手术过程。计算机根据第一运动臂200各运动关节的角度，实时计算第一运动臂200的手术进程(如：钻孔深度与方向)，并判断是否偏离方向，是否到达预定位置等。

在进行非手术任务时(如关机时运动臂的自动收起)，当第一运动臂200获得一个命令，驱动手术器械400到达指定位置时，计算机可以实时获得手术器械400相对于影像获取装置300及第二运动臂100的位置关系，进而计算出影像获取装置300的一个新的位置和方向，保证手术器械400处于影像获取装置300的跟踪与监控范围内。影像获取装置300实时对手术器械400周边的环境物体进行摄像，获得手术器械400所处环境背景物体表面的点云数据，这些点云数据的坐标都是相对于影像获取装置300计算得到的。这些点云数据被网格化成为实体表面(点云获取算法及将点云网格化的算法都属于成熟算法，市场上存在一些成熟产品，可以实时获取点云数据并网格化，如：微软的kinect)。如图4为一简化示意图，周边环境中，第一物体310与第二物体320为影像获取装置300获得的点云数据网格化得到的物体表面，通过第一运动臂200各运动关节的角度，以及第一运动臂200与手术器械400的相对固定位置关系，可以在计算机中虚拟建立第一运动臂200与手术器械400的模型，并在影像获取装置300的坐标系中绘制，即得到第一运动臂200与手术器械400以及第一物体310与第二物体320之间的相对位置关系，计算机实时计算第一运动臂200与手术器械400相对于第一物体310及第二物体320之间的距离。当距离小于设定安全值时，即停止第一运动臂200的运动，防止碰撞发生。这里距离计算属于碰撞检测的范畴，在此领域已有相应的算法。

图5为一个与患者刚性连接的机构600，其包含待跟踪的目标610，因机构600与患者刚性连接，跟踪机构600的位置变化等同于跟踪患者的移动。目标610包含4个标志点620，定义的中心630，相对于各标志点620处于中心位置。

使用300对目标进行曝光拍摄，使用前文提到的双目定位原理获得每一个标志点620的空间位置信息，并计算被跟踪目标的中心位置630。

在影像获取装置300中每个摄像机生成的图像上识别出所有标志点，当影像获取装置300中任意一个摄像机拍摄的图像中标志点的数目小于4个时，则判断为发生了遮挡。

使用影像获取装置300中两个摄像头获得的图像，通过计算机视觉领域常用的SIFT算法等，获得影像获取装置300视场范围内所有物体表面的三维位置信息，将这些空间位置网格化得到物体实体710，720，如图6所示。

在计算机渲染环境中布置这些实体，如在OpenGL环境下布置，场景中包含上述的所有实体以及被跟踪目标。如图6，计算机中布置的场景信息700，其中包含的实体，其位置关系与真实世界中相同。

在上述的计算机场景中，摆放好影像获取装置300中包含的两个相机的位置，并在摄像机组所处的位置附近建立一个平面800，该平面垂直于影像获取装置300所处的位置与被跟踪的目标(即机构600)之间的连线(参见图6)，这个平面与被跟踪目标之间的距离处在摄像机的最佳精度范围内，平面的大小根据第二运动臂100能够到达的范围确定。先将平面等分，譬如6等分，找到12个位置。如图6，平面800上共有12个位置代表点810，依次尝试在这12个位置进行虚拟拍摄。将影像获取装置300依次放置在12个代表点810位置上，对准目标拍摄，在得到的照片上识别标志点，若两个摄像机得到的相片识别出的标志点数目都为4，则认为该位置不存在遮挡。(所谓的虚拟拍摄，就是使用相机的参数(如方向，焦距等)，在计算机中模拟相机的成像原理将物体投影到相机成像平面)。

可以多次尝试等分平面，获得一个离获取装置300最近的位置作为目标位置，减少第二运动臂100的运动量，获得更快的机械响应速度。

在判断出沿哪个方向移动可以避开遮挡实现对目标的持续跟踪后，可以通过计算机将命令传递给第二运动臂100，第二运动臂100自动完成影像获取装置的重定位。

若新位置再次出现遮挡，则重复上述过程，直到找到一个不被遮挡的位置时停止。若多次尝试后仍然失败(如被跟踪目标被一个物体包裹)，则通知计算机无法找到位置，计算机向用户报告错误。

通过影像获取装置300可以获得跟踪目标及视野场景物体的表面点云数据，跟踪目标与患者手术部位刚性连接，在这些点云数据中识别患者手术部位，将该部分数据与CT三维重建得到的手术部位表面进行面配准，本发明提供的装置方便了这种配准的进行，点云通过摄像机拍摄获得，与人体无接触，保证手术部位表面组织(如皮肤)无任何变形，获得的点云更加精确。

Claims (5)

1.一种自动化协同工作组件，其特征在于包括：

第一运动臂，其包括若干第一支臂和至少1个第一运动关节；

第二运动臂，其包括若干第二支臂和至少1个第二运动关节；

工作部件，其设置于第一运动臂的一端，受第一运动臂的控制而运动；

影像获取装置，其设置于第二运动臂的一端，受第二运动臂的控制而运动，对目标实施光学定位和跟踪，目标遮挡的识别和避开；

分别对第一运动臂和第二运动臂的位置、姿态和运动速度进行数值化，建立第一运动臂和第二运动臂的关联关系，并在计算机控制下实施协同运动；

实施所述的协同运动的方法如下：

影像获取装置实时获取被跟踪目标的位置和姿态，计算机通过第一运动臂上各个第一关节的旋转角度实时获知工作部件的位置和姿态，***根据第一运动臂与第二运动臂之间的关联关系，完成被跟踪目标在影像获取装置坐标系下的位置及姿态与在第一运动臂坐标系下的位置与姿态之间的转换，使第一运动臂获取被跟踪目标在第一运动臂坐标系下的位置与姿态，进而获得预定的工作路径的具***置与方向，驱动第一运动臂在被跟踪目标附近按预定的路径完成工作任务；

计算机实时获知工作部件的位置信息与姿态信息，以及第一运动臂的位置信息和姿态信息；

此时，设置在第二运动臂上的影像获取装置实时调整获取影像的方向，朝向第一运动臂及工作部件，并保持对第一运动臂以及工作部件进行跟踪；

接着，及时获得工作部件及第一运动臂所处的周边环境的三维计算机场景，即获得周边环境物体表面点的三维信息，得到点云，并进行网格化；

之后，根据第一运动臂上各个第一运动关节位置及周边环境物体表面点的三维信息，计算第一运动臂及工作部件与周边环境物体的最小距离，预判第一运动臂或工作部件与周边环境物体是否会发生碰撞；

当存在发生碰撞的可能时，则停止第一运动臂的移动。

2.根据权利要求1所述的自动化协同工作组件，其特征在于所述的 影像获取装置对目标实施光学定位和跟踪，目标遮挡的识别和避开的方法如下：

先对影像获取装置进行标定，获得各摄像头的内部参数和外部参数；

然后，设定影像获取装置中各摄像头按设定的频率同步曝光拍摄，对于每一次曝光重复以下各步骤：

首先，识别目标之标志点，一个目标预设1个以上的标志点，识别并确定每个摄像头所获取的影像中目标各标志点的中心位置；同时也确定各标志点中心的三维空间位置，并通过这些中心位置计算出一个目标的中心位置；

当任一摄像头所获取的影像中，识别出的目标的标志点数量少于预定数量时，判定有遮挡发生；

计算当前影像获取装置视场范围内所有物体表面的三维位置信息，得到用点云表示的物体表面，并将这些点云网格化表示，得到物体的几何边界，再按以下步骤获得一个新的影像获取装置的位置和方向：

1)在影像获取装置和目标中心位置之间的距离上建立一个球面或者平面区域，亦或是任意选定的一个规则空间的三维区域，作为查找区域，查找区域的范围由第二运动臂能够到达的范围决定；

2)在查找区域上获得若干采样点；

3)将影像获取装置的中心位置依次设定在这些采样点上，并将影像获取装置的视场方向设定为采样点与目标中心位置之间的矢量；

4)在每个采样点，根据摄像头的内部参数和外部参数，并沿视场方向旋转影像获取装置，在计算机中虚拟连续曝光拍摄，直到找到一个意向位置，使得各个摄像头同时获取到目标的所有标志点；

最后，使用第二运动臂将影像获取装置移动旋转到计算得到的意向位置。

3.根据权利要求2所述的自动化协同工作组件，其特征在于所述的内部参数包括焦距。

4.根据权利要求2所述的自动化协同工作组件，其特征在于所述的外部参数包括位置和方向。

5.根据权利要求2所述的自动化协同工作组件，其特征在于若在计算得到的所述意向位置上依然存在遮挡，则继续寻找，直到两个摄像头获取目标预设的全部标志点。

Images