JP5807591B2

JP5807591B2 - 脚式歩行ロボットおよびその重心軌道生成方法

Info

Publication number: JP5807591B2
Application number: JP2012049260A
Authority: JP
Inventors: 将弘土井
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2012-03-06
Filing date: 2012-03-06
Publication date: 2015-11-10
Anticipated expiration: 2032-03-06
Also published as: JP2013184232A

Description

本発明は脚式歩行ロボットおよびその重心軌道生成方法に関し、特に、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成できる脚式歩行ロボットおよびその重心軌道生成方法に関する。

ロボットの脚式歩行制御において、足裏と地面との滑りに着目した技術が提案されている。このような技術として、例えば、予め水平方向並進床反力や鉛直軸周り床反力モーメントが、ある範囲内に収まるように歩容・軌道を生成する、という技術が知られている。このような技術として、例えば特許文献１に開示される技術がある。

特許第４１９９２３６号公報

特許文献１に開示される技術では、鉛直軸周り床反力モーメントの補償を、歩容生成・軌道生成とは別のこととして扱い、上半身の動作（例えば腕の振りや状態のひねり）によって抑制するという方針を採用している。

しかしながら、両腕で物体を運搬している状況（例えば、両腕でお盆を持って水を運んでいるという状況）では、床の摩擦が低い（鉛直軸周り床反力モーメントを低く制限する必要がある）からといって、腕を前後に振る、または上体を左右に振って補償動作を行う、ということはできない。また、腕の振りや上体のひねりによる補償では、例えば補償動作として上体を左にひねるとすると、いずれかのタイミングでひねった上体を正面に戻す必要があり、上体を戻す際に鉛直軸周りモーメントが不利な方向に発生することが避けられないという問題がある。加えて、補償量が大きな場合や上半身の質量が小さな場合には、腕の振りや上体のひねりを可動域限界まで行ったとしても補償しきれない、という事態が発生する可能性もある。

一方で、発明者は検討の結果、ＺＭＰの特性上、鉛直軸周りの床反力モーメントは、重心周りモーメントと、重心位置と、ＺＭＰ位置との関数になっており、必ずしも上半身の動きで全て補償する必要はないという事実を見出した。

本発明はこの事実に基づき、上述した問題点を解決するためになされたものであり、重心軌道生成時に鉛直軸周りの床反力モーメント制限を反映させることで、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成できる脚式歩行ロボットおよびその重心軌道生成方法を提供することを目的とする。

本発明に係る脚式歩行ロボットの重心軌道生成方法は、モデル予測制御を適用して重心軌道を生成する脚式歩行ロボットの重心軌道生成方法であって、前記脚式歩行ロボットを単一の質点で表した単質点モデルを用いて、設定される目標ＺＭＰ軌道に基づくＺＭＰ制約条件を満たすように、重心軌道の生成を行う第１の重心軌道生成ステップと、前記第１の重心軌道生成ステップで生成された重心軌道と、前記脚式歩行ロボットを複数の質点で表した多質点モデルと、を用いて、前記脚式歩行ロボットの重心周りに発生する重心周りモーメントを計算する重心周りモーメント計算ステップと、前記脚式歩行ロボットの進行方向と、当該進行方向に直交する方向と、に平行する方向を水平方向とし、当該水平方向に直交する方向を鉛直方向として、前記脚式歩行ロボットの足裏が床に接触することで受ける床反力に関して、前記脚式歩行ロボットのＺＭＰを基準として前記鉛直方向に沿って配置される鉛直軸周りに発生する鉛直軸周り床反力モーメントと、前記脚式歩行ロボットのＺＭＰを基準として前記水平方向に発生する水平方向並進床反力と、についてそれぞれ制限値が設定され、当該設定される制限値と、前記重心周りモーメント計算ステップで計算された重心周りモーメントと、を用いて、床反力の制約条件を反映させるための重心加速度の制約条件を計算する重心加速度制約条件計算ステップと、前記単質点モデルを用いて、前記ＺＭＰ制約条件と、前記重心加速度制約条件計算ステップで計算された重心加速度の制約条件と、を満たすように、重心軌道の生成を行う第２の重心軌道生成ステップと、を備えるものである。これにより、重心軌道生成時に鉛直軸周りの床反力モーメント制限を反映させることで、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成することができる。

また、前記重心加速度制約条件計算ステップでは、前記鉛直軸周り床反力モーメントの制限値および前記水平方向並進床反力の制限値として、それぞれ予め定めた固定値が設定されるようにするとよい。

さらにまた、前記第１の重心軌道生成ステップによる重心軌道生成回数および前記第２の重心軌道生成ステップによる重心軌道生成回数の合計が、規定回数に達したか否かを判定する判定ステップを更に備え、前記規定回数に達するまでの間、前記重心周りモーメント計算ステップと、前記重心加速度制約条件計算ステップと、前記第２の重心軌道生成ステップと、による処理を繰り返して実行させるようにするとよい。

また、前記第１の重心軌道生成ステップおよび前記第２の重心軌道生成ステップでは、重心軌道の生成を、制約条件の下で最適解を求解する凸二次計画問題として扱うようにするとよい。

さらにまた、前記脚式歩行ロボットが、２脚歩行ロボットであるようにするとよい。

本発明に係る脚式歩行ロボットは、上述の脚式歩行ロボットの重心軌道生成方法を実行するコントローラを備えるものである。これにより、重心軌道生成時に鉛直軸周りの床反力モーメント制限を反映させることで、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成することができる。

本発明によれば、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成できる脚式歩行ロボットおよびその重心軌道生成方法を提供することができる。

実施の形態１に係る脚式歩行ロボットの模式図である。実施の形態１に係る重心軌道生成処理の概要を説明するための図である。実施の形態１に係る力の定義を説明するための図である。実施の形態１に係る線形倒立振子モデルを説明するための図である。実施の形態１に係る重心周りモーメントを説明するための図である。実施の形態１に係る各サンプリング点での入出力と状態変数を説明するための図である。実施の形態１に係るReceding Horizon制御の考え方を説明するための図である。実施の形態１に係る支持多角形を説明するための図である。実施の形態１に係る多質点モデルを説明するための図である。実施の形態１に係る初期化処理を示すフローチャートである。実施の形態１に係る周期実行処理を示すフローチャートである。

以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一又は対応する要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略される。

＜発明の実施の形態１＞
図１は、本実施の形態に係る脚式歩行ロボットとしての２脚歩行ロボット１０の模式的な構成例を示している。以下では、２脚歩行ロボット１０を単にロボット１０と称する。ロボット１０は、関節角を制御するアクチュエータと、関節角を検出するエンコーダと、が脚の各関節に内蔵されている。また、ロボット１０は、コントローラ２０を備えている。コントローラ２０が、歩容データに基づいて脚の各関節を制御する。すなわち、コントローラ２０は、歩容データから得られる各関節の目標角度に追従するように、アクチュエータを制御する。より具体的には、コントローラ２０は、歩容データ（足の目標軌道と重心の目標軌道）を用いて各関節の目標角の時系列データを算出し、算出した目標角に追従するように、各関節を制御する。歩容データを用いて関節目標角を生成する手法については、従来から知られている手法を用いればよいため、ここではその詳細な説明を省略する。

なお、コントローラ２０は、主要なハードウェア構成として、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される制御プログラムと、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを用いて構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバスによって相互に接続されている。

図２は、コントローラ２０が実行する重心軌道生成処理の概要を示すフローチャートである。コントローラ２０は、まず、モデル予測制御に基づいて、ロボット１０の重心軌道の生成を行う（Ｓ２０１）。コントローラ２０は、単質点モデルを用いて、設定される目標ＺＭＰ軌道に基づくＺＭＰ制約条件を満たすように重心軌道の生成を行うが、ここでの重心軌道の生成は床反力の制約条件（制限）を設けずに行う。

次いで、コントローラ２０は、Ｓ２０１で生成された重心軌道と、ロボット１０のフルモデルと、を用いて、Ｒｏｌｌ・Ｐｉｔｃｈ・Ｙａｗ軸周りの重心周りモーメントを計算する（Ｓ２０２）。

次いで、コントローラ２０は、Ｓ２０２で計算された重心周りモーメントと、ロボット１０の足裏が床に接触することで受ける床反力に関する制限値としての、設定される鉛直軸（Ｙａｗ軸）周り床反力モーメント制限値と、設定される水平方向並進床反力の制限値と、を用いて、床反力の制約条件（制限）を反映させるための重心加速度の制約条件を計算する（Ｓ２０３）。

最後に、コントローラ２０は、再度モデル予測制御を用いて、ロボット１０の重心軌道の再生成を行う（Ｓ２０４）。コントローラ２０は、単質点モデルを用いて、ＺＭＰ制約条件と、計算された重心加速度の制約条件と、を満たすように重心軌道の生成を行うが、ここでの重心軌道の再生成は計算した重心加速度の制約条件を加えて行う。

これにより、水平方向並進床反力および鉛直軸周り床反力モーメントの制限値を満たす軌道を生成することができる。この軌道生成処理で特筆すべき点は、軌道の生成処理自体は重心軌道の生成を行う処理であり、かつ、上半身動作による鉛直軸周り床反力モーメントの補償を全く行っていないことである。

以下、Ｓ２０１〜Ｓ２０４での処理を具体的に説明する。なお、説明の都合上、まずＳ２０１およびＳ２０４で用いるモデル予測制御を説明し、続いてＳ２０２、Ｓ２０３の順で説明を行う。また、本実施の形態に係る重心軌道生成処理の初期化ルーチン処理および周期実行ルーチン処理の詳細ついては後述する。

（Ｓ２０１、Ｓ２０４：モデル予測制御の説明）
まず、図３を用いて、軌道生成処理に用いる力の定義を説明する。図３左図に示すように、ロボット１０の進行方向をｘ軸方向、ｘ軸方向に直交する方向をｙ軸方向とし、ｘ軸およびｙ軸と直交する軸をｚ軸方向とする。また、図３右図にロボットの足下部分を拡大して示す。図３右図に示すように、ロボット１０の足裏が床に接触することで受ける反力に関して、ロボット１０のＺＭＰを基準として、水平方向並進床反力をそれぞれＦ_ｘ、Ｆ_ｙとし、鉛直方向並進床反力をＦ_ｚとし、鉛直軸周り床反力モーメントをＭ_ｚとする。また、ＺＭＰの位置は、ｘ_ｚｍｐ，ｙ_ｚｍｐ，ｚ_ｚｍｐとする。

Ｓ２０１およびＳ２０４の軌道生成処理では、ロボット１０を単一の質点として表現した単質点モデルを用いる。ロボット１０の重心位置を、ｘ_ｇ，ｙ_ｇ，ｚ_ｇと定義する。ロボット１０の全質量をｍとし、重力加速度をｇとする。ロボット１０の重心モデルのＺＭＰ方程式は、以下の式（１）により示される。式（１）において、Ｉ^ｘ、およびＩ^ｙは、重心周りモーメントを示す（図５参照。）。重心周りモーメントの詳細は後述する。なお、以下の式において、変数の上部に配置される"・"は一階微分を示し、"・・"は二階微分を示している。

次に、線形倒立振子モデルを用いた近似を行う。この線形倒立振子モデルを用いた近似はよく知られている。式（２）に示すように、床面からの重心高さｚ_ｇを一定値（ｃｏｎｓｔ）にし（図４を参照。）、重心周りモーメントＩ^ｘ、Ｉ^ｙを０に設定する。

すると、上述した式（１）は、線形化された簡単な式として示すことができる。この線形化された式を、以下の式（３）に示す。

次に、ｘ軸方向に関して、システムへの入力と出力を以下の式（４）に示すように定義する。

さらに、上述した式（３）を線形の連続システムとして記述する。この連続システムを、以下の式（５）に示す。

上述した式（５）における行列ｘ（ｔ）、行列Ａ、行列ｂ、行列ｃ_ｐのチルダ、行列ｃ_ａのチルダは、以下の式（６）に示される。

上述した式（５）を離散時間間隔Δｔで離散化する。この離散化の様子を図６に示す。図６は、離散化した各サンプリング点における入出力と状態変数の関係を示す図である。得られる離散システムを、以下の式（７）に示す。

上述した式（７）における行列ｘ_ｋ、行列Ａ、行列ｂ、行列ｃ_ｐ、行列ｃ_ａは、以下の式（８）に示される。

本実施の形態に係るモデル予測制御は、Receding Horizon Controlとも呼ばれる。図７を用いて、このReceding Horizon Controlの考え方を簡単に説明する。図７に示すように、現在においてＮΔｔ未来までの期間のモデル挙動を予測して最適化計算を行い、現在の入力値ｕ_ｋを決定する。Δｔ秒後にはまた同様の計算を行いΔｔ秒後の入力値ｕ_ｋ＋１を決定する。このような計算過程を一定周期毎に行い、結果として最適な軌道を生成することができる。

ＮΔｔ未来までの期間の予測に関しては、以下の式（９）に示すように、離散システムを未来に向かって積算していくことで得ることができる。式（９）において、現在から未来までの各サンプリング点における入力値をｕ^ｘ _ｋ，ｕ^ｘ _ｋ＋１，ｕ^ｘ _ｋ＋２，・・・，ｕ^ｘ _ｋ＋Ｎとし、現在から未来までの各サンプリング点における状態変数値をｘ_ｋ，ｘ_ｋ＋１，ｘ_ｋ＋２，・・・，ｘ_ｋ＋Ｎとする。また、出力に関しても同様に、ｐ^ｘ _ｋ，ｐ^ｘ _ｋ＋１，ｐ^ｘ _ｋ＋２，・・・，ｐ^ｘ _ｋ＋Ｎ、ａ^ｘ _ｋ，ａ^ｘ _ｋ＋１，ａ^ｘ _ｋ＋２，・・・，ａ^ｘ _ｋ＋Ｎとする。これらの入出力は、図６に示したとおりである。なお、式（９）では、現在のタイミングを、ｋサンプル目としている。

また、入力値列ｕ^ｘ _ｋ、出力値列ｐ^ｘ _ｋ、ａ^ｘ _ｋを用いて、以下の式（１０）に示す行列ｕ^ｘ _ｋ、ｖ^ｘ _ｋ、ｗ^ｘ _ｋを定義する。

上述した式（７）、（９）、（１０）を用いて、未来の出力値列ｖ^ｘ _ｋおよびｗ^ｘ _ｋを計算する。すると、未来の出力値列ｖ^ｘ _ｋおよびｗ^ｘ _ｋは、それぞれ以下の式（１１）および（１２）に示される。

ここで、ＮΔｔ未来までの目標ＺＭＰ列をｖ^ｘ _ｋ＋１のハットとし、以下の式（１３）により定める。ここで、目標ＺＭＰ軌道（ｐ^ｘ _ｋ＋１のハットの列）は、予め適切な値が設定されるものとする。

モデル予測制御型軌道生成方法では、最適化の評価関数を、例えば以下の式（１４）に示す形で設定する。式（１４）に示す評価関数は、予測区間内の目標ＺＭＰとＺＭＰの誤差の二乗と、入力値の二乗と、を足し合わせた値を最小化する形で示される。

上述した式（１４）に示した評価関数はモデル予測制御の一般的な与え方である。本実施の形態では、式（１４）に関して、第１項の役割は、ＺＭＰを目標ＺＭＰに近づけることであり、第２項の役割は、入力値が過剰に働くのを抑止すること（重心が発散するのを抑制すること）であると考える。また、式（１４）に示した行列ＱおよびＲは、上記第１項および第２項をバランスするための重み行列であり、以下の式（１５）により示される。これらの重み行列の値は、制御対象に応じて予め適切な値が設定される。

上述した式（１４）に式（１１）を代入することで、以下に示す式（１６）を得る。

上述した式（１６）における行列Ｈ^ｃ _ｋ、ベクトルｆ^ｘ _ｋは、以下の式（１７）に示される。

従って、上述した式（１４）の評価関数は、以下の式（１８）により示す形で定式化することができる。

上述した式の展開はｙ軸方向に関しても同様に成り立つ。このため、ｙ軸方向に関する評価関数は、以下の式（１９）により示される。

ここで、ベクトルｕ_ｋ、行列Ｈ_ｋ、ベクトルｆ_ｋを、以下の式（２０）に示すように定める。

すると、上述した式（１８）および（１９）をまとめることで、以下に示す式（２１）を得ることができる。すなわち、式（２１）は、本実施の形態に係るモデル予測制御で用いる、ｘ軸方向とｙ軸方向の両方に関して考慮した最適化の評価関数となる。

次に、評価関数に対する制約条件を説明する。本実施の形態では、ＺＭＰ制約条件と重心加速度制約条件を、制約条件として評価関数に設定する。ただし、Ｓ２０１での軌道生成処理では、ＺＭＰ制約条件のみを設定して軌道生成処理を行い、Ｓ２０４での軌道生成処理では、ＺＭＰ制約条件に加えて重心加速度制約条件を設定して軌道生成処理を行う。

まず、ＺＭＰ制約条件を説明する。ロボット１０の重心軌道を生成する場合、ロボット１０の足裏の範囲内（より正確には、足裏外縁から一定のマージンを設けて配置される多角形内）にＺＭＰが留まるような軌道を生成することで、安定な歩行を実現できる。このような多角形は、一般的に支持多角形と呼ばれる。

図８に、単脚支持期および両脚支持期における支持多角形を例示する。図８左図に示すように、左足平３０Ｌと右足平３０Ｒに設けられたＺＭＰ設定可能域４０に関して、左足支持期では左足平３０Ｌに支持多角形が構成される。また、両足支持期では、図８右図に示すように支持多角形５０が構成される。

その名前からも分かるように、支持多角形は単脚支持期や両脚支持期などの状況によって形状は変化するものの、基本的には多角形で表される。すなわち、支持多角形内にＺＭＰが留まる、という条件は、任意のベクトルλ^ｘ、λ^ｙ、ｄ^ｚを用いて、以下の式（２２）により示すことができる。これらのベクトルλ^ｘ、λ^ｙ、ｄ^ｚの値は、予め適切な値が設定されるものとする。

従って、未来の各サンプリング点ｋ＋ｊ（ｊ＝１，・・・，Ｎ）において上述した式（２２）に示したＺＭＰ制約を仮定することで、以下に示す式（２３）を得ることができる。

さらに、上述した式（２３）を展開することで、以下に示す式（２４）を得る。

そして、上述した式（２４）をさらに整理することで、以下に示す式（２５）を得る。

上述した式（２５）において、ベクトルｖ_ｋ＋１およびベクトルη^ｚは、以下の式（２６）に示される。

また、上述した式（２５）において、行列Γ^ｚは、以下の式（２７）に示される。

さらに、上述した式（２５）に対して上述した式（１１）を代入することで、以下の式（２８）に示す線形不等式を導くことができる。すなわち、ＺＭＰを支持多角形内に留めるというＺＭＰ制約条件に関して、最終的には、入力値列ベクトルｕ^ｋに関する線形不等式により記述できることを示している。

なお、上述した式（２８）では、以下の式（２９）に示すように、行列Ｐ^ｚ _ｋ、行列ｑ^ｚ _ｋを定めた。

次に、重心加速度制約条件を説明する。ｘｙ方向の重心加速度が、上述した式（２２）に示したような線形な関係で記述されているとする。すなわち、以下の式（３０）に示すような線形関係で記述されているとする。

すると、以降は上述したＺＭＰ制約条件に関する式の導出と全く同様の式変形を行うことで、最終的に、重心の加速度制約条件を、以下の式（３１）に示すようなｕ^ｋに関する線形不等式で記述することができる。

以上に説明したようにして、ＺＭＰ制約条件と重心加速度制約条件を導入することができた。Ｓ２０１では、ＺＭＰ制約条件のみを設定し、重心加速度制約条件を設定しないため、制約条件Ｐ_ｋおよびｑ_ｋを、以下の式（３２）に示すように定める。

Ｓ２０４では、ＺＭＰ制約条件に加えて、重心加速度制約条件を設定する。このため、制約条件Ｐ_ｋおよびｑ_ｋを、以下の式（３３）に示すように定める。

上述した式（２８）、（３１）、（３２）、（３３）から、制約条件はいずれの場合であっても、以下の式（３４）により記述することができる。
式（２１）

以上をまとめると、上述した式（２１）に示した評価関数と、上述した式（３４）に示した制約条件と、を用いることで、以下の式（３５）に示す最適化問題を構成することができる。式（３５）に示すように、最適化問題は凸二次計画問題として定式化することができる。この凸二次計画問題は高速かつ安定に求解することが可能である。モデル予測制御では、現在状態を入力して最適化問題を求解するまでの計算処理を時間を進めながら毎周期実行していき、毎周期ごとに入力値を決めていく。

（Ｓ２０２：フルモデルによる重心周りモーメントの計算の説明）
続いて、重心周りモーメントの計算を説明する。
上述したモデル予測制御による重心軌道の計算では、ロボット１０を単一の質点として表現した単質点モデルを用いて計算するが、複数の質点を持つロボット１０が実際に運動をする場合には、単質点モデルでは表現されない重心周りのモーメントが発生する（図５参照）。この重心周りのモーメントは、重心周りに発生する角運動量の微分、もしくは、重心周りに発生する角運動量の時間変化率などとも呼ばれ、いずれも同じ値を意味している。

本実施の形態では、図９に例示するフルモデルとしての多質点モデルを用いて、ロボット１０が表現されるものとする。なお、Ｍ個の質点それぞれに順番に番号が付与され、各質点が、質量ｍ_ｉと、位置ｒ_ｉ＝［ｘ_ｉｙ_ｉｚ_ｉ］^Ｔのパラメータを持つものとする。この時、ロボット１０全体の重心の位置ｒ_ｃｏｇは、以下の式（３６）により示される。ロボット１０の各質点の質量と位置の設定は、ユーザ、あるいはコントローラ２０に指令を与える上位コントローラによって設定すればよい。

すると、重心周りモーメントＩは、以下の式（３７）により計算することができる。

なお、上述した式（３７）において、行列Ｉは以下の式（３８）により示される。

上述した式（３７）から分かるように、重心周りモーメントは、重心と各質点の相対位置・加速度を外積計算することで各質点が重心周りに発生するモーメント量を計算し、さらに、この計算量を足し合わせる形になっている、と解釈することができる。

本実施の形態では、生成した重心軌道と、ロボット１０の足先軌道と、に基づいて、未来のＮ個の各サンプリング点におけるロボット１０の姿勢を計算し、その姿勢から各質点の位置および加速度を計算する。そして、この計算した各質点の位置および加速度と、式（３６）を用いて計算するロボット１０の全体重心位置およびその加速度と、を式（３６）に代入する。すなわち、未来のＮ個の各サンプリング点において上述した式（３７）を計算することで、各サンプリング点における重心周りモーメントＩ^ｘ _ｋ＋ｊ、Ｉ^ｙ _ｋ＋ｊ、Ｉ^ｚ _ｋ＋ｊ（ｊ＝１，・・・，Ｎ）を計算する。

なお、重心軌道を再び更新する際には、この重心周りモーメントの計算値を用いて、ｘ、ｙ軸の重心周りモーメントＩ^ｘ _ｋ＋ｊ、Ｉ^ｙ _ｋ＋ｊによって発生するＺＭＰ偏差を計算し、目標ＺＭＰ軌道を修正するものとしてもよい（この手法は、例えば特開２００４−１１４２４３号公報にて詳細に提案されている。）。これによって、ＺＭＰのずれをより効果的に抑制することができ、より安定した動作を実現することができる。

（Ｓ２０３：床反力制約を満たすための重心加速度制約条件の計算の説明）
続いて、重心加速度制約条件の計算を説明する。
床反力制約を満たすための重心加速度制約条件は、上述した式（３７）により計算される重心周りモーメントと、鉛直軸（Ｙａｗ軸）周り床反力モーメント制限値と、水平方向並進床反力の制限値と、を用いて計算される。以下、鉛直軸（Ｙａｗ軸）周り床反力モーメント制限値と、水平方向並進床反力の制限値と、を説明する。

まず、鉛直軸（Ｙａｗ軸）周り床反力モーメント制限値を説明する。
上述した式（１）の１番目の式にｙ^（・・） _ｇ（ｙ_ｇの二階微分）を掛け、２番目の式にｘ^（・・） _ｇ（ｘ_ｇの二階微分）を掛け、１番目の式から２番目の式を加減することで、以下に示す式（３９）を得る。

また、鉛直軸周り床反力モーメントＭ_ｚのつり合い式は、以下の式（４０）により示される。

上述した式（３９）および（４０）から、以下の式（４１）を得る。

鉛直軸周り床反力モーメントＭ_ｚに関して、床面に合わせてロボット１０の足が滑らないように上限Ｍ^ｍａｘ _ｚおよび下限Ｍ^ｍｉｎ _ｚを設定すると、この条件は以下の式（４２）により示される。なお、本実施の形態では、鉛直軸周り床反力モーメントの上下限制限値は、固定値として設定される。鉛直軸周り床反力モーメントの上下限制限値の設定は、ユーザ、あるいはコントローラ２０に指令を与える上位コントローラによって設定すればよい。

従って、上述した式（４１）および（４２）から、以下の式（４３）を得る。このようにして、鉛直軸周り床反力モーメントの上下限から、重心水平方向加速度の制約条件を導出することができる。

次に、水平方向並進床反力の制限値を説明する。並進床反力の制限についても、床面に合わせてロボット１０の足が滑らないように上限Ｆ^ｍａｘ _ｘ、Ｆ^ｍａｘ _ｙおよび下限Ｆ^ｍｉｎ _ｘ、Ｆ^ｍｉｎ _ｙを設定すると、この条件は以下の式（４４）により示される。そして、式（４４）を式（４５）へと変換することで、上述した重心水平方向加速度の制約条件へと変換することができる。なお、本実施の形態では、水平方向並進床反力の上下限制限値は、固定値として設定される。水平方向並進床反力の上下限制限値の設定は、ユーザ、あるいはコントローラ２０に指令を与える上位コントローラによって設定すればよい。

未来のＮ個の各サンプリング点において、鉛直軸周り床反力モーメントの上下限値を用いた上述した式（４３）と、水平方向並進床反力の制限値を用いた上述した（４５）と、を計算することで、各サンプリング点における重心水平方向加速度の制約条件を計算することができる。

すなわち、任意のサンプリング点ｋ＋ｊにおける重心水平方向加速度の制約条件は、鉛直軸周り床反力モーメントの制約条件が以下に示す式（４６）により示される。ただし、ｚ_ｇ[ｋ＋ｊ]のツードットは、未来のサンプリング点ｋ＋ｊにおける鉛直方向重心加速度である。

また、水平方向並進床反力の制約条件が以下に示す式（４７）により示される。

そして、上述した式（４６）および（４７）をまとめることで、以下に示す式（４８）を得る。

さらに、上述した式（４８）をよりまとめることで、以下に示す式（４９）を得る。

さらに、上述した式（４９）を、上述した式（２５）〜（２８）にかけて変換の流れと同様にして変換する。まず、上述した式（４９）を、以下の式（５０）に変換する。

そして、行列Γ^ａを以下の式（５１）に示すように定める。

また、ベクトルη^ａを以下の式（５２）に示すように定める。

すると、上述した式（５０）、（５１）、（５２）から、以下の式（５３）を得る。

さらに、上述した式（５３）を変換することで、最終的に、以下の式（５４）に示す入力の制約条件式を導出することができる。この導出された制約条件に関して、本実施の形態と従来技術との間での一番の相違点は、鉛直軸周り床反力モーメントの制約を重心の加速度制約に変換し、軌道生成時に反映させるように構成したことである。

上述した式（５４）に示した重心の加速度制約条件を、式（３４）に示したＺＭＰ制約条件と合わせて制約条件式として設定し、この制約条件の下で、式（３５）に示した凸二次計画問題を求解する。これにより、地面に対して足裏が滑ることを回避して安定して動作可能な重心軌道を生成することが可能となる。

（重心軌道生成処理の初期化ルーチン処理および周期実行ルーチン処理の説明）
以下、本実施の形態に係る重心軌道生成処理の初期化ルーチン処理および周期実行ルーチン処理の詳細を図１０及び図１１を用いて説明する。

（重心軌道生成処理の初期化ルーチン処理）
図１０は、コントローラ２０が実行する重心軌道生成処理の初期化ルーチン処理のフローチャートを示す。初期化ルーチンは、後述する図１１に示す周期実行ルーチン開始前の初期化時に実行される。

コントローラ２０は、サンプリング時間間隔である周期Δｔと、基準重心高さｈと、を設定する（Ｓ１００１）。コントローラ２０は、システム行列およびベクトルを計算する（Ｓ１００２）。より具体的には、上述した式（８）に示した行列Ａ、ｂ、ベクトルｃ_ｐ、ｃ_ａを計算する。コントローラ２０は、上述した式（１５）に示した重み行列Ｑ、Ｒを設定する（Ｓ１００３）。コントローラ２０は、上述した式（１１）および（１２）に示した出力値系列の係数（Ｇ_ｐ１、Ｇ_ｐ２、Ｇ_ａ１、Ｇ_ａ２）を計算する（Ｓ１００４）。

（重心軌道生成処理の周期実行ルーチン処理の説明）
図１１は、コントローラ２０が実行する重心軌道生成処理の周期実行ルーチン処理のフローチャートを示す。コントローラ２０は、ロボット１０の現在状態（重心位置ｘ_ｋ、ｙ_ｋ、速度、加速度）を設定する（Ｓ１１０１）。ロボット１０の現在状態は、ロボット１０自らが現在の重心位置・速度・加速度を取得して設定することができる。また、現在状態は、ユーザ、あるいはコントローラ２０に指令を与える上位コントローラによって設定するものとしてもよい。

コントローラ２０は、目標ＺＭＰ軌道（ｖ^ｘ _ｋのハット、ｖ^ｙ _ｋのハット）を設定する（Ｓ１１０２）。コントローラ２０は、上述した式（２０）に示したＨ_ｋ、ｆ_ｋを用いて、上述した式（２１）に示した評価関数を設定する（Ｓ１１０３）。

コントローラ２０は、上述した式（２９）、（２６）、（２７）に示したＺＭＰ制約条件（Ｐ^ｚ _ｋ、ｑ^ｚ _ｋ）を計算する（Ｓ１１０４）。コントローラ２０は、上述した式（３４）に示した不等式制約条件に関して、Ｓ２００４で計算した値を、上述した式（３２）に示すようにして不等式制約として設定する（Ｓ２００５）。すなわち、ＺＭＰ制約条件のみを不等式制約条件に設定する。

コントローラ２０は、上述した式（３５）に示した凸二次計画問題を求解する（Ｓ１１０６）。コントローラ２０は、凸二次計画問題の求解結果ｕ_ｋを用いて、重視軌道を更新する（Ｓ１１０７）。すなわち、現在の時刻から未来のＮ個のサンプリング点における各重心位置を計算する。ｊは１〜Ｎまでの値を取る整数である。

コントローラ２０は、重心軌道のＳ１１０７での更新回数が規定回数に到達したか否か判定する（Ｓ２００８）。Ｓ１１０９で計算される重心周りモーメントの計算値は、Ｓ１１０７において１回目に生成した重心軌道に基づいて生成される。このため、厳密に言えば、重心軌道を再更新すると重心周りモーメントの計算値も変化してしまう。しかし、重心軌道更新のプロセスを再度繰り返すことで、この変化による影響を無視することができる。そこで、本実施の形態では、予め設定される規定回数に到達するまでの間、重心軌道の更新を行うことで、重心周りモーメントの計算値の変化を収束させる。なお、鉛直軸周り床反力モーメントの上下限値に多少余裕を持たせておくことで、この変化による影響を無視することもできる。

コントローラ２０は、上述した式（３７）を用いて、生成された重心軌道に基づく重心周りモーメントを計算する（Ｓ２００９）。より具体的に説明すると、コントローラ２０は、まず、Ｓ２００７で生成（あるいは更新）した重心軌道と、ロボット１０の足先軌道と、を用いて未来のＮ個の各サンプリング時刻点におけるロボット１０の姿勢を計算する。足先軌道の生成は、例えば、多項式補間を用いて生成すればよい。そして、コントローラ２０は、計算した各姿勢から、図９に例示した各質点の位置を計算し、この各質点の位置の変化から各質点の加速度を計算する。コントローラ２０は、この計算した各質点の位置および加速度と、式（３６）を用いて計算するロボット１０の全体重心位置およびその加速度と、を式（３７）に入力し、重心周りモーメントを計算する。なお、足先軌道の生成手法については、従来から知られている手法を用いればよいため、ここではその詳細な説明を省略する。また、ロボット１０の全体重心位置の計算方法については、式（３６）を用いて計算してもよいし、Ｓ１１０７で生成（あるいは更新）した重心軌道をそのまま用いるものとしてもよい。

コントローラ２０は、床反力制限値（鉛直軸周り床反力モーメントの上下限制限値および水平方向並進床反力の上下限制限値）を設定する（Ｓ１１１０）。コントローラ２０は、上述した式（４８）〜（５４）に示した加速度制約条件（Ｐ^ａ _ｋ、ｑ^ａ _ｋ）を計算する（Ｓ２０１１）。コントローラ２０は、上述した式（３４）に示した不等式制約条件に関して、Ｓ１１１１で計算した値を、上述した式（３３）に示すようにして不等式制約として設定する（Ｓ１１１２）。すなわち、ＺＭＰ制約条件に加えて重心加速度制約条件を不等式制約条件に設定する。

コントローラ２０は、時間Δｔ後から開始する次の周期での軌道生成処理を行うために、周期更新を行う（Ｓ１１１３）。すなわち、ｋを１増加させ、ｔをΔｔ増加させる。

なお、図２で示したＳ２０１の内容が図１１に示したＳ１１０５およびＳ１１０６での処理に相当し、Ｓ２０２の内容がＳ１１０９での処理に相当し、Ｓ２０３の内容がＳ１１１０およびＳ１１１１での処理に相当し、Ｓ２０４の内容がＳ１１１２および再び実行するＳ１１０６での処理に相当する。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述した実施の形態では、鉛直軸周り床反力モーメントの上下限制限値および水平方向並進床反力の水平方向床反力の上下限制限値を固定値とする場合を例に説明したいが、本発明はこれに限定されず、地面に応じてオンラインに変更する、時間経過に応じて変化させる、などのように状況に応じて変更させる構成としてもよい。

１０脚式歩行ロボット、
２０コントローラ、
３０Ｒ右足平、
３０Ｌ左足平、
４０ＺＭＰ設定可能域、
５０支持多角形、

Claims

モデル予測制御を適用して重心軌道を生成する脚式歩行ロボットの重心軌道生成方法であって、
前記脚式歩行ロボットを単一の質点で表した単質点モデルを用いて、設定される目標ＺＭＰ軌道に基づくＺＭＰ制約条件を満たすように、重心軌道の生成を行う第１の重心軌道生成ステップと、
前記第１の重心軌道生成ステップで生成された重心軌道と、前記脚式歩行ロボットを複数の質点で表した多質点モデルと、を用いて、前記脚式歩行ロボットの重心周りに発生する重心周りモーメントを計算する重心周りモーメント計算ステップと、
前記脚式歩行ロボットの進行方向と、当該進行方向に直交する方向と、に平行する方向を水平方向とし、当該水平方向に直交する方向を鉛直方向として、前記脚式歩行ロボットの足裏が床に接触することで受ける床反力に関して、前記脚式歩行ロボットのＺＭＰを基準として前記鉛直方向に沿って配置される鉛直軸周りに発生する鉛直軸周り床反力モーメントと、前記脚式歩行ロボットのＺＭＰを基準として前記水平方向に発生する水平方向並進床反力と、についてそれぞれ制限値が設定され、当該設定される制限値と、前記重心周りモーメント計算ステップで計算された重心周りモーメントと、を用いて、床反力の制約条件を反映させるための重心加速度の制約条件を計算する重心加速度制約条件計算ステップと、
前記単質点モデルを用いて、前記ＺＭＰ制約条件と、前記重心加速度制約条件計算ステップで計算された重心加速度の制約条件と、を満たすように、重心軌道の生成を行う第２の重心軌道生成ステップと、
を備える脚式歩行ロボットの重心軌道生成方法。
前記重心加速度制約条件計算ステップでは、前記鉛直軸周り床反力モーメントの制限値および前記水平方向並進床反力の制限値として、それぞれ予め定めた固定値が設定される、
請求項１に記載の脚式歩行ロボットの重心軌道生成方法。
前記第１の重心軌道生成ステップによる重心軌道生成回数および前記第２の重心軌道生成ステップによる重心軌道生成回数の合計が、規定回数に達したか否かを判定する判定ステップを更に備え、
前記規定回数に達するまでの間、前記重心周りモーメント計算ステップと、前記重心加速度制約条件計算ステップと、前記第２の重心軌道生成ステップと、による処理を繰り返して実行させる、
請求項１または２に記載の脚式歩行ロボットの重心軌道生成方法。
前記第１の重心軌道生成ステップおよび前記第２の重心軌道生成ステップでは、重心軌道の生成を、制約条件の下で最適解を求解する凸二次計画問題として扱う、
請求項１〜３いずれか１項に記載の脚式歩行ロボットの重心軌道生成方法。
前記脚式歩行ロボットが、２脚歩行ロボットである
請求項１〜４いずれか１項に記載の脚式歩行ロボットの重心軌道生成方法。
前記請求項１〜５のいずれか１項の脚式歩行ロボットの重心軌道生成方法を実行するコントローラを備える脚式歩行ロボット。