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

Description

本発明は脚式移動ロボットに対象物を移動させる作業を行わせるための目標歩容を生成する装置に関する。

２足移動ロボットなどの脚式移動ロボットに、ある対象物を押して移動させるなどの作業を行わせる場合には、通常、対象物の移動計画（対象物をどのようなタイミングでどのように移動させるというような計画）に基づく対象物の目標運動に応じてロボットの目標歩容が生成される。そして、この生成した目標歩容にロボットの動作を追従させるように、ロボットの動作制御が行なわれる（例えば、特許文献１を参照）。なお、ロボットに対象物を移動させる作業を行なわせる場合、該ロボットが単独で移動する場合と異なり、該ロボットはその移動環境の床から反力（外力）を受けるだけでなく、対象物からも反力（外力）を受ける。このため、ロボットの姿勢の安定性を確保するために、対象物からの反力による影響も含めて、いわゆるＺＭＰ（Zero Moment Point）がロボットの接地面内（より一般的にはいわゆる支持多角形内）に存在するように目標歩容が生成される。
特開平１０−２３０４８５号公報

ところで、例えば２足移動ロボットの前方に向かって対象物を直線的に移動させるような場合には、通常、ロボットの１歩毎の期間で、ロボットと対象物との位置姿勢関係が適切な位置姿勢関係になるようにロボットの目標歩容が生成される。別の言い方をすれば、ロボットの１歩毎の期間における該ロボットによる対象物の平均移動速度が、対象物の移動計画に基づく該期間における対象物の平均移動速度と等しくし得るように、ロボットの遊脚側脚体の目標着地位置姿勢が決定される。なお、この場合、ロボットの遊脚側脚体の目標着地位置姿勢は、支持脚側脚体に対する遊脚側脚体の機構的な可動範囲などの制約条件を満たす範囲内で設定する必要がある。ただし、移動計画における対象物の速度指令値が、ロボットの実現可能な最大移動速度よりも小さく設定されている限り、該制約条件による規制を受けることは実際上はほとんどない。

他方、対象物をロボットの前後方向に対して斜めに移動させたり、あるいは、該対象物を旋回移動させたり、あるいは、該対象物をロボットの横方向に移動させるような場合に、上記のようにロボットの目標歩容を生成すると次のような不都合を生じる。

例えば、２足移動ロボットの右方向に対象物を押して移動させる場合において、ロボットの脚体のうちの右側脚体は、通常、該対象物を移動させるべき向きに比較的大きく踏み出すことが可能であるものの、左側脚体は、右側脚体との干渉等を防止するための制約条件の影響を受けて、該対象物を移動させるべき向きに大きく踏み出すことは実際にはできない。このため、右側脚体が遊脚となる１歩の期間における対象物の目標位置姿勢に対してロボットの位置姿勢が適切な位置姿勢関係になるようにロボットの目標歩容を生成しても、その次の１歩の期間では、対象物の目標位置姿勢に対してロボットの位置姿勢が適切な位置姿勢関係になるようにロボットの目標歩容を生成することができない場合が多い。ひいては、対象物を移動計画通りに移動させることが困難となる。このことは、対象物を旋回移動させたり、ロボットの前後方向に対して斜め方向に移動させる場合についても同様に生じる不都合である。

このように、ロボットの１歩毎に、その１歩の期間におけるロボットと対象物との位置姿勢関係が適切な位置姿勢関係になるようにロボットの目標歩容が生成する手法では、ロボットに対する対象物の移動方向によって、遊脚側脚体が支持脚側脚体との干渉などを防止するための制約条件の影響を受けやすい。ひいては、移動計画を修正せざるを得ない状況が頻繁に生じたり、あるいは、移動計画をロボットに対する移動方向別に、ロボットの脚体の実現可能な運動形態に適合させるように設定しておかなければならない等の不都合がある。

本発明はかかる背景に鑑みてなされたものであり、対象物のロボットに対する移動方向によらずに、移動計画の修正を要する状況を必要最小限に留め、可能な限り、移動計画に則した対象物の移動を可能とするロボットの目標歩容を生成することができる脚式移動ロボットの歩容生成装置を提供することを目的とする。

脚式移動ロボットにより対象物を移動させる場合、一般に、対象物の瞬時瞬時の移動速度や移動位置を移動計画に基づく瞬時目標値に追従させることが要求されるものではなく、ロボットの複数歩分の期間毎に、対象物の移動速度や移動位置が移動計画に基づく目標値に追従していれば十分である場合が多い。また、ロボットの１歩毎に、脚体同士の干渉などを防止するための制約条件を満たしつつ、ロボットを移動計画に基づく対象物の目標運動に適合させるように目標歩容を生成することが困難となる場合であっても、ロボットの複数歩分の期間では、該制約条件を満たしつつ、該期間の終端時刻までにロボットを移動計画に基づく対象物の運動に適合させるように目標歩容を生成することが可能となる場合が多々ある。例えば、ある移動方向に対象物を移動させる場合において、１歩目の遊脚側脚体が制約条件の影響を受け難くく、該遊脚側脚体を当該移動方向で比較的遠方に踏み出すことが可能である場合には、２歩目の遊脚側脚体が制約条件の影響を受けて、踏み出し量が小さい範囲に限定されてしまうような場合であっても、１歩目の遊脚側脚体の踏み出し量を制約条件の範囲内で大きくすることで、少なくとも２歩目の遊脚側脚体の着地予定時刻で対象物を移動計画に基づく目標位置まで移動させることが可能となる場合が多々ある。

以下に説明する本発明の脚式移動ロボットの歩容生成装置は、このような知見に基づいて構成されたものである。

すなわち、本発明の脚式移動ロボットの歩容生成装置は、前記の目的を達成するために、上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための目標歩容を生成する歩容生成装置において、
前記ロボットを追従させる目標歩容を新たに生成するとき、少なくとも対象物の移動計画に基づいて、該ロボットの複数歩分の期間を含む将来の所定期間における対象物の第１仮目標運動軌道を決定する将来対象物運動仮決定手段と、
前記対象物の移動時の該対象物とロボットとの間の位置姿勢関係に関する所定の要求が少なくともロボットの将来の２以上の所定数の歩数目において満たされ、且つ、該ロボットの脚体の運動に関する所定の制約条件が当該所定数の歩数目までの各歩数毎に満たされることを必要要件として、前記決定された第１仮目標運動軌道に従って移動する対象物に対して該必要要件を満足するロボットの脚体の運動が可能であるか否かを少なくとも該第１仮目標運動軌道と該必要要件とに基づいて判断する判断手段とを備え、
前記判断手段の判断結果が肯定的であるときには、前記第１仮目標運動軌道に従って移動する対象物に対して前記必要要件を満足するように前記ロボットの目標歩容を生成し、前記判断手段の判断結果が否定的であるときには、前記移動計画を修正すると共に、少なくともその修正後の移動計画に基づいて前記所定期間における対象物の第２仮目標運動軌道を新たに決定し、該第２仮目標運動軌道に従って移動する対象物に対して前記必要要件を満足するように前記ロボットの目標歩容を生成するようにした歩容生成装置であって、
前記所定の要求は、前記ロボットの遊脚側脚体の着地位置姿勢と前記対象物の位置姿勢との関係を規定する要求を含むと共に、前記所定の制約条件は、前記ロボットの遊脚側脚体の支持脚脚体に対する相対的な着地位置姿勢を規定する制約条件を含み、
前記判断手段は、前記決定された対象物の第１仮目標運動軌道と前記所定の要求とに基づいて、前記所定数の歩数目までの各歩数目において該要求を満足するように前記ロボットの遊脚側脚体の第１仮着地予定位置姿勢を該所定数の歩数目まで決定する手段と、該所定数の歩数目までの各歩数目における該第１仮着地予定位置姿勢を、前記所定の制約条件により制限してなる第２仮着地予定位置姿勢を決定する手段と、前記所定数の歩数目における第１仮着地予定位置姿勢および第２仮着地予定位置姿勢が互いに一致もしくはほぼ一致しているか否かを判断し、一致していない場合には、該所定数の歩数目における第１仮着地予定位置姿勢と第２仮着地予定位置姿勢との差分に応じて、該所定数の歩数目よりも前の少なくとも１つの歩数目における第２仮着地予定位置姿勢を前記所定の制約条件を満たす範囲内で修正する手段とを備え、当該少なくとも１つの歩数目の修正によって、前記所定数の歩数目における第２仮着地予定位置姿勢を、前記所定の制約条件が満足される範囲内で、該所定数の歩数目における第１仮着地予定位置姿勢に一致もしくはほぼ一致する位置姿勢に修正可能であるか否かによって、前記必要要件を満足するロボットの脚体の運動が可能であるか否かを判断するものであり、
前記判断手段の判断結果が否定的であるときに前記移動計画を修正するために、該判断手段の判断結果が否定的であるとき、前記所定の制約条件を満たす範囲内で、前記所定数の歩数目のロボットの遊脚側脚体の着地予定位置姿勢を該所定数の歩数目における前記第１仮着地予定位置姿勢に近づけるように該所定数の歩数目までのロボットの遊脚側脚体の第３仮着地予定位置姿勢を決定する手段と、その決定した第３仮着地予定位置姿勢に対して前記所定の要求を満たす前記対象物の第１予想移動経路を推定する手段と、前記第１仮着地予定位置姿勢に対して前記所定の要求を満たす前記対象物の第２予想移動経路を推定する手段と、前記第１予想移動経路で前記対象物を移動させたとした場合における第１所定期間での該対象物の移動距離と、該第１所定期間の時間と、前記第２予想移動経路で前記対象物を移動させたとした場合における第２所定期間での該対象物の移動距離と、該第２所定期間の時間とに基づいて前記移動計画における対象物の速度指令値を修正する手段とを備えたことを特徴とするものである。

かかる本発明によれば、前記判断手段の判断結果が肯定的であるということは、前記対象物が前記決定された第１仮目標運動軌道に従って移動する場合に、該対象物とロボットとの間の位置姿勢関係に関する所定の要求が少なくともロボットの将来の２以上の所定数の歩数目において満たされ、且つ、該ロボットの脚体の運動に関する所定の制約条件が当該所定数の歩数目までの各歩数毎に満たされるという前記必要要件を満足するロボットの脚体の運動が可能であるということを意味する。従って、この場合には、前記第１仮目標運動軌道に従って移動する対象物に対して前記必要要件を満足するように（より詳しく言えば、対象物が第１仮目標運動軌道に従って移動すると仮定した場合における該対象物に対して前記必要要件を満足するように）ロボットの目標歩容を生成する。これにより、少なくとも前記所定数の歩数目における将来のロボットの位置姿勢が、前記移動計画に基づく第１仮目標運動軌道に従って移動する対象物の位置姿勢に整合した位置姿勢（前記所定の要求を満たす位置姿勢）になるようなロボットの目標歩容を生成できることとなる。なお、この場合、該目標歩容によって定まる対象物の運動軌道（目標歩容通りにロボットを動作させた場合における対象物の運動軌道）は、前記所定数の歩数目までの中間の瞬時時刻では、前記第１仮目標運動軌道に一致するとは限らない。

また、前記判断手段の判断結果が否定的であるということは、前記対象物が前記決定された第１仮目標運動軌道に従って移動すると仮定した場合に、該対象物とロボットとの間の位置姿勢関係に関する所定の要求が少なくともロボットの将来の２以上の所定数の歩数目において満たされ、且つ、該ロボットの脚体の運動に関する所定の制約条件が当該所定数の歩数目までの各歩数毎に満たされるという前記必要要件を満足するロボットの脚体の運動が不可能であるということを意味する。つまり、この場合には、前記所定数の歩数目までに移動計画に従って対象物を移動させ得るようなロボットの目標歩容を生成することは不可能である。そこで、この場合には、前記移動計画を修正すると共に、少なくともその修正後の移動計画に基づいて前記所定期間における対象物の第２仮目標運動軌道を新たに決定し、その新たに決定した第２仮目標運動軌道に従って移動する対象物に対して前記必要要件を満足するように前記ロボットの目標歩容を生成する。これにより、少なくとも前記所定数の歩数目における将来のロボットの位置姿勢が、修正後の移動計画に基づく第２仮目標運動軌道に従って移動する対象物の位置姿勢に整合した位置姿勢（前記所定の要求を満たす位置姿勢）になるようなロボットの目標歩容を生成できることとなる。なお、この場合に生成される目標歩容においても、前記所定数の歩数目までの中間の瞬時時刻では、該目標歩容によって定まる対象物の運動軌道が、前記第２仮目標運動軌道に一致している必要はない。

本発明は、上記のように必要要件を定めて、前記判断結果が否定的になる場合にのみ、対象物の移動計画を修正するので、対象物のロボットに対する移動方向によらずに、移動計画の修正を要する状況を必要最小限に留め、可能な限り、移動計画に則した対象物の移動を可能とするロボットの目標歩容を生成することが可能となる。

なお、前記所定の要求としては、例えばロボットの上体あるいは遊脚側脚体の着地予定位置姿勢と対象物との位置姿勢関係を規定する要求などがある。また、前記所定の制約条件としては、ロボットの遊脚側脚体の支持脚側脚体に対する可動範囲の条件や、該遊脚側脚体の関節の動作速度の範囲の条件、ＺＭＰの存在領域の条件などが挙げられる。

また、移動計画を修正する場合、その修正の都度、修正後の移動計画に基づいて対象物の仮目標運動軌道を決定し、この仮目標運動軌道と前記必要要件とに基づいて前記判断手段の判断を行なうようにしてもよい。この場合、該判断手段の判断結果が肯定的となるまで、移動計画を修正し、その最終的な修正後の移動計画に基づいて決定される対象物の仮目標運動軌道を前記第２仮目標運動軌道として使用すればよい。

特に、本発明では、前記所定の要求が、ロボットの遊脚側脚体の着地予定位置姿勢と前記対象物の位置姿勢との関係を規定する要求を含むと共に、前記所定の制約条件が、ロボットの遊脚側脚体の支持脚脚体に対する相対的な着地位置姿勢を規定する制約条件を含む。そして、前記判断手段は、前記した如く前記必要要件を満足するロボットの脚体の運動が可能であるか否かを判断する。

すなわち、該判断手段は、前記決定された対象物の第１仮目標運動軌道と前記所定の要求とに基づいて、前記所定数の歩数目までの各歩数目において該要求を満足するように前記ロボットの遊脚側脚体の第１仮着地予定位置姿勢を該所定数の歩数目まで決定する手段と、該所定数の歩数目までの各歩数目における該第１仮着地予定位置姿勢を、前記所定の制約条件により制限してなる第２仮着地予定位置姿勢を決定する手段と、前記所定数の歩数目における第１仮着地予定位置姿勢および第２仮着地予定位置姿勢が互いに一致もしくはほぼ一致しているか否かを判断し、一致していない場合には、該所定数の歩数目における第１仮着地予定位置姿勢と第２仮着地予定位置姿勢との差分に応じて、該所定数の歩数目よりも前の少なくとも１つの歩数目における第２仮着地予定位置姿勢を前記所定の制約条件を満たす範囲内で修正する手段とを備え、当該少なくとも１つの歩数目の修正によって、前記所定数の歩数目における第２仮着地予定位置姿勢を、前記所定の制約条件が満足される範囲内で、該所定数の歩数目における第１仮着地予定位置姿勢に一致もしくはほぼ一致する位置姿勢に修正可能であるか否かによって、前記必要要件を満足するロボットの脚体の運動が可能であるか否かを判断する。

これによれば、前記必要要件を満足するロボットの脚体の運動が可能であるか否かを容易に判断することができる。なお、第１仮着地予定位置姿勢のそれぞれ（該所定数の歩数目までの各歩数目における第１仮着地予定位置姿勢）を、前記所定の制約条件により制限してなる第２仮着地予定位置姿勢を決定するときには、該第１仮着地予定位置姿勢が前記所定の制約条件を満たす場合には、該第１仮着地予定位置姿勢を第２仮着地予定位置姿勢として決定すればよい。また、該第１仮着地予定位置姿勢が前記所定の制約条件を満たさない場合には、前記所定数の歩数目までの各歩数毎に該所定の制約条件を満たす範囲内で、各歩数毎の第２仮着地予定位置姿勢が第１仮着地予定位置姿勢に一致もしくは最も近くなるように該第２仮着地予定位置姿勢を決定すればよい。

上記ように判断手段を構成した場合において、前記判断手段の判断結果が肯定的であって、且つ、前記所定数の歩数目における第１仮着地予定位置姿勢および第２仮着地予定位置姿勢が互いに一致もしくはほぼ一致していないとき、前記所定数の歩数目よりも前の少なくとも１つの歩数目における遊脚側脚体の着地位置姿勢が前記第１仮着地予定位置姿勢と異なり、且つ、前記必要要件を満足するように該所定数の歩数目までの各歩数目における遊脚側脚体の着地予定位置姿勢を新たに決定する手段を備え、その新たに決定した前記所定数の歩数目までの遊脚側脚体の着地予定位置姿勢に応じてロボットの目標歩容を生成することが好ましい。

すなわち、前記判断手段の判断結果が肯定的であって、且つ、前記所定数の歩数目における第１仮着地予定位置姿勢および第２仮着地予定位置姿勢が互いに一致もしくはほぼ一致していないときには、前記所定数の歩数目よりも前の少なくと１つの歩数目における遊脚側脚体の着地予定位置姿勢を、第１仮着地予定位置姿勢と異ならせることによって、前記必要要件を満足するように所定数の歩数目までの遊脚側脚体の着地位予定位置姿勢を新たに決定できる。従って、この新たに決定した着地予定位置姿勢に応じて目標歩容を生成することで、少なくとも前記所定数の歩数目における将来のロボットの位置姿勢が、前記移動計画に基づく第１仮目標運動軌道に従って移動する対象物の位置姿勢に整合した位置姿勢（前記所定の要求を満たす位置姿勢）になるようなロボットの目標歩容を生成できる。

また、上記のように判断手段を構成した本発明では、前記した如く、前記判断手段の判断結果が否定的であるときに前記移動計画を修正するために、該判断手段の判断結果が否定的であるとき、前記所定の制約条件を満たす範囲内で、前記所定数の歩数目のロボットの遊脚側脚体の着地予定位置姿勢を該所定数の歩数目における前記第１仮着地予定位置姿勢に近づけるように該所定数の歩数目までのロボットの遊脚側脚体の第３仮着地予定位置姿勢を決定する手段と、
その決定した第３仮着地予定位置姿勢に対して前記所定の要求を満たす前記対象物の第１予想移動経路を推定する手段と、
前記第１仮着地予定位置姿勢に対して前記所定の要求を満たす前記対象物の第２予想移動経路を推定する手段と、
前記第１予想移動経路で前記対象物を移動させたとした場合における第１所定期間での該対象物の移動距離と、該第１所定期間の時間と、前記第２予想移動経路で前記対象物を移動させたとした場合における第２所定期間での該対象物の移動距離と、該第２所定期間の時間とに基づいて前記移動計画における対象物の速度指令値を修正する手段とを備える。

これによれば、前記第１予想移動経路で前記対象物を移動させたとした場合における第１所定期間での該対象物の移動距離と、該第１所定期間の時間と、前記第２予想移動経路で前記対象物を移動させたとした場合における第２所定期間での該対象物の移動距離と、該第２所定期間の時間とに基づいて前記移動計画における対象物の速度指令値を修正することによって、対象物の移動計画を適切に修正する、換言すれば、その修正後の移動計画に基づいて前記必要要件を満足する目標歩容を生成し得るように該移動計画の修正を行うことが可能となる。例えば、前記第１所定期間での該対象物の移動距離と該第１所定期間の時間（第１所定期間の開始時から終了時までの時間）とから対象物を前記第１予想移動経路で移動させたとした場合における該対象物の平均移動速度を決定でき、また、前記第２所定期間での該対象物の移動距離と該第２所定期間の時間（第２所定期間の開始時から終了時までの時間）とから対象物を前記第２予想移動経路で移動させたとした場合における該対象物の平均移動速度を決定できる。この場合、前者の平均移動速度は、前記必要要件を満足する上で好適な対象物の平均移動速度を意味し、後者の平均移動速度は、前記第１仮目標運動軌道に従う対象物の平均移動速度を意味する。従って、例えばこれらの平均移動速度の比率に応じて前記を算出し、前記移動計画における対象物の速度指令値を修正することによって、該移動計画の修正を適切に行なうことが可能となる。

なお、前記第１所定期間および第２所定期間は、一致していてもよいが、一致していなくてもよい。ただし、これらの第１所定期間および第２所定期間は、その終了時刻が例えば、前記所定数の歩数目の遊脚側脚体の着地時刻から次の遊脚側脚体の着地時刻までの期間内の時刻であることが望ましい。さらに、第１所定期間は、例えば前記所定数の歩数分の期間（１歩分の期間の所定数倍の期間）であることが望ましい。一方、第２所定期間は、例えば新たな目標歩容を生成しようとしている現在時刻と、前記所定数の歩数目の遊脚側脚体の着地時刻から次の遊脚側脚体の着地時刻までの期間内の時刻との間の期間など、１歩分の期間の所定数倍の期間でなくてもよい。

この場合、前記移動計画における対象物の速度指令値を修正する手段は、前記第１予想移動経路の方向が前記第２予想移動経路の方向と一致しないとき、前記第１所定期間での対象物の移動距離の代わりに、該第１予想移動経路を第２予想移動経路の方向に投影してなる経路で対象物を移動させたとした場合における前記第１所定期間での対象物の移動距離を用いることが好ましい。

すなわち、前記対象物の第１仮目標運動軌道が、旋回運動軌道のように、対象物の速度ベクトルの向きを時間的に変化させるような軌道である場合（第１仮目標運動軌道により定まる対象物の移動経路が直線状でない場合）には、一般に、前記第３仮着地予定位置姿勢に対して前記所定の要求を満たす対象物の第１予想移動経路と、前記第１仮着地予定位置姿勢に対して前記所定の要求を満たす対象物の第２予想移動経路（第１仮目標運動軌道による定まる対象物の移動経路に沿った経路）とは一致しない。そして、この場合、前記第１予想移動経路は、前記所定数の歩数目では、第１予想移動経路に近づくので、前記第３仮着地予定位置姿勢に対応するロボットの脚体の動作を行なって、対象物の移動させた場合には、ロボットの各歩数毎の実際の対象物の移動経路の向きは第２予想移動経路の向きに偏りやすい。従って、第１予想移動経路を第２予想移動経路の方向に投影してなる経路で対象物を移動させたとした場合における前記第１所定期間での対象物の移動距離は、前記第３仮着地予定位置姿勢に対応するロボットの脚体の動作を行なった場合の対象物の実際の移動距離の推定値としての信頼性が高い。従って、移動計画における対象物の速度指令値を修正するときに、前記第１所定期間での対象物の移動距離の代わりに、該第１予想移動経路を第２予想移動経路の方向に投影してなる経路で対象物を移動させたとした場合における前記第１所定期間での対象物の移動距離を用いることにより、移動計画をより適切に修正できる。

以下、添付図面を参照して本発明の実施形態に係る脚式移動ロボットの歩容生成装置を説明する。尚、脚式移動ロボットとしては２足移動ロボットを例にとる。

図１は、この実施形態に係る脚式移動ロボットとしての２足移動ロボットを全体的に示す概略図である。

図示の如く、２足移動ロボット（以下、ロボットという）１は上体(ロボット１の基体）３から下方に延設された左右一対の脚体（脚部リンク）２，２を備える。両脚体２，２は同一構造であり、それぞれ６個の関節を備える。その６個の関節は上体３側から順に、股（腰部）の回旋（回転）用（上体３に対するヨー方向の回転用）の関節１０Ｒ，１０Ｌ（符号Ｒ，Ｌはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。以下同じ）と、股（腰部）のロール方向（Ｘ軸まわり）の回転用の関節１２Ｒ，１２Ｌと、股（腰部）のピッチ方向（Ｙ軸まわり）の回転用の関節１４Ｒ，１４Ｌと、膝部のピッチ方向の回転用の関節１６Ｒ，１６Ｌと、足首のピッチ方向の回転用の関節１８Ｒ，１８Ｌと、足首のロール方向の回転用の関節２０Ｒ，２０Ｌとから構成される。

各脚体２の足首の２つの関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には、各脚体２の先端部を構成する足平（足部）２２Ｒ（Ｌ）が取着されると共に、両脚体２，２の最上位には、各脚体２の股の３つの関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）を介して前記上体３が取り付けられている。上体３の内部には、詳細を後述する制御ユニット６０などが格納される。なお、図１では図示の便宜上、制御ユニット６０を上体３の外部に記載している。

上記構成の各脚体２においては、股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から構成され、膝関節は関節１６Ｒ（Ｌ）から構成され、足首関節は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２４Ｒ（Ｌ）で連結され、膝関節と足首関節とは下腿リンク２６Ｒ（Ｌ）で連結される。

上体３の上部の両側部には左右一対の腕体（腕リンク）５，５が取り付けられると共に、上体３の上端部には頭部４が配置される。両腕体５，５は同一構造であり、それぞれ７個の関節を備える。すなわち、各腕体５は、３つの関節３０Ｒ（Ｌ），３２Ｒ（Ｌ），３４Ｒ（Ｌ）から構成された肩関節と、１つの関節３６Ｒ（Ｌ）から構成された肘関節と、３つの関節３８Ｒ（Ｌ），４０Ｒ（Ｌ），４２Ｒ（Ｌ）から構成された手首関節と、この手首関節に連結された手先部（ハンド）４０Ｒ（Ｌ）とを備えている。なお、頭部４は、本発明の要旨と直接的な関連を有しないため詳細な説明を省略する。

上記の構成により、各脚体２の足平２２Ｒ（Ｌ）は、上体３に対して６つの自由度を与えられている。そして、ロボット１の歩行等の移動中に、両脚体２，２を合わせて６＊２＝１２個（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）の関節を適宜な角度で駆動することで、両足平２２Ｒ，２２Ｌの所望の運動を行うことができる。これにより、ロボット１は任意に３次元空間を移動することができる。また、各腕体５は、上体３に対して７つの自由度を与えられ、両腕体５，５を合わせて７＊２＝１４個の関節を適宜な角度で駆動することで、後述する台車を押すなどの所望の作業を行なうことができる。

図１に示す如く、各脚体２の足首関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下方には足平２２Ｒ（Ｌ）との間に公知の６軸力センサ５０が介装されている。該６軸力センサ５０は、各脚体２の足平２２Ｒ（Ｌ）の着地の有無、および各脚体２に作用する床反力（接地荷重）等を検出するためのものであり、該床反力の並進力の３方向成分Ｆｘ，Ｆｙ，Ｆｚ並びにモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚの検出信号を制御ユニット６０に出力する。この６軸力センサ５０と同様の６軸力センサ５２が、各腕体５の手先部（ハンド）４０Ｒ（Ｌ）と手首関節３８Ｒ（Ｌ），４０Ｒ（Ｌ），４２Ｒ（Ｌ）との間に介装され、手先部４０Ｒ（Ｌ）に作用する外力の並進力の３方向成分並びにモーメントの３方向成分の検出信号が該６軸力センサ５２から制御ユニット６０に出力される。また、上体３には、Ｚ軸（鉛直方向（重力方向））に対する上体３の傾斜角およびその角速度、並びにＺ軸回りの上体３の回転角（ヨー角）およびその角速度を検出するための姿勢センサ５４が備えられ、その検出信号が該姿勢センサ５４から制御ユニット６０に出力される。この姿勢センサ５４は、図示を省略する３軸方向の加速度センサおよび３軸方向のジャイロセンサを備え、これらのセンサの検出信号が上体３の姿勢角（傾斜角およびヨー角）およびその角速度を検出するために用いられると共に、ロボット１の自己位置姿勢を推定するために用いられる。また、詳細構造の図示は省略するが、ロボット１の各関節には、それを駆動するための電動モータ６４（図２参照）と、その電動モータ６４の回転量（各関節の回転角）を検出するためのエンコーダ（ロータリエンコーダ）６５（図２参照）とが設けられ、該エンコーダ６５の検出信号が該エンコーダ６５から制御ユニット６０に出力される。

なお、図示は省略するが、各足平２２Ｒ（Ｌ）と６軸力センサ５０との間には、ばね等の弾性体が介装され、また、足平２２Ｒ（Ｌ）の底面には、ゴム等の弾性体が貼り付けられている。これらの弾性体は、コンプライアンス機構を構成するものであり、各脚体２が床反力を受けたときに弾性変形するようになっている。

図２は制御ユニット６０の構成を示すブロック図である。該制御ユニット６０はマイクロコンピュータにより構成されており、ＣＰＵからなる第１の演算装置９０及び第２の演算装置９２、Ａ／Ｄ変換器８０、カウンタ８６、Ｄ／Ａ変換器９６、ＲＡＭ８４、ＲＯＭ９４、並びにこれらの間のデータ授受を行うバスライン８２を備えている。この制御ユニット６０では、各脚体２の６軸力センサ５０、各腕体５の６軸力センサ５２、姿勢センサ５４（加速度センサおよびレートジャイロセンサ）等の出力信号はＡ／Ｄ変換器８０でデジタル値に変換された後、バスライン８２を介してＲＡＭ８４に送られる。またロボット１の各関節のエンコーダ６５（ロータリーエンコーダ）の出力は、カウンタ８６を介してＲＡＭ８４に入力される。

前記第１の演算装置９０は後述の如く目標歩容を生成すると共に、関節角変位指令（各関節の変位角もしくは各電動モータ６４の回転角の指令値）を算出し、ＲＡＭ８４に送出する。また、第２の演算装置９２はＲＡＭ８４から関節角変位指令と、前記エンコーダ６５の出力信号に基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出してＤ／Ａ変換器９６とサーボアンプ６４ａとを介して各関節を駆動する電動モータ６４に出力する。

図３は、本明細書の実施形態に係る脚式移動ロボットの制御装置の機能的構成を全体的に示すブロック図である。この図３中の破線で囲んだ部分が制御ユニット６０が実行する処理機能（主として第１の演算装置９０及び第２の演算装置９２の機能）によって構成されるものである。なお、以下の説明では、脚体２および腕体５の左右を特に区別する必要がないときは、前記符号Ｒ，Ｌを省略する。

以下説明すると、制御ユニット６０の機能的構成は、歩容生成装置１００、対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６から構成されている。この明細書の実施形態では、歩容生成装置１００の処理を除き、対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６の処理は、本願出願人が先に特開平１０−２３０４８５号公報にて提案したものと同じである。従って、以下の実施形態の説明では、歩容生成装置１００を中心に説明し、対象物反力平衡制御装置１０２、脚メイン制御装置１０４および腕メイン制御装置１０６の説明は概略的な説明に留める。

歩容生成装置１００は、ロボット１の目標歩容を自在かつリアルタイムに生成して出力するものである。この歩容生成装置１００が出力する目標歩容は、目標上体位置姿勢軌道（上体３の目標位置及び目標姿勢軌道）、目標足平位置姿勢軌道（各足平２２の目標位置及び目標姿勢軌道）、目標手先位置姿勢軌道（各腕体５の手先部４４の目標位置および目標姿勢軌道）、目標全床反力中心点軌道（全床反力中心点の目標位置の軌道））、目標全床反力軌道、および目標対象物反力軌道から構成される。なお、脚体２や腕体５以外に上体３に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。

ここで、歩容生成装置１００が生成する目標歩容に係る用語の意味および定義について説明しておく。歩容における「軌道」は時間的変化のパターン（時系列パターン）を意味する。

足平位置、上体位置など、ロボット１の各部位の「位置」は、その部位に固定的に設定されたある代表点の位置を意味する。例えば、本実施形態では、各足平２２の代表点は、その足平２２を備える脚体２の足首関節の中心から、該足平２２の底面に延ばした垂線が該底面と交わる点であり、その代表点の位置が足平位置である。また、「姿勢」は空間的な向きを意味する。具体的には、例えば上体姿勢はＺ軸（鉛直軸）に対するロール方向（Ｘ軸まわり）の上体３の傾斜角とピッチ方向（Ｙ軸まわり）の上体３の傾斜角とヨー方向（Ｚ軸まわり）の上体３の回転角（ヨー角）とで表され、足平姿勢は各足平２２に固定的に設定された２軸の空間的な方位角で表される。

なお、歩容のうちの床反力および対象物反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上体位置姿勢等、ロボット１の運動に係わる歩容を総称的に「運動」という。また、運動の軌道は、一定（時間的に変化しない）あるいはほぼ一定のものであってもよい。例えばロボット１がその両足平２２，２２を着地させた状態に維持して、移動しない状態では、足平位置姿勢軌道は一定になる。

歩容のうちの対象物反力は、ロボット１にある対象物を移動させるときに該対象物にロボット１から作用させる力の反力（対象物からロボット１に作用する力）を意味する。本明細書の実施形態では、例えば図４に示すように、ロボット１がある対象物１２０（図示の例では台車）の所定の部位に両腕体５，５の手先部４４Ｒ，４４Ｌを係合させた状態で歩行動作を行いながら該対象物１２０を押す作業を行なう場合を例にとって説明する。この対象物１２０からロボット１が受ける力が対象物反力である。対象物反力は、一般的には、並進力成分とモーメント成分とから構成されるが、本明細書の実施形態では、対象物反力はロボット１に対象物１２０から作用する並進力を意味するものとし、その並進力によってロボット１に作用する、ある点回りのモーメントを対象物反力モーメントという。補足すると、対象物反力は、ロボット１から対象物１２０に作用させる力の符号（向き）を反転させたものであるので、ロボット１から対象物１２０への作用力を対象物反力の代わりに用いてもよい。

なお、本明細書では、対象物１２０の運動に関し、対象物１２０の位置はロボット１の足平位置などと同様に、対象物１２０に固定的に設定された代表点の位置を意味する。また、対象物１２０の姿勢はロボット１の足平姿勢などと同様に、対象物１２０の空間的な向きを意味する。そして、対象物１２０の位置姿勢を総称的に対象物運動ということがある。

また、各足平２２に作用する床反力（並進力及びモーメントからなる床反力）を「各足平床反力」と呼び、ロボット１の全て（２つ）の足平２２Ｒ，２２Ｌについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義として扱う。

目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に目標床反力中心点（全床反力の中心点の目標位置）を作用点にして目標床反力を表現すると、目標床反力のモーメント成分は、鉛直成分（鉛直軸（Ｚ軸）まわりのモーメント）を除いて０になる。換言すれば、目標床反力中心点まわりの目標床反力のモーメントの水平成分（水平軸（Ｘ軸及びＹ軸）回りのモーメント）は０になる。そこで、本明細書の実施形態では、目標床反力中心点を目標全床反力の作用点として、歩容生成装置１００で生成するものとした。なお、目標床反力中心点は、目標全床反力の作用点としての意味を持つものであるから、本質的には目標全床反力の１つの構成要素であると言える。但し、本明細書の実施形態では、目標床反力のうちの目標床反力中心点がロボット１の動作制御と密接に関連するものであることから、図３では、目標全床反力のうちの作用点である目標床反力中心点を目標全床反力とは別に記載するものとした。

また、本明細書では、ＺＭＰ(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は、解析的あるいは探索的に求めることが可能である。

このように目標対象物反力軌道を修正することにより、ロボット１が積極的に対象物１２０を押すような上体３の運動が行なわれるように今回歩容パラメータのうちの対象物反力モーメント軌道パラメータがＳ１５で決定されることとなる。その結果、この今回歩容パラメータを基に生成される目標歩容が、安定余裕を確保しつつ、対象物１２０にロボット１の上体３が衝突することが無いようなような歩容になる。

補足すると、Ｓ２１において、目標対象物反力軌道を修正する代わりに、もしくは、その修正に加えて、今回歩容および次回歩容を含み複数歩分の遊脚足平２２の着地予定位置姿勢もしくは着地予定時刻を前記対象物・ロボット間位置姿勢要求を満たす範囲内で修正するようにしてもよい。この場合には、Ｓ２１の処理の後、Ｓ０７からの処理を実行するようにすればよい。

Ｓ２１の処理を行なった後、Ｓ０９〜Ｓ１７の処理が前記したように再度実行される。このとき、Ｓ１７の次のＳ１９の判断結果はＹＥＳになり、この場合には、次に図６のＳ２３に進む。

Ｓ２３では、所要の必要要件を満足する脚体２，２の運動が可能であるか否かを判断する判断処理が実行される。この判断処理は、本発明における判断手段に相当している。

ここで、本実施形態では、前記必要要件は、例えば、将来の２歩目の遊脚足平２２の着地予定位置姿勢（次回歩容での遊脚足平２２の着地予定位置姿勢）が、その着地予定時刻から半歩分の期間が経過した時刻において、前記対象物・ロボット間位置姿勢要求を満たすという条件と、その２歩目までの各歩容において、ロボット１の遊脚足平２２の着地予定位置が、支持脚足平２２に対して所定の許容領域内に存在するという条件とから構成される。後者の条件は、本発明における制約条件に相当するものである。

後者の条件に関して補足すると、ロボット１の遊脚足平２２の運動は、遊脚側脚体２が支持脚側脚体２に干渉しないこと、各脚体２の各関節の変位量や変位速度が所定の許容範囲を超えないこと、遊脚足平２２の運動中におけるロボット１のＺＭＰがロボット１の接地面内（支持多角形内）の、ロボット１の安定余裕を確保し得る領域から逸脱しないことなどの制約を受ける。そして、これらの制約を考慮して、各単位歩容における遊脚足平２２の着地予定位置姿勢の許容領域（支持脚足平２２に対する相対的な許容領域）があらかじめ設定されている。この許容領域は、概ね楕円形状（支持脚座標系のＸ軸に平行な方向に長い楕円形状）の領域である。以下、この許容領域を遊脚足平着地位置姿勢許容領域という。

以下に、Ｓ２３の判断処理を図１６および図１７を適宜、参照しつつ、具体的に説明する。この判断処理は、次の（１）〜（５）の手順で実行される。
（１）：まず、今回歩容（１歩目）の遊脚足平２２の着地予定時刻と、次回歩容（２歩目）の遊脚足平２２の着地予定時刻と、現在の目標対象物運動軌道とを基に、それらの着地予定時刻から半歩分の時間（歩容周期の半分の時間）が経過した時刻における対象物１２０の位置姿勢を求める。この場合、今回歩容および次回歩容の遊脚足平２２の着地予定位置姿勢は、前記Ｓ０５の処理に関して説明した如く決定されているので、求められる対象物１２０の位置姿勢は、２歩目までの各単位歩容における遊脚足平２２の着地予定位置姿勢との間で前記対象物・ロボット間位置姿勢要求を満たすものである。

図１６は、現在時刻での対象物１２０の位置姿勢と、上記（１）の処理により求められる対象物１２０の位置姿勢とを例示している。この例では、対象物運動軌道の経路は、図中の破線の曲線で示す如く時計まわり方向に旋回する経路である。図１６において、点Ｃ０、Ｃ１、Ｃ２は、それぞれ現在時刻での対象物１２０の位置（対象物１２０の代表点の位置）、今回歩容（１歩目）の遊脚足平２２の着地予定時刻から半歩分の時間が経過した時刻での対象物１２０の位置、次回歩容（２歩目）の遊脚足平２２の着地予定時刻から半歩分の時間が経過した時刻での対象物１２０の位置を表している。

なお、図１６の例では、今回歩容の支持脚足平２２が左側脚体２Ｌの足平２２Ｌであり、１歩目の遊脚足平２２は、右側脚体２Ｒの足平２２Ｒ、２歩目の遊脚足平２２は、左側脚体２Ｌの足平２２Ｌである。この場合、図１６中に破線の長方形で示す現在時刻での遊脚足平２２Ｒは空中にある。
（２）：次いで、現在時刻での対象物１２０の実際の位置（今回歩容の支持脚座標系での位置）である実対象物位置と、上記（１）の処理で求めた対象物１２０の位置とから、図１６に示すような移動距離Ｌ01，Ｌ12とを算出する。移動距離Ｌ01は、現在時刻での実対象物位置（点Ｃ０の位置）から今回歩容の遊脚足平２２の着地予定時刻から半歩分の時間が経過した時刻での対象物１２０の位置（点Ｃ１の位置）までの移動ベクトルＶ01の長さであり、移動距離Ｌ12は、今回歩容の遊脚足平２２の着地予定時刻から半歩分の時間が経過した時刻での対象物１２０の位置（点Ｃ１の位置）から、次回歩容の遊脚足平２２の着地予定時刻から半歩分の時間が経過した時刻での対象物１２０の位置（点Ｃ２の位置）までの移動ベクトルＶ12の長さである。なお、実対象物位置は、現在時刻におけるロボット１の各関節の変位量の検出値（もしくは目標値）から幾何学的演算によって決定される手先部４４の位置（ロボット１と対象物１２０との係合部分の位置）を基に求められる。また、移動距離Ｌ01，Ｌ12は、後述のＳ２９の処理で使用するものであり、該Ｓ２９の処理で求めるようにしてもよい。

補足すると、移動ベクトルＶ01およびＶ12により表される対象物１２０の移動経路は、本発明における第１予想移動経路に相当する。
（３）：次いで、遊脚足平２２の前記遊脚足平着地位置姿勢許容領域によって、今回歩容の遊脚足平２２の着地予定位置姿勢（以下、今回遊脚着地予定位置姿勢という）と次回歩容の遊脚足平２２の着地予定位置姿勢（以下、次回遊脚着地予定位置姿勢という）をそれぞれ制限し、第１制限済み遊脚着地予定位置姿勢を決定する。この第１制限済み遊脚着地予定位置姿勢は、次のように決定される。

すなわち、今回遊脚着地予定位置姿勢の現在仮決定値（Ｓ２３の処理の開始時における値）が、今回歩容支持脚座標系における前記遊脚着地位置姿勢許容領域内に存在する場合には、その今回遊脚着地予定位置姿勢の現在仮決定値をそのまま、今回歩容の第１制限済み遊脚着地予定位置姿勢（以下、今回第１制限済み遊脚着地予定位置姿勢という）として決定する。一方、今回遊脚着地予定位置姿勢の現在仮決定値が、遊脚着地位置姿勢許容領域から逸脱している場合には、該許容領域内で、今回遊脚着地予定位置姿勢の現在仮決定値に最も近くなるような遊脚足平２２の着地位置姿勢を今回第１制限済み遊脚着地予定位置姿勢として決定する。

また、次回遊脚着地予定位置姿勢の現在仮決定値（Ｓ２３の処理の開始時における値）が、今回第１制限済み遊脚着地予定位置姿勢に対応して定まる次回歩容支持脚座標系において、前記遊脚着地位置姿勢許容領域内に存在する場合には、その次回遊脚着地予定位置姿勢の現在仮決定値をそのまま、次回歩容の第１制限済み遊脚着地予定位置姿勢（以下、次回第１制限済み着地予定位置姿勢という）として決定する。一方、次回遊脚着地予定位置姿勢の現在仮決定値が、今回第１制限済み遊脚着地予定位置姿勢に対応して定まる次回歩容支持脚座標系において、前記遊脚着地位置姿勢許容領域から逸脱している場合には、該許容領域内で、次回遊脚着地予定位置姿勢の現在仮決定値に最も近くなるような遊脚足平２２の着地位置姿勢を次回第１制限済み遊脚着地予定位置姿勢として決定する。

補足すると、上記今回第１制限済み遊脚着地予定位置姿勢および次回遊脚着地予定位置姿勢は、本発明における第２仮着地予定位置姿勢に相当する。
（４）：次いで、次回第１制限済み遊脚着地予定位置姿勢が次回遊脚足平予定位置姿勢の現在仮決定値に対して一致するか否か、あるいは、次回第１制限済み遊脚着地予定位置姿勢が次回歩容の遊脚足平２２の着地予定時刻における対象物１２０の位置姿勢に対して前記対象物・ロボット間位置姿勢要求を満足するか否か（換言すれば次回第１制限済み遊脚着地予定位置姿勢が次回遊脚足平予定位置姿勢の現在仮決定値とほぼ一致するか否か）を判断する。

（４ａ）：上記（４）の判断結果が否定的である場合には、次回遊脚着地予定位置姿勢の現在仮決定値と次回第１制限済み遊脚着地予定位置姿勢との差分だけ、今回第１制限済み遊脚着地予定位置姿勢に加えることで、今回第１制限済み遊脚着地予定位置姿勢を補正する。そして、この補正後の今回第１制限済み遊脚着地予定位置姿勢を、前記遊脚着地位置姿勢許容領域によって再び制限し、今回第２制限済み遊脚着地予定位置姿勢を決定する。具体的には、補正後の今回第１制限済み遊脚着地予定位置姿勢が、今回歩容支持脚座標系において、遊脚着地位置姿勢許容領域内に存在する場合には、その補正後の今回第１制限済み遊脚着地予定位置姿勢をそのまま、今回第２制限済み遊脚着地予定位置姿勢として決定する。また、補正後の今回第１制限済み遊脚着地予定位置姿勢が、今回歩容支持脚座標系において、遊脚着地位置姿勢許容領域から逸脱している場合には、該許容領域内で、前記補正後の今回第１制限済み遊脚着地予定位置姿勢に最も近くなるような遊脚足平２２の着地位置姿勢を今回第２制限済み遊脚着地予定位置姿勢として決定する。

（４ｂ）：また、上記（４）の判断結果が肯定的である場合には、前記（３）の処理で決定された今回第１制限済み遊脚着地予定位置姿勢がそのまま、今回第２制限済み遊脚着地予定位置姿勢として決定される。

図１６に示した例では、例えば図１７の斜線を付した遊脚足平２２Ｒで示す如く、今回第２制限済み遊脚着地予定位置姿勢が決定される。この例では、遊脚着地位置姿勢許容領域による制限によって、今回第２制限済み遊脚着地予定位置姿勢は、今回遊脚着地予定位置姿勢の現在仮決定値と異なる位置姿勢となっている。
（５）：次に、上記（４ａ）または（４ｂ）の処理の後、今回第２制限済み着地予定位置姿勢に対応して定まる次回歩容支持脚座標系における遊脚着地位置姿勢許容領域内に、次回遊脚着地予定位置姿勢の現在値が存在するか否か、あるいは、今回第２制限済み着地予定位置姿勢に対応して定まる次回歩容支持脚座標系における遊脚着地位置姿勢許容領域を満足し、且つ、次回歩容の遊脚足平２２の着地予定時刻における対象物１２０の位置姿勢に対して前記対象物・ロボット間位置姿勢要求を満足できる次回歩容の遊脚足平２２の着地予定位置姿勢（これは次回遊脚着地予定位置姿勢の現在値に一致またはほぼ一致する）を設定することが可能あるか否かが判断される。

（５ａ）：上記（５）の判断結果が肯定的であるということは、前記必要要件を満足するように、今回歩容および次回歩容の遊脚足平２２の着地予定位置姿勢を決定することが可能であることを意味する。従って、この場合には、Ｓ２３の判断処理の結果をＹＥＳとし、後述するＳ２５の処理に進む。

（５ｂ）：一方、上記（５）の判断結果が否定的である場合には、対象物１２０の現状の移動計画では、前記必要要件を満足し得るような、今回歩容および次回歩容の遊脚足平２２の着地予定位置姿勢を決定できないことを意味する。そこで、この場合には、Ｓ２３の判断処理の結果をＮＯとし、後述のＳ２９の処理に進む。

以上が、Ｓ２３の判断処理の詳細である。

図６のフローチャートの説明に戻って、Ｓ２３の判断結果がＹＥＳである場合（該判断結果が肯定的である場合）に実行されるＳ２５の処理では、前記Ｓ２３の判断処理における前記（４ａ）または（４ｂ）の処理で決定された前記今回第２制限済み遊脚着地予定位置姿勢が、今回遊脚着地予定位置姿勢の現在値に一致しているか否かが判断される。

この判断結果がＹＥＳである場合には、今回遊脚着地予定位置姿勢の現在仮決定値および次回遊脚着地予定位置姿勢の現在仮決定値は、前記必要要件を満足し得るものとなっている。そこで、この場合には、後述のＳ３１の処理に進む。

また、Ｓ２５の判断結果がＮＯである場合には、Ｓ２７に進んで、今回歩容および次回歩容の遊脚足平２２の着地予定位置姿勢を再度、仮決定する。この場合、今回歩容の遊脚足平２２の着地予定位置姿勢の新たな仮決定値として、前記Ｓ２３の判断処理における前記（４ａ）または（４ｂ）の処理で決定された前記今回第２制限済み遊脚着地予定位置姿勢が設定される。

また、この前記次回遊脚着地予定位置姿勢の現在仮決定値が、次回歩容の遊脚足平２２の着地予定位置姿勢の新たな仮決定値として改めて設定される。あるいは、今回第２制限済み遊脚着地予定位置姿勢に対応して定まる次回歩容支持脚座標系における遊脚着地位置姿勢許容領域を満足し、且つ、次回歩容の遊脚足平２２の着地予定時刻における対象物１２０の位置姿勢に対して前記対象物・ロボット間位置姿勢要求を満足できる次回歩容の遊脚足平２２の着地予定位置姿勢を改めて求め、その求めた着地予定位置姿勢を次回歩容の遊脚足平２２の着地予定位置姿勢の新たな仮決定値として設定してもよい。

そして、このＳ２７の処理の後に、前記図５のＳ０７のからの処理が再度実行される。

補足すると、Ｓ２７で仮決定される今回遊脚着地予定位置姿勢および次回遊脚着地予定位置姿勢のうち、次回遊脚着地予定位置姿勢は、次回歩容の遊脚足平２２の着地予定時刻から半歩分の時間が経過した時刻における対象物１２０との位置姿勢（現在の移動計画に基づく位置姿勢）に対して前記対象物・ロボット間位置姿勢要求を満足する。これに対して、Ｓ２７で仮決定される今回遊脚着地予定位置姿勢は、一般には今回歩容の遊脚足平２２の着地予定時刻から半歩分の時間が経過した時刻における対象物１２０の位置姿勢（現在の移動計画に基づく位置姿勢）に対して前記対象物・ロボット間位置姿勢要求を満たさない。これは、前記Ｓ０５で１歩目および２歩目の着地予定位置姿勢を決定する場合と異なり、Ｓ２７では、１歩目の遊脚足平２２の着地予定位置姿勢を大きめに踏み込ませることで、今回歩容における対象物１２０の移動量を移動計画よりも大きく採り、現在時刻から次回歩容の終了時（次回歩容の遊脚足平２２の着地予定時刻）までに、対象物１２０を移動計画に基づく位置姿勢に一致させるように、今回遊脚着地予定位置姿勢および次回遊脚着地予定位置姿勢を仮決定するためにである。

一方、Ｓ２３の判断結果がＮＯである場合（該判断結果が否定的である場合）に実行されるＳ２９の処理では、移動計画の修正処理が実行される。この処理は、次の（６）〜（１１）の手順で実行される。
（６）：まず、Ｓ２３の判断処理における前記（４ａ）または（４ｂ）の処理で決定された前記今回第２制限済み遊脚着地予定位置姿勢（図１７を参照）に対応して定まる次回歩容支持脚座標系における遊脚着地位置姿勢許容領域を満足し、且つ、次回遊脚着地予定位置姿勢の現在仮決定値に最も近い着地予定位置姿勢を、次回第２制限済み遊脚着地予定位置姿勢を決定する。

図１７の例では、例えば同図の斜線を付した遊脚足平２２Ｌで示す如く、次回第２制限済み遊脚着地予定位置姿勢が決定される。この例では、遊脚着地位置姿勢許容領域による制限によって、次回第２制限済み遊脚着地予定位置姿勢は、次回遊脚着地予定位置姿勢の現在仮決定値と異なる位置姿勢となっている。

補足すると、ここで決定される次回第２制限済み遊脚着地予定位置姿勢と、前記今回第２制限済み遊脚着地予定位置姿勢は、本発明における第３仮着地予定位置姿勢に相当するものである。
（７）：次いで、今回歩容の支持脚足平２２（図１７の例では２２Ｌ）に対して、最適な対象物１２０の位置姿勢を決定する。その最適な位置姿勢は、今回歩容の開始時から半歩分の時間が経過した時刻における対象物１２０の最適な位置姿勢を意味し、今回歩容の開始時の支持脚足平２２の位置姿勢（すなわち前回歩容の遊脚足平２２の着地位置姿勢）との間で前記対象物・ロボット間位置姿勢要求を満足するように決定される。

図１７の例では、点Ｃ0aの位置にある破線で示す対象物１２０の位置姿勢が、今回歩容の開始時から半歩分の時間が経過した時刻における最適な位置姿勢（以下、対象物１２０の今回歩容内最適位置姿勢という）として決定される。

さらに、前記今回第２制限済み遊脚着地予定位置姿勢に対して、前記対象物・ロボット間位置姿勢要求を満足する対象物１２０の位置姿勢が、今回歩容の（１歩目の）遊脚足平２２の着地予定時刻から半歩分の時間が経過した時刻における対象物１２０の最適な位置姿勢である今回着地後最適位置姿勢として決定される。同様に、前記次回第２制限済み遊脚着地予定位置姿勢に対して、前記対象物・ロボット間位置姿勢要求を満足する対象物１２０の位置姿勢が、次回歩容の（２歩目の）遊脚足平２２の着地予定時刻から半歩分の時間が経過した時刻における対象物１２０の最適な位置姿勢である次回着地後最適位置姿勢として決定される。

図１７の例では、点Ｃ1aの位置にある破線で示す対象物１２０の位置姿勢が、今回着地後最適位置姿勢として決定され、点Ｃ2aの位置にある破線で示す対象物１２０の位置姿勢が、次回着地後最適位置姿勢として決定される。
（８）：次いで、上記の如く決定した対象物１２０の今回歩容内最適位置姿勢と、今回着地後最適位置姿勢と、次回着地後最適位置姿勢とを基に、今回歩容および次回歩容の遊脚足平２２の着地予定位置姿勢をそれぞれ前記今回第２制限済み遊脚着地予定位置姿勢、次回第制限済み遊脚着地予定位置姿勢に設定した場合における対象物１２０の移動距離Ｗ01，Ｗ12（図１７参照）を算出する。図１７に示す如く、移動距離Ｗ01は、対象物１２０の今回歩容内最適位置姿勢における位置（点Ｃ0aの位置）から今回着地後最適位置姿勢における位置（点Ｃ1aの位置）までの移動ベクトルＶ01aの長さであり、移動距離Ｗ12は、対象物１２０の今回着地後最適位置姿勢における位置（点Ｃ1aの位置）から次回着地後最適位置姿勢における位置（点Ｃ2aの位置）までの移動ベクトルＶ12aの長さである。

補足すると、移動ベクトルＶ01aおよびＶ12aにより表される対象物１２０の移動経路は、本発明における第１予想移動経路に相当する。また、前記（２）の処理で求めた移動ベクトルＶ01およびＶ12により表される対象物１２０の移動経路は、本発明における第２予想移動経路に相当する。
（９）：次いで、上記の如く求めた移動距離Ｗ01と、移動ベクトルＶ01aと、前記Ｓ２３の判断処理における（２）の処理で求めた移動ベクトルＶ01とを基に、移動ベクトルＶ01aを移動ベクトルＶ01の方向に投影してなるベクトルの長さＷ01s（移動距離Ｗ01の移動ベクトルＶ01の方向への射影成分）を求める（図１８参照）。すなわち、移動ベクトルＶ01aと移動ベクトルＶ01とのなす角度の余弦値を、Ｗ01に乗じることによりＷ01sを求める。同様に、移動距離Ｗ12と、移動ベクトルＶ12aと、前記Ｓ２３の判断処理における（２）の処理で求めた移動ベクトルＶ12とを基に、移動ベクトルＶ12aを移動ベクトルＶ12の方向に投影してなるベクトルの長さＷ12s（移動距離Ｗ12の移動ベクトルＶ12の方向への射影成分）を求める（図１８参照）。すなわち、移動ベクトルＶ12aと移動ベクトルＶ12とのなす角度の余弦値を、Ｗ12に乗じることによりＷ12sを求める。

ここで、前記の如く決定される今回第２制限済み遊脚着地予定位置姿勢および次回第２制限済み遊脚着地予定位置姿勢は、少なくとも次回歩容において（２歩目）において、対象物１２０を現在の移動計画に基づく対象物運動軌道の経路（図１８では破線で示す経路）に近づけるように決定されるので、その経路が対象物１２０を旋回させるような経路（より一般的には、対象物１２０の速度ベクトルの向きが経時的に変化するような経路）である場合には、一般に、１歩目（今回歩容）の移動ベクトルＶ01a，Ｖ01の向きが互いに異なると共に、２歩目（次回歩容）の移動ベクトルＶ12a，Ｖ12の向きが互いに異なる。そして、ロボット１の今回歩容および次回歩容における遊脚足平２２の着地予定位置姿勢を、それぞれ今回第２制限済み遊脚着地予定位置姿勢および次回第２制限済み遊脚着地予定位置姿勢に決定して対象物１２０の移動を実際に行なった場合には、対象物１２０の実際の移動経路は、それらの着地予定位置姿勢に対して好適な対象物１２０の位置（図１８の点Ｃ1a，Ｃ2aと通る経路）よりも、現在の移動計画に基づく対象物運動軌道の経路（図１８では破線で示す経路）寄りに偏りやすい。

そこで、本実施形態では、上記の如く移動距離Ｗ01s，Ｗ12sを求めることによって、ロボット１の今回歩容および次回歩容における遊脚足平２２の着地予定位置姿勢を、それぞれ今回第２制限済み遊脚着地予定位置姿勢および次回第２制限済み遊脚着地予定位置姿勢に決定した場合における、前記今回歩容内最適位置姿勢に対応する時刻（今回歩容の開始時から半歩分の時間が経過した時刻）から前記今回着地後最適位置姿勢に対応する時刻（次回歩容の開始時から半歩分の時間が経過した時刻）までの対象物１２０の実効的な移動距離と、前記今回着地後最適位置姿勢に対応する時刻から前記次回着地後最適位置姿勢に対応する時刻（次次回歩容の開始時から半歩分の時間が経過した時刻）までの対象物１２０の実効的な移動距離を推定するようにした。つまり、上記の如く求められる移動距離Ｗ01s，Ｗ12sは、それぞれ当該実効的な移動距離の推定値を意味する。

なお、対象物１２０の現在の移動計画に基づく移動経路が直線状である場合には、移動ベクトルＶ01a，Ｖ01の向きと、移動ベクトルＶ12a，Ｖ12の向きとは互いにほぼ同一となる。この場合には、移動距離Ｗ01s，Ｗ12sは、それぞれＷ01，Ｗ02にほぼ一致するので、上記（９）の処理は省略してもよい。この場合には、Ｗ01s＝Ｗ01、Ｗ02s＝Ｗ02とすればよい。

補足すると、前記今回歩容内最適位置姿勢に対応する時刻から、前記次回着地後最適位置姿勢に対応する時刻までの期間は、本発明における第１所定期間に相当する。
（１０）：次いで、上記の如く（９）の処理で求めた移動距離Ｗ01s，Ｗ12sの総和（＝Ｗ01s＋Ｗ12s）と、前記Ｓ２３の処理における（２）の処理で求めた移動距離Ｌ01，Ｌ12の総和（＝Ｌ01＋Ｌ12）との比（＝（Ｗ01s＋Ｗ12s）／（Ｌ01＋Ｌ12））に、次式１００により決定される補正係数Ｋtを乗じることで、移動計画における対象物１２０の目標移動速度（速度指令値）を修正するための速度指令修正パラメータＶelを決定する。つまり、Ｖelを式１０１により決定する。

Ｋt＝（Ｔrest＋Ｔｍ＋０．５・Ｔm）／（２・Ｔｍ） ……式１００

Ｖel＝Ｋt・（Ｗ01s＋Ｗ12s）／（Ｌ01＋Ｌ12） ……式１０１

なお、式１００におけるＴrestは、現在時刻から今回歩容の終端時刻までの残り時間、Ｔmは単位歩容の周期である。また、補正係数Ｋtは、その上限値を１として、式１００の右辺の演算結果が１を超える場合には、Ｋt＝１とされる。従って、Ｖelは、１以下の非負の値である。

ここで、式１００の分母は、前記今回歩容内最適位置姿勢に対応する時刻から、次回着地後最適位置姿勢に対応する時刻までの期間の時間（これは歩容周期Ｔｍの２倍の時間である）、換言すれば、（Ｗ01s＋Ｗ12s）の移動距離だけ、現在の移動計画に基づく対象物１２０の移動経路に沿わせるようにして対象物１２０を移動させるときに要する時間を意味する。また、式１００の分子は、対象物１２０を図１６〜図１８に示す現在位置（点Ｃ0の位置）から、点Ｃ2の位置まで移動させるにのに要する時間、換言すれば、（Ｌ01＋Ｌ12）の移動距離だけ、現在の移動計画に基づく対象物１２０の将来の移動経路に沿わせるようにして対象物１２０を移動させるときに要する時間を意味する。なお、この場合、図１７の点Ｃ0から点Ｃ1まで対象１２０を移動させるのに要する時間が、Ｔrest＋０．５・Ｔｍであり、点Ｃ1から点Ｃ2まで対象物１２０を移動させるのに要する時間がＴｍである。補足すると、現在時刻から、点Ｃ2に対応する時刻（次次回歩容の開始時から半歩分の時間が経過した時刻）までの期間は、本発明における第２所定期間に相当する。

従って、式１０１により決定される速度指令修正パラメータＶelは、（Ｗ01s＋Ｗ12s）の移動距離だけ対象物１２０を移動させるときの対象物１２０の平均移動速度と、(Ｌ01＋Ｌ12）の移動距離だけ対象物１２０を移動させるときの対象物１２０の平均移動速度との比を意味する。換言すれば、前記対象物・ロボット間位置姿勢要求と遊脚足平着地位置姿勢許容領域とを満足する上で好適な対象物１２０の平均移動速度と、現在の移動計画に基づく対象物１２０の平均移動速度との比が速度指令修正パラメータＶelである。
（１１）そこで、本実施形態では、次に、現在の移動計画に基づく対象物１２０の目標移動速度（現在時刻からロボット１の複数歩分の期間における目標移動速度の時系列）に、上記の如く（１０）の処理で求めた速度指令修正パラメータＶelを乗じることにより、移動計画を修正する。つまり、修正後の移動計画に基づく、対象物１２０の目標移動速度（現在時刻からロボット１の複数歩分の期間における目標移動速度の時系列）が、修正前の移動計画に基づく目標移動速度にＶelを乗じた値になるように移動計画を修正する。

以上が、Ｓ２９における処理の詳細である。

図５および図６のフローチャートの説明に戻って、Ｓ２９の処理の後には、図５のＳ０３からの処理が再度実行される。

そして、以上説明した処理によって、最終的に図６のＳ２５の判断処理の判断結果がＹＥＳになると、次に、図７のＳ３１からの処理が実行される。

なお、Ｓ３１からの処理を実行する前に、現在決定されている今回歩容パラメータと定常歩容パラメータとを基に、現在時刻から前記第２旋回歩容の終端までの目標上体位置姿勢軌道を試行的に作成すると共に、現在の目標対象物運動軌道を基に目標手先位置姿勢軌道を試行的に決定し、それらに基づいて、腕体４の伸びきりや、上体３と対象物１２０との干渉等が発生しないか否かを確認してもよい。そして、この確認により、腕体４の伸びきりや、上体３と対象物１２０との干渉等が発生するような場合には、今回歩容や次回歩容の着地予定位置姿勢、あるいは、目標対象物運動軌道、もしくは対象物１２０の移動計画をさらに修正するようにしてもよい。

図７のＳ３１の処理では、今回歩容パラメータ（Ｓ０１〜Ｓ２９までの処理で、最終的に決定された今回歩容パラメータ）を基に、目標ＺＭＰを満足するように、手先位置姿勢瞬時値を除く、現在時刻の今回歩容瞬時値を算出する。

この算出処理は、次のように行なわれる。すなわち、今回歩容パラメータのうちの足平軌道パラメータを基に、目標足平位置姿勢の瞬時値が算出される。この算出処理は、前記Ｓ１３の処理の中で、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から式１９によって、上体位置摂動量が求められる。なお、本実施形態では、修正目標上体位置姿勢のうちの修正目標上体姿勢は、目標上体姿勢に一致させるものとし、上体姿勢の摂動量は０とする。

対象物反力平衡制御装置１０２では、このようにして摂動動力学モデル２８０から出力される上体位置摂動量を目標上体位置姿勢（歩容生成装置１００の出力）に加算部２８２で加算することにより、修正目標上体位置姿勢を算出する。

以上が対象物反力平衡制御装置１０２の具体的な演算処理である。

以上説明した第１実施形態における歩容生成装置１００の処理によれば、前記必要要件を満足し得る範囲内で、将来の２歩目までの（次回歩容までの）遊脚足平２２の着地予定位置姿勢を決定し、将来の２歩目の（次回歩容の）遊脚足平２２の着地後の単位歩容（該遊脚足平２２が支持脚足平に切り換わる歩容）の期間内で、対象物１２０の位置姿勢を、移動計画に基づく位置姿勢の目標値に到達させ得るように今回歩容が生成されることとなる。このため、前記遊脚足平着地位置姿勢許容領域による制約によって、１歩毎に対象物１２０の位置姿勢を、移動計画に基づく位置姿勢の目標値に到達させることができないような状況であっても、移動計画を修正することなく、２歩単位で、対象物１２０を移動計画に則して移動させることが可能な目標歩容（今回歩容）を生成できる。この結果、移動計画に基づく対象物１２０の移動経路の方向（ロボット１に対する方向）によらずに、移動計画の修正頻度を少なくして、対象物１２０を移動計画に則して移動させることが可能な目標歩容（今回歩容）を生成できる。この場合、対象物１２０の移動経路は、ロボット１の前後方向に直線的に対象物１２０を移動させるような経路でもよいことはもちろん、対象物１２０の旋回移動を行なうような経路、あるいは、ロボット１の横歩きを行なわせながら、ロボット１の横方向に対象物１２０を移動させていくような経路であってもよい。

そして、前記遊脚足平着地位置姿勢許容領域による制約によって、必要要件を満足し得る遊脚足平２２の着地予定位置姿勢を決定することができないような状況では前記Ｓ２９の処理によって、移動計画を修正するので、その修正を適切に行なうことができる。

なお、前記第１実施形態では、Ｓ０３（図５を参照）の演算処理で用いる対象物動力学モデルとして、図９に示した順動力学モデルを用いたが、これに代えて、逆動力学モデルを用いて、Ｓ０３の演算処理を行なうようにしてもよい。以下に、その場合の一例を第２実施形態として説明する。

図２２は、本実施形態でのＳ０３の演算処理を示すブロック図であり、このブロック図中の破線で囲んだ部分が対象物動力学モデルに相当するものとなっている。この対象物動力学モデルは、対象物１２０の移動計画に基づく対象物１２０の目標運動状態量としての目標対象物移動速度を入力として、対象物１２０に作用する力（並進力）と対象物１２０の位置とを出力する逆動力学モデルである。より詳しくは、この対象物動力学モデルは、目標対象物移動速度（入力値）を微分器３００で微分（１階微分）することにより、対象物１２０の運動加速度を求め、さらに、この運動加速度に対象物１２０の質量Ｍを乗算部３０２で乗算することにより、対象物１２０に作用する力（並進力）の要求値を出力するものとなっている。ここで、対象物動力学モデルから出力される並進力の要求値は、対象物１２０の作用する全ての並進力（対象物１２０の移動方向の並進力）の合力の要求値である。さらに、対象物動力学モデルは、目標対象物移動速度を積分器３０４で積分することにより、対象物１２０の位置の目標値である目標対象物位置を出力するものとなっている。

なお、目標対象物速度は、第１実施形態と同様、歩容生成装置１００において、移動計画における対象物１２０の位置軌道の１階微分値として算出され、あるいは、移動計画の構成要素としての移動速度がそのまま、目標対象物速度として決定される。

かかる対象物動力学モデルを用いるＳ０３の演算処理を図２２を参照して具体的に説明すると、まず、現在の移動計画に基づく対象物１２０の瞬時瞬時の（各時刻の）目標対象物速度が対象物動力学モデルに入力され、前記した如く、対象物１２０に作用する並進力の要求値と、目標対象物位置とが時系列的に算出される。

次いで、上記並進力の要求値から、歩容生成装置１００の前回の演算処理周期において前記Ｓ３５（図７参照）で求められた推定外乱力を減算部３０６で減算することにより、ロボット１から対象物１２０に作用させるべき力（並進力）の目標値が求められる。そして、この目標値の符号を反転させたものの時系列が目標対象物反力軌道として求められる。

なお、対象物動力学モデルの積分器３０４の出力の初期値は、歩容生成装置１００の前回の演算処理周期でＳ０３の処理を実行して求めた目標対象物位置の時系列のうちの、前回の演算処理周期に対応する時刻における値に設定される。また、目標対象物運動軌道のうちの目標対象物姿勢軌道は、例えば目標対象物速度の向きにほぼ一致するように決定される。

以上説明したＳ０３の処理により、現在時刻から将来の所定期間分の目標対象物運動軌道と目標対象物反力軌道とが仮決定される。このＳ０３の処理以外は、前記第１実施形態と同一である。

かかる第２実施形態においても、前記第１実施形態と同様の効果を奏することができる。

なお、以上説明した実施形態では、２足移動ロボットを例に採って説明したが、より多くの脚体を有する脚式移動ロボットについても本発明を適用できる。また、前記実施形態では、ロボット１の目標歩容を、該目標歩容に追従するように動作させつつリアルタイムで生成するものを示したが、ロボット１の動作前に、事前に目標歩容を生成しておくような場合にも本発明を適用できる。

本発明の実施形態における脚式移動ロボットのとしての２足移動ロボットの概略構成を示す図。 図１のロボットに備えた制御ユニットの構成を示すブロック図。 図２の制御ユニットの要部の機能的構成を示すブロック図。 実施形態におけるロボットと対象物との関係を示す図。 第１実施形態における歩容生成装置の演算処理を示すフローチャート。 第１実施形態における歩容生成装置の演算処理を示すフローチャート。 第１実施形態における歩容生成装置の演算処理を示すフローチャート。 図５のＳ０２で決定する移動計画の例を示す図。 図５のＳ０２の処理を示すブロック図。 図１０（ａ）は図５のＳ０７に係る仮目標ＺＭＰ軌道の例を示す図、図１０（ｂ）は図５のＳ１７で決定されるＺＭＰ修正量の例を示す図、図１０（ｃ）はＳ１７で修正された目標ＺＭＰ軌道の例を示す図。 図５のＳ１１で決定する足平軌道パラメータの例を示す図。 図１２（ａ）は今回歩容の対象物床反力モーメント軌道の例を示す図、図１２（ｂ）は図５のＳ１１で決定する定常歩容の対象物反力モーメント軌道の例を示す図。 実施形態で用いるロボット動力学モデルの例を示す図。 図１３のロボット動力学モデルを使用して上体位置を求める処理を示すブロック図。 図１５（ａ），（ｂ）は、図５のＳ２１の処理を説明するための図。 図６のＳ２３およびＳ２９の処理を説明するための図。 図６のＳ２３およびＳ２９の処理を説明するための図。 図６のＳ２３およびＳ２９の処理を説明するための図。 図７のＳ３５の処理を示すブロック図。 図２０（ａ），（ｂ）は、図５のＳ０１における移動計画の修正処理を説明するための図。 図３に示す対象物反力平衡制御装置の処理を示すブロック図。 本発明の第２実施形態におけるＳ０３（図５）の処理を示すブロック図。

符号の説明

１…脚式移動ロボット（２足移動ロボット）、２…脚体、３…上体、１００…歩容生成装置（将来対象物運動仮決定手段、判断手段）。

Claims (3)

1. 上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための目標歩容を生成する歩容生成装置において、
   前記ロボットを追従させる目標歩容を新たに生成するとき、少なくとも対象物の移動計画に基づいて、該ロボットの複数歩分の期間を含む将来の所定期間における対象物の第１仮目標運動軌道を決定する将来対象物運動仮決定手段と、
   前記対象物の移動時の該対象物とロボットとの間の位置姿勢関係に関する所定の要求が少なくともロボットの将来の２以上の所定数の歩数目において満たされ、且つ、該ロボットの脚体の運動に関する所定の制約条件が当該所定数の歩数目までの各歩数毎に満たされることを必要要件として、前記決定された第１仮目標運動軌道に従って移動する対象物に対して該必要要件を満足するロボットの脚体の運動が可能であるか否かを少なくとも該第１仮目標運動軌道と該必要要件とに基づいて判断する判断手段とを備え、
   前記判断手段の判断結果が肯定的であるときには、前記第１仮目標運動軌道に従って移動する対象物に対して前記必要要件を満足するように前記ロボットの目標歩容を生成し、前記判断手段の判断結果が否定的であるときには、前記移動計画を修正すると共に、少なくともその修正後の移動計画に基づいて前記所定期間における対象物の第２仮目標運動軌道を新たに決定し、該第２仮目標運動軌道に従って移動する対象物に対して前記必要要件を満足するように前記ロボットの目標歩容を生成するようにした歩容生成装置であって、
   前記所定の要求は、前記ロボットの遊脚側脚体の着地位置姿勢と前記対象物の位置姿勢との関係を規定する要求を含むと共に、前記所定の制約条件は、前記ロボットの遊脚側脚体の支持脚脚体に対する相対的な着地位置姿勢を規定する制約条件を含み、
   前記判断手段は、前記決定された対象物の第１仮目標運動軌道と前記所定の要求とに基づいて、前記所定数の歩数目までの各歩数目において該要求を満足するように前記ロボットの遊脚側脚体の第１仮着地予定位置姿勢を該所定数の歩数目まで決定する手段と、該所定数の歩数目までの各歩数目における該第１仮着地予定位置姿勢を、前記所定の制約条件により制限してなる第２仮着地予定位置姿勢を決定する手段と、前記所定数の歩数目における第１仮着地予定位置姿勢および第２仮着地予定位置姿勢が互いに一致もしくはほぼ一致しているか否かを判断し、一致していない場合には、該所定数の歩数目における第１仮着地予定位置姿勢と第２仮着地予定位置姿勢との差分に応じて、該所定数の歩数目よりも前の少なくとも１つの歩数目における第２仮着地予定位置姿勢を前記所定の制約条件を満たす範囲内で修正する手段とを備え、当該少なくとも１つの歩数目の修正によって、前記所定数の歩数目における第２仮着地予定位置姿勢を、前記所定の制約条件が満足される範囲内で、該所定数の歩数目における第１仮着地予定位置姿勢に一致もしくはほぼ一致する位置姿勢に修正可能であるか否かによって、前記必要要件を満足するロボットの脚体の運動が可能であるか否かを判断するものであり、
   前記判断手段の判断結果が否定的であるときに前記移動計画を修正するために、該判断手段の判断結果が否定的であるとき、前記所定の制約条件を満たす範囲内で、前記所定数の歩数目のロボットの遊脚側脚体の着地予定位置姿勢を該所定数の歩数目における前記第１仮着地予定位置姿勢に近づけるように該所定数の歩数目までのロボットの遊脚側脚体の第３仮着地予定位置姿勢を決定する手段と、その決定した第３仮着地予定位置姿勢に対して前記所定の要求を満たす前記対象物の第１予想移動経路を推定する手段と、前記第１仮着地予定位置姿勢に対して前記所定の要求を満たす前記対象物の第２予想移動経路を推定する手段と、前記第１予想移動経路で前記対象物を移動させたとした場合における第１所定期間での該対象物の移動距離と、該第１所定期間の時間と、前記第２予想移動経路で前記対象物を移動させたとした場合における第２所定期間での該対象物の移動距離と、該第２所定期間の時間とに基づいて前記移動計画における対象物の速度指令値を修正する手段とを備えたことを特徴とする脚式移動ロボットの歩容生成装置。
2. 前記判断手段の判断結果が肯定的であって、且つ、前記所定数の歩数目における第１仮着地予定位置姿勢および第２仮着地予定位置姿勢が互いに一致もしくはほぼ一致していないとき、前記所定数の歩数目よりも前の少なくとも１つの歩数目における遊脚側脚体の着地位置姿勢が前記第１仮着地予定位置姿勢と異なり、且つ、前記必要要件を満足するように該所定数の歩数目までの各歩数目における遊脚側脚体の着地予定位置姿勢を新たに決定する手段を備え、その新たに決定した前記所定数の歩数目までの遊脚側脚体の着地予定位置姿勢に応じてロボットの目標歩容を生成するようにしたことを特徴とする請求項１記載の脚式移動ロボットの歩容生成装置。
3. 前記移動計画における対象物の速度指令値を修正する手段は、前記第１予想移動経路の方向が前記第２予想移動経路の方向と一致しないとき、前記第１所定期間での対象物の移動距離の代わりに、該第１予想移動経路を第２予想移動経路の方向に投影してなる経路で対象物を移動させたとした場合における前記第１所定期間での対象物の移動距離を用いることを特徴とする請求項１又は２記載の脚式移動ロボットの歩容生成装置。

Images