JP2012245574A - 移動体の制御装置 - Google Patents

Abstract

【課題】複数の接触対象面から移動体に実際に作用する全接触力を目標値に追従させるように移動体の運動を制御することを、移動体の各接触対象面に対する各接触部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行う。
【解決手段】第ｉ接触対象面から移動体１０１に作用する全接触力の観測値と目標全接触力との偏差をゼロに近づけるための全接触力の要求修正量を移動体１０１の接触面を代表する第ｉ代表接触面の位置及び姿勢のばね性変位量に変換し、このばね性変位量を全ての接触対象面について並べてなる総合ばね性変位量に、総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を乗じることにより移動体１０１の各関節の変位量の修正量を決定し、各関節の目標変位量を修正する。修正後の目標関節変位量に応じて各関節の変位量を制御する。
【選択図】図１

Description

本発明は、脚リンク等の複数の可動リンクを備える移動体の制御装置に関する。

基体に連結された複数の脚リンクの運動（各脚リンクの着床及び離床を繰り返す運動）によって床面上を移動する脚式移動ロボット等の移動体の動作制御では、基本的には、移動体の目標運動に実際の運動を追従させるように各脚リンクの関節が駆動される。

上記目標運動は、一般に、実際の床面を表すものとして設定された想定床面上で動力学的な必要要件（ＺＭＰが支持多角形内に存在する等の要件）を満足するように生成されるものの、目標運動を生成するために使用した想定床面の実際の床面に対する形状誤差等に起因して、移動体の実際の運動が目標運動からずれる場合がある。そして、このような場合には、移動体の姿勢が崩れやすい。

これを防止するための技術として、従来、例えば特許文献１にて本願出願人が提案したコンプライアンス制御の技術が知られている。このコンプライアンス制御の技術では、２足歩行ロボットに実際に作用する床反力を観測しつつ、目標全床反力中心点周りに発生する実際の床反力モーメントを、２足歩行ロボットの上体の姿勢を目標姿勢に復元させるための目標モーメントに追従させるように、各脚リンクの先端部（足平部）の目標位置及び姿勢が修正され、その修正後の目標位置及び姿勢に応じて該脚リンクの関節の変位量が制御される。

特許第３６２９１３３号

脚式移動ロボット等の移動体に種々様々な作業を行なわせるためには、移動体を単に床上で移動させるだけでなく、移動体の動作環境（外界）に存在する複数の接触対象面（例えば床面と壁面）のそれぞれに移動体の脚リンクもしくは腕リンク等の１つ以上の可動リンクを接触させながら、移動体の所要の運動を行なわせることが要求される場合もある。

このような場合には、移動体を単に床上で移動させる場合と異なり、移動体は各接触対象面から移動体に外力が作用することとなる。

そして、移動体の動作を安定して行なう上では、上記のように複数の接触対象面に移動体を接触させる状況においても、各接触対象面から移動体に実際に作用する接触力（反力）を所要の目標接触力に追従させるように移動体の関節の変位量を適宜調整することが望ましいと考えられる。

この場合、各接触対象面から移動体に実際に作用する接触力を制御するために、特許文献１に見られる如きコンプライアンス制御の技術を用いることが考えられる。

しかるに、特許文献１に見られるコンプライアンス制御の技術では、接触対象面として床面から移動体（脚式移動ロボット）に作用する実際の床反力モーメントを、目標モーメントに追従させるための各脚リンクの先端部（足平部）の目標位置及び姿勢の修正は、両脚リンクの先端部の上下方向の位置を互いに逆向きに変化させる修正（以降、本欄では第１の修正という）と、各脚リンクの先端部の姿勢を変化させる修正（以降、本欄では第２の修正という）とを合成したものとされる。

この場合、第１の修正における各脚リンクの先端部の位置の修正量と、第２の修正における各脚リンクの先端部の姿勢の修正量とは各別に決定される。

このため、かかる従来のコンプライアンス制御の技術では、第１の修正に起因して生じる床反力の変化と、第２の修正に起因する床反力の変化とが相互に干渉しやすい。このため、第１の修正と第２の修正とを合成してなる各脚リンクの先端部の位置及び姿勢の修正によって実際に生じる床反力モーメントとの変化が、目標モーメントに対して過不足を生じやすいという不都合がある。

また、その過不足の発生を極力抑制するためには、第１の修正による各脚リンクの先端部の位置の修正量を決定するためのゲイン（第１の修正によって発生させようとする床反力モーメントの目標モーメントに対する度合いを規定するゲイン）と、第２の修正による各脚リンクの先端部の姿勢の修正量を決定するためのゲイン（第２の修正によって発生させようとする床反力モーメントの目標モーメントに対する度合いを規定するゲイン）とを適切に調整しておく必要がある。そして、その調整作業には、一般に多大な工数を必要とするという不都合がある。

本発明にかかる背景に鑑みてなされたものであり、複数の接触対象面に接触する動作を行なう移動体に各接触対象面から実際に作用する全接触力を目標値に追従させるように該移動体の運動を制御することを、該移動体の各接触対象面に対する各接触部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行うことができる移動体の制御装置を提供することを目的とする。

まず、本発明の基礎となる技術事項を、以下に一般化して説明する。

図１に示す如く、基体１０２に複数の可動リンク１０３が連結された移動体１０１を想定する。この移動体１０１の各可動リンク１０３は、複数の関節を備えており、これらの関節の変位によって、空間的な運動が可能である。なお、各関節は、回転型又は直動型の関節である。

そして、移動体１０１のある運動状態において、移動体１０１の可動リンク１０３のうちの、ｍ(i)個（ｍ(i)≧１）の可動リンク１０３（１０３_1〜１０３_m(i)）の先端部が、移動体１０１の動作環境に存在するある１つの接触対象面である第ｉ接触対象面に接触し、これらの可動リンク１０３_1〜１０３_m(i)を介して移動体１０１が第ｉ接触対象面から反力としての接触力を受ける場合と想定する。以降の説明では、可動リンク１０３_1〜１０３_m(i)のそれぞれを総称的に第ｊ可動リンク１０３_ｊ（ｊ＝１，２，…，ｍ(i)）、又は、可動リンク１０３_ｊという。

このとき、第ｉ接触対象面から移動体１０１に作用するトータルの接触力である全接触力（ベクトル）を↑ＦＭt(i)、この↑ＦＭt(i)のうちの並進力ベクトルである全並進接触力を↑Ｆt(i)、↑ＦＭt(i)のうちのモーメントベクトルである全接触力モーメントを↑Ｍti(i)と表記する。

これらの全並進接触力↑Ｆt(i)及び全接触力モーメント↑Ｍt(i)は、それぞれ慣性座標系（移動体１０１の動作環境の床や壁等に対して固定された座標系）で見た３成分の縦ベクトルとして表現されるものとする。そして、第ｉ全接触力↑ＦＭt(i)は、↑Ｆt(i)及び↑Mt(i)の各成分を並べた６成分の縦ベクトル（＝[↑Ｆt(i)，↑Mt(i)]^T）として表現されるものとする。また、全接触力↑ＦＭt(i)の作用点Ｐt(i)は第ｉ接触面上の点とする。

なお、本明細書では、“↑”は、ベクトル（縦ベクトル）を表現する記号として用いる。また、上付き添え字“Ｔ”は、転置を意味する。また、本明細書では、理解の便宜上、並進力、モーメント、位置、姿勢等を表現する慣性座標系としては、例えば移動体１０１の前後方向の水平軸をＸ軸、鉛直方向をＺ軸、これらのＸ軸及びＺ軸に直交する方向（移動体１０１の左右方向）をＹ軸とする３軸直交座標系が用いられるものとする。

また、第ｊ可動リンク１０３_ｊ（ｊ＝１，２，…，Ｎ）に第ｉ接触対象面から作用する接触力（以下、可動リンク接触力という）を↑ＦＭ(i)_j、この↑ＦＭ(i)_jのうちの並進力ベクトルである並進接触力を↑Ｆ(i)_j、↑ＦＭ(i)_jのうちのモーメントベクトルである接触力モーメントを↑Ｍ(i)_jと表記する。

これらの並進接触力↑Ｆ(i)_j及び接触力モーメント↑Ｍ(i)_jは、第ｉ全接触力↑ＦＭt(i)の場合と同様に、それぞれ慣性座標系で見た３成分の縦ベクトルとして表現されるものとする。そして、第ｊ可動リンク接触力↑ＦＭ(i)_jは、↑Ｆ(i)_j及び↑Ｍ(i)_jの各成分を並べた６成分の縦ベクトル（＝[↑Ｆ(i)_j，↑Ｍ(i)_j]^T）として表現されるものとする。また、第ｊ可動リンク接触力↑ＦＭ(i)_jの作用点Ｐ(i)_jは、第ｊ可動リンク１０３_jと第ｉ接触対象面との接触面内における第ｉ接触対象面上の点とする。

このとき、全接触力↑ＦＭt(i)（以降、第ｉ全接触力↑ＦＭt(i)ということがある）と可動リンク接触力↑ＦＭ(i)_j（ｊ＝１，２，…，ｍ(i)）との間の関係は一般的に、次式（１）により表される。

なお、式（１）のただし書きにおいて、行列AA(i)_jの成分“Ｉ”、“０”はそれぞれ、単位行列、零行列である。また、“×”は外積（ベクトル積）を表す算術符号である。

ここで、第ｊ可動リンク接触力↑ＦＭ(i)_jの作用点Ｐ(i)_jに摂動分の接触力↑ΔＦＭ(i)_j（＝[↑ΔＦ(i)_j，↑ΔＭ(i)_j]^T）を付加し、それによって、第ｉ全接触力↑ＦＭt(i)の作用点Ｐt(i)に摂動分の接触力↑ΔＦＭt(i)（＝[↑ΔＦt(i)，↑ΔＭt(i)]^T）を付加することを想定する。なお、作用点Ｐ(i)_jに摂動分の接触力↑ΔＦＭ(i)_jを付加するということは、作用点Ｐ(i)_jに作用する接触力を↑ＦＭ(i)_jから↑ＦＭ(i)_j＋↑ΔＦＭ(i)_jに変更することを意味する。同様に、作用点Ｐt(i)に摂動分の接触力↑ΔＦＭt(i)を付加するということは、作用点Ｐt(i)に作用する全接触力を↑ＦＭt(i)から↑ＦＭt(i)＋↑ΔＦＭt(i)に変更することを意味する。

以降、↑ΔＦＭ(i)_jを第ｊ可動リンク摂動接触力、↑ΔＦＭ(i)_jのうちの並進力ベクトル↑ΔＦ(i)_j及びモーメントベクトル↑ΔＭ(i)_jをそれぞれ、第ｊ可動リンク摂動並進接触力、第ｊ可動リンク摂動接触力モーメントという。また、↑ΔＦＭt(i)を第ｉ摂動全接触力（又は摂動全接触力）、↑ΔＦＭt(i)のうちの並進力ベクトル↑ΔＦt及びモーメントベクトル↑ΔＭtをそれぞれ、第ｉ摂動全並進接触力（又は摂動全並進接触力）、第ｉ摂動全接触力モーメント（又は摂動全接触力モーメント）という。

上記摂動全接触力↑ΔＦＭt(i)と、第ｊ可動リンク摂動接触力↑ΔＦＭ(i)_j（ｊ＝１，２，…，ｍ(i)）との間の関係は、上記式（１）に基づいて、次式（２）により与えられる。

この式（２）におけるAA(i)_j（ｊ＝１，２，…，ｍ(i)）は、式（１）のただし書きで定義したものと同じである。

また、第ｊ可動リンク摂動接触力↑ΔＦＭ(i)_jが、第ｊ可動リンク１０３_ｊと第ｉ接触対象面との接触面（以降、第ｊ可動リンク接触面又は可動リンク接触面という）の位置及び姿勢のばね性変位によって発生するものと仮定する。より詳しくは、第ｊ可動リンク接触面の位置のばね性変位（並進変位）によって、第ｊ可動リンク摂動並進接触力↑ΔＦ(i)_jが発生し、第ｊ可動リンク接触面の姿勢のばね性変位（回転変位）によって、第ｊ可動リンク摂動接触力モーメント↑Ｍ(i)_jが発生するものと仮定する。なお、第ｊ可動リンク接触面の位置及び姿勢のばね性変位は、第ｊ可動リンク接触面での第ｉ接触対象面の形成部材の弾性変形又は第ｊ可動リンク１０３_jの接触部の弾性変形による並進変位及び回転変位にそれぞれ相当する。

このばね性変位による第ｊ可動リンク接触面の位置の変位量（第ｊ可動リンク接触面の３軸方向の並進変位量のベクトル。以降、ばね性並進変位量という）と姿勢の変位量（第ｉ可動リンク接触面の３軸周りの回転変位量のベクトル。以降、ばね性回転変位量という）とをそれぞれ↑Ｘorg(i)_j、↑Ｘrot(i)_jと表記する。このとき、↑Ｘorg(i)_jと↑ΔＦ(i)_jとの間の関係、及び↑Ｘrot(i)_jと↑ΔＭ(i)_jとの間の関係は、それぞれ、次式（３）、（４）により表されることとなる。

↑ΔＦ(i)_j＝Ｋorg(i)_j・↑Ｘorg(i)_j ……（３）
↑ΔＭ(i)_j＝Ｋrot(i)_j・↑Ｘrot(i)_j ……（４）

式（３）のＫorg(i)_jは、第ｊ可動リンク接触面のばね性並進変位量↑Ｘorg(i)_jの各成分毎のばね定数を対角成分として構成される３次の対角行列（以下、並進ばね定数行列Ｋorg(i)_jという）、式（４）のＫrot(i)_jは、第ｊ可動リンク接触面のばね性回転変位量↑Ｘrot(i)_jの各成分毎のばね定数を対角成分として構成される３次の対角行列（以下、回転ばね定数行列Ｋrot(i)_jという）である。

また、移動体１０１と第ｉ接触対象面との全ての接触面（第ｉ接触対象面に接触している全ての可動リンク１０３(i)_1〜１０３(i)_m(i)の接触面）を代表する単一の仮想的な接触面としての第ｉ代表接触面を想定し、この第ｉ代表接触面にて第ｉ全接触力↑ＦＭt(i)が移動体１０１に作用するものとする。該第ｉ代表接触面は、第ｉ接触対象面上での所謂支持多角形に相当するものである。

そして、第ｊ可動リンク接触面の場合と同様の考え方によって、第ｉ摂動全接触力↑ΔＦＭt(i)が、第ｉ代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定する。より詳しくは、第ｉ代表接触面の位置のばね性変位（並進変位）によって、摂動全並進接触力↑ΔＦt(i)が発生し、第ｉ代表接触面の姿勢のばね性変位（回転変位）によって、摂動全接触力モーメント↑ΔＭt(i)が発生するものと仮定する。

従って、第ｉ代表接触面の位置のばね性並進変位量（ばね性変位による３軸方向の並進変位量のベクトル）と姿勢のばね性回転変位量（ばね性変位による３軸周りの回転変位量のベクトル）とをそれぞれ↑Ｘc_org(i)、↑Ｘc_rot(i)とすると、↑Ｘc_org(i)と↑ΔＦt(i)との間の関係、及び↑Ｘc_rot(i)と↑ΔＭt(i)との間の関係は、それぞれ、次式（５）、（６）により表されることとなる。

↑ΔＦt(i)＝Ｋc_org(i)・↑Ｘc_org(i) ……（５）
↑ΔＭt(i)＝Ｋc_rot(i)・↑Ｘc_rot(i) ……（６）

式（５）のＫc_org(i)は、第ｉ代表接触面のばね性並進変位量↑Ｘc_org(i)の各成分毎のばね定数を対角成分として構成される３次の対角行列（以下、並進ばね定数行列Ｋc_org(i)という）、式（６）のＫc_rot(i)は、第ｉ代表接触面のばね性回転変位量↑Ｘc_rot(i)の各成分毎のばね定数を対角成分として構成される３次の対角行列（以下、回転ばね定数行列Ｋc_rot(i)という）である。

補足すると、上記第ｉ代表接触面は、移動体１０１の単一の可動リンク１０３_jだけが第ｉ接触対象面に接触している状態では、第ｉ代表接触面は、その単一の可動リンク１０３_jの第ｉ接触対象面との接触面（第ｊ可動リンク接触面）に一致するものとされる。そして、この状態では、第ｉ代表接触面の位置及び姿勢のばね性変位は、第ｊ可動リンク接触面の位置及び姿勢のばね性変位と同じであり、第ｊ可動リンク接触面での第ｉ接触対象面の形成部材の弾性変形又は第ｊ可動リンク１０３_jの接触部の弾性変形による並進変位及び回転変位にそれぞれ相当する。

また、移動体１０１の２つ以上の可動リンク１０３が第ｉ接触対象面に接触している状態では、上記第ｉ代表接触面は、第ｉ接触対象面に接触している全ての可動リンク１０３の接触部分をひとまとめにした単一の仮想的な接触部の第ｉ接触対象面との接触面としての意味を持つ。そして、この状態での第ｉ代表接触面の位置及び姿勢のばね性変位は、当該仮想的な接触部の接触面での第ｉ接触対象面の形成部材の弾性変形あるいは当該仮想的な接触部の弾性変形による並進変位及び回転変位にそれぞれ相当する。

前記式（２）において、第ｊ可動リンク接触力↑ＦＭ(i)_j（ｊ＝１，２，…，ｍ(i)）の作用点Ｐ(i)_jにそれぞれ付加する第ｊ可動リンク摂動並進接触力↑ΔＦ(i)_jによって全接触力↑ＦMt(i)の作用点Ｐt(i)に付加される摂動全並進接触力↑ΔＦt(i)に着目すると、次式（７）が得られる。

↑ΔＦt(i)＝↑ΔＦ(i)_1＋↑ΔＦ(i)_2＋……＋↑ΔＦ(i)_m(i) ……（７）

この式（７）と、前記式（３），（５）とから次式（８）が得られる。

Ｋc_org(i)・↑Ｘc_org(i)＝Ｋorg(i)_1・↑Ｘorg(i)_1＋Ｋorg(i)_2・↑Ｘorg(i)_2
＋……＋Ｋorg(i)_m(i)・↑Ｘorg(i)_m(i) ……（８）

一方、摂動全接触力↑ΔＦＭt(i)の付加前の全接触力↑ＦＭt(i)の作用点Ｐt(i)を、全接触力中心点とし、第ｊ可動リンク摂動接触力↑ΔＦＭ(i)_jの付加前の第ｊ可動リンク接触力↑ＦＭ(i)_jの作用点Ｐ(i)_jを第ｊ可動リンク接触面の接触力中心点としたとき、任意の基準点に対する作用点Ｐt(i)の位置ベクトル（これを↑Ｐt(i)と表記する）と、作用点Ｐ(i)_j（ｊ＝１，２，…，ｍ(i)）のそれぞれの位置ベクトル（これを↑Ｐ(i)_jと表記する）との間の関係は、次式（９）により与えられる。

なお、全接触力中心点は、全接触力↑ＦＭt(i)の作用点であって、その点周りの全接触力モーメント↑Ｍt(i)の第ｉ接触対象面に平行な成分がゼロとなるような点である。同様に、第ｊ可動リンク接触面の接触力中心点は、第ｊ可動リンク接触力↑ＦＭ(i)_jの作用点であって、その点周りの接触力モーメント↑Ｍ(i)_jの第ｉ接触対象面に平行な成分がゼロとなるような点である。

↑Ｐt(i)＝ｒ(i)_1・↑Ｐ(i)_1＋ｒ(i)_2・↑Ｐ(i)_2
＋……＋ｒ(i)_m(i)・↑Ｐ(i)_m(i) ……（９）

この式（９）におけるｒ(i)_j（ｊ＝１，２，…，ｍ(i)）は、ｒ(i)_j≡Fn(i)_j／Ｆnt(i)により定義される重み係数である。ここで、Ｆn(i)_j（ｊ＝１，２，…，ｍ(i)）は、第ｊ可動リンク並進接触力↑Ｆ(i)_jのうちの第ｉ接触対象面に垂直な垂直抗力成分の絶対値（以降、接触面垂直抗力成分という）、Ｆnt(i)は全並進接触力↑Ｆt(i)のうちの第ｉ接触対象面に垂直な垂直抗力成分の絶対値（接触面垂直抗力成分）であり、Ｆnt(i)＝Ｆn(i)_1＋Ｆn(i)_2＋……＋Ｆn(i)_m(i)である。従って、重み係数ｒ(i)_jは、全並進接触力↑Ｆt(i)の接触面垂直抗力成分Ｆnt(i)に対する、第ｊ可動リンク接触力↑Ｆ(i)_jの接触面垂直抗力成分Ｆn(i)_jの比率であり、その値は、０≦ｒ(i)_j≦１の範囲内の値である。

そして、この式（９）と同様の関係が、第ｉ代表接触面の位置のばね性並進変位量↑Ｘc_org(i)と、第ｊ可動リンク接触面の位置のばね性並進変位量↑Ｘorg(i)_jとの間にも成立するものと仮定する。すなわち、次式（１０）が成立するものと仮定する。

↑Ｘc_org(i)＝ｒ(i)_1・↑Ｘorg(i)_1＋ｒ(i)_2・↑Ｘorg(i)_2
＋……＋ｒ_m(i)・↑Ｘorg(i)_m(i) ……（１０）

このとき、この式（１０）と前記式（８）とから、次式（１１）が得られる。

Ｋorg(i)_j＝ｒ(i)_j・Ｋc_org(i) ……（１１）

本発明では、上記式（１０）が、第ｉ代表接触面のばね性並進変位量↑Ｘc_org(i)と第ｊ可動リンク接触面（ｊ＝１，２，…，ｍ(i)）のばね性並進変位量↑Ｘorg(i)_jとの間の関係を表す基本式、式（１１）が、第ｉ代表接触面の並進ばね定数行列Ｋc_org(i)と第ｊ可動リンク接触面（ｊ＝１，２，…，ｍ(i)）の並進ばね定数行列Ｋorg(i)_jとの関係を表す基本式とされる。

従って、第ｊ可動リンク接触面の並進ばね定数行列Ｋorg(i)_jの各対角成分（３軸方向のそれぞれの並進変位に関するばね定数）は、重み係数ｒ(i)_jに比例し、重み係数ｒ(i)_jの値が大きいほど（“１”に近いほど）、すなわち、全並進接触力↑Ｆt(i)の接触面垂直抗力成分Ｆnt(i)に対する第ｊ可動リンク並進接触力↑Ｆ(i)_jの接触面垂直抗力成分Ｆn(i)_jの比率が大きいほど、大きくなるものとされる。換言すれば、Ｆnt(i)に対するＦn(i)_jの比率が大きいほど、要求される第ｊ可動リンク摂動並進接触力に対する第ｊ可動リンク接触面のばね性並進変位量↑Ｘorg(i)_jの変化の感度が高くなるものとされる。

また、単一の第ｊ可動脚リンク１０３_jだけが第ｉ接触対象面に接触している場合（ｒ(i)_j＝１となる場合）には、第ｊ可動リンク接触面と第ｉ代表接触面とが一致することに対応して、第ｊ可動リンク接触面の並進ばね定数行列Ｋorg(i)_jと、第ｉ代表接触面の並進ばね定数行列Ｋc_org(i)とが互いに一致する。さらにこの場合、↑ΔＦ(i)_j＝↑ΔＦt(i)となるので、第ｊ可動リンク接触面のばね性並進変位量↑Ｘorg(i)_jと、第ｉ代表接触面のばね性並進変位量↑Ｘc_org(i)とが互いに一致することとなる。

そして、２つ以上の可動リンク１０３が第ｉ接触対象面に接触している場合には、第ｉ代表接触面のばね性並進変位量↑Ｘc_org(i)は、第ｉ接触対象面に接触している各可動リンク１０３_jに対応する第ｊ可動リンク接触面のばね性並進変位量↑Ｘorg(i)_jの重み付き平均値（ｒ(i)_jを重み係数とする重み付き平均値）となる。

次に、前記式（２）において、第１〜第ｍ(i)可動リンク接触力↑ＦＭ(i)_1〜↑ＦＭ(i)_m(i)のそれぞれの並進接触力↑Ｆ(i)_1〜↑Ｆ(i)_m(i)を一定に保持した状態で、第ｊ可動リンク接触力↑ＦＭ(i)_j（ｊ＝１，２，…，ｍ(i)）の作用点Ｐ(i)_jに第ｊ可動リンク摂動接触力モーメント↑ΔＭ(i)_jを付加することによって全接触力↑ＦMt(i)の作用点Ｐt(i)に付加される摂動全接触力モーメント↑ΔＭt(i)に着目すると、次式（１２）が得られる。

↑ΔＭt(i)＝↑ΔＭ(i)_1＋↑ΔＭ(i)_2＋……＋↑ΔＭ(i)_m(i) ……（１２）

そして、この式（１２）と、前記式（４），（６）とから次式（１３）が得られる。

Ｋc_rot(i)・↑Ｘc_rot(i)＝Ｋrot(i)_1・↑Ｘrot(i)_1＋Ｋrot(i)_2・↑Ｘrot(i)_2
＋……＋Ｋrot(i)_m(i)・↑Ｘrot(i)_m(i)
……（１３）

一方、第ｊ可動リンク接触力↑ＦＭ(i)_jの作用点Ｐ(i)_jを第ｊ可動リンク接触面の接触力中心点としたとき、第１〜第ｍ(i)可動リンク接触力↑ＦＭ(i)_1〜↑ＦＭ(i)_m(i)のそれぞれの並進接触力↑Ｆ_1〜↑Ｆ_Nを一定に保持した状態で、第ｊ可動リンク接触力↑ＦＭ(i)_jの作用点Ｐ(i)_j（接触力中心点）に第ｊ脚摂動接触力モーメント↑ΔＭ(i)_jを付加するということは、第ｊ可動リンク接触面での接触力中心点の位置を、第ｉ接触対象面上で点Ｐ(i)_jからずらすことと等価である。同様に、全接触力↑ＦＭt(i)の作用点Ｐt(i)を全接触力中心点としたとき、全接触力↑ＦＭt(i)の作用点Ｐt(i)に摂動全接触力モーメント↑ΔＭt(i)が付加されるということは、全接触力中心点の位置が第ｉ接触対象面上で点Ｐt(i)からずれることと等価である。

この場合、第ｊ可動リンク接触面での接触力中心点の位置の第ｉ接触対象面に平行な方向での変位量（２成分の変位量ベクトル）を↑ΔＲpt(i)_j、全接触力中心点の位置の第ｉ接触対象面に平行な方向での変位量（２成分の変位量ベクトル）を↑ΔＲptt(i)とすると、↑ΔＲptt(i)・Ｆnt(i)＝↑ΔＭt(i)の第ｉ接触対象面に平行な軸軸周り成分、↑ΔＲpt(i)_j・Ｆn(i)_j＝↑ΔＭ(i)_jの第ｉ接触対象面に平行な軸周り成分となる。このため、上記式（１２）から、次式（１４）が得られる。

↑ΔＲptt(i)＝ｒ(i)_1・↑ΔＲpt(i)_1＋ｒ(i)_2・↑ΔＲpt(i)_2
＋……＋ｒ(i)_m(i)・↑ΔＲpt(i)_m(i) ……（１４）

なお、式（１４）のｒ(i)_j（ｊ＝１，２，…，ｍ(i)）は、前記した重み係数ｒ(i)_j（≡Ｆn(i)_j／Ｆnt(i)）である。

また、第１〜第ｍ(i)可動リンク接触面は、共通の接触対象面（第ｉ接触対象面）上の部分であるから、第１〜第ｍ(i)可動リンク接触面のそれぞれに作用する垂直抗力Ｆn(i)_jが互いに同じである場合、第ｊ可動リンク接触面の接触力中心点の位置の第ｊ接触対象面に平行な方向での変位量↑ΔＲpt(i)_jに対応する摂動接触力モーメント（＝↑ΔＲpt(i)_j・Ｆn(i)_j）と、第ｊ可動リンク接触面のばね性変位による回転変位量↑Ｘrot(i)_jの第ｉ接触対象面に平行な軸周り成分との間の関係は、いずれの第ｊ可動リンク接触面についても同じになると考ええられる。

そこで、第１〜第ｍ(i)可動リンク接触面のそれぞれに作用する接触面垂直抗力成分Ｆn(i)_jが互いに同じ値（これをＦna(i)とおく）である場合、次式（１５）が成立すると考えられる。

↑ΔＲpt(i)_j・Ｆna(i)＝Ｋrot(i)・↑Ｘrot(i)_j_ab ……（１５）

なお、↑Ｘrot(i)_j_abは、第ｊ可動リンク接触面のばね性回転変位量↑Ｘrot(i)_jの、第ｉ接触対象面に平行な２軸周り成分であり、Ｋrot(i)は、２次の対角行列（第ｉ接触対象面に平行な２軸周りの回転に関するばね定数行列）である。

そして、上記式（１５）が、第ｉ代表接触面についても同様に成立するものと仮定すると、次式（１６）が得られる。

↑ΔＲptt(i)・Fna(i)＝Ｋrot(i)・↑Ｘc_rot(i)_ab ……（１６）

なお、↑Ｘc_rot(i)_abは、第ｉ代表接触面のばね性回転変位量↑Ｘc_rot(i)の、第ｉ接触対象面に平行な２軸周り成分である。

上記式（１４）〜（１６）により、次式（１７）が得られる。

Ｋrot(i)・↑Ｘc_rot(i)_ab
＝ｒ(i)_1・Ｋrot(i)・↑Ｘrot(i)_1_ab＋ｒ(i)_2・Ｋrot(i)・↑Ｘrot(i)_2_ab
＋……＋ｒ(i)_m(i)・Ｋrot(i)・↑Ｘrot(i)_m(i)_ab ……（１７）

さらに、この式（１７）から次式（１８）が得られる。

↑Ｘc_rot(i)_ab＝ｒ(i)_1・↑Ｘrot(i)_1_ab＋ｒ(i)_2・↑Ｘrot(i)_2_ab
＋……＋ｒ(i)_m(i)・↑Ｘrot(i)_m(i)_ab ……（１８）

そこで、本実施形態では、第ｉ代表接触面のばね性回転変位量↑Ｘc_org(i)と、第ｊ可動リンク接触面のばね性回転変位量↑Ｘorg(i)_jとの間に、それらの全ての成分（３成分）について、上記式（１８）と同じ関係が成立するものと仮定する。すなわち、次式（１９）が成立するものと仮定する。

↑Ｘc_rot(i)＝ｒ(i)_1・↑Ｘrot(i)_1＋ｒ(i)_2・↑Ｘrot(i)_2
＋……＋ｒ(i)_m(i)・↑Ｘrot(i)_m(i) ……（１９）

このとき、この式（１９）と前記式（１３）とから、次式（２０）が得られる。

Ｋrot(i)_j＝ｒ(i)_i・Ｋc_rot(i) ……（２０）

本実施形態では、上記式（１９）が、代表接触面（第ｉ代表接触面）のばね性回転変位量↑Ｘc_rot(i)と第ｊ可動リンク接触面（ｊ＝１，２，…，ｍ(i)）のばね性回転変位量↑Ｘrot(i)_jとの間の関係を表す基本式、式（２０）が、代表接触面の回転ばね定数行列Ｋc_rot(i)と第ｊ可動リンク接触面（ｊ＝１，２，…，ｍ(i)）の回転ばね定数行列Ｋrot(i)_jとの関係を表す基本式とされる。

従って、第ｊ可動リンク接触面の回転ばね定数行列Ｋrot(i)_jの各対角成分（３軸周りのそれぞれの回転変位に関するばね定数）は、重み係数ｒ(i)_jに比例し、重み係数ｒ(i)_jの値が大きいほど（“１”に近いほど）、すなわち、全並進接触力↑Ｆt(i)の接触面垂直抗力成分Ｆnt(i)に対する第ｊ可動リンク並進接触力↑Ｆ(i)_jの接触面垂直抗力成分Ｆn(i)_jの比率が大きいほど、大きくなるものとされる。換言すれば、Ｆnt(i)に対するＦn(i)_jの比率が大きいほど、要求される第ｊ可動リンク摂動接触力モーメントに対する第ｊ可動リンク接触面のばね性回転変位量↑Ｘrot(i)_jの変化の感度が高くなるものとされる。

また、単一の第ｊ可動リンク１００_jだけが第ｉ接触対象面に接触している場合（ｒ(i)_j＝１となる場合）には、第ｊ可動リンク接触面と第ｉ代表接触面とが一致することに対応して、第ｊ可動リンク接触面の回転ばね定数行列Ｋrot(i)_jと、第ｉ代表接触面の回転ばね定数行列Ｋc_rot(i)とが互いに一致する。さらにこの場合、↑ΔＭ(i)_j＝↑ΔＭt(i)となるので、第ｊ可動リンク接触面のばね性回転変位量↑Ｘrot(i)_jと、第ｉ代表接触面のばね性回転変位量↑Ｘc_rot(i)とが互いに一致することとなる。

そして、２つ以上の可動リンク１０３が第ｉ接触対象面に接触している場合には、代表接触面のばね性回転変位量↑Ｘc_rot(i)は、第ｉ接触対象面に接触している各可動リンク１０３_jに対応する第ｊ可動リンク接触面のばね性回転変位量↑Ｘrot(i)_jの重み付き平均値（ｒ(i)_jを重み係数とする重み付き平均値）となる。

次に、前記式（２）に式（３）〜（６）を適用し、さらに、式（１１），（２０）を適用することによって、次式（２１）が得られる。

なお、前記式（１１）、（２０）は、全接触力↑ＦＭt(i)の作用点Ｐt(i)が全接触力中心点、第ｊ可動リンク接触力↑ＦＭ(i)_jの作用点Ｐ(i)_jが第ｊ可動リンク接触面の接触力中心点であることを前提としているので、式（２１）におけるVV(i)_j（ｊ＝１，２，…，ｍ(i)）は、全接触力中心点としての作用点Ｐt(i)に対する第ｊ可動リンク接触面の接触力中心点の位置ベクトルを改めて↑Ｖ(i)_jとしたとき、次のように定義される行列である。

VV(i)_j： VV(i)_j・↑Ｆ(i)_j＝Ｖ(i)_j×↑Ｆ(i)_jとなる行列
ただし、↑Ｖ(i)_j：全接触力中心点に対する第ｊ可動リンク接触面の接触力中心点の位置ベクトル

なお、この場合、上記全接触力中心点は、より詳しくは、摂動全接触力↑ΔＦＭt(i)が付加される前の全接触力↑ＦＭt(i)に対応する全接触力中心点であり、第ｊ可動リンク接触面の接触力中心点は、第ｊ可動リンク摂動接触力↑ΔＦＭ(i)_jが付加される前の第ｊ可動リンク接触力↑ＦＭ(i)_jに対応する接触力中心点である。

さらに、上記式（２１）から、次式（２２）が得られる。

ここで、移動体１０１の基体１０２の位置（３軸方向の位置）及び姿勢（３軸周りの姿勢角）と移動体１０１の各関節の変位量とを成分として構成される一般化変数ベクトルを↑ｑ、代表接触面（第ｉ代表接触面）の位置のばね性並進変位量↑Ｘc_org(i)及び姿勢のばね性回転変位量↑Ｘc_rot(i)から成る変位量ベクトル（＝[↑Ｘc_org(i)，↑Ｘc_rot(i)]^T）を↑Ｘc(i)、第ｊ可動リンク接触面の位置のばね性並進変位量↑Ｘorg(i)_j及び姿勢のばね性回転変位量↑Ｘrot(i)_jから成る変位量ベクトル（＝[↑Ｘorg(i)_j，↑Ｘrot(i)_j]^T）を↑Ｘ(i)_jと表記する。以降、↑Ｘc(i)を代表接触面のばね性並進・回転変位量、↑Ｘ(i)_jを第ｊ可動リンク接触面のばね性並進・回転変位量という。なお、上記一般化変数ベクトル↑ｑは、より詳しくは、基体１０２の位置及び姿勢の６個の成分と、移動体１０１の各関節の変位量とを並べた縦ベクトルである。

この場合、代表接触面（第ｉ代表接触面）のばね性並進・回転変位量↑Ｘc(i)を代表接触面の位置及び姿勢の単位時間あたりの変位量（時間的変化率）とみなすと、代表接触面の位置及び姿勢の単位時間あたりの変位量と、一般化変数ベクトル↑ｑの単位時間当たりの変化量（時間的変化率）↑Δｑとの間の関係を表現するヤコビアン行列は、↑Ｘc(i)と↑Δｑとの間の関係を次式（２３）により表す行列Ｊc(i)として表現される。なお、↑Δｑは、一般化変数ベクトル↑ｑの各成分の単位時間当たりの変化量を並べた縦ベクトルである。

↑Ｘc(i)＝Ｊc(i)・↑Δｑ ……（２３）

同様に、第ｊ可動リンク接触面のばね性並進・回転変位量↑Ｘ(i)_jを第ｊ可動リンク接触面の位置及び姿勢の単位時間あたりの変位量（時間的変化率）とみなすと、第ｊ可動リンク接触面の位置及び姿勢の単位時間あたりの変位量と、一般化変数ベクトル↑ｑの単位時間当たりの変化量（時間的変化率）↑Δｑとの間の関係を表現するヤコビアン行列は、↑Ｘ(i)_jと↑Δｑとの間の関係を次式（２４）により表す行列Ｊ(i)_jとして表現される。

↑Ｘ(i)_j＝Ｊ(i)_j・↑Δｑ ……（２４）

補足すると、第ｉ代表接触面の位置の単位時間当たりの変位量と↑Δｑとの間の関係を表現するヤコビアン行列（↑Ｘc_org(i)と↑Δｑとの間の関係を次式（２５ａ）により表現する行列）をＪc_org(i)、第ｉ代表接触面の姿勢の単位時間当たりの変位量と↑Δｑとの間の関係を表現するヤコビアン行列（↑Ｘc_rotと↑Δｑとの間の関係を次式（２５ｂ）により表現する行列）をＪc_rot(i)と表記すると、式（２５ｃ）に示す如くＪc(i)＝[Ｊc_org(i)，Ｊc_rot(i)]^Tとなる。

↑Ｘc_org(i)＝Ｊc_org(i)・↑Δｑ ……（２５ａ）
↑Ｘc_rot(i)＝Ｊc_rot(i)・↑Δｑ ……（２５ｂ）
Ｊc(i)＝[Ｊc_org(i)，Ｊc_rot(i)]^T ……（２５ｃ）

同様に、第ｊ可動リンク接触面の位置の単位時間当たりの変位量と↑Δｑとの間の関係を表現するヤコビアン行列（↑Ｘorg(i)_jと↑Δｑとの間の関係を次式（２６ａ）により表す行列）をＪorg(i)_j、第ｊ可動リンク接触面の姿勢の単位時間当たりの変位量と↑Δｑとの間の関係を表現するヤコビアン行列（↑Ｘrot(i)_jと↑Δｑとの間の関係を次式（２６ｂ）により表す行列）をＪrot(i)_jと表記すると、式（２６ｃ）に示す如くＪ(i)_j＝[Ｊorg(i)_j，Ｊrot(i)_j]^Tとなる。

↑Ｘorg(i)_j＝Ｊorg(i)_j・↑Δｑ ……（２６ａ）
↑Ｘrot(i)_j＝Ｊrot(i)_j・↑Δｑ ……（２６ｂ）
Ｊ(i)_j＝[Ｊorg(i)_j，Ｊrot(i)_j]^T ……（２６ｃ）

前記式（２２）の両辺を微分し、前記式（２３）、（２４）を適用することで、次式（２７）が得られる。

従って、第ｉ代表接触面の位置及び姿勢の変位に関するヤコビアン行列Ｊc(i)（以降、第ｉ代表接触面ヤコビアン行列Ｊc(i)又は代表接触面ヤコビアン行列Ｊc(i)という）は、第ｉ接触対象面における各可動リンク接触面の位置及び姿勢の変位に関するヤコビアン行列Ｊ(i)_j（ｊ＝１，２，…，ｍ(i)）から、上記の式（２７）に基づいて決定できることとなる。

この場合、第ｊ可動リンク接触面の位置及び姿勢は、第ｊ可動リンク１０３_jの先端部の位置及び姿勢であるから、ヤコビアン行列Ｊ(i)_j（以降、可動リンクヤコビアン行列Ｊ(i)_jという）は、換言すれば、第ｊ可動リンク１０３_jの先端部の位置及び姿勢の単位時間当たりの変化量と、↑Δｑとの間の関係を表現するヤコビアン行列である。そして、このようなヤコビアン行列Ｊ(i)_jは、移動体１０１の各関節の実際の変位量の観測値及びその時間的変化率を基に特定できる。

そして、このように各脚リンクヤコビアン行列Ｊ_i（ｉ＝１，２，…，Ｎ）を特定すれば、それらのＪ_iを用いて、上記式（２７）により、代表接触面ヤコビンア行列Ｊcを決定できることとなる。

ここで、移動体１０１の動作環境にＮ個（Ｎ≧２）の互いに異なる接触対象面（第１〜第Ｎ接触対象面）が存在し、移動体１０１のある運動状態において、それらの第１〜第Ｎ接触対象面のそれぞれに、移動体１の１つ以上の可動リンク１０３が接触する場合を想定する。

この場合、第ｉ接触対象面に関する上記の事項は、第１〜第Ｎ接触対象面のそれぞれの接触対象面毎に成立する事項である。

このようにＮ個の接触対象面のそれぞれに移動体１０１の可動リンク１０３が接触する状態において、移動体１０１の所要の状態を実現するための要求操作量として、第ｉ接触対象面（ｉ＝１，２，…，Ｎ）に関するある値の摂動全接触力↑ΔＦＭt(i)が与えられたとき、この↑ΔＦＭt(i)は前記式（５），（６）に基づいて、第ｉ代表接触面のばね性並進・回転変位量↑Ｘc(i)（＝[↑Ｘc_org(i)，↑Ｘc_rot(i)]^T）の要求値に変換できることとなる。この変換は、次式（２８）により与えられる。

さらに、第１〜第Ｎ接触対象面のそれぞれに対応する第ｉ代表接触面（ｉ＝１，２，…，Ｎ）のばね性並進・回転変位量↑Ｘc(i)を並べたベクトル（縦ベクトル）を、総合ばね性並進・回転変位量↑Ｘc（≡[↑Ｘc(1)，↑Ｘc(2)，……，↑Ｘc(N)]^T）、第１〜第Ｎ接触対象面のそれぞれに対応する第ｉ代表接触面ヤコビアン行列Ｊc(i)（ｉ＝１，２，…，Ｎ）を縦に並べて構成される行列を、総合ヤコビアン行列Ｊc（≡[Ｊc(1)，Ｊc(2)，……，Ｊc(N)]^Tとおくと、↑Ｘcと、一般化変数ベクトル↑ｑの時間的変化率↑Δｑとの間の関係は、次式（２９）により与えられる。

そして、総合ヤコビアン行列Ｊcの擬似逆行列をＪc^-1とおくと、第１〜第Ｎ接触対象面のそれぞれに対応する第ｉ代表接触面（ｉ＝１，２，…，Ｎ）のばね性並進・回転変位量↑Ｘc(i)の要求値を並べてなる総合ばね性並進・回転変位量↑Ｘcの要求値から、次式（３０）により、第１〜第Ｎ接触対象面の摂動全接触力↑ΔＦＭt(i)（ｉ＝１，２，…，Ｎ）の要求値を実現するための、移動体１０１の各関節の変位量の修正量を決定できることとなる。

↑Δｑ＝Ｊc^-1・↑Ｘc ……式（３０）

そこで、移動体１０１の任意の運動状態において、第ｉ接触対象面から移動体１０１に作用する全接触力を制御するための要求操作量として、摂動全接触力↑ΔＦＭt(i)の要求値が与えられたとき、前記式（２７）に基づいて第ｉ代表接触面ヤコビアン行列Ｊc(i)を求めると共に、このＪc(i)（ｉ＝１，２，…，Ｎ）から構成される総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を決定する。さらに、前記式（２８）に基づいて、↑ΔＦＭt(i)の要求値に対応する第ｉ代表接触面のばね性並進・回転変位量↑Ｘc(i)を求める。そして、この↑Ｘc(i)から構成される代表接触面総合ばね性並進・回転変位量↑Ｘcと、Ｊc^-1とから式（２９ｂ）によって、摂動全接触力↑ΔＦＭt(i)（ｉ＝１，２，…，Ｎ）の全ての要求値を実現するための各関節の変位量の修正量を一括して決定できることとなる。

ひいては、各可動リンク１０３_jの先端部の位置及び姿勢の修正量自体をそれぞれ決定することなく、一括的に、各関節の変位量を決定できることとなる。

以上が、本発明の基礎とする技術事項である。

以上説明したことを基礎として、以下に本発明を説明する。

本発明は、基体と、基体に連結された複数の可動リンクと、各可動リンクの関節を駆動する関節アクチュエータとを備えた移動体の動作を、該移動体の動作環境に存在する互いに異なる複数の接触対象面である第１〜第Ｎ接触対象面（Ｎ：２以上の整数）のそれぞれに１つ以上の可動リンクを接触させつつ該移動体の運動を行なわせるための目標運動と、該目標運動を実現するために該移動体に前記第１〜第Ｎ接触対象面のそれぞれから作用させるべき全接触力の目標値である目標全接触力とに応じて制御する制御装置であって、
前記第１〜第Ｎ接触対象面のそれぞれである第ｉ接触対象面（ｉ＝１，２，…，Ｎ）から前記移動体に実際に作用する全接触力の観測値と、該第ｉ接触対象面に対応する前記目標全接触力との偏差をゼロに近づけるために該第ｉ接触対象面から該移動体に付加的に作用させるべき全接触力の要求修正量である第ｉ全接触力要求修正量（ｉ＝１，２，…，Ｎ）のそれぞれを前記偏差に応じて決定する全接触力要求修正量決定手段と、
前記第ｉ全接触力要求修正量が前記移動体と第ｉ接触対象面との全ての接触面を代表する単一の仮想的な接触面としての第ｉ代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定して、前記決定された第ｉ全接触力要求修正量と該第ｉ接触対象面に対応してあらかじめ定められた第ｉ代表接触面のばね定数とから、第ｉ代表接触面（ｉ＝１，２，…，Ｎ）のそれぞれの位置及び姿勢の要求変位量を算出する代表接触面位置姿勢変位量算出手段と、
前記第ｉ代表接触面の位置及び姿勢の時間的変化率と、前記基体の位置及び姿勢と前記移動体の各関節の変位量とを成分として構成される一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である第ｉ代表接触面ヤコビアン行列Ｊc(i)（ｉ＝１，２，…，Ｎ）のそれぞれを、該第ｉ接触対象面に接触している可動リンクのグループである第ｉ接触可動リンクグループの各可動リンクの接触部の位置の時間的変化率又は該第ｉ接触可動リンクグループの各可動リンクの接触部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各可動リンクヤコビアン行列Ｊ(i)_jと、前記第ｉ代表接触面のばね定数と、前記移動体に該第ｉ接触対象面から実際に作用する全接触力の作用点としての全接触力中心点に対する第ｉ接触可動リンクグループの各可動リンクの接触部の実際の接触力中心点の相対位置と、第ｉ接触可動リンクグループの各可動リンクに実際に作用する接触力の値とから前記式（２７）により算出する代表接触面ヤコビアン行列算出手段と、
前記算出された第１〜第Ｎ代表接触面の位置及び姿勢の要求変位量を並べてなる総合要求変位量↑Ｘcに、前記算出された第１〜第Ｎ代表接触面ヤコビアン行列Ｊc(i)（ｉ＝１，２，…，Ｎ）を並べてなる総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を乗じることにより前記第１〜第Ｎ代表接触面のそれぞれの位置及び姿勢の要求変位量を実現するための前記移動体の各関節の変位量の修正量である関節変位修正量を決定する関節変位修正量決定手段と、
前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、前記決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する関節変位制御手段とを備えることを特徴とする（第１発明）。

ここで、前記式（２７）と、この式（２７）の変数の意味とを改めて記載すると、次の通りである。

なお、本発明において、前記移動体の動作環境に存在する互いに異なる複数の接触対象面というのは、その複数の接触対象面に含まれる任意の２つの接触対象面（又はその２つの接触対象面をそれぞれ包含する平面）が互いに交差するか、又は、互いに間隔を存して平行もしくはほぼ平行に対向するような複数の接触対象面を意味する。この場合、各接触対象面は、それに接触させる可動リンクの部位に比して広い面積を有する面（例えば、通常の床面の如き面）である必要はなく、例えば、該部位と同程度又はそれよりも小さい面積を有する局所的な面であってもよい。

上記第１発明によれば、第ｉ接触対象面（ｉ＝１，２，…，Ｎ）から前記移動体に実際に作用する全接触力（以降、実全接触力ということがある）の観測値と、該第ｉ接触対象面に対応する前記目標全接触力との偏差をゼロに近づけるための第ｉ全接触力要求修正量（ｉ＝１，２，…，Ｎ）のそれぞれが前記全接触力要求修正量決定手段により決定される。すなわち、各接触対象面毎に、実全接触力を目標全接触力に追従させるためのフィードバック操作量（制御入力）として、前記第ｉ全接触力要求修正量が決定される。この第ｉ全接触力要求修正量は、第ｉ接触対象面に対応する前記摂動全接触力↑ΔＦＭt(i)（ｉ＝１，２，…，Ｎ）の要求値に相当するものである。

なお、第ｉ接触対象面（ｉ＝１，２，…，Ｎ）から前記移動体に作用する全接触力は、該第ｉ接触対象面に移動体の１つ以上の可動リンクが接触することによって、該第ｉ接触対象面から移動体が受けるトータルの反力を意味する。

また、本発明における前記目標全接触力は、例えば、実際の各接触対象面のモデルとしての想定接触対象面から前記移動体に作用する接触力（反力）の目標値として設定されるものである。この場合、各接触対象面の目標全接触力としては、例えば、それらの合力が、前記移動体の目標運動に対して適宜の動力学モデルの動力学的関係（該目標全接触力の合力が、移動体に作用する重力と、移動体の目標運動によって発生する慣性力との合力に釣り合う等の動力学的関係）が成立するように作成されたものを採用できる。あるいは、移動体の目標運動に対して上記動力学的関係が成立するように作成した各接触対象面の基準の全接触力を、移動体の運動に関する所定の状態量（移動体の全体重心点の位置、全体重心点周りの各運動量、又はそれらの変化速度等の状態量）の目標値と実際の値との偏差に応じて、該偏差をゼロに近づけるように補正したものであってもよい。

前記目標全接触力は、基本的には、移動体の目標運動を動力学的に実現可能な形態で設定されておれば、どのような仕方で設定されていてもよい。

そして、本発明では、決定された第ｉ全接触力要求修正量に対応する第ｉ代表接触面（ｉ＝１，２，…，Ｎ）の位置及び姿勢の要求変位量が代表接触面位置姿勢変位量算出手段により算出される。

これにより、各接触対象面毎に、実全接触力を目標全接触力に追従させるためのフィードバック操作量（制御入力）としての第ｉ全接触力要求修正量が、第ｉ接触対象面に対応する単一の仮想面たる第ｉ代表接触面の位置及び姿勢の要求変位量に変換される。

ここで、第ｉ代表接触面の位置及び姿勢の変位量は、前記ばね性並進・回転変位量↑Ｘc(i)に相当するものであり、前記一般化変数ベクトル↑ｑの単位時間当たりの変化量↑Δｑに対して、第ｉ代表接触面ヤコビアン行列Ｊc(i)によって表現される前記式（２３）の関係を有する。

そして、第１〜第Ｎ代表接触面のそれぞれの位置及び姿勢の変位量（ばね性並進・回転変位量↑Ｘc(i)）を並べてなる総合ばね性並進・回転変位量↑Ｘcは、前記一般化変数ベクトル↑ｑの単位時間当たりの変化量↑Δｑに対して、代表接触面ヤコビアン行列Ｊc(1)〜Ｊc(N)を並べた総合ヤコビン行列Ｊcによって表現される前記式（２９）の関係を有する。

従って、第１〜第Ｎ全接触力要求修正量にそれぞれ対応する第ｉ代表接触面の位置及び姿勢の要求変位量を並べてなる総合要求変位量（総合ばね性並進・回転変位量↑Ｘcの要求値）に、総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を乗じることによって（すなわち、前記式（３０）によって）、当該総合要求変位量の全体を実現するための（ひいては、第１〜第Ｎ全接触力要求修正量を実現するための）移動体の関節変位修正量を決定できることとなる。

そこで、本発明では、前記代表接触面ヤコビアン行列算出手段によって、第ｉ代表接触面ヤコビアン行列Ｊc(i)（ｉ＝１，２，…，Ｎ）のそれぞれを算出する。この場合、第ｉ代表接触面ヤコビアン行列Ｊc(i)は、第ｉ接触対象面に接触している可動リンクのグループである第ｉ接触可動リンクグループの各可動リンクの接触部の位置の時間的変化率又は第ｉ接触可動リンクグループの各可動リンクの接触部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各可動リンクヤコビアン行列Ｊ(i)_jと、前記第ｉ代表接触面のばね定数と、前記移動体に実際に作用する全接触力の作用点としての全接触力中心点に対する第ｉ接触可動リンクグループの各可動リンクの接触部の実際の接触力中心点の相対位置と、第ｉ接触可動リンクグループの各可動リンクに実際に作用する接触力の値とから、前記式（２７）によって算出される。

ここで、第ｉ接触対象面における上記全接触力中心点は、第ｉ接触対象面から移動体に作用する全接触力によって、その点周りに発生するモーメント（接触力モーメント）の第ｉ接触対象面に平行な軸周りの成分がゼロとなるような点を意味する。また、第ｉ接触可動リンクグループの各可動リンクの接触部の接触力中心点は、第ｉ接触対象面から該可動リンクに作用する接触力によって、その点周りに発生するモーメント（接触力モーメント）の第ｉ接触対象面に平行な軸周りの成分がゼロとなるような点を意味する。

なお、式（２７）の演算を行なうために用いる各可動リンクに実際に作用する接触力の値や、その接触力作用点の位置、全接触力作用点の位置は、移動体に搭載した力センサ等により計測した観測値でよいことはもちろんであるが、実際の値を精度よく近似し得るものであれば、前記目標接触力や適当なモデル等を基に、近似的に推定もしくは予測した値であってもよい。

また、上記可動リンクヤコビアン行列Ｊ(i)_jに関して補足すると、本発明では、各可動リンクは、接触対象面への接触部に作用する接触力モーメントを変化させることができる構造の可動リンク（例えば、接触対象面への接触部の接触状態が面接触状態となり、その接触部の姿勢をアクチュエータにより変化させることができるように構成され可動リンク）と、該接触部位に作用する接触力モーメントを変化させることができない構造の可動リンク（例えば該接触部と接触対象面との接触状態が点状となる可動リンク）とのいずれの構造の可動リンクであってもよい。

この場合、各可動リンクが、その接触部での接触力モーメントを変化させることができる構造のものである場合には、可動リンクヤコビアン行列は、該可動リンクの接触部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列となる。また、各可動リンクが、その接触部での接触力モーメントを変化させることができない構造のものである場合には、可動リンクヤコビアン行列は、該可動リンクの接触部の位置の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列となる。

これらの可動リンクヤコビアン行列は、移動体の各関節の実際の変位量の観測値を基に、公知の手法によって求めることができる。

そして、本発明においては、前記関節変位修正量決定手段が、上記の如く算出された第ｉ代表接触面ヤコビアン行列Ｊc(i)を並べてなる総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を、第ｉ代表接触面の位置及び姿勢の要求変位量を並べてなる総合要求変位量に乗じることによって、前記関節変位修正量が決定される。これにより、結果的に、第１〜第Ｎ全接触力要求修正量を実現するための関節変位修正量が決定されることとなる。

さらに、前記関節変位制御手段が、前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、上記の如く決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する。

これにより、移動体が複数の接触対象面に接触する運動状態において、各接触対象面の実全接触力を目標全接触力に追従させるように、移動体の各関節の変位量の制御が行われ、全ての接触対象面（第１〜第Ｎ接触対象面）についての所謂、コンプライアンス制御が実現される。

かかる第１発明によれば、各接触対象面毎の第ｉ全接触力要求修正量を該接触対象面に対応する第ｉ代表接触面の位置及び姿勢の要求変位量に変換し、これらの要求変位量を並べた総合要求変位量から前記総合ヤコビアン行列の擬似逆行列Ｊc^-1によって、移動体の各関節の関節変位修正量を求めることにより、個々の接触対象面での可動リンクの接触部の位置や姿勢の修正と実全接触力の変化との関係やそれらの相互の関係を考慮して各接触対象面に対する可動リンクの接触部の位置や姿勢の修正量を決定するような処理を実行することなく、全ての接触対象面についての実全接触力を目標全接触力に追従させるための移動体の各関節の変位量の修正量（関節変位修正量）を一括的に決定できる。そのため、関節変位修正量を決定する処理を効率よく短時間で行なうことができる。

この場合、前記式（２７）における各可動リンク（第ｉ接触可動リンクグループの各可動リンク）の重み係数ｒ(i)_jは、接触力の接触面垂直抗力成分が相対的に大きい可動リンクほど、より大きい値（“１”に近い値）に設定されることとなるため、第ｉ全接触力要求修正量に対する第ｉ接触可動リンクグループの各可動リンクの負担分は、結果的に、接触面垂直抗力成分が相対的に大きい可動リンクほど、大きくなる。従って、接触面垂直抗力成分が相対的に小さい可動リンクの接触部の位置や姿勢を不必要に修正したりすることなく、第１〜第Ｎ全接触力要求修正量を確実に実現し得るような関節変位修正量を決定できる。

また、第ｉ接触可動リンクグループの各可動リンクの重み係数ｒ(i)_jは連続的に変化するので、第ｉ代表接触面ヤコビアン行列Ｊc(i)が、不連続的な変化を生じることがない。このため、移動体の各関節の変位量を滑らかに連続的に変化させることができる。ひいては、移動体の運動を滑らかに行なうことができる。

よって、第１発明によれば、複数の接触対象面に接触する動作を行なう移動体に各接触対象面から実際に作用する全接触力を目標値に追従させるように該移動体の運動を制御することを、該移動体の各接触対象面に対する各接触部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行うことができる。

上記第１発明においては、前記全接触力要求修正量決定手段は、前記第ｉ全接触力要求修正量（ｉ＝１，２，…，Ｎ）のそれぞれを、例えば第ｉ接触対象面における前記偏差を積分することにより決定する（第２発明）。

そして、より好ましくは、前記全接触力要求修正量決定手段は、前記第ｉ全接触力要求修正量（ｉ＝１，２，…，Ｎ）のそれぞれを、少なくとも第ｉ接触対象面における前記偏差に比例する比例項と、該偏差を積分してなる積分項とを合成することにより決定する（第３発明）。

すなわち、各接触対象面から前記移動体に実際に作用する全接触力（実全接触力）の観測値と、前記目標全接触力との偏差には、目標運動の作成に際して想定されている想定接触対象面に対する実際の接触対象面の全体的な位置及び姿勢の定常的なずれに起因する定常的な偏差と、想定接触対象面で想定されていない実際の接触対象面の局所的な凹凸等に起因する一時的な偏差とがある。

そして、第２発明によれば、前者の偏差を補償することができる。また、第３発明によれば、前者の偏差を前記積分項により補償することができると共に、後者の偏差を前記比例項により補償することができる。

上記第２発明又は第３発明において、第ｉ接触対象面（ｉ＝１，２，…，Ｎ）において、実全接触力の観測値と、目標全接触力との偏差を積分したもの（以降、これを↑ΔＦＭt(i)_intと表記する）は、第ｉ接触対象面に対応して移動体の目標運動で想定されている想定接触対象面である想定第ｉ接触対象面の位置及び姿勢と、実際の第ｉ接触対象面の位置及び姿勢との定常的な差分に応じたものとなる。このため、当該偏差の積分値↑ΔＦＭt(i)_intを、前記式（２８）により変換することで算出される↑Ｘc(i)、すなわち、↑ΔＦＭt(i)_intから、次式（２８−１）により算出される↑Ｘc(i)_intは、想定第ｉ接触対象面の位置及び姿勢と、実際の第ｉ接触対象面の位置及び姿勢との定常的な誤差分に相当するものとなると考えられる。

なお、式（２８−１）においては、↑Ｘc(i)_intのうちの並進変位量の成分、回転変位量の成分をそれぞれ↑Ｘc_org(i)_int、↑Ｘc_rot(i)_intと表記し、↑ΔＦＭt(i)_intのうちの並進力成分、モーメント成分をそれぞれ↑ΔＦt(i)_int、↑ΔＭt(i)_intと表記している。

従って、想定第ｉ接触対象面の位置及び姿勢を上記↑Ｘc(i)_intにより補正することで、実際の第ｉ接触対象面の位置及び姿勢を推定できることとなる。

そこで、前記第２発明においては、前記第１〜第Ｎ接触対象面のうちのあらかじめ定められた特定の接触対象面である第ｈ接触対象面に対応して前記目標運動で想定されている接触対象面である想定第ｈ接触対象面の位置及び姿勢を、前記代表接触面位置姿勢変位量算出手段により算出された第ｈ代表接触面の前記要求変位量に応じて補正することにより実際の第ｈ接触対象面の位置及び姿勢を推定する接触対象面推定手段をさらに備えるようにしてもよい（第４発明）。

また、前記第３発明においては、前記第１〜第Ｎ接触対象面のうちのあらかじめ定められた特定の接触対象面である第ｈ接触対象面に対応して前記目標運動で想定されている接触対象面である想定第ｈ接触対象面の位置及び姿勢を補正することにより、実際の第ｈ接触対象面の位置及び姿勢を推定する接触対象面推定手段をさらに備えており、該接触対象面推定手段は、第ｈ接触対象面に対応する前記第ｈ全接触力要求修正量を組成する前記積分項と該第ｈ接触対象面に対応する前記第ｈ代表接触面のばね定数とから、該積分項に対応する第ｈ代表接触面の位置及び姿勢の変位量である第ｈ代表接触面定常変位量を算出し、該第ｈ代表接触面定常変位量に応じて前記想定第ｈ接触対象面の位置及び姿勢を補正することにより実際の第ｈ接触対象面の位置及び姿勢を推定するようにしてもよい（第５発明）。

なお、第４発明及び第５発明において、第ｈ接触対象面は、第１〜第Ｎ接触対象面のうちの１つ又は一部の接触対象面で良いことはもちろんであるが、第１〜第Ｎ接触対象面の全てであってもよい。

前記第４発明においては、移動体のコンプライアンス制御が前記した如く行われている状態において、第１〜第ｎ接触対象面のうちの任意の１つの第ｈ接触対対象面に関し、前記全接触力要求修正量決定手段により決定される第ｈ全接触力要求修正量は、第ｈ接触対象面での実全接触力の観測値と、目標全接触力との偏差を積分してなる値（積分項）であるから、該第ｈ全接触力要求修正量と前記第ｈ代表接触面のばね定数とから、前記代表接触面位置姿勢変位量算出手段により算出される第ｈ代表接触面の位置及び姿勢の要求変位量は、前記目標運動で想定されている第ｈ接触対象面である想定第ｈ接触対象面の位置及び姿勢と、実際の第ｈ接触対象面の位置及び姿勢と間の定常的な差分に相当するものとなる。すなわち、該要求変位量は、前記式（２８−１）における↑Ｘc(k)_intに相当するものである。

従って、第４発明においては、想定第ｈ接触対象面の位置及び姿勢を、前記代表接触面位置姿勢変位量算出手段により算出された第ｈ代表接触面の前記要求変位量に応じて補正することにより、実際の第ｈ接触対象面の位置及び姿勢を推定できることとなる。

また、前記第５発明においては、移動体のコンプライアンス制御が前記した如く行われている状態において、任意の１つの第ｈ接触対対象面に関して前記全接触力要求修正量決定手段により決定される第ｈ全接触力要求修正量のうちの積分項と前記第ｈ代表接触面のばね定数とから、前記代表接触面位置姿勢変位量算出手段と同様の演算によって算出される第ｈ代表接触面の位置及び姿勢の変位量（第ｈ全接触力要求修正量の代わりに前記積分項を用いて算出される第ｈ代表接触面の位置及び姿勢の変位量）は、前記想定第ｈ接触対象面の位置及び姿勢と、実際の第ｈ接触対象面の位置及び姿勢と間の定常的な差分に相当するもの、すなわち、前記式（２８−１）における↑Ｘc(k)_intに相当するものとなる。

従って、第５発明においては、第ｈ全接触力要求修正量に対応する前記積分項と前記第ｈ代表接触面のばね定数とから、該積分項に対応する第ｈ代表接触面定常変位量を算出し、想定第ｈ接触対象面の位置及び姿勢を、前記第ｈ代表接触面定常変位量に応じて補正することにより、実際の第ｈ接触対象面の位置及び姿勢を推定できることとなる。

この場合、前記のコンプライアンス制御によって、前記第ｈ全接触力要求修正量を確実に実現し得るような関節変位修正量を決定して、前記移動体の動作を制御できることから、第４発明における第ｈ全接触力要求操作量、又は第５発明における第ｈ代表接触面定常変位量は、想定第ｈ接触対象面の位置及び姿勢と、実際の第ｈ接触対象面の位置及び姿勢と間の定常的な差分に相当するものとしての信頼性が高いものとなる。従って、実際の第ｈ接触対象面の位置及び姿勢を精度よく推定することができることとなる。

上記第１〜第５発明において、前記総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1は、公知の適宜の手法により算出することができる。

ただし、前記算出された総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1は、あらかじめ設定された重み行列Ｗと前記算出された総合ヤコビアン行列Ｊcとから次式（３１）により算出される行列であると共に、該式（３１）におけるｋの値を、次式（３２）により表される行列式ＤＥＴが所定の正の閾値以上の値になるように決定する擬似逆行列演算用パラメータ決定手段を備えており、

Ｊc^-1＝Ｗ^-1・Ｊc^T・(Ｊc・Ｗ^-1・Ｊc^T＋ｋ・Ｉ)^-1 ……（３１）
ＤＥＴ＝det(Ｊc・Ｗ^-1・Ｊc^T＋ｋ・Ｉ) ……（３２）
ただし、Ｗ：あらかじめ定められた重み行列（対角行列）

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

なお、式（３１）において、Ｉは単位行列である。また、重み係数行列Ｗは、各関節の変位量の変化に対する代表接触面の位置及び姿勢の変化の応答性等を考慮し、代表接触面の位置及び姿勢の要求変位量を実現するための各関節の変位量の修正度合いを各関節毎に調整するものである。なお、重み係数行列Ｗは単位行列であってもよい。

また、ｋは、式（３１）の右辺の括弧内の行列の行列式、すなわち、式（３２）に示される行列式ＤＥＴの大きさが小さくなり過ぎるのを防止するための調整パラメータであり、ゼロ以上の実数値である。この調整パラメータｋについて補足すると、擬似逆行列Ｊc^-1は、基本的には、ｋ＝０とした場合の上記の式（３１）によって算出することができる。

ただし、この場合、上記行列式ＤＥＴの大きさが小さくなり過ぎる（ゼロに近い値となる）場合がある。そして、このような場合には、式（３１）の右辺の括弧内の行列の逆行列が発散し、適切な擬似逆行列Ｊc^-1を決定することができなくなる。これを防止するために、式（３１）では、右辺の括弧内の第１項に、単位行列Ｉのｋ倍の行列を付加するようにしている。

一方、前記行列式ＤＥＴの大きさが小さくなり過ぎないようにするための適切な調整パラメータｋの値はＪcに応じて変化する。また、行列式ＤＥＴの大きさは、ｋの値の変化に対して非線形な変化を呈する。

このため、第３発明では、上記行列式ＤＥＴの絶対値が前記所定の閾値以上となる（過小にならない）ｋの値を、前記擬似逆行列演算パラメータ決定手段により探索的に決定する。

具体的には、ｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（３１）により擬似逆行列を算出するために用いるｋの値として決定する。

この場合、この処理において、ｋの暫定値の増加量が一定であると、ｋの値が最終的に決定されるまでに時間がかかりすぎたり、あるいは、移動体の制御装置の制御処理周期毎に決定されるｋの値が頻繁に変動し、ひいては、算出される擬似逆行列Ｊc^-1の不連続的な変動が生じやすい。

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

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

これにより、第６発明によれば、移動体の制御装置の制御処理周期毎に、擬似逆行列Ｊc^-1を算出するために用いる適切なｋの値（ＤＥＴの絶対値が所定の閾値以上となるｋの値）を効率よく短時間で決定できると共に、擬似逆行列Ｊc^-1が滑らかに変化していくようにすることができる。ひいては、移動体の各関節の変位量を滑らかに変化させるように、関節変位修正量を決定できることとなる。

また、第６発明によれば、前記重み係数行列Ｗによって、各関節の変位量の変化に対する代表接触面の位置及び姿勢の変化の応答性等を考慮して、代表接触面の位置及び姿勢の要求変位量を実現するための各関節の変位量の修正度合いを各関節毎に調整することができる。

本発明を説明するための移動体と、これに作用する床反力とを一般化して模式的に示す図。 本発明の一実施形態における移動体の概略構成を示す図。 図２の移動体の各可動リンクの構造を示す図。 図２の移動体の制御に関する構成を示すブロック図。 図２に示す制御装置の機能を示すブロック図。 図２に示す移動体の動作例を示す図。 図５に示す関節変位修正量決定部に関する処理を示すフローチャート。 本発明を適用する移動体の他の構成形態の例を示す図。 本発明を適用する移動体の他の構成形態の例を示す図。

本発明の一実施形態を図２〜図７を参照して説明する。

図２を参照して、本実施形態で例示する移動体１は、基体２と、該基体２から延設された複数の可動リンク３ａ〜３ｄ（以降、総称的に可動リンク３ということがある）とを備える移動ロボットである。本実施形態の例では、可動リンク３の個数は４個である。

各可動リンク３は、移動体１の脚又は腕として機能し得るリンク機構である。各可動リンク３は、複数の要素リンク４ａ〜４ｃ（以降、総称的に要素リンク４ということがある）と、これらの要素リンク４を基体２側から順番に連接する複数の関節部５ａ〜５ｃ（以降、総称的に関節部５ということがある）とから構成されている。

本実施形態の例では、各可動リンク３を構成する要素リンク４及び関節部５のそれぞれの個数は３個である。そして、３個の要素リンク４ａ〜４ｃのうちの要素リンク４ａが関節部５ａを介して基体２に連結され、要素リンク４ｂが関節部５ｂを介して要素リンク４ａに連結され、要素リンク４ｃが関節部５ｃを介して要素リンク４ｂに連結されている。

要素リンク４ａ〜４ｃのうち、可動リンク３の先端部を構成する要素リンク４ｃは、移動体１の運動時に、該移動体１の動作環境に存在する床面や壁面等の接触対象面に接触させる部分（すなわち、足平部又は手平部に相当する部分）である。この要素リンク４ｃ（以下、可動リンク３の先端部４ｃということがある）は、本実施形態の例では、概略板状に形成されており、該要素リンク４ｃの床面や壁面等に接触させる接触面が平坦面となっている。そして、要素リンク４ｃの接触面には、ゴム等から成る弾性部材１０が取り付けられている。このため、要素リンク４ｃが、該弾性部材６を介して床面や壁面等の接触対象面に接触するようになっている。

関節部５ａ〜５ｃのそれぞれは、例えば図３に示す如く、１つ又は複数の関節により構成されている。具体的には、関節部５ａは、例えば３つの関節７ａ，７ｂ，７ｃにより構成され、３軸周りの回転自由度を有するものとされている。また、関節部５ｂは、例えば１つの関節７ｄにより構成され、１軸周りの回転自由度を有するものとされている。また、関節部５ｃは、例えば２つの関節７ｅ，７ｆにより構成され、２軸周りの回転自由度を有するものとされている。従って、各可動リンク３は、トータル６自由度の運動自由度を有するように構成されている。これにより、各可動リンク３は、空間的な運動を行なうことが可能となっている。

なお、各可動リンク３の構成は、上記の構成に限られるものではない。例えば、可動リンク３は、回転型の関節以外に直動型の関節を含んでいてもよい。また、各可動リンクは７自由度以上の運動自由度を有するように構成されていてもよい。また、各可動リンク３の構造（要素リンク４のサイズや個数、関節１１の構造や個数）は、全ての可動リンク３で同じである必要はない。例えば可動リンク３ａ，３ｂを主に脚として使用し、可動リンク３ｃ，３ｄを主に腕として使用するような場合には、可動リンク３ａ，３ｂと、可動リンク３ｃ，３ｄとが互いに異なる構造のものであってもよい。

図２及び図３では図示を省略するが、移動体１には、上記した各関節７ａ〜７ｆ（以降、総称的に関節７ということがある）をそれぞれ駆動する関節アクチュエータとしての複数の電動モータ８（図４に示す）が搭載されている。各電動モータ８は、それぞれに対応する関節７に、減速機を含む動力伝達機構（図示省略）を介して駆動力（回転駆動力）を伝達するように接続されている。

なお、関節アクチュエータは、電動モータ以外のアクチュエータ、例えば油圧式のアクチュエータであってもよい。

以上の構成を有する移動体１は、各可動リンク３の各関節７を電動モータ８により駆動することによって、各可動リンク３の空間的な運動が行なわれる。この運動によって移動体１の運動（移動を含む）を行なうことが可能となっている。例えば、可動リンク３ａ〜３ｄのうちの２つの可動リンク３ａ，３ｂを脚リンクとし、人の歩行動作と同様の形態（歩容）で可動リンク３ａ，３ｂを運動させることによって、移動体１の歩行動作を行なうことが可能である。また、可動リンク３ｃ，３ｄを腕リンクとして動かすことによって、壁面を押す等の作業を行なうことが可能である。あるいは、４つの可動リンク３ａ〜３ｄを脚リンクとし、これらの可動リンク３ａ〜３ｄを４足動物と同様の形態で動かすことによって、床面上を移動するようにすることも可能である。

補足すると、移動体１の基体２には、可動リンク３以外に、頭部等が連結されていてもよい。また、例えば、基体２を２つの半体で構成し、これらの半体を関節を介して連結してもよい。

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

この場合、センサとしては、図４に示すように、移動体１の基体２の姿勢角（鉛直方向に対する傾斜角度やヨー軸周りの方位角）やその時間的変化率（角速度）等を計測するために該基体２に搭載された姿勢センサ５１と、各可動リンク３の先端部４ｃが外部の接触対象面に接触した状態で該接触対象面から受ける接触力（反力）を計測するために各可動リンク３の関節部５ｃと先端部（要素リンク）４ｃとの間に介装された力センサ５２とが備えられている。

上記姿勢センサ５１は、例えば３軸周りの角速度を検出するジャイロセンサと３軸方向の加速度を検出する加速度センサとから構成される。また、各力センサ５２は、例えば３軸方向の並進力及び３軸周りのモーメントを検出する６軸力センサにより構成される。

また、図２及び図３では図示を省略したが、移動体１には、各関節７の変位量（回転角度）を検出するセンサとして、例えばロータリーエンコーダ５３（図４に示す）が備えられている。なお、各関節７の変位量（回転角度）を検出するセンサとしては、ポテンショメータ等の他のセンサを用いてもよい。

制御装置５０には、上記の各センサ５１，５２，５３の出力が入力される。そして、制御装置５０は、これらの入力値から認識される移動体１の動作状態の観測値（基体２の姿勢角やその時間的変化率（角速度）の計測値、基体２の移動速度の計測値、各可動リンク３の先端部４ｃに作用する接触力の計測値、各関節７の変位量やその時間的変化率の計測値など）を使用して、移動体１に目標とする運動を行なわせるための各関節７の変位量の目標値（以下、関節変位指令という）を決定すると共に、この関節変位指令に応じて、各関節７の実際の変位量（実変位量）を電動モータ８を介して制御する。

かかる制御処理を実行する制御装置５０は、実装されるプログラムにより実現される主要な機能として、図５に示す如く、基準歩容生成部６１、姿勢安定化補償力決定部６２、目標全接触力決定部６３、全接触力要求修正量決定部６４、代表接触面並進・回転変位量算出部６５、代表接触面ヤコビアン行列算出部６６、目標関節変位量決定部６７、関節変位修正量決定部６８、関節変位制御部６９、接触対象面推定部７０を備える。

そして、制御装置５０は、上記基準歩容生成部６１、姿勢安定化補償力決定部６２、目標全接触力決定部６３、全接触力要求修正量決定部６４、代表接触面並進・回転変位量算出部６５、代表接触面ヤコビアン行列算出部６６、目標関節変位量決定部６７、関節変位修正量決定部６８及び関節変位制御部６９の処理を所定の演算処理周期で逐次実行することにより、各関節の関節変位指令を逐次決定し、この関節変位指令に応じて、図示しないモータ駆動回路を介して電動モータ８を制御する。

さらに、制御装置５０は、上記の制御処理と並行して、接触対象面推定部７０の処理を実行することにより、移動体１の動作環境に存在する実際の接触対象面のうちの特定の接触対象面の位置及び姿勢を推定する。

以下に、移動体１に図６に示すような運動を行なわせる場合を一例として、上記各機能部の詳細な処理を含めて制御装置５０の処理を説明する。

［基準歩容生成部６１の処理］
本実施形態では、制御装置５０は、基準歩容生成部６１により、移動体１の基準の目標歩容としての基準歩容を生成する。

この基準歩容は、移動体１の各部の目標とする空間的な位置及び空間的な姿勢（空間的な向き）の軌道を規定する（ひいては移動体１の各関節の目標とする変位量の軌道を規定する）目標運動と、この目標運動を実現するために移動体１に接触対象面から作用させるべきトータルの外力である総外力の軌道を規定する目標総外力とから構成される。なお、「軌道」は、瞬時値の時系列を意味する。

基本歩容生成部６１が生成する基準歩容のうちの目標運動は、本実施形態では、床面等を含む複数の接触対象面が存在する動作環境で移動体１を移動させる目標運動である。この目標運動には、複数の接触対象面のそれぞれに１つ以上の可動リンク３を接触させ、それにより、複数の接触対象面から反力としての接触力を移動体１が受けることとなる運動状態が含まれる。

このような目標運動の一例として、本実施形態では、図６に示すような動作を移動体１に行なわせる目標運動を例示する。

この目標運動では、移動体１は、基本的には、その動作環境に存在する１つの接触対象面としての床面ＦＬ上で２つの可動リンク３ａ，３ｂを脚として動かす歩行動作を行なうことによって、破線Ａで示す如き移動経路で床面ＦＬ上を移動する。

この場合、移動経路Ａは、押し開き可能なドアＤが取り付けられた壁ＷＬの開口ＷＬａを通過する経路とされている。そして、上記目標運動では、移動体１は、開口ＷＬａを通過する際に、ドアＤを押し開く動作を行なう。この動作では移動体１は、ドアＤに近接して対面した状態で可動リンク３ｃ，３ｄのうちの一方、例えば可動リンク３ｄの先端部４ｃをドアＤの表面Ｄａ（以下、ドア表面Ｄａという）に接触させ、この接触状態で、可動リンク３ｄによりドアＤを押すことによって、該ドアＤを開く。このようにドアＤを開いた後に、移動体１は、開口ＷＬａを通って移動する。

このような目標運動では、ドアＤを押し開く時に、移動体１は、互いに異なる２つの接触対象面としての床面ＦＬと、ドア表面Ｄａとに接触して、これらの床面ＦＬ及びドア表面Ｄａから反力としての接触力を受けることとなる。

上記の如き動作を移動体１に行なわせる目標運動は、移動体１の動作環境に存在する実際の床面等の接触対象面の形状（接触対象面の各所の位置及び姿勢）に合致するものとしてあらかじめ位置及び姿勢が定められた接触対象面モデルである想定接触対象面に移動体１の可動リンク３を接触させるように生成されるものである。なお、本実施形態では、目標運動を作成するために使用する想定接触対象面は、接触対象面推定部７０で実際の接触対象面を表すものとして推定された接触対象面（推定接触対象面）に応じて定期的に（例えば移動体１の移動時に１歩毎又は複数歩毎に、あるいは、所定時間毎に）更新される。

この目標運動は、本実施形態の移動体１では、各可動リンク３の先端部（要素リンク）４ｃの目標位置及び目標姿勢の軌道である目標可動リンク位置姿勢軌道と、基体２の目標位置及び目標姿勢の軌道である目標基体位置姿勢軌道と、移動体１の全体重心の目標位置の軌道である目標重心位置軌道と、移動体１の全体重心の周りの角運動量の目標値の軌道である目標角運動量軌道とから構成される。

なお、可動リンク３の先端部（要素リンク）４ｃの「位置」は、該先端部４ｃの空間的な位置を代表的に示すものとして任意に定められた該先端部４ｃの代表点の位置（例えば先端部４ｃの接触面の特定の位置の点）の位置を意味する。基体２の「位置」についても同様である。また、可動リンク３の先端部４ｃの「姿勢」は、該先端部４ｃの空間的な向きを意味する。基体２の「姿勢」についても同様である。

また、上記の目標可動リンク位置姿勢及び目標基体位置姿勢は、移動体１の動作環境の床面等の任意の静止物に対して固定された慣性座標系としてのグローバル座標系で見た位置及び姿勢として表される。このグローバル座標系としては、例えば移動体１の支持脚（移動体１の重量を床に支える脚）としての１つの可動リンク３の接地面（床面との接触面）内の点を原点とし、該可動リンク３の先端部（要素リンク）４ｃの前後方向の水平軸をＸ軸、鉛直方向の軸をＺ軸、これらに直交する方向の水平軸をＹ軸とする支持脚座標系が用いられる。

この場合、移動体１の歩行動作においては、支持脚が切替わる毎に、グローバル座標系の原点の位置とＸ軸及びＹ軸の方向とが更新されることとなる。ただし、グローバル座標系は、床面等の任意の静止物に対して定常的に固定された座標系であってもよい。なお、以降の説明では、説明の便宜上、Ｘ軸、Ｙ軸、Ｚ軸は特にことわらない限り、上記支持脚座標系の３軸を意味するものとする。

補足すると、前記目標重心位置軌道及び目標角運動量軌道は、各可動リンク３の目標可動リンク位置姿勢軌道と、目標基体位置姿勢軌道とに応じて従属的に決まるものであり、移動体１の各関節７の変位量を直接的に規定するものではない。従って、これらの目標重心位置軌道及び目標角運動量軌道は、必ずしも、基準歩容生成部６１で生成する基本歩容の目標運動の構成要素とする必要はない。本実施形態において、基準歩容生成部６１で目標重心位置軌道及び目標角運動量軌道を生成するのは、これらの目標重心位置軌道及び目標角運動量軌道を後述する姿勢安定化補償力決定部６２の処理で使用するためである。

本実施形態では、基準歩容生成部６１は、図６に示したような形態で移動体１を動作させるための基準歩容の目標運動を例えば次のように生成する。

ドアＤを押し開く動作の前後において移動体１の歩行動作を行なうための目標運動の生成処理では、可動リンク３ｃ，３ｄに関する目標可動リンク位置姿勢軌道は、例えば基体２に対する可動リンク３ｃ，３ｄの全体の相対的な姿勢を定常的に一定に保つように決定される。そして、脚としての可動リンク３ａ，３ｂに関する目標可動リンク位置姿勢軌道と、目標基体位置姿勢軌道とは、例えば、本願出願人が特許第３７２６０８１号等に提案した手法と同様の手法で生成される。

この場合の可動リンク３ａ，３ｂの目標可動リンク位置姿勢軌道及び目標基体位置姿勢軌道の生成処理を概略的に説明すると、可動リンク３ａ，３ｂの足平部に相当する先端部４ｃの想定床面（床面に対応する想定接触対象面）に対する着床予定位置及び着床予定時刻等、目標可動リンク位置姿勢軌道を規定するパラメータが、移動体１の外部から制御装置５０に与えられる移動体１の移動方向や移動速度の要求、あるいは移動計画等に応じて決定される。なお、上記パラメータを制御装置５０に外部から入力し、あるいは、あらかじめ制御装置５０の記憶装置に記憶保持しておいてもよい。

そして、このパラメータにより規定される可動リンク３ａ，３ｂの目標可動リンク位置姿勢軌道に応じて、ＺＭＰ（Zero Moment Point）の目標位置である目標ＺＭＰの軌道が決定される。目標ＺＭＰ軌道は、目標ＺＭＰが可動リンク３ａ，３ｂの目標可動リンク位置姿勢軌道に応じて規定される想定床面上の支持多角形内の概ね中央付近の位置に極力収まり、且つ、滑らかに（ステップ状の変化を生じることなく）変位していくように決定される。

さらに、移動体１の動力学（外力としての床反力と移動体１の運動との間の関係）を表現する適宜の動力学モデルを用いて、目標ＺＭＰを満足するように（移動体１の運動によって発生する慣性力と移動体１に作用する重力との合力が目標ＺＭＰ周りに発生するモーメントの水平成分（Ｘ軸周り及びＹ軸周りの成分）がゼロになるように）目標基体位置姿勢軌道が決定される。

さらに、各可動リンク３の目標可動リンク位置姿勢軌道と目標基体位置姿勢軌道とから移動体１の幾何学モデル（剛体リンクモデル）を用いて、移動体１の目標重心位置軌道と目標重心位置の周りの目標角運動量軌道とが算出される。なお、移動体１の剛体リンクモデルは、その各要素リンクに質量やイナーシャが設定されたモデルである。

また、ドアＤを押し開く動作を行なうための目標運動の生成処理では、脚としての可動リンク３ａ，３ｂに関する目標可動リンク位置姿勢軌道は、例えば可動リンク３ａ，３ｂのそれぞれの先端部４ｃが、ドアＤの手前で想定床面に接触（接地）して静止した状態に保たれるように決定される。さらに、目標ＺＭＰが想定床面上の支持多角形内の概ね中央付近の位置に収まるように、目標ＺＭＰ軌道が決定される。

また、可動リンク３ｃに関する目標可動リンク位置姿勢軌道は、例えば基体２に対する可動リンク３ｃの全体の相対的な姿勢を定常的に一定に保つように決定される。

そして、ドアＤを押し開くための可動リンク３ｄに関する目標可動リンク位置姿勢軌道は、該可動リンク３ｄの先端部４ｃをドア表面Ｄの所定の箇所に接触させ、続いて、あらかじめ定められた開動作パターンでのドアＤの開動作（ヒンジ周りの回転運動）に追従して可動リンク３ｄの先端部４ｃを動かすように決定される。

さらに、ドアＤの開動作とドア表面Ｄａに作用する外力（又はドア表面Ｄａから移動体１が受ける反力）との間の関係を表現する適宜の動力学モデルを用いて、ドアＤの開動作時に移動体１が受ける反力（接触力）の推定値が求められる。

そして、移動体１の動力学（接触対象面としての床面ＦＬとドア表面Ｄａとから移動体１に作用する接触力（反力）と、移動体１の運動との間の関係）を表現する適宜の動力学モデルを用いて、目標ＺＭＰを満足するように（移動体１の運動によって発生する慣性力と移動体１に作用する重力とドア表面Ｄａから移動体１が受ける接触力との合力が目標ＺＭＰ周りに発生するモーメントの水平成分（Ｘ軸周り及びＹ軸周りの成分）がゼロになるように）目標基体位置姿勢軌道が決定される。

さらに、移動体１の歩行動作の場合と同様に、各可動リンク３の目標可動リンク位置姿勢軌道と目標基体位置姿勢軌道とから移動体１の幾何学モデル（剛体リンクモデル）を用いて、移動体１の目標重心位置軌道と目標重心位置の周りの目標角運動量軌道とが算出される。

補足すると、移動体１の目標運動を生成する手法は、上記の如き手法に限られるものではなく、想定接触対象面を設定した動作環境で移動体１が実現可能な目標運動を生成し得るものであれば、他のどのような手法によって生成してもよい。

また、移動体２の歩行動作時において、基体２に対する可動リンク３ｃ，３ｄの相対的な姿勢を、必要に応じて所望の形態で変化させるように決定するようにしてもよい。例えば両可動リンク３ｃ，３ｄを移動体１の歩行動作に合せて前後に振るようにしてもよい。その場合、可動リンク３ｃ，３ｄの基体２に対する相対的な姿勢の変化に伴う移動体１の全体重心の位置の変化や、該全体重心の周りの角運動量の変化を考慮した動力学モデルを用いて目標基体位置姿勢軌道を生成するようにしてもよい。

また、ドアＤの開動作時においては、例えばドアＤの開動作に伴い、脚としての可動リンク３ａ，３ｂをドアＤに近づけていくように可動リンク３ａ，３ｂの目標可動リンク位置姿勢軌道を決定するようにしてもよい。さらに、可動リンク３ｃ，３ｄの両方をドア表面Ｄａに接触させてドアＤの開動作を行なうように目標運動を決定するようにしてもよい。

また、目標運動の構成要素は、上記のものに限られるものではない。例えば可動リンク３以外に、上体２に対して可動な部位（頭部等）が移動体１に備えられている場合には、その部位の目標位置や目標姿勢の軌道も目標運動の構成要素して付加される。また、例えば、各可動リンク３が７自由度以上の自由度を有する場合には、目標可動リンク位置姿勢軌道に加えて、例えば各可動リンク３の中間部の部位の目標位置姿勢軌道を目標運動の構成要素として付加してもよい。

目標運動の構成要素は、それによって移動体１の各関節の変位量の軌道を規定し得るものであればよく、移動体１の構造等に応じて適宜定めればよい。

基準歩容のうちの目標総外力は、移動体１を目標運動に従って運動させるために、動力学的に移動体１に作用させることが必要となるトータルの外力（重力を除く）の軌道を規定するものである。その目標総外力は、移動体１に作用させるトータルの並進力の目標値である目標総並進外力と、移動体１に作用させるトータルのモーメントの目標値である目標総モーメント外力とから構成される。なお、目標総モーメント外力は、より詳しくは、慣性座標系の任意の基準点（例えば前記支持脚座標系の原点）周りのモーメントとして表現されるものである。

以降の説明では、基準歩容の目標総外力のうちの目標総並進外力（ベクトル）を↑Ｆt_cmd0、目標総モーメント外力（ベクトル）を↑Ｍt_cmd0と表記する。本実施形態では、↑Ｆt_cmd0，↑Ｍt_cmd0は、それぞれ、Ｘ軸成分、Ｙ軸成分、及びＺ軸成分の３成分からなる縦ベクトルである。また、↑Ｆt_cmd0と↑Ｍt_cmd0とから構成される目標総外力を↑ＦＭt_cmd0と表記する。この場合、↑ＦＭt_cmd0は、↑Ｆt_cmd0，↑Ｍt_cmd0の各成分を並べた縦ベクトル（６成分の縦ベクトル）であるとする。すなわち、↑ＦＭt_cmd0＝[↑Ｆt_cmd0，↑Ｍt_cmd0]^Tであるとする。

基本歩容のうちの目標総外力↑ＦＭt_cmd0のうちの目標総並進外力↑Ｆt_cmd0は、移動体１の全体重心に作用する重力（移動体１の全体質量と、重力加速度ベクトルとの積）と、移動体１の全体重心の運動（並進運動）によって発生する慣性力（移動体１の全体重心の並進加速度ベクトルと、全体質量との積の符号を反転させたもの）との合力に釣り合う並進力（当該合力の符号を反転させた並進力）として決定される。この場合、上記慣性力は、移動体１の目標重心位置軌道と、移動体１の全体質量の値とを用いて算出される。

また、目標総外力↑ＦＭt_cmd0のうちの目標総モーメント外力↑Ｍt_cmd0は、移動体１の全体重心の並進運動によって慣性座標系の基準点周りに発生する慣性力モーメントと、移動体１の目標運動によって該移動体１の全体重心周りに発生する慣性力モーメントとの合成モーメントに釣り合う釣り合うモーメント（当該合成モーメントの符号を反転させたモーメント）として決定される。

この場合、移動体１の全体重心の並進運動によって基準点周りに発生する慣性力モーメントは、基準点に対する移動体１の目標重心位置の位置ベクトルと、移動体１の全体質量の値とを用いて算出される。また、移動体１の目標運動によって該移動体１の全体重心周りに発生する慣性力モーメントは、移動体１の目標運動と、移動体１の剛体リンクモデル（各要素リンクに質量やイナーシャを持つモデル）とを用いて算出される。

以上が、基準歩容生成部６１の処理である。補足すると、前記基準歩容は、移動体１の移動動作中に生成する必要はなく、該移動体１の移動開始前に事前に作成しておき、それを制御装置５０の記憶装置に事前に記憶保持したり、外部から制御装置５０に適宜、無線通信によって入力するようにしてもよい。このようにした場合、制御装置５０に基準歩容生成部６１を備える必要はない。

［目標関節変位量決定部６７の処理］
制御装置５０は、前記の如く生成された基準歩容のうちの目標運動（より詳しくは、目標運動のうちの、各可動リンク３の目標可動リンク位置姿勢軌道、及び目標基体位置姿勢軌道）を、目標関節変位量決定部６７に入力し、該目標関節変位量決定部６７の処理を実行する。この目標関節変位量決定部６７は、基準歩容の目標運動によって規定される移動体１の各関節の変位量である基準の目標変位量を算出する機能部である。この場合、目標関節変位量決定部６７は、入力された目標運動から、逆運動学の演算処理によって移動体１の各関節の目標変位量を算出する。

なお、基準歩容を事前に作成しておく場合には、この基準歩容の目標運動に対応する各関節の目標変位量も事前に作成しておいてもよい。その場合には、制御装置５０に目標関節変位量決定部６７を備える必要はない。

［姿勢安定化補償力決定部６２の処理］
制御装置５０は、目標関節変位量決定部６７の処理と並行して（又は該処理の前もしくは後に）、姿勢安定化補償力決定部６２、目標全接触力決定部６３、全接触力要求修正量決定部６４、代表接触面並進・回転変位量算出部６５、代表接触面ヤコビアン行列算出部６６及び関節変位修正量決定部６８の処理を実行する。これらの処理は、コンプライアンス制御のための操作量（制御入力）を決定するための処理であり、以降、コンプライアンス操作量決定処理という。そして、制御装置５０は、このコンプライアンス操作量決定処理によって、前記基準歩容に対応する各関節の基準の目標変位量を修正するための関節変位修正量を決定する。

このコンプライアンス操作量決定処理では、制御装置５０は、まず、姿勢安定化補償力決定部６２の処理を実行する。この姿勢安定化補償力決定部６２は、移動体１の目標運動における所要の運動状態量の目標値と、実際の運動状態量の値との偏差をゼロに近づけるために、移動体１に付加的に作用させるべき外力である姿勢安定化補償力を基準歩容の目標総外力↑ＦＭt_cmd0を修正するための修正量として決定する機能部である。

上記運動状態量は、移動体１の姿勢を安定化するために重要なものとしてあらかじめ定められた種類の状態量である。その運動状態量としては、本実施形態では、移動体１の全体重心の位置と、移動体１の全体重心の周りの角運動量とを採用した。そして、姿勢安定化補償力決定部６２は、基準歩容における目標重心位置と、移動体１の実際の重心位置（実重心位置）の観測値との偏差をゼロに近づけるために移動体１に付加的に作用させるべき並進外力↑Ｆg_dmd（以降、補償並進外力↑Ｆg_dmdという）と、基準歩容における目標角運動量と移動体１の実際の重心周りの角運動量の観測値との偏差をゼロに近づけるために移動体１に付加的に作用させるべきモーメント外力↑Ｍg_dmd（以下、補償モーメント外力↑Ｍg_dmdという）との組を、姿勢安定化補償力として決定する。以降の説明では、補償並進外力↑Ｆg_dmdと補償モーメント外力↑Ｍg_dmdとを並べたベクトルを↑ＦＭg_dmd（＝[↑Ｆg_dmd，↑Ｍg_dmd]^T）と表記し、これを姿勢安定化補償力と呼ぶ。

上記姿勢安定化補償力↑ＦＭg_dmdのうちの補償並進外力↑Ｆg_dmdは、基準歩容における目標重心位置と、移動体１の実際の重心位置（実重心位置）の観測値との偏差から所定のフィードバック制御則により決定される。そのフィードバック制御則としては、本実施形態では、例えばＰＤ則が用いられる。

すなわち、本実施形態では、次式（３３）で示す如く、基準歩容生成部６１で生成された基準歩容の目標重心位置↑Ｐg_cmdと移動体１の実重心位置↑Ｐg_actの観測値との偏差に所定値のゲインＫpを乗じた比例項と、目標重心位置↑Ｐg_cmdの時間的変化率↑Ｐg'_cmdと移動体１の実重心位置↑Ｐg_actの時間的変化率↑Ｐg'_actの観測値（＝↑Ｐg_actの観測値の時間的変化率）との偏差に所定値のゲインＫvを乗じた微分項とを加え合わせることにより補償並進外力↑Ｆg_cmdが決定される。

↑Ｆg_dmd＝Ｋp・(↑Ｐg_cmd−↑Ｐg_act)＋Ｋv・(↑Ｐg'_cmd−↑Ｐg'_act)
……（３３）

なお、↑Ｆg_cmd，↑Ｐg_cmd，↑Ｐg_act，↑Ｐg'_cmd，↑Ｐg'_actは、いずれの３成分（３軸方向成分）のベクトルである。また、実重心位置↑Ｐg_actの観測値及びその時間的変化率は、前記ロータリーエンコーダ５３の出力により示される各関節７の実際の変位量（実変位量）の計測値と、移動体１の幾何学モデル（剛体リンクモデル）とを用いて算出される。

また、姿勢安定化補償力↑ＦＭg_dmdのうちの補償モーメント外力↑Ｍg_dmdは、基準歩容における目標角運動量と、移動体１の実重心位置の周りの角運動量の観測値との偏差から所定のフィードバック制御則により決定される。そのフィードバック制御則としては、本実施形態では、例えば比例則が用いられる。

すなわち、本実施形態では、次式（３４）で示す如く、基準歩容生成部６１で生成された基準歩容の目標角運動量↑Ｌg_cmdと、移動体１の実重心位置の周りの実際の角運動量（実角運動量）↑Ｌg_actの観測値との偏差に所定値のゲインＫLを乗じることにより、補償モーメント外力↑Ｍg_dmdが決定される。

↑Ｍg_dmd＝ＫL・(↑Ｌg_cmd−↑Ｌg_act) ……（３４）

なお、↑Ｍg_cmd，↑Ｌg_cmd，↑Ｌg_actはいずれも３成分（３軸周り成分）のベクトルである。また、実角運動量↑Ｌg_actの観測値は、前記ロータリーエンコーダ５３の出力により示される各関節７の実変位量の計測値と、移動体１の幾何学モデル（剛体リンクモデル）とを用いて算出される。

補足すると、補償並進外力↑Ｆg_dmdは、ＰＤ則以外のフィードバック制御則（例えば比例則）により決定するようにしてもよい。また、補償モーメント外力↑Ｍg_dmdは、比例則以外のフィードバック制御則（例えば、ＰＤ則）により決定するようにしてもよい。

また、本実施形態では、姿勢安定化補償力決定部６２に基準歩容生成部６１から目標重心位置↑Ｐg_cmdと目標角運動量↑Ｌg_cmdとを与えるようにしたが、これらの目標重心位置↑Ｐg_cmdと目標角運動量↑Ｌg_cmdとを姿勢安定化補償力決定部６２で、基準歩容の目標運動から算出するようにしてもよい。

［目標全接触力決定部６３の処理］
次いで、制御装置５０は、目標全接触力決定部６３の処理を実行する。この目標全接触力決定部６３は、上記の如く決定した姿勢安定化補償力↑ＦＭg_dmdにより基準歩容の目標総外力↑ＦＭt_cmd0を補正し、その補正後の目標総外力（以降、修正後目標総外力↑ＦＭt_cmd1という）を実現するために、移動体１が接触するＮ個（Ｎ≧１）の接触対象面のそれぞれから移動体１に作用させるべき目標全接触力↑ＦＭt(i)_cmd1（ｉ＝１，２，…，Ｎ）を決定する機能部である。

具体的には、目標全接触力決定部６３は、基準歩容生成部６１で生成された目標総外力↑ＦＭt_cmd0に、姿勢安定化補償力決定部６２で決定された姿勢安定化補償力↑ＦＭg_dmdを加え合せることによって、修正後目標総外力↑ＦＭt_cmd1を決定する。すなわち、↑ＦＭt_cmd0＋↑ＦＭg_dmdを↑ＦＭt_cmd1として決定する。

この場合、修正後目標総外力↑ＦＭt_cmd1は、並進外力↑Ｆt_cmd1とモーメント外力↑Ｍt_cmd1とから構成されるベクトル（＝[↑Ｆt_cmd1，↑Ｍt_cmd1]^T）である。そして、↑Ｆt_cmd1（以降、修正後目標総並進外力という）は、基準歩容の目標総並進外力↑Ｆt_cmd0と補償並進外力↑Ｆg_dmdとを加え合わせたもの（＝↑Ｆt_cmd0＋↑Ｆg_dmd）であり、↑Ｍt_cmd1（以降、修正後目標総モーメント外力という）は、基準歩容の目標総モーメント外力↑Ｍt_cmd0と、補償モーメント外力↑Ｍg_dmdとを加え合わせたもの（＝↑Ｍt_cmd0＋↑Ｍg_dmd）である。

そして、目標全接触力決定部６３は、修正後目標総外力↑ＦＭt_cmd1を、移動体１が現在接触している全ての接触対象面（第１〜第Ｎ接触対象面）に、あらかじめ定められた規則に従って分配し、各接触対象面（第ｉ接触対象面）から移動体１に作用させるべき目標全接触力↑ＦＭt(i)_cmd1（第ｉ目標全接触力↑ＦＭt(i)_cmd1）（ｉ＝１，２，…，Ｎ）をそれぞれ決定する。

ここで、第ｉ目標全接触力↑ＦＭt(i)_cmd1は、より詳しくは、第ｉ接触対象面のモデルとしての想定第ｉ接触対象面（その空間的な位置及び姿勢が、実際の第ｉ接触対象面に一致もしくは近似するものとしてあらかじめ定められた接触対象面）から移動体１に作用する接触力の目標値として決定されるものである。この第ｉ目標全接触力↑ＦＭ(i)_cmd1は、想定第ｉ接触対象面上のある作用点から移動体１に作用する並進力（全接触力の並進力成分）の目標値である目標全並進接触力↑Ｆt(i)_cmd1と、その作用点周りで移動体１に作用するモーメント（全接触力のモーメント成分）の目標値である目標全接触力モーメント↑Ｍt(i)_cmd1とから構成されるベクトル（＝[↑Ｆt(i)_cmd1，↑Ｍt(i)_cmd1]^T）である。

そして、本実施形態では、第ｉ目標全接触力↑ＦＭt(i)_cmd1の作用点は、第ｉ接触対象面から移動体１に作用する全接触力に関する全接触力中心点（その点周りの全接触力モーメントのうちの第ｉ接触対象面に平行な軸周りの成分がゼロとなるような全接触力の作用点）の目標位置として、想定第ｉ接触対象面上に設定される第ｉ目標全接触力中心点である。

従って、第ｉ目標全接触力↑ＦＭt(i)_cmd1は、第ｉ目標全接触力モーメント↑Ｍt(i)_cmd1のうちの想定第ｉ接触対象面に平行な軸周りの接触力モーメントがゼロとなるような全接触力である。そして、第ｉ目標全接触力↑ＦＭt(i)_cmd1（ｉ＝１，２，…，Ｎ）のそれぞれの決定処理においては、想定第ｉ接触対象面のそれぞれにおける目標全接触力中心点（第ｉ目標全接触力中心点）の決定も併せて行なわれる。

第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1の決定処理（修正後目標総外力↑ＦＭt_cmd1の分配処理）は、本実施形態では、例えば次のように行なわれる。すなわち、移動体１の現在の接触対象面が１つである場合（Ｎ＝１である場合）には、修正後目標総並進外力↑Ｆt_cmd1がそのまま、当該１つの接触対象面に対応する目標全接触力↑ＦＭt(i)_cmd1の目標全並進接触力↑Ｆt(i)_cmd1として決定される。

さらに、この目標全並進接触力↑Ｆt(i)_cmd1が当該１つの接触対象面に対応する想定接触対象面上の目標全接触力中心点に作用したときに、前記基準点周りに発生するモーメント（＝基準点に対する目標全床反力中心点の位置ベクトルと、目標全並進接触力↑Ｆt(i)_cmd1とのベクトル積）のうち、該想定接触対象面に平行な軸周り成分が、修正後目標総モーメント外力↑Ｍt_cmd1のうちの該想定接触対象面に平行な軸周りの成分に一致するように、目標全接触力中心点が該想定対象接触面上に決定される。

そして、目標全接触力↑ＦＭt(i)_cmd1の目標全接触力モーメント↑Ｍt(i)_cmd1は、↑Ｍt(i)_cmd1のうちの上記想定接触対象面に水平な軸周り成分がゼロとなり、且つ、↑Ｍt(i)_cmd1のうちの該想定接触対象面に垂直な軸周り成分と、修正後目標総並進外力↑Ｆt_cmd1のうちの該想定接触対象面に平行な軸周り成分とによって前記基準点周りに発生するモーメントが、修正後目標総モーメント外力↑Ｍt_cmd1のうちの該想定接触対象面に垂直な軸周りの成分に一致するように決定される。

また、移動体１の現在の接触対象面が２つ以上である場合（Ｎ≧２である場合）には、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1は例えば次のように決定される。

すなわち、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1のそれぞれの接触面垂直抗力成分（それぞれに対応する想定第ｉ接触対象面に垂直な並進力成分）の総和の並進力ベクトルが、修正後目標総並進外力↑Ｆt_cmd1のうちの、想定第ｉ接触対象面（ｉ＝１，２，…，Ｎ）の全てに平行な成分を除く成分に一致するように、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1のそれぞれの接触面垂直抗力成分が決定される。

さらに、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1のそれぞれの摩擦力成分（それぞれに対応する想定第ｉ接触対象面に平行な並進力成分）の総和の並進力ベクトルが、修正後目標総並進外力↑Ｆt_cmd1のうちの、想定第ｉ接触対象面（ｉ＝１，２，…，Ｎ）の全てに平行な成分に一致するように、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1のそれぞれの摩擦力成分が決定される。

なお、この場合、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1のそれぞれの摩擦力成分は、接触面垂直抗力成分の大きさが相対的に小さい第ｉ目標全接触力↑ＦＭt(i)_cmd1の摩擦力成分の大きさが相対的に小さくなるように決定される。

例えば、第１目標全接触力↑ＦＭt(1)_cmd1の接触面垂直抗力成分の大きさが、第２目標全接触力↑ＦＭt(2)_cmd1の接触面垂直抗力成分の大きさよりも小さい場合には、第１目標全接触力↑ＦＭt(1)_cmd1の摩擦力成分の大きさが、第２目標全接触力↑ＦＭt(2)_cmd1の摩擦力成分の大きさよりも小さくなるように、それらの摩擦力成分が決定される。

そして、第ｉ目標全接触力↑ＦＭt(i)_cmd1（ｉ＝１，２，…，Ｎ）の第ｉ目標全並進接触力↑Ｆt(i)_cmd1が、それぞれの接触面垂直抗力成分と摩擦力成分との合成並進力として決定される。

さらに、上記の如く決定した第ｉ目標全並進接触力↑Ｆt(i)_cmd1（ｉ＝１，２，…，Ｎ）のそれぞれが、対応する想定第ｉ接触対象面上の目標全接触力中心点（第ｉ目標全接触力中心点）に作用したときに、前記基準点周りに発生する総和のモーメント（＝基準点に対する第ｉ目標全床反力中心点の位置ベクトルと、第ｉ目標全並進接触力↑Ｆt(i)_cmd1とのベクトル積の総和のモーメントベクトル）が、修正後目標総モーメント外力↑Ｍt_cmd1にほぼ一致するか、もしくは極力近い値になるように、第１〜第Ｎ目標全接触力中心点が決定される。

このような第１〜第Ｎ目標全接触中心点は、例えば次のように決定できる。すなわち、想定第ｉ接触対象面（ｉ＝１，２，…，Ｎ）のそれぞれに対して、第ｉ目標全接触力中心点の暫定値を決定し、その暫定の第ｉ目標全接触中心点に第ｉ目標全接触力↑ＦＭt(i)_cmd1をそれぞれ作用させた場合に、前記基準点周りに発生する総和のモーメントを算出する。そして、この総和のモーメントと、前記修正後目標総モーメント外力↑Ｍt_cmd1との偏差を誤差モーメントとして算出する。

さらに、第ｉ目標全接触力中心点のそれぞれの位置の変化（暫定値からの変化）に対する前記基準点周りのモーメントの変化の感度を表すヤコビアン行列を算出し、このヤコビアン行列の擬似逆行列を上記誤差モーメントに乗じることによって、第ｉ目標全接触力中心点のそれぞれの位置の修正量を決定する。

そして、この決定した修正量によって、第ｉ目標全接触力中心点のそれぞれの位置を暫定値から修正することによって、第１〜第Ｎ目標全接触力中心点をそれぞれ決定する。

さらに、第ｉ目標全接触モーメントの、想定第ｉ接触対象面に垂直な軸周りの接触力モーメントがそれぞれ決定される。この場合、上記の如く決定した第ｉ目標全接触力中心点（ｉ＝１，２，…，Ｎ）にそれぞれ第ｉ目標全並進接触力↑Ｆt(i)_cmd1を作用させた場合に前記基準点周りに発生する総和のモーメントと、想定第ｉ接触対象面（ｉ＝１，２，…，Ｎ）に垂直な軸周りの接触力モーメントとの合成モーメントが修正後目標総モーメント外力↑Ｍt_cmd1に一致するように、想定第ｉ接触対象面のそれぞれに垂直な軸周りの接触力モーメント（以降、これをねじり力モーメントという）が決定される。

そして、第ｉ目標全接触力モーメントＭt(i)_cmd1（ｉ＝１，２，…，Ｎ）のそれぞれは、上記の如く決定したねじり力モーメントに一致するモーメント（想定第ｉ接触対象面に平行な軸周りの成分はゼロとなるモーメント）として決定される。

なお、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1によって前記基準点周りに発生する総和のモーメントを修正後目標総モーメント外力↑Ｍt_cmd1に一致させるために上記のようにねじり力モーメントを決定する代わりに、第１〜第Ｎ目標全並進接触力Ｆt(i)_cmd1のうちの摩擦力成分の総和を、修正後目標総並進外力↑Ｆt_cmd1のうちの、想定第ｉ接触対象面（ｉ＝１，２，…，Ｎ）の全てに平行な成分に一致する値から変化させないように、２つ以上の第ｉ目標全並進接触力Ｆt(i)_cmd1の摩擦力成分の修正量を決定するようにしてもよい。この場合は、第ｉ目標全接触力中心点（ｉ＝１，２，…，Ｎ）にそれぞれ当該修正後における第ｉ目標全並進接触力↑Ｆt(i)_cmd1を作用させた場合に前記基準点周りに発生する総和のモーメントが修正後目標総モーメント外力↑Ｍt_cmd1に一致するように、２つ以上の第ｉ目標全並進接触力Ｆt(i)_cmd1の摩擦力成分の修正量を決定すればよい。

あるいは、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1によって前記基準点周りに発生する総和のモーメントを修正後目標総モーメント外力↑Ｍt_cmd1に一致させるために、第ｉ目標全接触力ＦＭt(i)_cmd1の上記ねじり力モーメント（＝↑Ｍt(i)_cmd1）と２つ以上の第ｉ目標全並進接触力Ｆt(i)_cmd1の摩擦力成分の修正量との両方を決定するようにしてもよい。

本実施形態では、以上のようにして、第１〜第Ｎ目標全並進接触力↑Ｆt(i)_cmd1の総和の並進力が、修正後総並進外力↑Ｆt_cmd1に一致し、且つ、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1によって前記基準点周りに発生する総和のモーメントが修正後目標総モーメント外力↑Ｍt_cmd1に一致するように、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1が第１〜第Ｎ目標全接触中心点と共に決定される。

なお、この場合、第ｉ目標全接触力中心点（ｉ＝１，２，…，Ｎ）のそれぞれは、想定第ｉ接触対象面における移動体１の全ての接触面を連接した領域（所謂、支持多角形に相当する領域）内で決定される。

以上が本実施形態における目標全接触力決定部６３の具体的な処理である。

ここで、図６に示したような形態で移動体１の移動を行なう場合について例示的に説明すると、移動体１によりドアＤを押し開く動作の前後における歩行動作時においては、接触対象面は床面ＦＬ（以降、これを第１接触対象面とする）だけである。そして、この状況では、目標全床反力に相当する第１目標全接触力↑ＦＭt(1)__cmd1が上記の処理によって決定される。なお、この場合、第１目標全接触力中心点は、目標全床反力中心点であり、想定第１接触対象面（想定床面）上の支持多角形内に設定される。その支持多角形は、脚としての可動リンク３ａ，３ｂのうちの一方だけが接地する状態（片脚支持状態）では、その一方の可動リンク３ａ又は３ｂの先端部４ｃと想定第１接触対象面との接触面の領域である。また、可動リンク３ａ，３ｂの両方が接地する状態（両脚支持状態）では、可動リンク３ａ，３ｂのそれぞれの先端部４ｃと想定第１接触対象面との接触面を連接した領域である。

また、ドアＤを押し開く動作時においては、接触対象面は、床面ＦＬ（第１接触対象面）と、ドア表面Ｄａ（以降、これを第２接触対象面とする）との２つの接触対象面である。そして、この状況では、第１目標全接触力↑ＦＭt(1)_cmd1（目標全床反力）と、第２目標全接触力↑ＦＭt(2)_cmd1（ドア表面Ｄａからの目標全接触力）とが上記の処理によって決定される。

なお、この場合、目標全床反力中心点に相当する第１目標全接触力中心点は、想定第１接触対象面（想定床面）上の支持多角形の領域内に設定される。また、第２目標全接触中心点は、可動リンク３ｄの先端部４ｃとドア表面Ｄａに対応する想定第２接触対象面との接触面の領域内に設定される。この場合、想定第２接触対象面は、ドアＤのあらかじめ定めれた開動作パターンでドアＤのヒンジの周りに回転する面（位置及び姿勢が経時的に変化する接触対象面）である。

補足すると、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1を決定するための上記の手法は、本願出願人が先に特願２０１０−４４７１２号にて提案した手法を適用したものであり、その詳細は、特願２０１０−４４７１２号にて詳細に説明されている。そして、本発明は、各目標全接触力↑ＦＭt(i)_cmd1の決定の仕方を本質とするものではない。このため、冗長な記載を避けるために、本明細書では第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1の決定処理に関する説明は簡略的な説明に留めた。

ここで、移動体１が接触するＮ個の接触対象面に関する第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1を決定する手法は、第１〜第Ｎ目標全並進接触力Ｆt(i)_cmd1の総和の並進力が、修正後総並進外力↑Ｆt_cmd1に一致（もしくはほぼ一致）し、且つ、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1によって前記基準点周りに発生する総和のモーメントが修正後目標総モーメント外力↑Ｍt_cmd1に一致（もしくはほぼ一致）し、また、各接触対象面毎の目標全接触力↑ＦＭt(i)_cmd1が連続的に（滑らかに）変化するように第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1を決定する手法であれば、特願２０１０−４４７１２号にて本願出願人が提案した手法に限らず、どのような手法で第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1を決定するようにしてもよい。

［全接触力要求修正量決定部６４の処理］
次いで、制御装置５０は、全接触力要求修正量決定部６４の処理を実行する。この全接触力要求修正量決定部６４は、上記の如く決定した第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1のそれぞれに、第ｉ接触対象面（ｉ＝１，２，…，Ｎ）から移動体１に作用する実際の全接触力（第ｉ全接触力）の観測値を追従させるように第ｉ全接触力の要求修正量をそれぞれ決定する機能部である。

第ｉ全接触力の要求修正量は、前記式（２）の第ｉ摂動全接触力↑ΔＦＭt(i)の要求値に相当するものであり、第ｉ全接触力のうちの並進接触力の要求修正量（３成分の縦ベクトル）としての第ｉ摂動全並進接触力↑ΔＦt(i)の要求値と、第ｉ目標全接触力中心点周りの全接触力モーメントの要求修正量（３成分の縦ベクトル）としての摂動全接触力モーメント↑ΔＭt(i)の要求値とから構成される。以降の説明では、第ｉ摂動全並進接触力↑ΔＦt(i)の要求値を↑ΔＦt(i)_dmd、第ｉ摂動全接触力モーメント↑ΔＭt(i)の要求値を↑ΔＭt(i)_dmdと表記し、これらを合わせた６成分の縦ベクトルとしての第ｉ摂動全床反力↑ΔＦＭt(i)の要求値を↑ΔＦＭt(i)_dmd（＝[↑ΔＦt(i)_dmd，↑ΔＭt(i)_dmd]^T）と表記する。

また、第ｉ接触対象面から移動体１に作用する実際の全接触力（実全接触力）のうちの並進接触力の観測値（３成分の縦ベクトル）を↑Ｆt(i)_act、実全接触力（第ｉ実全接触力）のうちの接触力モーメントの観測値（３成分の縦ベクトル）を↑Ｍt(i)_actと表記し、さらに、これらの↑Ｆt(i)_act，↑Ｍt(i)_actを合わせた６成分の縦ベクトルとしての第ｉ実全接触力の観測値を↑ＦＭt(i)_act（＝[↑Ｆt(i)_act，↑Ｍt(i)_act]^T）と表記する。

全接触力要求修正量決定部６４は、以下に説明する処理によって、上記第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmd（ｉ＝１，２，…，Ｎ）をそれぞれ決定する。

すなわち、全接触力要求修正量決定部６４は、まず、第ｉ接触対象面（ｉ＝１，２，…，Ｎ）のそれぞれに関し、第ｉ目標全接触力↑ＦＭt(i)_cmd1と、第ｉ実全接触力の観測値↑ＦＭt(i)_actとの偏差↑Ｄfmt(i)（＝↑ＦＭt(i)_cmd1−↑ＦＭt(i)_act）を算出する。この場合、第ｉ実全接触力の観測値↑ＦＭt(i)_actは、第ｉ接触対象面に接触している可動リンク３のグループである第ｉ接触可動リンクグループの各可動リンク３の前記力センサ５２の出力により示される該可動リンク３の接触力（６軸力）の観測値を、第ｉ目標全接触力中心点を作用点として合成することにより算出される。

ただし、本実施形態では、偏差↑Ｄfmtが過剰に変動するのを防止するために、全接触力要求修正量決定部６４は、第ｉ接触可動リンクグループのそれぞれについて、前記力センサ５２の出力により示される各可動リンク３の接触力（６軸力）の観測値を合成してなる第ｉ実全接触力の値に、ローパス特性のフィルタリング処理を施すことによって、第ｉ目標全接触力中心点に作用する第ｉ実全接触力の観測値↑ＦＭt(i)_actを算出する。なお、この場合、第ｉ接触可動リンクグループの各可動リンク３の接触力（６軸力）の観測値に、それぞれローパス特性のフィルタリング処理を施したものを合成することによって第ｉ実全接触力の観測値↑ＦＭt(i)_actを算出するようにしてもよい。

次いで、全接触力要求修正量決定部６４は、第ｉ接触可動リンクグループのそれぞれについて、次式（４１）に示す如く、上記偏差↑Ｄfmt(i)に応じて決定した比例項（式（４１）の右辺の第１項）と積分項（式（４１）の右辺の第２項）と加え合わせることによって、第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdを決定する。

上記比例項は、偏差↑Ｄfmt(i)に所定値のゲインＫcmpを乗じたものである。また、上記積分項は、偏差↑Ｄfmt(i)にローパス特性のフィルタリング処理を施してなる値↑Ｄfmt(i)_filtに所定値のゲインＫestmを乗じてなる値を積分したものである。ここで、↑Ｄfmt(i)_filtを得るためのローパス特性のフィルタリング処理は、その高周波側のカットオフ周波数が、前記偏差↑Ｄfmt(i)を算出するときに用いたローパス特性のフィルタリング処理の高周波側のカットオフ周波数よりも低周波数となる特性のフィルタリング処理である。

↑ΔＦＭt(i)_dmd＝Ｋcmp・↑Ｄfmt(i)＋∫（Ｋestm・↑Ｄfmt(i)_filt）
……（４１）

なお、ゲインＫcmp，Ｋestmは、スカラーでもよいが、対角行列であってもよい。

ここで、式（４１）の積分項は、第ｉ目標全接触力↑ＦＭt(i)_cmd1と、第ｉ実全接触力の観測値↑ＦＭt(i)_actとの定常偏差に相当し、これは、実際の第ｉ接触対象面に対する想定第ｉ接触対象面の位置及び姿勢の誤差分に相当するものである。

なお、本実施形態では、上記式（４１）により第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdを算出したが、比例項を省略して要求値↑ΔＦＭt(i)_dmdを算出するようにしてもよい。また、積分項の演算では、↑ΔＤfmt(i)_filtの代わりに、↑Ｄfmt(i)をそのまま用いるようにしてもよい。

以上が、本実施形態における全接触力要求修正量決定部６４の処理である。

ここで、図６に示したような形態で移動体１の移動を行なう場合について例示的に説明すると、移動体１によりドアＤを押し開く動作の前後における歩行動作時においては、床面ＦＬ（第１接触対象面）に関する第１摂動全接触力↑ΔＦＭt(1)の要求値↑ΔＦＭt(1)_dmdが、上記式（４１）により算出される。

また、ドアＤを押し開く動作時においては、床面ＦＬ（第１接触対象面）に関する第１摂動全接触力↑ΔＦＭt(1)の要求値↑ΔＦＭt(1)_dmdと、ドア表面Ｄａ（第２接触対象面）に関する第２摂動全接触力↑ＦＭt(2)の要求値↑ΔＦＭt(2)_dmdとがそれぞれ上記式（４１）により算出される。

［代表接触面並進・回転変位量算出部６５の処理］
以上の如く全接触力要求修正量決定部６４の処理を実行した後、制御装置５０は、代表接触面並進・回転変位量算出部６５の処理を実行する。この代表接触面並進・回転変位量算出部６５は、上記の如く決定された第ｉ摂動全床反力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmd（ｉ＝１，２，…，Ｎ）のそれぞれを、前記式（２８）によって、第ｉ接触対象面に対応する仮想的な第ｉ代表接触面のばね性並進・回転変位量↑Ｘc(i)（＝[↑Ｘc(i)_org，↑Ｘc(i)_rot]^T）に変換する機能部である。

この場合、本実施形態では、第ｉ接触対象面（ｉ＝１，２，…，Ｎ）のそれぞれに対する移動体１の接触面を代表する第ｉ代表接触面の並進ばね定数行列Ｋc_org(i)の各対角成分（第ｉ代表接触面の３軸方向の並進変位に関する各成分のばね定数）と、該第ｉ代表接触面の回転ばね定数行列Ｋc_rot(i)の各対角成分（第ｉ代表接触面の３軸周りの回転変位に関する各成分のばね定数）とは、第ｉ接触対象面（ｉ＝１，２，…，Ｎ）のそれぞれ毎に、あらかじめ定められた定数値とされている。

そして、代表接触面並進・回転変位量算出部６５は、これらのばね定数行列Ｋc_org(i)，Ｋc_rot(i)を用いて、前記式（２８）に基づいて、第ｉ代表接触面のばね性並進・回転変位量↑Ｘc(i)を算出する。具体的には、式（２８）の右辺の↑ΔＦＭt(i)に↑ΔＦＭt(i)_dmdを代入してなる次式（４２）により、第ｉ代表接触面のばね性並進・回転変位量↑Ｘc(i)をそれぞれ算出する。

この式（４２）におけるＫc_org(i)^-1は、並進ばね定数行列Ｋc_org(i)の逆行列（Ｋc_org(i)の各対角成分の逆数値を対角成分とする行列）、Ｋc_rot(i)^-1は、回転ばね定数行列Ｋc_rot(i)の逆行列（Ｋc_rot(i)の各対角成分の逆数値を対角成分とする行列）である。これらのＫc_org(i)^-1，Ｋc_rot(i)^-1は、それぞれ、制御装置５０で並進ばね定数行列Ｋc_org(i)、回転ばね定数行列Ｋc_rot(i)から算出するようにしてもよいが、Ｋc_org(i)^-1，Ｋc_rot(i)^-1をあらかじめ制御装置５０の記憶装置に記憶保持しておくようにしてもよい。

以上が、本実施形態における代表接触面並進・回転変位量算出部６５の処理である。

ここで、図６に示したような形態で移動体１の移動を行なう場合について例示的に説明すると、移動体１によりドアＤを押し開く動作の前後における歩行動作時においては、床面ＦＬ（第１接触対象面）に対応する第１代表接触面のばね性並進・回転変位量↑Ｘc(1)が、上記式（４２）により算出される。

また、ドアＤを押し開く動作時においては、床面ＦＬ（第１接触対象面）に対応する第１代表接触面のばね性並進・回転変位量↑Ｘc(1)と、ドア表面Ｄａ（第２接触対象面）に対応する第２代表接触面のばね性並進・回転変位量↑Ｘc(2)とがそれぞれ上記式（４２）により算出される。

［代表接触面ヤコビアン行列算出部６６の処理］
コンプライアンス操作量決定処理では、制御装置５０は、上記の如く第１〜第Ｎ代表接触面のばね性並進・回転変位量↑Ｘc(1)〜↑Ｘc(N)を算出するのと並行して（又は↑Ｘc(1)〜↑Ｘc(N)の算出処理の前もしくは後に）、代表接触面ヤコビアン行列算出部６６の処理を実行する。

この代表接触面ヤコビアン行列算出部６６は、前記式（２３）の関係を表現する第ｉ代表接触面ヤコビアン行列Ｊc(i)（ｉ＝１，２，…，Ｎ）をそれぞれ算出する機能部である。なお、本実施形態における一般化変数ベクトル↑ｑは、より詳しくは、基体２の位置及び姿勢の６個の成分と、移動体１の各関節７の変位量とを並べた縦ベクトルである。そして、この↑ｑの各成分の単位時間当たりの変化量を並べた縦ベクトルが、式（２３）の↑Δｑである。

代表接触面ヤコビアン行列算出部６６は、前記式（２７）により、第ｉ代表接触面ヤコビアン行列Ｊc(i)をそれぞれ算出する。ここで、前記式（２７）と、この式（２７）の主な変数の意味とを改めて記載すると次の通りである。

この場合、式（２７）の右辺の演算に必要な変数値は、次のように決定される。

第ｉ接触対象面に関する重み係数ｒ(i)_jに関し、ロータリエンコーダ５３の出力により示される各関節７の実際の変位量（実関節変位量）の観測値（計測値）に基づいて、第ｉ接触可動リンクグループの各可動リンク３の先端部４ｃの現在の実姿勢の観測値が決定される。さらに、第ｉ接触可動リンクグループの各可動リンク３（第１〜第ｍ(i)可動リンク３のそれぞれ）の先端部４ｃの実姿勢の観測値と、前記力センサ５２の出力とを基に、第ｉ接触可動リンクグループの各可動リンク３の現在の実際の接触面垂直抗力成分Ｆn(i)_j（実接触面垂直抗力成分Ｆn(i)_j）（ｊ＝１，２，…，ｍ(i)）の観測値が求められる。そして、第ｉ接触可動リンクグループの各可動リンク３の実接触面垂直抗力成分Ｆn(i)_jの観測値から、式（２７−１）により示される定義式に従って、重み係数ｒ(i)_j（ｊ＝１，２，…，ｍ(i)）が決定される。なお、この場合、重み係数ｒ(i)_jの値が頻繁に変動するのを防止するために、Ｆn(i)_jのそれぞれの観測値にローパスフィルタ等のフィルタリング処理を施してもよい。

補足すると、重み係数ｒ(i)_jは、第ｉ接触可動リンクグループの各可動リンク３の各可動リンク３の実接触面垂直抗力成分Ｆn(i)_jの観測値に基づいて決定することが望ましいが、該観測値の代わりに、実接触面垂直抗力成分Ｆn(i)_jの近似的な推定値もしくは予測値を使用して、重み係数ｒ(i)_jを決定するようにしてもよい。例えば第ｉ接触可動リンクグループの各可動リンク３の目標接触力を設定するようにした場合で、且つ、各可動リンク３の実接触力が目標接触力に近似的に一致すると考えられる場合には、実接触面垂直抗力成分Ｆn(i)_jの観測値の代わりに目標値を使用して、重み係数ｒ(i)_jを決定するようにしてもよい。

また、第ｉ接触対象面に関する行列Ａ(i)_j（ｊ＝１，２，…，ｍ(i)）に関する係数行列Ｒk(i)は、第ｉ代表接触面に関してあらかじめ定められた並進ばね定数行列Ｋc_org(i)と、回転ばね定数行列Ｋc_rot(i)（又はその逆行列Ｋc_rot(i)^-1）とから上記式（２７−３）に従って決定される。なお、係数行列Ｒk(i)は、制御装置５０の記憶装置にあらかじめ記憶保持しておいてもよい。

また、行列Ａ(i)_jに関する行列VV(i)_jを決定するために必要となる位置ベクトル↑Ｖ(i)_jは、例えば、次のように決定される。

ロータリエンコーダ５３の出力により示される各関節７の現在の実変位量の観測値（計測値）と、前記力センサ５２の出力とを基に、第ｉ接触可動リンクグループの各可動リンク３の接触面（第ｉ接触対象面との接触面）における現在の実際の接触力中心点（実接触力中心点）の位置ベクトル（以降、これを↑Ｖa(i)_jと表記する）が特定される。なお、この位置ベクトルの基準点は任意の点でよい。

次いで、第ｉ接触可動リンクグループの各可動リンク３に対応する実接触力中心点の、前記重み係数ｒ(i)_jの応じた重み付き平均の点の位置ベクトル↑Ｖc(i)（＝ｒ(i)_1・↑Ｖa(i)_1＋ｒ(i)_2・↑Ｖa(i)_2＋……＋ｒ(i)_m(i)・↑Ｖa(i)_m(i)）を、第ｉ実全接触力中心点の位置ベクトルとして算出する。

そして、↑Ｖ(i)_j＝↑Ｖa(i)_j−↑Ｖc(i)（ｊ＝１，２，…，ｍ(i)）により、第ｉ実全接触力中心点に対する第ｉ接触可動リンクグループの各可動リンク３（第ｊ可動リンク３）の接触面の実接触力中心点の位置ベクトル↑Ｖ(i)_jがそれぞれ決定される。

そして、行列Ａ(i)_jに関する行列VV(i)_jは、定義に従って、VV(i)_j・↑Ｆ(i)_j＝↑Ｖ(i)_j×↑Ｆ(i)_jとなるように決定される。

式（２７）の右辺の演算に必要な変数値のうち、第ｉ接触可動リンクグループの第ｊ可動リンク３に関する可動リンクヤコビアン行列Ｊ(i)_jは、次のように決定される。可動リンクヤコビアン行列Ｊ(i)_jは、第ｊ可動リンク接触面（第ｊ可動リンク３と第ｉ接触対象面との接触面）のばね性並進・回転変位量↑Ｘ(i)_j（＝[↑Ｘorg(i)_j，↑Ｘrot(i)_j]^Tと、一般化変数ベクトル↑ｑの単位時間当たりの変化量↑Δｑとの間の関係を前記式（２４）により表現する行列である。

ここで、↑Ｘ(i)_jのうちのばね性並進変位量↑Ｘorg(i)_jは、第ｊ可動リンク接触面の単位時間当たりの並進変位量であるとみなすと、第ｊ脚リンク３の先端部４ｃの現在の接触部分の位置の時間的変化率（並進速度）に一致すると考えてよい。また、↑Ｘ(i)_jのうちのばね性回転変位量↑Ｘrot(i)_jは、第ｊ可動リンク接触面の単位時間当たりの回転変位量であるとみなすと、第ｊ可動リンク３の先端部４ｃの現在の姿勢の時間的変化率（角速度）に一致すると考えてよい。

そこで、本実施形態では、第ｊ可動リンク３の先端部４ｃの現在の接触部分の位置及び該先端部４ｃの姿勢（より詳しく当該位置及び姿勢を成分とする縦ベクトル）の単位時間当たりの変化量（時間的変化率）が、前記式（２４）の左辺の↑Ｘ(i)_jに一致するものとして、第ｉ接触可動リンクグループの第ｊ可動リンク３に関する脚リンクヤコビアン行列Ｊ(i)_jを決定する。なお、第ｊ可動リンク３の先端部４ｃの現在の接触部分の位置としては、例えば、該第ｊ可動リンク３の接触面（第ｊ可動リンク接触面）の実接触力中心点に相当する該先端部４ｃの点の位置が用いられる。

この場合、可動リンクヤコビアン行列Ｊ(i)_jは、移動体１の現在の動作状態（これは各関節７の現在の実変位量の計測値により特定される）を↑ｑの微小変化（摂動）の起点として、公知の手法によって決定される。例えば、移動体１の幾何学モデル（剛体リンクモデル）を用いて、あるいは、解析的な演算によって、一般化変数ベクトル↑ｑの各成分の微小変化（現在状態からの微小変化）に対する第ｊ可動リンク３の先端部４ｃの接触部分の位置の変化と該先端部４ｃの姿勢の変化とを算出することで、可動リンクヤコビアン行列Ｊ(i)_jが決定される。

なお、この場合、第ｉ接触可動リンクグループの第ｊ可動リンク３の先端部４ｃの接触部分の現在の位置は、第ｊ可動リンク３の第ｉ接触対象面との接触面の現在の実接触力中心点の位置の観測値に一致するものとされる。そして、この点の位置の変化（↑ｑの各成分の微小変化に対する変化）が、第ｊ可動リンク３の先端部４ｃの接触部分の位置の変化として算出される。

また、第ｊ可動リンク３の先端部４ｃの現在の姿勢は、ロータリエンコーダ５３の出力が示す各関節の実変位量の観測値（計測値）を基に算出される該第ｊ可動リンク３の先端部４ｃの現在の実姿勢の観測値に一致するものとされて、該先端部４ｃの姿勢の変化（↑ｑの各成分の微小変化に対する変化）が算出される。

本実施形態では、代表接触面ヤコビアン行列算出部６６は、移動体１が接触する第ｉ接触対象面（ｉ＝１，２，…，Ｎ）にそれぞれに関して、以上の如く決定した重み係数ｒ(i)_j（ｊ＝１，２，…，ｍ(i)）、行列VV(i)_j、及び可動リンクヤコビアン行列Ｊ(i)_jを用いて、前記式（２７）の右辺の演算を行なうことで、第ｉ代表接触面ヤコビアン行列Ｊc(i)をそれぞれ算出する。

以上が、本実施形態における代表接触面ヤコビアン行列算出部６６の処理である。

ここで、図６に示したような形態で移動体１の移動を行なう場合について例示的に説明すると、移動体１によりドアＤを押し開く動作の前後における歩行動作時においては、床面ＦＬ（第１接触対象面）に対応する第１代表接触面ヤコビアン行列Ｊc(1)が、前記式（２７）により算出される。

この場合、移動体１の可動リンク３ａ，３ｂの一方だけが床面ＦＬに接触している状態では、式（２７）におけるｍ(1)は、ｍ(1)＝１であり、可動リンクヤコビアン行列Ｊ(1)_1は、床面ＦＬに接触している可動リンク３ａ又は３ｂに関する可動リンクヤコビアン行列である。

また、可動リンク３ａ，３ｂの両方が床面ＦＬに接触している状態では、式（２７）におけるｍ(1)は、ｍ(1)＝２であり、可動リンクヤコビン行列Ｊ(1)_1，Ｊ(1)_2の一方が可動リンク３ａに関する可動リンクヤコビアン行列、他方が可動リンク３ｂに関する可動リンクヤコビアン行列となる。

また、ドアＤを押し開く動作時においては、床面ＦＬ（第１接触対象面）に対応する第１代表接触面ヤコビアン行列Ｊc(1)と、ドア表面Ｄａ（第２接触対象面）に対応する第２代表接触面ヤコビアン行列Ｊc(2)とがそれぞれ前記式（２７）により算出される。

この場合、Ｊc(1)は、歩行動作時に可動リンク３ａ，３ｂの両方が床面ＦＬに接触している場合と同様に算出される。また、Ｊc(2)の算出においては、式（２７）におけるｍ(2)は、ｍ(2)＝１であり、可動リンクヤコビアン行列Ｊ(2)_1は、ドア表面Ｄａに接触している可動リンク３ｄに関する可動リンクヤコビアン行列である。

［関節変位修正量決定部６８の処理］
コンプライアンス操作量決定処理では、制御装置５０は、以上の如く、第ｉ接触対象面（ｉ＝１，２，…，Ｎ）のそれぞれに関する第ｉ代表接触面のばね性並進・回転変位量↑Ｘc(i)と、代表接触面ヤコビアン行列Ｊc(i)とを算出した後、関節変位修正量決定部６８の処理を実行する。

この関節変位修正量決定部６８は、第１〜第Ｎ代表接触面のばね性並進・回転変位量↑Ｘc(1)〜↑Ｘc(N)の全てに対応する各関節の変位量の修正量（基準歩容の目標運動に対応する目標関節変位量からの修正量）である関節変位修正量を決定する機能部である。

この関節変位修正量決定部６８の処理では、まず、第１〜第Ｎ代表接触面ヤコビアン行列Ｊc(1)〜Ｊc(N)を並べて構成される総合ヤコビアン行列Ｊc（≡[Ｊc(1)，Ｊc(2)，……，Ｊc(N)]^T）の擬似逆行列Ｊc^-1が算出される。ここで、総合ヤコビアン行列Ｊcは、一般には正方行列ではないので、Ｊcの逆行列は存在しない。このため、関節変位修正量決定部６８は、総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を算出する。

そして、関節変位修正量決定部６８は、前記式（３０）で示す如く、この擬似逆行列Ｊｃ^-1を、第１〜第Ｎ代表接触面のばね性並進・回転変位量↑Ｘc(1)〜↑Ｘc(N)を並べたベクトル（縦ベクトル）である総合ばね性並進・回転変位量↑Ｘc（＝[↑Ｘc(1)，↑Ｘc(2)，……，↑Ｘc(N)]^T）に乗じることによって、前記一般化変数ベクトル↑ｑの各成分の要求修正量（要求摂動量）を構成要素とする一般化変数要求修正量ベクトル↑Δｑ_dmdを算出する。そして、この一般化変数要求修正量ベクトル↑Δｑ_dmdの成分のうち、各関節の変位量の要求修正量を示す成分が、関節変位修正量として決定される。

この場合、関節変位修正量決定部６８は、前記擬似逆行列Ｊc^-1を前記式（３１）により算出する。

すなわち、本実施形態では、擬似逆行列Ｊc^-1は、重み付きの擬似逆行列（重み付きのＳＲ−Inverse）として算出される。

具体的には、擬似逆行列Ｊc^-1を算出する処理は、前記調整パラメータｋの値の決定処理を含めて、図７のフローチャートに示す如く実行される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

関節変位修正量決定部６８は、以上説明した如く算出した擬似逆行列Ｊc^-1を用いて、前記式（３０）の右辺の演算を行なうことで、一般化変数要求修正量ベクトル↑Δｑ_dmdを算出する。そして、関節変位修正量決定部６８は、この一般化変数要求修正量ベクトル↑Δｑ_dmdの成分のうち、各関節の変位量の要求修正量を示す成分を、関節変位修正量として決定する。このようにして決定される関節変位修正量が、第１〜第Ｎ接触対象面のそれぞれについての第ｉ実全接触力（ｉ＝１，２，…，Ｎ）を第ｉ目標全接触力↑ＦＭt(i)_cmd1に近づけるための各関節の変位量の要求修正量としてのコンプライアンス操作量である。

［関節変位制御部６９の処理］
以上説明したコンプラインス操作量決定処理（姿勢安定化補償力決定部６２、目標全接触力決定部６３、全接触力要求修正量決定部６４、代表接触面並進・回転変位量算出部６５、代表接触面ヤコビアン行列算出部６６及び関節変位修正量決定部６８の処理）とを実行した後、次に、制御装置５０は、関節変位制御部６９の処理を実行する。

この関節変位制御部６９は、目標関節変位量決定部６７で決定した各関節の目標関節変位量（基準歩容の目標運動に対応する目標関節変位量）に、目標関節変位量修正部６８で決定した関節変位修正量を加え合わせることにより、各関節の最終的な関節変位指令としての修正後目標関節変位量を決定する。

そして、関節変位制御部６９は、上記の如く決定した修正後目標関節変位量に各関節の実変位量を一致させるように、電動モータ４１（関節アクチュエータ）を図示しないサーボアンプ等のモータ駆動回路を介して制御する。

［接触対象面推定部７０の処理］
制御装置５０は、移動体１の各関節の駆動制御と並行して、接触対象面推定部７０の処理を実行する。この接触対象面推定部７０は、移動体１が接触する実際の各接触対象面（実接触対象面）の位置及び姿勢の推定値である接触対象面推定値を算出する機能部である。

この処理のために接触対象面推定部７０には、前記全接触力要求修正量決定部６４から、第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmd（ｉ＝１，２，…，Ｎ）の組成成分である前記式（４１）の右辺の積分項（第２項）の値、すなわち、第ｉ目標全接触力↑ＦＭt_cmd1と、第ｉ実全接触力の観測値↑ＦＭt(i)_actとの偏差↑Ｄfmt(i)（＝↑ＦＭt(i)_cmd1−↑ＦＭt(i)_act）にローパス特性のフィルタリング処理を施してなる値↑Ｄfmt(i)_filtに、所定値のゲインＫestm（スカラー又は対角行列）を乗じてなる値を積分したもの（＝∫（Ｋestm・↑Ｄfmt(i)_filt））が入力される。以降、この積分項の値を次式（４４）で示す如く、ΔＦＭt(i)_intと表記する。

↑ΔＦＭt(i)_int≡∫（Ｋestm・↑Ｄfmt(i)_filt） ……（４４）

ここで、前記した移動体１の各関節の駆動制御によって、各関節の変位量は、第ｉ実全接触力の観測値↑ＦＭt(i)_act（ｉ＝１，２，…，Ｎ）をそれぞれ第ｉ目標全接触力↑ＦＭt(i)_cmd1に追従させるように（上記偏差↑Ｄfmt(i)をゼロに近づけるように）制御されるので、上記積分項の値↑ΔＦＭt(i)_intは、基準歩容の作成のために使用された想定第ｉ接触対象面の位置及び姿勢の、第ｉ実接触対象面に対する定常的な誤差に起因して発生するものとなる。

従って、この↑ΔＦＭt(i)_intに対応する第ｉ代表接触面のばね性並進・回転変位量が基準歩容の目標運動の作成のために使用された想定第ｉ接触対象面の位置及び姿勢の、実際の第ｉ接触対象面（第ｉ実接触対象面）に対する定常的な誤差分に相当するものと考えられる。

そこで、接触対象面推定部７０は、第ｉ接触対象面に関して与えられた積分項の値↑ΔＦＭt(i)_intを、前記式（２８−１）により、ばね性並進・回転変位量に変換したものを、想定第ｉ接触対象面の位置及び姿勢の誤差分↑Ｘc(i)_intとして得る。この場合、より詳しくは、積分項の値↑ΔＦＭt(i)_intのうちの並進力成分↑ΔＦt(i)_intに、第ｉ代表接触面の並進ばね定数行列Ｋc_org(i)の逆行列Ｋc_org(i)^-1を乗じたもの（＝Ｋc_org(i)^-1・↑ΔＦt(i)_int）が想定第ｉ接触対象面の位置の誤差分↑Ｘc_org(i)_intとされる。また、積分項の値↑ΔＦＭt(i)_intのうちのモーメント成分↑ΔＭt(i)_intに、第ｉ代表接触面の回転ばね定数行列Ｋc_rot(i)の逆行列Ｋc_rot(i)^-1を乗じたもの（＝Ｋc_rot(i)^-1・↑ΔＭt(i)_int）が想定第ｉ接触対象面の姿勢の誤差分↑Ｘc_rot(i)_intとされる。

そして、接触対象面推定部７０は、上記の如く算出した誤差分↑Ｘc(i)_intに応じて想定第ｉ接触対象面の位置及び姿勢を補正することにより、第ｉ実接触対象面の位置及び姿勢の推定値としての第ｉ接触対象面推定値（ｉ＝１，２，…，Ｎ）をそれぞれ決定する。

このように接触対象面推定部７０により決定された第ｉ接触対象面推定値（ｉ＝１，２，…，Ｎ）は、本実施形態では、それぞれ前記基準歩容生成処理部６１に与えられる。そして、基準歩容生成処理部６１では、第ｉ接触対象面推定値に応じて、基準歩容を作成するために使用する想定第ｉ接触対象面を定期的に（例えば、移動体１の歩行動作時の１歩毎、又は複数歩毎に）更新する。

なお、基準歩容の作成に使用する想定第ｉ接触対象面は、第ｉ接触対象面推定値だけに応じて決定する必要はない。例えば、移動体１に搭載した視覚センサや、外部から与えられる各接触対象面の情報に基づいて、第ｉ実接触対象面の位置及び姿勢を推定できるような場合には、その推定された第ｉ接触対象面の位置及び姿勢の信頼性の評価や補完的な修正のために、上記第ｉ接触対象面推定値を使用するようにしてもよい。

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

ここで、本実施形態と本発明との対応関係について補足しておく。本実施形態では、全接触力要求修正量決定部６４によって、本発明における全接触力要求修正量決定手段が実現される。この場合において、本実施形態では、目標全接触力決定部６３で決定する第ｉ目標全接触力↑ＦＭt(i)_cmd1が、本発明における第ｉ接触対象面に対応する目標全接触力に相当する。

また、代表接触面並進・回転変位量算出部６５、代表接触面ヤコビアン行列算出部６６、関節変位修正量決定部６８、関節変位制御部６９によって、それぞれ、本発明における代表接触面位置姿勢変位量算出手段、代表接触面ヤコビアン行列算出手段、関節変位修正量決定手段、関節変位制御手段が実現される。

また、関節変位修正量決定部６８において実行される処理のうち、図５のＳＴＥＰ２〜９の処理によって、本発明における擬似逆行列演算用パラメータ決定手段が実現される。

また、接触対象面推定部７０によって、本発明（前記第５発明）における接触対象面推定手段が実現される。この場合、該接触対象面推定部７０が、各接触対象面について、前記積分項の値↑ΔＦＭt(i)_intから、前記式（２８−１）により算出する誤差分↑Ｘc(i)_intが、本発明における代表接触面定常変位量に相当する。なお、本実施形態では、第１〜第Ｎ接触対象面のそれぞれが、本発明における第ｈ接触対象面に相当する。

以上説明した実施形態によれば、制御装置５０は、各制御処理周期において、第ｉ目標全接触力↑ＦＭt(i)_cmd1（ｉ＝１，２，…，Ｎ）のそれぞれに、第ｉ実全接触力を追従させるための操作量（制御入力）たる第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdを、第ｉ代表接触面のばね性並進・回転変位量↑Ｘc(i)に変換する。

さらに、制御装置５０は、この↑Ｘc(i)（ｉ＝１，２，…，Ｎ）を並べたベクトル（縦ベクトル）としての総合ばね性並進・回転変位量↑Ｘc（＝[↑Ｘc(1)，↑Ｘc(2)，……，↑Ｘc(N)]^T）に、第ｉ代表接触面ヤコビアン行列Ｊc(i)を並べた総合ヤコビン行列Ｊcの擬似逆行列Ｊc^-1を乗じることによって、↑Ｘc(i)（ｉ＝１，２，…，Ｎ）の全てを実現すための（ひいては各接触対象面の第ｉ実全接触力をそれぞれ第ｉ目標全床反力↑ＦＭt(i)_cmd1に追従させるための）コンプライアンス操作量である移動体１の各関節の関節変位修正量を決定する。そして、制御装置５０は、基準歩容の目標運動に対応する各関節の目標関節変位量を、関節変位修正量により修正してなる修正後目標関節変位量に応じて各関節の変位量を電動モータ８（関節アクチュエータ）を介して制御する。

これにより、本実施形態によれば、各接触対象面に接触する個々の可動リンク３の先端部（要素リンク）４ｃの位置や姿勢の修正と、各接触対象面の実全接触力の変化との関係やそれらの相互の関係を考慮して各可動リンク３の先端部の位置や姿勢の修正量を決定するような処理を実行することなく、第ｉ実全床反力をそれぞれ第ｉ目標全接触力↑ＦＭt(i)_cmd1に追従させるための各関節の関節変位修正量を一括的に決定できる。そのため、コンプライアンス制御用の関節変位修正量を決定する処理を効率よく短時間で行なうことができる。

この場合、第ｉ代表接触面ヤコビアン行列Ｊc(i)（ｉ＝１，２，…，Ｎ）を算出するための前記式（２７）における第ｉ接触可動リンクグループの各可動リンク３（第ｊ可動リンク３）の重み係数ｒ(i)_jは、接触面垂直抗力成分Ｆn(i)_jが相対的に大きい可動リンク３ほど、より大きい値（“１”に近い値）に設定されることとなる。

このため、第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdに対する第ｉ可動リンクグループの各可動リンク３の負担分は、結果的に、接触面垂直抗力成分Ｆn(i)_ｊが相対的に大きい可動リンク３ほど、大きくなる。換言すれば、第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdが、できるだけ、接触面垂直抗力成分Ｆn(i)_jが相対的に大きい可動リンク３の先端部４ｃの位置や姿勢の修正によって実現することができるように第ｉ代表接触面ヤコビアン行列Ｊc(i)が決定される。

従って、接触面垂直抗力成分Ｆn(i)_jが相対的に小さい可動リンク３の先端部４ｃの位置や姿勢を不必要に修正したりすることなく、第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdを確実に実現し得るような関節変位修正量、すなわち、第ｉ実全床反力（ｉ＝１，２，…，Ｎ）をそれぞれ第ｉ目標全接触力↑ＦＭt(i)_cmd1に追従させる上で適切な関節変位修正量を決定できる。

また、第ｉ接触可動リンクグループ（ｉ＝１，２，…，Ｎ）の各可動リンク３の重み係数ｒ(i)_jは連続的に変化するので、第ｉ代表接触面ヤコビアン行列Ｊc(i)が、不連続的な変化を生じることがない。このため、移動体１の各関節の変位量を滑らかに連続的に変化させることができる。ひいては、移動体１の運動を滑らかに行なうことができる。

また、前記実施形態では、関節変位修正量決定部６８で、前記行列式ＤＥＴの値が過小になるのを防止するための調整パラメータｋの値を探索する処理（図７のＳＴＥＰ２〜９の処理）において、調整パラメータｋの増加量Δｋを、下限閾値DET_Lから行列式ＤＥＴの絶対値を差し引いてなる偏差（＝DET_L−｜ＤＥＴ｜）のｎ乗根に比例する値に設定する。

これにより、制御装置５０の各制御処理周期において、｜ＤＥＴ｜≧DET_Lとなるようにするための適切な調整パラメータｋの値を、該ｋの値の不連続な変動が生じないようにしつつ、効率よく短時間で決定できる。このため、総合ばね性並進・回転変位量↑Ｘcから関節変位修正量を決定するための擬似逆行列Ｊc^-1を滑らかに変化させていくようにすることができる。ひいては、移動体１の各関節の変位量を滑らかに変化させるように、関節変位修正量を決定できる。

また、上記の如く第ｉ実全接触力をそれぞれ第ｉ目標全接触力↑ＦＭt(i)_cmd1に追従させるように、移動体１の各関節の駆動制御を行いつつ、第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdの構成要素である前記積分項の値↑ΔＦＭt(i)_intに対応する第ｉ代表接触面の並進・回転変位量↑Ｘc(i)_intを前記式（２８−１）により算出することによって、想定第ｉ接触対象面の位置及び姿勢の実際の第ｉ接触対象面に対する定常的な誤差分としての↑Ｘc(i)_intを高い信頼性で求めることができる。このため、実際の各接触対象面の位置及び姿勢の推定を精度よく行なうことができる。

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

前記実施形態では、移動体１として、４つの可動リンク３を備えた移動体を例示したが、５つ以上、又は３つの可動リンクを備えた移動体であってもよい。

また、前記実施形態の移動体１の各可動リンク３の先端部４ｃは、これに接触対象面から作用する接触力モーメントを変化させる（接触力中心点を該先端部の接触面で変化させる）ことができる要素リンク４ｃにより構成されたものであるが、各可動リンクの先端部は、接触力モーメントを変化させることができない構造のものであってもよい。

例えば、図８に示す如く、任意の第ｉ接触対象面に接触する第ｉ接触可動リンクグループのｍ(i)個の可動リンク１０３(i)_j（ｊ＝１，２，…，ｍ(i)）の先端部が球体状のもの（より一般的には、第ｉ接触対象面との接触面が実質的に点状になるもの（該接触面が微小面積となるものを含む））であってもよい。

この場合には、第ｉ接触可動リンクグループの各可動リンク１０３(i)_jの第ｉ接触対象面との点状の接触面（接触点）が接触力中心点に合致することとなるので、該可動リンク１０３(i)_jに接触力モーメントを作用させること（ひいては変化させること）が実質的にできない。そして、この場合には、該可動リンク１０３(i)_jに付加し得る摂動接触力モーメント↑Ｍ(i)_jは常にゼロであるから、該可動リンク１０３(i)_jの接触面のばね性回転変位量↑Ｘrot(i)_j（＝Ｋrot(i)_j・↑Ｍ(i)_j）も常にゼロとなる。ひいては、このばね性回転変位量↑Ｘrot(i)_jと、一般化変数ベクトル↑ｑの単位時間当たりの変化量↑Δｑとの間の関係を表すヤコビアン行列Ｊrot(i)_jも常にゼロ行列となる。

従って、この場合の可動リンクヤコビアン行列Ｊ(i)_jは、前記式（２６ｃ）によって、Ｊ(i)_j＝[Ｊorg(i)_j，０]^Tとなる。このため、前記式（２７）により第ｉ代表接触面ヤコビアン行列Ｊc(i)を算出する処理は、次式（２７ａ）によりＪcを算出する処理と等価である。

この式（２７ａ）におけるｒ(i)_j、Ｒk(i)、VV(i)_jは、それぞれ、前記式（２２）に示したものと同じある。また、Ｊorg(i)_jは、前記式（２６ａ）に示したヤコビアン行列、すなわち、第ｊ可動リンク接触面のばね性並進変位量↑Ｘorg_iと、一般化変数ベクトル↑ｑの単位時間当たりの変化量↑Δｑとの間の関係を表すヤコビアン行列である。

そして、この式（２７ａ）の演算を行なうためのｒ(i)_j、Ｒk(i)、VV(i)_jの値は、前記実施形態と同様の仕方で決定できる。また、Ｊ(i)_j＝[Ｊorg(i)_j，０]^Tであるから、前記実施形態で可動リンクヤコビアン行列Ｊ(i)_jを算出する場合と同様の仕方でＪorg(i)_jを算出することができる。

また、移動体は、例えば図９に示す如く、任意の第ｉ接触対象面に接触する第ｉ接触可動リンクグループのｍ(i)個の可動リンク１０３(i)_j（ｊ＝１，２，…，ｍ(i)）の先端部に車輪を備える構造の移動体であってもよい。

また、本実施形態では、移動体１の姿勢の安定性を高めるために、基準歩容の目標総外力↑ＦＭt_cmd0を修正した上で、第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1を決定するようにしたが、基準歩容の目標総外力↑ＦＭt_cmd0をそのまま用いて第１〜第Ｎ目標全接触力↑ＦＭt(1)_cmd1〜↑ＦＭt(N)_cmd1を決定するようにしてもよい。

また、前記実施形態では、第ｉ目標接触力↑ＦＭt(i)_cmd1と、第ｉ実全接触力↑ＦＭt(i)_actの観測値との偏差↑Ｄfmt(i)（＝↑ＦＭt(i)_cmd1−↑ＦＭt(i)_act）に応じた比例項と積分項とを合成する（加え合わせる）ことによって、第ｉ摂動全床反力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdを決定するようにしたが、積分項（＝↑ΔＦＭt(i)_int）をそのまま、第ｉ摂動全接触力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdとして決定するようにしてもよい。

このようにした場合には、前記代表接触面並進・回転変位量算出部６５により↑ΔＦＭt(i)_dmdから算出される第ｉ代表接触面のばね性並進・回転変位量↑ΔＸc(i)が、想定第ｉ接触対象面の位置及び姿勢の実際の第ｉ接触対象面に対する定常的な誤差分↑Ｘc(i)_intに相当するものとなる。

従って、この場合には、接触対象面推定部７０においては、前記式（２８−１）により↑Ｘc(i)_intを改めて算出する必要はなく、前記代表接触面並進・回転変位量算出部６５により算出された第ｉ代表接触面のばね性並進・回転変位量↑ΔＸc(i)に応じて、想定第ｉ接触対象面の位置及び姿勢を修正することで、第ｉ接触対象面推定値を決定するようにすればよい。このようにすることで、前記第２発明及び第４発明発明の一実施形態が構築されることとなる。

また、前記実施形態では、第１〜第Ｎ接触対象面の全てについて、実際の位置及び姿勢を推定するようにしたが、特定の接触対象面（第ｈ接触対象面）についてのみ、その実際の位置及び姿勢を推定するようにしてもよい。例えば図６に示した移動体１の動作を行なう場合に、床面ＦＬ（第１接触対象面）についてのみ、その実際の位置及び姿勢を接触対象面推定部７０で推定するようにしてもよい。

また、前記実施形態では、接触対象面推定部７０を備えるようにしたが、これを省略してもよい。その場合においては、第ｉ目標接触力↑ＦＭt(i)_cmd1と、第ｉ実全接触力↑ＦＭt(i)_actの観測値との偏差↑Ｄfmt(i)（＝↑ＦＭt(i)_cmd1−↑ＦＭt(i)_act）に応じた比例項を、第ｉ摂動全床反力↑ΔＦＭt(i)の要求値↑ΔＦＭt(i)_dmdとして決定するようにしてもよい。

また、前記実施形態では、２つの接触対象面に移動体１が接触する場合と例にとって説明したが、３つ以上の接触対象面に移動体１が接触するような場合でも、本発明を適用できることはもちろんである。さらに、移動体１を接触させる複数の接触対象面は、そのうちの２つの接触対象面が、互いにほぼ平行なものとなるような接触対象面であってもよい。

１，１０１…移動体、２，１０２…基体、３，１０３…可動リンク、５０…制御装置、６４…全接触力要求修正量決定部（全接触力要求修正量決定手段）、６５…代表接触面並進・回転変位量算出部（代表接触面位置姿勢変位量算出手段）、６６…代表接触面ヤコビアン行列算出部（代表接触面ヤコビアン行列算出手段）、６８…関節変位修正量決定部（関節変位修正量決定手段）、６９…関節変位制御部（関節変位制御手段、７０…接触対象面推定部（接触対象面推定手段）、ＳＴＥＰ２〜９…擬似逆行列演算用パラメータ決定手段。

Claims (6)

1. 基体と、基体に連結された複数の可動リンクと、各可動リンクの関節を駆動する関節アクチュエータとを備えた移動体の動作を、該移動体の動作環境に存在する互いに異なる複数の接触対象面である第１〜第Ｎ接触対象面（Ｎ：２以上の整数）のそれぞれに１つ以上の可動リンクを接触させつつ該移動体の運動を行なわせるための目標運動と、該目標運動を実現するために該移動体に前記第１〜第Ｎ接触対象面のそれぞれから作用させるべき全接触力の目標値である目標全接触力とに応じて制御する制御装置であって、
   前記第１〜第Ｎ接触対象面のそれぞれである第ｉ接触対象面（ｉ＝１，２，…，Ｎ）から前記移動体に実際に作用する全接触力の観測値と、該第ｉ接触対象面に対応する前記目標全接触力との偏差をゼロに近づけるために該第ｉ接触対象面から該移動体に付加的に作用させるべき全接触力の要求修正量である第ｉ全接触力要求修正量（ｉ＝１，２，…，Ｎ）のそれぞれを前記偏差に応じて決定する全接触力要求修正量決定手段と、
   前記第ｉ全接触力要求修正量が前記移動体と第ｉ接触対象面との全ての接触面を代表する単一の仮想的な接触面としての第ｉ代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定して、前記決定された第ｉ全接触力要求修正量と該第ｉ接触対象面に対応してあらかじめ定められた第ｉ代表接触面のばね定数とから、第ｉ代表接触面（ｉ＝１，２，…，Ｎ）のそれぞれの位置及び姿勢の要求変位量を算出する代表接触面位置姿勢変位量算出手段と、
   前記第ｉ代表接触面の位置及び姿勢の時間的変化率と、前記基体の位置及び姿勢と前記移動体の各関節の変位量とを成分として構成される一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である第ｉ代表接触面ヤコビアン行列Ｊc(i)（ｉ＝１，２，…，Ｎ）のそれぞれを、該第ｉ接触対象面に接触している可動リンクのグループである第ｉ接触可動リンクグループの各可動リンクの接触部の位置の時間的変化率又は該第ｉ接触可動リンクグループの各可動リンクの接触部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各可動リンクヤコビアン行列Ｊ(i)_jと、前記第ｉ代表接触面のばね定数と、前記移動体に該第ｉ接触対象面から実際に作用する全接触力の作用点としての全接触力中心点に対する第ｉ接触可動リンクグループの各可動リンクの接触部の実際の接触力中心点の相対位置と、第ｉ接触可動リンクグループの各可動リンクに実際に作用する接触力の値とから次式（２７）により算出する代表接触面ヤコビアン行列算出手段と、
   前記算出された第１〜第Ｎ代表接触面の位置及び姿勢の要求変位量を並べてなる総合要求変位量に、前記算出された第１〜第Ｎ代表接触面ヤコビアン行列Ｊc(i)（ｉ＝１，２，…，Ｎ）を並べてなる総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を乗じることにより前記第１〜第Ｎ代表接触面のそれぞれの位置及び姿勢の要求変位量を実現するための前記移動体の各関節の変位量の修正量である関節変位修正量を決定する関節変位修正量決定手段と、
   前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、前記決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する関節変位制御手段とを備えることを特徴とする移動体の制御装置。
   

   
2. 請求項１記載の移動体の制御装置において、
   前記全接触力要求修正量決定手段は、前記第ｉ全接触力要求修正量（ｉ＝１，２，…，Ｎ）のそれぞれを、第ｉ接触対象面における前記偏差を積分することにより決定することを特徴とする移動体の制御装置。
3. 請求項１記載の移動体の制御装置において、
   前記全接触力要求修正量決定手段は、前記第ｉ全接触力要求修正量（ｉ＝１，２，…，Ｎ）のそれぞれを、少なくとも第ｉ接触対象面における前記偏差に比例する比例項と、該偏差を積分してなる積分項とを合成することにより決定することを特徴とする移動体の制御装置。
4. 請求項２記載の移動体の制御装置において、
   前記第１〜第Ｎ接触対象面のうちのあらかじめ定められた特定の接触対象面である第ｈ接触対象面に対応して前記目標運動で想定されている接触対象面である想定第ｈ接触対象面の位置及び姿勢を、前記代表接触面位置姿勢変位量算出手段により算出された第ｈ代表接触面の前記要求変位量に応じて補正することにより実際の第ｈ接触対象面の位置及び姿勢を推定する接触対象面推定手段をさらに備えることを特徴とする移動体の制御装置。
5. 請求項３記載の移動体の制御装置において、
   前記第１〜第Ｎ接触対象面のうちのあらかじめ定められた特定の接触対象面である第ｈ接触対象面に対応して前記目標運動で想定されている接触対象面である想定第ｈ接触対象面の位置及び姿勢を補正することにより、実際の第ｈ接触対象面の位置及び姿勢を推定する接触対象面推定手段をさらに備えており、
   該接触対象面推定手段は、第ｈ接触対象面に対応する前記第ｈ全接触力要求修正量を組成する前記積分項と該第ｈ接触対象面に対応する前記第ｈ代表接触面のばね定数とから、該積分項に対応する第ｈ代表接触面の位置及び姿勢の変位量である第ｈ代表接触面定常変位量を算出し、該第ｈ代表接触面定常変位量に応じて前記想定第ｈ接触対象面の位置及び姿勢を補正することにより実際の第ｈ接触対象面の位置及び姿勢を推定することを特徴とする移動体の制御装置。
6. 請求項１〜５のいずれか１項に記載の移動体の制御装置において、
   前記算出された総合ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1は、あらかじめ設定された重み行列Ｗと前記算出された総合ヤコビアン行列Ｊcとから次式（３１）により算出される行列であると共に、該式（３１）におけるｋの値を、次式（３２）により表される行列式ＤＥＴが所定の正の閾値以上の値になるように決定する擬似逆行列演算用パラメータ決定手段を備えており、
   
   Ｊc^-1＝Ｗ^-1・Ｊc^T・(Ｊc・Ｗ^-1・Ｊc^T＋ｋ・Ｉ)^-1 ……（３１）
   ＤＥＴ＝det(Ｊc・Ｗ^-1・Ｊc^T＋ｋ・Ｉ) ……（３２）
   ただし、Ｗ：あらかじめ定められた重み行列（対角行列）
   
   前記擬似逆行列演算用パラメータ決定手段は、ｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（３１）により擬似逆行列を算出するために用いるｋの値として決定する手段であると共に、前記判断結果が否定的である場合のｋの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ｊc・Ｗ^-1・Ｊc^Tの次数）に比例する値に設定することを特徴とする移動体の制御装置。

Images