JP3367641B2 - ロボットの制御装置 - Google Patents
ロボットの制御装置Info
- Publication number
- JP3367641B2 JP3367641B2 JP21812498A JP21812498A JP3367641B2 JP 3367641 B2 JP3367641 B2 JP 3367641B2 JP 21812498 A JP21812498 A JP 21812498A JP 21812498 A JP21812498 A JP 21812498A JP 3367641 B2 JP3367641 B2 JP 3367641B2
- Authority
- JP
- Japan
- Prior art keywords
- collision
- motor
- robot
- servo motor
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1641—Programme controls characterised by the control loop compensation for backlash, friction, compliance, elasticity in the joints
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
- B25J9/1676—Avoiding collision or forbidden zones
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/406—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by monitoring or safety
- G05B19/4061—Avoiding collision or forbidden zones
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/406—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by monitoring or safety
- G05B19/4062—Monitoring servoloop, e.g. overload of servomotor, loss of feedback or reference
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/39—Robotics, robotics to robotics hand
- G05B2219/39186—Flexible joint
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/39—Robotics, robotics to robotics hand
- G05B2219/39355—Observer, disturbance observer
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/41—Servomotor, servo controller till figures
- G05B2219/41388—Observe input torque and feedback position, estimate reaction torque
Description
【０００１】
【産業上の利用分野】本発明は、サーボモータを駆動源
とするロボットにおいて、ロボット本体やロボットに負
荷されるツールやハンド等の治具がワーク等の他の物体
に衝突した事を検出し、被害を最小限に抑えるロボット
の制御装置に関する。
とするロボットにおいて、ロボット本体やロボットに負
荷されるツールやハンド等の治具がワーク等の他の物体
に衝突した事を検出し、被害を最小限に抑えるロボット
の制御装置に関する。
【０００２】
【従来の技術】従来においては、ロボット本体やツー
ル、治具等が他の物体に衝突したときの保護は、図５に
示すような方法をとっていた。以下にその構成を説明す
る。図５中、５１はサーボモータを制御する制御手段、
５２はロボットアームを駆動するサーボモータ、５３は
サーボモータ５２にかかる負荷（主に、モータのイナー
シャとロボットアームのイナーシャの和）、５４はアー
ムに加わる外乱力を推定するための減速機のバネ要素を
考慮しない剛体系外乱推定オブザーバ演算手段、５５は
剛体系外乱推定オブザーバ演算手段５４で演算された外
乱推定値を監視して、衝突したかどうかを判定する衝突
判定手段、５６は装置保護手段で、衝突と判定した場
合、直ちに制御対象を駆動するサーボモータ５２を停止
させ、装置の被害を抑える装置保護手段である。
ル、治具等が他の物体に衝突したときの保護は、図５に
示すような方法をとっていた。以下にその構成を説明す
る。図５中、５１はサーボモータを制御する制御手段、
５２はロボットアームを駆動するサーボモータ、５３は
サーボモータ５２にかかる負荷（主に、モータのイナー
シャとロボットアームのイナーシャの和）、５４はアー
ムに加わる外乱力を推定するための減速機のバネ要素を
考慮しない剛体系外乱推定オブザーバ演算手段、５５は
剛体系外乱推定オブザーバ演算手段５４で演算された外
乱推定値を監視して、衝突したかどうかを判定する衝突
判定手段、５６は装置保護手段で、衝突と判定した場
合、直ちに制御対象を駆動するサーボモータ５２を停止
させ、装置の被害を抑える装置保護手段である。
【０００３】この中で、剛体系外乱推定オブザーバ演算
手段５４では、以下の演算を行っていた。以下の構成
は、特開平６−２９２３７９号公報「異常負荷時のトル
クリミット変更方法」、特開平６−２４５５６１号公報
「サーボモータの異常負荷検出制御方法」、特開平６−
１３１０５０号公報「サーボモータで駆動される可動部
の衝突検出方法」、特開平３−１９６３１３号公報「外
乱オブザーバによる衝突検出方法」に記されている。図
６中、実線内が実機制御部６１である。破線部内は制御
対象モデル（以下、オブザーバモデルと呼ぶ）６２のブ
ロック線図であり、実機の機械的構成に相当するモデル
である。このように、実機の機械的構成に相当する部分
は、減速機のバネ要素を考慮せずに単純にモータのイナ
ーシャとロボットアームのイナーシャの和であるイナー
シャＪだけのモデルで構成されている。
手段５４では、以下の演算を行っていた。以下の構成
は、特開平６−２９２３７９号公報「異常負荷時のトル
クリミット変更方法」、特開平６−２４５５６１号公報
「サーボモータの異常負荷検出制御方法」、特開平６−
１３１０５０号公報「サーボモータで駆動される可動部
の衝突検出方法」、特開平３−１９６３１３号公報「外
乱オブザーバによる衝突検出方法」に記されている。図
６中、実線内が実機制御部６１である。破線部内は制御
対象モデル（以下、オブザーバモデルと呼ぶ）６２のブ
ロック線図であり、実機の機械的構成に相当するモデル
である。このように、実機の機械的構成に相当する部分
は、減速機のバネ要素を考慮せずに単純にモータのイナ
ーシャとロボットアームのイナーシャの和であるイナー
シャＪだけのモデルで構成されている。
【０００４】モデルの状態方程式は、式（１）に示すよ
うになる。θはモータ位置、Ｋｔはトルク定数、Ｊはイ
ナーシャ、ＴＬは外乱トルクを表す。また、図中Ｓはラ
プラス演算子を表し、微分を意味する。
うになる。θはモータ位置、Ｋｔはトルク定数、Ｊはイ
ナーシャ、ＴＬは外乱トルクを表す。また、図中Ｓはラ
プラス演算子を表し、微分を意味する。
【数１】
各変数上の「・」はその変数の一階時間微分を、「‥」
は二階時間微分を表すものとする。この式（１）から、
オブザーバを組む一般的な手法により、
は二階時間微分を表すものとする。この式（１）から、
オブザーバを組む一般的な手法により、
【数２】
外乱ＴＬを推定する同一次元オブザーバを構成すると、
図７の一点鎖線に示すオブザーバ７３になる。Ｋ３、Ｋ
４は外乱推定オブザーバ７３のパラメータであり、これ
らをオブザーバの系が安定するように選択することによ
り、推定値が得られる。
図７の一点鎖線に示すオブザーバ７３になる。Ｋ３、Ｋ
４は外乱推定オブザーバ７３のパラメータであり、これ
らをオブザーバの系が安定するように選択することによ
り、推定値が得られる。
【０００５】
【発明が解決するための課題】ところが、従来の技術で
は、サーボモータ５２とロボットアームの間にある減速
機のバネ要素をオブザーバのモデルとして考慮していな
かったため、ロボットの動作中のサーボモータとロボッ
トアーム間のねじれトルクまでも、アームに加わる外乱
として推定してしまっていた。それにより、検出用の設
定値を動作時のねじれトルクより小さくすることができ
ず、減速機の剛性が低いロボットアームなどでは、衝突
感度を上げることができないという問題があった。
は、サーボモータ５２とロボットアームの間にある減速
機のバネ要素をオブザーバのモデルとして考慮していな
かったため、ロボットの動作中のサーボモータとロボッ
トアーム間のねじれトルクまでも、アームに加わる外乱
として推定してしまっていた。それにより、検出用の設
定値を動作時のねじれトルクより小さくすることができ
ず、減速機の剛性が低いロボットアームなどでは、衝突
感度を上げることができないという問題があった。
【０００６】図９に、外乱推定値のシミュレーションの
結果を示す。図９（ａ）は、モータへの速度指令の図で
あり、図９（ｂ）が、ステップ上に印加された実外乱ト
ルク値（５．２Ｎ・ｍ）とオブザーバ外乱推定値の図で
ある。この図から明らかなように、加減速時にモータと
ロボットアームのねじれトルク分が、外乱推定値に現れ
てしまっていることが分かる。これにより、例えば、
５．３Ｎ・ｍに衝突感度設定値を設定しておいた場合
も、図中、○枠内Ａの部分が、設定値を越えてしまい、
実際の外乱は５．２Ｎ・ｍであるにもかかわらず、衝突
と誤検出してしまうことになる。すなわち、従来の剛体
外乱オブザーバを使用した場合は、○枠内Ａのような、
ねじれトルクより大きい値にしか、衝突感度を設定でき
ないという問題があった。そこで、本発明は、減速機の
剛性が低いロボットアームなどでも、高感度で検出でき
る、衝突検出装置を提供することを目的とする。
結果を示す。図９（ａ）は、モータへの速度指令の図で
あり、図９（ｂ）が、ステップ上に印加された実外乱ト
ルク値（５．２Ｎ・ｍ）とオブザーバ外乱推定値の図で
ある。この図から明らかなように、加減速時にモータと
ロボットアームのねじれトルク分が、外乱推定値に現れ
てしまっていることが分かる。これにより、例えば、
５．３Ｎ・ｍに衝突感度設定値を設定しておいた場合
も、図中、○枠内Ａの部分が、設定値を越えてしまい、
実際の外乱は５．２Ｎ・ｍであるにもかかわらず、衝突
と誤検出してしまうことになる。すなわち、従来の剛体
外乱オブザーバを使用した場合は、○枠内Ａのような、
ねじれトルクより大きい値にしか、衝突感度を設定でき
ないという問題があった。そこで、本発明は、減速機の
剛性が低いロボットアームなどでも、高感度で検出でき
る、衝突検出装置を提供することを目的とする。
【０００７】
【課題を解決するための手段】前記目的を達成するた
め、本発明のロボットの制御装置は、サーボモータを使
用し減速機を介して関節を駆動するロボットの制御装置
において、前記サーボモータを制御する制御手段と、前
記減速機のバネ要素を考慮したモデルを使用した、前記
制御手段内部のサーボモータへのトルク指令値と前記サ
ーボモータの位置とに基づいて、バネ要素とみなした前
記減速機から負荷側にあるロボットアームに作用する外
乱力を推定する状態推定オブザーバ演算手段と、この状
態推定オブザーバ演算手段で計算された外乱力推定値を
監視することにより外部環境との衝突発生を判断する衝
突判定手段と、衝突と判定した場合、前記ロボットアー
ムの動作がそのまま続行しないように前記サーボモータ
の運動状態を強制的に切り替える処理を行う装置保護手
段とを有する。前記装置保護手段として、衝突判定後、
前記制御手段内で、強制的に速度指令を０にし、速度ル
ープの積分要素を０にし、モータのフィードバック速度
に、ゲインを乗じたものをモータのトルク指令値とす
る。あるいは、前記装置保護手段として、衝突判定後、
一定時間、動作方向と逆向きにトルク指令値を加える手
段を有する。
め、本発明のロボットの制御装置は、サーボモータを使
用し減速機を介して関節を駆動するロボットの制御装置
において、前記サーボモータを制御する制御手段と、前
記減速機のバネ要素を考慮したモデルを使用した、前記
制御手段内部のサーボモータへのトルク指令値と前記サ
ーボモータの位置とに基づいて、バネ要素とみなした前
記減速機から負荷側にあるロボットアームに作用する外
乱力を推定する状態推定オブザーバ演算手段と、この状
態推定オブザーバ演算手段で計算された外乱力推定値を
監視することにより外部環境との衝突発生を判断する衝
突判定手段と、衝突と判定した場合、前記ロボットアー
ムの動作がそのまま続行しないように前記サーボモータ
の運動状態を強制的に切り替える処理を行う装置保護手
段とを有する。前記装置保護手段として、衝突判定後、
前記制御手段内で、強制的に速度指令を０にし、速度ル
ープの積分要素を０にし、モータのフィードバック速度
に、ゲインを乗じたものをモータのトルク指令値とす
る。あるいは、前記装置保護手段として、衝突判定後、
一定時間、動作方向と逆向きにトルク指令値を加える手
段を有する。
【０００８】
【発明の実施の形態】以下、本発明の実施例を図に基づ
いて説明する。図１は本発明の基本的構成を説明するブ
ロック図である。図１において、１はサーボモータの制
御手段、２はサーボモータ、３は制御対象であるロボッ
トアーム、４は減速機のバネ要素を考慮した２慣性系状
態推定オブザーバ演算部、５は衝突判定部、６は装置保
護処理部、７はモータとアームの間にある減速機であ
る。本実施例を実現するためのオブザーバモデルのブロ
ック図を図２の破線部に示す。図で使われる記号の説明
を以下に示す。 Ｋｐ：位置ループゲイン Ｋｖ：速度ループゲイン Ｋｉ：速度ループ積分ゲイン Ｋｔ：モータトルク定数 Ｓ：ラプラス演算子（微分を表す） １／Ｓ：積分を表す ＪＬ：負荷側イナーシャ Ｊｍ：モータイナーシャ Ｎ：減速比 Ｋ：減速機バネ定数 Ｃ：減速機粘性係数
いて説明する。図１は本発明の基本的構成を説明するブ
ロック図である。図１において、１はサーボモータの制
御手段、２はサーボモータ、３は制御対象であるロボッ
トアーム、４は減速機のバネ要素を考慮した２慣性系状
態推定オブザーバ演算部、５は衝突判定部、６は装置保
護処理部、７はモータとアームの間にある減速機であ
る。本実施例を実現するためのオブザーバモデルのブロ
ック図を図２の破線部に示す。図で使われる記号の説明
を以下に示す。 Ｋｐ：位置ループゲイン Ｋｖ：速度ループゲイン Ｋｉ：速度ループ積分ゲイン Ｋｔ：モータトルク定数 Ｓ：ラプラス演算子（微分を表す） １／Ｓ：積分を表す ＪＬ：負荷側イナーシャ Ｊｍ：モータイナーシャ Ｎ：減速比 Ｋ：減速機バネ定数 Ｃ：減速機粘性係数
【０００９】図２中、１１（実線部）はサーボモータ１
２を制御する制御部である。実施例では位置制御系は比
例制御、速度制御系は比例積分制御になっている。図２
中、１２はサーボモータを表す。１３（破線部）は、モ
ータ、減速機やロボットアームなどの制御対象に相当す
る部分であり、これがオブザーバモデルである。この中
で、減速機のバネ定数をＫでモデル化している。これに
より、バネ要素を介してモータイナーシャＪｍと負荷イ
ナーシャＪＬの２つの慣性を含んだモデルで構成されて
いる。これは２慣性系モデルと呼ばれる。図２の２慣性
系モデルを使用してオブザーバ演算部４ではロボットア
ームに作用する負荷側外乱力ｄ２を推定する。以下に、
オブザーバの演算関係式を書く。
２を制御する制御部である。実施例では位置制御系は比
例制御、速度制御系は比例積分制御になっている。図２
中、１２はサーボモータを表す。１３（破線部）は、モ
ータ、減速機やロボットアームなどの制御対象に相当す
る部分であり、これがオブザーバモデルである。この中
で、減速機のバネ定数をＫでモデル化している。これに
より、バネ要素を介してモータイナーシャＪｍと負荷イ
ナーシャＪＬの２つの慣性を含んだモデルで構成されて
いる。これは２慣性系モデルと呼ばれる。図２の２慣性
系モデルを使用してオブザーバ演算部４ではロボットア
ームに作用する負荷側外乱力ｄ２を推定する。以下に、
オブザーバの演算関係式を書く。
【００１０】
【数３】
ここで、ある変数の推定値を表す場合、その変数の上に
∧をつけて表す。ｋは自然数で制御周期の回数を表す。
（ｋ＝１、２、３、・・・）すなわちｋ番目のデータの
次のデータがｋ＋１番目のデータとなる。
∧をつけて表す。ｋは自然数で制御周期の回数を表す。
（ｋ＝１、２、３、・・・）すなわちｋ番目のデータの
次のデータがｋ＋１番目のデータとなる。
【数４】
ある変数の上にドット「・」を付けたものをその変数の
一階時間微分とする。また、行列の後に付加された［
］Tはその行列の転置行列を表すものとする。
一階時間微分とする。また、行列の後に付加された［
］Tはその行列の転置行列を表すものとする。
【数５】
ｄ２：負荷側外乱力
ｕｒｅｆ：モータ加速度指令
Ｔｓ：オブザーバ計算周期
【００１１】
【数６】
ＭＡＴ＿Ｂ＝［０ Ｔｓ ０ Ｔｓ／Ｎ ０］T ・・・（５）
ＭＡＴ＿Ｌ＝［Ｌ１ Ｌ２ Ｌ３ Ｌ４ Ｌ５］T ・・・（６）
Ａ２３ ＝−（Ｋ＊Ｔｓ）／（Ｎ＊Ｊｍ） ・・・（７）
Ａ２４ ＝−（Ｃ＊Ｔｓ）／（Ｎ＊Ｊｍ） ・・・（８）
Ａ４３ ＝−（Ｋ／（Ｎ2＊Ｊｍ）＋Ｋ／ＪＬ）＊Ｔｓ ・・・（９）
Ａ４４ ＝−（Ｃ／（Ｎ2＊Ｊｍ）＋Ｃ／ＪＬ）＊Ｔｓ＋１ ・・・（１０）
Ｌ１からＬ５は（ＭＡＴ＿Ａ−ＭＡＴ＿Ｌ＊［１ ０
０ ０ ０］）の固有値を設定することにより求める。
式(１１)に示す式をＭＡＴ＿Ｌに関して解くことにな
る。ここで、ｐ１からｐ５は任意に設定できる固有値
（極）の値である。 ｜ｓＩ−（ＭＡＴ＿Ａ−ＭＡＴ＿Ｌ＊［１ ０ ０ ０ ０］）｜ ＝(ｓ−ｐ１)(ｓ−ｐ２)(ｓ−ｐ３)(ｓ−ｐ４)(ｓ−ｐ５) …（１１） 以上の計算を行うことにより、
０ ０ ０］）の固有値を設定することにより求める。
式(１１)に示す式をＭＡＴ＿Ｌに関して解くことにな
る。ここで、ｐ１からｐ５は任意に設定できる固有値
（極）の値である。 ｜ｓＩ−（ＭＡＴ＿Ａ−ＭＡＴ＿Ｌ＊［１ ０ ０ ０ ０］）｜ ＝(ｓ−ｐ１)(ｓ−ｐ２)(ｓ−ｐ３)(ｓ−ｐ４)(ｓ−ｐ５) …（１１） 以上の計算を行うことにより、
【数７】
を推定することができる。
【００１２】図１の衝突判定部５では、
【数８】
予め計算しておいた各軸モータに作用する重力計算値と
摩擦計算値を減じて、真の外乱力のみを算出する。
摩擦計算値を減じて、真の外乱力のみを算出する。
【数９】
Ｍｇ ：重力計算値
Ｆｒｉｃ：摩擦計算値
真の外乱推定値が、ある設定しきい値ｌｉｍｉｔを越え
たとき、制御対象であるロボットアームもしくはツール
が外部装置に衝突したと判定し、装置保護処理部６に信
号を出す。
たとき、制御対象であるロボットアームもしくはツール
が外部装置に衝突したと判定し、装置保護処理部６に信
号を出す。
【００１３】図８に、本方式で推定される外乱の推定値
の波形を示す。図８（ａ）は、モータへの速度指令の図
であり、図８（ｂ）が、ステップ上に印加された実外乱
トルク値（５．２Ｎ・ｍ）とオブザーバ外乱推定値の図
である。図９に示す従来の剛体外乱オブザーバの外乱推
定値では、加減速時にモータとロボットアーム間のねじ
れトルクまで現れているが、本方式のオブザーバの推定
値には、加減速時のねじれトルクが現れていないことが
分かる。このように、本方式では、減速機のバネ定数を
考慮した正確な実機モデルを使用しているため、外乱推
定値にねじれトルクが現れず、検出すべき外乱トルクの
みを正確に推定している。結果として、ねじれトルクよ
りも小さい値に、衝突の感度を設定することができる。
これにより、剛性の弱いロボットや、ねじれトルクが大
きいロボットの場合も高感度に衝突検出を行うことが可
能になる。
の波形を示す。図８（ａ）は、モータへの速度指令の図
であり、図８（ｂ）が、ステップ上に印加された実外乱
トルク値（５．２Ｎ・ｍ）とオブザーバ外乱推定値の図
である。図９に示す従来の剛体外乱オブザーバの外乱推
定値では、加減速時にモータとロボットアーム間のねじ
れトルクまで現れているが、本方式のオブザーバの推定
値には、加減速時のねじれトルクが現れていないことが
分かる。このように、本方式では、減速機のバネ定数を
考慮した正確な実機モデルを使用しているため、外乱推
定値にねじれトルクが現れず、検出すべき外乱トルクの
みを正確に推定している。結果として、ねじれトルクよ
りも小さい値に、衝突の感度を設定することができる。
これにより、剛性の弱いロボットや、ねじれトルクが大
きいロボットの場合も高感度に衝突検出を行うことが可
能になる。
【００１４】図４は本発明を実施するためのロボット制
御装置の構成を示すブロック図である。同図において、
ロボット制御装置４０には全体を司るメインのプロセッ
サボード４１があり、プロセッサボード４１にはプロセ
ッサ４１ａ、ＲＯＭ４１ｂ、ＲＡＭ４１ｃおよび不揮発
性メモリ４１ｄがある。プロセッサ４１ａはＲＯＭ４１
ｂに従って、ロボット制御装置４０全体を制御する。Ｒ
ＡＭ４１ｃには各種のデータが格納されている。不揮発
性メモリ４１ｄには、ロボット１００の動作プログラム
等がＲＯＭ４１ｂからロードされている。プロセッサボ
ード４１はバス４７に結合されている。本実施例で示
す、衝突対応制御のための各計算は、ディジタルサーボ
制御回路４２内部で計算される、ソフトウェアによる機
能である。ディジタルサーボ制御回路４２はバス４７に
結合され、プロセッサボード４１からの指令によって、
サーボアンプ４３を経由して、サーボモータ４０１、４
０２、４０３、４０４、４０５および４０６を駆動す
る。これらのサーボモータはロボット１００に内蔵さ
れ、ロボット１００の各軸を動作させる。シリアルポー
ト４４はバス４７に結合され、教示操作盤４８やその他
のＲＳ２３２Ｃ機器４９と接続されている。教示操作盤
４８はロボットへの教示点入力に使用される。大容量メ
モリ４５にはティーチングデータ等が格納される。ま
た、Ｉ／Ｏ４６を経て外部とのデータおよび信号等の入
出力が行われる。
御装置の構成を示すブロック図である。同図において、
ロボット制御装置４０には全体を司るメインのプロセッ
サボード４１があり、プロセッサボード４１にはプロセ
ッサ４１ａ、ＲＯＭ４１ｂ、ＲＡＭ４１ｃおよび不揮発
性メモリ４１ｄがある。プロセッサ４１ａはＲＯＭ４１
ｂに従って、ロボット制御装置４０全体を制御する。Ｒ
ＡＭ４１ｃには各種のデータが格納されている。不揮発
性メモリ４１ｄには、ロボット１００の動作プログラム
等がＲＯＭ４１ｂからロードされている。プロセッサボ
ード４１はバス４７に結合されている。本実施例で示
す、衝突対応制御のための各計算は、ディジタルサーボ
制御回路４２内部で計算される、ソフトウェアによる機
能である。ディジタルサーボ制御回路４２はバス４７に
結合され、プロセッサボード４１からの指令によって、
サーボアンプ４３を経由して、サーボモータ４０１、４
０２、４０３、４０４、４０５および４０６を駆動す
る。これらのサーボモータはロボット１００に内蔵さ
れ、ロボット１００の各軸を動作させる。シリアルポー
ト４４はバス４７に結合され、教示操作盤４８やその他
のＲＳ２３２Ｃ機器４９と接続されている。教示操作盤
４８はロボットへの教示点入力に使用される。大容量メ
モリ４５にはティーチングデータ等が格納される。ま
た、Ｉ／Ｏ４６を経て外部とのデータおよび信号等の入
出力が行われる。
【００１５】以下に、図１の装置保護部処理部６の処理
を具体的に説明する。まず、装置保護処理部６の機能と
して、衝突判定後、制御手段内で、強制的に速度指令を
０にし、速度ループの積分要素を０にし、モータのフィ
ードバック速度に、ゲインを乗じたものをモータのトル
ク指令値とする第１実施例について説明する。図３は第
１実施例の、装置保護処理後の制御系を説明するブロッ
ク図である。図１の装置保護処理部６は、衝突判定後た
だちに、制御手段内で、強制的に速度指令を０にし、速
度ループの積分要素を０にし、モータのフィードバック
速度に、ある大きさのゲインＫ’を乗じたものをモータ
のトルク指令値とする。このとき、速度積分演算を中断
したことで、重力のかかるアームの場合、アームが落下
するため、モータへのトルク指令値に重力補償値を加算
する。重力補償値はロボットアームの姿勢および重量パ
ラメータから計算するか、または計測で予め求めてお
く。これにより、アームの速度に比例して、モータ発生
トルクがダンパ的に作用し、衝撃を少なくするととも
に、衝突後はロボットアームは衝突時に受ける外乱力に
対して柔軟にならう状態になる。
を具体的に説明する。まず、装置保護処理部６の機能と
して、衝突判定後、制御手段内で、強制的に速度指令を
０にし、速度ループの積分要素を０にし、モータのフィ
ードバック速度に、ゲインを乗じたものをモータのトル
ク指令値とする第１実施例について説明する。図３は第
１実施例の、装置保護処理後の制御系を説明するブロッ
ク図である。図１の装置保護処理部６は、衝突判定後た
だちに、制御手段内で、強制的に速度指令を０にし、速
度ループの積分要素を０にし、モータのフィードバック
速度に、ある大きさのゲインＫ’を乗じたものをモータ
のトルク指令値とする。このとき、速度積分演算を中断
したことで、重力のかかるアームの場合、アームが落下
するため、モータへのトルク指令値に重力補償値を加算
する。重力補償値はロボットアームの姿勢および重量パ
ラメータから計算するか、または計測で予め求めてお
く。これにより、アームの速度に比例して、モータ発生
トルクがダンパ的に作用し、衝撃を少なくするととも
に、衝突後はロボットアームは衝突時に受ける外乱力に
対して柔軟にならう状態になる。
【００１６】次に、装置保護手段として、衝突判定後、
一定時間、動作方向と逆向きに、ある大きさのモータト
ルク指令値を印加する第２実施例について説明する。衝
突判定部５で衝突を判定するまでは実施例１と同様であ
るので、省略する。この第２実施例では、衝突判定部５
で衝突と判定されると、装置保護処理部６は、ただち
に、衝突した方向と逆向きのトルク指令をある一定時間
ロボットアームに出す。これにより、アラームによりリ
レーが作動し電磁ブレーキが作動して、アームが惰走し
た後に停止する時間より速く、ロボットアームを停止す
ることができる。よって、衝突時にロボットアームが惰
走することによる、ロボットアームや、ロボットアーム
先端に負荷したツール、また、被衝突側のワークや周辺
機器の破損を防ぎ、被害を最小にすることができる。か
つ、従来のブレーキによる停止では起こりえた、アーム
が被衝突側内部に入り込んだ状態で停止するということ
がなく、衝突後の復旧が非常に簡単に行われる。
一定時間、動作方向と逆向きに、ある大きさのモータト
ルク指令値を印加する第２実施例について説明する。衝
突判定部５で衝突を判定するまでは実施例１と同様であ
るので、省略する。この第２実施例では、衝突判定部５
で衝突と判定されると、装置保護処理部６は、ただち
に、衝突した方向と逆向きのトルク指令をある一定時間
ロボットアームに出す。これにより、アラームによりリ
レーが作動し電磁ブレーキが作動して、アームが惰走し
た後に停止する時間より速く、ロボットアームを停止す
ることができる。よって、衝突時にロボットアームが惰
走することによる、ロボットアームや、ロボットアーム
先端に負荷したツール、また、被衝突側のワークや周辺
機器の破損を防ぎ、被害を最小にすることができる。か
つ、従来のブレーキによる停止では起こりえた、アーム
が被衝突側内部に入り込んだ状態で停止するということ
がなく、衝突後の復旧が非常に簡単に行われる。
【００１７】
【発明の効果】以上述べたように、本発明によれば、下
記の効果を奏する。 （１）減速機のバネ要素を考慮してオブザーバモデルを
構成しているため、衝突検出に必要な、減速機よりアー
ム側に作用する外乱力のみを精度良く推定するため、剛
性の弱いロボットアームに対しても、高感度で、衝突を
検出することが可能である。 （２）衝突検出後に、制御系にダンパ作用を設けるか、
もしくは逆向きのトルク指令を出すことにより、従来の
ように、停止指令を発生させブレーキを作動させる方式
に比較し、制御対象および制御対象に負荷した治具等お
よび被衝突側のワークおよび装置の破損を最小限に抑え
ることが可能になり、かつ、逆向きのトルク指令を出す
方法では、衝突後の復旧作業も簡単になる。
記の効果を奏する。 （１）減速機のバネ要素を考慮してオブザーバモデルを
構成しているため、衝突検出に必要な、減速機よりアー
ム側に作用する外乱力のみを精度良く推定するため、剛
性の弱いロボットアームに対しても、高感度で、衝突を
検出することが可能である。 （２）衝突検出後に、制御系にダンパ作用を設けるか、
もしくは逆向きのトルク指令を出すことにより、従来の
ように、停止指令を発生させブレーキを作動させる方式
に比較し、制御対象および制御対象に負荷した治具等お
よび被衝突側のワークおよび装置の破損を最小限に抑え
ることが可能になり、かつ、逆向きのトルク指令を出す
方法では、衝突後の復旧作業も簡単になる。
【図１】 本発明の具体的構成例を示すブロック図であ
る。
る。
【図２】 本発明のオブザーバモデルを示すブロック図
である。
である。
【図３】 本発明の第１実施例の構成を示すブロック図
である。
である。
【図４】 本発明を実現するハード構成を説明するブロ
ック図である。
ック図である。
【図５】 従来の衝突停止制御装置の構成を示すブロッ
ク図である。
ク図である。
【図６】 従来方式のオブザーバモデルを示すブロック
図である。
図である。
【図７】 従来の方式のオブザーバを示す図である。
【図８】 本発明によるオブザーバ外乱推定値の波形図
である。
である。
【図９】 従来方式のオブザーバ外乱推定値の波形図で
ある。
ある。
１ サーボモータの制御手段、２ サーボモータ、３
ロボットアーム、４ ２慣性系状態推定オブザーバ演算
部、５ 衝突判定部、６ 装置保護処理部、７減速機、
１１ 制御部、１２ サーボモータ、１３ 制御対象、
４０ ロボット制御装置、４１ プロセッサボード、４
１ａ プロセッサ、４１ｂ ＲＯＭ、４１ｃ ＲＡＭ、
４１ｄ 不揮発性メモリ、４２ ディジタルサーボ制御
回路、４３ サーボアンプ、４４ シリアルポート、４
５ 大容量メモリ、４６ Ｉ／Ｏ、４７ バス、４８
教示操作盤、４９ ＲＳ２３２Ｃ機器、４０１，４０
２，４０３，４０４，４０５，４０６ サーボモータ、
１００ ロボット ５１ 制御手段、５２ サーボモータ、５３ 負荷、５
４ 剛体系外乱推定オブザーバ演算手段、５５ 衝突判
定手段、５６ 装置保護手段、６１ 実機制御部、６２
制御対象モデル、７３ 外乱推定オブザーバ
ロボットアーム、４ ２慣性系状態推定オブザーバ演算
部、５ 衝突判定部、６ 装置保護処理部、７減速機、
１１ 制御部、１２ サーボモータ、１３ 制御対象、
４０ ロボット制御装置、４１ プロセッサボード、４
１ａ プロセッサ、４１ｂ ＲＯＭ、４１ｃ ＲＡＭ、
４１ｄ 不揮発性メモリ、４２ ディジタルサーボ制御
回路、４３ サーボアンプ、４４ シリアルポート、４
５ 大容量メモリ、４６ Ｉ／Ｏ、４７ バス、４８
教示操作盤、４９ ＲＳ２３２Ｃ機器、４０１，４０
２，４０３，４０４，４０５，４０６ サーボモータ、
１００ ロボット ５１ 制御手段、５２ サーボモータ、５３ 負荷、５
４ 剛体系外乱推定オブザーバ演算手段、５５ 衝突判
定手段、５６ 装置保護手段、６１ 実機制御部、６２
制御対象モデル、７３ 外乱推定オブザーバ
─────────────────────────────────────────────────────
フロントページの続き
(56)参考文献 特開 平８−66893（ＪＰ，Ａ)
特開 平７−136953（ＪＰ，Ａ)
特開 平６−180610（ＪＰ，Ａ)
(58)調査した分野(Int.Cl.7，ＤＢ名)
B25J 13/08
B25J 19/06
G05B 11/36
G05B 13/02
Claims (3)
- 【請求項１】 サーボモータを使用し減速機を介して関
節を駆動するロボットの制御装置において、前記サーボ
モータを制御する制御手段と、前記減速機のバネ要素を
考慮したモデルを使用した、前記制御手段内部のサーボ
モータへのトルク指令値と前記サーボモータの位置とに
基づいて、バネ要素とみなした前記減速機から負荷側に
あるロボットアームに作用する外乱力を推定する状態推
定オブザーバ演算手段と、この状態推定オブザーバ演算
手段で計算された外乱力推定値を監視することにより外
部環境との衝突発生を判断する衝突判定手段と、衝突と
判定した場合、前記ロボットアームの動作がそのまま続
行しないように前記サーボモータの運動状態を強制的に
切り替える処理を行う装置保護手段とを有することを特
徴とするロボットの制御装置。 - 【請求項２】 前記装置保護手段として、衝突判定後、
前記制御手段内で、強制的に速度指令を０にし、速度ル
ープの積分要素を０にし、モータのフィードバック速度
に、ゲインを乗じたものをモータのトルク指令値とする
ことを特徴とする請求項１記載のロボットの制御装置。 - 【請求項３】 前記装置保護手段として、衝突判定後、
一定時間、動作方向と逆向きに、ある大きさのモータト
ルク指令値を印加することを特徴とする請求項１記載の
ロボットの制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP21812498A JP3367641B2 (ja) | 1998-07-31 | 1998-07-31 | ロボットの制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP21812498A JP3367641B2 (ja) | 1998-07-31 | 1998-07-31 | ロボットの制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2000052286A JP2000052286A (ja) | 2000-02-22 |
| JP3367641B2 true JP3367641B2 (ja) | 2003-01-14 |
Family
ID=16715019
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP21812498A Expired - Fee Related JP3367641B2 (ja) | 1998-07-31 | 1998-07-31 | ロボットの制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3367641B2 (ja) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014064926A1 (ja) | 2012-10-25 | 2014-05-01 | パナソニック株式会社 | ロボットの異常表示方法 |
| CN103895016A (zh) * | 2012-12-27 | 2014-07-02 | 现代自动车株式会社 | 用于控制机器人步态的方法 |
| DE102016003250A1 (de) | 2015-03-23 | 2016-09-29 | Fanuc Corporation | Robotersteuervorrichtung mit einer Funktion zum Erfassen eines Kontakts mit einem Objekt oder einer Person |
Families Citing this family (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2003236787A (ja) * | 2002-02-18 | 2003-08-26 | Kawasaki Heavy Ind Ltd | 駆動制御方法および駆動制御装置 |
| DE10245594A1 (de) | 2002-09-30 | 2004-04-08 | Dürr Systems GmbH | Verfahren zur Kollisionserkennung |
| US7102315B2 (en) | 2003-07-29 | 2006-09-05 | Matsushita Electric Industrial Co., Ltd. | Robot arm control method and control device |
| JP3933158B2 (ja) * | 2004-10-21 | 2007-06-20 | 松下電器産業株式会社 | ロボットの衝突検出方法 |
| KR101262277B1 (ko) * | 2008-04-30 | 2013-05-08 | 현대중공업 주식회사 | 로봇의 충돌검지 방법 |
| KR101145243B1 (ko) | 2010-03-31 | 2012-05-24 | 한국과학기술연구원 | 다관절 매니퓰레이터의 위치센서를 이용한 제한 공간 산출 방법 |
| JPWO2013027250A1 (ja) * | 2011-08-19 | 2015-03-05 | 株式会社安川電機 | ロボットシステム、ロボット及びロボット制御装置 |
| CN103192413A (zh) * | 2012-01-06 | 2013-07-10 | 沈阳新松机器人自动化股份有限公司 | 一种无传感器的机器人碰撞检测保护装置及方法 |
| JP5912953B2 (ja) * | 2012-07-17 | 2016-04-27 | 株式会社ダイヘン | 制御装置、及び制御方法 |
| JP6019945B2 (ja) | 2012-08-31 | 2016-11-02 | ブラザー工業株式会社 | 制御装置及び画像形成システム |
| JP5820013B1 (ja) | 2014-04-30 | 2015-11-24 | ファナック株式会社 | ワークを把持して搬送するロボットの安全監視装置 |
| JP5937706B1 (ja) | 2015-01-21 | 2016-06-22 | ファナック株式会社 | ロボットに加わる外力に基づいてロボットを制御するロボット制御装置、およびロボットシステム |
| CN111511510A (zh) * | 2018-01-10 | 2020-08-07 | 索尼公司 | 控制设备、控制方法和程序 |
| JP7176255B2 (ja) * | 2018-07-04 | 2022-11-22 | 富士電機株式会社 | 電動機制御装置及び衝突検出方法 |
| CN110561429B (zh) * | 2019-08-30 | 2021-10-15 | 北京北方华创微电子装备有限公司 | 机械手碰撞保护检测方法、系统及下位机 |
| CN115674190A (zh) * | 2022-09-30 | 2023-02-03 | 深圳市越疆科技有限公司 | 协作机械臂及其运动控制方法、碰撞检测方法、控制系统 |
| CN115502960B (zh) * | 2022-10-28 | 2023-08-08 | 深圳市深科达智能装备股份有限公司 | 末端组件、作业装置及其控制方法 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0230487A (ja) * | 1988-07-18 | 1990-01-31 | Matsushita Electric Ind Co Ltd | 産業用ロボットの制御装置 |
| JP2665984B2 (ja) * | 1989-12-26 | 1997-10-22 | ファナック株式会社 | 外乱推定オブザーバによる衝突検出方法 |
| JPH06180610A (ja) * | 1992-12-14 | 1994-06-28 | Yamaha Motor Co Ltd | サーボ制御装置 |
| JPH06245561A (ja) * | 1993-02-10 | 1994-09-02 | Fanuc Ltd | サーボモータの異常負荷検出制御方法 |
| JP3355420B2 (ja) * | 1993-11-18 | 2002-12-09 | 株式会社椿本チエイン | 産業用ロボットの制御方法 |
| JPH0866893A (ja) * | 1994-08-24 | 1996-03-12 | Fanuc Ltd | 衝突検出方法 |
-
1998
- 1998-07-31 JP JP21812498A patent/JP3367641B2/ja not_active Expired - Fee Related
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014064926A1 (ja) | 2012-10-25 | 2014-05-01 | パナソニック株式会社 | ロボットの異常表示方法 |
| US9701022B2 (en) | 2012-10-25 | 2017-07-11 | Panasonic Intellectual Property Management Co., Ltd. | Robot malfunction indication method |
| CN103895016A (zh) * | 2012-12-27 | 2014-07-02 | 现代自动车株式会社 | 用于控制机器人步态的方法 |
| DE102016003250A1 (de) | 2015-03-23 | 2016-09-29 | Fanuc Corporation | Robotersteuervorrichtung mit einer Funktion zum Erfassen eines Kontakts mit einem Objekt oder einer Person |
| US9776328B2 (en) | 2015-03-23 | 2017-10-03 | Fanuc Corporation | Robot control apparatus having function of detecting contact with object or person |
| DE102016003250B4 (de) | 2015-03-23 | 2021-09-02 | Fanuc Corporation | Robotersteuervorrichtung mit einer Funktion zum Erfassen eines Kontakts mit einem Objekt oder einer Person |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2000052286A (ja) | 2000-02-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3367641B2 (ja) | ロボットの制御装置 | |
| US7328123B2 (en) | System for collision avoidance of rotary atomizer | |
| KR960005985B1 (ko) | 서어보 모우터에 의해 구동되는 기계 작동부의 충돌 검출 방법 | |
| US5719479A (en) | Collision detecting method | |
| JP4756618B2 (ja) | 多関節ロボットにおける衝突検出・停止制御法 | |
| JP2732159B2 (ja) | 異常負荷検出方法 | |
| US20090200978A1 (en) | Robot controller having component protecting function and robot control method | |
| JP6706741B2 (ja) | ロボットの衝突検出方法 | |
| US6298283B1 (en) | Industrial robot | |
| KR0144650B1 (ko) | 서보모터의 이상부하 검출 제어방법 | |
| JP2003025272A5 (ja) | ||
| JP2665984B2 (ja) | 外乱推定オブザーバによる衝突検出方法 | |
| JP2020101541A (ja) | トルク推定のための方法および装置 | |
| CN112809667B (zh) | 工业机器人的力控制方法、装置及其应用 | |
| JP2004364396A (ja) | モータの制御装置および制御方法 | |
| JPH1170490A (ja) | 産業用ロボットの衝突検出方法 | |
| EP1046470B1 (en) | Industrial robot with means for detecting collision and preventing re-collision | |
| JPH08211943A (ja) | 外乱オブザーバを使用したバックラッシュ補正方法 | |
| JP2906256B2 (ja) | サーボ制御装置 | |
| EP0441983A1 (en) | Method of controlling robot | |
| JPH08278821A (ja) | サーボ制御系の制振方法 | |
| JPH0682346A (ja) | イナーシャ、外乱トルクの推定及び異常負荷検出方法 | |
| JP3436087B2 (ja) | ロボットの制御方法および装置 | |
| JP7176255B2 (ja) | 電動機制御装置及び衝突検出方法 | |
| JP3165087B2 (ja) | 産業用ロボットの故障検出方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20081108 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20091108 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101108 Year of fee payment: 8 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101108 Year of fee payment: 8 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111108 Year of fee payment: 9 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111108 Year of fee payment: 9 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121108 Year of fee payment: 10 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121108 Year of fee payment: 10 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131108 Year of fee payment: 11 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20141108 Year of fee payment: 12 |
|
| LAPS | Cancellation because of no payment of annual fees |