JPH09212203A - ロボット制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 ロボットのアームの振動の抑制を図るととも
に、当該アームの位置決めの高速化を図る。 【解決手段】 第２アーム３を駆動するサーボモータ
４、サーボモータ４にトルクを発生させるアンプ部５及
びサーボモータ４の角度を検出する角度検出器６を備え
るロボット２を制御対象とし、指令トルクτとサーボモ
ータ４の角度θ_m とに基づいて、アーム３の角度θ_a 及
び角速度θ_a ^'、サーボモータ４の角速度θ_m ^'、外乱トル
クｄを推定するねじれ角・外乱推定オブザーバ７と、サ
ーボモータ４の目標角度θ_r に基づいて、サーボモータ
４の目標角速度θ_r ^'を算出するフィードフォワード部８
と、サーボモータ４の目標角度θ_rと角度θ_mと目標角速
度θ_r、第２アーム３の推定角度θ_a と推定角速度
θ_a ^' 、サーボモータ４の推定角速度θ_m ^' 及び推定外乱ト
ルクｄに基づいて、アンプ５に与える指令トルクτを算
出する制御部９とを備え、制御部９が算出した指令トル
クτに基づいてロボット２を制御する。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

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

【０００５】

【数１７】

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

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

【０００８】

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

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

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

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

【００１２】したがって、本発明は、上述した課題を解
決するために、アーム等の駆動部の振動の抑制を図ると
共に、当該駆動部の位置決めの高速化を図るロボット制
御装置を提供することを一つの目的とする。

【００１３】また、一般に、サーボモータとアンプは、
それぞれ発生することができる限界である最大トルクが
ある。前記従来のロボット制御装置１２０は、目標角加
速度を抑えることによって、制御部１２１からの指令ト
ルクが、上記最大トルクを越えないようにしていた。し
かし、ロボット１２２に与えられる指令トルクが上記最
大トルクより小さいと、アーム１２４の高速化が困難と
なる。

【００１４】さらに、通常、上記アンプ内にはトルクを
制御する機能が備えられている場合があるが、この場
合、制御部１２１から算出される指令トルクと、実際に
発生するトルクとが一致しなくなり、良好な制御結果が
得られなくなる。また、単に、指令トルクに対して制限
を設けたとしても、上述同様に、算出される指令トルク
と実際に発生するトルクとが一致しなくなり、良好な制
御結果が得られなくなる。

【００１５】そこで、本発明は、上述した課題を解決す
るために、指令トルクを制限しながら良好な制御結果を
得ることを実現すると共に、サーボモータとアンプの能
力を有効に用いることにより、駆動部の位置決めの高速
化を可能にするロボット制御装置を提供することをも目
的とする。

【００１６】

【課題を解決するための手段】本発明に係るロボット制
御装置は、アーム等の駆動部と、駆動部を駆動するサー
ボモータと、サーボモータにトルクを発生させるアンプ
と、サーボモータの角度を検出する角度検出手段とを備
えるロボットを制御対象とする。ロボット制御装置は、
アンプに与える指令トルクと角度検出手段より検出され
たサーボモータの角度とに基づいて、ロボットの駆動部
の角度、ロボットの駆動部の角度の時間微分値であるロ
ボットの駆動部の角速度、サーボモータの角度の時間微
分値であるサーボモータの角速度、ロボットに外乱とし
て加わる外乱トルクを推定するねじれ角・外乱推定手段
を備える。また、ロボット制御装置は、サーボモータの
目標角度に基づいて、サーボモータの目標角速度を算出
するフィードフォワード手段を備える。さらに、ロボッ
ト制御装置は、サーボモータの目標角度、角度検出手段
より検出されたサーボモータの角度、フィードフォワー
ド手段より算出されたサーボモータの目標角速度、上記
ねじれ角・外乱推定手段より推定されたロボットの駆動
部の推定角度、ロボットの駆動部の推定角速度、サーボ
モータの推定角速度及び推定外乱トルクに基づいて、ア
ンプに与える指令トルクを算出する制御手段とを備える
ことにより、上述した課題を解決する。

【００１７】すなわち、以上のように構成された本発明
に係るロボット制御装置によれば、フィードフォワード
手段が、サーボモータの目標角度に基づいて、サーボモ
ータの目標角速度を算出して制御手段に出力する。ねじ
れ角・外乱推定手段は、指令トルクとサーボモータの角
度とに基づいて、ロボットの駆動部の推定角度、ロボッ
トの駆動部の推定角速度、サーボモータの推定角速度、
推定外乱トルクを算出して制御手段に出力する。制御手
段は、サーボモータの目標角度、サーボモータの角度、
サーボモータの目標角速度、ロボットの駆動部の推定角
度、ロボットの駆動部の推定角速度、サーボモータの推
定角速度及び推定外乱トルクに基づいて、アンプに与え
る指令トルクを算出してロボットのアンプに出力する。
ロボット制御装置は、制御手段によって算出された指令
トルクに基づいて上記ロボットを制御する。

【００１８】また、本発明に係るロボット制御装置は、
アーム等の駆動部と、駆動部を駆動するサーボモータ
と、サーボモータにトルクを発生させるアンプと、サー
ボモータの角度を検出する角度検出手段とを備えるロボ
ットを制御対象とする。ロボット制御装置は、上記アン
プに与える指令トルクと角度検出手段より検出されたサ
ーボモータの角度とに基づいて、ロボットの駆動部の角
度、ロボットの駆動部の角度の時間微分値であるロボッ
トの駆動部の角速度、サーボモータの角度の時間微分値
であるサーボモータの角速度、ロボットに外乱として加
わる外乱トルクを推定するねじれ角・外乱推定手段を備
える。また、ロボット制御装置は、サーボモータの目標
角度に基づいて、サーボモータの目標角速度を算出する
フィードフォワード手段を備える。さらに、ロボット制
御装置は、サーボモータの目標角度、角度検出手段より
検出されたサーボモータの角度、フィードフォワード手
段より算出されたサーボモータの目標角速度、ねじれ角
・外乱推定手段より推定されたロボットの駆動部の推定
角度、ロボットの駆動部の推定角速度、サーボモータの
推定角速度及び推定外乱トルクに基づいて、計算トルク
を算出する制御手段を備える。さらに、ロボット制御装
置は、制御手段により算出された計算トルクに基づい
て、予め設定されたトルクリミット値によって制限され
たアンプに与える指令トルクを算出するトルクリミット
手段とを備えることにより、上述した課題を解決する。

【００１９】すなわち、以上のように構成された本発明
に係るロボット制御装置によれば、フィードフォワード
手段が、サーボモータの目標角度に基づいて、サーボモ
ータの目標角速度を算出して制御手段に出力する。ねじ
れ角・外乱推定手段は、指令トルクとサーボモータの角
度とに基づいて、ロボットの駆動部の推定角度、ロボッ
トの駆動部の推定角速度、サーボモータの推定角速度、
推定外乱トルクを算出して制御手段に出力する。制御手
段は、サーボモータの目標角度、サーボモータの角度、
サーボモータの目標角速度、ロボットの駆動部の推定角
度、ロボットの駆動部の推定角速度、サーボモータの推
定角速度及び推定外乱トルクに基づいて、計算トルクを
算出してトルクリミット手段に出力する。トルクリミッ
ト手段は、計算トルクとトルクリミット値によって算出
された指令トルクをロボットのアンプに出力する。ロボ
ット制御装置は、トルクリミット手段によって算出され
た指令トルクに基づいてロボットを制御する。

【００２０】

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

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

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

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

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

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

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

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

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

【００２９】ここで、上記ロボット２は、下記の式
（１）に示すように、指令トルクτ、サーボモータ４の
角度θ_m 、サーボモータ４の角速度θ_m ^'、第２アーム３
の角度θ_a、第２アーム３の角速度θ_a ^'、サーボモータ
４のモータ軸に加わる外乱トルクｄを、それぞれ状態変
数とした状態方程式で表されるものである。なお、式
（１）におけるＪ_m はサーボモータ４のイナーシャ、Ｄ
_m はサーボモータ４の粘性摩擦、Ｊ_a は第２アーム３の
イナーシャ、Ｄ_a は第２アーム３の粘性摩擦、Ｋはバネ
定数、Ｎはギヤ比である。

【００３０】

【数１８】

【００３１】すなわち、ロボット２では、上記式（１）
により表されたダイナミックスに基づいて、サーボモー
タ４が第２アーム３を駆動する。

【００３２】ここで、第１の実施の形態のロボット制御
装置１は、図２に示すように、主要構成要素として、ね
じれ角・外乱推定オブザーバ７と、フィードフォワード
部８と、制御部９とを備えており、端子２０には例えば
全体をコントロールするシステムコントローラ等から上
記サーボモータ４の所望の目標角度θ_rを示す信号が入
力される。

【００３３】上記ねじれ角・外乱推定オブザーバ７は、
図２に示すように、制御対象であるロボット２につい
て、最小次元オブザーバを構成することにより、ロボッ
ト２の第２アーム３の角度θ_a 、ロボット２の第２アー
ム３の角速度θ_a ^'、サーボモータ４の角速度θ_m ^'及び外
乱トルクｄを推定する。

【００３４】すなわち、ねじれ角・外乱推定オブザーバ
７は、指令トルクτと、角度検出器６から検出されたサ
ーボモータ４の実際の角度θ_m とが入力され、式
（２）、式（３）に示すように、ロボット２の第２アー
ム３の推定角度θ_a 、第２アーム３の推定角速度θ_a ^' 、
及びサーボモータ４の推定角速度θ_m ^' を算出し、さらに
ロボット２に外乱として加わる値として推定外乱トルク
ｄを算出する。これら第２アーム３の推定角度θ_a 、第
２アーム３の推定角速度θ_a ^' 、及びサーボモータ４の推
定角速度θ_m ^' 、及び推定外乱トルクｄの各値は、制御部
９に送られる。

【００３５】

【数１９】

【００３６】ここで、式（２）、式（３）におけるｚ
₁ 、ｚ₂ 、ｚ₃ 、ｚ₄ は最小次元オブザーバの状態変
数、ｋ₁ 、ｋ₂ 、ｋ₃ 、ｋ₄ は推定速度を決めるパラメ
ータである。また、ロボット２の第２アーム３の推定角
速度θ_a ^' は、第２アーム３の推定角度θ_a の時間微分に
よって求めるようにしても良い。さらに、ロボット２の
サーボモータ４の推定角速度θ_m ^' は、ロボット２のサー
ボモータ４の推定角度θ_m の時間微分によって求めるよ
うにしても良い。

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

【００３８】次に、フィードフォワード部８は、図２に
示すように、上記端子２０に供給された上記サーボモー
タ４の所望の目標角度θ_r の値が入力され、このサーボ
モータ４の目標角度θ_r を時間微分してサーボモータ４
の目標角速度θ_r ^'を算出して出力する。ここで、サーボ
モータ４の目標角速度θ_r ^'は、サーボモータ４の角速度
θ_m ^'と上記サーボモータ４の所望の目標角度θ_r との差
分によって求めるようにしても良い。またこのサーボモ
ータ４の目標角速度θ_r ^'は、サーボモータ４の目標角度
θ_r が求められるのと同時に求めるようにしても良い。
なお、フィードフォワード部８において、上記サーボモ
ータ４の目標角速度θ_r ^'を上記サーボモータ４の所望の
目標角度θ_r との差分によって求めるようにする場合に
は、当該フィードフォワード部８に対して上記ねじれ角
・外乱推定オブザーバ７からサーボモータ４の角速度θ
_m ^'を供給する。上記フィードフォワード部８から出力さ
れた上記サーボモータ４の目標角速度θ_r ^'の値は、制御
部９の後段に設けられている演算部２１に送られること
になる。

【００３９】制御部９には、図２に示すように、端子２
０からのサーボモータ４の目標角度θ_r の値と、ねじれ
角・外乱推定オブザーバ７からの第２アーム３の推定角
度θ_a 、第２アーム３の推定角速度θ_a ^' 、サーボモータ
４の推定角速度θ_m ^' 、推定外乱トルクｄの各値と、上記
角度検出器６にて検出された上記サーボモータ４の角度
θ_mの値とが入力される。

【００４０】この制御部９及びその後段の演算部２１で
は、上記供給された各値を用いて、上記サーボモータ４
の角度θ_m と第２アーム３の推定角度θ_a との差分、サ
ーボモータ４の推定角速度θ_m ^' と第２アーム３の推定角
速度θ_a ^' との差分、サーボモータ４の目標角速度θ_r ^'と
サーボモータ４の推定角速度θ_m ^' との差分、サーボモー
タ４の目標角度θ_r とサーボモータ４の角度θ_m との差
分をそれぞれ求める。

【００４１】そして、上記制御部９及びその後段の演算
部２１では、下記の式（４）により、上記のように求め
たサーボモータ４の角度θ_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は、端子２９から外部へ作用す
る力として取り出されることになる。

【００５５】上述したように、本発明の第１の実施の形
態ロボット制御装置１においては、ねじれ角と外乱とを
同時に推定するねじれ角・外乱推定手段を備えたことに
より、剛性の低い２慣性のロボット２に対して外乱に強
い制御を行うので、ロボット２側の第２アーム３の振動
の抑制が図られるとともに、当該ロボット２側の第２ア
ーム３の位置決めの高速化が図られる。したがって、こ
のロボット制御装置１によれば、ロボット２による電子
部品の組み立て等の作業効率の向上が図られる。

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

【００５７】次に、本発明の第２の実施の形態のロボッ
ト制御装置１を図４に示す。この第２の実施の形態のロ
ボット制御装置１は、図４に示すように、基本的な構成
を前記第１の実施の形態のロボット制御装置と同様とす
るが、ねじれ角・外乱推定オブザーバ１１と、フィード
フォワード部１２と、制御部１３の算出手段が異なる点
に特徴を有している。なお、以下の説明において、前述
した第１の実施の形態装置と同一部材、同一構成部につ
いては、同一符号を付すことにより、説明を省略する。

【００５８】この図４に示す第２の実施の形態のロボッ
ト制御装置１において、ねじれ角・外乱推定オブザーバ
１１は、制御対象であるロボット２について最小次元オ
ブザーバ１１を構成し、前述した状態方程式をもとに、
ロボット２側のアンプ部５に与える指令トルクτと、ロ
ボット２側の角度検出器６より検出されたサーボモータ
４の角度θ_m とに基づいて、ねじれ角度θ_t 、ねじれ角
速度θ_t ^'、サーボモータ４の角速度θ_m ^'及び外乱トルク
ｄを推定する。

【００５９】すなわち、ねじれ角・外乱推定オブザーバ
１１は、前記指令トルクτと前記角度検出器６から検出
されたサーボモータ４の角度θ_m とが入力される。この
ねじれ角・外乱推定オブザーバ１１は、下記の式
（６）、式（７）により、ロボット２の第２アーム３の
推定角度θ_a 及びロボット２側のアーム３の推定角速度
θ_a ^' と、サーボモータ４の推定角速度θ_m ^' とを算出し、
また、ロボット２に外乱として加わる値として推定外乱
トルクｄを算出する。さらに、ねじれ角・外乱推定オブ
ザーバ１１は、下記の式（８）を用い、サーボモータ４
の角度θ_m とロボット２側のアーム３の推定角度θ_a ^' と
の差分によって、ねじれ推定角度θ_t を算出し、また、
下記の式（９）を用い、サーボモータ４の推定角度θ_m
とロボット２側のアーム３の推定角速度θ_a ^' との差分に
よって、ねじれ推定角速度θ_t ^' を算出する。

【００６０】すなわち、上記ねじれ角・外乱推定オブザ
ーバ１１は、下記の式（６）、式（７）、式（８）、式
（９）に対して、指令トルクτ及びサーボモータ４の角
度θ_m を代入してねじれ推定角度θ_t と、ねじれ推定角
速度θ_t ^' と、サーボモータ４の推定角速度θ_m ^' 及び推定
外乱トルクｄをそれぞれ算出して出力する。

【００６１】

【数２１】

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

【００６３】ここで、ねじれ推定角速度θ_t ^' は、ねじれ
角度θ_t の時間微分によって算出するようにしても良
い。また、ロボット２の第２アーム３の推定角速度θ_a ^'
は、ロボット２の第２アーム３の推定角度θ_a の時間微
分によって求めるようにしても良い。さらに、ロボット
２のサーボモータ４の推定角速度θ_m ^' は、ロボット２の
サーボモータ４の推定角度θ_m の時間微分によって求め
らるようにしても良い。

【００６４】次に、フィードフォワード部１２には、図
４に示すように、端子４０を介してサーボモータ４の所
望の目標角度θ_r が入力される。当該フィードフォワー
ド部１２は、上記サーボモータ４の目標角速度θ_r ^'を時
間微分してサーボモータ４の目標角加速度θ_r ^"を求め
る。そして、フィードフォワード部１２は、下記の式
（１０）に対して、これらサーボモータ４の目標角速度
θ_r ^'及びサーボモータ４の目標角加速度θ_r ^"を代入し
て、フィードフォワード値ffを算出して出力する。

【００６５】

【数２２】

【００６６】なお、上記式（１０）におけるff₁ 、ff₂
はフィードフォワードパラメータである。ただし、この
第２の実施の形態のロボット制御装置１においては、サ
ーボモータ４の目標角加速度θ_r ^"を用いていないので、
フィードフォワードパラメータff₂ を零としている。

【００６７】ここで、サーボモータ４の目標角速度θ_r ^'
は、サーボモータ４の角速度θ_m ^'と上記サーボモータ４
の所望の目標角度θ_r との差分によって求めるようにし
ても良い。またこのサーボモータ４の目標角速度θ
_r ^'は、サーボモータ４の目標角度θ_r が求められるのと
同時に求めるようにしても良い。なお、フィードフォワ
ード部１２において、上記サーボモータ４の目標角速度
θ_r ^'を上記サーボモータ４の所望の目標角度θ_r との差
分によって求めるようにする場合には、当該フィードフ
ォワード部１２に対して上記ねじれ角・外乱推定オブザ
ーバ１１からサーボモータ４の角速度θ_m ^'を供給する。
また、サーボモータ４の目標角加速度θ_r ^"は、このサー
ボモータ４の角速度θ_m ^'を時間微分してサーボモータ４
の角加速度θ_m ^"を求めて、このサーボモータ４の角加速
度θ_m ^"との差分によって求めるようにしても良い。これ
らサーボモータ４の目標角速度θ_r ^'及びサーボモータ４
の目標角加速度θ_r ^"は、サーボモータ４の目標角度θ_r
が求められるのと同時に求められても良い。

【００６８】次に、制御部１３には、図４に示すよう
に、上記サーボモータ４の所望の目標角度θ_rと、ロボ
ット２側の角度検出器６で検出されたサーボモータ４の
角度θ_mと、フィードフォワード部１２で算出されたフ
ィードフォワード値ffと、ねじれ角・外乱推定オブザー
バ１１で算出されたねじれ推定角度θ_t 、ねじれ推定角
速度θ_t ^' 、サーボモータ４の推定角速度θ_m ^' 、推定外乱
トルクｄとが入力される。当該制御部１３では、これら
に基づいて、サーボモータ４の目標角度θ_r とサーボモ
ータ４の角度θ_m との差分、サーボモータ４の目標角速
度θ_r ^'とサーボモータ４の推定角速度θ_m ^' との差分を求
める。そして、制御部１３は、下記式（１１）に対し
て、これらねじれ角の推定値θ_t 、ねじれ推定角速度θ
_t ^' 、推定外乱トルクｄ、サーボモータ４の目標角度θ_r
とサーボモータ４の角度θ_m との差分、サーボモータ４
の目標角速度θ_r ^'とサーボモータ４の推定角速度θ_m ^' と
の差分を代入して、指令トルクτを算出し、この推定ト
ルクτをアンプ部５に出力する。

【００６９】

【数２３】

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

【００７１】上述したようにして求められた指令トルク
τは、ロボット２に供給される。

【００７２】ロボット２は、前述同様の状態方程式によ
って表現されるものであり、ロボット制御装置１の制御
部１３により算出された指令トルクτを用い、サーボモ
ータ４の角度θ_m 、サーボモータ４の角速度θ_m ^'、アー
ム３の角度θ_a 、アーム３の角速度θ_a ^'、外乱トルクｄ
を状態変数としたダイナミックスに基づいて、サーボモ
ータ４が第２アーム３を駆動する。ロボット２の角度検
出器６は、このサーボモータ４の角度θ_m を検出し、ね
じれ角・外乱推定オブザーバ１１及び制御部１３に出力
する。

【００７３】上述した第２の実施の形態ロボット制御装
置１によれば、ねじれ角と外乱とを同時に推定するねじ
れ角・外乱推定手段を備えたことにより、剛性の低い２
慣性のロボット２に対して外乱に強い制御を行うので、
ロボット２側の第２アーム３の振動の抑制が図られると
ともに、当該ロボット２側の第２アーム３の位置決めの
高速化が図られる。また、このロボット制御装置１によ
れば、ロボット２による電子部品の組み立て等の作業効
率の向上が図られる。

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

【００７５】次に、本発明の第３の実施の形態として示
すロボット制御装置について説明する。この第３の実施
の形態のロボット制御装置は、図５に示すように、前記
図２の第１の実施の形態のロボット制御装置１にトルク
リミット部９１を追加した構成であり、例えば基板に部
品を実装する際に用いる組立用の前記図１に示したもの
と同様のロボット２を制御するものである。

【００７６】この図５に示すように、第３の実施の形態
のロボット制御装置１は、主要構成要素として、ねじれ
角・外乱推定オブザーバ７と、フィードフォワード部８
と、制御部９０と、トルクリミット部９１とを備えてい
る。

【００７７】上記ねじれ角・外乱推定オブザーバ７とフ
ィードフォワード部８とは、前述の第１の実施の形態の
ロボット制御装置１と同様であり、ここでは詳細な内容
についての説明は省略する。

【００７８】制御部９０には、端子２０からのサーボモ
ータ４の目標角度θ_r の値と、ねじれ角・外乱推定オブ
ザーバ７からの第２アーム３の推定角度θ_a 、第２アー
ム３の推定角速度θ_a ^' 、サーボモータ４の推定角速度θ
_m ^' 、推定外乱トルクｄの各値と、上記角度検出器６にて
検出された上記サーボモータ４の角度θ_mの値とが入力
される。

【００７９】この制御部９０及びその後段の演算部２１
では、上記供給された各値を用いて、上記サーボモータ
４の角度θ_m と第２アーム３の推定角度θ_a との差分、
サーボモータ４の推定角速度θ_m ^' と第２アーム３の推定
角速度θ_a ^' との差分、サーボモータ４の目標角速度θ_r ^'
とサーボモータ４の推定角速度θ_m ^' との差分、サーボモ
ータ４の目標角度θ_r とサーボモータ４の角度θ_m との
差分をそれぞれ求める。そして、上記制御部９及びその
後段の演算部２１では、下記の式（１５）により、上記
のように求めたサーボモータ４の角度θ_m と第２アーム
３の推定角度θ_a との差分、サーボモータ４の推定角速
度θ_m ^' と第２アーム３の推定角速度θ_a ^' との差分、サー
ボモータ４の目標角速度θ_r ^'とサーボモータ４の推定角
速度θ_m ^' との差分、サーボモータ４の目標角度θ_r とサ
ーボモータ４の角度θ_m との差分、サーボモータ４の目
標角速度θ_r ^'と、前記推定外乱トルクｄとから、計算ト
ルクτ_eを算出して出力する。

【００８０】

【数２４】

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

【００８２】上述したようにして制御部９０にて求めら
れた計算トルクτ_eは、トルクリミット部９１に送られ
る。

【００８３】上記トルクリミット部９１は、下記の式
（１６）によって、制御部９０で算出された計算トルク
τ_eと、予め設定されたトルクリミット値τ_lとに基づい
て、ロボット２のアンプ部５に与える前記同様の指令ト
ルクτを算出する。

【００８４】

【数２５】

【００８５】上記トルクリミット部９１にて算出された
指令トルクτがロボット２に伝達される。なお、図５の
ロボット２の部分は、より詳細には前述した図３と同様
に表すものであり、その詳細な内容の説明については省
略する。

【００８６】上述した第３の実施の形態のロボット制御
装置１によれば、トルクリミット部９１を備えたことに
より、サーボモータ４とアンプ部５によるトルクの制限
に従いつつ、サーボモータ４とアンプ部５の能力を十分
に引き出すことができ、これにより、ロボット２側の第
２アーム３の位置決めの高速化が図られる。したがっ
て、このロボット制御装置１によれば、ロボット２によ
る電子部品の組み立て等の作業効率の向上が図られる。

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

【００８８】次に、本発明の第４の実施の形態のロボッ
ト制御装置１を図６に示す。この第４の実施の形態のロ
ボット制御装置は、図６に示すように、前記図４に示し
た第２の実施の形態のロボット制御装置１にトルクリミ
ット部９２を追加した構成であり、例えば基板に部品を
実装する際に用いる組立用の前記図１に示したものと同
様のロボット２を制御するものである。すなわち、この
第４の実施の形態のロボット制御装置１は、図６に示す
ように、基本的な構成を前記第２の実施の形態ロボット
制御装置と同様とするが、制御部９３と、トルクリミッ
ト部９２が異なる点に特徴を有している。なお、以下の
説明において、前述した第２の実施の形態装置と同一部
材、同一構成部については、同一符号を付すことによ
り、説明を省略する。

【００８９】この第３の実施の形態のロボット制御装置
１においては、上記ねじれ角・外乱推定オブザーバ１１
とフィードフォワード部１２とは、前述の第２の実施の
形態のロボット制御装置１と同様であるため、それらの
詳細な説明は省略する。

【００９０】制御部９３では、上記サーボモータ４の所
望の目標角度θ_rと、ロボット２側の角度検出器６で検
出されたサーボモータ４の角度θ_mと、フィードフォワ
ード部１２で算出されたフィードフォワード値ffと、ね
じれ角・外乱推定オブザーバ１１で算出されたねじれ推
定角度θ_t 、ねじれ推定角速度θ_t ^' 、サーボモータ４の
推定角速度θ_m ^' 、推定外乱トルクｄとが入力される。こ
の制御部９３では、これらに基づいて、サーボモータ４
の目標角度θ_r とサーボモータ４の角度θ_m との差分、
サーボモータ４の目標角速度θ_r ^'とサーボモータ４の推
定角速度θ_m ^' との差分を求める。そして、制御部１３
は、下記式（２３）に対して、これらねじれ角の推定値
θ_t 、ねじれ推定角速度θ_t ^' 、推定外乱トルクｄ、サー
ボモータ４の目標角度θ_r とサーボモータ４の角度θ_m
との差分、サーボモータ４の目標角速度θ_r ^'とサーボモ
ータ４の推定角速度θ_m ^' との差分を代入して、計算トル
クτ_eを算出し、当該算出トルクτ_eをトルクリミット部
９２に伝送する。

【００９１】

【数２６】

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

【００９３】上述したようにして制御部９３にて求めら
れた計算トルクτ_eは、トルクリミット部９２に送られ
る。

【００９４】上記トルクリミット部９２は、下記の式
（２４）によって、制御部９３で算出された計算トルク
τ_eと、予め設定されたトルクリミット値τ_lとに基づい
て、ロボット２のアンプ部５に与える前記同様の指令ト
ルクτを算出する。

【００９５】

【数２７】

【００９６】上記トルクリミット部９１にて算出された
指令トルクτがロボット２に伝達される。なお、図６の
ロボット２の部分は、より詳細には前述した図３と同様
に表すものであり、その詳細な内容の説明については省
略する。

【００９７】上述した第４の実施の形態のロボット制御
装置１によれば、トルクリミット部９２を備えたことに
より、サーボモータ４とアンプ部５によるトルクの制限
に従いつつ、サーボモータ４とアンプ部５の能力を十分
に引き出すことができ、これにより、ロボット２側の第
２アーム３の位置決めの高速化が図られる。したがっ
て、このロボット制御装置１によれば、ロボット２によ
る電子部品の組み立て等の作業効率の向上が図られる。

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

【００９９】

【発明の効果】上述したように、本発明に係るロボット
制御装置によれば、ねじれ角と外乱とを同時に推定する
ねじれ角・外乱推定手段を備えたことにより、ロボット
側の駆動部の振動の抑制が図られるとともに、当該駆動
部の位置決めの高速化が図られる。また、このロボット
制御装置によれば、ロボットによる電子部品の組み立て
等の作業効率の向上が図られる。

【０１００】また、本発明に係るロボット制御装置によ
れば、トルクリミット手段を設けたことにより、指令ト
ルクを制限しながら良好な制御結果を得ることができる
と共に、サーボモータとアンプの能力を有効に利用する
ことにより、駆動部の位置決めの高速化が可能となって
いる。

【図面の簡単な説明】

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

【図２】本発明に係る第１の実施の形態のロボット制御
装置がロボットを制御する状態を説明するために示すブ
ロック図である。

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

【図４】本発明に係る第２の実施の形態ロボット制御装
置がロボットを制御する状態を説明するために示すブロ
ック図である。

【図５】本発明に係る第３の実施の形態のロボット制御
装置がロボットを制御する状態を説明するために示すブ
ロック図である。

【図６】本発明に係る第４の実施の形態ロボット制御装
置がロボットを制御する状態を説明するために示すブロ
ック図である。

【図７】従来のロボット制御装置がロボットを制御する
状態を説明するために示すブロック図である。

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

【符号の説明】

１ ロボット制御装置、 ２ ロボット、 ３ 第２
アーム、 ４ サーボモータ、 ５ アンプ、 ６ 角
度検出器、 ７，１１ ねじれ角・外乱推定オブザー
バ、 ８，１２ フィードフォワード部、 ９，９０，
９３ 制御部、９１，９２ トルクリミット部、 ｄ
外乱トルク、 ｄ 推定外乱トルク、τ 指令トルク、
θ_a アームの角度、 θ_a ^' アームの角速度、 θ
_a アームの推定角度、 θ_a ^' アームの推定角速度、
θ_m サーボモータの角度、θ_m ^' サーボモータの角
速度、 θ_m ^' サーボモータの推定角速度、 θ_r サー
ボモータの目標角度、 θ_r ^' サーボモータの目標角速
度、 θ_t ねじれ角の推定値、 θ_t ^' ねじれ推定角
速度

Claims (20)

【特許請求の範囲】
1. 【請求項１】 駆動部と、上記駆動部を駆動するサーボ
   モータと、上記サーボモータにトルクを発生させるアン
   プと、上記サーボモータの角度を検出する角度検出手段
   とを備えるロボットを制御対象とし、 上記アンプに与える指令トルクと上記角度検出手段より
   検出されたサーボモータの角度とに基づいて、ロボット
   の駆動部の角度とロボットの駆動部の角度の時間微分値
   であるロボットの駆動部の角速度と、サーボモータの角
   度の時間微分値であるサーボモータの角速度と、ロボッ
   トに外乱として加わる外乱トルクとを推定するねじれ角
   ・外乱推定手段と、 サーボモータの目標角度に基づいて、サーボモータの目
   標角速度を算出するフィードフォワード手段と、 上記サーボモータの目標角度と、上記角度検出手段より
   検出されたサーボモータの角度と、上記フィードフォワ
   ード手段より算出されたサーボモータの目標角速度と、
   上記ねじれ角・外乱推定手段より推定されたロボットの
   駆動部の推定角度とロボットの駆動部の推定角速度とサ
   ーボモータの推定角速度及び推定外乱トルクとに基づい
   て、アンプに与える指令トルクを算出する制御手段とを
   備え、 上記制御手段が算出した指令トルクに基づいて上記ロボ
   ットを制御することを特徴とするロボット制御装置。
2. 【請求項２】 下記式（１）の状態方程式にて表される
   上記ロボットを制御することを特徴とする請求項１に記
   載のロボット制御装置。 【数１】 なお、上記式（１）におけるθ_m はサーボモータの角
   度、θ_m ^'はサーボモータの角速度、Ｊ_m はサーボモータ
   のイナーシャ、Ｄ_m はサーボモータの粘性摩擦、θ_a は
   駆動部の角度、θ_a ^'は駆動部の角速度、Ｊ_a は駆動部の
   イナーシャ、Ｄ_aは駆動部の粘性摩擦、Ｋはサーボモー
   タ軸と駆動部間のギヤのバネ定数、Ｎはギヤ比、ｄはサ
   ーボモータのモータ軸に加わる外乱トルク、τは指令ト
   ルクである。
3. 【請求項３】 上記ねじれ角・外乱推定手段は、指令ト
   ルクτ及びサーボモータの角度θ_m を用いて、下記式
   （２）、式（３）により、駆動部の推定角度θ_a と、駆
   動部の推定角速度θ_a ^' と、サーボモータの推定角速度θ
   _m ^' 及び推定外乱トルクｄとをそれぞれ算出することを特
   徴とする請求項２に記載のロボット制御装置。 【数２】 なお、上記式（２）、式（３）におけるｚ₁ 、ｚ₂ 、ｚ
   ₃ 、ｚ₄ は最小次元オブザーバの状態変数、ｋ₁ 、ｋ
   ₂ 、ｋ₃ 、ｋ₄ は推定速度を決めるパラメータである。
4. 【請求項４】 上記制御手段は、サーボモータの角度θ
   _m と、駆動部の推定角度θ_a と、サーボモータの推定角
   速度θ_m ^' と、駆動部の推定角速度θ_a ^' と、推定外乱トル
   クｄと、サーボモータの目標角度θ_r と、サーボモータ
   の目標角速度θ_r ^'とを用いて、下記式（４）により、指
   令トルクτを算出することを特徴とする請求項３に記載
   のロボット制御装置。 【数３】 なお、上記式（４）におけるｆ₁ 、ｆ₂ 、ｆ₃ 、ｆ₄は
   制御の特性を決める制御パラメータである。
5. 【請求項５】 駆動部と、上記駆動部を駆動するサーボ
   モータと、上記サーボモータにトルクを発生させるアン
   プと、上記サーボモータの角度を検出する角度検出手段
   とを備えるロボットを制御対象とし、 上記アンプに与える指令トルクと上記角度検出手段より
   検出されたサーボモータの角度とに基づいて、ロボット
   の駆動部の角度とサーボモータの角度との差であるねじ
   れ角度と、ねじれ角度の時間微分値であるねじれ角速度
   と、サーボモータの角度の時間微分値であるサーボモー
   タの角速度と、ロボットに外乱として加わる外乱トルク
   とを推定するねじれ角・外乱推定手段と、 サーボモータの目標角度に基づいて、フィードフォワー
   ド値を算出するフィードフォワード手段と、 上記サーボモータの目標角度と、上記角度検出手段より
   検出されたサーボモータの角度と、上記フィードフォワ
   ード手段より算出されたフィードフォワード値と、上記
   ねじれ角・外乱推定手段より推定されたねじれ推定角度
   とねじれ推定角速度と推定外乱トルク及びサーボモータ
   の推定角速度とに基づいて、上記アンプに与える指令ト
   ルクを算出する制御手段とを備え、 上記制御手段が算出した指令トルクに基づいて上記ロボ
   ットを制御することを特徴とするロボット制御装置。
6. 【請求項６】 下記式（５）の状態方程式にて表される
   上記ロボットを制御することを特徴とする請求項５に記
   載のロボット制御装置。 【数４】 なお、上記式（５）におけるθ_m はサーボモータの角
   度、θ_m ^'はサーボモータの角速度、Ｊ_m はサーボモータ
   のイナーシャ、Ｄ_m はサーボモータの粘性摩擦、θ_a は
   駆動部の角度、θ_a ^'は駆動部の角速度、Ｊ_a は駆動部の
   イナーシャ、Ｄ_aは駆動部の粘性摩擦、Ｋはサーボモー
   タ軸と駆動部間のギヤのバネ定数、Ｎはギヤ比、ｄはサ
   ーボモータのモータ軸に加わる外乱トルク、τは指令ト
   ルクである。
7. 【請求項７】 上記ねじれ角・外乱推定手段は、指令ト
   ルクτ及びサーボモータの角度θ_m を用いて、下記式
   （６）、式（７）、式（８）、式（９）により、駆動部
   の推定角度θ_a と、駆動部の推定角速度θ_a ^' と、サーボ
   モータの推定角速度θ_m ^' 及び推定外乱トルクｄとをそれ
   ぞれ算出し、ねじれ推定角度θ_t と、ねじれ推定角速度
   θ_t ^' とをそれぞれ算出することを特徴とする請求項６に
   記載のロボット制御装置。 【数５】 なお、上記式（６）、式（７）におけるｚ₁ 、ｚ₂ 、ｚ
   ₃ 、ｚ₄ は最小次元オブザーバの状態変数、ｋ₁ 、ｋ
   ₂ 、ｋ₃ 、ｋ₄ は推定速度を決めるパラメータである。
8. 【請求項８】 上記フィードフォワード手段は、サーボ
   モータの目標角度θ_rの時間微分値であるサーボモータ
   の目標角速度θ_r ^'と、サーボモータの目標角速度θ_r ^'の
   時間微分値であるサーボモータの目標角加速度θ_r ^"とを
   用いて、下記式（１０）により、フィードフォワード値
   ffを算出することを特徴とする請求項７に記載のロボッ
   ト制御装置。 【数６】 なお、上記式（１０）におけるff₁ 、ff₂ はフィードフ
   ォワードパラメータである。
9. 【請求項９】 上記制御手段は、下記式（１１）によ
   り、サーボモータの目標角度θ_rと、サーボモータの目
   標角速度θ_r ^'と、サーボモータの角度θ_mと、サーボモ
   ータの推定角速度θ_m ^' と、フィードフォワード値ffと、
   ねじれ角の推定値θ_t と、ねじれ推定角速度θ_t ^' と、推
   定外乱トルクｄとを用いて、指令トルクτを算出するこ
   とを特徴とする請求項８に記載のロボット制御装置。 【数７】 なお、上記式（１１）におけるｆ₁ 、ｆ
   ₂ 、ｆ₃ 、ｆ₄は制御の特 性を決める制御パラメータである。
10. 【請求項１０】 駆動部と、上記駆動部を駆動するサー
    ボモータと、サーボモータにトルクを発生させるアンプ
    と、サーボモータの角度を検出する角度検出手段とを備
    えるロボットを制御対象とし、 上記アンプに与える指令トルクと上記角度検出手段より
    検出されたサーボモータの角度とに基づいて、ロボット
    の駆動部の角度と、ロボットの駆動部の角度の時間微分
    値であるロボットの駆動部の角速度と、サーボモータの
    角度の時間微分値であるサーボモータの角速度と、ロボ
    ットに外乱として加わる外乱トルクとを推定するねじれ
    角・外乱推定手段と、 サーボモータの目標角度に基づいて、サーボモータの目
    標角速度を算出するフィードフォワード手段と、 上記サーボモータの目標角度と、上記角度検出手段より
    検出されたサーボモータの角度と、上記フィードフォワ
    ード手段より算出されたサーボモータの目標角速度と、
    上記ねじれ角・外乱推定手段より推定されたロボットの
    駆動部の推定角度とロボットの駆動部の推定角速度とサ
    ーボモータの推定角速度及び推定外乱トルクとに基づい
    て、計算トルクを算出する制御手段と、 上記制御手段が算出した計算トルクに基づいて、予め設
    定されたトルクリミット値によってトルクリミット値以
    内に制限された上記アンプに与える指令トルクを算出す
    るトルクリミット手段とを備え、 上記トルクリミット手段が算出した指令トルクに基づい
    て上記ロボットを制御することを特徴とするロボット制
    御装置。
11. 【請求項１１】 下記式（１２）の状態方程式にて表さ
    れる上記ロボットを制御することを特徴とする請求項１
    ０に記載のロボット制御装置。 【数８】 なお、上記式（１２）におけるθ_m はサーボモータの角
    度、θ_m ^'はサーボモータの角速度、Ｊ_m はサーボモータ
    のイナーシャ、Ｄ_m はサーボモータの粘性摩擦、θ_a は
    駆動部の角度、θ_a ^'は駆動部の角速度、Ｊ_a は駆動部の
    イナーシャ、Ｄ_a は駆動部の粘性摩擦、Ｋはサーボモー
    タ軸と駆動部間のギヤのバネ定数、Ｎはギヤ比、ｄはサ
    ーボモータのモータ軸に加わる外乱トルク、τは指令ト
    ルクである。
12. 【請求項１２】 上記ねじれ角・外乱推定手段は、指令
    トルクτ及びサーボモータの角度θ_m を用いて、下記式
    （１３）、式（１４）により、駆動部の推定角度θ_a
    と、駆動部の推定角速度θ_a ^' と、サーボモータの推定角
    速度θ_m ^' 及び推定外乱トルクｄとをそれぞれ算出するこ
    とを特徴とする請求項１１に記載のロボット制御装置。 【数９】 なお、上記式（１３）、式（１４）におけるｚ₁ 、ｚ
    ₂ 、ｚ₃ 、ｚ₄ は最小次元オブザーバの状態変数、ｋ
    ₁ 、ｋ₂ 、ｋ₃ 、ｋ₄ は推定速度を決めるパラメータで
    ある。
13. 【請求項１３】 上記制御手段は、サーボモータの角度
    θ_m と、駆動部の推定角度θ_a と、サーボモータの推定
    角速度θ_m ^' と、駆動部の推定角速度θ_a ^' と、推定外乱ト
    ルクｄと、サーボモータの目標角度θ_r と、サーボモー
    タの目標角速度θ_r ^'とを用いて、下記式（１５）によ
    り、計算トルクτ_eを算出することを特徴とする請求項
    １２に記載のロボット制御装置。 【数１０】 なお、上記式（１５）におけるｆ₁ 、ｆ₂ 、ｆ₃ 、ｆ₄
    は制御の特性を決める制御パラメータである。
14. 【請求項１４】 上記トルクリミット手段は、下記式
    （１６）により、計算トルクτ_eに基づいて、予め設定
    されたトルクリミット値τ_l以内に制限された指令トル
    クτを算出することを特徴とする請求項１３に記載のロ
    ボット制御装置。 【数１１】
15. 【請求項１５】 駆動部と、上記駆動部を駆動するサー
    ボモータと、上記サーボモータにトルクを発生させるア
    ンプと、上記サーボモータの角度を検出する角度検出手
    段とを備えるロボットを制御対象とし、 上記アンプに与える指令トルクと上記角度検出手段より
    検出されたサーボモータの角度とに基づいて、ロボット
    の駆動部の角度とサーボモータの角度との差であるねじ
    れ角度と、ねじれ角度の時間微分値であるねじれ角速度
    と、サーボモータの角度の時間微分値であるサーボモー
    タの角速度と、ロボットに外乱として加わる外乱トルク
    とを推定するねじれ角・外乱推定手段と、 サーボモータの目標角度に基づいて、フィードフォワー
    ド値を算出するフィードフォワード手段と、 上記サーボモータの目標角度と、上記角度検出手段より
    検出されたサーボモータの角度と、上記フィードフォワ
    ード手段より算出されたフィードフォワード値と、上記
    ねじれ角・外乱推定手段より推定されたねじれ推定角度
    とねじれ推定角速度と推定外乱トルク及びサーボモータ
    の推定角速度とに基づいて、計算トルクを算出する制御
    手段と、 上記制御手段が算出した計算トルクに基づいて、予め設
    定されたトルクリミット値によってトルクリミット値以
    内に制限された上記アンプに与える指令トルクを算出す
    るトルクリミット手段とを備え、 上記トルクリミット手段が算出した指令トルクに基づい
    て上記ロボットを制御することを特徴とするロボット制
    御装置。
16. 【請求項１６】 下記式（１７）の状態方程式にて表さ
    れる上記ロボットを制御することを特徴とする請求項１
    ５に記載のロボット制御装置。 【数１２】 なお、上記式（１７）におけるθ_m はサーボモータの角
    度、θ_m ^'はサーボモータの角速度、Ｊ_m はサーボモータ
    のイナーシャ、Ｄ_m はサーボモータの粘性摩擦、θ_a は
    駆動部の角度、θ_a ^'は駆動部の角速度、Ｊ_a は駆動部の
    イナーシャ、Ｄ_a は駆動部の粘性摩擦、Ｋはサーボモー
    タ軸と駆動部間のギヤのバネ定数、Ｎはギヤ比、ｄはサ
    ーボモータのモータ軸に加わる外乱トルク、τは指令ト
    ルクである。
17. 【請求項１７】 上記ねじれ角・外乱推定手段は、指令
    トルクτ及びサーボモータの角度θ_m を用いて、下記式
    （１８）、式（１９）、式（２０）、式（２１）によ
    り、駆動部の推定角度θ_a と、駆動部の推定角速度θ_a ^'
    と、サーボモータの推定角速度θ_m ^' 及び推定外乱トルク
    ｄとをそれぞれ算出し、ねじれ推定角度θ_t と、ねじれ
    推定角速度θ_t ^' とをそれぞれ算出することを特徴とする
    請求項１６に記載のロボット制御装置。 【数１３】 なお、上記式（１８）、式（１９）におけるｚ₁ 、ｚ
    ₂ 、ｚ₃ 、ｚ₄ は最小次元オブザーバの状態変数、ｋ
    ₁ 、ｋ₂ 、ｋ₃ 、ｋ₄ は推定速度を決めるパラメータで
    ある。
18. 【請求項１８】 上記フィードフォワード手段は、サー
    ボモータの目標角度θ_r の時間微分値であるサーボモー
    タの目標角速度θ_r ^'と、サーボモータの目標角速度θ_r ^'
    の時間微分値であるサーボモータの目標角加速度θ_r ^"と
    を用いて、下記式（２２）により、フィードフォワード
    値ffを算出することを特徴とする請求項１７に記載のロ
    ボット制御装置。 【数１４】 なお、上記式（２２）におけるff₁ 、ff₂ はフィードフ
    ォワードパラメータである。
19. 【請求項１９】 上記制御手段は、下記式（２３）によ
    り、サーボモータの目標角度θ_rと、サーボモータの目
    標角速度θ_r ^'と、サーボモータの角度θ_mと、サーボモ
    ータの推定角速度θ_m ^' と、フィードフォワード値ffと、
    ねじれ角の推定値θ_t と、ねじれ推定角速度θ_t ^' と、推
    定外乱トルクｄとを用いて、計算トルクτ_eを算出する
    ことを特徴とする請求項１８に記載のロボット制御装
    置。 【数１５】 なお、上記式（２３）におけるｆ₁ 、ｆ
    ₂ 、ｆ₃ 、ｆ₄は制御の 特性を決める制御パラメータである。
20. 【請求項２０】 上記トルクリミット手段は、下記式
    （２４）により、計算トルクτ_eに基づいて、予め設定
    されたトルクリミット値τ_l以内に制限された指令トル
    クτを算出することを特徴とする請求項１９に記載のロ
    ボット制御装置。 【数１６】