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

Description

本発明は、脚式移動ロボット等のロボットの制御装置に関する。

２足歩行ロボット等の脚式移動ロボットの制御技術としては、例えば特許文献１等にて本願出願人が提案した技術の如く、ロボットの目標とする運動（ロボットの脚体先端部の目標位置の軌道等）に応じてロボットの各関節の目標変位量を逐次決定し、その目標変位量に実際の変位量を一致させるように各関節のアクチュエータの駆動制御を行う、所謂、位置制御が一般的に知られている。

このような位置制御は、関節の目標変位量が一定である限り、該関節の実際の変位量を一定に保持するものであるから、ロボットに作用する外力の変化に対する柔軟性が乏しい。

このため、例えば、特許文献２に見られる如く、関節とアクチュエータとの間にばね部材を介装することで関節に柔軟性を持たせると共に、ロボットの目標とする運動を実現するための各関節の目標駆動力を逆動力学演算により決定し、所謂、力制御によって、各関節の実際の駆動力を目標駆動力に制御する技術も知られている。

特開平５−３０５５７９号公報 特開２００７−１６０４４６号公報

ところで、力制御を行う上記特許文献２に見られる技術では、ロボットの所望の運動を実現するための各関節の目標回転角度、角速度及び角加速度を決定した上で、それらの目標値を基に、逆動力学演算を行なうことによって、各関節に付与する目標駆動トルクのフィードフォワード値を決定するようにしている。そして、特許文献２に見られる技術では、このフィードフォワード値に、ロボットの各関節の回転角度を目標値に収束させるためのフィードバック操作トルクを加えることによって、各関節の目標駆動トルクを決定するようにしている。

従って、特許文献２に見られる技術では、ロボット全体の各関節の実際の動きは、各関節の回転角度や角速度、角加速度の目標値による拘束を強く受けることとなり、該目標値が一定である限り、ロボットに作用する外力が変化しても、各関節の動きを一定に保つように、各関節の目標駆動トルクが調整されてしまうこととなる。

このため、特許文献２に見られる技術では、ロボットの各関節の実際の動きは、柔軟性に乏しいものとならざるを得ず、ロボットに作用する外力の急変時等に、ロボットの姿勢を安定に保つことが困難となる恐れがあった。

本発明は、かかる背景に鑑みてなされてものであり、力制御によってロボットの動作制御を行う場合に、ロボットの関節の動きの柔軟性を高めつつ、各関節に付与する駆動力をロボットに所望の動作を行なせる上で適切な駆動力に制御することができる制御装置を提供することを目的とする。

本発明のロボットの制御装置は、かかる目的を達成するために、関節を介して相互に連結された複数のリンクと、各関節を駆動するアクチュエータとを備えたロボットにおいて、少なくとも該ロボットの１つ以上の接触部位を該ロボットの外界に接触させつつ該ロボットの運動を行なう場合に、各関節に付与する目標駆動力を決定し、その決定した目標駆動力に応じて前記アクチュエータの動作を制御する制御装置であって、
前記ロボットの各関節の変位量を成分として少なくとも含む該ロボットの一般化変数ベクトルの２階微分値を一般化力ベクトルに変換するためのイナーシャ行列と、前記ロボットの各リンクに作用する重力により発生する一般化力ベクトルの値である重力依存一般化力ベクトル値とから成る基本パラメータ群、又は、前記イナーシャ行列と前記重力依存一般化力ベクトルと前記ロボットの各リンクに作用する遠心力及びコリオリ力による発生する一般化力ベクトルの値である遠心力・コリオリ力依存一般化力ベクトル値とから成る基本パラメータ群を、少なくとも前記ロボットの各関節の実際の変位量の観測値を含む一般化変数観測情報に基づいて算出する基本パラメータ群算出手段と、
前記１つ以上の接触部位の接触状態を示す接触状態情報と前記一般化変数観測情報とが入力され、前記外界に接触している前記１つ以上の接触部位の運動を代表する要素として設定した所定の接触部位代表要素の運動速度と前記一般化変数ベクトルの１階微分値との間の関係を表現するヤコビアンである接触部位ヤコビアンを、少なくとも入力された接触状態情報と一般化変数観測情報とを基に算出する接触部位ヤコビアン算出手段と、
前記一般化変数観測情報が入力され、前記一般化変数ベクトルの値に依存する値を有する所定種類の状態量と該一般化変数ベクトルの１階微分値との間の関係を表現するヤコビアンである状態量ヤコビアンを、少なくとも入力された一般化変数観測情報に基づいて算出する状態量ヤコビアン算出手段と、
前記算出された基本パラメータ群、接触部位ヤコビアン及び状態量ヤコビアンと、前記接触部位代表要素の運動加速度の目標値である接触部位代表要素運動加速度目標値と、前記一般化変数観測情報と、前記所定種類の状態量の１階微分値の目標値とが入力され、当該入力データを用いて、次式０１の関係を満足する一般化力ベクトルの目標である目標一般化力ベクトルτcmdのうち各関節の変位量に対応する成分値を算出し、その算出した成分値を該関節に付与する目標駆動力として決定する目標駆動力決定手段と、
少なくとも前記決定された目標駆動力に応じて、前記アクチュエータの動作を制御するアクチュエータ制御手段とを備えることを特徴とする（第１発明）。

Ｓ'＋(Ｊs＊Ｍ^-1＊Ｔc−Ｊs')＊ｑ'
＝(Ｊs＊Ｍ^-1＊Ｐc)＊(τcmd−τcmpn) ……式０１

但し、
ｑ：一般化変数ベクトル
ｑ'：ｑの１階微分値（＝dｑ/dt）の観測値
Ｓ：状態量
Ｓ'：Ｓの１階微分値（＝dＳ/dt）の目標値
Ｍ：イナーシャ行列
τcmpn：τcmpn≡(Ｎ＋Ｇ)−Ｐc^-1＊Ｃc、又はτcmpn≡Ｇ−Ｐc^-1*Ｃcにより定義されるベクトル
Ｇ：重力依存一般化力ベクトル値
Ｎ：遠心力・コリオリ力依存一般化力ベクトル値
Ｐc：Ｐc≡Ｉ−Ｊc^T＊(Ｊc#)^Tにより定義される行列
Ｉ：単位行列
Ｊc：接触部位ヤコビアン行列
Ｊc#：Ｊc#≡(Ｍ^-1)^T＊Ｊc^T＊Ｒc^Tにより定義される行列
Ｒc：Ｒc≡((Ｊc＊Ｍ^-1＊Ｊc^T)^-1)^Tにより定義される行列
Ｃc：Ｃc≡−Ｊc^T＊Ｒc＊Ｃにより定義されるベクトル
Ｃ：接触部位代表要素運動加速度の目標値（スカラー又はベクトル）
Ｔc：Ｔc≡−Ｊc^T＊Ｒc＊Ｊc'により定義される行列
Ｊc'：Ｊcを１階微分してなる行列（＝dＪc/dt）
Ｊs：状態量ヤコビアン
Ｊs'：Ｊsを１階微分してなる行列（＝dＪs/dt）

なお、本明細書では、「＊」は乗算記号を意味する。

補足すると、本発明においては、前記「ロボット」は、少なくとも１つのリンクがロボットの動作環境におけるグローバル座標系に対して固定された（該動作環境の床等に固定された）設置型とのロボットと、該グローバル座標系に対してロボットの全体が移動可能な移動ロボットとのいずれのロボットであってもよい。なお、グローバル座標系は、慣性座標系又は近似的に慣性系と見なせる座標系を意味する。

また、前記「一般化変数ベクトル」は、前記ロボットが設置型のロボットである場合には、該ロボットの全ての関節の変位量（回転角度又はストローク量）を成分値として構成されるベクトルである。一方、前記ロボットが移動ロボットである場合には、前記一般化変数ベクトルは、該ロボットの任意の１つの代表リンクのグローバル座標系での位置及び姿勢と、該ロボットの全ての関節の変位量とを成分値として構成されるベクトルである。

なお、ロボットのリンクの「位置」は、該リンクに対して固定された代表点の空間的な位置を意味し、リンクの「姿勢」は、リンクの空間的な向きを意味する。

また、前記「一般化力ベクトル」は、その各成分が、一般化変数ベクトルの各成分に対応しているベクトル（一般化変数ベクトルと同数の成分からなるベクトル）である。この一般化力ベクトルは、より詳しくは、その各成分が、それに対応する一般化変数ベクトルの成分値の時間的変化を発生させる力（並進力又はモーメント）となるベクトルである。

また、前記「一般化変数観測情報」は、前記一般化変数ベクトルの成分（関節の変位量を含む）の実際の値に関する「観測値」（詳しくは、該成分の実際の瞬時値又はその時間的変化率（１階微分値）の観測値）を構成要素とする情報を意味する。この場合、「観測値」は、センサによる直接的な検出値でよいことはもちろんであるが、該「観測値」と相関性を有する１つ以上の他の物理量の検出値から、適当なモデルもしくは自然法則に基づいて推定される推定値でもよい。

かかる第１発明によれば、前記式０１に基づいて、目標一般化力ベクトルτcmdを算出するために必要なパラメータとして、前記基本パラメータ群（イナーシャ行列Ｍ、重力依存一般化力ベクトル値Ｇ、遠心力・コリオリ力依存一般化力ベクトル値Ｎ、又はＭ、Ｇ）と、前記接触部位ヤコビアンＪcと、前記状態量ヤコビアンＪsとがそれぞれ、前記基本パラメータ群算出手段、接触部位ヤコビアン算出手段、状態量ヤコビアン算出手段により算出される。

なお、前記基本パラメータ群に関して補足すると、遠心力・コリオリ力依存一般化力ベクトル値Ｎと重力依存一般化力ベクトル値Ｇとの総和（＝Ｎ＋Ｇ）は、Ｎ＋Ｇ≒Ｇとなる場合がある。このような場合には、基本パラメータ群として、Ｍ及びＧだけを算出するようにしてもよい。

また、前記接触部位ヤコビアンＪcに係わる前記接触部位代表要素は、外界との接触によって制約を受けることとなるロボットの前記１つ以上の接触部位の運動を代表する要素（前記１つ以上の接触部位の運動に応じて、その運動（位置や姿勢角、もしくは、それらの時間的変化率等）が規定される部位）を意味する。なお、「姿勢角」は、角度を単位として表現される空間的な向きを意味する。該接触部位代表要素は、実際の接触部位、あるいは、それに含まれる特定の部分もしくは点でもよいが、接触部位との位置関係もしくは姿勢関係が所定の関係を有する仮想的な要素であってもよい。

また、前記接触状態情報としては、前記１つ以上の接触部位と外界との接触状態がどのような状態であるかを示す情報（例えばどの接触部位が外界に接触しているかの情報や、各接触部位が外界との接触によって受ける外力の状態等）である。

そして、前記目標駆動力決定手段は、上記基本パラメータ群（Ｍ，Ｇ，Ｎ、又はＭ，Ｇ）、接触部位ヤコビアンＪc及び状態量ヤコビアンＪsと、前記接触部位代表要素運動加速度目標値Ｃと、前記一般化変数観測情報（詳しくは、該一般化変数観測情報から認識されるｑ'）と、前記所定種類の状態量の１階微分値の目標値Ｓ'とを入力データとして用いて、前記式０１の関係を満足する一般化力ベクトルの目標である目標一般化力ベクトルτcmdのうち各関節の変位量に対応する成分値を算出し、その算出した成分値を該関節に付与する目標駆動力として決定する。

このため、第１発明では、前記接触部位代表要素運動加速度目標値と、前記所定種類の状態量の１階微分値の目標値とをロボットの運動を制約する目標として、各関節に付与する目標駆動力が決定されることとなる。

ここで、前記接触部位の運動は、ロボットの一部分の運動であるので、前記接触部位代表要素運動加速度目標値を実現するためのロボットの各関節の運動には冗長性を有する。換言すれば、接触部位代表要素の運動加速度の自由度数がロボット全体の自由度数（一般化変数ベクトルの自由度数）よりも小さい。

また、ロボットに所要の動作を行なわせる場合、一般には、全ての関節の運動そのものを目標とする運動に一致させることが必須となるものではなく、それらの関節の運動に依存して値が変化する（関節の運動の関数値として表される）、ある状態量（例えばロボットの全体重心点の運動状態等）を適切な目標に制御することが肝要となる。そして、一般に、このような状態量に関する目標を実現するためのロボットの各関節の運動にも冗長性を有する。換言すれば、該状態量の自由度数がロボット全体の自由度数（一般化変数ベクトルの自由度数）よりも小さい。

従って、前記接触部位代表要素運動加速度目標値と、前記所定種類の状態量の１階微分値の目標値とをロボットの運動を制約する目標として、前記式０１に基づいて、各関節に付与する目標駆動力を決定することによって、ロボットの関節の運動の任意性が高まる。

また、前記接触部位代表要素運動加速度目標値を用い、接触部位代表要素の運動を規定することによって、前記接触部位でロボットに作用する外力（これは、ロボットの各関節に付与すべき駆動力と相互に影響を及ぼし合う）を特定することなく、前記所定種類の状態量の１階微分値の目標値を実現し得る各関節の目標駆動力を前記式０１に基づいて決定できる。

この結果、第１発明によれば、ロボットの関節の動きの柔軟性を高めつつ、各関節に付与する駆動力をロボットに所望の動作を行なせる上で適切な駆動力に制御することができる。

補足すると、前記第１発明において、前記式０１の右辺の行列(Ｊs＊Ｍ^-1＊Ｐc)（以降、これを行列Ａsということがある）が正則行列である場合には、後述する式５−２１ａにより決定される行列Ｂs（すなわちＡsの逆行列）を用いて、後述の式５−２２の右辺の演算を行なうことで、前記目標一般化力ベクトルτmcdを算出すればよい。また、行列Ａsが正則行列でない場合には、例えば、後述する式５−２１ｂにより決定される行列Ｂsを用いて、後述の式５−２２の右辺の演算を行なうことで、前記目標一般化力ベクトルτmcdを算出することができる。

かかる第１発明においては、前記接触部位代表要素の運動の自由度数が接触によって拘束される実際の自由度数よりも多いと、ロボットの関節の運動が、前記接触部位代表要素運動加速度目標値による制約を受けやすくなる。従って、ロボットの関節の運動の任意性を高める上では、前記接触部位代表要素の運動の自由度数の実際の自由度数に対する超過が極力少ないことが望ましい。また、接触部位代表要素は、その運動が不連続的な変化を生じないことが望ましい。

このような要望を考慮すると、例えば、前記ロボットが、２つの脚リンクの先端部を床面に交互に接地させる運動により該床面上を移動する２脚移動ロボット（すなわち、２つの脚リンクの先端部が外界との接触部位として機能するロボット）である場合には、次の構成を採用することが好適である。

すなわち、前記接触部位代表要素は、その位置及び姿勢が、前記２つの脚リンクにそれぞれ作用する床反力の合力である全床反力に応じたばね性変位を生じる要素であって、且つ、該接触部位代表要素の位置及び姿勢のばね性変位量が、前記２つの脚リンクのうちの第１の脚リンクに作用する床反力に応じて生じる該第１の脚リンクの先端部の位置及び姿勢のばね性変位量と、前記２つの脚リンクのうちの第２の脚リンクに作用する床反力に応じて生じる該第２の脚リンクの先端部の位置及び姿勢のばね性変位量と次式０２の関係を有する要素であり、前記接触部位ヤコビアン算出手段は、次式０３の右辺の演算によって、前記接触部位ヤコビアンＪcを算出する（第２発明）。

Ｘcc＝ｒat＊Ａ1＊Ｘ1＋（１−ｒat）＊Ａ2＊Ｘ2 ……式０２

Ｊc＝ｒat＊Ａ1＊Ｊ1＋（１−ｒat）＊Ａ2＊Ｊ2 ……式０３

但し、
Ｘcc：接触部位代表要素の位置及び姿勢のばね性変位量を成分とするベクトル
ｒat：前記第１の脚リンクのみが接地している状態でｒat＝０、前記第２の脚リンクのみが接地している状態でｒat＝１となり、且つ、第１の脚リンク及び第２の脚リンクの両方が接地している状態では、０≦ｒat≦１の範囲で連続的に変化するように前記接触状態情報に応じて設定される係数
Ｘ1：前記第１の脚リンクに作用する床反力に応じて生じる該第１の脚リンクの先端部の位置及び姿勢のばね性変位量を成分とするベクトル
Ｘ2：前記第２の脚リンクに作用する床反力に応じて生じる該第２の脚リンクの先端部の位置及び姿勢のばね性変位量を成分とするベクトル
Ａ1：前記第１の脚リンクに作用する床反力の並進力成分とモーメント成分とからなる第１床反力ベクトルを、前記接触部位代表要素に作用する床反力ベクトルに変換する行列
Ａ2：前記第２の脚リンクに作用する床反力の並進力成分とモーメント成分とからなる第２床反力ベクトルを、前記接触部位代表要素に作用する床反力ベクトルに変換する行列
Ｊ1：X1の１階微分値X1'（＝dX1'/dt）と一般化変数ベクトルｑの１階微分値q'との間の関係をX1'＝Ｊ1＊ｑ'により表すヤコビアン
Ｊ2：X2の１階微分値X2'（＝dX2'/dt）と一般化変数ベクトルｑの１階微分値q'との間の関係をX2'＝Ｊ2＊ｑ'により表すヤコビアン

この第２発明によれば、前記接触部位代表要素は、その位置及び姿勢が、前記２つの脚リンクにそれぞれ作用する床反力の合力である全床反力に応じたばね性変位を生じる要素であって、且つ、該接触部位代表要素の位置及び姿勢のばね性変位量が、前記２つの脚リンクのうちの第１の脚リンクに作用する床反力に応じて生じる該第１の脚リンクの先端部の位置及び姿勢のばね性変位量と、前記２つの脚リンクのうちの第２の脚リンクに作用する床反力に応じて生じる該第２の脚リンクの先端部の位置及び姿勢のばね性変位量と前記式０２の関係を有する要素として定義される。

このように定義される接触部位代表要素は、ロボットの２つの脚リンクのうちの一方の脚リンクの先端部のみが接地する（床に接触する）状態と、両方の脚リンクの先端部が接地する状態とのいずれの状態であっても、その運動の自由度数が、並進運動の３つの自由度数と回転運動の３つの自由度数とから成る総計６つの自由度数で済む。すなわち、接触部位代表要素の運動の自由度数は、単一の脚リンクの先端部の自由度数と同じ自由度数で済む。

従って、接触部位代表要素の運動を前記接触部位代表要素運動加速度目標値により拘束した場合、それによるロボットの自由度数の制約分は６自由度で済む。これにより、接触部位代表要素の運動の自由度数の実際の自由度数に対する超過を極力少ないものとしつつ、ロボットの運動の任意性をより一層高めることができる。

しかも、前記係数ｒatを０≦ｒat≦１の範囲内で連続的に変化させることによって、接触部位代表要素の運動の連続性を実現できる。ひいては、ロボットの各関節の目標駆動力が不連続的に変化しないようにして、ロボットの運動の安定性を高めることができる。

そして、第２発明では、上記のように定義される接触部位代表要素に関する接触部位ヤコビアンＪcを前記式０３により算出する。この場合、前記行列Ａ1，Ａ2とヤコビアンＪ1，Ｊ2と算出する必要があるが、は、前記行列Ａ1，Ａ2は、脚リンクの接地状態（床との接触状態）に依存するので、例えば前記接触状態情報に基づいて前記行列Ａ1，Ａ2を決定できる。

また、X1の１階微分値X1'（＝dX1'/dt）は、前記第１の脚リンクの先端部の運動速度（並進速度及び回転速度）に一致し、また、X2の１階微分値X2'（＝dX2'/dt）は、前記第２の脚リンクの先端部の運動速度（並進速度及び回転速度）に一致すると考えてよい。従って、式０３のヤコビアンＪ1、Ｊ2を、前記一般化変数観測情報に基づいて算出することができる。

よって、前記式０３の右辺の演算によって、接触部位ヤコビアンを算出することができる。

上記第１発明又は第２発明においては、前記アクチュエータ制御手段は、前記ロボットの各関節に実際に付与する駆動力を前記決定された目標駆動力に一致させ、且つ前記接触部位代表要素の実際の運動加速度を前記接触部位代表要素運動加速度目標値を一致させたと仮定した場合に前記接触部位に作用する外力の推定値としての接触部位外力目標値を、前記決定された目標駆動力と接触部位運動加速度目標値とを用いて逆動力学演算の処理を実行することにより決定する接触部位外力目標値決定手段と、該接触部位外力目標値と前記接触部位に実際に作用する外力の観測値との偏差を“０”に収束させるように、フィードバック制御則により前記目標駆動力の補正量を決定する駆動力補正量算出手段とを含み、該補正量により補正した前記目標駆動力に応じて、前記アクチュエータの動作を制御することが好ましい（第３発明）。

この第３発明によれば、前記ロボットの各関節に実際に付与する駆動力を前記決定された目標駆動力に一致させ、且つ前記接触部位代表要素の実際の運動加速度を前記接触部位代表要素運動加速度目標値に一致させた仮定した場合に前記接触部位に作用する外力の推定値が、前記接触部位外力目標値として決定される。そして、この接触部位外力目標値と前記接触部位に実際に作用する外力の観測値との偏差を“０”に収束させるように、フィードバック制御則により決定された補正量によって前記目標駆動力が補正される。そして、この補正後の目標駆動力に応じて前記アクチュエータの動作が制御される。

この結果、第３発明によれば、前記式０１の基礎となるモデルの誤差等の影響を補償し、前記式０１に基づき決定された目標駆動力に対応する接触部位外力目標値が、前記接触部位に実際に作用する外力と不整合なものとなるのを防止することができる。ひいては、ロボットの動作の安定性を高めることができる。

本発明の実施形態を適用するロボットの概略構成を模式的に示す図。 図１のロボットに備える関節駆動系の概略構成を模式的に示す図。 図１のロボットの制御装置の主要な機能を示すブロック図。 図３に示す目標関節駆動トルク決定部の機能を示すブロック図。 図５（ａ）〜（ｃ）は図４に示す接触部ヤコビアン決定部の第１実施形態での処理を説明するための図。 図４に示す接触部ヤコビアン決定部の第２実施形態での処理を説明するための図。

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

図１は、本実施形態で例示するロボットの概略構成を模式的に示している。このロボット１は、脚式移動ロボットとしての２足移動ロボットである。このロボット１を構成するリンクは、大別すると、上体リンク２と、左右一対の脚リンク３Ｒ，３Ｌと、左右一対の腕リンク４Ｒ，４Ｌと、頭部５とを有する。

なお、本明細書では、ロボット１の前方に向かって右側の部材又はその部材に関連する量を示す変数に符号“Ｒ”を付加し、ロボット１の前方に向かって左側の部材又はその部材に関連する量を示す変数に符号“Ｌ”を付加する。ただし、右側、左側を特に区別する必要が無いときは、符号“Ｒ”，“Ｌ”を省略することがある。

上体リンク２は、ロボット１の基体としての下側上体１０と、この下側上体１０の上方に配置された上側上体１１とから構成されている。上側上体１１は、ヨー軸周り（Ｚ軸周り）の回転自由度を有する上体回旋用関節１２を介して下側上体１０に連結されている。

左右一対の脚リンク３Ｒ，３Ｌは、互いに同じ構造のものである。具体的には、各脚リンク３は、下側上体１０に股関節部１３を介して連結された大腿部１４と、この大腿部１４に膝関節部１５を介して連結された下腿部１６と、この下腿部１６に足首関節部１７を介して連結された足平部１８とを、該脚リンク３を構成する複数のリンクとして備える。

この場合、股関節部１３は、ヨー軸周り、ピッチ軸周り（Ｙ軸周り）及びロール軸周り（Ｘ軸周り）の回転自由度をそれぞれ有する３つの関節１９，２０，２１により構成されている。また、膝関節部１５は、ピッチ軸まわりの回転自由度を有する関節２２により構成されている。また、足首関節部１７は、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する２つの関節２３，２４により構成されている。従って、各脚リンク３は、本実施形態では、下側上体１０に対して、６自由度の運動自由度を有する。なお、上記の説明における各脚リンク３の関節１９〜２４の回転軸（ロール軸、ピッチ軸、ヨー軸）は、脚リンク３を上下方向に延在させた状態での回転軸を示している。

左右一対の腕リンク４Ｒ，４Ｌは、互いに同じ構造のものである。具体的には、各腕リンク４は、上側上体１１に肩関節部２５を介して連結された上腕部２６と、この上腕部２６に肘関節部２７を介して連結された前腕部２８と、この前腕部２８に手首関節部２９を介して連結されたハンド３０とを、該腕リンク４を構成する複数のリンクとして備える。

この場合、肩関節部２５は、ピッチ軸周り、ロール軸周り、及びヨー軸周りの回転自由度をそれぞれ有する３つの関節３１，３２，３３により構成されている。また、肘関節部２７は、ピッチ軸周りの回転自由度を有する関節３４により構成されている。また、手首関節部２９は、ヨー軸周り、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する３つの関節３５，３６，３７により構成されている。従って、各腕リンク４は、本実施形態では、上側上体１１に対して、７自由度の運動自由度を有する。なお、上記の説明における各腕リンク４の関節３１〜３７の回転軸（ロール軸、ピッチ軸、ヨー軸）は、腕リンク４を上下方向に延在させた状態での回転軸を示している。

頭部５は、上側上体１１の上方に配置されており、頸関節部３８を介して上側上体１１に連結されている。この場合、頸関節部３８は、ピッチ軸周り及びヨー軸周りの回転自由度をそれぞれ有する２つの関節３９，４０により構成されている。従って、頭部５は、上側上体１１に対して、２自由度の運動自由度を有する。なお、関節３９，４０の上記の回転軸（ピッチ軸、ヨー軸）は、頸関節部３８を上下方向に伸ばした状態での回転軸を示している。

以上の構成を有するロボット１は、各脚リンク３の６個の関節１９〜２４を駆動することによって、各脚リンク３の空間的な運動が行なわれ、この運動によって床上を移動することが可能となっている。例えば、人の歩行動作あるいは走行動作と同様の形態（歩容）で脚リンク３Ｒ，３Ｌを運動させることによって、ロボット１の歩行動作あるいは走行動作が行なうことが可能である。この場合、ロボット１の各リンクは、その空間的な位置及び姿勢（空間的な向き）に関して６自由度を有する。

また、各腕リンク４の７個の関節３１〜３７を駆動することによって、各腕リンク４の空間的な運動が行なわれ、この運動によって各腕リンク４のハンド３０を、適宜の物体に接触させる等の作業を行なうことが可能となっている。

また、本実施形態では、上体リンク２を構成する下側上体１０と、上側上体１１とが関節１２を介して連結されているので、この関節１２を駆動することによって、上体リンク２を捻ることが可能となっている。

また、頸関節部３８の関節３９，４０を駆動することによって、頭部５のチルト動作（ピッチ軸周りの回動動作）あるいはパン動作（ヨー軸周りの回動動作）を行なうことが可能となっている。

補足すると、本実施形態のロボット１は、腕リンク４Ｒ，４Ｌ及び頭部５を有する人型のロボットであるが、これらの腕リンク４Ｒ，４Ｌ、あるいは、頭部５を備えないロボットであってもよい。また、上体リンク２の関節１２を省略し、下側上体１０及び上側上体１１を一体に構成してもよい。

図１での具体的な図示は省略するが、ロボット１には、上記の各関節を回転駆動するための動力を出力するアクチュエータとしての電動モータを含む関節駆動系（関節の駆動機構）が備えられている。そして、本実施形態では、各関節に柔軟性を持たせるために、各関節と電動モータとの間の動力伝達系には、以下に説明する如く、電動モータの動力を弾性力に変換して関節の被動部材に付与するばね部材が介装されている。

図２は、各関節毎の上記関節駆動系の概略構成を模式的に示している。この関節駆動系は、電動モータ５０と、関節の被動部材５１（以降、関節被動部５１という）との間の動力伝達要素として、波動歯車装置等から構成された減速機５２とばね部材であるトーションバー５３とを備える。そして、電動モータ５０の出力軸５０ａが減速機５２とトーションバー５３とを順に介して関節被動部５１に連結されている。従って、この関節駆動系は、電動モータ５０が出力する動力（トルク）を、減速機５２とトーションバー５３とを順に介して関節被動部５１に伝達するように構成されている。この場合、電動モータ５０が出力する動力（トルク）は、トーションバー５３で弾性力（捩り力）に変換され、この弾性力が関節被動部５１に駆動トルクとして付与される。

補足すると、ここで言うトーションバー５３（ばね部材）は、減速機５２と関節被動部５１との間に付加された弾性体、又は電動モータ５０と関節被動部５１との間の動力伝達系に元々存在する弾性体を総称的に表現するものである。従って、減速機５２が、ハーモニックドライブ（登録商標）等の波動歯車装置のように、その出力段に元々、弾性体を有するものである場合には、その弾性体もトーションバー５３（ばね部材）に含まれる。

なお、関節被動部５１は、これを有する関節によって互いに相対回転自在に連結された一対の部材のうちの一方である。例えば、前記膝関節部１５の関節２２を駆動する関節駆動系にあっては、大腿部１４及び下腿部１６のうちの一方のリンクが関節被動部５１となり、他方のリンクに電動モータ５０及び減速機５２のハウジングが固定される。

以上のように本実施形態では、関節駆動系にばね部材としてのトーションバー５３を備えることによって、各関節の柔軟な動作が可能となっている。

本実施形態では、以上の構造を有するロボット１の動作制御を行なうために、ＣＰＵ、ＲＡＭ、ＲＯＭ等を含む電子回路ユニットにより構成された制御装置６０と、各種のセンサとが備えられている。

この場合、センサとしては、図１に示すように、ロボット１の基体である下側上体１０の角速度や姿勢角（鉛直方向に対する傾斜角度やヨー軸周りの方位角）等を計測するために該下側上体１０に搭載された姿勢センサ６１と、各足平部１８がその接地時に受ける外力（床反力）を計測するために各脚リンク３の足首関節部２４と足平部１８との間に介装された力センサ６２（６２Ｒ，６２Ｌ）と、各ハンド３０がその接触対象物体から受ける外力を計測するために各腕リンク４の手首関節部２９とハンド３０との間に介装された力センサ６３（６３Ｒ，６３Ｌ）とが備えられている。

上記姿勢センサ６１は、例えば３軸周りの角速度を検出するジャイロセンサと３軸方向の加速度を検出する加速度センサとから構成される。また、各力センサ６２，６３は、それぞれ、例えば３軸方向の並進力及び３軸周りのモーメントを検出する６軸力センサにより構成される。なお、姿勢センサ６１を構成する加速度センサは、下側上体１０の移動速度を計測するためにも使用される。

また、本実施形態では、図２に示すように、各関節毎の関節駆動系には、電動モータ５０の出力軸５０ａの回転角度（ロータの回転角度）を検出する角度センサとしてのロータリーエンコーダ６４と、トーションバー５３の両端部間の回転角度差を検出する角度センサとしての差分エンコーダ６５とが備えられている。

なお、差分エンコーダ６５は、本実施形態では、トーションバー５３の両端部に各々固定された一対の円板６５ａ，６５ｂを有し、これらの円板６５ａ，６５ｂの一方に対する他方の相対的な回転角度に応じた信号をトーションバー５３の両端部間の回転角度差に応じた信号として出力するセンサである。

これらのロータリエンコーダ６４及び差分エンコーダ６５は、電動モータ５０の出力軸５０ａの回転角度（又はトーションバー５３の減速機５２側の一端部の回転角度（＝減速機５２の出力軸の回転角度））とその時間的変化率（回転角速度）、関節被動部５１の回転角度とその時間的変化率（回転角速度）、トーションバー５３の両端部間の回転角度差（＝トーションバー５３の捩れ角）等を計測するために使用されるセンサである。

なお、差分エンコーダ６５の代わりに、関節被動部５１の回転角度を検出するロータリーエンコーダ等の角度センサを備えるようにしてもよい。また、ロータリエンコーダ６４の代わりに、減速機５２の出力軸の回転角度を検出するロータリエンコーダ等の角度センサを備えるようにしてもよい。

また、以降の説明では、ロボット１の各関節の関節被動部５１の回転角度を、関節の回転角度又は関節角ということがある。

制御装置６０には、上記の各センサの検出出力（あるいは、該検出出力を適宜のフィルタに通してなるフィルタリング値）が入力される。そして、制御装置６０は、これらの入力値から認識される所要の計測値（下側上体１０の姿勢角やその時間的変化率（角速度）の計測値、下側上体１０の移動速度の計測値、各足平部１８やハンド３０に作用する外力の計測値、各関節の回転角度やその時間的変化率（角速度）の計測値など）を使用して、ロボット１に目標とする運動を行なわせるために各関節の関節被動部５１に付与すべき駆動トルクの目標値（以降、目標関節駆動トルクという）を決定すると共に、この目標関節駆動トルクに応じて各関節駆動系の電動モータ５０の出力トルクを制御する処理を実行する。

以下に制御装置６０の制御処理の詳細を説明する。なお、本実施形態では、ロボット１の目標とする運動が、床上を歩行動作により移動する運動である場合の制御処理を例にとって説明する。

図３に示すように、制御装置６０は、実装されたプログラム等により実現される主要な機能部として、歩容生成部７１、目標関節駆動トルク決定部７２及び関節駆動制御部７３を備える。

歩容生成部７１は、ロボット１の目標運動を少なくとも含む目標歩容を生成する処理を実行する機能部である。この歩容生成部７１が生成する目標運動は、ロボット１の各関節の変位量（回転角度）の目標値である目標関節角の時系列と、ロボット１の全体重心点の目標位置（以降、目標全体重心点ということがある）の時系列とを含む。

上記目標運動は、例えば、本願出願人が前記特許文献１等で提案した歩容生成手法を用いて生成される。

この歩容生成手法を概略的に説明すると、ロボット１の動作環境の床形状情報やロボット１の移動方向の要求等に基づいて、ロボット１の両足平部１８Ｒ，１８Ｌのそれぞれの目標とする着地予定位置及び着地予定姿勢と着地予定時刻とが決定される。

そして、これらの着地予定位置及び姿勢と、着地予定時刻とに応じて、各足平部１８の目標位置及び目標姿勢の軌道（時系列パターン）である足平位置姿勢軌道が決定される。さらに、この足平位置姿勢軌道に基づいて、ＺＭＰ（Zero Moment Point）の目標位置（以降、目標ＺＭＰということがある）の軌道である目標ＺＭＰ軌道とが決定される。

さらに、ロボット１の全体の運動によって発生する慣性力と、ロボット１に作用する重力との合力によって目標ＺＭＰ周りに発生するモーメントの水平成分が“０”になるという動力学的な平衡条件を満たしつつ、ロボット１の将来的な安定性を確保し得るように、ロボット１の両脚リンク３Ｒ，３Ｌ以外の各リンク（上体リンク２等）の目標位置及び目標姿勢の軌道が決定される。

そして、これらの目標位置及び目標姿勢から、ロボット１の幾何学モデル（剛体リンクモデル）を用いて、各関節の目標関節角が決定される。

なお、目標歩容は、ロボット１の実際の動作時にリアルタイムで生成されるものでなくてもよく、事前に（オフラインで）作成した目標歩容を、制御装置６０に外部から無線通信等により入力したり、あるいは、ロボット１の図示しない記憶装置に事前に記憶保持しておくようにしてもよい。この場合には、歩容生成部７１は不要である。

また、目標歩容には、ロボット１の目標運動以外に、足平部１８Ｒ，１８Ｌの一方又は両方に作用する全床反力（トータルの床反力）の特定成分（例えば並進力の鉛直方向成分）の目標値や、全床反力の作用点の目標位置（すなわち目標ＺＭＰ）等が含まれていてもよい。本実施形態では、歩容生成部７１が出力する目標歩容には、目標ＺＭＰが含まれる。

目標関節駆動トルク決定部７２は、歩容生成部７１が生成した上記目標運動を実現する（ロボット１の実際の運動を目標運動に追従させる）ための各関節の目標関節駆動トルクを決定する処理を逐次実行する機能部である。この目標関節駆動トルク決定部７２は、その詳細は後述するが、本発明に係わる中核的な処理を実行する機能部である。

関節駆動制御部７３は、目標関節駆動トルク決定部７２が逐次決定した各関節の目標関節駆動トルク（詳しくは、後述の補正後目標関節駆動トルク↑τtc_cmdにより規定される各関節の目標関節駆動トルク）を実現するように（各関節の関節被動部５１に実際に付与される駆動トルクを目標関節駆動トルクに追従させるように）、各関節駆動系の電動モータ５０の出力トルクを制御する処理を逐次実行する機能部である。

この関節駆動制御部７３は、例えば、次のように電動モータ５０の出力トルクを制御する。すなわち、関節駆動制御部７３は、前記トーションバー５３の両端部間の回転角度差（トーションバー５３の捩れ角）の計測値から、トーションバー５３が実際に発生している弾性力トルク（トーションバー５３から関節被動部５１に実際に付与される駆動トルク）を逐次推定する。さらに、関節駆動制御部７３は、この弾性力トルクの推定値と、目標関節駆動トルクとの偏差を“０”に収束させるように、電動モータ５０の出力軸５０ａの目標回転角速度を逐次設定する。

次いで、関節駆動制御部７３は、この目標回転角速度と、電動モータ５０の出力軸５０ａの回転角速度の計測値との偏差を“０”に収束させるように、電動モータ５０の目標出力トルクを決定する。そして、関節駆動制御部７３は、この目標出力トルクを電動モータ５０の通電電流の目標値に変換し、この目標値に、実際の通電電流をフィードバック制御することで、電動モータ５０の実際の出力トルクを目標出力トルクに制御する。

なお、関節駆動制御部７３の制御処理においては、トーションバー５３の振動の影響を補償したり、関節被動部５１に作用する外力の影響を補償しつつ、電動モータ５０の出力トルクを制御するようにしてもよい。

次に、説明を後回しにした、前記目標関節駆動トルク決定部７２の処理の詳細を説明する。

なお、以降の説明では、ベクトル（複数の成分の列）を表記する場合に、例えば“↑Ａ”というように、変数名の先頭に“↑”を付する。また、ベクトルは、特にことわらない限り、成分を縦に並べた縦ベクトル（列ベクトル）であるとする。また、本明細書では、乗算記号として「＊」を用いる。

図４に示すように、目標関節駆動トルク決定部７２は、その主要な機能として、基本パラメータ群決定部８１、接触部位ヤコビアン決定部８２、状態量ヤコビアン決定部８３、状態量目標値決定部８４、制御入力決定部８５及びコンプライアンス制御部８６を備える。なお、図４では、各機能部の主要な出力と、その入力先とを図示している。

目標関節駆動トルク決定部７２は、これらの機能部の処理を所定の演算処理周期で逐次実行することによって、各関節の目標関節駆動トルクを逐次決定する。

以下説明すると、目標関節駆動トルク決定部７２は、まず、基本パラメータ群決定部８１の処理と、接触部位ヤコビアン決定部８２の処理と、状態量ヤコビアン決定部８３の処理と、状態量目標値決定部８４の処理とを実行する。

基本パラメータ群決定部８１は、ロボット１の運動と力との間の関係（運動方程式）を表現するモデルであるロボット運動モデルのパラメータを算出する処理を逐次実行する機能部である。

ここで、ロボット運動モデルは、ロボット１に外界から作用する外力（床反力等）が“０”であるとした場合、次式１−１により表現される。

Ｍ＊↑ｑ''＋↑Ｎ＋↑Ｇ＝↑τ ……式１−１

この式１−１における↑ｑ''は、ロボット１の一般化変数ベクトル↑ｑの２階微分値（＝d²↑ｑ/dt²）、Ｍはイナーシャ行列、↑Ｎは遠心力・コリオリ力依存一般化力ベクトル、↑Ｇは重力依存一般化力ベクトル、↑τは駆動力依存一般化力ベクトルである。

上記一般化変数ベクトル↑ｑは、本実施形態では、ロボット１のあらかじめ定めた１つの代表リンクの位置及び姿勢（グローバル座標系で見た位置及び姿勢）と、各関節の変位量（回転角度）とを成分として、これらの成分を並べて構成されるベクトルである。上記代表リンクは、本実施形態では、ロボット１の基体としての下側上体１０である。以降、下側上体１０を代表リンク１０ということもある。

ここで、本実施形態の説明では、ロボット１の代表リンク１０を含めて、各リンクの「位置」は、より詳しくは、該リンクに対して固定的に設定された代表点のグローバル座標系での位置を意味する。また、各リンクの「姿勢」は、より詳しくは、グローバル座標系で見た該リンクの空間的な向き（角度の次元で表現される向き）を意味する（以降の説明では、「姿勢」を「姿勢角」ということもある）。

なお、上記グローバル座標系は、ロボット１の移動環境の床（地面を含む）に対して固定された座標系等、慣性座標系（近似的に慣性系と見なせる座標系を含む）である。

そして、各リンクの「位置」及び「姿勢」（姿勢角）は、本実施形態ではそれぞれ３自由度を有するので、それぞれ３つの座標成分により表現される。このため、本実施形態での一般化変数ベクトル↑ｑの成分個数（すなわち↑ｑの自由度数）は、代表リンク１０の位置及び姿勢のトータルの自由度数（＝６）と、ロボット１の全ての関節のトータルの自由度数との総和の個数である。なお、図１に示した本実施形態のロボット１では、全ての関節のトータルの自由度数は“２９”である。従って、↑ｑの自由度数は、“３５”である。

上記イナーシャ行列Ｍは、一般化変数ベクトル↑ｑの２階微分値↑ｑ''を、一般化力ベクトルに変換する行列（対称行列）、すなわち、↑ｑ''に乗じることによって得られるベクトル（＝Ｍ＊↑ｑ''）の各成分値が、↑ｑ''の各成分に対応する力（並進力又はモーメント）の次元を有する値となる行列である。

ここで、↑ｑの各成分は、位置又は姿勢角又は回転角度であるから、↑ｑ''の各成分は、並進加速度又は角加速度の次元を有する。従って、Ｍ＊↑ｑ''の符号を反転させてなるベクトル（−Ｍ＊↑ｑ''）の各成分値が、ロボット１の運動によって発生する慣性力を意味する。この場合、ベクトル（−Ｍ＊↑ｑ''）のうち、代表リンク１０の「位置」の２階微分値（並進加速度）に対応する成分の慣性力は並進慣性力（並進力となる慣性力）であり、代表リンク１０の「姿勢」の２階微分値（角加速度）に対応する成分の慣性力と、各関節の回転角度の２階微分値（角加速度）に対応する成分の慣性力とは、それぞれ慣性力モーメント（モーメントとなる慣性力）である。

なお、イナーシャ行列Ｍの逆行列Ｍ^-1は対角行列となる。

一般化力ベクトルは、その各成分が、一般化変数ベクトル↑ｑの各成分に対応するベクトル（↑ｑと同数の成分からなるベクトル）である。この一般化力ベクトルは、より詳しくは、その各成分が、それに対応する↑ｑの成分値の変化（加速度）を発生させる力（並進力又はモーメント）となるベクトルである。この場合、一般化力ベクトルの各成分は、それに対応する↑ｑの成分の２階微分値に応じた慣性力と同じ種類の力である。

遠心力・コリオリ力依存一般化力ベクトル↑Ｎは、ロボット１の各リンクに作用する遠心力及びコリオリ力により発生する一般化力ベクトルである。また、重力依存一般化力ベクトル↑Ｇは、ロボット１の各リンクに作用する重力により発生する一般化力ベクトルである。なお、以降の説明では、↑Ｎと↑Ｇとの和（＝↑Ｎ＋↑Ｇ）を非線形項一般化力ベクトルということがある。

駆動力依存一般化力ベクトル↑τは、ロボット１の各関節を駆動するアクチュエータ（本実施形態では電動モータ５０）の駆動力によって発生する一般化力ベクトルである。本実施形態のロボット１では、この駆動力依存一般化力ベクトル↑τのうち、代表リンク１０の位置及び姿勢に対応する成分の値は常に“０”である。また、↑τのうち、各関節の回転角度に対応する成分の値が該関節に付与する駆動トルク（以下、関節駆動トルクということがある）を意味するものとなる。

基本パラメータ群決定部８１が算出するパラメータは、式１−１におけるイナーシャ行列Ｍと、遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））とから成る。

補足すると、ロボット１に床反力等の外力（≠０）が作用する場合には、ロボット運動モデルは、その外力に依存する一般化力ベクトルの項（外力項）を式１−１に付加した式（後述の式５−１）によって表現される。ただし、基本パラメータ群決定部８１が算出するパラメータである上記Ｍ、↑Ｎ、↑Ｇは、該外力には依存せずに、ロボット１の各関節の回転角度やその時間的変化率（角速度）等のロボット１の運動状態に依存してその値が定まるパラメータである。このため、基本パラメータ群決定部８１を説明するための上記式１−１では外力項を省略している。

上記のパラメータＭ、↑Ｎ、↑Ｇ（あるいは、Ｍ及び（↑Ｎ＋↑Ｇ））を算出するために、基本パラメータ群決定部８１には、代表リンク（下側上体）１０の移動速度の計測値と、代表リンク１０の姿勢角の計測値及びその時間的変化率（角速度）の計測値と、各関節の回転角度の計測値及びその時間的変化率（角速度）の計測値とが逐次入力される。これらの計測値は、前記姿勢センサ６１、ロータリーエンコーダ６４、差分エンコーダ６５の検出出力を基に、制御装置６０において認識された計測値である。

そして、基本パラメータ群決定部８１は、これらの入力された計測値を用いて、ＲＮＥＡ（Recursive Newton Euler Algorithm）、単位ベクトル法等の公知の手法によりＭ、↑Ｎ、↑Ｇ（あるいは、Ｍ及び（↑Ｎ＋↑Ｇ））を逐次算出する。

補足すると、↑Ｎの各成分値の大きさが、それに対応する重力依存一般化力ベクトル↑Ｇの成分値の大きさに比して十分に微小なものとなるか、もしくは“０”となり、ひいては、↑Ｎ＋↑Ｇ≒↑Ｇとなる場合もある。このような場合には、↑Ｎ、↑Ｇのうちの↑Ｎを無視し（非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ）が↑Ｇに一致するものとして）、↑Ｇだけを算出するようにしてもよい。

前記接触部位ヤコビアン決定部８２は、ロボット１の外界との接触部位を代表する要素としての接触部位代表要素の運動速度（詳しくは接触部位代表要素の並進速度及び回転速度（角速度）のうちの一方又は両方）と、一般化変数ベクトル↑ｑの１階微分値（時間的変化率）との間の関係を表現するヤコビアン（以降、接触部位ヤコビアンという）を逐次算出する処理を実行する機能部である。接触部位ヤコビアンは、換言すれば、一般化変数ベクトル↑ｑの各成分値の変化（微小変化）に対する接触部位代表要素の空間的な位置・姿勢（詳しくは位置及び姿勢のうちの一方又は両方）の変化の割合い（感度）を表現する行列である。なお、ロボット１の外界との接触部位を代表する要素としての接触部位代表要素は、より詳しくは、その運動が、外界に対して接触する全ての接触部位の該外界に対する接触状態（各接触部位の接触時の運動状態又は各接触部位に対する外力の作用状態）に依存して規定されるように定義された実体的又は仮想的な要素を意味する。

ここで、本実施形態で説明するロボット１の目標運動は、前記したようにロボット１の歩行動作を行なう運動である。この場合、ロボット１の外界との実際の接触部位としての接地部位（床面との接触部位）は、経時的に変化する。すなわち、歩行動作におけるロボット１の接地状態は、足平部１８Ｒ，１８Ｌの一方だけが接地部位となる片脚接地状態と、両方が接地部位となる両脚接地状態と、他方だけが接触部位となる片脚接地状態とが順番に繰り返される。このため、接触部位代表要素は、その位置・姿勢が、足平部１８Ｒ，１８Ｌの接地状態に応じて変化していくべきものである。

そこで、接触部位ヤコビアン決定部８２は、ロボット１の接触部位代表要素の位置・姿勢を規定するパラメータ（後述の比率値ｒat）の値を足平部１８Ｒ，１８Ｌの接地状態に応じて逐次決定しつつ、接触部位ヤコビアンを逐次算出する。この場合、接触部位代表要素の位置・姿勢は、足平部１８Ｒ，１８Ｌの接地状態の切り替わりの際、あるいは、片脚接地状態及び両脚接地状態のそれぞれの期間内で不連続に変化することが無いように（常時、連続的に変化していくように）決定される。

より詳しくは、片脚接地状態においては、接触部位代表要素は、その位置・姿勢が、足平部１８Ｒ，１８Ｌのうちの接地している足平部１８の位置及び姿勢に依存して定まる要素として設定される。また、両脚接地状態では、接触部位代表要素は、その位置・姿勢が、両足平部１８Ｒ，１８Ｌの位置及び姿勢と、両足平部１８Ｒ，１８Ｌに対する床反力の作用状態とに依存して定まる要素として設定される。

以下、接触部位ヤコビアン決定部８２の処理を具体的に説明する。

本実施形態では、接触部位代表要素は、外界との接触部位としての足平部１８Ｒ，１８Ｌの一方もしくは両方の接地時（床との接触時）に運動の拘束を受ける複数の所定数の点を連接してなる要素であると見なし、その所定数の点の運動（並進運動）によって該接触部位代表要素の運動（並進運動及び回転運動）が表現されるものとする。以降、その複数の所定数の点を接触部位代表点という。この接触部位代表点の個数は、足平部１８Ｒ，１８Ｌのいずれの接地状態においても一定に維持される。

具体的には、接触部位代表要素は、図５（ａ）〜（ｃ）に黒丸点で示すように、足平部１８Ｒ，１８Ｌのいずれの接地状態においても、４つの接触部位代表点a1，a2，a3，a4から構成される。なお、図５（ａ）〜（ｃ）では、足平部１８Ｒ，１８Ｌのうちの接地している足平部を実線で示し、離床している（空中を移動している）足平部を２点鎖線で示している。

この場合、４つの接触部位代表点a1〜a4の位置は、足平部１８Ｒ，１８Ｌの接地状態に応じた位置に規定される。

具体的には、左側足平部１８Ｌが接地し、且つ右側足平部１８Ｒが離床している片脚接地状態（左脚接地状態）においては、図５（ａ）に示す如く、左側足平部１８Ｌの底面の所定の位置（左側足平部１８Ｌに対して固定された位置）にあらかじめ設定された４つの所定点a1L，a2L，a3L，a4Lが、それぞれ接触部位代表点a1，a2，a3，a4として決定される。

その４つの所定点a1L〜a4Lの位置は、本実施形態では、左側足平部１８Ｌの底面の４隅の位置である。より詳しくは、所定点a1L〜a4Lの位置は、それぞれ順番に、左側足平部１８Ｌの底面の左前側の隅部、右前側の隅部、左後側の隅部、右後側の隅部の位置である。従って、左脚接地状態における接触部位代表点a1，a2，a3，a4の位置は、左側足平部１８Ｌの位置及び姿勢に応じて規定される位置である。

また、右側足平部１８Ｒが接地し、且つ左側脚リンク３Ｌの足平部１８Ｌが離床している片脚接地状態（右脚接地状態）においては、図５（ｃ）に示す如く、右側足平部１８Ｒの底面の所定の位置（右側足平部１８Ｒに対して固定された位置）にあらかじめ設定された４つの所定点a1R，a2R，a3R，a4Rが、それぞれ接触部位代表点a1，a2，a3，a4として決定される。

その４つの所定点a1R〜a4Rの位置は、左側足平部１８Ｌの４つの所定点a1L〜a4Lと同様に、右側足平部１８Ｒの底面の４隅の位置（左前側の隅部、右前側の隅部、左後側の隅部、右後側の隅部の位置）である。従って、右脚接地状態における接触部位代表点a1，a2，a3，a4の位置は、右側足平部１８Ｒの位置及び姿勢に応じて規定される位置である。

なお、右側足平部１８Ｒの４つの所定点a1R〜a4Rの相互の位置関係と、左側足平部１８Ｌの４つの所定点a1L〜a4Lの相互の位置関係とは同じである。

また、両足平部１８Ｒ，１８Ｌが接地する両脚接地状態においては、図５（ｂ）に示す如く、接触部位代表点a1，a2，a3，a4の位置は、それぞれ順番に、両足平部１８Ｒ，１８Ｌのそれぞれの上記所定点a1R，a1Lを結ぶ線分上の位置、上記所定点a2R，a2Lを結ぶ線分上の位置、上記所定点a3R，a3Lを結ぶ線分上の位置、上記所定点a4R，a4Lを結ぶ線分上の位置に決定される。

ここで、以降の説明では、任意の２つの点Ａ，Ｂを結ぶ線分を線分A_Bというように表記し、その線分A_Bの長さ（線分長）をＬ(A_B)と表記する。

この場合、両脚接地状態での接触部位代表点a1〜a4の位置は、より詳しくは、次のように規定された位置である。

すなわち、接触部位代表点a1，a2，a3，a4の位置は、次式２−１ａにより示される如く、線分長L(a1R_a1L)に対する線分長L(a1_a1L)の比率L(a1_a1L)／L(a1R_a1L)と、線分長L(a2R_a2L)に対する線分長L(a2_a2L)の比率L(a2_a2L)／L(a2R_a2L)と、線分長L(a3R_a3L)に対する線分長L(a3_a3L)の比率L(a3_a3L)／L(a3R_a3L)と、線分長L(a4R_a4L)に対する線分長L(a4_a4L)の比率L(a4_a4L)／L(a4R_a4L)とが互いに等しい比率値ｒat（０≦ｒat≦１）となるように決定される。

換言すれば、接触部位代表点a1，a2，a3，a4の位置は、次式２−１ｂにより示される如く、線分長L(a1R_a1L)に対する線分長L(a1_a1R)の比率L(a1_a1R)／L(a1R_a1L)と、線分長L(a2R_a2L)に対する線分長L(a2_a2R)の比率L(a2_a2R)／L(a2R_a2L)と、線分長L(a3R_a3L)に対する線分長L(a3_a3R)の比率L(a3_a3R)／L(a3R_a3L)と、線分長L(a4R_a4L)に対する線分長L(a4_a4R)の比率L(a4_a4R)／L(a4R_a4L)とが互いに等しい比率値（１−ｒat）となるように決定される。

L(a1_a1L)／L(a1R_a1L)＝L(a2_a2L)／L(a2R_a2L)
＝L(a3_a3L)／L(a3R_a3L)＝L(a4_a4L)／L(a4R_a4L)＝ｒat
……式２−１ａ

L(a1_a1R)／L(a1R_a1L)＝L(a2_a2R)／L(a2R_a2L)
＝L(a3_a3R)／L(a3R_a3L)＝L(a4_a4R)／L(a4R_a4L)＝１−ｒat
……式２−１ｂ

そして、両脚接地状態での接触部位代表点a1〜a4の位置を規定する上記比率値ｒatは、本実施形態では、右側足平部１８Ｒに実際に作用する並進床反力（床反力のうちの並進力）の鉛直方向成分Fz_act_Rと、左側足平部１８Ｌに実際に作用する並進床反力の鉛直方向成分Fz_act_Lとの比率に応じて決定される。詳しくは、接触部位ヤコビアン決定部８２は、次式２−２ａ又は２−２ｂにより、上記比率値ｒatを決定する。

ｒat＝Fz_act_R／（Fz_act_R＋Fz_act_L） ……式２−２ａ

ｒat＝１−Fz_act_L／（Fz_act_R＋Fz_act_L） ……式２−２ｂ

従って、両脚接地状態での接触部位代表点a1〜a4の位置は、両足平部１８Ｒ，１８Ｌの位置及び姿勢と、両足平部１８Ｒ，１８Ｌに対する床反力の作用状態とに応じて規定される位置に決定されることとなる。

なお、式２−２ａ又は２−２ｂにおいて、Fz_act_R＝０である場合には、ｒat＝０となるので、この場合の接触部位代表点a1，a2，a3，a4は、それぞれ、左側足平部１８Ｌの前記所定点a1L，a2L，a3L，a4Lに一致することとなる。また、式２−２ａ又は２−２ｂにおいて、Fz_act_L＝０である場合には、ｒat＝１となるので、この場合の接触部位代表点a1，a2，a3，a4は、それぞれ、右側足平部１８Ｒの前記所定点a1R，a2R，a3R，a4Rに一致することとなる。そして、左脚接地状態では、Fz_act_R＝０となり、右脚接地状態では、Fz_act_L＝０となる。

従って、両脚接地状態だけでなく、左脚接地状態及び右脚接地状態においても、式２−２ａ又は式２−２ｂにより比率値ｒatを決定し、この比率値ｒatに応じて、前記式２−１ａ又は２−１ｂを満たすように接触部位代表点a1，a2，a3，a4の位置を規定すれば、結果的に、左脚接地状態及び右脚接地状態においても、接触部位代表点a1，a2，a3，a4の位置が前記した位置に規定されることとなる。

そこで、本実施形態では、接触部位ヤコビアン決定部８２は、上記比率値ｒatを接触部位代表点a1，a2，a3，a4の位置（ひいては接触部位代表要素の位置及び姿勢）を規定するパラメータとして決定する処理を実行する。この処理は、具体的には、次のように実行される。

すなわち、上記比率値ｒat（以降、接触部位代表要素規定比率ｒatという）を決定するために、接触部位ヤコビアン決定部８２には、各足平部１８Ｒ，１８Ｌに作用する実際の並進床反力鉛直成分Fz_act_R，Fz_act_Lの計測値が入力される。なお、これらのFz_act_R，Fz_act_Lの計測値は、それぞれ、前記力センサ６３Ｒ，６３Ｌの検出出力等に基づき、制御装置６０において認識された計測値である。

そして、接触部位ヤコビアン決定部８２は、入力された並進床反力鉛直成分Fz_act_R，Fz_act_Lの計測値から、前記式２−２ａ又は式２−２ｂにより接触部位代表要素規定比率ｒatを算出する。

このように接触部位代表要素規定比率ｒatを決定することで、結果的に、４つの接触部位代表点a1〜a4のそれぞれの位置が連続的に変化していくように決定されることとなる。ひいては、これらの４つの接触部位代表点a1〜a4を連接してなる要素としての接触部位代表要素の位置及び姿勢が、連続的に変化していくように決定されることとなる。

補足すると、本実施形態では、上記の如く４つの接触部位代表点a1〜a4の位置が決定されるので、ロボット１の片脚接地状態においては、接地している足平部１８Ｒ又は１８Ｌそのものが接地部位代表要素に相当するものとなる。一方、ロボット１の両脚接地状態においては、接地部位代表要素は、両足平部１８Ｒ，１８Ｌの間に存在する仮想的な要素に相当するものとなる。

さらに、接触部位ヤコビン決定部８２は、上記のように決定した接触部位代表要素規定比率ｒatを用いて接触部位ヤコビアンを決定する。

この場合、本実施形態では、接触部位代表要素の運動が、４つの接触部位代表点a1〜a4の運動によって表現されるものとしているので、接触部位ヤコビアンは、接触部位代表点a1〜a4のそれぞれの位置を成分として並べて構成されるベクトル↑Ｘｃの時間的変化率（１階微分値）↑Ｘc'（＝d↑Ｘc'/dt）、すなわち、接触部位代表点a1〜a4のそれぞれの運動速度を成分として構成されるベクトル↑Ｘc'と、前記一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を次式２−３により表現する行列Ｊcである。

↑Ｘc'＝Ｊc＊↑ｑ' ……式２−３

上記ベクトル↑Ｘｃは、接触部位代表点a1〜a4により構成される接触部位代表要素の位置及び姿勢を表すものとしての意味を持ち、↑Ｘｃの時間的変化率↑Ｘc'は、該接触部位代表要素の運動速度を示すものとしての意味を持つ。以降、↑Ｘｃを接触部位代表要素位置ベクトルという。なお、接触部位代表点a1〜a4のそれぞれの運動速度（並進速度）は３自由度を有するので、↑Ｘc'は、３＊４＝１２個の成分から成るベクトルとして表現される。

ここで、接触部位代表点a1〜a4をそれぞれ右側足平部１８Ｒの前記所定点a1R〜a4Rに一致させた場合の接触部位代表要素位置ベクトル↑Ｘcを↑Ｘc_R、接触部位代表点a1〜a4をそれぞれ左側足平部１８Ｌの前記所定点a1L〜a4Lに一致させた場合の接触部位代表要素位置ベクトル↑Ｘcを↑Ｘc_Lとおく。また、↑Ｘc_Rの１階微分値↑Ｘc_R'（＝d↑Ｘc_R/dt）と、一般化変数ベクトル↑ｑの一階微分値↑ｑ'との間の関係を表すヤコビアンをＪc_R、↑Ｘc_Lの１階微分値↑Ｘc_L'（＝d↑Ｘc_L/dt）と、一般化変数ベクトル↑ｑの一階微分値↑ｑ'との間の関係を表すヤコビアンをＪc_Lとおく。すなわち、↑Ｘc_L'＝Ｊc_L＊↑ｑ'、↑Ｘc_R'＝Ｊc_R＊↑ｑ'とおく。

このとき、上記接触部位ヤコビアンＪcは、次式２−４により与えられる。

Ｊc＝ｒat＊Ｊc_R＋（１−ｒat）＊Ｊc_L ……式２−４

そこで、本実施形態では、接触部位ヤコビアン決定部８２は、式２−４の右辺のヤコビアンＪc_R、Ｊc_Lを算出し、これらのヤコビアンＪc_R、Ｊc_Lの算出値と、前記の如く算出した接触部位代表要素規定比率ｒatとから式２−４の右辺の演算を行なうことで、接触部位ヤコビアンＪcを算出する。

この場合、上記ヤコビアンＪc_R、Ｊc_Lを算出するために、接触部位ヤコビアン決定部８２には、下側上体１０の移動速度、並びに姿勢角及びその時間的変化率（角速度）の計測値と、各脚リンク３Ｒ，３Ｌの各関節の回転角度及びその時間的変化率（角速度）の計測値とが入力される。そして、接触部位ヤコビアン決定部８２は、これらの入力された計測値から、上記ヤコビアンＪc_R、Ｊc_Lを算出する。

以上のように接触部位ヤコビアン決定部８２の処理では、ロボット１に作用する並進床反力鉛直成分Fz_act_R，Fz_act_Lの計測値に応じて（ひいては両足平部１８R，１８Ｌの接地状態に応じて）、前記接触部位代表要素規定比率ｒatが決定される。また、一般化変数ベクトル↑ｑ及びその一階微分値↑ｑ'の所要の成分の計測値に基づき、ヤコビアンＪc_R、Ｊc_Lが算出される。そして、これらの接触部位代表要素規定比率ｒat、ヤコビアンＪc_R、Ｊc_Lの算出値から前記式２−４により接触部位ヤコビアンＪcが算出される。

補足すると、前記式２−２ａ又は２−２ｂの右辺の分母がほぼ一定に維持されるとみなせるような場合には、式２−２ａ又は２−２ｂの右辺の分母を、ロボット１の全体に作用する重力（＝ロボット１の全体質量＊重力加速度定数）に置き換えた式によって、前記接触部位代表要素規定比率ｒatを決定するようにしてもよい。

また、前記歩容生成部７１が生成・出力する前記目標歩容（あるいは制御装置６０に外部から与えられる目標歩容）が、足平部１８Ｒ，１８Ｌのそれぞれの並進床反力鉛直成分の目標値Fz_cmd_R，Fz_cmd_Lを含んでいる場合には、前記式２−２ａ又は２−２ｂのFz_act_R，Fz_act_Lを、それぞれ、Fz_cmd_R，Fz_cmd_Lに置き換えた式によって、前記接触部位代表要素規定比率ｒatを決定するようにしてもよい。

あるいは、例えば、式２−２ａ又は２−２ｂのFz_act_R，Fz_act_Lを、それぞれ、Fz_cmd_R，Fz_cmd_Lに置き換えた式によって算出されるｒatの値にローパス特性のフィルタリング処理を施してなるフィルタリング値と、式２−２ａ又は２−２ｂの演算により算出されるｒatの値にローカット特性（ハイパス特性）のフィルタリング処理を施してなるフィルタリング値とを合成する（加え合わせる）ことによって、前記接触部位代表要素規定比率ｒatを決定するようにしてもよい。

換言すれば、目標値Fz_cmd_R，Fz_cmd_Lの比率に応じて接触部位代表要素規定比率ｒatの低周波側の成分を決定すると共に、Fz_act_R，Fz_act_Lの計測値の比率に応じて接触部位代表要素規定比率ｒatの高周波側の成分を決定し、それらの成分を合成する（加え合わせる）ことによって、接触部位代表要素規定比率ｒatを決定するようにしてもよい。

この場合、上記ローパス特性のフィルタリング処理及びローカット特性のフィルタリング処理としては、例えば、それぞれの伝達関数が、１／（Ｔc＊ｓ＋１）、Ｔc＊ｓ／（Ｔc＊ｓ＋１）（Ｔc：所定の時定数）により表されるフィルタを用いることができる。

また、本実施形態では、接触部位代表要素の運動を表現するための接触部位代表点として４つの点a1〜a4を用いたが、接触部位代表点として、３つ、もしくは５個以上の点を用いるようにすることも可能である。

前記状態量ヤコビアン決定部８３は、ロボット１の歩行動作を安定に行なう上で適切な目標値に制御することが望ましいものとしてあらかじめ定められた状態量に関して、その状態量（以降、制御対象状態量ということがある）と、一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を表現するヤコビアン（以降、状態量ヤコビアンという）を算出する処理を逐次実行する機能部である。

制御対象状態量は、その値が、一般化変数ベクトル↑ｑのいずれかの成分の値の変化に応じて変化する状態量である。より詳しくは、制御対象状態量は、その値（瞬時値）が、ある状態量ヤコビアンを、一般変数ベクトル↑ｑの１階微分値↑ｑ'に乗じてなる値となるような状態量である。

本実施形態では、制御対象状態量として、グローバル座標系で見たロボット１の全並進運動量↑Ｐtotalと全角運動量↑Ｌtotal（詳しくはグローバル座標系の所定の基準点周りの全角運動量↑Ｌtotal）との組を使用する。なお、グローバル座標系としては、本実施形態では、例えば、ロボット１の足平部１８Ｒ，１８Ｌのうちの支持脚側の足平部の接地位置に応じた位置（例えば接地面内の位置）に原点が設定される座標系（所謂、支持脚座標系）が使用される。

上記全並進運動量↑Ｐtotalは、より詳しくは、ロボット１の各リンクの重心点の運動量（＝リンクの質量＊該リンクの重心点の移動速度）をロボット１の全てのリンクについて加え合わせてなる並進運動量である。換言すれば、↑Ｐtotalは、ロボット１の全体重心の並進運動量である。この↑Ｐtotalは、本実施形態では、３自由度のベクトル（ロール軸方向、ピッチ軸方向及びヨー軸方向の３軸方向の成分から成るベクトル）である。

なお、以降の説明では、ロール軸は、ロボット１の前後方向での水平軸、ピッチ軸はロボット１の左右方向での水平軸、ヨー軸は鉛直方向（重力方向）の軸を意味する。

また、上記全角運動量↑Ｌtotalは、ロボット１の各リンクの重心点の並進運動による上記基準点周りの角運動量（＝基準点に対するリンクの重心点の位置ベクトルと、該重心点の並進運動量とのベクトル積）と、各リンクの重心点周りの回転運動による角運動量（＝リンクの慣性モーメント＊該リンクの重心点周りの角速度）とを、全てのリンクについて加え合わせてなる角運動量である。この全角運動量↑Ｌtotalは、本実施形態では、ヨー軸周りの成分を除く２自由度のベクトル（ロール軸及びピッチ軸の２軸周りの成分から成るベクトル）である。なお、上記基準点は、前記グローバル座標系にあらかじめ設定された点（例えば前記支持脚座標系の原点）である。

状態量ヤコビアン決定部８３は、上記の制御対象状態量に対応する状態量ヤコビアンを決定する。この場合、この状態量ヤコビアンは、全並進運動量↑Ｐtotalと全角運動量↑Ｌtotalとを成分として並べて構成されるベクトル↑Ｓ1（＝［↑Ｐ，↑Ｌ］^T。以降、状態量ベクトル↑Ｓ1という）と、前記一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を次式３−１により表現する行列Ｊs1である。

↑Ｓ1＝Ｊs1＊↑ｑ' ……式３−１

なお、本実施形態では、全並進運動量↑Ｐtotal及び全角運動量↑Ｌtotalは、それぞれ、３自由度、２自由度のベクトルであるので、↑Ｓ1は、３＋２＝５個の成分から成るベクトルである。

この状態量ヤコビアンＪs1を算出するために、状態量ヤコビアン決定部８３には、代表リンク（下側上体）１０の移動速度、並びに姿勢角及びその時間的変化率（角速度）の計測値と、各関節の回転角度及びその時間的変化率（角速度）の計測値とが入力される。そして、状態量ヤコビアン決定部８３は、これらの入力された計測値から、状態量ヤコビンＪs1を算出する。

前記状態量目標値決定部８４は、前記制御対象状態量に関する目標値を決定する処理を実行する機能部である。

本実施形態では、制御対象状態量に関する目標値は、前記状態量ベクトル↑Ｓ1の時間的変化率（１階微分値）↑Ｓ1'（＝d↑Ｓ1/dt）の目標値↑Ｓ1'_cmdである。

この場合、↑Ｓ1'は、ロボット１の全並進運動量↑Ｐtotalの１階微分値↑Ｐtotal'（＝d↑Ｐtotal/dt）と、全角運動量↑Ｌtotalの１階微分値↑Ｌtotal'（＝d↑Ｌtotal/dt）とを成分とするベクトル（＝［↑Ｐtotal'，↑Ｌtotal'］^T）となるので、↑Ｓ1'の目標値↑Ｓ1'_cmd（以下、状態量目標値ベクトル↑Ｓ1'_cmdという）は、ロボット１の全体の運動によって発生するトータルの慣性力（ただし、ヨー軸周りのモーメント成分を除く）の目標値に相当するものとなる。

より詳しくは、状態量目標値ベクトル↑Ｓ1'_cmdは、↑Ｐtotal'の目標値と↑Ｌtotal'の目標値とを成分とするベクトルとなる。そして、↑Ｓ1'_cmdのうちの↑Ｐtotal'の目標値は、ロボット１の全体の運動によって発生するトータルの並進慣性力の目標値（ロボット１の全体重心の並進慣性力の目標値）に相当し、↑Ｌtotal'の目標値は、ロボット１の全体の運動によって発生するトータルのモーメント慣性力（ただし、ヨー軸周りの成分を除く）の目標値に相当するものとなる。

そして、本実施形態では、状態量目標値決定部８４は、上記状態量目標値ベクトル↑Ｓ1'_cmdを以下に説明する如く決定する。

すなわち、状態量目標値決定部８４は、状態量目標値ベクトル↑Ｓ1'_cmdのうちの、全並進運動量↑Ｐtotalの時間的変化率↑Ｐtotal'の目標値↑Ｐtotal'_cmdと、全角運動量↑Ｌtotalの時間的変化率↑Ｌtotal'の目標値↑Ｌtotal'_cmdとを各別の演算処理により算出する。

さらに詳細には、↑Ｐtotal'_cmdの算出処理に関しては、状態量目標値決定部８４は、グローバル座標系でのロボット１の全体重心点の目標位置↑ｒgc_cmdと実際の位置↑ｒgc_actの計測値との偏差↑Δｒgc（＝↑ｒgc_cmd−↑ｒgc_act）と、その偏差↑Δｒgcの１階微分値↑Δｒgc'（＝d↑Δｒgc/dt）とを算出する。

この場合、ロボット１の全体重心点の目標位置↑ｒgc_cmdは、前記歩容生成部７１から状態量目標値決定部８４に入力される。また、該全体重心点の実際の位置↑ｒgc_actの計測値は、各関節の回転角度の計測値等から公知の手法（例えばカルマンフィルタを使用した手法）によって、制御装置６０が逐次算出し、その算出した計測値が、状態量目標値決定部８４に入力される。

なお、↑ｒgc_cmd及び↑ｒgc_actは、ロール軸方向、ピッチ軸方向及びヨー軸方向の３軸方向の位置から成るベクトル（３自由度の位置ベクトル）である。

そして、状態量目標値決定部８４は、上記偏差↑Δｒgcを“０”に収束させるためのフィードバック操作量を所定のフィードバック制御則により算出し、その算出したフィードバック操作量を↑Ｐtotal'の目標値↑Ｐtotal'_cmdとして決定する。

この場合、具体的には、状態量目標値決定部８４は、上記偏差↑ΔｒgcをＰＤ則（比例・微分則）によって“０”に収束させるように↑Ｐtotal'_cmdを決定する。すなわち、状態量目標値決定部８４は、偏差↑Δｒgcとその１階微分値↑Δｒgc'とから、次式４−１より↑Ｐtotal'_cmdを決定する。

↑Ｐtotal'_cmd＝Kp1＊↑Δｒgc＋Kv1＊↑Δｒgc' ……式４−１

なお、式４−１におけるKp1，Kv1は、それぞれあらかじめ設定された所定値の比例ゲイン、微分ゲインである。これらのゲインKp1，Kv1は対角行列（本実施形態では３次の対角行列）によって表現される。

次に、Ｌtotal'_cmdの算出処理に関しては、状態量目標値決定部８４は、グローバル座標系でのロボット１の目標ＺＭＰの水平位置（ロール軸方向の位置及びピッチ軸方向の位置）と、ロボット１の全体重心点の実際の水平位置↑ｒgc_act_xy（＝前記↑ｒgc_actのロール軸方向成分及びピッチ軸方向成分）の計測値との偏差↑Δｒg_xyを算出する。この場合、ロボット１の目標ＺＭＰの水平位置は、前記歩容生成部７１から状態量目標値決定部８４に入力される。

さらに、状態量目標値決定部８４は、ロボット１の全体重心点が、目標ＺＭＰを支点とする倒立振子の質点であると見なした場合における該倒立振子の鉛直方向に対する傾斜角度↑θpend（ロール軸及びピッチ軸の２軸周りの傾斜角度）と、その１階微分値↑θpend'（＝d↑θpend/dt）を上記偏差↑Δｒg_xyから算出する。具体的には、↑θpendのロール軸周り成分及びピッチ軸周り成分をそれぞれθpend_x、θpend_yとしたとき、θpend_x，θpend_yは、それぞれ、上記偏差↑Δｒg_xyのピッチ軸方向成分Δｒg_y、ロール軸方向成分Δｒg_xから次式４−２ａ，４−２ｂにより算出される。

θpend_x＝tan^-1(Δｒg_y／ｈ)≒Δｒg_y／ｈ ……式４−２ａ
θpend_y＝tan^-1(Δｒg_x／ｈ)≒Δｒg_x／ｈ ……式４−２ｂ

そして、これらのθpend_x，θpend_yの１階微分値（時間的変化率）を算出することで、↑θpend_xy'の各成分が算出される。

なお、式４−２ａ，４−２ｂにおける“ｈ”は、ロボット１の全体重心点の平均的な高さとしてあらかじめ設定された所定値である。

次いで、状態量目標値決定部８４は、上記の如く算出した倒立振子の傾斜角度↑θpend（＝［θpend_x，θpend_y］^T）を“０”に収束させるためのフィードバック操作量（制御入力）を所定のフィードバック制御則により算出し、その算出したフィードバック操作量を↑Ｌtotal'の目標値↑Ｌtotal'_cmdとして決定する。

この場合、具体的には、状態量目標値決定部８４は、上記傾斜角度↑θpendをＰＤ則（比例・微分則）によって“０”に収束させるように↑Ｌtotal'_cmdを決定する。すなわち、状態量目標値決定部８４は、傾斜角度↑θpendとその１階微分値↑θpend'とから、次式４−３により↑Ｌtotal'_cmdを決定する。

↑Ｌtotal'_cmd＝Kp2＊↑θpend＋Kv2＊↑θpend' ……式４−３

なお、式４−３におけるKp2，Kv2は、それぞれあらかじめ設定された所定値の比例ゲイン、微分ゲインである。これらのゲインKp2，Kv2は対角行列（本実施形態では２次の対角行列）によって表現される。

以上、説明した処理によって、↑Ｐtotal'の目標値↑Ｐtotal'_cmdと、↑Ｌtotal'の目標値↑Ｌtotal'_cmdとが決定され、これにより、状態量目標値ベクトル↑Ｓ1'_cmd（＝[↑Ｐtotal'_cmd，↑Ｌtotal'_cmd]^T）が決定されることとなる。

この場合、本実施形態では、↑Ｓ1'_cmdのうちの↑Ｐtotal'_cmdは、ロボット１の全体重心点の位置↑ｒgc_actを目標位置↑ｒgc_cmdに収束させるように決定される。

また、↑Ｓ1'_cmdのうちの↑Ｌtotal'_cmdは、ロボット１の実際の全体重心点を倒立振子の質点と見なした場合の該倒立振子の傾斜角度↑θpendを“０”に収束させる（換言すれば、ロボット１の実際の全体重心点を目標ＺＭＰの鉛直方向上方の位置に収束させる）ように決定される。

補足すると、θpend_x，θpend_yは、それぞれ、Δｒg_y，Δｒg_xにほぼ比例する。このため、θpend_x，θpend_yの算出処理を省略し、Δｒg_y，Δｒg_xをフィードバック制御則（例えばＰＤ則）により“０”に収束させるように、↑Ｌtotal'_cmd_xyを決定するようにしてもよい。例えば、式４−３の右辺の↑θpendをベクトル［Δｒg_y，Δｒg_x］^Tに置き換えた形の式によって、↑Ｌtotal'_cmd_xyを算出するようにしてもよい。

以上のように、基本パラメータ群決定部８１、接触部位ヤコビアン決定部８２、状態量ヤコビアン決定部８３及び状態量目標値決定部８４の処理を実行した後、目標関節駆動トルク決定部７２は、次に、制御入力決定部８５の処理を実行する。

この制御入力決定部８５は、制御対象状態量に関する目標値（前記状態量目標値ベクトル↑Ｓ1'_cmd）を実現するために必要なロボット１の各関節の目標関節駆動トルクを決定する処理を逐次実行する機能部である。以降、制御入力決定部８５で決定する各関節の目標関節駆動トルクを成分として並べたベクトルを目標関節駆動トルク↑τ1_cmdと表記する。

なお、本実施形態の説明では、↑τ1_cmdは、前記駆動力依存一般化力ベクトル↑τと同じ次数（一般化変数ベクトル↑ｑと同じ次数）のベクトルであるとする。この場合、↑τ1_cmdの成分のうち、ロボット１の代表リンク（下側上体１０）の位置及び姿勢に対応する成分は、いずれも“０”である。

ここで、制御入力決定部８５の処理に使用する基本式を以下に説明しておく。

ロボット１が外界との接触部位に外力を受ける場合、ロボット１の運動と力との関係（運動方程式）を表現するロボット運動モデルは、外力が“０”である場合のロボット運動モデルを表す前記式１−１に、外力の影響分を付加してなる次式５−１により表される。

Ｍ＊↑ｑ''＋↑Ｎ＋↑Ｇ＋Ｊc^T＊↑Ｆc＝↑τ ……式５−１

この式５−１における↑Ｆcは、ロボット１の外界との接触部位（本実施形態では足平部１８Ｒ，１８Ｌ）に作用する外力によって前記接触部位代表要素に作用する外力（詳しくは、足平部１８Ｒ，１８Ｌを介してロボット１に作用するトータルの外力を、接触部位代表要素を該外力の作用部位として表現した場合の外力）であり、Ｊc^Tは前記接触部位ヤコビアンＪcの転置行列である。

この式５−１は、以下に説明する如く、ロボット１が外力を受ける接触部位の運動、ひいては、前記接触部位代表要素の運動を規定する（該運動を既定の運動に一致させるものとする）ことによって、後述の式５−１１に変形することができる。

以下説明すると、式５−１の両辺に、行列Ｊc＊Ｍ^-1を乗じることによって、次式５−２が得られる。

Ｊc＊↑ｑ''＋Ｊc＊Ｍ^-1＊（↑Ｎ＋↑Ｇ）＋Ｊc＊Ｍ^-1＊Ｊc^T＊↑Ｆc
＝Ｊc＊Ｍ^-1＊↑τ ……式５−２

一方、前記接触部位代表要素の運動速度としての接触部位代表要素位置ベクトル↑Ｘcの１階微分値↑Ｘc'と、一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を表現する前記式２−３の両辺を１階微分した後、式変形を行なうことで、次式５−３が得られる。

Ｊc＊↑ｑ''＝↑Ｘc''−Ｊc'＊↑ｑ' ……式５−３

この式５−３におけるＪc'は前記接触部位ヤコビアンＪcの１階微分値（＝dＪc/dt）、↑ｑ''は↑ｑの２階微分値（＝d²↑ｑ/dt²）である。

さらに、この式５−３を式５−２に代入することで、次式５−４が得られる。

↑Ｘc''−Ｊc'＊↑ｑ'＋Ｊc＊Ｍ^-1＊（↑Ｎ＋↑Ｇ）
＋Ｊc＊Ｍ^-1＊Ｊc^T＊↑Ｆc＝Ｊc＊Ｍ^-1＊↑τ ……式５−４

ここで、ある重み行列Ｗat（対角行列）を用いて次式５−５，５−６によりそれぞれ定義される行列Ｊc#と行列Ｒcとを導入する。

Ｊc#≡Ｗat^T＊Ｊc^T＊Ｒc^T ……式５−５
Ｒc≡((Ｊc＊Ｗat＊Ｊc^T)^-1)^T ……式５−６

なお、式５−５により定義される行列Ｊc#は、接触部位ヤコビアンＪcの一般化重み付き擬似逆行列としての意味を持つものである。

この場合、上記重み行列Ｗatとして、例えば、前記イナーシャ行列Ｍの逆行列Ｍ^-1を採用することができる。そして、このようにＷat＝Ｍ^-1とした場合には、（Ｊc#）^T＝Ｒc＊Ｊc＊Ｗat＝Ｒc＊Ｊc＊Ｍ^-1となる。このため、前記式５−４の両辺に行列Ｒc（但し、Ｗat＝Ｍ^-1）を乗じることで、次式５−７が得られる。

すなわち、式５−５，５−６でＷat＝Ｍ^-1として得られる次式５−８，５−９によりそれぞれ定義される行列Ｊc#、Ｒcを用いることによって、前記式５−１は、次式５−７に変形される。

Ｒc＊↑Ｘc''−Ｒc＊Ｊc'＊↑ｑ'＋（Ｊc#）^T＊（↑Ｎ＋↑Ｇ）＋↑Ｆc
＝（Ｊc#）^T＊↑τ ……式５−７

但し、
Ｊc#≡(Ｍ^-1)^T＊Ｊc^T＊Ｒc^T ……式５−８
Ｒc≡((Ｊc＊Ｍ^-1＊Ｊc^T)^-1)^T ……式５−９

なお、Ｍ^-1は対角行列であるので、(Ｍ^-1)^T＝M^-1である。

ここで、前記接触部位代表要素の運動を規定し、前記接触部位代表要素位置ベクトル↑Ｘcの２階微分値↑Ｘc''（＝d²↑Ｘc/dt²）をある既定の目標値↑Ｃに一致させる場合（接触部位代表点a1〜a4の運動加速度を、それぞれ、ある既定の目標値に一致させる場合）を想定する。すなわち、↑Ｘc''＝↑Ｃとなるように、接触部位である足平部１８Ｒ，１８Ｌの運動が規定される場合を想定する。

この場合、上記式５−７から、次式５−１０が得られる。

↑Ｆc＝（Ｊc#）^T＊（↑τ−↑Ｎ−↑Ｇ）−Ｒc＊↑Ｃ−Ｒc＊Ｊc'＊↑ｑ'
……式５−１０

この式５−１０を前記式５−１に代入し、さらに、次式５−１２，５−１３，５−１４，５−１５によりそれぞれ定義される行列Ｔc，ベクトル↑Ｃc，行列Ｐc及び一般化力ベクトル↑τcmpnとを用いて整理することによって、次式５−１１が得られる。

Ｍ＊↑ｑ''＋Ｔc＊↑ｑ'＝Ｐc＊（↑τ−↑τcmpn） ……式５−１１
但し、
Ｔc≡−Ｊc^T＊Ｒc＊Ｊc' ……式５−１２
↑Ｃc≡−Ｊc^T＊Ｒc＊↑Ｃ …式５−１３
Ｐc≡Ｉ−Ｊc^T＊（Ｊc#）^T ……式５−１４
↑τcmpn≡（↑Ｎ＋↑Ｇ）−Ｐc^-1＊↑Ｃc ……式５−１５

上記式５−１１は、接触部位代表要素の運動を規定した場合のロボット１の運動方程式を、外力↑Ｆcを含まない形態で表現する式である。

次に、ロボット１の前記一般化変数ベクトル↑ｑの１つ以上の成分に依存する値を有する任意の状態量↑Ｓを想定し、この↑Ｓと、一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係が、あるヤコビアンＪsを用いて、次式５−１６により表現されるものとする。

↑Ｓ＝Ｊs＊↑ｑ' ……式５−１６

この式５−１６は、前記式３−１を一般化して表現したものである。なお、この式５−１６での↑Ｓは、ベクトルである必要はない（スカラーでもよい）が、ここでの説明では、前記式３−１との整合を採るために、便宜上、ベクトルとして表記している。

ここで、上記式５−１６から得られる次式５−１７の関係式を用いることによって、前記式５−１１は、次式５−２０に変形される。

Ｊs＊↑ｑ''＝↑Ｓ'−Ｊs'＊↑ｑ' ……式５−１７

↑Ｓ'＋（Ｊs＊Ｍ^-1＊Ｔc−Ｊs'）＊↑ｑ'
＝（Ｊs＊Ｍ^-1＊Ｐc）＊（↑τ−↑τcmpn） ……式５−２０

従って、この式５−２０における行列Ｊs，Ｍ，Ｔc，Ｊs'，Ｐcの値と、↑Ｓ'，↑ｑ'，↑τcmpnの値とを決定すれば、この式５−２０に基づいて、駆動力依存一般化力ベクトル↑τ（ひいては、各関節の関節駆動トルク）を決定することができることとなる。

ここで、式５−２０の右辺の行列（Ｊs＊Ｍ^-1＊Ｐc）（以降、Ａs≡Ｊs＊Ｍ^-1＊Ｐcと定義する）は、正則行列になるとは限らない（Ａsの逆行列Ａs^-1が存在するとは限らない）。

そこで、本実施形態では、行列Ａsから次式５−２１ａ又は５−２１ｂにより決定される行列Ｂsを使用することによって、次式５−２２により↑τを算出することとする。

Ａs（≡Ｊs＊Ｍ^-1＊Ｐc）が正則行列である場合
Ｂs＝Ａs^-1 ……式５−２１ａ

Ａs（≡Ｊs＊Ｍ^-1＊Ｐc）が正則行列でない場合
Ｂs＝Ａs^T＊(ｋ＊Ｉ＋Ａs＊Ａs^T)^-1 ……式５−２１ｂ
但し、
ｋ：(ｋ＊Ｉ＋Ａs＊Ａs^T)が正則行列になるように決定される正の実数

↑τ＝Ｂs＊（↑Ｓ'＋（Ｊs＊Ｍ^-1＊Ｔc−Ｊs'）＊↑ｑ'）＋↑τcmpn
……式５−２２

この式５−２２が、本実施形態において、前記目標関節駆動トルク↑τ1_cmdを算出するために使用する基本式である。この場合、前記式５−２１ｂにより決定される行列Ｂsは、行列Ａsの擬似逆行列としての意味を持つものである。

なお、以降の説明において、任意の行列Ａに対して、“Ａ^-1”は、Ａが正則行列でない場合には、次式５−２３により決定される行列Ｂに一致する行列（擬似逆行列）を意味するものとする。また、行列Ａが正則行列である場合には、Ａ^-1は、本来の意味でのＡの逆行列を意味するものとする。そして、Ａが正則行列である場合及び正則行列でない場合のいずれの場合でも、便宜上、Ａ^-1をＡの逆行列と称する。

Ｂ＝Ａ^T＊(ｋ＊Ｉ＋Ａ＊Ａ^T)^-1 ……式５−２３
但し、
ｋ：(ｋ＊Ｉ＋Ａ＊Ａ^T)が正則行列になるように決定される正の実数

前記式５−２２における行列Ｂsは、上記の意味でのＡsの逆行列（＝Ａs^-1）である。

補足すると、前記式５−２０の両辺に適当な行列を乗算した上で、↑τを算出する式を構築してもよい。例えば、Ｒsc≡(Ｊs＊Ｐc^T＊Ｍ^-1＊Ｊs^T)^-1)^Tにより定義される行列Ｒscを式５−２０の両辺に左側から乗じてなる式を基に、上記と同様に、↑τを算出する式を構築してもよい。この場合、行列Ｒscを式５−２０の両辺に左側から乗じることによって、式５−２０の両辺は、一般化力ベクトルの次元に変換されることとなる。また、↑τを算出する式は、式５−２２の行列Ｂsを、行列((Ｒsc＊Ｊs＊Ｍ^-1＊Ｐc)^-1＊Ｒsc)に置き換えた式となる。

以上説明したことを基礎として、以下に、制御入力決定部８５の処理を具体的に説明する。

制御入力決定部８５には、目標関節駆動トルク↑τ1_cmdを算出するために、基本パラメータ群決定部８１で決定されたイナーシャ行列Ｍ、遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、接触部位ヤコビアン決定部８２で決定された前記接触部位ヤコビアンＪcと、状態量ヤコビアン決定部８３で決定された状態量ヤコビアンＪs1と、状態量目標値決定部８４で決定された状態量目標値ベクトル↑Ｓ1'_cmdとが逐次入力される。この他、制御入力決定部８５には、一般化変数ベクトル↑ｑの時間的変化率（１階微分値）↑ｑ'の計測値（下側上体１０の移動速度、並びに姿勢角の時間的変化率（角速度）の計測値、各関節の回転角度の時間的変化率（角速度）の計測値）も逐次入力されると共に、後述の接触部位代表要素目標加速度↑Ｃも入力される。

そして、制御入力決定部８５は、式５−２２の右辺の演算に使用する各パラメータの値を所要の入力値を用いて決定し、その決定したパラメータを用いて式５−２２の右辺の演算を行なうことで、目標関節駆動トルク↑τ1_cmdを算出する。

さらに詳細には、制御入力決定部８５は、式５−２２の演算に必要な行列Ｔc，Ｐcと、一般化力ベクトル↑τcmpnとを所定の入力値を用いて算出する処理を実行する。

この場合、制御入力決定部８５は、入力されたイナーシャ行列Ｍ、遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、接触部位ヤコビアンＪcとを用いて、前記式５−８，５−９，５−１２，５−１３，５−１４，５−１５の定義式に基づいて、Ｔc、Ｐc、↑τcmpnを算出する。

具体的には、制御入力決定部８５は、入力されたイナーシャ行列Ｍ及び接触部位ヤコビアンＪcから、前記定義式５−８，５−９に基づいて、行列Ｊc#，Ｒcを算出する。さらに、これらの行列Ｊc#，Ｒcと、入力されたＪcとから、前記定義式５−１２，５−１４に基づいて、行列Ｔc，Ｐcを算出する。なお、この場合、Ｊc'、すなわち、Ｊcの時間的変化率（１階微分値）は、入力されたＪcの時系列から算出される。

また、制御入力決定部８５は、入力されたＪc及び算出したＲcと、前記接触部位代表要素位置ベクトル↑Ｘcの２階微分値↑Ｘc''（接触部位代表点a1〜a4の運動加速度）の目標値↑Ｃとから、前記式５−１３の定義式によりベクトルＣcを算出する。以降、目標値↑Ｃを接触部位代表要素目標加速度↑Ｃという。

ここで、本実施形態では、上記接触部位代表要素目標加速度↑Ｃの成分である４つの接触部位代表点a1〜a4のそれぞれの目標加速度は、いずれの接触部位代表点a1〜a4についても、あらかじめ“０”に設定されている。すなわち、本実施形態では、接触部位代表要素を構成する各接触部位代表点a1〜a4が床に対して静止するか、もしくは一定の速度で動くことを、接触部位としての足平部１８Ｒ，１８Ｌの運動に関する目標とされている。

そして、制御入力決定部８５は、上記接触部位代表要素目標加速度↑Ｃを用いて前記式５−１３の定義式によりベクトル↑Ｃcを算出する。なお、本実施形態では、接触部位代表要素目標加速度↑Ｃは“０”ベクトルであるから、ベクトル↑Ｃcの各成分は、いずれも“０”となる。従って、実際上は、↑Ｃcを算出する処理は不要であり、単に↑Ｃcの各成分を“０”に設定しておけばよい。

さらに制御入力決定部８５は、上記↑Ｃcと、算出したＰcと、入力された↑Ｎ及び↑Ｇとから前記式５−１５の定義式に基づいて↑τcmpnを算出する。

Ｔc、Ｐc、↑τcmpnの算出処理は以上説明した如く実行される。

補足すると、↑Ｎ＋↑Ｇ≒↑Ｇとなる場合には、前記式５−１５の↑Ｎを省略した式によって、↑τcmpnを算出するようにしてもよい。

また、制御入力決定部８５は、式５−２２の演算に用いる行列Ｂsを算出する処理を実行する。なお、以降の説明では、制御対象状態量に対応する行列ＢsをＢs1と表記する。

この場合、制御入力決定部８５は、入力されたイナーシャ行列Ｍ及び状態量ヤコビアンＪs1と、前記Ｐcの算出値とから、行列Ｂs1を算出する。より詳しくは、行列(Ｊs1＊Ｍ^-1＊Ｐc)（≡Ａs）の逆行列Ａs^-1がＢs1として決定される。なお、前記した如く、Ａsが正則行列でない場合には、Ａs^-1は、前記式５−２３に基づいて算出される擬似逆行列である。

そして、制御入力決定部８５は、Ｂs1，Ｔc，↑τcmpnの算出値と、入力された状態量目標値ベクトル↑Ｓ1'_cmd、状態量ヤコビアンＪs1及びイナーシャ行列Ｍと、↑ｑ'の計測値とを用いて、前記式５−２２の右辺の演算（詳しくは、式５−２２の右辺のＢs、↑Ｓ'、Ｊs、Ｊs'、↑ｑ'をそれぞれ、Ｂs1、↑Ｓ1'cmd、Ｊs1、Ｊs1'、↑ｑ'の計測値に置き換えた式の演算）を行なうことによって、↑τを算出し、この↑τの算出値を目標関節駆動トルク↑τ1_cmdとして決定する。なお、この場合、Ｊs1'（＝Ｊs1の１階微分値）は、状態量ヤコビアンＪs1の時系列から算出される。

以上説明した制御入力決定部８５の処理によって、本実施形態では、目標関節駆動トルク↑τ1_cmdは、Ｍ、↑Ｎ、↑Ｇ、Ｊc、Ｊs1の算出値（又はＭ、↑Ｇ、Ｊc、Ｊs1の算出値）と、状態量目標値ベクトル↑Ｓ1'_cmdと、↑ｑ'の計測値と、接触部位代表要素目標加速度↑Ｃとから決定される。

この場合、↑τ1_cmdは、接触部位代表点a1〜a4の運動加速度を接触部位代表要素目標加速度↑Ｃにより規定される加速度に一致させつつ、状態量ベクトル↑Ｓ1の時間的変化率↑Ｓ1'を、状態量目標値ベクトル↑Ｓ1'_cmdに一致させるように決定される。

このとき、前記したように、↑Ｓ1'_cmdのうちの↑Ｐtotal'_cmdは、ロボット１の全体重心点の位置↑ｒgc_actを目標位置↑ｒgc_cmdに収束させるように決定され、また、↑Ｓ1'_cmdのうちの↑Ｌtotal'_cmdは、ロボット１の実際の全体重心点を倒立振子の質点と見なした場合の該倒立振子の傾斜角度↑θpendを“０”に収束させるように決定されている。このため、↑τ1_cmdは、結果的に、ロボット１の全体重心点を適切な位置に保つことができるように決定されることとなる。

以上の如く制御入力決定部８５の処理を実行した後、目標関節駆動トルク決定部７２は、次に、コンプライアンス制御部８６の処理を実行する。

このコンプライアンス制御部８６は、制御入力決定部８５が決定した目標関節駆動トルク↑τ1_cmdを基に、ロボット１に作用するトータルの床反力（全床反力）の目標値を決定し、この目標値と実際の全床反力の計測値との偏差を“０”に近づけるように、目標関節駆動トルク↑τ1_cmdを補正する処理を逐次実行する機能部である。以降、このコンプライアンス制御部８６により目標関節駆動トルク↑τ1_cmdを補正してなるトルクを補正後目標関節駆動トルク↑τ1c_cmdという。本実施形態では、この補正後目標関節駆動トルク↑τ1c_cmdが、制御装置６０の各演算処理周期において、最終的に電動モータ５０を制御するための目標関節駆動トルク（前記関節駆動制御部７３に入力する目標関節駆動トルク）として使用されるものである。

コンプライアンス制御部８６には、補正後目標関節駆動トルク↑τ1c_cmdを算出するために、基本パラメータ群決定部８１で決定された遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、制御入力決定部８５で決定された目標関節駆動トルク↑τ1_cmdと、前記接触部位ヤコビアン決定部８２で決定された接触部位代表要素規定比率ｒat（又はそれにより規定される接触部位代表要素位置ベクトル↑Ｘｃ）とが逐次入力される。

この他、コンプライアンス制御部８６には、制御入力決定部８５で算出された行列Ｒc、Ｊc'、Ｊc#、前記接触部位代表要素目標加速度↑Ｃ（本実施形態では、“０”ベクトル）、各足平部１８Ｒ，１８Ｌにそれぞれ作用する実際の床反力↑FM_act_R，↑FM_act_Lの計測値、代表リンク１０の移動速度の計測値、ロボット１の代表リンク（下側上体）１０の姿勢角の計測値及びその時間的変化率（角速度）の計測値、並びに、各関節の回転角度の計測値及びその時間的変化率（角速度）の計測値も逐次入力される。

なお、床反力↑FM_act_R，↑FM_act_Lの計測値は、それぞれ、前記力センサ６３Ｒ，６３Ｌの検出出力に基づき、制御装置６０において認識された計測値である。

そして、コンプラインアンス制御部８６は、まず、入力された↑Ｎ，↑Ｇ（又は↑Ｎ＋↑Ｇ）と、Ｒc、Ｊc'、Ｊc#と、↑τ1_cmdと、一般化変数ベクトル↑ｑの１階微分値↑ｑ'の計測値とから、前記式５−１０の右辺の演算を行なうことによって、接触部位代表点a1〜a4にそれぞれ作用する力（並進力）の目標値を並べたベクトルとしての接触部位代表要素目標作用力↑Ｆc_cmdを算出する。

すなわち、次式７−１により、↑Ｆc_cmdが算出される。この接触部位代表要素目標作用力↑Ｆc_cmdは、接触部位代表要素の運動加速度を前記接触部位代表要素目標加速度↑Ｃに一致させ、且つ、ロボット１の各関節の実際の関節駆動トルクを↑τ1_cmdにより規定される目標関節駆動トルクに一致させた場合に、接触部位代表点a1〜a4でロボット１に作用すべき力（並進力）の推定値としての意味を持つものである。

↑Ｆc_cmd＝（Ｊc#）^T＊（↑τ1_cmd−↑Ｎ−↑Ｇ）
−Ｒc＊↑Ｃ−Ｒc＊Ｊc'＊↑ｑ' ……式７−１

なお、↑Ｎ＋↑Ｇ≒↑Ｇとなる場合には、式７−１の↑Ｎを省略してもよい。

次いで、コンプライアンス制御部８６は、上記接触部位代表要素目標作用力↑Ｆc_cmdを、ロボット１に対する全床反力の作用点としての所定の全床反力作用点（例えば目標ＺＭＰ）に作用する力の目標値としての目標全床反力↑FMt_cmdに変換する。

この目標全床反力↑FMt_cmdは、本実施形態では、３軸方向の並進力成分と３軸周りのモーメント成分とから成るベクトル（６自由度のベクトル）である。この場合、↑FMt_cmdの並進力成分は、各接触部位代表点a1〜a4に作用する目標並進力を加え合わせたものとなる。また、↑FMt_cmdのモーメント成分は、各接触部位代表点a1〜a4に作用する目標並進力によって、上記全床反力作用点の周りに発生するモーメントを加え合わせたものとなる。

なお、↑Ｆc_cmdの↑FMt_cmdへの変換は、各接触部位代表点a1〜a4と上記床反力作用点との間の位置関係に応じて決定される成分を有する変換行列を↑Ｆc_cmdに乗じることによって、行なわれる。

また、コンプライアンス制御部８６は、各足平部１８Ｒ，１８Ｌにそれぞれ作用する実際の床反力↑FM_act_R，↑FM_act_Lの計測値から、上記全床反力作用点に実際に作用している全床反力↑FMt_actの計測値を算出する。

この場合、本実施形態では、↑FM_act_Rの計測値は、右側足平部１８Ｒに対応する所定の床反力作用点に作用する３軸方向の並進力成分と３軸周りのモーメント成分とから成るベクトルである。同様に、↑FM_act_Lの計測値は、左側足平部１８Ｌに対応する所定の床反力作用点に作用する３軸方向の並進力成分と３軸周りのモーメント成分とから成るベクトルである。

そして、コンプライアンス制御部８６は、前記全床反力作用点と右側足平部１８Ｒの床反力作用点との位置関係に応じて決定される成分を有する変換行列によって、↑FM_act_Rの計測値を全床反力作用点に作用する力（並進力成分及びモーメント成分の組）に変換したものと、前記全床反力作用点と左側足平部１８Ｌの床反力作用点との位置関係に応じて決定される成分を有する変換行列によって、↑FM_act_Lの計測値を全床反力作用点に作用する力（並進力成分及びモーメント成分の組）に変換したものとを加え合わせることによって、全床反力↑FMt_actの計測値を算出する。

この↑FMt_actの計測値の並進力成分は、↑FM_act_Rの並進力成分と↑FM_act_Lの並進力成分との総和（ベクトル和）の並進力となる。また、↑FMt_actのモーメント成分は、↑FM_act_Rのモーメント成分と↑FM_act_Lのモーメント成分と↑FM_act_Rの並進力成分が全床反力作用点の周りに発生するモーメントと↑FM_act_Lの並進力成分が全床反力作用点の周りに発生するモーメントとの総和（ベクトル和）のモーメントとなる。

コンプライアンス制御部８６は、上記のように目標全床反力↑FMt_cmdと、全床反力↑FMt_actの計測値とを算出した後、これらの偏差↑ΔFMt（＝↑FMt_cmd−↑FMt_act）を算出する。そして、コンプラインアス制御部８６は、この偏差↑ΔFMtを“０”に収束させる操作量としての目標関節駆動トルク↑τ1_cmdの補正量↑Δτtを所定のフィードバック制御則により算出する。

具体的には、本実施形態では、コンプライアンス制御部８６は、偏差↑ΔFMtに所定値の比例ゲインＧkを乗じてなるベクトルＧk＊↑ΔFMtを、接触部位代表点a1〜a4にそれぞれ作用する実際の力（並進力）の目標値（接触部位代表要素目標作用力↑Ｆc_cmd）に対する誤差分↑ΔFcに変換する。

この変換は、前記↑Ｆc_cmdを↑FMt_cmdに変換する変換行列の逆行列を、Ｇk＊↑ΔFMtに乗じることによって行なわれる。なお、本実施形態では、↑Ｆc_cmdを↑FMt_cmdに変換する変換行列は正則行列とはならないので、その変換行列の逆行列は、前記式５−２３に基づいて決定される擬似逆行列である。

さらにコンプライアンス制御部８６は、誤差分↑ΔFcに次式７−２の如く、行列Ｊc#^Tの逆行列（Ｊc#^T）⁻¹を乗じることによって、合成目標関節駆動トルク↑τt_cmdの補正量↑Δτtを算出する。

↑Δτt＝（Ｊc#^T）^-1＊↑ΔFc ……式７−２

これにより、補正量↑Δτtは、フィードバック制御則としての比例則により、偏差↑ΔFMtを“０”に収束させるように決定されることとなる。

そして、コンプラインアンス制御部８６は、次式７−３の通り、この補正量↑Δτtを目標関節駆動トルク↑τ1_cmdに加えることによって、補正後目標関節駆動トルク↑τ1c_cmdを決定する。

↑τ1c_cmd＝↑τ1_cmd＋↑Δτt ……式７−３

以上のコンプライアンス制御部８６の処理によって、目標関節駆動トルク↑τ1_cmdに対応する全床反力である目標全床反力↑FMt_cmdと実際の全床反力↑FMt_actの計測値との偏差を“０”に近づけるように、目標関節駆動トルク↑τ1_cmdが補正されることとなる。

本実施形態では、このように決定された補正後目標関節駆動トルク↑τ1c_cmd（詳しくは、↑τ1c_cmdのうちの各関節の回転角度に対応する成分）が前記関節駆動制御部７３に入力される。

以上が、目標関節駆動トルク決定部７２の処理の詳細である。

以上説明した本実施形態によれば、状態量目標値ベクトル↑Ｓ1'_cmdのうちの全並進運動量の１階微分値↑Ｐtotal'の目標値は、鉛直方向の位置を含めたロボット１の全体重心点の位置を歩容生成部７１で生成された目標歩容の全体重心点の目標位置に収束させるように決定されている。換言すれば、ロボット１の運動によって発生する並進慣性力の目標値に相当する↑Ｐtotal'の目標値が、ロボット１の実際の全体重心点の位置を適切な目標位置に収束させるように決定されている。

さらに、↑Ｓ1'_cmdのうちの全角運動量の１階微分値↑Ｌtotal'の目標値は、ロボット１の全体重心点の水平位置を、該全体重心点に質点を有する仮想的な倒立振子の支点としての目標ＺＭＰの鉛直方向上方の位置に収束させるように決定されている。換言すれば、ロボット１の運動によって発生するモーメント慣性力の目標値に相当する↑Ｌtotal'の目標値が、ロボット１の実際の全体重心点の水平位置を、上記仮想的な倒立振子の傾倒を防止し得る位置に収束させるように決定されている。

そして、本実施形態では、状態量目標値ベクトル↑Ｓ1'_cmdの↑Ｐtotal'の目標値及び↑Ｌtotal'の目標値を実現し得るように、前記目標関節駆動トルク↑τ1_cmdが決定される。

ここで、本実施形態のロボット１では、各関節の関節駆動系にばね部材であるトーションバー５３を備えるため、各関節が柔軟性を有する。このため、ロボット１に作用する外力の影響で、各関節の回転角度の変動が生じやすく、ひいては、全体重心点の位置の変動（特に鉛直方向の位置の変動）も生じやすい。

しかるに、上記の如く状態量目標値ベクトル↑Ｓ1'_cmdを決定しているために、ロボット１の全体重心点の実際の位置が目標位置からずれると、それを解消するように目標関節駆動トルク↑τ1_cmdが決定される。

そのため、ロボット１の実際の全体重心点の位置が適切な目標位置からずれるの防止することができる。その結果、ロボット１の各関節が柔軟性を有していても、ロボット１の姿勢のふらつき等が生じるのを防止し、該ロボット１の挙動の安定性を高めることができる。

また、本実施形態では、前記式５−２０に基づいて、目標関節駆動トルク↑τ1_cmdが決定される。従って、接触部位代表要素の接触部位代表点a1〜a4の運動を前記接触部位代表要素目標加速度↑Ｃにより規定した上で、ロボット１の制御対象状態量に関する目標値としての状態量目標値ベクトル↑Ｓ1'_cmdに基づいて、↑τ1_cmdが決定される。

このため、ロボット１に作用する外力の値（これは外乱等の影響を受けやすい）を特定することなく、↑Ｃと↑Ｓ1'_cmdとを実現し得る適切な↑τ1_cmdを決定できる。この場合、↑Ｃと↑Ｓ1'_cmdとのトータルの自由度数は、ロボット１の全体のトータルの自由度数（前記一般化力ベクトル↑ｑの自由度数）よりも小さいため、関節駆動トルク↑τ1_cmdは、ロボット１の全ての関節の回転動作を特定することを必要とせずに決定できる。このため、ロボット１に作用する外力の変動等に伴うロボット１の関節の動きの柔軟性を高めることができる。

ここで、以上説明した実施形態と本発明との対応関係について補足しておく。

本実施形態では、前記基本パラメータ群決定部８１により、本発明における基本パラメータ群決定手段が実現される。なお、下側上体１０の移動速度、並びに姿勢角及びその時間的変化率（角速度）の計測値、各関節の回転角度及びその時間的変化率（角速度）の計測値が発明における一般化変数観測情報に相当する。

また、前記接触部位ヤコビアン決定部８２により、本発明における接触部位ヤコビアン算出手段が実現される。この場合、ロボット１の足平部１８Ｒ，１８Ｌが、本発明における接触部位に相当し、前記接触部位代表点a1〜a4を連接してなる要素として本発明における接触部位代表要素が設定される。なお、本実施形態では、前記並進床反力鉛直成分Fz_act_R，Fz_act_Lの計測値が、本発明における接触状態情報として使用される。

また、前記状態量ヤコビアン決定部８３により、本発明における状態量ヤコビアン算出手段が実現される。この場合、本実施形態では、前記全並進運動量↑Ｐtotalと全角運動量↑Ｌtotalとの組から構成される状態量ベクトル↑Ｓが本発明における所定種類の状態量に相当する。

また、前記制御入力決定部８５により、本発明における目標駆動力決定手段が実現される。この場合、前記目標関節駆動トルク↑τ1_cmdが本発明における目標一般化力ベクトルτcmdに相当し、この↑τ1_cmdのうちの各関節の回転角度に対応する成分が、本発明における目標駆動力に相当する。また、前記接触部位代表要素目標加速度↑Ｃが本発明における接触部位運動加速度目標値に相当し、状態量目標値ベクトル↑Ｓ1'_cmdが本発明における所定種類の状態量の１階微分値の目標値に相当する。

また、前記コンプライアンス制御部８６及び関節駆動制御部７３により、本発明におけるアクチュエータ制御手段が実現される。そして、コンプライアンス制御部８６が実行する処理によって、本発明における接触部位外力目標値決定手段と、駆動力補正量算出手段とが実現される。より詳しくは、前記接触部位代表要素目標作用力↑Ｆc_cmdが本発明における接触部位外力目標値に相当し、この↑Ｆc_cmdを前記式７−１により算出する処理によって、接触部位外力目標値決定手段が実現される。また、前記補正量↑Δτtが本発明における目標駆動力の補正量に相当し、この補正量↑Δτtを前記した如く算出する処理によって、本発明における駆動力補正量算出手段が実現される。

［第２実施形態］
次に、本発明の第２実施形態を図６等を参照して説明する。

本実施形態は、制御装置６０の目標関節駆動トルク決定部７２の処理において、接触部位代表要素の設定の仕方を第１実施形態と異ならせたものである。そして、これに付随して、目標関節駆動トルク決定部７２の処理のうち、接触部位ヤコビアン決定部８２、制御入力決定部８５、及びコンプライアンス制御部８６の一部の処理が第１実施形態と相違し、これ以外の目標関節駆動トルク決定部７２の処理（基本パラメータ群決定部８１、状態量ヤコビアン決定部８３、状態量目標値決定部８４の処理）と、歩容生成部７１及び関節駆動制御部７３の処理とは、第１実施形態と同じである。

そこで、本実施形態の説明では、第１実施形態との相違点を中心に説明し、第１実施形態と同一の事項については説明を省略する。

前記第１実施形態では、接触部位代表要素の運動が、４つの接触部位代表点a1〜a4の運動によって規定されるものとしているため、該接触部位代表要素の運動を拘束した場合、ロボット１の全体のトータルの自由度数（前記一般化力ベクトル↑ｑの自由度数）のうちの、３＊４＝１２個の自由度数分の制約を受けることとなる。本実施形態は、接触部位代表要素の運動を拘束することによって制約を受ける自由度数を第１実施形態よりも少なくできるように、接触部位代表要素を設定するものである。

本実施形態では、接触部位代表要素は、ロボット１の外界との各接触部位に外界から作用する外力の合力（全ての接触部位についての合力）が作用する要素であるとされる。そして、ロボット１の外界との各接触部位に外界から作用する外力（並進力及びモーメント）は、該接触部位と外界との接触面の位置及び姿勢のばね性変位（該接触部位又は外界の部位の弾性変形によって生じる変位）に起因する弾性力として該ロボット１の接触部位に作用する外力であると見なされる。同様に、接触部位代表要素に作用する外力の合力は、接触部位代表要素と外界との接触面（仮想的又は実体的な接触面）の位置及び姿勢のばね性変位（弾性変形）に起因する弾性力として該接触部位代表要素に作用する外力であると見なされる。

より具体的には、本実施形態においては、ロボット１が床面上を移動する動作、例えば歩行動作におけるける接触部位代表要素は、両足平部１８Ｒ，１８Ｌに作用する床反力の合力（トータルの床反力）が作用する要素とされる。

従って、本実施形態では、ロボット１の歩行動作中において、右側の足平部１８Ｒの接地面（床との接触面）内のある作用点に作用する床反力ベクトル（並進力及びモーメントを成分とするベクトル）を↑FM_R＝[↑F_R，↑M_R]^T、左側の足平部１８Ｌの接地面(床との接触面)内のある作用点に作用する床反力ベクトル（並進力及びモーメントを成分とするベクトル）を↑FM_L＝[↑F_L，↑M_L]^T、接触部位代表要素に作用する合力ベクトル（床反力ベクトル[↑F_R，↑M_R]^T，[↑F_L，↑M_L]^Tの合力ベクトル）を↑FMt＝[↑Ft，↑Mt]^Tとおくと、[↑Ft，↑Mt]^Tと、[↑F_R，↑M_R]^T及び[↑F_L，↑M_L]^Tとの間の関係が次式８−１により表されるものとされる。

ここで、式８−１における↑Ftは、接触部位代表要素に作用する合力ベクトル↑FMt（以降、全床反力ベクトル↑FMtという）のうちの３軸方向の並進力成分からなる並進力ベクトル、↑Mtは、該全床反力ベクトル↑FMtのうちの３軸周りのモーメント成分からなるモーメントベクトル、↑F_Rは、右側足平部１８Ｒに作用する床反力ベクトル↑FM_Rのうちの３軸方向の並進力成分からなる並進力ベクトル、↑M_Rは、該床反力ベクトル↑FM_Rのうちの３軸周りのモーメント成分からなるモーメントベクトル、↑F_Lは、左側足平部１８Ｌに作用する床反力ベクトル↑FM_Lのうちの３軸方向の並進力成分からなる並進力ベクトル、↑M_Lは、該床反力ベクトル↑FM_Lのうちの３軸周りのモーメント成分からなるモーメントベクトルである。

また、式８−１における行列A_R，A_Lの成分Ｉ、０はそれぞれ３次の単位行列、３次の零行列であり、行列A_Rの成分V_RはV_R＊↑F_Rが、接地部位代表要素の接地位置から見た右側足平部１８Ｒの接地位置の位置ベクトル（以降、これを↑Vp_Rと表記する）と、↑F_Rとのベクトル積（＝↑Vp_R×↑F_R）に一致するように設定された３次の行列、行列A_Lの成分V_Lは、V_L＊↑F_Rが、接地部位代表要素の位置から見た左側足平部１８Ｌの接地位置の位置ベクトル（以降、これを↑Vp_Lと表記する）と、↑F_Lとのベクトル積（＝↑Vp_L×↑F_L）に一致するように設定された３次の行列である。

なお、接地部位代表要素の接地位置は、詳しくは、接地部位代表要素に対する前記全床反力ベクトル↑FMtの作用点の位置、右側足平部１８Ｒの接地位置は、詳しくは、右側足平部１８Ｒに対する床反力ベクトル↑FM_Rの作用点の位置、左側足平部１８Ｌの接地位置は、詳しくは、左側足平部１８Ｌに対する床反力ベクトル↑FM_Lの作用点の位置である。

さらに、本実施形態では、ロボット１の外界との接触部位として床面に接地する各足平部１８に作用する外力としての床反力ベクトル↑FM_R，↑FM_Lは、各足平部１８と床との接触面の位置及び姿勢のばね性変位（該足平部１８又は床の弾性変形によって生じる変位）に起因する弾性力として該足平部１８に作用するものとみなされる。

同様に、本実施形態における接触部位代表要素に作用する外力としての全床反力ベクトル↑FMtは、接触部位代表要素と床との接触面（実体的又は仮想的な接触面）の位置及び姿勢のばね性変位（接触部位代表要素又は床の弾性変形によって生じる変位）に起因する弾性力として該接触部位代表要素に作用する外力であると見なされる。

従って、右側の足平部１８Ｒの接地時における接地面（床との接触面）の位置及び姿勢のばね性変位量（該接地面の基準位置からの並進変位量及び該接地面の基準姿勢からの角度変位量）を成分とするベクトルを↑X_R≡[↑Xorg_R，↑Xrot_R]^T、左側の足平部１８Ｌの接地時における接地面（床との接触面）の位置及び姿勢のばね性変位量（該接地面の基準位置からの並進変位量及び該接地面の基準姿勢からの角度変位量）を成分とするベクトルを↑X_L≡[↑Xorg_L，↑Xrot_L]^T、接触部位代表要素の接地面（床との接触面）の位置及び姿勢のばね性変位量（該接地面の基準位置からの並進変位量及び該接地面の基準姿勢からの角度変位量）を成分とするベクトルを↑Xcc≡[↑Xcc_org，↑Xcc_rot]^Tとおくと、↑FM_Rと↑X_Rとの間の関係、↑FM_Lと↑X_Lとの間の関係、及び↑FMtと↑Xccとの間の関係がそれぞれ、次式８−２、８−３、８−４により表されるものとされる。

ここで、式８−２における↑Xorg_Rは、ばね性変位量↑Ｘ_Rのうちの３軸方向の並進変位量ベクトル、↑Xrot_Rは、ばね性変位量↑X_Rのうちの３軸周りの角度変位量ベクトル、kf_Rは、↑F_Rの各成分と↑Xorg_Rの各成分との間の関係を↑F_R＝kf_R＊↑Xorg_Rという関係で表現するばね定数（並進変位に関するばね定数）を示す３次の対角行列、km_Rは、↑M_Rの各成分と↑Xrot_Rの各成分との間の関係を↑M_R＝km_R＊↑Xrot_Rという関係で表現するばね定数（回転角度変位に関するばね定数）を示す３次の対角行列である。

また、式８−３における↑Xorg_Lは、ばね性変位量↑X_Lのうちの３軸方向の並進変位量ベクトル、↑Xrot_Lは、ばね性変位量↑X_Lのうちの３軸周りの角度変位量ベクトル、kf_Lは、↑F_Lの各成分と↑Xorg_Lの各成分との間の関係を↑F_L＝kf_L＊↑Xorg_Lという関係で表現するばね定数（並進変位に関するばね定数）を示す３次の対角行列、km_Lは、↑M_Lの各成分と↑Xrot_Lの各成分との間の関係を↑M_L＝km_L＊↑Xrot_Lという関係で表現するばね定数（回転角度変位に関するばね定数）を示す３次の対角行列である。

また、式８−４における↑Xcc_orgは、ばね性変位量↑Xccのうちの３軸方向の並進変位量ベクトル、↑Xcc_rotは、ばね性変位量↑Xccのうちの３軸周りの角度変位量ベクトル、kfccは、↑Ftの各成分と↑Xcc_orgの各成分との間の関係を↑Ft＝kfcc＊↑Xcc_orgという関係で表現するばね定数（並進変位に関するばね定数）を示す３次の対角行列、kmccは、↑Mtの各成分と↑Xcc_rotの各成分との間の関係を↑Mt＝kmcc＊↑Xcc_rotという関係で表現するばね定数（回転角度変位に関するばね定数）を示す３次の対角行列である。

また、本実施形態では、接触部位代表要素の接地位置（↑FMtの作用点）は、右側足平部１８Ｒのみが接地する片脚接地状態では、右側足平部１８Ｒの接地位置（↑FM_Rの作用点）に一致し、左側足平部１８Ｌのみが接地する片脚接地状態では、左側足平部１８Ｌの接地位置（↑FM_Lの作用点）に一致し、両足平部１８Ｒ，１８Ｌが接地する両脚接地状態では、両足平部１８Ｒ，１８Ｌの接地位置を結ぶ線分の内分点の位置であるとされる。

より詳しくは、図６を参照して、接触部位代表要素に作用する全床反力ベクトル↑FMtの作用点から見た、右側足平部１８Ｒに作用する床反力ベクトル↑FM_Rの作用点の位置ベクトル↑Vp_Rと、↑FMccの作用点から見た、左側足平部１８Ｌに作用する床反力ベクトル↑FM_Lの作用点の位置ベクトル↑Vp_Lとの間の関係が、第１実施形態で説明した前記接触部位代表要素規定比率ｒatに応じて、次式８−５により与えられるものとする。

ｒat＊↑Vp_R＝−（１−ｒat）＊↑Ｖp_L ……式８−５

換言すれば、接触部位代表要素の接地位置（↑FMccの作用点）は、両脚接地状態での両足平部１８Ｒ，１８Ｌの接地位置を結ぶ線分上で、｜↑Vp_R｜：｜↑Vp_L｜＝（１−ｒat）：ｒatとなるような位置であるとされる（図６を参照）。

なお、本実施形態では、接触部位代表要素の接地位置（↑FMtの作用点）、右側足平部１８Ｒの接地位置（↑FM_Rの作用点）、左側足平部１８Ｌの接地位置（↑FM_Lの作用点）は、それぞれ、例えば、その作用点での床反力のモーメント（すなわち↑Mt、↑M_R、↑M_L）の水平成分（水平軸周り成分）が“０”となる点（すなわちＺＭＰ）の位置に設定される。このように各作用点を設定すると、式８−５の関係が成立する。

さらに、本実施形態では、接触部位代表要素に関するばね性変位量↑Xcc（以降、接触部位代表要素ばね性変位量↑Xccという）は、右側足平部１８Ｒのみが接地する片脚接地状態では、右側足平部１８Ｒに関するばね性変位量↑X_Rに一致し、左側足平部１８Ｌのみが接地する片脚接地状態では、左側足平部１８Ｌに関するばね性変位量↑X_Lに一致し、両足平部１８Ｒ，１８Ｌが接地する両脚接地状態では、両足平部１８Ｒ，１８Ｌのそれぞれに関するばね性変位量↑X_R，↑X_Lの重み付き平均の変位量になるものとする。

より詳しくは、接触部位代表要素ばね性変位量↑Xccと、各足平部１８Ｒ，１８Ｌに関するばね性変位量↑X_R，↑X_Lとの間の関係は、接触部位代表要素の接地位置（↑FMtの作用点）と、各足平部１８Ｒ，１８Ｌの接地位置（↑FM_R，↑FM_Lの作用点）との間の関係と同様に、ｒat、（１−ｒat）を重み係数として、次式８−６により表されるものとする。

↑Xcc＝ｒat＊↑X_R＋（１−ｒat）＊↑X_L ……式８−６

そして、本実施形態では、接触部位ヤコビアン決定部８２の処理では、前記接触部位代表要素ばね性変位量↑Xccを接触部位代表要素の位置及び姿勢に相当するものであるとし、この接触部位代表要素の運動速度、すなわち、↑Xccの時間的変化率（１階微分値）↑Xcc'（＝↑Xcc／dt）と、一般化変数ベクトル↑ｑの１階微分値↑ｑ’との間の関係を次式８−７により表現する行列Ｊcを、本実施形態における接触部位ヤコビアンとする。

↑Xcc’＝Ｊc＊↑ｑ' ……式８−７

なお、接触部位代表要素ばね性変位量↑Xccは、３軸方向の並進変位量と３軸周りの角度変位量とを成分とするベクトルであるので、６個の成分からベクトルである。また、本実施形態の説明では、便宜上、接触部位ヤコビアンの参照符号として、第１実施形態と同じ参照符号Ｊcを用いるが、以降の本実施形態の説明では、特にことわらない限り、接触部位ヤコビアンＪcは、↑Xcc'と、↑ｑ’との間の関係を式８−７により表現するヤコビアンを意味する。

以上を前提として、本実施形態における接触部位ヤコビアンＪcの算出の仕方を以下に説明する。

まず、前記式８−１に、式８−２、８−３、８−４を適用すると、次式８−８が得られる。

そして、この式８−８の第１行の式は、次式８−９により表される。

kfcc＊↑Xcc_org＝kf_R＊↑Xorg_R＋kf_L＊↑Xorg_L ……式８−９

一方、前記式８−６から次式８−１０が得られる。

↑Xcc_org＝ｒat＊↑Xorg_R＋（１−ｒat）＊↑Xorg_L ……式８−１０

そして、上記式８−９，８−１０を対比することによって、次式８−１１ａ，８−１１ｂが得られる。

kf_R＝wcc＊kfcc ……式８−１１ａ
kf_L＝（１−wcc）＊kfcc ……式８−１１ｂ

また、前記式８−８の第２行の式は、次式８−１２により表される。

kmcc＊↑Xcc_rot＝↑Vp_R×（kf_R＊↑Xorg_R）＋km_R＊↑Xrot_R
＋↑Vp_L×（kf_L＊↑Xorg_L）＋km_L＊↑Xrot_L ……式８−１２
但し、
↑Vp_R×（kf_R＊↑Xorg_R）＝V_R＊（kf_R＊↑Xorg_R）
↑Vp_L×（kf_L＊↑Xorg_L）＝V_L＊（kf_L＊↑Xorg_L）

ここで、図６を参照して、右側足平部１８Ｒの接地位置（↑FM_Rの作用点）から見た左側足平部１８Ｌの接地位置（↑FM_Lの作用点）の位置ベクトルを↑V_RL（＝↑Vp_L−↑Ｖp_R）とおくと、前記式８−５から次式８−１３ａ，８−１３ｂが得られる。

↑Vp_R＝−（１−ｒat）＊↑V_RL ……式８−１３ａ
↑Vp_L＝ｒat＊↑V_RL ……式８−１３ｂ

そして、上記式８−１３ａ、８−１３ｂと、前記式８−１１ａ、８−１１ｂとを前記式８−１２に適用して整理すると、次式８−１４が得られる。

kmcc＊↑Xcc_rot＝−wcc＊（１−wcc）＊↑V_RL×↑Xorg_R＋km_R＊↑Xrot_R
＋wcc＊（１−wcc）＊↑V_RL×↑Xorg_L＋km_L＊↑Xrot_L
＝wcc＊（１−wcc）＊↑V_RL×（↑Xorg_L−↑Xorg_R）
＋km_R＊↑Xrot_R＋km_L＊↑Xrot_L
……式８−１４

この場合、↑V_RLと、（↑Xorg_L−↑Xorg_R）とは、互いに平行なベクトルと考えてよいので、式８−１４の右辺の第１項の値は、“０”となる。従って、式８−１４は、次式８−１５に書き換えられる。

kmcc＊↑Xcc_rot＝km_R＊↑Xrot_R＋km_L＊↑Xrot_L ……式８−１５

また、前記式８−６から次式８−１６が得られる。

↑Xcc_rot＝ｒat＊↑Xrot_R＋（１−ｒat）＊↑Xrot_L ……式８−１６

そして、上記式８−１５，８−１６を対比することによって、次式８−１７ａ，８−１７ｂが得られる。

km_R＝ｒat＊kmcc ……式８−１７ａ
km_L＝（１−ｒat）＊kmcc ……式８−１７ｂ

さらに、前記式８−１１ａ，８−１１ｂと、上記式８−１７ａ，８−１７ｂとを前記式８−８に適用して整理すると、次式８−１８が得られる。

↑Xcc＝ｒat＊A_R＊↑X_R＋（１−ｒat）＊A_L＊↑X_L ……式８−１８

そして、この式８−１８の両辺の微分すると、次式８−１９が得られる。

↑Xcc'＝ｒat＊A_R＊↑X_R'＋（１−ｒat）＊A_L＊↑X_L' ……式８−１９
但し、↑Xcc'＝d↑Xcc/dt、↑X_R'＝d↑X_R/dt、↑X_L'＝d↑X_L/dt

なお、ここでは、ｒat＊A_R、（１−ｒat）＊A_Lの時間的変化率は、十分に微小なものとなり、ｒat＊A_R、（１−ｒat）＊A_Lは、ほぼ一定に維持されるとしている。

この場合、↑X_R'は、右側足平部１８Ｒの接地位置（↑FM_Rの作用点）の時間的変化率に一致し、また、↑X_L'は、左側足平部１８Ｌの接地位置（↑FM_Lの作用点）の時間的変化率に一致すると考えられる。

そこで、右側足平部１８Ｒの接地位置（↑FM_Rの作用点）の時間的変化率と一般化変数ベクトル↑ｑの一階微分値↑ｑ'との間の関係を表すヤコビアンをＪcc_R、左側足平部１８Ｌの接地位置（↑FM_Lの作用点）の時間的変化率と一般化変数ベクトル↑ｑの一階微分値↑ｑ'との間の関係を表すヤコビアンをＪcc_Lとおくと、前記式８−７と式８−１９とから次式８−２０が得られる。

Ｊc＝ｒat＊A_R＊Ｊcc_R＋（１−ｒat）＊A_L＊Ｊcc_L ……式８−２０

そこで、本実施形態では、接触部位ヤコビアン決定部８２は、式８−２０の右辺のヤコビアンＪcc_R、Ｊcc_Lと、行列A_R，A_Lとを算出し、これらのヤコビアンＪcc_R、Ｊcc_L及び行列A_R，A_Lの算出値と、第１実施形態で説明した如く算出した接触部位代表要素規定比率ｒatとから式８−２０の右辺の演算を行なうことで、接触部位ヤコビアンＪcを算出する。

以下に、本実施形態における接触部位ヤコビアン決定部８２の処理をより具体的に説明する。

前記式８−２０の右辺の演算に用いる接触部位代表要素規定比率ｒat、上記ヤコビアンＪcc_R、Ｊcc_L、及び行列A_R，A_Lを算出するために、接触部位ヤコビアン決定部８２には、前記力センサ６３Ｒ，６３Ｌの検出出力（各足平部１８Ｒ，１８Ｌに作用する床反力の計測値）が入力されると共に、下側上体１０の移動速度、並びに姿勢角及びその時間的変化率（角速度）の計測値と、各脚リンク３Ｒ，３Ｌの各関節の回転角度及びその時間的変化率（角速度）の計測値とが入力される。

そして、接触部位ヤコビアン決定部８２は、並進床反力鉛直成分Fz_act_R，Fz_act_Lの計測値から、第１実施形態と同様に、前記式２−２ａ又は２−２ｂの演算によって、接触部位代表要素規定比率ｒatを算出する。なお、第１実施形態で説明した如く、並進床反力鉛直成分Fz_act_R，Fz_act_Lの目標値を使用してｒatを算出するようにしてもよい。

また、接触部位ヤコビアン決定部８２は、各足平部１８Ｒ，１８Ｌに作用する床反力の計測値を基に、各足平部１８Ｒ，１８Ｌに対する床反力ベクトル↑FM_R，↑FM_Lの作用点（本実施形態ではＺＭＰ）の位置と、それらの床反力ベクトルの合力ベクトルである全床反力ベクトル↑FMtの作用点（本実施形態ではＺＭＰ）の位置とを算出する。そして、これらの作用点の位置の算出値を基に、前記式８−１のただし書きで定義される行列A_R，A_Lを算出する。

なお、前記歩容生成部７１が生成・出力する前記目標歩容（あるいは制御装置６０に外部から与えられる目標歩容）が、足平部１８Ｒ，１８Ｌのそれぞれの床反力ベクトル↑FM_R，↑FM_Lの目標値を含んでいる場合には、その目標値を↑FM_R，↑FM_Lの計測値の代わりに用いて、行列A_R，A_Lを算出するようにしてもよい。あるいは、前記目標歩容が、床反力ベクトル↑FM_R，↑FM_Lのそれぞれに係わるＺＭＰの目標位置と、全床反力ベクトル↑FMtに係わるＺＭＰの目標位置とを含んでいる場合には、それらの目標位置を用いて行列A_R，A_Lを算出するようにしてもよい。

また、接触部位ヤコビアン決定部８２は、下側上体１０の移動速度、並びに姿勢角及びその時間的変化率（角速度）の計測値と、各脚リンク３Ｒ，３Ｌの各関節の回転角度及びその時間的変化率（角速度）の計測値と、上記の如く算出した床反力ベクトル↑FM_R，↑FM_Lの作用点（本実施形態ではＺＭＰ）の位置（又はその目標位置）とに基づき、ヤコビアンＪcc_R，Ｊcc_Lを算出する。

そして、接触部位ヤコビアン決定部８２は、上記の如く算出した接触部位代表要素規定比率ｒat、上記ヤコビアンＪcc_R、Ｊcc_L、及び行列A_R，A_Lを用いて、前記式８−２０の右辺の演算を行なうことによって、接触部位ヤコビアンＪcを算出する。これにより、↑Xcc'と↑ｑ’との間の関係を前記式８−７により表すヤコビアンとしての接触部位ヤコビアンＪcが算出されることとなる。

以上が本実施形態における接触部位ヤコビアン決定部８２の処理の詳細である。

次に、本実施形態における制御入力決定部８５の処理を説明する。

本実施形態における制御入力決定部８５には、第１実施形態と同様に、基本パラメータ群決定部８１で決定されたイナーシャ行列Ｍ、遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、接触部位ヤコビアン決定部８２で決定された接触部位ヤコビアンＪcと、状態量ヤコビアン決定部８３で決定された状態量ヤコビアンＪs1と、状態量目標値決定部８４で決定された状態量目標値ベクトル↑Ｓ1'_cmdとが逐次入力される。さらに、制御入力決定部８５には、一般化変数ベクトル↑ｑの時間的変化率（１階微分値）↑ｑ'の計測値（下側上体１０の移動速度、並びに姿勢角の時間的変化率（角速度）の計測値、各関節の回転角度の時間的変化率（角速度）の計測値）も逐次入力されると共に、接触部位代表要素目標加速度↑Ｃ（接触部位代表要素の運動加速度の目標値↑Ｃ）も入力される。

そして、制御入力決定部８５は、これらの入力値を基に、前記式５−２２の右辺の演算によって、目標関節駆動トルク↑τ1_cmdを算出することは第１実施形態と同様である。

但し、本実施形態では、前記式５−８，５−９，５−１２，５−１３，５−１４によりそれぞれ定義される行列Ｊc#，Ｒc，Ｔc、ベクトル↑Ｃc、行列Ｐcを算出するために必要となる接触部位ヤコビアンＪcは、前記式８−２０により接触部位ヤコビアン決定部８２で算出したヤコビアンである。

また、本実施形態では、接触部位代表要素の運動加速度は、前記接触部位代表要素ばね性変位量↑Xccの２階微分値↑Xcc''（＝d²↑Xcc/dt²）である。このため、式５−１３により定義されるベクトル↑Ｃcを算出するために必要となる接触部位代表要素目標加速度↑Ｃは、↑Xcc''の目標値である。この場合、本実形態では、↑Xcc''の目標値↑Ｃは、第１実施形態のものと同様に、その各成分値が“０”となる“０”ベクトル（但し、本実施形態では６成分の“０”ベクトル）にあらかじめ設定されている。従って、本実施形態では、接触部位代表要素ばね性変位量↑Xccが一定に維持されることを、接触部位としての足平部１８Ｒ，１８Ｌの運動に関する目標とされている。

本実施形態における制御入力決定部８５の処理は、前記式５−２２の右辺の演算を行なうために用いる接触部位ヤコビアンＪc及び目標値↑Ｃが、第１実施形態のものと相違するものであること以外は、第１実施形態と同じである。

次に、本実施形態におけるコンプライアンス制御部８６の処理を説明する。

本実施形態におけるコンプライアンス制御部８６には、第１実施形態と同様に、基本パラメータ群決定部８１で決定された遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、制御入力決定部８５で決定された目標関節駆動トルク↑τ1_cmdと、前記接触部位ヤコビアン決定部８２で決定された接触部位代表要素規定比率ｒatとが逐次入力される。さらに、コンプライアンス制御部８６には、制御入力決定部８５で算出された行列Ｒc、Ｊc'、Ｊc#、接触部位代表要素目標加速度↑Ｃ（本実施形態では、“０”ベクトル）、各足平部１８Ｒ，１８Ｌにそれぞれ作用する実際の床反力↑FM_act_R，↑FM_act_Lの計測値、代表リンク１０の移動速度の計測値、ロボット１の代表リンク（下側上体）１０の姿勢角の計測値及びその時間的変化率（角速度）の計測値、並びに、各関節の回転角度の計測値及びその時間的変化率（角速度）の計測値も逐次入力される。

そして、コンプライアンス制御部８６は、第１実施形態と同様に、前記式７−１の演算によって、接触部位代表要素目標作用力↑Ｆc_cmdを算出する。この場合、本実施形態における接触部位代表要素目標作用力↑Ｆc_cmdは、接触部位代表要素の運動加速度↑Xcc'を↑Ｃに一致させ、且つ、ロボット１の各関節の実際の関節駆動トルクを↑τ1_cmdにより規定される目標関節駆動トルクに一致させた場合に、接触部位代表要素に作用すべき外力の推定値、すなわち前記全床反力ベクトル↑FMtの推定値としての意味を持つものである。

そこで、本実施形態では、コンプライアンス制御部８６は、前記式７−１の演算によって算出される接触部位代表要素目標作用力↑Ｆc_cmdを、そのまま、全床反力の目標値としての目標全床反力↑FMt_cmdとして設定する。

さらに、コンプラインアンス制御部８６は、第１実施形態と同様に、各足平部１８Ｒ，１８Ｌにそれぞれ作用する実際の床反力↑FM_act_R，↑FM_act_Lの計測値から、実際の全床反力↑FMt_actの計測値を算出する。

そして、コンプライアンス制御部８６は、算出した目標全床反力↑FMt_cmdと全床反力↑FMt_actの計測値との偏差↑ΔFMt（＝↑FMt_cmd−↑FMt_act）を算出し、この偏差↑ΔFMtを“０”に収束させる操作量としての目標関節駆動トルク↑τ1_cmdの補正量↑Δτtを、前記式７−２と同様の形態の次式７−４により算出する。

↑Δτt＝（Ｊc#^T）^-1＊↑ΔFMt ……式７−４

この場合、本実施形態では、接触部位代表要素に直接的に全床反力↑FMtが作用するものとしているので、前記式７−２の右辺の↑ΔFcを、↑ΔFMtに置き換えた式７−４によって補正量↑Δτtが算出される。なお、式７−４のＪc#は、本実施形態における接触部位ヤコビアンＪcを用いて前記式５−８の定義式に従って算出されたものである。

これにより、フィードバック制御則としての比例則により、偏差↑ΔFMtを“０”に収束させるように、↑Δτtが決定されることとなる。

そして、コンプライアンス制御部８６は、第１実施形態と同様に、前記式７−３の演算によって、補正後目標関節駆動トルク↑τ1c_cmdを決定する。

これにより、目標関節駆動トルク↑τ1_cmdに対応する全床反力である目標全床反力↑FMt_cmdと実際の全床反力↑FMt_actの計測値との偏差を“０”に近づけるように、目標関節駆動トルク↑τ1_cmdが補正されることとなる。

以上が本実施形態におけるコンプライアンス制御部８６の処理である。

本実施形態は、以上説明した事項以外は、前記第１実施形態と同じである。かかる本実施形態によれば、状態量目標値ベクトル↑Ｓ1'_cmdの決定の仕方は、第１実施形態と同じであるので、第１実施形態と同様に、ロボット１の実際の全体重心点の位置が適切な目標位置からずれるの防止することができる。ひいては、ロボット１の各関節が柔軟性を有していても、ロボット１の姿勢のふらつき等が生じるのを防止し、該ロボット１の挙動の安定性を高めることができる。

また、本実施形態では、接触部位代表要素の運動加速度↑Xcc''を接触部位代表要素目標加速度↑Ｃ（本実施形態では“０”ベクトル）により規定した上で、ロボット１の制御対象状態量に関する目標値としての状態量目標値ベクトル↑Ｓ1'_cmdに基づいて、前記式５−２０により↑τ1_cmdが決定される。

このため、第１実施形態と同様に、ロボット１に作用する外力の値を特定することなく、↑Ｃと↑Ｓ1'_cmdとを実現し得る適切な↑τ1_cmdを決定できる。この場合、本実施形態においても、↑Ｃと↑Ｓ1'_cmdとのトータルの自由度数は、ロボット１の全体のトータルの自由度数（前記一般化力ベクトル↑ｑの自由度数）よりも小さいため、第１実施形態と同様に、ロボット１に作用する外力の変動等に伴うロボット１の関節の動きの柔軟性を高めることができる。

加えて、本実施形態では、接触部位代表要素の運動加速度↑Xcc''及び接触部位代表要素目標加速度↑Ｃの自由度数が、前記第１実施形態の場合よりもより少ない自由度数（本実施形態では６）となる。このため、各足平部１８Ｒ，１８Ｌの接地（床との接触）に伴うロボット１の関節の自由度の制約を、第１実施形態のものよりも少なくできる。その結果、ロボット１の関節の動きの柔軟性をより一層高めることができる。

ここで、本実施形態と本発明との対応関係について補足しておく。

本実施形態では、接触部位代表要素ばね性変位量↑Xccが、本発明における接触部位代表要素の位置及び姿勢のばね性変位量Ｘccに相当する。

また、右側の脚リンク３Ｒが本発明における第１の脚リンクに相当すると共に、右側の足平部１８Ｒが該第１の脚リンクの先端部に相当する。そして、右側足平部１８Ｒに関するばね性変位量↑X_Rが本発明における第１の脚リンクの先端部の位置及び姿勢のばね性変位量Ｘ1に相当する。

また、左側の脚リンク３Ｌが本発明における第２の脚リンクに相当すると共に、左側の足平部１８Ｌが該第２の脚リンクの先端部に相当する。そして、左側足平部１８Ｌに関するばね性変位量↑X_Lが本発明における第２の脚リンクの先端部の位置及び姿勢のばね性変位量Ｘ2に相当する。

また、前記式８−１８が、本発明における前記式０２に相当する。この場合、式８−１８の右辺の行列A_R、A_Lがそれぞれ、本発明における行列Ａ1，Ａ2に相当する。

また、前記式８−２０が、本発明における前記式０３に相当する。この場合、式８−２０の右辺のヤコビアンＪcc_R，Ｊcc_Lがそれぞれ、本発明におけるヤコビアンＪ1，Ｊ2に相当する。

以上説明した事項以外は、本実施形態と本発明との対応関係は、前記第１実施形態と本発明との対応関係と同じである。

次に、以上説明した実施形態の変形態様をいくつか説明しておく。

前記各実施形態では、ロボット１を、脚式移動ロボットである２足歩行ロボットとしたが、本発明におけるロボットは、脚式移動ロボット以外の移動ロボット（例えば、車輪により移動する形態のロボット）でもよい。あるいは、本発明におけるロボットは、いずれかのリンクが床等に固定された設置型のロボットであってもよい。

また、ロボット１の歩行動作を行う場合に限らず、走行動作を行なったり、あるいは、ハンド３０Ｒ，３０Ｌによる作業と移動動作と並行して行なうような場合にも本発明を適用できる。

また、所定種類の状態量は、全並進運動量↑Ｐtotal及び全角運動量↑Ｌtotalに限らず、他の種類の状態量であってもよい。

また、前記第１実施形態では、接触部位代表要素目標加速度↑Ｃを“０”としたが、接触部位代表点a1〜a4の他の運動形態の目標を↑Ｃとして設定するようにしてもよい。例えば、接触部位代表点a1〜a4の運動パターンを正弦波状のパターンに設定してもよい。また、例えば、ロボット１がエレベータ等、加速度が発生する系に存在するような場合には、その系の加速度（慣性座標系から見た加速度）を接触部位代表要素目標加速度↑Ｃに付加するようにしてもよい。

同様に、前記第２実施形態における接触部位代表要素目標加速度↑Ｃとして、“０”ベクトル以外の目標値を設定するようにしてもよい。

また、前記コンプライアンス制御部８６を省略してもよい。すなわち、制御入力決定部８５が決定した目標関節駆動トルク↑τ1_cmdをそのまま使用して、ロボット１の各関節の駆動トルクを制御するようにしてもよい。

１…ロボット、２…上体リンク、３…脚リンク、４…腕リンク、５…頭部（リンク）、１０…下側上体（リンク）、１１…上側上体（リンク）、１２，１９〜２４，３１〜３７，３９，４０…関節、１４…大腿部（リンク）、１６…下腿部（リンク）、１８…足平部（リンク）、２６…上腕部（リンク）、２８…前腕部（リンク）、３０…ハンド（リンク）、６０…制御装置、７３…関節駆動制御部（アクチュエータ制御手段）、８１…基本パラメータ群決定部（基本パラメータ群算出手段）、８２…接触部位ヤコビアン決定部（接触部位ヤコビアン算出手段）、８３…状態量ヤコビアン決定部（状態量ヤコビアン算出手段）、８５…制御入力決定部（目標駆動力決定手段）、８６…コンプライアンス制御手段（アクチュエータ制御手段、接触部位外力目標値決定手段、駆動力補正量算出手段）。

Claims (3)

1. 関節を介して相互に連結された複数のリンクと、各関節を駆動するアクチュエータとを備えたロボットにおいて、少なくとも該ロボットの１つ以上の接触部位を該ロボットの外界に接触させつつ該ロボットの運動を行なう場合に、各関節に付与する目標駆動力を決定し、その決定した目標駆動力に応じて前記アクチュエータの動作を制御する制御装置であって、
   前記ロボットの各関節の変位量を成分として少なくとも含む該ロボットの一般化変数ベクトルの２階微分値を一般化力ベクトルに変換するためのイナーシャ行列と、前記ロボットの各リンクに作用する重力により発生する一般化力ベクトルの値である重力依存一般化力ベクトル値とから成る基本パラメータ群、又は、前記イナーシャ行列と前記重力依存一般化力ベクトルと前記ロボットの各リンクに作用する遠心力及びコリオリ力による発生する一般化力ベクトルの値である遠心力・コリオリ力依存一般化力ベクトル値とから成る基本パラメータ群を、少なくとも前記ロボットの各関節の実際の変位量の観測値を含む一般化変数観測情報に基づいて算出する基本パラメータ群算出手段と、
   前記１つ以上の接触部位の接触状態を示す接触状態情報と前記一般化変数観測情報とが入力され、前記外界に接触している前記１つ以上の接触部位の運動を代表する要素として設定した所定の接触部位代表要素の運動速度と前記一般化変数ベクトルの１階微分値との間の関係を表現するヤコビアンである接触部位ヤコビアンを、少なくとも入力された接触状態情報と一般化変数観測情報とを基に算出する接触部位ヤコビアン算出手段と、
   前記一般化変数観測情報が入力され、前記一般化変数ベクトルの値に依存する値を有する所定種類の状態量と該一般化変数ベクトルの１階微分値との間の関係を表現するヤコビアンである状態量ヤコビアンを、少なくとも入力された一般化変数観測情報に基づいて算出する状態量ヤコビアン算出手段と、
   前記算出された基本パラメータ群、接触部位ヤコビアン及び状態量ヤコビアンと、前記接触部位代表要素の運動加速度の目標値である接触部位代表要素運動加速度目標値と、前記一般化変数観測情報と、前記所定種類の状態量の１階微分値の目標値とが入力され、当該入力データを用いて、次式０１の関係を満足する一般化力ベクトルの目標である目標一般化力ベクトルτcmdのうち各関節の変位量に対応する成分値を算出し、その算出した成分値を該関節に付与する目標駆動力として決定する目標駆動力決定手段と、
   少なくとも前記決定された目標駆動力に応じて、前記アクチュエータの動作を制御するアクチュエータ制御手段とを備えることを特徴とするロボットの制御装置。
   
   Ｓ'＋(Ｊs＊Ｍ^-1＊Ｔc−Ｊs')＊ｑ'
   ＝(Ｊs＊Ｍ^-1＊Ｐc)＊(τcmd−τcmpn) ……式０１
   但し、
   ｑ：一般化変数ベクトル
   ｑ'：ｑの１階微分値（＝dｑ/dt）の観測値
   Ｓ：状態量
   Ｓ'：Ｓの１階微分値（＝dＳ/dt）の目標値
   Ｍ：イナーシャ行列
   τcmpn：τcmpn≡(Ｎ＋Ｇ)−Ｐc^-1＊Ｃc、又はτcmpn≡Ｇ−Ｐc^-1*Ｃcにより定義されるベクトル
   Ｇ：重力依存一般化力ベクトル値
   Ｎ：遠心力・コリオリ力依存一般化力ベクトル値
   Ｐc：Ｐc≡Ｉ−Ｊc^T＊(Ｊc#)^Tにより定義される行列
   Ｉ：単位行列
   Ｊc：接触部位ヤコビアン
   Ｊc#：Ｊc#≡(Ｍ^-1)^T＊Ｊc^T＊Ｒc^Tにより定義される行列
   Ｒc：Ｒc≡((Ｊc＊Ｍ^-1＊Ｊc^T)^-1)^Tにより定義される行列
   Ｃc：Ｃc≡−Ｊc^T＊Ｒc＊Ｃにより定義されるベクトル
   Ｃ：接触部位代表要素運動加速度目標値（スカラー又はベクトル）
   Ｔc：Ｔc≡−Ｊc^T＊Ｒc＊Ｊc'により定義される行列
   Ｊc'：Ｊcを１階微分してなる行列（＝dＪc/dt）
   Ｊs：状態量ヤコビアン
   Ｊs'：Ｊsを１階微分してなる行列（＝dＪs/dt）
2. 請求項１記載のロボットの制御装置において、
   前記ロボットは、２つの脚リンクの先端部を床面に交互に接地させる運動により該床面上を移動する２脚移動ロボットであり、
   前記接触部位代表要素は、その位置及び姿勢が、前記２つの脚リンクにそれぞれ作用する床反力の合力である全床反力に応じたばね性変位を生じる要素であって、且つ、該接触部位代表要素の位置及び姿勢のばね性変位量が、前記２つの脚リンクのうちの第１の脚リンクに作用する床反力に応じて生じる該第１の脚リンクの先端部の位置及び姿勢のばね性変位量と、前記２つの脚リンクのうちの第２の脚リンクに作用する床反力に応じて生じる該第２の脚リンクの先端部の位置及び姿勢のばね性変位量と次式０２の関係を有する要素であり、
   前記接触部位ヤコビアン算出手段は、次式０３の右辺の演算によって、前記接触部位ヤコビアンＪcを算出することを特徴とするロボットの制御装置。
   
   Ｘcc＝ｒat＊Ａ1＊Ｘ1＋（１−ｒat）＊Ａ2＊Ｘ2 ……式０２
   
   Ｊc＝ｒat＊Ａ1＊Ｊ1＋（１−ｒat）＊Ａ2＊Ｊ2 ……式０３
   
   但し、
   Ｘcc：接触部位代表要素の位置及び姿勢のばね性変位量を成分とするベクトル
   ｒat：前記第１の脚リンクのみが接地している状態でｒat＝０、前記第２の脚リンクのみが接地している状態でｒat＝１となり、且つ、第１の脚リンク及び第２の脚リンクの両方が接地している状態では、０≦ｒat≦１の範囲で連続的に変化するように前記接触状態情報に応じて設定される係数
   Ｘ1：前記第１の脚リンクに作用する床反力に応じて生じる該第１の脚リンクの先端部の位置及び姿勢のばね性変位量を成分とするベクトル
   Ｘ2：前記第２の脚リンクに作用する床反力に応じて生じる該第２の脚リンクの先端部の位置及び姿勢のばね性変位量を成分とするベクトル
   Ａ1：前記第１の脚リンクに作用する床反力の並進力成分とモーメント成分とからなる第１床反力ベクトルを、前記接触部位代表要素に作用する床反力ベクトルに変換する行列
   Ａ2：前記第２の脚リンクに作用する床反力の並進力成分とモーメント成分とからなる第２床反力ベクトルを、前記接触部位代表要素に作用する床反力ベクトルに変換する行列
   Ｊ1：X1の１階微分値X1'（＝dX1'/dt）と一般化変数ベクトルｑの１階微分値q'との間の関係をX1'＝Ｊ1＊ｑ'により表すヤコビアン
   Ｊ2：X2の１階微分値X2'（＝dX2'/dt）と一般化変数ベクトルｑの１階微分値q'との間の関係をX2'＝Ｊ2＊ｑ'により表すヤコビアン
3. 請求項１又は２記載のロボットの制御装置において、
   前記アクチュエータ制御手段は、前記ロボットの各関節に実際に付与する駆動力を前記決定された目標駆動力に一致させ、且つ前記接触部位代表要素の実際の運動加速度を前記接触部位代表要素運動加速度目標値を一致させたと仮定した場合に前記接触部位に作用する外力の推定値としての接触部位外力目標値を、前記決定された目標駆動力と接触部位運動加速度目標値とを用いて逆動力学演算の処理を実行することにより決定する接触部位外力目標値決定手段と、該接触部位外力目標値と前記接触部位に実際に作用する外力の観測値との偏差を“０”に収束させるように、フィードバック制御則により前記目標駆動力の補正量を決定する駆動力補正量算出手段とを含み、該補正量により補正した前記目標駆動力に応じて、前記アクチュエータの動作を制御することを特徴とするロボットの制御装置。

Images