JPH106261A

JPH106261A - ロボット制御装置

Info

Publication number: JPH106261A
Application number: JP15680096A
Authority: JP
Inventors: Yoshiyuki Urakawa; 禎之浦川
Original assignee: Sony Corp
Current assignee: Sony Corp
Priority date: 1996-06-18
Filing date: 1996-06-18
Publication date: 1998-01-13

Abstract

(57)【要約】【課題】測定された周波数特性から、推定演算に必要
な定数を簡便に求める。【解決手段】第２アーム３を駆動するサーボモータ
４、サーボモータ４にトルクを発生させるアンプ部５及
びサーボモータ４の角度を検出する角度検出器６を備え
るロボット２を制御対象とし、指令トルクτとサーボモ
ータ４の角度θ_mとに基づいて、少なくとも第２アーム
３の角度θ_a若しくは角速度θ_a ^'を推定するねじれ角・
外乱推定オブザーバ７と、ねじれ角・外乱推定オブザー
バ７が推定した第２アーム３の推定角度θ_a 若しくは推
定角速度θ_a ^' に基づいて、指令トルクτを算出する制御
部９と、第２アーム３の角度θ_a若しくは角速度θ_a ^'を
推定するための各種定数をアンプ部５に与える指令トル
クτから角度検出器６により検出されるサーボモータの
角度θ_mまでの伝達関数から決定する推定定数決定部６
０とを有し、制御部９が算出した指令トルクτに基づい
てロボット２を制御する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、組み立てロボット
等の、高速な位置決めの必要なロボットに用いて好適な
ロボット制御装置に関する。

【０００２】

【従来の技術】従来のロボット制御装置においては、い
わゆるＰＩＤ制御と呼ばれる制御系が用いられていた。
ＰＩＤ制御は、サーボモータの角度と目標角度の差の比
例（Ｐｒｏｐｏｒｔｉｏｎ）と積分（Ｉｎｔｅｇｒａｔ
ｉｏｎ）と時間微分（Ｄｉｆｆｅｒｅｎｔｉａｔｉｏ
ｎ）とによる制御系である。

【０００３】このロボット制御装置は、図５に示すよう
に、ロボット１２２を制御対象としている。このロボッ
ト１２２としては、例えば図６に示すように、駆動部と
して例えばアーム１２４と、この駆動部を駆動するサー
ボモータ１２３とが、減速機またはベルト等の結合部１
２５を介して連結されて構成されているものを挙げるこ
とができる。また、図示は省略するが、このロボット１
２２には、サーボモータ１２３に所望のトルクを発生さ
せるアンプと、サーボモータ１２３の角度を検出する角
度検出器とが設けられている。この角度検出器にて検出
されたサーボモータ１２３の角度θ_mの検出信号は、ロ
ボット制御装置１２０にフィードバックされている。な
お、図６の図中θ_aはアーム１２４の角度である。

【０００４】上記ロボット制御装置１２０は、主要構成
要素として制御部１２１を有している。この制御部１２
１には、サーボモータ１２３の目標角度θ_r を示す信号
が端子１０１を介して供給され、また、上記ロボット１
２２の角度検出器にて検出されたサーボモータ１２３の
角度θ_mを示す信号も供給される。当該制御部１２１
は、上記サーボモータ１２３の角度θ_mとサーボモータ
１２３の目標角度θ_r とから、下記の式（１１）によっ
て、上記アンプに与える指令トルクτを算出する。

【０００５】

【数４】

【０００６】なお、上記式（１１）におけるｋ_p は比例
パラメータを、ｋ_i は積分パラメータ、ｋ_d は時間微分
パラメータを示している。

【０００７】制御部１２１は、上記算出により求めた指
令トルクτをアンプに与えることにより、ロボット１２
２を制御する。

【０００８】ところで、上記ロボット１２２において
は、図６に示したように、サーボモータ１２３とアーム
１２４とが減速機またはベルト等の結合部１２５を介し
て連結されて構成されているものである。この結合部１
２５は、剛性が低いためにバネ要素として機能し、前記
図６の構成はこの結合部１２５をバネに置き換えてモデ
ル化したものである。したがって、記ロボット制御装置
１２０は、サーボモータ１２３とアーム１２４とがバネ
１２５を介して連結された２慣性系のものを制御対象と
している。

【０００９】一方、従来のロボット制御装置１２０に用
いられる上記ＰＩＤ制御は、結合部のバネ１２５要素と
しての機能が無視できる剛性の高い制御対象に対して有
効に制御を行う。したがって、ＰＩＤ制御は、結合部の
剛性が低い上記２慣性系の制御対象に対して有効に制御
を行えない。

【００１０】すなわち、上記ＰＩＤ制御では、結合部の
剛性が低いロボット１２２を、より高速に位置決めしよ
うとすると、アーム１２４の先端が振動するといった問
題点がある。このアーム１２４の先端が振動するという
問題点を解決する手段としては、例えば、上記アーム１
２４の加減速時間を長時間化することが考えられる。し
かし、これではアーム１２４操作の高速化が困難とな
る。また、このようなアーム１２４の加減速時間が長時
間化するといった問題点を解決する手段としては、例え
ば、アーム１２４の角度及びアーム１２４の角加速度を
検出する角加速度検出器を当該アーム１２４の先端に取
り付け、この角加速度検出器にて上記アーム１２４の先
端の位置を直接検出し、この検出出力を用いて制御する
ことが考えられる。しかし、この場合は、上記角加速度
検出器分のコストの増加やメンテナンス性の悪化等を招
くことになる。

【００１１】さらに、上述したような問題点を解決する
手段としては、特開平１−２９３０１号公報に記載され
た「産業用ロボットのサーボループ制御方法」がある
が、この公報記載の技術では、推定された外乱をフィー
ドバック要素としていないために外乱を抑制することが
できず、また、フィードフォワード要素がないために零
点の配置に自由度がないことにより、制御装置の性能的
に満足した結果が得られない。

【００１２】このようなことから、本件出願人は、特願
平７−３１３３３９号の明細書及び図面にて、アーム等
の駆動部の振動の抑制と外乱の抑制を図ると共に、当該
駆動部の位置決めの高速化を図るロボット制御装置を提
案している。すなわち、当該明細書及び図面に記載のロ
ボット制御装置は、アーム等の駆動部の角度や角速度を
推定して、この推定値を用いてトルク指令を生成するこ
とで、高速位置決め時のアーム先端の振動を抑制するよ
うにしている。なお、当該明細書及び図面に記載の技術
を用いて、振動を充分に抑制するためには、駆動部の角
度や角速度を精度良く推定すればよい。具体的には、下
記の式（２），式（３）で行われる推定演算に必要な定
数として、駆動部のイナーシャＪ_a、駆動部の粘性摩擦
Ｄ_a、サーボモータ軸と駆動部間に配されるギヤのバネ
定数Ｋ、サーボモータのイナーシャＪ_m、サーボモータ
の粘性摩擦Ｄ_mを精度良く求めるようにする。

【００１３】

【数５】

【００１４】なお、式（２），式（３）において、Ｎは
上記ギヤのギヤ比、θ_a は駆動部の推定角度、θ_a ^' は駆
動部の推定角速度、θ_m ^' はサーボモータの推定角速度で
あり、ｚ₁，ｚ₂，ｚ₃は最小次元オブザーバの状態変
数、ｋ₁，ｋ₂，ｋ₃は推定速度を決めるパラメータであ
る。

【００１５】

【発明が解決しようとする課題】ところで、上記定数を
求める場合には、上述した手法の他に、例えば、指令ト
ルクτから角度検出手段によって検出されるサーボモー
タの角度θ_mまでの振幅周波数特性Ｆ_a(w)や位相周波数
特性Ｆ_p(w)を測定し、これらから上記定数を求めること
も可能である。

【００１６】すなわち、前記図６の２慣性系は、一般に
は後述する図４のようなブロック線図で描き表せる。な
お、図４は本発明の実施の形態の説明でも用いるため、
その詳細については後述する。当該ブロック線図におい
て、指令トルクτからサーボモータの角速度θ_m ^'までの
伝達関数は、式（１２）で表される。

【００１７】

【数６】

【００１８】上記定数を求める場合には、上記伝達関数
Ｇ^'(s)より求められる振幅周波数特性Ｇ_a ^'(w)や位相周
波数特性Ｇ_p ^'(w)が、なるべく上記測定振幅周波数特性
Ｆ_a(w)や測定位相周波数特性Ｆ_p(w)と一致するように、
駆動部のイナーシャＪ_a、駆動部の粘性摩擦Ｄ_a、バネ定
数Ｋ、サーボモータのイナーシャＪ_m、サーボモータの
粘性摩擦Ｄ_mを合わせ込むようになる。

【００１９】しかしながら、この方法では、どの定数を
変えると周波数特性がどのように変化するかが判り難
く、試行錯誤に近い調整を行う必要がある。このため、
定数を決定するのに時間がかかり、また、得られた定数
で推定を行って得られる駆動部の角度の推定値（推定角
度）θ_m や、駆動部の角速度の推定値（推定角速度）θ_m
^'も、必ずしも充分な精度で推定できているとは限らな
い。

【００２０】そこで、本発明はこのような状況に鑑みて
なされたものであり、測定された周波数特性から、推定
演算に必要な定数、すなわち駆動部のイナーシャＪ_a、
駆動部の粘性摩擦Ｄ_a、バネ定数Ｋ、サーボモータのイ
ナーシャＪ_m、サーボモータの粘性摩擦Ｄ_mを簡便に求め
ることが可能なロボット制御装置を提供することを目的
とする。

【００２１】

【課題を解決するための手段】本発明のロボット制御装
置は、駆動部を駆動するサーボモータと、サーボモータ
にトルクを発生させるアンプと、サーボモータの角度を
検出する角度検出手段とを備えるロボットを制御対象と
する。ロボット制御装置は、アンプに与える指令トルク
と角度検出手段より検出されたサーボモータの角度とに
基づいて、少なくともロボット駆動部の角度やその時間
微分である角速度を推定する駆動部推定手段を備える。
また、ロボット制御装置は、駆動部推定手段より推定さ
れたロボット駆動部の速度、ロボット駆動部の角速度に
基づいて、アンプに与える指令トルクを算出する制御手
段とを備える。さらに、ロボット制御装置は、ロボット
の駆動部の角度若しくはロボットの駆動部の角速度を推
定するための各種定数を、アンプに与える指令トルクか
ら角度検出手段により検出されるサーボモータの角度ま
での伝達関数から、簡便に決定する推定定数決定手段を
備える。

【００２２】以上のように構成された本発明のロボット
制御装置によれば、推定定数決定手段において予め設定
された伝達関数から推定定数を決定する。駆動部推定手
段は、この推定定数を用いて、指令トルクとサーボモー
タの角度とに基づいて、ロボットの駆動部の推定角度、
推定角速度を算出して、制御手段に出力する。制御手段
では、この推定角度、推定角速度を用いてアンプに与え
る指令トルクを算出してロボットのアンプに出力する。
ロボット制御装置は、制御手段によって算出された指令
トルクに基づいてロボットを制御する。

【００２３】

【発明の実施の形態】以下、本発明の具体的な実施の形
態について、図１乃至図４の図面を参照して詳細に説明
する。

【００２４】先ず、本発明の実施の形態として示すロボ
ット制御装置１は、図１に示すように、例えば基板に部
品を実装する際に用いる組立用のロボット２を制御する
ものである。

【００２５】図１のロボット２の第１アーム駆動部２１
３は、第１アーム２０３を駆動する駆動モータ及び当該
駆動モータの出力軸に接続された減速機構とからなって
いる。この第１アーム駆動部２１３は、後述するアンプ
部５を介して基台２０５に固定され、回動軸に回転力を
伝達することで、第１アーム２０３の全体を回動可能に
している。なお、上記第１アーム駆動部２１３やアンプ
部５はカバー２０６により覆われている。

【００２６】ロボット２の第２アーム駆動部２０７は、
例えばスチールベルトからなる駆動ベルト２００を介し
て第２アーム３を駆動するサーボモータ４と、そのモー
タ出力軸に接続された減速機構２１５とからなってい
る。第２アーム駆動部２０７は、第１アーム駆動部２１
３と同様に基台２０５に対して回動方向に固定されてい
る。また支持部２０４は、第２アーム駆動部２０７のサ
ーボモータ４自体が回転しないように、上記カバー２０
６を介して上記基台２０５に固定するためのものであ
る。上記サーボモータ４の駆動力は、上記減速機構２１
５で減速され、さらに回動軸２０８を介してベルト駆動
プーリ２０２に伝達され、このプーリ２０２を回動させ
るようになっている。なお、これら第１アーム駆動部２
１３と第２アーム駆動部２０７は、第１アーム２０３の
一方の端部に対応して配置されている。

【００２７】駆動ベルト２００は、上記ベルト駆動プー
リ２０２と第２軸駆動プーリ２０１との間に懸架され、
上記第２アーム駆動部２０７にて発生した駆動力を、上
記第２軸駆動プーリ２０１に伝達するようになってい
る。なお、これらベルト駆動プーリ２０２と駆動ベルト
２００と第２軸駆動プーリ２０１とは、第１アーム２０
３内部に配置されている。

【００２８】第２軸駆動プーリ２０１は、上記第１アー
ム２０３の他方の端部に対応して配置され、回動軸２１
４を回転中心として回動可能になっている。

【００２９】第２アーム３の中央部は、上記第２軸駆動
プーリ２０１に固着されている。したがって、当該第２
軸駆動プーリ２０１が上記駆動ベルト２００を介して上
記第２アーム駆動部２０７にて回転されると、上記第２
アーム３は当該第２軸駆動プーリの回転に伴って駆動さ
れることになる。また、第２アーム３の一方の端部に
は、作用軸２１１を介して、例えば上記基板等に部品を
装填するための動作部２１０が配設されている。上記作
用軸２１１は、いわゆるツール搭載軸であり、上下方向
（Ｚ軸方向）の移動と軸回りの方向（Ｒ軸方向）の回転
とを行うことができるものである。したがって、当該作
用軸２１１の先端に配設された動作部２１０は、上下，
回転方向に自由に駆動されることになり、これにより基
板に対して部品を自在に装填可能となる。

【００３０】上述したように、ロボット２は、第１アー
ム２０３及び第２アーム３を有し、第２アーム３が駆動
ベルト２００を介して駆動される２慣性系の構成となっ
ている。すなわち、このロボット２は、前述した従来の
ロボット１２０に発生する反力、コリオリ・遠心力等の
干渉力を機構的にキャンセルしているものであり、ロボ
ット制御装置１における低剛性の２慣性系の制御対象と
なされている。

【００３１】また、上記ロボット２には、上述した構成
の他に、上記第２アーム３を駆動するサーボモータ４に
対して所望のトルクを発生させる上記アンプ部５を備
え、さらに上記サーボモータ４の角度を検出する例えば
ロータリーエンコーダ等からなる角度検出器６をも備え
ている。本発明の実施の形態にかかるロボット２のアン
プ部５には、ロボット制御装置１が算出した指令トルク
τを示す信号が、コード２１２を介して伝達供給される
ようになっており、アンプ部５は上記指令トルクτの値
に基づいて上記サーボモータ４を駆動する際の上記所望
のトルクを発生する。

【００３２】ここで、上記ロボット２は、下記式（１）
を含む下記の式（１３）の状態方程式にて表現されるも
のである。

【００３３】

【数７】

【００３４】

【数８】

【００３５】なお、式（１）及び式（１３）の状態方程
式における各状態変数のうち、θ_mはサーボモータ４の
角度、θ_m ^'はサーボモータ４の角速度、Ｊ_mはサーボモ
ータ４のイナーシャ、Ｄ_mはサーボモータ４の粘性摩
擦、θ_aは第２アーム３の角度、θ_a ^'は第２アーム３の
角速度、Ｊ_aは第２アーム３のイナーシャ、Ｄ_aは第２ア
ーム３の粘性摩擦、Ｋはサーボモータ軸と第２アーム３
の間に配されるギヤとしてのベルト２００等のバネ定
数、Ｎはギヤ比、τは指令トルクであり、ｄはサーボモ
ータ４のモータ軸に加わる外乱トルクである。

【００３６】このように、上記ロボット２では、上記式
（１）に対して外乱を含めた式（１３）により表された
ダイナミックスに基づいて、サーボモータ４が第２アー
ム３を駆動する。

【００３７】上述のようなロボット２を制御対象とする
ロボット制御装置１は、図２に示すように、主要構成要
素として、駆動部推定手段と、推定定数決定部６０と、
制御部９とを備えている。上記推定定数決定部６０は、
制御対象であるロボット２の伝達関数から、駆動部推定
手段での推定演算に必要な定数、すなわち第２アーム３
のイナーシャＪ_a、第２アーム３の粘性摩擦Ｄ_a、バネ定
数Ｋ、サーボモータ４のイナーシャＪ_m、サーボモータ
４の粘性摩擦Ｄ_mを算出する。上記駆動部推定手段は、
上記外乱も含めた推定を行う、ねじれ角・外乱推定オブ
ザーバ７であり、上記推定定数決定部６０により得られ
た定数を用いて、ロボット２の状態方程式をもとに、ロ
ボット２側のアンプ部５に与える指令トルクτと、ロボ
ット２側の角度検出器６より検出されたサーボモータ４
の角度θ_mとに基づいて、ロボット２の第２アーム３の
角度θ_aと、第２アームの角速度θ_a ^'と、サーボモータ
４の角速度θ_m ^'及び外乱トルクｄとを推定する。制御部
９は、上記サーボモータ４の所望の目標角度θ_rと、ロ
ボット２側の角度検出器６で検出されたサーボモータ４
の角度θ_mと、ねじれ角・外乱推定オブザーバ７で算出
された第２アーム３の推定角度θ_a 、第２アーム３の推
定角速度θ_a ^' 、サーボモータ４の推定角速度θ_m ^'、推定
外乱トルクｄに基づいて、アンプ部５に与える指令トル
クτを算出する。

【００３８】以下、上記ロボット制御装置１の各構成要
素について詳細に説明する。

【００３９】先ず、上記推定定数決定部６０には、上記
アンプ部５に与える指令トルクτから上記角度検出器６
により検出されるサーボモータ４の角度θ_mまでの伝達
関数Ｇ(s)を下記の式（４）で表したときの定数、すな
わち共振極の実部ｐ_rと、共振極の虚部ｐ_iと、反共振零
点の実部ｚ_rと、反共振零点の虚部ｚ_iと、モータ軸から
みたサーボモータ４及び第２アーム３を含めたイナーシ
ャＪの各値が、端子６１から入力される。なお、これら
の各定数の値は予め設定されており、端子６１を介して
例えば全体を制御するシステムコントローラ等が当該推
定定数決定部６０に供給する。

【００４０】

【数９】

【００４１】ここで、一般的に、指令トルクτからサー
ボモータ４の角速度θ_m ^'までの周波数特性は、比較的容
易に測定することができる。本発明の実施の形態におけ
る制御対象は、前記図１及び前記図６に示したような２
慣性系であり、この周波数特性は一般に図３のような共
振点、反共振点を持つ周波数特性となる。

【００４２】すなわち、図３に示すように、２慣性系の
周波数特性は、共振周波数ｆ_p［Ｈｚ］と、共振の鋭さ
Ｑ_pと、反共振の周波数ｆ_zと、反共振の鋭さＱ_zと、積
分ゲインＪと、低周波数でのカットオフ周波数ｆ_cとの
６つの特徴量で表現される。ここで、上記式（４）にお
ける定数、すなわち共振極の実部ｐ_r、共振極の虚部
ｐ_i、反共振零点の実部ｚ_r、反共振零点の虚部ｚ_i、モ
ータ軸から見たサーボモータ４及びアーム３を含めたイ
ナーシャＪ、モータ軸から見たサーボモータ４及びアー
ム３を含めた粘性摩擦Ｄと上記の特徴量は、下記の式
（１４），式（１５），式（１６）のように関係付けら
れる。

【００４３】

【数１０】

【００４４】残りの特徴量について、伝達関数での積分
ゲインＪはそのままモータ軸からみたサーボモータ４及
び第２アーム３を含めたイナーシャＪであり、また、共
振の鋭さＱ_pはｐ_rに反比例し、反共振の鋭さＱ_zはｚ_rに
反比例する。

【００４５】これらの関係より、測定した周波数特性か
ら上記式（４）で表される伝達関数Ｇ(s)の定数、すな
わち共振極の実部ｐ_r、共振極の虚部ｐ_i、反共振零点の
実部ｚ_r、反共振零点の虚部ｚ_i、モータ軸からみたサー
ボモータ４及び第２アーム３を含めたイナーシャＪ、モ
ータ軸からみたサーボモータ４及び第２アーム３を含め
た粘性摩擦Ｄは、さほど困難なく決定することができ
る。

【００４６】上記推定定数決定部６０では、このように
して得られた定数、すなわち共振極の実部ｐ_r、共振極
の虚部ｐ_i、反共振零点の実部ｚ_r、反共振零点の虚部ｚ
_i、モータ軸からみたサーボモータ４及び第２アーム３
を含めたイナーシャＪから、下記の式（５）、式
（６）、式（７）、式（８）、式（９）、式（１０）に
従って、後段の駆動部推定手段（ねじれ角・外乱推定オ
ブザーバ７）で必要とされる推定定数、すなわち第２ア
ーム３のイナーシャＪ_a、第２アーム３の粘性摩擦Ｄ_a、
バネ定数Ｋ、サーボモータ４のイナーシャＪ_m、サーボ
モータ４の粘性摩擦Ｄ_mを算出し、上記ねじれ角・外乱
推定オブザーバ７に対して出力する。

【００４７】

【数１１】

【００４８】次に、上記ねじれ角・外乱推定オブザーバ
７は、上記駆動部推定手段として、基本的には上記式
（１）から求められる最小次元オブザーバであり、下記
の式（２）及び式（３）を含む式より、指令トルクτ及
びサーボモータ４の角度θ_mを用いて、上記第２アーム
３の推定角度θ_a と、第２アーム３の推定角速度θ
_a ^' と、サーボモータ４の推定角速度θ_m ^' とを算出するも
のである。ただし、前述したように外乱の推定をも行う
ため、当該ねじれ角・外乱推定オブザーバ７は、前記式
（１３）から求められる最小次元オブザーバとなり、下
記の式（１７）及び式（１８）の式より、上記指令トル
クτ及びサーボモータの角度θ_mを用いて、上記第２ア
ーム２の推定角度θ_a 及び推定角速度θ_a ^' と、サーボモ
ータ４の推定角速度θ_m ^' と、推定外乱トルクｄとを算出
するようにしている。

【００４９】

【数１２】

【００５０】

【数１３】

【００５１】ここで、上記式（２），式（３），式（１
７），式（１８）におけるｚ₁，ｚ₂，ｚ₃，ｚ₄は最小次
元オブザーバの状態変数、ｋ₁，ｋ₂，ｋ₃，ｋ₄は推定速
度を決めるパラメータである。

【００５２】なお、本発明の実施の形態の例では、サー
ボモータ４のイナーシャＪ_m を１．７×１０^-4［ｋｇｍ
² ］、サーボモータ４の粘性摩擦Ｄ_m を１.１×１０^-4
［Ｎｍ／ｒａｄ／ｓ］、第２アーム３のイナーシャＪ_a
を０．７６［ｋｇｍ² ］、第２のアーム３の粘性摩擦
Ｄ_a を５［Ｎｍ／ｒａｄ／ｓ］、バネ定数Ｋを８３００
［Ｎｍ／ｒａｄ］、ギヤ比Ｎを５０としている。

【００５３】上記ねじれ角・外乱推定オブザーバ７が算
出した上記第２アーム２の推定角度θ_a 及び推定角速度
θ_a ^' と、サーボモータ４の推定角速度θ_m ^' と、推定外乱
トルクｄとの各値は、制御部９に伝達される。また、当
該制御部９には、端子２０から例えば全体をコントロー
ルするシステムコントローラ等から上記サーボモータ４
の所望の目標角度θ_rを示す信号も供給される。

【００５４】この制御部９は、上記サーボモータ４の所
望の目標角度θ_rと、ロボット２側の角度検出器６で検
出されたサーボモータ４の角度θ_mと、ねじれ角・外乱
推定オブザーバ７で算出された上記第２アーム２の推定
角度θ_a 及び推定角速度θ_a ^' 、サーボモータ４の推定角
速度θ_m ^' 、推定外乱トルクｄの各値に基づいて、上記ア
ンプ部５に指令トルクτを算出する。

【００５５】すなわち当該制御部９は、上記供給された
各値を用いて、上記サーボモータ４の角度θ_m と第２ア
ーム３の推定角度θ_a との差分、サーボモータ４の推定
角速度θ_m ^' と第２アーム３の推定角速度θ_a ^' との差分、
サーボモータ４の目標角速度θ_r ^'とサーボモータ４の推
定角速度θ_m ^' との差分、サーボモータ４の目標角度θ_r
とサーボモータ４の角度θ_m との差分をそれぞれ求め
る。

【００５６】そして、上記制御部９は、下記の式（１
９）により、上記のように求めたサーボモータ４の角度
θ_m と第２アーム３の推定角度θ_a との差分、サーボモ
ータ４の推定角速度θ_m ^' と第２アーム３の推定角速度θ
_a ^' との差分、サーボモータ４の目標角速度θ_r ^'とサーボ
モータ４の推定角速度θ_m ^' との差分、サーボモータ４の
目標角度θ_r とサーボモータ４の角度θ_m との差分、サ
ーボモータ４の目標角速度θ_r ^'と、前記推定外乱トルク
ｄとから、指令トルクτを算出して出力する。

【００５７】

【数１４】

【００５８】なお、上記式（１９）におけるｆ₁ 、ｆ
₂ 、ｆ₃ 、ｆ₄は制御の特性を決める制御パラメータで
ある。

【００５９】上述したようにして求められた指令トルク
τは、制御対象である前記ロボット２に供給される。

【００６０】ここで、図２のロボット２の部分は、より
詳細には図４のように表すことができる。すなわち、こ
の図４は、ロボット２の各部の動作をブロック線図とし
て表すものであり、図２と対応する構成要素には同一の
指示符号を付している。

【００６１】この図４及び図２において、上記ロボット
制御装置１からの指令トルクτの値は、端子３１に伝達
される。この端子３１に伝達された指令トルクτの値
は、演算部３３に加算値として入力する。また、この演
算部３３には、端子３０から前記外乱トルクｄが減算値
として伝達される。すなわち、当該演算部３３では、上
記指令トルクτに対する外乱トルクｄの影響量を差し引
くことを行っている。この演算部３３の出力値は、演算
部３４に加算値として入力する。

【００６２】この演算部３４には、後述する演算部２４
からの出力が減算値として伝達され、当該演算部３４で
は上記演算部３３の出力から演算部２４の出力値を減算
した値を求める。この演算部３４の出力は、演算部２３
に入力する。なお、上記演算部３３及び３４が図２の演
算部２２に対応している。

【００６３】上記演算部２３は、演算部３５及び３６か
らなり、前記サーボモータ４に対応している。上記演算
部２３では、演算部３５にて前記サーボモータ４のイナ
ーシャＪ_mとサーボモータ４の粘性摩擦Ｄ_mより、１／
（Ｊ_mＳ＋Ｄ_m）で表される演算を行い、さらに演算部３
６にて上記演算部３５の出力値を積分する演算を行い、
これら演算｛１／（Ｊ_mＳ＋Ｄ_m）Ｓ｝により得られた値
を出力する。すなわち、当該演算部２３において、上記
演算部３５の出力値は上記サーボモータ４の角速度θ_m ^'
に対応し、上記演算部３６の出力値は上記サーボモータ
４の速度θ_mに対応する。なお、上記式中のＳはラプラ
ス変換演算子である。

【００６４】上述したようなことから、演算部２２から
演算部２３までのブロックは、制御対象であるロボット
２における指令トルクτに基づいた実際のサーボモータ
４の駆動と、このサーボモータ４の駆動に応じた角度検
出器６における角度検出動作とに対応している。上記角
度検出器６にて検出されたサーボモータ４の角度θ
_mは、端子３９を介してロボット制御装置１にフィード
バックされることになる。

【００６５】上記演算部２３の出力値は、演算部２５に
伝達され、この演算部２５にて１／Ｎ倍になされる。す
なわち、上記Ｎは前記ギヤ比であり、当該演算部２５
は、上記サーボモータ４のモータ出力軸に接続された前
記減速機構２１５と対応している。この演算部２５の出
力値は、第２アーム３の角度θ_aに相当し、この値が演
算部２６に加算値として伝達される。

【００６６】上記演算部２６から演算部２８までの構成
は、ロボット２の第２アーム３に対応している。上記演
算部２６には、後述する演算部２８から伝達されたロボ
ット２の第２アーム３の実際の角度θ_aが、減算値とし
て供給されている。すなわちこの演算部２６では、演算
部２８から伝送される第２アーム３の実際の角度θ
_aを、上記演算部２５から伝達される角度θ_aまで変化さ
せる際の、角度変化量を求めている。

【００６７】上記演算部２６の出力値は、演算部２７に
伝達され、ここで前記バネ定数であるＫが掛けられた
後、さらに演算部２４にて１／Ｎ倍されて、前記演算部
３４に減算値として伝達される。すなわち、上記演算部
２７及び２４では、上記サーボモータ４の角度の値をサ
ーボモータ４のトルクの値に変換している。これによ
り、前記演算部３４では、前記演算部３３で求められた
トルクの値から現在のサーボモータ４のトルクの値が差
し引かれた値が求められる。

【００６８】また、上記演算部２７の出力値は、演算部
２８にも伝達される。当該演算部２８は、演算部３７及
び３８からなる。当該演算部２８では、演算部３７にて
前記第２アーム３のイナーシャＪ_aと第２アーム３の粘
性摩擦Ｄ_aより、１／（Ｊ_aＳ＋Ｄ_a）で表される演算を
行い、さらに演算部３８にて上記演算部３７の出力値を
積分する演算を行い、これら演算｛１／（Ｊ_aＳ＋Ｄ_a）
Ｓ｝により得られた値を出力する。すなわち、当該演算
部２８において、上記演算部３７の出力値は上記第２ア
ーム３の角速度θ_a ^'に対応し、上記演算部３８の出力値
は上記第２アーム３の角度θ_aに対応する。

【００６９】上記演算部２８にて求められた上記第２ア
ーム３の実際の角度θ_aは、端子２９から外部へ作用す
る力として取り出されることになる。

【００７０】以上の説明で明らかなように、本発明の実
施の形態のロボット制御装置においては、剛性の低い２
慣性のロボット２に対して外乱に強い制御を行い、ロボ
ット２側の第２アーム３の振動の抑制を図ると共に、当
該ロボット２側の第２アーム３の位置決めの高速化を図
るために必要不可欠である推定演算で用いる定数を簡便
に求めることができる。これにより、ロボット２の振動
抑制能力が向上すると共に、ロボット２の調整の効率を
著しく向上させることができる。

【００７１】なお、図２の例では、説明を容易にするた
め、ロボット制御装置１を機能的な面から、ねじれ角・
外乱推定オブザーバ７と、制御部９と、推定定数決定部
６０との、各主要演算手段に便宜上分割しているが、こ
れら各演算手段は必ずしも上述のように区切る必要はな
く、共通化した演算手段としたり、さらにこれらに属さ
ない他の演算手段を有するものとしても良い。また、本
発明の実施の形態ロボット制御装置１は、各演算手段の
代わりに、専用の演算手段を用いても良く、例えばパー
ソナルコンピュータなどの演算装置による全体の演算手
段を用いても良い。

【００７２】

【発明の効果】以上の説明で明らかなように、本発明の
ロボット制御装置によれば、測定された周波数特性か
ら、推定演算に必要な定数、すなわち駆動部のイナーシ
ャＪ_a、駆動部の粘性摩擦Ｄ_a、バネ定数Ｋ、サーボモー
タのイナーシャＪ_m、サーボモータの粘性摩擦Ｄ_mを簡便
に求めることが可能となる。すなわち、本発明のロボッ
ト制御装置においては、剛性の低い２慣性のロボットに
対して外乱に強い制御を行い、ロボット側の駆動部の振
動の抑制を図ると共に、ロボット側の駆動部の位置決め
の高速化を図るために必要不可欠である推定演算で用い
る定数を簡便に求めることができ、これにより、ロボッ
トの振動抑制能力が向上すると共に、ロボットの調整の
効率を著しく向上させることができる。

【図面の簡単な説明】

【図１】本発明の実施の形態のロボット制御装置とその
制御対象であるロボットを示す斜視図である。

【図２】本発明のロボット制御装置においてロボットを
制御するための具体的な構成を示すブロック線図であ
る。

【図３】２慣性系の周波数特性とその特徴量の説明に用
いる特性図である。

【図４】ロボットのアームを駆動する状態を説明するた
めに示すブロック線図である。

【図５】従来のロボット制御装置においてロボットを制
御するための具体的な構成を示すブロック線図である。

【図６】従来のロボット制御装置の制御対象であるロボ
ットを模式的に示す図である。

【符号の説明】

１ロボット制御装置、２ロボット、３第２
アーム、４サーボモータ、５アンプ、６角
度検出器、７ねじれ角・外乱推定オブザーバ、９
制御部、６０推定定数決定部、ｄ外乱トル
ク、ｄ推定外乱トルク、 τ 指令トルク、 θ_a
第２アームの角度、 θ_a ^' 第２アームの角速度、
θ_a 第２アームの推定角度、 θ_a ^' 第２アームの推
定角速度、θ_m サーボモータの角度、 θ_m ^' サーボ
モータの角速度、 θ_m ^' サーボモータの推定角速度、
θ_r サーボモータの目標角度、ｐ_r 共振極の実
部、ｐ_i 共振極の虚部、ｚ_r 反共振零点の実部、
ｚ_i 反共振零点の虚部、Ｊモータ軸からみたサーボ
モータ及び第２アームを含めたイナーシャ、Ｄモータ
軸からみたサーボモータ及び第２アームを含めた粘性摩
擦、Ｊ_a 第２アームのイナーシャ、Ｄ_a 第２アー
ムの粘性摩擦、Ｋバネ定数、Ｊ_mサーボモータの
イナーシャ、Ｄ_m サーボモータの粘性摩擦、ｆ_c
低域のカットオフ周波数、ｆ_p 共振周波数、ｆ_z
反共振周波数、Ｑ_p 共振のＱ値、Ｑ_z 反共振のＱ
値

Claims

【特許請求の範囲】

【請求項１】駆動部と、上記駆動部を駆動するサーボ
モータと、上記サーボモータにトルクを発生させるアン
プと、上記サーボモータの角度を検出する角度検出手段
とを備えるロボットを制御対象とし、上記アンプに与える指令トルクと上記角度検出手段より
検出されたサーボモータの角度とに基づいて、少なくと
も上記ロボットの駆動部の角度若しくは上記ロボットの
駆動部の角速度を推定する駆動部推定手段と、上記駆動部推定手段により推定された上記ロボットの駆
動部の推定角度若しくは上記ロボットの駆動部の推定角
速度に基づいて、上記アンプに与える指令トルクを算出
する制御手段と、上記ロボットの駆動部の角度若しくは上記ロボットの駆
動部の角速度を推定するための定数を、上記アンプに与
える指令トルクから上記角度検出手段により検出される
サーボモータの角度までの伝達関数から決定する推定定
数決定手段とを有し、上記制御手段によって算出された指令トルクに基づいて
上記ロボットを制御することを特徴とするロボット制御
装置。
【請求項２】下記の式（１）を含む状態方程式にて表
現される上記ロボットを制御することを特徴とする請求
項１記載のロボット制御装置。【数１】なお、上記式（１）におけるθ_mはサーボモータの角
度、θ_m ^'はサーボモータの角速度、Ｊ_mはサーボモータ
のイナーシャ、Ｄ_mはサーボモータの粘性摩擦、θ_aは駆
動部の角度、θ_a ^'は駆動部の角速度、Ｊ_aは駆動部のイ
ナーシャ、Ｄ_aは駆動部の粘性摩擦、Ｋはサーボモータ
軸と駆動部間に配されるギヤのバネ定数、Ｎは上記ギヤ
のギヤ比、τは指令トルクである。
【請求項３】上記駆動部推定手段は、上記式（１）か
ら求められる最小次元オブザーバであり、下記の式
（２）及び式（３）を含む式より、指令トルクτ及びサ
ーボモータの角度θ_mを用いて、上記駆動部の推定角度
θ_a と、上記駆動部の推定角速度θ_a ^' と、サーボモータ
の推定角速度θ_m ^' とを算出することを特徴とする請求項
２記載のロボット制御装置。【数２】なお、上記式（２），式（３）におけるｚ₁，ｚ₂，ｚ₃
は最小次元オブザーバの状態変数、ｋ₁，ｋ₂，ｋ₃は推
定速度を決めるパラメータである。
【請求項４】上記推定定数決定手段は、上記アンプに
与える指令トルクから上記角度検出手段により検出され
るサーボモータの角度までの伝達関数Ｇ(s)が下記の式
（４）で表されるときに、下記の式（５），式（６），
式（７），式（８），式（９）に従って、上記駆動部推
定手段における推定演算で必要とされる定数として、駆
動部のイナーシャＪ_aと、駆動部の粘性摩擦Ｄ_aと、バネ
定数Ｋと、サーボモータのイナーシャＪ_mと、サーボモ
ータの粘性摩擦Ｄ_mとを算出することを特徴とする請求
項３記載のロボット制御装置。【数３】なお、上記式（５），式（６），式（７），式（８）に
おいて、ｐ_rは共振極の実部、ｐ_iは共振極の虚部、ｚ_r
は反共振零点の実部、ｚ_iは反共振零点の虚部、Ｊはモ
ータ軸からみたサーボモータ及び駆動部を含めたイナー
シャあり、Ｄ’は仮想的な粘性摩擦である。