(57)【要約】脚式移動ロボット、特に２足歩行ロボットにおいて、上体と脚部の運動と、床反力との関係を表す動力学モデルを備え、脚部の軌道などを決定するパラメータを含む今回歩容のパラメータを仮決定し、今回歩容に続く周期的歩容のパラメータを想定し、動力学モデルと今回歩容のパラメータなどに基づいて決定される前記上体の軌道が、周期的歩容のパラメータから決定される上体の軌道に収束するように今回歩容のパラメータを修正し、修正したパラメータに基づいて今回歩容の瞬時値を決定する。これにより、床反力を含む歩容を動力学的平衡条件を満足しながら、自在かつリアルタイムに生成して任意の歩幅、旋回角、歩行周期などの歩容を生成することができると共に、生成された歩容同士の境界においてロボットの各部位の変位および速度が連続で、かつ安定余裕の高い歩容を生成することができる。また、ロボットの将来の挙動を予測して姿勢の発散などの不都合が生じないように歩容を生成することができる。
2002040224.tif 000002
【特許請求の範囲】
【請求項１】
少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、
ａ．少なくとも前記上体と脚部の運動と、床反力との関係を表す動力学モデル、
ｂ．少なくとも要求に応じ、前記脚部の軌道と前記床反力の軌道を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段、
ｃ．少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容の前記パラメータを想定する周期的歩容パラメータ想定手段、
ｄ．前記動力学モデルおよび前記今回歩容のパラメータとそれに続く前記周期的歩容のパラメータに基づいて決定される前記上体の軌道が、前記周期的歩容のパラメータから決定される前記周期的歩容の前記上体の軌道に実質的に収束あるいは一致するように、少なくとも前記仮決定された今回歩容のパラメータを修正する今回歩容パラメータ修正手段、
および
ｅ．少なくとも前記修正された前記今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段、
を備えたことを特徴とする脚式移動ロボットの歩容生成装置。
【請求項２】
少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、
ａ．少なくとも前記上体と脚部の運動と、床反力との関係を表す動力学モデル、
ｂ．少なくとも要求に応じ、前記脚部の軌道と前記床反力の軌道を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段、
ｃ．少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容の前記パラメータを想定する周期的歩容パラメータ想定手段、
ｄ．前記周期的歩容の前記パラメータに基づき、前記周期的歩容の前記上体の挙動を前記動力学モデルで近似したときの発散成分の歩容境界での値である、周期的歩容境界発散成分を決定する周期的歩容境界発散成分決定手段、
ｅ．前記今回歩容のパラメータに基づき、前記動力学モデルを用いて生成される今回歩容の終端における少なくとも前記発散成分が一致あるいは実質的に一致するように、前記今回歩容のパラメータを修正する今回歩容パラメータ修正手段、
および
ｆ．少なくとも前記修正された前記今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段、
を備えたことを特徴とする脚式移動ロボットの歩容生成装置。
【請求項３】
前記今回歩容と前記周期的歩容の間に過渡的な歩容が挿入されることを特徴とする請求の範囲の１項または２項記載の脚式移動ロボットの歩容生成装置。
【請求項４】
前記周期的歩容が静止歩容を含むことを特徴とする請求の範囲の３項記載の脚式移動ロボットの歩容生成装置。
【請求項５】
少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、
ａ．前記脚部の運動によって発生する慣性力と重力の合力である脚部反力を前記上体の挙動に依存せずに算出する脚部反力算出手段、
ｂ．少なくとも前記算出された脚部反力と目標床反力とから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段、
ｃ．少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段、
ｄ．少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置を算出する上体位置算出手段、
および
ｅ．少なくとも前記算出された上体位置を用いて歩容を生成する歩容生成手段、を備えたことを特徴とする脚式移動ロボットの歩容生成装置。
【請求項６】
少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、
ａ．前記脚部の運動によって発生する慣性力と重力の合力に疑似的に対応する前記脚部のＺＭＰに相当する脚部ＺＭＰを前記上体の挙動に依存せずに算出する脚部ＺＭＰ算出手段、
ｂ．少なくとも前記算出された脚部ＺＭＰと目標ＺＭＰとから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段、
ｃ．少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段、
ｄ．少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置を算出する上体位置算出手段、
および
ｅ．少なくとも前記算出された上体位置を用いて歩容を生成する歩容生成手段、を備えたことを特徴とする脚式移動ロボットの歩容生成装置。
【請求項７】
前記倒立振子モデル支点位置算出手段は、前記目標ＺＭＰに第１の係数を乗じて得た積から、前記脚部ＺＭＰに第２の係数を乗じて得た積を減算して前記倒立振子モデルの支点位置を算出することを特徴とする請求の範囲の６項記載の脚式移動ロボットの歩容生成装置。
【請求項８】
少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、
ａ．前記脚部の運動によって発生する慣性力と重力の合力である脚部反力を前記上体の挙動に依存せずに算出する脚部反力算出手段と、少なくとも前記算出された脚部反力と目標床反力とから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段と、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段と、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置を算出する上体位置算出手段とから構成される動力学モデル、
ｂ．少なくとも要求に応じ、前記脚部の軌道と前記床反力の軌道を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段、
ｃ．少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容の前記パラメータを想定する周期的歩容パラメータ想定手段、
ｄ．前記周期的歩容の前記パラメータに基づき、前記周期的歩容の境界における前記倒立振子モデルの発散成分である周期的歩容境界発散成分を決定する周期的歩容境界発散成分決定手段、
ｅ．前記今回歩容のパラメータに基づき、前記動力学モデルを用いて生成される今回歩容の終端における前記倒立振子モデルの発散成分が前記周期的歩容境界発散成分に一致または実質的に一致するように、前記今回歩容のパラメータを修正する今回歩容パラメータ修正手段、
および
ｆ．少なくとも前記修正された今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段、
を備えたことを特徴とする脚式移動ロボットの歩容生成装置。
【請求項９】
前記床反力が少なくともＺＭＰを含むことを特徴とする請求の範囲の１項から５項および８項のいずれかに記載の脚式移動ロボットの歩容生成装置。
【請求項１０】
前記倒立振子モデルが線形モデルであることを特徴とする請求の範囲の１項から９項のいずれかに記載の脚式移動ロボットの歩容生成装置。
【発明の詳細な説明】
技術分野
この発明は脚式移動ロボットの歩容生成装置に関し、より詳しくは脚式移動ロボットにおいて、その歩容を自在かつリアルタイムに生成するようにしたものに関する。
背景技術
本出願人は先に特開平１０−８６０８１号において、オフラインで設計した基準歩容をパラメータと上体軌道の時系列の組にしてロボットに搭載したマイクロコンピュータのメモリに記憶させ、歩行周期など、時間に関するパラメータが同一である複数の歩容の瞬時値の重み付き平均を求めることによって、自在な歩容を生成する技術を提案している。
しかしながら、この提案技術においては、重み付き平均の算出に際して歩行周期などの時間的パラメータが異なる複数の基準歩容を単に時間パラメータも含めて重み付き平均を取ると、動力学的平衡条件が、近似的にも成立しなくなっていた。したがって、この提案手法では、基準歩容と異なる周期の歩容を生成することはできなかった。
尚、動力学的平衡条件とは、目標歩容の重力と慣性力から計算されるＺＭＰが目標ＺＭＰに一致していること、より詳細には、ロボットの慣性力と重力の合力が目標ＺＭＰまわりに作用するモーメントの水平成分が０であることを意味する。また、ＺＭＰ（Ｚｅｒｏ　Ｍｏｍｅｎｔ　Ｐｏｉｎｔ）は、運動による慣性力と重力の合力が床と交わる点を意味する。より厳密には、ロボットの運動による慣性力と重力の合力のモーメントの水平成分が０となる、床上の作用点を意味する。
また、上記した提案技術にあっては、１歩の歩容と次の１歩の歩容の境界において、上体の位置と速度の両方を連続にするために、場合によっては目標ＺＭＰを理想の軌道から大きくずらす必要があったため、安定余裕が低下する場合もあった。
さらに、多様な歩行を実現するためには数多くの基準歩容の時系列データを記憶する必要があり、メモリ容量が増加する不都合があった。さらに基準歩容をオフラインで試行錯誤によって生成しなければならないと共に、基準歩容からかけ離れた歩容を生成しようとすると、近似度が大きく低下する不都合があった。
ところで、歩容は物理法則に逆らうことができないので、その許容範囲内でＺＭＰなどの歩容パラメータを生成する必要があり、しかも、２足歩行ロボットの動力学系は、ＺＭＰを入力、上体位置を出力とする系とみなすと発散系になるので、生成した歩容パラメータによってロボットの挙動が発散しないように配慮する必要がある。尚、「発散」とは、後で触れる第８図に示すように、２足歩行ロボットの上体の位置が両足部（足平）の位置からかけ離れた位置にずれてしまうことを意味する。
その点から、リアルタイムに歩容を生成する場合、生成した歩容パラメータによってロボットに生じるであろう将来の挙動を予測し、発散する可能性があるときは発散を防止することが望ましい。
しかしながら、第２３図に示すように、ロボット（２足歩行ロボット）を多質点系でモデル化すると、演算量が多くかつ非線形性が強くなるため、歩容の終端状態をリアルタイムに求めるのは、ロボットに搭載可能な通常のＣＰＵの能力では困難である。
他方、将来挙動予測とそれに基づく発散防止制御は、多くの知識を蓄積しておき、歩容切り変わり目毎に、そのときの状態と目的に応じ、蓄積した知識に照らし合わせて目標歩容を選び出す手法でも、あるレベルまでは達成することができるであろうが、ありとあらゆる状況に対応しようとすれば組合わせ爆発を起こし、事実上は不可能である。
したがって、脚式移動ロボットにおいて、リアルタイムに解析的に将来挙動が予測計算できるように、ロボットの動力学を記述する動力学モデルを単純化することが望まれていた。
動力学モデルとしては、従来から代表的なものとして次の２つが知られている。
１）１質点モデル
２）影響度の小さい質点を無視した多質点モデル
１質点モデルは、例えば特公平４−１５０６８号公報で提案されており、その提案技術にあっては、質点を上体だけに設定した１質点系としてロボットをモデル化すると共に、上体の高さを一定にすることで、線形性を確保している。このモデルは挙動を解析的に求めることが容易である。
この提案技術に係るロボットは脚が極めて軽いので、脚反力を無視したことの影響は小さいが、通常の人間型の２足歩行ロボットの場合、脚部の質量が無視できないほど大きいことから、この提案技術によるときは近似精度が低下し、高速で歩行させると、最悪の場合、転倒する。
２）の影響度の小さい質点を無視した多質点モデルの例としては、「路面形状に偏差のある環境における２足歩行制御」（日本ロボット学会誌（ＪＲＳＪ），Ｖｏｌ．１３，Ｎｏ．７，Ｏｃｔ．，１９９５）を挙げることができる。
その提案技術にあっては、各脚の膝と足首および上体にそれぞれ質点が設定されている。より具体的には、各質点は、あるリンクまたは関節上にローカルに設定された座標系内のある固定された点（座標）に設定される。
このモデルでは、第２３図に示すような全リンクに質点とイナーシャを設定する場合に比較すると、演算量が１０分の１程度に減少する。また、上記した１質点モデルよりは、近似精度が高くなり、脚の質量が大きいロボットでも、この手法で生成した目標歩容を用いて歩行させることができる。
しかしながら、第２３図に示した例と同様に、影響度の小さい質点を無視した多質点モデルにあっても、非線型性が依然強いので、解析的に将来挙動を予測して発散しないように対処することはできなかった。
発明の開示
したがって、この発明の第１の目的は、本出願人が先に特開平１０−８６０８１号において提案した技術や従来のモデル単純化技術の上記した不都合を解消し、脚式移動ロボットの床反力を含む歩容を、動力学的平衡条件をほぼ満足しながら、自在かつリアルタイムに生成して、任意の歩幅、旋回角、歩行周期などの歩容を生成できるようにした脚式移動ロボットの歩容生成装置を提供することにある。
さらに、この発明の第２の目的は、同様に先に提案した技術や従来のモデル単純化技術の上記した不都合を解消して脚式移動ロボットの歩容を自在かつリアルタイムに生成すると共に、生成された歩容同士の境界においてロボットの各部位の変位および速度が連続で、かつ安定余裕の高い歩容を生成できるようにした脚式移動ロボットの歩容生成装置を提供することにある。
さらに、この発明の第３の目的は、ロボットの将来の挙動を予測して姿勢の発散などの不都合が生じないように歩容を生成できるようにした脚式移動ロボットの歩容生成装置を提供することにある。
さらに、この発明の第４の目的は、高速演算を可能としてリアルタイムに歩容を生成できるようにした脚式移動ロボットの歩容生成装置を提供することにある。
上記した目的を達成するために、請求の範囲の１項にあっては、少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、少なくとも前記上体と脚部の運動と、床反力との関係を表す動力学モデル、少なくとも要求に応じ、前記脚部の軌道と前記床反力の軌道を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段、少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容の前記パラメータを想定する周期的歩容パラメータ想定手段、前記動力学モデルおよび前記今回歩容のパラメータとそれに続く前記周期的歩容のパラメータに基づいて決定される前記上体の軌道が、前記周期的歩容のパラメータから決定される前記周期的歩容の前記上体の軌道に実質的に収束あるいは一致するように、少なくとも前記仮決定された今回歩容のパラメータを修正する今回歩容パラメータ修正手段、および少なくとも前記修正された前記今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段を備えるように構成した。
上体と脚部の運動と、床反力との関係を表す動力学モデルを備え、要求に応じ、脚部の軌道などを決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定し、今回歩容に続く周期的歩容のパラメータを想定し、動力学モデルおよび今回歩容のパラメータとそれに続く周期的歩容のパラメータに基づいて決定される前記上体の軌道が、周期的歩容のパラメータから決定される上体の軌道に実質的に収束あるいは一致するように、少なくとも今回歩容のパラメータを修正し、修正今回歩容のパラメータに基づいて今回歩容の瞬時値を決定する如く構成したので、脚式移動ロボットの床反力を含む歩容を、動力学的平衡条件をほぼ満足しながら、自在かつリアルタイムに生成して、任意の歩幅、旋回角、歩行周期などの歩容を生成できるようにした脚式移動ロボットの歩容生成装置を提供することができる。
さらに、生成された歩容同士の境界においてロボットの各部位の変位および速度が連続で、かつ安定余裕の高い歩容を生成することができると共に、ロボットの将来の挙動を予測して姿勢の発散などの不都合が生じないように歩容を生成することができる。さらに、高速演算を可能としてリアルタイムに歩容を生成することができる。尚、動力学モデルは、例えば特公平４−１５０６８号公報で提案されるような１質点モデルであっても良い。
請求の範囲の２項にあっては、少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、少なくとも前記上体と脚部の運動と、床反力との関係を表す動力学モデル、少なくとも要求に応じ、前記脚部の軌道と前記床反力の軌道を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段、少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容の前記パラメータを想定する周期的歩容パラメータ想定手段、前記周期的歩容の前記パラメータに基づき、前記周期的歩容の前記上体の挙動を前記動力学モデルで近似したときの発散成分の歩容境界での値である、周期的歩容境界発散成分を決定する周期的歩容境界発散成分決定手段、前記今回歩容のパラメータに基づき、前記動力学モデルを用いて生成される今回歩容の終端における少なくとも前記発散成分が一致あるいは実質的に一致するように、前記今回歩容のパラメータを修正する今回歩容パラメータ修正手段、および少なくとも前記修正された前記今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段を備える如く構成した。これによって、請求の範囲の１項で述べたと同様の作用効果を得ることができる。
請求の範囲の３項にあっては、前記今回歩容と前記周期的歩容の間に過渡的な歩容が挿入される如く構成した。これによって、請求の範囲の１項で述べた作用効果に加え、周期的歩容を決定あるいは設定するときの制約条件（境界条件）が緩和され、一層所望の周期的歩容に近いものを設定することができるので、一層安定余裕を高めることができる。
請求の範囲の４項にあっては、前記周期的歩容が静止歩容を含む如く構成した。これにより、上記した作用効果に加え、前記周期的歩容境界発散成分の決定が容易となると共に、急停止における安定余裕を高めることができる。
請求の範囲の５項にあっては、少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、前記脚部の運動によって発生する慣性力と重力の合力である脚部反力を前記上体の挙動に依存せずに算出する脚部反力算出手段、少なくとも前記算出された脚部反力と目標床反力とから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置を算出する上体位置算出手段、および少なくとも前記算出された上体位置を用いて歩容を生成する歩容生成手段を備える如く構成した。
脚部の運動によって発生する慣性力と重力の合力である脚部反力を上体の挙動に依存せずに算出し、それと目標床反力とから、上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出し、それに基づいて倒立振子モデルの変位を算出し、それに基づいて前記上体の位置を算出すると共に、算出した上体位置を用いて歩容を生成する如く構成したので、脚式移動ロボットの動力学モデルを単純化して線形度を高めることができる。
即ち、脚式移動ロボットを２足歩行ロボットとするとき、倒立振子モデルでその動力学的挙動を記述すると共に、全ての力を床反力（より具体的にはＺＭＰ）相当値に換算しているので、ロボットの上体挙動を線形結合可能とすることができ、モデルの挙動が発散するか否かなどの挙動予測を、逐次、リアルタイムに演算することも可能となり、動力学的に一層安定した歩容を生成することができる。
請求の範囲の６項にあっては、少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、前記脚部の運動によって発生する慣性力と重力の合力に疑似的に対応する前記脚部のＺＭＰに相当する脚部ＺＭＰを前記上体の挙動に依存せずに算出する脚部ＺＭＰ算出手段、少なくとも前記算出された脚部ＺＭＰと目標ＺＭＰとから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置を算出する上体位置算出手段、および少なくとも前記算出された上体位置を用いて歩容を生成する歩容生成手段を備える如く構成した。これにより、請求の範囲の５項で述べたと同様の作用効果を得ることができる。
請求の範囲の７項にあっては、前記倒立振子モデル支点位置算出手段は、前記目標ＺＭＰに第１の係数を乗じて得た積から、前記脚部ＺＭＰに第２の係数を乗じて得た積を減算して前記倒立振子モデルの支点位置を算出する如く構成した。これによってモデルの上体挙動を完全に線形結合可能とすることができ、モデルの挙動が発散するか否かなどの挙動予測を、逐次、リアルタイムに演算することができ、動力学的に一層安定した歩容を生成することができる。また、これによって歩行以外の動作もリアルタイムに生成できるようになるので、歩行以外の任意の動作と歩行を連続的に滑らかにつなげることも可能となる。
請求の範囲の８項にあっては、少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、少なくとも前記上体と脚部の運動と、ＺＭＰとの関係を倒立振子を用いて表す動力学モデル、前記脚部の運動によって発生する慣性力と重力の合力である脚部反力を前記上体の挙動に依存せずに算出する脚部反力算出手段と、少なくとも前記算出された脚部反力と目標床反力とから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段と、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段と、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置を算出する上体位置算出手段とから構成される動力学モデル、少なくとも要求に応じ、前記脚部の軌道と前記床反力の軌道を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段、少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容の前記パラメータを想定する周期的歩容パラメータ想定手段、前記周期的歩容の前記パラメータに基づき、前記周期的歩容の境界における前記倒立振子モデルの発散成分である周期的歩容境界発散成分を決定する周期的歩容境界発散成分決定手段、前記今回歩容のパラメータに基づき、前記動力学モデルを用いて生成される今回歩容の終端における前記倒立振子モデルの発散成分が前記周期的歩容境界発散成分に一致または実質的に一致するように、前記今回歩容のパラメータを修正する今回歩容パラメータ修正手段、および少なくとも前記修正された今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段を備える如く構成した。これにより、請求の範囲の１項および５項で述べたと同様の作用効果を得ることができる。
請求の範囲の９項にあっては、前記床反力が少なくともＺＭＰを含む如く構成した。これにより、請求の範囲の１項および５項で述べたと作用効果に加え、床反力の記述が容易となって上記した作用効果を一層良く奏することができる。
請求の範囲の１０項にあっては、前記倒立振子モデルが線形モデルである如く構成した。これにより、請求の範囲の１項および５項、特に５項で述べたと同様の作用効果を得ることができる。
発明を実施するための最良の形態
以下、添付図面を参照してこの発明の一つの実施の形態に係る脚式移動ロボットの歩容生成装置を説明する。尚、脚式移動ロボットとしては２足歩行ロボットを例にとる。
第１図は、この実施の形態に係る歩容生成装置が適用される脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。
図示の如く、２足歩行ロボット（以下「ロボット」という）１は左右それぞれの脚部（脚部リンク）２に６個の関節を備える。６個の関節は上から順に、股（腰部）の脚部回旋（回転）用の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、股（腰部）のロール方向（Ｘ軸まわり）の関節１２Ｒ，１２Ｌ、股（腰部）のピッチ方向（Ｙ軸まわり）の関節１４Ｒ，１４Ｌ、膝部のピッチ方向の関節１６Ｒ，１６Ｌ、足首のピッチ方向の関節１８Ｒ，１８Ｌ、同ロール方向の関節２０Ｒ，２０Ｌから構成される。
関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には足平（足部）２２Ｒ，２２Ｌが取着されると共に、最上位には上体（基体）２４が設けられ、その内部にマイクロコンピュータからなる制御ユニット２６（後述）などが格納される。上記において、股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から、足関節（足首関節）は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２８Ｒ，２８Ｌ、膝関節と足関節とは下腿リンク３０Ｒ，３０Ｌで連結される。
尚、上体２４の上部には腕が取り付けられると共に、その上部には頭部が配置されるが、その詳細は、この発明の要旨と直接の関連を有しないため、省略する。
上記の構成により、脚部２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６＊２＝１２個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行させることができる（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）。
尚、この明細書で後述する上体２４の位置およびその速度は、上体２４の所定位置、具体的には上体２４の代表点の位置およびその移動速度を意味する。それについては後述する。
第１図に示す如く、足関節の下方には公知の６軸力センサ３４が取着され、力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚとを測定し、足部の着地の有無および床反力（接地荷重）などを検出する。また、上体２４には傾斜センサ３６が設置され、Ｚ軸（鉛直方向（重力方向））に対する傾きとその角速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコーダが設けられる。
第２図に示すように、足平２２Ｒ（Ｌ）の上方には、ばね機構３８が装備されると共に、足底にはゴムなどからなる足底弾性体４０が貼られてコンプライアンス機構４２を構成する。ばね機構３８は具体的には、足平２２Ｒ（Ｌ）に取り付けられた方形状のガイド部材（図示せず）と、足首関節１８Ｒ（Ｌ）および６軸力センサ３４側に取り付けられ、前記ガイド部材に弾性材を介して微動自在に収納されるピストン状部材（図示せず）とからなる。
図中に実線で表示された足平２２Ｒ（Ｌ）は、床反力を受けていないときの状態を示す。床反力を受けると、コンプライアンス機構４２においてバネ機構３８と足底弾性体４０がたわみ、足平２２Ｒ（Ｌ）は図中に点線で表示された位置姿勢に移る。この構造は、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。尚、その詳細は本出願人に先に提案した特開平５−３０５５８４号に記載されているので、詳細な説明は省略する。
さらに、第１図では図示を省略するが、ロボット１の適宜な位置にはジョイスティック４４が設けられ、外部から必要に応じて直進歩行しているロボット１を旋回させるなど歩容に対する要求を入力できるように構成される。
第３図は制御ユニット２６の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ３６などの出力はＡ／Ｄ変換器（図に「Ａ／Ｄ」と示す）５０でデジタル値に変換され、その出力はバス５２を介してＲＡＭ５４に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ５６を介してＲＡＭ５４内に入力される。
制御ユニット２６の内部にはそれぞれＣＰＵからなる第１、第２の演算装置６０，６２が設けられており、第１の演算装置６０は後述の如く、目標歩容を生成すると共に、後述の如く関節角変位指令を算出し、ＲＡＭ５４に送出する。また第２の演算装置６２はＲＡＭ５４からその指令と検出された実測値とを読み出し、各関節の駆動に必要な操作量を算出してＤ／Ａ変換器（図に「Ｄ／Ａ」と示す）６６とサーボアンプを介して各関節を駆動する電動モータに出力する。
第４図は、この実施の形態に係る脚式移動ロボットの歩容生成装置の構成および動作を全体的に示すブロック図である。
以下説明すると、この装置は歩容生成部１００を備え、歩容生成部１００は後述の如く目標歩容を自在かつリアルタイムに生成して出力する。目標歩容は、目標上体位置姿勢（軌道）、目標足平位置姿勢（軌道）、目標全床反力中心点（目標ＺＭＰ）（軌道）および目標全床反力（軌道あるいはパターン）からなる。
尚、各足平２２Ｒ，Ｌの床反力を「各足平床反力」と呼び、全ての（２本の）足平の床反力の合力を「全床反力」と呼ぶ。ただし、以降においては、各足平床反力はほとんど言及しないので、断らない限り、「床反力」は「全床反力」と同義として扱う。
最初にこの装置が生成する歩容について説明すると、この発明の目的は前記したように歩容をリアルタイムに生成する装置を提供することにあり、より具体的には本出願人が先に特開平１０−２７７９６９号公報において提案した複合コンプライアンス制御によるロボットの姿勢安定化制御を行うために必要な目標歩容を自在に生成することを目的としている。
その先に提案した姿勢安定化制御にあっては、床反力中心点（ＺＭＰ）の目標位置と実測位置の偏差を検出し、その偏差を解消するように脚部の一方または双方を駆動して姿勢の安定化を図っている。それと共に、ロボットが倒れそうになると、目標床反力を意図的にずらすことによって実床反力をずらし、姿勢復元力を得るようにしている。
かかる如く、脚式移動ロボットにおいては、目標運動軌跡と目標床反力パターンの関係が動力学的平衡条件を満足していなければ、安定した歩行が実現できない。動力学的平衡条件は前記した如く、具体的には、例えば、目標床反力中心点（目標床反力分布の２次モーメントが０になる床上の作用点）とＺＭＰが一致することである。一致していないと、コンプライアンス制御が働いたときに、慣性力と重力の合力と床反力との間のバランスを崩し、最悪の場合、転倒する。
先に提案した技術においては、それをロボットが不安定となったときの姿勢回復に逆用しているが、上記から明らかな如く、本出願人が提案する脚式移動ロボットにおいてはこのように歩行制御に与える目標値として、目標の運動パターンだけではなく、目標の運動パターンに対して動力学的平衡条件を満足する目標床反力パターンも必要である。
目標床反力は、一般的には、作用点とその点に作用する力と力のモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に前述の目標床反力中心点を作用点にして目標床反力を表現すると、力のモーメントは、床に垂直な成分を除けば、０になる。
尚、前述のように、動力学的平衡条件を満足する歩容では、目標とする運動軌跡から算出されるＺＭＰと目標床反力中心点は一致することから、目標床反力中心点軌道の代わりに目標ＺＭＰ軌道を与えると言っても同じことである。
従って、上記は、「歩行制御に与える目標値としては、目標運動軌跡だけでなく、目標ＺＭＰ軌道（目標床反力パターン）も必要である」と言い換えることができる。このような背景から、この明細書では目標歩容を、次のように定義する。
ａ）広義の目標歩容とは、１歩ないしは複数歩の期間の目標運動軌跡とその目標床反力パターンの組である。
ｂ）狭義の目標歩容とは、１歩の期間の目標運動軌跡とそのＺＭＰ軌道の組である。
ｃ）一連の歩行は、いくつかの歩容がつながったものとする。
尚、以下では、理解を容易にするために、特にことわらない限り、目標歩容は狭義の目標歩容の意味で使用する。より詳しくは、この明細書では目標歩容は、両脚支持期の初期から片脚支持期の終端までの意味で使用する。
また、両脚支持期とは言うまでもなく、ロボット１がその自重を脚部リンク２の双方で支持する期間を、片脚支持期とは脚部リンク２の一方で支持する期間をいう。片脚支持期においてロボット１の自重を支持しない側の脚部（リンク）を「遊脚」と呼ぶ。定義の詳細は、先に提案した特開平１０−８６０８１号公報に記載されているので、この程度の説明に止める。
この発明は、具体的には、上記に定義した目標歩容を自在かつリアルタイムに生成することを目的とする。自在かつリアルタイムに生成できれば、例えばロボット１を遠隔操縦によってオペレータが自由自在に操ることができ、また、自動操縦においても、単なる固定シーケンス通りに移動するだけでなく、誘導制御やデリケートな位置決めも可能となる。
ここで、目標歩容としての条件を説明する。
目標歩容が満たさなければならない条件は、大きく分けて以下の５つに分類される。
条件１）動力学的平衡条件を満足していること。即ち、ロボット１の目標運動軌跡から動力学的に算出されるＺＭＰ軌道が目標ＺＭＰ軌道と一致していること。
条件２）ロボット１の歩行計画部や歩行経路誘導部（共に図示せず）、あるいはオペレータから歩幅や旋回角など歩容が満たすべき条件が要求される場合、それらの要求条件を満たしていること。
条件３）足平が床を掘ったり擦ったりしない、関節角度が可動範囲を越えない、関節速度が限界を越えないなどの、キネマティクス（運動学）に関する制約条件を満たしていること。
条件４）片脚支持期においてＺＭＰが支持脚足平接地面内になければならない、駆動系の最大能力を越えないなどの、動力学に関する制約条件を満たしていること。
条件５）境界条件を満たしていること。即ち、条件１）の当然の帰結として、歩容と歩容の境界では、少なくとも、各部位の位置と速度が連続であるという境界条件が導かれる（不連続であれば、無限大の力が発生したり、ＺＭＰが接地面からはるかに遠くの点に移動してしまうから）。
また、第ｎ＋１回歩容の初期状態は、第ｎ回歩容の終端状態（特に、足平位置に対する上体の位置・姿勢および速度）に一致するように設定されなければならない。このとき、第ｎ回歩容の終端状態は、第ｎ＋１回歩容の初期状態が決まっていれば、第ｎ＋１回歩容の初期状態に一致させれば良い。
決まっていなければ、第ｎ回歩容の終端状態が、姿勢が崩れないで長期的な歩行ができる範囲に入っていれば良い。但し、後述するように、姿勢が崩れないで長期的な歩行ができるための終端状態の範囲を求めることは、極めて難しい。
一般的に、目標歩容は、パラメータ値あるいは時系列テーブルを含む歩容発生アルゴリズムによって発生させられる（歩容を決定するということは、パラメータ値あるいは時系列テーブルを適当に設定することに他ならない）。
パラメータ値あるいは時系列テーブルを変えることによって、様々な歩容が生成される。しかし、パラメータ値あるいは時系列テーブルを十分な配慮もせずに設定しただけでは、作成された歩容が前記の歩容条件をすべて満足しているか否かは分からない。
特に、長期的歩行に適した終端上体位置および速度の範囲が分かっていたとしても、ＺＭＰ軌道に基づいて上記した条件１）を満足する上体の軌道を歩容発生アルゴリズムに従って生成する場合には、発生した歩容の終端での上体位置と速度の両方がその範囲に入るようにＺＭＰ軌道に関するパラメータを設定することは、極めて難しい。
その理由は次の通りである。
理由１）上体は、一旦、ＺＭＰから遠くに離れると、さらに遠くに離れようとする発散傾向がある。これを説明するために、ロボットの上体の挙動に近い倒立振子の挙動を例に挙げる。
倒立振子は重心の床投影点が支点からずれると、ずれが発散して倒れる。しかし、そのときも倒立振子の慣性力と重力の合力は支点上に作用し（即ち、ＺＭＰが支点に一致し）、支点から受ける床反力に釣り合っている。即ち、動力学的平衡条件は、運動する物体のその瞬間における慣性力と重力と床反力の関係を表すだけのものである。
動力学的平衡条件を満足していればロボットの長期歩行が保証されたように錯覚しやすいが、ロボットの姿勢が崩れているか否かとは全く関係がない。倒立振子の重心が支点の真上から離れると、ますます遠くに離れようとする傾向があるように、ロボットの重心がＺＭＰの真上から遠くに離れるとますます遠くに離れようとする発散傾向がある。
理由２）片脚支持期においてＺＭＰが支持脚足平接地面内になければならないなどの厳しい制約条件があるために、上体の加減速パターンを恣意的に設定することができず、位置を合わせようとすると速度が合わず、速度を合わせようとすると位置が合わず、なかなか両方を同時に一致させることは難しい。以上の問題点のために、前記歩容条件を全て満たした目標歩容のパラメータやテーブルなどのオフラインで設計した基準歩容を持たず、完全にリアルタイムに決定することは、先に提案した技術も含めて従来では不可能であった。
歩容の説明を続けると、歩容は、歩容パラメータによって記述される。歩容パラメータは、運動パラメータとＺＭＰパラメータ（より一般的に表現すれば、床反力パラメータ）から構成される。尚、この明細書で「床反力パラメータ」なる語は、「床反力の時間的なパターンに関するパラメータ」を意味するものとして使用する。ＺＭＰパラメータは後で第１６図に示すように、Ｘ，Ｙ，Ｚ座標（方向）について折れ線グラフ状のＺＭＰ軌道の折れ点の位置と通過時刻で示す（Ｘ座標のみ図示）。
運動パラメータは、足平（軌道）パラメータと上体（軌道）パラメータとから構成される。
足平軌道パラメータは、初期（離床時）遊脚位置および姿勢、終端（着床時）遊脚位置および姿勢、両脚支持期時間、片脚支持期時間などを含む。これらに基づき、先に本出願人が特開平５−３１８３３９号および特開平５−３２４１１５号公報などで提案した技術を用いて、両足平の位置（ｘｆ，ｙｆ，ｚｆ）および姿勢（θｘｆ，θｙｆ，θｚｆ）の瞬時値が求められる。尚、ここで姿勢は「空間上の傾斜または向き」を意味する。
上体軌道パラメータは、上体の姿勢（空間上の上体２４の向きあるいは傾き）を決定するパラメータ、上体高さ（Ｚ方向の値）を決定するパラメータ、初期の上体位置（変位）および速度パラメータなどから構成される。
上体の位置および姿勢を決定するパラメータに基づき、時間関数または先に特開平５−３２４１１５号公報で本出願人が提案した技術や後述する手法から上体の水平位置（ｘｂ，ｙｂ）および姿勢（θｘｂ，θｙｂ，θｚｂ）の瞬時値が求められる。上体高さｚｂは適宜な手法、例えば本出願人が特開平１０−８６０８０号公報で提案する手法で決定される。
脚式移動ロボット１の関節は第１図に示すように１２関節から構成されているので、得られた両足平の位置・姿勢と上体位置・姿勢とから、後述するように逆キネマティクス演算によって目標関節変位が一義的に決定される。即ち、今回のロボットの目標姿勢が一義的に決定される。
ところで、理想的目標歩容は、物理法則に逆らうことができないので、希望する状態にすぐに到達することはできない。許容範囲内でＺＭＰの軌道、着地位置および着地時期などの歩容パラメータを変更することによって、時間をかけて希望する状態に遷移しなければならない。特に、この実施の形態において２足歩行ロボットの動力学系は、ＺＭＰを入力、上体位置を出力とする系とみなすと発散系になるので、慎重に歩容パラメータを変更しないと、正常な状態に復元するのが困難となる。
したがって、目標歩容を自在かつリアルタイムに生成するときは、ロボットの将来の挙動を予測し、どのように歩容パラメータ値を設定すれば、ロボットの将来、例えば数歩先の挙動が発散しないか否かを判断すると共に、発散する可能性が予測されるときは発散を防止するように歩容を調整することが、望ましい。
しかしながら、前記したように、第１９図に示すような多質点系モデルを用いるとき、その動力学演算は、演算量が多くかつ非線形性が強いので、終端状態をリアルタイムに求めるのは、ロボットに搭載可能な通常のＣＰＵ（第１の演算装置６０）の能力では困難である。
他方、将来挙動予測とそれに基づく発散防止制御は、多くの知識を蓄積させておき、歩容切り変わり目毎に、そのときの状態と目的に応じ、知識に照らし合わせて目標歩容を選び出す手法でも、あるレベルまでは達成することができるであろうが、ありとあらゆる状況に対応することは、組合わせ爆発を起こし、事実上不可能である。
そこで、この実施の形態においては、ロボット１の動力学的挙動を記述する動力学モデルを単純化し、リアルタイムかつ解析的に将来挙動が予測計算できるようにした。第５図に、その単純化した動力学モデルを示す。図示の如く、この動力学モデルは３質点モデルであり、デカップルド、即ち、脚部の動力学と上体の動力学が相互に非干渉に構成されると共に、ロボット全体の動力学は、それらの線形結合で表される。第６図は、歩容生成部１００の動力学演算部において第５図に示す動力学モデルを用いて行われる動力学演算を示すブロック図である。
以下、この動力学モデルを説明する。
１）このモデルは、倒立振子、支持脚足平質点、遊脚足平質点の３質点から構成される。
２）支持脚足平質点は、支持脚足平にローカルに設定された座標系（具体的には、原点が足首中心から足底への垂直投影点、ＸＹ平面が足底に一致し、かかとからつまさきへの向きをＸ軸にとったＸＹＺ直交座標系であり、これを以降、「支持脚ローカル座標系」と呼ぶ）上のある固定された点に設定される。この固定された点の支持脚ローカル座標系上の座標を、「支持脚足平質点オフセット」と呼ぶ。
同様に、遊脚足平質点は、遊脚足平にローカルに設定された座標系（これを以降、「遊脚ローカル座標系」と呼ぶ）上のある固定された点に設定される。この固定された点の遊脚ローカル座標系上の座標を、「遊脚足平質点オフセット」と呼ぶ。
尚、支持脚が床に全面的に密着しているときの足首位置の床への鉛直投影点を原点とし、床に固定され、支持脚足平の前方向をＸ軸、左方向をＹ軸、鉛直方向をＺ軸にとった座標系を「支持脚座標系」と呼ぶ（これは．上記した支持脚ローカル座標とは異なる。）。ことわらない限り、位置、速度、力などは支持脚座標系で示す。
３）倒立振子は、水平に移動するフリーの支点ａと、ひとつの質点ｂと、支点と質点を結ぶ質量のない可変長のリンクｃから構成される。また、リンクが傾いてもリンクが伸縮し、支点から見た質点高さが一定値に保たれるものとする。（換言すれば、この倒立振子は、前記した特公平４−１５０６８号公報に記載されるロボットの線形倒立振子と同一である。）
倒立振子質点は、物理的意味としては上体２４の質点（必ずしも重心位置を意味しない）に対応する。したがって、倒立振子質点と上体質点は、今後、同意語として扱う。倒立振子質点の位置（より広義に言えば変位）を、以降、略して「倒立振子位置」と呼ぶ。
４）上体の水平位置は、倒立振子の水平位置から幾何学的に決定される。具体的には、例えば、上体にローカルに設定された座標系（これを以降、「上体座標系」と呼ぶ）上のある固定された代表点（この点を以降、「上体代表点」と呼ぶ）の水平位置（支持脚座標系から見たＸＹ座標）が、倒立振子の水平位置に一致するように決定される。即ち、第５図に示すように、上体代表点と倒立振子質点ｂは、同一鉛直線上にあるように決定される。
上体代表点の上体座標系上の水平座標（ＸＹ座標）を「上体質点オフセット」と呼ぶ。
次いで、図示の動力学演算モデルに関する変数およびパラメータなどの記述法について説明する。
説明の便宜上、動力学演算モデルに関する変数およびパラメータなどを以下のように定義し、記述する。
倒立振子のＺＭＰは、支点ａの位置にある。なぜなら、ＺＭＰは定義からモーメントが発生しない点のことであり、フリーの支点ａにはモーメントが発生しないからである。そこで、倒立振子支点位置を倒立振子自身のＺＭＰ位置（ＺＭＰ相当値）ということで、以降、「倒立振子ＺＭＰ」と呼び、「ＺＭＰｐｅｎｄ」と記述する。
各質点の質量と位置は次のように記述する。
ｍｓｕｐ：支持脚質点質量
ｍｓｗｇ：遊脚質点質量
ｍｂ　　：倒立振子質点質量（上体質点質量）
ｍｔｏｔａｌ：ロボット質量（＝ｍｂ＋ｍｓｕｐ＋ｍｓｗｇ）
ｍｆｅｅｔ　：両脚質量（＝ｍｓｕｐ＋ｍｓｗｇ）
ｘｓｕｐ：支持脚質点位置
ｘｓｗｇ：遊脚質点位置
ｘｂ　　：倒立振子位置（上体質点位置）
以降、ことわらない限り、ｘｂは３次元ベクトル（ＸＹＺ座標ベクトル）で表わす。倒立振子の高さは、倒立振子の支点から質点までの高さを意味し、ｈと記述する。
ｄ（ｘｂ）／ｄｔはｘｂの１階微分を表わし、倒立振子の速度を示す。ｄ２（ｘｂ）／ｄｔ２はｘｂの２階微分を表わし、倒立振子の加速度を示す。ｇは重力加速度定数を示す。Ｇは重力加速度ベクトルを示し、Ｘ，Ｙ成分が０、Ｚ成分が−ｇであるベクトルと定義する。
図示の３質点モデルにおいて、脚質点の総慣性力がある作用点Ｐまわりに作用するモーメントを、点Ｐまわりの脚総慣性力モーメントと定義する（慣性力と重力の合力を「総慣性力」と呼ぶ）。作用点Ｐの座標（あるいは位置）をｘｐと置く。
下記の数式１は、点Ｐまわりの脚総慣性力モーメントの厳密な動力学的定義式である。
2002040224.tif 000003
脚ＺＭＰをＺＭＰｆｅｅｔと記述し、脚ＺＭＰを数式２で定義する。ただし、脚ＺＭＰの高さ（ＺＭＰｆｅｅｔのＺ成分）は、点Ｐ位置の高さと同一とする。このように、脚ＺＭＰは、脚部の運動によって発生する慣性力と重力の合力（総慣性力）に疑似的に対応させた値である。
2002040224.tif 000004
本来、第１図に示すロボット１の動力学は非線形であるが、近似して目標ＺＭＰ、脚ＺＭＰ（ＺＭＰｆｅｅｔ）、および倒立振子ＺＭＰ（ＺＭＰｐｅｎｄ）の間には数式３の線形関係を与える。
2002040224.tif 000005
一般に、線形倒立振子の挙動を表わす微分方程式は、数式４で表わされる。
2002040224.tif 000006
ただし、作用点Ｐは、モデルの近似精度が高くなるように設定される。例えば、作用点Ｐは、第７図タイム・チャートに示すように、直前（前回）歩容の支持脚座標系上の原点から、今回歩容の支持脚座標系の原点に、両脚支持期の間に直線的に等速移動する点に設定される。
この実施の形態に係る脚式移動ロボットの歩容生成装置で提案する動力学モデルは、第５図に示すような足平、上体と各質点の位置の関係を表わす前記オフセットと、上記の数式（数式１から４）で表わされる。これにより、後述の如く、将来挙動予測が容易となった。
第６図を参照して歩容生成部１００（第１図に示す）の動力学演算部の動作を説明すると、動力学演算部は脚ＺＭＰ算出器２００を備え、脚ＺＭＰ算出器２００は、数式１および数式２ならびに第７図で示される作用点Ｐに基づいて脚ＺＭＰ（ＺＭＰｆｅｅｔ）を算出する。
次いで、上記算出したＺＭＰｆｅｅｔをｍｆｅｅｔ／ｍｔｏｔａｌ倍したものを、目標ＺＭＰから減じ、さらに因って得た差をｍｔｏｔａｌ／ｍｂ倍することによって、倒立振子ＺＭＰ（ＺＭＰｐｅｎｄ）を算出する。即ち、倒立振子ＺＭＰ（ＺＭＰｐｅｎｄ）は、前記ロボットの質量ｍｔｏｔａｌと前記上体の質点の質量ｍｂの比に前記目標ＺＭＰを乗じて得た積から、前記脚部の質量ｍｆｅｅｔと前記上体の質点の質量ｍｂの比に脚部のＺＭＰ相当値ＺＭＰｆｅｅｔを乗じて得た積を減算して算出される。この処理は、数式３に相当する。
倒立振子の挙動は、数式４で表現され、倒立振子ＺＭＰ（ＺＭＰｐｅｎｄ）から倒立振子質点水平位置ｘｂが算出される。
さらに、第６図に示す如く、動力学演算部は上体位置決定器２０２を備え、上体位置決定器２０２は、倒立振子質点水平位置から上体の水平位置ｘｂを決定する。具体的には、上体位置決定器２０２は、前述した上体代表点（第５図に示す）の水平位置が、倒立振子の水平位置に一致するように上体の水平位置を決定する。
次いで、歩容の継続的姿勢安定性について述べる。
ここまで述べてきた動力学モデル自身は、単に、各瞬間における動力学的平衡条件を近似的に満足するように、目標ＺＭＰから上体軌道を算出するだけのものであり、上体軌道が発散すること（第８図に示すように上体２４の位置が両足平２２Ｒ，Ｌの位置からかけ離れた位置にずれてしまうこと）を防止するものではない。
以下では、上体軌道の発散を防止し、上体と両足平の間の適切な位置関係を継続させるための手段について説明する。
最初に、そのための予備知識として、将来の上体軌道を解析的に求める上で特に重要な、線形倒立振子の性質について議論する。ここでは、離散化モデルを用いて説明する。
先ず、倒立振子に関し、新たに以下の変数やパラメータを定義する。
ω０：　　倒立振子の固有角周波数
ω０＝ｓｑｒｔ（ｇ／ｈ）（ただし、ｓｑｒｔは平方根を表わす。）
Δｔ：　　刻み時間
ｘ［ｋ］：　ｋステップ目（時刻ｋΔｔ）の倒立振子位置
ｖ［ｋ］：　ｋステップ目の倒立振子速度
ＺＭＰｐｅｎｄ［ｋ］：　ｋステップ目の倒立振子ＺＭＰ（詳しくは、時刻ｋΔｔから時刻（ｋ＋１）Δｔまで、入力ＺＭＰｐｅｎｄ［ｋ］の０次ホールドした信号が入力、即ち、その時刻の間は同一信号が倒立振子に入力され続けられるものとする。）
さらに、ｑ［ｋ］とｐ［ｋ］を数式５で定義する。
2002040224.tif 000007
倒立振子の運動方程式を離散化し、ｑ［ｋ］とｐ［ｋ］に関して解くと、以下の数式６と数式７が得られる。尚、ｅｘｐは指数関数（自然対数）を表す。
2002040224.tif 000008
これらの式の意味を考える。
ある有限な定数ＺＭＰｍｉｎおよびＺＭＰｍａｘに対し、数式８を常に満足するように、ＺＭＰｐｅｎｄ［ｉ］が設定されるものと仮定する。
2002040224.tif 000009
数式８の中辺と右辺の関係を、数式６に代入すると、下記の数式９が得られる。
2002040224.tif 000010
等比級数の和の定理から、下記の数式１０が得られる。
2002040224.tif 000011
したがって、数式９は、数式１１のように書き直すことができる。
2002040224.tif 000012
同様に、数式８の左辺と中辺の関係から、数式１２を得ることができる。
2002040224.tif 000013
ｅｘｐ（−ω０ｋΔｔ）は、ｋが無限大になると０に収束（漸近）するので、数式１１，１２は、ＺＭＰｐｅｎｄが変動してもｐ［ｋ］は発散せず、いずれはＺＭＰｍａｘとＺＭＰｍｉｎの間に入ることを意味する。
さらに、具体例として、ＺＭＰｐｅｎｄがある時点以降、一定値ＺＭＰ０になる場合を考える。この場合、その時点を改めて時刻０とすると、数式６は、数式１３のように書き直すことができる。これは、ｐ［ｋ］が、その初期値にかかわらず、ＺＭＰ０に等比級数的に収束することを意味する。
2002040224.tif 000014
より一般的には、ｐ［ｋ］は、ある時点でどのような値であっても、その後のＺＭＰｐｅｎｄ波形がある同一波形であれば、ＺＭＰｐｅｎｄ波形に追従する、ある軌道に収束する。
一方、ｑ［ｋ］は、数式７から明らかなように発散する傾向がある。
具体例として、ＺＭＰｐｅｎｄがある時点以降、一定値ＺＭＰ０になる場合を考える。この場合、一定値ＺＭＰ０になる時点を改めて時刻０とすると、数式７は、数式１４となり、これは、ｑ［０］＝ＺＭＰ０ではない限り、ｑ［ｋ］が、ＺＭＰ０から等比級数的に発散することを意味する。
2002040224.tif 000015
そこで、以降、数式５によって定義されるｐ［ｋ］を収束成分、ｑ［ｋ］を「発散成分」と呼ぶ。
以上より、上体軌道の発散を防止し、上体と両足平の間の適切な位置関係を継続させるためには、事実上、収束成分は無視して構わず、支持脚から見た発散成分を歩行に支障ない範囲から越えないように管理することが重要と言うことができる。
より具体的には、発散成分を歩行に支障ない範囲（姿勢が大きく崩れない範囲）から越えないように、ＺＭＰ軌道パラメータなどを適切に決定することが重要であると言える。尚、上体挙動を線形化しなければ、上体挙動を求める式が、数式６，７のように発散成分と収束成分に分離した式にすることができないので、上記のような議論を展開することができなかったが、この実施の形態において可能となった。
収束成分と発散成分の性質を以下に列挙する。
性質１：対称性
収束成分ｐ［ｋ］と発散成分ｑ［ｋ］を求める式の違いは、ｐ［ｋ］とｑ［ｋ］が置き換わったことと、ω０が−ω０に置き換わったことだけである。
性質２：等価換算
上記性質から、以下では発散成分についてのみ議論する。
入力ＺＭＰｐｅｎｄ［ｉ］として、ＺＭＰｐｅｎｄ［０］＝Ｕ０と、ＺＭＰｐｅｎｄ［ｉ］＝０（ｉ＝１，２，．．．）で表わされるパルスを考えると、そのときのｑ［ｋ］は、数式７にこのＺＭＰｐｅｎｄ［ｉ］を代入することにより得られ、数式１５となる。
2002040224.tif 000016
これより、発散成分ｑ［ｋ］に対する影響だけを考えると、初期パルスＵ０を入力することは、Ｕ０＊（ｅｘｐ（−ω０Δｔ）−１）だけ初期発散成分をずらすことと同じ影響を与える。即ち、初期パルスは、（ｅｘｐ（−ω０Δｔ）−１）倍することによって、初期発散成分に等価換算される。
また、数式７と数式１５を比較することにより、発散成分ｑ［ｋ］に対する影響だけを考えると、入力ＺＭＰｐｅｎｄ［ｉ］（ｉ＝０，１，２．．．，ｋ−１）を入力することは、数式１６に示す初期パルス入力Ｕ０と同じ影響を与える。
2002040224.tif 000017
即ち、入力ＺＭＰｐｅｎｄ［ｉ］（ｉ＝０，１，２．．．，ｋ−１）は、数式１６によって、初期パルス入力Ｕ０に等価換算される。
性質３：時間遅れの影響
また、第９図に示すように、ＺＭＰｐｅｎｄ［ｉ］がある関数ｆ（ｉ）であり、ＺＭＰｐｅｎｄ’［ｉ］が関数ｆ（ｉ）を時間ｎΔｔだけ遅らせた関数ｆ（ｉ−ｎΔｔ）であるとし、入力列ＺＭＰｐｅｎｄ［ｉ］を初期パルスに変換したものをＵ０、入力列ＺＭＰｐｅｎｄ’［ｉ］を初期パルスに変換したものをＵ０’とすると、Ｕ０’は数式１７のようになる。
2002040224.tif 000018
即ち、同一波形の入力を時間ｎΔｔだけ遅らせて入力すると、その入力によって発生する発散成分は、ｅｘｐ（−ｎω０Δｔ）倍になる。
次いで、第５図に示す倒立振子モデルに、複雑な倒立振子ＺＭＰ波形を入力したときの発散成分について述べる。
ところで、第５図（または数式３）に示す動力学モデル内の倒立振子モデルに与えられるＺＭＰ入力は、目標ＺＭＰにｍｔｏｔａｌ／ｍｂを乗じたものと、脚ＺＭＰにｍｆｅｅｔ／ｍｂを乗じたものの差である。目標ＺＭＰは具体的には、折れ線状の波形であるので、目標ＺＭＰにｍｔｏｔａｌ／ｍｂを乗じたものも、やはり折れ線の波形である。
そこで、以下では、折れ線状のＺＭＰが倒立振子に入力されたことによって発生する発散成分を解析的に求める原理とアルゴリズムを説明する。尚、脚ＺＭＰに関する影響については、後述する。また、実際には、倒立振子ＺＭＰは、２次元空間（水平面上）を移動するが、以下の議論では、理解の容易のため、１次元空間を移動するものとする。
倒立振子のＺＭＰ（ＺＭＰｐｅｎｄ）波形として第１０図に示すような折れ線を想定する。区間ｉは、期間（時間）がｐｅｒｉｏｄ［ｉ］でその区間の初期値がＺＭＰ［ｉ］であるとする。このＺＭＰによって発生する発散成分を求めるための原理とアルゴリズムを説明する。
倒立振子モデルに倒立振子ＺＭＰ入力として、ｘ＝ｆ（ｔ）が時刻範囲［０，Ｔ］の間入力されたことによって発生する発散成分を、時刻０に入力され、時間幅が刻み時間Δｔのパルスの大きさに換算したものをＨ（ｆ（ｔ），Ｔ）とする。
数式１６から、Ｈ（ｆ（ｔ），Ｔ）は、次の数式１８に示す積和演算で表わされる（ω：倒立振子の固有角周波数）。
2002040224.tif 000019
ところで一般的に、区間ｉの倒立振子ＺＭＰ波形は、第１１図に示すように、ある遅れを持つステップ波形とランプ波形とに分解できる。そこで、まずステップ状の波形とランプ状の波形に対するＨ（ｆ（ｔ），Ｔ）を以下に述べる手順で求めておく。
ｘ＝ｆ（ｔ）＝１のステップ波形のＨ（ｆ（ｔ），Ｔ）をＨ（１，Ｔ）と記述すると、等比級数の和の公式から、数式１９に示すようになる。
2002040224.tif 000020
ｘ＝ｆ（ｔ）＝ｔのランプ波形のＨ（ｆ（ｔ），Ｔ）をＨ（ｔ，Ｔ）と記述すると、数式２０に示すようになる。
2002040224.tif 000021
尚、ＺＭＰ波形が折れ線以外であるときは、上記以外の関数ｆが必要となる。上記以外の関数ｆでは、解析解Ｆ（ｆ（ｔ），Ｔ）が存在しない場合もあるが、その際には、関数ｆを正規化してから、Ｈ（ｆ（ｔ），Ｔ）をテーブル化すれば良い。尚、上記例でもＦ（ｆ（ｔ），Ｔ）をテーブル化しても良い。
さて、ここまで準備しておいてから、第１０図の倒立振子ＺＭＰによって発生する発散成分を、時刻０に入力されるパルスであって、その時間幅が刻み時間Δｔの倒立振子ＺＭＰパルスの大きさに換算したもの（以降、これをＷと記す）を以下の手順で求める。
まず、区間ｉ（ｉ＝０，１，…５）の倒立振子ＺＭＰの影響を、区間ｉの初期時刻に入力されるパルスであって、その時間幅が刻み時間Δｔのパルスの大きさｉｎｆ［ｉ］に換算する。区間ｉの倒立振子ＺＭＰ波形は、第１１図に示すように、高さがＺＭＰ［ｉ］の矩形波と傾きが（ＺＭＰ［ｉ＋１］−ＺＭＰ［ｉ］）／ｐｅｒｉｏｄ［ｉ］のランプ波の和で表わされるので、パルス換算値は数式２１に示すようになる。
2002040224.tif 000022
次いで、区間ｐｅｒｉｏｄ［ｉ］（ｉ＝０，１，．．，ｍ）の倒立振子ＺＭＰの影響を、時刻０に入力されるパルスであって、その時間幅が刻み時間Δｔのパルスの大きさｉｎｆ’［ｉ］に換算する。性質３から、パルス換算値は数式２２のように算出される。
2002040224.tif 000023
第１０図に示す倒立振子ＺＭＰ波形全区間の影響は、各区間の倒立振子ＺＭＰの影響の和になる。即ち、パルス換算値Ｗは、数式２３に示すように算出される。
2002040224.tif 000024
さらに、性質２より、上記Ｗを初期発散成分に等価換算したものは、以下の数式２４に示す値になる。
2002040224.tif 000025
さらに、数式２４または性質３より、これを歩容終端時刻での発散成分に換算したものは、数式２５に示す値になる。
2002040224.tif 000026
以上の手順により、折れ線の倒立振子ＺＭＰによって発生する歩容終端発散成分を解析的に求めることができる。
次いで、歩行の継続性を保証する歩容生成アルゴリズムについて述べる。
先に、上体軌道の発散を防止し、上体と両足平の間の適切な位置関係を継続させるためには、発散成分を歩行に支障ない範囲（姿勢が大きく崩れない範囲）から越えないように、ＺＭＰ軌道パラメータなどを適切に決定することが重要であると述べた。次の問題は、歩行に支障ない発散成分の範囲あるいは適切な発散成分の値を如何に求めるかである。
この実施の形態に係る歩容生成部１００は、今回生成する歩容につながるべき歩容（以下「定常旋回歩容」と呼ぶ。これについては後述する）を仮に想定することによって、適切な発散成分の値を決定するようにした。以下に歩容生成部１００の動作を詳細に説明する。
歩容生成部は、２歩先までの遊脚足平着地位置姿勢、着地時刻の要求値（要求）を入力として、目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標ＺＭＰ軌道を決定する。このとき、歩容パラメータの一部は、歩行の継続性を満足するように修正される。生成しようとしている歩容を「今回歩容」、その次の歩容を「次回歩容」、さらにその次の歩容を「次次回歩容」と呼ぶ。
第１２図は、その歩容生成部１００の歩容生成処理を示すフロー・チャート（構造化フロー・チャート）である。
以下説明すると、まずＳ１０において時刻ｔを０に初期化するなど種々の初期化作業を行う。次いでＳ１２を経てＳ１４に進み、制御周期毎のタイマ割り込みを待つ。制御周期はΔｔである。次いでＳ１６に進み、歩容切り変わり目であるか否かを判断し、肯定されるときはＳ１８に進むと共に、否定されるときはＳ２０に進む。
Ｓ１８に進むときは時刻ｔを０に初期化し、次いでＳ２２に進み、次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期を読み込む。これらは前記した要求に相当するが、あらかじめ歩行スケジュールとして記憶しておいても良く、あるいはジョイスティック４４などの操縦装置からの指令（要求）とそのときまでの歩行履歴を基に決定しても良い。
次いで、Ｓ２４に進み、今回歩容の歩容パラメータを仮決定（仮に算出）する。
今回歩容初期遊脚足平位置姿勢は、今回支持脚座標系から見た現在遊脚位置姿勢にする（初期値とする）。今回歩容初期支持脚足平位置姿勢は、今回支持脚座標系から見た現在支持脚位置姿勢にする（初期値とする）。今回歩容終端遊脚足平位置姿勢は、今回支持脚座標系から見た次回支持脚座標系に対応して決定する。（即ち、今回歩容終端遊脚足平位置姿勢から足平を床に接触させたまま、すべらないように足平を水平まで回転させたときの位置姿勢が、次回支持脚座標系となるように設定する。）
今回歩容終端支持脚足平位置姿勢は、現在支持脚位置姿勢から足平を床に接触させたまま、すべらないように足平を床に面接触するまで回転させたときの位置姿勢とする。したがって、床が平面であるならば、今回歩容終端支持脚足平位置姿勢は、今回支持脚座標系に一致する。尚、この歩容では、歩容終端において支持脚足平は水平になるが、必ずしも、このように設定する必要はない。
今回歩容のＺＭＰ軌道パラメータは、安定余裕が高く、かつ急激な変化をしないように決定する。（接地面を含む最小の凸多角形（いわゆる支持多角形）の中央付近にＺＭＰが存在する状態を安定余裕が高いと言う（詳細は特開平１０−８６０８１号公報に記述））。ただし、今回歩容のＺＭＰ軌道パラメータは、仮決定されただけであり、後述するように修正される。
次いでＳ２６に進み、今回歩容につながる定常旋回歩容の歩容パラメータを決定する。
尚、この明細書で「定常旋回歩容」は、その歩容を繰り返したときに歩容の境界において運動状態に不連続が生じないような周期的歩容を意味するものとして使用する。
定常旋回歩容は、通常、第１旋回歩容と第２旋回歩容とからなる。尚、ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。定常旋回歩容は、歩容生成部１００で今回歩容の終端における発散成分を決定するために暫定的に作成されるものであり、歩容生成部１００からそのまま出力されるものではない。
補足すると、定常旋回歩容は、狭義の意味での歩容（両脚支持期の初めから次の片脚支持期の終わりまでを１歩とする歩容）が、少なくとも２歩必要である。３歩以上の歩容からなる複雑な定常旋回歩容を設定することも可能であるが、今回歩容の終端における発散成分を決定するだけに用いられるので、処理が煩雑となるにも関わらず、効果は少ない。尚、３足以上の脚式移動ロボットにあっては、それに応じて旋回を定義するに足る歩容数が増加する。
先ず、今回歩容、第１旋回歩容、第２旋回歩容の順に脚軌道がつながるように、第１旋回歩容と第２旋回歩容の歩容パラメータ中の脚軌道の境界条件を設定する。
具体的には、第１旋回歩容初期遊脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とする。第１旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とする。
第１旋回歩容終端遊脚足平位置姿勢は、今回歩容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容支持脚座標系から見た次次回歩容支持脚座標系に対応して決定する。第１旋回歩容終端支持脚足平位置姿勢は、次回歩容支持脚座標系に一致させた足平を床に接触させたまま、すべらないように足平を床に面接触するまで回転させたときの位置姿勢とする。（したがって、床が平面であるならば、第１旋回歩容終端支持脚足平位置姿勢は、次回支持脚座標系に一致する。）
第２旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端支持脚足平位置姿勢にする。第２旋回歩容初期支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端遊脚足平位置姿勢にする。
第２旋回歩容終端遊脚足平位置姿勢は、今回支持脚座標系から見た今回歩容終端遊脚足平位置姿勢にする。第２旋回歩容終端支持脚足平位置姿勢は、今回支持脚座標系から見た今回歩容終端支持脚足平位置姿勢にする。
これらの関係を第１３図に示す。
第１旋回歩容および第２旋回歩容の歩行周期は、次回歩容周期と同一にする（尚、同一にすることは必ずしも必要ではないが、次回歩容周期に応じて決定するのが好ましい）。今回歩容、第１旋回歩容および第２旋回歩容の上記以外の運動パラメータ（両脚支持期時間などの時間パラメータを含む）は、上記決定されたパラメータに応じて、歩容の条件（電動モータ（アクチュエータ）の速度が許容範囲に入っていることなど）を満足するように適宜決定する。
第１旋回歩容および第２旋回歩容のＺＭＰ軌道パラメータも、安定余裕が高くかつ急激な変化をしないように決定する。
ところで、上体代表点の初期位置と速度をある値Ｘ０，Ｖ０に設定すると、第５図に示す単純モデルを用いて上記第１旋回歩容と第２旋回歩容を生成し、再び第１旋回歩容の生成を開始するときの初期上体代表点位置速度が、前記設定した初期上体代表点位置速度の値Ｘ０，Ｖ０に一致する。この値Ｘ０，Ｖ０を「定常旋回歩容の初期上体代表点位置速度」と呼ぶ。この関係を図に表すと、第１４図のようになる。ただし、Ｘ０を（ｘ０，ｙ０）と表すこととし、Ｖ０については表記は省略した。
このように設定すると、第５図に示す単純モデルを用いて第１旋回歩容と第２旋回歩容を交互に繰り返し生成しても、演算誤差が蓄積しない限り、第１旋回歩容の初期上体代表点位置速度は、値Ｘ０，Ｖ０になる。即ち、歩行の継続性が保証される。このときの発散成分、即ち、Ｘ０＋Ｖ０／ω０を、「定常旋回歩容の初期発散成分」と呼ぶ。
第１２図の説明に戻ると、次いでＳ２８に進み、定常旋回歩容の初期発散成分を求める。
第１５図は、第１２図フロー・チャートのＳ２８の定常旋回歩容の初期発散成分を算出するサブルーチン・フロー・チャートである。
ここで、同図の説明に入る前に、定常旋回歩容の初期発散成分の算出原理を説明する。
数式３と数式７から、次の数式２６が得られる。尚、目標ＺＭＰをＺＭＰｔｏｔａｌと記述する。
2002040224.tif 000027
実際には、各歩容はその歩容の支持脚座標系上で表現されるが、ここでは、説明を容易にするために、第１旋回歩容と第２旋回歩容は、第１旋回歩容の支持脚座標系で表現されているものとし、前記したように第１旋回歩容と第２旋回歩容を合わせて一つの歩容（定常旋回歩容）とみなす。
数式２６を用いて、定常歩旋回容の終端における発散成分ｑ［ｋ］（これを以降、「終端発散成分」と呼ぶ）を求めることを考える。時刻０は、定常旋回歩容の初期時刻とする。定常旋回歩容の終端時刻はｋΔｔとする。尚、終端時刻は、その次の歩容の時刻０に対応する。
右辺第１項は、初期発散成分によって発生する終端発散成分を表わす。右辺第２項は、目標ＺＭＰパターンによって発生する終端発散成分を表わす。右辺第３項は、脚運動によって発生する終端発散成分を表わす。
まず、目標ＺＭＰパターンによって発生する終端発散成分は、目標ＺＭＰパターンが折れ線であるので、前述の折れ線状ＺＭＰｐｅｎｄによって発生する終端発散成分を求める、数式１９，２０，２１，２２，２３，２５に示すアルゴリズムを用いて解析的に求める。これをＷｚｍｐｔｏｔａｌとする。（ＸＹ方向があるので、Ｗｚｍｐｔｏｔａｌは２次元ベクトルになる。）
次に、脚運動によって発生する終端発散成分を求める。最も単純な求め方は、歩容生成部において内部的に、上記した定常旋回歩容の歩容パラメータから脚運動を生成し、さらにＺＭＰｆｅｅｔ［ｉ］（ｉ＝０，１，…，ｋ）を、数式１，２から求め、これを、数式２６の右辺第３項に代入する手法である。
演算量は多いが、ロボット１に搭載可能な高速のコンピュータを用いれば制御周期内に演算を完了することができる。また、コンピュータの性能を同一とするときも、演算に際してサンプリング周期を実際の制御周期よりも長く設定すれば、若干の誤差は発生するが、演算量を減少させることもできる。
上記の如くして求められた値をＷｆｅｅｔとする。（ＸＹ方向があるので、Ｗｆｅｅｔは２次元ベクトルになる。）
以上より、次の数式２７を得る。
2002040224.tif 000028
さて、定常発散成分の定義から、定常旋回歩容の次の歩容の支持脚座標系から見た定常旋回終端発散成分をｑ’［ｋ］とすると、次の数式２８を満足しなければならない。
2002040224.tif 000029
一方、定常旋回歩容の支持脚座標系（第１３図に示すＸ’，Ｙ’座標系）から見た定常旋回歩容の次の歩容の支持脚座標系（同図に示すＸ”，Ｙ”座標系）を、座標系の回転を表わす回転行列Ｍと座標系の原点座標を表わす平行移動ベクトルＢで表わすと、以下の数２９に示すようになる。
2002040224.tif 000030
数式２８と数式２９より、次の数式３０を得る。
2002040224.tif 000031
さらに、数式３０と数式２７より、次の数式３１を得る。
2002040224.tif 000032
マトリックスＡの逆行列をｉｎｖ（Ａ）と記述すると、数式３１から数式３２が得られる。ただし、Ｉは単位行列である。
2002040224.tif 000033
上記した算出原理を前提とし、第１５図フロー・チャートを参照して説明すると、Ｓ１００において目標ＺＭＰによって発生する終端発散成分を上記手法で求め、次いでＳ１０２に進み、数式２６の右辺第３項を用いて脚反力によって発生する終端発散成分を求め、次いでＳ１０４に進み、数式３２を用いて定常旋回初期発散成分ｑ［０］を求める。定常旋回初期発散成分ｑ［０］は、定常旋回歩容の支持脚座標系から見た値になっている。
次いで、Ｓ１０６に進み、定常旋回初期発散成分ｑ［０］を今回歩容の支持脚座標系から見た値に変換する。これをｑ”とする。
次の課題は、今回歩容の終端発散成分がｑ”に一致するように、上記仮決定した今回歩容のパラメータを修正することである。この処理においては、上記定常旋回歩容における処理とほとんど同じであるので、以下では、変数、値、および記号を、改めて今回歩容に対応させる。
この実施の形態では、ＺＭＰ軌道パラメータを修正することとする。その他の歩容パラメータを修正しても良いが、通常、パラメータの修正量と終端発散成分の関係が非線形になるので、解析的に修正量を決定することはできない。
前記した仮決定された目標ＺＭＰ（これを「仮目標ＺＭＰ」と呼ぶ）に、ＺＭＰ修正量（補正量あるいは付加量）を加えることにより、ＺＭＰを修正する。修正されたＺＭＰを改めて目標ＺＭＰという。これらの関係を第１６図に示す。
同図に示す如く、目標ＺＭＰ（ＺＭＰｔｏｔａｌ）＝仮目標ＺＭＰ＋ＺＭＰ修正量、となるように、目標ＺＭＰパラメータを修正する。
以降は、説明を簡単にするために、すべての変数を１次元の世界で扱う。
ＺＭＰ修正量は第１６図中段に示すように台形状であり、その高さをａとする。目標ＺＭＰパターンによって発生する終端発散成分Ｗｚｍｐｔｏｔａｌは、目標ＺＭＰと終端発散成分の関係が線形であることから、以下の数式３３に示すように表すことができる。
2002040224.tif 000034
ここで、Ｗｚｍｐｔｍｐは、今回歩容の仮目標ＺＭＰパターンによって発生する終端発散成分である。Ｗｔｒｉｍは、ａ＝１とした場合のＺＭＰ修正量によって発生する終端発散成分である。これらは、前記定常旋回歩容の目標ＺＭＰパターンによって発生する終端発散成分を求める方法によって、解析的に求められる。
終端発散成分ｐ［ｋ］がｐ”であること、および、数式２７と数式３３より、次の数式３４を得る。
2002040224.tif 000035
ただし、ｑ［０］は、今回歩容の初期発散成分（初期倒立振子位置＋（初期倒立振子速度／ω０））である。したがって、次の数式３５に示すようになる。
2002040224.tif 000036
最後に、ＺＭＰ修正量を仮目標ＺＭＰに加算したものが目標ＺＭＰとなるように、目標ＺＭＰのパラメータを修正すれば、今回歩容の終端発散成分が、定常旋回歩容の初期発散成分を今回歩容から見た値ｑ”に一致する。
第１２図フロー・チャートの説明に戻ると、次いでＳ３０に進み、今回歩容を修正する。具体的には、今回歩容の終端発散成分が定常旋回歩容の初期発散成分ｑ”に一致するように、今回歩容のパラメータを修正する。
第１７図はその処理を示すサブルーチン・フロー・チャートである。
以下説明すると、Ｓ２００において今回歩容の仮目標ＺＭＰパターンによって発生する終端発散成分Ｗｚｍｐｔｍｐを求め、次いでＳ２０２に進み、ａ＝１とした場合のＺＭＰ修正量によって発生する終端発散成分Ｗｔｒｉｍを求める。
次いでＳ２０４に進み、今回歩容の脚運動によって発生する終端発散成分たるＷｆｅｅｔを求め、次いでＳ２０６に進み、今回歩容の初期発散成分ｑ［０］を初期倒立振子位置と速度から求め、求めた値を数式３５に代入してＺＭＰ補正量の台形高さａを求める。次いでＳ２０８に進み、前記したように目標ＺＭＰのパラメータを修正する。
以上が、歩容の切り変わり目の処理である。
第１２図フロー・チャートの説明に戻ると、次いでＳ２０に進み、決定された歩容パラメータから今回歩容の瞬時値を決定する。
第１８図はその処理を示すサブルーチン・フロー・チャートである。
以下説明すると、Ｓ３００において、今回歩容パラメータを基に、時刻ｔにおける目標ＺＭＰを求め、Ｓ３０２に進み、今回歩容パラメータを基に、時刻ｔにおける目標両足平位置姿勢を求める。
次いでＳ３０４に進み、今回歩容パラメータを基に、時刻ｔにおける目標上体姿勢を求め、Ｓ３０６に進み、数式１および数式２を用いて、時刻ｔおよびそれ以前の目標足平位置姿勢から、時刻ｔにおける脚ＺＭＰ（ＺＭＰｆｅｅｔ）を算出する。
次いでＳ３０８に進み、数式３を用いて、倒立振子ＺＭＰ（ＺＭＰｐｅｎｄ）を算出し、Ｓ３１０に進み、数式４を用いて、倒立振子ＺＭＰから倒立振子水平位置を算出する。次いでＳ３１２に進み、倒立振子水平位置から上体の水平位置を決定する。具体的には、前述したように、上体代表点の水平位置が、倒立振子質点水平位置に一致するように決定する。
次いでＳ３１４に進み、本出願人が先に特開平１０−８６０８０号公報で提案した上体高さ決定手法を用いて上体高さを決定する。
第１２図フロー・チャートの説明に戻ると、次いでＳ３２に進み、時刻ｔにΔｔを加え、再びＳ１４に戻り、上記の処理を繰り返す。
第４図を参照してこの実施の形態に係る歩容生成装置の動作をさらに説明すると、歩容生成部１００において、上記したように目標歩容が生成される。生成された目標歩容の歩容パラメータの中、目標上体位置姿勢（軌道）は、ロボット幾何学モデル（逆キネマティクス演算部）１０２に直接送られる。
また、その他の目標足平（足部）位置姿勢（軌道）、目標全床反力中心点（即ち、目標ＺＭＰ）軌道、および目標全床反力（軌道）は、複合コンプライアンス動作決定部１０４に直接送られる一方、目標床反力分配器１０６にも送られ、そこで床反力は各足平（足部２２Ｒ，Ｌ）に分配され、目標各足平床反力中心点および目標各足平床反力が決定されて複合コンプライアンス動作決定部１０４に送られる。
複合コンプライアンス動作決定部１０４から、機構変形補償付き修正目標足平位置姿勢（軌道）がロボット幾何学モデル１０２に送られる。ロボット幾何学モデル１０２は、目標上体位置姿勢（軌道）と機構変形補償付き修正目標足平位置姿勢（軌道）を入力されると、それらを満足する１２個の関節（１０Ｒ（Ｌ）など）の関節変位指令（値）を算出して変位コントローラ１０８に送る。変位コントローラ１０８は、ロボット幾何学モデル１０２で算出された関節変位指令（値）を目標値としてロボット１の１２個の関節の変位を追従制御する。
よってロボット１に生じた床反力は実各足床反力平検出器１１０（６軸力センサ３４）によって検出される。検出値は前記した複合コンプライアンス動作決定部１０４に送られる。また、ロボット１に生じた姿勢傾斜偏差θｅｒｒｘ，θｅｒｒｙは傾斜センサ３６によって検出され、検出値は姿勢安定化制御演算部１１２に送られ、そこで姿勢傾斜を復元する目標全床反力中心点まわり補償全床反力モーメントが算出されて複合コンプライアンス動作決定部１０４に送られる。複合コンプライアンス動作決定部１０４は、入力値に基づいて目標値を修正する。
尚、この発明の要旨は歩容生成部１００におけるロボットの歩容生成にあり、上記した複合コンプライアンス動作決定部１０４などの構成および動作は、本出願人が先に出願した特開平１０−２７７９６９号公報などに詳細に記載されているので、説明を以上に止める。
この実施の形態に係る脚式移動ロボットの歩容生成装置は上記の如く構成したので、本出願人が先に特開平１０−８６０８１号において提案した技術の上記した不都合を解消し、脚式移動ロボットの床反力を含む歩容を、動力学的平衡条件をほぼ満足しながら、自在かつリアルタイムに生成して任意の歩幅、旋回角、歩行周期などの歩容を生成することができると共に、生成された歩容同士の境界においてロボットの各部位の変位および速度が連続な歩容を生成することができる。
即ち、第１９図は上体代表点のＹ軸方向（左右方向）の軌道を示すタイム・チャートであるが、この実施の形態に係る脚式移動ロボットの歩容生成装置にあっては、同図に符合Ｂで示すように、上体代表点の軌道が定常旋回歩容の上体代表点の軌道Ａに漸近（収束）するような歩容を生成することができる。
さらに、脚式移動ロボットの動力学モデルを単純化して線形度を高めたので、発散などのロボットの将来の挙動を予測して歩容を生成することができる。
また、それによって歩行以外の動作もリアルタイムに生成できるようになるので、歩行以外の任意の動作と歩行を連続的に滑らかにつなげることも可能となる。
尚、上記で留意されるべきことは、上記した実施の形態において、第５図に示す３質点モデルは、必須ではないことである。即ち、第５図に示す３質点モデルに代え、例えば特公平４−１５０６８号公報で提案される１質点モデルを使用しても良い。その場合は、上記した実施の形態において脚質量を０にしたことと等価となる。近似精度は低下するが、脚運動によって発生する発散成分が０となることから、演算量を低減することができる。
第２０図は、この発明の第２の実施の形態に係る脚式移動ロボットの歩容生成装置の動作の一部を示す、第１の実施の形態の第１５図に類似する、定常旋回歩容の初期発散成分の算出作業のサブルーチン・フロー・チャートである。
上で３質点モデルに代えて１質点モデルを使用することも可能と述べたが、第２の実施の形態にあっては、搭載するＣＰＵ（第１の演算装置６０）の能力が格段に高いことを前提とし、３質点モデルに代え、第２３図に示したような全リンクに質点を設定した、換言すれば非線形なモデルを用いて歩容を生成するようにした。
第２の実施の形態に係る装置の動作を説明すると、第１の実施の形態の第１２図においてＳ１０からＳ２６までに示すのと同様の処理を行ってＳ２８に進んだ後、そのサブルーチン・フロー・チャートである第２０図に進み、定常旋回歩容の初期発散成分を算出する。
第２０図フロー・チャートにおいては、先ずＳ４００において初期上体位置速度候補（Ｘ０，Ｖ０）を仮決めする。上体位置速度は、上体代表点の位置と速度を意味する。尚、説明を簡略にするため、Ｘ方向だけを探索する場合を例にとるが、実際には位置速度ともＸ，Ｙ方向を別々にあるいは同時に探索する必要がある。Ｘ，Ｙ方向を同時に探索するときは、探索空間がＸ位置、Ｙ位置、Ｘ速度、Ｙ速度の４次元になる。
次いでＳ４０２を経てＳ４０４に進み、動力学モデル（第２３図に示したような全リンクに質点を設定した非線形なモデル）を用いて（Ｘ０，Ｙ０）を上体の初期状態として定常旋回歩容パラメータを基に、時刻０から終端時刻までの歩容を生成する。
具体的には、目標ＺＭＰと目標足平位置姿勢と目標上体姿勢を求め、（Ｘ０，Ｖ０）を上体の初期位置速度とし、動力学モデルを用いて目標ＺＭＰを満足する上体軌道を生成する。第１の実施の形態と同様に、軌道生成は歩容生成部１００の内部で行われるだけであって、出力されることはない。
次いでＳ４０６に進み、生成した歩容の終端上体位置速度を次ぎの１歩の支持脚座標系（第１４図のＸ’”，Ｙ’”座標系と同様）から見た値に変換し、その値を（Ｘｅ，Ｖｅ）とする。
次いでＳ４０８に進み、図示の如く、（Ｘ０，Ｖ０）と（Ｘｅ，Ｖｅ）の差（ｅｒｒｘ，ｅｒｒｙ）を算出する。即ち、定常旋回歩容では（Ｘ０，Ｖ０）と（Ｘｅ，Ｖｅ）が一致していなければならないことから、両者の差（ｅｒｒｘ，ｅｒｒｙ）が０となるように探索することを考える。
次いでＳ４１０に進み、算出した差が、適宜設定する許容範囲内にあるか否か判断する。尚、このように位置と速度の境界条件ずれの許容値を設定する代わりに、初期発散成分（Ｘ０＋Ｖ０／ω０）と初期収束成分（Ｘ０−Ｖ０／ω０）が、それぞれある許容範囲内にあるか否か判断するようにしても良い。
Ｓ４１０で否定されるときはＳ４１２に進み、（Ｘ０，Ｖ０）の付近に複数の初期値候補（Ｘ０＋ΔＸ０，Ｖ０），（Ｘ０，Ｖ０＋ΔＶ０）を決め、それぞれを上体の初期状態として定常旋回歩容の歩容パラメータを基に歩容を生成し、生成した歩容の終端上体位置速度を次ぎの１歩の支持脚座標系から見た値に変換した値（Ｘｅ＋ΔＸｅ１，Ｖｅ＋ΔＶｅ１），（Ｘｅ＋ΔＸｅ２，Ｖｅ＋ΔＶｅ２）を求める。
次いでＳ４１４に進み、（Ｘ０，Ｖ０）およびその付近の初期値候補に対する終端上体位置速度と前記した差を基に、次ぎの初期値候補（Ｘ０，Ｖ０）を決定する。
Ｓ４１０で否定される限りは上記した処理を繰り返すと共に、肯定されるときは繰り返しループ（Ｓ４０２）を抜けてＳ４１６に進み、定常旋回初期発散成分ｑ〔０］を図示の式から求める。次いでＳ４１８に進み、第１の実施の形態の第１５図のＳ１０６と同様な処理を行う。
尚、第１の実施の形態で用いたような線形な３質点モデルを用いていないが、発散成分および収束成分の概念は、第２３図に示すような非線形なモデルの挙動の摂動分に対しても近似的に十分な精度で適用することができるので、第１の実施の形態と同様な式を用いて発散成分を定義することとする。
次いで第１２図と同様のフロー・チャートにおいてＳ３０に進み、今回歩容の歩容パラメータを修正する。
第２１図はその処理を示すサブルーチン・フロー・チャートである。
以下説明すると、Ｓ５００を経てＳ５０２に進み、仮目標ＺＭＰパターンとその他の今回歩容の歩容パラメータを基に、終端時刻までの今回歩容を計算することによって今回歩容終端での上体位置速度（Ｘｅ，Ｖｅ）を求め、さらに終端発散成分ｑ［０］を図示の式を用いて求める。
次いでＳ５０４に進み、終端発散成分誤差ｅｒｒｑを図示の式を用いて求め、Ｓ５０６に進んで求めた誤差が許容範囲内にあるか否か判断し、否定されるときはＳ５０８に進み、ａ＝Δａとし、第１の実施の形態と同様に第１６図の関係に従って仮目標ＺＭＰパターンに修正を加えた目標ＺＭＰを基に、終端時刻までの今回歩容を計算することによって、今回歩容終端での上体位置速度（Ｘｅ，Ｖｅ）を求め、さらには終端発散成分ｑ１［ｋ］を図示の式を用いて求める。
尚、Ｓ５０８においてΔａは適宜に設定される微小量の定数である。繰り返し演算によって誤差ｅｒｒｑが小さくなるにつれてΔａも減少するように設定しても良い。ただし、定数としても、数回の繰り返し演算で誤差は許容範囲内になる。
次いでＳ５１０に進み、パラメータ感度ｒを図示の式から求め、Ｓ５１２に進み、図示のように求めた修正量を仮目標ＺＭＰパターンに加え、仮目標ＺＭＰパターンを修正する。
Ｓ５０６で否定される限りは上記した処理を繰り返すと共に、肯定されるときは繰り返しループ（Ｓ５００）を抜けてＳ５１４に進み、最終的に得られた仮目標ＺＭＰパターンの値を目標ＺＭＰと決定する。
上記を説明すると、動力学モデルを用いて今回歩容パラメータに従って歩容を生成し、そのまま連続して定常旋回歩容パラメータに従って歩容を繰り返し生成したとき、生成した歩容が定常旋回歩容に収束するためには、第１の実施の形態と同様に、今回歩容の終端発散成分が、定常旋回初期発散成分ｑ［０］を今回歩容の支持脚座標系から見た値であるｑ”に、完全にあるいはほぼ一致していなければならない。
そこで、第２の実施の形態においても、その条件を満足するように、今回歩容の歩容パラメータの中の目標ＺＭＰパターンを修正するようにした。
尚、第２の実施の形態にあっても、定常旋回歩容との境界における上体位置速度が一致するように今回歩容パラメータを修正しても良い。ただし、発散成分に関する境界条件だけを満足する場合に比較すると、条件が厳しくなり、修正すべきパラメータを２倍に増やすこととなって探索空間が増大する。
また、上記したように、目標ＺＭＰを修正して上体位置速度を一致させる場合には、ＺＭＰ修正量の波形も、第１６図に示すような単純な台形形状に止まらず、２段式の台形形状のような複雑な形状としなければならず、修正量も大きくなりがちである。
第１９図を再び参照して説明すると、前述したように、同図に符合Ａで示す軌道は、定常旋回歩容との境界における上体代表点の位置速度が一致するように今回歩容を生成し、次いで定常旋回歩容を生成した場合の上体代表点軌道を指す。符号Ｂで示す軌道は、今回歩容の終端発散成分がｑ”に一致するように今回歩容を生成し、次いで定常旋回歩容パラメータを用いて連続する歩容を生成し続けた場合の上体代表点軌道を示す。
図示の如く、符合Ｂで示す軌道は、今回歩容と最初の定常旋回歩容との境界においては、通常、符号Ａで示す軌道からずれているが、その後、徐々に符号Ａで示す軌道に収束（漸近）し、次ぎの定常旋回歩容時期で符号Ａで示す軌道にほぼ一致する。このように、発散成分のみを一致させる歩容生成手法でも、位置速度までも一致させる歩容生成手法と同様、歩容の発散を防止する効果を得ることができる。
同図に符合Ｃで示す個は、それらを考慮せずに軌道を生成した場合を示す。このような場合は、生成した軌道は経時的に符合Ａで示す軌道から発散してしまう。尚、位置速度までも一致させる歩容生成手法は、換言すれば、発散成分のみならず、収束成分も一致させる歩容生成手法である。したがって、位置速度までも一致させる歩容生成手法は、発散成分のみを一致させる歩容生成手法の特殊例であると言うことができる。
尚、目標ＺＭＰ以外のパラメータを修正しても良い（これは第１の実施の形態でも同様である）。また、説明を簡略化するために、Ｘ方向だけの１次元探索としたが、実際には、定常旋回歩容の上体位置速度の初期値探索と同様に次元が増加する。
尚、第２の実施の形態においては、疑似的なニュートン法（あるいは（Ｘ０，Ｖ０）での疑似的なヤコビアン（感度マトリックス）を求めて次ぎの候補を決定する手法）を用いるようにしたが、シンプレックス法などの探索法を用いても良い。さらには、予め種々のパラメータに対して探索計算を行ってマップ化しておいても良い。
さらに、第２の実施の形態で述べた非線形モデルを用いて探索的に歩容を求める手法は、第１の実施の形態の解析的に歩容を求めるのに使用した３質点モデル（あるいは１質点モデル）などの線形モデルを用いても良いことは言うまでもない。ただし、解析的に歩容を求めることができるので、非効率な探索法を用いることの意義は少ない。その他、実質的に十分な精度があるならば、どのような動力学モデルを用いても良い。
第１の実施の形態あるいは第２の実施の形態において、想定した定常旋回歩容通りに歩容を生成し続けているときには、ＺＭＰ修正量が０となり、今回歩容として決定される目標ＺＭＰは、想定した定常旋回歩容のＺＭＰ（仮目標ＺＭＰ）と一致する。ところで、定常旋回歩容のＺＭＰ（仮目標ＺＭＰ）は通常、安定余裕が高くなるように設定される。したがって、想定した定常旋回歩容通りに歩容を生成し続けている場合には、今回歩容の安定余裕も高い。
それに対し、今回歩容を１歩前に想定した定常旋回歩容と大きく異なるように生成しようとすると、ＺＭＰ修正量の絶対値が大きくなるので、今回歩容として決定される目標ＺＭＰ軌道は理想的な軌道から大きくずれ、安定余裕が低くなる。例えば、右に旋回しようとするときは右に重心をずらすように歩容が生成されるが、その次ぎの１歩で急に左旋回するような要求が生じた場合、重心を右から左に移動させるために目標ＺＭＰ軌道を大きく右にずらす必要が生じ、安定余裕が低下して右に倒れやすくなる。
以上の性質から、想定した定常旋回歩容は、将来実際に生成されるであろう歩容になるべく近くなるように設定すべきである。
定常旋回歩容で歩き続けることは特殊なことであり、一般的には、定常旋回歩容からずれた歩容が将来生成されるが、上記における定常旋回歩容設定手法は、将来実際に生成されるであろう歩容になるべく近くなるように設定する例である。
上記以外の定常旋回歩容の設定手法を、以下の１）から４）までに示す。４）を除くと、将来実際に生成されるであろう歩容と定常旋回歩容のずれは、上記で得るよりも大きくなる傾向があるが、演算が容易になる。
１）次次回支持脚座標系と次次回歩行周期は、今回支持脚座標系、今回歩行周期およびそれまでの歩行履歴を基に、生成したい歩容の移動速度や旋回速度が急激に変化しないであろうということを前提にして予想する、あるいは外挿的に求めしても良い。
２）定常旋回歩容を常に直線歩容にしても良い。ただし、旋回速度が大きくなるにつれ、ＺＭＰ修正量が増加する。即ち、安定余裕が低下する。
３）定常旋回歩容を常に静止歩容あるいはその場足踏み歩容にしても良い。ただし、歩行速度が上がるにつれ、ＺＭＰ修正量が増加する。尚、「静止歩容」とは、動きのない静止状態の歩容を意味する。
４）今回歩容と定常旋回歩容の間に過渡期歩容を追加しても良い。挙動予測演算が複雑になるが、定常旋回歩容を決定するときの制約条件（境界条件）が緩くなり、より望みに近い定常旋回歩容が設定できるので、より一層安定余裕を高めることができる。
さらには、３）と４）を組み合わせることも可能であり、次いで述べるこの発明の第３の実施の形態に係る脚式移動ロボットの歩容生成装置は、その点に焦点をおいたものである。
第２２図は、その第３の実施の形態に係る装置の動作の一部を示す、第１の実施の形態の第１３図に類似する、定常旋回歩容の別の設定例を示す説明図である。
即ち、第２２図に示す如く、定常旋回歩容が静止歩容を含むように構成すると共に、今回歩容と静止歩容（定常旋回歩容）の間に過渡的な歩容（過渡歩容。定常旋回歩容以外の歩容を意味する）を挿入するようにした。
このように、今回歩容、過渡歩容、静止歩容からなる３つの歩容を設定することによって、姿勢の発散を効果的に防止することができる。また、いつでも静止状態に移行できるように歩容が生成されるので、急に停止要求が与えられても、安定余裕の高い停止動作が可能となる。
具体的には、静止歩容も定常旋回歩容の１つと考え、前述の定常旋回歩容の初期発散成分算出アルゴリズムを使用することとする。ただし、静止時の上体質点速度が０であることから、発散成分は倒立振子質点位置に一致するので、わざわざこのアルゴリズムを用いなくても容易に求めることができる。
第３の実施の形態は上記の如く構成したので、第１の実施の形態および第２の実施の形態の場合と同様に、安定余裕の高い歩容を自在かつリアルタイムに生成することができる。
第１の実施の形態から第３の実施の形態は上記の如く、少なくとも上体２４と、前記上体に連結される複数本、より具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボット１の歩容生成装置（歩容生成部１００）において、少なくとも前記上体と脚部の運動と、床反力、より具体的にはＺＭＰとの関係を表す動力学モデル（第５図および第６図に示す）、少なくとも要求（次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期、次回歩容周期など。Ｓ２２）に応じ、前記脚部の軌道（目標足平位置姿勢軌道）と前記床反力の軌道（目標全床反力中心点軌道（目標ＺＭＰ軌道））を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段（Ｓ２４）、少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容（定常旋回歩容）の前記パラメータを想定（決定）する周期的歩容パラメータ想定手段（Ｓ２６）、前記動力学モデルおよび前記今回歩容のパラメータとそれに続く前記周期的歩容のパラメータに基づいて決定される前記上体の軌道（目標上体位置姿勢軌道）が、前記周期的歩容のパラメータから決定される前記周期的歩容の前記上体の軌道に実質的に収束あるいは一致するように、少なくとも前記仮決定された今回歩容のパラメータを修正する今回歩容パラメータ修正手段（Ｓ３０，Ｓ２００からＳ２０８）、および少なくとも前記修正された前記今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段（Ｓ２０，Ｓ３００からＳ３１４）を備えるように構成した。
また、少なくとも上体２４と、前記上体に連結される複数本、具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボット１の歩容生成装置（歩容生成部１００）において、少なくとも前記上体と脚部の運動と、床反力（具体的にはＺＭＰ）との関係を表す動力学モデル（第５図および第６図に示す）、少なくとも要求（Ｓ２２）に応じ、前記脚部の軌道（目標足平位置姿勢軌道）と前記床反力の軌道（目標全床反力中心点軌道（目標ＺＭＰ軌道））を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段（Ｓ２４）、少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容（定常旋回歩容）の前記パラメータを想定（決定）する周期的歩容パラメータ想定手段（Ｓ２６）、前記周期的歩容の前記パラメータに基づき、前記周期的歩容の前記上体の挙動を前記動力学モデルで近似したときの発散成分の歩容境界での値である、周期的歩容境界発散成分（定常旋回初期発散成分）を決定する周期的歩容境界発散成分決定手段（Ｓ２８，Ｓ１００からＳ１０６）、前記今回歩容のパラメータに基づき、前記動力学モデルを用いて生成される今回歩容の終端における少なくとも前記発散成分（終端発散成分）が完全に一致あるいは実質的に一致するように、前記今回歩容のパラメータ、より具体的には目標ＺＭＰのパラメータを修正する今回歩容パラメータ修正手段（Ｓ３０，Ｓ２００からＳ２０８）、および少なくとも前記修正された前記今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段（Ｓ２０，Ｓ３００からＳ３１４）を備える如く構成した。
また、前記今回歩容と前記周期的歩容の間に過渡的な歩容（過渡歩容）が挿入される如く構成した。
また、前記周期的歩容が静止歩容を含む如く構成した。
また、少なくとも上体２４と、前記上体に連結される複数本、より具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボット１の歩容生成装置（歩容生成部１００）において、前記脚部の運動によって発生する慣性力と重力の合力（総慣性力）である脚部反力、より具体的には脚ＺＭＰ（ＺＭＰｆｅｅｔ）を前記上体の挙動に依存せずに算出する脚部反力算出手段（脚ＺＭＰ算出器２００，Ｓ２０，Ｓ３０６）、少なくとも前記算出された脚部反力と目標床反力、より具体的には目標ＺＭＰとから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置（ＺＭＰｐｅｎｄ）を算出する倒立振子モデル支点位置算出手段（Ｓ２０，Ｓ３０８）、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位、より具体的には前記倒立振子質点位置（倒立振子水平位置ｘｂ）を算出する倒立振子モデル変位算出手段（Ｓ２０，Ｓ３１０）、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置、より具体的には前記上体の水平位置を算出する上体位置算出手段（上体位置決定器２０２，Ｓ２０，Ｓ３１２，Ｓ３１４）、および少なくとも前記算出された上体位置を用いて歩容、より具体的には今回歩容の瞬時値を生成（算出）する歩容生成手段（Ｓ２０，Ｓ３００からＳ３１４、歩容生成部１００）を備えるように構成した。
また、少なくとも上体２４と、前記上体に連結される複数本、より具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボットの歩容生成装置（歩容生成部１００）において、前記脚部の運動によって発生する慣性力と重力の合力に疑似的に対応する前記脚部のＺＭＰに相当する脚部ＺＭＰ（ＺＭＰｐｅｎｄ）を前記上体の挙動に依存せずに算出する脚部ＺＭＰ算出手段（脚ＺＭＰ算出器２００，Ｓ２０，Ｓ３０６）、少なくとも前記算出された脚部ＺＭＰと目標ＺＭＰとから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置（ＺＭＰｐｅｎｄ）を算出する倒立振子モデル支点位置算出手段（Ｓ２０，Ｓ３０８）、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位、より具体的には前記倒立振子質点位置（倒立振子水平位置ｘｂ）を算出する倒立振子モデル変位算出手段（Ｓ２０，Ｓ３１０）、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置を算出する上体位置算出手段（Ｓ２０，Ｓ３１２，Ｓ３１４。上体位置決定器２０２）、および少なくとも前記算出された上体位置を用いて歩容、より具体的には今回歩容の瞬時値を生成（算出）する歩容生成手段（Ｓ２０，Ｓ３００からＳ３１４、歩容生成部１００）を備える如く構成した。
また、前記倒立振子モデル支点位置算出手段は、前記目標ＺＭＰに第１の係数ｍｔｏｔａｌ／ｍｂを乗じて得た積から、前記脚部ＺＭＰに第２の係数ｍｆｅｅｔ／ｍｔｏａｌを乗じて得た積を減算して前記倒立振子モデルの支点位置（倒立振子水平位置ｘｂ）を算出する如く構成した。
また、少なくとも上体２４と、前記上体に連結される複数本、より具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボット１の歩容生成装置（歩容生成部１００）において、前記脚部の運動によって発生する慣性力と重力の合力である脚部反力（脚ＺＭＰ（ＺＭＰｆｅｅｔ））を前記上体の挙動に依存せずに算出する脚部反力算出手段（脚ＺＭＰ算出器２００，Ｓ２０，Ｓ３０６）と、少なくとも前記算出された脚部反力と目標床反力（目標ＺＭＰ）とから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置（倒立振子ＺＭＰ（ＺＭＰｐｅｎｄ））を算出する倒立振子モデル支点位置算出手段（Ｓ２０，Ｓ３０８）と、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位、より具体的には位置（倒立振子水平位置）を算出する倒立振子モデル変位算出手段（Ｓ２０，Ｓ３１０）と、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置（上体の水平位置）を算出する上体位置算出手段（上体位置決定器２０２，Ｓ２０，Ｓ３１２，Ｓ３１４）とから構成される動力学モデル、少なくとも要求（Ｓ２２）に応じ、前記脚部の軌道（目標足平位置姿勢軌道）と前記床反力の軌道（目標全床反力中心点軌道（目標ＺＭＰ軌道））を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段（Ｓ２４）、少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容（定常旋回歩容）の前記パラメータを想定する周期的歩容パラメータ想定手段（Ｓ２６）、前記周期的歩容の前記パラメータに基づき、前記周期的歩容の境界における前記倒立振子モデルの発散成分である周期的歩容境界発散成分（定常旋回歩容の初期発散成分）を決定する周期的歩容境界発散成分決定手段（Ｓ２８，Ｓ１００からＳ１０６）、前記今回歩容のパラメータに基づき、前記動力学モデルを用いて生成される今回歩容の終端における前記倒立振子モデルの発散成分が前記周期的歩容境界発散成分（今回歩容の終端発散成分）に完全に一致または実質的に一致するように、前記今回歩容のパラメータを修正する今回歩容パラメータ修正手段（Ｓ３０，Ｓ２００からＳ２０８）、および少なくとも前記修正された今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段（Ｓ２０，Ｓ３００からＳ３１４）を備える如く構成した。
また、前記床反力が少なくともＺＭＰを含む如く構成した。前記（第５図および第６図に示す）倒立振子モデルが線形モデルである如く構成した。
次いで、脚ＺＭＰ（ＺＭＰｆｅｅｔ）の算出について付言する。
脚ＺＭＰ（ＺＭＰｆｅｅｔ）を求める関数は、以下の条件を満たすことが望ましい（第１の実施の形態では、これら全ての条件を満足している）。
条件ａ）足平軌道を決定する歩容パラメータの全てまたは一部を基に決定されること。
条件ｂ）上体代表点の運動の影響を受けないこと。
条件ｃ）単純化モデルの近似精度が十分に高いこと。
条件ｄ）可能な限り連続であること。（上体加速度を連続にするため。）
これらの内、条件ａ，ｂは予測演算を容易にするための条件である。上記では、数式１と数式２から脚ＺＭＰを求めているが、数式１と続いて述べる数式３６および数式３７を用いても良い。
2002040224.tif 000037
ただし、Ｃは定数であり、また、両脚重心加速度ベクトルａｆｅｅｔが数式３７によって求められ、数式３６の両脚重心鉛直方向加速度ベクトルａｆｅｅｔｚは、ａｆｅｅｔのＸ，Ｙ成分を０にしたベクトルである。
2002040224.tif 000038
数式３６は、数式２よりも調整自由度が高いので、近似精度を高くすることができる。また、点Ｐまわりの脚総慣性力モーメントを求める数式１において、脚加速度に関する項を故意に定数倍するなど、力学的には正しくない式に変更しても、最終的に単純化モデルの近似精度が高くなるのであれば、差し支えない。
続いて、作用点Ｐの設定に関して付言する。
作用点Ｐは次のように設定することも可能である。
１）今回歩容の支持脚座標系原点あるいはその近傍の所定の点に作用点Ｐを設定する。
２）両脚支持期の間に直前歩容の支持脚座標系原点あるいは近傍の所定の点から今回歩容の支持脚座標系原点あるいは近傍の所定の点に、連続的に移動するように作用点Ｐを設定する。
３）作用点Ｐを仮目標ＺＭＰ軌道に一致させる。
以下、それぞれの特徴を解説する。
１）については、作用点Ｐが脚位置に対して相対的に不連続（即ち、床に設定された絶対座標系から見て不連続）になるので、ＺＭＰｆｅｅｔも脚位置に対して相対的に不連続（即ち、前記した絶対座標系から見て不連続）になる欠点がある。しかしながら、両足平の加速度があまり大きくない両脚支持期に座標系を切り替える限り、不連続の大きさは、歩行上、あまり問題にならない。
２）については、近似精度は、１）よりも良い。また、作用点Ｐが脚位置に対して相対的に連続になるので、ＺＭＰｆｅｅｔも脚位置に対して相対的に連続になる。
３）については、２）の手法よりもさらに近似精度が高くなる。また２）と同様に、ＺＭＰｆｅｅｔも脚位置に対して相対的に連続になる。
続いて、脚総慣性力モーメントの算出について付言する。
各足平質点軌道は、足平運動パラメータから決定される軌道であるが、各足平質点軌道を求める際に用いる足平位置姿勢の軌道は、歩容生成部１００から出力される目標歩容の足平位置姿勢と一致していなくても良い。
たとえば、目標歩容の足平が回転する場合であっても、各足平質点軌道を求める際に用いる足平位置姿勢の軌道は、足平を水平姿勢を保ったまま（即ち、べた足）であっても良い。精度が著しく低下しない限り、持ち上げ量等、他のパラメータを変えた軌道にしても良い。
また、脚質点は次のように設定しても良い。
１）足平内の所定の点
２）上記点から鉛直方向に一定距離ずらした点
３）足平内に複数の質点を設定しても良い。また足平にイナーシャを設定しても良い。
上記いずれの手法も、前記条件ｂ）を満足している。
上体および脚の質点オフセット設定について付言すると、上体および脚の質点オフセット設定値は、固定値でもほとんど問題ないが、重心位置のモデル化誤差をできるかぎり小さくするために、歩容パラメータに応じて補正されるようにしても良い。また、前記したような倒立振子の水平位置を上体代表点水平位置に対応させる代わりに、ロボットの全重心位置あるいは一部の重心位置（脚部の全部あるいは一部の質量分布を除く、ロボットの質量分布の重心位置）に対応させても良い。
また、倒立振子の高さに関しては、歩容パラメータに応じて変更しても構わないが、将来挙動予測を容易にするために、短期的（例えば１歩の期間）には一定値にするのが望ましい。
さらに、脚反力によって発生する発散成分の算出について付言すると、脚反力によって発生する発散成分を、制御周期毎の脚軌道を生成せずに、歩容パラメータから近似的に直接求めるようにしても良い。
例えば、脚ＺＭＰが、一歩の間、歩容初期の両脚重心位置と歩容終端での両脚重心位置の平均値にあると近似すれば、この平均値にＨ（１，Ｔ）を乗じたものが、脚反力によって発生する発散成分を時刻０のインパルスに換算したものの近似値になる。
脚運動によって発生する終端発散成分は、歩容パラメータ（特に脚運動パラメータと時間パラメータ）を入力とする関数になっている。したがって、歩容パラメータと脚運動によって発生する終端発散成分の関係を表わすマップを作成しておけば、演算量を減らすこともできる。ただし、必要なメモリ容量が増加する。いずれの手法を用いても良い。
上記した変形例を組み合わせることも可能である。例えば、ＺＭＰ修正値を求めるための（将来挙動予測時の）脚ＺＭＰ算出手法と、今回制御周期の瞬時値を求めるときの脚ＺＭＰ算出法は厳密に一致していなくても良い。少し差があっても、ＺＭＰ修正値を求めるときに初期発散成分の影響が考慮されているので、前記差によって、発散成分がずれても、１歩後に再び歩容を設計するときに、発散しないように補正されるからである。
尚、上記した第１の実施の形態の第１２図フロー・チャートにおいてｔ＝０のときに歩容を修正または変更したが（Ｓ１０）、それ以外の時点で修正または変更しても良い。そのときは現在時刻を今回歩容の初期時刻とみなせば良い。即ち、今回歩容の期間が現在時刻から今回歩容終端時刻までであるとすれば良い。
尚、上記において、第６図に示したブロック線図は、演算処理順序を変えるなどの等価変形をしても良い。
また、この発明を２足歩行ロボットに関して説明してきたが、３足以上の多脚ロボットにも応用することができる。
産業上の利用可能性
この発明によれば、例えば、上体と脚部の運動と、床反力との関係を表す動力学モデルを備え、要求に応じ、脚部の軌道などを決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定し、今回歩容に続く周期的歩容のパラメータを想定し、動力学モデルおよび今回歩容のパラメータとそれに続く周期的歩容のパラメータに基づいて決定される前記上体の軌道が、周期的歩容のパラメータから決定される上体の軌道に実質的に収束あるいは一致するように、少なくとも今回歩容のパラメータを修正し、修正今回歩容のパラメータに基づいて今回歩容の瞬時値を決定する如く構成したので、脚式移動ロボットの床反力を含む歩容を、動力学的平衡条件をほぼ満足しながら、自在かつリアルタイムに生成して、任意の歩幅、旋回角、歩行周期などの歩容を生成できるようにした脚式移動ロボットの歩容生成装置を提供することができる。
さらに、生成された歩容同士の境界においてロボットの各部位の変位および速度が連続で、かつ安定余裕の高い歩容を生成することができると共に、ロボットの将来の挙動を予測して姿勢の発散などの不都合が生じないように歩容を生成することができる。さらに、高速演算を可能としてリアルタイムに歩容を生成することができる。
【図面の簡単な説明】
第１図は、この発明の一つの実施の形態に係る脚式移動ロボットの歩容生成装置を全体的に示す概略図である。
第２図は、第１図に示す脚式移動ロボットの足部の構造を示す説明断面図である。
第３図は、第１図に示す制御ユニットの詳細を示すブロック図である。
第４図は、第１図に示す脚式移動ロボットの歩容生成装置の構成を機能的に示すブロック図である。
第５図は、第４図に示す構成の中の歩容生成部で使用する、第１図に示す脚式移動ロボットを倒立振子で近似して得た動力学モデルを示す説明図である。
第６図は、第５図に示す動力学モデルを用いて第４図に示す歩容生成部が行う動力学演算を示すブロック図である。
第７図は、第４図に示す動力学モデルの倒立振子の支点位置を示すＺＭＰ相当値ＺＭＰｐｅｎｄを演算するのに用いる、脚部の質点の慣性力と重力の合力の作用点Ｐの軌跡を示すタイム・チャートである。
第８図は、第１図に示す脚式移動ロボットにおいて上体軌道が発散した場合を示す説明図である。
第９図は、第４図に示す動力学モデルの倒立振子の支点位置を示すＺＭＰ相当値ＺＭＰｐｅｎｄを離散関数化して示すタイム・チャートである。
第１０図は、同様に第４図に示す動力学モデルの倒立振子の支点位置を示すＺＭＰ相当値ＺＭＰｐｅｎｄの軌道を波形で示すタイム・チャートである。
第１１図は、第１０図に示すＺＭＰ相当値ＺＭＰｐｅｎｄの波形を分解して示すタイム・チャートである。
第１２図は、第１図に示す脚式移動ロボットの歩容生成装置の動作を示すフロー・チャートである。
第１３図は、第１２図フロー・チャートで使用する定常旋回歩容を着地位置などから説明する説明図である。
第１４図は、同様に、第１２図フロー・チャートで使用する定常旋回歩容の上体軌道を着地位置などから説明する説明図である。
第１５図は、第１２図フロー・チャートの定常旋回歩容の初期発散成分の算出作業を示すサブルーチン・フロー・チャートである。
第１６図は、第１５図フロー・チャートで算出される初期発散成分に基づいて第１２図フロー・チャートにおいて今回歩容の歩容パラメータである目標ＺＭＰを修正する動作を説明するタイム・チャートである。
第１７図は、第１２図フロー・チャートの初期発散成分に基づく今回歩容の歩容パラメータの目標ＺＭＰの修正動作を示すサブルーチン・フロー・チャートである。
第１８図は、第１２図フロー・チャートの今回歩容の瞬時値算出作業を示すサブルーチン・フロー・チャートである。
第１９図は、第１の実施の形態に係る脚式移動ロボットの歩容生成装置によって生成される歩容の中の上体軌道を示すタイム・チャートである。
第２０図は、この発明の第２の実施の形態に係る脚式移動ロボットの歩容生成装置の動作の中の、定常旋回歩容の初期発散成分の算出作業を示す、第１５図に類似するサブルーチン・フロー・チャートである。
第２１図は、第２の実施の形態に係る脚式移動ロボットの歩容生成装置の動作の中の、今回歩容の歩容パラメータの目標ＺＭＰの修正動作を示す、第１７図に類似するサブルーチン・フロー・チャートである。
第２２図は、この発明の第３の実施の形態に係る脚式移動ロボットの歩容生成装置で使用される定常旋回歩容の設定例を示す、第１３図と同様の説明図である。
第２３図は、脚式移動ロボットである２足歩行ロボットの全リンクに質点を設定した例を示す説明図である。
【図１】
2002040224.tif 000039
【図２】
2002040224.tif 000040
【図３】
2002040224.tif 000041
【図４】
2002040224.tif 000042
【図５】
2002040224.tif 000043
【図６】
2002040224.tif 000044
【図７】
2002040224.tif 000045
【図８】
2002040224.tif 000046
【図９】
2002040224.tif 000047
【図１０】
2002040224.tif 000048
【図１１】
2002040224.tif 000049
【図１２】
2002040224.tif 000050
【図１３】
2002040224.tif 000051
【図１４】
2002040224.tif 000052
【図１５】
2002040224.tif 000053
【図１６】
2002040224.tif 000054
【図１７】
2002040224.tif 000055
【図１８】
2002040224.tif 000056
【図１９】
2002040224.tif 000057
【図２０】
2002040224.tif 000058
【図２１】
2002040224.tif 000059
【図２２】
2002040224.tif 000060
【図２３】
2002040224.tif 000061
【国際調査報告】
2002040224.tif 000062
2002040224.tif 000063