(57)【特許請求の範囲】
【請求項１】
複数の接触点を有し、床と床以外の両方に前記接触点を接触させて、移動を行う多点接触ロボットであって、
目標重心軌道及び前記接触点各々の反力目標値を生成する軌道生成部であって、第１の周期で、現在の状態を初期状態として、将来にわたる前記目標重心軌道及び前記反力目標値を生成する軌道生成部と、
現在の重心を推定し、前記目標重心軌道と前記現在の重心との誤差を算出する状態推定部と、
前記誤差が前記目標重心軌道に収束するよう前記反力目標値を修正する状態安定化部と、を有し、
前記軌道生成部は、前記第１の周期で、評価関数を最小化する最適レギュレータを適用して、前記誤差を前記目標重心軌道に収束させるためのフィードバックゲインを算出し、
前記状態安定化部は、前記第１の周期よりも短い第２の周期で、前記誤差と前記フィードバックゲインとに基づき前記反力目標値を修正する、
多点接触ロボット。
【請求項２】
前記軌道生成部は、前記接触点における摩擦力又は鉛直力を含む制約条件に基づいて、前記フィードバックゲインを算出する
請求項１記載の多点接触ロボット。
【請求項３】
前記状態安定化部は、前記誤差と前記フィードバックゲインとを乗ずることにより、前記反力目標値を修正する
請求項１又は２記載の多点接触ロボット。
【請求項４】
複数の接触点を有し、床と床以外の両方に前記接触点を接触させて、移動を行う多点接触ロボットの制御方法であって、
現在の状態を初期状態として、将来にわたる前記多点接触ロボットの目標重心軌道及び前記接触点各々の反力目標値を第１の周期で生成する軌道生成ステップと、
現在の重心を推定し、前記目標重心軌道と前記現在の重心との誤差を算出する誤差計算ステップと、
評価関数を最小化する最適レギュレータを適用して、前記第１の周期で、前記誤差を前記目標重心軌道に収束させるためのフィードバックゲインを算出する最適ゲイン計算ステップと、
前記第１の周期よりも短い第２の周期で、前記誤差と前記フィードバックゲインとに基づき前記反力目標値を修正する状態フィードバックステップと、を含む
多点接触ロボットの制御方法。
【請求項５】
前記最適ゲイン計算ステップは、前記接触点における摩擦力又は鉛直力を含む制約条件に基づいて、前記フィードバックゲインを算出する
請求項４記載の多点接触ロボットの制御方法。
【請求項６】
前記状態フィードバックステップは、前記誤差と前記フィードバックゲインとを乗ずることにより、前記反力目標値を修正する
請求項４又は５記載の多点接触ロボットの制御方法。
【請求項７】
コンピュータに、請求項４乃至６いずれか１項記載の多点接触ロボットの制御方法を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、多点接触ロボットの制御に関する。より具体的には、多点接触ロボットを安定かつ高精度に移動させるための制御技術に関する。
【背景技術】
【０００２】
自律的に移動する機能を備えたロボットが開発されており、例えば、多点接触制御により動作するロボットがある。多点接触でロボットを安定に動作させるためには、大きく（Ａ）長期的に安定な軌道を生成する技術、（Ｂ）ロボットを生成された軌道に漸近させるため、各接触点における望ましい接触力を計算する安定化技術、及び（Ｃ）計算された接触力目標値を達成するように各関節を駆動する力制御技術、の３種類の技術を考慮する必要がある。これら３つの技術を（Ａ）、（Ｂ）及び（Ｃ）の順に用いることにより、安定な多点接触の動作が実現できる。すなわち、まず（Ａ）比較的長期、つまりある程度の未来にわたる安定な軌道、つまり重心の発散や運動の破綻がない軌道を生成し、次に（Ｂ）ロボットの現在の状態量（重心位置、速度及び胴体姿勢等）を上記軌道に漸近させるべく、ロボットの各接触点の力指令値を決定及び修正し、続いて（Ｃ）各接触点の反力が力指令値に追従するように力制御を行う。
【０００３】
ここで、（Ｂ）の安定化技術に注目すると、多点接触制御に適用可能な先行技術としては、特許文献１及び特許文献２が挙げられる。特許文献１は、従来の２足歩行向け安定化技術である複合コンプライアンス制御を、床面上を含む３点以上の接地点を持つ多脚ロボットにも使えるように拡張したものである。特許文献２は、床面以外の接触点を持つ多点接触制御に対応できる安定化技術を開示している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特許第４１２６０６４号
【特許文献２】特許第５３９８５９２号
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１の複合コンプライアンス制御はＺＭＰを用いた安定化技術である。ＺＭＰ（ｚｅｒｏ  ｍｏｍｅｎｔ  ｐｏｉｎｔ）とは、所定のＺＭＰ平面（通常は床面）上のモーメントの釣り合い位置（全体のモーメントが釣り合って丁度０になる位置）のことである。ＺＭＰを用いた安定化技術は、歩行のように環境との接触点がほぼ床面に限定されているような運動には適するが、任意の複数の接触点で環境と接触するような運動には適さない。すなわち、特許文献１は多点接触に対応してはいるが、基本的には床面上に接触点を持つロボット（つまり、多脚ロボット）を対象にした技術である。
【０００６】
一方、特許文献２の安定化技術は、ＺＭＰを用いておらず、床面以外の接触点を持つロボットの多点接触制御に対応できる。しかしながら、特許文献２が開示するアルゴリズムは、各接触点が属する面を、予め設定された仮想面に投影することを前提として制御則が組み立てられている。すなわち、実際に手や足等の接触点が接触している、壁や床等の接触面上における力の分配を直接計算することなく、仮想面群というギミックを用いて制御則を組み立てている。
【０００７】
特許文献２のアルゴリズムは、面同士が直交もしくは平行に対向した面群（仮想面群）を前提とするものである。そして、例えば接触点の摩擦力が不足して所望の並進力が得られないような場合に、平行に対向する各仮想面への押し付け力（垂直抗力）を増加させることで摩擦力を増加させる、という補償方法を用いている。ここで、仮に仮想面が平行に対向しておらず任意の傾きを持っている場合には、互いの面に生ずる垂直抗力の向きが一致しないので、上述のように単純に垂直抗力を増加させるというようなロジックではこの問題を解決できない。すなわち、特許文献２の安定化技術は、制御ロジック全体が、面同士が直交もしくは平行に対向した面群を前提にする必要がある。そのために、実際の接触面ではなく仮想面を設定しているのである。
【０００８】
そのため、特許文献２の安定化技術は、任意の角度を持つ接触面群（例えば相対的に４５度ずつ傾いた複数の面など）に対する適用性が著しく低いという問題がある。また、摩擦力と内力を活用することによって安定化可能な場合であっても、運動状態が不適切である旨の誤判定を起こすという問題も抱えている。
【０００９】
また、特許文献２の安定化技術では、重心や姿勢角のフィードバック制御が公知のＰＤ制御で構成されている。ＰＤ制御では、各接触点の位置や姿勢の情報（例えばどのような向きに力を発生し易いか、どの方向ならどの程度の合力を安定に発生できるか等の情報）は全く考慮されない。
【００１０】
本発明はこうした課題に鑑みてなされたものであり、その目的は、高い安定化性能を有する多点接触ロボット、多点接触ロボットの制御方法及びプログラムを提供することにある。
【課題を解決するための手段】
【００１１】
本発明にかかる多点接触ロボットは、
複数の接触点を有する多点接触ロボットであって、
目標重心軌道及び前記接触点各々の反力目標値を生成する軌道生成部と、
現在の重心を推定し、前記目標重心軌道と前記現在の重心との誤差を算出する状態推定部と、
前記誤差が前記目標重心軌道に収束するよう前記反力目標値を修正する状態安定化部と、を有し、
前記軌道生成部は、第１の周期で、前記誤差を前記目標重心軌道に収束させるためのフィードバックゲインを算出し、
前記状態安定化部は、前記第１の周期よりも短い第２の周期で、前記誤差と前記フィードバックゲインとに基づき前記反力目標値を修正するものである。
【００１２】
本発明にかかる多点接触ロボットの制御方法は、
複数の接触点を有する多点接触ロボットの目標重心軌道及び前記接触点各々の反力目標値を生成する軌道生成ステップと、
現在の重心を推定し、前記目標重心軌道と前記現在の重心との誤差を算出する誤差計算ステップと、
第１の周期で、前記誤差を前記目標重心軌道に収束させるためのフィードバックゲインを算出する最適ゲイン計算ステップと、
前記第１の周期よりも短い第２の周期で、前記誤差と前記フィードバックゲインとに基づき前記反力目標値を修正する状態フィードバックステップと、を含むものである。
【００１３】
本発明にかかるプログラムは、上記多点接触ロボットの制御方法をコンピュータに実行させるためのプログラムである。
【発明の効果】
【００１４】
本発明によれば、高い安定化性能を有する多点接触制御技術を導入した多点接触ロボット、多点接触ロボットの制御方法及びプログラムを提供できる。
【図面の簡単な説明】
【００１５】
【図１】本発明の実施の形態にかかる多点接触ロボット１００の構成を示す図である。
【図２】多点接触ロボット１００にかかる接触力を示す図である。
【図３】多点接触ロボット１００の接触点の座標系及び接触多角形を示す図である。
【図４】多点接触ロボット１００の接触点の安定化条件を示す図である。
【図５】多点接触ロボット１００の制御フレームワークを示す図である。
【図６】多点接触ロボット１００の制御フレームワークを示す図である。
【図７】多点接触ロボット１００の制御方法を示す図である。
【図８】多点接触ロボット１００の制御方法を示す図である。
【図９】多点接触ロボット１００の構成を示す図である。
【図１０】多点接触ロボット１００の構成を示す図である。
【発明を実施するための形態】
【００１６】
本発明の実施形態を図示するとともに図中の各要素に付した符号を参照して説明する。
【００１７】
本実施形態は多点接触ロボット制御方法に特徴があり、具体的には、多点接触ロボットの移動動作を制御するための多点接触制御技術に特徴を有するのであるが、具体的な多点接触制御方法を説明する前に、制御対象となる多点接触ロボットのハードウェア構成について予め説明しておく。
【００１８】
図９は、多点接触ロボットの機械構成の一例を示した図である。多点接触ロボット１００は、股関節が３軸、膝関節が１軸、足首関節が２軸、さらに、肩関節が３軸（肩ピッチ、肩ロール、肩ヨー）、肘関節が１軸（肘ピッチ）、および、手首関節が３軸（手首ヨー、手首ピッチ、手首ロール）、で夫々構成されている。多点接触ロボットの機械構成はこれに限定されないが、手（腕）の自由度は６以上、足（脚）の自由度も６以上は必要である。
【００１９】
多点接触ロボット１００は、各関節にエンコーダ付きモータ１、２、・・・、２８を有している。各関節のモータ（関節駆動手段）１ａ、２ａ、・・・、２８ａ（図１０）は、各関節の関節角度θ１、θ２、・・・、θ２８を調整できる。一方、各関節のエンコーダ（関節角度検出手段）１ｂ、２ｂ、・・・、２８ｂは、各関節の関節角度θ１、θ２・・・、θ２８を計測することができる。
【００２０】
また、多点接触ロボット１００は、足先部（足平部）および手先部（手の平部）に接触力センサ２５を有している。ここで接触力とは６軸力であり、図２に示すように、ｘ軸、ｙ軸およびｚ軸方向の力ｆの組（ｆｘ、ｆｙ、ｆｚ）Ｔと、ｘ軸回り、ｙ軸回りおよびｚ軸回りの力τの組（τｘ、τｙ、τｚ）Ｔと、である。なお、ｘ軸およびｙ軸は、鉛直方向であるｚ軸に垂直な面内で互いに直交する軸とする。
【００２１】
この多点接触ロボットは、移動時に、右足、左足、右手又は左手などの接触点のうち複数を、床、壁、あるいはテーブルなどの接触面に接触させながら移動する。
【００２２】
図１０は、多点接触ロボット１００の機能ブロック図である。多点接触ロボット１００は、各関節のモータ１ａ乃至２４ａ及びエンコーダ１ｂ乃至２４ｂと、力センサ２５と、コントローラ２１０と、を備えている。
【００２３】
コントローラ２１０には、各関節のエンコーダ１ｂ乃至２４ｂ及び力センサ２５から、センサ検出値が入力される。また、コントローラ２１０は、各関節のモータ１ａ乃至２４ａに対して駆動信号を出力する。
【００２４】
コントローラ２１０は、主要なハードウェア構成として、制御処理、演算処理等を行うＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）２１０ａと、ＣＰＵ２１０ａによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）２１０ｂと、処理データ等を一時的に記憶するＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）２１０ｃと、を有するマイクロコンピュータにより構成されている。また、これらＣＰＵ２１０ａ、ＲＯＭ２１０ｂ、及びＲＡＭ２１０ｃは、データバス２１０ｄによって相互に接続されている。必要なプログラムを不揮発性記録媒体に記録しておき、必要に応じてインストールするようにしてもよい。
【００２５】
ここで図１の機能ブロック図を用いて、コントローラ２１０の機能構成について説明する。同時に、図５を用いて、多点接触ロボットにおける多点接触動作の生成及び制御フレームワークについて説明する。
【００２６】
（ａ）接触点計画
接触点計画部２１１は、多点接触ロボットがあるタスク（移動も含む）を実行するための、適切な接触点の位置及び姿勢を計画する。なお、接触点計画はユーザにより入力されても良い。
【００２７】
（ｂ）軌道生成
軌道生成部２１２は、接触点計画部２１１が計画した接触点位置及び姿勢に基づいて、将来にわたって発散及び破綻しない安定な重心軌道、力指令値軌道、手先や足先の位置、及び姿勢軌道を生成する。
【００２８】
（ｃ）状態推定
状態推定部２１３は、多点接触ロボットの胴体に搭載されたＩＭＵ（３軸ジャイロ、３軸加速度計）や各接触点（例えば手先や足先等）の力センサ等を用いて、現在の重心位置、速度、及び胴体姿勢を推定する。
【００２９】
（ｄ）状態安定化
状態安定化部２１４は、状態推定部２１３が推定した多点接触ロボットの状態と、軌道生成部２１２が生成した目標軌道と、の差が０に漸近するよう、状態フィードバックにより各接触点の６軸力指令値を修正する。
【００３０】
（ｅ）接触力制御
接触力制御部２１５は、各接触点の６軸力指令値を実現するよう、力センサを用いてインピーダンス制御を行い、接触点（例えば手先や足先等）の位置及び姿勢を修正する。
【００３１】
（ｆ）全身ＩＫ
全身ＩＫ部２１６は、接触力制御部２１５が算出した重心位置、手先や足先の位置、姿勢、及び重心周りの角運動量等の指令値を達成するように、逆運動学（ＩＫ：ｉｎｖｅｒｓｅ  ｋｉｎｅｍａｔｉｃｓ）により全身の関節角を求める。
【００３２】
（ｇ）関節サーボ
関節サーボ部２１７は、全身ＩＫ部２１６が求めた関節角度指令値に基づいて各軸で位置サーボ制御を行う。
【００３３】
ここで、上記（ａ）乃至（ｇ）にかかる各処理自体は公知技術等を用いて実現できるため、本実施の形態においては詳細な説明を省略する。
【００３４】
コントローラ２１０は、典型的には（ａ）接触点計画、（ｂ）軌道生成、（ｄ）状態安定化、（ｅ）接触力制御、（ｆ）全身ＩＫ、及び（ｇ）関節サーボの順に処理を行うことで、多点接触ロボットの動作を制御する。そして、重要な点として、コントローラ２１０は、（ｂ）軌道生成と（ｃ）状態推定とのフィードバック処理を実施する。ここで軌道生成部２１２は、一定時間ごとに、現在の多点接触ロボットの状態を起点として、将来にわたる目標軌道を生成する。同様に、（ｄ）状態安定化と（ｃ）状態推定との間にもフィードバックループが存在する。ここでは、状態安定化部２１４が、現在の多点接触ロボットの状態と目標軌道との差が０に漸近するよう、力指令値を修正する。
【００３５】
本実施の形態の特徴は、これら２つのフィードバックループの周期が異なる点にある。すなわち、状態推定部２１３は、多点接触ロボットの状態推定結果を、軌道生成部２１２と状態安定化部２１４とに対して、それぞれ異なる周期でフィードバックする。コントローラ２１０は、軌道生成部２１２と状態推定部２１３とのフィードバックループは比較的長い周期（例えば２０ｍｓ）で実行する。一方、状態安定化部２１４と状態推定部２１３とのフィードバックループは比較的短い周期（例えば１ｍｓ）実行する。
【００３６】
フィードバック周期を異ならせてよい理由は次のとおりである。（ｄ）状態安定化処理は、現在のロボットの状態と目標軌道との差を修正するための制御である。もし、何らかの事情により多点接触ロボットの実際の状態と目標軌道とに差異が生じると、多点接触ロボットの運動は破綻して転倒するなどの可能性がある。そこで、（ｃ）状態推定と（ｄ）状態安定化との間のフィードバック処理はできるだけ短い時間（周期）で実施し、できるだけ素早く（ｅ）乃至（ｇ）による姿勢制御を実行して、多点接触ロボットの状態を安定化させる必要がある。一方、（ｂ）軌道生成処理は、現在の多点接触ロボットの重心位置や速度等を初期状態として、そこから将来にわたる安定軌道を生成する処理である。この処理は必ずしも（ｄ）ほど短い周期で実施する必要はない。
【００３７】
このように、コントローラ２１０は、（ｂ）軌道生成処理と、（ｄ）乃至（ｇ）にかかるリアルタイムの姿勢制御処理と、をそれぞれ異なる周期で実行できる。当然ながら、軌道生成処理と、リアルタイムの姿勢制御処理と、では許容される計算量も各々異なる。リアルタイムの姿勢制御処理は短い時間で実行する必要があるから、計算量をできる限り少なく抑える工夫が必要である。
【００３８】
次に、本実施の形態にかかる多点接触制御処理のアルゴリズムの一例について説明する。はじめに、本実施の形態にかかる多点接触制御処理の考え方について説明する。
【００３９】
本実施の形態では、多点接触ロボット全体の慣性が１つの重心で表され、各接触点には６軸力が定義されたモデルを用いる（図２）。この時、重心の並進運動量をＰ、重心回りの回転運動量をＬ、接触点の数をｎとすると、運動方程式は
【数１】
0006447278.tif 000002
と表される。
【００４０】
ここで、「重心位置、胴体姿勢角度、重心周り角運動量、各接触点の力、各接触点のモーメント」の指令値を
【数２】
0006447278.tif 000003
とし、誤差量を導入して
【数３】
0006447278.tif 000004
とすると、式（１）は
【数４】
0006447278.tif 000005
と表される。
【００４１】
軌道生成部２１２によって生成される指令値は当然、式（１）を満たし
【数５】
0006447278.tif 000006
が成り立っているので、上式（数４）と差を取れば
【数６】
0006447278.tif 000007
となる。ただし、誤差の２乗項は十分小さいとして近似的に
【数７】
0006447278.tif 000008
とした。
【００４２】
式（２）を行列表現に書き直せば
【数８】
0006447278.tif 000009
となる。ただし、
【数９】
0006447278.tif 000010
とする。
【００４３】
さらに、
【数１０】
0006447278.tif 000011
とおけば、式（３）は簡単に
【数１１】
0006447278.tif 000012
と書くことができる。ここで、ηは多点接触ロボットと目標軌道とのずれ量、ｕは接触点における荷重を示す。Ａ及びＢをシステム行列という。
【００４４】
ここで、状態推定部２１３で推定された結果として、「重心位置、速度、胴体姿勢角度、重心周り角運動量」の推定値
【数１２】
0006447278.tif 000013
が状態安定化部２１４に入力されるものとする。
【００４５】
状態安定化部２１４の目的は
【数１３】
0006447278.tif 000014
を達成することである。
【００４６】
ここで誤差を
【数１４】
0006447278.tif 000015
と定義する。ただし、ｋは姿勢角誤差のフィードバックゲイン、ｅΦは瞬間的な姿勢角誤差をグローバル座標系で表現したベクトルである。ｅΦは、オイラー角とクォータニオンとの変換関数をＣｑｕａｔとすれば次式で表される。
【数１５】
0006447278.tif 000016
【００４７】
式（６）の第２式は、胴体姿勢角度の誤差にゲインを掛けて重心周り角運動量誤差に足し合わせる形になっており、これは近似的には重心周り角運動量誤差を積分しているとも解釈できる。つまり、重心周り角運動量誤差を減少させるようにフィードバックを行った場合には、近似的にはＰＩ制御を行ったのと同等と解釈できる。
【００４８】
したがって、状態安定化部２１４は、式（６）の誤差をフィードバックして、誤差が減少するように制御を行えば、式（５）の目的を達成できる。すなわち、誤差を式（６）で定義した時に、
【数１６】
0006447278.tif 000017
となれば良い。
【００４９】
そこで本実施の形態では、誤差の漸近収束と各接触力への負荷の分配を目的として、次のような評価関数で表される最適レギュレータを適用する。
【数１７】
0006447278.tif 000018
この最適レギュレータ問題は従来から非常に多くの分野で使われており、解もよく知られている。
【数１８】
0006447278.tif 000019
ここで、Ｐはリッカチ方程式の解
【数１９】
0006447278.tif 000020
である。また、Ｋはずれ量ηを打ち消す荷重ｕを得るためのゲインを示す。
【００５０】
このように、状態安定化部２１４は、状態推定部２１３が観測した誤差量（状態量）をフィードバックすることによって、式（７）を達成する補正力及びモーメントを算出することができる。
【００５１】
ここで、以下の２つの課題が生ずる。
（ア）接触点数や接触部位が変化していくのに伴って、システム式（４）が変化する。そのため、上記リッカチ方程式を予め解いておくことができない。一方、リアルタイムで毎回解くと計算時間を要するため、制御周期の上限が制限される。
（イ）状態フィードバックを示す式（８）では補正力及びモーメントの制限が考慮されていない。そのため、補正により、接触点で接触部位が滑る又は剥がれる等の接触不安定を引き起こす可能性がある。
【００５２】
まず課題（イ）の解決策を検討する。図３に、接触点の座標系（上添え字ｌがついている）と、接触多角形（接触点の支持多角形）を示した。接触点座標は接触点を原点として接触面の姿勢ｒｉに合わせて定義されているものとする。このとき、接触座標系で定義された６軸力
【数２０】
0006447278.tif 000021
は、接触面の姿勢行列
【数２１】
0006447278.tif 000022
（ｒｏｔ（）はオイラー角と姿勢行列に変換する関数）を用いて、
【数２２】
0006447278.tif 000023
と表される。
【００５３】
図４に示すように、接触点が安定して接触を保つためには以下の３つの条件を満たす必要がある。
（１）接触点が接触面から離れないこと
（２）接触点が接触面で滑らないこと
（３）接触点が接触面から剥がれないこと
【００５４】
（１）接触点が接触面から離れない為には、接触面の鉛直力が正であれば良い。すなわち、次式を満たす必要がある。
【数２３】
0006447278.tif 000024
【００５５】
（２）接触点が接触面で滑らない為には、接触面に平行な２軸力が摩擦力以下であれば良い。すなわち次式がその条件である。ただし接触面の摩擦係数をμｉとする。
【数２４】
0006447278.tif 000025
【００５６】
（３）接触点が剥がれない為の条件は、接触多角形のｈ個の頂点座標（ｘｌｉｌ，ｙｌｉｌ），・・・，（ｘｌｉｈ，ｙｌｉｈ）を用いて以下のように表される（ただし接触多角形の頂点は反時計回りに順に与えられているものとする）。
【数２５】
0006447278.tif 000026
【００５７】
式（９）、（１０）、（１１）及び（１２）をまとめると
【数２６】
0006447278.tif 000027
となる。
【数２７】
0006447278.tif 000028
の関係を用いて更に式（１３）を書き直せば、
【数２８】
0006447278.tif 000029
となる。ただし、
【数２９】
0006447278.tif 000030
とした。
【００５８】
式（１４）を全接触点について書き並べると
【数３０】
0006447278.tif 000031
と表すことができる。ただし、
【数３１】
0006447278.tif 000032
とする。
【００５９】
課題（イ）とは、式（８）のフィードバックの結果、式（１５）が満たされないために、接触点を安定に保つ条件式（１０）、（１１）及び（１２）が満たされず、もって接触点で接触部位が滑りや剥がれを引き起こすという問題である。そこで本実施の形態では、直近で式（１５）を満たしつつ、評価関数
【数３２】
0006447278.tif 000033
を最小化するようなフィードバックゲインを算出する。
【００６０】
この問題は、ＬＭＩ（Ｌｉｎｅａｒ  Ｍａｔｒｉｘ  Ｉｎｅｑｕａｌｉｔｙ）を用いて次のような最適化問題を解くことにより、解を得ることが可能である。
【数３３】
0006447278.tif 000034
ただし、ηｎは最も直近で得られた誤差値である。
【００６１】
式（１６）の形式の最適化問題は、収束計算による求解法である内点法を用いて効率良く解を求められることがよく知られている。式（１６）の求解結果Ｐ＊を用いて
【数３４】
0006447278.tif 000035
のようにフィードバックゲインが計算される。
接触点における摩擦力又は鉛直力を含む制約条件を課すことにより、補正によって接触点におけるすべりやはがれなどの不安定性を考慮した状態安定を行うことができる。
【００６２】
次に、課題（ア）の解決策について検討する。上記最適化問題は高速に求解することは可能だが、ある程度の計算時間が必要である。そこで、本実施の形態では、フィードバックゲインの計算を（ｂ）軌道生成と同時に行うことにより、課題（ア）を解決する手法を提案する。なお、フィードバックゲインの計算は、（ｄ）状態安定化におけるフィードバックループよりも遅い周期で実行すれば良く、必ずしも（ｂ）軌道生成のフィードバックループと同じ周期で行う必要はない。
【００６３】
すなわち、図５に示すように、本実施の形態では、２０ｍｓ周期で現在の状態推定量に基づいて式（１６）及び（１７）を解くことで最適フィードバックゲインＫを計算し、求められたフィードバックゲインＫを用いて１ｍｓ周期で状態フィードバックを行う、というフレームワークを採用する。状態フィードバックの計算自体は、フィードバックゲインと現在の状態推定量から算出した誤差量を乗算するだけなので、非常に少ない計算量で補正力及びモーメントを計算することができる。状態フィードバックの計算を、誤差とフィードバックゲインを積算するだけの計算にとどめるので、短周期で計算することができる。
【００６４】
図６乃至図８を用いて、本実施の形態の特徴的な部分について再度説明する。図６は、本実施の形態のフレームワークを示すブロック線図である。図７は、最適ゲイン計算器２１２２の動作を示すフローチャートである。図８は、誤差計算器２１３２及び状態フィードバック２１４２の動作を示すフローチャートである。
【００６５】
図６のブロック線図は、複数の機能ブロックと、機能ブロック間のデータの入出力を示す矢印とからなっている。軌道生成器２１２１及び最適ゲイン計算器２１２２は、軌道生成部２１２に含まれる機能ブロックである。状態推定器２１３１及び誤差計算器２１３２は、状態推定部２１３に含まれる。状態フィードバック２１４２は、状態安定化部２１４に含まれる。また、図６において、接触点パラメータｓｉ＝（μｉ，（ｘｌｉｌ，ｙｌｉｌ），・・・，（ｘｌｉｈ，ｙｌｉｈ））は接触点の摩擦係数及び接触多角形を表すものとする。
【００６６】
このうち、軌道生成器２１２１及び最適ゲイン計算器２１２２は、長い周期、典型的には２０ｍｓ周期で処理を実行する。一方、状態推定器２１３１、誤差計算器２１３２及び状態フィードバック２１４２は、短い周期、典型的には１ｍｓ周期で処理を実行する。
【００６７】
まず長い周期、典型的には２０ｍｓ周期で処理が実行される機能ブロックの動作について説明する。軌道生成器２１２１が、接触点計画部２１１又は上位装置によって計画された接触点位置及び姿勢ｐｉ［ｋ］，ｒｉ［ｋ］及び接触点パラメータｓｉ［ｋ］に基づいて、重心位置及び速度ｑｄ［ｋ］，ｑｄ［ｋ］、胴体姿勢角φｄ［ｋ］、重心周り角運動量Ｌｄ［ｋ］、接触点の力及びモーメントｆｉｄ［ｋ］，τｉｄ［ｋ］、手先及び足先位置並びに姿勢（接触点含む）ｐｔｉ［ｋ］，ｒｔｉ［ｋ］の時系列軌道を生成し、軌道バッファに格納する。ここで、軌道生成器としては、例えば特願２０１３−２５４９８９に記載のものを利用できる。
【００６８】
最適ゲイン計算器２１２２は、軌道生成器２１２１から、重心位置、重心周り角運動量、接触点の６軸力の現在の目標値ｑｄ、Ｌｄ、ｆｉｄ及びτｉｄ、並びに接触点の位置ｐｉ及び接触点パラメータｓｉを取得する（図７、Ｓ１０１）。そして、これらの取得値を用い、数１０に従ってシステム行列Ａ、Ｂを求める（Ｓ１０２）。また、数３１に示した不等式制約条件Ｇ、ｗを求める（Ｓ１０３）。
【００６９】
また、最適ゲイン計算器２１２２は、誤差計算器２１３２から状態誤差値の最新値（現在値）ηｎを取得する（図７、Ｓ１０４）。そして、式（１６）により最適化問題の求解結果Ｐ＊を得る（Ｓ１０５）。最後に、式（１７）及びＰ＊を用いて、最適フィードバックゲインＫを算出し、出力する（Ｓ１０６）。
【００７０】
次に短い周期、典型的には１ｍｓ周期で処理が実行される機能ブロックの動作について説明する。状態推定器２１３１が、各センサ値を用いて現在の重心位置及び速度ｑ，ｑ、胴体姿勢角度φ、重心周り角運動量Ｌを出力する。なお、状態推定器２１３１としては、例えば特願２０１４−０８９９４３に記載のものを利用できる。
【００７１】
誤差計算器２１３２は、軌道バッファから現在時刻における目標値ｑｄ、ｑｄ、φｄ、Ｌｄを取得する（図８、Ｓ２０２）。また、状態推定器２１３１から状態推定値ｑ、ｑ、φ、Ｌを取得する（Ｓ２０３）。そして、誤差計算器２１３２は、これらの値を用い、式（６）に基づいて現在誤差ベクトルηを計算する。
【００７２】
状態フィードバック２１４２は、最適ゲイン計算器２１２２から最適フィードバックゲインＫを取得する（図８、Ｓ２０１）。また、誤差計算器２１３２から誤差ベクトルηを取得する（Ｓ２０４）。そして、Ｋとηとを乗算して、接触点の補正力及びモーメントΔｆｉ，Δτｉを計算する（Ｓ２０５）。さらに、ｆｒｅｆｉ＝ｆｉｄ［ｊ］＋Δｆｉ，τｒｅｆｉ＝τｉｄ［ｊ］＋Δτｉのように、現在時刻における目標６軸力に上記補正量を加算することで、６軸力指令値を出力する（Ｓ２０６）。
【００７３】
本実施の形態によれば、軌道生成部２１２における軌道生成処理（最適ゲイン計算を含む）と、状態安定化部２１４による姿勢制御処理（誤差フィードバックを含む）とは、異なる周期で実行される。これにより、状態安定化部２１４は、比較的計算量の多い、すなわち計算に時間を要する最適ゲイン計算処理に影響されることなく、比較的短い時間で誤差フィードバック処理を実行できるようになる。そのため、早い周期で姿勢制御を行うことができ、任意の多点接触状態において重心軌道追従・安定化性能を向上させることができる。
【００７４】
また、本実施の形態によれば、状態安定化部２１４は、式（１６）による定式化と、図５に示すフレームワークとにより、各接触点の安定化を保ちつつ軌道追従誤差をゼロに漸近させる処理を、低い計算コストで高速に実行することができる。これにより、状態安定化部２１４は、早い周期で姿勢制御を行うことができ、任意の多点接触状態において重心軌道追従・安定化性能を向上させることができる。
【００７５】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述の実施の形態では、式（１３）を接触点の接触安定性を保証する条件のみで構成したが、例えば鉛直接触力の上限値を設定して
【数３５】
0006447278.tif 000036
のような条件を追加し、式（１３）を
【数３６】
0006447278.tif 000037
とし、式（１４）を
【数３７】
0006447278.tif 000038
とすることも可能である。
【００７６】
また、上述の実施の形態では、本発明を主にハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、様々なタイプの非一時的なコンピュータ可読媒体（ｎｏｎ−ｔｒａｎｓｉｔｏｒｙ  ｃｏｍｐｕｔｅｒ  ｒｅａｄａｂｌｅ  ｍｅｄｉｕｍ）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（ｔａｎｇｉｂｌｅ  ｓｔｏｒａｇｅ  ｍｅｄｉｕｍ）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Ｐｒｏｇｒａｍｍａｂｌｅ  ＲＯＭ）、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ  ＰＲＯＭ）、フラッシュＲＯＭ、ＲＡＭ（ｒａｎｄｏｍ  ａｃｃｅｓｓ  ｍｅｍｏｒｙ））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（ｔｒａｎｓｉｔｏｒｙ  ｃｏｍｐｕｔｅｒ  ｒｅａｄａｂｌｅ  ｍｅｄｉｕｍ）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００７７】
１００  多点接触ロボット
２１０  コントローラ
２１１  接触点計画部
２１２  軌道生成部
２１２１  軌道生成器
２１２２  最適ゲイン計算器
２１３  状態推定部
２１３１  状態推定器
２１４  状態安定化部
２１３２  誤差計算器
２１４２  状態フィードバック
２１５  接触力制御部
２１６  全身ＩＫ部
２１７  関節サーボ部
【図１】
0006447278.tif 000039
【図２】
0006447278.tif 000040
【図３】
0006447278.tif 000041
【図４】
0006447278.tif 000042
【図５】
0006447278.tif 000043
【図６】
0006447278.tif 000044
【図７】
0006447278.tif 000045
【図８】
0006447278.tif 000046
【図９】
0006447278.tif 000047
【図１０】
0006447278.tif 000048