JPH11245191A - 産業用ロボットの駆動軸制御方法及びその装置 - Google Patents

Abstract

(57)【要約】 【課題】 関節部を駆動する駆動軸モータが減速機を介
してアームと連結される構造を有する産業用ロボットに
おいて、アームあるいはエンドエフェクタが障害物と衝
突した際に、衝突したアームあるいはエンドエフェクタ
及び減速機等の駆動系にかかる負荷を最小限に抑えるこ
とができるような産業用ロボットの駆動軸制御方法及び
その装置を提供する。 【解決手段】 駆動軸モータの時々刻々の現在位置を検
出し、これを位置データ群として記憶し、アームあるい
はアームに把持されたエンドエフェクタが障害物と衝突
したことを検出した際には、駆動軸モータを駆動させる
ための指令位置を記憶されている前記位置データ群に基
づいて変更することにより駆動軸モータに逆転トルクを
発生させ、この逆転トルクの発生によりアームを衝突直
前の位置にまで回避させる。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】産業用ロボットを構成する各
アームを駆動する駆動軸モータの制御に関し、特に、ア
ームあるいはエンドエフェクタが障害物などと衝突した
場合に、駆動軸が受ける衝突の影響を最小限に抑えるこ
とが可能な駆動軸制御方法及びその装置に関する。

【０００２】

【従来の技術】産業用ロボットを構成するアーム自身あ
るいはこれに把持されたエンドエフェクタが障害物に衝
突した場合、各アームを駆動する駆動軸モータは予め設
定された移動指令に従ってなおも移動し続けようとし、
この結果駆動軸モータは拘束状態となり、大きなトルク
を発生し続けることになる。この状態が長く続くと駆動
軸モータや減速機を含むアームの機構部が破損する可能
性が生ずるので、従来より、何らかの手法により衝突の
発生を検出し、駆動軸モータの移動指令を即時に中断す
る等の処置を行わせていた。

【０００３】例えば、特開平６−１３１０５０号に開示
されている方法では、外乱推定オブザーバによって摩擦
トルクを考慮した外乱トルクを推定し、この推定外乱ト
ルクが規定値以上になったとき、負荷異常として衝突等
が生じているものと判断するようにしていた。この方法
は、衝突検知用の特別な検出器を使用することなく、ソ
フトウェア上での処理により衝突の発生を検出し、駆動
軸モータへの供給動力を遮断し、これによりアームを即
座に緊急停止させることができるというものである。

【０００４】

【発明が解決しようとする課題】しかし、ソフトウェア
上での処理においては、衝突の発生を検知してからアー
ムが完全停止するまでにはある程度のタイムラグが生ず
るために、アーム自身あるいはこれに把持されたエンド
エフェクタが障害物に接触した位置で静止している一方
で、駆動軸モータは僅かに障害物にめり込んだ位置まで
到達した状態となっている。よって、衝突検出直後に駆
動軸モータへの供給動力を直ちに遮断しても、制御装置
に対する手動操作などにより障害物と接触しない位置ま
でアームが移動されない限り、駆動軸モータに連結され
ている減速機には過大な力がかかり続けていることにな
る。すなわち、衝突の発生を検出した時点で即座に駆動
軸モータへの供給動力を遮断しても、アーム自身あるい
はエンドエフェクタが障害物にめり込んだ状態のままで
停止し続けていると、復元力により減速機には過大な力
がかかり続けていることになる。

【０００５】本発明は上記の課題を解決するためになさ
れたものであり、アームあるいはエンドエフェクタが障
害物と衝突した際に、衝突したアームあるいはエンドエ
フェクタ及び減速機等の駆動系にかかる負荷を最小限に
抑えることができるような産業用ロボットの駆動軸制御
方法及びその装置を提供することを目的とする。

【０００６】

【課題を解決するための手段】上記の目的を達成するた
めに、請求項１にかかる発明では、関節部を駆動する駆
動軸モータが減速機を介してアームと連結される構造を
有する産業用ロボットにおいて、駆動軸モータの時々刻
々の現在位置を検出し、これを位置データ群として記憶
し、アームあるいはアームに把持されたエンドエフェク
タが障害物と衝突したことを検出した際には、駆動軸モ
ータを駆動させるための指令位置を記憶されている前記
位置データ群に基づいて変更することにより駆動軸モー
タに逆転トルクを発生させ、この逆転トルクの発生によ
りアームを衝突直前の位置にまで回避させるようにした
ことを特徴とする産業用ロボットの駆動軸制御方法を提
供した。

【０００７】また、本発明では、上記請求項１にかかる
産業用ロボットの駆動軸制御方法を実施するための装置
をも提供する。すなわち、請求項４にかかる発明では、
関節部を駆動する駆動軸モータが減速機を介してアーム
と連結される構造を有する産業用ロボットにおいて、駆
動軸モータの指令位置を出力する指令位置発生装置と、
駆動軸モータの現在位置を検出する位置検出器と、検出
された時々刻々の現在位置を位置データ群として記憶す
る現在位置記憶バッファと、アームあるいはアームに把
持されたエンドエフェクタが障害物と衝突したことを検
出する衝突検出装置と、この衝突検出装置において衝突
の発生が検出された際に、現在位置記憶バッファに記憶
されている位置データ群に基づいて前記指令位置を変更
する指令位置変更装置と、を有することを特徴とする産
業用ロボットの駆動軸制御装置を提供した。

【０００８】請求項１及び４について補足説明を行う。
図１は請求項４にかかる本発明の産業用ロボットの駆動
軸制御装置の一実施形態を示すブロック図である。この
ブロック図に示される回路は予め設定されたスキャンタ
イム毎に各種信号が送信されるようになっている。その
ため、位置検出器としてのエンコーダ２においては設定
されたスキャンタイム毎に駆動軸モータとしてのサーボ
モータ１の位置が検出されることになるので、現在位置
記憶バッファ１８にはスキャンタイム毎のサーボモータ
１の位置すなわち位置データ群が記憶されることにな
る。衝突検出の方法については、従来技術、例えば前述
の特開平６−１３１０５０号に開示されているように、
外乱推定オブザーバによって外乱トルクを推定し、この
推定された外乱トルクに基づいて算出された外乱トルク
の衝突成分が規定値以上になったときに衝突が生じてい
るものと判断するようにすればよい。

【０００９】請求項１及び４の作用について説明する。
衝突検出装置７においてアームあるいはアームに把持さ
れたエンドエフェクタが障害物と衝突したことを検出し
た場合は、駆動軸モータを駆動させるための指令位置を
現在位置記憶バッファ１８に記憶されている位置データ
群すなわちスキャンタイム毎のサーボモータ１の位置に
基づいて変更することにより、駆動軸モータに逆転トル
クを発生させる。詳述すると、予め設定されたロボット
プログラムが実行されると、アームを動作させる駆動軸
モータを駆動させるための指令位置が指令位置発生装置
１０から出力され、その結果アームが所定の方向に動作
することになる。かかる状況において万一アームが障害
物と衝突した場合には、衝突直前の動作方向とは逆向き
にアームを瞬時に動作させることにより、衝突状態を回
避させることができる。しかし、衝突直前の動作方向を
一義的に決定することは容易ではない。

【００１０】そこで、本発明では、アームの実際の動作
軌跡を位置検出器により検出された位置データ群として
現在位置記憶バッファ１８に記憶させ、衝突時には位置
データ群の中から抽出した位置データを回避位置への指
令位置とすることにより、アームを衝突直前の位置にま
で瞬時に移動させる。また、指令位置を位置データ群の
中から抽出した回避位置に変更した時点で駆動軸モータ
には逆転トルクが発生するので、従来のように駆動軸モ
ータが拘束状態になることによって衝突したアームある
いはエンドエフェクタ及び減速機等の駆動系に負荷がか
かる続けることはない。なお、現在位置記憶バッファ１
８には予め設定された個数の位置データが格納可能にさ
れているので、アームが動作するにしたがい新しい位置
データが蓄積されるとともに古い位置データから順に消
去されることになる。

【００１１】ところで、衝突時にアームを衝突直前のど
の位置まで移動させるか、言い換えればアームの戻り量
を幾らにするかは、位置データ群の中からの回避位置の
抽出方法による。最も単純な抽出方法としては、位置デ
ータ群の中から衝突時を起点として予め設定されたデー
タ点数前の位置データを抽出し、これを回避位置として
選定する方法がある。しかし、この方法の場合、衝突時
のアームの動作速度によってアームの戻り量が異なって
くるという問題がある。すなわち、衝突時のアームの動
作速度が速いほどアームの戻り量は大きくなり、逆に、
アームの動作速度が遅いほどアームの戻り量は小さくな
ってしまうため、この場合衝突状態から解放されない状
況となることも起こりうる。

【００１２】そこで、請求項２にかかる発明では、衝突
の発生が検出された際には、衝突時の駆動軸モータの速
度に基づいて、記憶されている位置データ群の中から指
令位置に置き換えるための駆動軸モータの位置すなわち
回避位置を選定するようにし、また、この請求項２にか
かる方法を実施するために、指令位置変更装置では、衝
突検出装置において衝突の発生が検出された際に、衝突
時の駆動軸モータの速度に基づいて、現在位置記憶バッ
ファに記憶されている位置データ群の中から指令位置に
置き換えるための駆動軸モータの位置すなわち回避位置
を選定するようにしたことを特徴とする産業用ロボット
の駆動軸制御装置とした（請求項５）。これにより、例
えば、衝突時のアームの戻り量を２００ｍｍとして予め
設定していたならば、衝突時の駆動軸モータの速度が２
０００ｍｍ／秒の場合は位置データ群の中から衝突時よ
りも０．１秒前の駆動軸モータの位置を回避位置として
選定し、また、衝突時の駆動軸モータの速度が１０００
ｍｍ／秒の場合は位置データ群の中から衝突時よりも
０．２秒前の駆動軸モータの位置を回避位置として選定
するようにすればよい。

【００１３】さらに、請求項３及び６にかかる発明で
は、衝突時の駆動軸モータの速度の算出について規定す
る。すなわち、請求項３にかかる発明では、衝突時の駆
動軸モータの速度は記憶されている位置データ群に基づ
いて算出するようにし、また、この請求項３にかかる方
法を実施するために、指令位置変更装置では、衝突時の
駆動軸モータの速度は現在位置記憶バッファに記憶され
ている位置データ群に基づいて算出するようにしたこと
を特徴とする産業用ロボットの駆動軸制御装置とした
（請求項６）。現在位置記憶バッファに記憶されている
位置データ群はスキャンタイム毎すなわち一定時間毎に
得られた駆動軸モータの位置であるから、時々刻々の速
度は位置データ群に基づいて容易に算出できる。

【００１４】

【発明の実施の形態】以下、本発明の一実施形態につい
て図面を参照して説明する。図１は本発明の一実施形態
における駆動軸制御装置を構成するサーボ系のブロック
図である。図中、１は産業用ロボットの各アームの駆動
軸を駆動するための駆動軸モータとしてのサーボモー
タ、２は駆動軸の位置を検出するための位置検出器とし
てのエンコーダ、３は増幅器としてのパワーアンプであ
る。サーボループは内側から電流制御装置４、速度制御
装置５、位置制御装置６という３重のループで構成され
ている。１０は目標位置・出発位置・要求速度・加速度
の制限値などを基に産業用ロボットが適正に動作するた
めに求められた時々刻々の各駆動軸の指令位置を出力す
る指令位置発生装置である。７はアーム２３あるいはア
ーム２３に把持された図示しないエンドエフェクタが障
害物と衝突した際、これを検出する衝突検出装置であ
る。１８はエンコーダ２から読み込まれたサーボモータ
１の時々刻々の現在位置を記憶するとともに、衝突検出
時には回避位置としての衝突直前のサーボモータ１の位
置を出力する現在位置記憶バッファである。９は衝突検
出時に指令位置を変更する指令位置変更装置である。一
方、図２はサーボモータ１、回転減速機２２、及びアー
ム２３の関係を示す、ばね−質量系の概念図である。

【００１５】このサーボ系の機能について説明すると、
位置制御装置６は、指令位置発生装置１０が発生した動
作プログラム等に従ったサーボモータ１の時々刻々の指
令位置と、サーボモータ１に取り付けられたエンコーダ
２から読み込まれた現在位置としての位置フィードバッ
ク１１との差分をとって位置偏差１４とし、この位置偏
差１４に位置ループゲインを乗じて求められた速度指令
１５を出力するようにされている。速度制御装置５は、
位置制御装置６から出力された速度指令１５と、エンコ
ーダ２から読み込まれた位置フィードバック１１を微分
器ｓにて微分することにより求められた速度フィードバ
ック１２との差分をとって速度偏差とし、この速度偏差
に基づいて電流指令１６を出力するようにされている。
電流制御装置４は、速度制御装置５から出力された電流
指令１６と、電流検出器１７により検出されたサーボモ
ータ１へ流れる実電流としての電流フィードバック１３
との差分をとってサーボモータ１へモータ電流を出力す
るようにされている。

【００１６】衝突検出装置７は、外乱推定オブザーバに
よって外乱トルクを推定し、この推定された外乱トルク
に基づいて算出された外乱トルクの衝突成分が規定値以
上になったとき、衝突が生じているものと判断するよう
にしている。衝突時には、サーボループは通常よりも大
きなトルク（電流指令１６）をサーボモータ１に対して
出力しようとするが、衝突により実際にはサーボモータ
１の位置はほとんど変化しないので速度フィードバック
１２の値はほとんどゼロとなる。したがって、速度制御
装置５から出力された電流指令１６と速度フィードバッ
ク１２の値を監視し、これらの値を基にねじれ量を算出
し、このねじれ量をアーム２３に加わる外乱トルクに換
算し、この換算値から、重力により発生するアンバラン
ストルクや、他の駆動軸の運動により発生する遠心力・
コリオリ力・慣性力等を源とする所謂軸干渉トルクとい
った既知の外乱トルクを差し引けば、外乱トルクの衝突
による増加分が算出できる。この通常はほとんどゼロで
ある外乱トルクの衝突による増加分が予め設定された規
定値を超えたときに、衝突が発生しているものと判断す
るようにする。

【００１７】位置記憶バッファ１８は、通常はエンコー
ダ２から読み込まれたサーボモータ１の時々刻々の位置
を位置データ群として記憶しているが、衝突検出装置７
にて衝突の発生が検出された際には記憶されている位置
データ群に基づいて回避位置を選定し、この回避位置を
指令位置変更装置に対して出力するようにされている。
詳細には、位置記憶バッファ１８は通常はエンコーダ２
から読み込まれたサーボモータ１のスキャンタイム毎の
位置を随時記憶し続けている。位置記憶バッファ１８に
は予め設定された個数の位置データが格納可能にされて
いるので、アームが動作するにしたがい新しい位置デー
タが蓄積されるとともに古い位置データから順に消去さ
れる。

【００１８】位置記憶バッファ１８では衝突検出装置７
から衝突発生信号１９を受信すると、まず、記憶されて
いる位置データ群に基づいて衝突直前のサーボモータ１
の速度を算出する。記憶されている位置データ群はスキ
ャンタイム毎すなわち一定時間毎に得られた駆動軸モー
タの位置であるから、時々刻々の速度はこの位置データ
群に基づいて容易に算出できる。次に、算出されたサー
ボモータ１の衝突直前の速度に基づいて、衝突の何秒前
の位置データを選定するかを求める。例えば、衝突時の
アームの戻り量を２００ｍｍとして予め設定していたな
らば、衝突時の駆動軸モータの速度が２０００ｍｍ／秒
の場合は位置データ群の中から衝突時よりも０．１秒前
（２００ｍｍ／（２０００ｍｍ／秒））の駆動軸モータ
の位置を回避位置として選定し、また、衝突時の駆動軸
モータの速度が１０００ｍｍ／秒の場合は位置データ群
の中から衝突時よりも０．２秒前（２００ｍｍ／（１０
００ｍｍ／秒））の駆動軸モータの位置を回避位置とし
て選定する。

【００１９】次に、位置記憶バッファ１８において、衝
突の何秒前の位置データを選定するかにより、スキャン
タイムに基づいて衝突時を起点として何データ点数前の
位置データを回避位置として選定するを求める。例え
ば、衝突時よりも０．１秒前の駆動軸モータの位置は、
スキャンタイムが５ミリ秒であれば衝突時を起点として
２０データ点数前（０．１秒／５ミリ秒）の位置データ
を回避位置として選定する。最後に、選定された回避位
置を指令位置変更装置９に対して出力する。

【００２０】指令位置変更装置９は、位置記憶バッファ
１８から衝突直前のサーボモータ１の位置として選定さ
れた回避位置が入力されると、この回避位置を衝突時の
指令位置として指令位置発生装置１０へ出力する。指令
位置発生装置１０は、前述のように、通常は動作プログ
ラム等に従いサーボモータ１の時々刻々の指令位置を発
生しているが、衝突検出時には指令位置変更装置９から
入力した回避位置を指令位置として位置制御装置６へ出
力する。この時の新たな指令位置はそれまでの指令位置
に対して逆方向となるため、サーボモータ１には逆転ト
ルクが発生し、サーボモータ１によって動作されるアー
ムの障害物へのくい込みが解消されるとともに、逆転ト
ルクの発生により動作しだしたアームは瞬時に回避位置
まで移動したのち停止することになる。さらに、安全の
ためにサーボモータ１への電力供給を遮断する。

【００２１】なお、上記の実施形態では、回避位置は、
位置記憶バッファ１８に記憶されている位置データ群に
基づいて得られる衝突直前のサーボモータ１の速度より
算出していたが、衝突時を起点として予め設定されたデ
ータ点数前の位置データとしてもよい。ただし、この場
合は、アームの動作速度によって回避位置が異なってく
るという問題がある。すなわち、アームの動作速度が速
い場合は回避位置は衝突位置から大きく離れた位置まで
戻ることになり、一方、アームの動作速度が遅い場合は
回避位置は衝突位置から僅かしか離れていない位置とな
ってしまうため場合によっては衝突状態から解放されな
い状況となるので、アームの最大動作速度や安全な回避
位置などを考慮して、前記データ点数を決定する必要が
ある。

【００２２】また、衝突直前のサーボモータ１の速度
は、上記の実施形態のように位置記憶バッファ１８に記
憶されている位置データ群に基づいて求めるのではな
く、速度フィードバック１２のデータを使用するように
してもよい。

【００２３】以上、本発明の一実施形態について説明し
た。上記の実施形態では、衝突検出は、速度制御装置５
から出力された電流指令１６と速度フィードバック１２
の値とを監視し、外乱推定オブザーバを用いてソフトウ
ェア上で行わせていたが、この代わりに、衝突検出用の
力センサを用いるようにしてもよい。また、上記の実施
形態は産業用ロボットの駆動軸に本発明を適用した場合
について説明したものであるが、関節部を駆動する駆動
軸モータが減速機を介してアームあるいはこれに相当す
る部材を動作させる形態のものであれば、産業用ロボッ
ト以外のものにも本発明の技術は容易に展開可能であ
り、例えば、駆動軸にサーボモータ及び減速機を使用し
ている工作機械にも適用できる。

【００２４】

【発明の効果】本発明の請求項１及び４にかかる発明に
よれば、駆動軸モータの時々刻々の現在位置を検出し、
これを位置データ群として記憶し、アームあるいはアー
ムに把持されたエンドエフェクタが障害物と衝突したこ
とを検出した際には、駆動軸モータを駆動させるための
指令位置を記憶されている前記位置データ群に基づいて
変更することにより駆動軸モータに逆転トルクを発生さ
せ、この逆転トルクの発生によりアームを衝突直前の位
置にまで回避させるようにしたことを特徴とする産業用
ロボットの駆動軸制御方法及びその装置を提供したの
で、アームの実際の動作軌跡が位置データ群として記憶
され、衝突時にはこの位置データ群の中から抽出した位
置データを回避位置への指令位置とされることにより、
アームを衝突直前の位置にまで瞬時に移動させることが
できるものとなった。そのため、指令位置を位置データ
群の中から抽出した回避位置に変更した時点で駆動軸モ
ータには逆転トルクが発生することとなるので、従来の
ように駆動軸モータが拘束状態になることによって衝突
したアームあるいはエンドエフェクタ及び減速機等の駆
動系に負荷がかかる続けることはなくなった。

【００２５】また、本発明の請求項２及び５にかかる発
明によれば、請求項１及び４にかかる発明において、衝
突の発生が検出された際には、衝突時の駆動軸モータの
速度に基づいて、記憶されている位置データ群の中から
指令位置に置き換えるための駆動軸モータの位置を抽出
し、これを回避位置として選定するようにしたので、衝
突時の駆動軸モータの速度の如何にかかわらず、予め設
定された戻り量までアームが衝突位置から回避されるこ
とになる。

【００２６】さらに、本発明の請求項３及び６にかかる
発明によれば、請求項２及び５にかかる発明において、
衝突時の駆動軸モータの速度は記憶されている位置デー
タ群に基づいて算出するようにしたので、記憶されてい
る位置データ群とスキャンタイムとにより、時々刻々の
駆動軸モータの速度が容易に算出できることになる。

【図面の簡単な説明】

【図１】本発明の一実施形態における駆動軸制御装置を
構成するサーボ系のブロック図である。

【図２】サーボモータ１、回転減速機２２、及びアーム
２３の関係を示す、ばね−質量系の概念図である。

【符号の説明】

１ 駆動軸モータ（サーボモータ） ２ 位置検出器（エンコーダ） ３ パワーアンプ ４ 電流制御装置 ５ 速度制御装置 ６ 位置制御装置 ７ 衝突検出装置 ９ 指令位置変更装置 １０ 指令位置発生装置 １１ 位置フィードバック １２ 速度フィードバック １３ 電流フィードバック １４ 位置偏差 １５ 速度指令 １６ 電流指令 １７ 電流検出器 １８ 現在位置記憶バッファ １９ 衝突発生信号 ２２ 減速機 ２３ アーム

Claims (6)

【特許請求の範囲】
1. 【請求項１】関節部を駆動する駆動軸モータが減速機を
   介してアームと連結される構造を有する産業用ロボット
   において、 駆動軸モータの時々刻々の現在位置を検出し、 該検出された駆動軸モータの時々刻々の現在位置を位置
   データ群として記憶し、 アームあるいはアームに把持されたエンドエフェクタが
   障害物と衝突したことを検出した際には、駆動軸モータ
   を駆動させるための指令位置を記憶されている前記位置
   データ群に基づいて変更することにより駆動軸モータに
   逆転トルクを発生させ、 該逆転トルクの発生によりアームを衝突直前の位置にま
   で回避させるようにしたことを特徴とする産業用ロボッ
   トの駆動軸制御方法。
2. 【請求項２】衝突の発生が検出された際には、衝突時の
   駆動軸モータの速度に基づいて、記憶されている前記位
   置データ群の中から前記指令位置に置き換えるための駆
   動軸モータの位置を選定するようにしたことを特徴とす
   る請求項２に記載の産業用ロボットの駆動軸制御方法。
3. 【請求項３】前記衝突時の駆動軸モータの速度は記憶さ
   れている前記位置データ群に基づいて算出するようにし
   たことを特徴とする請求項２に記載の産業用ロボットの
   駆動軸制御方法。
4. 【請求項４】関節部を駆動する駆動軸モータが減速機を
   介してアームと連結される構造を有する産業用ロボット
   において、 駆動軸モータの指令位置を出力する指令位置発生装置
   と、 駆動軸モータの現在位置を検出する位置検出器と、 該位置検出器にて検出された駆動軸モータの時々刻々の
   現在位置を位置データ群として記憶する現在位置記憶バ
   ッファと、 アームあるいはアームに把持されたエンドエフェクタが
   障害物と衝突したことを検出する衝突検出装置と、 該衝突検出装置において衝突の発生が検出された際に、
   前記現在位置記憶バッファに記憶されている位置データ
   群に基づいて前記指令位置を変更する指令位置変更装置
   と、 を有することを特徴とする産業用ロボットの駆動軸制御
   装置。
5. 【請求項５】前記指令位置変更装置では、前記衝突検出
   装置において衝突の発生が検出された際に、衝突時の駆
   動軸モータの速度に基づいて、前記現在位置記憶バッフ
   ァに記憶されている位置データ群の中から前記指令位置
   に置き換えるための駆動軸モータの位置を選定するよう
   にしたことを特徴とする請求項４に記載の産業用ロボッ
   トの駆動軸制御装置。
6. 【請求項６】前記指令位置変更装置では、前記衝突時の
   駆動軸モータの速度は前記現在位置記憶バッファに記憶
   されている位置データ群に基づいて算出するようにした
   ことを特徴とする請求項５に記載の産業用ロボットの駆
   動軸制御装置。