脚式移動ロボットの姿勢安定化制御装置
【構成】 ２足歩行の脚式移動ロボットの姿勢安定化制御装置であって、力学モデルを追従する様にロボットを関節変位すると共に、モデルと実ロボットの上体の傾きに偏差が生じたとき、モデルの床反力をずらして姿勢安定化を図る。
【効果】 足裏を接地させたまま大きな復元力を得ることができる。
【効果】 足裏を接地させたまま大きな復元力を得ることができる。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】この発明は脚式移動ロボットの姿勢安定化制御装置に関し、より具体的には力学モデルを追従する様にロボットが関節変位されると共に、モデルと実ロボットに傾き偏差が生じたとき、実ロボットの床反力をずらす代わりに、モデルの床反力をずらして姿勢の安定を回復する様にしたものに関する。
【０００２】
【従来の技術】脚式移動ロボット、特に２足歩行の脚式移動ロボットとしては、特開昭６２−９７００５号、特開昭６３−１５０１７６号公報記載のものなどが知られている。また脚式移動ロボットを含むロボットの制御については、「ロボット工学ハンドブック」（日本ロボット学会編、１９９０年１０月２０日）に詳しい。
【０００３】
【発明が解決しようとする課題】ところで、脚式移動ロボット、特に２足歩行の脚式移動ロボットは本来的に安定性が低いことから、外乱などを受けると姿勢が不安定となる。そこでロボットの上体の傾き角度と傾き角速度を検出して姿勢制御する必要がある。その意図から本出願人は先に特願平２−３３６，４２０号（平成２年１１月３０日出願）において、着地候補位置を何点か予め用意しておき、一歩ごとの制御周期でいずれかを選択して目標歩容を切り換える制御を提案している。しかし、この制御によるときは、姿勢が崩れた結果、ＺＭＰ（垂直床反力の作用重心点。Zero MomentPoint)が存在可能範囲の限界付近に偏って接地性が失われても、次の一歩で歩容が修正されて姿勢が復元するまではＺＭＰが限界付近に偏ったままなので、それまで接地性を回復できないと言う不都合がある。
【０００４】そこで本出願人は近時別の出願（平成４年４月３０日、整理番号Ａ９２−０１１６）において、ロボットの力学モデルを基に力学的平衡状態が保証される様に床反力を設定した目標歩容を生成し、モデル化誤差や外乱によって傾き角度と傾き角速度に目標値と実際値との間で偏差が生じたときは、実ロボットの接地部の変位あるいはモーメントを基準歩容から故意にずらすことによって実ロボットの床反力をモデルから計算される目標歩容のそれから故意にずらし、実ロボットの傾きをモデルの傾きに近づける復元力を得る技術を提案している（ここで言う「床反力」は、各脚に作用する床反力を全て合成した、ある作用点に働く合力と合モーメントとを意味する）。
【０００５】しかし提案したこの技術によるときは、例えば２足歩行ロボットの片脚支持期では、実ロボットの床反力を故意に大きくずらそうとして足首角度を大きくずらしても、ＺＭＰが足平接地面を超える全床反力を発生することができず、足平の一部が浮いてしまうことがある。従って、実ロボットがモデルに対して大きく傾いてしまった場合には、実ロボットの傾斜を正常に戻すために必要な十分な復元力を得ることができないことがあり、そのときは実ロボットが転倒する。即ち、この技術では目標値と実際値との偏差を縮める復元作用がＺＭＰの存在範囲に制約される。
【０００６】図で説明すると、実ロボットの関節変位をモデルの通りに追従する制御においては、実ロボットの傾き角度と傾き角速度とがそれぞれモデルのそれらと一致していれば、図１に実線で示す実ロボットのＺＭＰはモデルから決定される目標ＺＭＰに一致し、ロボットは所期の姿勢で歩行する。しかし、実際には先に述べた様にモデル化誤差や外乱によって破線で示す様に傾き角度と傾き角速度とに偏差が生じ、しかもそのずれは重力の作用によって偏差が大きくなるほど益々大きくなろうとする発散傾向を持つ。ところで実ＺＭＰが目標ＺＭＰからずれると、図２に示す様に、その間の距離ｘに力Ｆを乗じたモーメントＭが目標ＺＭＰのまわりに生じる。換言すれば、実ＺＭＰが故意に目標ＺＭＰからずれる様に関節変位を制御することにより、所望のモーメントを生じさせることができる。先に提案した技術ではその思想に基づき、傾き（角度と角速度）偏差が生じたときは図２R>２に示す様に、故意に実ＺＭＰをずらして姿勢復元方向にモーメントを生じさせる様にした。しかし、この技術によるときは同図に示す如く、実ＺＭＰをずらすことができる範囲は、足平領域ｘsole内に限られる。
【０００７】従って、この発明の目的は近時提案した技術を改良し、更に効果的な脚式移動ロボットの姿勢安定化制御装置を提案することにある。
【０００８】
【課題を解決するための手段】上記した課題を解決するためにこの発明は例えば請求項１項に示す如く、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態の偏差に応じた操作量を少なくとも前記モデルにフィードバックしてモデルの挙動を修正する様に構成した。
【０００９】
【作用】
【００１０】重力作用によって発散しようとする傾き偏差を零に収束させようとする作用は、実ロボットとモデルの床反力の差によって生じるものである。従って、偏差を収束させるためには、実ロボットの床反力をずらす代わりに、モデルの床反力を反対側にずらすことによっても等価な効果を得ることができる筈である。本発明はこの効果を利用する。そして、それによって図３に示す様に、ずらすＺＭＰがモデルのそれであることから、ＺＭＰをずらす範囲はもはや足平直下の領域内に限られることがなく、足平接地面を遠く超えた位置までずらすことができる。言い換えれば、前記した距離ｘを大きくすることができ、結果的にモーメントを大きくすることができて大きな姿勢復元力を得ることができる。
【００１１】以下、更に説明する。分かり易くするために図４に示す様に、足平と１リンクの脚と上体から構成される単純なロボットを考える。脚の質量をｍ、足首まわりの脚の慣性モーメントをＩ、脚重心の足首との距離をｈとする。上体の質量および慣性モーメントは０とする。また足首の床からの高さも０とする（こうすると、床反力と足首モーメントとが一致して議論が簡単になる）。鉛直方向に対する脚の傾き（即ち、足首と重心とを結ぶ線分の傾き角度）をθlink、鉛直方向に対する上体の傾きをθtrunk 、腰関節の曲げ角をθhip とする。尚、図でθlinkとθtrunk は負、θhip は正とする。更に、足首に作用するモーメントをＭact とする( 足首の床からの高さが０であるから、床反力もＭact である）。重力加速度はｇとする。足首には公知のトルク制御が施されるものとし、腰の曲げ角は変位制御されるものとする。
【００１２】上記において、幾何学的関係から数１の式が成立する。
【００１３】
【数１】
【００１４】このロボットの脚の運動方程式は数２の様になる。
【００１５】
【数２】
【００１６】これをラプラス変換して数３の式を得る。
【００１７】
【数３】
【００１８】さて、ここで、この実ロボットと全く同じ構造と全く同じパラメータ値を持つ数式モデルを用意する。その鉛直方向に対する脚の傾き（即ち、前記と同様な足首と重心とを結ぶ線分の傾き）をθmdllink 、鉛直方向に対する上体の傾きをθmdltrunk、腰関節の曲げ角をθmdlhipとし、モデルの足首に作用するモーメント( 即ち、モデルの床反力) をＭmdl とすると、幾何学的な数式モデルと動力学的な数式モデルは、数４、数５に示す様になる。
【００１９】
【数４】
【００２０】
【数５】
【００２１】次に、モデルを基に目標歩容を作成する。目標歩容は、一般的にはモデルの初期状態、モデルに加えられる操作量、および拘束条件によって表現される。具体的には、脚の初期傾き角度θreflink0と脚の初期傾き角速度ωreflink0、足首モーメントの時間関数Ｍref(t)、および上体の傾き角度の時間関数θreftrunk(t)によって表現される。上体の傾き角度の時間関数θreftrunk(t) は、議論を単純化するために、ここでは常に０であるものとする。
【００２２】次に、以下の制御を実施する。
【００２３】モデルの上体の傾き角度θmdltrunkを常にθreftrunk(t) に一致させる( θreftrunk(t) は０としているので、θmdltrunkも常に０に維持される）。モデルの足首には、Ｍref(t)と後述する上体の傾き角偏差に応じた操作量Ｍactfb の和に、実際値Mactに一致する様にトルク制御が実施される。実ロボットの腰の曲げ角度θacthipはモデルの腰の曲げ角θmdlhipを目標変位として変位制御が施される。即ち、θacthipは、θmdlhipに一致する様に制御される。実ロボットの上体には、傾斜センサが備えられており、上体の傾き角θacttrunkが検出される。
【００２４】モデルに対する実ロボットの上体傾き角偏差θerr は数６の式で求められる。この例では、実ロボットの上体傾き角偏差θerr は、θmdltrunkが常に０に維持されているから、θacttrunkそのものである。
【００２５】
【数６】
【００２６】制御の目的は、歩行中においてモデルに対する実ロボットの上体傾き角偏差θerr を０に収束させることである。先に述べた近時提案した技術では、上体の傾き角偏差に応じた操作量Ｍactfb を算出し、実ロボットの足首モーメントＭactに加えることによりフィードバックループを構成していた。これに対し、この発明による制御装置では、上体の傾き角偏差に応じた操作量Ｍmdlfb をモデルの足首モーメントＭmdl に加えることによりフィードバックループを構成する。
【００２７】この発明による制御と近時提案した制御の効果を比較するために、Ｍactfb とＭmdlfb から上体の傾き角偏差θerr までのブロック図を図５に示す。但し、この図ではθerr からＭactfb へのフィードバックとθerr からＭmdlfb へのフィードバックを省略した。
【００２８】さて、トルク制御部と変位制御部の伝達関数は、θactlink の挙動に比べて制御の応答性が十分高ければ１とみなすことができる。このとき、図５を、Ｍactfb,Ｍmdlfb およびθerr の関係のみに着目して変形すると、図６が得られる。この図から明らかな様に、例えばＨ（Ｓ）をフィードバック則とすると、Ｍact に数７の式で求められるＭactfb をフィードバックすることと、Ｍmdl に数８の式で求められるＭmdlfb をフィードバックすることは、θerr の挙動に対して同一の効果を持つ。
【００２９】
【数７】
【００３０】
【数８】
【００３１】更に、θerr の挙動を決定するものは、Ｍactfb とＭmdlfb との差であるから、数９の式となる様に、Ｈ（Ｓ）をＭactfb とＭmdlfb に分配すれば、この手法もやはり同一の効果を持つ。
【００３２】
【数９】
【００３３】以上が、この発明の根本原理である。実ロボットが複雑なものであっても、そのダイナミクスを忠実に模擬するモデルを用意すれば、実ロボットの床反力とモデルの床反力の差を制御することにより、上記例と全く同一原理で同一の効果を得ることができる。後で実施例で示す様に、動力学モデルを基準歩容からの摂動分で表現しても、θerr とその微分値ωerr が微小であれば、同一の効果が得られる。
【００３４】この構成により、近時提案した制御と異なり、実ロボットの床反力ＺＭＰ位置を本来あるべき位置から故意にずらす必要がないので、何らかの外乱などによって前記傾き角（角速度）偏差が非常に大きくなっても、接地性を確保したまま（即ち、足裏が浮かない様にしながら）、近時提案した制御では不可能であった大きな姿勢復元作用を得ることが可能となり、短時間で偏差を０に収束させることができる。また、モデルは床と干渉しないから（但し、実ロボットの全床反力をずらすために故意に干渉させる分は除く）、モデルに追従している実ロボットも床と干渉せず、着地衝撃を小さい値に抑制することも可能となる。
【００３５】尚、上記において、足平が浮かないためには、Ｍact(上記例ではＭref ＋Ｍactfb)は、ある範囲内に制限されなければならない( 例えば、平地歩行では実ロボットのＺＭＰが足平の接地面を超えない範囲に限定される）。従って、Ｍact がこの制限を超えない様な値をとり、かつ数１０の式を満足する様に、Ｍactfb とＭmdlfb は決定されるべきである。
【００３６】
【数１０】
【００３７】尚、Ｍactfb からθmdllink までのオープンの伝達関数とＭmdlfb からθmdllink でのオープンの伝達関数は一致しないので、近時提案した制御とこの発明による制御とでは、θmdllink の挙動に関して特性が異なる。
【００３８】
【実施例】以下、脚式移動ロボットとして２足歩行の脚式移動ロボットを例にとって、この発明の実施例を説明する。図７はそのロボット１を全体的に示す説明スケルトン図であり、左右それぞれの脚部リンク２に６個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。該６個の関節は上から順に、腰の脚部回旋用（ｚ軸まわり）の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、腰のロール方向（ｘ軸まわり）の関節１２Ｒ，１２Ｌ、同ピッチ方向（ｙ軸まわり）の関節１４Ｒ，１４Ｌ、膝部のピッチ方向の関節１６Ｒ，１６Ｌ、足首部のピッチ方向の関節１８Ｒ，１８Ｌ、同ロール方向の関節２０Ｒ，２０Ｌとなっており、その下部には足平２２Ｒ，２２Ｌが取着されると共に、最上位には上体（筐体２４）が設けられ、その内部には制御ユニット２６が格納される。
【００３９】上記において腰関節は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から構成され、また足関節は、関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また、腰関節と膝関節との間は大腿リンク３２Ｒ，３２Ｌで、膝関節と足関節との間は下腿リンク３４Ｒ，３４Ｌで連結される。ここで、脚部リンク２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６×２＝１２個の関節（軸）をそれぞれ適宜な角度に駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行することができる様に構成される。先に述べた様に、上記した関節は電動モータからなり、更にはその出力を倍力する減速機などを備えるが、その詳細は先に本出願人が提案した出願（特願平１−３２４２１８号、特開平３−１８４７８２号）などに述べられており、それ自体はこの発明の要旨とするところではないので、これ以上の説明は省略する。
【００４０】図７に示すロボット１において、足首部には公知の６軸力センサ３６が設けられ、足平を介してロボットに伝達されるｘ，ｙ，ｚ方向の力成分Ｆｘ，Ｆｙ，Ｆｚとその方向まわりのモーメント成分Ｍｘ，Ｍｙ，Ｍｚとを測定し、足部の着地の有無と支持脚に加わる力の大きさと方向とを検出する。また足平２２Ｒ（Ｌ）の四隅には静電容量型の接地スイッチ３８（図７で図示省略）が設けられて、足平の接地の有無を検出する。更に、上体２４には傾斜センサ４０が設置され、ｘ−ｚ平面内とｙ−ｚ平面内のｚ軸に対する、即ち、重力方向に対する傾斜角度と傾斜角速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコーダが設けられる。更に、図７では省略するが、ロボット１の適宜な位置には傾斜センサ４０の出力を補正するための原点スイッチ４２と、フェール対策用のリミットスイッチ４４が設けられる。これらの出力は前記した上体２４内の制御ユニット２６に送られる。
【００４１】図８は制御ユニット２６の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ４０などの出力はＡ／Ｄ変換器５０でデジタル値に変換され、その出力はバス５２を介してＲＡＭ５４に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ５６を介してＲＡＭ５４内に入力されると共に、接地スイッチ３８などの出力は波形整形回路５８を経て同様にＲＡＭ５４内に格納される。制御ユニット内にはＣＰＵからなる第１、第２の演算装置６０，６２が設けられており、第１の演算装置６０は後で述べる様に目標関節角度を算出してＲＡＭ５４に送出する。また第２の演算装置６２はＲＡＭ５４からその目標値と検出された実測値とを読み出し、各関節の駆動に必要な制御値を算出し、Ｄ／Ａ変換器６６とサーボアンプを介して各関節を駆動する電動モータに出力する。
【００４２】続いて、この制御装置の動作を説明する。
【００４３】図９はその動作を示すブロック図であり、図１０ないし図１２はその動作を示すフロー・チャートである。図１０以降のフロー・チャートを参照して具体的に説明する前に、図９ブロック図を参照してこの制御を概説する。尚、以下ではｘ方向（進行方向）の制御だけを例にとって説明しているが、ｙ方向（横方向）も同様である。
【００４４】図９に示す様に、この実施例においてロボットの幾何学モデルとしては剛体モデルを備え、ロボットの動力学モデルとしては、基準歩容からの上体位置摂動分の挙動を倒立振子で近似したものを備える。倒立振子の質量をｍ、支点まわりの慣性モーメントをＩ、支点と重心の距離をｈとする。基準歩容は、上体２４の位置・姿勢、および両足平２２Ｒ（Ｌ）の位置・姿勢で表現される。両足平２２Ｒ（Ｌ）の位置・姿勢については、一歩ごとの足平軌道パラメータ（例えば、遊脚着地位置や着地時刻など）が、大局的姿勢制御部から目標足平軌道生成部に与えられ、目標足平軌道生成部においてリアルタイムに各瞬間のそれらの位置や姿勢が生成される。尚、この制御においてはロボットの姿勢が崩れたとき、先ずモデルの姿勢をそれに合わせて崩し、次いで安定方向に回復させると共に、実ロボットをそれに追従制御して姿勢安定を回復する様にしていることから、ここで「大局的」なる語は、瞬間々々ではなく、比較的長い時間をかけて滑らかに姿勢回復を最終的に実現する如く、この制御の特質を示す意味で使用する。
【００４５】またＺＭＰ目標軌道については、一歩ごとのＺＭＰ目標軌道パラメータ（例えば、折れ線表現されているときは折れ点の座標や時刻）が、大局的姿勢制御部からＺＭＰ目標軌道生成部に与えられ、ＺＭＰ目標軌道生成部においてリアルタイムに各瞬間のその位置が算出される。上体２４の位置・姿勢については、ＺＭＰ目標軌道を満足する様に、予めオフラインで作成されて時系列データとして大局的姿勢制御部に記憶され、歩行時に吐き出される。尚、以降、足平軌道パラメータとＺＭＰ目標軌道パラメータなど歩容の特徴を表現するものを総称して「歩容パラメータ」と呼ぶ。
【００４６】次いで、幾何学モデルに対する実ロボットの姿勢の傾き角偏差θerr と角速度偏差ωerr を検出する（実ロボット１の関節変位が忠実にモデルに追従すると考えられるならば、姿勢の傾き角（角速度）偏差は、上体２４の傾き角（角速度）偏差で代表して差し支えない。あるいは重心位置のずれを用いても良い。）。
【００４７】これらを状態量として、これらを０に収束させようと働く安定化制御則から求められる制御操作量（状態フィードバック量）は、モーメントの次元で検出されて倒立振子型動力学モデルの支点に与えられる。ここで、この操作量を先に述べたと同様に、モデル操作モーメントＭmdl と呼ぶ。Ｍmdl は、倒立振子型動力学モデルが受ける床反力モーメントであるとも言える。倒立振子型動力学モデルは、Ｍmdl と重心に作用する重力の影響を受けて運動する。倒立振子の状態量、即ち、傾き角θmdl とその角速度ωmdl の挙動は、過去の状態と支点に加えられるモーメントＭmdl を基に逐次計算される。
【００４８】数１１の式により、上体２４のｘ方向のずれΔｘが求められる。但し、ｈtrunk は上体２４の高さ（より正確に表現すれば、床から腰関節１（Ｌ）までの高さ）とする。
【００４９】
【数１１】
【００５０】ロボット幾何学モデルの上体位置は、歩容パラメータの上体位置ｘにΔｘが加えられた位置に指定される。
【００５１】また、先に述べた近時提案した技術で用いられるコンプライアンス制御によって、ＺＭＰ目標位置まわりの実床反力モーメントＭact に応じて、基準歩容の足平位置・姿勢が修正され、それが幾何学モデルに与えられる。図９で用いられている制御では、Ｍact から床反力モーメントフィードバック則によって仮想床傾斜角指令θctrlが求められ、モデルの足平位置は、モーメント足平軌道生成部で生成される基準歩容の足平位置をＺＭＰ目標位置を中心として仮想床傾斜角指令の分だけ、座標回転させられる。これにより、実ロボット１の姿勢傾きの挙動は、基準点（例えば、ＺＭＰ目標位置）を支点とし、支点に床から傾きに比例した床反力モーメントが発生する倒立振子に近似される。
【００５２】以上によって求められ、修正された上体位置・姿勢から、逆キネマティクス計算によって、幾何学モデルの各関節の変位が求められる。尚、このロボットでは可動部はすべて回転自由度しかないから、関節の変位はすべて角度で表現される。実ロボット１には、その関節変位をロボット幾何学モデルの関節変位に追従させる制御システムが備えられる。
【００５３】以上のシステムにおいて、状態量θerr とωerr に応じたモデル操作モーメントＭmdl をロボット動力学モデルに与えるフィードバック制御は、先に作用欄で説明した原理によって、θerr とωerr を０に収束させる様に作用する。即ち、実ロボットとモデルの傾きが一致する様に作用する。
【００５４】ここで、図９において、倒立振子型動力学モデルにモデル操作モーメントＭmdl が加わって、モデルの傾きθmdl とその角速度ωmdl が中立点（直立状態）から一旦ずれると、このモデルは不安定系であるので、θmdl ，ωmdl は発散する。そこで、状態量θmdl ，ωmdl に応じて、次の遊脚着地位置または着地タイミングを変更することによってθmdl ，ωmdl の発散を抑制する。
【００５５】ところで、基準歩容におけるある一歩（これを「第１歩容」と呼ぶ）において着地位置を変更すると、次の一歩（これを「第２歩容」と呼ぶ）の初めでは、支持脚足平の位置だけが、基準の第２歩容の位置からずれる。このことを、第２歩容の基準座標から見ると、支持脚足平は基準歩容のままで、遊脚足平と上体の位置が変更量だけずれたことに相当する。そのまま放置すると、第１歩容と第２歩容の境界が不連続になる。そこで、ロボット幾何学モデルの歩容が連続となる様に、歩容の変わり目において次の操作を行う。
【００５６】遊脚軌道は初期位置や次期着地位置などの足平軌道パラメータで与えられているので、第２歩容の足平軌道パラメータの一つである初期遊脚位置を−Δｘfoottotal だけずらす。第２歩容の上体軌道はそのままとし、その代わりに倒立振子型動力学モデルの傾き角θmdl を−Δｘfoottotal/ｈtrunk だけずらす。この結果、ロボット幾何学モデルの歩容の連続性を保ちつつ、倒立振子型動力学モデルの状態量の一つである傾き角θmdl を−ｘfoottotal/ｈtrunk だけ操作することができる。即ち、着地位置を変更することにより、倒立振子型動力学モデルの状態量を制御することができる。
【００５７】上記を前提とし、以下図１０フロー・チャートを参照して、着地位置を進行方向に変更した場合のモデル傾き制御アルゴリズムを説明する。
【００５８】先ず、Ｓ１０において倒立振子型モデルを安定な直立位置に初期化し、Ｓ１２に進んで歩数カウンタが基準歩容で設定した目標歩数になるまで、Ｓ１４からＳ２６を繰り返す。ここで、用語を次の様に定義する。
１．一歩の歩容は、両脚支持期の初めから次の遊脚が着地するまでの期間を指す。
２．ある歩容の支持脚とは、その歩容の片脚支持期に支持脚となる脚とし、遊脚とは、その歩容の片脚支持期に遊脚となる脚とする。
３．歩容の基準座標を、その一歩の支持脚の足平接地点にとることとする。
【００５９】Ｓ１４では今回の歩容パラメータをセットし、Ｓ１６に進んで前回の歩容の修正後の着地位置と前回の歩容の基準歩容の着地位置との差を前記したΔｘfoottotal とする。次いで、Ｓ１８に進んで今回の歩容の基準初期遊脚位置からΔｘfoottotal を減算し、Ｓ２０に進んで倒立振子型モデルの傾斜角θmdl から、Δｘfoottotal ／ｈtrunk だけ減算する。
【００６０】次いでＳ２２に進み、一歩分のサンプリング回数だけＳ２４とＳ２６を繰り返す。即ち、Ｓ２４で１周期が経過したことを確認した後、Ｓ２６に進んで姿勢演算および制御のサブルーチンを実施する。
【００６１】図１１はそれを示し、先ずＳ１００において実ロボットの傾斜を計測し、前記した様に幾何学モデルに対する姿勢傾き角偏差θerr と傾き角速度偏差ωerr を求め、Ｓ１０２に進んでそれらから前記したモデル操作モーメントＭmdl を求め、Ｓ１０４に進んでＭmdl を倒立振子型モデルの支点に加えてモデルの今回の傾斜角θmdl と角速度ωmdl を求める。次いでＳ１０６に進み、前記した理由から上体ずれΔｘを図示の如く求め、Ｓ１０８に進んで着地までに時間の余裕があれば、倒立振子型モデルの挙動を予測して着地目標位置パラメータの修正量Δｘfootを求め、基準歩容の着地目標位置パラメータにΔｘfootを加算し、着地目標位置パラメータを修正する。
【００６２】これについては先にも述べたが、更に敷衍すると、着地位置の変更量は、次の一歩の間に、倒立振子型動力学モデルの傾き角θmdl と傾き角速度ωmdl 、および幾何学モデルに対する実ロボットの傾き偏差θerr と傾き角速度偏差ωerr が、なるべく０に収束する様に決定するべきである。そのためには、θmdl ，ωmdl ，θerr ，ωerr などの現在状態から、着地位置の変更により、次の一歩の間にθmdl ，ωmdl ，θerr ，ωerr がどの様な挙動を示すかを推測しなければならない。
【００６３】θerr とωerr はこの発明の根本原理によって０に収束するので、やや予測精度が低下するが、θerr とωerr の現在状態を無視しても支障ない。つまり、θmdl とωmdl の現在状態だけから予測計算して着地位置の変更量を決定しても差し支えない。あるいは、予め、現在状態と着地位置の変更量との関係をシミュレーションによりマップ化しておいて、実際の歩行時にこのマップを用いて着地位置の変更量を決定しても良い。
【００６４】θmdl とωmdl の現在状態だけから着地位置の変更量を決定する手法としては、竹馬ロボットなどで行われている様に、倒立振子型動力学モデルの着地瞬間の全エネルギ予測値が直立時の全エネルギに一致する様に、変更量を決定する手法が一般的に知られている。以下にその手法を説明する。
【００６５】着地瞬間の全エネルギ予測値が直立時の全エネルギに一致するためには、予測される着地直後のθmdl とωmdl が数１２の式を満足する様に、着地位置が決定されなければならない。
【００６６】
【数１２】
【００６７】また、直立状態に近づいていくために、θmdl とωmdl が逆極性でなければならないから、即ち、数１３の式を満足しなければならない。
【００６８】
【数１３】
【００６９】数１２式、数１３式より、数１４の式が得られる。
【００７０】
【数１４】
【００７１】数１４式を近似すれば、数１５の式が得られる。
【００７２】
【数１５】
【００７３】つまり、予測される直立直後のωmdl とθmdl との比が、数１５式で定められる比になる様に着地位置修正量Δｘfootが決定されれば良い。着地位置を修正しない場合の予測される着地直後のωmdl とθmdl は、倒立振子で現在のωmdl ，θmdl ，θerr ，ωerr から動力学演算により求めることができ、それをωmdleとθmdleとする。（特に、ωmdl およびθmdl に比べてθerr およびωerr が無視できるほど小さい場合には、解析的に容易に求めることができる。）。θmdleをωmdle／平方根（２ｍｇｈ／Ｉ）に修正できれば、数１５式を満足する。そのためには、前述の様に、着地位置をΔｘfootだけ修正することによって、θmdlを−Δｘfoot／ｈtrunk だけ操作することができるので、着地位置修正量Δｘfootを数１６の式より決定すれば良い。
【００７４】
【数１６】
【００７５】以上の様に、着地位置修正量Δｘfootが決定され、着地目標位置パラメータｘfootは、歩容の途中で修正される。この修正は、着地目標位置を変更したときの足平の軌道生成計算が大変な場合には、一歩ごとに１つずつだけ実行されることとなるが、足平の軌道を本出願人が近時提案した別の技術（平成４年５月２２日出願、整理番号Ａ９２−０４９３）を用いて発生させれば、着地目標位置の変更をサンプリング周期毎に行うことができる。なぜなら、この技術を用いれば、着地目標位置パラメータの変更をサンプリング周期ごとに行っても、足平の軌道は、そこからまた滑らかに修正されるからである。但し、着地時刻に近づいてから着地位置を大幅に変更すると、遊脚軌道が急激に変わるので、着地までの残り時間に応じて再修正の可能限界量を求め、それを制限値として修正量の変化率にリミットをかけるのが望ましい。
【００７６】図１１フロー・チャートにおいては次いでＳ１１０に進んでＺＭＰ目標軌道パラメータからＺＭＰ目標位置を算出し、Ｓ１１２に進んで着地目標位置パラメータなどの足平軌道パラメータから両足平の位置・姿勢を算出し、Ｓ１１４に進んで基準歩容の上体位置にθmdl とｈtrunk の積を加算した位置を上体目標位置とし（上体の目標姿勢は基準通りとする）、Ｓ１１６に進んでモデル足平修正によるコンプライアンス制御を行う。
【００７７】図１２はそれを示すサブルーチン・フロー・チャートであり、先ずＳ２００において６軸力センサ３６の検出値を取り込み、Ｓ２０２に進んでそれからＺＭＰ目標位置まわりの実床反力モーメントＭact を求め、Ｓ２０４に進んで図示の様に座標回転角θctrlを算出し、Ｓ２０６に進んでその値だけ両足平の位置・姿勢をＺＭＰまわりに回転させ、幾何学モデルの両足平の位置・姿勢とする。尚、この制御は先の出願（平成４年４月３０日）に詳細に述べられているので、ここではこの程度の説明に止める。
【００７８】次いで、図１１フロー・チャートに戻り、Ｓ１１８に進んで上体目標位置・姿勢と修正された足平位置・姿勢から幾何学モデルの関節変位を求め、Ｓ１２０に進んで実ロボットの関節変位を幾何学モデルの変位に追従させる制御を行う。
【００７９】この実施例は上記の如く、モデルに対する実ロボットの傾き制御と倒立振子モデルの傾き偏差の一歩ごとの離散制御により、安定な歩行を実現することができる。更に、故意にずらすＺＭＰをモデルのそれとしたので、先に提案した制御に比べてＺＭＰの移動範囲を大きくとることができ、姿勢復元力を大きくすることができる。従って、何らかの外乱によって傾き偏差が非常に大きくなっても、接地性を確保したまま、姿勢を安定に回復することができる。また、姿勢が崩れたときはモデルの姿勢をそれに応じて一旦崩し、その後に姿勢を回復させると共に、実ロボットをそのモデルの姿勢を追従する様に制御することから姿勢回復を滑らかな動きで実現することができる。更に、モデルは床と干渉しないことから、モデルに追従する実ロボットも床と干渉することがなく、また着地衝撃も小さい値に抑制することができる。
【００８０】図１３はこの発明の第２実施例を示すブロック図である。第１実施例では倒立振子モデルの傾斜制御において、着地位置を操作量としていたが、第２実施例ではモデル操作モーメントに、新たに倒立振子モデルの傾斜に応じた操作量を加えて安定化を図る様にした。
【００８１】また第２実施例においては、幾何学モデルに対する実ロボットの傾き角偏差θerr をＰＤなどのフィードバック則Ｈ１（Ｓ）を通して得られる実傾き偏差制御要求制御量Ｍerrdmd（即ち、Ｍerrdmd＝Ｈ１（Ｓ）θerr ）と、倒立振子モデルの傾斜角θmdl をフィードバック則Ｈ２（Ｓ）を通して得られるモデル傾き制御要求制御量Ｍmdldmd（即ち、Ｍmdldmd＝Ｈ２（Ｓ）θmdl ）をリミッタおよび分配器に通して実モーメント操作量Ｍactcomm とモデルモーメント操作量Ｍmdl に分配する様にした。
【００８２】図１４にリミッタおよび分配器を示す。この例においては、実モーメント操作量Ｍactcomm は、ＭerrdmdとＭmdldmdの和にリミッタをかけることによって求められる。このリミッタ作用により、実ロボットの床反力が存在可能領域から超えない様に管理され、実ロボットの接地性が確保される（例えば、平地歩行であれば、床反力のＺＭＰが接地面を含む最小凸多角形の中にあることが、床反力の存在可能条件である。）。
【００８３】モデルモーメント操作量Ｍmdl は、Ｍmdldmdに前記リミッタの入力超過分を加えることによって求められ、第１実施例と同様、倒立振子型ロボット摂動動力学モデルに入力される。また、ＺＭＰまわりの実床反力モーメントＭact から床反力モーメントフィードバック則によって、床反力モーメントフィードバック操作量θcompが求められる。Ｍactcomm とコンプライアンス定数Ｋcompの積に床反力モーメントフィードバック操作量θcompを加えることにより、仮想床傾斜角指令が求められる。幾何学モデルの足平位置は、目標足平軌道生成部で生成される基準歩容の足平位置をＺＭＰ目標位置を中心として、仮想床傾斜角指令の分だけ、座標回転させられる。
【００８４】実ロボットの傾き角偏差θerr の挙動を決定するものは、ＺＭＰまわりの実床反力モーメントＭact とモデルモーメント操作量Ｍmdl の差である。ところで、コンプライアンス制御が十分に柔らかければ、Ｍact は実モーメント操作量Ｍactcomm にほぼ一致する。従って、この場合、実ロボットの傾き角偏差θerr の挙動を決定するものは、Ｍactcomm とＭmdl の差であると言っても良い。Ｍactcomm とＭmdl の差は、前記リミッタが働いても働かなくても常にＭerrdmdとなるので、幾何学モデルに対する実ロボットの傾き角偏差θerr を０に収束させる制御は常に働く。
【００８５】一方、モデルの傾き角θmdl の挙動を決定するものは、Ｍmdl である。Ｍmdlは、前記リミッタが働いていないときにはＭmdldmdに一致し、モデルの傾きを０に収束させる制御が正常に働く。前記リミッタが働いているときには、幾何学モデルに対する実ロボットの傾き角偏差θerr を０に収束させる制御を優先した上で、可能な範囲でモデルの傾きを０に収束させようと作用する。更に、Ｍerrdmdがリミッタ設定値に比べて過大である場合にはモデルの傾き制御が犠牲になり、モデルの傾きは発散しようとすることもある。即ち、実ロボットのＺＭＰが存在可能領域の限界までまだ余裕があるときのみ、倒立振子モデル傾斜制御が実行される。従って、瞬間的な外乱や微小外乱に対しては、倒立振子モデルが直立状態に回復できるが、大きな外乱が継続すると、倒立振子モデルが直立状態に回復できなくなり、ロボット幾何学モデルの姿勢が大きく崩れて転倒する。しかし、足平の軌道が基準歩容通りであるので、階段や飛び石の上を歩行するときの様に、着地位置が制約されている際には、第２実施例は好適である。
【００８６】ここで、倒立振子モデルの傾き制御ゲインの設定について説明する。モデルの傾き角θmdl を０に収束させる制御則を、例えば数１７の式で与えるものとする。
【００８７】
【数１７】
【００８８】単純倒立振子を、現在状態から、支点にモーメントを加えず、フリーな運動をさせるときに、直立状態で静止するためには、現在の全エネルギが直立静止状態の全エネルギに一致していなければならない。つまり、数１８の式が成立していなければならない。
【００８９】
【数１８】
【００９０】また、直立状態に近づいていくために、ωmdl とθmdl が逆極性でなければならないから、数１９の式を満足しなければならない。
【００９１】
【数１９】
【００９２】数１８、数１９式より、数２０の式を得る。
【００９３】
【数２０】
【００９４】数２０式を近似して数２１の式を得る。
【００９５】
【数２１】
【００９６】数２１式を満足した状態にあれば、倒立振子モデルの支点にモデル操作モーメントＭmdl を加えなくても、直立状態で静止する。従って、数２１式を満足する状態でモデル操作モーメントＭmdldmdが０になる様に制御則を設定すれば、倒立振子モデルに必要以上の余分なモデル操作モーメントＭmdl を発生させず、直立状態に収束させることができる。この様な制御則で数１７の式を満足するものは、数２２の式である。
【００９７】
【数２２】
【００９８】数２２式を満足していても、Ｋωをやや小さめに設定すると、倒立振子の挙動にオーバーシュートが発生する。Ｋωをもっと小さく設定すると、重力モーメントに負けて直立状態に復元できなくなる。直立状態に復元できてかつオーバーシュートが発生しない様にするためには、フィードバックループの特性根が負の実数になることであり、そのためには、Ｋωは次の数２３式を満足しなければならない。
【００９９】
【数２３】
【０１００】数２２と数２３の式を満足する様にゲインを設定すれば、Ｍmdldmdの絶対値が小さく抑えられてＭactcomm の絶対値も小さく抑えられるので、実ロボットの接地性が高くなる。
【０１０１】図１５から図１７は第２実施例の制御を示すフロー・チャートであり、うち図１５はＳ３００からＳ３１０にメイン・ルーチンを、図１６はＳ４００からＳ４２２にその姿勢演算および制御のサブ・ルーチンを、図１７はＳ５００からＳ５０６にそのコンプライアンスおよび姿勢安定化制御のサブ・ルーチンを示す。第１実施例と相違する点に焦点をおいて説明すると、図１５R>５フロー・チャートにおいてＳ３０４とＳ３０６との間には第１実施例の図１０のＳ１６からＳ２０に相当するステップは存在しない。これは、姿勢演算および制御のサブルーチンで着地位置修正を行っていないためである。また、図１６の姿勢演算および制御サブルーチン・フロー・チャートにおいては、先に述べた構成からＳ４０２からＳ４０６が追加されると共に、第１実施例の図１１１１のＳ１０８に相当するステップは削除される。また図１７サブルーチン・フロー・チャートにおいてはＳ５０４の座標回転角の演算で先に述べた様に、実ロボット操作モーメントＭactcomm にコンプライアンス定数Ｋcompを乗じたものが加算される。尚、残余の構成は、第１実施例と相違しない。
【０１０２】第２実施例は足平の軌道が基準歩容通りであることから、階段や飛び石の上を歩行するときなど、着地位置が制約される際に特に好適である。
【０１０３】図１８はこの発明の第３実施例を示すブロック図である。第３実施例では、倒立振子モデルの傾斜制御において、第１実施例と第２実施例の制御を併用した。倒立振子モデルを直立状態に回復させるために、瞬間的な外乱や微小外乱に対しては、主に倒立振子モデルにモーメントを加える制御が働き、大きな外乱が継続すると、主に着地位置や着地タイミングを修正する制御が働く。
【０１０４】第３実施例においても、着地位置や着地タイミングの修正量は、第１実施例と同様、倒立振子モデルが直立状態に戻る様に決定される。第１実施例に比べると、モデル傾き制御要求制御量Ｍmdldmdの影響がある分だけ、モデルの挙動予測演算が複雑になる様に見えるが、モデル傾き制御要求制御量Ｍmdldmdにより、倒立振子モデルの発散が抑えられるので、むしろ予測精度が高いとも言える。特に、第２実施例で述べた倒立振子モデルの傾き制御ゲインの設定値（数２２式と数２３式を満足するゲイン）を採用すれば、倒立振子モデルの全エネルギが直立時の全エネルギに一致しているときには、モデル傾き制御要求制御量Ｍmdldmdが０になるので、第１実施例で説明した、倒立振子型動力学モデルの着地瞬間の全エネルギ予測値が直立時の全エネルギに一致する様に、着地位置変更量を決定する手法をそのまま活用することができる。
【０１０５】図１９は第３実施例のＳ６００からＳ６１６よりなるメイン・ルーチン・フロー・チャート、図２０R>０はＳ７００からＳ７２４よりなるその姿勢演算および制御サブルーチン・フロー・チャート、図２１はＳ８００からＳ８０６よりなるそのコンプライアンスおよび姿勢安定化制御サブルーチン・フロー・チャートである。図１９のメイン・ルーチン・フロー・チャートは第１実施例のそれと、図２１のサブルーチン・フロー・チャートは第２実施例のそれと同様である。また図２０の姿勢演算および制御サブルーチン・フロー・チャートは、第２実施例と比べて、着地目標位置パラメータを修正するステップＳ７１２が追加されている点で異なるのみである。尚、残余の構成は従前の実施例と相違しない。
【０１０６】第３実施例の場合、前述の如く、倒立振子モデルを直立状態に回復させるために、瞬間的な外乱や微小外乱に対しては主に倒立振子モデルにモーメントを加える制御が働き、大きな外乱が継続すると主に着地位置や着地タイミングを修正する制御が働くので、第１実施例に比べ、着地位置の変更量が少なくなり、歩行経路のふらつきが小さくなると共に、第１実施例、第２実施例よりも安定度が一層向上する。
【０１０７】図２２はこの発明の第４実施例を示すブロック図である。第１から第３までの実施例では、ロボットのモデルを、基準歩容からの上体位置摂動分に対する動力学モデルと、上体や足平位置と関節角の関係を表現する幾何学モデルに分割して備えていた。それに対し、第４実施例では、剛体リンクで構成される動力学・幾何学複合モデルを備える様にした。その他は後で述べる様に、大局的姿勢制御部の詳細が異なることを除くと、第１実施例とほぼ同一構成である。
【０１０８】第４実施例において、歩容は、ＺＭＰ目標位置、および両足平の位置・姿勢で表現される。両足平の位置・姿勢については、一歩ごとの足平軌道パラメータ（例えば、遊脚着地位置や着地時刻など）が、大局的姿勢制御部から目標足平軌道生成部に与えられ、目標足平軌道生成部において、リアルタイムに各瞬間のそれらの位置や姿勢が算出される。ＺＭＰ目標軌道については、一歩ごとのＺＭＰ目標軌道パラメータが、大局的姿勢制御部からＺＭＰ目標軌道生成部に与えられ、ＺＭＰ目標軌道生成部においてリアルタイムに各瞬間のその位置が算出される。上体の位置・姿勢については、第１実施例ないし第３実施例ではＺＭＰ目標軌道を満足する様に、予めオフラインで作成された時系列データとして大局的姿勢制御部に記憶されていたが、第４実施例ではロボットの幾何学・動力学モデルにてモデルの両足平の位置・姿勢、ＺＭＰ目標軌道およびＺＭＰ目標位置まわりのモデル操作モーメントを基に、リアルタイムに生成される。
【０１０９】第４実施例のロボットモデルでは、両足平の位置・姿勢は、足平軌道生成部から与えられた足平の位置・姿勢を仮想床傾斜指令θctrlだけ座標回転させた位置・姿勢に変換される。ロボットモデルでは、上体の位置・姿勢が、ＺＭＰ目標位置に発生する床反力モーメントがＭmdl となる様に動力学計算によって求められ、同時に各関節の変位が求められる。尚、Ｍmdl が０のときには、ロボットモデルのＺＭＰ位置はＺＭＰ目標位置に一致するが、Ｍmdl が０でないときには、ＺＭＰ目標位置にモーメントが発生していることから、もはやＺＭＰ目標位置はロボットモデルのＺＭＰ位置ではなく、ロボットモデルのＺＭＰ位置は、ＺＭＰ目標位置からずれたところに存在する。
【０１１０】モデルの床面には、あらゆる全床反力が発生可能であると仮定する。即ち、モデルの接地面には吸着力も発生可能であるとし、例えば、平地歩行においてＺＭＰが接地面を含む最小凸多角形を超えても、接地面が離れない（脚と床の間の拘束条件が所期通り確保される）ものとする。この様に想定することによって、モデルに対する実ロボットの傾き偏差制御において、実ロボットの全床反力を操作するだけでは不可能であった大きな復元作用も発生可能となる。従って、フィードバックゲインを大きくして短時間で偏差を０に収束させることができる。つまり、実ロボットは、関節変位のみならず、傾きまでもモデルに高応答で追従する。但し、モデルの全床反力の床法線方向成分は、上向き（正）になる様にとるべきである。なぜなら、そうしないと、実ロボットが一瞬床から浮いてしまう恐れがあるからである。
【０１１１】第４実施例においては、大局的姿勢制御部において、モデルの挙動やモデルに対する実ロボットの傾き偏差などから、次の着地位置、着地タイミングまたはＺＭＰ目標軌道などの歩容パラメータを決定する。第１から第３実施例では基準歩容が存在するので、基準歩容からのずらし量を決定するだけで済むが、第４実施例においては基準歩容が存在しないので、歩容パラメータの決定が難しくなる。決定をなるべく容易にするためには、ある基準点から見たロボット全体の角運動量や重心位置などのマクロな状態量に着目すれば良い。局所的姿勢制御がこれらのマクロな状態量にあまり影響を与えない場合には、これらのマクロな状態量だけから将来の挙動を予測し、それらのマクロな状態量が発散しない様に歩容パラメータを決定すれば良い。具体的には、先に述べた竹馬型２足歩行ロボットの着地位置制御で行われている手法を用いたり、シミュレーションによりマクロな状態量に応じた適切な着地位置を予め学習させておく手法などが考えられる。
【０１１２】第４実施例のロボットにおいては、ゆっくり歩行する場合には遊脚の反動が無視でき、単純倒立振子で近似することができる。ロボットが単純倒立振子で近似できる場合には、角運動量を縦軸に、重心位置を横軸にとったときの軌跡は、双曲線上を移動することが知られているので、挙動が解析的に予測でき、着地位置決定も解析的に行うことができる。竹馬型２足歩行ロボットの着地位置制御では、これを利用している。第４実施例のロボットが高速で歩行する場合（例えば、３ｋｍ／ｈなど）には、遊脚の反動が無視できなくなり、単純倒立振子で近似できなくなる。この場合には、解析的な決定法ではなく、マクロな状態量に応じた適切な着地位置を予め学習させる手法などが有効である。学習には、人工知能、ファジー、ニューロなどが使用できよう。
【０１１３】図２３は第４実施例のＳ９００からＳ９１０よりなるメイン・ルーチン・フロー・チャートを、図２４２４はＳ１０００からＳ１０１４よりなるその姿勢演算および制御サブルーチン・フロー・チャートを、図２５はＳ１１００からＳ１１０６よりなるそのコンプライアンス制御サブルーチン・フロー・チャートを示す。図２５R>５サブルーチン・フロー・チャートは、第１実施例のそれと同様である。
【０１１４】第４実施例においては、第１実施例に比べて動力学モデルがより精密になっているので、より正確な姿勢制御が実現される。特に、大きな外乱が加わってモデルの姿勢が大幅に崩れても、転倒することなく、復元することができる。
【０１１５】図２６はこの発明の第５実施例を示すブロック図である。この例において大局的姿勢制御部はなく、歩容パラメータ決定部は、予め外乱がない状況で長期的に安定な歩行が実現できる様に設定された一連の歩容パラメータを吐き出しているだけである。
【０１１６】第５実施例において、ロボットの幾何学・動力学モデルとしては、第１モデルと第２モデルの２つが備えられる。第１モデルと第２のモデルの初期状態は、前記の予め設定された一連の歩容の初期状態に一致させておく。第１モデルには目標足平軌道とＺＭＰ目標軌道が与えられ、ＺＭＰ目標軌道を満足する様に上体軌道が生成される。歩容パラメータ決定部が上に述べた様に、予め外乱がない状況で長期的に安定な歩行が実現できる様に設定された一連の歩容パラメータを吐き出しているので、第１モデルの挙動は、その通り忠実に再現する。第２モデルには、目標足平軌道とＺＭＰ軌道以外に、第１モデルと第２モデルの上体位置の差を０に収束させる制御の操作量と、第２モデルと実ロボットの姿勢傾斜ずれを０に収束させる制御の操作量が、ＺＭＰ目標位置まわりにモーメントとして加えられる。
【０１１７】第２モデルに対する実ロボットの傾き角偏差θerr をＰＤなどのフィードバック則Ｈ１（Ｓ）を通して得られる実傾き偏差制御要求制御量Ｍerrdmdと、第１モデルに対する第２モデルの上体位置ずれをフィードバック則Ｈ２（Ｓ）を通して得られるモデル上体位置制御要求制御量Ｍmdldmdを第２実施例で用いたと同様なリミッタおよび分配器を通して、実モーメント操作量Ｍactcomm とモデルモーメント操作量Ｍmdl に分配する。
【０１１８】ＺＭＰまわりの実床反力モーメントＭact から床反力モーメントフィードバック則によって床反力モーメントフィードバック操作量θcompが求められる。Ｍactcomm とコンプライアンス定数Ｋcompの積に床反力モーメントフィードバック操作量θcompを加えることにより、仮想床傾斜角指令θctrlが求められる。第２モーメントの足平位置は、目標足平軌道生成部で生成される足平位置をＺＭＰ目標位置を中心として、仮想床傾斜角指令の分だけ、座標回転させられる。
【０１１９】第１モデルと第２モデルは、第４実施例のそれと同一で良い。但し、第１モデルにはモデルモーメント操作量Ｍmdl と仮想床傾斜角指令θctrlが与えられないので、第１モデルを第２モデルと同一のプログラムで作るならば、第１モデルに入力されるＭmdl とθctrlを０にしておけば良い。
【０１２０】図２７は第５実施例のＳ１２００からＳ１２１０よりなるメイン・ルーチン・フロー・チャートを、図２８はＳ１３００からＳ１３１８よりなるその姿勢演算および制御サブルーチン・フロー・チャートを、図２９はＳ１４００からＳ１４０６よりなるそのコンプライアンスおよび姿勢安定化制御サブルーチン・フロー・チャートを示す。第２実施例のそれらとほぼ同様である。
【０１２１】第２実施例では予め上体軌道が生成されているのに対し、第５実施例では目標ＺＭＰ軌道を満足する様に、第１モデルにおいてリアルタイムに上体軌道が生成される。第５実施例の効果は、本質的に第２実施例と同じである。ただ、動力学モデルがより忠実になっているため、姿勢安定化制御の精度が高く、モデルの姿勢が大きく崩れても復元することができる。
【０１２２】図３０はこの発明の第６実施例を示すブロック図である。第６実施例ではモデルの姿勢安定化において、第４実施例と第５実施例の制御を併用した。第６実施例においては、第２モデルの角運動量や重心位置などのマクロな状態量をある範囲に収めるために、瞬間的な外乱や微小外乱に対しては主に実ロボットと第２モデルにモーメントを加える制御が働き、大きな外乱が継続すると、主に着地位置や着地タイミングを変更する制御が働く。
【０１２３】第１モデルは、歩容の切り替わり目において、第２モデルの状態に合わせる（つまり、一歩ごとの歩容の初めに強制的に第１モデルの初期状態を第２モデルのそのときの状態に合わせる）。こうしないと、過大な外乱が長期的に実ロボットに加わって、第２モデルに対する実ロボットの傾斜ずれが大きくなったときに、この傾斜ずれを修正しようとして第２モデルに大きな操作モーメントが作用すると、第１モデルと第２モデルの姿勢ずれを０に収束させる制御が十分効かず、第１モデルと第２モデルの姿勢ずれが発散してしまうからである。但し、第１モデルの初期状態を第２モデルに急激に合わせると、第１モデルと第２モデルの姿勢ずれを０に収束させる制御の操作量が急激に変化する恐れがあるので、第１モデルと第２モデルの姿勢ずれを０に収束させる制御のフィードバックゲインを歩容の変わり目の直前に連続的に下げておいた方が良い。
【０１２４】第４実施例では不確定な外乱が実ロボットに加わることによって、モデルの挙動も不確定な挙動を示すため、モデルの将来挙動の予測精度が低下する。第６実施例では、一歩の間、第２モデルは第１モデルの挙動に収束しようとする。一歩の間、第１モデルの挙動には実ロボットに加わる外乱の影響を受けないので、第１モデルの挙動は、確定的に予想できる。従って、実ロボットに加わる外乱によって第２モデルの挙動が変化しても、しばらくすれば第１モデルの挙動に収束しようとするので、第２モデルの挙動の不確定性が減少する。即ち、第６実施例では、第４実施例に比べ、着地位置などの歩容パラメータを操作することによる姿勢安定化作用がより確実に働くこととなる。
【０１２５】図３１は第６実施例のＳ１５００からＳ１５１０よりなるメイン・ルーチン・フロー・チャート、図３２はＳ１６００からＳ１６２０よりなるその姿勢演算および制御サブルーチン・フロー・チャート、図３３はＳ１７００からＳ１７０６よりなるそのコンプライアンスおよび姿勢安定化制御サブルーチン・フロー・チャートである。
【０１２６】第６実施例の場合は、第４実施例に比べ、モデルの将来の挙動の不確定性が小さいので、より確実かつ長期的な姿勢安定化が実現される。
【０１２７】尚、第６実施例では、第１モデルと第２モデルの偏差を歩容の切り替わり目で強制的に０にしていたが、それ以外の時期でも偏差が過大になったとき、第１モデルと第２モデルロボットの偏差を０に収束させる様に第１モデルに操作モーメントを加える制御を実行しても良い。但し、第１モデルに操作モーメントを加えると、上体の位置がずれて第１モデルの姿勢が崩れようとする。そこで、姿勢の崩れを防ぐために、第１モデルに操作モーメントを加えるときには同時に着地位置などの歩容パラメータも修正する。ところで、着地直前に、着地位置や着地時期を大きく変更すると、脚の挙動が急激に変化する。即ち、着地までの時間余裕が少なくなってくるにつれ、着地位置や着地時期の修正可能量は小さくなる。従って、第１モデルに操作モーメントを加え、それに見合った着地位置などの修正を行う際には、着地までの時間余裕が少なくなるにつれ、操作モーメントの印加量の制限値も小さくするべきである。
【０１２８】更に、第５、第６実施例においては、動力学モデルが２段に構成されている。これを更に段数を増やし、実ロボットとモデルとの偏差、およびモデルと他のモデルとの偏差を制御する様にしても良い。上位のモデル（即ち、多段連結モデルのうちで実ロボットから遠く離れているモデル）ほど実ロボットに作用する外乱の影響が小さくなり、モデルの挙動予測精度が高くなる。従って、上位のモデルの挙動を基に着地位置修正量を求めれば、上位のモデルの姿勢安定性が確保でき、それより下位のモデルはモデル操作モーメントを制御することによって上位のモデルに追従するので、結果的に実ロボットと全モデルすべての姿勢安定性が確保される。摂動モデルを用いた場合も同様である。
【０１２９】ここで、第２実施例などで用いたリミッタおよび分配器について説明を補足する。リミッタおよび分配器の目的は、実ロボットの接地性を確保しながら、制御操作量を分配することであるが、リミッタの変形例としては、第２実施例で挙げた例以外に、以下の様な手法やそれらの組み合わせが考えられる。
ａ．制御則によってモーメントの次元で算出される操作量に、一定の制限を加える手法ｂ．床反力の床垂直方向成分の実際値あるいは設計値に応じて、モーメントの次元で算出される操作量の制限値を変動させる手法この場合、（ＺＭＰのずれ量＝モーメント／床反力垂直成分）であるから、リミッタは、ＺＭＰ目標位置からＺＭＰ存在領域の境界までの距離余裕×床反力垂直成分の値に応じて変動させる方が、より的確に接地性が確保される。
ｃ．目標ＺＭＰ位置からＺＭＰ存在可能領域の境界までの余裕によって、モーメントの次元で算出される操作量の制限値を変動させる手法例えば、平地歩行では両脚支持期の間はＺＭＰ存在可能領域が大きいので、実ロボットに大きな復元力を発生させることができる。
ｄ．制御則によって求められる操作量を一旦ＺＭＰ位置のずれに変換し、これに目標ＺＭＰ位置からＺＭＰ存在可能領域の境界までの余裕に応じた制限を加える手法ｅ．操作量がＰＤ制御則によって求められる場合、Ｐ成分から求められる操作量にのみ制限を加え、発振を抑えるダンピング効果を持つＤ成分には制限を加えない手法、あるいはその逆の手法ｆ．実床反力モーメントＭact が限界値を超えない様に、Ｍact に応じて制限値が可変となるリミッタに、実床反力操作モーメントＭactcomm を通す手法ｇ．Ｍact が限界値を超えたら、超えた分に、あるゲインを乗じてＭactcomm から減算する手法ｈ．実ロボットの床反力計測値から実ロボットのＺＭＰ位置を求め、これがＺＭＰ存在可能領域を超えそうになったら、それ以上Ｍactcomm の絶対値を増やさない手法
【０１３０】更に、分配を数２４の式の様に単純に比例配分する手法も考えられる。
【０１３１】
【数２４】
【０１３２】また、周波数帯域に応じて分配する手法も考えられる。例えば、実ロボットの床反力操作量Ｍactcomm には、実傾き偏差制御要求制御量Ｍerrdmdとモデル傾き（あるいはモデル上体位置）制御要求制御量Ｍmdldmdの高域成分だけを与え、低域をモデル操作量Ｍmdl に与えれば、リミッタをかけなくても実ロボットの床反力操作量は小さく抑えられるので、接地性は高くなる。
【０１３３】また、第２実施例ではフィードバック則で操作量を求めてから、リミッタおよび分配器に通しているが、逆の順序で求めても良く、あるいは分配器の前後両方にリミッタを挿入する手法なども考えられよう。
【０１３４】更に、第１実施例から第６実施例で述べた歩容についての記述を補足すると、通常、歩容は接地を確保するための床反力に関する要件とその他の拘束条件により記述され、これらを満足する様に関節変位が決定される。拘束条件としては、足平軌道や上体姿勢、関節の変位が無理な挙動をしないための上体高さの決定式などが用いられる。床反力に関する要件としては、代表的なものとして例えば、平面床歩行においてはＺＭＰの存在可能領域が知られている。接地面を含む最小凸多角形がＺＭＰの存在可能領域であり、その中にＺＭＰ目標軌道を設定し、歩容からオイラー・ニュートン方程式などを用いて動力学的に求められるＺＭＰが、ＺＭＰ目標軌道に一致する様に歩容が生成される。また、床反力に関する要件の代わりに、足首トルクの様なある特定の部位に作用する力やモーメントに関する要件を与えても良い。例えば、２足歩行ロボットの片脚支持期においては、支持脚足首は床に近いので、足首トルクは、足首の床への垂直投影点に作用する床反力モーメントと密接な関係を持つからである。
【０１３５】また、動力学モデルでは、目標床反力の要件を満足するモデルの挙動を求めなければならない。目標床反力の要件とは、例えば、ＺＭＰ目標位置にモーメントＭmdl が発生することである。ところで、モデルの足平位置・姿勢と上体の姿勢は歩容生成部から与えられているので、片脚あたり６自由度の実施例に係るロボットでは、上体の姿勢を決定すれば、全関節の変位が決定される。上体のうち、高さは例えば、各瞬間における膝などの関節の変位、速度、加速度などがアクチュエータの能力を超えない様に、かつ床垂直抗力が負にならない様に決定される。従って、上体の位置のうちで、残った上体の前後左右の挙動によって目標床反力の要件を満足させることとなる。しかし、各瞬間における目標床反力の要件を満足する上体前後左右位置を解析的に直接求めることは困難であるので、実際には逆に上体の位置を与え、それに対する床反力を求めながら、擬似的なニュートン法などの収束法により、目標床反力の要件を満足する上体の位置を探索する。尚、演算の高速性を重視する場合には、多少の誤差を生じても、探索回数に制限を加えれば良い。極端な場合、探索回数は１回にしても良いであろう。
【０１３６】また、両脚支持期にはＭact を大きくしても接地性が失われない。そこで、両脚支持期にリミッタ制限値を大きくしたり、モデル上体位置制御則などのゲインを高くしても良い。
【０１３７】また、歩行の環境や目的によって、姿勢を重視したいときと、階段や飛び石上の歩行の様に着地位置精度を重視したいときとがある。そこで、各制御ゲインを可変にすると、状況に応じたより緻密な制御が実現できる。例えば、姿勢安定を重視したいときには、モデル上体位置制御（あるいはモデル傾き制御）の制御ゲインを下げて要求制御量Ｍmdldmdを小さくし、モデル傾き（あるいはモデル上体位置）を０に収束させる制御を主に着地位置などの歩容パラメータ修正によって行う。こうすれば、実ロボットの操作モーメントＭactcomm が小さくなって、実ロボットのＺＭＰ位置がＺＭＰ存在可能領域の中央付近に寄ってくるので、安定余裕が大きくなるからである。但し、その代わり着地位置などの歩容パラメータが大きく修正されるので、着地位置精度は悪くなる。また歩行の環境や目的によって、制御則の周波数ゲイン特性を調整しても、状況に応じたより緻密な制御が実現できる。
【０１３８】尚、上記した第１ないし第６実施例において、実ロボットの床反力のフィードバック制御として、コンプライアンス制御を導入しているが、トルク制御を導入しても良い。床の形状が既知で、床の凹凸による外乱があまり発生しない場合や、メカニカル機構によってコンプライアンスが実現されている場合には、特に床反力フィードバック制御を行わなくても良い。その場合でもこの発明の効果が失われる訳ではない。
【０１３９】更に、上記において２足歩行の脚式移動ロボットを例にとって説明してきたが、それに限られるものではなく、この発明は３足以上の脚式移動ロボットにも妥当し、更には脚式に限らず、車輪型やクローラ型など他の形態の移動ロボットに妥当するものである。
【０１４０】
【発明の効果】請求項１項にあっては、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差に応じた操作量を少なくとも前記モデルにフィードバックしてモデルの挙動を修正する様に構成したので、何らかの外乱などによって前記傾き角（角速度）偏差が非常に大きくなっても、接地性を確保したまま（即ち、足裏が浮かない様にしながら）、前記モデルと実ロボットの床反力の差を活用して大きな姿勢復元作用を得ることができる。またモデルは床と干渉しないから、モデルに追従する実ロボットも床と干渉せず、着地衝撃を小さい値に抑制することができる。
【０１４１】請求項２項にあっては、制御対象のモデルを少なくとも２つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差及び／又は前記モデル間の傾斜などの状態量の偏差に応じた操作量を少なくとも前記モデルのいずれかにフィードバックしてモデルの挙動を修正する様に構成したので、上位のモデル（即ち、多段連結モデルのうちで実ロボットから遠く離れているモデル）ほど実ロボットに作用する外乱の影響が小さくなり、モデルの挙動予測精度が高くなって、実ロボットと全モデルすべての姿勢安定性を一層確実に実現することができる。
【０１４２】請求項３項にあっては、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差に応じた操作量を前記モデルと実ロボットに分配しつつフィードバックしてモデルの挙動を修正する様に構成したので、請求項１項で述べた効果に加え、モデルの姿勢の崩れをある程度は抑制することができる。
【０１４３】請求項４項にあっては、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルに対する実ロボットの傾き偏差に応じて前記モデルに与える床反力目標値を修正し、修正された床反力目標値を満足する様に前記モデルの歩容を修正して前記傾き偏差を収束させる様に構成したので、請求項１項で述べた効果をより直接的に得ることができる。
【０１４４】請求項５項にあっては、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾き偏差に応じた操作量を前記モデルと実ロボットに分配しつつ与えて床反力目標値を修正し、修正された床反力目標値を満足する様に前記モデルも歩容を修正して前記傾き偏差を収束させる様に構成したので、請求項１項で述べた効果をより直接的に得ることができると共に、一層効果的にモデルの姿勢を回復することができる。
【０１４５】請求項６項にあっては、脚式移動ロボットの姿勢安定化制御装置において、実ロボットの力学モデルを基に接地性が保証される様に床反力が設計された基準歩容からの挙動摂動分と、床反力摂動分の関係を模擬する少なくとも１つの摂動動力学モデル、目標歩容と実ロボットの姿勢傾き偏差を検出する手段、および前記偏差を小さくする様に、前記摂動動力学モデルに床反力を与え、それによって生じた摂動動力学モデルの変位に応じて前記基準歩容を修正したものを前記目標歩容として前記実ロボットの関節変位を追従させる手段を備える様に構成したので、請求項１項で述べた効果に加えて、演算量を低減することができ、コンピュータを用いて実現するときもその能力が低いもので足りる。
【０１４６】請求項７項にあっては、脚式移動ロボットの姿勢安定化制御装置において、実ロボットの力学モデルを基に接地性が保証される様に床反力が設計された基準歩容からの挙動摂動分と、床反力摂動分の関係を模擬する少なくとも１つの摂動動力学モデル、実ロボットのリンク構造を表す幾何学モデル、前記幾何学モデルと実ロボットの姿勢傾き偏差を検出する手段、および前記偏差を小さくする様に前記摂動動力学モデルに床反力を加え、それによって生じた摂動動力学モデルの変位に応じて前記基準歩容を前記幾何学モデル上で修正し、幾何学モデルの関節変位を目標として前記実ロボットの関節変位を追従させる手段を備える様に構成したので、請求項６項で述べた効果に加えて、床との干渉をより小さくすることができる。
【０１４７】請求項８項にあっては、少なくとも着地位置などの歩容パラメータを修正して前記モデルの姿勢を安定に保つ様に構成したので、従前までの請求項で述べた効果に加えて、一層簡易にモデルの姿勢を安定に回復することができる。
【０１４８】請求項９項にあっては、少なくとも新たな床反力を加えて前記モデルの姿勢を安定に保つ様に構成したので、請求項８項で述べたと同様の効果を得ることができる。
【０１４９】請求項１０項にあっては、着地位置などの歩容パラメータを修正すると共に、新たな床反力を加えて前記モデルの姿勢を安定に保つ様に構成したので、従前までの請求項で述べた効果に加えて、一層効果的にモデルの姿勢を安定に回復することができる。
【図面の簡単な説明】
【図１】図１は本出願人が先に提案したＺＭＰ制御を示す説明図である。
【図２】図１に示す制御において実ＺＭＰが目標ＺＭＰ（モデルから設定されるＺＭＰ）からずれた状態を示す説明図である。
【図３】この発明に係る脚式移動ロボットの姿勢安定化制御装置の特徴（原理）を示す説明図である。
【図４】この発明の原理（特徴）を説明するものであって、単純な実ロボットの構造パラメータと実ロボットの基本局所制御を示す説明図である。
【図５】図４に例示したものとこの発明の原理（特徴）を比較する説明ブロック図である。
【図６】図５に示すブロック図を変形したものを簡略的に示す説明図である。
【図７】この発明に係る脚式移動ロボットの姿勢安定化制御装置を全体的に示す概略図である。
【図８】図７に示す制御ユニットのブロック図である。
【図９】この発明の第１実施例の動作を示すブロック図である。
【図１０】第１実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図１１】図１０フロー・チャートのうちの姿勢演算および制御のサブルーチン・フロー・チャートである。
【図１２】図１０フロー・チャートのうちのコンプライアンス制御のサブルーチン・フロー・チャートである。
【図１３】この発明の第２実施例の動作を示すブロック図である。
【図１４】第２実施例で使用するリミッタおよび分配器を説明するブロック図である。
【図１５】第２実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図１６】図１５フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図１７】図１５フロー・チャートの中のコンプライアンスおよび姿勢安定化制御のサブルーチン・フロー・チャートである。
【図１８】この発明の第３実施例の動作を示すブロック図である。
【図１９】第３実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図２０】図１９フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図２１】図１９フロー・チャートの中のコンプライアンスおよび姿勢安定化制御のサブルーチン・フロー・チャートである。
【図２２】この発明の第４実施例の動作を示すブロック図である。
【図２３】第４実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図２４】図２３フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図２５】図２３フロー・チャートの中のコンプライアンス制御のサブルーチン・フロー・チャートである。
【図２６】この発明の第５実施例の動作を示すブロック図である。
【図２７】第５実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図２８】図２７フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図２９】図２７フロー・チャートの中のコンプライアンスおよび姿勢安定化制御のサブルーチン・フロー・チャートである。
【図３０】この発明の第６実施例の動作を示すブロック図である。
【図３１】第６実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図３２】図３１フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図３３】図３１フロー・チャートの中のコンプライアンスおよび姿勢安定化制御のサブルーチン・フロー・チャートである。
【符号の説明】
１ 脚式移動ロボット（２足歩行ロボット）
２ 脚部リンク
１０Ｒ，１０Ｌ 脚部回旋用の関節
１２Ｒ，１２Ｌ 腰部のロール方向の関節
１４Ｒ，１４Ｌ 腰部のピッチ方向の関節
１６Ｒ，１６Ｌ 膝部のピッチ方向の関節
１８Ｒ，１８Ｌ 足首部のピッチ方向の関節
２０Ｒ，２０Ｌ 足首部のロール方向の関節
２２Ｒ，２２Ｌ 足平
２４ 上体
２６ 制御ユニット
３６ ６軸力センサ
【０００１】
【産業上の利用分野】この発明は脚式移動ロボットの姿勢安定化制御装置に関し、より具体的には力学モデルを追従する様にロボットが関節変位されると共に、モデルと実ロボットに傾き偏差が生じたとき、実ロボットの床反力をずらす代わりに、モデルの床反力をずらして姿勢の安定を回復する様にしたものに関する。
【０００２】
【従来の技術】脚式移動ロボット、特に２足歩行の脚式移動ロボットとしては、特開昭６２−９７００５号、特開昭６３−１５０１７６号公報記載のものなどが知られている。また脚式移動ロボットを含むロボットの制御については、「ロボット工学ハンドブック」（日本ロボット学会編、１９９０年１０月２０日）に詳しい。
【０００３】
【発明が解決しようとする課題】ところで、脚式移動ロボット、特に２足歩行の脚式移動ロボットは本来的に安定性が低いことから、外乱などを受けると姿勢が不安定となる。そこでロボットの上体の傾き角度と傾き角速度を検出して姿勢制御する必要がある。その意図から本出願人は先に特願平２−３３６，４２０号（平成２年１１月３０日出願）において、着地候補位置を何点か予め用意しておき、一歩ごとの制御周期でいずれかを選択して目標歩容を切り換える制御を提案している。しかし、この制御によるときは、姿勢が崩れた結果、ＺＭＰ（垂直床反力の作用重心点。Zero MomentPoint)が存在可能範囲の限界付近に偏って接地性が失われても、次の一歩で歩容が修正されて姿勢が復元するまではＺＭＰが限界付近に偏ったままなので、それまで接地性を回復できないと言う不都合がある。
【０００４】そこで本出願人は近時別の出願（平成４年４月３０日、整理番号Ａ９２−０１１６）において、ロボットの力学モデルを基に力学的平衡状態が保証される様に床反力を設定した目標歩容を生成し、モデル化誤差や外乱によって傾き角度と傾き角速度に目標値と実際値との間で偏差が生じたときは、実ロボットの接地部の変位あるいはモーメントを基準歩容から故意にずらすことによって実ロボットの床反力をモデルから計算される目標歩容のそれから故意にずらし、実ロボットの傾きをモデルの傾きに近づける復元力を得る技術を提案している（ここで言う「床反力」は、各脚に作用する床反力を全て合成した、ある作用点に働く合力と合モーメントとを意味する）。
【０００５】しかし提案したこの技術によるときは、例えば２足歩行ロボットの片脚支持期では、実ロボットの床反力を故意に大きくずらそうとして足首角度を大きくずらしても、ＺＭＰが足平接地面を超える全床反力を発生することができず、足平の一部が浮いてしまうことがある。従って、実ロボットがモデルに対して大きく傾いてしまった場合には、実ロボットの傾斜を正常に戻すために必要な十分な復元力を得ることができないことがあり、そのときは実ロボットが転倒する。即ち、この技術では目標値と実際値との偏差を縮める復元作用がＺＭＰの存在範囲に制約される。
【０００６】図で説明すると、実ロボットの関節変位をモデルの通りに追従する制御においては、実ロボットの傾き角度と傾き角速度とがそれぞれモデルのそれらと一致していれば、図１に実線で示す実ロボットのＺＭＰはモデルから決定される目標ＺＭＰに一致し、ロボットは所期の姿勢で歩行する。しかし、実際には先に述べた様にモデル化誤差や外乱によって破線で示す様に傾き角度と傾き角速度とに偏差が生じ、しかもそのずれは重力の作用によって偏差が大きくなるほど益々大きくなろうとする発散傾向を持つ。ところで実ＺＭＰが目標ＺＭＰからずれると、図２に示す様に、その間の距離ｘに力Ｆを乗じたモーメントＭが目標ＺＭＰのまわりに生じる。換言すれば、実ＺＭＰが故意に目標ＺＭＰからずれる様に関節変位を制御することにより、所望のモーメントを生じさせることができる。先に提案した技術ではその思想に基づき、傾き（角度と角速度）偏差が生じたときは図２R>２に示す様に、故意に実ＺＭＰをずらして姿勢復元方向にモーメントを生じさせる様にした。しかし、この技術によるときは同図に示す如く、実ＺＭＰをずらすことができる範囲は、足平領域ｘsole内に限られる。
【０００７】従って、この発明の目的は近時提案した技術を改良し、更に効果的な脚式移動ロボットの姿勢安定化制御装置を提案することにある。
【０００８】
【課題を解決するための手段】上記した課題を解決するためにこの発明は例えば請求項１項に示す如く、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態の偏差に応じた操作量を少なくとも前記モデルにフィードバックしてモデルの挙動を修正する様に構成した。
【０００９】
【作用】
【００１０】重力作用によって発散しようとする傾き偏差を零に収束させようとする作用は、実ロボットとモデルの床反力の差によって生じるものである。従って、偏差を収束させるためには、実ロボットの床反力をずらす代わりに、モデルの床反力を反対側にずらすことによっても等価な効果を得ることができる筈である。本発明はこの効果を利用する。そして、それによって図３に示す様に、ずらすＺＭＰがモデルのそれであることから、ＺＭＰをずらす範囲はもはや足平直下の領域内に限られることがなく、足平接地面を遠く超えた位置までずらすことができる。言い換えれば、前記した距離ｘを大きくすることができ、結果的にモーメントを大きくすることができて大きな姿勢復元力を得ることができる。
【００１１】以下、更に説明する。分かり易くするために図４に示す様に、足平と１リンクの脚と上体から構成される単純なロボットを考える。脚の質量をｍ、足首まわりの脚の慣性モーメントをＩ、脚重心の足首との距離をｈとする。上体の質量および慣性モーメントは０とする。また足首の床からの高さも０とする（こうすると、床反力と足首モーメントとが一致して議論が簡単になる）。鉛直方向に対する脚の傾き（即ち、足首と重心とを結ぶ線分の傾き角度）をθlink、鉛直方向に対する上体の傾きをθtrunk 、腰関節の曲げ角をθhip とする。尚、図でθlinkとθtrunk は負、θhip は正とする。更に、足首に作用するモーメントをＭact とする( 足首の床からの高さが０であるから、床反力もＭact である）。重力加速度はｇとする。足首には公知のトルク制御が施されるものとし、腰の曲げ角は変位制御されるものとする。
【００１２】上記において、幾何学的関係から数１の式が成立する。
【００１３】
【数１】
【００１４】このロボットの脚の運動方程式は数２の様になる。
【００１５】
【数２】
【００１６】これをラプラス変換して数３の式を得る。
【００１７】
【数３】
【００１８】さて、ここで、この実ロボットと全く同じ構造と全く同じパラメータ値を持つ数式モデルを用意する。その鉛直方向に対する脚の傾き（即ち、前記と同様な足首と重心とを結ぶ線分の傾き）をθmdllink 、鉛直方向に対する上体の傾きをθmdltrunk、腰関節の曲げ角をθmdlhipとし、モデルの足首に作用するモーメント( 即ち、モデルの床反力) をＭmdl とすると、幾何学的な数式モデルと動力学的な数式モデルは、数４、数５に示す様になる。
【００１９】
【数４】
【００２０】
【数５】
【００２１】次に、モデルを基に目標歩容を作成する。目標歩容は、一般的にはモデルの初期状態、モデルに加えられる操作量、および拘束条件によって表現される。具体的には、脚の初期傾き角度θreflink0と脚の初期傾き角速度ωreflink0、足首モーメントの時間関数Ｍref(t)、および上体の傾き角度の時間関数θreftrunk(t)によって表現される。上体の傾き角度の時間関数θreftrunk(t) は、議論を単純化するために、ここでは常に０であるものとする。
【００２２】次に、以下の制御を実施する。
【００２３】モデルの上体の傾き角度θmdltrunkを常にθreftrunk(t) に一致させる( θreftrunk(t) は０としているので、θmdltrunkも常に０に維持される）。モデルの足首には、Ｍref(t)と後述する上体の傾き角偏差に応じた操作量Ｍactfb の和に、実際値Mactに一致する様にトルク制御が実施される。実ロボットの腰の曲げ角度θacthipはモデルの腰の曲げ角θmdlhipを目標変位として変位制御が施される。即ち、θacthipは、θmdlhipに一致する様に制御される。実ロボットの上体には、傾斜センサが備えられており、上体の傾き角θacttrunkが検出される。
【００２４】モデルに対する実ロボットの上体傾き角偏差θerr は数６の式で求められる。この例では、実ロボットの上体傾き角偏差θerr は、θmdltrunkが常に０に維持されているから、θacttrunkそのものである。
【００２５】
【数６】
【００２６】制御の目的は、歩行中においてモデルに対する実ロボットの上体傾き角偏差θerr を０に収束させることである。先に述べた近時提案した技術では、上体の傾き角偏差に応じた操作量Ｍactfb を算出し、実ロボットの足首モーメントＭactに加えることによりフィードバックループを構成していた。これに対し、この発明による制御装置では、上体の傾き角偏差に応じた操作量Ｍmdlfb をモデルの足首モーメントＭmdl に加えることによりフィードバックループを構成する。
【００２７】この発明による制御と近時提案した制御の効果を比較するために、Ｍactfb とＭmdlfb から上体の傾き角偏差θerr までのブロック図を図５に示す。但し、この図ではθerr からＭactfb へのフィードバックとθerr からＭmdlfb へのフィードバックを省略した。
【００２８】さて、トルク制御部と変位制御部の伝達関数は、θactlink の挙動に比べて制御の応答性が十分高ければ１とみなすことができる。このとき、図５を、Ｍactfb,Ｍmdlfb およびθerr の関係のみに着目して変形すると、図６が得られる。この図から明らかな様に、例えばＨ（Ｓ）をフィードバック則とすると、Ｍact に数７の式で求められるＭactfb をフィードバックすることと、Ｍmdl に数８の式で求められるＭmdlfb をフィードバックすることは、θerr の挙動に対して同一の効果を持つ。
【００２９】
【数７】
【００３０】
【数８】
【００３１】更に、θerr の挙動を決定するものは、Ｍactfb とＭmdlfb との差であるから、数９の式となる様に、Ｈ（Ｓ）をＭactfb とＭmdlfb に分配すれば、この手法もやはり同一の効果を持つ。
【００３２】
【数９】
【００３３】以上が、この発明の根本原理である。実ロボットが複雑なものであっても、そのダイナミクスを忠実に模擬するモデルを用意すれば、実ロボットの床反力とモデルの床反力の差を制御することにより、上記例と全く同一原理で同一の効果を得ることができる。後で実施例で示す様に、動力学モデルを基準歩容からの摂動分で表現しても、θerr とその微分値ωerr が微小であれば、同一の効果が得られる。
【００３４】この構成により、近時提案した制御と異なり、実ロボットの床反力ＺＭＰ位置を本来あるべき位置から故意にずらす必要がないので、何らかの外乱などによって前記傾き角（角速度）偏差が非常に大きくなっても、接地性を確保したまま（即ち、足裏が浮かない様にしながら）、近時提案した制御では不可能であった大きな姿勢復元作用を得ることが可能となり、短時間で偏差を０に収束させることができる。また、モデルは床と干渉しないから（但し、実ロボットの全床反力をずらすために故意に干渉させる分は除く）、モデルに追従している実ロボットも床と干渉せず、着地衝撃を小さい値に抑制することも可能となる。
【００３５】尚、上記において、足平が浮かないためには、Ｍact(上記例ではＭref ＋Ｍactfb)は、ある範囲内に制限されなければならない( 例えば、平地歩行では実ロボットのＺＭＰが足平の接地面を超えない範囲に限定される）。従って、Ｍact がこの制限を超えない様な値をとり、かつ数１０の式を満足する様に、Ｍactfb とＭmdlfb は決定されるべきである。
【００３６】
【数１０】
【００３７】尚、Ｍactfb からθmdllink までのオープンの伝達関数とＭmdlfb からθmdllink でのオープンの伝達関数は一致しないので、近時提案した制御とこの発明による制御とでは、θmdllink の挙動に関して特性が異なる。
【００３８】
【実施例】以下、脚式移動ロボットとして２足歩行の脚式移動ロボットを例にとって、この発明の実施例を説明する。図７はそのロボット１を全体的に示す説明スケルトン図であり、左右それぞれの脚部リンク２に６個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。該６個の関節は上から順に、腰の脚部回旋用（ｚ軸まわり）の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、腰のロール方向（ｘ軸まわり）の関節１２Ｒ，１２Ｌ、同ピッチ方向（ｙ軸まわり）の関節１４Ｒ，１４Ｌ、膝部のピッチ方向の関節１６Ｒ，１６Ｌ、足首部のピッチ方向の関節１８Ｒ，１８Ｌ、同ロール方向の関節２０Ｒ，２０Ｌとなっており、その下部には足平２２Ｒ，２２Ｌが取着されると共に、最上位には上体（筐体２４）が設けられ、その内部には制御ユニット２６が格納される。
【００３９】上記において腰関節は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から構成され、また足関節は、関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また、腰関節と膝関節との間は大腿リンク３２Ｒ，３２Ｌで、膝関節と足関節との間は下腿リンク３４Ｒ，３４Ｌで連結される。ここで、脚部リンク２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６×２＝１２個の関節（軸）をそれぞれ適宜な角度に駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行することができる様に構成される。先に述べた様に、上記した関節は電動モータからなり、更にはその出力を倍力する減速機などを備えるが、その詳細は先に本出願人が提案した出願（特願平１−３２４２１８号、特開平３−１８４７８２号）などに述べられており、それ自体はこの発明の要旨とするところではないので、これ以上の説明は省略する。
【００４０】図７に示すロボット１において、足首部には公知の６軸力センサ３６が設けられ、足平を介してロボットに伝達されるｘ，ｙ，ｚ方向の力成分Ｆｘ，Ｆｙ，Ｆｚとその方向まわりのモーメント成分Ｍｘ，Ｍｙ，Ｍｚとを測定し、足部の着地の有無と支持脚に加わる力の大きさと方向とを検出する。また足平２２Ｒ（Ｌ）の四隅には静電容量型の接地スイッチ３８（図７で図示省略）が設けられて、足平の接地の有無を検出する。更に、上体２４には傾斜センサ４０が設置され、ｘ−ｚ平面内とｙ−ｚ平面内のｚ軸に対する、即ち、重力方向に対する傾斜角度と傾斜角速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコーダが設けられる。更に、図７では省略するが、ロボット１の適宜な位置には傾斜センサ４０の出力を補正するための原点スイッチ４２と、フェール対策用のリミットスイッチ４４が設けられる。これらの出力は前記した上体２４内の制御ユニット２６に送られる。
【００４１】図８は制御ユニット２６の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ４０などの出力はＡ／Ｄ変換器５０でデジタル値に変換され、その出力はバス５２を介してＲＡＭ５４に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ５６を介してＲＡＭ５４内に入力されると共に、接地スイッチ３８などの出力は波形整形回路５８を経て同様にＲＡＭ５４内に格納される。制御ユニット内にはＣＰＵからなる第１、第２の演算装置６０，６２が設けられており、第１の演算装置６０は後で述べる様に目標関節角度を算出してＲＡＭ５４に送出する。また第２の演算装置６２はＲＡＭ５４からその目標値と検出された実測値とを読み出し、各関節の駆動に必要な制御値を算出し、Ｄ／Ａ変換器６６とサーボアンプを介して各関節を駆動する電動モータに出力する。
【００４２】続いて、この制御装置の動作を説明する。
【００４３】図９はその動作を示すブロック図であり、図１０ないし図１２はその動作を示すフロー・チャートである。図１０以降のフロー・チャートを参照して具体的に説明する前に、図９ブロック図を参照してこの制御を概説する。尚、以下ではｘ方向（進行方向）の制御だけを例にとって説明しているが、ｙ方向（横方向）も同様である。
【００４４】図９に示す様に、この実施例においてロボットの幾何学モデルとしては剛体モデルを備え、ロボットの動力学モデルとしては、基準歩容からの上体位置摂動分の挙動を倒立振子で近似したものを備える。倒立振子の質量をｍ、支点まわりの慣性モーメントをＩ、支点と重心の距離をｈとする。基準歩容は、上体２４の位置・姿勢、および両足平２２Ｒ（Ｌ）の位置・姿勢で表現される。両足平２２Ｒ（Ｌ）の位置・姿勢については、一歩ごとの足平軌道パラメータ（例えば、遊脚着地位置や着地時刻など）が、大局的姿勢制御部から目標足平軌道生成部に与えられ、目標足平軌道生成部においてリアルタイムに各瞬間のそれらの位置や姿勢が生成される。尚、この制御においてはロボットの姿勢が崩れたとき、先ずモデルの姿勢をそれに合わせて崩し、次いで安定方向に回復させると共に、実ロボットをそれに追従制御して姿勢安定を回復する様にしていることから、ここで「大局的」なる語は、瞬間々々ではなく、比較的長い時間をかけて滑らかに姿勢回復を最終的に実現する如く、この制御の特質を示す意味で使用する。
【００４５】またＺＭＰ目標軌道については、一歩ごとのＺＭＰ目標軌道パラメータ（例えば、折れ線表現されているときは折れ点の座標や時刻）が、大局的姿勢制御部からＺＭＰ目標軌道生成部に与えられ、ＺＭＰ目標軌道生成部においてリアルタイムに各瞬間のその位置が算出される。上体２４の位置・姿勢については、ＺＭＰ目標軌道を満足する様に、予めオフラインで作成されて時系列データとして大局的姿勢制御部に記憶され、歩行時に吐き出される。尚、以降、足平軌道パラメータとＺＭＰ目標軌道パラメータなど歩容の特徴を表現するものを総称して「歩容パラメータ」と呼ぶ。
【００４６】次いで、幾何学モデルに対する実ロボットの姿勢の傾き角偏差θerr と角速度偏差ωerr を検出する（実ロボット１の関節変位が忠実にモデルに追従すると考えられるならば、姿勢の傾き角（角速度）偏差は、上体２４の傾き角（角速度）偏差で代表して差し支えない。あるいは重心位置のずれを用いても良い。）。
【００４７】これらを状態量として、これらを０に収束させようと働く安定化制御則から求められる制御操作量（状態フィードバック量）は、モーメントの次元で検出されて倒立振子型動力学モデルの支点に与えられる。ここで、この操作量を先に述べたと同様に、モデル操作モーメントＭmdl と呼ぶ。Ｍmdl は、倒立振子型動力学モデルが受ける床反力モーメントであるとも言える。倒立振子型動力学モデルは、Ｍmdl と重心に作用する重力の影響を受けて運動する。倒立振子の状態量、即ち、傾き角θmdl とその角速度ωmdl の挙動は、過去の状態と支点に加えられるモーメントＭmdl を基に逐次計算される。
【００４８】数１１の式により、上体２４のｘ方向のずれΔｘが求められる。但し、ｈtrunk は上体２４の高さ（より正確に表現すれば、床から腰関節１（Ｌ）までの高さ）とする。
【００４９】
【数１１】
【００５０】ロボット幾何学モデルの上体位置は、歩容パラメータの上体位置ｘにΔｘが加えられた位置に指定される。
【００５１】また、先に述べた近時提案した技術で用いられるコンプライアンス制御によって、ＺＭＰ目標位置まわりの実床反力モーメントＭact に応じて、基準歩容の足平位置・姿勢が修正され、それが幾何学モデルに与えられる。図９で用いられている制御では、Ｍact から床反力モーメントフィードバック則によって仮想床傾斜角指令θctrlが求められ、モデルの足平位置は、モーメント足平軌道生成部で生成される基準歩容の足平位置をＺＭＰ目標位置を中心として仮想床傾斜角指令の分だけ、座標回転させられる。これにより、実ロボット１の姿勢傾きの挙動は、基準点（例えば、ＺＭＰ目標位置）を支点とし、支点に床から傾きに比例した床反力モーメントが発生する倒立振子に近似される。
【００５２】以上によって求められ、修正された上体位置・姿勢から、逆キネマティクス計算によって、幾何学モデルの各関節の変位が求められる。尚、このロボットでは可動部はすべて回転自由度しかないから、関節の変位はすべて角度で表現される。実ロボット１には、その関節変位をロボット幾何学モデルの関節変位に追従させる制御システムが備えられる。
【００５３】以上のシステムにおいて、状態量θerr とωerr に応じたモデル操作モーメントＭmdl をロボット動力学モデルに与えるフィードバック制御は、先に作用欄で説明した原理によって、θerr とωerr を０に収束させる様に作用する。即ち、実ロボットとモデルの傾きが一致する様に作用する。
【００５４】ここで、図９において、倒立振子型動力学モデルにモデル操作モーメントＭmdl が加わって、モデルの傾きθmdl とその角速度ωmdl が中立点（直立状態）から一旦ずれると、このモデルは不安定系であるので、θmdl ，ωmdl は発散する。そこで、状態量θmdl ，ωmdl に応じて、次の遊脚着地位置または着地タイミングを変更することによってθmdl ，ωmdl の発散を抑制する。
【００５５】ところで、基準歩容におけるある一歩（これを「第１歩容」と呼ぶ）において着地位置を変更すると、次の一歩（これを「第２歩容」と呼ぶ）の初めでは、支持脚足平の位置だけが、基準の第２歩容の位置からずれる。このことを、第２歩容の基準座標から見ると、支持脚足平は基準歩容のままで、遊脚足平と上体の位置が変更量だけずれたことに相当する。そのまま放置すると、第１歩容と第２歩容の境界が不連続になる。そこで、ロボット幾何学モデルの歩容が連続となる様に、歩容の変わり目において次の操作を行う。
【００５６】遊脚軌道は初期位置や次期着地位置などの足平軌道パラメータで与えられているので、第２歩容の足平軌道パラメータの一つである初期遊脚位置を−Δｘfoottotal だけずらす。第２歩容の上体軌道はそのままとし、その代わりに倒立振子型動力学モデルの傾き角θmdl を−Δｘfoottotal/ｈtrunk だけずらす。この結果、ロボット幾何学モデルの歩容の連続性を保ちつつ、倒立振子型動力学モデルの状態量の一つである傾き角θmdl を−ｘfoottotal/ｈtrunk だけ操作することができる。即ち、着地位置を変更することにより、倒立振子型動力学モデルの状態量を制御することができる。
【００５７】上記を前提とし、以下図１０フロー・チャートを参照して、着地位置を進行方向に変更した場合のモデル傾き制御アルゴリズムを説明する。
【００５８】先ず、Ｓ１０において倒立振子型モデルを安定な直立位置に初期化し、Ｓ１２に進んで歩数カウンタが基準歩容で設定した目標歩数になるまで、Ｓ１４からＳ２６を繰り返す。ここで、用語を次の様に定義する。
１．一歩の歩容は、両脚支持期の初めから次の遊脚が着地するまでの期間を指す。
２．ある歩容の支持脚とは、その歩容の片脚支持期に支持脚となる脚とし、遊脚とは、その歩容の片脚支持期に遊脚となる脚とする。
３．歩容の基準座標を、その一歩の支持脚の足平接地点にとることとする。
【００５９】Ｓ１４では今回の歩容パラメータをセットし、Ｓ１６に進んで前回の歩容の修正後の着地位置と前回の歩容の基準歩容の着地位置との差を前記したΔｘfoottotal とする。次いで、Ｓ１８に進んで今回の歩容の基準初期遊脚位置からΔｘfoottotal を減算し、Ｓ２０に進んで倒立振子型モデルの傾斜角θmdl から、Δｘfoottotal ／ｈtrunk だけ減算する。
【００６０】次いでＳ２２に進み、一歩分のサンプリング回数だけＳ２４とＳ２６を繰り返す。即ち、Ｓ２４で１周期が経過したことを確認した後、Ｓ２６に進んで姿勢演算および制御のサブルーチンを実施する。
【００６１】図１１はそれを示し、先ずＳ１００において実ロボットの傾斜を計測し、前記した様に幾何学モデルに対する姿勢傾き角偏差θerr と傾き角速度偏差ωerr を求め、Ｓ１０２に進んでそれらから前記したモデル操作モーメントＭmdl を求め、Ｓ１０４に進んでＭmdl を倒立振子型モデルの支点に加えてモデルの今回の傾斜角θmdl と角速度ωmdl を求める。次いでＳ１０６に進み、前記した理由から上体ずれΔｘを図示の如く求め、Ｓ１０８に進んで着地までに時間の余裕があれば、倒立振子型モデルの挙動を予測して着地目標位置パラメータの修正量Δｘfootを求め、基準歩容の着地目標位置パラメータにΔｘfootを加算し、着地目標位置パラメータを修正する。
【００６２】これについては先にも述べたが、更に敷衍すると、着地位置の変更量は、次の一歩の間に、倒立振子型動力学モデルの傾き角θmdl と傾き角速度ωmdl 、および幾何学モデルに対する実ロボットの傾き偏差θerr と傾き角速度偏差ωerr が、なるべく０に収束する様に決定するべきである。そのためには、θmdl ，ωmdl ，θerr ，ωerr などの現在状態から、着地位置の変更により、次の一歩の間にθmdl ，ωmdl ，θerr ，ωerr がどの様な挙動を示すかを推測しなければならない。
【００６３】θerr とωerr はこの発明の根本原理によって０に収束するので、やや予測精度が低下するが、θerr とωerr の現在状態を無視しても支障ない。つまり、θmdl とωmdl の現在状態だけから予測計算して着地位置の変更量を決定しても差し支えない。あるいは、予め、現在状態と着地位置の変更量との関係をシミュレーションによりマップ化しておいて、実際の歩行時にこのマップを用いて着地位置の変更量を決定しても良い。
【００６４】θmdl とωmdl の現在状態だけから着地位置の変更量を決定する手法としては、竹馬ロボットなどで行われている様に、倒立振子型動力学モデルの着地瞬間の全エネルギ予測値が直立時の全エネルギに一致する様に、変更量を決定する手法が一般的に知られている。以下にその手法を説明する。
【００６５】着地瞬間の全エネルギ予測値が直立時の全エネルギに一致するためには、予測される着地直後のθmdl とωmdl が数１２の式を満足する様に、着地位置が決定されなければならない。
【００６６】
【数１２】
【００６７】また、直立状態に近づいていくために、θmdl とωmdl が逆極性でなければならないから、即ち、数１３の式を満足しなければならない。
【００６８】
【数１３】
【００６９】数１２式、数１３式より、数１４の式が得られる。
【００７０】
【数１４】
【００７１】数１４式を近似すれば、数１５の式が得られる。
【００７２】
【数１５】
【００７３】つまり、予測される直立直後のωmdl とθmdl との比が、数１５式で定められる比になる様に着地位置修正量Δｘfootが決定されれば良い。着地位置を修正しない場合の予測される着地直後のωmdl とθmdl は、倒立振子で現在のωmdl ，θmdl ，θerr ，ωerr から動力学演算により求めることができ、それをωmdleとθmdleとする。（特に、ωmdl およびθmdl に比べてθerr およびωerr が無視できるほど小さい場合には、解析的に容易に求めることができる。）。θmdleをωmdle／平方根（２ｍｇｈ／Ｉ）に修正できれば、数１５式を満足する。そのためには、前述の様に、着地位置をΔｘfootだけ修正することによって、θmdlを−Δｘfoot／ｈtrunk だけ操作することができるので、着地位置修正量Δｘfootを数１６の式より決定すれば良い。
【００７４】
【数１６】
【００７５】以上の様に、着地位置修正量Δｘfootが決定され、着地目標位置パラメータｘfootは、歩容の途中で修正される。この修正は、着地目標位置を変更したときの足平の軌道生成計算が大変な場合には、一歩ごとに１つずつだけ実行されることとなるが、足平の軌道を本出願人が近時提案した別の技術（平成４年５月２２日出願、整理番号Ａ９２−０４９３）を用いて発生させれば、着地目標位置の変更をサンプリング周期毎に行うことができる。なぜなら、この技術を用いれば、着地目標位置パラメータの変更をサンプリング周期ごとに行っても、足平の軌道は、そこからまた滑らかに修正されるからである。但し、着地時刻に近づいてから着地位置を大幅に変更すると、遊脚軌道が急激に変わるので、着地までの残り時間に応じて再修正の可能限界量を求め、それを制限値として修正量の変化率にリミットをかけるのが望ましい。
【００７６】図１１フロー・チャートにおいては次いでＳ１１０に進んでＺＭＰ目標軌道パラメータからＺＭＰ目標位置を算出し、Ｓ１１２に進んで着地目標位置パラメータなどの足平軌道パラメータから両足平の位置・姿勢を算出し、Ｓ１１４に進んで基準歩容の上体位置にθmdl とｈtrunk の積を加算した位置を上体目標位置とし（上体の目標姿勢は基準通りとする）、Ｓ１１６に進んでモデル足平修正によるコンプライアンス制御を行う。
【００７７】図１２はそれを示すサブルーチン・フロー・チャートであり、先ずＳ２００において６軸力センサ３６の検出値を取り込み、Ｓ２０２に進んでそれからＺＭＰ目標位置まわりの実床反力モーメントＭact を求め、Ｓ２０４に進んで図示の様に座標回転角θctrlを算出し、Ｓ２０６に進んでその値だけ両足平の位置・姿勢をＺＭＰまわりに回転させ、幾何学モデルの両足平の位置・姿勢とする。尚、この制御は先の出願（平成４年４月３０日）に詳細に述べられているので、ここではこの程度の説明に止める。
【００７８】次いで、図１１フロー・チャートに戻り、Ｓ１１８に進んで上体目標位置・姿勢と修正された足平位置・姿勢から幾何学モデルの関節変位を求め、Ｓ１２０に進んで実ロボットの関節変位を幾何学モデルの変位に追従させる制御を行う。
【００７９】この実施例は上記の如く、モデルに対する実ロボットの傾き制御と倒立振子モデルの傾き偏差の一歩ごとの離散制御により、安定な歩行を実現することができる。更に、故意にずらすＺＭＰをモデルのそれとしたので、先に提案した制御に比べてＺＭＰの移動範囲を大きくとることができ、姿勢復元力を大きくすることができる。従って、何らかの外乱によって傾き偏差が非常に大きくなっても、接地性を確保したまま、姿勢を安定に回復することができる。また、姿勢が崩れたときはモデルの姿勢をそれに応じて一旦崩し、その後に姿勢を回復させると共に、実ロボットをそのモデルの姿勢を追従する様に制御することから姿勢回復を滑らかな動きで実現することができる。更に、モデルは床と干渉しないことから、モデルに追従する実ロボットも床と干渉することがなく、また着地衝撃も小さい値に抑制することができる。
【００８０】図１３はこの発明の第２実施例を示すブロック図である。第１実施例では倒立振子モデルの傾斜制御において、着地位置を操作量としていたが、第２実施例ではモデル操作モーメントに、新たに倒立振子モデルの傾斜に応じた操作量を加えて安定化を図る様にした。
【００８１】また第２実施例においては、幾何学モデルに対する実ロボットの傾き角偏差θerr をＰＤなどのフィードバック則Ｈ１（Ｓ）を通して得られる実傾き偏差制御要求制御量Ｍerrdmd（即ち、Ｍerrdmd＝Ｈ１（Ｓ）θerr ）と、倒立振子モデルの傾斜角θmdl をフィードバック則Ｈ２（Ｓ）を通して得られるモデル傾き制御要求制御量Ｍmdldmd（即ち、Ｍmdldmd＝Ｈ２（Ｓ）θmdl ）をリミッタおよび分配器に通して実モーメント操作量Ｍactcomm とモデルモーメント操作量Ｍmdl に分配する様にした。
【００８２】図１４にリミッタおよび分配器を示す。この例においては、実モーメント操作量Ｍactcomm は、ＭerrdmdとＭmdldmdの和にリミッタをかけることによって求められる。このリミッタ作用により、実ロボットの床反力が存在可能領域から超えない様に管理され、実ロボットの接地性が確保される（例えば、平地歩行であれば、床反力のＺＭＰが接地面を含む最小凸多角形の中にあることが、床反力の存在可能条件である。）。
【００８３】モデルモーメント操作量Ｍmdl は、Ｍmdldmdに前記リミッタの入力超過分を加えることによって求められ、第１実施例と同様、倒立振子型ロボット摂動動力学モデルに入力される。また、ＺＭＰまわりの実床反力モーメントＭact から床反力モーメントフィードバック則によって、床反力モーメントフィードバック操作量θcompが求められる。Ｍactcomm とコンプライアンス定数Ｋcompの積に床反力モーメントフィードバック操作量θcompを加えることにより、仮想床傾斜角指令が求められる。幾何学モデルの足平位置は、目標足平軌道生成部で生成される基準歩容の足平位置をＺＭＰ目標位置を中心として、仮想床傾斜角指令の分だけ、座標回転させられる。
【００８４】実ロボットの傾き角偏差θerr の挙動を決定するものは、ＺＭＰまわりの実床反力モーメントＭact とモデルモーメント操作量Ｍmdl の差である。ところで、コンプライアンス制御が十分に柔らかければ、Ｍact は実モーメント操作量Ｍactcomm にほぼ一致する。従って、この場合、実ロボットの傾き角偏差θerr の挙動を決定するものは、Ｍactcomm とＭmdl の差であると言っても良い。Ｍactcomm とＭmdl の差は、前記リミッタが働いても働かなくても常にＭerrdmdとなるので、幾何学モデルに対する実ロボットの傾き角偏差θerr を０に収束させる制御は常に働く。
【００８５】一方、モデルの傾き角θmdl の挙動を決定するものは、Ｍmdl である。Ｍmdlは、前記リミッタが働いていないときにはＭmdldmdに一致し、モデルの傾きを０に収束させる制御が正常に働く。前記リミッタが働いているときには、幾何学モデルに対する実ロボットの傾き角偏差θerr を０に収束させる制御を優先した上で、可能な範囲でモデルの傾きを０に収束させようと作用する。更に、Ｍerrdmdがリミッタ設定値に比べて過大である場合にはモデルの傾き制御が犠牲になり、モデルの傾きは発散しようとすることもある。即ち、実ロボットのＺＭＰが存在可能領域の限界までまだ余裕があるときのみ、倒立振子モデル傾斜制御が実行される。従って、瞬間的な外乱や微小外乱に対しては、倒立振子モデルが直立状態に回復できるが、大きな外乱が継続すると、倒立振子モデルが直立状態に回復できなくなり、ロボット幾何学モデルの姿勢が大きく崩れて転倒する。しかし、足平の軌道が基準歩容通りであるので、階段や飛び石の上を歩行するときの様に、着地位置が制約されている際には、第２実施例は好適である。
【００８６】ここで、倒立振子モデルの傾き制御ゲインの設定について説明する。モデルの傾き角θmdl を０に収束させる制御則を、例えば数１７の式で与えるものとする。
【００８７】
【数１７】
【００８８】単純倒立振子を、現在状態から、支点にモーメントを加えず、フリーな運動をさせるときに、直立状態で静止するためには、現在の全エネルギが直立静止状態の全エネルギに一致していなければならない。つまり、数１８の式が成立していなければならない。
【００８９】
【数１８】
【００９０】また、直立状態に近づいていくために、ωmdl とθmdl が逆極性でなければならないから、数１９の式を満足しなければならない。
【００９１】
【数１９】
【００９２】数１８、数１９式より、数２０の式を得る。
【００９３】
【数２０】
【００９４】数２０式を近似して数２１の式を得る。
【００９５】
【数２１】
【００９６】数２１式を満足した状態にあれば、倒立振子モデルの支点にモデル操作モーメントＭmdl を加えなくても、直立状態で静止する。従って、数２１式を満足する状態でモデル操作モーメントＭmdldmdが０になる様に制御則を設定すれば、倒立振子モデルに必要以上の余分なモデル操作モーメントＭmdl を発生させず、直立状態に収束させることができる。この様な制御則で数１７の式を満足するものは、数２２の式である。
【００９７】
【数２２】
【００９８】数２２式を満足していても、Ｋωをやや小さめに設定すると、倒立振子の挙動にオーバーシュートが発生する。Ｋωをもっと小さく設定すると、重力モーメントに負けて直立状態に復元できなくなる。直立状態に復元できてかつオーバーシュートが発生しない様にするためには、フィードバックループの特性根が負の実数になることであり、そのためには、Ｋωは次の数２３式を満足しなければならない。
【００９９】
【数２３】
【０１００】数２２と数２３の式を満足する様にゲインを設定すれば、Ｍmdldmdの絶対値が小さく抑えられてＭactcomm の絶対値も小さく抑えられるので、実ロボットの接地性が高くなる。
【０１０１】図１５から図１７は第２実施例の制御を示すフロー・チャートであり、うち図１５はＳ３００からＳ３１０にメイン・ルーチンを、図１６はＳ４００からＳ４２２にその姿勢演算および制御のサブ・ルーチンを、図１７はＳ５００からＳ５０６にそのコンプライアンスおよび姿勢安定化制御のサブ・ルーチンを示す。第１実施例と相違する点に焦点をおいて説明すると、図１５R>５フロー・チャートにおいてＳ３０４とＳ３０６との間には第１実施例の図１０のＳ１６からＳ２０に相当するステップは存在しない。これは、姿勢演算および制御のサブルーチンで着地位置修正を行っていないためである。また、図１６の姿勢演算および制御サブルーチン・フロー・チャートにおいては、先に述べた構成からＳ４０２からＳ４０６が追加されると共に、第１実施例の図１１１１のＳ１０８に相当するステップは削除される。また図１７サブルーチン・フロー・チャートにおいてはＳ５０４の座標回転角の演算で先に述べた様に、実ロボット操作モーメントＭactcomm にコンプライアンス定数Ｋcompを乗じたものが加算される。尚、残余の構成は、第１実施例と相違しない。
【０１０２】第２実施例は足平の軌道が基準歩容通りであることから、階段や飛び石の上を歩行するときなど、着地位置が制約される際に特に好適である。
【０１０３】図１８はこの発明の第３実施例を示すブロック図である。第３実施例では、倒立振子モデルの傾斜制御において、第１実施例と第２実施例の制御を併用した。倒立振子モデルを直立状態に回復させるために、瞬間的な外乱や微小外乱に対しては、主に倒立振子モデルにモーメントを加える制御が働き、大きな外乱が継続すると、主に着地位置や着地タイミングを修正する制御が働く。
【０１０４】第３実施例においても、着地位置や着地タイミングの修正量は、第１実施例と同様、倒立振子モデルが直立状態に戻る様に決定される。第１実施例に比べると、モデル傾き制御要求制御量Ｍmdldmdの影響がある分だけ、モデルの挙動予測演算が複雑になる様に見えるが、モデル傾き制御要求制御量Ｍmdldmdにより、倒立振子モデルの発散が抑えられるので、むしろ予測精度が高いとも言える。特に、第２実施例で述べた倒立振子モデルの傾き制御ゲインの設定値（数２２式と数２３式を満足するゲイン）を採用すれば、倒立振子モデルの全エネルギが直立時の全エネルギに一致しているときには、モデル傾き制御要求制御量Ｍmdldmdが０になるので、第１実施例で説明した、倒立振子型動力学モデルの着地瞬間の全エネルギ予測値が直立時の全エネルギに一致する様に、着地位置変更量を決定する手法をそのまま活用することができる。
【０１０５】図１９は第３実施例のＳ６００からＳ６１６よりなるメイン・ルーチン・フロー・チャート、図２０R>０はＳ７００からＳ７２４よりなるその姿勢演算および制御サブルーチン・フロー・チャート、図２１はＳ８００からＳ８０６よりなるそのコンプライアンスおよび姿勢安定化制御サブルーチン・フロー・チャートである。図１９のメイン・ルーチン・フロー・チャートは第１実施例のそれと、図２１のサブルーチン・フロー・チャートは第２実施例のそれと同様である。また図２０の姿勢演算および制御サブルーチン・フロー・チャートは、第２実施例と比べて、着地目標位置パラメータを修正するステップＳ７１２が追加されている点で異なるのみである。尚、残余の構成は従前の実施例と相違しない。
【０１０６】第３実施例の場合、前述の如く、倒立振子モデルを直立状態に回復させるために、瞬間的な外乱や微小外乱に対しては主に倒立振子モデルにモーメントを加える制御が働き、大きな外乱が継続すると主に着地位置や着地タイミングを修正する制御が働くので、第１実施例に比べ、着地位置の変更量が少なくなり、歩行経路のふらつきが小さくなると共に、第１実施例、第２実施例よりも安定度が一層向上する。
【０１０７】図２２はこの発明の第４実施例を示すブロック図である。第１から第３までの実施例では、ロボットのモデルを、基準歩容からの上体位置摂動分に対する動力学モデルと、上体や足平位置と関節角の関係を表現する幾何学モデルに分割して備えていた。それに対し、第４実施例では、剛体リンクで構成される動力学・幾何学複合モデルを備える様にした。その他は後で述べる様に、大局的姿勢制御部の詳細が異なることを除くと、第１実施例とほぼ同一構成である。
【０１０８】第４実施例において、歩容は、ＺＭＰ目標位置、および両足平の位置・姿勢で表現される。両足平の位置・姿勢については、一歩ごとの足平軌道パラメータ（例えば、遊脚着地位置や着地時刻など）が、大局的姿勢制御部から目標足平軌道生成部に与えられ、目標足平軌道生成部において、リアルタイムに各瞬間のそれらの位置や姿勢が算出される。ＺＭＰ目標軌道については、一歩ごとのＺＭＰ目標軌道パラメータが、大局的姿勢制御部からＺＭＰ目標軌道生成部に与えられ、ＺＭＰ目標軌道生成部においてリアルタイムに各瞬間のその位置が算出される。上体の位置・姿勢については、第１実施例ないし第３実施例ではＺＭＰ目標軌道を満足する様に、予めオフラインで作成された時系列データとして大局的姿勢制御部に記憶されていたが、第４実施例ではロボットの幾何学・動力学モデルにてモデルの両足平の位置・姿勢、ＺＭＰ目標軌道およびＺＭＰ目標位置まわりのモデル操作モーメントを基に、リアルタイムに生成される。
【０１０９】第４実施例のロボットモデルでは、両足平の位置・姿勢は、足平軌道生成部から与えられた足平の位置・姿勢を仮想床傾斜指令θctrlだけ座標回転させた位置・姿勢に変換される。ロボットモデルでは、上体の位置・姿勢が、ＺＭＰ目標位置に発生する床反力モーメントがＭmdl となる様に動力学計算によって求められ、同時に各関節の変位が求められる。尚、Ｍmdl が０のときには、ロボットモデルのＺＭＰ位置はＺＭＰ目標位置に一致するが、Ｍmdl が０でないときには、ＺＭＰ目標位置にモーメントが発生していることから、もはやＺＭＰ目標位置はロボットモデルのＺＭＰ位置ではなく、ロボットモデルのＺＭＰ位置は、ＺＭＰ目標位置からずれたところに存在する。
【０１１０】モデルの床面には、あらゆる全床反力が発生可能であると仮定する。即ち、モデルの接地面には吸着力も発生可能であるとし、例えば、平地歩行においてＺＭＰが接地面を含む最小凸多角形を超えても、接地面が離れない（脚と床の間の拘束条件が所期通り確保される）ものとする。この様に想定することによって、モデルに対する実ロボットの傾き偏差制御において、実ロボットの全床反力を操作するだけでは不可能であった大きな復元作用も発生可能となる。従って、フィードバックゲインを大きくして短時間で偏差を０に収束させることができる。つまり、実ロボットは、関節変位のみならず、傾きまでもモデルに高応答で追従する。但し、モデルの全床反力の床法線方向成分は、上向き（正）になる様にとるべきである。なぜなら、そうしないと、実ロボットが一瞬床から浮いてしまう恐れがあるからである。
【０１１１】第４実施例においては、大局的姿勢制御部において、モデルの挙動やモデルに対する実ロボットの傾き偏差などから、次の着地位置、着地タイミングまたはＺＭＰ目標軌道などの歩容パラメータを決定する。第１から第３実施例では基準歩容が存在するので、基準歩容からのずらし量を決定するだけで済むが、第４実施例においては基準歩容が存在しないので、歩容パラメータの決定が難しくなる。決定をなるべく容易にするためには、ある基準点から見たロボット全体の角運動量や重心位置などのマクロな状態量に着目すれば良い。局所的姿勢制御がこれらのマクロな状態量にあまり影響を与えない場合には、これらのマクロな状態量だけから将来の挙動を予測し、それらのマクロな状態量が発散しない様に歩容パラメータを決定すれば良い。具体的には、先に述べた竹馬型２足歩行ロボットの着地位置制御で行われている手法を用いたり、シミュレーションによりマクロな状態量に応じた適切な着地位置を予め学習させておく手法などが考えられる。
【０１１２】第４実施例のロボットにおいては、ゆっくり歩行する場合には遊脚の反動が無視でき、単純倒立振子で近似することができる。ロボットが単純倒立振子で近似できる場合には、角運動量を縦軸に、重心位置を横軸にとったときの軌跡は、双曲線上を移動することが知られているので、挙動が解析的に予測でき、着地位置決定も解析的に行うことができる。竹馬型２足歩行ロボットの着地位置制御では、これを利用している。第４実施例のロボットが高速で歩行する場合（例えば、３ｋｍ／ｈなど）には、遊脚の反動が無視できなくなり、単純倒立振子で近似できなくなる。この場合には、解析的な決定法ではなく、マクロな状態量に応じた適切な着地位置を予め学習させる手法などが有効である。学習には、人工知能、ファジー、ニューロなどが使用できよう。
【０１１３】図２３は第４実施例のＳ９００からＳ９１０よりなるメイン・ルーチン・フロー・チャートを、図２４２４はＳ１０００からＳ１０１４よりなるその姿勢演算および制御サブルーチン・フロー・チャートを、図２５はＳ１１００からＳ１１０６よりなるそのコンプライアンス制御サブルーチン・フロー・チャートを示す。図２５R>５サブルーチン・フロー・チャートは、第１実施例のそれと同様である。
【０１１４】第４実施例においては、第１実施例に比べて動力学モデルがより精密になっているので、より正確な姿勢制御が実現される。特に、大きな外乱が加わってモデルの姿勢が大幅に崩れても、転倒することなく、復元することができる。
【０１１５】図２６はこの発明の第５実施例を示すブロック図である。この例において大局的姿勢制御部はなく、歩容パラメータ決定部は、予め外乱がない状況で長期的に安定な歩行が実現できる様に設定された一連の歩容パラメータを吐き出しているだけである。
【０１１６】第５実施例において、ロボットの幾何学・動力学モデルとしては、第１モデルと第２モデルの２つが備えられる。第１モデルと第２のモデルの初期状態は、前記の予め設定された一連の歩容の初期状態に一致させておく。第１モデルには目標足平軌道とＺＭＰ目標軌道が与えられ、ＺＭＰ目標軌道を満足する様に上体軌道が生成される。歩容パラメータ決定部が上に述べた様に、予め外乱がない状況で長期的に安定な歩行が実現できる様に設定された一連の歩容パラメータを吐き出しているので、第１モデルの挙動は、その通り忠実に再現する。第２モデルには、目標足平軌道とＺＭＰ軌道以外に、第１モデルと第２モデルの上体位置の差を０に収束させる制御の操作量と、第２モデルと実ロボットの姿勢傾斜ずれを０に収束させる制御の操作量が、ＺＭＰ目標位置まわりにモーメントとして加えられる。
【０１１７】第２モデルに対する実ロボットの傾き角偏差θerr をＰＤなどのフィードバック則Ｈ１（Ｓ）を通して得られる実傾き偏差制御要求制御量Ｍerrdmdと、第１モデルに対する第２モデルの上体位置ずれをフィードバック則Ｈ２（Ｓ）を通して得られるモデル上体位置制御要求制御量Ｍmdldmdを第２実施例で用いたと同様なリミッタおよび分配器を通して、実モーメント操作量Ｍactcomm とモデルモーメント操作量Ｍmdl に分配する。
【０１１８】ＺＭＰまわりの実床反力モーメントＭact から床反力モーメントフィードバック則によって床反力モーメントフィードバック操作量θcompが求められる。Ｍactcomm とコンプライアンス定数Ｋcompの積に床反力モーメントフィードバック操作量θcompを加えることにより、仮想床傾斜角指令θctrlが求められる。第２モーメントの足平位置は、目標足平軌道生成部で生成される足平位置をＺＭＰ目標位置を中心として、仮想床傾斜角指令の分だけ、座標回転させられる。
【０１１９】第１モデルと第２モデルは、第４実施例のそれと同一で良い。但し、第１モデルにはモデルモーメント操作量Ｍmdl と仮想床傾斜角指令θctrlが与えられないので、第１モデルを第２モデルと同一のプログラムで作るならば、第１モデルに入力されるＭmdl とθctrlを０にしておけば良い。
【０１２０】図２７は第５実施例のＳ１２００からＳ１２１０よりなるメイン・ルーチン・フロー・チャートを、図２８はＳ１３００からＳ１３１８よりなるその姿勢演算および制御サブルーチン・フロー・チャートを、図２９はＳ１４００からＳ１４０６よりなるそのコンプライアンスおよび姿勢安定化制御サブルーチン・フロー・チャートを示す。第２実施例のそれらとほぼ同様である。
【０１２１】第２実施例では予め上体軌道が生成されているのに対し、第５実施例では目標ＺＭＰ軌道を満足する様に、第１モデルにおいてリアルタイムに上体軌道が生成される。第５実施例の効果は、本質的に第２実施例と同じである。ただ、動力学モデルがより忠実になっているため、姿勢安定化制御の精度が高く、モデルの姿勢が大きく崩れても復元することができる。
【０１２２】図３０はこの発明の第６実施例を示すブロック図である。第６実施例ではモデルの姿勢安定化において、第４実施例と第５実施例の制御を併用した。第６実施例においては、第２モデルの角運動量や重心位置などのマクロな状態量をある範囲に収めるために、瞬間的な外乱や微小外乱に対しては主に実ロボットと第２モデルにモーメントを加える制御が働き、大きな外乱が継続すると、主に着地位置や着地タイミングを変更する制御が働く。
【０１２３】第１モデルは、歩容の切り替わり目において、第２モデルの状態に合わせる（つまり、一歩ごとの歩容の初めに強制的に第１モデルの初期状態を第２モデルのそのときの状態に合わせる）。こうしないと、過大な外乱が長期的に実ロボットに加わって、第２モデルに対する実ロボットの傾斜ずれが大きくなったときに、この傾斜ずれを修正しようとして第２モデルに大きな操作モーメントが作用すると、第１モデルと第２モデルの姿勢ずれを０に収束させる制御が十分効かず、第１モデルと第２モデルの姿勢ずれが発散してしまうからである。但し、第１モデルの初期状態を第２モデルに急激に合わせると、第１モデルと第２モデルの姿勢ずれを０に収束させる制御の操作量が急激に変化する恐れがあるので、第１モデルと第２モデルの姿勢ずれを０に収束させる制御のフィードバックゲインを歩容の変わり目の直前に連続的に下げておいた方が良い。
【０１２４】第４実施例では不確定な外乱が実ロボットに加わることによって、モデルの挙動も不確定な挙動を示すため、モデルの将来挙動の予測精度が低下する。第６実施例では、一歩の間、第２モデルは第１モデルの挙動に収束しようとする。一歩の間、第１モデルの挙動には実ロボットに加わる外乱の影響を受けないので、第１モデルの挙動は、確定的に予想できる。従って、実ロボットに加わる外乱によって第２モデルの挙動が変化しても、しばらくすれば第１モデルの挙動に収束しようとするので、第２モデルの挙動の不確定性が減少する。即ち、第６実施例では、第４実施例に比べ、着地位置などの歩容パラメータを操作することによる姿勢安定化作用がより確実に働くこととなる。
【０１２５】図３１は第６実施例のＳ１５００からＳ１５１０よりなるメイン・ルーチン・フロー・チャート、図３２はＳ１６００からＳ１６２０よりなるその姿勢演算および制御サブルーチン・フロー・チャート、図３３はＳ１７００からＳ１７０６よりなるそのコンプライアンスおよび姿勢安定化制御サブルーチン・フロー・チャートである。
【０１２６】第６実施例の場合は、第４実施例に比べ、モデルの将来の挙動の不確定性が小さいので、より確実かつ長期的な姿勢安定化が実現される。
【０１２７】尚、第６実施例では、第１モデルと第２モデルの偏差を歩容の切り替わり目で強制的に０にしていたが、それ以外の時期でも偏差が過大になったとき、第１モデルと第２モデルロボットの偏差を０に収束させる様に第１モデルに操作モーメントを加える制御を実行しても良い。但し、第１モデルに操作モーメントを加えると、上体の位置がずれて第１モデルの姿勢が崩れようとする。そこで、姿勢の崩れを防ぐために、第１モデルに操作モーメントを加えるときには同時に着地位置などの歩容パラメータも修正する。ところで、着地直前に、着地位置や着地時期を大きく変更すると、脚の挙動が急激に変化する。即ち、着地までの時間余裕が少なくなってくるにつれ、着地位置や着地時期の修正可能量は小さくなる。従って、第１モデルに操作モーメントを加え、それに見合った着地位置などの修正を行う際には、着地までの時間余裕が少なくなるにつれ、操作モーメントの印加量の制限値も小さくするべきである。
【０１２８】更に、第５、第６実施例においては、動力学モデルが２段に構成されている。これを更に段数を増やし、実ロボットとモデルとの偏差、およびモデルと他のモデルとの偏差を制御する様にしても良い。上位のモデル（即ち、多段連結モデルのうちで実ロボットから遠く離れているモデル）ほど実ロボットに作用する外乱の影響が小さくなり、モデルの挙動予測精度が高くなる。従って、上位のモデルの挙動を基に着地位置修正量を求めれば、上位のモデルの姿勢安定性が確保でき、それより下位のモデルはモデル操作モーメントを制御することによって上位のモデルに追従するので、結果的に実ロボットと全モデルすべての姿勢安定性が確保される。摂動モデルを用いた場合も同様である。
【０１２９】ここで、第２実施例などで用いたリミッタおよび分配器について説明を補足する。リミッタおよび分配器の目的は、実ロボットの接地性を確保しながら、制御操作量を分配することであるが、リミッタの変形例としては、第２実施例で挙げた例以外に、以下の様な手法やそれらの組み合わせが考えられる。
ａ．制御則によってモーメントの次元で算出される操作量に、一定の制限を加える手法ｂ．床反力の床垂直方向成分の実際値あるいは設計値に応じて、モーメントの次元で算出される操作量の制限値を変動させる手法この場合、（ＺＭＰのずれ量＝モーメント／床反力垂直成分）であるから、リミッタは、ＺＭＰ目標位置からＺＭＰ存在領域の境界までの距離余裕×床反力垂直成分の値に応じて変動させる方が、より的確に接地性が確保される。
ｃ．目標ＺＭＰ位置からＺＭＰ存在可能領域の境界までの余裕によって、モーメントの次元で算出される操作量の制限値を変動させる手法例えば、平地歩行では両脚支持期の間はＺＭＰ存在可能領域が大きいので、実ロボットに大きな復元力を発生させることができる。
ｄ．制御則によって求められる操作量を一旦ＺＭＰ位置のずれに変換し、これに目標ＺＭＰ位置からＺＭＰ存在可能領域の境界までの余裕に応じた制限を加える手法ｅ．操作量がＰＤ制御則によって求められる場合、Ｐ成分から求められる操作量にのみ制限を加え、発振を抑えるダンピング効果を持つＤ成分には制限を加えない手法、あるいはその逆の手法ｆ．実床反力モーメントＭact が限界値を超えない様に、Ｍact に応じて制限値が可変となるリミッタに、実床反力操作モーメントＭactcomm を通す手法ｇ．Ｍact が限界値を超えたら、超えた分に、あるゲインを乗じてＭactcomm から減算する手法ｈ．実ロボットの床反力計測値から実ロボットのＺＭＰ位置を求め、これがＺＭＰ存在可能領域を超えそうになったら、それ以上Ｍactcomm の絶対値を増やさない手法
【０１３０】更に、分配を数２４の式の様に単純に比例配分する手法も考えられる。
【０１３１】
【数２４】
【０１３２】また、周波数帯域に応じて分配する手法も考えられる。例えば、実ロボットの床反力操作量Ｍactcomm には、実傾き偏差制御要求制御量Ｍerrdmdとモデル傾き（あるいはモデル上体位置）制御要求制御量Ｍmdldmdの高域成分だけを与え、低域をモデル操作量Ｍmdl に与えれば、リミッタをかけなくても実ロボットの床反力操作量は小さく抑えられるので、接地性は高くなる。
【０１３３】また、第２実施例ではフィードバック則で操作量を求めてから、リミッタおよび分配器に通しているが、逆の順序で求めても良く、あるいは分配器の前後両方にリミッタを挿入する手法なども考えられよう。
【０１３４】更に、第１実施例から第６実施例で述べた歩容についての記述を補足すると、通常、歩容は接地を確保するための床反力に関する要件とその他の拘束条件により記述され、これらを満足する様に関節変位が決定される。拘束条件としては、足平軌道や上体姿勢、関節の変位が無理な挙動をしないための上体高さの決定式などが用いられる。床反力に関する要件としては、代表的なものとして例えば、平面床歩行においてはＺＭＰの存在可能領域が知られている。接地面を含む最小凸多角形がＺＭＰの存在可能領域であり、その中にＺＭＰ目標軌道を設定し、歩容からオイラー・ニュートン方程式などを用いて動力学的に求められるＺＭＰが、ＺＭＰ目標軌道に一致する様に歩容が生成される。また、床反力に関する要件の代わりに、足首トルクの様なある特定の部位に作用する力やモーメントに関する要件を与えても良い。例えば、２足歩行ロボットの片脚支持期においては、支持脚足首は床に近いので、足首トルクは、足首の床への垂直投影点に作用する床反力モーメントと密接な関係を持つからである。
【０１３５】また、動力学モデルでは、目標床反力の要件を満足するモデルの挙動を求めなければならない。目標床反力の要件とは、例えば、ＺＭＰ目標位置にモーメントＭmdl が発生することである。ところで、モデルの足平位置・姿勢と上体の姿勢は歩容生成部から与えられているので、片脚あたり６自由度の実施例に係るロボットでは、上体の姿勢を決定すれば、全関節の変位が決定される。上体のうち、高さは例えば、各瞬間における膝などの関節の変位、速度、加速度などがアクチュエータの能力を超えない様に、かつ床垂直抗力が負にならない様に決定される。従って、上体の位置のうちで、残った上体の前後左右の挙動によって目標床反力の要件を満足させることとなる。しかし、各瞬間における目標床反力の要件を満足する上体前後左右位置を解析的に直接求めることは困難であるので、実際には逆に上体の位置を与え、それに対する床反力を求めながら、擬似的なニュートン法などの収束法により、目標床反力の要件を満足する上体の位置を探索する。尚、演算の高速性を重視する場合には、多少の誤差を生じても、探索回数に制限を加えれば良い。極端な場合、探索回数は１回にしても良いであろう。
【０１３６】また、両脚支持期にはＭact を大きくしても接地性が失われない。そこで、両脚支持期にリミッタ制限値を大きくしたり、モデル上体位置制御則などのゲインを高くしても良い。
【０１３７】また、歩行の環境や目的によって、姿勢を重視したいときと、階段や飛び石上の歩行の様に着地位置精度を重視したいときとがある。そこで、各制御ゲインを可変にすると、状況に応じたより緻密な制御が実現できる。例えば、姿勢安定を重視したいときには、モデル上体位置制御（あるいはモデル傾き制御）の制御ゲインを下げて要求制御量Ｍmdldmdを小さくし、モデル傾き（あるいはモデル上体位置）を０に収束させる制御を主に着地位置などの歩容パラメータ修正によって行う。こうすれば、実ロボットの操作モーメントＭactcomm が小さくなって、実ロボットのＺＭＰ位置がＺＭＰ存在可能領域の中央付近に寄ってくるので、安定余裕が大きくなるからである。但し、その代わり着地位置などの歩容パラメータが大きく修正されるので、着地位置精度は悪くなる。また歩行の環境や目的によって、制御則の周波数ゲイン特性を調整しても、状況に応じたより緻密な制御が実現できる。
【０１３８】尚、上記した第１ないし第６実施例において、実ロボットの床反力のフィードバック制御として、コンプライアンス制御を導入しているが、トルク制御を導入しても良い。床の形状が既知で、床の凹凸による外乱があまり発生しない場合や、メカニカル機構によってコンプライアンスが実現されている場合には、特に床反力フィードバック制御を行わなくても良い。その場合でもこの発明の効果が失われる訳ではない。
【０１３９】更に、上記において２足歩行の脚式移動ロボットを例にとって説明してきたが、それに限られるものではなく、この発明は３足以上の脚式移動ロボットにも妥当し、更には脚式に限らず、車輪型やクローラ型など他の形態の移動ロボットに妥当するものである。
【０１４０】
【発明の効果】請求項１項にあっては、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差に応じた操作量を少なくとも前記モデルにフィードバックしてモデルの挙動を修正する様に構成したので、何らかの外乱などによって前記傾き角（角速度）偏差が非常に大きくなっても、接地性を確保したまま（即ち、足裏が浮かない様にしながら）、前記モデルと実ロボットの床反力の差を活用して大きな姿勢復元作用を得ることができる。またモデルは床と干渉しないから、モデルに追従する実ロボットも床と干渉せず、着地衝撃を小さい値に抑制することができる。
【０１４１】請求項２項にあっては、制御対象のモデルを少なくとも２つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差及び／又は前記モデル間の傾斜などの状態量の偏差に応じた操作量を少なくとも前記モデルのいずれかにフィードバックしてモデルの挙動を修正する様に構成したので、上位のモデル（即ち、多段連結モデルのうちで実ロボットから遠く離れているモデル）ほど実ロボットに作用する外乱の影響が小さくなり、モデルの挙動予測精度が高くなって、実ロボットと全モデルすべての姿勢安定性を一層確実に実現することができる。
【０１４２】請求項３項にあっては、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差に応じた操作量を前記モデルと実ロボットに分配しつつフィードバックしてモデルの挙動を修正する様に構成したので、請求項１項で述べた効果に加え、モデルの姿勢の崩れをある程度は抑制することができる。
【０１４３】請求項４項にあっては、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルに対する実ロボットの傾き偏差に応じて前記モデルに与える床反力目標値を修正し、修正された床反力目標値を満足する様に前記モデルの歩容を修正して前記傾き偏差を収束させる様に構成したので、請求項１項で述べた効果をより直接的に得ることができる。
【０１４４】請求項５項にあっては、制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾き偏差に応じた操作量を前記モデルと実ロボットに分配しつつ与えて床反力目標値を修正し、修正された床反力目標値を満足する様に前記モデルも歩容を修正して前記傾き偏差を収束させる様に構成したので、請求項１項で述べた効果をより直接的に得ることができると共に、一層効果的にモデルの姿勢を回復することができる。
【０１４５】請求項６項にあっては、脚式移動ロボットの姿勢安定化制御装置において、実ロボットの力学モデルを基に接地性が保証される様に床反力が設計された基準歩容からの挙動摂動分と、床反力摂動分の関係を模擬する少なくとも１つの摂動動力学モデル、目標歩容と実ロボットの姿勢傾き偏差を検出する手段、および前記偏差を小さくする様に、前記摂動動力学モデルに床反力を与え、それによって生じた摂動動力学モデルの変位に応じて前記基準歩容を修正したものを前記目標歩容として前記実ロボットの関節変位を追従させる手段を備える様に構成したので、請求項１項で述べた効果に加えて、演算量を低減することができ、コンピュータを用いて実現するときもその能力が低いもので足りる。
【０１４６】請求項７項にあっては、脚式移動ロボットの姿勢安定化制御装置において、実ロボットの力学モデルを基に接地性が保証される様に床反力が設計された基準歩容からの挙動摂動分と、床反力摂動分の関係を模擬する少なくとも１つの摂動動力学モデル、実ロボットのリンク構造を表す幾何学モデル、前記幾何学モデルと実ロボットの姿勢傾き偏差を検出する手段、および前記偏差を小さくする様に前記摂動動力学モデルに床反力を加え、それによって生じた摂動動力学モデルの変位に応じて前記基準歩容を前記幾何学モデル上で修正し、幾何学モデルの関節変位を目標として前記実ロボットの関節変位を追従させる手段を備える様に構成したので、請求項６項で述べた効果に加えて、床との干渉をより小さくすることができる。
【０１４７】請求項８項にあっては、少なくとも着地位置などの歩容パラメータを修正して前記モデルの姿勢を安定に保つ様に構成したので、従前までの請求項で述べた効果に加えて、一層簡易にモデルの姿勢を安定に回復することができる。
【０１４８】請求項９項にあっては、少なくとも新たな床反力を加えて前記モデルの姿勢を安定に保つ様に構成したので、請求項８項で述べたと同様の効果を得ることができる。
【０１４９】請求項１０項にあっては、着地位置などの歩容パラメータを修正すると共に、新たな床反力を加えて前記モデルの姿勢を安定に保つ様に構成したので、従前までの請求項で述べた効果に加えて、一層効果的にモデルの姿勢を安定に回復することができる。
【図面の簡単な説明】
【図１】図１は本出願人が先に提案したＺＭＰ制御を示す説明図である。
【図２】図１に示す制御において実ＺＭＰが目標ＺＭＰ（モデルから設定されるＺＭＰ）からずれた状態を示す説明図である。
【図３】この発明に係る脚式移動ロボットの姿勢安定化制御装置の特徴（原理）を示す説明図である。
【図４】この発明の原理（特徴）を説明するものであって、単純な実ロボットの構造パラメータと実ロボットの基本局所制御を示す説明図である。
【図５】図４に例示したものとこの発明の原理（特徴）を比較する説明ブロック図である。
【図６】図５に示すブロック図を変形したものを簡略的に示す説明図である。
【図７】この発明に係る脚式移動ロボットの姿勢安定化制御装置を全体的に示す概略図である。
【図８】図７に示す制御ユニットのブロック図である。
【図９】この発明の第１実施例の動作を示すブロック図である。
【図１０】第１実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図１１】図１０フロー・チャートのうちの姿勢演算および制御のサブルーチン・フロー・チャートである。
【図１２】図１０フロー・チャートのうちのコンプライアンス制御のサブルーチン・フロー・チャートである。
【図１３】この発明の第２実施例の動作を示すブロック図である。
【図１４】第２実施例で使用するリミッタおよび分配器を説明するブロック図である。
【図１５】第２実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図１６】図１５フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図１７】図１５フロー・チャートの中のコンプライアンスおよび姿勢安定化制御のサブルーチン・フロー・チャートである。
【図１８】この発明の第３実施例の動作を示すブロック図である。
【図１９】第３実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図２０】図１９フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図２１】図１９フロー・チャートの中のコンプライアンスおよび姿勢安定化制御のサブルーチン・フロー・チャートである。
【図２２】この発明の第４実施例の動作を示すブロック図である。
【図２３】第４実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図２４】図２３フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図２５】図２３フロー・チャートの中のコンプライアンス制御のサブルーチン・フロー・チャートである。
【図２６】この発明の第５実施例の動作を示すブロック図である。
【図２７】第５実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図２８】図２７フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図２９】図２７フロー・チャートの中のコンプライアンスおよび姿勢安定化制御のサブルーチン・フロー・チャートである。
【図３０】この発明の第６実施例の動作を示すブロック図である。
【図３１】第６実施例の動作を示すメイン・ルーチン・フロー・チャートである。
【図３２】図３１フロー・チャートの中の姿勢演算および制御のサブルーチン・フロー・チャートである。
【図３３】図３１フロー・チャートの中のコンプライアンスおよび姿勢安定化制御のサブルーチン・フロー・チャートである。
【符号の説明】
１ 脚式移動ロボット（２足歩行ロボット）
２ 脚部リンク
１０Ｒ，１０Ｌ 脚部回旋用の関節
１２Ｒ，１２Ｌ 腰部のロール方向の関節
１４Ｒ，１４Ｌ 腰部のピッチ方向の関節
１６Ｒ，１６Ｌ 膝部のピッチ方向の関節
１８Ｒ，１８Ｌ 足首部のピッチ方向の関節
２０Ｒ，２０Ｌ 足首部のロール方向の関節
２２Ｒ，２２Ｌ 足平
２４ 上体
２６ 制御ユニット
３６ ６軸力センサ
【特許請求の範囲】
【請求項１】 制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差に応じた操作量を少なくとも前記モデルにフィードバックしてモデルの挙動を修正することを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項２】 制御対象のモデルを少なくとも２つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差及び／又は前記モデル間の傾斜などの状態量の偏差に応じた操作量を、少なくとも前記モデルのいずれかにフィードバックしてモデルの挙動を修正することを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項３】 制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差に応じた操作量を前記モデルと実ロボットに分配しつつフィードバックしてモデルの挙動を修正することを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項４】 制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルに対する実ロボットの傾き偏差に応じて少なくとも前記モデルに与える床反力目標値を修正し、修正された床反力目標値を満足する様に前記モデルの歩容を修正して前記傾き偏差を収束させることを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項５】 制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾き偏差に応じた操作量を前記モデルと実ロボットに分配しつつ与えて床反力目標値を修正し、修正された床反力目標値を満足する様に前記モデルの歩容を修正して前記傾き偏差を収束させることを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項６】 脚式移動ロボットの姿勢安定化制御装置において、ａ．実ロボットの力学モデルを基に接地性が保証される様に床反力が設計された基準歩容からの挙動摂動分と、床反力摂動分の関係を模擬する少なくとも１つの摂動動力学モデル、ｂ．目標歩容と実ロボットの姿勢傾き偏差を検出する手段、およびｃ．前記偏差を小さくする様に、少なくとも前記摂動動力学モデルに床反力を与え、それによって生じた摂動動力学モデルの変位に応じて前記基準歩容を修正したものを前記目標歩容として前記実ロボットの関節変位を追従させる手段、を備えたことを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項７】 脚式移動ロボットの姿勢安定化制御装置において、ａ．実ロボットの力学モデルを基に接地性が保証される様に床反力が設計された基準歩容からの挙動摂動分と、床反力摂動分の関係を模擬する少なくとも１つの摂動動力学モデル、ｂ．実ロボットのリンク構造を表す幾何学モデル、ｃ．前記幾何学モデルと実ロボットの姿勢傾き偏差を検出する手段、およびｄ．前記偏差を小さくする様に、少なくとも前記摂動動力学モデルに床反力を与え、それによって生じた摂動動力学モデルの変位に応じて前記基準歩容を前記幾何学モデル上で修正し、幾何学モデルの関節変位を目標として前記実ロボットの関節変位を追従させる手段、を備えたことを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項８】 少なくとも着地位置などの歩容パラメータを修正して前記モデルの姿勢を安定に保つことを特徴とする請求項１項ないし７項のいずれかに記載の脚式移動ロボットの姿勢安定化制御装置。
【請求項９】 少なくとも新たな床反力を加えて前記モデルの姿勢を安定に保つことを特徴とする請求項１項ないし７項のいずれかに記載の脚式移動ロボットの姿勢安定化制御装置。
【請求項１０】 着地位置などの歩容パラメータを修正すると共に、新たな床反力を加えて前記モデルの姿勢を安定に保つことを特徴とする請求項１項ないし７項のいずれかに記載の脚式移動ロボットの姿勢安定化制御装置。
【請求項１】 制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差に応じた操作量を少なくとも前記モデルにフィードバックしてモデルの挙動を修正することを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項２】 制御対象のモデルを少なくとも２つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差及び／又は前記モデル間の傾斜などの状態量の偏差に応じた操作量を、少なくとも前記モデルのいずれかにフィードバックしてモデルの挙動を修正することを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項３】 制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾斜などの状態量の偏差に応じた操作量を前記モデルと実ロボットに分配しつつフィードバックしてモデルの挙動を修正することを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項４】 制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルに対する実ロボットの傾き偏差に応じて少なくとも前記モデルに与える床反力目標値を修正し、修正された床反力目標値を満足する様に前記モデルの歩容を修正して前記傾き偏差を収束させることを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項５】 制御対象のモデルを少なくとも１つ備えるモデル追従型の脚式移動ロボットの姿勢安定化制御装置であって、前記モデルと実ロボットの傾き偏差に応じた操作量を前記モデルと実ロボットに分配しつつ与えて床反力目標値を修正し、修正された床反力目標値を満足する様に前記モデルの歩容を修正して前記傾き偏差を収束させることを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項６】 脚式移動ロボットの姿勢安定化制御装置において、ａ．実ロボットの力学モデルを基に接地性が保証される様に床反力が設計された基準歩容からの挙動摂動分と、床反力摂動分の関係を模擬する少なくとも１つの摂動動力学モデル、ｂ．目標歩容と実ロボットの姿勢傾き偏差を検出する手段、およびｃ．前記偏差を小さくする様に、少なくとも前記摂動動力学モデルに床反力を与え、それによって生じた摂動動力学モデルの変位に応じて前記基準歩容を修正したものを前記目標歩容として前記実ロボットの関節変位を追従させる手段、を備えたことを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項７】 脚式移動ロボットの姿勢安定化制御装置において、ａ．実ロボットの力学モデルを基に接地性が保証される様に床反力が設計された基準歩容からの挙動摂動分と、床反力摂動分の関係を模擬する少なくとも１つの摂動動力学モデル、ｂ．実ロボットのリンク構造を表す幾何学モデル、ｃ．前記幾何学モデルと実ロボットの姿勢傾き偏差を検出する手段、およびｄ．前記偏差を小さくする様に、少なくとも前記摂動動力学モデルに床反力を与え、それによって生じた摂動動力学モデルの変位に応じて前記基準歩容を前記幾何学モデル上で修正し、幾何学モデルの関節変位を目標として前記実ロボットの関節変位を追従させる手段、を備えたことを特徴とする脚式移動ロボットの姿勢安定化制御装置。
【請求項８】 少なくとも着地位置などの歩容パラメータを修正して前記モデルの姿勢を安定に保つことを特徴とする請求項１項ないし７項のいずれかに記載の脚式移動ロボットの姿勢安定化制御装置。
【請求項９】 少なくとも新たな床反力を加えて前記モデルの姿勢を安定に保つことを特徴とする請求項１項ないし７項のいずれかに記載の脚式移動ロボットの姿勢安定化制御装置。
【請求項１０】 着地位置などの歩容パラメータを修正すると共に、新たな床反力を加えて前記モデルの姿勢を安定に保つことを特徴とする請求項１項ないし７項のいずれかに記載の脚式移動ロボットの姿勢安定化制御装置。
【図６】
【図１】
【図２】
【図３】
【図７】
【図１４】
【図４】
【図５】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１５】
【図１３】
【図１６】
【図１７】
【図２１】
【図１８】
【図１９】
【図２０】
【図２２】
【図２３】
【図２５】
【図２４】
【図２６】
【図２７】
【図２９】
【図２８】
【図３０】
【図３１】
【図３３】
【図３２】
【図１】
【図２】
【図３】
【図７】
【図１４】
【図４】
【図５】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１５】
【図１３】
【図１６】
【図１７】
【図２１】
【図１８】
【図１９】
【図２０】
【図２２】
【図２３】
【図２５】
【図２４】
【図２６】
【図２７】
【図２９】
【図２８】
【図３０】
【図３１】
【図３３】
【図３２】
【公開番号】特開平５−３３７８４９
【公開日】平成５年（１９９３）１２月２１日
【国際特許分類】
【出願番号】特願平４−１６４２９７
【出願日】平成４年（１９９２）５月２９日
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【公開日】平成５年（１９９３）１２月２１日
【国際特許分類】
【出願日】平成４年（１９９２）５月２９日
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
[ Back to top ]