碰撞檢測系統、碰撞檢測用數據生成裝置以及機械手的制作方法

碰撞檢測系統、碰撞檢測用數據生成裝置以及機械手的制作方法
【專利摘要】本發明涉及碰撞檢測系統、碰撞檢測用數據生成裝置以及機械手。其中碰撞檢測系統包含：存儲部，其存儲與第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日
【發明者】稻積滿廣 申請人:精工愛普生株式會社