力制御装置
【課題】力覚センサの低コスト化を実現しながら、ロボットの先端部に作用する外力を高精度に算出すること。
【解決手段】モータにより駆動されるロボット３と、前記ロボット３の先端部が指令位置に追従するように前記モータを駆動する電流指令を生成するロボット制御手段２と、前記ロボット３の先端部の６個の動作方向のうち、ｘ軸方向、ｙ軸方向、ｚ軸方向の３個の動作方向の夫々にかかる外力を検出する力覚センサ３１と、前記力覚センサ３１の検出値、前記モータの位置、および前記電流指令に基づいて、前記ロボットの先端部のｎ個の動作方向のうちの少なくとも前記力覚センサ３１が外力を非検出とする３つの軸の回転方向の外力算出に必要となる摩擦係数を夫々同定し、当該同定した摩擦係数に基づいて当該３つの軸の回転方向にかかるモーメントの推定値を算出する力推定オブザーバ４と、を備える。
【解決手段】モータにより駆動されるロボット３と、前記ロボット３の先端部が指令位置に追従するように前記モータを駆動する電流指令を生成するロボット制御手段２と、前記ロボット３の先端部の６個の動作方向のうち、ｘ軸方向、ｙ軸方向、ｚ軸方向の３個の動作方向の夫々にかかる外力を検出する力覚センサ３１と、前記力覚センサ３１の検出値、前記モータの位置、および前記電流指令に基づいて、前記ロボットの先端部のｎ個の動作方向のうちの少なくとも前記力覚センサ３１が外力を非検出とする３つの軸の回転方向の外力算出に必要となる摩擦係数を夫々同定し、当該同定した摩擦係数に基づいて当該３つの軸の回転方向にかかるモーメントの推定値を算出する力推定オブザーバ４と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば組立、ばりとり、研磨、基板実装、検査などを行うロボットや、自動組立装置、実装機、加工機、検査装置などに搭載されるロボットを制御する力制御装置に関する。
【背景技術】
【０００２】
例えば特許文献１には、各軸を駆動するトルク指令と各軸の駆動トルクの推定値との差を外部トルクとして算出し、算出した各軸の外乱トルクとロボットの作業座標系と関節座標系とのヤコビ行列からロボット先端位置に作用する外力を算出し、算出した外力を用いて力制御を行うロボット制御装置に関する技術が開示されている。当該ロボット制御装置は、具体的には、先端位置におけるコンプライアンスモデルを記憶しておき、前記コンプライアンスモデルを用いて外力に応じてロボットを駆動する位置指令を修正する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１０−１０５１３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記従来の技術によれば、ロボット制御装置は、各軸の駆動トルクの推定値を算出する際、予め記憶してある摩擦係数を用いて摩擦トルクを算出し、慣性力トルク、重力トルク、摩擦力トルクを加算することで駆動トルク推定値を算出している。しかしながら、各軸の摩擦係数は温度などの条件で変動するため、季節あるいは朝晩でも変動するほか、ロボットを連続動作させて各軸の温度が上昇しても変動する。そのため、駆動トルクの推定値を算出する際の各軸の実際の摩擦力トルクと、摩擦係数を求めた際の各軸の摩擦力トルクは異なっており、駆動トルクの推定精度が劣化し、トルク指令と駆動トルクの差に基づいて算出するロボット先端部に作用する外力の推定値の精度も劣化する問題があった。
【０００５】
本発明は、上記に鑑みてなされたものであって、力覚センサの低コスト化を実現しながら、ロボットの先端部に作用する外力を高精度に算出することができる力制御装置を得ることを目的とする。
【課題を解決するための手段】
【０００６】
上述した課題を解決し、目的を達成するために、本発明は、モータにより駆動されるロボットと、前記ロボットの先端部が指令位置に追従するように前記モータを駆動する電流指令を生成するロボット制御手段と、前記ロボットの先端部のｍ個の動作方向のうち、ｎ（ｍ＞ｎ≧１）個の動作方向の夫々にかかる外力を検出する力覚センサと、前記力覚センサの検出値、前記モータの位置、および前記電流指令に基づいて、前記ｎ−ｍ個の動作方向にかかる外力の推定値を算出するために必要となるロボットの摩擦係数を夫々同定し、当該同定した摩擦係数に基づいて前記ｎ−ｍ個の動作方向にかかる外力の推定値を算出する力推定オブザーバと、を備えることを特徴とする。
【発明の効果】
【０００７】
本発明によれば、６軸力覚センサよりも廉価な例えば３軸力覚センサを用いても、当該３軸力覚センサでは測定できないモーメントを推定でき、力覚センサの低コスト化を実現しながら、ロボットの先端部に作用する外力を高精度に算出することができるという効果を奏する。
【図面の簡単な説明】
【０００８】
【図１】図１は、実施の形態１にかかる力制御装置の構成を示すブロック図である。
【図２】図２は、ロボット制御手段の構成を示すブロック図である。
【図３】図３は、各軸位置制御手段の構成例を示すブロック図である。
【図４】図４は、力覚考慮補正手段の構成を示すブロック図である。
【図５】図５は、力推定オブザーバの構成を示すブロック図である。
【図６】図６は、実施の形態５の力制御装置の構成を示すブロック図である。
【図７】図７は、実施の形態５のロボット制御手段の詳細な構成を説明するブロック図である。
【発明を実施するための形態】
【０００９】
以下に、本発明にかかる力制御装置の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１０】
実施の形態１．
図１は、本発明の実施の形態１にかかる力制御装置の構成を示すブロック図である。図示するように、実施の形態１の力制御装置は、指令生成手段１と、ロボット制御手段２と、ロボット３と、力推定オブザーバ４とを備えている。
【００１１】
指令生成手段１は、ロボット３に時々刻々の目標位置としての位置指令を与える。ロボット制御手段２は、与えられた位置指令にロボット３の先端部（エンド・エフェクタ）が追従するようにロボット３を制御する。ロボット３は、ロボット制御手段２の制御により動作するロボットであって、１軸もしくは複数軸を夫々駆動するモータを備えるものであり、例えば産業用ロボットと呼ばれる６軸垂直多関節ロボット、４軸水平多関節ロボットなどが該当する。
【００１２】
ロボット３の先端部の近辺（ここではロボット３の手首）には当該先端部に作用する力を計測するための３軸力覚センサ３１が取り付けられており、３軸力覚センサ３１で測定された力は力測定値として力推定オブザーバ４に入力される。ここで、実施の形態１では、３軸力覚センサ３１は、センサ座標系でのＸＹＺ各方向の力を測定し、各軸まわりのモーメントは測定しないタイプのものであるとする。ロボット３を駆動する各軸のモータには、エンコーダやレゾルバなどの各軸のモータ位置を測定する位置センサ３２が取り付けられており、位置センサ３２が測定した各軸にかかるモータ位置はロボット制御手段２に入力される。
【００１３】
力推定オブザーバ４には、３軸力覚センサ３１が測定した力測定値に加えて、位置センサ３２が検出した各軸のモータ位置およびロボット制御手段２が算出した各軸のモータ電流指令も入力される。力推定オブザーバ４は、力覚センサで測定できないＸＹＺ各軸周りのモーメントを推定し、ＸＹＺ各方向の力測定値とともにロボット制御手段２に出力する。
【００１４】
図２は、ロボット制御手段２の構成を示すブロック図である。図示するように、ロボット制御手段２は、加算器６、力覚考慮補正手段７、逆変換手段８、各軸位置制御手段９を備えている。力覚考慮補正手段７は、モータ位置および力推定値に基づいて位置指令を補正するための位置指令補正量を出力する。力覚考慮補正手段７の詳細な構成については後述する。加算器６は、指令生成手段１が生成した位置指令と力覚考慮補正手段７が出力した位置指令補正量とを加算する。逆変換手段８は、位置指令補正量の加算により補正された位置指令に対して、手先位置姿勢を実現する各軸のモータ位置を算出するための逆変換計算を行い、ロボット３の各軸の位置指令である関節位置指令に変換する。変換した関節位置指令は各軸位置制御手段９に入力される。各軸位置制御手段９は入力された関節位置指令に各軸のモータ位置が追従するように、ロボット３に対する電流指令を出力する。
【００１５】
図３は、各軸位置制御手段９の構成例を示すブロック図である。図示するように、各軸位置制御手段９は、減算器９１、比例手段９２、微分手段９３、減算器９４、比例積分手段９５を軸毎に備えている。ここでは、軸数はｎ個であるとしている。減算器９１は、各軸の位置指令と当該軸のモータ位置との差分を算出し、算出結果は比例手段９２に入力される。比例手段９２は、入力された値を定数倍する。微分手段９３は、モータ位置を微分してモータ速度を算出する。減算器９４は、比例手段９２の出力値と微分手段９３が算出したモータ速度との差分を求める。比例積分手段９５は、減算器９４の出力値に対して比例積分演算を行って、電流指令を求める。この各軸位置制御手段９は、位置比例制御系の内部に速度比例積分制御系がある構成になっており、モータの位置制御系としてはよく使われる構成である。なお、本実施の形態１では、各軸位置制御手段９は、各軸ごとにモータ位置制御と速度制御とを行うフィードバック制御系の構成を有するものとしたが、各軸毎にオブザーバを含む制御系の構成を有するものであってもよいし、フィードフォワードを含む制御系の構成を有するものであっても構わない。また、全て各軸独立した構成ではなく、軸間の干渉トルクをフィードフォワードもしくはフィードバックで補正する構成であっても構わない。
【００１６】
図４は、力覚考慮補正手段７の構成を示すブロック図である。図示するように、力覚考慮補正手段７は、減算器７１、第１座標変換手段７２、スティフネス行列部７３、加算器７４、減算器７５、比例ゲインフィルタ７６、第２座標変換手段７７、積分手段７８、微分手段７９、ダンピング行列部８０、第３座標変換手段８１、順変換手段８２を備えている。
【００１７】
順変換手段８２は、各軸モータ位置に対して順変換を行ってその時点での手先位置姿勢を算出する。減算器７１は、指令生成手段１が生成した位置指令と算出された手先位置姿勢との差分を算出する。第１座標変換手段７２は、減算器７１が出力した差分値をツール座標系における値に座標変換する。ツール座標系とは、ロボット先端に取り付けたツールやハンドに固定された座標系である。
【００１８】
スティフネス行列部７３は、第１座標変換手段７２の出力値とスティフネス行列とのベクトルの積を求め、求めた積を出力する。微分手段７９は、第１座標変換手段７２の出力の各要素の微分を算出する。ダンピング行列部８０は、微分手段７９の出力の各要素から構成されるベクトルとダンピング行列との積を求めて出力する。加算器７４は、スティフネス行列部７３の出力値とダンピング行列部８０の出力値とを加算する。
【００１９】
第３座標変換手段８１は、力推定オブザーバ４が出力した力推定値をツール座標系における値に変換する。減算器７５は、加算器７４の出力値と第３座標変換手段８１の出力値との差分を求めて出力する。比例ゲインフィルタ７６は、減算器７５の出力値に所定の比例ゲインを乗じる。第２座標変換手段７７は、比例ゲインフィルタ７６の出力値を直交座標系へ座標変換する。積分手段７８は、第２座標変換手段７７の出力値を積分し、得られた値を位置指令修正量（位置指令補正量）として出力する。
【００２０】
図５は、力推定オブザーバ４の構成を示すブロック図である。力推定オブザーバ４は、外力算出手段４１、推定トルク算出手段４２、および摩擦係数推定手段４３を備えている。
【００２１】
摩擦係数推定手段４３は、各軸の電流指令ｉを入力値とし、各軸のモータ位置ｑおよび力測定値を入力値とし、摩擦係数を求めるべきパラメータとしたシステム同定により摩擦係数を求める。以下に、摩擦係数推定手段４３による演算処理について具体的に述べる。
【００２２】
摩擦係数推定手段４３には、各軸の電流指令ｉ、各軸のモータ位置ｑ、３軸力覚センサ３１で測定した力測定値としての手先外力ｆが入力される。ロボット３が６軸垂直多関節ロボットである場合、電流指令ｉ、モータ位置ｑは６要素のベクトルとなり、外力ｆは、３軸力覚センサ３１の測定値なので、３要素のベクトルとなる。摩擦係数推定手段４３は、まず、各軸のトルクτｉ（ｉ＝１〜６）を算出する。各軸のトルクτｉ（ｉ＝１〜６）は、各軸のトルク定数ｋｔｉ（ｉ＝１〜６）、各軸の減速比ｇｅｎｉ、各軸の電流指令をｉｉとすると（ｉ（添え字）＝１〜６）、
τｉ＝ｋｔｉ＊ｉｉ／ｇｅｎｉ （１）
により算出される。
【００２３】
次に、摩擦係数推定手段４３は、各軸のモータ位置ｑｉから各軸のリンク位置ｑｌｉを次の式により求める。
ｑｌｉ＝ｑｉ／ｇｅｎｉ （２）
【００２４】
さらに、摩擦係数推定手段４３は、リンク速度ｖｉ、リンク加速度ａｉを前回周期の値との差をサンプリング時間で除算することで算出する。各軸のリンク位置、速度、加速度から構成されるベクトルをそれぞれｑｌ、ｖ、ａとし、慣性行列をＭ（ｑｌ）、遠心コリオリ力をｈ（ｑｌ，ｖ）、重力をｇ（ｑｌ）とし、摩擦係数推定手段４３は、摩擦力を除く駆動トルクの推定値τｒを次の式により求める。
τｒ＝Ｍ（ｑｌ）ａ＋ｈ（ｑｌ，ｖ）＋ｇ（ｑｌ） （３）
【００２５】
このとき、摩擦力をｆｒ（ｖ）、力覚センサ座標系を手先座標系としたときの手先速度ｖｘと関節速度ｖとの関係をヤコビ行列をＪ（ｑ）を用いて
ｖｘ＝Ｊ（ｑ）ｖ （４）
で表すとすると、手先座標系での各方向の外力及びモーメントで構成されるベクトルＦａｌｌは、
Ｆａｌｌ＝（Ｊ（ｑ）Ｔ）−１τｄ （５）
と表される。ここでτｄは各軸の外力を並べたベクトルであり、
τｄ＝τ−τｒ−ｆｒ（ｖ） （６）
である。式（５）および（６）から、
Ｆａｌｌ＝（Ｊ（ｑ）Ｔ）−１（τ−τｒ−ｆｒ（ｖ）） （７）
が導き出せる。
【００２６】
式（７）の左辺のＦａｌｌの最初の３要素は３軸力覚センサ３１により測定される力測定値である。また、式（７）の右辺の最初の３要素は、算出対象の摩擦係数を含む力推定値を示している。摩擦係数推定手段４３は、式（７）における最初の３要素の右辺と左辺との対応関係を用いて摩擦係数を算出する。
【００２７】
具体的には、各軸のクーロン摩擦係数をｆｃｉ、粘性摩擦係数をｆｖｉとし、ｓｇｎ（ｖｉ）を速度ｖｉが正のとき１、負のとき−1、０のとき０を取る関数とすると、各軸の摩擦力はｆｃｉ＊ｓｇｎ（ｖｉ）＋ｆｖｉ＊ｖｉで表される。（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの最初の３要素を取り出したベクトルをｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））の最初の３要素を取り出したベクトルをｆｂとすると、ｆｂの各要素はｆｃｉ、ｆｖｉに速度ｖ、位置ｑの関数を乗じた構成になっているので、ｆｃｉ、ｆｖｉを順番にたてからｆｃ１、ｆｖ１、ｆｃ２、ｆｖ２、・・・と並べた１２要素のベクトルをｐとすれば、
ｆａ＝ｙ（ｑ，ｖ）ｐ （８）
とかける。ここでｙは３行１２列の行列である。第ｋ番目の制御周期の演算結果を［ｋ］で表記することとすると、摩擦係数推定手段４３は、
Ｒ［ｋ］＝Ｒ［ｋ−１］＋ｓｔ＊（−ｋ１＊Ｒ［ｋ−１］＋ｙ［ｋ］Ｔｙ［ｋ］） （９）
ｒ［ｋ］＝ｒ［ｋ−１］＋ｓｔ＊（−ｋ１＊ｒ［ｋ−１］＋ｆａ［ｋ］ｙ［ｋ］Ｔ） （１０）
によりＲ［ｋ］およびｒ［ｋ］を算出し、
ｐ［ｋ］＝ｐ［ｋ−１］−ｓｔ＊ｋ２＊（Ｒ［ｋ］ｐ［ｋ−１］−ｒ［ｋ］） （１１）
により各軸にかかる摩擦係数を構成要素とするｐを算出する。なお、ｓｔ、ｋ１、ｋ２は予め設定されるシステム同定のためのパラメータである。
【００２８】
以上の演算により、摩擦係数推定手段４３は、時々刻々変化する各軸の電流指令ｉ、各軸のモータ位置ｑおよび力測定値に基づいて、その時点における摩擦係数を求めることができる。すなわち、摩擦係数推定手段４３は、摩擦係数が温度などにより変化しても、その摩擦係数を求めることができる。
【００２９】
推定トルク算出手段４２は、摩擦係数推定手段４３が推定した摩擦係数を用いて摩擦トルクｆｒ（ｖ）を算出し、
τｄ＝τ−τｒ−ｆｒ（ｖ） （１２）
で各軸の外乱トルクの推定値τｄを算出する。
【００３０】
外力算出手段４１では推定トルク算出手段４２で算出した外乱トルクτｄとヤコビ行列Ｊ（ｑ）をもちいて、
Ｆ＝（Ｊ（ｑ）Ｔ）−１τｄ （１３）
を算出する。このうち、最初の３要素は直接測定できるので、３軸力覚センサ３１の測定値に置き換えて出力し、残りの３要素は（１３）式の計算結果をそのまま出力する。
【００３１】
なお、以上の説明においては、（８）式の関係から（９）式〜（１１）式を用いて摩擦係数を算出したが（８）式の関係に対して最小二乗法などを適用することによって算出しても構わない。また本実施の形態１ではモータ電流としてモータ電流指令を用いたが、モータ電流のフィードバックを用いるようにしても構わない。またモータ位置としてモータ位置フィードバックを用いているが、最終的にモータに与えるモータ位置指令もしくはモータ位置指令から算出したモータモデル位置を用いるようにしても構わない。
【００３２】
以上述べたように、本発明の実施の形態１によれば、モータにより駆動されるロボット３と、前記ロボット３の先端部が指令位置に追従するように前記モータを駆動する電流指令を生成するロボット制御手段２と、前記ロボット３の先端部の６個の動作方向のうち、ｘ軸方向、ｙ軸方向、ｚ軸方向の３個の動作方向の夫々にかかる外力を検出する力覚センサ３１と、前記力覚センサ３１の検出値、前記モータの位置、および前記電流指令に基づいて、前記ロボットの先端部のｎ個の動作方向のうちの少なくとも前記力覚センサ３１が外力を非検出とする３つの軸の回転方向にかかる摩擦係数を夫々同定し、当該同定した摩擦係数に基づいて当該３つの軸の回転方向にかかるモーメントの推定値を算出する力推定オブザーバ４と、を備えるように構成したので、６軸力覚センサよりも廉価な３軸力覚センサ３１を用いても、当該３軸力覚センサ３１では測定できないモーメントを推定でき、さらに、時々刻々変化する各軸の電流指令ｉ、各軸のモータ位置ｑおよび力測定値に基づいて求めた摩擦係数を用いて測定できないモーメントを推定することができるので、予め設定された摩擦係数によりモーメントを推定する場合に比して高精度にモーメントを推定できる。すなわち、力覚センサの低コスト化を実現しながら、ロボットの先端部に作用する外力を高精度に算出することができる。
【００３３】
また、前記ロボット制御手段２は、前記モーメントの推定値とモータ位置とに基づいて前記位置指令を補正し、前記ロボットの先端部が前記補正した位置指令に追従するように前記電流指令を生成する、ように構成したので、精度よく力制御をすることができる。
【００３４】
実施の形態２．
実施の形態２の力制御装置の構成は、ロボット３の手首に取り付けられた力覚センサと、摩擦係数推定手段による摩擦係数の演算方法が異なるだけで、実施の形態１と等しい。実施の形態２の構成について実施の形態１と異なる構成についてのみ説明する。
【００３５】
実施の形態２の力制御装置のロボット手首に取り付けられた力覚センサは、ＸＹＺ３方向の力を測定できるセンサ（３軸力覚センサ３１）ではなく、Ｚ軸方向のみの１軸方向の力のみを測定できる。実施の形態２の力制御装置によれば、（５）式のＦａｌｌの第３要素のみ測定できるので、実施の形態１では、摩擦係数推定手段４３は、（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの最初の３要素を取り出したベクトルをｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））の最初の３要素をとりだしたベクトルをｆｂとして摩擦係数を算出したところ、実施の形態２では、（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの第３要素をｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））をｆｂとし、第３要素のみを用いて摩擦係数を算出する。
【００３６】
なお、本実施の形態２の説明では、Ｚ軸方向のみの力を測定できるタイプの力覚センサが採用されたが、測定できる方向はＸ軸方向やＹ軸方向であっても構わない。また、当該力覚センサが測定可能な外力は、Ｘ、Ｙ、Ｚ軸の周りのモーメントのうちの何れか１つであっても構わない。
【００３７】
以上述べたように、本発明の実施の形態２によれば、測定可能な方向は１方向しかない廉価な力覚センサを用いることができる。
【００３８】
実施の形態３．
実施の形態３の力制御装置の構成は、摩擦係数推定手段による摩擦係数を算出する演算方法が異なるだけで実施の形態１と同等である。実施の形態３の構成について実施の形態１と異なる構成についてのみ説明する。
【００３９】
実施の形態３の力制御装置によれば、摩擦係数推定手段は、各軸の摩擦係数を未知パラメータとして同定するのではなく、各軸の摩擦力を摩擦係数の初期値を用いて算出した摩擦力ｆｖ０（ｖｉ）＝ｆｃ０ｉ＊ｓｇｎ（ｖｉ）＋ｆｖ０ｉ＊ｖｉに共通に乗じる係数ｐ１の積で算出する、すなわち
ｆｖ＝ｐ１＊ｆｖ０ （１４）
で算出できるとみなし、ｐ１を未知パラメータとして同定する。具体的にはｆａ＝ｆｂからｐ１を算出し、摩擦係数をｐ１＊ｆｃ０ｉ，ｐ１＊ｆｖ０ｉとして出力する。
【００４０】
このように、本発明の実施の形態３によれば、実施の形態１のように摩擦係数そのものを未知のパラメータとしなくても、最終的に摩擦係数を用いることが可能となる。
【００４１】
実施の形態４．
実施の形態４の力制御装置の構成は、力覚センサと、摩擦係数推定手段による摩擦係数を算出する演算方法とが異なるだけで実施の形態１と同等である。実施の形態４の構成について実施の形態１と異なる構成についてのみ説明する。
【００４２】
実施の形態４にかかる力制御装置のロボット手首に取り付けた力覚センサは、ＸＹＺ３方向の力を測定できるセンサではなく、Ｘ方向およびＹ方向の力とＺ軸方向周りのモーメントの３成分のみを測定できる。（５）式のＦａｌｌの第１、２、６要素のみ測定できるので、摩擦係数推定手段は、実施の形態１では（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの最初の３要素を取り出したベクトルをｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））の最初の３要素をとりだしたベクトルをｆｂとして摩擦係数を算出したが、実施の形態４では（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの第１、２、６要素をｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））をｆｂとし、第１、２、６要素のみを用いて摩擦係数を算出する。
【００４３】
なお、以上の説明においては、力覚センサは、Ｘ方向およびＹ方向の力とＺ軸方向周りのモーメントの３成分のみを測定できるとしたが、本発明の実施の形態３は、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向、Ｘ軸を回転軸とした回転方向、Ｙ軸を回転軸とした回転方向、およびＺ軸を回転軸とした回転方向のうちの任意の３成分のみを測定できる力覚センサを採用した力制御装置に適用できる。
【００４４】
このように、本発明の実施の形態４によれば、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向、Ｘ軸を回転軸とした回転方向、Ｙ軸を回転軸とした回転方向、およびＺ軸を回転軸とした回転方向のうちの任意の３成分のみを測定できる力覚センサを採用した力制御装置であっても精度よく外力を算出することができる。
【００４５】
実施の形態５．
図６は、実施の形態５の力制御装置の構成を示すブロック図である。図示するように、実施の形態５の力制御装置は、指令生成手段１、ロボット制御手段１０、ロボット３、力推定オブザーバ４、および力情報モニター１１を備えている。なお、本図において、実施の形態１と同じ構成要素には同一の符号を付している。すなわち、実施の形態５によれば、力推定オブザーバ４の出力をモニター用途にのみ用い、制御には用いない点が実施の形態１と異なっている。
【００４６】
力情報モニター１１は、例えば各方向の力とモーメントとが許容範囲であるかを監視し、許容範囲を超過した場合にエラー信号を発生する。また、力情報モニター１１は、単に力推定値のログを取る、あるいは力推定値を表示出力する表示装置として機能するようにしてもよい。
【００４７】
図７は、実施の形態５のロボット制御手段１０の詳細な構成を説明するブロック図である。図示するように、実施の形態５では、力制御装置はコンプライアンス制御などは行わず、力情報（力推定値）をモニターするのみとする。したがって、ロボット制御手段１０の内部構成は、力覚考慮補正手段７を備えない一般に知られた位置制御のための構成と同一である。
【００４８】
このように、力推定値を制御ではなく状態の監視の用途に用いるようにしてもよい。
【産業上の利用可能性】
【００４９】
以上のように、本発明にかかる力制御装置は、ロボットを制御する力制御装置に適用して好適である。
【符号の説明】
【００５０】
１ 指令生成手段
２ ロボット制御手段
３ ロボット
４ 力推定オブザーバ
６ 加算器
７ 力覚考慮補正手段
８ 逆変換手段
９ 各軸位置制御手段
１０ ロボット制御手段
１１ 力情報モニター
３１ ３軸力覚センサ
３２ 位置センサ
４１ 外力算出手段
４２ 推定トルク算出手段
４３ 摩擦係数推定手段
７１ 減算器
７２ 座標変換手段
７３ スティフネス行列部
７４ 加算器
７５ 減算器
７６ 比例ゲインフィルタ
７７ 座標変換手段
７８ 積分手段
７９ 微分手段
８０ ダンピング行列部
８１ 座標変換手段
８２ 順変換手段
９１ 減算器
９２ 比例手段
９３ 微分手段
９４ 減算器
９５ 比例積分手段
【技術分野】
【０００１】
本発明は、例えば組立、ばりとり、研磨、基板実装、検査などを行うロボットや、自動組立装置、実装機、加工機、検査装置などに搭載されるロボットを制御する力制御装置に関する。
【背景技術】
【０００２】
例えば特許文献１には、各軸を駆動するトルク指令と各軸の駆動トルクの推定値との差を外部トルクとして算出し、算出した各軸の外乱トルクとロボットの作業座標系と関節座標系とのヤコビ行列からロボット先端位置に作用する外力を算出し、算出した外力を用いて力制御を行うロボット制御装置に関する技術が開示されている。当該ロボット制御装置は、具体的には、先端位置におけるコンプライアンスモデルを記憶しておき、前記コンプライアンスモデルを用いて外力に応じてロボットを駆動する位置指令を修正する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１０−１０５１３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記従来の技術によれば、ロボット制御装置は、各軸の駆動トルクの推定値を算出する際、予め記憶してある摩擦係数を用いて摩擦トルクを算出し、慣性力トルク、重力トルク、摩擦力トルクを加算することで駆動トルク推定値を算出している。しかしながら、各軸の摩擦係数は温度などの条件で変動するため、季節あるいは朝晩でも変動するほか、ロボットを連続動作させて各軸の温度が上昇しても変動する。そのため、駆動トルクの推定値を算出する際の各軸の実際の摩擦力トルクと、摩擦係数を求めた際の各軸の摩擦力トルクは異なっており、駆動トルクの推定精度が劣化し、トルク指令と駆動トルクの差に基づいて算出するロボット先端部に作用する外力の推定値の精度も劣化する問題があった。
【０００５】
本発明は、上記に鑑みてなされたものであって、力覚センサの低コスト化を実現しながら、ロボットの先端部に作用する外力を高精度に算出することができる力制御装置を得ることを目的とする。
【課題を解決するための手段】
【０００６】
上述した課題を解決し、目的を達成するために、本発明は、モータにより駆動されるロボットと、前記ロボットの先端部が指令位置に追従するように前記モータを駆動する電流指令を生成するロボット制御手段と、前記ロボットの先端部のｍ個の動作方向のうち、ｎ（ｍ＞ｎ≧１）個の動作方向の夫々にかかる外力を検出する力覚センサと、前記力覚センサの検出値、前記モータの位置、および前記電流指令に基づいて、前記ｎ−ｍ個の動作方向にかかる外力の推定値を算出するために必要となるロボットの摩擦係数を夫々同定し、当該同定した摩擦係数に基づいて前記ｎ−ｍ個の動作方向にかかる外力の推定値を算出する力推定オブザーバと、を備えることを特徴とする。
【発明の効果】
【０００７】
本発明によれば、６軸力覚センサよりも廉価な例えば３軸力覚センサを用いても、当該３軸力覚センサでは測定できないモーメントを推定でき、力覚センサの低コスト化を実現しながら、ロボットの先端部に作用する外力を高精度に算出することができるという効果を奏する。
【図面の簡単な説明】
【０００８】
【図１】図１は、実施の形態１にかかる力制御装置の構成を示すブロック図である。
【図２】図２は、ロボット制御手段の構成を示すブロック図である。
【図３】図３は、各軸位置制御手段の構成例を示すブロック図である。
【図４】図４は、力覚考慮補正手段の構成を示すブロック図である。
【図５】図５は、力推定オブザーバの構成を示すブロック図である。
【図６】図６は、実施の形態５の力制御装置の構成を示すブロック図である。
【図７】図７は、実施の形態５のロボット制御手段の詳細な構成を説明するブロック図である。
【発明を実施するための形態】
【０００９】
以下に、本発明にかかる力制御装置の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１０】
実施の形態１．
図１は、本発明の実施の形態１にかかる力制御装置の構成を示すブロック図である。図示するように、実施の形態１の力制御装置は、指令生成手段１と、ロボット制御手段２と、ロボット３と、力推定オブザーバ４とを備えている。
【００１１】
指令生成手段１は、ロボット３に時々刻々の目標位置としての位置指令を与える。ロボット制御手段２は、与えられた位置指令にロボット３の先端部（エンド・エフェクタ）が追従するようにロボット３を制御する。ロボット３は、ロボット制御手段２の制御により動作するロボットであって、１軸もしくは複数軸を夫々駆動するモータを備えるものであり、例えば産業用ロボットと呼ばれる６軸垂直多関節ロボット、４軸水平多関節ロボットなどが該当する。
【００１２】
ロボット３の先端部の近辺（ここではロボット３の手首）には当該先端部に作用する力を計測するための３軸力覚センサ３１が取り付けられており、３軸力覚センサ３１で測定された力は力測定値として力推定オブザーバ４に入力される。ここで、実施の形態１では、３軸力覚センサ３１は、センサ座標系でのＸＹＺ各方向の力を測定し、各軸まわりのモーメントは測定しないタイプのものであるとする。ロボット３を駆動する各軸のモータには、エンコーダやレゾルバなどの各軸のモータ位置を測定する位置センサ３２が取り付けられており、位置センサ３２が測定した各軸にかかるモータ位置はロボット制御手段２に入力される。
【００１３】
力推定オブザーバ４には、３軸力覚センサ３１が測定した力測定値に加えて、位置センサ３２が検出した各軸のモータ位置およびロボット制御手段２が算出した各軸のモータ電流指令も入力される。力推定オブザーバ４は、力覚センサで測定できないＸＹＺ各軸周りのモーメントを推定し、ＸＹＺ各方向の力測定値とともにロボット制御手段２に出力する。
【００１４】
図２は、ロボット制御手段２の構成を示すブロック図である。図示するように、ロボット制御手段２は、加算器６、力覚考慮補正手段７、逆変換手段８、各軸位置制御手段９を備えている。力覚考慮補正手段７は、モータ位置および力推定値に基づいて位置指令を補正するための位置指令補正量を出力する。力覚考慮補正手段７の詳細な構成については後述する。加算器６は、指令生成手段１が生成した位置指令と力覚考慮補正手段７が出力した位置指令補正量とを加算する。逆変換手段８は、位置指令補正量の加算により補正された位置指令に対して、手先位置姿勢を実現する各軸のモータ位置を算出するための逆変換計算を行い、ロボット３の各軸の位置指令である関節位置指令に変換する。変換した関節位置指令は各軸位置制御手段９に入力される。各軸位置制御手段９は入力された関節位置指令に各軸のモータ位置が追従するように、ロボット３に対する電流指令を出力する。
【００１５】
図３は、各軸位置制御手段９の構成例を示すブロック図である。図示するように、各軸位置制御手段９は、減算器９１、比例手段９２、微分手段９３、減算器９４、比例積分手段９５を軸毎に備えている。ここでは、軸数はｎ個であるとしている。減算器９１は、各軸の位置指令と当該軸のモータ位置との差分を算出し、算出結果は比例手段９２に入力される。比例手段９２は、入力された値を定数倍する。微分手段９３は、モータ位置を微分してモータ速度を算出する。減算器９４は、比例手段９２の出力値と微分手段９３が算出したモータ速度との差分を求める。比例積分手段９５は、減算器９４の出力値に対して比例積分演算を行って、電流指令を求める。この各軸位置制御手段９は、位置比例制御系の内部に速度比例積分制御系がある構成になっており、モータの位置制御系としてはよく使われる構成である。なお、本実施の形態１では、各軸位置制御手段９は、各軸ごとにモータ位置制御と速度制御とを行うフィードバック制御系の構成を有するものとしたが、各軸毎にオブザーバを含む制御系の構成を有するものであってもよいし、フィードフォワードを含む制御系の構成を有するものであっても構わない。また、全て各軸独立した構成ではなく、軸間の干渉トルクをフィードフォワードもしくはフィードバックで補正する構成であっても構わない。
【００１６】
図４は、力覚考慮補正手段７の構成を示すブロック図である。図示するように、力覚考慮補正手段７は、減算器７１、第１座標変換手段７２、スティフネス行列部７３、加算器７４、減算器７５、比例ゲインフィルタ７６、第２座標変換手段７７、積分手段７８、微分手段７９、ダンピング行列部８０、第３座標変換手段８１、順変換手段８２を備えている。
【００１７】
順変換手段８２は、各軸モータ位置に対して順変換を行ってその時点での手先位置姿勢を算出する。減算器７１は、指令生成手段１が生成した位置指令と算出された手先位置姿勢との差分を算出する。第１座標変換手段７２は、減算器７１が出力した差分値をツール座標系における値に座標変換する。ツール座標系とは、ロボット先端に取り付けたツールやハンドに固定された座標系である。
【００１８】
スティフネス行列部７３は、第１座標変換手段７２の出力値とスティフネス行列とのベクトルの積を求め、求めた積を出力する。微分手段７９は、第１座標変換手段７２の出力の各要素の微分を算出する。ダンピング行列部８０は、微分手段７９の出力の各要素から構成されるベクトルとダンピング行列との積を求めて出力する。加算器７４は、スティフネス行列部７３の出力値とダンピング行列部８０の出力値とを加算する。
【００１９】
第３座標変換手段８１は、力推定オブザーバ４が出力した力推定値をツール座標系における値に変換する。減算器７５は、加算器７４の出力値と第３座標変換手段８１の出力値との差分を求めて出力する。比例ゲインフィルタ７６は、減算器７５の出力値に所定の比例ゲインを乗じる。第２座標変換手段７７は、比例ゲインフィルタ７６の出力値を直交座標系へ座標変換する。積分手段７８は、第２座標変換手段７７の出力値を積分し、得られた値を位置指令修正量（位置指令補正量）として出力する。
【００２０】
図５は、力推定オブザーバ４の構成を示すブロック図である。力推定オブザーバ４は、外力算出手段４１、推定トルク算出手段４２、および摩擦係数推定手段４３を備えている。
【００２１】
摩擦係数推定手段４３は、各軸の電流指令ｉを入力値とし、各軸のモータ位置ｑおよび力測定値を入力値とし、摩擦係数を求めるべきパラメータとしたシステム同定により摩擦係数を求める。以下に、摩擦係数推定手段４３による演算処理について具体的に述べる。
【００２２】
摩擦係数推定手段４３には、各軸の電流指令ｉ、各軸のモータ位置ｑ、３軸力覚センサ３１で測定した力測定値としての手先外力ｆが入力される。ロボット３が６軸垂直多関節ロボットである場合、電流指令ｉ、モータ位置ｑは６要素のベクトルとなり、外力ｆは、３軸力覚センサ３１の測定値なので、３要素のベクトルとなる。摩擦係数推定手段４３は、まず、各軸のトルクτｉ（ｉ＝１〜６）を算出する。各軸のトルクτｉ（ｉ＝１〜６）は、各軸のトルク定数ｋｔｉ（ｉ＝１〜６）、各軸の減速比ｇｅｎｉ、各軸の電流指令をｉｉとすると（ｉ（添え字）＝１〜６）、
τｉ＝ｋｔｉ＊ｉｉ／ｇｅｎｉ （１）
により算出される。
【００２３】
次に、摩擦係数推定手段４３は、各軸のモータ位置ｑｉから各軸のリンク位置ｑｌｉを次の式により求める。
ｑｌｉ＝ｑｉ／ｇｅｎｉ （２）
【００２４】
さらに、摩擦係数推定手段４３は、リンク速度ｖｉ、リンク加速度ａｉを前回周期の値との差をサンプリング時間で除算することで算出する。各軸のリンク位置、速度、加速度から構成されるベクトルをそれぞれｑｌ、ｖ、ａとし、慣性行列をＭ（ｑｌ）、遠心コリオリ力をｈ（ｑｌ，ｖ）、重力をｇ（ｑｌ）とし、摩擦係数推定手段４３は、摩擦力を除く駆動トルクの推定値τｒを次の式により求める。
τｒ＝Ｍ（ｑｌ）ａ＋ｈ（ｑｌ，ｖ）＋ｇ（ｑｌ） （３）
【００２５】
このとき、摩擦力をｆｒ（ｖ）、力覚センサ座標系を手先座標系としたときの手先速度ｖｘと関節速度ｖとの関係をヤコビ行列をＪ（ｑ）を用いて
ｖｘ＝Ｊ（ｑ）ｖ （４）
で表すとすると、手先座標系での各方向の外力及びモーメントで構成されるベクトルＦａｌｌは、
Ｆａｌｌ＝（Ｊ（ｑ）Ｔ）−１τｄ （５）
と表される。ここでτｄは各軸の外力を並べたベクトルであり、
τｄ＝τ−τｒ−ｆｒ（ｖ） （６）
である。式（５）および（６）から、
Ｆａｌｌ＝（Ｊ（ｑ）Ｔ）−１（τ−τｒ−ｆｒ（ｖ）） （７）
が導き出せる。
【００２６】
式（７）の左辺のＦａｌｌの最初の３要素は３軸力覚センサ３１により測定される力測定値である。また、式（７）の右辺の最初の３要素は、算出対象の摩擦係数を含む力推定値を示している。摩擦係数推定手段４３は、式（７）における最初の３要素の右辺と左辺との対応関係を用いて摩擦係数を算出する。
【００２７】
具体的には、各軸のクーロン摩擦係数をｆｃｉ、粘性摩擦係数をｆｖｉとし、ｓｇｎ（ｖｉ）を速度ｖｉが正のとき１、負のとき−1、０のとき０を取る関数とすると、各軸の摩擦力はｆｃｉ＊ｓｇｎ（ｖｉ）＋ｆｖｉ＊ｖｉで表される。（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの最初の３要素を取り出したベクトルをｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））の最初の３要素を取り出したベクトルをｆｂとすると、ｆｂの各要素はｆｃｉ、ｆｖｉに速度ｖ、位置ｑの関数を乗じた構成になっているので、ｆｃｉ、ｆｖｉを順番にたてからｆｃ１、ｆｖ１、ｆｃ２、ｆｖ２、・・・と並べた１２要素のベクトルをｐとすれば、
ｆａ＝ｙ（ｑ，ｖ）ｐ （８）
とかける。ここでｙは３行１２列の行列である。第ｋ番目の制御周期の演算結果を［ｋ］で表記することとすると、摩擦係数推定手段４３は、
Ｒ［ｋ］＝Ｒ［ｋ−１］＋ｓｔ＊（−ｋ１＊Ｒ［ｋ−１］＋ｙ［ｋ］Ｔｙ［ｋ］） （９）
ｒ［ｋ］＝ｒ［ｋ−１］＋ｓｔ＊（−ｋ１＊ｒ［ｋ−１］＋ｆａ［ｋ］ｙ［ｋ］Ｔ） （１０）
によりＲ［ｋ］およびｒ［ｋ］を算出し、
ｐ［ｋ］＝ｐ［ｋ−１］−ｓｔ＊ｋ２＊（Ｒ［ｋ］ｐ［ｋ−１］−ｒ［ｋ］） （１１）
により各軸にかかる摩擦係数を構成要素とするｐを算出する。なお、ｓｔ、ｋ１、ｋ２は予め設定されるシステム同定のためのパラメータである。
【００２８】
以上の演算により、摩擦係数推定手段４３は、時々刻々変化する各軸の電流指令ｉ、各軸のモータ位置ｑおよび力測定値に基づいて、その時点における摩擦係数を求めることができる。すなわち、摩擦係数推定手段４３は、摩擦係数が温度などにより変化しても、その摩擦係数を求めることができる。
【００２９】
推定トルク算出手段４２は、摩擦係数推定手段４３が推定した摩擦係数を用いて摩擦トルクｆｒ（ｖ）を算出し、
τｄ＝τ−τｒ−ｆｒ（ｖ） （１２）
で各軸の外乱トルクの推定値τｄを算出する。
【００３０】
外力算出手段４１では推定トルク算出手段４２で算出した外乱トルクτｄとヤコビ行列Ｊ（ｑ）をもちいて、
Ｆ＝（Ｊ（ｑ）Ｔ）−１τｄ （１３）
を算出する。このうち、最初の３要素は直接測定できるので、３軸力覚センサ３１の測定値に置き換えて出力し、残りの３要素は（１３）式の計算結果をそのまま出力する。
【００３１】
なお、以上の説明においては、（８）式の関係から（９）式〜（１１）式を用いて摩擦係数を算出したが（８）式の関係に対して最小二乗法などを適用することによって算出しても構わない。また本実施の形態１ではモータ電流としてモータ電流指令を用いたが、モータ電流のフィードバックを用いるようにしても構わない。またモータ位置としてモータ位置フィードバックを用いているが、最終的にモータに与えるモータ位置指令もしくはモータ位置指令から算出したモータモデル位置を用いるようにしても構わない。
【００３２】
以上述べたように、本発明の実施の形態１によれば、モータにより駆動されるロボット３と、前記ロボット３の先端部が指令位置に追従するように前記モータを駆動する電流指令を生成するロボット制御手段２と、前記ロボット３の先端部の６個の動作方向のうち、ｘ軸方向、ｙ軸方向、ｚ軸方向の３個の動作方向の夫々にかかる外力を検出する力覚センサ３１と、前記力覚センサ３１の検出値、前記モータの位置、および前記電流指令に基づいて、前記ロボットの先端部のｎ個の動作方向のうちの少なくとも前記力覚センサ３１が外力を非検出とする３つの軸の回転方向にかかる摩擦係数を夫々同定し、当該同定した摩擦係数に基づいて当該３つの軸の回転方向にかかるモーメントの推定値を算出する力推定オブザーバ４と、を備えるように構成したので、６軸力覚センサよりも廉価な３軸力覚センサ３１を用いても、当該３軸力覚センサ３１では測定できないモーメントを推定でき、さらに、時々刻々変化する各軸の電流指令ｉ、各軸のモータ位置ｑおよび力測定値に基づいて求めた摩擦係数を用いて測定できないモーメントを推定することができるので、予め設定された摩擦係数によりモーメントを推定する場合に比して高精度にモーメントを推定できる。すなわち、力覚センサの低コスト化を実現しながら、ロボットの先端部に作用する外力を高精度に算出することができる。
【００３３】
また、前記ロボット制御手段２は、前記モーメントの推定値とモータ位置とに基づいて前記位置指令を補正し、前記ロボットの先端部が前記補正した位置指令に追従するように前記電流指令を生成する、ように構成したので、精度よく力制御をすることができる。
【００３４】
実施の形態２．
実施の形態２の力制御装置の構成は、ロボット３の手首に取り付けられた力覚センサと、摩擦係数推定手段による摩擦係数の演算方法が異なるだけで、実施の形態１と等しい。実施の形態２の構成について実施の形態１と異なる構成についてのみ説明する。
【００３５】
実施の形態２の力制御装置のロボット手首に取り付けられた力覚センサは、ＸＹＺ３方向の力を測定できるセンサ（３軸力覚センサ３１）ではなく、Ｚ軸方向のみの１軸方向の力のみを測定できる。実施の形態２の力制御装置によれば、（５）式のＦａｌｌの第３要素のみ測定できるので、実施の形態１では、摩擦係数推定手段４３は、（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの最初の３要素を取り出したベクトルをｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））の最初の３要素をとりだしたベクトルをｆｂとして摩擦係数を算出したところ、実施の形態２では、（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの第３要素をｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））をｆｂとし、第３要素のみを用いて摩擦係数を算出する。
【００３６】
なお、本実施の形態２の説明では、Ｚ軸方向のみの力を測定できるタイプの力覚センサが採用されたが、測定できる方向はＸ軸方向やＹ軸方向であっても構わない。また、当該力覚センサが測定可能な外力は、Ｘ、Ｙ、Ｚ軸の周りのモーメントのうちの何れか１つであっても構わない。
【００３７】
以上述べたように、本発明の実施の形態２によれば、測定可能な方向は１方向しかない廉価な力覚センサを用いることができる。
【００３８】
実施の形態３．
実施の形態３の力制御装置の構成は、摩擦係数推定手段による摩擦係数を算出する演算方法が異なるだけで実施の形態１と同等である。実施の形態３の構成について実施の形態１と異なる構成についてのみ説明する。
【００３９】
実施の形態３の力制御装置によれば、摩擦係数推定手段は、各軸の摩擦係数を未知パラメータとして同定するのではなく、各軸の摩擦力を摩擦係数の初期値を用いて算出した摩擦力ｆｖ０（ｖｉ）＝ｆｃ０ｉ＊ｓｇｎ（ｖｉ）＋ｆｖ０ｉ＊ｖｉに共通に乗じる係数ｐ１の積で算出する、すなわち
ｆｖ＝ｐ１＊ｆｖ０ （１４）
で算出できるとみなし、ｐ１を未知パラメータとして同定する。具体的にはｆａ＝ｆｂからｐ１を算出し、摩擦係数をｐ１＊ｆｃ０ｉ，ｐ１＊ｆｖ０ｉとして出力する。
【００４０】
このように、本発明の実施の形態３によれば、実施の形態１のように摩擦係数そのものを未知のパラメータとしなくても、最終的に摩擦係数を用いることが可能となる。
【００４１】
実施の形態４．
実施の形態４の力制御装置の構成は、力覚センサと、摩擦係数推定手段による摩擦係数を算出する演算方法とが異なるだけで実施の形態１と同等である。実施の形態４の構成について実施の形態１と異なる構成についてのみ説明する。
【００４２】
実施の形態４にかかる力制御装置のロボット手首に取り付けた力覚センサは、ＸＹＺ３方向の力を測定できるセンサではなく、Ｘ方向およびＹ方向の力とＺ軸方向周りのモーメントの３成分のみを測定できる。（５）式のＦａｌｌの第１、２、６要素のみ測定できるので、摩擦係数推定手段は、実施の形態１では（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの最初の３要素を取り出したベクトルをｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））の最初の３要素をとりだしたベクトルをｆｂとして摩擦係数を算出したが、実施の形態４では（Ｊ（ｑ）Ｔ）−１（τ−τｒ）−Ｆａｌｌの第１、２、６要素をｆａ、（Ｊ（ｑ）Ｔ）−１（ｆｒ（ｖ））をｆｂとし、第１、２、６要素のみを用いて摩擦係数を算出する。
【００４３】
なお、以上の説明においては、力覚センサは、Ｘ方向およびＹ方向の力とＺ軸方向周りのモーメントの３成分のみを測定できるとしたが、本発明の実施の形態３は、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向、Ｘ軸を回転軸とした回転方向、Ｙ軸を回転軸とした回転方向、およびＺ軸を回転軸とした回転方向のうちの任意の３成分のみを測定できる力覚センサを採用した力制御装置に適用できる。
【００４４】
このように、本発明の実施の形態４によれば、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向、Ｘ軸を回転軸とした回転方向、Ｙ軸を回転軸とした回転方向、およびＺ軸を回転軸とした回転方向のうちの任意の３成分のみを測定できる力覚センサを採用した力制御装置であっても精度よく外力を算出することができる。
【００４５】
実施の形態５．
図６は、実施の形態５の力制御装置の構成を示すブロック図である。図示するように、実施の形態５の力制御装置は、指令生成手段１、ロボット制御手段１０、ロボット３、力推定オブザーバ４、および力情報モニター１１を備えている。なお、本図において、実施の形態１と同じ構成要素には同一の符号を付している。すなわち、実施の形態５によれば、力推定オブザーバ４の出力をモニター用途にのみ用い、制御には用いない点が実施の形態１と異なっている。
【００４６】
力情報モニター１１は、例えば各方向の力とモーメントとが許容範囲であるかを監視し、許容範囲を超過した場合にエラー信号を発生する。また、力情報モニター１１は、単に力推定値のログを取る、あるいは力推定値を表示出力する表示装置として機能するようにしてもよい。
【００４７】
図７は、実施の形態５のロボット制御手段１０の詳細な構成を説明するブロック図である。図示するように、実施の形態５では、力制御装置はコンプライアンス制御などは行わず、力情報（力推定値）をモニターするのみとする。したがって、ロボット制御手段１０の内部構成は、力覚考慮補正手段７を備えない一般に知られた位置制御のための構成と同一である。
【００４８】
このように、力推定値を制御ではなく状態の監視の用途に用いるようにしてもよい。
【産業上の利用可能性】
【００４９】
以上のように、本発明にかかる力制御装置は、ロボットを制御する力制御装置に適用して好適である。
【符号の説明】
【００５０】
１ 指令生成手段
２ ロボット制御手段
３ ロボット
４ 力推定オブザーバ
６ 加算器
７ 力覚考慮補正手段
８ 逆変換手段
９ 各軸位置制御手段
１０ ロボット制御手段
１１ 力情報モニター
３１ ３軸力覚センサ
３２ 位置センサ
４１ 外力算出手段
４２ 推定トルク算出手段
４３ 摩擦係数推定手段
７１ 減算器
７２ 座標変換手段
７３ スティフネス行列部
７４ 加算器
７５ 減算器
７６ 比例ゲインフィルタ
７７ 座標変換手段
７８ 積分手段
７９ 微分手段
８０ ダンピング行列部
８１ 座標変換手段
８２ 順変換手段
９１ 減算器
９２ 比例手段
９３ 微分手段
９４ 減算器
９５ 比例積分手段
【特許請求の範囲】
【請求項１】
モータにより駆動されるロボットと、
前記ロボットの先端部が指令位置に追従するように前記モータを駆動する電流指令を生成するロボット制御手段と、
前記ロボットの先端部のｍ個の動作方向のうち、ｎ（ｍ＞ｎ≧１）個の動作方向の夫々にかかる外力を検出する力覚センサと、
前記力覚センサの検出値、前記モータの位置、および前記電流指令に基づいて、前記ロボットの先端部のｎ個の動作方向のうちの少なくとも前記力覚センサが外力を非検出とするｎ−ｍ個の動作方向の外力算出に必要となる摩擦係数を夫々同定し、当該同定した摩擦係数に基づいて前記ｎ−ｍ個の動作方向にかかる外力の推定値を算出する力推定オブザーバと、
を備えることを特徴とする力制御装置。
【請求項２】
前記ロボット制御手段は、前記外力の推定値とモータ位置とに基づいて前記位置指令を補正し、前記ロボットの先端部が前記補正した位置指令に追従するように前記電流指令を生成する、
ことを特徴とする請求項１に記載の力制御装置。
【請求項３】
前記外力の推定値をモニタリングする力情報モニターをさらに備えることを特徴とする請求項１に記載の力制御装置。
【請求項４】
前記ｍは６であって、前記ｍ個の動作方向は、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向、Ｘ軸を回転軸とした回転方向、Ｙ軸を回転軸とした回転方向、およびＺ軸を回転軸とした回転方向である、
ことを特徴とした請求項１〜３のうちの何れか一項に記載の力制御装置。
【請求項１】
モータにより駆動されるロボットと、
前記ロボットの先端部が指令位置に追従するように前記モータを駆動する電流指令を生成するロボット制御手段と、
前記ロボットの先端部のｍ個の動作方向のうち、ｎ（ｍ＞ｎ≧１）個の動作方向の夫々にかかる外力を検出する力覚センサと、
前記力覚センサの検出値、前記モータの位置、および前記電流指令に基づいて、前記ロボットの先端部のｎ個の動作方向のうちの少なくとも前記力覚センサが外力を非検出とするｎ−ｍ個の動作方向の外力算出に必要となる摩擦係数を夫々同定し、当該同定した摩擦係数に基づいて前記ｎ−ｍ個の動作方向にかかる外力の推定値を算出する力推定オブザーバと、
を備えることを特徴とする力制御装置。
【請求項２】
前記ロボット制御手段は、前記外力の推定値とモータ位置とに基づいて前記位置指令を補正し、前記ロボットの先端部が前記補正した位置指令に追従するように前記電流指令を生成する、
ことを特徴とする請求項１に記載の力制御装置。
【請求項３】
前記外力の推定値をモニタリングする力情報モニターをさらに備えることを特徴とする請求項１に記載の力制御装置。
【請求項４】
前記ｍは６であって、前記ｍ個の動作方向は、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向、Ｘ軸を回転軸とした回転方向、Ｙ軸を回転軸とした回転方向、およびＺ軸を回転軸とした回転方向である、
ことを特徴とした請求項１〜３のうちの何れか一項に記載の力制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１２−１３５８２５（Ｐ２０１２−１３５８２５Ａ）
【公開日】平成２４年７月１９日（２０１２．７．１９）
【国際特許分類】
【出願番号】特願２０１０−２８８７０５（Ｐ２０１０−２８８７０５）
【出願日】平成２２年１２月２４日（２０１０．１２．２４）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２４年７月１９日（２０１２．７．１９）
【国際特許分類】
【出願日】平成２２年１２月２４日（２０１０．１２．２４）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]