JPH01119808A - サーボ制御装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、速度計画に基づいたロボットの軌跡制御を行
うサーボ制御装置に係り、特にロボットのアーム手先の
位置誤差の過大時における停止・減速動作の円滑化対策
に関する。

（従来の技術） 従来より、ロボットの動作制御を行うサーボ制御装置に
おいて、アーム手先が基本計画に基づく軌跡上を移動中
に、その軌跡とアーム手先位置との間に過大な位置偏差
が生じた場合、モータへの過大な速度指令出力を避ける
ために位置指令を直ちに打切って、モータ電源をオフに
するか、或いはアームを急停止させることが一般的な処
置として行われている。

（発明が解決しようとする問題点） しかしながら、上記従来の方法による場合、各関節が急
激な減速力を受けるために、破損の招来等の悪影響を及
ぼすとともに、アーム手先にも衝撃力が加わることにな
り、把持物に対して衝撃力を与えるという問題があった
。

本発明は斯かる点に鑑みてなされたものであり、その目
的は、ロボットのアーム手先が移動中に軌跡から大きく
外れた場合、その位置誤差に基づく過大な制御電流信号
の発生を防止することにより、モータの減速・停止動作
の円滑化を図ることにある。

（問題点を解決するための手段） 上記目的を達成するため本発明の解決手段は、第１図に
示すように、ロボットのアーム手先を駆動するためのサ
ーボモータ（２０）と、該サーボモータ（２０）により
駆動されるロボットのアーム手先の位置と基本的な速度
計画に基づく軌跡との位置偏差を検出して位置偏差信号
を出力する位置偏差検出手段（７）と、該位置偏差検出
手段（７）の出力を受け、位置偏差信号に応じた制御信
号を出力して該サーボモータ（２０）の回転を制御する
制御手段（３０）とで構成されるサーボ制御装置を対象
とする。

そして、上記制御手段（３０）に、モータ（２０）の負
荷に応じて時定数を可変にして位置偏差信号の振動を除
去するローパスフィルタ（２３）と、上記位置偏差信号
の値が過大な所定値以上のときを検出する過大偏差検出
手段（３１）と、該過大偏差検出手段（３１）の出力を
受け、位置偏差信号値が過大な値のときには、上記ロー
パスフィルタ（２３）の時定数を上記サーボモータ（２
０）の最大負荷時の値よりも大きくするよう変更する時
定数変更手段（３２）とを設ける構成としたものである
。

（作用） 以上の構成により、本発明では、位置偏差検出手段（７
）により、サーボモータ（２０）によって駆動されるロ
ボットのアーム手先の位置と基本的な速度計画に基づく
軌跡との位置偏差が検出されて位置偏差信号が出力され
、制御手段（３０）により位置偏差信号値に応じた制御
信号が出力されて、アーム手先の位置が軌跡に一致する
ようにサーボモータ（２０）の回転が制御される。

その場合、アーム手先の位置が基本計画に基づく軌跡か
ら大きく外れて過大な位置偏差信号値が出力された時に
は、過大偏差検出手段（３１）によりそれが検出され、
さらに時定数変更手段（３２）により、ローパスフィル
タ（２３）の時定数がモータ（２０）の最大負荷時の時
定数よりも大きな値に変更されるので、対応する遮断周
波数の値が非常に低い値となり、大きなピーク電流を生
ぜしめるような入力信号は可及的にカットされることに
なる。よって、過大な位置偏差に基づく過大な制御電流
の発生が可及的に抑制され、通常動作時と同様な円滑な
減速・停止動作が実現されるのである。

（実施例） 以下、本発明の実施例について、図面に基づき説明する
。

第２図は、本発明を適用したロボット制御装置の信号系
を示し、（１）は入力装置（図示せず）からの命令を翻
訳してその解釈どおりに実行するインタプリタ、（２）
は該インタプリタ（１）からの命令に応じて最適なロボ
ットの運動の計画としての速度計画を立案するプランナ
、（３）は該プランナ（２）で立案された速度計画によ
る制御信号と、ロボットのアーム手先（図示せず）の実
際の位置とを比較してロボットの運動をフィードバック
制御するためのフィードバックループ部、（４）は該フ
ィードバックループ部（３）からの制御信号を受けてロ
ボットのアームを駆動するためのパルス信号を出力する
ドライバ部、（５）は該ドライバ部（４）の出力に応じ
て運動するロボットの機構部である。

そして、上記フィードバックループ部（３）において、
（７）は上記プランナ（２）からの速度計画に基づく目
標位置信号と後述の補償器（１５）からのロボットのア
ーム手先の位置信号との代数差を演算してアーム手先の
位置偏差を検出する位置偏差検出手段としての第１加え
合せ点、（８）は該第１加え合せ点（７）の演算結果に
位置ゲインに１を乗じる第１ループゲイン器、（９）は
該第１ループゲイン器（８）の乗算結果と補償器（１５
）からのロボットのアーム手先の速度信号との代数差を
演算する第２加え合せ点、（１０）は該第２加え合せ点
（９）の演算結果に速度ゲインに２を乗じてロボットの
アームを駆動するためのトルク信号を出力する第２ルー
プゲイン器である。また、（１２）はロボットのアーム
手先の位置信号を受けて実際の速度を演算する第１微分
要素、（１３）は該第１微分要素で演算されたロボット
のアーム手先の速度をさらに微分して加速度を演算する
第２微分要素、（１４）は上記ロボットのアーム手先の
位置信号と、第１微分要素（１２）で演算された速度と
、第２微分要素（１３）で演算された加速度とを加算し
ていわゆる速度のＰＩＤ成分を演算する第４加え合せ点
、（１５）は該第４加え合せ点（１４）からのＰＩＤ信
号のゲインを補償するための補償器である。

次に、上記ドライバ部（４）において、（１７）は上記
第２加え合せ点（１０）からのトルク信号と、上記機構
部（５）を駆動するための電流のフィードバック信号と
の代数差を演算して位相遅れを補償する第３加え合せ点
、（１８）は該第３加え合せ点（１７）からのトルク信
号をパルス信号に変換していわゆるＰＷＭ信号を出力す
る信号変換器である。

そして、上記機構部（５）において、（１９）は上記信
号変換器（１８）からのＰＷＭ信号を増幅するためのパ
ワーアンプ、（２０）は該パワーアンプ（１９）で増幅
されたＰＷＭ信号に応じて回転することによりロボット
のアーム手先を駆動するサーボモータ、（２１）は該サ
ーボモータ（２０）により駆動される関節ダイナミクス
としての積分要素である。

第３図は、上記制御系の信号変換器（１８）に□おける
信号変換を図式的に示し、他の要素等は簡略化している
。すなわち、上記信号変換器（１８）は、入力を等価に
変換すると共に所定の絶対値以上の入力は飽和させるリ
ミッタ−（２２）と、振動等の高周波のノイズを除去す
るためのローパスフィルタ（２３）とを内蔵している（
いずれも図中枠内図示）。該ローパスフィルタ（２３）
は、通常の動作時には、モータの負荷に応じて変化する
時定数τに対応する遮断周波数ｆ以上の高周波成分をカ
ットし、後述の一定の場合には、モータの最大負荷時の
時定数よりも大きく設定された大きい時定数τ１に対応
する低い遮断周波数ｆ１以上の高周波成分をカットする
ようになされている。

なお、第３図において、Ｇｌ　＋　０２はそれぞれフィ
ードバックループおよびパワーアンプのゲイン、（１６
）は枠内図示の変換特性を有する総合フィードバックル
ープ要素である。

上記制御系において、ロボットのアーム手先の移動経路
等の指令は上記インタプリタ（１）で翻訳され、その翻
訳された指令に基づいてプランナ（２）で最適な速度計
画が立案されると、該速度計画に応じてフィードバック
ループ部（３）で制御信号が出力され、次に、ドライバ
部（４）でＰＷＭ倍信号変換されて機構部（５）が駆動
され、ロボットのアーム手先の運動が制御されるように
なされている。上記フィードバックループ部（３）およ
びドライバ部（４）により、上記第１加え合せ点（位置
偏差検出手段）（７）で検出された位置偏差に応じて上
記サーボモータ（Ｍ）の回転を制御する制御手段（３０
）が構成されている。

次に、上記制御手段（３０）による制御内容について、
第４図のフローチャートに基づき説明するに、ステップ
Ｓ１で上記第１加え合せ点（７）でロボットのアーム手
先の位置と基本的な速度計画に基づく軌跡との間の位置
偏差を演算し、ステップＳ２で、上記ステップＳ１で算
出した位置偏差の値が過大か否かを判別する。そして、
その判別結果が位置偏差の値が通常値であるＮｏである
ときにはそのままで、過大であるＹＥＳのときにはステ
ップＳ３でエラーフラグを１″に立ててそれぞれステッ
プＳ４に進む。ステップＳ４では、エラーフラグが“１
”になっているか否かを判別し、判別結果がＮｏであれ
ば通常の動作を行っていると判断して、ステップＳ５で
上記ローパスフィルタ（２３）の時定数を通常の可変な
τにし、モータ（２０）の負荷に応じて可変に調節して
ステップＳ７に進む。一方、上記ステップＳ４における
判別がエラーフラグが“１″のＹＥＳであるときには、
位置偏差が過大であると判断して、ステップＳ６でロー
パスフィルタ（２３）の時定数をモータ（２０）の最大
付加炭よりも大きい所定の値τ１に設定し、ステップＳ
７に進む。そして、ステップＳ７で、上記ステップＳ５
．ＳＧで設定した時定数に応じたローパスフィルタ（２
３）のフィルタリングを行って制御を終了する。

よって、上記ステップＳ４により、」二記位置偏差が過
大な所定値以上のときを検出する過大偏差検出手段（３
１）が構成され、ステップＳ６により、」二記過大偏差
検出手段（３１）の出力を受け、位置偏差信号値が過大
な値のときにはローパスフィルタの時定数を上記サーボ
モータ（２０）の最大負荷時の値よりも大きくする変更
する時定数変更手段（３２）が構成されている。

したがって、上記実施例では、アーム手先の位置が基本
計画に基づく軌跡から大きく外れて過大な位置偏差が生
じた時、時定数変更手段（３２）により、制御系に内蔵
されたローパスフィルタ（２３）の時定数がモータ（２
０）の最大負荷時の時定数よりも大きな値τ１に変更さ
れるので、対応する遮断周波数の値ｆ１が非常に低い値
となり、大きなピーク電流を生せしめるような入力信号
は可及的にカットされることになる。よって、過大な位
置偏差に基づく過大な制御電流の発生が可及的に抑制さ
れ、通常動作時と同様な円滑な減速・停止動作が実現さ
れるのである。

また、そのことにより、関節等ロボット自体の破損を防
止し、把持物への衝撃力の軽減化を図ることができるの
である。

（発明の効果） 以上説明したように、本発明のサーボ制御装置によれば
、制御系内のローパスフィルタの時定数を可変なものと
し、ロボットのアーム手先の位置が基本速度計画に基づ
く軌跡から大きくずれたときには、時定数をモータの最
大負荷時のそれよりも大きく変更するようにしたので、
過大な位置偏差に起因する過大な制御電流の発生を可及
的に防止することができ、モータの減速・停止動作の円
滑化を図ることができる。

【図面の簡単な説明】

図面は本発明の実施例を示し、第１図はロボットのアー
ム手先の移動する経路図、第２図はロボットの制御装置
の構成を示すブロック線図、第３図は信号変換器の変換
特性を示す図、第４図は制御手段による制御の内容を示
すフローチャート図である。 （７）・・・位置偏差検出手段、（２０）・・・サーボ
モータ、（２３）・・・ローパスフィルタ、（３０）・
・・制御手段、（３１）・・・過大偏差検出手段、（３
２）・・・時定数変更手段。

Claims (1)

【特許請求の範囲】
1. （１）ロボットのアーム手先を駆動するためのサーボモ
   ータ（２０）と、該サーボモータ（２０）により駆動さ
   れるロボットのアーム手先の位置と基本的な速度計画に
   基づく軌跡との位置偏差を検出して位置偏差信号を出力
   する位置偏差検出手段（７）と、該位置偏差検出手段（
   ７）の出力を受け、位置偏差信号に応じた制御信号を出
   力して該サーボモータ（２０）の回転を制御する制御手
   段（３０）とで構成されるサーボ制御装置において、上
   記制御手段（３０）は、モータ（２０）の負荷に応じて
   時定数を可変にして位置偏差信号の振動を除去するロー
   パスフィルタ（２３）と、上記位置偏差信号の値が過大
   な所定値以上のときを検出する過大偏差検出手段（３１
   ）と、該過大偏差検出手段（３１）の出力を受け、位置
   偏差信号値が過大な値のときには、上記ローパスフィル
   タ（２３）の時定数を上記サーボモータ（２０）の最大
   負荷時の値よりも大きくするよう変更する時定数変更手
   段（３２）とを備えたことを特徴とするサーボ制御装置
   。