(57)【特許請求の範囲】
【請求項１】
互いの相対高さを操作可能に複数の関節を介して連結された３つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、
前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類し、
複数の接地部位を子孫ノードとして持つノードである第Ｃノードのそれぞれに対し、少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が０に近づくように、該第Ｃノードに属する複数の接地部位の間の目標相対高さの修正量を該第Ｃノード毎に決定し、その決定した修正量をすべての第Ｃノードについて合成してなる修正量を基に、前記目標運動に、各第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備えたことを特徴とする移動体の制御装置。
【請求項２】
前記ツリー構造における各ノードに重みが設定されており、該ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第Ｂノードの高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第Ｂノードの高さとして定義したとき、
前記ノード動作制御手段は、各第Ｃノードのうちの任意の一つのノードのみに対して、該一つのノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記修正目標運動を決定したとき、該修正目標運動における該一つのノードの目標高さは前記目標運動における目標高さに維持されることを特徴とする請求項１記載の移動体の制御装置。
【請求項３】
互いの相対高さを操作可能に複数の関節を介して連結された３つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、
前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類したとき、前記ツリー構造における各ノードに重みが設定されており、
少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が０に近づくように、前記目標運動に、所定の第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備え、
前記ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第Ｂノードの高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第Ｂノードの高さとして定義したとき、前記ノード動作制御手段が、前記第Ｃノードのみに対して、該第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記修正目標運動を決定したとき、該修正目標運動における該第Ｃノードの目標高さは前記目標運動における目標高さに維持されることを特徴とする移動体の制御装置。
【請求項４】
前記ツリー構造において子ノードを持つ各ノードに対し、該各ノードの全ての子ノードの重みは、その全ての子ノードの重みの総和が１となるように設定されていることを特徴とする請求項２または３記載の移動体の制御装置。
【請求項５】
前記各ノードの重みを可変的に設定する手段を備え、非接地状態である各接地部位にそれぞれ対応する葉ノードを第Ｄノードとしたとき、前記重みを設定する手段は、各第Ｄノードの重み、または該第Ｄノードの祖先ノードのうちの少なくともいずれか１つのノードの重みを０に設定することを特徴とする請求項２または３記載の移動体の制御装置。
【請求項６】
前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結されており、
前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記第Ｃノードに属する各接地部位の床面上でのこじりなどの滑りを抑制するための、前記基体の目標姿勢の修正を前記目標運動にさらに付加して前記修正目標運動を決定することを特徴とする請求項１または２記載の移動体の制御装置。
【請求項７】
前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結されており、
前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記第Ｃノードに属する各接地部位の床面上でのこじりなどの滑りを抑制するための、前記基体の目標姿勢の修正をさらに付加して前記修正目標運動を決定することを特徴とする請求項３記載の移動体の制御装置。
【請求項８】
前記ノード動作制御手段は、前記修正目標運動における前記移動体の全体重心の水平位置または該移動体の基体の所定の代表点の水平位置が前記目標運動における当該水平位置とほぼ同じになるように前記修正目標運動を決定することを特徴とする請求項６または７記載の移動体の制御装置。
【請求項９】
前記移動体は、その基体から延設された複数のリンク機構を備えるロボットであり、該複数のリンク機構のうちの少なくとも１つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記接地部位となっていることを特徴とする請求項６記載の移動体の制御装置。
【請求項１０】
前記移動体は、その基体から延設された複数のリンク機構を備えるロボットであり、該複数のリンク機構のうちの少なくとも１つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記第Ｃノードに属する接地部位となっていることを特徴とする請求項７記載の移動体の制御装置。
【請求項１１】
前記中間部位には、その接地時に弾性変形する弾性体が設けられていることを特徴とする請求項９または１０記載の移動体の制御装置。
【請求項１２】
前記移動体は、その基体から延設されて１つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも１つのリンク機構と前記基体とに前記接地部位が設けられていることを特徴とする請求項６記載の移動体の制御装置。
【請求項１３】
前記移動体は、その基体から延設されて１つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも１つのリンク機構と前記基体とに前記接地部位が設けられており、前記第Ｃノードに属する接地部位には、少なくとも前記基体の接地部位が含まれることを特徴とする請求項７記載の移動体の制御装置。
【請求項１４】
前記基体の少なくともひとつの接地部位には、その接地時に弾性変形する弾性体が設けられていることを特徴とする請求項１２または１３記載の移動体の制御装置。
【請求項１５】
前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結されており、
前記修正目標運動を満足するように前記関節を動作させたときに前記第Ｃノードに属する各接地部位に作用すべき目標床反力の並進力成分を決定する手段を備え、
前記ノード動作制御手段は、少なくとも前記第Ｃノードに属する各接地部位の目標床反力の並進力成分を基に、該目標床反力の並進力成分が該第Ｃノードに属する各接地部位に作用したときに前記基体と該第Ｃノードに属する各接地部位との間の連結機構および該接地部位で発生する変形量を推定する手段を備え、その推定した変形量を打ち消すための、前記第Ｃノードに属する複数の接地部位の目標高さの第２の修正を前記目標運動にさらに付加して前記修正目標運動を決定することを特徴とする請求項１または３記載の移動体の制御装置。
【請求項１６】
少なくとも前記第Ｃノードに属する複数の接地部位の接地面の間の相対高さを規定するパラメータを床形状を表す床形状パラメータとして推定する床形状推定手段を備え、
前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記前記床形状パラメータの推定値に応じた、前記第Ｃノードに属する複数の接地部位の間の目標相対高さの第３の修正をさらに付加して前記修正目標運動を決定することを特徴とする請求項１または３記載の移動体の制御装置。
【請求項１７】
前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、
前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードに属する各接地部位の実床反力とに基づき、前記床形状パラメータを逐次更新しつつ推定することを特徴とする請求項１６記載の移動体の制御装置。
【請求項１８】
前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備えると共に、
前記ツリー構造における各ノードである第Ａノードに対し、該第Ａノードが前記葉ノードである場合には、該葉ノードである接地部位に作用する床反力を第Ａノードのノード床反力と定義し、且つ、第Ａノードが子ノードを持つ場合には、該第Ａノードのすべての子ノードのノード床反力の合力を第Ａノードのノード床反力と定義したとき、少なくとも前記第Ｃノードの各子ノードのノード床反力の実際値である実ノード床反力を該第Ｃノードに属する各接地部位の実床反力から逐次決定する手段を備え、
前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードの各子ノードの実ノード床反力の間の相対関係とに基づき、前記床形状パラメータを逐次更新しつつ推定することを特徴とする請求項１６記載の移動体の制御装置。
【請求項１９】
前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結され、
該基体と各接地部位との間の連結機構および該接地部位で発生する変形量を検出または推定する変形量検出手段を備え、
前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードに属する各接地部位に対応する前記変形量とに基づき、前記床形状パラメータを逐次更新しつつ推定することを特徴とする請求項１６記載の移動体の制御装置。
【請求項２０】
前記第Ｃノードに属する前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、前記変形量検出手段は、該実床反力に基づき前記変形量を推定することを特徴とする請求項１９記載の移動体の制御装置。
【請求項２１】
前記ツリー構造における各ノードの重みが設定されており、該ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第Ｂノードの接地面の高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第Ｂノードの接地面の高さとして定義したとき、
前記床形状推定手段が推定する前記床形状パラメータは、前記第Ｃノードの複数の各子ノードの接地面の高さの間の相対関係を規定するパラメータであることを特徴とする請求項１６記載の移動体の制御装置。
【請求項２２】
前記床形状推定手段は、前記第Ｃノードの各子ノードのうちの少なくとも１つが浮きそうな状態であるときに、その浮きそうな子ノードの接地面の相対高さを一定値に保持しつつ、前記複数の子ノードの接地面の高さの相対関係を規定する前記床形状パラメータを推定することを特徴とする請求項２１記載の移動体の制御装置。
【請求項２３】
前記床形状推定手段は、前記床形状パラメータの変動を緩和するようにローパスフィルタを用いて該床形状パラメータを推定することを特徴とする請求項１６記載の移動体の制御装置。
【請求項２４】
前記ツリー構造の各ノードには床形状の推定用の重みが設定されており、
前記ツリー構造における各葉ノードに高さ、床反力などの所定の種類の状態量を対応づけたときに、子ノードを持つ各ノードの状態量を、該ノードの全ての子ノードの状態量の、前記重みを用いた重み付き平均値とし、根ノードを除く各ノードに対し、該ノードの状態量から該ノードの親ノードの状態量を減算してなる値を該ノードのノード相対状態量として決定すると共に０を根ノードの相対状態量として決定する処理を、当該所定の種類の状態量を各ノードについて階層相対化する処理として定義し、
前記葉ノードである接地部位のそれぞれに作用する床反力Fn(n=1,2,…)を基に、各ノードについて階層相対化したノード相対床反力Fn_rel(n=1,2,…)を求め、さらに子ノードを持つ任意のノードである第ｎノードのすべての子ノードaj (j＝1,2,…,r。ｒは第ｎノードの子ノードの総数)のノード相対床反力を要素とするベクトル(Fa1_rel,Fa2_rel,…,Far_rel)を、第ｎノードのすべての子ノードの重みを要素とするベクトル(Wa1,Wa2,…,War)にそれぞれ直交する所定の互いに独立な複数のベクトルR(j) (j=1,2,…,r−1)の線形結合で表したときの該線形結合の係数を要素とするベクトルを、第ｎノードのノード拡張床反力モーメントMn_expと定義し、
また、前記葉ノードである接地部位のそれぞれの接地面の高さZn (n=1,2,…)を基に、各ノードについて階層相対化したノード相対高さZn_rel(n=1,2,…)を求め、さらに前記第ｎノードのすべての子ノードaj (j=1,2,…,r。ｒは第ｎノードの子ノードの総数)のノード相対高さを要素とするベクトル(Za1_rel, Za2_rel,…,Zar_rel)を、前記所定の互いに独立な複数のベクトルR(j) (j=1,2,…,r-1)の線形結合で表したときの線形結合の係数を要素とするベクトルを、第ｎノードのノード拡張傾斜角θnと定義したとき、
前記床形状推定手段は、少なくとも前記第Ｃノードの前記ノード拡張床反力モーメントMn_exp(n=C)またはノード拡張傾斜角θn(n=C)を用いて前記床形状パラメータを推定することを特徴とする請求項１６記載の移動体の制御装置。
【請求項２５】
前記ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第Ｂノードの接地面の高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第Ｂノードの接地面の高さとして定義したとき、
前記床形状パラメータに、前記第Ｃノードの各子ノードの接地面の相対高さを前記ノード拡張傾斜角を用いて表したパラメータが含まれることを特徴とする請求項２４記載の移動体の制御装置。
【請求項２６】
前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、
前記ノード動作制御手段は、少なくとも前記各接地部位の実床反力を基に前記ノード拡張モーメントを逐次決定する手段と、この決定したノード拡張モーメントに所定の行列を乗じることにより前記ノード拡張傾斜角を逐次決定する手段とを備え、前記決定したノード拡張傾斜角と、前記床形状パラメータの推定値の過去値とを基に、新たな床形状パラメータを推定することを特徴とする請求項２４記載の移動体の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、脚式移動ロボット、アクティブサスペンションを持つ車輪型移動体などの移動体の床反力制御に関する。より具体的には脚式移動ロボット、アクティブサスペンションを持つ車輪型移動体などの移動体が接地する床（地面）に未知の傾斜や凹凸がある場合にも、その影響を受けにくく、適切に床反力を制御する装置に関する。さらに床形状を推定しながら、適切に床反力を制御する装置に関する。
【背景技術】
【０００２】
本願出願人による特開平５−３０５５８５号公報（特許文献１）には２足移動ロボットの両脚のコンプライアンス制御技術が開示されている。この技術は、４脚等のロボットにも適用可能なものである。この場合、同公報の技術のコンプラインス制御では、全脚を仮想平面上に接触させた状態で、仮想平面を傾ける動作を行なうので、床の大域的なうねりや傾斜には対応できる。しかし、同公報の技術では、局所的な床の傾斜や凹凸があると、荷重（床反力）の配分が適切に行われない場合があった。その結果、接地すべき面が浮いて接地部位（足平）が揺れたり、着地衝撃が大きく、足平の接地性の低下することがあった。さらにその結果、ロボットの滑りやスピンが発生し易くなる恐れがあった。また、ロボットの姿勢を所望の姿勢に復元させる床反力モーメントが適切に各脚先端部に分配されるように制御できず、ロボットの姿勢が揺れることがあった。また、ロボットのある部位が振動し始めると、その振動が減衰し難くいという不具合が生じることもあった。また、床反力の各脚先端部への分配制御が適切に行なわれないために、一部のアクチュエータの負荷が過大になる場合もあった。
【０００３】
さらに、本願出願人による特開平１０−２７７９６９号公報（特許文献２）、特開２００１−３２２０７６号公報（特許文献３）には、複合コンプライアンス制御の技術、および床形状の推定技術が開示されている。これらの技術では、２足移動ロボットにおいて、各脚先端部への床反力の適切な分配ができ、かつ、床形状を複合的に、具体的には各脚先端部の接地面傾斜と各接地面間の高低差とを同時に複合的に、精度良く推定することができた。これらの公報の技術は、原理的には４脚ロボット等の多脚ロボットに対応可能なものではあるが、それを具体的に適用するための技術については、記述されていない。
【０００４】
また、４脚ロボットの脚先端に備えた触覚（荷重センサまたは接触センサ）を用いて着地時に床の高さを推定しようとする手法も知られている。しかし、この手法は、シーケンス的な手法であり、軟弱地盤のように床が着地後に変形するような場合には対応できなかった。特に、ロボットの高速移動時等、ロボットの本体が激しく振動しているときには、床の高さを正しく推定できず、各脚への荷重の分配が適切に行われない恐れがあった。
【０００５】
また、ロボットの各脚のコンプライアンス制御を脚毎に独立に行う技術も知られている。しかし、この技術では、ロボットの上体の揺れを通して脚同士の干渉が生じるにもかかわらず、このことが考慮されていない。すなわち、各脚のコンプライアンス制御が個別に行なわれる。このため、各脚のコンプライアンス制御が互いに協調し合うことがないので、適切に各脚に床反力を分配することができなかった。
【特許文献１】特開平５−３０５５８５号公報
【特許文献２】特開平１０−２７７９６９号公報
【特許文献３】特開２００１−３２２０７６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
本発明はかかる背景に鑑みてなされたものであり、脚式移動ロボットや、アクティブサスペンションを持つ車輪型移動体などの移動体が接地する床に未知の傾斜や凹凸がある場合にも、その影響を受けにくく適切に床反力を制御する装置を提供することを目的とする。さらに床形状を精度よく推定しながら、床反力を適切に制御できる装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明の移動体の制御装置の第１発明は、互いの相対高さを操作可能に複数の関節を介して連結された３つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、
前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類し、
複数の接地部位を子孫ノードとして持つノードである第Ｃノードのそれぞれに対し、少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が０に近づくように、該第Ｃノードに属する複数の接地部位の間の目標相対高さの修正量を該第Ｃノード毎に決定し、その決定した修正量をすべての第Ｃノードについて合成してなる修正量を基に、前記目標運動に、各第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備えたことを特徴とするものである。
【０００８】
なお、本発明においては、子ノードを持つ各ノードに対し、その「ノードに属する接地部位」とは、より正確に言えば、その「ノードに子孫ノードとして属する」ということを意味するものである。このことは、第１発明に限らず、本願のいずれの発明についても同じである。
【０００９】
かかる第１発明によれば、移動体が備える３つ以上の接地部位をツリー構造状に分類し、子ノードを持つ第Ｃノードのそれぞれに対し、姿勢傾斜偏差が０に近づくように、該第Ｃノードに属する複数の接地部位の間の目標相対高さの修正量（換言すれば操作量）を該第Ｃノード毎に決定する。そして、それらの修正量を全ての第Ｃノードについて合成してなる修正量を基に、前記目標運動に、各第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を付加した修正目標運動を決定する。そして、その決定した修正目標運動を満足するように移動体の関節を動作させる。これにより、個々の接地部位の高さをそれぞれ独立に（各別）の操作する場合に較べて、複数の接地部位の床反力の相互のバランスを採りながら、それらの接地部位の相対高さを前記姿勢傾斜偏差を０に近づけるように操作することができる。その結果、移動体が接地する床に局所的に未知の傾斜や凹凸がある場合にも、その影響を受けにくくなり、ロボットの姿勢の安定性を高めることができる。
【００１０】
かかる第１発明では、好ましくは、前記ツリー構造における各ノードに重みが設定されており、該ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第Ｂノードの高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第Ｂノードの高さとして定義したとき、
前記ノード動作制御手段は、各第Ｃノードのうちの任意の一つのノードのみに対して、該一つのノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記修正目標運動を決定したとき、該修正目標運動における該一つのノードの目標高さは前記目標運動における目標高さに維持される（第２発明）。
【００１１】
この第２発明によれば、各第Ｃノードの目標高さは、この第Ｃノードの子ノードの目標高さ（これは、第Ｃノードに属する接地部位の、前記目標運動における目標高さに応じて定まる）の重み付き平均値となり、各第Ｃノードのうちの任意の一つのノードのみに対して、該一つのノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記修正目標運動を決定したときには、前記一つのノードの子ノードの上記重み付き平均値が、修正目標運動と目標運動とで同じになるように前記第１の修正が目標運動に付加される。
【００１２】
この結果、各第Ｃノード毎に、それに属する複数の接地部位の床反力の相互のバランスを採りながら、各Ｃノードに属する複数の接地部位の間の目標相対高さを修正することができることとなる。ひいては、ロボットの姿勢の安定性をさらに高めることができる。
【００１３】
また、本発明の移動体の制御装置の第３発明は、互いの相対高さを操作可能に複数の関節を介して連結された３つ以上の接地部位を有する移動ロボット等の移動体の目標運動を決定する目標運動決定手段と、前記移動体の基体などの所定の部位の実姿勢傾斜を検出または推定する実姿勢傾斜検出手段とを備え、前記移動体の運動を目標運動に追従させるように該移動体の動作制御を行う移動体の制御装置において、
前記接地部位のそれぞれが葉ノードとなり、且つ、該葉ノードと該葉ノードのすべてを子孫ノードとして持つ根ノードとの間に中間ノードが存在するように前記接地部位をツリー構造状に分類したとき、前記ツリー構造における各ノードに重みが設定されており、
少なくとも前記実姿勢傾斜と前記目標運動における前記所定の部位の目標姿勢傾斜との偏差である姿勢傾斜偏差に応じて、該姿勢傾斜偏差が０に近づくように、前記目標運動に、所定の第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を少なくとも付加した修正目標運動を決定し、その決定した修正目標運動を満足するように前記関節を動作させるノード動作制御手段を備え、
前記ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の高さを第Ｂノードの高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの高さの、前記重みを用いた重み付き平均値を該第Ｂノードの高さとして定義したとき、前記ノード動作制御手段が、前記第Ｃノードのみに対して、該第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記修正目標運動を決定したとき、該修正目標運動における該第Ｃノードの目標高さは前記目標運動における目標高さに維持されることを特徴とするものである。
【００１４】
この第３発明によれば、前記所定の第Ｃノードの目標高さは、この第Ｃノードの子ノードの目標高さ（これは、第Ｃノードに属する接地部位の、前記目標運動における目標高さに応じて定まる）の重み付き平均値となり、前記第Ｃノードのみに対して、該第Ｃノードに属する複数の接地部位の間の目標相対高さの第１の修正を前記目標運動に付加して前記修正目標運動を決定したときには、該第Ｃノードの子ノードの上記重み付き平均値が修正目標運動と目標運動と同じになるように前記第１の修正が目標運動に付加される。
【００１５】
この結果、第３発明によれば、第Ｃノードに属する複数の接地部位の床反力の相互のバランスを採りながら、それらの接地部位の相対高さを前記姿勢傾斜偏差を０に近づけるように操作することができる。このため、第１発明や第２発明と同様に、移動体が接地する床に局所的に未知の傾斜や凹凸がある場合にも、その影響を受けにくくなり、ロボットの姿勢の安定性を高めることができる。
【００１６】
なお、前記第２発明、第３発明においては、前記ツリー構造において子ノードを持つ各ノードに対し、該各ノードの全ての子ノードの重みは、その全ての子ノードの重みの総和が１となるように設定されていることが好ましい（第４発明）。
【００１７】
また、前記第２発明、第３発明では、前記各ノードの重みを可変的に設定する手段を備え、非接地状態である各接地部位にそれぞれ対応する葉ノードを第Ｄノードとしたとき、前記重みを設定する手段は、各第Ｄノードの重み、または該第Ｄノードの祖先ノードのうちの少なくともいずれか１つのノードの重みを０に設定することが好ましい（第５発明）。
【００１８】
この第５発明によれば、前記姿勢傾斜偏差を０に近づけるために寄与できない非接地状態の接地部位の高さが無駄に修正されるのを防止できる。なお、第５発明は、第４発明と複合してもよい。
【００１９】
また、前記第１発明、第２発明では、前記各接地部位は、前記移動体の基体に対して移動可能に該基体に連結されている場合に、前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記第Ｃノードに属する各接地部位の床面上でのこじりなどの滑りを抑制するための、前記基体の目標姿勢の修正を前記目標運動にさらに付加して前記修正目標運動を決定することが好ましい（第６発明）。そして、このことは、前記第３発明においても同じである（第７発明）。
【００２０】
ここで、移動体の基体に連結されている接地部位が多数あると、移動体の関節の自由度が不足して、それらの接地部位の間の相対高さの修正が機構的な制限を受け、決定した目標相対高さを満足するように強制的に関節を動作させると、いくつかの接地部位でこじりなどの滑りが発生する場合がある。そして、そのような滑りが生じる状況では、各接地部位の床反力が不適切なものとなると共にロボットの姿勢の安定性が却って損なわれる恐れがある。このような場合には、第６発明、第７発明の如く、第Ｃノードに属する接地部位の間の目標相対高さを修正するだけでなく、基体の目標姿勢の修正を目標運動にさらに付加して、前記修正目標運動を決定することことにより、上記こじりなどの滑りを抑制することができる。従って、接地部位に無理な床反力が作用したりすることなく、それらの床反力を適正なものとしつつ、移動体の姿勢の安定性を確保することができる。
【００２１】
前記第６発明、第７発明では、より好ましくは、前記ノード動作制御手段は、前記修正目標運動における前記移動体の全体重心の水平位置または該移動体の基体の所定の代表点の水平位置が前記目標運動における当該水平位置とほぼ同じになるように前記修正目標運動を決定する（第８発明）。この第８発明によれば、基体の目標姿勢の修正を目標運動に付加して修正目標運動を決定したときに、移動体の全体重心が、目標歩容における適正な位置に対して水平方向に大きくずれるようなことを防止できるので、移動体の安定性を適正に保ちつつ、第Ｃノードの接地部位の間の相対高さと基体の姿勢とを修正できる。
【００２２】
また、前記第６発明は、前記移動体が、その基体から延設された複数のリンク機構を備えるロボットであり、該複数のリンク機構のうちの少なくとも１つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記接地部位となっている場合に適している（第９発明）。
【００２３】
また、第７発明は、前記移動体が、その基体から延設された複数のリンク機構を備えるロボットであり、該複数のリンク機構のうちの少なくとも１つのリンク機構は、少なくともその先端部と基体側の端部との間の中間部位に関節が設けられていると共に、該中間部位が前記第Ｃノードに属する接地部位となっている場合に適している（第１０発明）。
【００２４】
すなわち、複数のリンク機構が基体から延設されたロボットでは、それらのリンク機構の先端部で接地することが通常的であるので、該リンク機構の中間部位で接地させようとした場合（例えば人間型移動ロボットをその脚の中間部（膝）で接地させようとした場合）には、関節の自由度が不足して、前記こじりなどの滑りが生じる場合がある。従って、第６発明、第７発明は、それぞれ第９発明、第１０発明の如く移動体が構成されている場合に適している。
【００２５】
これらの第９発明、第１０発明では、前記中間部位には、その接地時に弾性変形する弾性体が設けられていることが好ましい（第１１発明）。
【００２６】
これによれば、前記中間部位の間の相対高さの修正による床反力の調整能力が高まって、移動体（ロボット）の姿勢の安定性の確保が容易になる。
【００２７】
また、前記第６発明は、前記移動体が、その基体から延設されて１つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも１つのリンク機構と前記基体とに前記接地部位が設けられている場合にも適している（第１２発明）。
【００２８】
また、第７発明は、前記移動体が、その基体から延設されて１つ以上の関節をそれぞれ有する複数のリンク機構を備えると共に、該複数のリンク機構のうちの少なくとも１つのリンク機構と前記基体とに前記接地部位が設けられており、前記第Ｃノードに属する接地部位には、少なくとも前記基体の接地部位が含まれる場合に適している（第１３発明）。
【００２９】
例えば１つ以上の脚（リンク機構）が基体（上体など）から延設されたロボットに腰掛動作を行なわせる場合、その基体で接地することとなる。そしてこのような場合には、関節の自由度が不足して、前記こじりなどの滑りが生じる場合がある。従って、第６発明、第７発明は、それぞれ第１２発明、第１３発明の如く移動体が構成されている場合にも適している。
【００３０】
これらの第１２発明、第１３発明では、前記基体の少なくともひとつの接地部位には、その接地時に弾性変形する弾性体が設けられていることが好ましい（第１４発明）。
【００３１】
これによれば、前記基体における接地部位の間の相対高さの修正による床反力の調整能力が高まって、移動体の姿勢の安定性の確保が容易になる。
【００３２】
なお、前記第６〜第１４発明は、前記第２発明、第４発明、第５発明と複合してもよい。
【００３３】
また、前記第１発明、第３発明では（あるいは第１〜第１４発明では）、前記各接地部位が、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結されている場合に、前記修正目標運動を満足するように前記関節を動作させたときに前記第Ｃノードに属する各接地部位に作用すべき目標床反力の並進力成分を決定する手段を備え、
前記ノード動作制御手段は、少なくとも前記第Ｃノードに属する各接地部位の目標床反力の並進力成分を基に、該目標床反力の並進力成分が該第Ｃノードに属する各接地部位に作用したときに前記基体と該第Ｃノードに属する各接地部位との間の連結機構および該接地部位で発生する変形量を推定する手段を備え、その推定した変形量を打ち消すための、前記第Ｃノードに属する複数の接地部位の目標高さの第２の修正を前記目標運動にさらに付加して前記修正目標運動を決定することが好ましい（第１５発明）。
【００３４】
この第１５発明によれば、前記連結機構や接地部位が該接地部位の接地時に受ける床反力によってたわみなどの変形が生じることを考慮し、その変形量を目標床反力の並進力成分を基に推定する。そして、その推定した変形量を打ち消すための、前記第Ｃノードに属する各接地部位の目標高さの第２の修正を目標運動にさらに付加して前記修正目標運動を決定する。このため、第１５発明によれば、連結機構や接地部位の変形の影響を補償しつつ、適正に前記姿勢傾斜偏差を０に近づける床反力を移動体に作用させることが可能となり、移動体の姿勢の安定性をより一層高めることができる。
【００３５】
また、前記第１発明、第３発明（あるいは第１〜第１５発明）では、少なくとも前記第Ｃノードに属する複数の接地部位の接地面の間の相対高さを規定するパラメータを床形状を表す床形状パラメータとして推定する床形状推定手段を備え、
前記ノード動作制御手段は、前記修正目標運動を決定するとき、前記前記床形状パラメータの推定値に応じた、前記第Ｃノードに属する複数の接地部位の間の目標相対高さの第３の修正をさらに付加して前記修正目標運動を決定することが好ましい（第１６発明）。
【００３６】
この第１６発明によれば、少なくとも前記第Ｃノードに属する複数の接地部位の接地面の間の相対高さを規定する床形状パラメータの推定値に応じた、第Ｃノードに属する複数の接地部位の間の目標相対高さの第３の修正を目標運動にさらに付加するので、実際の床形状が前記目標運動で想定したものと相違しても、接地部位の接地面を、それが接地すべきときに実際の床面に適切に接地させ、ひいては、前記姿勢傾斜偏差を０に近づけるような床反力を適切に移動体に作用させることができる。従って、移動体の姿勢の安定性をより一層高めることができる。
【００３７】
この第１６発明では、前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、
前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードに属する各接地部位の実床反力とに基づき、前記床形状パラメータを逐次更新しつつ推定することが好ましい（第１７発明）。
【００３８】
あるいは、前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備えると共に、
前記ツリー構造における各ノードである第Ａノードに対し、該第Ａノードが前記葉ノードである場合には、該葉ノードである接地部位に作用する床反力を第Ａノードのノード床反力と定義し、且つ、第Ａノードが子ノードを持つ場合には、該第Ａノードのすべての子ノードのノード床反力の合力を第Ａノードのノード床反力と定義したとき、少なくとも前記第Ｃノードの各子ノードのノード床反力の実際値である実ノード床反力を該第Ｃノードに属する各接地部位の実床反力から逐次決定する手段を備え、
前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードの各子ノードの実ノード床反力の間の相対関係とに基づき、前記床形状パラメータを逐次更新しつつ推定することが好ましい（第１８発明）。
【００３９】
あるいは、前記各接地部位が、前記移動体の基体に対して移動可能に該基体に連結機構を介して連結されているときには、
該基体と各接地部位との間の連結機構および該接地部位で発生する変形量を検出または推定する変形量検出手段を備え、
前記床形状推定手段は、前記床形状パラメータの推定値の過去値と、前記修正目標運動と前記目標運動との差と、前記修正運動と、前記移動体の各関節の変位量である実関節変位の検出値もしくは推定値とのうちの少なくともいずれかと、前記実姿勢傾斜と、少なくとも前記第Ｃノードに属する各接地部位に対応する前記変形量とに基づき、前記床形状パラメータを逐次更新しつつ推定することが好ましい（第１９発明）。
【００４０】
これらの第１７発明、第１８発明、第１９発明によれば、前記床形状パラメータをリアルタイムで逐次更新しながら、適切に推定できる。補足すると、第１９発明では、前記連結機構は、その全てが必ずしも関節を備えている必要はない。
【００４１】
なお、第１９発明では、例えば前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、前記変形量検出手段は、該実床反力に基づき前記変形量を推定する（第２０発明）。
【００４２】
このように実床反力を検出または推定し、それを用いることで、前記連結機構や接地部位の変形量を適切に推定できる。
【００４３】
前記第１６発明（あるいは前記第１６〜第２０発明）においては、前記ツリー構造における各ノードの重みが設定されており、該ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第Ｂノードの接地面の高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第Ｂノードの接地面の高さとして定義したとき、
前記床形状推定手段が推定する前記床形状パラメータは、前記第Ｃノードの複数の各子ノードの接地面の高さの間の相対関係を規定するパラメータであることが好ましい（第２１発明）。
【００４４】
この第２１発明によれば、上記のように、子ノードを持つ第Ｂノードの接地面の高さをその子ノードの重み付き平均値として定義することによって、前記第Ｃノードの複数の各子ノードの接地面の高さの間の相対関係によって、該第Ｃノードに属する複数の接地部位が接地する床の形状に係わる前記床形状パラメータを決定できる。従って、床形状パラメータを各接地部位の絶対高さを把握したりすることなく推定できる。そのため、床形状の推定処理が容易になる。
【００４５】
この第２１発明では、前記床形状推定手段は、前記第Ｃノードの各子ノードのうちの少なくとも１つが浮きそうな状態であるときに、その浮きそうな子ノードの接地面の相対高さを一定値に保持しつつ、前記複数の子ノードの接地面の高さの相対関係を規定する前記床形状パラメータを推定することが好ましい（第２２発明）。
【００４６】
すなわち、前記第Ｃノードの各子ノードのうちの少なくとも１つが浮きそうな状態であるということは、その子ノードの接地面が実際の床面から離れたか、もしくは離れそうな状態であるので、その子ノードの接地面の高さは実際の床面の高さを反映していない。そこで、第２２発明では、このような状態では、当該浮きそうな状態の子ノードの接地面の相対高さを一定値に保持しつつ、前記複数の子ノードの接地面の高さの相対関係を規定する前記床形状パラメータを推定する。これにより、床形状パラメータが実際の床面形状を反映していない値に発散してしまうような事態を防止でき、実際の床面形状を反映した床形状パラメータを推定できる。ひいては、不適切な床形状パラメータを使用して、接地部位の相対高さを修正し、移動体の姿勢の安定性が損なわれるような事態を防止できる。
【００４７】
また、前記第１６発明（あるいは第１６〜第２２発明）では、前記床形状推定手段は、前記床形状パラメータの変動を緩和するようにローパスフィルタを用いて該床形状パラメータを推定することが好ましい（第２３発明）。このようにローパスフィルタを使用することで、移動体の移動時の振動などによって床形状パラメータが頻繁に変動したりするのを防止できる。
【００４８】
また、前記第１６発明（あるいは第１６〜第２３発明）では、前記ツリー構造の各ノードには床形状の推定用の重みが設定されており、
前記ツリー構造における各葉ノードに高さ、床反力などの所定の種類の状態量を対応づけたときに、子ノードを持つ各ノードの状態量を、該ノードの全ての子ノードの状態量の、前記重みを用いた重み付き平均値とし、根ノードを除く各ノードに対し、該ノードの状態量から該ノードの親ノードの状態量を減算してなる値を該ノードのノード相対状態量として決定すると共に０を根ノードの相対状態量として決定する処理を、当該所定の種類の状態量を各ノードについて階層相対化する処理として定義し、
前記葉ノードである接地部位のそれぞれに作用する床反力Fn(n=1,2,…)を基に、各ノードについて階層相対化したノード相対床反力Fn_rel(n=1,2,…)を求め、さらに子ノードを持つ任意のノードである第ｎノードのすべての子ノードaj (j＝1,2,…,r。ｒは第ｎノードの子ノードの総数)のノード相対床反力を要素とするベクトル(Fa1_rel,Fa2_rel,…,Far_rel)を、第ｎノードのすべての子ノードの重みを要素とするベクトル(Wa1,Wa2,…,War)にそれぞれ直交する所定の互いに独立な複数のベクトルR(j) (j=1,2,…,r−1)の線形結合で表したときの該線形結合の係数を要素とするベクトルを、第ｎノードのノード拡張床反力モーメントMn_expと定義し、
また、前記葉ノードである接地部位のそれぞれの接地面の高さZn (n=1,2,…)を基に、各ノードについて階層相対化したノード相対高さZn_rel(n=1,2,…)を求め、さらに前記第ｎノードのすべての子ノードaj (j=1,2,…,r。ｒは第ｎノードの子ノードの総数)のノード相対高さを要素とするベクトル(Za1_rel, Za2_rel,…,Zar_rel)を、前記所定の互いに独立な複数のベクトルR(j) (j=1,2,…,r-1)の線形結合で表したときの線形結合の係数を要素とするベクトルを、第ｎノードのノード拡張傾斜角θnと定義したとき、
前記床形状推定手段は、少なくとも前記第Ｃノードの前記ノード拡張床反力モーメントMn_exp(n=C)またはノード拡張傾斜角θn(n=C)を用いて前記床形状パラメータを推定するようにしてもよい（第２４発明）。
【００４９】
上記のように定義されるノード拡張床反力モーメントや、ノード拡張傾斜角を用いることで、第Ｃノードに属する接地部位が４つ以上であるような場合であっても、床形状を表す床形状パラメータを適切に推定できる。
【００５０】
この第２４発明では、前記ツリー構造における各ノードである第Ｂノードに対し、該第Ｂノードが前記葉ノードである場合には、該葉ノードである接地部位の接地面の高さを第Ｂノードの接地面の高さと定義し、且つ、第Ｂノードが子ノードを持つ場合には、該第Ｂノードのすべての子ノードの接地面の高さの、前記重みを用いた重み付き平均値を第Ｂノードの接地面の高さとして定義したとき、
前記床形状パラメータに、前記第Ｃノードの各子ノードの接地面の相対高さを前記ノード拡張傾斜角を用いて表したパラメータが含まれることが好ましい（第２５発明）。
【００５１】
これによれば、第Ｃノードに属する接地部位が４つ以上であっても、前記ノード拡張傾斜角を用いて、床形状パラメータを適切に推定できる。
【００５２】
また、第２４発明では、より具体的には、前記接地部位のそれぞれに作用する実床反力を検出または推定する床反力検出手段を備え、
前記ノード動作制御手段は、少なくとも前記各接地部位の実床反力を基に前記ノード拡張モーメントを逐次決定する手段と、この決定したノード拡張モーメントに所定の行列を乗じることにより前記ノード拡張傾斜角を逐次決定する手段とを備え、前記決定したノード拡張傾斜角と、前記床形状パラメータの推定値の過去値とを基に、新たな床形状パラメータを推定する（第２６発明）。
【００５３】
この第２６発明によれば、少なくとも前記各接地部位の実床反力を基に前記ノード拡張モーメントを逐次決定すると共に、この決定したノード拡張モーメントに所定の行列を乗じることにより前記ノード拡張傾斜角を逐次決定するので、その決定したノード拡張傾斜角と、前記床形状パラメータの推定値の過去値とを基に、新たな床形状パラメータを逐次更新しながら求めることができる。
【００５４】
なお、前記第１〜第２６発明においては、「床反力」は、本来の意味での床（もしくは地面）から移動体に作用する反力だけを意味するものではなく、移動体が接触する床以外の対象物から受ける反力をも含むものとする。そして、接地部位は、床（もしくは地面）への接触箇所だけでなく、移動体の対象物との接触箇所をも含むものとする。
【発明を実施するための最良の形態】
【００５５】
まず、本発明の実施形態の説明で用いる主な用語について説明しておく。
【００５６】
本願出願人が先に提案した特開平１０−２７７９６９号公報の複合コンプライアンス制御の技術においては、ロボットの脚先端部（足平）から床反力を受けることを前提として、脚毎に床反力を分別した。これに対して、本明細書の実施形態では、例えば人間型ロボットがひざまづいた状況や腰掛けた状況、腕を接地させた状況など、脚先端部以外からも床反力を受ける状況までも考慮する。このため、脚式移動ロボットの所要の動作において床と接して反力を受ける該ロボットの部位を「接地部位」と呼ぶ。この場合、「床」は、通常の意味での床（あるいは地面）だけではなく、該床（あるいは地面）に固定された椅子（ロボットが腰掛ける椅子）等のように、ロボットがその動作において接触して反力を受ける対象物も、「床」に含まれるものとする。従って、例えば２足移動ロボットが椅子などに腰掛けるときには、該ロボットの腰部は接地部位に相当するものとなる。なお、脚式移動ロボットの通常的な移動時（２足移動ロボットの歩行時など）には、各脚の先端部（足平など）が接地部位に相当するものとなることはもちろんである。
【００５７】
接地部位を分類（区別）するに当たっては、ロボットの同一のリンク（単一の剛体に相当する部分）上で接地部位が複数箇所に分離されて分布している場合（同一のリンクの、互いに分離した複数の部分が接地している場合）でも、すなわち同一リンク上で複数の接地面がつながっていなくても、これらをまとめてひとつの接地部位と定義する。例えば、スパイクピンが４本装着された接地部位がそれらのスパイクピンを介して接地する場合には、４本のスパイクピンのそれぞれの接地面をまとめてひとつの接地部位とみなす。ただし、それらの接地面を必ずしも一つにまとめる必要はない。
【００５８】
第n接地部位とは、このようなルールの基で分類（区別）されて識別番号ｎ（n=1,2,3,…）を割り振られた接地部位の識別名称である。
【００５９】
接地部位床反力とは、接地部位に作用する床反力とし、特に、第n接地部位に作用する床反力を第n接地部位床反力という。また、全床反力とは、すべての接地部位に作用する床反力の合力とする。更に、床反力中心点とは、床反力がその点に発生するモーメントの水平成分が０となるような作用点とする。
【００６０】
なお、接地部位床反力、全床反力などの床反力は、一般的には、力の作用点と、その作用点にかかる並進力およびモーメントとの組によって表現され、同一の床反力に対して、それを表現する作用点と、並進力およびモーメントとの組は無限にある。その中には、鉛直軸まわりの成分（鉛直成分）を除くモーメント成分（モーメントの水平成分）が０となるような点を作用点として用いる床反力の表現が存在する。この表現における作用点を、本明細書では床反力中心点という。また、床反力中心点を、床反力がその点に発生するモーメントの床面平行成分（床面に平行な成分）が０となるような点として定義してもよい。
【００６１】
尚、この明細書で「床面」は、本願出願人が先に提案した特開平５−３１８３４０号公報に記載される仮想的な床面（目標歩容上で想定される床面であって、実際の床面とは必ずしも一致しない）であっても良い。
【００６２】
補足すると、接地部位の床反力中心点は、その接地部位が接地している状態では、通常、その接地面（床との接触面）上に設定される。一方、接地部位が空中を移動しているときの接地部位床反力は常に０であるので、どの点を作用点としても該接地部位床反力のモーメント水平成分は０である。つまり、床反力中心点は任意に設定できる。ただし、ロボットの動作制御を円滑に行なう上では、目標とする床反力中心点は、連続的に移動することが望ましい。そこで、本明細書の実施形態では、接地部位床反力の床反力中心点は、接地部位が空中を移動するときに、接地部位と共に移動する作用点であるとする。
【００６３】
本明細書の実施形態においては、少なくとも実床反力（ロボットに実際に作用する床反力）に応じて各接地部位の位置姿勢の修正を行なうコンプライアンス制御動作において、単に、各接地部位毎に、互いに無関係に（独立して）コンプライアンス制御動作を行うのではない。本明細書の実施形態は、各接地部位をツリー構造で分類し、少なくとも分類された各グループに作用する実床反力（各グループに含まれる全ての接地部位に作用する実床反力もしくはその合力）に基づき、接地部位の位置姿勢の修正動作を決定することに特徴がある。尚、以降、ツリー構造で分類することを「階層化」と呼ぶこともある。
【００６４】
本明細書の実施形態における脚式移動ロボットの歩容生成装置が生成する変数の目標値には、変数名の頭に「目標」を付ける。実際の脚式移動ロボットにおける当該変数の値（真の値はわからないので、実際には、検出値または推定値）には、変数名の頭に「実」を付ける。前記「実床反力」はその例である。
【００６５】
後述するコンプライアンス制御（床反力制御）の全床反力（ロボットのすべての接地部位に作用する実床反力の合力）の目標を目標全床反力と呼ぶ。目標全床反力のモーメント水平成分が０となる点を目標全床反力中心点と呼ぶ。
【００６６】
移動ロボットの目標歩容の運動（目標歩容におけるロボットの各部位の運動）が発生する慣性力と該ロボットに作用する重力との合力のモーメント水平成分が０となる床面上の点を目標ＺＭＰと呼ぶ。なお、目標歩容の運動は、目標歩容におけるロボットの各部位の目標位置姿勢の時系列によって定まるので、その各部位の目標位置姿勢の時系列を総称的に目標歩容の運動もしくは目標運動という。補足すると、これらの時系列を規定できるならば、上記の如く定義した目標運動と異なる表現で目標運動を記述してもよい。例えば、ロボットの各関節の目標変位の時系列と基体などの特定の部位の目標位置姿勢の時系列との組を目標運動としてもよい。
【００６７】
目標全床反力は、通常、目標歩容の運動パターン（目標運動の時系列パターン）に対して動力学的に平衡する全床反力である。従って、通常、目標全床反力中心点は、目標ＺＭＰに一致する。そこで、以下の説明では、多くの場合、目標全床反力中心点と目標ＺＭＰとを区別せずに使用する。ただし、例外的に、本願出願人が先に提案した特許第３２６９８５２号などにて説明しているロボットの制御の場合には、目標全床反力中心点と目標ＺＭＰとは一致するとは限らない。以下の説明では、目標ＺＭＰという言葉を用いる場合もあるが、厳密には目標全床反力中心点と言うべき箇所もある。
【００６８】
尚、移動ロボットの目標歩容で該ロボットが床反力以外の反力（外力）を環境から受けるように想定してもよい。そして、その床反力以外の反力（外力）を、例えば目標対象物反力と称し、先に述べた目標ＺＭＰの定義を次のように拡張しても良い。即ち、ロボットの目標歩容の運動パターンによって発生する慣性力と該ロボットに作用する重力と目標対象物反力との合力を動力学的に求め、その合力が床面上のある点に発生するモーメントが、鉛直軸まわりの成分を除いて０であるならば、その点を改めて目標ＺＭＰとするようにしても良い。ただし、目標対象物反力も、床反力の一形態としてとらえるならば、目標ＺＭＰの定義は、先に述べた定義のままでよい。
【００６９】
以上を前提として、本発明の実施形態を以下に詳細に説明する。
【００７０】
［第１実施形態］
図１は、第１および第２実施形態に係る一般的な多脚の移動ロボット（脚式移動ロボット）の外観図である。ただし、図１では、ロボット１は、第１脚＃１〜第６脚＃６までの６個の脚（脚体）を有するものとして図示しているが、第１実施形態においては、第５脚＃５および第６脚＃６は無いものとする。すなわち、第１実施形態では、ロボット１は、第１脚＃１〜第４脚＃４の４個の脚（脚体）を有する４脚ロボットであるとする。図１では、第２実施形態に係るロボット１の構成要素については、括弧付きの参照符号を付している。
【００７１】
図１に示すように、第１実施形態のロボット１（４脚ロボット）では、該ロボット１の基体である上体２４の右側部から２個の脚（第１脚＃１、第３脚＃３）が前後方向に並んで延設され、これと同様に上体２４の左側部から２個の脚（第２脚＃２、第４脚＃４）が前後方向に並んで延設されている。各脚＃１〜＃４の先端部には、フリージョイントである球関節１２を介して円盤状の接地部位１０が取り付けられている。以降、接地部位１０を各脚＃１〜＃４毎に区別する必要があるときは、それぞれを第１接地部位、第２接地部位、第３接地部位、第４接地部位という。
【００７２】
各接地部位１０は、その中心点を球関節１２の中心点に一致させて該球関節１２に係合されており、該接地部位１０の中心点（厳密には球関節１２）には、床反力モーメント（床反力のモーメント成分）が作用しないようになっている。すなわち接地部位１０の中心点の床反力モーメント（実床反力モーメント）は常に０となる。
【００７３】
図示のロボット１では、各脚＃１〜＃４は、ロボット１の上体２４寄りの部分と中間部とにそれぞれ関節１４，１５を備えると共に、各脚＃１〜＃４の先端部近傍（球関節１２と関節１４とを連結するリンクの球関節１２寄りの部分）に、バネなどの弾性部材から構成されたコンプライアンス機構４２と、接地部位１０に作用する実床反力を検出する床反力検出手段としての６軸力センサ３４とが介装されている。図示の例では、関節１４は２軸回りの回転が可能とされ、関節１５は１軸回りの回転が可能とされている。なお、コンプライアンス機構とし、接地部位の底面にスポンジやゴムなどの弾性体を設けてもよい。
【００７４】
各関節１４，１５の変位動作（各軸回りの回転動作）は、図示を省略する電動モータなどのアクチュエータにより行なわれる。そして、各関節１４，１５の実際の変位量（各軸回りの回転角）である実関節変位が、図示しないロータリエンコーダ等のセンサにより検出される。なお、６軸力センサ３４は、３軸方向の並進力と３軸回りのモーメントとを検出可能なものであるが、第１実施形態のロボット１では、前記したように接地部位１０の中心点に実床反力モーメントが作用しない。従って、６軸力センサ３４の代わりに、３軸方向の並進力を検出する３軸力センサ、あるいは、実床反力の並進力の鉛直成分だけを検出する力センサを用いてもよい。
【００７５】
上体２４には、マイクロコンピュータやアクチュエータ駆動回路等を含む電子回路ユニットから構成された制御装置５０と、上体２４の姿勢を検出する姿勢センサ３６と、図示しない電源（２次電池、キャパシタなど）とが内蔵されている。姿勢センサ３６は、例えば加速度センサとジャイロセンサとから構成されている。なお、本明細書では、「姿勢」は一般的には空間的な向きを意味する（但し、ロボットの全体の「姿勢」は、ロボットの運動の瞬時値を意味する）。そして、本実施形態では、姿勢センサ３６は、上体２４の姿勢のうち、例えば鉛直方向に対するピッチ方向およびロール方向の姿勢傾斜（傾き角）を検出する。すなわち、姿勢センサ３６は上体２４の実姿勢傾斜を検出する実姿勢傾斜検出手段として機能する。
【００７６】
なお、ロボット１の上体２４のヨー方向の姿勢（回転角）も制御するような場合には、上体２４のヨー方向の回転角も（換言すれば上体２４の３軸方向の姿勢を）姿勢センサ３６で検出するようにしてもよい。
【００７７】
図２は、前記制御装置５０の機能的構成および動作を示すブロック図である。なお、図２では、図１のロボット１から、制御装置５０、姿勢センサ３６および６軸力センサ３４を除いたものを実ロボット１と称している。ここで、前記図１に示したように、ロボット１のおおよそ前方向をＸ軸、おおよそ左横方向をＹ軸、上方向をＺ軸とした、床に固定された所定の座標系（ＸＹＺ座標系）を「支持脚座標系」または「グローバル座標系」と呼ぶ。以降、特に断らない限り、位置、姿勢、並進力およびモーメントはこの支持脚座標系（グローバル座標系）の座標成分の組によって表現されるものとする。支持脚座標系（グローバル座標系）の原点は、定常的に単一の点に固定されている必要はなく、ロボット１の移動途中で床に対する原点位置を変更してもよい。例えば、ロボット１の所定の脚が着地する毎に支持脚座標系（グローバル座標系）の原点位置を変更してもよい。
【００７８】
図２に示すように、制御装置５０は、その機能的構成要素（機能的手段）として、歩容生成装置１００、目標床反力分配器１０２、ロボット幾何学モデル（逆キネマティクス演算部）１１０、階層型コンプライアンス動作決定部１１４、変位コントローラ１１２、実床反力検出器１０８、姿勢偏差演算部１０３、および姿勢安定化制御演算部１０４を備えている。以下に、制御装置５０のこれらの要素の概要を説明する。
【００７９】
歩容生成装置１００は、本発明における目標歩容決定手段あるいは目標運動決定手段としての機能を持ち、ロボット１の動作を規定する目標歩容を生成（決定）して出力する。目標歩容は、本実施形態では、ロボットの目標運動の軌道（ロボットの各部位の目標位置姿勢の時系列）と目標床反力の軌道（ロボットが床から受ける反力の作用点の目標位置とその作用点にかかる並進力およびモーメントの目標値との組の時系列）とから構成される。なお、本明細書では、「軌道」は、時系列パターン（時間的変化のパターン）を意味する。
【００８０】
歩容生成装置１００が出力する目標運動の軌道は、ロボット１の各接地部位１０の位置および姿勢の目標値の軌道である目標接地部位軌道と、ロボット１の上体２４の位置および姿勢の目標値の軌道である目標上体位置姿勢軌道とから構成される。なお、後述する第６実施形態のように、腕や頭部に係る関節を備えるロボットにおける歩容生成装置は、腕や頭部の目標位置姿勢軌道も目標運動の構成要素として決定して出力する。
【００８１】
また、歩容生成装置１００が出力する目標床反力の軌道は、本実施形態では、ロボット１の全床反力中心点の目標位置の軌道である目標全床反力中心点軌道と、この目標全床反力中心点を作用点とする全床反力の目標値の軌道である目標全床反力軌道とから構成される。本実施形態では、目標全床反力中心点軌道は、ＺＭＰの目標位置の軌道である目標ＺＭＰ軌道と同一とされている。
【００８２】
各接地部位１０の位置は、該接地部位１０のある代表点の位置であり、その代表点は、例えば、各接地部位１０の中心点（球関節１２の中心点）の、該接地部位１０の接地面（底面）への鉛直方向の投影点あるいは該接地部位１０の中心点（球関節１２の中心点）とされている。以降、各接地部位１０の代表点の位置を単に接地部位位置と呼ぶ。この接地部位位置の目標値の軌道（目標接地部位位置軌道）が歩容生成装置１００により決定される前記目標接地部位軌道に含まれる。
【００８３】
ここで、本実施形態のロボット１では、各接地部位１０はフリージョイントである球関節１２に係合されているので、各接地部位１０の姿勢を制御することはできない。このため、本実施形態では、歩容生成装置１００は、実際には各接地部位１０の目標姿勢の軌道を生成しない（生成する必要がない）。従って、本実施形態では、前記目標接地部位軌道は、目標接地部位位置軌道と同じものを意味する。
【００８４】
但し、後述する第６実施形態のロボットのように、各接地部位が各脚の先端部にその姿勢を制御可能に取り付けられている場合（各脚の先端部にアクチュエータにより動作可能な関節を介して接地部位が取り付けられている場合）には、各接地部位の目標姿勢の軌道も、前記目標接地部位軌道に含まれるべきものである。本明細書では、このような場合も一般的に考慮するために、しばしば「接地部位位置姿勢」という用語を用いるが、それは、本実施形態では、実質的には「接地部位位置」のことを意味する。
【００８５】
図３（ａ）〜図３（ｃ）および図４（ａ）〜図４（ｃ）を参照して、目標接地部位軌道（目標接地部位位置軌道）および目標全床反力中心点軌道をより具体的に説明する。第１実施形態では、脚＃１〜＃４のうち、第１脚＃１および第４脚＃４の組を支持脚、第２脚＃２および第３脚＃３の組を遊脚とする時期と、全ての脚＃１〜＃４を支持脚とする時期と、第１脚＃１および第４脚＃４の組を遊脚、第２脚＃２および第３脚＃３の組を支持脚とする時期とを順番に繰り返しつつ、遊脚となる脚の組を離床させて空中で移動させた後、所望の位置に着地させるという脚＃１〜＃４の運動を行なうことによりロボット１の移動を行なうようにしている。支持脚は、接地してロボット１の自重を支える脚（０でない床反力を作用させるべき脚）であり、遊脚は支持脚でない脚である。
【００８６】
図３（ａ）〜図３（ｃ）および図４（ａ）〜図４（ｃ）は、上記の如くロボット１を移動させるときの各脚＃１〜＃４の先端の目標接地部位位置（詳しくは水平面（ＸＹ平面）上での位置）を時系列的に順番に（時刻t1〜t6の順に）示したものである。これらの図中の参照符号Q1〜Q4に対応する三角形が、それぞれ第１〜第４接地部位１０の目標接地部位位置（第１〜第４接地部位１０の前記代表点の水平面（ＸＹ平面）上での位置）を示している。
【００８７】
補足すると、図３および図４中の参照符号Q1〜Q4を付した三角形は、正確には、各接地部位１０に係る後述の目標ノード床反力中心点（目標接地部位床反力中心点）の位置を示すものである。ただし、本実施形態では、各接地部位１０の代表点を前記した如く設定しているので、第１〜第４接地部位１０のそれぞれの目標接地部位位置（代表点の目標位置）は、後述する目標ノード床反力中心点Qn（n=1,2,3,4）のそれぞれの位置に一致するか、もしくは、該目標ノード床反力中心点Qn（n=1,2,3,4）のそれぞれの位置と一定の位置関係を有する（水平面（ＸＹ平面）上での位置は一致する）。このため、図３および図４中の参照符号Q1，Q2，Q3，Q4に対応する三角形は、第１〜第４接地部位１０のそれぞれの後述の目標ノード床反力中心点を表すと同時に、第１〜第４接地部位１０のそれぞれの位置を表している。
【００８８】
図３（ａ）は、第２脚＃２および第３脚＃３の組を支持脚とし、遊脚としての第１脚＃１および第４脚＃４の組を着地させる瞬間（時刻t1）での各接地部位１０の目標接地部位位置を示し、図３（ｂ）は、全ての脚＃１〜＃４が支持脚となっている状態の時刻t2での目標接地部位位置を示し、図３（ｃ）は、第１脚＃１および第４脚＃４の組を支持脚とし、遊脚としての第２脚＃２および第３脚＃３の組を離床させる（空中に持ち上げる）直前の時刻t3での目標接地部位位置を示している。また、図４（ａ）は、第１脚＃１および第４脚＃４の組を支持脚とし、遊脚としての第２脚＃２および第３脚＃３の組を空中に持ち上げた状態の時刻t4での目標接地部位位置を示し、図４（ｂ）は、第１脚＃１および第４脚＃４の組を支持脚とし、遊脚としての第２脚＃２および第３脚＃３の組を着地させた瞬間（時刻t5）での目標接地部位位置を示し、図４（ｃ）は、第２脚＃２および第３脚＃３の組を支持脚とし、遊脚としての第１脚＃１および第４脚＃４の組を離床させる直前の時刻t6での目標接地部位位置を示している。なお、図４（ａ）では遊脚である第２脚＃２および第３脚＃３の目標接地部位位置を破線の三角形で示している。補足すると、遊脚の接地部位１０の鉛直方向（Ｚ軸方向）の位置の軌道は、床面から所定の高さまで上昇した後、下降して、再び着地するように決定される。
【００８９】
これらの図３および図４における点Ｐは、目標全床反力中心点（目標ＺＭＰ）を示している。目標全床反力中心点軌道は、ＺＭＰの存在可能範囲（いわゆる支持多角形に相当する床面上の領域）内で該範囲の境界に近づきすぎない位置（例えばＺＭＰの存在可能範囲のほぼ中央位置）に存在しつつ、連続的に移動するように決定される。第１実施形態では、２つの脚＃１，＃４または＃２，＃３が支持脚となるときには（図３（ａ），（ｃ）、図４（ａ）〜（ｃ）を参照）、それらの脚の接地部位１０，１０の代表点を結ぶ線分上で、その線分の端点に近づき過ぎないように目標全床反力中心点が設定される。また、全ての脚＃１〜＃４が支持脚となるときには（図３（ｂ）を参照）、全ての接地部位１０の代表点を頂角とする多角形内で、該多角形の境界に近づき過ぎないように目標全床反力中心点が設定される。図５（ｄ）は、このように決定された目標全床反力中心点（目標ＺＭＰ）の位置のＸ軸方向（ロボット１の進行方向）の成分ＺＭＰxの軌道を例示したグラフである。なお、図５（ａ）〜（ｃ）は後述の重みの設定例を示すグラフである。
【００９０】
また、歩容生成装置１００が決定する目標上体位置姿勢軌道は、少なくとも、ロボット１の目標運動によって発生する慣性力とロボット１に作用する重力との合力が目標ＺＭＰまわりに作用するモーメントの水平成分が０になるようにロボット１の動力学モデルなどを使用して決定される。なお、「上体位置」は、上体２４のある代表点の位置である。
【００９１】
また、歩容生成装置１００が決定する目標全床反力は、目標全床反力中心点に作用する並進力およびモーメントの目標値から構成され、本実施形態では、ロボット１の目標運動によって発生する慣性力とロボット１に作用する重力との合力に目標全床反力中心点で釣り合うように決定される。なお、目標全床反力中心点（目標ＺＭＰ）まわりの目標全床反力のモーメント水平成分は０である。補足すると、目標全床反力中心点に作用する並進力およびモーメントの全ての成分を目標値として決定する必要はない。例えばロボット１の鉛直軸まわりの姿勢もしくは床反力を制御しない場合には、目標全床反力のモーメントの鉛直軸まわりの成分（Ｚ軸方向成分）を決定する必要はない。
【００９２】
なお、歩容生成装置１００が決定する目標接地部位軌道（目標接地部位位置軌道）は、後述する階層型コンプライアンス動作決定部１１４にて修正される。
【００９３】
前記目標床反力分配器１０２は、ロボット１の第１〜第４接地部位１０をツリー構造状にグループ分けし（すなわち階層化し）、階層化された各グループに、ツリー構造のノードを対応させる。したがって、以下の説明において、ノードをグループに置き換えて表現しても構わない。各ノードは、１つ以上の接地部位１０から構成されるグループである。
【００９４】
第１実施形態においては、図６に示す如く接地部位１０が階層化される。すなわち、第n接地部位１０（n=1,2,3,4）を第nノードに対応させ、第１ノードと第４ノードとを子ノードとして持つノードを第14ノードとし、第２ノードと第３ノードとを子ノードとして持つノードを第23ノードとし、第14ノードと第23ノードとを子ノードとして持つノードを第1423ノードとする。従って、第１〜第４ノードは、それぞれ第１、第２、第３、第４接地部位１０から成るノード、第14ノードは、第１接地部位１０と第４接地部位１０とから成るノード、第23ノードは、第２接地部位１０と第３接地部位１０とから成るノード、第1423ノードは全ての接地部位１０から成るノードである。
【００９５】
ツリー構造における一般的な呼び方に従い、子ノードを持たないノードを葉ノードと呼び、親ノードを持たないノードを根ノードと呼ぶ。したがって、第ｎノード（n=1,2,3,4）は葉ノード、第1423ノードは根ノードである。なお、本明細書では、ノードを区別するために、葉ノードには、それに対応する接地部位（もしくは脚）と同じ番号（1,2,3,…）を割り付け、葉ノード以外のノードには、葉ノードより大きい番号を割り付けることとする。また、葉ノードおよび根ノード以外のノードを中間ノードと呼ぶ。第１実施形態では、中間ノードは第14ノード、第23ノードである。
【００９６】
目標床反力分配器１０２には、歩容生成装置１００で決定された目標歩容のうち、目標全床反力中心点軌道、目標全床反力軌道および目標接地部位軌道とが入力される。なお、歩容生成装置１００が目標歩容を決定するために使用した歩容パラメータ（ロボット１の遊脚の接地部位１０の着地予定位置、着地予定時刻など）も目標床反力分配器１０２に入力してもよい。そして、目標床反力分配器１０２は、これらの入力を基に、上記のように階層化された各ノードの床反力中心点の目標位置である目標ノード床反力中心点（目標第ｎノード床反力中心点）Qn（n=1,2,3,4,14,23,1423）と、根ノードを除く各ノードの重みＷn（n=1,2,3,4,14,23）とを決定する。ただし、各重みＷnは１以下の非負の値とする。
【００９７】
目標ノード床反力中心点Qn（n=1,2,3,4,14,23）および重みＷn（n=1,2,3,4,14,23）の決定手法を図３（ｂ）、図５および図７などを参照しつつ以下に詳説する。図３（ｂ）には、同図示の状態におけるQnとＷnとの関係が示され、図５（ａ）〜（ｃ）にはWnの設定例が示され、図７には重みと目標ノード床反力中心点との関係が示されている。
【００９８】
根ノードである第1423ノードの目標ノード床反力中心点（目標第1423ノード床反力中心点）Q1423は、目標全床反力中心点（図３および図４中の点Ｐ）と同一に決定される（Ｐ＝Q1423）。従って、以降の説明では、目標第1423ノード床反力中心点を示す符号として、多くの場合、目標全床反力中心点と同じ参照符号Ｐを用いる。
【００９９】
また、根ノードの重みW1423は、表現の統一性を保つために、便宜上「１」に設定される。
【０１００】
第n接地部位１０（n=1,2,3,4）の目標床反力中心点（目標第ｎ接地部位床反力中心点とも言う）は、本実施形態においては、常に第ｎ接地部位１０の中心点（球関節１２の中心点）に設定される。そして、第ｎ接地部位１０に対応する葉ノードの目標ノード床反力中心点Qn（n=1,2,3,4）は、第ｎ接地部位１０の目標床反力中心点と同一に決定される。従って、目標ノード床反力中心点Qn（n=1,2,3,4）も常に第ｎ接地部位１０の中心点（この点の位置は該第ｎ接地部位１０の目標接地部位位置から一義的に定まる）に設定される。このように決定した葉ノードの目標ノード床反力中心点Qn（n=1,2,3,4）の位置は、第ｎ接地部位１０の目標接地部位位置から一義的に定まる。逆に言えば、葉ノードの目標ノード床反力中心点Qn（n=1,2,3,4）は、第ｎ接地部位１０の目標位置を規定するものとなる。なお、以降の説明では、葉ノードの目標ノード床反力中心点を目標接地部位床反力中心点ということもある。また、各目標接地部位軌道は、連続的に変化するように設定されるので、Qn（n=1,2,3,4）も連続的に変化するものとなる。
【０１０１】
補足すると、本実施形態では、第ｎ葉ノード（第ｎ接地部位１０）の目標床反力中心点Qn（n=1,2,3,4）を第ｎ接地部位１０の中心点に設定したが、接地部位の姿勢を制御できるロボットでは、各葉ノード（各接地部位）の目標床反力中心点は、例えば、その葉ノードに対応する接地部位の目標位置姿勢により定まる該接地部位の接地面（目標歩容で想定される床面との接触面）内に設定すればよい。
【０１０２】
以降、一般に、任意の２つの点Ａ，Ｂを結ぶ線分またはその長さをABというように表す。また、演算子「＊」は、スカラーとスカラーとの組もしくはスカラーとベクトルとの組に対しては、乗算を意味し、ベクトルとベクトルとの組に対しては、外積（ベクトル積）を意味する。
【０１０３】
根ノード以外の各ノードの重みＷn（n=1,2,3,4,14,23）と各中間ノードの目標ノード床反力中心点Q14，Q23とは、目標第14ノード床反力中心点Q14が、Q1Q14：Q14Q4＝W4：W1＝(1−W1)：W1という関係式を満たす線分Q1Q4の内分点、目標第23ノード床反力中心点Q23が、Q2Q23：Q23Q3＝W3：W2＝(1−W2)：W2という関係を満たす線分Q2Q3の内分点、目標第1423ノード床反力中心点Ｐ（＝目標全床反力中心点）が、Q14P：PQ23＝W23：W14＝(1−W14)：W14を満たす線分Q14Q23の内分点となるように決定される。
【０１０４】
言い換えると、少なくとも次式１，２，３を満足するように、Q14，Q23，Wn（n=1,2,3,4,14,23）が決定される。なお、これらの式１〜３中のQn（n=1,2,3,4,14,23）は、その点の位置（位置ベクトル）を意味している。
【０１０５】
Q14＝Q1＊W1＋Q4＊W4  ，  W1＋W4＝１      …式１
Q23＝Q2＊W2＋Q3＊W3  ，  W2＋W3＝１      …式２
Ｐ＝Q14＊W14＋Q23＊W23  ，W14＋W23＝１    …式３
Wn（n=1,2,3,4,14,23）が１以下の非負の値であることから、上式１，２，３の右辺におけるQn（n=1,2,3,4,14,23）の係数（重み）は、すべて非負の値となる。
【０１０６】
上式１〜３は、子ノードを持つ各ノード（すなわち葉ノードでない各ノード）の目標ノード床反力中心点の位置が、所定の非負の重みを用いて、そのノードの子ノードの目標ノード床反力中心点の位置の重み付き平均の位置に設定されることを意味する。言い換えると、前記図３（ｂ）および図７に示す如く、子ノードを持つ各ノードの目標ノード床反力中心点Qn（n=14,23,1423）は、そのノードのすべての子ノードの目標ノード床反力中心点の内分点に設定される。図７は、各ノードの目標ノード床反力中心点Qn（n=1,2,3,4,14,23,1423）と重みWn（n=1,2,3,4,14,23）との関係を示す図である。なお、図３（ｂ）中のL23，L14，Ｌ1423はそれぞれ線分Q2Q3、Q1Q4、Q23Q14の長さである。
【０１０７】
補足すると、Q1〜Q4およびＰ（=Q1423)は前記した如く決定されるので、Ｗn（n=1,2,3,4,14,23）を決定すると、式１〜３を満たすQ14，Q23は一義的に定まり、逆に、Q14，Q23を決定すると、式１〜３を満たすＷn（n=1,2,3,4,14,23）は一義的に定まる。従って、重みWn（n=1,2,3,4,14,23）を決定した上で、前記式１〜３に基づいて中間ノードの目標ノード床反力中心点Q14，Q23を決定しても、中間ノードの目標ノード床反力中心点Q14，Q23を決定した上で、前記式１〜３に基づいて重みWn（n=1,2,3,4,14,23）を決定しても、どちらでもよい。
【０１０８】
中間ノードの目標ノード床反力中心点Q14，Q23は、図３（ａ）〜図３（ｃ）および図４（ａ）〜図４（ｃ）に時系列的に示したように、各接地部位１０の移動に伴って移動する。この時の重みＷ14（＝１−W23），W1（＝１−W4），W3（＝１−W2)は、それぞれ例えば前記図５（ａ）〜（ｃ）のグラフで示す如く連続的に変化するように決定される。第ｎノード（n=1,2,3,4）のQnの軌道および重みWnが連続的に変化するように設定されるので、目標ノード床反力中心点Q14，Q23も連続的に移動するように決定される。つまり、すべての目標ノード床反力中心点（目標第nノード床反力中心点（n=1,2,3,4,14,23,1423））は、連続的に移動するように設定される。
【０１０９】
目標床反力分配器１０２は、上記のように決定した各ノードの目標ノード床反力中心点を出力する。なお、根ノードの目標床反力中心点は、歩容生成装置１００が決定する目標全床反力中心点と同じであるので、目標床反力分配器１０２から出力する必要はない。
【０１１０】
目標床反力分配器１０２は、さらに、各ノードの目標床反力中心点に作用する床反力の目標値である目標ノード床反力を決定して出力する。出力する目標ノード床反力には、少なくとも葉ノードである第nノード（n=1,2,3,4）の目標ノード床反力中心点Qnに作用する目標ノード床反力（目標第ｎノード床反力）、すなわち、各接地部位１０の目標床反力が含まれる。この目標ノード床反力は、後述する階層型コンプラインアス動作決定部１１４の処理で実行される、各脚＃１〜＃４のコンプライアンス機構４２など（図１参照）のたわみ補償（より詳しくはコンプライアンス機構４２および各脚のリンク機構のたわみによる各接地部位１０の位置ずれの補償）等のために必要である。補足すると、ロボット１が、各脚＃１〜＃４の先端部以外にもコンプライアンス機構を備える場合には、コンプライアンス機構の変形を求めるために、目標第14ノード床反力、目標第23ノード床反力（中間ノードの目標ノード床反力）も決定して出力することが望ましい。
【０１１１】
一般的に、各ノードの目標ノード床反力中心点に作用する目標床反力（目標ノード床反力）は、目標全床反力と各ノードの重みとから決定すればよい。具体的には、任意のノードの目標床反力は、そのノードの重みとそのノードのすべての先祖ノードの重みとの積に目標全床反力を乗じることによって決定すればよい。すなわち、次式４ａ（または式４ｂ）によって目標第ｎノード床反力が算出される。
【０１１２】
目標第nノード床反力
＝第nノード重み
＊第nノードのすべての先祖ノードの重みの積
＊目標全床反力                            …式４ａ
目標第nノード床反力
＝第nノード重み
＊第nノードのすべての先祖ノード（根ノードを除く）の重みの積
＊目標全床反力                            …式４ｂ
あるいは、葉ノードでない任意の第ｎノードの目標床反力が、第ｎノードのすべての子ノードの目標床反力の和（合力）に一致し、かつ、根ノードの目標床反力が目標全床反力に一致するように、目標各ノード床反力が決定される。この関係を示したものが図８である。
【０１１３】
すなわち、第ｎノードの目標床反力である目標第ｎノード床反力をFn（n=1,2,3,4,14,23,1423）、目標全床反力をFtotalrefで表すと、Ｆnは、図８中に示す式によって、Ftotalrefと、重みWn（n=1,2,3,4,14,23）とから決定される。図８中の式は前記式４ａまたは４ｂと等価である。
【０１１４】
以上のごとく各ノードの目標床反力中心点（目標ノード床反力中心点）と各ノードの目標床反力（目標ノード床反力）とを決定することは、第ｎノードのすべての子ノードの目標床反力の合力が目標第ｎノード床反力中心点まわりに発生するモーメントの水平成分が０となるように各ノードの目標床反力中心点と目標床反力とを決定していることになる。従って、目標ノード床反力のモーメント水平成分は、いずれのノードについても０である。
【０１１５】
なお、接地していない（非接地状態の）接地部位１０の目標床反力（接地していない接地部位１０に対応する葉ノードの目標ノード床反力）が０となるように、各目標ノード床反力中心点Qn（n=1,2,3,4,14,23）、各重みWn(n=1,2,3,4,14,23）および各目標ノード床反力Ｆn（n=1,2,3,4,14,23）の組が決定される。従って、第１実施形態では、図５（ａ）に示したように、第１接地部位１０および第４接地部位１０が遊脚となって離床する時期（該遊脚の接地部位１０が空中を移動する時期）では、重みW14が０（W23＝１）とされ、また、第２接地部位１０および第３接地部位１０が遊脚となって離床する時期では、重みW23が０（W14＝１）とされる。このように、子ノードを持つ中間ノードの重みは、その中間ノードに葉ノードとして属する全ての接地部位が接地していない時期では０に設定される。逆に言えば、中間ノードの重みは、その中間ノードに属するいずれかの接地部位が接地している時期（厳密にはいずれかの接地部位に０でない床反力が作用している時期）では０には設定されない。
【０１１６】
補足すると、第１実施形態では、中間ノードQ14に属する第１接地部位１０および第４接地部位１０のいずれか一方のみを非接地状態とする時期を有しないため、これらの接地部位１０，１０に対応する重みW1，W4は０または１となる時期を持たないものとなっている。ただし、第１接地部位１０および第４接地部位１０のいずれか一方のみが非接地状態となる時期を有するように、目標接地部位軌道を決定するようにした場合には、その時期で非接地状態となる接地部位１０に対応する重みを０に設定し、且つ、接地状態となる接地部位１０に対応する重みを１に設定するようにすればよい。この場合、その時期における中間ノードQ14の重みは、０で無い値に設定される。中間ノードQ23、並びにこれに属する葉ノードである第２接地部位１０および第３接地部位１０に係る重みについても同様である。一般的に言えば、子ノードを持つノードの重みは、そのノードに属する接地部位のいずれかが接地状態であるときには、０で無い値に設定され、そのノードに属する接地部位の全てが非接地状態となるときに、０に設定される。
【０１１７】
上記のように求めた各目標ノード床反力は連続的に変化するので、ロボット１の衝撃の少ない移動（歩行）を実現するために適している。
【０１１８】
以上の如く、第１実施形態では、各目標ノード床反力中心点Qn（n=1,2,3,4,14,23,1423）、各重みWn(n=1,2,3,4,14,23）および各目標ノード床反力Ｆn（n=1,2,3,4,14,23,1423）は、目標床反力分配器１０２によって次の条件Ａ）〜Ｇ）を満たすように決定される。
Ａ）各葉ノードの目標ノード床反力中心点Qn(n=1,2,3,4)は、該葉ノードに対応する接地部位１０の中心点に一致する。より一般的には、Qn(n=1,2,3,4)は、目標歩容（目標接地部位軌道などの目標運動）に応じて決定される。補足すると、例えば各接地部位１０の目標床反力中心点を歩容生成装置１００で決定するようにしてもよく、その場合には、歩容生成装置１００が決定する目標床反力を基に、目標ノード床反力中心点Qn(n=1,2,3,4)を決定すればよい。
Ｂ）根ノードの目標ノード床反力中心点は、目標全床反力中心点Ｐに一致する。
Ｃ）根ノードを除く各ノードの目標ノード床反力中心点Qn（n=1,2,3,4,14,23）と重みWn（n=1,2,3,4,14,23）とは、前記式１〜３の関係式を満たす。換言すれば、子ノードを持つ任意の第ｎノード(n=14,23,1423)の目標ノード床反力中心点Qnは、その第ｎノードの各子ノードの目標ノード床反力中心点の重み付き平均の点となる。
Ｄ）子ノードを持つ任意の第ｎノード（n=14,23,1423）の目標ノード床反力Fnは、該第ｎノードのすべての子ノードの目標ノード床反力の和（合力）に一致し、且つ、根ノード（第1423ノード）の目標ノード床反力F1423は目標全床反力Ftotalrefに一致する。換言すれば、第ｎノード（n=1,2,3,4,14,23,1423）の目標床反力Fnおよび重みWnが図８中の関係式を満たす。
Ｅ）接地していない接地部位１０に対応する葉ノードの目標ノード床反力は０となる。
Ｆ）Qn，Wn，Fn（n=1,2,3,4,14,23,1423）は連続的に変化する。
Ｇ）非接地状態の接地部位１０に対応する葉ノードの重み、または、その葉ノードの祖先ノードのいずれか１つの重みは０とされる。
【０１１９】
なお、目標ノード床反力を決定する際に、前記のごとく重みを基に決定する代わりに、各目標ノード床反力中心点を基に決定しても良い。すなわち、前記Ａ）〜Ｃ）の条件を満たすように各目標ノード床反力中心点を決定した上で、その目標ノード床反力中心点と、前記式１〜３とを基に、各重みを決定し、その決定した重みを用いて前記式４により目標各ノード床反力を決定するようにしても良い。
【０１２０】
図２の説明に戻って、前記姿勢偏差演算部１０３は、ロボット１に目標上体姿勢に対する実上体姿勢の偏差を算出して出力するものである。本実施形態では、該姿勢偏差演算部１０３には、前記姿勢センサ３６で検出された上体２４の鉛直方向に対する傾き角（以下、実上体姿勢傾斜という）と、歩容生成装置１００で決定された目標上体位置姿勢（詳しくは、目標上体姿勢のうちの鉛直方向に対する傾き角。以下、目標上体姿勢傾斜という）とが入力され、これらの偏差θberr（実上体姿勢傾斜−目標上体姿勢傾斜。以下、上体姿勢傾斜偏差θberrという）が算出される。算出するθberrは、Ｘ軸まわり成分（ロール方向成分）θberrxと、Ｙ軸まわり成分（ピッチ方向成分）θberryとから構成される。なお、目標上体姿勢傾斜が例えば定常的に０であるような場合には、θberr＝実上体姿勢傾斜であるので、姿勢センサ３６の検出値（実上体姿勢傾斜）をそのまま、上体姿勢傾斜偏差θberrとして出力すればよい。
【０１２１】
前記姿勢安定化制御演算部１０４は、一般的に言えば、前記上体姿勢傾斜偏差など、ロボット１に備えられたセンサの情報に基づいて検出または推定されるロボット１の状態に応じてロボット１の姿勢を安定化するための全床反力の補償量（目標全床反力の補正量）である補償全床反力を算出するものである。
【０１２２】
ロボット１の姿勢を長期的に安定化させるためには、ロボット１の上体２４などの所定の部位の実際の位置姿勢を望ましい位置姿勢に復元させるために必要な並進力とモーメントとを求め、これを目標全床反力中心点（目標ＺＭＰ）を作用点として付加的に発生させる必要がある。この付加的な並進力とモーメントとを補償全床反力という。また、補償全床反力のモーメント成分を「補償全床反力モーメントＭdmd」（詳しくは目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントＭdmd）と呼ぶ。
【０１２３】
本実施形態では、姿勢安定化制御演算部１０４は、実上体姿勢傾斜を目標上体姿勢傾斜に復元する（近づける）ように補償全床反力モーメントMdmdを算出する。このために、姿勢安定化制御演算部１０４には、前記姿勢偏差演算部１０３で求められた上体姿勢傾斜偏差θberr（θberrx，θberry）が入力される。
【０１２４】
姿勢安定化制御演算部１０４は、入力された上体姿勢傾斜偏差θberrを基に、前記補償全床反力モーメントMdmdを算出する。算出するMdmdは、Ｘ軸まわり成分Ｍdmdxと、Ｙ軸まわり成分Ｍdmdyとから構成される。
【０１２５】
ＭdmdxおよびＭdmdyは、具体的には、例えば次式５，６のフィードバック制御則（ここではＰＤ制御則）によって決定される。すなわち、ＭdmdxおよびＭdmdyは、上体姿勢傾斜偏差θberrx，θberryを０に近づけるように決定される。
【０１２６】
Mdmdx＝−Kthx＊θberrx−Kwx＊(dθberrx/dt)    …式５
Mdmdy＝−Kthy＊θberry−Kwy＊(dθberry/dt)    …式６
ここで、Ｋthx，Ｋthy，ＫwxおよびＫwyは、所定のゲインである。また、（ｄθberrx/dt），（ｄθberry/dt）は、それぞれ上体姿勢傾斜偏差θberrx、θberryの時間微分値である。
【０１２７】
本実施形態においては、補償全床反力モーメントMdmdのＺ軸まわり成分（ヨー方向成分）Mdmdzを用いないので、Mdmdzの決定を行なわないが、ロボット１のスピン（鉛直軸回りの滑り）を防止したりするためにMdmdzを決定するようにしても良い。Mdmdzの決定法に関しては、本願出願人が先に提案した特願２００３−１８５６１３号や特願２００３−１８５９３０号にて詳細に説明されている。また、例えばロボット１の実際の重心の位置を目標歩容での重心の位置に復元するために、その重心の位置偏差に応じて補償全床反力の並進力を決定するようにすることも可能である。
【０１２８】
前記床反力検出器１０８は、各脚＃１〜＃４の６軸力センサ３４の出力を基に、実際のロボット１の各接地部位１０に作用する床反力の実際値である実床反力（すなわち葉ノードの実床反力（実ノード床反力））を検出する。さらに、床反力検出器１０８は、ロボット１の各関節１４，１５に備えたエンコーダ等のセンサ（図示しない）によって検出される各脚＃１〜＃４の各関節１４，１５の実関節変位（各関節１４，１５の各回転軸まわりの実回転角）に基づき、上体２４に固定された座標系に対する各接地部位１０の相対位置姿勢（第１実施形態では相対位置）を算出する。なお、このとき、実関節変位の代わりに、各関節１４，１５の変位指令値（回転角指令値）である関節変位指令を用いたり、あるいは、実関節変位と関節変位指令との両者を使用してもよい。そして、算出した各接地部位１０の相対位置姿勢を基に、各脚＃１〜＃４毎の６軸力センサ３４の検出値（これは６軸力センサ３４などに固定されたローカル座標系での値である）を座標変換して、上体２４に固定された座標系で表現された実床反力を算出した後、これを支持脚座標系（グローバル座標系）で表現された実床反力に変換する。支持脚座標系への座標変換には、姿勢センサ３６の検出値、あるいは目標上体姿勢傾斜を用いればよい。補足すると、第１実施形態のロボット１では、前記したように接地部位１０の中心点に床反力モーメントが作用しないので、各接地部位１０の実床反力のうちのモーメント成分を検出する必要はない。この場合、前記したように６軸力センサ３４の代わりに、３軸力センサを用いて、実床反力の３軸方向の並進力成分を検出したり、１軸床反力センサを用いて実床反力の並進力鉛直成分だけを検出するようにしてもよい。
【０１２９】
前記ロボット幾何学モデル（逆キネマティクス演算部）１１０は、各接地部位位置姿勢の最終的な目標軌道（これは後述の階層型コンプライアンス動作決定部１１４により決定される）と目標上体位置姿勢などを基に、逆キネマティクスの演算によって、それらを満足するロボット１の各関節１４，１５の変位（回転角）の指令値である各関節変位指令を算出する。本実施形態では、逆キネマティクスの演算の解の式をあらかじめ求めておき、その式に目標上体位置姿勢と各接地部位の最終的な目標位置を代入するだけで各関節変位指令を算出するようにした。すなわち、ロボット幾何学モデル１１０は、歩容生成装置１００で決定された目標上体位置姿勢軌道と階層型コンプライアンス動作決定部１１４で後述する如く修正された修正目標接地部位軌道（機構変形補償入り修正目標接地部位軌道）とが入力され、それらの入力値から逆キネマティクスの演算によって、各脚＃１〜＃４の各関節１４，１５の関節変位指令を算出する。
【０１３０】
なお、後述する第６の実施形態のように腕関節や首関節など脚関節以外の関節があるロボットの場合には、手や頭部などの上体に対する相対位置姿勢を基に、逆キネマティクスの演算によって、脚関節以外の関節の変位が決定される。
【０１３１】
前記変位コントローラ１１２は、ロボット１の各関節１４，１５の実関節変位（検出値）と、ロボット幾何学モデル（逆キネマティクス演算部）１１０で算出された関節変位指令とが入力され、該関節変位指令を目標値として、この目標値に実関節変位を追従させるように各関節１４，１５のアクチュエータ（図示しない）を制御（フィードバック制御）する。
【０１３２】
階層型コンプライアンス動作決定部１１４は、目標全床反力と補償全床反力との合力に実全床反力を近づけるように目標接地部位軌道を修正し、その修正後の目標接地部位軌道である機構変形補償付き修正目標接地部位位置姿勢軌道を出力するものである。なお、本実施形態では、各接地部位１０の姿勢は制御できないので、機構変形補償付き修正目標接地部位位置姿勢軌道は、実際には、機構変形補償付き修正目標接地部位位置軌道である。
【０１３３】
階層型コンプライアンス動作決定部１１４は、一般的には、以下の３つの要求をできるだけ満足させるように各接地部位１０の目標接地部位軌道を修正するものである。
【０１３４】
要求１）ロボット１の位置姿勢を安定化するために、実全床反力を姿勢安定化制御演算部１０４が出力する補償全床反力（モーメントMdmd）と目標全床反力との合力に追従させる。なお、第１実施形態ではロボット１の上体２４の姿勢傾斜（鉛直方向に対する傾き）を安定化するため、目標全床反力中心点まわりの実全床反力モーメントの水平成分を補償全床反力モーメントＭdmdの水平成分Mdmdx，Mdmdyに追従させる。補足すると、目標全床反力中心点まわりの目標全床反力モーメントの水平成分は０であるので、これとMdmdとの合力は、Mdmdに一致する。
要求２）接地しているべき複数の接地部位１０に対し、その一部の接地部位１０に実床反力が集中し、それ以外のある一部の接地部位１０の実床反力が減少し過ぎ、その実床反力が減少した接地部位１０の接地性が極端に低下しないように、できる限り葉ノードでない各ノードの目標床反力中心点まわりの実床反力モーメントの絶対値を小さくする。なお、第１実施形態では、第14ノード、第23ノード、第1423ノードの目標床反力中心点まわりの実床反力モーメントの絶対値をできる限り小さくする。
要求３）接地しているべき各接地部位１０の接地性を確保するように、すなわち各接地部位１０におけるローカルな接地圧分布（実床反力の分布）が偏って、該接地部位１０の局所的な接地性が低下しないように、できるかぎり各接地部位１０（各葉ノード）の目標床反力中心点まわりの実床反力モーメントの絶対値を小さくする。但し、第１実施形態のロボット１では、各接地部位１０の目標床反力中心点まわりの実床反力モーメントは常に０となるので、この要求３）は考慮する必要はない。
【０１３５】
尚、一般に、要求１）〜３）の全て、あるいは要求１）、２）を完全に満足することはできない。例えば実全床反力を補償全床反力と目標全床反力との合力に一致させながら各接地部位１０の目標床反力中心点まわりの実床反力モーメントを０にすることが、物理的に不可能な場合が多い。従って、階層型コンプライアンス動作決定部１１４は、一般には、要求１）〜３）、あるいは要求１）、２）をできるだけ満たしつつ、ある妥協点で、各接地部位１０の目標接地部位軌道を修正することとなる。
【０１３６】
以上が制御装置５０の各機能的手段（機能的構成要素）の概要である。
【０１３７】
補足すると、前記階層型コンプライアンス動作決定部１１４、姿勢安定化制御演算部１０４およびロボット幾何学モデル（逆キネマティクス演算部）１１０は、本発明におけるノード動作制御手段に相当するものである。
【０１３８】
次に、図９のフローチャートを参照しつつ、制御装置５０の全体的な動作（演算処理）をより詳細に説明する。図９は制御装置５０のメインルーチン処理を示すフローチャート（構造化フローチャート）である。尚、図９の左端には、該当する処理を行う制御装置５０の構成要素を示す。
【０１３９】
先ず、Ｓ１０において制御装置５０の初期化が行なわれた後、Ｓ１２を経てＳ１４に進み、制御装置５０の演算処理は、該制御装置５０の制御周期毎のタイマ割り込みを待つ状態となる。制御周期は例えば５０ｍｓである。
【０１４０】
続いてＳ１６に進んで歩容の切り替わり目か否か判断し、その判断結果がＮＯであるときは後述のＳ２２に進む。また、Ｓ１６の判断結果がＹＥＳであるときはＳ１８に進んで時刻ｔを０に初期化した後、Ｓ２０に進んで歩容パラメータを設定する。本実施形態では、例えばロボット１のある所定の脚（例えば＃１）が離床してから次に離床するまで（もしくは着地してから次に着地するまで）の所定期間分の目標歩容を単位として、その所定期間分の目標歩容を規定するパラメータ（目標歩容を決定するためのアルゴリズムで使用するパラメータ）である歩容パラメータがＳ２０で設定される。そして、Ｓ１６でいうところの「歩容の切り替り目」は、上記所定期間分の目標歩容の切り替り目のことである。なお、目標歩容の切り替り目の判断は、時刻や、上記所定の脚の６軸力センサ３４の検出値などを基に行えばよい。
【０１４１】
Ｓ２０で設定される歩容パラメータは、ロボット１の目標運動軌道（詳しくは目標接地部位軌道および目標上体位置姿勢軌道）を規定する運動パラメータと、目標床反力軌道（詳しくは目標全床反力軌道および目標全床反力中心点軌道）を規定する床反力パラメータとから構成される。補足すると、ロボット１の目標運動が決まれば、その目標運動によって発生する慣性力とロボット１に作用する重力との合力の符号を反転させたものを目標全床反力とすればよいので、床反力パラメータは、目標全床反力中心点軌道のみを規定するものでもよい。また、目標接地部位軌道（より一般的には目標上体位置姿勢以外の目標運動のパラメータ）と目標全床反力中心点とを決定した上で、ロボット１の動力学モデルを使用し、ロボット１の目標上体位置姿勢を含む目標運動が発生する慣性力とロボット１に作用する重力との合力が目標全床反力中心点（目標ＺＭＰ）まわりに発生するモーメントの水平成分が０になるように、目標上体位置姿勢を決定するような場合には、運動パラメータには、目標上体位置姿勢軌道を規定するパラメータが含まれていなくてもよい。
【０１４２】
続いてＳ２２に進み、上記歩容パラメータを基に、目標歩容の瞬時値を決定する。ここで「瞬時値」は制御周期ごとの値を意味し、目標歩容瞬時値は、目標上体位置姿勢、目標接地部位位置（目標接地部位軌道の瞬時値）、目標全床反力、および目標全床反力中心点位置（目標ＺＭＰ位置）の瞬時値から構成される。なお、本実施形態では、接地部位１０の姿勢を制御できないので、各接地部位１０の目標姿勢の瞬時値は決定しない。各接地部位の姿勢を制御できる場合には、前記歩容パラメータに各接地部位の目標姿勢を規定するパラメータを含めておき、そのパラメータを基に各接地部位の目標姿勢の瞬時値を決定すればよい。
【０１４３】
上記Ｓ１４〜Ｓ２２の処理は、前記歩容生成装置１００により実行される処理である。
【０１４４】
続いてＳ２４に進んで、各ノードの重みWn(n=1,2,3,4,14,23）と各ノードの目標床反力中心点（目標ノード床反力中心点）Qn(n=1,2,3,4,14,23)とが求められる。この処理は目標床反力分配器１０２によって前記した如く実行される処理である。
【０１４５】
続いてＳ２６に進んで、目標ノード床反力（少なくとも各接地部位１０（各葉ノード）の目標床反力を含む）が求められる。第１実施形態では、各葉ノードの目標ノード床反力（各目標接地部位床反力）が求められる。このＳ２６処理も、目標床反力分配器１０２によって前記した如く実行される処理である。なお、前述の通り、各脚＃１〜＃４の先端部以外にもコンプライアンス機構を設けた場合には、さらに葉ノードでない各中間ノードの目標床反力も求めた方が良い。補足すると、目標ノード床反力のモーメント水平成分は０である。
【０１４６】
続いてＳ２８に進み、姿勢センサ３６などの出力から前記実上体姿勢傾斜などのロボット１の状態を検出する。第１実施形態では、前記姿勢偏差演算部１０３によって、姿勢センサ３６による実上体姿勢傾斜の検出値が取り込まれ、その検出値と目標上体位置姿勢のうちの目標上体姿勢傾斜（現在時刻の瞬時値）とから、上体姿勢傾斜偏差θberrが算出される。
【０１４７】
続いてＳ３０に進み、Ｓ２８で検出したロボット１の状態などからロボット１の姿勢を安定化するための補償全床反力を求める。第１実施形態では、前記姿勢安定化制御演算部１０４によって、前記上体姿勢傾斜偏差θberrから、目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントMdmdの水平成分Mdmdx，Mdmdyが前記式５および式６に従って算出される。
【０１４８】
続いてＳ３２に進んで各接地部位１０の実床反力を検出する。これは前記実床反力検出器１０８によって実行される処理であり、前記の如く、６軸力センサ３４によって検出された各接地部位１０毎の実床反力を支持脚座標系（グローバル座標系）に変換したものが求められる。以降、各接地部位１０の実床反力を実接地部位床反力ということがある。
【０１４９】
続いてＳ３４からＳ３８まで、前記階層型コンプライアンス動作決定部１１４の処理が実行される。
【０１５０】
以下、しばらくの間、これらのＳ３４〜Ｓ３８の処理を具体的に説明する前に、階層型コンプライアンス動作決定部１１４の全体的な処理を説明する。なお、この説明は、理解の便宜上、主に前記図３（ｂ）に示す状態（ロボット１の全ての脚＃１〜＃４が支持脚となっている状態）を例にとって行なう。
【０１５１】
階層型コンプライアンス動作決定部１１４は、目標床反力分配器１０２で決定された各葉ノードの目標ノード床反力などを基に、葉ノードを除く各ノードの目標ノード床反力の並進力成分とモーメント成分（より詳しくは目標ノード床反力中心点を作用点とする並進力成分とモーメント成分）とを決定する。
【０１５２】
図３（ｂ）の状態における各ノードの目標床反力（各目標ノード床反力）の並進力成分を、例示的に図１０に示す。この図において、ベクトルＦn_ref（n=1,2,3,4,14,23）が目標第ｎノード床反力の並進力成分を表している。また、Ｆtotalrefは目標全床反力（＝目標第1423ノード床反力）の並進力成分を表す。前記図８に示したように、葉ノードでない任意の第ｎノード(n=14,23,1423)の目標第ｎノード床反力は、第ｎノードのすべての子ノードの目標ノード床反力の合力に決定される。従って、F14ref＝F1ref＋F4ref、F23ref＝F2ref＋F3ref、Ftotalref(＝F1423ref)＝F14ref＋F23refである。階層型コンプライアンス動作決定部１１４は、目標床反力分配器１０２で決定された各葉ノード（各接地部位１０）の目標ノード床反力の並進力成分Fn_ref(n=1,2,3,4)から、上記の如く、各中間ノードの目標ノード床反力の並進力成分F14ref（＝F1ref＋F4ref)、F23ref（＝F2ref＋F3ref）を求める。なお、Ftotalref(＝F1423ref)は、前記歩容生成装置１００で決定された目標全床反力の並進力成分に設定される。
【０１５３】
さらに、階層型コンプライアンス動作決定部１１４は、目標ノード床反力の並進力成分の場合と同様に、葉ノードを除く各ノードの目標ノード床反力の、目標ノード床反力中心点を作用点とするモーメント成分を決定する。ただし、この場合、目標ノード床反力中心点Qn（n=1,2,3,4,14,23）の定義より、目標第nノード床反力のモーメント水平成分は、常に０に設定される。目標第1423ノード床反力（＝目標全床反力）のモーメント水平成分も０に設定される。
【０１５４】
本実施形態のロボット１においては、各接地部位１０は各脚＃１〜＃４の先端部の球関節１２（フリージョイント）に係合しているので、各接地部位１０（各葉ノード）に床反力モーメント（水平成分および鉛直成分）を発生させることができない。このため、階層型コンプライアンス動作決定部１１４では、各接地部位１０（各葉ノード）の目標ノード床反力のモーメント鉛直成分も０に設定される。
【０１５５】
葉ノードでないノードの目標ノード床反力モーメント鉛直成分は、ロボット１の目標運動に動力学的に釣り合うように決定すると、一般的には０ではない値も採り得るが、本実施形態では、ロボット１の姿勢の鉛直軸まわりの回転（ヨー方向の回転）に関する制御を行なわない。このため、本実施形態では、葉ノードでないノードの目標ノード床反力のモーメントの鉛直成分の設定は省略する。このようなことから、図３（ｂ）の状態における各目標ノード床反力のモーメント成分の図示は省略する。尚、ロボット１の姿勢の鉛直軸まわりの回転に関する制御を行なう場合には、各ノードの目標床反力モーメント鉛直成分も設定すべきである。
【０１５６】
補足すると、葉ノードを含めた各ノードの目標ノード床反力の並進力成分およびモーメント成分の決定を、前記目標床反力分配器１０２で行なうようにした場合には、その決定を階層型コンプラインアンス動作決定部１１４で行なう必要はない。
【０１５７】
また、階層型コンプライアンス動作決定部１１４は、各ノードの実床反力である実ノード床反力の並進力成分およびモーメント成分も決定する。
【０１５８】
図３（ｂ）の状態における各ノードの実床反力（実ノード床反力）の並進力成分を、例示的に図１１に示す。この図において、ベクトルＦn_act（n=1,2,3,4,14,23）が実第ｎノード床反力の並進力成分を表している。また、Ｆtotalactは実全床反力（＝実第1423ノード床反力）の並進力成分を表す。一般に、葉ノードでない各ノードの実床反力の並進力成分は、そのノードのすべての子ノードの実床反力の並進力成分の合力とする。従って、第14ノード、第23ノードおよび第1423ノードの実床反力の並進力成分はF14act＝F1act＋F4act、F23act＝F2act＋F3act、Ftotalact(＝F1423act）＝F14act＋F23actである。また、各葉ノードの実床反力の並進力成分F1act，F2act，F3act，F4actは、前記実床反力検出器１０８で得られる各接地部位１０の実床反力（実接地部位床反力）の並進力成分である。なお、図１１の点線で示したベクトルは、前記図１０に示した目標ノード床反力の並進力成分である。階層型コンプライアンス動作決定部１１４は、上記のように、実床反力検出器１０８で得られた各接地部位１０の実床反力から各ノードの実ノード床反力の並進力成分を決定する。
【０１５９】
また、図３（ｂ）の状態における各ノードの実床反力のモーメント成分を、例示的に図１２に示す。この図において、ベクトルMn_act（n=14,23,1423）は実第ｎノード床反力のモーメント成分を表す。この場合、各ノードの実床反力の並進力成分と同様に、一般的には、葉ノードでないノードの実床反力のモーメント成分M14act、M23act、M1423act（＝Mtotalact）は、そのノードのすべての子ノードの実床反力の合力のモーメント成分（そのノードの目標床反力中心点Q14，Q23，Q1423を作用点とするモーメント成分）とする。
【０１６０】
本実施形態のロボット１においては、前記したように各接地部位１０（各葉ノード）に床反力モーメントを発生させることができないので、各葉ノードの実床反力（実接地部位床反力）のモーメント成分は、常に０になる。よって、各葉ノードの実ノード床反力のモーメント成分の図示は省略する。
【０１６１】
葉ノードでないノードの実床反力モーメント成分（本実施形態では、M14act，M23act，M1423act）は、一般的に、０にならない。例えば目標第14ノード床反力中心点Q14のまわりには、一般には、第１接地部位１０（第１ノード）および第４接地部位１０（第４ノード）の実床反力の並進力成分の水平成分によってモーメントが発生する。ただし、本実施形態のロボット１においては、脚の先端部にフリージョイントである球関節１２を備えているので、M14actの線分Q1Q4と同方向の成分、M23actの線分Q2Q3と同方向の成分は０となる。
【０１６２】
尚、各脚の先端部の関節にアクチュエータを備えたロボット（各脚の先端部の接地部位の姿勢を制御可能なロボット）においては、該ロボットの姿勢の鉛直軸まわり回転に関する制御を行なわない場合でも、本願出願人が先に提案した特開平１０−２７７９６９号公報の実施例に示すごとく、各接地部位の目標床反力モーメント水平成分も設定し、また、各接地部位の実床反力モーメントも検出する。そして、各接地部位の実床反力モーメント水平成分を目標床反力モーメント水平成分に近づけるように、あるいは、各接地部位の実床反力モーメントを、目標床反力モーメント水平成分とその接地部位補償床反力モーメントとの和（ベクトル和）に近づけるように、各接地部位の姿勢の補正動作を行うべきである。
【０１６３】
階層型コンプライアンス動作決定部１１４の処理をより詳細に説明する。この場合、図３（ｂ）に示す状態で、ロボット１の上体２４の姿勢が左後ろ側に倒れそうな状態となり、姿勢安定化制御演算部１０４が決定した補償全床反力モーメントMdmdが、図１３のようになっている状況を想定する。
【０１６４】
ロボット１の上体２４の姿勢（鉛直方向に対する傾き）を復元させる（目標上体姿勢における傾きに復元させる）ためには、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平成分を、目標全床反力モーメントMtotalref（＝M1423ref）と補償全床反力モーメントMdmdとの和の水平成分に追従させれば良い。
【０１６５】
一方、目標全床反力中心点（目標ＺＭＰ）では目標全床反力モーメントMtotalrefの水平成分は０である。従って、ロボット１の上体２４の前後左右の姿勢（傾き）を復元させるためには、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平成分を、Ｍdmdの水平成分（Mdmdx，Mdmdy）に追従させれば良い。また、本実施形態では、各接地部位１０の目標床反力中心点まわりの実床反力モーメントは０である。
【０１６６】
そこで、第１実施形態における階層型コンプライアンス動作決定部１１４は、前記した要求１）、２）の要求をできる限り満足するよう歩容生成装置１００で決定した各接地部位１０の目標接地部位位置（特に高さ方向の位置）を修正する。
【０１６７】
この修正を行なうために、階層型コンプラインアス動作決定部１１４は、補償角というものを決定する。この補償角は、接地部位１０の位置の間の相対関係をある点まわりの回転操作によって修正する（本実施形態では上下方向で修正する）ための操作量（回転量）である。本実施形態においては、その補償角としては、第14ノード補償角θ14、第23ノード補償角θ23、第1423ノード補償角θ1423がある。すなわち、葉ノード以外のノードの補償角である。そして、階層型コンプライアンス動作決定部１１４は、これらの補償角θ14，θ23，θ1423を基に、目標第ｎノード床反力中心点Qn（n=1,2,3,4,14,23）を修正してなる目標第ｎノード床反力中心点Qn'を決定する。
【０１６８】
図１５に示すように、第14ノード補償角θ14は、線分Q1Q4と線分Q1'Q4'とが成す角度、第23ノード補償角θ23は、線分Q2Q3と線分Q2'Q3'とが成す角度である。また、図１６に示すように、第1423ノード補償角θ1423は、線分Q14Q23と線分Q14'Q23'とが成す角度である。これらのノード補償角θ14，θ23，θ1423の決定手法は後述する。
【０１６９】
図１５および図１６を参照して、補償角θ14，θ23，θ1423を基にした目標第ｎノード床反力中心点Qn（n=1,2,3,4,14,23）の修正は次のように行なわれる。
【０１７０】
図１５を参照して、目標第１ノード床反力中心点Q1（第１接地部位１０の目標床反力中心点）と目標第４ノード床反力中心点Q4（第２接地部位１０の目標床反力中心点）とを含みかつ水平面と垂直な平面の法線ベクトルV14を求める。V14の大きさは１とする。目標第１ノード床反力中心点Q1の座標（位置）を、目標第14ノード床反力中心点Q14を回転中心として法線ベクトルV14まわり（Q14を通ってV14に平行な軸まわり）に、前記第14ノード補償角θ14だけ回転移動する。この回転移動によるQ1の移動後の点をQ1'とする。同様に、目標第４ノード床反力中心点Q4の座標（位置）を、目標第14ノード床反力中心点を回転中心として法線ベクトルV14まわりに第14ノード補償角θ14だけ回転移動する。この回転移動によるQ4の移動後の点をQ4'とする。換言すれば、線分Q1Q4をその内分点であるQ14を回転中心としてV14まわりにθ14だけ回転させて得られる線分の端点をQ1'，Q4'とする。従って、第14ノード補償角θ14は、第14ノードの目標床反力中心点Q14の位置を動かさずに、第14ノードの子ノードである第１ノードおよび第４ノードのそれぞれの目標床反力中心点Q1，Q4の位置の相対関係を動かすための操作量である。
【０１７１】
また、目標第２ノード床反力中心点Q2と目標第３ノード床反力中心点Q3を含みかつ水平面と垂直な平面の法線ベクトルV23を求める。V23の大きさは１とする。目標第２ノード床反力中心点Q2の座標（位置）を、目標第23ノード床反力中心点Q23を回転中心として法線ベクトルV23まわり（Q23を通ってV23に平行な軸まわり）に、第23ノード補償角θ23だけ回転移動する。図１５に示すように、この回転移動によるQ2の移動後の点をQ2'とする。同様に、目標第３接地部位床反力中心点Q3の座標（位置）を、目標第23ノード床反力中心点を回転中心として法線ベクトルV23まわりに第23ノード補償角θ23だけ回転移動する。この回転移動によるQ3の移動後の点をQ3'とする。換言すれば、線分Q2Q3をその内分点であるQ23を回転中心としてV23まわりにθ23だけ回転させて得られる線分の端点をQ2'，Q3'とする。従って、第23ノード補償角θ23は、第23ノードの目標床反力中心点Q23の位置を動かさずに、第23ノードの子ノードである第２ノードおよび第３ノードのそれぞれの目標床反力中心点Q2，Q3の位置の相対関係を動かすための操作量である。
【０１７２】
次に、図１６を参照して、目標第14ノード床反力中心点Q14と目標第23ノード床反力中心点Q23を含みかつ水平面と垂直な平面の法線ベクトルV1423を求める。V1423の大きさは１とする。目標第14ノード床反力中心点Q14の座標（位置）を、目標全床反力中心点Ｐ（=Q1423）を回転中心として法線ベクトルV1423まわり（Ｐを通ってV1423に平行な軸まわり）に、第1423補償角θ1423だけ回転移動する。この回転移動によるQ14の移動後の点をQ14'とする。同様に、目標第23接地部位床反力中心点Q23の座標（位置）を、目標全床反力中心点Ｐを回転中心として法線ベクトルV1423まわりに第1423補償角θ1423だけ回転移動する。この回転移動によるQ23の移動後の点をQ23'とする。換言すれば、線分Q14Q23をその内分点であるＰ（=Q1423)を回転中心としてV1423まわりにθ1423だけ回転させて得られる線分の端点をQ14'，Q23'とする。従って、第1423ノード補償角θ1423は、第1423ノードの目標床反力中心点Ｐの位置を動かさずに、第1423ノードの子ノードである第14ノードおよび第23ノードのそれぞれの目標床反力中心点Q14，Q23の位置の相対関係を動かすための操作量である。
【０１７３】
なお、以降、一般に、始点がＡ、終点がＢであるベクトルをベクトルA_Bと表記する。
【０１７４】
次に、Q1'をベクトルQ14_Q14'だけ移動した点Q1''を求める。同様に、Q4'をベクトルQ14_Q14'だけ移動した点Q4''を求める。また、Q2'をベクトルQ23_Q23'だけ移動した点Q2''を求める。同様に、Q3'をベクトルQ23_Q23'だけ移動した点Q3''を求める。
【０１７５】
次いで、第ｎ接地部位（n=1,2,3,4）の目標接地部位位置をベクトルQn_Qn''だけ平行移動（ほぼ上下移動）させる。これにより、各接地部位１０の目標接地部位位置が修正されることとなる。補足すると、各ノードの目標床反力中心点と同様に、各葉ノードの位置を該葉ノードに対応する接地部位１０の位置とし、子ノードを持つ各ノードの位置を、そのノードの全ての子ノードの位置の、前記の如く設定された重みを用いた重み付け平均の位置として定義したとき、上記のように、第14ノード、第23ノード、第1423ノードのそれぞれの各子ノードの目標床反力中心点を補償角θ14、θ23、θ1423に応じて動かす処理は、第14ノード、第23ノード、第1423ノードのそれぞれの各子ノードの位置を補償角θ14、θ23、θ1423に応じて動かすことと同じことである。
【０１７６】
尚、各脚の先端部にフリージョイントがないロボット（接地部位の姿勢を制御可能なロボット）においては、上記平行移動を各接地部位の姿勢（目標姿勢）を変えずに行なった後に、さらに、第ｎ接地部位をＱn''を中心に、前後方向軸（Ｘ軸）まわりに、ある回転角θn_ｘだけ回転させると共に、左右方向軸（Ｙ軸）まわりに、ある回転角θn_ｙだけ回転させる。これにより各接地部位の目標姿勢を修正する。この場合の回転角θn（θn_x、θn_yからなる２次元量）を第ｎ接地部位補償角と呼び、θn_ｘを第ｎ接地部位補償角Ｘ成分、θn_yを第ｎ接地部位補償角Ｙ成分と呼ぶ。第ｎ接地部位補償角θnは、本願出願人が先に提案した特開平１０−２７７９６９号公報に記載された手法に従って決定すればよい。
【０１７７】
補足すると、前記各ノード補償角は、そのノードの目標ノード床反力中心点を作用点として、そこに本来発生すべき目標床反力モーメント（その水平成分は０）とノード補償床反力モーメントMn_dmdとの合力が発生するように決定される。この場合、ノード補償床反力モーメントMn_dmdの基となる補償全床反力モーメントMdmdは、実姿勢傾斜偏差を０に近づけるように決定されたものである。従って、各ノード補償角は、目標全床反力中心点まわりの床反力モーメントを所望のモーメント（この場合は補償全床反力モーメント）に近づけつつ、実姿勢傾斜偏差を０に近づけるよう各接地部位１０の相対位置関係を操作するための操作量として機能することとなる。
【０１７８】
以上のごとく、各接地部位１０の目標接地部位位置（詳しくはその位置の相互の相対関係）を修正する処理が第１実施形態における階層型のコンプライアンス動作である。このコンプライアンス動作では、子ノードを持つ各ノード毎に、そのノードの子孫ノードとなっている各接地部位１０の目標接地部位位置の間の相対関係（相対的位置関係）の操作量（修正量）が各補償角θ14，θ23，θ1423に応じて定められ、それらの操作量（修正量）を合成することで、各接地部位１０の目標接地部位位置の相互の相対関係が修正されることとなる。上記のように修正された目標接地部位位置姿勢を修正目標接地部位位置姿勢という。
【０１７９】
このようなコンプライアンス動作では、一般に、前記補償角（補償動作量）が過大でなければ、各接地部位１０の接地圧力分布は変わっても、接地領域（接地面の圧力が正の領域）は変わらない。このような場合には、補償角に比例して各接地部位１０に装着されたコンプライアンス機構４２などが変形し、その変形量に応じた各接地部位１０の実床反力が発生する。この結果、補償角と補償角によって発生する実床反力の変化量との間の関係は、以下に示す良好な特性、すなわち線形特性を持つ。
【０１８０】
特性１）葉ノードでないノードの目標床反力中心点まわりの補償角だけを操作して各接地部位１０の目標接地部位位置を移動させると、下がった接地部位１０の実床反力の並進力成分が増加し、上がった接地部位１０の実床反力の並進力成分が減少する。このとき、各接地部位１０（各葉ノード）の修正された目標床反力中心点まわりの実床反力モーメントは、ほとんど変化しない。
特性２）第ｎ接地部位補償角だけを操作して第ｎ接地部位の目標姿勢を回転させると、第ｎ接地部位の目標床反力中心点に作用する実床反力のモーメント成分が変化し、並進力成分はあまり変化しない。
特性３）葉ノードでないノードの目標床反力中心点まわりの補償角と第ｎ接地部位補償角を同時に操作すると、各接地部位１０の実床反力の変化量は、それぞれを単独に操作したときの変化量の和になる。
【０１８１】
図１７は、本実施形態における階層型コンプライアンス動作決定部１１４の機能を示すブロック図であり、同図を参照して階層型コンプライアンス動作決定部１１４の処理をさらに詳細に説明する。
【０１８２】
階層型コンプライアンス動作決定部１１４は、その機能的手段として、補償全床反力モーメント分配器１１４ａ、補償角決定部１１４ｂ，１１４ｃ，１１４ｄ、修正目標接地部位位置姿勢算出部１１４ｇ、機構変形補償量算出部１１４ｎ、および機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈを備えている。
【０１８３】
補償全床反力モーメント分配器１１４ａは、前記補償全床反力モーメントMdmd（Mdmdx，Mdmdy）を、第1423ノード補償床反力モーメントM1423dmd、第14ノード補償床反力モーメントM14dmdおよび第23ノード補償床反力モーメントM23dmdに分配するものである。
【０１８４】
第1423ノード補償床反力モーメントM1423dmdは、第1423補償角θ1423を操作することによって発生する各接地部位１０（詳しくは第１〜第４接地部位）の床反力の並進力成分が、目標第1423ノード床反力中心点（＝目標全床反力中心点（目標ＺＭＰ））まわりに発生すべきモーメントの目標値である。
【０１８５】
第1423ノード補償床反力モーメントM1423dmdの前記ベクトルV1423方向の成分（V1423方向の軸まわりの成分）をM1423dmdvと記述する。尚、ベクトルV1423は階層型コンプライアンス動作決定部１１４のコンプライアンス動作の全体的な説明で定義したベクトルである（図１６参照）。V1423に直交し、且つ鉛直方向にも直交するベクトルをU1423とすると、本実施形態では、第1423ノード補償床反力モーメントM1423dmdのU1423方向の成分（U1423方向の軸回りの成分）M1423dmduは０に設定される。これは、本実施形態のロボット１では、第1423ノード補償角θ1423を操作しても、床反力モーメントのU1423方向成分を発生することはできないからである。また、本実施形態では鉛直軸まわりのロボット１の姿勢の回転を制御しないので、M1423dmdの鉛直方向成分M1423dmdzも０に設定される。
【０１８６】
第14ノード補償床反力モーメントM14dmdは、第14補償角θ14を操作することによって発生する各接地部位１０（詳しくは第１および第４接地部位）の床反力の並進力成分が、目標第14ノード床反力中心点まわりに発生すべきモーメントの目標値である。
【０１８７】
第14ノード補償床反力モーメントM14dmdのベクトルV14方向成分をM14dmdvと記述する。尚、ベクトルV14は階層型コンプライアンス動作決定部１１４のコンプライアンス動作の全体的な説明で定義したベクトルである（図１５を参照）。V14に直交し、且つ鉛直方向にも直交するベクトルをU14とすると、本実施形態では、第14ノード補償床反力モーメントM14dmdのU14方向成分M14dmduは０に設定される。これは、本実施形態のロボット１では、第14ノード補償角θ14を操作しても、床反力モーメントのU14方向成分を発生することはできないからである。なお、本実施形態では、M14dmdの鉛直方向成分も０に設定される。
【０１８８】
第23ノード補償床反力モーメントM23dmdは、第23補償角θ23を操作することによって発生する各接地部位１０（詳しくは第２および第３接地部位）の床反力の並進力成分が、目標第23ノード床反力中心点まわりに発生すべきモーメントの目標値である。
【０１８９】
第23ノード補償床反力モーメントM23dmdのベクトルV23方向成分をM23dmdvと記述する。尚、ベクトルV23は階層型コンプライアンス動作決定部１１４のコンプライアンス動作の全体的な説明で定義したベクトルである（図１５を参照）。V23に直交し、且つ鉛直方向にも直交するベクトルをU23とすると、本実施形態では、第23ノード補償床反力モーメントM23dmdのU23方向成分M23dmduは０に設定される。これは、本実施形態のロボット１では、第23ノード補償角θ23を操作しても、床反力モーメントのU23方向成分を発生することはできないからである。なお、本実施形態では、M23dmdの鉛直方向成分も０に設定される。
【０１９０】
第1423ノード補償床反力モーメントM1423dmd、第14ノード補償床反力モーメントM14dmdおよび第23ノード補償床反力モーメントM23dmdは、例えば次のように決定される。
【０１９１】
任意の第nノードに対して、目標第ｎノード床反力中心点に第ｎノード補償床反力モーメントを加えることと等価になるように、第ｎノード補償床反力モーメントの水平成分を０にしたまま、目標第ｎノード床反力中心点を修正した場合における修正された目標第ｎノード床反力中心点位置を、修正目標第ｎノード床反力中心点位置と呼ぶ。
【０１９２】
修正目標第1423ノード床反力中心点Pmdfd（＝Q1423mdfd）、修正目標第１４ノード床反力中心点Q14mdfd、修正目標第２３ノード床反力中心点Q23mdfdと各ノード補償床反力モーメントとの関係を次式７〜９に示す。なお、目標第1423ノード床反力中心点Q1423は全床反力中心点Ｐと一致するので、修正目標第1423ノード床反力中心点Pmdfdは、修正目標全床反力中心点ということもある。
【０１９３】
M1423dmd＝(Pmdfd−P)＊Ftotalref      …式７
M14dmd＝(Q14mdfd−Q14)＊F14ref      …式８
M23dmd＝(Q23mdfd−Q23)＊F23ref      …式９
なお、式７中の(Pmdfd−P)など、２つの点の差は、それらの点の位置ベクトルの差を意味する。また、Ftotalref、F14ref、F23refは図１０に示したように、それぞれ第1423ノード、第14ノード、第23ノードの目標ノード床反力の並進力成分である。
【０１９４】
ロボット１の接地性を高く保つためには、接地すべき接地部位１０の床反力が０に近づき過ぎたり、負の値になろうとするように制御すべきではない。したがって、以下の条件１）〜３）を満足すべきである。
修正ノード存在位置条件１）
Pmdfdは、全床反力中心点Ｐから線分Q14Q23の端点に近づき過ぎず、かつ、線分Q14Q23上にあること。このように、修正目標第1423ノード床反力中心点（修正目標全床反力中心点）Pmdfdが存在すべき範囲を、修正目標第1423ノード床反力中心点（修正目標全床反力中心点）の存在許容範囲と呼ぶ。
修正ノード存在位置条件２）
Q14mdfdは、目標第14ノード床反力中心点Q14から線分Q1Q4の端点に近づき過ぎず、かつ、線分Q1Q4上にあること。このように、修正目標第14ノード床反力中心点Q14mdfdが存在すべき範囲を、修正目標第14ノード床反力中心点の存在許容範囲と呼ぶ。
修正ノード存在位置条件３）
Q23mdfdは、目標第23ノード床反力中心点Q23から線分Q2Q3の端点に近づき過ぎず、かつ、線分Q2Q3上にあること。このように、修正目標第23ノード床反力中心点Q23mdfdが存在すべき範囲を、修正目標第23ノード床反力中心点の存在許容範囲と呼ぶ。
【０１９５】
一方、ロボット１に実際に発生する姿勢復元力（上体２４の傾きの、目標上体姿勢の傾きへの復元力）を適切にするためには、第1423ノード補償床反力モーメントM1423dmd、第14ノード補償床反力モーメントM14dmdおよび第23ノード補償床反力モーメントM23dmdの合力が、補償全床反力モーメントMdmdに略一致すべきである。すなわち、以下の式１０を略満足すべきである。
【０１９６】
Mdmd＝M1423dmd+M14dmd+M23dmd      …式１０
そこで、本実施形態では、第1423ノード補償床反力モーメントM1423dmd、第14ノード補償床反力モーメントM14dmdおよび第23ノード補償床反力モーメントM23dmdは、それらと前記式７、式８、式９とによって定まる修正目標ノード床反力中心点Pmdfd，Q14mdfd，Q23mdfdの位置が前記修正ノード存在位置条件１）、２）および３）を満足している限りは、次式１１〜１３によって決定される。
【０１９７】
M1423dmd＝Mat1423＊Mdmd＊V1423      …式１１
M14dmd＝Mat14＊Mdmd＊V14            …式１２
M23dmd＝Mat23＊Mdmd＊V23            …式１３
ただし、Mat1423、Mat14、Mat23はゲイン行列（第３要素が０である１行３列の行列）であり、これらは、式１１、式１２および式１３よって求められるM1423dmd、M14dmd、M23dmdが、式１０を満足するように設定される。
【０１９８】
より厳密には、式１０、式１１、式１２および式１３からなる連立方程式がMdmdの値にかかわらず恒等的に成立するように、ゲイン行列Mat1423、Mat14、Mat23が決定される。上記連立方程式が恒等的に成立するためのゲイン行列は一義的に決定されるわけではないので、例えば、修正ノード存在位置条件１）、２）および３）のいずれを特に重視するかによって、それに適切なゲイン行列を決定すれば良い。補足すると、各補償角θ1423，θ14，θ23を連続的に変化させるために、ゲイン行列は連続的に変化させることが望ましい。また、ロボット１の直立時であるか否かや、移動形態の違いなどによって、ゲイン行列の設定方針を変えても良い。
【０１９９】
式１１、式１２および式１３にしたがって決定される第1423ノード補償床反力モーメントM1423dmd、第14ノード補償床反力モーメントM14dmdおよび第23ノード補償床反力モーメントM23dmdが修正ノード存在位置条件１）、２）および３）のいずれかを満足していない場合には、該条件１）、２）、３）を満足するように、これらを修正する。より具体的には、修正目標第ｎノード床反力中心点（n=1423,14,23）のうち、前記存在許容範囲を越えた修正目標ノード床反力中心点をその存在許容範囲の境界の点に設定し、その他の修正目標ノード床反力中心点を、式１１、式１２および式１３を可能な限り満足するように（左辺と右辺の差の絶対値がなるべく小さくなるように）決定する。ただし、一般的にフィードバック制御においてはフィードバック量が少々変っても、制御対象の挙動が大きく変わることはないので、式１１、式１２および式１３を厳密あるいは強引に満足させる必要はない。
【０２００】
以上のごとく、補償全床反力モーメント分配器１１４ａは、各ノード補償床反力モーメントM1423dmd、M14dmd、M23dmdを決定する。図１４には、このようにして決定された各ノード補償床反力モーメントM1423dmd、M14dmd、M23dmdの例を示した。図中のMdmdは、前記図１３に示したものと同じである。
【０２０１】
また、補償全床反力モーメント分配器１１４ａは、各接地部位１０の目標床反力（各葉ノードの目標ノード床反力）である目標接地部位床反力、各ノード補償床反力モーメントM1423dmd、M14dmd、M23dmdを基に、目標接地部位床反力に各ノード補償床反力モーメントを付加することによって修正される各接地部位１０の目標床反力である修正目標接地部位床反力Fn_refmdfd（n=1,2,3,4）を求める。
【０２０２】
この際、各接地部位１０の目標床反力中心点（葉ノードの目標ノード床反力中心点）Qn(n=1,2,3,4)、目標全床反力中心点Ｐ、各目標ノード床反力中心点Q14、Q23と目標全床反力Ftotalrefとから、各目標ノード床反力を求める手法と同様の手法によって、各接地部位１０の目標床反力中心点Qn(n=1,2,3,4)、修正全床反力中心点Pmdfd、各修正目標ノード床反力中心点Q14mdfd、Q23mdfdと目標全床反力Ftotalrefとから、修正目標接地部位床反力Fn_refmdfd（n=1,2,3,4）を求めればよい。すなわち、各接地部位１０の目標床反力中心点Qn(n=1,2,3,4)、修正全床反力中心点Pmdfd、各修正目標ノード床反力中心点Q14mdfd、Q23mdfdから、前記式１〜３に基づいて各ノードの重みを決定し、その重みを用いて前記式４に基づいて修正目標接地部位床反力Fn_refmdfd（n=1,2,3,4）を求めればよい。
【０２０３】
このことから明らかなように、子ノードを持つ第ｎノードの目標第ｎノード床反力中心点にノード補償床反力モーメントを発生させる（目標第ｎノード床反力中心点に作用する目標床反力のモーメント成分を修正する）ということは、その第ｎノードの子ノードの重みを修正することと同等である。
【０２０４】
以上説明した補償全床反力モーメント分配器１１４ａの処理は、図１８に示す機能ブロック図により表される。すなわち、目標ノード床反力中心点Qn(n=1,2,3,4,14,23)を基に、第14ノード、第23ノードおよび修正目標ノード床反力中心点Qn_mdfd（n=14,23,1423）の存在許容範囲が前記修正ノード存在位置条件１）、２）、３）に従って決定される。また、補償全床反力モーメントMdmdと、各ノードの目標床反力中心点Qn(n=1,2,3,4,14,23,1423)および目標床反力Fn_ref(n=1,2,3,4,14,23,1423)と、上記存在許容範囲とを基に、修正目標ノード床反力中心点Qn_mdfd（n=14,23,1423)およびノード補償床反力モーメントMn_dmd(n=14,23,1423)が決定される。また、修正目標ノード床反力中心点Qn_mdfd(n=14,23,1423)と、各接地部位１０の目標床反力中心点Qn(n=1,2,3,4)と、目標全床反力Ftotalrefとから、修正目標接地部位床反力Fn_refmdfd（n=1,2,3,4）が求められる。
【０２０５】
次に、前記補償角決定部１１４ｂ〜１１４ｄのうちの補償角決定部（θ1423決定部）１１４ｂの処理を図１９のブロック図を参照して説明する。目標第14ノード床反力中心点Q14に実第１ノード床反力と実第４ノード床反力との合力の並進力成分(F1act＋F4act）が作用すると共に、目標第23ノード床反力中心点Q23に実第２ノード床反力と実第３ノード床反力との合力の並進力成分(F2act＋F3act）が作用した場合に、それらの並進力成分が、目標全床反力中心点Ｐ（目標第1423ノード床反力中心点）のまわりに発生するモーメントM1423actを、次式１４により求める。
【０２０６】
M1423act＝P_Q14＊(F1act+F4act)＋P_Q23＊(F2act+F3act)
…式１４
ここで、P_Q14は始点がP、終点がQ14のベクトル、P_Q23は始点がP、終点がQ23のベクトルである。
【０２０７】
なお、式１４の代わりに、次式１５を用いてM1423actを算出しても実際上はほとんど問題がない。
【０２０８】
M1423act＝P_Q1＊F1act＋P_Q2＊F2act
＋P_Q3＊F3act＋P_Q4＊F4act
…式１５
式１５の右辺は、第１〜第４ノード（各葉ノード）の実床反力の並進力成分Fn_act(n=1,2,3,4)から目標全床反力中心点Ｐまわりに作用する実全床反力モーメントMtotalactを算出する式になっている。補足すると、式１４は、目標全床反力中心点Ｐまわりに作用する実全床反力モーメントMtotalactから、目標第14ノード床反力中心点Q14まわりに作用する実床反力モーメントと目標第23ノード床反力中心点Q23まわりに作用する実床反力モーメントとを減じたものになっている。
【０２０９】
一般的に、葉ノードである任意の第nノードの実第nノード床反力モーメントMn_actは、第ｎ接地部位の実床反力モーメントとする。また、葉ノードでない任意の第nノードに対し、そのすべての子ノードの床反力（ここでの子ノードの床反力は、厳密には、該子ノードの目標床反力中心点に作用する床反力）が目標第nノード床反力中心点に作用するモーメントを、実第nノード床反力モーメントMn_actと呼ぶ。式１４および式１５のように、実第nノード床反力モーメントは、子ノードの実床反力モーメントを含む定義と含まない定義とがあるがいずれを用いても構わない。特に、子ノードの実床反力モーメントの水平成分を制御するフィードバック制御の応答性が高ければ、子ノードの実床反力モーメントの水平成分はただちに０に収束するので、いずれの定義を用いても、第nノードの実床反力の制御の挙動はあまり変らない。
【０２１０】
以下に示す式１６は、式１４に対応した実第nノード床反力モーメントの算出の一般式、式１７は、式１５に対応した実第nノード床反力モーメントの算出の一般式である。ただし、式１６および式１７において、Qn_Qmは始点がQn、終点がQmのベクトルである。また、葉ノードである第mノードの実床反力モーメントMm_actは、実床反力検出器で検出された各接地部位の実床反力モーメントとする。
【０２１１】
m∈｛第ｎノードの子ノードの番号の集合｝とすると
Mn_act＝Σ(Qn_Qm＊Fm_act)        …式１６
m∈｛第ｎノードの子孫である葉ノードの番号の集合｝であるとすると
Mn_act＝Σ(Qn_Qm＊Fm_act＋Mm_act)      …式１７
なお、式１６、１７中のΣは、ｍについての総和を意味する。補足すると、第１実施形態では、葉ノード（各接地部位）の目標床反力中心点まわりの実床反力モーメントが０となることから、前記式１５の右辺では、式１７のMm_actの成分が含まれていない。接地部位の姿勢を制御可能なロボットでは、一般には、式１７のMm_actは０にならない。
【０２１２】
第1423ノード補償角θ1423は、一般的には、上記のように求められるM1423actと先に補償全床反力モーメント分配器１１４ａで求められた第1423ノード補償床反力モーメントM1423dmdとの偏差（M1423act−M1423dmd）を０に近づけるようにフィードバック制御則などにより決定すればよい。例えば該偏差に所定のゲイン行列（３次の対角行列）を乗算することでθ1423を求めればよい。
【０２１３】
但し、本実施形態では、前記ベクトルV1423の方向の軸回りの第1423ノード補償角θ1423を決定すればよいので、M1423actのベクトルV1423方向の成分M1423actvとM1423dmdのベクトルV1423方向の成分M1423dmdvとの偏差に応じてθ1423を決定すればよい。そして、この際、本実施形態では、床反力の制御の応答性や安定性を高めるために、M1423actvとM1423dmdvとをフィルタに通した上で、それらの偏差に応じてθ1423を決定することとした。
【０２１４】
具体的には、上記の如く求めたM1423actのベクトルV1423方向の成分M1423actvを抽出する。これは、ベクトルの内積演算「・」を用いた次式１８によって得られる。
【０２１５】
M1423actv＝M1423act・V1423        …式１８
以上のようにM1423actvを算出する処理が図１９の参照符号１１４ｋを付した演算部により実行される。
【０２１６】
次に、上記M1423actvをローパスフィルタ１１４ｉに通し、M1423actvfiltを得る。また、前記第1423ノード補償床反力モーメントM1423dmdのV1423方向の成分M1423dmdvを補償フィルタ１１４ｊに通し、M1423dmdvfiltを得る。なお、M1423dmdvは、前記式１８と同様に、M1423dmdとV1423との内積演算により求められる。そして、M1423dmdvfiltをM1423actvfiltから減じたものを、偏差モーメントV1423方向成分M1423errvとして得る。
【０２１７】
尚、前記補償フィルタ１１４ｊは、制御系におけるM1423dmdvから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。
【０２１８】
最後に次式１９のフィードバック制御則（ここでは比例制御則）の演算によって前記第1423ノード補償角θ1423を得る。ここでK1423は制御ゲインであり、通常、これは正の値に設定する。
【０２１９】
θ1423＝K1423＊M1423errv          …式１９
すなわち、偏差モーメントV1423方向成分M1423errvに制御ゲインK1423を乗じることにより、第1423ノード補償角θ1423を得る。
【０２２０】
次に、図１７の補償角決定部（θ14決定部）１１４ｃの処理を、図２０を参照して説明すると、目標第１ノード床反力中心点Q1に実第１ノード床反力（第１接地部位１０の実床反力）の並進力成分F1actが作用すると共に、目標第４ノード床反力中心点Q4に実第４ノード床反力（第４接地部位１０の実床反力）の並進力成分F4actと作用した場合にそれらの並進力成分が、目標第１４ノード床反力中心点Q14のまわりに発生するモーメントM14actを、一般式である前記式１６を用いて求める。
【０２２１】
より具体的には、次式２０により求める。
【０２２２】
M14act＝Q14_Q1＊F1act＋Q14_Q4＊F4act    …式２０
ここで、Q14_Q1は始点がQ14、終点がQ1のベクトル、Q14_Q4は始点がQ14、終点がQ4のベクトルである。
【０２２３】
また、式２０の代わりに、次式２１（前記式１７を具体化した式）を用いても、前述の理由により、実際上はほとんど問題がない。
【０２２４】
M14act＝Q14_Q1＊F1act＋Q14_Q4＊F4act＋M1act＋M4act
…式２１
だたし、M1actは、実第１ノード床反力モーメント、M4actは、実第４ノード床反力モーメントである。なお、第１実施形態では各脚＃１〜＃４の先端部にフリージョイント（球関節１２）を備えているので、M1act、M4actは０である。
【０２２５】
式２１は、第14ノードのすべての葉ノードの実床反力の合力が目標第14ノード床反力中心点まわりに作用するモーメントを算出する式になっている。尚、式２０は、第１４ノードのすべての葉ノードの実床反力の合力が目標第１４床反力中心点まわりに作用するモーメントから、実第１ノード床反力モーメントと実第４ノード床反力モーメントとを減じたものになっている。
【０２２６】
第14ノード補償角θ14は、一般的には、上記のように求められるM14actと先に補償全床反力モーメント分配器１１４ａで求められた第14ノード補償床反力モーメントM1423dmdとの偏差（M14act−M14dmd）を０に近づけるようにフィードバック制御則などにより求めればよい。
【０２２７】
但し、本実施形態では、θ1423の場合と同様の理由によって、M14actの前記ベクトルV14方向の成分M14actvとM14dmdのベクトルV14方向の成分M14dmdvとをそれぞれフィルタに通したものの偏差に応じてθ14を決定することとした。
【０２２８】
すなわち、上記の如くもとめたM14actのベクトルV14方向成分M14actvを抽出する。これは、ベクトルの内積演算を用いた次式２２によって得られる。
【０２２９】
M14actv＝M14act・V14          …式２２
以上のようにM14actvを算出する処理が図２０の参照符号１１４ｋ’を付した演算部により実行される。
【０２３０】
次に、上記M14actvをローパスフィルタ１１４ｉ’に通し、M14actvfiltを得る。また、前記第14ノード補償床反力モーメントM14dmdのV14方向の成分M14dmdvを補償フィルタ１１４ｊ’に通し、M14dmdvfiltを得る。なお、M14dmdvは、M14dmdとV14との内積演算により求められる。そして、M14dmdvfiltを、M14actvfiltから減じたものを、偏差モーメントV14方向成分M14errvとして得る。尚、補償フィルタ１１４ｊ’は、制御系におけるM14dmdvから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。
【０２３１】
最後に次式２３のフィードバック制御則（ここでは比例制御則）の演算によって前記第14ノード補償角θ14を得る。ここでK14は制御ゲインであり、通常、これは正の値に設定する。
【０２３２】
θ14＝K14＊M14errv          …式２３
図１７の補償角決定部（θ23決定部）１１４ｄの処理は、第14ノード補償角θ14決定部１１４ｃの処理と同様であるので、ここでは詳細な説明を省略する。その処理の概要は、次の通りである。第23ノードの子ノードの実床反力によって目標第23ノード床反力中心点Q23のまわりに発生するモーメントM23actのベクトルV23方向の成分M23actvが前記式１６または１７に基づいて算出される。そして、そのM23actvをローパスフィルタに通したものM23actvfiltを、前記第23ノード補償床反力モーメントM23dmdのV23方向の成分M23dmdvを補償フィルタに通したものM23dmdvfiltから減じてなる偏差モーメントV23方向成分M23errvからフィードバック制御則（比例制御則）の演算によって、前記第23ノード補償角θ23が算出される。
【０２３３】
以上の補償角決定部１１４ｂ〜１１４ｄの処理によって、目標全床反力中心点Ｐに作用する実モーメントMactの水平成分を補償全床反力モーメントMdmdに近づけるようにノード補償角θ1423，θ14，θ23の組が決定されることとなる。なお、θ1423は、本実施形態では、前記式１９により求めたが、式１９の右辺のM1423errの代わりに、M1423act+M14act+M23act（実ノード床反力モーメントの総和）と、M1423dmdとの偏差、あるいは、実ノード床反力モーメントの総和とM1423dmdとをそれぞれフィルタに通したものの偏差に応じて決定するようにしてもよい。
【０２３４】
補足すると、本実施形態では、各ノードの目標ノード床反力中心点Qnを変更せずに、そのQnを作用点として、ノード補償床反力モーメントを決定した。そして、このノード床反力補償モーメント（より正確には、該ノード補償床反力モーメントとQnを作用点とする目標ノード床反力のモーメント成分との合力）とQnを作用点とする実ノード床反力モーメントとの偏差を制御量とし、この制御量を０に近づけるようにノード補償角を決定するようにした。このようにノード補償角を決定する代わりに次のようにノード補償角θ1423，θ14，θ23を決定するようにしてもよい。すなわち、子ノードを持つ各第ｎノード(n=14,23,1423)に対し、その第ｎノードの実ノード床反力（第ｎノードの全ての子ノードの実ノード床反力の合力）のモーメントの水平成分が０となるような床反力中心点を実第ｎノード床反力中心点として求める。あるいは、第ｎノード(n=14,23,1423)の実ノード床反力のモーメントから、その各子ノードの実ノード床反力が該子ノードの目標ノード床反力中心点に作用するモーメントを減じてなるモーメントの水平成分が０となるような床反力中心点を実第ｎノード床反力中心点として求める。例えば第14ノードに関する実第14ノード床反力中心点は、前記式１６または式１７により求めたM14actの水平成分を、F1actとF4actとの合力（実第14ノード床反力の並進力成分）の鉛直成分で割った値だけ、目標第14ノード床反力中心点を線分Q1Q4上でずらした点として求められる。第23ノード、第1423ノードのノード床反力中心点についても同様である。そして、第ｎノード(n=14,23,1423)の目標ノード床反力中心点と、上記の如く求めた実第ｎノード床反力中心点との偏差、あるいは、それらのノード床反力中心点のそれぞれをフィルタに通した値の偏差を制御量とし、その制御量を０に近づけるように、該制御量に応じてノード補償角θ1423，θ14，θ23を決定する（例えば制御量にあるゲインを乗算することで、θ1423，θ14，θ23を決定する）。
【０２３５】
図１７の修正目標接地部位位置姿勢算出部１１４ｇは、第1423ノード補償角θ1423、第14ノード補償角θ14、第23ノード補償角θ23に基づき、前述した階層型コンプライアンス動作の目標接地部位位置姿勢の修正手法（図１５および図１６を参照して説明した手法）に従って各接地部位１０の目標位置および目標姿勢である目標接地部位位置姿勢を修正し、修正目標接地部位位置姿勢を得る。ただし、本実施形態においては、各脚＃１〜＃４の先端部にフリージョイント（球関節１２）があり、意図的に各接地部位１０の姿勢を変化させることはできないので、修正目標接地部位位置姿勢は、実際には、修正目標接地部位位置を意味する。
【０２３６】
図２１は、図１７の前記機構変形補償量算出部１１４ｎの処理を示す機能ブロック図である。図２１に示すように、機構変形補償量算出部１１４ｎは、前記補償全床反力モーメント分配器１１４ａから出力される各接地部位１０の修正目標接地部位床反力Fn_refmdfd (n=1,2,3,4)（目標接地部位床反力に、第1423ノード補償床反力モーメントM1423dmd、第14ノード補償床反力モーメントM14dmdおよび第23ノード補償床反力モーメントM23dmdを付加することによって修正された目標接地部位床反力）によって発生が予想される各脚＃１〜＃４の変形量（各脚のリンク機構およびコンプライアンス機構４２の変形量）である機構変形量En_mdfd(n=1,2,3,4)を求める。機構変形量En_mdfd(n=1,2,3,4)は、各脚に作用する力（あるいは床反力）と該脚の変形との関係を表す機構コンプライアンスモデルを用いて求められる。そして、機構変形補償量算出部１１４ｎは、機構変形量En_mdfd(n=1,2,3,4)を打ち消すための機構変形補償量En_cmpn(n=1,2,3,4)を求める。機構変形補償量En_cmpnは、各機構変形量En_mdfdに（−1）を乗じることにより求められる。
【０２３７】
図１７の機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈは、算出された機構変形量En_mdfdを打ち消すように、各接地部位１０の修正目標接地部位位置姿勢（前記修正目標接地部位位置姿勢算出部１１４ｇで求められた位置姿勢）をさらに修正し、各接地部位１０の機構変形補償入り修正目標接地部位位置姿勢を得る。機構変形補償入り修正目標接地部位位置姿勢は、各接地部位１０の修正目標接地部位位置姿勢に、それぞれに対応する機構変形補償量En_cmpnを付加することにより決定される。
【０２３８】
例えば、修正目標接地部位床反力によって、ある接地部位１０に対応する脚のコンプライアンス機構４２などが鉛直方向にzだけ縮むと算出された場合には、その接地部位１０の目標位置をzだけ下げるように修正目標接地部位床反力が修正される。即ち、機構変形補償後の接地部位１０の接地面（底面）が目標接地部位床反力を受けて変形したときの位置姿勢が、機構変形補償前の接地部位の接地面の目標位置姿勢に一致するように、機構変形補償入り修正目標接地部位位置姿勢を算出する。なお、これについての詳しい説明は、特開平１０−２７７９６９号公報に詳細に説明されている。また、本実施形態のロボット１では、接地部位１０の姿勢を制御できないので、機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈで実際に修正されるものは、各接地部位１０の修正目標接地部位位置である。
【０２３９】
上記のような機構変形補償は、コンプライアンス機構４２などの変形によって生じる実接地部位位置姿勢のずれをフィードフォワード的に打ち消す制御であり、この制御がない場合に比較し、より一層、目標歩容に近い形態でのロボット１の移動を実現することができる。
【０２４０】
以上が、階層型コンプライアンス動作決定部１１４の処理の詳細である。
【０２４１】
上記を前提として図９のフローチャートの説明に戻ると、Ｓ３４において、前記の如く補償角を決定する。図２２は、その補償角の決定処理のサブルーチンを一般化して示したフローチャートである。
【０２４２】
同図を参照して説明すると、先ずＳ１００において、各接地部位１０の実床反力（前記実床反力検出器１０８で得られる実床反力）を基に、実第nノード床反力の並進力成分Fn_act(n=1,2,3,4,14,23,1423)を算出する。この場合、各葉ノードの実ノード床反力の並進力成分Fn_act(n=1,2,3,4)は、前記実床反力検出器１０８による各接地部位１０の実床反力の検出値の並進力成分である。また、葉ノードでない各ノードの実ノード床反力の並進力成分Fn_act(n=14,23,1423)は、前記した如く、そのノードの子ノードの実ノード床反力の合力の並進力成分である。
【０２４３】
次いでＳ１０２に進み、実第nノード床反力モーメントMn_act（n=1,2,3,4,14,23,1423)を算出する。第１実施形態のロボット１では、各葉ノードの実ノード床反力モーメントMn_act（n=1,2,3,4）は０である。また、葉ノードでない各ノードの実ノード床反力モーメントMn_act(n=14,23,1423)は、Ｓ１００で得られたFn_act(n=1,2,3,4)と、目標ノード床反力中心点Qn(n=1,2,3,4,14,23,1423)とから前記式１６または式１７に基づき算出される。
【０２４４】
次いでＳ１０４に進み、前記姿勢安定化制御演算部１０４で求められた補償全床反力モーメントMdmdを基に、第nノード補償床反力モーメントMn_dmd（n=14,23,1423)を決定する。この処理は、前記補償全床反力モーメント分配器１１４ａにより前記した通り行なわれる。
【０２４５】
次いでＳ１０６に進み、階層型コンプライアンス動作の説明で定義したベクトルV1423，V14，V23およびこれに直交したベクトルU1423，U14，U23を求める。ただし、第１実施形態では、ベクトルU1423，U14，U23の方向の床反力モーメントを発生することはできないので、U1423，U14，U23を求める必要はない。
【０２４６】
補足すると、後述する第２実施形態の如く、第ｎノードの子ノードの数が３以上の場合には、Vnは、時間的に急激に変化しなければ、どの向きにとっても良いので、Vnの向きは、支持脚座標系のＸ軸の向きや、ロボットの上体の向きに合わせるなど、適当に決定すれば良い。また、UnはＶnに直交していればよい。
【０２４７】
次いでＳ１０８に進み、実第nノード床反力モーメントMn_act（n=14,23,1423)のVn方向成分Mn_actvとUn方向成分Mn_actuとを抽出する。これは、Mn_actとＶn、Unとの内積演算を行なえばよい。なお、第１実施形態のように葉ノードでない任意の第ｎノードの子ノードの数が２以下の場合には、Un方向成分Mn_actuは０である。従って、Mn_actuは求める必要はない。
【０２４８】
次いでＳ１１０に進み、第nノード補償床反力モーメントMn_dmd（n=14,23,1423)のVn方向成分Mn_dmdvとUn方向成分Mn_dmduを抽出する。これは、Mn_dmdとＶn、Unとの内積演算を行なえばよい。なお、第１実施形態のように葉ノードでない任意の第ｎノードの子ノードの数が２以下の場合には、第nノード補償床反力モーメントMn_dmdのUn方向成分Mn_dmduは０にする。あるいは、Mn_dmduを求める必要はない。
【０２４９】
次いでＳ１１２に進み、Mn_actvをフィルタに通した値とMn_dmdvをフィルタに通した値の差にゲインKnを乗じることにより（より一般的には、当該差からフィードバック制御則により）第nノード補償角θnのVn成分を決定する。第１実施形態では、この処理は、第14ノード、第23ノード、第1423ノードについて、前記各補償角決定部１１４ｂ，１１４ｃ，１１４ｄにより前記した如く実行される。
【０２５０】
次いでＳ１１４に進み、Mn_actuをフィルタに通した値とMn_dmduをフィルタに通した値の差にゲインKnを乗じることにより（より一般的には当該差からフィードバック制御則により）第nノード補償角θnのUn成分を決定する。ただし、第１実施形態のように葉ノードでない任意の第ｎノードの子ノードの数が２以下の場合には、第nノード補償角U成分は０にする。あるいは、Ｓ１１４の処理を実行する必要はない。
【０２５１】
以上が、図９のＳ３４のサブルーチン処理である。補足すると、Ｓ１０６〜Ｓ１１４の処理は、目標第ｎノード床反力中心点に作用する実第ｎノード床反力モーメントが第ｎノード補償床反力モーメント（より正確には第ｎノード補償床反力モーメントと目標第ｎノード床反力中心点に作用する目標ノード床反力モーメントとの合力）に収束するように第ｎノード補償角を決定する処理であると言える。
【０２５２】
次いで、図９のフローチャートのＳ３６に進み、前記機構変形補償量を算出する。この処理は、前記機構変形補償量算出部１１４ｎにより前記した如く行なわれる。
【０２５３】
次いでＳ３８に進んで目標接地部位位置姿勢をＳ３４で求めた補償角に応じて修正し、さらにこれをＳ３６で求めた機構変形補償量に応じて修正することによって、各接地部位１０の機構変形補償入り修正目標接地部位位置姿勢を得る。第１実施形態では、前記修正目標接地部位位置姿勢算出部１１４ｇによって、前記した如く（前記図１５および図１６を参照して説明した如く）補償角θ1423、θ14、θ23に基づき各接地部位１０の修正目標接地部位位置を求める。そして、この求めた修正目標接地部位位置が前記機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈによって、前記した如く前記機構変形補償量En_cmpn(n=1,2,3,4)に応じてさらに修正され、それによって、各接地部位１０の機構変形補償入り修正目標接地部位位置が得られる。
【０２５４】
以上説明した図９のＳ３２〜Ｓ３８の処理が、階層型コンプライアンス動作決定部１１４の処理である。
【０２５５】
次いでＳ４０に進み、目標上体位置姿勢と機構変形補償入り修正接地部位位置姿勢（第１実施形態では機構変形補償入り修正接地部位位置）からロボット１の関節変位指令を算出する。この処理は、前記ロボット幾何学モデル１１０により前記した如く実行される。
【０２５６】
次いでＳ４２に進んで、実関節変位を関節変位指令に追従させる。この追従制御（サーボ制御）は、前記変位コントローラ１１２により実行される。
【０２５７】
次いでＳ４４に進んで、時刻をΔｔ（制御装置５０の演算処理周期）だけ更新し、Ｓ１４に戻って上記の処理を繰り返す。
【０２５８】
以上説明した第１実施形態の如き階層型のコンプアライアンス動作によって、接地部位が３つ以上ある移動ロボットにおいても、各ノード床反力の制御が互いに殆ど干渉しないようになり、各ノード床反力を容易かつ適切に制御することができる。したがって、制御の干渉がなく、各ノードの実床反力が望ましい値からずれたり発振することがない。このため、床面の大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。また、移動ロボットの姿勢を安定化する制御を容易に実現できると共に、移動ロボットが受ける着地衝撃を低減することができ、移動ロボットの接地性を高め、移動時のスリップやスピンを防止することができる。さらに、移動ロボットのアクチュエータの負荷を低減することができる。したがって、各接地部位の床反力を適切に制御し、高い姿勢安定性を得ることができる。
【０２５９】
ロボットの姿勢の安定化に対する効果を補足すると、ロボットが想定した床面上を想定した通りに歩行している状態から、ロボット全体が傾いた場合に、傾き角偏差θberr（θberrx，θberry）とこれに応じて発生する目標全床反力中心点まわりのモーメント水平成分の増加量ΔMとの関係が、比例関係にあることが望ましい。そうでなくても、ある回転行列T、ある対角行列diag(a,b)に対し、次式２４が成立することが望ましい。なお、T，diag(a,b)は２次の正方行列である。
【０２６０】
T＊ΔM＝diag(a,b)＊T＊θberr        …式２４
これらの関係を満足しない場合には、ロボットの姿勢が傾いた状態から戻る時に、直線的に傾き角偏差θberrが０に収束せずに、味噌擂り運動を起こすおそれがある。例えば、ロボットの上体が前に傾いた状態では、後ろに戻す復元力以外に、横方向への転倒力（復元力）が余計に作用し、傾き角偏差θberrが直線的に０に戻らずに、後ろ横方向に復元力が働いて、傾き角偏差θberrが渦巻き状に０に収束していく。
【０２６１】
また、上記と同じ理由から、コンプライアンス制御においては、ロボット全体の傾き角偏差θberrの変化速度dθberr/dtとこれに応じて発生するモーメントの増加量ΔM_dとの関係も、比例関係にあることが望ましい。そうでなくても、ある回転行列をT、ある対角行列diag(e,f)に対し、次式２５が成立することが望ましい。なお、T，diag(e,f)は２次の正方行列である。
【０２６２】
T＊ΔM_d＝diag(e,f)＊T＊dθberr/dt              …式２５
一般的に、ロボットの各脚に独立にコンプライアンス制御を掛けると、これらの関係を満たさず、味噌擂り運動を起こすおそれがある。第１実施形態に示した階層型コンプライアンス制御においては、前記式２４，２５の関係を満足することができるので、ロボットの姿勢制御の収束性が高く、また、発振や振動を防止することができる。
【０２６３】
前記第１実施形態においては、目標接地部位位置を、各接地部位の姿勢を変えずに（姿勢を制御せずに）、各ノード床反力中心点まわりに回転移動させるように補正（修正）動作を行うことで、各接地部位１０の間の相対高さを目標接地部位位置の間の相対高さから修正した。但し、目標接地部位位置を鉛直方向にだけ移動させるようにして、各接地部位１０の高さだけを修正するようにしても良い。具体的には、以下の手順によって目標接地部位位置を修正する。
【０２６４】
まず、以下の式２６，２７により、目標第14ノード床反力中心点の鉛直位置修正量Z14と目標第23ノード床反力中心点の鉛直位置修正量Z23を求める。
【０２６５】
Z14＝−PQ14＊θ1423              …式２６
Z23＝PQ23＊θ1423              …式２７
但し、ここで、θ1423には前記式１９で求められる値を代入する。
【０２６６】
次いで、各接地部位の目標床反力中心点（各葉ノードの目標ノード床反力中心点）の鉛直位置修正量Zn(n=1,2,3,4)を次式により求める。
【０２６７】
Z1＝−Q14Q1＊θ14＋Z14          …式２８
Z4＝Q14Q4＊θ14＋Z14          …式２９
Z2＝−Q23Q2＊θ23＋Z23          …式３０
Z3＝Q23Q3＊θ23＋Z23          …式３１
但し、ここで、θ14には、前記式２３で求められる値を代入し、θ23には、θ14と同様にして求められる値を代入する。
【０２６８】
以上のように求めたZ1、Z2、Z3、Z4を目標接地部位位置に鉛直方向に加えることにより、修正目標接地部位位置を得る。
【０２６９】
また、前記第１実施形態では、接地部位１０を前記図６に示した如く階層化したが、その階層構造は、必ずしも１つの階層構造にあらかじめ決めておく必要はない。例えばトロット、ギャロップなど、ロボット１の移動形態（移動時の脚の運動形態）などに応じて、階層構造を変更しても良い。例えば、図２３に示す如く接地部位１０を階層化してもよい。図２３は、例えばロボット１の全ての脚＃１〜＃４を支持脚とする時期において、第１接地部位１０と第２接地部位１０との組を第12ノードとし、第３接地部位１０と第４接地部位１０との組を第34ノードとした場合の例を前記図３（ｂ）と同様に図示したものである。この例では、各ノードの目標ノード床反力中心点Qn(n=1,2,3,4,12,34,1234)は図示の如く設定される。このようにすることで、場合によっては、前記コンプライアンス動作と、後述の床形状推定をより的確に行なうことが可能となる。
【０２７０】
［第２実施形態］
以下に前記図１並びに図２４〜図３４を参照しつつ、本発明の第２実施形態を説明する。図１を参照して、第２実施形態のロボット１が第１実施形態のロボットと異なる点を中心に説明すると、第２実施形態のロボット１は、第１〜第４脚＃１〜＃４に加えて、これらの脚＃１〜＃４と同じ構造の第５脚＃５および第６脚＃６を備える。すなわち、第２実施形態のロボット１は６脚ロボットである。第５脚＃５は、第３脚＃３の後側でロボット１の上体２４の右側部から延設され、第６脚＃６は、第４脚＃４の後側でロボット１の上体２４の左側部から延設されている。これ以外のロボット１の機構的構造は、第１実施形態と同一であるので、第１実施形態と同一の参照符号を付して説明を省略する。
【０２７１】
この第２実施形態のロボット１（６脚ロボット）の階層型コンプライアンス制御の手法の基本的な考え方は第１実施形態のものと同じである。但し、第２実施形態の階層型コンプライアンス制御では、３つの子ノードを持つノードがあることと、それに対応して制御処理が拡張されていることとが第１実施形態のものと相違している。
【０２７２】
そこで、以降、その相違点を中心に第２実施形態のロボット１の制御処理について説明する。なお、第２実施形態の説明で用いる参照符号および用語は、第１実施形態と同等の意味を持つものは、第１実施形態と同じ参照符号および用語を用い、詳細な説明を省略する。
【０２７３】
図２４は、第２実施形態における階層構造を説明するための図であり、第１実施形態における図３（ｂ）に対応する図である。図２４に示す如く、第２実施形態では、６個の接地部位１０のそれぞれを葉ノード（第１〜第６ノード）とし、その６個の全ての接地部位１０の組を根ノード（第145236ノード）としていると共に、第１脚＃１、第４脚＃４、第５脚＃５のそれぞれの接地部位である第１、第４、第５接地部位１０の組を第145ノード、第２脚＃２、第３脚＃３、第６脚＃６のそれぞれの接地部位である第２、第３、第６接地部位１０の組を第236ノードとしている。つまり、第145ノードは第１、第４、第５ノード（３個の葉ノード）を子ノードとして持つ中間ノード、第236ノードは第２、第３、第６ノード（３個の葉ノード）を子ノードとして持つ中間ノードである。
【０２７４】
第２実施形態では、例えば第145ノードの３個の接地部位１０の組と、第236ノードの３個の接地部位１０の組とを交互に持ち上げて着地させることにより、ロボット１の移動を行なうようにしている。図２４は、両者の組を着地させた状態（両者の組を支持脚とした状態）を示しており、図中の参照符号Qn(n=1,2,3,4,5,6)を付した三角形が各接地部位１０の位置に対応している。
【０２７５】
なお、Qn(n=1,2,3,4,5,6)は、葉ノードである第１〜第６の各接地部位１０の目標床反力中心点（目標ノード床反力中心点）、Q145、Q236は、それぞれ第145ノード、第236ノードの目標床反力中心点（目標ノード床反力中心点）、Pは根ノード（第145236ノード）の目標床反力中心点（目標ノード床反力中心点）Q145236としての目標全床反力中心点（＝目標ＺＭＰ）を示すものである。第１実施形態と同様に、Qn(n=1,2,3,4,5,6)は、それぞれに対応する接地部位１０の中心点に一致している。
【０２７６】
第２実施形態における制御装置５０の全体的な機能的構成は、前記第１実施形態に関して説明した図２のものと同じである。
【０２７７】
この場合、第２実施形態における歩容生成装置１００は、第１実施形態のものと同様に、ロボット１の目標運動軌道（目標接地部位軌道、目標上体位置姿勢軌道）と目標床反力軌道（目標全床反力中心点軌道、目標全床反力軌道）とを決定して出力する。但し、第２実施形態では、目標接地部位軌道は、６個の接地部位１０のそれぞれの目標接地部位位置の軌道である。なお、各接地部位をその姿勢を制御可能に設けた場合には、目標接地部位姿勢の軌道も目標接地部位軌道に含まれる。また、目標全床反力中心点軌道は、第２実施形態における脚＃１〜＃６の運動形態（特に支持脚の着地予定位置）に合わせて、ＺＭＰの存在可能範囲内で該範囲の境界に近づきすぎない位置（例えばＺＭＰの存在可能範囲のほぼ中央位置）に存在しつつ、連続的に移動するように決定される。
【０２７８】
また、第２実施形態における目標床反力分配器１０２では、第１実施形態と同様に、次の条件Ａ’）〜Ｇ’）を満たすように、目標ノード床反力中心点と各ノードの重みと目標ノード床反力とを決定する。
Ａ’）各葉ノードの目標ノード床反力中心点Qn(n=1,2,3,4,5,6)は、該葉ノードに対応する接地部位１０の中心点に一致する。より一般的には、Qn(n=1,2,3,4,5,6)は、目標歩容（目標接地部位軌道などの目標運動）に応じて決定される。なお、例えば各接地部位１０の目標床反力中心点を歩容生成装置１００で決定した場合には、歩容生成装置１００が決定する目標床反力を基に、目標ノード床反力中心点Qn(n=1,2,3,4,5,6)を決定すればよい。
Ｂ’）根ノードの目標ノード床反力中心点は、目標全床反力中心点Ｐに一致する。
Ｃ’）子ノードを持つ任意の第ｎノード(n=145,236,145236)の目標ノード床反力中心点Qnは、その第ｎノードの各子ノードの目標ノード床反力中心点の重み付き平均の点となる。より具体的には、第145ノードの目標ノード床反力中心点Q145は、その子ノード（葉ノード）である第１、４、５ノードの目標ノード床反力中心点Q1，Q4，Q5を頂点とする三角形の内分点（三角形の境界の点を含む）となり、第236ノードの目標ノード床反力中心点Q236は、その子ノード（葉ノード）である第２、３、６ノードの目標ノード床反力中心点Q2，Q3，Q6を頂点とする三角形の内分点（三角形の境界の点を含む）となる。また、第145236ノード（根ノード）の目標ノード床反力中心点Q145236（＝目標全床反力中心点Ｐ）は、その子ノード（中間ノード）である第145、236ノードの目標ノード床反力中心点Q145，Q236を結ぶ線分の内分点となる。
Ｄ’）子ノードを持つ任意の第ｎノード（n=145,236,145236）の目標ノード床反力Fnは、該第ｎノードのすべての子ノードの目標ノード床反力の和（合力）に一致し、且つ、根ノード（第145236ノード）の目標ノード床反力F145236は目標全床反力Ftotalrefに一致する。あるいは、各ノードの目標ノード床反力Fn(n=1,2,3,4,5,6,145,236,145236)は、各ノードの重みと前記式４ａまたは４ｂの関係を有する。
Ｅ’）接地していない接地部位１０に対応する葉ノードの目標ノード床反力は０となる。
Ｆ’）各ノード（第ｎノード(n=1,2,3,4,145,236,145236)）の目標ノード床反力中心点、重み、目標ノード床反力は連続的に変化する。
Ｇ’）非接地状態の接地部位１０に対応する葉ノードの重み、または、その葉ノードの祖先ノードのいずれか１つの重みは０とされる。
【０２７９】
なお、根ノードの重みは、前記第１実施形態と同様、便宜的に「１」に設定した。
【０２８０】
第２実施形態における姿勢偏差演算部１０３および姿勢安定化制御演算部１０４の処理は、第１実施形態と同じであり、補償全床反力モーメントMdmd（Mdmdx，Mdmdy）が第１実施形態で説明した如く決定される。
【０２８１】
また、第２実施形態における実床反力検出器１０８は、第１実施形態のものと同様に、各接地部位１０（本実施形態では６個の接地部位）に作用する実床反力を各脚＃１〜＃６に備えた６軸力センサ３４の出力から検出し、それを支持脚座標系（床に固定された図１に示すグローバル座標系）で表現してなる実床反力に変換する。
【０２８２】
また、第２実施形態におけるロボット幾何学モデル（逆キネマティクス演算部）１１０は、第１実施形態のものと同様に、各接地部位位置の最終的な目標軌道と目標上体位置姿勢などを基に、逆キネマティクスの演算によって、それらを満足するロボット１の各関節変位指令を算出する。
【０２８３】
また、第２実施形態における変位コントローラ１１２は、第１実施形態のものと同様に、前記各関節変位指令に実関節変位を追従させるようにロボット１の各関節１４，１５のアクチュエータ（図示しない）を制御する。
【０２８４】
また、第２実施形態における階層型コンプライアンス動作決定部１１４は、第１実施形態のものと同様に前記要求１）、２）をできるだけ満足するように各接地部位１０の目標接地部位軌道を修正する。但し、本実施形態では、３つの子ノードを持つ中間ノードを有するため、階層型コンプライアンス動作決定部１１４の具体的な処理は、第１実施形態のものより多少複雑になる。なお、各接地部位１０の姿勢を制御可能にロボットを構成した場合には、階層型コンプライアンス動作決定部１１４は、前記要求３）もできるだけ満足するように各接地部位１０の目標接地部位軌道を修正すべきである。
【０２８５】
図２５は、第２実施形態の階層型コンプライアンス動作決定部１１４の機能を示すブロック図であり、第１実施形態における図１７に対応している。同図２５を参照して、第２実施形態の階層型コンプライアンス動作決定部１１４も、第１実施形態と同様に、補償全床反力モーメント分配器１１４ａ、補償角決定部１１４ｂ，１１４ｃ，１１４ｄと、修正目標接地部位位置姿勢算出部１１４ｇと、機構変形補償量算出部１１４ｎと、機構変形補償入り修正目標接地部位位置姿勢決定部１１４ｈとを構成要素（機能的手段）として備える。なお、第２実施形態では、補償角は、第145236ノード、第145ノード、第236ノードにそれぞれ係わる第145236補償角θ145236、第145補償角θ145および第236補償角θ236があり、補償角決定部１１４ｂ，１１４ｃ，１１４ｄは、それぞれθ145236、θ145、θ236を決定するものである。
【０２８６】
以下に、第２実施形態における階層型コンプライアンス動作決定部１１４の処理を、第１実施形態のものと異なる点を中心に説明する。
【０２８７】
第２実施形態における階層型コンプライアンス動作決定部１１４は、第１実施形態のものと同様に、目標床反力分配器１０２の出力などを基に、各ノードの目標ノード床反力の並進力成分およびモーメント成分を決定すると共に、実床反力検出器１０８の出力を基に、各ノードの実ノード床反力の並進力成分およびモーメント成分を決定する。
【０２８８】
すなわち、図２６を参照して、各葉ノードの目標ノード床反力の並進力成分Fn_ref(n=1,2,3,4,5,6)は、目標床反力分配器１０２で決定された目標ノード床反力の並進力成分と同一とされ、根ノードの目標ノード床反力の並進力成分F145236refは、歩容生成装置１００で決定された目標全床反力の並進力成分Ftotalrefと同一とされる。また、葉ノードおよび根ノード以外の中間ノードの目標ノード床反力の並進力成分Fn_ref(n=145,236)は、その中間ノードの各子ノードの目標ノード床反力の合力の並進力成分に決定される（F145ref＝F1ref＋F4ref＋F5ref、F236ref＝F2ref＋F3ref＋F6ref）。この場合、中間ノードの親ノードは根ノードであるので、F145ref＋F236ref＝Ftotalrefである。なお、図２６は、第２実施形態のロボット１の全ての接地部位１０が接地している状態での各ノードの目標ノード床反力の並進力成分Fn_refを例示している。
【０２８９】
また、各ノードの目標ノード床反力のモーメント成分（詳しくは該ノードの目標ノード床反力中心点を作用点とする目標ノード床反力のモーメント成分）は、いずれも０とされる。
【０２９０】
また、図３０を参照して、各葉ノードの実ノード床反力の並進力成分Fn_act(n=1,2,3,4,5,6)は、実床反力検出器１０８で検出された各接地部位１０の実床反力の並進力成分と同一とされる。そして、子ノードをもつ各ノードの実ノード床反力の並進力成分Fn_act(n=145,236,145236)は、そのノードの各子ノードの実ノード床反力の合力の並進力成分に決定される。従って、F145act＝F1act＋F4act＋F5act、F236act＝F2act＋F3act＋F6act、F145236act（＝Ftotalact）＝F145act＋F236actである。なお、図３０は、第２実施形態のロボット１の全ての接地部位１０が接地している状態での各ノードの実ノード床反力の並進力成分Fn_actを例示している。
【０２９１】
また、図３１を参照して、各葉ノードの実ノード床反力のモーメント成分（該葉ノードの目標ノード床反力中心点を作用点とするモーメント成分）Mn_act(n=1,2,3,4,5,6)は、基本的には、実床反力検出器１０８で検出された各接地部位１０の実床反力のモーメント成分と同一とされる。ただし、本実施形態では、各接地部位１０の中心点、すなわち該接地部位１０の目標床反力中心点に実モーメントは発生しないので、各葉ノードの実ノード床反力のモーメント成分は０に設定される。そして、子ノードをもつ各第ｎノード(n=145,236,145236)の実ノード床反力のモーメント成分Mn_actは、基本的にはその第ｎノードの各子ノードの実ノード床反力の合力が、該第ｎノードの目標ノード床反力中心点に作用するモーメント成分（これは一般に０にならない）に決定される。
【０２９２】
第２実施形態における前記補償全床反力モーメント分配器１１４ａは、前記補償全床反力モーメントMdmd（Mdmdx，Mdmdy）を、第145236ノード補償床反力モーメントM145236dmd、第145ノード補償床反力モーメントM145dmdおよび第236ノード補償床反力モーメントM236dmdに分配するものである。
【０２９３】
第145236ノード補償床反力モーメントM145236dmdは、第145236補償角θ145236を操作することによって（第１、第４、第５接地部位の組と、第２、第３、第6接地部位の組とを目標全床反力中心点P(=Q145236)まわりにθ145236だけ回転させることによって）発生する各接地部位１０の床反力の並進力成分が、目標全床反力中心点Ｐ（目標ＺＭＰ）まわりに発生すべきモーメントの目標値である。
【０２９４】
第145ノード補償床反力モーメントM145dmdは、第145補償角θ145を操作することによって（第145ノードに属する第１、第４および第５接地部位１０を目標第145ノード床反力中心点Q145まわりにθ145だけ回転させることによって）発生する各接地部位１０（詳しくは第１、第４、および第５接地部位）の床反力の並進力成分が、目標第145ノード床反力中心点まわりに発生すべきモーメントの目標値である。
【０２９５】
第236ノード補償床反力モーメントM236dmdは、第236補償角θ236を操作することによって（第236ノードに属する第２、第３および第６接地部位１０を目標第236ノード床反力中心点Q236まわりにθ236だけ回転させることによって）発生する各接地部位１０（詳しくは第２、第３、および第６接地部位）の床反力の並進力成分が、目標第236ノード床反力中心点まわりに発生すべきモーメントの目標値である。
【０２９６】
なお、本実施形態では、第１実施形態と同様、ロボット１の鉛直軸まわりの姿勢制御を行なわないので（補償全床反力モーメントMdmdの鉛直軸回りの成分が０であるので）、M145236dmd、M145dmd、M236dmdは、いずれも鉛直軸回りの成分が０であるモーメント（水平ベクトル）であり、補償角θ145236、θ145、θ236は、水平な軸回りの回転角である。また、特に、M145236dmdは、線分Q145Q236に垂直な水平軸回りのモーメントである。
【０２９７】
これらのノード補償床反力モーメントM145236dmd，M145dmd，M236dmdは、基本的には、次の条件１１）、１２）を満たすように決定される。
【０２９８】
１１）第145ノード、第236ノード、第145236ノードに対して、前記第１実施形態で定義した如く、次式７ａ〜９ａの関係式を満たす修正目標ノード床反力中心点Q145mdfd、Q236mdfd、Pmdfd（=Q145236mdfd)を導入したとき、これらのQ145mdfd、Q236mdfd、Pmdfdは、それぞれの所定の存在許容範囲内に在る。
【０２９９】
M145236dmd＝(Pmdfd−P)＊Ftotalref        …式７ａ
M145dmd＝(Q145mdfd−Q145)＊F145ref      …式８ａ
M236dmd＝(Q236mdfd−Q236)＊F236ref      …式９ａ
Q145mdfd、Q236mdfd、Pmdfdの存在可能範囲は、例えば第２実施形態のロボット１の全ての接地部位１０が接地している状態では、図２９（ａ）に示す如く設定される。すなわち、Q145mdfdの存在許容範囲は、図中の太線の三角形上の領域（その三角形の辺および内部の領域）であり、これは、第145ノードの子ノードの目標ノード床反力中心点Q1,Q4,Q5を頂点する三角形の内部で、該三角形Q1Q4Q5の境界に近づき過ぎないように設定された領域である。Q236mdfdの存在許容範囲も同様である。また、Pmdfdの存在許容範囲は、図中の太線の線分上の領域であり、これは、第145236ノード（根ノード）の子ノードの目標床反力中心点Q145,Q236を結ぶ線分Q145Q236上で、該線分Q145Q236の端点に近づきすぎないように設定された領域である。
１２）Mdmd＝M145236dmd＋M145dmd＋M236dmdをほぼ満足する。
【０３００】
また、本実施形態では、根ノード（第145236ノード）の子ノードが２つであることから、M145236dmdは、前記第１実施形態における各ノード補償床反力モーメントと同様に、線分Q145Q236に直交する水平な単位ベクトル（これをV145236で表す）と同方向のベクトルに制限される。そこで、本実施形態では、次の条件１３）を満たすようにM145236dmd，M145dmd，M236dmdを決定する。
【０３０１】
１３）M145236dmd＋M145dmd＋M236dmdの、ベクトルV145236方向の成分は、可能な限り、MdmdのベクトルV145236方向の成分に近い値とする。
【０３０２】
これらの条件１１）〜１３）を満たすM145236dmd、M145dmd、M236dmdは、例えば次のように決定される。まず、M145236dmdをMdmdのV145236方向の成分に決定する。ただし、前記式７ａにより定まる修正目標ノード床反力中心点Pmdfdがその存在許容範囲に収まらない場合には、Pmdfdがその存在許容範囲の境界の点になるように、M145236dmdを修正する。次いで、Mdmdから上記の如く決定したM145236dmdを差し引いたベクトルが、Mdmd145とMdmd236との和にほぼ一致し、且つ、Mdmd145＋Mdmd23のV145236方向の成分がMdmd−Mdmd145236のV145236成分にできるだけ近いものとなり、且つ前記条件１１）を満たすように、Mdmd145，Mdmd236を決定する。なお、この場合、Mdmd145，Mdmd236は、互いに平行なベクトルとする。
【０３０３】
このように決定されたM145236dmd、M145dmd、M236dmdの例を図２９（ｂ）に示す。補足すると、M145236dmdは、前記したように線分Q145Q236に垂直な水平ベクトルである。
【０３０４】
なお、ロボット１の鉛直軸回りの姿勢も制御するような場合には、M145236dmd、M145dmd、M236dmdの鉛直成分も決定するようにしてもよい。
【０３０５】
第２実施形態における各補償角決定部１１４ｂ〜１１４ｄは、基本的には、それぞれ第ｎノード(n=145236,145,236)の、ノード補償床反力モーメントと、実ノード床反力モーメント（目標ノード床反力中心点を作用点とするモーメント）との偏差に応じて、その偏差を０に近づけるようにノード補償角θ145236、θ145、θ236を決定する。図３０および図３１にその例を示す。この場合、本実施形態では、θ145は、図３０に示す如く、偏差M145act−M145dmdと同方向で、目標第145ノード床反力中心点Q145を通る軸回りの回転角、θ236は、図３０に示す如く、偏差M236act−M236dmdと同方向で、目標第236ノード床反力中心点Q236を通る軸回りの回転角である。また、θ145236は、図３１に示す如く、偏差M145236act−M145236dmdと同方向（線分Q145Q236に垂直な水平方向）で、目標全床反力中心点Pを通る軸回りの回転角である。
【０３０６】
但し、本実施形態においても、第１実施形態と同様に、第ｎノード(n=145236,145,236)の、ノード補償床反力モーメントをフィルタに通したものと、実ノード床反力モーメントをフィルタに通したものとの偏差に応じて、ノード補償角θ145236、θ145、θ236を決定するようにした。
【０３０７】
図３２は上記の如くノード補償角θ145236を決定する補償角決定部１１４ｂ（θ145236決定部）の機能を示すブロック図、図３３は上記の如くノード補償角θ145を決定する補償角決定部１１４ｃ(θ145決定部）の機能を示すブロック図である。なお、補償角決定部１１４ｄ（θ236決定部）の処理は、θ145決定部１１４ｂの処理と同様であるので詳細な説明および図示を省略する。
【０３０８】
補償角決定部（θ145236決定部）１１４ｂでは、まず、目標第145ノード床反力中心点Q145に実第１、第４、第５ノード床反力の合力の並進力成分(F1act＋F4act＋F5act）が作用すると共に、目標第236ノード床反力中心点Q236に実第２、第３、第６ノード床反力の合力の並進力成分(F2act＋F3act＋F6act）が作用した場合に、目標全床反力中心点Ｐ（目標第145236ノード床反力中心点）のまわりに発生するモーメントM145236actを前記式１６または１７に基づいて求める。そして、その求めたM145236actの、ベクトルV145236方向の成分M145236actvを内積演算によって求める。ここでベクトルV145236は、線分Q145Q236に垂直で且つ水平な単位ベクトルである。なお、第145236ノードに関しては、V145236に垂直で且つ水平な単位ベクトルU145236の方向の軸回りに子ノード（第145ノードおよび第236ノード）の目標床反力中心点の位置を操作できないので、M145236actのベクトルU145236方向の成分は求める必要はない。
【０３０９】
次いで、前記第１実施形態と同様に、このM145236actvをローパスフィルタに通したものM145236actvfiltと、前記第145236ノード補償床反力モーメントM145236dmdのベクトルV方向の成分M145236dmdvを補償フィルタに通したものM145236dmdvfiltとの偏差M145236errv（＝M145236actvfilt−M145236dmdvfilt）に所定のゲインK145236を乗算することで、第145236ノード補償角θ145236が決定される。
【０３１０】
補償角決定部（θ145決定部）１１４ｃでは、まず、目標第１、第４、第５ノード床反力中心点Q1，Q4，Q5にそれぞれ実第１、第４、第５ノード床反力の並進力成分F1act，F4act，F5actが作用した場合に、第145ノード床反力中心点Q145のまわりに発生するモーメントM145actを算出する。この場合、算出するM145actは、互いに直交する水平な単位ベクトルであるベクトルV145およびU145のそれぞれの方向の成分から構成される。ベクトルV145またはU145の向きは、任意でよい。
【０３１１】
次いで、このM145actをローパスフィルタに通したものM145actfiltと、前記第145ノード補償床反力モーメントM145dmd（ベクトルV方向の成分およびベクトルU方向の成分）を補償フィルタに通したものM145dmdfiltとの偏差M145err（＝M145actfilt−M145dmdfilt）に所定のゲイン行列K145（対角行列）を乗算することで、第145ノード補償角θ145が決定される。θ145は、ベクトルVの軸回り角度成分とベクトルUの軸回りの角度成分とから構成される。
【０３１２】
補償角決定部（θ236決定部）１１４ｄによる第236ノード補償角θ236の決定処理も、上記したθ145決定部１１４ｃの処理と同様に行なわれる。
【０３１３】
以上説明した補償角決定部１１４ｂ〜１１４ｄの処理によって、目標全床反力中心点Ｐに作用する実モーメントMactの水平成分を補償全床反力モーメントMdmdに近づけるようにノード補償角θ145236，θ145，θ236の組が決定されることとなる。
【０３１４】
なお、第１実施形態に関して補足説明したように、次のようにノード補償角θ145236，θ145，θ236を決定するようにしてもよい。すなわち、子ノードを持つ各第ｎノード(n=145,236,145236)に対し、その第ｎノードの実ノード床反力（第ｎノードの全ての子ノードの実ノード床反力の合力）のモーメントの水平成分が０となるような床反力中心点を実第ｎノード床反力中心点として求める。あるいは、第ｎノード(n=145,236,145236)の実ノード床反力のモーメントから、その各子ノードの実ノード床反力が該子ノードの目標ノード床反力中心点に作用するモーメントを減じてなるモーメントの水平成分が０となるような床反力中心点を実第ｎノード床反力中心点として求める。そして、第ｎノード(n=145,236,145236)の目標ノード床反力中心点と、上記の如く求めた実第ｎノード床反力中心点との偏差、あるいは、それらのノード床反力中心点のそれぞれをフィルタに通した値の偏差を制御量とし、その制御量を０に近づけるように、該制御量に応じてノード補償角θ145236，θ145，θ236を決定する（例えば制御量にあるゲインを乗算することで、θ145236，θ145，θ236を決定する）。
【０３１５】
図２５に示した第２実施形態における修正目標接地部位位置姿勢算出部１１４ｇは、次のように、各接地部位１０の目標接地部位位置姿勢（図１のロボットでは実際には目標接地部位位置）を修正し、修正目標接地部位位置姿勢を得る。すなわち、図３０および図３１を参照して、第145ノードの子ノードである第１、第４および第５ノードの目標床反力中心点Q1，Q4，Q5をそれぞれ、第145ノードの目標床反力中心点Q145を回転中心として、第145ノード補償角θ145（水平ベクトル）だけ回転移動する。この回転移動後のQ1，Q4，Q5をそれぞれQ1'，Q4'，Q5'とする。従って、第145ノード補償角θ145は、第145ノードの目標床反力中心点Q145の位置を動かさずに、第145ノードの子ノードである第１、第４、第５ノードのそれぞれの目標床反力中心点Q1，Q4，Q5の位置の相対関係を動かすための操作量である。
【０３１６】
同様に、第236ノードの子ノードである第２、第３および第６ノードの目標床反力中心点Q2，Q3，Q6をそれぞれ、第236ノードの目標床反力中心点Q236を回転中心として、第236ノード補償角θ236（水平ベクトル）だけ回転移動する。この回転移動後のQ2，Q3，Q6をそれぞれQ2'，Q3'，Q6'とする。従って、第236ノード補償角θ236は、第236ノードの目標床反力中心点Q236の位置を動かさずに、第236ノードの子ノードである第２、第３、第６ノードのそれぞれの目標床反力中心点Q2，Q3，Q6の位置の相対関係を動かすための操作量である。
【０３１７】
これらの回転移動を視覚的に示したものが図３０である。
【０３１８】
次いで、第145236ノードの子ノードである第145および第236ノードの目標床反力中心点Q145，Q236をそれぞれ、第145236ノードの目標床反力中心点P（目標全床反力中心点）を回転中心として、前記偏差M145236errvのベクトル（水平ベクトル）と同方向（線分Q145Q236と直交する水平方向）の軸心まわりに第145236ノード補償角θ145236だけ回転移動する。この回転移動後のQ145，Q236を図３１に示す如くそれぞれQ145'，Q236'とする。従って、第145236ノード補償角θ145236は、第145236ノード（根ノード）の目標床反力中心点Ｐの位置を動かさずに、第145236ノードの子ノードである第145、第236ノードのそれぞれの目標床反力中心点Q145，Q236の位置の相対関係を動かすための操作量である。
【０３１９】
次いで、図３１を参照して、先の回転移動後の目標ノード床反力中心点Q1'，Q4'，Q5'をベクトルQ145_Q145'だけ平行移動し、これにより、第１、第４および第５ノードの最終的な修正後の目標ノード床反力中心点Q1''，Q4''，Q5''を得る。同様に、先の回転移動後の目標ノード床反力中心点Q2'，Q3'，Q6'をベクトルQ236_Q236'だけ平行移動し、これにより、第２、第３および第６ノードの最終的な修正後の目標ノード床反力中心点Q2''，Q3''，Q6''を得る。
【０３２０】
最後に、第ｎ接地部位（n=1,2,3,4,5,6)の目標接地部位位置を、ベクトルQn_Qn''だけ平行移動（ほぼ上下移動）させる。これにより、各接地部位１０の目標接地部位位置（より正確には各接地部位１０の目標接地部位位置の相対関係）が修正されることとなる。すなわち、子ノードを持つ各ノード毎に、そのノードの子孫ノードとなっている各接地部位１０の目標接地部位位置の間の相対関係（相対的位置関係）の操作量（修正量）が各補償角θ145，θ236，θ145236に応じて定められ、それらの操作量（修正量）を合成することで、各接地部位１０の目標接地部位位置の相互の相対関係が修正されることとなる。
【０３２１】
なお、接地部位の姿勢を制御可能として、接地部位の目標床反力中心点まわりに床反力モーメントを発生させることができるロボットでは、各接地部位の目標接地部位床反力中心点（目標ノード床反力中心点）まわりの足平姿勢回転動作を、特開平１０−２７７９６９号公報（複合コンプライアンス制御）に示した手法で行なえば良い。すなわち、第１実施形態に関して補足説明した如く、第ｎ接地部位を上記の平行移動後のQn''を中心として、該第ｎ接地部位の目標姿勢を修正するようにすればよい。
【０３２２】
図２５に示した第２実施形態における機構変形補償量算出部１１４ｎは、前記第１実施形態のものと同様に、各各脚＃１〜＃６のコンプライアンス機構４２などの変形による影響を補償するための機構変形補償量En_cmpn(n=1,2,3,4,5,6)を求める。すなわち、第２実施形態における補償全床反力モーメント分配器１１４ａから出力される各接地部位１０の修正目標接地部位床反力Fn_refmdfd(n=1,2,3,4,5,6,)（目標接地部位床反力に、各ノード補償床反力モーメントM145236dmd、M145dmd、M236dmdを付加することによって修正された目標接地部位床反力）によって発生が予想される各脚＃１〜＃６のコンプライアンス機構４２などの機構変形量En_mdfd(n=1,2,3,4,5,6)を機構コンプライアンスモデルを用いて求める。このEn_mdfdに（−１）を乗算したものを機構変形補償量En_cmpnとして求める。
【０３２３】
図２５に示した第２実施形態における機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈは、第１実施形態のものと同様に、機構変形補償入り修正目標接地部位位置姿勢は、各接地部位１０の修正目標接地部位位置姿勢（前記修正目標接地部位位置姿勢算出部１１４ｇで求められ位置姿勢）に、それぞれに対応する機構変形補償量En_cmpnを付加することにより決定される。
【０３２４】
以上が、本実施形態（第２実施形態）における階層型コンプライアンス動作決定部１１４の処理の詳細である。
【０３２５】
以上説明した以外の制御装置５０の演算処理は、第１実施形態と同じである。
【０３２６】
なお、第２実施形態における階層構造は、第１実施形態の場合と同様に、ロボット１の動作形態（移動形態）などに応じて変更しても良い。例えば、図３４に示す如く、階層構造を設定してもよい。この図示の例では、第１接地部位および第２接地部位を子ノード（葉ノード）としてもつ第12ノードと、第３接地部位および第４接地部位を子ノード（葉ノード）としてもつ第34ノードと、第５接地部位および第６接地部位を子ノード（葉ノード）としてもつ第56ノードと、第56ノードおよび第34ノードを子ノードとしてもつ第3456ノードとを中間ノードとして備え、根ノードは、第3456ノードと、第12ノードとを子ノードとしてもつものとされている。このようにすることで、コンプライアンス動作および後述の床形状推定をより的確に行なうことが可能となる場合がある。なお、図中の符号の意味は、前記図３（ｂ）や図２４などに示したものと同様である。
【０３２７】
［第３実施形態］
次に、第２実施形態のロボット１（６脚ロボット）に床形状推定機能とその推定結果に応じたロボット１の動作補正機能とを付加した第３実施形態について説明する。なお、本実施形態では、主に、第２実施形態で示した６脚ロボットを中心として説明するが、４脚ロボットについても補足的に説明を付加する場合がある。また、本実施形態の理解の便宜上、２脚ロボットについても言及する場合がある。
【０３２８】
本実施形態におけるロボット１の機構的構成は、第１または第２実施形態で説明した図１に示したもの（ただし、６個の脚＃１〜＃６を有するロボット１）と同じである。従って、ロボット１の機構的構成の説明は省略する。また、本実施形態におけるロボット１に備えた制御装置５０の機能的構成も、前記図２に示したものと同じである。但し、本実施形態では、図２中の階層型コンプライアンス動作決定部１１４には、新たな機能が付加されており、その点が第２実施形態のものと相違している。そして、階層型コンプライアンス動作決定部１１４以外の制御装置５０の構成要素の処理は、第２実施形態と同一である。そこで、本実施形態での説明では、階層型コンプライアンス動作決定部１１４の処理を中心に説明し、これ以外の制御装置５０の処理については、詳細な説明を省略する。
【０３２９】
図３５は、本実施形態における階層型コンプライアンス動作決定部１１４の処理機能を示すブロック図である。このうち、第２実施形態のものと異なる機能を説明すると、本実施形態における階層型コンプライアンス動作決定部１１４には、新たに、床形状推定手段としての床形状推定器１３０と、床形状推定器１３０が出力（推定）する推定床形状偏差（より詳しくは各接地部位１０に係る後述の床高さ偏差の推定値）を修正目標接地部位位置姿勢に加算する加算器１３２とが付加され、加算器１３２の出力を、修正目標接地部位位置姿勢の代わりに機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈに入力するようにしている。これ以外の階層型コンプライアンス動作決定部１１４の構成要素の処理は第２実施形態のものと同一である。
【０３３０】
また、これに伴い、本実施形態での制御装置５０のメインルーチンの制御処理は、その一部が前記図９のフローチャートに示した制御処理と相違している。図３６は本実施形態における制御装置５０のメインルーチンの制御処理を示すフローチャートである。図示の如く、本実施形態では、Ｓ３６の後に、新たにＳ３７で床形状偏差を推定する処理（床形状推定器１３０の処理）が追加される。さらに、Ｓ３８’では、前記第２実施形態で説明した各補償角θ145236，θ145，θ236とＳ３７で推定された床形状偏差とに応じて各目標接地部位位置姿勢が修正され、その修正後の目標接地部位位置姿勢をさらに機構変形補償量に応じて修正することにより、最終的な目標接地部位位置姿勢たる機構変形補償入り修正目標接地部位位置姿勢が得られる。この場合、より詳しくは、各補償角θ145236，θ145，θ236に応じて第２実施形態と同様に修正目標接地部位位置姿勢を求めた後、その修正目標接地部位位置姿勢が床形状偏差に応じて修正され、さらに、これが機構変形補償量に応じて修正されることで、機構変形補償入り修正目標接地部位位置姿勢が得られる。上記した事項以外は、図９の処理と同じである。
【０３３１】
以降、本実施形態の第２実施形態と相違する点を具体的に説明する。
【０３３２】
まず、本実施形態の詳細な説明に入る前に、床形状推定器１３０が推定する上で使用する概念と用語を以下のように定義する。なお、ここでの説明は、便宜上、本実施形態の６脚ロボット１に限らずに、一般的なロボットの簡略的な図を使用して行なう。
【０３３３】
図３７、図３８および図３９に示すように、目標歩容において想定された床（あるいは床面）を「想定床」と呼ぶ。ロボットが移動する実際の床を「実床」と呼ぶ。なお、説明の便宜上、図３７では第１実施形態で説明した４脚ロボットを図示し、図３８および図３９では、２脚ロボットを図示しているが、以下に説明する用語の意味は、本実施形態における６脚ロボット１を含む任意の多脚ロボットにおいても同様である。
【０３３４】
前記第１および第２実施形態で説明した階層型コンプライアンス制御において定義された目標第ｎ接地部位床反力中心点Ｑｎは、第ｎ接地部位の中心点に設定された点であったが、その床反力中心点Qnは、該第ｎ接地部位の接地面（底面）に設定されていてもよい。この場合、目標歩容において、目標第ｎ接地部位床反力中心点Ｑｎと接することが想定された想定床面上の点を「想定第ｎ床接点Ｄn」と呼ぶ。
【０３３５】
この定義から明らかなように、ロボットの目標歩容において第ｎ接地部位が接地している時期では、目標第ｎ接地部位床反力中心点Ｑｎと想定第ｎ床接点Ｄnとは、支持脚座標系（グローバル座標系）から見て同一座標になる。これに対し、実際にロボット１が移動しているときに、実第ｎ接地部位の底面上における目標第ｎ接地部位床反力中心点Ｑｎに対応する点が、実床に接触する点を「実第ｎ床接点Ｄnact」と呼ぶ。
【０３３６】
これらの点の関係を表す例を図３７、図３８および図３９に示す。尚、図３７は４脚ロボットを、目標第１接地部位床反力中心点Ｑ１と目標第２接地部位床反力中心点Ｑ２とを通る垂直面の法線方向から（すなわちほぼ側方から）見た図、図３８は移動（歩行）している２脚ロボットを目標第１接地部位床反力中心点Ｑ１と目標第２接地部位床反力中心点Ｑ２とを通る垂直面の法線方向から（すなわちほぼ側方から）見た図、図３９はほぼ直立姿勢の２脚ロボットを目標第１接地部位床反力中心点Ｑ１と目標第２接地部位床反力中心点Ｑ２とを通る垂直面の法線方向から（すなわちほぼ背面から）見た図である。
【０３３７】
これらの図３７〜図３９には、前記垂直面における想定床の断面が細線で示され、前記垂直面における実床の断面が太線で示されている。なお、図３７では、ロボットの目標姿勢（目標歩容の瞬時値におけるロボットの全体的な姿勢）と、実姿勢とがそれぞれ破線、実線で示されている。また、図３８および図３９では、ロボットの目標姿勢（目標歩容の瞬時値におけるロボットの全体的な姿勢）と、実接地部位位置姿勢とがそれぞれ細線、太線で示されている。これらの状況における実第ｎ床接点は、実床面上の点であり、図３７、図３８および図３９に示す位置になる。
【０３３８】
想定床面に対する実床面の形状偏差を床形状偏差と呼ぶ。床形状偏差を定量的に表現する指標として、第ｎノード床高さ偏差、第ｎノード床傾斜偏差を以下のように定義する。
【０３３９】
第ｎ床接点における床面の高さを「第ｎ接地部位床高さ」と呼ぶ。葉である第ｎノードに対して、実第ｎ接地部位床高さと想定第ｎ接地部位床高さとの差を「第ｎ接地部位床高さ偏差」あるいは、「第ｎノード床高さ偏差」と呼ぶ。第ｎ床接点における床面の傾斜を「第ｎ接地部位床傾斜」と呼ぶ。葉である第ｎノードに対して、実第ｎ接地部位床傾斜と想定第ｎ接地部位床傾斜との差を「第ｎ接地部位床傾斜偏差」あるいは、「第ｎノード床傾斜偏差」と呼ぶ。接地部位床傾斜偏差の例を図３９に示す。
【０３４０】
葉ノードである、あらゆる第ｊノードに対して、目標第ｊ接地部位位置姿勢と想定第ｊ床面の高さと傾斜（詳しくは、想定第ｊ床接点における想定床面の高さと傾斜）の関係と、ノード補償角の組を用いたコンプライアンス動作によって修正された修正目標第ｊ接地部位位置姿勢と実第ｊ床面の高さと傾斜（詳しくは、実第ｊ床接点における実床面の高さと傾斜）の関係とが一致するためのノード補償角の組を「ノード床傾斜偏差（の組）」と呼び、このうちの第ｎノード補償角に対応するノード床傾斜偏差の成分を「第ｎノード床傾斜偏差」と呼ぶ。このように定義した「第ｎノード床傾斜偏差」は、第ｎノードが葉ノードである場合、先に上記で葉ノードに関して定義した「第ｎノード床傾斜偏差」（＝実第ｎ接地部位床傾斜と想定第ｎ接地部位床傾斜との差）に一致するものである。
【０３４１】
結局、ノード床傾斜偏差の組は、目標歩容通りに移動しているロボットの全ての接地部位を、目標床面に平行になっている状態から、実床面に平行にさせるために必要な補償量に相当する。
【０３４２】
従って、歩行中に床形状偏差を推定し、推定した床形状偏差を目標接地部位位置姿勢に加えれば、床形状偏差があっても、各ノードの実床反力モーメントは想定床を歩行しているときと同一になる。当然、葉ノードの実床反力モーメントである接地部位の実床反力モーメントも目標接地部位床反力モーメントに一致する。
【０３４３】
通常は、上記のごとく定義される「第ｎノード床傾斜偏差」を用いて床形状を表現して構わないが、あるノードが３つの子ノードを持ち、３つの子ノードの目標床反力中心点が同一直線上に並ぶ場合や、あるノードが４つ以上の子ノードを持つ場合などでは、床形状を表現することが困難になる。
【０３４４】
そこで、以降の説明においては、葉でない第ｎノードに対しては、より一般的に適用できる以下の表現を用いることとする。以下の定義における所定の重み付き平均を求めるために用いる重みは、前記目標床反力分配器１０２が前記した如く決定した前述の重みと同一のものとする。
【０３４５】
定義：
すべての目標接地部位の高さと傾斜を、それぞれ対応する実床面の高さと傾斜に一致させ、かつ、任意のノードの目標床反力中心点をそのすべての子ノードの目標床反力中心点の所定の重み付き平均（すなわち所定の内分比による内分点）で表したときに、任意の第ｎノードに対し、第ｎノードの目標床反力中心点の高さ（鉛直方向位置）から第ｎノードの親ノードの目標床反力中心点の高さ（鉛直方向位置）を減じた高さを「実第ｎノード相対床高さ」と呼ぶ。
【０３４６】
定義：
すべての目標接地部位の高さと傾斜を、それぞれ対応する想定床面の高さと傾斜に一致させ、かつ、任意のノードの目標床反力中心点をそのすべての子ノードの目標床反力中心点の所定の重み付き平均（すなわち所定の内分比による内分点）で表したときに、任意の第ｎノードに対し、第ｎノードの目標床反力中心点の高さ（鉛直方向位置）から第ｎノードの親ノードの目標床反力中心点の高さ（鉛直方向位置）を減じた高さを「想定第ｎノード相対床高さ」と呼ぶ。
【０３４７】
実第ｎノード相対床高さから想定第ｎノード相対床高さを減じた高さを「第ｎノード相対床高さ偏差」と呼ぶ。第ｎノード相対床高さ偏差は、以下のように定義しても、同一値となる。
【０３４８】
定義：
すべての目標接地部位の高さと姿勢を、それぞれ対応する接地部位床高さ偏差と接地部位床傾斜偏差に一致させ、かつ、任意のノードの目標床反力中心点をそのすべての子ノードの目標床反力中心点の所定の重み付き平均（すなわち所定の内分比による内分点）で表したときに、任意の第ｎノードに対し、第ｎノードの目標床反力中心点高さから第ｎノードの親ノードの目標床反力中心点高さを減じた高さを「第ｎノード相対床高さ偏差」と呼ぶ。
【０３４９】
上記した第ｎノード相対床高さ偏差の定義から明らかなように、子ノードを持つ各ノードに対し、その全ての子ノードの第ｎノード相対床高さ偏差は、床高さ偏差の相対関係を表すものとなる。また、子ノードを持つ各ノードに対し、その全ての子ノードの重み付き平均値は０となる。
【０３５０】
本実施形態（第３実施形態）では、実床面と目標歩容上での想定床面とのずれ（すなわち床形状偏差）を補償して、接地すべき接地部位１０を適正に実床面に接地させるように各接地部位１０の目標位置姿勢の修正を行なう。そして、本実施形態では、上記のように定義される第ｎノード相対床高さ偏差を床形状偏差を表す床形状パラメータとして用い、これを基に、各第ｎ接地部位床高さ偏差（床形状偏差）を推定する。そして、推定した各第ｎ接地部位床高さ偏差（以下、単に第ｎ床高さ偏差ということがある）に応じて第ｎ接地部位１０の目標位置を修正するようにした。
【０３５１】
以上を前提として、以下に本実施形態の要部の詳細を説明する。
【０３５２】
図３５に示した階層型コンプライアンス動作決定部１１４の床形状推定器１３０への入力は、一般的には大きく分けて以下のようになる。
１）各目標床反力中心点（目標全床反力中心点Ｐ、目標第ｎ接地部位床反力中心点Qn(n=1,2,3,4,5,6)）。
２）想定床面形状（想定第ｎ床接点の座標、想定第ｎ接地部位床傾斜）。
３）最終的に実ロボットが追従すべき目標姿勢（機構変形補償入り修正目標接地部位位置姿勢）または実関節変位またはこれらの周波数重み付き平均（周波数特性を持つ重みによる重み付き平均）。
４）上体姿勢傾斜偏差。
５）実床反力（実第ｎ接地部位床反力の並進力成分とモーメント成分）。
【０３５３】
なお、本実施形態におけるロボット１では、各接地部位１０の姿勢を制御できないので、想定床面形状のうちの、想定第ｎ接地部位傾斜は不要である。
【０３５４】
また、本実施形態におけるロボット１では、実床反力のうちの実第ｎ接地部位床反力のモーメント成分は０になるので、これも床形状推定器１３０に入力する必要は無い。ただし、以降の本実施形態の説明では、各接地部位１０の姿勢を制御できる場合も考慮し、第ｎ接地部位床反力には、一般に、実第ｎ接地部位床反力のモーメント成分も含まれるものとする。また、以降の説明では、接地部位の個数が４個や６個以外の場合にも容易に本実施形態を拡張的に適用できるように、接地部位の総数をしばしば「最終葉ノード番号」と表す。例えば図１の６脚ロボット１では、最終葉ノード番号は、「６」である。
【０３５５】
図４０は、床形状推定器１３０の処理機能を示すブロック図である。図４０に示す床形状推定器１３０を構成する各機能要素について説明すると、床形状推定器１３０は機構コンプライアンスモデル１３４を備える。
【０３５６】
機構コンプライアンスモデル１３４は、各接地部位１０が実床反力Fn_act(n=1,2,…,最終葉ノード番号)を受けたときの各脚＃１〜＃６のコンプライアンス機構４２など（各脚のコンプライアンス機構４２およびリンク機構）の変形量を求め、求めた変形量を、前記ロボット幾何学モデル１１０（図２参照）への入力である各機構変形補償入り修正目標接地部位位置姿勢に加えることにより、機構変形後の第ｎ接地部位（各接地部位）の位置姿勢の推定値（より詳しくは、実上体姿勢が目標上体姿勢に一致していると仮定した場合の第ｎ接地部位位置姿勢の推定値）である推定第ｎ接地部位位置姿勢(n=1,2,…,最終葉ノード番号)を求める。なお、このときに用いる各機構変形補償入り修正目標接地部位位置姿勢は、前回制御周期で求めた値などの過去値である。
【０３５７】
機構変形後の各推定接地部位位置姿勢を求める際に、ロボット１の関節変位の制御の追従遅れが無視できない場合には、機構変形補償入り修正目標接地部位位置姿勢をそのまま用いる代わりに、機構変形補償入り修正目標接地部位位置姿勢を、追従遅れに相当するローパスフィルタに通してなる位置姿勢を用いれば良い。
【０３５８】
あるいは、ロボット１の実関節変位の検出値からロボット幾何学モデル（図２のロボット幾何学モデル１１０と同様のモデル）を介して機構変形が無い場合の実接地部位位置姿勢である機構変形なし実接地部位位置姿勢を求め、求めた値を機構変形補償入り修正目標接地部位位置姿勢の代わりに用いれば良い。
【０３５９】
あるいは、周波数重み（周波数特性を持つ重み）を用い、前記機構変形なし実接地部位位置姿勢と機構変形補償入り修正目標接地部位位置姿勢の重み付き平均を求め、それを機構変形補償入り修正目標接地部位位置姿勢の代わりに用いれば良い。
【０３６０】
厳密には、前記した機構変形なし実接地部位位置姿勢を用いるべきであるが、関節変位制御の能力が高ければ、上記のいずれを用いても大差はない。
【０３６１】
なお、機構コンプライアンスモデル１３４については、本出願人が先に提案した特開平１０−２７７９６９号公報において詳細が説明してあるので、ここではこれ以上の説明を省略する。
【０３６２】
補足すると、図２においてロボット幾何学モデル１１０への入力に機構変形補償が含まれない場合、すなわち、機構変形補償のない修正目標接地部位位置姿勢（前記修正目標接地部位位置姿勢算出部１１４ｇで求められる位置姿勢）をロボット幾何学モデル１１０に入力する場合には、その機構変形補償のない修正目標接地部位位置姿勢を床形状推定器１３０の機構コンプライアンスモデル１３４に入力すれば良い。
【０３６３】
図４０の説明に戻ると、次いで、機構変形後の推定各接地部位位置姿勢(n=1,2,…,6)を、目標全床反力中心点Pを回転中心として前記上体姿勢傾斜偏差θberrだけ回転移動させることにより、グローバル座標系（支持脚座標系）から見た機構変形後の第n推定接地部位位置姿勢(n=1,2,…,最終葉ノード番号)を求める。次いで、グローバル座標系から見た機構変形後の第n推定接地部位位置姿勢(n=1,2,…,最終葉ノード番号)と第ｎ接地部位の目標接地部位位置（第ｎ接地部位の代表点の目標位置）から見た目標第n接地部位床反力中心点の位置とを基に、グローバル座標系から見た機構変形後の目標第n接地部位床反力中心点に対応する点の位置を求める。そして、この求めた点の位置を瞬間推定床接点位置（瞬間推定第ｎ床接点位置）Qn_estm'とする。このQn_estm'は、実第ｎ床接点の瞬時位置の推定値に相当するものである。尚、瞬間推定床接点位置Qn_estm'の求め方の詳細は、特開平１０−２７７９６９号公報中の式２１のＱ''を求めることに相当するので、ここではこれ以上の説明を省略する。
【０３６４】
次いで、瞬間推定第ｎ床接点位置Qn_estm'から想定第n床接点位置Dnを減じたものを、バイアス含有瞬間第ｎ接地部位床高さ偏差Zfn_with_bias'として得る。このZfn_with_bias'は、第ｎ接地部位床高さ偏差の瞬時推定値に相当するものであるが、本実施形態のように上体の実高さを検出しない場合には、一般にバイアス誤差を含んでいる。そのため、Zfn_with_bias'をバイアス含有瞬間第ｎ接地部位床高さ偏差と呼ぶ。
【０３６５】
ここで、想定第ｎ床接点Ｄｎは、先に定義したように、第ｎ接地部位が接地している時期においては、目標第ｎ接地部位床反力中心点Ｑｎと同一位置とする。目標歩容において第ｎ接地部位が接地する直前の時期では、次に接地する時に想定されている目標第ｎ接地部位床反力中心点Ｑｎの位置を想定第ｎ床接点Ｄｎとする。目標歩容において第ｎ接地部位が離床した直後の時期では、離床時に想定されていた目標第ｎ接地部位床反力中心点Ｑｎの位置を想定第ｎ床接点Ｄｎとする。
【０３６６】
次いで、バイアス含有瞬間第ｎ接地部位床高さ偏差Zfn_with_bias'(n=1,2,…,最終葉ノード番号)と第ｎ接地部位床反力Fn_act（n=1,2,…,最終葉ノード番号）とを基に、図４１のフローチャートで示す床高さ偏差推定処理サブルーチンによって、推定第ｎノード床高さ偏差Zfn_estm（n=1,2,…,最終葉ノード番号）を求める。
【０３６７】
ここで、床高さ偏差推定処理サブルーチン等に用いられる「階層相対化処理」（または「階層相対化」）について説明する。
【０３６８】
階層相対化処理とは、一般的に定義すると、すべての葉ノードに対する入力値（ある所定種類の状態量の値）の組に対して、すべてのノードの出力値を決定する処理である。より詳しくは、階層相対化処理は、葉ノードでない任意のノードのすべての子ノードに対応する出力値の重み付き平均が０であり、かつ、任意の葉ノードの入力値（状態量）が、そのノードの出力値とそのノードのすべての先祖ノードの出力値との和に一致するように各ノード出力値を決定する処理のことである。
【０３６９】
以降、葉ノードに対するある種類の入力（状態量）AAから、階層相対化処理によってある種類の出力BBの値を求めることを、「AAを階層相対化してBBを求める」と言う。
【０３７０】
以下に階層相対化処理のアルゴリズムを説明する。一般的に、階層相対化処理の入力を第n接地部位高さZfn(ｎは葉ノード番号)、出力を第nノード相対高さZn_rel(ｎ=1,2,…,最終ノード番号)とする。なお、「最終ノード番号」は、全てのノード番号のうちの最大番号を意味し、例えば第２実施形態で説明した階層構造では、最終ノード番号＝145236である。また、このアルゴリズムの説明では、「第ｎ接地部位高さ」は、前記第ｎ接地部位床高さ、あるいは第ｎ接地部位床高さ偏差など、階層化相対化処理の入力を総称的に表した名称であり、「第ｎノード相対高さ」は、後述する第ｎ接地部位相対床高さ偏差など、階層相対化処理の出力を総称的に表した名称である。
【０３７１】
まず、バイアス含有第nノード高さZn_with_biasを以下のように決定する。すなわち、第nノードが葉ノード（すなわち接地部位）であるならば、バイアス含有第nノード高さZn_with_biasに第n接地部位高さZfnの値を代入する。第nノードが葉ノード（すなわち接地部位）でないならば、第nノードのすべての子ノードのバイアス含有第nノード高さZn_with_biasの重み付き平均を求め、これをもってバイアス含有第nノード高さZn_with_biasとする。ただし、各子ノードに対する重みには、前記目標床反力分配器１０２が決定した重みWj(j=1,2,…)を用いる。
【０３７２】
すなわち、以下の式３２によって、バイアス含有第nノード高さZn_with_biasを得る。
【０３７３】
第nノードが葉ノードである場合、
Zn_with_bias＝Zfn
第nノードが葉ノードでない場合、
Zn_with_bias＝Σ(Zj_with_bias＊Wj)
ただし、Σはj∈{第ｎノードの子ノード番号の集合}であるjについての総和。
…式３２
【０３７４】
以上のルールに従って、すべてのノードに対してバイアス含有ノード高さZn_with_bias（n=1,2,…,最終ノード番号)を求める。
【０３７５】
最後に、第nノードの親ノード（これは第hノードであるとする）のバイアス含有高さZh_with_biasをバイアス含有第nノード高さZn_with_biasから減じることにより、第nノード相対高さZn_relを求める。
【０３７６】
すなわち、次式３３により第nノード相対高さZn_relを求める。ただし、第hノードは、第nノードの親ノードであるとする。
【０３７７】
Zn_rel＝Zn_with_bias−Zh_with_bias        …式３３
以上のルールに従って、すべてのノードに対してノード相対床高さZn_rel(nは各ノードの番号）を求める。ただし、根ノードに対するのード相対高さZk_rel(kは根ノードの番号）は０とする。図４２に第１実施形態で示した４脚ロボットのノード相対高さの算出例を示し、図４３に本実施形態における６脚ロボットのノード相対高さの算出例を示す。
【０３７８】
バイアス含有第nノード高さZn_rel_with_biasは、同一の結果が得られる以下の方法によって求めても良い。
【０３７９】
第nノードが葉ノード（すなわち接地部位）であるならば、バイアス含有第nノード高さZn_with_biasに第n接地部位高さZfnの値を代入する。第nノードが葉ノード（すなわち接地部位）でないならば、第nノードのすべての子孫の葉ノードのバイアス含有高さの重み付き平均を求め、これをもってバイアス含有第nノード高さZn_with_biasとする。ただし、ここでの各葉ノードjに対する重みWj'は、そのノードjに対して前記目標床反力分配器１０２が決定した重みWjとそのノードjの祖先ノードかつ第nノードの子孫ノードであるすべてのノードに対して前記目標床反力分配器１０２が決定した重みとの積とする。
【０３８０】
すなわち、以下の式３４によって、バイアス含有第nノード高さZn_with_biasを得る。
【０３８１】
第nノードが葉ノードであるならば、
Zn_with_bias＝Zfn
第nノードが葉ノードでないならば、
Zn_with_bias＝Σ(Zj_with_bias＊Wj')
ただし、Σはj∈{ノードnの子孫の葉ノード番号の集合}であるj
についての総和。
…式３４
【０３８２】
補足すると、この場合の重みWj'は、目標第ｊノード床反力が０でなければ、各葉ノードの目標床反力を目標第ｊノード床反力で割った値に一致する。
【０３８３】
以上が階層相対化処理である。
【０３８４】
次いで、床高さ偏差の推定処理のサブルーチン処理を、それを示す図４１のフローチャート等を用いて以下に説明する。なお、この処理では、階層相対化処理の入力として、第ｎ接地部位床高さ偏差を用い、出力を第ｎノード相対床高さ偏差とする。そして、これらの入力および出力の参照符号として、前記式３２〜３４などに示したものと同様の符号を使用する。
【０３８５】
図４１を参照して、まず、Ｓ５０において、前記バイアス含有瞬間第n接地部位床高さ偏差Zfn_with_bias'を階層相対化して瞬間第nノード相対床高さ偏差Zn_rel'（n=1,2,…,最終ノード番号)
を求める。
【０３８６】
次いで、Ｓ５２において、制御装置５０の前回制御周期で求めた、第ｎ接地部位床高さ偏差の推定値である推定第n接地部位床高さ偏差（以降、これを前回推定第n接地部位床高さ偏差Zfn_estm_pと呼ぶ）を階層相対化して得られる出力を前回推定第nノード相対床高さ偏差Zn_rel_estm_p(n=1,2,…,最終ノード番号)として求める。ただし、この階層相対化処理で使用する各ノードの重みには、制御装置５０の今回（現在）の制御周期で前記目標床反力分配器１０２が決定した重みWn(n=1,2,…,最終ノード番号)を用いる。
【０３８７】
補足すると、一般的に、今回の制御周期で決定された重み（以下、今回重みという）と前回の制御周期で決定された重み（以下、前回重みという）とが異なる場合には、実際の床の形状に変化がなくても、前回重みを用いて求めた各ノードの前回推定相対床高さ偏差の値と今回重みを用いて求めた各ノードの前回推定相対床高さ偏差の値とは異なる値となってしまう。そこで、前回の制御周期で求めた各ノードの推定相対床高さ偏差をそのまま各ノードの前回推定ノード相対床高さ偏差として用いず、上記のごとく、今回重みを用いて各ノードの前回推定ノード相対床高さ偏差を改めて計算するようにした。
【０３８８】
次いで、Ｓ５４において、次式３５により、第ｎノード相対床高さ偏差補正量候補値Zn_inc_cand(n=1,2,…,最終ノード番号)を求める。このZn_inc_candは、第ｎノード相対床高さ偏差を今回の制御周期で更新するときの該偏差の補正量の仮値を意味するものである。
【０３８９】
この場合、第ｎノード相対床高さ偏差補正量候補値Zn_inc_candは、次式３５に示す如く、瞬間第nノード相対床高さ偏差Zn_rel'と前回推定第nノード相対床高さ偏差Zn_rel_estm_pの差と所定の係数(ΔT/(Testm＋ΔT))との積に設定される。
【０３９０】
Zn_inc_cand＝(Zn_rel'−Zn_rel_estm_p)
＊ΔT/(Testm＋ΔT)
…式３５
ここで、式３５中のTestmは第ｎノード床高さ偏差の推定（更新）における時定数（一次遅れの時定数）であり、ΔＴは制御装置５０の制御周期である。
【０３９１】
このように、Zn_inc_candは設定したとき、制御装置５０の制御周期毎に、Zn_inc_candをZn_rel_estm_pに加算していくことで、その加算結果の値は、徐々にZn_rel'に近づいていくように変化する。
【０３９２】
次いで、Ｓ５６において、目標歩容の時期に応じて各ノードのモードの要求値であるノード要求モード（第ｎ要求モードmdn_dmd）を決定する。
【０３９３】
具体的には、前記６脚ロボット１においては、第nノード要求モード（n=1,2,3,4,5,6）、第145ノード要求モードmd145dmd、第236ノード要求モードmd236dmdおよび第145236ノード要求モードmd145236dmdを、目標歩容の時間経過に対して図４４のタイミングチャートで示すように設定し、これにしたがって現在の要求モードを決定する。ただし、歩容が異なれば、それに応じてノード要求モードのパターンも変えるべきである。なお、図４４の第１段（最上段）および第２段のタイミングチャートにおけるＯＮ／ＯＦＦは、接地部位が接地している状態がＯＮに対応しており、接地していない状態がＯＦＦに対応している。
【０３９４】
補足すると、本願出願人が、特開平１０−２７７９６９号公報に示したように、接地部位として足平を持ち、接地部位における床傾斜偏差を推定することができるロボットの場合には、接地部位の床傾斜偏差推定に対応する要求モードも、同公報に説明されているように設定すべきである。
【０３９５】
図４４に示すように、各ノードのモード（以下、単にノードモードという）には、準備完了モード、ホールドモード、およびリセットモードがある。
【０３９６】
準備完了モードは、床形状の推定を行なうモードである。ホールドモードは、床形状の推定値（推定第ｎ接地部位床高さ偏差）をホールドする（直前の値を維持する）モードである。ホールドモードは、床形状推定値が発散する恐れがある時期と、床形状推定値の精度が低下する恐れがある時期に存在させる。また、リセットモードは、床形状推定値を、次の床形状の推定を開始する（次の準備完了モードの開始）までに所定の初期値に移行させるモードである。
【０３９７】
葉ノードである第ｎノード（n=1,2,3,4,5,6）に対しては、目標歩容上で第ｎ接地部位が接地しているならば（すなわち目標第ｎ接地部位床反力が０でないならば）、第ｎノード要求モードは、準備完了モードに設定される。その後、目標歩容上で第ｎ接地部位が離床した（すなわち目標第ｎ接地部位床反力が０になった）ならば、その直後は第ｎノード要求モードがホールドモードに設定される。そして、その後しばらくしたら第ｎノード要求モードがリセットモードに設定される。さらに、目標歩容上で第ｎ接地部位が接地する直前から、第ｎノード要求モードが準備完了モードに設定される。
【０３９８】
また、葉ノードでない第nノード（n=145,236,145236)、すなわち、子ノードをもつ第ｎノードに対しては、その第ｎノードに属する少なくとも１つの接地部位が目標歩容上で接地している場合には、第ｎノード要求モードは、準備完了モードに設定される。その後、目標歩容上で第ｎノードに属するすべての接地部位が離床したならば、その直後までは、第ｎノード要求モードがホールドに設定される。なお、第ｎノードに属するすべての接地部位が離床する直前から第ｎノード要求モードをホールドモードに設定するようにしてもよい。そして、その後しばらくしたら第ｎノード要求モードがリセットモードに設定される。さらに、目標歩容上で第ｎノードに属する少なくとも１つの接地部位が接地する直前から、第ｎノード要求モードが準備完了モードに設定される。
【０３９９】
次いで、Ｓ５８において、推定許可条件を満足するか否かを判定し、その判定結果とノード要求モードに基づき各ノードのモードを最終的に決定する。決定される各ノードのモードは、前記準備完了モード、ホールドモード、およびリセットモードのいずれかである。前記ノード要求モードは、目標歩容上での各接地部位の接地、非接地に応じて決定したものであったが、Ｓ５８では、各接地部位の実際の接地、非接地などを考慮して、各ノードのモードが決定される。
【０４００】
推定許可条件とは、以下のいずれかの式３６、３７を満足することである。この場合、式３６、３７のいずれも満足しないときには、推定許可条件が成立しないとする。
【０４０１】
Fn_act_z＞Fn_min                        …式３６
Zn_rel'＜Zn_rel_estm_p            …式３７
ここで、Fn_act_zは、実第ｎ接地部位床反力(n=1,2,…,最終葉ノード番号)の並進力鉛直成分である。
【０４０２】
推定許可条件とは、第ｎノード相対床高さ偏差を推定しても（床形状偏差を推定しても）、その推定値が発散しない条件である。ここで発散とは、後述するごとく推定された推定第n床高さ偏差Zfn_estm（床形状偏差の推定値）を用いて、実際の第n床高さ偏差の影響を打ち消すように修正目標接地部位位置姿勢を修正する補正動作を行った場合に、推定第n床高さ偏差Zfn_estmが増加し続け、第ｎ接地部位が床から離れて行く（床からさらに浮いていく）状況を意味する。
【０４０３】
Zfn_estmの推定が理想的に実行されるならば、所定の許容値Fn_minは、０で良いが、実際にはFn_actの検出誤差が生じ、それにより推定値が発散する場合がある。それを防ぐために、Fn_minは、Fn_actの検出誤差よりも十分に大きい値に設定する。
【０４０４】
本実施形態では、推定許可条件とは、以上に述べた発散が生じない条件である。ただし、推定許可条件は、床形状の所要の推定精度を確保できる条件であってもよい。従って、実第ｎ接地部位床反力Ｆn_actの並進力鉛直成分の代わりに、実第ｎ接地部位床反力Ｆn_actの並進力成分の内の想定床面（あるいは推定床面（推定した実床面））に垂直な成分を用いても良い。
【０４０５】
なお、床形状偏差（実床面と想定床面との形状偏差）を推定するだけで歩行動作に反映しない場合、すなわち、床形状偏差の推定値を用いて床形状偏差の影響を打ち消すように修正目標接地部位位置姿勢を修正しない場合には、床形状偏差の推定値が発散することはない。
【０４０６】
推定許可条件を満足するか否かの判定結果とノード要求モードに基づき各ノードのモードを決定する。
【０４０７】
第nノード要求モードが準備完了モードであり、かつ、推定許可条件を満足するならば、第ｎノードモードを準備完了モードとする。
第nノード要求モードが準備完了モードであり、かつ、推定許可条件を満足しないならば、第ｎノードモードをホールドモードとする。
第nノード要求モードがホールドモードであるならば、第ｎノードモードをホールドモードとする。
第nノード要求モードがリセットモードであるならば、第ｎノードモードをリセットモードとする。
【０４０８】
従って、第ｎノードのモードは、要求モードが準備完了モードであり、且つ、推定許可条件を満足する場合に限って、最終的に準備完了モードの決定される。
【０４０９】
次いで、Ｓ６０〜Ｓ７０において、すべてのノードに対し、ノードに対応する第ｎノード相対床高さ偏差(n=1,2,…,最終ノード番号)を推定する。
【０４１０】
補足すると、特開平１０−２７７９６９号公報に示したように、姿勢を制御可能な接地部位（足平）を持ち、接地部位における床傾斜偏差を推定することができるロボットの場合には、接地部位に対応した床傾斜偏差も推定することが望ましい。
【０４１１】
このため、Ｓ６０〜Ｓ７０の処理は、接地部位の姿勢を制御可能な場合でも、床形状偏差の推定を行なうことができるように構成されている。
【０４１２】
以下にＳ６０〜Ｓ７０の処理を具体的に説明すると、Ｓ６２において、第nノードの子ノードの個数を判定する。このとき、子ノード数（子ノードの個数）が２個の場合には、Ｓ６４において、その個数に対応した床形状偏差の推定処理である２子ノード用床形状推定処理が行なわれる。また、子ノード数が３個の場合には、Ｓ６６において、その個数に対応した床形状偏差の推定処理である３子ノード用床形状推定処理が行なわれる。また、子ノード数が０個の場合には、Ｓ６８において、該第ｎノードの接地部位が床反力モーメントを制御できるか否かを判定する。
【０４１３】
子ノード数が０の場合の処理についてさらに説明すると、第ｎノードの子ノード数が０の場合、第ｎノードは葉ノードであり、第ｎノードは接地部位に対応している。この場合、上記の如くＳ６８において、まず、第ｎ接地部位が床反力モーメントを制御できるか否かを判定する。ここで、例えば、通常の２足歩行ロボットのように、接地部位として姿勢を制御可能な足平を持ち、脚にフリージョイントを含まないロボットの場合には、接地部位で床反力モーメントを発生することができる。ただし、本実施形態のロボット１（６脚ロボット）においては、各接地部位１０がフリージョイントである球関節１２に係合しているため、いずれの接地部位１０も床反力モーメントを発生することができない。この場合には、子ノード数が０である第ｎノードに対する処理は実行されない。
【０４１４】
一方、Ｓ６８において、床反力モーメントを制御できると判定された場合には、Ｓ７０において、接地部位床傾斜偏差推定処理が行なわれる。この処理は、各接地部位における床傾斜偏差を推定する処理である。この処理は、本出願人が先に提案した特開平１０−２７７９６９号公報の中の床傾斜推定処理における足平床反力中心点を本実施形態における第ｎ接地部位の目標床反力中心点に置き換えた処理である。したがって、本明細書では、これ以上の詳細説明を省略する。
【０４１５】
次に、前記２子ノード用床形状推定処理を図４５〜図４８を参照して説明する。図４５は２子ノード用床形状推定処理のサブルーチン処理を示すフローチャート、図４６、図４７はそれぞれ図４５のＳ６４０４、Ｓ６４０８のサブルーチン処理を示すフローチャート、図４８は図４５のＳ６４０６およびＳ６４１０のサブルーチン処理を示すフローチャートである。
【０４１６】
２子ノード用床形状推定処理では、図４５のフローチャートに示されるように、Ｓ６４００において、２個の子ノードをもつ第ｎノードのすべての子ノードのモード（図４１のＳ５８で決定されたモード）が判断される。その判断結果は、「すべて準備完了」、「すべてリセット」、および、「その他（else）」の３つに分けられる。なお、以降の説明では、第ｎノードの２個の子ノードを第ｉノード、第ｊノードとする。
【０４１７】
ここで、子ノードのモードが「すべて準備完了」と判断すると、Ｓ６４０２において、第ｎノードの子ノードの実ノード床反力の並進力鉛直成分Fi_act_z，Fj_act_zの合力Fn_z（＝Fi_act_z＋Fj_act_z）が所定の値Fn_min2より大きいか否かを判断する。Fn_zは、換言すれば、第ｎノードに属する全ての接地部位の実床反力の合力の並進力鉛直成分である。
【０４１８】
Ｓ６４０２の判断結果がＹＥＳである場合には、Ｓ６４０４において、２つの子ノードを持つノード用のグループ内全推定処理（第ｎノードが持つ２つの子ノードのそれぞれのノード相対床高さ偏差を実質的に推定する処理）を行なう。この処理では、図４６のフローチャート中に示す式に従って第ｎノードの２つの子ノードである第ｉノードおよび第ｊノードのそれぞれの推定ノード相対床高さ偏差Zi_rel_estm、Zj_rel_estmが求められる（更新される）。すなわち、Zi_rel_estmについて代表的に説明すると、Zi_rel_estmの前回制御周期での値Zi_rel_estm_pに、前記Ｓ５４で求めた第ｉノード相対床高さ偏差補正量候補値Zi_inc_candを加えることで、新たな推定第ｉノード相対床高さ偏差Zi_rel_estmが求められる。第ｊノードについても同様である。
【０４１９】
また、第ｎノードの２個の子ノードの床反力の合力Fn_zが所定の値Fn_min2より小さい場合（Ｓ６４０２の判断結果がＮＯである場合）には、床形状偏差の推定精度が下がり過ぎるので、実質的な推定処理を実行せずに、Ｓ６４０６において、２つの子ノードを持つノード用のグループ内全ホールド処理（第ｎノードが持つ２つの子ノードのそれぞれの推定ノード相対床高さ偏差を更新せずにホールドする処理）が行われる。そのホールド処理では、図４８のフローチャート中の式で示す如く、第ｉノードおよび第ｊノードのそれぞれの推定ノード相対床高さ偏差Zi_rel_estm、Zj_rel_estmの値が、前回制御周期での値Zi_rel_estm_p、Zj_rel_estm_pに維持される。
【０４２０】
次に、図４５のＳ６４００で、２個の子ノードのモードが「すべてリセット」と判断すると、Ｓ６４０８において、２つの子ノードを持つノード用グループ内全リセット処理（第ｎノードが持つ２つの子ノードのそれぞれの推定ノード相対床高さ偏差をリセットする処理）が行なわれる。そのリセット処理では、図４７のフローチャート中の式に従って第ｉノードおよび第ｊノードのそれぞれの推定ノード相対床高さ偏差Zi_rel_estm、Zj_rel_estmが０に徐々に近づいていくように更新される。なお、同式中のΔT、Testmの意味は、前記式３５のものと同じである。
【０４２１】
このリセット処理は、より一般的に言えば、Zi_rel_estm，Zj_rel_estmを、それらの重み付き平均値が０という条件、すなわちWi＊Zi_rel_estm＋Wj＊Zj_rel_estm＝0を満足しつつ、それぞれZi_rel_estm_p，Zj_rel_estm_pよりも０に近い値に決定するという処理である。なお、このリセット処理は、一次遅れ要素を含むため、ノード相対床高さ偏差が完全に０に戻るためには無限時間を要する。そこで、本出願人が先に提案した有限整定関数発生器（特開平５−３２４１１５号公報）を用いて、ノード相対床高さ偏差を徐々に０にリセットするようにしてもよい。
【０４２２】
また、図４５のＳ６４００で、２個の子ノードのモードが「その他」である場合、すなわち、「すべて準備完了」、「すべてリセット」のいずれでもない場合（例えば２個の子ノードのモードがすべてホールドモードである場合）には、Ｓ６４０６と同じ処理（図４８を参照）がＳ６４１０で実行される。
【０４２３】
以上のように、第ｎノードの２個の子ノードのモードがすべて準備完了である場合で、且つ、ノード相対床高さ偏差の推定処理の発散が生じる恐れが無い場合にのみ、その２個の子ノードのノード相対床高さ偏差の実質的な推定処理が行なわれる。そして、子ノードのモードがすべて準備完了である場合であっても、推定処理の発散が生じる恐れがある場合には、２個の子ノードの推定ノード相対床高さ偏差の値が、ホールドされる。また、２個の子ノードの実ノード床反力がいずれも０となる状況、すなわち、第ｎノードに属するすべての接地部位１０に実床反力が作用しない状況では、２個の子ノードのノード相対床高さ偏差は、徐々に０になるようにリセットされる。
【０４２４】
次に、図４１のＳ６６の３子ノード床形状推定処理を図４９〜図５５を参照して説明する。図４９は３子ノード用床形状推定処理のサブルーチン処理を示すフローチャート、図５０、図５１はそれぞれ図４９のＳ６６０４、Ｓ６６０８のサブルーチン処理を示すフローチャート、図５２は図４９のＳ６６０６、Ｓ６６１４およびＳ６６１８のサブルーチン処理を示すフローチャート、図５３、図５４はそれぞれ図４９のＳ６６１２、Ｓ６６１６のサブルーチン処理を示すフローチャートである。
【０４２５】
３子ノード用床形状推定処理では、図４９のフローチャートに示されるように、Ｓ６６００において、３個の子ノードをもつ第ｎノードのすべての子ノードのモード（図４１のＳ５８で決定されたモード）が判断される。その判断結果は、「すべて準備完了」、「すべてリセット」、「２つの子ノードのみ準備完了」、「１つの子ノードのみホールドで、残りがリセット」、「２つの子ノードのみホールドで、残りがリセット」、および「その他（else）」の６つに分けられる。
【０４２６】
ここで、図４９のフローチャートでは、３子ノード用床形状推定処理をより一般化して説明するために、前記第２実施形態で説明した如く、各中間ノード（第145ノード、第236ノード）に属する接地部位１０のすべてが同時に接地したり離床する場合だけでなく、各中間ノードの接地部位のうちのいずれかが接地し、その他の接地部位が離床するようにロボットの目標歩容を生成する場合についても考慮している。この場合、子ノードが２つの時と違い、３つの子ノードのモードが、「２つの子ノードのみ準備完了」、「１つの子ノードのみホールドで、残りがリセット」、もしくは「２つの子ノードのみホールドで、残りがリセット」の場合も考慮する必要がある。なお、以降の説明では、第ｎノードの３個の子ノードを第ｉノード、第ｊノード、第ｋノードとする。
【０４２７】
ここで、子ノードのモードが「すべて準備完了」と判断すると、Ｓ６６０２において、第ｎノードの子ノードの実ノード床反力の並進力鉛直成分Fi_act_z，Fj_act_z，Fk_act_zの合力Fn_z（＝Fi_act_z＋Fj_act_z＋Fk_act_z）が所定の値Fn_min2より大きいか否かを判断する。Fn_zは、換言すれば、第ｎノードに属する全ての接地部位の実床反力の合力の並進力鉛直成分である。
【０４２８】
Ｓ６６０２の判断結果がＹＥＳである場合には、Ｓ６６０４において、３つの子ノードを持つノード用グループ内全推定処理（第ｎノードが持つ３つの子ノードのそれぞれのノード相対床高さ偏差を実質的に推定する処理）を行なう。この処理では、図５０のフローチャート中に示す式に従って第ｎノードの３つの子ノードである第ｉノード、第ｊノードおよび第ｋノードのそれぞれの推定ノード相対床高さ偏差Zi_rel_estm、Zj_rel_estm、Zk_rel_estmが求められる（更新される）。すなわち、Zi_rel_estmについて代表的に説明すると、Zi_rel_estmの前回制御周期での値Zi_rel_estm_pに、前記Ｓ５４で求めた第ｉノード相対床高さ偏差補正量候補値Zi_inc_candを加えることで、新たな推定第ｉノード相対床高さ偏差Zi_rel_estmが求められる。第ｊノード、第ｋノードについても同様である。
【０４２９】
また、Ｓ６６０２の判断結果がＮＯである場合には、床形状偏差の推定精度が下がり過ぎるので、実質的な推定処理を実行せずに、Ｓ６６０６において、３つの子ノードを持つノード用のグループ内全ホールド処理（第ｎノードが持つ３つの子ノードのそれぞれの推定ノード相対床高さ偏差を更新せずにホールドする処理）が行われる。そのホールド処理では、図５２のフローチャート中の式で示す如く、第ｉノード、第ｊノードおよび第ｋノードのそれぞれの推定ノード相対床高さ偏差Zi_rel_estm、Zj_rel_estm、Zk_rel_estmの値が、前回制御周期での値Zi_rel_estm_p、Zj_rel_estm_p、Zk_rel_estm_pに維持される。
【０４３０】
また図４９のＳ６６００で、３個の子ノードのモードが「すべてリセット」と判断すると、Ｓ６６０８において、３つの子ノードを持つノード用グループ内全リセット処理（第ｎノードが持つ３つの子ノードのそれぞれの推定ノード相対床高さ偏差をリセットする処理）が行われる。そのリセット処理では、図５１のフローチャート中の式に従って第ｉノード、第ｊノードおよび第ｋノードのそれぞれの推定ノード相対床高さ偏差Zi_rel_estm、Zj_rel_estm、Zk_rel_estmが０に徐々に近づいていくように更新される。なお、同式中のΔT、Testmの意味は、前記式３５のものと同じである。
【０４３１】
このリセット処理はより一般的に言えば、Zi_rel_estm，Zj_rel_estm，Zk_rel_estmを、それらの重みつき平均値が０という条件、すなわちWi＊Zi_rel_estm＋Wj＊Zj_rel_estm＋Wk＊Zk_rel_estm＝0を満足しつつ、それぞれZi_rel_estm_p，Zj_rel_estm_p，Zk_rel_estm_pよりも０に近い値に決定するという処理である。なお、Zi_rel_estm_p，Zj_rel_estm_p，Zk_rel_estm_pを０にリセットするために、前記有限整定関数発生器（特開平５−３２４１１５号公報）を用いてもよい。
【０４３２】
また、図４９のＳ６６００で、３個の子ノードのモードが「２つの子ノードのみ準備完了」と判断すると、第ｎノードの子ノードの実ノード床反力の合力（第ｎノードに属するすべての接地部位１０の実床反力の合力）の並進力鉛直成分Fn_zが所定の値Fn_min2より大きいか否かをＳ６６１０で判断する。
【０４３３】
この判断結果がＹＥＳである場合には、Ｓ６６１２において、３つの子ノードを持つノード用グループ内部分推定処理（第ｎノードが持つ３つの子ノードのそれぞれのノード相対床高さ偏差を実質的に推定する処理）が行われる。この処理は、図５３のフローチャートに示す如く実行される。なお、ここでは、第ｉノードが準備完了モードでなく、第ｊノードおよび第ｋノードが準備完了モードであるとする。
【０４３４】
まず、Ｓ６６１２０において第ｉノードのモードが判断される。この判断結果がホールドモードである場合には、Ｓ６６１２２において、新たな第ｉノード相対床高さ偏差補正量候補値Zi_inc_cand'を０に決定し、リセットモードである場合には、Ｓ６６１２４において、図中の式により、新たな第ｉノード相対床高さ偏差補正量候補値Zi_inc_cand'を決定する。Ｓ６６１２４で決定されるZi_inc_cand'は、Zi_rel_estm_pを徐々に０に近づけるための第ｉノード相対床高さ偏差補正量候補値である。なお、Ｓ６６１２４の式中のΔT、Testmの意味は、前記式３５のものと同じである。補足すると、Ｓ６６１２４でZi_inc_cand'を決定するとき、前記有限整定関数発生器（特開平５−３２４１１５号公報）を用いてもよい。
【０４３５】
次いで、Ｓ６６１２６において、図中の式に従って、新たな第ｊノード相対床高さ偏差補正量候補値Zj_inc_cand'および新たな第ｋノード相対床高さ偏差補正量候補値Zk_inc_cand'を決定する。
【０４３６】
すなわち、Wi＊Zi_inc_cand'＋Wj＊Zj_inc_cand'＋Wk＊Zk_inc_cand'＝0という条件（Zi_inc_cand'，Zj_inc_cand'，Zk_inc_cand'の重み付き平均値が０という条件）と、Zj_inc_cand'−Zj_inc_cand＝Zk_inc_cand'−Zk_inc_candという条件を満足するようにZj_inc_cand'，Zk_inc_cand'を決定する。
【０４３７】
次いで、Ｓ６６１２８において、上記の如く決定されたZi_inc_cand'，Zj_inc_cand'，Zk_inc_cand'をそれぞれ第ｉノード、第ｊノード、第ｋノードの相対床高さ偏差の前回制御周期での値Zi_rel_estm_p，Zj_rel_estm_p，Zk_rel_estm_pに加えることで、新たなZi_rel_estm，Zj_rel_estm，Zk_rel_estmが決定される。
【０４３８】
このようにZi_rel_estm，Zj_rel_estm，Zk_rel_estmを決定することで、Wi＊Zi_rel_estm＋Wj＊Zj_rel_estm＋Wk＊Zk_rel_estm＝0を満足しつつ、Zj_rel_estm−Zk_rel_estmがZj_inc_cand−Zk_inc_candに近づくようにZi_rel_estm，Zj_rel_estm，Zk_rel_estmが決定されることとなる。
【０４３９】
また、図４９のＳ６６１０の判断結果がＮＯである場合には、床形状偏差の推定精度が下がり過ぎるので、実質的な推定処理を実行せずに、Ｓ６６１４において、前記Ｓ６６０６と同じ処理（図５２を参照）が実行される。
【０４４０】
また図４９のＳ６６００で、３個の子ノードのモードが「１つの子ノードのみホールドで、残りがリセット」と判断すると、Ｓ６６１６において、１つの子ノードのみホールドで、残りがリセットの場合に対応する処理が実行される。その処理で、図５４のフローチャート中に示す式によって、新たなノード相対床高さ偏差Zi_rel_estm，Zj_rel_estm，Zk_rel_estmが決定される。なお、ここでは、第ｉノードのモードがホールドモードで、第ｊノードおよび第ｋノードのモードがリセットモードであるとする。また、同式中の、ΔT、Testmの意味は、前記式３５のものと同じである。
【０４４１】
図５４の処理は、より一般的には、Zi_rel_estmを０に決定し、Zj_rel_estm，Zk_rel_estmを、Wi＊Zi_rel_estm＋Wj＊Zj_rel_estm＋＋Wk＊Zk_rel_estm＝０（Zi_rel_estm，Zj_rel_estm，Zk_rel_estmの重み付け平均値が０であるという条件）を満足しつつ、Zj_rel_estm_p，Zk_rel_estm_pよりも０に近い値に決定する処理である。補足すると、第ｊノードおよび第ｋノードのモードが共にリセットモードになる時点までに、それらのノードの重みWj，Wkは０となっているはずである。従って、重みWi，Wj，Wkの和が１であるという条件から、この時点までにWiが１となっており、また、Wi＊Zi_rel_estm＋Wj＊Zj_rel_estm＋Wk*Zk_rel_estm＝０であるから、この時点までに、Zi_rel_estmは０になっている。このため、第ｉノードのZi_rel_estmの値をホールドするということは、その値を０に維持するということである。そこで、図５４の処理では、Zi_rel_estmの値を０に決定するようにしている。
【０４４２】
なお、Zj_rel_estm，Zk_rel_estmを徐々に０にリセットする処理では、前記有限整定関数発生器（特開平５−３２４１１５号公報）を用いてもよい。
【０４４３】
更に図４９のＳ６６００で、子ノードのモードが「２つの子ノードのみホールドで、残りがリセット」と判断すると、Ｓ６６１８において、２つの子ノードのみホールドで、残りがリセットの場合の処理が行われる。その処理では、図５５のフローチャート中の式に従って、新たなノード相対床高さ偏差Zi_rel_estm，Zj_rel_estm，Zk_rel_estmが決定される。なお、ここでは、第ｉノードおよび第ｊノードのモードが共にホールドモードで、第ｋノードのモードがリセットモードであるとする。また、同式中のΔT、Testmの意味は、前記式３５のものと同じである。
【０４４４】
図５５の処理は、より一般的には、Zi_rel_estm，Zj_rel_estmをそれぞれ前回制御周期での値にホールドし、Zk_rel_estmを、Wi＊Zi_rel_estm＋Wj＊Zj_rel_estm＋Wk＊Zk_rel_estm＝０（Zi_rel_estm，Zj_rel_estm，Zk_rel_estmの重み付け平均値が０であるという条件）を満足しつつ、Zk_rel_estm_pよりも０に近い値に決定する処理である。補足すると、第ｋノードがリセットモードになる時点までに、Wkは０となっている。
【０４４５】
なお、Zk_rel_estmを徐々に０にリセットする処理では、前記有限整定関数発生器（特開平５−３２４１１５号公報）を用いてもよい。
【０４４６】
更に図４９のＳ６６２０で「その他」と判断した場合（例えば３個の子ノードのモードがすべてホールドモードである場合）には、Ｓ６６２０において、前記Ｓ６６０６と同じ処理（図５２を参照）が実行される。
【０４４７】
以上のように、第ｎノードの３個の子ノードのモードのうち、２つ以上の子ノードのモードが準備完了である場合で、且つ、ノード相対床高さ偏差の推定処理の発散が生じる恐れが無い場合にのみ、その２個以上の子ノードのノード相対床高さ偏差の実質的な推定処理が行なわれる。そして、２つ以上の子ノードのモードが準備完了である場合であっても、推定処理の発散が生じる恐れがある場合には、３個の子ノードの推定ノード相対床高さ偏差の値が、ホールドされる。
【０４４８】
以上のごとく、図４１のＳ６０〜Ｓ６６では、推定第nノード相対床高さ偏差Zn_rel_estm(n=1,2,…,最終ノード番号)が決定される。なお、根ノードの相対床高さ偏差は０である。
【０４４９】
図４１のフローチャートの処理では、最後に、Ｓ７２において、第nノード(n=1,2,…,最終葉ノード番号)のすべての先祖ノードの推定相対床高さ偏差と推定第nノード相対床高さ偏差との和を求め、求めた和を推定第n接地部位床高さ偏差（推定第ｎ床高さ偏差）Zfn_estmとする。
【０４５０】
以上が、図４０における床高さ偏差の推定処理のサブルーチン処理である。
【０４５１】
次に図３５の説明に戻って、上述のように床形状推定器１３０で逐次求められた推定第n床高さ偏差Zfn_estm(n=1,2,…,最終葉ノード番号)は、加算器１３２により、各接地部位１０の修正目標接地部位位置姿勢に加算され、これにより、床形状偏差補償入り修正目標接地部位位置姿勢が求められる。そして、この床形状偏差補償入り修正目標接地部位位置姿勢が修正目標接地部位位置姿勢の代わりに、前記機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈへ入力される。
【０４５２】
以上の説明した床形状推定器１３０および加算器１３２を含む制御装置５０の処理を該制御装置５０の制御周期毎に繰り返し実行することにより、推定第n床高さ偏差Zfn_estmが実際の第n床高さ偏差に収束する。また、各目標接地部位位置姿勢は、その接地部位に対応する推定第n床高さ偏差Zfn_estmに応じて修正され、実際の第n床高さ偏差が実床反力に及ぼす影響を吸収する。
【０４５３】
尚、すべての第ｎ接地部位（n=1,2,…,最終葉ノード番号）が接地しているならば、第ｎ接地部位の実際の接地面上における目標第ｎ接地部位床反力中心点Ｑｎに相当する点は、実第ｎ床接点Ｄn_actに一致するはずである。従って、前記瞬間第nノード相対床高さ偏差Zn_rel'は、変動しないはずである。しかし、実際にはロボット１の機械系や制御系の振動および電気的なノイズにより、前記床形状推定器１３０の前記した計算アルゴリズムによって得られた瞬間第nノード相対床高さ偏差Zn_rel'は、激しく変動する。従って、瞬間第nノード相対床高さ偏差をそのまま推定第nノード相対床高さ偏差として用いて、これを基に推定第n床高さ偏差Zfn_estmを求め（すなわち第nノード(n=1,2,…,最終葉ノード番号)のすべての先祖ノードの瞬間相対床高さ偏差と瞬間第nノード相対床高さ偏差との和をもって推定第n床高さ偏差Zfn_estmとし）、さらに図３５に示すように、推定第n床高さ偏差Zfn_estmを修正目標接地部位位置姿勢に加算したものを機構変形補償入り修正目標接地部位位置姿勢算出部１１４ｈへ入力させると、ロボット１の接地部位１０は発振するかあるいは激しく振動する。
【０４５４】
ところで、前記２つの子ノードを持つノード用グループ内全推定処理、３つの子ノードを持つノード用グループ内全推定処理、３つの子ノードを持つノード用グループ内部分推定処理では、瞬間第nノード相対床高さ偏差Zn_rel'から推定第nノード相対床高さ偏差Zn_rel_estmまでの伝達関数は、１次遅れのローパスフィルタになる。即ち、推定第nノード相対床高さ偏差Zn_rel_estmは瞬間第nノード相対床高さ偏差Zn_rel'にローパスフィルタを通したものとなる。従って、推定第nノード相対床高さ偏差Zn_rel_estmを基に求めた推定第n床高さ偏差Zfn_estmを図３５に示すように修正目標接地部位位置姿勢に加算しても、各接地部位１０の発振や振動を生じ難くなる。これにより、各接地部位１０の発振や振動を防止することができる。
【０４５５】
ちなみに、本実施形態では、上記ローパスフィルタの時定数はTestmである。なお、本実施形態で説明した床形状推定器１３０の処理以外にも、瞬間第nノード相対床高さ偏差Zn_rel'から推定第nノード相対床高さ偏差Zn_rel_estmまでの伝達関数がローパスフィルタになるように構成することは可能である。
【０４５６】
次いで、この実施の形態における床形状推定器１３０の特徴を説明する。
【０４５７】
ロボット１の姿勢安定化制御のために必要な各ノード補償モーメントを発生させようと、各ノード補償角を変化させ、最終的な目標接地部位位置姿勢である機構変形補償入り修正目標接地部位位置姿勢を変更させても、各接地部位１０が実際に接地している限り、第ｎ接地部位１０の実際の接地面上における目標第ｎ接地部位床反力中心点Ｑｎに相当する点は、実第ｎ床接点Ｄnactに一致したまま動かない。その代わりに、各脚のコンプライアンス機構４２などが変形したり、上体２４の位置姿勢が変化する。即ち、機構変形補償入り修正目標接地部位位置姿勢を変更させた分を打ち消すように、コンプライアンス機構４２などが変形したり、上体２４の位置姿勢が変化する。
【０４５８】
本実施形態における床形状推定器１３０は、機構変形補償入り修正目標接地部位位置姿勢から推定第n床高さ偏差Zfn_estm(n=1,2,…,最終葉ノード番号)を算出するまでに、実床反力を基に機構コンプライアンスモデル１３４によって算出されたコンプライアンス機構４２などの変形量と上体姿勢傾斜偏差θberrとによる推定第n床高さ偏差Zfn_estmへの影響を打ち消しているので、上体姿勢傾斜偏差が生じたり、機構変形補償入り修正目標接地部位位置姿勢が変動しても、推定第n床高さ偏差Zfn_estmはその影響を受けない。
【０４５９】
これは、床形状の推定処理が、ロボット１のコンプライアンス制御や姿勢制御から干渉を受けないことを意味する。従って、推定第n床高さ偏差Zfn_estmを図３５に示すように修正目標接地部位位置姿勢に加算しても、ロボット１の制御系の安定余裕（発振のし難さ）はほとんど低下しない。すなわち、コンプライアンス制御、姿勢制御、および床形状の推定処理と、床形状偏差の推定値を用いたロボット１の目標歩容の補正動作とを同時に行なっても、各制御・処理が干渉し合って発振することはほとんどない。すなわち、簡単に言えば、床形状偏差の複数のパラメータを同時に推定しながら、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作（接地部位の位置姿勢の修正動作）も同時に実行することができる。さらには、姿勢制御のための脚の補償動作（接地部位の位置姿勢の修正動作）も同時に実行することができる。
【０４６０】
また、各接地部位１０の推定第n床高さ偏差Zfn_estmを図３５に示すように修正目標接地部位位置姿勢に加算すると、実際に第n床高さ偏差が存在していても、その影響を打ち消すことができるので、実全床反力は、定常的にはロボット１が想定床を移動している場合と同一になる。しかも、推定第n床高さ偏差Zfn_estmの推定と、推定第n床高さ偏差Zfn_estmを用いた修正目標接地部位位置姿勢の修正とは制御周期毎に逐次実行されるので、床形状が途中で変化しても、変化後の第n床高さ偏差を推定し、その推定値に基づいて床形状の変化の影響を打ち消すことができる。ただし、床形状推定器１３０には前述したようにローパスフィルタが含まれるので、各接地部位１０が接地した直後や床形状が途中で変化したときには、過渡的には、実全床反力は、床形状偏差の影響を受ける。しかし、その後、時定数Testmでその影響が減衰する。
【０４６１】
また、以下の特徴もある。
１）床形状偏差が大きくても制御が破綻し難い。これは、階層型コンプライアンス制御は微小な補償角に限定した近似を行っていないからである。
２）床形状偏差の推定精度が高い。
３）ロボットの姿勢安定化制御との干渉による発振が生じにくいので、推定処理の時定数を短くすることができる。それによって、床形状偏差が床反力に及ぼす影響を打ち消すための脚の補償動作の応答性を高めることができるので、床形状偏差が床反力に及ぼす過渡的な影響が短時間で解消される。
【０４６２】
ロボットの移動時の１歩前の着地期（ある接地部位１０が接地状態となる時期）での推定第n床高さ偏差と、当該１歩前の着地期の次の着地期での推定第n床高さ偏差との関係に相関がない場合には、本実施形態に示したように推定第n床高さ偏差を強制的に徐々に０に収束させるのが良い。しかし、例えば、想定床に考慮されていない実床面のうねりがゆったりとしていることが分かっているならば、前記関係にある程度の相関があると考えられる。この場合には、現在の着地期における推定第n床高さ偏差を、強制的に徐々に１歩前の着地期における推定第n床高さ偏差に１より小さい正の定数を乗じた値に収束させるようにしても良い。
【０４６３】
さらに、１歩前の着地期での推定第n床高さ偏差だけでなく、複数歩前の着地期での推定第n床高さ偏差も用いて現在の着地期における推定第ｎ床高さ偏差の収束目標値を決定しても良い。また、現在や１歩前や複数歩前の着地期における他のノードの推定第nノード床高さ偏差および推定第nノード床傾斜偏差も用いて収束目標値を決定しても良い。
【０４６４】
本実施形態（第３実施形態）は上記の如く構成したので、従来技術では困難であった多くの接地部位を持つロボットにおいても、床形状の推定、具体的には、各接地部位に対応する床高さ偏差を同時に、換言すれば複合的に、精度良く推定することができる。さらに、ロボットの接地部位をその姿勢を制御可能に設け、床形状の推定処理において床高さ偏差だけでなく、床傾斜偏差も推定する場合には、床傾斜偏差も含めて同時に、精度良く推定することができる。
【０４６５】
さらには、床形状の推定結果を基に目標接地部位位置姿勢軌道を修正することにより、床形状が想定していた形状と異なっていても、その影響を吸収して望み通りの床反力を発生させることができる。
【０４６６】
特に、床形状偏差の推定値による補正がない階層型コンプライアンス制御で除去しきれなかった実床反力の制御目標値からの定常偏差を可能な限り零に近づけることができる。換言すれば床形状の偏差に起因する床反力の定常偏差を解消することができる。
【０４６７】
また、床形状の推定精度が低下する恐れがある状況あるいはその推定値が発散する恐れがある状況では床形状の推定を中断するように構成したので、前記した作用効果に加え、推定値が不適切なものとなることがない。
【０４６８】
尚、本実施形態に係るロボットの床形状推定装置１３０は前記した手法で床形状を推定すれば足り、推定値に基づいて運動を補正することは必須ではない。
【０４６９】
［第４実施形態］
次に、本発明の第４実施形態に係る移動ロボットの制御装置（特に床形状推定装置）を説明する。第４実施形態においては、それを簡単に説明すれば、前記第３実施形態の図４０に示した床形状推定器１３０のブロック線図を等価変換したものである。すなわち、直前に推定した床形状の推定値（前回制御周期での推定値などの該推定値の過去値）と、最終的に修正され決定された目標歩容（詳しくはコンプライアンス動作を含む機構変形補償入り修正目標接地部位位置姿勢）（もしくは実関節変位）と、実上体姿勢傾斜偏差θberrとを基に、各ノードの床反力を推定し、その推定値と各ノードの実床反力との差を基に、この差を０に近づけるように床形状の推定値の補正量候補値を決定し、床形状の推定が発散しないと判断されるならば、床形状の推定値の補正量候補値を基に、直前に推定した床形状の推定値を補正するようにした。この処理は、本出願人が先に提案した特開平１０−２７７９６９号公報において、２足移動ロボットの各脚の足平の床反力中心点まわりの想定されるモーメントと実モーメントとの差が０に近づくように各足平に係る床傾斜を推定する手段、ならびに、全床反力中心点まわりの想定されるモーメントと実モーメントとの差が０に近づくように両脚間の干渉角を推定する手段を発展させた階層型の床形状推定手段に相当するものである。
【０４７０】
このように本実施形態は、床形状推定器１３０の処理のみが、第３実施形態と相違するものであり、床形状推定器１３０以外の処理については説明を省略する。
【０４７１】
図５７は、本実施形態における床形状推定器１３０の機能的手段を示すブロック図である。この図５７を参照して本実施形態における床形状推定器１３０を以下に説明する。
【０４７２】
先ず、各接地部位１０の機構変形補償入り修正目標第ｎ接地部位位置姿勢(n=1,2,…,最終葉ノード番号)と、各接地部位１０の目標接地部位位置から見た該接地部位１０の目標第ｎ接地部位床反力中心点と、前記姿勢傾斜偏差θberrとを基に、機構変形補償入り修正目標第ｎ接地部位位置姿勢を、目標全床反力中心点を回転中心としてθberrだけ回転移動させてなる、回転後機構変形補償入り修正目標第ｎ接地部位位置姿勢(n=1,2,…,最終葉ノード番号)を求める。求められた回転後機構変形補償入り修正目標第ｎ接地部位位置姿勢(n=1,2,…,最終葉ノード番号)から、目標歩容上での想定第ｎ床接地位置Dn(n=1,2,…,最終葉ノード番号)を減算し、第ｎ接地部位干渉高さZn_intを求める。それを床形状推定器１３０に備えた機構コンプライアンスモデル（逆モデル）へ入力することによって、各接地部位１０の床反力の推定値である推定第ｎ接地部位床反力を求める。
【０４７３】
なお、ここでの機構コンプライアンスモデルは、各接地部位干渉高さの互いの相対関係に応じて推定接地部位床反力の互いの相対関係を求めるものであり、絶対値に意味をもたない。また、第ｎ接地部位の実床反力Fn_actの検出値がある閾値Fn_min3より小さい場合には、上記機構コンプライアンスモデルにおいても第ｎ接地部位が接地していないものと想定する。
【０４７４】
さらに、第ｎ接地部位(n=1,2,…,最終葉ノード番号)の実床反力Fn_actの検出値から、前記推定第ｎ接地部位床反力を減算し、第ｎ接地部位床反力推定誤差Ffn_estm_errを求める。この第ｎ接地部位床反力推定誤差Ffn_estm_errは、力で表現されているため、換算値Cn（例えば、バネ定数の逆数のようなもの）によって高さ偏差に換算し、それを第ｎ接地部位床高さ偏差補正量候補値Zfn_inc_candとして得る。なお、換算値Cnは、対角行列とは限らない。
【０４７５】
次いで、求められた第ｎ接地部位床高さ偏差補正量候補値Zfn_inc_candから、前記した「階層相対化処理」によって、階層相対化することで、第ｎノード相対床高さ偏差補正量候補値Zn_inc_cand(n=1,2,…,最終ノード番号)を求める。次に、床高さ偏差の推定処理のサブルーチン処理によって、第ｎノード相対床高さ偏差補正量候補値Zn_inc_candから、推定床高さ偏差（推定第ｎ床高さ偏差）Zfn_estm(n=1,2,…,最終葉ノード番号)を求める。ここでの床高さ偏差の推定処理のサブルーチン処理は、図４１のＳ５６〜Ｓ７２までの処理と同一である。
【０４７６】
なお、特開平１０−２７７９６９号公報に示したように、接地部位として姿勢を制御可能な足平を持ち、接地部位における床傾斜偏差を推定することができるロボットの場合には、図４１のＳ７０に関して説明した如く、接地部位床傾斜偏差推定処理を行なうのが望ましい。
【０４７７】
ここで、第３実施形態と第４実施形態（本実施形態）とを比較する。第４実施形態は、簡単に言えば、第３実施形態の床形状推定器１３０のブロック線図を等価変換したものになっている。したがって、第４実施形態の作用効果は、第３実施形態の作用効果と同じである。また、第３実施形態と同様、床形状の推定値に基づいて運動を補正することは必須ではない。
【０４７８】
尚、第３および第４実施形態において、床反力センサとして、分布圧センサを用いても良い。６軸力センサ３４などの力センサよりも分布圧センサの方が実床反力による接地部位の変形をより細緻に推定することができるので、床形状推定の精度を向上させることができる。
【０４７９】
さらには、本出願人による特許第３０３５０５１号公報の図１に示す如く、接地部位に接触センサを複数個配置（例えば接地部位の四隅に配置）し、その出力信号に基づいて接地部位がどちらの方向に浮きやすいかを判定しても良い。そして、接地していない接触センサが増々浮く方向での床形状の推定を中断したり、あるいは、全ての接触センサが浮いているときにも床形状の推定を中断するようにしてもよい。
【０４８０】
［第５実施形態］
前記第１〜第４実施形態では、子ノードを持つノードの、該子ノードの総数が、３個以下の場合を例にとって説明したが、子ノードの数が４つ以上の場合にも、重み付き平均の概念を持ち込むことによって、以下に示すようにモーメントおよび傾斜角の概念を拡張できる。
【０４８１】
目標値、実際値、偏差のいずれに関しても概念を拡張できるので、ここでは、目標値、実際値、偏差の区別をせず、一般的な接地部位床反力の組Fn(n=1,2,...)に対するモーメントを、以下のように拡張して、定義する。
【０４８２】
第ｎ接地部位の床反力の組Fn(n=1,2,...,最終葉ノード番号)を階層相対化して、第ｎノード相対床反力Fn_rel(n=1,2,...,最終ノード番号）を求める。
【０４８３】
第ｎノードの子ノードの数をrとする。
【０４８４】
第ｎノードの第ｊ番目の子ノードの識別番号がaj (j=1,2,…,r)であるとする。
【０４８５】
第ｎノードの第ｊ番目の子ノードの重みが、Wj (j=1,2,…,r)であるとする。
【０４８６】
第ｎノードの第ｊ番目の子ノードの重みを第ｊ要素として持つ列ベクトルをUWnとする。
【０４８７】
すなわち、下式３８のようにUWnを定義する。ここで「T」は、転置を意味し、行ベクトルの転置によって列ベクトルを表現することとする。
【０４８８】
UWn = (Wa1,Wa2,…,War)T      …式３８
ベクトルUWnに直交する（すなわちベクトルUWnとの内積が０となる）r−1個の互いに独立なベクトルをR(1),R(2),…,R(r−1)とする。R(j) (j=1,2,…,r−1)は、r行１列の列ベクトルである。なお、R(j) (j=1,2,…,r−1)は、演算の容易さと制御精度を考慮すると、互いに直交することが望ましい。R(j)を第ｊ列(j=1,2,…,r−1)とする行列をHnとする。Hnは、r行r−1列の行列である。
【０４８９】
第ｎノードの第ｊ番目の子ノードの相対床反力Faj_relを第ｊ要素(j=1,2,…,r)とする列ベクトルを第ｎグループ相対床反力ベクトルFn_rel_cとする。
すなわち、次式３９により、Fn_rel_cを定義する。
【０４９０】
Fn_rel_c = (Fa1_rel, Fa2_rel,…,Far_rel)T        …式３９
Fn_rel_cは、r行１列の列ベクトルである。
【０４９１】
次式４０を満足する列ベクトルMn_expを第ｎノード拡張床反力モーメントと呼ぶ。
【０４９２】
Fn_rel_c = Hn＊Mn_exp                …式４０
Mn_expは、r−1行１列の列ベクトルである。
【０４９３】
以上のように、接地部位床反力の組Fn(n=1,2,…)に対し、第ｎノード拡張床反力モーメントMn_expを定義する。
【０４９４】
すなわち、接地部位床反力Fn(n=1,2,…)を基に、階層化したノード相対床反力Fn_rel(n=1,2,...)を求め、さらに第ｎノードのすべての子ノードaj (j=a1,a2,…,ar)の相対床反力を要素とするベクトル(Fa1_rel, Fa2_rel,…,Far_rel)Tを、第ｎノードのすべての子ノードの重みを要素とするベクトル(Wa1,Wa2,…,War)Tに直交するある所定の互いに独立なベクトルR(j) (j=1,2,…,r−1)の線形結合で表したときの線形結合の係数を要素とするベクトルを、第ｎノード拡張床反力モーメントMn_expと呼ぶ。
【０４９５】
なお、第ｎノード拡張床反力モーメントMn_expの第ｊ番目要素（第ｊ行目要素）を第ｎノード拡張床反力モーメント第j成分と呼ぶ。
【０４９６】
また、実接地部位床反力の組Fn(n=1,2,…)に対する第ｎノード拡張床反力モーメントを実第ｎノード拡張床反力モーメントMn_exp_actと呼ぶ。
【０４９７】
また、目標接地部位床反力の組Fn(n=1,2,…)に対する第ｎノード拡張床反力モーメントを目標第ｎノード拡張床反力モーメントMn_exp_refと呼ぶ。目標第ｎノード拡張床反力モーメントは、通常０ベクトルに設定される。
【０４９８】
また、接地部位床反力偏差の組Fn(n=1,2,…)に対する第ｎノード拡張床反力モーメントを第ｎノード拡張床反力モーメント偏差Mn_exp_errと呼ぶ。
【０４９９】
同様に、一般的な接地部位高さの組Zn(n=1,2,...)に対する傾斜角を、以下のように拡張して、定義する。
【０５００】
接地部位高さの組Zn(n=1,2,…,最終葉ノード番号)を階層相対化して、第ｎノード相対高さZn_rel(n=1,2,…,最終ノード番号)を求める。
【０５０１】
第ｎノードの第ｊ番目の子ノードの相対高さZaj_relを第ｊ要素(j=1,2,…,r)とする列ベクトルを第ｎノード相対高さベクトルZn_rel_cとする。
すなわち、次式４１によりZn_rel_cを定義する。
【０５０２】
Zn_rel_c = (Za1_rel, Za2_rel,…,Zar_rel)T          …式４１
次式４２を満足する列ベクトルθn_expを第ｎノード拡張床反力傾斜角と呼ぶ。
【０５０３】
Zn_rel_c = Hn＊θn_exp                …式４２
θn_expは、r−1行１列の列ベクトルである。
【０５０４】
すなわち、接地部位高さZn(n=1,2,…)を基に、階層化したノード相対高さZn_rel(n=1,2,…)を求め、さらに第ｎノードのすべての子ノードaj (j=1,2,…,r)の相対高さを要素とするベクトル(Za1_rel, Za2_rel,…,Zar_rel)Tを、第ｎノードのすべての子ノードの重みを要素とするベクトル(Wa1,Wa2,…,War)Tに直交する前記所定の互いに独立なベクトルR(j) (j=1,2,…,r−1)の線形結合で表したときの線形結合の係数を要素とするベクトルを、第ｎノード拡張傾斜角θnと呼ぶ。
【０５０５】
上記のように定義された拡張床反力モーメントと拡張傾斜角とを用いることにより、第aノードの子ノードの数が４つ以上の場合にも、コンプライアンス制御および床形状推定の処理は、ほとんどそのまま拡張される。以下にその例を第５の実施形態として説明する。なお、第５実施形態は、ロボットの制御装置の機能的構成は、前記図２に示したものと同様とする。また、この場合、階層型コンプライアンス動作決定部は、補償全床反力モーメント分配器、補償角決定部、および床形状推定器以外の機能的構成は、前記図３５を参照して説明したものと同様でよい。
【０５０６】
［補償全床反力モーメント分配器の処理の拡張について］
例えば、階層型コンプライアンス動作決定部の中の補償全床反力モーメント分配器の処理は、次のように拡張される。
【０５０７】
ロボットの姿勢を復元させるために目標第ｎノード拡張床反力モーメントMn_exp_rel（通常０）に付加的に発生すべき拡張床反力モーメントを第ｎノード補償拡張床反力モーメントMn_exp_dmdと呼ぶ。
【０５０８】
第ｎノード補償拡張床反力モーメントMn_exp_dmdと第ｎノード補償床反力モーメントMn_dmdの間には、次式４３の関係がある。ただし、Cn_mechは、各目標ノード床反力中心点とロボットの機構のコンプライアンス特性とによって決定される所定の係数行列（２行r−1列の行列）である。
【０５０９】
Mn_dmd＝Cn_mech ＊ Mn_exp_dmd            …式４３
一方、前記式１０を一般化すると次式４４になる。
【０５１０】
Mdmd＝ΣMn_dmd                          …式４４
なお、Σは、全ての第ｎノードについての総和を意味する。
【０５１１】
よって、補償全床反力モーメント分配器では、式４３と式４４を満足するように、第ｎノード補償拡張床反力モーメントMn_exp_dmd（n=1,2,…）および第ｎノード補償床反力モーメントMn_dmdを決定すれば良い。
【０５１２】
［補償角決定部（θn決定部）の処理の拡張について］
第５実施形態における各第ｎノードの補償角決定部の処理は、例えば第３実施形態のθ145決定部（図２５および図３３を参照）の処理において、M145actを求めるまでの処理を、各接地部位の実床反力を基に前記の実第ｎノード拡張床反力モーメントMn_exp_actを求める処理に置き換え、145をｎに置き換え、図３３中の補償フィルタ、ローパスフィルタおよびゲインKnの次元を第ｎノードの子ノード数から１を減じた数に拡張すれば良い。上記の置き換えを行なった処理によって、第ｎノードの補償角θnを求めることができる。
【０５１３】
［拡張床反力モーメントを用いた床形状推定ついて］
以下のように、床形状推定器の処理の一部に拡張床反力モーメントの概念を用いた手法を組み込んでも良い。
【０５１４】
すなわち、前記第４実施形態で説明した床形状推定器の図５６に示した処理の代わりに、図５７のブロック図で示す処理を用いて、床形状偏差を推定する。
【０５１５】
図５７を参照して、この処理を説明すると、まず、前記図５６で説明した如く算出される推定第ｎ接地部位床反力と、第ｎ接地部位の実床反力との差（各ノード毎の差の組）から、拡張モーメントの定義にしたがって、第ｎ接地部位床反力推定誤差Ffn_estm_err (n=1,2,...)に対応する第ｎノード拡張床反力モーメント推定誤差Mn_exp_estm_errを求める。なお、第ｎノード拡張床反力モーメント推定誤差を決定する上で必要となる前記ベクトルR(j)は、各ノードの重みを基に決定される。
【０５１６】
次いで、次式４５のように、所定の係数マトリックスKn_cmplを第ｎノード拡張床反力モーメント推定誤差Mn_exp_estm_errに乗算することにより、第ｎノード拡張傾斜角の補正量の候補値である第ｎノード拡張傾斜角補正量候補値θn_inc_candを求める。
【０５１７】
θn_inc_cand = Kn_cmpl＊Mn_exp_estm_err      …式４５
なお、Kn_cmplは、対角行列とは限らない。
【０５１８】
次いで、ベクトルR(j) (j=1,2,…r−1)に対する係数をθn_inc_candの第ｊ要素として、ベクトルR(j)を線形結合したベクトルを求め、これをもって、第ｎノード相対床高さ偏差の補正量の候補値である第ｎノード相対床高さ偏差補正量候補値Zn_inc_candとする。
【０５１９】
すなわち、次式４６によって、Zn_inc_candを求める。
【０５２０】
Zn_inc_cand = Hn＊θn_inc_cand                    …式４６
以上のごとく、第ｎノード床反力推定誤差Fn_estm_errから第ｎノード相対床高さ偏差補正量候補値Zn_inc_candを求めるまでの処理を図５６のものから図５７のものに置き換えればよい。
【０５２１】
上記以外の図５７の演算処理は、図５６の演算処理と同様である。
【０５２２】
また、第５実施形態における床高さ偏差の推定処理のサブルーチン処理では、第ｎノードの子ノード数が４以上の場合の処理が必要となる。この処理は、前記３子ノード用床形状推定処理（図４１を参照）を拡張し、該３子ノード用床形状推定処理と同様に、いずれかの子ノードjに対応する接地部位が浮いて発散してしまいそうな場合には、推定第jノード相対床高さ偏差Zj_rel_estmを前回値Zj_rel_estm_pに維持する（第ｊノード相対床高さ偏差補正量候補値Zj_inc_candを０とする）。また、それ以外の子ノードの推定ノード相対床高さ偏差Zk_rel_estmを前回推定第kノード相対床高さ偏差Zk_rel_estm_pとZn_inc_candとの和にある所定のバイアス値ｃを加えた値に近づくように決定する。ただし、バイアス値ｃは、第ｎノードのすべての子ノードの推定ノード相対床高さ偏差の重み付き平均が０になるように決定される。
【０５２３】
以上のごとく、床形状の推定処理を、４個以上の子ノードを持つノードがある場合にも、拡張することができる。ただし、このような処理を行なうよりは、４個以上の子ノードを持たないように、階層構造を決定した方が、演算処理は容易となる。
【０５２４】
［第６実施形態］
次に、本発明の第６実施形態に係る移動ロボットの制御装置を図５８〜図６５を参照して説明する。図５８および図５９は、第６実施形態に係る脚式移動ロボット５１（本実施形態では２足移動ロボットとする）が後述する膝関節５６の部位を床に付けた状況（ひざまずいた状況）を側面示で示している。なお、図５８ではロボット５１を線状のリンクの連結体として模式化して示している。
【０５２５】
本実施形態に係わる発明目的は、膝、肘、胴体など、ロボットの脚や腕の先端部以外の部分が床あるいは床に固定（設置）された対象物（床の延長物と考えられる対象物）と接触する状態において、該対象物を含む床から受ける反力（外力）を操作して、ロボットの姿勢を安定に制御することにある。
【０５２６】
より具体的には、ロボットがひざまずいた状態やいすに座った状態における姿勢制御を可能とすることにある。
【０５２７】
従来の公知の人間型ロボットには、例えば膝には、床反力を検出するセンサが備えられておらず、膝に作用する床反力を制御することはできなかった。例えば従来の人間型ロボットでは、図５８および図５９のようにロボットをひざまずかせた状態では、左右の膝に作用する床反力は、床の凹凸に依存（支配）され、また、床の凹凸に応じて、脚の関節角を調節することが行われていなかった。このため、床の凹凸によって、ロボットの床との接地部位が揺れるなど、その接地部位の接地状態が不安定なものとなる場合もあった。そして、このような状態では、ロボットの動作や作業を行おうとするとロボットの姿勢が不安定にある場合があった。
【０５２８】
また、例えばロボットがひざまずいた状態から立ち上がる場合に、実際の床の傾きが目標歩容で想定していた傾きと異なっていた場合には、ロボットの重心位置が適切に制御されず、ロボットの上体の姿勢が傾いたまま立ち上がろうとしてしまうために、該ロボットが転倒してしまう恐れもあった。
【０５２９】
また、ロボットが椅子に座ったまま、その姿勢を安定に保ちつつ動作や作業を行う場合において、椅子から立ち上がる時、あるいは椅子に座る時に、ロボットの姿勢を適切な姿勢に復元できるようにするために、ロボットの各脚の足平に作用する床反力のみならず、椅子からロボットの臀部に作用する反力も適切に制御できることが望ましい。
【０５３０】
なお、概念的には、椅子などのように床に固定設置された対象物も床の一部と考えれば、前記反力も広義の床反力であると言える。
【０５３１】
第６実施形態は、以上のごとき従来方式の問題点に鑑み、前記問題点を解消し、移動ロボットの膝、肘、胴体、臀部など、脚や腕の先端部以外の部分が、床あるいは床の延長物と考えられる対象物と接触して反力を受ける状態において、前記した階層型コンプライアンスの制御を使用しつつ、ロボットの脚や腕の先端部に作用する反力のみならず、脚や腕の先端部以外の部分に作用する反力をも適切に制御し、移動ロボットの姿勢を安定に保つことを可能としたものである。
【０５３２】
以下に、図５８および図５９に示すロボット（２足移動ロボット）５１を例にとって、第６実施形態をより具体的に説明する。
【０５３３】
まず、ロボット５１の機構的構成を説明すると、このロボット５１では、上体（基体）５３の下端部から２つの脚（リンク機構）５２（一方の脚は図示を省略している）が延設されている。また、上体５３の上部から２つの腕（リンク機構）５４（一方の腕は図示を省略している）が延設されている。各脚５２は、上体５３側の基端部（上体５３との連結箇所）と、中間部と、先端部とにそれぞれ電動モータなどのアクチュエータにより動作する股関節５５、膝関節５６、足首関節５７を備え、足首関節５７を介して各脚５２の先端部に足平５８が連結されている。同様に、各腕５４は、上体５３側の基端部（上体５３との連結箇所）と、中間部と、先端部とにそれぞれ電動モータなどのアクチュエータにより動作する肩関節５９、肘関節６０、手首関節６１を備え、手首関節６１を介して手６２が各腕５４の先端部に連結されている。この例では、各脚２の各関節５５、５６、５７はそれぞれ、例えば３自由度、１自由度、２自由度をもつ関節となっており、足平５８は上体５３に対して６自由度を有する。また、各腕５４の肩関節５９、肘関節６０、手首関節６１はそれぞれの自由度が、上体５３に対して手６２が６自由度以上の自由度を持つように構成されている。
【０５３４】
なお、ロボット５１の上体５３には、図示を省略するが、前記第１実施形態などと同様の制御装置５０が搭載されている。また、上体５３の上端には、頭部６３も設けられている。
【０５３５】
このロボット５１では、図５８および図５９のようにロボット５１をひざまずかせた状態（特定姿勢状態）では、各脚５２の足平５８と各膝関節５６の部位（より詳しくは、膝関節５６の近傍で膝関節５６と足首関節５７とを連結するリンク（脛リンク）の表面部。以下、単に膝という）と、各腕５４の手６２とが接地部位となる。そして、本実施形態では、図５９に示すように、接地部位である膝に床反力センサ９０（荷重センサ）を備えている。この床反力センサ９０の本体（センサ部）９２とスポンジ等の柔軟な部材（弾性体）９４とから構成され、本体９２を膝（脚のリンク）に固定し、その本体９２の外側を柔軟な部材（弾性体）９４によって覆っている。なお、膝におけるコンプライアンス制御の精度を良くするために、膝を柔軟な部材９４によって覆うだけでなく、その柔軟な部材９４の表面（接地面）を丸く凸面状に形成することが望ましい。こうすることにより、ロボット５１の目標運動の修正動作と床反力との関係の非線形性が小さくなるので、コンプラインス制御の制御特性が良くなる。なお、図示は省略するが、足平５８と足首関節５７とは６軸力センサなどの床反力センサとコンプラインス機構とを介して連結されている。同様に手６２と手首関節６１とは図示を省略する６軸力センサなどの床反力センサとコンプラインス機構とを介して連結されている。それらの連結構成は、公知のものでよい。
【０５３６】
膝関節の接地部位の床反力センサ９０としては、以下のいずれでも良い。
１）接地部位の床への接触面（接地面）に垂直な方向の並進力だけを検出するセンサ
２）接地部位の床への接触面（接地面）に垂直な方向の並進力だけでなく該接触面に垂直な方向以外の方向の並進力も検出するセンサ
３）分布圧センサ
４）並進力以外にモーメントも検出するセンサ（例えば６軸力センサ）
また、荷重を直接的に検出するものでなくとも、前記柔軟な部材９４の変形（ひずみ）を検出する変位センサを用いても良い。
【０５３７】
本実施形態においては、床反力センサ９０として、上記２）のセンサを用いることとする。コンプライアンス制御の精度をより一層高めるためは、床反力センサ９０として、上記３）または４）のセンサを用いれば良い。なお、膝の構造としては、本出願人が先に提案した特開２０００−６２７６０号公報に示すごとく、ばねを備えた保護パッド（膝の保護のためのパッド）に床反力センサを追加したものにしても良い。
【０５３８】
本実施形態では、上記のようにひざまずいたロボット５１に対して、図６１に示す如く階層構造を設定している。すなわち、接地部位としての右足平５８、左足平５８、右膝、左膝、右手６２、左手６２をそれぞれ葉ノードである第１ノード、第２ノード、第３ノード、第４ノード、第５ノード、第６ノードに対応させる。また、第１ノードと第２ノードとを子ノードとして持つ第１２ノード、第３ノードと第４ノードを子ノードとして持つ第３４ノード、第５ノードと第６ノードを子ノードとして持つ第５６ノード、並びに第１２ノードと第３４ノードを子ノードとして持つ第１２３４ノードを中間ノードとして設定した。さらに、第１２３４ノードと第５６ノードを子ノードとして持つ第１２３４５６ノードを根ノードとして設定した。また、各ノードには、前記第１実施形態などと同様の方針によって、図示の如く、目標床反力中心点Qn(n=1,2,3,4,5,6,12,34,56,1234,123456)が対応づけられる。なお、根ノードである第123456ノードの目標床反力中心点Q123456は、目標全床反力中心点Ｐと同一である。
【０５３９】
本実施形態では、制御装置５０は、前記図２に示したものと同様の機能的構成を有するものとされている。
【０５４０】
この場合、本実施形態における歩容生成装置１００が出力する目標歩容のうちの目標運動の目標接地部位軌道は、各手６２の目標位置姿勢軌道と各足平５８の目標位置姿勢軌道と各膝の目標位置軌道とから構成される。この場合、歩容生成装置１００は、想定した床面上で足平５８と手６２と膝が歩容に対する要求通りに接地するように、目標足平位置姿勢（目標第１、２接地部位位置姿勢）と目標手位置姿勢（目標第５、６接地部位位置姿勢）と目標膝位置（目標第３、４接地部位位置）を優先的に決定し、次いで目標ＺＭＰ（目標全床反力中心点）を各接地部位の目標接地点（あるいは目標接地線もしくは目標接地面）を含む最小凸多角形である支持多角形内に決定し、次いで、目標上体位置姿勢を、ロボット５１の動力学モデルを使用して、目標足平位置姿勢と目標手位置姿勢と目標膝位置と目標ＺＭＰとを満足するように決定する。
【０５４１】
なお、頭部６３が上体５３に対して可動である場合には、その頭部６３の目標位置姿勢軌道が目標運動に含まれる。
【０５４２】
また、図２のロボット幾何学モデル（逆キネマティクス演算部）１１０の演算処理は、本実施形態では、後述するように、第１〜第３実施形態と異なる。
【０５４３】
また、本実施形態では、階層型コンプライアンス動作決定部１１４は、その一部の処理手法が前記第１〜第３実施形態で説明したものと相違する。図６２は、本実施形態の階層型コンプラインス動作決定部１１４の機能的構成を示している。この機能的構成の処理において、第１〜第３実施形態と相違する主な点を説明すると、補償全床反力モーメント分配器１１４ａは、各中間ノードおよび根ノードのノード補償床反力モーメントだけでなく、葉ノードのうちの第ｎノード(n=1,2,5,6）のノード補償床反力モーメントMn_dmd、すなわち、目標第ｎ接地部位床反力中心点(n=1,2,5,6）まわりの要求床反力モーメント（姿勢復元用の要求モーメント）も決定して出力する。これは、ロボット５１では、接地部位としての足平５８および手６２にその目標床反力中心点まわりにモーメント発生できるからである。なお、これらの要求モーメントを含めたノード補償床反力モーメントMn_dmdの決定手法は、前記第１および第２実施形態で説明した手法と同じでよい。また、本実施形態では、補償全床反力モーメント分配器１１４ａは、ノード補償床反力モーメントをそれぞれに対応する各ノードの目標床反力中心点まわりに発生させることによって修正される、目標第ｎ接地部位床反力中心点(n=1,2,5,6)まわりの修正目標第ｎ接地部位床反力モーメントMn_refmdfd(n=1,2,5,6)を、修正目標接地部位床反力の並進力成分Fn_refmdfd(n=1,2,3,4,5,6)と共に決定して出力する。これらは、機構変形補償量算出部１１４ｎで機構変形補償量を算出するために使用される。
【０５４４】
また、補償角決定部（図６２の仮想線で囲んだ部分）は、本実施形態では、各中間ノードおよび根ノードの補償角だけでなく、葉ノードのうちの第ｎノード(n=1,2,5,6）のノード補償角θnも決定して出力する。これらの第ｎノード補償角θn(n=1,2,5,6）を決定するθn決定部は、特開平１０−２７７９６９号公報に示した足平補償角決定部と同様の処理によって、第ｎノード補償角θn(n=1,2,5,6）を決定する。なお、これらのノード以外の各中間ノードおよび根ノードの補償角θn(n=12,34,56,1234,123456)の決定手法は、前記第１および第２実施形態で説明した手法で同じでよい。
【０５４５】
また、本実施形態における修正目標接地部位位置姿勢算出部１１４ｇは、前記第１および第２実施形態で説明した手法によって、各第ｎノード補償角θn(n=12,34,56,1234,123456)を基にしたノードの回転操作を合成することによって、目標接地部位位置を修正し、さらに、第ｎノード補償角θn(n=1,2,5,6)を基に、第ｎ接地部位(n=1,2,5,6)の目標姿勢を修正し、これによって、各接地部位の修正目標接地部位位置姿勢を求める。
【０５４６】
また、本実施形態では、床形状推定器１３０は、前記第３実施形態で説明した手法と同じ手法で推定第ｎ床高さ偏差Zfn_estm(n=1,2,3,4,5,6)を求めることに加えて、第ｎ接地部位の接地部位床傾斜偏差θfn(n=1,2,5,6)を推定する。この接地部位床傾斜偏差θfnの推定は、特開平１０−２７７９６９号公報に示した床形状推定器の足平床傾斜を推定する処理と同様の処理によって行なわれる。
【０５４７】
本実施形態の階層型コンプラインス動作決定部１１４の、上記した以外の機能的構成の処理手法は、第３実施形態のものと同じである。
【０５４８】
以上説明した相違点を除けば、本実施形態における制御装置５０の演算処理は、第１〜第３実施形態と同様である。
【０５４９】
次に、本実施形態における前記ロボット幾何学モデル１１０の処理に関して詳説する。
【０５５０】
本実施形態のロボット５１においては、上体５３から膝に至るまでの関節の自由度の数が３しかないので（股関節５５に３自由度あるだけなので）、厳密に上体位置姿勢と足平位置姿勢と左右の膝の水平位置と左右の膝の高さの和とをそれらの目標に維持したまま、左右の膝の高さの差だけを変えることはできない。言い換えると、強引に左右の膝の高さの差だけを変えようとすると、接地部位（すなわち膝および足平）と床との間でこじりあるいは滑りが生じてしまう。
【０５５１】
同様に、厳密に上体位置姿勢と両膝の位置と両足平の水平位置と左右の足平の高さの和とをそれらの目標に維持したまま、左右の足平の高さの差だけを変えることはできない。言い換えると、強引に左右の足平の高さの差だけを変えようとすると、接地部位（すなわち膝および足平）と床との間でこじりあるいは滑りが生じてしまう。
【０５５２】
同様に、厳密に上体位置姿勢と足平位置姿勢と左右の膝の水平位置と左右の膝の高さの差とをそれらの目標に維持したまま、左右の膝の高さの和だけを変えることはできない。言い換えると、強引に左右の膝の高さの和だけを変えようとすると、接地部位（すなわち膝および足平）と床との間でこじりあるいは滑りが生じることはなくとも、上体位置が目標位置からずれる。その結果、ロボット５１の全体の重心位置と慣性力とが目標値からずれ、ロボット５１の安定性が低下する。
【０５５３】
すなわち、いずれにせよ、階層型コンプライアンス動作決定部１１４において決定された機構変形補償付き修正目標接地部位位置姿勢と目標上体位置姿勢とを同時に厳密に満足することはできないので、ロボット５１の図５８および図５９の姿勢状態において、該ロボット５１の足平５８もしくは膝と床との間でこじりあるいは滑りが生じたり、ロボット５１の全体の重心位置と慣性力とが目標値からずれ、ロボット５１の安定性が低下したりする。なお、接地部位のうちの手６２は、上体５３に対して６自由度以上の自由度を持つので、腕５４の手６２以外の部位で床に接地するような姿勢をロボット５１に採らせない限り、手６２と床との間でこじりなどの滑りが発生することは無い。
【０５５４】
そこで、本実施形態においては、ロボット５１の接地部位のうちの膝および足平５８と床との間で極力こじりあるいは滑りが生じず、かつ上体５３の重心位置（特に水平位置）が極力変わらないようにしつつ、左右の膝の高さの差の変化などに応じて上体５３の姿勢、あるいは姿勢と位置とを修正することとする。
【０５５５】
図６３（ａ），（ｂ）に、その上体５３の姿勢の修正動作の例を示す。なお、図６３（ｃ）については後述する。これらの図６３（ａ），（ｂ），（ｃ）では、腕５４と頭部６３は図示を省略している。
【０５５６】
上体５３の姿勢の修正動作の１つは、図６３（ａ）に示すようにロボット５１がひざまずいた状態から、コンプライアンス制御による左右の膝の高さの差の変化に応じて、図６３（ｂ）に示すように、おおよそ上体５３の重心Ｇを通る体幹軸を回転軸として、上体５３を回転させる（矢印y1のように回転させる）動作である。
【０５５７】
また、図６３のようなロボット５１の膝付き姿勢においては、左右の足平５８，５８の高さの差の変化によって、左右の膝の高さの差はおおよそ左右の足平５８，５８の高さの差の変化の半分だけ変化する。そこで、コンプラインス制御による左右の足平５８，５８の高さの差の変化に応じて、左右の膝の高さの差の変化に応じた上体５３の姿勢修正と同様の動作を、左右の足平の高さの差の変化の場合に較べて半分の量だけ行なうようにする。
【０５５８】
さらに、このように上体５３の姿勢を修正するとき、接地部位（膝および足平５８）と床との間で極力こじりあるいは滑りが生じず、かつ上体５３の重心Ｇ位置（特にその水平位置）、あるいはその重心Ｇと全床反力中心点と結ぶ線分の傾きが極力変わらないようにしつつ、コンプラインアンス制御による左右の膝の高さの和の変化に応じて上体５３の位置と姿勢とを修正することとする。
【０５５９】
図６４に、左右の膝の高さの和の変化に応じて上体５３の位置と姿勢とを修正する動作を視覚的に示す。すなわち、点線で示すロボット５１の姿勢から、実線で示すロボット５１の姿勢のように、コンプライアンス制御により両膝を下げる動作に伴い、上体５３の下端部（あるいは腰部）を矢印y3で示す如く前方側にずらし、上体５３の傾きを矢印y2で示す如く後ろに（上体５３の起立方向に）ずらすようにする。すなわち、上体５３の重心Ｇ位置（あるいは上体５３の所定の代表点の位置）、特にその水平位置を維持しつつ上体５３を後ろに傾けるようにする。あるいは、該重心Ｇと目標全床反力中心点Ｐとを結ぶ線分の傾きを維持しつつ上体５３を後ろに傾けるようにする。また、コンプラインス制御により両膝を上げる動作に伴い、上記と逆に、上体５３の下端部（あるいは腰部）を後ろにずらし、上体の傾きを前にずらすようにする。すなわち、上体重心Ｇ位置（あるいは上体の所定の代表点の位置）、特にその水平位置を維持しつつ上体５３を前に傾けるようにする。あるいは、該重心Ｇと目標全床反力中心点Ｐとを結ぶ線分の傾きを維持しつつ上体５３を前に傾けるようにする。なお、図６４中Q1''，Q3''は、それぞれ上記のように上体５３の位置姿勢を修正した後の足平５８の目標床反力中心点、膝の目標床反力中心点を示しており、この例では、Q1''は修正前の足平５８の目標床反力中心点Q1と同じである。
【０５６０】
以上が、上体５３の姿勢や位置の修正動作の概要である。
【０５６１】
本実施形態におけるロボット幾何学モデル（逆キネマティクス演算部）１１０の処理機能を図６５のブロック図に示す。
【０５６２】
逆キネマティクス演算部１１０では、まず、機構変形補償入り修正目標第n接地部位位置姿勢（n=1,2,…,6）と歩容生成装置１００が決定した各足平５８および膝の目標接地部位位置姿勢とを基に、歩容生成装置１００が決定した左右の膝の目標位置に対する、左右の膝の高さの差の修正量Zkneediffmdfdおよび左右の膝の高さの和の修正量Zkneesummdfdと、歩容生成装置１００が決定した左右の足平の目標位置に対する、左右の足平５８，５８の高さの差の修正量Zfootdiffmdfdを求める。
【０５６３】
具体的には、以下の式４７、４８、４９によって、左右の膝の高さの差の修正量Zkneediffmdfd、左右の膝の高さの和の修正量Zkneesummdfd、左右の足平の高さの差の修正量Zfootdiffmdfdを求める。
【０５６４】
Zkneediffmdfd
＝  機構変形補償入り修正目標第４接地部位位置
−目標第４接地部位位置
−機構変形補償入り修正目標第３接地部位位置
+ 目標第３接地部位位置
…式４７
Zkneesummdfd
＝  機構変形補償入り修正目標第４接地部位位置
−目標第４接地部位位置
+ 機構変形補償入り修正目標第３接地部位位置
−目標第３接地部位位置
…式４８
Zfootdiffmdfd
＝  機構変形補償入り修正目標第２接地部位位置
−目標第２接地部位位置
−機構変形補償入り修正目標第１接地部位位置
+ 目標第１接地部位位置
…式４９
なお、式４７〜４９の「部位位置」はより正確には、その「部位位置」の高さ成分（鉛直方向成分）である。
【０５６５】
次いで、左右の膝の高さの差の修正量Zkneediffmdfd、左右の膝の高さの和の修正量Zkneesummdfdおよび左右の足平の高さの差の修正量Zfootdiffmdfdを基に、上体位置姿勢修正量を求める。
【０５６６】
上体位置姿勢修正量は、膝高さ差用上体位置修正量Xbkneediffmdfd、膝高さ差用上体姿勢修正量θbkneediffmdfd、膝高さ和用上体位置修正量Xbkneesummdfd、膝高さ和用上体姿勢修正量θbkneesummdfdおよび足平高さ差用上体位置修正量Xbfootdiffmdfd、足平高さ差用上体姿勢修正量θbfootdiffmdfdから成る。具体的にこれらの値は次のように求められる。
【０５６７】
まず、左右の膝の高さの差の修正量Zkneediffmdfdに応じて、これによって接地部位（膝および足平５８）と床との間でこじりあるいは滑りが生じないための、膝高さ差用上体位置修正量Xbkneediffmdfdと膝高さ差用上体姿勢修正量θbkneediffmdfdとを、左右の膝の高さの差の修正量Zkneediffmdfdとその瞬間（現在時刻）におけるロボット５１の目標姿勢（目標運動）とを基に幾何学演算によって求める。
【０５６８】
より具体的には、例えば、次式５０によって、Xbkneediffmdfdとθbkneediffmdfdとを求める。ただし、KxkneediffとKthkneediffとは、ロボット５１の目標姿勢に応じた比例係数である。
【０５６９】
Xbkneediffmdfd＝Kxkneediff＊Zkneediffmdfd
θbkneediffmdfd＝Kthkneediff＊Zkneediffmdfd
…式５０
これ以外にも、あらかじめ、いくつかの代表的なロボット５１の目標姿勢に対して、左右の膝の高さの差の修正量（あるいは第３４ノード補償角θ34）と上体位置姿勢の修正量との関係を求めておいて、これをマップあるいは関数として記憶し、これと左右の膝の高さの差の修正量Zkneediffmdfdを基にXbkneediffmdfdおよびθbkneediffmdfdを求めても良い。なお、本実施形態では、左右の膝の高さの差の修正量に対して、上体５３をその体幹軸まわり回転させて上体姿勢を修正するので、膝高さ差用上体位置修正量Xbkneediffmdfdは０で良い。
【０５７０】
次いで、上記と同様に、左右の足平５８，５８の高さの差の修正量Zfootdiffmdfdに応じて、これによって接地部位（膝および足平５８）と床との間でこじりあるいは滑りが生じないための、足平高さ差用上体位置修正量Xbfootdiffmdfdと足平高さ差用上体姿勢修正量θbfootdiffmdfdとを、左右の足平の高さの差の修正量Zfootdiffmdfdとその瞬間（現在時刻）におけるロボット５１の目標姿勢（目標運動）とを基に幾何学演算によって求める。
【０５７１】
より具体的には、例えば、次式５１によって、Xbfootdiffmdfdとθbfootdiffmdfdとを求める。ただし、KxfootdiffとKthfootdiffとは、ロボット５１の目標姿勢に応じた比例係数である。前述したように足平５８の高さ差の影響は膝の高さの差の影響のほぼ半分であることから、KxfootdiffとKthfootdiffとは、それぞれ、KxkneediffとKthkneediffの２分の１とする。
【０５７２】
Xbfootdiffmdfd＝Kxfootdiff＊Zfootdiffmdfd
θbfootdiffmdfd＝Kthfootdiff＊Zfootdiffmdfd
…式５１
なお、本実施形態では、左右の足平５８，５８の高さの差の修正量に対して、上体５３をその体幹軸まわり回転させて上体姿勢を修正するので、足平高さ差用上体位置修正量Xbfootdiffmdfdは０で良い。
【０５７３】
次いで、上記と同様に、左右の膝の高さの和の修正量Zsummdfdに応じて、これによって接地部位（膝および足平）と床との間でこじりあるいは滑りが生じないための、膝高さ和用上体位置修正量Xbsummdfdと膝高さ和用上体姿勢修正量θbsummdfdとを、左右の膝の高さの和の修正量Zsummdfdとその瞬間（現在時刻）におけるロボット５１の目標姿勢を基に幾何学演算によって求める。
【０５７４】
これ以外にも、あらかじめ、いくつかの代表的な目標姿勢に対して、左右の膝の高さの和の修正量と上体姿勢の修正量の関係を求めておいて、これをマップあるいは関数として記憶し、これと左右の膝の高さの和の修正量Zsummdfdとを基にXbsummdfdとθbsummdfdとを求めても良い。
【０５７５】
なお、次のように、上体位置姿勢の修正量を決定してもよい。すなわち、左右の膝の高さの差を、その差を生じさせる第34ノード補償角θ34に換算し、左右の膝の高さの和を、その和を生じさせる第1234ノード補償角θ1234に換算し、左右の足平の高さの差を、その差を生じさせる第12ノード補償角θ12に換算する。そして、これらの換算された補償角を基に、幾何学演算によって上体位置姿勢修正量を決定するようにしても良い。あるいは、あらかじめ、いくつかの代表的なロボット５１の目標姿勢に対して、換算された補償角と上体位置姿勢修正量との関係を求めておいて、これをマップあるいは関数として記憶し、これと換算された補償角とを基に、上体位置姿勢修正量を決定するようにしても良い。
【０５７６】
次いで、目標上体位置姿勢を上体位置姿勢修正量だけ移動（回転移動と平行移動）して、こじり補正入り目標上体位置姿勢を求める。具体的には、体幹軸（あるいは所定の回転軸（ほぼ鉛直面内の回転軸））まわりに目標上体位置姿勢を膝高さ差用上体姿勢修正量と足平高さ差用上体姿勢修正量との和(θbkneediffmdfd+θbfootdiffmdfd)だけ回転移動し、さらにこれを左右軸まわりに膝高さ和用上体姿勢修正量θbfootsummdfdだけ回転移動し、さらにこれをロボット５１の前後方向で膝高さ差用上体位置修正量と膝高さ和用上体位置修正量と足平高さ差用上体位置修正量の和(Xbkneediffmdfd＋Xbkneesummdfd＋Xbfootdiffmdfd)だけ平行移動することによって、こじり補正入り目標上体位置姿勢を求める。
【０５７７】
次いで、こじり補正入り目標上体位置姿勢と機構変形補償入り目標第n接地部位位置姿勢(n=1,2,…,6)とを基に、ロボット５１の関節変位指令を求める。
【０５７８】
以上のごとく逆キネマティクス演算部１１０の処理が実行される。
【０５７９】
すなわち、第６実施形態は、ロボット５１の接地部位位置姿勢の修正動作のために、幾何学的に自由度が不足している場合には、階層型コンプライアンス動作によって目標接地部位位置姿勢を修正するだけでなく、上体位置の点（上体の代表点）あるいは上体を含む複数部位の重み付き平均位置（この場合の重みは各部位の質量比となっていることが望ましい）の点（全体重心など）、特にその点の水平位置を保持するように、あるいは、その点と目標全床反力中心点（目標ＺＭＰ）とを結ぶ線の角度を目標歩容における角度にできるだけ保持するように、目標上体位置姿勢も修正するようにした。
【０５８０】
また、別の言い方をすれば、ロボット５１の全体の姿勢制御の安定性を高めるために、目標全床反力中心点（目標ＺＭＰ）まわりに発生する実床反力モーメントの制御性を優先し、ある所定の接地部位Ａ（ここでは膝）の相対高さまたは傾斜角度の操作量、あるいはある所定の接地部位Ａを子孫ノードとして持つノードの相対高さまたは傾斜角度の操作量としての補償高さおよび補償角のうちの少なくともいずれか一方を決定する手段と、前記補償高さおよび補償角のうちの少なくともいずれか一方に応じて、上体位置あるいは上体を含む複数部位の前記重み付き平均位置を概ね目標歩容での位置に保持しつつ、上体の姿勢および位置のうちの少なくともいずれか一方の修正量を決定する手段と、前記所定の接地部位Ａを除く所定の接地部位Ｂ（ここでは足平）の位置および姿勢のうちの少なくともいずれか一方と、前記修正量によって修正した上体位置姿勢とに基づいて、関節変位を決定する逆キネマティクス演算手段を備えるようにした。
【０５８１】
なお、第６実施形態では、ロボット５１の上体５３の体幹軸まわりに上体姿勢を回転させる代わりに、図６４（ｃ）のように、鉛直軸を回転軸として、ロボット５１の腰（上体５３の下端部）を中心に上体５３を回転させるようにしても良い。ただし、この場合には、上体５３の重心Ｇ位置の左右方向へのずれが大きくなる。あるいは、上体５３の体幹軸と鉛直軸との中間的な軸を回転軸として上体姿勢を回転させるようにしても良い。あるいは上体５３の位置と姿勢とを両膝の高さの差の修正量や両足平５８，５８の高さの差の修正量に応じて同時に修正しても良い。また、上体５３の重心Ｇ位置が変わらないようにする代わりに、ロボット５１の全体重心位置あるいは上体５３の代表点位置が変わらないように上体５３の位置姿勢を修正しても良い。
【０５８２】
いずれにせよ、コンプライアンス動作などによるロボット５１の接地部位位置姿勢の修正動作のために、幾何学的に自由度が不足している場合に、接地部位のこじりなどの滑りが生じないように、上体５３の位置および姿勢のうちの少なくともいずれか一方を目標歩容の位置姿勢から修正するようにすればよい。また、上体５３の位置および姿勢のうちの少なくともいずれか一方を修正する代わりに、上体５３以外の所定の部位の位置および姿勢のうちの少なくともいずれか一方を修正するようにしても良い。
【０５８３】
［第７実施形態］
次いで、本発明の第７実施形態に係る移動ロボットの制御装置を説明する。
【０５８４】
第６実施形態においては、接地部位のこじりなどの滑りを防止し、かつロボット５１の全体重心位置と慣性力とができるだけずれないようにするために、上体位置姿勢を修正するようにした。本実施形態（第７実施形態）においては、接地部位のこじりなどの滑りをある程度までは許容し、優先的にロボット５１の全体重心位置と慣性力とがずれないように、関節変位（股関節５５と膝関節５６の変位）あるいは接地部位位置姿勢を修正するようにした。
【０５８５】
すなわち、本実施形態においては、逆キネマティクス演算部１１０は、第６実施形態で説明した図６５の処理に代えて、図６６のブロック図で示す処理を行なうようにした。この相違点を除けば、本実施形態は、第６実施形態と同じである。
【０５８６】
本実施形態の逆キネマティクス演算部１１０を図６６を参照して説明すると、まず、各機構変形補償入り修正目標第ｎ接地部位位置姿勢(n=1,2,…,6)（詳しくは位置姿勢のうちの位置）と各目標第ｎ接地部位位置姿勢(n=1,2,…,6)（詳しくは位置姿勢のうちの位置）とを基に、前記式４７、式４８および式４９にしたがって左右の膝の高さの差の修正量Zkneediffmdfd、左右の膝の高さの和の修正量Zkneesummdfd、および左右の足平の高さの差の修正量Zfootdiffmdfdを求め、さらに、左右の膝の高さの差を、その差を生じさせる第３４ノード補償角θ34に換算する。同様に、左右の膝の高さの和を、その和を生じさせる第1234ノード補償角θ1234に換算し、左右の足平の高さの差を、その差を生じさせる第12ノード補償角θ12に換算する。
【０５８７】
次いで、換算された各ノード補償角を基に、関節変位修正量を決定する。これは次のように行なわれる。まず、目標歩容の上体位置姿勢を固定し、且つ目標全床反力中心点（目標第123456ノード床反力中心点（図６１の点Ｐ））とロボット５１の全体重心（あるいは、上体重心、あるいは、上体５３を含む複数の部位の重心）とを結ぶ線分の角度が変わらないようにしつつ、関節変位を修正した場合を想定し、この場合に、各接地部位（膝および足平５８）に対応した第ｎノード(n=12,34,1234)の傾斜角度（補償角）が単位摂動量になるための第ｊ関節変位修正量（j=1,2,…,関節の総数）を求め、これを第ｎノード補償角に対する第ｊ関節変位の感度Ln_jとする。
【０５８８】
より一般的には、目標歩容の関節変位を修正した場合に、目標全床反力中心点（目標第123456ノード床反力中心点）とロボット５１の全体重心（あるいは、上体重心、あるいは、上体５３を含む複数の部位の重心）を結ぶ線分と、各接地部位（膝および足平５８）に対応した第ｎノード(n=12,34,1234)の傾斜角度との間の相対角度が単位摂動量になるための第ｊ関節変位修正量（j=1,2,…,関節の総数）を求め、これを第ｎノード補償角に対する第ｊ関節変位の感度Ln_jとする。
【０５８９】
具体的には、Ln_knee_rを第ｎノード補償角(n=12,34,1234)に対する右膝関節変位の感度、Ln_knee_lを第ｎノード補償角に対する左膝関節変位の感度、Ln_hip_rを第ｎノード補償角に対する右股ピッチ関節変位の感度、Ln_hip_lを第ｎノード補償角に対する左股ピッチ関節変位の感度とすると、図５８および図５９に示すロボット５１の膝付き姿勢においては、各感度は次式５２のように設定する。
【０５９０】
L1234_hip_r＝０、  L1234_hip_l＝０、  L1234_knee_r＝１、
L1234_knee_l＝１、  L12_hip_r＝０、  L12_hip_l＝０
L12_knee_r＝a12、  L12_knee_l＝−a12、  L34_hip_r＝a34
L34_hip_l＝−a34、  L34_knee_r＝０、  L34_knee_l＝０
…式５２
ただし、a12、a34は所定の定数である。
【０５９１】
次いで、次式５３〜５６により各関節変位修正量を決定する。ただし、θknee_rは、右膝関節変位修正量、θknee_lは、左膝関節変位修正量、θhip_rは、右股関節変位修正量（より詳しくは、右股関節のピッチ方向の関節変位修正量）、θhip_lは、左股関節変位修正量（より詳しくは、左股関節のピッチ方向の関節変位修正量）とする。
【０５９２】
θknee_r＝L1234_knee_r＊θ1234＋L12_knee_r＊θ12
＋L34_knee_r＊θ34
…式５３
θknee_l＝L1234_knee_l＊θ1234＋L12_knee_l＊θ12
＋L34_knee_l＊θ34
…式５４
θhip_r＝L1234_hip_r＊θ1234＋L12_hip_r＊θ12
＋L34_hip_r＊θ34
…式５５
θhip_l＝L1234_hip_l＊θ1234＋L12_hip_l＊θ12
＋L34_hip_l＊θ34
…式５６
なお、他の関節変位修正量も同様に求めるが、説明を容易にするために、ここでは省略する。
【０５９３】
すなわち、第1234補償角θ1234に対しては、図６７に示すごとく両膝関節５６の変位（角度）をθ1234と同量だけ修正する。また、第12補償角θ12に対しては、右膝関節５６の変位（角度）をθ12に比例して修正しつつ、左膝関節５６の変位（角度）を右膝関節５６の変位の修正量に−１を乗じた量だけ修正する。つまり右膝関節５６の変位（角度）を図６７に示すごとく修正し、左膝関節５６の変位（角度）はその逆に修正する。また、第34補償角θ34に対しては、右股関節５５のピッチ方向の変位（角度）をθ34に比例して修正し、左股関節５５のピッチ方向の変位（角度）を右股関節５５のピッチ方向の変位の修正量に−１を乗じた量だけ修正するようにした。つまり右股関節５５のピッチ方向の変位（角度）を図６８に示すごとく修正し、左股関節５５のピッチ方向の変位（角度）はその逆に修正する。なお、図６７のQ1，Q1''はそれぞれ上記のような関節変位の修正前の足平５８の目標床反力中心点、修正後の足平５８の目標床反力中心点を示している。また、図６８のQ3，Q3''はそれぞれ上記のような関節変位の修正前の膝の目標床反力中心点、修正後の膝の目標床反力中心点を示している。
【０５９４】
このような関節変位の修正により、目標全床反力中心点（目標第123456ノード床反力中心点）とロボット５１の全体重心（あるいは、上体重心、あるいは、上体５３を含む複数の部位の重心）を結ぶ線分と、接地部位（膝および足平５８）に対応した第ｎノード(n=12,34,1234)の傾斜角度との間の相対角度が、前記換算された各ノード補償角θ12，θ34，θ1234だけ変化する。したがって、実床反力が忠実に制御され、ロボット５１の姿勢安定性および接地性が向上する。
【０５９５】
上記の如く関節変位指令（目標関節変位）を直接修正する代わりに、略同一の目標関節変位を生じさせるために優先的に位置姿勢を修正する接地部位（具体的には、足平５８、手６２）の修正後の目標位置姿勢である優先修正目標接地部位位置姿勢を決定するようにし、決定された優先修正目標接地部位位置姿勢を基に逆キネマティクス演算によって関節変位指令を決定するようにしても良い。
【０５９６】
なお、関節の自由度不足のため、目標上体位置姿勢とすべての修正目標接地部位位置姿勢とを満足するように逆キネマティクス演算によって関節変位を決定することはできないので、逆キネマティクス演算を行なうときには、すべての修正目標接地部位位置姿勢のうちの一部の修正目標接地部位位置姿勢を用いることとなる。この一部の修正目標接地部位位置姿勢を前記優先修正目標接地部位位置姿勢と呼ぶ。
【０５９７】
たとえば、図６７の状況においては、第1234ノード補償角θ1234に対しては、膝を中心として目標足平位置を回転移動させた位置を優先修正目標足平位置と決定すれば良い。
【０５９８】
なお、第６実施形態および第７実施形態では、一例として、ロボット５１のひざまずき状況におけるコンプライアンス動作について説明したが、ロボットが椅子に座った状況でのコンプライアンス動作に適応する場合には、次のようにすれば良い。以下に、それを第８実施形態として説明する。
【０５９９】
［第８実施形態］
図６９は本実施形態のロボットの要部の構成を示している。このロボット７１は、臀部７２の底面部の左右にそれぞれ床反力を検出する床反力センサ７３，７３（荷重センサ。例えば６軸力センサ）を備える。なお、左右に床反力センサ７３，７３を設ける代わりに、臀部７２の底面部の左右に加わる力の合力を検出する１つの床反力センサを備えるようにしてもよい。
【０６００】
この場合、また、床反力センサ７３，７３の外側は、図示の如くスポンジ等の柔軟な部材（弾性体）７４によって覆われている。なお、臀部７２の接地部位におけるコンプライアンス制御の精度を良くするために、柔軟な部材によって覆うだけでなく、図示の如く柔軟な部材（弾性体）７４の表面（接地面）を丸く凸面状に形成することが望ましい。特に、ロボット７１がその臀部７２を介して椅子などに座った状態において床反力センサ７３，７３の検知位置（センサ本体の位置）と上記部材７４の凸面の頂点との水平位置を一致させるように、部材７４を設けておくことが望ましい。このようにすることで、ロボット７１の接地部位の位置姿勢の修正動作と床反力との関係の非線形性が小さくなるので、ロボット７１のコンプライアンス制御などの制御特性が良好になる。
【０６０１】
なお、ロボット７１では、臀部７２の左右の側部から、脚（リンク機構）５２，５２が延設されている。脚５２，５２の構造は、例えばその関節を含めて前記第６実施形態のものと同一とされている。従って、脚５２，５２についての第６実施形態と同一の参照符号を用い、説明を省略する。ただし、本実形態では、脚５２の膝に床反力センサを備えていなくてもよい。
【０６０２】
また、図示のように、臀部７２の上側には、胴体（上体）７７が設けられ、この胴体７７の上部の両側部から肩関節７８，７８を介して腕７９，７９が延設されている。なお、腕７９は、前記第６実施形態のロボット５１のものと同一構造でもよい。さらに、胴体７７は、関節８０を介して臀部７２に連結されている。この場合、関節８０は、臀部７２に対して胴体う７７をヨー方向に旋回させる胴体旋回用関節８０ａと、胴体７７を臀部７２に対して前後および左右方向に傾斜させる胴体屈曲用関節８０ｂとから構成されている。ロボット７１に上述のように備えられた各関節は、図示しないアクチュエータにより動作する。また、臀部７２または胴体７７には、図示を省略するが、前記第１実施形態などと同様の制御装置５０が搭載されている。
【０６０３】
本実施形態では、臀部７２を介して椅子などに腰掛けるロボット７１に対して、図７０に示す如く階層構造を設定すればよい。すなわち、接地部位としての右足平５８、左足平５８、臀部７２の底面部の右側部（右側床反力センサ７３の取り付け部位）、臀部７２の底面部の左側部（左側床反力センサ７３の取り付け部位）をそれぞれ葉ノードである第１ノード、第２ノード、第３ノード、第４ノードに対応させる。また、第１ノードと第２ノードとを子ノードとして持つ第１２ノード、第３ノードと第４ノードを子ノードとして持つ第３４ノードを中間ノードとして設定し、第12ノードと第34ノードを子ノードとして持つ第1234ノードを根ノードとして設定した。また、各ノードには、前記第１実施形態などと同様の方針によって、図示の如く、目標床反力中心点Qn(n=1,2,3,4,12,34,1234)が対応づけられる。なお、根ノードである第1234ノードの目標床反力中心点Q1234は、目標全床反力中心点Ｐと同一である。
【０６０４】
本実施形態では、制御装置５０は、前記図２に示したものと同様の機能的構成を有するものとされている。
【０６０５】
この場合、本実施形態における歩容生成装置１００が出力する目標歩容のうちの目標運動の目標接地部位軌道は、各足平５８の目標位置姿勢軌道と臀部７２の目標位置姿勢軌道とから構成される。目標運動のうちの上体位置姿勢軌道は、本実施形態では、胴体７７の位置姿勢軌道を意味する。なお、目標運動には、各腕７９の先端部の位置姿勢軌道も含まれる。また、目標全床反力中心点Ｐは、実際の床面上にはなく、空中の仮想面上にある。
【０６０６】
また、本実施形態では、階層型コンプライアンス動作決定部１１４は、その機能的構成要素は、前記第６実施形態のものと同様である（図６２を参照）。ただし、本実施形態では、補償全床反力モーメント分配器は、図７０に示した階層構造における各中間ノードおよび根ノードのノード補償床反力モーメントと、各足平５８に対応する葉ノードのノード補償床反力モーメントとを決定して出力する。また、補償角決定部は、図７０に示した階層構造における各中間ノードおよび根ノードのノード補償角と、各足平５８に対応する葉ノードのノード補償角とを決定して出力する。この場合、これらのノード補償床反力モーメントおよびノード補償角の基本的な決定手法は、前記第１〜第３実施形態や第６実施形態で説明した手法と同じでよい。また、床形状推定器は、前記第６実施形態の場合と同様に、前記第３実施形態で説明した手法と同じ手法で推定第ｎ床高さ偏差Zfn_estm(n=1,2,3,4)を求めることに加えて、各足平５８の接地部位床傾斜偏差θfn(n=1,2)を推定する。
【０６０７】
また、制御装置５０の機能的構成要素であるロボット幾何学モデル（逆キネマティクス演算部）は、前記第６実施形態と同様の手法によって、基本的には、前記機構変形補償入り修正目標接地部位位置姿勢と目標上体位置姿勢とを基に、臀部７２の接地部位と足平５８（接地部位）とでこじりなどの滑りが発生しないように、それらの接地部位の位置姿勢を修正すると共に、上体（胴体）７７の位置姿勢を修正する。
【０６０８】
以上説明した以外の制御装置５０の処理は、前記第６実施形態と同様でよい。
【０６０９】
次に、以上説明した実施形態に係わるいくつかの変形態様を説明する。
【０６１０】
床反力センサによって直接的に検出できない実ノード床反力は、床反力センサによって直接的に検出される他の実接地部位床反力、加速度センサの検出値、傾斜計の検出値などを用いて、オブザーバにより推定するか、もしくは、簡易的な代数関係によって推定すればよい。例えば、階層型コンプライアンス動作は、目標全床反力中心点まわりに接地部位を回転させる回転型のコンプライアンス動作であるので、コンプライアンス動作を行なっても、ロボットの全体重心の鉛直加速度（あるいは目標全床反力中心点と全体重心とを結ぶ線分の方向の加速度成分）は、ロボットの目標歩容（目標運動）における全体重心の鉛直加速度（あるいは目標全床反力中心点と全体重心とを結ぶ線分の方向の加速度成分）にほぼ一致する。したがって、すべての実接地部位床反力鉛直成分の和は、ロボットの目標歩容（目標運動）における全体重心の鉛直加速度と重力加速度との和にロボットの全体質量を乗じた値にほぼ一致する。
【０６１１】
よって、実第ｎノード床反力が直接的に検出できない場合、まず、第ｎノードを先祖ノードに持たず、かつ、第ｎノードそのものでないすべての葉ノードの実床反力（実接地部位床反力）の和（以降、これを実第ｎノード外床反力と呼ぶ）を求める。
【０６１２】
次いで、次式５７によって、実第ｎノード床反力の推定値である推定第ｎノード床反力を求める。
【０６１３】
推定第ｎノード床反力
＝(目標歩容の全体重心加速度＋重力加速度）＊全体質量
−実第ｎノード外床反力                      …式５７
上体などに加速度センサを備えている場合には、加速度センサの検出値と、目標歩容および実関節変位のうちの少なくともいずれか一方によりロボット全体の重心加速度（以降、推定全体重心加速度と呼ぶ）を推定し、次式５８によって、実第ｎノード床反力の推定値である推定第ｎノード床反力を求めても良い。
【０６１４】
推定第ｎノード床反力
＝（推定全体重心加速度＋重力加速度）＊全体質量
−実第ｎノード外床反力                    …式５８
例えば、前記第６実施形態において、膝の床反力を検知する床反力センサが備わっていない場合、あるいは、膝の床反力を検知する床反力センサが故障した場合には、推定第３４ノード床反力を次式５９によって求める。
【０６１５】
推定第34ノード床反力
＝（目標歩容の全体重心加速度＋重力加速度）＊全体質量
−実第34ノード外床反力
＝目標歩容の全体重心加速度＊全体質量
−（実第12ノード床反力＋実第56ノード床反力）
…式５９
さらに、推定第34ノード床反力を実第34ノード床反力の代わりに用いて、コンプライアンス動作処理によって第1234ノード補償角θ1234を決定し、また床形状推定処理によって推定第1234ノード相対床高さ偏差Z1234rel_estmを推定する。
【０６１６】
第34ノード補償角θ34の決定では、想定した床通りであると仮定して、機構変形補償入り修正目標第３接地部位位置姿勢、機構変形補償入り修正目標第４接地部位位置姿勢と推定第34ノード床反力を基に、実第３ノード床反力の推定値である推定第３ノード床反力と実第４ノード床反力の推定値である推定第４ノード床反力とを求める。
【０６１７】
さらに、推定第３ノード床反力と推定第４ノード床反力を実第３ノード床反力と実第４ノード床反力の代わりに用いて、コンプライアンス動作処理によって第34ノード補償角θ34を決定する。
【０６１８】
ロボットの姿勢の復元要求モーメント（補償全床反力モーメント）を基に、前述の如く復元要求モーメントを加えることと等価になるように目標ノード床反力中心点まわりの修正目標ノード床反力モーメントを決定する代わりに、復元要求モーメント（補償全床反力モーメント）を基に、目標ノード床反力中心点を修正した修正目標ノード床反力中心点を決定しても良い。この場合、目標ノード床反力中心点まわりの目標ノード床反力モーメントは修正しないで０のままとする。
【０６１９】
また、第ｎノード床反力の内で制御できなかった成分（予想される、または検出された制御偏差成分）を補うために、この成分に応じて、第ｎノードの親ノードの目標床反力を修正しても良い。すなわち、実第ｎノード床反力と目標第ｎノード床反力との差の前回制御周期の値あるいは前記差をローパスフィルタに通した値を第ｎノードの目標床反力に付加しても良い。
【０６２０】
床形状の推定処理においては、コンプライアンス制御用のツリー構造と異なるツリー構造を設定してもよい。そのツリー構造は、根ノードと葉ノードとからなる２層のものであってもよい。また、各ノードの重みもコンプライアンス制御用のものと異なるものとしてもよい。ノード床反力が０となるような時期（そのノードに属する全ての接地部位、またはそのノードに対応する接地部位が空中を移動する時期）でそのノードの重みが０であれば良い。
【０６２１】
また、床形状の推定処理においては、前述の修正目標ノード床反力中心点に基づいて、各修正ノード重みを決定し、これを要素とするベクトルを、床形状推定に用いる重みとして用いても良い。
【０６２２】
床形状推定の処理で、各点（各接地部位）の推定相対床高さ偏差を求める際に、実床反力と機構コンプライアンスモデルと姿勢センサの検出値とを用いる代わりに、コンプライアンス機構などの変形量を検出する変形量検出器を備え、その変形量検出器の検出値と姿勢センサの検出値とを用いるようにしてもよい。
【０６２３】
床形状を相対的に推定するだけでなく、グローバルに推定（グローバル座標系での床形状の推定）したい場合には、次のようにしてもよい。
１）ロボットの上体のグローバル座標系での高さ（以下、グローバル高さという）を検知できる場合における床のグローバル高さの推定：
上体などロボットのある所定部位のグローバル高さを検出する検出器（加速度センサを用いた上体高さ推定装置（例えば本願出願人によるＰＣＴ／ＪＰ０３／０５４４８号に記載されている推定装置）あるいは視覚センサ等の外界センサなど）を用いる。推定された上体のグローバル高さと、関節変位（実関節変位あるいは目標関節変位）、実上体姿勢傾斜の検出値および実床反力の検出値を基に、推定床高さ偏差のグローバル座標系での値を求める。従って、根ノードの推定床高さ偏差は、すべての接地部位のグローバルな推定床高さ偏差の重み付き平均値としての意味を持つこととなる。
２）上体のグローバル高さがわからない状況での床のグローバル高さの推定：
ある推定第n床高さ偏差のグローバル値（グループ内相対値ではなくグローバル座標系における偏差）がわかっているならば、それと推定各相対床高さ偏差を基に、すべての点（接地部位）の推定床高さ偏差のグローバル値を求める。ロボットの移動のスタート点での推定第n床高さ偏差のグローバル値がわかっていれば、ロボットの移動を続けることで、逐次新しく接地した接地部位の床高さ偏差のグローバル値を求められるので、これを記憶することにより、ロボットの移動軌跡上での床高さのマップを作成できる。
【０６２４】
また、床形状の推定処理においては、前述の修正目標ノード床反力中心点に基づいて、各修正ノード重み（各ノードの重みの値を修正したもの）を決定し、これを、床形状推定において、実第ｎノード相対床高さ等を定義するために用いる重みとしても良い。また、床形状推定において、実第ｎノード相対床高さ等を定義するために用いる重みは、目標床反力分配器が決定した前述の重みと必ずしも同一のものにする必要はない。
【０６２５】
補足すると、実第ｎノード相対床高さ等を定義するための重みは、目標床反力分配器が決定する重みに必ずしも一致させなくてはならない訳ではないが、一致させるならば、（この場合、自動的に、根ノードの目標床反力中心点は目標全床反力中心点に一致する、）コンプライアンス動作用の第ｎノード補償角（n=1,2,…）に第ｎノード推定床傾斜偏差を付加するだけで、床形状偏差の影響を打ち消すことができるので、床形状偏差の影響を打ち消すための演算量を低減することができる。
【０６２６】
さらに、推定床形状偏差を床形状の地図情報として記憶しておき、次回同一場所を歩行する時に、記憶しておいた床形状の地図情報を基に目標歩容を生成するようにしても良い。
【０６２７】
尚、前述の各実施形態における演算処理機能を示すブロック線図もしくはフローチャートにおける演算処理順序を等価変形をしても良い。
【０６２８】
また、本発明は、移動ロボットに限らず、アクティブサスペンションを持つ車輪型移動体にも応用することが可能である。
【０６２９】
産業上の利用可能性
以上の如く、本発明は、床反力を適切に制御して、高い安定性で円滑な動作を可能とする脚式移動ロボットなどの移動体を提供し得るものとして有用である。
【図面の簡単な説明】
【０６３０】
【図１】本発明の第１実施形態および第２実施形態に係るロボットの全体外観図。
【図２】第１実施形態の制御装置の機能的構成を示すブロック図。
【図３】図３（ａ）〜（ｃ）は、第１実施形態のロボット（４脚ロボット）の動作を説明するための図。
【図４】図４（ａ）〜（ｃ）は、第１実施形態のロボット（４脚ロボット）の動作を説明するための図。
【図５】図５（ａ）〜（ｃ）は第１実施形態における重みの設定例を示すグラフ、図５（ｄ）は第１実施形態におけるＺＭＰ（目標全床反力中心点）の設定例を示すグラフ。
【図６】第１実施形態のノードの階層構造を示す図。
【図７】目標ノード床反力中心点と重みとの関係を示す図。
【図８】目標ノード床反力と重みとの関係を示す図。
【図９】第１実施形態の制御装置のメインルーチン処理を示すフローチャート。
【図１０】第１実施形態のロボット（４脚ロボット）の目標ノード床反力並進力成分を例示する図。
【図１１】第１実施形態のロボット（４脚ロボット）の実ノード床反力並進力成分を例示する図。
【図１２】第１実施形態のロボット（４脚ロボット）の実ノード床反力モーメントを例示する図。
【図１３】第１実施形態のロボット（４脚ロボット）の補償全床反力モーメントを例示する図。
【図１４】第１実施形態のロボット（４脚ロボット）のノード補償床反力モーメントを例示する図。
【図１５】第１実施形態におけるノード補償角による位置修正（コンプライアンス動作）を説明するための図。
【図１６】第１実施形態におけるノード補償角による位置修正（コンプライアンス動作）を説明するための図。
【図１７】第１実施形態における階層型コンプライアンス動作決定部の機能的構成を示すブロック図。
【図１８】第１実施形態における補償全床反力モーメント分配器の機能的構成を示すブロック図。
【図１９】第１実施形態におけるθ1423決定部（補償角決定部）の機能的構成を示すブロック図。
【図２０】第１実施形態におけるθ14決定部（補償角決定部）の機能的構成を示すブロック図。
【図２１】第１実施形態における機構変形補償量算出部の機能的構成を示すブロック図。
【図２２】補償角を決定する処理を示すフローチャート。
【図２３】第１実施形態のロボットに係る階層構造の他の例を説明するための図。
【図２４】第２実施形態のロボット（６脚ロボット）の階層構造を説明するための図。
【図２５】第２実施形態における階層型コンプライアンス動作決定部の機能的構成を示すブロック図。
【図２６】第２実施形態のロボット（６脚ロボット）の目標ノード床反力の並進力成分を例示する図。
【図２７】第２実施形態のロボット（６脚ロボット）の実ノード床反力の並進力成分を例示する図。
【図２８】第２実施形態のロボット（６脚ロボット）の実ノード床反力モーメントを例示する図。
【図２９】図２９（ａ），（ｂ）は第２実施形態におけるノード補償床反力モーメントの設定手法を説明するための図。
【図３０】第２実施形態におけるノード補償角による位置修正（コンプライアンス動作）を説明するための図。
【図３１】第２実施形態におけるノード補償角による位置修正（コンプライアンス動作）を説明するための図。
【図３２】第２実施形態におけるθ145236決定部（補償角決定部）の機能を示すブロック図。
【図３３】第２実施形態におけるθ145決定部（補償角決定部）の機能を示すブロック図。
【図３４】第２実施形態のロボットに係る階層構造の他の例を説明するための図。
【図３５】第３実施形態における階層型コンプライアンス動作決定部の機能的構成を示すブロック図。
【図３６】第３実施形態における制御装置のメインルーチン処理を示すフローチャート。
【図３７】第３実施形態における概念および用語を説明するための図。
【図３８】第３実施形態における概念および用語を説明するための図。
【図３９】第３実施形態における概念および用語を説明するための図。
【図４０】第３実施形態における床形状推定器の機能を示すブロック図。
【図４１】図４０における床高さ偏差推定処理サブルーチンの処理を示すフローチャート。
【図４２】４脚ロボットにおける階層相対化の処理を説明するための図。
【図４３】６脚ロボットにおける階層相対化の処理を説明するための図。
【図４４】床形状推定に係る各ノードの要求モードの設定例を示す図。
【図４５】床形状推定処理を示すフローチャート。
【図４６】床形状推定処理を示すフローチャート。
【図４７】床形状推定処理を示すフローチャート。
【図４８】床形状推定処理を示すフローチャート。
【図４９】床形状推定処理を示すフローチャート。
【図５０】床形状推定処理を示すフローチャート。
【図５１】床形状推定処理を示すフローチャート。
【図５２】床形状推定処理を示すフローチャート。
【図５３】床形状推定処理を示すフローチャート。
【図５４】床形状推定処理を示すフローチャート。
【図５５】床形状推定処理を示すフローチャート。
【図５６】第４実施形態における床形状推定器の機能を示すブロック図。
【図５７】第５実施形態における床形状推定器の機能を示すブロック図。
【図５８】第６実施形態におけるロボット（２足移動ロボット）が膝を付いた状態を示す図。
【図５９】第６実施形態におけるロボット（２足移動ロボット）が膝を付いた状態を示す図。
【図６０】第６実施形態におけるロボットの膝の床反力センサの構成を示す図。
【図６１】第６実施形態におけるノードの階層構造を示す図。
【図６２】第６実施形態における階層型コンプライアンス動作決定部の機能的構成を示すブロック図。
【図６３】図６３（ａ）〜（ｃ）は第６実施形態におけるロボットの上体の姿勢の修正手法を説明するための図。
【図６４】第６実施形態におけるロボットの上体の位置姿勢の修正手法を説明するための図。
【図６５】第６実施形態における逆キネマティクス演算部の機能を示すブロック図。
【図６６】第７実施形態における逆キネマティクス演算部の機能を示すブロック図。
【図６７】第７実施形態におけるロボットの姿勢の修正手法を説明するための図。
【図６８】第７実施形態におけるロボットの姿勢の修正手法を説明するための図。
【図６９】第８実施形態におけるロボットの構成を示す図。
【図７０】第８実施形態におけるノードの階層構造を示す図。
【図１】
0004126063.tif 000002
【図２】
0004126063.tif 000003
0004126063.tif 000004
0004126063.tif 000005
0004126063.tif 000006
【図６】
0004126063.tif 000007
【図７】
0004126063.tif 000008
【図８】
0004126063.tif 000009
【図９】
0004126063.tif 000010
【図１０】
0004126063.tif 000011
【図１１】
0004126063.tif 000012
【図１２】
0004126063.tif 000013
【図１３】
0004126063.tif 000014
【図１４】
0004126063.tif 000015
【図１５】
0004126063.tif 000016
【図１６】
0004126063.tif 000017
【図１７】
0004126063.tif 000018
【図１８】
0004126063.tif 000019
【図１９】
0004126063.tif 000020
【図２０】
0004126063.tif 000021
【図２１】
0004126063.tif 000022
【図２２】
0004126063.tif 000023
【図２３】
0004126063.tif 000024
【図２４】
0004126063.tif 000025
【図２５】
0004126063.tif 000026
【図２６】
0004126063.tif 000027
【図２７】
0004126063.tif 000028
【図２８】
0004126063.tif 000029
0004126063.tif 000030
【図３０】
0004126063.tif 000031
【図３１】
0004126063.tif 000032
【図３２】
0004126063.tif 000033
【図３３】
0004126063.tif 000034
【図３４】
0004126063.tif 000035
【図３５】
0004126063.tif 000036
【図３６】
0004126063.tif 000037
【図３７】
0004126063.tif 000038
【図３８】
0004126063.tif 000039
【図３９】
0004126063.tif 000040
【図４０】
0004126063.tif 000041
【図４１】
0004126063.tif 000042
【図４２】
0004126063.tif 000043
【図４３】
0004126063.tif 000044
【図４４】
0004126063.tif 000045
【図４５】
0004126063.tif 000046
【図４６】
0004126063.tif 000047
【図４７】
0004126063.tif 000048
【図４８】
0004126063.tif 000049
【図４９】
0004126063.tif 000050
【図５０】
0004126063.tif 000051
【図５１】
0004126063.tif 000052
【図５２】
0004126063.tif 000053
【図５３】
0004126063.tif 000054
【図５４】
0004126063.tif 000055
【図５５】
0004126063.tif 000056
【図５６】
0004126063.tif 000057
【図５７】
0004126063.tif 000058
【図５８】
0004126063.tif 000059
【図５９】
0004126063.tif 000060
【図６０】
0004126063.tif 000061
【図６１】
0004126063.tif 000062
【図６２】
0004126063.tif 000063
0004126063.tif 000064
【図６４】
0004126063.tif 000065
【図６５】
0004126063.tif 000066
【図６６】
0004126063.tif 000067
【図６７】
0004126063.tif 000068
【図６８】
0004126063.tif 000069
【図６９】
0004126063.tif 000070
【図７０】
0004126063.tif 000071