(57)【要約】【課題】対象物動力学モデル上での対象物の運動状態と実際の運動状態とのずれを小さくしつつ、対象物の目標運動と、対象物・ロボットの間の作用力目標値とを決定しロボットの安定性を確保しながら対象物を移動させる作業をロボットに行わせる。【解決手段】対象物動力学モデル２の運動状態量（対象物モデル速度）を対象物の移動計画に基づく目標運動状態量に追従させるように対象物動力学モデル２に入力する作用力を決定すると共に、実対象物位置と目標対象物位置との偏差を０に近づけるように対象物動力学モデル２の運動状態量の操作量（対象物モデル速度）を決定し、決定した作用力と操作量とを対象物動力学モデル２に入力して目標対象物位置を逐次求める。また、決定した作用力により対象物からロボットへの目標対象物反力を決定する。【選択図】図１７
2006110676.tif 000002
【特許請求の範囲】
【請求項１】
上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための制御装置において、
対象物に作用する力と該対象物の運動との関係を表す対象物動力学モデル上での対象物の運動状態量の目標値である目標運動状態量を少なくとも対象物の移動計画に基づいて逐次決定する目標運動状態量決定手段と、
少なくとも前記決定された目標運動状態量に前記対象物動力学モデル上での対象物の運動状態量を追従させるようにロボットと前記対象物との間で作用する力の目標値であるロボット・対象物間目標作用力を逐次決定する目標作用力決定手段と、
前記決定されたロボット・対象物間目標作用力を前記対象物動力学モデルに入力して、該動力学モデルの出力としての対象物の目標移動位置を逐次決定する目標移動位置決定手段と、
少なくとも前記決定されたロボット・対象物間目標作用力と前記決定された目標移動位置とに応じてロボットの動作を制御する制御手段と、
前記対象物の実際の移動位置である実移動位置を把握する実移動位置把握手段と、
前記把握された実移動位置と前記決定された目標移動位置との偏差に応じて、該偏差を０に近づけるように前記対象物動力学モデル上での対象物の運動操作量を決定する運動操作量決定手段とを備え、
前記目標移動位置決定手段は、前記決定された運動操作量に応じて前記対象物動力学モデル上での対象物の運動状態量を修正しつつ、前記目標移動位置を決定することを特徴とする脚式移動ロボットの制御装置。
【請求項２】
前記対象物に作用する力のうち、前記ロボットから前記対象物に作用する力以外の外乱力を逐次推定する外乱力推定手段と、前記推定された外乱力の変化に応じて前記移動計画を修正する移動計画修正手段とを備えたことを特徴とする請求項１記載の脚式移動ロボットの制御装置。
【請求項３】
前記対象物に作用する力のうち、前記ロボットから前記対象物に作用する力以外の外乱力を逐次推定する外乱力推定手段を備え、前記目標運動状態量決定手段は、少なくとも前記移動計画と前記推定された外乱力とに基づき前記目標運動状態量を決定することを特徴とする請求項１記載の脚式移動ロボットの制御装置。
【請求項４】
前記外乱力推定手段は、前記決定されたロボット・対象物間目標作用力と、前記対象物動力学モデル上での対象物の移動速度とに基づき前記外乱力を推定する手段であることを特徴とする請求項２または３記載の脚式移動ロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、脚式移動ロボットに対象物を移動させる作業を行わせるための制御装置に関する。
【背景技術】
【０００２】
２足移動ロボットなどの脚式移動ロボットに、ある対象物を押して移動させるなどの作業を行わせる場合には、ロボットはその脚体の先端部が接触する床から床反力を受けるだけでなく、対象物から反力を受ける。このように対象物からロボットが受ける反力を本明細書では対象物反力という。
【０００３】
このように脚式移動ロボットに対象物反力が作用する状態でロボットの歩容を生成したり、動作制御を行なう技術としては、例えば本願発明者による特開平１０−２３０４８５号公報（特許文献１）に見られるものが知られている。この技術は、ロボットの運動により発生する慣性力と、ロボットに作用する重力および対象物反力との合力が目標ＺＭＰ（目標床反力中心点）まわりに発生するモーメントの鉛直成分を除く成分（水平成分）が０になる（目標床反力中心点に作用する床反力と上記合力とが釣り合う）という動力学的平衡条件を満たすように、目標歩容を生成するものである。さらに、この技術では、ロボットに作用する外力が予期しない外力になっても（目標外力と実外力との偏差がある程度大きくなっても）、ロボットの重心位置がロボットの動バランスを保ち得るような重心位置に平衡するようにロボットの目標歩容を調整するようにしている。
【特許文献１】特開平１０−２３０４８５号公報（特許文献１）
【発明の開示】
【発明が解決しようとする課題】
【０００４】
ところで、上記の如くロボットに対象物を移動させる作業を行なわせるためには（より詳しくは当該作業を行なわせるロボットの歩容を決定し、その歩容に追従するようにロボットを動作させるためには）、ロボットから対象物に作用させる力の目標値、または前記対象物反力の目標値を決定しておく必要がある。なお、ロボットから対象物に作用させる力の符号を反転させたものが対象物反力であるから、本欄での説明では、以降、代表的に対象物反力の目標値を決定しておくものとする。
【０００５】
この場合、対象物反力の目標値（以下、目標対象物反力という）は、対象物の移動計画（どのタイミングでどのように対象物を移動させるなどの計画）や、あらかじめ想定した環境条件（対象物と床との摩擦係数、床の傾斜角など）に基づいて決定されることとなる。さらには、この場合、想定した環境条件の基で構築した対象物の動力学モデル（対象物の位置、速度などの運動と、対象物に作用する力との関係を表すモデル）に、対象物の移動計画に基づく対象物の運動を適用することで、目標対象物反力を決定することも考えられる。
【０００６】
しかしながら、実際の環境条件が想定した環境条件と合致しない場合がしばしばあり、そのような場合には、目標対象物反力と実際の対象物反力（実対象物反力）との間にずれが生じる。その結果、目標対象物反力を前提にロボットの動力学的平衡条件を満足するようにロボットの動作制御を行なっても、目標対象物反力と実対象物反力との間のずれによって、実際のロボットの動力学的平衡条件が満たされず、ロボットの安定性が損なわれてしまう。
【０００７】
このために、前記特許文献１のものでは、目標対象物反力と実対象物反力との偏差を打ち消すように、ロボットの重心位置や床反力の目標値を修正するようにしている。
【０００８】
しかしながら、目標対象物反力と実対象物反力との間のずれが生じる状況では、一般に、移動計画に基づく対象物の移動位置（すなわち対象物の目標移動位置）と、対象物の実際の移動位置との間にもずれが生じる。この場合、目標対象物反力は、対象物の実際の移動位置に整合しないものとなる。このため、前記特許文献１のものの如く、目標対象物反力と実対象物反力との偏差を打ち消すように、ロボットの重心位置や床反力の目標値を修正しても、目標対象物反力と実対象物反力との間のずれがが過大になったり、そのずれが頻繁に生じることがある。そして、このような場合には、ロボットの過大な挙動変化が生じたりして、ロボットの安定性を継続的に保ちながら、連続的に対象物を移動させる作業をロボットに行わせることが困難となる恐れがあった。
【０００９】
また、実対象物反力の検出値に応じて目標対象物反力を修正することも考えられる。しかしながら、例えば実対象物反力が急変したような場合に、これに応じて目標対象物反力を急変させると、ロボットの過大な挙動変化が生じて、ロボットの安定性が損なわれやすい。また、この場合、実対象物反力の変化に対して、目標対象物反力を緩やかに変化させることも考えられるが、そのようにした場合には、実対象物反力の変化に対して目標対象物反力の変化が遅れる。このため、ロボットの動力学的な安定性を速やかに確保することができず、ひいては、ロボットの安定性が損なわれてしまう。
【００１０】
本発明はかかる背景に鑑みてなされたものであり、対象物の動力学モデルを用い、その動力学モデル上での対象物の運動状態と実際の対象物の運動状態とのずれを小さくしつつ、該動力学モデルを用いて対象物の目標とする運動と、対象物とロボットとの間の作用力の目標値とを決定することができ、ひいては、ロボットの安定性を確保しながら、対象物を移動させる作業をロボットに行わせることができる脚式移動ロボットの制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１１】
本発明の脚式移動ロボットの制御装置の第１発明は、かかる目的を達成するために、
上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための制御装置において、
対象物に作用する力と該対象物の運動との関係を表す対象物動力学モデル上での対象物の運動状態量の目標値である目標運動状態量を少なくとも対象物の移動計画に基づいて逐次決定する目標運動状態量決定手段と、
少なくとも前記決定された目標運動状態量に前記対象物動力学モデル上での対象物の運動状態量を追従させるようにロボットと前記対象物との間で作用する力の目標値であるロボット・対象物間目標作用力を逐次決定する目標作用力決定手段と、
前記決定されたロボット・対象物間目標作用力を前記対象物動力学モデルに入力して、該動力学モデルの出力としての対象物の目標移動位置を逐次決定する目標移動位置決定手段と、
少なくとも前記決定されたロボット・対象物間目標作用力と前記決定された目標移動位置とに応じてロボットの動作を制御する制御手段と、
前記対象物の実際の移動位置である実移動位置を把握する実移動位置把握手段と、
前記把握された実移動位置と前記決定された目標移動位置との偏差に応じて、該偏差を０に近づけるように前記対象物動力学モデル上での対象物の運動操作量を決定する運動操作量決定手段とを備え、
前記目標移動位置決定手段は、前記決定された運動操作量に応じて前記対象物動力学モデル上での対象物の運動状態量を修正しつつ、前記目標移動位置を決定することを特徴とする。
【００１２】
かかる第１発明によれば、対象物の移動計画（対象物をどのタイミングでどのように移動させるかの計画）に基づき決定される目標運動状態量（対象物の目標速度など）に対象物動力学モデル上での対象物の運動状態量を追従させるようにロボット・対象物間目標作用力が決定され、これが対象物動力学モデルに入力される。なお、ロボットと対象物との間で作用する力は、ロボットから対象物に作用する力と対象物からロボットに作用する力とがあるが、それらは符号（力の向き）が異なるだけであるので、いずれの力の目標値をロボット・対象物間目標作用力としてもよい。そして、前記目標移動位置決定手段は、実移動位置と目標移動位置との偏差を０に近づけるように決定された運動操作量に応じて対象物動力学モデル上での対象物の運動状態量を修正しつつ、対象物の目標移動位置を決定する。このため、対象物動力学モデル上での対象物の運動状態と実際の対象物の運動状態とのずれを小さくしつつ、該動力学モデルを用いて対象物の目標移動位置とロボット・対象物間目標作用力とを決定できる。そして、本発明は、これらの目標対象物位置とロボット・対象物間目標作用力とに応じてロボットの動作を制御するので、ロボットにその挙動が急変するような無理な動作をさせることなく、ロボットに対象物を移動させる作業を行わせることが可能となる。その結果、ロボットの安定性を確保しながら、ロボットに対象物を移動させる作業を行わせることができる。
【００１３】
なお、目標対象物位置とロボット・対象物間目標作用力とに応じてロボットの動作を制御する場合、例えば目標対象物位置に応じて脚体の着地位置や着地タイミング、ロボットの対象物に係合させるべき部位の位置を決定すると共に、ロボットの運動によって発生する慣性力とロボットに作用する重力とロボット・対象物間目標作用力（より詳しくは対象物からロボットに作用する力の目標値）との合力がロボットの目標ＺＭＰまわりに発生するモーメントの水平成分が０になるようにロボットの目標歩容を決定し、その目標歩容に追従するようにロボットの動作制御を行なえばよい。
【００１４】
かかる第１発明では、前記対象物に作用する力のうち、前記ロボットから前記対象物に作用する力以外の外乱力を逐次推定する外乱力推定手段と、前記推定された外乱力の変化に応じて前記移動計画を修正する移動計画修正手段とを備えることが好ましい（第２発明）。
【００１５】
すなわち、対象物の移動計画で想定されていない外乱力の変化があると、対象物の実移動位置と目標移動位置とのずれが生じやすいが、この外乱力を逐次推定して、この外乱力の変化に応じて移動計画を修正することにより、外乱力の大きな変化が生じたとき（推定した外乱力の変化量が所定量を超えるような場合）などに、対象物の実移動位置と目標移動位置とのずれが過大になるのを防止できる。その結果、ロボット・対象物間目標作用力が過大になったりするのを防止することが可能となり、ひいてはロボットの安定性を玉メルことが可能となる。
【００１６】
また、前記第１発明では、前記対象物に作用する力のうち、前記ロボットから前記対象物に作用する力以外の外乱力を逐次推定する外乱力推定手段を備え、前記目標運動状態量決定手段は、少なくとも前記移動計画と前記推定された外乱力とに基づき前記目標運動状態量を決定することが好ましい（第３発明）。
【００１７】
これによれば、対象物に実際に作用する力のうち、ロボットから作用する力以外の外乱力（例えば床から対象物に作用する摩擦力など）の変化を観測しながら、対象物の目標運動状態量を決定することとなるので、対象物を移動計画に合わせて移動させる上で適切な目標運動状態量を決定できる。そして、この目標運動状態量に、対象物動力学モデル上での対象物の運動状態量を追従させるようにロボット・対象物間目標作用力を決定するので、実際の外乱力の変化に合わせて、ロボット・対象物間目標作用力を変化させることができ、ひいては、ロボットの挙動の安定性を高めることができる。
【００１８】
なお、この第３発明は、前記第２発明と併用してもよい。この場合には、第３発明における外乱力推定手段と、第２発明における外乱力推定手段とは共通のものでよい。
【００１９】
補足すると、上記第３発明で、目標運動状態量を決定するときには、例えば対象物の移動計画に基づき、ロボット・対象物間作用力の仮目標値を決定し、この仮目標値と前記推定された外乱力とから、第２の対象物動力学モデル（対象物に作用する力と該対象物の運動との関係を表す第２の動力学モデル）を用いて目標運動状態量を決定するようにすればよい。この場合、ロボット・対象物間作用力の仮目標値は、移動計画により規定される対象物の運動状態量（対象物の移動速度など）に、上記第２の対象物動力学モデル上での対象物の運動状態量が追従するように決定するようにしてもよい。
【００２０】
また、前記第２発明および第３発明では、前記外乱力推定手段は、前記決定されたロボット・対象物間目標作用力と、前記対象物動力学モデル上での対象物の移動速度とに基づき前記外乱力を推定する手段であることが好ましい（第４発明）。
【００２１】
すなわち、本発明では、対象物の実移動位置と目標移動位置との偏差を小さくできるので、前記対象物動力学モデル上での対象物の移動速度は、実際の対象物の移動速度の推定値に相当するものとなる。そして、その移動速度の時間的変化量が対象物に作用する全ての力の合力（ロボットから対象物の作用する力と外乱力との合力）に応じたものとなるので、対象物動力学モデル上での対象物の移動速度とに基づき前記外乱力を推定できる。このように外乱力を推定することで、対象物に力センサなどのセンサを搭載したりすることなく、適切に外乱力を推定できる。
【００２２】
なお、第４発明で外乱力を推定するためには、より具体的には、例えば、対象物動力学モデル上での対象物の移動速度の微分値と対象物の質量との積に追従する値（例えば１次遅れを伴って追従する値）を対象物に作用する全ての力の合力の推定値とし、その合力の推定値から対象物動力学モデルに入力するロボット・対象物間目標作用力により定まる、ロボットから対象物への目標作用力を減算したものを外乱力の推定値として求めればよい。
【発明を実施するための最良の形態】
【００２３】
以下、添付図面を参照して本発明の実施形態に係る脚式移動ロボットの制御装置を説明する。尚、脚式移動ロボットとしては２足移動ロボットを例にとる。
【００２４】
図１は、この実施形態に係る脚式移動ロボットとしての２足移動ロボットを全体的に示す概略図である。
【００２５】
図示の如く、２足移動ロボット（以下、ロボットという）１は上体(ロボット１の基体）３から下方に延設された左右一対の脚体（脚部リンク）２，２を備える。両脚体２，２は同一構造であり、それぞれ６個の関節を備える。その６個の関節は上体３側から順に、股（腰部）の回旋（回転）用（上体３に対するヨー方向の回転用）の関節１０Ｒ，１０Ｌ（符号Ｒ，Ｌはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。以下同じ）と、股（腰部）のロール方向（Ｘ軸まわり）の回転用の関節１２Ｒ，１２Ｌと、股（腰部）のピッチ方向（Ｙ軸まわり）の回転用の関節１４Ｒ，１４Ｌと、膝部のピッチ方向の回転用の関節１６Ｒ，１６Ｌと、足首のピッチ方向の回転用の関節１８Ｒ，１８Ｌと、足首のロール方向の回転用の関節２０Ｒ，２０Ｌとから構成される。
【００２６】
各脚体２の足首の２つの関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には、各脚体２の先端部を構成する足平（足部）２２Ｒ（Ｌ）が取着されると共に、両脚体２，２の最上位には、各脚体２の股の３つの関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）を介して前記上体３が取り付けられている。上体３の内部には、詳細を後述する制御ユニット６０などが格納される。なお、図１では図示の便宜上、制御ユニット６０を上体３の外部に記載している。
【００２７】
上記構成の各脚体２においては、股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から構成され、膝関節は関節１６Ｒ（Ｌ）から構成され、足首関節は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２４Ｒ（Ｌ）で連結され、膝関節と足首関節とは下腿リンク２６Ｒ（Ｌ）で連結される。
【００２８】
上体３の上部の両側部には左右一対の腕体（腕リンク）５，５が取り付けられると共に、上体３の上端部には頭部４が配置される。両腕体５，５は同一構造であり、それぞれ７個の関節を備える。すなわち、各腕体５は、３つの関節３０Ｒ（Ｌ），３２Ｒ（Ｌ），３４Ｒ（Ｌ）から構成された肩関節と、１つの関節３６Ｒ（Ｌ）から構成された肘関節と、３つの関節３８Ｒ（Ｌ），４０Ｒ（Ｌ），４２Ｒ（Ｌ）から構成された手首関節と、この手首関節に連結された手先部（ハンド）４０Ｒ（Ｌ）とを備えている。なお、頭部４は、本発明の要旨と直接的な関連を有しないため詳細な説明を省略する。
【００２９】
上記の構成により、各脚体２の足平２２Ｒ（Ｌ）は、上体３に対して６つの自由度を与えられている。そして、ロボット１の歩行等の移動中に、両脚体２，２を合わせて６＊２＝１２個（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）の関節を適宜な角度で駆動することで、両足平２２Ｒ，２２Ｌの所望の運動を行うことができる。これにより、ロボット１は任意に３次元空間を移動することができる。また、各腕体５は、上体３に対して７つの自由度を与えられ、両腕体５，５を合わせて７＊２＝１４個の関節を適宜な角度で駆動することで、後述する台車を押すなどの所望の作業を行なうことができる。
【００３０】
図１に示す如く、各脚体２の足首関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下方には足平２２Ｒ（Ｌ）との間に公知の６軸力センサ５０が介装されている。該６軸力センサ５０は、各脚体２の足平２２Ｒ（Ｌ）の着地の有無、および各脚体２に作用する床反力（接地荷重）等を検出するためのものであり、該床反力の並進力の３方向成分Ｆｘ，Ｆｙ，Ｆｚ並びにモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚの検出信号を制御ユニット６０に出力する。この６軸力センサ５０と同様の６軸力センサ５２が、各腕体５の手先部（ハンド）４０Ｒ（Ｌ）と手首関節３８Ｒ（Ｌ），４０Ｒ（Ｌ），４２Ｒ（Ｌ）との間に介装され、手先部４０Ｒ（Ｌ）に作用する外力の並進力の３方向成分並びにモーメントの３方向成分の検出信号が該６軸力センサ５２から制御ユニット６０に出力される。また、上体３には、Ｚ軸（鉛直方向（重力方向））に対する上体３の傾斜角およびその角速度、並びにＺ軸回りの上体３の回転角（ヨー角）およびその角速度を検出するための姿勢センサ５４が備えられ、その検出信号が該姿勢センサ５４から制御ユニット６０に出力される。この姿勢センサ５４は、図示を省略する３軸方向の加速度センサおよび３軸方向のジャイロセンサを備え、これらのセンサの検出信号が上体３の姿勢角（傾斜角およびヨー角）およびその角速度を検出するために用いられると共に、ロボット１の自己位置姿勢を推定するために用いられる。また、詳細構造の図示は省略するが、ロボット１の各関節には、それを駆動するための電動モータ６４（図２参照）と、その電動モータ６４の回転量（各関節の回転角）を検出するためのエンコーダ（ロータリエンコーダ）６５（図２参照）とが設けられ、該エンコーダ６５の検出信号が該エンコーダ６５から制御ユニット６０に出力される。
【００３１】
なお、図示は省略するが、各足平２２Ｒ（Ｌ）と６軸力センサ５０との間には、ばね等の弾性体が介装され、また、足平２２Ｒ（Ｌ）の底面には、ゴム等の弾性体が貼り付けられている。これらの弾性体は、コンプライアンス機構を構成するものであり、各脚体２が床反力を受けたときに弾性変形するようになっている。
【００３２】
図２は制御ユニット６０の構成を示すブロック図である。該制御ユニット６０はマイクロコンピュータにより構成されており、ＣＰＵからなる第１の演算装置９０及び第２の演算装置９２、Ａ／Ｄ変換器８０、カウンタ８６、Ｄ／Ａ変換器９６、ＲＡＭ８４、ＲＯＭ９４、並びにこれらの間のデータ授受を行うバスライン８２を備えている。この制御ユニット６０では、各脚体２の６軸力センサ５０、各腕体５の６軸力センサ５２、姿勢センサ５４（加速度センサおよびレートジャイロセンサ）等の出力信号はＡ／Ｄ変換器８０でデジタル値に変換された後、バスライン８２を介してＲＡＭ８４に送られる。またロボット１の各関節のエンコーダ６５（ロータリーエンコーダ）の出力は、カウンタ８６を介してＲＡＭ８４に入力される。
【００３３】
前記第１の演算装置９０は後述の如く目標歩容を生成すると共に、関節角変位指令（各関節の変位角もしくは各電動モータ６４の回転角の指令値）を算出し、ＲＡＭ８４に送出する。また、第２の演算装置９２はＲＡＭ８４から関節角変位指令と、前記エンコーダ６５の出力信号に基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出してＤ／Ａ変換器９６とサーボアンプ６４ａとを介して各関節を駆動する電動モータ６４に出力する。
【００３４】
図３は、本明細書の実施形態に係る脚式移動ロボットの制御装置の機能的構成を全体的に示すブロック図である。この図３中の破線で囲んだ部分が制御ユニット６０が実行する処理機能（主として第１の演算装置９０及び第２の演算装置９２の機能）によって構成されるものである。なお、以下の説明では、脚体２および腕体５の左右を特に区別する必要がないときは、前記符号Ｒ，Ｌを省略する。
【００３５】
以下説明すると、制御ユニット６０の機能的構成は、歩容生成装置１００、対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６から構成されている。この明細書の実施形態では、歩容生成装置１００の処理を除き、対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６の処理は、本願出願人が先に特開平１０−２３０４８５号公報にて提案したものと同じである。従って、以下の実施形態の説明では、歩容生成装置１００を中心に説明し、対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６の説明は概略的な説明に留める。
【００３６】
歩容生成装置１００は、ロボット１の目標歩容を自在かつリアルタイムに生成して出力するものである。この歩容生成装置１００が出力する目標歩容は、目標上体位置姿勢軌道（上体３の目標位置及び目標姿勢軌道）、目標足平位置姿勢軌道（各足平２２の目標位置及び目標姿勢軌道）、目標手先位置姿勢軌道（各腕体５の手先部４４の目標位置および目標姿勢軌道）、目標全床反力中心点軌道（全床反力中心点の目標位置の軌道））、目標全床反力軌道、および目標対象物反力軌道から構成される。なお、脚体２や腕体５以外に上体３に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。
【００３７】
ここで、歩容生成装置１００が生成する目標歩容に係る用語の意味および定義について説明しておく。歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味する。
【００３８】
足平位置、上体位置など、ロボット１の各部位の「位置」は、その部位に固定的に設定されたある代表点の位置を意味する。例えば、本実施形態では、各足平２２の代表点は、その足平２２を備える脚体２の足首関節の中心から、該足平２２の底面に延ばした垂線が該底面と交わる点であり、その代表点の位置が足平位置である。また、「姿勢」は空間的な向きを意味する。具体的には、例えば上体姿勢はＺ軸（鉛直軸）に対するロール方向（Ｘ軸まわり）の上体３の傾斜角とピッチ方向（Ｙ軸まわり）の上体３の傾斜角とヨー方向（Ｚ軸まわり）の上体３の回転角（ヨー角）とで表され、足平姿勢は各足平２２に固定的に設定された２軸の空間的な方位角で表される。
【００３９】
なお、歩容のうちの床反力および対象物反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上体位置姿勢等、ロボット１の運動に係わる歩容を総称的に「運動」という。また、運動の軌道は、一定（時間的に変化しない）あるいはほぼ一定のものであってもよい。例えばロボット１がその両足平２２，２２を着地させた状態に維持して、移動しない状態では、足平位置姿勢軌道は一定になる。
【００４０】
歩容のうちの対象物反力は、ロボット１にある対象物を移動させるときに該対象物にロボット１から作用させる力の反力（対象物からロボット１に作用する力）を意味する。従って、目標対象物反力軌道は、本発明におけるロボット・対象物間目標作用力の軌道に相当する。本明細書の実施形態では、例えば図４に示すように、ロボット１がある対象物１２０（図示の例では台車）の所定の部位に両腕体５，５の手先部４４Ｒ，４４Ｌを係合させた状態で歩行動作を行いながら該対象物１２０を押す作業を行なう場合を例にとって説明する。この対象物１２０からロボット１が受ける力が対象物反力である。対象物反力は、一般的には、並進力成分とモーメント成分とから構成されるが、本明細書の実施形態では、対象物反力はロボット１に対象物１２０から作用する並進力を意味するものとし、その並進力によってロボット１に作用する、ある点回りのモーメントを対象物反力モーメントという。補足すると、対象物反力は、ロボット１から対象物１２０に作用させる力の符号（向き）を反転させたものであるので、ロボット１から対象物１２０への作用力を対象物反力の代わりに用いてもよい。
【００４１】
なお、本明細書では、対象物１２０の運動に関し、対象物１２０の位置はロボット１の足平位置などと同様に、対象物１２０に固定的に設定された代表点の位置を意味する。また、対象物１２０の姿勢はロボット１の足平姿勢などと同様に、対象物１２０の空間的な向きを意味する。そして、対象物１２０の位置姿勢を総称的に対象物運動ということがある。
【００４２】
また、各足平２２に作用する床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、ロボット１の全て（２つ）の足平２２Ｒ，２２Ｌについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義として扱う。
【００４３】
目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に目標床反力中心点（全床反力の中心点の目標位置）を作用点にして目標床反力を表現すると、目標床反力のモーメント成分は、鉛直成分（鉛直軸（Ｚ軸）まわりのモーメント）を除いて０になる。換言すれば、目標床反力中心点まわりの目標床反力のモーメントの水平成分（水平軸（Ｘ軸及びＹ軸）回りのモーメント）は０になる。そこで、本明細書の実施形態では、目標床反力中心点を目標全床反力の作用点として、歩容生成装置１００で生成するものとした。なお、目標床反力中心点は、目標全床反力の作用点としての意味を持つものであるから、本質的には目標全床反力の１つの構成要素であると言える。但し、本明細書の実施形態では、目標床反力のうちの目標床反力中心点がロボット１の動作制御と密接に関連するものであることから、図３では、目標全床反力のうちの作用点である目標床反力中心点を目標全床反力とは別に記載するものとした。
【００４４】
また、本明細書では、ＺＭＰ(zero moment point)は、ロボットの運動によって生じる慣性力と該ロボットに作用する重力と対象物反力との合力（あるいはその合力と釣り合う床反力）がその点まわりに作用するモーメントが、鉛直成分を除いて０になる点の意味で使用する。動力学的平衡条件を満足する歩容では、ロボットの目標運動軌道と重力と目標対象物反力とから算出されるＺＭＰと目標床反力中心点とは一致する。本明細書では、多くの場合、目標床反力中心点の代わりに、目標ＺＭＰという。
【００４５】
また、本明細書では、目標歩容は、所定期間分の目標運動と目標床反力（目標ＺＭＰを含む）と目標対象物反力との組を意味するものとする。特に、本実施形態では、目標歩容（単に歩容と称した場合を含む）は、ことわらない限り、ロボット１の１歩の期間分の目標運動と目標床反力（目標ＺＭＰを含む）と目標対象物反力との組を意味するものとする。この場合、目標歩容の「１歩」は、ロボット１の片方の脚体２が着地してから、もう一方の脚体２が着地するまでの意味で使用する。なお、一連の歩容は、いくつかの歩容（所定期間分の歩容）がつながったものとする。また、以降の説明では、１歩の期間分の目標歩容を単位歩容ということがある。また、ロボット１が移動しない状態（両足平２２，２２の接地を維持する状態）での１歩の期間は、ある所定の期間（ロボット１が移動する場合の１歩の期間に相当する期間）を意味する。
【００４６】
また、歩容における両脚支持期とは、ロボット１がその自重を両脚体２，２で支持する期間（両脚体２，２が支持脚となる期間）、片脚支持期とはいずれか一方のみの脚体２でロボット１の自重を支持する期間（一方のみの脚体２が支持脚となる期間）を言う。本実施形態での目標歩容（単位歩容）は、別の言い方をすれば、両脚支持期の開始時から次の片脚支持期の終了時までの期間分の目標歩容である。ロボット１の自重を支持する脚体２を支持脚と呼ぶ。両脚支持期では、両脚体２，２が支持脚となり、片脚支持期では１つの脚体２が支持脚となる。また、片脚支持期においてロボット１の自重を支持しない側の脚体２（支持脚でない脚体２）を遊脚と呼ぶ。なお、以降の説明では、特にことわならい限り、目標歩容における支持脚は、両脚支持期の開始時に着地する脚体２（該両脚支持期に続く方脚支持期で支持脚となる脚体２）を意味するものとする。また、支持脚側の脚体２の足平２２、遊脚側の脚体２の足平２２をそれぞれ支持脚足平２２、遊脚足平２２という。また、ロボット１が移動しない状態（両足平２２，２２の接地を維持する状態）であっても、一方の脚体２が支持脚、他方の脚体２が遊脚であるとする。
【００４７】
また、歩容生成装置１００により新たに生成しようとしている目標歩容（単位歩容）、あるいは生成しつつある目標歩容（単位歩容）を今回歩容と呼び、この今回歩容の１歩前の目標歩容を前回歩容、今回歩容の次の１歩の目標歩容を次回歩容、さらにその次の１歩の目標歩容を次次回歩容などと呼ぶ。
【００４８】
なお、目標歩容は、グローバル座標系としての支持脚座標系で記述される。本実施形態では、支持脚座標系は、支持脚足平２２を水平姿勢（より一般的には床面に平行な姿勢）にして該支持脚足平２２の底面のほぼ全面を床面に接触（密着）させた状態において、該支持脚の足首関節１８，２０の中心から床面に延ばした垂線が該床面と交わる点（この点は、本明細書の実施形態の例では支持脚足平２２の底面のほぼ全面を床面に接触させた状態では、該足平２２の代表点と合致する）を原点とし、その原点を通る水平面をＸＹ平面とするグローバル座標系（床に固定された座標系）である。この場合、Ｘ軸方向、Ｙ軸方向は、それぞれ支持脚足平２２の前後方向、左右方向である。以降の説明では、Ｘ軸、Ｙ軸、Ｚ軸は、特にことわらない限り、この支持脚座標系の３軸を意味するものとする。尚、支持脚座標系の原点は、必ずしも支持脚足平２２の底面のほぼ全面を床面に接触させた状態での該足平２２の代表点に合致する必要はなく、該代表点と異なる床面上の点に設定されてもよい。
【００４９】
以下に本発明の第１実施形態に係る歩容生成装置１００の詳細を説明する。図５〜図７は歩容生成装置１００の処理を示すフローチャートである。なお、この第１実施形態は、本発明の第１発明、第３発明、および第４発明の実施形態である。
【００５０】
歩容生成装置１００は、図５〜図７のフローチャートに示す処理を所定の演算処理周期で逐次実行する。
【００５１】
まず、Ｓ０１において、対象物１２０の移動計画が決定される。ここで決定される移動計画は、少なくとも現在時刻から将来の所定期間分（ロボット１の複数歩分）の対象物１２０の移動計画を含んでいる。この移動計画は、基本的には対象物１２０の移動要求（対象物１２０をどのようなタイミングでどのように動かすかという設計的な要求）に応じて決定され、該対象物１２０の位置姿勢軌道（位置および姿勢の時系列）、あるいは、その軌道を規定するパラメータもしくは関数式等から構成される。例えば対象物１２０の移動要求が、ある時刻ｔ0からＸ軸方向に一定速度で対象物１２０を移動させるという要求である場合には、移動計画は、図８のグラフg1で示す如く決定される。グラフg1は、移動計画における対象物位置（Ｘ軸方向位置）の経時変化を示している。この場合、例えば現在時刻以降の各時刻（ある刻み時間毎の時刻）における対象物１２０の位置の時系列を移動計画として決定してもよいが、時刻ｔ0とグラフg1の傾き（対象物１２０の移動速度）とを移動計画を規定する要素（パラメータ）として決定したり、あるいはグラフg1の関数式を移動計画を規定する要素（パラメータ）として決定してもよい。なお、移動要求は、歩容生成装置１００に外部から適宜与えられ、あるいは、あらかじめ制御ユニット６０の図示しない記憶手段に記憶保持される。
【００５２】
補足すると、Ｓ０１で決定する移動計画は、必ずしも移動要求通りに決定されるわけではなく、必要に応じて適宜修正されるが、これについては後述する。
【００５３】
次にＳ０３に進み、上記の如くＳ０１で決定された移動計画を基に、対象物動力学モデルを用いて目標対象物運動軌道（目標対象物位置姿勢軌道）と、目標対象物反力軌道とが仮決定される。
【００５４】
ここで、このＳ０３の処理と、この処理で使用する対象物動力学モデルとを図９のブロック図を参照して説明する。図９は、Ｓ０３で目標対象物運動軌道のうちの目標対象物位置軌道と、目標対象物反力軌道とを求める演算処理を示すブロック図である。このブロック図中の破線で囲んだ部分が対象物１２０に作用する力と対象物１２０の運動との関係を表す対象物動力学モデルとなっている。なお、本実施形態では、理解の便宜上、対象物１２０をほぼ水平な床上で移動させる場合を例に採って説明する。
【００５５】
図９に示す対象物動力学モデルは、対象物１２０に作用させる力（より詳しくは水平方向の並進力）を入力として、対象物１２０の位置を出力する動力学モデルである。より詳しくは、この対象物動力学モデルは、対象物１２０に作用させる水平方向の並進力の入力値（後述の加算部２０４で求められる値）に、対象物１２０の質量Ｍの逆数１／Ｍを乗算部２０６で乗算することにより対象物１２０の運動加速度を求め、これを積分器２０８，２１０で順次積分する（２重積分する）ことにより対象物１２０の位置（対象物動力学モデル上での位置）を出力するものとなっている。つまり、本実施形態では、図９に示す対象物動力学モデルは、対象物１２０に作用する並進力が、対象物１２０の加速度と質量との積に等しいという運動方程式を基に構成されている。なお、積分器２０８の出力は、対象物動力学モデル上での対象物１２０の移動速度を意味しており、以下、これを対象物モデル速度という。
【００５６】
かかる対象物動力学モデルを用いるＳ０３の演算処理を図９を参照して具体的に説明する。まず、前記Ｓ０１で決定された移動計画に基づく対象物１２０の瞬時瞬時の（各時刻の）目標速度である目標対象物速度と、積分器２０８で先に求められた対象物モデル速度とが減算部２００に入力され、それらの偏差（＝目標対象物速度−対象物モデル速度）が求められる。ここで、目標対象物速度は、移動計画における対象物１２０の位置軌道の１階微分値として得られる値、あるいは移動計画の構成要素である。なお、前記図８に示した移動計画の例では、グラフg1の傾きをそのまま目標対象物速度とすればよく、この場合には、目標対象物速度は継続的に一定値となる。そして、現在時刻から将来の所定期間分（ロボット１の複数歩分）の目標対象物速度の時系列が減算部２００に順次入力される。また、減算部２００に入力される対象物モデル速度は、新たに入力する目標対象物速度の１つ前の目標対象物速度を減算部２００に入力したときに積分器２０８から出力された値である。
【００５７】
次いで、上記偏差に所定のゲインＫｖを乗算部２０２で乗算することにより、対象物１２０にロボット１から作用させるべき並進力の要求値が求められる。つまり、本実施形態では、該並進力の要求値は、目標対象物速度と対象物モデル速度との偏差が０に収束するように（対象物モデル速度が目標対象物速度に追従するように）、フィードバック制御則により決定される。そのフィードバック制御則としては、この例では比例制御則が使用される。そして、求められた並進力の要求値の符号を反転させたものの時系列が目標対象物反力軌道として出力される。
【００５８】
また、求められた並進力の要求値と、対象物１２０にロボット１以外から作用する外乱力（床から対象物１２０に作用する摩擦力など）の推定値である推定外乱力とが加算部２０４に入力され、該並進力の要求値と推定外乱力との和が、前記対象物動力学モデルに対する並進力の入力値として加算部２０４により求められる。ここで、推定外乱力は、歩容生成装置１００の演算処理周期毎に後述のＳ３５の処理で求められるものであり、加算部１２０に入力する推定外乱力としては、歩容生成装置１００の前回の演算処理周期で求められた値（前回値）が用いられる。そして、このように求めた並進力の入力値を対象物動力学モデルに入力することで、前記した如く対象物１２０の位置が求められ、この求めた位置の時系列が積分器２１０から目標対象物位置軌道として出力される。
【００５９】
なお、対象物動力学モデルの積分器２０８の出力の初期値は、歩容生成装置１００の前回の演算処理周期でＳ０３の処理を実行して求めた対象物モデル速度の時系列のうちの、前回の演算処理周期に対応する時刻における値に設定される。また、積分器２１０の出力の初期値は、歩容生成装置１００の前回の演算処理周期でＳ０３の処理を実行して求めた目標対象物位置の時系列のうちの、前回の演算処理周期に対応する時刻における値に設定される。
【００６０】
また、目標対象物運動軌道のうちの目標対象物姿勢軌道は、例えば目標対象物速度の向きにほぼ一致するように決定される。
【００６１】
補足すると、移動計画および推定外乱力が一定に維持されている限り、目標対象物運動軌道や目標対象物反力軌道は一定になるので、Ｓ０３の処理は、必ずしも歩容生成装置１００の演算処理周期毎に行なう必要はない。従って、例えば、Ｓ０３の処理は、ロボット１の１歩毎あるいは歩容生成装置１００の演算処理周期の複数周期毎に行なったり、Ｓ０１で移動計画を変更した場合や、推定外乱力が比較的大きく変化した場合に行なうものとしてもよい。
【００６２】
以上説明したＳ０３の処理により、現在時刻から将来の所定期間分の目標対象物運動軌道と目標対象物反力軌道とが仮決定される。
【００６３】
なお、Ｓ０３の処理で用いる対象物動力学モデルは、本発明における対象物動力学モデルではなく、本発明における対象物動力学モデルに相当するものは、後述する対象物動力学モデル２である。
【００６４】
次いで、Ｓ０５に進んで、上記の如く仮決定された目標対象物運動軌道を基に、ロボット１の遊脚足平２２の着地予定位置姿勢および着地予定時刻が仮決定される。ここで仮決定する着地予定位置姿勢および着地予定時刻は、現在、作成しようとしている目標歩容である今回歩容での遊脚足平２２の着地予定位置姿勢および着地予定時刻を含む、ロボット１の複数歩分（少なくとも２歩分）の遊脚足平２２の着地予定位置姿勢および着地予定時刻である。すなわち、仮決定する着地予定位置姿勢および着地予定時刻は、少なくとも今回歩容の遊脚足平２２の着地予定位置姿勢および着地予定時刻と、次回歩容の遊脚足平２２の着地予定位置姿勢および着地予定時刻とを含む。この場合、遊脚足平２２の着地予定位置姿勢および着地予定時刻は、Ｓ０３で決定された目標対象物運動軌道に対して、着地予定時刻での遊脚足平２２の着地位置姿勢がその時刻での対象物位置姿勢と所定の相対的位置姿勢関係になるように決定される。
【００６５】
但し、遊脚足平２２の着地予定位置姿勢および着地予定時刻は、必ずしも目標対象物位置姿勢に追従するような形態で決定する必要はない。例えば、ロボット１の移動を行なわない状態（両足平２２，２２の接地を維持する状態）で、対象物１２０を押して移動させ、その移動開始後は、対象物１２０から腕体５，５の手先部４４を対象物１２０からはずすような場合には、各歩容における遊脚足平２２の着地予定位置姿勢は経時的に一定に維持されることとなる。
【００６６】
なお、遊脚足平２２の着地予定位置姿勢は、より詳しくは、遊脚足平２２の踵を着地してから、該遊脚足平２２を床に接触させたまま、すべらないようにピッチ方向に回転させて該遊脚足平２２の底面のほぼ全面を床面に接触させた状態での該遊脚足平２２の位置姿勢である。従って、本実施形態では、遊脚足平２２Ｒまたは２２Ｌの着地予定位置姿勢は、その着地時から次の遊脚足平２２Ｌまたは２２Ｒの着地までの単位歩容における支持脚座標系の位置姿勢を規定するものとなり、遊脚足平２２の着地予定位置姿勢を決定することで、それに対応して、前記した支持脚座標系の設定手法に従って各単位歩容における支持脚座標系の位置姿勢が決定されることとなる。具体的には、各単位歩容における支持脚座標系は、該単位歩容の一つ前の単位歩容の遊脚足平２２をその着地予定位置姿勢に合致させた状態において、該足平２２を有する脚体２の足首関節の中心から床面に延ばした垂線が該床面と交わる点を原点とする座標系となる。
【００６７】
次いで、Ｓ０７に進んで、今回歩容の目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータが仮決定される。ここで仮決定されるＺＭＰ軌道パラメータは、それにより規定される今回歩容の目標ＺＭＰ（以下、仮目標ＺＭＰという）が、Ｓ０５で仮決定された着地予定位置姿勢および着地予定時刻により定まる、今回歩容での支持脚足平２２の接地面（両脚支持期では、両足平２２の接地面を含む、いわゆる支持多角形）のほぼ中央付近に存在し、且つ、急激な変化をしないように決定される。すなわち、今回歩容の仮目標ＺＭＰが、ロボット１の接地面内（あるいは支持多角形内）の、ロボット１の安定余裕ができるだけ高くなるような位置に存在し、且つ、急激な変化をしないようにＺＭＰ軌道パラメータが決定される。このような指針で決定される仮目標ＺＭＰは、例えば図１０（ａ）で示すようなパターンのものとなる。なお、図１０（ａ）は仮目標ＺＭＰのＸ軸方向位置のパターンを示している。この例の場合には、仮目標ＺＭＰパターン（軌道）の折れ点の位置および時刻がＺＭＰ軌道パラメータとして仮決定される。
【００６８】
次いで、Ｓ０９に進んで、現在仮決定されている目標対象物運動軌道と目標対象物反力軌道とを基に、現在仮決定されているＺＭＰ軌道パラメータにより定まる仮目標ＺＭＰまわりの対象物反力モーメント軌道（対象物反力モーメントの瞬時値の時系列）が算出される。より具体的には、まず、目標対象物運動軌道を基に、ロボット１２０の両腕体５，５の手先部４４Ｒ，４４Ｌを係合させるべき対象物１２０の部位の運動軌道（位置姿勢軌道）が決定される。該部位の位置姿勢は、対象物１２０の位置姿勢に対して所定の位置姿勢関係を有するものとされる。そして、その部位の運動軌道（位置姿勢軌道）に合致するように、ロボット１の手先位置姿勢軌道（これはロボット１に対する対象物反力の作用点の軌道を規定する）が求められる。次いで、この求めた手先位置姿勢軌道と目標対象物反力軌道と仮目標ＺＭＰ軌道とから各時刻（ある刻み時間毎の時刻）の仮目標ＺＭＰまわりの対象物反力モーメントが算出される。そして、その算出された対象物反力モーメントの時系列が対象物反力モーメント軌道として得られる。なお、対象物１２０から離れた箇所からロボット１を対象物１２０の近くに移動させ、その後、手先部４４Ｒ，４４Ｌを対象物１２０に係合させて該対象物１２０を押す作業を開始する場合には、対象物１２０を手先部４４Ｒ，４４Ｌを係合させる時刻（この時刻は適宜決定される）までの対象物反力軌道、ひいては、対象物反力モーメント軌道は０とされる。
【００６９】
次いで、Ｓ１１に進んで、今回歩容に続く周期的歩容としての定常旋回歩容の歩容パラメータが決定される。該歩容パラメータは、定常旋回歩容における足平位置姿勢軌道を規定する足平軌道パラメータ、目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータ、目標対象物反力モーメント軌道を規定する対象物反力軌道パラメータを含む。
【００７０】
これらの歩容パラメータを具体的に説明する前に、定常旋回歩容の概要を説明しておく。なお、以降の説明では、歩容の「初期」、「終端」はそれぞれ歩容の開始時刻、終了時刻もしくはそれらの時刻における瞬時歩容を意味する。
【００７１】
定常旋回歩容は、その歩容を繰り返したときに歩容の境界（本実施形態では１歩毎の歩容の境界）においてロボット１の運動状態（足平位置姿勢、上体位置姿勢等の状態）に不連続が生じないような周期的歩容を意味する。
【００７２】
周期的歩容である定常旋回歩容は、本実施形態では、ロボット１の２歩分の歩容、すなわち今回歩容に続く第１旋回歩容と該第１旋回歩容に続く第２旋回歩容とからなる歩容を該定常旋回歩容の１周期分の歩容として、その１周期分の歩容を繰り返す歩容である。尚、ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。以降、定常旋回歩容を定常歩容と略す場合もある。
【００７３】
定常歩容について補足すると、ロボット１としての２足移動ロボットでは、定常歩容の１周期分は、少なくとも２歩分の歩容（連続する２つの単位歩容）から構成される。なお、３歩以上の歩容を１周期分の歩容とする複雑な定常歩容を設定することも可能である。但し、定常歩容は、後述の如く、今回歩容の終端における発散成分（詳細は後述する）を決定するためだけに用いられる。このため、３歩以上の歩容を１周期とする定常歩容を用いることは、歩容生成の処理が煩雑となるにも関わらず、効果は少ない。そこで、本実施形態での定常歩容の１周期分の歩容を２歩分の歩容（第１及び第２旋回歩容）により構成するようにしている。以下の定常歩容の説明では、説明の便宜上、２歩分の歩容から成る定常歩容を１歩の歩容とみなす。定常歩容は、歩容生成装置１００で今回歩容の終端における発散成分を決定するために暫定的に（歩容生成装置１００の演算処理上で）想定される仮想歩容であり、歩容生成装置１００からそのまま出力されるものではない。
【００７４】
尚、「発散」とは、２足移動ロボット１の上体３の水平位置が両足平２２，２２の位置からかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、２足移動ロボット１の上体３の水平位置が両足平２２，２２の位置（より具体的には、支持脚足平２２の接地面に設定された支持脚座標系の原点）からかけ離れていく具合を表す数値である。
【００７５】
本実施形態では、目標歩容が、前記発散を生じることなく、継続的に生成されるように、発散成分を指標にして歩容を生成する。すなわち、今回歩容の後に続く定常歩容の初期発散成分を決定してから、今回歩容の終端発散成分を定常歩容の初期発散成分に一致させる（より一般的には、今回歩容の上体位置姿勢を定常歩容の上体位置姿勢に収束させる）ように、今回歩容を生成する（今回歩容を規定する歩容パラメータを決定する）。そして、定常歩容の初期発散成分は、Ｓ１１で決定した歩容パラメータを基に、該定常歩容の初期（第１旋回歩容の初期）と終端（第２旋回歩容の終端）とでロボット１の運動状態が一致するという定常歩容の条件（以下、これを定常歩容の境界条件という）をロボット１の動力学モデル上で満たすように決定される。このような歩容生成の基本的な指針は本出願人が先に提案したＰＣＴ国際公開公報WO/02/40224A1のものと同様である。従って、以下の定常歩容に関する説明では、ＰＣＴ国際公開公報WO/02/40224A1の記載事項と異なる技術事項を主体に説明し、同様の技術事項については詳細な説明を省略することがある。
【００７６】
Ｓ１１の説明に戻って、定常歩容の歩容パラメータのうちの足平軌道パラメータは、第１旋回歩容および第２旋回歩容の初期及び終端のそれぞれにおける支持脚足平２２及び遊脚足平２２のそれぞれの位置姿勢、各旋回歩容の歩容周期等から構成され、今回歩容、第１旋回歩容、第２旋回歩容の順に、足平位置姿勢軌道がつながるように決定される。以下に具体的な設定方法を図１１を参照して説明する。
【００７７】
第１旋回歩容初期遊脚足平位置姿勢は、次回歩容の支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とする。この今回歩容終端支持脚足平位置姿勢は、今回歩容初期の支持脚足平２２を床に接触させたまま、すべらないように該支持脚足平２２をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚足平２２の位置姿勢（これは前回歩容の遊脚足平２２の着地予定位置姿勢に一致する）である。なお、次回歩容支持脚座標系の位置姿勢は、図１１に示す如く、今回歩容の遊脚足平２２の着地予定位置姿勢に対応して決定される。
【００７８】
第１旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とする。この今回歩容終端遊脚足平位置姿勢は、今回歩容の遊脚足平２２の着地予定位置姿勢（あるいはこれに対応して定まる次回歩容支持脚座標系）に対応した位置姿勢に決定される。具体的には、今回歩容の遊脚足平２２を今回歩容終端遊脚足平位置姿勢から床に接触させたまま、すべらないようにピッチ方向に所定角度、回転させたときの位置姿勢が今回歩容の遊脚足平２２の着地予定位置姿勢になるように今回歩容終端遊脚足平位置姿勢が決定される。
【００７９】
第１旋回歩容終端遊脚足平位置姿勢は、次回歩容支持脚座標系から見た次回歩容終端遊脚足平位置姿勢とする。この次回歩容終端遊脚足平位置姿勢は、今回歩容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容の遊脚足平２２の着地予定位置姿勢（あるいはこれに対応して定まる次次回歩容支持脚座標系）に対応した位置姿勢に決定される。
【００８０】
第１旋回歩容終端支持脚足平位置姿勢は、次回歩容支持脚座標系に位置姿勢を合致させた支持脚足平２２を床に接触させたまま、すべらないように該支持脚足平２２をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚足平２２の位置姿勢（この位置姿勢は、今回歩容の遊脚足平２２の着地予定位置姿勢に一致する）とする。
【００８１】
第２旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端支持脚足平位置姿勢とする。第２旋回歩容初期支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端遊脚足平位置姿勢とする。
【００８２】
第２旋回歩容終端遊脚足平位置姿勢は、それを次次回歩容支持脚座標系から見た位置姿勢が、今回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢に一致するように決定される。第２旋回歩容終端支持脚足平位置姿勢は、それを次次回歩容支持脚座標系から見た位置姿勢が、今回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢に一致するように決定される。
【００８３】
第１旋回歩容および第２旋回歩容の歩容周期は、次回歩容周期（これは今回歩容の遊脚足平２２の着地予定時刻と次回歩容の遊脚足平２２の着地予定時刻との差である）と同一に設定される。これらの第１旋回歩容及び第２旋回歩容の歩容周期は、互いに同一にすることは必ずしも必要ではないが、いずれの周期も、少なくとも次回歩容周期に応じて決定するのが好ましい。尚、今回歩容、第１旋回歩容および第２旋回歩容の上記以外の運動パラメータ（両脚支持期時間などの時間パラメータを含む）は、上記決定されたパラメータに応じて、歩容の条件（アクチュエータの速度が許容範囲に入っているか、可動角を超えていないか、床などと干渉していないかなど）を満足するように適宜決定する。
【００８４】
定常歩容の歩容パラメータのうちのＺＭＰ軌道パラメータは、前記Ｓ０７で目標ＺＭＰ軌道パラメータを仮決定した場合と同様に、該ＺＭＰ軌道パラメータにより規定されるＺＭＰ軌道が、ロボット１の安定余裕が高くなり、且つ、急激な変化をしないように決定される。
【００８５】
また、定常歩容の対象物反力モーメント軌道パラメータは、Ｓ０９で算出された対象物反力モーメント軌道を基に、周期性の条件を満足するように決定される。例えば、今回歩容、次回歩容および次次回歩容に対応する対象物反力モーメント軌道が図１２（ａ）に示すようになっているとする。ここで、今回歩容の対象物反力モーメント軌道はＳ０９で求められた軌道である。次回歩容の対象物反力モーメント軌道は、Ｓ０３で求めた次回歩容の対象物反力軌道および目標対象物運動軌道と、定常旋回歩容のＺＭＰ軌道パラメータにより規定されるＺＭＰ軌道とを基にＳ０９と同様に算出される軌道である。次次回歩容の対象物反力モーメント軌道は、Ｓ０３で求めた次次回歩容の対象物反力軌道および目標対象物運動軌道と、定常旋回歩容のＺＭＰ軌道パラメータにより規定されるＺＭＰ軌道とを基にＳ０９と同様に算出される軌道である。
【００８６】
このとき、定常旋回歩容の対象物反力モーメント軌道は、例えば図１２（ｂ）で示す軌道になるように、定常旋回歩容の対象物反力モーメント軌道パラメータが決定される。この例では、定常旋回歩容の第１旋回歩容の初期から終端までの対象物反力モーメント軌道は、今回歩容の対象物反力モーメント軌道に連続して、次回歩容の対象物反力モーメント軌道と一致するように決定されている。また、定常旋回歩容の第２旋回歩容の対象物反力モーメント軌道は、第１旋回歩容の対象物反力モーメント軌道に連続すると共に、第２旋回歩容の初期から終端の直前のある時刻ｔxまでは、次次回歩容の対象物反力モーメント軌道と一致し、時刻ｔxから終端までは、時刻ｔxの値から、第１旋回歩容の初期（定常旋回歩容の初期）の値と同じ値まで連続的に変化するように決定されている。時刻ｔxは、例えば定常旋回歩容のＺＭＰ軌道の折れ点の時刻に対応する。
【００８７】
なお、次回歩容の初期の対象物反力モーメントの値と、次次回歩容の終端の対象物反力モーメントの値との差がさほど大きくないときには、必ずしも、定常旋回歩容の初期の対象物反力モーメントと終端の対象物反力モーメントの値とを同じにしなくてもよい。
【００８８】
次いで、Ｓ１３に進み、定常旋回歩容の初期発散成分が決定される。この場合、定常旋回歩容は周期的歩容であるから、ロボット１のあらかじめ定めた動力学モデル上で、第１旋回歩容の運動の初期状態（初期発散成分を含む）と第２旋回歩容の運動の終端状態（終端発散成分を含む）とが一致するように定常旋回歩容の初期発散成分が決定される。
【００８９】
このＳ１３の処理を詳細に説明する前に、まず、このＳ１３での処理に用いるロボット１の動力学モデルと、この動力学モデルを使用して目標上体位置を決定するための動力学演算とを説明する。この動力学モデルは、Ｓ１３の処理に用いられるほか、後述のＳ３７までの処理で今回歩容を決定するために用いられ、ロボット１の動力学的挙動（ロボット１に作用する力と運動との関係）を単純化して（近似的に）表現した動力学モデルである。以下、この動力学モデルをロボット単純化モデルという。なお、このロボット単純化モデルの基本構造は、ＰＣＴ国際公開公報WO/02/40224A1と同様であるが、対象物反力モーメントが加味されている点で同公報のものと相違している。
【００９０】
図１３はロボット単純化モデルの構造を示している。図示の如く、このロボット単純化モデルは、３質点モデルであり、脚体２の動力学と上体３の動力学とが相互に非干渉に構成されると共に、ロボット１の全体の動力学は、それらの線形結合で表される。
【００９１】
このロボット単純化モデルは、倒立振子Ａと２つの脚質点２ｍ，２ｍ（支持脚質点および遊脚質点）とから構成される。各脚質点２ｍは、各脚体２に対応する質点である。この各脚質点２ｍは、それぞれに対応する脚体２の足平２２に任意に固定的に設定されたローカル座標系での固定点であり、その位置は、各足平位置姿勢に対応して一義的に定まるものとされている。例えば、各足平質点２ｍは、その足平２２の底面の代表点から足首関節の中心に向かって所定の距離だけ該足首関節側にずれた点に設定される。
【００９２】
倒立振子Ａは、水平に移動するフリーの支点ａと、一つの質点ｂと、支点ａと質点ｂとを結ぶ質量の無い可変長のリンクｃとから構成される。この場合、リンクｃが傾いてもリンクｃが伸縮し、支点ａから見た質点ｂの高さｈが一定値に保たれるものとする。
【００９３】
この倒立振子Ａの質点ｂ（以下、単に倒立振子質点ｂという）は、ロボット３の上体３の質点に相当するものであり、以降、倒立振子質点ｂを上体質点ｂということもある。倒立振子質点ｂの位置を以降、略して倒立振子位置という。なお、上体質点ｂの質量は、上体３の質量と腕体５，５および頭部４の質量を含んでいる。
【００９４】
ロボット１の上体３の水平位置は、倒立振子質点ｂの水平位置から幾何学的に決定される。具体的には、例えば、上体３の代表点の水平位置（支持脚座標系で見たＸＹ座標）、すなわち上体位置の水平成分が、倒立振子質点ｂの水平位置に一致するものとされる。換言すれば、図１３に示すように、上体３の代表点と倒立振子質点ｂとは、同一鉛直線上にあるものとされる。
【００９５】
なお、倒立振子ＡのＺＭＰは、フリーの支点ａの位置にあるので（支点ａのまわりにはモーメントが発生しない）、以降、倒立振子Ａの支点ａの位置を、倒立振子ＺＭＰと呼び、ZMPpendと記述する。
【００９６】
かかるロボット単純化モデルを数式表現するために、該モデルに関する変数およびパラメータを以下のように定義する。
【００９７】
msup：支持脚質点２ｍの質量、  mswg：遊脚質点２ｍの質量、  mb：倒立振子質点ｂの質量、  mtotal：ロボット１の全体質量（＝ｍsup＋ｍswg＋ｍb）、  mfeet：両脚体２，２の総質量（＝ｍsup＋ｍswg）、  xsup：支持脚質点２ｍの位置、  xswg：遊脚質点２ｍの位置、  xb：倒立振子位置（上体質点ｂの位置）、  ｈ：倒立振子高さ（倒立振子Ａの支点ａから倒立振子質点ｂまでの高さ）
以降、ことわらない限り、xb，xsup，xswgは３次元ベクトル（ＸＹＺ座標ベクトル）で表す。また、Ｘを任意の変数としたとき、dX/dtはＸの１階微分を表し、d2X/dt2はＸの２階微分を表す。ｇは重力加速度定数を示す。Ｇは、そのＸ成分、Ｙ成分、Ｚ成分がそれぞれ０、０、−ｇである重力加速度ベクトルとする。
【００９８】
図１３のロボット単純化モデルにおいて、両脚質点２ｍ，２ｍの総慣性力（両脚質点２ｍ，２ｍの運動による慣性力と重力との合力）がある作用点Ｐまわりに作用するモーメントを点Ｐまわりの脚総慣性力モーメントと定義し、その作用点Ｐの座標（位置）をxpとする。
【００９９】
下記の式０１は、点Ｐまわりの脚総慣性力モーメントの動力学的定義式である。
【０１００】
点Ｐまわりの脚総慣性力モーメント
＝msup(xsup−xp)＊Ｇ−msup(xsup−xp)＊d2xsup/dt2
＋mswg(xswg−xp)＊Ｇ−mswg(xswg−xp)＊d2xswg/dt2                ……式０１
脚ＺＭＰをZMPfeetと記述し、式０２で定義する。ただし、脚ＺＭＰの高さ（ZMPfeetのＺ成分）は、前記点Ｐの高さと同一とする。この脚ＺＭＰは、両脚体２，２の運動によって発生する慣性力と重力との合力に擬似的に対応させた値である。
【０１０１】
点Ｐまわりの脚総慣性力モーメント＝mfeet＊(ZMPfeet−xp)＊Ｇ      ……式０２
なお、前記作用点Ｐは、ロボット単純化モデルの近似精度が高くなるように設定される。例えば、今回歩容に係る作用点Ｐは、前回歩容の支持脚座標系の原点から、今回歩容の支持脚座標系の原点に、両脚支持期の間に直線的に等速移動し、該両脚支持期に続く片脚支持期では今回歩容支持脚座標系の原点に維持されるように設定される。このことは、定常旋回歩容の第１旋回歩容、第２旋回歩容に係る作用点Ｐについても同様である。
【０１０２】
また、目標ＺＭＰまわりの対象物反力モーメントをロボット１の全体質量mtotalで除算したものを、対象物反力ＺＭＰと定義し、これをZMPobjと記述する。すなわち、ZMPobjを次式０３で定義する。
【０１０３】
ZMPobj＝目標ＺＭＰまわりの対象物反力モーメント／mtotal              ……式０３
このとき、目標ＺＭＰと脚ＺＭＰ（ZMPfeet）と倒立振子ＺＭＰ(ZMPpend）と対象物反力ＺＭＰとの間には、近似的に次式０４の線形関係が成り立つ。
【０１０４】
ZMPpend＝mtotal／mb＊目標ＺＭＰ−mfeet／mb＊ZMPfeet
−mtotal／mb＊ZMPobj            ……式０４
なお、式０４は、サジタルプレーン（ＸＺ平面）上での関係式であり、ラテラルプレーン（ＹＺ平面）上では、式０３の右辺第３項の符号が「−」から「＋」に反転する。
【０１０５】
また、倒立振子Ａの挙動を表す微分方程式は、次式０５で表される。
【０１０６】
d2xb/dt2の水平成分＝g／h＊(xbの水平成分−ZMPpendの水平成分)        ……式０５
以上の式０１〜０５が図１３のロボット単純化モデルの動力学を記述する式である。
【０１０７】
このようなロボット単純化モデルを用いることで、次のような動力学的演算により、目標足平位置姿勢と目標ＺＭＰまわりの対象物反力モーメントと目標ＺＭＰと目標上体姿勢とから目標上体位置を決定できる。
【０１０８】
その動力学演算を図１４のブロック図を参照して説明しておく。図１４はこの動力学演算を示すブロック図である。目標両足平位置姿勢（支持脚足平２２および遊脚足平２２の目標位置姿勢）の軌道と、前記の如く設定される作用点Ｐとから、脚ＺＭＰ算出器２２０により、前記式０１および０２を用いて脚ＺＭＰ(ZMPfeet)が算出される。
【０１０９】
また、目標ＺＭＰまわりの対象物反力モーメントに、乗算部２２２で１／mtotalを乗算することで、対象物反力ＺＭＰ(ZMPobj)が算出される。そして、算出したZMPfeetに乗算部２２４でmfeet／mtotalを乗算したものとZMPobjとを減算部２２６で目標ＺＭＰから減算し、さらにその減算結果に乗算部２２８でmtotal／mbを乗算することによって、前記式０４の右辺の演算が行なわれる。これにより倒立振子ＺＭＰ(ZMPpend）が算出される。なお、上記の演算処理により求められるZMPpendは、サジタルプレーン上のものであり、ラテラルプレーン上のZMPpendを算出する場合には、乗算部２２２の演算結果の符号を反転させたものを減算部２２６に入力すればよい。
【０１１０】
このようにして算出されたZMPpendを倒立振子Ａに入力することで、前記式０５に基づいて、倒立振子水平位置xbが算出される。さらに、この倒立振子水平位置xbと目標上体姿勢とを基に、上体位置決定器２３０により目標上体位置が決定される。この場合、上体位置決定器２３０は、倒立振子水平位置xbを目標上体水平位置とする。また、目標上体鉛直位置は、例えば本出願人が先に特開平１０−８６０８０号公報で提案した上体高さ決定手法によって、目標上体姿勢などを基に決定される。
【０１１１】
図５の説明に戻って、以下に、Ｓ１３の処理を詳細に説明する。
【０１１２】
Ｓ１３の処理では、前記したロボット単純化モデルを基に、定常歩容の初期発散成分を決定する。歩容における発散成分をｑとおき、前記倒立振子Ａの質点ｂの水平速度（あるいは上体水平速度）vbとおくと、ｑは、本実施形態では次式０６により定義される。
【０１１３】
ｑ＝xb＋vb／ω0      ……式０６
ただし、ω0は、倒立振子Ａの固有周波数、すなわち、ｇ／ｈの平方根である。
【０１１４】
なお、このように定義される発散成分の技術的意味については、ＰＣＴ国際公開公報WO/02/40224A1に詳細に説明されているので、ここでの説明は省略する。
【０１１５】
このように発散成分ｑを定義したとき、倒立振子Ａの運動方程式（前記式０５）を離散化して、ｑに関して解き、それに前記式０４を適用すると、次式０７が得られる。なお、目標ＺＭＰをZMPtotalと記述する。
【０１１６】
ｑ[k]＝exp(ω0kΔt)＊ｑ[0]
＋exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mtotal／mb＊ZMPtotal[i])
−exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mfeet／mb＊ZMPfeet[i])
−exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mtotal／mb＊ZMPobj[i])
…式０７
ただし、exp(  )は、自然対数の底ｅの指数関数を意味する。また、Δｔは離散系の刻み時間、ｋはステップ数（ｋΔｔが時刻を意味する）である。また、式０７のΣ(  )は、ｉ＝０からｉ＝ｋ−１までの累積加算演算を意味している。
【０１１７】
なお、式０７はサジタルプレーン上での発散成分ｑを記述する式であり、ラテラルプレーン上での発散成分ｑを記述する式は、式０７の右辺の第４項の全体の符号を「−」から「＋」に反転させた式となる。
【０１１８】
ここで、定常旋回歩容の初期（第１旋回歩容の初期）における発散成分（以下、初期発散成分という）と終端（第２旋回歩容の終端）における発散成分（以下、終端発散成分という）に着目し、ｑ[0]（時刻０における発散成分）を初期発散成分、ｑ[k]（時刻ｋΔｔにおける発散成分）を終端発散成分とする。
【０１１９】
このとき、式０７の右辺第１項は、初期発散成分によって発生する終端発散成分を表す。右辺第２項は、目標ＺＭＰパターンによって発生する終端発散成分を表す。右辺第３項は、両脚体２，２の運動（両脚質点２ｍ，２ｍの運動）によって発生する終端発散成分を表す。右辺第４項は、対象物反力モーメントパターンによって発生する終端発散成分を表す。以降、右辺第２項をWzmptotal、右辺第３項をWfeet、右辺第４項をWobjと記述する。すなわち、Wzmptotal、Wfeet、Wobjを次式０８ａ，０８ｂ，０８ｃで定義する。
【０１２０】
Wzmptotal
＝exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mtotal／mb＊ZMPtotal[i])
……式０８ａ
Wfeet
＝−exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mfeet／mb＊ZMPfeet[i])
……式０８ｂ
Wobj
＝−exp(ω0kΔt)＊(exp(−ω0Δt)−１)＊Σ(exp(−iω0Δt)＊mtotal／mb＊ZMPobj[i])
……式０８ｃ
なお、式０８ｃは、サジタルプレーン上でのWobjを定義する式であり、ラテラルプレーン上でのWobjは、式０８ｃの右辺の符号を反転させたものとなる。
【０１２１】
これらのWzmptotal、Wfeet、Wobjを用いて前記０７を書き換えると、次式０９となる。
【０１２２】
ｑ[k]＝exp(ω0kΔt)＊ｑ[0]＋Wzmptotal＋Wfeet＋Wobj            ……式０９
本実施形態では、定常旋回歩容の境界条件を満足する初期発散成分は、今回歩容に続く定常旋回歩容の支持脚座標系（図１１の次回歩容支持脚座標系（Ｘ'Ｙ'座標系））で見た初期発散成分が、該定常旋回歩容の次の歩容の支持脚座標系（図１１の次次次回歩容支持脚座標系（X'''Ｙ'''座標系））から見た定常旋回歩容（今回歩容に続く定常旋回歩容）の終端発散成分に一致するように、上記式０８ａ，０８ｂ，０８ｃ，０９に基づいて解析的に求められる。
【０１２３】
ここで、定常旋回歩容の次の歩容の支持脚座標系（図１１の次次次回歩容支持脚座標系（X'''Ｙ'''座標系））から見た該定常旋回歩容の終端発散成分をｑ'[k]とおくと、定常旋回歩容の境界条件を満足するためには、次式１０が成立しなければならない。
【０１２４】
ｑ[0]＝ｑ'[k]        …式１０
また、定常旋回歩容の次の歩容の支持脚座標系（図１１の次次次回歩容支持脚座標系（X'''Ｙ'''座標系））で記述される値を、該定常旋回歩容の支持脚座標系（図１１の次回歩容支持脚座標系（Ｘ'Ｙ'座標系））で記述される値に座標変換するための回転行列をＭ、座標原点の平行移動ベクトルをＢとおくと、次式１１が得られる。
【０１２５】
ｑ[k]＝Ｍｑ'[k]＋Ｂ      ……式１１
従って、これらの式１０、１１から次式１２が得られる。
【０１２６】
ｑ[k]＝Ｍｑ[0]＋Ｂ      ……式１２
この式１２は、定常歩容の境界条件を満足するために、初期発散成分ｑ[0]と終端発散成分ｑ[k]とが満たすべき条件である。
【０１２７】
そして、この式１２と前記式０８ａ〜０８ｃおよび０９とから、次式１３が得られる。
【０１２８】
ｑ[0]＝inv(Ｍ−exp(ω0kΔt)Ｉ)(Wzmptotal＋Wfeet＋Wobj−Ｂ）    ……式１３
なお、式１３において、inv(  )は、括弧内の行列の逆行列であり、Ｉは単位行列である。
【０１２９】
本実施形態では、この式１３に基づいて、定常旋回歩容の境界条件を満足する初期発散成分ｑ[0]を決定する。
【０１３０】
具体的には、前記Ｓ１１で決定した定常旋回歩容のＺＭＰ軌道パラメータを基に、定常旋回歩容の各時刻ｉΔｔ（ｉ＝０，１，……，ｋ−１）における目標ＺＭＰの瞬時値であるZMPtotal[i]（ｉ＝０，１，……，ｋ−１）を求め、これを用いて前記式０８ａの右辺の演算を行なうことで、Wzmptotalを算出する。なお、Wzmptotalの算出は、ＰＣＴ国際公開公報WO/02/40224A1に説明されている如く、目標ＺＭＰ軌道が折れ線軌道であることを利用したアルゴリズムによって算出するようにしてもよい。
【０１３１】
また、前記Ｓ１１で決定した定常旋回歩容の足平軌道パラメータを基に、定常旋回歩容の各時刻ｉΔｔ（ｉ＝０，１，……，ｋ−１）における各足平位置姿勢の瞬時値を求め、それを、前記式０１，０２を離散系で表現した式に適用することで、ZMPfeet[i]を求める。なお、足平位置姿勢は、例えば本出願人が特許第３２３３４５０号にて提案した有限時間整定フィルタを用いて各足平２２毎に算出される。この場合、算出される足平位置姿勢の軌道は、定常旋回歩容の第１旋回歩容および第２旋回歩容のそれぞれにおける遊脚足平２２が片脚支持期の開始時刻から上昇して、該遊脚足平２２の着地予定位置に向かって移動し、該遊脚足平２２の着地予定時刻にて該遊脚足平２２がその踵で着地予定位置に対応する位置に着地するような軌道である。そして、上記の如く求めたZMPfeetl[i]を用いて前記式０８ｂの右辺の演算を行なうことで、Wfeetを算出する。補足すると、遊脚足平２２の着地予定位置姿勢が、１つ前の歩容における支持脚足平２２の着地位置姿勢と同じである場合には、足平位置姿勢軌道は、一定の着地位置姿勢に維持される軌道とされる。但し、遊脚足平２２を一旦上昇させ、その後、元の着地位置姿勢に戻すような足平位置姿勢軌道を生成するようにしてもよい。
【０１３２】
さらに、前記Ｓ１１で決定した定常旋回歩容の対象物反力モーメント軌道パラメータを基に、定常旋回歩容の各時刻ｉΔｔ（ｉ＝０，１，……，ｋ−１）における、目標ＺＭＰまわりの対象物反力モーメントを求めると共に、それをmtotalで除算することにより、ZMPobj[i]を求め、その求めたZMPobj[i]を用いて前記式０８ｃの右辺の演算を行なうことで、Wobjを算出する。
【０１３３】
なお、Wzmptotal、Wfeet、Wobjの算出で用いる刻み時間Δｔは、歩容生成装置１００の演算処理周期と同一であることが好ましいが、それよりも長い時間に設定して、演算処理時間を短縮するようにしてもよい。
【０１３４】
そして、上記の如く求めたWzmptotal、Wfeet、Wobjから、前記式１２の右辺の演算を行なうことで、定常旋回歩容の境界条件を満足する初期発散成分ｑ[0]が算出される。
【０１３５】
以上が、本実施形態におけるＳ１３の処理の詳細である。
【０１３６】
なお、本実施形態では、初期発散成分ｑ[0]を解析的に求めるようにしたが、例えばＰＣＴ国際公開公報WO/02/40224A1の第２実施形態で説明されている如く、探索的な手法により、今回歩容に続く定常旋回歩容の次の歩容の支持脚座標系から見た該定常旋回歩容の終端発散成分にほぼ一致するような該定常旋回歩容の初期発散成分を決定するようにしてもよい。
【０１３７】
図５のフローチャートの説明に戻って、上記の如くＳ１３の処理を実行した後、Ｓ１５に進んで、今回歩容の歩容パラメータが仮決定される。該歩容パラメータは、今回歩容における足平位置姿勢軌道を規定する足平軌道パラメータ、目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータ、目標対象物反力モーメント軌道を規定する対象物反力軌道パラメータを含む。
【０１３８】
今回歩容の歩容パラメータのうちの足平軌道パラメータは、今回歩容の初期及び終端のそれぞれにおける支持脚足平２２及び遊脚足平２２のそれぞれの位置姿勢、今回歩容の歩容周期等から構成される。この場合、今回歩容初期遊脚足平位置姿勢は、今回歩容支持脚座標系から見た、前回歩容終端における今回歩容の遊脚足平位置姿勢（前回歩容の支持脚足平位置姿勢）とする。今回歩容初期支持脚足平位置姿勢は、今回歩容支持脚座標系から見た、前回歩容終端における今回歩容の支持脚足平位置姿勢（前回歩容の遊脚足平位置姿勢）とする。今回歩容終端遊脚足平位置姿勢は、今回歩容の遊脚足平２２の着地予定位置姿勢に対応させて決定される。具体的には、今回歩容の遊脚足平２２を今回歩容終端遊脚足平位置姿勢から床に接触させたまま、すべらないようにピッチ方向に所定角度、回転させたときの位置姿勢が今回歩容の着地予定位置姿勢になるように今回歩容終端遊脚足平位置姿勢が決定される。今回歩容終端支持脚足平位置姿勢は、今回歩容支持脚座標系に位置姿勢を合致させた支持脚足平２２を床に接触させたまま、すべらないように該支持脚足平２２をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚足平２２の位置姿勢（この位置姿勢は、前回歩容の遊脚足平２２の着地予定位置姿勢に一致する）とする。
【０１３９】
なお、本実施形態では、今回歩容終端における支持脚足平２２は、床面にほぼ平行な姿勢になるが、必ずしもこのようにする必要はない。例えば歩容終端において、支持脚足平２２の踵側が床面から離れるようにしてもよい。
【０１４０】
今回歩容の歩容パラメータのうちのＺＭＰ軌道パラメータは、Ｓ０７で仮決定されたＺＭＰ軌道パラメータと同一に設定される。従って、このＺＭＰ軌道パラメータにより規定される目標ＺＭＰ軌道は、例えば前記図１０（ａ）に示したようなパターンのものである。
【０１４１】
また、今回歩容の歩容パラメータのうちの対象物反力モーメント軌道パラメータは、それにより規定される対象物反力モーメント軌道が、前記Ｓ０９で算出した対象物反力モーメント軌道のうちの今回歩容に対応する軌道に一致するように決定される。
【０１４２】
次いで、Ｓ１７に進んで、定常旋回歩容に今回歩容が収束するように、Ｓ１５で仮決定した歩容パラメータのうちのＺＭＰ軌道パラメータを修正する。より詳しくは、定常旋回歩容の初期発散成分に今回歩容の終端発散成分がほぼ一致するようにＺＭＰ軌道パラメータが修正される。
【０１４３】
本実施形態では、定常旋回歩容の初期発散成分に今回歩容の終端発散成分をほぼ一致させるようにするために、前記仮目標ＺＭＰに図１０（ｂ）に示すようなＺＭＰ修正量を加えることにより、該仮目標ＺＭＰを修正し、それにより、図１０（ｃ）に示すような目標ＺＭＰを得る。
【０１４４】
この場合、仮目標ＺＭＰの修正処理の基本的指針は、ＰＣＴ国際公開公報WO/02/40224A1と同様であるが、対象物反力モーメントが考慮される点で同出願のものと相違している。以下にその相違する点を中心にＳ１７の処理を具体的に説明する。なお、この処理で使用する変数、記号のうち、定常歩容の初期発散成分を決定する処理（Ｓ１３）に関して説明したものと同様の意味を持つ変数については、定常歩容に関して先に定義したものと同じ変数、記号を用いてＳ１７の処理を説明する。また、以下の説明では、理解の便宜上、ＺＭＰのＸ軸方向成分の修正に関して説明を行なう。
【０１４５】
本実施形態では、ＺＭＰ修正量は、図１０（ｂ）に示す如く台形状とされ、その高さをａとおく。なお、台形の折れ点の時刻は、図示の如く、仮目標ＺＭＰ軌道の折れ点の時刻に合わせて設定される。本実施形態では、ＺＭＰ修正量は、今回歩容の片脚支持期で発生させる。また、前記Ｓ１３で決定した初期発散成分を今回歩容支持脚座標系から見た値に座標変換したものをｑ''とおく。すなわち、今回歩容支持脚座標系で見た定常旋回歩容の初期発散成分をｑ''とおく。また、仮目標ＺＭＰ軌道によって発生する今回歩容の終端発散成分をWzmptmp、ａ＝１である場合のＺＭＰ修正量パターンによって発生する今回歩容の終端発散成分をWtrimとおく。
【０１４６】
このとき、修正後の目標ＺＭＰ軌道によって発生する終端発散成分をWzmptotalとおくと、近似的に次式１４が得られる。
【０１４７】
Wzmptotal＝Wzmptmp＋ａ＊Wtrim          ……式１４
従って、目標ＺＭＰを修正した後の今回歩容の終端発散成分ｑ''は、定常歩容に関して説明した前記式０９と同様の次式１５により表される。
【０１４８】
ｑ''＝exp(ω0kΔt)＊ｑ[0]＋Wzmptmp＋ａ＊Wtrim＋Wfeet＋Wobj        ……式１５
なお、式１５において、ｋΔｔは今回歩容の終端時刻、ｑ[0]は今回歩容の初期発散成分、Wfeetは今回歩容の両脚体２，２の運動によって発生する終端発散成分、Wobjは今回歩容の対象物反力モーメント軌道によって発生する終端発散成分である。
【０１４９】
この式１５から、ＺＭＰ修正量の台形高さａを決定するための次式１６が得られる。
【０１５０】
ａ＝(ｑ''−exp(ω0kΔt)＊ｑ[0]−Wzmptmp−Wfeet−Wobj)／Wtrim      ……式１６
そこで、本実施形態では、式１６により、ＺＭＰ修正量の台形高さａを求める。この場合、初期発散成分ｑ[0]は今回歩容初期の倒立振子位置（あるいは上体位置）とその変化速度とから、前記式０６により算出される。また、Wzmptmpは、Ｓ１５で仮決定されたＺＭＰ軌道パラメータにより規定される仮目標ＺＭＰ軌道から、定常旋回歩容に係るWzmptotalを求めた場合と同様に求められる。Wtrimは、台形高さａを１として定まるＺＭＰ修正量パターンから、定常旋回歩容に係るWzmptotalを求めた場合と同様に求められる。Wfeetは、Ｓ１５で仮決定された足平軌道パラメータから、定常旋回歩容に係るWfeetを求めた場合と同様に前記式０８ｂに基づいて求められる。Wobjは、Ｓ１５で仮決定された対象物反力モーメント軌道パラメータから、定常旋回歩容に係るWobjを求めた場合と同様に前記式０８ｃに基づいて求められる。
【０１５１】
Ｓ１７では、このようにして求めた台形高さａにより定まるＺＭＰ修正量を仮目標ＺＭＰ軌道に加えることで、ＺＭＰ軌道パラメータが修正される。
【０１５２】
なお、本実施形態では、ＺＭＰ修正量を解析的に求めるようにしたが、探索的に求めるようにしてもよい。この場合には、台形高さａの値を適当な値に設定したＺＭＰ修正量により仮目標ＺＭＰを修正してなる目標ＺＭＰ軌道を用いて、今回歩容を仮に作成し、その仮作成した今回歩容の終端発散成分と定常歩容の初期発散成分との差などに応じて、その差が小さくなるように台形高さａの値を修正する、という処理を繰り返す。そして、最終的に仮作成した今回歩容の終端発散成分と定常歩容の初期発散成分とがほぼ一致するときの台形高さａの値によってＺＭＰ修正量を決定すればよい。
【０１５３】
Ｓ１７の処理の後、Ｓ１９に進んで修正後のＺＭＰ軌道パラメータにより定まる目標ＺＭＰが適正か否かが判断される。具体的には、修正後の目標ＺＭＰが、今回歩容の支持脚足平２２の接地面から逸脱したり、接地面の境界付近に偏ったりしていない場合、すなわち、ロボット１の安定余裕が確保できる場合には、目標ＺＭＰが適正であると判断され、そうでない場合には、目標ＺＭＰが不適正であると判断される。
【０１５４】
Ｓ１９の判断結果がＮＯである場合には、Ｓ２１に進んで、今回歩容パラメータのうちのＺＭＰ軌道パラメータ以外のパラメータに係わる要素、例えば対象物反力モーメント軌道パラメータに係わる目標対象物反力軌道を修正する。この場合、目標対象物反力軌道は、その修正後にＳ０９〜Ｓ１７の処理を再度実行したときに、ＺＭＰ修正量ができるだけ小さくなるように（少なくとも目標対象物反力軌道の修正前に求めたＺＭＰ修正量よりも小さくなるように）修正される。
【０１５５】
この目標対象物反力軌道の修正例を以下に説明する。例えば図１５（ａ）のグラフg2で示すように移動計画に基づく目標対象物運動軌道（位置軌道）が時刻ｔ1からＸ軸方向で対象物１２０の減速を行なって、時刻ｔ3で停止させるような軌道であるとする。なお、図１５（ａ）のグラフg3は、目標対象物運動軌道に対応して決定される目標上体位置軌道の例である。このとき、時刻t1からの対象物１２０の減速加速度が比較的大きい場合には、その時刻t1以後の、ある現在時刻ｔ2における今回歩容に係るＺＭＰ修正量がＸ軸の正方向に過大になって、支持脚足平２２の接地面を逸脱する場合がある（この場合、Ｓ１９の判断結果がＮＯになる）。そして、仮に、そのまま目標歩容を生成し続けた場合には、図示のようにロボット１の上体３がある時刻ｔ4で対象物１２０に衝突するような目標歩容が生成されてしまう。
【０１５６】
そこで、このような場合には、目標対象物反力を、前記したようにＳ０３で仮決定された目標対象物反力軌道に対して、図１５（ｂ）に示す如く、現在時刻ｔ2以降のある期間ΔＴ1内で、Ｘ軸の負方向に増加させるように該目標対象物反力軌道を修正する。換言すれば、ロボット１から対象物１２０に加える作用力（目標対象物反力の符号を反転させた力）をＸ軸の正方向に増加させるように目標対象物反力を修正する。この場合、目標対象物反力を増加させる期間ΔＴ1あるいはその増加分ΔＦ1の大きさは、修正後の目標対象物反力を基に前記Ｓ０９〜Ｓ１７の処理により決定されるＺＭＰ修正量ができるだけ小さくなって、Ｓ１９の判断結果がＹＥＳになるように決定される。このようなΔＴ1あるいはΔＦ1は、解析的あるいは探索的に求めることが可能である。
【０１５７】
このように目標対象物反力軌道を修正することにより、ロボット１が積極的に対象物１２０を押すような上体３の運動が行なわれるように今回歩容パラメータのうちの対象物反力モーメント軌道パラメータがＳ１５で決定されることとなる。その結果、この今回歩容パラメータを基に生成される目標歩容が、安定余裕を確保しつつ、対象物１２０にロボット１の上体３が衝突することが無いようなような歩容になる。
【０１５８】
Ｓ２１の処理を行なった後、Ｓ０９〜Ｓ１７の処理が前記したように再度実行される。このとき、Ｓ１７の次のＳ１９の判断結果はＹＥＳになり、この場合には、次に図６のＳ２３に進む。
【０１５９】
Ｓ２３では、現在の今回歩容パラメータと定常歩容パラメータとを基に、現在時刻から定常歩容の第２旋回歩容終端までの（今回歩容の１歩を含めて３歩目までの）目標上体位置姿勢軌道を算出すると共に、目標対象物運動軌道を基に目標手先位置姿勢軌道を算出する。
【０１６０】
この場合、今回歩容、定常旋回歩容の第１旋回歩容、第２旋回歩容の順に、各歩容の歩容パラメータを基に、前記図１４のブロック図を参照して説明した如く、目標上体位置姿勢軌道を算出する。さらに、目標手先位置姿勢軌道は、今回歩容、定常旋回歩容の第１旋回歩容、第２旋回歩容の順に、Ｓ０９で求めた場合と同様に、目標対象物運動軌道を基に算出する。この場合、定常旋回歩容の第１旋回歩容、第２旋回歩容に対応する目標対象物運動軌道は、Ｓ０３で仮決定された目標対象物運動軌道のうちの、次回歩容、次次回歩容に対応する目標対象物運動軌道である。
【０１６１】
次いで、Ｓ２５に進んで、ロボット１と対象物１２０との幾何学的制約条件をチェックする。ここで、この幾何学的制約条件は、対象物１２０とロボット１との干渉（上体３が対象物１２０に当たるなど）の有無、並びに、各腕体３の関節の回転角が機構的な制限内に収まっているかなどの条件である。目標上体位置姿勢と目標対象物運動軌道とからロボット１の上体３と対象物１２０との干渉の有無を判断できる。また、目標上体位置姿勢と目標手先位置姿勢とから、各腕体３の関節の回転角を決定できるので、その回転角が機構的な制限内に収まっているか否かが判る。
【０１６２】
Ｓ２５の判断処理は、現在時刻から第２旋回歩容の終端まで、所定の刻み時間間隔の目標上体姿勢等の瞬時値に対して逐次行なわれる。すなわち、各瞬時値に対して、Ｓ２５の判断結果がＯＫである場合には、Ｓ２７において、現在時刻から第２旋回歩容の終端までの全時刻分のチェックが完了したか否かを判断する。そして、このＳ２７の判断結果がＮＯである場合には、Ｓ２５の判断処理を繰り返す。
【０１６３】
Ｓ２５の判断結果がＮＧである場合には、Ｓ２９に進んで、ＮＧとなった該当時刻における幾何学的制約条件を満たすように着地予定位置姿勢および／または目標対象物運動軌道を修正する。その修正例を図１６を参照して説明する。
【０１６４】
同図を参照して、現在時刻t1における移動計画に基づくＸ軸方向の目標対象物運動軌道（位置軌道）が図中のグラフg4で示す軌道（以下、元軌道という）であるとし、将来のある時刻t2で目標上体位置と目標対象物位置とが近すぎて、上体３と対象物１２０との干渉が発生し、Ｓ２５の判断結果がＮＧになるとする。このとき、その干渉が発生する時刻t2の目標対象物位置を、Ｘ軸の正方向、すなわち、ロボット１から遠ざける方向に図中の破線矢印で示す如くずらすと共に、時刻t2の前後に亘る期間ΔT2での目標対象物運動軌道（位置軌道）をグラフg5で示す修正軌道に変更する。この修正軌道g5は、ロボット１の挙動の急変を避けるために、その期間ΔT2の両端で元軌道g4に滑らかにつながるように決定される。また、時刻t2でのずらし量ΔＸは、それによって、ロボット１の上体３と対象物１２０との干渉が無くなるように（Ｓ２５の判断結果がＯＫとなるように）決定される。
【０１６５】
なお、上記の例では、目標対象物運動軌道を修正する場合を例に採って説明したが、目標対象物運動軌道を修正する代わりに、Ｓ２５の判断結果がＮＧとなる該当時刻を含む歩容での遊脚足平２２の着地予定位置姿勢を修正するようにしてもよい。例えば、図１６の場合のように時刻t2で干渉が発生する場合には、その時刻g2を含む歩容での着地予定位置姿勢を対象物１２０からＸ軸方向で遠ざけるような位置姿勢に修正すればよい。あるいは、目標対象物運動軌道と着地予定位置姿勢の両者を修正するようにしてもよい。
【０１６６】
Ｓ２９の修正処理が終了すると、Ｓ０５に戻って、このＳ０５からの処理が前述したように実行される。これにより、前記幾何学的制約条件が満たされない場合に、目標対象物運動軌道あるいは着地予定位置姿勢が修正され、ひいては、今回歩容パラメータのうちの足平軌道パラメータが修正されることとなる。なお、Ｓ２９で目標対象物運動軌道を修正せずに、遊脚足平２２の着地予定位置を修正した場合には、Ｓ０５の処理を省略してＳ０７からの処理を実行するようにしてもよい。
【０１６７】
一方、Ｓ２７の判断結果がＹＥＳになると、図７のＳ３１に進む。このＳ３１では、今回歩容パラメータを基に、目標ＺＭＰを満足するように、手先位置姿勢瞬時値を除く、現在時刻の今回歩容瞬時値を算出する。
【０１６８】
この算出処理は、次のように行なわれる。すなわち、今回歩容パラメータのうちの足平軌道パラメータを基に、目標足平位置姿勢の瞬時値が算出される。この算出処理は、前記Ｓ１３の処理の中で、ZMPfeet[i]を算出する処理に関して説明した場合と同様に、本出願人が特許第３２３３４５０号にて提案した有限時間整定フィルタを用いて各足平２２毎に算出される。なお、定常歩容に関して説明した場合と同様、今回歩容の遊脚足平２２の着地予定位置姿勢が、前回歩容の支持脚足平２２の着地位置姿勢と同じである場合には、今回歩容の各目標足平位置姿勢の瞬時値は、今回歩容初期の状態に維持される。但し、今回歩容の遊脚足平２２を一旦上昇させて、元の着地位置姿勢に戻すように今回歩容の遊脚足平２２の足平位置姿勢軌道の瞬時値を生成するようにしてもよい。
【０１６９】
また、今回歩容パラメータのうちの、ＺＭＰ軌道パラメータを基に、目標ＺＭＰの瞬時値が算出されると共に、対象物反力モーメント軌道パラメータを基に、目標対象物反力モーメントの瞬時値が算出される。
【０１７０】
さらに、上記算出された目標足平位置姿勢の瞬時値と、目標ＺＭＰの瞬時値と、目標対象物反力モーメントの瞬時値とから、前記図１４の動力学的演算によって、目標上体位置姿勢の瞬時値が算出される。なお、本実施形態では、目標上体姿勢は、例えば鉛直姿勢とされる。但し、目標上体姿勢を鉛直姿勢に維持する必要はなく、変化させるようにしてもよい。目標上体姿勢を変化させる場合には、ロボット単純化モデルは、上体３の姿勢変化に伴うロボット１の角運動量変化を考慮した動力学モデルにすることが望ましく、例えば本願出願人がＰＣＴ国際公開公報WO/03/057422A1にて提案した、同公報の図１０に示すような動力学モデルを使用すればよい。
【０１７１】
以上のＳ３１の処理によりロボット１の目標歩容の運動のうちの、目標足平位置姿勢および目標上体位置姿勢の瞬時値が決定される。本実施形態のロボット１では、各脚体２は６自由度を有するので、目標上体位置姿勢と目標足平位置姿勢とが決まれば、各脚体２の各関節の回転角は一義的に定まる。補足すると、本実施形態では、Ｓ３１で求められる目標上体位置姿勢は後述するＳ３９の処理によってさらに補正される。
【０１７２】
次いで、Ｓ３３に進んで、最終修正目標手先位置の前回値（前回の演算処理周期での値）を基に、前回の演算処理周期（歩容生成装置１００の演算処理周期）における対象物１２０の実際の位置（移動位置）である実対象物位置を推定する。ここで、最終修正目標手先位置は、後述する腕メイン制御装置１０６で求められるものであり、ロボット１の実際の手先位置の指令値に相当するものである。そこで、例えば最終修正目標手先位置の前回値をロボット１の実際の手先位置の前回値とし、これと所定の相対位置関係を有する位置を実対象物位置として推定する。なお、対象物１２０にジャイロセンサや加速度センサを搭載しておき、それらの検出値から実対象物位置を推定するようにすることも可能である。補足すると、Ｓ３３の処理は、本発明における実移動位置把握手段に相当する。
【０１７３】
次いで、Ｓ３５に進み、上記の如く推定した実対象物位置と、目標対象物位置との偏差（以下、対象物位置偏差という）と、現在の目標対象物運動軌道とを基に、対象物動力学モデルを用いて目標対象物運動の瞬時値（今回値）と、推定外乱力の瞬時値（今回値）と、目標対象物反力の瞬時値（今回値）とが算出される。なお、対象物位置偏差を規定する実対象物位置と目標対象物位置とのうちの目標対象物位置は、前回の演算処理周期で求めた値（前回値）が用いられる。このＳ３５の処理は、本発明の中核をなす処理である。
【０１７４】
ここで、このＳ３５の処理で使用する対象物動力学モデルとＳ３５の処理とを図１７のブロック図を参照して説明する。図１７は、Ｓ０３で目標対象物運動のうちの目標対象物位置と目標対象物反力と推定外乱力とを求める演算処理を示すブロック図であり、図中の参照符号２３８を付した部分が対象物１２０に作用する力と対象物１２０の運動との関係を表す対象物動力学モデルとなっている。この対象物動力学モデルは、前記図９に示したものと一部の構造が相違するので、以下の説明では、図１７中の対象物動力学モデルを図９の対象物動力学モデルと区別するため、対象物動力学モデル２という。なお、この対象物動力学モデル２は、本発明における対象物動力学モデルに相当するものである。
【０１７５】
この対象物動力学モデル２の基本構造は、図９の対象物動力学モデルと同様であり、対象物１２０に作用する力（より詳しくは水平方向の並進力）を入力として、その入力値（後述の乗算部２４２で求められる値）に、対象物１２０の質量Ｍの逆数１／Ｍを乗算部２４４で乗算することにより対象物１２０の運動加速度を求め、これを積分器２４６，２５０で順次積分する（２重積分する）ものとなっている。但し、対象物動力学モデル２では、図９の対象物動力学モデルと異なり、積分器２５０には、積分器２４６の出力（運動加速度の積分値）に加えて、モデル速度操作量が追加的に入力されるようになっている。このモデル速度操作量は、モデル速度操作量決定部２５２によって、実対象物位置と目標対象物位置との偏差を０に近づけるようにフィードバック制御則により決定される速度操作量であり、本実施形態では次式１７により決定される。
【０１７６】
モデル速度操作量＝Ke1＊対象物位置偏差＋∫(Ke2＊対象物位置偏差)dt    ……式１７
すなわち、モデル速度操作量は、対象物位置偏差からＰＩ制御則（比例積分制御則）により決定される。なお、式１７のKe1、Ke2は所定のゲインである。図１７のモデル速度操作量決定部２５２は、式１７の右辺の演算を行なう演算処理部である。すなわち、モデル速度操作量決定部２５２は、実対象物位置（現在の演算処理周期でのＳ３３で求めた今回値）と目標対象物位置（前回の演算処理周期でのＳ３５で求めた前回値）とから、それらの差である対象物位置偏差を減算部２５４で求める。そして、求めた対象物位置偏差に乗算部２５６でゲインKe1を乗算したもの（比例項）と、該対象物位置偏差に乗算部２５８でゲインKe2を乗算したものをさらに積分器２６０で積分したもの（積分項）とを、加算部２６２で加算することにより、モデル速度操作量を算出する。なお、算出されたモデル速度操作量は、対象物動力学モデル２の積分器２５０の入力側に備えた加算部２４８で、積分器２４６の出力に加算された後、積分器２５０に入力される。補足すると、図１７のブロック図では、モデル速度操作量を算出した後に、これを対象物動力学モデル２に追加的に入力するようになっているが、モデル速度操作量決定部２５２の積分器２６０を省略し、前記乗算部２５８の出力を追加的に積分器２４６に入力しつつ、このときの積分器２４６の出力と、乗算部２５６の出力との和を積分器２５０に入力するようにしてもよい。このようにしても、対象物動力学モデル２の出力（積分器２５０の出力）は図１７のものと同じになる。補足すると、モデル速度操作量決定部２５２は、本発明における運動操作量決定手段に相当し、それが求めるモデル速度操作量が対象物動力学モデル２上での対象物１２０の運動操作量に相当する。
【０１７７】
かかる対象物動力学モデル２では、加算部２４８の出力を積分器２５０で積分したものが目標対象物位置の瞬時値として得られる。また、加算部２４８の出力が、該対象物動力学モデル２上での対象物１２０の移動速度である対象物モデル速度となる。
【０１７８】
対象物動力学モデル２の入力である、対象物１２０への作用力（並進力）の要求値は、現在の目標対象物運動軌道（Ｓ２７の判断結果がＹＥＳになったときの目標対象物運動軌道）に基づく目標対象物速度と、対象物モデル速度とから減算部２４０で求められるそれらの偏差（目標対象物速度−対象物モデル速度）に乗算部２４２で所定のゲインKvを乗算することで求められる。すなわち、対象物動力学モデル２に入力する並進力の要求値は、図９のものと同様、対象物目標速度と対象物モデル速度との偏差が０に収束するように、フィードバック制御則（この例では比例制御則）により決定される。但し、この場合の対象物モデル速度は、前記したように加算部２４８の出力であるから、図９のものと異なり、モデル速度操作量が加味されたものである。また、目標対象物速度は、現在の目標対象物運動軌道のうちの位置軌道の１階微分値として得られる。
【０１７９】
そして、上記の如く求められた並進力の要求値の符号を反転させたものが目標対象物反力の瞬時値として求められる。また、該並進力の要求値が対象物動力学モデル２の乗算部２４４に入力されると共に、前記モデル速度操作量が加算部２４８に入力され、これにより、該対象物動力学モデル２の積分器２５０から目標対象物位置の瞬時値が出力される。換言すれば、対象物動力学モデル２上での対象物１２０の運動状態量としての対象物モデル速度を、目標対象物位置と実対象物位置との偏差を０に近づけるためのモデル速度操作量で修正しつつ、対象物動力学モデル２の動力学演算により目標対象物位置が逐次決定される。
【０１８０】
補足すると、本実施形態では、前記Ｓ０１〜Ｓ２９の処理と、これらの処理により最終的に得られる目標対象物運動軌道から目標対象物速度を求める処理とが本発明における目標運動状態量決定手段に相当している。この場合、目標対象物速度が目標運動状態量に相当する。また、前記減算部２４０および乗算部２４２の処理が、本発明における目標作用力決定手段に相当している。また、対象物動力学モデル２の演算処理が本発明における目標移動位置決定手段に相当する。
【０１８１】
また、図１７中の参照符号２６４を付した部分は、推定外乱力を求める処理を行なう推定外乱力決定部であり、この推定外乱力決定部２６４には、対象物モデル速度と、前記並進力の要求値とが入力される。対象物モデル速度は、M・s／(Tc・s＋1)という形の伝達関数で表される変換部２６６に入力され、この変換部２６６により、対象物１２０に作用している並進力の推定値が算出される。この変換部２６６の伝達関数中のＭは対象物１２０の質量、Ｔcは所定の時定数である。従って、変換部２６６は、対象物モデル速度の微分値（１階微分値）に対象物１２０の質量を乗算したもの（これは対象物１２０に作用する全ての力の合力の瞬時値に相当する）に、時定数Ｔcを有するフィルタリング処理を施したものを対象物１２０に作用している並進力の推定値として求める。さらに、この並進力の推定値から前記乗算部２４２の出力である並進力の要求値を減算部２６８で差し引くことにより、推定外乱力の瞬時値が算出される。ここで求められた推定外乱力の瞬時値は、前記したようにＳ０２の演算処理（図９を参照）で使用される。
【０１８２】
補足すると、前記推定外乱力決定部２６４は、本発明における外乱力推定手段に相当する。
【０１８３】
なお、目標対象物運動のうちの目標対象物姿勢の瞬時値は、例えば目標対象物速度の向きにほぼ一致するように決定される。
【０１８４】
以上が、Ｓ３５の演算処理である。
【０１８５】
次いでＳ３７に進んで、Ｓ３５で求めた目標対象物運動の瞬時値を基に、目標手先位置姿勢の瞬時値（今回値）を決定する。目標手先位置姿勢の瞬時値は、Ｓ０９で目標手先位置姿勢軌道を求めた場合と同様に求められる。補足すると、このＳ３７の処理と、後述する対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６の処理とは、本発明における制御手段に相当している。
【０１８６】
以上のＳ３７までの処理によって求められる目標歩容の瞬時値のうち、目標上体位置姿勢は、前記ロボット単純化モデルを用いて、該ロボット単純化モデル上で目標ＺＭＰを満足するように（ロボット１の運動による慣性力と重力と対象物反力モーメントとの合力が目標ＺＭＰまわりに発生するモーメントの水平成分が０になるように）求められている。従って、Ｓ３７までの処理によって求められる目標歩容は、目標ＺＭＰまわりの床反力モーメント水平成分がロボット単純化モデル上で０となる歩容である。
【０１８７】
ここで、ロボット単純化モデルは、歩容生成装置１００の演算負荷を効果的に軽減できる点での利点は大きいものもの、動力学的精度は必ずしも高精度であるとはいえない。そこで、本実施形態では、さらに、動力学的精度の高いロボット動力学モデル（以下、これをフルモデルという）を用いて、目標歩容のうちの一部の構成要素（具体的には、目標上体位置姿勢瞬時値と目標ＺＭＰまわりの床反力モーメント瞬時値）とを補正する。この補正処理をフルモデル補正と呼び、このフルモデル補正がＳ３７の処理の次にＳ３９で実行される。
【０１８８】
このフルモデル補正は、例えば本願出願人が先に提案した特開２００２−３２６１７３号公報にて説明したものと全く同様に行なわれる。従って、本明細書での説明は省略する。なお、フルモデル補正は、例えば、本願出願人によるＰＣＴ国際公開公報WO 03/057427 A1にて説明したものと同様に行なうようにしてもよい。
【０１８９】
このＳ３９のフルモデル補正により、目標上体位置姿勢と、目標ＺＭＰまわりの床反力モーメントとが補正され、これにより、最終的に歩容生成装置１００が出力する目標歩容の全ての構成要素の瞬時値が得られる。
【０１９０】
以上説明したＳ０１〜Ｓ３９までの処理が、歩容生成装置１００の演算処理周期毎に実行される処理である。
【０１９１】
次に、説明を後回しにした、Ｓ０１での移動計画の修正処理について説明する。
【０１９２】
ロボット１が対象物１２０を押しながら移動しているときに、現在の移動計画（前記移動要求通りの移動計画など）で想定していない段差（床の凸部）があったり、対象物１２０にロボット１以外から予期しない外力が作用したような場合には、移動計画に基づく目標対象物運動軌道と実際の対象物運動軌道（以下、実対象物運動軌道という）とのずれが大きくなる。一方、歩容生成装置１００は、目標対象物運動軌道を基に歩容パラメータ等を決定して、歩容を生成するので、目標対象物運動軌道と実対象物運動軌道とのずれが過大になると、ロボット１の継続的安定性を確保し得る歩容を生成することが困難となる。そこで、本実施形態におけるＳ０１では、そのずれがある程度大きくなった場合（あるいは大きくなると予測される場合）には、Ｓ０１で決定する移動計画をリアルタイムで修正する。
【０１９３】
以下、この処理の具体例を図１８（ａ），（ｂ）を参照して説明する。例えば現在（修正前の）移動計画が、対象物１２０を一定速度でＸ軸の正方向に移動させるという計画であるとする。このとき、この修正前の移動計画（以下、修正前移動計画という）を基に、前記した如くＳ３５で算出される目標対象物運動軌道（瞬時値の時系列）のうちの目標対象物位置軌道は、ほぼ、図１８（ａ）のグラフg6で示すような軌道となる。ここで、時刻t1において、実際の対象物１２０が、修正前移動計画で予期していない床の段差（凸部）に引っかかって、対象物１２０が止まってしまったとする。このとき、実対象物位置の軌道（Ｓ３３で推定される実対象物位置の時系列）は、図１８（ａ）のグラフg7で示すような軌道となる。この場合、時刻t1以後、修正前移動計画に基づく目標対象物位置と、実対象物位置との偏差が時間の経過に伴い大きくなっていく。
【０１９４】
そこで、Ｓ０１では、例えば前回の演算処理周期のＳ３５で求めた対象物位置偏差（実対象物位置と目標対象物位置との差）、すなわち、図１７の減算部２５４で求めた対象物位置偏差を所定値と比較するようにしている。そして、その対象物位置偏差が所定値よりも大きくなったとき（図１８（ａ）の時刻t2）には、図１８（ａ）のグラフg8で示す如く、対象物位置偏差の増加を抑制するように修正前移動計画を修正する。修正した移動計画を以下、修正後移動計画という。図１８（ａ）の例では、修正後移動計画は、時刻t2以後、対象物１２０の移動速度ほぼ０になるまで徐々に減速するような移動計画となる。なお、修正後移動計画は、それにより規定される目標対象物運動が修正前移動計画に対して急激な変化を生じることなく滑らかに連続するように決定することが望ましい。
【０１９５】
このように移動計画を修正することによって、目標対象物反力が過剰に大きくなったりすることがなく、ロボット１の継続的な安定性を確保し得る歩容を生成することができる。
【０１９６】
さらに、本実施形態でのＳ０１の処理では、上記のように移動計画を修正した後、実対象物位置の、前記修正後移動計画に対応する目標対象物位置軌道への追従性が良くなり、目標対象物運動軌道と実対象物運動軌道とのずれがある程度小さくなったら、修正後移動計画を修正前移動計画に近づけるように移動計画を修正する。この修正後の移動計画を以下、再修正後移動計画という。
【０１９７】
図１８（ａ）に示したように修正後移動計画を決定した場合を例に採って説明すると、対象物１２０が段差を乗り越えると（図１８（ｂ）の時刻t3）、実対象物位置軌道は、図１８（ｂ）のグラフg7で示すように修正後移動計画に基づく目標対象物位置軌道g8に近づいていく。
【０１９８】
このとき、Ｓ０１では、対象物位置偏差が所定値よりも小さくなると（図１８（ｂ）の時刻t4）、図１８（ｂ）のグラフg9で示す如く、対象物１２０の移動速度が当初の修正前移動計画に基づく対象物１２０の移動速度に近づくように、修正後移動計画を再修正して、再修正後移動計画を決定する。図１８（ｂ）の例では、再修正後移動計画は、時刻t4以後、対象物１２０の移動速度が当初の修正前移動計画に基づく移動速度にほぼ一致するまで、該移動速度を徐々に増加させるような移動計画となる。なお、再修正後移動計画は、それにより規定される目標対象物運動が修正後移動計画に対して急減な変化を生じることなく滑らかに連続するように決定することが望ましい。
【０１９９】
上記の例では、再修正後移動計画に基づく対象物１２０の移動速度を修正前移動計画に基づく移動速度に近づけるようにしたが、再修正後移動計画に基づく目標対象物位置軌道を修正前移動計画に基づく対象物位置軌道に近づけるようにすることも可能である。
【０２００】
補足すると、上記のようにＳ０１で移動計画を修正する処理が、本発明の第８発明で移動計画を修正する処理に相当するものである。
【０２０１】
以上が、本実施形態における歩容生成装置１００の処理の詳細である。
【０２０２】
次に、制御ユニット６０の歩容生成装置１００以外の処理を以下に説明する。なお、この処理は、先に述べたように、特開平１０−２３０４８５号公報の第１実施例にて提案したものと同じであるので、概略的な説明に留める。
【０２０３】
歩容生成装置１００で生成された目標歩容のうち、目標上体位置姿勢軌道と、目標ＺＭＰ軌道と、目標対象物反力軌道とが対象物反力平衡制御装置１０２に入力される。対象物反力平衡制御装置１０２は、目標対象物反力と実際のロボット１が対象物１２０から受ける反力である実対象物反力との偏差を解消する（０に近づける）ように、目標ＺＭＰまわりの目標床反力モーメントを修正するための対象物反力平衡制御用補償全床反力を算出すると共に、目標運動のうちの目標上体位置姿勢を修正してなる修正目標上体位置姿勢を求めるものである。この対象物反力平衡制御装置１０２の処理については後述する。
【０２０４】
また、目標歩容のうちの目標足平位置姿勢軌道、目標ＺＭＰ軌道および目標全床反力軌道は、脚メイン制御装置１０４に入力される。さらに、脚メイン制御装置１０４には、対象物反力平衡制御装置１０２から修正目標上体位置姿勢と対象物反力平衡制御用補償全床反力が入力される。脚メイン制御装置１０４は、コンプライアンス制御処理によって、目標歩容の運動（腕体５，５の運動を除く）と床反力とに追従させるように脚体２，２の関節アクチュエータ（電動モータ）を制御する。より具体的には、姿勢センサ５４の検出値（実上体姿勢）を修正目標上体姿勢に復元させるために、目標ＺＭＰに発生させるべき復元全床反力を算出し、目標ＺＭＰに作用する実全床反力モーメント成分（これは脚体２，２の６軸力センサ５０の検出値から求められる）が、この復元全床反力と目標全床反力と対象物反力平衡制御用補償全床反力との合力のモーメント成分に一致するように、目標足平位置姿勢を修正する。修正された目標足平位置姿勢を修正目標足平位置姿勢という。そして、脚メイン制御装置１０４は、この修正目標足平位置姿勢と修正目標上体位置姿勢とから決定される両脚体２，２の目標関節変位に実関節変位が追従するように両脚体２，２の関節アクチェエータを制御する（各脚体２のモータ駆動指令を各関節アクチュエータに出力する）。
【０２０５】
また、目標歩容のうちの目標手先位置姿勢軌道と目標対象物反力軌道とは、腕メイン制御装置１０６に入力される。さらに、腕メイン制御装置１０６には、対象物反力平衡制御装置１０２から修正目標上体位置姿勢が入力される。腕メイン制御装置１０６は、コンプライアンス制御処理によって、目標手先位置姿勢軌道と目標対象物反力軌道に追従させるように腕体５，５の関節アクチュエータ（電動モータ）を制御する。より具体的には、６軸力センサ５２の検出値（実対象物反力）と目標対象物反力との差に応じて目標手先位置姿勢を修正する。修正された目標手先位置姿勢を最終修正目標手先位置姿勢という。そして腕メイン制御装置１０６は、この最終修正目標手先位置姿勢と修正目標上体位置姿勢とから決定される両腕体５，５の目標関節変位に実関節変位が追従するように両腕体５，５の関節アクチュエータを制御する（各腕体５のモータ駆動指令を各関節アクチュエータに出力する）。
【０２０６】
対象物反力平衡制御装置１０２の処理をより具体的に以下に説明する。図１９は、対象物反力平衡制御装置１０２の処理を機能的に示すブロック図である。
【０２０７】
対象物反力平衡制御装置１０２では、まず、目標対象物反力による目標ＺＭＰまわりの目標対象物反力モーメントと実対象物反力による目標ＺＭＰまわりの実対象物反力モーメントとの偏差である対象物反力モーメント偏差がモーメント偏差算出部２７０により算出される。この場合、目標対象物反力モーメントは、歩容生成装置１００から出力された目標対象物反力および目標ＺＭＰと、腕メイン制御装置１０６で求められた最終修正目標手先位置姿勢（より詳しくは前回の制御処理周期で値）とから算出される。また、実対象物反力モーメントは、６軸力センサ５２の検出値（実対象物反力）と、目標ＺＭＰと、最終修正目標手先位置姿勢（より詳しくは前回の制御処理周期で値）とから算出される。
【０２０８】
この対象物反力モーメント偏差に応じて、重心位置摂動量算出部２７２により、ロボット１の全体重心位置の目標摂動量である目標重心位置摂動量が算出される。目標重心位置摂動量は、対象物反力モーメント偏差を、ロボット１に作用する重力によって長期的に解消するためのロボット１の全体重心の摂動量としての意味を持ち、例えば対象物反力モーメントに比例した値に決定される。次いで、後述する摂動動力学モデルで算出される、該摂動動力学モデル上でのロボット１の全体重心の摂動量である全体重心位置モデル摂動量と、前記目標重心位置摂動量との偏差が減算部２７４により算出され、この偏差からフィードバック制御則２７６、例えばＰＤ制御則によって、該偏差を０に収束させるための、目標ＺＭＰまわりのモーメント操作量である対象物反力平衡制御用補償全床反力モーメントが算出される。さらに、この対象物反力平衡制御用補償全床反力モーメントと前記対象物反力モーメント偏差との和が加算部２７８により算出される。そして、この加算部２７８の出力がロボット１の全体重心の摂動と、目標ＺＭＰまわりのモーメントの摂動との関係、並びに、全体重心の摂動と上体位置姿勢の摂動との関係を表す摂動動力学モデル２８０に入力され、この摂動動力学モデルで、上体位置姿勢摂動量が算出される。
【０２０９】
この場合、摂動動力学モデルは、ロボット１の全体重心の摂動と、目標ＺＭＰまわりのモーメントの摂動との関係（動力学的関係）を次式１８により記述するモデルである。
【０２１０】
mtotal＊hG＊d2ΔxG/dt2＝ΔxG＊mtotal＊g＋ΔMx      ……式１８
ここで、hGは、目標ＺＭＰから全体重心までの高さ、ΔxGは全体重心の水平方向摂動量、ΔMxは目標ＺＭＰまわりのモーメント水平成分である。その他の変数は、前記ロボット単純化モデルに関して定義したものと同じである。なお、式１８は、サジタルプレーン上での式であり、ラテラルプレーン上での関係式は、式１８の右辺の第２項の符号を反転させればよい。
【０２１１】
この式１８は、ロボット１の全体質量mtotalの質点を持ち、また、その質点の支点である目標ＺＭＰまわりに発生するモーメント水平成分がΔMxとなるような倒立振子の動力学的挙動を示す式である。
【０２１２】
また、ロボット１の全体重心位置の摂動量ΔxGと上体位置の摂動量（以下、これをΔxbと記述する）との関係は、次式１９により表される。
【０２１３】
Δxb＝ｋ＊ΔxG    ……式１９
ここで、ｋはある比例定数である。従って、Δxｂは、ΔxGに比例するものとされる。摂動運動に対しては、式１９は近似的に成立すると考えてよい。
【０２１４】
従って、摂動動力学モデル２８０では、式１８の右辺のΔMxとして、加算部２７８の出力を用いることで、全体重心の摂動量ΔxGが算出され、さらに、このΔxGから式１９によって、上体位置摂動量が求められる。なお、本実施形態では、修正目標上体位置姿勢のうちの修正目標上体姿勢は、目標上体姿勢に一致させるものとし、上体姿勢の摂動量は０とする。
【０２１５】
対象物反力平衡制御装置１０２では、このようにして摂動動力学モデル２８０から出力される上体位置摂動量を目標上体位置姿勢（歩容生成装置１００の出力）に加算部２８２で加算することにより、修正目標上体位置姿勢を算出する。
【０２１６】
以上が対象物反力平衡制御装置１０２の具体的な演算処理である。
【０２１７】
以上説明した第１実施形態の作動、特に歩容生成装置１００の処理によって、対象物反力を考慮しつつ、今回歩容が定常歩容に収束するように今回歩容が生成される。このため、ロボット１の継続的な安定性を確保しながら、ロボット１により対象物１２０を押すなどの作業を円滑に行なうことができる。また、歩容生成装置１００で目標対象物位置と目標対象物反力を最終的に決定するときには、前記Ｓ３５の処理によって、対象物動力学モデル２上での対象物１２０の運動状態量としての対象物モデル速度が移動計画に基づく目標対象物運動軌道における目標対象物速度に追従するように対象物動力学モデル２の入力である目標対象物反力を決定しつつ、目標対象物位置と実対象物位置との偏差ができるだけ０に近づくように（目標対象物位置と実対象物位置とのずれが小さくなるように対象物動力学モデル２の出力である目標対象物位置が決定される。このため、対象物動力学モデル２の挙動が実際の対象物１２０の挙動とかけ離れてしまうことがなく、実際の対象物１２０の挙動との整合性を採りながら、目標対象物位置と目標対象物反力とを決定できる。その結果、ロボット１に無理な動作を行わせるような歩容が決定されるような事態を回避することができ、ロボット１の挙動の安定性を高めることができる。
【０２１８】
次に、本発明の第２実施形態を図２０を参照して説明する。なお、この第２実施形態の説明では、第１実施形態と同一構成部分あるいは同一機能部分については第１実施形態と同じ参照符号を用い、説明を省略する。この第２実施形態は、本発明の第１〜第４発明の実施形態である。
【０２１９】
第２実施形態は、図５のＳ０１において、移動計画を修正する処理のみが第１実施形態と相違するものである。以下説明すると、実対象物運動軌道と、移動計画に基づく目標対象物運動軌道とのずれが大きくなるような状況は、実際の対象物１２０に作用する力のうち、ロボット１から作用する力以外の外乱力が比較的大きく変化した場合に発生する。
【０２２０】
そこで、第２実施形態では、第１実施形態で説明した如く対象物位置偏差に応じて移動計画を修正する代わりに、例えば前記推定外乱力の変化に応じて移動計画を修正する。これ以外は、第１実施形態と同じである。補足すると、本実施形態でのＳ０１の処理は、第２発明における移動計画修正手段に相当する。
【０２２１】
以下、具体例を説明すると、例えば、前記図１８（ａ）に示した例の場合には、前記Ｓ３５で算出される推定外乱力（Ｘ軸方向成分）は、図２０（ａ）に示すように、対象物１２０が段差に引っかかる時刻t1以後に負方向に急激に大きくなる。そこで、第２実施形態では、この推定外乱力が所定値以上変化したとき（図２０（ａ）の時刻t5）に、図１８（ａ）に関して第１実施形態で説明した如く、修正前移動計画を修正して、修正後移動計画を決定する。
【０２２２】
さらに、図１８（ｂ）に示した例の場合には、前記Ｓ３５で算出される推定外乱力（Ｘ軸方向成分）は、図２０（ｂ）に示すように、対象物１２０が段差を乗り越えた時刻t3以後に、時刻t1以前の元の値とほぼ同じ値に戻っていく。そこで、この推定外乱力が、所定値以上変化した後に、元の値にほぼ近い値に戻ったとき（図２０（ｂ）の時刻t6）に、図１８（ｂ）に関して第１実施形態で説明した如く、修正後移動計画を修正して、再修正後移動計画を決定する。
【０２２３】
以上説明した以外の処理は、前記第１実施形態と同じである。
【０２２４】
かかる第２実施形態においても、第１実施形態と同様の作用効果が得られる。
【０２２５】
補足すると、第２実施形態では、推定外乱力の変化に応じて移動計画を修正するようにしたが、両腕体５，５の６軸力センサ５２の検出値から把握される実対象物反力と、Ｓ３５で決定される目標対象物反力との偏差が所定値よりも大きくなったときに移動計画を修正するようにしてもよい。
【０２２６】
次に本発明の第３実施形態を図２１〜図２３を参照して説明する。なお、この第３実施形態の説明では、第１実施形態と同一構成部分あるいは同一機能部分については第１実施形態と同じ参照符号を用い、説明を省略する。この第３実施形態は、本発明の第１発明、第３発明および第４発明の実施形態である。
【０２２７】
図２１は第３実施形態での歩容生成装置１００の処理のうち、第１実施形態に係る図５の処理に対応する部分の処理を示すフローチャートである。同図２１に示すように、第３実施形態では、Ｓ１９の判断結果がＮＯである場合の処理のみが第１実施形態と相違している。
【０２２８】
すなわち、第１実施形態では、Ｓ１９の判断結果がＮＯであるときに、目標対象物反力軌道を修正するようにしたが、第３実施形態では、これに代えて、Ｓ２１’において、遊脚足平２２の着地予定位置姿勢または着地予定時刻を修正する。これは今回歩容パラメータのうちの足平軌道パラメータを修正することを意味する。この場合、着地予定位置姿勢または着地予定時刻は、その修正後にＳ０７〜Ｓ１７の処理を再度実行したときに、ＺＭＰ修正量ができるだけ小さくなるように（少なくとも着地予定位置姿勢または着地予定時刻の修正前に求めたＺＭＰ修正量よりも小さくなるように）修正される。そして、その修正後に、Ｓ０７からの処理を再度実行する。これ以外は、第１実施形態と同一である。
【０２２９】
Ｓ２１’での具体的な修正例を以下に説明する。例えば今現在、今回歩容の遊脚足平２２の着地予定位置姿勢と次回歩容の遊脚足平２２の着地予定位置姿勢が図２２の実線で示す如く決定されているとする。なお、図示の例では、ロボット１をほぼ一定の歩幅で、今回歩容支持脚座標系のＸ軸方向に直進歩行させるものとなっている。
【０２３０】
そして、今回歩容まではロボット１が対象物１２０に近づき、次回歩容の途中から、ロボット１が対象物１２０をＸ軸の正方向に押す作業を開始するものとする。この場合、図２３（ａ）に示す如く、目標対象物反力軌道は、例えば次回歩容の途中から立ち上がるようなステップ状の軌道となる。
【０２３１】
このとき、図２２の実線で示すような着地予定位置姿勢を維持したまま、Ｓ１７までの処理を実行すると、Ｓ１７の処理で決定されるＺＭＰ修正量（Ｘ軸方向成分）は、例えば図２３（ｂ）に実線で示す如く、比較的大きなものとなって、Ｓ１９の判断結果がＮＯとなる場合がある。
【０２３２】
この場合に、Ｓ２１’の処理では、例えば次回歩容の遊脚足平２２の着地予定位置姿勢を図２２の破線で示す如く、今回歩容の支持脚足平２２ＬにＸ軸方向で近づけるように修正する。すなわち、次回歩容の歩幅をより小さくするように遊脚足平２２Ｌの着地予定位置姿勢を修正する。このように次回歩容の着地予定位置姿勢を修正した後に、Ｓ０５からの処理を再び実行すると、Ｓ１７で決定されるＺＭＰ修正量が、図２３（ｂ）に破線で示す如く小さくなる。その結果、Ｓ１９の判断結果がＹＥＳになる。補足すると、次回歩容の遊脚足平２２Ｌの着地予定位置姿勢を図２２の如く修正したとき、前記した定常歩容の足平軌道パラメータの決定手法によって、定常歩容の第２旋回歩容の終端遊脚足平位置も、第２旋回歩容の初期遊脚足平位置（今回歩容の遊脚足平２２Ｒの着地予定位置）に近づくこととなる。
【０２３３】
なお、上記の例では、次回歩容の遊脚足平２２Ｌの着地予定位置姿勢を修正するようにしたが、今回歩容の遊脚足平２２Ｒの着地予定位置姿勢を修正する余裕がある場合には、それを修正するようにしてもよい。あるいは、今回歩容および次回歩容の両者の着地予定位置姿勢を修正するようにしてもよい。
【０２３４】
また、上記の例では、着地予定位置姿勢を修正するようにしたが、今回歩容および／または次回歩容の遊脚足平２２の着地予定時刻を修正するようにしてもよい。図２３に示した状況では、例えば次回歩容の着地予定時刻を遅らせるようにすればよい。
【０２３５】
以上が本発明の第３実施形態である。かかる第３実施形態においても、第１実施形態と同様の作用効果を奏することができる。なお、第３実施形態では、Ｓ１９の判断結果がＮＯである場合に遊脚足平２２の着地予定位置姿勢または着地予定時刻を修正するようにしたが、それと併せて、第１実施形態の如く、目標対象物反力軌道を修正するようにしてもよい。また、Ｓ０１では、第２実施形態で説明した如く、推定外乱力に応じて移動計画を推定するようにしてもよい。
【０２３６】
なお、以上説明した第１〜第３実施形態では、ロボット１により対象物１２０を押して移動させる場合を例に採って説明したが、対象物１２０を引いて移動させる場合や、対象物１２０を持ち上げて移動させる場合などにも本発明を適用することができる。
【図面の簡単な説明】
【０２３７】
【図１】本発明の実施形態における脚式移動ロボットのとしての２足移動ロボットの概略構成を示す図。
【図２】図１のロボットに備えた制御ユニットの構成を示すブロック図。
【図３】図２の制御ユニットの要部の機能的構成を示すブロック図。
【図４】実施形態におけるロボットと対象物との関係を示す図。
【図５】第１実施形態における歩容生成装置の演算処理を示すフローチャート。
【図６】第１実施形態における歩容生成装置の演算処理を示すフローチャート。
【図７】第１実施形態における歩容生成装置の演算処理を示すフローチャート。
【図８】図５のＳ０２で決定する移動計画の例を示す図。
【図９】図５のＳ０２の処理を示すブロック図。
【図１０】図１０（ａ）は図５のＳ０７に係る仮目標ＺＭＰ軌道の例を示す図、図１０（ｂ）は図５のＳ１７で決定されるＺＭＰ修正量の例を示す図、図１０（ｃ）はＳ１７で修正された目標ＺＭＰ軌道の例を示す図。
【図１１】図５のＳ１１で決定する足平軌道パラメータの例を示す図。
【図１２】図１２（ａ）は今回歩容の対象物床反力モーメント軌道の例を示す図、図１２（ｂ）は図５のＳ１１で決定する定常歩容の対象物反力モーメント軌道の例を示す図。
【図１３】実施形態で用いるロボット動力学モデルの例を示す図。
【図１４】図１３のロボット動力学モデルを使用して上体位置を求める処理を示すブロック図。
【図１５】図１５（ａ），（ｂ）は、図５のＳ２１の処理を説明するための図。
【図１６】図６のＳ２９の処理を説明するための図。
【図１７】図７のＳ３５の処理を示すブロック図。
【図１８】図１８（ａ），（ｂ）は、図５のＳ０１の移動計画の修正処理を説明するための図。
【図１９】図３に示す対象物反力平衡制御装置の処理を示すブロック図。
【図２０】本発明の第２実施形態におけるＳ０１（図５）の移動計画の修正処理を説明するための図。
【図２１】本発明の第３実施形態における歩容生成装置の要部の処理を示すフローチャート。
【図２２】図２１のＳ２１’の処理を説明するための図。
【図２３】図２３（ａ），（ｂ）は図２１のＳ２１’の処理を説明するための図。
【符号の説明】
【０２３８】
１…脚式移動ロボット（２足移動ロボット）、２…脚体、３…上体、１００…歩容生成装置（目標運動状態量決定手段、目標作用力決定手段、目標移動位置決定手段、実移動位置把握手段、運動操作量決定手段、移動計画修正手段）、１０２…対象物反力平衡制御装置（制御手段）、１０４…脚メイン制御装置（制御手段）、１０６…腕メイン制御装置（制御手段）。
【図１】
2006110676.tif 000003
【図２】
2006110676.tif 000004
【図３】
2006110676.tif 000005
【図４】
2006110676.tif 000006
【図５】
2006110676.tif 000007
【図６】
2006110676.tif 000008
【図７】
2006110676.tif 000009
【図８】
2006110676.tif 000010
【図９】
2006110676.tif 000011
【図１０】
2006110676.tif 000012
【図１１】
2006110676.tif 000013
【図１２】
2006110676.tif 000014
【図１３】
2006110676.tif 000015
【図１４】
2006110676.tif 000016
【図１５】
2006110676.tif 000017
【図１６】
2006110676.tif 000018
【図１７】
2006110676.tif 000019
【図１８】
2006110676.tif 000020
【図１９】
2006110676.tif 000021
【図２０】
2006110676.tif 000022
【図２１】
2006110676.tif 000023
【図２２】
2006110676.tif 000024
【図２３】
2006110676.tif 000025