(57)【特許請求の範囲】
【請求項１】
関節を介して相互に連結された複数のリンクと、各関節を駆動するアクチュエータとを備えるロボットの制御装置であって、
前記ロボットの各関節の変位量を成分として少なくとも含むベクトルである該ロボットの一般化変数ベクトルの１つ以上の成分値に依存してそれぞれの値が規定される複数種類の状態量のそれぞれに関する目標値が入力され、各種類の状態量毎に、該状態量に対応する目標値を実現するために必要な前記アクチュエータの動作を規定する制御入力である状態量別制御入力を、少なくとも該状態量に対応する目標値に基づいて決定する状態量別制御入力決定手段と、
あらかじめ設定された複数の周波数領域のそれぞれにおける前記アクチュエータの動作を規定する制御入力である周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応して決定された前記状態量別制御入力のうちの１つ以上の状態量別制御入力に応じて決定しつつ、前記複数の周波数領域のそれぞれに対応する該周波数領域別制御入力を合成する処理を実行することによって、合成制御入力を決定する合成制御入力決定手段と、
少なくとも前記合成制御入力に応じて、前記アクチュエータの動作を制御するアクチュエータ制御手段とを備え、
前記合成制御入力決定手段が実行する処理は、前記複数の周波数領域のうちの少なくともいずれか１つの周波数領域に対応する前記周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応する状態量別制御入力のうちの複数の状態量別制御入力を相互に非干渉に合成することによって決定する処理を含むことを特徴とするロボットの制御装置。
【請求項２】
請求項１記載のロボットの制御装置において、
前記合成制御入力をτtotal、前記複数の周波数領域の総個数をｎ、ｎ個の該周波数領域のそれぞれを第ｉ周波数領域（ｉ＝１，２，…，ｎ）、第ｉ周波数領域が通過可能な周波数帯となるように周波数通過特性が設定された周波数重みをＷ(i)としたとき、前記合成制御入力決定手段は、次式０２ａにより表される処理によって算出されるｎ個のτf(i)（ｉ＝１，２，…，ｎ）と前記周波数重みＷ(i)（ｉ＝１，２，…，ｎ）とから次式０１により表される処理によって、前記合成制御入力τtotalを決定することを特徴とするロボットの制御装置。
【数１】
0005506823.tif 000007
但し、
【数２】
0005506823.tif 000008
Ｗ(i)＊τf(i)：第ｉ周波数領域に対応する前記周波数領域別制御入力である第ｉ周波数領域別制御入力
ｍ(i)：前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、１≦ｍ(i)≦Ｍ（Ｍ：前記状態量の種類数）、且つ、いずれかのｉに対してｍ(i)＞１という条件とを満たすように設定された整数
τ(i,j)：前記所定の制約条件を満たす範囲内で、第ｉ周波数別制御入力Ｗ(i)＊τf(i)の組成成分として決定されたｍ(i)個の状態量別制御入力τ(i,j)（ｊ＝１，２，…，ｍ(i)）のうちの第ｊ番目の状態量別制御入力である第(i,j)状態量別制御入力（ベクトル）
前記所定の制約条件：全てのｉ（ｉ＝１，２，…，ｎ）に対してｍ(i)＝Ｍが成立し、且つ、互いに異なる任意のｉの値i1、i2（１≦i1≦ｎ、１≦i2≦ｎ）に対してτ(i1,j)＝τ(i2,j)が１からＭまでのすべてのｊ（ｊ＝１，２，…，Ｍ）に対して成立する、ということがないという条件
Ｎu(i,j)：Ｎu(i,j)≡Ｉ−Ｊs(i,j)T＊(Ｊs#(i,j))Tにより定義される行列
Ｊs#(i,j)：Ｊs#(i,j)≡(Ｗat)T＊Ｊs(i,j)T＊(Ｊs(i,j)＊Ｗat＊Ｊs(i,j)T)-1により定義される行列
Ｉ：単位行列
Ｊs(i,j)：Ｓ(i,j)＝Ｊs(i,j)・ｑ'となるヤコビアン
Ｗat：Ｗat≡Ｗd＊Ｍ-1により定義される行列
Ｗd：あらかじめ設定された重み行列（対角行列）
Ｍ：ｑ''を一般化力ベクトルに変換するイナーシャ行列
（Ｍ＊ｑ''＝一般化力ベクトル）
Ｓ(i,j)：第(i,j)状態量別制御入力に対応する状態量（ベクトル又はスカラー）
ｑ：一般化変数ベクトル
ｑ'：ｑの１階微分値（＝dｑ/dt）
ｑ''：ｑの２階微分値（＝d2ｑ/dt2）
【請求項３】
請求項１記載のロボットの制御装置において、
前記合成制御入力をτtotal、前記複数の周波数領域の総個数をｎ、ｎ個の該周波数領域のそれぞれを第ｉ周波数領域（ｉ＝１，２，…，ｎ）、第ｉ周波数領域が通過可能な周波数帯となるように周波数通過特性が設定された周波数重みをＷ(i)としたとき、前記合成制御入力決定手段は、次式０２ｂにより表される処理によって算出されるｎ個のτf(i)（ｉ＝１，２，…，ｎ）と前記周波数重みＷ(i)（ｉ＝１，２，…，ｎ）とから次式０１により表される処理によって、前記合成制御入力τtotalを決定することを特徴とするロボットの制御装置。
【数３】
0005506823.tif 000009
但し、
【数４】
0005506823.tif 000010
Ｗ(i)＊τf(i)：第ｉ周波数領域に対応する前記周波数領域別制御入力である第ｉ周波数領域別制御入力
ｍ(i)：前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、１≦ｍ(i)≦Ｍ（Ｍ：前記状態量の種類数）、且つ、いずれかのｉに対してｍ(i)＞１という条件とを満たすように設定された整数
τ(i,j)：前記所定の制約条件を満たす範囲内で、第ｉ周波数別制御入力Ｗ(i)＊τf(i)の組成成分として決定されたｍ(i)個の状態量別制御入力τ(i,j)（ｊ＝１，２，…，ｍ(i)）のうちの第ｊ番目の状態量別制御入力である第(i,j)状態量別制御入力（ベクトル）
前記所定の制約条件：全てのｉ（ｉ＝１，２，…，ｎ）に対してｍ(i)＝Ｍが成立し、且つ、互いに異なる任意のｉの値i1、i2（１≦i1≦ｎ、１≦i2≦ｎ）に対してτ(i1,j)＝τ(i2,j)が１からＭまでのすべてのｊ（ｊ＝１，２，…，Ｍ）に対して成立する、ということがないという条件
wc(i,j)：第(i,j)状態量別制御入力τ(i,j)に対応する重み係数であり、０≦wc(i,j)≦１の範囲内で一定値に維持されるか、又は連続的に変化するように設定された重み係数範囲内で固定的又は可変的に設定された重み係数
Ｎuw(i,j)：Ｎuw(i,j)≡Ｉ−wc(i,j)＊Ｊs(i,j)T＊(Ｊs#(i,j))Tにより定義される行列
Ｊs#(i,j)：Ｊs#(i,j)≡(Ｗat)T＊Ｊs(i,j)T＊(Ｊs(i,j)＊Ｗat＊Ｊs(i,j)T)-1により定義される行列
Ｉ：単位行列
Ｊs(i,j)：Ｓ(i,j)＝Ｊs(i,j)・ｑ'となるヤコビアン
Ｗat：Ｗat≡Ｗd＊Ｍ-1により定義される行列
Ｗd：あらかじめ設定された重み行列（対角行列）
Ｍ：ｑ''を一般化力ベクトルに変換するイナーシャ行列
（Ｍ＊ｑ''＝一般化力ベクトル）
Ｓ(i,j)：第(i,j)状態量別制御入力に対応する状態量（ベクトル又はスカラー）
ｑ：一般化変数ベクトル
ｑ'：ｑの１階微分値（＝dｑ/dt）
ｑ''：ｑの２階微分値（＝d2ｑ/dt2）
【請求項４】
請求項３記載のロボットの制御装置において、
前記ロボットはその所定の部位を該ロボットの外界の接触対象部に接触させる動作と該接触対象部から離脱させる動作とを含む運動を行なうロボットであると共に、前記複数種類の状態量は、前記ロボットの所定の部位を前記接触対象部から離脱させた状態での該所定の部位の運動状態量を、前記複数の周波数領域のうちの少なくとも１つの周波数領域における制御対象として含んでおり、
前記所定の部位の運動状態量を制御対象として含む周波数領域を第ｉa周波数領域とし、該第ｉa周波数領域に対応する第ｉa周波数領域別制御入力（Ｗ(ia)＊τf(i)）の組成成分となる前記状態量別制御入力のうちの前記所定の部位の運動状態量に対応する状態量別制御入力を第(ia,ja)状態量別制御入力τ(ia,ja)としたとき、該τ(ia,ja)に対応する前記重み係数ｗc(ia,ja)は、前記所定の部位が前記接触対象部に接触する期間では、該重み係数ｗc(ia,ja)の値が“０”に維持され、且つ、前記所定の部位が前記接触対象部から離脱する期間では、該重み係数ｗc(ia,ja)の値が“０”から“１”まで連続的に増加した後、“１”から“０”まで連続的に減少するように設定されることを特徴とするロボットの制御装置。
【請求項５】
請求項２記載のロボットの制御装置において、
前記各周波数重みW(i)（ｉ＝１，２，…，ｎ）は、次式０３の伝達関数により表される周波数重みであることを特徴とするロボットの制御装置。
【数５】
0005506823.tif 000011
但し、
ｓ：ラプラス変換の演算子
T(i) (i =1, 2,…, n）：T(1)＜T(2)＜……＜Ｔ(n)となるように設定された時定数
W(n+1)≡０
【請求項６】
請求項３記載のロボットの制御装置において、
前記各周波数重みW(i)（ｉ＝１，２，…，ｎ）は、次式０３の伝達関数により表される周波数重みであることを特徴とするロボットの制御装置。
【数６】
0005506823.tif 000012
但し、
ｓ：ラプラス変換の演算子
T(i) (i =1, 2,…, n）：T(1)＜T(2)＜……＜Ｔ(n)となるように設定された時定数
W(n+1)≡０
【請求項７】
請求項２記載のロボットの制御装置において、
前記個数m(i)がm(i)＞１である前記第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力τ(i,j)（ｊ＝１，２，…，m(i)）は、前記式０２ａの演算に用いるm(i)個の前記行列Nu(i,j)（ｊ＝１，２，…，m(i)）が、rank(Nu(i,1))≧rank(Nu(i,2))≧……≧rank(Nu(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するＭ個の状態量別制御入力の中から選定された状態量別制御入力であることを特徴とするロボットの制御装置。
【請求項８】
請求項３記載のロボットの制御装置において、
前記個数m(i)がm(i)＞１である前記第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力τ(i,j)（ｊ＝１，２，…，m(i)）は、前記式０２ｂの演算に用いるm(i)個の前記行列Nuw(i,j)（ｊ＝１，２，…，m(i)）が、rank(Nuw(i,1))≧rank(Nuw(i,2))≧……≧rank(Nuw(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するＭ個の状態量別制御入力の中から選定された状態量別制御入力であることを特徴とするロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、脚式移動ロボット等のロボットの制御装置に関する。
【背景技術】
【０００２】
２足歩行ロボット等の脚式移動ロボットの制御技術としては、例えば特許文献１等にて本願出願人が提案した技術の如く、ロボットの目標とする運動（ロボットの脚体先端部の目標位置の軌道等）に応じてロボットの各関節の目標変位量を逐次決定し、その目標変位量に実際の変位量を一致させるように各関節のアクチュエータの駆動制御を行う、所謂、位置制御が一般的に知られている。【０００３】
このような位置制御は、関節の目標変位量が一定である限り、該関節の実際の変位量を一定に保持するものであるから、ロボットに作用する外力の変化に対する柔軟性が乏しい。【０００４】
このため、例えば、特許文献２に見られる如く、関節とアクチュエータとの間にばね部材を介装することで関節に柔軟性を持たせると共に、ロボットの目標とする運動を実現するための各関節の目標駆動力を逆動力学演算により決定し、所謂、力制御によって、各関節の実際の駆動力を目標駆動力に制御する技術も知られている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平５−３０５５７９号公報
【特許文献２】特開２００７−１６０４４６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ところで、ロボットの運動を制御する場合、その運動の安定性などを確保するために、ロボットの１つ以上の関節の変位量に応じて値が変化する複数種類の状態量（例えば、ロボットの互いに異なる複数の部位の位置や姿勢、あるいは、互いに異なる部位又は代表点の運動速度、あるいは、ロボットの所定の部位の互いに異なる種類の運動状態量等）をそれぞれに対応する適切な目標値に制御することが望ましい状況が多々ある。特に、力制御方式のロボットでは、ロボットに作用する外力の変化に応じて各関節の変位量が変動し易いことから、上記のように、複数種類の状態量をそれぞれに対応する適切な目標値に制御することの必要性が高い。【０００７】
この場合、一般には、各種類の状態量の値を調整し得る関節が、該状態量の種類毎に常に互いに異なる関節となるとは限らず、ロボットの瞬時瞬時の運動状態等に依存して、各種類の状態量の値を調整し得る関節が互いに重複する場合もある。また、各種類の状態量を目標値に制御することの優先度や該制御に要求される速応性が、状態量の種類毎に相違したり、あるいは、ロボットの動作状況等に応じて変化する場合もある。【０００８】
このため、ロボットの種々様々な動作状況で、複数種類の状態量をそれぞれに対応する適切な目標値に効率よく制御するようにロボットの動作制御を行なうことができる汎用性の高い制御技術が望まれていた。【０００９】
しかるに、従来のロボットの制御技術では、このような要望を十分に満足し得る技術が確立されていないのが実状である。【００１０】
本発明は、かかる背景に鑑みてなされてものであり、ロボットの種々様々な動作状況で、複数種類の状態量をそれぞれに対応する目標値に効率よく制御するようにロボットの動作制御を行なうことができる汎用性の高い制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１１】
本発明のロボットの制御装置は、かかる目的を達成するために、関節を介して相互に連結された複数のリンクと、各関節を駆動するアクチュエータとを備えるロボットの制御装置であって、
前記ロボットの各関節の変位量を成分として少なくとも含むベクトルである該ロボットの一般化変数ベクトルの１つ以上の成分値に依存してそれぞれの値が規定される複数種類の状態量のそれぞれに関する目標値が入力され、各種類の状態量毎に、該状態量に対応する目標値を実現するために必要な前記アクチュエータの動作を規定する制御入力である状態量別制御入力を、少なくとも該状態量に対応する目標値に基づいて決定する状態量別制御入力決定手段と、
あらかじめ設定された複数の周波数領域のそれぞれにおける前記アクチュエータの動作を規定する制御入力である周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応して決定された前記状態量別制御入力のうちの１つ以上の状態量別制御入力に応じて決定しつつ、前記複数の周波数領域のそれぞれに対応する該周波数領域別制御入力を合成する処理を実行することによって、合成制御入力を決定する合成制御入力決定手段と、
少なくとも前記合成制御入力に応じて、前記アクチュエータの動作を制御するアクチュエータ制御手段とを備え、
前記合成制御入力決定手段が実行する処理は、前記複数の周波数領域のうちの少なくともいずれか１つの周波数領域に対応する前記周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応する状態量別制御入力のうちの複数の状態量別制御入力を相互に非干渉に合成することによって決定する処理を含むことを特徴とする（第１発明）。【００１２】
補足すると、本発明においては、前記「ロボット」は、少なくとも１つのリンクがロボットの動作環境におけるグローバル座標系に対して固定された（該動作環境の床等に固定された）設置型とのロボットと、該グローバル座標系に対してロボットの全体が移動可能な移動ロボットとのいずれのロボットであってもよい。なお、グローバル座標系は、慣性座標系又は近似的に慣性系と見なせる座標系を意味する。【００１３】
また、前記一般化変数ベクトルは、前記ロボットが設置型のロボットである場合には、該ロボットの全ての関節の変位量（回転角度又はストローク量）を成分値として構成されるベクトルである。一方、前記ロボットが移動ロボットである場合には、前記一般化変数ベクトルは、該ロボットの任意の１つの代表リンクのグローバル座標系での位置及び姿勢と、該ロボットの全ての関節の変位量とを成分値として構成されるベクトルである。【００１４】
なお、ロボットのリンクの「位置」は、該リンクに対して固定された代表点の空間的な位置を意味し、リンクの「姿勢」は、リンクの空間的な向きを意味する。【００１５】
前記第１発明によれば、前記ロボットの複数種類の状態量を所要の目標値に制御するために、各種類の状態量毎に、該状態量に対応する目標値を実現する（該状態量の実際の値を目標値に制御する）ために必要な前記アクチュエータの動作を規定する状態量別制御入力が、前記状態量別制御入力決定手段によって、少なくとも該状態量に対応する目標値に基づいて決定される。【００１６】
なお、各種類の状態量は、該ロボットの一般化変数ベクトルの１つ以上の成分値に依存してそれぞれの値が規定される状態量、すなわち、一般化変数ベクトルの１つ又は複数の成分値の関数値となる状態量である。【００１７】
そして、第１発明では、前記合成制御入力決定手段が、前記複数の周波数領域のそれぞれにおける周波数領域別制御入力を前記状態量別制御入力に応じて決定しつつ、前記複数の周波数領域のそれぞれに対応する該周波数領域別制御入力を合成する処理を実行することによって、前記合成制御入力を決定する。【００１８】
さらに、前記アクチュエータ制御手段が、少なくとも前記合成制御入力に応じて前記アクチュエータの動作を制御する。これにより、各周波数領域毎の周波数領域別制御入力を反映させて、ロボットの動作制御が行なわれる。【００１９】
ここで、各周波数領域毎の周波数領域別制御入力は、前記複数種類の状態量のそれぞれに対応する状態量別制御入力のうちの１つ又は複数の状態量別制御入力（すなわち、１つ以上の状態量別制御入力）に応じて決定される。なお、各周波数領域毎の周波数領域別制御入力の基となる状態量別制御入力は、より詳しくは、該当する周波数領域内の周波数成分である。【００２０】
そして、少なくともいずれか１つの周波数領域（１つ以上の周波数領域）に関しては、該周波数領域（以降、特定周波数領域ということがある）に対応する周波数別制御入力は、前記複数種類の状態量のそれぞれに対応する状態量別制御入力のうちの２つ以上（複数）の状態量別制御入力（該状態量別制御入力の個数は、前記状態量の種類数よりも少なくてもよい）に応じて決定される。さらに、この場合、該特定周波数領域に対応する周波数別制御入力は、当該２つ以上の状態量別制御入力を相互に非干渉に合成することによって決定される。【００２１】
なお、前記複数の周波数領域は、部分的な重複を有する周波数領域であってもよい。【００２２】
ここで、本発明において、「当該２つ以上の状態量別制御入力を相互に非干渉に合成する」ということは、その合成後の周波数別制御入力を制御入力Ａと定義すると共に、該制御入力Ａの基となる当該２つ以上の状態量別制御入力のうちの任意の１つの状態量別制御入力を制御入力Ｂと定義し、制御入力Ａによってロボットのアクチュエータの動作制御を行なった場合と、制御入力Ｂの前記特定周波数領域の成分によってロボットのアクチュエータの動作制御を行った場合とを想定したとき、その両方の場合において、前記複数種類の状態量のうちの制御入力Ｂに対応する種類の状態量とを同じ状態に制御し得るように、当該２つ以上の状態量別制御入力を合成することを意味する。【００２３】
換言すれば、「当該２つ以上の状態量別制御入力を相互に非干渉に合成する」ということは、前記一般化変数ベクトルのうち、制御入力Ｂに対応する状態量の値に影響を及ぼす成分に対する操作が、制御入力Ａと制御入力Ｂとで同じ操作となり、その他の成分のうちの１つ又は複数の成分に対する操作が、制御入力Ａと制御入力Ｂとで異なる操作となるように、当該２つ以上の状態量別制御入力を合成することを意味する。【００２４】
かかる第１発明によれば、各周波数領域毎に、それに対応する周波数別制御入力の基となる状態量別制御入力を選定できる。換言すれば、各周波数領域毎に、制御対象（制御しようとする対象）としての状態量を選定できる。そのため、各周波数領域毎に、前記複数の状態量の制御に関する要求（該制御の優先度合いや速応性の要求度合い、該制御の必要性等）に適した周波数領域別制御入力を決定できる。【００２５】
また、前記特定周波数領域に関しては、複数の状態量別制御入力を相互に非干渉に合成することによって、それぞれの状態量別制御入力に対応する複数種類の状態量をそれぞれ目標値に制御することを、相互に干渉することがないように（該複数種類の状態量のそれぞれの制御が他の状態量の制御に影響を及ぼすことが無いように）行なうことができる。【００２６】
この結果、第１発明によれば、ロボットの種々様々な動作状況で、複数種類の状態量をそれぞれに対応する目標値に効率よく制御するようにロボットの動作制御を行なうことができる。また、ロボットの構成や、制御対象の状態量として、種々様々な態様を採用できるので、上記の如き動作制御を行う制御装置の汎用性を高めることができる。【００２７】
補足すると、本発明において、前記状態量別制御入力決定手段が、前記各種類の状態量に対応させて決定する状態量別制御入力は、各種類の状態量に対して単一の制御入力である必要はなく、いずれかの種類の状態量に対して、該状態量に関する前記目標値を実現するための複数の状態量別制御入力を、それぞれ各別の制御則によって決定するようにしてもよい。例えば、該状態量に関する前記目標値を実現するための２つの状態量別制御入力を、フィードフォワード制御則と、フィードバック制御則とによって各別に決定するようにしてもよい。そして、この場合、互いに異なる周波数領域での制御対象に、当該状態量を含めると共に、そのそれぞれの周波数領域に対応する周波数領域別制御入力を、当該状態量に対応する各別の状態量別制御入力（互いに異なる制御則によって決定した各別の状態量別制御入力）を用いて決定するようにしてもよい。【００２８】
前記第１発明においては、前記合成制御入力決定手段の処理は、より具体的には、例えば次のような処理によって実現される。【００２９】
すなわち、前記合成制御入力をτtotal、前記複数の周波数領域の総個数をｎ、ｎ個の該周波数領域のそれぞれを第ｉ周波数領域（ｉ＝１，２，…，ｎ）、第ｉ周波数領域が通過可能な周波数帯となるように周波数通過特性が設定された周波数重みをＷ(i)としたとき、前記合成制御入力決定手段は、例えば、次式０２ａにより表される処理によって算出されるｎ個のτf(i)（ｉ＝１，２，…，ｎ）と前記周波数重みＷ(i)（ｉ＝１，２，…，ｎ）とから次式０１により表される処理によって、前記合成制御入力τtotalを決定する（第２発明）。【００３０】
なお、本明細書では、「＊」は乗算記号を意味する。【００３１】
【数１】
0005506823.tif 000002
【００３２】
但し、【００３３】
【数２】
0005506823.tif 000003
【００３４】
Ｗ(i)＊τf(i)：第ｉ周波数領域に対応する前記周波数領域別制御入力である第ｉ周波数領域別制御入力
ｍ(i)：前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、１≦ｍ(i)≦Ｍ（Ｍ：前記状態量の種類数）、且つ、いずれかのｉに対してｍ(i)＞１という条件とを満たすように設定された整数
τ(i,j)：前記所定の制約条件を満たす範囲内で、第ｉ周波数別制御入力Ｗ(i)＊τf(i)の組成成分として決定されたｍ(i)個の状態量別制御入力τ(i,j)（ｊ＝１，２，…，ｍ(i)）のうちの第ｊ番目の状態量別制御入力である第(i,j)状態量別制御入力（ベクトル）
前記所定の制約条件：全てのｉ（ｉ＝１，２，…，ｎ）に対してｍ(i)＝Ｍが成立し、且つ、互いに異なる任意のｉの値i1、i2（１≦i1≦ｎ、１≦i2≦ｎ）に対してτ(i1,j)＝τ(i2,j)が１からＭまでのすべてのｊ（ｊ＝１，２，…，Ｍ）に対して成立する、ということがないという条件
Ｎu(i,j)：Ｎu(i,j)≡Ｉ−Ｊs(i,j)T＊(Ｊs#(i,j))Tにより定義される行列
Ｊs#(i,j)：Ｊs#(i,j)≡ＷatT＊Ｊs(i,j)T＊(Ｊs(i,j)＊Ｗat＊Ｊs(i,j)T)-1により定義される行列
Ｉ：単位行列
Ｊs(i,j)：Ｓ(i,j)＝Ｊs(i,j)・ｑ'となるヤコビアン
Ｗat：Ｗat≡Ｗd＊Ｍ-1により定義される行列
Ｗd：あらかじめ設定された重み行列（対角行列）
Ｍ：ｑ''を一般化力ベクトルに変換するイナーシャ行列
（Ｍ＊ｑ''＝一般化力ベクトル）
Ｓ(i,j)：第(i,j)状態量別制御入力に対応する状態量（ベクトル又はスカラー）
ｑ：一般化変数ベクトル
ｑ'：ｑの１階微分値（＝dｑ/dt）
ｑ''：ｑの２階微分値（＝d2ｑ/dt2）
なお、前記式０１におけるＷ(i)＊τf(i)という演算処理は、τf(i)を、周波数重みＷ(i)と同じ周波数通過特性のフィルタに通す処理（フィルタリング処理）と等価である。【００３５】
また、前記「一般化力ベクトル」は、その各成分が、一般化変数ベクトルｑの各成分に対応しているベクトル（ｑと同数の成分からなるベクトル）である。この一般化力ベクトルは、より詳しくは、その各成分が、それに対応する一般化変数ベクトルｑの成分値の時間的変化を発生させる力（並進力又はモーメント）となるベクトルである。【００３６】
かかる第２発明によれば、ｍ(i)＞１となる第ｉ周波数領域が前記特定周波数領域に相当する。【００３７】
この場合、行列Ｎu(i,j)が前記した通り定義される行列であるので、前記式０２において、例えば、Ｎu(i,1)＊τ(i,2)は、状態量別制御入力τ(i,2)のうち、状態量別制御入力τ(i,1)に対応する種類の状態量の値に影響を及ぼすことのない（該状態量の値を変化させない）成分から構成されるベクトル（一般化力ベクトル）を意味する。【００３８】
より一般的に言えば、前記式０２における、Ｎu(i,1)＊Ｎu(i,2)＊……＊Ｎu(i,m(i)-1)＊τ(i,m(i))は、状態量別制御入力τ(i,m(i))のうち、（ｍ(i)−１）個の状態量別制御入力τ(i,1)〜τ(i,m(i)-1)にそれぞれ対応する状態量の値に影響を及ぼすことのない成分から構成されるベクトル（一般化ベクトル）を意味する。【００３９】
そして、前記第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力の個数ｍ(i)（換言すれば、第ｉ周波数領域における制御対象として選定される状態量の個数）が前記の条件を満たすように設定される。【００４０】
このため、第２発明によれば、ｍ(i)＞１となる第ｉ周波数領域（特定周波数領域）に対応する第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)は、ｍ(i)個の状態量別制御入力τ(i,1)〜τ(i,m(i))を相互に非干渉に合成したものとなる。【００４１】
従って、第２発明によれば、前記第１発明の効果を実現し得る前記合成制御入力を適切に決定することができる。【００４２】
補足すると、前記所定の制約条件は、換言すれば、いずれかのｉ（１≦ｉ≦ｎ）に対してｍ(i)≠Ｍとなるか、又は、いずれかのｊ（１≦ｉ≦Ｍ）に対して、τ(i3,j)≠τ(i4,j)となるようなｉの値のi3，i4の組が存在する、という条件と等価である。【００４３】
また、前記重み行列Ｗdは単位行列であってもよい。すなわち、Ｗat＝Ｍ-1であってもよい。【００４４】
ところで、各周波数領域において、制御対象として選定すべき状態量の種類や個数は、ロボットの動作中に常時、一定であることが要求されるとは限らず、ロボットの動作状況等に応じて、制御対象とする状態量の種類や個数を変更することが必要もしくは好適となる場合もある。【００４５】
例えば、ロボットの一連の動作の中での特定の動作を行なっている期間でのみ、ある特定種類の状態量を、特定の周波数領域における制御対象として選定し、それ以外の期間では、当該特定種類の状態量を、当該特定の周波数領域における制御対象から除外したり、あるいは別の種類の状態量に置き換えることが必要となる場合もある。【００４６】
このような場合に、ロボットの動作中のあるタイミング（制御対象の状態量の種類や個数を変更しようとするタイミング）で、任意の周波数領域における制御対象の状態量の種類や個数を直ちに変更するようにすると、その変更の前後で該周波数領域に対応する周波数領域別制御入力の不連続的な変化が生じやすい。ひいては、前記合成制御入力の不連続的な変化が生じやすい。【００４７】
そして、前記合成制御入力の不連続的な変化が生じると、ロボットのアクチュエータの動作の急激な変化が要求されることとなる。従って、前記複数の周波数領域のうちのいずれかの周波数領域において、制御対象とする状態量の種類や個数を変更しようとする場合には、前記合成制御入力の不連続的な変化を防止し得るように当該変更を行なうことが望まれる。【００４８】
このような要望を達成するためには、前記第１発明において、前記合成制御入力決定手段の処理を、例えば次のような処理によって実現することが好ましい。【００４９】
すなわち、前記合成制御入力決定手段は、前記式０２ａの代わりに、次式０２ｂにより表される処理によって算出されるｎ個のτf(i)（ｉ＝１，２，…，ｎ）と前記周波数重みＷ(i)（ｉ＝１，２，…，ｎ）とから前記式０１により表される処理によって、前記合成制御入力τtotalを決定する（第３発明）。【００５０】
【数３】
0005506823.tif 000004
【００５１】
但し、【００５２】
【数４】
0005506823.tif 000005
【００５３】
Ｗ(i)＊τf(i)：第ｉ周波数領域に対応する前記周波数領域別制御入力である第ｉ周波数領域別制御入力
ｍ(i)：前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、１≦ｍ(i)≦Ｍ（Ｍ：前記状態量の種類数）、且つ、いずれかのｉに対してｍ(i)＞１という条件とを満たすように設定された整数
τ(i,j)：前記所定の制約条件を満たす範囲内で、第ｉ周波数別制御入力Ｗ(i)＊τf(i)の組成成分として決定されたｍ(i)個の状態量別制御入力τ(i,j)（ｊ＝１，２，…，ｍ(i)）のうちの第ｊ番目の状態量別制御入力である第(i,j)状態量別制御入力（ベクトル）
前記所定の制約条件：全てのｉ（ｉ＝１，２，…，ｎ）に対してｍ(i)＝Ｍが成立し、且つ、互いに異なる任意のｉの値i1、i2（１≦i1≦ｎ、１≦i2≦ｎ）に対してτ(i1,j)＝τ(i2,j)が１からＭまでのすべてのｊ（ｊ＝１，２，…，Ｍ）に対して成立する、ということがないという条件
ｗc(i,j)：第(i,j)状態量別制御入力τ(i,j)に対応する重み係数であり、０≦ｗc(i,j)≦１の範囲内で一定値に維持されるか、又は連続的に増加もしくは減少するように設定された重み係数
Ｎuw(i,j)：Ｎuw(i,j)≡Ｉ−wc(i,j)＊Ｊs(i,j)T＊(Ｊs#(i,j))Tにより定義される行列
Ｊs#(i,j)：Ｊs#(i,j)≡(Ｗat)T＊Ｊs(i,j)T＊(Ｊs(i,j)＊Ｗat＊Ｊs(i,j)T)-1により定義される行列
Ｉ：単位行列
Ｊs(i,j)：Ｓ(i,j)＝Ｊs(i,j)・ｑ'となるヤコビアン
Ｗat：Ｗat≡Ｗd＊Ｍ-1により定義される行列
Ｗd：あらかじめ設定された重み行列（対角行列）
Ｍ：ｑ''を一般化力ベクトルに変換するイナーシャ行列
（Ｍ＊ｑ''＝一般化力ベクトル）
Ｓ(i,j)：第(i,j)状態量別制御入力に対応する状態量（ベクトル又はスカラー）
ｑ：一般化変数ベクトル
ｑ'：ｑの１階微分値（＝dｑ/dt）
ｑ''：ｑの２階微分値（＝d2ｑ/dt2）
上記第３発明における式０２ｂは、前記第２発明における式０２ａの右辺の各項に対して、τ(i,j)（ｊ＝１，２，…，ｍ(i)）に乗じる重み係数ｗc(i,j)を付加し、且つ、前記式０２ａにおける行列Ｎu(i,j)（≡Ｉ−＊Ｊs(i,j)T＊(Ｊs#(i,j))T）を、該重み係数ｗc(i,j)を付加して定義される上記行列Ｎuw(i,j)（≡Ｉ−wc(i,j)＊Ｊs(i,j)T＊(Ｊs#(i,j))T）に置き換えたものである。【００５４】
かかる第３発明によれば、第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分となる全ての状態量別制御入力τ(i,j)（ｊ＝１，２，…，ｍ(i)）に対応する前記重み係数ｗc(i,j)の値を“１”に設定した場合には、式０２ｂは前記式０２ａに一致するものとなる。従って、ｍ(i)＞１となる第ｉ周波数領域（特定周波数領域）においては、前記第２発明と同様に、ｍ(i)個の状態量別制御入力τ(i,1)〜τ(i,m(i))を相互に非干渉に合成したものととして、第ｉ周波数領域別制御入力を決定できることとなる。【００５５】
また、いずれかの第ｉ周波数領域において、いずれかの第(i,j)状態量別制御入力τ(i,j)に乗じる重み係数ｗc(i,j)を“０”に設定した場合には、該第ｉ周波数領域に対応する第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)は、該第(i,j)状態量別制御入力τ(i,j)や、第(i,j)状態量別制御入力τ(i,j)に対応する前記ヤコビアンＪs(i,j)に対する依存性が無くなる。このため、該第(i,j)状態量別制御入力τ(i,j)に対応する状態量は、実質的に、該第ｉ周波数領域における制御対象から除外されることとなる。【００５６】
さらに、いずれかの第ｉ周波数領域において、いずれかの第(i,j)状態量別制御入力τ(i,j)に対応する重み係数ｗc(i,j)を“０”と“１”との間の範囲内で連続的に増加もしくは減少させるように設定した場合には、該第ｉ周波数領域に対応する第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の、該第(i,j)状態量別制御入力τ(i,j)に対する依存度が連続的に変化することとなる。例えば、該重み係数wc(i,j)の値を“１”から“０”に向かって連続的に減少させていくと、第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)は、第(i,j)状態量別制御入力τ(i,j)に対する依存度が連続的に低下していく。【００５７】
従って、各周波数領域における制御対象の状態量の種類及び個数の両方又は一方を、重み係数ｗc(i,j)の値の設定変更によって適宜変更することができることとなる。さらに、その変更の際に、該重み係数ｗc(i,j)の値を連続的に増加もしくは減少させることによって、第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)を連続的に変化させることができることとなる。ひいては、前記合成制御入力τtotalを、不連続的に変化することがないように（連続的に変化するように）決定できることとなる。【００５８】
この場合、例えば、各周波数領域において、ある状態量を制御対象から除外しようとする場合には、その状態量に対応する状態量別制御入力τ(i,j)に乗じる重み係数ｗc(i,j)の値を“１”から“０”に連続的に減少させるように設定すればよい。逆に、各周波数領域において、制御対象から除外した状態量を、制御対象に復帰させようとする場合には、その状態量に対応する状態量別制御入力τ(i,j)に乗じる重み係数ｗc(i,j)の値を“０”から“１”に連続的に増加させるように設定すればよい。【００５９】
さらに、各周波数領域において、ある状態量を制御対象から除外すると共に、別の種類の状態量を制御対象に加えようとする場合、換言すれば、制御対象とする状態量の種類を変更しようとする場合には、制御対象から除外しようとする状態量に対応する状態量別制御入力τ(i,j)に乗じる重み係数ｗc(i,j)の値を“１”から“０”に連続的に減少させると共に、制御対象に加えようとする別の種類の状態量に対応する状態量別制御入力τ(i,j)に乗じる重み係数ｗc(i,j)の値を“０”から“１”に連続的に増加させるように設定すればよい。【００６０】
以上のことから、第３発明によれば、前記第１発明の効果を実現できる前記合成制御入力を連続的に変化させるようにしつつ、適切に決定することが可能となる。また、各周波数領域毎に、ロボットの動作状況等に応じて重み係数ｗc(i,j)の値を可変的に設定することで、制御対象とする状態量の種類や個数を、ロボットの動作状況等に応じて適宜変更することができる。そして、この場合、制御対象とする状態量の種類や個数を変更する周波数領域に対応する周波数領域別制御入力Ｗ(i)＊τf(i)を連続的に変化させることができるので、前記合成制御入力を不連続的に変化することがないように決定できる。ひいては、ロボットの動作をスムーズに行なうことができる。【００６１】
かかる第３発明では、例えば、前記ロボットがその所定の部位（例えば脚体やアームの先端部）を該ロボットの外界の接触対象部に接触させる動作と該接触対象部から離脱させる動作とを含む運動を行なうロボットであると共に、前記複数種類の状態量が、前記ロボットの所定の部位を前記接触対象部から離脱させた状態での該所定の部位の運動状態量を、前記複数の周波数領域のうちの少なくとも１つの周波数領域における制御対象として含んでいる場合に、次のような態様を採用することができる。【００６２】
すなわち、前記所定の部位の運動状態量を制御対象として含む周波数領域を第ｉa周波数領域とし、該第ｉa周波数領域に対応する第ｉa周波数領域別制御入力Ｗ(ia)＊τf(i)の組成成分となる前記状態量別制御入力のうちの前記所定の部位の運動状態量に対応する状態量別制御入力を第(ia,ja)状態量別制御入力τ(ia,ja)としたとき、該τ(ia,ja)に対応する前記重み係数ｗc(ia,ja)は、前記所定の部位が前記接触対象部に接触する期間では、該重み係数ｗc(ia,ja)の値が“０”に維持され、且つ、前記所定の部位が前記接触対象部から離脱する期間では、該重み係数ｗc(ia,ja)の値が“０”から“１”まで連続的に増加した後、“１”から“０”まで連続的に減少するように設定される（第４発明）。【００６３】
この第４発明によれば、前記所定の部位が前記接触対象部に接触する期間では、前記重み係数ｗc(ia,ja)の値が“０”に維持される。【００６４】
ここで、該期間では、前記所定の部位が前記接触対象部に接触するので、その接触によって、該所定の部位の運動が拘束を受けることとなる。そして、このような状況で、該所定の部位の運動状態量（例えば、該所定の部位の位置、姿勢、もしくはその時間的変化率、加速度等）を強制的に所望の目標値に制御しようとすると、該運動状態量の実際の値と目標値とのずれが生じた場合に、前記アクチュエータの過大な駆動力が要求される恐れがある。【００６５】
そこで、第４発明では、前記所定の部位が前記接触対象部に接触する期間では、該重み係数ｗc(ia,ja)の値が“０”に維持する。これにより、該期間では、前記所定の部位の運動状態量が、前記第ｉa周波数領域での制御対象から実質的に除外されることとなる。【００６６】
一方、前記所定の部位が前記接触対象部から離脱する期間では、前記重み係数ｗc(ia,ja)の値が“０”から“１”まで連続的に増加した後、“１”から“０”まで連続的に減少するように設定される。このため、該期間では、前記所定の部位の運動状態量が、前記第ｉa周波数領域での制御対象に含まれるようになる。この場合、前記所定の部位は前記接触対象部から離脱しており、外界との接触に伴う拘束を受けない状態であるので、当該期間での第ｉa周波数領域における前記所定の部位の運動状態量を、円滑に所望の目標値に制御することができる。【００６７】
さらに、この場合、前記重み係数ｗc(ia,ja)の値が“０”から“１”まで連続的に増加した後、“１”から“０”まで連続的に減少するように設定されるので、前記所定の部位が前記接触対象部から離脱する期間内で、第ｉa周波数領域別制御入力Ｗ(ia)＊τf(ia)を連続的に滑らかに変化させることができる。ひいては、前記合成制御入力が不連続的に変化しないようにして、ロボットの動作を円滑に行うことができる。【００６８】
前記第２〜第４発明では、前記各周波数重みW(i)（ｉ＝１，２，…，ｎ）としては、例えば、次式０３の伝達関数により表される周波数重みを採用することができる（第５発明）。【００６９】
【数５】
0005506823.tif 000006
【００７０】
但し、
ｓ：ラプラス変換の演算子
T(i) (i =1, 2,…, n）：T(1)＜T(2)＜……＜Ｔ(n)となるように設定された時定数
W(n+1)≡０
この第５発明によれば、ｎ個の周波数重みＷ(1)〜Ｗ(n)のうち、Ｗ(n)がローパス特性の周波数重み、その他の周波数重みＷ(1)〜Ｗ(n-1)がバンドパス特性の周波数重みとなる。この場合、Ｗ(1)〜Ｗ(n)は、その番数ｉ（ｉ＝１，２，…，ｎ）が小さいほど（“１”に近いほど）、通過可能な周波数領域がより高周波域の領域となる。【００７１】
そして、これらの周波数重みＷ(1)〜Ｗ(n)によって、時定数Ｔ(1)により規定されるカットオフ周波数以下の周波数域を間断なく分離できる。このため、時定数Ｔ(1)により規定されるカットオフ周波数以下の周波数域でのロボットの所望の制御特性を容易に実現できる。【００７２】
また、前記第２発明においては、前記個数m(i)がm(i)＞１である前記第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力τ(i,j)（ｊ＝１，２，…，m(i)）は、前記式０２の演算に用いるm(i)個の前記行列Nu(i,j)（ｊ＝１，２，…，m(i)）が、rank(Nu(i,1))≧rank(Nu(i,2))≧……≧rank(Nu(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するＭ個の状態量別制御入力の中から選定された状態量別制御入力であることが好ましい（第６発明）。【００７３】
同様に、前記第３発明においては、前記個数m(i)がm(i)＞１である前記第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力τ(i,j)（ｊ＝１，２，…，m(i)）は、前記式０２ａの演算に用いるm(i)個の前記行列Nuw(i,j)（ｊ＝１，２，…，m(i)）が、rank(Nuw(i,1))≧rank(Nuw(i,2))≧……≧rank(Nuw(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するＭ個の状態量別制御入力の中から選定された状態量別制御入力であることが好ましい（第７発明）。【００７４】
なお、rank(Ｎu(i,j))は行列Ｎu(i,j)の階数であり、rank(Ｎuw(i,j))は行列Ｎuw(i,j)の階数である。【００７５】
このようにした場合には、前記第６発明においては、m(i)＞１である前記第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力τ(i,j)（ｊ＝１，２，…，m(i)）が、rank(Nu(i,1))≧rank(Nu(i,2))≧……≧rank(Nu(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するＭ個の状態量別制御入力の中から選定された状態量別制御入力である。【００７６】
また、前記第７発明においては、m(i)＞１である前記第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)の組成成分として決定される状態量別制御入力τ(i,j)（ｊ＝１，２，…，m(i)）が、rank(Nuw(i,1))≧rank(Nuw(i,2))≧……≧rank(Nuw(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するＭ個の状態量別制御入力の中から選定された状態量別制御入力である。【００７７】
このため、第６発明及び第７発明にいずれにおいても、第ｉ周波数領域別制御入力Ｗ(i)＊τf(i)を組成する状態量別制御入力の個数ｍ(i)に関して、前記式０２ａ又は式０２ｂに従って相互に非干渉に合成することが可能な状態量別制御入力の個数ｍ(i)をできるだけ多くするようにすることができる。【００７８】
この結果、第ｉ周波数領域で制御し得る状態量の種類数（第ｉ周波数領域における制御対象として選定し得る状態量の種類数）を多くすることができる。ひいては、その第ｉ周波数領域でのロボットの動作を、種々様々な要求を満足し得るように制御することが可能となる。【００７９】
なお、前記第６発明は、前記第５発明と組み合わせてもよい。また、前記第７発明は、前記第４発明又は第５発明と組み合わせてもよい。
【図面の簡単な説明】
【００８０】
【図１】本発明の第１実施形態及び第２実施形態を適用するロボットの概略構成を模式的に示す図。
【図２】図１のロボットに備える関節駆動系の概略構成を模式的に示す図。
【図３】図１のロボットの制御装置の主要な機能を示すブロック図。
【図４】図３に示す目標関節駆動トルク決定部の、第１実施形態における機能を示すブロック図。
【図５】図５（ａ）〜（ｃ）は図４に示す接触部ヤコビアン決定部の処理を説明するための図。
【図６】図３に示す目標関節駆動トルク決定部の、第２実施形態における機能を示すブロック図。
【図７】図１のロボットの足平部の目標位置の軌道例を示すグラフ。
【図８】第２実施形態で使用する重み係数ｗ3の設定例を示すグラフ。
【発明を実施するための形態】
【００８１】
［第１実施形態］
以下、本発明の第１実施形態を図１〜図５を参照して説明する。
【００８２】
図１は、本実施形態で例示するロボットの概略構成を模式的に示している。このロボット１は、脚式移動ロボットとしての２足移動ロボットである。このロボット１を構成するリンクは、大別すると、上体リンク２と、左右一対の脚リンク３Ｒ，３Ｌと、左右一対の腕リンク４Ｒ，４Ｌと、頭部５とを有する。
【００８３】
なお、本明細書では、ロボット１の前方に向かって右側の部材又はその部材に関連する量を示す変数に符号“Ｒ”を付加し、ロボット１の前方に向かって左側の部材又はその部材に関連する量を示す変数に符号“Ｌ”を付加する。ただし、右側、左側を特に区別する必要が無いときは、符号“Ｒ”，“Ｌ”を省略することがある。
【００８４】
上体リンク２は、ロボット１の基体としての下側上体１０と、この下側上体１０の上方に配置された上側上体１１とから構成されている。上側上体１１は、ヨー軸周り（Ｚ軸周り）の回転自由度を有する上体回旋用関節１２を介して下側上体１０に連結されている。
【００８５】
左右一対の脚リンク３Ｒ，３Ｌは、互いに同じ構造のものである。具体的には、各脚リンク３は、下側上体１０に股関節部１３を介して連結された大腿部１４と、この大腿部１４に膝関節部１５を介して連結された下腿部１６と、この下腿部１６に足首関節部１７を介して連結された足平部１８とを、該脚リンク３を構成する複数のリンクとして備える。
【００８６】
この場合、股関節部１３は、ヨー軸周り、ピッチ軸周り（Ｙ軸周り）及びロール軸周り（Ｘ軸周り）の回転自由度をそれぞれ有する３つの関節１９，２０，２１により構成されている。また、膝関節部１５は、ピッチ軸まわりの回転自由度を有する関節２２により構成されている。また、足首関節部１７は、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する２つの関節２３，２４により構成されている。従って、各脚リンク３は、本実施形態では、下側上体１０に対して、６自由度の運動自由度を有する。なお、上記の説明における各脚リンク３の関節１９〜２４の回転軸（ロール軸、ピッチ軸、ヨー軸）は、脚リンク３を上下方向に延在させた状態での回転軸を示している。
【００８７】
左右一対の腕リンク４Ｒ，４Ｌは、互いに同じ構造のものである。具体的には、各腕リンク４は、上側上体１１に肩関節部２５を介して連結された上腕部２６と、この上腕部２６に肘関節部２７を介して連結された前腕部２８と、この前腕部２８に手首関節部２９を介して連結されたハンド３０とを、該腕リンク４を構成する複数のリンクとして備える。
【００８８】
この場合、肩関節部２５は、ピッチ軸周り、ロール軸周り、及びヨー軸周りの回転自由度をそれぞれ有する３つの関節３１，３２，３３により構成されている。また、肘関節部２７は、ピッチ軸周りの回転自由度を有する関節３４により構成されている。また、手首関節部２９は、ヨー軸周り、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する３つの関節３５，３６，３７により構成されている。従って、各腕リンク４は、本実施形態では、上側上体１１に対して、７自由度の運動自由度を有する。なお、上記の説明における各腕リンク４の関節３１〜３７の回転軸（ロール軸、ピッチ軸、ヨー軸）は、腕リンク４を上下方向に延在させた状態での回転軸を示している。
【００８９】
頭部５は、上側上体６の上方に配置されており、頸関節部３８を介して上側上体６に連結されている。この場合、頸関節部３８は、ピッチ軸周り及びヨー軸周りの回転自由度をそれぞれ有する２つの関節３９，４０により構成されている。従って、頭部５は、上側上体６に対して、２自由度の運動自由度を有する。なお、関節３１，３７の上記の回転軸（ピッチ軸、ヨー軸）は、頸関節部３８を上下方向に伸ばした状態での回転軸を示している。
【００９０】
以上の構成を有するロボット１は、各脚リンク３の６個の関節１９〜２４を駆動することによって、各脚リンク３の空間的な運動が行なわれ、この運動によって床上を移動することが可能となっている。例えば、人の歩行動作あるいは走行動作と同様の形態（歩容）で脚リンク３Ｒ，３Ｌを運動させることによって、ロボット１の歩行動作あるいは走行動作が行なうことが可能である。この場合、ロボット１の各リンクは、その空間的な位置及び姿勢（空間的な向き）に関して６自由度を有する。
【００９１】
また、各腕リンク４の７個の関節３１〜３７を駆動することによって、各腕リンク４の空間的な運動が行なわれ、この運動によって各腕リンク４のハンド３０を、適宜の物体に接触させる等の作業を行なうことが可能となっている。
【００９２】
また、本実施形態では、上体リンク２を構成する下側上体１０と、上側上体１１とが関節１２を介して連結されているので、この関節１２を駆動することによって、上体リンク２を捻ることが可能となっている。
【００９３】
また、頸関節部３８の関節３９，４０を駆動することによって、頭部５のチルト動作（ピッチ軸周りの回動動作）あるいはパン動作（ヨー軸周りの回動動作）を行なうことが可能となっている。
【００９４】
補足すると、本実施形態のロボット１は、腕リンク４Ｒ，４Ｌ及び頭部５を有する人型のロボットであるが、これらの腕リンク４Ｒ，４Ｌ、あるいは、頭部５を備えないロボットであってもよい。また、上体リンク２の関節１２を省略し、下側上体１０及び上側上体１１を一体に構成してもよい。また、ロボット１は、回転型の関節に限らず、直動型の関節を備えていてもよい。
【００９５】
図１での具体的な図示は省略するが、ロボット１には、上記の各関節を回転駆動するための動力を出力するアクチュエータとしての電動モータを含む関節駆動系（関節の駆動機構）が備えられている。そして、本実施形態では、各関節に柔軟性を持たせるために、各関節と電動モータとの間の動力伝達系には、以下に説明する如く、電動モータの動力を弾性力に変換して関節の被動部材に付与するばね部材が介装されている。
【００９６】
図２は、各関節毎の上記関節駆動系の概略構成を模式的に示している。この関節駆動系は、電動モータ５０と、関節の被動部材５１（以降、関節被動部５１という）との間の動力伝達要素として、波動歯車装置等から構成された減速機５２とばね部材であるトーションバー５３とを備える。そして、電動モータ５０の出力軸５０ａが減速機５２とトーションバー５３とを順に介して関節被動部５１に連結されている。従って、この関節駆動系は、電動モータ５０が出力する動力（トルク）を、減速機５２とトーションバー５３とを順に介して関節被動部５１に伝達するように構成されている。この場合、電動モータ５０が出力する動力（トルク）は、トーションバー５３で弾性力（捩り力）に変換され、この弾性力が関節被動部５１に駆動トルクとして付与される。
【００９７】
補足すると、ここで言うトーションバー５３（ばね部材）は、減速機５２と関節被動部５１との間に付加された弾性体、又は電動モータ５０と関節被動部５１との間の動力伝達系に元々存在する弾性体を総称的に表現するものである。従って、減速機５２が、ハーモニックドライブ（登録商標）等の波動歯車装置のように、その出力段に元々、弾性体を有するものである場合には、その弾性体もトーションバー５３（ばね部材）に含まれる。
【００９８】
なお、関節被動部５１は、これを有する関節によって互いに相対回転自在に連結された一対の部材のうちの一方である。例えば、前記膝関節部１５の関節２２を駆動する関節駆動系にあっては、大腿部１４及び下腿部１６のうちの一方のリンクが関節被動部５１となり、他方のリンクに電動モータ５０及び減速機５２のハウジングが固定される。
【００９９】
以上のように本実施形態では、関節駆動系にばね部材としてのトーションバー５３を備えることによって、各関節の柔軟な動作が可能となっている。
【０１００】
本実施形態では、以上の構造を有するロボット１の動作制御を行なうために、ＣＰＵ、ＲＡＭ、ＲＯＭ等を含む電子回路ユニットにより構成された制御装置６０と、各種のセンサとが備えられている。
【０１０１】
この場合、センサとしては、図１に示すように、ロボット１の基体である下側上体１１の角速度や姿勢角（鉛直方向に対する傾斜角度やヨー軸周りの方位角）等を計測するために該下側上体１１に搭載された姿勢センサ６１と、各足平部１８がその接地時に受ける外力（床反力）を計測するために各脚リンク３の足首関節部２４と足平部１８との間に介装された力センサ６２（６２Ｒ，６２Ｌ）と、各ハンド３０がその接触対象物体から受ける外力を計測するために各腕リンク４の手首関節部２９とハンド３０との間に介装された力センサ６３（６３Ｒ，６３Ｌ）とが備えられている。
【０１０２】
上記姿勢センサ６１は、例えば３軸周りの角速度を検出するジャイロセンサと３軸方向の加速度を検出する加速度センサとから構成される。また、各力センサ６２，６３は、それぞれ、例えば３軸方向の並進力及び３軸周りのモーメントを検出する６軸力センサにより構成される。なお、姿勢センサ６１を構成する加速度センサは、下側上体１１の移動速度を計測するためにも使用される。
【０１０３】
また、本実施形態では、図２に示すように、各関節毎の関節駆動系には、電動モータ５０の出力軸５０ａの回転角度（ロータの回転角度）を検出する角度センサとしてのロータリーエンコーダ６４と、トーションバー５３の両端部間の回転角度差を検出する角度センサとしての差分エンコーダ６５とが備えられている。
【０１０４】
なお、差分エンコーダ６５は、本実施形態では、トーションバー５３の両端部に各々固定された一対の円板６５ａ，６５ｂを有し、これらの円板６５ａ，６５ｂの一方に対する他方の相対的な回転角度に応じた信号をトーションバー５３の両端部間の回転角度差に応じた信号として出力するセンサである。
【０１０５】
これらのロータリエンコーダ６４及び差分エンコーダ６５は、電動モータ５０の出力軸５０ａの回転角度（又はトーションバー５３の減速機５２側の一端部の回転角度（＝減速機５２の出力軸の回転角度））とその時間的変化率（回転角速度）、関節被動部５１の回転角度とその時間的変化率（回転角速度）、トーションバー５３の両端部間の回転角度差（＝トーションバー５３の捩れ角）等を計測するために使用されるセンサである。
【０１０６】
なお、差分エンコーダ６５の代わりに、関節被動部５１の回転角度を検出するロータリーエンコーダ等の角度センサを備えるようにしてもよい。また、ロータリエンコーダ６４の代わりに、減速機５２の出力軸の回転角度を検出するロータリエンコーダ等の角度センサを備えるようにしてもよい。
【０１０７】
また、以降の説明では、ロボット１の各関節の関節被動部５１の回転角度を、関節の回転角度又は関節角ということがある。
【０１０８】
制御装置６０には、上記の各センサの検出出力（あるいは、該検出出力を適宜のフィルタに通してなるフィルタリング値）が入力される。そして、制御装置６０は、これらの入力値から認識される所要の計測値（下側上体１０の姿勢角やその時間的変化率（角速度）の計測値、下側上体１０の移動速度の計測値、各足平部１８やハンド３０に作用する外力の計測値、各関節の回転角度やその時間的変化率（角速度）の計測値など）を使用して、ロボット１に目標とする運動を行なわせるために各関節の関節被動部５１に付与すべき駆動トルクの目標値（以降、目標関節駆動トルクという）を決定すると共に、この目標関節駆動トルクに応じて各関節駆動系の電動モータ５０の出力トルクを制御する処理を実行する。
【０１０９】
以下に制御装置６０の制御処理の詳細を説明する。なお、本実施形態では、ロボット１の目標とする運動が、床上を歩行動作により移動する運動である場合の制御処理を例にとって説明する。
【０１１０】
図３に示すように、制御装置６０は、実装されたプログラム等により実現される主要な機能部として、歩容生成部７１、目標関節駆動トルク決定部７２及び関節駆動制御部７３を備える。
【０１１１】
歩容生成部７１は、ロボット１の目標運動を少なくとも含む目標歩容を生成する処理を実行する機能部である。この歩容生成部７１が生成する目標運動は、ロボット１の各関節の変位量（回転角度）の目標値である目標関節角の時系列と、ロボット１の全体重心点の目標位置（以降、目標全体重心点ということがある）の時系列とを含む。
【０１１２】
上記目標運動は、例えば、本願出願人が前記特許文献１等で提案した歩容生成手法を用いて生成される。
【０１１３】
この歩容生成手法を概略的に説明すると、ロボット１の動作環境の床形状情報やロボット１の移動方向の要求等に基づいて、ロボット１の両足平部１８Ｒ，１８Ｌのそれぞれの目標とする着地予定位置及び着地予定姿勢と着地予定時刻とが決定される。
【０１１４】
そして、これらの着地予定位置及び姿勢と、着地予定時刻とに応じて、各足平部１８の目標位置及び目標姿勢の軌道（時系列パターン）である足平位置姿勢軌道が決定される。さらに、この足平位置姿勢軌道に基づいて、ＺＭＰ（Zero Moment Point）の目標位置（以降、目標ＺＭＰということがある）の軌道である目標ＺＭＰ軌道とが決定される。
【０１１５】
さらに、ロボット１の全体の運動によって発生する慣性力と、ロボット１に作用する重力との合力によって目標ＺＭＰ周りに発生するモーメントの水平成分が“０”になるという動力学的な平衡条件を満たしつつ、ロボット１の将来的な安定性を確保し得るように、ロボット１の両脚リンク３Ｒ，３Ｌ以外の各リンク（上体リンク２等）の目標位置及び目標姿勢の軌道が決定される。
【０１１６】
そして、これらの目標位置及び目標姿勢から、ロボット１の幾何学モデル（剛体リンクモデル）を用いて、各関節の目標関節角が決定される。
【０１１７】
なお、目標歩容は、ロボット１の実際の動作時にリアルタイムで生成されるものでなくてもよく、事前に（オフラインで）作成した目標歩容を、制御装置６０に外部から無線通信等により入力したり、あるいは、ロボット１の図示しない記憶装置に事前に記憶保持しておくようにしてもよい。この場合には、歩容生成部７１は不要である。
【０１１８】
また、目標歩容には、ロボット１の目標運動以外に、足平部１８Ｒ，１８Ｌの一方又は両方に作用する全床反力（トータルの床反力）の特定成分（例えば並進力の鉛直方向成分）の目標値や、全床反力の作用点の目標位置（すなわち目標ＺＭＰ）等が含まれていてもよい。本実施形態では、歩容生成部７１が出力する目標歩容には、目標ＺＭＰが含まれる。
【０１１９】
目標関節駆動トルク決定部７２は、歩容生成部７１が生成した上記目標運動を実現する（ロボット１の実際の運動を目標運動に追従させる）ための各関節の目標関節駆動トルクを決定する処理を逐次実行する機能部である。この目標関節駆動トルク決定部７２は、その詳細は後述するが、本発明に係わる中核的な処理を実行する機能部である。
【０１２０】
関節駆動制御部７３は、目標関節駆動トルク決定部７２が逐次決定した各関節の目標関節駆動トルク（詳しくは、後述の補正後目標関節駆動トルク↑τtc_cmdにより規定される各関節の目標関節駆動トルク）を実現するように（各関節の関節被動部５１に実際に付与される駆動トルクを目標関節駆動トルクに追従させるように）、各関節駆動系の電動モータ５０の出力トルクを制御する処理を逐次実行する機能部である。
【０１２１】
この関節駆動制御部７３は、例えば、次のように電動モータ５０の出力トルクを制御する。すなわち、関節駆動制御部７３は、前記トーションバー５３の両端部間の回転角度差（トーションバー５３の捩れ角）の計測値から、トーションバー５３が実際に発生している弾性力トルク（トーションバー５３から関節被動部５１に実際に付与される駆動トルク）を逐次推定する。さらに、関節駆動制御部７３は、この弾性力トルクの推定値と、目標関節駆動トルクとの偏差を“０”に収束させるように、電動モータ５０の出力軸５０ａの目標回転角速度を逐次設定する。
【０１２２】
次いで、関節駆動制御部７３は、この目標回転角速度と、電動モータ５０の出力軸５０ａの回転角速度の計測値との偏差を“０”に収束させるように、電動モータ５０の目標出力トルクを決定する。そして、関節駆動制御部７３は、この目標出力トルクを電動モータ５０の通電電流の目標値に変換し、この目標値に、実際の通電電流をフィードバック制御することで、電動モータ５０の実際の出力トルクを目標出力トルクに制御する。
【０１２３】
なお、関節駆動制御部７３の制御処理においては、トーションバー５３の振動の影響を補償したり、関節被動部５１に作用する外力の影響を補償しつつ、電動モータ５０の出力トルクを制御するようにしてもよい。
【０１２４】
次に、説明を後回しにした、前記目標関節駆動トルク決定部７２の処理の詳細を説明する。
【０１２５】
なお、以降の説明では、ベクトル（複数の成分の列）を表記する場合に、例えば“↑Ａ”というように、変数名の先頭に“↑”を付する。また、ベクトルは、特にことわらない限り、成分を縦に並べた縦ベクトル（列ベクトル）であるとする。また、本明細書では、乗算記号として「＊」を用いる。
【０１２６】
図４に示すように、目標関節駆動トルク決定部７２は、その主要な機能として、基本パラメータ群決定部８１、接触部位ヤコビアン決定部８２、状態量ヤコビアン決定部８３、状態量目標値決定部８４、状態量別制御入力決定部８５、合成制御入力決定部８６、コンプライアンス制御部８７、及び接触部弾性変形補償部８８を備える。なお、図４では、各機能部の主要な出力と、その入力先とを図示している。
【０１２７】
目標関節駆動トルク決定部７２は、これらの機能部の処理を所定の演算処理周期で逐次実行することによって、各関節の目標関節駆動トルクを逐次決定する。
【０１２８】
以下説明すると、目標関節駆動トルク決定部７２は、まず、基本パラメータ群決定部８１の処理と、接触部位ヤコビアン決定部８２の処理と、状態量ヤコビアン決定部８３の処理と、状態量目標値決定部８４の処理とを実行する。
【０１２９】
基本パラメータ群決定部８１は、ロボット１の運動と力との間の関係（運動方程式）を表現するモデルであるロボット運動モデルのパラメータを算出する処理を逐次実行する機能部である。
【０１３０】
ここで、ロボット運動モデルは、ロボット１に外界から作用する外力（床反力等）が“０”であるとした場合、次式１−１により表現される。
【０１３１】
Ｍ＊↑ｑ''＋↑Ｎ＋↑Ｇ＝↑τ    ……式１−１
この式１−１における↑ｑ''は、ロボット１の一般化変数ベクトル↑ｑの２階微分値（＝d2↑ｑ/dt2）、Ｍはイナーシャ行列、↑Ｎは遠心力・コリオリ力依存一般化力ベクトル、↑Ｇは重力依存一般化力ベクトル、↑τは駆動力依存一般化力ベクトルである。
【０１３２】
上記一般化変数ベクトル↑ｑは、本実施形態では、ロボット１のあらかじめ定めた１つの代表リンクの位置及び姿勢（グローバル座標系で見た位置及び姿勢）と、各関節の変位量（回転角度）とを成分として、これらの成分を並べて構成されるベクトルである。上記代表リンクは、本実施形態では、ロボット１の基体としての下側上体１０である。以降、下側上体１０を代表リンク１０ということもある。
【０１３３】
ここで、本実施形態の説明では、ロボット１の代表リンク１０を含めて、各リンクの「位置」は、より詳しくは、該リンクに対して固定的に設定された代表点のグローバル座標系での位置を意味する。また、各リンクの「姿勢」は、より詳しくは、グローバル座標系で見た該リンクの空間的な向き（角度の次元で表現される向き）を意味する（以降の説明では、「姿勢」を「姿勢角」ということもある）。
【０１３４】
なお、上記グローバル座標系は、ロボット１の移動環境の床（地面を含む）に対して固定された座標系等、慣性座標系（近似的に慣性系と見なせる座標系を含む）である。
【０１３５】
そして、各リンクの「位置」及び「姿勢」（姿勢角）は、本実施形態ではそれぞれ３自由度を有するので、それぞれ３つの座標成分により表現される。このため、本実施形態での一般化変数ベクトル↑ｑの成分個数（すなわち↑ｑの自由度数）は、代表リンク１０の位置及び姿勢のトータルの自由度数（＝６）と、ロボット１の全ての関節のトータルの自由度数との総和の個数である。なお、図１に示した本実施形態のロボット１では、全ての関節のトータルの自由度数は“２９”である。従って、↑ｑの自由度数は、“３５”である。
【０１３６】
上記イナーシャ行列Ｍは、一般化変数ベクトル↑ｑの２階微分値↑ｑ''を、一般化力ベクトルに変換する行列（対称行列）、すなわち、↑ｑ''に乗じることによって得られるベクトル（＝Ｍ＊↑ｑ''）の各成分値が、↑ｑ''の各成分に対応する力（並進力又はモーメント）の次元を有する値となる行列である。
【０１３７】
ここで、↑ｑの各成分は、位置又は姿勢角又は回転角度であるから、↑ｑ''の各成分は、並進加速度又は角加速度の次元を有する。従って、Ｍ＊↑ｑ''の符号を反転させてなるベクトル（−Ｍ＊↑ｑ''）の各成分値が、ロボット１の運動によって発生する慣性力を意味する。この場合、ベクトル（−Ｍ＊↑ｑ''）のうち、代表リンク１０の「位置」の２階微分値（並進加速度）に対応する成分の慣性力は並進慣性力（並進力となる慣性力）であり、代表リンク１０の「姿勢」の２階微分値（角加速度）に対応する成分の慣性力と、各関節の回転角度の２階微分値（角加速度）に対応する成分の慣性力とは、それぞれ慣性力モーメント（モーメントとなる慣性力）である。
【０１３８】
なお、イナーシャ行列Ｍの逆行列Ｍ-1は対角行列となる。
【０１３９】
一般化力ベクトルは、その各成分が、一般化変数ベクトル↑ｑの各成分に対応するベクトル（↑ｑと同数の成分からなるベクトル）である。この一般化力ベクトルは、より詳しくは、その各成分が、それに対応する↑ｑの成分値の変化（加速度）を発生させる力（並進力又はモーメント）となるベクトルである。この場合、一般化力ベクトルの各成分は、それに対応する↑ｑの成分の２階微分値に応じた慣性力と同じ種類の力である。
【０１４０】
遠心力・コリオリ力依存一般化力ベクトル↑Ｎは、ロボット１の各リンクに作用する遠心力及びコリオリ力により発生する一般化力ベクトルである。また、重力依存一般化ベクトル↑Ｇは、ロボット１の各リンクに作用する重力により発生する一般化力ベクトルである。なお、以降の説明では、↑Ｎと↑Ｇとの和（＝↑Ｎ＋↑Ｇ）を非線形項一般化力ベクトルということがある。
【０１４１】
駆動力依存一般化力ベクトル↑τは、ロボット１の各関節を駆動するアクチュエータ（本実施形態では電動モータ５０）の駆動力によって発生する一般化力ベクトルである。本実施形態のロボット１では、この駆動力依存一般化力ベクトル↑τのうち、代表リンク１０の位置及び姿勢に対応する成分の値は常に“０”である。また、↑τのうち、各関節の回転角度に対応する成分の値が該関節に付与する駆動トルク（以下、関節駆動トルクということがある）を意味するものとなる。
【０１４２】
基本パラメータ群決定部８１が算出するパラメータは、式１−１におけるイナーシャ行列Ｍと、遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））とから成る。
【０１４３】
補足すると、ロボット１に床反力等の外力（≠０）が作用する場合には、ロボット運動モデルは、その外力に依存する一般化力ベクトルの項（外力項）を式１−１に付加した式（後述の式５−１）によって表現される。ただし、基本パラメータ群決定部８１が算出するパラメータである上記Ｍ、↑Ｎ、↑Ｇは、該外力には依存せずに、ロボット１の各関節の回転角度やその時間的変化率（角速度）等のロボット１の運動状態に依存してその値が定まるパラメータである。このため、基本パラメータ群決定部８１を説明するための上記式１−１では外力項を省略している。
【０１４４】
上記のパラメータＭ、↑Ｎ、↑Ｇ（あるいは、Ｍ及び（↑Ｎ＋↑Ｇ））を算出するために、基本パラメータ群決定部８１には、代表リンク（下側上体）１０の移動速度の計測値と、代表リンク１０の姿勢角の計測値及びその時間的変化率（角速度）の計測値と、各関節の回転角度の計測値及びその時間的変化率（角速度）の計測値とが逐次入力される。これらの計測値は、前記姿勢センサ６１、ロータリーエンコーダ６４、差分エンコーダ６５の検出出力を基に、制御装置６０において認識された計測値である。
【０１４５】
そして、基本パラメータ群決定部８１は、これらの入力された計測値を用いて、ＲＮＥＡ（Recursive Newton Euler Algorithm）、単位ベクトル法等の公知の手法によりＭ、↑Ｎ、↑Ｇ（あるいは、Ｍ及び（↑Ｎ＋↑Ｇ））を逐次算出する。
【０１４６】
補足すると、↑Ｎの各成分値の大きさが、それに対応する重力依存一般化力ベクトル↑Ｇの成分値の大きさに比して十分に微小なものとなるか、もしくは“０”となり、ひいては、↑Ｎ＋↑Ｇ≒↑Ｇとなる場合もある。このような場合には、↑Ｎ、↑Ｇのうちの↑Ｎを無視し（非線形項一般化ベクトル（↑Ｎ＋↑Ｇ）が↑Ｇに一致するものとして）、↑Ｇだけを算出するようにしてもよい。
【０１４７】
前記接触部位ヤコビアン決定部８２は、ロボット１の外界との接触部位の代表点（以下、接触部位代表点という）の運動速度と、一般化変数ベクトル↑ｑの１階微分値（時間的変化率）との間の関係を表現するヤコビアン（以降、接触部位ヤコビアンという）を逐次算出する処理を実行する機能部である。接触部位ヤコビアンは、換言すれば、一般化変数ベクトル↑ｑの各成分値の変化（微小変化）に対する接触部位代表点の空間的な位置の変化の割合い（感度）を表現する行列である。
【０１４８】
ここで、本実施形態で説明するロボット１の目標運動は、前記したようにロボット１の歩行動作を行なう運動である。この場合、ロボット１の外界との接触部位（接地部位）は、経時的に変化する。すなわち、歩行動作におけるロボット１の接地状態は、足平部１８Ｒ，１８Ｌの一方だけが接地部位となる片脚接地状態と、両方が接地部位となる両脚接地状態と、他方だけが接触部位となる片脚接地状態とが順番に繰り返される。このため、接触部位代表点の位置は、足平部１８Ｒ，１８Ｌの接地状態に応じて変化していくべきものである。
【０１４９】
そこで、接触部位ヤコビアン決定部８２は、ロボット１の接触部位代表点の位置（より正確には、該位置を規定するパラメータ（後述の比率値ｒat）の値）を足平部１８Ｒ，１８Ｌの接地状態に応じて逐次決定しつつ、接触部位ヤコビアンを逐次算出する。この場合、接触部位代表点の位置は、足平部１８Ｒ，１８Ｌの接地状態の切り替わりの際等に不連続に変化することが無いように（常時、連続的に変化していくように）規定される。
【０１５０】
以下、接触部位ヤコビアン決定部８２の処理を具体的に説明する。
【０１５１】
本実施形態では、接触部位代表点は、複数の点から構成されると共に、その個数は、足平部１８Ｒ，１８Ｌのいずれの接地状態においても一定に維持される。具体的には、接触部位代表点は、図５（ａ）〜（ｃ）に黒丸点で示すように、足平部１８Ｒ，１８Ｌのいずれの接地状態においても、４つの点a1，a2，a3，a4から構成される。なお、図５（ａ）〜（ｃ）では、足平部１８Ｒ，１８Ｌのうちの接地している足平部を実線で示し、離床している（空中を移動している）足平部を２点鎖線で示している。
【０１５２】
この場合、４つの接触部位代表点a1〜a4の位置は、足平部１８Ｒ，１８Ｌの接地状態に応じた位置に規定される。
【０１５３】
具体的には、左側足平部１８Ｌが接地し、且つ右側足平部１８Ｒが離床している片脚接地状態（左脚接地状態）においては、図５（ａ）に示す如く、左側足平部１８Ｌの底面の所定の位置（左側足平部１８Ｌに対して固定された位置）にあらかじめ設定された４つの所定点a1L，a2L，a3L，a4Lが、それぞれ接触部位代表点a1，a2，a3，a4として決定される。
【０１５４】
その４つの所定点a1L〜a4Lの位置は、本実施形態では、左側足平部１８Ｌの底面の４隅の位置である。より詳しくは、所定点a1L〜a4Lの位置は、それぞれ順番に、左側足平部１８Ｌの底面の左前側の隅部、右前側の隅部、左後側の隅部、右後側の隅部の位置である。
【０１５５】
また、右側足平部１８Ｒが接地し、且つ左側脚リンク３Ｌの足平部１８Ｌが離床している片脚接地状態（右脚接地状態）においては、図５（ｃ）に示す如く、右側足平部１８Ｒの底面の所定の位置（右側足平部１８Ｒに対して固定された位置）にあらかじめ設定された４つの所定点a1R，a2R，a3R，a4Rが、それぞれ接触部位代表点a1，a2，a3，a4として決定される。
【０１５６】
その４つの所定点a1R〜a4Rの位置は、左側足平部１８Ｌの４つの所定点a1L〜a4Lと同様に、右側足平部１８Ｒの底面の４隅の位置（左前側の隅部、右前側の隅部、左後側の隅部、右後側の隅部の位置）である。
【０１５７】
なお、右側足平部１８Ｒの４つの所定点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）    ……式２−２ｂ
【０１６５】
なお、式２−２ａ又は２−２ｂにおいて、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のそれぞれの位置が連続的に変化していくように決定されることとなる。
【０１７１】
さらに、接触部位ヤコビン決定部８２は、上記のように決定した接触部位代表点規定比率ｒatを用いて接触部位ヤコビアンを決定する。
【０１７２】
この場合、本実施形態では、接触部位代表点a1〜a4が４つの点から構成されるので、接触部位ヤコビアンは、接触部位代表点a1〜a4のそれぞれの位置を成分として並べて構成されるベクトル↑Ｘｃ（以降、接触部位代表点位置ベクトル↑Ｘｃという）の時間的変化率（１階微分値）↑Ｘc'（＝d↑Ｘc'/dt）、すなわち、接触部位代表点a1〜a4のそれぞれの運動速度を成分として構成されるベクトル↑Ｘc'と、前記一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を次式２−３により表現する行列Ｊcである。
【０１７３】
↑Ｘc'＝Ｊ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を算出する。
【０１９８】
また、上記第２種類状態量に対応する状態量ヤコビアン（以降、第２状態量ヤコビアンという）は、ロボット１の各関節の回転角速度を成分として並べて構成されるベクトル↑Ｓ2（以降、第２種類状態量ベクトル↑Ｓ2という）と、前記一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を次式３−２により表現する行列Ｊs2である。
【０１９９】
↑Ｓ2＝Ｊs2＊↑ｑ'      ……式３−２
【０２００】
この場合、↑ｑ'は、各関節の回転角度の一階微分値、すなわち、回転角速度を成分として含むベクトルであるので、上記第２種類状態量ベクトル↑Ｓ2の各成分は、それぞれに対応する↑ｑ'の成分に一致する。従って、本実施形態では、第２状態量ヤコビアンＪs2は、その各成分値が固定値（一定値）となるヤコビアンである。
【０２０１】
このため、状態量ヤコビアン決定部８３は、第２状態量ヤコビアンＪs2を算出する処理は実質的には行わず、制御装置６０であらかじめ記憶保持された第２状態量ヤコビアンＪs2を出力する。
【０２０２】
前記状態量目標値決定部８４は、前記第１種類状態量及び第２種類状態量に関する目標値を決定する処理を実行する機能部である。
【０２０３】
本実施形態では、第１種類状態量に関する目標値は、前記第１種類状態量ベクトル↑Ｓ1の時間的変化率（１階微分値）↑Ｓ1'（＝d↑Ｓ1/dt）の目標値↑Ｓ1'_cmdである。また、第２種類状態量に関する目標値は、前記第２種類状態量ベクトル↑Ｓ2の時間的変化率（１階微分値）↑Ｓ2'（＝d↑Ｓ2/dt）の目標値↑Ｓ2'_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'の目標値は、ロボット１の全体の運動によって発生するトータルのモーメント慣性力（ただし、ヨー軸周りの成分を除く）の目標値に相当するものとなる。
【０２０６】
また、↑Ｓ2'の目標値↑Ｓ2'_cmd（以下、第２種類状態量目標値ベクトル↑Ｓ2'_cmdという）は、ロボット１の各関節の回転角加速度の目標値である。
【０２０７】
そして、本実施形態では、状態量目標値決定部８４は、上記第１種類状態量目標値ベクトル↑Ｓ1'_cmdと第２種類状態量目標値ベクトル↑Ｓ2'_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を算出するようにしてもよい。
【０２２９】
次に、第２種類状態量目標値ベクトル↑Ｓ2'cmdの算出処理に関しては、状態量目標値決定部８４は、ロボット１の各関節の回転角度（以降、関節角ということがある）の所要の目標値θcmdと、該関節の実際の回転角度θactの計測値との偏差Δθ（＝θcmd−θact）と、その１階微分値Δθ'（＝dΔθ/dt）とを算出する。この場合、各関節角の目標値θcmdは、制御装置６０の前回の演算処理周期において、歩容生成部７１で生成された目標関節角を、詳細を後述する接触部弾性変形補償部８８によって補正してなる値（前回値）である。
【０２３０】
さらに、状態量目標値決定部８４は、各関節の上記偏差Δθを“０”に収束させるためのフィードバック操作量を所定のフィードバック制御則により算出し、その算出したフィードバック操作量を成分とするベクトル（各関節毎のフィードバック操作量を並べたベクトル）を第２種類状態量目標値ベクトル↑Ｓ2'_cmdとして決定する。
【０２３１】
この場合、具体的には、状態量目標値決定部８４は、上記偏差ΔθをＰＤ則（比例・微分則）によって“０”に収束させるように↑Ｓ2'_cmdを決定する。すなわち、状態量目標値決定部８４は、各関節毎の上記偏差Δθを並べたベクトル↑ΔΘと、上記偏差Δθの１階微分値Δθ'を並べたベクトル↑ΔΘ'（すなわち↑ΔΘの１階微分値）とから、次式４−４により↑Ｓ2'_cmdを決定する。
【０２３２】
↑Ｓ2'cmd＝Kp3＊↑ΔΘ＋Kv3＊↑ΔΘ'      ……式４−４
【０２３３】
なお、式４−４におけるKp3，Kv3は、それぞれあらかじめ設定された所定値の比例ゲイン、微分ゲインである。これらのゲインKp3，Kv3は対角行列（↑Ｓ2'_cmdと同じ次数の対角行列）によって表現される。
【０２３４】
以上、説明した処理によって、第２種類状態量目標値ベクトル↑Ｓ2'_cmdが決定されることとなる。この場合、本実施形態では、↑Ｓ2'_cmdは、ロボット１の各関節の実際の関節角θactを所要の目標値θcmdに収束させるように決定される。
【０２３５】
以上のように、基本パラメータ群決定部８１、接触部位ヤコビアン決定部８２、状態量ヤコビアン決定部８３及び状態量目標値決定部８４の処理を実行した後、目標関節駆動トルク決定部７２は、次に、状態量別制御入力決定部８５の処理を実行する。
【０２３６】
この状態量別制御入力決定部８５は、各種類の状態量に関する目標値（前記第１種類状態量目標値ベクトル↑Ｓ1'_cmd及び第２種類状態量目標値ベクトル↑Ｓ2'_cmd）を実現するために必要なロボット１の各関節の目標関節駆動トルクを各種類の状態量毎に決定する処理を逐次実行する機能部である。以降、前記第１種類状態量に対応する各関節の目標関節駆動トルクを成分として並べたベクトルを第１目標関節駆動トルク↑τ1_cmd、前記第２種類状態量に対応する各関節の目標関節駆動トルクを成分として並べたベクトルを第２目標関節駆動トルク↑τ2_cmdと表記する。
【０２３７】
なお、本実施形態の説明では、↑τ1_cmd，↑τ2_cmdは、前記駆動力依存一般化力ベクトル↑τと同じ次数（一般化変数ベクトル↑ｑと同じ次数）のベクトルであるとする。この場合、↑τ1_cmd，↑τ2_cmdのそれぞれの成分のうち、ロボット１の代表リンク（下側上体１０）の位置及び姿勢に対応する成分は、いずれも“０”である。
【０２３８】
ここで、状態量別制御入力決定部８５の処理に使用する基本式を以下に説明しておく。
【０２３９】
ロボット１が外界との接触部位に外力を受ける場合、ロボット１の運動と力との関係（運動方程式）を表現するロボット運動モデルは、外力が“０”である場合のロボット運動ロボットモデルを表す前記式１−１に、外力の影響分を付加してなる次式５−１により表される。
【０２４０】
Ｍ＊↑ｑ''＋↑Ｎ＋↑Ｇ＋ＪcT＊↑Ｆc＝↑τ    ……式５−１
【０２４１】
この式５−１における↑Ｆcは、ロボット１の外界との接触部位（本実施形態では足平部１８Ｒ，１８Ｌ）に作用する外力であり、ＪcTは前記接触部位ヤコビアンＪcの転置行列である。
【０２４２】
この式５−１は、以下に説明する如く、ロボット１が外力を受ける接触部位の運動を規定する（該運動を既定の運動に一致させるものとする）ことによって、後述の式５−１１に変形することができる。
【０２４３】
以下説明すると、式５−１の両辺に、行列Ｊc＊Ｍ-1を乗じることによって、次式５−２が得られる。
【０２４４】
Ｊc＊↑ｑ''＋Ｊc＊Ｍ-1＊（↑Ｎ＋↑Ｇ）＋Ｊc＊Ｍ-1＊ＪcT＊↑Ｆc
＝Ｊc＊Ｍ-1＊↑τ
……式５−２
【０２４５】
一方、前記接触部位代表点a1〜a4の運動速度（＝接触部位代表点位置ベクトル↑Ｘcの１階微分値↑Ｘc'）と、一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を表現する前記式２−３の両辺を１階微分した後、式変形を行なうことで、次式５−３が得られる。
【０２４６】
Ｊc＊↑ｑ''＝↑Ｘc''−Ｊc'＊↑ｑ'      ……式５−３
【０２４７】
この式５−３におけるＪc'は前記接触部位ヤコビアンＪcの１階微分値（＝dＪc/dt）、↑ｑ''は↑ｑの２階微分値（＝d2↑ｑ/dt2）である。
【０２４８】
さらに、この式５−３を式５−２に代入することで、次式５−４が得られる。
【０２４９】
↑Ｘc''−Ｊc'＊↑ｑ'＋Ｊc＊Ｍ-1＊（↑Ｎ＋↑Ｇ）
＋Ｊc＊Ｍ-1＊ＪcT＊↑Ｆc＝Ｊc＊Ｍ-1＊↑τ
……式５−４
【０２５０】
ここで、ある重み行列Ｗat（対角行列）を用いて次式５−５，５−６によりそれぞれ定義される行列Ｊc#と行列Ｒcとを導入する。
【０２５１】
Ｊc#≡ＷatT＊ＪcT＊ＲcT      ……式５−５
Ｒc≡((Ｊc＊Ｗat＊ＪcT)-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＊ＪcT＊ＲcT      ……式５−８
Ｒc≡((Ｊc＊Ｍ-1＊ＪcT)-1)T      ……式５−９
【０２５７】
なお、Ｍ-1は対角行列であるので、(Ｍ-1)T＝M-1である。
【０２５８】
ここで、前記接触部位代表点a1〜a4の運動を規定し、前記接触部位代表点位置ベクトル↑Ｘcの２階微分値↑Ｘc''（＝d2↑Ｘc/dt2）をある既定の目標値↑Ｃに一致させる場合（接触部位代表点a1〜a4の運動加速度を、それぞれ、ある既定の目標値に一致させる場合）を想定する。すなわち、↑Ｘc''＝↑Ｃとなるように、接触部位である足平部１８Ｒ，１８Ｌの運動が規定される場合を想定する。
【０２５９】
この場合、上記式５−７から、次式５−１０が得られる。
【０２６０】
↑Ｆc＝（Ｊc#）T＊（↑τ−↑Ｎ−↑Ｇ）−Ｒc＊↑Ｃ−Ｒc＊Ｊc'＊↑ｑ'
……式５−１０
【０２６１】
この式５−１０を前記式５−１に代入し、さらに、次式５−１２，５−１３，５−１４，５−１５によりそれぞれ定義される行列Ｔc，ベクトル↑Ｃc，行列Ｐc及び一般化力ベクトル↑τcmpnとを用いて整理することによって、次式５−１１が得られる。
【０２６２】
Ｍ＊↑ｑ''＋Ｔc＊↑ｑ'＝Ｐc＊（↑τ−↑τcmpn）      ……式５−１１
但し、
Ｔc≡−ＪcT＊Ｒc＊Ｊc'      ……式５−１２
↑Ｃc≡−ＪcT＊Ｒc＊↑Ｃ      …式５１−１３
Ｐc≡Ｉ−ＪcT＊（Ｊc#）T      ……式５−１４
↑τcmpn≡（↑Ｎ＋↑Ｇ）−Ｐc-1＊Ｃc      ……式５−１５
【０２６３】
上記式５−１１は、接触部位代表点a1〜a4の運動を規定した場合のロボット１の運動方程式を、外力↑Ｆ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＝ＡsT＊(ｋ＊Ｉ＋Ａs＊ＡsT)-1      ……式５−２１ｂ
但し、
ｋ：(ｋ＊Ｉ＋Ａs＊ＡsT)が正則行列になるように決定される正の実数
↑τ＝Ｂs＊（↑Ｓ'＋（Ｊs＊Ｍ-1＊Ｔc−Ｊs'）＊↑ｑ'）＋↑τcmpn
……式５−２２
【０２７３】
この式５−２２が、本実施形態において、第１種類状態量に対応する前記第１目標関節駆動トルク↑τ1_cmdと第２種類状態量に対応する第２目標関節駆動トルク↑τ2_cmdとを算出するために使用する基本式である。この場合、前記式５−２１ｂにより決定される行列Ｂsは、行列Ａsの擬似逆行列としての意味を持つものである。
【０２７４】
なお、以降の説明において、任意の行列Ａに対して、“Ａ-1”は、Ａが正則行列でない場合には、次式５−２３により決定される行列Ｂに一致する行列（擬似逆行列）を意味するものとする。また、行列Ａが正則行列である場合には、Ａ-1は、本来の意味でのＡの逆行列を意味するものとする。そして、Ａが正則行列である場合及び正則行列でない場合のいずれの場合でも、便宜上、Ａ-1をＡの逆行列と称する。
【０２７５】
Ｂ＝ＡT＊(ｋ＊Ｉ＋Ａ＊ＡT)-1      ……式５−２３
但し、
ｋ：(ｋ＊Ｉ＋Ａ＊ＡT)が正則行列になるように決定される正の実数
【０２７６】
前記式５−２２における行列Ｂsは、上記の意味でのＡsの逆行列（＝Ａs-1）である。
【０２７７】
補足すると、前記式５−２０の両辺に適当な行列を乗算した上で、↑τを算出する式を構築してもよい。例えば、Ｒsc≡(Ｊs＊ＰcT＊Ｍ-1＊ＪsT)-1)Tにより定義される行列Ｒscを式５−２０の両辺に左側から乗じてなる式を基に、上記と同様に、↑τを算出する式を構築してもよい。この場合、行列Ｒscを式５−２０の両辺に左側から乗じることによって、式５−２０の両辺は、一般化力ベクトルの次元に変換されることとなる。また、↑τを算出する式は、式５−２２の行列Ｂsを、行列((Ｒsc＊Ｊs＊Ｍ-1＊Ｐc)-1＊Ｒsc)に置き換えた式となる。
【０２７８】
以上説明したことを基礎として、以下に、状態量別制御入力決定部８５の処理を具体的に説明する。
【０２７９】
状態量別制御入力決定部８５には、各種類の状態量に対応する目標関節駆動トルク（第１目標関節駆動トルク↑τ1_cmd及び第２目標関節駆動トルク↑τ2cmd）を算出するために、基本パラメータ群決定部８１で決定されたイナーシャ行列Ｍ、遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、接触部位ヤコビアン決定部８２で決定された前記接触部位ヤコビアンＪcと、状態量ヤコビアン決定部８３で決定された各種類の状態量に対応する状態量ヤコビアン（前記第１状態量ヤコビアンＪs1及び第２状態量ヤコビアンＪs2）と、状態量目標値決定部８４で決定された各種類の状態量に関する目標値（前記第１種類状態量目標値ベクトル↑Ｓ1'_cmd及び第２種類状態量目標値ベクトル↑Ｓ2'_cmd）とが逐次入力される。
【０２８０】
この他、状態量別制御入力決定部８５には、一般化変数ベクトル↑ｑの時間的変化率（１階微分値）↑ｑ'の計測値（下側上体１０の移動速度、並びに姿勢角の時間的変化率（角速度）の計測値、各関節の回転角度の時間的変化率（角速度）の計測値）も逐次入力されると共に、後述の接触部位代表点目標加速度↑Ｃも入力される。
【０２８１】
そして、状態量別制御入力決定部８５は、式５−２２の右辺の演算に使用する各パラメータの値を所要の入力値を用いて決定し、その決定したパラメータを用いて式５−２２の右辺の演算を行なうことで、第１目標関節駆動トルク↑τ1_cmd及び第２目標関節駆動トルク↑τ2_cmdをそれぞれ算出する。この場合、式５−２２の右辺のパラメータのうちの一部は、↑τ1_cmd及び↑τ2_cmdの両方に対して共用され、残りのパラメータは、↑τ1_cmd及び↑τ2_cmdのそれぞれに対して各別に決定される。
【０２８２】
さらに詳細には、状態量別制御入力決定部８５は、式５−２２の演算に必要な行列Ｔc，Ｐcと、一般化力ベクトル↑τcmpnとを所定の入力値を用いて算出する処理を実行する。これらのＴc，Ｐc，↑τcmpnは、↑τ1_cmd及び↑τ2_cmdの両方に対して共用されるパラメータである。
【０２８３】
この場合、状態量別制御入力決定部８５は、入力されたイナーシャ行列Ｍ、遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、接触部位ヤコビアンＪ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を算出する。
【０２８９】
↑τ1_cmd及び↑τ2_cmdの両方に対して共用されるＴc、Ｐc、↑τcmpnの算出処理は以上説明した如く実行される。
【０２９０】
補足すると、↑Ｎ＋↑Ｇ≒↑Ｇとなる場合には、前記式５−１５の↑Ｎを省略した式によって、↑τcmpnを算出するようにしてもよい。
【０２９１】
また、状態量別制御入力決定部８５は、第１種類状態量に対応する第１目標関節駆動トルク↑τ1_cmdを算出するために、式５−２２の演算に用いる行列Ｂ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'、↑qをそれぞれ、Ｂs1、Ｒs1、↑Ｓ1'cmd、Ｊs1、Ｊs1'、↑ｑ'の計測値に置き換えた式の演算）を行なうことによって、↑τを算出し、この↑τの算出値を第１目標関節駆動トルク↑τ1_cmdとして決定する。なお、この場合、Ｊs1'（＝Ｊs1の１階微分値）は、第１状態量ヤコビアンＪs1の時系列から算出される。
【０２９４】
また、状態量別制御入力決定部８５は、第２種類状態量に対応する第２目標関節駆動トルク↑τ2_cmdを算出するために、式５−２２の演算に用いる行列Ｂsを算出する処理を実行する。なお、以降の説明では、第２種類状態量に対応する行列ＢsをＢs2と表記する。
【０２９５】
この場合、状態量別制御入力決定部８５は、入力されたイナーシャ行列Ｍ及び第２状態量ヤコビアンＪs2と、前記Ｐcの算出値とから、前記式５−２１に基づいて、行列Ｂs2を算出する。より詳しくは、行列(Ｊs2＊Ｍ-1＊Ｐc)（≡Ａs）の逆行列Ａs-1がＢs2として決定される。なお、Ｂs1を算出する場合と同様、Ａsが正則行列でない場合には、Ａs-1は、前記式５−２３に基づいて算出される擬似逆行列である。
【０２９６】
そして、状態量別制御入力決定部８５は、Ｂs2，Tc，↑τcmpnの算出値と、入力された第２種類状態量目標値ベクトル↑Ｓ2'_cmd、第２状態量ヤコビアンＪs2及びイナーシャ行列Ｍと、↑ｑ'の計測値とを用いて、前記式５−２２の右辺の演算（詳しくは、式５−２２の右辺のＢs、Ｒs、↑Ｓ'、Ｊs、Ｊs'、↑ｑ'をそれぞれ、Ｂs2、Ｒs2、↑Ｓ2'_cmd、Ｊs2、Ｊs2'、↑ｑ'の計測値に置き換えた式の演算）を行なうことによって、↑τを算出し、この↑τの算出値を第２目標関節駆動トルク↑τ2_cmdとして決定する。なお、この場合、Ｊs2'（＝Ｊs2の１階微分値）は、第２状態量ヤコビアンＪs2の時系列から算出される。
【０２９７】
以上説明した状態量別制御入力決定部８５の処理によって、本実施形態では、第１目標関節駆動トルク↑τ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は、結果的に、ロボット１の全体重心点を適切な位置に保つことができるように決定されることとなる。
【０３００】
また、第２目標関節駆動トルク↑τ2_cmdは、Ｍ、↑Ｎ、↑Ｇ、Ｊc、Ｊs2の算出値（又はＭ、↑Ｇ、Ｊc、Ｊs2の算出値）と、第２種類状態量に関する目標値である第２種類状態量目標値ベクトル↑Ｓ2'_cmdと、↑ｑ'の計測値と、接触部位代表点目標加速度Ｃとから決定される。
【０３０１】
この場合、↑τ2_cmdは、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Ｃにより規定される加速度に一致させつつ、第２種類状態量ベクトル↑Ｓ2の時間的変化率↑Ｓ2'を、第２種類状態量目標値ベクトル↑Ｓ2'_cmdに一致させるように決定される。
【０３０２】
このとき、前記したように、↑Ｓ2'_cmdは、ロボット１の各関節の実際の回転角度θactを所要の目標値θcmdに収束させるように決定されている。このため、↑τ2_cmdは、結果的に、ロボット１の各関節の実際の回転角度を、ロボット１の目標とする運動を実現するための所要の目標値θcmdに一致させるように決定される。
【０３０３】
以上の如く状態量別制御入力決定部８５の処理を実行した後、目標関節駆動トルク決定部７２は、次に、合成制御入力決定部８６の処理を実行する。
【０３０４】
この合成制御入力決定部８６は、状態量別制御入力決定部８５で決定された、各種類の状態量に対応する目標関節駆動トルク（第１目標関節駆動トルク↑τ1_cmd及び第２目標関節駆動トルク↑τ2_cmd）を逐次合成することによって、各関節の合成目標関節駆動トルクを逐次決定する処理を実行する機能部である。以降、各関節の合成目標関節駆動トルクを成分として並べたベクトルを合成目標関節駆動トルク↑τt_cmdと表記する。
【０３０５】
なお、本実施形態の説明では、↑τ1_cmd，↑τ2_cmdと同様、↑τt_cmdは、前記駆動力依存一般化力ベクトル↑τと同じ次数（一般化変数ベクトル↑ｑと同じ次数）のベクトルであるとする。この場合、↑τt_cmdの成分のうち、ロボット１の代表リンク（下側上体１０）の位置及び姿勢に対応する成分は、いずれも“０”である。
【０３０６】
本実施形態では、合成制御入力決定部８６は、次式６−２，６−３の演算処理によりそれぞれ算出されるベクトル↑τf1，↑τf2を、式６−１の演算処理により合成することによって、合成目標関節駆動トルク↑τt_cmdを決定する。
【０３０７】
↑τt_cmd＝Ｗ1＊↑τf1＋Ｗ2＊↑τf2      ……式６−１
ただし、
↑τf1＝↑τ1_cmd＋Ｎu1＊↑τ2_cmd      ……式６−２
↑τf2＝↑τ2_cmd      ……式６−３
【０３０８】
ここで、式６−１のＮu1は、前記第１状態量ヤコビアンＪs1と、次式６−４−１により定義される行列Ｊs1#とに応じて、次式６−４により定義される行列である。
【０３０９】
Ｎu1≡Ｉ−Ｊs1T＊Ｊs1#T         ……式６−４
ただし、Ｊs1#≡ＷatT＊Ｊs1T＊(Ｊs1＊Ｗat＊Ｊs1T)-1
……式６−４−１
【０３１０】
なお、式６−４−１におけるＷatは、重み行列（対角行列）であり、本実施形態では、前記イナーシャ行列Ｍの逆行列Ｍ-1を重み行列Ｗatとして使用する。
【０３１１】
このように定義される行列Ｎu1に対して、式６−２の右辺の第２項の一般化力ベクトル（Ｎu1＊↑τ2_cmd）は、その成分のうち、第１種類状態量ベクトル↑Ｓ1に影響を及ぼす成分（詳しくは、↑Ｓ1'を変化させる成分）が“０”となり、その他の成分が↑τ2_cmdと一致するようなベクトルである。
【０３１２】
従って、（Ｎu1＊↑τ2_cmd）は、ロボット１の各関節の関節駆動トルクを、↑τ1_cmdに制御した場合と、↑τ1_cmd＋Ｎu1＊↑τ2_cmd（＝↑τｆ1）に制御した場合とで、第１種類状態量ベクトル↑Ｓ1の制御状態を同じにすることができるような、一般化力ベクトルである。
【０３１３】
このため、↑τ1_cmd＋Ｎu1＊↑τ2_cmd（＝↑τf1）は、第１種類状態量目標値ベクトル↑Ｓ1'_cmdを実現するための第１目標関節駆動トルク↑τ1_cmdに、第１種類状態量ベクトル↑Ｓ1に影響を及ぼさない範囲で、第２種類状態量目標値ベクトル↑Ｓ2'_cmdを実現するための第２目標関節駆動トルク↑τ2_cmdの成分を重畳させた（加え合わせた）目標関節駆動トルクとしての意味を持つものである。
【０３１４】
また、Ｗ1，Ｗ2は、互いに異なる周波数通過特性を有する周波数重みである。従って、Ｗ1＊↑τf1は、↑τf1にＷ1の周波数通過特性のフィルタリング処理を施したものとしての意味を持つ。また、Ｗ2＊↑τf2は、↑τf2にＷ2の周波数通過特性のフィルタリング処理を施したものとしての意味を持つ。
【０３１５】
この場合、本実施系形態では、Ｗ1は、Ｗ2よりも相対的に高周波域の周波数成分を通過可能な周波数通過特性を有する。
【０３１６】
具体的には、本実施形態では、Ｗ1，Ｗ2は、それぞれ、その伝達関数が次式６−５，６−６により表されるフィルタとして機能する周波数重みである。
【０３１７】
Ｗ1＝（１／（Ｔ1＊ｓ＋１））＊（１−W2）      ……式６−５
Ｗ2＝１／（Ｔ2＊ｓ＋１）      ……式６−６
【０３１８】
式６−５、６−６におけるＴ1，Ｔ2は、Ｔ1＜Ｔ2となるようにあかじめ設定された所定値の時定数である。
【０３１９】
従って、周波数重みＷ1は、低周波側のカットオフ周波数と高周波側のカットオフ周波数とがそれぞれ、時定数Ｔ2、Ｔ1により規定されるバンドパスフィルタとして機能する周波数重みである。また、周波数重みＷ2は、高周波側のカットオフ周波数が、時定数Ｔ2により規定されるローパスフィルタとして機能する周波数重みである。
【０３２０】
以上説明した合成制御入力決定部８６の処理によって、合成目標関節駆動トルク↑τt_cmdは、第１目標関節駆動トルク↑τ1_cmdに、第２目標関節駆動トルク↑τ2_cmdのうちの第１種類状態量ベクトル↑Ｓ1'に影響を及ぼさない成分を付加してなる目標関節駆動トルク↑τf1の高周波側の成分（周波数重みＷ1の通過周波数帯の成分）と、第２目標関節駆動トルク↑τ2_cmd（＝↑τf2）の低周波側の成分（周波数重みＷ2の通過周波数帯の成分）とを合成したもの（加え合わせたもの）として決定されることとなる。
【０３２１】
以上の如く合成制御入力決定部８６の処理を実行した後、目標関節駆動トルク決定部７２は、次に、コンプライアンス制御部８７の処理を実行する。
【０３２２】
このコンプライアンス制御部８７は、合成制御入力決定部８６が決定した合成目標関節駆動トルク↑τt_cmdを基に、ロボット１に作用するトータルの床反力（全床反力）の目標値を決定し、この目標値と実際の全床反力の計測値との偏差を“０”に近づけるように、合成目標関節駆動トルク↑τt_cmdを補正する処理を逐次実行する機能部 である。以降、このコンプライアンス制御部８７により合成目標関節駆動トルク↑τt_cmdを補正してなるトルクを補正後合成目標関節駆動トルク↑τtc_cmdという。本実施形態では、この補正後合成目標関節駆動トルク↑τtc_cmdが、制御装置６０の各演算処理周期において、最終的に電動モータ５０を制御するための目標関節駆動トルク（前記目標関節駆動制御部７３に入力するも目標関節駆動トルク）として使用されるものである。
【０３２３】
コンプライアンス制御部８７には、補正後合成目標関節駆動トルク↑τtc_cmdを算出するために、基本パラメータ群決定部８１で決定された遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、合成制御入力決定部８６で決定された合成目標関節駆動トルク↑τt_cmdと、前記接触部位ヤコビアン決定部８２で決定された接触部位代表点規定比率ｒat（又はそれにより規定される接触部位代表点a1〜a4の位置）とが逐次入力される。
【０３２４】
この他、コンプライアンス制御部８７には、状態量別制御入力決定部８５で算出された行列Ｒc、Ｊc'、Ｊc#、前記接触部位代表点目標加速度↑Ｃ（本実施形態では、“０”ベクトル）、各足平部１８Ｒ，１８Ｌにそれぞれ作用する実際の床反力↑FM_act_R，↑FM_act_Lの計測値、代表リンク１０の移動速度の計測値、ロボット１の代表リンク（下側上体）１０の姿勢角の計測値及びその時間的変化率（角速度）の計測値、並びに、各関節の回転角度の計測値及びその時間的変化率（角速度）の計測値も逐次入力される。
【０３２５】
なお、床反力↑FM_act_R，↑FM_act_Lの計測値は、それぞれ、前記力センサ６２Ｒ，６２Ｌの検出出力に基づき、制御装置６０において認識された計測値である。
【０３２６】
そして、コンプラインアンス制御部８７は、まず、入力された↑Ｎ，↑Ｇ（又は↑Ｎ＋↑Ｇ）と、Ｒc、Ｊc'、Ｊc#と、↑τt_cmdと、一般化変数ベクトル↑ｑの１階微分値↑ｑ'の計測値とから、前記式５−１０の右辺の演算を行なうことによって、接触部位代表点a1〜a4にそれぞれ作用する力（並進力）の目標値を並べたベクトルとしての接触部位代表点目標作用力↑Ｆc_cmdを算出する。
【０３２７】
すなわち、次式７−１により、↑Ｆc_cmdが算出される。この接触部位代表点目標作用力↑Ｆc_cmdは、ロボット１の各関節の実際の関節駆動トルクを↑τt_cmdにより規定される目標関節駆動トルクに一致させた場合に、接触部位代表点a1〜a4でロボット１に作用すべき力（並進力）としての意味を持つものである。
【０３２８】
↑Ｆc_cmd＝（Ｊc#）T＊（↑τt_cmd−↑Ｎ−↑Ｇ）
−Ｒc＊↑Ｃ−Ｒc＊Ｊc'＊↑ｑ'    ……式７−１
【０３２９】
なお、↑Ｎ＋↑Ｇ≒↑Ｇとなる場合には、式７−１の↑Ｎを省略してもよい。
【０３３０】
次いで、コンプライアンス制御部８７は、上記接触部位代表点目標作用力↑Ｆc_cmdを、ロボット１に対する全床反力の作用点としての所定の全床反力作用点（例えば目標ＺＭＰ）に作用する力の目標値としての目標全床反力↑FMt_cmdに変換する。
【０３３１】
この目標全床反力↑FMt_cmdは、本実施形態では、３軸方向の並進力成分と３軸周りのモーメント成分とから成るベクトル（６自由度のベクトル）である。この場合、↑FMcmdの並進力成分は、各接触部位代表点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の計測値は、左側足平部１８Ｌに対応する所定の床反力作用点に作用する３軸方向の並進力成分と３軸周りのモーメント成分とから成るベクトルである。
【０３３５】
そして、コンプライアンス制御部８７は、前記全床反力作用点と右側足平部１８Ｒの床反力作用点との位置関係に応じて決定される成分を有する変換行列によって、↑FMact_Rの計測値を全床反力作用点に作用する力（並進力成分及びモーメント成分の組）に変換したものと、前記全床反力作用点と左側足平部１８Ｌの床反力作用点との位置関係に応じて決定される成分を有する変換行列によって、↑FMact_Lの計測値を全床反力作用点に作用する力（並進力成分及びモーメント成分の組）に変換したものとを加え合わせることによって、全床反力↑FMt_actの計測値を算出する。
【０３３６】
この↑FMt_actの計測値の並進力成分は、↑FMact_Rの並進力成分と↑FMact_Lの並進力成分との総和（ベクトル和）の並進力となる。また、↑FMt_actのモーメント成分は、↑FMact_Rのモーメント成分と↑FMact_Lのモーメント成分と↑FMact_Rの並進力成分が全床反力作用点の周りに発生するモーメントと↑FMact_Lの並進力成分が全床反力作用点の周りに発生するモーメントとの総和（ベクトル和）のモーメントとなる。
【０３３７】
コンプライアンス制御部８７は、上記のように目標全床反力↑FMt_cmdと、全床反力↑FMt_actの計測値とを算出した後、これらの偏差↑ΔFMt（＝↑FMt_cmd−↑FMt_act）を算出する。そして、コンプラインアス制御部８７は、この偏差↑ΔFMtを“０”に収束させる操作量としての合成目標関節駆動トルク↑τt_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）-1を乗じることによって、合成目標関節駆動トルク↑τt_cmdの補正量↑Δτtを算出する。
【０３４１】
↑Δτt＝（Ｊc#T）-1＊↑ΔFc      ……式７−２
【０３４２】
これにより、補正量↑Δτtは、フィードバック制御則としての比例則により、偏差↑ΔFMtを“０”に収束させるように決定されることとなる。
【０３４３】
そして、コンプラインアンス制御部８７は、次式７−３の通り、この補正量↑Δτtを合成目標関節駆動トルク↑τt_cmdに加えることによって、補正後合成目標関節駆動トルク↑τtc_cmdを決定する。
【０３４４】
↑τtc_cmd＝↑τt_cmd＋↑Δτt      ……式７−３
【０３４５】
以上のコンプライアンス制御部８７の処理によって、合成目標関節駆動トルク↑τt_cmdに対応する全床反力である目標全床反力↑FMt_cmdと実際の全床反力↑FMt_actの計測値との偏差を“０”に近づけるように、合成目標関節駆動トルク↑τt_cmdが補正されることとなる。
【０３４６】
本実施形態では、このように決定された補正後合成目標関節駆動トルク↑τtc_cmd（詳しくは、↑τtc_cmdのうちの各関節の回転角度に対応する成分）が前記関節駆動制御部７３に入力される。
【０３４７】
以上の如くコンプライアンス制御部８７の処理を実行した後、目標関節駆動トルク決定部７２は、次に、接触部弾性変形補償部８８の処理を実行する。
【０３４８】
この接触部弾性変形補償部８８は、前記状態量目標値決定部８４で、ロボット１の接地している足平部１８の弾性変形や床面の弾性変形を考慮し、前記第２種類状態量目標値ベクトル↑Ｓ2'cmdを決定するために使用するロボット１の各関節の目標関節角θcmdとして、前記歩容生成部７１で生成した目標関節角（以下、基本目標関節角θcmd_baseという）から修正してなる値を設定する処理を逐次実行する機能部である。
【０３４９】
この接触部弾性変形補償部８８には、コンプライアンス制御部８７で算出された前記接触部位代表点目標作用力↑Ｆc_cmdと、接触部位ヤコビアン決定部８２で算出された前記接触部位ヤコビアンＪcとが入力されると共に、歩容生成部７１で生成された基本目標関節角θcmd_baseが入力される。
【０３５０】
そして、接触部弾性変形補償部８８は、入力された↑Ｆc_cmd及びＪcを用いて、次式８−１により、各関節の基本目標関節角θcmd_baseを補正するための関節角補正量Δθcmdを成分として含むベクトル↑Δｑcmd（以降、関節角補正量ベクトル↑Δｑcmdという）を算出する。なお、この関節角補正量ベクトル↑Δｑcmdの各成分は、一般化変数ベクトル↑ｑの各成分にそれぞれ対応しており、↑Δｑcmdの成分のうちの各関節の回転角度に対応する成分が、該関節に対応する上記関節角補正量Δθcmdである。
【０３５１】
↑Δｑcmd＝(Ｊc）-1＊（Ｋstif＊↑Ｆc_cmd）    ……式８−１
【０３５２】
この式８−１における“Ｋstif”は、接地している足平部１８の弾性特性や床面の弾性特性に依存するばね定数の逆数値に相当するものとしたあらかじめ定められた係数行列（対角行列）である。換言すれば、前記接触部位代表点a1〜a4に作用する力（並進力）を、その力に起因する足平部１８（接地している足平部１８）や床面の弾性変形によって生じる接触部位代表点a1〜a4の変位量に変換する係数行列である。
【０３５３】
従って、式８−１の（Ｋstif＊↑Ｆc_cmd）は、前記接触部位代表点a1〜a4に作用する力（並進力）を↑Ｆc_cmdに一致させた場合に生じる接触部位代表点a1〜a4の位置の変位量の推定値に相当するものとなる。
【０３５４】
そして、（Ｋstif＊↑Ｆc_cmd）に、接触部位ヤコビアンＪcの逆行列(Ｊc）-1を乗じることによって、接触部位代表点a1〜a4の位置の上記変位量が、ロボット１の各関節の回転角度を含む一般化変数ベクトル↑ｑの各成分の変化量に変換され、この変換された変化量を成分として構成されるベクトルが、上記関節角補正量ベクトル↑Δｑとして算出される。なお、(Ｊc)-1は、前記式５−２３に基づいて算出される擬似逆行列である。
【０３５５】
次いで、接触部弾性変形補償部８８は、次式８−２の如く、上記関節角補正量ベクトル↑Δｑのうちの関節角補正量Δθcmdを、それに対応する関節の基本目標関節角θcmd_baseに加算することによって、前記状態量目標値決定部８８に入力する各関節の目標関節角θcmdを決定する。
【０３５６】
θcmd＝θcmd_base＋Δθcmd    ……式８−２
【０３５７】
以上の接触部弾性変形補償部８８の処理によって、接触部位として接地している足平部１８の弾性変形や床面の弾性変形を加味した適切な目標関節角θcmd（実際の関節角の収束目標として適切な目標関節角θcmd）を決定できる。
【０３５８】
以上が、目標関節駆動トルク決定部７２の処理の詳細である。
【０３５９】
以上説明した本実施形態によれば、前記合成目標関節駆動トルク↑τt_cmdの高周波側の成分（Ｗ1＊↑τf1）は、主に、第１目標関節駆動トルク↑τ1_cmdの高周波側の成分に依存する成分となり、↑τt_cmdの低周波側の成分（Ｗ2＊↑τf2）は、第２目標関節駆動トルク↑τ2_cmdの低周波側の成分に依存する成分となる。
【０３６０】
このため、前記第１種類状態量目標値ベクトル↑Ｓ1'_cmdを達成すること（ひいては、ロボット１の全体重心点の位置を目標とする位置に制御すること）を高い速応性で行いつつ、長期的には、前記第２種類状態量目標値ベクトル↑Ｓ2'_cmdを達成すること（ひいては、各関節の実際の回転角度を目標関節角に制御すること）を満足し得るように、ロボット１の各関節の駆動トルクを制御することができる。
【０３６１】
さらに、合成目標関節駆動トルク↑τt_cmdの高周波側の成分（Ｗ1＊↑τf1）は、第２目標関節駆動トルク↑τ2_cmdの高周波側の成分のうちの、第１種類状態量ベクトル↑Ｓ1に影響を及ぼさない成分をも含んでいる。このため、第１種類状態量ベクトル↑Ｓ1の時間的変化率↑Ｓ1'の制御性を損なうことなく、高周波域での第２種類状態量ベクトル↑Ｓ2の時間的変化率↑Ｓ2'の制御性を高めることができる。
【０３６２】
また、本実施形態では、第１種類状態量目標値ベクトル↑Ｓ1'_cmdのうちの全並進運動量の１階微分値↑Ｐtotal'の目標値は、鉛直方向の位置を含めたロボット１の全体重心点の位置を歩容生成部７１で生成された目標歩容の全体重心点の目標位置に収束させるように決定されている。換言すれば、ロボット１の運動によって発生する並進慣性力の目標値に相当する↑Ｐtotal'の目標値が、ロボット１の実際の全体重心点の位置を適切な目標位置に収束させるように決定されている。
【０３６３】
さらに、↑Ｓ1'_cmdのうちの全角運動量の１階微分値↑Ｌtotal'の目標値は、ロボット１の全体重心点の水平位置を、該全体重心点に質点を有する仮想的な倒立振子の支点としての目標ＺＭＰの鉛直方向上方の位置に収束させるように決定されている。換言すれば、ロボット１の運動によって発生するモーメント慣性力の目標値に相当する↑Ｌtotal'の目標値が、ロボット１の実際の全体重心点の水平位置を、上記仮想的な倒立振子の傾倒を防止し得る位置に収束させるように決定されている。
【０３６４】
そして、本実施形態では、前記周波数重みＷ1に対応する高周波域において、第１種類状態量目標値ベクトル↑Ｓ1'_cmdの↑Ｐtotal'の目標値及び↑Ｌtotal'の目標値を実現し得るように、前記合成目標関節駆動トルク↑τt_cmdが決定される。
【０３６５】
ここで、本実施形態のロボット１では、各関節の関節駆動系にばね部材であるトーションバー５３を備えるため、各関節が柔軟性を有する。このため、ロボット１に作用する外力の影響で、各関節の回転角度の変動が生じやすく、ひいては、全体重心点の位置の変動（特に鉛直方向の位置の変動）も生じやすい。
【０３６６】
しかるに、上記の如く第１種類状態量目標値ベクトル↑Ｓ1'_cmdを決定しているために、ロボット１の全体重心点の実際の位置が目標位置からのずれると、それを素早い応答性で解消するように合成目標関節駆動トルク↑τt_cmdが決定される。
【０３６７】
そのため、ロボット１の実際の全体重心点の位置が適切な目標位置からずれるの防止することができる。その結果、ロボット１の各関節が柔軟性を有していても、ロボット１の姿勢のふらつき等が生じるのを防止し、該ロボット１の挙動の安定性を高めることができる。
【０３６８】
また、本実施形態では、前記式５−２０に基づいて、各種類の状態量毎の目標関節駆動トルク↑τ1_cmd，↑τ2_cmdが決定される。従って、接触部位代表点a1〜a4の運動を前記目標値↑Ｃにより規定した上で、ロボット１の制御すべき状態量（第１種類状態量及び第２種類状態量）に関する目標値としての第１種類状態量目標値ベクトル↑Ｓ1'_cmdと第２種類状態量目標値ベクトル↑Ｓ2'_cmdとに基づいて、それぞれ、↑τ1_cmd、↑τ2_cmdが決定される。
【０３６９】
このため、ロボット１に作用する外力の値（これは外乱等の影響を受けやすい）を特定することなく、↑Ｓ1'_cmd、↑Ｓ2'_cmdを実現し得る適切な↑τ1_cmd、↑τ2_cmdを決定できる。また、特に、第１目標関節駆動トルク↑τ1_cmdは、ロボット１の全ての関節の回転動作を特定することを必要とせずに決定できる。このため、ロボット１に作用する外力の変動等に伴うロボット１の関節の動きの柔軟性を高めることができる。
【０３７０】
ここで、以上説明した実施形態と本発明との対応関係について補足しておく。
【０３７１】
本実施形態では、前記状態量別制御入力決定部８５により、本発明における状態量別制御入力決定手段が実現される。この場合、前記第１種類状態量及び第２種類状態量が本発明における複数種類の状態量に相当し、前記第１目標関節駆動トルク↑τ1_cmd及び第２目標関節駆動トルク↑τ2_cmdがそれぞれ、本発明における状態量別制御入力に相当する。また、第１種類状態量目標値ベクトル↑Ｓ1'_cmdと第２種類状態量目標値ベクトル↑Ｓ2'_cmdとが、各種類の状態量に関する目標値に相当する。なお、本実施形態では、状態量の種類数Ｍは“２”である。
【０３７２】
また、前記合成制御入力決定部８５により、本発明における合成制御入力決定手段が実現される。この場合、前記合成目標関節駆動トルク↑τt_cmdが、本発明における合成制御入力τtotalに相当し、前記式６−１のＷ1＊↑τf1と、Ｗ2＊↑τf2とがそれぞれ、本発明における周波数領域別制御入力（Ｗ(i)＊τf(i)）に相当し、周波数重みＷ1，Ｗ2のそれぞれの通過可能な周波数帯が、本発明における周波数領域に相当する。なお、本実施形態では、周波数領域の個数ｎは２つである。
【０３７３】
また、本実施形態では、↑τf1、↑τf2がそれぞれ、本発明における（前記式０１、０２ａにおける）τf(i)に相当する。この場合、↑τ1_cmd，↑τ2_cmdがそれぞれ、↑τf1（τf(1)）の組成成分τ(1,1)、τ(1,2)に相当し、行列Ｎu1が本発明におけるＮu(1,1)に相当する。また、↑τ2_cmdが、↑τf2（τf(2)）の組成成分τ(2,1)に相当する。なお、本実施形態では、↑τf1（τf(1)）の組成成分の個数ｍ(1)は“２”、↑τf2（τf(2)）の組成成分の個数ｍ(2)は“１”となる。
【０３７４】
また、本実施形態では、前記行列Ｊs1#、Ｊs2#が、それぞれ本発明におけるＪs#(i,j)に相当し、Ｊs1、Ｊs2がそれぞれ、本発明におけるＪs(i,j)に相当し、↑Ｓ1，↑Ｓ2がそれぞれ、本発明におけるＳ(i,j)に相当する。また、本実施形態におけるＭ、↑ｑがそれぞれ、本発明におけるイナーシャ行列Ｍ、一般化変数ベクトルｑに相当する。なお、本実施形態では、本発明における重み行列Ｗdを単位行列とし、Ｗat＝Ｍ-1としている。
【０３７５】
また、前記コンプライアンス制御部８７及び関節駆動制御部７３により、本発明におけるアクチュエータ制御手段が実現される。
【０３７６】
［第２実施形態］
次に、本発明の第２実施形態を図６〜図８を参照して説明する。なお、本実施形態の説明では、第１実施形態と同一の構成部分については、第１実施形態と同一の参照符号を用い詳細な説明を省略する。
【０３７７】
本実施形態におけるロボットは、前記第１実施形態で例示したロボット１と同じである。そして、本実施形態では、制御装置６０は、その機能のうちの目標関節駆動トルク決定部の一部の処理が第１実施形態と相違する。すなわち、制御装置６０は、第１実施形態で説明した目標関節駆動トルク決定部７２の代わりに、図６のブロック図に示す目標関節駆動トルク決定部９０を備える。
【０３７８】
この目標関節駆動トルク決定部９０は、その主要な機能として、基本パラメータ群決定部９１、接触部位ヤコビアン決定部９２、状態量ヤコビアン決定部９３、状態量目標値決定部９４、状態量別制御入力決定部９５、合成制御入力決定部９６、コンプライアンス制御部９７、及び接触部弾性変形補償部９８を備える。
【０３７９】
この場合、これらの機能のうち、基本パラメータ群決定部９１、接触部位ヤコビアン決定部９２、コンプライアンス制御部９７、及び接触部弾性変形補償部９８は、それぞれ、第１実施形態で説明した基本パラメータ群決定部８１、接触部位ヤコビアン決定部８２、コンプライアンス制御部８７、及び接触部弾性変形補償部８８と同じである。そこで、これらの機能部の処理については説明を省略する。
【０３８０】
一方、本実施形態では、状態量ヤコビアン決定部９３、状態量目標値決定部９４、状態量別制御入力決定部９５、及び合成制御入力決定部９６の処理が第１実施形態と相違しており、以下にこれらの処理を説明する。
【０３８１】
まず、本実施形態において制御対象とする複数種類の状態量について説明する。本実施形態では、ロボット１の歩行動作を安定に行なう上で適切な目標値に制御することが望ましい制御対象として３種類の状態量があらかじめ選定されている。その３種類の状態量のうちの２種類の状態量は、第１実施形態で説明した第１種類状態量（全並進運動量↑Ｐtotalと全角運動量↑Ｌtotaとの組）及び第２種類状態量（ロボット１の各関節の回転角速度）である。
【０３８２】
一方、残りの１種類の状態量（以降、第３種類状態量という）は、脚体２Ｒ，２Ｌのうち、ロボット１の歩行動作中に先端部（足平部１８）を離床させた側の脚体２、すなわち遊脚となる脚体２の先端部（足平部１８）の運動状態量である。その運動状態量は、本実施形態の例では、遊脚となる脚体２（以下、遊脚側脚体２ということがある）の足平部１８の位置の時間的変化率と姿勢の時間的変化率との組、すなわち、該遊脚側脚体２の足平部１８の移動速度と姿勢の角速度との組である。従って、本実施形態では、遊脚側脚体２の足平部１８が空中を移動するときの該足平部１８の移動速度及び姿勢の角速度を制御対象の１つの状態量とする。
【０３８３】
そして、本実施形態における状態量ヤコビアン決定部９３は、上記の３種類の状態量のそれぞれと、一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を表現するヤコビアン（状態量ヤコビアン）を算出する処理を逐次実行する。
【０３８４】
この場合、前記第１種類状態量及び第２種類状態量にそれぞれ対応する状態量ヤコビアンＪs1，Ｊs2の算出処理は、前記第１実施形態と同じである。
【０３８５】
一方、前記第３種類状態量に対応する状態量ヤコビアンＪs3（以降、第３状態量ヤコビアンＪs3という）は、遊脚側脚体２の足平部１８の並進移動速度↑Ｖfootと該足平部１８の姿勢の時間的変化率（角速度）↑ωfootとを成分として並べて構成されるベクトル↑Ｓ3（＝［↑Ｖfoot，↑ωfoot］T。以降、第３種類状態量ベクトル↑Ｓ3という）と、前記一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を次式３−３より表現する行列Ｊs3である。
【０３８６】
↑Ｓ3＝Ｊs3＊↑ｑ'      ……式３−３
【０３８７】
なお、本実施形態では、↑Ｖfoot，↑ωfootは、それぞれ、３自由度のベクトルであるので、↑Ｓ3は、６自由度のベクトルである。
【０３８８】
この第３状態量ヤコビアンＪs3を算出するために、状態量ヤコビアン決定部９３には、代表リンク（下側上体）１０の移動速度、並びに姿勢角及びその時間的変化率（角速度）の計測値と、各関節の回転角度及びその時間的変化率（角速度）の計測値とが入力される。
【０３８９】
そして、状態量ヤコビアン決定部９３は、右側脚体２Ｒが遊脚となる期間（片脚支持期）では、第３種類状態量ベクトル↑Ｓ3が、右側脚体２Ｒの足平部１８Ｒの並進移動速度↑Ｖfootと該足平部１８Ｒの姿勢の角速度↑ωfootとを成分として構成される状態量ベクトルであるとして、この↑Ｓ3と、↑ｑ'との間の関係を上記式３−３により表現する第３状態量ヤコビアンＪs3を入力された計測値から算出する。
【０３９０】
また、状態量ヤコビアン決定部９３は、左側脚体２Ｌが遊脚となる期間（片脚支持期）では、第３種類状態量ベクトル↑Ｓ3が、左側脚体２Ｌの足平部１８Ｌの並進移動速度↑Ｖfootと該足平部１８Ｌの姿勢の角速度↑ωfootとを成分として構成される状態量ベクトルであるとして、この↑Ｓ3と、↑ｑ'との間の関係を上記式３−３により表現する第３状態量ヤコビアンＪs3を入力された計測値から算出する。
【０３９１】
なお、右側脚体２Ｒが遊脚となる期間であるか、左側脚体２Ｌが遊脚となる期間であるかの判断は、例えば、前記力センサ６２Ｒ，６２Ｌの検出出力、あるいは、前記歩容生成部７１から目標関節駆動トルク決定部９０に与えられる目標歩容に基づいて行なわれる。
【０３９２】
また、ロボット１の歩行動作中に両脚体２Ｒ，２Ｌの足平部１８Ｒ，１８Ｌが着床状態となる両脚支持期においては、状態量ヤコビアン決定部９３は、第３状態量ヤコビアンＪs3の算出処理を中断する。
【０３９３】
以上が、本実施形態における状態量ヤコビアン決定部９３の処理の詳細である。
【０３９４】
また、本実施形態における前記状態量目標値決定部９４は、前記した３種類の状態量に関する目標値を決定する処理を実行する。
【０３９５】
この場合、本実施形態では、前記第１種類状態量に関する目標値は、前記第１実施形態と同じである。すなわち、該目標値は、前記第１種類状態量ベクトル↑Ｓ1の時間的変化率（１階微分値）↑Ｓ1'（＝d↑Ｓ1/dt）の目標値↑Ｓ1'_cmd（前記第１種類状態量目標値ベクトル↑Ｓ1'_cmd）である。そして、状態量目標値決定部９４は、この第１種類状態量目標値ベクトル↑Ｓ1'_cmdを、第１実施形態と同じ処理によって決定する。
【０３９６】
また、前記第２種類状態量に関する目標値は、第１実施形態と同様に前記第２種類状態量ベクトル↑Ｓ2の時間的変化率（１階微分値）↑Ｓ2'（＝d↑Ｓ2/dt）の目標値である。但し、本実施形態では、前記状態量目標値決定部９４は、↑Ｓ2'の目標値として、複数種類、例えば２種類の目標値↑Ｓ2'_cmd1，↑Ｓ2'_cmd2をそれぞれ各別の処理によって決定する。
【０３９７】
さらに詳細には、本実施形態では、状態量目標値決定部９４は、上記２種類の目標値↑Ｓ2'_cmd1，↑Ｓ2'_cmd2をそれぞれ、ロボット１の各関節の実際の回転角度（関節角）を所要の目標値に追従させるための制御入力として決定する。
【０３９８】
但し、この場合、２種類の目標値↑Ｓ2'_cmd1，↑Ｓ2'_cmd2のうちの目標値↑Ｓ2'_cmd1は、各関節の実際の関節角の計測値をフィードバックすることなく、該関節角の目標値の時系列から直接的に（フィードフォワード則により）決定される。また、目標値↑Ｓ2'_cmd2は、各関節の実際の関節角の計測値のフィードバックを行なうフィードバック制御則により決定される。以降、↑Ｓ2'_cmd1，↑Ｓ2'_cmd2をそれぞれ、第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1、第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2という。
【０３９９】
これらの第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1、第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2は、具体的には、次のように決定される。
【０４００】
すなわち、第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1の決定処理においては、状態量目標値決定部９４は、接触部弾性変形補償部９８で第１実施形態と同じ処理によって算出されるロボット１の各関節角の目標値θcmdの時系列に基づいて、該目標値θcmdの２階微分値（θcmdの時系列により規定される各関節の目標角加速度）を逐次算出し、その算出した各関節の目標角加速度を成分として構成されるベクトルを、第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1として決定する。
【０４０１】
また、第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2の決定処理においては、状態量目標値決定部９４は、第１実施形態における第２種類状態量目標値ベクトル↑Ｓ2'_cmdの算出処理と同じ処理によって、第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2を決定する。すなわち、状態量目標値決定部９４は、接触部弾性変形補償部９８から逐次入力される各関節の関節角の目標値θcmdと、該関節の実際の回転角度θactの計測値との偏差Δθ（＝θcmd−θact）を並べたベクトル↑ΔΘと、その１階微分値↑Θ’とから、フィードバック制御則としてのＰＤ則によって（前記式４−４の右辺の演算によって）、第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2を決定する。従って、↑Ｓ2'_cmd2は、各関節におけるθactをフィードバック制御則にθcmdに収束させるように決定される目標値ベクトルである。
【０４０２】
また、前記第３種類状態量に関する目標値は、前記第３種類状態量ベクトル↑Ｓ3の時間的変化率（１階微分値）↑Ｓ3'（＝d↑Ｓ3/dt）の目標値である。そして、本実施形態では、前記状態量目標値決定部９４は、↑Ｓ2'の目標値と同様に、↑Ｓ3'の目標値として、複数種類、例えば２種類の目標値↑Ｓ3'_cmd1，↑Ｓ3'_cmd2をそれぞれ各別の処理によって決定する。
【０４０３】
さらに詳細には、本実施形態では、状態量目標値決定部９４は、上記２種類の目標値↑Ｓ3'_cmd1，↑Ｓ3'_cmd2をそれぞれ、ロボット１の遊脚側脚体２の足平部１８の実際の位置及び姿勢を所要の目標値に追従させるための制御入力として決定する。
【０４０４】
但し、この場合、２種類の目標値↑Ｓ3'_cmd1，↑Ｓ3'_cmd2のうちの目標値↑Ｓ3'_cmd1は、遊脚側脚体２の足平部１８の実際の位置及び姿勢の計測値をフィードバックすることなく、該足平部１８の目標位置及び目標姿勢の時系列から直接的に（フィードフォワード則により）決定される。また、目標値↑Ｓ3'_cmd2は、遊脚側脚体２の足平部１８の実際の位置及び姿勢の計測値のフィードバックを行なうフィードバック制御則により決定される。以降、↑Ｓ3'_cmd1，↑Ｓ3'_cmd2をそれぞれ、第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1、第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2という。
【０４０５】
これらの第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1、第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2は、具体的には、次のように決定される。
【０４０６】
すなわち、状態量目標値決定部９４は、接触部弾性変形補償部９８から逐次入力される各関節の関節角の目標値θcmdの時系列に基づいて（又は歩容生成部７１から入力される目標歩容のうちの目標関節角の時系列に基づいて）、遊脚側脚体２の足平部１８の目標位置及び目標姿勢を逐次算出する。図７は、このように算出される遊脚側足平部１８の目標位置（鉛直方向の目標位置）の軌道の例を代表的に示している。
【０４０７】
また、状態量目標値決定部９４は、ロボット１の各関節の実際の関節角の計測値の時系列に基づいて、遊脚側脚体２の足平部１８の実際の位置及び姿勢の計測値を逐次算出する。
【０４０８】
そして、第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1の決定処理においては、状態量目標値決定部９４は、遊脚側脚体２の足平部１８の算出した目標位置及び目標姿勢の時系列に基づいて、該目標位置及び目標姿勢のそれぞれの２階微分値（該目標位置の時系列により規定される該足平部１８の目標並進加速度と、該目標姿勢の時系列により規定される該足平部１８の姿勢の目標角加速度）を逐次算出し、その算出した目標並進加速度及び目標角加速度を成分として構成されるベクトルを、第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1として決定する。
【０４０９】
例えば、図７に示す目標位置の軌道の例では、第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1の目標並進加速度の鉛直方向成分が、図示の軌道を２階微分してなる値として決定される。
【０４１０】
また、第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2の決定処理においては、状態量目標値決定部９４は、遊脚側脚体２の足平部１８の算出した目標位置及び目標姿勢のそれぞれと、該足平部１８の実際の位置の計測値及び姿勢の計測値のそれぞれとの偏差↑ΔＸfootと、該偏差↑ΔＸfootの１階微分値↑ΔＸfoot’とを算出する。
【０４１１】
なお、上記偏差↑ΔＸは、遊脚側脚体２の足平部１８の目標位置と実際の位置の計測値との偏差（３自由度の偏差）と、該足平部１８の目標姿勢と実際の姿勢の計測値との偏差（３自由度偏差）とを並べて構成されるベクトルである。
【０４１２】
さらに、状態量目標値決定部９４は、各関節の上記偏差↑ΔＸを“０”に収束させるためのフィードバック操作量を所定のフィードバック制御則により算出し、その算出したフィードバック操作量を第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2として決定する。
【０４１３】
この場合、具体的には、状態量目標値決定部９４は、上記偏差↑ΔＸをＰＤ則（比例・微分則）によって“０”に収束させるように↑Ｓ3'_cmd2を決定する。すなわち、状態量目標値決定部９４は、上記偏差↑ΔＸと、その１階微分値↑ΔＸ’とから、次式４−５により↑Ｓ3'_cmd2を決定する。
【０４１４】
↑Ｓ3'cmd2＝Kp4＊↑ΔＸfoot＋Kv4＊↑ΔＸfoot'      ……式４−５
【０４１５】
なお、式４−５におけるKp4，Kv4は、それぞれあらかじめ設定された所定値の比例ゲイン、微分ゲインである。これらのゲインKp4，Kv4は対角行列（↑Ｓ3'_cmd2と同じ次数の対角行列）によって表現される。
【０４１６】
補足すると、第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1及び第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2の決定処理において、右側脚体２Ｒ及び左側脚体２Ｌのいずれが遊脚であるかの判断は、例えば、前記力センサ６２Ｒ，６２Ｌの検出出力、あるいは、前記歩容生成部７１から目標関節駆動トルク決定部９０に与えられる目標歩容に基づいて行なわれる。
【０４１７】
また、ロボット１の歩行動作中に両脚体２Ｒ，２Ｌの足平部１８Ｒ，１８Ｌが着床状態となる両脚支持期においては、状態量目標値決定部９４は、↑Ｓ3'_cmd1及び↑Ｓ3'_cmd2の決定処理を中断する。
【０４１８】
以上が本実施形態における状態量目標値決定部９４の処理の詳細である。
【０４１９】
次に、本実施形態における状態量別制御入力決定部９５は、前記３種類の状態量に関する目標値、すなわち前記第１種類状態量目標値ベクトル↑Ｓ1'_cmd、第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1、第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2、第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1、及び第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2を実現するために必要な制御入力（アクチュエータ（電動モータ５０）の動作制御用の制御入力）として、ロボット１の各関節の目標関節駆動トルクを各目標値ベクトル毎に決定する処理を逐次実行する。
【０４２０】
以降、前記↑Ｓ1'_cmd、↑Ｓ2'_cmd1、↑Ｓ2'_cmd2、↑Ｓ3'_cmd1、↑Ｓ3'_cmd2のそれぞれに対応する各関節の目標関節駆動トルクを成分として並べたベクトルを、それぞれ第１目標関節駆動トルク↑τ1_cmd、第２ＦＦ目標関節駆動トルク↑τ2_cmd1、第２ＦＢ目標関節駆動トルク↑τ2_cmd2、第３ＦＦ目標関節駆動トルク↑τ3_cmd1、第３ＦＢ目標関節駆動トルク↑τ3_cmd2と表記する。
【０４２１】
なお、本実施形態の説明では、↑τ1_cmd，↑τ2_cmd1，↑τ2_cmd2，↑τ3_cmd1，↑τ3_cmd2は、第１実施形態における↑τ1_cmd、↑τ2cmdと同様に、前記駆動力依存一般化力ベクトル↑τと同じ次数（一般化変数ベクトル↑ｑと同じ次数）のベクトルであるとする。
【０４２２】
状態量別制御入力決定部９５は、前記第１実施形態と同様に、前記式５−２２を用いて↑τ1_cmd，↑τ2_cmd1，↑τ2_cmd2，↑τ3_cmd1，↑τ3_cmd2を算出する。その算出は、第１実施形態と同様の仕方で行なわれる。
【０４２３】
さらに詳細には、状態量別制御入力決定部９５には、各目標関節駆動トルク↑τ1_cmd，↑τ2_cmd1，↑τ2_cmd2，↑τ3_cmd1，↑τ3_cmd2を算出するために、基本パラメータ群決定部９１で決定されたイナーシャ行列Ｍ、遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、接触部位ヤコビアン決定部９２で決定された接触部位ヤコビアンＪcと、状態量ヤコビアン決定部９３で決定された各種類の状態量に対応する状態量ヤコビアンＪs1，Ｊs2，Ｊs3と、状態量目標値決定部９４で決定された各目標値ベクトル↑Ｓ1'_cmd、↑Ｓ2'_cmd1、↑Ｓ2'_cmd2、↑Ｓ3'_cmd1、↑Ｓ3'_cmd2とが逐次入力される。
【０４２４】
この他、状態量別制御入力決定部９５には、一般化変数ベクトル↑ｑの時間的変化率（１階微分値）↑ｑ'の計測値（下側上体１０の移動速度、並びに姿勢角の時間的変化率（角速度）の計測値、各関節の回転角度の時間的変化率（角速度）の計測値）も逐次入力されると共に、接触部位代表点目標加速度↑Ｃも入力される。なお、本実施形態における接触部位代表点目標加速度↑Ｃは、第１実施形態のものと同一（“０”ベクトル）である。
【０４２５】
そして、状態量別制御入力決定部９５は、入力されたイナーシャ行列Ｍ、遠心力・コリオリ力依存一般化力ベクトル↑Ｎ及び重力依存一般化力ベクトル↑Ｇ（又は非線形項一般化力ベクトル（↑Ｎ＋↑Ｇ））と、接触部位ヤコビアンＪcと、接触部位代表点目標加速度↑Ｃとを用いて、行列Ｔc、Ｐcと、一般化力ベクトル↑τcmpnとを算出する。この算出の仕方は第１実施形態と同じである。
【０４２６】
また、状態量別制御入力決定部９５は、第１種類状態量、第２種類状態量及び第３種類状態量にそれぞれ対応する前記式５−２２の右辺の行列Ｂs（Ｂs1，Ｂs2，Ｂs3）を第１実施形態と同様の仕方で算出する。
【０４２７】
すなわち、第１種類状態量に対応する行列Ｂs1は、入力されたイナーシャ行列Ｍ及び第１状態量ヤコビアンＪs1と、前記行列Ｐcの算出値とから、前記式５−２１ａ又は５−２１ｂに基づいて算出される。
【０４２８】
また、第２種類状態量に対応する行列Ｂs2は、入力されたイナーシャ行列Ｍ及び第２状態量ヤコビアンＪs2と、前記行列Ｐcの算出値とから、前記式５−２１ａ又は５−２１ｂに基づいて算出される。
【０４２９】
また、第３種類状態量に対応する行列Ｂs3は、入力されたイナーシャ行列Ｍ及び第３状態量ヤコビアンＪs3と、前記行列Ｐcの算出値とから、前記式５−２１ａ又は５−２１ｂに基づいて算出される。
【０４３０】
そして、状態量別制御入力決定部９５は、Ｂs1，Ｔc，↑τcmpnの算出値と、入力された第１種類状態量目標値ベクトル↑Ｓ1'_cmd、第１状態量ヤコビアンＪs1及びイナーシャ行列Ｍと、↑ｑ'の計測値とを用いて、前記式５−２２の右辺の演算（詳しくは、式５−２２の右辺のＢs、↑Ｓ'、Ｊs、Ｊs'、↑ｑ'をそれぞれ、Ｂs1、↑Ｓ1'_cmd、Ｊs1、Ｊs1'、↑ｑ'の計測値に置き換えた式の演算）を行なうことによって、↑τを算出し、この↑τの算出値を第１目標関節駆動トルク↑τ1_cmdとして決定する。
【０４３１】
この場合、↑τ1_cmdは、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Ｃにより規定される加速度に一致させつつ、第１種類状態量ベクトル↑Ｓ1の時間的変化率↑Ｓ1'を、ロボット１の全体重心点を適切な位置に保つように決定された第１種類状態量目標値ベクトル↑Ｓ1'_cmdに一致させるように決定される。
【０４３２】
また、状態量別制御入力決定部９５は、Ｂs2，Ｔc，↑τcmpnの算出値と、入力された第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1、第２状態量ヤコビアンＪs2及びイナーシャ行列Ｍと、↑ｑ'の計測値とを用いて、前記式５−２２の右辺の演算（詳しくは、式５−２２の右辺のＢs、↑Ｓ'、Ｊs、Ｊs'、↑ｑ'をそれぞれ、Ｂs2、↑Ｓ2'_cmd1、Ｊs2、Ｊs2'、↑ｑ'の計測値に置き換えた式の演算）を行なうことによって、↑τを算出し、この↑τの算出値を第２ＦＦ目標関節駆動トルク↑τ2_cmd1として決定する。
【０４３３】
この場合、↑τ2_cmd1は、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Ｃにより規定される加速度に一致させつつ、第２種類状態量ベクトル↑Ｓ2の時間的変化率↑Ｓ2'を、ロボットの１の各関節の実際の関節角を目標値に追従させるべくフィードフォワード則により決定した第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd2に一致させるように決定される。
【０４３４】
また、状態量別制御入力決定部９５は、Ｂs2，Ｔc，↑τcmpnの算出値と、入力された第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2、第２状態量ヤコビアンＪs2及びイナーシャ行列Ｍと、↑ｑ'の計測値とを用いて、前記式５−２２の右辺の演算（詳しくは、式５−２２の右辺のＢs、↑Ｓ'、Ｊs、Ｊs'、↑ｑ'をそれぞれ、Ｂs2、↑Ｓ2'_cmd2、Ｊs2、Ｊs2'、↑ｑ'の計測値に置き換えた式の演算）を行なうことによって、↑τを算出し、この↑τの算出値を第２ＦＢ目標関節駆動トルク↑τ2_cmd2として決定する。
【０４３５】
この場合、↑τ2_cmd2は、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Ｃにより規定される加速度に一致させつつ、第２種類状態量ベクトル↑Ｓ2の時間的変化率↑Ｓ2'を、ロボットの１の各関節の実際の関節角を目標値に追従させるべくフィードバック制御則（ＰＤ則）により決定した第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2に一致させるように決定される。
【０４３６】
また、状態量別制御入力決定部９５は、Ｂs3，Ｔc，↑τcmpnの算出値と、入力された第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1、第３状態量ヤコビアンＪs3及びイナーシャ行列Ｍと、↑ｑ'の計測値とを用いて、前記式５−２２の右辺の演算（詳しくは、式５−２２の右辺のＢs、↑Ｓ'、Ｊs、Ｊs'、↑ｑ'をそれぞれ、Ｂs3、↑Ｓ3'_cmd1、Ｊs3、Ｊs3'、↑ｑ'の計測値に置き換えた式の演算）を行なうことによって、↑τを算出し、この↑τの算出値を第３ＦＦ目標関節駆動トルク↑τ3_cmd1として決定する。
【０４３７】
この場合、↑τ3_cmd1は、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Ｃにより規定される加速度に一致させつつ、第３種類状態量ベクトル↑Ｓ3の時間的変化率↑Ｓ3'を、遊脚側脚体２の足平部１８の実際の位置及び姿勢をそれぞれ目標位置及び目標姿勢に追従させるべくフィードフォワード則により決定した第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1に一致させるように決定される。
【０４３８】
また、状態量別制御入力決定部９５は、Ｂs3，Ｔc，↑τcmpnの算出値と、入力された第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2、第３状態量ヤコビアンＪs3及びイナーシャ行列Ｍと、↑ｑ'の計測値とを用いて、前記式５−２２の右辺の演算（詳しくは、式５−２２の右辺のＢs、↑Ｓ'、Ｊs、Ｊs'、↑ｑ'をそれぞれ、Ｂs3、↑Ｓ3'_cmd2、Ｊs3、Ｊs3'、↑ｑ'の計測値に置き換えた式の演算）を行なうことによって、↑τを算出し、この↑τの算出値を第３ＦＢ目標関節駆動トルク↑τ3_cmd2として決定する。
【０４３９】
この場合、↑τ3_cmd2は、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Ｃにより規定される加速度に一致させつつ、第３種類状態量ベクトル↑Ｓ3の時間的変化率↑Ｓ3'を、遊脚側脚体２の足平部１８の実際の位置及び姿勢をそれぞれ目標位置及び目標姿勢に追従させるべくフィードバック制御則により決定した第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2に一致させるように決定される。
【０４４０】
なお、上記Ｊs1'（＝Ｊs1の１階微分値）、Ｊs2'（＝Ｊs2の１階微分値）、及びＪs3'（＝Ｊs3の１階微分値）は、それぞれ、第１状態量ヤコビアンＪs1の時系列、第２状態量ヤコビアンＪs2の時系列、第３状態量ヤコビアンＪs3の時系列から算出される。
【０４４１】
以上が本実施形態における状態量別制御入力決定部９５の処理の詳細である。
【０４４２】
また、本実施形態における合成制御入力決定部９６は、状態量別制御入力決定部９５で決定された、第１目標関節駆動トルク↑τ1_cmd、第２ＦＦ目標関節駆動トルク↑τ2_cmd1、第２ＦＢ目標関節駆動トルク↑τ2_cmd2、第３ＦＦ目標関節駆動トルク↑τ3_cmd1及び第３ＦＢ目標関節駆動トルク↑τ3_cmd2を逐次合成することによって、各関節の合成目標関節駆動トルク（合成目標関節駆動トルク↑τt_cmd）を逐次決定する。
【０４４３】
なお、本実施形態における↑τt_cmdは、第１実施形態のものと同様に、前記駆動力依存一般化力ベクトル↑τと同じ次数（一般化変数ベクトル↑ｑと同じ次数）のベクトルであるとする。
【０４４４】
本実施形態では、合成制御入力決定部９６は、次式６−８，６−９の演算処理によりそれぞれ算出されるベクトル↑τf1a，↑τf2aを、式６−７の演算処理により合成することによって、合成目標関節駆動トルク↑τt_cmdを決定する。
【０４４５】
↑τt_cmd＝Ｗ1＊↑τf1a＋Ｗ2＊↑τf2a      ……式６−７
ただし、
↑τf1a＝ｗc3＊↑τ3_cmd1＋Ｎuw3＊ｗc1＊↑τ1_cmd
＋Ｎuw3＊Ｎuw1＊ｗc2＊↑τ2_cmd1        
……式６−８
↑τf2a＝ｗc3＊↑τ3_cmd2＋Ｎuw3＊ｗc1＊↑τ1_cmd
＋Ｎuw3＊Ｎuw1＊ｗc2＊↑τ2_cmd2
……式６−９
【０４４６】
ここで、式６−７におけるＷ1，Ｗ2は、互いに異なる周波数通過特性を有する周波数重みである。この場合、本実施形態では、これらの周波数重みＷ1，Ｗ2の特性は、それぞれ第１実施形態のものと同一とされている。すなわち、Ｗ1，Ｗ2は、それぞれ、その伝達関数が前記式６−５，式６−６により表される周波数重みである。
【０４４７】
また、式６−８，式６−９におけるｗc1，ｗc2，ｗc3は、それぞれ“０”から“１”の間の範囲で設定された重み係数である。この場合、重み係数ｗc1は、第１種類状態量に係わる第１目標関節駆動トルク↑τ1_cmdに対応する重み係数であり、本実施形態では、固定的に“１”に設定されている。
【０４４８】
また、重み係数ｗc2は、第２種類状態量に係わる第２ＦＦ目標関節駆動トルク↑τ2_cmd1及び第２ＦＢ目標関節駆動トルク↑τ2_cmd2に対応する重み係数であり、本実施形態では、固定的に“１”に設定されている。
【０４４９】
また、重み係数ｗc3は、第３種類状態量に係わる第３ＦＦ目標関節駆動トルク↑τ3_cmd1及び第３ＦＢ目標関節駆動トルク↑τ3_cmd2に対応する重み係数であり、本実施形態では、例えば図８に示す如く可変的に設定される。
【０４５０】
具体的には、本実施系形態では、合成制御入力決定部９６は、図８に示す如く、遊脚側脚体２の足平部１８が空中を移動する期間（片脚支持期）において、ｗc3の値が、“０”から“１”まで連続的に増加し、続いて、“１”から“０”まで連続的に減少するように、重み係数ｗc3の値を設定する。そして、両脚体２Ｒ，２Ｌの足平部１８Ｒ，１８Ｌが着床する両脚支持期では、合成制御入力決定部９６は、重み係数ｗc3の値を定常的に“０”に維持する。
【０４５１】
なお、図８に示す例では、重み係数ｗc3の値は、片脚支持期において“１”まで連続的に増加した後、直ちに、連続的に減少するように設定されているが、ｗc3の値が片脚支持期の中間で“１”に保持される期間が設定されていてもよい。また、ｗc3の値の連続的な増加又は減少は、図８に示した如く直線的な形態で行なうことに限らず、曲線状の形態で（ｗc3の値の時間的変化率が徐々に変化するように）行なうようにしてもよい。
【０４５２】
また、前記式６−８，式６−９のＮuw1は、前記第１状態量ヤコビアンＪs1と、次式６−１０−１により定義される行列Ｊs1#と、重み係数ｗc1とに応じて、次式６−１０により定義される行列である。さらに、式６−８，式６−９のＮuw3は、前記第３状態量ヤコビアンＪs3と、次式６−１１−１により定義される行列Ｊs3#と、重み係数ｗc3とに応じて、次式６−１１により定義される行列である。なお、式６−１０−１は、前記式６−４−１と同じ式である。
【０４５３】
Ｎuw1≡Ｉ−ｗc1＊Ｊs1T＊Ｊs1#T      ……式６−１０
ただし、Ｊs1#≡ＷatT＊Ｊs1T＊(Ｊs1＊Ｗat＊Ｊs1T)-1
……式６−１０−１
Ｎuw3≡Ｉ−ｗc3＊Ｊs3T＊Ｊs3#T 
……式６−１１
ただし、Ｊs3#≡ＷatT＊Ｊs3T＊(Ｊs3＊Ｗat＊Ｊs3T)-1
……式６−１１−１
【０４５４】
なお、式６−１０−１及び式６−１１−１におけるＷatは、重み行列（対角行列）である。本実施形態では、第１実施形態と同様に、前記イナーシャ行列Ｍの逆行列Ｍ-1を重み行列Ｗatとして使用する。
【０４５５】
ここで、Ｎuw1、Ｎuw3が上記の如く定義される行列であるので、式６−８における第２項の一般化力ベクトル（Ｎuw3＊ｗc1＊↑τ1_cmd）は、ロボット１の各関節の関節駆動トルクを、ｗc31＊↑τ3_cmdに制御した場合と、ｗc31＊↑τ3_cmd1＋Ｎuw3＊ｗc1＊↑τ1_cmdに制御した場合とで、第３種類状態量ベクトル↑Ｓ3の制御状態を同じにすることができるような、一般化力ベクトルである。
【０４５６】
また、式６−８における第３項の一般化力ベクトル（Ｎuw3＊Ｎuw1＊ｗc21＊↑τ2_cmd1）は、ロボット１の各関節の関節駆動トルクを、（ｗc31＊↑τ3_cmd1＋Ｎuw3＊ｗc1＊↑τ1_cmd）に制御した場合と、上記式６−８により決定される↑τf1aに制御した場合とで、第３種類状態量ベクトル↑Ｓ3及び第１種類状態量ベクトル↑Ｓ1の制御状態を同じにすることができるような、一般化力ベクトルである。
【０４５７】
このため、式６−８により決定される↑τf1aは、第３種類状態量目標値ベクトル↑Ｓ3'_cmdを実現するための第３ＦＦ目標関節駆動トルク↑τ3_cmd1に、第３種類状態量ベクトル↑Ｓ3に影響を及ぼさない範囲で、第１種類状態量目標値ベクトル↑Ｓ1'_cmdを実現するための第１目標関節駆動トルク↑τ1_cmdの成分を重畳し（加え合わせ）、さらに、第３種類状態量ベクトル↑Ｓ3及び第１種類状態量ベクトル↑Ｓ1に影響を及ぼさない範囲で、第２種類状態量目標値ベクトル↑Ｓ2'_cmdを実現するための第２ＦＦ目標関節駆動トルク↑τ2_cmd1の成分を重畳し（加え合わせ）た目標関節駆動トルクとしての意味を持つものである。
【０４５８】
同様に、式６−９により決定される↑τf2aは、第３種類状態量目標値ベクトル↑Ｓ3'_cmdを実現するための第３ＦＢ目標関節駆動トルク↑τ3_cmd2に、第３種類状態量ベクトル↑Ｓ3に影響を及ぼさない範囲で、第１種類状態量目標値ベクトル↑Ｓ1'_cmdを実現するための第１目標関節駆動トルク↑τ1_cmdの成分を重畳し（加え合わせ）、さらに、第３種類状態量ベクトル↑Ｓ3及び第１種類状態量ベクトル↑Ｓ1に影響を及ぼさない範囲で、第２種類状態量目標値ベクトル↑Ｓ2'_cmdを実現するための第２ＦＢ目標関節駆動トルク↑τ2_cmd2の成分を重畳し（加え合わせ）た目標関節駆動トルクとしての意味を持つものである。
【０４５９】
以上の如く、本実施形態では、合成制御入力決定部９６は、重み係数ｗc1，ｗc2，ｗc3を含む前記６−８、式６−９により算出される↑τf1aと、↑τf2aとを前記式６−７により合成することによって、合成目標関節駆動トルク↑τt_cmdが決定される。
【０４６０】
この場合、合成目標関節駆動トルク↑τt_cmdは、↑τf1aの高周波側の成分（周波数重みＷ1の通過周波数帯の成分）と、↑τf2aの低周波側の成分（周波数重みＷ2の通過周波数帯の成分）とを合成したもの（加え合わせたもの）として決定される。そして、上記高周波側成分を組成する↑τf1aは、第３ＦＦ目標関節駆動トルク↑τ3_cmd1に、第１目標関節駆動トルク↑τ1_cmdのうちの↑Ｓ3'に影響を及ぼさない成分と、第２ＦＦ目標関節駆動トルク↑τ2_cmd1のうちの↑Ｓ3'及び↑Ｓ1'に影響を及ぼさない成分とを付加してなる目標関節駆動トルクとして決定される。また、上記低周波側成分を組成する↑τf2aは、第３ＦＢ目標関節駆動トルク↑τ3_cmd2に、第１目標関節駆動トルク↑τ1_cmdのうちの↑Ｓ3'に影響を及ぼさない成分と、第２ＦＢ目標関節駆動トルク↑τ2_cmd2のうちの↑Ｓ3'及び↑Ｓ1'に影響を及ぼさない成分とを付加してなる目標関節駆動トルクとして決定される。
【０４６１】
補足すると、本実施形態では、ロボット１の両脚支持期においては、重み係数ｗc3が“０”に設定されるので、Ｎuw3＝単位行列Ｉとなる。また、本実施形態では、ｗc1＝ｗc2＝１である。従って、両脚支持期においては、式６−８と式６−９とは、それぞれ、第３種類状態量に係わる目標関節駆動トルク↑τ3_cmd1，↑τ3_cmd2を除外してなる次式６−８ａ、次式６−９ａに一致することとなる。
【０４６２】
↑τf1a＝↑τ1_cmd＋Ｎuw1＊↑τ2_cmd1        ……式６−８ａ
↑τf2a＝↑τ1_cmd＋Ｎuw1＊↑τ2_cmd2        ……式６−９ａ
【０４６３】
従って、両脚支持期においては、第３種類状態量が実質的に制御対象から除外されることとなる。
【０４６４】
以上が本実施形態における合成制御入力決定部９６の処理の詳細である。そして、本実施形態は、以上説明した以外の事項は、前記第１実施形態と同じである。
【０４６５】
以上説明した本実施形態によれば、ロボット１の片脚支持期においては、前記合成目標関節駆動トルク↑τt_cmdの高周波側の成分（Ｗ1＊↑τf1a）は、第３ＦＦ目標関節駆動トルク↑τ3_cmd2の高周波側の成分主要成分として含み、↑τt_cmdの低周波側の成分（Ｗ2＊↑τf2a）は、第３ＦＢ目標関節駆動トルク↑τ3_cmd2の低周波側の成分を主要成分として含む。
【０４６６】
このため、前記第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1を達成すること（ロボット１の遊脚側脚体２の足平部１８の位置及び姿勢を目標とする位置及び姿勢にフィードフォワード則により制御すること）を高い速応性で行いつつ、長期的には、前記第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2を達成すること（ひいては、ロボット１の遊脚側脚体２の足平部１８の位置及び姿勢を目標とする位置及び姿勢にフィードバック制御すること）を満足し得るように、ロボット１の各関節の駆動トルクを制御することができる。
【０４６７】
これにより、遊脚側脚体２の足平部１８の運動を適切に行ない、該足平部１８の着地位置及び着地姿勢が目標とする位置及び姿勢からずれるのを効果的に防止することができる。
【０４６８】
さらに、片脚支持期において、合成目標関節駆動トルク↑τt_cmdの高周波側の成分（Ｗ1＊↑τf1a）は、第１目標関節駆動トルク↑τ1_cmdの高周波側の成分のうちの、第３種類状態量ベクトル↑Ｓ3に影響を及ぼさない成分と、第２ＦＦ目標関節駆動トルク↑τ2_cmd1の高周波側の成分のうちの、第３種類状態量ベクトル↑Ｓ3及び第１種類状態量ベクトル↑Ｓ1に影響を及ぼさない成分とを含んでいる。
【０４６９】
同様に、合成目標関節駆動トルク↑τt_cmdの低周波側の成分（Ｗ2＊↑τf2a）は、第１目標関節駆動トルク↑τ1_cmdの低周波側の成分のうちの、第３種類状態量ベクトル↑Ｓ3に影響を及ぼさない成分と、第２ＦＢ目標関節駆動トルク↑τ2_cmd2の高周波側の成分のうちの、第３種類状態量ベクトル↑Ｓ3及び第１種類状態量ベクトル↑Ｓ1に影響を及ぼさない成分とを含んでいる。
【０４７０】
このため、第３種類状態量ベクトル↑Ｓ3の時間的変化率↑Ｓ3'の制御性を損なうことなく、第１種類状態量ベクトル↑Ｓ1の時間的変化率↑Ｓ1'の制御性を高めることができると共に、第３種類状態量ベクトル↑Ｓ3の時間的変化率↑Ｓ3'の制御性と第１種類状態量ベクトル↑Ｓ1の時間的変化率↑Ｓ1'の制御性とを損なうことなく、第２種類状態量ベクトル↑Ｓ2の時間的変化率↑Ｓ2'の制御性を高めることができる。
【０４７１】
この場合、第１種類状態量ベクトル↑Ｓ1の時間的変化率↑Ｓ1'の制御性については、高周波側と低周波側と両方において、前記第１種類状態量目標値ベクトル↑Ｓ1'_cmdを達成すること（ひいては、ロボット１の全体重心の位置を目標位置に制御すること）を適切に実現できる。
【０４７２】
また、第２種類状態量ベクトル↑Ｓ2の時間的変化率↑Ｓ2'の制御性については、前記第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1を達成すること（ロボット１の各関節の関節角を目標とする関節角にフィードフォワード則により制御すること）を高い速応性で行いつつ、長期的には、前記第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2を達成すること（ロボット１の各関節の関節角を目標とする関節角にフィードバック制御すること）を実現できる。
【０４７３】
一方、ロボット１の両脚支持期においては、前記合成目標関節駆動トルク↑τt_cmdの高周波側の成分（Ｗ1＊↑τf1a）と、低周波側の成分（Ｗ2＊↑τf2a）とは、それぞれ、第１目標関節駆動トルク↑τ1_cmdの高周波側の成分、低周波側の成分を主要成分として含む。
【０４７４】
このため、高周波側と低周波側と両方において、前記第１種類状態量目標値ベクトル↑Ｓ1'_cmdを達成すること（ひいては、ロボット１の全体重心の位置を目標位置に制御すること）を適切に実現できる。
【０４７５】
さらに、両脚支持期において、合成目標関節駆動トルク↑τt_cmdの高周波側の成分（Ｗ1＊↑τf1a）は、第２ＦＦ目標関節駆動トルク↑τ2_cmd1の高周波側の成分のうちの、第１種類状態量ベクトル↑Ｓ1に影響を及ぼさない成分を含んでいる。同様に、合成目標関節駆動トルク↑τt_cmdの低周波側の成分（Ｗ2＊↑τf2a）は、第２ＦＢ目標関節駆動トルク↑τ2_cmd2の低周波側の成分のうちの、第１種類状態量ベクトル↑Ｓ1に影響を及ぼさない成分を含んでいる。
【０４７６】
このため、第１種類状態量ベクトル↑Ｓ1の時間的変化率↑Ｓ1'の制御性を損なうことなく、前記第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1を達成すること（ロボット１の各関節の関節角を目標とする関節角にフィードフォワード則により制御すること）を高い速応性で行いつつ、長期的には、前記第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2を達成すること（ロボット１の各関節の関節角を目標とする関節角にフィードバック制御すること）を実現できる。
【０４７７】
また、両脚支持期において、第３種類状態量が実質的に制御対象から除外され、合成目標関節駆動トルク↑τt_cmdは、第３ＦＦ目標関節駆動トルク↑τ3_cmd1及び第３ＦＢ目標関節駆動トルク↑τ3_cmd2に依存しないものとなる。このため、両脚支持期における各足平部１８、すなわち、床面と接触して拘束を受けた状態の足平部１８の位置及び姿勢を強制的に目標とする位置及び姿勢に修正するような制御が行なわれないようにすることができる。このため、電動モータ５０に過大な動力（トルク）が要求されることとなるような事態が発生するのを防止することができる。
【０４７８】
さらに、前記重み係数ｗc3を、その値が連続的に変化するように設定しているので、第３種類状態量を制御対象として含む片脚支持期と、該第３種類状態量が制御対象から除外される両脚支持期との切替わりの前後で、合成目標関節駆動トルク↑τt_cmdが不連続的に変化するようなことを防止し、該↑τt_cmdを連続的に変化させるようにすることができる。その結果、ロボット１の動作をスムーズに行なうことができる。
【０４７９】
また、本実施形態では、前記式５−２０に基づいて、目標関節駆動トルク↑τ1_cmd，↑τ2_cmd1，↑τ2_cmd2、↑τ3_cmd1，↑τ3_cmd2が決定される。このため、第１実施形態と同様に、ロボット１に作用する外力の値（これは外乱等の影響を受けやすい）を特定することなく、↑Ｓ1'_cmd、↑Ｓ2'_cmd1、↑Ｓ2'_cmd2，↑Ｓ3'_cmd1，↑Ｓ3'_cmd2をそれぞれ実現し得る適切な↑τ1_cmd，↑τ2_cmd1，↑τ2_cmd2、↑τ3_cmd1，↑τ3_cmd2を決定できる。
【０４８０】
ここで、以上説明した実施形態と本発明との対応関係について補足しておく。
【０４８１】
本実施形態では、前記状態量別制御入力決定部９５により、本発明における状態量別制御入力決定手段が実現される。この場合、前記第１種類状態量、第２種類状態量及び第３種類状態量が本発明における複数種類の状態量に相当する。従って、本実施形態では、制御対象とする状態量の種類数はＭは“３”である。
【０４８２】
そして、第１種類状態量目標値ベクトル↑Ｓ1'_cmdが第１種類状態量に関する２つの目標値に相当し、前記第１目標関節駆動トルク↑τ1_cmdが↑Ｓ1'_cmdを実現するための状態量別制御入力に相当する。
【０４８３】
また、第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1及び第２種類状態量ＦＢ目標値ベクトル↑Ｓ2'_cmd2が第２種類状態量に関する２つの目標値に相当し、第２ＦＦ目標関節駆動トルク↑τ2_cmd1及び第２ＦＢ目標関節駆動トルク↑τ2_cmd2がそれぞれ、↑Ｓ2'_cmd1、↑Ｓ2'_cmd2を実現するための状態量別制御入力に相当する。
【０４８４】
また、第３種類状態量ＦＦ目標値ベクトル↑Ｓ3'_cmd1及び第３種類状態量ＦＢ目標値ベクトル↑Ｓ3'_cmd2が第３種類状態量に関する２つの目標値に相当し、第３ＦＦ目標関節駆動トルク↑τ3_cmd1及び第３ＦＢ目標関節駆動トルク↑τ3_cmd2がそれぞれ、↑Ｓ3'_cmd1、↑Ｓ3'_cmd2を実現するための状態量別制御入力に相当する。
【０４８５】
また、前記合成制御入力決定部９５により、本発明における合成制御入力決定手段が実現される。この場合、前記式６−７により表される処理により算出される合成目標関節駆動トルク↑τt_cmdが、本発明における合成制御入力τtotalに相当し、式６−７のＷ1＊↑τf1aと、Ｗ2＊↑τf2aとがそれぞれ、本発明における周波数領域別制御入力（Ｗ(i)＊τf(i)）に相当し、周波数重みＷ1，Ｗ2のそれぞれの通過可能な周波数帯が、本発明における周波数領域に相当する。なお、本実施形態では、周波数領域の個数ｎは２つである。
【０４８６】
また、本実施形態では、↑τf1a、↑τf2aがそれぞれ、本発明における（前記式０１、０２ｂにおける）τf(i)に相当する。この場合、↑τ3_cmd1，↑τ1_cmd，↑τ2_cmd1がそれぞれ、↑τf1a（τf(1)）の組成成分τ(1,1)、τ(1,2)、τ(1,3)に相当し、行列Ｎuw3、Ｎuw1が本発明におけるＮuw(1,1)、Ｎuw(1,2)に相当し、重み係数ｗc3、ｗc1、ｗc2がそれぞれ、本発明におけるｗc(1,1)、ｗc(1,2)、ｗc(1,3)に相当する。
【０４８７】
また、↑τ3_cmd2，↑τ1_cmd，↑τ2_cmd2がそれぞれ↑τf2a(τf(2))の組成成分τ(2,1)、τ(2,2)、τ(2,3)に相当する。この場合、↑τ3_cmd2，↑τ1_cmd，↑τ2_cmd2がそれぞれ、↑τf2a（τf(2)）の組成成分τ(2,1)、τ(2,2)、τ(2,3)に相当し、行列Ｎuw3、Ｎuw1が本発明におけるＮuw(2,1)、Ｎuw(2,2)に相当し、重み係数ｗc3、ｗc1、ｗc2がそれぞれ、本発明におけるｗc(2,1)、ｗc(2,2)、ｗc(2,3)に相当する。
【０４８８】
さらに、本実施形態では、行列Ｎuw3、Ｎuw1のそれぞれの階数rank(Ｎuw3)、rank(Ｎuw1)のうちのrank(Ｎuw3)は、ロボット１のほとんどの姿勢の場合、５又は６であり、rank(Ｎuw1)は６である。このため、rank(Nuw(1,1))≧rank(Nuw(1,2))という関係、並びに、rank(Nuw(2,1))≧rank(Nuw(2,2))という関係が実現される。
【０４８９】
なお、本実施形態では、↑τf1a（τf(1)）の組成成分の個数ｍ(1)と、↑τf2a（τf(2)）の組成成分の個数ｍ(2)とはいずれも“１”である。
【０４９０】
また、本実施形態では、前記行列Ｊs1#、Ｊs2#が、それぞれ本発明におけるＪs#(i,j)に相当し、Ｊs1、Ｊs2がそれぞれ、本発明におけるＪs(i,j)に相当し、↑Ｓ1，↑Ｓ2，↑Ｓ3がそれぞれ、本発明におけるＳ(i,j)に相当する。また、本実施形態におけるＭ、↑ｑがそれぞれ、本発明におけるイナーシャ行列Ｍ、一般化変数ベクトルｑに相当する。なお、本実施形態では、本発明における重み行列Ｗdを単位行列とし、Ｗat＝Ｍ-1としている。
【０４９１】
また、本実施形態における前記コンプライアンス制御部９７及び関節駆動制御部７３により、本発明におけるアクチュエータ制御手段が実現される。
【０４９２】
また、本実施形態では、脚体２Ｒ，２Ｌが本発明における所定の部位に相当し、ロボット１の歩行動作時の床が本発明における接触対象部に相当する。そして、前記第３種類状態量（↑Ｓ3）が本発明における所定の部位の運動状態量に相当し、第３ＦＦ目標関節駆動トルク↑τ3_cmd1及び第３ＦＢ目標関節駆動トルク↑τ3_cmd2が本発明における第(ia,ja)状態量別制御入力τ(ia,ja)に相当する。また、ｗc3が、τ(ia,ja)に対応する重み係数に相当する。
【０４９３】
［変形態様について］
次に、以上説明した実施形態の変形態様をいくつか説明しておく。
【０４９４】
本実施形態では、ロボット１を、脚式移動ロボットである２足歩行ロボットとしたが、本発明におけるロボットは、脚式移動ロボット以外の移動ロボット（例えば、車輪により移動する形態のロボット）でもよい。あるいは、本発明におけるロボットは、いずれかのリンクが床等に固定された設置型のロボットであってもよい。
【０４９５】
また、ロボット１のアクチュエータは、電動モータ５０に限らず、油圧式のアクチュエータであってもよい。
【０４９６】
また、ロボット１の歩行動作を行う場合に限らず、走行動作を行なったり、あるいは、ハンド３０Ｒ，３０Ｌによる作業と移動動作と並行して行なうような場合にも本発明を適用できる。
【０４９７】
本発明における前記式０１及び０１ａを適用した前記第１実施形態では、制御対象の状態量を第１種類状態量及び第２種類状態量の２種類としたが、より多くの種類の状態量を制御対象してもよい。より具体的な例を挙げると、例えば、前記第１種類状態量、第２種類状態量に加えて、ロボット１の頭部５の姿勢（ピッチ軸周りとヨー軸周りとの２軸周りの姿勢）の時間的変化率（角速度）を制御対象の状態量に加えてもよい。この場合の合成目標関節駆動トルク↑τt_cmd（合成制御入力）の算出例を以下に示す。
【０４９８】
ロボット１の頭部５のロボット１の頭部５の姿勢の角速度（ここではピッチ軸周りとヨー軸周りとの２軸周りの角速度）を制御対象の第４種類状態量とし、第４種類状態量の２つの成分を並べたベクトルを第４種類状態量ベクトル↑Ｓ4、この↑Ｓ4と一般化変数ベクトル↑ｑの１階微分値↑ｑ'との間の関係を、次式３−４により表現するヤコビアンを第４状態量ヤコビアンＪs4とする。
【０４９９】
↑Ｓ4＝Ｊs4＊↑ｑ'      ……式３−４
【０５００】
そして、↑Ｓ4の１階微分値↑Ｓ4'の目標値↑Ｓ4'_cmdを第４種類状態量に関する目標値とし、この↑Ｓ4'_cmdを、頭部４の実際の姿勢の計測値Ｘhead_actと該姿勢に対する目標値Ｘhead_cmdとの偏差ΔＸhead（＝Ｘhead_act−Ｘhead_cmd）と、該偏差ΔＸheadの１階微分値ΔＸhead'とから、フィードバック制御則、例えばＰＤ則により逐次決定する。すなわち、次式４−６により↑Ｓ4'_cmdを逐次決定する。
【０５０１】
↑Ｓ4'_cmd＝Kp5＊↑ΔＸhead＋Kv5＊↑ΔＸhead'      ……式４−６
【０５０２】
さらに、この式４−６により算出される目標値↑Ｓ4'_cmd（以降、第４種類状態量目標値ベクトル↑Ｓ4'_cmdという）を実現するための目標関節駆動トルク↑τ4_cmd（以降、第４目標関節駆動トルク↑τ4_cmdという）を、第１実施形態で説明した↑τ1_cmd及び↑τ2_cmdの算出手法を同じ手法によって（前記式５−２２の右辺の演算によって）、決定する。
【０５０３】
そして、例えば、次式６−１３，６−１４の演算処理によりそれぞれ算出されるベクトル↑τf1b、↑τf2bを、式６−１２の演算処理により合成することによって、合成目標関節駆動トルク↑τt_cmdを決定する。
【０５０４】
↑τt_cmd＝Ｗ1＊↑τf1b＋Ｗ2＊↑τf2b      ……式６−１２
ただし、
↑τf1b＝↑τ1_cmd＋Ｎu1＊↑τ2_cmd1    ……式６−１３
↑τf2b＝↑τ1_cmd＋Ｎu1＊↑τ4_cmd
＋Ｎu1＊Ｎu4＊↑τ2_cmd2    ……式６−１４
【０５０５】
なお、この例では、↑τ1_cmdは、第１実施形態のものと同じ第１目標関関節駆動トルクであり、↑τ2_cmd1，↑τ2_cmd2は、それぞれ第２実施形態のものと同じ第２ＦＦ目標関節駆動トルク、第２ＦＢ目標関節駆動トルクである。また、Ｗ1，Ｗ2は、第１実施形態と同じ周波数重みである。
【０５０６】
また、行列Ｎu1は、第１実施形態における前記式６−４により定義される行列であり、行列Ｎu4は、前記第４状態量ヤコビアンＪs4と、次式６−１５−１により定義される行列Ｊs4#とに応じて、次式６−１５により定義される行列である。
【０５０７】
Ｎu4≡Ｉ−Ｊs4T＊Ｊs4#T      ……式６−１５
【０５０８】
ただし、Ｊs4#≡ＷatT＊Ｊs4T＊(Ｊs4＊Ｗat＊Ｊs4T)-1
……式６−１５−１
【０５０９】
なお、式６−１５−１における重み行列Ｗatとしては、前記イナーシャ行列Ｍの逆行列Ｍ-1を使用する。
【０５１０】
以上のようにして、第１種類状態量に関する第１種類状態量目標値ベクトル↑Ｓ1'_cmdを実現するための↑τ1_cmdと、第４種類状態量に関する第４種類状態量目標値ベクトル↑Ｓ4'_cmdを実現するための↑τ4_cmdと、第２種類状態量に関する第２種類状態量ＦＦ目標値ベクトル↑Ｓ2'_cmd1，↑Ｓ2'_cmd2をそれぞれ実現するための↑τ2_cmd1，↑τ2_cmd2とを合成した合成目標関節駆動トルク↑τt_cmdを決定できる。
【０５１１】
なお、この場合、式６−１４において、行列Ｎu1（これは本発明の式０２ａにおけるτ(2,1)に相当する）の階数は、ロボットのほとんどの姿勢の場合、５又は６、行列Ｎu4（これは本発明の式０２ａにおけるτ(2,2)に相当する）の階数は２であるから、rank(Ｎu1)≧rank(Ｎu4)という関係が成立することとなる。
【０５１２】
また、本発明における前記式０１及び０１ｂを適用した前記第２実施形態では、制御対象の状態量を第１種類状態量、第２種類状態量、第３種類状態量の３種類としたが、例えば、制御対象の状態量の種類数は、２種類であってもよく、あるいは、さらに多くの状態量（例えば、前記第４種類状態量）を制御対象に含めるようにしてもよい。
【０５１３】
また、前記各実施形態では、接触部位代表点目標加速度↑Ｃを“０”としたが、接触部位代表点a1〜a4の他の運動形態の目標を↑Ｃとして設定するようにしてもよい。例えば、接触部位代表点a1〜a4の運動パターンを正弦波状のパターンに設定してもよい。また、例えば、ロボット１がエレベータ等、加速度が発生する系に存在するような場合には、その系の加速度（慣性座標系から見た加速度）を接触部位代表点目標加速度↑Ｃに付加するようにしてもよい。
【０５１４】
また、前記コンプライアンス制御部８７，９７を省略してもよい。すなわち、合成制御入力決定部８７，９７が決定した目標関節駆動トルク↑τt_cmdをそのまま使用して、ロボット１の各関節の駆動トルクを制御するようにしてもよい。
【０５１５】
また、足平部１８Ｒ，１８Ｌの接地時における足平部１８Ｒ，１８Ｌや床面の弾性変形が十分に微小なものとなる場合には、前記接触部弾性変形補償部８８，９８を省略し、歩容生成部７１で生成された目標関節角を状態量目標値決定部８４，９４に入力するようにしてもよい。
【産業上の利用可能性】
【０５１６】
脚式移動ロボット等のロボットの種々様々な動作状況で、複数種類の状態量をそれぞれに対応する目標値に効率よく制御するようにロボットの動作制御を行なうことができる。
【符号の説明】
【０５１７】
１…ロボット、２…上体リンク、３…脚リンク、４…腕リンク、５…頭部（リンク）、１０…下側上体（リンク）、１１…上側上体（リンク）、１２，１９〜２４，３１〜３７，３９，４０…関節、１４…大腿部（リンク）、１６…下腿部（リンク）、１８…足平部（リンク）、２６…上腕部（リンク）、２８…前腕部（リンク）、３０…ハンド（リンク）、６０…制御装置、７３…関節駆動制御部（アクチュエータ制御手段）、８５，９５…状態量別制御入力決定部（状態量別制御入力決定手段）、８６，９６…合成制御入力決定部（合成制御入力決定手段）、８７，９７…コンプライアンス制御手段（アクチュエータ制御手段）。
【図１】
0005506823.tif 000013
【図２】
0005506823.tif 000014
【図３】
0005506823.tif 000015
【図４】
0005506823.tif 000016
【図５】
0005506823.tif 000017
【図６】
0005506823.tif 000018
【図７】
0005506823.tif 000019
【図８】
0005506823.tif 000020