(57)【特許請求の範囲】
【請求項１】
基体と、基体に連結された複数の脚リンクと、各脚リンクの関節を駆動する関節アクチュエータとを備え、前記複数の脚リンクの運動によって床面上を移動する移動体の動作を、該移動体の目標運動と、該目標運動を実現するために該移動体に作用させるべき全床反力の目標値である目標全床反力とに応じて制御する移動体の制御装置において該移動体が移動する実際の床面の位置及び姿勢を推定する床面推定装置であって、
前記移動体に実際に作用する全床反力の観測値と、前記目標全床反力との偏差を積分してなる値を、該偏差をゼロに近づけるために該移動体に付加的に作用させるべき全床反力の修正量である全床反力要求修正量として決定する全床反力要求修正量決定手段と、
前記全床反力要求修正量が前記移動体と床面との全ての接触面を代表する単一の仮想的な接触面としての代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定して、前記決定された全床反力要求修正量と前記代表接触面のあらかじめ定められたばね定数とから、該全床反力要求修正量に対応する該代表接触面の位置及び姿勢の要求変位量を算出する代表接触面位置姿勢変位量算出手段と、
前記代表接触面の位置及び姿勢の時間的変化率と、前記基体の位置及び姿勢と前記移動体の各関節の変位量とを成分として構成される一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である代表接触面ヤコビアン行列Ｊcを、各脚リンクの先端部の位置の時間的変化率又は各脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各脚リンクヤコビアン行列Ｊ_iと、前記ばね定数と、前記移動体に実際に作用する全床反力の作用点としての全床反力中心点に対する各脚リンクの先端部の実際の床反力中心点の相対位置と、各脚リンクに実際に作用する床反力の値とから、次式（２７）により算出する代表接触面ヤコビアン行列算出手段と、
前記算出された前記代表接触面の位置及び姿勢の要求変位量に前記算出された代表接触面ヤコビアン行列Ｊｃの擬似逆行列Ｊｃ-1を乗じることにより前記代表接触面の位置及び姿勢の要求変位量を実現するための前記移動体の各関節の変位量の修正量である関節変位修正量を決定する関節変位修正量決定手段と、
前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、前記決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する関節変位制御手段とを備え、
前記目標運動で想定されている床面である想定床面の位置及び姿勢を、前記代表接触面位置姿勢変位量算出手段により算出された前記要求変位量に応じて補正することにより実際の床面の位置及び姿勢を推定することを特徴とする床面推定装置。
【数１】
0005616289.tif 000013
【請求項２】
基体と、基体に連結された複数の脚リンクと、各脚リンクの関節を駆動する関節アクチュエータとを備え、前記複数の脚リンクの運動によって床面上を移動する移動体の動作を、該移動体の目標運動と、該目標運動を実現するために該移動体に作用させるべき全床反力の目標値である目標全床反力とに応じて制御する移動体の制御装置において該移動体が移動する実際の床面の位置及び姿勢を推定する床面推定装置であって、
前記移動体に実際に作用する全床反力の観測値と、前記目標全床反力との偏差をゼロに近づけるために該移動体に付加的に作用させるべき全床反力の修正量である全床反力要求修正量を、少なくとも該偏差に比例する比例項と、該偏差を積分してなる積分項とを合成することにより決定する全床反力要求修正量決定手段と、
前記全床反力要求修正量が前記移動体と床面との全ての接触面を代表する単一の仮想的な接触面としての代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定して、前記決定された全床反力要求修正量と前記代表接触面のあらかじめ定められたばね定数とから、該全床反力要求修正量に対応する該代表接触面の位置及び姿勢の要求変位量を算出する代表接触面位置姿勢変位量算出手段と、
前記代表接触面の位置及び姿勢の時間的変化率と、前記基体の位置及び姿勢と前記移動体の各関節の変位量とを成分として構成される一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である代表接触面ヤコビアン行列Ｊcを、各脚リンクの先端部の位置の時間的変化率又は各脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各脚リンクヤコビアン行列Ｊ_iと、前記ばね定数と、前記移動体に実際に作用する全床反力の作用点としての全床反力中心点に対する各脚リンクの先端部の実際の床反力中心点の相対位置と、各脚リンクに実際に作用する床反力の値とから、次式（２７）により算出する代表接触面ヤコビアン行列算出手段と、
前記算出された前記代表接触面の位置及び姿勢の要求変位量に前記算出された代表接触面ヤコビアン行列Ｊｃの擬似逆行列Ｊｃ-1を乗じることにより前記代表接触面の位置及び姿勢の要求変位量を実現するための前記移動体の各関節の変位量の修正量である関節変位修正量を決定する関節変位修正量決定手段と、
前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、前記決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する関節変位制御手段と、
前記全床反力要求修正量のうちの前記積分項と前記代表接触面のばね定数とから、該積分項に対応する該代表接触面の位置及び姿勢の変位量である代表接触面定常変位量を算出する代表接触面定常変位量算出手段とを備え、
前記目標運動で想定されている床面である想定床面の位置及び姿勢を、前記代表接触面定常変位量算出手段により算出された前記代表接触面定常変位量に応じて補正することにより実際の床面の位置及び姿勢を推定することを特徴とする床面推定装置。
【数２】
0005616289.tif 000014
【請求項３】
請求項１又は２記載の床面推定装置において、
前記算出された代表接触面ヤコビアン行列Ｊcの擬似逆行列Ｊc-1は、あらかじめ設定された重み行列Ｗと前記算出された代表接触面ヤコビアンＪcとから次式（３０）により算出される行列であると共に、該式（３０）におけるｋの値を、次式（３１）により表される行列式ＤＥＴが所定の正の閾値以上の値になるように決定する擬似逆行列演算用パラメータ決定手段を備えており、
Ｊc-1＝Ｗ-1・ＪcT・(Ｊc・Ｗ-1・ＪcT＋ｋ・Ｉ)-1    ……（３０）
ＤＥＴ＝det(Ｊc・Ｗ-1・ＪcT＋ｋ・Ｉ)    ……（３１）
ただし、Ｗ：あらかじめ定められた重み行列（対角行列）
前記擬似逆行列演算用パラメータ決定手段は、ｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（３０）により擬似逆行列を算出するために用いるｋの値として決定する手段であると共に、前記判断結果が否定的である場合のｋの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ｊc・Ｗ-1・ＪcTの次数）に比例する値に設定することを特徴とする床面推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、脚式移動ロボット等の移動体の運動を行なう床面の位置及び姿勢を推定する装置に関する。
【背景技術】
【０００２】
基体に連結された複数の脚リンクの運動（各脚リンクの着床及び離床を繰り返す運動）によって床面上を移動する脚式移動ロボット等の移動体の動作制御では、基本的には、移動体の目標運動に実際の運動を追従させるように各脚リンクの関節が駆動される。
【０００３】
上記目標運動は、一般に、実際の床面を表すものとして設定された想定床面上で動力学的な必要要件（ＺＭＰが支持多角形内に存在する等の要件）を満足するように生成されるものの、目標運動を生成するために使用した想定床面の実際の床面に対する形状誤差等に起因して、移動体の実際の運動が目標運動からずれる場合がある。そして、このような場合には、移動体の姿勢が崩れやすい。
【０００４】
これを防止するための技術として、従来、例えば特許文献１にて本願出願人が提案したコンプライアンス制御の技術が知られている。このコンプライアンス制御の技術では、２足歩行ロボットに実際に作用する床反力を観測しつつ、目標全床反力中心点周りに発生する実際の床反力モーメントを、２足歩行ロボットの上体の姿勢を目標姿勢に復元させるための目標モーメントに追従させるように、各脚リンクの先端部（足平部）の目標位置及び姿勢が修正され、その修正後の目標位置及び姿勢に応じて該脚リンクの関節の変位量が制御される。
【０００５】
また、脚式移動ロボットの移動環境の実際の床面の傾斜角を推定する技術として、例えば特許文献２に見られる技術が本願出願人により提案されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特許第３６２９１３３号
【特許文献２】特許第３０２４０２８号
【発明の概要】
【発明が解決しようとする課題】
【０００７】
ところで、上記の如きコンプライアンス制御を行う移動体の動作時において、該移動体に実際に作用する全床反力と、目標全床反力との定常的な偏差は、移動体の目標運動を作成するときに使用した想定床面の位置や姿勢の実際の床面に対する誤差に起因するものと考えられる。従って、特許文献１に見られる如きコンプライアンス制御を行いつつ、実際の全床反力の観測値と、目標全床反力との偏差を基に、特許文献２に見られる如き手法によって、実際の床面の位置や姿勢を推定することが考えられる。
【０００８】
しかるに、この場合、次のような不都合を生じる恐れがある。
【０００９】
すなわち、特許文献１に見られる如き従来のコンプライアンス制御の技術では、目標全床反力中心点周りに発生する実際の床反力モーメントを、目標モーメントに追従させるための各脚リンクの先端部（足平部）の目標位置及び姿勢の修正は、両脚リンクの先端部の上下方向の位置を互いに逆向きに変化させる修正（以降、本欄では第１の修正という）と、各脚リンクの先端部の姿勢を変化させる修正（以降、本欄では第２の修正という）とを合成したものとされる。
【００１０】
この場合、第１の修正における各脚リンクの先端部の位置の修正量と、第２の修正における各脚リンクの先端部の姿勢の修正量とは各別に決定される。
【００１１】
しかるに、かかる従来のコンプライアン制御の技術では、第１の修正に起因して生じる床反力の変化と、第２の修正に起因する床反力の変化とが相互に干渉しやすい。このため、第１の修正と第２の修正とを合成してなる各脚リンクの先端部の位置及び姿勢の修正によって実際に生じる床反力モーメントの変化が、目標モーメントに対して過不足を生じやすいという不都合がある。
【００１２】
その結果、実際の全床反力の観測値と、目標全床反力との偏差を基に、特許文献２に見られる如き手法によって、実際の床面の位置や姿勢を推定した場合には、その推定精度を十分に高めることが困難となる恐れがある。
【００１３】
また、上記過不足の発生を極力抑制するためには、第１の修正による各脚リンクの先端部の位置の修正量を決定するためのゲイン（第１の修正によって目標全床反力中心点周りに発生させようとするモーメントの目標モーメントに対する度合いを規定するゲイン）と、第２の修正による各脚リンクの先端部の姿勢の修正量を決定するためのゲイン（第２の修正によって目標全床反力中心点周りに発生させようとするモーメントの目標モーメントに対する度合いを規定するゲイン）とを適切に調整しておく必要がある。そして、その調整作業には、一般に多大な工数を必要とするという不都合がある。
【００１４】
また、特許文献１に見られる如き技術に特許文献２に見られる如き技術を適用する場合には、特許文献１に見られるコンプライアンス制御の技術とは別に、床面推定用のコンプライアンスモデルの設定が必要となる。このため、移動体の制御処理が煩雑なものとなるとという不都合もある。
【００１５】
本発明にかかる背景に鑑みてなされたものであり、移動体に実際に作用する全床反力を目標全床反力に追従させるように該移動体の運動を制御することを、各脚リンクの先端部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行うようにしつつ、移動体の動作環境の実際の床面の位置や姿勢を適切に推定することができる床面推定装置を提供することを目的とする。
【課題を解決するための手段】
【００１６】
まず、本発明の基礎となる技術事項を、以下に一般化して説明する。
【００１７】
図１に示す如く、基体１０２にＮ個（Ｎ≧２）の脚リンク１０３_1〜１０３_Nが連結された移動体１０１を想定する。この移動体１０１の脚リンク１０３_1〜１０３_Nのそれぞれは、複数の関節を備えており、これらの関節の変位によって、空間的な運動が可能である。なお、各関節は、回転型又は直動型の関節である。
【００１８】
そして、移動体１０１は、Ｎ個の脚リンク１０３_1〜１０３_Nのうちのいずれか１つ又は複数の脚リンクを支持脚として、その支持脚の先端部を接地（床面に接触）させることで移動体１０１の自重を該支持脚を介して床面に支えることが可能であると共に、脚リンク１０３_1〜１０３_Nの空間的な運動を行なうことによって、移動体１０１が床面上で移動可能である。以降、これらの脚リンク１０３_1〜１０３_Nのそれぞれを総称的に第ｉ脚リンク１０３_i（ｉ＝１，２，…，Ｎ）、又は、脚リンク１０３_iという。
【００１９】
この移動体１０１の任意の運動状態において、床面から移動体１０１に作用するトータルの床反力である全床反力（ベクトル）を↑ＦＭt、この↑ＦＭtのうちの並進力ベクトルである全並進床反力を↑Ｆt、↑ＦＭtのうちのモーメントベクトルである全床反力モーメントを↑Ｍtと表記する。
【００２０】
これらの全並進床反力↑Ｆt及び全床反力モーメント↑Ｍtは、それぞれ慣性座標系（床に対して固定された座標系）で見た３成分の縦ベクトルとして表現されるものとする。そして、全床反力↑ＦＭtは、↑Ｆt及び↑Ｍtの各成分を並べた６成分の縦ベクトル（＝[↑Ｆt，↑Ｍt]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（ｉ＝１，２，…，Ｎ）との間の関係は一般的に、次式（１）により表される。
【００２６】
【数１】
0005616289.tif 000002
なお、式（１）のただし書きにおいて、行列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と、第ｉ脚摂動床反力↑ΔＦＭ_ｉ（ｉ＝１，２，…，Ｎ）との間の関係は、上記式（１）に基づいて、次式（２）により与えられる。
【００３０】
【数２】
0005616289.tif 000003
この式（２）における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を重み係数とする重み付き平均値）となる。
【００７０】
次に、前記式（２）に式（３）〜（６）を適用し、さらに、式（１１），（２０）を適用することによって、次式（２１）が得られる。
【００７１】
【数３】
0005616289.tif 000004
【００７２】
なお、前記式（１１）、（２０）は、全床反力↑ＦＭtの作用点Ｐtが全床反力中心点、第ｉ脚床反力↑ＦＭ_iの作用点Ｐ_iが第ｉ脚接地面の床反力中心点であることを前提としているので、式（２１）におけるVV_i（ｉ＝１，２，…，Ｎ）は、全床反力中心点としての作用点Ｐtに対する第ｉ脚接地面の床反力中心点の位置ベクトルを改めて↑Ｖ_iとしたとき、次のように定義される行列である。
【００７３】
VV_i：VV_i・↑Ｆ_i＝Ｖ_i×↑Ｆ_iとなる行列
ただし、↑Ｖ_i：全床反力中心点に対する第ｉ脚接地面の床反力中心点の位置ベクトル
なお、この場合、上記全床反力中心点は、より詳しくは、摂動全床反力↑ΔＦＭtが付加される前の全床反力↑ＦＭtに対応する全床反力中心点であり、第ｉ脚接地面の床反力中心点は、第ｉ脚摂動床反力↑ΔＦＭ_iが付加される前の第ｉ脚床反力↑ＦＭ_iに対応する床反力中心点である。
【００７４】
さらに、上記式（２１）から、次式（２２）が得られる。
【００７５】
【数４】
0005616289.tif 000005
【００７６】
ここで、移動体１０１の基体１０２の位置（３軸方向の位置）及び姿勢（３軸周りの姿勢角）と移動体１０１の各関節の変位量とを成分として構成される一般化変数ベクトルを↑ｑ、代表接触面の位置のばね性並進変位量↑Ｘ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    ……（２６ｃ）
前記式（２２）の両辺を微分し、前記式（２３）、（２４）を適用することで、次式（２７）が得られる。
【００８２】
【数５】
0005616289.tif 000006
従って、代表接触面の位置及び姿勢の変位に関するヤコビアン行列Ｊc（以降、代表接触面ヤコビアン行列Ｊcという）は、各脚接地面の位置及び姿勢の変位に関するヤコビアン行列Ｊ_i（ｉ＝１，２，…，Ｎ）から、上記の式（２７）に基づいて決定できることとなる。
【００８３】
この場合、第ｉ脚接地面の位置及び姿勢は、第ｉ脚リンク１０３_iの先端部の位置及び姿勢であるから、ヤコビアン行列Ｊ_i（以降、脚リンクヤコビアン行列Ｊ_iという）は、換言すれば、第ｉ脚リンク１０３_iの先端部の位置及び姿勢の単位時間当たりの変化量と、↑Δｑとの間の関係を表現するヤコビアン行列である。そして、このようなヤコビアン行列Ｊ_iは、移動体１０１の各関節の実際の変位量の観測値及びその時間的変化率を基に特定できる。
【００８４】
そして、このように各脚リンクヤコビアン行列Ｊ_i（ｉ＝１，２，…，Ｎ）を特定すれば、それらのＪ_iを用いて、上記式（２７）により、代表接触面ヤコビンア行列Ｊcを決定できることとなる。
【００８５】
一方、移動体１０１の所要の状態を実現するための要求操作量として、ある値の摂動全床反力↑ΔＦＭtが与えられたとき、この↑ΔＦＭtは前記式（５），（６）に基づいて、代表接触面のばね性並進・回転変位量↑Ｘｃ（＝[↑Ｘc_org，↑Ｘc_rot]T）の要求値に変換できることとなる。この変換は、次式（２８）により与えられる。
【００８６】
【数６】
0005616289.tif 000007
そして、代表接触面ヤコビアン行列Ｊcの擬似逆行列をＪc-1とおくと、代表接触面のばね性並進・回転変位量変位量↑Ｘcの要求値から、次式（２９）により、摂動全床反力↑ΔＦＭtの要求値を実現するための、移動体１０１の各関節の変位量の修正量を決定できることとなる。
【００８７】
↑Δｑ＝Ｊｃ-1・↑Ｘｃ    ……式（２９）
そこで、移動体１０１の任意の運動状態において、移動体１０１に作用する全床反力を制御するための要求操作量として、摂動全床反力↑ΔＦＭtの要求値が与えられたとき、前記式（２７）に基づいて代表接触面ヤコビアン行列Ｊcを求めると共に、このＪcの擬似逆行列Ｊc-1を決定する。さらに、前記式（２８）に基づいて、↑ΔＦＭtの要求値に対応する代表接触面の変位量ベクトル↑Ｘcを求める。そして、この↑Ｘcと、Ｊｃ-1とから式（２９）によって、摂動全床反力↑ΔＦＭtの要求値を実現するための各関節の変位量の修正量を決定できることとなる。
【００８８】
ひいては、各脚リンク１０３_iの先端部の位置及び姿勢の修正量自体をそれぞれ決定することなく、一括的に、各関節の変位量を決定できることとなる。
【００８９】
次に、移動体１０１に実際に作用する全床反力を、該移動体１０１の目標運動を実現するための目標全床反力に追従させるようにように、上記摂動全床反力↑ΔＦＭtの要求値を決定することを想定する。そして、この場合、↑ΔＦＭtの要求値を、実際の全床反力の観測値と、目標全床反力との偏差を積分したものとして決定し、あるいは、少なくとも該偏差を積分してなる積分項と、該偏差に比例する比例項とを合成することによって決定するものとする。
【００９０】
この場合、実際の全床反力の観測値と、目標全床反力との偏差を積分したもの（以降、これを↑ΔＦＭt_intと表記する）は、移動体１０１の目標運動で想定されている想定床面の位置及び姿勢と、実際の床面の位置及び姿勢との定常的な差分に応じたものとなる。このため、当該偏差の積分値↑ΔＦＭt_intを、前記式（２８）により変換することで算出される↑Ｘc、すなわち、↑ΔＦＭt_intから、次式（２８−１）により算出される↑Ｘc_intは、想定床面の位置及び姿勢と、実際の床面の位置及び姿勢との定常的な誤差分に相当するものとなると考えられる。
【００９１】
【数７】
0005616289.tif 000008
なお、式（２８−１）においては、↑Ｘc_intのうちの並進変位量の成分、回転変位量の成分をそれぞれ↑Ｘc_org_int、↑Ｘc_rot_intと表記し、↑ΔＦＭt_intのうちの並進力成分、モーメント成分をそれぞれ↑ΔＦt_int、↑ΔＭt_intと表記している。
【００９２】
従って、想定床面の位置及び姿勢を上記↑Ｘc_intにより補正することで、実際の床面の位置及び姿勢を推定できることとなる。
【００９３】
以上が、本発明の基礎とする技術事項である。
【００９４】
以上説明したことを基礎として、以下に本発明を説明する。
【００９５】
本発明の第１の態様は、基体と、基体に連結された複数の脚リンクと、各脚リンクの関節を駆動する関節アクチュエータとを備え、前記複数の脚リンクの運動によって床面上を移動する移動体の動作を、該移動体の目標運動と、該目標運動を実現するために該移動体に作用させるべき全床反力の目標値である目標全床反力とに応じて制御する移動体の制御装置において該移動体が移動する実際の床面の位置及び姿勢を推定する床面推定装置であって、
前記移動体に実際に作用する全床反力の観測値と、前記目標全床反力との偏差を積分してなる値を、該偏差をゼロに近づけるために該移動体に付加的に作用させるべき全床反力の修正量である全床反力要求修正量として決定する全床反力要求修正量決定手段と、
前記全床反力要求修正量が前記移動体と床面との全ての接触面を代表する単一の仮想的な接触面としての代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定して、前記決定された全床反力要求修正量と前記代表接触面のあらかじめ定められたばね定数とから、該全床反力要求修正量に対応する該代表接触面の位置及び姿勢の要求変位量を算出する代表接触面位置姿勢変位量算出手段と、
前記代表接触面の位置及び姿勢の時間的変化率と、前記基体の位置及び姿勢と前記移動体の各関節の変位量とを成分として構成される一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である代表接触面ヤコビアン行列Ｊcを、各脚リンクの先端部の位置の時間的変化率又は各脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各脚リンクヤコビアン行列Ｊ_iと、前記ばね定数と、前記移動体に実際に作用する全床反力の作用点としての全床反力中心点に対する各脚リンクの先端部の実際の床反力中心点の相対位置と、各脚リンクに実際に作用する床反力の値とから、前記式（２７）により算出する代表接触面ヤコビアン行列算出手段と、
前記算出された前記代表接触面の位置及び姿勢の要求変位量に前記算出された代表接触面ヤコビアン行列Ｊｃの擬似逆行列Ｊｃ-1を乗じることにより前記代表接触面の位置及び姿勢の要求変位量を実現するための前記移動体の各関節の変位量の修正量である関節変位修正量を決定する関節変位修正量決定手段と、
前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、前記決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する関節変位制御手段とを備え、
前記目標運動で想定されている床面である想定床面の位置及び姿勢を、前記代表接触面位置姿勢変位量算出手段により算出された前記要求変位量に応じて補正することにより実際の床面の位置及び姿勢を推定することを特徴とする（第１発明）。
【００９６】
あるいは、本発明の第２の態様は、基体と、基体に連結された複数の脚リンクと、各脚リンクの関節を駆動する関節アクチュエータとを備え、前記複数の脚リンクの運動によって床面上を移動する移動体の動作を、該移動体の目標運動と、該目標運動を実現するために該移動体に作用させるべき全床反力の目標値である目標全床反力とに応じて制御する移動体の制御装置において該移動体が移動する実際の床面の位置及び姿勢を推定する床面推定装置であって、
前記移動体に実際に作用する全床反力の観測値と、前記目標全床反力との偏差をゼロに近づけるために該移動体に付加的に作用させるべき全床反力の修正量である全床反力要求修正量を、少なくとも該偏差に比例する比例項と、該偏差を積分してなる積分項とを合成することにより決定する全床反力要求修正量決定手段と、
前記全床反力要求修正量が前記移動体と床面との全ての接触面を代表する単一の仮想的な接触面としての代表接触面の位置及び姿勢のばね性変位によって発生するものと仮定して、前記決定された全床反力要求修正量と前記代表接触面のあらかじめ定められたばね定数とから、該全床反力要求修正量に対応する該代表接触面の位置及び姿勢の要求変位量を算出する代表接触面位置姿勢変位量算出手段と、
前記代表接触面の位置及び姿勢の時間的変化率と、前記基体の位置及び姿勢と前記移動体の各関節の変位量とを成分として構成される一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である代表接触面ヤコビアン行列Ｊcを、各脚リンクの先端部の位置の時間的変化率又は各脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各脚リンクヤコビアン行列Ｊ_iと、前記ばね定数と、前記移動体に実際に作用する全床反力の作用点としての全床反力中心点に対する各脚リンクの先端部の実際の床反力中心点の相対位置と、各脚リンクに実際に作用する床反力の値とから、前記式（２７）により算出する代表接触面ヤコビアン行列算出手段と、
前記算出された前記代表接触面の位置及び姿勢の要求変位量に前記算出された代表接触面ヤコビアン行列Ｊｃの擬似逆行列Ｊｃ-1を乗じることにより前記代表接触面の位置及び姿勢の要求変位量を実現するための前記移動体の各関節の変位量の修正量である関節変位修正量を決定する関節変位修正量決定手段と、
前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、前記決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する関節変位制御手段と、
前記全床反力要求修正量のうちの前記積分項と前記代表接触面のばね定数とから、該積分項に対応する該代表接触面の位置及び姿勢の変位量である代表接触面定常変位量を算出する代表接触面定常変位量算出手段とを備え、
前記目標運動で想定されている床面である想定床面の位置及び姿勢を、前記代表接触面定常変位量算出手段により算出された前記代表接触面定常変位量に応じて補正することにより実際の床面の位置及び姿勢を推定することを特徴とする（第２発明）。
【００９７】
ここで、前記式（２７）と、この式（２７）の変数の意味とを改めて記載すると、次の通りである。
【００９８】
【数８】
0005616289.tif 000009
なお、本発明において、「床面」は、屋内の通常の床面に限らず、屋外の地面もしくは路面であってもよい。また、想定床面、あるいは、実際の床面の位置及び姿勢は、移動体の各脚リンクの先端部の接地面もしくはその近辺の床面の位置及び姿勢を意味する。
【００９９】
上記第１発明及び第２発明によれば、前記移動体に実際に作用する全床反力（以降、実全床反力ということがある）の観測値と、前記目標全床反力との偏差をゼロに近づけるためのフィードバック操作量（制御入力）としての前記全床反力要求修正量が、該偏差に応じて決定される。
【０１００】
この場合、第１発明では、該偏差を積分してなる値（積分項）が、そのまま全床反力要求修正量として決定される。また、第２発明では、少なくとも該偏差に比例する比例項（該偏差に所定値のゲインを乗じてなる値）と、該偏差を積分してなる積分とを合成することにより前記全床反力要求修正量が決定される。
【０１０１】
このように決定される全床反力要求修正量は、第１発明及び第２発明のいずれにおいても、前記摂動全床波力↑ΔＦＭtの要求値に相当するものである。
【０１０２】
なお、第２発明では、比例項及び積分項だけを合成したものを前記全床反力要求修正量としてもよいが、比例項及び積分項以外の項、例えば微分項をさらに合成することで、前記全床反力要求修正量を決定してもよい。
【０１０３】
また、第１発明及び第２発明における前記目標全床反力としては、例えば、実際の床面のモデルとしての想定床面上で、前記移動体の目標運動に対して適宜の動力学モデルの動力学的関係が成立するように作成されたものを採用できる。あるいは、移動体の目標運動に対して上記動力学的関係が成立するように作成した基準の全床反力を、移動体の運動に関する所定の状態量（移動体の特定の部位（基体等）もしくは全体重心点の位置、姿勢、又はそれらの変化速度等の状態量）の目標値と実際の値との偏差に応じて、該偏差をゼロに近づけるように補正したものであってもよい。
【０１０４】
第１発明及び第２発明では、上記の如く決定された全床反力要求修正量に対応する代表接触面の位置及び姿勢の要求変位量が代表接触面位置姿勢変位量算出手段により算出される。
【０１０５】
これにより、実全床反力を目標全床反力に追従させるためのフィードバック操作量（制御入力）としての全床反力要求修正量が、単一の仮想面たる代表接触面の位置及び姿勢の要求変位量に変換される。
【０１０６】
ここで、代表接触面の位置及び姿勢の変位量は、前記ばね性並進・回転変位量↑Ｘcに相当するものであり、前記一般化変数ベクトル↑ｑの単位時間当たりの変化量↑Δｑに対して、代表接触面ヤコビアン行列Ｊcによって表現される前記式（２３）の関係を有する。
【０１０７】
従って、全床反力要求修正量に対応する代表接触面の位置及び姿勢の要求変位量に、代表接触面ヤコビアン行列Ｊcの擬似逆行列Ｊc-1を乗じることによって（すなわち、前記式（２９）によって）、当該要求変位量を実現するための（ひいては、全床反力要求修正量を実現するための）移動体の関節変位修正量を決定できることとなる。
【０１０８】
そこで、本発明では、前記代表接触面ヤコビアン行列算出手段によって、代表接触面ヤコビアン行列Ｊcを算出する。この場合、代表接触面ヤコビアン行列Ｊcは、各脚リンクの先端部の位置の時間的変化率又は各脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列である各脚リンクヤコビアン行列J_i（ｉ＝１，２，…，Ｎ）と、前記ばね定数と、前記移動体に実際に作用する全床反力の作用点としての全床反力中心点に対する各脚リンクの先端部の実際の床反力中心点の相対位置と、各脚リンクに実際に作用する床反力の値とから、前記式（２７）によって算出される。
【０１０９】
なお、式（２７）の演算を行なうために用いる各脚リンクに実際に作用する床反力の値や、その床反力作用点の位置、全床反力作用点の位置は、移動体に搭載した力センサ等により計測した観測値でよいことはもちろんであるが、実際の値を精度よく近似し得るものであれば、前記目標床反力や適当なモデル等を基に、近似的に推定もしくは予測した値であってもよい。
【０１１０】
また、上記脚リンクヤコビアン行列に関して補足すると、本発明では、各脚リンクは、その先端部を接地させた状態で、該先端部に作用する床反力モーメントを変化させることができる構造の脚リンク（例えば、先端部がアクチュエータにより姿勢を変化させることができる足平部により構成され、該先端部と床面との接触が面接触状態となる脚リンク）と、該先端部に作用する床反力モーメントを変化させることができない構造の脚リンク（例えば該先端部と床面との接触状態が点状となる脚リンク）とのいずれの構造の脚リンクであってもよい。
【０１１１】
この場合、各脚リンクが、その先端部での床反力モーメントを変化させることができる構造のものである場合には、脚リンクヤコビアン行列は、該脚リンクの先端部の位置及び姿勢の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列となる。また、各脚リンクが、その先端部での床反力モーメントを変化させることができない構造のものである場合には、脚リンクヤコビアン行列は、該脚リンクの先端部の位置の時間的変化率と前記一般化変数ベクトルの時間的変化率との間の関係を表すヤコビアン行列となる。
【０１１２】
これらの脚リンクヤコビアン行列は、移動体の各関節の実際の変位量の観測値を基に、公知の手法によって求めることができる。
【０１１３】
そして、本発明においては、前記関節変位修正量決定手段が、上記の如く算出された代表接触面ヤコビアン行列Ｊcの擬似逆行列Ｊc-1を、代表接触面の位置及び姿勢の要求変位量に乗じることによって、前記関節変位修正量が決定される。これにより、結果的に、前記全床反力要求修正量を実現するための関節変位修正量が決定されることとなる。
【０１１４】
さらに、前記関節変位制御手段が、前記移動体の目標運動により規定される該移動体の各関節の変位量である目標関節変位量を、上記の如く決定された関節変位修正量により修正してなる修正後目標関節変位量に応じて前記関節アクチュエータを制御する。
【０１１５】
これにより、実全床反力を目標全床反力に追従させるように、移動体の各関節の変位量の制御が行われ、所謂、コンプライアンス制御が実現される。
【０１１６】
かかる第１発明及び第２発明によれば、全床反力要求修正量を代表接触面の位置及び姿勢の要求変位量に変換し、この要求変位量に応じて移動体の各関節の関節変位修正量を求めることにより、個々の脚リンクの先端部の位置や姿勢の修正と実全床反力の変化との関係やそれらの相互の関係を考慮して各脚リンクの先端部の位置や姿勢の修正量を決定するような処理を実行することなく、実全床反力を目標全床反力に追従させるための移動体の各関節の変位量の修正量（関節変位修正量）を一括的に決定できる。そのため、関節変位修正量を決定する処理を効率よく短時間で行なうことができる。
【０１１７】
この場合、前記式（２７）における各脚リンクの重み係数ｒ_iは、床反力の垂直抗力成分が相対的に大きい脚リンクほど、より大きい値（“１”に近い値）に設定されることとなるため、全床反力要求修正量に対する各脚リンクの負担分は、結果的に、床反力の垂直抗力成分が相対的に大きい脚リンクほど、大きくなる。従って、床反力の垂直抗力成分が相対的に小さい脚リンクの先端部の位置や姿勢を不必要に修正したりすることなく、全床反力要求修正量を確実に実現し得るような関節変位修正量を決定できる。
【０１１８】
また、各脚リンクの重み係数ｒ_iは連続的に変化するので、代表接触面ヤコビアン行列Ｊｃが、不連続的な変化を生じることがない。このため、移動体の各関節の変位量を滑らかに連続的に変化させることができる。ひいては、移動体の運動を滑らかに行なうことができる。
【０１１９】
よって、第１発明及び第２発明によれば、移動体に実際に作用する全床反力を目標全床反力に追従させるように該移動体の運動を制御することを、各脚リンクの先端部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行うことができる。
【０１２０】
一方、第１発明において、上記の如く移動体のコンプライアンス制御が行われている状態において、前記全床反力要求修正量決定手段により決定される全床反力要求修正量は、実全床反力の観測値と、目標全床反力との偏差を積分してなる値（積分項）であるから、該全床反力要求修正量と前記ばね定数とから、前記代表接触面位置姿勢変位量算出手段により算出される代表接触面の位置及び姿勢の要求変位量は、前記目標運動で想定されている床面である想定床面の位置及び姿勢と、実際の床面の位置及び姿勢と間の定常的な差分に相当するものとなる。すなわち、該要求変位量は、前記式（２８−１）における↑Ｘc_intに相当するものである。
【０１２１】
また、第２発明においては、上記の如く移動体のコンプライアンス制御が行われている状態において、前記全床反力要求修正量決定手段により決定される全床反力要求修正量のうちの積分項と前記ばね定数とから、前記代表接触面位置姿勢変位量算出手段と同様の演算によって算出される代表接触面の位置及び姿勢の変位量（全床反力要求修正量の代わりに前記積分項を用いて算出される代表接触面の位置及び姿勢の変位量）は、前記想定床面の位置及び姿勢と、実際の床面の位置及び姿勢と間の定常的な差分に相当するもの、すなわち、前記式（２８−１）における↑Ｘc_intに相当するものとなる。
【０１２２】
そこで、第１発明においては、前記想定床面の位置及び姿勢から、前記代表接触面位置姿勢変位量算出手段により算出された前記要求変位量だけ修正してなる位置及び姿勢を実際の床面の位置及び姿勢として推定する。
【０１２３】
また、第２発明においては、前記代表接触面定常変位量算出手段によって、前記全床反力要求修正量のうちの前記積分項と前記代表接触面のばね定数とから、該積分項に対応する該代表接触面の位置及び姿勢の変位量である代表接触面定常変位量を算出する。そして、前記想定床面の位置及び姿勢から、前記代表接触面定常変位量算出手段により算出された前記代表接触面定常変位量だけ修正してなる位置及び姿勢を実際の床面の位置及び姿勢として推定する。
【０１２４】
これにより、第１発明及び第２発明によれば、実際の床面の位置及び姿勢を推定することができる。この場合、前記のコンプライアンス制御によって、前記全床反力要求修正量を確実に実現し得るような関節変位修正量を決定して、前記移動体の動作を制御できることから、第１発明における全床反力要求修正量、又は第２発明における代表接触面定常変位量は、想定床面の位置及び姿勢と、実際の床面の位置及び姿勢と間の定常的な差分に相当するものとしての信頼性が高いものとなる。従って、実際の床面の位置及び姿勢を精度よく推定することができることとなる。
【０１２５】
よって第１発明及び第２発明によれば、移動体に実際に作用する全床反力を目標全床反力に追従させるように該移動体の運動を制御することを、各脚リンクの先端部の位置や姿勢の目標運動からの修正量をそれぞれ決定する処理を必要とせずに適切に行うようにしつつ、移動体の動作環境の実際の床面の位置や姿勢を適切に推定することができる。
【０１２６】
なお、第２発明においては、前記全床反力要求修正量は、少なくとも前記比例項と積分項とを合成したものであるので、想定床面の位置及び姿勢の実際の床面に対する定常的な誤差の影響だけでなく、想定床面で想定されていない実際の床面の局所的な凹凸等に起因して生じる、実全床反力の観測値と目標全床反力との一時的な偏差の影響を補償するよにして、実全床反力を目標全床反力に追従させるための関節変位修正量を決定できる。
【０１２７】
上記第１発明又は第２発明において、前記代表接触面ヤコビアン行列Ｊcの擬似逆行列Ｊc-1は、公知の適宜の手法により算出することができる。
【０１２８】
ただし、前記算出された代表接触面ヤコビアン行列の擬似逆行列Ｊc-1は、あらかじめ設定された重み行列Ｗと前記算出された代表接触面ヤコビアン行列Ｊcとから次式（３０）により算出される行列であると共に、該式（２８）におけるｋの値（実数値）を、次式（３１）により表される行列式ＤＥＴが所定の正の閾値以上の値になるように決定する擬似逆行列演算用パラメータ決定手段を備えており、
Ｊc-1＝Ｗ-1・ＪcT・(Ｊc・Ｗ-1・ＪcT＋ｋ・Ｉ)-1    ……（３０）
ＤＥＴ＝det(Ｊc・Ｗ-1・ＪcT＋ｋ・Ｉ)    ……（３１）
ただし、Ｗ：あらかじめ定められた重み行列（対角行列）
前記擬似逆行列演算用パラメータ決定手段は、ｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（３０）により擬似逆行列を算出するために用いるｋの値として決定する手段であると共に、前記判断結果が否定的である場合のｋの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ｊc・Ｗ-1・ＪcTの次数）に比例する値に設定することが好ましい（第３発明）。
【０１２９】
なお、式（３０）において、Ｉは単位行列である。また、重み係数行列Ｗは、各関節の変位量の変化に対する代表接触面の位置及び姿勢の変化の応答性等を考慮し、代表接触面の位置及び姿勢の要求変位量を実現するための各関節の変位量の修正度合いを各関節毎に調整するものである。なお、重み係数行列Ｗは単位行列であってもよい。
【０１３０】
また、ｋは、式（３０）の右辺の括弧内の行列の行列式、すなわち、式（３１）に示される行列式ＤＥＴの大きさが小さくなり過ぎるのを防止するための調整パラメータであり、ゼロ以上の実数値である。この調整パラメータｋについて補足すると、擬似逆行列Ｊｃ-1は、基本的には、ｋ＝０とした場合の上記の式（３０）によって算出することができる。
【０１３１】
ただし、この場合、上記行列式ＤＥＴの大きさが小さくなり過ぎる（ゼロに近い値となる）場合がある。そして、このような場合には、式（３０）の右辺の括弧内の行列の逆行列が発散し、適切な擬似逆行列Ｊc-1を決定することができなくなる。これを防止するために、式（３０）では、右辺の括弧内の第１項に、単位行列Ｉのｋ倍の行列を付加するようにしている。
【０１３２】
一方、前記行列式ＤＥＴの大きさが小さくなり過ぎないようにするための適切な調整パラメータｋの値はＪcに応じて変化する。また、行列式ＤＥＴの大きさは、ｋの値の変化に対して非線形な変化を呈する。
【０１３３】
このため、第３発明では、上記行列式ＤＥＴの絶対値が前記所定の閾値以上となる（過小にならない）ｋの値を、前記擬似逆行列演算パラメータ決定手段により探索的に決定する。
【０１３４】
具体的には、ｋの暫定値を所定の初期値から段階的に増加させていくように設定することと、設定した各暫定値を用いて前記行列式ＤＥＴの値を算出することと、算出した行列式ＤＥＴの絶対値が前記所定の閾値以上の値であるか否かを判断することとを繰り返し、該判断結果が肯定的となったときのｋの暫定値を、前記式（３０）により擬似逆行列を算出するために用いるｋの値として決定する。
【０１３５】
この場合、この処理において、ｋの暫定値の増加量が一定であると、ｋの値が最終的に決定されるまでに時間がかかりすぎたり、あるいは、移動体の制御装置の制御処理周期毎に決定されるｋの値が頻繁に変動し、ひいては、算出される擬似逆行列Ｊc-1の不連続的な変動が生じやすい。
【０１３６】
一方、本願発明者の知見によれば、Ｊc・Ｗ-1・ＪcTの次数をｎとしたとき、行列式ＤＥＴの値は、ｋの値のｎ乗に比例して変化する。
【０１３７】
そこで、第３発明では、前記擬似逆行列演算パラメータ決定手段の処理において、前記判断結果が否定的である場合のｋの暫定値の増加量を、その増加前の暫定値を用いて算出した行列式ＤＥＴの絶対値と前記所定の閾値との偏差の絶対値のｎ乗根（ｎ：Ｊc・Ｗ-1・ＪcTの次数）に比例する値に設定することとした。
【０１３８】
これにより、第３発明によれば、移動体の制御装置の制御処理周期毎に、擬似逆行列Ｊ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であるとする。
【０１８８】
補足すると、前記基準歩容は、移動体１の移動動作中に生成する必要はなく、該移動体１の移動開始前に事前に作成しておき、それを制御装置５０の記憶装置に事前に記憶保持したり、外部から制御装置５０に適宜、無線通信によって入力するようにしてもよい。このようにした場合、制御装置５０に基準歩容生成部６１を備える必要はない。
【０１８９】
［目標関節変位量決定部６７の処理］
制御装置５０は、前記の如く生成された基準歩容のうちの目標運動を、目標関節変位量決定部６７に入力し、該目標関節変位量決定部６７の処理を実行する。この目標関節変位量決定部６７は、基準歩容の目標運動によって規定される移動体１の各関節の変位量である基準の目標変位量を算出する機能部である。この場合、目標関節変位量決定部６７は、入力された目標運動から、逆運動学の演算処理によって移動体１の各関節の目標変位量を算出する。
【０１９０】
なお、基準歩容を事前に作成しておく場合には、この基準歩容の目標運動に対応する各関節の目標変位量も事前に作成しておいてもよい。その場合には、制御装置５０に目標関節変位量決定部６７を備える必要はない。
【０１９１】
［姿勢安定化補償力決定部６２の処理］
制御装置５０は、目標関節変位量決定部６７の処理と並行して（又は該処理の前もしくは後に）、姿勢安定化補償力決定部６２、目標全床反力修正部６３、全床反力要求修正量決定部６４、代表接触面位置・姿勢変位量算出部６５、代表接触面ヤコビアン行列算出部６６及び関節変位修正量決定部６８の処理を実行する。これらの処理は、コンプライアンス制御のための操作量（制御入力）を決定するための処理であり、以降、コンプライアンス操作量決定処理という。そして、制御装置５０は、このコンプライアンス操作量決定処理によって、前記基準歩容に対応する各関節の基準の目標変位量を修正するための関節変位修正量を決定する。
【０１９２】
このコンプライアンス操作量決定処理では、制御装置５０は、まず、姿勢安定化補償力決定部６２の処理を実行する。この姿勢安定化補償力決定部６２は、基準歩容の目標上体姿勢と、移動体１の上体２の実際の姿勢（実上体姿勢）の観測値との偏差をゼロに近づけるために移動体１に付加的に作用させるべき摂動床反力である姿勢安定化補償力↑Ｍdmdを、基準歩容の目標全床反力を修正するための修正量として決定する機能部である。
【０１９３】
上記姿勢安定化補償力↑Ｍ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を算出する。
【０２１４】
【数９】
0005616289.tif 000010
この式（４２）におけるＫ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を算出する。なお、本実施形態では、両脚リンク３Ｒ，３Ｌのうちのいずれか一方と他方とが、それぞれ式（２７）における第１脚リンク、第２脚リンクに相当する。以降の説明では、便宜上、例えば脚リンク３Ｒを第１脚リンク、脚リンク３Ｌを第２脚リンクとする。
【０２１８】
【数１０】
0005616289.tif 000011
この場合、式（４３）の右辺の演算に必要な変数値は、次のように決定される。
【０２１９】
重み係数ｒ_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の転置行列ＪcTと、重み係数行列Ｗの逆行列Ｗ-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＋ａ・ｋn-1＋……という形の関数）となるので、ｋの値の変化に対する行列式ＤＥＴの値の変化は、ほぼｋn（ｋのｎ乗）に比例すると考えられる。そこで、本実施形態では、上記（３２）により算出される値を、増加量Δｋの暫定値Δｋpとして設定する。
【０２５６】
そして、ＳＴＥＰ６〜８において、増加量Δｋが過剰に小さい値になるのを防止するためのリミット処理が暫定値Δｋpに対して施され、これにより増加量Δｋが確定される。具体的には、ＳＴＥＰ６において、増加量Δｋの暫定値Δｋpと所定の下限値Δｋmin（＞０）とが比較され、Δｋp≧Δｋminである場合には、ＳＴＥＰ７において、暫定値Δｋpがそのまま増加量Δｋとして決定される。また、Δｋp＜Δｋminである場合には、ＳＴＥＰ８において、Δｋminが増加量Δｋとして決定される。
【０２５７】
これにより、増加量Δｋは、Δｋminを下限値として、行列式ＤＥＴの大きさと、前記下限閾値DET_Lとの偏差の大きさに応じて前記式（３２）により算出される値に決定されることとなる。
【０２５８】
関節変位修正量決定部６８は、以上説明した如く算出した擬似逆行列Ｊｃ-1を用いて、前記式（２９）の右辺の演算を行なうことで、一般化変数要求修正量ベクトル↑Δｑ_dmdを算出する。そして、関節変位修正量決定部６８は、この一般化変数要求修正量ベクトル↑Δｑ_dmdの成分のうち、各関節の変位量の要求修正量を示す成分を、関節変位修正量として決定する。このようにして決定される関節変位修正量が、実全床反力を修正後目標全床反力↑ＦＭt_cmd1に近づけるための各関節の変位量の要求修正量としてのコンプライアンス操作量である。
【０２５９】
［関節変位制御部６９の処理］
以上説明したコンプラインス操作量決定処理（姿勢安定化補償力決定部６２、目標全床反力修正部６３、全床反力要求修正量決定部６４、代表接触面並進・回転変位量算出部６５、代表接触面ヤコビアン行列算出部６６及び関節変位修正量決定部６８の処理）とを実行した後、次に、制御装置５０は、関節変位制御部６９の処理を実行する。
【０２６０】
この関節変位制御部６９は、目標関節変位量決定部６７で決定した各関節の目標関節変位量（基準歩容の目標運動に対応する目標関節変位量）に、目標関節変位量修正部６８で決定した関節変位修正量を加え合わせることにより、各関節の最終的な関節変位指令としての修正後目標関節変位量を決定する。
【０２６１】
そして、関節変位制御部６９は、上記の如く決定した修正後目標関節変位量に各関節の実変位量を一致させるように、電動モータ４１（関節アクチュエータ）を図示しないサーボアンプ等のモータ駆動回路を介して制御する。
【０２６２】
［床面推定部７０の処理］
制御装置５０は、移動体１の各関節の駆動制御と並行して、床面推定部７０の処理を実行する。この床面推定部７０は、移動体１の各脚リンク３が接地する実際の床面（実床面）の位置及び姿勢の推定値である床面推定値を算出する機能部である。
【０２６３】
この処理のために床面推定部７０には、前記全床反力要求修正量決定部６４から、摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdの組成成分である前記式（４１）の右辺の積分項（第２項）の値、すなわち、修正後目標床反力↑ＦＭt_cmd1と、実全床反力の観測値↑ＦＭt_actとの偏差↑Ｄfmt（＝↑ＦＭt_cmd1−↑ＦＭt_act）にローパス特性のフィルタリング処理を施してなる値↑Ｄfmt_filtに、所定値のゲインＫestm（スカラー又は対角行列）を乗じてなる値を積分したもの（＝∫（Ｋestm・↑Ｄfmt_filt））が入力される。以降、この積分項の値を次式（３３）で示す如く、ΔＦＭt_intと表記する。
【０２６４】
↑ΔＦＭt_int≡∫（Ｋestm・↑Ｄfmt_filt）    ……（３３）
ここで、前記した移動体１の各関節の駆動制御によって、各関節の変位量は、実全床反力の観測値↑ＦＭt_actを修正後目標床反力↑ＦＭt_cmd1に追従させるように（上記偏差↑Ｄfmtをゼロに近づけるように）制御されるので、上記積分項の値↑ΔＦＭt_intは、基準歩容の作成のために使用された想定床面の位置及び姿勢の、実床面に対する定常的な誤差に起因して発生するものとなる。
【０２６５】
従って、この↑ΔＦＭt_intに対応する代表接触面のばね性並進・回転変位量が基準歩容の目標運動の作成のために使用された想定床面の位置及び姿勢の、実際の床面（実床面）に対する定常的な誤差分に相当するものと考えられる。
【０２６６】
そこで、床面推定部７０は、与えられた積分項の値↑ΔＦＭt_intを、前記式（２８−１）により、ばね性並進・回転変位量に変換したものを、想定床面の位置及び姿勢の誤差分↑Ｘc_intとして得る。この場合、より詳しくは、積分項の値↑ΔＦＭt_intのうちの並進力成分↑ΔＦt_intに、代表接触面の並進ばね定数行列Ｋc_orgの逆行列Ｋc_org-1を乗じたもの（＝Ｋc_org-1・↑ΔＦt_int）が想定床面の位置の誤差分↑Ｘc_org_intとされる。また、積分項の値↑ΔＦＭt_intのうちのモーメント成分↑ΔＭt_intに、代表接触面の回転ばね定数行列Ｋc_rotの逆行列Ｋc_rot-1を乗じたもの（＝Ｋc_rot-1・↑ΔＭt_int）が想定床面の姿勢の誤差分↑Ｘc_rot_intとされる。
【０２６７】
そして、床面推定部７０は、上記の如く算出した誤差分↑Ｘc_intに応じて想定床面の位置及び姿勢を補正することにより、実床面の位置及び姿勢の推定値としての床面推定値を決定する。
【０２６８】
このように床面推定部７０により決定された床面推定値は、本実施形態では、前記基準歩容生成部６１に与えられる。そして、基準歩容生成部６１では、床面推定値に応じて、基準歩容を作成するために使用する想定床面を定期的に（例えば、移動体１の歩行動作時の１歩毎、又は複数歩毎に）更新する。
【０２６９】
なお、基準歩容の作成に使用する想定床面は、床面推定値だけに応じて決定する必要はない。例えば、移動体１に搭載した視覚センサや、外部から与えられる床情報に基づいて、実床面の位置及び姿勢を推定できるような場合には、その推定された床面の位置及び姿勢の信頼性の評価や補完的な修正のために、上記床面推定値を使用するようにしてもよい。
【０２７０】
以上が、本実施形態における制御装置５０の処理の詳細である。
【０２７１】
ここで、本実施形態と本発明との対応関係について補足しておく。本実施形態では、全床反力要求修正量決定部６４によって、本発明における全床反力要求修正量決定手段が実現される。この場合において、本実施形態では、修正後目標全床反力↑ＦＭt_cmd1が、本発明における目標全床反力に相当する。
【０２７２】
また、代表接触面並進・回転変位量算出部６５、代表接触面ヤコビアン行列算出部６６、関節変位修正量決定部６８、関節変位制御部６９によって、それぞれ、本発明における代表接触面位置姿勢変位量算出手段、代表接触面ヤコビアン行列算出手段、関節変位修正量決定手段、関節変位制御手段が実現される。
【０２７３】
また、関節変位修正量決定部６８において実行される処理のうち、図５のＳＴＥＰ２〜９の処理によって、本発明における擬似逆行列演算用パラメータ決定手段が実現される。
【０２７４】
また、床面推定部７０は、本発明（第２の態様）における代表接触面定常変位量算出手段としての機能を有する。この場合、該床面推定部７０が、前記積分項の値↑ΔＦＭt_intから、前記式（２８−１）により、想定床面の位置及び姿勢の誤差分↑Ｘc_intを算出する処理によって、代表接触面定常変位量算出手段が実現される。そして、この誤差分↑Ｘc_intが本発明における代表接触面定常変位量に相当する。
【０２７５】
以上説明した実施形態によれば、制御装置５０は、各制御処理周期において、修正後目標全床反力↑ＦＭ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を滑らかに変化させていくようにすることができる。ひいては、移動体１の各関節の変位量を滑らかに変化させるように、関節変位修正量を決定できる。
【０２８３】
また、上記の如く実全床反力を修正後目標全床反力↑ＦＭt_cmd1に追従させるように、移動体１の各関節の駆動制御を行いつつ、前記摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdの構成要素である前記積分項の値↑ΔＦＭt_intに対応する代表接触面の並進・回転変位量↑Ｘc_intを前記式（２８−１）により算出することによって、想定床面の位置及び姿勢の実床面に対する定常的な誤差分としての↑Ｘc_intを高い信頼性で求めることができる。このため、実床面の位置及び姿勢の推定を精度よく行なうことができる。
【０２８４】
次に、以上説明した実施形態の変形態様をいくつか説明する。
【０２８５】
前記実施形態では、移動体１は、２つの脚リンク３Ｒ，３Ｌを備えた脚式移動ロボットであるが、３つ以上の脚リンクを備えたロボットであってもよい。その場合、制御装置５０の処理のうち、代表接触面ヤコビアン行列算出部６６以外の処理は前記実施形態と同じでよい。そして、代表接触面ヤコビアン行列算出部６６の処理では、前記式（２７）のＮの値を、脚リンクの個数とした式に基づいて、代表接触面ヤコビアン行列Ｊ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を算出する処理と等価である。
【０２９０】
【数１１】
0005616289.tif 000012
_  この式（２７ａ）におけるｒ_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を決定するようにすればよい。
【０２９４】
また、前記実施形態では、修正後目標床反力↑ＦＭt_cmd1と、実全床反力の観測値↑ＦＭt_actとの偏差↑Ｄfmt（＝↑ＦＭt_cmd1−↑ＦＭt_act）に応じた比例項と積分項とを合成する（加え合わせる）ことによって、前記摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdを決定するようにしたが、積分項（＝↑ΔＦＭt_int）をそのまま、前記式前記摂動全床反力↑ΔＦＭtの要求値↑ΔＦＭt_dmdとして決定するようにしてもよい。
【０２９５】
このようにした場合には、前記代表接触面並進・回転変位量算出部６５により↑ΔＦＭt_dmdから算出される代表接触面のばね性並進・回転変位量↑ΔＸcが、想定床面の位置及び姿勢の実床面に対する定常的な誤差分↑Ｘc_intに相当するものとなる。
【０２９６】
従って、この場合には、床面推定部７０においては、前記式（２８−１）により↑Ｘc_intを改めて算出する必要はなく、前記代表接触面並進・回転変位量算出部６５により算出された代表接触面のばね性並進・回転変位量↑ΔＸcに応じて、想定床面の位置及び姿勢を修正することで、床面推定値を決定するようにすればよい。このようにすることで、本願発明の第１の態様の一実施形態が構築されることとなる。
【符号の説明】
【０２９７】
１，１０１…移動体、２，１０２…基体、３_i，１０３_ｉ…脚リンク、５０…制御装置、６４…全床反力要求修正量決定部（全床反力要求修正量決定手段）、６５…代表接触面並進・回転変位量算出部（代表接触面位置姿勢変位量算出手段）、６６…代表接触面ヤコビアン行列算出部（代表接触面ヤコビアン行列算出手段）、６８…関節変位修正量決定部（関節変位修正量決定手段）、６９…関節変位制御部（関節変位制御手段、７０…床面推定部（代表接触面定常変位量算出手段）、ＳＴＥＰ２〜９…擬似逆行列演算用パラメータ決定手段。
【図１】
0005616289.tif 000015
【図２】
0005616289.tif 000016
【図３】
0005616289.tif 000017
【図４】
0005616289.tif 000018
【図５】
0005616289.tif 000019
【図６】
0005616289.tif 000020
【図７】
0005616289.tif 000021