JP5616288B2

JP5616288B2 - 移動体の制御装置

Info

Publication number: JP5616288B2
Application number: JP2011117462A
Authority: JP
Inventors: 豊生織田
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2011-05-25
Filing date: 2011-05-25
Publication date: 2014-10-29
Anticipated expiration: 2031-05-25
Also published as: JP2012245572A

Description

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

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

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

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

特許第３６２９１３３号

特許文献１に見られる如き従来のコンプライアンス制御の技術では、目標全床反力中心点周りに発生する実際の床反力モーメントを、目標モーメントに追従させるための各脚リンクの先端部（足平部）の目標位置及び姿勢の修正は、両脚リンクの先端部の上下方向の位置を互いに逆向きに変化させる修正（以降、本欄では第１の修正という）と、各脚リンクの先端部の姿勢を変化させる修正（以降、本欄では第２の修正という）とを合成したものとされる。

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

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

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

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

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

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

そして、移動体１０１は、Ｎ個の脚リンク１０３_1〜１０３_Nのうちのいずれか１つ又は複数の脚リンクを支持脚として、その支持脚の先端部を接地（床面に接触）させることで移動体１０１の自重を該支持脚を介して床面に支えることが可能であると共に、脚リンク１０３_1〜１０３_Nの空間的な運動を行なうことによって、移動体１０１が床面上で移動可能である。以降、これらの脚リンク１０３_1〜１０３_Nのそれぞれを総称的に第ｉ脚リンク１０３_i（ｉ＝１，２，…，Ｎ）、又は、脚リンク１０３_iという。

この移動体１０１の任意の運動状態において、床面から移動体１０１に作用するトータルの床反力である全床反力（ベクトル）を↑ＦＭt、この↑ＦＭtのうちの並進力ベクトルである全並進床反力を↑Ｆt、↑ＦＭtのうちのモーメントベクトルである全床反力モーメントを↑Ｍtと表記する。

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

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

また、第ｉ脚リンク１０３_i（ｉ＝１，２，…，Ｎ）に床面から作用する床反力（以下、脚床反力という）を↑ＦＭ_i、この↑ＦＭ_iのうちの並進力ベクトルである並進床反力を↑Ｆ_i、↑ＦＭ_iのうちのモーメントベクトルである床反力モーメントを↑Ｍ_iと表記する。

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

補足すると、第ｉ脚リンク１０３_iが非接地状態である場合（↑ＦＭ_i＝ゼロベクトルとなる場合）には、第ｉ脚床反力↑ＦＭ_iの作用点Ｐ_iは存在しないこととなるが、便宜上、該作用点Ｐ_iは任意に設定された点（例えば第ｉ脚リンク１０３_iの先端部の代表点など）とする。

このとき、全床反力↑ＦＭtと脚床反力↑ＦＭ__i（ｉ＝１，２，…，Ｎ）との間の関係は一般的に、次式（１）により表される。

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

ここで、第ｉ脚床反力↑ＦＭ_iの作用点Ｐ_iに摂動分の床反力↑ΔＦＭ_i（＝[↑ΔＦ_i，↑ΔＭ_i]^T）を付加し、それによって、全床反力↑ＦＭtの作用点Ｐｔに摂動分の床反力↑ΔＦＭt（＝[↑ΔＦt，↑ΔＭt]^T）を付加することを想定する。なお、作用点Ｐ_iに摂動分の床反力↑ΔＦＭ_iを付加するということは、作用点Ｐ_iに作用する床反力を↑ＦＭ_iから↑ＦＭ_i＋↑ΔＦＭ_iに変更することを意味する。同様に、作用点Ｐtに摂動分の床反力↑ΔＦＭtを付加するということは、作用点Ｐtに作用する床反力を↑ＦＭtから↑ＦＭt＋↑ΔＦＭtに変更することを意味する。

以降、↑ΔＦＭ_iを第ｉ脚摂動床反力、↑ΔＦＭ_iのうちの並進力ベクトル↑ΔＦ_i及びモーメントベクトル↑ΔＭ_iをそれぞれ、第ｉ脚摂動並進床反力、第ｉ脚摂動床反力モーメントという。また、↑ΔＦＭtを摂動全床反力、↑ΔＦＭtのうちの並進力ベクトル↑ΔＦt及びモーメントベクトル↑ΔＭtをそれぞれ、摂動全並進床反力、摂動全床反力モーメントという。

上記摂動全床反力↑ΔＦＭtと、第ｉ脚摂動床反力↑ΔＦＭ_ｉ（ｉ＝１，２，…，Ｎ）との間の関係は、上記式（１）に基づいて、次式（２）により与えられる。

この式（２）におけるAA_i（ｉ＝１，２，…，Ｎ）は、式（１）のただし書きで定義したものと同じである。

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

このばね性変位による第ｉ脚接地面の位置の変位量（第ｉ脚接地面の３軸方向の並進変位量のベクトル。以降、ばね性並進変位量という）と姿勢の変位量（第ｉ脚接地面の３軸周りの回転変位量のベクトル。以降、ばね性回転変位量という）とをそれぞれ↑Ｘorg_i、↑Ｘrot_iと表記する。このとき、↑Ｘorg_iと↑ΔＦ_iとの間の関係、及び↑Ｘrot_iと↑ΔＭ_iとの間の関係は、それぞれ、次式（３）、（４）により表されることとなる。

↑ΔＦ_i＝Ｋorg_i・↑Ｘorg_i ……（３）
↑ΔＭ_i＝Ｋrot_i・↑Ｘrot_i ……（４）

式（３）のＫorg_iは、第ｉ脚接地面のばね性並進変位量↑Ｘorg_iの各成分毎のばね定数を対角成分として構成される３次の対角行列（以下、並進ばね定数行列Ｋorg_iという）、式（４）のＫrot_iは、第ｉ脚接地面のばね性回転変位量↑Ｘrot_iの各成分毎のばね定数を対角成分として構成される３次の対角行列（以下、回転ばね定数行列Ｋrot_iという）である。

また、移動体１０１と床面との全ての接触面（接地状態の全ての脚リンク１０３の接地面）を代表する単一の仮想的な接触面としての代表接触面を想定し、この代表接触面にて全床反力↑ＦＭtが移動体１０１に作用するものとする。該代表接触面は、所謂、支持多角形に相当するものである。

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

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

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

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

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

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

前記式（２）において、第ｉ脚床反力↑ＦＭ_i（ｉ＝１，２，…，Ｎ）の作用点Ｐ_iにそれぞれ付加する第ｉ脚摂動並進床反力↑ΔＦ_iによって全床反力↑ＦMtの作用点Ｐtに付加される摂動全並進床反力↑ΔＦtに着目すると、次式（７）が得られる。

↑ΔＦt＝↑ΔＦ_1＋↑ΔＦ_2＋……＋↑ΔＦ_N ……（７）

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

Ｋc_org・↑Ｘc_org＝Ｋorg_1・↑Ｘorg_1＋Ｋorg_2・↑Ｘorg_2
＋……＋Ｋorg_N・↑Ｘorg_N ……（８）

一方、摂動全床反力↑ΔＦＭtの付加前の全床反力↑ＦＭtの作用点Ｐtを、全床反力中心点（ＣＯＰ）とし、第ｉ脚摂動床反力↑ΔＦＭ_iの付加前の第ｉ脚床反力↑ＦＭ_iの作用点Ｐ_iを第ｉ脚接地面の床反力中心点としたとき、任意の基準点に対する作用点Ｐtの位置ベクトル（これを↑Ｐtと表記する）と、作用点Ｐ_i（ｉ＝１，２，…，Ｎ）のそれぞれの位置ベクトル（これを↑Ｐ_iと表記する）との間の関係は、次式（９）により与えられる。

なお、全床反力中心点は、全床反力↑ＦＭtの作用点であって、その点周りの全床反力モーメント↑Ｍtの水平成分（水平軸周り成分）がゼロとなるような点である。同様に、第ｉ脚接地面の床反力中心点は、第ｉ脚床反力↑ＦＭ_iの作用点であって、その点周りの床反力モーメント↑Ｍ_iの水平成分（水平軸周り成分）がゼロとなるような点である。

↑Ｐt＝ｒ_1・↑Ｐ_1＋ｒ_2・↑Ｐ_2＋……＋ｒ_N・↑Ｐ_N ……（９）

この式（９）におけるｒ_i（ｉ＝１，２，…，Ｎ）は、ｒ_i≡Fn_i／Ｆntにより定義される重み係数である。ここで、Ｆn_i（ｉ＝１，２，…，Ｎ）は、第ｉ脚並進床反力↑Ｆ_iのうちの床面に垂直な垂直抗力成分の絶対値（以降、床面垂直抗力成分という）、Ｆntは全並進床反力↑Ｆtのうちの床面に垂直な垂直抗力成分の絶対値（床面垂直抗力成分）であり、Ｆnt＝Ｆn_1＋Ｆn_2＋……＋Ｆn_Nである。従って、重み係数ｒ_iは、全並進床反力↑Ｆtの床面垂直抗力成分Ｆntに対する、第ｉ脚床反力↑Ｆ_iの床面垂直抗力成分Ｆn_iの比率であり、その値は、０≦ｒ_i≦１の範囲内の値である。

なお、本明細書では、Ｆn_i，Ｆntは、それぞれ、↑Ｆ_iの鉛直成分（Ｚ軸成分）、↑Ｆtの鉛直成分（Ｚ軸成分）に一致又はほぼ一致するものとする。

そして、この式（９）と同様の関係が、代表接触面の位置のばね性並進変位量↑Ｘc_orgと、第ｉ脚接地面の位置のばね性並進変位量↑Ｘorg_iとの間にも成立するものと仮定する。すなわち、次式（１０）が成立するものと仮定する。

↑Ｘc_org＝ｒ_1・↑Ｘorg_1＋ｒ_2・↑Ｘorg_2＋……＋ｒ_N・↑Ｘorg_N ……（１０）

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

Ｋorg_i＝ｒ_i・Ｋc_org ……（１１）

本発明では、上記式（１０）が、代表接触面のばね性並進変位量↑Ｘc_orgと第ｉ脚接地面（ｉ＝１，２，…，Ｎ）のばね性並進変位量↑Ｘorg_iとの間の関係を表す基本式、式（１１）が、代表接触面の並進ばね定数行列Ｋc_orgと第ｉ脚接地面（ｉ＝１，２，…，Ｎ）の並進ばね定数行列Ｋorg_iとの関係を表す基本式とされる。

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

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

そして、２つ以上の脚リンク１０３が接地している場合には、代表接触面のばね性並進変位量↑Ｘc_orgは、非接地状態の脚リンクを除く各脚リンク１０３_iに対応する第ｉ脚接地面のばね性並進変位量↑Ｘorg_iの重み付き平均値（ｒ_iを重み係数とする重み付き平均値）となる。

次に、前記式（２）において、第１〜第Ｎ脚床反力↑ＦＭ_1〜↑ＦＭ_Nのそれぞれの並進床反力↑Ｆ_1〜↑Ｆ_Nを一定に保持した状態で、第ｉ脚床反力↑ＦＭ_i（ｉ＝１，２，…，Ｎ）の作用点Ｐ_iに第ｉ脚摂動床反力モーメント↑ΔＭ_iを付加することによって全床反力↑ＦMtの作用点Ｐtに付加される摂動全床反力モーメント↑ΔＭtに着目すると、次式（１２）が得られる。

↑ΔＭt＝↑ΔＭ_1＋↑ΔＭ_2＋……＋↑ΔＭ_N ……（１２）

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

Ｋc_rot・↑Ｘc_rot＝Ｋrot_1・↑Ｘrot_1＋Ｋrot_2・↑Ｘrot_2
＋……＋Ｋrot_N・↑Ｘrot_N ……（１３）

一方、第ｉ脚床反力↑ＦＭ_iの作用点Ｐ_iを第ｉ脚接地面の床反力中心点としたとき、第１〜第Ｎ脚床反力↑ＦＭ_1〜↑ＦＭ_Nのそれぞれの並進床反力↑Ｆ_1〜↑Ｆ_Nを一定に保持した状態で、第ｉ脚床反力↑ＦＭ_iの作用点Ｐ_i（床反力中心点）に第ｉ脚摂動床反力モーメント↑ΔＭ_iを付加するということは、第ｉ脚接地面での床反力中心点の水平方向位置を点Ｐ_iからずらすことと等価である。同様に、全床反力↑ＦＭtの作用点Ｐtを全床反力中心点（ＣＯＰ）としたとき、全床反力↑ＦＭtの作用点Ｐtに摂動全床反力モーメント↑ΔＭtが付加されるということは、全床反力中心点の水平方向位置が点Ｐtからずれることと等価である。

この場合、第ｉ脚接地面での床反力中心点の水平方向位置の変位量（２成分の変位量ベクトル）を↑ΔＲpt_i、全床反力中心点の水平方向位置の変位量（２成分の変位量ベクトル）を↑ΔＣＯＰとすると、↑ΔＣＯＰ・Ｆnt≒↑ΔＭtの水平軸周り成分（Ｘ軸及びＹ軸周り成分）、↑ΔＲpt_i・Ｆn_i≒↑ΔＭ_iの水平軸周り成分（Ｘ軸及びＹ軸周り成分）となる。このため、上記式（１２）から、次式（１４）が得られる。

↑ΔＣＯＰ＝ｒ_1・↑ΔＲpt_1＋ｒ_2・↑ΔＲpt_2
＋……＋ｒ_N・↑ΔＲpt_N ……（１４）

なお、式（１４）のｒ_i（ｉ＝１，２，…，Ｎ）は、前記した重み係数ｒ_i（≡Ｆn_i／Ｆnt）である。

また、第１〜第Ｎ脚接地面は、共通の床面上の部分であるから、第１〜第Ｎ脚接地面のそれぞれに作用する垂直抗力Ｆn_iが互いに同じである場合、第ｉ脚接地面の床反力中心点の水平方向位置の変位量↑ΔＲpt_iに対応する摂動床反力モーメント（＝↑ΔＲpt_i・Ｆn_i）と、第ｉ脚接地面のばね性変位による回転変位量↑Ｘrot_iの水平軸周り成分との間の関係は、いずれの第ｉ脚接地面についても同じになると考ええられる。

そこで、第１〜第Ｎ脚接地面のそれぞれに作用する床面垂直抗力成分Ｆn_iが互いに同じ値（これをＦnaとおく）である場合、次式（１５）が成立すると考えられる。

↑ΔＲpt_i・Ｆna＝Ｋrot・↑Ｘrot_i_xy ……（１５）

なお、↑Ｘrot_i_xyは、第ｉ脚接地面のばね性回転変位量↑Ｘrot_iの水平軸周り成分（Ｘ軸及びＹ軸周り成分）であり、Ｋrotは、２次の対角行列（Ｘ軸周り及びＹ軸周りの回転に関するばね定数行列）である。

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

↑ΔＣＯＰ・Fna＝Ｋrot・↑Ｘc_rot_xy ……（１６）

なお、↑Ｘc_rot_xyは、代表接触面のばね性回転変位量↑Ｘc_rotの水平軸周り成分（Ｘ軸及びＹ軸周り成分）である。

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

Ｋrot・↑Ｘc_rot_xy＝ｒ_1・Ｋrot・↑Ｘrot_1_xy＋ｒ_2・Ｋrot・↑Ｘrot_2_xy
＋……＋ｒ_N・Ｋrot・↑Ｘrot_N_xy ……（１７）

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

↑Ｘc_rot_xy＝ｒ_1・↑Ｘrot_1_xy＋ｒ_2・↑Ｘrot_2_xy
＋……＋ｒ_N・↑Ｘrot_N_xy ……（１８）

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

↑Ｘc_rot＝ｒ_1・↑Ｘrot_1＋ｒ_2・↑Ｘrot_2
＋……＋ｒ_N・↑Ｘrot_N ……（１９）

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

Ｋrot_i＝ｒ_i・Ｋc_rot ……（２０）

本実施形態では、上記式（１９）が、代表接触面のばね性回転変位量↑Ｘc_rotと第ｉ脚接地面（ｉ＝１，２，…，Ｎ）のばね性回転変位量↑Ｘrot_iとの間の関係を表す基本式、式（２０）が、代表接触面の回転ばね定数行列Ｋc_rotと第ｉ脚接地面（ｉ＝１，２，…，Ｎ）の回転ばね定数行列Ｋrot_iとの関係を表す基本式とされる。

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

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

そして、２つ以上の脚リンク１０３が接地している場合には、代表接触面のばね性回転変位量↑Ｘc_rotは、非接地状態の脚リンクを除く各脚リンク１０３_iに対応する第ｉ脚接地面のばね性回転変位量↑Ｘrot_iの重み付き平均値（ｒ_iを重み係数とする重み付き平均値）となる。

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

なお、前記式（１１）、（２０）は、全床反力↑ＦＭtの作用点Ｐtが全床反力中心点、第ｉ脚床反力↑ＦＭ_iの作用点Ｐ_iが第ｉ脚接地面の床反力中心点であることを前提としているので、式（２１）におけるVV_i（ｉ＝１，２，…，Ｎ）は、全床反力中心点としての作用点Ｐtに対する第ｉ脚接地面の床反力中心点の位置ベクトルを改めて↑Ｖ_iとしたとき、次のように定義される行列である。

VV_i：VV_i・↑Ｆ_i＝Ｖ_i×↑Ｆ_iとなる行列
ただし、↑Ｖ_i：全床反力中心点に対する第ｉ脚接地面の床反力中心点の位置ベクトル

なお、この場合、上記全床反力中心点は、より詳しくは、摂動全床反力↑ΔＦＭtが付加される前の全床反力↑ＦＭtに対応する全床反力中心点であり、第ｉ脚接地面の床反力中心点は、第ｉ脚摂動床反力↑ΔＦＭ_iが付加される前の第ｉ脚床反力↑ＦＭ_iに対応する床反力中心点である。

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

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

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

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

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

↑Ｘ_i＝Ｊ_i・↑Δｑ ……（２４）

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

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

同様に、第ｉ脚接地面の位置の単位時間当たりの変位量と↑Δｑとの間の関係を表現するヤコビアン行列（↑Ｘorg_iと↑Δｑとの間の関係を次式（２６ａ）により表す行列）をＪorg_i、第ｉ脚接地面の姿勢の単位時間当たりの変位量と↑Δｑとの間の関係を表現するヤコビアン行列（↑Ｘrot_iと↑Δｑとの間の関係を次式（２６ｂ）により表す行列）をＪrot_iと表記すると、式（２６ｃ）に示す如くＪ_i＝[Ｊorg_i，Ｊrot_i]^Tとなる。

↑Ｘorg_i＝Ｊorg_i・↑Δｑ ……（２６ａ）
↑Ｘrot_i＝Ｊrot_i・↑Δｑ ……（２６ｂ）
Ｊ_i＝[Ｊorg_i，Ｊrot_i]^T ……（２６ｃ）

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

従って、代表接触面の位置及び姿勢の変位に関するヤコビアン行列Ｊc（以降、代表接触面ヤコビアン行列Ｊcという）は、各脚接地面の位置及び姿勢の変位に関するヤコビアン行列Ｊ_i（ｉ＝１，２，…，Ｎ）から、上記の式（２７）に基づいて決定できることとなる。

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

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

一方、移動体１０１の所要の状態を実現するための要求操作量として、ある値の摂動全床反力↑ΔＦＭtが与えられたとき、この↑ΔＦＭtは前記式（５），（６）に基づいて、代表接触面のばね性並進・回転変位量↑Ｘｃ（＝[↑Ｘc_org，↑Ｘc_rot]^T）の要求値に変換できることとなる。この変換は、次式（２８）により与えられる。

そして、代表接触面ヤコビアン行列Ｊcの擬似逆行列をＪc^-1とおくと、代表接触面のばね性並進・回転変位量変位量↑Ｘcの要求値から、次式（２９）により、摂動全床反力↑ΔＦＭtの要求値を実現するための、移動体１０１の各関節の変位量の修正量を決定できることとなる。

↑Δｑ＝Ｊｃ^-1・↑Ｘｃ ……式（２９）

そこで、移動体１０１の任意の運動状態において、移動体１０１に作用する全床反力を制御するための要求操作量として、摂動全床反力↑ΔＦＭtの要求値が与えられたとき、前記式（２７）に基づいて代表接触面ヤコビアン行列Ｊcを求めると共に、このＪcの擬似逆行列Ｊc^-1を決定する。さらに、前記式（２８）に基づいて、↑ΔＦＭtの要求値に対応する代表接触面の変位量ベクトル↑Ｘcを求める。そして、この↑Ｘcと、Ｊｃ^-1とから式（２９）によって、摂動全床反力↑ΔＦＭtの要求値を実現するための各関節の変位量の修正量を決定できることとなる。

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

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

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

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

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

なお、本発明において、「床面」は、屋内の通常の床面に限らず、屋外の地面もしくは路面であってもよい。

上記第１発明によれば、前記移動体に実際に作用する全床反力（以降、実全床反力ということがある）の観測値と、前記目標全床反力との偏差をゼロに近づけるための全床反力要求修正量が全床反力要求修正量決定手段により決定される。すなわち、実全床反力を目標全床反力に追従させるためのフィードバック操作量（制御入力）として、前記全床反力要求修正量が決定される。この全床反力要求修正量は、前記摂動全床波力↑ΔＦＭtの要求値に相当するものである。

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

そして、決定された全床反力要求修正量に対応する代表接触面の位置及び姿勢の要求変位量が代表接触面位置姿勢変位量算出手段により算出される。

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

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

従って、全床反力要求修正量に対応する代表接触面の位置及び姿勢の要求変位量に、代表接触面ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を乗じることによって（すなわち、前記式（２９）によって）、当該要求変位量を実現するための（ひいては、全床反力要求修正量を実現するための）移動体の関節変位修正量を決定できることとなる。

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

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

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

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

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

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

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

これにより、実全床反力を目標全床反力に追従させるように、移動体の各関節の変位量の制御が行われ、所謂、コンプライアンス制御が実現される。

かかる第１発明によれば、全床反力要求修正量を代表接触面の位置及び姿勢の要求変位量に変換し、この要求変位量に応じて移動体の各関節の関節変位修正量を求めることにより、個々の脚リンクの先端部の位置や姿勢の修正と実全床反力の変化との関係やそれらの相互の関係を考慮して各脚リンクの先端部の位置や姿勢の修正量を決定するような処理を実行することなく、実全床反力を目標全床反力に追従させるための移動体の各関節の変位量の修正量（関節変位修正量）を一括的に決定できる。そのため、関節変位修正量を決定する処理を効率よく短時間で行なうことができる。

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

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

よって、第１発明によれば、移動体に実際に作用する全床反力を目標全床反力に追従させるように該移動体の運動を制御することを、各脚リンクの先端部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行うことができる。

上記第１発明においては、前記全床反力要求修正量決定手段が決定する全床反力要求修正量は、前記偏差に比例させた値として決するようにしてもよいが、前記偏差に比例する比例項と、前記偏差の積分項との総和であることが好ましい（第２発明）。

すなわち、前記移動体に実際に作用する全床反力の観測値と、前記目標全床反力との偏差には、目標運動の作成に際して想定されている想定床面で想定されていない実際の床面の局所的な凹凸等に起因する一時的な偏差と、実際の床面の全体的な位置及び姿勢の想定床面に対する定常的なずれに起因する定常的な偏差とがある。そして、第２発明によれば、前者の偏差を前記比例項により補償することができると共に、後者の偏差を前記積分項により補償することができる。

また、上記第１発明又は第２発明において、前記代表接触面ヤコビアン行列Ｊ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であるとする。

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

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

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

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

このコンプライアンス操作量決定処理では、制御装置５０は、まず、姿勢安定化補償力決定部６２の処理を実行する。この姿勢安定化補償力決定部６２は、基準歩容の目標上体姿勢と、移動体１の上体２の実際の姿勢（実上体姿勢）の観測値との偏差をゼロに近づけるために移動体１に付加的に作用させるべき摂動床反力である姿勢安定化補償力↑Mdmdを、基準歩容の目標全床反力を修正するための修正量として決定する機能部である。

上記姿勢安定化補償力↑Ｍdmdは、本実施形態では、基準歩容の目標全床反力中心点周りに付加する水平軸周り（Ｘ軸周り及びＹ軸周り）の摂動モーメントとされる。従って、姿勢安定化補償力↑Ｍdmdは、基準歩容の目標全床反力のうち、目標床反力モーメントの水平軸周り成分を修正する操作量である。換言すれば、姿勢安定化補償力↑Ｍdmdは、全床反力中心点の目標を基準歩容の目標全床反力中心点からずらす機能を有する操作量である。

そして、姿勢安定化補償力決定部６２は、姿勢安定化補償力↑Ｍdmdを、前記姿勢センサ５１の出力により示される実上体姿勢の観測値と、基準歩容の目標上体姿勢との偏差から所定のフィードバック制御則により決定する。そのフィードバック制御則としては、例えばＰＤ則が用いられる。

すなわち、本実施形態では、実上体姿勢の観測値と目標上体姿勢との偏差に所定値のゲインを乗じた比例項と、該偏差の時間的変化率に所定値のゲインを乗じた微分項とを加え合わせることにより姿勢安定化補償力（モーメント）↑Ｍdmdが決定される。この場合、↑ＭdmdのＸ軸周り成分は、上記偏差のＸ軸周り成分に応じて決定され、↑ＭdmdのＹ軸周り成分は、上記偏差のＹ軸周り成分に応じて決定される。

なお、実上体姿勢の観測値と目標上体姿勢との偏差は、移動体１の実際の全体重心と基準歩容の目標運動における全体重心との間の水平方向の位置偏差にほぼ比例する。従って、移動体１の実際の全体重心の水平位置の観測値と、基準歩容の目標運動における移動体１の全体重心の水平位置との偏差を、実上体姿勢の目標上体姿勢からのずれの程度を表す指標として用い、この偏差に応じてＰＤ則等のフィードバック制御則により姿勢安定化補償力（モーメント）↑Ｍdmdを決定するようにしてもよい。この場合、移動体１の実際の全体重心の位置は、例えば、移動体１の各関節の実変位量の観測値（計測値）と、移動体１の剛体リンクモデルとを用いて推定することが可能である。

補足すると、本実施形態では、↑ＭdmdのＸ軸周り成分及びＹ軸周り成分の２成分であるが、Ｚ軸周り成分をさらに含めてもよい。その場合、例えば、基準歩容の目標上体姿勢のうちのＺ軸周り成分と、実上体姿勢の観測値のうちのＺ軸周り成分との偏差に応じてＰＤ則等のフィードバック制御則により、↑ＭdmdのＺ軸周り成分を決定するようにすればよい。

［目標全床反力修正部６３の処理］
次いで、制御装置５０は、目標全床反力修正部６３の処理を実行する。この目標全床反力修正部６３は、上記の如く決定した姿勢安定化補償力↑Ｍdmdにより基準歩容の目標全床反力↑ＦＭt_cmd0を補正する機能部である。この目標全床反力修正部６３は、上記の如く決定された姿勢安定化補償力↑Ｍdmdを基準歩容の目標床反力に加え合わせることによって、修正後目標床反力を決定する。この場合、修正後目標床反力は、基準歩容の目標全床反力中心点周りの目標床反力モーメントのうちのＸ軸周り成分及びＹ軸周り成分だけを基準歩容の目標床反力から修正したものである。

以降、修正後目標床反力のうちの並進床反力（３成分の縦ベクトル）及び床反力モーメント（３成分の縦ベクトル）をそれぞれ↑Ｆt_cmd1、↑Ｍt_cmd1と表記し、これらを合わせた６成分の縦ベクトル（＝[↑Ｆt_cmd1，↑Ｍt_cmd1]^T）を修正後目標床反力↑ＦＭt_cmd1と表記する。この場合、↑Ｆt_cmd1＝↑Ｆt_cmd0である。また、↑Ｍt_cmd1＝↑Ｍt_cmd0＋↑Ｍdmdである（ただし、本実施形態では↑ＭdmdのＺ軸周り成分はゼロとする）。

［全床反力要求修正量決定部６４の処理］
次いで、制御装置５０は、全床反力要求修正量決定部６４の処理を実行する。この全床反力要求修正量決定部６４は、上記の如く決定した修正後目標床反力↑ＦＭt_cmd1に、移動体１に作用する実際の全床反力（実全床反力）の観測値を追従させるように全床反力の要求修正量を決定する機能部である。

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

また、移動体１に作用する実全床反力のうちの並進床反力の観測値（３成分の縦ベクトル）を↑Ｆt_act、実全床反力のうちの床反力モーメントの観測値（３成分の縦ベクトル）を↑Ｍt_actと表記し、さらに、これらの↑Ｆt_act，↑Ｍt_actを合わせた６成分の縦ベクトルとしての実全床反力の観測値を↑ＦＭt_act（＝[↑Ｆt_act，↑Ｍt_act]^T）と表記する。

全床反力要求修正量決定部６４は、以下に説明する処理によって、上記摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdを決定する。

すなわち、全床反力要求修正量決定部６４は、まず、修正後目標床反力↑ＦＭt_cmd1と、実全床反力の観測値↑ＦＭt_actとの偏差↑Ｄfmt（＝↑ＦＭt_cmd1−↑ＦＭt_act）を算出する。この場合、実全床反力の観測値↑ＦＭt_actは、前記力センサ５２Ｒ，５２Ｌの出力により示される各脚リンク３毎の床反力（６軸力）の観測値を、目標全床反力中心点を作用点として合成することにより算出される。

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

次いで、全床反力要求修正量決定部６４は、次式（４１）に示す如く、上記偏差↑Ｄfmtに応じて決定した比例項（式（４１）の右辺の第１項）と積分項（式（４１）の右辺の第２項）と加え合わせることによって、摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdを決定する。

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

↑ΔＦＭt_dmd＝Ｋcmp・↑Ｄfmt＋∫（Ｋestm・↑Ｄfmt_filt） ……（４１）

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

ここで、式（４１）の積分項は、修正後目標床反力↑ＦＭt_cmd1と、実全床反力の観測値↑ＦＭt_actとの定常偏差に相当し、これは、実床面に対する想定床面の位置及び姿勢の誤差分に相当するものである。

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

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

この場合、本実施形態では、移動体１の脚リンク３Ｒ，３Ｌの接地面を代表する代表接触面の並進ばね定数行列Ｋc_orgの各対角成分（代表接触面の３軸方向の並進変位に関する各成分のばね定数）と、該代表接触面の回転ばね定数行列Ｋc_rotの各対角成分（代表接触面の３軸周りの回転変位に関する各成分のばね定数）とはあらかじめ定められた定数値とされている。

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

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

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

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

本実施形態では、代表接触面ヤコビアン行列算出部６６は、前記式（２７）において、Ｎ＝２とした次式（４３）により代表接触面ヤコビアン行列Ｊcを算出する。なお、本実施形態では、両脚リンク３Ｒ，３Ｌのうちのいずれか一方と他方とが、それぞれ式（２７）における第１脚リンク、第２脚リンクに相当する。以降の説明では、便宜上、例えば脚リンク３Ｒを第１脚リンク、脚リンク３Ｌを第２脚リンクとする。

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

重み係数ｒ_i（ｉ＝１，２）に関し、ロータリエンコーダ５４の出力により示される各関節の実際の変位量（実関節変位量）の観測値（計測値）に基づいて、床面に対する各脚リンク３の足平部１８の現在の実姿勢の観測値が決定される。さらに、各脚リンク３の足平部１８の実姿勢の観測値と、前記力センサ５２Ｒ，５２Ｌの出力とを基に、各脚リンク３毎の現在の実際の床面垂直抗力成分Ｆn_1，Ｆn_2（実床面垂直抗力成分Ｆn_1，Ｆn_2）の観測値が求められる。そして、各脚リンク３毎の実床面垂直抗力成分Ｆn_1，Ｆn_2の観測値から、式（４３−１）により示される定義式に従って、重み係数ｒ_i（ｉ＝１，２）が決定される。なお、この場合、重み係数ｒ_iの値が頻繁に変動するのを防止するために、Ｆn_iの観測値にローパスフィルタ等のフィルタリング処理を施してもよい。

補足すると、重み係数ｒ_iは、各脚リンク３毎の実床面垂直抗力成分Ｆn_iの観測値に基づいて決定することが望ましいが、該観測値の代わりに、実床面垂直抗力成分Ｆn_iの近似的な推定値もしくは予測値を使用して、重み係数ｒ_iを決定するようにしてもよい。例えば基準歩容の構成要素として、各脚リンク３毎の目標床反力を含み、且つ、各脚リンク３毎の実床反力が目標床反力に近似的に一致すると考えられる場合には、実床面垂直抗力成分Ｆn_iの観測値の代わりに目標値を使用して、重み係数ｒ_iを決定するようにしてもよい。

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

また、行列Ａ_i（ｉ＝１，２）に関する行列VV_iを決定するために必要となる位置ベクトル↑Ｖ_iは、例えば、次のように決定される。

ロータリエンコーダ５４の出力により示される各関節の現在の実変位量の観測値（計測値）と、前記力センサ５２Ｒ，５２Ｌの出力とを基に、各脚リンク３の接地面における現在の実際の床反力中心点（実床反力中心点）の位置ベクトル（以降、これを↑Ｖa_i（ｉ＝１，２）と表記する）が特定される。なお、この位置ベクトルの基準点は任意の点でよい。

次いで、各脚リンク３に対応する実床反力中心点を結ぶ線分を前記重み係数ｒ_i（ｉ＝１，２）の比率で内分してなる内分点の位置ベクトル↑Ｖc（＝ｒ_1・↑Ｖa_1＋ｒ_2・↑Ｖa_2）を、実全床反力中心点の位置ベクトルとして算出する。

そして、↑Ｖ_i＝↑Ｖa_i−↑Ｖc（ｉ＝１，２）により、実全床反力中心点に対する各脚リンク３の接地面の実床反力中心点の位置ベクトル↑Ｖ_1，↑Ｖ_2がそれぞれ決定される。

なお、↑Ｖ_1＝↑Ｖa_1−↑Ｖc＝ｒ_2・（↑Ｖa_1−↑Ｖa_2）、↑Ｖ_2＝↑Ｖa_2−↑Ｖｃ＝ｒ_1・（↑Ｖa_2−↑Ｖa_1）となるので、これらの式に基づいて、位置ベクトル↑Ｖ_1，↑Ｖ_2を決定するようにしてもよい。この場合、↑Ｖcを算出する必要はない。

そして、行列Ａ_i（ｉ＝１，２）に関する行列VV_iは、定義に従って、VV_i・↑Ｆ_i＝↑Ｖ_i×↑Ｆ_iとなるように決定される。

なお、非接地状態の脚リンク３（Ｆn_i＝０となる脚リンク３）については、実床反力中心点が存在しないので、該脚リンク３に対応する↑Ｖ_iは不定となる。この場合、Ｆn_i＝０となる脚リンク３に対応する重み係数ｒ_iはゼロであるので、Ａ_iやＪ_iの成分値によらずに、ｒ_i・Ａ_i・Ｊ_i＝０となる。従って、非接地状態の脚リンク３に対応する行列VV_i（ひいては行列Ａ_i）を決定する必要はない。このことは、以下に説明する脚リンクヤコビアン行列Ｊ_iについても同様である。

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

ここで、↑Ｘ_iのうちのばね性並進変位量↑Ｘorg_iは、第ｉ脚接地面の単位時間当たりの並進変位量であるとみなすと、第ｉ脚リンク３の足平部１８の現在の接地部分の位置の時間的変化率（並進速度）に一致すると考えてよい。また、↑Ｘ_iのうちのばね性回転変位量↑Ｘrot_iは、第ｉ脚接地面の単位時間当たりの回転変位量であるとみなすと、第ｉ脚リンク３の足平部１８の現在の姿勢の時間的変化率（角速度）に一致すると考えてよい。

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

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

なお、この場合、第ｉ脚リンク３の足平部１８の接地部分の現在の位置は、第ｉ脚リンク３の接地面の現在の実床反力中心点の位置の観測値に一致するものとされる。そして、この点の位置の変化（↑ｑの各成分の微小変化に対する変化）が、該足平部１８の接地部分の位置の変化として算出される。

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

本実施形態では、代表接触面ヤコビアン行列算出部６６は、以上の如く決定した重み係数ｒ_1，ｒ_2、行列VV_1，VV_2、及び脚リンクヤコビアン行列Ｊ_1，Ｊ_2を用いて、前記式（４３）の右辺の演算を行なうことで、代表接触面ヤコビアン行列Ｊcを算出する。

［関節変位修正量決定部６８の処理］
コンプライアンス操作量決定処理では、制御装置５０は、以上の如く、代表接触面のばね性並進・回転変位量↑Ｘcと、代表接触面ヤコビアン行列Ｊcとを算出した後、関節変位修正量決定部６８の処理を実行する。

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

この関節変位修正量決定部６８の処理では、まず、代表接触面ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1が算出される。ここで、一般化変数ベクトル↑ｑの成分数、ひいては、↑Δｑの成分数は、ばね性並進・回転変位量↑Ｘcの成分数よりも多いので、代表接触面ヤコビアン行列Ｊcは、正方行列ではない。従って、Ｊcの逆行列は存在しない。このため、関節変位修正量決定部６８は、代表接触面ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を算出する。

そして、関節変位修正量決定部６８は、前記式（２９）で示す如く、この擬似逆行列Ｊｃ^-1を代表接触面のばね性並進・回転変位量↑Ｘcに乗じることによって、前記一般化変数ベクトル↑ｑの各成分の要求修正量（要求摂動量）を構成要素とする一般化変数要求修正量ベクトル↑Δｑ_dmdを算出する。そして、この一般化変数要求修正量ベクトル↑Δｑ_dmdの成分のうち、各関節の変位量の要求修正量を示す成分が、関節変位修正量として決定される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

関節変位修正量決定部６８は、以上説明した如く算出した擬似逆行列Ｊｃ^-1を用いて、前記式（２９）の右辺の演算を行なうことで、一般化変数要求修正量ベクトル↑Δｑ_dmdを算出する。そして、関節変位修正量決定部６８は、この一般化変数要求修正量ベクトル↑Δｑ_dmdの成分のうち、各関節の変位量の要求修正量を示す成分を、関節変位修正量として決定する。このようにして決定される関節変位修正量が、実全床反力を修正後目標全床反力↑ＦＭt_cmd1に近づけるための各関節の変位量の要求修正量としてのコンプライアンス操作量である。

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

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

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

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

ここで、本実施形態と本発明との対応関係について補足しておく。本実施形態では、全床反力要求修正量決定部６４によって、本発明における全床反力要求修正量決定手段が実現される。この場合において、本実施形態では、修正後目標全床反力↑ＦＭt_cmd1が、本発明における目標全床反力に相当する。

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

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

以上説明した実施形態によれば、制御装置５０は、各制御処理周期において、修正後目標全床反力↑ＦＭt_cmd1に実全床反力を追従させるための操作量（制御入力）たる摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdを、代表接触面のばね性並進・回転変位量↑Ｘcに変換する。さらに、制御装置５０は、この↑Ｘcに代表接触面ヤコビアン行列Ｊcの擬似逆行列Ｊc^-1を乗じることによって、↑Ｘcを実現するための（ひいては実全床反力を修正号目標全床反力↑ＦＭt_cmd1に追従させるための）コンプライアンス操作量である移動体１の各関節の関節変位修正量を決定する。そして、制御装置５０は、基準歩容の目標運動に対応する各関節の目標関節変位量を、関節変位修正量により修正してなる修正後目標関節変位量に応じて各関節の変位量を電動モータ４１（関節アクチュエータ）を介して制御する。

これにより、本実施形態によれば、個々の脚リンク３の先端部（足平部１８）の位置や姿勢の修正と実全床反力の変化との関係やそれらの相互の関係を考慮して各脚リンク３の先端部の位置や姿勢の修正量を決定するような処理を実行することなく、実全床反力を修正後目標全床反力↑ＦＭt_cmd1に追従させるための各関節の関節変位修正量を一括的に決定できる。そのため、コンプライアンス制御用の関節変位修正量を決定する処理を効率よく短時間で行なうことができる。

この場合、代表接触面ヤコビアン行列Ｊcを算出するための前記式（４３）における各脚リンク３_iの重み係数ｒ_iは、床面垂直抗力成分Ｆn_iが相対的に大きい脚リンク３_iほど、より大きい値（“１”に近い値）に設定されることとなる。

このため、摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdに対する各脚リンク３_iの負担分は、結果的に、床面垂直抗力成分Ｆn_iが相対的に大きい脚リンク３_iほど、大きくなる。換言すれば、摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdが、できるだけ、床面垂直抗力成分Ｆn_iが相対的に大きい脚リンクの先端部の位置や姿勢の修正によって実現することができるように代表接触面ヤコビアン行列Ｊcが決定される。

従って、床面垂直抗力成分Ｆn_iが相対的に小さい脚リンク３_iの先端部の位置や姿勢を不必要に修正したりすることなく、摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdを確実に実現し得るような関節変位修正量、すなわち、実全床反力を修正後目標全床反力↑ＦＭt_cmd1に追従させる上で適切な関節変位修正量を決定できる。

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

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

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

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

前記実施形態では、移動体１は、２つの脚リンク３Ｒ，３Ｌを備えた脚式移動ロボットであるが、３つ以上の脚リンクを備えたロボットであってもよい。その場合、制御装置５０の処理のうち、代表接触面ヤコビアン行列算出部６６以外の処理は前記実施形態と同じでよい。そして、代表接触面ヤコビアン行列算出部６６の処理では、前記式（２７）のＮの値を、脚リンクの個数とした式に基づいて、代表接触面ヤコビアン行列Ｊcを算出すればよい。なお、この場合の演算に必要な重み係数ｒ_i、行列Ａ_i、脚リンクヤコビアン行列Ｊ_iの算出の仕方は、前記実施形態と同様である。

前記実施形態の移動体１は、各脚リンク３の先端部は、これに作用する床反力モーメントを変化させる（床反力中心点を該先端部の接地面で変化させる）ことができる足平部１８により構成されたものであるが、各脚リンクの先端部は、床反力モーメントを変化させることができない構造のものであってもよい。

例えば、図６に示す如く、各脚リンク１０３_i（ｉ＝１，２，…，Ｎ）の先端部が球体状のもの（より一般的には、床面との接触面が実質的に点状になるもの（該接触面が微小面積となるものを含む））であってもよい。

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

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

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

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

また、移動体は、例えば図７に示す如く、各脚リンク１０３_i（ｉ＝１，２，…，Ｎ）の先端部に車輪を備える構造の移動体であってもよい。

また、前記実施形態では、基準歩容の目標全床反力を、目標上体姿勢と実上体姿勢との偏差に応じて決定した姿勢安定化補償力（床反力モーメント）↑Ｍdmdにより補正して修正後目標全床反力を、実全床反力の追従目標としたが、基準歩容の目標全床反力を実床反力の追従目標としてもよい。その場合には、姿勢安定化補償力決定部６２及び目標全床反力修正部６３は不要となる。そして、この場合には、全床反力要求修正量決定部６４において、前記修正後目標全床反力↑ＦＭt_cmd1の代わりに、基準歩容の目標全床反力↑ＦＭt_cmd0を用いて摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdを決定するようにすればよい。

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

Claims

基体と、基体に連結された複数の脚リンクと、各脚リンクの関節を駆動する関節アクチュエータとを備え、前記複数の脚リンクの運動によって床面上を移動する移動体の動作を、該移動体の目標運動と、該目標運動を実現するために該移動体に作用させるべき全床反力の目標値である目標全床反力とに応じて制御する移動体の制御装置において、
前記移動体に実際に作用する全床反力の観測値と、前記目標全床反力との偏差をゼロに近づけるために該移動体に付加的に作用させるべき全床反力の要求修正量である全床反力要求修正量を前記偏差に応じて決定する全床反力要求修正量決定手段と、
前記全床反力要求修正量が前記移動体と床面との全ての接触面を代表する単一の仮想的な接触面としての代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定して、前記決定された全床反力要求修正量と前記代表接触面のあらかじめ定められたばね定数とから、該全床反力要求修正量に対応する該代表接触面の位置及び姿勢の要求変位量を算出する代表接触面位置姿勢変位量算出手段と、
前記代表接触面の位置及び姿勢の時間的変化率と、前記基体の位置及び姿勢と前記移動体の各関節の変位量とを成分として構成される一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である代表接触面ヤコビアン行列Ｊcを、各脚リンクの先端部の位置の時間的変化率又は各脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各脚リンクヤコビアン行列J_iと、前記ばね定数と、前記移動体に実際に作用する全床反力の作用点としての全床反力中心点に対する各脚リンクの先端部の実際の床反力中心点の相対位置と、各脚リンクに実際に作用する床反力の値とから、次式（２７）により算出する代表接触面ヤコビアン行列算出手段と、
前記算出された前記代表接触面の位置及び姿勢の要求変位量に前記算出された代表接触面ヤコビアン行列Ｊ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の次数）に比例する値に設定することを特徴とする移動体の制御装置。