WO1991010181A1 - Method of detecting collision using observer - Google Patents

Description

明 細 書

オブザーバを用いた衝突検出方法

技 術 分 野

本発明は、 サーポモー タで駆動される被駆動体と障害 物との衝突をオブザーバを用いて迅速かつ確実に検出で きる衝突検出方法に関する。

背 景 技 術

π ボ ッ ト， 工作機械等の運転中、 被駆動体例えばロボ ッ ト アー ム， 機械テー ブル等の機械作動部又はテーブル に装着したワ ークが、 障害物例えば被駆動体の作動領域 に入り込んだ異物に衝突する こ とがある。 この場合、 サ - ボモ -タを駆動源とする機械では、 被駆動体が障害物 に衝突して移動停止した後もサ ―ボモータ は被駆動体を 指令位置まで移動させるベく大きい出力 ト ルクを発生す る。 こ のため、 サ―ボモータを含む機械構成部品が破損 する に至った り、 2次災害を生 じる こ とがある。 従来、 斯かる不具合を解消するため、 種々の方法で衝突発生を 検出し、 衝突発生時にモータ回転を停止させている。

例えば、 被駆動体の外面に取り付けたタ ツ チセ ンサを 用いて被駆動体と障害物との衝突を検出する こ とが知ら れている。 しかし、 セ ンサの設置に伴って機械のコス ト が増大する と共にセ ンサの作動領域外における被駆動体 と障害物との衝突を検出できない。 また、 被駆動体をサ - ボモータで駆動する機械において、 被駆動体と障害物 との衝突に伴つて増大するサー ボモータ駆動電流が所定 値を越えたと きに衝突発生を検出する こ とが知られてい る。 しかし、 衝突発生時点からサーボモータ駆動電流が 実際に所定値に到達するまでに時間を要するので、 検出 遅れが生じ、 被害が大き く なる。 更に、 被駆動体の指令 位置と実際位置との差 （位置偏差） が許容値を上回る過 大な値に達したと きに、 衝突発生を判別してアラ ームを 発する こ と も公知である。 しか し、 衝突時点から位置偏 差が許容値を上回るに至るまでに機械構成部品が破壊す る こ と力 ある。

そ こで、 衝突発生を迅速に検出すべく、 速度偏差の微 分値又は ト ルク指令値の微分値が閾値を上回った ときに 衝突発生を判別する方法が提案されている （曰本国特願 昭 6 3 — 3 2 8 6 6号参照） 。 しかしながら、 速度指令 自体が小さい値をとるサーボモー タの低速回転中に衝突 が生じた場合、 速度指令と実際速度とに応じて定まる速 度偏差は比較的緩やかに増大し、 従って、 速度偏差の微 分値及びこれに対応する ト ルク指令の微分値は、 さほど 大きい値にはならない。 このため、 衝突発生を迅速に検 出するには閾値を小さい値に設定しなければならない。 この場合、 通常回転速度域において、 衝突が発生 してい ないに もかかわらず衝突発生と誤判別し易い。

発 明 の 開 示

本発明の目的は、 サーポモー タ によ り駆動される被駆 動体と障害物との衝突をオブザーバを用いて迅速かつ確 実に検出でき、 衝突による機械構成部品の破損を防止で き る衝突検出方法を提供する こ と にある。 .

上述の目的を達成するため、 本発明によれば、 サーボ 系によ り駆動制御されるサーボモー タ によ り被駆動体を 駆動する機械に適用される衝突検出方法が提供される。 この衝突検出方法は、 サ―ボ系に加わる外乱の大きさを オブザーバを用いて推定する工程と、 斯く推定した外乱 の大き さが予め設定した閾値を上回ったと きに被駆動体 が障害物に衝突したと判別する工程とを備える。

上述のよ う に、 本発明によれば、 オブザーバを用いて 推定した外乱の大き さが予め設定した閾値を上回ったと き に被駆動体が障害物に衝突したと判別するよ う にした ので、 サ―ボモー タ の低速運転中においても衝突発生を 迅速かつ確実に検出でき、 衝突による機械構成部品の破 損を防止でき る。

図 面 の 簡 単 な 説 明

第 1 図は本発明の一実施例の衝突検出方法を実施する ためのオブザーバ及び周辺要素を示すプロ ッ ク線図、 第 2 図は第 1 図のオブザーバと共に用いられるサーボ 系を示すブロ ッ ク線図、

第 3 図は第 2 図のサーボ系の う ちの、 オブザーバ構成 上対象にされる部分を示すブロ ッ ク線図、 及び

第 4 図はデジタ ルサ一ボ回路によ り実行される処理を 示すフ ロ ー チ ャ ー トである。

発明を実施するための最良の形態

本発明の一実施例による衝突検出方法が適用される機 械 （図示略） は、 被駆動体を駆動するための一つ以上の サ— ボ乇一 夕 を備えている。 サ一ボモー タの対応する一 つを駆動制御するためのサーボ系は、 第 2 図に例示する よ う に、 数値制御装置等から送出される位置指令 0 r と 実際位置 0 と の差 （位置偏差） s に比例ゲイ ン K pを乗 じて速度指令を発生するための第 1 ブロ ッ ク 1 0 と、 速 度指令と実際速度 との差 （速度偏差） に基づいて積分 定数 Κ 1 及び比例定数 Κ 2で比例積分制御を行ってモ- タ駆動電流 （ ト ルク指令） I を求めるための第 2 ブロッ ク 1 2 とを備えている。 換言すれば、 サ―ボ系は、 比例 制御を行う位置ループと、 位置ループのマイ ナー ループ と して設けられ比例積分制御を行う速度ループとを備え ている。 更に、 サーボ系は、 第 3及び第 4 ブロ ッ ク 1 4 , 1 6 で表されるサーボモータ に駆動電流 I を供給してモ ータ を速度 0 で回転させるよ う になつている。 第 5プロ ッ ク 1 8 は、 実際モー タ回転位置 ø を検出するためのパ ル ス コ ーダ等を表す。 実際モー タ回転速度 0 は、 例えば パル ス コ ーダ出力に基づいて検出される。 第 2 図中、 記 号 K t及び J はサーボモータ の ト ルク定数及びィ ナーシ ャ を夫々表し、 T及び T L はサ一ボモー タの出力 トルク 及び外乱 ト ルク を夫々表す。

本実施例の衝突検出方法を実施するため、 外乱 トルク T L を推定するためのォブザーバと推定外乱 トルクが閾 値を上回った と きにサ―ボモー夕の駆動を停止するため の手段とから成る装置が用いられる。 後で詳述するよう に、 衝突検出装置は、 好ま し く は、 第 2 図の位置ループ 及び速度ルー プな らびにオブザーバ等の夫々の機能をソ フ ト ウ ユア処理によ り達成する よ う にされたデジ夕ルサ ーボ回路からなる。

以下、 衝突検出装置について説明する。

第 3 図は、 第 2 図のサーボ系のう ちのォブザーバ構成 上対象にされる部分 （モデル） を示す。 第 3 図に示すモ デルは、 第 2 図の第 3 ブロ ッ ク 1 4 に対応する第 1 プロ ッ ク 1 4 と、 第 2 図の第 4 プロ ッ ク 1 6 に対応する第 2 及び第 3 ブロ ッ ク 1 6 a， 1 6 b とからなる。 外乱トル ク T L が時間経過につれて変化 しない場合 （ T L = 0 ) 、 第 3 図に示すモデルについて第 （ 1 ) 式で表される状態 方程式が成立する。

I

· · · ( 1 ) こ こで、 0 はモータ加速度を表 し、 及び T L は状態変 数と してのモータ速度及び外乱 ト ルクを夫々表す。 又、 I は入力と しての ト ルク指令を表し、 K t及び J はモー タ の ト ルク定数及びイ ナ一 シャ を夫々表す。 T L は外乱 ト ルク の変化率を表す。

第 （ 1 ) 式に照ら して、 外乱 ト ルク T L を推定するた めのオブザー バは第 1 図に示すよ う に構成される。 第 1 図のオ ブザー バ 5 0 は、 伝達関数が夫々 K t J , K 3， K 4 S及び 1 / S である第 1 〜第 4 ブロ ッ ク 5 1〜 5 4 を備えてい る。 ォブザーノ、' 5 0 において、 ト ルク指令 I を入力する第 1 ブロ ッ ク 5 1 の出力と第 2 プロ ッ ク 5 2 の出力と第 3 ブロ ッ ク 5 3 の出力 X との加算値が第 4 ブロ ッ ク 5 4 に供給される。 又、 実際速度 0 と第 4プロ ッ ク 5 4から送出される推定速度 V との偏差が第 2及び 第 3 ブロ ッ ク 5 2， 5 3 の各々 に供袷される。

第 1 図に示すブロ ッ ク線図から明らかなよ うに、 実際 モータ速度 及び推定モ—夕速度 Vは第 （ 2 ) 式及び第 ( 3 ) 式によ り夫々表される。

= ( I ' K t + T L ) · ( 1 / J · S )

( 2 ) v = { I - K t / J + ( Θ - v ) · Κ 3

+ ( θ - v ) · ( Κ 4 / S ) } · ( 1 S )

( 3 ) 第 （ 2 ) 式から

I = ( Θ - J - S - T L) / K t ( 4 ) を得る。

第 （ 4 ) 式を第 （ 3 ) 式に代入して

V · S = ( 0 · J · S - T L) / ' J + ( Θ - V )

K 3 + ί Θ - V ) · ( K 4 / S )

( 5 ) を得る。

第 （ 5 ) 式を整理して

S ( 一 ν ) + ( 0 — ν ) · Κ 3

+ ( θ - V ) · Κ 4 / S = T

( 6 ) を得る。

第 （ 6 ) 式から、 実際速度と推定速度との偏差 — V を表す第 （ 7 ) 式を得る。 又、 オブザー バ 5 0 の第 3 フ ロ ッ ク 5 3 の出力 X は、 第 （ 8 ) 式で表される。

T L

Θ - V =

S + K 3 + ( K 4 / S )

( 7 )

X = ( Θ - V ) K 4 S

T L K 4

J S ² + K 3 - S + K 4

• · · ( 8 ) こ こで、 極が安定するよ う にオブザーバ 5 0 の第 2， 第 3 ブロ ッ ク 5 2， 5 3 の伝達関数 K 3， K 4 を選択す る と、 第 （ 9 ) 式に示すよ う に、 オブザー バ 5 0 の第 3 ブロ ッ ク 5 3 の出力 X は外乱 ト ルク T L をイ ナ 一 シャ J で除した値に略等し く なる。

X ^ T L/ J · · · ( 9 ) 衝突検出装置は、 上述の如 く 構成したオブザーバ 5 0 に加えて、 乗算器 6 1 と、 これと協働して上記モ―タ停 止手段の一部を成す比較器 6 2 とを備えている。 乗算器 6 1 において、 オブザーバ 5 0 の第 3 ブロ ッ ク 5 3の出 力 X にイ ナ一 シ ャ J と単位系を整合させるための変換定 数 A と の積が乗じ られ、 推定外乱 ト ルク y ( TL) が 求められる。 又、 比較器 6 2 において、 推定外乱 トルク yが衝突検出判別用の閾値 T s と比較される。 比較器 6 2 は、 閾値 T s に等しいかこれよ り も大きい推定外乱ト ルク y を検出 したと きに衝突発生と判別してア ラ ームを 送出するよ う になっている。 後述のよ う に、 モー タ停止 手段は、 ア ラ ー ムに応じてモー タ の駆動を停止するよう にな っている。

閾値 T s は、 機械の各種機構部品の破壊限界に夫々対 応するモータ 出力 ト ルク の最小値よ り も小さい値に設定 されている。 このため、 サーボモータが低速回転する場 台にあっても、 機械の被駆動体が障害物に衝突したとき には、 各種機械構成部品が破損する前にアラームが発せ られる。 又、 閾値 T s は、 機械に発生する静摩擦力， 機 械のバネ系が発生する反力及び機械の運転状態に応じて 変化する重力項に夫々対応する モータ出力 ト ルク の最大 値よ り も大き い値に設定されている。 こ のため、 機械が 正常に運転され、 従って、 モー タ出力 ト ルクが通常範囲 内で変動する限り、 ア ラ ームが発せられる こ とがない。

以下、 第 4 図を参照して、 第 2図のサーボ系の位置ル 一プ及び速度ルー プの機能， ォ ブザーバ 5 0 の機能なら びに乗算器 6 1 及び比較器 6 2 を含むモータ停止手段の 機能を備える デジタルサー ボ回路 （図示略） の作動を説 明する。

デジタ ルサーボ回路に内蔵のプロセ ッ サは第 4 図に示 す処理を周期的に実行する。 即ち、 プロ セ ッサは、 各々 の処理周期において、 数値制御装置から送出された位置 指令 0 r とパ ル ス コ ー ダによ り検出した実際位置 0 とに 基づいて位置ループ処理を実行 して速度指令を算出する (ス テ ッ プ 1 0 0 ) 。 次に、 プロセ ッ サは、 フ ラ グ Fが 衝突発生を表す値 「 1 」 であるか否かを判別する （ステ ッ プ 1 0 1 〉 。 こ こでは、 フ ラ グ Fが衝突未発生を表す 値 「 0 」 に初期設定されている ので、 ス テ ッ プ 1 0 1 で の判別結果は否定となる。 この場合、 プロセ ッ サは、 ォ ブザーバ 5 0 の機能を達成する ためのソ フ ト ウ - ァ処理 を実行する。 即ち、 プロセ ッ サは、 既知のパラ メ 一夕値 K t ， J ， K 3 ， Κ 4及び前回処理周期で検出した実際 速度 な らびに前回処理周期の速度ルー プ処理で算出し た ト ルク指令 I を用いかつ第 （ 3 ) 式に従って、 前回処 理周期での推定速度 V、 即ち、 外乱 ト ルク T L が加わら ないと きの速度を算出する。 次いで、 プロセ ッ サは、 前 回処理周期での実際速度と推定速度との誤差 一 Vを求 め、 こ の算出誤差を用いかつ第 （ 8 ) 式に従ってォブザ ー バ 5 0 の第 3 プロ ッ ク出力 X に対応する値を算出する。 そ して、 乗算器 6 1 の機能を達成すべく、 プロセ ッサは 算出値 X に値 J · Aを乗じて推定外乱 ト ルク y を求める (ス テ ッ プ 1 0 2 ) 。 次に、 プロ セ ッ サは、 推定外乱ト ルク yが閾値 T s に等しいかこれよ り も大きいか否かを 判別する （ス テ ッ プ 1 0 3 ) 。 障害物によ り妨げられる こ とな く 被駆動体が正常に移動する と き、 換言すれば、 サ— ボモ— 夕 が ト ルク指令 I に正常に追従する と きは、 実際速度と推定速度との誤差 θ ' - V は小さ く、 推定外乱 ト ルク y は小さい値をと る。 一方、 被駆動体が障害物に 衝突する と速度誤差 一 V ひいては推定外乱 ト ルク y は 衝突発生を表す相当大きい値になる。 推定外乱 ト ルク yが閾値 T s より も小さいと判別する と、 プロ セ ッ サは、 速度指令と実際速度 Θ とに基づいて 速度ループ処理を行って電流指令 （ ト ルク指令:^ I を算 出し （ス テ ッ プ 1 0 6 ) 、 算出指令 I を電流ルー プに引 き渡す （ス テ ッ プ 1 0 7 ) 。 こ れによ り、 今回処理周期 での第 4 図の処理を終了する。 上述のよ う に、 閾値 T s は機械に発生する摩擦力等よ り も大きい値に設定されて おり、 従って、 推定外乱 ト ルク yが正常範囲内で変動す る限り、 衝突未発生にもかかわ らず誤って衝突発生と判 別される こ と はない。

—方、 推定外乱 ト ルク yが閾値 T s に等しいかこれよ り も大きいと ステ ッ プ 1 0 3 で判別したときには、 プロ セ ッ サは、 衝突が発生したと判別してアラームを発する と共にフ ラ グ Fを衝突発生を表す値 「 1 」 にセ ッ 卜する (ス テ ッ プ 1 0 4 ) 。 次いで、 プロセ ッ サは、 ス テッ プ 1 0 0 で算出 した速度指令の値及び速度ル-プの積分器 (図示略） の値を夫々 「 0」 に リ セ ッ ト した後 （ステツ プ 1 0 5 ) 、 上記電流指令算出ステ ッ プ 1 0 6及び電流 指令送出ステ ツ プ 1 0 7 を順次実行する。 衝突検出以降 の各処理周期では、 速度指令算出ステ ッ プ 1 0 0 に続く ス テ ッ プ 1 0 1 での判別結果が肯定とな り、 上記ステツ プ 1 0 5 〜 1 0 7 が順次実行される。 この結果、 衝突発 生が判別される と、 機械の各種機構部品が破損する前に サ―ボモータ の駆動が迅速に停止される。 なお、 衝突発 生時以外にあ つても、 サーボモータ に過大な負荷が加わ る と、 衝突発生時と同様、 モー タ の駆動が停止される。 本発明は上記実施例に限定されず、 種々の変形が可能 である。 例えば、 上記実施例ではデジタ ルサーボ回路に よ り本発明を実施したが、 その他のタ イ プのサー ボ回路 を用いても良い。

Claims

請 求 の 範 囲

1 . サーボ系により駆動制御されるサーボモータ により 被駆動体を駆動する機械に適用される衝突検出方法に おいて、 前記サーボ系に加わる外乱の大き さをォブザ —バを用いて推定し、 斯く推定した外乱の大き さが予 め設定した閾値を上回ったと きに被駆動体が障害物に 衝突したと判別する こ とを特徴とするォブザーバを用 いた衝突検出方法。

2 . 前記外乱は外乱 ト ルクである請求の範囲第 1 項記載 のォブザーバを用いた衝突検出方法。

3 . 前記サー ボ系の位置ループのマイ ナーループとして 設けられた速度ループに関連して構成したォブザーバ を用いて前記外乱 ト ルク の推定を行う請求の範囲第 2 項記載のオ ブザーバを用いた衝突検出方法。

4 . 前記閾値は、 前記機械の構成部品の破壞限界に対応. するサーボモータ出力 ト ルク よ り も小さい値に設定さ れる請求の範囲第 2項記載のォブザ-バを用いた衝突 検出方法。

5 . 前記閾値は、 前記機械の正常運転中に生じるサーボ モータ出力 ト ルク よ り も大きい値に設定される請求の 範囲第 2項記載のォブザーバを用いた衝突検出方法。