JP2018185747A - 非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム - Google Patents

Abstract

【課題】不連続な状態変化を伴う非線形システムの動作を実時間で制御することが可能な非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラムを提供する。【解決手段】非線形システムに関する状態を示す状態パラメータが取得される。モデル予測制御のアルゴリズムを用いて非線形システムを制御するための制御入力値が算出される。算出された制御入力値を用いて非線形システムが制御される。制御入力値は、指定されたタイミングにおいて状態が不連続に変化するように前記非線形システムの状態を拘束するペナルティ関数を用いて、算出される。【選択図】図１０

Description

本発明は、非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラムに関する。

二足歩行ロボットのような安定性の低いシステムを制御する際には、未来（有限時間後まで）のシステム挙動を予測しながら制御を行うモデル予測制御（receding horizon control；リシーディングホライズン制御）を用いることが有効である。モデル予測制御は、制御周期（サンプリング周期）ごとに各時刻から有限時間未来までの最適制御問題を解き、制御入力値を決定するフィードバック制御である。

フィードバック制御において、二足歩行ロボットは歩行動作等を伴う非線形性の高いシステムであるので、二足歩行ロボットの制御は、非線形モデル予測制御によって行われることが好ましい。ここで、非線形モデル予測制御は、一般に、多大な計算時間を要する。したがって、非線形モデル予測制御を用いて実時間（リアルタイム）で制御入力値の最適解を決定することは困難であった。

この技術に関連し、非特許文献１は、非線形モデル予測制御を実時間（リアルタイム）で行うことが可能な、Ｃ／ＧＭＲＥＳ法（continuation／generalized minimum residual method）と呼ばれる技術を開示する。Ｃ／ＧＭＲＥＳ法は、連続変形法（continuation method）とＧＭＲＥＳ法とを組み合わせたアルゴリズムである。Ｃ／ＧＭＲＥＳ法は、状態変化が連続であるシステムに対し、最適解の連続性を利用して、最適解の変化率を求めながら最適解を追跡していく計算方法である。このＣ／ＧＭＲＥＳ法を用いることにより、非線形モデル予測制御においても、実時間でシステムを制御することが可能となる。

Toshiyuki OHTSUKA and Hironori A. FUJII、「Real-Time Receding-Horizon Control Algorithm for Nonlinear Systems」、計測自動制御学会論文集、１９９７年１２月、Vol.33, No.12, p. 1131-1139

非線形性を有するシステムである非線形システムでは、周囲の環境との物理的な接触を伴って移動するとき、物理的な接触により状態変化が不連続となる場合がある。一方、非特許文献１にかかる技術は、システムの状態変化が連続であることを前提としている。したがって、非特許文献１にかかる技術を用いて、物理的接触を行う可能性がある非線形システムを制御することは困難である。したがって、非線形モデル予測制御を用いて、不連続な状態変化を伴う非線形システムを実時間で制御することは困難であった。

本発明は、不連続な状態変化を伴う非線形システムを実時間で制御することが可能な非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラムを提供する。

本発明にかかる非線形システムの制御方法は、前記非線形システムの状態を示す状態パラメータを取得する取得ステップと、前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記非線形システムを制御するための制御入力値を算出する算出ステップと、前記算出された制御入力値を用いて、前記非線形システムを制御する制御ステップとを有し、前記算出ステップにおいて、指定されたタイミングにおいて状態が不連続に変化するように前記非線形システムの状態を拘束する拘束パラメータを用いて、前記非線形システムの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する。

本発明は、上述したように、指定されたタイミングにおいて状態が不連続に変化するように非線形システムの状態を拘束する拘束パラメータを用いることで、想定しているタイミングで、不連続な状態変化を起こさせることができる。したがって、非線形システムの制御に非線形モデル予測制御の理論を容易に適用でき、さらにＣ／ＧＭＲＥＳ法を適用することも可能となる。したがって、本発明は、不連続な状態変化を伴う非線形システムを実時間で制御することが可能となる。

また、本発明にかかる二足歩行ロボットの制御装置は、２つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御装置であって、前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する状態取得手段と、前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出手段と、前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御手段とを有し、前記算出手段は、指定されたタイミングにおいて前記２つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する。

また、本発明にかかる二足歩行ロボットの制御方法は、２つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御方法であって、前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する取得ステップと、前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出ステップと、前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御ステップとを有し、前記算出ステップにおいて、指定されたタイミングにおいて前記２つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する。

また、本発明にかかるプログラムは、２つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御方法を実現するプログラムであって、前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する取得ステップと、前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出ステップであって、指定されたタイミングにおいて前記２つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する、算出ステップと、前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御ステップとをコンピュータに実行させる。

本発明は、上述したように、指定されたタイミングにおいて遊脚が着地するように二足歩行ロボットの状態を拘束する拘束パラメータを用いることで、想定しているタイミングで、遊脚の着地といった不連続な状態変化を起こさせることができる。したがって、二足歩行ロボットの制御に非線形モデル予測制御の理論を容易に適用でき、さらにＣ／ＧＭＲＥＳ法を適用することも可能となる。したがって、本発明は、不連続な状態変化を伴う二足歩行ロボットの動作を実時間で制御することが可能となる。

また、好ましくは、前記拘束パラメータは、前記モデル予測制御のアルゴリズムで用いられる評価関数に含まれている。これにより、不連続な状態変化の無い非線形モデル予測制御と同様に最適化問題を扱うことが可能となる。

また、好ましくは、前記拘束パラメータは、前記タイミングにおいて遊脚が着地したときの前記二足歩行ロボットの姿勢を指定する。これにより、想定した姿勢で遊脚を着地させるように二足歩行ロボットを制御することが可能となる。

また、好ましくは、前記拘束パラメータは、前記タイミングにおいて遊脚が着地したときの前記２つの脚の関節部の目標角度を指定する。これにより、想定した関節角度で遊脚を着地させるように関節部を制御することが可能となる。

また、好ましくは、前記拘束パラメータは、調整可能なゲインを含む。これにより、制御装置の性能によらないで制御を安定化させることが可能となる。

本発明によれば、不連続な状態変化を伴う非線形システムを実時間で制御することが可能な非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラムを提供できる。

実施の形態１にかかるロボットシステムを示す概略図である。 実施の形態１にかかるロボットシステムの構成を示す機能ブロック図である。 非線形モデル予測制御を説明するための図である。 入力系列の更新について説明するための図である。 実施の形態１にかかるロボットをコンパス型モデルに適用する方法を説明するための図である。 実施の形態１にかかるロボットをコンパス型モデルに適用した例を示す図である。 状態ジャンプを説明するための図である。 遊脚リンクの衝突直前のロボットの状態を示す図である。 遊脚リンクの衝突直後のロボットの状態を示す図である。 実施の形態１にかかる制御装置によって行われるロボットの制御方法を示すフローチャートである。 実施の形態２にかかるロボットを示す図である。 実施の形態２にかかるロボットを膝屈曲モデルに適用した状態を示す図である。 実施の形態２にかかるロボットを膝屈曲モデルに適用した例を示す図である。 本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。 本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。 本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。 本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。 本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。 本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。 シミュレーション結果において定常状態の制御入力のグラフを示す図である。

（実施の形態１）
以下、図面を参照して本発明の実施の形態について説明する。なお、各図面において、同一の要素には同一の符号が付されており、必要に応じて重複説明は省略されている。

図１は、実施の形態１にかかるロボットシステム１を示す概略図である。また、図２は、実施の形態１にかかるロボットシステム１の構成を示す機能ブロック図である。ロボットシステム１は、ロボット１００と、ロボットの動作を制御する制御装置２とを有する。

ロボット１００は、胴体１０２と、２つの脚である右脚１１０Ｒ及び左脚１１０Ｌとを有する。ロボット１００は、２つの脚（右脚１１０Ｒ及び左脚１１０Ｌ）を用いて歩行動作を行うことが可能な二足歩行ロボットである。右脚１１０Ｒ及び左脚１１０Ｌは、ロボット１００の胴体１０２の下部に設けられている。ここで、図１に示すように、ロボット１００の前方向をＸ軸方向、上方向をＹ軸方向とする。また、以下、右脚１１０Ｒに関する構成要素の符号に「Ｒ」を付し、左脚１１０Ｌに関する構成要素の符号に「Ｌ」を付すが、それぞれの構成要素について左右を区別しない場合には、「Ｒ」及び「Ｌ」は、適宜、省略され得る。

右脚１１０Ｒは、胴体１０２に近い方から順に、股関節部１２０Ｒと、上腿部１１２Ｒと、膝関節部１２２Ｒと、下腿部１１４Ｒと、足首関節部１２４Ｒと、足部１１６Ｒとを有する。同様に、左脚１１０Ｌは、胴体１０２に近い方から順に、股関節部１２０Ｌと、上腿部１１２Ｌと、膝関節部１２２Ｌと、下腿部１１４Ｌと、足首関節部１２４Ｌと、足部１１６Ｌとを有する。足部１１６Ｒ及び足部１１６Ｌの底部には、それぞれ足裏センサ１１８が設けられている。足裏センサ１１８は、足部１１６の底部に加わる荷重を検出する。

股関節部１２０Ｒ及び股関節部１２０Ｌは、胴体１０２の下部に取り付けられている。そして、股関節部１２０Ｒ及び股関節部１２０Ｌを介して、それぞれ、上腿部１１２Ｒ及び上腿部１１２Ｌが胴体１０２と接続されている。言い換えると、右脚１１０Ｒ及び左脚１１０Ｌは、それぞれ、股関節部１２０Ｒ及び股関節部１２０Ｌを介して、胴体１０２と接続されている。

また、膝関節部１２２Ｒを介して、上腿部１１２Ｒと下腿部１１４Ｒとが接続されている。同様に、膝関節部１２２Ｌを介して、上腿部１１２Ｌと下腿部１１４Ｌとが接続されている。また、足首関節部１２４Ｒを介して、下腿部１１４Ｒと足部１１６Ｒとが接続されている。同様に、足首関節部１２４Ｌを介して、下腿部１１４Ｌと足部１１６Ｌとが接続されている。

股関節部１２０は、ＸＹ平面に垂直な軸（つまりロボット１００の横方向に水平な軸）の周りに回転する。これにより、右脚１１０Ｒ及び左脚１１０Ｌは、前後に動作し得る。したがって、ロボット１００は、右脚１１０Ｒ及び左脚１１０Ｌを交互に前に出すことにより歩行動作を行うことができる。

膝関節部１２２は、ＸＹ平面に垂直な軸の周りに回転する。これにより、右脚１１０Ｒ及び左脚１１０Ｌは、膝関節部１２２で屈曲動作を行うことができる。また、足首関節部１２４は、ＸＹ平面に垂直な軸の周りに回転する。これにより、足部１１６は、下腿部１１４に対して上下に動作し得る。

図２に示すように、ロボット１００の各関節部（股関節部１２０、膝関節部１２２及び足首関節部１２４）は、角度センサ１３０と、モータ１４０とを有する。角度センサ１３０は、例えばエンコーダであって、各関節部の関節角度を検出する。モータ１４０は、各関節部を動作させる、アクチュエータとしての機能を有する。また、各関節部は、各関節部のモータ１４０のトルクを検出するトルクセンサ１３６を有してもよい。また、ロボット１００の周囲の状態を検出するためのカメラが、胴体１０２に内蔵されていてもよい。

制御装置２は、例えばコンピュータとしての機能を有する。制御装置２は、ロボット１００の内部（例えば胴体１０２）に搭載されてもよい。また、制御装置２は、ロボット１００と物理的に離れていてもよく、その場合、ロボット１００と有線又は無線を介して通信可能に接続されてもよい。制御装置２は、ロボット１００の動作、特に、右脚１１０Ｒ及び左脚１１０Ｌの動作を制御する。さらに具体的には、制御装置２は、各関節部のモータのトルクを制御することで、右脚１１０Ｒ及び左脚１１０Ｌの姿勢を制御する。つまり、ロボットシステム１において、制御装置２はマスタ装置としての機能を有し、ロボット１００はスレーブ装置としての機能を有する。

制御装置２は、主要なハードウェア構成として、ＣＰＵ（Central Processing Unit）４と、ＲＯＭ（Read Only Memory）６と、ＲＡＭ（Random Access Memory）８とを有する。ＣＰＵ４は、制御処理及び演算処理等を行う演算装置としての機能を有する。ＲＯＭ６は、ＣＰＵ４によって実行される制御プログラム及び演算プログラム等を記憶するための機能を有する。ＲＡＭ８は、処理データ等を一時的に記憶するための機能を有する。

また、制御装置２は、状態取得部１２、非線形モデル予測制御部１４、及びサーボ制御部１６（以下、「各構成要素」と称する）を有する。各構成要素は、例えば、ＣＰＵ４がＲＯＭ６に記憶されたプログラムを実行することによって実現可能である。また、各構成要素は、必要なプログラムを任意の不揮発性記録媒体に記録しておき、必要に応じてインストールするようにして、実現するようにしてもよい。なお、各構成要素は、上記のようにソフトウェアによって実現されることに限定されず、何らかの回路素子等のハードウェアによって実現されてもよい。

状態取得部１２は、ロボット１００の現在の歩行に関する状態を示すデータ（状態パラメータ）を取得する、状態取得手段としての機能を有する。状態取得部１２は、各センサ（角度センサ１３０、足裏センサ１１８及びトルクセンサ１３６）から、各センサの検出値を取得する。そして、状態取得部１２は、取得された検出値（及び検出値から得られた値）を非線形モデル予測制御部１４に対して出力する。なお、「検出値から得られた値」とは、例えば、「検出値」が角度センサ１３０から検出された関節角度である場合、関節角度の速度（変化量，時間微分）であってもよい。この場合、状態パラメータは、関節角度及び関節角度の速度を示してもよい。

非線形モデル予測制御部１４は、ロボット１００の動作を制御するための制御入力値（入力値）を算出する算出手段としての機能を有する。非線形モデル予測制御部１４は、状態取得部１２からの検出値（及び検出値から得られた値）の少なくとも一部を状態パラメータとして、その状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用してロボット１００の動作を制御するための制御入力値を算出する。また、非線形モデル予測制御部１４は、算出された制御入力値をサーボ制御部１６に対して出力する。詳しくは後述する。また、非線形モデル予測制御部１４は、ロボットシステム１の外部の上位コントローラ（図示せず）によって、必要な指示値（歩幅、歩行周期等）を入力されてもよい。

サーボ制御部１６は、非線形モデル予測制御部１４によって算出された制御入力値を用いてロボット１００の動作を制御する制御手段としての機能を有する。サーボ制御部１６は、算出された制御入力値となるように、ロボット１００の各関節部を制御する。また、サーボ制御部１６は、サーボアンプの機能を有してもよい。また、サーボ制御部１６は、トルク制御を行う場合、各関節部のトルク（関節トルク）が算出された制御入力値となるように、各関節のモータ１４０を制御する。このとき、サーボ制御部１６は、各関節部のトルクセンサ１３６によって検出されたトルク値を用いてフィードバック制御を行ってもよい。

（モデル予測制御）
ここで、本実施の形態にかかる非線形モデル予測制御部１４によって行われる、モデル予測制御（非線形モデル予測制御）の手法の概要について説明する。非線形モデル予測制御とは、非線形システムに対し、各サンプリング時刻で有限時刻未来までの最適入力（制御入力値の最適解）を求め、得られた入力のうち初期値を実際の入力とする制御である。非線形モデル予測制御には、非線形最適制御である、フィードバック制御である、及び、拘束条件を組み込み易いという、３つの利点がある。

このように、非線形モデル予測制御は、フィードバック制御であるため外乱に対して強く、拘束条件も多様に組み合わせることができる。このような特徴があるため、非線形モデル予測制御は、多くのシステムへの導入が期待されている。しかしながら、ニュートン法などの従来の反復法では、サンプリング周期内で最適解に収束させることは困難であった。

近年、この問題に対する有効な数値計算法として、Ｃ／ＧＭＲＥＳ法が新たに考案された。Ｃ／ＧＭＲＥＳ法を用いることで有限時刻未来までの最適制御問題をサンプリング周期内で解くことが可能になった。しかしながら、以下に示すように、現状では、Ｃ／ＧＭＲＥＳ法による非線形モデル予測制御を適用できない場合もある。

すなわち、ロボット１００等の非線形システムの多くは、状態が不連続に変化する事象（「状態ジャンプ」と称する）を伴い得る。この状態ジャンプを含む問題に対し、現状のＣ／ＧＭＲＥＳ法による非線形モデル予測制御を適用することは困難であった。状態ジャンプを伴うシステムを直接最適化しようとすると、状態ジャンプの時刻及び入力を同時に最適化する必要がある。また、状態ジャンプにかかるラグランジュ乗数がさらに追加されることになる。

また、これらの問題をＣ／ＧＭＲＥＳ法を用いて数値的に解くには、Ｃ／ＧＭＲＥＳ法のアルゴリズムを大きく変える必要がある。そこで、本願の発明者らは、ペナルティ関数（ペナルティ項）を非線形モデル予測制御の評価関数に加えるという手法を見出した。ペナルティ関数を評価関数に加えることで、状態ジャンプが発生する時刻（本実施の形態においては遊脚が着地する時刻）を指定できる。また、ラグランジュ乗数を増やさずにＣ／ＧＭＲＥＳ法を適用することができる。

まず、非線形モデル予測制御の概要について説明する。制御対象として、以下の式（１）で示すような状態方程式で表される非線形システムを考える。

ただし，ｘ（ｔ）は状態ベクトルであり、ｕ（ｔ）は制御入力ベクトル（制御入力値を示すベクトル）である。また、Ｒ^ｎ及びＲ^ｍは、それぞれ、ｎ次元実数ベクトル全体の集合及びｍ次元実数ベクトル全体の集合を示す。非線形モデル予測制御とは、式（１）で表されるシステムに対し、各時刻ｔにおいて、以下の式（２）で表される評価関数を最小にする入力ｕ_ｏｐｔ（ｔ＋τ）を求め、その初期値ｕ_ｏｐｔ（ｔ）を時刻ｔにおける実際の制御入力値ｕ（ｔ）とする制御である。

ここで、Ｔは時刻ｔにおける評価区間の長さである。関数φ（ｘ）は終端コストと呼ばれるスカラー値関数である。関数Ｌ（ｘ，ｕ）はステージコストと呼ばれるスカラー値関数である。τは評価区間における時間パラメータであって、０≦τ≦Ｔである。なお、Ｔは、通常、正のスカラー値Ｔ_ｆ及びα（α＞０）を用いて以下の式（３）のように与えられる。

このように、非線形モデル予測制御は、各時刻ｔで状態に基づいた最適入力を求めているため、フィードバック制御となっている。
また、以下の式（４）で表されるベクトル関数を拘束条件として与えることもできる。
なお、拘束条件については、等式拘束条件だけでなく、不等式拘束条件を組み込むこともできる。

図３は、非線形モデル予測制御を説明するための図である。図３に示すように、非線形モデル予測制御では、現在時刻ｔ_０においてＴ秒後までの期間のモデル挙動を予測して最適化計算を行って入力ｕ_ｏｐｔ（ｔ_０＋τ）を求める。そして、その初期値ｕ_ｏｐｔ（ｔ_０）を現在時刻ｔ_０における実際の制御入力値ｕ（ｔ_０）とする。

同様に、制御周期であるサンプリング周期Δｔ秒後に、その時点での現在時刻ｔ_１においてＴ秒後までの期間のモデル挙動を予測して最適化計算を行って入力ｕ_ｏｐｔ（ｔ_１＋τ）を求める。そして、その初期値ｕ_ｏｐｔ（ｔ_１）を現在時刻ｔ_１における実際の制御入力値ｕ（ｔ_１）とする。以下同様に、サンプリング周期ごとに、制御入力値ｕ（ｔ）が算出されることとなる。

非線形モデル予測制御で各時刻ｔにおいて解くべき問題は、評価区間上の時刻τについて以下の式（５）〜（８）に示すような最適制御問題である。
ただし、時刻ｔにおける評価区間上の時刻τ（０≦τ≦Ｔ）の状態変数ベクトルと制御入力ベクトルとを、それぞれ、ｘ^＊（τ；ｔ）＝ｘ^＊（ｔ＋τ）、ｕ^＊（τ；ｔ）＝ｕ^＊（ｔ＋τ）とした。また、添字＊は、評価区間上の値であることを示す。

式（７）のＪを汎関数とみなして変分法を用いて停留条件を求めると、最適制御の必要条件（オイラー・ラグランジュ方程式）が、以下の式（９）〜（１４）のように得られる。

ここで、
は、ハミルトン関数と呼ばれるスカラー値関数であり、λ^＊（τ；ｔ）は式（９）に対する随伴変数、μ^＊（τ；ｔ）は式（１４）に対するラグランジュ乗数である。

一方、実際の数値計算は、すべて離散化して行われる。したがって、式（９）〜（１４）は、すべて離散近似して扱わなければならない。そこで、評価区間（０≦τ≦Ｔ）をＮステップに離散近似することを考える。その際の評価区間の時間刻みを、以下の式（１６）で示すようにする。

その上で、評価区間上のｉ番目（１≦ｉ≦Ｎ）のステップ、つまり時刻ｔ＋ｉΔτにおける状態を、
と表す。ｕ^＊ _ｉ（ｔ）、λ^＊ _ｉ（ｔ）及びμ^＊ _ｉ（ｔ）についても同様に表される。なお、Ｔ及びＮは、予め定められた値である。したがって、Δτも、予め定められた値である。

この条件の下で式（９）〜（１４）を離散近似すると、以下の式（１７）〜（２２）で示すような、離散近似されたオイラー・ラグランジュ方程式が得られる。

但し、離散近似されたハミルトン関数は、以下の式（２３）で定義される。

以上より，非線形モデル予測制御における最適入力を求める問題というのは、上記の式（１７）〜（２２）を解いて、ｉ＝０からｉ＝Ｎについて、ｘ^＊ _ｉ（ｔ）、ｕ^＊ _ｉ（ｔ）、λ^＊ _ｉ（ｔ）及びμ^＊ _ｉ（ｔ）を求めるという問題に帰着される。
ここで、ｘ^＊ _ｉ（ｔ）は、上記式（１７），（１８）より陽に求められる。また、λ^＊ _ｉ（ｔ）は、求められたｘ^＊ _ｉ（ｔ）と上記式（１９），（２０）とから陽に求められる。したがって、ｘ^＊ _ｉ（ｔ）、ｕ^＊ _ｉ（ｔ）、λ^＊ _ｉ（ｔ）及びμ^＊ _ｉ（ｔ）のうちの本質的な未知量は、以下の式（２４）で表されるベクトルＵ（ｔ）で定義される。
なお、「：＝」は、定義を意味する等号である。つまり、上記式（２４）において、左辺Ｕ（ｔ）は、右辺のベクトルで定義される。

そして、このＵ（ｔ）は、以下の式（２５）で示される方程式を解くことによって得られる。

次に、Ｃ／ＧＭＲＥＳ法について説明する。ニュートン法などの反復法では、式（２５）を各サンプリング時間内で解くのは難しい。一方、Ｕ（ｔ）が時刻ｔに関して連続であれば、Ｕ（ｔ）は、サンプリング周期Δｔごとに、以下の式（２６）で示されるようにして更新され得る。

したがって、Ｕ（ｔ）を求めるためには、ｔ＝０ではＵ（０）を求め、ｔ＞０では、Ｕ（ｔ）の時間微分、つまりＵ（ｔ）の変化量である
を求めればよい。なお、式（２６）の計算は、上記のＵ（ｔ）の変化量を数値積分することに対応する。

ここで、
を求めるために、式（２５）が全てのｔで成り立つことを考慮して、上記式（２５）と等価である、以下の式（２７）で表される方程式を扱うことを考える。

さらに、式（２７）は、以下の式（２８）で示すように書き換えられ得る。
但し、ζは正の実数で、安定化パラメータと呼ばれる。

式（２８）の全微分を実行して整理すると、Ｕの変化量
は、次の式（２９）で表される連立方程式を解くことで得られる。

したがって、ｔ＞０のとき、各時刻で解くべき問題は、上記式（２９）で示される連立方程式のみとなる。さらに、式（２９）で示される連立方程式の数値解法として、連立方程式から少ない反復回数で高精度な解を得ることが可能なＧＭＲＥＳ法を用いることができる。

上述した手法が、非線形モデル予測制御の実時間最適化アルゴリズムである。このＵ（ｔ）の連続性を利用した変形法（連続変形法；Continuation method）とＧＭＲＥＳ法を組み合わせたアルゴリズムを、Ｃ／ＧＭＲＥＳ法と称する。

次に、状態ジャンプを考慮した非線形モデル予測制御について説明する。状態ｘ（ｔ）∈Ｒ^ｎが以下の式（３０）で表される条件を満たすと、その直後より状態ジャンプが生じるシステムを仮定する。
ただし、ξ（ｘ（ｔ））∈Ｒ^ｌはベクトル値である。

ここで、状態ジャンプが、時刻ｔ_ｊの前後で起こるとする。つまり、ｔ_ｊの直前及び直後の時刻をそれぞれ「ｔ_ｊ−」及び「ｔ_ｊ＋」と表すと，ここで仮定されるシステムは、以下の式（３１）を満たしている。

また、ここで仮定されるシステムは、状態ジャンプ直後の状態ｘ（ｔ_ｊ＋）が、以下の式（３２）で表されるように、状態ジャンプ直前の状態ｘ（ｔ_ｊ−）から陽に求められるとする。

さらに、状態ジャンプとともにシステムの状態方程式が切り替わり得るとする。このとき、状態ジャンプの前後の状態方程式は、以下の式（３３），（３４）で示すように記述される。
なお、状態方程式は必ずしも切り替わる必要はなく、ｆ_１（ｘ，ｕ）＝ｆ_２（ｘ，ｕ）であってもよい。また、説明の明確化のため、ここで扱うシステムでは、式（４）で示したような拘束条件は存在しないと仮定する。

非線形モデル予測制御の問題を考えるために、以下の説明では、評価区間上でのジャンプ時刻をτ_ｊとする。つまり、τ_ｊは、以下の式（３５），（３６）を満たす。

一般的に、上記のシステムを最適化するためには、変分法により導かれた停留条件を数値的に解けばよい。システムの方程式が上記式（３３），（３４），（３２）で与えられ、ｔ_ｊが固定されていないとき、停留条件は、以下の式（３７）〜（４８）で与えられる。
但し、ν∈Ｒ^ｌは、は式（３１）に対するラグランジュ乗数である。

また、ハミルトン関数Ｈ_１，Ｈ_２は、それぞれ以下の式（４９），（５０）で表される。

上記の停留条件の中で、時刻ｔ_ｊに関する条件を示す式は、式（４１）,（４８）である。ここで、上述したように実際には離散化して数値計算することを考慮すると、これらの式からτ_ｊを求めることは困難である。τ_ｊを求めることができないと、評価区間上の状態ｘ^＊（τ；ｔ）及び評価区間上の随伴変数λ^＊（τ；ｔ）を求めることもできない。さらに、νも新たな未知量として追加されている。したがって、仮にτ_ｊが求められたとしても、本質的な未知量を算出するための式として上記の式（２４）をそのまま用いることはできず、新たに未知量及び未知方程式を定義しなければならない。

そこで、本実施の形態にかかるアルゴリズムでは、十分大きな正のスカラー値ｐを用いた以下の式（５１）で表されるペナルティ関数が、評価関数に追加されている。

式（５１）で表されるペナルティ関数を加えた評価関数を最適化すれば、指定した時刻τ_ｊに対して、以下の式（５２）が成り立ち得る。

つまり、上記のペナルティ関数を用いることにより、指定した時刻ｔ_ｊにおいて状態ジャンプを生じさせることが可能となる。言い換えると、上記のペナルティ関数は、指定したタイミングで状態が不連続となるようにシステムの状態を拘束する拘束パラメータである。また、式（５２）で表される拘束条件がペナルティ関数について追加されているため、ラグランジュ乗数νは追加されなくてもよい。したがって、本実施の形態にかかるアルゴリズムでは、非線形モデル予測制御の最適化問題を、状態ジャンプの無い非線形モデル予測制御と同数の未知数の問題として扱うことができる。

したがって、状態ジャンプを含む非線形モデル予測制御の問題は、式（５１）を評価関数に加えることにより、状態ジャンプの無い非線形モデル予測制御と同様に扱うことができる。但し、ペナルティ関数を用いるため、あらかじめ適切な状態ジャンプ時刻ｔ_ｊを指定する必要がある。

このように、本実施の形態にかかるアルゴリズムでは、ペナルティ関数を用いることにより、想定しているタイミングで、不連続な状態変化を起こさせることができる。したがって、元々の非線形モデル予測制御の理論を容易に適用でき、さらにＣ／ＧＭＲＥＳ法を適用することも可能となる。したがって、後述するように、二足歩行ロボットのような非線形システムに対しても実時間で制御を行うことが可能となる。

次に、ペナルティ関数を用いた場合の停留条件について説明する。ここで、仮に、連続時間で停留条件を導出した場合、以下の式（５３），（５４）で表される項が生じる。

上述したように、コンピュータで数値計算を行うためには、停留条件の各方程式を離散近似して考える必要がある。しかしながら、式（５３）,（５４）については、τ_ｊの前後で離散近似を行うことができない。したがって、本実施の形態にかかる問題に対しては、状態方程式及び評価関数を離散近似したのち、変分法より停留条件を導出することとする。

まず、以下の式（５５）を満たすステップｉをジャンプステップｉ_ｊと定義する。

ｉ_ｊを用いると、上記の式（３３）,（３４）,（３２）は、それぞれ、以下の式（５６），（５７），（５８）で示すように、離散近似される。

また、式（５１）で表されるペナルティ項（ペナルティ関数）は、以下の式（５９）で示すように、離散近似される。

次に、変分法より停留条件を求める。離散近似された本システムに対する最適制御問題とは、式（５９）が追加された、以下の式（６０）で表されるような離散近似された評価関数を最小にする入力の系列ｕ_０（ｔ），・・・，ｕ_Ｎ−１（ｔ）を求める問題である。

ここで、式（５６），（５７），（５８）は、それぞれ、以下の式（６１），（６２），（６３）で示されるような等式拘束条件とみなすことができる。

式（６１），（６２）のラグランジュ乗数ベクトルをλ_ｉ＋１（ｔ）、式（６３）のラグランジュ乗数ベクトルを
とする。このとき、ラグランジュ関数は、以下の式（６４）で定義される。

式（６４）のＪに式（６０）で示されたＪを代入すると、以下の式（６５）が得られる。

但し、ハミルトン関数Ｈ_１，Ｈ_２は、それぞれ以下の式（６６），（６７）で定義される。

また、式（６５）の変分は、以下の式（６８）のように表される。

制御入力値が最適であれば、式（６８）において、任意のδｘ_ｉ（ｔ），δｕ_ｉ（ｔ）について、
が成り立つ。ここで、ｘ_０（ｔ）＝ｘ（ｔ）よりδｘ_０（ｔ）＝０であることに注意すると、以下の式（６９）〜（７９）で表される停留条件が導かれる。但し、添字＊は、評価区間上の値であることを示す。

ここで、評価区間上の状態ｘ^＊ _ｉ（ｔ）は、上記式（６９），（７０），（７１），（７２）から算出され得る。また、随伴変数λ^＊ _ｉ（ｔ）は、上記式（７３），（７４），（７５），（７６）から算出され得る。したがって、本実施の形態にかかるモデルでは、拘束条件を考えていないため、未知量を示すベクトルＵ（ｔ）は、以下の式（８０）で定義される。

このＵ（ｔ）は、以下の式（８１）で示される方程式を解くことによって得られる。
この方程式を、上述したＣ／ＧＭＲＥＳ法を用いて解けばよい。

ここで、サンプリング周期ごとの入力系列（最適解）の更新について考える。図４は、入力系列の更新について説明するための図である。図４の矢印Ａに示すように、Ｃ／ＧＭＲＥＳ法では、上記式（２６）で示すように前進差分近似を行っている。ここで、時刻ｔにおける評価区間でのｉ_ｊステップ目の時刻つまり（ｔ＋ｉ_ｊΔτ）が状態ジャンプ前の時刻（つまり時刻ｔ_ｊより前の時刻）であり、時刻ｔ＋Δｔにおける評価区間でのｉ_ｊステップ目の時刻つまり（ｔ＋Δｔ＋ｉ_ｊΔτ）が状態ジャンプ後の時刻（つまり時刻ｔ_ｊより後の時刻）であるとする。このとき、時刻ｔにおけるジャンプステップはｉ_ｊであるが、時刻ｔ＋Δｔにおけるジャンプステップはｉ_ｊ−１である。このとき、
は状態ジャンプ前の入力（以下、式Ｕｊ_ｂと表記）であり、
は状態ジャンプ後の入力（以下、式Ｕｊ_ａと表記）となる。

したがって、本実施の形態にかかるシステムは、状態ジャンプの前後で状態が不連続であることから、ｉ＝ｉ_ｊのとき、単純に、
とすることはできない。つまり、式（２６）’のように計算すると、上記の状態ジャンプ後の入力Ｕｊ_ａは、最適入力として更新されていないこととなる。

そこで、本実施の形態においては、状態ジャンプ後の入力Ｕｊ_ａを、ｉ≠ｉ_ｊのｕ^＊ _ｉより近似することと考える。状態ジャンプ後の入力Ｕｊ_ａは、時刻ｔ＋Δｔ＋ｉ_ｊΔτの評価区間上の入力である。適切に近似するためには、図４の矢印Ｂで示すように、この時刻に最も近い時刻における入力、つまり時刻ｔ＋（ｉ_ｊ＋１）Δτの入力
から近似すればよい。

したがって、時刻ｔにおけるジャンプステップと時刻ｔ＋Δｔにおけるジャンプステップとが異なるとき、状態ジャンプ後の入力Ｕｊ_ａは、以下の式（８２）で示すように更新される。

但し、ｉ_ｊ＝Ｎ−１のときは、上記式（８２）で示した近似は行えないので、通常通り、以下の式（８３）で示すように入力は更新される。

なお、上式は、ΔｔとΔτとが、以下の式（８４）を満たすことを仮定している。
Δｔ＜２Δτ ・・・（８４）
しかしながら、
２Δτ≦Δｔ ・・・（８５）
であるときも、ｕ^＊ _ｉ（ｔ＋Δｔ）の系列は、実時間上のｕ^＊ _ｉ（ｔ）の系列から適切に近似され得る。

なお、Ｃ／ＧＭＲＥＳ法においては、Ｕ（ｔ）の変化量を求める際に，Ｆ（Ｕ，ｘ，ｔ）を用いて、以下の式（８６）で示すように、前進差分近似を行っている。

しかしながら、差分近似の差分時間をｈとすると，時刻ｔと時刻ｔ＋ｈとの間で状態ジャンプが生じた場合、状態量が不連続に大きく変化してしまい、差分近似を正確に行うことができない。したがって、時刻ｔが以下の式（８７）で示される
ｔ＜ｔ_ｊ≦ｔ＋ｈ ・・・（８７）
を満たすとき，以下の式（８８）で示すように後退差分近似を行う。

（二足歩行ロボットへの適用）
次に、上述した非線形モデル予測制御を、本実施の形態にかかるロボット１００の動作の制御に適用した例について説明する。なお、実施の形態１においては、ロボット１００がコンパス型モデルである例について説明するが、後述するように、非線形モデル予測制御は、ロボット１００がコンパス型モデルでなくても適用可能である。

なお、ロボット１００の歩行動作は、遊脚が地面と衝突する（着地する）という動作を含む。この衝突の前後で、ロボット１００の一般化速度が不連続に変化する。つまり、このとき、状態ジャンプが発生する。また、一般的に、歩行動作は、周期的な運動である。したがって、ロボット１００を、予め定められた周期ごとに状態ジャンプを生じさせる（つまり遊脚を着地させる）ように制御を行うことが可能である。なお、「着地」とは、遊脚が地面と衝突（接触）することに限定されない。つまり、「着地」とは、ロボット１００がその上を歩行している面（歩行面）に遊脚が接触することを意味する。

図５は、実施の形態１にかかるロボット１００をコンパス型モデルに適用する方法を説明するための図である。図５に示した例では、右脚１１０Ｒが支持脚であり、左脚１１０Ｌが遊脚（振り脚）である。制御装置２は、支持脚が地面と点接触していることを模擬するため、支持脚（図５の例では右脚１１０Ｒ）の足首関節部１２４に設けられたトルクセンサ１３６を用いて、支持脚の足首関節部１２４のトルクを０に制御する。また、制御装置２は、右脚１１０Ｒ及び左脚１１０Ｌの膝関節部１２２を、伸展状態でロックするように制御する。つまり、制御装置２は、右脚１１０Ｒ及び左脚１１０Ｌの膝関節部１２２の関節角度が伸展状態に対応する角度（例えば０）となるように、膝関節部１２２のモータ１４０を制御する。さらに、制御装置２は、遊脚（図５の例では左脚１１０Ｌ）の足裏センサ１１８を用いて、遊脚の着地を検出する。このようにして、ロボットシステム１は、コンパス型モデルを模擬することができる。

図６は、実施の形態１にかかるロボット１００をコンパス型モデルに適用した例を示す図である。図６に示す例では、ロボット１００は、関節１５０と、支持脚リンク１５１と、遊脚リンク１５２とから構成されるコンパス型モデルにモデル化されている。ここで、関節１５０は、胴体１０２及び股関節部１２０に対応する。また、支持脚リンク１５１は、右脚１１０Ｒ及び左脚１１０Ｌのうちの支持脚に対応する。また、遊脚リンク１５２は、右脚１１０Ｒ及び左脚１１０Ｌのうちの遊脚に対応する。

関節１５０の質量をｍ_０とする。また、図６の矢印で示すように、関節１５０の周りに、制御入力値として入力トルクｕが入力される。ここで、支持脚リンク１５１及び遊脚リンク１５２の物理的性質は、互いに同じであるとする。支持脚リンク１５１及び遊脚リンク１５２の長さを、ｌとする。また、支持脚リンク１５１及び遊脚リンク１５２の質量を、ｍとする。

また、鉛直方向に対する支持脚リンク１５１の角度をθ_１とし、鉛直方向に対する遊脚リンク１５２の角度をθ_２とする。但し、図６において時計回り（各リンクの下端を中心に関節１５０が前方に回る方向）を正とする。したがって、図６の状態では、θ_２＜０である。

次に、図６に例示したコンパス型モデルの歩行動作に関して、以下のような仮定があるとする。
・遊脚リンク１５２と地面９０との衝突（着地）は一瞬である。
・遊脚リンク１５２と地面９０との衝突は完全非弾性衝突である。
・リンクと地面との摩擦係数は∞である。
・両脚（両リンク）が同時に地面９０から力を受けることはない。

上記の仮定より、両脚（支持脚リンク１５１及び遊脚リンク１５２）が同時に地面９０に着くことはない。また、衝突時に遊脚リンク１５２の速度は０となる。したがって、本モデルの歩行制御に必要な方程式は、片脚支持期の運動方程式（状態方程式）と、遊脚リンク１５２の衝突時の方程式（衝突方程式）との２つである。

片脚（つまり支持脚リンク１５１）だけが地面９０に接触しているとき、ラグランジュの運動方程式より、以下の式（８９）で示す方程式が導き出される。

但し、ｑは、以下の式（Ｓ１）で示される一般化座標ベクトルである。
また、Ｍ（ｑ）は慣性行列、Ｈ（ｑ、ｑ（ドット））は重力とコリオリ力の項、Ｎｕはｑに対する一般化力である。なお、Ｒ^ｎ×ｍは、ｎ×ｍの実数行列全体の集合を示す。

ここで、式（８９）に示した運動方程式の詳細を、以下の式（Ｅ１），（Ｅ２），（Ｅ３）に示す。なお、以下の式において、Ｉ^ｍは、支持脚リンク１５１及び遊脚リンク１５２の重心（重心１５１ｍ及び重心１５２ｍ）周りの慣性モーメントである。また、ｌ_Ｇは、関節１５０から各リンクの重心（重心１５１ｍ及び重心１５２ｍ）までの長さである。

また、状態ベクトルｘ∈Ｒ^４を、以下の式（Ｓ２）に示す。

この場合、状態方程式は、上記式（８９）より、以下の式（９０）で表される。
なお、本実施の形態かかるコンパス型モデルでは、歩行の拘束条件として、ＺＭＰ（zero moment point）は考慮されないものとする。

次に、衝突方程式について説明する。衝突方程式の説明の前に、状態ジャンプについて説明する。
図７は、状態ジャンプを説明するための図である。図７は、右脚１１０Ｒ又は左脚１１０Ｌの状態を示す図である。ここでは、右脚１１０Ｒの状態を示すとする。図７は、横軸が右脚１１０Ｒの角度を示し、縦軸が右脚１１０Ｒの角速度を示す、グラフ（位相線図）である。

状態Ｉにおいて、右脚１１０Ｒが地面から離れて遊脚となる。したがって、状態Ｉから、右脚１１０Ｒ（遊脚リンク１５２）の角度及び角速度は、（θ_２，θ_２（ドット））である。このとき、状態Ｉから後述する状態ＩＩまでの期間では、角度及び角速度は、連続的に変化している。そして、状態ＩＩで、遊脚であった右脚１１０Ｒ（遊脚リンク１５２）が地面９０に着地する。そして、直ちに、状態は状態ＩＩＩに移行して右脚１１０Ｒは支持脚（支持脚リンク１５１）となる。このとき、状態ＩＩから状態ＩＩＩに遷移するときに、角度はほとんど変わらないが、角速度が急激に変化する。したがって、状態ＩＩから状態ＩＩＩに遷移する際に、状態ジャンプが発生している。

状態ＩＩＩから、右脚１１０Ｒ（支持脚リンク１５１）の角度及び角速度は、（θ_１，θ_１（ドット））である。このとき、状態ＩＩＩから後述する状態ＩＶまでの期間では、角度及び角速度は、連続的に変化している。そして、状態ＩＶで、遊脚であった左脚１１０Ｌ（遊脚リンク１５２）が地面９０に着地する。そして、直ちに、状態は状態Ｉに移行して右脚１１０Ｒは遊脚（遊脚リンク１５２）となる。このとき、状態ＩＶから状態Ｉに遷移するときに、角度はほとんど変わらないが、角速度が急激に変化する。したがって、状態ＩＶから状態Ｉに遷移する際に、状態ジャンプが発生している。
このように、遊脚が地面に着地すると、状態を示すパラメータ（図７の例では脚の角度及び角速度）が、不連続に変化する。この現象が、状態ジャンプである。

次に、衝突前後の一般化座標及び一般化速度の定義について説明する。
図８は、遊脚リンク１５２の衝突直前のロボット１００の状態を示す図である。図８に示すように、衝突直前の一般化座標及び一般化速度は、それぞれ以下の式（Ｇ１）及び式（Ｇ２）で表される。

図９は、遊脚リンク１５２の衝突直後のロボット１００の状態を示す図である。図９に示すように、衝突直後の一般化座標及び一般化速度は、それぞれ以下の式（Ｇ３）及び式（Ｇ４）で表される。

ここで、遊脚リンク１５２と地面９０との衝突について、以下の２つの角運動量についての保存則を適用する。
・衝突前に遊脚であった脚（遊脚リンク１５２）の先端まわりの系全体の角運動量。
・関節１５０まわりの、衝突前に支持脚であった脚（支持脚リンク１５１）の角運動量。
上記の角運動量保存則は、上記式（Ｇ１），（Ｇ２），（Ｇ３），（Ｇ４）より、以下の式（９１）で表される。

なお、Ｑ^＋及びＱ⁻は、それぞれ以下の式（Ｅ４），（Ｅ５）で表される。

ここで、衝突前後で、一般化座標は変わらない。したがって、衝突直前の一般化座標ｑ⁻及び衝突直後の一般化座標ｑ^＋について、以下の式（９２）が成り立つ。

また、上記式（９１），（９２）は、衝突直前の状態空間ベクトルｘ⁻及び衝突直後の状態空間ベクトルｘ^＋を用いて、以下の式（９３）で表される。
但し、Ｉ_２は２×２の単位行列である。

また、Ｚ（ｑ⁻）は、以下の式（９４）を満たす２×２行列である。

次に、実施の形態１にかかるコンパス型モデルに、上述した非線形モデル予測制御を適用することを考える。上述したように、歩行動作とは、「連続して脚を前に出す」という動作である。本モデルにおいては、脚（支持脚リンク１５１及び遊脚リンク１５２）の開き角を目標値に近づける、という評価関数を設定し、遊脚リンク１５２が着地するたびに各リンクの座標を入れ替えることで歩行制御を行う。

このとき、上述した評価関数Ｊの終端コストφ及びステージコストＬは、それぞれ、次の式（９５），（９６）のように表される。
但し、ｓ_ｆ、ｑ_０及びｒは、それぞれ重みを表す正のスカラー値である。また、θ_ｒｅｆは、脚の開き角の目標値（目標角度）である。

また、遊脚の着地の度にθ_１とθ_２とを入れ替え、θ_１（ドット）とθ_２（ドット）とを入れ替えることを考慮すると、状態ジャンプの方程式である式（９３）は、以下の式（９７）のように書き換えられる。

但し、Ｉ'_２∈Ｒ^２×２は、以下の行列である。

次に、歩行周期をＴ_ｓｔｅｐとすると、ジャンプ時刻ｔ_ｊは、整数ｋを用いて以下の式（９８）で表される。
ｔ_ｊ＝ｋＴ_ｓｔｅｐ ・・・（９８）
なお、評価区間中に状態ジャンプが２回以上生じないように、式（３）で示したＴ（ｔ）を設定する。このとき，周期Ｔ_ｓｔｅｐごとに式（９１）で表される脚（遊脚リンク１５２）と地面９０との衝突が起こり、それ以外のときは、式（９７）で表される運動方程式でモデルの状態を記述することができる。つまり、モデルの状態は、以下の式（９９），（１００）で表される。

ここで、時刻ｔ_ｊで状態ジャンプ、つまり脚（遊脚リンク１５２）と地面９０との衝突が起こらなければならない。また、支持脚リンク１５１及び遊脚リンク１５２の長さが互いに同じであることから、遊脚リンク１５２が地面９０に着地したとき、θ_１＝−θ_２である。したがって、状態ジャンプが起こる条件は、以下の式（１０１）で表される。

したがって、評価関数に追加されるペナルティ関数（ペナルティ項）は、以下の式（５１）’で表される。ここで、ｐ_１は、ゲイン（重み）であって、十分大きな正のスカラー値である。このゲインｐ_１は、制御装置２のコンピュータの性能等に応じて、適宜調整可能である。

さらに、上記のペナルティ項とは別に、遊脚リンク１５２の着地時の脚の開き角を目標値に近づくけるような項を付け加えることを考える。したがって、以下の式（５１）”で示す項も、ペナルティ項として評価関数に加える。ここで、ｐ_２は、ゲイン（重み）であって、十分大きな正のスカラー値である。このゲインｐ_２は、制御装置２のコンピュータの性能等に応じて、適宜調整可能である。

式（５１）’及び式（５１）”を足し合わせると、実施の形態１にかかるペナルティ項は、以下の式（１０２）で表される。これにより、ペナルティ項（拘束パラメータ）は、予め指定したタイミング（時刻ｔ_ｊ）において遊脚が着地するようにロボット１００の状態を指定することとなる。言い換えると、ペナルティ項（拘束パラメータ）は、予め指定したタイミング（時刻ｔ_ｊ）において遊脚が着地したときのロボット１００の姿勢（各関節部の関節角度）を指定することとなる。
したがって、実施の形態１にかかるコンパス型モデルにおける非線形モデル予測制御を用いたロボット１００の制御では、式（５１）で示したペナルティ関数として、上記式（１０２）で表したものが適用される。

図１０は、実施の形態１にかかる制御装置２によって行われるロボット１００の制御方法を示すフローチャートである。図１０に示した制御方法は、上述した非線形モデル予測制御を用いている。したがって、実施の形態１にかかるロボット１００の制御方法は、式（６９）〜（７９）で表される停留条件から、式（８０）で表されるベクトルＵ（ｔ）を求め、このベクトルＵ（ｔ）の各成分の値を、式（８１）で示される方程式を解くことによって算出する。これにより、ロボット１００の動作を制御するための制御入力値が算出される。ここで、図１０に示したフローチャートにおいて、Ｓ１０２は、状態パラメータを取得する取得ステップに対応し、Ｓ１０４〜Ｓ１１４は、制御入力値を算出する算出ステップに対応し、Ｓ１１６は、ロボット１００の動作を制御する制御ステップに対応する。

まず、制御装置２は、ロボット１００の状態を示す状態ベクトル（状態パラメータ）を取得して、状態観測を行う（ステップＳ１０２）。具体的には、制御装置２の状態取得部１２は、股関節部１２０Ｒ及び股関節部１２０Ｌの角度センサ１３０から、股関節部１２０Ｒ及び股関節部１２０Ｌの関節角度を取得する。そして、状態取得部１２は、これらの関節角度から、現在時刻ｔにおけるθ_１及びθ_２（図６）を算出する。なお、状態取得部１２は、例えば、支持脚である方の脚にかかる股関節部１２０の関節角度と、胴体１０２の傾きとから、鉛直方向に対する支持脚リンク１５１の角度θ_１を取得できる。同様に、状態取得部１２は、例えば、遊脚である方の脚にかかる股関節部１２０の関節角度と、胴体１０２の傾きとから、鉛直方向に対する遊脚リンク１５２の角度θ_２を取得できる。なお、胴体１０２の傾きは、例えばジャイロセンサ等の傾斜センサを用いて取得可能である。

また、状態取得部１２は、θ_１及びθ_２の変化量θ_１（ドット）及びθ_２（ドット）を算出する。例えば、状態取得部１２は、時刻ｔの１つ前のサンプリング周期（制御周期）における時刻ｔ−Δｔにおけるθ_１及びθ_２と時刻ｔにおけるθ_１及びθ_２との差分から、それぞれ変化量θ_１（ドット）及びθ_２（ドット）を算出してもよい。

これにより、状態取得部１２は、時刻ｔにおける状態ベクトルｘ（ｔ）を取得する。さらに、状態取得部１２は、式（７２）で示すように、このｘ（ｔ）を、時刻ｔについての評価区間における、状態ベクトルの初期値とする。つまり、ｘ^＊ _０（ｔ）＝ｘ（ｔ）とする。なお、この式（７２）についての処理は、非線形モデル予測制御部１４が行ってもよい。

次に、制御装置２の非線形モデル予測制御部１４は、時刻ｔについての評価区間における状態変数を、各ｉ＝１〜Ｎについて更新する（ステップＳ１０４）。具体的には、非線形モデル予測制御部１４は、式（６９）〜（７１）で示したように、状態変数ｘ^＊ _ｉ（ｔ）を更新する。なお、実施の形態１にかかるコンパス型モデルの遊脚の着地の例では、状態方程式に関するｆは、ジャンプステップの前後で同じであるとする。つまり、ｆ_１（ｘ，ｕ）＝ｆ_２（ｘ，ｕ）である。

そして、非線形モデル予測制御部１４は、式（９０）に示した状態方程式を用いて、式（６９）〜（７０）で示すように、ｉ≠ｉ_ｊにおける状態変数を更新する。また、非線形モデル予測制御部１４は、ｉ＝ｉ_ｊにおいては、式（９７）に示した状態ジャンプの方程式を用いて、式（７１）で示すように、状態変数を更新する。また、制御装置２のＲＡＭ８は、得られた状態変数ｘ^＊ _ｉ（ｔ）を記憶する。

次に、制御装置２は、時刻ｔについての評価区間における随伴変数を、各ｉ＝１〜Ｎについて更新する（ステップＳ１０６）。具体的には、非線形モデル予測制御部１４は、式（７３）〜（７６）で示したように、Ｓ１０４で更新された各ｘ^＊ _ｉを用いて、随伴変数λ^＊ _ｉ（ｔ）を更新する。また、制御装置２のＲＡＭ８は、得られた随伴変数λ^＊ _ｉ（ｔ）を記憶する。

なお、上述したように、ｆ_１（ｘ，ｕ）＝ｆ_２（ｘ，ｕ）であるので、Ｈ_１＝Ｈ_２である。そして、非線形モデル予測制御部１４は、式（９０）に示した状態方程式及び式（９６）に示したステージコストＬの式を用いてハミルトン関数Ｈを算出し、式（７３）〜（７４）で示すように、ｉ≠ｉ_ｊにおける随伴変数を更新する。また、非線形モデル予測制御部１４は、ｉ＝ｉ_ｊにおいては、式（９７）に示した状態ジャンプの方程式、式（１０２）に示したペナルティ関数の式及び式（９６）に示したステージコストＬの式を用いて、式（７５）で示すように、随伴変数を更新する。また、非線形モデル予測制御部１４は、ｉ＝Ｎにおいては、式（９５）に示した終端コストの式を用いて、式（７６）で示すように、随伴変数を更新する。

次に、制御装置２は、ベクトル関数Ｆを導出する（ステップＳ１０８）。具体的には、非線形モデル予測制御部１４は、式（８０）で示されたベクトルＵ（ｔ）を算出するため、Ｓ１０４及びＳ１０６の処理で得られた状態変数ｘ^＊ _ｉ（ｔ）及び随伴変数λ^＊ _ｉ（ｔ）を用いて、式（８１）で示されたベクトル関数Ｆ（Ｕ，ｘ）の方程式を導出する。なお、上述したように、実施の形態１においては、Ｈ_１＝Ｈ_２である。

次に、制御装置２は、ベクトルＵ（ｔ）の全微分を計算する（ステップＳ１１０）。具体的には、非線形モデル予測制御部１４は、式（２９）を変形した以下の式（１０３）から、Ｃ／ＧＭＲＥＳ法を用いて、Ｕの全微分（Ｕ（ドット））、つまりＵ（ｔ）の変化率を算出する。言い換えると、非線形モデル予測制御部１４は、制御周期ごとに、制御入力値の最適解の変化率を算出する。
これにより、式（８０）から明らかなように、ｉ＝０〜Ｎ−１について、ｕ^＊ _ｉ（ｔ）の時間微分（ｕ^＊ _ｉ（ドット））が得られることとなる。ＲＡＭ８は、得られたｕ^＊ _ｉ（ｔ）の時間微分の値を記憶する。

次に、制御装置２は、入力系列ｕ^＊ _ｉ（ｔ）の更新を行う（ステップＳ１１２）。具体的には、非線形モデル予測制御部１４は、式（２６）及び式（８２）から、以下の式（１０４）及び（１０５）により、入力系列ｕ^＊ _ｉ（ｔ）の更新を行う。

ここで、式（１０４），（１０５）から、非線形モデル予測制御部１４は、ある時刻ｔにおける入力系列ｕ^＊ _ｉ（ｔ）及びｕ^＊ _ｉ（ｔ）の変化率を用いて、次のサンプリング周期Δｔである時刻ｔ＋Δｔにおける入力系列を算出する。したがって、時刻ｔにおける入力系列ｕ^＊ _ｉ（ｔ）を算出するためには、現在の時刻ｔの１つ前のサンプリング周期Δｔの時刻ｔ−Δｔにおける入力系列ｕ^＊ _ｉ（ｔ−Δｔ）及びｕ^＊ _ｉ（ｔ−Δｔ）の時間微分を用いることとなる。これにより、式（８０）で示したＵ（ｔ）の各成分の値が得られる。言い換えると、入力系列ｕ^＊ _ｉ（ｔ）のｉ＝０〜Ｎ−１それぞれの値が得られる。ＲＡＭ８は、入力系列ｕ^＊ _ｉ（ｔ）のｉ＝０〜Ｎ−１それぞれの値を記憶する。

次に、制御装置２は、入力値（制御入力値）を決定する（ステップＳ１１４）。具体的には、非線形モデル予測制御部１４は、以下の式（１０６）により、入力値ｕ_０を決定する。
つまり、非線形モデル予測制御部１４は、Ｕ（ｔ）の成分のうちの１番目の成分を入力値と決定する。非線形モデル予測制御部１４は、決定された入力値を、サーボ制御部１６に対して出力する。

次に、制御装置２は、ロボット１００の制御を行う（ステップＳ１１６）。具体的には、サーボ制御部１６は、Ｓ１１４で決定された入力値から、各関節部に指示する関節トルクを決定する。さらに具体的には、サーボ制御部１６は、支持脚リンク１５１に対応する脚の股関節部１２０の関節トルクτ_１と、遊脚リンク１５２に対応する脚の股関節部１２０の関節トルクτ_２とを、以下の式（１０７），（１０８）によって決定する。
τ_１＝ｕ_０ ・・・（１０７）
τ_２＝−ｕ_０ ・・・（１０８）

なお、胴体１０２（関節１５０）の姿勢が崩れることを防止するため、サーボ制御部１６は、以下の式（１０７），（１０８）のように関節トルクτ_１及び関節トルクτ_２を決定してもよい。
ここで、θ_{ｔｏｒｓｏ}は、鉛直方向に対する胴体１０２の前方への傾き角度である。また、ｋ_ｐ及びｋ_ｄは、ゲイン（重み）であって、予め定められた定数である。このゲインは、制御装置２のコンピュータの性能に応じて、適宜調整可能である。
サーボ制御部１６は、決定された関節トルクとなるように、各関節部（股関節部１２０）のモータ１４０を制御する。

以上のように、実施の形態１にかかる制御装置２は、非線形モデル予測制御のアルゴリズムを用いてロボット１００の動作を制御するに際し、指定したタイミングで遊脚が着地するようにロボット１００の状態を拘束するペナルティ関数（拘束パラメータ）を用いている。これにより、想定しているタイミングで、遊脚の着地という不連続な状態変化を起こさせることができる。したがって、元々の非線形モデル予測制御の理論を容易に適用でき、さらにＣ／ＧＭＲＥＳ法を適用することも可能となる。したがって、二足歩行ロボットのような非線形システムに対しても実時間で制御を行うことが可能となる。

また、実施の形態１にかかるペナルティ関数は、上記式（１０２）で示すように、予め指定したタイミングにおいて遊脚が着地したときのロボット１００の姿勢を指定している。これにより、想定した姿勢で遊脚を着地させるようにロボット１００を制御することが可能となる。さらに、実施の形態１にかかるペナルティ関数は、予め指定したタイミングにおいて遊脚が着地したときのロボット１００の各関節部の関節角度を指定している。これにより、想定した関節角度で遊脚を着地させるようにロボット１００の関節部を制御することが可能となる。

また、式（１０２）で示すように、ペナルティ関数は、調整可能なゲイン（ｐ_１及びｐ_２）を含む。指定されたタイミングで状態変化を確実に起こさせるため、ゲインは十分大きなスカラー値とする必要がある。しかしながら、制御装置２のコンピュータの性能等によっては、ゲインがあまりにも大きすぎると感度が過大となるため、不安定な制御となる可能性がある。したがって、ゲインを調整することにより、制御を安定化させることが可能となる。

（実施の形態２）
次に、実施の形態２について説明する。実施の形態２では、ロボット１００がより人間に似たヒューマノイドロボットである点で、実施の形態１と異なる。そして、実施の形態２では、膝を曲げて二足歩行を行うモデル（膝屈曲モデル）について、上述した非線形モデル予測制御を適用する。その他の点については、実施の形態１と実質的に同様であるので、説明を省略する。

図１１は、実施の形態２にかかるロボット１００を示す図である。実施の形態１にかかるロボット１００と同様に、実施の形態２にかかるロボット１００は、胴体１０２と、右脚１１０Ｒと、左脚１１０Ｌとを有する。右脚１１０Ｒ及び左脚１１０Ｌの構成については、実施の形態１のものと同様である。また、胴体１０２は、腰部１０２ａと、胸部１０２ｂと、腰関節部１０２ｃとを有する。また、胴体１０２の上側には、頭部１０４が設けられている。この頭部１０４に、カメラ等のセンサが設けられていてもよい。

また、ロボット１００は、胴体１０２（胸部１０２ｂ）の右側及び左側に、それぞれ右腕１６０Ｒ及び左腕１６０Ｌを有する。実施の形態２にかかるロボット１００は、右腕１６０Ｒ及び左腕１６０Ｌを用いて、所定の動作を行うことが可能である。なお、図１１には図示されていないが、右腕１６０Ｒ及び左腕１６０Ｌの先端に、対象物を把持することが可能なエンドエフェクタが設けられていてもよい。

右腕１６０Ｒは、胴体１０２に近い方から順に、肩関節部１７０Ｒと、上腕部１６２Ｒと、肘関節部１７２Ｒと、前腕部１６４Ｒとを有する。同様に、左腕１６０Ｌは、胴体１０２に近い方から順に、肩関節部１７０Ｌと、上腕部１６２Ｌと、肘関節部１７２Ｌと、前腕部１６４Ｌとを有する。肩関節部１７０Ｒ及び肩関節部１７０Ｌは、胴体１０２の右側及び左側にそれぞれ取り付けられている。そして、肩関節部１７０Ｒ及び肩関節部１７０Ｌを介して、それぞれ、上腕部１６２Ｒ及び上腕部１６２Ｌが胴体１０２と接続されている。言い換えると、右腕１６０Ｒ及び左腕１６０Ｌは、それぞれ、肩関節部１７０Ｒ及び肩関節部１７０Ｌを介して、胴体１０２と接続されている。

また、肘関節部１７２Ｒを介して、上腕部１６２Ｒと前腕部１６４Ｒとが接続されている。同様に、肘関節部１７２Ｌを介して、上腕部１６２Ｌと前腕部１６４Ｌとが接続されている。また、肩関節部１７０は、互いに直交した３軸の周りをそれぞれ回転するように構成され得る。また、肘関節部１７２は、１軸の周りを回転する。また、図２に示したように、肩関節部１７０及び肘関節部１７２は、角度センサ１３０と、モータ１４０とを有する。

図１２は、実施の形態２にかかるロボット１００を膝屈曲モデルに適用した状態を示す図である。図１２に示した例では、右脚１１０Ｒが支持脚であり、左脚１１０Ｌが遊脚である。制御装置２は、支持脚が地面と点接触していることを模擬するため、支持脚（図１２の例では右脚１１０Ｒ）の足首関節部１２４に設けられたトルクセンサ１３６を用いて、足首関節部１２４のトルクを０に制御する。さらに、制御装置２は、遊脚（図１２の例では左脚１１０Ｌ）の足裏センサ１１８を用いて、遊脚の着地を検出する。ここで、実施の形態１とは異なり、実施の形態２では、膝関節部１２２は、ロックされていない。このようにして、ロボットシステム１は、膝屈曲モデルを模擬することができる。

図１３は、実施の形態２にかかるロボット１００を膝屈曲モデルに適用した例を示す図である。図１３に示す例では、ロボット１００は、支持脚下腿リンク２０１と、支持脚上腿リンク２０２と、胴体リンク２０３と、遊脚上腿リンク２０４と、遊脚下腿リンク２０５と、関節２１１，２１２，２１３，２１４，２１５とから構成される、膝屈曲モデルにモデル化されている。ここで、胴体リンク２０３は、ロボット１００の胴体１０２から上の構成要素に対応する。

また、支持脚下腿リンク２０１は、右脚１１０Ｒ及び左脚１１０Ｌのうちの支持脚にかかる下腿部１１４に対応する。支持脚上腿リンク２０２は、右脚１１０Ｒ及び左脚１１０Ｌのうちの支持脚にかかる上腿部１１２に対応する。遊脚上腿リンク２０４は、右脚１１０Ｒ及び左脚１１０Ｌのうちの遊脚にかかる上腿部１１２に対応する。遊脚下腿リンク２０５は、右脚１１０Ｒ及び左脚１１０Ｌのうちの遊脚にかかる下腿部１１４に対応する。

また、関節２１１は、支持脚にかかる足首関節部１２４に対応する。関節２１２は、支持脚にかかる膝関節部１２２に対応する。関節２１３は、股関節部１２０に対応する。関節２１４は、遊脚にかかる膝関節部１２２に対応する。関節２１５は、遊脚にかかる足首関節部１２４に対応する。ここで、関節２１１は、地面９０に接触している。この関節２１１の位置、つまり支持脚の先端の位置を、（Ｘ_ｂ，Ｙ_ｂ）とする。また、関節２１５の位置、つまり遊脚の先端の位置を、（Ｘ_ｃ，Ｙ_ｃ）とする。

ここで、各リンク及び各関節２１１〜２１５を区別するパラメータｋ（ｋ＝１〜５）を設ける。ｋ＝１は、支持脚下腿リンク２０１及び関節２１１に対応する。同様に、ｋ＝２は、支持脚上腿リンク２０２及び関節２１２に対応する。ｋ＝３は、胴体リンク２０３及び関節２１３に対応する。ｋ＝４は、遊脚上腿リンク２０４及び関節２１４に対応する。そして、ｋ＝５は、遊脚下腿リンク２０５及び関節２１５に対応する。

したがって、支持脚下腿リンク２０１、支持脚上腿リンク２０２、胴体リンク２０３、遊脚上腿リンク２０４、及び遊脚下腿リンク２０５を、それぞれ、リンク＃１、＃２、＃３、＃４、＃５と示すことがある。そして、各リンクについて一般化して示すときに、リンク＃ｋと示すことがある。関節２１１〜２１５についても同様に、それぞれ、関節＃１、＃２、＃３、＃４、＃５と示すことがある。そして、各関節について一般化して示すときに、関節＃ｋと示すことがある。

支持脚下腿リンク２０１、支持脚上腿リンク２０２、胴体リンク２０３、遊脚上腿リンク２０４、及び遊脚下腿リンク２０５の長さ（リンク長）を、それぞれ、ｌ_１、ｌ_２、ｌ_３、ｌ_４、ｌ_５とする。また、支持脚下腿リンク２０１、支持脚上腿リンク２０２、胴体リンク２０３、遊脚上腿リンク２０４、及び遊脚下腿リンク２０５の質量（リンク質量）を、それぞれ、ｍ_１、ｍ_２、ｍ_３、ｍ_４、ｍ_５とする。また、支持脚下腿リンク２０１、支持脚上腿リンク２０２、胴体リンク２０３、遊脚上腿リンク２０４、及び遊脚下腿リンク２０５の各リンクの重心周りの慣性モーメントを、それぞれ、Ｉ^ｍ _１、Ｉ^ｍ _２、Ｉ^ｍ _３、Ｉ^ｍ _４、Ｉ^ｍ _５とする。なお、ｌ_ｋ、ｍ_ｋ及びＩ^ｍ _ｋは、予め定められた値である。

また、支持脚下腿リンク２０１、支持脚上腿リンク２０２、胴体リンク２０３、遊脚上腿リンク２０４、及び遊脚下腿リンク２０５の鉛直方向に対する角度（リンク角度）を、それぞれ、θ_１、θ_２、θ_３、θ_４、θ_５とする。なお、リンク角度θ_１、θ_２、θ_３、θ_４、θ_５は、ロボット１００の各関節部（股関節部１２０、膝関節部１２２及び足首関節部１２４）の角度センサ１３０で検出された関節角度から、幾何学的に一意に算出可能である。したがって、リンク角度θ_１、θ_２、θ_３、θ_４、θ_５は、ロボット１００の各関節部の関節角度に対応する。

また、支持脚下腿リンク２０１の重心２０１ｍの位置を、（Ｘ_ｃ１，Ｙ_ｃ１）とする。支持脚上腿リンク２０２の重心２０２ｍの位置を、（Ｘ_ｃ２，Ｙ_ｃ２）とする。胴体リンク２０３の重心２０３ｍの位置を、（Ｘ_ｃ３，Ｙ_ｃ３）とする。遊脚上腿リンク２０４の重心２０４ｍの位置を、（Ｘ_ｃ４，Ｙ_ｃ４）とする。遊脚下腿リンク２０５の重心２０５ｍの位置を、（Ｘ_ｃ５，Ｙ_ｃ５）とする。また、ｋ＝１〜５それぞれについて、関節＃ｋから、リンク＃ｋの重心＃ｋ（質点）までの距離を、ｒ_ｋとする。このｒ_ｋは、予め定められた値である。

ここで、重心＃ｋの位置（Ｘ_ｃｋ，Ｙ_ｃｋ）は、以下の式（１０９），（１１０）で表される。なお、式（１０９），（１１０）の第２項については、ｋ＜４の場合は０とする。

式（１０９），（１１０）について時間微分を行うことで、以下の式（１１１），（１１２）で示すように、重心＃ｋの速度が得られる。

この場合、ラグランジアンΓを以下の式（１１３）のように表すことができる。なお、ｇは重力加速度である。

このとき、ラグランジュの運動方程式は、以下の式（１１４）のように表される。

なお、ｑ及びηは、以下に示すベクトルである。
なお、ηは、関節トルクベクトルである。ここで、η_１は、足首関節部１２４のトルクである。また、η_２は、支持脚の膝関節部１２２のトルクである。また、η_３は、支持脚の股関節部１２０のトルク、言い換えると、支持脚に対する胴体リンク２０３のトルクである。η_４は、遊脚の股関節部１２０のトルクである。また、η_５は、遊脚の膝関節部１２２のトルクである。

そして、式（１１４）から、以下の式（１１５）を導き出すことができる。つまり、実施の形態２にかかる膝屈曲モデルでは、コンパス型モデルにおける式（８９）の方程式が、以下の式（１１５）で示すように修正される。なお、Ｍ（ｑ）及びＨ（ｑ、ｑ（ドット））は、膝屈曲モデルに対応するように修正され得る。

ここで、Ｎは、以下の式（１１６）で表される行列である。

また、実施の形態２にかかる膝屈曲モデルにおいても、式（９１）が成り立つので、Ｑ^＋及びＱ⁻についても、膝屈曲モデルに対応するように修正され得る。また、式（９５）及び式（９６）に示した評価関数Ｊの終端コストφ及びステージコストＬについても、膝屈曲モデルに対応するように修正され得る。
また、実施の形態２にかかる膝屈曲モデルにおけるペナルティ項については、式（１０２）で示したものを、以下の式（１１７）で示したものに修正する。なお、ｑ_ｒｅｆは、遊脚が着地するときの目標姿勢を示す。言い換えると、ｑ_ｒｅｆは、遊脚が着地するときの各関節の関節角度に対応するリンク角度θ_１、θ_２、θ_３、θ_４、θ_５の目標値（目標角度）のベクトルを示す。具体的には、ｑ_ｒｅｆは、遊脚が着地した時点において、Ｘ_ｃ−Ｘ_ｂが実現したい歩幅となり、Ｙ_ｃ＝Ｙ_ｂとなる（遊脚の先端の高さが支持脚の先端の高さと同じとなる）ようなｑである。

但し、Ｒは、以下の式（１１８）で表されるペナルティ重み行列である。なお、ｐ_１〜ｐ_５は、予め定められた値である。

そして、図１０に示したフローチャートにおいて、Ｓ１０２の処理で、状態取得部１２は、股関節部１２０、膝関節部１２２及び足首関節部１２４の角度センサ１３０から、各関節部の関節角度を取得する。そして、状態取得部１２は、これらの関節角度から、現在時刻ｔにおけるθ_１〜θ_５（図１３）及びこれらの変化量を算出して、状態ベクトルｘ（ｔ）を取得する。そして、非線形モデル予測制御部１４は、修正された各関数を用いてＳ１０４〜Ｓ１１４の計算を行って、関節トルクη_１〜η_５を算出することができる。そして、サーボ制御部１６は、算出された関節トルクη_１〜η_５から、各関節部のモータを制御することができる。

以上のように、状態方程式及びペナルティ項等を、実施の形態１にかかるコンパス型モデルにおけるものから、実施の形態２にかかる膝屈曲モデルにおけるものに置き換えることができる。これにより、実施の形態２にかかる膝屈曲モデルにかかるロボット１００の制御においても、上記のコンパス型モデルの制御で用いた非線形モデル予測制御のアルゴリズムを用いることが可能となる。したがって、実施の形態２にかかる膝屈曲モデルについても、実施の形態１と同様に、実時間で非線形モデル予測制御を行うことが可能となる。

（シミュレーション結果）
次に、本実施の形態にかかる非線形モデル予測制御のアルゴリズムを用いて非線形システムについて行ったシミュレーション結果について説明する。以下に説明するシミュレーションは、本実施の形態にかかる非線形モデル予測制御のアルゴリズムを、実施の形態１にかかるコンパス型モデルにかかるロボット１００に適用したものである。

表１は、シミュレーションで用いたコンパス型モデルの物理パラメータを示す。また、表２は、シミュレーションで用いた非線形モデル予測制御の評価関数の重みを示す。また、脚の開き角の目標値をθ_ｒｅｆ＝０．３２［ｒａｄ］とし、歩行周期をＴ_ｓｔｅｐ＝０．８［ｓ］とする。また、式（Ｓ２）に示した状態ベクトルの初期値を、ｘ（ｔ）＝［−０．１６６，０．１６５，０．６，０．７５］^Ｔとする。

表３は、Ｃ／ＧＭＲＥＳ法の数値計算に用いる各パラメータを示す。ここで、ｈ_ｄｉｒは、式（８６）で示したＧＭＲＥＳ法における前進差分近似の差分時間ｈである。また、ｒ_ｔｏｌは、シミュレーション開始時における最適性条件残差の許容値である。また、シミュレーション時間は１０［ｓ］とした。また、式（３）に示した評価区間Ｔ（ｔ）において、Ｔ_ｆ＝０．８［ｓ］、α＝１．０とした。

図１４〜図１９は、本実施の形態にかかる非線形システムに非線形モデル予測制御のアルゴリズムを適用したシミュレーション結果を示す図である。また、図２０は、シミュレーション結果において定常状態の制御入力のグラフを示す図である。図１４は、表１〜表３に示した条件下において、本実施の形態にかかる非線形モデル予測制御のアルゴリズムを、実施の形態１にかかるコンパス型モデルにかかるロボット１００に適用したシミュレーション結果を示す。図１４〜図１７は、それぞれ、θ_１、θ_２、θ_１（ドット）及びθ_２（ドット）のシミュレーション結果を示す。また、図１８は、制御入力値ｕのシミュレーション結果を示す。また、図１９は、式（８１）で表されるベクトルＦの大きさである｜｜Ｆ｜｜（エラーノルム）のシミュレーション結果を示す。

図２０のｔ＝８．０［ｓ］の近傍及びｔ＝８．８［ｓ］の近傍においてグラフが垂直に立っている箇所で、状態ジャンプが生じていることが分かる。このように、本シミュレーションでは、定常状態において、周期的な状態ジャンプを生じさせることに成功している。また、図１８のｕ（ｔ）のグラフから、ｔ＝５［ｓ］以降で、定常的な歩行をシミュレーションしていることが分かる。

（変形例）
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、ロボット１００の片方の脚は、股関節部１２０、膝関節部１２２及び足首関節部１２４を有するとしたが、このような構成に限られない。ロボット１００の脚は、３個よりも少ない数の関節部を有してもよいし、３個よりも多い数の関節部を有してもよい。この場合、状態ベクトル及び関節トルクベクトル（制御入力値）は、関節部の数に応じて、適宜、変更され得る。そして、状態方程式及びペナルティ関数等の関数も、関節部の数に応じて、適宜、変更され得る。

また、上述した実施の形態においては、非線形システムが二足歩行ロボットである例について説明したが、本実施の形態にかかる非線形モデル予測制御のアルゴリズムは、二足歩行ロボット以外の非線形システムについても適用可能である。つまり、本実施の形態にかかる非線形システムの制御方法は、以下に例示するような、状態ジャンプを伴う任意の非線形システムに対して、適用可能である。そして、上述したように、非線形モデル予測制御において状態ジャンプが発生するタイミングを指定するようにすればよい。

例えば、本実施の形態にかかる非線形システムは、図１１に示したロボット１００の腕（右腕１６０Ｒ又は左腕１６０Ｌ）のようなロボットハンド又はロボットアーム等であってもよい。この例における状態ジャンプは、ロボットハンド又はロボットアームが、周辺環境又は操作対象等の物体を押圧するとき、物体を把持し又は離すとき、球体等の物体を叩く又は打ち返すとき等に、発生し得る。なお、球体等の物体を打ち返す非線形システムの例として、例えば、卓球ロボットがある。

また、例えば、本実施の形態にかかる非線形システムは、腕及び脚を同時に床等に着地して移動可能な人型ロボット又は動物型ロボット等であってもよい。この例における状態ジャンプは、人型ロボット又は動物型ロボットが、腕と脚とを同時に、壁、床又はテーブル等に接触して移動するとき、又は、人型ロボット又は動物型ロボットが、梯子又は壁等を登るとき等に、発生し得る。

また、例えば、本実施の形態にかかる非線形システムは、ドローン等の無人航空機などであってもよい。この例における状態ジャンプは、無人航空機が、操作対象又は検査対象の物体に接触するとき又はその物体から離れるとき、輸送対象又は捕獲対象の物体を把持し又は離すとき等に、発生し得る。

また、例えば、本実施の形態にかかる非線形システムは、加工機械の工具等であってもよい。この例における状態ジャンプは、加工機械の工具が、加工対象等の物体に接触し又は離れるとき等に、発生し得る。

また、例えば、本実施の形態にかかる非線形システムは、自動車のトランスミッション等であってもよい。この例における状態ジャンプは、トランスミッションのクラッチが、接触状態（動力の伝達状態）となったとき又は離間状態（動力の遮断状態）なったとき等に、発生し得る。

また、例えば、本実施の形態にかかる非線形システムは、ハイブリッド車の動力源等であってもよい。この例における状態ジャンプは、ハイブリッド車の動力源が、モータとエンジンとの間で切り替わるとき等に、発生し得る。また、例えば、本実施の形態にかかる非線形システムは、電気自動車又はハイブリッド車等のバッテリーであってもよい。この例における状態ジャンプは、バッテリーが充電と放電の間で切り替わるとき等に、発生し得る。

また、例えば、本実施の形態にかかる非線形システムは、自動車等の自動運転システムであってもよい。この例における状態ジャンプは、自車の車線変更又は合流等によって先行車両や後続車両の有無が変化するとき等に、発生し得る。また、この例における状態ジャンプは、物体との衝突が避けられない場合に衝突後の状況まで含めて可能な範囲で最善の動作を行うように制御するとき等に、発生し得る。

また、例えば、本実施の形態にかかる非線形システムは、飛行機等であってもよい。この例における状態ジャンプは、飛行機の離着陸において、接地の前後を含めて運動を最適化するように制御するとき等に、発生し得る。具体的には、所望の経路で着陸しつつ、着陸後すみやかに減速するようにエンジン及び機体を制御するような場合である。

また、例えば、本実施の形態にかかる非線形システムは、列車等であってもよい。この例における状態ジャンプは、列車の連結において、連結の前後を含めて運動を最適化するように制御するとき等に、発生し得る。具体的には、連結時の衝撃及び駆動モータの負荷を軽減するようにモータを制御するような場合である。

上述したような任意の非線形システムについて、図１０で示したような制御方法が実行され得る。この場合、非線形システムの制御方法は、式（６９）〜（７９）で表される停留条件から、式（８０）で表されるベクトルＵ（ｔ）を求め、このベクトルＵ（ｔ）の各成分の値を、式（８１）で示される方程式を解くことによって算出する。これにより、非線形システムを制御するための制御入力値が算出される。そして、図１０に示したフローチャートにおいて、Ｓ１０２は、非線形システムの状態パラメータを取得する取得ステップに対応し、Ｓ１０４〜Ｓ１１４は、非線形システムを制御するための制御入力値を算出する算出ステップに対応し、Ｓ１１６は、非線形システムを制御する制御ステップに対応する。そして、算出ステップにおいて、指定されたタイミングにおいて状態が不連続に変化するように非線形システムの状態を拘束する拘束パラメータを用いて、制御入力値が算出される。このとき、非線形システムの制御周期ごとに、モデル予測制御のアルゴリズムにおける予め定められた評価区間における制御入力値の最適解の変化率が算出され、変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出され、最適解から、現在の前記制御入力値が算出される。

上述の例において、プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１・・・ロボットシステム、２・・・制御装置、１２・・・状態取得部、１４・・・非線形モデル予測制御部、１６・・・サーボ制御部、１００・・・ロボット、１０２・・・胴体、１１０Ｌ・・・左脚、１１０Ｒ・・・右脚、１１２・・・上腿部、１１４・・・下腿部、１１６・・・足部、１１８・・・足裏センサ、１２０・・・股関節部、１２２・・・膝関節部、１２４・・・足首関節部、１３０・・・角度センサ、１３６・・・トルクセンサ、１４０・・・モータ、１５０・・・関節、１５１・・・支持脚リンク、１５２・・・遊脚リンク、２０１・・・支持脚下腿リンク、２０２・・・支持脚上腿リンク、２０３・・・胴体リンク、２０４・・・遊脚上腿リンク、２０５・・・遊脚下腿リンク、２１１，２１２，２１３，２１４，２１５・・・関節

Claims (8)

1. 非線形システムの制御方法であって、
   前記非線形システムの状態を示す状態パラメータを取得する取得ステップと、
   前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記非線形システムを制御するための制御入力値を算出する算出ステップと、
   前記算出された制御入力値を用いて、前記非線形システムを制御する制御ステップと
   を有し、
   前記算出ステップにおいて、指定されたタイミングにおいて状態が不連続に変化するように前記非線形システムの状態を拘束する拘束パラメータを用いて、前記非線形システムの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する
   非線形システムの制御方法。
2. ２つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御装置であって、
   前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する状態取得手段と、
   前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出手段と、
   前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御手段と
   を有し、
   前記算出手段は、指定されたタイミングにおいて前記２つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する
   二足歩行ロボットの制御装置。
3. 前記拘束パラメータは、前記モデル予測制御のアルゴリズムで用いられる評価関数に含まれている
   請求項２に記載の二足歩行ロボットの制御装置。
4. 前記拘束パラメータは、前記タイミングにおいて遊脚が着地したときの前記二足歩行ロボットの姿勢を指定する
   請求項２又は３に記載の二足歩行ロボットの制御装置。
5. 前記拘束パラメータは、前記タイミングにおいて遊脚が着地したときの前記２つの脚の関節部の目標角度を指定する
   請求項４に記載の二足歩行ロボットの制御装置。
6. 前記拘束パラメータは、調整可能なゲインを含む
   請求項２から５のいずれか１項に記載の二足歩行ロボットの制御装置。
7. ２つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御方法であって、
   前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する取得ステップと、
   前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出ステップと、
   前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御ステップと
   を有し、
   前記算出ステップにおいて、指定されたタイミングにおいて前記２つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する
   二足歩行ロボットの制御方法。
8. ２つの脚を用いて二足歩行を行うことが可能な二足歩行ロボットの動作を制御する二足歩行ロボットの制御方法を実現するプログラムであって、
   前記二足歩行ロボットの歩行に関する状態を示す状態パラメータを取得する取得ステップと、
   前記取得された状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用して、前記二足歩行ロボットの動作を制御するための制御入力値を算出する算出ステップであって、指定されたタイミングにおいて前記２つの脚のうちの遊脚が着地するように前記二足歩行ロボットの状態を拘束する拘束パラメータを用いて、前記二足歩行ロボットの制御周期ごとに、前記モデル予測制御のアルゴリズムにおける予め定められた評価区間における前記制御入力値の最適解の変化率を算出し、前記変化率を用いて当該制御周期の次の制御周期における前記制御入力値の最適解を算出し、前記最適解から、現在の前記制御入力値を算出する、算出ステップと、
   前記算出された制御入力値を用いて、前記二足歩行ロボットの動作を制御する制御ステップと
   をコンピュータに実行させるプログラム。