JP2014018941A

JP2014018941A - 制御装置、及び制御方法

Info

Publication number: JP2014018941A
Application number: JP2012162418A
Authority: JP
Inventors: Hiroto Sakahara; 洋人坂原
Original assignee: Daihen Corp
Current assignee: Daihen Corp
Priority date: 2012-07-23
Filing date: 2012-07-23
Publication date: 2014-02-03

Abstract

【課題】衝突の発生を早期に検知できない。
【解決手段】マニピュレータの作業内容がステップごとに記述された作業プログラムが記憶される作業プログラム記憶部１１、作業プログラムで記述されている作業内容に対応するステップごとの指令値をサーボコントローラに出力する作業内容解釈部１２、作業プログラムのステップとしきい値が対応付けられて記憶されるしきい値記憶部１５、出力された指令値によって制御されるモータに関する推定値を取得する推定値取得部１３、推定値に対応するモータに関する現実値を取得する現実値取得部１４、出力された指令値に応じた動作がマニピュレータで実行されている際に、推定値と現実値との差分値が、動作に応じたステップに対応するしきい値を超えた場合に、衝突を検知する衝突判断部１８を備える。様々な動作に対して適切なしきい値を設定することで早期の衝突検知が可能となる。
【選択図】図１

Description

本発明は、マニピュレータの衝突が発生したかどうかを判断する制御装置等に関する。

産業用ロボットにおいて、衝突の発生を検知することは、事故の影響を小さくする上で非常に重要である。そのような衝突を検知する一般的な方法として、位置やトルクの推定値と現実値とを比較し、差分があるしきい値を超えると衝突が発生したと判断する手法がある。例えば、特許文献１には、外乱推定オブザーバによって外乱トルクを推定し、その推定された外乱トルクを用いて算出された衝突成分がしきい値以上となった場合に衝突が発生していると判断する手法が記載されている。また、その特許文献１には、衝突のない状況における衝突成分の最大値に所定のマージン値を乗じた値をしきい値として用いることが記載されている。また、特許文献２には、モデルベースの学習によりロボットのパラメータを自動調整する手法が記載されている。また、特許文献３には、各軸角度と学習パラメータとを入力とするニューラルネットワークによる衝突検知手法が記載されている。

特開平１１−７０４９０号公報特開昭６３−２６７０２号公報特開平７−２２３１８１号公報

しかしながら、特許文献１の手法では、衝突成分の最大値よりも大きいしきい値が設定されるため、衝突成分が大きくならない動作時には、衝突検知が遅れる可能性があるという問題があった。
また、衝突の検知にはマニピュレータの形状や摩擦の要素など、複雑な要素が関係するため、特許文献２，３のような自動化技術を用いたとしても、様々な動作に対して早期に衝突の発生を検知することは困難であった。

本発明は、上記問題点を解決するためになされたものであり、様々な動作に対して、早期に衝突の発生を検知することができる制御装置等を提供することを目的とする。

上記目的を達成するため、本発明による制御装置は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの制御を行う制御装置であって、マニピュレータの作業内容がステップごとに記述された作業プログラムが記憶される作業プログラム記憶部と、作業プログラム記憶部で記憶されている作業プログラムで記述されている作業内容に対応するステップごとの指令値を、モータを制御するサーボコントローラに出力する作業内容解釈部と、作業プログラムのステップと、ステップが実行される期間に用いられるしきい値とが対応付けられて記憶されるしきい値記憶部と、作業内容解釈部が出力した指令値を用いて、指令値によって制御されるモータに関する推定値を取得する推定値取得部と、推定値に対応するモータに関する現実値を取得する現実値取得部と、作業内容解釈部が出力した指令値に応じた動作がマニピュレータにおいて実行されている際に、推定値取得部が取得した推定値と、現実値取得部が取得した現実値との差である差分値が、その動作に応じたステップに対応するしきい値を超えた場合に、衝突が発生したと判断する衝突判断部と、を備えたものである。
このような構成により、各ステップに応じた動作が実行されている場合に、そのステップに対応するしきい値を用いて衝突の判断を行うことができる。その結果、様々な動作に対して適切なしきい値を設定することによって、早期の衝突検知が可能となる。

また、本発明による制御装置では、しきい値記憶部において、ステップごとにしきい値が対応付けられていてもよい。
このような構成により、例えば、ステップごとに異なるしきい値を設定することができる。

また、本発明による制御装置では、作業プログラムは繰り返して実行されるものであり、各ステップにおいて、差分値の過去の最大値を用いてしきい値を算出し、しきい値記憶部に蓄積するしきい値算出部をさらに備えてもよい。
このような構成により、差分値の過去の最大値を用いて、しきい値を自動的に算出することができる。

また、本発明による制御装置では、作業プログラムが実行されるごとに、各ステップにおける差分値の過去の最大値の平均値及び標準偏差を更新する更新部をさらに備え、しきい値算出部は、更新部が更新した各ステップにおける差分値の過去の最大値の平均値及び標準偏差を用いて、ステップごとにしきい値を算出してもよい。
このような構成により、差分値の過去の最大値の平均値及び標準偏差を更新し、その更新された平均値等を用いることによって、しきい値を適切な値に設定することができる。

また、本発明による制御装置では、しきい値記憶部では、作業プログラムにおける２以上のステップを含むグループに対してしきい値が対応付けられており、衝突判断部は、あるグループに属するステップが実行される期間には、グループに対応するしきい値を用いて衝突の発生を判断してもよい。
このような構成により、複数のステップに対応するしきい値を用いて、衝突の検知を行うことができる。類似する動作に対応する推定値や現実値は類似した値になると考えられるため、例えば、類似した動作については、同じしきい値を用いても適切に衝突検知を行うことができると考えられる。

また、本発明による制御装置では、推定値は、モータのトルク推定値であり、現実値は、モータの実トルク値であってもよい。

また、本発明による産業用ロボットは、上記制御装置と、マニピュレータと、その制御装置から出力された指令値に応じて、マニピュレータを制御するサーボコントローラと、を備えたものである。

本発明によるマニピュレータの制御装置等によれば、しきい値を細かく設定することができ、早期の衝突検知が可能となる。

本発明の実施の形態１による産業用ロボットの構成を示すブロック図同実施の形態によるサーボコントローラの構成を示すブロック図同実施の形態による制御装置の動作を示すフローチャート同実施の形態におけるステップとしきい値等との対応の一例を示す図同実施の形態におけるステップとしきい値等との対応の一例を示す図同実施の形態におけるステップとしきい値等との対応の一例を示す図同実施の形態におけるコンピュータシステムの構成の一例を示す図

以下、本発明による制御装置について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。

（実施の形態１）
本発明の実施の形態１による制御装置について、図面を参照しながら説明する。本実施の形態による制御装置は、作業プログラムのステップに対応するしきい値を用いて衝突の判断を行うものである。

図１は、本実施の形態による産業用ロボット１００の構成を示すブロック図である。本実施の形態による産業用ロボット１００は、制御装置１と、サーボコントローラ２と、マニピュレータ３とを備える。制御装置１は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータ３を制御する。具体的には、制御装置１は、マニピュレータ３のモータを制御するサーボコントローラ２を介して、マニピュレータ３を制御する。そのマニピュレータ３において、各モータとアームとは減速器を介して接続されている。また、直列に接続されたアームの先端に、手先効果器が設けられていてもよい。マニピュレータ３は、例えば、垂直多関節ロボットのマニピュレータであってもよく、水平多関節ロボットのマニピュレータであってもよい。また、そのマニピュレータ３の軸数は問わない。マニピュレータ３の軸数は、例えば、４軸であってもよく、５軸であってもよく、６軸であってもよく、または、その他の軸数であってもよい。サーボコントローラ２は、制御装置１から出力された指令値に応じて、マニピュレータ３を制御する。図１では、説明の便宜上、１個のサーボコントローラ２のみを示しているが、通常、産業用ロボット１００は、マニピュレータ３の軸数と同数のサーボコントローラを備えている。したがって、サーボコントローラ２は、各軸に対応するサーボコントローラの集合であると考えてもよい。サーボコントローラ２の詳細な構成については、図２のブロック図を用いて後述する。また、制御装置１は、ティーチングプレイバック方式によりマニピュレータ３を制御する。

本実施の形態では、図１で示されるように、制御装置１は、作業プログラム記憶部１１と、作業内容解釈部１２と、推定値取得部１３と、現実値取得部１４と、しきい値記憶部１５と、更新部１６と、値算出部１７と、衝突判断部１８と、判断結果出力部１９とを備える。

作業プログラム記憶部１１には、作業プログラムが記憶される。その作業プログラムは、マニピュレータ３の作業内容がステップごとに記述された情報である。その作業内容は、例えば、ティーチングされた作業内容であってもよく、または、そのティーチングに基づいて生成された作業内容であってもよい。その作業プログラムには、ステップごとにマニピュレータ３の先端の位置を示す情報が含まれていると考えてもよい。各ステップは、例えば、マニピュレータ３の先端の位置や速度を示す一つの移動指示であってもよい。また、マニピュレータ３の先端の位置は、例えば、手先効果器の３次元空間における位置や姿勢によって示されてもよく、または、マニピュレータ３の各軸の位置によって示されてもよい。その作業内容は、例えば、ステップごとに手先効果器の位置と姿勢とを示す情報であってもよい。この作業プログラムは、通常、繰り返して実行されるが、そうでなくてもよい。

作業プログラム記憶部１１に作業プログラムが記憶される過程は問わない。例えば、記録媒体を介して作業プログラムが作業プログラム記憶部１１で記憶されるようになってもよく、通信回線等を介して送信された作業プログラムが作業プログラム記憶部１１で記憶されるようになってもよく、あるいは、入力デバイスを介してティーチングされた作業プログラムが作業プログラム記憶部１１で記憶されるようになってもよい。作業プログラム記憶部１１での記憶は、ＲＡＭ等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。作業プログラム記憶部１１は、所定の記録媒体（例えば、半導体メモリや磁気ディスクなど）によって実現されうる。

作業内容解釈部１２は、作業プログラム記憶部１１で記憶されている作業プログラムで記述されている作業内容を解釈し、その作業内容に対応するステップごとの指令値を、モータを制御するサーボコントローラ２に出力する。作業内容解釈部１２は、通常、作業プログラムをステップごとに読み出して指令値を生成する。したがって、その指令値は、通常、ステップごとの指令値である。また、その指令値は、例えば、マニピュレータ３の各軸の位置指令値であってもよい。本実施の形態では、その指令値が位置指令値である場合について主に説明する。具体的には、作業内容解釈部１２は、例えば、作業プログラムで記述されるステップごとの手先効果器の位置・姿勢から、各軸の位置指令値を算出し、その位置指令値を各軸に対応するサーボコントローラ２に出力してもよい。それにより、ティーチングに応じたマニピュレータ３の制御が行われる。なお、作業プログラムが、ステップごとにマニピュレータ３の各軸の位置を示す場合には、作業内容解釈部１２は、そのステップを読み出し、各軸に対応するサーボコントローラ２にその読み出した各軸の位置である位置指令値を出力してもよい。

推定値取得部１３は、作業内容解釈部１２が出力した指令値を用いて、その指令値によって制御されるモータに関する推定値を取得する。指令値を用いるとは、直接的に用いることであってもよく、または、間接的に用いることであってもよい。直接的に用いるとは、例えば、指令値を引数とする関数などを用いて推定値を取得することであってもよい。間接的に用いるとは、例えば、指令値を用いて算出された他の値等を用いて推定値を取得することであってもよい。推定値取得部１３が取得する推定値については後述する。

現実値取得部１４は、推定値取得部１３が取得する推定値に対応するモータに関する現実値を取得する。この現実値については後述する。
なお、マニピュレータ３が複数軸を有する場合には、推定値取得部１３及び現実値取得部１４は、その軸ごとに、推定値や現実値を取得してもよい。

しきい値記憶部１５には、作業プログラムのステップと、そのステップが実行される期間に用いられるしきい値とが対応付けられて記憶される。なお、ステップが実行される期間とは、そのステップに対応する作業が実行中である期間のことであり、通常、そのステップが開始されてから、次のステップが開始される直前までの期間である。ステップとしきい値とが対応付けられているとは、所望のステップに対応するしきい値を特定することができるようになっているという意味であり、例えば、ステップの識別子と、しきい値とが対応付けられて記憶されていることであってもよい。そのステップの識別子は、例えば、そのステップを識別するための文字列や記号等であってもよく、または、そのステップに応じた作業内容が記憶されている作業プログラム記憶部１１の記憶領域を示すポインタであってもよい。また、作業プログラムのステップと、しきい値とが対応付けられているとは、両者が直接に対応付けられていていることであってもよく、または、他の情報を介して対応付けられていることであってもよい。後者の場合には、ステップと他の情報とが対応付けられており、他の情報としきい値とが対応付けられていることによって、ステップとしきい値とが間接的に対応付けられていてもよい。また、しきい値記憶部１５には、そのしきい値以外のパラメータも、ステップごとに記憶されてもよく、あるいは、そうでなくてもよい。本実施の形態では、しきい値記憶部１５において、しきい値以外のパラメータとして、後述する差分値の平均値、差分値の標準偏差、平均及び標準偏差の算出で用いる差分値の個数、並びに、作業プログラムの最新の実行時の差分値の最大値が記憶されている場合について主に説明する。また、本実施の形態では、しきい値記憶部１５において、ステップごとにしきい値が対応付けられている場合について主に説明する。すなわち、各ステップには、対応するしきい値が存在するものとする。なお、一部のステップについては、ステップごとに異なるしきい値が対応付けられていてもよい。また、他のステップにおいては、異なるステップに対して、同じしきい値が対応付けられていてもよい。また、マニピュレータ３が複数軸を有する場合には、しきい値記憶部１５では、その軸ごとに、ステップと、しきい値等とが対応付けられて記憶されてもよい。

本実施の形態では、しきい値記憶部１５で記憶されているしきい値は、しきい値算出部１７によって算出されたものである。ただし、しきい値の初期値は、あらかじめしきい値記憶部１５に蓄積されていてもよい。しきい値記憶部１５での記憶は、ＲＡＭ等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。しきい値記憶部１５は、所定の記録媒体（例えば、半導体メモリや磁気ディスクなど）によって実現されうる。

更新部１６は、作業プログラムが実行されるごとに、各ステップにおける差分値の過去の最大値の平均値及び標準偏差を更新する。その差分値は、後述するように、推定値と現実値との差である。更新部１６は、通常、作業プログラムが１回実行されるごとに、最後に実行された作業プログラムの各ステップにおける差分値の最大値をそれぞれ取得する。そして、更新部１６は、その取得した差分値の最大値と、作業プログラムの前回の実行までの差分値の最大値の平均値及び標準偏差とを用いて、その差分値の最大値の平均値及び標準偏差を更新する。その更新に関する具体的な式については後述する。更新部１６は、その更新を、作業プログラムのステップごとに行う。また、更新後の平均値等は、しきい値記憶部１５において、ステップごとに記憶されてもよい。

また、更新部１６は、しきい値記憶部１５で記憶されている、作業プログラムの最新の実行時の差分値の最大値をも更新してもよい。具体的には、更新部１６は、作業プログラムが実行されている際に、ステップごとに差分値を取得し、その差分値が最大値よりも大きい場合には、その差分値を最大値に上書きで蓄積してもよい。その処理を繰り返すことによって、ステップごとに、最新の作業プログラムの実行に応じた差分値の最大値を記録できる。

しきい値算出部１７は、各ステップにおいて、差分値の過去の最大値を用いてしきい値を算出し、しきい値記憶部１５に蓄積する。差分値の過去の最大値とは、作業プログラムの過去の実行ごとの差分値の最大値である。例えば、作業プログラムが１０回実行された場合には、差分値の最大値が１０個得られる。その場合には、しきい値算出部１７は、その１０個の最大値のうち、全部または一部を用いてしきい値を算出する。差分値の最大値を用いるとは、直接的に用いることであってもよく、あるいは、間接的に用いることであってもよい。直接的に用いるとは、例えば、差分値の最大値を引数とする関数などを用いてしきい値を算出することであってもよい。間接的に用いるとは、差分値の最大値を用いて算出された値、例えば、平均値や標準偏差等を用いてしきい値を算出することであってもよい。本実施の形態では、後者の場合、すなわち、しきい値算出部１７が差分値の最大値を間接的に用いてしきい値を算出する場合について主に説明する。具体的には、しきい値算出部１７が、更新部１６が更新した各ステップにおける差分値の過去の最大値の平均値及び標準偏差を用いてしきい値を算出する場合について説明する。その算出に関する具体的な式については後述する。しきい値はステップごとに存在するため、しきい値算出部１７は、ステップごとにしきい値の算出を行うものとする。その際に、あるステップに対応するしきい値を算出する際に用いられる差分値の過去の最大値は、そのステップに対応する差分値の過去の最大値である。しきい値算出部１７は、各ステップについて算出したしきい値を、しきい値記憶部１５に、各ステップに対応付けて蓄積する。後述する衝突の検知では、最新のしきい値のみが用いられるため、その蓄積は、通常、上書きによる蓄積であるが、そうでなくてもよい。
なお、マニピュレータ３が複数軸を有する場合には、更新部１６は、その軸ごとに各ステップにおける差分値の過去の最大値の平均値等を更新してもよく、しきい値算出部１７は、その軸ごとに各ステップのしきい値を算出してもよい。

衝突判断部１８は、作業内容解釈部１２が出力した指令値に応じた動作がマニピュレータ３において実行されている際に、その動作に応じたステップに対応するしきい値を用いて衝突に関する判断を行う。動作に応じたステップとは、その動作を行う原因となった指令値を出力する際に用いられたステップのことである。そのしきい値は、その動作がマニピュレータ３で実行される前に、しきい値記憶部１５から読み出されていてもよい。そして、衝突判断部１８は、推定値取得部１３が取得した推定値と、現実値取得部１４が取得した現実値との差である差分値がしきい値を超えた場合に、衝突が発生したと判断する。一方、その差分値がしきい値を超えていない場合には、衝突判断部１８は、衝突が発生していないと判断してもよい。差分値がしきい値と一致する場合には、衝突判断部１８は、しきい値を超えたと判断してもよく、あるいは、そうでなくてもよい。差分値がしきい値を超えているとは、差分値の絶対値がしきい値を超えていることであってもよい。その場合には、しきい値は正値である。なお、マニピュレータ３が複数軸を有する場合には、衝突判断部１８は、その軸ごとに衝突に関する判断を行ってもよい。そして、いずれかの軸で衝突が検知された場合に、衝突判断部１８は、衝突が発生したと判断してもよい。

判断結果出力部１９は、衝突が発生したと衝突判断部１８が判断した場合に、その判断結果を出力する。なお、判断結果出力部１９は、衝突が発生していないと衝突判断部１８が判断した場合に、その判断結果を出力してもよく、または、出力しなくてもよい。ここで、この出力は、例えば、表示デバイス（例えば、ＣＲＴや液晶ディスプレイなど）への表示でもよく、所定の機器への通信回線を介した送信でもよく、スピーカによる音声出力でもよく、記録媒体への蓄積でもよく、他の構成要素への引き渡しでもよい。なお、判断結果出力部１９は、出力を行うデバイス（例えば、表示デバイスや通信デバイスなど）を含んでもよく、あるいは含まなくてもよい。また、判断結果出力部１９は、ハードウェアによって実現されてもよく、あるいは、それらのデバイスを駆動するドライバ等のソフトウェアによって実現されてもよい。

なお、作業プログラム記憶部１１と、しきい値記憶部１５とは、同一の記録媒体によって実現されてもよく、あるいは、別々の記録媒体によって実現されてもよい。前者の場合には、作業プログラムを記憶している領域が作業プログラム記憶部１１となり、しきい値等を記憶している領域がしきい値記憶部１５となる。本実施の形態では、作業プログラム記憶部１１としきい値記憶部１５とが同一の記録媒体によって実現されており、しきい値等が、作業プログラムにおけるステップそのものに対応付けられている場合について主に説明する。

図２は、本実施の形態によるサーボコントローラ２の構成を示すブロック図である。本実施の形態によるサーボコントローラ２は、速度指令生成器２１と、電流指令生成器２２と、トルク指令生成器２３とを備える。

速度指令生成器２１は、モータの現在位置と、作業内容解釈部１２から受け取った位置指令値とに応じて速度指令値を生成する。速度指令生成器２１は、モータの現在位置が位置指令値に近づくようにフィードバック制御をするための速度指令値を生成する。
電流指令生成器２２は、モータの現在速度と、速度指令生成器２１が生成した速度指令値とに応じて電流指令値を生成する。電流指令生成器２２は、モータの現在速度が速度指令値に近づくようにフィードバック制御をするための電流指令値を生成する。
トルク指令生成器２３は、モータの現在電流と、電流指令生成器２２が生成した電流指令値とに応じてトルク指令値を生成し、モータに出力する。トルク指令生成器２３は、モータの現在電流が電流指令値に近づくようにフィードバック制御をするためのトルク指令値を生成する。モータは、このトルク指令値によって動作する。

なお、これらのフィードバック制御はすでに公知であり、その詳細な説明を省略する。また、モータの現在電流は、トルク指令生成器２３に入力される。また、モータの現在速度は、電流指令生成器２２に入力される。また、モータの現在位置は、速度指令生成器２１に入力される。なお、モータの現在速度は、モータの現在位置が時間微分されたものであってもよい。また、マニピュレータ３が複数のモータを有する場合には、そのモータごとに図２で示されるサーボコントローラ２が存在すると考えてもよい。

次に、推定値取得部１３が取得する推定値、及び現実値取得部１４が取得する現実値について説明する。推定値取得部１３が取得する推定値、及び現実値取得部１４が取得する現実値は、それぞれ前述の衝突の判断において用いられる値である。すなわち、推定値と現実値との差分がしきい値を超えた場合に、衝突が発生したと判断されることになる。本実施の形態による制御装置１のように、衝突検知用の追加的なセンサを用いないで衝突を検知する方法としては、例えば、外乱オブザーバを用いる方法や、適応制御を用いる方法などが知られている。それぞれの方法では、トルクなどの推定値（予測値）と、それに対応する現実値（実測値）とを比較して衝突検知を行う。したがって、本実施の形態における推定値と現実値は、外乱オブザーバを用いる方法や、適応制御を用いる方法などにおいて用いられる推定値や現実値であることが好適である。

例えば、推定値は、モータのトルク推定値であり、現実値は、モータの実トルク値であってもよい。その場合には、例えば、推定値取得部１３は、いわゆるオブザーバによってトルク推定値を取得してもよい。また、例えば、現実値取得部１４は、サーボコントローラ２から受け取ったモータの現在電流をトルクに換算することにより、実トルク値である現実値を取得してもよい。

また、例えば、推定値が位置指令値であり、現実値が現在位置であってもよく、推定値が速度指令値であり、現実値が現在速度であってもよい。なお、その推定値として、それらの値から、重力や、遠心力・コリオリ力、慣性力などの影響に応じた値を除いた値を用いてもよい。
なお、推定値と現実値との差分である差分値が、衝突に起因する外乱成分となり、その差分値と、しきい値とを用いて衝突の検知を行うことができるのであれば、その推定値や現実値は、特に限定されるものではない。

次に、制御装置１の動作について図３のフローチャートを用いて説明する。
（ステップＳ１０１）作業内容解釈部１２は、指令値の生成を行うかどうか判断する。そして、指令値の生成を行う場合には、ステップＳ１０２に進み、そうでない場合には、ステップＳ１０５に進む。なお、作業内容解釈部１２は、例えば、あるステップに対応するマニピュレータ３の動作が終了した場合に、次のステップの指令値の生成を行うと判断してもよい。また、作業内容解釈部１２は、例えば、作業プログラムの開始の指示が受け付けられた場合に、作業プログラムに含まれる１個目のステップに対応する指令値の生成を行うと判断してもよい。

（ステップＳ１０２）作業内容解釈部１２は、指令値の生成対象となるステップを作業プログラム記憶部１１から読み出す。

（ステップＳ１０３）作業内容解釈部１２は、読み出したステップを解釈し、それに応じた指令値を生成してサーボコントローラ２に出力する。

（ステップＳ１０４）衝突判断部１８は、作業内容解釈部１２がサーボコントローラ２に出力した指令値に応じたステップに対応するしきい値を、しきい値記憶部１５から読み出す。その読み出されたしきい値が、ステップＳ１０３で出力された指令値に応じた作業が行われる期間において衝突検知に用いられるしきい値となる。そのしきい値は、図示しない記録媒体で記憶されてもよい。そして、ステップＳ１０１に戻る。

（ステップＳ１０５）衝突判断部１８は、衝突に関する判断を行うかどうか判断する。そして、衝突に関する判断を行う場合には、ステップＳ１０６に進み、そうでない場合には、ステップＳ１１３に進む。なお、衝突判断部１８は、例えば、衝突に関する判断を行うと定期的に判断してもよい。

（ステップＳ１０６）現実値取得部１４は、現実値を取得する。その現実値は、図示しない記録媒体で記憶されてもよい。

（ステップＳ１０７）推定値取得部１３は、推定値を取得する。その推定値は、図示しない記録媒体で記憶されてもよい。

（ステップＳ１０８）衝突判断部１８は、推定値と現実値との差分である差分値を算出する。その差分値は、前述のように、差分の絶対値であってもよく、または、そうでなくてもよい。

（ステップＳ１０９）衝突判断部１８は、差分値がステップＳ１０４で読み出したしきい値を超えているかどうか判断する。そして、しきい値を超えている場合には、衝突判断部１８は、衝突が発生したと判断してステップＳ１１０に進み、そうでない場合には、ステップＳ１１１に進む。

（ステップＳ１１０）判断結果出力部１９は、衝突が発生した旨を出力する。

（ステップＳ１１１）更新部１６は、ステップＳ１０８で算出された差分値が、それまでの差分値の最大値より大きいかどうか判断する。そして、算出された差分値が、それまでの差分値の最大値より大きい場合には、ステップＳ１１２に進み、そうでない場合には、ステップＳ１０１に戻る。

（ステップＳ１１２）更新部１６は、しきい値記憶部１５で記憶されている差分値の最大値を、ステップＳ１０８で算出された差分値に更新する。なお、更新対象の差分値の最大値は、ステップＳ１０３で出力された最新の指令値に対応する動作に応じたステップに応じた差分値の最大値である。そして、ステップＳ１０１に戻る。

（ステップＳ１１３）更新部１６は、差分値の最大値の平均値や標準偏差を更新するかどうか判断する。そして、平均値等を更新する場合には、ステップＳ１１４に進み、そうでない場合には、ステップＳ１０１に戻る。なお、更新部１６は、例えば、あるステップに対応するマニピュレータ３の動作が終了した場合に、そのステップに対応する平均値等を更新すると判断してもよく、作業プログラムのすべてのステップに対応するマニピュレータ３の動作が終了した場合に、全ステップに対応する平均値等を更新すると判断してもよい。

（ステップＳ１１４）更新部１６は、差分値の最大値の最新の平均値や標準偏差、作業プログラムの実行時における最新の差分値の最大値等を用いて、差分値の最大値の平均値と標準偏差とを更新する。例えば、更新部１６は、後述する式（１）、（２）を用いて平均値等を更新してもよい。更新部１６は、更新した平均値等を、しきい値記憶部１５に蓄積する。

（ステップＳ１１５）しきい値算出部１７は、差分値の最大値の更新された平均値や標準偏差等を用いて、しきい値を算出する。例えば、しきい値算出部１７は、後述する式（３）を用いてしきい値を算出してもよい。しきい値算出部１７は、算出したしきい値を、しきい値記憶部１５に蓄積する。そして、ステップＳ１０１に戻る。

なお、図３のフローチャートにおいて、作業プログラムのステップごとに平均値等が更新される場合には、ステップＳ１１４，Ｓ１１５の処理は、１個のステップに対応する平均値の更新等の処理となる。一方、作業プログラムごとに平均値等が更新される場合には、ステップＳ１１４，Ｓ１１５の処理は、作業プログラムに含まれる全ステップに対応する平均値の更新等の処理となる。また、ステップＳ１１０において衝突の発生した旨が出力された後は、ステップＳ１１１に進まず、その衝突の発生に応じた回避の処理が行われるようにしてもよい。また、図３のフローチャートにおいて、衝突に関する判断を行うたびに推定値を取得する場合について説明したが、そうでなくてもよい。推定値の取得の間隔は、衝突に関する判断の間隔よりも長くてもよい。すなわち、推定値の取得は、衝突に関する判断が複数回行われるごとに行われてもよい。また、マニピュレータ３が複数軸を有する場合には、図３のフローチャートにおける各処理は、その軸ごとに行われてもよい。例えば、指令値の生成やしきい値の読み出し、しきい値を用いた衝突に関する判断、差分値の最大値の更新、平均値等の更新やしきい値の算出等の処理は、軸ごとに行われてもよい。また、図３のフローチャートにおいて、電源オフや処理終了の割り込みにより処理は終了する。

ここで、差分値の最大値の平均値及び標準偏差を更新する方法について説明する。
ｎ回目の作業プログラムの実行におけるｋ番目のステップに対応する差分値の最大値をｄ_ｎ（ｋ）とする。また、説明の便宜のため、ｄ_ｎ＋１（ｋ）＝Ｄ（ｋ）とする。また、ｎ回目の作業プログラムの実行後に更新された、ｋ番目のステップに対応する差分値の最大値の平均値をＡ_ｎ（ｋ）とし、差分値の最大値の標準偏差をＳＤ_ｎ（ｋ）とし、平均値や標準偏差で用いる差分値の最大値の個数をＴ_ｎ（ｋ）とする。

Ｔ_ｎ＋１（ｋ）とＴ_ｎ（ｋ）との関係は次式のようになる。

ただし、Ｔ_ｎ'（ｋ）は、次の通りである。

すなわち、作業プログラムの実行回数ｎがα以下であれば、作業プログラムが実行されるごとにＴ_ｎ（ｋ）がインクリメントされ、作業プログラムの実行回数ｎがαより大きければ、作業プログラムが実行されてもＴ_ｎ（ｋ）はインクリメントされず、定数となる。このことは、しきい値を更新する際に、α回よりも以前の作業プログラムの実行時における差分値の最大値の影響を受けないようにするためになされるものである。例えば、同一のマニピュレータ３であっても、季節に応じた温度変化や湿度変化、部品の摩耗等によって、摩擦等の要素が微妙に変化する可能性もある。そのため、しきい値の算出には、差分値の最近の最大値を用いることが好適であると考えられるため、上述のようにしている。なお、調整パラメータであるαは、例えば、１０であってもよく、または、その他の適切な値であってもよい。

ｎ回目の作業プログラムの実行後におけるｋ番目のステップの差分値の最大値の平均値Ａ_ｎ（ｋ）は、次式のようになる。ただし、Σの和は、最新のＴ_ｎ（ｋ）個についての和である。

したがって、Ａ_ｎ＋１（ｋ）は、次式のようになる。

次に、ｎ回目の作業プログラムの実行後におけるｋ番目のステップの差分値の最大値の標準偏差ＳＤ_ｎ（ｋ）は、次式のようになる。ただし、Σの和のとり方は前述の通りである。

したがって、ＳＤ_ｎ＋１（ｋ）は、次式のようになる。

ｎ＋１回目の作業プログラムの実行後におけるｋ番目のステップのしきい値ＴＨ_ｎ（ｋ）は、上述の値を使用して次式のように表される。なお、調整パラメータであるβは、例えば、３であってもよく、または、その他の値であってもよい。βを３にすることによって、理論上は、衝突が発生していないのに衝突が発生したと誤検出する確率が０．３％以下となる。このしきい値ＴＨ_ｎ＋１（ｋ）は、ｎ＋２回目の作業プログラムの実行中にｋ番目のステップのしきい値として使用される。マニピュレータ３が複数軸を有する場合には、各軸について、平均値や標準偏差がステップごとに更新されることになり、各軸について、しきい値がステップごとに算出されることになる。

次に、本実施の形態による制御装置１の動作について、具体例を用いて説明する。この具体例では、作業プログラム記憶部１１と、しきい値記憶部１５とが一体として構成されており、作業プログラムと、しきい値等とが直接、対応づけられているものとする。具体的には、図４Ａで示されるように、作業プログラム記憶部１１及びしきい値記憶部１５（以下、単に「作業プログラム記憶部１１等」とする）において、作業プログラムと、パラメータとが、ステップごとに対応付けられている。作業プログラムは、１番目のステップであるＳｔｅｐ（１）から、ｍ番目のステップ（ｍは２以上の整数である）であるＳｔｅｐ（ｍ）までのｍ個のステップを含んでいる。１番目のステップは、３次元空間における手先効果器の位置を示すＰｏｓｉｔｉｏｎ（１）と、手先効果器の方向を示すＰｏｓｅ（１）とを含んでいる。それらがＳｔｅｐ（１）の動作である。Ｐｏｓｉｔｉｏｎ（１）は、例えば、ｘｙｚ直交座標系における座標値であってもよい。また、Ｐｏｓｅ（１）は、例えば、球面座標系における仰角及び方位角と、その仰角及び方位角によって特定される方向を中心とした回転角とであってもよい。作業プログラムの各ステップは、例えば、ティーチングによって設定される。パラメータには、衝突検知で用いられるしきい値と、差分値の最大値の平均値と、差分値の最大値の標準偏差と、平均値や標準偏差の算出で用いる差分値の最大値の個数と、作業プログラムが最後に実行された際の差分値の最大値とが含まれている。また、しきい値等の下付き添字「ｎ」は、作業プログラムの実行回数を示す。すなわち、図４Ａでは、作業プログラムの各ステップがｎ回実行された後のパラメータが示されている。また、この具体例では、マニピュレータ３の１軸に対応する差分値の最大値の平均値及び標準偏差の更新やしきい値の算出が行われる場合について説明する。なお、マニピュレータ３が複数軸を有する場合には、その処理が軸数だけ繰り返して実行されることになる。

作業プログラム記憶部１１等において、図４Ａで示される作業プログラム等が記憶されている状況において、制御装置１に、作業プログラムを実行する旨の指示が入力されたとする。すると、作業内容解釈部１２は、作業プログラムの１個目のステップに対応する指令値を生成すると判断し（ステップＳ１０１）、作業プログラム記憶部１１等からＳｔｅｐ（１）の「Ｐｏｓｉｔｉｏｎ（１）、Ｐｏｓｅ（１）」を読み出す（ステップＳ１０２）。そして、作業内容解釈部１２は、手先効果器がそのＰｏｓｉｔｉｏｎ（１）の位置、Ｐｏｓｅ（１）の姿勢となるようにマニピュレータ３の各軸の位置指令値を生成し、それぞれ対応するサーボコントローラ２に渡す（ステップＳ１０３）。なお、サーボコントローラ２は、位置指令値を受け取ると、位置指令値と各軸の現在位置とを用いて速度指令値を生成し、その速度指令値と各軸の現在速度とを用いて電流指令値を生成し、その電流指令値と各軸の現在電流とを用いてトルク指令値を生成してモータに出力する。そのようなサーボコントロールによって、Ｓｔｅｐ（１）に応じた手先効果器の位置や姿勢の制御が行われる。

作業内容解釈部１２は、サーボコントローラ２に位置指令値を出力すると、衝突判断部１８に、その時点のステップを示すＳｔｅｐ（１）を渡す。すると、衝突判断部１８は、作業プログラム記憶部１１等を参照し、そのＳｔｅｐ（１）に対応するしきい値「ＴＨ_ｎ（１）」を読み出す（ステップＳ１０４）。そのしきい値は、その後の衝突検知で用いられる。なお、衝突判断部１８は、この時点でそのＳｔｅｐ（１）に対応する最大値Ｄ（１）を０に更新する。

その後、衝突判断部１８が、衝突に関する判断を行うと判断したとする（ステップＳ１０５）。すると、衝突判断部１８は、現実値取得部１４に、現実値である実トルク値を取得するように指示し、推定値取得部１３に、推定値である推定トルク値を取得するように指示する。そして、それらの指示に応じて、現実値取得部１４は、実トルク値ＥＴを取得して衝突判断部１８に渡し、推定値取得部１３は、推定トルク値ＰＴを取得して衝突判断部１８に渡す（ステップＳ１０６，Ｓ１０７）。すると、衝突判断部１８は、両トルクの差分値｜ＰＴ−ＥＴ｜を算出し（ステップＳ１０８）、｜ＰＴ−ＥＴ｜＞ＴＨ_ｎ（１）がみたされるかどうか判断する（ステップＳ１０９）。この場合には、衝突が発生しておらず、その関係がみたされなかったとする。すると、衝突判断部１８は、｜ＰＴ−ＥＴ｜＞Ｄ（１）がみたされるかどうか判断する（ステップＳ１１１）。この時点では、Ｄ（１）＝０であるため、その関係はみたされることになり、衝突判断部１８は、Ｄ（１）に｜ＰＴ−ＥＴ｜を上書きする（ステップＳ１１２）。このようにして、Ｓｔｅｐ（１）が実行されている期間に、順次、衝突に関する判断と、最大値Ｄ（１）の更新とが行われることになる（ステップＳ１０５〜Ｓ１１２）。

その後、Ｓｔｅｐ（１）の実行が終了し、次のＳｔｅｐ（２）の実行に移る前に、更新部１６は、平均値等を更新すると判断し（ステップＳ１１３）、前述の式のように平均値Ａ_ｎ＋１（１）と、標準偏差ＳＤ_ｎ＋１（１）と、個数Ｔ_ｎ＋１（１）とを算出し、それらを上書きで作業プログラム記憶部１１等に蓄積する（ステップＳ１１４）。また、しきい値算出部１７は、更新後の平均値Ａ_ｎ＋１（１）と、標準偏差ＳＤ_ｎ＋１（１）とを用いて、前述の式のように新たなしきい値ＴＨ_ｎ＋１（１）を算出し、そのしきい値を上書きで作業プログラム記憶部１１等に蓄積する（ステップＳ１１５）。その結果、Ｓｔｅｐ（１）に対応するパラメータは、図４Ｂで示されるように更新される。この後、それ以降のＳｔｅｐ（２）からＳｔｅｐ（ｍ）についても、順次、位置指令値の生成や衝突に関する判断、平均値や標準偏差、しきい値等の更新が実行される。

以上のように、本実施の形態による制御装置１によれば、作業プログラムの各ステップにしきい値が対応付けられているため、ステップごとに適切なしきい値を用いて衝突の検知を行うことができる。したがって、きめ細かい衝突検知を実現することができる。例えば、全ステップにわたって同じしきい値を用いた場合において、大きいしきい値を用いたときには、衝突の検知が遅れる可能性があり、小さいしきい値を用いたときには、衝突が発生していないのに衝突を誤検知する可能性があった。しかし、ステップごとのしきい値を用いることによって、各ステップの動作に適したしきい値を設定することができ、衝突検知の遅れや、衝突の誤検知を防止することができる。また、作業プログラムの実行に応じてしきい値を更新することにより、しきい値を自動的に適切な値に設定することができる。また、その際に用いる差分値の最大値の個数を限定することにより、より最新の実行結果を用いたしきい値の算出が可能となり、例えば、季節に応じてしきい値が微妙に変化するような場合に、その季節の変化を考慮することができるようになる。

なお、本実施の形態では、しきい値記憶部１５において、ステップごとにしきい値が対応付けられている場合について説明したが、しきい値はステップごとに対応付けられていなくてもよい。しきい値記憶部１５において、作業プログラムにおける２以上のステップを含むグループに対してしきい値が対応付けられていてもよい。すなわち、ステップの集合に対して、しきい値が対応付けられていてもよい。１個のグループに含まれる２以上のステップは、連続したステップであってもよく、あるいは、そうでなくてもよい。１個のグループに含まれる２以上のステップに対応するマニピュレータ３の動作は、似ている動作であることが好適である。似ている動作に対しては、しきい値も同様の値になると考えられるからである。そのように、ステップのグループにしきい値が対応付けられている場合には、衝突判断部１８は、あるグループに属するステップが実行される期間には、そのグループに対応するしきい値を用いて衝突の発生を判断するものとする。グループにしきい値が対応付けられている場合には、例えば、作業プログラムとパラメータとの関係は、図４Ｃで示されるようになっていてもよい。図４Ｃにおいて、作業プログラムの各ステップと、グループＩＤと、パラメータとが対応付けられている。同じグループＩＤに対応するステップは、同一のグループに属するステップである。図４Ｃでは、Ｓｔｅｐ（１）に対応するグループＩＤが「Ｇ００１」であり、Ｓｔｅｐ（３）に対応するグループＩＤも「Ｇ００１」であるため、Ｓｔｅｐ（１）とＳｔｅｐ（３）とは同じグループに属することになる。したがって、パラメータは、Ｓｔｅｐ（１）に対してのみ対応付けられており、Ｓｔｅｐ（３）には対応付けられていない。Ｓｔｅｐ（３）が実行される際には、そのＳｔｅｐ（３）に対応するグループＩＤ「Ｇ００１」に対応付けられているしきい値、すなわち、Ｓｔｅｐ（１）に対応するしきい値「ＴＨ_ｎ（１）」を用いて処理が行われるものとする。なお、パラメータの更新は、Ｓｔｅｐ（１）の実行のみに応じて行われてもよく、あるいは、Ｓｔｅｐ（３）の実行にも応じて行われてもよい。

また、本実施の形態では、例えば、図４Ａで示されるように、しきい値記憶部１５において、すべてのステップにしきい値が対応付けられている場合について説明したが、そうでなくてもよい。例えば、今から実行されるステップに対応するしきい値のみが、ステップに対応付けられていてもよい。具体的には、各ステップに対応付けられてしきい値以外のパラメータが記憶されており、あるステップを実行する前に、そのステップに対応するパラメータを用いて、そのステップに対応するしきい値が上述のようにして算出され、その算出されたしきい値を用いて衝突検知が行われてもよい。そして、そのステップが実行された後に、そのしきい値が削除されてもよい。その場合には、しきい値記憶部１５は、実行対象のステップに対応するしきい値を一時的に記憶する記録媒体であってもよい。

また、本実施の形態では、更新部１６が差分値の最大値の平均値及び標準偏差を更新する場合について説明したが、更新部１６は、それら以外の値の更新を行ってもよい。例えば、更新部１６は、差分値の最大値の標準偏差に代えて差分値の最大値の分散を更新してもよい。その場合には、式（３）において、標準偏差に代えて分散を用いるようにした式を用いてしきい値を算出してもよい。

また、本実施の形態では、更新部１６が差分値の最大値の平均値及び標準偏差を更新する場合について説明したが、その更新が行われなくてもよい。例えば、作業プログラムが実行されるごとに、ステップごとの差分値の最大値が蓄積され、その差分値の過去の最大値のみを用いて、平均値や標準偏差を算出してもよい。その場合には、制御装置１は更新部１６を備えていなくてもよい。

また、本実施の形態では、しきい値算出部１７が、差分値の最大値の平均値及び標準偏差を用いてしきい値を算出する場合について説明したが、そうでなくてもよい。しきい値算出部１７は、平均値や標準偏差を用いないで、差分値の過去の最大値が大きいほど大きな値となるしきい値を算出してもよい。このしきい値の算出も、ステップごとに行われることが好適である。また、しきい値の算出に平均値や標準偏差を用いない場合には、制御装置１は、更新部１６を備えていなくてもよい。

また、本実施の形態では、しきい値算出部１７が、差分値の過去の最大値を用いてしきい値を算出する場合について説明したが、そうでなくてもよい。ステップに対応するしきい値は、使用者や設計者によって設定され、その設定されたしきい値がしきい値記憶部１５に蓄積されてもよい。しきい値が手動によって設定される場合にも、適切な設定が行われることによって、ステップに応じたきめ細かい適切な衝突検知を実現することができる。しきい値が手動で設定されてもよいことは、ステップごとにしきい値が対応付けられている場合であっても、２以上のステップを含むグループに対してしきい値が対応付けられている場合であっても同様である。なお、しきい値の算出が行われない場合には、制御装置１は、しきい値算出部１７を備えていなくてもよい。また、制御装置１がしきい値算出部１７を備えていない場合には、しきい値記憶部１５にしきい値等が記憶される過程は問わない。例えば、記録媒体を介してしきい値等がしきい値記憶部１５で記憶されるようになってもよく、通信回線等を介して送信されたしきい値等がしきい値記憶部１５で記憶されるようになってもよく、あるいは、入力デバイスを介して入力されたしきい値等がしきい値記憶部１５で記憶されるようになってもよい。

また、本実施の形態では、サーボコントローラ２が図２で示される構成である場合について説明したが、そうでなくてもよい。サーボコントローラ２は、図２以外の構成によって、各軸のモータに関するフィードバック制御を行ってもよい。そのフィードバック制御は、ＰＩ制御であってもよく、ＰＩＤ制御であってもよく、その他のフィードバック制御であってもよい。

また、上記実施の形態において、各処理または各機能は、単一の装置または単一のシステムによって集中処理されることによって実現されてもよく、あるいは、複数の装置または複数のシステムによって分散処理されることによって実現されてもよい。

また、上記実施の形態において、各構成要素間で行われる情報の受け渡しは、例えば、その情報の受け渡しを行う２個の構成要素が物理的に異なるものである場合には、一方の構成要素による情報の出力と、他方の構成要素による情報の受け付けとによって行われてもよく、あるいは、その情報の受け渡しを行う２個の構成要素が物理的に同じものである場合には、一方の構成要素に対応する処理のフェーズから、他方の構成要素に対応する処理のフェーズに移ることによって行われてもよい。

また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いるしきい値や数式、アドレス、設定値、パラメータ等の情報等は、上記説明で明記していない場合であっても、図示しない記録媒体において、一時的に、あるいは長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、あるいは、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、あるいは、図示しない読み出し部が行ってもよい。

また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いるしきい値やアドレス、各種の設定値、パラメータ等の情報がユーザによって変更されてもよい場合には、上記説明で明記していない場合であっても、ユーザが適宜、それらの情報を変更できるようにしてもよく、あるいは、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。

また、上記実施の形態において、制御装置１に含まれる２以上の構成要素が通信デバイスや入力デバイス等を有する場合に、２以上の構成要素が物理的に単一のデバイスを有してもよく、あるいは、別々のデバイスを有してもよい。

また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、あるいは、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。なお、上記実施の形態における制御装置１を実現するソフトウェアは、以下のようなプログラムである。つまり、このプログラムは、マニピュレータの作業内容がステップごとに記述された作業プログラムが記憶される作業プログラム記憶部と、作業プログラムのステップと、ステップが実行される期間に用いられるしきい値とが対応付けられて記憶されるしきい値記憶部とにアクセス可能なコンピュータを、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの制御を行う制御装置として機能させるためのプログラムであって、作業プログラム記憶部で記憶されている作業プログラムで記述されている作業内容に対応するステップごとの指令値を、モータを制御するサーボコントローラに出力する作業内容解釈部、作業内容解釈部が出力した指令値を用いて、指令値によって制御されるモータに関する推定値を取得する推定値取得部、推定値に対応するモータに関する現実値を取得する現実値取得部、作業内容解釈部が出力した指令値に応じた動作がマニピュレータにおいて実行されている際に、推定値取得部が取得した推定値と、現実値取得部が取得した現実値との差である差分値が、その動作に応じたステップに対応するしきい値を超えた場合に、衝突が発生したと判断する衝突判断部として機能させるためのプログラムである。

なお、上記プログラムにおいて、上記プログラムが実現する機能には、ハードウェアでしか実現できない機能は含まれない。例えば、情報を取得する取得部や、情報を出力する出力部などにおけるモデムやインターフェースカードなどのハードウェアでしか実現できない機能は、上記プログラムが実現する機能には少なくとも含まれない。

また、このプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体（例えば、ＣＤ−ＲＯＭなどの光ディスクや磁気ディスク、半導体メモリなど）に記録されたプログラムが読み出されることによって実行されてもよい。また、このプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。

また、このプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、あるいは分散処理を行ってもよい。

図５は、上記プログラムを実行して、上記実施の形態による制御装置１を実現するコンピュータシステム９００の構成を示す図である。図５において、コンピュータシステム９００は、ＣＤ−ＲＯＭドライブ９０５を含むコンピュータ９０１と、キーボード９０２と、マウス９０３と、モニタ９０４とを備える。コンピュータ９０１は、ＣＤ−ＲＯＭドライブ９０５に加えて、ＭＰＵ９１１と、ブートアッププログラム等のプログラムを記憶するためのＲＯＭ９１２と、ＭＰＵ９１１に接続され、アプリケーションプログラムの命令を一時的に記憶すると共に、一時記憶空間を提供するＲＡＭ９１３と、アプリケーションプログラム、システムプログラム、及びデータを記憶するハードディスク９１４と、ＭＰＵ９１１、ＲＯＭ９１２等を相互に接続するバス９１５とを備える。なお、コンピュータ９０１は、ＬＡＮやＷＡＮ等への接続を提供する図示しないネットワークカードを含んでいてもよい。また、コンピュータシステム９００は、例えば、キーボード９０２、マウス９０３、及びモニタ９０４に代えてタッチパネルを有してもよい。

コンピュータシステム９００に、上記実施の形態による制御装置１の機能を実行させるプログラムは、例えば、ＣＤ−ＲＯＭ９２１に記憶されて、ＣＤ−ＲＯＭドライブ９０５に挿入され、ハードディスク９１４に転送されてもよい。これに代えて、そのプログラムは、図示しないネットワークを介してコンピュータ９０１に送信され、ハードディスク９１４に記憶されてもよい。プログラムは実行の際にＲＡＭ９１３にロードされる。なお、プログラムは、ＣＤ−ＲＯＭ９２１、またはネットワークから直接、ロードされてもよい。

プログラムは、コンピュータ９０１に、上記実施の形態による制御装置１の機能を実行させるオペレーティングシステム（ＯＳ）、またはサードパーティプログラム等を必ずしも含んでいなくてもよい。プログラムは、制御された態様で適切な機能（モジュール）を呼び出し、所望の結果が得られるようにする命令の部分のみを含んでいてもよい。コンピュータシステム９００がどのように動作するのかについては周知であり、詳細な説明は省略する。

また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。

以上より、本発明による制御装置等によれば、しきい値を細かく設定できるという効果が得られ、衝突の検知を行う制御装置等として有用である。

１制御装置
２サーボコントローラ
３マニピュレータ
１１作業プログラム記憶部
１２作業内容解釈部
１３推定値取得部
１４現実値取得部
１５値記憶部
１６更新部
１７値算出部
１８衝突判断部
１９判断結果出力部
１００産業用ロボット

Claims

モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの制御を行う制御装置であって、
前記マニピュレータの作業内容がステップごとに記述された作業プログラムが記憶される作業プログラム記憶部と、
前記作業プログラム記憶部で記憶されている作業プログラムで記述されている作業内容に対応するステップごとの指令値を、前記モータを制御するサーボコントローラに出力する作業内容解釈部と、
前記作業プログラムのステップと、当該ステップが実行される期間に用いられるしきい値とが対応付けられて記憶されるしきい値記憶部と、
前記作業内容解釈部が出力した指令値を用いて、当該指令値によって制御されるモータに関する推定値を取得する推定値取得部と、
前記推定値に対応するモータに関する現実値を取得する現実値取得部と、
前記作業内容解釈部が出力した指令値に応じた動作が前記マニピュレータにおいて実行されている際に、前記推定値取得部が取得した推定値と、前記現実値取得部が取得した現実値との差である差分値が、当該動作に応じたステップに対応するしきい値を超えた場合に、衝突が発生したと判断する衝突判断部と、を備えた制御装置。
前記しきい値記憶部では、ステップごとにしきい値が対応付けられている、請求項１記載の制御装置。
前記作業プログラムは繰り返して実行され、
各ステップにおいて、差分値の過去の最大値を用いてしきい値を算出し、前記しきい値記憶部に蓄積するしきい値算出部をさらに備えた、請求項２記載の制御装置。
前記作業プログラムが実行されるごとに、各ステップにおける差分値の過去の最大値の平均値及び標準偏差を更新する更新部をさらに備え、
前記しきい値算出部は、前記更新部が更新した各ステップにおける差分値の過去の最大値の平均値及び標準偏差を用いて、ステップごとにしきい値を算出する、請求項３記載の制御装置。
前記しきい値記憶部では、前記作業プログラムにおける２以上のステップを含むグループに対してしきい値が対応付けられており、
前記衝突判断部は、あるグループに属するステップが実行される期間には、当該グループに対応するしきい値を用いて衝突の発生を判断する、請求項１記載の制御装置。
前記推定値は、モータのトルク推定値であり、
前記現実値は、モータの実トルク値である、請求項１から請求項５のいずれか記載の制御装置。
モータによって駆動される関節によって連結された複数のアームを有するマニピュレータの制御方法であって、
前記マニピュレータの作業内容がステップごとに記述された作業プログラムが作業プログラム記憶部に記憶されおり、
前記作業プログラムのステップと、当該ステップが実行される期間に用いられるしきい値とが対応付けられてしきい値記憶部に記憶されており、
前記作業プログラム記憶部で記憶されている作業プログラムで記述されている作業内容に対応するステップごとの指令値を、前記モータを制御するサーボコントローラに出力する作業内容解釈ステップと、
前記作業内容解釈ステップで出力した指令値を用いて、当該指令値によって制御されるモータに関する推定値を取得する推定値取得ステップと、
前記推定値に対応するモータに関する現実値を取得する現実値取得ステップと、
前記作業内容解釈ステップで出力した指令値に応じた動作が前記マニピュレータにおいて実行されている際に、前記推定値取得ステップで取得した推定値と、前記現実値取得ステップで取得した現実値との差である差分値が、当該動作に応じたステップに対応するしきい値を超えた場合に、衝突が発生したと判断する衝突判断ステップと、を備えた制御方法。