JP2010069585A - 衝突検出装置及び方法並びにロボット制御装置 - Google Patents

Abstract

【課題】衝突を精度良く検出できるとともに、誤判定による発生頻度を低減することができるようにした衝突検出装置及び方法並びにロボット制御装置を提供する。
【解決手段】ロボットの衝突検出装置であって、関節部により回転可能に連結された先端側部材に取り付けられ、少なくとも先端側部材の回転面内の加速度を、角方向成分が前記先端側部材を駆動する関節の回転方向と一致する先端側部材に固定される極座標の動径方向成分と角方向成分として検出する多軸加速度センサ１５と、多軸加速度センサ１５の出力値を含む複数種類の判定用パラメータに基づいて衝突を判定する手段２１１と、ロボットの運転状態を取得する手段２３０と、運転状態に基づいて、ロボット２が衝突が生じたと判定するための判定条件を変更する手段２０１，２３５とを有して構成する。
【選択図】図１

Description

本発明は、モータで駆動するアームを有するロボットにおいて、ロボットの衝突を検出する衝突検出装置及び方法並びにロボット制御装置に関する。

産業用ロボットのアーム等が周辺機材等の障害物に意図せずに衝突すると、周辺機材やロボットを損傷してしまうことが考えられる。
そこで、従来、ロボットと障害物との衝突を早期に検出し、衝突が生じた場合にはロボットを自動停止させる技術が提案されている。

ロボットを自動停止させる技術としては、例えば、アームの位置情報、速度情報及び加速度情報、あるいは、アームを駆動させるアクチュエータに供給される電流値等のパラメータを検出し、ロボットの動作状態量の変化を算出することにより衝突を検出している。
一般に衝突の検出は衝突時に発生する動作状態量の変化をあらかじめ定めたしきい値と比較して動作状態量の変化がしきい値を越える場合、衝突が発生したと判定する。このしきい値は最大の動作速度で通常動作するときの動作状態量の変化の最大値より大きい値に設定する必要がある。

ところが、ロボットの動作状態量の変化を算出するパラメータを検出するセンサに電気ノイズが乗ったりギアの噛みこみなどによって発生する機械的な特性（機械的ノイズ特性）により動作状態量が変化すると、実際には衝突が生じていないにもかかわらず、衝突と誤判定してロボットを非常停止させてしまうことがある。
ロボットの種類によっては軽微な衝突であっても性能劣化の原因となる場合がありうるため、ロボットが非常停止した場合にはその都度点検等の対策を施す必要がある。このため、誤判定による非常停止（異常停止）が頻発すると生産ラインの作業効率が低下してしまう。

一方、異常停止を防止するために、通常動作中にこれらのノイズ原因によって発生する動作状態量の変化がしきい値を越えないように大きいマージンを持たせてしきい値を高く設定する場合には、衝突検出の感度が低下するため衝突検出が遅れ、衝突が生じてからロボットを停止させるまでの時間が長くなり、衝突によってロボットや周辺機材が受けるダメージが大きくなってしまう。衝突感度を高くするためには通常動作中の動作状態量の変化の最大値の近くにしきい値を定める必要がある。
このように、衝突感度を上げることと異常停止の発生を減少させることはトレードオフの関係にあり、２つの特性がある程度満足される範囲にしきい値を設定する必要がある。

このような問題を回避するため、特許文献１には、液晶搬送用ロボットのアームの衝突を加速度センサ等を用いて検出するものにおいて、高速送り、低速送り、上下動、アームの旋回等のロボットの動作モードと動作速度に応じて、検出手段の選択の変更としきい値を変更する技術が開示されている。
これによれば、ロボットの動作モードと動作速度に応じて衝突を検出する検出手段としきい値を適切な判定基準に変更して衝突を判定して衝突検出精度を向上し誤判定の頻度を低減させる技術が開示されている。

特許文献２には、サーボモータへ出力されるトルク指令値を所定周期で検出し、前周期で検出されたトルク指令値と今周期で検出されたトルク指令値の差がしきい値（所定値）以上となったとき、サーボモータの回転を停止させるサーボモータにより駆動される被駆動体の衝突検出方法が開示されている。また、特許文献２には、サーボモータへ指令される速度指令値とサーボモータの回転速度を検出する速度検出器からの検出速度との差である速度偏差値を、所定周期で検出し、前周期で検出された速度偏差値と今周期で検出された速度偏差値との差がしきい値（所定値）以上となったとき、サーボモータの回転を停止させるサーボモータにより駆動される被駆動体の衝突検出方法が開示されている。
特許文献３には、産業用ロボットの制御系の遅れ時間に基づいて理論上の位置偏差を実時間で算出し、実際の位置偏差との差、または比が所定の許容範囲内であるか否かをチェックして、差または比が所定のしきい値（許容値）より大きくなる場合に産業用ロボットが障害物と衝突したと判定する産業用ロボットの衝突検出方法が開示されている。
特許文献１：特開２００５−３４２８５８（第６頁 図4）
特許文献２：特開平１−２３０１０７ （第３頁 図１）
特許文献３：特開平０８−２２９８６４（第３頁 図１）

ところで、ロボットが同じ動作（例えば、アームの旋回や伸縮動作）をする場合であっても、加速度センサ等の衝突検出用のセンサの感度（あるいは精度）は、アームの向きや位置等のロボットの姿勢やセンサの取り付け位置に応じて変化する。

しかしながら、特許文献１の技術では、衝突発生時におけるロボットの姿勢やセンサの取り付け位置、センサの感度軸の方向との関係に関しては何ら記載されておらず、ロボットの姿勢によっては加速度センサによる衝突検出が困難になる領域が発生し、その領域では衝突の検出は他の検出手段のみで行うことになる。具体的には「通常送り」と「動作２」の動作モードにおいては加速度センサで検出できない領域では速度による検出のみで衝突検出を行うことになり、その領域でロボットが動作する場合、加速度センサと速度の２つの検出手段を用いて検出するメリットが無くなり、衝突検出の精度を上げるためには速度を用いた衝突検出手段の検出精度を上げることが要求される。

衝突の検出手段の数を増やせば全動作領域と全動作モードを考える場合は衝突検出の可能性は大きくなるが、同時に通常動作中に電気ノイズや機械的ノイズ特性によって異常停止を起こす可能性も大きくなる。
このように特許文献１の技術では異常停止を防止するためには個々の衝突検出手段の検出精度を高める必要があり、単に検出手段の数を増やすだけでは異常停止も頻発することが考えられる。
つまり、特許文献１の技術のようにロボットの動作モードや動作速度に応じて検出手段や衝突判定基準を変更させたとしても、衝突検出精度を上げるとともに異常停止の発生を減少させることには限界がある。

また、特許文献２の方法により衝突検出する場合、速度偏差値の微分値あるいはトルク指令値の微分値によってアーム等の被駆動体が異物に衝突したか否かを検出しているため検出速度は速くなるが、速度偏差値あるいはトルク指令値にノイズが乗るような場合には異常停止が発生しやすいという課題がある。
特許文献３のように位置を用いて衝突検出を行うものは、衝突時に発生するアームのたわみにより衝突を検出しているので衝突の検出精度は上がるが、衝突発生から衝突の検出までに時間がかかり衝突によってアームが受けるダメージが大きくなるという課題がある。

本発明はこのような課題に鑑み創案されたものであり、衝突を精度良く検出できるとともに、誤判定の発生頻度を低減することができるようにした衝突検出装置及び方法並びにロボット制御装置を提供することを目的とする。

上記問題を解決するため、本発明は、次のように構成した。
本願発明（請求項１）かかる衝突検出装置は、アクチュエータにより駆動する関節部を備えたアームを有するロボットの衝突を検出するロボットの衝突検出装置であって、前記アームの先端側の構造体であり、前記関節部により回転可能に連結された先端側部材と、前記先端側部材に取り付けられ、少なくとも前記先端側部材の回転面内の加速度を、角方向成分が前記先端側部材を駆動する関節の回転方向と一致する先端側部材に固定される極座標の動径方向成分と角方向成分として検出する多軸加速度センサと、前記多軸加速度センサの前記動径方向成分と前記角方向成分の出力値を含む複数種類の判定用パラメータに基づいて前記ロボットが衝突が生じたと判定する衝突判定手段と、ロボットの運転状態を取得する運転状態取得手段と、前記運転状態取得手段により取得された前記運転状態に基づいて、前記衝突判定手段がロボットが衝突が生じたと判定するための判定条件を変更する判定条件変更手段と、を有していることを特徴としている。

前記判定用パラメータには、前記アクチュエータに対するトルク指令値又はロボット制御装置内部のロボット動作モデルに基づいて計算される外乱トルクの大きさの条件が含まれることが好ましい（請求項２）。
前記判定パラメータには、前記多軸加速度センサにより得られる前記動径方向成分と前記角方向成分とから求められる加速度ベクトルの大きさが含まれることが好ましい（請求項３）。
前記衝突判定手段は、前記判定パラメータに基づいて個別に衝突を検出する衝突検出手段とすべての衝突検出手段の出力に基づいてロボットに衝突が生じたと判定する衝突判定器から構成されることが好ましい（請求項４）。

前記衝突判定器の判定条件は、複数の判定用パラメータの判定結果を積和演算することによって設定され、前記判定条件変更手段は、前記運転状態に応じて、前記積和演算の積項にかかる係数を変更することが好ましい（請求項５）。
前記運転状態取得手段は、前記ロボットの動作を制御するための動作プログラムから前記ロボットの動作モード、姿勢情報、動作速度情報のうちの少なくともいずれかを取得することが好ましい（請求項６）。
前記衝突判定手段は、前記多軸加速度センサからの出力を受けると、予め設定された時間だけ前記出力を保持するとが好ましい（請求項７）。
前記衝突判定器は、前記衝突検出手段の検出ありの出力信号を、予め設定された時間だけ保持することが好ましい（請求項８）。

また、本願発明（請求項９）にかかるロボット制御装置は、ロボットの動作を制御するロボット制御装置であって、請求項１〜８のいずれか１項に記載の衝突検出装置を備えていることを特徴としている。
前記衝突判定手段により衝突が生じたと判定されると、前記ロボットを非常停止させるロボット非常停止手段を有していることが好ましい（請求項１０）。
前記ロボットは、前記アームを水平に旋回動作及び伸縮動作させる少なくとも２以上の前記関節部を有する水平多関節ロボットであることが好ましい（請求項１１）。

また、本願発明（請求項１２）にかかる衝突検出方法は、アクチュエータにより駆動する関節部を備えたアームの先端側の部材に取り付けられた多軸加速度センサにより、前記アームの先端側部材の角方向成分が先端側部材を駆動する関節の回転方向と一致する先端側部材に固定される極座標の動径方向成分と角方向成分として検出し、前記多軸加速度センサの出力値を含む判定用パラメータに基づいて前記ロボットに衝突が生じたことを検出するロボットの衝突検出方法であって、前記ロボットの運転状態を取得する運転状態取得ステップと、前記運転状態取得ステップで取得された前記運転状態に基づいて、ロボットが衝突が生じたと判定するための判定条件を変更することを特徴としている。

本願発明（請求項１，９〜１２）によれば、ロボットに生じた衝突を精度良く検出できるとともに、誤判定の発生頻度を低減することができる。これにより、衝突時に速やかにロボットを非常停止させて衝突時にロボットにかかるダメージを低減することができる。また、誤判定によるロボットの非常停止（異常停止）の発生頻度を減少させることができ、異常停止による生産ラインの作業効率の低下を改善することができる。

本願発明（請求項２）によれば、衝突を精度良く検出してロボットを非常停止できるとともに、誤判定により発生するロボットの異常停止の頻度を低減することができる。
本願発明（請求項３）によれば、加速度ベクトルの大きさを検出手段としているので、ロボットの姿勢（運転状態）が変わっても安定した感度で、衝突を検出することができる。
本願発明（請求項４）によれば、安定した感度で衝突を検出することができる。
本願発明（請求項５）によれば、判定条件に用いる判定用パラメータ値の種類の組み合わせと判定用パラメータ値に対する閾値条件の切替が、論理式の係数を変更するだけで行えるので判定条件変更手段をシンプルに構成でき、計算量を低減することで衝突検出の反応速度を向上することができる。

本願発明（請求項６）によれば、ロボットの動作モード、姿勢、動作速度等の運転条件に従い、センサの組み合わせ、判定条件、判定用パラメータに対する閾値を変更できるので、衝突の検出精度を高めることができる。また、ロボットの動作モード、姿勢、動作速度を確実に取得することができる。
本願発明（請求項７，８）によれば、判定用パラメータの出力パルスの時間やタイミングが異なる場合も精度良く衝突を判定でき、衝突検出の感度を高めることができる。

以下、本発明の実施の形態について図を参照して説明する。
図１〜図８はいずれも本発明の一実施形態を説明するものであり、図１はロボットシステムを説明するためのブロック図、図２はロボット制御装置のフローチャート、図３はロボット制御装置により使用される対応表、図４（ａ）〜図４（ｃ）はいずれもロボット姿勢に応じた衝突検出感度の変化を示す図、図５は検出手段の感度を求めるための図、ロボットの運転状態に応じた衝突に関する検出感度の変化を示す図、図６は検出手段のパルスタイミングを示す図、図７はロボットシステムの概略構成図、図８はロボット制御装置の概略的な機能構成を示すブロック図である。

図７に示すようにロボットシステム１は、水平多関節型のロボット２及びロボット制御装置２０により構成されている。そして、ロボット２は、基台３，第１アーム４，第２アーム（先端側部材）５，フォーク６，Ｓ軸モータ（関節部，アクチュエータ）７、L軸モータ（関節部，アクチュエータ）８及び加速度センサ１５を備えている。加速度センサ１５は第２アーム５の先端付近に取り付けられており、第２アーム５の回転面内の加速度を、角方向成分がＬ軸モータ８の回転方向と一致する先端側部材に固定される極座標の動径方向成分（Ｘ軸加速度）と角方向成分（Ｙ軸加速度）として検出する２軸加速度センサとして構成されている。
換言すると、加速度センサ１５は、第２アーム５がＬ軸モータ８の駆動より旋回される際の遠心方向の加速度成分をＸ軸加速度とし、第２アーム５の旋回方向の加速度成分をＹ軸加速度として検出するように構成されている。

基台３は、円柱状に形成されており上下方向に昇降可能に構成されている。第１アーム４と第２アーム５はそれぞれアームを構成する構造体であり、第１アーム４が基端側（基台３側）、第２アーム５が先端側（フォーク６側）に設けられている。
第１アーム４は、水平面内で旋回可能に基台３に取り付けられている。また、第１アーム４の先端側にはＬ軸モータ８によって回転可能に連結され、水平に旋回可能（旋回面が水平面）に第２アーム５が取り付けられている。
第２アーム５の先端側には、フォーク６が水平面内で旋回自在に取り付けられており、フォーク６は搬送するワークＷが載置可能に形成されている。

第２アーム５およびフォーク６は、L軸モータ８の駆動によって予め定められた速度比（回転比）を保って旋回するようになっており、これによりフォーク６が基台３の半径方向に直線的に伸縮移動できるように構成されている。ロボット２はロボット制御装置２０によって動作が制御されるようになっている。
ロボット制御装置２０は、図８に示すようにロボット用多軸制御演算装置２０１とロボット用多軸アンプ２０２とにより構成される。
ロボット制御装置２０は例えばロボット用多軸制御演算装置２０１としてパーソナルコンピュータを用い、ロボット用多軸アンプ２０２はトルク制御される複数のモータ制御器を用いて構成される。
この場合、ロボット用多軸制御演算装置２０の主要機能であるモーション部とサーボ部は、パーソナルコンピュータ内部で実行されるソフトウェアによって構成され、実行周期の異なる２つのタスクとなっている。

ロボット動作プログラムはあらかじめユーザによって作成され、ロボット用多軸制御演算装置２０１上の不揮発性メモリ内部に記憶されている。ロボットを動作させるときは、ロボット動作プログラムを読み出しモーション部で解読して各軸モータの目標位置を計算する。サーボ部では、モーション部で計算された目標位置とモータに取り付けられたエンコーダから得られる各軸モータ位置を用いて位置速度制御ループを構成して各軸モータのトルク指令を計算している。

ロボット多軸アンプ２０２では、サーボ部で計算された各軸モータのトルク指令に相当する電流を各軸モータに供給してモータ位置とモータ速度を制御して動作プログラムが要求するロボットの姿勢と動作を得ている。
サーボ部には各軸のモータの状態を監視してサーボ異常を検出する機能がある。モーション部ではサーボ部からの異常検出信号に基づいて各軸の速度指令を零とするか、ロボット用多軸アンプ２０２に停止指令を与えてモータ内部に取り付けられたブレーキを動作させるかモータの入力を短絡するダイナミックブレーキによってモータに制動力を与えてロボットを急停止させることができる。

次にロボット制御装置２０の衝突検出部の構成について説明する。図１に示すように衝突検出部はロボット用多軸制御演算装置２０１内のソフトウェアとしてロボット用多軸制御演算装置２０１に実装される。
衝突検出部は次の主要構成要素からなる。２１１は衝突判定器である。２３５はロボット２の衝突検出のための判定条件を変更（あるいは設定）するための対応表である。対応表２３５について詳細は後述する。２１２−１〜２１２−２は複数の判定用パラメータを監視するための衝突検出手段である。ロボット用多軸制御演算装置２０１及び対応表２３５は判定条件変更手段に相当する。

衝突検出手段は、Ｓ軸モータ７に対するトルク指令をモニタ（監視）する第１の衝突検出手段２１２−１、L軸モータ８に対するトルク指令をモニタする第２の衝突検出手段２１２−２、Ｘ軸加速度をモニタする第３の衝突検出手段２１２−３、Ｙ軸加速度をモニタする第４の衝突検出手段２１２−４としている。２２０はロボット停止手段である。２３０は数値演算装置（運転状態取得手段）である。
本実施形態では、判定用パラメータとして、加速度センサ１５で検出されるＸ軸加速度，Ｙ軸加速度，Ｓ軸モータ７へのトルク指令値，Ｌ軸モータ８へのトルク指令値がそれぞれ判定用パラメータとして用いられる。

また、位置速度制御ループ２４０，２４１にはロボット停止手段２２０からの停止信号を受けると速度指令を零にする機能がある。
衝突判定器２１１と衝突検出手段２１２−１〜２１２−４は、サーボ部内部に構成される。対応表２３５、数値演算装置２３０、ロボット停止手段２２０は、モーション部内部に構成されている。

衝突検出手段２１２−１〜２１２−４はそれぞれモニタしている各信号にフィルタ処理とゲイン倍処理を行い、信号が、あらかじめそれぞれに設定されたしきい値を超える場合、各衝突検出手段２１２−１〜２１２−４の出力にオン信号を出力する。衝突検出手段２１２−１〜２１２−４は周期時間が短いサーボ部ループで行われることが望ましい。
動作プログラムが読み込まれると数値演算装置２３０でロボットの動作モードとロボットの姿勢（姿勢情報）が計算されて出力される。

対応表２３５はロボットの運転状態（ここでは、動作モードと姿勢）に応じて衝突検出手段の組と検出手段の出力の組み合わせに応じて衝突判定を行う判定条件を出力する。
衝突判定器２１１は、各衝突検出手段の出力とから衝突検出手段の組と判定条件に応じて衝突判定結果を出力する。各衝突検出手段２１２−１〜２１２−４の出力がすべて一致すれば衝突判定の必要はないが、出力が一致しない場合は衝突発生の判定が必要になる。
対応表２３５に対応させる（動作モード、姿勢）は、ロボット２の動作プログラムから取得される情報に応じてその都度変化し、対応表２３５から読み出され動作モードと姿勢によって変化する衝突検出手段の組と判定条件に基づいて衝突判定器２１１は衝突判定結果を出力するようになっている。

ところで、ロボット２が障害物に衝突した場合、各モータ（Ｓ軸モータ７及びＬ軸モータ８）には大きなトルクが発生する。このため衝突時の各モータに対するトルク指令は通常動作時のトルク指令と比較して十分に大きい。
衝突検出手段２１２−１、２１２−２は通常動作時のトルク指令と衝突動作時のトルク指令を比較して衝突を検出する。比較に用いるしきい値は通常動作時のトルク指令の最大値に予め設定したマージンを持たせた値に設定されている。衝突検出手段２１２−１、２１２−２はそれぞれのトルク指令値がしきい値を超えると衝突検出信号を出力するようになっている。

加速度センサ１５の出力を監視する衝突検出手段２１２−３，２１２−４は、通常動作の加速度が検出されるため、それぞれしきい値の設定が高くなり、衝突の検出精度が悪化する問題がある。ここでは、各モータに対する位置指令から計算される通常動作時の加速度を補償して、しきい値が低くなるように衝突検出手段を構成した。

ここで、対応表２３５について、より詳細に説明する。衝突の判定を４つの衝突検出手段の出力結果の組み合わせに従って行う衝突判定器２１１を持ち、判定の条件が動作プログラムから得られるロボット２の動作モードの情報と、各軸モータの位置指令から得られるロボットの姿勢情報とからなるロボット２の運転状態に応じて変更される構成となっている。判定条件は動作モード、姿勢情報の組と１対１に対応付けられている。

対応表２３５としては、論理式を与えるための対応表としておき、ロボット２の動作モード及び姿勢が変更されると記憶された対応表から運転状態に応じた論理式を衝突判定器２１１上に読み出して衝突判定器２１１が論理式に従い衝突検出手段の出力結果から衝突発生の有無を計算する構成としてもよい。
そして、衝突判定器２１１は衝突発生を判定する計算結果が得られるとロボット停止手段２２０に衝突の発生を知らせる信号を出力し、ロボット停止手段２２０がロボット２を停止させるように構成される。

対応表２３５の作成について説明する。図５と下記に示す１〜５の特性に従って得られる以下に示す式セット（１）と式セット（２）に基づいて検出手段の検出感度特性を求め図３に示す対応表２３５を作成した。

１．伸縮動作の場合は外乱力が基台３の中心に向かう方向に加わる。
２．旋回動作の場合は、外乱力がアーム先端で衝突する場合は動作の接線方向に加わる。
３．２つのアームがなす角度２θでロボットの姿勢が決まる。
４．アーム長は第１アーム、第２アーム共にｄである。
５．定速動作をしているアームが衝突により加速度Ａと力Ｆを受けて停止する。
２軸加速度（Ｘ軸、Ｙ軸）と２軸モータトルク（Ｓ軸、Ｌ軸）はロボットの姿勢角度θに対して次のように計算できる。
（１）伸縮動作時（先端衝突）
Ｘ軸＝Ａ×ｓｉｎ（θ） ・・・・（１）
Ｙ軸＝Ａ×ｃｏｓ（θ）
Ｌ軸＝ｄ×Ｆ×ｃｏｓ（θ）
Ｓ軸＝ｄ×Ｆ×ｃｏｓ（θ）
（２）旋回動作時（先端衝突）
Ｘ軸＝Ａ×ｃｏｓ（θ） ・・・・（２）
Ｙ軸＝Ａ×ｓｉｎ（θ）
Ｌ軸＝ｄ×Ｆ×ｓｉｎ（θ）
Ｓ軸＝ｄ×Ｆ×ｓｉｎ（θ）

図５には姿勢角度θに対する４つの検出手段で測定される信号の大きさを示す。
ロボットの動作モードとロボットの姿勢によって衝突検出の感度が変化する。
姿勢角度θに対して検出信号が０となる検出手段は、その姿勢の近くでは検出手段に衝突に伴い発生する信号が得られないため衝突検出感度が低くなる。ロボットの動作モードとロボットの姿勢に対して衝突検出感度が高くなる検出手段を選択し、それらを組み合わせる論理式によって図３に示す対応表２３５を作成した。

ロボット停止手段２２０はサーボＯＮ（オン）の状態でロボットを停止させる必要がある場合は位置速度制御ループの速度指令を零としてロボットを停止させる。この場合は重力によりアームが落下する問題があるため重力方向に動くアームに関してはトルク指令に補償を行う必要がある。ロボットを急停止させる必要がありサーボＯＦＦ（オフ）としてよい場合は、ロボット用多軸アンプ１０３にサーボＯＦＦの指令を出した後ブレーキ４ｄ−１、４ｄ−２を動作させる指令を出すかモータの入力を短絡する指令を出してモータに制動をかけてロボットを停止させる。この場合は、ロボットの位置はブレーキによって保持されるため停止後の措置は必要ない。

本発明の動作を示すフローチャートを図２に示す。衝突判定式の変更プログラムはモーション部に組み込まれ、衝突判定の計算プログラムはサーボ部に組み込まれているとする。
ロボット動作プログラムに従いロボットが動作する場合、ロボットモーション部で動作プログラムが解読され（図２のＭ１ステップ）、動作モードが検出される（Ｍ２ステップ）。
ロボットのダイナミクスに従い各軸モータの位置指令が計算される（Ｍ３ステップ）。位置指令からロボットの姿勢が計算される（Ｍ４ステップ）。（動作モード、姿勢）の組が与えられると対応表２３５によって（衝突検出手段の組、論理式）の組が選択され、衝突判定器２１１に組み込まれて衝突判定の条件が変更される（Ｍ５ステップ）。各軸モータの位置指令がサーボ部に出力される（Ｍ６ステップ）。

衝突の判定計算プログラムはサーボ部に組み込まれ次の手順に従い処理される。各衝突検出手段の出力が読み込まれる（Ｓ１ステップ）。衝突判定式が計算される（Ｓ２ステップ）。衝突と判定されるとＳ４以下の衝突に対する処理が行われる。サーボＯＮを持続する場合は各軸モータの速度指令を零にしてロボットを停止し（Ｓ６ステップ）、サーボＯＦＦして緊急停止する場合はブレーキ処理を行いロボットを停止させる（Ｓ５ステップ）。
衝突が発生していない場合は各軸モータのトルク指令が計算されて出力され（Ｓ７ステップ）、ロボットの動作が継続される。

本発明の一実施形態にかかる衝突検出装置及びロボットシステムはこのように構成されており、以下に本実施形態の作用効果について説明する。
本発明は旋回方向の加速度、遠心方向の加速度、各モータに対するトルク指令による衝突検出手段の検出感度がロボット２の姿勢に応じて変化することに着目したものである。
図４に示すように、ロボット２が一定速度で伸縮動作を行い姿勢が図４（ａ）から図４（ｂ）、図４（ｃ）へと順に変化する場合について考える。

障害物が動作方向にあり、同じ方向から大きさが等しい外乱力が働く衝突の場合でもＳ軸モータが受ける外乱トルクは図４（ａ）の場合が最も大きくなり、図４（ｂ）、図４（ｃ）の順で低下する。図４（ｃ）の場合外乱力が加わる方向はＳ軸モータのトルクを変化させずモータの中心に向かう方向となる。Ｓ軸モータのトルクの変化で衝突を検出する場合、図４（ｃ）の姿勢では検出感度が低下する。

アーム先端の２軸加速度センサの場合、図の方向の衝突により図４（ａ）では、Ｙ軸加速度センサ方向の動作が影響され、Ｙ軸方向の加速度の測定により感度良く衝突検出ができる。図４（ｃ）の姿勢で衝突が発生するとＸ軸加速度センサ方向の動作が影響され、Ｘ軸方向の加速度の測定により感度良く衝突検出ができるのである。

本実施形態では対応表に論理式を記憶させておきロボットの運転状態に応じて論理式を変更して判定条件を変更する方法を示した。図３の対応表において論理式の替わりに積和演算の１６個の係数の値を記憶させておき、ロボットの運転状態に応じて係数の値の組み合わせを変更することにより判定条件を変更することができる。
第１の衝突検出手段２１２−１の出力Ｓとその否定¬Ｓ、第２の衝突検出手段２１２−２の出力Ｌとその否定¬Ｌ、第３の衝突検出手段２１２−３の出力Ｘとその否定¬Ｘ、第４の衝突検出手段２１２−４の出力Ｙとその否定¬Ｙとすると衝突判定器２１１で計算する論理式は４つの衝突検出手段の出力またはその否定を用いて計算される１６個の積の項とその係数によって次の積和演算によって計算できる。
Ｕ＝Ａ１∧［Ｓ∧Ｌ∧Ｘ∧Ｙ］∨Ａ２∧［Ｓ∧Ｌ∧Ｘ∧（¬Ｙ）］∨・・・∨Ａ１６∧［（¬Ｓ）∧（¬Ｌ）∧（¬Ｘ）∧（¬Ｙ）］
（なお、Ｕは判定結果、大カッコ［］内は積の項、Ａ１・・・Ａ１６は係数）
この式は４つの衝突検出手段及び出力から計算される積をハードウェアで構成することが容易であり、衝突検出の反応速度を向上することが可能である。
また、本実施形態では動作モードと姿勢の組に対して（衝突検出手段の組、論理式）の組を与える対応表の例を示した。
対応表を（動作モード、姿勢、動作速度）の組から（衝突検出手段の組、論理式、しきい値）の組を与えるように容易に拡張できる。（動作モード、姿勢、動作速度）の組に対して衝突検出のしきい値が変更できるので衝突の検出感度を上げることができる。
本実施形態では図５に示すロボット２の姿勢θから各判定用パラメータの検出信号の大きさを計算して検出感度のよい（衝突検出手段の組、論理式）の組を決定した。
ロボットの動作条件を（動作モード、姿勢、動作速度）が異なるいくつかの組に分けて、各組の動作条件のもとで衝突検出手段の出力を測定して衝突検出手段の選択と論理式及びしきい値を決める対応表を作成するようにしても良い。

本実施形態では第１、第２の衝突検出手段２１２−１、２１２−２としてＳ軸モータ及びＬ軸モータのトルク指令を用いた場合を示した。トルク指令の代わりにロボット制御装置２０内部で各軸モータのトルク指令、エンコーダ位置とロボット動作モデルに基づいて計算されるＳ軸モータ７及びＬ軸モータ８の外乱トルクを用いて衝突検出手段を構成してもよい。即ち、外乱トルクを判定用パラメータとしてもよい。

また、加速度センサ１５出力を成分として合成される加速度ベクトルの大きさを判定用パラメータとしてもよい。これにより、姿勢角度θによって検出感度が変化しない衝突検出手段を構成することができる。
本実施形態ではモーション部で得られる位置指令からロボットの姿勢と動作速度を計算したが、ロボットの姿勢と動作速度を各軸モータのエンコーダ位置から得るように構成してもよい。
各衝突検出手段は、モニタする信号がしきい値を越える場合にパルスを出力する。
出力されるパルスは、衝突検出手段によって出力のタイミングが異なる。

本発明のように複数の検出手段の出力から衝突の発生を判定する場合、パルスが同時に
出力している状態が必要になる。衝突が激しい場合は、各衝突検出手段の出力パルス幅が広がりパルスの発生が重なる状態が現れる。
図６に示すように、検出手段Ｎｏ．１は衝突の初期に短時間検出信号を出力し、検出手段Ｎｏ．２が少し遅れて検出信号を出力し、検出手段Ｎｏ．３がさらに遅れて検出信号を出力する場合を考える。

衝突判定器２１１の入力には最後に入力された検出信号の立ち上がりから衝突検出出力が最も遅い衝突検出手段から決まる検出時間の間、検出信号を保持する機能を持たせる。
複数の衝突検出手段がオーバラップして同時にパルスを出力できない軽度の衝突の場合
でも検出信号を保持する機能を持たせて検出が可能となる。
検出時間を短くすれば、衝突発生から衝突発生判定信号出力の間の時間を短くでき、
衝突発生から短時間でロボットの停止処理を行うことができる。検出時間を衝突発生の判定ができる最短の値に設定することによって衝突検出の感度を上げることができる。
以上、本発明の実施形態について説明したが、本発明は上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲で種々変形して実施することができる。
例えば、実施形態では、多軸加速度センサの一例として、第２アーム５の先端部における加速度の動径方向成分（Ｘ軸加速度）と角方向成分（Ｙ軸加速度）とを検出する２軸加速度センサを適用した例について説明したが、多軸加速度センサは上述のものに限定されるものではなく、３軸の加速度成分を検出するものであっても当然適用可能である。

本発明の一実施形態を説明するものでありロボットシステムを説明するためのブロック図である。 本発明の一実施形態を説明するものでありロボット制御装置のフローチャートである。 本発明の一実施形態を説明するものでありロボット制御装置により使用される対応表である。 本発明の一実施形態を説明するものであり、（ａ）〜（ｃ）はいずれもロボット姿勢に応じた衝突検出感度の変化を示す図である。 本発明の一実施形態を説明するものであり検出手段の感度を求めるための図である。 本発明の一実施形態を説明するものであり検出手段のパルスタイミングを示す図である。 本発明の一実施形態を説明するものでありロボットシステムの概略構成図である。 本発明の一実施形態を説明するものでありロボット制御装置の機能構成を示すブロック図である。

符号の説明

１ ロボットシステム
２ ロボット
３ 基台
４ 第１アーム
５ 第２アーム
６ フォーク
７ S軸モータ
８ L軸モータ
１５ 加速度センサ
２０ ロボット制御装置
W ワーク
２０１ ロボット用多軸制御演算装置（判定条件変更手段）
２０２ ロボット用多軸アンプ
２１１ 衝突判定器（衝突判定手段）
２１２−１〜２１２−４ 衝突検出手段
２２０ ロボット停止手段
２３０ 数値演算装置（運転状態取得手段）
２３５ 対応表（判定条件変更手段）
４ａ−１，４ａ−２ エンコーダ
４ｂ−１，４ｂ−２ トルク指令
４ｄ−１，４ｄ−２ ブレーキ
２４０，２４１ 位置速度制御ループ

Claims (12)

1. アクチュエータにより駆動する関節部を備えたアームを有するロボットの衝突を検出するロボットの衝突検出装置であって、
   前記アームの先端側の構造体であり、前記関節部により回転可能に連結された先端側部材と、
   前記先端側部材に取り付けられ、少なくとも前記先端側部材の回転面内の加速度を、角方向成分が前記先端側部材を駆動する関節の回転方向と一致する先端側部材に固定される極座標の動径方向成分と角方向成分として検出する多軸加速度センサと、
   前記多軸加速度センサの前記動径方向成分と前記角方向成分の出力値を含む複数種類の判定用パラメータに基づいて前記ロボットが衝突が生じたと判定する衝突判定手段と、
   ロボットの運転状態を取得する運転状態取得手段と、
   前記運転状態取得手段により取得された前記運転状態に基づいて、前記衝突判定手段がロボットが衝突が生じたと判定するための判定条件を変更する判定条件変更手段と、を有している
   ことを特徴とする、衝突検出装置。
2. 前記判定用パラメータには、
   前記アクチュエータに対するトルク指令値又はロボット制御装置内部のロボット動作モデルに基づいて計算される外乱トルクの大きさの条件が含まれる
   ことを特徴とする、請求項１記載の衝突検出装置。
3. 前記判定パラメータには、
   前記多軸加速度センサにより得られる前記動径方向成分と前記角方向成分とから求められる加速度ベクトルの大きさが含まれる
   ことを特徴とする、請求項１又は２記載の衝突検出装置。
4. 前記衝突判定手段は、前記判定パラメータに基づいて個別に衝突を検出する衝突検出手段とすべての衝突検出手段の出力に基づいてロボットに衝突が生じたと判定する衝突判定器から構成される
   ことを特徴とする、請求項１〜３のいずれか１項に記載の衝突検出装置。
5. 前記衝突判定器の判定条件は、複数の判定用パラメータの判定結果を積和演算することによって設定され、
   前記判定条件変更手段は、前記運転状態に応じて、前記積和演算の積項にかかる係数を変更する
   ことを特徴とする、請求項４記載の衝突検出装置。
6. 前記運転状態取得手段は、
   前記ロボットの動作を制御するための動作プログラムから前記ロボットの動作モード、姿勢情報、動作速度情報のうちの少なくともいずれかを取得する
   ことを特徴とする、請求項１〜５のいずれか１項に記載の衝突検出装置。
7. 前記衝突判定手段は、前記多軸加速度センサからの出力を受けると、予め設定された時間だけ前記出力を保持する
   ことを特徴とする、請求項１〜６のいずれか１項に記載の衝突検出装置。
8. 前記衝突判定器は、前記衝突検出手段の検出ありの出力信号を、予め設定された時間だけ保持する
   ことを特徴とする、請求項４〜６のいずれか１項に記載の衝突検出装置。
9. ロボットの動作を制御するロボット制御装置であって、
   請求項１〜８のいずれか１項に記載の衝突検出装置を備えている
   ことを特徴とする、ロボット制御装置。
10. 前記衝突判定手段により衝突が生じたと判定されると、前記ロボットを非常停止させるロボット非常停止手段を有している
    ことを特徴とする、請求項９記載のロボット制御装置。
11. 前記ロボットは、前記アームを水平に旋回動作及び伸縮動作させる少なくとも２以上の前記関節部を有する水平多関節ロボットである
    ことを特徴とする、請求項９又は１０記載のロボット制御装置。
12. アクチュエータにより駆動する関節部を備えたアームの先端側の部材に取り付けられた多軸加速度センサにより、前記アームの先端側部材の角方向成分が先端側部材を駆動する関節の回転方向と一致する先端側部材に固定される極座標の動径方向成分と角方向成分として検出し、前記多軸加速度センサの出力値を含む判定用パラメータに基づいて前記ロボットに衝突が生じたことを検出するロボットの衝突検出方法であって、
    前記ロボットの運転状態を取得する運転状態取得ステップと、
    前記運転状態取得ステップで取得された前記運転状態に基づいて、ロボットが衝突が生じたと判定するための判定条件を変更する
    ことを特徴とする、衝突検出方法。

Images