(57)【特許請求の範囲】
【請求項１】
モデル予測制御を適用して重心軌道を生成する脚式歩行ロボットの重心軌道生成方法であって、
前記脚式歩行ロボットを単一の質点で表した単質点モデルを用いて、設定される目標ＺＭＰ軌道に基づくＺＭＰ制約条件を満たすように、重心軌道の生成を行う第１の重心軌道生成ステップと、
前記第１の重心軌道生成ステップで生成された重心軌道と、前記脚式歩行ロボットを複数の質点で表した多質点モデルと、を用いて、前記脚式歩行ロボットの重心周りに発生する重心周りモーメントを計算する重心周りモーメント計算ステップと、
前記脚式歩行ロボットの進行方向と、当該進行方向に直交する方向と、に平行する方向を水平方向とし、当該水平方向に直交する方向を鉛直方向として、前記脚式歩行ロボットの足裏が床に接触することで受ける床反力に関して、前記脚式歩行ロボットのＺＭＰを基準として前記鉛直方向に沿って配置される鉛直軸周りに発生する鉛直軸周り床反力モーメントと、前記脚式歩行ロボットのＺＭＰを基準として前記水平方向に発生する水平方向並進床反力と、についてそれぞれ制限値が設定され、当該設定される制限値と、前記重心周りモーメント計算ステップで計算された重心周りモーメントと、を用いて、床反力の制約条件を反映させるための重心加速度の制約条件を計算する重心加速度制約条件計算ステップと、
前記単質点モデルを用いて、前記ＺＭＰ制約条件と、前記重心加速度制約条件計算ステップで計算された重心加速度の制約条件と、を満たすように、重心軌道の生成を行う第２の重心軌道生成ステップと、
を備える脚式歩行ロボットの重心軌道生成方法。
【請求項２】
前記重心加速度制約条件計算ステップでは、前記鉛直軸周り床反力モーメントの制限値および前記水平方向並進床反力の制限値として、それぞれ予め定めた固定値が設定される、
請求項１に記載の脚式歩行ロボットの重心軌道生成方法。
【請求項３】
前記第１の重心軌道生成ステップによる重心軌道生成回数および前記第２の重心軌道生成ステップによる重心軌道生成回数の合計が、規定回数に達したか否かを判定する判定ステップを更に備え、
前記規定回数に達するまでの間、前記重心周りモーメント計算ステップと、前記重心加速度制約条件計算ステップと、前記第２の重心軌道生成ステップと、による処理を繰り返して実行させる、
請求項１または２に記載の脚式歩行ロボットの重心軌道生成方法。
【請求項４】
前記第１の重心軌道生成ステップおよび前記第２の重心軌道生成ステップでは、重心軌道の生成を、制約条件の下で最適解を求解する凸二次計画問題として扱う、
請求項１〜３いずれか１項に記載の脚式歩行ロボットの重心軌道生成方法。
【請求項５】
前記脚式歩行ロボットが、２脚歩行ロボットである
請求項１〜４いずれか１項に記載の脚式歩行ロボットの重心軌道生成方法。
【請求項６】
前記請求項１〜５のいずれか１項の脚式歩行ロボットの重心軌道生成方法を実行するコントローラを備える脚式歩行ロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は脚式歩行ロボットおよびその重心軌道生成方法に関し、特に、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成できる脚式歩行ロボットおよびその重心軌道生成方法に関する。
【背景技術】
【０００２】
ロボットの脚式歩行制御において、足裏と地面との滑りに着目した技術が提案されている。このような技術として、例えば、予め水平方向並進床反力や鉛直軸周り床反力モーメントが、ある範囲内に収まるように歩容・軌道を生成する、という技術が知られている。このような技術として、例えば特許文献１に開示される技術がある。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第４１９９２３６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１に開示される技術では、鉛直軸周り床反力モーメントの補償を、歩容生成・軌道生成とは別のこととして扱い、上半身の動作（例えば腕の振りや状態のひねり）によって抑制するという方針を採用している。
【０００５】
しかしながら、両腕で物体を運搬している状況（例えば、両腕でお盆を持って水を運んでいるという状況）では、床の摩擦が低い（鉛直軸周り床反力モーメントを低く制限する必要がある）からといって、腕を前後に振る、または上体を左右に振って補償動作を行う、ということはできない。また、腕の振りや上体のひねりによる補償では、例えば補償動作として上体を左にひねるとすると、いずれかのタイミングでひねった上体を正面に戻す必要があり、上体を戻す際に鉛直軸周りモーメントが不利な方向に発生することが避けられないという問題がある。加えて、補償量が大きな場合や上半身の質量が小さな場合には、腕の振りや上体のひねりを可動域限界まで行ったとしても補償しきれない、という事態が発生する可能性もある。
【０００６】
一方で、発明者は検討の結果、ＺＭＰの特性上、鉛直軸周りの床反力モーメントは、重心周りモーメントと、重心位置と、ＺＭＰ位置との関数になっており、必ずしも上半身の動きで全て補償する必要はないという事実を見出した。
【０００７】
本発明はこの事実に基づき、上述した問題点を解決するためになされたものであり、重心軌道生成時に鉛直軸周りの床反力モーメント制限を反映させることで、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成できる脚式歩行ロボットおよびその重心軌道生成方法を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明に係る脚式歩行ロボットの重心軌道生成方法は、モデル予測制御を適用して重心軌道を生成する脚式歩行ロボットの重心軌道生成方法であって、前記脚式歩行ロボットを単一の質点で表した単質点モデルを用いて、設定される目標ＺＭＰ軌道に基づくＺＭＰ制約条件を満たすように、重心軌道の生成を行う第１の重心軌道生成ステップと、前記第１の重心軌道生成ステップで生成された重心軌道と、前記脚式歩行ロボットを複数の質点で表した多質点モデルと、を用いて、前記脚式歩行ロボットの重心周りに発生する重心周りモーメントを計算する重心周りモーメント計算ステップと、前記脚式歩行ロボットの進行方向と、当該進行方向に直交する方向と、に平行する方向を水平方向とし、当該水平方向に直交する方向を鉛直方向として、前記脚式歩行ロボットの足裏が床に接触することで受ける床反力に関して、前記脚式歩行ロボットのＺＭＰを基準として前記鉛直方向に沿って配置される鉛直軸周りに発生する鉛直軸周り床反力モーメントと、前記脚式歩行ロボットのＺＭＰを基準として前記水平方向に発生する水平方向並進床反力と、についてそれぞれ制限値が設定され、当該設定される制限値と、前記重心周りモーメント計算ステップで計算された重心周りモーメントと、を用いて、床反力の制約条件を反映させるための重心加速度の制約条件を計算する重心加速度制約条件計算ステップと、前記単質点モデルを用いて、前記ＺＭＰ制約条件と、前記重心加速度制約条件計算ステップで計算された重心加速度の制約条件と、を満たすように、重心軌道の生成を行う第２の重心軌道生成ステップと、を備えるものである。これにより、重心軌道生成時に鉛直軸周りの床反力モーメント制限を反映させることで、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成することができる。
【０００９】
また、前記重心加速度制約条件計算ステップでは、前記鉛直軸周り床反力モーメントの制限値および前記水平方向並進床反力の制限値として、それぞれ予め定めた固定値が設定されるようにするとよい。
【００１０】
さらにまた、前記第１の重心軌道生成ステップによる重心軌道生成回数および前記第２の重心軌道生成ステップによる重心軌道生成回数の合計が、規定回数に達したか否かを判定する判定ステップを更に備え、前記規定回数に達するまでの間、前記重心周りモーメント計算ステップと、前記重心加速度制約条件計算ステップと、前記第２の重心軌道生成ステップと、による処理を繰り返して実行させるようにするとよい。
【００１１】
また、前記第１の重心軌道生成ステップおよび前記第２の重心軌道生成ステップでは、重心軌道の生成を、制約条件の下で最適解を求解する凸二次計画問題として扱うようにするとよい。
【００１２】
さらにまた、前記脚式歩行ロボットが、２脚歩行ロボットであるようにするとよい。
【００１３】
本発明に係る脚式歩行ロボットは、上述の脚式歩行ロボットの重心軌道生成方法を実行するコントローラを備えるものである。これにより、重心軌道生成時に鉛直軸周りの床反力モーメント制限を反映させることで、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成することができる。
【発明の効果】
【００１４】
本発明によれば、ロボットの上半身動作を用いずに、鉛直軸周りに滑ることを回避して安定して動作可能な重心軌道を生成できる脚式歩行ロボットおよびその重心軌道生成方法を提供することができる。
【図面の簡単な説明】
【００１５】
【図１】実施の形態１に係る脚式歩行ロボットの模式図である。
【図２】実施の形態１に係る重心軌道生成処理の概要を説明するための図である。
【図３】実施の形態１に係る力の定義を説明するための図である。
【図４】実施の形態１に係る線形倒立振子モデルを説明するための図である。
【図５】実施の形態１に係る重心周りモーメントを説明するための図である。
【図６】実施の形態１に係る各サンプリング点での入出力と状態変数を説明するための図である。
【図７】実施の形態１に係るReceding Horizon制御の考え方を説明するための図である。
【図８】実施の形態１に係る支持多角形を説明するための図である。
【図９】実施の形態１に係る多質点モデルを説明するための図である。
【図１０】実施の形態１に係る初期化処理を示すフローチャートである。
【図１１】実施の形態１に係る周期実行処理を示すフローチャートである。
【発明を実施するための形態】
【００１６】
以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一又は対応する要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略される。
【００１７】
＜発明の実施の形態１＞
図１は、本実施の形態に係る脚式歩行ロボットとしての２脚歩行ロボット１０の模式的な構成例を示している。以下では、２脚歩行ロボット１０を単にロボット１０と称する。ロボット１０は、関節角を制御するアクチュエータと、関節角を検出するエンコーダと、が脚の各関節に内蔵されている。また、ロボット１０は、コントローラ２０を備えている。コントローラ２０が、歩容データに基づいて脚の各関節を制御する。すなわち、コントローラ２０は、歩容データから得られる各関節の目標角度に追従するように、アクチュエータを制御する。より具体的には、コントローラ２０は、歩容データ（足の目標軌道と重心の目標軌道）を用いて各関節の目標角の時系列データを算出し、算出した目標角に追従するように、各関節を制御する。歩容データを用いて関節目標角を生成する手法については、従来から知られている手法を用いればよいため、ここではその詳細な説明を省略する。
【００１８】
なお、コントローラ２０は、主要なハードウェア構成として、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される制御プログラムと、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを用いて構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバスによって相互に接続されている。
【００１９】
図２は、コントローラ２０が実行する重心軌道生成処理の概要を示すフローチャートである。コントローラ２０は、まず、モデル予測制御に基づいて、ロボット１０の重心軌道の生成を行う（Ｓ２０１）。コントローラ２０は、単質点モデルを用いて、設定される目標ＺＭＰ軌道に基づくＺＭＰ制約条件を満たすように重心軌道の生成を行うが、ここでの重心軌道の生成は床反力の制約条件（制限）を設けずに行う。
【００２０】
次いで、コントローラ２０は、Ｓ２０１で生成された重心軌道と、ロボット１０のフルモデルと、を用いて、Ｒｏｌｌ・Ｐｉｔｃｈ・Ｙａｗ軸周りの重心周りモーメントを計算する（Ｓ２０２）。
【００２１】
次いで、コントローラ２０は、Ｓ２０２で計算された重心周りモーメントと、ロボット１０の足裏が床に接触することで受ける床反力に関する制限値としての、設定される鉛直軸（Ｙａｗ軸）周り床反力モーメント制限値と、設定される水平方向並進床反力の制限値と、を用いて、床反力の制約条件（制限）を反映させるための重心加速度の制約条件を計算する（Ｓ２０３）。
【００２２】
最後に、コントローラ２０は、再度モデル予測制御を用いて、ロボット１０の重心軌道の再生成を行う（Ｓ２０４）。コントローラ２０は、単質点モデルを用いて、ＺＭＰ制約条件と、計算された重心加速度の制約条件と、を満たすように重心軌道の生成を行うが、ここでの重心軌道の再生成は計算した重心加速度の制約条件を加えて行う。
【００２３】
これにより、水平方向並進床反力および鉛直軸周り床反力モーメントの制限値を満たす軌道を生成することができる。この軌道生成処理で特筆すべき点は、軌道の生成処理自体は重心軌道の生成を行う処理であり、かつ、上半身動作による鉛直軸周り床反力モーメントの補償を全く行っていないことである。
【００２４】
以下、Ｓ２０１〜Ｓ２０４での処理を具体的に説明する。なお、説明の都合上、まずＳ２０１およびＳ２０４で用いるモデル予測制御を説明し、続いてＳ２０２、Ｓ２０３の順で説明を行う。また、本実施の形態に係る重心軌道生成処理の初期化ルーチン処理および周期実行ルーチン処理の詳細ついては後述する。
【００２５】
（Ｓ２０１、Ｓ２０４：モデル予測制御の説明）
まず、図３を用いて、軌道生成処理に用いる力の定義を説明する。図３左図に示すように、ロボット１０の進行方向をｘ軸方向、ｘ軸方向に直交する方向をｙ軸方向とし、ｘ軸およびｙ軸と直交する軸をｚ軸方向とする。また、図３右図にロボットの足下部分を拡大して示す。図３右図に示すように、ロボット１０の足裏が床に接触することで受ける反力に関して、ロボット１０のＺＭＰを基準として、水平方向並進床反力をそれぞれＦｘ、Ｆｙとし、鉛直方向並進床反力をＦｚとし、鉛直軸周り床反力モーメントをＭｚとする。また、ＺＭＰの位置は、ｘｚｍｐ，ｙｚｍｐ，ｚｚｍｐとする。
【００２６】
Ｓ２０１およびＳ２０４の軌道生成処理では、ロボット１０を単一の質点として表現した単質点モデルを用いる。ロボット１０の重心位置を、ｘｇ，ｙｇ，ｚｇと定義する。ロボット１０の全質量をｍとし、重力加速度をｇとする。ロボット１０の重心モデルのＺＭＰ方程式は、以下の式（１）により示される。式（１）において、Ｉｘ、およびＩｙは、重心周りモーメントを示す（図５参照。）。重心周りモーメントの詳細は後述する。なお、以下の式において、変数の上部に配置される"・"は一階微分を示し、"・・"は二階微分を示している。
【数１】
0005807591.tif 000002
【００２７】
次に、線形倒立振子モデルを用いた近似を行う。この線形倒立振子モデルを用いた近似はよく知られている。式（２）に示すように、床面からの重心高さｚｇを一定値（ｃｏｎｓｔ）にし（図４を参照。）、重心周りモーメントＩｘ、Ｉｙを０に設定する。
【数２】
0005807591.tif 000003
【００２８】
すると、上述した式（１）は、線形化された簡単な式として示すことができる。この線形化された式を、以下の式（３）に示す。
【数３】
0005807591.tif 000004
【００２９】
次に、ｘ軸方向に関して、システムへの入力と出力を以下の式（４）に示すように定義する。
【数４】
0005807591.tif 000005
【００３０】
さらに、上述した式（３）を線形の連続システムとして記述する。この連続システムを、以下の式（５）に示す。
【数５】
0005807591.tif 000006
【００３１】
上述した式（５）における行列ｘ（ｔ）、行列Ａ、行列ｂ、行列ｃｐのチルダ、行列ｃａのチルダは、以下の式（６）に示される。
【数６】
0005807591.tif 000007
【００３２】
上述した式（５）を離散時間間隔Δｔで離散化する。この離散化の様子を図６に示す。図６は、離散化した各サンプリング点における入出力と状態変数の関係を示す図である。得られる離散システムを、以下の式（７）に示す。
【数７】
0005807591.tif 000008
【００３３】
上述した式（７）における行列ｘｋ、行列Ａ、行列ｂ、行列ｃｐ、行列ｃａは、以下の式（８）に示される。
【数８】
0005807591.tif 000009
【００３４】
本実施の形態に係るモデル予測制御は、Receding Horizon Controlとも呼ばれる。図７を用いて、このReceding Horizon Controlの考え方を簡単に説明する。図７に示すように、現在においてＮΔｔ未来までの期間のモデル挙動を予測して最適化計算を行い、現在の入力値ｕｋを決定する。Δｔ秒後にはまた同様の計算を行いΔｔ秒後の入力値ｕｋ＋１を決定する。このような計算過程を一定周期毎に行い、結果として最適な軌道を生成することができる。
【００３５】
ＮΔｔ未来までの期間の予測に関しては、以下の式（９）に示すように、離散システムを未来に向かって積算していくことで得ることができる。式（９）において、現在から未来までの各サンプリング点における入力値をｕｘｋ，ｕｘｋ＋１，ｕｘｋ＋２，・・・，ｕｘｋ＋Ｎとし、現在から未来までの各サンプリング点における状態変数値をｘｋ，ｘｋ＋１，ｘｋ＋２，・・・，ｘｋ＋Ｎとする。また、出力に関しても同様に、ｐｘｋ，ｐｘｋ＋１，ｐｘｋ＋２，・・・，ｐｘｋ＋Ｎ、ａｘｋ，ａｘｋ＋１，ａｘｋ＋２，・・・，ａｘｋ＋Ｎとする。これらの入出力は、図６に示したとおりである。なお、式（９）では、現在のタイミングを、ｋサンプル目としている。
【数９】
0005807591.tif 000010
【００３６】
また、入力値列ｕｘｋ、出力値列ｐｘｋ、ａｘｋを用いて、以下の式（１０）に示す行列ｕｘｋ、ｖｘｋ、ｗｘｋを定義する。
【数１０】
0005807591.tif 000011
【００３７】
上述した式（７）、（９）、（１０）を用いて、未来の出力値列ｖｘｋおよびｗｘｋを計算する。すると、未来の出力値列ｖｘｋおよびｗｘｋは、それぞれ以下の式（１１）および（１２）に示される。
【数１１】
0005807591.tif 000012
【数１２】
0005807591.tif 000013
【００３８】
ここで、ＮΔｔ未来までの目標ＺＭＰ列をｖｘｋ＋１のハットとし、以下の式（１３）により定める。ここで、目標ＺＭＰ軌道（ｐｘｋ＋１のハットの列）は、予め適切な値が設定されるものとする。
【数１３】
0005807591.tif 000014
【００３９】
モデル予測制御型軌道生成方法では、最適化の評価関数を、例えば以下の式（１４）に示す形で設定する。式（１４）に示す評価関数は、予測区間内の目標ＺＭＰとＺＭＰの誤差の二乗と、入力値の二乗と、を足し合わせた値を最小化する形で示される。
【数１４】
0005807591.tif 000015
【００４０】
上述した式（１４）に示した評価関数はモデル予測制御の一般的な与え方である。本実施の形態では、式（１４）に関して、第１項の役割は、ＺＭＰを目標ＺＭＰに近づけることであり、第２項の役割は、入力値が過剰に働くのを抑止すること（重心が発散するのを抑制すること）であると考える。また、式（１４）に示した行列ＱおよびＲは、上記第１項および第２項をバランスするための重み行列であり、以下の式（１５）により示される。これらの重み行列の値は、制御対象に応じて予め適切な値が設定される。
【数１５】
0005807591.tif 000016
【００４１】
上述した式（１４）に式（１１）を代入することで、以下に示す式（１６）を得る。
【数１６】
0005807591.tif 000017
【００４２】
上述した式（１６）における行列Ｈｃｋ、ベクトルｆｘｋは、以下の式（１７）に示される。
【数１７】
0005807591.tif 000018
【００４３】
従って、上述した式（１４）の評価関数は、以下の式（１８）により示す形で定式化することができる。
【数１８】
0005807591.tif 000019
【００４４】
上述した式の展開はｙ軸方向に関しても同様に成り立つ。このため、ｙ軸方向に関する評価関数は、以下の式（１９）により示される。
【数１９】
0005807591.tif 000020
【００４５】
ここで、ベクトルｕｋ、行列Ｈｋ、ベクトルｆｋを、以下の式（２０）に示すように定める。
【数２０】
0005807591.tif 000021
【００４６】
すると、上述した式（１８）および（１９）をまとめることで、以下に示す式（２１）を得ることができる。すなわち、式（２１）は、本実施の形態に係るモデル予測制御で用いる、ｘ軸方向とｙ軸方向の両方に関して考慮した最適化の評価関数となる。
【数２１】
0005807591.tif 000022
【００４７】
次に、評価関数に対する制約条件を説明する。本実施の形態では、ＺＭＰ制約条件と重心加速度制約条件を、制約条件として評価関数に設定する。ただし、Ｓ２０１での軌道生成処理では、ＺＭＰ制約条件のみを設定して軌道生成処理を行い、Ｓ２０４での軌道生成処理では、ＺＭＰ制約条件に加えて重心加速度制約条件を設定して軌道生成処理を行う。
【００４８】
まず、ＺＭＰ制約条件を説明する。ロボット１０の重心軌道を生成する場合、ロボット１０の足裏の範囲内（より正確には、足裏外縁から一定のマージンを設けて配置される多角形内）にＺＭＰが留まるような軌道を生成することで、安定な歩行を実現できる。このような多角形は、一般的に支持多角形と呼ばれる。
【００４９】
図８に、単脚支持期および両脚支持期における支持多角形を例示する。図８左図に示すように、左足平３０Ｌと右足平３０Ｒに設けられたＺＭＰ設定可能域４０に関して、左足支持期では左足平３０Ｌに支持多角形が構成される。また、両足支持期では、図８右図に示すように支持多角形５０が構成される。
【００５０】
その名前からも分かるように、支持多角形は単脚支持期や両脚支持期などの状況によって形状は変化するものの、基本的には多角形で表される。すなわち、支持多角形内にＺＭＰが留まる、という条件は、任意のベクトルλｘ、λｙ、ｄｚを用いて、以下の式（２２）により示すことができる。これらのベクトルλｘ、λｙ、ｄｚの値は、予め適切な値が設定されるものとする。
【数２２】
0005807591.tif 000023
【００５１】
従って、未来の各サンプリング点ｋ＋ｊ（ｊ＝１，・・・，Ｎ）において上述した式（２２）に示したＺＭＰ制約を仮定することで、以下に示す式（２３）を得ることができる。
【数２３】
0005807591.tif 000024
【００５２】
さらに、上述した式（２３）を展開することで、以下に示す式（２４）を得る。
【数２４】
0005807591.tif 000025
【００５３】
そして、上述した式（２４）をさらに整理することで、以下に示す式（２５）を得る。
【数２５】
0005807591.tif 000026
【００５４】
上述した式（２５）において、ベクトルｖｋ＋１およびベクトルηｚは、以下の式（２６）に示される。
【数２６】
0005807591.tif 000027
【００５５】
また、上述した式（２５）において、行列Γｚは、以下の式（２７）に示される。
【数２７】
0005807591.tif 000028
【００５６】
さらに、上述した式（２５）に対して上述した式（１１）を代入することで、以下の式（２８）に示す線形不等式を導くことができる。すなわち、ＺＭＰを支持多角形内に留めるというＺＭＰ制約条件に関して、最終的には、入力値列ベクトルｕｋに関する線形不等式により記述できることを示している。
【数２８】
0005807591.tif 000029
【００５７】
なお、上述した式（２８）では、以下の式（２９）に示すように、行列Ｐｚｋ、行列ｑｚｋを定めた。
【数２９】
0005807591.tif 000030
【００５８】
次に、重心加速度制約条件を説明する。ｘｙ方向の重心加速度が、上述した式（２２）に示したような線形な関係で記述されているとする。すなわち、以下の式（３０）に示すような線形関係で記述されているとする。
【数３０】
0005807591.tif 000031
【００５９】
すると、以降は上述したＺＭＰ制約条件に関する式の導出と全く同様の式変形を行うことで、最終的に、重心の加速度制約条件を、以下の式（３１）に示すようなｕｋに関する線形不等式で記述することができる。
【数３１】
0005807591.tif 000032
【００６０】
以上に説明したようにして、ＺＭＰ制約条件と重心加速度制約条件を導入することができた。Ｓ２０１では、ＺＭＰ制約条件のみを設定し、重心加速度制約条件を設定しないため、制約条件Ｐｋおよびｑｋを、以下の式（３２）に示すように定める。
【数３２】
0005807591.tif 000033
【００６１】
Ｓ２０４では、ＺＭＰ制約条件に加えて、重心加速度制約条件を設定する。このため、制約条件Ｐｋおよびｑｋを、以下の式（３３）に示すように定める。
【数３３】
0005807591.tif 000034
【００６２】
上述した式（２８）、（３１）、（３２）、（３３）から、制約条件はいずれの場合であっても、以下の式（３４）により記述することができる。
式（２１）
【数３４】
0005807591.tif 000035
【００６３】
以上をまとめると、上述した式（２１）に示した評価関数と、上述した式（３４）に示した制約条件と、を用いることで、以下の式（３５）に示す最適化問題を構成することができる。式（３５）に示すように、最適化問題は凸二次計画問題として定式化することができる。この凸二次計画問題は高速かつ安定に求解することが可能である。モデル予測制御では、現在状態を入力して最適化問題を求解するまでの計算処理を時間を進めながら毎周期実行していき、毎周期ごとに入力値を決めていく。
【数３５】
0005807591.tif 000036
【００６４】
（Ｓ２０２：フルモデルによる重心周りモーメントの計算の説明）
続いて、重心周りモーメントの計算を説明する。
上述したモデル予測制御による重心軌道の計算では、ロボット１０を単一の質点として表現した単質点モデルを用いて計算するが、複数の質点を持つロボット１０が実際に運動をする場合には、単質点モデルでは表現されない重心周りのモーメントが発生する（図５参照）。この重心周りのモーメントは、重心周りに発生する角運動量の微分、もしくは、重心周りに発生する角運動量の時間変化率などとも呼ばれ、いずれも同じ値を意味している。
【００６５】
本実施の形態では、図９に例示するフルモデルとしての多質点モデルを用いて、ロボット１０が表現されるものとする。なお、Ｍ個の質点それぞれに順番に番号が付与され、各質点が、質量ｍｉと、位置ｒｉ＝［ｘｉ  ｙｉ  ｚｉ］Ｔのパラメータを持つものとする。この時、ロボット１０全体の重心の位置ｒｃｏｇは、以下の式（３６）により示される。ロボット１０の各質点の質量と位置の設定は、ユーザ、あるいはコントローラ２０に指令を与える上位コントローラによって設定すればよい。
【数３６】
0005807591.tif 000037
【００６６】
すると、重心周りモーメントＩは、以下の式（３７）により計算することができる。
【数３７】
0005807591.tif 000038
【００６７】
なお、上述した式（３７）において、行列Ｉは以下の式（３８）により示される。
【数３８】
0005807591.tif 000039
【００６８】
上述した式（３７）から分かるように、重心周りモーメントは、重心と各質点の相対位置・加速度を外積計算することで各質点が重心周りに発生するモーメント量を計算し、さらに、この計算量を足し合わせる形になっている、と解釈することができる。
【００６９】
本実施の形態では、生成した重心軌道と、ロボット１０の足先軌道と、に基づいて、未来のＮ個の各サンプリング点におけるロボット１０の姿勢を計算し、その姿勢から各質点の位置および加速度を計算する。そして、この計算した各質点の位置および加速度と、式（３６）を用いて計算するロボット１０の全体重心位置およびその加速度と、を式（３６）に代入する。すなわち、未来のＮ個の各サンプリング点において上述した式（３７）を計算することで、各サンプリング点における重心周りモーメントＩｘｋ＋ｊ、Ｉｙｋ＋ｊ、Ｉｚｋ＋ｊ（ｊ＝１，・・・，Ｎ）を計算する。
【００７０】
なお、重心軌道を再び更新する際には、この重心周りモーメントの計算値を用いて、ｘ、ｙ軸の重心周りモーメントＩｘｋ＋ｊ、Ｉｙｋ＋ｊによって発生するＺＭＰ偏差を計算し、目標ＺＭＰ軌道を修正するものとしてもよい（この手法は、例えば特開２００４−１１４２４３号公報にて詳細に提案されている。）。これによって、ＺＭＰのずれをより効果的に抑制することができ、より安定した動作を実現することができる。
【００７１】
（Ｓ２０３：床反力制約を満たすための重心加速度制約条件の計算の説明）
続いて、重心加速度制約条件の計算を説明する。
床反力制約を満たすための重心加速度制約条件は、上述した式（３７）により計算される重心周りモーメントと、鉛直軸（Ｙａｗ軸）周り床反力モーメント制限値と、水平方向並進床反力の制限値と、を用いて計算される。以下、鉛直軸（Ｙａｗ軸）周り床反力モーメント制限値と、水平方向並進床反力の制限値と、を説明する。
【００７２】
まず、鉛直軸（Ｙａｗ軸）周り床反力モーメント制限値を説明する。
上述した式（１）の１番目の式にｙ（・・）ｇ（ｙｇの二階微分）を掛け、２番目の式にｘ（・・）ｇ（ｘｇの二階微分）を掛け、１番目の式から２番目の式を加減することで、以下に示す式（３９）を得る。
【数３９】
0005807591.tif 000040
【００７３】
また、鉛直軸周り床反力モーメントＭｚのつり合い式は、以下の式（４０）により示される。
【数４０】
0005807591.tif 000041
【００７４】
上述した式（３９）および（４０）から、以下の式（４１）を得る。
【数４１】
0005807591.tif 000042
【００７５】
鉛直軸周り床反力モーメントＭｚに関して、床面に合わせてロボット１０の足が滑らないように上限Ｍｍａｘｚおよび下限Ｍｍｉｎｚを設定すると、この条件は以下の式（４２）により示される。なお、本実施の形態では、鉛直軸周り床反力モーメントの上下限制限値は、固定値として設定される。鉛直軸周り床反力モーメントの上下限制限値の設定は、ユーザ、あるいはコントローラ２０に指令を与える上位コントローラによって設定すればよい。
【数４２】
0005807591.tif 000043
【００７６】
従って、上述した式（４１）および（４２）から、以下の式（４３）を得る。このようにして、鉛直軸周り床反力モーメントの上下限から、重心水平方向加速度の制約条件を導出することができる。
【数４３】
0005807591.tif 000044
【００７７】
次に、水平方向並進床反力の制限値を説明する。並進床反力の制限についても、床面に合わせてロボット１０の足が滑らないように上限Ｆｍａｘｘ、Ｆｍａｘｙおよび下限Ｆｍｉｎｘ、Ｆｍｉｎｙを設定すると、この条件は以下の式（４４）により示される。そして、式（４４）を式（４５）へと変換することで、上述した重心水平方向加速度の制約条件へと変換することができる。なお、本実施の形態では、水平方向並進床反力の上下限制限値は、固定値として設定される。水平方向並進床反力の上下限制限値の設定は、ユーザ、あるいはコントローラ２０に指令を与える上位コントローラによって設定すればよい。
【数４４】
0005807591.tif 000045
【数４５】
0005807591.tif 000046
【００７８】
未来のＮ個の各サンプリング点において、鉛直軸周り床反力モーメントの上下限値を用いた上述した式（４３）と、水平方向並進床反力の制限値を用いた上述した（４５）と、を計算することで、各サンプリング点における重心水平方向加速度の制約条件を計算することができる。
【００７９】
すなわち、任意のサンプリング点ｋ＋ｊにおける重心水平方向加速度の制約条件は、鉛直軸周り床反力モーメントの制約条件が以下に示す式（４６）により示される。ただし、ｚｇ[ｋ＋ｊ]のツードットは、未来のサンプリング点ｋ＋ｊにおける鉛直方向重心加速度である。
【数４６】
0005807591.tif 000047
【００８０】
また、水平方向並進床反力の制約条件が以下に示す式（４７）により示される。
【数４７】
0005807591.tif 000048
【００８１】
そして、上述した式（４６）および（４７）をまとめることで、以下に示す式（４８）を得る。
【数４８】
0005807591.tif 000049
【００８２】
さらに、上述した式（４８）をよりまとめることで、以下に示す式（４９）を得る。
【数４９】
0005807591.tif 000050
【００８３】
さらに、上述した式（４９）を、上述した式（２５）〜（２８）にかけて変換の流れと同様にして変換する。まず、上述した式（４９）を、以下の式（５０）に変換する。
【数５０】
0005807591.tif 000051
【００８４】
そして、行列Γａを以下の式（５１）に示すように定める。
【数５１】
0005807591.tif 000052
【００８５】
また、ベクトルηａを以下の式（５２）に示すように定める。
【数５２】
0005807591.tif 000053
【００８６】
すると、上述した式（５０）、（５１）、（５２）から、以下の式（５３）を得る。
【数５３】
0005807591.tif 000054
【００８７】
さらに、上述した式（５３）を変換することで、最終的に、以下の式（５４）に示す入力の制約条件式を導出することができる。この導出された制約条件に関して、本実施の形態と従来技術との間での一番の相違点は、鉛直軸周り床反力モーメントの制約を重心の加速度制約に変換し、軌道生成時に反映させるように構成したことである。
【数５４】
0005807591.tif 000055
【００８８】
上述した式（５４）に示した重心の加速度制約条件を、式（３４）に示したＺＭＰ制約条件と合わせて制約条件式として設定し、この制約条件の下で、式（３５）に示した凸二次計画問題を求解する。これにより、地面に対して足裏が滑ることを回避して安定して動作可能な重心軌道を生成することが可能となる。
【００８９】
（重心軌道生成処理の初期化ルーチン処理および周期実行ルーチン処理の説明）
以下、本実施の形態に係る重心軌道生成処理の初期化ルーチン処理および周期実行ルーチン処理の詳細を図１０及び図１１を用いて説明する。
【００９０】
（重心軌道生成処理の初期化ルーチン処理）
図１０は、コントローラ２０が実行する重心軌道生成処理の初期化ルーチン処理のフローチャートを示す。初期化ルーチンは、後述する図１１に示す周期実行ルーチン開始前の初期化時に実行される。
【００９１】
コントローラ２０は、サンプリング時間間隔である周期Δｔと、基準重心高さｈと、を設定する（Ｓ１００１）。コントローラ２０は、システム行列およびベクトルを計算する（Ｓ１００２）。より具体的には、上述した式（８）に示した行列Ａ、ｂ、ベクトルｃｐ、ｃａを計算する。コントローラ２０は、上述した式（１５）に示した重み行列Ｑ、Ｒを設定する（Ｓ１００３）。コントローラ２０は、上述した式（１１）および（１２）に示した出力値系列の係数（Ｇｐ１、Ｇｐ２、Ｇａ１、Ｇａ２）を計算する（Ｓ１００４）。
【００９２】
（重心軌道生成処理の周期実行ルーチン処理の説明）
図１１は、コントローラ２０が実行する重心軌道生成処理の周期実行ルーチン処理のフローチャートを示す。コントローラ２０は、ロボット１０の現在状態（重心位置ｘｋ、ｙｋ、速度、加速度）を設定する（Ｓ１１０１）。ロボット１０の現在状態は、ロボット１０自らが現在の重心位置・速度・加速度を取得して設定することができる。また、現在状態は、ユーザ、あるいはコントローラ２０に指令を与える上位コントローラによって設定するものとしてもよい。
【００９３】
コントローラ２０は、目標ＺＭＰ軌道（ｖｘｋのハット、ｖｙｋのハット）を設定する（Ｓ１１０２）。コントローラ２０は、上述した式（２０）に示したＨｋ、ｆｋを用いて、上述した式（２１）に示した評価関数を設定する（Ｓ１１０３）。
【００９４】
コントローラ２０は、上述した式（２９）、（２６）、（２７）に示したＺＭＰ制約条件（Ｐｚｋ、ｑｚｋ）を計算する（Ｓ１１０４）。コントローラ２０は、上述した式（３４）に示した不等式制約条件に関して、Ｓ２００４で計算した値を、上述した式（３２）に示すようにして不等式制約として設定する（Ｓ２００５）。すなわち、ＺＭＰ制約条件のみを不等式制約条件に設定する。
【００９５】
コントローラ２０は、上述した式（３５）に示した凸二次計画問題を求解する（Ｓ１１０６）。コントローラ２０は、凸二次計画問題の求解結果ｕｋを用いて、重視軌道を更新する（Ｓ１１０７）。すなわち、現在の時刻から未来のＮ個のサンプリング点における各重心位置を計算する。ｊは１〜Ｎまでの値を取る整数である。
【００９６】
コントローラ２０は、重心軌道のＳ１１０７での更新回数が規定回数に到達したか否か判定する（Ｓ２００８）。Ｓ１１０９で計算される重心周りモーメントの計算値は、Ｓ１１０７において１回目に生成した重心軌道に基づいて生成される。このため、厳密に言えば、重心軌道を再更新すると重心周りモーメントの計算値も変化してしまう。しかし、重心軌道更新のプロセスを再度繰り返すことで、この変化による影響を無視することができる。そこで、本実施の形態では、予め設定される規定回数に到達するまでの間、重心軌道の更新を行うことで、重心周りモーメントの計算値の変化を収束させる。なお、鉛直軸周り床反力モーメントの上下限値に多少余裕を持たせておくことで、この変化による影響を無視することもできる。
【００９７】
コントローラ２０は、上述した式（３７）を用いて、生成された重心軌道に基づく重心周りモーメントを計算する（Ｓ２００９）。より具体的に説明すると、コントローラ２０は、まず、Ｓ２００７で生成（あるいは更新）した重心軌道と、ロボット１０の足先軌道と、を用いて未来のＮ個の各サンプリング時刻点におけるロボット１０の姿勢を計算する。足先軌道の生成は、例えば、多項式補間を用いて生成すればよい。そして、コントローラ２０は、計算した各姿勢から、図９に例示した各質点の位置を計算し、この各質点の位置の変化から各質点の加速度を計算する。コントローラ２０は、この計算した各質点の位置および加速度と、式（３６）を用いて計算するロボット１０の全体重心位置およびその加速度と、を式（３７）に入力し、重心周りモーメントを計算する。なお、足先軌道の生成手法については、従来から知られている手法を用いればよいため、ここではその詳細な説明を省略する。また、ロボット１０の全体重心位置の計算方法については、式（３６）を用いて計算してもよいし、Ｓ１１０７で生成（あるいは更新）した重心軌道をそのまま用いるものとしてもよい。
【００９８】
コントローラ２０は、床反力制限値（鉛直軸周り床反力モーメントの上下限制限値および水平方向並進床反力の上下限制限値）を設定する（Ｓ１１１０）。コントローラ２０は、上述した式（４８）〜（５４）に示した加速度制約条件（Ｐａｋ、ｑａｋ）を計算する（Ｓ２０１１）。コントローラ２０は、上述した式（３４）に示した不等式制約条件に関して、Ｓ１１１１で計算した値を、上述した式（３３）に示すようにして不等式制約として設定する（Ｓ１１１２）。すなわち、ＺＭＰ制約条件に加えて重心加速度制約条件を不等式制約条件に設定する。
【００９９】
コントローラ２０は、時間Δｔ後から開始する次の周期での軌道生成処理を行うために、周期更新を行う（Ｓ１１１３）。すなわち、ｋを１増加させ、ｔをΔｔ増加させる。
【０１００】
なお、図２で示したＳ２０１の内容が図１１に示したＳ１１０５およびＳ１１０６での処理に相当し、Ｓ２０２の内容がＳ１１０９での処理に相当し、Ｓ２０３の内容がＳ１１１０およびＳ１１１１での処理に相当し、Ｓ２０４の内容がＳ１１１２および再び実行するＳ１１０６での処理に相当する。
【０１０１】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述した実施の形態では、鉛直軸周り床反力モーメントの上下限制限値および水平方向並進床反力の水平方向床反力の上下限制限値を固定値とする場合を例に説明したいが、本発明はこれに限定されず、地面に応じてオンラインに変更する、時間経過に応じて変化させる、などのように状況に応じて変更させる構成としてもよい。
【符号の説明】
【０１０２】
１０  脚式歩行ロボット、
２０  コントローラ、
３０Ｒ  右足平、
３０Ｌ  左足平、
４０  ＺＭＰ設定可能域、
５０  支持多角形、
【図１】
0005807591.tif 000056
【図２】
0005807591.tif 000057
【図３】
0005807591.tif 000058
【図４】
0005807591.tif 000059
【図５】
0005807591.tif 000060
【図６】
0005807591.tif 000061
【図７】
0005807591.tif 000062
【図８】
0005807591.tif 000063
【図９】
0005807591.tif 000064
【図１０】
0005807591.tif 000065
【図１１】
0005807591.tif 000066