JP2016179523A - ロボット制御装置およびロボットシステム - Google Patents

Abstract

【課題】広い空間を確保しなくてもロボットのパラメーターを得ることができる技術の提供。【解決手段】本発明のロボット制御装置は、先端にエンドエフェクターが装着されたアームと、第１の駆動軸と前記第１の駆動軸よりも先端側に位置する第２の駆動軸とで前記アームを駆動する駆動部と、を備えたロボットを制御するロボット制御装置であって、前記アームが複数の姿勢をとるように前記駆動部を制御する制御部と、前記アームが前記複数の姿勢をとった際の力検出器の検出結果に基づいて、前記エンドエフェクターの質量と、前記エンドエフェクターの重心位置と、前記ロボットの重力方向と、前記力検出器のオフセット誤差と、のうちの少なくとも１個を導出する導出部と、を備え、前記制御部は、前記第２の駆動軸の駆動量が前記第１の駆動軸の駆動量よりも大きくなるように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる。【選択図】図４

Description

本発明は、ロボット制御装置およびロボットシステムに関する。

ロボットアームを複数の姿勢に動作させ、そのときの力センサーの計測値に基づいて、ロボットアームの手先に装着されたツールのパラメーターを算出するキャリブレーション装置が知られている（特許文献１、参照。）。

特開２０１２−４０６３４号公報

しかしながら、複数の姿勢に動作させる際のロボットアームの可動範囲が大きく、キャリブレーションを行うための空間を広く確保しなければならないという問題があった。
本発明は、前記問題を解決するために創作されたものであって、広い空間を確保しなくてもロボットのパラメーターを得ることができる技術の提供を目的の一つとする。

前記目的を達成するためのロボット制御装置は、先端にエンドエフェクターが装着されたアームと、第１の駆動軸と第１の駆動軸よりも先端側に位置する第２の駆動軸とでアームを駆動する駆動部と、を備えたロボットを制御するロボット制御装置であって、アームが複数の姿勢をとるように駆動部を制御する制御部と、アームが複数の姿勢をとった際の力検出器の検出結果に基づいて、エンドエフェクターの質量と、エンドエフェクターの重心位置と、ロボットの重力方向と、力検出器のオフセット誤差と、のうちの少なくとも１個を導出する導出部と、を備え、制御部は、第２の駆動軸の駆動量が第１の駆動軸の駆動量よりも大きくなるように駆動部を制御して、アームに複数の姿勢をとらせる。

以上の構成において、駆動軸が駆動すれば、アームのうち当該駆動軸よりも先端側の部分が円軌道をとって移動することとなる。駆動する駆動軸が先端側であるほど、アームのうち当該駆動軸よりも先端側の部分の長さが小さくなり、当該先端側の部分の円軌道の半径が小さくなる。従って、先端側に存在する第２の駆動軸の駆動量が第１の駆動軸の駆動量よりも大きくなるように駆動部を制御することにより、アームの軌道円の半径を抑制することができる。すなわち、先端側に存在する第２の駆動軸の駆動量が第１の駆動軸の駆動量よりも大きくなるように駆動部を制御することにより、アームの可動範囲を抑制することができ、広い空間を確保しなくてもエンドエフェクターや力検出器のパラメーターを得ることができる。

なお請求項に記載された各手段の機能は、構成自体で機能が特定されるハードウェア資源、プログラムにより機能が特定されるハードウェア資源、又はそれらの組み合わせにより実現される。また、これら各手段の機能は、各々が物理的に互いに独立したハードウェア資源で実現されるものに限定されない。

ロボットシステムの模式図である。 ロボットシステムのブロック図である。 パラメーター導出処理のフローチャートである。 姿勢と関節の回転角との関係を示す表である。 エンドエフェクターの斜視図である。

以下、本発明の実施の形態を以下の順序にしたがって添付図面を参照しながら説明する。なお、各図において対応する構成要素には同一の符号が付され、重複する説明は省略される。
（１）第１実施形態：
（１−１）ロボットシステムの構成：
（１−２）パラメーターの導出方法：
（２）他の実施形態：

（１）第１実施形態：
（１−１）ロボットシステムの構成：
本発明の第一実施例としてのロボットシステムは、図１に示すように、ロボット１と、エンドエフェクター２と、制御端末３（コントローラー）と、教示端末４（ティーチングペンダント）と、を備えている。制御端末３と教示端末４とは、本発明のロボット制御装置を構成する。制御端末３は、本発明の制御部を構成する。制御端末３と教示端末４とは、それぞれ専用のコンピューターであってもよいし、ロボット１のためのプログラムがインストールされた汎用のコンピューターであってもよい。さらに、制御端末３と教示端末４とは、別々のコンピューターでなくてもよく、単一のコンピューターであってもよい。

ロボット１は、１つのアームＡを備える単腕ロボットであり、駆動部としてのアームＡは６つの関節Ｊ１、Ｊ２、Ｊ３、Ｊ４、Ｊ５、Ｊ６を備える。関節Ｊ１、Ｊ２、Ｊ３、Ｊ４、Ｊ５、Ｊ６によって６個のアーム部材Ａ１〜Ａ６が連結される。関節Ｊ２、Ｊ３、Ｊ５は曲げ関節であり、関節Ｊ１、Ｊ４、Ｊ６はねじり関節である。関節Ｊ６には、ワークに対して把持や加工等を行うためのエンドエフェクター２が装着される。先端の関節Ｊ６の回転軸上の所定位置をツールセンターポイント（ＴＣＰ）と表す。ＴＣＰの位置は各種のエンドエフェクター２の位置の基準となる。また、関節Ｊ６には力覚センサーＦＳが備えられている。力覚センサーＦＳは、６軸の力検出器である。力覚センサーＦＳは、固有の座標系であるセンサー座標系において互いに直交する３個の検出軸上の力の大きさと、当該３個の検出軸まわりのトルクの大きさとを検出する。

図１において、ワークＷを把持するエンドエフェクター２が関節Ｊ６の先端に装着されている。ロボット１が設置された空間を規定する座標系をロボット座標系と表す。ロボット座標系は、水平面上において互いに直交するＸ軸とＹ軸と、鉛直上向きを正方向とするＺ軸とによって規定される３次元の直交座標系である。Ｚ軸における負の方向は概ね重力方向と一致する。またＸ軸周りの回転角をＲＸで表し、Ｙ軸周りの回転角をＲＹで表し、Ｚ軸周りの回転角をＲＺで表す。Ｘ，Ｙ，Ｚ方向の位置により３次元空間における任意の位置を表現でき、ＲＸ，ＲＹ，ＲＺ方向の回転角により３次元空間における任意の姿勢（回転方向）を表現できる。以下、位置と表記した場合、姿勢も意味し得ることとする。また、力と表記した場合、トルクも意味し得ることとする。制御端末３は、アームＡを駆動することによって、ロボット座標系においてＴＣＰの位置を制御する。

図２は、ロボットシステムのブロック図である。制御端末３にはロボット１の制御を行うための制御プログラムがインストールされている。制御端末３は、プロセッサーやＲＡＭやＲＯＭを備え、これらのハードウェア資源が制御プログラムと協働する。これにより、制御端末３は、本発明の制御部として機能することとなる。

制御端末３は、例えばユーザーによる教示によって設定された目標位置と目標力とがＴＣＰにて実現されるようにアームＡを制御する。目標力とは、力覚センサーＦＳが検出すべき力である。Ｓの文字は、ロボット座標系を規定する軸の方向（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）のなかのいずれか１個の方向を表すこととする。例えば、Ｓ＝Ｘの場合、ロボット座標系にて設定された目標位置のＸ方向成分がＳ_t＝Ｘ_tと表記され、目標力のＸ方向成分がｆ_St＝ｆ_Xtと表記される。また、Ｓは、Ｓ方向の位置も表すこととする。

ロボット１は、図１に図示した構成のほかに、駆動部としてのモーターＭ１〜Ｍ６と、エンコーダーＥ１〜Ｅ６とを備える。アームＡを制御するとは、モーターＭ１〜Ｍ６を制御することを意味することとする。モーターＭ１〜Ｍ６とエンコーダーＥ１〜Ｅ６とは、関節Ｊ１〜Ｊ６のそれぞれに対応して備えられており、エンコーダーＥ１〜Ｅ６はモーターＭ１〜Ｍ６の駆動位置を検出する。制御端末３は、ロボット１と通信可能なっている。制御端末３は、モーターＭ１〜Ｍ６の駆動位置の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕ１を記憶している。また、制御端末３は、ロボット１が行う作業の工程ごとに目標位置Ｓ_tと目標力ｆ_Stとを記憶している。目標位置Ｓ_tと目標力ｆ_Stは例えば教示作業によってロボット１が行う作業の工程ごとに設定される。

制御端末３は、モーターＭ１〜Ｍ６の駆動位置Ｄ_aを取得すると、対応関係Ｕ１に基づいて、当該駆動位置Ｄ_aをロボット座標系におけるＴＣＰの位置Ｓ（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）に変換する。制御端末３は、ＴＣＰの位置Ｓと、力覚センサーＦＳの検出値とに基づいて、力覚センサーＦＳに現実に作用している作用力ｆ_Sをロボット座標系において特定する。制御端末３は、後述するパラメーター導出処理によって導出されたオフセット誤差を補償するように力覚センサーＦＳの検出値を補正する。制御端末３は、ロボット座標系におけるＴＣＰの位置Ｓごとに、力覚センサーＦＳのセンサー座標系における検出軸の方向を規定した対応関係Ｕ２を記憶している。従って、制御端末３は、ロボット座標系におけるＴＣＰの位置Ｓと対応関係Ｕ２とに基づいて、ロボット座標系における作用力ｆ_Sを特定できる。制御端末３は、作用力ｆ_Sに対して重力補償を行う。重力補償とは、作用力ｆ_Sから重力に起因する力やトルクの成分を除去することである。この重力補償において、後述するパラメーター導出処理によって導出されたパラメーターが利用される。また、重力補償を行った作用力ｆ_Sは、エンドエフェクター２に作用している重力以外の力と見なすことができる。

制御端末３は、目標力ｆ_Stと作用力ｆ_Sとをインピーダンス制御の運動方程式に代入することにより、力由来補正量ΔＳを特定する。下記の（１）式は、インピーダンス制御の運動方程式である。

（１）式の左辺は、ＴＣＰの位置Ｓの２階微分値に仮想慣性係数ｍを乗算した第１項と、ＴＣＰの位置Ｓの微分値に仮想粘性係数ｄを乗算した第２項と、ＴＣＰの位置Ｓに仮想弾性係数ｋを乗算した第３項とによって構成される。（１）式の右辺は、目標力ｆ_Stから現実の力ｆを減算した力偏差Δｆ_S（ｔ）によって構成される。（１）式における微分とは、時間による微分を意味する。ロボット１が行う工程において、目標力ｆ_Stとして一定値が設定される場合もあるし、目標力ｆ_Stとして時間に依存する関数によって導出される値が設定される場合もある。

インピーダンス制御とは、仮想の機械的インピーダンスをモーターＭ１〜Ｍ６によって実現する制御である。仮想慣性係数ｍはＴＣＰが仮想的に有する質量を意味し、仮想粘性係数ｄはＴＣＰが仮想的に受ける粘性抵抗を意味し、仮想弾性係数ｋはＴＣＰが仮想的に受ける弾性力のバネ定数を意味する。各係数ｍ，ｄ，ｋは方向ごとに異なる値に設定されてもよいし、方向に拘わらず共通の値に設定されてもよい。力由来補正量ΔＳとは、ＴＣＰが機械的インピーダンスを受けた場合に、目標力ｆ_Stとの力偏差Δｆ_S（ｔ）を解消するために、ＴＣＰが移動すべき位置Ｓの大きさを意味する。制御端末３は、目標位置Ｓ_tに、力由来補正量ΔＳを加算することにより、インピーダンス制御を考慮した補正目標位置（Ｓ_t＋ΔＳ）を特定する。

そして、制御端末３は、対応関係Ｕ１に基づいて、ロボット座標系を規定する各軸の方向の補正目標位置（Ｓ_t＋ΔＳ）を、各モーターＭ１〜Ｍ６の目標の駆動位置である目標駆動位置Ｄ_tに変換する。そして、制御端末３は、目標駆動位置Ｄ_tからモーターＭ１〜Ｍ６の現実の駆動位置Ｄ_aを減算することにより、駆動位置偏差Ｄ_e（＝Ｄ_t−Ｄ_a）を算出する。そして、制御端末３は、駆動位置偏差Ｄ_eに位置制御ゲインＫ_pを乗算した値と、現実の駆動位置Ｄ_aの時間微分値である駆動速度との差である駆動速度偏差に、速度制御ゲインＫ_vを乗算した値とを加算することにより、制御量Ｄ_cを特定する。なお、位置制御ゲインＫ_pおよび速度制御ゲインＫ_vは、比例成分だけでなく微分成分や積分成分にかかる制御ゲインを含んでもよい。制御量Ｄ_cは、モーターＭ１〜Ｍ６のそれぞれについて特定される。以上説明した構成により、制御端末３は、目標位置Ｓ_tと目標力ｆ_Stとに基づいてアームＡを制御することができる。

教示端末４には、制御端末３に目標位置Ｓ_tと目標力ｆ_Stとを教示するための教示プログラムがインストールされている。教示端末４は、プロセッサーやＲＡＭやＲＯＭを備え、これらのハードウェア資源が制御プログラムと協働する。これにより、図２に示すように、教示端末４は、機能構成として姿勢設定部４１と導出部４２とを備えることとなる。図示しないが、教示端末４は、ユーザーからの指示を受け付ける入力装置（マウス、キーボード、タッチパネル等）と、ユーザーに各種情報を出力する出力装置（ディスプレイ、スピーカー等）を備える。以下、姿勢設定部４１と導出部４２とが行う処理の詳細をフローチャートとともに説明する。

図３は、パラメーター導出処理のフローチャートである。パラメーター導出処理は、例えばエンドエフェクター２を交換した場合に実行される処理である。パラメーター導出処理は、アームＡにエンドエフェクター２が装着された状態で行われ、エンドエフェクター２でワークＷを把持した状態で行われてもよい。まず、姿勢設定部４１は、アームＡの先端側の３個の関節Ｊ４〜Ｊ６の回転軸方向を取得する（ステップＳ１００）。パラメーター導出処理を開始するにあたり、例えばアームＡが所望の姿勢をとるようにユーザーが手でアームＡを動かしてもよい。パラメーター導出処理の開始時におけるアームＡの姿勢はどのようなものであってもよいが、ユーザーはロボット１が設置された環境において障害物からできるだけ離れた位置となるようにアームＡの姿勢をとらせておくことが望ましい。

姿勢設定部４１は、現在におけるモーターＭ１〜Ｍ６の駆動位置Ｄ_aに基づいてロボット座標系における関節Ｊ４〜Ｊ６の回転軸方向を算出できる。例えば、アームＡの根本側から数えて４個目の関節Ｊ４の回転軸方向は、関節Ｊ４よりも根本側の関節Ｊ１〜Ｊ３を駆動するモーターＭ１〜Ｍ３の駆動位置Ｄ_aに基づいて算出できる。

次に、姿勢設定部４１は、関節Ｊ４〜Ｊ６の回転軸方向が適正であるか否かを判定する（ステップＳ１１０）。すなわち、姿勢設定部４１は、アームＡが備える６軸の関節Ｊ１〜Ｊ６のうち、先端側の３軸の関節Ｊ４〜Ｊ６の回転軸方向が適正であるか否かを判定する。関節Ｊ４〜Ｊ６の回転軸方向が適正であるとは、関節Ｊ４〜Ｊ６のすべての回転軸方向がＺ軸の方向から所定角度（例えば３度）以上ずれた方向であることを意味する。本実施形態において、重力方向は厳密には未知であるが、ロボット座標系のＺ軸における負の方向は概ね重力方向と一致するため、Ｚ軸の方向から所定角度以上ずれた方向は重力方向から所定基準以上ずれた方向であると見なすことができる。

関節Ｊ４〜Ｊ６の回転軸方向が適正であると判定しなかった場合（ステップＳ１１０：Ｎ）、姿勢設定部４１は、アームＡを駆動させる（ステップＳ１２０）。すなわち、姿勢設定部４１は、制御端末３に対して、関節Ｊ４〜Ｊ６の回転軸方向が変化するようにアームＡの制御を実行させる。例えば、姿勢設定部４１は、先端側の３個の関節Ｊ４〜Ｊ６のうち、最も根本側の関節Ｊ３が現在の位置から所定角度（例えば５度）だけ回転するようにアームＡを駆動させることにより、関節Ｊ４〜Ｊ６の回転軸方向を一括して変化させてもよい。アームＡを駆動させると、姿勢設定部４１は、ステップＳ１００に戻る。以上の処理により、姿勢設定部４１は、関節Ｊ４〜Ｊ６のすべての回転軸方向が重力方向から所定基準以上ずれたと見なせる方向となるまでアームＡを駆動することができる。

一方、関節Ｊ４〜Ｊ６の回転軸方向が適正であると判定した場合（ステップＳ１１０：Ｙ）、姿勢設定部４１と導出部４２とは、複数の姿勢をとるようにアームＡを制御し、姿勢ごとに力覚センサーＦＳの検出結果を取得する（ステップＳ１３０）。具体的に、姿勢設定部４１は、制御端末３に指令することにより、アームＡが備える駆動軸としての関節Ｊ１〜Ｊ６のうち先端側の３個の関節Ｊ４〜Ｊ６が駆動し、残りの３個の関節Ｊ１〜Ｊ３が駆動しないようにモーターＭ１〜Ｍ６を制御して、アームＡに複数の姿勢をとらせる。すなわち、姿勢設定部４１は、第１の駆動軸としての関節Ｊ１〜Ｊ３よりも先端側に位置する第２の駆動軸としての関節Ｊ４〜Ｊ６の駆動量が、第１の駆動軸としての関節Ｊ１〜Ｊ３の駆動量よりも大きくなるようにモーターＭ１〜Ｍ６を制御して、アームＡに複数の姿勢をとらせる。なお、姿勢設定部４１は、ステップＳ１１０において関節Ｊ４〜Ｊ６の回転軸方向が適正であると判定した際における関節Ｊ４〜Ｊ６の回転角をモーターＭ４〜Ｍ６の駆動位置Ｄ_aに基づいて取得し、当該回転角を初期角として記憶媒体に記憶する。

図４は、アームＡの複数の姿勢と関節Ｊ１〜Ｊ６の回転角との関係を示す表である。図４に示すように、アームＡの複数の姿勢として、第１〜第８姿勢が定義されている。姿勢設定部４１は、制御端末３に対して、関節Ｊ１〜Ｊ６の回転角に対応するモーターＭ１〜Ｍ６の目標駆動位置Ｄ_tを出力することにより、アームＡに第１〜第８姿勢をとらせる。図４に示すように、第１〜第８姿勢のいずれにおいても、第１の駆動軸としての関節Ｊ１〜Ｊ３の回転角は初期角に維持される。すなわち、アームＡが第１〜第８姿勢をとった場合における関節Ｊ１〜Ｊ３に対応するモーターＭ１〜Ｍ３の駆動量はいずれも０となる。第２〜第８姿勢において、第２の駆動軸としての関節Ｊ４〜Ｊ６のうち少なくとも１個の回転角は初期角から６０度変化する。

ここで、関節Ｊ４の回転軸方向が重力方向であるとした場合、関節Ｊ４以外の関節Ｊ１〜Ｊ３，Ｊ５，Ｊ６の回転角が同一の姿勢同士が、重力方向の軸まわりに回転対称となる。関節Ｊ４の回転軸方向が重力方向である場合、第１姿勢と第５姿勢とが重力方向の軸まわりに回転対称となり、第２姿勢と第６姿勢とが重力方向の軸まわりに回転対称となり、第３姿勢と第７姿勢とが重力方向の軸まわりに回転対称となり、第４姿勢と第８姿勢とが重力方向の軸まわりに回転対称となる。従って、関節Ｊ４の回転軸方向が重力方向であるとした場合、第１〜第８姿勢のうち、重力方向の軸まわりに対称とならない姿勢の個数は４個となる。同様に、関節Ｊ５の回転軸方向が重力方向であるとした場合、第１〜第８姿勢のうち、重力方向の軸まわりに対称とならない姿勢の個数は４個となる。さらに、関節Ｊ６の回転軸方向が重力方向であるとした場合、第１〜第８姿勢のうち、重力方向の軸まわりに対称とならない姿勢の個数は４個となる。本実施形態では、ステップＳ１００〜Ｓ１２０によって、鉛直方向に対して関節Ｊ４〜Ｊ６の回転軸方向がずれた方向となっていることが保証されているため、第１〜第８姿勢のうち、重力方向の軸まわりに対称とならない姿勢の個数は閾値としての８個以上となる。

ここで、駆動する関節Ｊ１〜Ｊ６が先端側であるほど、アームＡのうち当該関節Ｊ４〜Ｊ６よりも先端側の部分の長さが小さくなり、当該先端側の部分の円軌道の半径が小さくなる。従って、先端側の関節Ｊ４〜Ｊ６の駆動量が根本側の関節Ｊ１〜Ｊ３の駆動量よりも大きくなるようにモーターＭ１〜Ｍ６を制御することにより、アームＡの軌道円の半径を抑制することができる。すなわち、ステップＳ１３０においてアームＡの可動範囲を抑制することができ、広い空間を確保しなくても済む。なお、ステップＳ１３０においては、エンドエフェクター２に重力のみが作用するようにする必要があり、アームＡが障害物に接触しないことが要求される。

ステップＳ１３０において、導出部４２は、第１〜第８姿勢のそれぞれにおける力覚センサーＦＳの検出結果をロボット１から取得する。すなわち、導出部４２は、力覚センサーＦＳに固有のセンサー座標系において互いに直交する３個の検出軸上の力の大きさと、当該３個の検出軸まわりのトルクの大きさとを取得する。

そして、導出部４２は、第１〜第８姿勢と力覚センサーＦＳの検出結果とに基づいてパラメーターを導出する（ステップＳ１４０）。ここで導出されるパラメーターおよびパラメーターを導出方法の詳細については後述する。最後に、導出部４２は、導出したパラメーターを制御端末３に出力する（ステップＳ１５０）。これにより、導出したパラメーターに基づいて制御端末３はアームＡの制御（例えば重力補償）を行うことが可能となる。

（１−２）パラメーターの導出方法：
以下、図３のステップＳ１４０においてパラメーターを導出する手法について説明する。図５は、エンドエフェクター２の斜視図である。図５に示すように、力覚センサーＦＳの基準位置Ｏから互いに直交する方向（α方向，β方向，γ方向）に延びる３個の検出軸によってセンサー座標系が規定されており、各検出軸の方向における力のベクトルＦ＝［Ｆ_α，Ｆ_β，Ｆ_γ］と、各検出軸まわりのトルクのベクトルＴ＝［Ｔ_α，Ｔ_β，Ｔ_γ］とが力覚センサーＦＳの検出結果として取得されている。

まず、センサー座標系におけるエンドエフェクター２および把持しているワークの重心位置ＧＰの位置ベクトルｒを下記の（２）式のように定義する。

ここで、エンドエフェクター２に重力のみが作用している場合の力の釣り合いを考えると、以下の（３）式が成り立つ。

また、エンドエフェクター２に重力のみが作用している場合のモーメントの釣り合いを考えると、以下の（４）式が成り立つ。

前記の（３）（４）式において、姿勢行列Ｒは、ロボット座標系からみた力覚センサーＦＳの姿勢を表す回転行列である。導出部４２は、モーターＭ１〜Ｍ６の駆動位置の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕ１に基づいて、図４に示す各姿勢をとった場合のＴＣＰの位置Ｓを導出する。さらに、導出部４２は、ロボット座標系におけるＴＣＰの位置Ｓごとに、力覚センサーＦＳのセンサー座標系における検出軸の方向を規定した対応関係Ｕ２に基づいて、図４に示す姿勢のそれぞれについて姿勢行列Ｒを導出できる。ベクトルｇは、ロボット座標系における重力加速度のベクトルであり、そのベクトルの大きさの絶対値は重量加速度の大きさを示すスカラーであり、例えば９．８ｍ／ｓ²である。ベクトルＦ_ofsはセンサー座標系における力のオフセット誤差であり、ベクトルＴ_ofsはセンサー座標系におけるトルクのオフセット誤差である。また、行列の右肩のＴは転置行列であることを意味する。ｍｅはエンドエフェクター２および把持しているワークの質量を表す。

図４に示すｉ（ｉ＝１〜７）番目の姿勢において検出された力のベクトルＦから、０（ｉ＝０）番目の第１姿勢において検出された力のベクトルＦを減算した力差ベクトルをＦ_iと表すこととする。前記の（３）式により、力差ベクトルをＦ_iは、下記の（５）式のように表すことができる。

姿勢行列Ｒ_iはｉ番目の姿勢において、ロボット座標系からみた力覚センサーＦＳの姿勢を表す回転行列である。

ｉ（ｉ＝１〜７）番目の姿勢において検出されたトルクのベクトルＴから、０（ｉ＝０）番目の第１姿勢において検出されたトルクのベクトルＴを減算したトルク差ベクトルをＴ_iと表すこととする。前記の（４）式により、トルク差ベクトルをＴ_iは、下記の（６）式のように表すことができる。

ここで、（５）式の左辺の力差ベクトルＦ_iを１〜７番目の姿勢についてまとめたベクトルを統合力差ベクトルＦ_allとし、（５）式の右辺の行列（Ｒ_i−Ｒ₀）^Tを１〜７番目の姿勢についてまとめた行列を統合姿勢差行列Ｒ_allとする。統合力差ベクトルＦ_allと統合姿勢差行列Ｒ_allは下記の（７）式で表される。

統合力差ベクトルＦ_allと統合姿勢差行列Ｒ_allを用いると（５）式は、下記の（８）式のように表すことができる。

さらに、（８）式は下記の（９）式のように変形できる。

最小二乗法等によって、（９）式に基づいてｇｍｅを求め、当該ｇｍｅのベクトルの大きさを既知の重力加速度の大きさのスカラー量で除算することにより、導出部４２は、エンドエフェクター２の質量ｍｅを導出できる。

また、ｇｍｅをｍｅで除算することにより、導出部４２は、重力加速度の方向を含む重力加速度ベクトルｇを導出できる。
次に、エンドエフェクター２の重心位置ＧＰの導出方法について説明する。まず、各姿勢において力覚センサーの姿勢とエンドエフェクター２の質量ｍｅと重力加速度ベクトルｇにより算出される力をベクトルａで表し、当該算出された力の差のベクトルＦ_iを算出力差ベクトルａ_i＝［ａ_α，ａ_β，ａ_γ］と定義する。これを（５）式に当てはめると、下記の（１０）式のように表すことができる。

また、（１０）式の関係を（６）式に代入すると、下記の（１１）式を得ることができる。

ただし、Ａ_ｉは下記の（１２）式で表される。また、Ａ_ｉを算出力差行列と表記する。

ここで、（６）式の左辺のトルク差ベクトルＴ_iを１〜７番目の姿勢についてまとめたベクトルを統合トルク差ベクトルＴ_allとし、（１２）式の算出力差行列Ａ_ｉを１〜７番目の姿勢についてまとめたベクトルを統合算出力差ベクトルＡ_allとする。統合力差ベクトルＦ_allと統合算出力差ベクトルＡ_allは下記の（１３）式で表される。

統合トルク差ベクトルＴ_allと統合算出力差ベクトルＡ_allと用いると（１１）式は、下記の（１４）式のように表すことができる。

さらに、（１４）式は下記の（１５）式のように変形できる。

最小二乗法等によって、（１５）式に基づいて、導出部４２は、（２）式に示す重心位置ＧＰの位置ベクトルｒを導出できる。重心位置ＧＰの位置ベクトルｒを導出することは、エンドエフェクター２の重心位置を導出することを意味する。

導出したエンドエフェクター２の質量ｍｅを（３）式に代入して解くことにより、導出部４２は、力のオフセット誤差Ｆ_ofsを導出できる。さらに、導出したエンドエフェクター２の質量ｍｅと重心位置ＧＰの位置ベクトルｒを（４）式に代入して解くことにより、導出部４２は、トルクのオフセット誤差Ｍ_ofsを導出できる。

以上のようにして導出したパラメーターを制御端末３に出力することにより、制御端末３は力覚センサーＦＳの検出値に対してオフセット誤差補償と重力補償とを行うことが可能となる。エンドエフェクター２に作用する重力は、重心位置ＧＰに位置する質量ｍｅの質点に作用する重力と同視できるため、制御端末３は、以上のようにして導出したパラメーターを使用して力覚センサーＦＳの検出値から重力成分（力，トルク）を除去できる。むろん、以上のようにして導出したパラメーターに基づいて、制御端末３の制御における各種パラメーターｆ_St，ｋ，ｍ，ｄ，Ｋ_p，Ｋ_vが設定されてもよい。

（２）他の実施形態
図３のステップＳ１１０において、関節Ｊ４〜Ｊ６の回転軸方向が適正であると判定しなかった場合、姿勢設定部４１は、制御端末３にアームＡを駆動させるのではなく、ユーザーにアームＡを動かすよう促してもよい。すなわち、重力方向からのずれが所定基準以内の軸まわりに対称とならない姿勢の個数は閾値としての８個未満となる場合おいて、導出部４２は、ユーザーに通知を行ってもよい。例えば、導出部４２は、ユーザーにアームＡを動かすべき旨のメッセージを教示端末４のディスプレイに表示させてもよい。なお、閾値は必ずしも８個でなくてもよく、導出パラメーターの精度に応じて閾値が増減してもよい。

ロボット１は、必ずしも６軸の単腕ロボットでなくてもよく、ロボット１は、双腕ロボットであってもよいし、スカラーロボットであってもよい。また、ロボット１は、少なくとも２軸の駆動軸を備えたロボットであればよい。例えば、２軸のうち、先端側の駆動軸の回転角の範囲をＱ度とするのに対して、もう一方の駆動軸の回転角の範囲をＰ（＜Ｑ）度に制限して、複数の姿勢をとるようにしてもよい。また、力検出器は、力覚センサーＦＳでなくてもよく、関節Ｊ１〜Ｊ６ごとに当該関節Ｊ１〜Ｊ６に作用するトルクを検出するトルクセンサーであってもよい。また、トルクセンサーの代わりにモーターＭ１〜Ｍ６の負荷に基づいてトルクが検出されてもよい。

１…ロボット、２…エンドエフェクター、３…制御端末、４…教示端末、４１…姿勢設定部、４２…導出部、Ａ１〜Ａ６…アーム部材、Ｅ１〜Ｅ６…エンコーダー、ＦＳ…力覚センサー、ＧＰ…重心位置、Ｊ１〜Ｊ６…関節、Ｍ１〜Ｍ６…モーター、Ｗ…ワーク、ｄ…仮想粘性係数、ｋ…仮想弾性係数、ｍ…仮想慣性係数

Claims (6)

1. 先端にエンドエフェクターが装着されたアームと、第１の駆動軸と前記第１の駆動軸よりも先端側に位置する第２の駆動軸とで前記アームを駆動する駆動部と、を備えたロボットを制御するロボット制御装置であって、
   前記アームが複数の姿勢をとるように前記駆動部を制御する制御部と、
   前記アームが前記複数の姿勢をとった際の力検出器の検出結果に基づいて、
   前記エンドエフェクターの質量と、
   前記エンドエフェクターの重心位置と、
   前記ロボットの重力方向と、
   前記力検出器のオフセット誤差と、のうちの少なくとも１個を導出する導出部と、を備え、
   前記制御部は、前記第２の駆動軸の駆動量が前記第１の駆動軸の駆動量よりも大きくなるように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
   ロボット制御装置。
2. 前記制御部は、回転軸方向が重力方向から所定基準以上ずれた駆動軸が駆動するように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
   請求項１に記載のロボット制御装置。
3. 前記制御部は、重力方向からのずれが所定基準以内の軸まわりに対称とならない姿勢の個数が閾値以上となるように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
   請求項１または請求項２に記載のロボット制御装置。
4. 前記導出部は、前記複数の姿勢のうち、重力方向からのずれが所定基準以内の軸まわりに対称とならない姿勢の個数が閾値未満となる場合に、ユーザーに通知を行う、
   請求項１または請求項２に記載のロボット制御装置。
5. 前記アームは６軸の駆動軸を備え、
   前記制御部は、前記アームが備える駆動軸のうち先端側の３軸が駆動し、残りの３軸が駆動しないように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
   請求項１から請求項４のいずれか一項に記載のロボット制御装置。
6. 第１の駆動軸と前記第１の駆動軸よりも先端に位置する第２の駆動軸とを有するアームを駆動する駆動部と、
   前記アームに作用する力を検出する力検出器と、
   前記アームが複数の姿勢をとるように前記駆動部を制御する制御部と、
   前記アームが前記複数の姿勢をとった際の前記力検出器の検出結果に基づいて、
   前記エンドエフェクターの質量と、
   前記エンドエフェクターの重心位置と、
   前記ロボットの重力方向と、
   前記力検出器のオフセット誤差と、のうちの少なくとも１個を導出する導出部と、を備え、
   前記制御部は、前記第２の駆動軸の駆動量が前記第１の駆動軸の駆動量よりも大きくなるように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
   ロボットシステム。

Images