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

Abstract

【課題】脚式移動ロボットの動力学的挙動を記述するモデルを単純化して線形度を高め、モデルの挙動を通じてロボットの挙動が発散するか否かなどの予測を、逐次、リアルタイムに演算することも可能とすることで、動力学的に一層安定した歩容を生成できるようにした脚式移動ロボットの歩容生成装置を提供する。
【解決手段】脚部の運動によって発生する慣性力と重力の合力（総慣性力）である脚部反力（ZMPfeet)を算出し、算出された脚部反力と目標床反力（目標ＺＭＰ）とから上体２４の動力学的挙動を記述する倒立振子モデルの支点位置（ZMPpend)を算出し、その支点位置に基づいて倒立振子モデルの変位（倒立振子水平位置xb) を算出し、その変位に基づいて上体の位置（水平位置）を算出し、算出された上***置を用いて歩容を生成する。
【選択図】図５

Description

この発明は脚式移動ロボットの歩容生成装置に関し、より詳しくは脚式移動ロボットにおいて、その歩容を自在かつリアルタイムに生成するようにしたものに関する。

本出願人は先に、以下に示す特許文献１において、オフラインで設計した基準歩容をパラメータと上体軌道の時系列の組にしてロボットに搭載したマイクロコンピュータのメモリに記憶させ、歩行周期など、時間に関するパラメータが同一である複数の歩容の瞬時値の重み付き平均を求めることによって、自在な歩容を生成する技術を提案している。
特開平１０−８６０８１号公報 特公平４−１５０６８号公報 『路面形状に偏差のある環境における２足歩行制御』（日本ロボット学会誌（ＪＲＳＪ），Ｖｏｌ．１３，Ｎｏ．７，Ｏｃｔ．，１９９５

しかしながら、特許文献１においては、重み付き平均の算出に際して歩行周期などの時間的パラメータが異なる複数の基準歩容を単に時間パラメータも含めて重み付き平均を取ると、動力学的平衡条件が、近似的にも成立しなくなっていた。したがって、この提案手法では、基準歩容と異なる周期の歩容を生成することはできなかった。

尚、動力学的平衡条件とは、目標歩容の重力と慣性力から計算されるＺＭＰが目標ＺＭＰに一致していること、より詳細には、ロボットの慣性力と重力の合力が目標ＺＭＰまわりに作用するモーメントの水平成分が０であることを意味する。また、ＺＭＰ（Zero Moment Point)は、運動による慣性力と重力の合力が床と交わる点を意味する。より厳密には、ロボットの運動による慣性力と重力の合力のモーメントの水平成分が０となる、床上の作用点を意味する。

また、上記した特許文献１にあっては、１歩の歩容と次の１歩の歩容の境界において、上体の位置と速度の両方を連続にするために、場合によっては目標ＺＭＰを理想の軌道から大きくずらす必要があったため、安定余裕が低下する場合もあった。

さらに、多様な歩行を実現するためには数多くの基準歩容の時系列データを記憶する必要があり、メモリ容量が増加する不都合があった。さらに基準歩容をオフラインで試行錯誤によって生成しなければならないと共に、基準歩容からかけ離れた歩容を生成しようとすると、近似度が大きく低下する不都合があった。

ところで、歩容は物理法則に逆らうことができないので、その許容範囲内でＺＭＰなどの歩容パラメータを生成する必要があり、しかも、２足歩行ロボットの動力学系は、ＺＭＰを入力、上***置を出力とする系とみなすと発散系になるので、生成した歩容パラメータによってロボットの挙動が発散しないように配慮する必要がある。尚、「発散」とは、後で触れる図８に示すように、２足歩行ロボットの上体の位置が両足部（足平）の位置からかけ離れた位置にずれてしまうことを意味する。

その点から、リアルタイムに歩容を生成する場合、生成した歩容パラメータによってロボットに生じるであろう将来の挙動を予測し、発散する可能性があるときは発散を防止することが望ましい。

しかしながら、図２３に示すように、ロボット（２足歩行ロボット）を多質点系でモデル化すると、演算量が多くかつ非線形性が強くなるため、歩容の終端状態をリアルタイムに求めるのは、ロボットに搭載可能な通常のＣＰＵの能力では困難である。

他方、将来挙動予測とそれに基づく発散防止制御は、多くの知識を蓄積しておき、歩容切り変わり目毎に、そのときの状態と目的に応じ、蓄積した知識に照らし合わせて目標歩容を選び出す手法でも、あるレベルまでは達成することができるであろうが、ありとあらゆる状況に対応しようとすれば組合わせ爆発を起こし、事実上は不可能である。

したがって、脚式移動ロボットにおいて、リアルタイムに解析的に将来挙動が予測計算できるように、ロボットの動力学を記述する動力学モデルを単純化することが望まれていた。

動力学モデルとしては、従来から代表的なものとして次の２つが知られている。
１）１質点モデル
２）影響度の小さい質点を無視した多質点モデル

１質点モデルは、例えば上記した特許文献２で提案されており、その提案技術にあっては、質点を上体だけに設定した１質点系としてロボットをモデル化すると共に、上体の高さを一定にすることで、線形性を確保している。このモデルは挙動を解析的に求めることが容易である。

この特許文献２に係るロボットは脚が極めて軽いので、脚反力を無視したことの影響は小さいが、通常の人間型の２足歩行ロボットの場合、脚部の質量が無視できないほど大きいことから、この提案技術によるときは近似精度が低下し、高速で歩行させると、最悪の場合、転倒する。

２）の影響度の小さい質点を無視した多質点モデルの例としては、非特許文献１を挙げることができる。

非特許文献１にあっては、各脚の膝と足首および上体にそれぞれ質点が設定されている。より具体的には、各質点は、あるリンクまたは関節上にローカルに設定された座標系内のある固定された点（座標）に設定される。

このモデルでは、図２３に示すような全リンクに質点とイナーシャを設定する場合に比較すると、演算量が１０分の１程度に減少する。また、上記した１質点モデルよりは、近似精度が高くなり、脚の質量が大きいロボットでも、この手法で生成した目標歩容を用いて歩行させることができる。

しかしながら、図２３に示した例と同様に、影響度の小さい質点を無視した多質点モデルにあっても、非線型性が依然強いので、解析的に将来挙動を予測して発散しないように対処することはできなかった。

したがって、この発明の目的は、脚式移動ロボットの動力学的挙動を記述するモデルを単純化して線形度を高め、モデルの挙動を通じてロボットの挙動が発散するか否かなどの予測を、逐次、リアルタイムに演算することも可能とすることで、動力学的に一層安定した歩容を生成できるようにした脚式移動ロボットの歩容生成装置を提供することにある。

上記した目的を達成するために、請求項１にあっては、少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、前記脚部の目標運動に基づき、前記脚部の目標運動によって発生する慣性力と重力の合力である脚部反力を算出する脚部反力算出手段、少なくとも目標床反力と前記脚部反力算出手段が算出した脚部反力とに基づき、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段、少なくとも前記倒立振子モデル支点位置算出手段が算出した倒立振子モデルの支点位置に基づき、前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段、少なくとも前記倒立振子モデル変位算出手段が算出した倒立振子モデルの変位に基づき、前記上体の位置を算出する上***置算出手段、および、少なくとも前記上***置算出手段が算出した上***置を用いて歩容を生成する歩容生成手段を備える如く構成した。

請求項２にあっては、少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、前記脚部の目標運動に基づき、前記脚部の目標運動によって発生する慣性力と重力の合力に疑似的に対応する前記脚部のＺＭＰに相当する脚部ＺＭＰを算出する脚部ＺＭＰ算出手段、少なくとも目標ＺＭＰと前記脚部ＺＭＰ算出手段が算出した脚部ＺＭＰとに基づき、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段、少なくとも前記倒立振子モデル支点位置算出手段が算出した倒立振子モデルの支点位置に基づき、前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段、少なくとも前記倒立振子モデル変位算出手段が算出した倒立振子モデルの変位に基づき、前記上体の位置を算出する上***置算出手段、および、少なくとも前記上***置算出手段が算出した上***置を用いて歩容を生成する歩容生成手段を備える如く構成した。

請求項３に係る脚式移動ロボットの歩容生成装置にあっては、前記倒立振子モデル支点位置算出手段は、前記目標ＺＭＰに第１の係数を乗じて得た積から、前記脚部ＺＭＰに第２の係数を乗じて得た積を減算して前記倒立振子モデルの支点位置を算出する如く構成した。

請求項４に係る脚式移動ロボットの歩容生成装置にあっては、前記床反力が少なくともＺＭＰを含む如く構成した。

請求項５に係る脚式移動ロボットの歩容生成装置にあっては、前記倒立振子モデルが線形モデルである如く構成した。

請求項１にあっては、脚部の目標運動に基づき、脚部の目標運動によって発生する慣性力と重力の合力である脚部反力を算出し、少なくとも目標床反力と算出した脚部反力とに基づき、少なくとも上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出し、少なくとも算出した倒立振子モデルの支点位置に基づき、倒立振子モデルの変位を算出し、少なくとも算出した倒立振子モデルの変位に基づき、上体の位置を算出すると共に、少なくとも算出した上***置を用いて歩容を生成する如く構成したので、脚式移動ロボットの動力学的挙動を記述するモデルを単純化して線形度を高めることができる。

即ち、脚式移動ロボットを２足歩行ロボットとするとき、倒立振子モデルでその動力学的挙動を記述すると共に、全ての力を床反力（より具体的にはＺＭＰ）相当値に換算しているので、ロボットの上体挙動を線形結合可能とすることができ、モデルの挙動を通じてロボットが発散するか否かなどの予測を、逐次、リアルタイムに演算することも可能となり、動力学的に一層安定した歩容を生成することができる。

請求項２にあっては、脚部の目標運動に基づき、脚部の目標運動によって発生する慣性力と重力の合力に疑似的に対応する脚部のＺＭＰに相当する脚部ＺＭＰを算出し、少なくとも目標ＺＭＰと算出した脚部ＺＭＰとに基づき、少なくとも上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出し、少なくとも算出した倒立振子モデルの支点位置に基づき、倒立振子モデルの変位を算出し、少なくとも算出した倒立振子モデルの変位に基づき、上体の位置を算出すると共に、少なくとも算出した上***置を用いて歩容を生成する如く構成したので、請求項１で述べたと同様の作用効果を得ることができる。

請求項３にあっては、目標ＺＭＰに第１の係数を乗じて得た積から、脚部ＺＭＰに第２の係数を乗じて得た積を減算して倒立振子モデルの支点位置を算出する如く構成したので、モデルの上体挙動を完全に線形結合可能とすることができ、モデルの挙動が発散するか否かなどの挙動予測を、逐次、リアルタイムに演算することができ、動力学的に一層安定した歩容を生成することができる。また、これによって歩行以外の動作もリアルタイムに生成できるようになるので、歩行以外の任意の動作と歩行を連続的に滑らかにつなげることも可能となる。

請求項４にあっては、床反力が少なくともＺＭＰを含む如く構成したので、請求項１で述べたと同様の作用効果に加え、床反力の記述が容易となって上記した作用効果を一層良く奏することができる。

請求項５にあっては、倒立振子モデルが線形モデルである如く構成したので、請求項１で述べたと同様の作用効果を得ることができる。

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

図１は、この実施例に係る歩容生成装置が適用される脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。

図示の如く、２足歩行ロボット（以下「ロボット」という）１は左右それぞれの脚部（脚部リンク）２に６個の関節を備える。６個の関節は上から順に、股（腰部）の脚部回旋（回転）用の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、股（腰部）のロール方向（Ｘ軸まわり）の関節１２Ｒ，１２Ｌ、股（腰部）のピッチ方向（Ｙ軸まわり）の関節１４Ｒ，１４Ｌ、膝部のピッチ方向の関節１６Ｒ，１６Ｌ、足首のピッチ方向の関節１８Ｒ，１８Ｌ、同ロール方向の関節２０Ｒ，２０Ｌから構成される。

関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には足平（足部）２２Ｒ，２２Ｌが取着されると共に、最上位には上体（基体）２４が設けられ、その内部にマイクロコンピュータからなる制御ユニット２６（後述）などが格納される。上記において、股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から、足関節（足首関節）は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２８Ｒ，２８Ｌ、膝関節と足関節とは下腿リンク３０Ｒ，３０Ｌで連結される。

尚、上体２４の上部には腕が取り付けられると共に、その上部には頭部が配置されるが、その詳細は、この発明の要旨と直接の関連を有しないため、省略する。

上記の構成により、脚部２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６＊２＝１２個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行させることができる（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）。

尚、この明細書で後述する上体２４の位置およびその速度は、上体２４の所定位置、具体的には上体２４の代表点の位置およびその移動速度を意味する。それについては後述する。

図１に示す如く、足関節の下方には公知の６軸力センサ３４が取着され、力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚとを測定し、足部の着地の有無および床反力（接地荷重）などを検出する。また、上体２４には傾斜センサ３６が設置され、Ｚ軸（鉛直方向（重力方向））に対する傾きとその角速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコーダが設けられる。

図２に示すように、足平２２Ｒ（Ｌ）の上方には、ばね機構３８が装備されると共に、足底にはゴムなどからなる足底弾性体４０が貼られてコンプライアンス機構４２を構成する。ばね機構３８は具体的には、足平２２Ｒ（Ｌ）に取り付けられた方形状のガイド部材（図示せず）と、足首関節１８Ｒ（Ｌ）および６軸力センサ３４側に取り付けられ、前記ガイド部材に弾性材を介して微動自在に収納されるピストン状部材（図示せず）とからなる。

図中に実線で表示された足平２２Ｒ（Ｌ）は、床反力を受けていないときの状態を示す。床反力を受けると、コンプライアンス機構４２においてバネ機構３８と足底弾性体４０がたわみ、足平２２Ｒ（Ｌ）は図中に点線で表示された位置姿勢に移る。この構造は、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。尚、その詳細は本出願人に先に提案した特開平５−３０５５８４号に記載されているので、詳細な説明は省略する。

さらに、図１では図示を省略するが、ロボット１の適宜な位置にはジョイスティック４４が設けられ、外部から必要に応じて直進歩行しているロボット１を旋回させるなど歩容に対する要求を入力できるように構成される。

図３は制御ユニット２６の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ３６などの出力はＡ／Ｄ変換器（図に「Ａ／Ｄ」と示す）５０でデジタル値に変換され、その出力はバス５２を介してＲＡＭ５４に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ５６を介してＲＡＭ５４内に入力される。

制御ユニット２６の内部にはそれぞれＣＰＵからなる第１、第２の演算装置６０，６２が設けられており、第１の演算装置６０は後述の如く、目標歩容を生成すると共に、後述の如く関節角変位指令を算出し、ＲＡＭ５４に送出する。また第２の演算装置６２はＲＡＭ５４からその指令と検出された実測値とを読み出し、各関節の駆動に必要な操作量を算出してＤ／Ａ変換器（図に「Ｄ／Ａ」と示す）６６とサーボアンプを介して各関節を駆動する電動モータに出力する。

図４は、この実施例に係る脚式移動ロボットの歩行（姿勢）制御装置の構成および動作を全体的に示すブロック図である。

以下説明すると、この装置は歩容生成部１００を備え、歩容生成部１００は後述の如く目標歩容を自在かつリアルタイムに生成して出力する。目標歩容は、目標上***置姿勢（軌道）、目標足平位置姿勢（軌道）、目標全床反力中心点（目標ＺＭＰ）（軌道）および目標全床反力（軌道あるいはパターン）からなる。

尚、各足平２２Ｒ，Ｌの床反力を「各足平床反力」と呼び、全ての（２本の）足平の床反力の合力を「全床反力」と呼ぶ。ただし、以降においては、各足平床反力はほとんど言及しないので、断らない限り、「床反力」は「全床反力」と同義として扱う。

最初にこの装置が生成する歩容について説明すると、この発明の目的は前記したように歩容をリアルタイムに生成する装置を提供することにあり、より具体的には本出願人が先に特開平１０−２７７９６９号公報において提案した複合コンプライアンス制御によるロボットの姿勢安定化制御を行うために必要な目標歩容を自在に生成することを目的としている。

その先に提案した姿勢安定化制御にあっては、床反力中心点（ＺＭＰ）の目標位置と実測位置の偏差を検出し、その偏差を解消するように脚部の一方または双方を駆動して姿勢の安定化を図っている。それと共に、ロボットが倒れそうになると、目標床反力を意図的にずらすことによって実床反力をずらし、姿勢復元力を得るようにしている。

かかる如く、脚式移動ロボットにおいては、目標運動軌跡と目標床反力パターンの関係が動力学的平衡条件を満足していなければ、安定した歩行が実現できない。動力学的平衡条件は前記した如く、具体的には、例えば、目標床反力中心点（目標床反力分布の２次モーメントが０になる床上の作用点）とＺＭＰが一致することである。一致していないと、コンプライアンス制御が働いたときに、慣性力と重力の合力と床反力との間のバランスを崩し、最悪の場合、転倒する。

先に提案した技術においては、それをロボットが不安定となったときの姿勢回復に逆用しているが、上記から明らかな如く、本出願人が提案する脚式移動ロボットにおいてはこのように歩行制御に与える目標値として、目標の運動パターンだけではなく、目標の運動パターンに対して動力学的平衡条件を満足する目標床反力パターンも必要である。

目標床反力は、一般的には、作用点とその点に作用する力と力のモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に前述の目標床反力中心点を作用点にして目標床反力を表現すると、力のモーメントは、床に垂直な成分を除けば、０になる。

尚、前述のように、動力学的平衡条件を満足する歩容では、目標とする運動軌跡から算出されるＺＭＰと目標床反力中心点は一致することから、目標床反力中心点軌道の代わりに目標ＺＭＰ軌道を与えると言っても同じことである。

従って、上記は、『歩行制御に与える目標値としては、目標運動軌跡だけでなく、目標ＺＭＰ軌道（目標床反力パターン）も必要である』と言い換えることができる。このような背景から、この明細書では目標歩容を、次のように定義する。

ａ）広義の目標歩容とは、１歩ないしは複数歩の期間の目標運動軌跡とその目標床反力パターンの組である。
ｂ）狭義の目標歩容とは、１歩の期間の目標運動軌跡とそのＺＭＰ軌道の組である。
ｃ) 一連の歩行は、いくつかの歩容がつながったものとする。

尚、以下では、理解を容易にするために、特にことわらない限り、目標歩容は狭義の目標歩容の意味で使用する。より詳しくは、この明細書では目標歩容は、両脚支持期の初期から片脚支持期の終端までの意味で使用する。

また、両脚支持期とは言うまでもなく、ロボット１がその自重を脚部リンク２の双方で支持する期間を、片脚支持期とは脚部リンク２の一方で支持する期間をいう。片脚支持期においてロボット１の自重を支持しない側の脚部（リンク）を「遊脚」と呼ぶ。定義の詳細は、先に提案した特開平１０−８６０８１号公報に記載されているので、この程度の説明に止める。

この発明は、具体的には、上記に定義した目標歩容を自在かつリアルタイムに生成することを目的とする。自在かつリアルタイムに生成できれば、例えばロボット１を遠隔操縦によってオペレータが自由自在に操ることができ、また、自動操縦においても、単なる固定シーケンス通りに移動するだけでなく、誘導制御やデリケートな位置決めも可能となる。

ここで、目標歩容としての条件を説明する。

目標歩容が満たさなければならない条件は、大きく分けて以下の５つに分類される。

条件１）動力学的平衡条件を満足していること。即ち、ロボット１の目標運動軌跡から動力学的に算出されるＺＭＰ軌道が目標ＺＭＰ軌道と一致していること。
条件２）ロボット１の歩行計画部や歩行経路誘導部（共に図示せず）、あるいはオペレータから歩幅や旋回角など歩容が満たすべき条件が要求される場合、それらの要求条件を満たしていること。
条件３）足平が床を掘ったり擦ったりしない、関節角度が可動範囲を越えない、関節速度が限界を越えないなどの、キネマティクス（運動学）に関する制約条件を満たしていること。
条件４）片脚支持期においてＺＭＰが支持脚足平接地面内になければならない、駆動系の最大能力を越えないなどの、動力学に関する制約条件を満たしていること。
条件５）境界条件を満たしていること。即ち、条件１）の当然の帰結として、歩容と歩容の境界では、少なくとも、各部位の位置と速度が連続であるという境界条件が導かれる（不連続であれば、無限大の力が発生したり、ＺＭＰが接地面からはるかに遠くの点に移動してしまうから）。

また、第ｎ＋１回歩容の初期状態は、第ｎ回歩容の終端状態（特に、足平位置に対する上体の位置・姿勢および速度）に一致するように設定されなければならない。このとき、第ｎ回歩容の終端状態は、第ｎ＋１回歩容の初期状態が決まっていれば、第ｎ＋１回歩容の初期状態に一致させれば良い。

決まっていなければ、第ｎ回歩容の終端状態が、姿勢が崩れないで長期的な歩行ができる範囲に入っていれば良い。但し、後述するように、姿勢が崩れないで長期的な歩行ができるための終端状態の範囲を求めることは、極めて難しい。

一般的に、目標歩容は、パラメータ値あるいは時系列テーブルを含む歩容発生アルゴリズムによって発生させられる（歩容を決定するということは、パラメータ値あるいは時系列テーブルを適当に設定することに他ならない）。

パラメータ値あるいは時系列テーブルを変えることによって、様々な歩容が生成される。しかし、パラメータ値あるいは時系列テーブルを十分な配慮もせずに設定しただけでは、作成された歩容が前記の歩容条件をすべて満足しているか否かは分からない。

特に、長期的歩行に適した終端上***置および速度の範囲が分かっていたとしても、ＺＭＰ軌道に基づいて上記した条件１）を満足する上体の軌道を歩容発生アルゴリズムに従って生成する場合には、発生した歩容の終端での上***置と速度の両方がその範囲に入るようにＺＭＰ軌道に関するパラメータを設定することは、極めて難しい。

その理由は次の通りである。

理由１）上体は、一旦、ＺＭＰから遠くに離れると、さらに遠くに離れようとする発散傾向がある。これを説明するために、ロボットの上体の挙動に近い倒立振子の挙動を例に挙げる。

倒立振子は重心の床投影点が支点からずれると、ずれが発散して倒れる。しかし、そのときも倒立振子の慣性力と重力の合力は支点上に作用し（即ち、ＺＭＰが支点に一致し）、支点から受ける床反力に釣り合っている。即ち、動力学的平衡条件は、運動する物体のその瞬間における慣性力と重力と床反力の関係を表すだけのものである。

動力学的平衡条件を満足していればロボットの長期歩行が保証されたように錯覚しやすいが、ロボットの姿勢が崩れているか否かとは全く関係がない。倒立振子の重心が支点の真上から離れると、ますます遠くに離れようとする傾向があるように、ロボットの重心がＺＭＰの真上から遠くに離れるとますます遠くに離れようとする発散傾向がある。

理由２）片脚支持期においてＺＭＰが支持脚足平接地面内になければならないなどの厳しい制約条件があるために、上体の加減速パターンを恣意的に設定することができず、位置を合わせようとすると速度が合わず、速度を合わせようとすると位置が合わず、なかなか両方を同時に一致させることは難しい。以上の問題点のために、前記歩容条件を全て満たした目標歩容のパラメータやテーブルなどのオフラインで設計した基準歩容を持たず、完全にリアルタイムに決定することは、先に提案した技術も含めて従来では不可能であった。

歩容の説明を続けると、歩容は、歩容パラメータによって記述される。歩容パラメータは、運動パラメータとＺＭＰパラメータ（より一般的に表現すれば、床反力パラメータ）から構成される。尚、この明細書で『床反力パラメータ』なる語は、『床反力の時間的なパターンに関するパラメータ』を意味するものとして使用する。ＺＭＰパラメータは後で図１６に示すように、Ｘ，Ｙ，Ｚ座標（方向）について折れ線グラフ状のＺＭＰ軌道の折れ点の位置と通過時刻で示す（Ｘ座標のみ図示）。

運動パラメータは、足平（軌道）パラメータと上体（軌道）パラメータとから構成される。

足平軌道パラメータは、初期（離床時）遊脚位置および姿勢、終端（着床時）遊脚位置および姿勢、両脚支持期時間、片脚支持期時間などを含む。これらに基づき、先に本出願人が特開平５−３１８３３９号および特開平５−３２４１１５号公報などで提案した技術を用いて、両足平の位置（ｘf ，ｙf ，ｚf ）および姿勢（θｘf ，θｙf ，θｚf ）の瞬時値が求められる。尚、ここで姿勢は『空間上の傾斜または向き』を意味する。

上体軌道パラメータは、上体の姿勢（空間上の上体２４の向きあるいは傾き）を決定するパラメータ、上体高さ（Ｚ方向の値）を決定するパラメータ、初期の上***置（変位）および速度パラメータなどから構成される。

上体の位置および姿勢を決定するパラメータに基づき、時間関数または先に特開平５−３２４１１５号公報で本出願人が提案した技術や後述する手法から上体の水平位置（ｘb ，ｙb ）および姿勢（θｘb ，θｙb ，θｚb ）の瞬時値が求められる。上体高さｚb は適宜な手法、例えば本出願人が特開平１０−８６０８０号公報で提案する手法で決定される。

脚式移動ロボット１の関節は図１に示すように１２関節から構成されているので、得られた両足平の位置・姿勢と上***置・姿勢とから、後述するように逆キネマティクス演算によって目標関節変位が一義的に決定される。即ち、今回のロボットの目標姿勢が一義的に決定される。

ところで、理想的目標歩容は、物理法則に逆らうことができないので、希望する状態にすぐに到達することはできない。許容範囲内でＺＭＰの軌道、着地位置および着地時期などの歩容パラメータを変更することによって、時間をかけて希望する状態に遷移しなければならない。特に、この実施例において２足歩行ロボットの動力学系は、ＺＭＰを入力、上***置を出力とする系とみなすと発散系になるので、慎重に歩容パラメータを変更しないと、正常な状態に復元するのが困難となる。

したがって、目標歩容を自在かつリアルタイムに生成するときは、ロボットの将来の挙動を予測し、どのように歩容パラメータ値を設定すれば、ロボットの将来、例えば数歩先の挙動が発散しないか否かを判断すると共に、発散する可能性が予測されるときは発散を防止するように歩容を調整することが、望ましい。

しかしながら、前記したように、図２３に示すような多質点系モデルを用いるとき、その動力学演算は、演算量が多くかつ非線形性が強いので、終端状態をリアルタイムに求めるのは、ロボットに搭載可能な通常のＣＰＵ（第１の演算装置６０）の能力では困難である。

他方、将来挙動予測とそれに基づく発散防止制御は、多くの知識を蓄積させておき、歩容切り変わり目毎に、そのときの状態と目的に応じ、知識に照らし合わせて目標歩容を選び出す手法でも、あるレベルまでは達成することができるであろうが、ありとあらゆる状況に対応することは、組合わせ爆発を起こし、事実上不可能である。

そこで、この実施例においては、ロボット１の動力学的挙動を記述する動力学モデルを単純化し、リアルタイムかつ解析的に将来挙動が予測計算できるようにした。図５に、その単純化した動力学モデルを示す。図示の如く、この動力学モデルは３質点モデルであり、デカップルド、即ち、脚部の動力学と上体の動力学が相互に非干渉に構成されると共に、ロボット全体の動力学は、それらの線形結合で表される。図６は、歩容生成部１００の動力学演算部において、図５に示す動力学モデルを用いて行われる動力学演算を示すブロック図である。

以下、この動力学モデルを説明する。

１）このモデルは、倒立振子、支持脚足平質点、遊脚足平質点の３質点から構成される。
２）支持脚足平質点は、支持脚足平にローカルに設定された座標系（具体的には、原点が足首中心から足底への垂直投影点、ＸＹ平面が足底に一致し、かかとからつまさきへの向きをＸ軸にとったＸＹＺ直交座標系であり、これを以降、「支持脚ローカル座標系」と呼ぶ）上のある固定された点に設定される。この固定された点の支持脚ローカル座標系上の座標を、「支持脚足平質点オフセット」と呼ぶ。

同様に、遊脚足平質点は、遊脚足平にローカルに設定された座標系（これを以降、「遊脚ローカル座標系」と呼ぶ）上のある固定された点に設定される。この固定された点の遊脚ローカル座標系上の座標を、「遊脚足平質点オフセット」と呼ぶ。

尚、支持脚が床に全面的に密着しているときの足首位置の床への鉛直投影点を原点とし、床に固定され、支持脚足平の前方向をＸ軸、左方向をＹ軸、鉛直方向をＺ軸にとった座標系を「支持脚座標系」と呼ぶ（これは. 上記した支持脚ローカル座標とは異なる。）。ことわらない限り、位置、速度、力などは支持脚座標系で示す。

３）倒立振子は、水平に移動するフリーの支点ａと、ひとつの質点ｂと、支点と質点を結ぶ質量のない可変長のリンクｃから構成される。また、リンクが傾いてもリンクが伸縮し、支点から見た質点高さが一定値に保たれるものとする。（換言すれば、この倒立振子は、前記した特公平４−１５０６８号公報に記載されるロボットの線形倒立振子と同一である。）

倒立振子質点は、物理的意味としては上体２４の質点（必ずしも重心位置を意味しない）に対応する。したがって、倒立振子質点と上体質点は、今後、同意語として扱う。倒立振子質点の位置（より広義に言えば変位）を、以降、略して「倒立振子位置」と呼ぶ。

４）上体の水平位置は、倒立振子の水平位置から幾何学的に決定される。具体的には、例えば、上体にローカルに設定された座標系（これを以降、「上体座標系」と呼ぶ）上のある固定された代表点（この点を以降、「上体代表点」と呼ぶ）の水平位置（支持脚座標系から見たＸＹ座標）が、倒立振子の水平位置に一致するように決定される。即ち、図５に示すように、上体代表点と倒立振子質点ｂは、同一鉛直線上にあるように決定される。

上体代表点の上体座標系上の水平座標（ＸＹ座標）を「上体質点オフセット」と呼ぶ。

次いで、図示の動力学演算モデルに関する変数およびパラメータなどの記述法について説明する。

説明の便宜上、動力学演算モデルに関する変数およびパラメータなどを以下のように定義し、記述する。

倒立振子のＺＭＰは、支点ａの位置にある。なぜなら、ＺＭＰは定義からモーメントが発生しない点のことであり、フリーの支点ａにはモーメントが発生しないからである。そこで、倒立振子支点位置を倒立振子自身のＺＭＰ位置（ＺＭＰ相当値）ということで、以降、「倒立振子ＺＭＰ」と呼び、「ZMPpend 」と記述する。

各質点の質量と位置は次のように記述する。
msup: 支持脚質点質量
mswg: 遊脚質点質量
mb: 倒立振子質点質量（上体質点質量）
mtotal:ロボット質量（= mb + msup + mswg）
mfeet: 両脚質量（= msup + mswg）
xsup: 支持脚質点位置
xswg: 遊脚質点位置
xb: 倒立振子位置（上体質点位置）

以降、ことわらない限り、xbは３次元ベクトル（ＸＹＺ座標ベクトル）で表わす。倒立振子の高さは、倒立振子の支点から質点までの高さを意味し、ｈと記述する。

d(xb)/dtはxbの１階微分を表わし、倒立振子の速度を示す。d2(xb)/dt2はxbの２階微分を表わし、倒立振子の加速度を示す。ｇは重力加速度定数を示す。Ｇは重力加速度ベクトルを示し、Ｘ，Ｙ成分が０、Ｚ成分が−ｇであるベクトルと定義する。

図示の３質点モデルにおいて、脚質点の総慣性力がある作用点Ｐまわりに作用するモーメントを、点Ｐまわりの脚総慣性力モーメントと定義する（慣性力と重力の合力を「総慣性力」と呼ぶ）。作用点Ｐの座標（あるいは位置）をxpと置く。

下記の数式１は、点Ｐまわりの脚総慣性力モーメントの厳密な動力学的定義式である。

脚ＺＭＰをZMPfeetと記述し、脚ＺＭＰを数式２で定義する。ただし、脚ＺＭＰの高さ（ZMPfeet のＺ成分）は、点Ｐ位置の高さと同一とする。このように、脚ＺＭＰは、脚部の運動によって発生する慣性力と重力の合力（総慣性力）に疑似的に対応させた値である。

本来、図１に示すロボット１の動力学は非線形であるが、近似して目標ＺＭＰ、脚ＺＭＰ(ZMPfeet) 、および倒立振子ＺＭＰ(ZMPpend) の間には数式３の線形関係を与える。

一般に、線形倒立振子の挙動を表わす微分方程式は、数式４で表わされる。

ただし、作用点Ｐは、モデルの近似精度が高くなるように設定される。例えば、作用点Ｐは、図７タイム・チャートに示すように、直前（前回）歩容の支持脚座標系上の原点から、今回歩容の支持脚座標系の原点に、両脚支持期の間に直線的に等速移動する点に設定される。

この実施例に係る脚式移動ロボットの歩容生成装置で提案する動力学モデルは、図５に示すような足平、上体と各質点の位置の関係を表わす前記オフセットと、上記の数式（数式１から４）で表わされる。これにより、後述の如く、将来挙動予測が容易となった。

図６を参照して歩容生成部１００（図１に示す）の動力学演算部の動作を説明すると、動力学演算部は脚ＺＭＰ算出器２００を備え、脚ＺＭＰ算出器２００は、数式１および数式２ならびに図７で示される作用点Ｐに基づいて脚ＺＭＰ（ZMPfeet ）を算出する。

次いで、上記算出したZMPfeet をmfeet/mtotal倍したものを、目標ＺＭＰから減じ、さらによって得た差をmtotal/mb 倍することによって、倒立振子ＺＭＰ(ZMPpend) を算出する。即ち、倒立振子ＺＭＰ(ZMPpend）は、前記ロボットの質量mtotalと前記上体の質点の質量mbの比に前記目標ＺＭＰを乗じて得た積から、前記脚部の質量mfeet と前記上体の質点の質量mbの比に脚部のＺＭＰ相当値ZMPfeet を乗じて得た積を減算して算出される。この処理は、数式３に相当する。

倒立振子の挙動は、数式４で表現され、倒立振子ＺＭＰ(ZMPpend) から倒立振子質点水平位置xbが算出される。

さらに、図６に示す如く、動力学演算部は上***置決定器２０２を備え、上***置決定器２０２は、倒立振子質点水平位置から上体の水平位置xbを決定する。具体的には、上***置決定器２０２は、前述した上体代表点（図５に示す）の水平位置が、倒立振子の水平位置に一致するように上体の水平位置を決定する。

次いで、歩容の継続的姿勢安定性について述べる。

ここまで述べてきた動力学モデル自身は、単に、各瞬間における動力学的平衡条件を近似的に満足するように、目標ＺＭＰから上体軌道を算出するだけのものであり、上体軌道が発散すること（図８に示すように上体２４の位置が両足平２２Ｒ，Ｌの位置からかけ離れた位置にずれてしまうこと）を防止するものではない。

以下では、上体軌道の発散を防止し、上体と両足平の間の適切な位置関係を継続させるための手段について説明する。

最初に、そのための予備知識として、将来の上体軌道を解析的に求める上で特に重要な、線形倒立振子の性質について議論する。ここでは、離散化モデルを用いて説明する。

先ず、倒立振子に関し、新たに以下の変数やパラメータを定義する。
ω0： 倒立振子の固有角周波数
ω0 = sqrt(g/h)（ただし、sqrtは平方根を表わす。）
Δt： 刻み時間
ｘ[k]：ｋステップ目（時刻kΔt）の倒立振子位置
ｖ[k]：ｋステップ目の倒立振子速度
ZMPpend[k]： ｋステップ目の倒立振子ＺＭＰ（詳しくは、時刻ｋΔｔから時刻(k+1) Δｔまで、入力ZMPpend[k]の０次ホールドした信号が入力、即ち、その時刻の間は同一信号が倒立振子に入力され続けられるものとする。）

さらに、ｑ［ｋ］とｐ［ｋ］を数式５で定義する。

倒立振子の運動方程式を離散化し、ｑ[ｋ] とｐ［ｋ］に関して解くと、以下の数式６と数式７が得られる。尚、exp は指数関数を表す。

これらの式の意味を考える。

ある有限な定数ZMPminおよびZMPmaxに対し、数式８を常に満足するように、ZMPpend[i]が設定されるものと仮定する。

数式８の中辺と右辺の関係を、数式６に代入すると、下記の数式９が得られる。

等比級数の和の定理から、下記の数式１０が得られる。

したがって、数式９は、数式１１のように書き直すことができる。

同様に、数式８の左辺と中辺の関係から、数式１２を得ることができる。

exp(-ω0kΔt)は、ｋが無限大になると０に収束（漸近）するので、数式１１，１２は、ZMPpend が変動してもp[ｋ] は発散せず、いずれはZMPmaxとZMPminの間に入ることを意味する。

さらに、具体例として、ZMPpend がある時点以降、一定値ZMP0になる場合を考える。この場合、その時点を改めて時刻０とすると、数式６は、数式１３のように書き直すことができる。これは、が、その初期値にかかわらず、ZMP0に等比級数的に収束することを意味する。

より一般的には、p[ｋ]は、ある時点でどのような値であっても、その後のZMPpend 波形がある同一波形であれば、ZMPpend 波形に追従する、ある軌道に収束する。

一方、ｑ［ｋ］は、数式７から明らかなように発散する傾向がある。

具体例として、ZMPpend がある時点以降、一定値ZMP0になる場合を考える。この場合、一定値ZMP0になる時点を改めて時刻０とすると、数式７は、数式１４となり、これは、q[0] = ZMP0 ではない限り、ｑ［ｋ］が、ZMP0から等比級数的に発散することを意味する。

そこで、以降、数式５によって定義されるｐ［ｋ］を収束成分、ｑ［ｋ］を「発散成分」と呼ぶ。

以上より、上体軌道の発散を防止し、上体と両足平の間の適切な位置関係を継続させるためには、事実上、収束成分は無視して構わず、支持脚から見た発散成分を歩行に支障ない範囲から越えないように管理することが重要と言うことができる。

より具体的には、発散成分を歩行に支障ない範囲（姿勢が大きく崩れない範囲）から越えないように、ＺＭＰ軌道パラメータなどを適切に決定することが重要であると言える。尚、上体挙動を線形化しなければ、上体挙動を求める式が、数式６，７のように発散成分と収束成分に分離した式にすることができないので、上記のような議論を展開することができなかったが、この実施例において可能となった。

収束成分と発散成分の性質を以下に列挙する。

性質１：対称性
収束成分ｐ［ｋ］と発散成分ｑ［ｋ］を求める式の違いは、ｐ［ｋ］とｑ［ｋ］が置き換わったことと、ω０が−ω０に置き換わったことだけである。

性質２：等価換算
上記性質から、以下では発散成分についてのみ議論する。

入力ZMPpend[i]として、ZMPpend[0]＝ U0 と、ZMPpend[i]＝０ (i = 1,2,...)で表わされるパルスを考えると、そのときのｑ［ｋ］は、数式７にこのZMPpend[i]を代入することにより得られ、数式１５となる。

これより、発散成分ｑ［ｋ］に対する影響だけを考えると、初期パルスU0を入力することは、U0 * (exp(-ω0 Δt) - 1) だけ初期発散成分をずらすことと同じ影響を与える。即ち、初期パルスは、(exp(-ω0Δt) - 1) 倍することによって、初期発散成分に等価換算される。

また、数式７と数式１５を比較することにより、発散成分ｑ［ｋ］に対する影響だけを考えると、入力ZMPpend[i](i＝0,1,2...,k-1）を入力することは、数式１６に示す初期パルス入力U0と同じ影響を与える。

即ち、入力ZMPpend[i](i＝0,1,2...,k-1）は、数式１６によって、初期パルス入力U0に等価換算される。

性質３：時間遅れの影響
また、図９に示すように、ZMPpend[i]がある関数f(i)であり、ZMPpend'[i] が関数f(i)を時間ｎΔｔだけ遅らせた関数f(i- nΔt)であるとし、入力列ZMPpend[i]を初期パルスに変換したものをU0、入力列ZMPpend'[i] を初期パルスに変換したものをU0' とすると、U0' は数式１７のようになる。

即ち、同一波形の入力を時間ｎΔｔだけ遅らせて入力すると、その入力によって発生する発散成分は、exp(-nω0 Δt)倍になる。

次いで、図５に示す倒立振子モデルに、複雑な倒立振子ＺＭＰ波形を入力したときの発散成分について述べる。

ところで図５（または数式３）に示す動力学モデル内の倒立振子モデルに与えられるＺＭＰ入力は、目標ＺＭＰにmtotal/mb を乗じたものと、脚ＺＭＰにmfeet/mbを乗じたものの差である。目標ＺＭＰは具体的には、折れ線状の波形であるので、目標ＺＭＰにmtotal/mb を乗じたものも、やはり折れ線の波形である。

そこで、以下では、折れ線状のＺＭＰが倒立振子に入力されたことによって発生する発散成分を解析的に求める原理とアルゴリズムを説明する。尚、脚ＺＭＰに関する影響については、後述する。また、実際には、倒立振子ＺＭＰは、２次元空間（水平面上）を移動するが、以下の議論では、理解の容易のため、１次元空間を移動するものとする。

倒立振子のＺＭＰ（ZMPpend ）波形として図１０に示すような折れ線を想定する。区間ｉは、期間（時間）がperiod[i] でその区間の初期値がZMP[i]であるとする。このＺＭＰによって発生する発散成分を求めるための原理とアルゴリズムを説明する。

倒立振子モデルに倒立振子ＺＭＰ入力として、ｘ＝f(t)が時刻範囲 [０,T] の間入力されたことによって発生する発散成分を、時刻０に入力され、時間幅が刻み時間Δｔのパルスの大きさに換算したものをH(f(t),T) とする。

数式１６から、H(f(t),T) は、次の数式１８に示す積和演算で表わされる（ω：倒立振子の固有角周波数）。

ところで一般的に、区間ｉの倒立振子ＺＭＰ波形は、図１１に示すように、ある遅れを持つステップ波形とランプ波形とに分解できる。そこで、まずステップ状の波形とランプ状の波形に対するH(f(t),T) を以下に述べる手順で求めておく。

ｘ＝f(t)＝１のステップ波形のH(f(t),T) をH(1,T)と記述すると、等比級数の和の公式から、数式１９に示すようになる。

ｘ＝f(t)＝ｔのランプ波形のH(f(t),T) をH(t,T)と記述すると、数式２０に示すようになる。

尚、ＺＭＰ波形が折れ線以外であるときは、上記以外の関数f が必要となる。上記以外の関数ｆでは、解析解F(f(t),T) が存在しない場合もあるが、その際には、関数ｆを正規化してから、Ｈ(f(t),T)をテーブル化すれば良い。尚、上記例でもF(f(t),T) をテーブル化しても良い。

さて、ここまで準備しておいてから、図１０の倒立振子ＺＭＰによって発生する発散成分を、時刻０に入力されるパルスであって、その時間幅が刻み時間Δｔの倒立振子ＺＭＰパルスの大きさに換算したもの（以降、これをＷと記す）を以下の手順で求める。

まず、区間ｉ(i＝0,1,…5)の倒立振子ＺＭＰの影響を、区間ｉの初期時刻に入力されるパルスであって、その時間幅が刻み時間Δｔのパルスの大きさinf[i]に換算する。区間ｉの倒立振子ＺＭＰ波形は、図１１に示すように、高さがZMP[i]の矩形波と傾きが(ZMP[i+1]-ZMP[i])/period[i] のランプ波の和で表わされるので、パルス換算値は数式２１に示すようになる。

次いで、区間period[i] (i＝0,1,.., m)の倒立振子ＺＭＰの影響を、時刻０に入力されるパルスであって、その時間幅が刻み時間Δｔのパルスの大きさinf'[i] に換算する。性質３から、パルス換算値は数式２２のように算出される。

図１０に示す倒立振子ＺＭＰ波形全区間の影響は、各区間の倒立振子ＺＭＰの影響の和になる。即ち、パルス換算値Ｗは、数式２３に示すように算出される。

さらに、性質２より、上記Ｗを初期発散成分に等価換算したものは、以下の数式２４に示す値になる。

さらに、数式２４または性質３より、これを歩容終端時刻での発散成分に換算したものは、数式２５に示す値になる。

以上の手順により、折れ線の倒立振子ＺＭＰによって発生する歩容終端発散成分を解析的に求めることができる。

次いで、歩行の継続性を保証する歩容生成アルゴリズムについて述べる。

先に、上体軌道の発散を防止し、上体と両足平の間の適切な位置関係を継続させるためには、発散成分を歩行に支障ない範囲（姿勢が大きく崩れない範囲）から越えないように、ＺＭＰ軌道パラメータなどを適切に決定することが重要であると述べた。次の問題は、歩行に支障ない発散成分の範囲あるいは適切な発散成分の値を如何に求めるかである。

この実施例に係る歩容生成部１００は、今回生成する歩容につながるべき歩容（以下「定常旋回歩容」と呼ぶ。これについては後述する）を仮に想定することによって、適切な発散成分の値を決定するようにした。以下に歩容生成部１００の動作を詳細に説明する。

歩容生成部は、２歩先までの遊脚足平着地位置姿勢、着地時刻の要求値（要求）を入力として、目標上***置姿勢軌道、目標足平位置姿勢軌道、目標ＺＭＰ軌道を決定する。このとき、歩容パラメータの一部は、歩行の継続性を満足するように修正される。生成しようとしている歩容を「今回歩容」、その次の歩容を「次回歩容」、さらにその次の歩容を「次次回歩容」と呼ぶ。

図１２は、その歩容生成部１００の歩容生成処理を示すフロー・チャート（構造化フロー・チャート）である。

以下説明すると、まずＳ１０において時刻ｔを０に初期化するなど種々の初期化作業を行う。次いでＳ１２を経てＳ１４に進み、制御周期毎のタイマ割り込みを待つ。制御周期はΔｔである。次いでＳ１６に進み、歩容切り変わり目であるか否かを判断し、肯定されるときはＳ１８に進むと共に、否定されるときはＳ２０に進む。

Ｓ１８に進むときは時刻ｔを０に初期化し、次いでＳ２２に進み、次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期を読み込む。これらは前記した要求に相当するが、あらかじめ歩行スケジュールとして記憶しておいても良く、あるいはジョイスティック４４などの操縦装置からの指令（要求）とそのときまでの歩行履歴を基に決定しても良い。

次いで、Ｓ２４に進み、今回歩容の歩容パラメータを仮決定（仮に算出）する。

今回歩容初期遊脚足平位置姿勢は、今回支持脚座標系から見た現在遊脚位置姿勢にする（初期値とする）。今回歩容初期支持脚足平位置姿勢は、今回支持脚座標系から見た現在支持脚位置姿勢にする（初期値とする）。今回歩容終端遊脚足平位置姿勢は、今回支持脚座標系から見た次回支持脚座標系に対応して決定する。（即ち、今回歩容終端遊脚足平位置姿勢から足平を床に接触させたまま、すべらないように足平を水平まで回転させたときの位置姿勢が、次回支持脚座標系となるように設定する。）

今回歩容終端支持脚足平位置姿勢は、現在支持脚位置姿勢から足平を床に接触させたまま、すべらないように足平を床に面接触するまで回転させたときの位置姿勢とする。したがって、床が平面であるならば、今回歩容終端支持脚足平位置姿勢は、今回支持脚座標系に一致する。尚、この歩容では、歩容終端において支持脚足平は水平になるが、必ずしも、このように設定する必要はない。

今回歩容のＺＭＰ軌道パラメータは、安定余裕が高く、かつ急激な変化をしないように決定する。（接地面を含む最小の凸多角形（いわゆる支持多角形）の中央付近にＺＭＰが存在する状態を安定余裕が高いと言う（詳細は特開平１０−８６０８１号公報に記述））。ただし、今回歩容のＺＭＰ軌道パラメータは、仮決定されただけであり、後述するように修正される。

次いでＳ２６に進み、今回歩容につながる定常旋回歩容の歩容パラメータを決定する。

尚、この明細書で「定常旋回歩容」は、その歩容を繰り返したときに歩容の境界において運動状態に不連続が生じないような周期的歩容を意味するものとして使用する。

定常旋回歩容は、通常、第１旋回歩容と第２旋回歩容とからなる。尚、ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。定常旋回歩容は、歩容生成部１００で今回歩容の終端における発散成分を決定するために暫定的に作成されるものであり、歩容生成部１００からそのまま出力されるものではない。

補足すると、定常旋回歩容は、狭義の意味での歩容（両脚支持期の初めから次の片脚支持期の終わりまでを１歩とする歩容）が、少なくとも２歩分必要である。３歩以上の歩容からなる複雑な定常旋回歩容を設定することも可能であるが、今回歩容の終端における発散成分を決定するだけに用いられるので、処理が煩雑となるにも関わらず、効果は少ない。尚、３足以上の脚式移動ロボットにあっては、それに応じて旋回を定義するに足る歩容数が増加する。

先ず、今回歩容、第１旋回歩容、第２旋回歩容の順に脚軌道がつながるように、第１旋回歩容と第２旋回歩容の歩容パラメータ中の脚軌道の境界条件を設定する。

具体的には、第１旋回歩容初期遊脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とする。第１旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とする。

第１旋回歩容終端遊脚足平位置姿勢は、今回歩容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容支持脚座標系から見た次次回歩容支持脚座標系に対応して決定する。第１旋回歩容終端支持脚足平位置姿勢は、次回歩容支持脚座標系に一致させた足平を床に接触させたまま、すべらないように足平を床に面接触するまで回転させたときの位置姿勢とする。（したがって、床が平面であるならば、第１旋回歩容終端支持脚足平位置姿勢は、次回支持脚座標系に一致する。）

第２旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端支持脚足平位置姿勢にする。第２旋回歩容初期支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た第１旋回歩容終端遊脚足平位置姿勢にする。

第２旋回歩容終端遊脚足平位置姿勢は、今回支持脚座標系から見た今回歩容終端遊脚足平位置姿勢にする。第２旋回歩容終端支持脚足平位置姿勢は、今回支持脚座標系から見た今回歩容終端支持脚足平位置姿勢にする。

これらの関係を図１３に示す。

第１旋回歩容および第２旋回歩容の歩行周期は、次回歩容周期と同一にする（尚、同一にすることは必ずしも必要ではないが、次回歩容周期に応じて決定するのが好ましい）。今回歩容、第１旋回歩容および第２旋回歩容の上記以外の運動パラメータ（両脚支持期時間などの時間パラメータを含む）は、上記決定されたパラメータに応じて、歩容の条件（電動モータ（アクチュエータ）の速度が許容範囲に入っていることなど）を満足するように適宜決定する。

第１旋回歩容および第２旋回歩容のＺＭＰ軌道パラメータも、安定余裕が高くかつ急激な変化をしないように決定する。

ところで、上体代表点の初期位置と速度をある値Ｘ０，Ｖ０に設定すると、図５に示す単純モデルを用いて上記第１旋回歩容と第２旋回歩容を生成し、再び第１旋回歩容の生成を開始するときの初期上体代表点位置速度が、前記設定した初期上体代表点位置速度の値Ｘ０，Ｖ０に一致する。この値Ｘ０，Ｖ０を「定常旋回歩容の初期上体代表点位置速度」と呼ぶ。この関係を図に表すと、図１４のようになる。ただし、Ｘ０を（ｘ０，ｙ０）と表すこととし、Ｖ０については表記を省略した。

このように設定すると、図５に示す単純モデルを用いて第１旋回歩容と第２旋回歩容を交互に繰り返し生成しても、演算誤差が蓄積しない限り、第１旋回歩容の初期上体代表点位置速度は、値Ｘ０，Ｖ０になる。即ち、歩行の継続性が保証される。このときの発散成分、即ち、X0 + V0/ω0 を、「定常旋回歩容の初期発散成分」と呼ぶ。

図１２の説明に戻ると、次いでＳ２８に進み、定常旋回歩容の初期発散成分を求める。

図１５は、図１２フロー・チャートのＳ２８の定常旋回歩容の初期発散成分を算出するサブルーチン・フロー・チャートである。

ここで、同図の説明に入る前に、定常旋回歩容の初期発散成分の算出原理を説明する。

数式３と数式７から、次の数式２６が得られる。尚、目標ＺＭＰをZMPtotalと記述する。

実際には、各歩容はその歩容の支持脚座標系上で表現されるが、ここでは、説明を容易にするために、第１旋回歩容と第２旋回歩容は、第１旋回歩容の支持脚座標系で表現されているものとし、前記したように第１旋回歩容と第２旋回歩容を合わせて一つの歩容（定常旋回歩容）とみなす。

数式２６を用いて、定常歩旋回容の終端における発散成分ｑ［ｋ］（これを以降、「終端発散成分」と呼ぶ）を求めることを考える。時刻０は、定常旋回歩容の初期時刻とする。定常旋回歩容の終端時刻はｋΔｔとする。尚、終端時刻は、その次の歩容の時刻０に対応する。

右辺第１項は、初期発散成分によって発生する終端発散成分を表わす。右辺第２項は、目標ＺＭＰパターンによって発生する終端発散成分を表わす。右辺第３項は、脚運動によって発生する終端発散成分を表わす。

まず、目標ＺＭＰパターンによって発生する終端発散成分は、目標ＺＭＰパターンが折れ線であるので、前述の折れ線状ZMPpend によって発生する終端発散成分を求める、数式１９，２０，２１，２２，２３，２５に示すアルゴリズムを用いて解析的に求める。これをＷzmptotalとする。（ＸＹ方向があるので、Ｗzmptotalは２次元ベクトルになる。）

次に、脚運動によって発生する終端発散成分を求める。最も単純な求め方は、歩容生成部において内部的に、上記した定常旋回歩容の歩容パラメータから脚運動を生成し、さらにZMPfeet[i](i＝0,1,…,k) を、数式１，２から求め、これを、数式２６の右辺第３項に代入する手法である。

演算量は多いが、ロボット１に搭載可能な高速のコンピュータを用いれば制御周期内に演算を完了することができる。また、コンピュータの性能を同一とするときも、演算に際してサンプリング周期を実際の制御周期よりも長く設定すれば、若干の誤差は発生するが、演算量を減少させることもできる。

上記の如くして求められた値をＷfeetとする。（ＸＹ方向があるので、Ｗfeetは２次元ベクトルになる。）

以上より、次の数式２７を得る。

さて、定常発散成分の定義から、定常旋回歩容の次の歩容の支持脚座標系から見た定常旋回終端発散成分をｑ’［ｋ］とすると、次の数式２８を満足しなければならない。

一方、定常旋回歩容の支持脚座標系（図１３に示すＸ’，Ｙ’座標系）から見た定常旋回歩容の次の歩容の支持脚座標系（同図に示すＸ”，Ｙ”座標系）を、座標系の回転を表わす回転行列Ｍと座標系の原点座標を表わす平行移動ベクトルＢで表わすと、以下の数２９に示すようになる。

数式２８と数式２９より、次の数式３０を得る。

さらに、数式３０と数式２７より、次の数式３１を得る。

マトリックスＡの逆行列をinv(Ａ) と記述すると、数式３１から数式３２が得られる。ただし、Ｉは単位行列である。

上記した算出原理を前提とし、図１５フロー・チャートを参照して説明すると、Ｓ１００において目標ＺＭＰによって発生する終端発散成分を上記手法で求め、次いでＳ１０２に進み、数式２６の右辺第３項を用いて脚反力によって発生する終端発散成分を求め、次いでＳ１０４に進み、数式３２を用いて定常旋回初期発散成分ｑ［０］を求める。定常旋回初期発散成分ｑ［０］は、定常旋回歩容の支持脚座標系から見た値になっている。

次いで、Ｓ１０６に進み、定常旋回初期発散成分ｑ［０］を今回歩容の支持脚座標系から見た値に変換する。これをｑ”とする。

次の課題は、今回歩容の終端発散成分がｑ”に一致するように、上記仮決定した今回歩容のパラメータを修正することである。この処理においては、上記定常旋回歩容における処理とほとんど同じであるので、以下では、変数、値、および記号を、改めて今回歩容に対応させる。

この実施例では、ＺＭＰ軌道パラメータを修正することとする。その他の歩容パラメータを修正しても良いが、通常、パラメータの修正量と終端発散成分の関係が非線形になるので、解析的に修正量を決定することはできない。

前記した仮決定された目標ＺＭＰ（これを「仮目標ＺＭＰ」と呼ぶ）に、ＺＭＰ修正量（補正量あるいは付加量）を加えることにより、ＺＭＰを修正する。修正されたＺＭＰを改めて目標ＺＭＰという。これらの関係を図１６に示す。

同図に示す如く、目標ＺＭＰ（ZMPtotal）＝仮目標ＺＭＰ＋ＺＭＰ修正量、となるように、目標ＺＭＰパラメータを修正する。

以降は、説明を簡単にするために、すべての変数を１次元の世界で扱う。

ＺＭＰ修正量は図１６中段に示すように台形状であり、その高さをａとする。目標ＺＭＰパターンによって発生する終端発散成分Ｗzmptotalは、目標ＺＭＰと終端発散成分の関係が線形であることから、以下の数式３３に示すように表すことができる。

ここで、Ｗzmptmpは、今回歩容の仮目標ＺＭＰパターンによって発生する終端発散成分である。Ｗtrimは、ａ＝１とした場合のＺＭＰ修正量によって発生する終端発散成分である。これらは、前記定常旋回歩容の目標ＺＭＰパターンによって発生する終端発散成分を求める方法によって、解析的に求められる。

終端発散成分p[k]がp"であること、および、数式２７と数式３３より、次の数式３４を得る。

ただし、ｑ［０］は、今回歩容の初期発散成分（初期倒立振子位置＋（初期倒立振子速度／ω０））である。したがって、次の数式３５に示すようになる。

最後に、ＺＭＰ修正量を仮目標ＺＭＰに加算したものが目標ＺＭＰとなるように、目標ＺＭＰのパラメータを修正すれば、今回歩容の終端発散成分が、定常旋回歩容の初期発散成分を今回歩容から見た値ｑ" に一致する。

図１２フロー・チャートの説明に戻ると、次いでＳ３０に進み、今回歩容を修正する。具体的には、今回歩容の終端発散成分が定常旋回歩容の初期発散成分ｑ" に一致するように、今回歩容のパラメータを修正する。

図１７はその処理を示すサブルーチン・フロー・チャートである。

以下説明すると、Ｓ２００において今回歩容の仮目標ＺＭＰパターンによって発生する終端発散成分Ｗzmptmpを求め、次いでＳ２０２に進み、ａ＝１とした場合のＺＭＰ修正量によって発生する終端発散成分Ｗtrimを求める。

次いでＳ２０４に進み、今回歩容の脚運動によって発生する終端発散成分たるＷfeetを求め、次いでＳ２０６に進み、今回歩容の初期発散成分ｑ［０］を初期倒立振子位置と速度から求め、求めた値を数式３５に代入してＺＭＰ補正量の台形高さａを求める。次いでＳ２０８に進み、前記したように目標ＺＭＰのパラメータを修正する。

以上が、歩容の切り変わり目の処理である。

図１２フロー・チャートの説明に戻ると、次いでＳ２０に進み、決定された歩容パラメータから今回歩容の瞬時値を決定する。

図１８はその処理を示すサブルーチン・フロー・チャートである。

以下説明すると、Ｓ３００において、今回歩容パラメータを基に、時刻ｔにおける目標ＺＭＰを求め、Ｓ３０２に進み、今回歩容パラメータを基に、時刻ｔにおける目標両足平位置姿勢を求める。

次いでＳ３０４に進み、今回歩容パラメータを基に、時刻ｔにおける目標上体姿勢を求め、Ｓ３０６に進み、数式１および数式２を用いて、時刻ｔおよびそれ以前の目標足平位置姿勢から、時刻ｔにおける脚ＺＭＰ（ZMPfeet ）を算出する。

次いでＳ３０８に進み、数式３を用いて、倒立振子ＺＭＰ(ZMPpend) を算出し、Ｓ３１０に進み、数式４を用いて、倒立振子ＺＭＰから倒立振子水平位置を算出する。次いでＳ３１２に進み、倒立振子水平位置から上体の水平位置を決定する。具体的には、前述したように、上体代表点の水平位置が、倒立振子質点水平位置に一致するように決定する。

次いでＳ３１４に進み、本出願人が先に特開平１０−８６０８０号公報で提案した上体高さ決定手法を用いて上体高さを決定する。

図１２フロー・チャートの説明に戻ると、次いでＳ３２に進み、時刻ｔにΔｔを加え、再びＳ１４に戻り、上記の処理を繰り返す。

図４を参照してこの実施例に係る歩容生成装置の動作をさらに説明すると、歩容生成部１００において、上記したように目標歩容が生成される。生成された目標歩容の歩容パラメータの中、目標上***置姿勢（軌道）は、ロボット幾何学モデル（逆キネマティクス演算部）１０２に直接送られる。

また、その他の目標足平（足部）位置姿勢（軌道）、目標全床反力中心点（即ち、目標ＺＭＰ）軌道、および目標全床反力（軌道）は、複合コンプライアンス動作決定部１０４に直接送られる一方、目標床反力分配器１０６にも送られ、そこで床反力は各足平（足部２２Ｒ，Ｌ）に分配され、目標各足平床反力中心点および目標各足平床反力が決定されて複合コンプライアンス動作決定部１０４に送られる。

複合コンプライアンス動作決定部１０４から、機構変形補償付き修正目標足平位置姿勢（軌道）がロボット幾何学モデル１０２に送られる。ロボット幾何学モデル１０２は、目標上***置姿勢（軌道）と機構変形補償付き修正目標足平位置姿勢（軌道）を入力されると、それらを満足する１２個の関節（１０Ｒ（Ｌ）など）の関節変位指令（値）を算出して変位コントローラ１０８に送る。変位コントローラ１０８は、ロボット幾何学モデル１０２で算出された関節変位指令（値）を目標値としてロボット１の１２個の関節の変位を追従制御する。

よってロボット１に生じた床反力は実各足床反力検出器１１０（６軸力センサ３４）によって検出される。検出値は前記した複合コンプライアンス動作決定部１０４に送られる。また、ロボット１に生じた姿勢傾斜偏差θerrx, θerryは傾斜センサ３６によって検出され、検出値は姿勢安定化制御演算部１１２に送られ、そこで姿勢傾斜を復元する目標全床反力中心点まわり補償全床反力モーメントが算出されて複合コンプライアンス動作決定部１０４に送られる。複合コンプライアンス動作決定部１０４は、入力値に基づいて目標値を修正する。

尚、この発明の要旨は歩容生成部１００におけるロボットの歩容生成にあり、上記した複合コンプライアンス動作決定部１０４などの構成および動作は、本出願人が先に出願した特開平１０−２７７９６９号公報などに詳細に記載されているので、説明は以上に止める。

この実施例に係る脚式移動ロボットの歩容生成装置は上記の如く構成したので、本出願人が先に特開平１０−８６０８１号において提案した技術の上記した不都合を解消し、脚式移動ロボットの床反力を含む歩容を、動力学的平衡条件をほぼ満足しながら、自在かつリアルタイムに生成して任意の歩幅、旋回角、歩行周期などの歩容を生成することができると共に、生成された歩容同士の境界においてロボットの各部位の変位および速度が連続な歩容を生成することができる。

即ち、図１９は上体代表点のＹ軸方向（左右方向）の軌道を示すタイム・チャートであるが、この実施例に係る脚式移動ロボットの歩容生成装置にあっては、同図に符合Ｂで示すように、上体代表点の軌道が定常旋回歩容の上体代表点の軌道Ａに漸近（収束）するような歩容を生成することができる。

さらに、脚式移動ロボットの動力学モデルを単純化して線形度を高めたので、発散などのロボットの将来の挙動を予測して歩容を生成することができる。

また、それによって歩行以外の動作もリアルタイムに生成できるようになるので、歩行以外の任意の動作と歩行を連続的に滑らかにつなげることも可能となる。

尚、搭載するＣＰＵ（第１の演算装置６０）の能力が格段に高いことを前提とすれば、３質点モデルに代え、図２３に示したような全リンクに質点を設定した、換言すれば非線形なモデルを用いて歩容を生成することも可能となる。以下、その構成をこの発明の参考例として記載する。

図２０は、その参考例に係る脚式移動ロボットの歩容生成装置の動作の一部を示す、第１実施例の図１５に類似する、定常旋回歩容の初期発散成分の算出作業のサブルーチン・フロー・チャートである。

参考例に係る装置の動作を説明すると、第１実施例の図１２においてＳ１０からＳ２６までに示すのと同様の処理を行ってＳ２８に進んだ後、そのサブルーチン・フロー・チャートである図２０に進み、定常旋回歩容の初期発散成分を算出する。

図２０フロー・チャートにおいては、先ずＳ４００において初期上***置速度候補（Ｘ０，Ｖ０）を仮決めする。上***置速度は、上体代表点の位置と速度を意味する。尚、説明を簡略にするため、Ｘ方向だけを探索する場合を例にとるが、実際には位置速度ともＸ，Ｙ方向を別々にあるいは同時に探索する必要がある。Ｘ，Ｙ方向を同時に探索するときは、探索空間がＸ位置、Ｙ位置、Ｘ速度、Ｙ速度の４次元になる。

次いでＳ４０２を経てＳ４０４に進み、動力学モデル（図２３に示したような全リンクに質点を設定した非線形なモデル）を用いて（Ｘ０，Ｖ０）を上体の初期状態として定常旋回歩容パラメータを基に、時刻０から終端時刻までの歩容を生成する。

具体的には、目標ＺＭＰと目標足平位置姿勢と目標上体姿勢を求め、（Ｘ０，Ｖ０）を上体の初期位置速度とし、動力学モデルを用いて目標ＺＭＰを満足する上体軌道を生成する。第１実施例と同様に、軌道生成は歩容生成部１００の内部で行われるだけであって、出力されることはない。

次いでＳ４０６に進み、生成した歩容の終端上***置速度を次ぎの１歩の支持脚座標系（図１４のＸ’”，Ｙ’”座標系と同様）から見た値に変換し、その値を（Ｘｅ，Ｖｅ）とする。

次いでＳ４０８に進み、図示の如く、（Ｘ０，Ｖ０）と（Ｘｅ，Ｖｅ）の差（errx,erry ）を算出する。即ち、定常旋回歩容では（Ｘ０，Ｖ０）と（Ｘｅ，Ｖｅ）が一致していなければならないことから、両者の差（errx,erry ）が０となるように探索することを考える。

次いでＳ４１０に進み、算出した差が、適宜設定する許容範囲内にあるか否か判断する。尚、このように位置と速度の境界条件ずれの許容値を設定する代わりに、初期発散成分（Ｘ０＋Ｖ０／ω０）と初期収束成分（Ｘ０−Ｖ０／ω０）がそれぞれある許容範囲内にあるか否か判断するようにしても良い。

Ｓ４１０で否定されるときはＳ４１２に進み、（Ｘ０，Ｖ０）の付近に複数の初期値候補（Ｘ０＋ΔＸ０，Ｖ０），（Ｘ０，Ｖ０＋ΔＶ０）を決め、それぞれを上体の初期状態として定常旋回歩容の歩容パラメータを基に歩容を生成し、生成した歩容の終端上***置速度を次ぎの１歩の支持脚座標系から見た値に変換した値（Ｘｅ＋ΔＸｅ１，Ｖｅ＋ΔＶｅ１），（Ｘｅ＋ΔＸｅ２，Ｖｅ＋ΔＶｅ２）を求める。

次いでＳ４１４に進み、（Ｘ０，Ｖ０）およびその付近の初期値候補に対する終端上***置速度と前記した差を基に、次ぎの初期値候補（Ｘ０，Ｖ０）を決定する。

Ｓ４１０で否定される限りは上記した処理を繰り返すと共に、肯定されるときは繰り返しループ（Ｓ４０２）を抜けてＳ４１６に進み、定常旋回初期発散成分ｑ［０］を図示の式から求める。次いでＳ４１８に進み、第１実施例の図１５のＳ１０６と同様な処理を行う。

尚、参考例にあっては、第１実施例で用いたような線形な３質点モデルを用いていないが、発散成分および収束成分の概念は、図２３に示すような非線形なモデルの挙動の摂動分に対しても近似的に十分な精度で適用することができるので、第１実施例と同様な式を用いて発散成分を定義することとする。

次いで図１２と同様のフロー・チャートにおいてＳ３０に進み、今回歩容の歩容パラメータを修正する。

図２１はその処理を示すサブルーチン・フロー・チャートである。

以下説明すると、Ｓ５００を経てＳ５０２に進み、仮目標ＺＭＰパターンとその他の今回歩容の歩容パラメータを基に、終端時刻までの今回歩容を計算することによって今回歩容終端での上***置速度（Ｘｅ，Ｖｅ）を求め、さらに終端発散成分ｑ［０］を図示の式を用いて求める。

次いでＳ５０４に進み、終端発散成分誤差errqを図示の式を用いて求め、Ｓ５０６に進んで求めた誤差が許容範囲内にあるか否か判断し、否定されるときはＳ５０８に進み、ａ＝Δａとし、第１実施例と同様に図１６の関係に従って仮目標ＺＭＰパターンに修正を加えた目標ＺＭＰを基に、終端時刻までの今回歩容を計算することによって、今回歩容終端での上***置速度（Ｘｅ，Ｖｅ）を求め、さらには終端発散成分ｑ１［ｋ］を図示の式を用いて求める。

尚、Ｓ５０８においてΔａは適宜に設定される微小量の定数である。繰り返し演算によって誤差errqが小さくなるにつれてΔａも減少するように設定しても良い。ただし、定数としても、数回の繰り返し演算で誤差は許容範囲内になる。

次いでＳ５１０に進み、パラメータ感度ｒを図示の式から求め、Ｓ５１２に進み、図示のように求めた修正量を仮目標ＺＭＰパターンに加え、仮目標ＺＭＰパターンを修正する。

Ｓ５０６で否定される限りは上記した処理を繰り返すと共に、肯定されるときは繰り返しループ（Ｓ５００）を抜けてＳ５１４に進み、最終的に得られた仮目標ＺＭＰパターンの値を目標ＺＭＰと決定する。

上記を説明すると、動力学モデルを用いて今回歩容パラメータに従って歩容を生成し、そのまま連続して定常旋回歩容パラメータに従って歩容を繰り返し生成したとき、生成した歩容が定常旋回歩容に収束するためには、第１実施例と同様に、今回歩容の終端発散成分が、定常旋回初期発散成分ｑ［０］を今回歩容の支持脚座標系から見た値であるｑ”に、完全にあるいはほぼ一致していなければならない。

そこで、参考例においても、その条件を満足するように、今回歩容の歩容パラメータの中の目標ＺＭＰパターンを修正するようにした。尚、参考例にあっても、定常旋回歩容との境界における上***置速度が一致するように今回歩容パラメータを修正しても良い。ただし、発散成分に関する境界条件だけを満足する場合に比較すると、条件が厳しくなり、修正すべきパラメータを２倍に増やすこととなって探索空間が増大する。また、上記したように、目標ＺＭＰを修正して上***置速度を一致させる場合には、ＺＭＰ修正量の波形も、図１６に示すような単純な台形形状に止まらず、２段式の台形形状のような複雑な形状としなければならず、修正量も大きくなりがちである。図１９を再び参照して説明すると、前述したように、同図に符合Ａで示す軌道は、定常旋回歩容との境界における上体代表点の位置速度が一致するように今回歩容を生成し、次いで定常旋回歩容を生成した場合の上体代表点軌道を指す。符号Ｂで示す軌道は、今回歩容の終端発散成分がｑ”に一致するように今回歩容を生成し、次いで定常旋回歩容パラメータを用いて連続する歩容を生成し続けた場合の上体代表点軌道を示す。

図示の如く、符合Ｂで示す軌道は、今回歩容と最初の定常旋回歩容との境界においては、通常、符号Ａで示す軌道からずれているが、その後、徐々に符号Ａで示す軌道に収束（漸近）し、次の定常旋回歩容時期で符号Ａで示す軌道にほぼ一致する。このように、発散成分のみを一致させる歩容生成手法でも、位置速度までも一致させる歩容生成手法と同様、歩容の発散を防止する効果を得ることができる。同図に符合Ｃで示す例は、それらを考慮せずに軌道を生成した場合を示す。このような場合は、生成した軌道は経時的に符合Ａで示す軌道から発散してしまう。尚、位置速度までも一致させる歩容生成手段は、換言すれば、発散成分のみならず、収束成分も一致させる歩容生成手法である。したがって、位置速度までも一致させる歩容生成手法は、発散成分のみを一致させる歩容生成手法の特殊例であると言うことができる。

尚、目標ＺＭＰ以外のパラメータを修正しても良い（これは第１実施例でも同様である）。また、説明を簡略化するために、Ｘ方向だけの１次元探索としたが、実際には、定常旋回歩容の上***置速度の初期値探索と同様に次元が増加する。

尚、参考例においては、疑似的なニュートン法（あるいは（Ｘ０，Ｖ０）での疑似的なヤコビアン（感度マトリックス）を求めて次ぎの候補を決定する手法）を用いるようにしたが、シンプレックス法などの探索法を用いても良い。さらには、予め種々のパラメータに対して探索計算を行ってマップ化しておいても良い。

さらに、参考例で述べた非線形モデルを用いて探索的に歩容を求める手法は、第１実施例の解析的に歩容を求めるのに使用した３質点モデル（あるいは１質点モデル）などの線形モデルを用いても良いことは言うまでもない。ただし、解析的に歩容を求めることができるので、非効率な探索法を用いることの意義は少ない。その他、実質的に十分な精度があるならば、どのような動力学モデルを用いても良い。

第１実施例あるいは参考例において、想定した定常旋回歩容通りに歩容を生成し続けているときには、ＺＭＰ修正量が０となり、今回歩容として決定される目標ＺＭＰは、想定した定常旋回歩容のＺＭＰ（仮目標ＺＭＰ）と一致する。ところで、定常旋回歩容のＺＭＰ（仮目標ＺＭＰ）は通常、安定余裕が高くなるように設定される。したがって、想定した定常旋回歩容通りに歩容を生成し続けている場合には、今回歩容の安定余裕も高い。

それに対し、今回歩容を１歩前に想定した定常旋回歩容と大きく異なるように生成しようとすると、ＺＭＰ修正量の絶対値が大きくなるので、今回歩容として決定される目標ＺＭＰ軌道は理想的な軌道から大きくずれ、安定余裕が低くなる。例えば、右に旋回しようとするときは右に重心をずらすように歩容が生成されるが、その次ぎの１歩で急に左旋回するような要求が生じた場合、重心を右から左に移動させるために目標ＺＭＰ軌道を大きく右にずらす必要が生じ、安定余裕が低下して右に倒れやすくなる。

以上の性質から、想定した定常旋回歩容は、将来実際に生成されるであろう歩容になるべく近くなるように設定すべきである。

定常旋回歩容で歩き続けることは特殊なことであり、一般的には、定常旋回歩容からずれた歩容が将来生成されるが、上記における定常旋回歩容設定手法は、将来実際に生成されるであろう歩容になるべく近くなるように設定する例である。

上記以外の定常旋回歩容の設定手法を、以下の１）から４）までに示す。４）を除くと、将来実際に生成されるであろう歩容と定常旋回歩容のずれは、上記で得るよりも大きくなる傾向があるが、演算が容易になる。

１）次次回支持脚座標系と次次回歩行周期は、今回支持脚座標系、今回歩行周期およびそれまでの歩行履歴を基に、生成したい歩容の移動速度や旋回速度が急激に変化しないであろうということを前提にして予想する、あるいは外挿的に求めしても良い。

２）定常旋回歩容を常に直線歩容にしても良い。ただし、旋回速度が大きくなるにつれ、ＺＭＰ修正量が増加する。即ち、安定余裕が低下する。

３）定常旋回歩容を常に静止歩容あるいはその場足踏み歩容にしても良い。ただし、歩行速度が上がるにつれ、ＺＭＰ修正量が増加する。尚、静止歩容とは、動きのない静止状態の歩容を意味する。

４）今回歩容と定常旋回歩容の間に過渡期歩容を追加しても良い。挙動予測演算が複雑になるが、定常旋回歩容を決定するときの制約条件（境界条件）が緩くなり、より望みに近い定常旋回歩容が設定できるので、より一層安定余裕を高めることができる。

さらには、３）と４）を組み合わせることも可能である。

次いで述べるこの発明の第２実施例に係る脚式移動ロボットの歩容生成装置は、その点に焦点をおいたものである。

図２２は、その第２実施例に係る装置の動作の一部を示す、第１実施例の図１３に類似する、定常旋回歩容の別の設定例を示す説明図である。即ち、図２２に示す如く、定常旋回歩容が静止歩容を含むように構成すると共に、今回歩容と静止歩容（定常旋回歩容）の間に過渡的な歩容（過渡歩容。定常旋回歩容以外の歩容を意味する）が挿入するようにした。

このように、今回歩容、過渡歩容、静止歩容からなる３つの歩容を設定することによって、姿勢の発散を効果的に防止することができる。また、いつでも静止状態に移行できるように歩容が生成されるので、急に停止要求が与えられても、安定余裕の高い停止動作が可能となる。

具体的には、静止歩容も定常旋回歩容の１つと考え、前述の定常旋回歩容の初期発散成分算出アルゴリズムを使用することとする。ただし、静止時の上体質点速度が０であることから、発散成分は倒立振子質点位置に一致するので、わざわざこのアルゴリズムを用いなくても容易に求めることができる。

第２実施例は上記の如く構成したので、第１実施例の場合と同様に、安定余裕の高い歩容を自在かつリアルタイムに生成することができる。

第１実施例から第２実施例は上記の如く、少なくとも上体２４と、前記上体に連結される複数本、より具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボット１の歩容生成装置（歩容生成部１００）において、少なくとも前記上体と脚部の運動と、床反力、より具体的にはＺＭＰとの関係を表す動力学モデル（図５および図６に示す）、少なくとも要求（次回歩容指示脚座標系、次次回歩容指示脚座標系、今回歩容周期、次回歩容周期など。Ｓ２２）に応じ、前記脚部の軌道（目標足平位置姿勢軌道）と前記床反力の軌道（目標全床反力中心点軌道（目標ＺＭＰ軌道））を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段（Ｓ２４）、少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容（定常旋回歩容）の前記パラメータを想定（決定）する周期的歩容パラメータ想定手段（Ｓ２６）、前記動力学モデルおよび前記今回歩容のパラメータとそれに続く前記周期的歩容のパラメータに基づいて決定される前記上体の軌道（目標上***置姿勢軌道）が、前記周期的歩容のパラメータから決定される前記周期的歩容の前記上体の軌道に実質的に収束あるいは一致するように、少なくとも前記仮決定された今回歩容のパラメータを修正する今回歩容パラメータ修正手段（Ｓ３０，Ｓ２００からＳ２０８）、および少なくとも前記修正された前記今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段（Ｓ２０，Ｓ３００からＳ３１４）を備えるように構成した。

また、少なくとも上体２４と、前記上体に連結される複数本、具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボット１の歩容生成装置（歩容生成部１００）において、少なくとも前記上体と脚部の運動と、床反力（具体的にはＺＭＰ）との関係を表す動力学モデル（図５および図６に示す）、少なくとも要求（Ｓ２２）に応じ、前記脚部の軌道（目標足平位置姿勢軌道）と前記床反力の軌道（目標全床反力中心点軌道（目標ＺＭＰ軌道））を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段（Ｓ２４）、少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容（定常旋回歩容）の前記パラメータを想定（決定）する周期的歩容パラメータ想定手段（Ｓ２６）、前記周期的歩容の前記パラメータに基づき、前記周期的歩容の前記上体の挙動を前記動力学モデルで近似したときの発散成分の歩容境界での値である、周期的歩容境界発散成分（定常旋回初期発散成分）を決定する周期的歩容境界発散成分決定手段（Ｓ２８，Ｓ１００からＳ１０６）、前記今回歩容のパラメータに基づき、前記動力学モデルを用いて生成される今回歩容の終端における少なくとも前記発散成分（終端発散成分）が完全に一致あるいは実質的に一致するように、前記今回歩容のパラメータ、より具体的には目標ＺＭＰのパラメータを修正する今回歩容パラメータ修正手段（Ｓ３０，Ｓ２００からＳ２０８）、および少なくとも前記修正された前記今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段（Ｓ２０，Ｓ３００からＳ３１４）を備える如く構成した。

また、前記今回歩容と前記周期的歩容の間に過渡的な歩容（過渡歩容）が挿入される如く構成した。

また、前記周期的歩容が静止歩容を含む如く構成した。

また、少なくとも上体２４と、前記上体に連結される複数本、より具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボット１の歩容生成装置（歩容生成部１００）において、前記脚部の運動によって発生する慣性力と重力の合力（総慣性力）である脚部反力、より具体的には脚ＺＭＰ（ZMPfeet)を前記上体の挙動に依存せずに算出する脚部反力算出手段（脚ＺＭＰ算出器２００，Ｓ２０，Ｓ３０６）、少なくとも前記算出された脚部反力と目標床反力、より具体的には目標ＺＭＰとから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置（ZMPpend)を算出する倒立振子モデル支点位置算出手段（Ｓ２０，Ｓ３０８）、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位、より具体的には前記倒立振子質点位置（倒立振子水平位置xb) を算出する倒立振子モデル変位算出手段（Ｓ２０，Ｓ３１０）、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置、より具体的には前記上体の水平位置を算出する上***置算出手段（上***置決定器２０２，Ｓ２０，Ｓ３１２，Ｓ３１４）、および少なくとも前記算出された上***置を用いて歩容、より具体的には今回歩容の瞬時値を生成（算出）する歩容生成手段（Ｓ２０，Ｓ３００からＳ３１４，歩容生成部１００）を備えるように構成した。

また、少なくとも上体２４と、前記上体に連結される複数本、より具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボットの歩容生成装置（歩容生成部１００）において、前記脚部の運動によって発生する慣性力と重力の合力に疑似的に対応する前記脚部のＺＭＰに相当する脚部ＺＭＰ（ZMPpend)を前記上体の挙動に依存せずに算出する脚部ＺＭＰ算出手段（脚ＺＭＰ算出器２００，Ｓ２０，Ｓ３０６）、少なくとも前記算出された脚部ＺＭＰと目標ＺＭＰとから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置（ZMPpend)を算出する倒立振子モデル支点位置算出手段（Ｓ２０，Ｓ３０８）、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位、より具体的には前記倒立振子質点位置（倒立振子水平位置xb) を算出する倒立振子モデル変位算出手段（Ｓ２０，Ｓ３１０）、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置を算出する上***置算出手段（Ｓ２０，Ｓ３１２，Ｓ３１４。上***置決定器２０２）、および少なくとも前記算出された上***置を用いて歩容、より具体的には今回歩容の瞬時値を生成（算出）する歩容生成手段（Ｓ２０，Ｓ３００からＳ３１４、歩容生成部１００）を備える如く構成した。

また、前記倒立振子モデル支点位置算出手段は、前記目標ＺＭＰに第１の係数mtotal/mb を乗じて得た積から、前記脚部ＺＭＰに第２の係数mfeet/mtoal を乗じて得た積を減算して前記倒立振子モデルの支点位置（倒立振子水平位置xb）を算出する如く構成した。

また、少なくとも上体２４と、前記上体に連結される複数本、より具体的には２本の脚部（脚部リンク）２を備えた脚式移動ロボット１の歩容生成装置（歩容生成部１００）において、前記脚部の運動によって発生する慣性力と重力の合力である脚部反力（脚ＺＭＰ(ZMPfeet))を前記上体の挙動に依存せずに算出する脚部反力算出手段（脚ＺＭＰ算出器２００，Ｓ２０，Ｓ３０６）と、少なくとも前記算出された脚部反力と目標床反力（目標ＺＭＰ）とから、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置（倒立振子ＺＭＰ（ZMPpend)) を算出する倒立振子モデル支点位置算出手段（Ｓ２０，Ｓ３０８）と、少なくとも前記算出された倒立振子モデルの支点位置に基づいて前記倒立振子モデルの変位、より具体的には位置（倒立振子水平位置）を算出する倒立振子モデル変位算出手段（Ｓ２０，Ｓ３１０）と、少なくとも前記算出された倒立振子モデルの変位に基づいて前記上体の位置（上体の水平位置）を算出する上***置算出手段（上***置決定器２０２，Ｓ２０，Ｓ３１２，Ｓ３１４）とから構成される動力学モデル、少なくとも要求（Ｓ２２）に応じ、前記脚部の軌道（目標足平位置姿勢軌道）と前記床反力の軌道（目標全床反力中心点軌道（目標ＺＭＰ軌道））を決定するパラメータを少なくとも含む、今回歩容のパラメータを仮決定する今回歩容パラメータ仮決定手段（Ｓ２４）、少なくとも前記要求に応じ、前記今回歩容に続く周期的歩容（定常旋回歩容）の前記パラメータを想定する周期的歩容パラメータ想定手段（Ｓ２６）、前記周期的歩容の前記パラメータに基づき、前記周期的歩容の境界における前記倒立振子モデルの発散成分である周期的歩容境界発散成分（定常旋回歩容の初期発散成分）を決定する周期的歩容境界発散成分決定手段（Ｓ２８，Ｓ１００からＳ１０６）、前記今回歩容のパラメータに基づき、前記動力学モデルを用いて生成される今回歩容の終端における前記倒立振子モデルの発散成分が前記周期的歩容境界発散成分（今回歩容の終端発散成分）に完全に一致または実質的に一致するように、前記今回歩容のパラメータを修正する今回歩容パラメータ修正手段（Ｓ３０，Ｓ２００からＳ２０８）、および少なくとも前記修正された今回歩容のパラメータに基づいて前記今回歩容の瞬時値を決定する今回歩容瞬時値決定手段（Ｓ２０，Ｓ３００からＳ３１４）を備える如く構成した。

また、前記床反力が少なくともＺＭＰを含む如く構成した。前記（図５および図６に示す）倒立振子モデルが線形モデルである如く構成した。

次いで、脚ＺＭＰ（ZMPfeet)の算出について付言する。

脚ＺＭＰ（ZMPfeet)を求める関数は、以下の条件を満たすことが望ましい（第１実施例では、これら全ての条件を満足している）。

条件ａ）足平軌道を決定する歩容パラメータの全てまたは一部を基に決定されること。
条件ｂ）上体代表点の運動の影響を受けないこと。
条件ｃ）単純化モデルの近似精度が十分に高いこと。
条件ｄ）可能な限り連続であること。（上体加速度を連続にするため。）

これらの内、条件ａ，ｂは予測演算を容易にするための条件である。上記では、数式１と数式２から脚ＺＭＰを求めているが、数式１と続いて述べる数式３６および数式３７を用いても良い。

ただし、Ｃは定数であり、また、両脚重心加速度ベクトルafeet が数式３７によって求められ、数式３６の両脚重心鉛直方向加速度ベクトルafeetzは、afeetのＸ，Ｙ成分を０にしたベクトルである。

数式３６は、数式２よりも調整自由度が高いので、近似精度を高くすることができる。また、点Ｐまわりの脚総慣性力モーメントを求める数式１において、脚加速度に関する項を故意に定数倍するなど、力学的には正しくない式に変更しても、最終的に単純化モデルの近似精度が高くなるのであれば、差し支えない。

続いて、作用点Ｐの設定に関して付言する。

作用点Ｐは次のように設定することも可能である。
１）今回歩容の支持脚座標系原点あるいはその近傍の所定の点に作用点Ｐを設定する。
２）両脚支持期の間に直前歩容の支持脚座標系原点あるいは近傍の所定の点から今回歩容の支持脚座標系原点あるいは近傍の所定の点に、連続的に移動するように作用点Ｐを設定する。
３）作用点Ｐを仮目標ＺＭＰ軌道に一致させる。

以下、それぞれの特徴を解説する。

１）については、作用点Ｐが脚位置に対して相対的に不連続（即ち、床に設定された絶対座標系から見て不連続）になるので、ZMPfeet も脚位置に対して相対的に不連続（即ち、前記した絶対座標系から見て不連続）になる欠点がある。しかしながら、両足平の加速度があまり大きくない両脚支持期に座標系を切り替える限り、不連続の大きさは、歩行上、あまり問題にならない。

２）については、近似精度は、１）よりも良い。また、作用点Ｐが脚位置に対して相対的に連続になるので、ZMPfeet も脚位置に対して相対的に連続になる。

３）については、２）の手法よりもさらに近似精度が高くなる。また２）と同様に、ZMPfeet も脚位置に対して相対的に連続になる。

続いて、脚総慣性力モーメントの算出について付言する。

各足平質点軌道は、足平運動パラメータから決定される軌道であるが、各足平質点軌道を求める際に用いる足平位置姿勢の軌道は、歩容生成部１００から出力される目標歩容の足平位置姿勢と一致していなくても良い。

たとえば、目標歩容の足平が回転する場合であっても、各足平質点軌道を求める際に用いる足平位置姿勢の軌道は、足平を水平姿勢を保ったまま（即ち、べた足）であっても良い。精度が著しく低下しない限り、持ち上げ量等、他のパラメータを変えた軌道にしても良い。

また、脚質点は次のように設定しても良い。
１）足平内の所定の点
２）上記点から鉛直方向に一定距離ずらした点
３）足平内に複数の質点を設定しても良い。また足平にイナーシャを設定しても良い。

上記いずれの手法も、前記条件ｂ）を満足している。

上体および脚の質点オフセット設定について付言すると、上体および脚の質点オフセット設定値は、固定値でもほとんど問題ないが、重心位置のモデル化誤差をできるかぎり小さくするために、歩容パラメータに応じて補正されるようにしても良い。また、前記したような倒立振子の水平位置を上体代表点水平位置に対応させる代わりに、ロボットの全重心位置あるいは一部の重心位置（脚部の全部あるいは一部の質量分布を除く、ロボットの質量分布の重心位置）に対応させても良い。

また、倒立振子の高さに関しては、歩容パラメータに応じて変更しても構わないが、将来挙動予測を容易にするために、短期的（例えば１歩の期間）には一定値にするのが望ましい。

さらに、脚反力によって発生する発散成分の算出について付言すると、脚反力によって発生する発散成分を、制御周期毎の脚軌道を生成せずに、歩容パラメータから近似的に直接求めるようにしても良い。

例えば、脚ＺＭＰが、一歩の間、歩容初期の両脚重心位置と歩容終端での両脚重心位置の平均値にあると近似すれば、この平均値にH(1,T)を乗じたものが、脚反力によって発生する発散成分を時刻０のインパルスに換算したものの近似値になる。

脚運動によって発生する終端発散成分は、歩容パラメータ（特に脚運動パラメータと時間パラメータ）を入力とする関数になっている。したがって、歩容パラメータと脚運動によって発生する終端発散成分の関係を表わすマップを作成しておけば、演算量を減らすこともできる。ただし、必要なメモリ容量が増加する。いずれの手法を用いても良い。

上記した変形例を組み合わせることも可能である。例えば、ＺＭＰ修正値を求めるための（将来挙動予測時の）脚ＺＭＰ算出手法と、今回制御周期の瞬時値を求めるときの脚ＺＭＰ算出法は厳密に一致していなくても良い。少し差があっても、ＺＭＰ修正値を求めるときに初期発散成分の影響が考慮されているので、前記差によって、発散成分がずれても、１歩後に再び歩容を設計するときに、発散しないように補正されるからである。

尚、上記した第１実施例の図１２フロー・チャートにおいて、ｔ＝０のときに歩容を修正または変更したが（Ｓ１０）、それ以外の時点で修正または変更しても良い。そのときは現在時刻を今回歩容の初期時刻とみなせば良い。即ち、今回歩容の期間が現在時刻から今回歩容終端時刻までであるとすれば良い。

尚、上記において、図６に示したブロック線図は、演算処理順序を変えるなどの等価変形をしても良い。

また、この発明を２足歩行ロボットに関して説明してきたが、３足以上の多脚ロボットにも応用することができる。

この発明の第１実施例に係る脚式移動ロボットの歩容生成装置を全体的に示す概略図である。 図１に示す脚式移動ロボットの足部の構造を示す説明断面図である。 図１に示す制御ユニットの詳細を示すブロック図である。 図１に示す脚式移動ロボットの歩容生成装置の構成を機能的に示すブロック図である。 図４に示す構成の中の歩容生成部で使用する、図１に示す脚式移動ロボットを倒立振子で近似して得た動力学モデルを示す説明図である。 図５に示す動力学モデルを用いて図４に示す歩容生成部が行う動力学演算を示すブロック図である。 図５に示す動力学モデルの倒立振子の支点位置を示すＺＭＰ相当値ZMPpend を演算するのに用いる、脚部の質点の慣性力と重力の合力の作用点Ｐの軌跡を示すタイム・チャートである。 図１に示す脚式移動ロボットにおいて上体軌道が発散した場合を示す説明図である。 図５に示す動力学モデルの倒立振子の支点位置を示すＺＭＰ相当値ZMPpend を離散関数化して示すタイム・チャートである。 同様に図５に示す動力学モデルの倒立振子の支点位置を示すＺＭＰ相当値ZMPpend の軌道を波形で示すタイム・チャートである。 図１０に示すＺＭＰ相当値ZMPpend の波形を分解して示すタイム・チャートである。 図１に示す脚式移動ロボットの歩容生成装置の動作を示すフロー・チャートである。 図１２フロー・チャートで使用する定常旋回歩容を着地位置などから説明する説明図である。 同様に、図１２フロー・チャートで使用する定常旋回歩容の上体軌道を着地位置などから説明する説明図である。 図１２フロー・チャートの定常旋回歩容の初期発散成分の算出作業を示すサブルーチン・フロー・チャートである。 図１５フロー・チャートで算出される初期発散成分に基づいて図１２フロー・チャートにおいて今回歩容の歩容パラメータである目標ＺＭＰを修正する動作を説明するタイム・チャートである。 図１２フロー・チャートの初期発散成分に基づく今回歩容の歩容パラメータの目標ＺＭＰの修正動作を示すサブルーチン・フロー・チャートである。 図１２フロー・チャートの今回歩容の瞬時値算出作業を示すサブルーチン・フロー・チャートである。 第１実施例に係る脚式移動ロボットの歩容生成装置によって生成される歩容の中の上体軌道を示すタイム・チャートである。 この発明の参考例に係る脚式移動ロボットの歩容生成装置の動作の中の、定常旋回歩容の初期発散成分の算出作業を示す、図１５に類似するサブルーチン・フロー・チャートである。 参考例に係る脚式移動ロボットの歩容生成装置の動作の中の、今回歩容の歩容パラメータの目標ＺＭＰの修正動作を示す、図１７に類似するサブルーチン・フロー・チャートである。 この発明の第２実施例に係る脚式移動ロボットの歩容生成装置で使用される定常旋回歩容の設定例を示す、図１３と同様の説明図である。 脚式移動ロボットである２足歩行ロボットの全リンクに質点を設定した例を示す説明図である。

符号の説明

１ ２足歩行ロボット（脚式移動ロボット）、２ 脚部（脚部リンク）、１０，１２，１４Ｒ，Ｌ 股関節、１６Ｒ，Ｌ 膝関節、１８，２０Ｒ，Ｌ 足関節、２２Ｒ，Ｌ 足平（足部）、２４ 上体、２６ 制御ユニット、３４ ６軸力センサ、３６ 傾斜センサ、４２ コンプライアンス機構、６０ 第１の演算装置、６２ 第２の演算装置、１００ 歩容生成部、２００ 脚ＺＭＰ算出器、２０２ 上***置決定器

Claims (5)

1. 少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、
   ａ．前記脚部の目標運動に基づき、前記脚部の目標運動によって発生する慣性力と重力の合力である脚部反力を算出する脚部反力算出手段、
   ｂ．少なくとも目標床反力と前記脚部反力算出手段が算出した脚部反力とに基づき、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段、
   ｃ．少なくとも前記倒立振子モデル支点位置算出手段が算出した倒立振子モデルの支点位置に基づき、前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段、
   ｄ．少なくとも前記倒立振子モデル変位算出手段が算出した倒立振子モデルの変位に基づき、前記上体の位置を算出する上***置算出手段、
   および
   ｅ．少なくとも前記上***置算出手段が算出した上***置を用いて歩容を生成する歩容生成手段、
   を備えたことを特徴とする脚式移動ロボットの歩容生成装置。
2. 少なくとも上体と、前記上体に連結される複数本の脚部を備えた脚式移動ロボットの歩容生成装置において、
   ａ．前記脚部の目標運動に基づき、前記脚部の目標運動によって発生する慣性力と重力の合力に疑似的に対応する前記脚部のＺＭＰに相当する脚部ＺＭＰを算出する脚部ＺＭＰ算出手段、
   ｂ．少なくとも目標ＺＭＰと前記脚部ＺＭＰ算出手段が算出した脚部ＺＭＰとに基づき、少なくとも前記上体の動力学的挙動を記述する倒立振子モデルの支点位置を算出する倒立振子モデル支点位置算出手段、
   ｃ．少なくとも前記倒立振子モデル支点位置算出手段が算出した倒立振子モデルの支点位置に基づき、前記倒立振子モデルの変位を算出する倒立振子モデル変位算出手段、
   ｄ．少なくとも前記倒立振子モデル変位算出手段が算出した倒立振子モデルの変位に基づき、前記上体の位置を算出する上***置算出手段、
   および
   ｅ．少なくとも前記上***置算出手段が算出した上***置を用いて歩容を生成する歩容生成手段、
   を備えたことを特徴とする脚式移動ロボットの歩容生成装置。
3. 前記倒立振子モデル支点位置算出手段は、前記目標ＺＭＰに第１の係数を乗じて得た積から、前記脚部ＺＭＰに第２の係数を乗じて得た積を減算して前記倒立振子モデルの支点位置を算出することを特徴とする請求項２記載の脚式移動ロボットの歩容生成装置。
4. 前記床反力が少なくともＺＭＰを含むことを特徴とする請求項１記載の脚式移動ロボットの歩容生成装置。
5. 前記倒立振子モデルが線形モデルであることを特徴とする請求項１から４のいずれかに記載の脚式移動ロボットの歩容生成装置。

Images