JP5752502B2 - ロボットの制御装置 - Google Patents

Description

本発明は、複数の関節を備えるロボットの制御装置に関する。

複数の関節を動かして所要の動作を行なうロボットの制御装置は、通常、各関節の動作状態（各関節の変位量や、各関節の駆動力）を制御するための操作量（制御入力）を逐次決定し、その操作量に応じて各関節の動作状態が電動モータ等のアクチュエータを介して制御される。

このようなロボットの制御処理においては、上記操作量を決定するために、ある線形写像を表現する行列（一般には正則行列とならないような行列）、例えばヤコビアン行列の擬似逆行列を算出し、この擬似逆行列を用いる演算処理を行なうことが必要となる場合が多々ある。

例えば、特許文献１には、ロボットの各関節の角度の変化分と、ＺＭＰの変化分とを関係づけるＺＭＰヤコビアン行列の擬似逆行列を用いて、ＺＭＰの速度からロボットの各関節の目標角速度を決定する技術が記載されている。

ここで、擬似逆行列を求めようとする行列をＡとおくと、その行列Ａの擬似逆行列（以降、これをＡ^*と表記する）は、一般に、次式（１）により算出される。なお、添え字“Ｔ”は転置を意味する。

Ａ^*＝Ａ^T・(Ａ・Ａ^T)^-1 ……（１）

ただし、この場合、式（１）の右辺に含まれる行列(Ａ・Ａ^T)の行列式det(Ａ・Ａ^T)の大きさがゼロもしくはゼロ近傍の微小値となるような場合には、式（１）の右辺の演算結果が発散し、適正な擬似逆行列Ａ^*を算出することができない。

これを防止するために、例えば非特許文献１にて提案されている如く、次式（２）により擬似行列Ａ^*を算出することが知られている。

Ａ^*＝Ａ^T・(Ａ・Ａ^T＋ｋ・Ｉ)^-1 ……（２）

この式（２）におけるｋは、同式（２）の右辺の括弧内の行列(＝Ａ・Ａ^T＋ｋ・Ｉ)の行列式det(Ａ・Ａ^T＋ｋ・Ｉ)（以降、これをＤＥＴと表記する）の大きさが小さくなり過ぎるのを防止するために、ゼロ以上の適当な実数値に設定される調整パラメータである。また、Ｉは単位行列である。

上記式（２）により擬似逆行列Ａ^*を求めるようにした場合には、調整パラメータｋの値を適切に設定することで、上記行列式ＤＥＴの大きさが小さくなり過ぎるのを防止し、ひいては、擬似逆行列Ａ^*が発散するのを防止することが可能となる。

特開２００６−１５０５６７号公報

Nakamura Y. and Hanafusa H.，"InverseKinematic Solutions with Singularity Robustness for Robot Manipulator Control"，Journal of Dynamic Systems，Measurement and Contro，Vol.108，Sept，(1986).pp.163-171.

ところで、前記行列式ＤＥＴ（≡det(Ａ・Ａ^T＋ｋ・Ｉ)）の大きさが小さくなり過ぎないようにするための適切な調整パラメータｋの値は行列Ａに依存して変化する。また、行列式ＤＥＴの大きさは、ｋの値の変化に対して非線形な変化を呈する。

このため、上記行列式ＤＥＴの絶対値が前記所定の閾値以上となる（過小にならない）ようにし得る調整パラメータｋの値は、一般には、探索的な処理によって決定されることとなる。

例えば、ｋの暫定値をあらかじめ定めた所定量ずつ、所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（２）により擬似逆行列Ａ^*を算出するために用いる調整パラメータｋの値として決定する。

しかるに、このように調整パラメータｋの値を探索的に決定する処理においては、ｋの暫定値の増加量を一定値としておくと、次のような不都合が生じることが本願発明者の検討により判明した。

すなわち、擬似逆行列Ａ^*は、ロボットの各関節の動作状態を制御するための操作量を逐次決定する演算処理で使用されるものであるから、被疑逆行列Ａ^*を算出するために使用する調整パラメータｋの値は、制御装置の各制御処理周期において、迅速に決定することが望ましい。

しかるに、適切なｋの値をできるだけ迅速に決定し得るようにするために、ｋの暫定値の増加量を比較的大きめの値に設定しておくと、ロボットの制御装置の制御処理周期毎に決定されるｋの値が頻繁に変動しやすい。ひいては、このｋの値を用いて制御処理周期毎に算出される擬似逆行列Ａ^*の不連続的な変動が生じるために、ロボットの各関節の動作状態を制御するための操作量も不連続的な変動を生じやすい。その結果、ロボットの動作の滑らかさが損なわれてしまうという不都合がある。

そして、制御処理周期毎に決定されるｋの値が頻繁に変動することを抑制するために、上記と逆に、ｋの暫定値の増加量を比較的小さめの値に設定しておくと、適切なｋの値が最終的に決定されるまでに時間がかかり過ぎる恐れがある。ひいては、ロボットの制御装置の制御処理周期内で適切なｋの値を決定することができなくなる恐れがある。その結果、ロボットの制御装置の制御処理周期の短縮化を図ることが困難となり、ひいては、ロボットの素早い動作を行なうことが困難となるという不都合がある。

本発明は、かかる背景に鑑みてなされたものであり、ロボットの各関節の動作状態の制御のための操作量を、擬似逆行列を用いる演算処理によって決定する場合に、その擬似逆行列を算出するために用いる適切なパラメータの値を効率よく短時間で決定するようにすることができる装置を提供することを目的とする。

本発明のロボットの制御装置は、上記の目的を達成するために、複数の関節を介して相互に連結された複数のリンク要素を備えるロボットの制御装置であって、
前記ロボットのあらかじめ定められた所定種類の状態量の値を目標値に制御するための操作量としてのベクトルであって、Ｍ個（Ｍ：１以上の整数）の成分により構成される基本操作量ベクトル↑ａを少なくとも前記所定種類の状態量の目標値に応じて逐次決定する基本操作量決定手段と、
前記ロボットの各関節の動作状態を制御するための操作量としてのベクトルであって、Ｎ個（Ｎ：Ｎ≧Ｍとなる整数）の成分により構成されると共に前記基本操作量ベクトル↑ａとの間に次式（３）により示される線形写像の関係を有する関節操作量ベクトル↑ｂを、前記決定された基本操作量ベクトル↑ａの値に、次式（３）の行列Ａの擬似逆行列Ａ^*を乗じることにより逐次決定する関節操作量決定手段と、

↑ａ＝Ａ・↑ｂ ……（３）

少なくとも前記決定された関節操作量ベクトル↑ｂに応じて前記ロボットの各関節の動作状態をアクチュエータを介して制御する関節制御手段とを備えており、
前記関節操作量決定手段は、少なくとも前記ロボットの各関節の現在の動作状態に応じて前記行列Ａを決定する行列決定手段と、その決定された行列Ａとあらかじめ設定された重み係数行列Ｗ（Ｗ：対角行列）と調整パラメータｋ（ｋ：ゼロ以上の実数値）の値とを用いて次式（４）により前記擬似逆行列Ａ^*を算出する擬似逆行列算出手段と、該式（４）の演算に用いる前記調整パラメータｋの値を、次式（５）により表される行列式ＤＥＴの絶対値が所定の閾値以上の値になるように決定する調整パラメータ決定手段とを備えており、

Ａ^*＝Ｗ^-1・Ａ^T・(Ａ・Ｗ^-1・Ａ^T＋ｋ・Ｉ)^-1 ……（４）
ＤＥＴ＝det(Ａ・Ｗ^-1・Ａ^T＋ｋ・Ｉ) ……（５）

前記調整パラメータ決定手段は、前記調整パラメータｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（４）の演算に用いる擬似逆行列演算パラメータｋの値として決定する手段であると共に、前記判断結果が否定的である場合のｋの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ａ・Ｗ^-1・Ａ^Tの次数）に比例する値に設定することを特徴とする（第１発明）。

なお、上記式（４）における“Ｉ”は単位行列である。また、重み係数行列Ｗは、各関節の動作状態の制御に応じた前記所定種類の状態量の変化の応答性や感度を調整するものである。この重み係数行列Ｗは単位行列であってもよい。該重み係数行列Ｗとして単位行列を採用した場合には、上記式（４）、（５）は、それぞれ、次式（４ａ），（５ａ）と等価になる。

Ａ^*＝Ａ^T・(Ａ・Ａ^T＋ｋ・Ｉ)^-1 ……（４ａ）
ＤＥＴ＝det(Ａ・Ａ^T＋ｋ・Ｉ) ……（５ａ）

従って、第１発明は、上記式（４ａ），（５ａ）によりそれぞれ擬似逆行列Ａ^*、行列式ＤＥＴを算出する場合の態様を包含するものである。このことは、後述する第５発明においても同様である。

上記第１発明によれば、前記基本操作量決定手段が、前記ロボットの所定種類の状態量の値を目標値に制御するための基本操作量ベクトル↑ａを少なくとも前記所定種類の状態量の目標値に応じて逐次決定する。この基本操作量ベクトル↑ａは、前記式（３）により表される線形写像によって、ある関節操作量ベクトル↑ｂの関数値となるベクトルである。

なお、本明細書では、“↑”は、ベクトル（縦ベクトル）を表現する記号として用いる。

そして、前記関節操作量決定手段が、上記基本操作量ベクトル↑ａから関節操作量ベクトル↑ｂを逐次決定する。より詳しくは、関節操作量決定手段は、行列Ａと調整パラメータｋとをそれぞれ、前記行列決定手段、前記調整パラメータ決定手段により決定した上で、前記擬似逆行列算出手段によって、それらの行列Ａと調整パラメータｋと前記重み係数行列Ｗとを用いて前記式（４）の演算を行なうことで、擬似逆行列Ａ^*を決定する。さらに、関節操作量決定手段は、擬似逆行列Ａ^*を上記基本操作量ベクトル↑ａに乗じることによって、関節操作量ベクトル↑ｂを決定する。

そして、前記関節制御手段が、上記の如く決定された関節操作量ベクトル↑ｂに応じて前記ロボットの各関節の動作状態をアクチュエータを介して制御する。これにより、前記所定種類の状態量の値が目標値に制御されるように、ロボットの各関節の動作状態が制御されることとなる。

かかる制御処理において、関節操作量決定手段は、瞬時瞬時の関節操作量ベクトル↑ｂを決定するために用いる擬似逆行列Ａ^*を算出するために、前記調整パラメータｋの値を調整パラメータ決定手段により決定する。

より詳しくは、調整パラメータ決定手段は、前記調整パラメータｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値（＞０）以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（４）の演算に用いる擬似逆行列演算パラメータｋの値として決定する。これにより、行列式ＤＥＴの絶対値を前記所定の閾値以上の値とし得るような調整パラメータｋの値が探索的に決定される。

ここで、本願発明者の知見によれば、Ａ・Ｗ^-1・Ａ^Tの次数をｎとしたとき、行列式ＤＥＴの値は、ｋの値のｎ乗に比例して変化する。

そこで、第１発明では、前記調整パラメータ決定手段の処理において、前記判断結果が否定的である場合のｋの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ａ・Ｗ^-1・Ａ^Tの次数）に比例する値に設定することとした。

これにより、第１発明によれば、制御装置の制御処理周期毎に、擬似逆行列Ａ^*を算出するために用いる適切な調整パラメータｋの値（ＤＥＴの絶対値が所定の閾値以上となるｋの値）を効率よく短時間で決定できると共に、擬似逆行列Ａ^*が滑らかに変化していくようにすることができる。ひいては、ロボットの各関節の動作状態を滑らかに変化させるように、前記関節操作量ベクトル↑ｂを決定できることとなる。

また、第１発明によれば、前記重み係数行列Ｗによって、各関節の動作状態の制御に応じた前記所定種類の状態量の変化の応答性や感度を調整することができる。

上記第１発明においては、前記所定種類の状態量として、種々様々な状態量を採用することができる。例えば、該所定種類の状態量は、前記ロボットの特定部位の運動の状態量と、前記ロボットに作用する外力とのうちの少なくともいずれか一方を含むものとすることができる（第２発明）。

この第２発明によれば、ロボットの特定部位の運動の状態量（例えば、特定部位の空間的な位置、姿勢、あるいは、それらの変化速度）や、ロボットに作用する外力（例えば床反力等の接触力）を所望の目標値に制御することができる。なお、上記特定部位は、ロボットを構成する任意のリンク要素でよいことはもちろんであるが、ロボットの全体重心点であってもよい。

また、上記第１発明又は第２発明においては、前記基本操作量ベクトル↑ａ及び関節操作量ベクトル↑ｂとしては、例えば次のような形態の操作量ベクトルを採用することができる。

すなわち、一例として、前記基本操作量ベクトル↑ａは、前記ロボットの特定部位の運動状態を規定する操作量成分により構成されたベクトルとされ、前記関節操作量ベクトル↑ｂは、前記ロボットの各関節の変位量を規定する操作量成分により構成されたベクトルとされる（第３発明）。

あるいは、他の例として、前記基本操作量ベクトル↑ａは、前記ロボットに作用する外力を規定する操作量成分により構成されたベクトルとされ、前記関節操作量ベクトル↑ｂは、前記ロボットの各関節の駆動力を規定する操作量成分により構成されたベクトルとされる（第４発明）。

上記第３発明によれば、前記ロボットの各関節の変位量の制御（所謂、位置制御）によって、該ロボットの所望の動作を実現できる。また、第４発明によれば、前記ロボットの各関節の駆動力の制御（所謂、力制御）によって、該ロボットの所望の動作を実現できる。

ところで、ロボットの制御装置における演算処理で算出することが必要となる擬似逆行列は、前記基本操作量ベクトル↑ａと関節操作量ベクトル↑ｂとの間の関係を直接的に規定する行列Ａの擬似逆行列Ａ^*に限られるものではなく、ロボットの各関節の動作状態を制御するための操作量を決定する過程の中間的な演算処理において、ある線形写像を表す行列の擬似逆行列を算出することが必要となる場合もある。

そして、前記第１発明に関して説明した擬似逆行列の決定技術は、前記第１発明における行列Ａ以外の行列の擬似逆行列を算出する場合にも適用できる技術である。

そこで、本発明のロボットの制御装置は、より一般化した態様として、複数の関節を介して相互に連結された複数のリンク要素を備えるロボットの制御装置であって、
線形写像を表す行列Ａの擬似逆行列Ａ*を使用する演算処理によって、前記ロボットの
各関節の動作状態を制御するための操作量を逐次決定する関節操作量決定手段と、
決定された前記操作量に応じて前記ロボットの各関節の動作状態をアクチェータを介して制御する関節制御手段とを備えており、
前記関節操作量決定手段は、前記行列Ａとあらかじめ設定された重み行列Ｗ（Ｗ：対角行列）と調整パラメータｋ（ｋ：ゼロ以上の実数値）の値とを用いて前記式（４）により前記擬似逆行列Ａ*を算出する擬似逆行列算出手段と、該式（４）の演算に用いる前記調
整パラメータｋの値を、前記式（５）により表される行列式ＤＥＴの絶対値が所定の閾値以上の値になるように決定する調整パラメータ決定手段とを備えており、
前記調整パラメータ決定手段は、前記調整パラメータｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（４）の演算に用いる擬似逆行列演算パラメータｋの値として決定する手段であると共に、前記判断結果が否定的である場合のｋの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ａ・Ｗ^-1・Ａ^Tの次数）に比例する値と当該増加量についての所定の下限値とのうちのより大きい方の値に設定することを特徴とする（第５発明）。

上記第５発明によれば、前記関節操作量決定手段は、線形写像を表す行列Ａの擬似逆行列Ａ^-1を使用する演算処理によって、前記ロボットの各関節の動作状態を制御するための操作量を逐次決定する。より詳しくは、関節操作量決定手段は、前記擬似逆行列算出手段によって、上記行列Ａと調整パラメータｋと前記重み係数行列Ｗとを用いて前記式（４）の演算を行なうことで、擬似逆行列Ａ^*を決定する。さらに、関節操作量決定手段は、擬似逆行列Ａ^*を使用する演算処理によって、前記ロボットの各関節の動作状態を制御するための操作量を決定する。

そして、前記関節制御手段が、上記の如く決定された操作量に応じて前記ロボットの各関節の動作状態をアクチュエータを介して制御する。これにより、ロボットの各関節の動作状態が制御されることとなる。

かかる制御処理において、関節操作量決定手段は、瞬時瞬時の前記操作量を決定するために用いる擬似逆行列Ａ^*を算出するために、前記調整パラメータｋの値を調整パラメータ決定手段により決定する。

この調整パラメータ決定手段の処理は、前記第１発明と同様に行なわれる。ただし、前記判断結果が否定的である場合のｋの暫定値の増加量は、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ａ・Ｗ ^-1 ・Ａ ^T の次数）に比例する値と当該増加量についての所定の下限値とのうちのより大きい方の値に設定される。これにより、第５発明によれば、第１発明と同様に、制御装置の制御処理周期毎に、擬似逆行列Ａ*を算出するために用いる適切な調整パラメータｋの値（ＤＥＴの絶対値が所定の閾値以上となるｋの値）を効率よく短時間で決定できると共に、擬似逆行列Ａ*が滑らかに変化していくようにすることができる。ひいては、ロボットの各関節の動作状態を滑らかに変化させるように、前記操作量を決定できることとなる。

また、第５発明によれば、前記重み係数行列Ｗによって、各関節の動作状態の制御に応じたロボットの動作の変化の応答性や感度を調整することができる。また、調整パラメータ決定手段の処理において、前記ｋの暫定値の増加量が過剰に小さい値になるのが防止される。

補足すると、以上説明した本発明におけるロボットは、設置型のロボット及び移動ロボットのいずれの形態のロボットであってもよい。

本発明の実施形態におけるロボットの概略構成を示す図。 図１のロボットの制御に関する構成を示すブロック図。 図２に示す制御装置の第１実施形態における機能を示すブロック図。 図３に示す擬似逆行列算出部における処理を示すフローチャート。 図２に示す制御装置の第２実施形態における機能を示すブロック図。 図５に示す基本操作量決定部における処理を示すフローチャート。 図５に示す擬似逆行列算出部における処理を示すフローチャート。

［第１実施形態］
本発明の第１実施形態を図１〜図４を参照して以下に説明する。

図１を参照して、本実施形態で例示するロボット１は、設置型の作業ロボットである。このロボット１は、床上に設置された基台２から順番に連接された複数（Ｎ個）のアーム状の要素リンク３(1)〜３(N)と、これらの基台２及び要素リンク３(1)〜３(N)の相互の連結部分に設けられた複数（Ｎ個）の関節４(1)〜４(N)とを備えている。そして、最も先端側の要素リンク３(N)の先端部には、ハンド部５が取付けられている。

要素リンク３(1)〜３(N)及び関節４(1)〜４(N)の個数Ｎは、例えば６個である。ただし、それらの個数Ｎは、６個に限られるものではなく、他の個数（例えば４個、７個等）であってもよい。

以降の説明では、要素リンク３(1)〜３(N)のそれぞれを区別する必要がないときは、要素リンク３(1)〜３(N)のそれぞれを総称的に要素リンク３(i)（ｉ＝１，２，…，Ｎ）又は要素リンク３という。同様に、関節４(1)〜４(N)のそれぞれを区別する必要がないときは、関節４(1)〜４(N)のそれぞれを総称的に関節を４(i)（ｉ＝１，２，…，Ｎ）又は関節４という。

詳細な図示は省略するが、各関節４は、本実施形態では、１軸周りの回転自由度を有する公知の構造の関節である。各関節４は、それぞれに対応してロボット１に備えられている関節アクチュエータ６（図２に示す）から、図示しない減速機等の動力伝達機構を介して駆動力（回転駆動力）が伝達されるように、該関節アクチュエータ６に接続されている。各関節アクチュエータ６は、例えば電動モータにより構成されたアクチュエータである。

そして、関節４のそれぞれを関節アクチュエータ６により駆動することで、ハンド部５の空間的な運動が行なわれるようになっている。このハンド部５の運動によって、該ハンド部５を接触させた対象物Ｗを動かす等の作業を行なうことが可能となっている。

なお、関節アクチュエータ６は、電動モータに限らず、油圧式のアクチュエータにより構成されていてもよい。さらには、関節アクチュエータ６は、回転型に限らず、直動型のアクチュエータであってもよい。

ロボット１の動作制御のための構成として、図２に示すように、各関節４の変位量（実施形態では回転角度）を計測するための関節変位センサ７と、ハンド部５に作用する外力を計測するための力センサ８と、制御装置９とが備えられている。

関節変位センサ７は、各関節４（又は各関節アクチュエータ６）に搭載されたロータリエンコーダやポテンショメータ等のセンサにより構成され、各関節４の変位量（回転角度）に応じた信号を制御装置９に出力する。また、力センサ８は、例えば要素リンク３(N)の先端部とハンド部５との間に介装された６軸力センサにより構成され、ハンド部５に作用する力（並進力及びモーメント）に応じた信号を制御装置９に出力する。なお、力センサ８は、特定成分の力（例えば１軸方向の並進力等）だけを検出するセンサであってもよい。

制御装置９は、図示しないＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等から構成された電子回路ユニットである。図３に示すように、この制御装置９は、実装されたプログラム等により実現される機能として、ロボット１の基準動作目標を出力する基準動作目標出力部１０と、ロボット１の所定種類の状態量の実際の値（実状態量）を所要の目標値に制御する（追従させる）ための制御入力（操作量）としての基本操作量ベクトル↑ΔＸdmdを決定する基本操作量決定部１１と、この基本操作量ベクトル↑ΔＸdmdから、各関節４の変位量（本実施形態では回転角度）を制御するための制御入力（操作量）としての関節操作量ベクトル↑ΔΘdmdを決定する関節操作量決定部１２と、基準動作目標により規定される関節４(1)〜４(N)のそれぞれの変位量の基準目標値θ(1)cmd0〜θ(N)cmd0（以下、基準目標変位量θ(1)cmd0〜θ(N)cmd0という）を関節操作量ベクトル↑ΔΘdmdにより修正してなる修正後目標変位量θ(1)cmd1〜θ(N)cmd1を関節４(1)〜４(N)のそれぞれの変位量の最終的な指令値（以下、関節変位指令という）として決定し、この関節変位指令θ(1)cmd1〜θ(N)cmd1に関節４(1)〜４(N)のそれぞれの実際の変位量（実変位量）を追従させるように各関節アクチュエータ６を図示しない駆動回路を介して制御する関節制御部１５とを備える。そして、制御装置９は、これらの機能部の処理を所定の制御処理周期で逐次実行することによって、ロボット１の動作制御を行う。

以下に、ハンド部５により対象物Ｗを動かす作業をロボット１に行わせる場合を例にとって、上記各機能部の詳細な処理を含めて制御装置９の制御処理を説明する。

制御装置９の各制御処理周期において、基準動作目標出力部１０からロボット１の基準動作目標が出力される。この基準動作目標は、本実施形態では、ロボット１に所要の作業を行わせるためにあらかじめ作成されたティーチングデータである。該基準動作目標は、例えば、ロボット１のＮ個の関節４(1)〜４(N)のそれぞれの基準目標変位量θ(1)cmd0〜θ(N)cmd0の軌道と、ハンド部５の位置↑Ｘの基準の目標位置である基準目標ハンド部位置↑Ｘcmd0の軌道と、ハンド部５を接触させる対象物Ｗからハンド部５に作用する外力↑Ｆ（接触力）の基準の目標値である基準目標ハンド部外力↑Ｆcmd0の軌道とから構成される。

なお、上記「軌道」は瞬時値の時系列を意味する。また、以降の説明では、ロボット１のＮ個の関節４(1)〜４(N)のそれぞれの基準目標変位量θ(1)cmd0〜θ(N)cmd0を並べたベクトル（縦ベクトル）を基準目標変位量ベクトル↑Θcmd0（≡[θ(1)cmd0，θ(2)cmd0，……，θ(N)cmd0]^T）と称する。

基準動作目標の各構成要素は、制御装置９の記憶装置にあらかじめ記憶保持され、あるいは、外部から無線通信により制御装置９に与えられる。そして、基準動作目標出力部１０は、記憶装置から読み出した基準動作目標の各構成要素、あるいは、外部から与えられた基準動作目標の各構成要素を逐次出力する。

ここで、基準目標ハンド部位置↑Ｘcmd0は、より詳しくは、ハンド部５の代表点（ハンド部５に対して固定された点）の空間的な位置の目標を意味している。そして、基準目標ハンド部位置↑Ｘcmd0は、その位置等を表現するための座標系としてあらかじめ定められた慣性座標系（図１を参照）で見た３成分の位置ベクトル（縦ベクトル）として表記されるものである。該慣性座標系は、本実施形態では、ロボット１が設置された床に対して固定された３軸座標系（ＸＹＺ座標系）である。

また、基準目標ハンド部外力↑Ｆcmd0は、本実施形態では、より詳しくは、ハンド部５に対象物Ｗから作用する並進力（ベクトル）の目標であり、上記慣性座標系で見た３成分のベクトル（縦ベクトル）として表記されるものである。

補足すると、基準目標ハンド部位置↑Ｘcmd0の軌道は、Ｎ個の関節４(1)〜４(N)のそれぞれの基準目標変位量θ(1)cmd0〜θ(N)cmd0の組、すなわち基準目標変位量ベクトル↑Θcmd0の軌道から幾何学的演算によって一義的に算出し得るものである。従って、↑Ｘcmd0の軌道は、基準動作目標の構成要素から除外するようにしてもよい。また、↑Θcmd0、↑Ｘcmd0、↑Ｆcmd0のそれぞれの軌道の代わりに、その軌道を規定する演算式のパラメータ等を基準動作目標の構成要素とするようにしてもよい。

次いで、制御装置９は、基本操作量決定部１１の処理を実行する。この基本操作量決定部１１には、基準動作目標出力部１０から出力される基準動作目標の構成要素のうちの基準目標ハンド部位置↑Ｘcmd0と基準目標ハンド部外力↑Ｆcmd0とが入力されると共に、各関節４毎の前記関節変位センサ７の出力により示される関節４(1)〜４(N)のそれぞれの実変位量θ(1)act〜θ(N)actの計測値と、前記力センサ８の出力により示される実外力↑Ｆact（ハンド部５に作用する実際の並進外力）の計測値とが入力される。以降、ロボット１の関節４(1)〜４(N)のそれぞれの実変位量θ(i)act（ｉ＝１，２，…，Ｎ）を並べたベクトル（縦ベクトル）を実変位量ベクトル↑Θact（≡[θ(1)act，θ(2)act，……，θ(N)act]^T）と称する。

なお、基準目標ハンド部位置↑Ｘcmd0の代わりに、基準目標変位量ベクトル↑Θcmd0を基本操作量決定部１１に入力し、この↑Θcmd0から↑Ｘcmd0を基本操作量決定部１１で算出するようにしてもよい。

基本操作量決定部１１は、入力されたデータを用いて、ロボット１の所定種類の状態量の実際の値（実状態量）を、所要の目標値に制御する（追従させる）ための基本操作量ベクトル↑ΔＸdmdを決定する。

本実施形態では、制御の対象たる上記所定種類の状態量として、例えば、ハンド部５の位置↑Ｘと、ハンド部５に作用する外力↑Ｆとの組を用いる。そして、基本操作量決定部１１が決定する基本操作量ベクトル↑ΔＸdmdは、ハンド部５の位置↑Ｘの修正量である。従って、この基本操作量ベクトル↑ΔＸdmdは、基準目標ハンド部位置↑Ｘcmd0と同様に、前記慣性座標系の３成分のベクトルとして表現されるものである。

本実施形態では、基本操作量決定部１１は、ハンド部５の実際の位置↑Ｘactと、ハンド部５に作用する実外力↑Ｆactとを、それぞれ基準目標ハンド部位置↑Ｘcmd0、基準目標ハンド部外力↑Ｆcmd0に追従させるように、制御入力（操作量）としての基本操作量ベクトル↑ΔＸdmdを決定する。

具体的には、基本操作量決定部１１は、基本操作量ベクトル↑ΔＸdmdを、次式（６ａ），（６ｂ）により決定される操作量成分↑ΔＸａ，↑ΔＸｂを次式（６）で示す如く加え合わせることによって決定する。

↑ΔＸdmd＝↑ΔＸａ＋↑ΔＸｂ ……（６）
ただし、
↑ΔＸａ＝Ｋp1・(↑Ｘcmd0−↑Ｘact)＋Ｋv1・(↑Ｘcmd0'−↑Ｘact')
……（６ａ）
↑ΔＸｂ＝Ｋs1・↑Ｆcmd0＋Ｋc1・(↑Ｆcmd0−↑Ｆact)
＋Ｋe1・∫(↑Ｆcmd0−↑Ｆact)dt ……（６ｂ）

ここで、式（６ａ）により算出される操作量成分↑ΔＸａは、本実施形態における所定種類の状態量のうちのハンド部５の実際の位置↑Ｘactを、基準目標ハンド部位置↑Ｘcmd0に追従させるための操作量である。

この場合、式（６ａ）の右辺第１項は、↑Ｘcmd0と↑Ｘact（計測値）との偏差に所定の比例ゲインＫp1を乗じてなる比例項、右辺第２項は、↑Ｘcmd0の時間的変化率↑Ｘcmd0'（ハンド部５の目標並進速度）と↑Ｘactの時間的変化率↑Ｘact'（ハンド部５の実際の並進速度）との偏差に所定の微分ゲインＫv1を乗じてなる微分項である。

従って、本実施形態では、操作量成分↑ΔＸｂは、↑Ｘcmd0と↑Ｘact（計測値）との偏差をゼロに近づけるようにフィードバック制御則としてのＰＤ則（比例・微分則）により決定されるフィードバック操作量である。

なお、式（６ｂ）の演算に使用する↑Ｘactの計測値は、関節４の実変位量ベクトル↑Θactの計測値から算出される。また、↑Ｘcmd0'、↑Ｘact'はそれぞれ、↑Ｘcmd0の時系列、↑Ｘactの計測値の時系列から算出される。また、Ｋp1、Ｋv1は、それぞれ、その値があらかじめ定められたスカラー又は対角行列である。

また、式（６ｂ）により算出される操作量成分↑ΔＸｂは、本実施形態における所定種類の状態量のうち、ハンド部５に作用する実外力↑Ｆactを、基準目標ハンド部外力↑Ｆcmd0に追従させるための操作量である。

この場合、式（６ｂ）の右辺第１項は、↑Ｆcmd0に所定のフードフォワードゲインＫs1を乗じてなるフィードフォワード項である。また、式（６ｂ）の右辺第２項は、↑Ｆcmd0と↑Ｆactの計測値との偏差に所定の比例ゲインＫc1を乗じてなる比例項、右辺第３項は、↑Ｆcmd0と↑Ｆactの計測値との偏差の積分値に所定の積分ゲインＫe1を乗じてなる積分項である。

従って、本実施形態では、操作量成分↑ΔＸａは、↑Ｆcmd0に応じたフィードフォワード項と、↑Ｆcmd0と↑Ｆactの計測値との偏差をゼロに近づけるようにフィードバック制御則としてのＰＩ則（比例・積分則）により求めたフィードバック項とを合成してなる操作量である。

なお、式（６ｂ）の演算に使用するＫs1、Ｋc1、Ｋe1は、それぞれ、その値があらかじめ定められたスカラー又は対角行列である。

基本操作量決定部１１は、上記の如く決定した操作量成分↑ΔＸａと↑ΔＸｂとを加え合わせたものを基本操作量ベクトル↑ΔＸdmdとして決定する。

これにより、基本操作量ベクトル↑ΔＸdmdは、ハンド部５の実際の位置↑Ｘactと、実外力↑Ｆactとをそれぞれ、基準目標ハンド部位置↑Ｘcmd0、基準目標ハンド部外力↑Ｆcmd0に追従させるように機能する操作量ベクトルとして決定されることとなる。

補足すると、例えば操作量成分↑ΔＸａ，↑ΔＸｂの一方を省略し、他方を基本操作量ベクトル↑ΔＸdmdとして決定するようにしてもよい。基本操作量ベクトル↑ΔＸdmdは、ロボット１の制御したい状態量に応じて適宜決定するようにすればよい。

さらには、ハンド部５の位置↑Ｘや外力↑Ｆの目標値は、それぞれ、必要に応じて↑Ｘcmd0、↑Ｆcmd0から動的にずらすようにしてもよい。

また、操作量成分↑ΔＸａをＰＤ則以外のフィードバック制御則、例えば比例則により決定するようにしてもよい。また、操作量成分↑ΔＸｂのフィードバック項をＰＩ則以外のフィードバック制御則、例えば比例則により決定するようにしてもよい。

また、本実施形態では、制御しようとする所定種類の状態量をハンド部５の位置↑Ｘ及び外力↑Ｆ（並進外力）としたが、ハンド部５の姿勢や、ハンド部５に作用する外力のうちのモーメント成分を当該状態量に含めるようにしてもよい。

上記の如く基本操作量決定部１１の処理を実行した後、次に制御装置９は、関節操作量決定部１２の処理を実行する。

この関節操作量決定部１２には、各関節４毎の関節変位センサ７の出力により示される関節４(1)〜４(N)のそれぞれの実変位量θ(1)act〜θ(N)actの計測値（実変位量ベクトル↑Θactの計測値）と、基本操作量決定部１１で決定された基本操作量ベクトル↑ΔＸdmdとが入力される。

そして、関節操作量決定部１２は、入力されたデータを用いて、関節４(1)〜４(N)のそれぞれの変位量を制御するための関節操作量ベクトル↑ΔΘdmdを決定する。この関節操作量ベクトル↑ΔΘdmdは、本実施形態では、ロボット１のＮ個の関節４(1)〜４(N)のそれぞれの変位量の修正量（増減操作量）Δθ(1)〜Δθ(N)を並べたベクトル（≡[Δθ(1)，Δθ(2)，……，Δθ(N)]^T）である。

関節操作量決定部１２は、その処理の構成要素として、ヤコビアン行列決定部１３と、擬似逆行列算出部１４とを備えている。そして、関節操作量決定部１２は、まず、これらのヤコビアン行列算出部１３、擬似逆行列算出部１４の処理を順次実行する。

ヤコビアン行列決定部１３は、関節４(1)〜４(N)の変位量θ(1)〜θ(N)を並べたベクトル（縦ベクトル）である関節変位量ベクトル↑Θ（≡[θ(1)，θ(2)，……，θ(N)]^T）の単位時間当たりの微小変化量↑ΔΘ（＝d↑Θ/dt）と、それに応じたハンド部５の位置↑Ｘの単位時間当たりの変化量↑ΔＸ（＝d↑Ｘ/dt）との間の線形写像関係を、次式（７）により表す行列としてのヤコビアン行列Ｊを決定する処理を実行するものである。

↑ΔＸ＝Ｊ・↑ΔΘ ……（７）

なお、↑ΔΘは、より詳しくは、ロボット１の現在の動作状態を起点とする単位時間当たりの変化量である。

かかるヤコビアン行列Ｊは、本実施形態では、各関節４毎の関節変位センサ７の出力により示される関節４(1)〜４(N)のそれぞれの実変位量θ(1)act〜θ(N)actの現在の計測値（実変位量ベクトル↑Θactの現在の計測値）を関節変位量ベクトル↑Θの微小変化（摂動）の起点として、公知の手法によって決定される。例えば、ロボット１の幾何学モデル（剛体リンクモデル）を用いて、あるいは、解析的な演算によって、関節変位量ベクトル↑Θの各成分の微小変化（現在の実変位量の計測値からの微小変化）に対するハンド部５の位置↑Ｘの変化を算出することで、ヤコビアン行列Ｊが決定される。

なお、本実施形態では、↑ΔＸは、ハンド部５の空間的な位置の変化量であるから３次のベクトルである。さらに、↑ΔΘは、Ｎ次（本実施形態では例えば６次）のベクトルである。従って、ヤコビアン行列Ｊは正方行列ではない。

上記のごとくヤコビアン行列決定部１３により決定されたヤコビアン行列Ｊの擬似逆行列Ｊ^*が、次に、擬似逆行列算出部１４の処理によって算出される。

この場合、擬似逆行列算出部１４は、前記式（４）の行列Ａにヤコビアン行列Ｊを代入してなる式によって算出される重み付きの擬似逆行列（重み付きのＳＲ−Inverse）Ａ^*を、ヤコビアン行列Ｊの擬似逆行列Ｊ^*として決定する。

具体的には、擬似逆行列Ｊ^*を算出する処理は、式（４）の演算に必要な調整パラメータｋの値の決定処理を含めて、図４のフローチャートに示す如く実行される。

まずＳＴＥＰ１において、式（４）の行列Ａとしてヤコビアン行列Ｊが代入されると共に、該行列Ａの転置行列Ａ^Tと、重み係数行列Ｗの逆行列Ｗ^-1とが算出される。なお、重み係数行列Ｗは、本実施形態ではその各対角成分（重み係数）の値があらかじめ定められた対角行列である。ただし、ロボット１の動作状態に応じて適宜、重み係数行列Ｗの各対角成分の値を変化させるようにしてもよい。

次いでＳＴＥＰ２において、調整パラメータｋの候補値の初期値として、ｋの値の標準値であるゼロが設定される。

次いで、ＳＴＥＰ３において、調整パラメータｋの現在の候補値を用いて、前記式（５）により行列式ＤＥＴの値が算出される。

次いで、ＳＴＥＰ４において、この行列式ＤＥＴの大きさ（絶対値）が、あらかじめ定められた所定の下限閾値DET_L（＞０）よりも小さいか否か（ＤＥＴの大きさが過小であるか否か）が判断される。

このＳＴＥＰ４の判断結果が否定的である場合（ＤＥＴの大きさが過小でない適切な大きさである場合）には、ＳＴＥＰ１０の処理が実行される。この処理では、ｋの現在の候補値が調整パラメタータｋの値として確定され、この調整パラメータｋの値を用いて、前記式（４）の演算により、擬似逆行列Ａ^*が算出される。そして、ＳＴＥＰ１１において、この擬似逆行列Ａ^*がヤコビアン行列Ｊの擬似逆行列Ｊ^*として決定される。

一方、ＳＴＥＰ４の判断結果が肯定的である場合（ＤＥＴの大きさが過小である場合）には、ＳＴＥＰ５〜８の処理（詳細は後述する）によって調整パラメータｋの候補値の増加量Δｋ（＞０）が決定される。そして、ＳＴＥＰ９において、調整パラメータｋの候補値が、現在値から上記増加量Δｋだけ増加させた値に更新される。以降、ＳＴＥＰ４の判断結果が否定的となるまで、ＳＴＥＰ３〜９の処理が繰り返される。これにより、行列式ＤＥＴの大きさが所定の下限閾値DET_L以上の適切な大きさとなるような調整パラメータｋの値が探索的に決定される。

上記増加量Δｋの決定処理を以下に説明する。行列式ＤＥＴの大きさが所定の下限閾値DET_L以上の適切な大きさとなるような調整パラメータｋの値を探索的に決定する処理では、例えば、調整パラメータｋの候補値の増加量Δｋをあらかじめ定めた一定値として、調整パラメータｋの候補値を更新することが一般的に考えられる。

ただし、その場合、増加量Δｋを大きめの値に設定しておくと、ｋ＝０として算出した前記行列式ＤＥＴの大きさが過小となる状況で、制御装置９の各演算処理周期で逐次決定されるｋの値が、Δｋの整数倍の刻み幅の離散的な値となるために、ＤＥＴ≧DET_Lとなるような最小のｋの値に対して必要以上に過大な値となりやすい。このため、制御装置９の各制御処理周期で逐次決定されるｋの値に対応する行列式ＤＥＴの大きさと前記下限閾値DET_Lとの差のばらつきを生じやすい。ひいては、前記式（８）により決定した擬似逆行列Ｊ^*を用いて後述する如く関節操作量ベクトル↑ΔΘdmdを決定した場合に、関節操作量ベクトル↑ΔΘdmdの不連続的な変動が生じやすい。その結果、ロボット１の動作の滑らかさが損なわれることがある。

一方、増加量Δｋを十分に小さい値に設定することで、上記の不都合を解消することができる。ただし、その場合、前記ＳＴＥＰ４の判断結果が否定的となる適切な調整パラメータｋの値が決定されるまでに要する時間が長くなりやすい。このため、制御装置９の各制御処理周期の時間内で適切な調整パラメータｋの値を決定できない場合が生じる恐れがある。

そこで、ＳＴＥＰ５〜８の処理においては、調整パラメータｋの候補値の増加量Δｋを、ＳＴＥＰ３で算出した行列式ＤＥＴの大きさと、前記下限閾値DET_Lとの偏差の大きさに応じて、可変的に設定する。

具体的には、ＳＴＥＰ５において、次式（８）により算出される値が、調整パラメータｋの増加量Δｋの暫定値Δｋpとして設定される。

Δｋp＝Ｇ・(DET_L−｜ＤＥＴ｜)^1/n ……（８）

すなわち、前記下限閾値DET_Lから行列式ＤＥＴの絶対値を差し引いてなる偏差（＝DET_L−｜ＤＥＴ｜）のｎ乗根に、所定値のゲインＧ（＞０）を乗じてなる値（偏差（＝DET_L−｜ＤＥＴ｜）のｎ乗根に比例する値）が増加量Δｋの暫定値Δｋpとして設定される。なお、ｎは行列（Ａ・Ｗ^-1・Ａ^T）の次数である（本実施形態ではｎ＝Ｎ）。また、ゲインＧの値は、あらかじめ定められた定数値である。

このように増加量Δｋの暫定値Δｋpを決定するのは、次の理由による。行列式det（Ａ・Ｗ^-1・Ａ^T）の値（ｋ＝０の場合のＤＥＴの値）と、行列式ＤＥＴ（＝det(Ａ・Ｗ^-1・Ａ^T＋ｋ・Ｉ)）の値との差分は、ｋのｎ次関数（ｋⁿ＋ａ・ｋ^n-1＋……という形の関数）となるので、ｋの値の変化に対する行列式ＤＥＴの値の変化は、ほぼｋⁿ（ｋのｎ乗）に比例すると考えられる。そこで、本実施形態では、上記式（８）により算出される値を、増加量Δｋの暫定値Δｋpとして設定する。

そして、ＳＴＥＰ６〜８において、増加量Δｋが過剰に小さい値になるのを防止するためのリミット処理が暫定値Δｋpに対して施され、これにより増加量Δｋが確定される。具体的には、ＳＴＥＰ６において、増加量Δｋの暫定値Δｋpと所定の下限値Δｋmin（＞０）とが比較され、Δｋp≧Δｋminである場合には、ＳＴＥＰ７において、暫定値Δｋpがそのまま増加量Δｋとして決定される。また、Δｋp＜Δｋminである場合には、ＳＴＥＰ８において、Δｋminが増加量Δｋとして決定される。

これにより、増加量Δｋは、Δｋminを下限値として、行列式ＤＥＴの大きさと、前記下限閾値DET_Lとの偏差の大きさに応じて前記式（８）により算出される値に決定されることとなる。

擬似逆行列算出部１４は、以上の如く決定した調整パラメータｋの値を用いて、前記式（４）の右辺の演算により算出されるＡ^*を、ヤコビアン行列Ｊの擬似逆行列Ｊ^*として決定する。

そして、関節操作量決定部１２は、次に、この擬似逆行列Ｊ^*と、入力された基本操作量ベクトル↑ΔＸdmdとを用いて関節操作量ベクトル↑ΔΘdmdを決定する。

具体的には、本実施形態では、関節操作量決定部１２は、次式（９）により、↑ΔΘdmdを決定する。

↑ΔΘdmd＝Ｊ^*・↑ΔＸdmd＋(Ｉ−Ｊ^*・Ｊ)・↑Ｃ(θ) ……（９）

ここで、式（９）の右辺の第１項は、式（７）の左辺の↑ΔＸが基本操作量ベクトル↑ΔＸdmdである場合に、式（７）の関係が成立するような↑ΔΘの値（↑ΔＸdmdを実現するために必要となる↑ΔΘの値）を求める項である。この第１項は、関節操作量決定部１２に入力された基本操作量ベクトル↑ΔＸdmdに、前記擬似逆行列算出部１４により算出した擬似逆行列Ｊ^*を乗じることによって算出される。

また、式（９）の右辺の第２項は、各関節４の変位量が、極力、上限又は下限の変位量に近づき過ぎないようにするために、↑ΔＸdmdにより実現しようとするハンド部５の位置に影響を及ぼさないような１つ以上の関節４の変位量を、上限の変位量と下限の変位量との間の中間の標準変位量（例えば、上限の変位量と下限の変位量との間の中央の変位量）に近づけていく機能を有する↑ΔΘの値を求める項である。

換言すれば、該第２項は、所謂、Nullスペース（ヌル空間）を利用して、制御対象の状態（本実施形態ではハンド部５の位置）に影響を及ぼさないように変化させ得る関節４の変位量を、上記標準変位量に近づけていく機能を有する↑ΔΘの値を求める項である。

この場合、式（９）の右辺の第２項の↑Ｃ(θ)は、関節４(1)〜４(N)の変位量θ(1)〜θ(N)の関数としてあらかじめ定義されたベクトル（↑ΔΘと同次数の縦ベクトル）である。具体的には、この↑Ｃ(θ)は、本実施形態では、その各成分ｃ(i)（ｉ＝１，２，…，Ｎ）が、次式（１０）により定義されるベクトルである。

ｃ(i)＝dＨ(θ(i))／dθ(i) ……（１０）
ただし、Ｈ(θ(i))≡α・(θ(i)−θ(i)s)²ⁿ

すなわち、↑Ｃ(θ)の各成分ｃ(i)（ｉ＝１，２，…，Ｎ）は、式（１０）のただし書きで定義した関数Ｈ(θ(i))をθ(i)で微分したものである。従って、本実施形態では、↑Ｃ(θ)は、その各成分ｃ(i)（ｉ＝１，２，…，Ｎ）が次式（１０ａ）により決定されるものである。

ｃ(i)＝２n・α・(θ(i)−θ(i)s)^2n-1 ……（１０ａ）

この場合、式（１０ａ）の右辺のθ(i)sは、関節４(i)の標準変位量としてあらかじめ定めれた値である。また、ｎは１以上の整数であり、例えばｎ＝１である。また、αはあらじめ定めた正の定数である。

そして、関節操作量決定部１２は、前記式（９）の右辺の２項を算出するに際して、式（１０ａ）のθ(i)（ｉ＝１，２，…，Ｎ）の値として、関節４(i)の実変位量θ(i)actの計測値を用いて該式（１０ａ）により各ｃ(i)を算出する。これにより、↑Ｃ(θ)（≡[ｃ(1)，c(2)，……，c(N)]^T)が決定される。

このようにして決定される↑Ｃ(θ)は、各関節４(i)の変位量を、上限の変位量と下限の変位量との間の標準変位量に戻すような↑ΔΘの値となる。

また、関節操作量決定部１２は、ヤコビアン行列Ｊ及び擬似逆行列Ｊ^*と単位行列Ｉとから行列（Ｉ−Ｊ^*・Ｊ)を算出する。そして、関節操作量決定部１２は、この行列（Ｉ−Ｊ^*・Ｊ)を上記の如く決定したベクトル↑Ｃ(θ)に乗じることによって、前記式（９）の右辺の第２項を算出する。

これにより、基本操作量ベクトル↑ΔＸdmdによるハンド部５の位置の操作に影響を及ぼすことなく変化させ得る関節４の変位量を、上記標準変位量に近づけていく機能を有する↑ΔΘの値としての前記式（９）の右辺の第２項が算出されることとなる。

関節操作量決定部１２は、上記の如く算出した前記式（９）の右辺の第１項及び第２項を加え合わせることで、関節操作量ベクトル↑ΔΘdmdを決定する。

これにより、基本操作量ベクトル↑ΔＸdmdよるハンド部５の位置の操作を実現するための関節４(1)〜４(N)の変位量の修正量を示す関節操作量ベクトル↑ΔΘdmdが決定される。この場合、本実施形態では、関節操作量ベクトル↑ΔΘdmdのうち、基本操作量ベクトル↑ΔＸdmdよるハンド部５の位置の操作に影響を及ぼさない関節４に対応する成分は、その関節４の変位量を前記標準変位量に近づけるような操作量（変位量の修正量）として決定される。

補足すると、前記式（９）の右辺の第２項は、必須なものではなく、それを省略した式によって、関節操作量ベクトル↑ΔΘdmdを決定するようにしてもよい。すなわち、基本操作量ベクトル↑Ｘdmdに、擬似逆行列Ｊ^*を乗じたもの（＝Ｊ^*・↑ΔＸdmd）をそのまま関節操作量ベクトル↑ΔΘdmdとして決定するようにしてもよい。

上記の如く関節操作量決定部１２の処理を実行した後、次に制御装置９は、関節制御部１５の処理を実行する。

この関節制御部１５には、前記基本動作目標のうち、関節４(1)〜４(N)の基準目標変位量θ(1)cmd0〜θ(N)cmd0の組、すなわち、基準目標変位量ベクトル↑Θcmd0と、関節操作量決定部１２で決定された関節操作量ベクトル↑ΔΘdmdとが入力される。

そして、関節制御部１５は、入力された操作量決定部１２は、基準目標変位量ベクトル↑Θcmd0に、関節操作量ベクトル↑ΔΘdmdを加え合わせることによって、基準目標変位量ベクトル↑Θcmd0を修正することで、関節４(1)〜４(N)のそれぞれの関節変位指令θ(1)cmd1〜θ(N)cmd1を並べたベクトル（≡[θ(1)cmd1，θ(2)cmd1，……，θ(N)cmd1]^T)である関節変位指令ベクトル↑Θcmd1（＝↑Θcmd0＋↑ΔΘdmd）を算出する。

そして、関節制御部１５は、上記の如く決定した関節変位指令ベクトル↑Θcmd1の各成分θ(i)cmd1に各関節４(i)の実変位量θ(i)actを一致させるように、各関節４(i)の関節アクチュエータ６を図示しないサーボアンプ等の駆動回路を介して制御する。

以上が、本実施形態における制御装置９の処理の詳細である。

ここで、本実施形態と本発明との対応関係について補足しておく。本実施形態では、基本操作量決定部１１、関節操作量決定部１２、関節制御部１５の処理によって、それぞれ、本発明における基本操作量決定手段、関節操作量決定手段、関節制御手段が実現される。

また、ヤコビアン行例決定部１３、擬似逆行列算出部１４によりそれぞれ、本発明におえる行列決定手段、擬似逆行列算出手段が実現される。さらに、擬似逆行列算出部１４が実行する図３の処理のうち、図４のＳＴＥＰ２〜９の処理によって、本発明における調整パラメータ決定手段が実現される。

そして、本実施形態では、前記基本操作量ベクトル↑ΔＸdmd、関節操作量ベクトル↑ΔΘdmd、ヤコビアン行列Ｊ、擬似逆行列Ｊ^*が、それぞれ本発明における基本操作量ベクトル↑ａ、関節操作量ベクトル↑ｂ、行列Ａ、擬似逆行列Ａ^*に相当する。

以上説明した実施形態においては、関節操作量ベクトル↑ΔΘdmdを算出するための演算処理において、ヤコビアン行列Ｊの擬似逆行列Ｊ^*が擬似逆行列算出部１４により算出される。この場合、前記行列式ＤＥＴの値が過小になるのを防止するための調整パラメータｋの値を探索する処理（図４のＳＴＥＰ２〜９の処理）において、調整パラメータｋの増加量Δｋを、下限閾値DET_Lから行列式ＤＥＴの絶対値を差し引いてなる偏差（＝DET_L−｜ＤＥＴ｜）のｎ乗根に比例する値に設定する。

これにより、制御装置９の各制御処理周期において、｜ＤＥＴ｜≧DET_Lとなるようにするための適切な調整パラメータｋの値を、該ｋの値の不連続な変動が生じないようにしつつ、効率よく短時間で決定できる。このため、基本操作量ベクトル↑Ｘdmdによるハンド部５の位置の操作を実現するための関節操作量ベクトル↑ΔΘdmdを決定するために用いる擬似逆行列Ｊ^*を滑らかに変化させていくようにすることができる。ひいては、ロボット１の各関節４の変位量を滑らかに変化させるように、関節操作量ベクトル↑ΔΘdmdの各成分を決定できる。

よって、ロボット１による作業を該ロボット１の滑らかな動きで実現できる。

［第２実施形態］
次に、本発明の第２実施形態を図５を参照して以下に説明する。なお、本実施形態は、前記第１実施形態と制御装置９の処理だけが相違するので、その相違する事項を中心に説明する。そして、第１実施形態と同じ事項については説明を省略する。

前記第１実施形態は、ロボット１の各関節４の変位量を目標値に制御する位置制御を行うものである。これに対して、本実施形態では、所定種類の状態量を目標値に制御するためにロボット１の各関節４の目標駆動力を決定し、これに応じて、関節アクチュエータ６によって各関節４に付与される駆動力を、当該目標駆動力に追従させるように各関節４の関節アクチュエータ６の制御を行うものである。

このような制御を行うために、本実施形態における制御装置９は、実装されたプログラム等により実現される機能として、ロボット１の基準動作目標を出力する基準動作目標出力部２０と、ロボット１の所定種類の状態量の実際の値（実状態量）を所要の目標値に制御する（追従させる）ための制御入力（操作量）としての基本操作量ベクトル↑ΔＳdmdを決定する基本操作量決定部２１と、この基本操作量ベクトル↑ΔＳdmdを用いて、各関節４の駆動力を制御するための制御入力（操作量）としての関節操作量ベクトル↑τcmdを決定する関節操作量決定部２２と、この関節操作量ベクトル↑τcmdに応じて各関節アクチュエータ６を図示しない駆動回路を介して制御する関節制御部２５とを備える。そして、本実施形態における制御装置９は、これらの機能部の処理を所定の制御処理周期で逐次実行することによって、ロボット１の動作制御を行う。

以下に、ハンド部５により対象物Ｗを動かす作業をロボット１に行わせる場合を例にとって、本実施形態における制御装置９の制御処理を説明する。

制御装置９の各制御処理周期において、基準動作目標出力部２０からロボット１の基準動作目標が出力される。本実施形態では、この基準動作目標は、前記第１実施形態における基準動作目標のうちの基準目標ハンド部位置↑Ｘcmd0と、基準目標ハンド部外力↑Ｆcmd0とから構成される。

次いで、制御装置９は、基本操作量決定部２１の処理を実行する。本実施形態では、この基本操作量決定部２１には、基準動作目標出力部１０から出力される基準動作目標の構成要素のうちの基準目標ハンド部位置↑Ｘcmd0が入力されると共に、関節変位センサ７の出力により示される関節４(1)〜４(N)のそれぞれの実変位量θ(1)act〜θ(N)actの計測値（実変位量ベクトル↑Θactの計測値）が入力される。

基本操作量決定部２１は、入力されたデータを用いて、ロボット１の所定種類の状態量の実際の値（実状態量）を、所要の目標値に制御する（追従させる）ための基本操作量ベクトル↑Ｓdmdを決定する。

本実施形態では、この基本操作量ベクトル↑Ｓdmdにより制御しようとする所定種類の状態量は、例えば、ハンド部５の位置↑Ｘである。そして、基本操作量決定部２１が決定する基本操作量ベクトル↑Ｓdmdは、基準目標ハンド部位置↑Ｘcmd0の時系列により規定されるハンド部５の運動（基準目標運動）を実現するために、ロボット１から該ハンド部５に作用させるべき並進力を規定する制御入力である。そして、この基本操作量ベクトル↑Ｓdmdは、前記慣性座標系の３成分のベクトルとして表現されるものである。

本実施形態では、基本操作量決定部２１は、ハンド部５の上記基準目標運動を動力学的に実現し得るように制御入力（操作量）としての基本操作量ベクトル↑Ｓdmdを決定する。併せて、基本操作量決定部１１は、前記したNullスペースを利用して、ハンド部５の基準目標運動の実現に影響を及ぼさない範囲で、関節４の変位量を前記標準変位量に近づけていくように、基本操作量ベクトル↑Ｓdmdを決定する。

具体的には、基本操作量決定部２１は、基本操作量ベクトル↑Ｓdmdを、次式（１１ａ），（１１ｂ）により決定される操作量成分↑Ｓａ（＝↑Ｓa1＋↑Ｓa2）と、次式（１１ｃ）により決定される操作量成分↑Ｓｂとを次式（１１）で示す如く加え合わせることによって決定する。

↑Ｓdmd＝↑Ｓａ＋↑Ｓｂ ……（１１）
ただし、
↑Ｓａ＝↑Ｓa1＋↑Ｓa2 ……（１１ａ）
↑Ｓa1＝Λ・(↑Ｘcmd0''＋Ｋp2・(↑Ｘcmd0−↑Ｘact)
＋Ｋv2・(↑Ｘcmd0'−↑Ｘact')) ……（１１ｂ）
↑Ｓｂ＝(Ｉ−Ｊ#・Ｊ)^T・(Ｍ・↑Ｃ(θ)) ……（１１ｃ）

なお、Λ、Ｊ#は、それぞれ次式（１２ａ），（１２ｂ）により定義される行列である。
Λ≡(Ｊ・Ｍ^-1・Ｊ^T)^T ……（１２ａ）
Ｊ#≡(Ｍ^-1)^T・Ｊ^T・(Ｊ・Ｍ^-1・Ｊ^T)^* ……（１２ｂ）

ここで、上記操作量成分↑Ｓａは、ハンド部５の上記基準目標運動を動力学的に実現するためにロボット１に作用させるべき並進外力に相当している。この場合、↑Ｓａは、要求慣性力に相当する操作量成分↑Ｓa1と、ロボット１に作用する重力によりハンド部５の運動が受ける影響、あるいは、重力と遠心力及びコリオリ力とによりハンド部５の運動が受ける影響を補償するための操作量成分↑Ｓa2とを上記式（１１ａ）により加え合わせた操作量成分である。

要求慣性力に相当する操作量成分↑Ｓa1は、式（１１ｂ）により算出される。この場合、式（１１ｂ）の演算に必要な行列Λは、第１実施形態で説明したヤコビアン行列Ｊ（関節変位量ベクトル↑Θの単位時間当たりの微小変化量↑ΔΘと、ハンド部５の位置↑Ｘの単位時間当たりの変化量↑ΔＸとの間の線形写像関係を表すヤコビアン行列Ｊ）と、イナーシャ行列Ｍとから前記式（１２ａ）の定義式に従って算出される。

この場合、ヤコビアン行列Ｊは、前記第１実施形態で説明したヤコビアン行列決定部１３の処理と同じ処理によって決定される。

また、イナーシャ行列Ｍは、より詳しくは、関節４(1)〜４(N)のそれぞれの変位量の２階微分値（本実施形態では角加速度）を並べたベクトル（すなわち、関節変位量ベクトル↑Θの２階微分値）に、（−１）・Ｍ（＝−Ｍ）を乗じたものが、関節４(1)〜４(N)のそれぞれの慣性力（本実施形態では慣性力モーメント）となるようにあらかじめ定められた行列である。なお、イナーシャ行列Ｍは対角行列である。従って、Ｍ^-1は、その各対角成分がＭの各対角成分の逆数値となる対角行列である。

また、式（１１ｂ）の演算に必要な↑Ｘcmd0''、↑Ｘcmd0'はそれぞれ基準目標ハンド部位置↑Ｘcmd0の２階微分値、１階微分値である。これらはそれぞれ、↑Ｘcmd0の時系列から算出される。

また、式（１１ｂ）の演算に必要な↑Ｘact（ハンド部５の実際の位置）は、実変位量ベクトル↑Θactの計測値から幾何学的演算によって算出される。さらに、この↑Ｘactに時間的変換率が、↑Ｘact'として算出される。

なお、式（１１ｂ）におけるＫp2、Ｋv2は、それぞれ、比例ゲイン、微分ゲインであり、これらは、その値があらかじめ定めれたスカラー又は対角行列である。

操作量成分↑Ｓa1は、上記の如く決定される行列Λ等を用いて、前記式（１１ｂ）の右辺の演算により算出される。この場合、本実施形態では、式（１１ｂ）の右辺の括弧内の値（Λに乗じる値）は、↑Ｘcmd0の２階微分値↑Ｘcmd0''に、↑ＸactをＰＤ則により↑Ｘcmd0に追従させるためのフィードバック操作量としての比例項と微分項とを加え合わせたものとなっている。

また、操作量成分↑Ｓa2は、ロボット１の運動モデルに基づいて、実変位量ベクトル↑Θactの計測値や、その時間的変化率を用いて算出される。

そして、このように算出された操作量成分↑Ｓa1と↑Ｓa2とを加え合わせたものが、ハンド部５の基準目標運動を動力学的に実現するための並進外力に相当する前記操作量成分↑Ｓａとして算出される。

なお、↑Ｓa1の算出においては、式（１１ｂ）の右辺の第３項を省略したり、あるいは、第２項及び第３項を省略するようにしてもよい。

また、式（１１）の演算に必要な操作量成分↑Ｓｂは、Nullスペース（ヌル空間）を利用して、制御対象の状態（本実施形態ではハンド部５の位置）に影響を及ぼさないように変化させ得る関節４の変位量を、前記標準変位量（上限の変位量と下限の変位量との間の中間の所定変位量）に近づけていく機能を有する操作量成分である。

この操作量成分↑Ｓｂは、前記式（１１ｃ）により算出される。この場合、式（１１ｃ）の演算に必要な↑Ｃ(θ)は、本実施形態では、前記第１実施形態で説明したものと同じである。従って、↑Ｃ(θ)は、その各成分ｃ(i)（ｉ＝１，２，…，Ｎ）が、各関節４(i)の実変位量の計測値と、標準変位量との偏差に応じて、前記式（１０ａ）により決定されるものである。

また、Ｊ、Ｍは、前記式（１１ｂ）の行列Λの算出処理に関して説明したものと同じである。

さらに、Ｊ#は、ヤコビアン行列Ｊとイナーシャ行列Ｍとから前記式（１２ｂ）の定義式に従って算出される。

ここで、式（１２ｂ）の(Ｊ・Ｍ^-1・Ｊ^T)^*は、行列(Ｊ・Ｍ^-1・Ｊ^T)の擬似逆行列を意味しており、この擬似逆行列は、前記第１実施形態でＪ^*を算出した場合と同様の処理によって算出される。

具体的には、基本操作量決定部２１は、図６のフローチャートの処理を実行することで、擬似逆行列(Ｊ・Ｍ^-1・Ｊ^T)^*を算出する。すなわち、まずＳＴＥＰ１ａにおいて、前記式（４）の行列Ａとして行列(Ｊ・Ｍ^-1・Ｊ^T)が代入されると共に、該行列Ａの転置行列Ａ^Tと、重み係数行列Ｗの逆行列Ｗ^-1とが算出される。なお、重み係数行列Ｗは、第１実施形態のものと同様にあらかじめ定められた対角行列である。

次いで、第１実施形態で説明した図４のＳＴＥＰ２〜１０の処理（調整パラメータｋの探索的決定処理を含む）と同じ処理を実行することで、擬似逆行列Ａ^*が算出される。

そして、ＳＴＥＰ１ｂにおいて、この擬似逆行列Ａ^*が、行列(Ｊ・Ｍ^-1・Ｊ^T)の擬似逆行列(Ｊ・Ｍ^-1・Ｊ^T)^*として決定される。

基本操作量決定部２１は、このようにして算出した擬似逆行列(Ｊ・Ｍ^-1・Ｊ^T)^*を用いて前記式（１２ｂ）の演算により操作量成分↑Ｓｂを算出する。

そして、基本操作量決定部２１は、以上の如く算出した操作量成分↑Ｓａと↑Ｓｂとを加え合わせたものを基本操作量ベクトル↑Ｓdmdとして決定する。

補足すると、操作量成分↑Ｓｂを省略し、操作量成分↑Ｓａをそのまま基本操作量ベクトル↑Ｓdmdとして決定するようにしてもよい。

また、ハンド部５の位置↑Ｘの目標値は、必要に応じて↑Ｘcmd0から動的にずらすようにしてもよい。

また、本実施形態では、基本操作量ベクトル↑Ｓdmdにより制御しようとする所定種類の状態量をハンド部５の位置↑Ｘとしたが、ハンド部５の姿勢を当該状態量に含めるようにしてもよい。

上記の如く基本操作量決定部２１の処理を実行した後、次に制御装置９は、関節操作量決定部２２の処理を実行する。

この関節操作量決定部２２には、各関節４毎の関節変位センサ７の出力により示される関節４(1)〜４(N)のそれぞれの実変位量θ(1)act〜θ(N)actの計測値（実変位量ベクトル↑Θactの計測値）と、前記力センサ８の出力により示される実外力↑Ｆact（ハンド部５に作用する実際の並進外力）の計測値と、基本操作量決定部２１で決定された基本操作量ベクトル↑Ｓdmdと、前記基準動作目標のうちの基準目標ハンド部外力↑Ｆcmd0とが入力される。

そして、関節操作量決定部２２は、入力されたデータを用いて、関節４(1)〜４(N)のそれぞれの駆動力を制御するための関節操作量ベクトル↑τcmdを決定する。この関節操作量ベクトル↑τcmdは、本実施形態では、ロボット１のＮ個の関節４(1)〜４(N)のそれぞれの駆動力（本実施形態ではトルク）の目標値τ(1)cmd〜τ(N)cmdを並べたベクトル（≡[τ(1)cmd，Δτ(2)cmd，……，Δτ(N)cmd]^T）である。

関節操作量決定部２２は、その処理の構成要素として、行列決定部２３と、擬似逆行列算出部２４とを備えている。そして、関節操作量決定部２２は、まず、これらの行列決定部２３、擬似逆行列算出部２４の処理を順次実行する。

行列決定部２３は、関節４(1)〜４(N)の駆動力τ(1)〜τ(N)を並べたベクトル（縦ベクトル）である関節駆動力ベクトル↑τと、基本操作量ベクトル↑Ｓdmdが表す力（本実施形態では３成分の並進力ベクトル）↑Ｓとの間の線形写像関係を、次式（１３）により表す行列Ｊｓを決定する処理を実行するものである。

↑Ｓ＝Ｊｓ・↑τ ……（１３）
（＝Ｊ#^T・↑τ）

このＪｓは前記式（１２ｂ）により定義される行列Ｊ#の転置行列Ｊ#^Tに一致するものである。

そこで、本実施形態では、行列決定部２３は、基本操作決定部２１の演算処理で先に算出された行列Ｊ#の転置行列Ｊ#^Tを上記行列Ｊｓとして決定する。なお、行列決定部２３において、Ｊ#を前記式（１２ｂ）の定義式に従って改めて算出した上で、その転置行列を求めるようにしてもよい。

このように決定された行列Ｊｓ（＝Ｊ#^T）の擬似逆行列(Ｊ#^T)^*が、次に、擬似逆行列算出部２４の処理によって算出される。

この擬似逆行列(Ｊ#^T)^*は、前記第１実施形態でＪ^*を算出した場合と同様の処理によって算出される。

具体的には、擬似逆行列算出部２４は、図７のフローチャートの処理を実行することで、擬似逆行列(Ｊ#^T)^*を算出する。すなわち、まずＳＴＥＰ１ｃにおいて、前記式（４）の行列Ａとして行列Ｊ#^T(＝Ｊｓ)が代入されると共に、該行列Ａの転置行列Ａ^Tと、重み係数行列Ｗの逆行列Ｗ^-1とが算出される。なお、重み係数行列Ｗは、第１実施形態のものと同様にあらかじめ定められた対角行列である。

次いで、第１実施形態で説明した図４のＳＴＥＰ２〜１０の処理（調整パラメータｋの探索的決定処理を含む）と同じ処理を実行することで、擬似逆行列Ａ^*が算出される。

そして、ＳＴＥＰ１ｄにおいて、この擬似逆行列Ａ^*が、Ｊ#^T(＝Ｊｓ)の擬似逆行列(Ｊ#^T)^*として決定される。

以上の如く擬似逆行列(Ｊ#^T)^*を算出した後、関節操作量決定部２２は、次に、この擬似逆行列(Ｊ#^T)^*と、入力された基本操作量ベクトル↑Ｓdmdと、基準目標ハンド部外力↑Ｆcmd0と、ハンド部５に作用する実外力↑Ｆactの計測値とを用いて関節操作量ベクトル↑τcmdを決定する。

具体的には、本実施形態では、関節操作量決定部２２は、次式（１４ａ），（１４ｂ）により決定される操作量成分↑τａ，↑τｂを次式（１４）で示す如く加え合わせることによって決定する。

↑τcmd＝↑τａ＋↑τｂ ……（１４）
ただし、
↑τａ＝(Ｊ#^T)^*・↑Ｓdmd ……（１４ａ）
↑τｂ＝Ｊ^T・(↑Ｆcmd0＋Ｋc2・(↑Ｆcmd0−↑Ｆact)
＋Ｋe2・∫(↑Ｆcmd0−↑Ｆact)dt) ……（１４ｂ）

ここで、式（１４ａ）により算出される操作量成分↑τａは、ハンド部５の実際の位置↑Ｘactを、基準目標ハンド部位置↑Ｘcmd0に追従させるための操作量である。なお、本実施形態ではこの操作量成分↑τａには、ハンド部５の実際の位置↑Ｘactの制御に影響を及ぼさないように動作させ得る関節４の変位量を標準変位量に近づけるようにするための操作量成分も含まれる。

そして、この↑τａは、基本操作量ベクトル↑Ｓdmdに擬似逆行列(Ｊ#^T)^*を乗じることによって算出される。

また、式（１４ｂ）により算出される操作量成分↑τｂは、対象物Ｗからハンド部５に作用する実外力↑Ｆactを基準目標ハンド部外力↑Ｆcmd0に追従させるために↑τａに付加する操作量である。

この場合、式（１４ｂ）の右辺の括弧内の値（Ｊ^Tに乗じる値）は、↑Ｆact（計測値）をＰＩ則（比例・積分則）により↑Ｆcmd0に追従させるためのフィードバック操作量としての比例項と積分項とを、基準目標ハンド部外力Ｆcmd0に加え合わせたものとして算出される。そして、この値に、前記ヤコビアン行列Ｊの転置行列Ｊ^Tを乗じることによって、操作量成分↑τｂが算出される。

なお、式（１４ｂ）の演算に使用する比例ゲインＫc2及び積分ゲインＫe2は、それぞれ、その値があらかじめ定められたスカラー又は対角行列である。

関節操作量決定部２２は、上記の如く決定した操作量成分↑τａと↑τｂとを加え合わせたものを関節操作量ベクトル↑τcmdとして決定する。

これにより、関節操作量ベクトル↑τcmdは、ハンド部５の実際の位置↑Ｘactと、実外力↑Ｆactとをそれぞれ、基準目標ハンド部位置↑Ｘcmd0、基準目標ハンド部外力↑Ｆcmd0に追従させるように機能する操作量ベクトルとして決定されることとなる。

補足すると、関節操作量ベクトル↑τcmdの操作量成分↑τｂの算出においては、式（１４ｂ）の右辺の第３項を省略したり、あるいは、第２項及び第３項を省略するようにしてもよい。また、外力↑Ｆの目標値を、必要に応じて↑Ｆcmd0から動的にずらすようにしてもよい。

上記の如く関節操作量決定部２２の処理を実行した後、次に制御装置９は、関節制御部２５の処理を実行する。

この関節制御部２５には、関節操作量決定部２２で決定された関節操作量ベクトル↑τcmdが入力される。

そして、関節制御部２５は、入力された関節操作量ベクトル↑τcmdの各成分τ(i)cmd（ｉ＝１，２，…，Ｎ）に各関節４(i)の実際の駆動力を一致させるように、各関節４(i)の関節アクチュエータ６の駆動力を図示しない駆動回路を介して制御する。

以上が、本実施形態における制御装置９の処理の詳細である。

ここで、本実施形態と本発明との対応関係について補足しておく。本実施形態では、基本操作量決定部２１、関節操作量決定部２２、関節制御部２５の処理によって、それぞれ、本発明における基本操作量決定手段、関節操作量決定手段、関節制御手段が実現される。

また、行例決定部２３、擬似逆行列算出部２４によりそれぞれ、本発明におえる行列決定手段、擬似逆行列算出手段が実現される。さらに、擬似逆行列算出部２４が実行する図７の処理のうち、ＳＴＥＰ２〜９の処理によって、本発明における調整パラメータ決定手段が実現される。

そして、本実施形態では、前記基本操作量ベクトル↑Ｓdmd、関節操作量ベクトル↑τdmdの操作量成分↑τａ、行列Ｊｓ（＝Ｊ#^T）、擬似逆行列(Ｊ#^T)^*が、それぞれ本発明における基本操作量ベクトル↑ａ、関節操作量ベクトル↑ｂ、行列Ａ、擬似逆行列Ａ^*に相当する。

なお、本実施形態では、基本操作量決定部２１及び関節操作量決定部２２を合わせた処理によって、前記第５発明おける関節操作量決定手段が実現されるとみなすこともできる。この場合、基本操作量決定部２１が実行する図６の処理のうちのＳＴＥＰ２〜９の処理と、関節操作量決定部２２の擬似逆行列算出部２４が実行する図７の処理のうちのＳＴＥＰ２〜９の処理とがそれぞれ第５発明における調整パラメータ決定手段に相当するものとなる。そして、この場合は、(Ｊ#^T)^*に加えて、(Ｊ・Ｍ^-1・Ｊ^T)^*も第５発明における擬似逆行列Ａ^*に相当するものとなる。

以上説明した実施形態においては、擬似逆行列(Ｊ#^T)^*、(Ｊ・Ｍ^-1・Ｊ^T)^*のそれぞれの算出において、調整パラメータｋの増加量Δｋを、下限閾値DET_Lから行列式ＤＥＴの絶対値を差し引いてなる偏差（＝DET_L−｜ＤＥＴ｜）のｎ乗根に比例する値に設定する。

これにより、制御装置９の各制御処理周期において、擬似逆行列(Ｊ#^T)^*、(Ｊ・Ｍ^-1・Ｊ^T)^*のいずれの算出処理においても、｜ＤＥＴ｜≧DET_Lとなるようにするための適切な調整パラメータｋの値を、該ｋの値の不連続な変動が生じないようにしつつ、効率よく短時間で決定できる。このため、ロボット１の各関節４の駆動力を滑らかに変化させるように、関節操作量ベクトル↑τcmdの各成分を決定できる。

よって、ロボット１による作業を該ロボット１の滑らかな動きで実現できる。

なお、以上説明した各実施形態では、設置型のロボット１の動作制御を例にとって説明したが、本発明の対象とするロボットは、設置型に限らず、脚式移動ロボット等の移動ロボットであってもよい。

１…ロボット、３(1)〜３(N)…リンク要素、４(1)〜４(N)…関節、６…関節アクチュエータ、９…制御装置、１１，２１…基本操作量決定部（基本操作量決定手段）、１２，２２…関節操作量決定部（関節操作量決定手段）、１３…ヤコビアン行列決定部（行列決定手段）、２３…行列決定部（行列決定手段）、１４，２４…擬似逆行列算出部（擬似逆行列算出手段）、１５，２５…関節制御部（関節制御手段）、ＳＴＥＰ２〜９…調整パラメータ決定手段。

Claims (5)

1. 複数の関節を介して相互に連結された複数のリンク要素を備えるロボットの制御装置であって、
   前記ロボットのあらかじめ定められた所定種類の状態量の値を目標値に制御するための操作量としてのベクトルであって、Ｍ個（Ｍ：１以上の整数）の成分により構成される基本操作量ベクトル↑ａを少なくとも前記所定種類の状態量の目標値に応じて逐次決定する基本操作量決定手段と、
   前記ロボットの各関節の動作状態を制御するための操作量としてのベクトルであって、Ｎ個（Ｎ：Ｎ≧Ｍとなる整数）の成分により構成されると共に前記基本操作量ベクトル↑ａとの間に次式（３）により示される線形写像の関係を有する関節操作量ベクトル↑ｂを、前記決定された基本操作量ベクトル↑ａの値に、次式（３）の行列Ａの擬似逆行列Ａ^*を乗じることにより逐次決定する関節操作量決定手段と、
   
   ↑ａ＝Ａ・↑ｂ ……（３）
   
   少なくとも前記決定された関節操作量ベクトル↑ｂに応じて前記ロボットの各関節の動作状態をアクチュエータを介して制御する関節制御手段とを備えており、
   前記関節操作量決定手段は、少なくとも前記ロボットの各関節の現在の動作状態に応じて前記行列Ａを決定する行列決定手段と、その決定された行列Ａとあらかじめ設定された重み係数行列Ｗ（Ｗ：対角行列）と調整パラメータｋ（ｋ：ゼロ以上の実数値）の値とを用いて次式（４）により前記擬似逆行列Ａ^*を算出する擬似逆行列算出手段と、該式（４）の演算に用いる前記調整パラメータｋの値を、次式（５）により表される行列式ＤＥＴの絶対値が所定の閾値以上の値になるように決定する調整パラメータ決定手段とを備えており、
   
   Ａ^*＝Ｗ^-1・Ａ^T・(Ａ・Ｗ^-1・Ａ^T＋ｋ・Ｉ)^-1 ……（４）
   ＤＥＴ＝det(Ａ・Ｗ^-1・Ａ^T＋ｋ・Ｉ) ……（５）
   
   前記調整パラメータ決定手段は、前記調整パラメータｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（４）の演算に用いる調整パラメータｋの値として決定する手段であると共に、前記判断結果が否定的である場合のｋの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ａ・Ｗ^-1・Ａ^Tの次数）に比例する値に設定することを特徴とするロボットの制御装置。
2. 請求項１記載のロボットの制御装置において、
   前記所定種類の状態量は、前記ロボットの特定部位の運動の状態量と、前記ロボットに作用する外力とのうちの少なくともいずれか一方を含むことを特徴とするロボットの制御装置。
3. 請求項１又は２記載のロボットの制御装置において、
   前記基本操作量ベクトル↑ａは、前記ロボットの特定部位の運動状態を規定する操作量成分により構成されたベクトルであり、前記関節操作量ベクトル↑ｂは、前記ロボットの各関節の変位量を規定する操作量成分により構成されたベクトルであることを特徴とするロボットの制御装置。
4. 請求項１又は２記載のロボットの制御装置において、
   前記基本操作量ベクトル↑ａは、前記ロボットに作用する外力を規定する操作量成分により構成されたベクトルであり、前記関節操作量ベクトル↑ｂは、前記ロボットの各関節の駆動力を規定する操作量成分により構成されたベクトルであることを特徴とするロボットの制御装置。
5. 複数の関節を介して相互に連結された複数のリンク要素を備えるロボットの制御装置であって、
   線形写像を表す行列Ａの擬似逆行列Ａ ^* を使用する演算処理によって、前記ロボットの各関節の動作状態を制御するための操作量を逐次決定する関節操作量決定手段と、
   決定された前記操作量に応じて前記ロボットの各関節の動作状態をアクチェータを介して制御する関節制御手段とを備えており、
   前記関節操作量決定手段は、前記行列Ａとあらかじめ設定された重み係数行列Ｗ（Ｗ：対角行列）と調整パラメータｋ（ｋ：ゼロ以上の実数値）の値とを用いて次式（４）により前記擬似逆行列Ａ^*を算出する擬似逆行列算出手段と、該式（４）の演算に用いる前記
   調整パラメータｋの値を、次式（５）により表される行列式ＤＥＴの絶対値が所定の閾値以上の値になるように決定する調整パラメータ決定手段とを備えており、
   
   Ａ^*＝Ｗ^-1・Ａ^T・(Ａ・Ｗ^-1・Ａ^T＋ｋ・Ｉ)^-1 ……（４）
   ＤＥＴ＝det(Ａ・Ｗ^-1・Ａ^T＋ｋ・Ｉ) ……（５）
   
   前記調整パラメータ決定手段は、前記調整パラメータｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（４）の演算に用いる調整パラメータｋの値として決定する手段であると共に、前記判断結果が否定的である場合のｋの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ａ・Ｗ^-1・Ａ^Tの次数）に比例する値と当該増加量についての所定の下限値とのうちのより大きい方の値に設定することを特徴とするロボットの制御装置。

Images