JP5727103B2 - ロボット装置およびその制御方法 - Google Patents

Description

本発明は、駆動モータにより関節を駆動するロボット装置およびその制御方法に関する。

ロボット装置の関節の駆動を制御する場合、バックラッシュ等の機械的精度により、関節の回転方向が正回転から負回転に、または、負回転から正回転に反転するときの関節角速度と関節トルクの関係が定まらないという問題が生じ易い。

摩擦を考慮して駆動を制御する従来のロボット装置では、この問題に対して、速度が０付近の領域に不感帯と呼ばれるヒステリシスを任意に設けて制御を行っている（特許文献１）。
一方、摩擦を考慮して制御対象の駆動を制御する場合に、制御対象がクーロン摩擦の影響と粘性摩擦の影響を受けることから、両者を同定して補償することも行われている（特許文献２）。
なお、移動物体の速度に応じて、クーロン摩擦の影響を受ける領域と粘性摩擦の影響を受ける領域とに区分し、それぞれの領域における摩擦力を不連続摩擦モデルのモデル式に基づいて推定することも行われている（非特許文献１）。

特開平７−２８５２７号公報 特開平７−３３３０８４号公報

菊植亮，武居直行，佐野明人，望山洋，藤本英雄："有限時間で零速度に収束する不連続摩擦モデル"，第１０回ロボティクスシンポジア予稿集，ｐｐ．２１５−２２０，２００５，

しかし、不連続摩擦モデルや不感帯を導入し、関節に作用するクーロン摩擦や粘性摩擦により生じる摩擦力トルクを考慮してロボット装置の関節の駆動を制御する場合、クーロン摩擦の影響を受ける領域から粘性摩擦の影響を受ける領域に移行する際や、粘性摩擦の影響を受ける領域からクーロン摩擦の影響を受ける領域に移行する際に、境界となる不連続点で、推定された摩擦力トルクと実際に関節に作用する摩擦力トルクとが合わず、関節の駆動が滑らかに制御されないという問題があった。

本発明は、関節に作用する摩擦力トルクを考慮して関節の駆動を制御する従来のロボット装置に生じる前述の問題を解決し、関節の駆動が、クーロン摩擦の影響を受ける領域から粘性摩擦の影響を受ける領域に移行するときや、粘性摩擦の影響を受ける領域からクーロン摩擦の影響を受ける領域に移行するときであっても、滑らかに駆動制御されるロボット装置を提供することを課題とする。

上記課題を解決するため、本発明のロボット装置は、駆動モータにより駆動される関節と、前記駆動モータの回転角を検出する回転角検出手段と、前記回転角検出手段の検出値より前記関節の駆動を制御する際に、前記関節に作用する関節トルクを推定する関節トルク推定部の推定値より補正を行う制御装置とを備え、前記関節トルク推定部は、慣性トルク推定部とコリオリ力トルク推定部と遠心力トルク推定部と重力トルク推定部と摩擦力トルク推定部とを有し、前記摩擦力トルク推定部は、クーロン摩擦力トルク推定部と粘性摩擦力トルク推定部と、クーロン摩擦力トルク推定部から粘性摩擦力トルク推定部への移行と粘性摩擦力トルク推定部からクーロン摩擦力トルク推定部への移行を滑らかにする移行区間演算部とを有することを特徴とする。

また、本発明のロボット装置の制御方法は、駆動モータの回転角の検出値より関節を駆動する際に、関節に作用する関節トルクの推定値より補正を行うロボット装置の制御方法であって、関節に作用する慣性トルクとコリオリ力トルクと遠心力トルクと重力トルクと摩擦力トルクとを推定して関節の関節トルクの推定値を得るときに、クーロン摩擦力トルクから粘性摩擦力トルクへ移行する間の摩擦力トルクと粘性摩擦力トルクからクーロン摩擦力トルクへ移行する間の摩擦力トルクを、移行区間演算部で演算して推定することを特徴とする。

本発明によれば、ロボット装置の関節の駆動が、クーロン摩擦の影響を受ける領域から粘性摩擦の影響を受ける領域に移行するときや、粘性摩擦の影響を受ける領域からクーロン摩擦の影響を受ける領域に移行するときであっても、滑らかに駆動制御される。

図１は、本発明の一実施形態によるロボット装置を示す斜視図。 図２は、ロボット装置の関節の構造を示す模式図。 図３は、ロボット装置の制御装置の制御ブロック図。 図４は、制御装置の関節トルク推定部の制御ブロック図。 図５は、関節トルク推定部の摩擦トルク推定部により推定される摩擦トルクを示すグラフ。 図６は、図１に示されたロボット装置のフローチャート。 図７は、関節トルク推定部のフローチャート。 図８は、摩擦トルク推定部のフローチャート。

以下に本発明の一実施形態について、図１から図８を参照して説明する。ここで、図１から図８は、本発明の一実施形態を示す図である。

まず、図１および図２により、本発明の一実施形態によるロボット装置１の概略について説明する。

図１に示すように、ロボット装置１は、ロボット本体２と、ロボット本体２に接続されロボット本体２の駆動を制御する制御装置３とを備えている。

ロボット本体２は、ベース１０と、ベース１０から順次設けられ、６個の関節２０〜２５（第１関節２０、第２関節２１、第３関節２２、第４関節２３、第５関節２４、および第６関節２５）を介して、互いに連結された６個のアーム体３０〜３５（第１アーム体３０、第２アーム体３１、第３アーム体３２、第４アーム体３３、第５アーム体３４、および第６アーム体３５）とを備えている。

図２に示すように、第１アーム体３０に一体的に設けられて第１関節２０を構成するアーム軸４０は、その一端がベース１０に取付けられた減速機４１の出力側に接続されている。減速機４１の入力側には駆動モータ４２が接続され、駆動モータ４２にはその回転角を検出する回転角検出手段であるエンコーダ４３が設けられている。駆動モータ４２が正回転または逆回転すると、その回転は減速機４１で減速されてアーム軸４０に伝達され、第１アーム体３０が正方向または逆方向に回転する。
なお、第２関節２１〜第６関節２５についても、それぞれ第１関節２０と同様にアーム軸４０を有し、減速機４１、駆動モータ４２、エンコーダ４３が設けられ、同様に動作するので、説明を省略する。

次に、図３および図４により、制御装置３について説明する。

図３に示すように、制御装置３は、位置指令部５０と、θ・θ’・θ”演算部５１と、サーボ制御演算部５２と、関節トルク推定部５３と、外力演算部５４と、コンプライアンスモデル演算部５５と、制御対象部５６とを備えている。
制御装置３は、ロボット本体２の各関節２０〜２５のそれぞれの駆動モータ４２に設けられたエンコーダ４３の検出値に基づき、所定の制御周期にて位置指令部５０が目標位置・姿勢に関する指令を発し、指令を受けた制御対象部５６がそれぞれの駆動モータ４２を作動させ、各関節２０〜２５を駆動する。

θ・θ’・θ”演算部５１は、位置指令部５０が算出した目標位置・姿勢に関する指令と、後述するコンプライアンスモデル演算部５５で得た修正量とを加算して、逆運動学に基づく演算を行い、関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を算出する。

サーボ制御演算部５２は、θ・θ’・θ”演算部５１で算出された関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を基に、制御対象部５６に発するトルク指令を算出する。

関節トルク推定部５３は、動力学モデルに基づき、各関節２０〜２５に作用する関節トルクを推定するが、その詳細については後述する。

外力演算部５４は、サーボ制御演算部５２で算出されたトルク指令から関節トルク推定部５３で推定された関節トルクの推定トルク値を減算することにより求められた誤差トルクを基に、推定外力を算出する。

コンプライアンスモデル演算部５５は、外力演算部５４で算出された推定外力を基に、目標位置・姿勢に関する修正量を算出する。

ここで、関節トルク推定部５３による、各関節２０〜２５に作用する関節トルクの推定について、詳細を説明する。

関節トルク推定部５３は、動力学モデルのモデル式に基づき、関節トルクを推定する。
ｎ個の関節に作用する推定関節トルクＴ（Θ）は、以下の式で表される。
Ｔ（Θ）＝Ｍ（Θ）Θ”＋Ｂ（Θ）［Θａ’Θｂ’］＋Ｃ（Θ）［Θ’^２］＋Ｇ（Θ）＋Ｆ（Θ’）
上式の各項は以下の通りである。
Ｔ（Θ）：推定関節トルク行列（ｎ行１列の行列）
Θ、Θ’、Θ”：それぞれ関節角度行列、関節角速度行列、関節角加速度行列（ｎ行１列の行列）
Θ’^２：同じ関節の関節角速度積行列（ｎ行１列の行列）
Θａ’Θｂ’：異なる関節の関節角速度積行列（｛ｎ・（ｎ−１）／２｝行１列の行列）
Ｍ（Θ）：慣性（質量）行列（ｎ行ｎ列の行列）
Ｂ（Θ）：コリオリ力行列（ｎ行｛ｎ・（ｎ−１）／２｝列の行列）
Ｃ（Θ）：遠心力行列（ｎ行ｎ列の行列）
Ｇ（Θ）：重力行列（ｎ行１列の行列）
Ｆ（Θ’）：摩擦力行列（ｎ行１列の行列）
なお、本実施形態では、ロボット本体２が６個の関節２０〜２５を有するので、ｎ＝６となる。

図４に示すように、関節トルク推定部５３は、上述の推定関節トルクＴ（Θ）の式に基づき、慣性トルク推定部６０とコリオリ力トルク推定部６１と遠心力トルク推定部６２と重力トルク推定部６３と摩擦力トルク推定部６４を備えている。

慣性トルク推定部６０は、上述の推定関節トルクＴ（Θ）の式のＭ（Θ）Θ”の項に基づき、所定の制御周期で発せられる関節角度指令θと関節角加速度指令θ”に対して、慣性により生ずる慣性トルクを算出する。

コリオリ力トルク推定部６１は、上述の推定関節トルクＴ（Θ）の式のＢ（Θ）［Θａ’Θｂ’］の項に基づき、所定の制御周期で発せられる関節角度指令θと関節角速度指令θ’に対して、コリオリ力により生ずるコリオリ力トルクを算出する。

遠心力トルク推定部６２は、上述の推定関節トルクＴ（Θ）の式のＣ（Θ）［Θ’^２］の項に基づき、所定の制御周期で発せられる関節角度指令θと関節角速度指令θ’に対して、遠心力により生ずる遠心力トルクを算出する。

重力トルク推定部６３は、上述の推定関節トルクＴ（Θ）の式のＧ（Θ）の項に基づき、所定の制御周期で発せられる関節角度指令θに対して、重力により生ずる重力トルクを算出する。

摩擦力トルク推定部６４は、上述の推定関節トルクＴ（Θ）の式のＦ（Θ’）の項に基づき、所定の制御周期で発せられる関節角度指令θと関節角速度指令θ’に対して、摩擦力により生ずる摩擦力トルクを算出する。

関節トルク推定部５３は、慣性トルク推定部６０で算出された慣性トルクと、コリオリ力トルク推定部６１で算出されたコリオリ力トルクと、遠心力トルク推定部６２で算出された遠心力トルクと、重力トルク推定部６３で算出された重力トルクと、摩擦力トルク推定部６４で算出された摩擦力トルクとを加算することにより関節トルクを推定し、関節トルクの推定値を得る。

各関節２０〜２５に作用する摩擦力トルクを算出するに当たり、関節角速度の増加に応じて各関節２０〜２５に作用する摩擦がクーロン摩擦から粘性摩擦に移行すること、および、関節角速度の減少に応じて各関節２０〜２５に作用する摩擦が粘性摩擦からクーロン摩擦に移行することから、摩擦力トルク推定部６４は、クーロン摩擦モデルのモデル式に基づくクーロン摩擦力トルク推定部７０と、粘性摩擦モデルのモデル式に基づく粘性摩擦力トルク推定部７１とを備えている。
そして、クーロン摩擦モデルのモデル式と粘性摩擦モデルのモデル式が不連続である場合に、その接続が不連続となる関節角速度の前後で、推定される摩擦力トルクが急激に変化して各関節２０〜２５の駆動が乱れることを避けるために、摩擦力トルク推定部６４は、さらに、クーロン摩擦力トルク推定部７０から粘性摩擦力トルク推定部７１への移行と、粘性摩擦力トルク推定部７１からクーロン摩擦力トルク推定部７０への移行を滑らかにする移行区間演算部７２を備えている。
摩擦力トルク推定部６４は、クーロン摩擦力トルク推定部７０と粘性摩擦力トルク推定部７１と移行区間演算部７２との中から、関節角速度に応じて、いずれか１つを選択する切替えスイッチ７３も有している。

次に、クーロン摩擦力トルク推定部７０と粘性摩擦力トルク推定部７１と移行区間演算部７２とにおける摩擦力トルクの推定と、切替えスイッチ７３による切替えについて説明する。

クーロン摩擦力トルク推定部７０は、クーロン摩擦モデルに準じ、各関節２０〜２５の関節角度指令θと関節角速度指令θ’に対して、各関節２０〜２５に作用するクーロン摩擦力トルクＦｃ（θ）を、Ｆｃ（θ）＝ｆｃ×ｓｇｎ（θ’）の式に従って演算して推定する。ただし、ｆｃはクーロン摩擦モデルのモデル式に準じた所定の定数を表す。

粘性摩擦力トルク推定部７１は、粘性摩擦モデルに準じ、各関節２０〜２５の関節角度指令θと関節角速度指令θ’に対して、各関節２０〜２５に作用する粘性摩擦力トルクＦｖ（θ）を、Ｆｖ（θ）＝ｆｖ（θ’）の式に従って演算して推定する。ただし、ｆｖ（θ’）は、関節角速度指令θ’を変数とする粘性摩擦モデルのモデル式に準じた所定の関数を表す。

移行区間演算部７２は、各関節２０〜２５の関節角度指令θと関節角速度指令θ’に対して、各関節２０〜２５に作用する移行区間摩擦力トルクＦｔ（θ）を、Ｆｔ（θ）＝ｋ×（θ−θｓ）＋ｆｃ×ｓｇｎ（θ’）の式に従って演算して推定する。ただし、ｋは後述する摩擦力トルク係数を表し、θｓは所定の制御周期における前回指令時の関節角度指令を表す。

摩擦力トルク係数ｋは、予め各関節２０〜２５を駆動し、各関節２０〜２５に影響を及ぼす摩擦が、関節角度の増加によりクーロン摩擦から粘性摩擦に移行する際の各関節２０〜２５に作用する関節トルクを測定して、その測定結果を解析することにより定められる。各関節２０〜２５の駆動モータ４２の電流値とエンコーダ４３の検出値とを所定のサンプリング周期で複数測定し、駆動モータ４２の電流値を関節トルクに、エンコーダ４３の検出値を関節角度に換算する。ロボット本体２の寸法や質量等に基づき、測定時に各関節２０〜２５に作用していた慣性トルクとコリオリ力トルクと遠心力トルクと重力トルクを算出し、換算された関節トルクからそれらを減算することにより、測定時に各関節２０〜２５に作用していた摩擦力トルクを抽出する。換算された関節角度と抽出された摩擦力トルクにより、測定に基づく関節角度と摩擦力トルクとの関係Ｆｍ（θ）を得る。

大略、測定に基づく関節角度と摩擦力トルクとの関係Ｆｍ（θ）は、関節角度が微小な領域では、クーロン摩擦の影響により関節角度の変化に対して摩擦力トルクはほぼ一定となるが、この微小な領域を超えると、関節角度の増加に従い摩擦力トルクが滑らかに増加する。この摩擦力トルクが滑らかに増加する領域において、関節角度に対する摩擦力トルクの傾きｋｍを抽出し、その傾きｋｍを摩擦力トルク係数ｋとする。

なお、本実施形態では、駆動モータ４２の電流値を関節トルクに、エンコーダ４３の検出値を関節角度に換算しているが、他の測定装置を用いて関節トルクと関節角度を測定してもよい。

また、ロボット装置１が複数台ある場合は、それぞれのロボット装置１について摩擦力トルク係数ｋを求めることが好ましいが、同一工程で製作された同一仕様の複数台のロボット装置１については、いずれか１台のロボット装置１の摩擦力トルク係数ｋを求め、その摩擦力トルク係数ｋを残りのロボット装置１に適用してもよい。

切替えスイッチ７３は、各関節２０〜２５の関節角速度指令θ’に応じて、クーロン摩擦力トルク推定部７０と粘性摩擦力トルク推定部７１と移行区間演算部７２との中から、いずれか１つを選択する。
各関節２０〜２５の回転方向が正回転の場合、関節角速度指令θ’は正の値となり、逆回転の場合、関節角速度指令θ’は負の値となる。
関節角速度指令θ’が、０≦｜θ’｜≦θ１’（０＜θ１’）の範囲にあるとき、切替えスイッチ７３はクーロン摩擦力トルク推定部７０を選択して接続し、摩擦力トルク推定部６４は、クーロン摩擦力トルク推定部７０にて演算することにより、各関節２０〜２５に作用する摩擦力トルクを推定する。なお、θ１’は、クーロン摩擦切替え関節速度指令θ１’を表し、これについては後述する。
関節角速度指令θ’が、θ１’＜｜θ’｜＜θ２’（θ１’＜θ２’）の範囲にあるとき、切替えスイッチ７３は移行区間演算部７２を選択して接続し、摩擦力トルク推定部６４は、移行区間演算部７２にて演算することにより、各関節２０〜２５に作用する摩擦力トルクを推定する。なお、θ２’は、粘性摩擦切替え関節速度指令θ２’を表し、これについては後述する。
関節角速度指令θ’が、θ２’≦｜θ’｜の範囲にあるとき、切替えスイッチ７３は粘性摩擦力トルク推定部７１を選択して接続し、摩擦力トルク推定部６４は、粘性摩擦力トルク推定部７１にて演算することにより、各関節２０〜２５に作用する摩擦力トルクを推定する。
なお、クーロン摩擦力トルク推定部７０から粘性摩擦力トルク推定部７１への直接の切替えと、粘性摩擦力トルク推定部７１からクーロン摩擦力トルク推定部７０への直接の切替えは行われない。

クーロン摩擦切替え関節速度指令θ１’は、クーロン摩擦力トルク推定部７０がクーロン摩擦モデルに準じて算出したクーロン摩擦力トルクＦｃ（θ）と、測定に基づく関節角度と摩擦力トルクとの関係Ｆｍ（θ）とを比較し、Ｆｍ（θ）がＦｃ（θ）を超えるときの関節角速度が適用される。
粘性摩擦切替え関節速度指令θ２’は、粘性摩擦力トルク推定部７１が粘性摩擦モデルに準じて算出した粘性摩擦力トルクＦｖ（θ）と、測定に基づく関節角度と摩擦力トルクとの関係Ｆｍ（θ）とを比較し、Ｆｍ（θ）がＦｖ（θ）の最小値に到達するときの関節角速度が適用される。

本実施形態において、切替えスイッチ７３によるクーロン摩擦力トルク推定部７０と移行区間演算部７２との間の切替え、粘性摩擦力トルク推定部７１と移行区間演算部７２の間の切替えは、関節角速度指令θ’とクーロン摩擦切替え関節速度指令θ１’、粘性摩擦切替え関節速度指令θ２’とを比較することにより行われるが、切替えのための条件を、適宜、変更や付加してもよい。
例えば、本実施の形態において、移行区間演算部７２から粘性摩擦力トルク推定部７１への切替えは、関節角速度指令θ’が粘性摩擦切替え関節速度指令θ２’以上になった（θ２’≦｜θ’｜）時点で切替わるとしているが、関節角速度指令θ’が粘性摩擦切替え関節速度指令θ２’よりも速くなり（θ２’＜｜θ’｜）、かつ、移行区間摩擦力トルクＦｔ（θ）が粘性摩擦力トルクＦｖ（θ）の最小値よりも大きくなった（｜Ｆｖ（θ）の最小値｜＜｜Ｆｔ（θ）｜）時点で切替わるとしてもよい。

本実施形態では、上述の関節トルクの測定、測定結果の解析、測定に基づく関節角度と摩擦トルクとの関係Ｆｍ（θ）の取得、関節角度に対する摩擦力トルクの傾きｋｍの取得、および、クーロン摩擦切替え関節速度指令θ１’、粘性摩擦切替え関節速度指令θ２’の取得は、オペレータによりオフラインにて実施されるが、制御装置３が実行するようにプログラムソフトを追加してもよい。

図５に、各関節２０〜２５に対し、本実施形態の摩擦力トルク推定部６４により推定される摩擦力トルクのグラフを示す。
横軸は関節角速度を、縦軸は摩擦力トルクを示し、白丸（○）印は摩擦力トルク推定部６４により推定された摩擦力トルクを示す。

区間Ａと区間Ａ’は、関節角速度指令θ’が０≦｜θ’｜≦θ１’の範囲であって、所定の制御周期にて、摩擦力トルク推定部６４が、クーロン摩擦力トルク推定部７０で演算して、摩擦力トルクを推定したものである。
区間Ｂと区間Ｂ’は、関節角速度指令θ’がθ１’＜｜θ’｜＜θ２’の範囲であって、所定の制御周期にて、摩擦力トルク推定部６４が、移行区間演算部７２で演算して、摩擦力トルクを推定したものである。
区間Ｃと区間Ｃ’は、関節角速度指令θ’がθ２’≦｜θ’｜の範囲であって、所定の制御周期にて、摩擦力トルク推定部６４が、粘性摩擦力トルク推定部７１で演算して、摩擦力トルクを推定したものである。
移行区間演算部７２で推定された区間Ｂ、Ｂ’の摩擦トルクは、クーロン摩擦モデルのモデル式に基づき推定された区間Ａ、Ａ’の摩擦力トルクと、粘性摩擦モデルのモデル式に基づき推定された区間Ｃ、Ｃ’の摩擦力トルクを滑らかに接続している。

区間Ｂ、Ｂ’の黒丸（●）印で示すように、従来通り、この区間の摩擦力トルクをクーロン摩擦力トルク推定部７０により推定すると、区間Ｂと区間Ｃの境界、および、区間Ｂ’と区間Ｃ’の境界において、摩擦力トルクが急激に変化するため、各関節２０〜２５の駆動が乱れる可能性がある。
本実施形態によれば、摩擦力トルク推定部６４が移行区間演算部７２を備えているので、クーロン摩擦力トルク推定部７０から移行区間演算部７２を介して粘性摩擦力トルク推定部７１へ、および、粘性摩擦力トルク推定部７１から移行区間演算部７２を介してクーロン摩擦力トルク推定部７０へ、滑らかに移行する。

次に、本実施形態によるロボット装置１の動作を、図６に示すフローチャートを参照して説明する。
ロボット装置１は、図６に示すフローチャートの如く制御される。

ロボット装置１は、ステップＳ１００においてサーボオンされ、制御装置３が起動する。

ステップＳ１０１において、制御装置３は、そのメモリに保存されている動力学モデルに基づく推定関節トルクＴ（Θ）の式のパラメータを読み込む。読み込まれるパラメータは、関節トルク推定部５３の慣性トルク推定部６０、コリオリ力トルク推定部６１、遠心力トルク推定部６２、重力トルク推定部６３に適用され、ロボット本体２の寸法や質量に基づき予め求められるアーム質量、リンクオフセット量、重心オフセット、重心周り慣性モーメント等と、摩擦力トルク推定部６４に含まれるクーロン摩擦力トルク推定部７０のクーロン摩擦力トルクＦｃ（θ）の式に関する情報、粘性摩擦力トルク推定部７１の粘性摩擦力トルクＦｖ（θ）の式に関する情報、移行区間演算部７２の移行区間摩擦力トルクＦｔ（θ）の式の摩擦力トルク係数ｋ、切替えスイッチ７３のクーロン摩擦切替え関節速度指令θ１’、粘性摩擦切替え関節速度指令θ２’等である。

ステップＳ１０２において、制御装置３は、そのメモリに保存されているロボット本体２の先端の到達すべき目標位置、姿勢を読み込む。

ステップＳ１０３において、制御装置３は、その位置指令部５０にて、各関節２０〜２５のそれぞれの駆動モータ４２に設けられたエンコーダ４３の検出値に基づき、所定の制御周期にて、目標位置・姿勢を算出し、目標位置・姿勢に関する指令を発する。

ステップＳ１０４において、制御装置３は、目標位置・姿勢に関する指令を基に、そのθ・θ’・θ”演算部５１にて、逆運動学に基づく演算を行い、関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を算出する。

ステップＳ１０５において、制御装置３は、関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を基に、そのサーボ制御演算部５２により、トルク指令値を算出する。

ステップＳ１０６において、制御装置３は、トルク指令を基に、制御対象部５６がそれぞれの駆動モータ４２を作動させ、ロボット本体２の各関節２０〜２５を回転させる。

ステップＳ１０７において、制御装置３は、ロボット本体２の各関節２０〜２５の位置・姿勢が到達すべき目標値になっていればステップＳ１０８に進みサーボオフする。目標値に達していなければ、ステップＳ１０３に戻る。

なお、ステップＳ１０９において、制御装置３は、ステップＳ１０４にて算出された関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を基に、その関節トルク推定部５３にて、各関節２０〜２５のそれぞれに作用する関節トルクを推定し、ステップＳ１１０において、その推定された関節トルクの推定トルク値からステップＳ１０５にて算出されたトルク指令を減算して、誤差トルクを算出する。
ステップＳ１１１において、制御装置３は、誤差トルクを基に、その外力演算部５４にて、推定外力を算出し、ステップＳ１１２において、推定外力を基に、そのコンプライアンスモデル演算部５５にて、目標位置・姿勢の修正量を演算する。
この目標位置・姿勢の修正量は、ステップＳ１０３において演算される目標位置・姿勢に加算され、算出された目標位置・姿勢はコンプライアンス制御を考慮した目標位置・姿勢となる。

次に、関節トルク推定部５３の動作を、図７に示すフローチャートを参照して説明する。

関節トルク推定部５３は、ステップＳ２００において関節トルクの推定を開始する。

ステップＳ２０１において、関節トルク推定部５３は、慣性トルク推定部６０により慣性トルクを算出する。

ステップＳ２０２において、関節トルク推定部５３は、コリオリ力トルク推定部６１によりコリオリ力トルクを算出する。

ステップＳ２０３において、関節トルク推定部５３は、遠心力トルク推定部６２により遠心力トルクを算出する。

ステップＳ２０４において、関節トルク推定部５３は、重力トルク推定部６３により重力トルクを算出する。

ステップＳ２０５において、関節トルク推定部５３は、摩擦力トルク推定部６４により摩擦力トルクを算出する。

ステップＳ２０６において、関節トルク推定部５３は、ステップＳ２０１〜ステップＳ２０５で算出された慣性トルクとコリオリ力トルクと遠心力トルクと重力トルクと摩擦力トルクを加算して、関節トルクを推定する。
そして、ステップＳ２０７において、関節トルク推定部５３は、関節トルクの推定を終了する。

次に、摩擦力トルク推定部６４の動作を、図８に示すフローチャートを参照して説明する。

摩擦力トルク推定部６４は、ステップＳ３００において摩擦力トルクの推定を開始する。

ステップＳ３０１において、摩擦力トルク推定部６４は、切替えスイッチ７３により、クーロン摩擦力トルク推定区間にあるか、ないか、すなわち関節角速度指令θ’が０≦｜θ’｜≦θ１’の範囲にあるか、ないかを判定する。関節角速度指令θ’が０≦｜θ’｜≦θ１’の範囲にある場合はステップＳ３０５に進み、ない場合はステップＳ３０２に進む。
ステップＳ３０５において、摩擦力トルク推定部６４は、クーロン摩擦力トルク推定部７０にてクーロン摩擦力トルクＦｃ（θ）を演算することにより、摩擦力トルクを推定する。

ステップＳ３０２において、摩擦力トルク推定部６４は、切替えスイッチ７３により、移行区間にあるか、ないか、すなわち関節角速度指令θ’がθ１’＜｜θ’｜＜θ２’の範囲にあるか、ないかを判定する。関節角速度指令θ’がθ１’＜｜θ’｜＜θ２’の範囲にある場合はステップＳ３０６に進み、ない場合はステップＳ３０３に進む。
ステップＳ３０６において、摩擦力トルク推定部６４は、移行区間演算部７２にて移行区間摩擦力トルクＦｔ（θ）を演算することにより、摩擦力トルクを推定する。

ステップＳ３０３において、摩擦力トルク推定部６４は、切替えスイッチ７３により、粘性摩擦力トルク推定区間にあるか、ないか、すなわち関節角速度指令θ’がθ２’≦｜θ’｜の範囲にあるか、ないかを判定する。関節角速度指令θ’がθ２’≦｜θ’｜の範囲にある場合はステップＳ３０７に進み、ない場合は摩擦力トルクが推定されないものとして、ステップＳ３０４に進む。
ステップＳ３０７において、摩擦力トルク推定部６４は、粘性摩擦力トルク推定部７１にて粘性摩擦力トルクＦｖ（θ）を演算することにより、摩擦力トルクを推定する。

ステップＳ３０４において、摩擦力トルク推定部６４は、摩擦力トルクの推定を終了する。

なお、本発明を実施するに当たっては、ロボット装置の具体的な構成により、適宜変更して実施できる。実行する作業に応じて、ロボット本体の関節およびアーム体の個数、および、それを制御する制御装置の制御点数が決定される。

Claims (4)

1. ロボット装置において、
   駆動モータにより駆動される関節と、
   前記駆動モータの回転角を検出する回転角検出手段と、
   前記回転角検出手段の検出値より前記関節の駆動を制御する際に、前記関節に作用する関節トルクを推定する関節トルク推定部の推定値より補正を行う制御装置とを備え、
   前記関節トルク推定部は、慣性トルク推定部と、コリオリ力トルク推定部と、遠心力トルク推定部と、重力トルク推定部と、摩擦力トルク推定部とを有し、
   前記摩擦力トルク推定部は、クーロン摩擦力トルク推定部と、粘性摩擦力トルク推定部と、前記クーロン摩擦力トルク推定部から前記粘性摩擦力トルク推定部への移行と前記粘性摩擦力トルク推定部から前記クーロン摩擦力トルク推定部への移行を滑らかにする移行区間演算部とを有することを特徴とするロボット装置。
2. 前記移行区間演算部は、予め測定された前記関節の関節トルクより抽出された摩擦力トルク係数を用いて、前記関節の摩擦力トルクを演算することを特徴とする請求項１に記載のロボット装置。
3. 駆動モータの回転角の検出値より関節を駆動する際に、前記関節に作用する関節トルクの推定値より補正を行うロボット装置の制御方法において、
   前記関節に作用する慣性トルクとコリオリ力トルクと遠心力トルクと重力トルクと摩擦力トルクとを推定して前記関節の関節トルクの推定値を得る工程を備え、
   前記関節の関節トルクの推定値を得る工程は、クーロン摩擦力トルクから粘性摩擦力トルクへ移行する間の摩擦力トルクと、粘性摩擦力トルクからクーロン摩擦力トルクへ移行する間の摩擦力トルクとを、移行区間演算部で演算して推定する工程を含むことを特徴とするロボット装置の制御方法。
4. 前記移行区間演算部は、予め測定された前記関節の関節トルクより抽出された摩擦力トルク係数を用いて、前記関節の摩擦力トルクを演算することを特徴とする請求項３に記載のロボット装置の制御方法。

Images