JP3677623B2

JP3677623B2 - 脚式ロボットのリアルタイム最適制御方法

Info

Publication number: JP3677623B2
Application number: JP2000275690A
Authority: JP
Inventors: 裕喜竹内
Original assignee: National Institute of Advanced Industrial Science and Technology AIST
Current assignee: National Institute of Advanced Industrial Science and Technology AIST
Priority date: 2000-09-11
Filing date: 2000-09-11
Publication date: 2005-08-03
Anticipated expiration: 2020-09-11
Also published as: WO2002022319A1; EP1348521A1; JP2002086373A; CA2421882A1

Description

【０００１】
【産業上の利用分野】
本発明は、リアルタイム最適化計算が可能なリシーデイング・ホライゾン制御（ＲｅｃｅｄｉｎｇＨｏｒｉｚｏｎＣｏｎｔｒｏｌ。以下「ＲＨＣ」という。）を用いた、脚式ロボットのゼロ・モーメント・ポイント（ＺｅｒｏＭｏｍｅｎｔＰｏｉｎｔ。本明細書では「ＺＭＰ」という。）制御に関する。
【０００２】
【従来の技術】
図１に示すように、脚式ロボットが１脚で立っているとき、静的に安定して歩行するには、ロボットの重心が足裏平面内に収まっていなければならず、また、２脚で立っているときには、両足裏平面を結ぶ直線上に重心がなければならない。さらに、４脚で立っているときには、４つの足裏平面で構成される多角形内に重心がなければならない。ロボットが運動しているとき、動的に安定して歩行するには、重心点と同じような概念が必要となる。一般にこれをＺＭＰと呼んでいる。
【０００３】
図２において、ＺＭＰは、矢状面（Ｓａｇｉｔｔａｌ平面）内において、第０関節から第ｎ関節までのリンクについて
【数１】

と表すことが出来る。ここでロボット全体の重心位置で代表すれば上式は簡単に
【数２】

とかける。即ち、ロボットの原点まわりの総モーメントが、ＺＭＰと床反力から得られるモーメントと釣合う点である。ＺＭＰが、重心と同じように、足裏平面によって構成される多角形内に収まっていれば、ロボットは安定して動的歩行をすることができ、外れるとロボットは転倒を起こし、歩行が継続できない。
【０００４】
従来の脚式ロボットにおいて問題であったのは、（２）式でＺＭＰを算出しようとしたときに、ｘとｚの２変数があるために解が一意に決定しないことである。従来これを回避するために、例えば重心位置を一定高度ｈ_ｒｅｆに保つ条件を仮定すると
【数２２】

となり、
【数３】

とすることにより、擬似的なＺＭＰを算出するような方法を取らざるを得なかった。しかし、これはあくまでも擬似的な解が得られるだけで、真のＺＭＰは得られない。そして例えば、重心加速度を、
【数４】

のように入力することができれば実全床反力が目標ＺＭＰに収束するような復元トルクが得られる。
【０００５】
しかし、このように重心高度を一定に保つという点において、設計者の意思が入り込んでしまっているが、これは、（４）式で高度一定にするような手法をとらないとＺＭＰが一意に定まらないからである。真のＺＭＰを従来の方法で求めるには、ＺＭＰを予め最適化問題として定式化し、勾配法等を用いて計算する必要がある。勾配法による最適化の場合、繰り返し計算を必要とするので、このような最適化計算は通常オフライン計算となる。しかし、ロボット制御のようなリアルタイム性が要求される計算では、オフライン最適化計算は予期しない障害物が現れたような場合、対処できないという問題があった。
【０００６】
【発明が解決しようとする課題】
本発明は、リアルタイム最適化計算が可能なＲＨＣを用いた、脚式ロボットのＺＭＰ制御を提供することを目的とする。また、本発明は、リアルタイムで自動生成される目標ＺＭＰ軌道と、非線形制御とを併用してロボットを制御することを特徴とする。
【０００７】
【課題を解決するための手段】
本発明は、上記の課題を解決することを目的として、脚式ロボットのＺＭＰが
【数１５】

の等式拘束条件を満足するものにおいて、ｘ_ｚｍｐを新たな入力ｕ_ｚｍｐとして
【数１６】

を用いて最適解を求めたときに最適な目標ＺＭＰ軌道をリアルタイムで得るようにした脚式ロボットのリアルタイム最適制御方法（以下、単に「第一の発明」ともいう。）を特徴とする。
本発明は、ロボット全体重心の目標軌道と、上記第一の発明に基づく最適な目標ＺＭＰ軌道生成とともにロボットの各関節の制御を非線形制御により行うようにした脚式ロボットのリアルタイム最適制御方法を特徴とする。即ち本発明では、ロボットがリアルタイム制御されるために必要なすべての目標軌道がリアルタイムに生成され、通常の非線形制御によって実際のロボットが制御される。また、ＲＨＣ定式化において、ロボットの非線形性も含めて定式化すれば、ロボットのすべてのリアルタイム制御自体もＲＨＣのみによって可能となる。
また、本発明は、上記第一の発明に基づいて、予め、各パラメータを変化させた場合の計算結果を集計し、設定したロボット動作に対する目標ＺＭＰ軌道の足裏内の動作範囲を把握可能とすることによって、ロボットの足裏面積を適切に設定することが可能となることを特徴とする脚式ロボットのリアルタイム最適制御方法を特徴とする。
【０００８】
【発明の実施の形態】
ここで、等式拘束を含んだＲＨＣについて説明する。従来、ＲＨＣの定式化において、等式拘束条件を含む定式化は行われていなかった。
【０００９】
ｘを状態変数、ｕを制御入力、ｔを時間とし、ＲＨＣのリアルタイム制御の制御対象の状態方程式を、
【数５】

とする。ｆは、必要なだけの微分可能性を有する関数とする。
等式拘束条件として、
【数６】

とする。このときのハミルトニアンＨは、
【数７】

となる。Ｌはラグランジアン、λとρは未定乗数である。
【００１０】
Ｅｕｌｅｒ−Ｌａｇｒａｎｇｅ方程式（オイラー．ラグランジュ方程式）は、
【数８】

というように定式化される。
【００１１】
最適軌道からの微少変化は、
【数９】

として考えられる。ここで、Ｈ_ｕ＝０，Ｈ_ρ＝０（ｉ．ｅ．Ｃ＝０）の微少変化の２式を連立方程式として解くとδｕ、δρが消去できる。
【００１２】
これを後の２式に代入して、
【数１０】

を得る。但し、
【数１１】

である。これで等式拘束条件を含有した時変線形方程式が得られた。後は、連続変形法（ＣｏｎｔｉｎｕａｔｉｏｎＭｅｔｈｏｄ）に従って、リアルタイム最適解が得られる。この点については、参考文献（大塚敏之著、「非線形最適フィードバック制御のための実時間最適化手法」、計測と制御ｖｏｌ．３６ｎｏ．１１ｐｐ．７７６−７８３１９９７）等を参照されたい。
【００１３】
ここでは、２脚式ロボットや４脚式ロボットをモデル化するときに最も基本的な例として、ロボット全体を１つの代表質点として扱う。ロボット全体を１質点の倒立振子として扱うことは従来から一般に行われている手法である。このような代表質点によるモデル化によれば、後述するように、２脚式ロボットや４脚式ロボット、その他の多脚式ロボットに簡単に応用することができる。
【００１４】
そして、系の入力として、Ｘ、Ｚ方向の加速度を設定する。即ち、図３において
【数１２】

とおく。
鉛直下方には重力が加わる。従って、状態方程式として、
【数１３】

とする。これは矢状面（Ｓａｇｉｔｔａｌ平面）上で考えられているが、正面（Ｆｒｏｎｔａｌ平面）で考えれば、
【数１４】

となる。
【００１５】
次に等式拘束条件を考えると、脚式ロボットは、各時刻において、ＺＭＰは以下のような等式拘束条件を満足していなければならない。ＺＭＰは、総モーメントが０となる点である。これが、足裏または接地脚先によって構成される多角形内にあれば、モーメントを発生せずに床反力を支持することができる。
【数１５】

なお、ｇは重力の加速度である。
【００１６】
これは、図３のように、質点に加わる加速度入力や、重力による総モーメントが釣り合うことを意味している。ＺＭＰは、これが足裏内または接地脚先によって構成される多角形内に位置すれば、モーメントが発生せずに床反力を支持することができる。
しかし、最適制御問題の定式化の中で、ＺＭＰを状態変数の１つとして状態方程式内に含めようとするとＺＭＰの微分をとらなくてはならなくなり、工夫を要する。
そこで、ここでは、式（１５）において、ｘ_ｚｍｐを新たな入力ｕ_ｚｍｐとして考えることにより、うまく定式化できることを示す。
【００１７】
従って、式（１５）は次のようになる。
【数１６】

このように、ＺＭＰを入力の一つとして扱えば、最適解を求めたときに最適なＺＭＰ入力も同時に得られることになる。この点において、等式拘束条件は重要な役割を持っている。
【００１８】
評価関数は、入力のノルムとする。ここでは、ｕ_ＺＭＰを評価関数内に含めている点が特徴である。このように設定することにより、各軸加速度とＺＭＰのノルムを最小に抑える解を求めることになる。ＺＭＰを、足裏内でなるべく揺動しないように設定することはロボットの足裏をなるべく小さく設計できることにつながる。
【数１７】

また、ＺＭＰ入力ｕ_ｚｍｐ以外の項を異なる数式によって構成した評価関数も考えられる。即ち、本発明の範囲は、ｕ_ｚｍｐが含まれた評価関数すべてについて請求している。具体的には、式（１７）上段の式の「Ｌ」部において、Ｌ内に、ｕ^２ _ｚｍｐが含まれる式Ｊのすべてについて請求している。
評価関数はＲＨＣの場合、時刻ｔからｔ＋Ｔまでの区間が移動していくことになる。
【００１９】
数値計算の実際の例を図４に示す。使用したパラメータは、質量ｍ＝1.0ｋｇ、始点、及び終点における水平方向速度0.5ｍ／sec、鉛直方向速度0.0ｍ/secとした。初期値は、（ｘ、ｚ）＝（−0.25、0.8、0.5、0.0）目標値は（ｘ、ｚ）＝（0.25、0.8、0.5、0.0）である。また、Ｒ＝(1.0,1.0,1.0),Ｓｆ＝(1.0,4400.0,1.0,0.0)である。図４（ａ）の目標値に移動していくに伴い、図４（ｇ）でＺＭＰも移動している様子が確認できる。この例では、系の挙動を見易いように、高度0.8ｍに対し、水平方向位置は1.0secで0.5ｍ進む。脚式ロボットとしては少し大きな挙動となっている。本発明の実施例では、このような大きな挙動でもＺＭＰは生成される。
【００２０】
図４（ｉ），（ｊ）は、生成されたＺＭＰ値が妥当な値か確認するため、拘束条件式、
【数１６】

の左辺と右辺を時間履歴に沿って各々計算したものである。図より左辺と右辺はほぼ均衡が保たれており、逐次生成されるＺＭＰ値が妥当なものであることがわかる。
この計算に要したＣＰＵ時間は、0.09(sec)であり,シミュレーション時間0.5(sec)に対して充分短い時間で完了でき、リアルタイム制御が可能であることがわかる。
【００２１】
このように本実施例で示した方法によれば、リアルタイムにＺＭＰ入力を生成できるため、予めＺＭＰ軌道をオフライン計算で求めておくような従来の手法に比較して非常に便利である。
図４（ｈ）は、ＲＨＣを計算する上での誤差Ｆの値の推移である。このシミュレーションでは、Ｆ値は途中で少し大きくなるものの、全体的には充分低い値に抑えられている。
【００２２】
ここまでは、説明を分かり易くするために２次元平面で述べたが、この応用として３次元空間での定式化も可能である。３次元質点の状態方程式として、
【数１８】

拘束条件は、図３に示すように、ｘ軸方向の釣合いとｙ軸方向の釣合いを考慮しなければならない。
【数１９】

拘束条件の数を減らすためには、２式をまとめて、
【数２０】

とすることも可能である。
【００２３】
評価関数は、各軸加速度とＺＭＰ入力のノルムを最小にする。
【数２１】

ここでも、本発明の範囲は、２次元の場合と同様に、式（２１）最上段の式の「Ｌ」部において、Ｌ内に、ｕ^２ _ｘｚｍｐ＋ｕ^２ _ｙｚｍｐが含まれる式Ｊのすべてについて請求している。
以上の定式化を踏まえてシミュレーション計算を行った結果を図５（ａ）〜図６（ｏ）に示す。初期値は、(-0.125,-0.125,0.800,0.500,0.500,0.0000)、目標値は、(0.125,0.125,0.800,0.500,0.500,0.000)である。また、Ｒ＝(1.0,1.0,1.0,1.0,1.0)、Ｓｆ＝(100.0,600.0,3400.0,0.1,2.0,0.1)である。（ｊ）（ｋ）は各軸のＺＭＰ入力の推移である。図より、ｘ軸上では、±0.1m以内、ｙ軸上では、±0.05m以内に収まっている様子がわかる。この範囲であれば、ロボット脚の妥当な足裏を設計することができる。また、（ｌ）（ｍ）は、２０式の等式拘束条件の左辺値と右辺値を示しており、ほぼ拘束条件を満たしている。
【００２４】
本発明を実施する方法は、２通りの方法が考えられる。
第一は、ＲＨＣと非線形制御の併用である。
ロボット本体は一般に非線形な機械的リンク系であり、実機への応用に当たっては上述のリアルタイムにＺＭＰを生成する方法と、従来の非線形制御とを併用することが考えられる。即ち、ロボットの目標軌道は、上述の方法によりリアルタイムに生成し、ロボットの各関節の制御は現代制御理論の非線形制御により行う。
従来の脚式ロボットでは、目標軌道はオフライン計算によって予め求めておいた各軌道をいくつか用意しておく必要があった。しかし、これでは例えば障害物を回避しながら脚を進めるといった行動が不可能である。この点において、本実施例の方法は有用性がある。
【００２５】
実機では、現在のＺＭＰ値を力センサ等にて測定することが可能であるため、ＲＨＣにて逐次得られるｕ_ＺＭＰ値を新たに目標値と考えて、実際のＺＭＰがこれに追従するように制御系を組む。ｕ_ｘ，ｕ_ｚについても加速度入力であるため、これを新たに加速度目標値と考えて、実機の重心がこの加速度に追従するような制御系とすればよい。このように、本実施例の方法は、ロボット全体重心についてモデル化したものなので、２脚式ロボットや４脚式ロボットあるいはそれ以外の脚式ロボットにも応用が可能である点が利点である。
第２は、すべての処理をＲＨＣで行う方法である。ＲＨＣの定式化の際、状態方程式を全体重心で代表するのでなく、ロボットの各リンクの非線形性を考慮した非線形状態方程式をたてる。この方法は、厳密なモデルによって非線形性が考慮されるが、脚の数が変更されたりロボットの構造が違うと、モデル化を初めからやり直さなければならない。
【００２６】
幅や歩行速度が決定されると、遊脚の振り出し等で生じる非線形力は、図７の非線形制御部において行う。
ロボットを設計する際に、前記第一の発明に基づいて、予め、各パラメータを変化させた場合の計算結果を集計しておき、設定したロボット動作に対する目標ＺＭＰ軌道の足裏内の動作範囲を把握可能とすることによって、ロボットの足裏面積を適切に設定することが可能となる。
【００２７】
４脚式の場合、２脚づつを交互に支持脚、遊脚として扱うことができる。２脚先端が構成する平面は、２脚式ロボットの場合に比較して、広い面積を確保することができる。
例えば４脚式ロボットがＴｒｏｔ歩容を行う場合、対角どおしの脚を交互に支持脚期−遊脚期を繰返す必要がある。このときには、図９のように、平面内の始点から、面内を横切り、次の支持脚ペアの平面に突き当たる点を終端と設定すれば、ＺＭＰはその周辺を移動する。ＺＭＰが収まるように平面面積をとっておけば、安定した歩行パターンが実現される。
【００２８】
【発明の効果】
本発明に係る脚式ロボットのリアルタイム最適制御方法においては、リアルタイム最適化計算が可能なＲＨＣ制御（ＲｅｃｅｄｉｎｇＨｏｒｉｚｏｎＣｏｎｔｒｏｌ）を用いた、脚式ロボットのＺＭＰ（ＺｅｒｏＭｏｍｅｎｔＰｏｉｎｔ）制御を得ることができる。
この結果、ロボットの目標軌道をリアルタイムで生成することができ、従来のように、目標軌道をオフライン計算によって予め求めておく必要がなく、したがって、例えば障害物を回避しながら脚を進めるといった臨機応変な行動を可能とするものである。
【図面の簡単な説明】
【図１】脚式ロボットのＺＭＰを示す説明図である。
【図２】脚式ロボットの矢状面（ｓａｇｉｔｔａｌ平面）と正面（ｆｌｏｎｔａｌ平面）を示した図面である。
【図３】３次元空間における動的釣合いを示した図である。
【図４】数値計算した実際の例を示した図である。
【図５】３次元空間において定式化した場合のシミュレーション結果を一部示した図である。
【図６】図５に続いて３次元空間において定式化した場合のシミュレーション結果の残りを示した図である。
【図７】制御ブロック線図である。
【図８】２脚式ロボットの歩行状態を示した説明図である。
【図９】４脚式ロボットの歩行状態を示した説明図である。
【符号の説明】
ＺＭＰゼロ・モーメント・ポイント

Claims

脚式ロボットのＺＭＰが

の等式拘束条件を満足するものにおいて、ｘ_ｚｍｐを新たな入力ｕ_ｚｍｐとして

を用いて最適解を求めたときに最適な目標ＺＭＰ軌道をリアルタイムで得るようにしたことを特徴とする脚式ロボットのリアルタイム最適制御方法。
ロボット全体重心の目標軌道と、請求項１の最適な目標ＺＭＰ軌道生成とともにロボットの各関節の制御を非線形制御により行うことを特徴とする脚式ロボットのリアルタイム最適制御方法
請求項１の方法に基づいて、予め、各パラメータを変化させた場合の計算結果を集計し、設定したロボット動作に対する目標ＺＭＰ軌道の足裏内の動作範囲を把握可能とすることによって、ロボットの足裏面積を適切に設定することが可能となることを特徴とする脚式ロボットのリアルタイム最適制御方法。