碰撞检测系统、碰撞检测用数据生成装置以及机械手的制作方法

碰撞检测系统、碰撞检测用数据生成装置以及机械手的制作方法
【专利摘要】本发明涉及碰撞检测系统、碰撞检测用数据生成装置以及机械手。其中碰撞检测系统包含：存储部，其存储与第1物体对应的第1碰撞检测用数据和与第2物体对应的第2碰撞检测用数据作为物体的碰撞检测用数据；以及处理部，其基于第1碰撞检测用数据和第2碰撞检测用数据来进行世界坐标系中的第1物体与第2物体的碰撞判定。存储部存储利用设定在模型坐标系中的立方体区域对在物体的模型坐标系中从规定的视点观察物体时的深度映射数据进行离散化而得到的代表点数据，作为碰撞检测用数据。
【专利说明】碰撞检测系统、碰撞检测用数据生成装置以及机械手
【技术领域】
[0001]本发明涉及碰撞检测系统、碰撞检测用数据生成装置以及机械手等。
【背景技术】
[0002]在多个领域需要判定有无物体间的碰撞、接近。例如在机械手等领域，碰撞的产生成为非常大的问题。因此，从以往开始研究、开发在实际产生碰撞之前，通过由计算机进行的计算来判定碰撞的有无、允许程度以上的接近的方法。作为这样的碰撞判定方法的现有技术，公知有例如在专利文献I等中公开的技术。
[0003]在专利文献I的方法中，利用多边形数据表示物体，以规定半径的球覆盖该多边形数据的各多边形，并将这些球合并到更大的半径的球中，将这些球的数据构成为表示球的合并关系的二叉树结构的数据。并且，通过按照每个层依次地对该二叉树结构的数据进行碰撞判定，来进行物体间的碰撞判定。
[0004]专利文献1:日本特开平11 - 250122号公报
[0005]在如专利文献I那样使用多边形数据进行碰撞检测的方法中，需要成为检测对象的物体的CAD (Computer Aided Design:计算机辅助设计)数据。然而,在现实中，存在很多不存在CAD数据的物体、不能够得到CAD数据的物体，所以存在在那样的物体上应用上述方法很困难这样的课题。

【发明内容】

[0006]根据本发明的几个方式，能够提供即使在不能够得到物体的多边形数据的情况下等也能够进行碰撞检测的碰撞检测系统、碰撞检测用数据生成装置、机械手系统、机械手、碰撞检测用数据生成方法以及程序等。
[0007]本发明的一个方式涉及如下的碰撞检测系统，包含:存储部，其存储与第I物体对应的第I碰撞检测用数据和与第2物体对应的第2碰撞检测用数据作为物体的碰撞检测用数据；以及处理部，其基于上述第I碰撞检测用数据和上述第2碰撞检测用数据来进行世界坐标系中的上述第I物体与上述第2物体的碰撞判定，上述存储部存储对在上述物体的模型坐标系中从规定的视点观察上述物体时的深度映射数据利用设定在上述模型坐标系中的立方体区域进行离散化而得到的代表点数据，作为上述碰撞检测用数据。
[0008]这样，利用设定在模型坐标系中的立方体区域对物体的深度映射数据进行离散化而得到的代表点数据作为碰撞检测用数据而被存储在存储部中，基于该碰撞检测用数据进行第I物体与第2物体的碰撞检测。由此，即使是不能够得到物体的多边形数据的情况等也能够进行碰撞检测。
[0009]另外在本发明的一个方式中，也可以构成为，上述存储部存储内含上述物体的包围盒的上述代表点数据、和利用分割上述包围盒而成的立方体区域对上述深度映射数据进行离散化而得到的上述代表点数据即、分割代表点数据，作为上述碰撞检测用数据，上述处理部在判定为内含上述第I物体的第I包围盒和内含上述第2物体的第2包围盒发生碰撞的情况下，基于上述第I物体的上述分割代表点数据和上述第2物体的上述分割代表点数据进行上述第I物体和上述第2物体的碰撞判定。
[0010]这样，在包围盒被判定为无碰撞的阶段，能够明确判定为第I物体与第2物体无碰撞，所以省略利用更小的立方体区域的碰撞判定，从而能够简单化处理。
[0011]另外在本发明的一个方式中，也可以构成为，上述存储部存储树结构的数据作为上述碰撞检测用数据，上述树结构的数据具有与分割父节点的立方体区域而成的多个立方体区域对应的上述代表点数据作为从上述父节点分支的子节点的上述代表点数据。
[0012]这样，能够从树结构的上位层的节点向下位层的节点按照每个层依次进行碰撞检测。通过这样的递归处理，能够将碰撞检测并行处理化。
[0013]另外在本发明的一个方式中，也可以构成为，分割上述父节点的立方体区域而成的上述子节点的上述多个立方体区域是通过将从上述规定的视点观察时的上述父节点的立方体区域分割为2X2区域，并且将上述2X2区域的各区域在上述规定的视点的进深方向上进行2分割而得到的2X2X2个立方体区域，上述树结构的数据是与从上述规定的视点观察时的上述2X2区域的各区域对应地设定有上述子节点的四叉树结构的数据，上述四叉树结构中的上述子节点的数据是在上述2X2区域的上述各区域中存在于上述进深方向的2个立方体区域中的至少一方的上述代表点数据。
[0014]基于这样的四叉树结构的数据来进行碰撞检测，能够将各层中的节点对的组合设为4X4 = 16个,例如能够使用线程数为数十左右的CPU (Central Processing Unit:中央处理器)等来实现碰撞检测系统。
[0015]另外在本发明的一个方式中，也可以构成为，上述处理部在基于上述父节点的数据的上述碰撞判定中，在存在判定为碰撞的上述父节点的情况下，基于从判定为上述碰撞的上述父节点分支的上述子节点的数据来进行上述碰撞判定，在基于上述父节点的数据的上述碰撞判定中，在不存在判定为碰撞的上述父节点的情况下，明确判定为上述第I物体与上述第2物体无碰撞。
[0016]这样，能够实现从树结构的上位层的节点向下位层的节点按照每个层依次进行碰撞检测的递归碰撞检测处理。另外，在有针对节点对的全部的组合判定为无碰撞的层的情况下，在处理该层的阶段能够明确判定为第I物体与第2物体无碰撞，所以能够将处理简单化。
[0017]另外在本实施方式中，也可以构成为，上述深度映射数据是由测量上述物体的三维信息的三维信息测量装置生成的深度映射数据。
[0018]另外本发明的其他的方式涉及如下的碰撞检测用数据生成装置，包含:深度映射数据获取部，其获取物体的模型坐标系中的从规定的视点观察上述物体时的深度映射数据；以及碰撞检测用数据生成部，其生成上述物体的上述模型坐标系中的代表点数据作为上述碰撞检测用数据，上述碰撞检测用数据生成部利用设定在上述物体的上述模型坐标系中的立方体区域对上述深度映射数据进行离散化，从而生成上述代表点数据。
[0019]根据本发明的其他方式，利用设定在模型坐标系中的立方体区域对物体的深度映射数据进行离散化，通过该离散化生成代表点数据作为碰撞检测用数据。通过这样生成碰撞检测用数据，即使是不能够得到物体的多边形数据的情况下也能够进行碰撞检测。
[0020]另外在本发明的其他的方式中，也可以构成为，上述碰撞检测用数据生成部将与分割父节点的立方体区域而成的多个立方体区域对应的节点作为子节点与上述父节点连接，并生成树结构的数据作为上述碰撞检测用数据。
[0021]这样，能够由对深度映射数据进行离散化后的代表点数据来构成树结构的数据。另外，通过生成这样的树结构的数据，能够在碰撞检测中进行递归的并行处理。
[0022]另外在本发明的其他方式中，也可以构成为，分割上述父节点的立方体区域而成的上述子节点的上述多个立方体区域是通过将从上述规定的视点观察时的上述父节点的立方体区域分割为2X2区域，并且将上述2X2区域的各区域在上述规定的视点的进深方向上进行2分割而得到的2X2X2个立方体区域，上述碰撞检测用数据生成部生成与从上述规定的视点观察时的上述2X2区域的各区域对应地设置有上述子节点的四叉树结构的数据，作为上述树结构的数据，上述四叉树结构的数据中的上述子节点的数据是在上述2X2区域的上述各区域中存在于上述进深方向的2个立方体区域中的至少一方的上述代表点数据。
[0023]这样，通过与从规定的视点观察时的2X2区域的各区域对应地设定子节点，能够由利用立方体区域对深度映射数据进行离散化后的代表点数据构成四叉树结构的数据。
[0024]另外在本发明的其他方式中，也可以构成为，上述碰撞检测用数据生成部在判断为在处理对象的代表点、和存在于上述处理对象的代表点的周围26个附近的立方体区域的外侧的代表点之间存在缺少上述代表点数据的立方体区域的情况下，在缺少上述代表点数据的立方体区域上增补上述代表点数据。
[0025]通过这样生成碰撞检测用数据，能够抑制虽然实际上在缺少代表点数据的立方体区域中有碰撞可能性但判定为无碰撞这样的错误的检测。
[0026]另外在本发明的其他方式中，也可以构成为，在上述规定的视点的进深方向上越远离则深度值越大的情况下，上述碰撞检测用数据生成部在判定为从上述处理对象的代表点的代表深度值减去存在于上述处理对象的代表点的周围的代表点的代表深度值后的差值为负的情况下，在相对于上述处理对象的代表点靠上述进深方向侧的立方体区域上增补上述代表点数据。
[0027]这样，在差值为负的情况下，该代表点存在于与处理对象的代表点相比靠进深方向侧，所以能够在相对于处理对象的代表点数据靠进深方向侧的立方体区域上增补代表点数据。
[0028]另外在本发明的其他的方式中，也可以构成为，上述深度映射数据是由测量上述物体的三维信息的三维信息测量装置生成的深度映射数据。
[0029]另外本发明的另一其他的方式涉及如下的机械手系统，包含:机械手，其具有可动部；存储部，其存储与第I物体对应的第I碰撞检测用数据和与第2物体对应的第2碰撞检测用数据作为物体的碰撞检测用数据；处理部，其基于上述第I碰撞检测用数据和上述第2碰撞检测用数据来进行世界坐标系中的上述第I物体与上述第2物体的碰撞判定；以及控制部，其基于由上述处理部进行的上述碰撞判定的结果来控制上述可动部的动作，上述存储部存储将在上述物体的模型坐标系中从规定的视点观察上述物体时的深度映射数据利用设定在上述模型坐标系中的立方体区域进行离散化而得到的代表点数据，作为上述碰撞检测用数据。
[0030]另外本发明的另一其他方式涉及如下的机械手，包含:可动部；存储部，其存储与第I物体对应的第I碰撞检测用数据和与第2物体对应的第2碰撞检测用数据作为物体的碰撞检测用数据；处理部，其基于上述第I碰撞检测用数据和上述第2碰撞检测用数据来进行世界坐标系中的上述第I物体和上述第2物体的碰撞判定；以及控制部，其基于由上述处理部进行的上述碰撞判定的结果来控制上述可动部的动作，上述存储部存储将在上述物体的模型坐标系中从规定的视点观察上述物体时的深度映射数据利用设定在上述模型坐标系中的立方体区域进行离散化而得到的代表点数据，作为上述碰撞检测用数据。
[0031]另外本发明的另一其他方式涉及如下的碰撞检测用数据生成方法，获取物体的模型坐标系中的从规定的视点观察上述物体时的深度映射数据，并利用设定在上述物体的上述模型坐标系中的立方体区域对上述深度映射数据进行离散化，并将进行上述离散化而得到的代表点数据作为上述碰撞检测用数据来生成。
[0032]另外，本发明的另一其他方式涉及如下的程序，使计算机作为如下部分发挥作用，即、深度映射数据获取部，其获取物体的模型坐标系中的从规定的视点观察上述物体时的深度映射数据；和碰撞检测用数据生成部，其生成上述物体的上述模型坐标系中的代表点数据作为上述碰撞检测用数据，上述碰撞检测用数据生成部利用设定在上述物体的上述模型坐标系中的立方体区域对上述深度映射数据进行离散化，从而生成上述代表点数据。
【专利附图】

【附图说明】
[0033]图1 (A)是本实施方式的碰撞检测用数据生成装置的构成例。图1 (B)是本实施方式的碰撞检测系统的构成例。
[0034]图2 (A)是包含本实施方式的碰撞检测系统的机械手系统的例子。图2 (B)是包含本实施方式的碰撞检测系统的机械手的例子。
[0035]图3是关于碰撞检测用数据的生成方法的说明图。
[0036]图4是关于碰撞检测用数据的生成方法的说明图。
[0037]图5是关于碰撞检测用数据的生成方法的说明图。
[0038]图6是关于碰撞检测用数据的生成方法的说明图。
[0039]图7是关于碰撞检测用数据的生成方法的说明图。
[0040]图8是关于碰撞检测用数据的生成方法的说明图。
[0041]图9 (A)?图9 (C)是本实施方式的碰撞检测用数据生成装置所生成的四叉树结构的数据的例子。
[0042]图10 (A)、图10 (B)是本实施方式的碰撞检测用数据生成装置所生成的四叉树结构的数据的例子。
[0043]图11 (A)?图11 (C)是本实施方式的碰撞检测用数据生成装置所生成的四叉树结构的数据的例子。
[0044]图12是关于碰撞检测的方法的说明图。
[0045]图13 (A)?图13 (C)是关于碰撞检测的方法的说明图。
[0046]图14是关于碰撞检测的方法的说明图。
[0047]图15是关于碰撞检测的方法的说明图。
[0048]图16是关于碰撞检测的方法的说明图。
[0049]图17是本实施方式的碰撞检测用数据生成装置的详细构成例。[0050]图18是碰撞检测用数据生成处理的流程图。
[0051]图19是一层的数据生成处理的详细流程图。
[0052]图20是四叉树结构生成处理的详细流程图。
[0053]图21是本实施方式的碰撞检测系统的详细构成例。
[0054]图22是碰撞检测处理的流程图。
[0055]图23是递归的节点对碰撞检测处理的详细流程图。
【具体实施方式】
[0056]以下，对本发明的优选实施方式进行详细说明。此外，以下说明的本实施方式并无不当地限定权利要求书所记载的本发明的内容，在本实施方式中说明的构成的全部作为本发明的解决手段未必是必需的。
[0057]1.构成
[0058]在机械手(机械臂)的动作中，与周边构造物、周边设备的碰撞、自碰撞、与其他的机械手的碰撞成为非常大的问题。在本实施方式的碰撞检测方法中，通过模拟预先检测这样的碰撞。
[0059]作为使用这样的本实施方式的碰撞检测方法的方式，可以认为大致分为离线中的使用(预先确认)、和运行时间中的使用(预测、预读)。在离线的使用中，在周边环境等是已知并且是静态的、机械手的动作是已知的情况下，在系统作成时，验证该碰撞。另一方面，在运行时间的使用中，在周边环境等动态地变化的情况(例如在周围存在多个机械手，或者存在作业者的情况)下，在机械手的实际动作之前，通过模拟来检测碰撞。
[0060]在以往，作为这样的机械手中的碰撞检测方法，使用以能够得到物体(对象)的多边形数据为前提的算法的情况较多。多边形数据是在设计物体的结构时等通过多个多边形的组合来表现物体的形状的CAD数据。然而，存在成为碰撞检测的对象的物体多种多样，针对它们全部得到CAD数据实际上很困难这样的课题。
[0061]另外，如上述的专利文献1，在使用多边形数据的以往的碰撞检测方法中，具有对于各多边形生成二叉树结构的球面数据，并使用该数据来进行碰撞检测的方法。然而，在这样的方法中，产生大量的不必要的碰撞检测用数据、不必要的碰撞检测处理，所以进行高效的碰撞检测很困难。
[0062]具体而言，描述现实的物体的多边形数据未必仅包含在碰撞检测中重要的物体表面的多边形数据。作为在碰撞检测中不重要的多边形数据，大量地包含有表现物体内部的多边形数据、表现从物体的外部看不见部分的多边形数据等。若根据这样的多边形数据以多边形为单位生成球面数据，则在二叉树结构的数据中大量地包含有与碰撞检测没有关系的球面数据。在专利文献I中，对于针对这样的在碰撞检测中不重要的多边形数据的处理方法没有说明，即使对与碰撞检测没有关系的球面数据也进行碰撞检测处理会导致低效率的处理。
[0063]另外，在描述现实的物体的多边形数据中，包含涉及非常多方面的大小的多边形，以同一算法处理这些多边形并不高效。例如，在机械手的零件、机械手所操作的工具类中细棒上的物体较多，以微小的多边形表现这样的物体。另外，在物体是复杂的形状等时，也使用多个微小的多边形。可以认为覆盖多边形的球的大小是接近的允许范围程度的大小即可，所以在以树结构的数据表现以比该球小的多边形表现的结构的情况下，数据变得非常冗长。这样的话，虽然微小的结构在碰撞检测中成为重要的情况较少，但由于微小的结构而大量地产生不必要的碰撞检测处理。
[0064]在图1 (A)中，示出能够解决如以上那样的问题的本实施方式的碰撞检测用数据生成装置的构成例。此外，本实施方式的碰撞检测用数据生成装置的构成并不限于图1(A)的构成，能够进行省略其一部分的构成要素(例如操作部、外部I / F部等)，或者追加其他的构成要素等各种的变形实施。
[0065]该碰撞检测用数据生成装置包含处理部110、和存储部150。另外碰撞检测用数据生成装置能够包含操作部170、外部I / F (接口)部180、和信息存储介质190。该碰撞检测用数据生成装置例如由信息处理装置构成，通过该信息处理装置的硬件、程序来实现碰撞检测用数据生成装置。
[0066]处理部110进行各种数据生成处理、控制处理等，例如能够通过CPU等各种处理器或专用电路(ASIC)等硬件、在处理器上执行的程序等来实现。处理部110包含深度映射数据获取部112、和碰撞检测用数据生成部114。
[0067]深度映射数据获取部112进行获取用于生成碰撞检测用数据的深度映射数据的处理。这里所谓深度映射是通过从规定的视点(例如无限远的视点)观察的情况下的物体的深度值表示的映射，是对像素单位的深度值进行矩阵排列而成的映射。向深度映射数据获取部112例如输入预先保存在信息存储介质190中的CAD数据，或者经由外部I / F部180输入来自未图示的三维信息测量装置(例如3D扫描仪)的测量信息。而且，深度映射数据获取部112根据输入的CAD数据、测量信息来生成深度映射数据。
[0068]碰撞检测用数据生成部114进行根据深度映射数据来生成用于在碰撞检测处理中使用的数据的处理。具体而言，如利用图3等后述，在立方体区域中对深度映射数据中的位置以及深度值进行离散化，并生成覆盖物体的表面的被离散化的代表值数据作为碰撞检测用数据。碰撞检测用数据生成部114 一边依次分割(或者合并)立方体领域的尺寸，一边生成代表点数据，并构成表示该分割(或者合并)的从属关系的树结构的数据。生成的碰撞检测用数据被储存到信息存储介质190中。
[0069]存储部150成为处理部110等的工作区域，能够通过RAM (SRAM、DRAM等)等存储器来实现。操作部170是用于使用者输入各种操作信息的。外部I / F部180与外部之间以有线、无线的方式进行信息的通信处理等。信息存储介质190 (通过计算机可读取的介质)储存程序、数据等，其功能能够通过光盘、HDD、或者存储器等实现。处理部110基于储存在信息存储介质190中的程序(数据)来进行本实施方式的各种处理。即、在信息存储介质190中存储用于使计算机(具备操作部、处理部、存储部、输出部的装置)作为本实施方式的各部发挥作用的程序(用于使计算机执行各部的处理的程序)。
[0070]在图1 (B)中，示出能够解决上述的问题的本实施方式的碰撞检测系统的构成例。此外，本实施方式的碰撞检测系统的构成并不限于图1 (B)的构成，能够进行省略其一部分的构成要素(例如操作部、外部I / F部等)，或者追加其他的构成要素等的各种变形实施。
[0071]该碰撞检测系统包含处理部10、和存储部50。另外碰撞检测系统能够包含操作部70、外部I / F (接口)部80、和信息存储介质90。
[0072]存储部50成为处理部10等的工作区域，能够通过RAM (SRAM、DRA M等)等存储器实现。该存储部50包含代表点数据存储部52。
[0073]代表点数据存储部52存储由碰撞检测用数据生成装置生成的碰撞检测用数据。例如，与碰撞检测系统独立地构成碰撞检测用数据生成装置，碰撞检测用数据经由外部
I/ F部80被存储到信息存储介质90中。而且，在执行碰撞检测处理时，处理部10将信息存储介质90的碰撞检测用数据展开到存储部50的RAM中，并参照该RAM上的数据进行碰撞检测处理。此外，也可以与碰撞检测系统一体地构成碰撞检测用数据生成装置。该情况下，深度映射数据获取部112和碰撞检测用数据生成部114包含于处理部10，由处理部10生成的碰撞检测用数据储存在信息存储介质90中。
[0074]处理部10进行各种判定处理、控制处理等，例如能够通过CPU等各种处理器、专用电路(ASIC)等硬件、在处理器上执行的程序等来实现。处理部10包含对象空间设定部12、和碰撞判定部14。
[0075]对象空间设定部12进行将多个物体配置设定在对象空间的处理等。具体而言，决定在世界坐标系中的物体的位置、旋转角度，在该位置上以该旋转角度配置物体。这里所谓世界坐标系是在进行碰撞检测处理的空间中所设定的坐标系，是对碰撞检测对象的物体共用地设定的坐标系。另外所谓物体(对象)，是将机械手等碰撞检测对象物、周边构造物、周边设备等被碰撞检测对象物进行了模型化的物体。在本实施方式中，分别对各物体设定模型坐标系，通过该模型坐标系中的代表点数据来表现物体。对象空间设定部12将该模型坐标系中的代表点数据的坐标变换为世界坐标系中的坐标，从而在世界坐标系中配置多个物体。
[0076]碰撞判定部14进行碰撞检测对象的物体(第I物体)与被碰撞检测对象的物体(第2物体)之间的碰撞判定处理。具体而言，如利用图8以后后述，以立方体区域的尺寸较大的上位层的代表点数据进行碰撞判定，在存在判定为碰撞(有碰撞的可能性)的节点的情况下，以该节点的子节点的代表点数据进行碰撞判定。在最下位层存在判定为碰撞的节点的情况下，明确判定为碰撞，在与最下位层相比上位的层判定为无碰撞(没有碰撞的可能性)的情况下，不进行与该判定为无碰撞的层相比下位层的碰撞判定，明确判定为无碰撞。
[0077]操作部70用于使用者输入各种操作信息。外部I / F部80与外部之间以有线、无线的方式进行信息的通信处理等。信息存储介质90 (通过计算机可读取的介质)储存程序、数据等，其功能能够通过光盘、HDD、或者存储器等实现。处理部10基于储存在信息存储介质90中的程序(数据)来进行本实施方式的各种处理。即、在信息存储介质90中，存储用于使计算机(具备操作部、处理部、存储部、输出部的装置)作为本实施方式的各部发挥作用的程序(用于使计算机执行各部的处理的程序)。
[0078]如以上，以立方体区域对深度映射数据进行离散化并生成碰撞检测用数据，从而即使是不能够得到CAD数据的物体也能够进行碰撞检测。另外，代表点数据仅以在碰撞检测中表示重要的物体表面的数据构成，所以能够进行高效的碰撞检测处理。另外，代表点数据是不受多边形的大小影响的非冗长的数据，所以能够抑制不必要的碰撞检测处理。
[0079]在图2 (A)中示出包含本实施方式的碰撞检测系统的机械手系统的例子。该机械手系统包含控制装置300 (信息处理装置)和机械手310。控制装置300进行机械手310的控制处理。具体而言，基于动作顺序信息(方案信息)来进行使机械手310动作的控制。机械手310具有臂320以及手部(把持部)330等可动部。而且可动部根据来自控制装置300的动作指示进行动作。例如，进行把持或者移动放置在未图示的托盘上的工件等的动作。另外，基于由未图示的拍摄装置获取的拍摄图像信息来检测机械手的姿势、工件的位置等信息，检测出的信息被发送至控制装置300。
[0080]这里所谓可动部是通过可动部动作来改变(或者移动)物体间的相对的距离、姿势的。例如，本实施方式的机械手310具有机械手臂320、手部330，在使该机械手臂320、手部330移动来进行作业的情况下，构成该机械手臂320、手部330的各零件、连接该零件的关节相当于可动部。在该例中，例如手部330保持(或者把持、吸附等)物体，机械手臂320、手部330动作，从而手部330所保持的物体与机械手310周边的物体(例如构造物、设置物、部件等)相对地移动。或者，通过机械手臂320、手部330动作，从而构成机械手臂320、手部330的零件和机械手310周边的物体相对地移动，或者，以机械手臂320、手部330的关节连接的零件和零件相对地移动。在本实施方式中，检测像这样通过可动部来移动的物体间的碰撞。
[0081]本实施方式的碰撞检测系统例如设置在图2 (A)`的控制装置300，例如通过控制装置300的硬件、程序来实现碰撞检测系统。而且，在运行时间的使用中，在周边环境等动态地变化的情况下，在机械手310的实际动作之前，本实施方式的碰撞检测系统通过模拟进行碰撞的判定处理。而且，为了机械手310不碰撞周边构造物、周边设备等，控制装置300基于判定处理的结果来进行机械手310的控制。另一方面，在离线的使用中，通过本实施方式的碰撞检测系统，在作成动作顺序信息等时通过模拟来验证碰撞。而且控制装置300基于为了不发生碰撞而作成的动作顺序信息(方案信息)来控制机械手310。
[0082]此外，图2 (A)是机械手310和控制装置300独立存在的机械手系统的例子，但在本实施方式中，也可以是控制装置300内置于机械手310中的机械手。
[0083]在图2 (B)中示出包含本实施方式的碰撞检测系统的机械手的例子。该机械手包含机械手主体310 (具有臂320以及手部330)、和支撑机械手主体310的基座单元部，在该基座单元部中内置有控制装置300。在图2 (B)的机械手中，在基座单元部上设置有车轮等，机械手整体成为能够移动的构成。此外，图2 (A)是单臂型的例子，但如图2 (B)所示机械手也可以是双臂型等多臂型的机械手。此外，机械手的移动也可以以人工进行，也可以设置驱动车轮的马达，通过控制装置300控制该马达来进行。
[0084]2.碰撞检测用数据的生成方法
[0085]接下来，对本实施方式中的碰撞检测用数据的生成方法进行说明。此外，在图3~图?中，对物体OB设定模型坐标系，将该模型坐标系作为以右旋坐标系的正交XYZ坐标表示的坐标系。
[0086]如图3所示，深度映射数据获取部112获取从规定的视点(视线方向)观察物体OB时的深度映射数据ZD。规定的视点例如是从+ Z方向侦彳、一 Z方向侦彳、+ X方向侧、一 X方向侧、十Y方向侧、一 Y方向侧观察物体OB的6个视点(视线方向)。深度映射数据获取部112分别对这6个视点获取深度映射数据。在图3中，作为例子示出从+ Z方向侧的视点观察时的深度映射数据ZD的XZ平面中的剖视图。在该例中，深度映射数据的深度值沿着模型坐标系的Z轴变化，深度映射数据上的位置(像素位置)沿着模型坐标系的X轴、Y轴变化。
[0087]碰撞检测用数据生成部114以规定的间隔对X坐标、Y坐标、Z坐标进行离散化，将设定了模型坐标系的空间(以下适当地称为模型空间)离散化为立方体区域CA。该立方体区域CA是相对于模型空间以及物体OB固定的区域，在从上述的6个视点的任意一个观察的情况下都设定为同一位置。
[0088]碰撞检测用数据生成部114沿着方向DS (例如+ X方向)扫描离散化的模型空间，在与物体OB的表面(轮廓)对应的立方体区域CAH上设定代表点PA。具体而言，在关注同一 XY坐标的立方体区域的列DIR的情况下，如Al所示，在从视点观察列DIR时最初与物体OB交叉的立方体区域上设定代表点。而且，一边沿着方向DS移动一边依次进行该处理，在从视点观察时的物体OB的表面上设定代表点PA。代表点例如设定在立方体区域的重心上，由该重心的XYZ坐标表示。在深度映射数据中与最背面对应的区域上未设定代表点PA。最背面是在能够以深度值表现的深度范围中成为距离视点最远的深度的面。
[0089]这里在模型空间中的离散化相当于在深度映射数据中对深度值和其位置进行离散化，与模型空间的代表点对应，决定深度映射数据中的代表深度值和代表位置。在图3的例子中，代表点的Z坐标和代表深度值相对应，代表点的XY坐标和代表位置相对应。
[0090]如图4所示，碰撞检测用数据生成部114例如一边在+ Y方向上依次移动一边进行+ X方向的扫描，对离散化的XY坐标的全部进行代表点PA的设定处理。在图4中，仅图示设定的代表点PA的一部分，但以若扫描结束则覆盖物体OB的方式设定代表点PA。
[0091]那么，如图5的BI所示，有时即使进行上述的代表点的设定处理也不能以代表点完全覆盖物体OB的表面。这是因为仅在具有同一 XY坐标的立方体区域中的一个立方体区域中设定代表点。在使用这样的缺少代表点的数据进行碰撞判定的情况下，若其他的物体从一 X方向侧接近BI所示的立方体区域，则有可能不能够正确地检测出碰撞。
[0092]因此，如图6的El所示，碰撞检测用数据生成部114以连续地覆盖物体OB的表面的方式进行增补(补充)代表点的处理。具体而言，在关注(作为处理对象)了图5的B2所示的代表点和立方体区域的情况下，判定在B3所示的周围26个附近的立方体区域中是否缺少代表点。即、如B4所示,在从视点观察周围26个附近的立方体区域时,在与该周围26个附近的立方体区域相比更靠远方侧(进深方向侧，在图5中为一 Z方向侧)存在代表点的情况下，在与B2所示的立方体领域的远方侧邻接的BI所示的立方体区域上增补代表点。这里所谓周围26个附近的立方体区域是包围关注的立方体区域的最近的26 (= 3X3X3 一I)个立方体区域。在图5中，由于图示二维的剖视图所以为8个附近，但在三维中，相对于关注的立方体区域加上在土Y方向侧存在的立方体区域而成为26个附近。
[0093]碰撞检测用数据生成部114 一边在方向DS (例如+ X方向)上扫描一边进行上述那样的增补处理，如图5的B5、B6所示对缺少代表点的立方体区域如图6的E2、E3所示增补代表点。而且，例如一边在+ Y方向上依次移动一边进行方向DS的扫描,并对离散化的XY坐标的全部进行增补处理。
[0094]如以上所示，生成最终连续地覆盖物体OB的表面的代表点数据，从而能够正确地检测与物体OB的碰撞可能性。此外，在关注的代表点(图5的B2)的远方侧(BI)增补代表点是为了不使物体OB的凸部变大。假设若在与关注的代表点相比离视点较近的一侧增补代表点，则例如不是在B6所示的立方体区域而是在B7所示的立方体区域增补代表点。这样的话，与实际的凸部相比，代表点所表现的凸部不必要地变大，有可能实际上虽然不与凸部碰撞但也被判定为存在碰撞可能性。在本实施方式中在关注的代表点的远方侧增补代表点，所以不使凸部变大，能够进行正确的碰撞判定。
[0095]这里在图5中，以B2的立方体区域和B4的立方体区域的间隔是一个立方体区域的情况为例进行了说明，但在间隔是2个立方体区域以上的情况下，对这2个以上的立方体区域增补代表点。例如假设B4所示的代表点存在于B8所示的位置上的情况下，除了 BI所示的立方体区域之外在B9所示的立方体区域上也增补代表点。
[0096]在图7中，是从+ X方向侧的视点观察时的深度映射数据ZD’的XZ平面中的剖视图。在图7中，深度映射数据的深度值沿着模型坐标系的X轴变化，深度映射数据中的位置(像素位置)沿着模型坐标系的Y轴、Z轴变化。
[0097]如图7所示，碰撞检测用数据生成部114对于从+ Z方向侧观察的视点以外的视点，也通过上述的方法根据深度映射数据ZD’生成代表点数据。此时，如图7的Fl所示的代表点，存在与在图6的E4所示的其他的视点上的代表点重复的情况。碰撞检测用数据生成部114删除如Fl所示的重复的代表点，生成在从+ X方向侧观察的视点上的最终的代表点数据。而且，将在6个视点上生成的代表点数据合在一起，生成从全部的视点观察以代表点覆盖物体OB的表面那样的碰撞检测用数据。
[0098]如图8所示，碰撞检测用数据生成部114改变立方体区域的尺寸对模型空间进行离散化，生成与该立方体区域CB对应的代表点PB的数据。立方体区域CB的尺寸是立方体区域CA的边的长度的2倍的尺寸，对立方体区域CB进行2X2X2分割后的各区域与立方体区域CA对应。碰撞检测用数据生成部114这样依次增大立方体区域的尺寸并进行数据生成处理，生成与各尺寸对应的代表点数据，并将其合并来生成四叉树结构的数据。在四叉树中最上位的立方体区域是内含物体OB的立方体区域(包围盒(bounding box))。另外在四叉树中最下位层的立方体区域的尺寸设为例如在机械手等的碰撞检测中的允许误差程度(例如数cm)即可。
[0099]在以上说明的每层的处理中一层的处理相当于对深度映射进行三维向量量化。另夕卜，碰撞检测用数据相当于将三维向量量化的量化步长不同的数据合并到四叉树结构中的数据。三维向量量化中的量化步长与立方体区域的尺寸对应，将量化步长相同的数据作为同一层的数据合并到四叉树结构中。
[0100]此外，立方体区域CA、CB的尺寸比并不限于2倍，例如也可以是3倍、4倍等。并且，在上述中，以依次增大立方体区域的尺寸来生成数据的情况为例进行了说明，但本实施方式并不局限于此，也可以一边依次减小(例如为I / 2倍)立方体区域的尺寸一边生成数据。
[0101]3.数据构成例
[0102]在图9 (A)?图11 (C)中，是本实施方式的碰撞检测用数据生成装置所生成的四叉树结构的数据的例子。此外，以下，以将从+ Z方向侧观察的视点作为规定的视点生成3层的数据的情况为例进行说明。
[0103]图9 (A)是表示从+ Z方向侧观察时的立方体区域的分割关系的图。区域A?V表示将立方体区域相对于XY平面垂直地投影的区域。区域A与内含物体OB的立方体区域对应。而且，按照内含物体OB的立方体区域被2X2X2分割，再进一步2X2X2分割该各立方体区域，从而区域A被2X 2分割为区域B?D，区域B?D被2X 2分割为区域F?1、J?M、O?R、S?V。[0104]如图9 (B)所示，根据区域A?V的分割关系将代表点数据构成为四叉树结构。即，在四叉树结构的最上位(根)的节点A上设定与图9 (A)的区域A对应的内含物体OB的立方体区域的代表点数据。而且，在将节点A作为父节点的子节点B?E上设定存在于图9(A)的区域B?E上的代表点数据。这里，在区域B?E上，分别在Z方向上排列2个立方体区域，该在Z方向上排列的2个立方体区域中基本上在一方上设定有代表点。例如，若分割设定有图8的Gl所示的代表点的立方体区域，则成为图3的A2所示的立方体区域。在从视点观察图3的A2所示的立方体区域时，在Z方向上排列的2个立方体区域中在一方设定有代表点。在节点B?E上，基本上设定存在于这样的2个立方体区域中一方的代表点数据。同样在分别将节点B?E作为父节点的子节点F?1、J?M、O?R、S?V上，设定存在于图9 (A)的区域F?1、J?M、O?R、S?V的代表点数据。
[0105]在图9(C)上，示出各节点的数据构成例。此外，为了简单，省略了节点C?E的子节点的图示。如图9 (C)所示,节点A?V由节点Axy?Vxy、和从属于该节点Axy?Vxy的子节点Az?Vz构成。在节点Axy?Vxy上存储代表点的XY坐标,在子节点Az?Vz上存储代表点的深度值(Z值)。虽然在从±X、土Y方向侧观察的视点上的代表点数据的情况下，深度值在X、Y方向上变化，但以深度值在Z方向上变化的方式进行适当的坐标变换来构成四叉树。或者，在从±Χ、土Y方向侧观察的视点上的代表点数据中，也可以在节点Axy?Vxy上存储代表点的TL、ZX坐标，在子节点Az?Vz上存储X、Y方向中的深度值。
[0106]这里，在多个子节点与上述的父节点连接的树结构中，将该节点的父子关系中的世代称为数据的层。即，在节点的父子关系中同一世代的节点为同一层的节点。例如在图9 (B)的例子中，根节点A构成一个层，该根节点A的子节点B?E构成一个层。而且，将子节点B?E作为父节点的子节点F?V (从根节点A来看是孙子一代的节点)再构成一个层。
[0107]在图10 (A)中，示出进行了以图6等说明的代表点增补处理的情况下的数据构成例。例如设为对节点C的代表点Ca，增补有在同一 XY坐标中深度值不同的2个代表点Cb、Ce。该情况下，将这些同一 XY坐标的3个代表点数据Ca、Cb、Ce设定为一个节点C。
[0108]具体而言，如图10 (B)所示，将代表点数据Ca、Cb、Ce的共用的XY坐标存储到节点Cxy中，并在从属于该节点Cxy的子节点Caz、Cbz、Ccz中分别存储代表点数据Ca、Cb、Cc的深度值。假定与节点Cxy连接的子节点Caz、Cbz、Ccz例如由列表结构构成。若像这样构成数据，则节点Axy?Vxy—定以四叉树连接，所以即使存在增补数据也能够构成四叉树结构的数据。此外，在图10 (B)中为了简单，省略节点C?E的子节点的图示。
[0109]图11 (A)是表示从+ Z方向侧观察时的物体OBX和立方体区域的图。在图11(A)的例子中，分割区域E而成的区域S?V中，与物体OBX交叉的区域只是S。因此如图
11(B)所示，与节点E连接的子节点S?V中，存在代表点的只是节点S。
[0110]该情况下,如图11 (C)所示,在节点Sxy、子节点Sz中分别存储代表点的XY坐标、深度值。在节点Txy?Vxy中不存在代表点，但存储设定代表点的位置的XY坐标。在子节点Tz?Vz中不存储深度值，而在节点Txy?Vxy上连接NULL列表。例如，在图11 (C)中从节点Axy?Vxy指向子节点Az?Vz的箭头在实际处理中例如以指针实现的情况下,指示子节点Tz?Vz的箭头例如以NULL指针实现。若这样构成数据，则节点Axy?Vxy —定以四叉树连接，所以即使不存在代表点的节点存在也能够构成四叉树结构的数据。此外，在图11 (C)中为了简单省略了节点C?E的子节点的图示。
[0111]4.碰撞检测的方法
[0112]接下来，对在图1 (B)中说明的碰撞检测系统进行的碰撞检测的方法进行说明。在图12中，示出作为进行碰撞判定的对象的第I物体OBl和第2物体0B2的、世界坐标系中的剖视图。物体0B1、0B2例如是机械手等的零件、连接该零件间的关节部分、配置在机械手等的作业空间中的构造物等。
[0113]如图12所示，碰撞判定部14首先使用碰撞检测用数据中立方体区域的尺寸最大的代表点数据(即，四叉树结构的根数据)进行碰撞判定。具体而言，对象空间设定部12将四叉树结构的根的代表点和立方体领域从物体OB1、0B2的模型坐标系坐标变换为世界坐标系。而且,碰撞判定部14判断物体0B1、0B2的立方体区域BA1、BA2在世界坐标系中是否交叉。
[0114]具体而言如图13 (A)所示，碰撞判定部14求出物体OBl的代表点DPl和物体0B2的代表点DP2的距离DS。这里，将物体OBl的立方体区域B Al的一边的长度设为SI I，将物体0B2的立方体区域BA2的一边的长度设为SI2。在碰撞判定部14判定为满足DS > V 3X(SIl + SI2)的情况下，判断为内含立方体区域BAl的球KYl和内含立方体区域BA2的球KY2无交叉。该情况下，明确判定为立方体区域BA1、BA2无碰撞。
[0115]另一方面，如图13 (B)所示，在判定为不满足DS > V 3X (SIl + SI2)的情况下，判断为球KY1、KY2交叉，并判定立方体区域BA1、BA2是否交叉。具体而言如图13 (C)所示，碰撞判定部14基于立方体区域BAl、BA2的相对位置和相对的旋转角度来进行交叉判定。该相对位置和相对的旋转角度例如能够根据世界坐标系中的立方体区域BA1、BA2的位置和姿势得知，例如以立方体区域BA2为基准来求出立方体区域BAl的位置和旋转角度即可。
[0116]这样，通过组合球的交叉判定和立方体区域的交叉判定，从而能够从简处理。gp，在球不交叉的情况下，能够只通过与立方体区域的交叉判定相比比较简单的处理亦即球的交叉判定就结束。此外，在上述中在判定为球交叉的情况下判定立方体区域是否交叉，但在本实施方式中在判定为球交叉的时刻也可以明确判定为立方体区域发生碰撞。该情况下，能够进一步将处理简单化。
[0117]在上述的交叉判定中判定为立方体区域BAl、BA2交叉的情况下，碰撞判定部14在比立方体区域BA1、BA2小的尺寸的立方体区域进行碰撞判定。即，如图14所示，对全部的组合进行分割立方体区域BAl而成的立方体区域BBl?BGl、和分割立方体区域BA2而成的立方体区域BB2?BG2的交叉判定。例如在判定为立方体区域BBl和立方体区域BB2、BC2交叉的情况下，如图15所示，在进一步分割立方体区域BB1、BB2、BC2而成的立方体区域进行交叉判定。
[0118]具体而言，碰撞判定部14在判定为在立方体区域BB1、BB2内代表点最近的立方体区域BH1、BH2不交叉、且判定为在立方体区域BB1、BC2内代表点最近的立方体区域BI1、BI2不交叉的情况下，明确判定为物体0B1、0B2无碰撞。另一方面，在存在判定为交叉的立方体区域、且判定的层是四叉树结构的最下位层的情况下，明确判定为物体OB1、0B2存在碰撞可能性。
[0119]应予说明，在上述的图14、图15中，省略四叉树结构的一层的碰撞判定，图示2层下的碰撞判定，但实际上I层层地进行碰撞判定。
[0120]S卩，如图16所示，碰撞判定部14在物体OB1、0B2的四叉树结构的数据中，首先使用根的节点NA1、NA2的代表点数据进行碰撞判定。而且，在判定为节点NA1、NA2的立方体区域交叉的情况下，使用节点ΝΑΙ、NA2的子节点NBl?NE1、NB2?NE2的代表点数据，对全部的组合进行碰撞判定。例如在判定为节点NE1、NC2的立方体区域交叉的情况下，使用节点NEl的子节点即、NSl?NVl的代表点数据和NC2的子节点即、NJ2?匪2的代表点数据，对这些代表点数据的全部的组合进行碰撞判定。另一方面，对于判定为立方体区域无交叉的节点的子节点，不再进行碰撞判定。
[0121]在节点NSl?NV1、NJ2?匪2的碰撞判定中，例如为判定为节点NT1、NK2的立方体区域交叉。在图16的例子中，节点NTl是最下位层的节点，节点NK2存在更下位的子节点。该情况下，碰撞判定部14使用节点NTl的代表点数据、和节点NK2的子节点NW2?NZ2的代表点数据进行碰撞判定。在判定为节点NTl的立方体区域、和作为最下位层的节点的例如节点NX2的立方体区域交叉的情况下，明确判定为物体OB1、0B2有碰撞可能性。另一方面，在判定为在任意一层中全部的节点无碰撞的情况下，明确判定为在该时刻物体0B1、0B2无碰撞，并结束针对物体OB1、0B2的碰撞判定。
[0122]于是如上述那样，在使用了多边形数据的以往的碰撞检测方法中，由于现实中存在很多不能够得到CAD数据的物体，所以存在难以应用于那样的物体的课题。另外，存在依赖于多边形的大小而处理变得冗长这样的课题、包含多个在碰撞检测中不重要的多边形数据所以进行不必要的处理这样的课题。
[0123]这一点在本实施方式中，如利用图3等说明的那样，碰撞检测用数据生成部114通过利用在物体OB的模型坐标系中设定的立方体区域CA对深度映射数据进行离散化，从而生成物体OB的模型坐标系中的代表点PA的数据作为碰撞检测用数据。这里所谓的模型坐标系，是对各个碰撞检测对象的物体设定的模型空间的坐标系。另外所谓的立方体区域是在模型空间中各边的长度相同的立方体，在深度映射数据上，边长与深度以及平面方向的距离对应。另外所谓的代表点数据是表示代表立方体区域的位置的代表点(例如立方体区域的中心点)的数据，是深度映射数据上的代表深度值以及代表位置的数据(或者模型坐标系中的XYZ坐标的数据)。
[0124]另外，在本实施方式中，存储部50存储代表点PA的数据作为碰撞检测用数据，处理部10基于与第I物体对应的第I碰撞检测用数据和与第2物体对应的第2碰撞检测用数据，进行世界坐标系中的第I物体和第2物体的碰撞判定。这里所谓世界坐标系例如与机械手的作业空间等对应，通过从模型坐标系进行坐标变换将碰撞检测对象的物体配置到世界坐标系。
[0125]这样在本实施方式中，能够根据深度映射数据生成碰撞检测用数据，所以即使没有CAD数据也能够进行碰撞检测。例如，在碰撞检测对象的物体中存在没有CAD数据的物体的情况下，通过使用3D扫描仪等来获取该物体的深度映射数据，从而能够生成碰撞检测用数据。
[0126]另外，将模型空间以立方体区域进行离散化，所以在代表点数据中不产生像多边形那样的大小的偏差，也不产生以球覆盖了多边形的情况下那样的冗长的重叠。使用这样的非冗长的数据从而能够减少碰撞检测的处理负荷。另外，通过使用深度映射数据，能够仅在物体的外侧表面设定代表点，所以不产生使用多边形的情况下的物体内部的数据，能够消除在碰撞检测中不重要的不必要的处理。另外，进行碰撞检测的节点对的个数与节点数的平方成比例，所以在不产生冗长的数据、不必要的数据的本实施方式中，能够期待处理的高速化。
[0127]这里所谓的节点对是作为碰撞判定的判定对象而选择的I组节点。在图16中说明的进行第I对象OBl和第2对象0B2之间的碰撞判定的情况下，从OBl的数据中选择的一个节点、和从0B2的数据中选择的一个节点的组合成为节点对。在本实施方式中，对判定为有碰撞的节点的子节点按照每个层进行碰撞判定，所以例如从OBl的第3层中的NEl的子节点NSl?NVl中选择一个节点，从0B2的第3层中的NC2的子节点NJ2?匪2中选择一个节点，从而选择节点对。这样在本实施方式中，从判定对象的层、判定对象的子节点中选择出的节点的组合是节点对。
[0128]另外在本实施方式中，存储部50存储内含物体OB的包围盒(图9 (A)的区域A)的代表点数据(图9 (B)的节点A)、和通过分割该包围盒而成的立方体区域(区域B?EWi深度映射数据进行离散化而得到的分割代表点数据(节点B?E)作为碰撞检测用数据。在判定为内含第I物体OBl的包围盒(图16的节点NAl)和内含第2物体0B2的包围盒(节点NA2)发生碰撞的情况下，处理部10基于第I物体OBl的分割代表点数据(节点NBl?NEl)和第2物体0B2的分割代表点数据(节点NB2?NE2)进行碰撞判定。
[0129]若这样，在判定为包围盒无碰撞的阶段，不进行分割代表点数据的处理而能够明确判定为第I物体和第2物体无碰撞,所以能够简单化处理。
[0130]更具体而言，碰撞检测用数据生成部114将与分割父节点(例如图9 (B)的节点B)的立方体区域(图9 (A)的区域B)而成的多个立方体区域(区域F?I)对应的节点(节点F?I)作为子节点与父节点连接，并生成树结构的数据作为碰撞检测用数据。碰撞检测系统的存储部50存储这样的树结构的数据，处理部10基于该树结构的数据进行碰撞检测。
[0131]通过使用这样的树结构的数据，从而能够进行节点对的递归的碰撞检测，所以例如能够容易地实现由CPU等进行的并行处理。具体而言，在本实施方式中生成四叉树结构的数据，并在使用了这样的四叉树结构的情况下，在递归的碰撞检测的各层中进行4X4 =16组的节点对的碰撞检测。因此，即使不使用能够进行数万?数十万的并行处理的GPU(Graphics Processing Unit:图形处理单元)，也能够使用进行数十线程左右的并行处理的CPU来实现碰撞检测系统，能够实现由于省略GPU而带来的成本减少。
[0132]应予说明本实施方式的树结构的数据并不限于四叉树结构的数据，例如也可以是最小立方体区域的节点与包围盒的节点直接连接(没有中间层)的数据。该情况下，最小立方体区域的节点对的组合数量增多，所以假定使用GPU等构成碰撞检测系统。
[0133]5.碰撞检测用数据生成装置的详细构成
[0134]在图17中示出本实施方式的碰撞检测用数据生成装置的详细构成例。该碰撞检测用数据生成装置包含处理部110、和存储部150。处理部110包含深度映射数据获取部112、代表点设定部200、和四叉树结构生成部220。存储部150包含存储物体I?物体N的碰撞检测用数据的代表点数据存储部MAl?MN。
[0135]代表点设定部200进行对模型空间进行离散化并设定代表点的处理，包含空间离散化部202、代表点选择部204、代表点增补部206、和代表点重复删除部208。此外，代表点设定部200和四叉树结构生成部220与图1 (A)的碰撞检测用数据生成部114对应。
[0136]接下来，使用图18?图20的流程图对该碰撞检测用数据生成装置的详细处理例进行说明。
[0137]如图18所示，若开始数据生成处理，则深度映射数据获取部112获取多个视点的深度映射数据(步骤SI)。深度映射数据获取部112例如基于从CAD数据输入部280输入的CAD数据(多边形数据)，对从多个视点观察的物体进行描绘，并生成针对各视点的深度映射数据。或者，基于从三维信息测量装置290输入的信息来获取深度映射数据。作为三维信息测量装置290例如假定3D扫描仪、立体相机等。在是3D扫描仪的情况下，深度映射数据获取部112获取3D扫描仪生成的深度映射数据。
[0138]接下来，空间离散化部202将根节点的立方体区域的一边的长度(最大离散化值)设定为离散化值SI (步骤S2)。接着，空间离散化部202判定离散化值SI是否比预先设定的规定的最小值小(步骤S3)。作为规定的最小值，例如考虑机械手等的位置把握精度，设定为比接近允许误差小的值。在离散化值SI比规定的最小值小的情况下，在代表点数据存储部MAl?MAN中对应的存储部中储存数据，结束数据生成处理。在离散化值SI是规定的最小值以上的情况下，进行四叉树结构的一层的数据生成处理(步骤S4)。一层的数据生成处理的详细内容后述。
[0139]接下来，四叉树结构生成部220判定是否存在步骤S4中数据被生成的层的上位层(步骤S5)。在存在上位层的情况下，四叉树结构生成部220进行将在步骤S4中生成的数据构成为四叉树结构的处理(步骤S6)，空间离散化部202执行步骤S7。四叉树结构生成处理的详细内容后述。在不存在上位层的情况下，空间离散化部202将离散化值SI更新为I /2倍的值(步骤S7)，并再次执行步骤S3。
[0140]在图19中示出步骤S4的一层的数据生成处理的详细流程图。应予说明在图19中，以从视点远离的方向上深度值增大的情况为例进行说明，但本实施方式并不限于此。
[0141]若开始该处理，则空间离散化部202设定模型空间的离散化值SI (步骤S20)。接下来空间离散化部202判定是否对多个视点的全部视点进行了代表点的设定处理(步骤S21)。在存在未处理的视点的情况下，从未处理的视点中选择一个视点(步骤S22)。接下来空间离散化部202以I边的长度为离散化值SI的立方体区域对深度映射数据进行离散化(步骤S23)。
[0142]接下来代表点选择部204扫描离散化的深度映射数据，设定代表点(步骤S24)。接下来代表点选择部204删除深度映射中的最背面的代表点数据(步骤S25)。最背面的代表点数据是具有能够取得的深度值范围中最大(或者其附近)的深度值的代表点。
[0143]接下来代表点增补部206扫描设定的代表点，在代表点、和比该代表点的26个附近外侧的代表点之间未连续地设定代表点的情况下，在该代表点的背面侧(深度值较大的一侧，例如图5的一 Z方向侧)增补代表点(步骤S26)。具体而言，代表点增补部206求出从关注的代表点(例如图5的B2)的深度值减去了存在于该代表点的附近的代表位置上的代表点(B4)的深度值后的差量DV。这里所谓的附近的代表位置是在从视点观察关注的代表点时，以该关注的代表点的代表位置为中心的周围8个代表位置。而且，在差量DV < O、且I DV I > SI的情况下，代表点增补部206在关注的代表点的背面侧(BI)增补I DV I /SI个代表点。若步骤S26结束，则执行步骤S20。[0144]在步骤S21中，在对多个视点的全部的视点设定了代表点的情况下，代表点重复删除部208判定是否对全部的代表点进行了在多个视点的代表点中删除重复的代表点的处理(步骤S27)。在存在未处理的代表点的情况下，代表点重复删除部208从未处理的代表点中选择一个代表点(步骤S28)。代表点重复删除部208对选择的代表点和其他的全部的代表点进行比较，在存在相同的代表点的情况下，删除该相同的代表点(步骤S29)，执行步骤S27。在步骤S27中，在对全部的代表点结束了处理的情况下，结束一层的数据生成处理。
[0145]在图20中，示出步骤S6的四叉树结构生成处理的详细流程图。若开始该处理，则四叉树结构生成部220获取在步骤S4中数据被生成的层的上位层的离散化值SI (步骤
540)。接下来，四叉树结构生成部220判定是否对多个视点的全部的视点进行了处理(步骤
541)。在对全部的视点结束了处理的情况下，结束四叉树结构生成处理。在存在未处理的视点的情况下，从未处理的视点中选择一个视点(步骤S42)。
[0146]接下来四叉树结构生成部220判定是否对上位层的全部的代表点进行了处理(步骤S43)。例如在对图9 (B)的节点F?V进行处理的情况下，对作为其上位层的节点B?C的全部进行判定。在对上位层的全部的代表点结束了处理的情况下，执行步骤S41。在存在未处理的代表点的情况下，四叉树结构生成部220从未处理的代表点中选择一个代表点(步骤S44)。接下来四叉树结构生成部220将该选择的代表点作为父节点来连接子节点的代表点，构成下位层的四叉树结构(步骤S45)。例如在选择图9 (C)的节点Bxy的情况下，节点Bxy是父节点,将子节点Fxy?Ixy、Fz?Iz与该节点Bxy连接。在节点Fxy?Ixy上设定代表位置的XY坐标，节点Fz?Iz在该时刻是空的节点(例如NULL节点)。
[0147]接下来四叉树结构生成部220判定是否对全部4个子节点进行了代表点数据的设定处理(步骤S46)。在对全部4个子节点结束了处理的情况下，执行步骤S43。在存在未处理的子节点的情况下，四叉树结构生成部220从未处理的子节点中选择一个子节点(步骤S47)。接下来四叉树结构生成部220检测存在于所选择的子节点的代表位置上的代表点(步骤S48)。
[0148]接下来，判定在选择的子节点的代表位置上是否检测出代表点(步骤S49)。在检测出代表点的情况下，四叉树结构生成部220将检测出全部的代表点与子节点连结(步骤S50)。例如在选择图9 (C)的节点Fxy，只检测出一个代表点的情况下，将这一个代表点的代表深度值作为节点Fz与节点Fxy连接。另外在如图10 (B)的节点Cxy那样检测出多个代表点的情况下，将该多个代表点的代表深度值作为列表结构的节点Ca z?Ccz与节点Cxy连接。在步骤S49中未检测出代表点的情况下，四叉树结构生成部220设定在子节点上没有代表点的意思的信息(步骤S51)。例如以图11 (C)说明的那样，对节点Txy连接NULL节点Tz。若步骤S50、S51结束了则执行步骤S46。
[0149]6.碰撞检测系统的详细构成
[0150]在图21中示出本实施方式的碰撞检测系统的详细构成例。该碰撞检测系统包含处理部10、和存储部50。处理部10包含代表点数据选择部250、递归的节点对碰撞检测部260、和碰撞判定输出部270。存储部50包含存储物体I?物体N的碰撞检测用数据的代表点数据存储部MBl?ΜΒΝ。
[0151]此外，代表点数据选择部250、递归的节点对碰撞检测部260、碰撞判定输出部270与图1 (B)的碰撞判定部14、对象空间设定部12对应。这里，在一体地构成碰撞检测系统和碰撞检测用数据生成装置的情况下，也可以将代表点数据存储部MBl?MBN和图17的代表点数据存储部MAl?MAN共用化。
[0152]接下来，利用图22、图23的流程图对该碰撞检测系统的详细处理例进行说明。
[0153]如图22所示，若开始碰撞检测处理，则代表点数据选择部250判定是否对全部的物体的组合进行了碰撞检测处理(步骤S60)。在存在未处理的组合的情况下，代表点数据选择部250从未处理的组合中选择I组物体(第I物体、第2物体)(步骤S61)。
[0154]接下来递归的节点对碰撞检测部260将第I物体的最上位节点设定为节点NI，将第2物体的最上位节点设定为节点N2 (步骤S62)。接下来递归的节点对碰撞检测部260对节点N1、N2进行递归的节点对碰撞检测处理(步骤S63)。该递归的节点对碰撞检测处理的详细内容后述。接下来碰撞判定输出部270输出碰撞判定结果,处理部10进行与碰撞判定结果对应的各种处理(步骤S64)。例如，在判定为发生碰撞的情况下，处理部10进行将物体的轨道修正为不碰撞的处理、停止动作的处理等。若步骤S64结束了则执行步骤S60。在步骤S60中在对全部的物体的组合都结束了处理的情况下，结束碰撞检测处理。
[0155]在图23中示出递归的节点对碰撞检测处理的详细流程图。若开始该处理，则递归的节点对碰撞检测部260设定作为处理对象的节点对的节点N1、N2 (步骤S80)。接下来，判定在世界坐标系中在节点N1、N2的立方体区域上是否有重叠(步骤S81)。在没有重叠的情况下，判断为节点N1、N2无碰撞并结束处理。在有重叠的情况下，递归的节点对碰撞检测部260判定对于节点NI是否存在子节点(步骤S82)。
[0156]在对于节点NI存在子节点的情况下，递归的节点对碰撞检测部260判定对于节点N2是否存在子节点(步骤S83)。在对于节点N2存在子节点的情况下，对节点N1、N2的子节点的全部组合递归地进行节点对的碰撞检测(步骤S84)。S卩，在节点N1、N2的子节点的组合中，存在判定为立方体区域重叠的组合(例如图16的节点NEl、NCl)的情况下，将该节点对重新设定为节点N1、N2，并再次执行步骤S81以下的处理。该递归的碰撞检测针对在步骤S84中判定为立方体区域重叠的全部节点对来进行。假设，到最下位层为止存在立方体领域重叠的节点对，则直到最下位层反复进行该递归的处理。在步骤S83中对于节点N2不存在子节点的情况下，递归的节点对碰撞检测部260针对节点NI的子节点和节点N2的全部组合进行递归的节点对的碰撞检测(步骤S85)。
[0157]在步骤S82中对于节点NI不存在子节点的情况下，递归的节点对碰撞检测部260判定对于节点N2是否存在子节点(步骤S86)。在对于节点N2存在子节点的情况下，对节点NI和节点N2的子节点的全部组合，递归地进行节点对的碰撞检测(步骤S87)。在对于节点N2不存在子节点的情况下，判断为节点N1、N2存在碰撞可能性并结束处理。
[0158]在执行步骤S84、S85、S87的情况下，递归的节点对碰撞检测部260在该递归的节点对碰撞检测中判定在最下位层的节点对中是否检测出碰撞(步骤S88)，并输出该判定结果后结束处理。
[0159]此外，如上述那样对本实施方式进行了详细说明，但能够进行在实质上不脱离本发明的新的事项以及效果的多数的变形对于本领域技术人员来说是能够容易地理解的。因此，这样的变形例全部包含在本发明的范围内。例如，在说明书或者附图中，至少一次，共同记载了更广义或者同义的不同的用语的用语即使在说明书或者附图的任何位置上，都能够置换为该不同的用语。另外本实施方式以及变形例的全部组合也包含在本发明的范围内。另外，碰撞检测用数据生成装置、碰撞检测系统的构成/动作、碰撞检测用数据的生成方法、碰撞检测的方法等也不限于在本实施方式中说明的例子，能够进行各种变形实施。[0160]图中符号说明:[0161]10...处理部，12...对象空间设定部，14...碰撞判定部，50...存储部，52...代表点数据存储部，70…操作部，80...外部I / F部，90...信息存储介质，110…处理部，112…深度映射数据获取部，114…碰撞检测用数据生成部，150…存储部，170…操作部，180…外部I /F部，190...信息存储介质，200...代表点设定部，202...空间离散化部，204...代表点选择部，206…代表点增补部，208…代表点重复删除部，220…四叉树结构生成部，250…代表点数据选择部，260…递归的节点对碰撞检测部，270…碰撞判定输出部，280…CAD数据输入部，290…三维信息测量装置，300…控制装置，310…机械手，320…臂，A~V…节点，Axy~Vxy…节点，Az~Vz…子节点，CA、CB…立方体区域，MAl~MAN、MB1_MBN…代表点数据存储部，OB…物体，OBl…第I物体，0B2…第2物体，PA、PB…代表点，SI…离散化值，ZD…深度映射数据。
【权利要求】
1.一种碰撞检测系统，其特征在于，包含: 存储部，其存储与第I物体对应的第I碰撞检测用数据和与第2物体对应的第2碰撞检测用数据作为物体的碰撞检测用数据；以及 处理部，其基于所述第I碰撞检测用数据和所述第2碰撞检测用数据来进行世界坐标系中的所述第I物体与所述第2物体的碰撞判定， 所述存储部存储代表点数据作为所述碰撞检测用数据，该代表点数据是对在所述物体的模型坐标系中从规定的视点观察所述物体时的深度映射数据利用设定在所述模型坐标系中的立方体区域进行离散化而得到的。
2.根据权利要求1所述的碰撞检测系统，其特征在于， 所述存储部存储内含所述物体的包围盒的所述代表点数据、和利用分割所述包围盒而成的立方体区域对所述深度映射数据进行离散化而得到的所述代表点数据亦即分割代表点数据，作为所述碰撞检测用数据， 在判定为内含所述第I物体的第I包围盒和内含所述第2物体的第2包围盒发生碰撞的情况下，所述处理部基于所述第I物体的所述分割代表点数据和所述第2物体的所述分割代表点数据来进行所述第I物体与所述第2物体的碰撞判定。
3.根据权利要求1所述的碰撞检测系统，其特征在于， 所述存储部存储树结构的数据作为所述碰撞检测用数据， 所述树结构的数据具有与分割父节点的立方体区域而成的多个立方体区域对应的所述代表点数据作为从所述父节点分支的子节点的所述代表点数据。
4.根据权利要求3所述的碰撞检测系统，其特征在于， 分割所述父节点的立方体区域而成的所述子节点的所述多个立方体区域是通过将从所述规定的视点观察时的所述父节点的立方体区域分割为2X2区域，并且将所述2X2区域的各区域在所述规定的视点的进深方向上进行2分割而得到的2X2X2个立方体区域， 所述树结构的数据是与从所述规定的视点观察时的所述2X2区域的各区域对应地设定有所述子节点的四叉树结构的数据， 所述四叉树结构中的所述子节点的数据是在所述2X2区域的所述各区域中存在于所述进深方向的2个立方体区域中的至少一方的所述代表点数据。
5.根据权利要求3所述的碰撞检测系统，其特征在于， 在基于所述父节点的数据的所述碰撞判定中，在存在判定为碰撞的所述父节点的情况下，所述处理部基于从判定为所述碰撞的所述父节点分支的所述子节点的数据来进行所述碰撞判定， 在基于所述父节点的数据的所述碰撞判定中，在不存在判定为碰撞的所述父节点的情况下，所述处理部明确判定为所述第I物体与所述第2物体无碰撞。
6.根据权利要求1所述的碰撞检测系统，其特征在于， 所述深度映射数据是由测量所述物体的三维信息的三维信息测量装置生成的深度映射数据。
7.—种碰撞检测用数据生成装置，其特征在于，包含: 深度映射数据获取部，其获取物体的模型坐标系中的从规定的视点观察所述物体时的深度映射数据；以及碰撞检测用数据生成部，其生成所述物体的所述模型坐标系中的代表点数据作为所述碰撞检测用数据， 所述碰撞检测用数据生成部利用设定在所述物体的所述模型坐标系中的立方体区域对所述深度映射数据进行离散化，从而生成所述代表点数据。
8.根据权利要求7所述的碰撞检测用数据生成装置，其特征在于， 所述碰撞检测用数据生成部将与分割父节点的立方体区域而成的多个立方体区域对应的节点作为子节点与所述父节点连接，并生成树结构的数据作为所述碰撞检测用数据。
9.根据权利要求8所述的碰撞检测用数据生成装置，其特征在于，分割所述父节点的立方体区域而成的所述子节点的所述多个立方体区域是通过将从所述规定的视点观察时的所述父节点的立方体区域分割为2X2区域，并且将所述2X2区域的各区域在所述规定的视点的进深方向上进行2分割而得到的2X2X2个立方体区域，所述碰撞检测用数据生成部生成与从所述规定的视点观察时的所述2X2区域的各区域对应地设置有所述子节点的四叉树结构的数据，作为所述树结构的数据， 所述四叉树结构的数据中的所述子节点的数据是在所述2X2区域的所述各区域中存在于所述进深方向的2个立方体区域中的至少一方的所述代表点数据。
10.根据权利要求7所述的碰撞检测用数据生成装置，其特征在于， 在判断为在处理对象的代表点和存在于所述处理对象的代表点的周围26个附近的立方体区域的外侧的代表点之间存在缺少所述代表点数据的立方体区域的情况下，所述碰撞检测用数据生成部在缺少所述代表点数据的立方体区域上增补所述代表点数据。
11.根据权利要求10所述的碰撞检测用数据生成装置，其特征在于， 当在所述规定的视点的进深方向上越远离深度值越大时， 在判定为从所述处理对象的代表点的代表深度值减去存在于所述处理对象的代表点的周围的代表点的代表深度值后的差值为负的情况下，所述碰撞检测用数据生成部在相对于所述处理对象的代表点靠所述进深方向侧的立方体区域上增补所述代表点数据。
12.根据权利要求7所述的碰撞检测用数据生成装置，其特征在于， 所述深度映射数据是由测量所述物体的三维信息的三维信息测量装置生成的深度映射数据。
13.一种机械手，其特征在于，包含: 可动部； 存储部，其存储与第I物体对应的第I碰撞检测用数据和与第2物体对应的第2碰撞检测用数据作为物体的碰撞检测用数据； 处理部，其基于所述第I碰撞检测用数据和所述第2碰撞检测用数据来进行世界坐标系中的所述第I物体与所述第2物体的碰撞判定；以及 控制部，其基于由所述处理部进行的所述碰撞判定的结果来控制所述可动部的动作，所述存储部存储代表点数据作为所述碰撞检测用数据，该代表点数据是对在所述物体的模型坐标系中从规定的视点观察所述物体时的深度映射数据利用设定在所述模型坐标系中的立方体区域进行离散化而得到的。
【文档编号】B25J19/00GK103568022SQ201310303138
【公开日】2014年2月12日 申请日期:2013年7月18日 优先权日:2012年7月20日
【发明者】稻积满广 申请人:精工爱普生株式会社