(57)【要約】【課題】ロボットの可動部の手先位置を指令通りに、かつ高速で動かすことが可能なロボットの制御装置及び制御方法を提供する。【解決手段】二関節同時駆動機構を備えた可動体の手先の位置を所定の位置に到達させるに際し、関節角度指令値から基端側関節及び手先側関節に付与されるトルクである暫定目標トルクを算出する暫定目標トルク算出手段と、暫定目標トルクから手先力ベクトルを算出するとともに、当該手先力ベクトルの方向を移動軌跡の移動方向に回転させたベクトルである目標手先力ベクトルを算出する目標手先力算出部と、目標手先力ベクトルを当該目標手先力ベクトルの方向とは垂直な方向の成分を有する第１の力ベクトル及び第２の力ベクトルに分配する手先力分配部と、二関節トルク指令値と、基端側単関節トルク指令値及び手先側単関節トルク指令値を算出する目標トルク値算出部とを有するトルク指令値算出手段とを備えた構成とした。【選択図】図１
2015160274.tif 000002
【特許請求の範囲】
【請求項１】
基端側関節により回転可能とされた第１のアームと、前記第１のアームに設けられた手先側関節により回転可能とされた第２のアームとを有する可動体と、
前記基端側関節及び前記手先側関節に対しトルクを個別に付与する基端側単関節アクチュエータ及び手先側単関節アクチュエータと、前記基端側関節及び前記手先側関節に対し同一のトルクを付与する二関節アクチュエータとを有する二関節同時駆動機構と、
を備え、
前記二関節同時駆動機構の駆動制御により前記可動体の手先の位置を所定の位置に到達させるロボットの制御装置であって、
前記手先の移動軌跡である手先位置指令値を出力する目標位置指令手段と、
前記手先位置指令値を関節角度指令値に変換する逆キネマ演算手段と、
前記関節角度指令値から前記基端側関節及び手先側関節に付与されるトルクである暫定目標トルクを算出する暫定目標トルク算出手段と、
前記暫定目標トルクから手先力ベクトルを算出するとともに、当該手先力ベクトルの方向を移動軌跡の移動方向に回転させたベクトルである目標手先力ベクトルを算出する目標手先力算出部と、
前記目標手先力ベクトルを当該目標手先力ベクトルの方向とは垂直な方向の成分を有する第１の力ベクトル及び第２の力ベクトルに分配する手先力分配部と、
前記第１の力ベクトルを発生させるための目標トルク値である二関節トルク指令値と、前記第２の力ベクトルを発生させるための目標トルク値である基端側単関節トルク指令値及び手先側単関節トルク指令値を算出する目標トルク値算出部と、
を有するトルク指令値算出手段と、
前記二関節トルク指令値に基づいて前記二関節アクチュエータを制御するとともに、前記基端側単関節トルク指令値及び手先側単関節トルク指令値に基づいて基端側単関節アクチュエータ及び手先側単関節アクチュエータをそれぞれ制御する駆動制御手段と、
を備えたことを特徴とするロボットの制御装置。
【請求項２】
前記二関節アクチュエータは、前記基端側関節及び前記手先側関節に対してそれぞれ設けられるモータを有し、
駆動制御手段は、各モータを前記二関節トルク指令値に基づいて制御し、前記基端側関節及び前記手先側関節に対して同一のトルクを付与することを特徴とする請求項１記載のロボットの制御装置。
【請求項３】
基端側関節により回転可能とされた第１のアームと、前記第１のアームに設けられた手先側関節により回転可能とされた第２のアームとを有する可動体と、
前記基端側関節及び前記手先側関節に対し、トルクを個別に付与する基端側単関節アクチュエータ及び手先側単関節アクチュエータと、前記基端側関節及び前記手先側関節に対し、同一のトルクを付与する二関節アクチュエータとを有する二関節同時駆動機構と、
を備え、
前記二関節同時駆動機構の駆動制御により前記可動体の手先の位置を所定の位置に到達させるロボットの制御方法であって、
前記可動体の手先の移動軌跡から前記基端側関節及び手先側関節に付与されるトルクである暫定目標トルクを算出し、
前記暫定目標トルクから算出される手先力ベクトルの方向を移動軌跡の移動方向に回転させたベクトルである目標手先力ベクトルを算出し、
前記目標手先力ベクトルを当該目標手先力ベクトルの方向とは垂直な方向の成分を有する第１の力ベクトルと第２の力ベクトルとに分配し、
前記第１の力ベクトルを発生させるための目標トルク値である二関節トルク指令値と、前記第２の力ベクトルを発生させるための目標トルク値である基端側単関節トルク指令値及び手先側単関節トルク指令値を算出し、
前記二関節トルク指令値に基づいて前記二関節アクチュエータを制御するとともに、前記基端側単関節トルク指令値及び手先側単関節トルク指令値に基づいて基端側単関節アクチュエータ及び手先側単関節アクチュエータをそれぞれ制御することを特徴とするロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の関節を有するロボットの手先位置と手先力とを制御する制御装置及び制御方法に関する。
【背景技術】
【０００２】
従来、ロボットの一例として、ロボットアームの制御においては、手先位置を所望の時刻に目標手先位置に一致させるように関節を回転させる制御が行われている。しかし、手先には、機械系の慣性モーメントやコリオリ力などの外力が作用するため、手先位置方向と手先力方向とが一致せず、結果として位置誤差が大きくなるという問題があった。
このような問題を解決するため、特許文献１には、ロボットアームの先端に位置センサ及び力センサを設け、これらのセンサで検出された手先位置と目標手先位置との誤差と、手先力と目標手先力との誤差とをそれぞれフィードバックすることで、手先位置制御と手先力制御とを行う、いわゆるハイブリット制御が開示されている。
【０００３】
しかし、手先位置誤差及び手先力誤差をそれぞれフィードバックすると、一方の誤差の修正により他方の誤差が大きくなるというように、両方の誤差を共にゼロとすることが困難であった。また、両誤差を小さくするためには、指令した位置に手先を移動させる際の移動速度を遅くする必要があった。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平１１−４２５７３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
本発明は、従来の問題点に鑑みてなされたもので、ロボットの可動部の手先位置を指令通りに、かつ高速で動かすことが可能なロボットの制御装置及び制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するためのロボットの制御装置の構成として、基端側関節により回転可能とされた第１のアームと、第１のアームに設けられた手先側関節により回転可能とされた第２のアームとを有する可動体と、基端側関節及び手先側関節に対しトルクを個別に付与する基端側単関節アクチュエータ及び手先側単関節アクチュエータと、基端側関節及び手先側関節に対し同一のトルクを付与する二関節アクチュエータとを有する二関節同時駆動機構とを備え、二関節同時駆動機構の駆動制御により可動体の手先の位置を所定の位置に到達させるロボットの制御装置であって、手先の移動軌跡である手先位置指令値を出力する目標位置指令手段と、手先位置指令値を関節角度指令値に変換する逆キネマ演算手段と、関節角度指令値から基端側関節及び手先側関節に付与されるトルクである暫定目標トルクを算出する暫定目標トルク算出手段と、暫定目標トルクから手先力ベクトルを算出するとともに、当該手先力ベクトルの方向を移動軌跡の移動方向に回転させたベクトルである目標手先力ベクトルを算出する目標手先力算出部と、目標手先力ベクトルを当該目標手先力ベクトルの方向とは垂直な方向の成分を有する第１の力ベクトル及び第２の力ベクトルに分配する手先力分配部と、第１の力ベクトルを発生させるための目標トルク値である二関節トルク指令値と、第２の力ベクトルを発生させるための目標トルク値である基端側単関節トルク指令値及び手先側単関節トルク指令値を算出する目標トルク値算出部とを有するトルク指令値算出手段と、二関節トルク指令値に基づいて二関節アクチュエータを制御するとともに、基端側単関節トルク指令値及び手先側単関節トルク指令値に基づいて基端側単関節アクチュエータ及び手先側単関節アクチュエータをそれぞれ制御する駆動制御手段とを備えた構成とした。
本構成に係るロボットの制御装置によれば、位置センサ及び力センサを用いることなく、基端側関節及び手先側関節に対して、手先力が手先軌跡の方向を向くような目標トルクを付与することができ、可動体の手先を指令通りの所定の位置に正確に到達させることができる。また、可動体に各種のセンサが不要となるため、外部の物体との接触による破損等を防止でき、ロボットとしての信頼性を向上させることができる。更に、従来のフィードバック制御と比較して、フィードフォワード制御となるため、可動体の手先位置を高速で動かすことができる。
【０００７】
また、二関節アクチュエータの構成に関し、二関節アクチュエータが基端側関節及び手先側関節に対してそれぞれ設けられるモータを有し、駆動制御手段が各モータを二関節トルク指令値に基づいて制御し、基端側関節及び手先側関節に対して同一のトルクを付与する構成とした。
本構成により、基端側関節及び手先側関節に対して同一のトルクを確実に付与することができ、可動体の手先位置をより正確に動かすことができる。
【０００８】
また、上記ロボットの構成を前提とするロボットの制御方法の態様として、可動体の手先の移動軌跡から基端側関節及び手先側関節に付与されるトルクである暫定目標トルクを算出し、暫定目標トルクから算出される手先力ベクトルの方向を移動軌跡の移動方向に回転させたベクトルである目標手先力ベクトルを算出し、目標手先力ベクトルを当該目標手先力ベクトルの方向とは垂直な方向の成分を有する第１の力ベクトルと第２の力ベクトルとに分配し、第１の力ベクトルを発生させるための目標トルク値である二関節トルク指令値と、第２の力ベクトルを発生させるための目標トルク値である基端側単関節トルク指令値及び手先側単関節トルク指令値を算出し、二関節トルク指令値に基づいて二関節アクチュエータを制御するとともに、基端側単関節トルク指令値及び手先側単関節トルク指令値に基づいて基端側単関節アクチュエータ及び手先側単関節アクチュエータをそれぞれ制御する態様とした。
本態様に係る制御方法によれば、上記制御装置の構成から生じる効果と同様の効果を得ることができる。
【図面の簡単な説明】
【０００９】
【図１】ロボット制御システムの全体構成を示す図である。
【図２】ロボットアームモデル、及び二関節同時駆動機構の一例を示す図である。
【図３】ロボットアームの制御ブロック図である。
【図４】目標手先力ベクトルの設定を説明するための模式図である。
【図５】二関節手先力ベクトルの設定を説明するための模式図である。
【図６】単関節手先力ベクトルの設定を説明するための模式図である。
【図７】目標トルクの設定を説明するための模式図である。
【発明を実施するための形態】
【００１０】
図１は、ロボット制御システム１の全体構成を示す図である。
ロボット制御システム１は、二関節同時駆動機構２を備えた可動体としてのロボットアーム３と、ロボットアーム３の関節角度を制御する可動体制御装置４とを備える。
ロボットアーム３は、図外のロボットの胴部や作業平面上に固定される基体３１と、肩関節３２を介して基体３１と連結され、基体３１に対して回転可能に取付けられる第１のアームとしての基体側腕部３３とを備える。また、ロボットアーム３は、肘関節３４を介して基体側腕部３３と連結され、基体側腕部３３に対して回転可能に取り付けられる第２のアームとしての先端側腕部３５を備える。
【００１１】
このように、ロボットアーム３の基体側腕部３３及び先端側腕部３５は、基体側関節としての肩関節３２、及び手先側関節としての肘関節３４の回転動作により自在に回転可能である。そして、ロボットアーム３に組み付けられた二関節同時駆動機構２が可動体制御装置４により駆動制御されることにより、先端側腕部３５の先端側の手先３６の位置を自在に変位させる。また、図１に示すように、手先３６には、ロボットアーム３の用途に応じて、物品を把持する把持部や、その他の治具等の手先機構３７が固定的、或いは、手首関節を介して回転可能に取り付けられる。
【００１２】
詳細については後述するが、二関節同時駆動機構２は、肩関節３２及び肘関節３４に対して回転トルクを個別に付与する単関節アクチュエータと、肩関節３２及び肘関節３４に対して同一の回転トルクを同時に付与する二関節アクチュエータとを備える。
図２（ａ）に示すように、肩関節３２及び肘関節３４に対して個別に付与される回転トルクをそれぞれτmono_sh，τmono_elとし、肩関節３２及び肘関節３４に対して同時に付与される回転トルクをτbi_pとすると、以下の式［数１］に示すように、肩関節３２に付与される回転トルクτp_sh、は、τmono_shとτbi_pとの和になり、肘関節３４に付与される回転トルクτp_elはτmono_elとτbi_pとの和になる。
【数１】
2015160274.tif 000003
また、［数１］で示すように、手先３６に発生させたい力（手先力）をＦpとすると、そのときに必要な回転トルクτpは、ヤコビ行列Ｊの転置行列ＪTと手先力Ｆpとの積から算出される。本例では、手先力Ｆpの方向が、現在の手先位置指令値Ｐref（ｔ）の位置から次の手先位置指令値Ｐref（ｔ＋１）の位置へ向かう方向である手先軌跡の方向となるようなトルク指令値τp_refを求め、このトルク指令値τp_refを回転トルクτpの目標値に設定する。
【００１３】
図２（ｂ）は、二関節同時駆動機構２の一例を示す図である。二関節同時駆動機構２は、肩関節３２の回転中心となる回転軸３２Ｊを回転させるモータ（肩側モータ２２，肩側二関節用モータ２４）を備える。また、二関節同時駆動機構２は、肩関節３２とは反対側の端部に設けられた肘関節３４の回転軸３４Ｊを回転駆動するモータ（肘側モータ２３，肘側二関節用モータ２５）を備える。
【００１４】
複数のモータのうち、肩側モータ２２及び肘側モータ２３は、それぞれ肩関節３２及び肘関節３４に対して回転トルクを個別に付与する基端側単関節アクチュエータ及び手先側単関節アクチュエータである。これに対して、肩側二関節用モータ２４及び肘側二関節用モータ２５は、肩関節３２及び肘関節３４に対して同一の回転トルクを同時に付与する二関節アクチュエータである。
【００１５】
肩側モータ２２及び肩側二関節用モータ２４は、固定板２１に対して離間して設けられた基台側取付板２６上に固定的に搭載される。肩側モータ２２の出力軸２２ａは、軸受けを介して基台側取付板２６を貫通する。出力軸２２ａの先端部には、歯車２２ｂが取り付けられる。同様に肩側二関節用モータ２４の出力軸２４ａは、軸受けを介して基台側取付板２６を貫通する。出力軸２４ａの先端部には、歯車２４ｂが取り付けられる。歯車２２ｂ及び歯車２４ｂは、ともに肩関節３２の回転軸３２Ｊの中間部に取り付けられた歯車２８と噛み合う。
【００１６】
回転軸３２Ｊは、基台側取付板２６及び基体側腕部３３間に渡って延長する。回転軸３２Ｊの基台側取付板２６側の端部は、基台側取付板２６に設けられた軸受けＲ１を介して回転自在に取り付けられる。また、回転軸３２Ｊの基体側腕部３３側の端部は、固定板２１に設けられた軸受けＲ３を介して基体側腕部３３と固定される。
【００１７】
つまり、肩関節３２の回転軸３２Ｊには、基端側単関節アクチュエータとしての肩側モータ２２と、二関節アクチュエータの一方を構成する肩側二関節用モータ２４とからの回転トルクが付与される。そして、肩側モータ２２及び肩側二関節用モータ２４の正逆方向への回転駆動により、固定板２１に対して肩関節３２を介して連結された基体側腕部３３が自在に回転動作する。
【００１８】
肘側モータ２３及び肘側二関節用モータ２５は、基体側腕部３３に対して離間して設けられた先端側取付板２７上に固定的に搭載される。肘側モータ２３の出力軸２３ａは、軸受けを介して先端側取付板２７を貫通する。出力軸２３ａの先端部には、歯車２３ｂが取り付けられる。同様に、肘側二関節用モータ２５の出力軸２５ａは、軸受けを介して先端側取付板２７を貫通する。出力軸２５ａの先端部には、歯車２５ｂが取り付けられる。歯車２３ｂ及び歯車２５ｂは、ともに肘関節３４の回転軸３４Ｊの中間部に取り付けられた歯車２９と噛み合う。
【００１９】
回転軸３４Ｊは、先端側取付板２７及び先端側腕部３５間に渡って延長する。回転軸３４Ｊの先端側取付板２７側の端部は、先端側取付板２７に設けられた軸受けＲ２を介して回転自在に取り付けられる。また、回転軸３４Ｊの先端側腕部３５側の端部は、軸受けＲ４を介して先端側腕部３５と固定される。
【００２０】
つまり、肘関節３４の回転軸３４Ｊには、先端側単関節アクチュエータとしての肘側モータ２３と、二関節アクチュエータの他方を構成する肘側二関節用モータ２５とからの回転トルクが付与される。そして、肘側モータ２３及び肘側二関節用モータ２５の正逆方向への回転駆動により、基体側腕部３３に対して肘関節３４を介して連結された先端側腕部３５が自在に回転動作する。以下、上記構成からなるロボットアーム３の駆動制御について説明する。
【００２１】
図１に示すように、可動体制御装置４は、目標位置指令手段４１、逆キネマ演算手段４２、角速度算出手段４３、角加速度算出手段４４、暫定目標トルク算出手段４５、トルク指令値算出手段４６、及び駆動制御手段４７とを備える。可動体制御装置４は、図外のＣＰＵ，ＲＯＭ，ＲＡＭ等の各種のハードウェアを含む装置であって、ＣＰＵがＲＯＭに記憶されたプログラムに従って演算処理を行うことにより、目標位置指令手段４１、逆キネマ演算手段４２、角速度算出手段４３、角加速度算出手段４４、暫定目標トルク算出手段４５、トルク指令値算出手段４６として機能する。また、駆動制御手段４７は、モータ駆動回路から構成される。以下、可動体制御装置４が備える各手段の概要について説明する。
【００２２】
目標位置指令手段４１は、予め設定されたロボットアーム３の手先３６の移動軌跡である手先位置指令値Ｐref（t）＝（ｘref，ｙref）を逆キネマ演算手段４２に出力する。逆キネマ演算手段４２は、手先位置指令値Ｐref（t）を関節角度指令値θrefに変換する。上述のとおり、ロボットアーム３は、肩関節３２及び肘関節３４を有する２リンクアームであるため、図２（ａ）に示すようにｘ軸と基体側腕部３３とのなす角θ1に対する関節角度指令値である肩関節角度指令値をθ1_ref、基体側腕部３３と先端側腕部３５とのなす角θ2に対する関節角度指令である肘関節角度指令値をθ2_refとすると、θref＝（θ1_ref，θ2_ref）となる。
【００２３】
角速度算出手段４３は、関節角度指令値θrefの微分値である角速度指令値ｄθref/ｄｔを算出する。また、角加速度算出手段４４は、角速度指令値の微分値である角加速度指令値ｄ2θref/ｄｔ2を算出する。
【００２４】
暫定目標トルク算出手段４５は、算出された関節角度指令値θref、角速度指令値ｄθref/ｄｔ及び角加速度指令値ｄ2θref/ｄｔ2から肩関節３２及び肘関節３４に付与される回転トルクである暫定目標トルクτ0_ref＝（τ0_sh_ref，τ0_el_ref）を算出する。ここで、τ0_sh_refは、肩関節３２に付与される暫定目標トルクを表し、τ0_el_refは肘関節３４に付与される暫定目標トルクを表す。
【００２５】
トルク指令値算出手段４６は、目標手先力算出部４６ａ、手先力分配部４６ｂ、及び目標トルク算出部４６ｃを備える。目標手先力算出部４６ａは、暫定目標トルクτ0_refから手先力ベクトルＦ0_refを算出するとともに、この手先力ベクトルＦ0_refの方向を手先軌跡の移動方向に回転させたベクトルＦp_refを算出し、このベクトルＦp_refを目標手先力ベクトルとして出力する。
【００２６】
手先力分配部４６ｂは、目標手先力算出部４６ａにより算出された目標手先力ベクトルＦp_refを、肩関節３２及び肘関節３４に回転トルクτbi_pを同時に付与したときに発生する二関節手先力ベクトルＦbi_p_refと、肩関節３２及び肘関節３４に回転トルクτp_mono_sh及びτp_mono_elを個別に付与したときに発生する単関節手先力ベクトルＦmono_p_refとに分配する。
【００２７】
目標トルク算出部４６ｃは、二関節手先力ベクトルＦbi_p_refと単関節手先力ベクトルＦmono_p_refとから、肩関節３２及び肘関節３４に付与する目標トルクであるトルク指令値τp_ref＝（τp_sh_ref，τp_el_ref）を算出する。ここで、τp_sh_refは肩関節３２の回転軸３２Ｊを回転駆動するための肩関節トルク指令値を表し、τp_el_refは肘関節３４の回転軸３４Ｊを回転駆動するための肘関節トルク指令値を表す。
【００２８】
駆動制御手段４７は、トルク指令値τp_ref＝（τp_sh_ref，τp_el_ref）に基づいて、二関節同時駆動機構２の肩側モータ２２、肩側二関節用モータ２４、肘側モータ２３、及び肘側二関節用モータ２５を制御し、肩関節３２及び肘関節３４を同時に回転動作させる。
【００２９】
次に、ロボット制御システム１の動作について、図３の制御ブロック図を参照して説明する。ロボットアーム３は、基体側腕部３３の長さＬshと先端側腕部３５の長さＬelがそれぞれ一定であるため、手先３６の位置が肩関節３２及び肘関節３４の回転角度を決定する。換言すれば、手先３６が移動すれば、それに従って肩関節３２及び肘関節３４がそれぞれ回転する。
【００３０】
本実施形態では、逆キネマ演算手段４２が目標位置指令手段４１から出力される手先位置指令値Ｐref（t）を関節角度指令値θrefに変換し、関節角度指令値θrefに基づいて肩関節３２及び肘関節３４を回転駆動するためのトルク指令値τp_sh_ref，τp_el_refを算出し、これらトルク指令値τp_sh_ref，τp_el_refに基づいて肩関節３２及び肘関節３４を回転動作させてロボットアーム３の手先３６の位置を移動させる。
ここで、肩関節３２とｘ軸とのなす角度である肩関節角度指令値をθ1_ref、肘関節３４と肩関節３２の延長方向とのなす角である肘関節角度指令値をθ2_refとすると、手先位置指令値Ｐref（t）と各関節角度の指令値θ1_ref，θ2_refとの関係は以下の式［数２］のようになる。
【数２】
2015160274.tif 000004
【００３１】
次に、関節角度指令値θrefから角速度指令値ｄθref/ｄｔを算出し、角速度指令値ｄθref/ｄｔから角加速度指令値ｄ2θref/ｄｔ2を算出する。そして、以下の式［数３］に示すラグランジュの運動方程式を用いて、暫定目標トルクτ0_ref＝（τ0_sh_ref，τ0_el_ref）を算出する。
【数３】
2015160274.tif 000005
なお、τ0_sh_refは肩関節３２に付与される暫定肩関節目標トルク、τ0_el_refは肘関節３４に付与される暫定肘関節目標トルクを表す。また、Ｍは慣性モーメント行列で、ｈはコリオリ力である。なお、［数３］の右辺に重力項ｇ（θref）を加えてもよい。
本発明では、手先力Ｆpが手先軌跡の方向を向くように目標トルクτp_refを設定するので、式［数３］で求められる目標トルクτ0_refを「暫定目標トルク」と呼ぶ。
【００３２】
肩側モータ２２を暫定肩関節目標トルクτ0_sh_refにより駆動して肩関節３２を回転させ、肘側モータ２３を暫定肘関節目標トルクτ0_el_refにより駆動して肘関節３４を回転させると、手先３６には以下の式［数４］に示す手先力Ｆ0_ref＝（ｆ0x_ref，ｆ0y_ref）が発生する。ここで、Ｊは、ヤコビ行列である。また、Ｆ0_refはベクトルであるので、以下、Ｆ0_refを手先力ベクトルという。
【数４】
2015160274.tif 000006
図４に示すように、Ｐref（ｔk）を時刻t＝ｔkにおける目標手先位置、Ｐref（ｔk+1）を時刻t＝ｔk+1における目標手先位置とすると、手先力ベクトルＦ0_refの方向と、手先軌跡の方向（同図のＸ軸方向；Ｐref（ｔk）からＰref（ｔk+1）に向かう方向）とは必ずしも一致しない。そして、手先力ベクトルＦ0_refの方向と手先軌跡の方向とが一致しない場合には、時刻t＝ｔk+1における手先位置Ｐ（ｔk+1）が目標手先位置Ｐref（ｔk+1）とずれてしまい、位置誤差が生じてしまう。本例では、肩関節３２と肘関節３４とが回転動作したときの手先力ベクトルＦpの方向と手先軌跡の方向とが一致するようなトルク指令値τp_ref＝（τp_sh_ref，τp_el_ref）を求め、このトルク指令値τp_refを用いて二関節同時駆動機構２の各モータ２２〜２５を制御することで、位置誤差を低減する。
【００３３】
ここで、手先軌跡の方向を向く手先力Ｆpの目標値である目標手先力ベクトルをＦp_refとする。目標手先力ベクトルＦp_refの大きさは手先力ベクトルＦ0_refと同じである。したがって、暫定肘関節目標トルクτ0_el_refを用いて求められた手先力ベクトルＦ0_refの方向と手先軌跡の方向とのなす角度、すなわち、手先力ベクトルＦ0_refの方向と目標手先力ベクトルＦp_refの方向とのなす角度をδとすると、上記Ｆ0_refとＦp_refとは、以下の式［数５］に示す関係にある。
【数５】
2015160274.tif 000007
すなわち、目標手先力ベクトルＦp_refは、手先力ベクトルＦ0_refを角度δだけ回転させて、その向きを手先軌跡の方向と一致させたものである。
【００３４】
次に、目標手先力ベクトルＦp_refを、回転トルクτbi_pを肩関節３２及び肘関節３４に同時に付与したときに発生する手先力の目標値である二関節手先力ベクトルＦbi_p_refと、回転トルクτmono_p_sh，τmono_p_elを肩関節３２及び肘関節３４に対して個別に付与したときに発生する手先力の目標値である単関節手先力ベクトルＦmono_p_refとに分配する方法について説明する。なお、τbi_pは、肩関節３２及び肘関節３４に対して同時に付与される回転トルクで、τmono_sh，τmono_elは、肩関節３２及び肘関節３４に対して個別に付与される回転トルクである。
【００３５】
図５に示すように、手先軸方向をＸ軸とする座標系をΣtipとし、座標系ΣtipのＸ軸と元の座標系Σ0のｘ軸とのなす角度をφとすると、座標系Σtipにおける目標手先力ベクトルＦtip_refと、元の座標系Σ0での目標手先力ベクトルＦp_refとは以下に示す関係にある。
【数６】
2015160274.tif 000008
ここで、ｆpx_ref，ｆpy_refは、それぞれ、元の座標系Σ0での目標手先力ベクトルＦp_refのｘ成分とｙ成分である。
【００３６】
次に、座標系Σtipにおける目標手先力ベクトルＦtip_refを、肩関節３２及び肘関節３４に対して同時に付与される回転トルクτbi_pにより発生する手先力である二関節手先力ベクトルＦbi_tip_refと、肩関節３２及び肘関節３４に対して個別に付与される回転トルクτmono_sh，τmono_elにより発生する手先力である単関節手先力ベクトルＦmono_tip_refとに分配する。二関節手先力ベクトルＦbi_tip_refが座標系Σtipにおける本発明の第１の力ベクトルに相当し、単関節手先力ベクトルＦmono_tip_refが本発明の第２の力ベクトルに相当する。
【００３７】
具体的には、図５に示すように座標系ΣtipにおいてＸ軸に対して角度（θ1−φ）だけ回転させた方向を向く力ベクトルを二関節手先力ベクトルＦbi_tip_refとし、図６に示すように、Ｘ軸に対してθ1とは反対方向に角度φmonoだけ回転させた方向を向く力ベクトルを単関節手先力ベクトルＦmono_tip_refとする。
ここで、二関節目標手先力ベクトルＦbi_tip_refの大きさ‖Ｆbi_tip_ref‖を以下の式［数７］のように設定する。なお、αはユーザーが設定できる定数である。また、角度φmonoと単関節手先力ベクトルＦmono_tip_refの大きさ‖Ｆmono_tip_ref‖は、以下の式［数８］のようになる。角度φmonoは、二関節手先力ベクトルＦbi_tip_refと単関節手先力ベクトルＦmono_tip_refとのベクトル和が目標手先力ベクトルＦtip_refとなる、という条件から導かれる。
【数７】
2015160274.tif 000009
【数８】
2015160274.tif 000010
【００３８】
目標手先力ベクトルＦtip_refは、二関節手先力ベクトルＦbi_tip_refと単関節手先力ベクトルＦmono_tip_refとのベクトル和であるので、‖Ｆbi_tip_ref‖と‖Ｆmono_tip_ref‖とは、以下の式［数９］に示すような関係にある。
【数９】
2015160274.tif 000011
【００３９】
すなわち、図６のＰref（tk）で示す手先３６のＹ軸方向（目標手先力ベクトルＦtip_refの方向と垂直な方向）には、二関節手先力ベクトルＦbi_tip_refのＹ成分であるｆbi_tipY_refと、ｆbi_tipY_refと大きさが同じで、かつ、方向が逆である単関節手先力ベクトルＦmono_tip_refのＹ成分であるｆmono_tipY_refとが作用する。これらの力ｆbi_tipY_ref，ｆmono_tipY_refは、目標手先力ベクトルＦp_refの方向が手先軌跡からそれないように作用する。すなわち、手先力方向に垂直な方向には剛性が付与されるため、手先３６をＰref（tk）からＰref（tk+1）に精度良く移動させることが可能となる。
【００４０】
次に、二関節手先力ベクトルＦbi_tip_refと単関節手先力ベクトルＦmono_tip_refとを、以下の式［数１０］を用いて、手先軌跡方向をＸ軸とする座標系Σtipから元の座標系Σ0の手先力ベクトルに戻す。［数１０］において、Ｆmono_p_refは座標系Σ0の単関節手先力ベクトル、Ｆbi_p_refは座標系Σ0の二関節手先力ベクトル、角度φは、Ｘ軸と元の座標系Σ0のｘ軸とのなす角度である。
【数１０】
2015160274.tif 000012
手先力ベクトルＦpと、当該手先力ベクトルＦpを発生させる回転とトルクτpとの関係は、上述の式［数１］に記載された通りである。したがって、下記の式［数１１］に示すように、単関節手先力ベクトルＦmono_p_refにヤコビ行列Ｊの転置行列ＪTを積算することで、単関節トルク指令値τmono_p_refを得ることができる。また、二関節手先力ベクトルＦbi_p_refにヤコビ行列Ｊの転置行列ＪTを積算することで、二関節トルク指令値τbi_p_refを得ることができる。
【数１１】
2015160274.tif 000013
また、肩関節３２を駆動するための回転トルクの目標値τp_sh_refと肘関節３４とを駆動するための回転トルクの目標値τp_el_refとは、以下の式［数１２］で表せる。
【数１２】
2015160274.tif 000014
すなわち、肩関節３２に付与すべき回転トルクの目標値τp_sh_refは、肩関節３２に対して個別に付与すべき回転トルクの目標値τmono_sh_refと肩関節３２及び肘関節３４に対して同時に付与すべき回転トルクの目標値τbi_p_refとの和となる。また、肘関節３４に付与すべき回転トルクの目標値τp_el_refは、肘関節３４に対して個別に付与すべき回転トルクτmono_el_refと上記回転トルクの目標値τbi_p_refとの和になる。
従って、駆動制御手段４７に対して肩関節トルク指令値τp_sh_ref（＝τmono_sh_ref＋τbi_p_ref）及び肘関節トルク指令値τp_el_ref（＝τmono_el_ref＋τbi_p_ref）を出力し、駆動制御手段４７が上記各指令値に基づいて、肩関節３２に回転トルクτp_shが付与され、肘関節３４に回転トルクτp_elが付与されるように、二関節同時駆動機構２の各モータ２２〜２５を駆動制御すれば、手先位置を精度よく移動させることが可能となる。
【００４１】
以上説明したとおり、本実施形態に係る可動体制御装置４によれば、手先位置指令値Ｐref（t）から算出される暫定目標トルクτ0_refにより手先３６に発生する手先力ベクトルＦ0_refの方向を手先軌跡の方向を向くように回転させた目標手先力ベクトルＦp_refを算出し、目標手先力ベクトルＦp_refを発生させるための回転トルクを算出してこれを肩関節３２及び肘関節３４に付与する目標トルクτp_sh_ref，τp_el_refとしたので、位置センサ及び力センサを用いることなく、ロボットアーム３の手先３６を指令通りの所定の位置に正確に到達させることができる。
また、目標手先力ベクトルＦp_refを、Ｆp_refの方向とは垂直な方向の成分を有する二関節手先力ベクトルＦbi_p_refと単関節手先力ベクトルＦmono_p_refとに分配し、二関節手先力ベクトルＦbi_p_refを発生させるための二関節トルク指令値τbi_p_ref及び単関節手先力ベクトルＦmono_p_refを発生させるための単関節トルク指令値τmono_p_refを算出し、二関節トルク指令値τbi_p_refに基づいて二関節アクチュエータ（肩側二関節用モータ２４及び肘側二関節用モータ２５）を制御し、単関節トルク指令値τmono_p_refに基づいて単関節アクチュエータ（肩側モータ２２及び肘側モータ２３）制御するようにしたので、手先力方向に垂直な方向に剛性を付与することができる。したがって、手先力Ｆpの方向を確実に手先軌跡の方向に向けることができる。
また、可動体制御装置４は、フィードフォワード制御を行っているため、従来のハイブリット制御に採用されるフィードバック制御に比較して、手先位置を高速で動かすことができる。
【００４２】
上述の実施形態では、可動体制御装置４に駆動制御される可動部として、人体の腕部を模したロボットアーム３を例としたが、制御対象はこれに限られるものではない。例えば、人体の脚部を模したロボットレッグ等をも対象とできる。この場合、上述の基端側関節としての肩関節３２は股関節として適用でき、先端側関節としての肘関節３４は膝関節として適用できる。またこの場合、手先３６は、ロボットレッグの足先として概念することができる。また、可動部の態様としては、三関節以上を有するものであってもよい。
【００４３】
上述の実施形態では、手先３６がｘｙ平面内で移動する２自由度のロボットアーム３を例として説明したが、ロボットアーム３としては、これに限られるものではない。例えば、ｚ軸方向にも移動する３自由度のロボットアームや、ローリング、ピッチング、ヨーイング等のｘ軸、ｙ軸、ｚ軸周りの回転移動を行う４自由度以上の自由度を持つロボットアームにも適用可能である。
【００４４】
また、上述の実施形態では、二関節手先力ベクトルＦbi_p_refの方向をｘ軸に対して角度θ1だけ回転させた方向としたが、移動軌跡の方向（目標手先力ベクトルＦp_refの方向）と交差する方向であれば、他の方向であってもよい。
【００４５】
また、上述の実施形態では、肩側二関節用モータ２４及び肘側二関節用モータ２５により、肩関節３２及び肘関節３４に対して同一の回転トルクを付与する二関節アクチュエータを構成したが、これに限られるものではない。例えば、二関節アクチュエータを単一のモータにより構成し、当該モータの出力をギアや無端ベルトで分配することにより肩関節３２及び肘関節３４に対して同一の回転トルクを付与する構成としてもよい。このように、二関節アクチュエータを１個のモータにより構成すれば、制御が容易になるだけでなく、コストも低減できる。
【符号の説明】
【００４６】
１  ロボット制御システム，２  二関節同時駆動機構，２２  肩側モータ，
２３  肘側モータ，２４  肩側二関節用モータ，２５  肘側二関節用モータ，
３  ロボットアーム，３１  基体，３２  肩関節，３２Ｊ  回転軸，
３３  基体側腕部，３４  肘関節，３４Ｊ  回転軸，３５  先端側腕部，３６  手先，
４  可動体制御装置，４１  目標位置指令手段，４２  逆キネマ演算手段，
４３  角速度算出手段，４４  角加速度算出手段，４５  暫定目標トルク算出手段，
４６  トルク指令値算出手段，４６ａ  目標手先力算出部，４６ｂ  手先力分配部，
４６ｃ  目標トルク算出部，４７  駆動制御手段，Ｒ１〜Ｒ４  軸受け
【図１】
2015160274.tif 000015
【図２】
2015160274.tif 000016
【図３】
2015160274.tif 000017
【図４】
2015160274.tif 000018
【図５】
2015160274.tif 000019
【図６】
2015160274.tif 000020
【図７】
2015160274.tif 000021