JP6421683B2 - 最適制御装置、最適制御方法及び最適制御プログラム - Google Patents

Description

本発明は、移動ロボットのモデル予測制御を行う最適制御装置、最適制御方法及び制御プログラムに関するものである。

例えば、ロボットの機械的リンク系に対してモデル予測制御（リシーディングホライゾン制御）を行う最適制御装置が知られている（特許文献１参照）。

特開２０００−３３０６０９号公報

上記モデル予測制御では、ロボットの物理的な制約条件が設定される。そして、制御周期毎にこの制約条件付き最適化問題を求解し、その求解した最適解に基づいてロボットの重心軌道を生成することとなる。しかし、この最適解の求解において、従来、多大な時間を要するという、問題が生じていた。

本発明は、このような問題点を解決するためになされたものであり、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる最適制御装置、最適制御方法及び制御プログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する接触点計画手段と、前記接触点計画設定手段により設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する軌道生成手段と、を備える最適制御装置であって、前記軌道生成手段は、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、前記軌道生成手段は、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御装置である。
この一態様において、前記軌道生成手段は、前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築し、前記評価基準は、前記各接触点に対応して設定された重みに基づいて前記各接触点に前記接触力と、前記接触力の微分値とを配分するという基準が含まれ、前記接触力および接触力の微分値の二乗和を含む評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力の微分値の入力と前記重心の状態変数と関係を示す状態方程式と、前記移動ロボットの力の釣合いの拘束を示す等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換されてもよい。
この一態様において、前記等式制約条件を示す式に対してＱＲ分解を行って状態変数の変換式が導出され、前記接触力の微分値の入力と重心の状態変数との関係を示す状態方程式から導出した式に対してＱＲ分解を行って入力の変換式が導出され、前記状態方程式と、前記状態変数の変換式と、前記入力の変換式と、前記状態変数の変換式と、に基づいて状態方程式の変換式が導出され、前記導出した状態変数の変換式と、入力の変換式と、等式制約条件付き最適化問題の評価関数と、に基づいて、評価関数の変換式が導出され、
前記無制約条件の最適化問題は、前記導出された評価関数の変換式と、前記状態方程式の変換式と、を含んでいてもよい。
この一態様において、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して、再帰的計算法を用いて最適解を求解し、前記求解した最適解と、前記等式制約条件を示す式をＱＲ分解して導出した状態変数の変換式と、に基づいて前記重心の状態変数の時系列データを算出してもよい。
この一態様において、前記等式制約条件は、所定の区間内だけ前記接触力が変化しないように設定した入力を含んでいてもよい。
この一態様において、前記軌道生成手段は、前記等式制約条件と前記接触点の安定性の拘束を示す不等式制約条件とを含む等式制約条件及び不等式制約条件付き最適化問題を直交補空間を用いて変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数の時系列データを算出してもよい。
この一態様において、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対してニュートン法を適用し、該ニュートン法の収束演算の中で前記再帰的計算法を用いてニュートン方向を算出し、該算出したニュートン方向に基づいて、最適解を算出してもよい。
この一態様において、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して内点法又はアクティブセット法を適用してもよい。
この一態様において、前記不等式制約条件は、所定の区間内だけ前記接触力に制限をかけるように設定した入力を含んでいてもよい。
この一態様において、前記最適化問題の状態方程式は、線形時変の制御パラメータを含んでいてもよい。
この一態様において、前記軌道生成手段により生成された重心軌道に基づいて前記移動手段を制御する制御手段を更に備えていてもよい。
上記目的を達成するための本発明の一態様は、二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定するステップと、前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成するステップと、を含む最適制御方法であって、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御方法であってもよい。
上記目的を達成するための本発明の一態様は、二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する処理と、前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する処理と、をコンピュータに実行させる最適制御プログラムであって、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、 前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御プログラムであってもよい。

本発明によれば、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる最適制御装置、最適制御方法及び制御プログラムを提供することができる。

移動ロボットの動作の一例を示す図。である。 移動ロボットの機械構成の一例を示す図である。 移動ロボットの機能ブロック図である。 ６軸力を示す図である。 最適制御装置の機能ブロック図である。 接触点計画の概要の一例を示す図である。 接触点計画の一例を示す図である。 予測区間の例を示す図である。 予測区間での動きを表わした図である。 予測区間のシフトを説明するための図である。 予測に用いる移動ロボットのモデルを示す図である。 予測区間の離散化を説明するための図である。 直交補空間のイメージ図である。 無制約条件のＬＱ最適化問題に変換する際のフローを示す図である。 最適制御方法を示すフローチャートである。 接触点の座標系と接触多角形とを示す図である。 接触点が不安定化する場合を例示した図である。 等式制約条件及び不等式制約条件付きＬＱ最適化問題の最適解の求解フローを示すフローチャートである。

本発明の実施形態を図示するとともに図中の各要素に付した符号を参照して説明する。
（第１実施形態）
本実施形態は移動ロボットの最適制御装置に特徴があり、具体的には、移動ロボットの移動動作（図１）を制御するための軌道生成に特徴を有するのであるが、具体的な制御（軌道生成）を説明する前に、制御対象となる移動ロボットのハードウェア構成について予め説明しておく。

図２は、移動ロボットの機械構成の一例を示した図である。
移動ロボット１００は、股関節が３軸、膝関節が１軸、足首関節が２軸、さらに、肩関節が３軸（肩ピッチ、肩ロール、肩ヨー）、肘関節が１軸（肘ピッチ）、および、手首関節が３軸（手首ヨー、手首ピッチ、手首ロール）、で夫々構成されている。

（移動ロボットの機械構成はこれに限定されないが、手（腕）の自由度は６以上、足（脚）の自由度も６以上は必要である。）
移動ロボット１００は、各関節にエンコーダ付きモータ１、２、・・・、２８を有している。
各関節のモータ１ａ、２ａ、・・・、２８ａ（図３）は、各関節の関節角度θ１、θ２、・・・、θ２８を調整できる。
一方、各関節のエンコーダ１ｂ、２ｂ、・・・、２８ｂは、各関節の関節角度θ１、θ２・・・、θ２８を計測することができる。

また、移動ロボット１００は、足先部（足平部）および手先部（手の平部）に接触力センサ２５を有している。
ここで接触力とは６軸力であり、図４に示すように、ｘ軸、ｙ軸およびｚ軸方向の力ｆの組（ｆ_ｘ、ｆ_ｙ、ｆ_ｚ）^Ｔと、ｘ軸回り、ｙ軸回りおよびｚ軸回りの力τの組（τ_ｘ、τ_ｙ、τ_ｚ）^Ｔと、である。
（なお、ｘ軸およびｙ軸は、鉛直方向であるｚ軸に垂直な面内で互いに直交する軸とする。）

この移動ロボットは、移動時に、右足、左足、右手および左手のうちの一つ以上を床、壁、あるいはテーブルなどに接触させながら移動する。
そこで、本明細書の以下の説明では、右足、左足、右手および左手を接触点候補と称することがある。また、手先、足先というのは、移動手段の一具体例である。

図３は、移動ロボット１００の機能ブロック図である。
移動ロボット１００は、各関節のモータ１ａ〜２４ａ及びエンコーダ１ｂ〜２４ｂと、接触力センサ２５と、最適制御装置２１０と、を備えている。

最適制御装置２１０には、各関節のエンコーダ１ｂ〜２４ｂ及び接触力センサ２５から、センサ検出値が入力される。また、最適制御装置２１０は、各関節のモータ１ａ〜２４ａに対して駆動信号を出力する。

最適制御装置２１０は、主要なハードウェア構成として、制御処理、演算処理等を行うＣＰＵ（Central Processing Unit）２１０ａと、ＣＰＵ２１０ａによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）２１０ｂと、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）２１０ｃと、を有するマイクロコンピュータにより構成されている。また、これらＣＰＵ２１０ａ、ＲＯＭ２１０ｂ、及びＲＡＭ２１０ｃは、データバス２１０ｄによって相互に接続されている。必要なプログラムを不揮発性記録媒体に記録しておき、必要に応じてインストールするようにしてもよい。

図５は、本発明の一実施形態に係る最適制御装置２１０の機能ブロック図である。本実施形態に係る最適制御装置２１０は、移動ロボット１００の接触点計画を設定する接触点計画設定部（接触点計画手段の一具体例）２２１と、安定に実行できる移動ロボット１００の重心軌道を生成する軌道生成部（軌道生成手段の一具体例）２２２と、生成された重心軌道に従って移動ロボット１００の全身動作を実行させる動作制御部（制御手段の一具体例）２２３と、を有する。

ここで、軌道生成部２２２は、接触点計画に従った動作を実行できる重心軌道を生成するのであるが、この重心軌道生成には必要に応じた接触点変更を含む。
これら機能部の具体的な処理動作については後述する。

（多点接触移動のための軌道生成方法）
本実施形態に係る軌道生成部２２２は、（１）多点接触移動を実現できる重心軌道を生成し、かつ、（２）必要に応じて接触点の変更を行っている。ここで、（１）多点接触移動を実現できる重心軌道を生成するための方法を説明する。なお、本出願人は、特願２０１３−２５４９８９（平成２５年１２月１０日出願）においてこの方法を出願している。

そもそも、将来の目標重心位置を予め知ることはできないのであり、制御目標値として未知であるはずの将来の重心位置をユーザが設定するというのは無理がある。ユーザとしてはロボットに接触点の計画情報だけを与え、あとは、移動ロボットが設定された接触点の計画情報に基づいて自動的に安定な重心軌道を生成して自律的に移動してくれることが望ましい。

さて、移動ロボットに多点接触移動を安定して行わせるためには、時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成する技術が必要である。

このために本実施形態に係る軌道生成部２２２は、モデル予測制御（所謂リシーディングホライゾン制御：Receding Horizon Control）を用いて重心軌道を生成する。
最初にモデル予測制御の概要を説明しておく。

（モデル予測制御の概要説明）
例えば、図１に図示したような移動動作を移動ロボットに行わせたいとする。
ここでは、２本の腕と２本の脚とを有する人型の移動ロボットに、テーブルの奥側にあるボトルを掴ませるという一連の動作を想定する。

この場合、接触点計画設定部２２１は、ユーザから指令される接触点の計画情報に基づいて、この一連動作（タスク）を実行できるような接触点計画を作成する。
つまり、接触点計画設定部２２１は、例えば、図６のように、手先および足先を、どの順番で、どこに、どのように、着くか、という計画を作成する。
図６においては、床、壁およびテーブルにおいて足先および手先を接触させる箇所にマークを付けている。

この接触点計画は、具体的には図７のようになる。
接触点計画は、左手（ＬＨ）、右手（ＲＨ）、左足（ＬＦ）および右足（ＲＦ）に関し、どの順番で、どこに、どのように、着いていくか、という時系列のデータである。

図１、図６および図７の対応関係を簡単に説明する。
当初（ｔ０）左足１本だけで立ち、遊脚である右足を前に振り出し、そして、右足を着地させる（ｔ１）。
この動きに従った接触点計画を移動ロボット１００に実行させるためには、左足が最初に着地している床上の接触点の座標Ｐ_ＬＦ１、そのときの左足の姿勢ｒ_ＬＦ１、そして、右足が着地する床上の接触点の座標Ｐ_ＲＦ１、そのときの右足の姿勢ｒ_ＲＦ１、を指定することが必要である。

ここで、接触点の座標は、空間座標としてＰ＝（Ｐ_ｘ、Ｐ_ｙ、Ｐ_ｚ）の組で表わされる。
また、姿勢というのは、接触点に着地したときの足の裏面の向きであり、例えばオイラー角の組としてｒ＝（ｒ_ｘ、ｒ_ｙ、ｒ_ｚ）として表わされる。
（すなわち、ｒ_ｘ、ｒ_ｙおよびｒ_ｚは、ロール、ピッチおよびヨー角をそれぞれ表わす。）
足に関する接触点の座標およびそのときの姿勢を指令する形式は今後の説明でも同様なので、以後は適宜説明を省略する。

両足で立った後、左足を振り出し（ｔ_２）、左足を前方に着地する（ｔ_４）。
その間に、左手を壁に着くようにする（ｔ_３）。
ここで、左手を着く壁上の接触点の座標Ｐ_ＬＨ１、および、そのときの左手の姿勢ｒ_ＬＨ１を指定する。

この接触点の座標は空間座標としてＰ＝（Ｐ_ｘ、Ｐ_ｙ、Ｐ_ｚ）の組で表わされ、姿勢は接触点に着いたときの手の平の向きとしてオイラー角の組としてｒ＝（ｒ_ｘ、ｒ_ｙ、ｒ_ｚ）として表わされる。

これ以降の接触点計画は図１、図６および図７を対比して頂ければ自明と思われるので省略する。
このようにして、接触点計画設定部２２１は、接触点計画を時系列のデータとして作成する。

軌道生成部２２２は、上記のように接触点計画設定部２２１により設定された接触点計画を実現するように重心軌道を生成する。動作制御部２２３は、軌道生成部２２２により生成された重心軌道に従って移動ロボット１００の全身動作させるように、各関節のモータ１ａ〜２４ａを制御する。これにより、移動ロボット１００は、設定された接触点計画に基づいて安定な重心軌道に従って、自律的に移動できる。

このとき、軌道生成部２２２は、軌道生成にあたってモデル予測制御を行う。
すなわち、軌道生成部２２２は、ある時間幅を持った予測区間内で移動ロボット１００が安定移動できる軌道を生成し、予測区間を微小時間（Δｔ）ずつシフトさせながら安定動作を行える軌道を順次更新していくようにする。

例えば、図８に予測区間の例を示す。
軌道生成部２２２は、現在から所定時間（例えば１．６秒）先の未来までを予測区間として設定する。

そして、軌道生成部２２２は、この予測区間の間で発散しないように安定な軌道を生成する。
この予測区間での動きをイメージしたものが図９である。
このように、軌道生成部２２２は、ある時間幅を持つ予測区間で安定な軌道を生成した上で、最初の一点だけを現在の入力値として使用する。

軌道生成部２２２は、次の軌道更新周期（Δｔ秒後）には予測区間をシフトさせ、新たな予測区間において同様に安定な軌道を生成する（図１０参照）。

現在だけ、あるいは、現在から次ぎの制御周期（Δｔ秒）まで、だけを見るのではなく、上記のように、ある程度の未来までを予測区間とし、この予測区間内で発散しない軌道が生成されるようにする。
これを繰り返すことで移動ロボットは安定に移動することができる。

さて、ここで問題なのは、ある時間幅を持った予測区間のなかで時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成するにはどのようにすればよいか、ということである。

本発明者らは、ある予測区間における安定軌道の生成問題をＬＱ（Linear Quadratic）最適化問題（凸二次計画問題：Quadratic Programming: QP）に帰着させるという着想を得た。
具体的には、軌道生成部２２２は、各接触点における接触力の二乗和と、前記６軸力（接触力）の微分値の二乗和と、を含む評価関数Ｊを最小化するというＬＱ最適化問題を解くことで、多点接触移動の安定軌道を求める。

そこで、次に、この評価関数Ｊの導出およびその解法（ＬＱ最適化問題への帰着）を説明する。
この解法により、ある予測区間内で安定な多点接触移動を実現するための、重心位置、重心速度、接触力および接触力の微分値の時系列データが得られることを示す。（ここからの説明では、まず、接触点計画で指示された通りの位置（接触点）に手足を着くことだけを考える。なお、必要に応じて、スラック変数などを導入し条件式や評価式を緩和するなどの処置を行って接触点を変更してもよい。

予測に用いる移動ロボットのモデルを改めて図１１に示す。
移動ロボット全体の慣性を一つの重心Ｇで表わす。各接触点には６軸力を定義する。

この時、重心Ｇの並進運動量をＰ、重心回りの回転運動量（角運動量）をＬ、接触点の数をｎとすると、運動方程式は次のように書ける。

添え字ｉは接触点のインデックスを表す。
例えば接触点の候補が左手、右手、左足、右足の４点であれば、ｎ＝４（左手：ＬＨ＝１、右手：ＲＨ＝２、左足：ＬＦ＝３、右足：ＲＦ＝４）とすればよい。ただし、床や壁に接触していない接触点候補については接触力を０にするように拘束条件を設定しておく。例えば図１１の例であれば次のようにする。

（１）式の第１式、第２式を微分すると次の式が得られる。（（１）式はベクトルで表現しているが、これをｘ、ｙ、ｚに分解した上で、上から順に第１式、第２式・・・第６式と称する。）

本実施形態では、この２式をシステムとして用いる。そして、（１）式の第３から第５式を拘束条件として定式化する。

さらに、予測区間内を図１２のように、Ｎ個の区間に分割し、（３）式、（４）式を離散化する。（３）式を離散化すると次のようになる。

また、サンプリング点で常に（４）式の拘束が成り立つとすると、（４）式は次のように離散化される。

ここで、パラメータを次ぎのように置く。

θ_ｉは、６軸力としての接触力を並べたベクトルである。そして、ｘは、重心Ｇのｘ座標、重心Ｇのｘ軸方向速度、重心Ｇのｙ座標、重心Ｇのｙ軸方向速度、および、各接触点における接触力（６軸力）、を並べたベクトルである。このｘを、重心の状態変数ｘと称する。さらに、ｕは、接触力（６軸力）の微分値を並べたベクトルである。

このようにパラメータを設定すると、（５）式を次ぎの状態方程式として記述することができる。

この（８）式は、（ｊ＋１）のときの状態変数ｘを、その一つ前の状態で記述できる。（８）式を用いて予測区間内の状態変数ｘを順に計算していくと次のようになる。

したがって、時系列的に求められる状態変数ｘを並べて大文字のＸで表わすと、状態変数の時系列データＸを次のように表わすことができる。

この（１０）式は、接触力の微分値（ｕ［ｋ］）を入力として、ある予測区間内における移動ロボットの状態遷移を表わす予測モデルとなる。なお、上記（１０）式において、接触力を入力してもよい。この場合、状態変数ｘは、重心位置と重心速度のみを含むこととなる。また、上記（３）式は、Ｇ（２ドット）（２階微分）とｆとの関係式となり、この関係式と、上記（５）式のｆ（ドット）の項を０にした式とから、上記（８）式のような線形の状態方程式が導出できる。
さて、ここで、本発明者らは、予測区間内において安定な軌道を生成するために次ぎのような評価関数Ｊの評価基準を導入した。

なお、Ｑ_ｉ、Ｒ_ｉは、適宜設定した重みである。例えば、接触点候補すべてに力を均等配分した場合、Ｑ_ｉはすべて１となり、Ｒ_ｉはすべて１×１０^−６と設定できる。

ここで、θ_ｉは、６軸力としての接触力の成分を並べたベクトルであった。したがって、上記（１１）式は、「予測区間内で、接触力（６軸力）と接触力の微分値との２乗和を最小化する」という意味の式である。上記（１１）式の第１項は、接触力（６軸力）の２乗和を最小化することを意味する。

この第１項には、次の作用が含まれている。
（１）各接触点への接触力を均等分配すること。これにより、重心をできる限り安定な位置に動かすという効果がある。
（２）不必要な内力を打ち消すこと。
（３）接触点の接地安定性を高めること。すなわち、接触面内の反力中心点を接触面の中心に設定するという効果がある。

また、上記（１１）式の第２項は、接触力（６軸力）微分値（６軸力の時間変化率）の２乗和を最小化することを意味する。

この第２項には次の作用が含まれている。
（１）重心の発散を抑制すること。
（２）滑らかに接触力を切り替えていくこと。

これらをＱ、Ｒという重みによって適切に足し合わせることによって、この評価関数Ｊを最小化するということは、
「高い接触安定性、滑らかな接触力遷移、最低限の内力、といった条件を満たしながら、安定な重心軌道と各接触点の接触力とを出力する」
ということを意味することとなる。

上記（１１）式を離散化し一般的な形式に書き換えると、次の評価関数（１２）式が得られる。

次に、移動ロボットの力の釣合いの拘束を示す等式制約条件（拘束制約条件）について考える。
等式制約条件としては、
（１）移動ロボットの非接触の接触点候補に対して６軸力が０という拘束、
（２）移動ロボットの鉛直方向の力の釣り合いの拘束、および、
（３）移動ロボットのｘｙ軸回りのモーメント力の釣り合いの拘束、
が予測区間の全サンプリング点に渡って成り立つ必要がある。

ここで、例えば、あるサンプリング点ｋにおいて、ｉ番目とｉ＋２番目の接触点が非接触であったとする。
この時、上記等式制約条件（１）乃至（３）は、下記（１３）式のように記述できる。

なお、係数行列Ｃ_ｋ、ｄ_ｋの成分はサンプリング点によって異なり、接触点候補の接触／非接触といった情報や接触点位置は接触点計画設定部２２１によって設定される。例えば、上記（１３）式のｐ_ｉｘ［ｋ］、ｐ_ｉｙ［ｋ］、ｐ_ｉｚ［ｋ］は、接触点計画設定部２２１によって設定される。

以上から、現在の状態量（状態変数の初期値）をｘ_０とすると、上記（８）式、（１２）式、及び（１３）式より、最適制御装置２１０の軌道生成部２２２は、下記（１４）式に示す等式制約条件付きＬＱ最適化問題を求解し、重心軌道を生成することとなる。

なお、上記（１４）式において、１行目の式（ｍｉｎＪ＝・・）は、上述の如く、予測区間内において、接触力と接触力の微分値との２乗和を最小化するという意味の式である。２行目の式（ｘ［ｋ＋１］＝・・）は、接触力の微分値の入力と重心の状態変数と関係を示す状態方程式である。３行目の式（Ｃ_ｋｘ［ｋ］＝ｄ_ｋ）は、移動ロボットの力の釣合いの拘束を示す等式制約条件である。

ところで、上述のように移動ロボットの最適制御装置は、多点接触で安定的な動作軌道を生成するためにモデル予測制御を行っている。このモデル予測制御では、移動ロボットの物理的な制約条件（上述の等式制約条件）が設定される。そして、最適制御装置は、制御周期毎にＬＱ最適化問題を求解し、その求解した最適解に基づいて制御を行なっている。しかし、この最適解の求解において、従来、多大な時間を要し、モデル予測制御の周期（軌道更新の周期）に遅延が生じ、制御性能を上げることができないという問題が生じていた。

これに対し本実施形態においては、直交補空間を用いて等式制約条件付きのＬＱ最適化問題を無制約条件のＬＱ最適化問題に変換する。そして、最適制御装置２１０の軌道生成部２２２は、この変換した無制約条件のＬＱ最適化問題をリカッチ型再帰的計算法（Riccati recursion）を用いて解き、最適解を求解する。そして、軌道生成部２２２は、求解した最適解に基づいて重心の状態変数を算出し、該算出した重心の状態変数に基づいて重心軌道を生成する。
直交補空間を用いて無制約条件のＬＱ最適化問題に変換することで、その求解に高速かつ安定的なリカッチ型再帰的計算法を用いることができる。これにより、モデル予測制御においてＬＱ最適化問題の最適解を高速に求解し重心軌道を生成できる。

なお、上記Riccati recursionは、最適化問題を行列表現した式に変換し、その変換した行列表現の式の最適解条件（ＫＫＴ（Karush-Kuhn- Tucker） 条件）を示す式に対して再帰的計算を行うことにより、最適化問題の最適解を高速に求解するものである。詳細な計算方法については、既に、非特許文献（Parallel Implementation of Riccati Recursion for Solving Linear-Quadratic, Gianluca Frison John Bagterp Jorgensen）などに開示されており、これを援用できるものとする。

ここで、最初に、上述した直交補空間について詳細に説明する。直交補空間は、以下（１）−（３）のように定義される。
（１）２つの部分空間Ｖ及びＵの基底｛ｖ_ｉ｝^ｋ _ｉ＝１および｛ｕ｝^ｍ _ｉ＝１に含まれるベクトルが線形独立であるとき、基底｛ｖ_ｉ∈Ｒ^ｎ｝^ｋ _ｉ＝１∪で張られる部分空間をＶとＵの直和（direct sum）といい、Ｕ（＋）Ｖと表記する。以下、○の中に＋を（＋）と表記する。特に、Ｒ^ｎ＝Ｒ^ｋ＋ｍ＝Ｖ（＋）Ｕが成立するとき、ＵをＶの補空間（complement）という。
（２）部分空間Ｖ⊂Ｒ^ｎと部分空間Ｕ⊂Ｒ^ｎとが、_ｖＴ_ｕ＝０ for all ｖ ∈ Ｖ、all u ∈ Ｕ を満たすとき、２つの部分空間は直交するという。
（３）部分空間Ｖとその補空間Ｕが直交するとき、ＵをＶの直交補空間（orthogonal complement）といい、Ｖ^⊥と表記する。
上記定義に基づいて下記命題（４）−（５）が成立する。
（４）線形独立なｍ（＜ｎ）個のベクトル｛ｙ_ｉ｝^ｍ _ｉ＝１と直交するベクトル集合α＝｛ｘ∈Ｒ^ｎ｜ｙ^Ｔ _１ｘ＝ｙ^Ｔ _２ｘ＝・・・＝ｙ^Ｔ _ｍｘ＝０｝は、ｎ−ｍ次元部分空間である。
（５）非直交基底｛ｕ_ｉ∈Ｒ^ｎ｝^ｎ _ｉ＝１からｍ個選択された基底ベクトルによって張られる部分空間Ｖ＝＜ｕ_１、ｕ_２、・・・、ｕ_ｍ＞の直交補空間は、その双直交基底｛ｖ_ｉ∈Ｒ^ｎ｝^ｎ _ｉ＝１によって、Ｖ^⊥＝＜ｖ_ｍ＋１、ｖ_ｍ＋２、・・・、ｖ_ｎ＞で表される。

上記命題を簡略して説明すると、Ｃ_ｋ∈Ｒ^{ｍｋ×ｎｘ}の直交補空間Ｃ^⊥ _ｋ∈Ｒ^{ｎｘ×（ｎｘ−ｍｋ）}とは、ｎ_ｘ×ｎ_ｘの線形空間のうち、Ｃ_ｋの残りの空間（補空間）でＣ_ｋに直交する空間である。この直交補空間を用いて上記（１４）式のＬＱ最適化問題を変換することで、図１３に示す如く、等式制約条件Ｃ_ｋｘ＝ｄ_ｋ上に存在するｘを、Ｃ_ｋに平行なベクトルζと直交しＣ_ｋに終端する定数ベクトルσで表すことができる。換言すると、直交補空間を用いて、ｘをζに変数変換することで、ζをどのように動かしても必ず等式制約条件Ｃ_ｋｘ＝ｄ_ｋは満たされることとなる。このため、この等式制約条件を考慮することなく無制約条件でＬＱ最適化問題を求解できる。

次に、上述した直交補空間を用いた変換方法（以下、直交補空間変換と称す）について詳細に説明する。
本実施形態において、例えば、下記（１５）式に示すＱＲ分解（直交行列Ｑと上三角形行列Ｒの積に分解）を用いて直交補空間変換を行うことができる。

以上から、等式制約条件付きＬＱ最適化問題を直交補空間に投影することで、直交補空間変換を行い無制約条件のＬＱ最適化問題を次のように導出する。
まず、等式制約条件を示す上記（１３）式（Ｃ_ｋｘ［ｋ］＝ｄ_ｋ）をＱＲ分解することで、状態変数ｘの変換式である下記（１６）式が導出される。

次に上記状態方程式（８）式の左からＣ_ｋ＋１を掛けると下記（１７）式が導出される。
Ｃ_ｋ＋１ｘ［ｋ＋１］＝Ｃ_ｋ＋１Ａｘ［ｋ］＋Ｃ_ｋ＋１Ｂｕ［ｋ］ ・・・（１７）
さらに、上記（１７）式に上記（１３）式を代入して下記（１８）式を導出する。
Ｃ_ｋ＋１Ａｘ［ｋ］＋Ｃ_ｋ＋１Ｂｕ［ｋ］＝ｄ_ｋ＋１ ・・・（１８）
（ｋ＝０、１、・・・、Ｎ−１）

上記変換と同様に、Ｃ_ｋ＋１Ｂの直交補空間を用いて変数変換を行う。
Ｃ_ｋ＋１Ｂを下記（１９）式に示すようにＱＲ分解する。

上記（１９）式を用いて上記（１８）式を変換し（ＱＲ分解を行い）、入力ｕの変換式である下記（２０）式を導出する。

但し、上記（２０）式における各パラメータを下記（２１）式に示すように設定する。

ｋ＝０のときは、上記（２０）式における各パラメータを下記（２２）式に示すように設定する。

上記（１６）式の左からＤ^Ｔ _ｋを掛けて下記（２３）式を導出する。

但し、上記（２３）式において、正規直交性から下記（２４）式が成立する。

以上より、上記（８）式を上記（１６）式、（２０）式、及び（２３）式を用いて変形し、状態方程式の変換式である下記（２５）式を導出する。

但し、上記（２５）式における各パラメータを下記（２６）式に示すように設定する。

ｋ＝０のときは、上記（２５）式における各パラメータを下記（２７）式に示すように設定する。

また、上記（１６）式及び（２０）式を用いて、上記（１２）式に示す評価関数ＪのΣの項は、下記（２８）式に示すように変形できる。

但し、ｋ＝０のときは、下記（２９）式が成立する。

また、ｋ＝Ｎのときは、下記（３０）式が成立する。

上記（１６）式及び（２０）式を用いて上記（１２）式に示す評価関数Ｊを変形し、評価関数の変換式である下記（３１）式を導出する。

但し、上記（３１）式における各パラメータを下記（３２）式に示すように設定する。

以上のように、等式制約条件付きＬＱ最適化問題に対して直交補空間変換を行い、下記（３３）式に示す無制約条件のＬＱ最適化問題を導出できる。すなわち、直交補空間変換を行うことで、上記（１４）式に示す等式制約条件付きＬＱ最適化問題を、下記（３３）式に示す無制約条件のＬＱ最適化問題に変換できる。本実施形態に係る軌道生成部２２２は、下記（３３）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解を高速に求解できる。

次に、上記直交補空間変換により変換した無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて求解する方法を説明する。
まず、上記（３３）式を行列表現すると、下記（３４）式及び（３５）式のように表現できる。

上記（３４）式及び（３５）式の最適解条件（ＫＫＴ条件）は、下記（３６）式となる。但し、下記（３７）式は、上記（３５）式のラグランジュ乗数である。

軌道生成部２２２は、上記（３６）式に示す式に対して、次のように、再帰的計算を行うことで、上記無制約条件のＬＱ最適化問題を高速かつ安定的に求解する。
まず、軌道生成部２２２は、上記（３６）式の行列内の各パラメータの順番を入れ替えることで、下記（３８）式のように表現する。

そして、軌道生成部２２２は、上記（３８）式に対して、下記（３９）式に示す再帰計算を繰り返す。

上記再帰計算を繰り返すことで、上記（３８）式は、下記（４０）式のように変形される。

さらに、軌道生成部２２２は、上記（４０）式に対して、下記（４１）式に示す再帰計算を行うことで、上記（３３）式に示すＬＱ最適化問題の最適解ζを高速で求解する。

最後に、軌道生成部２２２は、上記求解した最適解ζと、上記（１６）式及び（２０）式（下記２式）と、を用いて、上記（１４）式に示す等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する。
ｘ［ｋ］＝Ｄ_ｋζ［ｋ］＋ｅ_ｋ
ｕ［ｋ］＝Ｎ_ｋζ［ｋ］＋Ｍ_ｋｖ［ｋ］＋ｌ_ｋ

軌道生成部２２２は、算出したｘ［ｋ］（重心Ｇのｘ座標、重心Ｇのｘ軸方向速度、重心Ｇのｙ座標、重心Ｇのｙ軸方向速度、および、各接触点における接触力（６軸力） ）の時系列データに基づいて、重心軌道を生成する。このようにして、予測区間内において、等式制約条件を満たし、かつ評価関数Ｊを最小化する重心軌道が高速に生成される。すなわち、予測区間内において移動ロボットの安定な移動を実現する重心軌道を高速に生成することができる。

図１４は、上述した直交補空間変換を行った上記（３３）式に示す無制約条件のＬＱ最適化問題に変換する際のフローを示す図である。
等式制約条件の上記（１３）式に対してＱＲ分解を行って、状態変数の変換式である上記（１６）式が導出される（ステップＳ１０１）。

上記入力（接触力（６軸力）の微分値）ｕと重心の状態変数ｘとの関係を示す状態方程式（８）式から導出した（１８）式に対してＱＲ分解を行って、入力ｕの変換式である上記（２０）式が導出される（ステップＳ１０２）。

状態方程式（８）式を、導出された状態変数ｘの変換式（１６）式、入力ｕの変換式（２０）式、及び、状態変数ｘの変換式（１６）式から導出した（２３）式を用いて変形し、状態方程式の変換式である（２５）式が導出される（ステップＳ１０３）。

上記導出した状態変数ｘの変換式（１６）式と、入力ｕの変換式（２０）式に基づいて、上記（１２）式に示す評価関数を変形し、評価関数の変換式である上記（３１）式が導出される（ステップＳ１０４）。変換後の無制約条件のＬＱ最適化問題は、上述の如く、上記導出された評価関数の変換式（３１）式と、状態方程式の変換式（２５）式と、を含むこととなる。

図１５は、本実施形態に係る最適制御装置による最適制御方法を示すフローチャートである。
接触点計画設定部２２１は接触点計画（等式制約条件のＣ_ｋ及びｄ_ｋ）を設定する（ステップＳ２０１）。

軌道生成部２２２は、接触点計画設定部２２１により設定された接触点計画に基づいて、上記（３３）式のＬＱ最適化問題を行列表現し、その最適解条件に対して再帰的計算を行うことで、ＬＱ最適化問題の最適解ζを求解する（ステップＳ２０２）。

軌道生成部２２２は、求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（１４）式に示す等式制約条件付きＬＱ最適化問題のパラメータを復元し、重心の状態変数ｘ［ｋ］及び入力ｕ［ｋ］を算出する（ステップＳ２０３）。
軌道生成部２２２は、算出したｘ［ｋ］の時系列データに基づいて、重心軌道を生成する（ステップＳ２０４）。

動作制御部２２３は、軌道生成部２２２により生成された重心軌道に従って移動ロボット１００の全身動作させるように、各関節のモータ１ａ〜２４ａを制御する（ステップＳ２０５）。

以上、本実施形態において、軌道生成部２２２は、等式制約条件付き最適化問題を直交補空間を用いて変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて重心の状態変数を算出し、算出した重心の状態変数に基づいて重心軌道を生成する。これにより、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる。

（第２実施形態）
本実施形態において、軌道生成部２２２は、上記等式制約条件に加えて不等式制約条件を加えたＬＱ最適化問題を求解する。ここで、接触点の安定性の拘束を示す不等式制約条件について説明する。

移動ロボットの接触点が安定して接触を保つ為の不等式制約条件を導入する。
図１６に接触点の座標系（上添え字ｌ（エル）がついている）と、接触多角形（接触点の支持多角形）と、を示した。

接触点の座標系は、接触点を原点とし、かつ、接触面の姿勢ｒ_ｉに合わせて定義されているとする。
ここで、接触点の座標系で定義される接触力（６軸力）θ_ｉ ^ｌを次のように表わす。
θ_ｉ ^ｌ＝［ｆ_ｉｘ ^ｌ、ｆ_ｉｙ ^ｌ、ｆ_ｉｚ ^ｌ、τ_ｉｘ ^ｌ、τ_ｉｙ ^ｌ、τ_ｉｚ ^ｌ］^Ｔ

すると、接触力（６軸力）θ_ｉ ^ｌは、接触面の姿勢行列Φ_ｉ＝ｒｏｔ（ｒ_ｉ）を用いて下記（４２）式のように表現できる。
なお、ｒｏｔは、オイラー角を姿勢行列に変換する関数である。

接触点が安定して接触を保つ為には、
（１）接触点が離れないこと、
（２）接触点が滑らないこと、
（３）接触点が剥がれないこと、
という３つの制約条件を満たす必要がある。
上記３つの制約条件が理解しやすいように、図１７に、接触点が不安定化する場合を例示した。

（１）接触点が離れない為には、接触面の鉛直力が正であれば良い。即ち、下記（４３）式を満たす必要がある。

（２）接触点が滑らない為には、接触面に平行な２軸力が摩擦力以下であれば良い。即ち下記（４４）式がその条件である。ただし接触面の摩擦係数をμ_ｉとする。

（３）接触点が剥がれない為の条件は、接触多角形のｈ個の頂点座標
（ｘ_ｉ１ ^ｌ， ｙ_ｉ１ ^ｌ），・・・・・（ｘ_ｉｈ ^ｌ， ｙ_ｉｈ ^ｌ）
を用いて下記（４５）式のように表される。
（ただし接触多角形の頂点は反時計回りに順に与えられているとする）。

以上、（４３）、（４４）、（４５）式をまとめると次のようになる。

（４６）式に（４２）式を代入し、ｋ番目のサンプリング点としてインデックスを付け加える。すなわち、下記（４７）式は、ｋ番目の接触点が安定な接触を維持するための条件式である。したがって、安定な多点接触動作を実現するためには、全サンプリング点の全接触点において下記（４７）式が成立する必要がある。

ｋ番目のサンプリング点において全接触点が上記（４７）式を満足する為の条件は、下記（４８）式のように表現できる。

（不等式制約条件）
（Ｐ_ｋｘ［ｘ］≦ｑ_ｋ）・・・（４９）
なお、上記不等式制約条件の一般式（４９）式の右辺ｑ_ｋをｑ_ｋ＝Ｏと置けば、上記導出した（４８）式と一致する。上記（１４）式に示す等式制約条件付きＬＱ最適化問題に上記（４８）を加えることで、下記（５０）式に示す等式制約条件及び不等式制約条件付きＬＱ最適化問題が導出される。

本実施形態において、上記実施形態１で行った直交補空間変換に加えて、さらに、上記（４９）式に示す不等式制約条件の変換を行う。具体的には、上記（１６）式を上記（４９）式に代入することで、下記（５１）式を導出する。

以上から、本実施形態において、直交補空間変換を行うことで、上記（５０）式に示す等式制約条件及び不等式制約条件付きＬＱ最適化問題を、下記（５２）式に示す無制約条件のＬＱ最適化問題に変換できる。本実施形態に係る軌道生成部２２２は、下記（５２）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解を求解する。

次に、上記直交補空間変換により変換した無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて求解する方法を説明する。
まず、上記実施形態１と同様に、上記（５２）式を行列表現すると、下記（５３）式乃至（５５）式のように表現できる。

下記（５６）式に示す、上記（５４）式及び（５５）式のラグランジュ乗数を導入する。

続いて、下記（５７）式に示すように、上記（５５）式にスラック変数を導入する。

上記（５６）式及び（５７）式の導入により、上記（５３）式乃至（５５）式に示すＬＱ最適化問題の最適解条件（ＫＫＴ）は、下記（５８）式で表現できる。

上記（５３）式乃至（５５）式は、凸２次計画問題と称されるＬＱ最適化問題であり。内点法やアクティブセット法などの既知の求解法（収束演算）を用いて効率的に求解できる。これら求解法はニュートン法をベースにした求解法であり、ニュートン法の収束演算中でニュートンの方向計算を行い、リカッチ型再帰的計算法による連立一次方程式の求解を行うこととなる。また、凸２次計画問題の計算量の大部分は、この連立一方程式の計算が占めているため、この計算の高速化が非常に有効となる。

上述の如く、直交補空間変換を行うことで、上記（５０）式に示す等式制約条件及び不等式制約条件付きＬＱ最適化問題を、上記（５２）式に示す無制約条件のＬＱ最適化問題に変換する。これにより、このＬＱ最適化問題の連立一次方程式の求解に、安定かつ高速のリカッチ型再帰的計算法を用いることができる。したがって、ＬＱ最適化問題の最適解を高速に求解できる。

本実施形態において、軌道生成部２２２は、リカッチ型再帰的計算法による連立一次方程式の求解を、例えば、内点法やアクティブセット法などの収束演算の中で行う。以下、本実施形態において、内点法を用いた求解法を説明するがこれに限定されない。アクティブセット法を用いた求解法も、内点法と同様の手法で求解できる。

内点法は、上記（５８）式をニュートン法とラインサーチにより効率的に解くことにより、最適解を求解する手法である。なお、本実施形態においては、内点法の中で、最もスタンダードな主双対内点法を用いる場合について説明するが、これに限定されない。

まず、上記（５８）式にニュートン法を適用すると、下記（５９）式及び（６０）式が導出される。但し、下記（６０）式において、（○の中に×）は要素同志の積を意味し、Λ≒ｄｉａｇ（λ）、Ｚ≒ｄｉａｇ（ｚ）とする。

次に、下記（６１）式及び（６２）式に示すようにcomplementary measure μとステップ幅α_ｐを導入する。

ここで、complementary measure μは収束演算の残差の総計、ステップ幅α_ｐはλ≧０、ｚ≧０を満足する範囲でニュートン方向への最大のスッテップ幅を求めていると理解すると分かり易い。

なお、主双対内点法のアルゴリズムを簡略して記載すると以下のようになる。

軌道生成部２２２は、上記主双対内点法を用いて収束演算を行い、最適解ζを求解する。軌道生成部２２２は、上記実施形態１と同様に、上記求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（５０）式に示す等式制約条件及び不等式条件付きＬＱ最適化問題のパラメータを復元し、その最適解であるｘ［ｋ］及びｕ［ｋ］を得る。

ここで、主双対内点法のアルゴリズム内に示した上記（５９）式の求解について詳細に説明する。まず、下記（６３）式が成立する。

上記（６３）式を用いて、上記（５９）式は、下記（６４）式のように表現できる。

ここで、Θ≒Ｚ^−１Λ及びｒ_ｚ′≒ｒ_ｚ−Ｚ^−１ｒ_λと置くと、上記（６４）式は、下記（６５）式のように表現できる。

さらに、上記行列（６５）式の各係数を並べ替えると、下記（６６）式のように表現できる。

ここで、下記（６７）式が成立する。

上記（６７）式を用いて、上記（６６）式は、下記（６８）式のように表現できる。

但し、上記（６８）式において、パラメータを下記（６９）式のように設定している。

上記（６８）式は、上述した実施形態１のリカッチ型再帰的計算法で示した（３８）式と同様の形となっている。したがって、軌道生成部２２２は、上記（６８）式に示す連立１次方程式についても、上記実施形態１と同様に、リカッチ型再帰的計算法を用いて高速かつ安定的に求解できる。

すなわち、軌道生成部２２２は、上記（６８）式に対して、下記（７０）式に示す再帰計算を繰り返す。

さらに、軌道生成部２２２は、下記（７１）式に示す再帰計算を行うことで、（Δｖ_ｋ、Δζ_ｋ、Δｙ_ｋ）を算出する。

軌道生成部２２２は、算出したΔζ_ｋを上記（６７）式に代入することで、Δｚ_ｋを算出する。軌道生成部２２２は、算出したΔｚ_ｋ＝[Δｚ_１ ^Ｔ、Δｚ_２ ^Ｔ、・・・Δｚ_Ｎ ^Ｔ]^Ｔを上記（６３）式に代入することで、Δλを算出する。以上により、上記（５９）式の求解が完了する。

図１８は、上述した等式制約条件及び不等式制約条件付きＬＱ最適化問題の最適解の求解フローを示すフローチャートである。
まず、軌道生成部２２２は、解ベクトルの初期解（η＝η_０、ｙ＝ｙ_０、ｚ＝ｚ_０、λ＝λ_０）を行う（ステップＳ３０１）。

軌道生成部２２２は、繰返パラメータｎ＝０を設定する（ステップＳ３０２）。
軌道生成部２２２は、上記（６０）式を用いて、残差［ｒ_η、ｒ_ｙ、ｒ_ｚ、ｒ_λ］を算出する（ステップＳ３０３）。

軌道生成部２２２は、リカッチ型再帰的計算法によるニュートン方向［Δη、Δｙ、Δｚ、Δλ］の計算を行う（ステップＳ３０４）。
軌道生成部２２２は、上記（６２）式を用いて、ステップ幅α_ｐを算出する（ステップＳ３０５）。

軌道生成部２２２は、上記算出したニュートン方向［Δη、Δｙ、Δｚ、Δλ］とステップ幅α_ｐとに基づいて、下記式を用いて解ベクトルの更新を行う（ステップＳ３０６）。
［η、ｙ、ｚ、λ］＝［η、ｙ、ｚ、λ］＋βα_ｐ［Δη、Δｙ、Δｚ、Δλ］

軌道生成部２２２は、上記（６１）式を用いて、complementary measure μを算出する（ステップＳ３０７）。
軌道生成部２２２は、条件（μ＞μ_min and ｎ＜ｎ_max）を満足するか否かを判定する（ステップＳ３０８）。

軌道生成部２２２は、条件（μ＞μ_min and ｎ＜ｎ_max）を満足すると判定したとき（ ステップＳ３０８のＹＥＳ）、ｎ＝ｎ＋１を設定し、上記（ステップＳ３０３）の処理に戻る。

軌道生成部２２２は、条件（μ＞μ_min and ｎ＜ｎ_max）を満足しないと判定したとき（ステップＳ３０８のＮＯ）、上記収束したときのηに基づいて、上記（５３）乃至（５５）式からの最適解ζを算出する。そして、軌道生成部２２２は、この最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（５２）式に示す等式制約条件及び不等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する（ステップＳ３０９）。

（第３実施形態）
上記実施形態１に係る軌道生成部２２２は、線形不変な等式制約条件付き最適化問題を求解しているが、本実施形態３に係る軌道生成部２２２は、線形時変な等式制約条件付き最適化問題を求解する。

例えば、サンプリング間隔が変化するような場合を考えると、Δｔは固定ではなく、Δｔ_ｋのようにサンプリング点毎に変化することとなる。この場合、上記（８）式は、下記（７２）式のように線形時変の制御システムとして表現できる。

ここで、線形時変の最適化問題の状態方程式は、例えば、下記（７３）式に示す関係が成立する。すなわち、（７３）式に示す状態方程式は、線形時変の制御パラメータＡ_ｋ、Ｂ_ｋを含むこととなる。

上記（７３）式、（１２）式、及び（１３）式より、軌道生成部２２２は、下記（７４）式に示す等式制約条件付きＬＱ最適化問題を求解し、重心軌道を生成することとなる。

但し、上記（７４）式において、下記（７５）式が成立するものとする。

以上から、実施形態１と同様に、等式制約条件付きＬＱ最適化問題を直交補空間に投影することで、直交補空間変換を行い無制約条件のＬＱ最適化問題を導出する。
まず、上記（１３）式（等式制約条件：Ｃ_ｋｘ［ｋ］＝ｄ_ｋ）より、状態変数ｘの変換式である下記（７６）式が上記実施形態１と同様に導出される。

Ｃ_ｋ＋１Ｂの直交補空間を用いて変数変換を行う。Ｃ_ｋ＋１Ｂを下記（７７）式に示すようにＱＲ分解する。

上記（７７）式を用いて上記（２０）式と同様に、入力ｕの変換式である下記（７８）式を導出する。

但し、上記（７８）式における各パラメータを下記（７９）式に示すように設定する。

ｋ＝０のときは、上記（７８）式における各パラメータを下記（８０）式に示すように設定する。

以上より、上記（８）式を上記（７３）式、（７６）式、及び（７８）式を用いて変形し、状態方程式の変換式である下記（８１）式を導出する。

但し、上記（８１）式における各パラメータを下記（８２）式に示すように設定する。

ｋ＝０のときは、上記（８１）式における各パラメータを下記（８３）式に示すように設定する。

上記実施形態１と同様に、上記（１２）式に示す評価関数Ｊを変形し、評価関数の変換式である下記（８４）式を導出する。

但し、上記（８４）式における各パラメータを下記（８５）式に示すように設定する。

以上から、本実施形態に係る軌道生成部２２２は、実施形態１と同様に、上記直交補空間を用いて変換した下記（８６）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解ζを高速に求解できる。

最後に、軌道生成部２２２は、上記求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（７４）式に示す等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する。

（第４実施形態）
本実施形態４に係る軌道生成部２２２は、線形時変な、所定の区間内だけ接触力が変化しないように設定した、入力を含む等式制約条件付き最適化問題を求解する。

例えば、未来のサンプリング区間において、移動ロボットが一定の力で物体を押して動かす等の、接触力を変動させたくない区間が存在する場合を想定する。より具体的には、２番目の接触点と最後から２番目の接触点をある区間内だけ接触力を変化しないようにした場合、当該区間における等式制約条件を下記（８７）式に示すように入力Ｅ_ｋ（ｕ）を含むこととなる。

なお、上記接触力を変動させたくない区間以外の区間においては、Ｃ_ｋは下記（８８）式のように設定できる。

上記式より、軌道生成部２２２は、下記（８９）式に示す入力を含む等式制約条件付きＬＱ最適化問題を求解し、重心軌道を生成することとなる。

但し、下記（９０）式が成立する。

まず、等式制約条件内の入力ｕに対する係数Ｅ_ｋの転置行列をＱＲ分解すると下記（９１）式が導出される。

上記（９１）式を用いて、上記入力を含む等式制約条件の（８７）式を、下記（９２）式に示すように変換できる。

上記（９２）式を上記状態方程式（８）式に代入することで、下記（９３）式が導出される。

以降の式変換の方法は、上記実施形態３と同一であるため、省略して説明する。以上から、線形時変な、入力を含む等式制約条件付き最適化問題を直交補空間変換を行い、下記（９４）に示す無制約条件のＬＱ最適化問題を導出する。

軌道生成部２２２は、導出した上記（９４）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解ζを高速に求解できる。最後に、軌道生成部２２２は、上記求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（８９）式に示す入力を含む等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する。なお、軌道生成部２２２は、上記同様に、線形不変な、入力を含む等式制約条件付き最適化問題を求解してもよい。

（第５実施形態）
本実施形態４に係る軌道生成部２２２は、線形時変な、所定の区間内だけ接触力が変化しないように設定した入力を含む等式制約条件、及び、所定の区間内だけ接触力に制限をかけるように設定した入力を含む不等式制約条件付き最適化問題を求解する。

例えば、移動ロボットの手先や足先の急激な接触力の変動を防ぐように、接触力の変化に制限をかけたい場合を想定する。例えば、２番目の接触点の接触力の増加量と、最後から２番目の接触点の接触力の減少量と、をある区間内だけ、制限したい場合、制限をかけたい区間における不等式制約条件は、下記（９５）式のように入力Γ_ｋｕ[ｋ]を含むこととなる。

但し、上記（９５）式において、Δｆ_ｌｍは各接触点の６軸力の制限値を縦に並べたベクトルである。また、上記制限をかけたい区間以外の区間においては、Ｐ_ｋは下記（９６）式のように設定できる。

上記式より、軌道生成部２２２は、下記（９７）式に示す入力を含む等式制約条件付きＬＱ最適化問題を求解し、重心軌道を生成することとなる。

但し、下記（９８）式が成立する。

不等式制約条件を示す上記（９５）式に上記（９２）式を代入することで、不等式制約条件を下記（９９）式に変換する。

但し、上記（９９）式のパラメータを下記（１００）式のように設定する。

上記（３３）式と上記（９９）式から、軌道生成部２２２は、下記（１０１）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解ζを求解することとなる。

以降に示す、上記無制約条件のＬＱ最適化問題に対するリカッチ型再帰的計算法による最適解ζの求解方法は、上記実施形態２において説明した求解方法と略同一であるため、相違点のみを説明する。

上記（５３）式乃至（５５）式の係数行列Ｐが下記（１０２）式に置き換わる。

したがって、上記（６６）式は、下記（１０３）式に置き換わる。

ここで、下記（１０４）式が成立する。

上記（１０４）式を用いて、上記（１０３）式を下記（１０５）式のように変形できる。

但し、上記（１０５）式の各パラメータを下記（１０６）式のように設定する。

実施形態２の上記（６８）式と上記（１０５）式との相違は、Ｓ_ｋ（ハット）がＳ′_ｋ（ハット）となっているだけで、その他のパラメータは同一である。したがって、軌道生成部２２２は、以降の計算について、上記実施形態２と同一の計算を行い、最適解ζを高速に求解できる。最後に、軌道生成部２２２は、上記求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、入力を含む等式制約条件及び不等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する。なお、軌道生成部２２２は、上記同様に、線形不変な、入力を含む等式制約条件及び不等式制約条件付き最適化問題を求解してもよい。また、軌道生成部２２２は、線形不変あるいは線形時変な、等式制約条件及び入力を含む不等式制約条件付き最適化問題を求解してもよい。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
本発明は、例えば、図１５や図１８に示す処理を、ＣＰＵ２１０ａにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ａ-２８ａ…モータ、１ｂ-２８ｂ…エンコーダ、２５…接触力センサ、１００…移動ロボット、２１０…最適制御装置、２２１…接触点計画設定部、２２２…軌道生成部、２２３…動作制御部。

Claims (13)

1. 二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する接触点計画手段と、
   前記接触点計画手段により設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する軌道生成手段と、
   を備える最適制御装置であって、
   前記軌道生成手段は、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、
   前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、
   前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、
   前記軌道生成手段は、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、
   ことを特徴とする最適制御装置。
2. 請求項１記載の最適制御装置であって、
   前記軌道生成手段は、前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築し、
   前記評価基準は、前記各接触点に対応して設定された重みに基づいて前記各接触点に前記接触力と、前記接触力の微分値とを配分するという基準が含まれ、前記接触力および接触力の微分値の二乗和を含む評価関数を予測区間内において最小化するものであり、
   前記評価基準と、前記接触力の微分値の入力と前記重心の状態変数と関係を示す状態方程式と、前記移動ロボットの力の釣合いの拘束を示す等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換される、
   ことを特徴とする最適制御装置。
3. 請求項１又は２記載の最適制御装置であって、
   前記等式制約条件を示す式に対してＱＲ分解を行って状態変数の変換式が導出され、前記接触力の微分値の入力と重心の状態変数との関係を示す状態方程式から導出した式に対してＱＲ分解を行って入力の変換式が導出され、前記状態方程式と、前記状態変数の変換式と、前記入力の変換式と、前記状態変数の変換式と、に基づいて状態方程式の変換式が導出され、前記導出した状態変数の変換式と、入力の変換式と、等式制約条件付き最適化問題の評価関数と、に基づいて、評価関数の変換式が導出され、
   前記無制約条件の最適化問題は、前記導出された評価関数の変換式と、前記状態方程式の変換式と、を含む、
   ことを特徴する最適制御装置。
4. 請求項１乃至３のうちのいずれか１項記載の最適制御装置であって、
   前記軌道生成手段は、
   前記無制約条件の最適化問題を行列表現した式の最適解条件に対して、再帰的計算法を用いて最適解を求解し、前記求解した最適解と、前記等式制約条件を示す式をＱＲ分解して導出した状態変数の変換式と、に基づいて前記重心の状態変数の時系列データを算出する、
   ことを特徴とする最適制御装置。
5. 請求項１乃至４のうちいずれか１項記載の最適制御装置であって、
   前記等式制約条件は、所定の区間内だけ前記接触力が変化しないように設定した入力を含む、
   ことを特徴とする最適制御装置。
6. 請求項１乃至５のうちいずれか１項記載の最適制御装置であって、
   前記軌道生成手段は、
   前記等式制約条件と前記接触点の安定性の拘束を示す不等式制約条件とを含む等式制約条件及び不等式制約条件付き最適化問題を直交補空間を用いて変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数の時系列データを算出する、
   ことを特徴とする最適制御装置。
7. 請求項６記載の最適制御装置であって、
   前記軌道生成手段は、
   前記無制約条件の最適化問題を行列表現した式の最適解条件に対してニュートン法を適用し、該ニュートン法の収束演算の中で前記再帰的計算法を用いてニュートン方向を算出し、該算出したニュートン方向に基づいて、最適解を算出する、
   ことを特徴とする最適制御装置。
8. 請求項７記載の最適制御装置であって、
   前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して内点法又はアクティブセット法を適用する、ことを特徴する最適制御装置。
9. 請求項６乃至８のうちいずれか１項記載の最適制御装置であって、
   前記不等式制約条件は、所定の区間内だけ前記接触力に制限をかけるように設定した入力を含む、
   ことを特徴とする最適制御装置。
10. 請求項１乃至９のうちいずれか１項記載の最適制御装置であって、
    前記最適化問題の状態方程式は、線形時変の制御パラメータを含む、ことを特徴とする最適制御装置。
11. 請求項１乃至１０のうちいずれか１項記載の最適制御装置であって、
    前記軌道生成手段により生成された重心軌道に基づいて前記移動手段を制御する制御手段を更に備える、ことを特徴とする最適制御装置。
12. 二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定するステップと、
    前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成するステップと、
    を含む最適制御方法であって、
    前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、
    前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、
    前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、
    前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、
    ことを特徴とする最適制御方法。
13. 二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する処理と、
    前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する処理と、
    をコンピュータに実行させる最適制御プログラムであって、
    前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、
    前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、
    前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、
    前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、
    ことを特徴とする最適制御プログラム。

Images