JPH04242406A - 推定外乱による衝突検出方法 - Google Patents
推定外乱による衝突検出方法Info
- Publication number
- JPH04242406A JPH04242406A JP3015932A JP1593291A JPH04242406A JP H04242406 A JPH04242406 A JP H04242406A JP 3015932 A JP3015932 A JP 3015932A JP 1593291 A JP1593291 A JP 1593291A JP H04242406 A JPH04242406 A JP H04242406A
- Authority
- JP
- Japan
- Prior art keywords
- torque
- disturbance
- disturbance torque
- collision
- estimated
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title description 3
- 238000001514 detection method Methods 0.000 claims description 15
- 230000005484 gravity Effects 0.000 claims description 6
- 238000010586 diagram Methods 0.000 description 12
- 230000001133 acceleration Effects 0.000 description 5
- 101150115013 DSP1 gene Proteins 0.000 description 1
- 101150013423 dsl-1 gene Proteins 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Landscapes
- Safety Devices In Control Systems (AREA)
- Control Of Position Or Direction (AREA)
- Control Of Electric Motors In General (AREA)
- Manipulator (AREA)
Abstract
め要約のデータは記録されません。
Description
動される可動部の衝突検出方法に関し、特にロボット等
でのオブザーバを使用した推定外乱による衝突検出方法
に関する。
障害物に衝突すると、機構的な破損等の2次災害、ある
いは安全のために、早期に衝突を検出して、サーボモー
タを停止させる必要がある。このような例として、本出
願人による特願平1−334967号(オブザーバを用
いた衝突検出方法)がある。この出願では、衝突による
トルク変動を外乱トルクとみなして、外乱推定オブザー
バにより、アームの衝突を検出している。すなわち、外
乱トルクが一定以上になったら衝突とみなして衝突を検
出して、アラームを出力し、速度指令を0にしている。 速度指令を0にすることによりモータに停止トルクを発
生させる。
は、衝突による外乱トルクのみでなく、摩擦、重力、あ
るいは他軸の干渉による外乱トルクが含まれる。これら
の外乱トルクはロボットの速度、姿勢等によって大きく
変化する。従って、単に外乱トルクのみから衝突を検出
するとこれらの摩擦等による外乱トルクによって誤って
衝突を検出してしまう。逆に、これらの摩擦等の外乱ト
ルクによるマージンを大きくとると、実際の衝突が検出
できなくなる場合がある。本発明はこのような点に鑑み
てなされたものであり、より確実に衝突を検出できる推
定外乱による衝突検出方法を提供することを目的とする
。
決するために、サーボモータによって駆動される可動部
が障害物と衝突したことを推定外乱によって検出する推
定外乱による衝突検出方法において、前記可動部の第1
の外乱トルクを推定し、少なくとも摩擦トルク、重力ト
ルク、他軸の干渉トルクの1つを含む第2の外乱トルク
を計算し、前記第1の外乱トルクから、前記第2の外乱
トルクを引いた第3の外乱トルクを求め、前記第3の外
乱トルクによって、前記可動部の衝突を検出することを
特徴とする推定外乱による衝突検出方法が、提供される
。
クを推定する。第1の外乱トルクは摩擦、重力トルク、
他軸の干渉トルク等の外乱トルク及び衝突による外乱ト
ルクを含む。一方、可動部が本来有する摩擦トルク、重
力トルク、他軸の干渉トルク等の第2の外乱トルクを計
算する。第2の外乱トルクは可動部の速度、姿勢が決ま
れば一般に計算できる。第1の外乱トルクから第2の外
乱トルクを引いた第3の外乱トルクは、ほぼ衝突による
外乱トルクのみとなる。従って、第3の外乱トルクによ
って衝突を検出すれば、可動部の速度、姿勢等と無関係
に衝突を確実に検出できる。
明する。図2は本発明を実施するためのロボットシステ
ムのハードウェアの構成図である。ホストプロセッサ9
はロボット全体を制御するプロセッサである。ホストプ
ロセッサ9からはロボットの位置指令が共有RAM10
に書き込まれる。なお、ホストプロセッサ9に結合され
るROM、RAM等は省略してある。ロボットに内蔵さ
れたサーボモータ22を制御するDSP(ディジタル・
シグナル・プロセッサ)11はROM12のシステムプ
ログラムに従って、共有RAM10の位置指令を一定時
間ごとに読み取る。DSP11は、この位置指令とサー
ボモータ22に内蔵されたパルスコーダ23からの位置
フィードバックとの差分による位置偏差を求める。この
位置偏差に比例した速度指令を出力する。また、位置フ
ィードバックを微分して、サーボモータ22の速度を求
める。この速度指令と速度から速度ループを形成する。 また、速度ループからの速度指令を電流指令に変換して
、電流指令とサーボモータ22の電流から電流ループを
形成する。電流ループから電流指令が出力され、これか
らサーボモータ22を駆動するためのPWM波形を生成
して、DSL(ディジタル・サーボ・LSI)14経由
でサーボアンプ21に送る。サーボアンプ21はPWM
指令を受けて、サーボモータ22を駆動する。サーボモ
ータ22は減速機を介して、アーム26を駆動する。 サーボモータ22にはパルスコーダ23が内蔵されてお
り、位置フィードバックとしての帰還パルスをDSL1
4経由でDSP11に帰還する。さらに、DSP11は
後述するように外乱トルクの推定、既知トルクの計算等
の機能を果たす。
式を示す図であり、図3(A)はサーボ系のブロック図
、図3(B)はサーボ系の状態方式を示す図である。 電流U1はトルク定数Ktを有する要素31に入力され
る。要素31の出力はサーボモータ22を駆動する駆動
トルクとなる。これに外乱トルクX2が加算器32で加
算される。加算器32の出力はサーボモータ22とロボ
ットのアーム26等を含めたイナーシャJを有する要素
33に入力される。要素33の出力は加速度X1(1)
となる。この加速度X1(1) は積分要素34で積
分され、速度X1となる。図3(B)は図3(A)の状
態方程式である。図3(B)に示す状態方程式からX1
,X2を推定する同一次元オブザーバを組み外乱トルク
を推定する。
法を実施するためのブロック図である。このブロック図
の処理は図2に示したDSP11によって処理される。 電流U1は要素41に入力され、サーボモータ22(図
2)の出力トルクとなる。サーボモータ22の出力トル
クには演算要素42によって、外乱トルクX2が加えら
れる。演算要素42の出力は要素43によって、速度X
1となる。ここで、Jはサーボモータ22及びアーム2
6を含むイナーシャである。
れる。オブザーバ50は電流U1とサーボモータ22の
速度X1から、推定速度XX1を求め、サーボモータ2
2の速度を制御する。ここでは、これらのサーボモータ
22の速度制御については、省略し、外乱トルクを推定
するための演算のみを説明する。電流U1は要素51で
(Kt/J)をかけ、演算要素52へ出力される。演算
要素52では、後述する演算要素54からの帰還を加え
、さらに、演算要素53で演算要素55からの帰還を加
算する。演算要素52及び53の出力単位は加速度であ
る。演算要素53の出力は積分要素56に入力され、推
定速度XX1として出力される。推定速度XX1と実速
度X1との差を演算要素57で求め、それぞれ、演算要
素54及び55に帰還する。ここで、比例要素54はゲ
インK1を有する。積分要素55のゲインはK2である
。ここで、ゲインK1とゲインK2によって、帰還すべ
き周波数帯域が決められる。
2をJで除した推定加速度(XX2/J)であり、比例
要素61によって、電流値に変換される。ただし、トル
ク表示をするために、この電流値を推定外乱トルクTd
で表示する。ここで、Jは先の要素43のJと同じサー
ボモータ22及びアーム26のイナーシャの合計であり
、Ktは要素41のトルク定数と同じである。Aは係数
であり、1以下の数値であり、推定加速度(XX2/J
)を補正するための係数である。すなわち、1とすれば
推定加速度(XX2/J)が100%帰還される。
トの姿勢から決められる既知外乱トルクTfを求める。 既知外乱トルクTfには、摩擦トルク、重力の影響によ
るトルク、他軸(アーム)の干渉によるトルクが含まれ
る。これらの既知外乱トルクTfはサーボモータ22の
速度及びロボットの姿勢が決まれば計算できる。例えば
、摩擦トルクは静摩擦トルクと、速度に比例する動摩擦
トルクの和として計算できる。また、重力の影響による
トルク及び他の軸の干渉によるトルクはホストプロセッ
サ9によって、予め用意されたパラメータから計算する
ことができる。ただし、既知外乱トルクTfの単位も電
流である。
Tfを引くと、障害物との接触、衝突等の外乱トルクT
nが求められる。この外乱トルクTnは既知外乱トルク
Tfを含んでいないので、既知外乱トルクTf等のマー
ジンを取る必要はなくなる。従って、実際の衝突のとき
に発生する外乱トルクに近い値となる。この外乱トルク
Tnを比較器63で予め決めた基準のトルクTeと比較
し、外乱トルクTnが基準トルクTeを越えたら、衝突
を検出したものとし、アラームとする。さらに速度指令
も同時に0にして、サーボモータ22を急停止する。こ
の外乱トルクTnは既知外乱トルクTfを含まないので
、基準トルクTeの値は小さくとることができ、衝突を
正確に検出できる。
法のフローチャートである。図において、Sに続く数値
はステップ番号を示す。これらの処理は図2のDSP1
1によって処理される。 〔S1〕電流ループによって、電流指令値を計算し、電
流U1が出力される。 〔S2〕オブザーバ50によって、推定外乱トルクTd
を推定する。 〔S3〕既知外乱トルクTfを計算する。既知外乱トル
クTfは先に説明したように、サーボモータ22の速度
、アーム26の姿勢等によって計算される。 〔S4〕推定外乱トルクTdから既知外乱トルクTfを
引いて、外乱トルクTnを求める。 〔S5〕外乱トルクTnと予め決められた基準トルクT
eを比較し、外乱トルクTnが基準トルクTeを越えた
ときは、衝突を検出したものとして、S6へ進む。 〔S6〕衝突を検出したので、アラームを出力し、速度
指令を0にする。
トルクを引いた外乱トルクを基準トルクと比較して、衝
突を検出するようにしたので、サーボモータの速度、ロ
ボットの姿勢等による影響がなく、基準トルクの値を小
さく選択でき、より正確に衝突を検出することができる
。
、数値制御工作機械の可動部等にも適用できる。ただし
、数値制御工作機械の場合は既知トルクとして他の軸に
よる干渉は殆ど考える必要はない。一方、切削中の衝突
等を検出するときは、切削負荷等も考慮する必要がある
。
乱トルクから計算可能な既知外乱トルクを引いた外乱ト
ルクを基準トルクと比較して、衝突を検出するようにし
たので、衝突が可動部の速度、姿勢に無関係に確実に検
出できる。
チャートである。
ードウェアの構成図である。
方程式を示す図であり、図3(A)はサーボ系のブロッ
ク図、図3(B)はサーボ系の状態方式を示す図である
。
るためのブロック図である。
14 DSL(ディジタル・サーボ・LSI)21
サーボアンプ 22 サーボモータ 23 パルスコーダ 26 アーム 50 オブザーバ
Claims (4)
- 【請求項1】 サーボモータによって駆動される可動
部が障害物と衝突したことを推定外乱によって検出する
推定外乱による衝突検出方法において、前記可動部の第
1の外乱トルクを推定し、少なくとも摩擦トルク、重力
トルク、他軸の干渉トルクの1つを含む第2の外乱トル
クを計算し、前記第1の外乱トルクから、前記第2の外
乱トルクを引いた第3の外乱トルクを求め、前記第3の
外乱トルクによって、前記可動部の衝突を検出すること
を特徴とする推定外乱による衝突検出方法。 - 【請求項2】 前記可動部はロボットのアームである
ことを特徴とする請求項1記載の推定外乱による衝突検
出方法。 - 【請求項3】 前記第1の外乱トルクはオブザーバに
よって求めることを特徴とする請求項1記載の推定外乱
による衝突検出方法。 - 【請求項4】 前記衝突を検出したときは、アラーム
を出力し、速度指令を0にすることによりモータに停止
トルクを発生することを特徴とする請求項1記載の推定
外乱による衝突検出方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP3015932A JP2749724B2 (ja) | 1991-01-16 | 1991-01-16 | 推定外乱による衝突検出方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP3015932A JP2749724B2 (ja) | 1991-01-16 | 1991-01-16 | 推定外乱による衝突検出方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH04242406A true JPH04242406A (ja) | 1992-08-31 |
JP2749724B2 JP2749724B2 (ja) | 1998-05-13 |
Family
ID=11902548
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP3015932A Expired - Lifetime JP2749724B2 (ja) | 1991-01-16 | 1991-01-16 | 推定外乱による衝突検出方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2749724B2 (ja) |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09305235A (ja) * | 1996-05-10 | 1997-11-28 | Nec Corp | サーボモータの位置制御方法および制御装置 |
JPH11254380A (ja) * | 1998-03-10 | 1999-09-21 | Nachi Fujikoshi Corp | 産業用ロボットの衝突検出方法 |
JP2002283276A (ja) * | 2001-03-21 | 2002-10-03 | Daihen Corp | 多関節ロボットにおける衝突検出・停止制御法 |
JP2008183680A (ja) * | 2007-01-31 | 2008-08-14 | Yaskawa Electric Corp | 負荷機械の制御装置とその衝突検出しきい値更新方法 |
JP2010204784A (ja) * | 2009-03-02 | 2010-09-16 | Fuji Electric Systems Co Ltd | 外乱制御装置、外乱制御方法、外乱制御プログラムおよび記録媒体 |
JP2012061535A (ja) * | 2010-09-15 | 2012-03-29 | Yaskawa Electric Corp | 減速機の異常判定方法、異常判定装置、ロボット及びロボットシステム |
JP2014048930A (ja) * | 2012-08-31 | 2014-03-17 | Brother Ind Ltd | 制御装置及び画像形成システム |
US20160008978A1 (en) * | 2014-07-09 | 2016-01-14 | Fanuc Corporation | Robot control device for preventing misjudgment by collision judging part |
CN109834705A (zh) * | 2017-11-28 | 2019-06-04 | 发那科株式会社 | 机器人及其碰撞检测方法 |
-
1991
- 1991-01-16 JP JP3015932A patent/JP2749724B2/ja not_active Expired - Lifetime
Cited By (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09305235A (ja) * | 1996-05-10 | 1997-11-28 | Nec Corp | サーボモータの位置制御方法および制御装置 |
JPH11254380A (ja) * | 1998-03-10 | 1999-09-21 | Nachi Fujikoshi Corp | 産業用ロボットの衝突検出方法 |
JP2002283276A (ja) * | 2001-03-21 | 2002-10-03 | Daihen Corp | 多関節ロボットにおける衝突検出・停止制御法 |
JP2008183680A (ja) * | 2007-01-31 | 2008-08-14 | Yaskawa Electric Corp | 負荷機械の制御装置とその衝突検出しきい値更新方法 |
JP2010204784A (ja) * | 2009-03-02 | 2010-09-16 | Fuji Electric Systems Co Ltd | 外乱制御装置、外乱制御方法、外乱制御プログラムおよび記録媒体 |
US9321177B2 (en) | 2010-09-15 | 2016-04-26 | Kabushiki Kaisha Yaskawa Denki | Reducer abnormality determination method, abnormality determination device, and robot system |
JP2012061535A (ja) * | 2010-09-15 | 2012-03-29 | Yaskawa Electric Corp | 減速機の異常判定方法、異常判定装置、ロボット及びロボットシステム |
CN102431043A (zh) * | 2010-09-15 | 2012-05-02 | 株式会社安川电机 | 减速器的异常判定方法、装置和机器人及机器人系统 |
JP2014048930A (ja) * | 2012-08-31 | 2014-03-17 | Brother Ind Ltd | 制御装置及び画像形成システム |
US20160008978A1 (en) * | 2014-07-09 | 2016-01-14 | Fanuc Corporation | Robot control device for preventing misjudgment by collision judging part |
DE102015008577A1 (de) | 2014-07-09 | 2016-01-14 | Fanuc Corporation | Robotersteuervorrichtung zum Verhindern von Fehlbeurteilungen durch Kollisionsbeurteilungsteil |
DE102015008577B4 (de) * | 2014-07-09 | 2016-12-15 | Fanuc Corporation | Robotersteuervorrichtung zum Verhindern von Fehlbeurteilungen durch Kollisionsbeurteilungsteil |
US9701014B2 (en) | 2014-07-09 | 2017-07-11 | Fanuc Corporation | Robot control device for preventing misjudgment by collision judging part |
CN109834705A (zh) * | 2017-11-28 | 2019-06-04 | 发那科株式会社 | 机器人及其碰撞检测方法 |
JP2019098413A (ja) * | 2017-11-28 | 2019-06-24 | ファナック株式会社 | ロボットとその衝突検知方法 |
US11130234B2 (en) | 2017-11-28 | 2021-09-28 | Fanuc Corporation | Robot and collision detection method therefor |
Also Published As
Publication number | Publication date |
---|---|
JP2749724B2 (ja) | 1998-05-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US5719479A (en) | Collision detecting method | |
US5304906A (en) | Collision detecting method using an observer | |
KR0144650B1 (ko) | 서보모터의 이상부하 검출 제어방법 | |
JP2732159B2 (ja) | 異常負荷検出方法 | |
JP2665984B2 (ja) | 外乱推定オブザーバによる衝突検出方法 | |
JP2573120B2 (ja) | 同期形交流サーボモータの速度制御方法及び制御装置 | |
JP3212571B2 (ja) | 産業用ロボット | |
US5637969A (en) | Vibration control method | |
JP2749724B2 (ja) | 推定外乱による衝突検出方法 | |
JPH0751991A (ja) | 工具破損防止方式 | |
JP2000052286A (ja) | ロボットの制御装置 | |
JP2875646B2 (ja) | バックラッシ加速補正装置 | |
JP2004364396A (ja) | モータの制御装置および制御方法 | |
JP3204207B2 (ja) | ロボットの制御装置とその制御方法 | |
JP4055090B2 (ja) | ロボットの制御装置 | |
JPH1170490A (ja) | 産業用ロボットの衝突検出方法 | |
JP2001051721A (ja) | 衝突検出装置 | |
JP2870482B2 (ja) | サーボモータの位置制御方法および制御装置 | |
EP0604663A1 (en) | Method for estimating inertia and disturbance torque, and method for detecting abnormal load | |
JP3997201B2 (ja) | 産業用ロボットおよびその異常判断方法 | |
JPH04142604A (ja) | サーボモータの制御方式 | |
JPH06131050A (ja) | サーボモータで駆動される可動部の衝突検出方法 | |
JPH0580805A (ja) | Pi制御ループをベースにした適応スライデイングモード制御方式 | |
JP2001353687A (ja) | ロボットの制御装置とその制御方法 | |
JP2000099105A (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: 20080220 Year of fee payment: 10 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090220 Year of fee payment: 11 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090220 Year of fee payment: 11 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100220 Year of fee payment: 12 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100220 Year of fee payment: 12 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110220 Year of fee payment: 13 |