JP5623375B2 - 脚式移動ロボットの歩容生成装置 - Google Patents

Description

本発明は脚式移動ロボットの歩容生成装置に関する。

従来、脚式移動ロボットの目標歩容を生成する技術として、例えば非特許文献１に見られるように、移動ロボットの全体の並進運動量ベクトル（全体重心の並進運動量ベクトル）と、該移動ロボットの全体の角運動量ベクトルとの目標を設定し、その目標を満足するように移動ロボットの各関節の変位量を規定する目標歩容を生成する技術が提案されている。

また、この技術を利用して、二足歩行ロボットの歩容生成を行う技術が、例えば特許文献１に記載されている。

ＷＯ２００８／１０５５０６

「分解運動量制御：運動量と角運動量に基づくヒューマノイドロボットの全身運動生成」／梶田、金広、金子、藤原、原田、横井、比留川／日本ロボット学会誌、Vol.22 No.6，pp.772-779, 2004

移動ロボットに移動環境の床面等から作用する外力は、該移動ロボットの全体の並進運動量ベクトル及び角運動量ベクトルの時間的変化率と、該移動ロボットに作用する重力とに依存して規定される。従って、前記非特許文献１に見られるように、移動ロボットの全体の並進運動量ベクトル及び角運動量ベクトルの目標を設定して、この目標を満たすように移動ロボットの目標歩容を決定することで、間接的に、移動ロボットに作用させるべき外力を制御できることとなる。

しかるに、前記非特許文献１に見られる如き従来の技術では、移動ロボットの全体の並進運動量ベクトルと角運動量ベクトルとを目標に追従させるような該移動ロボットの目標歩容を生成することは可能であるものの、該並進運動量ベクトルと角運動量ベクトルとは移動ロボットの各部の並進移動速度や回転速度に依存する物理量であるために、生成される目標歩容における瞬時瞬時の移動ロボットの基体の位置又は姿勢が、安定性の高い適切な位置又は姿勢からオフセットを生じやすいということが本願発明者の検討により判明した。

そして、このように移動ロボットの目標歩容における基体の位置又は姿勢が安定性の高い適切な位置又は姿勢からオフセットを生じると、そのオフセットが拡大して、該移動ロボットの姿勢が崩れやすい目標歩容が生成されるようになる。ひいては、移動ロボットの運動を安定に継続し得る目標歩容を生成することが困難となるという不都合がある。

本発明は、かかる背景に鑑みてなされたものであり、脚式移動ロボットの基体の位置や姿勢が適切な状態からずれるのを防止しつつ、該脚式移動ロボットの全体の並進運動量ベクトルと角運動量ベクトルとを目標に追従させるように目標歩容を生成することができる歩容生成装置を提供することを目的とする。

本発明の脚式移動ロボットの歩容生成装置は、かかる目的を達成するために、基体から延設されると共に複数の関節がそれぞれ備えられた複数の脚リンクの運動によって移動する脚式移動ロボットの各関節の変位量を規定する目標歩容を逐次生成する歩容生成装置であって、
前記目標歩容の構成要素としての前記移動ロボットの各脚リンクの先端部の位置及び姿勢の目標値である目標脚位置姿勢を逐次決定する目標脚位置姿勢決定手段と、
前記移動ロボットの運動を行なうための動力学的な制約条件が該移動ロボットの動力学を表す動力学モデル上で満たされるように、該動力学モデルと前記決定された目標脚位置姿勢とを用いて、該移動ロボットの基体の位置及び姿勢の基本目標値である基本目標基***置姿勢と、該移動ロボットの全体重心の位置の基本目標値である基本目標全体重心位置と、該移動ロボットの全体重心の並進運動量ベクトルの基本目標値である基本目標並進運動量ベクトルと、該移動ロボットの全体重心周りの角運動量ベクトルの基本目標値である基本目標角運動量ベクトルとを逐次決定する基本動作目標値決定手段と、
前記基体の位置及び姿勢の単位時間当たりの変化量を成分として構成される基体運動速度ベクトルから、該基体運動速度ベクトルの各成分と前記移動ロボットの全体重心の並進運動量ベクトルの各成分と該移動ロボットの全体重心の周りの角運動量ベクトルの各成分とにより構成されるベクトルである制御対象ベクトルへの線形写像を表すヤコビアン行列を、既に決定された前記移動ロボットの目標歩容のうちの少なくとも最新値の目標歩容を用いて逐次決定しつつ、該ヤコビアン行列の擬似逆行列を、前記制御対象ベクトルの基本目標値に乗じることにより、前記基体運動速度ベクトルの目標値を逐次決定する基体運動速度目標値決定手段と、
前記決定された基体運動速度ベクトルの目標値を逐次積分することにより、前記目標歩容の構成要素としての前記基体の位置及び姿勢の目標値である目標基***置姿勢を逐次決定する目標基***置姿勢決定手段と、
前記決定された目標脚位置姿勢と目標基***置姿勢とを構成要素とする前記目標歩容に対応する前記移動ロボットの全体重心の位置である目標歩容上全体重心位置を、該目標歩容から前記移動ロボットの幾何学モデルに基づいて逐次算出する目標歩容上重心位置算出手段と、
前記算出された目標歩容上全体重心位置を前記決定された基本目標全体重心位置に追従させるために該目標歩容上全体重心位置と基本目標全体重心位置との偏差に応じて決定したフィードバック操作量により前記決定された基本目標並進運動量ベクトルを修正してなる修正後基本目標並進運動量ベクトルを逐次決定する基本目標並進運動量修正手段と、
前記目標基***置姿勢を前記基本目標基***置姿勢に追従させるように前記決定された目標基***置姿勢と前記決定された基本目標基***置姿勢との偏差に応じて前記基体運動速度ベクトルの基本目標値である基本目標基体運動速度ベクトルを逐次決定する基本目標基体運動速度決定手段とを備え、
前記基体運動速度目標値決定手段が前記基体運動速度ベクトルの目標値を決定するために用いる前記制御対象ベクトルの基本目標値として、前記決定された基本目標基体運動速度ベクトルの各成分と、前記決定された修正後基本目標並進運動量ベクトルの各成分と、前記決定された基本目標角運動量ベクトルの各成分とにより構成されるベクトルが該基体運動速度目標値決定手段に入力されることを特徴とする（第１発明）。

かかる第１発明によれば、前記目標脚位置姿勢決定手段により決定された各脚リンクの目標脚位置姿勢（各脚リンクの先端部の位置及び姿勢の目標値）と、前記基体運動速度目標値決定手段に逐次決定される基体運動速度ベクトル（基体の位置及び姿勢の単位時間当たりの変化量を成分として構成されるベクトル）の目標値を前記目標基***置姿勢決定手段により逐次積分することにより決定された目標基***置姿勢（基体の位置及び姿勢の目標値）とが前記目標歩容の構成要素として逐次決定されることとなる。

この場合、基体運動速度ベクトルの目標値を決定する基体運動速度目標値決定手段は、基体運動速度ベクトルから、該基体運動速度ベクトルの各成分と前記移動ロボットの全体重心の並進運動量ベクトルの各成分と該移動ロボットの全体重心の周りの角運動量ベクトルの各成分とにより構成されるベクトルである制御対象ベクトルへの線形写像を表すヤコビアン行列を、既に決定された前記移動ロボットの目標歩容のうちの少なくとも最新値の目標歩容を用いて逐次決定しつつ、該ヤコビアン行列の擬似逆行列を、前記制御対象ベクトルの基本目標値に乗じることにより、前記基体運動速度ベクトルの目標値を決定する。

このため、基体運動速度ベクトルの目標値は、制御対象ベクトルの基本目標値を構成する成分のうちの、前記移動ロボットの全体重心の並進運動量ベクトルの値と該全体重心周りの角運動量ベクトルの値とに加えて、制御対象ベクトルの基本目標値のうちの基体運動速度ベクトルの値をも目標として決定されることとなる。

すなわち、基体運動速度ベクトルの目標値は、それに対応して生じる基体の位置及び姿勢の変化（目標歩容の現在の状態からの変化）に伴い発生する移動ロボットの全体重心の並進運動量ベクトルと、該全体重心周りの角運動量ベクトルとが、それぞれ制御対象ベクトルの基本目標値により規定される値に追従するようにしつつ、該基体の位置及び姿勢が、制御対象ベクトルの基本目標値により規定される値から乖離しないように決定されることとなる。

この場合、前記制御対象ベクトルの基本目標値のうち、移動ロボットの全体重心の並進運動量ベクトルは、前記修正後基本目標並進運動量ベクトルある。この修正後基本目標並進運動量ベクトルは、前記基本動作目標値決定手段により決定された基本目標並進運動量ベクトルを、既に決定された目標歩容に対応する目標歩容上全体重心位置と前記基本動作目標値決定手段により決定した基本全体重心位置との偏差に応じて決定したフィードバック操作量によって修正してなるベクトルである。

また、前記制御対象ベクトルの基本目標値のうち、移動ロボットの全体重心周りの角運動量ベクトルは、前記基本動作目標値決定手段により決定された基本目標角運動量ベクトルである。

従って、前記制御対象ベクトルの基本目標値のうち、移動ロボットの全体重心の並進運動量ベクトルと、該全体重心周りの角運動量ベクトルとは、決定しようとする目標歩容により規定される移動ロボットの全体重心の並進運動量ベクトルと、該全体重心の位置と、該全体重心の周りの角運動量とを、それぞれ、前記移動ロボットの運動を行なうための動力学的な制約条件が前記動力学モデル上で満たされるように前記基本動作目標値決定手段が決定した基本目標並進運動量ベクトル、基本目標全体重心位置、基本目標角運動量ベクトルとに追従させるべく決定されたものとなる。

なお、上記動力学的な制約条件は、移動ロボットの所要の運動を動力学的に行い得るようにするために、移動ロボットに作用する外力、あるいは、移動ロボットが運動によって発生する慣性力を制約する条件である。その制約条件の例としては、例えば、ＺＭＰ（Zero Moment Point）が支持多角形内に収まるという条件や、移動ロボットに作用する並進床反力がある許容範囲に収まるという条件等が挙げられる。

また、前記制御対象ベクトルの基本目標値のうち、前記基体運動速度ベクトルの基本目標値は、前記目標基***置姿勢を前記基本目標基***置姿勢に追従させるように、既に決定された目標基***置姿勢と基本目標基***置姿勢との偏差に応じて決定されたものである。

このため、かかる制御対象ベクトルの基本目標値を目標として、前記基体運動速度ベクトルの目標値を決定することにより、該基体運動速度ベクトルの目標値を積分してなる目標基***置姿勢を構成要素として最終的に逐次決定される目標歩容は、それにより規定される移動ロボットの全体重心の並進運動量ベクトルと該全体重心周りの角運動量ベクトルとを、それぞれ上記修正後基本並進運動量ベクトルと基本角運動量ベクトルとに追従させつつ、該目標歩容における目標基***置姿勢が、上記基本目標基***置姿勢から乖離しないように決定されることなる。

よって、第１発明によれば、脚式移動ロボットの基体の位置や姿勢が適切な状態からずれるの防止しつつ、該脚式移動ロボットの全体の並進運動量ベクトルと角運動量ベクトルとを目標に追従させるように目標歩容を生成することができる。

また、この場合、制御対象ベクトルの基本目標値を構成する修正後基本並進運動量ベクトルと基本角運動量ベクトルと基本目標基***置姿勢とが上記の如く決定されたものであるから、最終的に逐次決定される目標歩容は、換言すれば、前記動力学モデル上での動力学的な制約条件から逸脱するようなことがなく、しかも、該目標歩容における目標基***置姿勢が、該動力学モデル上で適切な運動を行ない得るように決定された基本目標上***置姿勢から乖離することがないように決定されることとなる。

従って、第１発明によれば、動力学的な要件を満たしつつ、移動ロボットが姿勢を崩すことが無いような目標歩容を生成できる。

かかる第１発明において、基体運動速度ベクトルの積分値としての移動ロボットの基体の位置及び姿勢と、前記制御対象ベクトルのうちの移動ロボットの全体重心の並進運動量ベクトルの積分値に相当する状態量との間の関係は、一般には非線形な関係となる。同様に、移動ロボットの基体の位置及び姿勢と、前記制御対象ベクトルのうちの移動ロボットの全体重心周りの角運動量ベクトルの積分値に相当する状態量との間の関係も一般には非線形な関係となる。

このため、前記基体運動速度目標値決定手段の処理において、前記ヤコビアン行列を、既に決定された前記移動ロボットの目標歩容のうちの最新値の目標歩容を用いて逐次決定した場合、すなわち、当該最新値の目標歩容を、前記基体運動速度ベクトルの新たな目標値による前記基体の位置及び姿勢の変化の起点となる前記移動ロボットの動作状態であるとして前記ヤコビアン行列を決定した場合には、該ヤコビアン行列の擬似逆行列を前記制御対象ベクトルの基本目標値に乗算することにより決定される基体運動速度ベクトルの目標値は、制御対象ベクトルの基本目標値を満たす上で、過不足を生じることがある。

そこで、第１発明では、前記基体運動速度目標値決定手段は、前記基体運動速度ベクトルの新たな目標値を決定するために用いる前記ヤコビアン行列を決定するとき、既に決定された前記移動ロボットの目標歩容のうちの最新値の目標歩容に対応する時刻と前記基体運動速度ベクトルの新たな目標値に応じて前記目標基***置姿勢決定手段により決定される目標基***置姿勢を構成要素とする新たな目標歩容に対応する時刻との間の期間内での前記移動ロボットの将来の歩容の瞬時値を、前記最新値の目標歩容とその直前の１つ以上の過去値の目標歩容とに基づいて推定し、その推定した将来の歩容の瞬時値を、前記基体運動速度ベクトルによる前記基体の位置及び姿勢の変化の起点となる前記移動ロボットの動作状態として用いて、該移動ロボットの当該起点の動作状態に対応する前記ヤコビアン行列を決定することが好ましい（第２発明）。

この第２発明によれば、前記最新値の目標歩容により規定される前記移動ロボットの動作状態よりも、将来の予測される動作状態寄りにずらした動作状態を、前記基体運動速度ベクトルによる前記基体の位置及び姿勢の変化（換言すれば基体運動速度ベクトルの積分値の変化）の起点として、前記ヤコビアン行列が決定される。

このようにすることで、上記非線形性の影響を補償し、制御対象ベクトルの基本目標値を満たす上で最適な基体運動速度ベクトルの値に対してより一致度合いの高い基体運動速度ベクトルの目標値を決定できる。ひいては、制御対象ベクトルの基本目標値を満たし得る目標歩容を高い信頼性で生成することができる。

この第２発明では、より具体的な形態として、次のような構成を採用できる。すなわち、前記基体運動速度目標値決定手段は、前記最新値の目標歩容における目標基***置姿勢と該最新値の目標歩容の１つ前の過去値の目標歩容における目標基***置姿勢との差分に“０”と“１”との間の範囲内であらかじめ定めた係数Ｋｐを乗じてなる補正量を、前記最新値の目標歩容における目標基***置姿勢に加えることにより該目標基***置姿勢の将来の瞬時値を推定する手段と、各脚リンク毎に前記最新値の目標歩容における目標脚位置姿勢と該最新値の１つ前の過去値の目標歩容における目標脚位置姿勢との差分に前記係数Ｋｐを乗じてなる補正量を、前記最新値の目標歩容における目標脚位置姿勢に加えることにより各脚リンクの目標脚位置姿勢の将来の瞬時値を推定する手段とを備え、前記推定された目標基***置姿勢の将来の瞬時値と各脚リンクの目標脚位置姿勢の将来の瞬時値とを構成要素とする歩容を、前記移動ロボットの当該起点の動作状態として用いる（第３発明）。

この第３発明によれば、上記係数Ｋｐを用いることで、既に決定された目標歩容のうちの前記最新値の目標歩容と、その１つ前の過去値の目標歩容とから、最新値の目標歩容に対応する時刻と前記新たな目標歩容に対応する時刻との間の期間内での前記移動ロボットの将来の歩容の瞬時値を、簡易な手法で適切に推定することができる。

また、前記第１〜第３発明では、前記基体運動速度目標値決定手段は、前記ヤコビアン行列の特異点低感度運動分解行列を該ヤコビアン行列の擬似逆行列として用いて前記基体運動速度ベクトルの目標値を決定することが好ましい（第４発明）。

この第４発明によれば、基体運動速度ベクトルの目標値の各成分の二乗値を、任意に設定した重み係数を用いて線形結合してなる値と、該基体運動速度ベクトルの目標値に前記決定されたヤコビアン行列を乗じてなるベクトルと制御対象ベクトルの基本目標値との差分のベクトルの各成分の二乗値を、任意に設定した重み係数を用いて線形結合してなる値との総和を最小にするような、前記擬似逆行列を決定できる。

従って、基体運動速度ベクトルの各成分の大きさの許容範囲や、基体運動速度ベクトルの目標値に対応する制御対象ベクトルの値の該制御対象ベクトルの基本目標値に対する誤差の許容度合いを考慮して、前記擬似逆行列を決定できる。

本発明の一実施形態における脚式移動ロボットの概略構成を示す図。 図１に示す脚式移動ロボットの制御に関する構成を示す図。 図２に示す歩容生成部の機能を示すブロック図。 図３に示す目標脚位置姿勢決定部で決定する目標脚位置姿勢の軌道の例を示す図。 図３に示す基本動作目標値決定部の処理で用いる動力学モデルを模式的に示す図。 図３に示す基体運動速度目標値決定部の処理を説明するためのグラフ。 図７（ａ），（ｂ）はそれぞれ実施例、比較例におけるロボットの基体の姿勢（傾斜角度）の経時変化の例を示すグラフ。 図８（ａ），（ｂ）はそれぞれ実施例、比較例におけるロボットの全体重心の並進運動量の誤差の経時変化の例を示すグラフ。 図９（ａ），（ｂ）はそれぞれ実施例、比較例におけるロボットの全体重心周りの角運動量の誤差の経時変化の例を示すグラフ。

以下、本発明の一実施形態を図１〜図６を参照して説明する。

図１は、本実施形態で例示する脚式移動ロボットの概略構成を模式的に示している。この脚式移動ロボット１（以下、単にロボット１という）は、基体２と、基体２から延設された左右一対（２つ）の脚リンク３Ｒ，３Ｌとを有する２足移動ロボットである。

なお、本明細書では、ロボット１の前方に向かって右側の部材又はその部材に関連する量を示す変数に符号“Ｒ”を付加し、ロボット１の前方に向かって左側の部材又はその部材に関連する量を示す変数に符号“Ｌ”を付加する。ただし、右側、左側を特に区別する必要が無いときは、符号“Ｒ”，“Ｌ”を省略することがある。

基体２は、ロボット１の上体に相当するリンク部分である。この基体２は、本実施形態では、下側基体１０と、この下側基体１０の上方に配置された上側基体１１との２つの要素リンクから構成されている。下側基体１０は、ロボット１の腰部に相当する部分、上側基体１１は、ロボット１胸部に相当する部分である。そして、上側基体１１は、ヨー軸周り（Ｚ軸周り）の回転自由度を有する基体回旋用関節１２を介して下側基体１０に連結されている。

左右一対の脚リンク３Ｒ，３Ｌは、互いに同じ構造のものである。具体的には、各脚リンク３は、下側基体１０に股関節部１３を介して連結された大腿部１４と、この大腿部１４に膝関節部１５を介して連結された下腿部１６と、この下腿部１６に足首関節部１７を介して連結された足平部１８とを、該脚リンク３を構成する複数の要素リンクとして備える。この場合、各脚リンク３の先端部は、足平部１８により構成される。

そして、各脚リンク３の股関節部１３は、ヨー軸周り、ピッチ軸周り（Ｙ軸周り）及びロール軸周り（Ｘ軸周り）の回転自由度をそれぞれ有する３つの関節１９，２０，２１により構成されている。また、膝関節部１５は、ピッチ軸まわりの回転自由度を有する関節２２により構成されている。また、足首関節部１７は、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する２つの関節２３，２４により構成されている。

従って、各脚リンク３の先端部である足平部１８は、本実施形態では、下側基体１０に対して、６自由度の運動自由度を有する。なお、上記の説明における各脚リンク３の関節１９〜２４の回転軸（ロール軸、ピッチ軸、ヨー軸）は、脚リンク３を上下方向に延在させた状態での回転軸を示している。

以上が、本実施形態のロボット１の基本構造である。かかる構成のロボット１は、各脚リンク３の６個の関節１９〜２４を駆動することによって、各脚リンク３の空間的な運動が行なわれ、この運動によって床上を移動することが可能となっている。例えば、人の歩行動作あるいは走行動作と同様の形態（歩容）で脚リンク３Ｒ，３Ｌを運動させることによって、ロボット１の歩行動作あるいは走行動作を行なうことが可能である。

また、本実施形態では、基体２を構成する下側基体１０と、上側基体１１とが関節１２を介して連結されているので、この関節１２を駆動することによって、基体２をその体幹軸に相当する関節１２の回転軸周りに捩じることが可能となっている。

補足すると、ロボット１は、上記基体２、及び脚リンク３Ｒ，３Ｌだけでなく、例えば上側基体１１の両側部から延設された腕リンクや、該上側基体１１の上端部に搭載した頭部等を備えていてもよい。また、基体２の関節１２を省略し、下側基体１０及び上側基体１１を一体に構成してもよい。

図１での図示は省略したが、ロボット１には、図２に示すように、上記の各関節をそれぞれ回転駆動する関節アクチュエータ３０と、ロボット１の動作制御を行なう制御処理ユニット３１とが搭載されている。

関節アクチュエータ３０は、例えば電動モータあるいは油圧アクチュエータにより構成されたものであり、各関節毎に備えられている。この場合、各関節アクチュエータ３０による各関節の駆動機構は公知の構造のものでよい。また、関節アクチュエータ３０は、回転型のアクチュエータに限らず、直動型のアクチュエータであってもよい。

制御処理ユニット３１は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む電子回路ユニットである。この制御処理ユニット３１は、実装されるプログラム等により実現される主要な機能として、ロボット１の目標歩容を逐次生成する歩容生成部３２と、該目標歩容に応じてロボット１の各関節の変位量（回転角）を関節アクチュエータ３０を介して制御する関節アクチュエータ制御部３３とを備える。

歩容生成部３２は、本発明の装置としての機能を制御処理ユニット３１に付与するものである。この歩容生成部３２は、ロボット１の目標歩容の構成要素として、各脚リンク３の先端部を構成する足平部１８の空間的な位置及び姿勢の目標値である目標脚位置姿勢と、基体２の空間的な位置及び姿勢の目標値である目標基***置姿勢とを逐次決定する処理を後述する如く実行する。

この場合、本実施形態では、基体２は関節１２を備えるため、上側基体１１を下側基体１０に対して関節１２の回転軸周りに捩じることが可能となっている。このため、本実施形態における目標基***置姿勢は、より詳しくは、下側基体１０の空間的な位置及び姿勢の目標値と、上側基体１１の下側基体１０に対する捩じり角（関節１２の変位量）の目標値とから構成される。

また、各脚リンク３の足平部１８の位置は、より詳しくは、該足平部１８のあらかじめ定めた代表点の位置であり、該足平部１８の姿勢は、該足平部１８の空間的な向きを意味する。このことは、下側基体１０の位置及び姿勢についても同様である。

これらの位置及び姿勢は、ロボット１の動作環境の床面に対して固定された慣性座標系であるグローバル座標系で見た位置及び姿勢として記述される。このグローバル座標系は、任意に設計的に定めたものでよい。例えばロボット１の両脚リンク３Ｒ，３Ｌのうちの支持脚（遊脚としての一方の脚リンク３の足平部１８を空中で動かすときにロボット１の自重を支えることとなる他方の脚リンク３）の足平部１８の接地時における該足平部１８の直下（もしくはその近辺）の床面上に原点を有し、且つ、該足平部１８の前後方向の水平軸及び左右方向の水平軸をそれぞれＸ軸、Ｙ軸とし、鉛直軸をＺ軸とする３軸直交座標系（所謂、支持脚座標系）をグローバル座標系として用いることができる。

本実施形態では、便宜上、グローバル座標系として上記支持脚座標系が用いられるものとする。そして、特にことわらない限り、Ｘ軸、Ｙ軸、Ｚ軸は、上記支持脚座標系の座標軸を意味するものとする。

補足すると、基体２を構成する下側基体１０及び上側基体１１の一方の基体１０又は１１の空間的な位置及び姿勢と、関節１２の変位量とが決まれば、他方の基体１１又は１０の空間的な位置及び姿勢も一義的に決まる。従って、目標基***置姿勢は、上側基体１１の空間的な位置及び姿勢の目標値と、関節１２の変位量の目標値との組により構成されていてもよい。

関節アクチュエータ制御部３３は、各関節の実際の変位量を、歩容生成部３２により決定された目標歩容により規定されるロボット１の各関節の変位量の目標値に追従させるように、各関節に対応する関節アクチュエータ３０を制御する。

この場合、各関節の実際の変位量（回転角）が、図示しないロータリエンコーダ等の変位量センサにより検出される。そして、関節アクチュエータ制御部３３は、各関節の実際の変位量の検出値と目標値との偏差に応じて、フィードバック制御により関節アクチュエータ３０の駆動力を制御する。

以降、制御処理ユニット３１の歩容生成部３２の処理を詳細に説明する。

図３に示すように、歩容生成部３２は、目標歩容を生成するための主要な処理部として、目標脚位置姿勢を逐次決定する目標脚位置姿勢決定部４１と、各脚リンク３の足平部１８の位置及び姿勢以外のロボット１の所定種類の状態量に関する基本の動作目標を逐次決定する基本動作目標値決定部４２とを備える。

この場合、基本動作目標値決定部４２が決定する動作目標は、ロボット１の全体重心の位置の基本の目標値である基本目標重心位置↑Ｘg_cmd1と、ロボット１の全体の並進運動量ベクトル（＝全体重心の並進運動量ベクトル）の基本の目標値である基本目標並進運動量ベクトル↑Ｐ_cmd1と、ロボット１の全体重心周りの全体の角運動量ベクトルの基本の目標値である基本目標角運動量ベクトル↑Ｌ_cmd1と、基体２の位置及び姿勢の基本の目標値である基本目標基***置姿勢とから構成される。

なお、基本目標重心位置↑Ｘg_cmd1、基本目標並進運動量ベクトル↑Ｐ_cmd1、基本目標角運動量ベクトル↑Ｌ_cmd1は、それぞれ、グローバル座標系の各座標成分を構成要素とする３成分のベクトル（縦ベクトル）である。本実施形態の説明では、“↑”は、ベクトル（縦ベクトル）を表記する記号として用いる。

また、歩容生成部３２は、基本動作目標値決定部４２により決定する基本の動作目標のうちの基本目標並進運動量ベクトル↑Ｐ_cmd1を修正してなる修正後基本目標並進運動量ベクトル↑Ｐ_cmd2を逐次決定する基本目標並進運動量修正部４３と、基体２の位置及び姿勢の単位時間当たりの変化量（すなわち変化速度）の基本の目標値である基本目標基体運動速度ベクトル↑Ｖb_cmd1を逐次決定する基本目標基体運動速度決定部４４と、修正後基本目標並進運動量ベクトル↑Ｐ_cmd2、基本目標角運動量ベクトル↑Ｌ_cmd1及び基本目標基体運動速度ベクトル↑Ｖb_cmd1を目標として、基体２の位置及び姿勢の単位時間当たりの変化量の目標値である目標基体運動速度ベクトル↑Ｖb_cmd2を逐次決定する基体運動速度目標値決定部４５とを備える。

上記基本目標基体運動速度ベクトル↑Ｖb_cmd1及び目標基体運動速度ベクトル↑Ｖb_cmd2は、それぞれ、グローバル座標系で見た基体２の代表点（本実施形態では下側基体１０の代表点）の並進移動速度の各座標軸成分と、グローバル座標系で見た基体２の姿勢（本実施形態では下側基体１０の姿勢）の各座標軸周りの角速度成分と、基体２の関節１２の変位量の時間的変化率（角速度）とを並べた７成分のベクトル（縦ベクトル）である。

また、目標基体運動速度ベクトル↑Ｖb_cmd2は、換言すれば、歩容生成部３２が逐次決定する目標歩容における目標基***置姿勢の時間的変化率である。

歩容生成部３２はさらに、基体運動速度目標値決定部４５により逐次決定される目標基体運動速度ベクトル↑Ｖb_cmd2を積分する（↑Ｖb_cmd2の各成分を積分する）ことにより目標基***置姿勢を逐次決定する目標基***置姿勢決定部４６と、目標基***置姿勢及び両脚リンク３Ｒ，３Ｌの目標脚位置姿勢を構成要素とする目標歩容により規定されるロボット１の全体重心の位置である目標歩容上重心位置↑Ｘg_cmd2を逐次算出する目標歩容上重心位置算出部４７とを備える。

上記目標歩容上重心位置↑Ｘg_cmd2は、基本目標重心位置↑Ｘg_cmd1と同様に、グローバル座標系の各座標成分を構成要素とする３成分のベクトル（縦ベクトル）である。

本実施形態では、歩容生成部３２はさらに、目標歩容を構成する目標基***置姿勢及び両脚リンク３Ｒ，３Ｌの目標脚位置姿勢を用いて、ロボット１の各関節の変位量の目標値である目標関節変位量を逐次決定する目標関節変位量決定部４８を備えている。ただし、この目標関節変位量決定部４８は、前記関節アクチュエータ制御部３３に備えるようにしてもよい。

歩容生成部３２は、以上の各処理部４１〜４８の処理を所定の演算処理周期で逐次実行することにより、目標歩容の構成要素である目標基***置姿勢と、両脚リンク３Ｒ，３Ｌの目標脚位置姿勢とを逐次決定すると共に、該目標歩容に対応する各関節の目標関節変位量を逐次決定する。

以下に、歩容生成部３２の各処理部４１〜４８の処理の詳細を含めて、歩容生成部３２の処理の全体を説明する。なお、本実施形態では、主に、ロボット１の歩行動作又は走行動作による移動を行なうための目標歩容を生成する場合を一例として歩容生成部３２の処理を説明する。

歩容生成部３２は、各演算処理周期において、まず、目標脚位置姿勢決定部４１及び基本動作目標値決定部４２の処理を実行する。

本実施形態では、目標脚位置姿勢決定部４１及び基本動作目標値決定部４２には、脚リンク３Ｒ，３Ｌのうちの遊脚の着地が行なわれる毎に、次回の遊脚を含めて複数歩分の遊脚の足平部１８の目標とする着地位置及び着地姿勢である目標着地位置姿勢と、目標とする着地時刻である目標着地時刻とが与えられるようになっている。なお、目標着地時刻の代わりに、ロボット１の１歩分の期間（歩容周期）の目標時間を用いててもよい。

上記目標着地位置姿勢及び目標着地時刻は、ロボット１の移動計画、あるいは、操縦装置による移動指令に基づいて決定される。その決定処理は、制御処理ユニット３１で行なうようにしてもよいことはもちろんであるが、ロボット１の外部のサーバで行うようにしてもよい。

そして、目標脚位置姿勢決定部４１は、与えられた目標着地位置姿勢及び目標着地時刻に応じて、各脚リンク３の足平部１８の離床直前の接地状態での位置及び姿勢から次の目標着地位置姿勢に至る位置及び姿勢の目標軌道（足平部１８の位置及び姿勢の目標値の時系列パターン）を決定し、その目標軌道の瞬時値として各脚リンク３の目標脚位置姿勢を逐次決定する。

この場合、各脚リンク３の足平部１８の位置及び姿勢の目標軌道（目標脚位置姿勢の軌道）は、該脚リンク３が遊脚となる期間で、例えば図４に例示するようなパターンで該足平部１８の目標位置及び目標姿勢が連続的に変化しつつ、該足平部１８が空中を移動するように決定される。この目標軌道の始端の足平部１８の位置及び姿勢は、離床前の着地動作時の目標着地位置姿勢に相当し、該目標軌道の終端の足平部１８の位置及び姿勢は、次の目標着地位置姿勢に相当する。

なお、各脚リンク３の足平部１８の接地中においては、該脚リンク３の位置及び姿勢の目標軌道は、該脚リンク３の足平部１８の位置及び姿勢を一定もしくはほぼ一定に保つように決定してもよいことはもちろんであるが、該足平部１８の接地箇所が、該足平部１８の踵寄りの箇所から爪先寄りの箇所に移行していくように決定してもよい。

また、各脚リンク３の足平部１８の空中での姿勢の軌道は、該足平部１８の姿勢を一定もしくはほぼ一定に保つように決定してもよい。

基本動作目標値決定部４２は、与えられた目標着地位置姿勢及び目標着地時刻に応じて（あるいは、各脚リンク３の足平部１８の接地中の目標脚位置姿勢の軌道に応じて）、ＺＭＰ（Zero Moment Point）の目標位置である目標ＺＭＰの軌道を決定する。

この場合、目標ＺＭＰの軌道は、目標ＺＭＰが所謂、支持多角形内でその境界に近づき過ぎずないような領域（ＺＭＰの存在許容領域）に位置し、且つ、不連続的な変化を生じないように設定される。このような目標ＺＭＰの軌道の設定手法としては、例えば特許第３６７４７８９号にて本願出願人が説明した手法等、公知の手法を用いることができる。

そして、基本動作目標値決定部４２は、目標脚位置姿勢決定部４１により決定された目標脚位置姿勢の時系列と、ロボット１の動力学（ロボット１に作用する外力（ここでは床反力）とロボット１の運動との関係）を表すものとしてあらかじめ定められた動力学モデルとを用いて、該動力学モデル上で目標ＺＭＰを満足し得る基体２の位置及び姿勢が前記基本目標基***置姿勢として逐次決定される。

ここで、目標ＺＭＰを満足し得る基体２の位置及び姿勢というのは、上記動力学モデルにおいて、該基体２の位置及び姿勢の時系列と、両脚リンク３Ｒ，３Ｌの目標脚位置姿勢の時系列とにより規定されるロボット１の全体の運動によって発生する慣性力と該ロボット１に作用する重力との合力によって、目標ＺＭＰ周りに発生するモーメントの水平軸周り成分（Ｘ軸周り成分及びＹ軸周り成分）がゼロになるような、基体２の位置及び姿勢を意味する。

上記動力学モデルとしては、目標ＺＭＰを満足し得る基体２の位置及び姿勢を比較的容易に算出し得るように、線形性の高い動力学モデルが使用される。例えば、特許第３６７４７８９号にて本願出願人が説明した動力学モデル（特許第３６７４７８９号の図１０に示す動力学モデル）が、基本動作目標値決定部４２の処理における動力学モデルとして採用することができる。

該動力学モデルは、図５に示すように、各脚リンク３Ｒ，３Ｌと基体２（ロボット１の上体）とにそれぞれ対応する３つの質点３ｍＲ，３ｍＬ，２ｍと、基体２の姿勢（詳しくは、下側基体１０のＸ軸周り及びＹ軸周りの姿勢）の変化に伴う慣性力モーメントを発生するフライホイールＦＨ（質量を持たずにイナーシャを有するフライホイール）とを備える動力学モデルである。そして、この場合、基本目標基***置姿勢は、特許第３６７４７８９号にて本願出願人が説明した目標上***置姿勢の決定手法と同様の手法によって、決定することができる。

このようにすることにより、上記動力学モデル上において、ＺＭＰが支持多角形内に収まり、また、ロボット１に作用する床反力のうちの摩擦力成分が適切な許容範囲内に収まる、というような所要の動力学的な制約条件が満たされるように、基本目標基***置姿勢を決定できることとなる。

なお、本実施形態では、基本目標基***置姿勢のうち、基体２の関節１２の変位量の目標値は、例えば、上側基体１１が関節１２の回転軸周りに回転することで目標ＺＭＰ周りに発生する鉛直軸周りのモーメントが、各質点３ｍＲ，３ｍＬ，２ｍの並進運動によって目標ＺＭＰ周りに発生する鉛直軸周りのモーメントを打ち消すように決定される。

ただし、関節１２の変位量の目標値は、例えば一定値であってもよく、あるいは、一定のパターンで周期的に（脚リンク３Ｒ，３Ｌの運動に同期した周期で）変化するように設定されていてもよい。

本実施形態では、基本動作目標値決定部４２は、さらに、上記の如く決定した基本目標基***置姿勢の時系列と、目標脚位置姿勢決定部４１により決定された目標脚位置姿勢の時系列とから、上記動力学モデルを用いて、該動力学モデル上でのロボット１の全体重心の位置と、該全体重心の並進運動量ベクトルと、該全体重心周りの角運動量ベクトルとを、それぞれ、前記基本目標重心位置↑Ｘg_cmd1、基本目標並進運動量ベクトル↑Ｐ_cmd1、基本目標角運動量ベクトル↑Ｌ_cmd1として算出する。

この場合、基本目標重心位置↑Ｘg_cmd1は、各演算処理周期における脚リンク３Ｒ，３Ｌのそれぞれの目標脚位置姿勢と基本目標基***置姿勢とにより規定される質点３ｍＲ，３ｍＬ，２ｍの全体の重心の位置（位置ベクトル）として算出される。すなわち、脚リンク３Ｒ，３Ｌのそれぞれの目標脚位置姿勢と基本目標基***置姿勢とにより規定される各質点３ｍＲ，３ｍＬ，２ｍの位置ベクトルに、各質点３ｍＲ，３ｍＬ，２ｍの質量を乗じてなるベクトルの総和のベクトルを、ロボット１の全体の質量（＝質点３ｍＲ，３ｍＬ，２ｍの総和の質量）により除算してなるベクトルが、基本目標重心位置↑Ｘg_cmd1として算出される。

また、基本目標並進運動量ベクトル↑Ｐ_cmd1は、脚リンク３Ｒ，３Ｌのそれぞれの目標脚位置姿勢の時系列と基本目標基***置姿勢の時系列とにより規定される質点３ｍＲ，３ｍＬ，２ｍの全体の重心の移動速度ベクトル（＝↑Ｘg_cmd1の時間的変化率）にロボット１の全体の質量を乗じることにより算出される。すなわち、質点３ｍＲ，３ｍＬ，２ｍの全体の重心の並進運動量ベクトルが、基本目標並進運動量ベクトル↑Ｐ_cmd1として算出される。

また、基本目標角運動量ベクトル↑Ｌ_cmd1は、脚リンク３Ｒ，３Ｌのそれぞれの目標脚位置姿勢の時系列により規定される質点３ｍＲ，３ｍＬのそれぞれの並進運動量ベクトルが、質点３ｍＲ，３ｍＬ，２ｍの全体の重心の周りに発生する角運動量ベクトルと、基本目標基***置姿勢の時系列により規定される質点２ｍの並進運動量ベクトルが、質点３ｍＲ，３ｍＬ，２ｍの全体の重心の周りに発生する角運動量ベクトルと、基本目標基***置姿勢のうちの基体２の姿勢の時系列により規定されるフライホイールＦＨの回転運動によって発生する角運動量ベクトルと、下側基体１０に対する上側基体１１の回転運動（関節１２の回転軸周りの回転運動）によって発生する角運動量ベクトルとの総和の角運動量ベクトルとして算出される。

補足すると、ロボット１が腕リンク等、基体２に対して可動な他のリンクを備える場合には、基本動作目標値決定部４２は、当該他のリンクの各関節の変位量を規定する目標値（例えば、当該他のリンクの先端部の目標位置及び目標姿勢、あるいは、当該他のリンクの先端部の各関節の変位量の目標値等）も決定する。

この場合、ロボット１の歩行動作又は走行動作を行なう目標歩容の生成処理では、当該他のリンク（例えば腕リンク）の各関節の変位量を規定する目標値は、例えば、基体２のうちの上側基体１１に対する当該他のリンクの各部の相対的な位置及び姿勢を定常的に一定に保持するような目標値でよい。このようにした場合には、上記動力学モデルにおける質点２ｍの質量や、フライホイールＦＨのイナーシャ（慣性モーメント）は、当該他のリンクの影響分を含めて設定しておけばよい。

以上のように目標脚位置姿勢決定部４１及び基本動作目標値決定部４２の処理を実行した後、次に、歩容生成部３２は、目標歩容上重心位置算出部４７の処理と、前記基本目標並進運動量修正部４３及び基本目標基体運動速度決定部４４の処理とを実行する。

目標歩容上重心位置算出部４７には、歩容生成部３２が前回の演算処理周期で最終的に決定した目標歩容の構成要素（目標脚位置姿勢及び目標基***置姿勢）、換言すれば、歩容生成部３２が既に決定した目標歩容のうちの最新の（現在の）目標歩容の構成要素が与えられる。

そして、目標歩容上重心位置算出部４７は、与えられた目標歩容の構成要素により規定されるロボット１の全体重心の位置（グローバル座標系で見た位置ベクトル）を、ロボット１の幾何学モデル（剛体リンクモデル）に基づく幾何学演算によって算出し、この算出した位置を目標歩容上重心位置↑Ｘg_cmd2として決定する。

さらに詳細には、上記幾何学演算に用いるロボット１の幾何学モデルは、例えばロボット１の各要素リンクの重心位置に質点（該要素リンクの質量を有する質点）が設定されたモデルである。

そして、目標歩容上重心位置算出部４７は、まず、与えられた最新の目標歩容から、該目標歩容により規定されるロボット１の各関節の変位量を逆運動学演算の処理により算出する。

次いで、目標歩容上重心位置算出部４７は、この算出した各関節の変位量と、与えられた最新の目標歩容の目標脚位置姿勢又は目標基***置姿勢とにより規定される上記幾何学モデルの各質点の位置を算出する。

そして、目標歩容上重心位置算出部４７は、このように算出した各質点の位置と、該質点の質量とから、該幾何学モデルの全ての質点の質量中心の位置としてのロボット１の全体重心の位置を算出する。このように算出されたロボット１の全体重心の位置が、上記目標歩容上重心位置↑Ｘg_cmd2として決定される。

なお、歩容生成部３２の前回の演算処理周期で最終的に決定された最新の目標歩容に対応する目標歩容上重心位置↑Ｘg_cmd2を目標歩容上重心位置算出部４７により上記の如く決定する処理は、前回の演算処理周期で行なうようにしてもよい。

補足すると、ロボット１が腕リンク等、基体２に対して可動な他のリンクを備える場合には、当該他のリンクの質量を考慮した幾何学モデルを用いて上記目標歩容上重心位置↑Ｘg_cmd2を決定すればよい。

この場合には、目標歩容上重心位置算出部４７には、目標脚位置姿勢及び目標基***置姿勢に加えて、当該他のリンクの各関節の変位量を規定する目標値（例えば、当該他のリンクの先端部の目標位置及び目標姿勢、あるいは、当該他のリンクの先端部の各関節の変位量の目標値等）も与えられる。

そして、ロボット１の全体重心の位置としての目標歩容上重心位置↑Ｘg_cmd2は、当該他のリンクに対応する質点を含めた幾何学モデルに基づく幾何学演算によって、算出するようにすればよい。

ただし、上記他のリンクの質量が基体２の質量に比して十分に微小である場合には、当該他のリンクの質量を無視して構成した幾何学モデル、あるいは、当該他のリンクの質量を基体２の質量に含めて構成した幾何学モデルを用いて目標歩容上重心位置↑Ｘg_cmd2を決定するようにしてもよい。

また、例えば、基体２の上側基体１１から腕リンクが延設されている場合において、移動ロボット１の歩行動作又は走行動作における上側基体１１に対する腕リンクの姿勢を一定もしくはほぼ一定に保持するような場合には、該腕リンクが実質的に上側基体１１と一体の部分となる。このため、この場合には、当該他のリンクの質量を上側基体１１の質量に含めて構成した幾何学モデルを用いて目標歩容上重心位置↑Ｘg_cmd2を決定するようにすればよい。

基本目標並進運動量修正部４３には、基本動作目標値決定部４２から基本目標重心位置↑Ｘg_cmd1と基本目標並進運動量ベクトル↑Ｐ_cmd1とが逐次与えられると共に、目標歩容上重心位置算出部４７から目標歩容上重心位置↑Ｘg_cmd2が逐次与えられる。

なお、以降の説明では、歩容生成部３２の処理で決定される↑Ｘg_cmd1、↑Ｐ_cmd1、↑Ｘg_cmd2等の各変数の値に関し、歩容生成部３２の現在の（今回の）演算処理周期で決定される値と、前回の演算処理周期で決定された値とを区別する必要がある場合に、前者の値を今回値、後者の値を前回値ということがある。

基本目標並進運動量修正部４３は、まず、与えられた基本目標重心位置↑Ｘg_cmd1と、目標歩容上重心位置↑Ｘg_cmd2との偏差（＝↑Ｘg_cmd1−↑Ｘg_cmd2）を演算部４３ａで算出する。

この場合、基本目標重心位置↑Ｘg_cmd1の今回値に対応する目標歩容の今回値は未だ決定されていない。そして、本実施形態では、目標歩容上重心位置↑Ｘg_cmd2の今回値は、目標歩容の前回値に対応するロボット１の全体重心の位置である。このため、演算部４３ａの処理では、基本目標重心位置↑Ｘg_cmd1の前回値と、目標歩容上重心位置↑Ｘg_cmd2の今回値と用いて上記偏差（以降、重心位置偏差という）が算出される。

さらに、基本目標並進運動量修正部４３は、演算部４３ａで算出した重心位置偏差偏差（＝↑Ｘg_cmd1−↑Ｘg_cmd2）にあらかじめ定めた所定値のゲインＫｇを乗じる処理を演算部４３ｂで実行する。これにより、上記偏差をゼロに近づけるように基本目標並進運動量ベクトル↑Ｐ_cmd1を補正するための操作量（フィードバック操作量）が算出される。

次いで、基本目標並進運動量修正部４３は、基本目標並進運動量ベクトル↑Ｐ_cmd1の今回値を、演算部４３ｂで算出した操作量によって補正する処理を演算部４３ｃで実行する。この補正処理は、↑Ｐ_cmd1の今回値に演算部４３ｂで算出した操作量を加算することにより行なわれる。これにより、演算部４３ｃの出力として、修正後基本並進運動量ベクトル↑Ｐ_cmd2（今回値）が算出される。

従って、各演算処理周期において、修正後基本並進運動量ベクトル↑Ｐ_cmd2（今回値）は、次式（１）により基本目標並進運動量ベクトル↑Ｐ_cmd1の今回値を修正することで算出される。

↑Ｐ_cmd2＝↑Ｐ_cmd1＋Ｋｇ・（↑Ｘg_cmd1−↑Ｘg_cmd2） ……（１）

すなわち、↑Ｐ_cmd2は、重心位置偏差（↑Ｘg_cmd1−↑Ｘg_cmd2）をフィードバック制御則としての比例則によりゼロに近づけるように決定したフィードバック操作量によって↑Ｐ_cmd1を補正することにより決定される。

なお、上記ゲインＫｇはスカラー及び対角行列（ゲイン行列）のいずれであってもよい。ゲインＫｇを対角行列とした場合には、重心位置偏差の各成分（Ｘ軸、Ｙ軸、Ｚ軸の各座標軸成分）毎にフィードバックゲインを設定することが可能である。

基本目標基体運動速度決定部４４には、基本動作目標値決定部４２から基本目標基***置姿勢が逐次与えられると共に、目標基***置姿勢決定部４６（詳細は後述する）から目標基***置姿勢が逐次与えられる。

そして、基本目標基体運動速度決定部４４は、まず、与えられた基本目標基***置姿勢と、目標基***置姿勢との偏差（＝基本目標基***置姿勢−目標基***置姿勢）を演算部４４ａで算出する。

この場合、基本目標基***置姿勢の今回値に対応する目標基***置姿勢は未だ決定されていない。このため、演算部４４ａの処理では、基本目標基***置姿勢の前回値と、目標基***置姿勢の前回値と用いて上記偏差（以降、基***置姿勢偏差という）が算出される。

さらに、基本目標基体運動速度決定部４４は、演算部４４ａで算出した基***置姿勢偏差にあらかじめ定めた所定値のゲインＫｂを乗じる処理を演算部４４ｂで実行する。これにより、上記基***置姿勢偏差をゼロに近づけて、目標基***置姿勢が基本目標基***置姿勢から乖離しないようにするための操作量（フィードバック操作量）として、基体２の位置及び姿勢の単位時間当たりの変化量の目標値により構成される基本目標基体運動速度ベクトル↑Ｖb_cmd1が算出される。

従って、各演算処理周期において、基本目標基体運動速度ベクトル↑Ｖb_cmd1は、次式（２）により算出される。

↑Ｖb_cmd1＝Ｋｂ・（基本目標基***置姿勢−目標基***置姿勢） ……（２）

すなわち、↑Ｖb_cmd1は、基***置姿勢偏差（＝基本目標基***置姿勢−目標基***置姿勢）をフィードバック制御則としての比例則によりゼロに近づけるように決定したフィードバック操作量として決定される。

なお、上記ゲインＫｂはスカラー及び対角行列（ゲイン行列）のいずれであってもよい。ゲインＫｂを対角行列とした場合には、基***置姿勢偏差の各成分（基体２の各座標軸方向の位置の偏差、基体２の各座標軸周りの姿勢の偏差（角度偏差）、及び基体２の関節１２の変位量の偏差（角度偏差））毎にフィードバックゲインを設定することが可能である。

以上のように目標歩容上重心位置算出部４７、基本目標並進運動量修正部４３及び基本目標基体運動速度決定部４４の処理を実行した後、歩容生成部３２は、次に基体運動速度目標値決定部４５の処理を実行する。

この基体運動速度目標値決定部４５は、基体２の位置及び姿勢の単位時間当たりの変化量を成分として構成される基体運動速度ベクトル↑Ｖｂから、ロボット１の全体重心の並進運動量ベクトル↑Ｐの各成分と、ロボット１の全体重心の周りの角運動量ベクトル↑Ｌの各成分と、基体運動速度ベクトル↑Ｖｂの各成分とにより構成されるベクトルである制御対象ベクトル↑Ｓへの線形写像を表すヤコビアン行列を用いて基体運動速度ベクトル↑Ｖｂの目標値である目標基体運動速度ベクトル↑Ｖb_cmd2を決定する。

上記制御対象ベクトル↑Ｓは、↑Ｐ、↑Ｌ、↑Ｖｂの各成分を並べた縦ベクトルである。より具体的には、↑Ｐ、↑Ｌ、↑Ｖｂをそれぞれの成分を用いて表記したものを、↑Ｐ≡[Ｐx，Ｐy，Ｐz]^T、↑Ｌ≡[Ｌx，Ｌy，Ｌz]^T、↑Ｖｂ≡[Ｖbx，Ｖby，Ｖbz，ωbx，ωby，ωbz，ωbt]^Tとおくと（上付き添え字Ｔは転置を表す）、↑Ｓは、例えば次式（３）により表記される縦ベクトルである。

↑Ｓ≡[↑Ｐ，↑Ｌ，↑Ｖｂ]^T
≡[Ｐx，Ｐy，Ｐz，Ｌx，Ｌy，Ｌz，Ｖbx，Ｖby，Ｖbz，ωbx，ωby，ωbz，ωbt]^T
……（３）

なお、Ｐx，Ｐy，Ｐzは、それぞれ↑ＰのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分であり、Ｌx，Ｌy，Ｌzは、それぞれ↑ＬのＸ軸周り成分、Ｙ軸周り成分、Ｚ軸周り成分である。また、Ｖbx、Ｖby、Ｖbzは、それぞれ、基体２の代表点の移動速度のうちのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分、ωbx、ωby、ωbzは、それぞれ、基体２の姿勢の時間的変化率のうちのＸ軸周りの角速度成分、Ｙ軸周りの角速度成分、Ｚ軸周りの角速度成分、ωbtは、基体２の関節１２の変位量の時間的変化率（角速度）である。

また、↑Ｖｂから↑Ｓへの線形写像を表すヤコビアン行列をＡとおくと、その線形写像は、次式（４）により表される。

↑Ｓ＝Ａ・↑Ｖｂ ……（４）

この場合、ヤコビアン行列Ａは、次式（５）で示すように、↑Ｖｂから↑Ｐ、↑Ｌのそれぞれへの線形写像を表す慣性行列Ｍ，Ｈと、↑Ｖｂから↑Ｖｂへの線形写像を表す単位行列Ｅ（↑Ｖｂの成分数と同次数の単位行列）とを縦に並べた行列となる。なお、本実施形態では、↑Ｖｂは７成分のベクトルであるので、Ｍ，Ｈはそれぞれ３行７列の慣性行列、Ｅは７行７列の単位行列である。従って、本実施形態では、ヤコビアン行列Ａは、１３行７列の行列である。

かかるヤコビアン行列Ａを用いる基体運動速度目標値決定部４５の処理の全体は、次のように行なわれる。

すなわち、基体運動速度目標値決定部４５は、各演算処理周期において、まず、式（４）の線形写像を表すヤコビアン行列Ａを決定する。さらに、基体運動速度目標値決定部４５は、算出したヤコビアン行列Ａの擬似逆行列Ａ⁺を決定する。

そして、基体運動速度目標値決定部４５は、基本目標並進運動量修正部４３により決定された修正後基本並進運動量ベクトル↑Ｐ_cmd2（今回値）と、基本動作目標値決定部４２により決定された基本目標角運動量ベクトル↑Ｌ_cmd1（今回値）と、基本目標基体運動速度決定部４４により決定された基本目標基体運動速度ベクトル↑Ｖｂ_cmd1（今回値）とをそれぞれ、前記式（３）の↑Ｐ、↑Ｌ、↑Ｖｂの目標値とすることで構成される制御対象ベクトル↑Ｓの目標値↑Ｓ_cmd（以下、目標制御対象ベクトル↑Ｓ_cmdという）と、上記擬似逆行列Ａ⁺とから次式（６）の右辺の演算（↑Ｓ_cmdにＡ⁺を乗じる演算処理）を行なうことで、↑Ｓ_cmdに対応する↑Ｖｂの目標値としての目標基体運動速度ベクトル↑Ｖb_cmd2を決定する。

↑Ｖb_cmd2＝Ａ⁺・↑Ｓ_cmd ……（６）

このように目標基体運動速度ベクトル↑Ｖb_cmd2を決定する処理において、基体運動速度目標値決定部４５は、本実施形態ではヤコビアン行列Ａを次のように決定する。

まず、本実施形態におけるヤコビアン行列Ａの決定手法に関する前提事項を説明しておく。前記式（４）の線形写像を表すヤコビアン行列Ａは、その構成要素である慣性行列Ｍ、Ｈと単位行列ＥとのうちのＭ、Ｈを変数とするヤコビアン行列である。

かかるヤコビアン行列Ａは、基本的には、ロボット１の現在の目標歩容（前回の演算処理周期で決定した目標歩容）により規定されるロボット１の動作状態を起点として、当該起点の動作状態から基体２の位置及び姿勢の各成分を微小変化させた場合にそれに起因して発生するロボット１の全体重心の並進運動量ベクトル↑Ｐ及び該全体重心周りの角運動量ベクトル↑Ｌの発生感度を表す行列として算出すればよい。

この場合、上記起点の動作状態から基体２の位置及び姿勢の各成分を微小変化させた場合にそれに起因して発生するロボット１の全体重心の並進運動量ベクトル↑Ｐ及び該全体重心周りの角運動量ベクトル↑Ｌは、例えばロボット１の各リンク毎にその重心位置の質点と該重心位置周りのイナーシャ（慣性モーメント）とを設定した動力学モデル（以降、このような動力学モデルをフルモデルという）を用いて算出することができる。その算出手法は、公知の手法を採用することができる。

ただし、本実施形態では、目標制御対象ベクトル↑Ｓ_cmdに応じて決定する目標基体運動速度ベクトル↑Ｖb_cmd2の信頼性をより一層高めるために、現在の時刻（今回の演算処理周期の時刻）から次回の演算処理周期の時刻との間の期間内（１演算処理周期の時間内）での将来の目標歩容（目標脚位置姿勢及び目標基***置姿勢）の瞬時値を推定し、その推定した将来の目標歩容の瞬時値により規定されるロボット１の動作状態を起点としてヤコビアン行列Ａを決定することとした。

換言すれば、各演算処理周期において、ヤコビアン行列Ａを算出するための起点とするロボット１の動作状態を、現在の目標歩容により規定される動作状態から、将来の予想される動作状態寄りにずらした上で、ヤコビアン行列Ａを算出する。

これは次の理由による。すなわち、基体２の位置及び姿勢の微小変化と、それに起因して発生する↑Ｐ及び↑Ｌとの間の関係は一般には非線形性を有するため、ロボット１の現在の目標歩容の状態を起点として決定したヤコビアン行列Ａを用いた場合には、上記式（６）により決定される↑Ｖb_cmd2が、↑Ｓ_cmdを実現するための最適な値からずれを生じやすい。

例えば、脚リンク３Ｒ，３Ｌの足平部１８Ｒ，１８Ｌの位置及び姿勢を一定に保持した場合における基体２の位置Ｘｂと、ロボット１の全体重心の位置Ｘｇにロボット１の全体質量Ｍallを乗算したもの（＝Ｍall・Ｘｇ）との間の関係に着目すると、該関係は、一般には、図６に実線の曲線ａ１で例示する如く、非線形な関係となる。

なお、ここでは、Ｘｂ、Ｘｇは、理解の便宜上、グローバル座標系のＸ軸方向又はＹ軸方向又はＺ軸方向のいずれかの一軸方向の位置、例えばＸ軸方向の位置とする。この場合、Ｍall・Ｘｇは、ロボット１の全体重心のＸ軸方向の並進運動量の積分値に相当する状態量である。

そして、現在の目標歩容における基体２の現在の位置が、例えば図６中に示すＸb(k)であるとした場合、現在の目標歩容の状態を起点として決定されるヤコビアン行列Ａ（以降、これを基本ヤコビアン行列Ａ１という）では、Ｘｂの微小変化と、これに対応するＭall・Ｘｇの変化との間の関係は、Ｘb(k)の位置での曲線ａ１の接線ａ２により示される線形関係として表されることとなる。

従って、目標制御対象ベクトル↑Ｓ_cmdに対応する目標基体運動速度ベクトル↑Ｖb_cmd2を、上記基本ヤコビアン行列Ａ１の擬似逆行列を用いて前記式（６）により決定した場合には、歩容生成部３２の演算処理の１周期（１演算処理周期）Δｔに時間における基体２の位置Ｘｂの目標とする変化量は、図６に示すΔＸb_aとなる。

このΔＸb_aは、上記の如く基本ヤコビアン行列Ａ１の擬似逆行列を用いて決定した↑Ｖb_cmd2のうちの、位置Ｘｂの時間的変化率に相当する成分Ｖbxの値にΔｔを乗じてなる値である。換言すれば、ΔＸb_aは、基体２の位置Ｘｂと、Ｍall・Ｘｇとの間の関係が、上記接線ａ２で表される関係であると仮定した場合に、ΔＸb_aに対応するＭall・Ｘｇの変化量が、目標制御対象ベクトル↑Ｓ_cmdの構成要素である↑Ｐ_cmd2のうちのＸ軸方向の並進運動量の目標値Ｐx_cmd2にΔｔを乗じてなる値（＝Ｐx_cmd2・Δｔ）となるようなＸｂの変化量である。

ただし、基体２の位置Ｘｂと、Ｍall・Ｘｇとの間の実際の関係が、曲線ａ１で表される関係である場合には、ΔＸb_aに対応するＭall・Ｘｇの実際の変化量は、図６中のＭall・ΔＸg_aとなる。この値Ｍall・ΔＸg_aは、一般には、目標制御対象ベクトル↑Ｓ_cmdにより規定されるＭall・Ｘｇの変化量の目標値としての上記の値Ｐx_cmd2・Δｔに対して誤差を生じるものとなる。

そして、基体２の位置Ｘｂと、Ｍall・Ｘｇとの間の実際の関係が、曲線ａ１で表される関係である場合には、Ｍall・Ｘｇの変化量が、目標制御対象ベクトル↑Ｓ_cmdにより規定されるＭall・Ｘｇの変化量の目標値Ｐx_cmd2・Δｔになるようにするための、基体２の位置Ｘｂの適切な変化量は、図６に示すΔＸb_b（≠ΔＸb_a）である。

一方、現在の目標歩容における基体２の位置Ｘb(k)と、この位置Ｘb(k)から上記ΔＸb_bだけ変化させた位置Ｘb(k)＋ΔＸb_bとの間の中間の位置Ｘb(k)＋ΔＸb_c（ただし、０＜ΔＸb_c＜ΔＸb_b）を想定した場合、微分積分学における平均値の定理に基づいて、当該中間の位置Ｘb(k)＋ΔＸb_cでの曲線ａ１の接線ａ３の傾きを、位置Ｘb(k)での曲線ａ１上の点と、位置Ｘb(k)＋ΔＸb_bでの曲線ａ１上の点とを結ぶ直線ａ４の傾きに一致もしくは近似させることができる。

従って、基体２の位置Ｘｂが当該中間の位置Ｘb(k)＋ΔＸb_cとなるような将来の目標歩容を起点として、ヤコビアン行列Ａを決定し、このヤコビアン行列Ａの擬似逆行列Ａ⁺を用いて↑Ｖb_cmd2を決定するようにすれば、この↑Ｖb_cmd2により規定される基体２の位置Ｘｂの変化量を、上記の適切な変化量ΔＸb_bに一致させ、もしくは、該変化量ΔＸb_bに対して近似精度の高い値にすることができる。すなわち、↑Ｖb_cmd2の信頼性を高めることができる。

かかる事項は、↑Ｓ_cmdのうちの↑Ｐ_cmd2に影響を及ぼす↑Ｖb_cmd2の成分に限らず、↑Ｌ_cmd1に影響を及ぼす↑Ｖb_cmd2の成分についても同様に言える事項である。

そこで、本実施形態では、上記の如く、現在の時刻（今回の演算処理周期の時刻）から次回の演算処理周期の時刻との間の期間内（１演算処理周期の時間内）での将来の目標歩容（目標脚位置姿勢及び目標基***置姿勢）の瞬時値を推定し、その推定した将来の目標歩容の瞬時値により規定されるロボット１の動作状態を起点として、ヤコビアン行列Ａを決定することとした。

この場合、本実施形態では、ヤコビアン行列Ａを決定するための起点とする動作状態を規定する将来の目標歩容（以降、これをヤコビアン行列決定用目標歩容という）の構成要素である目標脚位置姿勢及び目標基***置姿勢（以降、これらをヤコビアン行列決定用目標脚位置姿勢、ヤコビアン行列決定用目標基***置姿勢という）の瞬時値は、次のように決定される。

すなわち、基体運動速度目標値決定部４５は、次式（７ａ）で示す如く、現在の目標基***置姿勢（前回の演算処理周期で決定された目標基***置姿勢。以下、これを目標基***置姿勢(k)と表記する）と、１演算処理周期前の目標基***置姿勢（前々回の演算処理周期で決定された目標基***置姿勢。以下、これを目標基***置姿勢(k-1)と表記する）との偏差、すなわち、前回の演算処理周期で決定された目標基***置姿勢の変化量（＝目標基***置姿勢(k)−目標基***置姿勢(k-1)）に所定値の係数Ｋｐを乗じてなる補正項を、目標基***置姿勢(k)に加え合わせることによって、ヤコビンアン行列決定用目標基***置姿勢を決定する。

同様に、基体運動速度目標値決定部４５は、次式（７ｂ）で示す如く、各脚リンク３毎の現在の目標脚位置姿勢（前回の演算処理周期で決定された目標脚位置姿勢。以下、これを目標脚位置姿勢(k)と表記する）と、１演算処理周期前の目標脚位置姿勢（前々回の演算処理周期で決定された目標脚位置姿勢。以下、これを目標脚位置姿勢(k-1)と表記する）との偏差、すなわち、前回の演算処理周期で決定された目標脚位置姿勢の変化量（＝目標脚位置姿勢(k)−目標脚位置姿勢(k-1)）に上記係数Ｋｐを乗じてなる補正項を、目標脚位置姿勢(k)に加え合わせることによって、ヤコビンアン行列決定用目標基***置姿勢を決定する。

なお、上記係数Ｋｐは、０≦Ｋｐ≦１の範囲であらかじめ決定された値である。本実施形態では、例えばＫｐ＝０．５である。

ヤコビンアン行列決定用目標基***置姿勢
＝目標基***置姿勢(k)＋Ｋｐ・（目標基***置姿勢(k)−目標基***置姿勢(k-1)）
……（７ａ）

ヤコビンアン行列決定用目標脚位置姿勢
＝目標脚位置姿勢(k)＋Ｋｐ・（目標脚位置姿勢(k)−目標脚位置姿勢(k-1)）
……（７ｂ）

従って、本実施形態では、ヤコビアン行列決定用目標歩容は、歩容生成部３２の演算処理周期毎の目標歩容の変化量の時系列のうちの最新値に応じて、現在の目標歩容を補正してなる歩容として決定される。

なお、ロボット１が腕リンク等、基体２に対して可動な他のリンクを備える場合には、当該他のリンクの先端部の将来の目標位置及び目標姿勢、あるいは、当該他のリンクの各関節の将来の目標変位量を、ヤコビンアン行列決定用目標基***置姿勢やヤコビアン行列決定用目標脚位置姿勢と同じ手法で求めるようにすればよい。

また、ヤコビアン行列決定用目標歩容は、現在の目標歩容と、１演算処理周期前の目標歩容とを含めて、３個以上の演算処理周期分の目標歩容から決定するようにしてもよい。

そして、基体運動速度目標値決定部４５は、上記の如く決定したヤコビアン行列決定用目標歩容の状態を起点として、当該起点の状態から基体２の位置及び姿勢の各成分を微小変化させた場合にそれに起因して発生するロボット１の全体重心の並進運動量ベクトル↑Ｐ及び該全体重心周りの角運動量ベクトル↑Ｌの発生感度を表す行列としてのヤコビアン行列Ａを前記フルモデルを用いて算出する。この算出の仕方は、公知の手法と同様でよい。

なお、ヤコビアン行列Ａを算出するために用いる動力学モデルは、前記フルモデルと異なる動力学モデル（例えば一部のリンクのイナーシャを無視した動力学モデル等）であってもよいが、できるだけ動力学的精度の高い動力学モデル（前記基本動作目標値決定部４２の処理で用いる動力学モデルよりも動力学的精度の高い動力学モデル）を使用することが望ましい。ここで、動力学的精度が高いというのは、ロボット１の運動によって動力学モデル上で発生するトータルの慣性力の、実際のロボット１で発生するトータルの慣性力に対する近似精度が高いことを意味する。

次に、上記の如く決定したヤコビアン行列Ａの擬似逆行列Ａ⁺は、本実施形態では、次のように決定される。

本実施形態では、所謂、特異点低感度運動分解行列（SR-Inverse）を擬似逆行列Ａ⁺として決定する。この場合、ヤコビアン行列Ａの擬似逆行列Ａ⁺は、Ｗ、Ｖを任意の重み行列（対角行列）として、次式（８ａ）又は（８ｂ）により算出することができる。

Ａ⁺＝Ｗ^-1・Ａ^T・(Ａ・Ｗ^-1・Ａ^T＋Ｖ)^-1 ……（８ａ）
Ａ⁺＝(Ａ^T・Ｖ^-1・Ａ＋Ｗ)^-1・Ａ^T・Ｖ^-1 ……（８ｂ）

これらの式（８ａ）又は（８ｂ）によりに算出される擬似逆行列Ａ⁺は、次式（９）に与えられる評価関数ｆの値が最小となるような目標基体運動速度ベクトル↑Ｖb_cmd2を、前記式（６）により算出することができる擬似逆行列である。

ｆ＝(↑Ｖb_cmd2)^T・Ｗ・↑Ｖb_cmd2
＋(↑Ｓ_cmd - Ａ・↑Ｖb_cmd2)^T・Ｖ・(↑Ｓ_cmd - Ａ・↑Ｖb_cmd2)
……（９）

なお、評価関数ｆの値が最小になるということは、↑Ｖb_cmd2の各成分の二乗値を、Ｗの各対角成分を重み係数として線形結合してなる値と、↑Ｓ_cmd及びＡ・↑Ｖb_cmd2の差分のベクトルの各成分の二乗値を、Ｖの各対角成分を重み係数として線形結合してなる値との総和が最小になることを意味する。

前記重み行列Ｗは、目標基体運動速度ベクトル↑Ｖb_cmd2の成分数と同じ次数（本実施形態では、７次数）の対角行列である。この重み行列Ｗの各対角成分の値（重み係数）は、例えば↑Ｖb_cmd2の各成分の値の許容範囲等を考慮して設定すればよい。

また、重み行列Ｖは、目標制御対象ベクトル↑Ｓ_cmdと同じ次数（本実施形態では、１３次数）の対角行列である。この重み行列Ｖの各対角成分の値（重み係数）は、例えばＡ・↑Ｖb_cmd2により算出される制御対象ベクトル↑Ｓの各成分の値の、目標制御対象ベクトル↑Ｓ_cmdにより規定される目標値に対する誤差の許容度合いに応じて設定すればよい。

なお、重み行列Ｗ，Ｖは、それぞれ単位行列であってもよい。また、式（８ａ）におけるＶ、式（８ｂ）におけるＷは、それぞれ零行列であってもよい。

ヤコビアン行列Ａの擬似逆行列Ａ⁺は、前記式（８ａ），（８ｂ）のいずれの演算式により算出してもよいが、本実施形態では、基体運動速度目標値決定部４５は、例えば前記式（８ｂ）を用いて擬似逆行列Ａ⁺を算出する。この場合、本実施形態では、前記基本目標基体運動速度ベクトル↑Ｖb_cmd1を目標基体運動速度ベクトル↑Ｖb_cmd2の基準目標とすることから、重み行列Ｗを例えば零行列に設定した。

また、重み行列Ｖに関しては、例えば、目標制御対象ベクトル↑Ｓ_cmdのうちの並進運動量ベクトル↑Ｐの目標値（＝↑Ｐ_cmd2）と角運動量ベクトル↑Ｌの目標値（＝↑Ｌ_cmd1）とに対応する対角成分（６個の対角成分）と、↑Ｓ_cmdのうちの基体運動速度ベクトル↑Ｖｂの目標値（＝↑Ｖb_cmd）のうち、関節１２の変位量の目標値を除く成分（すなわち下側基体１０の位置及び姿勢の目標値）に対応する対角成分（６個の対角成分）と、関節１２の変位量の目標値に対応する対角成分（１個の対角成分）との３種類の対角成分に分類して、各種類の対角成分の値を設定した。

一例として、重み行列Ｖの対角成分のうち、並進運動量ベクトル↑Ｐの目標値と角運動量ベクトル↑Ｌの目標値とに対応する６個の対角成分の値を１．０、下側基体１０の位置及び姿勢の目標値に対応する６個の対角成分の値を０．５、基体２の関節１２の変位量の目標値に対応する１個の対角成分の値を０．１というように、重み行列Ｖを設定した。

本実施形態では、基体運動速度目標値決定部４５は、各演算処理周期において、以上の如くあらかじめ設定された重み行列Ｗ，Ｖを用いて前記式（８ｂ）によりヤコビアン行列Ａの擬似逆行列Ａ⁺を決定する。

そして、この擬似逆行列Ａ⁺と、前記目標制御対象ベクトル↑Ｓ_cmd（今回値）とから前記式（６）により目標基体運動速度ベクトル↑Ｖb_cmd2（今回値）を決定する。

この場合、本実施形態では重み行列Ｗを零行列としたので、式（９）の右辺の第２項を最小化するように、すなわち、↑Ｓ_cmd及びＡ・↑Ｖb_cmd2の差分のベクトルの各成分の二乗値を、重み行列Ｖの各対角成分を重み係数として線形結合してなる値を最小化するように、↑Ｖb_cmd2が決定されることとなる。

また、この場合、↑Ｓ_cmdは、修正後基本並進運動量ベクトル↑Ｐ_cmd2及び基本目標角運動量ベクトル↑Ｌ_cmd1に加えて、前記基***置偏差をゼロに近づけるための操作量たる基本目標基体運動速度ベクトル↑Ｖb_cmd1を含んでいるため、↑Ｖb_cmd2は、↑Ｖb_cmd1から乖離しないようにしつつ、前記式（６）の右辺により算出される制御対象ベクトル（＝Ａ・↑Ｖb_cmd2）のうちの並進運動量ベクトル↑Ｐと、角運動量ベクトル↑Ｌとがそれぞれ極力、修正後目標並進運動量ベクトル↑Ｐ_cmd2、目標角運動量ベクトル↑Ｌ_cmd1に一致するように決定されることとなる。

以上が本実施形態における基体運動速度目標値決定部４５の処理の詳細である。

歩容生成部３２は、以上の如く基体運動速度目標値決定部４５の処理を実行した後、次に、目標基***置姿勢決定部４６の処理を実行する。

この目標基***置姿勢決定部４６は、基体運動速度目標値決定部４５で決定された目標基体運動速度ベクトル↑Ｖb_cmd2を逐次積分することによって、目標基***置姿勢を決定する。

具体的には、目標基***置姿勢決定部４６は、各演算処理周期において、目標基体運動速度ベクトル↑Ｖb_cmd2の今回値に歩容生成部３２の演算処理周期の時間Δｔを乗じてなる値（＝↑Ｖb_cmd2・Δｔ）を、現在の目標基***置姿勢の値（前回の演算処理周期において決定した値）に加えることにより、目標基***置姿勢の今回値を決定する。

本実施形態では、歩容生成部３２の各演算処理周期において、以上の如く目標基***置姿勢決定部４６により決定される目標基***置姿勢（今回値）と、前記目標脚位置姿勢決定部４１により決定される各脚リンク３の目標脚位置姿勢（今回値）とが、目標歩容（今回値）の構成要素とされる。

そして、本実施形態では、歩容生成部３２は、さらに、目標関節変位量決定部４８の処理を実行する。この目標関節変位量決定部４８には、各演算処理周期において、決定された目標歩容の構成要素（目標基***置姿勢及び目標脚位置姿勢）が与えられる。

そして、目標関節変位量決定部４８は、与えられた目標歩容により規定されるロボット１の各関節の目標関節変位量を、ロボット１の幾何学モデル（剛体リンクモデル）に基づく逆運動学の演算処理によって算出する。この場合、ロボット１の幾何学モデルは、前記目標歩容上重心位置算出部４７で用いるものと同じである。

なお、前記目標歩容上重心位置算出部４７では、目標歩容（目標基***置姿勢及び目標脚位置姿勢）の代わりに、目標関節変位量決定部４８で決定された各関節の目標関節変位量（前回値）を用いて、目標歩容上重心位置↑Ｘg_cmd2を決定するようにしてもよい。このようにした場合には、目標歩容上重心位置算出部４７で、目標歩容により規定される各関節の変位量を算出する処理は不要である。

補足すると、ロボット１が腕リンク等、基体２に対して可動な他のリンクを備える場合には、ロボット１の目標歩容のうちの当該他のリンクに関する構成要素は、例えば基本動作目標値決定部４２で適宜、決定しておくようにすればよい。一例として、ロボット１が上側基体１１から延設された腕リンクを備える場合には、該ロボット１の歩行動作又は走行動作を行なう目標歩容の生成処理では、該目標歩容における上側基体１１に対する該腕リンクの全体の姿勢（ひいては、該腕リンクの各関節の変位量）を、一定の姿勢としたり、あるいは、基体２の関節１２の回転軸周りに周期的に腕リンクの全体を動かすような該腕リンクの目標歩容を採用すればよい。

以上が、本実施形態における歩容生成部３２の処理の詳細である。

ここで、以上説明した実施形態と本願発明との対応関係について補足しておく。本実施形態では、制御処理ユニット３１は、歩容生成部３２を備えることによって、本発明における歩容生成装置としての機能を有する。

そして、歩容生成部３２の目標脚位置姿勢決定部４１、基本動作目標値決定部４２、基本目標並進運動量修正部４３、基本目標基体運動速度決定部４４、基体運動速度目標値決定部４５、目標基***置姿勢決定部４６、目標歩容上重心位置算出部４７の処理によってそれぞれ、本発明における目標脚位置姿勢決定手段、基本動作目標値決定手段、基本目標並進運動量修正手段、基本基体運動速度決定手段、基体運動速度目標値決定手段、目標基***置姿勢決定手段、目標歩容上重心位置算出手段が実現される。

以上説明した本実施形態によれば、前記基体運動速度目標値決定部４５は、修正後基本並進運動量ベクトル↑Ｐ_cmd2、基本目標角運動量ベクトル↑Ｌ_cmd1及び基本目標基体運動速度ベクトル↑Ｖb_cmd1により構成される目標制御対象ベクトル↑Ｓ_cmdを目標として、この↑Ｓ_cmdにヤコビアン行列Ａの擬似逆行列Ａ⁺に乗じることにより目標基体運動速度ベクトル↑Ｖb_cmd2が逐次決定される。そして、この↑Ｖb_cmd2を積分することにより、目標歩容における目標基***置姿勢が決定される。

この場合、目標制御対象ベクトル↑Ｓ_cmdの構成要素のうちの並進運動量ベクトル↑Ｐの目標値として設定される修正後基本並進運動量ベクトル↑Ｐ_cmd2は、基本動作目標値決定部４２で決定した基本目標並進運動量ベクトル↑Ｐ_cmd1を基準の目標としつつ、目標歩容上重心位置↑Ｘg_cmd2が、基本動作目標値決定部４２で決定した基本目標重心位置↑Ｘg_cmd1からずれるのを抑制するためのフィードバック操作量により該基本目標並進運動量ベクトル↑Ｐ_cmd1を修正することにより決定される。

また、目標制御対象ベクトル↑Ｓ_cmdの構成要素のうちの角運動量ベクトル↑Ｌの目標値として、基本動作目標値決定部４２で決定された基本角並進運動量ベクトル↑Ｐ_cmd2が設定される。

また、目標制御対象ベクトル↑Ｓ_cmdの構成要素のうちの基体運動速度ベクトル↑Ｖｂの目標値して設定される基本目標基体運動速度ベクトル↑Ｖb_cmd1は、目標歩容における目標基***置姿勢が、基本動作目標値決定部４２で決定した基本目標基***置姿勢からずれるのを抑制するためのフィードバック操作量として決定される。

従って、基体運動速度目標値決定部４５で逐次決定される目標基体運動速度ベクトル↑Ｖb_cmd2は、基本動作目標値決定部４２の処理における動力学モデル上で満たすようにした動力学を極力崩さないようにしつつ、目標基体運動速度ベクトル↑Ｖb_cmd2を積分することにより決定される目標基***置姿勢が、基本動作目標値決定部４２で決定した基本目標基***置姿勢から乖離することがないように、決定されることとなる。

このため、本実施形態によれば、動力学的な要件を満たしつつ、ロボット１が姿勢を崩すことが無いような目標歩容を生成できる。

また、基体運動速度目標値決定部４５の処理では、ロボット１の基体２の位置及び姿勢と、ロボット１の全体重心の並進運動量ベクトルの積分値に相当する状態量や全体重心周りの角運動量ベクトルの積分値に相当する状態量との間の非線形性を考慮して、将来の目標歩容の推定値としての前記ヤコビアン行列決定用目標歩容を起点として、ヤコビアン行列Ａを決定するようにしたので、目標制御対象ベクトル↑Ｓ_cmdを実現するための基体運動速度ベクトル↑Ｖｂの値として、このヤコビアン行列Ａの擬似逆行列Ａ⁺を目標制御対象ベクトル↑Ｓ_cmdに乗じることによって決定される目標基体運動速度ベクトル↑Ｖb_cmd2の信頼性を高めることができる。

換言すれば、↑Ｖb_cmd2に、決定したヤコビアン行列Ａを乗じることによって得られる制御対象ベクトル↑Ｓの値の、目標制御対象ベクトル↑Ｓ_cmdに対する一致度合いを高めることができる。

加えて、本実施形態では、ヤコビアン行列Ａの擬似逆行列Ａ⁺を、特異点低感度運動分解行列（SR-Inverse）としたので、を該ヤコビアン行列の擬似逆行列として用いて前記基体運動速度ベクトルの目標値を決定する↑Ｓ_cmdとＡ・↑Ｖb_cmd2との差分のベクトルの各成分の二乗値を、対角行列Ｖの各対角成分である重み係数により線形結合してなる値を最小化することができる。

従って、目標制御対象ベクトル↑Ｓ_cmdを実現する上で適切な目標歩容を逐次決定することができる。

ここで、本実施形態の上記の効果に関する検証結果について図７〜図９を参照して以下に説明する。

図７（ａ）のグラフａ１は、本実施形態の実施例における目標基***置姿勢のうちの、下側基体１０のＹ軸周り（ピッチ軸周り）の傾斜角の経時変化を示している。この場合、ロボット１の目標歩容は、例えば４００ｍｍの最大歩幅で歩行を行なう歩容である。また、この実施例における基本目標基***置姿勢のうちの、下側基体１０のＹ軸周り（ピッチ軸周り）の姿勢は０degである。

また、図７（ｂ）のグラフａ２は、比較例における目標基***置姿勢のうちの、下側基体１０のＹ軸周り（ピッチ軸周り）の傾斜角の経時変化を示している。

この場合、比較例におけるロボット１の目標歩容は、図７（ａ）の実施例と同様に、４００ｍｍの最大歩幅で歩行を行なう歩容である。また、この比較例における基本目標基***置姿勢のうちの、下側基体１０のＹ軸周り（ピッチ軸周り）の姿勢は、図７（ａ）の実施例と同様に０degである。

ただし、この比較例においては、目標制御対象ベクトルから基本基体運動速度ベクトル↑Ｖb_cmd1を除外し、修正後基本目標並進運動量ベクトル↑Ｐ_cmd2と、基本目標角運動量ベクトル↑Ｌ_cmd1とにより目標制御対象ベクトルを構成するものとした。

図７（ｂ）を参照して判るように、基体運動速度ベクトル↑Ｖb_cmd1を目標制御対象ベクトル↑Ｓ_cmdの構成要素として含まない比較例では目標基***置姿勢が、最終的に基本目標基***置姿勢（＝０deg）に対して１３deg程度の定常的なずれを生じた。

これに対して、基体運動速度ベクトル↑Ｖb_cmd1を目標制御対象ベクトル↑Ｓ_cmdの構成要素として含む実施例では、図７（ａ）に示すように目標基***置姿勢の、基本目標基***置姿勢に対する定常的なずれを解消することができた。

従って、基体運動速度ベクトル↑Ｖb_cmd1を目標制御対象ベクトル↑Ｓ_cmdの構成要素として含む本実施形態によれば、基体運動速度ベクトル↑Ｖb_cmd1を目標制御対象ベクトル↑Ｓ_cmdに含めない場合に比して、目標基***置姿勢を、基本目標基***置姿勢から大きく乖離させることなく該基本目標基***置姿勢に追従させることができることが判る。

次に、図８（ａ）及び図９（ａ）のそれぞれのグラフｂ１、ｃ１は、それぞれ、本実施形態の実施例で作成した目標歩容により規定されるロボット１の全体重心のＸ軸方向の並進運動量の誤差（基本目標並進運動量ベクトル↑Ｐ_cmd1のＸ軸方向成分に対する誤差）、該目標歩容により規定されるロボット１の全体重心でのＹ軸周り（ピッチ軸周り）の角運動量の誤差（基本目標角運動量ベクトル↑Ｌ_cmd1のＹ軸周り成分に対する誤差）の経時変化を示すグラフである。この場合、ロボット１の目標歩容は、例えば３５５ｍｍの最大歩幅で歩行を行なう歩容である。

また、図８（ｂ）及び図９（ｂ）のそれぞれのグラフｂ２、ｃ３は、それぞれ、比較例で作成した目標歩容により規定されるロボット１の全体重心のＸ軸方向の並進運動量の誤差（基本目標並進運動量ベクトル↑Ｐ_cmd1のＸ軸方向成分に対する誤差）、該目標歩容により規定されるロボット１の全体重心でのＹ軸周り（ピッチ軸周り）の角運動量の誤差（基本目標角運動量ベクトル↑Ｌ_cmd1のＹ軸周り成分に対する誤差）の経時変化を示すグラフである。

ただし、図８（ｂ）及び図９（ｂ）の比較例においては、各演算処理周期においてヤコビアン行列Ａを算出するときの起点とするロボット１の動作状態は、図８（ａ）及び図９（ａ）の実施例と異なり、前回の演算処理周期で決定した目標歩容（既に決定された目標歩容のうちの最新値の目標歩容）により規定される動作状態とした。

なお、図８（ａ）及び図９（ａ）の実施例と、図８（ｂ）及び図９（ｂ）の比較例とでは、ヤコビアン行列Ａの決定処理の相違による効果を明確にするために、目標制御対象ベクトル↑Ｓ_cmdの構成要素のうち、前記修正後基本並進運動量ベクトル↑Ｐ_cmd2を、基本目標並進運動量ベクトル↑Ｐ_cmd1に一致させるものとした。

図８（ｂ）のグラフｂ２で見られるように、比較例では、ロボット１の全体重心の並進運動量（Ｘ軸方向の並進運動量）の誤差の絶対値は、最大で２．３kg・m/s程度であった。これに対して、図８（ａ）のグラフｂ１で見られるように、実施例では、ロボット１の全体重心の並進運動量（Ｘ軸方向の並進運動量）の誤差の絶対値は、最大でも０．００８kg・m/s以下に収まっている。

また、図９（ｂ）のグラフｃ２で見られるように、比較例では、ロボット１の全体重心周りの角運動量（Ｙ軸周りの角運動量）の誤差の絶対値は、最大で０．８kg・m/s程度であった。これに対して、図９（ａ）のグラフｃ１で見られるように、実施例では、ロボット１の全体重心周りの角運動量（Ｙ軸周りの角運動量）の誤差の絶対値は、最大でも０．００３kg・m/s以下に収まっている。

従って、ヤコビアン行列Ａを算出するためのロボット１の起点の動作状態として、現在の目標歩容により規定される動作状態よりも将来の予測される動作状態寄りにずらした動作状態を用いる本実施形態によれば、ヤコビアン行列Ａを算出するためのロボット１の起点の動作状態として、現在の目標歩容により規定される動作状態を用いる場合に比して、目標制御対象ベクトル↑Ｓ_cmdにより精度よく追従させ得る目標歩容を生成できることがわかる。

なお、前記実施形態では、２つの脚リンクを有する２足移動ロボット１を例にとって説明したが、本発明は、３つの以上の脚リンクを有する脚式移動ロボットについても適用できる。

また、前記実施形態では、ヤコビアン行列Ａを算出するためのロボット１の起点の動作状態として、現在の目標歩容により規定される動作状態よりも将来の予測される動作状態寄りにずらした動作状態を用いるようにしたが、ヤコビアン行列Ａを算出するためのロボット１の起点の動作状態として、現在の目標歩容により規定される動作状態を用いるようにしてもよい。

このようにしても、基本目標基体運動速度ベクトル↑Ｖb_cmd1を目標制御対象ベクトル↑Ｓ_cmdの構成要素として含む実施形態によれば、目標歩容における目標基***置姿勢が、基本目標基***置姿勢から乖離するのを防止することができる。

なお、特に歩容生成部３２の演算処理周期を十分に短い時間に設定できる場合には、基体２の位置及び姿勢と、制御対象ベクトル↑Ｓの積分値との間の関係が非線形な関係であっても、現在の目標歩容により規定される動作状態を起点として算出したヤコビアン行列Ａの擬似逆行列Ａ⁺と、目標制御対象ベクトル↑Ｓ_cmdとから前記式（６）により算出される目標基体運動速度ベクトル↑Ｖb_cmd2の、目標制御対象ベクトル↑Ｓ_cmdに対応する最適な値に対する誤差を十分に微小なものに留めることができる。

また、前記実施形態では、擬似逆行列Ａ⁺を前記式（８ｂ）により算出するようにしたが、前記式（８ａ）により算出するようにしてもよい。

また、前記実施形態におけるロボット１は、基体２に関節１２を備えるロボットであるが、基体２の下側基体１０と上側基体１１とが一体に構成されたものであってもよい。この場合には、基本目標基体運動速度ベクトル↑Ｖb_cmd1や、目標基体運動速度ベクトル↑Ｖb_cmd2は、関節１２の変位量の時間的変化率の成分を除いた６成分のベクトルにより構成すればよい。

あるいは、下側基体１０と上側基体１１との間に２自由度以上の自由度を有する関節（２軸以上の軸周りの回転可能な関節）を備えるようにしてもよい。そして、この場合、基本目標基体運動速度ベクトル↑Ｖb_cmd1や、目標基体運動速度ベクトル↑Ｖb_cmd2を、下側基体１１の位置及び姿勢の時間的変化率の成分と、下側基体１０及び上側基体１１の間の関節の各軸周りの方向の変位量の時間的変化率の成分とにより構成してもよい。

また、前記実施形態におけるロボット１の各脚リンク３は、６自由度のリンク機構であるが、７自由度以上の自由度を有するリンク機構であってもよい。

４１…目標脚位置姿勢決定部（目標脚位置姿勢決定手段）、４２…基本動作目標値決定部（基本動作目標値決定手段）、４３…基本目標並進運動量修正部（基本目標並進運動量修正手段）、４４…基本目標基体運動速度決定部（基本目標基体運動速度決定手段）、４５…基体運動速度目標値決定部（基体運動速度目標値決定手段）、４６…目標基***置姿勢決定部（目標基***置姿勢決定手段）、４７…目標歩容上重心位置算出部（目標歩容上重心位置算出手段）。

Claims (4)

1. 基体から延設されると共に複数の関節がそれぞれ備えられた複数の脚リンクの運動によって移動する脚式移動ロボットの各関節の変位量を規定する目標歩容を逐次生成する歩容生成装置であって、
   前記目標歩容の構成要素としての前記移動ロボットの各脚リンクの先端部の位置及び姿勢の目標値である目標脚位置姿勢を逐次決定する目標脚位置姿勢決定手段と、
   前記移動ロボットの運動を行なうための動力学的な制約条件が該移動ロボットの動力学を表す動力学モデル上で満たされるように、該動力学モデルと前記決定された目標脚位置姿勢とを用いて、該移動ロボットの基体の位置及び姿勢の基本目標値である基本目標基***置姿勢と、該移動ロボットの全体重心の位置の基本目標値である基本目標全体重心位置と、該移動ロボットの全体重心の並進運動量ベクトルの基本目標値である基本目標並進運動量ベクトルと、該移動ロボットの全体重心周りの角運動量ベクトルの基本目標値である基本目標角運動量ベクトルとを逐次決定する基本動作目標値決定手段と、
   前記基体の位置及び姿勢の単位時間当たりの変化量を成分として構成される基体運動速度ベクトルから、該基体運動速度ベクトルの各成分と前記移動ロボットの全体重心の並進運動量ベクトルの各成分と該移動ロボットの全体重心の周りの角運動量ベクトルの各成分とにより構成されるベクトルである制御対象ベクトルへの線形写像を表すヤコビアン行列を、既に決定された前記移動ロボットの目標歩容のうちの少なくとも最新値の目標歩容を用いて逐次決定しつつ、該ヤコビアン行列の擬似逆行列を、前記制御対象ベクトルの基本目標値に乗じることにより、前記基体運動速度ベクトルの目標値を逐次決定する基体運動速度目標値決定手段と、
   前記決定された基体運動速度ベクトルの目標値を逐次積分することにより、前記目標歩容の構成要素としての前記基体の位置及び姿勢の目標値である目標基***置姿勢を逐次決定する目標基***置姿勢決定手段と、
   前記決定された目標脚位置姿勢と目標基***置姿勢とを構成要素とする前記目標歩容に対応する前記移動ロボットの全体重心の位置である目標歩容上全体重心位置を、該目標歩容から前記移動ロボットの幾何学モデルに基づいて逐次算出する目標歩容上重心位置算出手段と、
   前記算出された目標歩容上全体重心位置を前記決定された基本目標全体重心位置に追従させるために該目標歩容上全体重心位置と基本目標全体重心位置との偏差に応じて決定したフィードバック操作量により前記決定された基本目標並進運動量ベクトルを修正してなる修正後基本目標並進運動量ベクトルを逐次決定する基本目標並進運動量修正手段と、
   前記目標基***置姿勢を前記基本目標基***置姿勢に追従させるように前記決定された目標基***置姿勢と前記決定された基本目標基***置姿勢との偏差に応じて前記基体運動速度ベクトルの基本目標値である基本目標基体運動速度ベクトルを逐次決定する基本目標基体運動速度決定手段とを備え、
   前記基体運動速度目標値決定手段が前記基体運動速度ベクトルの目標値を決定するために用いる前記制御対象ベクトルの基本目標値として、前記決定された基本目標基体運動速度ベクトルの各成分と、前記決定された修正後基本目標並進運動量ベクトルの各成分と、前記決定された基本目標角運動量ベクトルの各成分とにより構成されるベクトルが該基体運動速度目標値決定手段に入力されることを特徴とする脚式移動ロボットの歩容生成装置。
2. 請求項１記載の脚式移動ロボットの歩容生成装置において、
   前記基体運動速度目標値決定手段は、前記基体運動速度ベクトルの新たな目標値を決定するために用いる前記ヤコビアン行列を決定するとき、既に決定された前記移動ロボットの目標歩容のうちの最新値の目標歩容に対応する時刻と前記基体運動速度ベクトルの新たな目標値に応じて前記目標基***置姿勢決定手段により決定される目標基***置姿勢を構成要素とする新たな目標歩容に対応する時刻との間の期間内での前記移動ロボットの将来の歩容の瞬時値を、前記最新値の目標歩容とその直前の１つ以上の過去値の目標歩容とに基づいて推定し、その推定した将来の歩容の瞬時値を、前記基体運動速度ベクトルによる前記基体の位置及び姿勢の変化の起点となる前記移動ロボットの動作状態として用いて、該移動ロボットの当該起点の動作状態に対応する前記ヤコビアン行列を決定することを特徴とする脚式移動ロボットの歩容生成装置。
3. 請求項２記載の脚式移動ロボットの歩容生成装置において、
   前記基体運動速度目標値決定手段は、前記最新値の目標歩容における目標基***置姿勢と該最新値の目標歩容の１つ前の過去値の目標歩容における目標基***置姿勢との差分に“０”と“１”との間の範囲内であらかじめ定めた係数Ｋｐを乗じてなる補正量を、前記最新値の目標歩容における目標基***置姿勢に加えることにより該目標基***置姿勢の将来の瞬時値を推定する手段と、各脚リンク毎に前記最新値の目標歩容における目標脚位置姿勢と該最新値の１つ前の過去値の目標歩容における目標脚位置姿勢との差分に前記係数Ｋｐを乗じてなる補正量を、前記最新値の目標歩容における目標脚位置姿勢に加えることにより各脚リンクの目標脚位置姿勢の将来の瞬時値を推定する手段とを備え、前記推定された目標基***置姿勢の将来の瞬時値と各脚リンクの目標脚位置姿勢の将来の瞬時値とを構成要素とする歩容を、前記移動ロボットの当該起点の動作状態として用いることを特徴とする脚式移動ロボットの歩容生成装置。
4. 請求項１〜３のいずれか１項に記載の脚式移動ロボットの歩容生成装置において、
   前記基体運動速度目標値決定手段は、前記ヤコビアン行列の特異点低感度運動分解行列を該ヤコビアン行列の擬似逆行列として用いて前記基体運動速度ベクトルの目標値を決定することを特徴とする脚式移動ロボットの歩容生成装置。

Images