TWI809899B - 控制裝置、控制方法以及程式 - Google Patents

Abstract

控制裝置包括：生成部，生成一指令，所述指令用於使機械手依據自群組中根據風險值而選擇的對象動作來運作；以及設定部，在機械手較基準位置接近開始位置時設定第一群組，在機械手較基準位置接近終點位置時設定第二群組，以作為群組。第一群組包含變更軌道的第一動作，來作為風險值處於第一範圍內時的動作。第二群組包含第一動作來作為風險值處於第一範圍的第一子範圍內時的動作，且包含不變更軌道而使速度下降的第二動作，來作為風險值處於所述第一範圍的第二子範圍內時的動作。

Description

控制裝置、控制方法以及程式

本揭示是有關於一種控制裝置、控制方法以及程式。

近年來，在工廠自動化（Factory Automation，FA）領域中，正在推進人與機器人協同作業的協同作業系統的開發。在協同作業系統中，必須避免人與機器人的干涉。

「賈斯汀·米塞基斯及另外三名，「用於預測和自反性機械手軌跡預估的多3D攝影機映射」（2016 IEEE計算智能（SSCI）系列研討會）（Justinas Miseikis，他3名，「Multi 3D camera mapping for predictive and reflexive robot manipulator trajectory estimation」，2016 IEEE Symposium Series on Computational Intelligence (SSCI)）」（非專利文獻1）揭示了一種技術：根據表示物體與機器人的干涉的可能性的成本來變更機器人的軌道，以避免物體與機器人的干涉。 [現有技術文獻] [非專利文獻]

[非專利文獻1]賈斯汀·米塞基斯及另外三名，「用於預測和自反性機械手軌跡預估的多3D攝影機映射」（2016 IEEE計算智能（SSCI）系列研討會）

[發明所欲解決之課題]

非專利文獻1所揭示的技術中，在物體為人的情況下，可能會對應於人的移動來頻繁地實施機器人的軌道變更。機器人的軌道變更會伴隨大的計算負載。因此，期望一種減輕計算負載並且抑制物體與機器人的干涉的發生的技術。

本揭示是有鑒於所述問題而完成，其目的在於提供一種控制裝置、控制方法以及程式，可減輕計算負載，並且抑制物體與機器人的干涉的發生。 [解決課題之手段]

根據本揭示的一例，控制裝置控制機器人，以使機器人的機械手自開始位置移動至終點位置為止。控制裝置包括：計算部，算出機械手與物體的干涉的風險值；以及生成部，生成一指令，所述指令用於使機械手依據自包含多種動作的群組中根據風險值而選擇的對象動作來運作。進而，控制裝置包括設定部，所述設定部在機械手較基準位置接近開始位置時設定第一群組，在機械手較基準位置接近終點位置時設定第二群組，以作為群組。第一群組包含變更軌道以降低風險值的第一動作，來作為風險值處於第一範圍內時的動作。第二群組包含第一動作，來作為風險值處於第一範圍的一部分即第一子範圍內時的動作，且包含不變更軌道而使速度下降的第二動作，來作為風險值處於第一範圍中的與第一子範圍不同的第二子範圍內時的動作。

根據該揭示，當機械手接近終點位置時，變更軌道的頻率下降，控制裝置的計算負載得以減輕。而且，當機械手較基準位置接近開始位置，且風險值處於第一範圍內時，變更機械手的軌道以降低風險值。進而，當機械手較基準位置接近終點位置，且風險值處於第一子範圍內時，亦變更機械手的軌道以降低風險值。藉此，可抑制物體與機械手的干涉的發生。如此，根據所述控制裝置，可減輕計算負載，並且抑制物體與機器人的干涉的發生。

所述的揭示中，第一群組以及第二群組各自包含不變更機械手的軌道而以標準速度來使機械手運作的第三動作，來作為風險值處於較第一範圍低的第二範圍內時的動作。第二動作是以低於標準速度的速度來使機械手運作。

根據該揭示，當風險值處於第二範圍內時，不變更軌道。因此，變更軌道的頻率下降，控制裝置的計算負載得以減輕。而且，由於風險值處於較第一範圍低的第二範圍內，因此可使機械手以較處於第二子範圍內時的速度快的標準速度來運作。

所述的揭示中，第一群組以及第二群組各自包含使機械手朝向開始位置進行返回的第四動作，來作為風險值處於較第一範圍高的第三範圍內時的動作。

根據該揭示，機械手朝遠離物體的方向移動。其結果，物體與機器人的干涉得以抑制。

所述的揭示中，計算部以物體相對於機械手的相對移動方向與連結物體和機械手的線所成的角度越接近零度則變得越大的方式，來算出風險值。

根據該揭示，物體朝向機械手移動的可能性越高時，風險值變得越大。因此，風險值可準確地表示物體與機械手發生干涉的可能性。

所述的揭示中，計算部以物體的速度越快則變得越大的方式來算出風險值，並且以物體與機械手的距離越短則變得越大的方式來算出風險值。

藉由該揭示，風險值亦能準確地表示物體與機械手發生干涉的可能性。

所述的揭示中，計算部以沿著機械手的軌道的自開始位置直至機械手的當前位置為止的長度越長則變得越大的方式來算出風險值。

與機器人的動作剛剛開始之後相比，機械手接近終點位置時，因物體（例如人）與機械手的干涉造成的對物體的危害更大。根據該揭示，風險值可表示反映出對物體的危害的大小的值。

所述的揭示中，計算部使用風險函數來算出風險值，所述風險函數包含含有物體與機械手的距離和物體的速度的項、以及常數項。常數項的值為負。第二範圍的上限值為0。

根據該揭示，藉由風險值小於0，可容易地認識到物體與機械手發生干涉的可能性小的情況。

所述的揭示中，計算部使用風險函數來算出風險值。當將物體相對於機械手的相對移動方向與連結物體和機械手的線所成的角度的餘弦設為x，將物體的速度設為v _obs，將機械手與物體的距離設為D，將沿著機械手的軌道的自開始位置直至機械手的當前位置為止的長度設為S _cur時，風險函數包含 H（x）×（v _obs ²/D）×f（S _cur） 的項。x越接近1，則H（x）越大。L越長，則f（S _cur）越大。

根據該揭示，風險值可準確地表示物體與機械手發生干涉的可能性，並且可表示反映出對物體的危害的大小的值。

H（x）例如包含雙曲線正切函數。f（S _cur）例如包含S形函數（sigmoid function）。

所述的揭示中，沿著機械手的軌道的自開始位置直至機械手的當前位置為止的長度越長，則第二動作中的機械手的移動速度相對於標準速度的比例越小。

根據該揭示，機械手越接近終點位置，可使第二動作中的速度越慢。藉此，物體與機械手的干涉的發生得到抑制。

所述的揭示中，計算部以機械手的裝載量越大則變得越大的方式來算出風險值。

根據該揭示，當機械手的裝載量大時，機器人可更快地變更機械手的動作，以抑制機械手與物體的干涉。

所述的揭示中，計算部使用風險函數來算出風險值，所述風險函數包含含有物體與機械手的距離和物體的速度的項、以及常數項。常數項是根據與機器人協同動作而進行作業的人來設定。

根據該揭示，越是進行容易與機械手發生干涉的作業的人，可使風險值越大。藉此，物體與機械手的干涉的發生得到抑制。

根據本揭示的一例，控制方法控制機器人，以使機器人的機械手自開始位置移動至終點位置為止。控制方法包括下述步驟：算出機械手與物體的干涉的風險值；以及生成一指令，所述指令用於使機械手依據自包含多種動作的群組中根據風險值而選擇的對象動作來運作。進而，控制方法包括下述步驟：在機械手較基準位置接近開始位置時設定第一群組，在機械手較基準位置接近終點位置時設定第二群組，以作為群組。第一群組包含變更軌道以降低風險值的第一動作，來作為風險值處於第一範圍內時的動作。第二群組包含第一動作，來作為風險值處於第一範圍的一部分即第一子範圍內時的動作，且包含不變更軌道而使速度下降的第二動作，來作為風險值處於第一範圍中的與第一子範圍不同的第二子範圍內時的動作。

根據本揭示的一例，程式使電腦執行所述控制方法。藉由該些揭示，亦能減輕計算負載，並且抑制物體與機器人的干涉的發生。 [發明的效果]

根據本揭示，可減輕計算負載，並且抑制物體與機器人的干涉的發生。

一邊參照圖式，一邊詳細說明本發明的實施形態。再者，對於圖中的相同或相當的部分，標註相同的符號並不再重複其說明。以下說明的各變形例亦可適當選擇性地加以組合。

§1 適用例 對適用本實施形態的控制裝置的系統的概略進行說明。圖1是表示適用實施形態的控制裝置的系統的一例的圖。如圖1所示，系統1包括控制裝置100、機器人200以及多個感測設備300。

系統1被設於工廠等的生產現場。在生產現場，如圖1所示，人400與機器人200協同地進行作業。人400對於機器人200而言為移動障礙物，可能與機器人200之間產生干涉。系統1進行用於抑制人400與機器人200的干涉的處理。

機器人200包含機械手202與機械手控制器204。

機械手202具有多個臂211、212、213、多個關節221、222、223、轉動部230以及台座部240。具體而言，機械手202中，自前端側起依序設有臂211、關節221、臂212、關節222、臂213、關節223、轉動部230、台座部240。

在臂211的前端部安裝有末端執行器。臂211的後端部連接於關節221。在臂212的前端部安裝有關節221。臂212的後端部連接於關節222。在臂213的前端部安裝有關節222。臂213的後端部連接於關節223。

關節221使臂211相對於臂212而移動。關節222使臂212相對於臂213而移動。關節223使臂213相對於轉動部230而移動。

轉動部230支持關節223，並且以旋轉軸為中心可旋轉地構成。台座部240可轉動地支持轉動部230。

機械手控制器204控制機械手202的動作。具體而言，機械手控制器204自控制裝置100獲取與機械手202的前端部即臂211的前端部（以下稱作「工具中心點（Tool Center Point，TCP）」）的目標位置以及移動速度相關的指令。機械手控制器204依據所獲取的指令來控制關節221～關節223以及轉動部230的動作，以使工具中心點移動。

感測設備300是被配置於機器人200所處的空間內，對所述空間內的物體的位置進行偵測的設備。物體包含機械手202以及人400。感測設備300例如為紅綠藍-深度（Red Green Blue-Depth，RGB-D）攝影機或雷射測距儀（laser range finder），生成視野的點陣資料。較佳的是，例如將四個以上的感測設備300配置於互不相同的位置，以使得空間內無死角。

控制裝置100控制機器人200，以使機器人200的機械手202自開始位置移動至終點位置為止。

控制裝置100與多個感測設備300可通訊地連接。控制裝置100例如使用GigE Vision（註冊商標）或通用串列匯流排（Universal Serial Bus，USB）來與多個感測設備300進行通訊。

控制裝置100與機械手控制器204可通訊地連接。控制裝置100例如使用Ethernet/IP（註冊商標）來與機械手控制器204進行通訊。控制裝置100藉由與機械手控制器204的通訊交換，來識別機械手202的當前位置。

如圖1所示，控制裝置100包括計算部10、生成部12以及設定部14。

計算部10基於多個感測設備300的偵測結果來算出機械手202與機械手202周圍的物體（包含人400）的干涉的風險值。

生成部12生成一指令，所述指令用於使機器人200依據自包含多種動作的群組中根據風險值而選擇的對象動作來運作。具體而言，生成部12依據對象動作來決定自當前位置直至終點位置為止的工具中心點的軌道。生成部12基於所決定的軌道來確定每個控制週期的工具中心點的目標位置以及移動速度。控制裝置100在每個控制週期生成與所確定的目標位置以及移動速度相關的指令，並將所生成的指令輸出至機械手控制器204。

設定部14根據機械手202的位置來設定所述的包含多種動作的群組。

圖2是說明設定部的處理的圖。如圖2所示，設定部14在機械手202較基準位置接近開始位置PS時設定第一群組，在機械手202較基準位置接近終點位置PE時設定第二群組。

第一群組包含變更機械手202的軌道以降低風險值的迂迴動作，來作為風險值處於第一範圍內時的動作。

另一方面，第二群組包含所述迂迴動作，以作為風險值處於第一範圍的一部分即第一子範圍內時的動作。進而，第二群組包含不變更軌道而使機械手202的速度下降的警戒動作，來作為風險值處於第一範圍中的與第一子範圍不同的第二子範圍內時的動作。

藉此，當機械手202接近終點位置PE時，變更軌道的頻率下降，控制裝置100的計算負載得以減輕。

而且，當機械手202較基準位置接近開始位置PS，且風險值處於第一範圍內時，變更機械手202的軌道以降低風險值。進而，當機械手202較基準位置接近終點位置PE，且風險值處於第一子範圍內時，亦變更機械手202的軌道以降低風險值。藉此，可抑制人400與機械手202的干涉的發生。

如此，根據本實施形態的控制裝置100，可減輕計算負載，並且抑制人400與機器人200的干涉的發生。

§2 具體例 ＜控制裝置的硬體結構＞ 圖3是表示控制裝置的硬體結構的示意圖。典型的是，控制裝置100具有遵循通用的電腦架構的結構。

如圖3所示，控制裝置100包含控制處理電路110以及現場網路控制器120。

控制處理電路110執行驅動機器人200所需的運算處理。作為一例，控制處理電路110包含處理器112、主記憶體114、儲存器116以及介面電路118。

處理器112執行用於驅動機器人200的控制運算。主記憶體114例如包含動態隨機存取記憶體（Dynamic Random Access Memory，DRAM）或靜態隨機存取記憶體（Static Random Access Memory，SRAM）等的揮發性記憶裝置等。儲存器116例如包含固態硬碟（Solid State Drive，SSD）或硬碟驅動器（Hard Disk Drive，HDD）等的非揮發性記憶裝置等。

在儲存器116中，保存有用於實現機器人200的控制的系統程式130。系統程式130包含執行與機器人200的動作相關的控制運算的命令、以及與跟機器人200之間的介面相關的命令。圖1所示的計算部10、生成部12以及設定部14是藉由處理器112執行系統程式130來實現。

示教資料132是與預先規定的機械手202的工具中心點的軌道相關的資料。

介面電路118與機器人200之間交換資料。 現場網路控制器120經由現場網路，主要與感測設備300之間交換資料。

＜計算部的處理內容＞ 計算部10以固定週期來算出機械手202與物體的干涉的風險值。以下，對計算部10的具體的處理內容進行說明。

計算部10在各週期中，自多個感測設備300獲取點陣資料。自各感測設備300獲取的點陣資料是以所述感測設備300的座標系（以下稱作「攝影機座標系」）來表示。因此，計算部10將自各感測設備300獲取的點陣資料由攝影機座標系轉換為機器人200的座標系（以下稱作「機器人座標系」）。用於由攝影機座標系轉換為機器人座標系的轉換矩陣是藉由對每個感測設備300預先實施的校準而製作。

若使用所獲取的所有點陣資料，則計算負載將變大。因此，計算部10根據點陣資料來生成八叉樹地圖（OctoMap）。

圖4是表示八叉樹地圖的一例的圖。如圖4所示，八叉樹地圖表示藉由劃分空間而獲得的、三次元立方體形狀的多個體素（voxel）500各自中的物體的佔有率。物體不僅包含機械手202以及人400，亦可能包含例如被無意地放置於機械手202周圍的障礙物。計算部10將體素500導入至點陣資料，算出每個體素500的佔有率。佔有率例如是以0.00～100的值來表示。作為由點陣資料生成八叉樹地圖的方法，可使用公知的技術（例如非專利文獻1所記載的技術）。如此，計算部10生成各週期的八叉樹地圖。

再者，控制裝置100藉由與機械手控制器204的相互通訊來識別機械手202的當前位置。因此，計算部10亦可自八叉樹地圖中刪除機械手202所處的體素。藉此，使用八叉樹地圖的後述計算負載得以減輕。

計算部10依據下述的式（1），來算出各週期中的機械手202與物體（主要是人400）的干涉的風險值I _risk。

[數1]

式（1）中，H _dict為機械手202以及人400的速度向量的方向函數。v _obs為人400的速度向量。D _eucl為機械手202的工具中心點與人400的距離。Sigmoid（S _cur，S _thrd）為S形函數。S _cur為沿著軌道的、自開始位置直至工具中心點的當前位置為止的長度。S _thrd為預先規定的臨限值，例如為沿著軌道的自開始位置直至終點位置為止的長度的0.6倍～0.7倍的長度。P _scoli為與靜態障礙物相關的迂迴動作項。P _bias為正常性偏差項，是預先規定的負的常數項。

計算部10使用各週期的八叉樹地圖來確定人400的位置。人400所處的體素的佔有率取高的值。因此，計算部10基於八叉樹地圖中佔有率高的體素的位置來確定人400的位置。

再者，計算部10亦可對八叉樹地圖進行二值化處理，以使各體素的佔有率取0、1中的任一個，並使用經二值化的八叉樹地圖來確定人400的位置。藉此，計算負載得到進一步減輕。

計算部10基於機械手202的工具中心點的當前位置與根據在當前的週期內生成的八叉樹地圖所確定的人400的位置，來算出距離D _eucl。

計算部10使用在過去一定期間內生成的八叉樹地圖，來算出人400的速度向量v _obs。

計算部10基於機械手202的工具中心點的當前位置與工具中心點的軌道，來算出長度S _cur。如後所述，工具中心點的軌道是由生成部12所生成。

S形函數Sigmoid（S _cur，S _thrd）是以下述的式（2）來表示。式（2）中，a為常數。

[數2]

圖5是表示S形函數的一例的圖表。如圖5所示，S形函數Sigmoid（S _cur，S _thrd）隨著S _cur自小於S _thrd的值接近S _thrd而變大，當S _cur超過S _thrd時接近1。S _thrd附近的斜度依存於常數a。常數a的值越大，則S _thrd附近的斜度變得越陡峭。

計算部10算出將位於自當前位置直至目標位置為止的工具中心點的軌道上的一個以上的體素的佔有率的合計值乘以規定係數所得的值來作為迂迴動作項P _scoli。若軌道上不存在物體，則迂迴動作項P _scoli為0。如後所述，軌道是由生成部12所生成。因此，計算部10使用由生成部12所生成的軌道來算出迂迴動作項P _scoli。再者，動作開始時的軌道是由記憶於儲存器116中的示教資料132來表示。

速度向量的方向函數H _dict是以下述的式（3）來表示。如式（3）所示，方向函數H _dict為雙曲線正切函數。式（3）中，K、b為常數，例如被分別設定為3/2、10。式（3）的變量x是依據式（4）來算出。

[數3]

[數4]

v _tcp為機械手202的工具中心點的速度向量。計算部10基於向機械手控制器204輸出的最新的指令來算出速度向量v _tcp。

v是以人400的位置為起點，以工具中心點的位置為終點的向量。計算部10基於工具中心點的當前位置與根據當前週期的八叉樹地圖所確定的人400的位置，來算出向量v。

圖6是表示速度向量v _obs、速度向量v _tcp以及向量v的關係的圖。如圖6所示，依據所述式（4）而算出的x為向量（v _obs-v _tcp）與向量v所成的角度θ的餘弦（cosθ）。因此，x取-1至1的值。

角度θ換言之為人400相對於工具中心點TCP的相對移動方向與連結人400和工具中心點TCP的線所成的角度。因此，x越接近1，則表示人400正朝向工具中心點TCP移動的可能性越高。

圖7是表示速度向量的方向函數H _dict的一例的圖。圖7中表示K=3/2、b=10、x _thrd=0.999時的方向函數H _dict。如圖7所示，方向函數H _dict自x=0.76附近陡峭地上升。圖6所示的角度θ=40°時為x=0.766。即，在人400相對於工具中心點TCP的相對移動方向與連結人400和工具中心點TCP的線所成的角度θ處於-40°～40°的範圍內，方向函數H _dict取有意義的值，當角度θ處於所述範圍外時，方向函數H _dict約為0。

餘弦為0.999的角度是±2.56°。因此，在角度θ為-2.56°～2.56°的範圍內，方向函數H _dict為K以上的值。

計算部10將如此般算出的變量代入式（1），藉此來算出各週期中的機械手202與物體的干涉的風險值I _risk。

式（1）中，方向函數H _dict是作為第一項的係數而存在。因此，當人400相對於工具中心點的相對移動方向與連結人400和工具中心點的線所成的角度θ處於-40°～40°的範圍外時，第一項約為0。當角度θ處於-40°～40°的範圍內時，第一項取正的有意義的值。此時，角度θ越小（越接近0°），則風險值I _risk變得越大。而且，第一項中，工具中心點與人400的距離D _eucl位於分母。因此，距離D _eucl越小，則風險值I _risk變得越大。進而，第一項中，人400的速度向量v _obs的平方位於分子。因此，人400的速度越大，則風險值I _risk變得越大。

當角度θ處於-40°～40°的範圍外時，人400與機械手202的干涉的可能性低。另一方面，當角度θ處於-40°～40°的範圍外時，存在人400與機械手202發生干涉的可能性，角度θ越小，則該可能性越高。而且，工具中心點與人400越接近，則機械手202與人400的干涉的可能性越高。進而，人400的速度越大，則機械手202與人400的干涉的可能性越高。因此，風險值I _risk準確地表示了人400與機械手202發生干涉的可能性。

第一項包含S形函數Sigmoid（S _cur，S _thrd）。如圖5所示，隨著S _cur變大，即，隨著機械手202的工具中心點越接近終點位置，則S形函數Sigmoid（S _cur，S _thrd）變得越大。因此，式（1）被設定為，隨著機械手202的工具中心點接近終點位置，而風險值I _risk變高。這是因為，與機器人200的動作剛剛開始之後相比，機械手202的工具中心點接近終點位置時，因機械手202與人400的干涉造成的對人400的危害更大。

在機械手202的周圍空間，有可能在機械手202的軌道上存在意料外的障礙物。此時應避免機械手202與障礙物的碰撞。因此，式（1）中包含迂迴動作項P _scoli。

＜設定部的處理內容＞ 設定部14根據機械手202的位置來設定第一群組以及第二群組中的任一個，以作為包含機器人200可採用的多種動作的群組。以下，對設定部14的處理內容進行說明。

設定部14依據下述的式（5）來決定警戒行動激活旗標W _act。設定部14在警戒行動激活旗標W _act為0時設定第一群組，在警戒行動激活旗標W _act為1時設定第二群組。

[數5]

式（5）中，臨限值Th是預先規定的。沿著軌道而自開始位置離開S形函數Sigmoid（S _cur，S _thrd）從臨限值Th以下超過臨限值Th時的長度S _cur的位置對應於基準位置。因此，根據基準位置來設定臨限值Th。臨限值Th例如被設定為接近0的值（0.05～0.1等）。再者，根據有效數字的位數，在工具中心點自開始位置出發起不久的期間，S形函數Sigmoid（S _cur，S _thrd）的值有可能連續為0。因此，亦可設定0來作為臨限值Th。

＜生成部的處理內容＞ 生成部12生成一指令，所述指令用於使機器人200依據自所設定的群組中根據風險值I _risk而選擇的對象動作來運作。以下，對生成部12的處理內容進行說明。

圖8是表示屬於群組的動作的種類的一例的圖。如圖8所示、第一群組包含均衡動作、迂迴動作與反射動作。第二群組包含均衡動作、迂迴動作、警戒動作與反射動作。

在設定有第一群組的情況下，生成部12如以下般選擇對象動作。即，生成部12在風險值I _risk處於範圍Ra內時，選擇均衡動作來作為對象動作。生成部12在風險值I _risk處於較範圍Ra高的範圍Rb內時，選擇迂迴動作。生成部12在風險值I _risk處於較範圍Rb高的範圍Rc內時，選擇反射動作。

在設定有第二群組的情況下，生成部12如以下般選擇對象動作。即，生成部12在風險值I _risk處於範圍Ra內時，選擇均衡動作來作為對象動作。生成部12在風險值I _risk處於範圍Rb的一部分即子範圍Rb-1內時，選擇迂迴動作。生成部12在風險值I _risk處於範圍Rb中的與子範圍Rb-1不同的子範圍Rb-2內時，選擇警戒動作。子範圍Rb-2為較子範圍Rb-1高的範圍。生成部12在風險值I _risk處於範圍Rc內時，選擇反射動作。

（均衡動作） 均衡動作是在機械手202與物體的干涉的可能性小時所選擇的動作。因此，範圍Ra例如被設定為風險值I _risk可取的最小值即P _bias（負的值）至0。即，範圍Ra的上限值為0。此時，當風險值I _risk為0以下時，可容易地認識到機械手202與物體的干涉的可能性小。

均衡動作是不變更軌道而維持標準速度的動作。標準速度是以軌道上的最高速度達到預先設定的速度的方式而關於軌道上的各點所算出的、工具中心點的速度。生成部12在動作開始時，選擇由示教資料132所示的軌道。生成部12只要不選擇迂迴動作來作為對象動作，便維持由示教資料132所示的軌道。生成部12在選擇了迂迴動作來作為對象動作後選擇均衡動作來作為對象動作的情況下，維持藉由迂迴動作而變更的軌道。

生成部12基於軌道來確定每個控制週期的工具中心點的目標位置以及標準速度。控制裝置100在每個控制週期生成與所確定的目標位置以及標準速度相關的指令，並將所生成的指令輸出至機械手控制器204。

（迂迴動作） 迂迴動作是變更軌道以降低風險值I _risk的動作。生成部12基於變更後的軌道來確定下個控制週期的工具中心點的目標位置以及移動速度。控制裝置100生成與所確定的目標位置以及移動速度相關的指令，並將所生成的指令輸出至機械手控制器204。

生成部12只要使用公知的技術（例如非專利文獻1所揭示的技術）來決定變更後的軌道即可。具體而言，生成部12使用快速搜尋隨機樹（Rapidly-exploring Random Trees，RRT）-連接（Connect）的方法，來探索自當前位置直至終點位置為止的多個軌道候選。所述探索是在預先規定的限制時間內實施。生成部12對於所探索的多個軌道候選的各個，使用下述的式（6）來算出成本C _traj。

[數6]

D _traj為體素的歐幾裡德距離（Euclidean distance）。式（6）的第一項為軌道候選所通過的體素的歐幾裡德距離的合計。C _voxel表示軌道候選所通過的各體素的佔有率。式（6）的第二項表示在軌道候選上存在物體的概率。

生成部12將成本C _traj最小的軌道候選決定為變更後的軌道。藉此，變更為機械手202與物體的干涉的可能性小的軌道。

（警戒動作） 警戒動作是不變更軌道而使速度下降的動作。具體而言，生成部12利用與均衡動作相同的方法，來確定每個控制週期的工具中心點的目標位置以及標準速度。接下來，生成部12將所確定的標準速度的1/2決定為移動速度。生成部12生成與目標位置以及所決定的移動速度相關的指令，並將所生成的指令輸出至機械手控制器204。

或者，生成部12亦可依據下述的式（7）來決定移動速度V _curr。式（7）中，V _std為在均衡動作中所確定的標準速度。

[數7]

藉由依據式（7）來決定移動速度V _curr，從而隨著機械手202接近終點位置，警戒行動時的移動速度V _curr變慢。換言之，沿著機械手202的軌道的自開始位置直至機械手202的當前位置為止的長度S _cur越長，則警戒動作中的機械手202的移動速度V _curr相對於標準速度V _std的比例越小。

（反射動作） 反射動作是沿著軌道來使機械手202朝向開始位置返回的動作。具體而言，生成部12生成返回至一個以上的控制週期前的目標位置為止的指令，並將所生成的指令輸出至機械手控制器204。

＜控制裝置的處理流程＞ 圖9是表示控制裝置的處理流程的一例的圖。圖9所示的步驟S1～步驟S6是在自機器人的動作開始時直至動作結束時為止的期間內每隔固定週期而反覆實施。

首先，控制裝置100的處理器112自多個感測設備300分別獲取點陣資料（步驟S1）。

接下來，處理器112使用所獲取的點陣資料，生成機械手202的周圍空間的八叉樹地圖（步驟S2）。

接下來，處理器112基於機械手202的當前位置來設定包含多種動作的群組（步驟S3）。具體而言，處理器112算出警戒行動激活旗標W _act，在警戒行動激活旗標W _act為0時設定第一群組，在警戒行動激活旗標W _act為1時設定第二群組。

接下來，處理器112使用八叉樹地圖來算出機械手202與人400的干涉的風險值I _risk（步驟S4）。

接下來，處理器112自所設定的群組中選擇與風險值I _risk相應的對象動作（步驟S5）。

接下來，處理器112依據所選擇的對象動作來生成指令，並將所生成的指令輸出至機械手控制器204（步驟S6）。

在動作開始時，若在軌道上放置有意料外的障礙物，則式（1）中的迂迴動作項P _scoli取大的值。在動作剛剛開始之後，設定第一群組，因此風險值I _risk處於範圍Rb內，實施迂迴動作。其結果，變更為繞過障礙物的軌道。

在動作開始時，若在軌道上不存在障礙物，則依據由示教資料132所示的軌道來實施均衡動作。若在機械手202的周圍不存在人400，則每隔固定週期算出的風險值I _risk小於0，被維持在範圍Ra內。因此，機械手202沿著由示教資料132所示的軌道自開始位置移動至終點位置為止。

動作開始後，在直至機械手202到達基準位置為止的期間，設定第一群組。在該期間內，當人400接近機器人200時，風險值I _risk超過0，處於範圍Rb內。藉此，變更為繞過人400的軌道。其結果，機械手202與人400的干涉得以抑制。軌道變更後，當風險值I _risk處於範圍Ra內時，實施均衡動作，機械手202沿著變更後的軌道而以標準速度來運作。

另一方面，在自機械手202通過基準位置直至到達終點位置為止的期間，設定第二群組。在該期間內，當人400接近機器人200時，風險值I _risk超過0，處於範圍Rb內。當風險值I _risk處於範圍Rb中的下側的範圍Rb-1內時，有可能藉由輕微的軌道變更便可避免人400的干涉。因此，當風險值I _risk處於範圍Rb-1內時，實施迂迴動作。另一方面，若在風險值I _risk處於範圍Rb中的上側的範圍Rb-2內的情況下選擇迂迴動作，則為了避免人400的干涉，有可能要大幅變更軌道。機械手202接近終點位置的時機的軌道的大幅變更不佳。因此，當風險值I _risk處於範圍Rb-2內時，實施警戒動作。即，不變更軌道而以低於標準速度的速度來使機械手202運作。藉此，人400注意到機械手202的存在，而可取得遠離機械手202的時間。其結果，機械手202與人400的干涉得以抑制。

當風險值I _risk處於較範圍Rb高的範圍Rc內時，為機械手202與人400即將發生干涉的狀態。因此，當風險值I _risk處於範圍Rc內時，實施反射動作。藉此，機械手202朝遠離人400的方向移動。其結果，機械手202與人400的干涉得以抑制。

＜變形例＞ 機械手202可在裝載有各種物體的狀態下移動。機械手202的裝載量越大，則機械手202與人400發生干涉時的對人400的影響越大。因此，計算部10亦可以機械手202的裝載量越大則變得越大的方式來算出風險值I _risk。例如，計算部10依據下述的式（8）來算出各週期中的、機械手202與物體（主要是人400）的干涉的風險值I _risk。

[數8]

與式（1）同樣，H _dict是機械手202以及人400的速度向量的方向函數。但是，H _dict既可利用所述的式（3）來表示，亦可利用下述的式（9）來表示。

[數9]

式（9）中，a、b為預先規定的係數。例如設定為a=10、b=0。x是藉由所述的式（4）而算出。

圖10是表示速度向量v _obs、速度向量v _tcp以及向量v與方向函數H _dict的值的關係的圖。如圖10所示，當速度向量v _obs、速度向量v _tcp彼此相對時，即，當向量（v _obs-v _tcp）與向量v所成的角度θ為0時，H _dict為1。當角度θ大於0且小於90°時，H _dict成為大於0.5的值。當角度θ大於90°時，H _dict為小於0.5的值。當角度θ接近180°時，H _dict接近0。

式（8）中，m _payload是機械手202的裝載量越大則取越大的值的項。m _payload例如以下述的式（10）來表示。

[數10]

式（10）中，α為預先規定的係數，例如為1.0。m _{payload_min}為最小的風險因子值，是預先規定的。例如，m _{payload_min}為1.0。P _max為機械手202的最大裝載量，P _min為機械手202的最小裝載量。P _max以及P _min是根據機械手202的規格而預先規定。例如，P _max為10 kg，P _min為1 kg。P _curr為當前的機械手202的裝載量。機器人200包括對當前的機械手202的裝載量P _curr進行測量的感測器。計算部10自機器人200的所述感測器獲取裝載量P _curr，將所獲取的裝載量P _curr代入所述式（10），藉此來算出m _payload。

R _bias是對風險值I _risk進行調整的常數項。R _bias例如亦可為式（1）的P _scoli與Pbias之和。或者，R _bias亦可根據過去的運用環境的風險來預先規定。例如，在風險高的生產現場，設定正的數值來作為R _bias。在風險低的生產現場，為了降低對機器人200的行動進行變更的頻率，設定負的數值來作為R _bias。

可能存在進行容易與機械手202發生干涉的作業的人與並非如此的人。因此，R _bias的值亦可根據與機器人200協同動作而進行作業的人400來設定。例如，設定0來作為R _bias的默認值。隨後，針對每個人400來記錄風險值I _risk的推移。根據所記錄的風險值I _risk的代表值（例如中央值、平均值），來設定每個人400的R _bias。藉此，越是進行容易與機械手202發生干涉的作業的人400，可使風險值越大。其結果，人400與機械手202的干涉的發生得到抑制。再者，式（1）中的P _scoli以及Pbias亦可亦根據與機器人200協同動作而進行作業的人400來設定。

藉由使用式（8），算出與機械手202的裝載量相應的風險值I _risk。其結果，當機械手202的裝載量大時，機器人200可更快地變更機械手202的動作，以抑制機械手202與人400的干涉。

§3 附註 如上所述，本實施形態包含如下所述的揭示。

（結構1） 一種控制裝置100，控制機器人200，以使所述機器人200的機械手202自開始位置移動至終點位置為止，所述控制裝置100包括： 計算部10，算出所述機械手202與物體400的干涉的風險值； 生成部12，生成一指令，所述指令用於使所述機械手202依據自包含多種動作的群組中根據所述風險值而選擇的對象動作來運作；以及 設定部14，在所述機械手202較基準位置接近所述開始位置時設定第一群組，在所述機械手202較所述基準位置接近所述終點位置時設定第二群組，以作為所述群組， 所述第一群組包含變更軌道以降低所述風險值的第一動作，來作為所述風險值處於第一範圍內時的動作， 所述第二群組包含所述第一動作，來作為所述風險值處於所述第一範圍的一部分即第一子範圍內時的動作，且 所述第二群組包含不變更軌道而使速度下降的第二動作，來作為所述風險值處於所述第一範圍中的與所述第一子範圍不同的第二子範圍內時的動作。

（結構2） 如結構1所述的控制裝置100，其中 所述第一群組以及所述第二群組各自包含不變更所述機械手的軌道而以標準速度來使所述機械手202運作的第三動作，來作為所述風險值處於較所述第一範圍低的第二範圍內時的動作， 所述第二動作是以低於所述標準速度的速度來使所述機械手202運作。

（結構3） 如結構1或結構2所述的控制裝置100，其中 所述第一群組以及所述第二群組各自包含使所述機械手202朝向所述開始位置進行返回的第四動作，來作為所述風險值處於較所述第一範圍高的第三範圍內時的動作。

（結構4） 如結構1至結構3中任一項所述的控制裝置100，其中 所述計算部10以所述物體400相對於所述機械手202的相對移動方向、與連結所述物體400和所述機械手202的線所成的角度越接近零度則變得越大的方式，來算出所述風險值。

（結構5） 如結構1至結構4中任一項所述的控制裝置100，其中 所述計算部10以所述物體400的速度越快則變得越大的方式來算出所述風險值，並且以所述物體400與所述機械手202的距離越短則變得越大的方式來算出所述風險值。

（結構6） 如結構1至結構5中任一項所述的控制裝置100，其中 所述計算部10以沿著所述機械手202的軌道的自所述開始位置直至所述機械手202的當前位置為止的長度越長則變得越大的方式來算出所述風險值。

（結構7） 如結構2所述的控制裝置100，其中 所述計算部10使用風險函數來算出所述風險值，所述風險函數包含含有所述物體400與所述機械手202的距離和所述物體400的速度的項、以及常數項， 所述常數項的值為負， 所述第二範圍的上限值為0。

（結構8） 如結構1至結構3中任一項所述的控制裝置100，其中 所述計算部10使用風險函數來算出所述風險值， 當將所述物體400相對於所述機械手202的相對移動方向與連結所述物體400和所述機械手202的線所成的角度的餘弦設為x，將所述物體400的速度設為v _obs，將所述機械手202與所述物體400的距離設為D，將沿著所述機械手202的軌道的自所述開始位置直至所述機械手202的當前位置為止的長度設為S _cur時，所述風險函數包含 H（x）×（v _obs ²/D）×f（S _cur） 的項， x越接近1，則H（x）越大， L越長，則f（S _cur）越大。

（結構9） 如結構8所述的控制裝置100，其中 H（x）包含雙曲線正切函數。

（結構10） 如結構8或結構9所述的控制裝置100，其中 f（S _cur）包含S形函數。

（結構11） 如結構2所述的控制裝置100，其中 沿著所述機械手202的軌道的自所述開始位置直至所述機械手202的當前位置為止的長度越長，則所述第二動作中的所述機械手202的移動速度相對於所述標準速度的比例越小。

（結構12） 如結構1至結構3中任一項所述的控制裝置100，其中 所述計算部10以所述機械手202的裝載量越大則變得越大的方式來算出所述風險值。

（結構13） 如結構1至結構3中任一項所述的控制裝置100，其中 所述計算部10使用風險函數來算出所述風險值，所述風險函數包含含有所述物體400與所述機械手202的距離和所述物體400的速度的項、以及常數項， 所述常數項是根據與所述機器人協同動作而進行作業的人來設定。

（結構14） 一種控制方法，控制機器人200，以使所述機器人200的機械手202自開始位置移動至終點位置為止，所述控制方法包括下述步驟： 算出所述機械手202與物體400的干涉的風險值； 生成一指令，所述指令用於使所述機械手202依據自包含多種動作的群組中根據所述風險值而選擇的對象動作來運作；以及 在所述機械手202較基準位置接近所述開始位置時設定第一群組，在所述機械手202較所述基準位置接近所述終點位置時設定第二群組，以作為所述群組， 所述第一群組包含變更軌道以降低所述風險值的第一動作，來作為所述風險值處於第一範圍內時的動作， 所述第二群組包含所述第一動作，來作為所述風險值處於所述第一範圍的一部分即第一子範圍內時的動作，且 所述第二群組包含不變更軌道而使速度下降的第二動作，來作為所述風險值處於所述第一範圍中的與所述第一子範圍不同的第二子範圍內時的動作。

（結構15） 一種程式130，使電腦100執行如結構14所述的控制方法。

對本發明的實施形態進行了說明，但應認為，此次揭示的實施形態在所有方面為例示而非限制者。本發明的範圍是藉由申請專利範圍來表示，且意圖包含與申請專利範圍均等的含義以及範圍內的所有變更。

1:系統 10:計算部 12:生成部 14:設定部 100:控制裝置 110:控制處理電路 112:處理器 114:主記憶體 116:儲存器 118:介面電路 120:現場網路控制器 130:系統程式 132:示教資料 200:機器人 202:機械手 204:機械手控制器 211～213:臂 221～223:關節 230:轉動部 240:台座部 300:感測設備 400:人 500:體素 I _risk:風險值 P _bias:正常性偏差項 PE:終點位置 PS:開始位置 Ra～Rc:範圍 Rb-1、Rb-2:子範圍 S1～S6:步驟 TCP:工具中心點 V _obs:速度向量 V _tcp:速度向量 θ:角度

圖1是表示適用實施形態的控制裝置的系統的一例的圖。 圖2是說明設定部的處理的圖。 圖3是表示控制裝置的硬體結構的示意圖。 圖4是表示八叉樹地圖（OctoMap）的一例的圖。 圖5是表示S形函數的一例的圖表。 圖6是表示速度向量v _obs、速度向量v _tcp以及向量v的關係的圖。 圖7是表示速度向量的方向函數H _dict的一例的圖。 圖8是表示屬於群組的動作的種類的一例的圖。 圖9是表示控制裝置的處理流程的一例的圖。 圖10是表示速度向量v _obs、速度向量v _tcp以及向量v與方向函數H _dict的值的關係的圖。

1:系統

10:計算部

12:生成部

14:設定部

100:控制裝置

200:機器人

202:機械手

204:機械手控制器

211~213:臂

221~223:關節

230:轉動部

240:台座部

300:感測設備

400:人

Claims (15)

1. 一種控制裝置，控制機器人，以使所述機器人的機械手自開始位置移動至終點位置為止，所述控制裝置包括： 計算部，算出所述機械手與物體的干涉的風險值； 生成部，生成一指令，所述指令用於使所述機械手依據自包含多種動作的群組中根據所述風險值而選擇的對象動作來運作；以及 設定部，在所述機械手較基準位置接近所述開始位置時設定第一群組，在所述機械手較所述基準位置接近所述終點位置時設定第二群組，以作為所述群組， 所述第一群組包含變更軌道以降低所述風險值的第一動作，來作為所述風險值處於第一範圍內時的動作， 所述第二群組包含所述第一動作，來作為所述風險值處於所述第一範圍的一部分即第一子範圍內時的動作，且 所述第二群組包含不變更軌道而使速度下降的第二動作，來作為所述風險值處於所述第一範圍中的與所述第一子範圍不同的第二子範圍內時的動作。
2. 如請求項1所述的控制裝置，其中 所述第一群組以及所述第二群組各自包含不變更所述機械手的軌道而以標準速度來使所述機械手運作的第三動作，來作為所述風險值處於較所述第一範圍低的第二範圍內時的動作， 所述第二動作是以低於所述標準速度的速度來使所述機械手運作。
3. 如請求項1所述的控制裝置，其中 所述第一群組以及所述第二群組各自包含使所述機械手朝向所述開始位置進行返回的第四動作，來作為所述風險值處於較所述第一範圍高的第三範圍內時的動作。
4. 如請求項1至請求項3中任一項所述的控制裝置，其中 所述計算部以所述物體相對於所述機械手的相對移動方向與連結所述物體和所述機械手的線所成的角度越接近零度則變得越大的方式，來算出所述風險值。
5. 如請求項1至請求項3中任一項所述的控制裝置，其中 所述計算部以所述物體的速度越快則變得越大的方式來算出所述風險值，並且以所述物體與所述機械手的距離越短則變得越大的方式來算出所述風險值。
6. 如請求項1至請求項3中任一項所述的控制裝置，其中 所述計算部以沿著所述機械手的軌道的自所述開始位置直至所述機械手的當前位置為止的長度越長則變得越大的方式來算出所述風險值。
7. 如請求項2所述的控制裝置，其中 所述計算部使用風險函數來算出所述風險值，所述風險函數包含含有所述物體與所述機械手的距離和所述物體的速度的項、以及常數項， 所述常數項的值為負， 所述第二範圍的上限值為0。
8. 如請求項1至請求項3中任一項所述的控制裝置，其中 所述計算部使用風險函數來算出所述風險值， 當將所述物體相對於所述機械手的相對移動方向與連結所述物體和所述機械手的線所成的角度的餘弦設為x，將所述物體的速度設為v _obs，將所述機械手與所述物體的距離設為D，將沿著所述機械手的軌道的自所述開始位置直至所述機械手的當前位置為止的長度設為S _cur時，所述風險函數包含 H（x）×（v _obs ²/D）×f（S _cur） 的項， x越接近1，則H（x）越大， L越長，則f（S _cur）越大。
9. 如請求項8所述的控制裝置，其中 H（x）包含雙曲線正切函數。
10. 如請求項8所述的控制裝置，其中 f（S _cur）包含S形函數。
11. 如請求項2所述的控制裝置，其中 沿著所述機械手的軌道的自所述開始位置直至所述機械手的當前位置為止的長度越長，則所述第二動作中的所述機械手的移動速度相對於所述標準速度的比例越小。
12. 如請求項1至請求項3中任一項所述的控制裝置，其中 所述計算部以所述機械手的裝載量越大則變得越大的方式來算出所述風險值。
13. 如請求項1至請求項3中任一項所述的控制裝置，其中 所述計算部使用風險函數來算出所述風險值，所述風險函數包含含有所述物體與所述機械手的距離和所述物體的速度的項、以及常數項， 所述常數項是根據與所述機器人協同動作而進行作業的人來設定。
14. 一種控制方法，控制機器人，以使所述機器人的機械手自開始位置移動至終點位置為止，所述控制方法包括下述步驟： 算出所述機械手與物體的干涉的風險值； 生成一指令，所述指令用於使所述機械手依據自包含多種動作的群組中根據所述風險值而選擇的對象動作來運作；以及 在所述機械手較基準位置接近所述開始位置時設定第一群組，在所述機械手較所述基準位置接近所述終點位置時設定第二群組，以作為所述群組， 所述第一群組包含變更軌道以降低所述風險值的第一動作，來作為所述風險值處於第一範圍內時的動作， 所述第二群組包含所述第一動作，來作為所述風險值處於所述第一範圍的一部分即第一子範圍內時的動作，且 所述第二群組包含不變更軌道而使速度下降的第二動作，來作為所述風險值處於所述第一範圍中的與所述第一子範圍不同的第二子範圍內時的動作。
15. 一種程式，使電腦執行如請求項14所述的控制方法。