JP4055090B2

JP4055090B2 - Robot control device

Info

Publication number: JP4055090B2
Application number: JP03141498A
Authority: JP
Inventors: 賢一安田; 英夫永田; 康之井上; 洋一田中; 信治奥村
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 1997-07-08
Filing date: 1998-02-13
Publication date: 2008-03-05
Anticipated expiration: 2018-02-13
Also published as: JPH1177580A

Description

【０００１】
【発明の属する技術分野】
本発明はロボットに用いられるモータの制御系において、ロボットアームが周辺物体に衝突した場合のロボットアーム本体やツール、ワークの保護、あるいはロボットアームがオペレータに接触した場合の安全を確保することが可能な制御を実現する制御装置に関する。
【０００２】
【従来の技術】
従来のロボット関節を駆動するモータのサーボ系では、位置フィードバックループと速度フィードバックループを有し、指令との差であるサーボ偏差を増幅し、その増幅ゲインをできるだけ高くとることにより制御を行っている。図１３にロボットの従来の運動制御のブロック図を示す。図において、１０は軌道発生ブロックであり、ティーチペンダントＪＯＧキー１２の入力から、あるいはメモリ１３のデータからモータの運動の指令を生成する。メモリ１３にはあらかじめ教示された位置、速度等のデータ及び補間方法が保存されている。ティーチペンダント１２あるいはメモリ１３のデータをもとに、軌道生成部座標変換部１４で生成された軌道データは座標変換され、各関節のモータの回転指令として運動制御ブロック１１へ送られる。運動制御ブロック１１はモータのサーボ系であり、ロボット関節駆動モータ１５の位置・速度フィードバック信号と軌道生成部座標変換部１４で生成された各モータの運動指令とに基づいて制御信号を出力するサーボ制御系メインループ１６と、制御信号を電力増幅するアンプ１７を備えている。この運動制御ブロック１１は、ロボットの駆動系を構成しているモータの数だけ存在する。
【０００３】
図１４は、各関節で位置制御系を構成する場合の運動制御ブロック１１を更に詳しく示している。ここでは、１軸のサーボ系のみを記載しているが他のモータ軸に関しても同様の構成である。図１４において、Ｋｐは位置制御ゲイン、Ｋｖは速度制御ゲイン、Ｔｉは積分器の時定数である。ここで、従来においては、位置ループと速度ループのゲインＫｐ、Ｋｖは高めにトルク外乱の影響を受けにくくし、積分時定数Ｔｉは短めに設定することにより、位置の指令値に対してモータの追従性を良くしている。このような従来の制御方法では、指令値と現在値の差、すなわち制御偏差によりモータのトルクが発生する。制御偏差は通常、ロボットアームの慣性や制御装置の応答遅れなどサーボの追従遅れによるものと、ロボットアームに作用する重力や減速機の摩擦、その他、ロボットアームに作用する外力などのトルク外乱により発生する。ロボットを希望する位置や方向に動かす場合は、動作指令に対してはロボットは高速に追従することが望ましい。一方、ロボットが運動中、周辺の物体に衝突したり、人間に接触した場合には、それを出来るだけ速く運動を停止することが望ましい。
【０００４】
ところが、従来のロボット制御装置では、指令値応答、外乱応答ともに区別することなく、制御偏差が発生すれば、制御偏差量、もしくは偏差の過去の積算値に比例したトルクをモータにより発生していた。そのため、教示中に誤ってロボットを周辺の物体やワークに衝突させた場合には、教示ツールが変形する、さらにはロボット自体が破損するなどの不都合を起こしていた。ロボットが破損することは重大な問題であるが、教示ツールが変形することで、過去の教示データが使用不能になったり、教示データを変形ツールに合わせて変更するなど、めんどうな手続きが必要であるという問題があった。また、さらに重大なことは、教示者の安全性の問題である。すなわち、教示中はロボットの先端の微妙な位置決めを行う必要があるため、教示者はロボットの近傍で作業を行うことが多い。このとき、教示中に誤って教示者自身の体にロボットアームをぶつけてしまう、あるいは、ロボットアームと周辺ワークの間に腕などを挟む等の危険性がある。さらに、複雑なワーク、たとえば自動車の車体フレーム内に教示者が入り、微妙な位置決めなど行う場合に、ロボット動作速度の選択、ロボット動作方向のＪＯＧキーを押し間違う等の危険が常に存在していた。
【０００５】
このような教示時の安全面を考慮した方策として、柔軟教示モードを操作者に表示する方法がある。この教示モードにおける柔軟制御状態を表示するために、図１５に示すように、プログラムペンダント上ＬＣＤ４０１の文字表示や、教示状態表示灯４０２のような教示確認手段が設けられ、操作者は表示灯を確認しながら教示作業を行うようにしている。図１５中、４０３は制御盤４０５に設けられプレイ状態表示灯、４０４は制御盤４０５内に設けられたモード切替を行うリレー回路である。しかしながら、図１５に示すプログラムペンダント上ＬＣＤ４０１の文字表示や、教示状態表示灯４０２だけでは柔軟教示モードであることを操作者に知らせる効果が低く、教示時とプレイバック時のロボット４００のコントロール状態の相違を明確に教示者に通告できない。このため柔軟制御が有効でないプレイバック時にロボットの可動範囲に入ってしまう可能性があり危険であった。ロボットはプレイバックモードにおいて作業を短時間で行うため、頻繁に急加速、急減速を繰り返すことが多く、モータは非常に大きなトルクを発生させ、ロボットアームは高速で運動するため大きな運動エネルギーを有し、教示者にとって非常に危険である。また、教示モードでロボットが柔軟制御状態にある場合、ロボットの発生トルクが非常に小さく押さえられているため、外部から作用する力でロボットが容易に動作する。このようなロボットアームの動きが、ロボットアームとロボットの作業対象であるワークとの接触や、ロボットの周辺の機械との干渉を誘発し、不具合を引き起こすことがあった。
【０００６】
【０００７】
【０００８】
【発明が解決しようとする課題】
そこで本発明は、予期せぬ外乱力がロボットに作用した場合の、操作者の安全確保、あるいはロボットや周辺機器の破損防止を解決課題とする。
【０００９】
【課題を解決するための手段】
前記課題を解決するための手段は、位置及び速度の状態フィードバックループを有し、各関節を駆動するモータの制御回路を備えたロボットの制御装置であって、モータの動作指令値からモータの加速トルク及びモータの速度を維持するための速度維持トルクを算出する手段と、前記加速トルクと速度維持トルクを加算してモータの運動に必要な仮想トルク指令を算出する手段と、前記算出結果に基づいてロボットの発生トルクを制限する手段とを有するロボットの制御装置において、前記ロボットの発生トルクを制限する手段によって制限されたトルクに前記ロボットに作用する重力トルクを補償するトルクを加える重力補償手段を有し、前記ロボットの発生トルクを制限する手段が、ロボットヘの位置指令や速度指令の動作指令の状態によって前記動作指令を加速・等速、減速、指令なしの３つのモードに分類し前記モードに応じて発生トルクの制限方法を切り替える手段と、前記トルク制限に一定時間かかった際に接触や衝突が発生したと判断して動作を停止する手段とからなり、前記ロボットの発生トルクを制限する手段は、前記指令なしモードおよび接触や衝突が発生したと判断した時点から前記指令なしモードに至るまでの間においてはトルク制限を０にするものである。
【００１０】
【発明の実施の形態】
以下、本発明の実施の形態について説明する。
図１は、本発明の第１実施例を示すものであり、運動制御ブロック１１に、各モータの運動指令から加速トルク及び速度トルクを演算する演算手段１８と、演算手段１８により演算された仮想トルク指令とサーボ制御系メインループ１６から得られたトルク指令を比較する比較器１９と、比較器１９の出力に基づいてブレーキを制御するブレーキ制御部２０を有している。その他の構成は従来例を示す図１３と同様である。図２は第１実施例における制御ブロック図を示している。図中、２１は時間遅れ補償のためのフィルタ、２２は関節駆動モータ１５の位置信号を記憶するメモリ、２５，２７は加減算器である。
本実施例において、ティーチペンダント上にはジョグキー１２と呼ばれる、ロボット誘導の動作ボタンが配置されている。ジョグキー１２を押すと、その時に選択されている座標系と速度に応じて、押下するジョグキー１２で示される方向にロボットが動作することになる。その時の信号の流れは、ジョグキー１２が押されるとその方向と大きさの情報が軌道生成部１４に入力される。軌道生成部１４では現在のロボットの指令に対して、指定座標系から基準座標系に変換された増分値が加算され新たなロボットの位置指令が生成される。基準座標系は通常ロボットのベースを中心とした直交座標系で表現されるため、動作指令値は実際のロボットの動作を行うためにロボットの関節座標系に変換され、モータのサーボ系の指令値となる。
【００１１】
各モータの制御を行うサーボ系では、例えば図３のブロック図で示されるような処理が行われる。図３において、位置指令信号は２つのブロック１０１，１０２の処理系に信号が分かれる。１つのブロックは通常のモータの位置、速度制御系１０１であり、もう１つは本発明で付加される制御系の処理系１０２のブロックである。速度制御系１０１では位置制御および速度制御が実施される。位置制御では比例制御、速度制御系では比例積分制御などが用いられることが一般的である。図３に示す制御系１０２は、仮想トルク指令を算出する部分である。通常のロボットの運動で必要となるトルクは、ロボットアームとロボットの先端負荷の両者を加速するトルク、運動状態で速度を維持するためのトルクの２種類である。１８−２は加速トルクの演算を行うブロック、１８−１は速度を維持するためのブロックを示している。ブロック１８−２では慣性と加速度の積から必要となるトルクが算出される。慣性の情報は、以下のいずれかの方法により求められる。
１）ロボットの関節の動きに応じて変動する値を演算により求める。
２）適応オブザーバなどのパラメータ同定手法を用いて推定する。
３）代表的な慣性の値を用いる。例えば常に一定の値を用いる、あるいは変位に応じてテーブルから値を引く。図３は１）の場合の例を示している。
【００１２】
ブロック１８−１では速度を維持するためのトルクが算出される。速度トルクはほぼ動摩擦の値に等しい。従って、あらかじめ求めた速度と摩擦の関係を用いて、現在の速度から必要なトルクを求めることができる。ブロック１８−１，１８−２で算出され、加算器２４で加算されたトルクの加算値はロボットが運動を行うトルクとほぼ等しい値である。演算結果は仮想トルク指令１０３として、加減算器２５により、メインループ（ブロック１０１）のトルク指令１０４より減算され、予め設定した値１０５と比較され、予め設定した値１０５より大きいときはブレーキ制御装置２０によりロボットの運動を停止する。ロボットアームが外部環境に接触などを起こした場合には、位置速度制御系の制御偏差が発生してメインループ（ブロック１０１）のトルク指令１０４は大きな値になるが、位置指令の変化はないため仮想トルク指令１０３は通常の動作で得られる値になり差が生じる。この差を予め設定した値１０５と比較し、その比較した結果の符号によりロボットの運動を停止する。重力方向に動くロボットでは重力トルクは１０６で示される補償演算をもとにメインループで補償されるため、仮想トルク指令値が重力の影響で大きくズレを生じることはない。重力補償演算は、例えばロボットの関節角度とアームの重心位置、重量から必要となるトルクを算出することで行われる。ブロック１８−１，１８−２で生成される仮想トルク指令１０３とメインループのトルク指令１０４の発生時間のずれを補償するために、ロボットの加速トルクの算出手段の後段あるいは速度維持トルクの算出手段の後段に高周波成分を除去するフィルタ２１を有する。フィルタ２１は一般的な１次遅れフィルタや２次遅れフィルタにより調整することが可能である。
【００１３】
以上の説明はティーチングペンダントで教示を行う場合を例にとって説明をおこなったが、プレイバックの状態でも当然用いることができる。プレイバック時にはティーチペンダントのかわりにメモリ１３から軌道生成部１４にデータの情報が払い出される。メモリ１３にはあらかじめ教示されたロボットの位置、姿勢の情報が保存されている。また、本発明はロボットに力センサを取け付け、操作者が加える力を計測しロボットを誘導するタイプの、いわゆるダイレクトティーチングのロボットにも適用が可能であり、教示者の安全を確保する上で有効に作用する。ダイレクトティーチング時にはティーチペンダントの代わりに力センサの信号からロボットの運動指令を生成し、軌道生成部１４にデータの情報が払い出される。このように、ダイレクトティーチではロボットを直接把持しての作業となるため、本発明は特に有効に作用する。
【００１４】
図４は本発明の第２実施例を示すものである。本実施例では、運動制御ブロック１１に、トルク制限値をサーボ制御系メインループ１６に与えるかどうかの切り替えを行う切替スイッチ２３を設け、切替スイッチ２３をオンにしたときに、各モータの運動指令から加速トルク及び速度トルクを演算する演算部１８によりトルク制限値を演算するようにしたものである。以下、本実施例について説明する。図４に示すジョグキー１２を押すと、その時に選択されている座標系と速度に応じて、押下するジョグキー１２で示される方向にロボットが動作することになる。そのときの信号の流れは、ジョグキー１２が押されるとその方向と大きさの情報が軌道生成部１４に入力される。軌道生成部１４では、現在のロボットの指令に対して、指定座標系から基準座標系に変換され増分値が加算され新たなロボットの位置指令が生成される。基準座標系は通常ロボットのベースを中心とした直交座標系で表現されるため、動作指令値は実際のロボットの動作を行うためにロボットの関節座標系に変換され、モータのサーボ系の指令値となる。
【００１５】
各モータの制御を行うサーボ系では、例えば図５のブロック図で示されるような処理が行われる。図５において、位置指令信号は２つのブロックの処理系に信号が分かれる。１つのブロックは通常の位置、速度制御系２０１であり、もう１つは本発明で付加される制御系の処理系２０２のブロックである。ブロック２０２の処理系では位置制御および速度制御が施される。位置制御では比例制御、速度制御系では比例積分制御などが用いられることが一般的である。図５で示す２０２の制御系は、運動に必要となるトルクをもとにトルク制限値を算出する部分である。通常のロボットの運動で必要となるトルクは、ロボットアームとロボット先端負荷の両者を加速するトルク、運動状態で速度を維持するためのトルクの２種類である。図５の実施例では、ロボットの関節の動きに応じて変動する慣性を演算により求める場合の例を示している。ブロック１８−１では速度を維持するためのトルクが算出される。速度トルクはほぼ動摩擦の値に等しい。従って、あらかじめ求めた速度と摩擦の関係を用いて、現在の速度から必要なトルクを求めることができる。ブロック１８−１，１８−２で算出されたトルクの加算値はロボットが運動を行うトルクとほぼ等しい値である。計算結果は、トルク制限値演算ブロック２０３ヘ入力される。ここでは、前段の演算をもとにメインブロック２０１で発生するトルクの制限値を演算する。ここでは、制限値は前段で制限された値に適度幅を設けることにより求めている。幅を設ける理由は、１つは、指令トルクと発生トルクが必ずしも一致していないことがあるため、その誤差を吸収することである。上記で述べた通り、ブロック２０３で演算された制限値がロボットの運動で必要となるトルクからずれが生ずることがありうる。このような状態は先端負荷の質量などの正確な情報が得られない場合などに起こりうる。したがって、指令通りの正確な運動が必要とされるプレイバック動作では制御ブロック２０２を用いず、教示モードでのみブロック２０２が機能するよう切り替えることができる。図４では切り替えスイッチ２３がこの部分の働きを行う。
【００１６】
また、図６は操作者に教示時であることを知らせる例を示している。ロボット上に表示ランプ３１を取り付け、教示時には制御盤３３内のリレー回路３２が作動して表示ランプ３１を任意の点灯パターンで表示させ操作者に通知する。
【００１７】
【００１８】
【００１９】
【００２０】
次に、本発明の第３実施例を説明する。図７に示す第３実施例は、図４に示す第２実施例の構成に、ロボット関節駆動モータの回転位置を検出するエンコーダ３４から、その回転位置を現在値として軌道生成部１４にフィードバックするようにしたものである。本実施例において、ティーチングペンダント上にはＪＯＧキーと呼ばれる、ロボット誘導の動作ボタンが配置されている。ＪＯＧキー１２を押すと、そのときに選択されている座標系と速度に応じて、押下するＪＯＧキーで示される方向にロボットが動作することになる。そのときの信号の流れは、ＪＯＧキー１２が押されるとその方向と大きさの情報が軌道生成部１４に入力される。軌道生成部１４では現在のロボットの指令に対して、指定座標系から基準座標系に変換された増分値が加算され新たなロボットの位置指令が生成される。基準座標系は通常ロボットのベースを中心とした直交座標系で表現されるため、実際のロボットの動作を行うためにロボットの関節座標系に変換される。関節座標系での変位の値は時間あたりの変位量としてロボットの関節を駆動するパルス列に変換される。変換されたパルス列は関節を駆動するサーボ系への位置指令信号となる。各モータの制御を行うサーボ系では例えば図８のブロック図で示されるような処理が行われる。図８で、位置指令信号は２つのブロックの処理系に信号が分かれる。１つは通常のモータの位置、速度制御系２０１であり、もう１つは本発明で付加される制御系の処理系２０２である。２０１の処理系では位置制御および速度制御が施される。位置制御では比例制御、速度制御系では比例積分制御などが用いられることが一般的である。
【００２１】
図８で示す２０２の制御系は、運動に必要となるトルクを算出する部分である。通常のロボットの運動で必要となるトルクは、ロボットアームとロボットの先端負荷の両者を加速するトルク、通常の運動状態で速度を維持するためのトルクの２種類である。１８−２は加速トルクの演算を行うブロック、１８−１は速度を維持するためのブロックを示している。ブロック１８−２では慣性と加速度の積から必要となるトルクが算出される。慣性の情報は、以下のいずれかの方法が用いられる。
１）慣性はロボットの動きに応じて変動する値を演算により求める。
２）適応オブザーバなどのパラメータ同定手法を用いて推定する。
３）代表的な慣性の値を用いる。
図８では１）の場合を示している。ブロック１８−１では速度トルクが算出される。速度トルクはほぼ動摩擦の値に等しい。従って、あらかじめ求めた速度と摩擦の関係から、現在の位置指令の速度を求めることで必要なトルクを求めることができる。
【００２２】
ブロック１８−１，１８−２で算出されたトルクの加算値はロボットが運動を行うトルクとほぼ等しい値である。計算結果はトルク制限値演算ブロック２０３ヘ入力される。ここでは、前段の演算をもとにメインブロック２０１で発生するトルクの制限値を演算する。制限値は前段で演算された値に適度な幅を設けることにより求めることができる。ここで適度な幅を設ける理由は、指令トルクと発生トルクが必ずしも一致していないことがあるため、その誤差を吸収すること、また、時間的な発生トルクのずれを吸収することである。時間的なずれの補償とは、指令値により発生トルクを演算した瞬間のトルクとメインブロック２０１で発生するトルクが時間的なずれを生ずる可能性があり、その誤差をトルクの制限の幅で吸収することである。この時間的なずれは演算ブロックの出力に時間遅れを補償するフィルタ２１を挿入することでも吸収することができる。フィルタは一般的な１次遅れフィルタや２次遅れフィルタにより調整する事が可能である。
【００２３】
このようなトルク制限を設けることによる物理的な作用の説明を次に行う。ブロック２０１で示される制御系のメインループではロボットに外部からの力が作用した場合、位置偏差と速度偏差が発生する。位置偏差は定数倍され速度指令となり、速度偏差は比例積分制御され、その出力はモータのトルク指令となる。ロボットに特別の外力が作用しない場合、モータで発生すべきトルクは加速トルク、速度トルクである。通常の動作でロボットが運動するためには上記のトルクを発生すれば良い。ところが、ロボットに外部から力が作用する（またはロボット自身が外部に接触する）場合には、外力によりロボットが動作し前述の制御偏差が発生することになる。従って、そのときの指令トルクはブロック２０２で演算を行ったトルクの制限域から逸脱したものになる。制限がなければその可能な限りのトルクが発生することになる。適度に制限された場合には制限内のトルクが発生し、外部からの力により偏差をゆるす、すなわちロボットが外力により動くことになる。また、重力方向に動くロボットでは重力トルクはメインブロック２０１で補償されるため、トルク制限演算ブロック２０３によりメインループのトルクが小さく抑えられた場合でも、ロボットアームが重力で落下するなどのことはない。
【００２４】
次にトルクの制限の方法を図９を用いて具体的に述べる。図９はロボットアームの１つの関節の運動に着目した場合である。関節５は図９（ａ）で示されるような加速、等速、減速の運動指令が与えられるものとする。図９（ｂ）は図８の信号２０５、図９（ｃ）は図８での信号２０６を表している。また、図９（ｄ）の細線はトルク制限ブロックでのトルク制限の上限値と下限値を表している。下限値については例えば、アームを進行方向に押すような場合に下限値にかかることが考えられる。しかしながら、このような場合はほとんど現れることはなく、また危険な方向ではないので、必ずしもトルク制限の下限値を設定する必要はない。トルク制限の一例を図１０を用いてさらに詳しく説明する。図１０は図９と同様、１つの関節の運動に着目した場合であり、図１０（ａ）のような加速、等速、減速の運動指令が与えられたときの通常動作のトルク制限の状態（ｂ）とぶつかり時のトルク制限の状態（ｃ）を示している。ここでは簡単に上限値のみを示している。図１０（ｂ）では、まず、速度指令（一定時間当たりの位置指令）がないときにはモード０として、トルク出力（図８のトルク制限部２０４の出力）を０にする。このような運動指令がない場合でも図８のトルク制限演算２０３の結果は上記で述べたように適度な幅を持っているので、位置偏差が生じた場合にはそれを解消するような方向にある程度のトルクが出力される。従ってロボットアームが指令を待っている状態で停止しているときに作業者の手でアームを押す際にはトルク制限の幅の分以上の力で押さなければならない。
【００２５】
しかしながら、このとき上述のようにモード０としてトルク出力を強制的に０にすれば、人間の手で軽く動かすことができる。このときにも、図８のように後段で重力補償１０６が施されるので、重力方向にアームが落下するようなことはない。これは、例えば、操作ミスにより作業者の腕がワークとロボットの間に挟まれて止まった場合などに有効であり、簡単にアームを手で押して回避させ、脱出することが可能である。このような場合、従来では操作者自身や共同作業者が非常停止ボタンを押すか、モータが過負荷状態になって非常停止が作用するまでロボットは動き続けることになる。非常停止が働くと、モータの供給電源を遮断すると共に、ブレーキが作動しロボットは運動を停止する。ブレーキが作動するとロボットは機械的にロックされた状態になり、挟みこまれた教示者は自力では脱出できなかった。速度指令区間では図９で説明したようにモード１としてトルク制限演算２０３で計算されたトルク制限を施す。速度指令区間後ある一定時間を減速区間、モード２とする。この区間では速度指令はないが、減速しながら最終的な指令位置に行き着くまで動作している。この区間では図９のようにトルク制限演算２０３の結果でトルク制限してもよいし、図１０（ｂ）のように一定のトルク制限を施してもよい。このときはモード１のトルク制限値をそのまま用いたり、あるいは、予め測定しておいた摩擦分のトルクをトルク制限値として用いる。すなわち最終的な位置決めに必要な最低限のトルク制限をしておき、この区間として十分指令位置へ行き着く時間を設定しておけば、運動指令に対しての追従性は確保される。また、一定時間後はモード０へ移行する。
【００２６】
ロボットアームが動作中に周辺の物体５１に接触した場合は、図１０（ｃ）のように、モード１でトルク制限値にかかるが、このとき一定時間このトルク制限値にかかり続ければ、ぶつかったと判断してトルク出力（図８のトルク制限部２０４の出力）を０にする。ぶつかりを検出した場合は強制的にモード３へ移行し、トルク出力を０にするとともに、アラームを発生させ作業者にこれを知らせる。このとき仮に教示者がロボットアームの接触に気がつかず、ＪＯＧキーを押し続けており、速度指令が発生している場合でも、トルク出力（図８のトルク制限部２０４の出力）は０となっているので、周辺物体への接触力は十分小さいものとなる。またこのぶつかりを検出する一定時間内でも、トルク制限ブロック２０４により速度を維持するためのトルクにある程度の幅を持たせたトルクのみが発生トルクであるため、衝撃は十分小さいものになる。ぶつかりを検出後、教示者がＪＯＧキーを離して速度指令がなくなればモード０へ移行する。ぶつかりを検出する時間は短ければ短いほどよいが、上述したように指令トルクと発生トルクとの誤差や時間的なずれのために、ぶつかってもいないのに誤ってぶつかったと判断されないように適度に調整する必要がある。ロボットアームが周辺物体に接触した後や、さらにその後ロボットアームを作業者の手で回避させた後では大きな位置偏差を生じる。例えば図８のサーボ系２０１ではこのとき位置偏差が大きくなっている。しかし、軌道発生ブロック１０からの指令がなければモード０でトルク出力（図８のトルク制限部２０４の出力）は０になっているので、ロボットアームが動くことはない。ただし、教示者が再びＪＯＧキーを押して指令が発生した場合にはその指令に応じたトルクに加えて、位置偏差分のトルクも発生してしまうので大変危険である。ここで、ぶつかりを検出した際に残った位置偏差や過去の偏差の積算値をクリアすることでこのような危険が回避できる。
【００２７】
しかしながら、これだけでは図１１（ｂ）のように軌道発生ブロック１０で指令した位置と現在の位置（エンコーダ値）とは矛盾が生じたままであり、そのまま続けて動作させてもこの指令値と現在値との差は残ったままである。特に、直線補間をさせながらロボットアームの先端を直線移動させるような場合では、この差のために座標系が歪んでしまい、思った通りの直線動作ができなくなってしまう。そこで、軌道発生ブロック１０で保持している指令値をエンコーダ３４の現在の値で再生成すれば、このような矛盾を解消でき、ぶつかった後でも再び正確な直線動作が可能となる。このような指令値の再生成のタイミングとしては例えば、指令値と現在値との比較によって、大きな偏差を検出したときに行ったり、モード０のときに行ってもよい。あるいはモード０で指令値と現在値との大きな偏差を検出したときに行ってもよい。また、大きな偏差が生じたときやぶつかりを検出した際にアラームを発生し、操作者がアラームリセットを行うときに指令値の再生成を行っても良い。ロボットアームの動作中は常に指令値と現在値との定常的な偏差が存在するのでこれは停止中に行うべきである。以上の説明はティーチングペンダントで教示を行う場合を例にとって説明をおこなったが、プレイバックの状態でも当然用いることができる。プレイバック時にはティーチペンダントのかわりにメモリから軌道生成ブロックにデータの情報が払い出される。メモリにはあらかじめ教示されたロボットの位置、姿勢の情報が保存されている。
【００２８】
さて、ブロック２０３で演算されたトルク制限値がロボットの運動で必要となるトルクからのずれが大きく生ずることがありうる。このような状態は先端の負荷の重力の正確な情報が得られない場合などに起こり得る。したがって、指令通りの正確な運動が必要とされるプレイバック動作では制御ブロック２０２を用いず、教示モードでのみブロック２０２が機能するよう切り替えることができる。図７では切り替えスイッチ２３がこの部分の働きを行う。教示モードにおいても負荷の変動等、必ずしも指令通りにロボットの動きが行われない場合が起こり得る。このような場合、エンコーダなど現在のロボットの関節の変位量を教示データとしてメモリ１３に保存することで、教示者の意図したポイントの位置及び姿勢が登録されることになる。また、本実施例はロボットに力センサを取け付け、操作者が加える力を計測しロボットを誘導するタイプの、いわゆるダイレクトティーチングのロボットにも適用が可能であり、教示者の安全を確保する上で有効に作用する。ダイレクトティーチングではロボット近傍での作業となるため、本実施例は特に有効である。
【００２９】
図１２は上記実施例のハードウェア構成を示している。図７の軌道発生ブロック１０と軌道生成部１４と運動制御ブロック１１がそれぞれ図１２のシステム制御基板６０１とティーチングペンダント６０２、演算基板６１１、サーボ基板６２１とサーボアンプ６２６に対応している。それぞれの基板は共有のバスによって接続されている。軌道発生ブロック６００において、ティーチングペンダント６０２で教示したデータは不揮発性メモリ６０６に記憶される。また、ロボットの機構的、制御的なパラメータもこの不揮発性メモリ６０６に記憶されており、バスで共有化されている。軌道発生の機能はＲＯＭ６０４に格納されたプログラムをＣＰＵ６０３が実行することによって実現される。軌道生成ブロック６１０においては、軌道生成や座標変換の機能はＲＯＭ６１３に格納されたプログラムをＣＰＵ６１２が実行することによって実現される。図８における制御ブロックは運動制御ブロック６２０のサーボ基板６２１の中のＲＯＭ６２３にプログラムされている。このプログラムをＣＰＵ６２２が読み込んで実行することによって実際にトルク制限処理が行われ、その結果がサーボアンプ６２６に出力される。さらにダイレクトティーチングを行う場合は、センサブロック６３０を追加することによって実現可能となる。センサ基板６３１では力センサ６３７の値をＡＤ変換部６３３で読み込み、インピーダンス制御などの力制御手段によって位置修正量等が出力される。
【００３０】
以上説明したように、第３実施例によれば、従来のロボットのモータの制御系に動作指令から生成される新たなブロックを付け加え、ロボットの関節を駆動するモータの発生トルクを制限することで、ロボットの運転中に外部環境にロボットアームが衝突する、あるいはロボットアームに外部の機械等が衝突するなどした場合、ロボットが柔軟に変形することで、ロボットやワーク、あるいは外部の機械が損傷することがなくなる。また、教示中の操作者の安全が守られるなど従来のロボットでは実現が不可能であった安全な制御機能を具備することが可能となる。また、衝突があった場合、ぶつかりが検出されトルク出力（トルク制限部の出力）を０にするモードによって接触力を十分小さくすることができ、ロボットアームを人間の力で安全な位置・姿勢に軽く回避させることができる。あるいは衝突がなくても、ロボットアームが停止しているときにも人間の力で軽く動かすことができる。このようにトルク出力が０であっても、重力トルク分は常に加算されており、アームが落下することはない。さらに、減速区間で最終位置決めに必要な最低限のトルクを出力することによって最終的な指令値まで行き着くことが可能である。このように柔軟な性能を持ちながら、指令に対する追従性を確保できる。また、ぶつかった後や人間の力でロボットアームを動かした場合でも制御偏差をクリアし、エンコーダの現在値から指令値を再生成することによって、サーボ電源を落とさずにそのまま正確な直線動作など、教示作業を続けることが可能となる。
【００３１】
【発明の効果】
以上説明したように、本発明によれば、ロボットの運転中にロボットが外部環境に衝突したり、ロボットアームに外部の機械が衝突した場合、ロボットやワークあるいは外部の機械の損傷がなくなり、また、教示中の操作者の安全が守られるなど、従来のロボットでは実現が不可能であった安全な制御機能が可能となる。
【図面の簡単な説明】
【図１】本発明の第１実施例を示すブロック図である。
【図２】第１実施例のサーボ制御部のブロック図である。
【図３】第１実施例の制御部の詳細を示すブロック図である。
【図４】本発明の第２実施例を示すブロック図である。
【図５】第２実施例の制御部の詳細を示すブロック図である。
【図６】教示状態表示器の例を示す説明図である。
【図７】本発明の第３実施例を示すブロック図である。
【図８】第３実施例のサーボ系制御ブロック図である。
【図９】第３実施例の作用を示すグラフである。
【図１０】第３実施例の各状態を示すグラフである。
【図１１】第３実施例のアームの状態を示す図である。
【図１２】第３実施例におけるハードウェア構成を示す図である。
【図１３】従来の制御装置の信号の流れを示すブロック図である。
【図１４】従来のモータのサーボ制御部のブロック図である。
【図１５】従来の教示モード確認方法を示す説明図である。[0001]
BACKGROUND OF THE INVENTION
  The present invention can protect the robot arm body, tools, and workpieces when the robot arm collides with a surrounding object in a motor control system used in the robot, or can ensure safety when the robot arm comes in contact with the operator. The present invention relates to a control device that realizes simple control.
[0002]
[Prior art]
  A conventional servo system for a motor that drives a robot joint has a position feedback loop and a speed feedback loop, amplifies the servo deviation that is the difference from the command, and performs control by taking the amplification gain as high as possible. .FIG.Fig. 1 shows a block diagram of conventional motion control of a robot. In the figure, reference numeral 10 denotes a trajectory generation block which generates a motor motion command from the input of the teach pendant JOG key 12 or from data in the memory 13. The memory 13 stores data such as position and speed taught in advance and an interpolation method. Based on the data in the teach pendant 12 or the memory 13, the trajectory data generated by the trajectory generation unit coordinate conversion unit 14 is coordinate-converted and sent to the motion control block 11 as a rotation command for each joint motor. The motion control block 11 is a servo system of the motor, and outputs a control signal based on the position / speed feedback signal of the robot joint drive motor 15 and the motion command of each motor generated by the trajectory generation unit coordinate conversion unit 14. A control system main loop 16 and an amplifier 17 that amplifies the power of the control signal are provided. There are as many motion control blocks 11 as the number of motors constituting the drive system of the robot.
[0003]
  FIG.These show in more detail the motion control block 11 when the position control system is constituted by each joint. Although only a single-axis servo system is described here, the same configuration is applied to other motor shafts.FIG., Kp is a position control gain, Kv is a speed control gain, and Ti is a time constant of the integrator. Here, conventionally, the gains Kp and Kv of the position loop and the speed loop are increased to be less susceptible to the influence of torque disturbance, and the integration time constant Ti is set to be shorter, so that the motor's relative to the position command value is set. Followability is improved. In such a conventional control method, the torque of the motor is generated by the difference between the command value and the current value, that is, the control deviation. Control deviation is usually caused by servo disturbances such as inertia of the robot arm and response delay of the control device, and due to torque disturbance such as gravity acting on the robot arm, friction of the reducer, and other external force acting on the robot arm. To do. When moving the robot to a desired position or direction, it is desirable that the robot follows the operation command at high speed. On the other hand, if the robot collides with a surrounding object or comes into contact with a human during movement, it is desirable to stop the movement as quickly as possible.
[0004]
  However, in the conventional robot control device, if a control deviation occurs without distinguishing both the command value response and the disturbance response, the motor generates a torque proportional to the control deviation amount or the past integrated value of the deviation. . For this reason, when the robot is accidentally collided with a surrounding object or workpiece during teaching, the teaching tool is deformed, and further, the robot itself is damaged. It is a serious problem that the robot is damaged. However, if the teaching tool is deformed, past teaching data becomes unusable, or the teaching data needs to be changed according to the deforming tool. There was a problem that there was. More serious is the teacher's safety issue. That is, since it is necessary to perform delicate positioning of the tip of the robot during teaching, the teacher often performs work near the robot. At this time, there is a risk of accidentally hitting the robot arm against the teacher's own body during teaching, or pinching the arm between the robot arm and the peripheral work. Furthermore, when an instructor enters a complicated work, for example, a car body frame, and performs delicate positioning, there is always a risk of selecting the robot operating speed and pressing the JOG key in the robot operating direction. .
[0005]
  As a measure considering the safety aspect at the time of teaching, there is a method of displaying a flexible teaching mode to an operator. In order to display the flexible control state in this teaching mode, as shown in FIG. 15, character display on the LCD 401 on the program pendant and teaching confirmation means such as the teaching state indicator lamp 402 are provided, and the operator turns on the indicator lamp. Teaching work is performed while checking. In FIG. 15, 403 is a play status indicator lamp provided on the control panel 405, and 404 is a relay circuit for switching modes provided in the control panel 405. However,FIG.The character display of the LCD 401 on the program pendant and the teaching state indicator light 402 alone have a low effect of notifying the operator that the flexible teaching mode is in effect, and the difference in the control state of the robot 400 during teaching and playback is clearly clarified. Cannot notify the teacher. For this reason, there is a possibility that the robot may enter the movable range during playback when the flexible control is not effective. Since robots work in playback mode in a short time, they frequently repeat rapid acceleration and rapid deceleration, the motor generates very large torque, and the robot arm moves at high speed and has large kinetic energy. However, it is very dangerous for the teacher. In addition, when the robot is in the flexible control state in the teaching mode, the generated torque of the robot is suppressed to a very small level, so that the robot easily operates with a force acting from the outside. Such movement of the robot arm may cause contact between the robot arm and the work that is the work target of the robot, and interference with the machines around the robot, resulting in problems.
[0006]
[0007]
[0008]
[Problems to be solved by the invention]
  Therefore, the present invention can ensure the safety of the operator or prevent the robot and peripheral devices from being damaged when an unexpected disturbance force acts on the robot.SolutionMake a decision.
[0009]
[Means for Solving the Problems]
    Means for solving the problem is a robot control device having a position and speed state feedback loop and having a motor control circuit for driving each joint, and the motor acceleration is determined from the motor operation command value. Means for calculating the speed maintenance torque for maintaining the torque and the speed of the motor, and adding the acceleration torque and the speed maintenance torque, which is necessary for the motor movement.Virtual torque commandA control apparatus for a robot, comprising: means for calculating a value; and means for limiting a generated torque of the robot based on the calculation result.Gravity compensation means for adding a torque compensating the gravity torque acting on the robot to the torque limited by the means for limiting the generated torque of the robot;The means for limiting the torque generated by the robot is a position command or speed command to the robot.ofDepending on the status of the operation commandThe operation command is classified into three modes: acceleration / constant speed, deceleration, no command, and according to the mode.A means for switching the method for limiting the generated torque, and a means for determining that a contact or collision has occurred when the torque limit takes a certain time and stopping the operation.Thus, the means for limiting the torque generated by the robot sets the torque limit to 0 during the period from the time when it is determined that the no command mode and contact or collision have occurred until the mode without command is reached.Is.
[0010]
DETAILED DESCRIPTION OF THE INVENTION
  Hereinafter, embodiments of the present invention will be described.
  FIG. 1 shows a first embodiment of the present invention. In the motion control block 11, a calculation means 18 for calculating acceleration torque and speed torque from a motion command of each motor, and a virtual calculated by the calculation means 18 are shown. A comparator 19 that compares the torque command and the torque command obtained from the servo control system main loop 16 and a brake control unit 20 that controls the brake based on the output of the comparator 19 are provided. Other configurations show conventional examplesFIG.It is the same. FIG. 2 shows a control block diagram in the first embodiment. In the figure, 21 is a filter for time delay compensation, 22 is a memory for storing the position signal of the joint drive motor 15, and 25 and 27 are adders / subtracters.
  In this embodiment, a robot guidance operation button called a jog key 12 is arranged on the teach pendant. When the jog key 12 is pressed, the robot moves in the direction indicated by the pressed jog key 12 according to the coordinate system and speed selected at that time. As for the signal flow at that time, when the jog key 12 is pressed, information on its direction and size is input to the trajectory generator 14. In the trajectory generation unit 14, an increment value converted from the designated coordinate system to the reference coordinate system is added to the current robot command to generate a new robot position command. Since the reference coordinate system is usually expressed in an orthogonal coordinate system centered on the base of the robot, the motion command value is converted to the robot's joint coordinate system to perform the actual robot motion, and the motor servo system command value It becomes.
[0011]
  In the servo system that controls each motor, for example, processing as shown in the block diagram of FIG. 3 is performed. In FIG. 3, the position command signal is divided into processing systems of two blocks 101 and 102. One block is a normal motor position and speed control system 101, and the other is a block of a control system processing system 102 added in the present invention. The speed control system 101 performs position control and speed control. In general, proportional control is used in position control, and proportional-integral control is used in a speed control system. The control system 102 shown in FIG. 3 is a part that calculates a virtual torque command. There are two types of torque required for normal robot motion: torque for accelerating both the robot arm and the tip load of the robot, and torque for maintaining speed in the motion state. Reference numeral 18-2 represents a block for calculating acceleration torque, and reference numeral 18-1 represents a block for maintaining the speed. In block 18-2, the required torque is calculated from the product of inertia and acceleration. The inertia information is obtained by any of the following methods.
1) A value that varies according to the movement of the joint of the robot is obtained by calculation.
2) Estimation is performed using a parameter identification method such as an adaptive observer.
3) Use typical inertia values. For example, a constant value is always used, or a value is subtracted from the table according to the displacement. FIG. 3 shows an example in the case of 1).
[0012]
  In block 18-1, a torque for maintaining the speed is calculated. The speed torque is approximately equal to the value of dynamic friction. Therefore, the necessary torque can be obtained from the current speed using the relationship between the speed and the friction obtained in advance. The added value of the torque calculated by the blocks 18-1 and 18-2 and added by the adder 24 is substantially equal to the torque at which the robot moves. The calculation result is subtracted from the torque command 104 of the main loop (block 101) by the adder / subtractor 25 as a virtual torque command 103, compared with a preset value 105, and when it is greater than the preset value 105, the brake control device 20 To stop the robot movement. When the robot arm is brought into contact with the external environment, a control deviation of the position / speed control system occurs, and the torque command 104 of the main loop (block 101) becomes a large value, but there is no change in the position command. The virtual torque command 103 becomes a value obtained by normal operation, and a difference occurs. This difference is compared with a preset value 105, and the movement of the robot is stopped according to the sign of the comparison result. In a robot that moves in the direction of gravity, the gravitational torque is compensated by the main loop based on the compensation calculation indicated by 106, so that the virtual torque command value does not greatly deviate due to the influence of gravity. Gravity compensation calculation is performed, for example, by calculating the required torque from the joint angle of the robot, the position of the center of gravity of the arm, and the weight. In order to compensate for the difference in generation time between the virtual torque command 103 generated in the blocks 18-1 and 18-2 and the main loop torque command 104, the subsequent stage of the acceleration torque calculation means or the speed maintenance torque calculation means A filter 21 for removing high-frequency components is provided at the subsequent stage. The filter 21 can be adjusted by a general primary delay filter or secondary delay filter.
[0013]
  In the above description, the teaching pendant is used as an example. However, the teaching pendant can also be used in a playback state. At the time of playback, data information is delivered from the memory 13 to the trajectory generator 14 instead of the teach pendant. The memory 13 stores information on the position and posture of the robot taught in advance. In addition, the present invention can be applied to a so-called direct teaching type robot in which a force sensor is attached to the robot, and the force applied by the operator is measured to guide the robot. It works effectively. At the time of direct teaching, a robot motion command is generated from the signal of the force sensor instead of the teach pendant, and data information is delivered to the trajectory generation unit 14. As described above, since direct teaching is performed by directly gripping the robot, the present invention works particularly effectively.
[0014]
  FIG. 4 shows a second embodiment of the present invention. In this embodiment, the motion control block 11 is provided with a changeover switch 23 for switching whether or not to apply a torque limit value to the servo control system main loop 16, and when the changeover switch 23 is turned on, a motion command for each motor is provided. Thus, the torque limit value is calculated by the calculation unit 18 for calculating the acceleration torque and the speed torque. Hereinafter, this embodiment will be described. When the jog key 12 shown in FIG. 4 is pressed, the robot operates in the direction indicated by the pressed jog key 12 according to the coordinate system and speed selected at that time. As for the signal flow at that time, when the jog key 12 is pressed, information on its direction and size is input to the trajectory generator 14. The trajectory generation unit 14 converts the current robot command from the designated coordinate system to the reference coordinate system, adds the increment value, and generates a new robot position command. Since the reference coordinate system is usually expressed in an orthogonal coordinate system centered on the base of the robot, the motion command value is converted to the robot's joint coordinate system to perform the actual robot motion, and the motor servo system command value It becomes.
[0015]
  In the servo system that controls each motor, for example, processing as shown in the block diagram of FIG. 5 is performed. In FIG. 5, the position command signal is divided into two block processing systems. One block is a normal position / speed control system 201, and the other is a block of a control system processing system 202 added in the present invention. In the processing system of block 202, position control and speed control are performed. In general, proportional control is used in position control, and proportional-integral control is used in a speed control system. The control system 202 shown in FIG. 5 is a part that calculates a torque limit value based on the torque required for exercise. There are two types of torque required for normal robot motion: torque for accelerating both the robot arm and the robot tip load, and torque for maintaining speed in the motion state. In the embodiment of FIG. 5, an example is shown in which the inertia that varies according to the movement of the joint of the robot is obtained by calculation. In block 18-1, a torque for maintaining the speed is calculated. The speed torque is approximately equal to the value of dynamic friction. Therefore, the necessary torque can be obtained from the current speed using the relationship between the speed and the friction obtained in advance. The added value of the torque calculated in blocks 18-1 and 18-2 is substantially equal to the torque with which the robot moves. The calculation result is input to the torque limit value calculation block 203. Here, the limit value of the torque generated in the main block 201 is calculated based on the previous calculation. Here, the limit value is obtained by providing a moderate width to the value limited in the previous stage. One reason for providing the width is to absorb the error because the command torque and the generated torque may not always match. As described above, the limit value calculated in the block 203 may deviate from the torque required for the robot motion. Such a state may occur when accurate information such as the mass of the tip load cannot be obtained. Therefore, in the playback operation that requires an exact motion as instructed, the control block 202 is not used, and the block 202 can be switched to function only in the teaching mode. In FIG. 4, the changeover switch 23 performs this function.
[0016]
  FIG. 6 shows an example of notifying the operator that teaching is in progress. A display lamp 31 is mounted on the robot, and at the time of teaching, the relay circuit 32 in the control panel 33 is activated to display the display lamp 31 in an arbitrary lighting pattern and notify the operator..
[0017]
[0018]
[0019]
[0020]
  Next, the present inventionThirdExamples will be described.FIG.Shown inThirdIn the embodiment, in the configuration of the second embodiment shown in FIG. 4, an encoder 34 that detects the rotation position of the robot joint drive motor is fed back to the trajectory generation unit 14 as the rotation position. . In this embodiment, robot guidance operation buttons called JOG keys are arranged on the teaching pendant. When the JOG key 12 is pressed, the robot operates in the direction indicated by the pressed JOG key according to the coordinate system and speed selected at that time. As for the signal flow at that time, when the JOG key 12 is pressed, information on the direction and size is input to the trajectory generator 14. In the trajectory generation unit 14, an increment value converted from the designated coordinate system to the reference coordinate system is added to the current robot command to generate a new robot position command. Since the reference coordinate system is usually expressed by an orthogonal coordinate system centered on the base of the robot, it is converted into a joint coordinate system of the robot in order to perform an actual robot operation. The displacement value in the joint coordinate system is converted into a pulse train for driving the joint of the robot as a displacement amount per time. The converted pulse train becomes a position command signal to the servo system that drives the joint. In the servo system that controls each motor, for exampleFIG.Processing as shown in the block diagram of FIG.FIG.Thus, the position command signal is divided into two block processing systems. One is a normal motor position and speed control system 201, and the other is a control system processing system 202 added in the present invention. In the processing system 201, position control and speed control are performed. In general, proportional control is used in position control, and proportional-integral control is used in a speed control system.
[0021]
  FIG.202 is a part for calculating the torque required for the exercise. The torque required for normal robot motion is between the robot arm and the robot.Both tip loadsThere are two types of torque: a torque for accelerating the speed and a torque for maintaining the speed in a normal motion state. Reference numeral 18-2 represents a block for calculating acceleration torque, and reference numeral 18-1 represents a block for maintaining the speed. In block 18-2, the required torque is calculated from the product of inertia and acceleration. For the inertia information, one of the following methods is used.
1) Inertia is calculated by calculating a value that varies according to the movement of the robot.
2) Estimation is performed using a parameter identification method such as an adaptive observer.
3) Use typical inertia values.
  FIG.Shows the case of 1). In block 18-1, the speed torque is calculated. The speed torque is approximately equal to the value of dynamic friction. Accordingly, the necessary torque can be obtained by obtaining the speed of the current position command from the relationship between the speed and the friction obtained in advance.
[0022]
  Torque calculated in blocks 18-1 and 18-2ofThe added value is approximately equal to the torque at which the robot moves. The calculation result is input to the torque limit value calculation block 203. Here, the limit value of the torque generated in the main block 201 is calculated based on the previous calculation. The limit value can be obtained by providing an appropriate width to the value calculated in the previous stage. Here, the reason why the appropriate width is provided is that the command torque and the generated torque do not necessarily coincide with each other, so that the error is absorbed and the temporal deviation of the generated torque is absorbed. Compensation of time lag means that there is a possibility that time lag occurs between the moment when the generated torque is calculated based on the command value and the torque generated in the main block 201, and this error is absorbed within the limit of the torque. It is to be. This time lag can also be absorbed by inserting a filter 21 that compensates for the time delay in the output of the calculation block. The filter can be adjusted by a general primary delay filter or secondary delay filter.
[0023]
  The physical action by providing such a torque limit will be described next. In the main loop of the control system indicated by block 201, when an external force acts on the robot, a position deviation and a speed deviation are generated. The position deviation is multiplied by a constant to become a speed command, the speed deviation is controlled by proportional integral control, and its output becomes a motor torque command. When no special external force is applied to the robot, the torque to be generated by the motor is acceleration torque and speed torque. In order for the robot to move in a normal operation, the above torque may be generated. However, when a force is applied to the robot from the outside (or the robot itself contacts the outside), the robot operates due to the external force and the above-described control deviation occurs. Therefore, the command torque at that time deviates from the torque limit range calculated in block 202. If there is no limit, as much torque as possible is generated. When the torque is appropriately limited, a torque within the limit is generated, and the deviation is relaxed by an external force, that is, the robot moves by an external force. In the case of a robot moving in the direction of gravity, the gravitational torque is compensated by the main block 201. Therefore, even when the torque of the main loop is kept small by the torque limit calculation block 203, the robot arm does not fall by gravity. .
[0024]
  Next, a method for limiting the torque will be specifically described with reference to FIG. FIG. 9 shows a case where attention is paid to the motion of one joint of the robot arm. Assume that the joint 5 is given a motion command for acceleration, constant speed, and deceleration as shown in FIG. FIG. 9 (b)FIG.The signal 205 of FIG. 9 and FIG. 9C represent the signal 206 of FIG. The thin line in FIG. 9D represents the upper limit value and the lower limit value of the torque limit in the torque limit block. As for the lower limit value, for example, it may be considered that the lower limit value is applied when the arm is pushed in the traveling direction. However, in such a case, it hardly appears, and since it is not a dangerous direction, it is not always necessary to set the lower limit value of the torque limit. An example of torque limitation will be described in more detail with reference to FIG. FIG. 10 is a case where attention is paid to the motion of one joint, as in FIG. 9, and the torque limit state of the normal operation when the acceleration, constant speed, and deceleration motion commands are given as shown in FIG. (B) and the state (c) of the torque limitation at the time of a collision are shown. Here, only the upper limit value is simply shown. In FIG. 10B, first, when there is no speed command (position command per fixed time), mode 0 is set and torque output (output of torque limiting unit 204 in FIG. 8) is set to zero. Even if there is no such motion command, the result of the torque limit calculation 203 in FIG. 8 has an appropriate width as described above. A certain amount of torque is output. Therefore, when the robot arm is stopped while waiting for a command, it must be pushed with a force equal to or greater than the width of the torque limit when the arm is pushed by the operator's hand.
[0025]
  However, at this time, if the torque output is forcibly set to 0 in mode 0 as described above, it can be moved lightly by a human hand. At this time,FIG.Thus, since the gravity compensation 106 is performed at a later stage, the arm does not fall in the direction of gravity. This is effective when, for example, the operator's arm is pinched between the workpiece and the robot due to an operation error, and the arm can be easily pushed by hand to escape. In such a case, conventionally, the robot continues to move until the operator or the collaborator presses the emergency stop button or until the motor is overloaded and the emergency stop is activated. When the emergency stop is activated, the power supply to the motor is cut off, the brake is activated, and the robot stops moving. When the brake actuated, the robot was mechanically locked, and the caught teacher could not escape by himself. In the speed command sectionFIG.As described above, the torque limit calculated in the torque limit calculation 203 is applied as mode 1. A certain time after the speed command section is set as a deceleration section, mode 2. There is no speed command in this section, but it operates until it reaches the final command position while decelerating. In this sectionFIG.The torque may be limited by the result of the torque limit calculation 203 as shown in FIG.FIG.A constant torque limit may be applied as in (b). At this time, the torque limit value of mode 1 is used as it is, or the torque measured in advance is used as the torque limit value. In other words, if the minimum torque limit necessary for final positioning is set, and the time for reaching the command position is set sufficiently as this section, followability to the motion command is ensured. Further, after a predetermined time, the mode is shifted to mode 0.
[0026]
  If the robot arm touches the surrounding object 51 during operation,FIG.As shown in (c), the torque limit value is applied in mode 1, but if this torque limit value continues to be applied for a certain period of time at this time, it is determined that a collision has occurred and torque output (FIG.The output of the torque limiter 204 is set to zero. When a collision is detected, the mode is forcibly shifted to mode 3, the torque output is set to 0, and an alarm is generated to notify the operator of this. At this time, even if the instructor is unaware of the contact of the robot arm and keeps pressing the JOG key, the torque output (FIG.Since the output of the torque limiter 204 is 0, the contact force to the surrounding objects is sufficiently small. Even within a certain time to detect this collision, the torque limit block204As a result, only the torque that has a certain width in the torque for maintaining the speed is the generated torque, so the impact is sufficiently small. After the collision is detected, if the teacher releases the JOG key and there is no speed command, the mode shifts to mode 0. The shorter the time to detect a collision, the better. However, as described above, due to the error between the command torque and the generated torque and the time lag, it should be reasonably moderate so that it is not judged that it has collided. It needs to be adjusted. A large positional deviation occurs after the robot arm comes into contact with a surrounding object or after the robot arm is avoided by the operator's hand. For exampleFIG.In the servo system 201, the position deviation is large at this time. However, if there is no command from the trajectory generation block 10, torque output in mode 0 (FIG.Since the output of the torque limiter 204 is 0, the robot arm does not move. However, when a teacher generates a command by pressing the JOG key again, a torque corresponding to the command and a torque corresponding to the position deviation are also generated, which is very dangerous. Here, such danger can be avoided by clearing the accumulated value of the position deviation and the past deviation remaining when the collision is detected.
[0027]
  However, this aloneFIG.Orbit generation block as in (b)10There is a discrepancy between the position commanded in step 1 and the current position (encoder value), and the difference between the command value and the current value remains even if the operation is continued as it is. In particular, when the tip of the robot arm is linearly moved while performing linear interpolation, the coordinate system is distorted due to this difference, and the desired linear motion cannot be performed. Therefore, if the command value held in the trajectory generation block 10 is regenerated with the current value of the encoder 34, such a contradiction can be resolved, and an accurate linear motion can be performed again even after the collision. Such timing of regeneration of the command value may be performed, for example, when a large deviation is detected by comparing the command value with the current value or when the mode is 0. Alternatively, it may be performed when a large deviation between the command value and the current value is detected in mode 0. Further, an alarm may be generated when a large deviation occurs or a collision is detected, and the command value may be regenerated when the operator resets the alarm. Since there is always a steady deviation between the command value and the current value during operation of the robot arm, this should be done while the robot arm is stopped. In the above description, the teaching pendant is used as an example. However, the teaching pendant can also be used in a playback state. At the time of playback, data information is sent from the memory to the trajectory generation block instead of the teach pendant. Information on the position and posture of the robot taught in advance is stored in the memory.
[0028]
  Now, calculated in block 203torqueThe limit value may greatly deviate from the torque required for the robot motion. Such a situation may occur when accurate information on the gravity at the tip load cannot be obtained. Therefore, in the playback operation that requires an exact motion as instructed, the control block 202 is not used, and the block 202 can be switched to function only in the teaching mode.FIG.The changeover switch 23 performs this function. Even in the teaching mode, there may occur a case where the robot does not necessarily move according to the command, such as a load change. In such a case, the current position of the joint of the robot such as an encoder is stored in the memory 13 as teaching data, whereby the position and posture of the point intended by the teacher is registered. In addition, this embodiment can be applied to a so-called direct teaching robot that attaches a force sensor to the robot and measures the force applied by the operator to guide the robot, thereby ensuring the safety of the teacher. It works effectively on the above. In direct teaching, since the work is performed in the vicinity of the robot, this embodiment is particularly effective.
[0029]
  FIG.Shows the hardware configuration of the above embodiment.FIG.Trajectory generation block 10, trajectory generation unit 14 and motion control block 11 respectivelyFIG.The system control board 601, teaching pendant 602, arithmetic board 611, servo board 621, and servo amplifier 626 are supported. Each board is connected by a shared bus. In the trajectory generation block 600, data taught by the teaching pendant 602 is stored in the nonvolatile memory 606. In addition, the mechanical and control parameters of the robot are also stored in the nonvolatile memory 606 and shared by the bus. The function of generating the trajectory is realized by the CPU 603 executing a program stored in the ROM 604. In the trajectory generation block 610, the trajectory generation and coordinate conversion functions are realized by the CPU 612 executing a program stored in the ROM 613.FIG.The control block is programmed in the ROM 623 in the servo board 621 of the motion control block 620. When the CPU 622 reads and executes this program, the torque limiting process is actually performed, and the result is output to the servo amplifier 626. Further, direct teaching can be realized by adding a sensor block 630. In the sensor substrate 631, the value of the force sensor 637 is read by the AD conversion unit 633, and a position correction amount or the like is output by force control means such as impedance control.
[0030]
  As explained above,ThirdAccording to the embodiment, a new block generated from an operation command is added to the conventional motor control system of the robot, and the generated torque of the motor that drives the joint of the robot is limited. If the robot arm collides with the robot arm or an external machine or the like collides with the robot arm, the robot or the work or the external machine is not damaged by the flexible deformation of the robot. In addition, it is possible to provide a safe control function that could not be realized by a conventional robot, such as the safety of an operator who is teaching. In addition, in the event of a collision, the collision force is detected and the torque output (output of the torque limiter) is set to 0, so that the contact force can be made sufficiently small, and the robot arm is brought to a safe position and posture by human power. It can be avoided lightly. Or even if there is no collision, the robot arm can be moved lightly by human power even when the robot arm is stopped. Thus, even if the torque output is 0, the gravitational torque is always added and the arm does not fall. Furthermore, it is possible to reach the final command value by outputting the minimum torque necessary for final positioning in the deceleration zone. In this way, it is possible to ensure followability to commands while having flexible performance. In addition, even if the robot arm is moved by a human force after collision, the control deviation is cleared, and the command value is regenerated from the current encoder value, so that the linear motion can be performed without turning off the servo power. It is possible to continue teaching work.
[0031]
【The invention's effect】
  As described above, according to the present invention, when the robot collides with the external environment during operation of the robot or when an external machine collides with the robot arm, the robot, the workpiece or the external machine is not damaged. This enables safe control functions that could not be realized with conventional robots, such as the safety of the operator being taught..
[Brief description of the drawings]
FIG. 1 is a block diagram showing a first embodiment of the present invention.
FIG. 2 is a block diagram of a servo control unit of the first embodiment.
FIG. 3 is a block diagram illustrating details of a control unit according to the first embodiment.
FIG. 4 is a block diagram showing a second embodiment of the present invention.
FIG. 5 is a block diagram illustrating details of a control unit according to a second embodiment.
FIG. 6 is an explanatory diagram showing an example of a teaching state display.
[Fig. 7] of the present invention.ThirdIt is a block diagram which shows an Example.
FIG. 8 is a servo system control block diagram of a third embodiment.
FIG. 9 is a graph showing the operation of the third embodiment.
FIG. 10 is a graph showing each state of the third embodiment.
FIG. 11 is a diagram illustrating a state of an arm according to a third embodiment.
FIG. 12 is a diagram illustrating a hardware configuration in a third embodiment.
FIG. 13 is a block diagram showing a signal flow of a conventional control device.
FIG. 14 is a block diagram of a servo control unit of a conventional motor.
FIG. 15 is an explanatory diagram showing a conventional teaching mode confirmation method.

Claims

A control apparatus for a robot having a position and velocity state feedback loop and having a motor control circuit for driving each joint,
Means for calculating the motor acceleration torque and the speed maintenance torque for maintaining the motor speed from the motor operation command value;
Means for adding the acceleration torque and the speed maintenance torque to calculate a virtual torque command necessary for motor movement;
In a robot control device having means for limiting the generated torque of the robot based on the calculation result,
Gravity compensation means for adding a torque compensating the gravity torque acting on the robot to the torque limited by the means for limiting the generated torque of the robot;
The means for limiting the generated torque of the robot is
Means for classifying the operation command into three modes of acceleration / constant speed, deceleration, no command according to the state of the position command or speed command to the robot, and switching the method for limiting the generated torque according to the mode; It consists of a means to stop the operation by judging that a contact or collision has occurred when it takes a certain time for the restriction ,
The means for limiting the torque generated by the robot sets the torque limit to 0 during the period from the time when it is determined that the no command mode and contact or collision have occurred until the mode without command is reached. Control device.