JP2521458B2 - マニピユレ−タの防振制御装置 - Google Patents

Description

【発明の詳細な説明】 〈産業上の利用分野〉 本発明は、マニピュレータの防振制御装置に関する。

〈従来の技術〉 一般に、マニピュレータにおいては高速化および高精
度化が要望されている。高精度化に着目した場合、回転
駆動機（モータ）から被駆動物までの伝達機構の剛性を
大きくすることによって実施することができるが、しか
し、伝達機構の剛性を大きくすれば、モータおよび伝達
機構が大型化するために、高速化が阻害される。このよ
うに、高速化および高精度化という相反する問題に対処
するために、マニピュレータの用途に応じて高速化また
は高精度化のいずれかを優先的に採用して、他方の問題
を付随的に考えなければならない。

第14図は、従来のマニピュレータの制御装置のブロッ
ク図である。同図において、 １は、被駆動機構９の移動開始、移動速度、停止位置
等の動作指令設定信号r1を出力する動作指令信号設定回
路である。

２は、動作指令設定信号r1とモータ回転位置を検出す
るエンコーダ12の位置検出信号r12とを比較して位置補
償信号r2を出力する位置信号比較回路である。

３は、位置補償信号r2を入力して、速度指令信号r3を
出力する速度指令信号回路である。

４は、速度指令信号r3とモータ速度検出信号r11とフ
ィルタ出力信号r23とを比較して速度補償信号r4を出力
する速度信号比較回路である。

５は、速度補償信号r4を入力して、モータ駆動電流指
令信号r5を出力するモータ駆動電流指令信号回路であ
る。

６は、モータ駆動電流指令信号r5とモータ電流検出信
号r10とを入力して比較し、モータ駆動電流補償信号r6
を出力するモータ電流信号比較回路である。

７は、モータ駆動電流補償信号r6を増幅して、モータ
駆動電流Imを出力するサーボアンプである。

８は駆動力を発生して伝達する駆動力伝達側のモータ
であり、９は駆動力が伝達される駆動力被伝達側の被駆
動機構である。

10は、モータ駆動電流Imを検出してモータ電流検出信
号r10を出力するモータ電流検出器である。

11は、モータの駆動速度を検出してモータ速度検出信
号r11を出力するモータ速度検出器11である。

12は、モータ回転角度を検出して被駆動機構の位置に
対応した位置検出信号r12を出力するエンコーダであ
る。

21は、被駆動機構９に取り付けられて被駆動機構の加
速度βを検出して加速度検出信号r21を出力する加速度
検出器である。

22′は、加速度検出信号r21をゲイン調整して加速度
検出信号に対応したゲイン調整信号r22′を出力するゲ
イン調整回路である。

23は、ゲイン調整信号r22′を入力して高周波成分を
除去したフィルタ出力信号r23を出力するフィルタ回路
である。

上記の従来の制御装置において、符号６乃至８および
10で示すブロックは、被駆動機構９を駆動するマニピュ
レータの制御装置を示し、さらに、マニピュレータの制
御装置に、符号１乃至５および11,12のブロックを追加
した構成は、設定位置まで自動的に被駆動機構９を駆動
するロボットの制御装置を示す。また、符号21,22′お
よび23のブロックは、被駆動機構９のハンチング抑制回
路を示す。

被駆動機構９、例えばロボットのアームは、モータ８
によって駆動され、モータの駆動速度を検出してモータ
速度検出信号r11を出力するモータ速度検出器11と、モ
ータ回転角度を検出して被駆動機構の位置に対応した位
置検出信号r12を出力するエンコーダ12とが取り付けら
れている。動作指令信号設定回路１に設定された停止さ
せる位置（停止位置）までの動作指令設定信号r1は、位
置信号比較回路２によってエンコーダ12の位置検出信号
r12と比較して、位置補償信号r2を出力する。この位置
補償信号r2は、速度指令信号回路３によって速度指令信
号r3に変換され、この速度指令信号r3と、モータ速度検
出信号r11と、後述するフィルタ出力信号r23とが速度信
号比較回路４に入力され、速度補償信号r4が出力され
る。

この速度補償信号r4は、モータ駆動電流指令信号回路
５によってモータ速度に対応するモータ駆動電流指令信
号r5に変換され、この指令信号r5とモータ電流検出信号
r10とがモータ電流信号比較回路６に入力されてモータ
駆動電流補償信号r6が出力される。モータ電流信号比較
回路６とサーボアンプ７とモータ電流検出器10とは、電
流ループを形成し、過大なモータ電流を抑制する。

また、速度信号比較回路４、モータ駆動電流指令信号
回路５、モータ電流信号比較回路６、サーボアンプ７、
モータ８およびモータ速度検出器11は、速度ループを形
成し、過大なモータ速度を抑制する。

さらに、位置信号比較回路２、速度指令信号回路３、
上述した符号４乃至８のブロック及びエンコーダ12は、
位置ループを形成し、設定した停止位置に被駆動機構９
を停止させる。

被駆動機構に取付けられた加速度検出器21の加速度検
出信号r21は、ゲイン調整回路22′によってゲイン調整
され、さらにフィルタ回路23によって高周波成分が除去
され、加速度検出信号に対応したフィルタ出力信号r23
が速度信号比較回路４に加算入力されて、被駆動機構９
のハンチングを抑制しようとするものである。

〈発明が解決しようとする問題点〉 第14図で説明した従来の装置においては、被駆動機構
９の移動により発生する加速度に対応した加速度検出信
号r21をゲイン調整してフィルタ回路23による高周波成
分を除去した信号を速度信号比較回路４にフィードバッ
クしているだけなので、加速度検出信号r21が増加すれ
ばモータ８の速度補償信号r4を減少させてモータ速度を
抑制しようとするものである。

このような従来の装置においては、被駆動機構９を、
例えば停止から速度を増加させて移動させようとする場
合、加速度検出器21の加速度検出信号r21がプラス方向
に発生し、この信号を速度信号比較回路４にフィードバ
ックして、モータの速度補償信号r4を減少させることに
よりモータの速度を減少させるように動作する。したが
って、モータ駆動速度と被駆動機構を移動させようとす
る速度との差を減少させる方向に動作するので、振動の
発生を抑制する方向に動作すると同時に、モータ駆動速
度の増加を抑制する方向に動作する。したがって、被駆
動機構９を短時間に移動させることができないので、加
速時間が長くなり、応答性が悪くなる欠点があった。逆
に、被駆動機構９を移動状態から停止させようとする場
合、加速度検出器21の加速度検出信号r21がマイナス方
向に発生し、この信号を速度信号比較回路４にフィード
バックするときは、モータの速度補償信号r4を増加させ
てモータ速度を増加させるように動作するので、振動の
発生を抑制する方向に動作すると同時に、モータ駆動速
度の減少を抑制する方向に動作する。したがって、被駆
動機構９を短時間に停止させることができないので、減
速時間が長くなり、応答性が悪くなるという欠点があっ
た。

即ち、被駆動機構９の加速度を検出するだけでは、後
述する第４図のモータ軸とアーム92とが連成して振動す
る連成振動系のモータ軸の振動状態までを知ることはで
きない。そのために、アーム92の振動を抑制するための
モータ速度制御をすることができない。

そこで、本発明の目的は、マニピュレータの被駆動機
構（アーム）に取り付けた加速度検出器の実際のアーム
の加速度と駆動機構の実際のモータ駆動電流とから、被
駆動機構に生じる振動を減衰させるための減衰力を発生
させるモータ速度を推定して、その推定した減衰モータ
速度をフィードバックしてモータ速度を制御し、モータ
軸の振動ではなく被駆動機構（アーム）の振動を減衰さ
せるマニピュレータの防振制御装置を提供することにあ
る。

〈問題点を解決するための手段〉 上記の目的を達成するために、特許請求の範囲の第１
項の構成は、 駆動力を発生して伝達する駆動力伝達側のモータ８
と、 駆動力が伝達される駆動力被伝達側の被駆動機構９
と、 モータ８および被駆動機構９を連結する伝達機構91
と、 モータ駆動電流Imを検出してモータ電流検出信号r10
を出力するモータ電流検出回路10と、 被駆動機構９に取り付けられて被駆動機構の加速度β
を検出して加速度検出信号r21を出力する加速度検出器2
1と、 加速度検出信号r21とモータ電流検出信号r10とを入力
し、モータ電流検出信号r10の変化状態に対応した加速
度検出信号r21の変化状態を判別して、被駆動機構９に
生じる振動を減衰させる減衰モータ速度を推定した減衰
モータ速度推定信号Xhdを出力する状態観測回路30と、 減衰モータ速度推定信号Xhdを入力し、減衰力を増大
させるために付加して減衰トルクT0の大きさを決定し、
減衰係数C0＝T0/Xhdを調整して減衰係数レベル調整信号
r22を出力する減衰係数レベル調整回路22と、 減衰係数レベル調整信号r22を入力し、減衰係数レベ
ル調整信号r22の振幅および位相を、被伝達側の被駆動
機構９の加速度βと伝達側のモータのトルクＴとの比の
伝達関数β/Tの真の値に近づけるための補償をして減衰
モータ駆動電流信号r31を出力する減衰モータ駆動電流
信号回路31と、 被駆動機構９の動作指令に対応したモータ駆動電流指
令信号r5とモータ電流検出信号r10と減衰モータ駆動電
流信号r31とを入力して、モータ駆動電流補償信号r6を
出力するモータ電流信号比較回路６と、 モータ駆動電流補償信号r6を入力して、モータの駆動
電流Imを出力するサーボアンプ７とから成り、 モータ８にモータ駆動電流Imを供給し、モータ速度を
制御して、被駆動機構９の振動を減衰させるマニピュレ
ータの防振制御装置である。

なお、上記の伝達関数とは、伝達側のモータの出力状
態から被伝達側の被駆動機構９の移動速度の変化状態ま
での伝達特性を数式に置換したものであって、 JISZ9212では、伝達関数とは、出力信号と入力信号の
間の関係を表す関数的関係。線形系においては、出力信
号の入力信号に対する比を、すべての初期条件を零とし
てラプラス変換し、演算子ｓの関数の形に表したもので
あると定義されている。

特許請求の範囲の第２項の構成は、 駆動力を発生して伝達する駆動力伝達側のモータ８
と、 駆動力が伝達される駆動力被伝達側の被駆動機構９
と、 モータ８および被駆動機構９を連結する伝達機構91
と、 モータ駆動電流Imを検出してモータ電流検出信号r10
を出力するモータ電流検出回路10と、 被駆動機構９に取り付けられて被駆動機構の加速度β
を検出して加速度検出信号r21を出力する加速度検出器2
1と、 加速度検出信号r21とモータ電流検出信号r10とを入力
し、モータ電流検出信号r10の変化状態に対応した加速
度検出信号r21の変化状態を判別して、被駆動機構９に
生じる振動を減衰させる減衰モータ速度を推定した減衰
モータ速度推定信号Xhdを出力する状態観測回路30と、 減衰モータ速度推定信号Xhdを増幅して減衰モータ駆
動電流Inを出力するモータ駆動補償電流回路32と、 被駆動機構９の動作指令に対応したモータ駆動電流指
令信号r5とモータ電流検出信号r10とを入力して、モー
タ駆動電流補償信号r6を出力するモータ電流信号比較回
路６と、 モータ駆動電流補償信号r6を増幅してモータ駆動電流
Imを出力するサーボアンプ７とから成り、 モータ８にモータ駆動電流Imおよび減衰モータ駆動電
流Inを供給し、モータ速度を制御して、被駆動機構９の
振動を減衰させるマニピュレータの防振制御装置であ
る。

〈作 用〉 （１）特許請求の範囲第１項の制御装置は、 a.後述する第４図に示すように、モータ８および被駆動
機構９およびモータと被駆動機構とを連結する伝達機構
91とからなる制御対象の振動系をモデル化して、この振
動系モデル9aの振動状態を推定する状態観測回路30に、
加速度検出信号r21とモータ電流検出信号r10とを入力
し、モータ電流検出信号r10の変化状態に対応した加速
度検出信号r21の変化状態を判別して、被駆動機構９に
生じる振動を減衰させる減衰モータ速度を推定して減衰
モータ速度推定信号Xhdを出力し、 b.この減衰モータ速度推定信号Xhdを減衰係数レベル調
整回路22に入力し、減衰力を増大させるために付加して
減衰トルクT0の大きさを決定し、減衰係数C0＝T0/Xhdを
調整する減衰係数レベル調整信号r22を出力し、 c.この減衰係数レベル調整信号r22を減衰モータ駆動電
流信号回路31に入力して、減衰係数レベル調整信号r22
の振幅および位相を、伝達側のモータのトルクＴから被
伝達側の被駆動機構９の加速度βまでの伝達関数β/Tを
真の値に近づけるための補償をして減衰モータ駆動電流
信号r31を出力し、 d.この減衰モータ駆動電流信号r31とモータ駆動電流指
令信号r5とモータ電流検出信号r10とをモータ電流信号
比較回路６に入力して比較し、モータ駆動電流補償信号
r6を出力し、 e.このモータ駆動電流補償信号r6をサーボアンプ７に入
力して増幅し、モータ駆動電流Imを出力して、 f.モータ速度を制御して、被駆動機構の移動速度に対応
したモータの速度と被駆動機構の移動速度とを一致させ
ることにより、被駆動機構の加速度または減速を妨げる
ことなく、マニピュレータの被駆動機構（アーム）の実
際の加速度の変動を小さくして、モータ軸の振動ではな
く被駆動機構の振動を減衰させる。

（２）さらに、特許請求の範囲第２項の制御装置は、 a.上記の特許請求の範囲第１項の制御装置と同様に、モ
ータ８および被駆動機構９およびモータと被駆動機構と
を連結する伝達機構91からなる制御対象の振動系をモデ
ル化して、この振動系モデル9aの振動状態を推定する状
態観測回路30に、加速度検出信号r21とモータ電流検出
信号r10とを入力し、モータ電流検出信号r10の変化状態
に対応した加速度検出信号r21の変化状態を判別して、
被駆動機構９に生じる振動を減衰させる減衰モータ速度
を推定して減衰モータ速度推定信号Xhdを出力し、 g.この減衰モータ速度推定信号Xhdを減衰モータ駆動電
流回路32に入力して増幅し、減衰モータ駆動電流Inを出
力し、 h.この減衰モータ駆動電流Inとモータ駆動電流Imとをモ
ータ電流加算回路33に加算入力してモータに供給し、 f.モータ速度を制御して、被駆動機構の移動速度に対応
したモータの速度と被駆動機構の移動速度とを一致させ
ることにより、被駆動機構の加速または減速を妨げるこ
となく、マニピュレータの被駆動機構（アーム）に取り
付けた加速度検出器の実際のアームの加速度の変動を小
さくして、被駆動機構の振動を減衰させる。

〈実施例〉 以下、本発明を図示の実施例により詳細に説明する。

第１図は、本第１の発明の実施例のブロック図あっ
て、例えば第３図に示すように、マニピュレータを水平
多関節式の産業ロボットに適用する。第３図において、
41は支柱40に対して上下自在に調整して位置決めされた
枠体、８は枠体41に支持された回転駆動機（モータ）、
11および12はモータ８に連結されたモータ速度検出器
（タコメータジェネレータ）および位置検出器（エンコ
ーダ）、91はモータ８の出力側に配設された減速機、92
は減速機91の出力軸に連結された第１のアーム92で、こ
の減速機91と第１のアーム92とから被駆動機構９が構成
されている。なお、第１のアーム92の自由端部にはモー
タ42によって駆動される第２のアーム43が支持され、第
２のアーム43の自由端部には上下方向または水平旋回な
ど自在に移動する操作要素44が支持されている。実施例
では、第１のアームのみに本発明を適用している。

（第１実施例の構成） 第１図において、符号１乃至12は前述した第14図に示
される従来装置を構成するブロックと同一であるので、
符号および作用の説明を省略する。

30は、後述する状態観測回路であって、被駆動機構９
に取付けられた加速度検出器21、例えば圧電形の検出器
から出力される加速度検出信号r21とモータ電流検出信
号r10とを入力し、モータ電流検出信号r10の変化状態に
対応した加速度検出信号r21の変化状態を判別して、被
駆動機構９に生じる振動を減衰させる減衰モータ速度を
推定して減衰モータ速度推定信号Xhdを出力する。

22は、減衰係数レベル調整回路であって、減衰モータ
速度推定信号Xhdを入力し、減衰力を増大させるために
付加して減衰トルクT0の大きさを決定し、減衰係数C0＝
T0/Xhdを調整して減衰係数レベル調整信号r22を出力す
る。31は、減衰モータ駆動電流信号回路であって、減衰
係数レベル調整信号r22を入力して、減衰係数レベル調
整信号r22の振幅および位相を、被伝達側の被駆動機構
９の加速度βと伝達側のモータのトルクＴとの比β/Tの
伝達関数の真の値に近づける補償をして減衰モータ駆動
電流信号r31を出力する。

加速度検出器21、状態観測回路30、減衰係数レベル調
整回路22および減衰モータ駆動電流信号回路31により、
防振制御回路20が形成される。

（第２実施例の構成） 第２図は、本第２の発明の実施例のブロック図であ
る。同図において、符号１乃至12は、前述した第14図に
示される従来装置を構成するブロックと同一であるので
符号および作用の説明を省略する。

30は前述した状態観測回路であって、加速度検出信号
r21とモータ電流検出信号r10とを入力し、モータ電流検
出信号r10の変化状態に対応した加速度検出信号r21の変
化状態を判別して、被駆動機構９に生じる振動を減衰さ
せる減衰モータ速度を推定して減衰モータ速度推定信号
Xhdを出力する。

32は、減衰モータ速度推定信号Xhdを増幅して減衰モ
ータ駆動電流Inを出力する減衰モータ駆動電流回路であ
る。

33は、モータ駆動電流Imに減衰モータ駆動電流Inを加
算したモータ駆動補償電流をモータに供給するモータ電
流加算回路である。

上記の加速度検出器21、状態観測回路30、減衰モータ
駆動電流回路32およびモータ電流加算回路33は、防振制
御回路20を形成する。

本発明の装置においては、伝達側の実際のモータ駆動
補償電流に対応したモータ電流検出信号r10と被伝達側
の被駆動機構９の実際の加速度に対応した加速度検出信
号r21との両信号によって、伝達側の駆動状態と被伝達
側の振動状態とを比較判断するために、被駆動機構９の
停止から定速までの加速期間および定速から停止までの
減速期間において加速時間および減速時間を長くするこ
となく、略定速度で駆動中または停止後における被駆動
機構９の振動を減衰させることができる。

（第１および第２実施例の動作説明） 被駆動機構９を、例えば停止から速度を増加させて移
動させる場合、加速度検出器21の加速度検出信号r21が
発生していても、モータ電流検出信号r10が増加してい
るときは、この信号をモータ電流信号比較回路６または
モータ電流加算回路33にフィードバックする量を減少さ
せて、モータおよび被駆動機構を加速させて短時間で略
一定速度まで上昇させる。モータ電流検出信号r10が略
一定値に達した後に、加速度検出信号r21が検出された
ときは、モータ速度を微小量減速して、被駆動機構の移
動速度に対応した伝達側のモータ速度と被伝達側の被駆
動機構の移動速度とを一致させ、被駆動機構の振動を抑
制する。

逆に、被駆動機構９を移動速度から停止させる場合、
加速度検出器21の加速度検出信号r21が発生していて
も、モータ電流検出信号r10が減少しているときは、こ
の信号をモータ電流信号比較回路６またはモータ電流加
算回路33にフィードバックする量を減少させて、モータ
および被駆動機構を短時間で減速させる。モータ電流検
出信号r10が略零に接近したときに、加速度検出信号r21
が検出されているときは、モータ速度を停止寸前に微小
量増速させて、被駆動機構の停止寸前の移動速度に対応
した伝達側のモータの停止寸前の速度と被伝達側の被駆
動機構の停止寸前の移動速度とを一致させ、被駆動機構
の振動を防止する。

以上の説明は、被駆動機構を停止から加速して略一定
速度に達するまで、または逆に、略一定速度から減速し
て停止するまでの例について説明したが、本発明に係る
装置においては、モータ電流検出信号r10の変化状態と
加速度検出信号r21の変化状態とを判別して、時々刻々
とモータ速度を微小制御し、被駆動機構の移動速度に対
応した伝達側のモータ速度と被伝達側の被駆動機構の移
動速度とを一致させて振動を減衰させることができる。

（第４図を用いた制御理論の説明） つぎに、上述した制御を実現するための制御理論につ
いて説明する。

第４図は、モータ８および被駆動機構９の振動系モデ
ル9aを示す図で、91はモータ８および被駆動機構９を連
結する伝達機構（減速機）であり、92は、アームであ
る。9aは、モータ８、伝達機構（減速機）91および被駆
動機構９の振動系モデルである。アーム92を剛体とし、
モータ８とアーム92との伝達機構（減速機91）をバネお
よびダンパとし、モータ軸とアーム92とを２自由度系連
成振動系として考えている。

上記の振動系モデル9aにおいて、T:モータトルク、θ
f:モータ回転角、θf1:モータ速度、θf2:モータ加速
度、θs:アームの回転角、θs1:アームの移動速度、θs
2:アームの移動加速度、Jf:モータ慣性、Js:アーム慣
性、N:減速比、K:減速機のバネ定数、C:減速機の減衰係
数とすれば、２自由度系の運動方程式は、下記の式で表
される。

Jsθs2＋Ｃ（θs1−θf1/N） ＋Ｋ（θｓ−θf/N）＝０ ……（１） Jfθf2＝（θf1/N−θs1）C/N ＋（θf/N−θｓ）K/N＋Ｔ ……（２） ここで、ラプラス演算子:Sを導入すれば、上記（１）
および（２）式は下記のごとく表わされる。

JsS²θｓ＋CS（θｓ−θf/N） ＋Ｋ（θｓ−θf/N）＝０ ……（３） JfS²θｆ＝（θf/N−θｓ）CS/N ＋（θf/N−θｓ）K/N＋Ｔ ……（４） 上記の（３）および（４）式を、θｓおよびθｆにつ
いて整理すると、下記の式で表わされる。

（JsS²＋CS＋Ｋ）θｓ＝（CS/N＋K/N）θｆ……（3.1） （JfS²−CS/N²−K/N²）θｆ ＝−（CS/N＋K/N）θｓ＋Ｔ ……（4.1） つぎに、上記の（3.1）および（4.1）式から、θｆを
消去すると、モータトルクＴからアーム加速度θs2（以
下、θs2＝βで示す。）までの伝達関数β/Tは、下記の
式で表わされる。

β＋Ｔ＝（C2S＋K2）/S²＋C1S＋K1 ……（５） ただし、K1:モータトルクから見たバネ定数、K2:アー
ム慣性から見たバネ定数、C1:モータトルクから見た減
衰係数、C2:アーム慣性から見た減衰係数で、それぞ
れ、以下の式で与えられる。

K1＝（N²Jf＋Js）K/N²JfJs ……（5.1） K2＝２πNK/（N²Jf＋Js） ……（5.1） K2＝２πNK/（N²Jf＋Js） ……（5.2） C1＝（N²Jf＋Js）C/（N²JfJs） ……（5.3） C2＝２πNC/（N²Jf＋Js） ……（5.4） （５）式の上辺および下辺に適当な変数Ｘを各々掛け
て、逆ラプラス演算を行なうと、（５）式は次式で表わ
される。ただし、X1:変数Ｘの１次微分値（変位Ｘの速
度）、X2:変数Ｘの２次微分値（変位Ｘの加速度）を表
す。

β/T＝（C2X1＋K2X）／（X2＋C1X1＋K1X） ……（６） （６）式より、 β＝C2X＋K2X ……（７） Ｔ＝X2＋C1X1＋K1X ……（８） となるようなＸ、X1およびX2を導入する。制御工学にお
いてはこのＸを状態変数という。

（８）式において、Ｘの定数C1はモータトルクＴにお
ける振動系モデル9aの減衰係数に対応している。この減
衰係数を増加させると、被駆動機構の振動を抑制するこ
とができるが、この減衰係数は機械的に定まるために、
任意に設定することができない。したがって、入力（モ
ータトルク）Ｔに制御量T0（見かけ上のトルク）を付加
することによって、見かけ上の減衰係数を大きくするこ
とができる。

そこで、（８）式において、入力Ｔに制御量T0を付加
すると、（８）式は下記の式となる。

Ｔ＋T0＝X2＋C1X1＋K1X ……（９） Ｔ＝X2＋C2X1＋K1X−T0 ……（9.1） ここで仮に、T0＝−C0Xで表わされる信号を作れば、
上記（9.1）式は、下記の式となる。

Ｔ＝X2＋（C2＋C0）X1＋K1X ……（10） （８）式と（10）式とを比較すると、入力（モータト
ルク）Ｔから見た減衰係数は、C1からC1＋C0に増大する
ために、被駆動機構（アーム92）の減衰が増大して、被
駆動機構９の振動を減衰させる。上記の制御量T0を特定
するためには、変数Ｘの１次微分値（変位Ｘの速度）X1
を知得すればよい。

（第５図を用いた状態観測回路の説明） ここで、変数Ｘの１次微分値（変位Ｘの速度）X1を知
得するための状態観測回路30について説明する。

第５図は、振動系モデル9aの状態観測回路30を示す図
であって、この状態観測回路30は制御工学の現代制御理
論において多用されている。第４図に示される振動系モ
デル9aを状態観測回路30で置換すると、51はモータ電流
検出信号r10を入力して信号ａを出力する伝達定数Ｂの
減衰器であって、アームの加速度とモータ電流検出信号
とのレベルを合わせるための減衰器である。52は入力信
号ｂを、1/S（ただし、Ｓはラプラス演算子）の積分し
て積分信号ｄを出力する積分器である。53は積分信号ｄ
を入力して信号をｅを出力する状態観測回路の固有値Ｈ
を付与する定数設定器である。54はアームの加速度に対
応する信号ｈを入力して、信号ｆを出力する伝達定数の
W1のゲイン調整器である。

55は積分信号ｄを入力して信号ｇを出力する伝達定数
のW2のゲイン調整器である。56は加速度検出信号r21を
入力して加速度に対応するｈを出力する減衰器である。

次に、第４図で説明した各定数Jf,Js,K,CおよびＮを
使用して、現代制御理論を適用すると、下記の式が成立
する。ただし、C1,C2,K1およびK2は、上記（5.1）乃至
（5.4）式の定数と同じである。

ｈ＝（−HC2−K2）／（H²＋K1＋HC1） ……（11.1） W1＝（K1C2−K2H−K2C1）／（K1C2²＋K2²−K2C1C2） …
…（11.2） W2＝［−K2（H²＋K1＋HC1）］／（K1C2²＋K2²−K2C1C
2） ……（11.3） 第５図において、加算器CM1の出力信号（積算器52の
入力信号）ｂは、下記式で表わされる。ただし、モータ
トルクＴとモータ駆動電流値Imとは、Ｔ＝AIm（Ａは定
数）で正比例するので、モータ駆動電流Imに対応する信
号をＩとし、アームの加速度に対応する信号をｈとする
と、つぎの式が成立する。

ｂ＝ａ＋ｈ＋ｅ＝Ｂ・Ｉ＋ｈ＋Ｈ・ｄ ……（12） 加算器CM2の入力信号ｆおよびｇと状態変数の推定値
の出力信号（減衰モータ速度指定信号）Xhdとは、下記
の式となる。

Xhd＝ｆ＋ｇ＝W1h＋W2d ……（13） 積算器52の入力信号ｂと出力の積分信号ｄとは、出力
信号ｄの微分値をd1とすれば、下記の式となる。

ｂ＝d1 ……（14） （14）式を（12）式に代入すると、つぎの式が成立す
る。

d1＝Ｂ・Ｉ＋ｈ＋Ｈ・ｄ ……（15） 第５図の状態観測回路30において、モータ駆動電流Im
に対応する信号Ｉとアームの加速度に対応する信号ｈと
を入力すると、（15）式で示す信号ｄが出力され、この
信号ｄの値を（13）式で示す振動系モデル9aの状態変数
の推定値Xhdが出力される。この振動系モデル9aの状態
変数の推定値Xhdは、第１図および第２図に示す被駆動
機構９に生じる振動を減衰させる減衰モータ速度を推定
した減衰モータ速度推定信号Xhdと同じである。

（第６図を用いた状態観測回路の実施例の説明） 第６図は状態観測回路30の実施例を示す図である。同
図において、各符号は第４図の符号に対応し、r10はモ
ータ電流検出信号、r21は加速度検出信号、ｈは加速度
に対応する信号、51は演算増幅器と抵抗器とから成る伝
達定数Ｂの減衰器である。CM1は減衰器51の出力信号ａ
と加速度に対応する信号ｈと抵抗器から成る定数設定器
53の出力信号ｅとを入力して差の信号ｂを出力する抵抗
器から成る加算器である。52は差の信号ｂを積分して積
分信号ｄを出力する積分器であって、演算増幅器とコン
デンサとから構成される。54は加速度に対応する信号ｈ
を入力して信号ｆを出力するゲイン調整器であって、抵
抗器とコンデンサとから構成される。55は積分器52の出
力信号ｄを入力して信号ｇを出力するゲイン調整器であ
って、抵抗器とコンデンサとから構成される。

CM2は信号ｆと信号ｇとを入力して差の信号r31（第１
図および第２図の減衰モータ駆動電流信号）を出力する
加算器であって、演算増幅器、抵抗器およびコンデンサ
から構成される。これらの各ブロックの機能は、第５図
において説明した機能と同一であるので説明を省略す
る。

（第７図乃至第13図を用いた効果の説明） 第７図および第８図は、第３図に示す被駆動機構（ア
ーム）を一定の速度で移動させているときの被駆動機構
（アーム）の加速度の時間的経過を示す図であって、第
７図はフィードバック制御をしないときの加速度の時間
的経過を示し、第８図は本発明に係る制御装置によりフ
ィードバック制御をしたときの加速度の時間的経過を示
す。

振動系モデル9aの状態変数の推定値Xhdをフィードバ
ックする減衰係数のレベル調整は、振動系モデル9aの減
衰係数C0を変化させることに相当し、減衰係数レベル調
整回路で適正値を設定することができる。

例えば、実施例の装置において、被駆動機構（アー
ム）を一定の速度で移動させている間、フィードバック
制御をしないときは、第７図に示すように、加速度が変
動して、被駆動機構（アーム）は、モータの駆動によっ
て共振して振動が発生するのに対して、フィードバック
制御をすると、第８図に示すように、被駆動機構の加速
度の変動が微細となるので、被駆動機構（アーム）はほ
とんど振動しない。

第９図および第10図は、第３図に示す被駆動機構を移
動中に瞬間停止させたときに、被駆動機構の加速度の減
衰状態を示す図であって、第９図はフィードバック制御
をしないときの加速度減衰状態を示し、第10図は本発明
に係る制御装置によって、適切なフィードバック制御を
したときの加速度減衰状態を示す。

また、実施例の装置において、被駆動機構を瞬間停止
させた後は、フィードバック制御をしないときは、第９
図に示すように、被駆動機構の加速度がほとんど零にな
るまでに、約0.4秒かかっていたために、被駆動機構に
振動を生じたのに対して、フィードバック制御をしたと
きは、第10図に示すように、被駆動機構の加速度が急速
に減衰して、約0.18秒後に、ほとんど零になっているた
めに、被駆動機構に振動が生じなかった。

第11図は、被駆動機構（アーム）を一定の速度で移動
させている間の加速度とフィードバックゲイン（C0）と
の関係を示す図である。同図は、フィードバックゲイン
（C0）が大になるほど、被駆動機構（アーム）を一定の
速度で移動させている間の加速度は小になり、被駆動機
構（アーム）の振動を抑制する効果が大であることを示
している。

第12図は、被駆動機構（アーム）を例えば26.5度／秒
で移動中に瞬間停止させたときに、フィードバックゲイ
ン（C0）と加速度がほとんど零になるまでの吸束時間と
の関係を示す図である。同図は、被駆動機構（アーム）
を移動中に瞬間停止させたときに、フィードバックゲイ
ン（C0）が大になるほど、加速度がほとんど零になるま
での吸束時間が短くなり、被駆動機構（アーム）の振動
を抑制する効果が大であることを示している。

第13図は、第３図に示す装置において、フィードバッ
クゲイン（C0値）を変更したときに、モータ駆動電流の
周波数とモータトルクから被駆動機構までの伝達関数
（伝達ゲイン）との関係を示す図である。

マニピュレータ装置は、その構造によって、その装置
に特有の共振点が存在するために、一般に共振周波数と
なる範囲ではマニピュレータを運転しないようにしてい
る。第13図において、フィードバック制御をしないとき
は、フィードバックゲインC0＝０に示すように、12［H
z］の共振周波数で、伝達ゲインが12［db］の共振状態
が発生している。

しかし、第13図に示すように、本実施例の装置におい
ては、フィードバックゲインC0＝1200に示すように、適
切なフィードバック制御をすることによって、12［Hz］
の共振周波数で、伝達ゲインが１［db］の共振状態しか
発生していない。

このように、本発明は、適切なフィードバック制御を
することによって、共振状態がほとんど発生しないよう
にすることができるので、共振周波数の範囲を除いてマ
ニピュレータを運転する必要がなくなるので、低速度か
ら高速度まで広範囲に運転することができる。また、マ
ニピュレータの運転中に、被駆動機構の加速度を適宜に
変更することができるので、例えば、物品の搬送用マニ
ピュレータに、本発明に係る防振制御装置を適用し、物
品の重量に応じて、フィードバックゲインC0を変更し
て、適切なフィードバック制御をすることができる。

さらに、本発明は、直線移動式、円筒座標式、極座標
式、垂直多関節式等の産業用ロボットに好適である。

〈発明の効果〉 本発明のマニピュレータの防振制御装置は、モータ８
と、被駆動機構９と、モータおよび被駆動機構を連結す
る伝達機構とからなる制御対象の振動系をモデル化し、
この振動系モデルの振動状態を推定する状態観測回路30
に、マニピュレータの被駆動機構（アーム）に取り付け
た加速度検出器の実際のアームの加速度に対応する加速
度検出信号r21と駆動機構の実際のモータ電流に相当す
るモータ電流検出信号r10とを入力して、モータ電流検
出信号r10の変化状態に対応した加速度検出信号r21の変
化状態を判別して、被駆動機構９に生じる振動を減衰さ
せる減衰モータ速度を推定した減衰モータ速度推定信号
Xhdをフィードバックして、加速度の変動を減衰させて
いるので、マニピュレータの被駆動機構（アーム）に取
り付けた加速度検出器の実際のアームの加速度の変動が
極めて小さく、被駆動機構９の振動を抑制する。従来技
術では、モータ軸の振動を止めることによってアームの
振動も止めようとしているために、減速機がバネ要素を
持つようなモータ軸とアームとの２自由度振動系では、
アームの共振周波数において、モータの軸の振動を減衰
させても、アームの振動までも減衰させることができな
かった。これに対して、本発明では、アームの加速度を
直接に検出して、モータ軸の振動ではなくアームの振動
を減衰させる制御を行っているので、アームの共振周波
数領域においても、振動を減衰させることができる。ま
た、本発明のマニピュレータの防振制御装置は、被駆動
機構の加速または減速を妨げることなく、被駆動機構９
の振動を減衰させることができる。

【図面の簡単な説明】

第１図は第１の発明の実施例を示すブロック図、第２図
は第２の発明の実施例を示すブロック図、第３図は本発
明の対象例を示す正面図である。 第４図は第３図に示すモータと、被駆動機構と、モータ
および被駆動機構を連結する伝達機構とよりなる制御対
象の振動系モデルを示す図、第５図は第４図に示す振動
系モデルの状態観測回路を示す図、第６図は第５図に示
す状態観測回路の実施例を示す図である。 第７図および第８図は、第３図に示す被駆動機構（アー
ム）を一定の速度で移動させているときの被駆動機構
（アーム）の加速度の時間的経過を示す図であって、第
７図はフィードバック制御をしないときの加速度の時間
的経過を示し、第８図は本発明に係る制御装置によって
フィードバック制御をしたときの加速度の時間的経過を
示す。 第９図および第10図は、第３図に示す被駆動機構を移動
中に瞬間停止させたときに、被駆動機構の加速度の減衰
状態を示す図であって、第９図はフィードバック制御を
しないときの加速度減衰状態を示し、第10図は本発明に
係る制御装置によって、適切なフィードバック制御をし
たときの加速度減衰状態を示す。 第11図は、被駆動機構（アーム）を一定の速度で移動さ
せている間の加速度とフィードバックゲイン（C0）との
関係を示す図である。 第12図は、被駆動機構を移動中に瞬間停止させたとき
に、フィードバックゲイン（C0）と加速度がほとんど零
になるまでの吸束時間との関係を示す図である。 第13図は、第３図に示す装置において、フィードバック
ゲイン（C0値）を変更したときに、モータ駆動電流の周
波数とモータトルクから被駆動機構までの伝達関数の比
（伝達ゲイン）との関係を示す図である。 第14図は従来例を示すブロック図である。 １……動作指令信号設定回路 ２……位置信号比較回路 ３……速度指令信号回路 ４……速度信号比較回路 ５……モータ駆動電流指令信号回路 ６……モータ電流信号比較回路 ７……サーボアンプ ８……モータ ９……被駆動機構（アーム） 91……伝達機構（減速機） 92……アーム 9a……振動系モデル 10……モータ電流検出器 11……モータ速度検出器 12……位置検出器（エンコーダ） 20……防振制御回路 21……加速度検出器 22……減衰係数レベル調整回路 30……状態観測回路 31……減衰モータ駆動電流信号回路 32……減衰モータ駆動電流回路 33……モータ電流加算回路 r1……動作指令設定信号 r2……位置補償信号 r3……速度指令信号 r4……速度補償信号 r5……モータ駆動電流指令信号 r6……モータ駆動電流補償信号 r10……モータ電流検出信号 r11……モータ速度検出信号 r12……位置検出信号 r21……加速度検出信号 r22……減衰係数レベル調整信号 r31……減衰モータ駆動電流信号 Im……モータ駆動電流 In……減衰モータ駆動電流 Xhd……減衰モータ速度推定信号（状態変数の推定値）

───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 昭61−164481（ＪＰ，Ａ) 特開 昭60−200788（ＪＰ，Ａ) 特開 昭60−177906（ＪＰ，Ａ) 特開 昭60−184421（ＪＰ，Ａ) 特開 昭59−612（ＪＰ，Ａ) 特開 昭61−251915（ＪＰ，Ａ)

Claims (2)

(57)【特許請求の範囲】
1. 【請求項１】駆動力を発生して伝達する駆動力伝達側の
   モータと、 駆動力が伝達される駆動力被伝達側の被駆動機構と、 モータおよび被駆動機構を連結する伝達機構と、 モータ駆動電流を検出してモータ電流検出信号を出力す
   るモータ電流検出回路と、 被駆動機構に取り付けられて被駆動機構の加速度を検出
   して加速度検出信号を出力する加速度検出器と、 加速度検出信号とモータ電流検出信号とを入力し、モー
   タ電流検出信号の変化状態に対応した加速度検出信号の
   変化状態を判別して、被駆動機構に生じる振動を減衰さ
   せる減衰モータ速度を推定した減衰モータ速度推定信号
   を出力する状態観測回路と、 減衰モータ速度推定信号を入力し、減衰力を増大させる
   ために付加して減衰トルクの大きさを決定し、減衰係数
   を調整して減衰係数レベル調整信号を出力する減衰係数
   レベル調整回路と、 減衰係数レベル調整信号を入力し、減衰係数レベル調整
   信号の振幅および位相を、被伝達側の被駆動機構の加速
   度と伝達側のモータのトルクとの比の伝達関数の真の値
   に近づけるための補償をして減衰モータ駆動電流信号を
   出力する減衰モータ駆動電流信号回路と、 被駆動機構の動作指令に対応したモータ駆動電流指令信
   号とモータ電流検出信号と減衰モータ駆動電流信号とを
   入力して、モータ駆動電流補償信号を出力するモータ電
   流信号比較回路と、 モータ駆動電流補償信号を入力して、モータの駆動電流
   を出力するサーボアンプとから成り、 モータにモータ駆動電流を供給し、モータ速度を制御し
   て、被駆動機構の振動を減衰させるマニピュレータの防
   振制御装置。
2. 【請求項２】駆動力を発生して伝達する駆動力伝達側の
   モータと、 駆動力が伝達される駆動力被伝達側の被駆動機構と、 モータおよび被駆動機構を連結する伝達機構と、 モータ駆動電流を検出してモータ電流検出信号を出力す
   るモータ電流検出回路と、 被駆動機構に取り付けられて被駆動機構の加速度を検出
   して加速度検出信号を出力する加速度検出器と、 加速度検出信号とモータ電流検出信号とを入力し、モー
   タ電流検出信号の変化状態に対応した加速度検出信号の
   変化状態を判別して、被駆動機構に生じる振動を減衰さ
   せる減衰モータ速度を推定した減衰モータ速度推定信号
   を出力する状態観測回路と、 減衰モータ速度推定信号を増幅して減衰モータ駆動電流
   を出力するモータ駆動補償電流回路と、 モータ駆動電流補償信号を増幅してモータ駆動電流を出
   力するサーボアンプとから成り、 被駆動機構の動作指令に対応したモータ駆動電流指令信
   号とモータ電流検出信号とを入力して、モータ駆動電流
   補償信号を出力するモータ電流信号比較回路と、 モータにモータ駆動電流および減衰モータ駆動電流を供
   給し、モータ速度を制御して、被駆動機構の振動を減衰
   させるマニピュレータの防振制御装置。