(57)【特許請求の範囲】
【請求項１】
二以上の移動手段を交互に接地しながら移動する移動ロボットの移動制御方法であって、
前記移動手段が接地する接触点の位置を時系列のデータとした接触点計画を設定する接触点計画設定工程と、
前記接触点計画設定工程で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成工程と、を備え、
前記軌道生成工程は、
軌道が発散する場合には接触点の位置を変更する工程を含み、
前記軌道生成工程は、
移動ロボットへの制御入力と移動ロボットの運動との関係を記述する予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、力学的釣り合いに基づく制約条件と所定の安定性評価基準とを用いて移動ロボットの安定性を確保できる制御入力の時系列データを求め、
この求まった制御入力と前記予測モデルとから、移動ロボットの軌道を得るものであり、
前記制約条件に制約を緩和するスラック変数を含めるとともに、
前記安定性評価基準にスラック変数を含ませるにあたって、制約条件下で安定な軌道が求解できる場合には前記スラック変数ができるかぎり実質的にゼロになり、制約条件下で軌道が発散する場合にはスラック変数に実質的にゼロ以上の値が発生するようにし、
さらに、発生したスラック変数の値を打ち消すように接触点の位置を変更する工程を含む
ことを特徴とする移動ロボットの移動制御方法。
【請求項２】
請求項１に記載の移動ロボットの移動制御方法において、
前記制約条件にスラック変数を含めるにあたっては、
移動ロボットの重心のX軸回りモーメントの釣合い式と、移動ロボットの重心のY軸回りモーメントの釣合い式と、にそれぞれスラック変数を導入する
ことを特徴とする移動ロボットの移動制御方法。
ただし、X軸およびY軸は、鉛直方向であるZ軸に垂直な面内で互いに直交する軸である。
【請求項３】
請求項１または請求項２に記載の移動ロボットの移動制御方法において、
前記予測区間を、位置を変更できる接触点が無い調整不能サブ区間と、位置を変更できる接触点が一つ以上有る調整可能サブ区間と、に区別し、
前記調整可能サブ区間にのみ実質的にゼロ以上のスラック変数が発生することを許容する
ことを特徴とする移動ロボットの移動制御方法。
【請求項４】
請求項１から請求項３のいずれかに記載の移動ロボットの移動制御方法において、
前記制約条件の式には、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを含んでおり、
前記軌道生成工程は、
予測区間内において、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを一度仮設定する工程と、
前記安定性評価基準の関数を制約条件のもとで一度求解した後、その結果に応じて、予測区間内において移動ロボットの動作が安定するように接触点の変更および重心鉛直軌道と重心回りモーメント軌道との修正を行い、
変更後の接触点位置、修正した重心鉛直軌道および重心回りモーメント軌道を用いて少なくとも一回以上、前記安定性評価基準の関数の求解を再計算する
ことを特徴とする移動ロボットの移動制御方法。
【請求項５】
請求項１から請求項４のいずれかに記載の移動ロボットの移動制御方法において、
前記軌道生成工程において予測区間内で軌道を求め、
この予測区間の最初の一点のデータを現在の入力値として移動ロボットを駆動させ、
予測区間を微小時間だけ進めて、前記軌道生成工程を繰り返す
ことを特徴とする移動ロボットの移動制御方法。
【請求項６】
二以上の移動手段を交互に接地しながら移動する移動ロボットであって、
前記移動手段が接地する接触点の位置を時系列のデータとした接触点計画を設定する接触点計画設定部と、
前記接触点計画設定部で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成部と、を備え、
前記軌道生成部は、
軌道が発散する場合には接触点の位置を変更し、
移動ロボットへの制御入力と移動ロボットの運動との関係を記述する予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、力学的釣り合いに基づく制約条件と所定の安定性評価基準とを用いて移動ロボットの安定性を確保できる制御入力の時系列データを求め、
この求まった制御入力と前記予測モデルとから、移動ロボットの軌道を得るものであり、
前記制約条件に制約を緩和するスラック変数を含めるとともに、
前記安定性評価基準にスラック変数を含ませるにあたって、制約条件下で安定な軌道が求解できる場合には前記スラック変数ができるかぎり実質的にゼロになり、制約条件下で軌道が発散する場合にはスラック変数に実質的にゼロ以上の値が発生するようにし、
さらに、発生したスラック変数の値を打ち消すように接触点の位置を変更する
ことを特徴とする移動ロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動ロボットの制御に関する。より具体的には、移動ロボットが安定に移動できるような軌道を生成するための軌道生成技術に関する。
【背景技術】
【０００２】
自律的に移動する機能を備えたロボットが開発されており、典型的には、二足歩行する脚式ロボットがある。二足歩行ロボットにあっては、ＺＭＰを用いた軌道生成と動作制御により、安定な二足歩行移動が実現されている（例えば特許文献１）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００５−１７７８８４号公報
【特許文献２】特許第５０３４２３５号公報
【特許文献３】特許第３２６９８５２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
これからのロボットには様々な環境で活躍することが期待されている。例えば、サービスロボットは人と同じ住空間で活動することが期待される。住空間のような環境でロボットが移動するためには、単に脚式の二足歩行ができるだけでは不十分であると考えられる。例えば、図１のような状況が考えられる。図１において、テーブルが壁の近くにあり、そして、テーブルの奥側において壁に近い方にボトルが置かれている。ロボットがこのボトルを掴むためには、単に二足歩行で進むだけでは無理である。ロボットは、壁やテーブルに手を着きながら移動したり、体を傾けて手を伸ばしたりしなければならない。このように、サービスロボットには、複数の任意の点で環境と接触しながら運動を行える能力が必要である。
【０００５】
今後の説明のために、複数の任意の点で環境と接触しながら運動（移動）することを、本明細書では「多点接触移動」と称することにする。
多点接触移動に対応すべく開発された制御システムが例えば特許文献２に開示されている。（ただし、特許文献２に開示されている方法で滑らかな多点接触移動を実現できるとは言えず種々の問題を抱えている。詳しくは実施形態のなかで説明する。）
【０００６】
多点接触移動を実現するサービスロボットの実用化にあたっては安定性の向上が肝要である。実機を実際に運用すれば、ロボットの姿勢が想定からずれていくことも考慮に入れておかなければならない。関節が指令通りに動かないこともあるし、誤差が累積していくこともある。あるいは、人とロボットとが同じ空間にいれば、ロボットに人や物が接触することもある。何らかの外乱によってロボットの姿勢が想定からずれてしまってきているにもかかわらず、計画された通りの位置に手足を着地（着手）していては安定が保てず、制御が破綻（発散）してしまうのは明らかである。つまり、重心が想定よりも傾き始めているにも関わらず、計画通りに手足を着くと、重心がさらにどんどん倒れていくことになる。
【０００７】
ロボットの姿勢が想定からずれてきた場合には、着地点を計画から変更して重心の安定を保つようにすることが考えられよう。二足歩行のロボットにあっては、遊脚の着地位置を変更することによって安定性を向上させること、つまり、予定していた着地位置を変更して転倒を回避する技術がいくつか知られている（例えば特許文献３）。
【０００８】
従来の人型ロボットの安定化手法は大多数が２足歩行の安定化に関する技術であり、多点接触移動に対応するための技術はこれまであまり開発されてこなかった。２足歩行も単脚状態と両脚状態とが切り替わりながら移動していく動作であるので多点接触移動の一種と言える。しかし、２足歩行の技術と多点接触移動の技術とで大きく違う点がある。２足歩行技術は多くの場合（ＺＭＰがその代表格）、各接触点（２足で言うと左足と右足）が同一平面内に含まれていることを前提にしている。（このことは、例えば、特許文献２の段落００１２にも記載されている。）すなわち、ＺＭＰ制御では、本来的には合計６次元である力・モーメントのうち２次元の情報しか表現できておらず、極めて限定された接触状態の遷移しか扱うことができないのである。これまでのようにZMPをベースにした安定化技術では、多点接触移動の場合を原理的に解くことができない。すなわち、“接触点計画に従って多点接触移動を行いつつ、必要に応じて接触点の位置を変更して安定性を向上させる”という技術はこれまでなかった。（特許文献はもちろん、学術論文にまで調査範囲を広げても上記技術を開示した文献は無い。）
【０００９】
本発明の目的は、接触点計画に基づいて自動的に安定な軌道を生成する移動ロボットの制御方法を提供することにあり、詳細には、接触点計画に従って多点接触移動を行いつつ必要に応じて接触点の位置を変更して安定性を向上させる移動ロボットの制御方法を提供することにある。
【課題を解決するための手段】
【００１０】
本発明の移動ロボットの移動制御方法は、
二以上の移動手段を交互に接地しながら移動する移動ロボットの移動制御方法であって、
前記移動手段が接地する接触点の位置を時系列のデータとした接触点計画を設定する接触点計画設定工程と、
前記接触点計画設定工程で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成工程と、を備え、
前記軌道生成工程は、
軌道が発散する場合には接触点の位置を変更する工程を含む
ことを特徴とする。
【００１１】
本発明では、
前記軌道生成工程は、
移動ロボットへの制御入力と移動ロボットの運動との関係を記述する予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、力学的釣り合いに基づく制約条件と所定の安定性評価基準とを用いて移動ロボットの安定性を確保できる制御入力の時系列データを求め、
この求まった制御入力と前記予測モデルとから、移動ロボットの軌道を得るものであり、
前記制約条件に制約を緩和するスラック変数を含めるとともに、
前記安定性評価基準にスラック変数を含ませるにあたって、制約条件下で安定な軌道が求解できる場合には前記スラック変数ができるかぎり実質的にゼロになり、制約条件下で軌道が発散する場合にはスラック変数に実質的にゼロ以上の値が発生するようにし、
さらに、発生したスラック変数の値を打ち消すように接触点の位置を変更する工程を含む
ことが好ましい。
【００１２】
本発明では、
前記制約条件にスラック変数を含めるにあたっては、
移動ロボットの重心のX軸回りモーメントの釣合い式と、移動ロボットの重心のY軸回りモーメントの釣合い式と、にそれぞれスラック変数を導入する
ことが好ましい。
ただし、X軸およびY軸は、鉛直方向であるZ軸に垂直な面内で互いに直交する軸である。
【００１３】
本発明では、
前記予測区間を、位置を変更できる接触点が無い調整不能サブ区間と、位置を変更できる接触点が一つ以上有る調整可能サブ区間と、に区別し、
前記調整可能サブ区間にのみ実質的にゼロ以上のスラック変数が発生することを許容する
ことが好ましい。
【００１４】
本発明では、
前記制約条件の式には、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを含んでおり、
前記軌道生成工程は、
予測区間内において、移動ロボットの重心鉛直軌道と重心回りモーメント軌道とを一度仮設定する工程と、
前記安定性評価基準の関数を制約条件のもとで一度求解した後、その結果に応じて、予測区間内において移動ロボットの動作が安定するように接触点の変更および重心鉛直軌道と重心回りモーメント軌道との修正を行い、
変更後の接触点位置、修正した重心鉛直軌道および重心回りモーメント軌道を用いて少なくとも一回以上、前記安定性評価基準の関数の求解を再計算する
ことが好ましい。
【００１５】
本発明では、
前記軌道生成工程において予測区間内で軌道を求め、
この予測区間の最初の一点のデータを現在の入力値として移動ロボットを駆動させ、
予測区間を微小時間だけ進めて、前記軌道生成工程を繰り返す
ことが好ましい。
【００１６】
本発明の移動ロボットは、
二以上の移動手段を交互に接地しながら移動する移動ロボットであって、
前記移動手段が接地する接触点の位置を時系列のデータとした接触点計画を設定する接触点計画設定部と、
前記接触点計画設定部で設定したように前記移動手段を接触点に接地しながら移動するための軌道を生成する軌道生成部と、を備え、
前記軌道生成部は、
軌道が発散する場合には接触点の位置を変更する
ことを特徴とする。
【００１７】
本発明では、
前記軌道生成部は、
移動ロボットへの制御入力と移動ロボットの運動との関係を記述する予測モデルを構築して、この予測モデルによって所定時間幅の予測区間における移動ロボットの状態変化を表わし、
前記予測区間において、力学的釣り合いに基づく制約条件と所定の安定性評価基準とを用いて移動ロボットの安定性を確保できる制御入力の時系列データを求め、
この求まった制御入力と前記予測モデルとから、移動ロボットの軌道を得るものであり、
前記制約条件に制約を緩和するスラック変数を含めるとともに、
前記安定性評価基準にスラック変数を含ませるにあたって、制約条件下で安定な軌道が求解できる場合には前記スラック変数ができるかぎり実質的にゼロになり、制約条件下で軌道が発散する場合にはスラック変数に実質的にゼロ以上の値が発生するようにし、
さらに、発生したスラック変数の値を打ち消すように接触点の位置を変更する
ことが好ましい。
【発明の効果】
【００１８】
本発明によれば、設定された接触点の計画に基づいて移動ロボットが自動的に安定な重心軌道を生成して自律的に移動できる。このとき、必要な分だけ接触点を変更することで制御の破綻を回避し、安定性を向上させることができる。
【図面の簡単な説明】
【００１９】
【図１】ロボットの動作の一例を示す図。
【図２】移動ロボットの機械構成の一例を示す図。
【図３】移動ロボットの機能ブロック図。
【図４】６軸力を示す図。
【図５】コントローラの機能ブロック図。
【図６】接触点計画の概要の一例を示す図。
【図７】接触点計画の一例を示す図。
【図８】予測区間の例を示す図。
【図９】予測区間での動きを表わした図。
【図１０】予測区間のシフトを説明するための図。
【図１１】予測に用いる移動ロボットのモデルを示す図。
【図１２】予測区間の離散化を説明するための図。
【図１３】接触点の座標系と接触多角形とを示す図。
【図１４】接触点が不安定化する場合を例示した図。
【図１５】接触点位置の変更が無理である場合を例示した図。
【図１６】接触位置の変更が可能である場合を例示した図。
【図１７】接触点位置の変更量を例示した図。
【図１８】個別の接触点に対して位置の変更量を定義する様子を例示した図。
【図１９】移動ロボットの移動制御方法の全体フローを示す図。
【図２０】軌道生成工程の手順を説明するための詳細なフローチャート。
【図２１】軌道生成工程の手順を説明するための詳細なフローチャート。
【図２２】軌道生成工程の手順を説明するための詳細なフローチャート。
【図２３】移動ロボットの多質点モデルを示す図。
【図２４】手先、足先の軌道を補間した様子を示す図。
【図２５】４足歩行で手足の着き方がランダムであるような接触点計画を示す図。
【図２６】外乱が無いとした場合の実行結果のスナップショットを示す図。
【図２７】重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。
【図２８】外乱が入る場合の実行結果のスナップショットを示す図。
【図２９】重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフ。
【図３０】スラック変数値のグラフを示す図。
【図３１】変形例を示す図。
【発明を実施するための形態】
【００２０】
本発明の実施形態を図示するとともに図中の各要素に付した符号を参照して説明する。
（第１実施形態）
本実施形態は移動ロボット制御装置に特徴があり、具体的には、移動ロボットの移動動作を制御するための軌道生成に特徴を有するのであるが、具体的な制御（軌道生成）を説明する前に、制御対象となる移動ロボットのハードウェア構成について予め説明しておく。
【００２１】
図２は、移動ロボットの機械構成の一例を示した図である。
移動ロボット１００は、股関節が３軸、膝関節が１軸、足首関節が２軸、さらに、肩関節が３軸（肩ピッチ、肩ロール、肩ヨー）、肘関節が１軸（肘ピッチ）、および、手首関節が３軸（手首ヨー、手首ピッチ、手首ロール）、で夫々構成されている。
（移動ロボットの機械構成はこれに限定されないが、手（腕）の自由度は６以上、足（脚）の自由度も６以上は必要である。）
移動ロボット１００は、各関節にエンコーダ付きモータ１、２、・・・、２８を有している。
各関節のモータ（関節駆動手段）１ａ、２ａ、・・・、２８ａ（図３）は、各関節の関節角度θ１、θ２、・・・、θ２８を調整できる。
一方、各関節のエンコーダ（関節角度検出手段）１ｂ、２ｂ、・・・、２８ｂは、各関節の関節角度θ１、θ２・・・、θ２８を計測することができる。
【００２２】
また、移動ロボット１００は、足先部（足平部）および手先部（手の平部）に接触力センサ２５を有している。
ここで接触力とは６軸力であり、図４に示すように、ｘ軸、ｙ軸およびｚ軸方向の力ｆの組（ｆｘ、ｆｙ、ｆｚ）Ｔと、ｘ軸回り、ｙ軸回りおよびｚ軸回りの力τの組（τｘ、τｙ、τｚ）Ｔと、である。
（なお、ｘ軸およびｙ軸は、鉛直方向であるｚ軸に垂直な面内で互いに直交する軸とする。）
【００２３】
この移動ロボットは、移動時に、右足、左足、右手および左手のうちの一つ以上を床、壁、あるいはテーブルなどに接触させながら移動する。
そこで、本明細書の以下の説明では、右足、左足、右手および左手を接触点候補と称することがある。
また、手先、足先というのは、すなわち、移動手段のことである。
【００２４】
図３は、移動ロボット１００の機能ブロック図である。
移動ロボット１００は、各関節のモータ１ａ〜２４ａ及びエンコーダ１ｂ〜２４ｂと、力センサ２５と、コントローラ２１０と、を備えている。
【００２５】
コントローラ２１０には、各関節のエンコーダ１ｂ〜２４ｂ及び力センサ２５から、センサ検出値が入力される。また、コントローラ２１０は、各関節のモータ１ａ〜２４ａに対して駆動信号を出力する。
【００２６】
コントローラ２１０は、主要なハードウェア構成として、制御処理、演算処理等を行うＣＰＵ（Central Processing Unit）２１０ａと、ＣＰＵ２１０ａによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）２１０ｂと、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）２１０ｃと、を有するマイクロコンピュータにより構成されている。また、これらＣＰＵ２１０ａ、ＲＯＭ２１０ｂ、及びＲＡＭ２１０ｃは、データバス２１０ｄによって相互に接続されている。必要なプログラムを不揮発性記録媒体に記録しておき、必要に応じてインストールするようにしてもよい。
【００２７】
コントローラ２１０の機能ブロック図を示すと図５のようになる。
コントローラ２１０は、ユーザから指令される接触点計画を格納する接触点計画設定部２２１と、安定に実行できる軌道を生成する軌道生成部２２２と、生成された軌道に従って移動ロボット１００の全身動作を実行させる動作制御部２２３と、を有する。
ここで、軌道生成部２２２は、接触点計画に従った動作を実行できる軌道を生成するのであるが、軌道生成には必要に応じた接触点変更を含む。
これら機能部の具体的な処理動作については後述する。
【００２８】
（多点接触移動のための軌道生成方法）
本発明のポイントは、（１）多点接触移動を実現できる軌道を生成し、かつ、（２）必要に応じて接触点の変更を行うこと、にあり、本件出願時点において本願が権利請求したいポイントはどちらかといえば後段、つまり、多点接触移動の安定性を向上させるように接触点を変更（修正）できるようにすること、にある。
しかし、多点接触移動のための軌道生成方法が現時点において公知技術とも言えないので、まず、（１）多点接触移動を実現できる軌道を生成するための方法を説明する。
なお、本出願人は、特願2013-254989（平成２５年１２月１０日出願）においてこの方法を出願しているが、未だ公開されていない。
【００２９】
ちなみに、前述のように特許文献２（特許５０３４２３５号）には多点接触移動に対応すべく開発された制御システムが記載されている。
本願としては、特許文献２の方法で多点接触移動の軌道を生成しつつ、さらに後述する方法で接触点修正を行ってもよい。
（もっというと、本発明の主眼は「（２）必要に応じて接触点の変更を行うこと」にあるのであるから、特許文献２の方法に限らず多点接触移動を実現できれば軌道生成のための個別具体的な方法は限定されない。）
つまり、本願としては軌道生成方法として特許文献２の方法を排除するわけではないのだが、特許文献２の方法には次のような問題があることを指摘しておく。
特許文献２に開示の技術では、接触点の計画だけではなく、目標重心位置や目標運動量を与えないと安定な軌道を作れないという問題がある。
そもそも、将来の目標重心位置を予め知ることはできないのであり、制御目標値として未知であるはずの将来の重心位置をユーザが設定するというのは無理がある。
ユーザとしてはロボットに接触点の計画だけを与え、あとは、ロボットが設定された接触点の計画に基づいて自動的に安定な重心軌道を生成して自律的に移動してくれることが望ましい。
また、特許文献２に開示の技術では、接触点の切替（接触状態の遷移）に伴う力の変化が急峻になってしまい、滑らかな力の遷移が実現できないという問題もある。
【００３０】
さて、移動ロボットに多点接触移動を安定して行わせるためには、時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成する技術が必要である。
このために本発明者はモデル予測制御を用いることを試みた。
最初にモデル予測制御の概要を説明しておく。
【００３１】
（モデル予測制御の概要説明）
例えば図１に図示したような移動動作をロボットに行わせたいとする。
ここでは、２本の腕と２本の脚とを有する人型ロボットに、テーブルの奥側にあるボトルを掴ませるという一連の動作を想定する。
この場合、ユーザは、この一連動作（タスク）を実行できるような接触点計画を作成する。
つまり、図６のように、手先および足先を、どの順番で、どこに、どのように、着くか、という計画を作成する。
図６においては、床、壁およびテーブルにおいて足先および手先を接触させる箇所にマークを付けている。
【００３２】
この接触点計画は、具体的には図７のようになる。
接触点計画は、左手（ＬＨ）、右手（ＲＨ）、左足（ＬＦ）および右足（ＲＦ）に関し、どの順番で、どこに、どのように、着いていくか、という時系列のデータである。
図１、図６および図７の対応関係を簡単に説明する。
当初（ｔ０）左足１本だけで立ち、遊脚である右足を前に振り出し、そして、右足を着地させる（ｔ１）。
この動きの計画を移動ロボットに指令するには、左足が最初に着地している床上の接触点の座標ＰＬＦ１、そのときの左足の姿勢ｒＬＦ１、そして、右足が着地する床上の接触点の座標ＰＲＦ１、そのときの右足の姿勢ｒＲＦ１、を指定することが必要である。
ここで、接触点の座標は、空間座標としてＰ＝（Ｐｘ、Ｐｙ、Ｐｚ）の組で表わされる。
また、姿勢というのは、接触点に着地したときの足の裏面の向きであり、例えばオイラー角の組としてｒ＝（ｒｘ、ｒｙ、ｒｚ）として表わされる。
（すなわち、ｒｘ、ｒｙおよびｒｚは、ロール、ピッチおよびヨー角をそれぞれ表わす。）
足に関する接触点の座標およびそのときの姿勢を指令する形式は今後の説明でも同様なので、以後は適宜説明を省略する。
【００３３】
両足で立った後、左足を振り出し（ｔ２）、左足を前方に着地する（ｔ４）。
その間に、左手を壁に着くようにする（ｔ３）。
ここで、左手を着く壁上の接触点の座標ＰＬＨ１、および、そのときの左手の姿勢ｒＬＨ１を指定する。
この接触点の座標は空間座標としてＰ＝（Ｐｘ、Ｐｙ、Ｐｚ）の組で表わされ、姿勢は接触点に着いたときの手の平の向きとしてオイラー角の組としてｒ＝（ｒｘ、ｒｙ、ｒｚ）として表わされる。
【００３４】
これ以降の接触点計画は図１、図６および図７を対比して頂ければ自明と思われるので省略する。
このようにしてユーザにより接触点計画が時系列のデータとして作成される。
【００３５】
このように作成された接触点計画が移動ロボットに入力されると、移動ロボットはこの接触点計画を実現するように軌道を生成して自律的に移動する。
このとき、軌道生成にあたって、移動ロボットはモデル予測制御を行う。
すなわち、ある時間幅を持った予測区間内で移動ロボットが安定移動できる軌道を生成し、予測区間を微小時間（Δｔ）ずつシフトさせながら安定動作を行える軌道を順次更新していくようにする。
例えば、図８に予測区間の例を示す。
現在から所定時間（例えば１．６秒）先の未来までを予測区間として設定する。
【００３６】
そして、この予測区間の間で発散しないように安定な軌道を生成する。
この予測区間での動きをイメージしたものが図９である。
このように、ある時間幅を持つ予測区間で安定な軌道を生成した上で、最初の一点だけを現在の入力値として使用する。
【００３７】
次の軌道更新周期（Δｔ秒後）には予測区間をシフトさせ、新たな予測区間において同様に安定な軌道を生成する（図１０参照）。
【００３８】
現在だけ、あるいは、現在から次ぎの制御周期（Δｔ秒）まで、だけを見るのではなく、上記のように、ある程度の未来までを予測区間とし、この予測区間内で発散しない軌道を生成するようにする。
これを繰り返すことで移動ロボットは安定に移動することができる。
【００３９】
さて、ここで問題なのは、ある時間幅を持った予測区間のなかで時々刻々と移り変わっていく接触点に応じて接触力を滑らかに適切に分配し、なおかつ、安定な重心軌道を生成するにはどのようにすればよいか、ということである。
【００４０】
本発明者らは、ある予測区間における安定軌道の生成問題を凸二次計画問題（Quadratic Programming: QP）に帰着させるという着想を得た。
具体的には、各接触点における接触力の二乗和と、前記６軸力の微分値の二乗和と、を含む評価関数Ｊを最小化するという問題を解くことで、多点接触移動の安定軌道を求める。
【００４１】
そこで、次に、この評価関数Ｊの導出およびその解法（凸二次計画問題への帰着）を説明する。
この解法により、ある予測区間内で安定な多点接触移動を実現するための、重心位置、重心速度、接触力および接触力の微分値の時系列データが得られることを示す。（ここからの説明では、まず、接触点計画で指示された通りの位置（接触点）に手足を着くことだけを考える。つまり、接触点を変更することは考えない。必要に応じて接触点を変更してもいいようにするには条件式や評価式を緩和するなどの処置が必要になるが、これについてはさらに後ほど詳しく説明する。）
【００４２】
予測に用いる移動ロボットのモデルを改めて図１１に示す。
移動ロボット全体の慣性を一つの重心Ｇで表わす。各接触点には６軸力を定義する。
【００４３】
この時、重心Ｇの並進運動量をＰ、重心回りの回転運動量（角運動量）をＬ、接触点の数をｎとすると、運動方程式は次のように書ける。
【００４４】
【数１】
0005910647.tif 000002
【００４５】
添え字ｉは接触点のインデックスを表す。
例えば接触点の候補が左手、右手、左足、右足の４点であれば、ｎ＝４（左手：ＬＨ＝１、右手：ＲＨ＝２、左足：ＬＦ＝３、右足：ＲＦ＝４）とすればよい。ただし、床や壁に接触していない接触点候補については接触力を０にするように拘束条件を設定しておく。例えば図１１の例であれば次のようにする。
【００４６】
【数２】
0005910647.tif 000003
【００４７】
（１）式の第１式、第２式を微分すると次の式が得られる。（（１）式はベクトルで表現しているが、これをｘ、ｙ、ｚに分解した上で、上から順に第１式、第２式・・・第６式と称する。）
【００４８】
【数３】
0005910647.tif 000004
【００４９】
本実施形態では、この２式をシステムとして用いる。そして、（１）式の第３から第５式を拘束条件として定式化する。
【００５０】
【数４】
0005910647.tif 000005
【００５１】
さらに、予測区間内を図１２のように、Ｎ個の区間に分割し、（３）式、（４）式を離散化する。（３）式を離散化すると次のようになる。
【００５２】
【数５】
0005910647.tif 000006
【００５３】
また、サンプリング点で常に（４）式の拘束が成り立つとすると、（４）式は次のように離散化される。
【００５４】
【数６】
0005910647.tif 000007
【００５５】
ここで、パラメータを次ぎのように置く。
【００５６】
【数７】
0005910647.tif 000008
【００５７】
θｉは、６軸力としての接触力を並べたベクトルである。そして、ｘは、重心Ｇのｘ座標、重心Ｇのｘ軸方向速度、重心Ｇのｙ座標、重心Ｇのｙ軸方向速度、および、各接触点における接触力（６軸力）、を並べたベクトルである。このｘを、状態変数ｘと称する。さらに、ｕは、接触力（６軸力）の微分値を並べたベクトルである。
【００５８】
このようにパラメータを設定すると、（５）式を次ぎのように記述することができる。
【００５９】
【数８】
0005910647.tif 000009
【００６０】
この（８）式は、（ｊ＋１）のときの状態変数ｘを、その一つ前の状態で記述できることを表わしている。（８）式を用いて予測区間内の状態変数ｘを順に計算していくと次のようになる。
【００６１】
【数９】
0005910647.tif 000010
【００６２】
したがって、時系列的に求められる状態変数ｘを並べて大文字のＸで表わすと、状態変数の時系列データＸを次のように表わすことができる。
【００６３】
【数１０】
0005910647.tif 000011
【００６４】
この（１０）式は、接触力の微分値（U［ｋ］）を入力として、ある予測区間内における移動ロボットの状態遷移を表わす予測モデルとなる。
さて、ここで、本発明者らは、予測区間内において安定な軌道を生成するために次ぎのような評価関数Ｊを導入した。
【００６５】
【数１１】
0005910647.tif 000012
【００６６】
なお、Ｑｉ、Ｒｉは、適宜設定した重みである。
重みの意味や接触点候補にどのように重みを配分するかなどの考え方については後ほど詳しく説明する。ここでは、例えば、接触点候補すべてに力を均等配分すると考えて、Ｑｉはすべて１とし、Ｒｉはすべて１×１０−６とするようなことをイメージして頂ければよい。
【００６７】
ここで、θｉは、６軸力としての接触力の成分を並べたベクトルであった。θｉ（・）は、接触力成分の微分値を並べたベクトルである。（後ろにカッコで付けたドットは微分を表わすと解釈されたい。）したがって、（１１）式は、「予測区間内で、接触力（６軸力）と接触力（６軸力）微分値との２乗和を最小化する」という意味の式である。（１１）式の第１項は、接触力（６軸力）の２乗和を最小化することを意味する。
この第１項には、次の作用が含まれている。
（１）各接触点への接触力を均等分配すること。これにより、重心をできる限り安定な位置に動かすという効果がある。
（２）不必要な内力を打ち消すこと。
（３）接触点の接地安定性を高めること。すなわち、接触面内の反力中心点を接触面の中心に設定するという効果がある。
【００６８】
また、（１１）式の第２項は、接触力（６軸力）微分値（６軸力の時間変化率）の２乗和を最小化することを意味する。
この第２項には次の作用が含まれている。
（１）重心の発散を抑制すること。
（２）滑らかに接触力を切り替えていくこと。
【００６９】
これらをＱ、Ｒという重みによって適切に足し合わせることによって、この評価関数Ｊを最小化するということは、
「高い接触安定性、滑らかな接触力遷移、最低限の内力、といった条件を満たしながら、安定な重心軌道と各接触点の接触力とを出力する」
ということを意味することとなる。
【００７０】
（１１）式を離散化すると次の式が得られる。
【００７１】
【数１２】
0005910647.tif 000013
【００７２】
ここで、接触力（６軸力）については状態変数ｘに含まれているので、（７）式を変形すると次のようになる。
【００７３】
【数１３】
0005910647.tif 000014
【００７４】
したがって、（１２）式の一つめのシグマの計算は次ぎのようになる。
【００７５】
【数１４】
0005910647.tif 000015
【００７６】
したがって、（１２）式は次のようになる。
【００７７】
【数１５】
0005910647.tif 000016
【００７８】
さらに、（１０）式を用いることで評価関数Ｊを次のように定式化することができる。
【００７９】
【数１６】
0005910647.tif 000017
【００８０】
次に拘束条件について考える。
拘束条件としては、
（２）式のように非接触の接触点候補に対して６軸力が０という拘束、
（６）式の鉛直方向の力の釣り合いの拘束、および、
（６）式のｘｙ軸回りのモーメントの釣り合いの拘束、
が予測区間の全サンプリング点に渡って成り立つ必要がある。
ここで、例えば、あるサンプリング点ｊにおいて、ｉ番目とｉ＋２番目の接触点が非接触であったとする。
この時、（２）式および（６）式は次のように記述できる。
【００８１】
【数１７】
0005910647.tif 000018
【００８２】
ここで、係数行列Ｃ［ｊ］、ｄ［ｊ］の成分はサンプリング点によって異なるが、接触点候補の接触／非接触といった情報や接触点位置は既に与えられている。
【００８３】
重心鉛直軌道（Ｇｚ［ｊ］）や重心回りモーメント（Ｌｘ（・）［ｊ］、Ｌｙ（・）［ｊ］）については詳しくは後述するが、ここでは、差し当って次ぎのように理解されたい。
もしも、重心鉛直軌道や重心回りモーメントが定まっていない場合には仮の値を設定して利用する。例えば、一つ前の制御サイクルで用いた重心鉛直軌道、重心回りモーメントを仮の値として利用してもよい。
もしくは、重心鉛直軌道、重心回りモーメントは予測区間内においては現時点の状態から動かない、と仮設定してもよい。
そして、仮の値で一度計算したあとで、得られた結果を用いて重心鉛直軌道、重心回りモーメントを更新し、求解を繰り返す。
このようにして、重心鉛直軌道や重心回りモーメントについても既知の値として計算を進行させ、ずれについては更新計算のなかで修正するとする。
【００８４】
さて、（１７）式が未来の全サンプリング点で成り立つためには、次式が成り立つ必要がある。
【００８５】
【数１８】
0005910647.tif 000019
【００８６】
この式に（１０）式を代入する。
【００８７】
【数１９】
0005910647.tif 000020
【００８８】
最後に、接触点が安定して接触を保つ為の条件を導入する。
図１３に接触点の座標系（上添え字ｌ（エル）がついている）と、接触多角形（接触点の支持多角形）と、を示した。
接触点の座標系は、接触点を原点とし、かつ、接触面の姿勢ｒｉに合わせて定義されているとする。
ここで、接触点の座標系で定義される接触力（６軸力）θｉｌを次のように表わす。
θｉｌ＝［ｆｉｘｌ、ｆｉｙｌ、ｆｉｚｌ、τｉｘｌ、τｉｙｌ、τｉｚｌ］Ｔ
すると、接触力（６軸力）θｉｌは、接触面の姿勢行列Φｉ＝ｒｏｔ（ｒｉ）を用いて次ぎのように表現できる。
なお、ｒｏｔは、オイラー角を姿勢行列に変換する関数である。
【００８９】
【数２０】
0005910647.tif 000021
【００９０】
接触点が安定して接触を保つ為には、
（１）接触点が離れないこと、
（２）接触点が滑らないこと、
（３）接触点が剥がれないこと、
という３つの条件を満たす必要がある。
上記３つの要件が理解しやすいように、図１４に、接触点が不安定化する場合を例示した。
【００９１】
（１）接触点が離れない為には、接触面の鉛直力が正であれば良い。即ち、次式を満たす必要がある。
【００９２】
【数２１】
0005910647.tif 000022
【００９３】
（２）接触点が滑らない為には、接触面に平行な２軸力が摩擦力以下であれば良い。即ち次式がその条件である。ただし接触面の摩擦係数をμｉとする。
【００９４】
【数２２】
0005910647.tif 000023
【００９５】
（３）接触点が剥がれない為の条件は、接触多角形のｈ個の頂点座標
（ｘｉ１ｌ， ｙｉ１ｌ），・・・・・（ｘｉｈｌ， ｙｉｈｌ）
を用いて以下のように表される。
（ただし接触多角形の頂点は反時計回りに順に与えられているとする）。
【００９６】
【数２３】
0005910647.tif 000024
【００９７】
以上、（２１）、（２２）、（２３）式をまとめると次のようになる。
【００９８】
【数２４】
0005910647.tif 000025
【００９９】
（２４）式に（２０）式を代入し、ｊ番目のサンプリング点としてインデックスを付け加える。
【０１００】
【数２５】
0005910647.tif 000026
【０１０１】
（２５）式は、ｊ番目のサンプリング点におけるｉ番目の接触点が安定な接触を保つ為に満たすべき条件であり、安定な多点接触動作を実現する為には、全サンプリング点の全接触点において（２５）式が成り立つ必要がある。
まずｊ番目のサンプリング点において全接触点が（２５）式を満たす為の条件は次のように書ける。
【０１０２】
【数２６】
0005910647.tif 000027
【０１０３】
これが全サンプリング点で成り立つ条件は次のように書ける。
【０１０４】
【数２７】
0005910647.tif 000028
【０１０５】
これに（１０）式を代入する。
【０１０６】
【数２８】
0005910647.tif 000029
【０１０７】
最終的に、（１６）、（１９）、（２８）式により、多点接触時の安定軌道生成問題は次の凸２次計画問題に帰着される。
【０１０８】
【数２９】
0005910647.tif 000030
【０１０９】
凸２次計画問題（Quadratic Programming: QP）にまで帰着できてしまえば、凸２次計画問題（ＱＰ）自体はよく知られた最適化問題の１つであり、高速な求解が可能である。（求解するとＵ［ｋ］が求まる。）
ＱＰの求解後は、重心軌道と各接触点の接触力とが次ぎのようにして計算される。
【０１１０】
【数３０】
0005910647.tif 000031
【０１１１】
このようにして、予測区間内において常に拘束条件（１）−（３）を満たし、かつ、評価関数Ｊを最小化するＸ［ｋ＋１］が求まる。
Ｘ［ｋ＋１］というのは、（７）式と（１０）式とを見てわかる通り、重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標、重心のｙ軸速度、および、各接触点の接触力（６軸力）の時系列データである。
【０１１２】
さらに、式の展開の途中で説明したように、重心の鉛直軌道（ｚ座標）や重心回りモーメントについては更新計算のなかで修正されたものが得られている（詳しくは後述する）。すなわち、これで予測区間内で安定な移動を実現する軌道を得ることができたわけである。
【０１１３】
用語が混乱しないように付言しておく。
上記の演算過程およびこの演算で求められる結果を見て分かるように、ここでいう「軌道」というのは、重心の三次元的軌道に加えて、各接触点で発生すべき接触力（６軸力）の時系列データを含めて考えると分かりやすいであろう。
なお、移動ロボットを移動させるための「軌道」といえば一般的には手先、足先の軌道も含む概念であろうが、ここまでの説明における「軌道」には、手先、足先の軌道を含んでいてもいなくても大差無い。
接触点計画通りに手足を着くことを前提にすれば、手先、足先の軌道は、接触点計画、重心軌道および接触力の時系列データから補間によってほぼ自動的に決定され得る。
（ただし、後述するように、接触点の変更まで考慮にいれるようになると、手先、足先の軌道も重要になってくる。）
【０１１４】
（制約の緩和）
ここまでの説明では、接触点計画通りに手足を着くことを前提にしていた。しかし、各種外乱があるような場合には接触点計画通りに手足を着くだけではロボットの安定軌道が定まらないという事態が起こりうる。例えば、ロボットが後ろから強く押される等して大きな外乱が入ったとする。すると、後ろから押された分だけ、ロボットの重心は大きな速度を持つ。このようにロボットの重心に大きな速度が発生した状態からモデル予測制御で軌道を求めたとする。つまり（２９）式を解いて（３０）式のように求解しようとしたとする。この場合、接触点で発生可能な接触力では止めきれず、軌道が発散してしまうという事態になる。
【０１１５】
例えば二足歩行ロボットで考えてみる。後ろから強く押されてロボットの上体姿勢が前のめりになったら、遊脚の着地点を想定していたよりも前に大きく踏み出して上体が倒れるのを防がなければならない。もし、計画通りに着地してしまうと、上体の倒れを止めきれずにロボットがますます倒れていき、ロボットが倒れてしまう。（計画通りの着地点でロボットの倒れを防ごうと無理に求解するとすれば、接触点（足裏）に過大な接触力を発生させたり各関節を極めて大きな力で駆動したり必要があるであろうがそんなことは無理である。）これはつまり、外乱によって重心に大きな速度が発生した時点で、未来に計画していた接触点の接触位置では物理的に軌道を安定にすることができない状態だった、ということである。
【０１１６】
ところで、（１）式のダイナミクスは、重心に関する力の釣合式とモーメントの釣合式とを表しているが、例えばモーメントの釣合式を無視すると“発散”は起こらない。例えば地面と一点で接触している状況（つまり一本足で立っている状況）を考えると、ロボットのダイナミクスは倒立振子になるので、足平で踏ん張りきれない位の大きな外力が作用した場合、重心は発散する。ところがモーメントの釣合式を無視してしまうと、ダイナミクスは力の釣合式（力と加速度の関係式）のみとなり、“発散”することはない。即ち、力の釣合式にモーメントの釣合式が加わることで、倒立振子のダイナミクスが生まれていると考えることができる。
【０１１７】
そこでスラック変数を導入してモーメントの釣合式を破ることを許すようにする。すなわち、（１７）式の拘束条件において、モーメントの釣り合い式にスラック項εｘ、εｙを導入し、モーメントの釣り合いを破ることを許容するようにする。
【０１１８】
【数３１】
0005910647.tif 000032
【０１１９】
そして、このスラック項を評価関数（（１２）式）に含めるようにする。
【０１２０】
【数３２】
0005910647.tif 000033
【０１２１】
（３２）式におけるスラック変数項（ρ（｜εｘ｜＋｜εｙ｜）は、スラック変数に絶対値符号を付けて評価関数に足し合わされている。これは、スラック変数をできる限り０にすることを意味している。ρは、スラック変数に掛ける重みである。
【０１２２】
重心が発散し始めると重心の加速度や躍度も急激に上昇していくので、（３２）式の第１項および第２項は急激に増大していく。スラック変数を増加させてモーメントの釣り合いを破ると発散が抑制されるので（３２）式の第１項および第２項の増加は抑制されるが、もちろん、スラック変数項の値は増加することになる。
【０１２３】
（３２）式、（３１）式の意味を正確ではないが少しイメージしやすく説明しておく。
（３２）式を見てもらうと、スラック項は二重シグマ計算の外にある。
このことは、イメージ的にいうと、予測区間内においてモーメントの釣り合いを保てず重心軌道が発散してしまうような場合には、予測区間全体でロボットが少し傾いたような状態の解を許容する、といった感じである。（ただし、予測区間内においてもスラック変数の発生を許容できる場合と許容できない場合がある。これは後述する。）
【０１２４】
スラック変数の発生をどの程度許容するかは、ＱやＲとスラック変数重みρとの比（バランス）で決まってくる。重み（Q、R、ρ）のバランスを適切に設定することによって、軌道が発散しない時にはスラック変数（εｘ、εｙ）は０のままで、軌道が発散してしまう時だけ必要十分なスラック変数の値が発生する、という機能を実現できる。また、この後の展開で説明するが、「スラック変数値≒接触点位置の変更量」となるので、外乱に対してどの程度敏感に接触点位置の変更を行うか、をスラック変数重みρを使ってユーザが調整することも可能である。
【０１２５】
では、（３１）式、（３２）式を用いて凸２次計画問題を導く。
【０１２６】
スラック項εｘ、εｙを次のように置き換える。これは、絶対値を外すための処置である。
【０１２７】
【数３３】
0005910647.tif 000034
【０１２８】
また、入力ベクトルUを次のように拡張する。要は、入力ベクトルUにスラック変数が含まれるようにする。
【０１２９】
【数３４】
0005910647.tif 000035
【０１３０】
ここで、スラック変数項は、（３３）式により次のように変換できる。
【０１３１】
【数３５】
0005910647.tif 000036
【０１３２】
したがって、（３２）式の評価関数Ｊ１は次のようになる。
【０１３３】
【数３６】
0005910647.tif 000037
【０１３４】
さらに整理すると、評価関数J１は次のようになる。
【０１３５】
【数３７】
0005910647.tif 000038
【０１３６】
次に、制約条件について考えてみる。
制約条件（（３１）式）を次のように書き直す。（先ほどの（３１）式では、ｄ［ｊ］のなかにスラック項を含ませていたが、スラック項を明示的に表わすようにする。）
【０１３７】
【数３８】
0005910647.tif 000039
【０１３８】
ここで注意すべきことは、スラック項をいつでも発生させられるわけではない、ということである。
すなわち、場合分けが必要になる。
この場合分けは、予測区間内において、
位置を変更できる接触点があるサブ区間と、
位置を変更できる接触点がないサブ区間と、を区別する、ということである。
【０１３９】
（３１）式で導入したスラック変数が発生するということは、物理的にロボットが満たすべき力学的な釣り合い式（モーメント釣合式）から逸脱してしまっていることを意味する。
後述するように、スラック変数が発生するような場合には接触位置を変更することによってスラック項を打ち消し、最終的にはスラック変数が発生しないような軌道を生成しなければならない。
【０１４０】
すなわち、位置を変更できる接触点がない状況ではスラック項が発生したとしても接触位置の変更による補償ができないので、制約の緩和を行ってはならない。これは例えば、予測区間内の接触点計画が、図１５のようにずっと両足立ちという状況を考えると分かり易い。このような状況では接触位置変更による安定化は不可能であり、スラック変数による制約の緩和を行ってはならない。
【０１４１】
一方、図１６のような状況であれば、この予測区間内の途中から左足や右足が着地し始めるので、これら左足や右足の接触位置を変更することによって安定化を図ることができるサブ区間が存在している。つまり、このサブ区間であれば、スラック項を打ち消すように接触点の位置を変更することが可能である。したがって、この図１６の予測区間において、前半は「位置を変更できる接触点がないサブ区間」であり、遊脚であった左足を着地する時点以降は「位置を変更できる接触点があるサブ区間」ということである。
なお、一点でも位置変更可能な接触点があれば、その時点以降はすべて「位置を変更できる接触点があるサブ区間」とする。
【０１４２】
したがって、「位置を変更できる接触点がないサブ区間」では、スラック項は全部０である。
【０１４３】
本明細書においては、以降の説明において、「位置を変更できる接触点があるサブ区間」を「調整可能サブ区間」と称し、「位置を変更できる接触点がないサブ区間」を「調整不能サブ区間」と称することがある。
【０１４４】
【数３９】
0005910647.tif 000040
【０１４５】
一方、「位置を変更できる接触点があるサブ区間」（調整可能サブ区間）では、スラック項を許容するようにスラック項を導入する。
【０１４６】
【数４０】
0005910647.tif 000041
【０１４７】
このように「位置を変更できる接触点があるサブ区間」（調整可能サブ区間）でのみスラック変数を用いて制約緩和を行うことを許容する。
【０１４８】
このことを踏まえて（３８）式を変形していく。
（３８）式の要素を明示的に書くと次のようになる。
【０１４９】
【数４１】
0005910647.tif 000042
【０１５０】
この（４１）式に（１０）式を代入し、移項して整理すると次の式が導かれる。
【０１５１】
【数４２】
0005910647.tif 000043
【０１５２】
ただし、Ｖεを次のようにおいた。
【０１５３】
【数４３】
0005910647.tif 000044
【０１５４】
Ｖεの最後の列にスラック項を導入している。
ここでは、前半はスラック項を０とし、後半はスラック項を許容するように表現している。
これは、予測区間の内の前半が「位置を変更できる接触点がないサブ区間」（調整不能サブ区間）で、予測区間の内の後半で「位置を変更できる接触点があるサブ区間」（調整可能サブ区間）となる場合を分かりやすく表現したものである。
いつもこの通りになるとは限らない。例えば、予測区間の全体が「位置を変更できる接触点がないサブ区間」（調整不能サブ区間）になることも有り得る。
しかし、予測区間の全体が「位置を変更できる接触点がないサブ区間」（調整不能サブ区間）の場合を取り上げていては以後の話しが続かないので、予測区間内に調整可能サブ区間を持つ場合を例にしたのである。
【０１５５】
さらに、（２８）式をＵεに合わせて拡張する。スラック変数を（３３）式のように非負と置いたことに留意しつつ、接触点が安定して接触を保つ為の不等式条件式を導く。
【０１５６】
【数４４】
0005910647.tif 000045
【０１５７】
以上、（３７）式、（４２）式および（４４）式をまとめると次のように第１凸２次計画問題として表わされる。
【０１５８】
【数４５】
0005910647.tif 000046
【０１５９】
この第１凸二次計画問題を解くことにより軌道を求めることができる。
（スラック変数が必要に応じて発生するので、必要であればスラック変数を含んだ形のＵε［ｋ］が求まるのであるが、とりあえずスラック変数は度外視してＵ［ｋ］を（３０）式に代入すれば、Ｘ［ｋ＋１］が求まる。つまり、ｘｙ重心軌道と各接触点で発生させる力（６軸力）が求まる。）
このとき、スラック変数に値を発生させてもよい場合、すなわち、「位置を変更できる接触点があるサブ区間」（調整可能サブ区間）においてどうしても釣り合い式を満たせない場合にはスラック項に値が入った形で解（Ｕε［ｋ］）が求まることになる。
これによって軌道の発散は回避されるのであるが、スラック項の値は接触位置の変更によって補償しなければならない。
【０１６０】
もちろん、スラック変数が無くても釣り合い式を満たせる場合や、「位置を変更できる接触点がないサブ区間」（調整不能サブ区間）では、スラック変数はゼロになる。
なお、厳密にいうと、スラック変数が無くても釣り合い式を満たせるような場合であってもスラック変数は完全に０（ゼロ）ではなく、計算の結果としてはスラック変数には極微小ながら値が発生する。例えば、１０のマイナス１０乗などのオーダーでスラック変数が発生した形で求解される。しかし、１０のマイナス１０乗などのオーダーの値は常識的に考えて実質的にゼロといっていいであろう。
【０１６１】
（接触位置変更量の算出）
発生したスラック変数値から安定性維持に必要な接触位置変更量を求める計算処理について説明する。
（４５）式を求解するとUε［ｋ］が求まり、εが得られるので、（３３）式を用いてεｘ、εｙが得られる。
ここで、第i接触点の接触位置変更量Δｐiを次のように表わし、（６）式のモーメント釣合式に加える。すると、次の（４６）式のようになる。
【０１６２】
【数４６】
0005910647.tif 000047
【０１６３】
この（４６）式から本来満たすべき（６）式を引くと、次のようになる。
【０１６４】
【数４７】
0005910647.tif 000048
【０１６５】
さて、以下では、接触位置変更を接触面内に限定した場合を考える。
すなわち、接触面内を平行移動する形で接触位置変更を行うこととする。歩行の場合でいうと、予定していた地面上の着地位置を地面上で平行移動させることに相当する。
【０１６６】
（着地（着手）位置をもっと自由に移動させることもできる。空間内にどのような面が存在しているかをロボットに教えておけば、着地（着手）位置をもっと自由に選択することは可能である。例えば、棚の下段から棚の上段に着手位置を変えるようなこともできる。ただし、演算負荷が増えることは当然である。）
【０１６７】
ここで、接触点の座標系における接触位置移動量Δｐiｌを導入する。（この上付き文字はエルの小文字。）
【０１６８】
【数４８】
0005910647.tif 000049
【０１６９】
Δｐiｘｌ、Δｐiｙｌは、図１７に示すように、接触点のローカル座標系におけるｘｙ方向変化量で定義されている。接触点の座標系における接触位置移動をグローバル座標系に変換する。
【０１７０】
【数４９】
0005910647.tif 000050
【０１７１】
これを４７式に代入すると、次式を得る。
【０１７２】
【数５０】
0005910647.tif 000051
【０１７３】
ここで、発生したスラック変数を補償する為に必要な接触位置変更量の条件を式で表わすと次のように表現される。
【０１７４】
【数５１】
0005910647.tif 000052
【０１７５】
（５１）式は、接触位置を変更することによる効果（左辺）がスラック変数（右辺）以上になれば良いということを表している。
上式を符号に留意してまとめると次のように表せる。
【０１７６】
【数５２】
0005910647.tif 000053
【０１７７】
（５２）式を整理しなおすと次のようになる。
【０１７８】
【数５３】
0005910647.tif 000054
【０１７９】
ところで、接触位置の変更量は、当然のことながら、対象となる接触点が存在する場合にしか許されない。
例えば図１８に示した例で言うと、まず、「位置を変更できる接触点がないサブ区間」（調整不能サブ区間）では当然ながら接触位置変更量が存在しない。
また、「位置を変更できる接触点があるサブ区間」（調整可能サブ区間）においても、実際に着地（着手）位置を変更できる対象があるところにだけ接触位置の変更量を定義できる。
具体的にいうと、ΔｐLF２ｌ（左足の着地位置変更）はk+s〜k+Nの範囲でのみ許容でき、ΔｐRF２ｌ（右足の着地位置変更）がk+Nのみで許容される。
（分かりやすいように、iをLFやRFと表わした。）
【０１８０】
従って、例えばこの図１８の例に沿って（４１）式を書き直せば、次のようになる。
【０１８１】
【数５４】
0005910647.tif 000055
【０１８２】
さらに、接触位置変更量を縦に書き並べたベクトルUΔを定義しておく。
【数５５】
0005910647.tif 000056
【０１８３】
すると、（５４）式は次式のようにまとめられる。
【０１８４】
【数５６】
0005910647.tif 000057
【０１８５】
接触位置の変更量は出来る限り小さい（必要最小限）ことが好ましいと考え、次式の評価関数をさらに導入する。
【０１８６】
【数５７】
0005910647.tif 000058
【０１８７】
（５７）式を整理すると次式になる。
【０１８８】
【数５８】
0005910647.tif 000059
【０１８９】
（５６）式、（５８）式をまとめると最終的に次ぎのようになる。
【０１９０】
【数５９】
0005910647.tif 000060
【０１９１】
このようにして第２凸２次計画問題が構成できた。
これを解くことによって安定性を維持する為に必要な最小限の接触位置変更量を求めることができる。
なお、接触力変更量をある凸領域内に制限する（可動範囲の考慮）等の機能も、（５９）式の不等式制約条件に条件を追加することで容易に実現できる。
【０１９２】
（移動ロボットの制御）
移動ロボットの移動制御方法を説明する。
図１９は、移動ロボットの移動制御方法の全体フローである。
移動ロボットの移動制御の工程には、大きく分けて、接触点計画の設定（ＳＴ１００）と、軌道生成（ＳＴ２００）と、動作指令（ＳＴ３００）と、動作制御の実行（ＳＴ４００）と、がある。
【０１９３】
接触点計画の設定（ＳＴ１００）においては、これまで概略説明してきたように、手先および足先を、どの順番で、どこに、どのように、着くか、という多点接触移動の計画（図７）をユーザが移動ロボットに入力する。
入力された接触点計画は、コントローラ２１０の接触点計画設定部２２１に記憶される。
【０１９４】
軌道生成工程（ＳＴ２００）について説明する。
図２０、図、図に軌道生成工程（ＳＴ２００）の手順を説明するための詳細なフローチャートを示す。
軌道生成工程（ＳＴ２００）はコントローラ２１０の軌道生成部２２２にて実行される。
まず、ＳＴ２０１において、軌道生成部２２２は現在状態ｘ［ｋ］を設定する。ここで、現在状態（ｘ［ｋ］）として設定すべきは、現在の重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標、重心のｙ軸速度、および、各接触点の接触力（６軸力）である。各接触点における接触力は力センサ２５からの検出値によって得ることができる。重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標および重心のｙ軸速度は、現在の移動ロボットの状態から推定して求める値である。例えば、各エンコーダのセンサ値を取得して移動ロボットの全身姿勢を把握した上で、重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標および重心のｙ軸速度を求めてもよい。あるいは、想定している姿勢に対して実際の移動ロボットの姿勢がどの程度傾いているか（ずれているか）を把握することによって、重心位置を修正するようにしてもよい。
【０１９５】
次に、ＳＴ２０２Ａにおいて、各接触点候補に重み（Ｑｉ、Ｒｉ）を設定する。
（先の式の展開でいうと、重み（Ｑｉ、Ｒｉ）は、（１１）式のところで評価関数Ｊに組み込まれたことを確認されたい。もちろん、制約を緩和してスラック項を取り込んだ（３２）式の評価関数Ｊ１にもこの重み（Ｑｉ、Ｒｉ）は引き継がれている。）
重み（Ｑｉ、Ｒｉ）は、ユーザが予め設定入力しているもので、軌道生成部２２２は、必要に応じて読み出し、更新設定する。
（更新する必要がなければ、再設定の必要はない。）
接触点候補すべてに力を均等配分する場合には、例えば、Ｑｉをすべて１とし、Ｒｉをすべて１×１０−６とするなどしてもよいが、もっと細かく設定することもできる。
この重みを調整することで、接触力の変化をどれぐらい滑らかにするかや、手先にはあまり力を掛けないようにする、などの調整ができる。
【０１９６】
例えば、重みを全部同じにしたとする。すると、演算結果として、着地している手足（各接触点）に均等に力が掛かるような解が得られる。一般的に考えると、この場合が移動ロボットとしては一番安定した形（姿勢）になる。
【０１９７】
一方、重みの配分を変えたい場合もある。
例えば、足に比べて手が弱い（脆弱な）移動ロボットであって手に荷重を掛け過ぎるとよくないという場合には、手の重みを「大きく」する。重みを大きくするほど、重みに乗算されている手のθｉなどが小さくなるので、手には接触力の分配が小さくなる。
【０１９８】
あるいは、テーブルに一方の手を着いて、他方の手を前方に大きく伸ばしたいとする。この場合、手足に均等に力が配分されているだけでは無理であり、体をもっと前に傾ける必要がある。体を傾け、重心をもっと前方にシフトさせるためには、一方の手に掛かる力の分配をやや大きくする必要がある。この場合、一方の手の重みを減らし、体を傾けられるようにし、他方の手をより大きく前方に伸ばすようにする、という調整が必要になる。
【０１９９】
一般的な動作では重みは変更の必要がないが、状況に応じて軌道更新のタイミングで毎回変更してもよい。
【０２００】
ＳＴ２０２Ｂにおいて、スラック項重みρを設定する。
スラック項重みρは、（３２）式のところで導入されたことを確認されたい。ＱやＲとスラック変数重みρとの比（バランス）によってスラック変数の発生をどの程度許容するかを決めることができる点は前述の通りである。
【０２０１】
次に、ＳＴ２０３において、予測区間を設定する。
これは、図８で説明したように、現在から所定の時間幅（例えば１．６秒）を予測区間として設定する、ということである。
【０２０２】
次に、ＳＴ２０４において、この予測区間内の接触点位置およびその姿勢（ｐｉ、ｒｉ）を設定する。これは、接触点計画（図７）のなかから予測区間内の接触点計画を切り出してくることに相当する。
【０２０３】
次に、ＳＴ２０５においては、重心鉛直軌道と重心回りモーメントとを仮設定する。
次ステップの演算工程（ＳＴ２０６）で（４５）式（凸二次計画問題）を解いて、重心のｘｙ方向軌道、各接触点の接触力および接触力微分値、を得るのであるが、しかし、この求解には、各接触点の位置、姿勢、重心鉛直軌道、および、重心回りモーメントが与えられていることが前提となる。
予測区間内における各接触点の位置および姿勢は、接触点計画で与えられる。
一方、この予測区間内における重心鉛直軌道および重心回りモーメントは、未知である。そこで、演算工程（ＳＴ２０６）で（４５）式（凸二次計画問題）が解けるように重心鉛直軌道および重心回りモーメントを一回仮設定する。
【０２０４】
全くの一回目の制御サイクルでは、例えば、重心鉛直軌道、重心回りモーメントは予測区間内においては現時点の状態から動かない、と仮設定してもよい。
（例えば、重心高さは変化しない、と仮定してもよい。）
もしくは、一つ前の制御サイクルで用いた重心鉛直軌道、重心回りモーメントを仮の値として利用してもよい。
予測区間は１．６秒ほどの微小な時間であり、最終的に求まる安定軌道においてもこの間の重心高さの変化はやはり微小である。
したがって、一回目の仮計算としては、重心高さが変化しないとしても、前回の制御周期と同じであるとしても計算は進められる。
【０２０５】
なお、この後、仮の演算で求まった重心のｘｙ方向軌道でも移動ロボットの動きが安定するように重心鉛直軌道および重心回りモーメントを修正する工程（ＳＴ２０９）がある。そして、この修正した重心鉛直軌道および重心回りモーメントを使って再度演算（ＳＴ２０６）するようにループを回すので、最終的には、重心鉛直軌道を含めて安定な軌道が適切に求まることはご理解頂けよう。
【０２０６】
ここで、重心回りモーメントを定義しておく。
移動ロボットをリンク同士が関節でジョイントされたリンク構造体と見なすと、重心回りモーメントは、各リンクに分布した質点の運動によって重心の回りに発生するモーメントの合計値である。
移動ロボットが図２３に示すような多質点モデルで表されており、Ｍ個の質点がそれぞれ、
質量：ｍｉ、
位置：ｓｉ＝［ｘｉ、ｙｉ、ｚｉ］Ｔ
のパラメータを持っているとする。
移動ロボット全体の重心をＧ＝（Ｇｘ、Ｇｙ、Ｇｚ）Ｔとすると、重心回りモーメントは次のように表わされる。
【０２０７】
【数６０】
0005910647.tif 000061
【０２０８】
次に、ＳＴ２０６Ａにおいて第１演算工程を実行する。
具体的には、（４５）式の第１凸二次計画問題を解くということである。
予測区間内において、接触点おのおのに力を適切に分散させつつ、なおかつ、この接触力を実現しながら重心（ここでの重心はｘとｙ）が安定に遷移するような軌道が求められるわけである。
具体的には、Ｈε、Ｆε、Ｐε、Ｓε、Ｖε、Ｗεが求まり、さらに、Ｕε［ｋ］が求まる。
（このＵε［ｋ］は接触力微分値のベクトルであり、さらに、（３４）式でスラック変数を含むように拡張したことを思い出されたい。さらに、Ｖεにおいて、「位置を変更できる接触点がないサブ区間」（調整不能サブ区間）と、「位置を変更できる接触点があるサブ区間」（調整可能サブ区間）と、を区別したことを思い出されたい。（４３）式参照。）
【０２０９】
そして、（３０）式によって、Ｘｏｕｔ［ｋ＋１］が求まる（ＳＴ２０６Ｂ）。（このＸｏｕｔ［ｋ＋１］は、重心のｘ座標、重心のｘ軸方向速度、重心のｙ座標、重心のｙ軸速度、および、各接触点の接触力（６軸力）の時系列データである。）
【０２１０】
さらに、ＳＴ２０７Ａにおいて第２演算工程を実行する。具体的には、５９式の第２凸２次計画問題を解くということである。これを解くことによって、安定性を維持する為に必要な最小限の接触位置変更量が求められる。したがって、ＳＴ２０７Ｂにおいて、接触点位置の修正を行う。
【０２１１】
続いて、ＳＴ２０８において、手先、足先の軌道を補間して求める。
接触点計画においては、接触点とそのときの姿勢だけが設定されている。さらに、先のＳＴ２０７Ｂにおいて安定性を維持できるように接触位置の変更を行った。これらを踏まえて、各接触点間を結ぶように手先、足先の補間軌道を求める。すると、図２４のように、接触点計画で指示された接触点同士の間が埋まる。
【０２１２】
さて、次に、ＳＴ２０９において、重心鉛直軌道と重心回りモーメントとを修正する作業を行う。
まず、重心鉛直軌道を修正する。
これまでの工程により、重心のｘｙ方向の軌道と、手先足先の軌道と、が得られている。（さらに、重心鉛直軌道も仮設定している。）これに基づいて、予測区間内における移動ロボットの姿勢の変化を推定することができるが、このとき、膝や腕が伸びきってしまうなど安定しない要因がある場合には安定するように重心鉛直軌道を調整する。例えば、膝が伸びきりそうになったら重心鉛直軌道を下げる、といった調整を行う。予測区間内における移動ロボットの姿勢が決定したら、それに基づいて重心回りモーメントの計算を行う。
【０２１３】
さて、次に、ＳＴ２１０において、ＳＴ２０６Ａ−ＳＴ２０９を１回以上繰り返したかどうかを判断し、繰り返していなければＳＴ２０６Ａに戻って第１凸二次計画問題の求解を行う。
この繰り返し演算（ＳＴ２０６）によって修正された重心鉛直軌道および重心回りモーメントを用いて再度Ｘｏｕｔ［ｋ＋１］およびＵΔ［ｋ］が求まる。
このようにして、予測区間内において移動ロボットの安定な移動を実現する、重心位置、重心速度、必要な接触点位置の変更、および、接触力の時系列データが求まった（ＳＴ２１１）。
【０２１４】
このように、ある時間幅を持つ予測区間で安定な軌道を生成した上で、最初の一点だけを現在の入力値として使用する。すなわち、最初の一点だけを動作指令として動作制御部２２３に出力する（ＳＴ３００）。
【０２１５】
所定の終了条件を満たすまで周期を更新（ＳＴ２１３）すなわち現時点を微小時間進ませて、先頭（ＳＴ２０１）に戻り、ループを繰り返す。このように、予測区間を微小時間だけ先に進ませながら軌道生成を繰り返す。このようにして多点接触移動を安定に実現する軌道の算出が行われる。
【０２１６】
動作指令を受けた動作制御部２２３が移動ロボット１００をどのように制御して動かすか（関節を駆動させるか）は本実施形態のポイントではないが、簡単に説明しておく。
本実施形態においては、重心および手先足先の軌道に加えて、各接触点における接触力も動作指令として与えられる。そこで、まず、各接触点で指示された接触力が発生するように手先足先の位置（軌道）を微修正するという工程が必要になる（ＳＴ４１０）。例えば、手先をテーブルに着いているときに、比較的大きな接触力が指示された場合には、手先をテーブルに押し込むように手先の軌道を微修正する。
【０２１７】
そして、各関節のモータを駆動して全身を動作させる（ＳＴ４２０）。これで移動ロボットが動作することになる。
【０２１８】
なお、動作の実行（ＳＴ４００）において、接触力制御（ＳＴ４１０）を行ったり、関節が指示した通りに正確に駆動しなかったり、外乱の影響を受けたりして、実機とモデルとの間には当然にずれが生じる。したがって、軌道生成（ＳＴ２００）のループにあっては、毎回、現在状態ｘの設定を行う必要がある（ＳＴ２０１）。
【０２１９】
以上、本実施形態によれば、設定された接触点の計画に基づいて移動ロボットが自動的に安定な重心軌道を生成して自律的に移動できる。
このとき、もし接触点計画通りの着地（着手）ではロボットの安定を保てないとなった場合には、必要な分だけ接触位置を変更することによって軌道の発散を回避し、ロボットの安定動作を継続することができる。
また、軌道生成のために用いた評価関数J１の意味から分かるように、接触点の切替（接触状態の遷移）に伴う力の変化が滑らかになり、移動ロボットの安定かつ滑らかな移動が実現される。
【０２２０】
（実験例）
実験例を説明する。
４足歩行であって手足の着き方がランダムであるような接触点計画をロボットに与えたとする。
この接触点計画のタイムラインを図２５に示す。
そして、図２６、図２７は、外乱が無いとした場合のシミュレーション結果である。
図２６は、実行結果のスナップショットである。
図２７（A）、（B）、（C）は、実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。
この場合は、接触点計画通りの着地（着手）位置に手足を着きながら、接触点の切り替えに伴う接触点反力が滑らかに遷移し、そして、滑らかな重心移動を実現していることが分かる。
【０２２１】
さて、一方、図２８、図２９、図３０は、外乱を作用させた場合のシミュレーション結果である。
図２８は、実行結果のスナップショットである。ここで、２．２秒頃に大きな外乱が入ったとする。
図２９は、（A）、（B）、（C）は、実行時の重心位置、反力中心の位置、および、接触点の鉛直方向荷重、を示したグラフである。
さらに、図３０は、スラック変数値のグラフである。
図２６と図２８とを見比べてもらうと、外乱が入った直後（２．２秒頃）に左足の着地位置を変更することで転倒が回避されていることが分かるであろう。図３０により、外乱が入った直後にスラック変数が発生しているが、その後、外乱が無い状態ではスラック変数が０になることがわかる。
【０２２２】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
移動ロボットとしてのハードウェア構成としては、二足歩行、４足歩行、さらには、６足でも８足でもよいのであって、手足の数に制限があるわけではない。
ただし、少なくとも一つの接触点がどこかに接地している必要がある。（接地、というが、壁に手を着く場合も含めて解釈されたい。）
極端なケースであるが、例えば、図３１のように、足が１本、手が１本で、床と壁に手足を交互に着けながら移動するような移動ロボット１００であってもよい。
【０２２３】
必要な接触点の変更量を求め（ＳＴ２０７Ａ）、それに従って接触点を変更する（ＳＴ２０７Ｂ）という処理は、必要な場合だけに行うという考え方もある。
すなわち、ＳＴ２０６ＡとＳＴ２０７Ａとの間にスラック変数の大きさを所定閾値と対比する判断工程を入れ、実質的にゼロ以上であるスラック変数が発生しているかどうかを判断してもよい。
実質的にゼロ以上であるスラック変数が発生している場合には接触点位置の変更を行う必要があるし、スラック変数が実質的にゼロであればＳＴ２０７Ａ、Ｂの処理は必要無い。
【０２２４】
または、スラック変数の大きさに関わらずＳＴ２０７Ａを実行し、ＳＴ２０７ＡとＳＴ２０８Ｂとの間に接触点位置の変更量を所定閾値と対比する判断工程を入れてもよい。
接触点位置の変更が所定値以上であれば接触点を変更する必要があるし（ＳＴ２０７Ｂ）、接触点位置の変更量が実質的にゼロであればＳＴ２０７Ｂを実行する必要は無い。
閾値をどの程度にするかは求められる安定性の精度によるのであって一概に言えないが、接触点位置の変更量が１mmや２mmになるような場合には接触点の変更を実行する必要があると言えるであろう。
もちろん、求められる安定性の精度によるのであって、０．０１mmや０．０２mmを接触点の変更の閾値とする場合もあるであろう。
接触点位置の変更量が１mmや２mmになる場合のスラック変数を逆算で求めれば、スラック変数の大きさを判定する閾値も得られる。
【符号の説明】
【０２２５】
１ａ-２８ａ…モータ、１ｂ-２８ｂ…エンコーダ、２５…接触力センサ、１００…移動ロボット、２１０…コントローラ、２２１…接触点計画設定部、２２２…軌道生成部、２２３…動作制御部。
【図１】
0005910647.tif 000062
【図２】
0005910647.tif 000063
【図３】
0005910647.tif 000064
【図４】
0005910647.tif 000065
【図５】
0005910647.tif 000066
【図６】
0005910647.tif 000067
【図７】
0005910647.tif 000068
【図８】
0005910647.tif 000069
【図９】
0005910647.tif 000070
【図１０】
0005910647.tif 000071
【図１１】
0005910647.tif 000072
【図１２】
0005910647.tif 000073
【図１３】
0005910647.tif 000074
【図１４】
0005910647.tif 000075
【図１５】
0005910647.tif 000076
【図１６】
0005910647.tif 000077
【図１７】
0005910647.tif 000078
【図１８】
0005910647.tif 000079
【図１９】
0005910647.tif 000080
【図２０】
0005910647.tif 000081
【図２１】
0005910647.tif 000082
【図２２】
0005910647.tif 000083
【図２３】
0005910647.tif 000084
【図２４】
0005910647.tif 000085
【図２５】
0005910647.tif 000086
【図２６】
0005910647.tif 000087
【図２７】
0005910647.tif 000088
【図２８】
0005910647.tif 000089
【図２９】
0005910647.tif 000090
【図３０】
0005910647.tif 000091
【図３１】
0005910647.tif 000092