JP2014046425A - 駆動装置 - Google Patents

Abstract

【課題】関節の目標角加速度に対する実角加速度の追従性を向上させる駆動装置を提供する。
【解決手段】駆動装置１０は、複数の関節１６を有するリンク２０と、複数の関節１６を駆動する複数のモータ３２とを有する駆動部１２と、駆動部１２を駆動制御する制御部１４とを備え、制御部１４は、目標角加速度ｄｄθ＿ｃｎｔｒｌ、前回の目標角加速度ｄｄθ＿ｃｎｔｒｌ、変位部トルク応答性行列Ｅｆｆ、慣性行列Ｍ、力学補正力Ｃ＿ｃｍｐｎから、数１１の関係式を用いて算出した制御トルクｍ＿τ＿ｃｎｔｒｌで複数のモータ３２を制御する。
【選択図】図５

Description

本発明は、ロボットの関節を駆動させる駆動装置に関する。

下記特許文献１には、モデルパラメータ誤差や外乱に対する制御性を向上させた柔軟受動関節を有するロボットアームの制御装置が記載されている。

特開２００５−３４９５５５号公報

上記特許文献１に記載の技術では、外乱に対する制御性を向上することはできるが、目標角加速度で関節を駆動させると、目標角加速度と実際の関節の実角加速度との間には、一次遅れ等の応答遅れが発生し、目標角加速度に対する実角加速度の追従性が低下してしまう。

そこで本発明は、関節の目標角加速度に対する実角加速度の追従性を向上させる駆動装置を提供することを目的とする。

本発明は、複数の変位部を有するリンクと、前記複数の変位部を駆動する複数の回転駆動源とを有する駆動部と、前記駆動部を駆動制御する制御部と、を備える駆動装置において、前記制御部は、前記複数の変位部のトルクの応答性を示す変位部トルク応答性行列、前記複数の変位部の角度によって決定された前記駆動部の慣性行列、前記複数の変位部の今回の目標角加速度、及び前記複数の変位部の前回の目標角加速度を用いて、前記複数の変位部の実角加速度が、サンプリング時間後に、前記今回の目標角加速度となるように、前記複数の変位部の制御トルクを算出する制御トルク算出部を備えることを特徴とする。

前記駆動装置において、前記変位部は、回転駆動する関節又は進退駆動する進退部であることを特徴とする。

前記駆動装置において、前記制御部は、下記の関係式を用いて、前記制御トルクを算出することを特徴とする。

前記駆動装置において、前記複数の関節の最小時定数を測定しておき、前記最小時定数を用いて、変位部トルク応答性行列の対角要素である変位部トルク応答係数を設定することを特徴とする。

前記駆動装置において、前記制御トルク算出部は、前記リンクの予め定められたタスク位置の今回の目標変化加速度及び前回の目標変化加速度と、前記タスク位置の変化速度と前記複数の変位部の角速度との関係を示す係数行列とを用いて、前記複数の変位部の今回の目標角加速度、及び前記複数の変位部の前回の目標角加速度を算出することで、前記タスク位置の実変化加速度が、サンプリング時間後に、前記今回の目標変化加速度となるように、前記複数の変位部の制御トルクを算出することを特徴とする。

駆動装置において、駆動部は、前記リンクに支持されるベース部を備え、前記制御トルク算出部は、前記リンクの先端であるタスク位置が地面から受ける力の応答性を示す力応答性行列と、ベース部の各方向における変化速度と前記リンクの前記タスク位置の変化速度との関係を示す係数行列と、前記タスク位置の変化速度と前記複数の変位部の角速度との関係を示す係数行列とを用いて、前記目標角加速度及び前記ベース部の各方向の目標変位加速度を含む今回の駆動部目標変位加速度と、前回の駆動部目標変位加速度とから、前記複数の変位部の実角加速度及び前記ベース部の各方向の実変位加速度を含む駆動部実変位加速度が、サンプリング時間後に、前記今回の駆動部目標変位加速度となるように、前記複数の変位部の制御トルクを算出することを特徴とする。

本願発明によれば、複数の変位部のトルクの応答性を示す変位部トルク応答性行列、複数の変位部の角度によって決定された駆動部の慣性行列、複数の変位部の今回の目標角加速度、及び複数の変位部の前回の目標角加速度を用いて、複数の変位部の実角加速度が、サンプリング時間後に、今回の目標角加速度となるように、複数の変位部の制御トルクを算出するので、応答性を改善して複数の関節の駆動を制御することができ、複数の関節の目標角加速度に対する複数の関節の実角加速度の追従性を向上させることができる。

また、本願発明によれば、リンクの予め定められたタスク位置の今回の目標変化加速度及び前回の目標変化加速度と、タスク位置の変化速度と複数の変位部の角速度との関係を示す係数行列とを用いて、複数の変位部の今回の目標角加速度、及び複数の変位部の前回の目標角加速度を算出することで、タスク位置の実変化加速度が、サンプリング時間後に、今回の目標変化加速度となるように、複数の変位部の制御トルクを算出するので、応答性を改善してタスク位置の駆動を制御することができ、タスク位置の目標変化加速度に対するタスク位置の実変化加速度の追従性を向上させることができる。

また、本願発明によれば、リンクの先端であるタスク位置が地面から受ける力の応答性を示す力応答性行列と、リンクに支持されるベース部の各方向における変化速度とリンクのタスク位置の変化速度との関係を示す係数行列と、タスク位置の変化速度と複数の変位部の角速度との関係を示す係数行列とを用いて、目標角加速度及びベース部の各方向の目標変位加速度を含む今回の駆動部目標変位加速度と、前回の駆動部目標変位加速度とから、複数の変位部の実角加速度及びベース部の各方向の実変位加速度を含む駆動部実変位加速度が、サンプリング時間後に、今回の駆動部目標変位加速度となるように、複数の変位部の制御トルクを算出するので、応答性を改善してベース部の駆動を制御することができ、ベース部の目標変位加速度に対するベース部の実変位加速度の追従性を向上させることができる。

実施の形態の駆動装置を示す図である。 駆動部の変位部駆動系の構成を示す模式図である。 目標トルクに対する実トルクの一次遅れを示す図である。 最小時定数及び最大時定数を説明する図である。 制御部の機能ブロック図である。 変形例１の駆動部を示す図である。 変形例２の駆動部の変位部駆動系の他の構成を示す模式図である。 変形例３の駆動部の変位部駆動系の他の構成を示す模式図である。

本発明に係る駆動装置について、好適な実施の形態を掲げて添付の図面を参照しながら以下詳細に説明する。

図１は、実施の形態の駆動装置１０を示す図である。駆動装置１０は、駆動部１２と、制御部１４とを備え、駆動部１２は、複数の関節１６（１６₁、１６₂）と複数の連結部１８（１８₁、１８₂）とを有するリンク２０を有する。複数の連結部１８は、変位部である関節１６を介して互いに接続されている。リンク２０の根本の関節１６₁は、基体１９側に設けられている。

なお、図１では、関節１６及び連結部１８を２個としたが、以下、本実施の形態では、関節１６及び連結部１８がｎ個として説明する。関節１６及び連結部１８がｎ個の場合は、各関節１６を１６_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＿ｃｎｔｒｌ＿ｐをベクトル表示したものであり、τ＿ａｃｔ＿ｐは、前回の各関節１６_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＿ａｃｔをベクトル表示したものである。この実角度θ_i＿ａｃｔは、各関節１６_iのモータ３２の角度センサ３６が検出した値であり、実角速度ｄθ_i＿ａｃｔは、実角度θ_i＿ａｃｔから求めることができる。

また、リンク２０のうち予め定められたタスク位置Ａ（図１参照）の応答性を改善して、タスク位置Ａの駆動を制御することもできる。例えば、リンク２０がある姿勢で複数の関節１６によって支持されているので、各関節１６_iのモータ３２の角速度ｄθ_iが変化すると、それに応じてタスク位置Ａが変化する。これを、ヤコビ行列Ｊを用いて以下の関係式で表すことができる。

ここで、目標角速度ｄθ＿ｃｎｔｒｌは、各関節１６_iのモータ３２の目標角速度ｄθ_i＿ｃｎｔｒｌをベクトル表示したものである。また、目標変化速度ｄｘ＿ｃｎｔｒｌは、タスク位置Ａの各方向における目標変化速度ｄｘ₁＿ｃｎｔｒｌ、ｄｘ₂＿ｃｎｔｒｌ、・・・、ｄｘ_n＿ｃｎｔｒｌをベクトル表示したものである。各方向とは、例えば、ｘ軸方向、ｙ軸方向、ｘ軸の回転方向、ｙ軸の回転方向などである。

数１２を微分すると、以下の関係式で表すことができる。なお、目標変化加速度ｄｄｘ＿ｃｎｔｒｌは、タスク位置Ａの各方向における目標変化加速度ｄｄｘ₁＿ｃｎｔｒｌ、ｄｄｘ₂＿ｃｎｔｒｌ、・・・、ｄｄｘ_n＿ｃｎｔｒｌをベクトル表示したものである。

従って、Ｊ・ｄｄθ＿ｃｎｔｒｌ−Ｊ・ｍＥｆｆ・ｄｄθ＿ｃｎｔｒｌ＿ｐは、数１３を用いて、以下の関係式で表すことができる。なお、ｄθ＿ｃｎｔｒｌ＿ｐは、前回の各関節１６_iのモータ３２の目標角速度ｄθ_i＿ｃｎｔｒｌ＿ｐをベクトル表示したものであり、ｄｄｘ＿ｃｎｔｒｌ＿ｐは、前回のタスク位置Ａの各方向における目標変化加速度ｄｄｘ_i＿ｃｎｔｒｌ＿ｐをベクトル表示したものである。

数１４は、以下の関係式に変形することができる。

また、数１１は、以下の関係式に変形することができる。

数１６の各項に、Ｊ・（Ｍ´）^-1をかけることで以下の関係式を導くことができる。

数１７は、数１５の但し書きの関係式を用いて、以下のように表すことができる。

また、制御トルクｍ＿τ＿ｃｎｔｒｌと、タスク位置Ａの力Ｆは、ヤコビ行列Ｊの転置行列Ｊ^Tを用いて以下の関係式で表すことができる。なお、Ｆは、タスク位置Ａの各方向における力ｆ₁、ｆ₂、・・・、ｆ_nをベクトル表示したものである。

数１８を、数１５及び数１９を用いて変形すると、以下の関係式を導くことができる。

数２０は、以下の関係式に変更することができる。

数２１は、以下の関係式に変更することができる

従って、数１１を数２２のように変形することができるので、各関節１６_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の実角度θ_i＿ａｃｔ及び実角速度ｄθ_i＿ａｃｔによって生じる力ベクトルを表す力学補正力Ｃ_i＿ｃｍｐｎを算出する。この各関節１６_iの力学補正力Ｃ_i＿ｃｍｐｎは、各関節１６_iの角度θ_iに応じて決定する。この力学補正力Ｃ_i＿ｃｍｐｎの算出方法は周知技術なので説明を割愛する。力学補正決定部５４は、算出した力学補正力Ｃ_i＿ｃｍｐｎを制御トルク算出部５８に出力する。

目標値設定部５６は、複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌ、若しくは、タスク位置Ａの目標変化加速度ｄｄｘ＿ｃｎｔｒｌを設定する。この目標角加速度ｄｄθ＿ｃｎｔｒｌは、ｄｄθ＿ｃｎｔｒｌ＝Ｋｐ・（θ＿ｄｅｓ−θ＿ａｃｔ）＋Ｋｖ・（ｄθ＿ｄｅｓ−ｄθ＿ａｃｔ）としてＰＤ制御を実行して設定することができる。この要求角度θ＿ｄｅｓ及び要求角速度ｄθ＿ｄｅｓ、又は、目標変化加速度ｄｄｘ＿ｃｎｔｒｌは、複数の関節１６又はタスク位置Ａの状態等の種々の要因によって決定される。目標値設定部５６は、設定した複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌ又は目標変化加速度ｄｄｘ＿ｃｎｔｒｌを制御トルク算出部５８に出力する。

制御トルク算出部５８は、目標値設定部５６から送られてきた目標角加速度θ＿ｃｎｔｒｌ又は目標変化加速度ｄｄｘ＿ｃｎｔｒｌに基づいて、複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌを算出する。詳しくは、目標角加速度θ＿ｃｎｔｒｌが送られてきた場合は、目標角加速度θ＿ｃｎｔｒｌ、前回の目標角加速度θ＿ｃｎｔｒｌ＿ｐ、変位部トルク応答性行列Ｅｆｆ、慣性行列Ｍ、力学補正力Ｃ＿ｃｍｐｎから、数１１の関係式を用いて制御トルクｍ＿τ＿ｃｎｔｒｌを算出する。

また、目標変化加速度ｄｄｘ＿ｃｎｔｒｌが送られてきた場合は、目標変化加速度ｄｄｘ＿ｃｎｔｒｌ、前回の目標変化加速度ｄｄｘ＿ｃｎｔｒｌ＿ｐ、変位部トルク応答性行列Ｅｆｆ、慣性行列Ｍ、力学補正力Ｃ＿ｃｍｐｎ、ヤコビ行列Ｊから、数２２（数１２〜数２１を参照）を用いて、又は、数１１、数２３、及び数２４を用いて、複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌを算出する。制御トルク算出部５８は、算出した複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌをモータ制御部６０に出力する。

モータ制御部６０は、送られてきた複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌで、複数のモータ３２を制御する。これにより、各関節１６_iの応答性又はタスク位置Ａの応答性を改善した、各関節１６_i又はタスク位置Ａの駆動を制御することができる。従って、目標トルクτ＿ｃｎｔｒｌに対する実トルクτ＿ａｃｔの追従性、つまり、目標角加速度ｄｄθ＿ｃｎｔｒｌに対する実角加速度ｄｄθ＿ａｃｔの追従性を向上させることができる。

このように、制御部１４は、複数の関節１６のトルクの応答性を示す変位部トルク応答性行列Ｅｆｆ、複数のモータ３２の角度θによって決定された駆動部１２の慣性行列Ｍ、複数の関節１６の今回の目標角加速度ｄｄθ＿ｃｎｔｒｌ、及び複数の関節１６の前回の目標角加速度ｄｄθ＿ｃｎｔｒｌ＿ｐを用いて、複数の関節１６の実角加速度ｄｄθ＿ａｃｔが、サンプリング時間ＤＴ後に、複数の関節１６の今回の目標角加速度ｄｄθ＿ｃｎｔｒｌとなるように、複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌを算出するので、応答性を改善して複数の関節１６の駆動を制御することができ、複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌに対する複数の関節１６の実角加速度ｄｄθ＿ａｃｔの追従性を向上させることができる。

また、制御部１４は、複数の関節１６のトルクの応答性を示す変位部トルク応答性行列Ｅｆｆ、複数のモータ３２の角度θによって決定された駆動部１２の慣性行列Ｍ及びヤコビ行列Ｊ、リンク２０のうち、予め定められたタスク位置Ａの今回の目標変化加速度ｄｄｘ＿ｃｎｔｒｌ、及び前回のタスク位置Ａの目標変化加速度ｄｄｘ＿ｃｎｔｒｌ＿ｐを用いて、タスク位置Ａの実変化加速度ｄｄｘ＿ａｃｔが、サンプリング時間ＤＴ後に、タスク位置Ａの目標変化加速度ｄｄｘ＿ｃｎｔｒｌとなるように、複数の関節１６の制御トルクｍ＿τ＿ｃｎｔｒｌを算出するので、応答性を改善して複数の関節１６の駆動を制御することができ、複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌに対する複数の関節１６の実角加速度ｄｄθ＿ａｃｔの追従性を向上させることができる。従って、応答性を改善してタスク位置Ａの駆動を制御することができ、タスク位置Ａの目標変化加速度ｄｄｘ＿ｃｎｔｒｌに対するタスク位置Ａの実変化加速度ｄｄｘ＿ａｃｔの追従性を向上させることができる。

［変形例］
なお、上記実施の形態は、以下のように変形してもよい。

（変形例１）変形例１では、リンクに接続され、自身では駆動することができない物体のトルク応答を改善するというものである。

図６は、変形例１における駆動部１２を示す図である。駆動部１２は、両脚として用いられる２つのリンク２０と、該２つのリンク２０によって支持されるベース部８０とを有する。左側のリンクをリンク２０Ｌで表し、右側のリンクをリンク２０Ｒで表す。

図６では、リンク２０Ｌ、２０Ｒは、関節１６₁Ｌ、１６₁Ｒ、関節１６₂Ｌ、１６₂Ｒ、関節１６₃Ｌ、１６₃Ｒの３つの関節を有し、関節１６₁Ｌ、１６₁Ｒがベース部８０に接続されている構造とするが、リンク２０Ｌ、２０Ｒは、２つ、又は４つ以上の関節１６_iを有していてもよい。変形例１では、関節１６はｎ個あるものとし、リンク２０Ｌ、２０Ｒの各関節１６を１６_iで表す。

ベース部８０は、関節及び該関節を駆動するモータを有しないので自ら駆動することはないが、リンク２０Ｌ、２０Ｒの駆動によって、間接的に自在に駆動する。この図６から、以下の運動方程式が得られる。

実変位加速度ｄｄｂａｓｅ＿ａｃｔは、ベース部８０の各方向における実変位加速度ｄｄｂａｓｅ₁＿ａｃｔ、ｄｄｂａｓｅ₂＿ａｃｔ、・・・、ｄｄｂａｓｅ_n＿ａｃｔをベクトル表示したものである。各方向とは、例えば、ｘ軸方向、ｙ軸方向、ｚ軸方向、ｘ軸の回転方向、ｙ軸の回転方向、ｚ軸の回転方向等がある。力学補正力Ｃ＿ｃｍｐｎ＿ｂａｓｅは、遠心力やコリオリ力等のベース部８０の各方向に生じる力学補正力Ｃ₁＿ｃｍｐｎ＿ｂａｓｅ、Ｃ₂＿ｃｍｐｎ＿ｂａｓｅ、・・・、Ｃ_n＿ｃｍｐｎ＿ｂａｓｅをベクトル表示したものである。この力学補正力Ｃ＿ｃｍｐｎ＿ｂａｓｅは、力学補正決定部５４によって決定される。

Ｊｂａｓｅは、ベース部８０の各方向における変化速度とリンク２０Ｌ（２０Ｒ）の先端である足先（タスク位置Ａ）の変化速度との関係を示すヤコビ行列（係数行列）である。なお、Ｊは、各関節１６_iのモータ３２の角速度ｄθ_iとリンク２０Ｌ（２０Ｒ）の足先（タスク位置Ａ）の変化速度との関係を示すヤコビ行列である。このヤコビ行列Ｊｂａｓｅ、及びＪは、ベース部８０の位置、足先の位置、及び複数の関節１６の実角度θ＿ａｃｔ等に応じて、行列決定部５２によって決定される。

数２５からわかるように、図６の場合は、複数の関節１６のモータ３２の実角加速度ｄｄθ＿ａｃｔによって定まるトルク（Ｍ・ｄｄθ＿ａｃｔ）と、タスク位置Ａが地面から受ける力Ｆによって複数の関節１６に与えられるトルク（Ｊ^T・Ｆ）とによって、各関節１６_iのモータ３２の実トルクτ＿ａｃｔが定まる。ベース部８０の実変位加速度ｄｄｂａｓｅ＿ａｃｔによって定まるトルク（Ｍ・ｄｄｂａｓｅ＿ａｃｔ）と、タスク位置Ａが受ける力Ｆによってベース部８０の各方向に与えられるトルク（Ｊｂａｓｅ^T・Ｆ）によって、ベース部８０に与えられる実トルクは定まるが、ベース部８０は、モータを有しないので、実トルクは０となる。

数２５は、以下の関係式に変形することができる。

数２６は、以下の関係式で表すことができる。なお、複数の関節１６の実角加速度ｄｄθ＿ａｃｔ及びベース部８０の実変位加速度ｄｄｂａｓｅ＿ａｃｔを表すｄｄｑ＿ａｃｔを駆動部実変位加速度と呼び、τ＿ｘ＿ａｃｔ及びτ＿θ＿ａｃｔを表すτ＿ｔｏｔａｌ＿ａｃｔを総合実トルクと呼ぶ。

ここで、タスク位置Ａが受ける力Ｆは、数６に示す関係式と同様に、数２８で表すことができる。Ｆ＿Ｅｆｆは、タスク位置Ａである足先が受ける力の応答性を示す力応答性行列であり、その対角要素は、力応答係数であるｆ＿ｅｆｆ₁、ｆ＿ｅｆｆ₂、・・・、ｆ＿ｅｆｆ_nで表すことができる。Ｆ＿ｃｎｔｒｌは、目標となるタスク位置Ａが受ける力（目標力）であり、数２８を用いて、このＦ＿ｃｎｔｒｌに対してタスク位置Ａが実際に受ける力Ｆが定まる。従って、目標力Ｆに対する実際の力Ｆを測定することで、所望の力応答性行列Ｆ＿Ｅｆｆを設定する。この力応答性行列Ｆ＿Ｅｆｆは、応答性行列設定部５０によって設定される。

また、実トルク＿θ＿ａｃｔは、数６に示す関係式と同様に、数２９で表すことができる。τ＿θ＿ｃｎｔｒｌは、目標トルクであり、数２９を用いて、この目標トルクτ＿θ＿ｃｎｔｒｌに対する実トルクτ＿θ＿ａｃｔを求めることができる。

また、実トルクτ＿ｘ＿ａｃｔは、数６に示す関係式と同様に、数３０で表すことができる。τ＿ｘ＿ｃｎｔｒｌは目標トルクであり、数３０を用いて、この目標トルクτ＿ｘ＿ｃｎｔｒｌに対する実トルクτ＿ｘａｃｔを求めることができる。

数２７の実トルクτ＿θ＿ａｃｔ、実トルクτ＿ｘ＿ａｃｔに数２９、数３０をそれぞれ代入すると、以下の関係式で表すことができる。

また、一般的な運動方程式から、以下の関係式を得ることができる。

なお、目標変位加速度ｄｄｂａｓｅ＿ｃｎｔｒｌは、ベース部８０の各方向における目標変位加速度ｄｄｂａｓｅ₁＿ｃｎｔｒｌ、ｄｄｂａｓｅ₂＿ｃｎｔｒｌ、・・・、ｄｄｂａｓｅ_n＿ｃｎｔｒｌをベクトル表示したものである。また、複数の関節１６の目標角加速度ｄｄθ＿ｃｎｔｒｌ及びベース部８０の目標変位加速度ｄｄｂａｓｅ＿ｃｎｔｒｌを表すｄｄｑ＿ｃｎｔｒｌを、駆動部目標変位加速度と呼び、τ＿ｘ＿ｃｎｔｒｌ及びτ＿θ＿ｃｎｔｒｌを表すτ＿ｔｏｔａｌ＿ｃｎｔｒｌを総合目標トルクと呼ぶ。この駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌは、ベース部８０の状態、複数の関節１６等の種々に要因によって、目標値設定部５６によって設定される。

この数３１、数３２とから、数１１を導き出したのと同様の手順で、以下に示す関係式を得ることができる。

従って、制御トルク算出部５８は、目標値設定部５６から送られてきた駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌに基づいて、総合制御トルクｍ＿τ＿ｄｄｑ＿ｃｎｔｒｌを算出し、モータ制御部６０は、該求めた総合制御トルクｍ＿τ＿ｄｄｑ＿ｃｎｔｒｌに基づいて、複数のモータ３２を制御する。

このように、今回の駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌ、前回の駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌ＿ｐ、慣性行列Ｍ、力応答性行列Ｆ＿Ｅｆｆ、変位部トルク応答性行列Ｅｆｆ等によって、駆動部実変位加速度ｄｄｑ＿ａｃｔがサンプリング時間ＤＴ後に今回の駆動部目標変位加速度ｄｄｑ＿ｃｎｔｒｌとなるような、総合制御トルクｍ＿τ＿ｄｄｑ＿ｃｎｔｒｌを求めることができる。この総合制御トルクｍ＿τ＿ｄｄｑ＿ｃｎｔｒｌで、各関節１６_iのモータ３２を駆動制御することで、各関節１６_iは勿論、ベース部８０の駆動の応答性をも改善することができる。

（変形例２）上記実施の形態、及び、変形例１では、変位部として回転駆動する関節１６を用いて説明したが、変位部は、直進駆動（進退駆動）する進退部であってもよい。

図７は、変形例２の駆動部１２の変位部駆動系３０の他の構成を示す模式図である。変位部駆動系３０は、モータ３２と、駆動力変換部１００と、弾性体１０２とを有する動力伝達部１０４、弾性体１０２を介して変位部である進退部１０６が接続されている。なお、図２と同様の構成については同一の符号を付す。

駆動力変換部１００は、モータ３２の出力軸３２ａのネジ溝に嵌合されおり、出力軸３２ａが回転することで駆動力変換部１００が軸方向に矢印方向に移動する。駆動力変換部１００と進退部１０６とは、動力伝達部１０４を介して接続されている。従って、駆動力変換部１００の移動に応じて進退部１０６も矢印方向に移動する。なお、図７では動力伝達部１０４に弾性体１０２を設けたが、弾性体１０２を設けなくてもよい。なお、モータ３２にはモータ３２の角度を検出する角度センサ３６が、進退部１０６には進退部１０６のトルクを検出するトルクセンサ３８が設けられている。

関節１６を図２に示す様なモータ３２の回転によって進退可能な進退部１０６であっても、上記実施の形態及び変形例１で示した関係式（数２〜数３３）と同一の関係式が成り立つので、関節１６を進退部１０６に替えても応答性を改善することができる。なお、モータ３２の出力軸３２ａに減速機を接続し、減速機の出力軸のネジ溝に駆動力変換部１００を嵌合してもよい。

（変形例３）上記実施の形態及び上記変形例１では、モータ３２と関節１６との間には、弾性部材を設けないようにしたが、モータ３２と関節１６との間に弾性部材を設けてもよい。

図８は、変形例３の駆動部１２の変位部駆動系３０の他の構成を示す模式図である。変位部駆動系３０は、モータ３２、減速機３４、第１プーリ１１０、ワイヤ１１２、第２プーリ１１４、関節１６を備える。

減速機３４の出力軸３４ｂに、第１プーリ１１０の入力軸１１０ａが連結される。ワイヤ１１２は、円環状の弾性部材であり、第１プーリ１１０の回転力を第２プーリ１１４に伝達する。そして、第２プーリ１１４の出力軸１１４ａに関節１６が連結される。なお、モータ３２にはモータ３２の角度を検出する角度センサ３６が、第２プーリ１１４には、関節１６に与えられるトルクを検出するトルクセンサ３８が設けられている。

以上、本発明を実施の形態を用いて説明したが、本発明の技術的範囲は上記実施の形態に記載の範囲には限定されない。上記実施の形態に、多様な変更又は改良を加えることが可能であることが当業者に明らかである。その様な変更又は改良を加えた形態も本発明の技術的範囲に含まれ得ることが、特許請求の範囲の記載から明らかである。

１０…駆動装置 １２…駆動部
１４…制御部 １６…関節
１８…連結部 ２０…リンク
３０…変位部駆動系 ３２…モータ
３４…減速機 ３６…角度センサ
５０…応答性行列設定部 ５２…行列決定部
５６…目標値設定部 ５８…制御トルク算出部
６０…モータ制御部 ８０…ベース部
１００…駆動力変換部 １０２…弾性体
１０４…動力伝達部 １０６…進退部
１１０…第１プーリ １１２…ワイヤ
１１４…第２プーリ

Claims (6)

1. 複数の変位部を有するリンクと、前記複数の変位部を駆動する複数の回転駆動源とを有する駆動部と、
   前記駆動部を駆動制御する制御部と、
   を備える駆動装置において、
   前記制御部は、
   前記複数の変位部のトルクの応答性を示す変位部トルク応答性行列、前記複数の変位部の角度によって決定された前記駆動部の慣性行列、前記複数の変位部の今回の目標角加速度、及び前記複数の変位部の前回の目標角加速度を用いて、前記複数の変位部の実角加速度が、サンプリング時間後に、前記今回の目標角加速度となるように、前記複数の変位部の制御トルクを算出する制御トルク算出部を備える
   ことを特徴とする駆動装置。
2. 請求項１に記載の駆動装置において、
   前記変位部は、回転駆動する関節又は進退駆動する進退部である
   ことを特徴とする駆動装置。
3. 請求項１又は２に記載の駆動装置において、
   前記制御部は、下記の関係式を用いて、前記制御トルクを算出する
   ことを特徴とする駆動装置。
   

   
4. 請求項３に記載の駆動装置において、
   前記複数の関節の最小時定数を測定しておき、
   前記最小時定数を用いて、変位部トルク応答性行列の対角要素である変位部トルク応答係数を設定する
   ことを特徴とする駆動装置。
5. 請求項１〜４の何れか１項に記載の駆動装置において、
   前記制御トルク算出部は、
   前記リンクの予め定められたタスク位置の今回の目標変化加速度及び前回の目標変化加速度と、前記タスク位置の変化速度と前記複数の変位部の角速度との関係を示す係数行列とを用いて、前記複数の変位部の今回の目標角加速度、及び前記複数の変位部の前回の目標角加速度を算出することで、前記タスク位置の実変化加速度が、サンプリング時間後に、前記今回の目標変化加速度となるように、前記複数の変位部の制御トルクを算出する
   ことを特徴とする駆動装置。
6. 請求項１〜４の何れか１項に記載の駆動装置において、
   駆動部は、前記リンクに支持されるベース部を備え、
   前記制御トルク算出部は、
   前記リンクの先端であるタスク位置が地面から受ける力の応答性を示す力応答性行列と、ベース部の各方向における変化速度と前記リンクの前記タスク位置の変化速度との関係を示す係数行列と、前記タスク位置の変化速度と前記複数の変位部の角速度との関係を示す係数行列とを用いて、前記目標角加速度及び前記ベース部の各方向の目標変位加速度を含む今回の駆動部目標変位加速度と、前回の駆動部目標変位加速度とから、前記複数の変位部の実角加速度及び前記ベース部の各方向の実変位加速度を含む駆動部実変位加速度が、サンプリング時間後に、前記今回の駆動部目標変位加速度となるように、前記複数の変位部の制御トルクを算出する
   ことを特徴とする駆動装置。

Images