JPH0719176B2

JPH0719176B2 - サーボ制御装置

Info

Publication number: JPH0719176B2
Application number: JP62277767A
Authority: JP
Inventors: 晃福田
Original assignee: Daikin Industries Ltd
Current assignee: Daikin Industries Ltd
Priority date: 1987-11-02
Filing date: 1987-11-02
Publication date: 1995-03-06
Anticipated expiration: 2010-03-06
Also published as: JPH01119808A

Description

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

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

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

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

（問題点を解決するための手段）上記目的を達成するため本発明の解決手段は、第１図に
示すように、ロボットのアーム手先を駆動するためのサ
ーボモータ（20）と、上記ロボットのアーム手先の移動
経路に沿った加減速曲線についての基本的な速度計画を
立案する基本計画立案手段と、上記サーボモータ（20）
により駆動されるロボットのアーム手先の位置と基本的
な速度計画に基づく軌跡との位置偏差を検出して位置偏
差信号を出力する位置偏差検出手段（７）と、該位置偏
差検出手段（７）の出力を受け、位置偏差信号に応じた
制御信号を出力して該サーボモータ（20）の回転を制御
する制御手段（30）とを備え、上記位置偏差検出手段
（７）からの過大な位置偏差信号に応じて上記サーボモ
ータ（20）を減速，停止させるように構成されたサーボ
制御装置を前提とする。

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

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

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

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

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

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

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

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

第３図は、上記制御系の信号変換器（18）における信号
変換を図式的に示し、他の要素等は簡略化している。す
なわち、上記信号変換器（18）は、入力を等価に変換す
ると共に所定の絶対値以上の入力は飽和させるリミッタ
ー（22）と、振動等の高周波のノイズを除去するための
ローパスフィルタ（23）とを内蔵している（いずれも図
中枠内図示）。該ローパスフィルタ（23）は、通常の動
作時には、モータの負荷に応じて変化する時定数τに対
応する遮断周波数ｆ以上の高周波成分をカットし、後述
の一定の場合には、モータの最大負荷時の時定数よりも
大きく設定された大きい時定数τ₁に対応する低い遮断
周波数f₁以上の高周波成分をカットするようになされて
いる。なお、第３図において、G₁，G₂はそれぞれフィー
ドバックループおよびパワーアンプのゲイン、（16）は
枠内図示の変換特性を有する総合フィードバックループ
要素である。

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

次に、上記制御手段（30）による制御内容について、第
４図のフローチャートに基づき説明するに、ステップS₁
で上記第１加え合せ点（７）でロボットのアーム手先の
位置と基本的な速度計画に基づく軌跡との間の位置偏差
を演算し、ステップS₂で、上記ステップS₁で算出した位
置偏差の値が過大か否かを判別する。そして、その判別
結果が位置偏差の値が通常値であるNOであるときにはそ
のままで、過大であるYESのときにはステップS₃でエラ
ーフラグを“1"に立ててそれぞれステップS₄に進む。ス
テップS₄では、エラーフラグが“1"になっているか否か
を判別し、判別結果がNOであれば通常の動作を行ってい
ると判断して、ステップS₅で上記ローパスフィルタ（2
3）の時定数を通常の可変なτにし、モータ（20）の負
荷に応じて可変に調節してステップS₇に進む。一方、上
記ステップS₄における判別がエラーフラグが“1"のYES
であるときには、位置偏差が過大であると判断して、ス
テップS₆でローパスフィルタ（23）の時定数をモータ
（20）の最大負荷次よりも大きい所定の値τ₁に設定
し、ステップS₇に進む。そして、ステップS₇で、上記ス
テップS₅，S₆で設定した時定数に応じたローパスフィル
タ（23）のフィルタリングを行って制御を終了する。

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

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

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

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

【図面の簡単な説明】

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

Claims

【特許請求の範囲】

【請求項１】ロボットのアーム手先を駆動するためのサ
ーボモータ（20）と、上記ロボットのアーム手先の移動
経路に沿った加減速曲線についての基本的な速度計画を
立案する基本計画立案手段と、上記サーボモータ（20）
により駆動されるロボットのアーム手先の位置と基本的
な速度計画に基づく軌跡との位置偏差を検出して位置偏
差信号を出力する位置偏差検出手段（７）と、該位置偏
差検出手段（７）の出力を受け、位置偏差信号に応じた
制御信号を出力して該サーボモータ（20）の回転を制御
する制御手段（30）とを備え、上記位置偏差検出手段
（７）からの過大な位置偏差信号に応じて上記サーボモ
ータ（20）を減速，停止させるように構成されたサーボ
制御装置において、上記制御手段（30）は、位置偏差信号の振動を除去するための時定数がモータ
（20）の負荷に応じて変化するように構成されたローパ
スフィルタ（23）と、上記位置偏差信号の値が所定値以上の過大な値となると
きを検出する過大偏差検出手段（31）と、該過大偏差検出手段（31）の出力を受け、位置偏差信号
値が過大な値のときには、上記ローパスフィルタ（23）
の時定数を上記サーボモータ（20）の最大負荷時の値よ
りも大きくするよう変更する時定数変更手段（32）とを
備えたことを特徴とするサーボ制御装置。