JP2006175567A - 脚式移動ロボットの制御装置 - Google Patents

Abstract

【課題】対象物を移動させる作業をロボットに行わせるための歩容を、将来のロボットと対象物との間の作用力が目標値からずれても、ロボットの姿勢の安定性を確保することができるようにロボットの歩容を生成する。
【解決手段】新たな目標歩容を生成するとき、将来の所定時点でロボット・対象物間の作用力の軌道が目標軌道と異なる軌道に変化したとして、該所定時点以後の目標歩容を生成した場合に所定の動力学的制約条件を満たすことができるか否かを判断する。満たすことができない場合には、対象物の移動計画を修正した上で、ロボット・対象物間の作用力の目標軌道などを再決定し、それを用いて新たな目標歩容を生成する。
【選択図】図６

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は、解析的あるいは探索的に求めることが可能である。

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

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

補足すると、図示は省略するが、Ｓ１７の判断結果がＹＥＳになったときには、Ｓ１７で修正されたＺＭＰ軌道パラメータにより規定される今回歩容の目標ＺＭＰ軌道を用いて、前記Ｓ０９の処理と同様に、今回歩容の目標対象物反力モーメント軌道が改めて算出され、これに応じて今回歩容の対象物反力モーメント軌道パラメータが更新される。

Ｓ２３では、現在設定されている目標対象物反力軌道（Ｓ１９の判断結果がＹＥＳになった時に設定されている目標対象物反力軌道）において、次回歩容の期間内に目標対象物反力の変化点があるか否かが判断される。ここで、目標対象物反力の変化点は、本実施形態では、その変化点の時刻前後で目標対象物が比較的大きく変化するようなポイントである。そして、その変化点は、目標対象物反力の１階微分値（時間的変化率）の大きさが所定の閾値を超えるようなポイント、あるいは目標対象物反力の大きさ（絶対値）が所定の閾値を超えるようなポイントとして抽出される。

そして、Ｓ２３の判断結果がＮＯである場合には後述のＳ４９に進み、ＹＥＳである場合にはＳ２５に進む。Ｓ２５では、今回歩容およびこれに続く定常旋回歩容の現在設定されている歩容パラメータを基に、現在時刻から前記対象物反力変化時刻までのロボット１の目標歩容（所定のきざみ時間毎の目標歩容の瞬時値の時系列）が仮生成される。

より具体的には、今回歩容の歩容パラメータ（以下、単に今回歩容パラメータということがある）のうちの足平軌道パラメータを基に、現在時刻から今回歩容の終了時刻まで各足平２２の目標足平位置姿勢の瞬時値の時系列を算出し、さらにこれに続いて、定常旋回歩容の歩容パラメータ（以下、単に定常歩容パラメータということがある）のうちの第１旋回歩容（今回歩容の次の第１旋回歩容）に係る足平軌道パラメータを基に、今回歩容の終了時刻から対象物反力変化時刻までの各足平２２の目標足平位置姿勢の瞬時値を時系列的に算出する。この目標足平位置姿勢の算出処理は、前記Ｓ１３の処理の中でZMPfeet[i]を算出する処理に関して説明した場合と同様に、本出願人が特許第３２３３４５０号にて提案した有限時間整定フィルタを用いて各足平２２毎に算出される。

さらに、今回歩容パラメータのＺＭＰ軌道パラメータおよび対象物反力モーメント軌道パラメータ（Ｓ１７の修正後の軌道パラメータ）を基に、それぞれ今回歩容の終了時刻までの目標ＺＭＰの瞬時値と標対象物反力モーメントの瞬時値とを時系列的に算出し、さらにこれに続いて、定常歩容パラメータのうちの第１旋回歩容（今回歩容の次の第１旋回歩容）に係るＺＭＰ軌道パラメータおよび対象物反力モーメント軌道パラメータを基に、それぞれ今回歩容の終了時刻から対象物反力変化時刻までの目標ＺＭＰの瞬時値と目標対象物反力モーメントの瞬時値とが時系列的に算出される。

そして、上記の如く、目標足平位置姿勢、目標ＺＭＰ、および目標対象物反力モーメントの瞬時値を算出しながら、これらの瞬時値を用いて、前記図１５のブロック図で示した動力学的演算によって、対象物反力変化時刻までの目標上体位置姿勢の瞬時値が目標ＺＭＰを満足するように算出される。なお、本実施形態では、目標上体姿勢は、例えば定常的な鉛直姿勢とされる。

このようにＳ２５では、現在時刻から対象物反力変化時刻までの目標歩容のうちの目標足平位置姿勢、目標ＺＭＰ、および目標対象物反力モーメントの瞬時値が時系列的に算出される。補足すると、Ｓ２５では目標手先位置姿勢の瞬時値の時系列は算出する必要はない。これはロボット１の腕体５の運動はＺＭＰへの影響が小さいからである。

以降、対象物反力変化時刻を含む歩容（次回歩容）を仮想今回歩容、その次の歩容を仮想次回歩容、さらにその次の歩容を仮想次次回歩容という。仮想今回歩容は、対象物反力変化時刻を現在時刻とみなしたときの今回歩容を意味する。

次いでＳ２７に進んで、対象物反力変化時刻以後の仮想的な目標対象物反力軌道である仮想目標対象物反力軌道を決定する。この仮想目標対象物反力軌道は、前記したＳ０９〜Ｓ２１のループ処理で最終的に決定された目標対象物反力軌道のうちの対象物反力変化時刻以後の軌道とは異なる仮想的な軌道であり、本実施形態では、対象物反力変化時刻で目標対象物反力が変化せず、該対象物反力変化時刻以後、その直前の時刻（例えば対象物反力変化時刻よりも所定のきざみ時間前の時刻）の値に継続的に維持される軌道に決定される。つまり、仮想目標対象物反力軌道は、対象物反力変化時刻での対象物１２０の実際の位置が目標対象物運動軌道における位置からずれているなどの原因によって、実際の対象物反力が対象物反力変化時刻にて目標対象物反力通りに変化しないような状況を想定して設定される対象物反力軌道である。例えば図１７（ａ）の実線のグラフで示す如く、対象物１２０の移動計画（現在の移動計画）による対象物１２０の移動速度（対象物速度）が、次回歩容内の時刻t1までは０で、該時刻t1から時刻t2まで増加した後、時刻t2から一定速度に維持されるようになっているとする。この場合、目標対象物反力軌道は、概略的には、例えば図１７（ｂ）の実線のグラフで示すように、時刻t1から時刻t2の間で目標対象物反力がある値に増加するような軌道に決定されることとなる。そして、このとき、仮想目標対象物反力軌道は、図１７（ｂ）に二点鎖線のグラフで示す如く、対象物反力変化時刻である時刻t1から対象物反力が増加することなく、対象物反力が時刻t1以前と同じ値に継続的に維持されるような軌道に決定される。

なお、このように決定される仮想目標対象物反力軌道は、本発明における第２ロボット・対象物間目標作用力軌道に相当するものである。従って、Ｓ２７の処理は、本発明における第２作用力軌道設定手段としての機能を持つ。

次いで、Ｓ２９に進んで、対象物反力変化時刻以後の遊脚足平２２の着地予定位置姿勢および着地予定時刻が仮決定される。ここで仮決定される遊脚足平２２の着地予定位置姿勢および着地予定時刻は、少なくとも仮想今回歩容と仮想次回歩容とにおける遊脚足平２２の着地予定位置姿勢および着地予定時刻が含まれる。この場合、例えば、仮想今回歩容の遊脚足平２２の着地予定位置姿勢および時刻は、今回歩容に続く前記定常旋回歩容（Ｓ０９〜Ｓ２１のループ処理で最終的に歩容パラメータが決定された定常旋回歩容）の第１番目の第１旋回歩容の遊脚足平２２の着地予定位置姿勢および時刻（これは本実施形態ではＳ０５で決定した次回歩容の遊脚足平２２の着地予定位置姿勢および時刻である）と同一に仮決定され、仮想次回歩容の遊脚足平２２の着地予定位置姿勢および時刻は、今回歩容に続く定常旋回歩容（Ｓ０９〜Ｓ２１のループ処理で最終的に歩容パラメータが決定された定常旋回歩容）の第１番目の第２旋回歩容の遊脚足平２２の着地予定位置姿勢および時刻と同一に仮決定される。なお、仮想次回歩容の遊脚足平２２の着地予定位置姿勢および時刻は、前記Ｓ０５で決定した次次回歩容の遊脚足平２２の着地予定位置姿勢および時刻と同じに決定してもよく、あるいは、次次回歩容までの目標対象物運動軌道を基に決定してもよい。

次いで、Ｓ３１に進んで、対象物反力変化時刻以後の仮想今回歩容の目標ＺＭＰ軌道を規定するＺＭＰ軌道パラメータを仮決定する。このＳ３１の処理では、前記Ｓ２５で仮生成した仮想今回歩容の支持脚足平２２の目標位置姿勢と、仮想今回歩容の遊脚足平２２の着地予定位置姿勢および時刻とを基に、仮想今回歩容の支持脚足平２２の接地面（両脚支持期では支持多角形）の概ね中央付近に目標ＺＭＰが位置するように対象物反力変化時刻以後の仮想今回歩容のＺＭＰ軌道パラメータが仮決定される。なお、この場合、仮想今回歩容の遊脚足平２２の着地予定位置姿勢および時刻としては、Ｓ２９の後に最初にＳ３１の処理を実行するときには、Ｓ２９で仮決定されたものが用いられ、後述のＳ４７の後にＳ３１の処理を実行するときには、Ｓ４７で後述する如く修正されたものが用いられる。

次いで、Ｓ３３に進んで、対象物反力変化時刻以後の対象物反力が前記Ｓ２７で決定された仮想目標対象物反力軌道になったと仮定して、該対象物反力変化時刻以後の目標ＺＭＰ（Ｓ３１で仮決定したＺＭＰ軌道パラメータにより規定される目標ＺＭＰ）まわりの対象物反力モーメント軌道（仮想今回歩容の終了時刻までの軌道）を算出する。この算出処理は、対象物反力変化時刻以後の目標対象物運動軌道（前記Ｓ０３で仮決定された軌道のうちの対象物反力変化時刻以後の軌道）と前記仮想目標対象物反力軌道とを基に、前記Ｓ０９の処理と同様に行なわれる。

次いで、Ｓ３５に進んで、前記仮想今回歩容に続く周期的な歩容としての仮想定常旋回歩容の歩容パラメータ（仮想定常歩容パラメータ）が決定される。すなわち、仮想定常旋回歩容の足平軌道パラメータ、ＺＭＰ軌道パラメータ、対象物反力モーメント軌道パラメータが決定される。

この場合、仮想定常歩容パラメータの足平軌道パラメータ（仮想定常歩容における第１旋回歩容および第２旋回歩容の初期および終端のそれぞれにおける支持脚足平２２および遊脚足平２２のそれぞれの位置姿勢など）は、前記Ｓ１１の処理と同様に、仮想今回歩容、仮想定常旋回歩容の第１旋回歩容、第２旋回歩容の順に足平位置姿勢軌道がつながり、且つ、仮想定常旋回歩容の足平位置姿勢軌道が周期性の条件を満たすように決定される。なお、この場合、Ｓ２９の後にＳ３５の処理を最初に実行するときには、仮想今回歩容および仮想次回歩容の遊脚足平２２の着地予定位置および着地予定時刻としては、前記Ｓ２９で仮決定したものが用いられ、後述のＳ４７の後にＳ３５の処理を実行するときには、Ｓ４７で後述するように修正されたものが用いられる。また、仮想定常歩容パラメータのＺＭＰ軌道パラメータは、Ｓ０７の処理と同様に、仮想定常歩容パラメータの足平軌道パラメータを基に、該足平軌道パラメータにより定まる、仮想定常歩容の支持脚足平２２の接地面（両脚支持期では支持多角形）の中央付近に目標ＺＭＰが位置するように決定される。また、仮想定常歩容パラメータの対象物反力モーメント軌道パラメータは、仮想次回歩容の目標対象物運動軌道および仮想目標対象物反力軌道と、仮想次回歩容（仮想定常歩容の第１番目の第１旋回歩容）のＺＭＰ軌道パラメータにより規定されるＺＭＰ軌道とを基に前記Ｓ０９と同様に求められる仮想次回歩容の対象物反力モーメント軌道と、仮想次次回歩容の目標対象物運動軌道および仮想目標対象物反力軌道と、仮想次次回歩容（仮想定常歩容の第１番目の第２旋回歩容）のＺＭＰ軌道パラメータにより規定されるＺＭＰ軌道とを基に前記Ｓ０９と同様に求められる仮想次次回歩容の対象物反力モーメント軌道とから、前記Ｓ１１の処理と同様に決定される。補足すると、仮想次回歩容および仮想次次回歩容の仮想目標対象物反力軌道は、仮想今回歩容の対象物反力変化時刻以後の一定の軌道である。

次いで、Ｓ３７に進み、仮想定常旋回歩容の初期発散成分が決定される。すなわち、仮想定常旋回歩容の第１旋回歩容の初期状態と第２旋回歩容の終端状態とが一致するように仮想定常旋回歩容の初期状態が決定される。この処理は、前記Ｓ１３の処理と同様に行なわれる。

次いで、Ｓ３９に進み、仮想今回歩容の歩容パラメータ（足平軌道パラメータ、ＺＭＰ軌道パラメータ、対象物反力モーメント軌道パラメータ）が仮決定される。この処理では、仮想今回歩容の足平軌道パラメータが前記Ｓ１５の処理と同様に決定される。また、仮想今回歩容のＺＭＰ軌道パラメータは、前記Ｓ３１で仮決定したものと同一に決定される。なお、ここで、決定される仮想今回歩容のＺＭＰ軌道パラメータは仮値である。また、仮想今回歩容の対象物反力モーメント軌道パラメータは、それにより規定される仮想今回歩容の目標対象物反力モーメント軌道が、対象物反力変化時刻までは、Ｓ２５で算出した目標対象物反力モーメント軌道に一致し、且つ、対象物反力変化時刻から仮想今回歩容の終了時刻まで、Ｓ３３で決定した対象物反力モーメント軌道に一致するように決定される。

補足すると、Ｓ３９で仮決定される足平軌道パラメータは、本発明における第２脚体運動パラメータに相当する。

次いでＳ４１に進んで、仮想定常旋回歩容に仮想今回歩容が収束するように（仮想定常旋回歩容の初期発散成分に仮想今回歩容の終端発散成分がほぼ一致するように）、Ｓ３９で仮決定した仮想今回歩容のＺＭＰ軌道パラメータを修正する。この処理は、前記Ｓ１７の処理と同様に行なわれる。

次いで、Ｓ４３に進んで、Ｓ４１で修正したＺＭＰ軌道パラメータにより規定される仮想今回歩容の目標ＺＭＰが適正であるか否かが判断される。この判断は、前記Ｓ１９の場合と同様に行なわれる。つまり、仮想今回歩容の目標ＺＭＰが、仮想今回歩容の支持脚足平２２の接地面内（両脚支持期では支持多角形内）で、その境界に近づきすぎない位置に存在すべきであるという動力学的な制約条件（以下、ＺＭＰ制約条件という）を満たすか否かがＳ４３で判断される。

そして、Ｓ４３の判断結果がＹＥＳである場合には後述のＳ４９に進み、ＮＯである場合にはＳ４５に進む。Ｓ４５では、仮想今回歩容の目標ＺＭＰの適正な修正が可能であるか否かを判断する。より詳しくは、後述のＳ４７の処理で仮想今回歩容における遊脚足平２２の着地予定位置姿勢および着地予定時刻を修正して、Ｓ３１からの処理を再び実行したときに、Ｓ４３の判断結果をＹＥＳとすることができるか否か（仮想今回歩容の目標ＺＭＰが、前記ＺＭＰ制約条件を満たし得るか否か）を判断する。この場合、例えば、仮想今回歩容の目標ＺＭＰが、仮想今回歩容の支持脚足平２２の接地面から逸脱し、且つ、その逸脱量（接地面の境界からの距離など）が所定量以上大きいときには、Ｓ４５の判断結果をＮＯとする。あるいは、後述のＳ４７の処理で、仮想今回歩容における遊脚足平２２の着地予定位置姿勢および着地予定時刻を修正した回数が所定数を超えた場合に、Ｓ４５の判断結果をＮＯとする。

補足すると、Ｓ４５の判断処理は、本発明における判断手段の処理に相当する。

Ｓ４５の判断結果がＹＥＳである場合には、Ｓ４７に進んで、対象物反力変化時刻以後の遊脚足平２２（仮想今回歩容と仮想次回歩容における遊脚足平２２）の着地予定位置及び時刻を修正する。この場合、Ｓ４１で修正する仮想今回歩容のＺＭＰ軌道パラメータにより規定される目標ＺＭＰが仮想今回歩容の支持脚足平２２の接地面内により近づくように仮想今回歩容の遊脚足平２２の着地予定位置及び時刻を修正する。なお、Ｓ４７での修正では、仮想今回歩容と仮想次回歩容の遊脚足平２２の着地予定時刻は修正せずに、着地予定位置のみを修正するようにしてもよい。また、仮想今回歩容の遊脚足平２２の着地予定位置を併せて修正するようにしてもよい。

Ｓ４７の修正後には、Ｓ３１からの処理が再び実行される。そして、Ｓ４３の判断結果がＹＥＳになれば、後述のＳ４９に進む。

一方、Ｓ４５の判断結果がＮＯである場合、すなわち、ＺＭＰ制約条件を満たすような仮想今回歩容の目標ＺＭＰの修正を行なうことができない場合には、仮に、実際の対象物反力が前記Ｓ０１〜Ｓ２１の処理で最終的に決定された目標対象物反力軌道に則したものとならずに、前記対象物反力変化時刻で、該目標対象物反力軌道どおりに変化しないような状況が発生すると、該対象物反力変化時刻以後のロボット１の姿勢（全体的な姿勢）を安定に保つことが困難となる。

そこで、このような場合には、前記Ｓ０１に戻って、対象物１２０の移動計画を改めて決定しなおす（移動計画を修正する）。この場合に再決定する対象物１２０の移動計画は、実際の対象物反力が目標対象物反力軌道どおりには変化しない状況が発生しても、ロボット１の姿勢を安定に保つことが可能となるように決定される。この場合、目標対象物反力の急激な変動を少なくするように移動計画が再決定される。

以下に、前記Ｓ４７で仮想今回歩容の遊脚足平２２の着地予定位置を修正する場合の具体例と、Ｓ０１で移動計画を決定しなおす場合の具体例とを図１７と図１８とを参照して説明する。図１８には、図１７（ａ）の実線のグラフで表した移動計画（現在の移動計画）による目標対象物位置軌道（Ｘ軸方向の位置軌道）を実線のグラフで示している。また、今回歩容、次回歩容（仮想今回歩容）および次次回歩容のそれぞれの終了時刻における支持脚足平２２および遊脚足平２２をそれぞれの歩容の終了時刻に対応する箇所で図１８中に図示している。この場合、支持脚足平２２、遊脚足平２２はそれぞれ白抜きの長方形、斜線付きの長方形で表している。参照符号２２Ｌ１，２２Ｒ２，２２Ｌ３を付した支持脚足平は、それぞれ今回歩容、次回歩容、次次回歩容の終了時刻における支持脚足平であり、参照符号２２Ｒ１，２２Ｌ２，２２Ｒ３を付した遊脚足平は、それぞれ今回歩容、次回歩容、次次回歩容の終了時刻における遊脚足平である。これらの各歩容に係る遊脚足平の位置は、図１８中の実線のグラフの目標対象物位置軌道に応じて決定される位置の例である。より詳しくは、今回歩容に係る遊脚足平２２Ｒ１の位置は、Ｓ０５で決定した今回歩容の遊脚足平２２の着地予定位置であり、次回歩容に係る遊脚足平２２Ｌ２の位置は、Ｓ２９で決定した仮想今回歩容の遊脚足平２２の着地予定位置、次次回歩容に係る遊脚足平２２Ｒ３の位置は、Ｓ２９で決定した仮想次回歩容の遊脚足平２２の着地予定位置である。また、今回歩容に係る支持脚足平２２Ｌ１内の黒点は、Ｓ０１〜Ｓ２１の処理で最終的に決定された今回歩容のＺＭＰ軌道パラメータにより規定される、今回歩容の終了時刻における目標ＺＭＰの位置を例示し、次回歩容に係る支持脚足平２２Ｒ２内の黒点は、Ｓ３１で最初に仮決定されたＺＭＰ軌道パラメータにより規定される、仮想今回歩容の終了時刻における目標ＺＭＰの位置を例示し、次次回歩容に係る支持脚足平２２Ｌ３内の黒点は、Ｓ３５で最初に決定された仮想定常旋回歩容のＺＭＰ軌道パラメータにより規定される、仮想次次回歩容（仮想定常旋回歩容の第１番目の第１旋回歩容）の終了時刻における目標ＺＭＰの位置を例示している。

前記Ｓ４１の処理で仮想今回歩容のＺＭＰ軌道パラメータを修正したとき、その修正後のＺＭＰ軌道パラメータにより規定される目標ＺＭＰ（仮想今回歩容の終了時刻における目標ＺＭＰ）は、仮想今回歩容の支持脚足平２２Ｒ２の接地面から逸脱し、図１８中の○で示す点になることがある。これは、仮想今回歩容の対象物反力変化時刻までは、対象物反力が目標対象物反力どおりに変化するとして、仮想今回歩容の目標歩容がＳ２５で仮生成される一方、仮想定常旋回歩容の歩容パラメータのうちの対象物反力モーメント軌道は、対象物反力が対象物反力変化時刻で変化せずに、仮想目標対象物反力軌道に従うものとなると仮定して決定されるからである。すなわち、仮想今回歩容の対象物反力変化時刻までは、対象物反力変化時刻で目標対象物反力が急増することが予定されているので、ロボット１の上体３が前方に加速するようにＳ２５で目標歩容（特に目標上体位置軌道）が生成される。しかるに、対象物反力変化時刻以後は、仮想目標対象物反力軌道に従って、対象物反力が対象物反力変化時刻の直前と同じに継続的に維持されることが予定されているので、仮想今回歩容を仮想定常旋回歩容に収束させるためには、ロボット１の上体３を減速する必要がある。その結果、Ｓ４１の処理で仮想今回歩容のＺＭＰ軌道パラメータを修正したとき、その修正後のＺＭＰ軌道パラメータにより規定される目標ＺＭＰ（仮想今回歩容の終了時刻における目標ＺＭＰ）は、仮想今回歩容の支持脚足平２２Ｒ２の接地面からロボット１の前方側（Ｘ軸の正方向）に逸脱することがある。

このような場合、前記Ｓ４７の処理では、仮想今回歩容の遊脚足平２２の着地予定位置が、図１８の参照符号２２Ｌ２で示す位置から、例えば同図中に参照符号２２Ｌ２’を付した破線の長方形で示すように、ロボット１の前方側に修正される。このようにすることで、再びＳ３１〜Ｓ４１の処理を実行して、仮想今回歩容のＺＭＰ軌道パラメータを修正したとき、その修正後のＺＭＰ軌道パラメータにより規定される目標ＺＭＰが前記ＺＭＰ制約条件を満たすことが可能となって、Ｓ４３の判断結果がＹＥＳとなる場合がある。

つまり、この場合には、実際の対象物反力が将来の対象物反力変化時刻で仮に目標対象物反力どおりに変化しなかったとしても、その対象物反力変化時刻を含む歩容での遊脚足平２２の着地予定位置を修正することで、ロボット１の姿勢の安定性を確保できることとなる。

一方、前記Ｓ４１の処理で仮想今回歩容のＺＭＰ軌道パラメータを修正したとき、その修正後のＺＭＰ軌道パラメータにより規定される目標ＺＭＰ（仮想今回歩容の終了時刻における目標ＺＭＰ）が、図１８中の△で示す点の如く、仮想今回歩容の支持脚足平２２Ｒ２の接地面から大きく逸脱する場合もある。そして、このような場合には、仮想今回歩容、あるいは仮想今回歩容と仮想次回歩容の遊脚足平２２の着地予定位置を遊脚足平２２の機構的な可動範囲内でどのように修正しても、目標ＺＭＰがＺＭＰ制約条件を満たすようにＺＭＰ軌道パラメータを決定できない場合があり、このときＳ４５の判断結果がＮＯになる。

このような場合は、元々の対象物１２０の移動計画がロボット１の安定性を継続的に確保する上で不適切であるので、Ｓ０１において、対象物１２０の移動計画を再決定（修正する）。この場合、図１８にて説明した上記の例では、図１７（ａ）の破線のグラフで示すように、移動計画を修正する。すなわち、修正後の移動計画による対象物１２０の移動速度が図１７（ａ）に実線のグラフで示した元の移動計画よりも時刻t1（対象物反力変化時刻）から緩やかに増加し、さらに時刻t2以後に、元に移動計画よりも小さい速度で一定に維持されるように移動計画を決定しなおす。

このようにしたとき、この修正後の移動計画に対応すつ対象物反力は、図１７（ｂ）に破線のグラフで示す如く、時刻t1から時刻t2の期間で元の移動計画に対応する対象物反力よりも小さい反力に抑えられる。従って、対象物反力変化時刻t1における目標対象物反力の急激な変化が緩和される。その結果、再度、Ｓ４３までの処理を実行したときに、Ｓ４３の判断結果をＹＥＳにすることが可能となる。

つまり、前述したＳ０１〜Ｓ４７の処理によって、実際の対象物反力が目標対象物反力どおりに変化しない場合でも、ロボット１の安定性を確保できるように、今回歩容パラメータを決定できることとなる。

前記Ｓ２３の判断結果がＮＯである場合、あるいは、Ｓ４３の判断結果がＹＥＳである場合には、図７のＳ４９に進む。

このＳ４９では、現在の今回歩容パラメータと定常歩容パラメータとを基に、現在時刻から定常歩容の第２旋回歩容終端までの（今回歩容の１歩を含めて３歩目までの）目標上体位置姿勢軌道を算出すると共に、目標対象物運動軌道を基に目標手先位置姿勢軌道を算出する。つまり、Ｓ４９では、今回歩容を含めて３歩の期間における目標歩容が仮生成される。

この場合、今回歩容、定常旋回歩容の第１旋回歩容、第２旋回歩容の順に、各歩容の歩容パラメータを基に、前記図１５のブロック図を参照して説明した如く、目標上体位置姿勢軌道を算出する。より具体的には、各歩容の足平軌道パラメータを基に、目標両足平位置姿勢軌道が算出されると共に、ＺＭＰ軌道パラメータおよび対象物反力モーメント軌道パラメータを基に、それぞれ目標ＺＭＰ軌道、対象物反力モーメント軌道が算出される。なお、目標両足平位置姿勢軌道の算出処理は、前記Ｓ１３の処理の中で、ZMPfeet[i]を算出する処理に関して説明した場合と同様に、本出願人が特許第３２３３４５０号にて提案した有限時間整定フィルタを用いて各足平２２毎に算出される。そして、これらの目標両足平位置姿勢軌道、目標ＺＭＰ軌道、および対象物反力モーメント軌道を図１５のブロック図の演算処理に用いることで、目標上体位置姿勢が決定される。なお、本実施形態では、目標上体姿勢は定常的に鉛直姿勢とされる。このように、目標上体位置姿勢を算出することで、ロボット単純化モデル上で、ロボット１の運動が発生する慣性力と、ロボット１に作用する目標対象物反力と重力との合力が目標ＺＭＰまわりの発生するモーメントの水平成分が０になるという動力学的平衡条件を満たすようにロボット１の目標運動のうちの目標上体位置姿勢軌道が算出される。

さらに、目標手先位置姿勢軌道は、今回歩容、定常旋回歩容の第１旋回歩容、第２旋回歩容の順に、Ｓ０９で求めた場合と同様に、目標対象物運動軌道を基に算出する。なお、定常旋回歩容の第１旋回歩容、第２旋回歩容に対応する目標対象物運動軌道は、Ｓ０３で仮決定された目標対象物運動軌道のうちの、次回歩容、次次回歩容に対応する目標対象物運動軌道である。

次いで、Ｓ５１に進んで、ロボット１と対象物１２０との幾何学的制約条件をチェックする。ここで、この幾何学的制約条件は、対象物１２０とロボット１との干渉（上体３が対象物１２０に当たるなど）の有無、並びに、各腕体３の関節の回転角が機構的な制限内に収まっているかなどの条件である。目標上体位置姿勢と目標対象物運動軌道とからロボット１の上体３と対象物１２０との干渉の有無を判断できる。また、目標上体位置姿勢と目標手先位置姿勢とから、各腕体３の関節の回転角を決定できるので、その回転角が機構的な制限内に収まっているか否かが判る。

Ｓ５１の判断処理は、現在時刻から第２旋回歩容の終端まで、所定の刻み時間間隔の目標上体姿勢等の瞬時値に対して逐次行なわれる。すなわち、各瞬時値に対して、Ｓ５１の判断結果がＯＫである場合には、Ｓ５３において、現在時刻から第２旋回歩容の終端までの全時刻分のチェックが完了したか否かを判断する。そして、このＳ５３の判断結果がＮＯである場合には、Ｓ５１の判断処理を繰り返す。

Ｓ５１の判断結果がＮＧである場合には、Ｓ５５に進んで、ＮＧとなった該当時刻における幾何学的制約条件を満たすように着地予定位置姿勢と目標対象物運動軌道とのうちの少なくともいずれか一方を修正する。その修正例を図１９を参照して説明する。

同図を参照して、現在時刻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から式１９によって、上体位置摂動量が求められる。なお、本実施形態では、修正目標上体位置姿勢のうちの修正目標上体姿勢は、目標上体姿勢に一致させるものとし、上体姿勢の摂動量は０とする。

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

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

以上説明した第１実施形態の作動、特に歩容生成装置１００の処理によって、対象物反力を考慮しつつ、今回歩容が定常歩容に収束するように今回歩容が生成される。このため、ロボット１の継続的な安定性を確保しながら、ロボット１により対象物１２０を押すなどの作業を円滑に行なうことができる。

特に、Ｓ２３〜Ｓ２７の処理では、次回歩容の期間内に目標対象物反力の変化点がある場合に、対象物反力変化時刻で対象物反力が目標対象物反力軌道どおりには変化しない場合を想定し、そのような場合でもＺＭＰ制約条件を満たしつつ、定常旋回歩容に収束するような次回歩容（仮想今回歩容）を生成可能であるか否かが確認される。そして、生成可能でないときには、移動計画を修正した上で、今回歩容パラメータを改めて決定し、今回歩容パラメータを基に算出される目標歩容の瞬時値に応じてロボット１の動作制御を行なう。このため、対象物反力が目標対象物反力軌道どおりに変化しない場合であっても、ロボット１の継続的な安定性を確保できるように目標歩容を生成することができる。

次に本発明の第２実施形態を図２３〜図２５を参照して説明する。なお、この第２実施形態の説明では、第１実施形態と同一構成部分あるいは同一機能部分については第１実施形態と同じ参照符号を用い、説明を省略する。

図２３は第２実施形態での歩容生成装置１００の処理のうち、第１実施形態に係る図５の処理に対応する部分の処理を示すフローチャートである。同図２３に示すように、第２実施形態では、Ｓ１９の判断結果がＮＯである場合の処理のみが第１実施形態と相違している。

すなわち、第１実施形態では、Ｓ１９の判断結果がＮＯであるときに、目標対象物反力軌道を修正するようにしたが、第２実施形態では、これに代えて、Ｓ２１’において、遊脚足平２２の着地予定位置姿勢または着地予定時刻を修正する。これは今回歩容パラメータのうちの足平軌道パラメータを修正することを意味する。この場合、着地予定位置姿勢または着地予定時刻は、その修正後にＳ０７〜Ｓ１７の処理を再度実行したときに、ＺＭＰ修正量ができるだけ小さくなるように（少なくとも着地予定位置姿勢または着地予定時刻の修正前に求めたＺＭＰ修正量よりも小さくなるように）修正される。そして、その修正後に、Ｓ０７からの処理を再度実行する。これ以外は、第１実施形態と同一である。

Ｓ２１’での具体的な修正例を以下に説明する。例えば今現在、今回歩容の遊脚足平２２の着地予定位置姿勢と次回歩容の遊脚足平２２の着地予定位置姿勢が図２２の実線で示す如く決定されているとする。なお、図示の例では、ロボット１をほぼ一定の歩幅で、今回歩容支持脚座標系のＸ軸方向に直進歩行させるものとなっている。

そして、今回歩容まではロボット１が対象物１２０に近づき、次回歩容の途中から、ロボット１が対象物１２０をＸ軸の正方向に押す作業を開始するものとする。この場合、図２５（ａ）に示す如く、目標対象物反力軌道は、例えば次回歩容の途中から立ち上がるようなステップ状の軌道となる。

このとき、図２４の実線で示すような着地予定位置姿勢を維持したまま、Ｓ１７までの処理を実行すると、Ｓ１７の処理で決定されるＺＭＰ修正量（Ｘ軸方向成分）は、例えば図２５（ｂ）に実線で示す如く、比較的大きなものとなって、Ｓ１９の判断結果がＮＯとなる場合がある。

この場合に、Ｓ２１’の処理では、例えば次回歩容の遊脚足平２２の着地予定位置姿勢を図２４の破線で示す如く、今回歩容の支持脚足平２２ＬにＸ軸方向で近づけるように修正する。すなわち、次回歩容の歩幅をより小さくするように遊脚足平２２Ｌの着地予定位置姿勢を修正する。このように次回歩容の着地予定位置姿勢を修正した後に、Ｓ０５からの処理を再び実行すると、Ｓ１７で決定されるＺＭＰ修正量が、図２５（ｂ）に破線で示す如く小さくなる。その結果、Ｓ１９の判断結果がＹＥＳになる。補足すると、次回歩容の遊脚足平２２Ｌの着地予定位置姿勢を図２４の如く修正したとき、前記した定常歩容の足平軌道パラメータの決定手法によって、定常歩容の第２旋回歩容の終端遊脚足平位置も、第２旋回歩容の初期遊脚足平位置（今回歩容の遊脚足平２２Ｒの着地予定位置）に近づくこととなる。

なお、上記の例では、次回歩容の遊脚足平２２Ｌの着地予定位置姿勢を修正するようにしたが、今回歩容の遊脚足平２２Ｒの着地予定位置姿勢を修正する余裕がある場合には、それを修正するようにしてもよい。あるいは、今回歩容および次回歩容の両者の着地予定位置姿勢を修正するようにしてもよい。

また、上記の例では、着地予定位置姿勢を修正するようにしたが、今回歩容と次回歩容とのうちの少なくともいずれか一方の遊脚足平２２の着地予定時刻を修正するようにしてもよい。図２５に示した状況では、例えば次回歩容の着地予定時刻を遅らせるようにすればよい。

以上が本発明の第２実施形態である。かかる第２実施形態においても、第１実施形態と同様の作用効果を奏することができる。なお、第５実施形態では、Ｓ１９の判断結果がＮＯである場合に遊脚足平２２の着地予定位置姿勢または着地予定時刻を修正するようにしたが、それと併せて、第１実施形態の如く、目標対象物反力軌道を修正するようにしてもよい。

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

符号の説明

１…脚式移動ロボット（２足移動ロボット）、２…脚体、３…上体、１００…歩容生成装置（第１作用力軌道決定手段、第１脚体運動パラメータ決定手段、第２作用力軌道設定手段、判断手段）。

Claims (3)

1. 上体から延設された複数の脚体を備える脚式移動ロボットに対象物を移動させる作業を行わせるための該ロボットの目標歩容を生成する装置において、
   新たな目標歩容を生成するとき、少なくとも対象物の移動計画に基づいて、該新たな目標歩容におけるロボットと対象物との間の作用力の目標値の軌道である第１ロボット・対象物間目標作用力軌道を仮決定する第１作用力軌道決定手段と、
   少なくとも前記対象物の移動計画に基づいて、前記新たな目標歩容におけるロボットの脚体の運動を規定する第１脚体運動パラメータを仮決定する第１脚体運動パラメータ決定手段と、
   前記仮決定された第１ロボット・対象物間目標作用力軌道に対し、実際のロボットと対象物との間の作用力の軌道が将来の所定時点以後に該第１ロボット・対象物間目標作用力軌道と異なる軌道に変化したと仮定して、該所定時点以後のロボットと対象物との間の作用力の目標値の軌道である第２ロボット・対象物間目標作用力軌道を設定する第２作用力軌道設定手段と、
   前記所定時点までのロボットの目標歩容を前記第１脚体運動パラメータと前記第１ロボット・対象物間目標作用力軌道とを用いて生成し、さらに該所定時点までの目標歩容に続く該所定時点以後のロボットの目標歩容を、該所定時点以後のロボットの脚体の運動を規定する脚体運動パラメータである第２脚体運動パラメータを可変的に設定可能な変数として、該第２脚体運動パラメータと前記第２ロボット・対象物間作用力とを用いて生成した場合に、該所定時点以後のロボットの目標歩容が所定の動力学的制約条件を満たし得る第２脚体運動パラメータを設定可能であるか否かを判断する判断手段とを備え、
   該判断手段によって、前記所定時点以後のロボットの目標歩容が前記所定の動力学的制約条件を満たし得る前記第２脚体運動パラメータを設定可能であると判断されたときには、前記第１脚体運動パラメータと第１ロボット・対象物間目標作用力軌道とを用いて前記新たな目標歩容を生成し、前記所定時点以後のロボットの目標歩容が前記所定の動力学的制約条件を満たし得る前記第２脚体運動パラメータを設定可能でないと判断されたときには、前記対象物の移動計画を修正すると共に、その修正した移動計画を少なくとも用いて前記第１脚体運動パラメータと第１ロボット・対象物間目標作用力軌道とを再決定し、その再決定した第１脚体運動パラメータと第１ロボット・対象物間目標作用力軌道とを用いて前記新たな目標歩容を生成するようにしたことを特徴とする脚式移動ロボットの歩容生成装置。
2. 前記所定時点は、前記第１作用力軌道決定手段が仮決定した第１ロボット・対象物間目標作用力軌道の作用力の大きさ、または該作用力の変化速度の大きさが所定の閾値を超える時点であり、前記第２ロボット・対象物間目標作用力軌道は、前記所定時点以後のロボットと対象物との間の作用力の大きさ、または該作用力の変化速度の大きさが前記所定の閾値よりも小さい値となる軌道であることを特徴とする請求項１記載の脚式移動ロボットの歩容生成装置。
3. 前記所定の動力学的制約条件は、少なくともロボットの目標歩容の運動によって発生する慣性力と該ロボットに作用する重力と該ロボットに対象物から作用する作用力との合力によって定まるＺＭＰが所定の許容範囲内に存在するという条件を含むことを特徴とする請求項１または２記載の脚式移動ロボットの歩容生成装置。

Images