(57)【要約】【課題】関節の目標角加速度に対する実角加速度の追従性を向上させる駆動装置を提供する。【解決手段】駆動装置１０は、複数の関節１６を有するリンク２０と、複数の関節１６を駆動する複数のモータ３２とを有する駆動部１２と、駆動部１２を駆動制御する制御部１４とを備え、制御部１４は、目標角加速度ｄｄθ＿ｃｎｔｒｌ、前回の目標角加速度ｄｄθ＿ｃｎｔｒｌ、変位部トルク応答性行列Ｅｆｆ、慣性行列Ｍ、力学補正力Ｃ＿ｃｍｐｎから、数１１の関係式を用いて算出した制御トルクｍ＿τ＿ｃｎｔｒｌで複数のモータ３２を制御する。【選択図】図５
2014046425.tif 000002
【特許請求の範囲】
【請求項１】
複数の変位部を有するリンクと、前記複数の変位部を駆動する複数の回転駆動源とを有する駆動部と、
前記駆動部を駆動制御する制御部と、
を備える駆動装置において、
前記制御部は、
前記複数の変位部のトルクの応答性を示す変位部トルク応答性行列、前記複数の変位部の角度によって決定された前記駆動部の慣性行列、前記複数の変位部の今回の目標角加速度、及び前記複数の変位部の前回の目標角加速度を用いて、前記複数の変位部の実角加速度が、サンプリング時間後に、前記今回の目標角加速度となるように、前記複数の変位部の制御トルクを算出する制御トルク算出部を備える
ことを特徴とする駆動装置。
【請求項２】
請求項１に記載の駆動装置において、
前記変位部は、回転駆動する関節又は進退駆動する進退部である
ことを特徴とする駆動装置。
【請求項３】
請求項１又は２に記載の駆動装置において、
前記制御部は、下記の関係式を用いて、前記制御トルクを算出する
ことを特徴とする駆動装置。
【数１】
2014046425.tif 000036
【請求項４】
請求項３に記載の駆動装置において、
前記複数の関節の最小時定数を測定しておき、
前記最小時定数を用いて、変位部トルク応答性行列の対角要素である変位部トルク応答係数を設定する
ことを特徴とする駆動装置。
【請求項５】
請求項１〜４の何れか１項に記載の駆動装置において、
前記制御トルク算出部は、
前記リンクの予め定められたタスク位置の今回の目標変化加速度及び前回の目標変化加速度と、前記タスク位置の変化速度と前記複数の変位部の角速度との関係を示す係数行列とを用いて、前記複数の変位部の今回の目標角加速度、及び前記複数の変位部の前回の目標角加速度を算出することで、前記タスク位置の実変化加速度が、サンプリング時間後に、前記今回の目標変化加速度となるように、前記複数の変位部の制御トルクを算出する
ことを特徴とする駆動装置。
【請求項６】
請求項１〜４の何れか１項に記載の駆動装置において、
駆動部は、前記リンクに支持されるベース部を備え、
前記制御トルク算出部は、
前記リンクの先端であるタスク位置が地面から受ける力の応答性を示す力応答性行列と、ベース部の各方向における変化速度と前記リンクの前記タスク位置の変化速度との関係を示す係数行列と、前記タスク位置の変化速度と前記複数の変位部の角速度との関係を示す係数行列とを用いて、前記目標角加速度及び前記ベース部の各方向の目標変位加速度を含む今回の駆動部目標変位加速度と、前回の駆動部目標変位加速度とから、前記複数の変位部の実角加速度及び前記ベース部の各方向の実変位加速度を含む駆動部実変位加速度が、サンプリング時間後に、前記今回の駆動部目標変位加速度となるように、前記複数の変位部の制御トルクを算出する
ことを特徴とする駆動装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの関節を駆動させる駆動装置に関する。
【背景技術】
【０００２】
下記特許文献１には、モデルパラメータ誤差や外乱に対する制御性を向上させた柔軟受動関節を有するロボットアームの制御装置が記載されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００５−３４９５５５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記特許文献１に記載の技術では、外乱に対する制御性を向上することはできるが、目標角加速度で関節を駆動させると、目標角加速度と実際の関節の実角加速度との間には、一次遅れ等の応答遅れが発生し、目標角加速度に対する実角加速度の追従性が低下してしまう。
【０００５】
そこで本発明は、関節の目標角加速度に対する実角加速度の追従性を向上させる駆動装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明は、複数の変位部を有するリンクと、前記複数の変位部を駆動する複数の回転駆動源とを有する駆動部と、前記駆動部を駆動制御する制御部と、を備える駆動装置において、前記制御部は、前記複数の変位部のトルクの応答性を示す変位部トルク応答性行列、前記複数の変位部の角度によって決定された前記駆動部の慣性行列、前記複数の変位部の今回の目標角加速度、及び前記複数の変位部の前回の目標角加速度を用いて、前記複数の変位部の実角加速度が、サンプリング時間後に、前記今回の目標角加速度となるように、前記複数の変位部の制御トルクを算出する制御トルク算出部を備えることを特徴とする。
【０００７】
前記駆動装置において、前記変位部は、回転駆動する関節又は進退駆動する進退部であることを特徴とする。
【０００８】
前記駆動装置において、前記制御部は、下記の関係式を用いて、前記制御トルクを算出することを特徴とする。
【０００９】
【数１】
2014046425.tif 000003
【００１０】
前記駆動装置において、前記複数の関節の最小時定数を測定しておき、前記最小時定数を用いて、変位部トルク応答性行列の対角要素である変位部トルク応答係数を設定することを特徴とする。
【００１１】
前記駆動装置において、前記制御トルク算出部は、前記リンクの予め定められたタスク位置の今回の目標変化加速度及び前回の目標変化加速度と、前記タスク位置の変化速度と前記複数の変位部の角速度との関係を示す係数行列とを用いて、前記複数の変位部の今回の目標角加速度、及び前記複数の変位部の前回の目標角加速度を算出することで、前記タスク位置の実変化加速度が、サンプリング時間後に、前記今回の目標変化加速度となるように、前記複数の変位部の制御トルクを算出することを特徴とする。
【００１２】
駆動装置において、駆動部は、前記リンクに支持されるベース部を備え、前記制御トルク算出部は、前記リンクの先端であるタスク位置が地面から受ける力の応答性を示す力応答性行列と、ベース部の各方向における変化速度と前記リンクの前記タスク位置の変化速度との関係を示す係数行列と、前記タスク位置の変化速度と前記複数の変位部の角速度との関係を示す係数行列とを用いて、前記目標角加速度及び前記ベース部の各方向の目標変位加速度を含む今回の駆動部目標変位加速度と、前回の駆動部目標変位加速度とから、前記複数の変位部の実角加速度及び前記ベース部の各方向の実変位加速度を含む駆動部実変位加速度が、サンプリング時間後に、前記今回の駆動部目標変位加速度となるように、前記複数の変位部の制御トルクを算出することを特徴とする。
【発明の効果】
【００１３】
本願発明によれば、複数の変位部のトルクの応答性を示す変位部トルク応答性行列、複数の変位部の角度によって決定された駆動部の慣性行列、複数の変位部の今回の目標角加速度、及び複数の変位部の前回の目標角加速度を用いて、複数の変位部の実角加速度が、サンプリング時間後に、今回の目標角加速度となるように、複数の変位部の制御トルクを算出するので、応答性を改善して複数の関節の駆動を制御することができ、複数の関節の目標角加速度に対する複数の関節の実角加速度の追従性を向上させることができる。
【００１４】
また、本願発明によれば、リンクの予め定められたタスク位置の今回の目標変化加速度及び前回の目標変化加速度と、タスク位置の変化速度と複数の変位部の角速度との関係を示す係数行列とを用いて、複数の変位部の今回の目標角加速度、及び複数の変位部の前回の目標角加速度を算出することで、タスク位置の実変化加速度が、サンプリング時間後に、今回の目標変化加速度となるように、複数の変位部の制御トルクを算出するので、応答性を改善してタスク位置の駆動を制御することができ、タスク位置の目標変化加速度に対するタスク位置の実変化加速度の追従性を向上させることができる。
【００１５】
また、本願発明によれば、リンクの先端であるタスク位置が地面から受ける力の応答性を示す力応答性行列と、リンクに支持されるベース部の各方向における変化速度とリンクのタスク位置の変化速度との関係を示す係数行列と、タスク位置の変化速度と複数の変位部の角速度との関係を示す係数行列とを用いて、目標角加速度及びベース部の各方向の目標変位加速度を含む今回の駆動部目標変位加速度と、前回の駆動部目標変位加速度とから、複数の変位部の実角加速度及びベース部の各方向の実変位加速度を含む駆動部実変位加速度が、サンプリング時間後に、今回の駆動部目標変位加速度となるように、複数の変位部の制御トルクを算出するので、応答性を改善してベース部の駆動を制御することができ、ベース部の目標変位加速度に対するベース部の実変位加速度の追従性を向上させることができる。
【図面の簡単な説明】
【００１６】
【図１】実施の形態の駆動装置を示す図である。
【図２】駆動部の変位部駆動系の構成を示す模式図である。
【図３】目標トルクに対する実トルクの一次遅れを示す図である。
【図４】最小時定数及び最大時定数を説明する図である。
【図５】制御部の機能ブロック図である。
【図６】変形例１の駆動部を示す図である。
【図７】変形例２の駆動部の変位部駆動系の他の構成を示す模式図である。
【図８】変形例３の駆動部の変位部駆動系の他の構成を示す模式図である。
【発明を実施するための形態】
【００１７】
本発明に係る駆動装置について、好適な実施の形態を掲げて添付の図面を参照しながら以下詳細に説明する。
【００１８】
図１は、実施の形態の駆動装置１０を示す図である。駆動装置１０は、駆動部１２と、制御部１４とを備え、駆動部１２は、複数の関節１６（１６1、１６2）と複数の連結部１８（１８1、１８2）とを有するリンク２０を有する。複数の連結部１８は、変位部である関節１６を介して互いに接続されている。リンク２０の根本の関節１６1は、基体１９側に設けられている。
【００１９】
なお、図１では、関節１６及び連結部１８を２個としたが、以下、本実施の形態では、関節１６及び連結部１８がｎ個として説明する。関節１６及び連結部１８がｎ個の場合は、各関節１６を１６iで表し、各連結部１８を１８iで表す。本実施の形態では、ｉは、ｉ＝１、２、・・・、ｎ、であり、関節１６の番号を表している。
【００２０】
図２は、駆動部１２の変位部駆動系３０の構成を示す模式図である。変位部駆動系３０は、モータ３２と、減速機３４とを有し、減速機３４を介して、モータ３２に関節１６が接続されている。
【００２１】
回転駆動源であるモータ３２は、いわゆる電動機であり、図示しない電源から電力が供給されることで、モータ３２に接続された出力軸３２ａを軸周りに回転させるトルクを発生する。この出力軸３２ａには、該出力軸３２ａと一体に回転し得るように減速機３４の入力軸３４ａが連結されている。減速機３４は、入力軸３４ａに入力された回転力を所定の減速比で減速して出力軸３４ｂから出力する。減速機３４の出力軸３４ｂは、関節１６の入力軸１６ａに接続されている。
【００２２】
変位部駆動系３０には、モータ３２の角度を検出する角度センサ３６及び関節１６に加えられるトルクを検出するトルクセンサ３８が設けられている。なお、本実施の形態では、説明をわかり易くするため、減速機３４はないものとして扱い、モータ３２の角度及びトルクを関節１６の角度及びトルクとして説明し、各関節１６iの角度（各関節１６iを駆動するモータ３２の角度）をθiで表す。この関節１６iの角度θiとは、ある基準角度からの角度である。ここで、運動方程式から以下の関係式が得られる。
【００２３】
【数２】
2014046425.tif 000004
【００２４】
【数３】
2014046425.tif 000005
【００２５】
目標トルクτ＿ｃｎｔｒｌは、各関節１６iを駆動するモータ３２の目標トルクτi＿ｃｎｔｒｌ（各関節１６iの目標トルクτi＿ｃｎｔｒｌ）をベクトル表示したものであり、目標角加速度ｄｄθ＿ｃｎｔｒｌは、各関節１６iを駆動するモータ３２の目標角加速度ｄｄθi＿ｃｎｔｒｌ（各関節１６iの目標角加速度ｄｄθi＿ｃｎｔｒｌ）をベクトル表示したものである。各関節１６iの目標角加速度ｄｄθi＿ｃｎｔｒｌは、各関節１６iに目標トルクτi＿ｃｎｔｒｌを加えた時の各関節１６iの目標角加速度である。
【００２６】
実トルクτ＿ａｃｔは、各関節１６iを駆動するモータ３２の実際のトルク（実トルク）τi＿ａｃｔ（各関節１６iの実トルクτi＿ａｃｔ）をベクトル表示したものであり、実角加速度ｄｄθ＿ａｃｔは、各関節１６iを駆動するモータ３２の実際の角加速度（実角加速度）ｄｄθi＿ａｃｔ（各関節１６iの実角加速度ｄｄθi＿ａｃｔ）をベクトル表示したものである。各関節１６iの実角加速度ｄｄθi＿ａｃｔは、各関節１６iに実トルクτi＿ａｃｔが加えられた時の各関節１６iの角加速度である。
【００２７】
力学補正力Ｃ＿ｃｍｐｎは、遠心力やコリオリ力等の各関節１６iのモータ３２の角度θi及び角速度ｄθiによって生じる力ベクトルを表す力学補正力Ｃi＿ｃｍｐｎをベクトル表示したものである。慣性行列Ｍは、駆動部１２の慣性モーメント（イナーシャ）を行列で表したものである。各関節１６iの角度（各関節１６iのモータ３２の角度）θiに応じて、力学補正力Ｃ＿ｃｍｐｎ及び慣性行列Ｍが決定される。
【００２８】
理想的には、目標トルクτ＿ｃｎｔｒｌ＝実トルクτ＿ａｃｔ、なので、実角加速度ｄｄθ＿ａｃｔ＝目標角加速度ｄｄθ＿ｃｎｔｒｌ、の関係となるが、実際には、図３に示すように、実トルクτ＿ａｃｔは、目標トルクτ＿ｃｎｔｒｌの１次遅れ系となり、サンプリング時間経過時には、この関係は成り立たたない。実トルクτ＿ａｃｔは、以下の関係式で表すことができる。
【００２９】
【数４】
2014046425.tif 000006
【００３０】
τ＿ｃｎｔｒｌ＿ｐは、前回の各関節１６iのモータ３２の目標トルクτi＿ｃｎｔｒｌ＿ｐをベクトル表示したものであり、τ＿ａｃｔ＿ｐは、前回の各関節１６iのモータ３２の実トルクτi＿ａｃｔ＿ｐをベクトル表示したものである。ｔは時間を表し、Ｔｃiは、各関節１６iのモータ３２の時定数を表している。また、Ｌは、対角行列である。図３では、静定状態からのスタートを仮定しており、前回の目標トルクτ＿ｃｎｔｒｌ＿ｐと、前回の実トルクτ＿ａｃｔ＿ｐとは同じ値である。なお、本実施の形態では、＿ｐと表す場合は、前回の値を示している。
【００３１】
この数４をサンプリング時間ＤＴで積分して、サンプリング時間ＤＴで除算すると、以下の関係式を導くことができる。なお、対角行列であるＥｆｆは、複数の関節１６のモータ３２のトルクの応答性を示す変位部トルク応答性行列であり、その対角要素は、変位部トルク応答係数であるｅｆｆiで表すことができる。この各ｅｆｆiは、各関節１６iのモータ３２の変位部トルク応答係数を表している。つまり、ｅｆｆ1は、関節１６1のモータ３２の変位部トルク応答係数を表しており、ｅｆｆ2は、関節１６2のモータ３２の変位部トルク応答係数を表している。なお、静定状態からのスタートを仮定しているので、τ＿ａｃｔ＿ｐ＝τ＿ｃｎｔｒｌ＿ｐ、の関係式を用いて、数４から数５を導出している。
【００３２】
【数５】
2014046425.tif 000007
【００３３】
数５から、以下の関係式を導くことができる。
【００３４】
【数６】
2014046425.tif 000008
【００３５】
数２と数６とから、以下の関係式を導くことができる。
【００３６】
【数７】
2014046425.tif 000009
【００３７】
ｄｄθ＿ｃｎｔｒｌ＿ｐは、前回の各関節１６iのモータ３２の目標角加速度ｄｄθi＿ｃｎｔｒｌ＿ｐをベクトル表示したものであり、Ｃ＿ｃｍｐｎ＿ｐは、前回の力学補正力Ｃi＿ｃｍｐｎ＿ｐをベクトル表示したものである。
【００３８】
１サンプリング時間ＤＴでは、力学補正力Ｃ＿ｃｍｐｎと前回の力学補正力Ｃ＿ｃｍｐｎ＿ｐとは大きく変化しないので、Ｃ＿ｃｍｐｎ＝Ｃ＿ｃｍｐｎ＿ｐ、と見做すことができる。従って、数３、数７、及び、Ｃ＿ｃｍｐｎ＝Ｃ＿ｃｍｐｎ＿ｐ、を用いて、以下の関係式を導くことができる。
【００３９】
【数８】
2014046425.tif 000010
【００４０】
実角加速度ｄｄθ＿ａｃｔが、サンプリング時間後に、目標角加速度ｄｄθ＿ｃｎｔｒｌの値になるためには、以下の関係式を満たす必要がある。
【００４１】
【数９】
2014046425.tif 000011
【００４２】
制御角加速度ｍ＿ｄｄθ＿ｃｎｔｒｌは、各関節１６iのモータ３２の制御角加速度ｍ＿ｄｄθi＿ｃｎｔｒｌをベクトル表示したものであり、制御角加速度ｍ＿ｄｄθi＿ｃｎｔｒｌは、各関節１６iのモータ３２の実角加速度ｄｄθi＿ａｃｔが、サンプリング時間後に目標角加速度ｄｄθi＿ｃｎｔｒｌの値となる制御角加速度である。
【００４３】
数２は、制御角加速度ｍ＿ｄｄθ＿ｃｎｔｒｌを用いて、以下のように表すことができる。
【００４４】
【数１０】
2014046425.tif 000012
【００４５】
制御トルクｍ＿τ＿ｃｎｔｒｌは、各関節１６iのモータ３２の制御トルクｍ＿τi＿ｃｎｔｒｌをベクトル表示したものであり、各関節１６iのモータ３２に制御トルクｍ＿τi＿ｃｎｔｒｌを加えた時に各関節１６iのモータ３２の制御角加速度がｍ＿ｄｄθi＿ｃｎｔｒｌとなる制御トルクである。
【００４６】
数９と数１０とから、以下の関係式を導くことができる。
【００４７】
【数１１】
2014046425.tif 000013
【００４８】
このように、今回の各関節１６iのモータ３２の目標角加速度ｄｄθi＿ｃｎｔｒｌ、前回の各関節１６のモータ３２の目標角加速度ｄｄθi＿ｃｎｔｒｌ＿ｐ、慣性行列Ｍ、及び変位部トルク応答性行列Ｅｆｆ等によって、各関節１６iのモータ３２の実角加速度ｄｄθi＿ａｃｔがサンプリング時間ＤＴ後に今回の目標角加速度ｄｄθi＿ｃｎｔｒｌとなるような、各関節１６iのモータ３２の制御トルクｍ＿τi＿ｃｎｔｒｌを求めることができる。
【００４９】
この目標角加速度ｄｄθ＿ｃｎｔｒｌは、ＰＤ制御によって（ｄｄθ＿ｃｎｔｒｌ＝Ｋｐ・（θ＿ｄｅｓ−θ＿ａｃｔ）＋Ｋｖ・（ｄθ＿ｄｅｓ−ｄθ＿ａｃｔ）によって）、求めることができる。そして、求めた目標角加速度ｄｄθ＿ｃｎｔｒｌから制御トルクｍ＿τi＿ｃｎｔｒｌを求め、該求めた制御トルクｍ＿τi＿ｃｎｔｒｌで各関節１６iのモータ３２を駆動制御することで、応答性を改善して各関節１６iのモータ３２を制御することができる。
【００５０】
なお、Ｋｐは、比例ゲインであり、Ｋｖは、微分ゲインである。また、θ＿ｄｅｓは、各関節１６iのモータ３２の要求角度θi＿ｄｅｓをベクトル表示したものであり、θ＿ａｃｔは、各関節１６iのモータ３２の実際の角度（実角度）θi＿ａｃｔをベクトル表示したものである。また、ｄθ＿ｄｅｓは、各関節１６iのモータ３２の要求角速度ｄθi＿ｄｅｓをベクトル表示したものであり、ｄθ＿ａｃｔは、各関節１６iのモータ３２の実際の角速度（実角速度）ｄθi＿ａｃｔをベクトル表示したものである。この実角度θi＿ａｃｔは、各関節１６iのモータ３２の角度センサ３６が検出した値であり、実角速度ｄθi＿ａｃｔは、実角度θi＿ａｃｔから求めることができる。
【００５１】
また、リンク２０のうち予め定められたタスク位置Ａ（図１参照）の応答性を改善して、タスク位置Ａの駆動を制御することもできる。例えば、リンク２０がある姿勢で複数の関節１６によって支持されているので、各関節１６iのモータ３２の角速度ｄθiが変化すると、それに応じてタスク位置Ａが変化する。これを、ヤコビ行列Ｊを用いて以下の関係式で表すことができる。
【００５２】
【数１２】
2014046425.tif 000014
【００５３】
ここで、目標角速度ｄθ＿ｃｎｔｒｌは、各関節１６iのモータ３２の目標角速度ｄθi＿ｃｎｔｒｌをベクトル表示したものである。また、目標変化速度ｄｘ＿ｃｎｔｒｌは、タスク位置Ａの各方向における目標変化速度ｄｘ1＿ｃｎｔｒｌ、ｄｘ2＿ｃｎｔｒｌ、・・・、ｄｘn＿ｃｎｔｒｌをベクトル表示したものである。各方向とは、例えば、ｘ軸方向、ｙ軸方向、ｘ軸の回転方向、ｙ軸の回転方向などである。
【００５４】
数１２を微分すると、以下の関係式で表すことができる。なお、目標変化加速度ｄｄｘ＿ｃｎｔｒｌは、タスク位置Ａの各方向における目標変化加速度ｄｄｘ1＿ｃｎｔｒｌ、ｄｄｘ2＿ｃｎｔｒｌ、・・・、ｄｄｘn＿ｃｎｔｒｌをベクトル表示したものである。
【００５５】
【数１３】
2014046425.tif 000015
【００５６】
従って、Ｊ・ｄｄθ＿ｃｎｔｒｌ−Ｊ・ｍＥｆｆ・ｄｄθ＿ｃｎｔｒｌ＿ｐは、数１３を用いて、以下の関係式で表すことができる。なお、ｄθ＿ｃｎｔｒｌ＿ｐは、前回の各関節１６iのモータ３２の目標角速度ｄθi＿ｃｎｔｒｌ＿ｐをベクトル表示したものであり、ｄｄｘ＿ｃｎｔｒｌ＿ｐは、前回のタスク位置Ａの各方向における目標変化加速度ｄｄｘi＿ｃｎｔｒｌ＿ｐをベクトル表示したものである。
【００５７】
【数１４】
2014046425.tif 000016
【００５８】
数１４は、以下の関係式に変形することができる。
【００５９】
【数１５】
2014046425.tif 000017
【００６０】
また、数１１は、以下の関係式に変形することができる。
【００６１】
【数１６】
2014046425.tif 000018
【００６２】
数１６の各項に、Ｊ・（Ｍ´）-1をかけることで以下の関係式を導くことができる。
【００６３】
【数１７】
2014046425.tif 000019
【００６４】
数１７は、数１５の但し書きの関係式を用いて、以下のように表すことができる。
【００６５】
【数１８】
2014046425.tif 000020
【００６６】
また、制御トルクｍ＿τ＿ｃｎｔｒｌと、タスク位置Ａの力Ｆは、ヤコビ行列Ｊの転置行列ＪTを用いて以下の関係式で表すことができる。なお、Ｆは、タスク位置Ａの各方向における力ｆ1、ｆ2、・・・、ｆnをベクトル表示したものである。
【００６７】
【数１９】
2014046425.tif 000021
【００６８】
数１８を、数１５及び数１９を用いて変形すると、以下の関係式を導くことができる。
【００６９】
【数２０】
2014046425.tif 000022
【００７０】
数２０は、以下の関係式に変更することができる。
【００７１】
【数２１】
2014046425.tif 000023
【００７２】
数２１は、以下の関係式に変更することができる
【００７３】
【数２２】
2014046425.tif 000024
【００７４】
従って、数１１を数２２のように変形することができるので、各関節１６iのモータ３２を駆動制御することで、タスク位置Ａの応答性を改善することができる。つまり、タスク位置Ａの今回の目標変化加速度ｄｄｘ＿ｃｎｔｒｌ及び前回の目標変化加速度ｄｄｘ＿ｃｎｔｒｌ＿ｐを用いて、タスク位置Ａの実変化加速度ｄｄｘ＿ａｃｔが、サンプリング時間ＤＴ後に、タスク位置Ａの目標変化加速度ｄｄｘ＿ｃｎｔｒｌとなるように、タスク位置Ａの力Ｆを求めることができる。そして、この力Ｆに基づいて、数１９から各関節１６iの制御トルクｍ＿τ＿ｃｎｔｒｌを求めて各関節１６iのモータ３２を駆動制御することで、タスク位置Ａの応答性を改善することができる。
【００７５】
なお、数１３を、数１２を用いて以下のように変形することができる。
【００７６】
【数２３】
2014046425.tif 000025
【００７７】
また、数２３から以下の関係式を導くことができる。
【００７８】
【数２４】
2014046425.tif 000026
【００７９】
従って、数２３と数２４とを、数１１のｄｄθ＿ｃｎｔｒｌとｄｄθ＿ｃｎｔｒｌ＿ｐとに代入することで、タスク位置Ａの応答性を改善する各関節１６iのモータ３２の制御トルクｍ＿τi＿ｃｎｔｒｌを求めることもできる。つまり、タスク位置Ａの今回の目標変化加速度ｄｄｘ＿ｃｎｔｒｌ、前回の目標変化加速度ｄｄｘ＿ｃｎｔｒｌ＿ｐ、今回の目標変化速度ｄｘ＿ｃｎｔｒｌ、及び前回の目標変化速度ｄｘ＿ｃｎｔｒｌ＿ｐを用いて、各関節１６iのモータ３２の制御トルクｍ＿τi＿ｃｎｔｒｌを求めることができる。
【００８０】
ここで、各関節１６iのモータ３２のトルク応答性を表す時定数Ｔｃiは、リンク２０の状態、例えば、関節１６iのモータ３２の実角度θi＿ａｃｔの状態等、支持側と負荷側の慣性モーメントに応じて異なる。
【００８１】
例えば、図４に示す様に、リンク２０が、実線で示すリンク２０の根本の関節１６1と、二点鎖線で示すリンク２０の根本の関節１６1とでは、負荷側の状態が異なり、トルク応答、時定数Ｔｃiが異なる。従って、各関節１６iのモータ３２の時定数Ｔｃiを測定し、各関節１６iのモータ３２の時定数Ｔｃiを、最小時定数Ｔｃi＿ｍｉｎ≦時定数Ｔｃi≦最大時定数Ｔｃi＿ｍａｘの範囲で設定する。
【００８２】
ここで、時定数Ｔｃiを最大時定数Ｔｃi＿ｍａｘに設定してしまうと、挙動改善がやり過ぎになる場合があり、また、最小時定数Ｔｃi＿ｍｉｎに設定すると、安定側に振り過ぎる可能性があるが、通常は、最小時定数Ｔｃi＿ｍｉｎに設定することが好ましい。
【００８３】
このように、各関節１６iのモータ３２の時定数Ｔｃiを設定し、該設定したＴｃiに基づいて、上述した変位部トルク応答性行列Ｅｆｆの対角要素であるｅｆｆiの値を決定する。
【００８４】
図５は、制御部１４の機能ブロック図である。制御部１４は、応答性行列設定部５０、行列決定部５２、力学補正決定部５４、目標値設定部５６、制御トルク算出部５８、及びモータ制御部６０を有する。
【００８５】
応答性行列設定部５０は、測定された各関節１６iの最小時定数Ｔｃi＿ｍｉｎ及び最大時定数Ｔｃi＿ｍａｘに基づいて、各関節１６iの時定数Ｔｃiを決定する。そして、数５に示すように、該決定した時定数Ｔｃiで各関節１６iの変位部トルク応答係数ｅｆｆiを求めることで、変位部トルク応答性行列Ｅｆｆを設定する。本実施の形態では、各関節１６iの最小時定数Ｔｃi＿ｍｉｎを、各関節１６iの時定数Ｔｃiとして設定する。応答性行列設定部５０は、設定した変位部トルク応答性行列Ｅｆｆを制御トルク算出部５８に出力する。
【００８６】
行列決定部５２は、複数の関節１６の実角度θ＿ａｃｔに基づいて、駆動部１２の慣性モーメントを表す慣性行列Ｍを決定する。また、行列決定部５２は、タスク位置Ａと、複数の関節１６の実角度θ＿ａｃｔとに応じて、ヤコビ行列（係数行列）Ｊを決定する。タスク位置Ａは、タスク位置Ａにジャイロセンサ等を設けてタスク位置Ａを検出してもよいし、各関節１６iの実角度θi＿ａｃｔからタスク位置Ａを算出してもよい。この慣性行列Ｍ及びヤコビ行列Ｊの算出については周知技術なので説明を割愛する。行列決定部５２は、決定した慣性行列Ｍ及びヤコビ行列Ｊを制御トルク算出部５８に出力する。
【００８７】
力学補正決定部５４は、遠心力やコリオリ力等の各関節１６iの実角度θi＿ａｃｔ及び実角速度ｄθi＿ａｃｔによって生じる力ベクトルを表す力学補正力Ｃi＿ｃｍｐｎを算出する。この各関節１６iの力学補正力Ｃi＿ｃｍｐｎは、各関節１６iの角度θiに応じて決定する。この力学補正力Ｃi＿ｃｍｐｎの算出方法は周知技術なので説明を割愛する。力学補正決定部５４は、算出した力学補正力Ｃi＿ｃｍｐｎを制御トルク算出部５８に出力する。
【００８８】
目標値設定部５６は、複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌ、若しくは、タスク位置Ａの目標変化加速度ｄｄｘ＿ｃｎｔｒｌを設定する。この目標角加速度ｄｄθ＿ｃｎｔｒｌは、ｄｄθ＿ｃｎｔｒｌ＝Ｋｐ・（θ＿ｄｅｓ−θ＿ａｃｔ）＋Ｋｖ・（ｄθ＿ｄｅｓ−ｄθ＿ａｃｔ）としてＰＤ制御を実行して設定することができる。この要求角度θ＿ｄｅｓ及び要求角速度ｄθ＿ｄｅｓ、又は、目標変化加速度ｄｄｘ＿ｃｎｔｒｌは、複数の関節１６又はタスク位置Ａの状態等の種々の要因によって決定される。目標値設定部５６は、設定した複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌ又は目標変化加速度ｄｄｘ＿ｃｎｔｒｌを制御トルク算出部５８に出力する。
【００８９】
制御トルク算出部５８は、目標値設定部５６から送られてきた目標角加速度θ＿ｃｎｔｒｌ又は目標変化加速度ｄｄｘ＿ｃｎｔｒｌに基づいて、複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌを算出する。詳しくは、目標角加速度θ＿ｃｎｔｒｌが送られてきた場合は、目標角加速度θ＿ｃｎｔｒｌ、前回の目標角加速度θ＿ｃｎｔｒｌ＿ｐ、変位部トルク応答性行列Ｅｆｆ、慣性行列Ｍ、力学補正力Ｃ＿ｃｍｐｎから、数１１の関係式を用いて制御トルクｍ＿τ＿ｃｎｔｒｌを算出する。
【００９０】
また、目標変化加速度ｄｄｘ＿ｃｎｔｒｌが送られてきた場合は、目標変化加速度ｄｄｘ＿ｃｎｔｒｌ、前回の目標変化加速度ｄｄｘ＿ｃｎｔｒｌ＿ｐ、変位部トルク応答性行列Ｅｆｆ、慣性行列Ｍ、力学補正力Ｃ＿ｃｍｐｎ、ヤコビ行列Ｊから、数２２（数１２〜数２１を参照）を用いて、又は、数１１、数２３、及び数２４を用いて、複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌを算出する。制御トルク算出部５８は、算出した複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌをモータ制御部６０に出力する。
【００９１】
モータ制御部６０は、送られてきた複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌで、複数のモータ３２を制御する。これにより、各関節１６iの応答性又はタスク位置Ａの応答性を改善した、各関節１６i又はタスク位置Ａの駆動を制御することができる。従って、目標トルクτ＿ｃｎｔｒｌに対する実トルクτ＿ａｃｔの追従性、つまり、目標角加速度ｄｄθ＿ｃｎｔｒｌに対する実角加速度ｄｄθ＿ａｃｔの追従性を向上させることができる。
【００９２】
このように、制御部１４は、複数の関節１６のトルクの応答性を示す変位部トルク応答性行列Ｅｆｆ、複数のモータ３２の角度θによって決定された駆動部１２の慣性行列Ｍ、複数の関節１６の今回の目標角加速度ｄｄθ＿ｃｎｔｒｌ、及び複数の関節１６の前回の目標角加速度ｄｄθ＿ｃｎｔｒｌ＿ｐを用いて、複数の関節１６の実角加速度ｄｄθ＿ａｃｔが、サンプリング時間ＤＴ後に、複数の関節１６の今回の目標角加速度ｄｄθ＿ｃｎｔｒｌとなるように、複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌを算出するので、応答性を改善して複数の関節１６の駆動を制御することができ、複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌに対する複数の関節１６の実角加速度ｄｄθ＿ａｃｔの追従性を向上させることができる。
【００９３】
また、制御部１４は、複数の関節１６のトルクの応答性を示す変位部トルク応答性行列Ｅｆｆ、複数のモータ３２の角度θによって決定された駆動部１２の慣性行列Ｍ及びヤコビ行列Ｊ、リンク２０のうち、予め定められたタスク位置Ａの今回の目標変化加速度ｄｄｘ＿ｃｎｔｒｌ、及び前回のタスク位置Ａの目標変化加速度ｄｄｘ＿ｃｎｔｒｌ＿ｐを用いて、タスク位置Ａの実変化加速度ｄｄｘ＿ａｃｔが、サンプリング時間ＤＴ後に、タスク位置Ａの目標変化加速度ｄｄｘ＿ｃｎｔｒｌとなるように、複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌを算出するので、応答性を改善して複数の関節１６の駆動を制御することができ、複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌに対する複数の関節１６の実角加速度ｄｄθ＿ａｃｔの追従性を向上させることができる。従って、応答性を改善してタスク位置Ａの駆動を制御することができ、タスク位置Ａの目標変化加速度ｄｄｘ＿ｃｎｔｒｌに対するタスク位置Ａの実変化加速度ｄｄｘ＿ａｃｔの追従性を向上させることができる。
【００９４】
［変形例］
なお、上記実施の形態は、以下のように変形してもよい。
【００９５】
（変形例１）変形例１では、リンクに接続され、自身では駆動することができない物体のトルク応答を改善するというものである。
【００９６】
図６は、変形例１における駆動部１２を示す図である。駆動部１２は、両脚として用いられる２つのリンク２０と、該２つのリンク２０によって支持されるベース部８０とを有する。左側のリンクをリンク２０Ｌで表し、右側のリンクをリンク２０Ｒで表す。
【００９７】
図６では、リンク２０Ｌ、２０Ｒは、関節１６1Ｌ、１６1Ｒ、関節１６2Ｌ、１６2Ｒ、関節１６3Ｌ、１６3Ｒの３つの関節を有し、関節１６1Ｌ、１６1Ｒがベース部８０に接続されている構造とするが、リンク２０Ｌ、２０Ｒは、２つ、又は４つ以上の関節１６iを有していてもよい。変形例１では、関節１６はｎ個あるものとし、リンク２０Ｌ、２０Ｒの各関節１６を１６iで表す。
【００９８】
ベース部８０は、関節及び該関節を駆動するモータを有しないので自ら駆動することはないが、リンク２０Ｌ、２０Ｒの駆動によって、間接的に自在に駆動する。この図６から、以下の運動方程式が得られる。
【００９９】
【数２５】
2014046425.tif 000027
【０１００】
実変位加速度ｄｄｂａｓｅ＿ａｃｔは、ベース部８０の各方向における実変位加速度ｄｄｂａｓｅ1＿ａｃｔ、ｄｄｂａｓｅ2＿ａｃｔ、・・・、ｄｄｂａｓｅn＿ａｃｔをベクトル表示したものである。各方向とは、例えば、ｘ軸方向、ｙ軸方向、ｚ軸方向、ｘ軸の回転方向、ｙ軸の回転方向、ｚ軸の回転方向等がある。力学補正力Ｃ＿ｃｍｐｎ＿ｂａｓｅは、遠心力やコリオリ力等のベース部８０の各方向に生じる力学補正力Ｃ1＿ｃｍｐｎ＿ｂａｓｅ、Ｃ2＿ｃｍｐｎ＿ｂａｓｅ、・・・、Ｃn＿ｃｍｐｎ＿ｂａｓｅをベクトル表示したものである。この力学補正力Ｃ＿ｃｍｐｎ＿ｂａｓｅは、力学補正決定部５４によって決定される。
【０１０１】
Ｊｂａｓｅは、ベース部８０の各方向における変化速度とリンク２０Ｌ（２０Ｒ）の先端である足先（タスク位置Ａ）の変化速度との関係を示すヤコビ行列（係数行列）である。なお、Ｊは、各関節１６iのモータ３２の角速度ｄθiとリンク２０Ｌ（２０Ｒ）の足先（タスク位置Ａ）の変化速度との関係を示すヤコビ行列である。このヤコビ行列Ｊｂａｓｅ、及びＪは、ベース部８０の位置、足先の位置、及び複数の関節１６の実角度θ＿ａｃｔ等に応じて、行列決定部５２によって決定される。
【０１０２】
数２５からわかるように、図６の場合は、複数の関節１６のモータ３２の実角加速度ｄｄθ＿ａｃｔによって定まるトルク（Ｍ・ｄｄθ＿ａｃｔ）と、タスク位置Ａが地面から受ける力Ｆによって複数の関節１６に与えられるトルク（ＪT・Ｆ）とによって、各関節１６iのモータ３２の実トルクτ＿ａｃｔが定まる。ベース部８０の実変位加速度ｄｄｂａｓｅ＿ａｃｔによって定まるトルク（Ｍ・ｄｄｂａｓｅ＿ａｃｔ）と、タスク位置Ａが受ける力Ｆによってベース部８０の各方向に与えられるトルク（ＪｂａｓｅT・Ｆ）によって、ベース部８０に与えられる実トルクは定まるが、ベース部８０は、モータを有しないので、実トルクは０となる。
【０１０３】
数２５は、以下の関係式に変形することができる。
【０１０４】
【数２６】
2014046425.tif 000028
【０１０５】
数２６は、以下の関係式で表すことができる。なお、複数の関節１６の実角加速度ｄｄθ＿ａｃｔ及びベース部８０の実変位加速度ｄｄｂａｓｅ＿ａｃｔを表すｄｄｑ＿ａｃｔを駆動部実変位加速度と呼び、τ＿ｘ＿ａｃｔ及びτ＿θ＿ａｃｔを表すτ＿ｔｏｔａｌ＿ａｃｔを総合実トルクと呼ぶ。
【０１０６】
【数２７】
2014046425.tif 000029
【０１０７】
ここで、タスク位置Ａが受ける力Ｆは、数６に示す関係式と同様に、数２８で表すことができる。Ｆ＿Ｅｆｆは、タスク位置Ａである足先が受ける力の応答性を示す力応答性行列であり、その対角要素は、力応答係数であるｆ＿ｅｆｆ1、ｆ＿ｅｆｆ2、・・・、ｆ＿ｅｆｆnで表すことができる。Ｆ＿ｃｎｔｒｌは、目標となるタスク位置Ａが受ける力（目標力）であり、数２８を用いて、このＦ＿ｃｎｔｒｌに対してタスク位置Ａが実際に受ける力Ｆが定まる。従って、目標力Ｆに対する実際の力Ｆを測定することで、所望の力応答性行列Ｆ＿Ｅｆｆを設定する。この力応答性行列Ｆ＿Ｅｆｆは、応答性行列設定部５０によって設定される。
【０１０８】
【数２８】
2014046425.tif 000030
【０１０９】
また、実トルク＿θ＿ａｃｔは、数６に示す関係式と同様に、数２９で表すことができる。τ＿θ＿ｃｎｔｒｌは、目標トルクであり、数２９を用いて、この目標トルクτ＿θ＿ｃｎｔｒｌに対する実トルクτ＿θ＿ａｃｔを求めることができる。
【０１１０】
【数２９】
2014046425.tif 000031
【０１１１】
また、実トルクτ＿ｘ＿ａｃｔは、数６に示す関係式と同様に、数３０で表すことができる。τ＿ｘ＿ｃｎｔｒｌは目標トルクであり、数３０を用いて、この目標トルクτ＿ｘ＿ｃｎｔｒｌに対する実トルクτ＿ｘａｃｔを求めることができる。
【０１１２】
【数３０】
2014046425.tif 000032
【０１１３】
数２７の実トルクτ＿θ＿ａｃｔ、実トルクτ＿ｘ＿ａｃｔに数２９、数３０をそれぞれ代入すると、以下の関係式で表すことができる。
【０１１４】
【数３１】
2014046425.tif 000033
また、一般的な運動方程式から、以下の関係式を得ることができる。
【０１１５】
【数３２】
2014046425.tif 000034
【０１１６】
なお、目標変位加速度ｄｄｂａｓｅ＿ｃｎｔｒｌは、ベース部８０の各方向における目標変位加速度ｄｄｂａｓｅ1＿ｃｎｔｒｌ、ｄｄｂａｓｅ2＿ｃｎｔｒｌ、・・・、ｄｄｂａｓｅn＿ｃｎｔｒｌをベクトル表示したものである。また、複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌ及びベース部８０の目標変位加速度ｄｄｂａｓｅ＿ｃｎｔｒｌを表すｄｄｑ＿ｃｎｔｒｌを、駆動部目標変位加速度と呼び、τ＿ｘ＿ｃｎｔｒｌ及びτ＿θ＿ｃｎｔｒｌを表すτ＿ｔｏｔａｌ＿ｃｎｔｒｌを総合目標トルクと呼ぶ。この駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌは、ベース部８０の状態、複数の関節１６等の種々に要因によって、目標値設定部５６によって設定される。
【０１１７】
この数３１、数３２とから、数１１を導き出したのと同様の手順で、以下に示す関係式を得ることができる。
【０１１８】
【数３３】
2014046425.tif 000035
【０１１９】
従って、制御トルク算出部５８は、目標値設定部５６から送られてきた駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌに基づいて、総合制御トルクｍ＿τ＿ｄｄｑ＿ｃｎｔｒｌを算出し、モータ制御部６０は、該求めた総合制御トルクｍ＿τ＿ｄｄｑ＿ｃｎｔｒｌに基づいて、複数のモータ３２を制御する。
【０１２０】
このように、今回の駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌ、前回の駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌ＿ｐ、慣性行列Ｍ、力応答性行列Ｆ＿Ｅｆｆ、変位部トルク応答性行列Ｅｆｆ等によって、駆動部実変位加速度ｄｄｑ＿ａｃｔがサンプリング時間ＤＴ後に今回の駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌとなるような、総合制御トルクｍ＿τ＿ｄｄｑ＿ｃｎｔｒｌを求めることができる。この総合制御トルクｍ＿τ＿ｄｄｑ＿ｃｎｔｒｌで、各関節１６iのモータ３２を駆動制御することで、各関節１６iは勿論、ベース部８０の駆動の応答性をも改善することができる。
【０１２１】
（変形例２）上記実施の形態、及び、変形例１では、変位部として回転駆動する関節１６を用いて説明したが、変位部は、直進駆動（進退駆動）する進退部であってもよい。
【０１２２】
図７は、変形例２の駆動部１２の変位部駆動系３０の他の構成を示す模式図である。変位部駆動系３０は、モータ３２と、駆動力変換部１００と、弾性体１０２とを有する動力伝達部１０４、弾性体１０２を介して変位部である進退部１０６が接続されている。なお、図２と同様の構成については同一の符号を付す。
【０１２３】
駆動力変換部１００は、モータ３２の出力軸３２ａのネジ溝に嵌合されおり、出力軸３２ａが回転することで駆動力変換部１００が軸方向に矢印方向に移動する。駆動力変換部１００と進退部１０６とは、動力伝達部１０４を介して接続されている。従って、駆動力変換部１００の移動に応じて進退部１０６も矢印方向に移動する。なお、図７では動力伝達部１０４に弾性体１０２を設けたが、弾性体１０２を設けなくてもよい。なお、モータ３２にはモータ３２の角度を検出する角度センサ３６が、進退部１０６には進退部１０６のトルクを検出するトルクセンサ３８が設けられている。
【０１２４】
関節１６を図２に示す様なモータ３２の回転によって進退可能な進退部１０６であっても、上記実施の形態及び変形例１で示した関係式（数２〜数３３）と同一の関係式が成り立つので、関節１６を進退部１０６に替えても応答性を改善することができる。なお、モータ３２の出力軸３２ａに減速機を接続し、減速機の出力軸のネジ溝に駆動力変換部１００を嵌合してもよい。
【０１２５】
（変形例３）上記実施の形態及び上記変形例１では、モータ３２と関節１６との間には、弾性部材を設けないようにしたが、モータ３２と関節１６との間に弾性部材を設けてもよい。
【０１２６】
図８は、変形例３の駆動部１２の変位部駆動系３０の他の構成を示す模式図である。変位部駆動系３０は、モータ３２、減速機３４、第１プーリ１１０、ワイヤ１１２、第２プーリ１１４、関節１６を備える。
【０１２７】
減速機３４の出力軸３４ｂに、第１プーリ１１０の入力軸１１０ａが連結される。ワイヤ１１２は、円環状の弾性部材であり、第１プーリ１１０の回転力を第２プーリ１１４に伝達する。そして、第２プーリ１１４の出力軸１１４ａに関節１６が連結される。なお、モータ３２にはモータ３２の角度を検出する角度センサ３６が、第２プーリ１１４には、関節１６に与えられるトルクを検出するトルクセンサ３８が設けられている。
【０１２８】
以上、本発明を実施の形態を用いて説明したが、本発明の技術的範囲は上記実施の形態に記載の範囲には限定されない。上記実施の形態に、多様な変更又は改良を加えることが可能であることが当業者に明らかである。その様な変更又は改良を加えた形態も本発明の技術的範囲に含まれ得ることが、特許請求の範囲の記載から明らかである。
【符号の説明】
【０１２９】
１０…駆動装置                      １２…駆動部
１４…制御部                        １６…関節
１８…連結部                        ２０…リンク
３０…変位部駆動系                  ３２…モータ
３４…減速機                        ３６…角度センサ
５０…応答性行列設定部              ５２…行列決定部
５６…目標値設定部                  ５８…制御トルク算出部
６０…モータ制御部                  ８０…ベース部
１００…駆動力変換部                １０２…弾性体
１０４…動力伝達部                  １０６…進退部
１１０…第１プーリ                  １１２…ワイヤ
１１４…第２プーリ
【図１】
2014046425.tif 000037
【図２】
2014046425.tif 000038
【図３】
2014046425.tif 000039
【図４】
2014046425.tif 000040
【図５】
2014046425.tif 000041
【図６】
2014046425.tif 000042
【図７】
2014046425.tif 000043
【図８】
2014046425.tif 000044