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

Description

この発明は脚式移動ロボットの姿勢制御装置に関し、より詳しくは脚式移動ロボットの歩容の動作を生成し、さらには生成された動作を追従するようにロボットを制御するようにした姿勢制御装置に関する。

従来、脚式移動ロボットの厳密な動力学モデルを用い、試行錯誤によって歩容を生成していたため、リアルタイムに歩容を生成することは困難であった。そこで、本出願人は先に特許文献１において、オフラインで設計した基準歩容をパラメータと上体軌道の時系列の組にしてロボットに搭載したマイクロコンピュータのメモリに記憶させ、歩行周期など、時間に関するパラメータが同一である複数の歩容の瞬時値の重み付き平均を求めることによって、自在な歩容をリアルタイムに生成する技術を提案している。

また、本出願人は、特許文献２において、単純化モデルに基づいて自在かつリアルタイムに目標歩容を生成する手法を提案している。
特開平１０−８６０８１号 再公表特許ＷＯ２００２／０４０２２４号

しかしながら、特許文献１の提案技術にあっては、近似計算であったことから、生成した歩容が動力学的平衡条件を十分に満足できない場合があった。尚、動力学的平衡条件とは、ロボットの挙動を精密に表す厳密動力学モデルを用いて目標歩容の重力と慣性力から計算されるＺＭＰが、目標ＺＭＰに一致することを意味する。より詳細には、ロボットの挙動を精密に表す厳密動力学モデルを用いて算出されるロボットの慣性力と重力の合力が目標ＺＭＰまわりに作用させるモーメントの水平成分が、０であることを意味する（Ｍ．Ｖｕｋｏｂｒａｔｏｖｉｃ（加藤、山下訳）、『歩行ロボットと人工の足』、日刊工業新聞社（１９７５年））。

また、特許文献２の提案技術にあっては、リアルタイム性を重視して大幅に単純化したモデルを用いて歩容を生成すると、動力学的平衡条件を満足する状態から大幅にずれるようになる。言い換えれば、ＺＭＰの誤差が大きくなる。

したがって、この発明の目的は、生成した歩容による歩行などの動作が動力学的平衡条件を精度良く満足すると共に、その生成された動作に追従するようにロボットを制御することで、姿勢安定性を高めるようにした脚式移動ロボットの姿勢制御装置を提供することにある。

上記した目的を達成するために、請求項１にあっては、少なくとも上体と、前記上体に関節を介して連結される複数本の脚部からなる脚式移動ロボットの姿勢制御装置において、前記ロボットの目標ＺＭＰを決定する目標ＺＭＰ決定手段、前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰを満足するように第１の動力学モデルを用いて目標運動を生成する目標運動生成手段、前記目標運動生成手段によって生成された目標運動に基づき、前記第１の動力学モデルよりも精度が高い第２の動力学モデルを用いて前記目標運動によって第２の動力学モデル上で前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰまわりに発生する床反力モーメントである第２モデル床反力モーメントを算出する第２モデル床反力モーメント算出手段、前記ロボットの実姿勢傾斜偏差を検出する傾斜検出手段、前記傾斜検出手段によって検出された実姿勢傾斜偏差に基づき、前記実姿勢傾斜偏差を解消するために必要な補償全床反力モーメントを決定する補償全床反力モーメント決定手段、および、前記ロボットの運動を前記目標運動生成手段によって生成された目標運動に追従させつつ、前記補償全床反力モーメント決定手段によって決定された補償全床反力モーメントと、前記第２モデル床反力モーメント算出手段によって算出された第２モデル床反力モーメントとの和が、前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰまわりに、前記ロボットの床反力モーメントとして作用するように、前記ロボットの関節のアクチュエータを駆動するアクチュエータ制御手段を備える如く構成した。

請求項２にあっては、少なくとも上体と、前記上体に関節を介して連結される複数本の脚部からなる脚式移動ロボットの姿勢制御装置において、前記ロボットの目標ＺＭＰを決定する目標ＺＭＰ決定手段、前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰを満足するように第１の動力学モデルを用いて目標運動を生成する目標運動生成手段、前記目標運動生成手段によって生成された目標運動を、前記第１の動力学モデルよりも精度が高い第２の動力学モデルに入力することによって第２の動力学モデル上のＺＭＰである第２モデルＺＭＰを算出する第２モデルＺＭＰ算出手段、前記ロボットの実姿勢傾斜偏差を検出する傾斜検出手段、前記傾斜検出手段によって検出された実姿勢傾斜偏差に基づき、前記実姿勢傾斜偏差を解消するために必要な補償全床反力モーメントを決定する補償全床反力モーメント決定手段、および、前記ロボットの運動を前記目標運動生成手段によって生成された目標運動に追従させつつ、前記補償全床反力モーメント決定手段によって決定された補償全床反力モーメントが、前記第２モデルＺＭＰ算出手段によって算出された第２モデルＺＭＰまわりに、前記ロボットの床反力モーメントとして作用するように、前記ロボットの関節のアクチュエータを駆動するアクチュエータ制御手段を備える如く構成した。

請求項１に係る脚式移動ロボットの姿勢制御装置においては、目標ＺＭＰを満足するように第１の動力学モデルを用いて目標運動を生成すると共に、第１の動力学モデルよりも精度が高い第２の動力学モデルを用いてその目標運動によって第２の動力学モデル上で目標ＺＭＰまわりに発生する床反力モーメントである第２モデル床反力モーメントを算出し、検出された実姿勢傾斜偏差を解消するために必要な補償全床反力モーメントを決定し、ロボットの運動を目標運動に追従させつつ、補償全床反力モーメントと第２モデル床反力モーメントとの和が目標ＺＭＰまわりにロボットの床反力モーメントとして作用するようにロボットの関節のアクチュエータを駆動する如く構成したので、動力学的平衡条件を精度良く満足する歩容などの動作を生成することができると共に、歩行時や作業時の安定性を高めることができる。また、動力学的平衡条件を精度良く満足する歩容などの動作を生成することができると共に、その生成された動作を追従するようにロボットを制御することにより姿勢の安定性を高めることができる。

請求項２に係る脚式移動ロボットの姿勢制御装置においても、目標ＺＭＰを満足するように第１の動力学モデルを用いて目標運動を生成すると共に、その目標運動を第１の動力学モデルよりも精度が高い第２の動力学モデルに入力することによって第２の動力学モデル上のＺＭＰである第２モデルＺＭＰを算出し、検出された実姿勢傾斜偏差を解消するために必要な補償全床反力モーメントを決定し、ロボットの運動を目標運動に追従させつつ、補償全床反力モーメントが第２モデルＺＭＰまわりにロボットの床反力モーメントとして作用するようにロボットの関節のアクチュエータを駆動する如く構成したので、同様に、動力学的平衡条件を精度良く満足する歩容などの動作を生成することができると共に、歩行時や作業時の安定性を高めることができる。また、動力学的平衡条件を精度良く満足する歩容などの動作を生成することができると共に、その生成された動作を追従するようにロボットを制御することにより姿勢の安定性を高めることができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

最初にこの装置が生成する歩容について説明すると、このように、ロボット１においては歩行制御に与える目標値として、目標の運動パターンだけではなく、目標の運動パターンに対して動力学的平衡条件を満足する目標床反力パターンも必要である。

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

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

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

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

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

この発明は具体的には、上記に定義した目標歩容を精度良く、かつリアルタイムに生成すると共に、生成した歩容などの動作が動力学的平衡条件を精度良く満足するようにし、よって歩行などの動作の安定性を高めることを目的とする。

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

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

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

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

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

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

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

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

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

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

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

理由２）片脚支持期においてＺＭＰが支持脚足平接地面内になければならないなどの厳しい制約条件があるために、上体の加減速パターンを恣意的に設定することができず、位置を合わせようとすると速度が合わず、速度を合わせようとすると位置が合わず、なかなか両方を同時に一致させることは難しい。

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

運動パラメータは、足平（軌道）パラメータと上体（軌道）パラメータとから構成される。尚、足平軌道パラメータは、初期（離床時）遊脚位置および姿勢、終端（着床時）遊脚位置および姿勢、両脚支持期時間、片脚支持期時間などを含む。

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

図５は、フルモデル補正入り歩容生成部１００の詳細を示すブロック図である。

図示の如く、フルモデル補正入り歩容生成部１００は歩容パラメータ決定部１００ａを備え、歩容パラメータ決定部１００ａは、目標歩容が満たさなければならない、前記した条件を満足するように、パラメータ値あるいは時系列テーブルを決定する。

決定された歩容パラメータは目標（および仮）瞬時値発生部１００ｂに入力される。目標（および仮）瞬時値発生部１００ｂは入力値に基づき、先に本出願人が特開平５−３１８３３９号および特開平５−３２４１１５号公報などで提案した技術を用い、目標足平位置姿勢、目標ＺＭＰおよび目標上体姿勢、より詳しくは、それらの現在時刻ｔにおける目標瞬時値および仮瞬時値を算出（発生）する。尚、姿勢は『空間上の傾斜または向き』を意味する。また、図示の簡略化のために、図５を含む図のほとんどにおいて目標上体姿勢の表示を省略する。

目標（および仮）瞬時値発生部１００ｂで算出（発生）された目標足平位置姿勢、目標ＺＭＰおよび目標上体姿勢（より詳しくは、それらの目標瞬時値および仮瞬時値）は、フルモデル補正部１００ｃに入力される。フルモデル補正部１００ｃは、本出願人が、近時、特許文献２で提案した単純化モデル１００ｃ１と、この出願で提案されるフルモデル１００ｃ２（後述）を備え、単純化モデルに基づいて入力値から目標上体位置（より詳しくは目標上体水平位置）を決定すると共に、さらに決定された目標上体位置から補正目標上体位置を決定する。尚、「補正目標上体位置」と呼んでいるが、後述するように、「目標上体位置」と同一である。

尚、単純化モデル１００ｃ１をフルモデル補正部１００ｃに含ませない構成も可能である。また、フルモデル１００ｃ２は、後述するように、逆フルモデル（逆動力学フルモデル）と順フルモデル（順動力学フルモデル）のいずれかを含む。

フルモデル補正入り歩容生成部１００は、具体的には、単に単純化モデルに基づいて生成した歩容よりも高い精度で動力学的平衡条件を満足するように、補正目標上体位置と目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値を出力する。尚、目標ＺＭＰまわりのフルモデル補正モーメントは、ＺＭＰ換算値とすることなく、モーメントのまま出力しても良い。

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

理解の便宜のため、ここで、本出願人が上記した特許文献２で提案した単純化モデルに基づいて自在かつリアルタイムに目標歩容を生成する手法を説明する。

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

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

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

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

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

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

尚、支持脚が床に全面的に密着しているときの足首位置の床への鉛直投影点を原点とし、床に固定され、支持脚足平の前方向をＸ軸、左方向をＹ軸、鉛直方向をＺ軸にとった座標系を「支持脚座標系」と呼ぶ（これは、上記した支持脚ローカル座標とは異なる。）。ことわらない限り、位置、速度、力などは支持脚座標系で示す。
３）倒立振子は、水平に移動するフリーの支点ａと、ひとつの質点ｂと、支点と質点を結ぶ質量のない可変長のリンクｃから構成される。また、リンクが傾いてもリンクが伸縮し、支点から見た質点高さが一定値に保たれるものとする。

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

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

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

倒立振子のＺＭＰは、支点ａの位置にある。なぜなら、ＺＭＰは定義からモーメントが発生しない点のことであり、フリーの支点ａにはモーメントが発生しないからである。そこで、倒立振子支点位置を倒立振子自身のＺＭＰ位置（ＺＭＰ相当値）ということで、以降、「倒立振子ＺＭＰ」と呼び、「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を決定する。具体的には、上体位置決定器２０２は、前述した上体代表点（図７に示す）の水平位置が、倒立振子の水平位置に一致するように上体の水平位置を決定する。

上記をより一般的に言えば、このモデル（第１のモデル）は、ロボット１を、脚部２の所定位置あるいはその付近に設定された少なくとも１つの質点（慣性モーメントがあっても良い）と、床面上を移動自在な支点と上体２４の所定位置に設定された少なくとも１つの質点に対応する質点からなる倒立振子とでモデル化してなるモデルであると共に、動力学演算部（第１モデル上体位置算出手段）は、少なくとも脚部２の運動によって発生する慣性力と重力の合力に疑似的に対応する脚部のＺＭＰに相当する脚部ＺＭＰZMPfeet を上体２４の挙動に依存せずに算出する脚部ＺＭＰ算出手段と、少なくとも前記算出された脚部ＺＭＰZMPfeetと目標ＺＭＰに基づき、倒立振子の支点のＺＭＰ相当値ZMPpend を算出するＺＭＰ相当値算出手段と、算出された倒立振子の支点のＺＭＰ相当値ZMPpend に基づいて前記倒立振子の変位（位置）xbを算出する倒立振子変位算出手段と、および算出された倒立振子の変位xbに基づいて第１のモデルの上体位置を示すモデル上体位置を決定する第１モデル上体位置決定手段とを備える如く構成した。

また、倒立振子の支点のＺＭＰ相当値ZMPpend が、目標ＺＭＰに第１の係数（mtotal/mfeet) を乗じて得た積から、脚部ＺＭＰに第２の係数（mfeet/motal)を乗じて得た積を減算して算出される如く構成した。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

即ち、上体軌道の発散を防止し、上体と両足平の間の適切な位置関係を継続させるためには、発散成分を歩行に支障ない範囲（姿勢が大きく崩れない範囲）から越えないように、ＺＭＰ軌道パラメータなどを適切に決定すれば良い。

そこで、特許文献２で提案した技術においては、今回生成する歩容につながるべき、長期的な継続性が保証された歩容（後述する定常旋回歩容）を仮に想定することによって、適切な発散成分の値を決定するようにした。

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

このように、特許文献２で提案した技術においては、ロボット１の動力学的挙動を記述する動力学モデルを単純化し、リアルタイムかつ解析的に将来挙動が予測できるように構成し、それによって、床反力（目標ＺＭＰ）を含む歩容を、自由かつリアルタイムに生成して、任意の歩幅、旋回角、歩行周期などを実現できるようにした。

しかしながら、その提案技術においては、リアルタイム性を重視して単純化モデルとして大幅に単純化したモデルを用いると、動力学的平衡条件を満足する状態から大幅にずれるようになる。言い換えれば、ＺＭＰの誤差が大きくなる。

従って、この実施例に係る脚式移動ロボットの姿勢制御装置においては、そのような先に提案した単純化モデルを用いて歩容などの動作を生成するときも、生成された動作が、より一層精度良く動力学的平衡条件を満足するように動作を修正するようにした。尚、この実施例は、特許文献２で提案した単純化モデルを用いて歩容などの動作を生成する場合に止まらず、テーブル化された歩容に基づいて歩容などの動作を生成する場合の動作修正にも妥当する。

図５のフルモデル補正入り歩容生成部１００の説明に戻ると、この実施例で「フルモデル」は、今回歩容パラメータを決定する際に用いるものとは異なるロボット動力学モデルを意味する。これは、今回歩容パラメータを決定する際に用いるものよりも近似精度の高いロボット動力学モデルであることが望ましい。図示例で説明すれば、先の提案技術に係る単純化モデル（図７に示す）を今回歩容パラメータの決定に用いたことから、それよりも、近似精度の高い、例えば図６に示す多質点モデルのようなロボット動力学モデルを意味する（質点のまわりに慣性モーメントを設定するものであっても良い）。

尚、この明細書において、目標足平位置姿勢、目標上体姿勢、目標ＺＭＰに基づいて（入力して）上体位置を算出する（出力させる）のに使用するモデルを「順動力学モデル」と呼び、目標足平位置姿勢、目標上体姿勢、上体位置に基づいて（入力して）目標ＺＭＰを算出する（出力させる）のに使用するモデルを「逆動力学モデル」と呼ぶ。

フルモデル補正部１００ｃが備えるフルモデルは、逆動力学フルモデル（しばしば「逆フルモデル」と略称）または順動力学フルモデル（しばしば「順フルモデル」と略称）を備える。一般的には、逆動力学モデルの演算に比べ、順動力学モデルの演算は、解析的に求めることができないので、探索的に上体位置を求める必要があり、演算量が多くなりがちである。

次いで、この実施例に係る脚式移動ロボットの姿勢制御装置の動作、具体的には、歩容の修正手法を説明する。

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

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

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

次いで、Ｓ２２に進み、今回歩容初期遊脚足平位置姿勢などの今回歩容の歩容パラメータを仮決定（仮に算出）する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

図１１の説明に戻ると、次いでＳ２６に進み、今述べた定常旋回歩容の初期発散成分を求める。尚、その詳細は特許文献２に記載されているので、詳細な説明は省略する。

次いで、Ｓ２８に進み、今回歩容を修正する。具体的には、今回歩容の終端発散成分が定常旋回歩容の初期発散成分に一致するように、今回歩容のパラメータを修正する。これも、その詳細は特許文献２に記載されているので、詳細な説明は省略する。

次いでＳ３０に進み、決定された歩容パラメータから今回歩容の目標瞬時値（および仮目標瞬時値）を発生させる（決定あるいは算出する）。

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

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

次いでＳ１０４に進み、今回歩容パラメータを基に時刻ｔにおける目標上体姿勢を求める。

図１１フロー・チャートの説明に戻ると、次いでＳ３２に進み、フルモデルを用いた補正歩容の発生（歩容の補正）を行う。即ち、図５を参照して説明したように、補正目標上体位置および／または目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値の算出（決定）などを行う。

図１１フロー・チャートのＳ３２の歩容補正手法がこの出願に係る脚式移動ロボットの姿勢制御装置の特徴をなすので、その歩容の補正手法をここで概説する。

図１５は、第１実施例に係る姿勢制御装置の歩容の補正手法を詳細に示す機能ブロック図である。

尚、図１５に示す第１実施例において、歩容の補正は、次式の条件を満足する。

フルモデルＺＭＰ＝目標ＺＭＰ
＋目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値 ．．．式１５

これは、図示のフルモデルが厳密モデルであることを前提とするとき、厳密に動力学的平衡条件を満足するように歩容が補正されることを意味する。このように、フルモデル補正部１００ｃは、目標上体軌道、目標足平軌道および目標ＺＭＰから構成される目標歩容の中の、目標上体軌道を補正する、および／または目標ＺＭＰまわりに目標床反力モーメントを発生させることにより（本来の目標歩容では０）、歩容を補正する。上記で、「フルモデルＺＭＰ」は、逆動力学フルモデル（逆フルモデル）を用いて算出される（から出力される）ＺＭＰを意味する。尚、後述するように、目標上体位置を補正することにより得られる補正目標上体位置は、目標上体位置に一致させられるので、目標上体軌道を補正するといっても、補正量は０である。

図示の如く、第１実施例においては、目標足平位置姿勢、目標上体姿勢（図示省略）および単純化モデルを用いて単純化モデル上体位置（目標上体位置）を算出する（目標上体位置は、図４の「補正目標上体位置」を補正する前の「目標上体位置」を意味する）。

具体的には、数１および数２を用いて時刻ｔおよびそれ以前の目標足平位置姿勢から時刻ｔにおける脚ＺＭＰ（ZMPfeet ）を算出する。次いで、数３を用いて倒立振子ＺＭＰ(ZMPpend) を算出し、数４を用いて倒立振子ＺＭＰから倒立振子水平位置を算出し、上体代表点の水平位置が倒立振子質点水平位置に一致するように上体の水平位置を決定すると共に、本出願人が先に特開平１０−８６０８０号公報で提案した上体高さ決定手法を用いて上体高さを決定する。

さらに、決定した上体位置に基づき、逆フルモデルを用いてフルモデルＺＭＰを算出し、次式に従ってフルモデルＺＭＰ誤差を算出する。

フルモデルＺＭＰ誤差＝フルモデルＺＭＰ−目標ＺＭＰ．．．．．式１５ａ

次いでフルモデルＺＭＰ誤差を目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値として決定（出力）するようにした。尚、この実施例および後述の実施例において、逆フルモデルに入力される目標上体姿勢は、説明の便宜のため、直立姿勢とする。

言い換えれば、単純化モデルのモデル化誤差によって生じる動力学的平衡条件からのずれを、目標ＺＭＰまわりの床反力モーメントによって打ち消す、即ち、脚部２が床面を押す動作を変化させ、目標ＺＭＰまわりの床反力モーメントを発生させて上記のずれを打ち消すようにした。

尚、逆フルモデルを用いて算出するＺＭＰを上記したように、「フルモデルＺＭＰ」という。単純化モデルを用いて算出した上体位置は補正目標上体位置として決定（出力）され、図４に示すロボット幾何学モデル１０３に入力される。

図１５の構成を式で著すと、以下の２つの式のようになる。

補正目標上体位置＝単純化モデル上体位置．．．．．．．．．．．式１６ａ
目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値
＝フルモデルＺＭＰ−目標ＺＭＰ．．．．．．．式１６ｂ

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

図４の説明に戻り、この実施例に係る姿勢制御装置の動作をさらに説明すると、フルモデル補正入り歩容生成部１００で生成された目標歩容の瞬時値の中、目標上体姿勢および補正目標上体位置（軌道）は、後段のブロック１０２に送られ、その中の上記したロボット幾何学モデル（逆キネマティクス演算部）１０３にそのまま入力される。

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

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

ロボット１に生じた実各足床反力は６軸力センサ３４の出力から検出され、検出値は前記した複合コンプライアンス動作決定部１０４に送られる。また、ロボット１に生じた実傾斜角偏差は傾斜センサ３６の出力から検出され、検出値は姿勢安定化制御演算部１１２に送られ、そこで姿勢傾斜を復元するための目標ＺＭＰ（目標全床反力中心点）まわりの補償全床反力モーメントＭdmd が算出される。

また、前記した目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値はモーメント変換部１１４でモーメント値に変換され、補償全床反力モーメントＭdmd に加算される。よって得られた和のモーメントは複合コンプライアンス動作決定部１０４に送られる。複合コンプライアンス動作決定部１０４は、入力値に基づいて目標足平位置姿勢を修正することにより、目標ＺＭＰまわりに上記のように得られた和のモーメントを発生させる。尚、目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値は、上記の如く、目標ＺＭＰまわりの補償全床反力モーメントＭdmd に加算されるので、複合コンプライアンス動作決定部１０４あるいは目標床反力分配器１０６に直接、入力しなくても良い。

尚、図４に破線で示す上記した複合コンプライアンス動作決定部１０４などの構成および動作は、本出願人が先に出願した特開平１０−２７７９６９号公報などに詳細に記載されているので、説明を以上に止める。

この実施例は上記の如く構成したので、先に提案した単純化モデルを用いて生成した歩容を修正して動力学的平衡条件を精度良く満足することができる。また、動力学的平衡条件を精度良く満足する歩容などの動作を生成することができると共に、その生成された動作に追従するようにロボット１を制御することで、姿勢安定性を高めることができる。

さらに、先に提案した特許文献２に記載された自在な歩容の生成方法を組み合わせることにより、脚式移動ロボットの床反力を含む歩容を、動力学的平衡条件を精度良く満足しながら、自在かつリアルタイムに生成して任意の歩幅、旋回角、歩行周期を持つ歩容を生成することができると共に、生成された歩容同士の境界においてロボットの各部位の変位および速度が連続な歩容を生成することができる。

尚、図５に示したフルモデル補正入り歩容生成部１００の構成は、図１６のように変形することができる。

図１６に示す構成は、第１実施例に関する変形例である。

即ち、目標歩容パラメータは単純化モデルを基に作成されているはずであるから、単純化モデルＺＭＰが目標ＺＭＰに一致する手法では、単純化モデル上体軌道は、単純化モデルを基に作成された目標歩容の上体軌道そのものである。従って、これらの手法では、単純化モデル挙動演算部分をフルモデル補正部から分離させることができる。

そこで、図１６に示すように、歩容パラメータ決定部１００ａと目標瞬時値発生部１００ｂと単純化モデルとで単純化モデル歩容生成部１００ｄを構成し、そこで単純化モデルの挙動演算によって目標歩容を生成し、フルモデル補正部１００ｃが、生成された歩容を動力学的平衡条件を十分に満足するものに補正するような構成に置き換えるようにした。これによっても、ブロック図を単に等価変換したに過ぎないから、上記した従前の実施例と同様の効果を得ることができる。

尚、第１実施例において、単純化モデル歩容生成部１００ｄは、特許文献２で提案した技術に示したようなリアルタイム歩容生成装置でなくても良い。テーブル化された歩容を発生するだけでも良い。また、腕を使った作業動作など、歩行ではない動作を生成しても良い。

また、単純化モデル歩容生成部１００ｄによって生成される動作または歩容は、慣性力を無視して静力学的バランスだけを考慮した運動パターンと床反力パターンの組、換言すれば、運動パターンと重心の床投影点の組であっても良い。さらに、動力学的平衡条件を無視した、運動のパターンとＺＭＰ（床反力作用点）パターンの組であっても良い。ただし、動力学平衡条件から極端にずれていると、目標ＺＭＰまわりのフルモデル補正モーメントが過大になるので、動力学平衡条件に近いほど好ましい。

式１５の関係は、姿勢挙動に影響する低い周波数帯域においては満足すべきであるが、高い周波数領域では、大きくずれない限り満足しなくても良い。従って、前記した実施例の構成を示すブロック線図の結線にローパスフィルタなどを新たに追加しても良い。さらには変化率リミッタなどの非線形要素を加えても良い。

上記において、目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値を出力するようにしたが、補正モーメントをそのまま出力しても良い。

また、図４において、目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値を目標ＺＭＰに加えたものを「補正目標ＺＭＰ」とし、かつ目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値を０にして複合コンプライアンス動作決定部１０４に入力しても良い（目標ＺＭＰまわりのフルモデル補正モーメントのＺＭＰ換算値は、フルモデル補正入り歩容生成部から出力せず、複合コンプライアンス動作決定部１０４に入力しないよう構成しても良い）。即ち、フルモデル補正モーメントで補正する代わりに目標ＺＭＰを補正するようにしても良い。ただし、目標ＺＭＰを補正すると、複合コンプライアンス制御において各足平床反力中心点の設定が難しくなるという欠点が生じる。

また、上記した実施例において、（目標）ＺＭＰと表現したが、（目標）ＺＭＰは（目標）床反力の表現の一つの形態であり、それ以外にも、例えば、ある基準点での力とモーメントで表現しても良い。

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

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

この出願においては上記したように、少なくとも上体２４と、前記上体に関節１０，１２，１４Ｒ（Ｌ）を介して連結される複数本の脚部２からなる脚式移動ロボット１の姿勢制御装置において、前記ロボットの目標ＺＭＰを決定する目標ＺＭＰ決定手段（歩容パラメータ決定部１００ａ、目標（および仮）瞬時値発生部１００ｂ）、前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰを満足するように第１の動力学モデル（単純化モデル１００ｃ１）を用いて目標運動を生成する目標運動生成手段（単純化モデル１００ｃ１の演算）、前記目標運動生成手段によって生成された目標運動に基づき、前記第１の動力学モデルよりも精度が高い第２の動力学モデル（フルモデル１００ｃ２）を用いて前記目標運動によって第２の動力学モデル上で前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰまわりに発生する床反力モーメントである第２モデル床反力モーメント（目標ＺＭＰまわりのフルモデル補正モーメント（のＺＭＰ換算値））を算出する第２モデル床反力モーメント算出手段（フルモデル補正部１００ｃ２、図１５の逆フルモデル１００ｃ２の演算、モーメント変換部１１４）、前記ロボットの実姿勢傾斜偏差を検出する傾斜検出手段（傾斜センサ３６）、前記傾斜検出手段によって検出された実姿勢傾斜偏差に基づき、前記実姿勢傾斜偏差を解消するために必要な補償全床反力モーメント（目標ＺＭＰまわりの補償全床反力モーメントＭdmd）を決定する補償全床反力モーメント決定手段（ブロック１０２、姿勢安定化制御演算部１１２）、および、前記ロボットの運動を前記目標運動生成手段によって生成された目標運動に追従させつつ、前記補償全床反力モーメント決定手段によって決定された補償全床反力モーメント（目標ＺＭＰまわりの補償全床反力モーメントＭdmd）と、前記第２モデル床反力モーメント算出手段によって算出された第２モデル床反力モーメント（目標ＺＭＰまわりのフルモデル補正モーメント）との和が、前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰまわりに、前記ロボットの床反力モーメントとして作用するように、前記ロボットの関節のアクチュエータを駆動するアクチュエータ制御手段（複合コンプライアンス動作決定部１０４）を備える如く構成した。

また、少なくとも上体２４と、前記上体に関節１０，１２，１４Ｒ（Ｌ）を介して連結される複数本の脚部２からなる脚式移動ロボット１の姿勢制御装置において、前記ロボットの目標ＺＭＰを決定する目標ＺＭＰ決定手段（歩容パラメータ決定部１００ａ、目標（および仮）瞬時値発生部１００ｂ）、前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰを満足するように第１の動力学モデル（単純化モデル１００ｃ１）を用いて目標運動を生成する目標運動生成手段（単純化モデル１００ｃ１の演算）、前記目標運動生成手段によって生成された目標運動を、前記第１の動力学モデルよりも精度が高い第２の動力学モデル（フルモデル１００ｃ２）に入力することによって第２の動力学モデル上のＺＭＰである第２モデルＺＭＰ（フルモデルＺＭＰ）を算出する第２モデルＺＭＰ算出手段（フルモデル補正部１００ｃ２、図１５の逆フルモデル１００ｃ２の演算、モーメント変換部１１４）、前記ロボットの実姿勢傾斜偏差を検出する傾斜検出手段（傾斜センサ３６）、前記傾斜検出手段によって検出された実姿勢傾斜偏差に基づき、前記実姿勢傾斜偏差を解消するために必要な補償全床反力モーメント（目標ＺＭＰまわりの補償全床反力モーメントＭdmd）を決定する補償全床反力モーメント決定手段（ブロック１０２、姿勢安定化制御演算部１１２）、および、前記ロボットの運動を前記目標運動生成手段によって生成された目標運動に追従させつつ、前記補償全床反力モーメント決定手段によって決定された補償全床反力モーメント（目標ＺＭＰまわりの補償全床反力モーメントＭdmd）が、前記第２モデルＺＭＰ算出手段によって算出された第２モデルＺＭＰまわりに、前記ロボットの床反力モーメントとして作用するように、前記ロボットの関節のアクチュエータを駆動するアクチュエータ制御手段（複合コンプライアンス動作決定部１０４）を備える如く構成した。

また、この発明を２足歩行ロボットに関して説明してきたが、３足以上の多脚ロボットにも応用することができる。さらに、実物のロボット（実機）ではなくても、シミュレーションあるいはコンピュータゲームなどにおける仮想的なロボットの動作制御あるいは動作生成にも応用することができる。

この発明の第１実施例に係る脚式移動ロボットの姿勢制御装置が適用される脚式移動ロボット、より具体的には２足歩行ロボットを全体的に示す概略図である。 図１に示す脚式移動ロボットの足部の構造を示す説明断面図である。 図１に示す制御ユニットの詳細を示すブロック図である。 図１に示す脚式移動ロボットの姿勢制御装置の構成を機能的に示すブロック図である。 図４に示すフルモデル補正入り歩容生成部の構成を機能的に示すブロック図である。 図５に示すフルモデル補正入り歩容生成部のフルモデルの一例である多質点系モデルを機能的に示す説明図である。 図５に示すフルモデル補正入り歩容生成部で使用する、図１に示す脚式移動ロボットを倒立振子で近似して得た単純化モデル（動力学モデル）を示す説明図である。 図７に示す動力学モデルを用いて特許文献２で提案した歩容生成部が行う、動力学演算を示すブロック図である。 図８に示す動力学演算で倒立振子の支点位置を示すＺＭＰ相当値ZMPpend を演算するのに用いる、脚部の質点の慣性力と重力の合力のモーメントの作用点Ｐの軌跡を示すタイム・チャートである。 図１に示す脚式移動ロボットにおいて上体軌道が発散した場合を示す説明図である。 図１に示す脚式移動ロボットの姿勢制置の動作を示すフロー・チャートである。 図１１フロー・チャートで使用する定常旋回歩容を着地位置などから説明する説明図である。 同様に、図１１フロー・チャートで使用する定常旋回歩容を上体軌道などから説明する説明図である。 図１１フロー・チャートの目標瞬時値発生作業を示すサブルーチン・フロー・チャートである。 この発明の第１実施例に係る脚式移動ロボットの姿勢制御装置によって行われる歩容の補正手法を説明する機能ブロック図である。 この発明の第１実施例の等価変形例を示す、図５に類似する機能ブロック図である。

符号の説明

１ ２足歩行ロボット（脚式移動ロボット）、２ 脚部（脚部リンク）、１０、１２、１４Ｒ、Ｌ 股関節、１６Ｒ、Ｌ 膝関節、１８、２０Ｒ、Ｌ 足関節、２２Ｒ、Ｌ 足平（足部）、２４ 上体（基体）、２６ 制御ユニット、３４ ６軸力センサ、３６ 傾斜センサ、４２ コンプライアンス機構、６０ 第１の演算装置、６２ 第２の演算装置、１００ フルモデル補正入り歩容生成部、１００ｃ フルモデル補正部、１００ｃ１ 単純化モデル、１００ｃ２ フルモデル（逆フルモデル、順フルモデル）

Claims (2)

1. 少なくとも上体と、前記上体に関節を介して連結される複数本の脚部からなる脚式移動ロボットの姿勢制御装置において、
   ａ．前記ロボットの目標ＺＭＰを決定する目標ＺＭＰ決定手段、
   ｂ．前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰを満足するように第１の動力学モデルを用いて目標運動を生成する目標運動生成手段、
   ｃ．前記目標運動生成手段によって生成された目標運動に基づき、前記第１の動力学モデルよりも精度が高い第２の動力学モデルを用いて前記目標運動によって第２の動力学モデル上で前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰまわりに発生する床反力モーメントである第２モデル床反力モーメントを算出する第２モデル床反力モーメント算出手段、
   ｄ．前記ロボットの実姿勢傾斜偏差を検出する傾斜検出手段、
   ｅ．前記傾斜検出手段によって検出された実姿勢傾斜偏差に基づき、前記実姿勢傾斜偏差を解消するために必要な補償全床反力モーメントを決定する補償全床反力モーメント決定手段、
   および
   ｆ．前記ロボットの運動を前記目標運動生成手段によって生成された目標運動に追従させつつ、前記補償全床反力モーメント決定手段によって決定された補償全床反力モーメントと、前記第２モデル床反力モーメント算出手段によって算出された第２モデル床反力モーメントとの和が、前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰまわりに、前記ロボットの床反力モーメントとして作用するように、前記ロボットの関節のアクチュエータを駆動するアクチュエータ制御手段、
   を備えたことを特徴とする脚式移動ロボットの姿勢制御装置。
2. 少なくとも上体と、前記上体に関節を介して連結される複数本の脚部からなる脚式移動ロボットの姿勢制御装置において、
   ａ．前記ロボットの目標ＺＭＰを決定する目標ＺＭＰ決定手段、
   ｂ．前記目標ＺＭＰ決定手段によって決定された目標ＺＭＰを満足するように第１の動力学モデルを用いて目標運動を生成する目標運動生成手段、
   ｃ．前記目標運動生成手段によって生成された目標運動を、前記第１の動力学モデルよりも精度が高い第２の動力学モデルに入力することによって第２の動力学モデル上のＺＭＰである第２モデルＺＭＰを算出する第２モデルＺＭＰ算出手段、
   ｄ．前記ロボットの実姿勢傾斜偏差を検出する傾斜検出手段、
   ｅ．前記傾斜検出手段によって検出された実姿勢傾斜偏差に基づき、前記実姿勢傾斜偏差を解消するために必要な補償全床反力モーメントを決定する補償全床反力モーメント決定手段、
   および
   ｆ．前記ロボットの運動を前記目標運動生成手段によって生成された目標運動に追従させつつ、前記補償全床反力モーメント決定手段によって決定された補償全床反力モーメントが、前記第２モデルＺＭＰ算出手段によって算出された第２モデルＺＭＰまわりに、前記ロボットの床反力モーメントとして作用するように、前記ロボットの関節のアクチュエータを駆動するアクチュエータ制御手段、
   を備えたことを特徴とする脚式移動ロボットの姿勢制御装置。