(57)【特許請求の範囲】
【請求項１】
脚式ロボットの重心軌道を生成する重心軌道生成装置であって、
前記脚式ロボットにおける非線形のＺＭＰ方程式に基づいて、離散システムを生成するシステム生成手段と、
前記脚式ロボットの目標ＺＭＰを設定するＺＭＰ設定手段と、
前記ＺＭＰ設定手段により設定された前記目標ＺＭＰと、前記システム生成手段により生成された離散システムに基づいて算出される未来のＺＭＰと、の偏差、及び、入力値に基づいた評価関数を設定する評価関数設定手段と、
前記脚式ロボットのＺＭＰに対する制約条件を設定する制約条件設定手段と、
前記制約条件設定手段により設定された前記制約条件下で、前記評価関数設定手段により設定された前記評価関数の最適化計算を行って前記入力値を決定するプロセスを、所定周期毎に繰り返すことで、前記重心軌道を生成するモデル予測手段と、
を備え、
前記モデル予測手段は、予測区間において、前記脚式ロボットの重心高さ軌道に応じて重心高さを変化させて、前記重心軌道を生成し、
前記システム生成手段は、前記非線形のＺＭＰ方程式を線形化することなく前記離散システムを生成し定式化を行い、
前記モデル予測手段は、前記評価関数設定手段により設定された評価関数と、前記制約条件設定手段により設定された制約条件に基づいて定式化された凸２次計画問題の最適解を算出し、該最適解の第１要素を前記入力値として決定する、
ことを特徴とする重心軌道生成装置。
【請求項２】
請求項１記載の重心軌道生成装置であって、
前記評価関数設定手段は、予測区間内における前記目標ＺＭＰと前記未来のＺＭＰとの偏差の２乗と、前記入力値の２乗とを加算した値を最小化するような前記評価関数を設定する、ことを特徴とする重心軌道生成装置。
【請求項３】
請求項１又は２記載の重心軌道生成装置であって、
前記制約条件設定手段は、ＺＭＰが支持多角形から外れないような前記制約条件を設定する、ことを特徴とする重心軌道生成装置。
【請求項４】
脚式ロボットの重心軌道を生成する重心軌道生成装置の生成方法であって、
前記脚式ロボットにおける非線形のＺＭＰ方程式に基づいて、離散システムを生成するステップと、
前記脚式ロボットの目標ＺＭＰを設定するステップと、
前記設定された前記目標ＺＭＰと、前記生成された離散システムに基づいて算出される未来のＺＭＰと、の偏差、及び、入力値に基づいた評価関数を設定するステップと、
前記脚式ロボットのＺＭＰに対する制約条件を設定するステップと、
前記設定された制約条件下で、前記設定された評価関数の最適化計算を行って前記入力値を決定するプロセスを、所定周期毎に繰り返すことで、前記重心軌道を生成するステップと、
を含み、
予測区間において、前記脚式ロボットの重心高さ軌道に応じて重心高さを変化させて、前記重心軌道を生成し、
前記非線形のＺＭＰ方程式を線形化することなく前記離散システムを生成し定式化を行い、
前記設定された評価関数と、前記設定された制約条件に基づいて定式化された凸２次計画問題の最適解を算出し、該最適解の第１要素を前記入力値として決定する、
ことを特徴とする重心軌道生成装置の生成方法。
【請求項５】
脚式ロボットの重心軌道を生成する重心軌道生成装置のプログラムであって、
前記脚式ロボットにおける非線形のＺＭＰ方程式に基づいて、離散システムを生成する処理と、
前記脚式ロボットの目標ＺＭＰを設定する処理と、
前記設定された前記目標ＺＭＰと、前記生成された離散システムに基づいて算出される未来のＺＭＰと、の偏差、及び、入力値に基づいた評価関数を設定する処理と、
前記脚式ロボットのＺＭＰに対する制約条件を設定する処理と、
前記設定された制約条件下で、前記設定された評価関数の最適化計算を行って前記入力値を決定するプロセスを、所定周期毎に繰り返すことで、前記重心軌道を生成する処理と、
をコンピュータに実行させ、
予測区間において、前記脚式ロボットの重心高さ軌道に応じて重心高さを変化させて、前記重心軌道を生成し、
前記非線形のＺＭＰ方程式を線形化することなく前記離散システムを生成し定式化を行い、
前記設定された評価関数と、前記設定された制約条件に基づいて定式化された凸２次計画問題の最適解を算出し、該最適解の第１要素を前記入力値として決定する、
ことを特徴とする重心軌道生成装置のプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、安定的な歩行が可能な脚式ロボットの重心軌道を生成できる重心軌道生成装置、その生成方法及びプログラムに関するものである。
【背景技術】
【０００２】
脚式ロボットの歩行制御において、一般的に、ＺＭＰ（Zero Moment Point）方程式に基づいて、重心軌道を生成することで、安定歩行を実現している。ここで、ＺＭＰ方程式は非線形である為、重心高さを一定と仮定しＺＭＰ方程式を線形化した線形倒立振子モデルを用いて重心軌道を生成することが一般に行われている。
【０００３】
例えば、初回の予見制御による重心軌道生成後にモデル誤差に起因するＺＭＰ偏差を計算し、そのＺＭＰ偏差に基づいて再度予見制御による重心軌道の生成を行う方法が知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００４−１１４２４３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上記特許文献１に示す方法においては、近似的な計算を用いているため、ＺＭＰ偏差が大きいときは、そのＺＭＰ偏差を除去しきれず、歩行が不安定化する虞がある。
【０００６】
本発明は、このような問題点を解決するためになされたものであり、安定的な歩行が可能な重心軌道を生成できる重心軌道生成装置、その生成方法及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するための本発明の一態様は、脚式ロボットの重心軌道を生成する重心軌道生成装置であって、前記脚式ロボットにおける非線形のＺＭＰ方程式に基づいて、離散システムを生成するシステム生成手段と、前記脚式ロボットの目標ＺＭＰを設定するＺＭＰ設定手段と、前記ＺＭＰ設定手段により設定された前記目標ＺＭＰと、前記システム生成手段により生成された離散システムに基づいて算出される未来のＺＭＰと、の偏差、及び、入力値に基づいた評価関数を設定する評価関数設定手段と、前記脚式ロボットのＺＭＰに対する制約条件を設定する制約条件設定手段と、前記制約条件設定手段により設定された前記制約条件下で、前記評価関数設定手段により設定された前記評価関数の最適化計算を行って前記入力値を決定するプロセスを、所定周期毎に繰り返すことで、前記重心軌道を生成するモデル予測手段と、を備える、ことを特徴とする重心軌道生成装置である。
この一態様において、前記モデル予測手段は、予測区間において、前記脚式ロボットの重心高さ軌道に応じて重心高さを変化させて、前記重心軌道を生成してもよい。
この一態様において、前記システム生成手段は、前記非線形のＺＭＰ方程式を線形化することなく前記離散システムを生成し定式化を行ってもよい。
この一態様において、前記評価関数設定手段は、予測区間内における前記目標ＺＭＰと前記未来のＺＭＰとの偏差の２乗と、前記入力値の２乗とを加算した値を最小化するような前記評価関数を設定してもよい。
この一態様において、前記モデル予測手段は、前記評価関数設定手段により設定された評価関数と、前記制約条件設定手段により設定された制約条件に基づいて定式化された凸２次計画問題の最適解を算出し、該最適解の第１要素を前記入力値として決定してもよい。
この一態様において、前記制約条件設定手段は、ＺＭＰが支持多角形から外れないような前記制約条件を設定してもよい。
他方、上記目的を達成するための本発明の一態様は、脚式ロボットの重心軌道を生成する重心軌道生成装置の生成方法であって、前記脚式ロボットにおける非線形のＺＭＰ方程式に基づいて、離散システムを生成するステップと、前記脚式ロボットの目標ＺＭＰを設定するステップと、前記設定された前記目標ＺＭＰと、前記生成された離散システムに基づいて算出される未来のＺＭＰと、の偏差、及び、入力値に基づいた評価関数を設定するステップと、前記脚式ロボットのＺＭＰに対する制約条件を設定するステップと、前記設定された制約条件下で、前記設定された評価関数の最適化計算を行って前記入力値を決定するプロセスを、所定周期毎に繰り返すことで、前記重心軌道を生成するステップと、を含む、ことを特徴とする重心軌道生成装置の生成方法であってもよい。
また、上記目的を達成するための本発明の一態様は、脚式ロボットの重心軌道を生成する重心軌道生成装置のプログラムであって、前記脚式ロボットにおける非線形のＺＭＰ方程式に基づいて、離散システムを生成する処理と、前記脚式ロボットの目標ＺＭＰを設定する処理と、前記設定された前記目標ＺＭＰと、前記生成された離散システムに基づいて算出される未来のＺＭＰと、の偏差、及び、入力値に基づいた評価関数を設定する処理と、前記脚式ロボットのＺＭＰに対する制約条件を設定する処理と、前記設定された制約条件下で、前記設定された評価関数の最適化計算を行って前記入力値を決定するプロセスを、所定周期毎に繰り返すことで、前記重心軌道を生成する処理と、をコンピュータに実行させることを特徴とする重心軌道生成装置のプログラムであってもよい。
【発明の効果】
【０００８】
本発明によれば、安定的な歩行が可能な重心軌道を生成できる重心軌道生成装置、その生成方法及びプログラムを提供することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の一実施の形態に係る重心軌道生成装置の概略的なシステム構成を示すブロック図である。
【図２】重心高さを一定と仮定したときの線形倒立振子モデルの一例を示す図である。
【図３】Ｒｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ制御を説明するための図である。
【図４】入力値列、出力値列、及び状態変数列の一例を示す図である。
【図５】重心高さ軌道の変化の一例を示す図である。
【図６】従来の重心軌道、実ＺＭＰ軌道、及び目標ＺＭＰ軌道の一例を示す図である。
【図７】本発明の一実施の形態に係る重心軌道生成方法による初期化処理のフローを示すフローチャートである。
【図８】本発明の一実施の形態に係る重心軌道生成方法を示すフローチャートである。
【図９】本発明の一実施の形態に係る重心軌道生成装置により重心軌道を生成したときのシミュレーション結果を示す図である。
【図１０】外乱印加時における従来の重心軌道生成装置により重心軌道を生成したときのシミュレーション結果を示す図である。
【図１１】外乱印加時における本実施の形態に係る重心軌道生成装置により重心軌道を生成したときのシミュレーション結果を示す図である。
【発明を実施するための形態】
【００１０】
以下、図面を参照して本発明の実施の形態について説明する。
図１は、本発明の一実施の形態に係る重心軌道生成装置の概略的なシステム構成を示すブロック図である。本実施の形態に係る重心軌道生成装置１は、二足歩行ロボットなどの倒立制御型の脚式ロボット１００の重心軌道を生成するものである。重心軌道生成装置１は、脚式ロボット１００における非線形のＺＭＰ（Zero Moment Point）方程式に基づいて、離散システムを生成するシステム生成部２と、脚式ロボット１００の目標ＺＭＰを設定するＺＭＰ設定部３と、最適化のための評価関数を設定する評価関数設定部４と、脚式ロボット１００のＺＭＰに対する制約条件を設定する制約条件設定部５と、制約条件設定部５により設定された制約条件下で、評価関数設定部４により設定された評価関数の最適化計算を行って入力値を決定するプロセスを、所定周期毎に繰り返すことで、重心軌道を生成するモデル予測部６と、を備えている。
【００１１】
システム生成部２は、システム生成手段の一具体例であり、脚式ロボット１００における非線形のＺＭＰ方程式を線形化すること無く、連続システムを生成しサンプリング周期Δｔの離散システムを生成し定式化を行う。
【００１２】
ＺＭＰ設定部３は、ＺＭＰ設定手段の一具体例であり、例えば、予め設定された目標ＺＭＰ軌道に基づいて、所定時間未来の目標ＺＭＰ列を設定する。
【００１３】
評価関数設定部４は、評価関数設定手段の一具体例であり、ＺＭＰ設定部３により設定された目標ＺＭＰ列と、システム生成部２により生成された離散システムに基づいて算出される未来のＺＭＰ列と、の偏差、及び、入力値に基づいた評価関数を設定する。評価関数設定部４は、例えば、予測区間内における目標ＺＭＰ列と未来のＺＭＰ列との偏差の２乗と、入力値の２乗とを加算した値を最小化するような評価関数を設定する。
【００１４】
制約条件設定部５は、制約条件設定手段の一具体例であり、例えば、ユーザの入力情報などに基づいて、脚式ロボット１００のＺＭＰに対する制約条件を設定する。制約条件設定部５は、例えば、ＺＭＰが支持多角形から外れないような制約条件を設定する。
【００１５】
モデル予測部６は、モデル予測手段の一具体例であり、制約条件設定部５により設定された制約条件下で、評価関数設定部４により設定された評価関数の最適化計算を行って入力値を決定するプロセスを、所定周期毎に繰り返すことで、重心軌道を生成する。
例えば、モデル予測部６は、評価関数設定部４により設定された評価関数と、制約条件設定部により設定された制約条件に基づいて凸２次計画問題を構成する。そして、モデル予測部６は、構成した凸２次計画問題の最適解を算出し、算出した最適解の第１要素を入力値として決定する。モデル予測部６は、このプロセスを制御周期毎に繰り返して入力値を決定することで重心軌道を生成する。
【００１６】
なお、重心軌道生成装置１は、例えば、演算処理等を行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバスによって相互に接続されている。
ここで、重心高さを一定と仮定しＺＭＰ方程式を線形化した線形化倒立振子モデルを用いて重心軌道を求めるモデル予測制御型の重心軌道生成方法について説明する。なお、ｘ方向及びｙ方向は、基本的に同一のダイナミクスを有している為、以下ｘ方向のみについて説明を行い、ｙ方向の説明は省略する。
【００１７】
まず、図２に示すように、重心高さを一定（ｚｇ＝ｈ＝ｃｏｎｓｔ）として、モデル化を行う。単質点モデルのＺＭＰ方程式は、下記（１）式に示すように、簡易な線形式で表わすことができる。
【数１】
0005803751.tif 000002
【００１８】
入力をｕ（ｔ）＝ｘｇの３階微分値、出力をｗ（ｔ）＝ｘｚｍｐとすると、連続システムは、下記（２）式のように表わすことができる。
【数２】
0005803751.tif 000003
【００１９】
上記（２）式の連続システムを所定のサンプリング周期Δｔで離散化すると、下記（３）式に示すような離散システムを生成できる。
【数３】
0005803751.tif 000004
【００２０】
モデル予測制御においては、上記離散システム（３）式を用いて、「ある一定期間未来までのモデル挙動を予測しながら、制約条件下で評価関数を用いて最適化計算を行い、現在の入力値を決定する」というプロセスを制御周期毎に行う。
【００２１】
モデル予測制御（Ｒｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ制御）においては、例えば、図３に示すように、現在においてＮΔｔ未来までの期間のモデル挙動を予測して最適化計算を行って現在の入力値ｕｋを決定し、Δｔ秒後に同様の計算を行ってΔｔ秒後の入力値を決定する、という計算過程を一定周期毎に行う。その結果として、これら得られた入力値に基づいて最適な重心軌道を生成することができる。
【００２２】
ＮΔｔ未来までの期間のモデル挙動予測に関しては、下記（４）式のように離散システムを未来に向かって積算していくことで得られる。
【数４】
0005803751.tif 000005
【００２３】
なお、上記（４）式において、ｕｋ、ｕｋ＋１、ｕｋ＋２、・・・、ｕｋ＋Ｎ-1は、現在から未来までの各サンプリング点における入力値を表わしており、ｘｋ、ｘｋ＋１、ｘｋ＋２、・・・、ｘｋ＋Ｎは、現在から未来までの各サンプリング点における状態変数値を表わしている。上記（４）式は、現在がサンプリング点ｋの状態を示している。また、図４に示すように、ｗｋ、ｗｋ＋１、ｗｋ＋２、・・・、ｗｋ＋Ｎは、上記入力値と同様に、現在から未来までの各サンプリング点における出力値を表わしている。
上述の入力値列及び出力値列を下記（５）式に示すように定義する。
【数５】
0005803751.tif 000006
【００２４】
上記（４）式及び（５）式に基づいて、未来の出力値（ＺＭＰ）列を計算すると下記（６）式が得られる。
【数６】
0005803751.tif 000007
【００２５】
ここで、目標ＺＭＰ軌道が与えられると仮定し、ＮΔｔ未来までの目標ＺＭＰ列を下記（７）式に示すように定義することができる。
【数７】
0005803751.tif 000008
【００２６】
モデル予測制御型の重心軌道生成方法において、最適化の評価関数として、予測区間内の目標ＺＭＰ列Ｗｒｅｆｋ＋１とＺＭＰ列Ｗｋ＋１との間の偏差の２乗と、入力値Ｕｋの２乗と、を加算した値を最小化する下記の評価関数（８）式で与えられる。
【数８】
0005803751.tif 000009
【００２７】
上記評価関数（８）式において、例えば、第１項は、ＺＭＰ列Ｗｋ＋１を目標ＺＭＰ列Ｗｒｅｆｋ＋１に近づける機能を有し、第２項は、入力値Ｕｋが過剰に作用するのを抑止する（重心が発散するのを抑制する）機能を有している。
【００２８】
上記評価関数（８）式におけるＱ及びＲは、夫々、上記２項をバランスする為の重み行列及び重みベクトルであり、下記（９）式で表される。これら重み行列Ｑ及び重みベクトルＲは、制御対象に応じて適宜調整できるパラメータである。
【数９】
0005803751.tif 000010
【００２９】
ここで、上記（８）式を上記（６）式に代入すると、下記（１０）式が得られる。
【数１０】
0005803751.tif 000011
【００３０】
したがって、上記（１０）式を用いて、上記評価関数（８）式は、下記（１１）式として定式化することができる。
【数１１】
0005803751.tif 000012
【００３１】
ここで、モデル予測制御において、上記評価関数（１１）式に加えて、ＺＭＰに対する制約条件を設定することができる。本実施の形態において、制約設定部５は、例えば、ＺＭＰに対して下記（１２）式に示す制約条件を設定する。
【数１２】
0005803751.tif 000013
【００３２】
上記制約条件（１２）式は、ＺＭＰが支持多角形から外れないという条件を意味しており、例えば、Ｗｌｋ＋１を踵のｘ座標、Ｗｕｋ＋１を爪先のｘ座標というように設定できる（但し、この場合、単脚支持期である）。
さらに、上記同様に、上記（１２）式を上記（６）式に代入すると、下記（１３）式が得られる。
【数１３】
0005803751.tif 000014
【００３３】
最終的に、最適化問題は、上記（１１）式及び（１３）式に基づいて、下記（１４）式に示す凸２次計画問題として定式化され、最適解Ｕｋの第１要素ｕｋが現在の入力値としてシステムに入力される。上記プロセスを、所定周期毎に実行して入力値を決定していくことにより、重心軌道を生成できる。
【数１４】
0005803751.tif 000015
【００３４】
ところで、上述のようにして生成した重心軌道は、重心高さを一定にした線形倒立振子モデルを想定している。この場合、重心高さを変化させると、いわゆるＺＭＰ偏差が生じることとなる。特に、重心高さを大きく変動させた場合には、大きなＺＭＰ偏差が発生する。この場合、モデル予測制御計算上においてはＺＭＰ制約条件を満たしていても、実際のＺＭＰは制約条件の範囲を外れてしまうことが起こり得る。
【００３５】
例えば、図５に示すように重心高さ軌道（実線（１））を変化させた場合（但し、点線（２）は線形倒立振子モデルで用いた値）、従来のモデル予測制御を用いて重心軌道の生成を行う。この場合、図６に示すようにモデル予測制御を実行した後のＺＭＰ軌道（１）は、モデル予測制御上では目標ＺＭＰ軌道（２）にほぼ一致している。しかしながら、実際のＺＭＰ軌道（実ＺＭＰ軌道）（３）は目標ＺＭＰ軌道（２）から大きく外れた軌道になる。このように、線形倒立振子モデルにおいては目標ＺＭＰ軌道に略一致しているが、重心高さ変化によるモデル誤差がＺＭＰ偏差を生じさせることとなる。
【００３６】
そこで、本実施の形態に係る重心軌道生成装置１においては、モデル予測制御の予測区間における重心高さを、一定（ｈ＝const）ではなく、重心高さ軌道に合わせて変更する。すなわち、システム生成部２は、下記に示す元のＺＭＰ方程式（１５）式を線形化することなく、非線形状態のままで定式化を行う。
【数１５】
0005803751.tif 000016
【００３７】
このとき、サンプリング周期Δｔで離散化した離散システムは、下記（１６）式のようになる。
【数１６】
0005803751.tif 000017
【００３８】
上記（１６）式において、ｚｇ（ｋΔｔ）、及び（ｚｇ（ｋΔｔ）の２階微分値）は、予め設定された重心高さ軌道を離散的にサンプリングすることで得られる。
上記定式化と同様に、入力値列及び出力値列を上記（５）式に示すように定義し、上記（４）式及び（５）式に基づいて、未来の出力値（ＺＭＰ）列を計算すると、下記（１７）式を得ることができる。
【数１７】
0005803751.tif 000018
【００３９】
モデル予測部６は、上記（１７）式に基づいて、上記（１４）式に示すような凸２次計画問題を定式化し、最適解Ｕｋの第１要素ｕｋを現在の入力値としてシステムに入力する。モデル予測部６は、上記プロセスを、サンプリング周期毎に実行して入力値を決定していくことにより、重心軌道を生成する。
【００４０】
図７は、本実施の形態に係る重心軌道生成方法による初期化処理のフローを示すフローチャートである。
初期化処理として、以下（ステップＳ１０１）から（ステップＳ１０３）の処理を行う。所定のサンプリング周期Δｔが設定される（ステップＳ１０１）。離散システムにおけるシステム行列Ａ及びシステムベクトルｂの計算が実行される（ステップＳ１０２）。評価関数の比率を決める重み行列Ｑ及び重みベクトルＲが設定される（ステップＳ１０３）。
【００４１】
図８は、本実施の形態に係る重心軌道生成方法を示すフローチャートである。
まず、脚式ロボット１００の重心高さ軌道ｚｋ、（ｚｋの１階微分値）、（ｚｋの２階微分値）、が設定される（ステップＳ１０４）。
【００４２】
次に、システム生成部２は、設定された重心高さ軌道に基づいた非線形のＺＭＰ方程式（１５）式をそのまま用いて、サンプリング周期Δｔで離散化し離散システム（１６）式を生成する。その後、システム生成部２は、入力値列及び出力値列を上記（５）式のように定義し、上記（１７）式に示すような未来の出力値（ＺＭＰ）列Ｗｋ＋１を計算する。システム生成部２は、出力ベクトルｃｋ＋１、・・・、ｃｋ＋Ｎの計算をし（ステップＳ１０５）、出力値系列の係数Ｇ１、Ｇ２を計算する（ステップＳ１０６）。さらに、重心の現在状態ｘｋ、ｙｋが設定され（ステップＳ１０７）、ＺＭＰ設定部３は、上記（７）式に示すようなＮΔｔ未来までの目標ＺＭＰ列Ｗｒｅｆｋ＋１を設定する（ステップＳ１０８）。
【００４３】
そして、評価関数設定部４は、予測区間内における目標ＺＭＰ列Ｗｒｅｆｋ＋１とＺＭＰ列Ｗｋ＋１との偏差の２乗と、入力値Ｕｋの２乗を加算した値を最小化する評価関数（上記（１１）式）を設定する（ステップＳ１０９）。
【００４４】
次に、制約条件設定部５は、例えば、ＺＭＰが支持多角形から外れない条件（上記（１２）式）などの制約条件を設定する（ステップＳ１１０）。その後、制約条件設定部５は、設定した制約条件に基づいて、評価関数の不等式制約条件（上記（１３）式）を設定する（ステップＳ１１１）。モデル予測部６は、上記（１４）式に示すように定式化された凸２次計画問題の最適解Ｕｋを解く（ステップＳ１１２）。さらに、モデル予測部６は、下記（１８）式に基づいて、生成した重心軌道をオンライン状態で更新し（ステップＳ１１３）、時間の更新を行う（ステップＳ１１４）。
ｘｋ＋１＝Ａｘｋ＋ｂｕｘｋ
ｙｋ＋１＝Ａｙｋ＋ｂｕｙｋ      （１８）式
【００４５】
図９は、本実施の形態に係る重心軌道生成装置により重心軌道を生成したときのシミュレーション結果を示す図である。図９に示すように、モデル予測制御を実行した結果としてのＺＭＰ軌道（１）は、重心高さの変動に応じて目標ＺＭＰ軌道（２）に対して上下に変動しているが、実ＺＭＰ軌道（３）は、目標ＺＭＰ軌道に収束していることが分かる。
【００４６】
図１０は、外乱印加時における従来の重心軌道生成装置により重心軌道を生成したときのシミュレーション結果を示す図である。また、図１１は、外乱印加時における本実施の形態に係る重心軌道生成装置により重心軌道を生成したときのシミュレーション結果を示す図である。図１０に示すように、従来の重心軌道生成装置において、重心高さが変動すると、実ＺＭＰ軌道（３）が、目標ＺＭＰ軌道から大きく外れて、大きなＺＭＰ偏差が生じており、ＺＭＰが制約条件（足裏の範囲）をはみ出している。これはロボットが転倒に至ることを意味する。一方、図１１に示すように、本実施の形態に係る重心軌道生成装置１において、重心高さが変動しても、実ＺＭＰ軌道（３）が、目標ＺＭＰ軌道（２）に収束し、ＺＭＰ偏差が小さく抑えられており、ＺＭＰを制約条件内に保ったまま重心軌道が生成できていることが分かる。
【００４７】
以上、本実施の形態に係る重心軌道生成装置１は、モデル予測制御の予測区間における重心高さを、一定ではなく、重心高さ軌道に合わせて変更するものであって、システム生成部２は、ＺＭＰ方程式を線形化することなく、非線形状態のままで定式化を行う。これにより、ＺＭＰ偏差を小さく抑えることができ、安定的な歩行が可能な脚式ロボット１００の重心軌道を生成できる。
【００４８】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。また、上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、例えば、上記システム生成部２、ＺＭＰ設定部３、評価関数設定部４、制約条件設定部５、モデル予測部６、などが実行する処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【００４９】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ）を含む。
【００５０】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【産業上の利用可能性】
【００５１】
本発明は、例えば、安定的な歩行が可能な二足歩行ロボットなどの脚式ロボットの重心軌道を生成する重心軌道生成装置に利用可能である。
【符号の説明】
【００５２】
１    重心軌道生成装置
２    システム生成部
３    ＺＭＰ設定部
４    評価関数設定部
５    制約条件設定部
６    モデル予測部
１００    脚式ロボット
【図１】
0005803751.tif 000019
【図３】
0005803751.tif 000020
【図４】
0005803751.tif 000021
【図５】
0005803751.tif 000022
【図６】
0005803751.tif 000023
【図７】
0005803751.tif 000024
【図８】
0005803751.tif 000025
【図９】
0005803751.tif 000026
【図１０】
0005803751.tif 000027
【図１１】
0005803751.tif 000028
【図２】
0005803751.tif 000029