(57)【要約】【課題】広い空間を確保しなくてもロボットのパラメーターを得ることができる技術の提供。【解決手段】本発明のロボット制御装置は、先端にエンドエフェクターが装着されたアームと、第１の駆動軸と前記第１の駆動軸よりも先端側に位置する第２の駆動軸とで前記アームを駆動する駆動部と、を備えたロボットを制御するロボット制御装置であって、前記アームが複数の姿勢をとるように前記駆動部を制御する制御部と、前記アームが前記複数の姿勢をとった際の力検出器の検出結果に基づいて、前記エンドエフェクターの質量と、前記エンドエフェクターの重心位置と、前記ロボットの重力方向と、前記力検出器のオフセット誤差と、のうちの少なくとも１個を導出する導出部と、を備え、前記制御部は、前記第２の駆動軸の駆動量が前記第１の駆動軸の駆動量よりも大きくなるように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる。【選択図】図４
2016179523.tif 000002
【特許請求の範囲】
【請求項１】
先端にエンドエフェクターが装着されたアームと、第１の駆動軸と前記第１の駆動軸よりも先端側に位置する第２の駆動軸とで前記アームを駆動する駆動部と、を備えたロボットを制御するロボット制御装置であって、
前記アームが複数の姿勢をとるように前記駆動部を制御する制御部と、
前記アームが前記複数の姿勢をとった際の力検出器の検出結果に基づいて、
前記エンドエフェクターの質量と、
前記エンドエフェクターの重心位置と、
前記ロボットの重力方向と、
前記力検出器のオフセット誤差と、のうちの少なくとも１個を導出する導出部と、を備え、
前記制御部は、前記第２の駆動軸の駆動量が前記第１の駆動軸の駆動量よりも大きくなるように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
ロボット制御装置。
【請求項２】
前記制御部は、回転軸方向が重力方向から所定基準以上ずれた駆動軸が駆動するように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
請求項１に記載のロボット制御装置。
【請求項３】
前記制御部は、重力方向からのずれが所定基準以内の軸まわりに対称とならない姿勢の個数が閾値以上となるように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
請求項１または請求項２に記載のロボット制御装置。
【請求項４】
前記導出部は、前記複数の姿勢のうち、重力方向からのずれが所定基準以内の軸まわりに対称とならない姿勢の個数が閾値未満となる場合に、ユーザーに通知を行う、
請求項１または請求項２に記載のロボット制御装置。
【請求項５】
前記アームは６軸の駆動軸を備え、
前記制御部は、前記アームが備える駆動軸のうち先端側の３軸が駆動し、残りの３軸が駆動しないように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
請求項１から請求項４のいずれか一項に記載のロボット制御装置。
【請求項６】
第１の駆動軸と前記第１の駆動軸よりも先端に位置する第２の駆動軸とを有するアームを駆動する駆動部と、
前記アームに作用する力を検出する力検出器と、
前記アームが複数の姿勢をとるように前記駆動部を制御する制御部と、
前記アームが前記複数の姿勢をとった際の前記力検出器の検出結果に基づいて、
前記エンドエフェクターの質量と、
前記エンドエフェクターの重心位置と、
前記ロボットの重力方向と、
前記力検出器のオフセット誤差と、のうちの少なくとも１個を導出する導出部と、を備え、
前記制御部は、前記第２の駆動軸の駆動量が前記第１の駆動軸の駆動量よりも大きくなるように前記駆動部を制御して、前記アームに前記複数の姿勢をとらせる、
ロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置およびロボットシステムに関する。
【背景技術】
【０００２】
ロボットアームを複数の姿勢に動作させ、そのときの力センサーの計測値に基づいて、ロボットアームの手先に装着されたツールのパラメーターを算出するキャリブレーション装置が知られている（特許文献１、参照。）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１２−４０６３４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、複数の姿勢に動作させる際のロボットアームの可動範囲が大きく、キャリブレーションを行うための空間を広く確保しなければならないという問題があった。
本発明は、前記問題を解決するために創作されたものであって、広い空間を確保しなくてもロボットのパラメーターを得ることができる技術の提供を目的の一つとする。
【課題を解決するための手段】
【０００５】
前記目的を達成するためのロボット制御装置は、先端にエンドエフェクターが装着されたアームと、第１の駆動軸と第１の駆動軸よりも先端側に位置する第２の駆動軸とでアームを駆動する駆動部と、を備えたロボットを制御するロボット制御装置であって、アームが複数の姿勢をとるように駆動部を制御する制御部と、アームが複数の姿勢をとった際の力検出器の検出結果に基づいて、エンドエフェクターの質量と、エンドエフェクターの重心位置と、ロボットの重力方向と、力検出器のオフセット誤差と、のうちの少なくとも１個を導出する導出部と、を備え、制御部は、第２の駆動軸の駆動量が第１の駆動軸の駆動量よりも大きくなるように駆動部を制御して、アームに複数の姿勢をとらせる。
【０００６】
以上の構成において、駆動軸が駆動すれば、アームのうち当該駆動軸よりも先端側の部分が円軌道をとって移動することとなる。駆動する駆動軸が先端側であるほど、アームのうち当該駆動軸よりも先端側の部分の長さが小さくなり、当該先端側の部分の円軌道の半径が小さくなる。従って、先端側に存在する第２の駆動軸の駆動量が第１の駆動軸の駆動量よりも大きくなるように駆動部を制御することにより、アームの軌道円の半径を抑制することができる。すなわち、先端側に存在する第２の駆動軸の駆動量が第１の駆動軸の駆動量よりも大きくなるように駆動部を制御することにより、アームの可動範囲を抑制することができ、広い空間を確保しなくてもエンドエフェクターや力検出器のパラメーターを得ることができる。
【０００７】
なお請求項に記載された各手段の機能は、構成自体で機能が特定されるハードウェア資源、プログラムにより機能が特定されるハードウェア資源、又はそれらの組み合わせにより実現される。また、これら各手段の機能は、各々が物理的に互いに独立したハードウェア資源で実現されるものに限定されない。
【図面の簡単な説明】
【０００８】
【図１】ロボットシステムの模式図である。
【図２】ロボットシステムのブロック図である。
【図３】パラメーター導出処理のフローチャートである。
【図４】姿勢と関節の回転角との関係を示す表である。
【図５】エンドエフェクターの斜視図である。
【発明を実施するための形態】
【０００９】
以下、本発明の実施の形態を以下の順序にしたがって添付図面を参照しながら説明する。なお、各図において対応する構成要素には同一の符号が付され、重複する説明は省略される。
（１）第１実施形態：
（１−１）ロボットシステムの構成：
（１−２）パラメーターの導出方法：
（２）他の実施形態：
【００１０】
（１）第１実施形態：
（１−１）ロボットシステムの構成：
本発明の第一実施例としてのロボットシステムは、図１に示すように、ロボット１と、エンドエフェクター２と、制御端末３（コントローラー）と、教示端末４（ティーチングペンダント）と、を備えている。制御端末３と教示端末４とは、本発明のロボット制御装置を構成する。制御端末３は、本発明の制御部を構成する。制御端末３と教示端末４とは、それぞれ専用のコンピューターであってもよいし、ロボット１のためのプログラムがインストールされた汎用のコンピューターであってもよい。さらに、制御端末３と教示端末４とは、別々のコンピューターでなくてもよく、単一のコンピューターであってもよい。
【００１１】
ロボット１は、１つのアームＡを備える単腕ロボットであり、駆動部としてのアームＡは６つの関節Ｊ１、Ｊ２、Ｊ３、Ｊ４、Ｊ５、Ｊ６を備える。関節Ｊ１、Ｊ２、Ｊ３、Ｊ４、Ｊ５、Ｊ６によって６個のアーム部材Ａ１〜Ａ６が連結される。関節Ｊ２、Ｊ３、Ｊ５は曲げ関節であり、関節Ｊ１、Ｊ４、Ｊ６はねじり関節である。関節Ｊ６には、ワークに対して把持や加工等を行うためのエンドエフェクター２が装着される。先端の関節Ｊ６の回転軸上の所定位置をツールセンターポイント（ＴＣＰ）と表す。ＴＣＰの位置は各種のエンドエフェクター２の位置の基準となる。また、関節Ｊ６には力覚センサーＦＳが備えられている。力覚センサーＦＳは、６軸の力検出器である。力覚センサーＦＳは、固有の座標系であるセンサー座標系において互いに直交する３個の検出軸上の力の大きさと、当該３個の検出軸まわりのトルクの大きさとを検出する。
【００１２】
図１において、ワークＷを把持するエンドエフェクター２が関節Ｊ６の先端に装着されている。ロボット１が設置された空間を規定する座標系をロボット座標系と表す。ロボット座標系は、水平面上において互いに直交するＸ軸とＹ軸と、鉛直上向きを正方向とするＺ軸とによって規定される３次元の直交座標系である。Ｚ軸における負の方向は概ね重力方向と一致する。またＸ軸周りの回転角をＲＸで表し、Ｙ軸周りの回転角をＲＹで表し、Ｚ軸周りの回転角をＲＺで表す。Ｘ，Ｙ，Ｚ方向の位置により３次元空間における任意の位置を表現でき、ＲＸ，ＲＹ，ＲＺ方向の回転角により３次元空間における任意の姿勢（回転方向）を表現できる。以下、位置と表記した場合、姿勢も意味し得ることとする。また、力と表記した場合、トルクも意味し得ることとする。制御端末３は、アームＡを駆動することによって、ロボット座標系においてＴＣＰの位置を制御する。
【００１３】
図２は、ロボットシステムのブロック図である。制御端末３にはロボット１の制御を行うための制御プログラムがインストールされている。制御端末３は、プロセッサーやＲＡＭやＲＯＭを備え、これらのハードウェア資源が制御プログラムと協働する。これにより、制御端末３は、本発明の制御部として機能することとなる。
【００１４】
制御端末３は、例えばユーザーによる教示によって設定された目標位置と目標力とがＴＣＰにて実現されるようにアームＡを制御する。目標力とは、力覚センサーＦＳが検出すべき力である。Ｓの文字は、ロボット座標系を規定する軸の方向（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）のなかのいずれか１個の方向を表すこととする。例えば、Ｓ＝Ｘの場合、ロボット座標系にて設定された目標位置のＸ方向成分がＳt＝Ｘtと表記され、目標力のＸ方向成分がｆSt＝ｆXtと表記される。また、Ｓは、Ｓ方向の位置も表すこととする。
【００１５】
ロボット１は、図１に図示した構成のほかに、駆動部としてのモーターＭ１〜Ｍ６と、エンコーダーＥ１〜Ｅ６とを備える。アームＡを制御するとは、モーターＭ１〜Ｍ６を制御することを意味することとする。モーターＭ１〜Ｍ６とエンコーダーＥ１〜Ｅ６とは、関節Ｊ１〜Ｊ６のそれぞれに対応して備えられており、エンコーダーＥ１〜Ｅ６はモーターＭ１〜Ｍ６の駆動位置を検出する。制御端末３は、ロボット１と通信可能なっている。制御端末３は、モーターＭ１〜Ｍ６の駆動位置の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕ１を記憶している。また、制御端末３は、ロボット１が行う作業の工程ごとに目標位置Ｓtと目標力ｆStとを記憶している。目標位置Ｓtと目標力ｆStは例えば教示作業によってロボット１が行う作業の工程ごとに設定される。
【００１６】
制御端末３は、モーターＭ１〜Ｍ６の駆動位置Ｄaを取得すると、対応関係Ｕ１に基づいて、当該駆動位置Ｄaをロボット座標系におけるＴＣＰの位置Ｓ（Ｘ，Ｙ，Ｚ，ＲＸ，ＲＹ，ＲＺ）に変換する。制御端末３は、ＴＣＰの位置Ｓと、力覚センサーＦＳの検出値とに基づいて、力覚センサーＦＳに現実に作用している作用力ｆSをロボット座標系において特定する。制御端末３は、後述するパラメーター導出処理によって導出されたオフセット誤差を補償するように力覚センサーＦＳの検出値を補正する。制御端末３は、ロボット座標系におけるＴＣＰの位置Ｓごとに、力覚センサーＦＳのセンサー座標系における検出軸の方向を規定した対応関係Ｕ２を記憶している。従って、制御端末３は、ロボット座標系におけるＴＣＰの位置Ｓと対応関係Ｕ２とに基づいて、ロボット座標系における作用力ｆSを特定できる。制御端末３は、作用力ｆSに対して重力補償を行う。重力補償とは、作用力ｆSから重力に起因する力やトルクの成分を除去することである。この重力補償において、後述するパラメーター導出処理によって導出されたパラメーターが利用される。また、重力補償を行った作用力ｆSは、エンドエフェクター２に作用している重力以外の力と見なすことができる。
【００１７】
制御端末３は、目標力ｆStと作用力ｆSとをインピーダンス制御の運動方程式に代入することにより、力由来補正量ΔＳを特定する。下記の（１）式は、インピーダンス制御の運動方程式である。
【数１】
2016179523.tif 000003
（１）式の左辺は、ＴＣＰの位置Ｓの２階微分値に仮想慣性係数ｍを乗算した第１項と、ＴＣＰの位置Ｓの微分値に仮想粘性係数ｄを乗算した第２項と、ＴＣＰの位置Ｓに仮想弾性係数ｋを乗算した第３項とによって構成される。（１）式の右辺は、目標力ｆ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を出力することにより、アームＡに第１〜第８姿勢をとらせる。図４に示すように、第１〜第８姿勢のいずれにおいても、第１の駆動軸としての関節Ｊ１〜Ｊ３の回転角は初期角に維持される。すなわち、アームＡが第１〜第８姿勢をとった場合における関節Ｊ１〜Ｊ３に対応するモーターＭ１〜Ｍ３の駆動量はいずれも０となる。第２〜第８姿勢において、第２の駆動軸としての関節Ｊ４〜Ｊ６のうち少なくとも１個の回転角は初期角から６０度変化する。
【００２７】
ここで、関節Ｊ４の回転軸方向が重力方向であるとした場合、関節Ｊ４以外の関節Ｊ１〜Ｊ３，Ｊ５，Ｊ６の回転角が同一の姿勢同士が、重力方向の軸まわりに回転対称となる。関節Ｊ４の回転軸方向が重力方向である場合、第１姿勢と第５姿勢とが重力方向の軸まわりに回転対称となり、第２姿勢と第６姿勢とが重力方向の軸まわりに回転対称となり、第３姿勢と第７姿勢とが重力方向の軸まわりに回転対称となり、第４姿勢と第８姿勢とが重力方向の軸まわりに回転対称となる。従って、関節Ｊ４の回転軸方向が重力方向であるとした場合、第１〜第８姿勢のうち、重力方向の軸まわりに対称とならない姿勢の個数は４個となる。同様に、関節Ｊ５の回転軸方向が重力方向であるとした場合、第１〜第８姿勢のうち、重力方向の軸まわりに対称とならない姿勢の個数は４個となる。さらに、関節Ｊ６の回転軸方向が重力方向であるとした場合、第１〜第８姿勢のうち、重力方向の軸まわりに対称とならない姿勢の個数は４個となる。本実施形態では、ステップＳ１００〜Ｓ１２０によって、鉛直方向に対して関節Ｊ４〜Ｊ６の回転軸方向がずれた方向となっていることが保証されているため、第１〜第８姿勢のうち、重力方向の軸まわりに対称とならない姿勢の個数は閾値としての８個以上となる。
【００２８】
ここで、駆動する関節Ｊ１〜Ｊ６が先端側であるほど、アームＡのうち当該関節Ｊ４〜Ｊ６よりも先端側の部分の長さが小さくなり、当該先端側の部分の円軌道の半径が小さくなる。従って、先端側の関節Ｊ４〜Ｊ６の駆動量が根本側の関節Ｊ１〜Ｊ３の駆動量よりも大きくなるようにモーターＭ１〜Ｍ６を制御することにより、アームＡの軌道円の半径を抑制することができる。すなわち、ステップＳ１３０においてアームＡの可動範囲を抑制することができ、広い空間を確保しなくても済む。なお、ステップＳ１３０においては、エンドエフェクター２に重力のみが作用するようにする必要があり、アームＡが障害物に接触しないことが要求される。
【００２９】
ステップＳ１３０において、導出部４２は、第１〜第８姿勢のそれぞれにおける力覚センサーＦＳの検出結果をロボット１から取得する。すなわち、導出部４２は、力覚センサーＦＳに固有のセンサー座標系において互いに直交する３個の検出軸上の力の大きさと、当該３個の検出軸まわりのトルクの大きさとを取得する。
【００３０】
そして、導出部４２は、第１〜第８姿勢と力覚センサーＦＳの検出結果とに基づいてパラメーターを導出する（ステップＳ１４０）。ここで導出されるパラメーターおよびパラメーターを導出方法の詳細については後述する。最後に、導出部４２は、導出したパラメーターを制御端末３に出力する（ステップＳ１５０）。これにより、導出したパラメーターに基づいて制御端末３はアームＡの制御（例えば重力補償）を行うことが可能となる。
【００３１】
（１−２）パラメーターの導出方法：
以下、図３のステップＳ１４０においてパラメーターを導出する手法について説明する。図５は、エンドエフェクター２の斜視図である。図５に示すように、力覚センサーＦＳの基準位置Ｏから互いに直交する方向（α方向，β方向，γ方向）に延びる３個の検出軸によってセンサー座標系が規定されており、各検出軸の方向における力のベクトルＦ＝［Ｆα，Ｆβ，Ｆγ］と、各検出軸まわりのトルクのベクトルＴ＝［Ｔα，Ｔβ，Ｔγ］とが力覚センサーＦＳの検出結果として取得されている。
【００３２】
まず、センサー座標系におけるエンドエフェクター２および把持しているワークの重心位置ＧＰの位置ベクトルｒを下記の（２）式のように定義する。
【数２】
2016179523.tif 000004
【００３３】
ここで、エンドエフェクター２に重力のみが作用している場合の力の釣り合いを考えると、以下の（３）式が成り立つ。
【数３】
2016179523.tif 000005
また、エンドエフェクター２に重力のみが作用している場合のモーメントの釣り合いを考えると、以下の（４）式が成り立つ。
【数４】
2016179523.tif 000006
【００３４】
前記の（３）（４）式において、姿勢行列Ｒは、ロボット座標系からみた力覚センサーＦＳの姿勢を表す回転行列である。導出部４２は、モーターＭ１〜Ｍ６の駆動位置の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕ１に基づいて、図４に示す各姿勢をとった場合のＴＣＰの位置Ｓを導出する。さらに、導出部４２は、ロボット座標系におけるＴＣＰの位置Ｓごとに、力覚センサーＦＳのセンサー座標系における検出軸の方向を規定した対応関係Ｕ２に基づいて、図４に示す姿勢のそれぞれについて姿勢行列Ｒを導出できる。ベクトルｇは、ロボット座標系における重力加速度のベクトルであり、そのベクトルの大きさの絶対値は重量加速度の大きさを示すスカラーであり、例えば９．８ｍ／ｓ2である。ベクトルＦofsはセンサー座標系における力のオフセット誤差であり、ベクトルＴofsはセンサー座標系におけるトルクのオフセット誤差である。また、行列の右肩のＴは転置行列であることを意味する。ｍｅはエンドエフェクター２および把持しているワークの質量を表す。
【００３５】
図４に示すｉ（ｉ＝１〜７）番目の姿勢において検出された力のベクトルＦから、０（ｉ＝０）番目の第１姿勢において検出された力のベクトルＦを減算した力差ベクトルをＦiと表すこととする。前記の（３）式により、力差ベクトルをＦiは、下記の（５）式のように表すことができる。
【数５】
2016179523.tif 000007
姿勢行列Ｒiはｉ番目の姿勢において、ロボット座標系からみた力覚センサーＦＳの姿勢を表す回転行列である。
【００３６】
ｉ（ｉ＝１〜７）番目の姿勢において検出されたトルクのベクトルＴから、０（ｉ＝０）番目の第１姿勢において検出されたトルクのベクトルＴを減算したトルク差ベクトルをＴiと表すこととする。前記の（４）式により、トルク差ベクトルをＴiは、下記の（６）式のように表すことができる。
【数６】
2016179523.tif 000008
【００３７】
ここで、（５）式の左辺の力差ベクトルＦiを１〜７番目の姿勢についてまとめたベクトルを統合力差ベクトルＦallとし、（５）式の右辺の行列（Ｒi−Ｒ0）Tを１〜７番目の姿勢についてまとめた行列を統合姿勢差行列Ｒallとする。統合力差ベクトルＦallと統合姿勢差行列Ｒallは下記の（７）式で表される。
【数７】
2016179523.tif 000009
【００３８】
統合力差ベクトルＦallと統合姿勢差行列Ｒallを用いると（５）式は、下記の（８）式のように表すことができる。
【数８】
2016179523.tif 000010
さらに、（８）式は下記の（９）式のように変形できる。
【数９】
2016179523.tif 000011
最小二乗法等によって、（９）式に基づいてｇｍｅを求め、当該ｇｍｅのベクトルの大きさを既知の重力加速度の大きさのスカラー量で除算することにより、導出部４２は、エンドエフェクター２の質量ｍｅを導出できる。
【００３９】
また、ｇｍｅをｍｅで除算することにより、導出部４２は、重力加速度の方向を含む重力加速度ベクトルｇを導出できる。
次に、エンドエフェクター２の重心位置ＧＰの導出方法について説明する。まず、各姿勢において力覚センサーの姿勢とエンドエフェクター２の質量ｍｅと重力加速度ベクトルｇにより算出される力をベクトルａで表し、当該算出された力の差のベクトルＦiを算出力差ベクトルａi＝［ａα，ａβ，ａγ］と定義する。これを（５）式に当てはめると、下記の（１０）式のように表すことができる。
【数１０】
2016179523.tif 000012
また、（１０）式の関係を（６）式に代入すると、下記の（１１）式を得ることができる。
【数１１】
2016179523.tif 000013
ただし、Ａｉは下記の（１２）式で表される。また、Ａｉを算出力差行列と表記する。
【数１２】
2016179523.tif 000014
【００４０】
ここで、（６）式の左辺のトルク差ベクトルＴiを１〜７番目の姿勢についてまとめたベクトルを統合トルク差ベクトルＴallとし、（１２）式の算出力差行列Ａｉを１〜７番目の姿勢についてまとめたベクトルを統合算出力差ベクトルＡallとする。統合力差ベクトルＦallと統合算出力差ベクトルＡallは下記の（１３）式で表される。
【数１３】
2016179523.tif 000015
【００４１】
統合トルク差ベクトルＴallと統合算出力差ベクトルＡallと用いると（１１）式は、下記の（１４）式のように表すことができる。
【数１４】
2016179523.tif 000016
さらに、（１４）式は下記の（１５）式のように変形できる。
【数１５】
2016179523.tif 000017
最小二乗法等によって、（１５）式に基づいて、導出部４２は、（２）式に示す重心位置ＧＰの位置ベクトルｒを導出できる。重心位置ＧＰの位置ベクトルｒを導出することは、エンドエフェクター２の重心位置を導出することを意味する。
【００４２】
導出したエンドエフェクター２の質量ｍｅを（３）式に代入して解くことにより、導出部４２は、力のオフセット誤差Ｆofsを導出できる。さらに、導出したエンドエフェクター２の質量ｍｅと重心位置ＧＰの位置ベクトルｒを（４）式に代入して解くことにより、導出部４２は、トルクのオフセット誤差Ｍofsを導出できる。
【００４３】
以上のようにして導出したパラメーターを制御端末３に出力することにより、制御端末３は力覚センサーＦＳの検出値に対してオフセット誤差補償と重力補償とを行うことが可能となる。エンドエフェクター２に作用する重力は、重心位置ＧＰに位置する質量ｍｅの質点に作用する重力と同視できるため、制御端末３は、以上のようにして導出したパラメーターを使用して力覚センサーＦＳの検出値から重力成分（力，トルク）を除去できる。むろん、以上のようにして導出したパラメーターに基づいて、制御端末３の制御における各種パラメーターｆSt，ｋ，ｍ，ｄ，Ｋp，Ｋvが設定されてもよい。
【００４４】
（２）他の実施形態
図３のステップＳ１１０において、関節Ｊ４〜Ｊ６の回転軸方向が適正であると判定しなかった場合、姿勢設定部４１は、制御端末３にアームＡを駆動させるのではなく、ユーザーにアームＡを動かすよう促してもよい。すなわち、重力方向からのずれが所定基準以内の軸まわりに対称とならない姿勢の個数は閾値としての８個未満となる場合おいて、導出部４２は、ユーザーに通知を行ってもよい。例えば、導出部４２は、ユーザーにアームＡを動かすべき旨のメッセージを教示端末４のディスプレイに表示させてもよい。なお、閾値は必ずしも８個でなくてもよく、導出パラメーターの精度に応じて閾値が増減してもよい。
【００４５】
ロボット１は、必ずしも６軸の単腕ロボットでなくてもよく、ロボット１は、双腕ロボットであってもよいし、スカラーロボットであってもよい。また、ロボット１は、少なくとも２軸の駆動軸を備えたロボットであればよい。例えば、２軸のうち、先端側の駆動軸の回転角の範囲をＱ度とするのに対して、もう一方の駆動軸の回転角の範囲をＰ（＜Ｑ）度に制限して、複数の姿勢をとるようにしてもよい。また、力検出器は、力覚センサーＦＳでなくてもよく、関節Ｊ１〜Ｊ６ごとに当該関節Ｊ１〜Ｊ６に作用するトルクを検出するトルクセンサーであってもよい。また、トルクセンサーの代わりにモーターＭ１〜Ｍ６の負荷に基づいてトルクが検出されてもよい。
【符号の説明】
【００４６】
１…ロボット、２…エンドエフェクター、３…制御端末、４…教示端末、４１…姿勢設定部、４２…導出部、Ａ１〜Ａ６…アーム部材、Ｅ１〜Ｅ６…エンコーダー、ＦＳ…力覚センサー、ＧＰ…重心位置、Ｊ１〜Ｊ６…関節、Ｍ１〜Ｍ６…モーター、Ｗ…ワーク、ｄ…仮想粘性係数、ｋ…仮想弾性係数、ｍ…仮想慣性係数
【図１】
2016179523.tif 000018
【図２】
2016179523.tif 000019
【図３】
2016179523.tif 000020
【図４】
2016179523.tif 000021
【図５】
2016179523.tif 000022