特許 7221833 非線形モデル予測制御装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-02-06

(45)【発行日】2023-02-14

(54)【発明の名称】非線形モデル予測制御装置

(51)【国際特許分類】

   G05B 13/04 20060101AFI20230207BHJP

   G05B 13/02 20060101ALI20230207BHJP

【ＦＩ】

G05B13/04

G05B13/02 J

【請求項の数】 1

(21)【出願番号】P 2019158051

(22)【出願日】2019-08-30

(65)【公開番号】P2021036390

(43)【公開日】2021-03-04

【審査請求日】2022-01-17

【新規性喪失の例外の表示】特許法第３０条第２項適用 平成３０年８月３１日開催の「非線形現象の特徴化に基づく制御理論調査研究会 第５回研究会」にて発表

(73)【特許権者】

【識別番号】504132272

【氏名又は名称】国立大学法人京都大学

(73)【特許権者】

【識別番号】000003207

【氏名又は名称】トヨタ自動車株式会社

(74)【代理人】

【識別番号】100103894

【弁理士】

【氏名又は名称】家入 健

(72)【発明者】

【氏名】大塚 敏之

(72)【発明者】

【氏名】片山 想太郎

(72)【発明者】

【氏名】土井 将弘

【審査官】藤崎 詔夫

(56)【参考文献】

【文献】特開２０１６－１９８８７３（ＪＰ，Ａ）

【文献】特開２０１８－１８５７４７（ＪＰ，Ａ）

【文献】特開平９－２５１３２０（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｂ １３／０４

Ｇ０５Ｂ １３／０２

(57)【特許請求の範囲】

【請求項1】

制御対象の非線形制御モデルの最適化問題を演算しながらフィードバック制御を行うことによって、各時刻において将来の制御対象の応答を予測しながら制御対象の制御を行うことが可能に構成された非線形モデル予測制御装置であって、
繰り返し周囲物との衝突が発生することを前提とする動作を実行制御されるように構成された制御対象に対して、各時刻において当該時刻から所定期間後までにおける当該衝突の回数を予測する予測手段と、
周囲物との衝突が発生することを前提とした前記非線形制御モデルの最適化問題の演算を、予測した衝突回数に応じて実行する演算手段と、
を備える、非線形モデル予測制御装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、非線形モデル予測制御装置に関する。

【背景技術】

【0002】

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

【0003】

フィードバック制御において、二足歩行ロボットは歩行動作等を伴う非線形性の高いシステムであるので、二足歩行ロボットの制御は、非線形モデル予測制御によって行われることが好ましい。

【0004】

非特許文献１には、周囲物との衝突を前提とした制御対象（例えばロボットの脚の動作）の非線形制御モデル制御装置が開示されている。

【先行技術文献】

【非特許文献】

【0005】

【文献】M. Yamakita, A. Taura, and Y. Onodera、「An application of nonlinear receding horizon control to posture control with collisions」、Proceedings of lnternational Conference on Advanced lntelligent Mechatronics、２００５年７月、p.1505-1510

【発明の概要】

【発明が解決しようとする課題】

【0006】

しかしながら、周囲物との衝突（例えばロボットの脚の接地）が将来（次の瞬間）のどのタイミングで発生するかを予測しながら制御することは難しい。そのため、非特許文献１に記載の技術では、周囲物との衝突のタイミングを予め設定した仮の時刻として制御を実行している。よって、将来の実際の衝突タイミングに制御結果がより良く合う高精度な非線形モデル予測制御装置が望まれる。

【0007】

本発明は、上述のような実状に鑑みてなされたものであり、周囲物と衝突することを前提とした制御対象に対し、将来の実際の衝突タイミングに制御結果がより良く合う高精度な非線形モデル予測制御装置を提供することを、その目的とする。

【課題を解決するための手段】

【0008】

本発明にかかる非線形モデル予測制御装置は、制御対象の非線形制御モデルの最適化問題を演算しながらフィードバック制御を行うことによって、各時刻において将来の制御対象の応答を予測しながら制御対象の制御を行うことが可能に構成された非線形モデル予測制御装置であって、繰り返し周囲物との衝突が発生することを前提とする動作を実行制御されるように構成された制御対象に対して、各時刻において当該時刻から所定期間後までにおける当該衝突の回数を予測する予測手段と、周囲物との衝突が発生することを前提とした前記非線形制御モデルの最適化問題の演算を、予測した衝突回数に応じて実行する演算手段と、を備える、ものである。

【0009】

本発明は、上述したように、各時刻において所定期間後までに制御対象が衝突する回数を予測し、その予測した衝突回数に応じて非線形制御モデルの最適化問題を演算している。したがって、本発明にかかる非線形モデル予測制御装置によれば、将来の実際の衝突タイミングに制御結果がより良く合う高精度な予測制御を実行することができる。

【発明の効果】

【0010】

本発明によれば、周囲物と衝突することを前提とした制御対象に対し、将来の実際の衝突タイミングに制御結果がより良く合う高精度な非線形モデル予測制御装置を提供できる。

【図面の簡単な説明】

【0011】

【図1】実施の形態１にかかるロボットシステムを示す概略図である。

【図2】実施の形態１にかかるロボットシステムの構成を示す機能ブロック図である。

【図3】実施の形態１にかかるロボットをコンパス型モデルに適用する方法を説明するための図である。

【図4】実施の形態１にかかるコンパス型ロボットを示す模式図である。

【図5】図４のコンパス型ロボットにおける片脚支持期と両脚支持期の２つの状態を示す模式図である。

【図6】実施の形態１にかかるモデルを示す図である。

【図7】実施の形態１にかかる非線形モデル予測制御の例として、コンパス型モデルの歩行制御をシミュレーションした結果を示す図である。

【図8】実施の形態１にかかる非線形モデル予測制御の例として、コンパス型モデルの歩行制御をシミュレーションした結果を示す図である。

【図9】実施の形態１にかかる非線形モデル予測制御の例として、コンパス型モデルの歩行制御をシミュレーションした結果を示す図である。

【図10】実施の形態１にかかる非線形モデル予測制御の例として、コンパス型モデルの歩行制御をシミュレーションした結果を示す図である。

【図11】実施の形態１にかかる非線形モデル予測制御の例として、コンパス型モデルの歩行制御をシミュレーションした結果を示す図である。

【図12】実施の形態１にかかる非線形モデル予測制御の例として、コンパス型モデルの歩行制御をシミュレーションした結果を示す図である。

【図13】実施の形態１にかかる非線形モデル予測制御の例として、コンパス型モデルの歩行制御をシミュレーションした結果の、ホライゾン上での予測衝突時刻のグラフを示す図である。

【図14】図１３の一部を拡大したグラフを示す図である。

【発明を実施するための形態】

【0012】

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

【0013】

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

【0014】

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

【0015】

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

【0016】

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

【0017】

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

【0018】

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

【0019】

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

【0020】

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

【0021】

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

【0022】

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

【0023】

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

【0024】

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

【0025】

非線形モデル予測制御部１４は、本実施の形態にかかる非線形モデル予測制御装置に相当する部位である。非線形モデル予測制御部１４は、ロボット１００の動作を制御するための制御入力値（入力値）を算出する算出手段としての機能を有する。非線形モデル予測制御部１４は、状態取得部１２からの検出値（及び検出値から得られた値）の少なくとも一部を状態パラメータとして入力することができる。非線形モデル予測制御部１４は、その状態パラメータに基づいて、モデル予測制御のアルゴリズムを使用してロボット１００の動作を制御するための制御入力値を算出する。また、非線形モデル予測制御部１４は、算出された制御入力値をサーボ制御部１６に対して出力する。

【0026】

非線形モデル予測制御部１４は、その状態パラメータに基づいて、非線形モデル予測制御（モデル予測制御）のアルゴリズムを使用して制御対象の動作を制御するための制御入力値を算出し、サーボ制御部１６に出力する。非線形モデル予測制御についての詳細は後述する。また、非線形モデル予測制御部１４は、ロボットシステム１の外部の上位コントローラ（図示せず）によって、必要な指示値（歩幅、歩行周期等）を入力されてもよい。なお、制御対象がロボット以外のものであった場合でも、出力先が制御対象の部位になるだけで基本的に同様である。

【0027】

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

【0028】

次に、本実施の形態にかかる非線形モデル予測制御について説明する。
非線形モデル予測制御とは、非線形システムに対し、各サンプリング時刻で有限時刻未来までの最適入力（制御入力値の最適解）を求め、得られた入力のうち初期値を実際の入力とする制御である。換言すれば、非線形モデル予測制御部１４は、制御対象の非線形制御モデルの最適化問題を演算しながらフィードバック制御を行うことによって、各時刻において将来の制御対象の応答を予測しながら制御対象の制御を行うことが可能に構成されている。非線形モデル予測制御には、非線形最適制御である、フィードバック制御である、及び、拘束条件を組み込み易いという、３つの利点がある。

【0029】

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

【0030】

近年、この問題に対する有効な数値計算法として、Ｃ／ＧＭＲＥＳ（continuation／generalized minimum residual method）法が新たに考案された。Ｃ／ＧＭＲＥＳ法は、連続変形法（continuation method）とＧＭＲＥＳ法とを組み合わせたアルゴリズムである。Ｃ／ＧＭＲＥＳ法は、状態変化が連続であるシステムに対し、最適解の連続性を利用して、最適解の変化率を求めながら最適解を追跡していく計算方法である。このＣ／ＧＭＲＥＳ法を用いることにより、非線形モデル予測制御においても、実時間（リアルタイム）でシステムを制御することが可能となる。つまり、Ｃ／ＧＭＲＥＳ法を用いることで有限時刻未来までの最適制御問題をサンプリング周期内で解くことが可能になった。本実施の形態においても、Ｃ／ＧＭＲＥＳ法での計算を適用することができる。なお、Ｃ／ＧＭＲＥＳ法については後述する。

【0031】

そして、本実施の形態にかかる非線形モデル予測制御部１４は、その主たる特徴として、次の予測手段及び演算手段を備える。この予測手段は、繰り返し周囲物との衝突（周囲環境との接触等）が発生することを前提とする動作（例えば歩行）を実行制御されるように構成された制御対象に対して、各時刻において当該時刻から所定期間後までにおける当該衝突の回数を予測する。この予測手段は、衝突回数予測手段と称することもできる。

【0032】

上記の演算手段は、周囲物との衝突が起こり得ることを前提とした非線形制御モデルの最適化問題の演算を、予測した衝突回数に応じて（予測した衝突回数ごとに区別して）実行する。以下、このような非線形モデル予測制御部１４における制御例について説明する。

【0033】

［制御対象と制御目的］
次に、上述した非線形モデル予測制御を、本実施の形態にかかるロボット１００の動作の制御に適用した例について説明する。なお、実施の形態１においては、制御対象としてのロボット１００がコンパス型モデルである例について説明するが、非線形モデル予測制御は、ロボット１００がコンパス型モデルでなくても適用可能である。

【0034】

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

【0035】

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

【0036】

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

【0037】

関節１５０の質量をｍ０とする。また、図４の矢印で示すように、関節１５０の周りに、制御入力値として入力トルクｕが入力される。ここで、支持脚リンク１５１及び遊脚リンク１５２の物理的性質は、互いに同じであるとする。支持脚リンク１５１及び遊脚リンク１５２の長さを、ｌとする。また、支持脚リンク１５１及び遊脚リンク１５２の質量を、ｍとする。関節１５０から各リンクの重心（重心１５１ｍ及び重心１５２ｍ）までの長さを、ｄとする。支持脚リンク１５１及び遊脚リンク１５２の重心（重心１５１ｍ及び重心１５２ｍ）周りの慣性モーメントを、Ｉとする。

【0038】

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

【0039】

非線形モデル予測制御部１４は、図１のロボット１００で例示でき且つ図４で表されるような２足歩行ロボット１００を最も単純化したモデル（コンパス型モデル）に対し、制御を行うことになる。この制御は、非線形モデル予測制御を用いた実時間での動的歩行制御である。

【0040】

図５は、コンパス型ロボットの片脚支持期と両脚支持期の２つの状態を示す模式図である。図４に示すロボット１００の運動では、図５に示すロボット１００－１，１００－２のようにロボットが２つの状態をとる。ロボット１００－１で示す１つの状態は、片脚が地面９０に着きもう一方の脚が地面９０から離れている状態（片脚支持期）である。ロボット１００－２で示すもう１つの状態は、両脚が地面９０に着いている状態（両脚支持期）である。以下では、このような片脚支持期に地面９０に着いている脚を支持脚、その片脚支持期に地面９０に着いていない脚を遊脚と称する。

【0041】

また、歩行動作について次の４つの仮定を置く。
・遊脚と地面９０との衝突は完全非弾性衝突とする。すなわち、地面９０と衝突した脚が跳ね返ることはない。
・衝突の直後に、それまで支持脚だった脚は相互作用無しで地面９０から離れる。
・遊脚と地面９０との衝突は一瞬とする。すなわち、両脚支持期は一瞬とする。
・衝突の撃力によりロボットの速度は瞬間的に変わるが、座標は瞬間的には変わらないとものする。

【0042】

これらの仮定により、ロボットの歩行動作におけるダイナミクスは、片脚支持期の運動という連続変化、遊脚と地面９０との衝突という不連続変化、という２つの事象に分けることができる。次に、これらの仮定に基づき、２つの方程式を導出する。

【0043】

まず片脚支持期の運動方程式は、一般化座標ｑを

【数1】

ととると、ラグランジュの運動方程式が、

【数2】

と得られる。

【0044】

ここで、τ（ｕ）はジョイント部に対する制御入カトルク、Ｍ（ｑ）は慣性行列、Ｈ（ｑ，ｑ（ドット））は重力とコリオリの力を表す項であり、それぞれ次のような運動方程式で示される通りである。

【数3】

【0045】

これより、ロボットの運動方程式及び状態空間ベクトルｘを

【数4】

とすると、制御に用いるための状態方程式が

【数5】

と得られる。

【0046】

なお、ここで説明するコンパス型モデルでは、歩行の拘束条件として、ＺＭＰ（zero moment point）は考慮されないものとする。

【0047】

次に、衝突の式を導出する。簡単のため、本実施の形態では衝突後にθ_１＝θ_２という座標の取り直しを行う（但しこの際物理的な意味は一切不変である）。衝突の際に物理的な座標は変わらないという仮定から、衝突直前の座標をｑ^－＝［θ_１ ^－ θ_２ ^－］^Τ、直後の座標をｑ^＋＝［θ_１ ^＋ θ_２ ^＋］^Τと置き、正方行列を次式のＩ（バー）で置く。

【数6】

【0048】

すると、次式が成り立つ。

【数7】

【0049】

それぞれ次式で表される衝突直前の速度、衝突直後の速度

【数8】

については、遊脚についての角運動量保存則から次式が成り立つ。

【0050】

【数9】

【0051】

但し、Ｑ^－、Ｑ^＋は下式で示す通りである。

【数10】

【0052】

以上により、衝突前後のコンパス型ロボットの状態の変化は、衝突直前と直後の座標ｘ^－、ｘ^＋を用いて

【数11】

と表すことができる。また、衝突が起きる条件としては遊脚の先端の地面９０からの高さが０になること、すなわち
ψ（ｘ）＝ｌ（ｃｏｓθ_１－ｃｏｓθ_２）＝０ ・・・（Ａ１４）
である。

【0053】

上述の説明から分かるように、本実施の形態で例示するコンパス型歩行モデルは、図６で表されるような一般的なモデルで表すことができる。但し、ｆ_ｋ（ｘ（ｔ），ｕ（ｔ））はｋ歩目の時のコンパス型モデルの状態方程式、ψ_ｋ（ｘ）はｋ歩目の衝突の条件、γ_ｋ（ｘ）はｋ歩目の衝突による状態の不連続変化を表す。また、このモデルは歩行以外にもロボット等が外界との接触を行うモデル全般を表すことができる。

【0054】

［非線形モデル予測制御とＣ／ＧＭＲＥＳ法による実時間制御アルゴリズム］
次に、このように一般化したモデルに対して非線形モデル予測制御を適用する際の最適制御問題の定式化を予測ホライゾン上の衝突の回数ごとに導出するとともに、この非線形モデル予測制御を実時間で実行する実時間制御アルゴリズムについて説明する。

【0055】

ここでは、図６で示した歩行動作を一般化したモデルに対して非線形モデル予測制御を適用する手法を説明する。非線形モデル予測制御（Nonlinear model predictive control；以下ＮＭＰＣ）は、制御対象システムのモデルと、システムの現時刻ｔの状態に基づき制御される。ここでのモデルは、本実施の形態で言えば状態方程式ｘ（ドット）＝ｆｋ（ｘ，ｕ）と衝突の式ｘ＋＝γｋ（ｘ－）となる。具体的には、ＮＭＰＣは、上記モデルと現時刻ｔの状態に基づき、現時刻ｔから未来ｔ＋Ｔまでのシステムの挙動が最適になるような制御入力ｕＯＰＴ（τ）（ｔ≦τ≦ｔ＋Τ）を求める制御則である。また、ＮＭＰＣは、実際のシステムへの制御入力ｕ（ｔ）を、得られた最適制御入力の初期値、すなわちｕ（ｔ）＝ｕｏｐｔ（τ）として与える制御則である。

【0056】

＊最適性条件の導出＊
ここで、システムが衝突のような不連続現象を含む際は、それに応じた最適性条件を導出する必要がある。そこで、ここでは、ホライゾン上での衝突回数に応じた最適性条件を導出する。

【0057】

図６のような状態方程式の切り替えを持つシステムにおいて、状態ｘ∈Ｒ^ｎが状態方程式がｆ_ｋ（ｘ，ｕ）に支配されている時、状態ｘはサブシステムｋに支配され、サブシステムｋがアクティブであると記述する。

【0058】

すなわち、図６のモデルは、サブシステムｋがアクティブな時、状態ｘ∈Ｒ^ｎは

【数12】

に従い、ある条件ψ_ｋ（ｘ）∈Ｒ^ｎについて
ψ_ｋ（ｘ（ｔ_ｋ－））＝０ ・・・（Ａ１６）
を満たすことで、状態ジャンプ（状態の不連続変化）
ｘ（ｔ_ｋ＋）＝γ_ｋ（ｘ（ｔ_ｋ－）） ・・・（Ａ１７）
が起きる。

【0059】

また、アクティブなサブシステムは、サブシステムｋからサブシステムｋ＋１にスイッチし、状態は

【数13】

に支配される。

【0060】

このモデルをＮＭＰＣによって制御する際には、最適性条件、最適制御の必要条件を導出し、それを数値的に解く必要がある。一方、システムが衝突のような不連続事象を含む際はホライゾン上の衝突の数に応じて最適制御の必要条件が変わってくる。したがって、ここでは次の３つの場面、（場面１）ホライゾン上で衝突が起きない時、（場面２）ホライゾン上で衝突が１回起きる時、（場面３）ホライゾン上で衝突が複数回起きる時、についてそれぞれ最適性条件を導出する。

【0061】

つまり、上述のように、非線形モデル予測制御部１４は周囲物との衝突が起こり得ることを前提とした非線形制御モデルの最適化問題の演算を行うが、その際、これらの場面に応じて演算を行うことになる。具体的には次の（場面１）～（場面３）に説明するように、衝突が無い場合から複数回発生する場合（総じて、衝突が起こりえる場合）それぞれにおいて最適化問題の演算を行う。

【0062】

（場面１）ホライゾン上で衝突が起きない時
ホライゾン上で衝突が起きない時、ＮＭＰＣではシステムのモデル（Ａ１５）とシステムの現時刻の状態ｘ（ｔ）に基づき、評価関数

【数14】

が最小になるようなホライゾン上の最適制御入力ｕ_ＯＰＴ（τ）（ｔ≦τ≦ｔ＋Τ）を求める。そして、ＮＭＰＣでは、実際のシステムヘの制御入力をｕ（ｔ）＝ｕ_ＯＰＴ（ｔ）として与える。

【0063】

但し、Ｌ_ｋはサブシステムｋに割り当てられたステージコスト関数、φ_ｋはサブシステムｋに割り当てられた終端コスト以降では変数τ（０≦τ≦Τ）を評価区間上の時間パラメータとして扱い、ｔは定数パラメータとして扱う。また、ｘ^＊（τ；ｔ）、ｕ^＊（τ；ｔ）をそれぞれｘ（ｔ＋τ）、ｕ（ｔ＋τ）に一致するものとして扱う。

【0064】

このとき、ＮＭＰＣの最適制御問題は

【数15】

のもとで、評価関数

【数16】

を最小にするような最適制御入力ｕ^＊（τ；ｔ）（０≦τ≦Τ）を求める問題となる。

【0065】

また、実際のシステムヘの制御入力は、
ｕ（ｔ）＝ｕ^＊（０；ｔ） ・・・（Ａ２３）
として与えられる。

【0066】

次に、この非線形最適制御問題を数値的に解くために最適制御問題を離散化して扱う。ホライゾンＴをＮ分割すると、解くべき最適制御問題は、
ｘ_０ ^＊（ｔ）＝ｘ（ｔ） ・・・（Ａ２４）
ｘ_i+１ ^＊（ｔ）＝ｘ_i ^＊（ｔ）＋ｆ_ｋ（ｘ_i ^＊（ｔ），ｕ_i ^＊（ｔ））Δτ，
ｉ＝０，・・・，Ｎ－１
・・・（Ａ２５）
のもとで、評価関数

【数17】

を最小にするような制御入力ｕ_i ^＊（ｔ）、ｉ＝０，・・・，Ｎ－１を求める問題になる。但し、Δτ＝Τ／Ｎであり、ｘ_i ^＊（ｔ）、ｕ_i ^＊（ｔ）はそれぞれｘ_i ^＊（ｉΔτ；ｔ）、ｕ_i ^＊（ｉΔτ；ｔ）に相当する値である。

【0067】

この問題に対して停留条件、最適性条件を導出すると、

【数18】

が得られる。但し、Ｈ_ｋ（ｘ，λ，ｕ）はサブシステムｋに対するハミルトニアン、
Ｈ_ｋ（ｘ，ｕ，λ）＝Ｌ_ｋ（ｘ，ｕ）＋λ^Ｔｆ_ｋ（ｘ，ｕ） ・・・（Ａ３０）
である。

【0068】

このとき、最適制御問題は、（Ａ２４）－（Ａ２９）を満たすような未知量ｘ_０ ^＊（ｔ），・・・，ｘ_Ｎ ^＊（ｔ）、ｕ_０ ^＊（ｔ），・・・，ｕ_Ｎ－１ ^＊（ｔ）、λ_０ ^＊（ｔ），・・・，λ_Ｎ ^＊（ｔ）を求める問題となる。もしｕ_０ ^＊（ｔ），・・・，ｕ_Ｎ－１ ^＊（ｔ）を知っていればｘ_０ ^＊（ｔ），・・・，ｘ_Ｎ ^＊（ｔ）、λ_０ ^＊（ｔ），・・・，λ_Ｎ ^＊（ｔ）は（Ａ２４）－（Ａ２８）から計算することができる。

【0069】

そこで、本質的な未知量としてＵ（ｔ）を

【数19】

として定義する。

【0070】

このとき、Ｕ（ｔ）についての最適性条件で構成されたベクトルは

【数20】

となる。

【0071】

（場面２）ホライゾン上で衝突が１回起きる時
次に、ホライゾン上で衝突が１回起きる場合について最適性条件を導出する。ホライゾン上の時刻τ_ｋで衝突が起きるとすると、状態について、

【数21】

と予測することができる。

【0072】

このとき、最適制御問題は、これらの状態についての方程式のもとで

【数22】

を最小にするような最適制御入力ｕ^＊（τ；ｔ）（０≦τ≦Τ）を求める問題となる。

【0073】

次に、ホライゾン上で衝突が起きない場合と同様にホライゾン上を離散化して最適性条件を求める。まず、衝突が起きるステップｉ_ｋを
ｉ_ｋΔτ≦τ_ｋ≦（ｉ_ｋ＋１）Δτ ・・・（Ａ３８）
として定義すると、状態の予測は

【数23】

として行うことができる。ここで、Δτ_ｋ１＝τ_ｋ－ｉ_ｋΔτ、Δτ_ｋ２＝Δτ－Δτ_ｋ１である。

【0074】

また、ホライゾン上の状態の初期値はこれまでと同様に
ｘ_０ ^＊（ｔ）＝ｘ（ｔ） ・・・（Ａ４５）
として与えられる。評価関数Ｊは

【数24】

と離散化される。

【0075】

このとき最適制御問題は、（Ａ３９）－（Ａ４５）のもとで評価関数（Ａ４６）を最小にするような制御入力の系列ｕ_i ^＊（ｔ）（ｉ＝０，・・・，Ｎ－１）、ｕ^＊（τ_ｋ＋；ｔ）を求める制御入力を求める問題に帰着される。この問題に対して最適性条件を導出するために、衝突条件（Ａ４１）に対してラグランジュ乗数ν∈Ｒ^ｌを導入する。このとき、最適性条件は次のように得られる。

【0076】

【数25】

【0077】

【数26】

【0078】

このとき、最適制御問題は、（Ａ３９）－（Ａ４５）、（Ａ４８）－（Ａ５７）を満たすようなｘ_０ ^＊（ｔ），・・・，ｘ_ｉｋ ^＊（ｔ），ｘ^＊（τ_ｋ－；ｔ），ｘ^＊（τ_ｋ＋；ｔ），ｘ_ｉｋ＋１ ^＊（ｔ），・・・，ｘ_Ｎ ^＊（ｔ），λ_０ ^＊（ｔ），・・・，λ_ｉｋ ^＊（ｔ），λ^＊（τ_ｋ－；ｔ），λ^＊（τ_ｋ＋；ｔ），λ_ｉｋ ^＊（ｔ），・・・，λ_Ｎ ^＊（ｔ），ｕ_０ ^＊（ｔ），・・・，ｕ_ｉｋ ^＊（ｔ），ｕ^＊（τ_ｋ＋；ｔ），ｕ_ｉｋ＋１ ^＊（ｔ），・・・，ｕ_Ｎ－１ ^＊（ｔ），ν_ｋ ^＊（ｔ），τ_ｋを求める問題に帰着される。

【0079】

これらの未知量のうち、本質的な未知量を次のように定義する。

【数27】

【0080】

但し、Ｕ_ｋ（ｔ）を次のように定義する。

【数28】

【0081】

このような定義により、このＵ（ｔ）が分かっていればｘ_０ ^＊（ｔ），・・・，ｘ_ｉｋ ^＊（ｔ），ｘ^＊（τ_ｋ－；ｔ），ｘ^＊（τ_ｋ＋；ｔ），ｘ_ｉｋ＋１ ^＊（ｔ），・・・，ｘ_Ｎ ^＊（ｔ），λ_０ ^＊（ｔ），・・・，λ_ｉｋ ^＊（ｔ），・・・，λ^＊（τ_ｋ－；ｔ），λ^＊（τ_ｋ＋；ｔ），λ_ｉｋ ^＊（ｔ），・・・，λ_Ｎ ^＊（ｔ）を（Ａ３９）－（Ａ４５）、（Ａ４７）－（Ａ５２）から求めることができる。

【0082】

このときＵ（ｔ）が満たすべき条件は、

【数29】

と得られる。

【0083】

但しＦ_ｋ（Ｕ_ｋ（ｔ），ｔ）は次式で示される。

【数30】

【0084】

（場面３）ホライゾン上で衝突が複数回起きる時
ホライゾンで複数回衝突が起きる場合の最適性条件は、衝突の回数が１回の場合の最適性条件を拡張することで求めることができる。ｋ，・・・，ｋ＋ｌ回目の衝突がホライゾン上の時刻τ_ｋ，・・・，τ_ｋ＋ｌで起きるとする。このとき、（Ａ３８）で求めた衝突が起きるステップｉ_ｋ，・・・，ｉ_ｋ＋ｌについて、（Ａ４０）－（Ａ４３）からｘ^＊（τ_ｋ－；ｔ），ｘ^＊（τ_ｋ＋；ｔ），ｘ_ｉｋ＋１ ^＊（ｔ）を、ｋ＝ｋ，・・・，ｋ＋ｌについて求めることができる。λについても同様に、（Ａ４９）－（Ａ５１）からλ^＊（τ_ｋ－；ｔ），λ^＊（τ_ｋ＋；ｔ），λ_ｉｋ ^＊（ｔ）を各ｉ_ｋ，・・・，ｉ_ｋ＋ｌについて求めることができる。ｉ_ｋ，・・・，ｉ_ｋ＋ｌ以外の全てのｉについては、ｘ_ｉとλ_ｉはそれぞれ（Ａ３９）、（Ａ５２）から計算することができる。

【0085】

このとき本質的な未知量は、

【数31】

として定義することができる。

【0086】

このときＵ（ｔ）が満たすべき条件は次のようになる。

【数32】

【0087】

ここで、Ｆ_ｋは（Ａ６１）であり、Ｆの要素の各ｉ_ｋステップ目については、

【数33】

の代わりに

【数34】

を代用する。

【0088】

＊非線形モデル予測制御の実行アルゴリズム＊
非線形モデル予測制御を行うためには、上述のように求めた最適性条件を数値的に解く必要がある。本実施の形態では短いサンプリング周期内でもロボットのような複雑なシステムの制御を目標とするため、ＮＭＰＣの高速数値計算アルゴリズムであるＣ／ＧＭＲＥＳ法を用いる。

【0089】

＊＊Ｃ／ＧＭＲＥＳ法＊＊
Ｃ／ＧＭＲＥＳ法は連続変形法とＧＭＲＥＳ法を組み合わせた手法である。Ｃ／ＧＭＲＥＳ法では、非線形方程式Ｆ（Ｕ（ｔ），ｘ（ｔ），ｔ）＝０を直接解いてＵ（ｔ）を直接求めるのではない。Ｃ／ＧＭＲＥＳ法では、Ｕ（ｔ）の時間についての連続性を前提として、各サンプリング時刻で最適なＵ（ｔ）を求め、
Ｕ（ｔ＋Δｔ）＝Ｕ（ｔ）＋Ｕ（ｔ）Δｔ ・・・（Ａ６４）
として更新する。但し、Δｔはサンプリング周期である。

【0090】

Ｃ／ＧＭＲＥＳ法ではＵ（ｔ）を求めるために連続変形法を用いてＦ＝０を

【数35】

として変形する。ここで、ζ＞０は安定化パラメータである。この方程式は

【数36】

についての線形方程式としてみなすことができる。そこで、この方程式を線形方程式の高速数値解法であるＧＭＲＥＳ法を用いて解く。以上がＣ／ＧＭＲＥＳ法の簡単な説明である。

【0091】

＊＊Ｃ／ＧＭＲＥＳ法の衝突現象への拡張＊＊
Ｃ／ＧＭＲＥＳ法はＵ（ｔ）の連続性が前提にされている。その一方で、Ｃ／ＧＭＲＥＳ法は、衝突現象を含むシステムに対する問題は不連続現象が伴い、この（Ａ６４）による更新では解の連続性の前提が成り立たなくなる場合がある。この連続性を成り立たせるためには、ホライゾン上でのあるサブシステムｋに割り当てられた制御入力が、同様にあるサブシステムに割り当てられた制御入力によって更新される必要がある。

【0092】

すなわち、ホライゾン上の任意の衝突ｋについて、τ_ｋ（ｔ＋Δｔ）＜ｉ_ｋΔτのとき、τ_ｋ（ｔ＋Δｔ）／Δτ＜ｉ≦ｉ_ｋである各ｉについてｕ_ｉ ^＊（ｔ＋Δｔ）は

【数37】

として更新する。

【0093】

同様に、（ｉ_ｋ＋１）Δτ＜τ_ｋ（ｔ＋Δｔ）のとき、ｉ_ｋ＜ｉ＜τ_ｋ（ｔ＋Δｔ）／Δτを満たす各ｉについて、ｕ_ｉ ^＊（ｔ＋Δｔ）を

【数38】

として更新する。

【0094】

もう１つの修正点としてヤコビ行列とベクトルの前進差分近似がある。式（Ａ６５）のヤコビ行列は、あるベクトルＷ∈Ｒ^ｍＮ、ｗ∈Ｒ^ｎ、ω∈Ｒ、十分小さな実数ｈ＞０を用いて、

【数39】

として前進差分近似される。

【0095】

この差分近似も時刻ｔとｔ＋ｈにおける予測ホライゾン上のサブシステムについての連続性を前提としている。その一方で、
ｉ_ｋΔτ≦τ_ｋ（ｔ）＜ｉ_ｋΔτ＋ｈ ・・・（Ａ６９）
のとき、この連続性が成り立たなくなる。

【0096】

そこで、そのような場合は

【数40】

として後退差分近似を行う。但し、ｈを十分小さくしておけば、（Ａ６９）はほとんど起こらない。

【0097】

＊＊スイッチによって追加される変数の初期化アルゴリズム＊＊
次に、衝突によって追加される変数の初期化アルゴリズムについて説明する。
もし時刻ｔ－Δｔのホライゾン上にスイッチｋが存在しないが、時刻ｔには存在する場合、すなわちスイッチｋが時刻ｔにホライゾン上に現れる場合、次のようになる。すなわち、このような場合、時刻ｔ－Δｔのときの最適制御問題には含まれていなかった新たな変数ｕ^＊（τ_ｋ＋；ｔ）、ν_ｋ ^＊、τ_ｋが時刻ｔに追加されることになる。このとき新たな変数ｕ^＊（τ_ｋ＋；ｔ）、ν_ｋ ^＊、τ_ｋは前の時刻で求めてはいないため、Ｃ／ＧＭＲＥＳ法によりこれらを求めることができない。また、スイッチｋがＮ－１ステップ以前のｉ_ｋで起きたとき、それまでサブシステムｋについて最適に求められたｕ_ｉｋ ^＊（ｔ），・・・，ｕ_Ｎ－１ ^＊（ｔ）をサブシステムｋ＋１について最適に求め直す必要がある。但し、サンプリング周期を十分小さくしている限り、ほとんどのケースでは、ｉ_ｋ＝Ｎ－１であり、このとき初期化する変数はｕ^＊（τ_ｋ＋；ｔ）、ν_ｋ ^＊、τ_ｋだけでよい。このとき、本来であればホライゾン全体についてニュートン法などを用いてもう１度最適制御問題を解く必要があるが、計算時間が膨大になってしまうという問題点がある。そこで、少ない計算時間で部分的に最適な値を求めることでこれらの値を初期化する手法を採用するとよい。以下に、そのような手法について説明する。

【0098】

まず、ホライゾン上で衝突条件ψ＝０が満たされたことを観測したとする。すなわち、あるステップで初めて

【数41】

を観測したときを考える。このとき、部分的に初期化を行うためにまずｉ_ｋステップ目までの制御入力、状態の系列は最適であると仮定する。すなわち、ｉ_ｋステップ目以降での最適な変数を求める問題を定義し解くことでこの追加された変数の初期化を行う。以降では、ｉ_ｋ＝Ｎ－１のとき、ｉ_ｋ＜Ｎ－１の２通りに分けて記述を行う。

【0099】

ｉ_ｋ＝Ｎ－１のとき：
このとき、もしτ_ｋ、ｕ^＊（τ_ｋ＋；ｔ）、ν_ｋ ^＊が分かっていれば、既に計算されたｘ_Ｎ－１ ^＊（ｔ）、ｕ_Ｎ－１ ^＊（ｔ）を用いて、

【数42】

が計算できる。

【0100】

ここで解く最適制御問題は、（Ａ７２）－（Ａ７５）のもとで、Ｎ－１ステップ以降の評価関数

【数43】

を最小にするようなｕ^＊（τ_ｋ＋；ｔ）、ν_ｋ ^＊、τ_ｋを求める問題である。

【0101】

この問題に対して最適性条件を導出すると、

【数44】

が得られる。

【0102】

この初期化の実行は以下のように行われる。ここではホライゾン上で初めてψ_ｋ（ｘ_ｉ ^＊（ｔ））＜０となるのがｉ＝Ｎである場合を考えているため、まずτ_ｋをψ_ｋ（ｘ_Ｎ－１ ^＊（ｔ））とψ_ｋ（ｘ_Ｎ ^＊（ｔ））から

【数45】

として与える。次に、ｕ^＊（τ_ｋ＋；ｔ）とν_ｋ ^＊（ｔ）とについて、ニュートン法を行うための適当な初期推定解を与える。

【0103】

そして求めたい未知量を

【数46】

とする。

【0104】

また、Ｕ_{ｋ，ｉｎｉｔ}（ｔ）が満たすべき条件を

【数47】

とする。

【0105】

上述した求めたい未知量及びＵ_{ｋ，ｉｎｉｔ}（ｔ）が満たすべき条件に対して、前進差分ニュートンＧＭＲＥＳ法による反復を行う。次のアルゴリズム１では、このＵ_{ｋ，ｉｎｉｔ}（ｔ）についてこの初期化をまとめている。

【0106】

［アルゴリズム１：Ｕ_{ｋ，ｉｎｉｔ}（ｔ）の初期化（ｉ_ｋ＝Ｎ－１のとき）］
１：τ_ｋを（Ａ９８）により初期化する。
２：ｕ^＊（τ_ｋ＋；ｔ）とν_ｋ ^＊（ｔ）に適当な初期推定解を代入する。
３：ｘ^＊（τ_ｋ－；ｔ），ｘ^＊（τ_ｋ＋；ｔ），ｘ_Ｎ ^＊（ｔ），λ_Ｎ ^＊（ｔ），λ^＊（τ_ｋ＋；ｔ）を求め、Ｆ_{ｋ，ｉｎｉｔ}（Ｕ_{ｋ，ｉｎｉｔ}（ｔ），ｔ）を計算する。
４：ｗｈｉｌｅ ｜Ｆ_{ｋ，ｉｎｉｔ}（Ｕ_{ｋ，ｉｎｉｔ}（ｔ），ｔ）｜＜τ_ｉｎｉｔ ｏｒ ｉ＜ｉ_ｍａｘ ｄｏ
５：前進差分Ｎｅｗｔｏｎ－ＧＭＲＥＳ法をＦ_{ｋ，ｉｎｉｔ}（Ｕ_{ｋ，ｉｎｉｔ}（ｔ），ｔ）に用いることでΔＵ_{ｋ，ｉｎｉｔ}を求める。
６：Ｕ_{ｋ，ｉｎｉｔ}（ｔ）をＵ_{ｋ，ｉｎｉｔ}（ｔ）←Ｕ_{ｋ，ｉｎｉｔ}（ｔ）＋ΔＵ_{ｋ，ｉｎｉｔ}と更新する。
７：ｘ^＊（τ_ｋ－；ｔ），ｘ^＊（τ_ｋ＋；ｔ），ｘ_Ｎ ^＊（ｔ），λ_Ｎ ^＊（ｔ），λ^＊（τ_ｋ＋；ｔ）を求め、Ｆ_{ｋ，ｉｎｉｔ}（Ｕ_{ｋ，ｉｎｉｔ}（ｔ），ｔ）を計算する。
８：ｅｎｄ ｗｈｉｌｅ
９：Ｕ_{ｋ，ｉｎｉｔ}（ｔ）の初期化終わり。

【0107】

ｉ_ｋ＜Ｎ－１のとき：
このとき解くべき最適制御問題とは、

【数48】

のもとで、

【数49】

を最小にするようなｕ^＊（τ_ｋ＋；ｔ）、ｕ_ｉｋ＋１ ^＊（ｔ），・・・，ｕ_Ｎ－１ ^＊（ｔ）、ν_ｋ ^＊、τ_ｋを求める問題である。

【0108】

この問題に対して最適性条件を導出すると、次のようになる。

【数50】

【0109】

ｉ_ｋ＝Ｎ－１の場合と同様に、ｘ_ｉｋ ^＊（ｔ），ｕ_ｉｋ ^＊（ｔ）がこの初期化問題の境界条件として与えられる。この初期化の実行はｉ_ｋ＝Ｎ－１のときと同様である。まず、τ_ｋをψ_ｋ（ｘ_ｉｋ ^＊（ｔ））とψ_ｋ（ｘ_ｉｋ＋１ ^＊（ｔ））から

【数51】

として与える。

【0110】

次に、ｕ（τ_ｋ＋；ｔ）とν_ｋ ^＊（ｔ）に適当な初期推定解を与える。ｕ_ｉｋ＋１ ^＊（ｔ），・・・，ｕ_Ｎ－１ ^＊（ｔ）については前のサンプリング時刻で求めた値をそのままＮｅｗｔｏｎ－ＧＭＲＥＳ法の初期推定解として用いる。求める未知量を

【数52】

とする。

【0111】

Ｕ_{ｋ，ｉｎｉｔ}（ｔ）が満たすべき条件は

【数53】

となる。

【0112】

上述した求めたい未知量及びＵ_{ｋ，ｉｎｉｔ}（ｔ）が満たすべき条件に対して、前進差分ニュートンＧＭＲＥＳ法による反復を行う。次のアルゴリズム２では、このＵ_{ｋ，ｉｎｉｔ}（ｔ）についてこの初期化をまとめている。

【0113】

［アルゴリズム２：Ｕ_{ｋ，ｉｎｉｔ}（ｔ）の初期化（ｉ_ｋ＜Ｎ－１のとき）］
１：τ_ｋを（Ａ９８）により初期化する。
２：ｕ^＊（τ_ｋ＋；ｔ）とν_ｋ ^＊（ｔ）に適当な初期推定解を代入する。
３：ｘ^＊（τ_ｋ－；ｔ），ｘ^＊（τ_ｋ＋；ｔ），ｘ_ｉｋ＋１ ^＊（ｔ），・・・，ｘ_Ｎ ^＊（ｔ），λ_Ｎ ^＊（ｔ），・・・，λ_ｉｋ＋２ ^＊（ｔ），λ^＊（τ_ｋ＋；ｔ）を求め、Ｆ_{ｋ，ｉｎｉｔ}を計算する。
４：ｗｈｉｌｅ ｜Ｆ_{ｋ，ｉｎｉｔ}（Ｕ（ｔ），ｘ（ｔ），ｔ）｜＜τ_ｉｎｉｔ ｏｒ ｉ＜ｉ_ｍａｘ ｄｏ
５：前進差分Ｎｅｗｔｏｎ－ＧＭＲＥＳ法をＦ_{ｋ，ｉｎｉｔ}（Ｕ（ｔ），ｘ（ｔ），ｔ）に用いることでΔＵ_{ｋ，ｉｎｉｔ}を求める。
６：Ｕ_{ｋ，ｉｎｉｔ}（ｔ）をＵ_{ｋ，ｉｎｉｔ}（ｔ）←Ｕ_{ｋ，ｉｎｉｔ}（ｔ）＋ΔＵ_{ｋ，ｉｎｉｔ}と更新する。
７：ｘ^＊（τ_ｋ－；ｔ），ｘ^＊（τ_ｋ＋；ｔ），ｘ_ｉｋ＋１ ^＊（ｔ），・・・，ｘ_Ｎ ^＊（ｔ），λ_Ｎ ^＊（ｔ），・・・，λ_ｉｋ＋２ ^＊（ｔ），λ^＊（τ_ｋ＋；ｔ）を求め、Ｆ_{ｋ，ｉｎｉｔ}を計算する。
８：ｅｎｄ ｗｈｉｌｅ
９：Ｕ_{ｋ，ｉｎｉｔ}（ｔ）の初期化終わり。

【0114】

［数値シミュレーション］
次に、上述した手法を用いて具体的にコンパス型モデルの歩行制御の数値シミュレーションを実行した結果を示す。以下に説明するシミュレーションは、本実施の形態にかかる非線形モデル予測制御のアルゴリズムを、図４で例示したコンパス型モデルにかかるロボット１００に適用したものである。

【0115】

＊評価関数＊
まず、コンパス型歩行制御に用いる際の評価関数を設定する。継続的な歩行制御を実現するために、振り足を前に出す動作を評価関数として加えることを考える。すなわち、遊脚を前に出す速度

【数54】

を、適当な目標値ν_ｒｅｆに近づけるような項

【数55】

を評価関数に加える。また、その際に遊脚が地面から高く上がりすぎて非効率な穂動きをしないようｑ_２（θ_１＋θ_２）^２も加える。最後に、使用エネルギーの少ない自然な歩行を実現するため、ｒｕ^２を加える。

【0116】

以上から、評価関数は、

【数56】

となる。また、本実施の形態では、終端コストについてφ（ｘ）＝０としている。

【0117】

＊シミュレーション条件＊
シミュレーションに用いたコンパス型モデルの物理パラメータは、ｍ_０＝ｍ＝１．０［ｋｇ］、ｌ＝１．０［ｍ］、ｄ＝０．５［ｍ］、Ｉ＝０．０８３３３［ｋｇ・ｍ^２］として与える。

【0118】

シミュレーション条件としては、（Ａ６）に基づく状態の初期値はｘ（０）＝［－０．１４ ０．１４ ０．５０ ０．５８］^Ｔ、シミュレーション中のモデルの状態の更新及びサンプリング周期はΔｔ＝０．００１［ｓ］とする。評価関数内のパラメータは、ｑ_１＝ｑ_２＝１．０、ｒ＝０．５、ν_ｒｅｆ＝０．５［ｒａｄ／ｓ］とする。ホライゾンの長さはＴ（ｔ）＝Ｔ_ｆ（１－ｅ^－αｔ）、α＝１．０、Ｔ_ｆ＝０．８［ｓ］とし、評価区間の分割数は、Ｎ＝８０とする。また、差分近似（Ａ６８）、（Ａ７０）の差分は、ｈ＝１．０×１０^－８として与える。

【0119】

＊シミュレーション結果＊
図７～図１２は、本実施の形態にかかる非線形モデル予測制御の例として、コンパス型モデルの歩行制御をシミュレーションした結果を示す図である。図１３はその歩行制御におけるホライゾン上での予測衝突時刻のグラフを示す図で、図１４は図１３の一部を拡大したグラフを示す図である。

【0120】

図７ではθ_１の変化を、図８ではθ_２の変化を、図９ではθ_１（ドット）の変化を、図１０ではθ_２（ドット）の変化を、図１１ではｕの変化を、図１２では｜｜Ｆ｜｜の変化を、それぞれ示している。

【0121】

図１２における｜｜Ｆ｜｜（エラーノルム）は、（Ａ３２），（Ａ６１），（Ａ６３）で示す各場面におけるＦ（Ｕ（ｔ），ｘ（ｔ），ｔ）の大きさ、すなわち最適解からの現在の解の誤差を表す。｜｜Ｆ｜｜が他の点と比べ大きくなっている点がある。これは、θ_１、θ_２が垂直になっている時刻、すなわち実際の制御対象が地面と衝突を起こしている時刻と一致している。よって、これは衝突によってホライゾン上の最適性条件（場面１～３について説明した最適性条件）が変わったために生じたと考えられる。その点を考慮すると、図７～図１２で示すこのシミュレーション結果は、制御入力が滑らかな挙動となっているのが分かる。

【0122】

図１３においては、ｔ_ｋ＝０になっている時刻では、評価区間上に衝突を検出していないことを表している。また、このシミュレーションで設定した評価区間長さでは、ホライゾン上において１回の衝突のみが起こっていた。図１４では、各時刻でホライゾン上の衝突時刻が最適化されていることが分かる。また、サンプリング周期を１［ｍｓ］として行った本シミュレーションで、ＮＭＰＣの１サンプリングあたりの更新時刻は０．８［ｍｓ］前後であり、実時間での歩行制御に成功しているのが分かる。

【0123】

［本実施の形態の特徴について］
上述したように、本実施の形態では、その主たる特徴の一つとして、上記予測手段が制御対象に対して、各時刻において当該時刻から所定期間後までにおける当該衝突の回数を予測する。

【0124】

このような未来の衝突回数の予測について簡単に補足説明する。この予測は制御周期ごとに実行され、上述のような最適制御では求解の過程で未来（予測区間内）の各時刻における状態も予測されることになる。よって、今回の最適制御を実行する前に、前回の最適制御で予測された未来の状態に対して例えば（Ａ３３）～（Ａ３６）を適用することで、未来の衝突回数を予測することができる。また、上述のようにこの予測は制御周期ごとに実行されるため、予測区間（ホライゾン）の長さを一定とすると、歩行速度が上がる程、予測される衝突回数が増え、減速する程、予測される衝突回数が減少することになる。

【0125】

そして、上述したように、本実施の形態では、上記演算手段は、周囲物との衝突が起こり得ることを前提とした非線形制御モデルの最適化問題の演算を、予測した衝突回数に応じて（予測した衝突回数ごとに区別して）実行する。この演算について簡単に補足説明する。

【0126】

本実施の形態では、最適制御問題の中に衝突時刻を組み込んでいる。すなわち、本実施の形態では、「最適性条件の導出」において場面１～３ごとの制御について例示したように、衝突回数の予測に基づいて上記場面１～３のいずれを用いるか（どの最適制御の必要条件を用いるか）により最適制御問題の切り替えを行っている。なお、衝突時刻を最適化問題に組み込まない、即ち衝突時刻を変更しない場合は、所定の衝突時刻に衝突が起こるように定式化を行い、通常通りＣ／ＧＭＲＥＳ法を用いて制御を行うことができる。

【0127】

但し、単純に衝突時刻も最適制御問題に組み込むと、衝突回数によって解くべき最適制御問題が変化するため、前回の求解結果を用いたＣ／ＧＭＲＥＳ法のような手法では前回の求解結果を得ることが困難となる場合がある。これは、衝突回数が前回と今回の制御周期で異なっている場合には、前回解いた最適制御問題が今回解こうとしている最適制御問題と異なるためである。具体的には、衝突回数が変わると最適制御問題自体が変わるため、解ベクトルも異なった形になる。例えば衝突条件に対するラグランジュ定数ｖと衝突時間τなど、前回の最適化問題では解ベクトルに含まれていなかった変数が今回の最適化問題には含まれるようになる。

【0128】

したがって、本実施の形態として説明したように、最適性条件を切り替えるだけでなく、この切り替えに伴って新たに加わる制御の変数を初期化することが望ましい。この初期化は、前回の求解結果を基にして解ベクトルを初期化するものとなっている。「スイッチによって追加される変数の初期化アルゴリズム」において例示したように、この切り替えに伴って新たに加わる制御の変数とは例えばｕ^＊（τ_ｋ＋；ｔ）、ν_ｋ ^＊、τ_ｋを指す。

【0129】

このように、本実施の形態では、衝突回数の予測に基づいて最適制御問題を演算し、前回解いた最適制御問題の解を利用して、今回の最適制御問題の初期値を適切に設定することが好ましい。

【0130】

以上に説明したように、本実施の形態は、各時刻において所定期間後までに制御対象が衝突する回数を予測し、その予測した衝突回数に応じて非線形制御モデルの最適化問題を演算している。すなわち、本実施の形態では、繰り返し衝突が発生することを前提とする場合において、例えば同じ期間内に衝突が発生する回数ごとに変えて非線形制御モデルを最適化する。

【0131】

よって、本実施の形態によれば、個々の衝突におけるタイミングの予測と実際のずれが小さくなる確率が上がり、また、統計的に見るほど予測と実際のずれはより無くなっていく。これにより、本実施の形態にかかる非線形モデル予測制御装置は、将来の実際の衝突タイミングに制御結果がより良く合う高精度な予測制御を実行することができる。

【0132】

換言すれば、本実施の形態では、最適制御の予測区間上での衝突の回数に応じた定式化を行い、動作の制御に関する制御入力と衝突時刻を同時に最適化しており、それにより、状態に応じて適応的且つ最適な動作が生成可能となる。このような最適制御手法は、任意時刻での切り替えに対応した形で最適制御問題を定式化するとともに、切り替え時刻も制御変数に含めて制御入力と同時に最適化することにより、切替タイミングと予測区間の制御入力を同時最適化する手法と言える。

【0133】

よって、本実施の形態によれば、状態に応じて適切な切り替えタイミングと制御入力を発生できるため、制御対象のより効率的で安定な動作を実現できる。例えば、切り替えのタイミングを予め決めておく場合には、制御の最適性を満たすことは困難となり、どのような状態であっても決められた時刻に衝突を行おうとするため、不自然な動作になったり、電力が大きくなるような入力が必要となったりしてしまう。これに対し、本実施の形態では適切な切り替えタイミングを発生させることができるため制御の最適性を満たすことができる。例えば、歩行動作を例に考えると、外乱が生じて定常的な歩行動作から状態が外れた場合、動作軌道だけでなく、次の足が着地するタイミングも変更することで無理なく（より少ないトルクで）定常状態に戻すことができる。

【0134】

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

【0135】

また、上述した実施の形態においては、非線形システムが二足歩行ロボットである例について説明した。しかし、本実施の形態にかかる非線形モデル予測制御のアルゴリズムは、二足歩行ロボット以外の非線形システムについても適用可能であり、またコンパス型モデルでなくても適用可能である。

【0136】

つまり、本実施の形態にかかる非線形システムの制御方法は、以下に例示するような、繰り返し周囲物との衝突が発生することを前提とする任意の非線形システムに対して、適用可能である。なお、一見して周囲物との衝突が繰り返し起こりそうもないような制御対象であっても、周囲物との衝突が繰り返し起こり得るような制御を意図的に行うことはあり得るため、本実施の形態は、そのような制御を行う場合にも有益となる。

【0137】

例えば、ロボットに設けたロボットハンド又はロボットアーム等を制御対象とすることもできる。この例における衝突は、ロボットハンド又はロボットアームが、周辺環境又は操作対象等の物体を押圧するとき、物体を把持し又は離すとき、球体等の物体を叩く又は打ち返すとき等に、発生し得る。なお、球体等の物体を打ち返す非線形システムの例として、例えば、卓球ロボットがある。

【0138】

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

【0139】

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

【0140】

また、例えば、本実施の形態での制御対象は、加工機械の工具等であってもよい。この例における衝突は、加工機械の工具が、加工対象等の物体に接触し又は離れるとき等に、発生し得る。

【0141】

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

【0142】

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

【0143】

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

【0144】

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

【符号の説明】

【0145】

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

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】