特許 7582492 制御装置、制御方法および制御プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-11-05

(45)【発行日】2024-11-13

(54)【発明の名称】制御装置、制御方法および制御プログラム

(51)【国際特許分類】

   B25J 19/06 20060101AFI20241106BHJP

【ＦＩ】

B25J19/06

【請求項の数】 15

(21)【出願番号】P 2023543685

(86)(22)【出願日】2022-04-27

(86)【国際出願番号】 JP2022019089

(87)【国際公開番号】W WO2023026589

(87)【国際公開日】2023-03-02

【審査請求日】2024-01-23

(31)【優先権主張番号】P 2021138585

(32)【優先日】2021-08-27

(33)【優先権主張国・地域又は機関】JP

(73)【特許権者】

【識別番号】000002945

【氏名又は名称】オムロン株式会社

(74)【代理人】

【識別番号】110001195

【氏名又は名称】弁理士法人深見特許事務所

(72)【発明者】

【氏名】大城 篤志

【審査官】杉田 隼一

(56)【参考文献】

【文献】特開２０１９－２０６０８０（ＪＰ，Ａ）

【文献】特開２０１９－１２３０２２（ＪＰ，Ａ）

【文献】国際公開第２０１９／０９７６７６（ＷＯ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ２５Ｊ １９／０６

(57)【特許請求の範囲】

【請求項1】

ロボットのマニピュレータがスタート位置からゴール位置まで移動するように前記ロボットを制御する制御装置であって、
前記マニピュレータと物体との干渉のリスク値を算出する算出部と、
複数種類の動作を含むグループの中から前記リスク値に応じて選択された対象動作に従って前記マニピュレータを動作させるための指令を生成する生成部と、
前記グループとして、前記マニピュレータが基準位置よりも前記スタート位置に近いときに第１グループを設定し、前記マニピュレータが前記基準位置よりも前記ゴール位置に近いときに第２グループを設定する設定部と、を備え、
前記第１グループは、
前記リスク値が第１範囲内であるときの動作として、前記リスク値を下げるように軌道を変更する第１動作を含み、
前記第２グループは、
前記リスク値が前記第１範囲の一部である第１サブ範囲内であるときの動作として、前記第１動作を含み、
前記リスク値が前記第１範囲のうち前記第１サブ範囲とは異なる第２サブ範囲内であるときの動作として、軌道を変更せずに速度を低下させる第２動作を含む、制御装置。

【請求項2】

前記第１グループおよび前記第２グループの各々は、前記リスク値が前記第１範囲よりも下の第２範囲内であるときの動作として、前記マニピュレータの軌道を変更せずに、標準速度で前記マニピュレータを動作させる第３動作を含み、
前記第２動作は、前記標準速度より低い速度で前記マニピュレータを動作させる、請求項１に記載の制御装置。

【請求項3】

前記第１グループおよび前記第２グループの各々は、前記リスク値が前記第１範囲より上の第３範囲内であるときの動作として、前記マニピュレータを前記スタート位置に向かって戻す第４動作を含む、請求項１に記載の制御装置。

【請求項4】

前記算出部は、前記マニピュレータに対する前記物体の相対移動方向と、前記物体と前記マニピュレータとを結ぶ線とのなす角度が０°に近いほど大きくなるように前記リスク値を算出する、請求項１から３のいずれか１項に記載の制御装置。

【請求項5】

前記算出部は、前記物体の速度が大きいほど大きくなるように前記リスク値を算出するとともに、前記物体と前記マニピュレータとの距離が短いほど大きくなるように前記リスク値を算出する、請求項１から３のいずれか１項に記載の制御装置。

【請求項6】

前記算出部は、前記マニピュレータの軌道に沿った前記スタート位置から前記マニピュレータの現在位置までの長さが長いほど大きくなるように前記リスク値を算出する、請求項１から３のいずれか１項に記載の制御装置。

【請求項7】

前記算出部は、前記物体と前記マニピュレータとの距離と前記物体の速度とを含む項と、定数項とを含むリスク関数を用いて前記リスク値を算出し、
前記定数項の値は負であり、
前記第２範囲の上限値は０である、請求項２に記載の制御装置。

【請求項8】

前記算出部は、リスク関数を用いて前記リスク値を算出し、
前記リスク関数は、前記マニピュレータに対する前記物体の相対移動方向と、前記物体と前記マニピュレータとを結ぶ線とのなす角度の余弦をｘ、前記物体の速度をｖ_obs、前記マニピュレータと前記物体との距離をＤ、前記マニピュレータの軌道に沿った前記スタート位置から前記マニピュレータの現在位置までの長さをＳ_curとするとき、
Ｈ（ｘ）×（ｖ_obs ²／Ｄ）×ｆ（Ｓ_cur）
の項を含み、
Ｈ（ｘ）は、ｘが１に近いほど大きく、
ｆ（Ｓ_cur）は、Ｌが長いほど大きい、請求項１から３のいずれか１項に記載の制御装置。

【請求項9】

Ｈ（ｘ）は、双曲線正接関数を含む、請求項８に記載の制御装置。

【請求項10】

ｆ（Ｓ_cur）は、シグモイド関数を含む、請求項８に記載の制御装置。

【請求項11】

前記標準速度に対する、前記第２動作における前記マニピュレータの移動速度の割合は、前記マニピュレータの軌道に沿った前記スタート位置から前記マニピュレータの現在位置までの長さが長いほど小さい、請求項２に記載の制御装置。

【請求項12】

前記算出部は、前記マニピュレータの積載量が大きいほど大きくなるように前記リスク値を算出する、請求項１から３のいずれか１項に記載の制御装置。

【請求項13】

前記算出部は、前記物体と前記マニピュレータとの距離と前記物体の速度とを含む項と、定数項とを含むリスク関数を用いて前記リスク値を算出し、
前記定数項は、前記ロボットと協働して作業する人に応じて設定される、請求項１から３のいずれか１項に記載の制御装置。

【請求項14】

ロボットのマニピュレータがスタート位置からゴール位置まで移動するように前記ロボットを制御する制御方法であって、
前記マニピュレータと物体との干渉のリスク値を算出するステップと、
複数種類の動作を含むグループの中から前記リスク値に応じて選択された対象動作に従って前記マニピュレータを動作させるための指令を生成するステップと、
前記グループとして、前記マニピュレータが基準位置よりも前記スタート位置に近いときに第１グループを設定し、前記マニピュレータが前記基準位置よりも前記ゴール位置に近いときに第２グループを設定するステップと、を備え、
前記第１グループは、
前記リスク値が第１範囲内であるときの動作として、前記リスク値を下げるように軌道を変更する第１動作を含み、
前記第２グループは、
前記リスク値が前記第１範囲の一部である第１サブ範囲内であるときの動作として、前記第１動作を含み、
前記リスク値が前記第１範囲のうち前記第１サブ範囲とは異なる第２サブ範囲内であるときの動作として、軌道を変更せずに速度を低下させる第２動作を含む、制御方法。

【請求項15】

請求項１４に記載の制御方法をコンピュータに実行させる、プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、制御装置、制御方法および制御プログラムに関する。

【背景技術】

【0002】

近年、ＦＡ（Factory Automation）分野において、人とロボットとが協調して作業する協調作業システムの開発が進められている。協調作業システムでは、人とロボットとの干渉を回避する必要がある。

【0003】

「Justinas Miseikis、他３名、「Multi 3D camera mapping for predictive and reflexive robot manipulator trajectory estimation」、2016 IEEE Symposium Series on Computational Intelligence (SSCI)」（非特許文献１）は、物体とロボットとの干渉を回避するように、物体とロボットとの干渉の可能性を示すコストに応じてロボットの軌道を変更する技術を開示している。

【先行技術文献】

【非特許文献】

【0004】

【文献】Justinas Miseikis、他３名、「Multi 3D camera mapping for predictive and reflexive robot manipulator trajectory estimation」、2016 IEEE Symposium Series on Computational Intelligence (SSCI)

【発明の概要】

【発明が解決しようとする課題】

【0005】

非特許文献１に開示の技術では、物体が人である場合、人の動きに応じてロボットの軌道の変更が頻繁に実施され得る。ロボットの軌道の変更には、大きなコンピューティング負荷が伴う。そのため、コンピューティング負荷を軽減するとともに、物体とロボットとの干渉の発生を抑制する技術が望まれている。

【0006】

本開示は、上記の問題に鑑みてなされたものであり、その目的は、コンピューティング負荷を軽減するとともに、物体とロボットとの干渉の発生を抑制することが可能な制御装置、制御方法および制御プログラムを提供することである。

【課題を解決するための手段】

【0007】

本開示の一例によれば、制御装置は、ロボットのマニピュレータがスタート位置からゴール位置まで移動するようにロボットを制御する。制御装置は、マニピュレータと物体との干渉のリスク値を算出する算出部と、複数種類の動作を含むグループの中からリスク値に応じて選択された対象動作に従ってマニピュレータを動作させるための指令を生成する生成部と、を備える。さらに、制御装置は、グループとして、マニピュレータが基準位置よりもスタート位置に近いときに第１グループを設定し、マニピュレータが基準位置よりもゴール位置に近いときに第２グループを設定する設定部を備える。第１グループは、リスク値が第１範囲内であるときの動作として、リスク値を下げるように軌道を変更する第１動作を含む。第２グループは、リスク値が第１範囲の一部である第１サブ範囲内であるときの動作として、第１動作を含み、リスク値が第１範囲のうち第１サブ範囲とは異なる第２サブ範囲内であるときの動作として、軌道を変更せずに速度を低下させる第２動作を含む。

【0008】

この開示によれば、マニピュレータがゴール位置に近いときには、軌道が変更される頻度が低下し、制御装置のコンピューティング負荷が軽減される。また、マニピュレータが基準位置よりもスタート位置に近く、かつ、リスク値が第１範囲内であるとき、リスク値を下げるようにマニピュレータの軌道が変更される。さらに、マニピュレータが基準位置よりもゴール位置に近く、かつ、リスク値が第１サブ範囲内であるときも、リスク値を下げるようにマニピュレータの軌道が変更される。これにより、物体とマニピュレータとの干渉の発生を抑制することができる。このように、上記の制御装置によれば、コンピューティング負荷を軽減するとともに、物体とロボットとの干渉の発生を抑制することができる。

【0009】

上述の開示において、第１グループおよび第２グループの各々は、リスク値が第１範囲よりも下の第２範囲内であるときの動作として、マニピュレータの軌道を変更せずに、標準速度でマニピュレータを動作させる第３動作を含む。第２動作は、標準速度より低い速度でマニピュレータを動作させる。

【0010】

この開示によれば、リスク値が第２範囲内であるとき、軌道が変更しない。そのため、軌道が変更される頻度が低下し、制御装置のコンピューティング負荷が軽減される。また、リスク値が第１範囲よりも下の第２範囲内であるため、第２サブ範囲内であるときの速度よりも速い標準速度でマニピュレータを動作させることができる。

【0011】

上述の開示において、第１グループおよび第２グループの各々は、リスク値が第１範囲より上の第３範囲内であるときの動作として、マニピュレータをスタート位置に向かって戻す第４動作を含む。

【0012】

この開示によれば、マニピュレータが物体から遠ざかる方向に移動する。その結果、物体とロボットとの干渉が抑制される。

【0013】

上述の開示において、算出部は、マニピュレータに対する物体の相対移動方向と、物体とマニピュレータとを結ぶ線とのなす角度が０°に近いほど大きくなるようにリスク値を算出する。

【0014】

この開示によれば、物体がマニピュレータに向かって移動している可能性が高いときほどリスク値が大きくなる。そのため、リスク値は、物体とマニピュレータとが干渉する可能性を的確に表すことができる。

【0015】

上述の開示において、算出部は、物体の速度が大きいほど大きくなるようにリスク値を算出するとともに、物体とマニピュレータとの距離が短いほど大きくなるようにリスク値を算出する。

【0016】

この開示によっても、リスク値は、物体とマニピュレータとが干渉する可能性を的確に表すことができる。

【0017】

上述の開示において、算出部は、マニピュレータの軌道に沿ったスタート位置からマニピュレータの現在位置までの長さが長いほど大きくなるようにリスク値を算出する。

【0018】

ロボットの動作開始直後に比べて、マニピュレータがゴール位置に近いときの方が物体（例えば人）とマニピュレータとの干渉による物体への危害が大きい。この開示によれば、リスク値は、物体への危害の大きさを反映した値を示すことができる。

【0019】

上述の開示において、算出部は、物体とマニピュレータとの距離と物体の速度とを含む項と、定数項とを含むリスク関数を用いてリスク値を算出する。定数項の値は負である。第２範囲の上限値は０である。

【0020】

この開示によれば、リスク値が０未満であることにより、物体とマニピュレータとが干渉する可能性が小さいことが容易に認識される。

【0021】

上述の開示において、算出部は、リスク関数を用いてリスク値を算出する。リスク関数は、マニピュレータに対する物体の相対移動方向と、物体とマニピュレータとを結ぶ線とのなす角度の余弦をｘ、物体の速度をｖ_obs、マニピュレータと物体との距離をＤ、マニピュレータの軌道に沿ったスタート位置からマニピュレータの現在位置までの長さをＳ_curとするとき、
Ｈ（ｘ）×（ｖ_obs ²／Ｄ）×ｆ（Ｓ_cur）
の項を含む。Ｈ（ｘ）は、ｘが１に近いほど大きい。ｆ（Ｓ_cur）は、Ｌが長いほど大きい。

【0022】

この開示によれば、リスク値は、物体とマニピュレータとが干渉する可能性を的確に表すことができるとともに、物体への危害の大きさを反映した値を示すことができる。

【0023】

Ｈ（ｘ）は、例えば双曲線正接関数を含む。ｆ（Ｓ_cur）は、例えばシグモイド関数を含む。

【0024】

上述の開示において、標準速度に対する、第２動作におけるマニピュレータの移動速度の割合は、マニピュレータの軌道に沿ったスタート位置からマニピュレータの現在位置までの長さが長いほど小さい。

【0025】

この開示によれば、マニピュレータがゴール位置に近いほど、第２動作における速度を遅くすることができる。これにより、物体とマニピュレータとの干渉の発生が抑制される。

【0026】

上述の開示において、算出部は、マニピュレータの積載量が大きいほど大きくなるようにリスク値を算出する。

【0027】

この開示によれば、マニピュレータの積載量が大きいとき、ロボットは、マニピュレータと物体との干渉が抑制されるようにマニピュレータの動作をより早く変更することができる。

【0028】

上述の開示において、算出部は、物体とマニピュレータとの距離と物体の速度とを含む項と、定数項とを含むリスク関数を用いてリスク値を算出する。定数項は、ロボットと協働して作業する人に応じて設定される。

【0029】

この開示によれば、マニピュレータと干渉しやすい作業を行なう人ほどリスク値を大きくすることができる。これにより、物体とマニピュレータとの干渉の発生が抑制される。

【0030】

本開示の一例によれば、制御方法は、ロボットのマニピュレータがスタート位置からゴール位置まで移動するようにロボットを制御する。制御方法は、マニピュレータと物体との干渉のリスク値を算出するステップと、複数種類の動作を含むグループの中からリスク値に応じて選択された対象動作に従ってマニピュレータを動作させるための指令を生成するステップと、を備える。さらに、制御方法は、グループとして、マニピュレータが基準位置よりもスタート位置に近いときに第１グループを設定し、マニピュレータが基準位置よりもゴール位置に近いときに第２グループを設定するステップと、を備える。第１グループは、リスク値が第１範囲内であるときの動作として、リスク値を下げるように軌道を変更する第１動作を含む。第２グループは、リスク値が第１範囲の一部である第１サブ範囲内であるときの動作として、第１動作を含み、リスク値が第１範囲のうち第１サブ範囲とは異なる第２サブ範囲内であるときの動作として、軌道を変更せずに速度を低下させる第２動作を含む。

【0031】

本開示の一例によれば、プログラムは、上記の制御方法をコンピュータに実行させる。これらの開示によっても、コンピューティング負荷を軽減するとともに、物体とロボットとの干渉の発生を抑制することができる。

【発明の効果】

【0032】

本開示によれば、コンピューティング負荷を軽減するとともに、物体とロボットとの干渉の発生を抑制することができる。

【図面の簡単な説明】

【0033】

【図1】実施の形態に係る制御装置が適用されるシステムの一例を示す図である。

【図2】設定部の処理を説明する図である。

【図3】制御装置のハードウェア構成を示す模式図である。

【図4】OctoMapの一例を示す図である。

【図5】シグモイド関数の一例を示すグラフである。

【図6】速度ベクトルｖ_obs，ｖ_tcpおよびベクトルｖの関係を示す図である。

【図7】速度ベクトルの方向関数Ｈ_dictの一例を示す図である。

【図8】グループに属する動作の種類の一例を示す図である。

【図9】制御装置の処理の流れの一例を示す図である。

【図10】速度ベクトルｖ_obs，ｖ_tcpおよびベクトルｖと方向関数Ｈ_dictの値との関係を示す図である。

【発明を実施するための形態】

【0034】

本発明の実施の形態について、図面を参照しながら詳細に説明する。なお、図中の同一または相当部分については、同一符号を付してその説明は繰返さない。以下で説明される各変形例は、適宜選択的に組み合わされてもよい。

【0035】

§１ 適用例
本実施の形態に係る制御装置が適用されるシステムの概略について説明する。図１は、実施の形態に係る制御装置が適用されるシステムの一例を示す図である。図１に示されるように、システム１は、制御装置１００と、ロボット２００と、複数のセンシングデバイス３００と、を備える。

【0036】

システム１は、工場等の生産現場に設けられる。生産現場では、図１に示されるように、人４００とロボット２００とが協調して作業する。人４００は、ロボット２００にとって移動障害物となり、ロボット２００との間で干渉が生じ得る。システム１は、人４００とロボット２００との干渉を抑制するための処理を行なう。

【0037】

ロボット２００は、マニピュレータ２０２と、マニピュレータコントローラ２０４と、を含む。

【0038】

マニピュレータ２０２は、複数のアーム２１１，２１２，２１３と、複数の関節２２１，２２２，２２３と、回動部２３０と、台座部２４０とを有する。具体的には、マニピュレータ２０２では、先端側から、アーム２１１、関節２２１、アーム２１２、関節２２２、アーム２１３、関節２２３、回動部２３０、台座部２４０が、この順で設けられている。

【0039】

アーム２１１の先端部には、エンドエフェクタが取り付けられる。アーム２１１の後端部は、関節２２１に接続されている。アーム２１２の先端部には、関節２２１が取り付けられている。アーム２１２の後端部は、関節２２２に接続されている。アーム２１３の先端部には、関節２２２が取り付けられている。アーム２１３の後端部は、関節２２３に接続されている。

【0040】

関節２２１は、アーム２１２に対してアーム２１１を移動させる。関節２２２は、アーム２１３に対してアーム２１２を移動させる。関節２２３は、回動部２３０に対してアーム２１３を移動させる。

【0041】

回動部２３０は、関節２２３を支持するとともに、回転軸を中心に回転可能に構成されている。台座部２４０は、回動部２３０を回動可能に支持する。

【0042】

マニピュレータコントローラ２０４は、マニピュレータ２０２の動作を制御する。具体的には、マニピュレータコントローラ２０４は、マニピュレータ２０２の先端部、つまり、アーム２１１の先端部（以下、「ツールセンターポイント（ＴＣＰ）」と称する。」の目標位置および移動速度に関する指令を制御装置１００から取得する。マニピュレータコントローラ２０４は、取得した指令に従ってツールセンターポイントが移動するように、関節２２１～２２３および回動部２３０の動作を制御する。

【0043】

センシングデバイス３００は、ロボット２００が存在する空間に配置され、当該空間における物体の位置を検知するデバイスである。物体には、マニピュレータ２０２および人４００が含まれる。センシングデバイス３００は、例えばＲＧＢ－Ｄカメラまたはレーザレンジファインダであり、視野の点群データを生成する。空間において死角がないように、例えば４つ以上のセンシングデバイス３００が互いに異なる位置に配置されることが好ましい。

【0044】

制御装置１００は、ロボット２００のマニピュレータ２０２がスタート位置からゴール位置まで移動するようにロボット２００を制御する。

【0045】

制御装置１００は、複数のセンシングデバイス３００と通信可能に接続される。制御装置１００は、例えばGigE Vision（登録商標）またはＵＳＢ（Universal Serial Bus）を用いて、複数のセンシングデバイス３００と通信する。

【0046】

制御装置１００は、マニピュレータコントローラ２０４と通信可能に接続される。制御装置１００は、例えばEthernet/IP（登録商標）を用いて、マニピュレータコントローラ２０４と通信する。制御装置１００は、マニピュレータコントローラ２０４との通信のやり取りにより、マニピュレータ２０２の現在位置を認識する。

【0047】

図１に示されるように、制御装置１００は、算出部１０と、生成部１２と、設定部１４と、を備える。

【0048】

算出部１０は、複数のセンシングデバイス３００の検知結果に基づいて、マニピュレータ２０２とマニピュレータ２０２の周囲の物体（人４００を含む）との干渉のリスク値を算出する。

【0049】

生成部１２は、複数種類の動作を含むグループの中からリスク値に応じて選択された対象動作に従って、ロボット２００を動作させるための指令を生成する。具体的には、生成部１２は、対象動作に従って、現在位置からゴール位置までのツールセンターポイントの軌道を決定する。生成部１２は、決定した軌道に基づいて、制御周期ごとのツールセンターポイントの目標位置および移動速度を特定する。制御装置１００は、制御周期ごとに、特定した目標位置および移動速度に関する指令を生成し、生成した指令をマニピュレータコントローラ２０４に出力する。

【0050】

設定部１４は、マニピュレータ２０２の位置に応じて、上記の複数種類の動作を含むグループを設定する。

【0051】

図２は、設定部の処理を説明する図である。図２に示されるように、設定部１４は、マニピュレータ２０２が基準位置よりもスタート位置ＰＳに近いときに第１グループを設定し、マニピュレータ２０２が基準位置よりもゴール位置ＰＥに近いときに第２グループを設定する。

【0052】

第１グループは、リスク値が第１範囲内であるときの動作として、リスク値を下げるようにマニピュレータ２０２の軌道を変更する迂回動作を含む。

【0053】

一方、第２グループは、リスク値が第１範囲の一部である第１サブ範囲内であるときの動作として、上記の迂回動作を含む。さらに、第２グループは、リスク値が第１範囲のうち第１サブ範囲とは異なる第２サブ範囲内であるときの動作として、軌道を変更せずにマニピュレータ２０２の速度を低下させる警戒動作を含む。

【0054】

これにより、マニピュレータ２０２がゴール位置ＰＥに近いときには、軌道が変更される頻度が低下し、制御装置１００のコンピューティング負荷が軽減される。

【0055】

また、マニピュレータ２０２が基準位置よりもスタート位置ＰＳに近く、かつ、リスク値が第１範囲内であるとき、リスク値を下げるようにマニピュレータ２０２の軌道が変更される。さらに、マニピュレータ２０２が基準位置よりもゴール位置ＰＥに近く、かつ、リスク値が第１サブ範囲内であるときも、リスク値を下げるようにマニピュレータ２０２の軌道が変更される。これにより、人４００とマニピュレータ２０２との干渉の発生を抑制することができる。

【0056】

このように、本実施の形態に係る制御装置１００によれば、コンピューティング負荷を軽減するとともに、人４００とロボット２００との干渉の発生を抑制することができる。

【0057】

§２ 具体例
＜制御装置のハードウェア構成＞
図３は、制御装置のハードウェア構成を示す模式図である。制御装置１００は、典型的には、汎用的なコンピュータアーキテクチャに従う構造を有する。

【0058】

図３に示されるように、制御装置１００は、制御処理回路１１０と、フィールドネットワークコントローラ１２０と、を含む。

【0059】

制御処理回路１１０は、ロボット２００を駆動するために必要な演算処理を実行する。一例として、制御処理回路１１０は、プロセッサ１１２と、メインメモリ１１４と、ストレージ１１６と、インターフェイス回路１１８とを含む。

【0060】

プロセッサ１１２は、ロボット２００を駆動するための制御演算を実行する。メインメモリ１１４は、例えば、ＤＲＡＭ（Dynamic Random Access Memory）やＳＲＡＭ（Static Random Access Memory）などの揮発性記憶装置などで構成される。ストレージ１１６は、例えば、ＳＳＤ（Solid State Drive）やＨＤＤ（Hard Disk Drive）などの不揮発性記憶装置などで構成される。

【0061】

ストレージ１１６には、ロボット２００の制御を実現するためのシステムプログラム１３０が格納される。システムプログラム１３０は、ロボット２００の動作に係る制御演算を実行する命令、および、ロボット２００との間のインターフェイスに係る命令を含む。図１に示す算出部１０、生成部１２および設定部１４は、プロセッサ１１２がシステムプログラム１３０を実行することにより実現される。

【0062】

教示データ１３２は、予め定められたマニピュレータ２０２のツールセンターポイントの軌道に関するデータである。

【0063】

インターフェイス回路１１８は、ロボット２００との間でデータをやり取りする。
フィールドネットワークコントローラ１２０は、フィールドネットワークを介して、主として、センシングデバイス３００との間でデータをやり取りする。

【0064】

＜算出部の処理内容＞
算出部１０は、一定周期で、マニピュレータ２０２と物体との干渉のリスク値を算出する。以下、算出部１０の具体的な処理内容について説明する。

【0065】

算出部１０は、各周期において、複数のセンシングデバイス３００から点群データを取得する。各センシングデバイス３００から取得する点群データは、当該センシングデバイス３００の座標系（以下、「カメラ座標系」と称する。）で表される。そのため、算出部１０は、各センシングデバイス３００から取得した点群データを、カメラ座標系からロボット２００の座標系（以下、「ロボット座標系」と称する。）に変換する。カメラ座標系からロボット座標系に変換するための変換行列は、センシングデバイス３００ごとに予め実施されるキャリブレーションによって作成される。

【0066】

取得した全ての点群データを使用すると、コンピューティング負荷が大きくなる。そのため、算出部１０は、点群データからOctoMapを生成する。

【0067】

図４は、OctoMapの一例を示す図である。図４に示されるように、OctoMapは、空間を区分けすることにより得られる、３次元立方体形状の複数のボクセル５００の各々における物体の占有率を示す。物体には、マニピュレータ２０２および人４００だけでなく、例えばマニピュレータ２０２の周囲に意図せず置かれた障害物も含まれ得る。算出部１０は、点群データにボクセル５００を導入し、ボクセル５００ごとの占有率を算出する。占有率は、例えば０．００～１００の値で示される。点群データからOctoMapを生成する手法として、公知の技術（例えば非特許文献１に記載の技術）を用いることができる。このようにして、算出部１０は、各周期のOctoMapを生成する。

【0068】

なお、制御装置１００は、マニピュレータコントローラ２０４との通信の遣り取りにより、マニピュレータ２０２の現在位置を認識している。そのため、算出部１０は、OctoMapから、マニピュレータ２０２が存在しているボクセルを削除してもよい。これにより、OctoMapを用いた後述のコンピューティング負荷が軽減される。

【0069】

算出部１０は、以下の式（１）に従って、各周期における、マニピュレータ２０２と物体（主として人４００）との干渉のリスク値I_riskを算出する。

【0070】

【数1】

【0071】

式（１）において、Ｈ_dictは、マニピュレータ２０２および人４００の速度ベクトルの方向関数である。ｖ_obsは、人４００の速度ベクトルである。Ｄ_euclは、マニピュレータ２０２のツールセンターポイントと人４００との距離である。Sigmoid（Ｓ_cur，Ｓ_thrd）は、シグモイド関数である。Ｓ_curは、軌道に沿った、スタート位置からツールセンターポイントの現在位置までの長さである。Ｓ_thrdは、予め定められる閾値であり、例えば、軌道に沿った、スタート位置からゴール位置までの長さの０．６～０．７倍の長さである。Ｐ_scoliは、静的障害物に関する迂回動作項である。Ｐ_biasは、正常性バイアス項であり、予め定められる負の定数項である。

【0072】

算出部１０は、各周期のOctoMapを用いて、人４００の位置を特定する。人４００が存在するボクセルの占有率は高い値を取る。そのため、算出部１０は、OctoMapにおいて占有率の高いボクセルの位置に基づいて、人４００の位置を特定する。

【0073】

なお、算出部１０は、各ボクセルの占有率が０，１のいずれかをとるようにOctoMapに対して２値化処理を行ない、２値化されたOctoMapを用いて、人４００の位置を特定してもよい。これにより、コンピューティング負荷がさらに軽減される。

【0074】

算出部１０は、マニピュレータ２０２のツールセンターポイントの現在位置と、現在の周期において生成されたOctoMapから特定された人４００の位置とに基づいて、距離Ｄ_euclを算出する。

【0075】

算出部１０は、過去一定期間において生成されたOctoMapを用いて、人４００の速度ベクトルｖ_obsを算出する。

【0076】

算出部１０は、マニピュレータ２０２のツールセンターポイントの現在位置とツールセンターポイントの軌道とに基づいて、長さＳ_curを算出する。ツールセンターポイントの軌道は、後述するように生成部１２によって生成される。

【0077】

シグモイド関数Sigmoid（Ｓ_cur，Ｓ_thrd）は、以下の式（２）で表される。式（２）において、ａは定数である。

【0078】

【数2】

【0079】

図５は、シグモイド関数の一例を示すグラフである。図５に示されるように、シグモイド関数Sigmoid（Ｓ_cur，Ｓ_thrd）は、Ｓ_curがＳ_thrdよりも小さい値からＳ_thrdに近づくにつれて大きくなり、Ｓ_curがＳ_thrdを超えると１に近づく。Ｓ_thrd付近における傾きは、定数ａに依存する。定数ａの値を大きいほど、Ｓ_thrd付近における傾きが急峻となる。

【0080】

算出部１０は、現在位置から目標位置までのツールセンターポイントの軌道上に存在する１以上のボクセルの占有率の合計値に所定係数を乗じた値を迂回動作項Ｐ_scoliとして算出する。軌道上に物体が存在していない場合、迂回動作項Ｐ_scoliは０となる。軌道は、後述するように生成部１２によって生成される。そのため、算出部１０は、生成部１２によって生成される軌道を用いて迂回動作項Ｐ_scoliを算出する。なお、動作開始時の軌道は、ストレージ１１６に記憶される教示データ１３２によって示される。

【0081】

速度ベクトルの方向関数Ｈ_dictは、以下の式（３）で表される。式（３）に示されるように、方向関数Ｈ_dictは、双曲線正接関数である。式（３）において、Ｋ，ｂは、定数であり、例えば３／２，１０にそれぞれ設定される。式（３）の変数ｘは、式（４）に従って算出される。

【0082】

【数3】

【0083】

【数4】

【0084】

ｖ_tcpは、マニピュレータ２０２のツールセンターポイントの速度ベクトルである。算出部１０は、マニピュレータコントローラ２０４に出力された最新の指令に基づいて、速度ベクトルｖ_tcpを算出する。

【0085】

ｖは、人４００の位置を始点とし、ツールセンターポイントの位置を終点とするベクトルである。算出部１０は、ツールセンターポイントの現在位置と、現在の周期のOctoMapから特定された人４００の位置とに基づいて、ベクトルｖを算出する。

【0086】

図６は、速度ベクトルｖ_obs，ｖ_tcpおよびベクトルｖの関係を示す図である。図６に示されるように、上記の式（４）に従って算出されるｘは、ベクトル（ｖ_obs－ｖ_tcp）とベクトルｖとのなす角度θの余弦（cosθ）である。そのため、ｘは、－１から１の値をとる。

【0087】

角度θは、言い換えると、ツールセンターポイントＴＣＰに対する人４００の相対移動方向と、人４００とツールセンターポイントＴＣＰとを結ぶ線とのなす角度である。そのため、ｘが１に近いほど、人４００がツールセンターポイントＴＣＰに向かって移動している可能性が高いことを表す。

【0088】

図７は、速度ベクトルの方向関数Ｈ_dictの一例を示す図である。図７には、Ｋ＝３／２，ｂ＝１０，ｘ_thrd＝０．９９９であるときの方向関数Ｈ_dictが示される。図７に示されるように、方向関数Ｈ_dictは、ｘ＝０．７６付近から急峻に立ち上がる。図６に示す角度θ＝４０°のときにｘ＝０．７６６となる。すなわち、ツールセンターポイントＴＣＰに対する人４００の相対移動方向と、人４００とツールセンターポイントＴＣＰとを結ぶ線とのなす角度θが－４０°～４０°の範囲において、方向関数Ｈ_dictは有意が値をとり、角度θが当該範囲外において、方向関数Ｈ_dictは約０となる。

【0089】

余弦が０．９９９となる角度は、±２．５６°である。そのため、角度θが－２．５６°～２．５６°の範囲において、方向関数Ｈ_dictは、Ｋ以上の値となる。

【0090】

算出部１０は、このようにして算出された変数を式（１）に代入することにより、各周期における、マニピュレータ２０２と物体との干渉のリスク値I_riskを算出する。

【0091】

式（１）において、方向関数Ｈ_dictが第１項の係数として存在する。そのため、ツールセンターポイントに対する人４００の相対移動方向と、人４００とツールセンターポイントとを結ぶ線とのなす角度θが－４０°～４０°の範囲外であるとき、第１項は約０となる。角度θが－４０°～４０°の範囲内であるとき、第１項は、正の有意な値を取る。このとき、角度θが小さいほど（０°に近いほど）、リスク値I_riskは大きくなる。また、第１項において、ツールセンターポイントと人４００との距離Ｄ_euclは分母に存在する。そのため、距離Ｄ_euclが小さいほど、リスク値I_riskは大きくなる。さらに、第１項において、人４００の速度ベクトルｖ_obsの二乗が分子に存在する。そのため、人４００の速度が大きいほど、リスク値I_riskは大きくなる。

【0092】

角度θが－４０°～４０°の範囲外であるとき、人４００とマニピュレータ２０２との干渉の可能性は低い。一方、角度θが－４０°～４０°の範囲内であるとき、人４００とマニピュレータ２０２とが干渉する可能性があり、角度θが小さいほど、その可能性が高まる。また、ツールセンターポイントと人４００とが近いほど、マニピュレータ２０２と人４００との干渉の可能性が高くなる。さらに、人４００の速度が大きいほど、マニピュレータ２０２と人４００との干渉の可能性が高くなる。そのため、リスク値I_riskは、人４００とマニピュレータ２０２とが干渉する可能性を的確に表している。

【0093】

第１項には、シグモイド関数Sigmoid（Ｓ_cur，Ｓ_thrd）が含まれる。シグモイド関数Sigmoid（Ｓ_cur，Ｓ_thrd）は、図５に示されるように、Ｓ_curが大きくなるにつれて、つまり、マニピュレータ２０２のツールセンターポイントがゴール位置に近くなるにつれて大きくなる。そのため、式（１）は、マニピュレータ２０２のツールセンターポイントがゴール位置に近くなるにつれて、リスク値I_riskが高くなるように設定される。これは、ロボット２００の動作開始直後に比べて、マニピュレータ２０２のツールセンターポイントがゴール位置に近いときの方がマニピュレータ２０２と人４００との干渉による人４００への危害が大きいためである。

【0094】

マニピュレータ２０２の周囲の空間において、マニピュレータ２０２の軌道上に意図しない障害物が存在していることが有り得る。このような場合、マニピュレータ２０２と障害物との衝突を回避すべきである。そのため、式（１）には、迂回動作項Ｐ_scoliが含まれる。

【0095】

＜設定部の処理内容＞
設定部１４は、マニピュレータ２０２の位置に応じて、ロボット２００が取りうる複数種類の動作を含むグループとして第１グループおよび第２グループのいずれかを設定する。以下、設定部１４の処理内容について説明する。

【0096】

設定部１４は、以下の式（５）に従って、警戒行動活性化フラグＷ_actを決定する。設定部１４は、警戒行動活性化フラグＷ_actが０のときに第１グループを設定し、警戒行動活性化フラグＷ_actが１のときに第２グループを設定する。

【0097】

【数5】

【0098】

式（５）において、閾値Ｔｈは予め定められる。軌道に沿って、スタート位置から、シグモイド関数Sigmoid（Ｓ_cur，Ｓ_thrd）が閾値Ｔｈ以下から閾値Ｔｈを超えるときの長さＳ_curだけ離れた位置が基準位置に対応する。そのため、基準位置に応じて閾値Ｔｈが設定される。閾値Ｔｈは、例えば０に近い値（０．０５～０．１など）に設定される。なお、有効数字の桁数によっては、ツールセンターポイントがスタート位置から出発してからしばらくの間、シグモイド関数Sigmoid（Ｓ_cur，Ｓ_thrd）の値が連続して０となり得る。そのため、閾値Ｔｈとして０が設定されてもよい。

【0099】

＜生成部の処理内容＞
生成部１２は、設定されたグループの中からリスク値I_riskに応じて選択された対象動作に従って、ロボット２００を動作させるための指令を生成する。以下、生成部１２の処理内容について説明する。

【0100】

図８は、グループに属する動作の種類の一例を示す図である。図８に示されるように，第１グループは、平準動作と、迂回動作と、反射動作と、を含む。第２グループは、平準動作と、迂回動作と、警戒動作と、反射動作と、を含む。

【0101】

第１グループが設定された場合、生成部１２は、以下のようにして対象動作を選択する。すなわち、生成部１２は、リスク値I_riskが範囲Ｒａ内であるとき、平準動作を対象動作として選択する。生成部１２は、リスク値I_riskが範囲Ｒａよりも上の範囲Ｒｂ内であるとき、迂回動作を選択する。生成部１２は、リスク値I_riskが範囲Ｒｂよりも上の範囲Ｒｃ内であるとき、反射動作を選択する。

【0102】

第２グループが設定された場合、生成部１２は、以下のようにして対象動作を選択する。すなわち、生成部１２は、リスク値I_riskが範囲Ｒａ内であるとき、平準動作を対象動作として選択する。生成部１２は、リスク値I_riskが範囲Ｒｂの一部であるサブ範囲Ｒｂ－１内であるとき、迂回動作を選択する。生成部１２は、リスク値I_riskが範囲Ｒｂのうちサブ範囲Ｒｂ－１とは異なるサブ範囲Ｒｂ－２内であるとき、警戒動作を選択する。サブ範囲Ｒｂ－２は、サブ範囲Ｒｂ－１よりも上の範囲である。生成部１２は、リスク値I_riskが範囲Ｒｃ内であるとき、反射動作を選択する。

【0103】

（平準動作）
平準動作は、マニピュレータ２０２と物体との干渉の可能性が小さいときに選択される動作である。そのため、範囲Ｒａは、例えば、リスク値I_riskが取り得る最小値であるＰ_bias（負の値）から０に設定される。すなわち、範囲Ｒａの上限値が０である。この場合、リスク値I_riskが０以下であるときに、マニピュレータ２０２と物体との干渉の可能性が小さいことが容易に認識され得る。

【0104】

平準動作は、軌道を変更せずに標準速度を維持する動作である。標準速度は、軌道上の最高速度が予め設定された速度となるように、軌道上の各点について計算される、ツールセンターポイントの速度である。生成部１２は、動作開始時において、教示データ１３２によって示される軌道を選択する。生成部１２は、迂回動作を対象動作として選択しない限り、教示データ１３２によって示される軌道を維持する。生成部１２は、迂回動作を対象動作として選択した後に平準動作を対象動作として選択する場合、迂回動作によって変更された軌道を維持する。

【0105】

生成部１２は、軌道に基づいて、制御周期ごとのツールセンターポイントの目標位置および標準速度を特定する。制御装置１００は、制御周期ごとに、特定した目標位置および標準速度に関する指令を生成し、生成した指令をマニピュレータコントローラ２０４に出力する。

【0106】

（迂回動作）
迂回動作は、リスク値I_riskを下げるように軌道を変更する動作である。生成部１２は、変更後の軌道に基づいて、次の制御周期のツールセンターポイントの目標位置および移動速度を特定する。制御装置１００は、特定した目標位置および移動速度に関する指令を生成し、生成した指令をマニピュレータコントローラ２０４に出力する。

【0107】

生成部１２は、公知の技術（例えば非特許文献１に開示の技術）を用いて、変更後の軌道を決定すればよい。具体的には、生成部１２は、ＲＲＴ（Rapidly-exploring Random Trees）－Ｃｏｎｎｅｃｔの手法を用いて、現在位置からゴール位置までの複数の軌道候補を探索する。当該探索は、予め定められる制限時間内に実施される。生成部１２は、探索された複数の軌道候補の各々について、以下の式（６）を用いて、コストＣ_trajを算出する。

【0108】

【数6】

【0109】

Ｄ_trajは、ボクセルのユークリッド距離である。式（６）の第１項は、軌道候補が通過するボクセルのユークリッド距離の合計である。Ｃ_voxelは、軌道候補が通過する各ボクセルの占有率を示す。式（６）の第２項は、軌道候補上に物体が存在する確率を表す。

【0110】

生成部１２は、コストＣ_trajが最も小さい軌道候補を、変更後の軌道として決定する。これにより、マニピュレータ２０２と物体との干渉の可能性が小さい軌道に変更される。

【0111】

（警戒動作）
警戒動作は、軌道を変更せずに速度を低下させる動作である。具体的には、生成部１２は、平準動作と同じ方法で、制御周期ごとのツールセンターポイントの目標位置および標準速度を特定する。次に、生成部１２は、特定した標準速度の１／２を移動速度として決定する。生成部１２は、目標位置および決定した移動速度に関する指令を生成し、生成した指令をマニピュレータコントローラ２０４に出力する。

【0112】

あるいは、生成部１２は、以下の式（７）に従って、移動速度Ｖ_currを決定してもよい。式（７）において、Ｖ_stdは、平準動作において特定される標準速度である。

【0113】

【数7】

【0114】

式（７）に従って移動速度Ｖ_currが決定されることにより、マニピュレータ２０２がゴール位置に近づくにつれて、警戒行動時の移動速度Ｖ_currが遅くなる。言い換えると、標準速度Ｖ_stdに対する、警戒動作におけるマニピュレータ２０２の移動速度Ｖ_currの割合は、マニピュレータ２０２の軌道に沿ったスタート位置からマニピュレータ２０２の現在位置までの長さＳ_curが長いほど小さい。

【0115】

（反射動作）
反射動作は、軌道に沿って、スタート位置に向かってマニピュレータ２０２を戻す動作である。具体的には、生成部１２は、１以上の制御周期前の目標位置まで戻す指令を生成し、生成した指令をマニピュレータコントローラ２０４に出力する。

【0116】

＜制御装置の処理フロー＞
図９は、制御装置の処理の流れの一例を示す図である。図９に示すステップＳ１～Ｓ６は、ロボットの動作の開始時から動作終了時までの期間において一定周期ごとに繰り返し実施される。

【0117】

まず、制御装置１００のプロセッサ１１２は、複数のセンシングデバイス３００の各々から点群データを取得する（ステップＳ１）。

【0118】

次に、プロセッサ１１２は、取得した点群データを用いて、マニピュレータ２０２の周囲の空間のOctoMapを生成する（ステップＳ２）。

【0119】

次に、プロセッサ１１２は、マニピュレータ２０２の現在位置に基づいて、複数種類の動作を含むグループを設定する（ステップＳ３）。具体的には、プロセッサ１１２は、警戒行動活性化フラグＷ_actを算出し、警戒行動活性化フラグＷ_actが０である場合に第１グループを設定し、警戒行動活性化フラグＷ_actが１である場合に第２グループを設定する。

【0120】

次に、プロセッサ１１２は、OctoMapを用いて、マニピュレータ２０２と人４００との干渉のリスク値I_riskを算出する（ステップＳ４）。

【0121】

次に、プロセッサ１１２は、設定されたグループの中から、リスク値I_riskに応じた対象動作を選択する（ステップＳ５）。

【0122】

次に、プロセッサ１１２は、選択した対象動作に従って指令を生成し、生成した指令をマニピュレータコントローラ２０４に出力する（ステップＳ６）。

【0123】

動作開始時において、軌道上に意図しない障害物が置かれている場合、式（１）における迂回動作項Ｐ_scoliが大きな値をとる。動作開始直後では、第１グループが設定されるため、リスク値I_riskが範囲Ｒｂ内となり、迂回動作が実施される。その結果、障害物を迂回する軌道に変更される。

【0124】

動作開始時において、軌道上に障害物が存在しない場合、教示データ１３２によって示される軌道に従って平準動作が実施される。マニピュレータ２０２の周囲に人４００がいない場合、一定周期ごとに算出されるリスク値I_riskは、０未満となり、範囲Ｒａ内で維持される。そのため、教示データ１３２によって示される軌道に沿って、スタート位置からゴール位置までマニピュレータ２０２が移動する。

【0125】

動作開始後、マニピュレータ２０２が基準位置に達するまでの間、第１グループが設定される。この期間において、人４００がロボット２００に近づいた場合、リスク値I_riskは、０を超え、範囲Ｒｂ内となる。これにより、人４００を迂回する軌道に変更される。その結果、マニピュレータ２０２と人４００との干渉が抑制される。軌道の変更後、リスク値I_riskが範囲Ｒａ内になると、平準動作が実施され、変更後の軌道に沿って標準速度でマニピュレータ２０２が動作される。

【0126】

一方、マニピュレータ２０２が基準位置を通過してからゴール位置に達するまでの間、第２グループが設定される。この期間において、人４００がロボット２００に近づいた場合、リスク値I_riskは、０を超え、範囲Ｒｂ内となる。リスク値I_riskが範囲Ｒｂのうち下側の範囲Ｒｂ－１内である場合、わずかな軌道の変更によって、人４００との干渉を回避できる可能性がある。そのため、リスク値I_riskが範囲Ｒｂ－１内である場合、迂回動作が実施される。一方、リスク値I_riskが範囲Ｒｂのうち上側の範囲Ｒｂ－２内である場合において迂回動作が選択されたとすると、人４００との干渉を回避するためには、軌道が大きく変更される可能性がある。マニピュレータ２０２がゴール位置に近いタイミングでの軌道の大きな変更は好ましくない。そのため、リスク値I_riskが範囲Ｒｂ－２内である場合、警戒動作が実施される。すなわち、軌道を変更せずに、標準速度よりも低い速度でマニピュレータ２０２が動作される。これにより、人４００がマニピュレータ２０２の存在に気付いて、マニピュレータ２０２から遠ざかる時間が取れる。その結果、マニピュレータ２０２と人４００との干渉が抑制される。

【0127】

リスク値I_riskが範囲Ｒｂよりも上の範囲Ｒｃ内にある場合、マニピュレータ２０２と人４００とが干渉する間近の状態である。そのため、リスク値I_riskが範囲Ｒｃ内にある場合、反射動作が実施される。これにより、マニピュレータ２０２が人４００から遠ざかる方向に移動する。その結果、マニピュレータ２０２と人４００との干渉が抑制される。

【0128】

＜変形例＞
マニピュレータ２０２は、様々な物を積載した状態で移動し得る。マニピュレータ２０２の積載量が大きいほど、マニピュレータ２０２と人４００とが干渉したときの人４００への影響が大きい。そのため、算出部１０は、マニピュレータ２０２の積載量が大きいほど大きくなるようにリスク値I_riskを算出してもよい。例えば、算出部１０は、以下の式（８）に従って、各周期における、マニピュレータ２０２と物体（主として人４００）との干渉のリスク値I_riskを算出する。

【0129】

【数8】

【0130】

Ｈ_dictは、式（１）と同様に、マニピュレータ２０２および人４００の速度ベクトルの方向関数である。ただし、Ｈ_dictは、上記の式（３）で表されてもよいし、以下の式（９）で表されてもよい。

【0131】

【数9】

【0132】

式（９）において、ａ，ｂは予め定められる係数である。例えば、ａ＝１０、ｂ＝０に設定される。ｘは、上記の式（４）によって算出される。

【0133】

図１０は、速度ベクトルｖ_obs，ｖ_tcpおよびベクトルｖと方向関数Ｈ_dictの値との関係を示す図である。図１０に示されるように、速度ベクトルｖ_obs，ｖ_tcpが互いに向かい合うとき、つまりベクトル（ｖ_obs－ｖ_tcp）とベクトルｖとのなす角度θが０であるときにＨ_dictは１となる。角度θが０より大きく９０°未満であるとき、Ｈ_dictは０．５より大きい値となる。角度θが９０°より大きくなると、Ｈ_dictは０．５より小さい値となる。角度θが１８０°に近づくと、Ｈ_dictは０に近づく。

【0134】

式（８）において、ｍ_payloadは、マニピュレータ２０２の積載量が大きくなるほど大きな値をとる項である。ｍ_payloadは、例えば以下の式（１０）で表される。

【0135】

【数10】

【0136】

式（１０）において、αは、予め定められる係数であり、例えば１．０である。ｍ_{payload_min}は、最小のリスクファクタ値であり、予め定められる。例えば、ｍ_{payload_min}は、１．０である。Ｐ_maxは、マニピュレータ２０２の最大積載量であり、Ｐ_minは、マニピュレータ２０２の最小積載量である。Ｐ_maxおよびＰ_minは、マニピュレータ２０２の仕様に応じて予め定められる。例えば、Ｐ_maxは１０ｋｇであり、Ｐ_minは１ｋｇである。Ｐ_currは、現在のマニピュレータ２０２の積載量である。ロボット２００は、現在のマニピュレータ２０２の積載量Ｐ_currを計測するセンサを備えている。算出部１０は、ロボット２００の当該センサから積載量Ｐ_currを取得し、取得した積載量Ｐ_currを上記の式（１０）に代入することにより、ｍ_payloadを算出する。

【0137】

Ｒ_biasは、リスク値I_riskを調整する定数項である。Ｒ_biasは、例えば、式（１）のＰ_scoliとＰ _bias との和であってもよい。あるいは、Ｒ_biasは、過去の運用環境のリスクに応じて予め定められてもよい。例えば、リスクの高い生産現場ではＲ_biasとして正の数値が設定される。リスクが低い生産現場では、ロボット２００の行動が変更される頻度を下げるために、Ｒ_biasとして負の数値が設定される。

【0138】

マニピュレータ２０２と干渉しやすい作業を行なう人と、そうでない人とが存在しうる。そのため、Ｒ_biasの値は、ロボット２００と協働して作業する人４００に応じて設定されてもよい。例えば、Ｒ_biasのデフォルト値として０が設定される。その後、人４００ごとにリスク値I_riskの推移が記録される。記録されたリスク値I_riskの代表値（例えば、中央値、平均値）に応じて、人４００ごとのＲ_biasが設定される。これにより、マニピュレータ２０２と干渉しやすい作業を行なう人４００ほどリスク値を大きくすることができる。その結果、人４００とマニピュレータ２０２との干渉の発生が抑制される。なお、式（１）におけるＰ_scoliおよびＰ _bias も、ロボット２００と協働して作業する人４００に応じて設定されてもよい。

【0139】

式（８）を用いることにより、マニピュレータ２０２の積載量に応じたリスク値I_riskが算出される。その結果、マニピュレータ２０２の積載量が大きいとき、ロボット２００は、マニピュレータ２０２と人４００との干渉が抑制されるようにマニピュレータ２０２の動作をより早く変更することができる。

【0140】

§３ 付記
以上のように、本実施の形態は以下のような開示を含む。

【0141】

（構成１）
ロボット（２００）のマニピュレータ（２０２）がスタート位置からゴール位置まで移動するように前記ロボット（２００）を制御する制御装置（１００）であって、
前記マニピュレータ（２０２）と物体（４００）との干渉のリスク値を算出する算出部（１０）と、
複数種類の動作を含むグループの中から前記リスク値に応じて選択された対象動作に従って前記マニピュレータ（２０２）を動作させるための指令を生成する生成部（１２）と、
前記グループとして、前記マニピュレータ（２０２）が基準位置よりも前記スタート位置に近いときに第１グループを設定し、前記マニピュレータ（２０２）が前記基準位置よりも前記ゴール位置に近いときに第２グループを設定する設定部（１４）と、を備え、
前記第１グループは、
前記リスク値が第１範囲内であるときの動作として、前記リスク値を下げるように軌道を変更する第１動作を含み、
前記第２グループは、
前記リスク値が前記第１範囲の一部である第１サブ範囲内であるときの動作として、前記第１動作を含み、
前記リスク値が前記第１範囲のうち前記第１サブ範囲とは異なる第２サブ範囲内であるときの動作として、軌道を変更せずに速度を低下させる第２動作を含む、制御装置（１００）。

【0142】

（構成２）
前記第１グループおよび前記第２グループの各々は、前記リスク値が前記第１範囲よりも下の第２範囲内であるときの動作として、前記マニピュレータの軌道を変更せずに、標準速度で前記マニピュレータ（２０２）を動作させる第３動作を含み、
前記第２動作は、前記標準速度より低い速度で前記マニピュレータ（２０２）を動作させる、構成１に記載の制御装置（１００）。

【0143】

（構成３）
前記第１グループおよび前記第２グループの各々は、前記リスク値が前記第１範囲より上の第３範囲内であるときの動作として、前記マニピュレータ（２０２）を前記スタート位置に向かって戻す第４動作を含む、構成１または２に記載の制御装置（１００）。

【0144】

（構成４）
前記算出部（１０）は、前記マニピュレータ（２０２）に対する前記物体（４００）の相対移動方向と、前記物体（４００）と前記マニピュレータ（２０２）とを結ぶ線とのなす角度が０°に近いほど大きくなるように前記リスク値を算出する、構成１から３のいずれかに記載の制御装置（１００）。

【0145】

（構成５）
前記算出部（１０）は、前記物体（４００）の速度が大きいほど大きくなるように前記リスク値を算出するとともに、前記物体（４００）と前記マニピュレータ（２０２）との距離が短いほど大きくなるように前記リスク値を算出する、構成１から４のいずれかに記載の制御装置（１００）。

【0146】

（構成６）
前記算出部（１０）は、前記マニピュレータ（２０２）の軌道に沿った前記スタート位置から前記マニピュレータ（２０２）の現在位置までの長さが長いほど大きくなるように前記リスク値を算出する、構成１から５のいずれかに記載の制御装置（１００）。

【0147】

（構成７）
前記算出部（１０）は、前記物体（４００）と前記マニピュレータ（２０２）との距離と前記物体（４００）の速度とを含む項と、定数項とを含むリスク関数を用いて前記リスク値を算出し、
前記定数項の値は負であり、
前記第２範囲の上限値は０である、構成２に記載の制御装置（１００）。

【0148】

（構成８）
前記算出部（１０）は、リスク関数を用いて前記リスク値を算出し、
前記リスク関数は、前記マニピュレータ（２０２）に対する前記物体（４００）の相対移動方向と、前記物体（４００）と前記マニピュレータ（２０２）とを結ぶ線とのなす角度の余弦をｘ、前記物体（４００）の速度をｖ_obs、前記マニピュレータ（２０２）と前記物体（４００）との距離をＤ、前記マニピュレータ（２０２）の軌道に沿った前記スタート位置から前記マニピュレータ（２０２）の現在位置までの長さをＳ_curとするとき、
Ｈ（ｘ）×（ｖ_obs ²／Ｄ）×ｆ（Ｓ_cur）
の項を含み、
Ｈ（ｘ）は、ｘが１に近いほど大きく、
ｆ（Ｓ_cur）は、Ｌが長いほど大きい、構成１から３のいすれかに記載の制御装置（１００）。

【0149】

（構成９）
Ｈ（ｘ）は、双曲線正接関数を含む、構成８に記載の制御装置（１００）。

【0150】

（構成１０）
ｆ（Ｓ_cur）は、シグモイド関数を含む、構成８または９に記載の制御装置（１００）。

【0151】

（構成１１）
前記標準速度に対する、前記第２動作における前記マニピュレータ（２０２）の移動速度の割合は、前記マニピュレータ（２０２）の軌道に沿った前記スタート位置から前記マニピュレータ（２０２）の現在位置までの長さが長いほど小さい、構成２に記載の制御装置（１００）。

【0152】

（構成１２）
前記算出部（１０）は、前記マニピュレータ（２０２）の積載量が大きいほど大きくなるように前記リスク値を算出する、構成１から３のいずれか１項に記載の制御装置（１００）。

【0153】

（構成１３）
前記算出部（１０）は、前記物体（４００）と前記マニピュレータ（２０２）との距離と前記物体（４００）の速度とを含む項と、定数項とを含むリスク関数を用いて前記リスク値を算出し、
前記定数項は、前記ロボットと協働して作業する人に応じて設定される、構成１から３のいずれか１項に記載の制御装置（１００）。

【0154】

（構成１４）
ロボット（２００）のマニピュレータ（２０２）がスタート位置からゴール位置まで移動するように前記ロボット（２００）を制御する制御方法であって、
前記マニピュレータ（２０２）と物体（４００）との干渉のリスク値を算出するステップと、
複数種類の動作を含むグループの中から前記リスク値に応じて選択された対象動作に従って前記マニピュレータ（２０２）を動作させるための指令を生成するステップと、
前記グループとして、前記マニピュレータ（２０２）が基準位置よりも前記スタート位置に近いときに第１グループを設定し、前記マニピュレータ（２０２）が前記基準位置よりも前記ゴール位置に近いときに第２グループを設定するステップと、を備え、
前記第１グループは、
前記リスク値が第１範囲内であるときの動作として、前記リスク値を下げるように軌道を変更する第１動作を含み、
前記第２グループは、
前記リスク値が前記第１範囲の一部である第１サブ範囲内であるときの動作として、前記第１動作を含み、
前記リスク値が前記第１範囲のうち前記第１サブ範囲とは異なる第２サブ範囲内であるときの動作として、軌道を変更せずに速度を低下させる第２動作を含む、制御方法。

【0155】

（構成１５）
構成１４に記載の制御方法をコンピュータ（１００）に実行させる、プログラム（１３０）。

【0156】

本発明の実施の形態について説明したが、今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は請求の範囲によって示され、請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

【符号の説明】

【0157】

１ システム、１０ 算出部、１２ 生成部、１４ 設定部、１００ 制御装置、１１０ 制御処理回路、１１２ プロセッサ、１１４ メインメモリ、１１６ ストレージ、１１８ インターフェイス回路、１２０ フィールドネットワークコントローラ、１３０ システムプログラム、１３２ 教示データ、２００ ロボット、２０２ マニピュレータ、２０４ マニピュレータコントローラ、２１１～２１３ アーム、２２１～２２３ 関節、２３０ 回動部、２４０ 台座部、３００ センシングデバイス、４００ 人、５００ ボクセル、ＰＥ ゴール位置、ＰＳ スタート位置、Ｒａ～Ｒｃ 範囲、Ｒｂ－１，Ｒｂ－２ サブ範囲。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】