(57)【特許請求の範囲】
【請求項１】
二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する接触点計画手段と、
前記接触点計画手段により設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する軌道生成手段と、
を備える最適制御装置であって、
前記軌道生成手段は、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、
前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、
前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、
前記軌道生成手段は、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、
ことを特徴とする最適制御装置。
【請求項２】
請求項１記載の最適制御装置であって、
前記軌道生成手段は、前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築し、
前記評価基準は、前記各接触点に対応して設定された重みに基づいて前記各接触点に前記接触力と、前記接触力の微分値とを配分するという基準が含まれ、前記接触力および接触力の微分値の二乗和を含む評価関数を予測区間内において最小化するものであり、
前記評価基準と、前記接触力の微分値の入力と前記重心の状態変数と関係を示す状態方程式と、前記移動ロボットの力の釣合いの拘束を示す等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換される、
ことを特徴とする最適制御装置。
【請求項３】
請求項１又は２記載の最適制御装置であって、
前記等式制約条件を示す式に対してＱＲ分解を行って状態変数の変換式が導出され、前記接触力の微分値の入力と重心の状態変数との関係を示す状態方程式から導出した式に対してＱＲ分解を行って入力の変換式が導出され、前記状態方程式と、前記状態変数の変換式と、前記入力の変換式と、前記状態変数の変換式と、に基づいて状態方程式の変換式が導出され、前記導出した状態変数の変換式と、入力の変換式と、等式制約条件付き最適化問題の評価関数と、に基づいて、評価関数の変換式が導出され、
前記無制約条件の最適化問題は、前記導出された評価関数の変換式と、前記状態方程式の変換式と、を含む、
ことを特徴する最適制御装置。
【請求項４】
請求項１乃至３のうちのいずれか１項記載の最適制御装置であって、
前記軌道生成手段は、
前記無制約条件の最適化問題を行列表現した式の最適解条件に対して、再帰的計算法を用いて最適解を求解し、前記求解した最適解と、前記等式制約条件を示す式をＱＲ分解して導出した状態変数の変換式と、に基づいて前記重心の状態変数の時系列データを算出する、
ことを特徴とする最適制御装置。
【請求項５】
請求項１乃至４のうちいずれか１項記載の最適制御装置であって、
前記等式制約条件は、所定の区間内だけ前記接触力が変化しないように設定した入力を含む、
ことを特徴とする最適制御装置。
【請求項６】
請求項１乃至５のうちいずれか１項記載の最適制御装置であって、
前記軌道生成手段は、
前記等式制約条件と前記接触点の安定性の拘束を示す不等式制約条件とを含む等式制約条件及び不等式制約条件付き最適化問題を直交補空間を用いて変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数の時系列データを算出する、
ことを特徴とする最適制御装置。
【請求項７】
請求項６記載の最適制御装置であって、
前記軌道生成手段は、
前記無制約条件の最適化問題を行列表現した式の最適解条件に対してニュートン法を適用し、該ニュートン法の収束演算の中で前記再帰的計算法を用いてニュートン方向を算出し、該算出したニュートン方向に基づいて、最適解を算出する、
ことを特徴とする最適制御装置。
【請求項８】
請求項７記載の最適制御装置であって、
前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して内点法又はアクティブセット法を適用する、ことを特徴する最適制御装置。
【請求項９】
請求項６乃至８のうちいずれか１項記載の最適制御装置であって、
前記不等式制約条件は、所定の区間内だけ前記接触力に制限をかけるように設定した入力を含む、
ことを特徴とする最適制御装置。
【請求項１０】
請求項１乃至９のうちいずれか１項記載の最適制御装置であって、
前記最適化問題の状態方程式は、線形時変の制御パラメータを含む、ことを特徴とする最適制御装置。
【請求項１１】
請求項１乃至１０のうちいずれか１項記載の最適制御装置であって、
前記軌道生成手段により生成された重心軌道に基づいて前記移動手段を制御する制御手段を更に備える、ことを特徴とする最適制御装置。
【請求項１２】
二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定するステップと、
前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成するステップと、
を含む最適制御方法であって、
前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、
前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、
前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、
前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、
ことを特徴とする最適制御方法。
【請求項１３】
二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する処理と、
前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する処理と、
をコンピュータに実行させる最適制御プログラムであって、
前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、
前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、
前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、
前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、
ことを特徴とする最適制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動ロボットのモデル予測制御を行う最適制御装置、最適制御方法及び制御プログラムに関するものである。
【背景技術】
【０００２】
例えば、ロボットの機械的リンク系に対してモデル予測制御（リシーディングホライゾン制御）を行う最適制御装置が知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０００−３３０６０９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記モデル予測制御では、ロボットの物理的な制約条件が設定される。そして、制御周期毎にこの制約条件付き最適化問題を求解し、その求解した最適解に基づいてロボットの重心軌道を生成することとなる。しかし、この最適解の求解において、従来、多大な時間を要するという、問題が生じていた。
【０００５】
本発明は、このような問題点を解決するためになされたものであり、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる最適制御装置、最適制御方法及び制御プログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する接触点計画手段と、前記接触点計画設定手段により設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する軌道生成手段と、を備える最適制御装置であって、前記軌道生成手段は、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、前記軌道生成手段は、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御装置である。
この一態様において、前記軌道生成手段は、前記移動手段を接地するときの接触力の微分値を入力とする予測モデルを構築し、前記評価基準は、前記各接触点に対応して設定された重みに基づいて前記各接触点に前記接触力と、前記接触力の微分値とを配分するという基準が含まれ、前記接触力および接触力の微分値の二乗和を含む評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力の微分値の入力と前記重心の状態変数と関係を示す状態方程式と、前記移動ロボットの力の釣合いの拘束を示す等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換されてもよい。
この一態様において、前記等式制約条件を示す式に対してＱＲ分解を行って状態変数の変換式が導出され、前記接触力の微分値の入力と重心の状態変数との関係を示す状態方程式から導出した式に対してＱＲ分解を行って入力の変換式が導出され、前記状態方程式と、前記状態変数の変換式と、前記入力の変換式と、前記状態変数の変換式と、に基づいて状態方程式の変換式が導出され、前記導出した状態変数の変換式と、入力の変換式と、等式制約条件付き最適化問題の評価関数と、に基づいて、評価関数の変換式が導出され、
前記無制約条件の最適化問題は、前記導出された評価関数の変換式と、前記状態方程式の変換式と、を含んでいてもよい。
この一態様において、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して、再帰的計算法を用いて最適解を求解し、前記求解した最適解と、前記等式制約条件を示す式をＱＲ分解して導出した状態変数の変換式と、に基づいて前記重心の状態変数の時系列データを算出してもよい。
この一態様において、前記等式制約条件は、所定の区間内だけ前記接触力が変化しないように設定した入力を含んでいてもよい。
この一態様において、前記軌道生成手段は、前記等式制約条件と前記接触点の安定性の拘束を示す不等式制約条件とを含む等式制約条件及び不等式制約条件付き最適化問題を直交補空間を用いて変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数の時系列データを算出してもよい。
この一態様において、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対してニュートン法を適用し、該ニュートン法の収束演算の中で前記再帰的計算法を用いてニュートン方向を算出し、該算出したニュートン方向に基づいて、最適解を算出してもよい。
この一態様において、前記軌道生成手段は、前記無制約条件の最適化問題を行列表現した式の最適解条件に対して内点法又はアクティブセット法を適用してもよい。
この一態様において、前記不等式制約条件は、所定の区間内だけ前記接触力に制限をかけるように設定した入力を含んでいてもよい。
この一態様において、前記最適化問題の状態方程式は、線形時変の制御パラメータを含んでいてもよい。
この一態様において、前記軌道生成手段により生成された重心軌道に基づいて前記移動手段を制御する制御手段を更に備えていてもよい。
上記目的を達成するための本発明の一態様は、二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定するステップと、前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成するステップと、を含む最適制御方法であって、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御方法であってもよい。
上記目的を達成するための本発明の一態様は、二以上の移動手段を交互に接地しながら移動する移動ロボットの該移動手段が接地する接触点の位置と、接地するときの前記移動手段の姿勢と、を時系列のデータとした接触点計画を設定する処理と、前記設定された接触点計画に基づいて、前記移動手段が接触点に接地しながら前記移動ロボットが移動するための重心軌道を生成する処理と、をコンピュータに実行させる最適制御プログラムであって、前記移動手段を接地するときの接触力に基づく量を入力とする予測モデルを構築して、該予測モデルによって所定時間幅の予測区間における前記移動ロボットの重心の状態変数を表わし、前記予測区間において、所定の評価基準を用いて前記重心の状態変数を算出し、該算出した重心の状態変数に基づいて、前記移動ロボットの重心軌道を生成するモデル予測制御を行ない、前記評価基準は、各接触点における前記接触力に基づく量の二乗が含まれる評価関数を予測区間内において最小化するものであり、前記評価基準と、前記接触力に基づく入力と前記重心の状態変数と関係を示す線形な状態方程式と、前記移動ロボットの線形等式で表現される等式制約条件と、を含む等式制約条件付き最適化問題は、直交補空間を用いて、前記等式制約条件を含まない無制約条件の最適化問題に変換され、  前記予測区間において、該変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて前記重心の状態変数を算出する、ことを特徴とする最適制御プログラムであってもよい。
【発明の効果】
【０００７】
本発明によれば、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる最適制御装置、最適制御方法及び制御プログラムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】移動ロボットの動作の一例を示す図。である。
【図２】移動ロボットの機械構成の一例を示す図である。
【図３】移動ロボットの機能ブロック図である。
【図４】６軸力を示す図である。
【図５】最適制御装置の機能ブロック図である。
【図６】接触点計画の概要の一例を示す図である。
【図７】接触点計画の一例を示す図である。
【図８】予測区間の例を示す図である。
【図９】予測区間での動きを表わした図である。
【図１０】予測区間のシフトを説明するための図である。
【図１１】予測に用いる移動ロボットのモデルを示す図である。
【図１２】予測区間の離散化を説明するための図である。
【図１３】直交補空間のイメージ図である。
【図１４】無制約条件のＬＱ最適化問題に変換する際のフローを示す図である。
【図１５】最適制御方法を示すフローチャートである。
【図１６】接触点の座標系と接触多角形とを示す図である。
【図１７】接触点が不安定化する場合を例示した図である。
【図１８】等式制約条件及び不等式制約条件付きＬＱ最適化問題の最適解の求解フローを示すフローチャートである。
【発明を実施するための形態】
【０００９】
本発明の実施形態を図示するとともに図中の各要素に付した符号を参照して説明する。
（第１実施形態）
本実施形態は移動ロボットの最適制御装置に特徴があり、具体的には、移動ロボットの移動動作（図１）を制御するための軌道生成に特徴を有するのであるが、具体的な制御（軌道生成）を説明する前に、制御対象となる移動ロボットのハードウェア構成について予め説明しておく。
【００１０】
図２は、移動ロボットの機械構成の一例を示した図である。
移動ロボット１００は、股関節が３軸、膝関節が１軸、足首関節が２軸、さらに、肩関節が３軸（肩ピッチ、肩ロール、肩ヨー）、肘関節が１軸（肘ピッチ）、および、手首関節が３軸（手首ヨー、手首ピッチ、手首ロール）、で夫々構成されている。
【００１１】
（移動ロボットの機械構成はこれに限定されないが、手（腕）の自由度は６以上、足（脚）の自由度も６以上は必要である。）
移動ロボット１００は、各関節にエンコーダ付きモータ１、２、・・・、２８を有している。
各関節のモータ１ａ、２ａ、・・・、２８ａ（図３）は、各関節の関節角度θ１、θ２、・・・、θ２８を調整できる。
一方、各関節のエンコーダ１ｂ、２ｂ、・・・、２８ｂは、各関節の関節角度θ１、θ２・・・、θ２８を計測することができる。
【００１２】
また、移動ロボット１００は、足先部（足平部）および手先部（手の平部）に接触力センサ２５を有している。
ここで接触力とは６軸力であり、図４に示すように、ｘ軸、ｙ軸およびｚ軸方向の力ｆの組（ｆｘ、ｆｙ、ｆｚ）Ｔと、ｘ軸回り、ｙ軸回りおよびｚ軸回りの力τの組（τｘ、τｙ、τｚ）Ｔと、である。
（なお、ｘ軸およびｙ軸は、鉛直方向であるｚ軸に垂直な面内で互いに直交する軸とする。）
【００１３】
この移動ロボットは、移動時に、右足、左足、右手および左手のうちの一つ以上を床、壁、あるいはテーブルなどに接触させながら移動する。
そこで、本明細書の以下の説明では、右足、左足、右手および左手を接触点候補と称することがある。また、手先、足先というのは、移動手段の一具体例である。
【００１４】
図３は、移動ロボット１００の機能ブロック図である。
移動ロボット１００は、各関節のモータ１ａ〜２４ａ及びエンコーダ１ｂ〜２４ｂと、接触力センサ２５と、最適制御装置２１０と、を備えている。
【００１５】
最適制御装置２１０には、各関節のエンコーダ１ｂ〜２４ｂ及び接触力センサ２５から、センサ検出値が入力される。また、最適制御装置２１０は、各関節のモータ１ａ〜２４ａに対して駆動信号を出力する。
【００１６】
最適制御装置２１０は、主要なハードウェア構成として、制御処理、演算処理等を行うＣＰＵ（Central Processing Unit）２１０ａと、ＣＰＵ２１０ａによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）２１０ｂと、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）２１０ｃと、を有するマイクロコンピュータにより構成されている。また、これらＣＰＵ２１０ａ、ＲＯＭ２１０ｂ、及びＲＡＭ２１０ｃは、データバス２１０ｄによって相互に接続されている。必要なプログラムを不揮発性記録媒体に記録しておき、必要に応じてインストールするようにしてもよい。
【００１７】
図５は、本発明の一実施形態に係る最適制御装置２１０の機能ブロック図である。本実施形態に係る最適制御装置２１０は、移動ロボット１００の接触点計画を設定する接触点計画設定部（接触点計画手段の一具体例）２２１と、安定に実行できる移動ロボット１００の重心軌道を生成する軌道生成部（軌道生成手段の一具体例）２２２と、生成された重心軌道に従って移動ロボット１００の全身動作を実行させる動作制御部（制御手段の一具体例）２２３と、を有する。
【００１８】
ここで、軌道生成部２２２は、接触点計画に従った動作を実行できる重心軌道を生成するのであるが、この重心軌道生成には必要に応じた接触点変更を含む。
これら機能部の具体的な処理動作については後述する。
【００１９】
（多点接触移動のための軌道生成方法）
本実施形態に係る軌道生成部２２２は、（１）多点接触移動を実現できる重心軌道を生成し、かつ、（２）必要に応じて接触点の変更を行っている。ここで、（１）多点接触移動を実現できる重心軌道を生成するための方法を説明する。なお、本出願人は、特願２０１３−２５４９８９（平成２５年１２月１０日出願）においてこの方法を出願している。
【００２０】
そもそも、将来の目標重心位置を予め知ることはできないのであり、制御目標値として未知であるはずの将来の重心位置をユーザが設定するというのは無理がある。ユーザとしてはロボットに接触点の計画情報だけを与え、あとは、移動ロボットが設定された接触点の計画情報に基づいて自動的に安定な重心軌道を生成して自律的に移動してくれることが望ましい。
【００２１】
さて、移動ロボットに多点接触移動を安定して行わせるためには、時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成する技術が必要である。
【００２２】
このために本実施形態に係る軌道生成部２２２は、モデル予測制御（所謂リシーディングホライゾン制御：Receding Horizon Control）を用いて重心軌道を生成する。
最初にモデル予測制御の概要を説明しておく。
【００２３】
（モデル予測制御の概要説明）
例えば、図１に図示したような移動動作を移動ロボットに行わせたいとする。
ここでは、２本の腕と２本の脚とを有する人型の移動ロボットに、テーブルの奥側にあるボトルを掴ませるという一連の動作を想定する。
【００２４】
この場合、接触点計画設定部２２１は、ユーザから指令される接触点の計画情報に基づいて、この一連動作（タスク）を実行できるような接触点計画を作成する。
つまり、接触点計画設定部２２１は、例えば、図６のように、手先および足先を、どの順番で、どこに、どのように、着くか、という計画を作成する。
図６においては、床、壁およびテーブルにおいて足先および手先を接触させる箇所にマークを付けている。
【００２５】
この接触点計画は、具体的には図７のようになる。
接触点計画は、左手（ＬＨ）、右手（ＲＨ）、左足（ＬＦ）および右足（ＲＦ）に関し、どの順番で、どこに、どのように、着いていくか、という時系列のデータである。
【００２６】
図１、図６および図７の対応関係を簡単に説明する。
当初（ｔ０）左足１本だけで立ち、遊脚である右足を前に振り出し、そして、右足を着地させる（ｔ１）。
この動きに従った接触点計画を移動ロボット１００に実行させるためには、左足が最初に着地している床上の接触点の座標ＰＬＦ１、そのときの左足の姿勢ｒＬＦ１、そして、右足が着地する床上の接触点の座標ＰＲＦ１、そのときの右足の姿勢ｒＲＦ１、を指定することが必要である。
【００２７】
ここで、接触点の座標は、空間座標としてＰ＝（Ｐｘ、Ｐｙ、Ｐｚ）の組で表わされる。
また、姿勢というのは、接触点に着地したときの足の裏面の向きであり、例えばオイラー角の組としてｒ＝（ｒｘ、ｒｙ、ｒｚ）として表わされる。
（すなわち、ｒｘ、ｒｙおよびｒｚは、ロール、ピッチおよびヨー角をそれぞれ表わす。）
足に関する接触点の座標およびそのときの姿勢を指令する形式は今後の説明でも同様なので、以後は適宜説明を省略する。
【００２８】
両足で立った後、左足を振り出し（ｔ２）、左足を前方に着地する（ｔ４）。
その間に、左手を壁に着くようにする（ｔ３）。
ここで、左手を着く壁上の接触点の座標ＰＬＨ１、および、そのときの左手の姿勢ｒＬＨ１を指定する。
【００２９】
この接触点の座標は空間座標としてＰ＝（Ｐｘ、Ｐｙ、Ｐｚ）の組で表わされ、姿勢は接触点に着いたときの手の平の向きとしてオイラー角の組としてｒ＝（ｒｘ、ｒｙ、ｒｚ）として表わされる。
【００３０】
これ以降の接触点計画は図１、図６および図７を対比して頂ければ自明と思われるので省略する。
このようにして、接触点計画設定部２２１は、接触点計画を時系列のデータとして作成する。
【００３１】
軌道生成部２２２は、上記のように接触点計画設定部２２１により設定された接触点計画を実現するように重心軌道を生成する。動作制御部２２３は、軌道生成部２２２により生成された重心軌道に従って移動ロボット１００の全身動作させるように、各関節のモータ１ａ〜２４ａを制御する。これにより、移動ロボット１００は、設定された接触点計画に基づいて安定な重心軌道に従って、自律的に移動できる。
【００３２】
このとき、軌道生成部２２２は、軌道生成にあたってモデル予測制御を行う。
すなわち、軌道生成部２２２は、ある時間幅を持った予測区間内で移動ロボット１００が安定移動できる軌道を生成し、予測区間を微小時間（Δｔ）ずつシフトさせながら安定動作を行える軌道を順次更新していくようにする。
【００３３】
例えば、図８に予測区間の例を示す。
軌道生成部２２２は、現在から所定時間（例えば１．６秒）先の未来までを予測区間として設定する。
【００３４】
そして、軌道生成部２２２は、この予測区間の間で発散しないように安定な軌道を生成する。
この予測区間での動きをイメージしたものが図９である。
このように、軌道生成部２２２は、ある時間幅を持つ予測区間で安定な軌道を生成した上で、最初の一点だけを現在の入力値として使用する。
【００３５】
軌道生成部２２２は、次の軌道更新周期（Δｔ秒後）には予測区間をシフトさせ、新たな予測区間において同様に安定な軌道を生成する（図１０参照）。
【００３６】
現在だけ、あるいは、現在から次ぎの制御周期（Δｔ秒）まで、だけを見るのではなく、上記のように、ある程度の未来までを予測区間とし、この予測区間内で発散しない軌道が生成されるようにする。
これを繰り返すことで移動ロボットは安定に移動することができる。
【００３７】
さて、ここで問題なのは、ある時間幅を持った予測区間のなかで時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成するにはどのようにすればよいか、ということである。
【００３８】
本発明者らは、ある予測区間における安定軌道の生成問題をＬＱ（Linear Quadratic）最適化問題（凸二次計画問題：Quadratic Programming: QP）に帰着させるという着想を得た。
具体的には、軌道生成部２２２は、各接触点における接触力の二乗和と、前記６軸力（接触力）の微分値の二乗和と、を含む評価関数Ｊを最小化するというＬＱ最適化問題を解くことで、多点接触移動の安定軌道を求める。
【００３９】
そこで、次に、この評価関数Ｊの導出およびその解法（ＬＱ最適化問題への帰着）を説明する。
この解法により、ある予測区間内で安定な多点接触移動を実現するための、重心位置、重心速度、接触力および接触力の微分値の時系列データが得られることを示す。（ここからの説明では、まず、接触点計画で指示された通りの位置（接触点）に手足を着くことだけを考える。なお、必要に応じて、スラック変数などを導入し条件式や評価式を緩和するなどの処置を行って接触点を変更してもよい。
【００４０】
予測に用いる移動ロボットのモデルを改めて図１１に示す。
移動ロボット全体の慣性を一つの重心Ｇで表わす。各接触点には６軸力を定義する。
【００４１】
この時、重心Ｇの並進運動量をＰ、重心回りの回転運動量（角運動量）をＬ、接触点の数をｎとすると、運動方程式は次のように書ける。
【００４２】
【数１】
0006421683.tif 000002
【００４３】
添え字ｉは接触点のインデックスを表す。
例えば接触点の候補が左手、右手、左足、右足の４点であれば、ｎ＝４（左手：ＬＨ＝１、右手：ＲＨ＝２、左足：ＬＦ＝３、右足：ＲＦ＝４）とすればよい。ただし、床や壁に接触していない接触点候補については接触力を０にするように拘束条件を設定しておく。例えば図１１の例であれば次のようにする。
【００４４】
【数２】
0006421683.tif 000003
【００４５】
（１）式の第１式、第２式を微分すると次の式が得られる。（（１）式はベクトルで表現しているが、これをｘ、ｙ、ｚに分解した上で、上から順に第１式、第２式・・・第６式と称する。）
【００４６】
【数３】
0006421683.tif 000004
【００４７】
本実施形態では、この２式をシステムとして用いる。そして、（１）式の第３から第５式を拘束条件として定式化する。
【００４８】
【数４】
0006421683.tif 000005
【００４９】
さらに、予測区間内を図１２のように、Ｎ個の区間に分割し、（３）式、（４）式を離散化する。（３）式を離散化すると次のようになる。
【００５０】
【数５】
0006421683.tif 000006
【００５１】
また、サンプリング点で常に（４）式の拘束が成り立つとすると、（４）式は次のように離散化される。
【００５２】
【数６】
0006421683.tif 000007
【００５３】
ここで、パラメータを次ぎのように置く。
【００５４】
【数７】
0006421683.tif 000008
【００５５】
θｉは、６軸力としての接触力を並べたベクトルである。そして、ｘは、重心Ｇのｘ座標、重心Ｇのｘ軸方向速度、重心Ｇのｙ座標、重心Ｇのｙ軸方向速度、および、各接触点における接触力（６軸力）、を並べたベクトルである。このｘを、重心の状態変数ｘと称する。さらに、ｕは、接触力（６軸力）の微分値を並べたベクトルである。
【００５６】
このようにパラメータを設定すると、（５）式を次ぎの状態方程式として記述することができる。
【００５７】
【数８】
0006421683.tif 000009
【００５８】
この（８）式は、（ｊ＋１）のときの状態変数ｘを、その一つ前の状態で記述できる。（８）式を用いて予測区間内の状態変数ｘを順に計算していくと次のようになる。
【００５９】
【数９】
0006421683.tif 000010
【００６０】
したがって、時系列的に求められる状態変数ｘを並べて大文字のＸで表わすと、状態変数の時系列データＸを次のように表わすことができる。
【００６１】
【数１０】
0006421683.tif 000011
【００６２】
この（１０）式は、接触力の微分値（ｕ［ｋ］）を入力として、ある予測区間内における移動ロボットの状態遷移を表わす予測モデルとなる。なお、上記（１０）式において、接触力を入力してもよい。この場合、状態変数ｘは、重心位置と重心速度のみを含むこととなる。また、上記（３）式は、Ｇ（２ドット）（２階微分）とｆとの関係式となり、この関係式と、上記（５）式のｆ（ドット）の項を０にした式とから、上記（８）式のような線形の状態方程式が導出できる。
さて、ここで、本発明者らは、予測区間内において安定な軌道を生成するために次ぎのような評価関数Ｊの評価基準を導入した。
【００６３】
【数１１】
0006421683.tif 000012
【００６４】
なお、Ｑｉ、Ｒｉは、適宜設定した重みである。例えば、接触点候補すべてに力を均等配分した場合、Ｑｉはすべて１となり、Ｒｉはすべて１×１０−６と設定できる。
【００６５】
ここで、θｉは、６軸力としての接触力の成分を並べたベクトルであった。したがって、上記（１１）式は、「予測区間内で、接触力（６軸力）と接触力の微分値との２乗和を最小化する」という意味の式である。上記（１１）式の第１項は、接触力（６軸力）の２乗和を最小化することを意味する。
【００６６】
この第１項には、次の作用が含まれている。
（１）各接触点への接触力を均等分配すること。これにより、重心をできる限り安定な位置に動かすという効果がある。
（２）不必要な内力を打ち消すこと。
（３）接触点の接地安定性を高めること。すなわち、接触面内の反力中心点を接触面の中心に設定するという効果がある。
【００６７】
また、上記（１１）式の第２項は、接触力（６軸力）微分値（６軸力の時間変化率）の２乗和を最小化することを意味する。
【００６８】
この第２項には次の作用が含まれている。
（１）重心の発散を抑制すること。
（２）滑らかに接触力を切り替えていくこと。
【００６９】
これらをＱ、Ｒという重みによって適切に足し合わせることによって、この評価関数Ｊを最小化するということは、
「高い接触安定性、滑らかな接触力遷移、最低限の内力、といった条件を満たしながら、安定な重心軌道と各接触点の接触力とを出力する」
ということを意味することとなる。
【００７０】
上記（１１）式を離散化し一般的な形式に書き換えると、次の評価関数（１２）式が得られる。
【数１２】
0006421683.tif 000013
【００７１】
次に、移動ロボットの力の釣合いの拘束を示す等式制約条件（拘束制約条件）について考える。
等式制約条件としては、
（１）移動ロボットの非接触の接触点候補に対して６軸力が０という拘束、
（２）移動ロボットの鉛直方向の力の釣り合いの拘束、および、
（３）移動ロボットのｘｙ軸回りのモーメント力の釣り合いの拘束、
が予測区間の全サンプリング点に渡って成り立つ必要がある。
【００７２】
ここで、例えば、あるサンプリング点ｋにおいて、ｉ番目とｉ＋２番目の接触点が非接触であったとする。
この時、上記等式制約条件（１）乃至（３）は、下記（１３）式のように記述できる。
【数１３】
0006421683.tif 000014
【００７３】
なお、係数行列Ｃｋ、ｄｋの成分はサンプリング点によって異なり、接触点候補の接触／非接触といった情報や接触点位置は接触点計画設定部２２１によって設定される。例えば、上記（１３）式のｐｉｘ［ｋ］、ｐｉｙ［ｋ］、ｐｉｚ［ｋ］は、接触点計画設定部２２１によって設定される。
【００７４】
以上から、現在の状態量（状態変数の初期値）をｘ０とすると、上記（８）式、（１２）式、及び（１３）式より、最適制御装置２１０の軌道生成部２２２は、下記（１４）式に示す等式制約条件付きＬＱ最適化問題を求解し、重心軌道を生成することとなる。
【数１４】
0006421683.tif 000015
なお、上記（１４）式において、１行目の式（ｍｉｎＪ＝・・）は、上述の如く、予測区間内において、接触力と接触力の微分値との２乗和を最小化するという意味の式である。２行目の式（ｘ［ｋ＋１］＝・・）は、接触力の微分値の入力と重心の状態変数と関係を示す状態方程式である。３行目の式（Ｃｋｘ［ｋ］＝ｄｋ）は、移動ロボットの力の釣合いの拘束を示す等式制約条件である。
【００７５】
ところで、上述のように移動ロボットの最適制御装置は、多点接触で安定的な動作軌道を生成するためにモデル予測制御を行っている。このモデル予測制御では、移動ロボットの物理的な制約条件（上述の等式制約条件）が設定される。そして、最適制御装置は、制御周期毎にＬＱ最適化問題を求解し、その求解した最適解に基づいて制御を行なっている。しかし、この最適解の求解において、従来、多大な時間を要し、モデル予測制御の周期（軌道更新の周期）に遅延が生じ、制御性能を上げることができないという問題が生じていた。
【００７６】
これに対し本実施形態においては、直交補空間を用いて等式制約条件付きのＬＱ最適化問題を無制約条件のＬＱ最適化問題に変換する。そして、最適制御装置２１０の軌道生成部２２２は、この変換した無制約条件のＬＱ最適化問題をリカッチ型再帰的計算法（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）といい、Ｖ⊥と表記する。
上記定義に基づいて下記命題（４）−（５）が成立する。
（４）線形独立なｍ（＜ｎ）個のベクトル｛ｙｉ｝ｍｉ＝１と直交するベクトル集合α＝｛ｘ∈Ｒｎ｜ｙＴ１ｘ＝ｙＴ２ｘ＝・・・＝ｙＴｍｘ＝０｝は、ｎ−ｍ次元部分空間である。
（５）非直交基底｛ｕｉ∈Ｒｎ｝ｎｉ＝１からｍ個選択された基底ベクトルによって張られる部分空間Ｖ＝＜ｕ１、ｕ２、・・・、ｕｍ＞の直交補空間は、その双直交基底｛ｖｉ∈Ｒｎ｝ｎｉ＝１によって、Ｖ⊥＝＜ｖｍ＋１、ｖｍ＋２、・・・、ｖｎ＞で表される。
【００７９】
上記命題を簡略して説明すると、Ｃｋ∈Ｒｍｋ×ｎｘの直交補空間Ｃ⊥ｋ∈Ｒｎｘ×（ｎｘ−ｍｋ）とは、ｎｘ×ｎｘの線形空間のうち、Ｃｋの残りの空間（補空間）でＣｋに直交する空間である。この直交補空間を用いて上記（１４）式のＬＱ最適化問題を変換することで、図１３に示す如く、等式制約条件Ｃｋｘ＝ｄｋ上に存在するｘを、Ｃｋに平行なベクトルζと直交しＣｋに終端する定数ベクトルσで表すことができる。換言すると、直交補空間を用いて、ｘをζに変数変換することで、ζをどのように動かしても必ず等式制約条件Ｃｋｘ＝ｄｋは満たされることとなる。このため、この等式制約条件を考慮することなく無制約条件でＬＱ最適化問題を求解できる。
【００８０】
次に、上述した直交補空間を用いた変換方法（以下、直交補空間変換と称す）について詳細に説明する。
本実施形態において、例えば、下記（１５）式に示すＱＲ分解（直交行列Ｑと上三角形行列Ｒの積に分解）を用いて直交補空間変換を行うことができる。
【数１５】
0006421683.tif 000016
【００８１】
以上から、等式制約条件付きＬＱ最適化問題を直交補空間に投影することで、直交補空間変換を行い無制約条件のＬＱ最適化問題を次のように導出する。
まず、等式制約条件を示す上記（１３）式（Ｃｋｘ［ｋ］＝ｄｋ）をＱＲ分解することで、状態変数ｘの変換式である下記（１６）式が導出される。
【数１６】
0006421683.tif 000017
【００８２】
次に上記状態方程式（８）式の左からＣｋ＋１を掛けると下記（１７）式が導出される。
Ｃｋ＋１ｘ［ｋ＋１］＝Ｃｋ＋１Ａｘ［ｋ］＋Ｃｋ＋１Ｂｕ［ｋ］  ・・・（１７）
さらに、上記（１７）式に上記（１３）式を代入して下記（１８）式を導出する。
Ｃｋ＋１Ａｘ［ｋ］＋Ｃｋ＋１Ｂｕ［ｋ］＝ｄｋ＋１  ・・・（１８）
（ｋ＝０、１、・・・、Ｎ−１）
【００８３】
上記変換と同様に、Ｃｋ＋１Ｂの直交補空間を用いて変数変換を行う。
Ｃｋ＋１Ｂを下記（１９）式に示すようにＱＲ分解する。
【数１７】
0006421683.tif 000018
【００８４】
上記（１９）式を用いて上記（１８）式を変換し（ＱＲ分解を行い）、入力ｕの変換式である下記（２０）式を導出する。
【数１８】
0006421683.tif 000019
但し、上記（２０）式における各パラメータを下記（２１）式に示すように設定する。
【数１９】
0006421683.tif 000020
ｋ＝０のときは、上記（２０）式における各パラメータを下記（２２）式に示すように設定する。
【数２０】
0006421683.tif 000021
【００８５】
上記（１６）式の左からＤＴｋを掛けて下記（２３）式を導出する。
【数２１】
0006421683.tif 000022
但し、上記（２３）式において、正規直交性から下記（２４）式が成立する。
【数２２】
0006421683.tif 000023
【００８６】
以上より、上記（８）式を上記（１６）式、（２０）式、及び（２３）式を用いて変形し、状態方程式の変換式である下記（２５）式を導出する。
【数２３】
0006421683.tif 000024
但し、上記（２５）式における各パラメータを下記（２６）式に示すように設定する。
【数２４】
0006421683.tif 000025
ｋ＝０のときは、上記（２５）式における各パラメータを下記（２７）式に示すように設定する。
【数２５】
0006421683.tif 000026
【００８７】
また、上記（１６）式及び（２０）式を用いて、上記（１２）式に示す評価関数ＪのΣの項は、下記（２８）式に示すように変形できる。
【数２６】
0006421683.tif 000027
但し、ｋ＝０のときは、下記（２９）式が成立する。
【数２７】
0006421683.tif 000028
また、ｋ＝Ｎのときは、下記（３０）式が成立する。
【数２８】
0006421683.tif 000029
【００８８】
上記（１６）式及び（２０）式を用いて上記（１２）式に示す評価関数Ｊを変形し、評価関数の変換式である下記（３１）式を導出する。
【数２９】
0006421683.tif 000030
但し、上記（３１）式における各パラメータを下記（３２）式に示すように設定する。
【数３０】
0006421683.tif 000031
【００８９】
以上のように、等式制約条件付きＬＱ最適化問題に対して直交補空間変換を行い、下記（３３）式に示す無制約条件のＬＱ最適化問題を導出できる。すなわち、直交補空間変換を行うことで、上記（１４）式に示す等式制約条件付きＬＱ最適化問題を、下記（３３）式に示す無制約条件のＬＱ最適化問題に変換できる。本実施形態に係る軌道生成部２２２は、下記（３３）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解を高速に求解できる。
【数３１】
0006421683.tif 000032
【００９０】
次に、上記直交補空間変換により変換した無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて求解する方法を説明する。
まず、上記（３３）式を行列表現すると、下記（３４）式及び（３５）式のように表現できる。
【数３２】
0006421683.tif 000033
【００９１】
上記（３４）式及び（３５）式の最適解条件（ＫＫＴ条件）は、下記（３６）式となる。但し、下記（３７）式は、上記（３５）式のラグランジュ乗数である。
【数３３】
0006421683.tif 000034
【００９２】
軌道生成部２２２は、上記（３６）式に示す式に対して、次のように、再帰的計算を行うことで、上記無制約条件のＬＱ最適化問題を高速かつ安定的に求解する。
まず、軌道生成部２２２は、上記（３６）式の行列内の各パラメータの順番を入れ替えることで、下記（３８）式のように表現する。
【数３４】
0006421683.tif 000035
【００９３】
そして、軌道生成部２２２は、上記（３８）式に対して、下記（３９）式に示す再帰計算を繰り返す。
【数３５】
0006421683.tif 000036
【００９４】
上記再帰計算を繰り返すことで、上記（３８）式は、下記（４０）式のように変形される。
【数３６】
0006421683.tif 000037
【００９５】
さらに、軌道生成部２２２は、上記（４０）式に対して、下記（４１）式に示す再帰計算を行うことで、上記（３３）式に示すＬＱ最適化問題の最適解ζを高速で求解する。
【数３７】
0006421683.tif 000038
【００９６】
最後に、軌道生成部２２２は、上記求解した最適解ζと、上記（１６）式及び（２０）式（下記２式）と、を用いて、上記（１４）式に示す等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する。
ｘ［ｋ］＝Ｄｋζ［ｋ］＋ｅｋ
ｕ［ｋ］＝Ｎｋζ［ｋ］＋Ｍｋｖ［ｋ］＋ｌｋ
【００９７】
軌道生成部２２２は、算出したｘ［ｋ］（重心Ｇのｘ座標、重心Ｇのｘ軸方向速度、重心Ｇのｙ座標、重心Ｇのｙ軸方向速度、および、各接触点における接触力（６軸力）        ）の時系列データに基づいて、重心軌道を生成する。このようにして、予測区間内において、等式制約条件を満たし、かつ評価関数Ｊを最小化する重心軌道が高速に生成される。すなわち、予測区間内において移動ロボットの安定な移動を実現する重心軌道を高速に生成することができる。
【００９８】
図１４は、上述した直交補空間変換を行った上記（３３）式に示す無制約条件のＬＱ最適化問題に変換する際のフローを示す図である。
等式制約条件の上記（１３）式に対してＱＲ分解を行って、状態変数の変換式である上記（１６）式が導出される（ステップＳ１０１）。
【００９９】
上記入力（接触力（６軸力）の微分値）ｕと重心の状態変数ｘとの関係を示す状態方程式（８）式から導出した（１８）式に対してＱＲ分解を行って、入力ｕの変換式である上記（２０）式が導出される（ステップＳ１０２）。
【０１００】
状態方程式（８）式を、導出された状態変数ｘの変換式（１６）式、入力ｕの変換式（２０）式、及び、状態変数ｘの変換式（１６）式から導出した（２３）式を用いて変形し、状態方程式の変換式である（２５）式が導出される（ステップＳ１０３）。
【０１０１】
上記導出した状態変数ｘの変換式（１６）式と、入力ｕの変換式（２０）式に基づいて、上記（１２）式に示す評価関数を変形し、評価関数の変換式である上記（３１）式が導出される（ステップＳ１０４）。変換後の無制約条件のＬＱ最適化問題は、上述の如く、上記導出された評価関数の変換式（３１）式と、状態方程式の変換式（２５）式と、を含むこととなる。
【０１０２】
図１５は、本実施形態に係る最適制御装置による最適制御方法を示すフローチャートである。
接触点計画設定部２２１は接触点計画（等式制約条件のＣｋ及びｄｋ）を設定する（ステップＳ２０１）。
【０１０３】
軌道生成部２２２は、接触点計画設定部２２１により設定された接触点計画に基づいて、上記（３３）式のＬＱ最適化問題を行列表現し、その最適解条件に対して再帰的計算を行うことで、ＬＱ最適化問題の最適解ζを求解する（ステップＳ２０２）。
【０１０４】
軌道生成部２２２は、求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（１４）式に示す等式制約条件付きＬＱ最適化問題のパラメータを復元し、重心の状態変数ｘ［ｋ］及び入力ｕ［ｋ］を算出する（ステップＳ２０３）。
軌道生成部２２２は、算出したｘ［ｋ］の時系列データに基づいて、重心軌道を生成する（ステップＳ２０４）。
【０１０５】
動作制御部２２３は、軌道生成部２２２により生成された重心軌道に従って移動ロボット１００の全身動作させるように、各関節のモータ１ａ〜２４ａを制御する（ステップＳ２０５）。
【０１０６】
以上、本実施形態において、軌道生成部２２２は、等式制約条件付き最適化問題を直交補空間を用いて変換した無制約条件の最適化問題を、再帰的計算法を用いて最適解を求解し、該求解した最適解に基づいて重心の状態変数を算出し、算出した重心の状態変数に基づいて重心軌道を生成する。これにより、モデル予測制御において最適化問題の最適解を高速に求解し重心軌道を生成できる。
【０１０７】
（第２実施形態）
本実施形態において、軌道生成部２２２は、上記等式制約条件に加えて不等式制約条件を加えたＬＱ最適化問題を求解する。ここで、接触点の安定性の拘束を示す不等式制約条件について説明する。
【０１０８】
移動ロボットの接触点が安定して接触を保つ為の不等式制約条件を導入する。
図１６に接触点の座標系（上添え字ｌ（エル）がついている）と、接触多角形（接触点の支持多角形）と、を示した。
【０１０９】
接触点の座標系は、接触点を原点とし、かつ、接触面の姿勢ｒｉに合わせて定義されているとする。
ここで、接触点の座標系で定義される接触力（６軸力）θｉｌを次のように表わす。
θｉｌ＝［ｆｉｘｌ、ｆｉｙｌ、ｆｉｚｌ、τｉｘｌ、τｉｙｌ、τｉｚｌ］Ｔ
【０１１０】
すると、接触力（６軸力）θｉｌは、接触面の姿勢行列Φｉ＝ｒｏｔ（ｒｉ）を用いて下記（４２）式のように表現できる。
なお、ｒｏｔは、オイラー角を姿勢行列に変換する関数である。
【数３８】
0006421683.tif 000039
【０１１１】
接触点が安定して接触を保つ為には、
（１）接触点が離れないこと、
（２）接触点が滑らないこと、
（３）接触点が剥がれないこと、
という３つの制約条件を満たす必要がある。
上記３つの制約条件が理解しやすいように、図１７に、接触点が不安定化する場合を例示した。
【０１１２】
（１）接触点が離れない為には、接触面の鉛直力が正であれば良い。即ち、下記（４３）式を満たす必要がある。
【数３９】
0006421683.tif 000040
【０１１３】
（２）接触点が滑らない為には、接触面に平行な２軸力が摩擦力以下であれば良い。即ち下記（４４）式がその条件である。ただし接触面の摩擦係数をμｉとする。
【数４０】
0006421683.tif 000041
【０１１４】
（３）接触点が剥がれない為の条件は、接触多角形のｈ個の頂点座標
（ｘｉ１ｌ， ｙｉ１ｌ），・・・・・（ｘｉｈｌ， ｙｉｈｌ）
を用いて下記（４５）式のように表される。
（ただし接触多角形の頂点は反時計回りに順に与えられているとする）。
【数４１】
0006421683.tif 000042
【０１１５】
以上、（４３）、（４４）、（４５）式をまとめると次のようになる。
【数４２】
0006421683.tif 000043
【０１１６】
（４６）式に（４２）式を代入し、ｋ番目のサンプリング点としてインデックスを付け加える。すなわち、下記（４７）式は、ｋ番目の接触点が安定な接触を維持するための条件式である。したがって、安定な多点接触動作を実現するためには、全サンプリング点の全接触点において下記（４７）式が成立する必要がある。
【数４３】
0006421683.tif 000044
【０１１７】
ｋ番目のサンプリング点において全接触点が上記（４７）式を満足する為の条件は、下記（４８）式のように表現できる。
【数４４】
0006421683.tif 000045
【０１１８】
（不等式制約条件）
（Ｐｋｘ［ｘ］≦ｑｋ）・・・（４９）
なお、上記不等式制約条件の一般式（４９）式の右辺ｑｋをｑｋ＝Ｏと置けば、上記導出した（４８）式と一致する。上記（１４）式に示す等式制約条件付きＬＱ最適化問題に上記（４８）を加えることで、下記（５０）式に示す等式制約条件及び不等式制約条件付きＬＱ最適化問題が導出される。
【数４５】
0006421683.tif 000046
【０１１９】
本実施形態において、上記実施形態１で行った直交補空間変換に加えて、さらに、上記（４９）式に示す不等式制約条件の変換を行う。具体的には、上記（１６）式を上記（４９）式に代入することで、下記（５１）式を導出する。
【数４６】
0006421683.tif 000047
【０１２０】
以上から、本実施形態において、直交補空間変換を行うことで、上記（５０）式に示す等式制約条件及び不等式制約条件付きＬＱ最適化問題を、下記（５２）式に示す無制約条件のＬＱ最適化問題に変換できる。本実施形態に係る軌道生成部２２２は、下記（５２）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解を求解する。
【数４７】
0006421683.tif 000048
【０１２１】
次に、上記直交補空間変換により変換した無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて求解する方法を説明する。
まず、上記実施形態１と同様に、上記（５２）式を行列表現すると、下記（５３）式乃至（５５）式のように表現できる。
【数４８】
0006421683.tif 000049
【０１２２】
下記（５６）式に示す、上記（５４）式及び（５５）式のラグランジュ乗数を導入する。
【数４９】
0006421683.tif 000050
【０１２３】
続いて、下記（５７）式に示すように、上記（５５）式にスラック変数を導入する。
【数５０】
0006421683.tif 000051
【０１２４】
上記（５６）式及び（５７）式の導入により、上記（５３）式乃至（５５）式に示すＬＱ最適化問題の最適解条件（ＫＫＴ）は、下記（５８）式で表現できる。
【数５１】
0006421683.tif 000052
【０１２５】
上記（５３）式乃至（５５）式は、凸２次計画問題と称されるＬＱ最適化問題であり。内点法やアクティブセット法などの既知の求解法（収束演算）を用いて効率的に求解できる。これら求解法はニュートン法をベースにした求解法であり、ニュートン法の収束演算中でニュートンの方向計算を行い、リカッチ型再帰的計算法による連立一次方程式の求解を行うこととなる。また、凸２次計画問題の計算量の大部分は、この連立一方程式の計算が占めているため、この計算の高速化が非常に有効となる。
【０１２６】
上述の如く、直交補空間変換を行うことで、上記（５０）式に示す等式制約条件及び不等式制約条件付きＬＱ最適化問題を、上記（５２）式に示す無制約条件のＬＱ最適化問題に変換する。これにより、このＬＱ最適化問題の連立一次方程式の求解に、安定かつ高速のリカッチ型再帰的計算法を用いることができる。したがって、ＬＱ最適化問題の最適解を高速に求解できる。
【０１２７】
本実施形態において、軌道生成部２２２は、リカッチ型再帰的計算法による連立一次方程式の求解を、例えば、内点法やアクティブセット法などの収束演算の中で行う。以下、本実施形態において、内点法を用いた求解法を説明するがこれに限定されない。アクティブセット法を用いた求解法も、内点法と同様の手法で求解できる。
【０１２８】
内点法は、上記（５８）式をニュートン法とラインサーチにより効率的に解くことにより、最適解を求解する手法である。なお、本実施形態においては、内点法の中で、最もスタンダードな主双対内点法を用いる場合について説明するが、これに限定されない。
【０１２９】
まず、上記（５８）式にニュートン法を適用すると、下記（５９）式及び（６０）式が導出される。但し、下記（６０）式において、（○の中に×）は要素同志の積を意味し、Λ≒ｄｉａｇ（λ）、Ｚ≒ｄｉａｇ（ｚ）とする。
【数５２】
0006421683.tif 000053
【０１３０】
次に、下記（６１）式及び（６２）式に示すようにcomplementary measure  μとステップ幅αｐを導入する。
【数５３】
0006421683.tif 000054
【０１３１】
ここで、complementary measure  μは収束演算の残差の総計、ステップ幅αｐはλ≧０、ｚ≧０を満足する範囲でニュートン方向への最大のスッテップ幅を求めていると理解すると分かり易い。
【０１３２】
なお、主双対内点法のアルゴリズムを簡略して記載すると以下のようになる。
【数５４】
0006421683.tif 000055
【０１３３】
軌道生成部２２２は、上記主双対内点法を用いて収束演算を行い、最適解ζを求解する。軌道生成部２２２は、上記実施形態１と同様に、上記求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（５０）式に示す等式制約条件及び不等式条件付きＬＱ最適化問題のパラメータを復元し、その最適解であるｘ［ｋ］及びｕ［ｋ］を得る。
【０１３４】
ここで、主双対内点法のアルゴリズム内に示した上記（５９）式の求解について詳細に説明する。まず、下記（６３）式が成立する。
【数５５】
0006421683.tif 000056
【０１３５】
上記（６３）式を用いて、上記（５９）式は、下記（６４）式のように表現できる。
【数５６】
0006421683.tif 000057
【０１３６】
ここで、Θ≒Ｚ−１Λ及びｒｚ′≒ｒｚ−Ｚ−１ｒλと置くと、上記（６４）式は、下記（６５）式のように表現できる。
【数５７】
0006421683.tif 000058
さらに、上記行列（６５）式の各係数を並べ替えると、下記（６６）式のように表現できる。
【数５８】
0006421683.tif 000059
【０１３７】
ここで、下記（６７）式が成立する。
【数５９】
0006421683.tif 000060
【０１３８】
上記（６７）式を用いて、上記（６６）式は、下記（６８）式のように表現できる。
【数６０】
0006421683.tif 000061
但し、上記（６８）式において、パラメータを下記（６９）式のように設定している。
【数６１】
0006421683.tif 000062
【０１３９】
上記（６８）式は、上述した実施形態１のリカッチ型再帰的計算法で示した（３８）式と同様の形となっている。したがって、軌道生成部２２２は、上記（６８）式に示す連立１次方程式についても、上記実施形態１と同様に、リカッチ型再帰的計算法を用いて高速かつ安定的に求解できる。
【０１４０】
すなわち、軌道生成部２２２は、上記（６８）式に対して、下記（７０）式に示す再帰計算を繰り返す。
【数６２】
0006421683.tif 000063
【０１４１】
さらに、軌道生成部２２２は、下記（７１）式に示す再帰計算を行うことで、（Δｖｋ、Δζｋ、Δｙｋ）を算出する。
【数６３】
0006421683.tif 000064
軌道生成部２２２は、算出したΔζｋを上記（６７）式に代入することで、Δｚｋを算出する。軌道生成部２２２は、算出したΔｚｋ＝[Δｚ１Ｔ、Δｚ２Ｔ、・・・ΔｚＮＴ]Ｔを上記（６３）式に代入することで、Δλを算出する。以上により、上記（５９）式の求解が完了する。
【０１４２】
図１８は、上述した等式制約条件及び不等式制約条件付きＬＱ最適化問題の最適解の求解フローを示すフローチャートである。
まず、軌道生成部２２２は、解ベクトルの初期解（η＝η０、ｙ＝ｙ０、ｚ＝ｚ０、λ＝λ０）を行う（ステップＳ３０１）。
【０１４３】
軌道生成部２２２は、繰返パラメータｎ＝０を設定する（ステップＳ３０２）。
軌道生成部２２２は、上記（６０）式を用いて、残差［ｒη、ｒｙ、ｒｚ、ｒλ］を算出する（ステップＳ３０３）。
【０１４４】
軌道生成部２２２は、リカッチ型再帰的計算法によるニュートン方向［Δη、Δｙ、Δｚ、Δλ］の計算を行う（ステップＳ３０４）。
軌道生成部２２２は、上記（６２）式を用いて、ステップ幅αｐを算出する（ステップＳ３０５）。
【０１４５】
軌道生成部２２２は、上記算出したニュートン方向［Δη、Δｙ、Δｚ、Δλ］とステップ幅αｐとに基づいて、下記式を用いて解ベクトルの更新を行う（ステップＳ３０６）。
［η、ｙ、ｚ、λ］＝［η、ｙ、ｚ、λ］＋βαｐ［Δη、Δｙ、Δｚ、Δλ］
【０１４６】
軌道生成部２２２は、上記（６１）式を用いて、complementary measure μを算出する（ステップＳ３０７）。
軌道生成部２２２は、条件（μ＞μmin  and  ｎ＜ｎmax）を満足するか否かを判定する（ステップＳ３０８）。
【０１４７】
軌道生成部２２２は、条件（μ＞μmin  and  ｎ＜ｎmax）を満足すると判定したとき（  ステップＳ３０８のＹＥＳ）、ｎ＝ｎ＋１を設定し、上記（ステップＳ３０３）の処理に戻る。
【０１４８】
軌道生成部２２２は、条件（μ＞μmin  and  ｎ＜ｎmax）を満足しないと判定したとき（ステップＳ３０８のＮＯ）、上記収束したときのηに基づいて、上記（５３）乃至（５５）式からの最適解ζを算出する。そして、軌道生成部２２２は、この最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（５２）式に示す等式制約条件及び不等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する（ステップＳ３０９）。
【０１４９】
（第３実施形態）
上記実施形態１に係る軌道生成部２２２は、線形不変な等式制約条件付き最適化問題を求解しているが、本実施形態３に係る軌道生成部２２２は、線形時変な等式制約条件付き最適化問題を求解する。
【０１５０】
例えば、サンプリング間隔が変化するような場合を考えると、Δｔは固定ではなく、Δｔｋのようにサンプリング点毎に変化することとなる。この場合、上記（８）式は、下記（７２）式のように線形時変の制御システムとして表現できる。
【数６４】
0006421683.tif 000065
【０１５１】
ここで、線形時変の最適化問題の状態方程式は、例えば、下記（７３）式に示す関係が成立する。すなわち、（７３）式に示す状態方程式は、線形時変の制御パラメータＡｋ、Ｂｋを含むこととなる。
【数６５】
0006421683.tif 000066
【０１５２】
上記（７３）式、（１２）式、及び（１３）式より、軌道生成部２２２は、下記（７４）式に示す等式制約条件付きＬＱ最適化問題を求解し、重心軌道を生成することとなる。
【数６６】
0006421683.tif 000067
但し、上記（７４）式において、下記（７５）式が成立するものとする。
【数６７】
0006421683.tif 000068
【０１５３】
以上から、実施形態１と同様に、等式制約条件付きＬＱ最適化問題を直交補空間に投影することで、直交補空間変換を行い無制約条件のＬＱ最適化問題を導出する。
まず、上記（１３）式（等式制約条件：Ｃｋｘ［ｋ］＝ｄｋ）より、状態変数ｘの変換式である下記（７６）式が上記実施形態１と同様に導出される。
【数６８】
0006421683.tif 000069
【０１５４】
Ｃｋ＋１Ｂの直交補空間を用いて変数変換を行う。Ｃｋ＋１Ｂを下記（７７）式に示すようにＱＲ分解する。
【数６９】
0006421683.tif 000070
【０１５５】
上記（７７）式を用いて上記（２０）式と同様に、入力ｕの変換式である下記（７８）式を導出する。
【数７０】
0006421683.tif 000071
但し、上記（７８）式における各パラメータを下記（７９）式に示すように設定する。
【数７１】
0006421683.tif 000072
ｋ＝０のときは、上記（７８）式における各パラメータを下記（８０）式に示すように設定する。
【数７２】
0006421683.tif 000073
【０１５６】
以上より、上記（８）式を上記（７３）式、（７６）式、及び（７８）式を用いて変形し、状態方程式の変換式である下記（８１）式を導出する。
【数７３】
0006421683.tif 000074
但し、上記（８１）式における各パラメータを下記（８２）式に示すように設定する。
【数７４】
0006421683.tif 000075
ｋ＝０のときは、上記（８１）式における各パラメータを下記（８３）式に示すように設定する。
【数７５】
0006421683.tif 000076
【０１５７】
上記実施形態１と同様に、上記（１２）式に示す評価関数Ｊを変形し、評価関数の変換式である下記（８４）式を導出する。
【数７６】
0006421683.tif 000077
但し、上記（８４）式における各パラメータを下記（８５）式に示すように設定する。
【数７７】
0006421683.tif 000078
【０１５８】
以上から、本実施形態に係る軌道生成部２２２は、実施形態１と同様に、上記直交補空間を用いて変換した下記（８６）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解ζを高速に求解できる。
【数７８】
0006421683.tif 000079
【０１５９】
最後に、軌道生成部２２２は、上記求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（７４）式に示す等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する。
【０１６０】
（第４実施形態）
本実施形態４に係る軌道生成部２２２は、線形時変な、所定の区間内だけ接触力が変化しないように設定した、入力を含む等式制約条件付き最適化問題を求解する。
【０１６１】
例えば、未来のサンプリング区間において、移動ロボットが一定の力で物体を押して動かす等の、接触力を変動させたくない区間が存在する場合を想定する。より具体的には、２番目の接触点と最後から２番目の接触点をある区間内だけ接触力を変化しないようにした場合、当該区間における等式制約条件を下記（８７）式に示すように入力Ｅｋ（ｕ）を含むこととなる。
【数７９】
0006421683.tif 000080
【０１６２】
なお、上記接触力を変動させたくない区間以外の区間においては、Ｃｋは下記（８８）式のように設定できる。
【数８０】
0006421683.tif 000081
【０１６３】
上記式より、軌道生成部２２２は、下記（８９）式に示す入力を含む等式制約条件付きＬＱ最適化問題を求解し、重心軌道を生成することとなる。
【数８１】
0006421683.tif 000082
但し、下記（９０）式が成立する。
【数８２】
0006421683.tif 000083
【０１６４】
まず、等式制約条件内の入力ｕに対する係数Ｅｋの転置行列をＱＲ分解すると下記（９１）式が導出される。
【数８３】
0006421683.tif 000084
【０１６５】
上記（９１）式を用いて、上記入力を含む等式制約条件の（８７）式を、下記（９２）式に示すように変換できる。
【数８４】
0006421683.tif 000085
【０１６６】
上記（９２）式を上記状態方程式（８）式に代入することで、下記（９３）式が導出される。
【数８５】
0006421683.tif 000086
以降の式変換の方法は、上記実施形態３と同一であるため、省略して説明する。以上から、線形時変な、入力を含む等式制約条件付き最適化問題を直交補空間変換を行い、下記（９４）に示す無制約条件のＬＱ最適化問題を導出する。
【数８６】
0006421683.tif 000087
【０１６７】
軌道生成部２２２は、導出した上記（９４）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解ζを高速に求解できる。最後に、軌道生成部２２２は、上記求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、上記（８９）式に示す入力を含む等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する。なお、軌道生成部２２２は、上記同様に、線形不変な、入力を含む等式制約条件付き最適化問題を求解してもよい。
【０１６８】
（第５実施形態）
本実施形態４に係る軌道生成部２２２は、線形時変な、所定の区間内だけ接触力が変化しないように設定した入力を含む等式制約条件、及び、所定の区間内だけ接触力に制限をかけるように設定した入力を含む不等式制約条件付き最適化問題を求解する。
【０１６９】
例えば、移動ロボットの手先や足先の急激な接触力の変動を防ぐように、接触力の変化に制限をかけたい場合を想定する。例えば、２番目の接触点の接触力の増加量と、最後から２番目の接触点の接触力の減少量と、をある区間内だけ、制限したい場合、制限をかけたい区間における不等式制約条件は、下記（９５）式のように入力Γｋｕ[ｋ]を含むこととなる。
【数８７】
0006421683.tif 000088
【０１７０】
但し、上記（９５）式において、Δｆｌｍは各接触点の６軸力の制限値を縦に並べたベクトルである。また、上記制限をかけたい区間以外の区間においては、Ｐｋは下記（９６）式のように設定できる。
【数８８】
0006421683.tif 000089
【０１７１】
上記式より、軌道生成部２２２は、下記（９７）式に示す入力を含む等式制約条件付きＬＱ最適化問題を求解し、重心軌道を生成することとなる。
【数８９】
0006421683.tif 000090
但し、下記（９８）式が成立する。
【数９０】
0006421683.tif 000091
【０１７２】
不等式制約条件を示す上記（９５）式に上記（９２）式を代入することで、不等式制約条件を下記（９９）式に変換する。
【数９１】
0006421683.tif 000092
但し、上記（９９）式のパラメータを下記（１００）式のように設定する。
【数９２】
0006421683.tif 000093
【０１７３】
上記（３３）式と上記（９９）式から、軌道生成部２２２は、下記（１０１）式に示す無制約条件のＬＱ最適化問題を、リカッチ型再帰的計算法を用いて最適解ζを求解することとなる。
【数９３】
0006421683.tif 000094
以降に示す、上記無制約条件のＬＱ最適化問題に対するリカッチ型再帰的計算法による最適解ζの求解方法は、上記実施形態２において説明した求解方法と略同一であるため、相違点のみを説明する。
【０１７４】
上記（５３）式乃至（５５）式の係数行列Ｐが下記（１０２）式に置き換わる。
【数９４】
0006421683.tif 000095
【０１７５】
したがって、上記（６６）式は、下記（１０３）式に置き換わる。
【数９５】
0006421683.tif 000096
【０１７６】
ここで、下記（１０４）式が成立する。
【数９６】
0006421683.tif 000097
【０１７７】
上記（１０４）式を用いて、上記（１０３）式を下記（１０５）式のように変形できる。
【数９７】
0006421683.tif 000098
【０１７８】
但し、上記（１０５）式の各パラメータを下記（１０６）式のように設定する。
【数９８】
0006421683.tif 000099
実施形態２の上記（６８）式と上記（１０５）式との相違は、Ｓｋ（ハット）がＳ′ｋ（ハット）となっているだけで、その他のパラメータは同一である。したがって、軌道生成部２２２は、以降の計算について、上記実施形態２と同一の計算を行い、最適解ζを高速に求解できる。最後に、軌道生成部２２２は、上記求解した最適解ζと、上記（１６）式及び（２０）式と、を用いて、入力を含む等式制約条件及び不等式制約条件付きＬＱ最適化問題のパラメータを復元し、ｘ［ｋ］及びｕ［ｋ］を算出する。なお、軌道生成部２２２は、上記同様に、線形不変な、入力を含む等式制約条件及び不等式制約条件付き最適化問題を求解してもよい。また、軌道生成部２２２は、線形不変あるいは線形時変な、等式制約条件及び入力を含む不等式制約条件付き最適化問題を求解してもよい。
【０１７９】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
本発明は、例えば、図１５や図１８に示す処理を、ＣＰＵ２１０ａにコンピュータプログラムを実行させることにより実現することも可能である。
【０１８０】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【０１８１】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【０１８２】
１ａ-２８ａ…モータ、１ｂ-２８ｂ…エンコーダ、２５…接触力センサ、１００…移動ロボット、２１０…最適制御装置、２２１…接触点計画設定部、２２２…軌道生成部、２２３…動作制御部。
【図１】
0006421683.tif 000100
【図２】
0006421683.tif 000101
【図３】
0006421683.tif 000102
【図４】
0006421683.tif 000103
【図５】
0006421683.tif 000104
【図６】
0006421683.tif 000105
【図７】
0006421683.tif 000106
【図８】
0006421683.tif 000107
【図９】
0006421683.tif 000108
【図１０】
0006421683.tif 000109
【図１１】
0006421683.tif 000110
【図１２】
0006421683.tif 000111
【図１３】
0006421683.tif 000112
【図１４】
0006421683.tif 000113
【図１５】
0006421683.tif 000114
【図１６】
0006421683.tif 000115
【図１７】
0006421683.tif 000116
【図１８】
0006421683.tif 000117