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

Abstract

【課題】仮想バネ等が作用される方向において円滑な動作が妨げられるのを抑制する。
【解決手段】ロボットのツールと作業対象物との間に仮想バネまたは仮想ダンパを作用させる柔軟制御を行うロボット制御装置（ＲＣ）において、柔軟制御を行う際に、柔軟制御が開始される柔軟制御開始位置と仮想バネまたは仮想ダンパとに基づいてロボットの複数の関節軸（Ｊ１〜Ｊ６）から特定の関節軸を選択し、該特定の関節軸の位置ゲインおよび／または速度ゲインを特定の関節軸の位置ゲイン通常値および速度ゲイン通常値よりも低減させるゲイン低減手段（４１、４２）と、柔軟制御を行う際に、柔軟制御開始位置と仮想バネまたは仮想ダンパとに基づいて算出された特定の関節軸の補正トルクを前記特定の関節軸の補正トルク通常値よりも低減させる補正トルク低減手段（４３）とを含む。
【選択図】図６

Description

本発明は産業用ロボット（以下、単に「ロボット」と言う。）を制御するロボット制御装置に関する。

ロボットの複数の関節軸を駆動するサーボモータは、通常は、位置制御ループおよび速度制御ループを有するサーボ系によって制御されている。このようなサーボ系によってロボットのツール先端点が目標位置に向かって移動しているときにツール先端点が何らかの障害物に遭遇・接触した場合には、ロボットのツール先端点は障害物に抗して目標位置に向かって移動を続行しようとする現象が生じる。

この現象は次のように説明される。即ち、障害物の存在に関わらずサーボモータは目標位置に向かって移動しようとするが、実際には障害物によりその目標位置に移動することが阻まれるので位置偏差ｅが増大する。その結果、この位置偏差に位置ループゲインＫｐを乗じて得られる速度指令ｖｃも増大する。そして、この増大する速度指令ｖｃとモータの速度ｖ（障害物に当接しているときには、速度ｖは「０」に近いと考えられる。）との差は、速度ループの持つ積分器によって積分されて増大し、トルク指令ｔｃは大きな値となる。

最終的には、サーボモータは目標位置への移動を実現しようとして最大のトルクを出力するようになり、ロボットの停止やワーク、エンドエフェクタ等の破壊事故（干渉事故）を招く原因となる。そこで、このような不都合を回避する手段として、必要に応じて位置ループゲインＫｐおよび速度ループゲインＫｖを低下させることにより速度指令ｖｃ及びトルク指令ｔｃの値の増大を抑止する方式が用いられている。

この方式においては、柔軟制御用のゲイン値Ｋｐ’、Ｋｖ’が予め設定され、柔軟制御指令の入力時にゲインが通常制御用のゲイン値Ｋｐ、Ｋｖから柔軟制御用のゲイン値Ｋｐ’、Ｋｖ’に各々切り換えられる。

このような柔軟制御は、ロボットの関節軸（以下、単に「軸」と述べる場合もある）を柔らかくして「各軸空間上でのソフトフロート機能」（以下、「各軸ソフトフロート」と適宜称する）を行うものである。

特許文献１には、直交座標系において設定された仮想バネに関するパラメータ（Ｋｘ、Ｋｙ、Ｋｚ）を用いて直交座標系における力（Ｆｘ、Ｆｙ、Ｆｚ）を求め、この力をツール座標系の力に変換し、Ｎｅｗｔｏｎ−Ｅｕｌｅｒ法から各軸のトルクＴｉを算出し、算出されたトルクＴｉを各軸のトルク指令として用いるようにした柔軟制御が開示されている。
特開２００５−２１９２０５号公報

図７は直交座標系における柔軟制御（以下、「直交ソフトフロート」と適宜称する）の一例を示す図である。ロボットＲは図示されるように配置された六つの関節軸Ｊ１〜Ｊ６を備えており、ロボットＲの先端にはツールＨが取付けられている。また、直交座標系Σ０が図示されるように定められている。

成型品Ｗ１が成型機Ｗ２内のピンＰ３によって−Ｘ方向に押出されると、ロボットＲは成型品Ｗ１を取出すために、成型品Ｗ１の動作に追従してＸ方向に柔らかくかつＺ方向に固い直交ソフトフロートを行う必要がある。すなわち、図７においては、Ｘ方向が柔らかい方向、すなわち動作が円滑に行われるべき方向であり、Ｚ方向が固い方向、つまり動作すべきでない方向である。

図８は一例において直交ソフトフロートを行う際に作用する力を説明するための図である。図８においては、Ｘ方向およびＺ方向における仮想バネのパラメータＫｘ、Ｋｚとして、（Ｋｘ、Ｋｚ）＝（０、Ｋｚ）を定める。なお、説明を簡単にする目的でＫｙは示していない。

ピンＰ３が成型品Ｗ１を押出す力、および他の外乱により、図８に示されるように、目標位置ｐ０とフィードバック位置ｐｆとの間に差が生じる。そして、この偏差を偏差ｐｆ−ｐ０＝（Δｘ、Δｚ）とする。Δｘ、Δｚに対して仮想バネが出す力はそれぞれ、−ＫｘΔｘ、−ＫｚΔｚである。前述したように（Ｋｘ、Ｋｚ）＝（０、Ｋｚ）と定めているので、ここでは、−ＫｚΔｚ＝Ｆｚのみが作用する。

ツールＨのツール先端点ＴＣＰに力Ｆｚが発生するときの各軸のトルクをＮｅｗｔｏｎ−Ｅｕｌｅｒ法により算出すると、関節軸Ｊ２、Ｊ３、Ｊ５においてトルクＴ２、Ｔ３、Ｔ５が得られる。図８から分かるように、関節軸Ｊ３においては、トルクＴ３が−Ｘ方向への動作を妨げる方向に働き、その結果、Ｘ方向における柔らかさ、すなわち円滑な動作が妨げられる。それゆえ、トルクＴ３に抗して動作させる場合には、各軸ソフトフロートの場合よりも大きな力が必要とされる。しかしながら、特許文献１の場合においては、特定の関節軸、例えばトルクＴ３が作用する関節軸Ｊ３のみについて、トルクを調節することはできない。

本発明はこのような事情に鑑みてなされたものであり、柔軟制御を行う際に、仮想バネなどが作用される方向において円滑な動作が妨げられるのを抑制することのできるロボット制御装置を提供することを目的とする。

前述した目的を達成するために１番目の発明によれば、複数の関節軸を備えたロボットを制御するロボット制御装置であって、前記ロボットのツールに対して一定の位置姿勢関係にあるツール座標系または作業対象物に対して一定の位置姿勢関係にある作業座標系により定義された方向において、前記ロボットの前記ツールと前記作業対象物との間に仮想バネを作用させる柔軟制御を行うロボット制御装置において、前記柔軟制御を行う際に、前記柔軟制御が開始される柔軟制御開始位置と前記仮想バネとに基づいて前記ロボットの前記複数の関節軸から特定の関節軸を選択し、該特定の関節軸の位置ゲインおよび速度ゲインのうちの少なくとも一方を前記特定の関節軸の位置ゲイン通常値および速度ゲイン通常値よりも低減させるゲイン低減手段と、前記柔軟制御を行う際に、前記柔軟制御開始位置と前記仮想バネとに基づいて算出された前記特定の関節軸の補正トルクを前記特定の関節軸の補正トルク通常値よりも低減させる補正トルク低減手段とを具備する、ロボット制御装置が提供される。

２番目の発明によれば、複数の関節軸を備えたロボットを制御するロボット制御装置であって、前記ロボットのツールに対して一定の位置姿勢関係にあるツール座標系または作業対象物に対して一定の位置姿勢関係にある作業座標系により定義された方向において、前記ロボットの前記ツールと前記作業対象物との間に仮想ダンパを作用させる柔軟制御を行うロボット制御装置において、前記柔軟制御を行う際に、前記柔軟制御が開始される柔軟制御開始位置と前記仮想ダンパとに基づいて前記ロボットの前記複数の関節軸から特定の関節軸を選択し、該特定の関節軸の位置ゲインおよび速度ゲインのうちの少なくとも一方を前記特定の関節軸の位置ゲイン通常値および速度ゲイン通常値よりも低減させるゲイン低減手段と、前記柔軟制御を行う際に、前記柔軟制御開始位置と前記仮想ダンパとに基づいて算出された前記特定の関節軸の補正トルクを前記特定の関節軸の補正トルク通常値よりも低減させる補正トルク低減手段とを具備する、ロボット制御装置が提供される。

すなわち１番目または２番目の発明においては、ゲイン低減手段により特定の関節軸についての位置ゲインおよび／または速度ゲインを低減させると共に、補正トルク低減手段により特定の関節軸についてのトルクを低減させられる。その結果、仮想バネまたは仮想ダンパが作用される方向において円滑な動作が妨げられるのを抑制することができる。言い換えれば、１番目および２番目の発明においては、柔らかくする方向への動作を少ない力で行うことができる。

３番目の発明によれば、１番目または２番目の発明において、前記柔軟制御開始位置において関節軸から前記作業座標系へのヤコビ行列を算出し、前記柔軟制御を行う方向に対応する前記ヤコビ行列の行において絶対値が最大の列に対応する関節軸を、前記特定の関節軸として選択するようにした。
４番目の発明によれば、１番目から３番目のいずれかの発明において、前記特定の関節軸における前記位置ゲインおよび／または前記速度ゲインの低減量を自動的に設定するようにした。
５番目の発明によれば、１番目から４番目のいずれかの発明において、前記特定の関節軸における前記補正トルクの低減量を自動的に設定するようにした。

すなわち３番目から５番目の発明においては、関節軸の選択の設定、ならびに位置ゲイン、速度ゲインおよび／または補正トルクの低減量の設定を自動的に行うことができるので、不慣れな操作者であってもロボットを適切に操作することができる。

６番目の発明によれば、１番目から５番目のいずれかの発明において、前記柔軟制御を開始する前に、通常制御によって前記ロボットのツールを指定された方向に指定された距離だけ予備的に移動させるようにした。
すなわち６番目の発明においては、通常制御によって柔らかい方向にわずかながら動作させるという予備動作を行うことによって、関節軸毎に異なる摩擦を補償して、動作方向が直交方向からズレるのを抑えることができる。

７番目の発明によれば、１番目から５番目のいずれかの発明において、前記柔軟制御を行うときに、指定された方向でかつ指定された量の補正トルクを加えるようにした。
すなわち７番目の発明においては、ロボットと周辺機器のレイアウトの関係から予備動作に必要とされる距離が確保できない場合であっても、関節軸毎に異なる摩擦を補償して、動作方向が直交方向からズレるのを抑えることができる。

８番目の発明によれば、１番目から５番目のいずれかの発明において、前記柔軟制御を行うとき、トルクに周期的な補正トルクを加えるようにした。
すなわち８番目の発明においては、柔軟制御中に動作する方向が未知である場合であっても、関節軸毎に異なる摩擦を補償して、動作方向が直交方向からズレるのを抑えることができる。

９番目の発明によれば、１番目から８番目のいずれかの発明において、指定された直交座標系上における二つ以上の位置姿勢から前記柔軟制御を行う座標系またはツール座標系を決定するようにした。
１０番目の発明によれば、６番目の発明において、指定された直交座標系上における二つ以上の位置姿勢から前記予備的な移動を行う方向を決定するようにした。
１１番目の発明によれば、７番目の発明において、指定された直交座標系上における二つ以上の位置姿勢から前記補正トルクの方向を決定するようにした。

すなわち９番目から１１番目の発明においては、座標系における方向を操作者にとって直感的かつ明解に指定することができる。

以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同一の部材には同一の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
図１は本発明に基づくロボット制御装置を示す図であり、図２は図１に示されるロボット制御装置の機能ブロック図である。これら図面に示されるように、ロボット制御装置ＲＣは教示操作盤ＴＰとロボットＲ、厳密にはロボットＲのロボット機構部ＲＭとに接続されている。

図２に示されるようにロボット制御装置ＲＣはロボット制御装置ＲＣ全体を制御するホストＣＰＵ１１を含んでいる。また、ロボット制御装置ＲＣは、各種のシステムプログラムが記憶されているＲＯＭ１２ａと、ホストＣＰＵ１１がデータを一時的に記憶するのに使用されるＲＡＭ１２ｂと、ロボットの動作内容に関する各種プログラムおよび関連設定値などが格納される不揮発性メモリ１２ｃとを含んでいる。

図２に示されるように、ホストＣＰＵ１１には複数の共有ＲＡＭ＃１〜＃ｎが接続されており、これら共有ＲＡＭ＃１〜＃ｎにはデジタルサーボ回路Ｃ１〜Ｃｎが接続されている。複数の共有ＲＡＭ＃１〜＃ｎは、ホストＣＰＵ１１から出力される移動指令または制御信号をデジタルサーボ回路Ｃ１〜Ｃｎのそれぞれのプロセッサに引渡すと共に、デジタルサーボ回路Ｃ１〜Ｃｎのプロセッサからの各種信号をホストＣＰＵ１１に引き渡す役目を果たす。従って、図面には示さないものの、デジタルサーボ回路Ｃ１〜Ｃｎはプロセッサ、ＲＯＭ、ＲＡＭなどをそれぞれ含んでいるものとする。

また、共有ＲＡＭ＃１〜＃ｎの数およびデジタルサーボ回路Ｃ１〜Ｃｎの数はロボットのロボット機構部ＲＭのモータＭ１〜Ｍｎの数に対応している。これらモータＭ１〜Ｍｎはロボットの関節軸のそれぞれを駆動するのに使用されている。典型的な六軸構成の垂直多間接型ロボットは六つの関節軸Ｊ１〜Ｊ６を含んでおり、従って、モータ、共有ＲＡＭ、デジタルサーボ回路の数はそれぞれ六である。

図示されるように教示操作盤ＴＰは液晶ディスプレイ１５とキーボード１６とを備えている。これらは、前述したプログラムにおけるデータの入力および変更、関連する設定値の入力および変更を行うのに使用される。

図３は教示操作盤ＴＰに表示されるパラメータ設定画面を示す図である。柔軟制御を行う前に、操作者は、図３に示されるパラメータ設定画面を教示操作盤ＴＰの液晶ディスプレイ１５に呼出す。

次いで、設定画面における「柔らかさ」の項目において、Ｘ方向、Ｙ方向、Ｚ方向に「仮想バネ」を配置するかまたは「仮想ダンパ」を配置するかを設定する。実際の画面においては、「バネ」または「ダンパ」をキーボード１６から単に入力すればよい。なお、これらＸ方向、Ｙ方向、Ｚ方向は予め定めた直交座標系Σ０（図７を参照）における各方向に対応している。

次いで、操作者は設定したバネまたはダンパに関する比例定数を入力する。図３に示される設定画面においては、Ｘ方向、Ｙ方向、Ｚ方向に「バネ」を入力し、それぞれの方向における数値を０（ｋｇｆ／ｃｍ）、０（ｋｇｆ／ｃｍ）、２００（ｋｇｆ／ｃｍ）と入力している。入力される数値は零以上の値である。入力される数値が小さいほど柔らかい状態が設定され、数値が大きいほど固い状態が設定される。従って、この場合には、Ｘ方向およびＹ方向が柔軟制御を望む方向である。

「バネ」設定したときのこれら数値は、直交座標系Σ０における仮想バネの比例定数Ｋｘ、Ｋｙ、Ｋｚにそれぞれ対応する。また、「ダンパ」設定したときに入力される数値は、直交座標系Σ０における仮想ダンパの比例定数Ｄｘ、Ｄｙ、Ｄｚに対応する。なお、図示される設定画面においては、Ｘ軸回り、Ｙ軸回り、Ｚ軸回りについて仮想バネまたは仮想ダンパを設定して、同様に数値を入力することも可能である。

図３における設定画面には、「ゲイン下げ設定」の項目も存在する。この項目においては、既に設定された柔軟制御を望む方向（図３においてはＸ方向およびＹ方向）の動作に大きく影響を与える関節軸が以下のように自動的に選択される。

図３に示される設定画面の「開始位置」の項目に操作者が開始位置を入力すると、ロボット制御装置ＲＣは関節軸から作業座標へのヤコビ行列Ｊのその開始位置における値を計算する。そして、ロボット制御装置ＲＣは仮想バネの設定が柔軟な方向に対応する行のなかで、絶対値が大きい列に対応する関節軸の位置ゲインおよび／または速度ゲインを低減させるようにする。このため、ロボット制御装置ＲＣはこれら関節軸のゲイン下げ設定を「有効」であると表示する。

具体的には、操作者が開始位置を入力すると、ロボット制御装置ＲＣは以下に示されるヤコビ行列Ｊを算出する。

上の式においては、柔軟なＸ方向に対応する行が１行目、柔軟なＹ方向に対応する行が２行目であるものとする。そして、ヤコビ行列Ｊの１行目（∂Ｘ／∂Ｊ１、∂Ｘ／∂Ｊ２、∂Ｘ／∂Ｊ３、∂Ｘ／∂Ｊ４、∂Ｘ／∂Ｊ５、∂Ｘ／∂Ｊ６）において絶対値が最大である列を選択する。そのような列が∂Ｘ／∂Ｊ３である場合には、対応する軸であるＪ３軸のゲイン下げ設定を「有効」であると自動的に設定する（図３における「ゲイン下げ設定」の項目を参照されたい）。

同様に、ヤコビ行列Ｊの２行目（∂Ｙ／∂Ｊ１、∂Ｙ／∂Ｊ２、∂Ｙ／∂Ｊ３、∂Ｙ／∂Ｊ４、∂Ｙ／∂Ｊ５、∂Ｙ／∂Ｊ６）において絶対値が最大である列を選択する。そのような列が∂Ｙ／∂Ｊ１である場合には、対応する軸であるＪ１軸のゲイン下げ設定を「有効」であると自動的に設定する（図３における「ゲイン下げ設定」の項目を参照されたい）。

なお、このような関節軸の選択は、図１に示されるロボット制御装置ＲＣとは異なる装置により行うようにしてもよい。例えば、別のコンピュータのソフトウェアによってゲイン下げを有効にする関節軸を求め、操作者が、ゲイン下げを行う関節軸を教示操作盤ＴＰによって入力するようにしてもよい。

関節軸を設定した後で、その関節軸について位置ゲインおよび／または速度ゲインの低下の割合を設定する。具体的には、低下前ゲインと低下後ゲインとの比の項目である位置ゲイン比αｐ（＝低下後位置ゲインＫｐｓ／通常位置ゲインＫｐ）、速度ゲイン比αｖ（＝低下後速度ゲインＫｖｓ／通常速度ゲインＫｖ）を入力する（０≦αｐ≦１００、０≦αｖ≦１００）。図３においては、関節軸Ｊ１の位置ゲイン比αｐが５％で、速度ゲイン比αｖが５％であり、関節軸Ｊ３の位置ゲイン比αｐが１０％で、速度ゲイン比αｖが１０％であると設定されている。

さらに、これら関節軸Ｊ１、Ｊ３についての補正トルクＴｉ（後述する）のうち、トルク指令ｔｃ（後述する）に加える割合αｔ（０≦αｔ≦１００）も入力する。図３においては、関節軸Ｊ１、Ｊ３の補正トルクの割合が０％であると設定されている。

なお、ロボット制御装置ＲＣが柔軟制御用ゲインＫｐ’、Ｋｖ’と通常制御用のゲインＫｐ、Ｋｖを切替える機能を備えている場合には、低下後ゲインＫｐｓ、Ｋｖｓと柔軟制御用ゲインＫｐ’、Ｋｖ’との比をαｐ、αｖとして入力するようにしてもよい。なお、柔軟制御用ゲインＫｐ’、Ｋｖ’と通常制御用のゲインＫｐ、Ｋｖとの間には、Ｋｐ＞Ｋｐ’、Ｋｖ＞Ｋｖ’の関係があるものとする。

また、これら比または割合の設定は操作者がキーボード１６から入力してもよいが、予め定められた規則に基づいてロボット制御装置ＲＣが決定し、その結果のみを液晶ディスプレイ１５に表示するようにしても良い。例えば、これら位置ゲイン比αｐ、速度ゲイン比αｖおよび割合αｔに所定の定数、例えば（αｐ、αｖ、αｔ）＝（１０％、１０％、０％）を自動的に設定するようにしてもよい。このような場合には、不慣れな操作者であってもロボットを適切に操作できるのが分かるであろう。

通常はロボットＲはロボット制御装置ＲＣにより通常制御されている。そして、直交ソフトフロートを望む場合には、操作者が教示操作盤ＴＰによって直交ソフトフロート機能を有効に設定する。直交ソフトフロートを行う場合とは、例えば図７に示されるように、ロボットＲが成型品Ｗ１を取出すために、成型品Ｗ１の動作に追従してＸ方向に柔らかく動作すると共にＺ方向に固く動作する必要がある場合を指す。

図４および図５は本発明のロボット制御装置ＲＣにおいて直交ソフトフロート機能を行う際のフローチャートであり、図６は直交ソフトフロート機能を行う際の制御系の機能ブロック図である。以下、図４、図５および図６を参照しつつ、本発明のロボット制御装置ＲＣにおいて行われる直交ソフトフロート機能について説明する。

ロボット制御装置ＲＣが柔軟制御用ゲインＫｐ’、Ｋｖ’と通常制御用のゲインＫｐ、Ｋｖとを切換える機能を備えている場合には、直交ソフトフロート開始時に、まず全ての関節軸の位置ゲインＫｐおよび速度ゲインＫｖを柔軟制御用の位置ゲインＫｐ’、および速度ゲインＫｖ’にそれぞれ変更する。

ホストＣＰＵ１１においては、図４のフローチャートに示される処理が所定周期で実施される。図４のステップ１０１においては、各関節軸の位置指令から順運動学により直交指令位置を計算すると共に、各関節軸の位置フィードバックから順運動学により直交フィードバック位置を計算する。そして、これら直交指令位置と直交フィードバック位置との間の差を直交座標系Σ０上における位置偏差（Δｘ、Δｙ、Δｚ）、つまり柔軟制御開始位置として算出する（図６におけるブロック２１、２２を参照されたい）。

次いで、図３に示される設定画面に入力された仮想バネのパラメータ（Ｋｘ、Ｋｙ、Ｋｚ）をこの位置偏差に乗算し、それにより、直交座標系Σ０における力（Ｆｘ、Ｆｙ、Ｆｚ）を算出する（（Ｆｘ、Ｆｙ、Ｆｚ）＝（Ｋｘ、Ｋｙ、Ｋｚ）・（−Δｘ、−Δｙ、−Δｚ））（図６におけるブロック２３を参照されたい）。

次いで、ステップ１０２において、ロボットの姿勢データを用いて、直交座標系Σ０における力（Ｆｘ、Ｆｙ、Ｆｚ）をツール座標系Σｎ＋１における力（Ｆｘ（ｎ＋１）、Ｆｙ（ｎ＋１）、Ｆｚ（ｎ＋１））に変換する。

その後、ステップ１０３において、Ｎｅｗｔｏｎ−Ｅｕｌｅｒ法により、ロボットの現在位置とツール座標系Σｎ＋１における力（Ｆｘ（ｎ＋１）、Ｆｙ（ｎ＋１）、Ｆｚ（ｎ＋１））とから、各軸の補正トルクＴｉ（ｉ＝１〜ｎ）が算出される（図６におけるブロック２４を参照されたい）。

次いで、ステップ１０４においては、各関節軸について得られた補正トルクＴｉをそれぞれの共有ＲＡＭ＃１〜＃ｎに格納する。以上の処理はロボット制御装置ＲＣのホストＣＰＵ１１において行われる。

次いで、図５に示されるフローチャートが実施される。図５のステップ１１１においては、共有ＲＡＭ＃１〜＃ｎの補正トルクＴｉがデジタルサーボ回路Ｃ１〜Ｃｎのそれぞれに送られる。図５に示される以下の処理は、デジタルサーボ回路Ｃ１〜Ｃｎのそれぞれにおいて所定周期で実施されるものとする。

次いで、ステップ１１２において、或るデジタルサーボ回路Ｃｉに対応する関節軸Ｊｉが、図３において「ゲイン下げ設定」された関節軸であるか否かが判定される。「ゲイン下げ設定」された関節軸である場合には、ステップ１１３に進む。

ステップ１１３においては、位置ゲイン低減手段４１が、通常の位置ゲインＫｐあるいはＫｐ’に位置ゲイン比αｐ（図３参照）を乗算して低下後位置ゲインＫｐｓを算出する。同様に、速度ゲイン低減手段４２が、通常速度ゲインＫｖあるいはＫｖ’に速度ゲイン比αｖ（図３参照）を乗算して低下後速度ゲインＫｖｓを算出する。このような位置ゲインおよび速度ゲインを低減させる処理は、図６に示される位置ゲイン低減手段４１および速度ゲイン低減手段４２によってそれぞれ行われるものとする。

図６に示されるブロック３１は位置ループゲインＫｐの項であり、ブロック３２は速度ループゲインＫｖに項であり、ブロック３３は積分項であり、ブロック３４は補正トルクの割合αｔの項である。

ステップ１１３においては、前述した低下後ゲインＫｐｓ、Ｋｖｓがそれぞれブロック３１、３２において使用される。図６から分かるように、ロボット制御装置ＲＣで作成されるモータＭｉの位置指令とフィードバックされた実際のモータ位置とから作成された位置偏差ｅがブロック３１に入力される。そして、ブロック３１において低下後ゲインＫｐｓが乗算されて速度指令ｖｃが作成される。さらに、速度偏差ｅｖが速度指令ｖｃとモータ速度ｖより算出され、この速度偏差ｅｖに速度ループゲインＫｖｓを乗じてトルク指令ｔｃが出力される。

次いでステップ１１４に示されるように、補正トルク低減手段４３によって補正トルクＴｉに前述した割合αｔを乗算する。図３においてはαｔとして０％を設定しているので、修正後の補正トルクは零になる。このような補正トルクを低減させる処理は、図６に示される補正トルク低減手段４３によって行われるものとする。

次いで、トルク指令ｔｃに修正後補正トルク（この場合は零）を加算して新たなトルク指令ｔｃ’とする。そして、新たなトルク指令ｔｃ’に応じた駆動電流がモータＭｉに供給されるようにして、処理を終了する。

このように本発明においては、位置ゲイン低減手段４１により特定の関節軸についての位置ゲインＫｐを低下後ゲインＫｐｓに低下させ、速度ゲイン低減手段４２によって同じ関節軸についての速度ゲインＫｖを低下後速度ゲインｋｖｓに低下している。さらに、本発明においては、仮想バネを実現する制御を行う補正トルクＴｉを補正トルク低減手段４３によって低下させている。このようなことを行っているので、本発明においては、仮想バネが作用される方向において円滑な動作が妨げられるのを抑制することができる。言い換えれば、本発明においては、柔らかくする方向への動作を少ない力で行うことができる。

図８を参照して説明したように従来技術において例えば（Ｋｘ、Ｋｚ）＝（０、Ｋｚ）と定めた場合には、関節軸Ｊ２、Ｊ３、Ｊ５においてトルクＴ２、Ｔ３、Ｔ５が生じる。そして、関節軸Ｊ３においては、トルクＴ３が−Ｘ方向への動作を妨げる方向に働き、その結果、Ｘ方向における柔らかさが妨げられる。

しかしながら、本発明によれば、ゲイン低減手段４１、４２によってトルク指令ｔｃを抑えられることに加えて、追加される補正トルクＴｉを補正トルク低減手段４３によって低減できる。このため、本発明においては、ゲイン低減手段４１、４２および補正トルク低減手段４３によってトルクＴ３自体を抑えることが可能となる。それゆえ、本発明においては、Ｘ方向における柔らかさが妨げられるのを抑えられ、−Ｘ方向に動作させるのに必要な力が少なくて済むようになる。

再び図５を参照すると、ステップ１１２において「ゲイン下げ設定」された関節軸でないと判定された場合には、ステップ１１５に進む。この場合には、柔軟制御用のゲインＫｐ’、Ｋｖ’がブロック３１、３２において使用される。そして、前述したのと同様にトルク指令ｔｃが作成される。

次いで、ステップ１１６において、補正トルクＴｉをそのままトルク指令ｔｃに加算して新たなトルク指令ｔｃ’を作成する。すなわち、この場合には、ゲイン比αｐ、αｖおよび割合αｔ使用しない。その後、新たなトルク指令ｔｃ’に応じた駆動電流がモータＭｉに供給されるようにして、処理を終了する。

ところで、直交座標系Σ０においてトルク指令を補正する場合には、或る時点における関節軸の摩擦が関節軸ごとに異なるので、或る関節軸は直ぐに動作するものの、他の関節軸では実際に動作開始するまでにわずかながら遅れが生じ、従って、動作方向が設定した直交方向から次第にズレるという問題がある。

この問題を解決するために、通常制御によってわずかながら柔らかい方向に動作させるという予備動作を行い、速度ループの積分器３３にトルクを積算するのが好ましい。これにより、積分器３３には摩擦を補償する方向のトルクが積算される。具体的には、柔軟制御を開始する前に、操作者が予備動作を行う位置制御動作を命令する。このような予備動作により、関節軸毎に異なる摩擦が補償される。すなわち、関節軸毎の摩擦が概ね均等になる。従って、このような予備動作を行うことにより、動作方向が直交方向からズレるのを抑えることが可能となる。

このような摩擦補償のための予備動作は、必ずしも操作者が命令する必要はない。操作者が位置制御動作を命令しない場合であっても、柔軟制御開始命令があったときには予備動作を制御装置のインタプリタで生成実行させ、次いで柔軟制御を開始するようにしてもよい。ただし、この場合には、予備動作のための方向および距離を予め定める必要がある。

ところで、ロボットと周辺機器のレイアウトの関係から予備動作に必要とされる距離が確保できない場合もある。そのような場合には、予備動作において積分器３３に積算されるトルクに相当する量を予め設定しておく。そして、柔軟制御実行時にこのトルクを速度ループ出力に加えるようにしてもよい。すなわち予備動作を実際に行う必要が無いので、予備動作に必要とされる距離が確保できない場合であっても、前述したのと同様な効果を得ることが可能である。

なお、摩擦を補償するためのこのような方法は、柔軟制御において動作する方向が予め分かっている場合に限られる。柔軟制御中に動作する方向が未知である場合には、静摩擦の大きさに近い振幅の正弦波、三角波、矩形波などの周期的なトルクを加えるようにする。このような状態で一方向から外力を加えた場合には、外力と前記周期的トルクの方向が一致したときにこれらの合計のトルクが静摩擦分を上回りその方向に動作開始できるようになる。すなわち、柔軟制御中に動作する方向が未知である場合であっても、関節軸毎に異なる摩擦を補償して、動作方向が直交方向からズレるのを抑えることが可能となる。

また、直交ソフトフロートを作業座標系において行う場合、または予備動作の方向を座標系上で指定する場合には、座標系を予め設定する必要がある。そのために、座標系のパラメータを直接入力することが想定される。あるいは、ツール先端点ＴＣＰに関する二つの位置を教示して、その二点を結ぶ方向を、直交ソフトフロートの作業座標系の軸の一つ（例えばＺ軸）または予備動作の方向に設定するようにしてもよい。これにより、座標系における方向を操作者にとって直感的かつ明解に指定することが可能である。

なお、詳細には説明しないものの、図３の設定画面において仮想バネの代わりに、ダンピングパラメータＤｘ、Ｄｙ、Ｄｚを用いて仮想ダンパを設定した場合には、−Ｄｘ・Δｖｘ＝Ｆｘ、−Ｄｙ・Δｖｙ＝Ｆｙ、−Ｄｚ・Δｖｚ＝Ｆｚの関係を満たす速度偏差Δｖｘ、Δｖｙ、Δｖｚが用いられるものとする。そのような場合にも、前述したのと概ね同様な効果が得られるのは当業者であれば明らかであろう。

また、図３においては画面に入力することにより各種のパラメータを設定しているが、これらを動作プログラムの中で設定するようにしてもよい。このような場合であっても、本発明の範囲に含まれるものとする。

本発明に基づくロボット制御装置を示す図である。 図１に示されるロボット制御装置の機能ブロック図である。 教示操作盤に表示されるパラメータ設定画面を示す図である。 本発明のロボット制御装置において直交ソフトフロート機能を行う際の第一のフローチャートである。 本発明のロボット制御装置において直交ソフトフロート機能を行う際の第二のフローチャートである。 直交ソフトフロート機能を行う際の制御系の機能ブロック図である。 直交座標系における柔軟制御の一例を示す図である。 一例において直交ソフトフロートを行う際に作用する力を説明するための図である。

符号の説明

１１ ホストＣＰＵ
１２ａ ＲＯＭ
１２ｂ ＲＡＭ
１２ｃ 不揮発性メモリ
１５ 液晶ディスプレイ
１６ キーボード
３３ 積分器
４１ 位置ゲイン低減手段
４２ 速度ゲイン低減手段
４３ 補正トルク低減手段
Ｃ１〜Ｃｎ デジタルサーボ回路
Ｄｘ、Ｄｙ、Ｄｚ 仮想ダンパの比例定数
Ｈ ツール
Ｊ ヤコビ行列
Ｊ１〜Ｊ６ 関節軸
Ｋｐ 位置ゲイン
Ｋｐｓ 低下後位置ゲイン
Ｋｖ 速度ゲイン
ｋｖｓ 低下後速度ゲイン
Ｋｘ、Ｋｙ、Ｋｚ 仮想バネの比例定数
Ｍ１〜Ｍｎ モータ
ｐ０ 目標位置
ｐｆ フィードバック位置
Ｒ ロボット
＃１〜＃ｎ 共有ＲＡＭ
ＲＣ ロボット制御装置
ＲＭ ロボット機構部
Ｔｉ 補正トルク
ＴＰ 教示操作盤
Ｗ１ 成型品
Ｗ２ 成型機
αｐ 位置ゲイン比
αｔ 割合
αｖ 速度ゲイン比
Σ０ 直交座標系
Σｎ＋１ ツール座標系

Claims (11)

1. 複数の関節軸を備えたロボットを制御するロボット制御装置であって、前記ロボットのツールに対して一定の位置姿勢関係にあるツール座標系または作業対象物に対して一定の位置姿勢関係にある作業座標系により定義された方向において、前記ロボットの前記ツールと前記作業対象物との間に仮想バネを作用させる柔軟制御を行うロボット制御装置において、
   前記柔軟制御を行う際に、前記柔軟制御が開始される柔軟制御開始位置と前記仮想バネとに基づいて前記ロボットの前記複数の関節軸から特定の関節軸を選択し、該特定の関節軸の位置ゲインおよび速度ゲインのうちの少なくとも一方を前記特定の関節軸の位置ゲイン通常値および速度ゲイン通常値のそれぞれよりも低減させるゲイン低減手段と、
   前記柔軟制御を行う際に、前記柔軟制御開始位置と前記仮想バネとに基づいて算出された前記特定の関節軸の補正トルクを前記特定の関節軸の補正トルク通常値よりも低減させる補正トルク低減手段とを具備する、ロボット制御装置。
2. 複数の関節軸を備えたロボットを制御するロボット制御装置であって、前記ロボットのツールに対して一定の位置姿勢関係にあるツール座標系または作業対象物に対して一定の位置姿勢関係にある作業座標系により定義された方向において、前記ロボットの前記ツールと前記作業対象物との間に仮想ダンパを作用させる柔軟制御を行うロボット制御装置において、
   前記柔軟制御を行う際に、前記柔軟制御が開始される柔軟制御開始位置と前記仮想ダンパとに基づいて前記ロボットの前記複数の関節軸から特定の関節軸を選択し、該特定の関節軸の位置ゲインおよび速度ゲインのうちの少なくとも一方を前記特定の関節軸の位置ゲイン通常値および速度ゲイン通常値よりも低減させるゲイン低減手段と、
   前記柔軟制御を行う際に、前記柔軟制御開始位置と前記仮想ダンパとに基づいて算出された前記特定の関節軸の補正トルクを前記特定の関節軸の補正トルク通常値よりも低減させる補正トルク低減手段とを具備する、ロボット制御装置。
3. 前記柔軟制御開始位置において関節軸から前記作業座標系へのヤコビ行列を算出し、前記柔軟制御を行う方向に対応する前記ヤコビ行列の行において絶対値が最大の列に対応する関節軸を、前記特定の関節軸として選択するようにした請求項１または２に記載のロボット制御装置。
4. 前記特定の関節軸における前記位置ゲインおよび／または前記速度ゲインの低減量を自動的に設定するようにした請求項１から３のいずれか一項に記載のロボット制御装置。
5. 前記特定の関節軸における前記補正トルクの低減量を自動的に設定するようにした請求項１から４のいずれか一項に記載のロボット制御装置。
6. 前記柔軟制御を開始する前に、通常制御によって前記ロボットのツールを指定された方向に指定された距離だけ予備的に移動させるようにした請求項１から５のいずれか一項に記載のロボット制御装置。
7. 前記柔軟制御を行うときに、指定された方向でかつ指定された量の補正トルクを加えるようにした請求項１から５のいずれか一項に記載のロボット制御装置。
8. 前記柔軟制御を行うとき、トルクに周期的な補正トルクを加えるようにした請求項１から５のいずれか一項に記載のロボット制御装置。
9. 指定された直交座標系上における二つ以上の位置姿勢から前記柔軟制御を行う座標系またはツール座標系を決定するようにした請求項１から８のいずれか一項に記載のロボット制御装置。
10. 指定された直交座標系上における二つ以上の位置姿勢から前記予備的な移動を行う方向を決定するようにした請求項６に記載のロボット制御装置。
11. 指定された直交座標系上における二つ以上の位置姿勢から前記補正トルクの方向を決定するようにした、請求項７に記載のロボット制御装置。

Images