JP2016172309A - 多点接触ロボット、多点接触ロボットの制御方法及びプログラム - Google Patents

Abstract

【課題】高い安定化性能を有する多点接触ロボット、多点接触ロボットの制御方法及びプログラムを提供する。【解決手段】複数の接触点を有する多点接触ロボット１００は、目標重心軌道及び接触点各々の反力目標値を生成する軌道生成部２１２、現在の重心を推定し、目標重心軌道と現在の重心との誤差を算出する状態推定部２１３、誤差が目標重心軌道に収束するよう反力目標値を修正する状態安定化部２１４を有する。軌道生成部２１２は、第１の周期で、誤差を目標重心軌道に収束させるためのフィードバックゲインを算出する。状態安定化部２１４は、第１の周期よりも短い第２の周期で、誤差とフィードバックゲインとに基づき反力目標値を修正する。【選択図】図１

Description

本発明は、多点接触ロボットの制御に関する。より具体的には、多点接触ロボットを安定かつ高精度に移動させるための制御技術に関する。

自律的に移動する機能を備えたロボットが開発されており、例えば、多点接触制御により動作するロボットがある。多点接触でロボットを安定に動作させるためには、大きく（Ａ）長期的に安定な軌道を生成する技術、（Ｂ）ロボットを生成された軌道に漸近させるため、各接触点における望ましい接触力を計算する安定化技術、及び（Ｃ）計算された接触力目標値を達成するように各関節を駆動する力制御技術、の３種類の技術を考慮する必要がある。これら３つの技術を（Ａ）、（Ｂ）及び（Ｃ）の順に用いることにより、安定な多点接触の動作が実現できる。すなわち、まず（Ａ）比較的長期、つまりある程度の未来にわたる安定な軌道、つまり重心の発散や運動の破綻がない軌道を生成し、次に（Ｂ）ロボットの現在の状態量（重心位置、速度及び胴体姿勢等）を上記軌道に漸近させるべく、ロボットの各接触点の力指令値を決定及び修正し、続いて（Ｃ）各接触点の反力が力指令値に追従するように力制御を行う。

ここで、（Ｂ）の安定化技術に注目すると、多点接触制御に適用可能な先行技術としては、特許文献１及び特許文献２が挙げられる。特許文献１は、従来の２足歩行向け安定化技術である複合コンプライアンス制御を、床面上を含む３点以上の接地点を持つ多脚ロボットにも使えるように拡張したものである。特許文献２は、床面以外の接触点を持つ多点接触制御に対応できる安定化技術を開示している。

特許第４１２６０６４号 特許第５３９８５９２号

特許文献１の複合コンプライアンス制御はＺＭＰを用いた安定化技術である。ＺＭＰ（ｚｅｒｏ ｍｏｍｅｎｔ ｐｏｉｎｔ）とは、所定のＺＭＰ平面（通常は床面）上のモーメントの釣り合い位置（全体のモーメントが釣り合って丁度０になる位置）のことである。ＺＭＰを用いた安定化技術は、歩行のように環境との接触点がほぼ床面に限定されているような運動には適するが、任意の複数の接触点で環境と接触するような運動には適さない。すなわち、特許文献１は多点接触に対応してはいるが、基本的には床面上に接触点を持つロボット（つまり、多脚ロボット）を対象にした技術である。

一方、特許文献２の安定化技術は、ＺＭＰを用いておらず、床面以外の接触点を持つロボットの多点接触制御に対応できる。しかしながら、特許文献２が開示するアルゴリズムは、各接触点が属する面を、予め設定された仮想面に投影することを前提として制御則が組み立てられている。すなわち、実際に手や足等の接触点が接触している、壁や床等の接触面上における力の分配を直接計算することなく、仮想面群というギミックを用いて制御則を組み立てている。

特許文献２のアルゴリズムは、面同士が直交もしくは平行に対向した面群（仮想面群）を前提とするものである。そして、例えば接触点の摩擦力が不足して所望の並進力が得られないような場合に、平行に対向する各仮想面への押し付け力（垂直抗力）を増加させることで摩擦力を増加させる、という補償方法を用いている。ここで、仮に仮想面が平行に対向しておらず任意の傾きを持っている場合には、互いの面に生ずる垂直抗力の向きが一致しないので、上述のように単純に垂直抗力を増加させるというようなロジックではこの問題を解決できない。すなわち、特許文献２の安定化技術は、制御ロジック全体が、面同士が直交もしくは平行に対向した面群を前提にする必要がある。そのために、実際の接触面ではなく仮想面を設定しているのである。

そのため、特許文献２の安定化技術は、任意の角度を持つ接触面群（例えば相対的に４５度ずつ傾いた複数の面など）に対する適用性が著しく低いという問題がある。また、摩擦力と内力を活用することによって安定化可能な場合であっても、運動状態が不適切である旨の誤判定を起こすという問題も抱えている。

また、特許文献２の安定化技術では、重心や姿勢角のフィードバック制御が公知のＰＤ制御で構成されている。ＰＤ制御では、各接触点の位置や姿勢の情報（例えばどのような向きに力を発生し易いか、どの方向ならどの程度の合力を安定に発生できるか等の情報）は全く考慮されない。

本発明はこうした課題に鑑みてなされたものであり、その目的は、高い安定化性能を有する多点接触ロボット、多点接触ロボットの制御方法及びプログラムを提供することにある。

本発明にかかる多点接触ロボットは、
複数の接触点を有する多点接触ロボットであって、
目標重心軌道及び前記接触点各々の反力目標値を生成する軌道生成部と、
現在の重心を推定し、前記目標重心軌道と前記現在の重心との誤差を算出する状態推定部と、
前記誤差が前記目標重心軌道に収束するよう前記反力目標値を修正する状態安定化部と、を有し、
前記軌道生成部は、第１の周期で、前記誤差を前記目標重心軌道に収束させるためのフィードバックゲインを算出し、
前記状態安定化部は、前記第１の周期よりも短い第２の周期で、前記誤差と前記フィードバックゲインとに基づき前記反力目標値を修正するものである。

本発明にかかる多点接触ロボットの制御方法は、
複数の接触点を有する多点接触ロボットの目標重心軌道及び前記接触点各々の反力目標値を生成する軌道生成ステップと、
現在の重心を推定し、前記目標重心軌道と前記現在の重心との誤差を算出する誤差計算ステップと、
第１の周期で、前記誤差を前記目標重心軌道に収束させるためのフィードバックゲインを算出する最適ゲイン計算ステップと、
前記第１の周期よりも短い第２の周期で、前記誤差と前記フィードバックゲインとに基づき前記反力目標値を修正する状態フィードバックステップと、を含むものである。

本発明にかかるプログラムは、上記多点接触ロボットの制御方法をコンピュータに実行させるためのプログラムである。

本発明によれば、高い安定化性能を有する多点接触制御技術を導入した多点接触ロボット、多点接触ロボットの制御方法及びプログラムを提供できる。

本発明の実施の形態にかかる多点接触ロボット１００の構成を示す図である。 多点接触ロボット１００にかかる接触力を示す図である。 多点接触ロボット１００の接触点の座標系及び接触多角形を示す図である。 多点接触ロボット１００の接触点の安定化条件を示す図である。 多点接触ロボット１００の制御フレームワークを示す図である。 多点接触ロボット１００の制御フレームワークを示す図である。 多点接触ロボット１００の制御方法を示す図である。 多点接触ロボット１００の制御方法を示す図である。 多点接触ロボット１００の構成を示す図である。 多点接触ロボット１００の構成を示す図である。

本発明の実施形態を図示するとともに図中の各要素に付した符号を参照して説明する。

本実施形態は多点接触ロボット制御方法に特徴があり、具体的には、多点接触ロボットの移動動作を制御するための多点接触制御技術に特徴を有するのであるが、具体的な多点接触制御方法を説明する前に、制御対象となる多点接触ロボットのハードウェア構成について予め説明しておく。

図９は、多点接触ロボットの機械構成の一例を示した図である。多点接触ロボット１００は、股関節が３軸、膝関節が１軸、足首関節が２軸、さらに、肩関節が３軸（肩ピッチ、肩ロール、肩ヨー）、肘関節が１軸（肘ピッチ）、および、手首関節が３軸（手首ヨー、手首ピッチ、手首ロール）、で夫々構成されている。多点接触ロボットの機械構成はこれに限定されないが、手（腕）の自由度は６以上、足（脚）の自由度も６以上は必要である。

多点接触ロボット１００は、各関節にエンコーダ付きモータ１、２、・・・、２８を有している。各関節のモータ（関節駆動手段）１ａ、２ａ、・・・、２８ａ（図１０）は、各関節の関節角度θ１、θ２、・・・、θ２８を調整できる。一方、各関節のエンコーダ（関節角度検出手段）１ｂ、２ｂ、・・・、２８ｂは、各関節の関節角度θ１、θ２・・・、θ２８を計測することができる。

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

この多点接触ロボットは、移動時に、右足、左足、右手又は左手などの接触点のうち複数を、床、壁、あるいはテーブルなどの接触面に接触させながら移動する。

図１０は、多点接触ロボット１００の機能ブロック図である。多点接触ロボット１００は、各関節のモータ１ａ乃至２４ａ及びエンコーダ１ｂ乃至２４ｂと、力センサ２５と、コントローラ２１０と、を備えている。

コントローラ２１０には、各関節のエンコーダ１ｂ乃至２４ｂ及び力センサ２５から、センサ検出値が入力される。また、コントローラ２１０は、各関節のモータ１ａ乃至２４ａに対して駆動信号を出力する。

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

ここで図１の機能ブロック図を用いて、コントローラ２１０の機能構成について説明する。同時に、図５を用いて、多点接触ロボットにおける多点接触動作の生成及び制御フレームワークについて説明する。

（ａ）接触点計画
接触点計画部２１１は、多点接触ロボットがあるタスク（移動も含む）を実行するための、適切な接触点の位置及び姿勢を計画する。なお、接触点計画はユーザにより入力されても良い。

（ｂ）軌道生成
軌道生成部２１２は、接触点計画部２１１が計画した接触点位置及び姿勢に基づいて、将来にわたって発散及び破綻しない安定な重心軌道、力指令値軌道、手先や足先の位置、及び姿勢軌道を生成する。

（ｃ）状態推定
状態推定部２１３は、多点接触ロボットの胴体に搭載されたＩＭＵ（３軸ジャイロ、３軸加速度計）や各接触点（例えば手先や足先等）の力センサ等を用いて、現在の重心位置、速度、及び胴体姿勢を推定する。

（ｄ）状態安定化
状態安定化部２１４は、状態推定部２１３が推定した多点接触ロボットの状態と、軌道生成部２１２が生成した目標軌道と、の差が０に漸近するよう、状態フィードバックにより各接触点の６軸力指令値を修正する。

（ｅ）接触力制御
接触力制御部２１５は、各接触点の６軸力指令値を実現するよう、力センサを用いてインピーダンス制御を行い、接触点（例えば手先や足先等）の位置及び姿勢を修正する。

（ｆ）全身ＩＫ
全身ＩＫ部２１６は、接触力制御部２１５が算出した重心位置、手先や足先の位置、姿勢、及び重心周りの角運動量等の指令値を達成するように、逆運動学（ＩＫ：ｉｎｖｅｒｓｅ ｋｉｎｅｍａｔｉｃｓ）により全身の関節角を求める。

（ｇ）関節サーボ
関節サーボ部２１７は、全身ＩＫ部２１６が求めた関節角度指令値に基づいて各軸で位置サーボ制御を行う。

ここで、上記（ａ）乃至（ｇ）にかかる各処理自体は公知技術等を用いて実現できるため、本実施の形態においては詳細な説明を省略する。

コントローラ２１０は、典型的には（ａ）接触点計画、（ｂ）軌道生成、（ｄ）状態安定化、（ｅ）接触力制御、（ｆ）全身ＩＫ、及び（ｇ）関節サーボの順に処理を行うことで、多点接触ロボットの動作を制御する。そして、重要な点として、コントローラ２１０は、（ｂ）軌道生成と（ｃ）状態推定とのフィードバック処理を実施する。ここで軌道生成部２１２は、一定時間ごとに、現在の多点接触ロボットの状態を起点として、将来にわたる目標軌道を生成する。同様に、（ｄ）状態安定化と（ｃ）状態推定との間にもフィードバックループが存在する。ここでは、状態安定化部２１４が、現在の多点接触ロボットの状態と目標軌道との差が０に漸近するよう、力指令値を修正する。

本実施の形態の特徴は、これら２つのフィードバックループの周期が異なる点にある。すなわち、状態推定部２１３は、多点接触ロボットの状態推定結果を、軌道生成部２１２と状態安定化部２１４とに対して、それぞれ異なる周期でフィードバックする。コントローラ２１０は、軌道生成部２１２と状態推定部２１３とのフィードバックループは比較的長い周期（例えば２０ｍｓ）で実行する。一方、状態安定化部２１４と状態推定部２１３とのフィードバックループは比較的短い周期（例えば１ｍｓ）実行する。

フィードバック周期を異ならせてよい理由は次のとおりである。（ｄ）状態安定化処理は、現在のロボットの状態と目標軌道との差を修正するための制御である。もし、何らかの事情により多点接触ロボットの実際の状態と目標軌道とに差異が生じると、多点接触ロボットの運動は破綻して転倒するなどの可能性がある。そこで、（ｃ）状態推定と（ｄ）状態安定化との間のフィードバック処理はできるだけ短い時間（周期）で実施し、できるだけ素早く（ｅ）乃至（ｇ）による姿勢制御を実行して、多点接触ロボットの状態を安定化させる必要がある。一方、（ｂ）軌道生成処理は、現在の多点接触ロボットの重心位置や速度等を初期状態として、そこから将来にわたる安定軌道を生成する処理である。この処理は必ずしも（ｄ）ほど短い周期で実施する必要はない。

このように、コントローラ２１０は、（ｂ）軌道生成処理と、（ｄ）乃至（ｇ）にかかるリアルタイムの姿勢制御処理と、をそれぞれ異なる周期で実行できる。当然ながら、軌道生成処理と、リアルタイムの姿勢制御処理と、では許容される計算量も各々異なる。リアルタイムの姿勢制御処理は短い時間で実行する必要があるから、計算量をできる限り少なく抑える工夫が必要である。

次に、本実施の形態にかかる多点接触制御処理のアルゴリズムの一例について説明する。はじめに、本実施の形態にかかる多点接触制御処理の考え方について説明する。

本実施の形態では、多点接触ロボット全体の慣性が１つの重心で表され、各接触点には６軸力が定義されたモデルを用いる（図２）。この時、重心の並進運動量をＰ、重心回りの回転運動量をＬ、接触点の数をｎとすると、運動方程式は

と表される。

ここで、「重心位置、胴体姿勢角度、重心周り角運動量、各接触点の力、各接触点のモーメント」の指令値を

とし、誤差量を導入して

とすると、式（１）は

と表される。

軌道生成部２１２によって生成される指令値は当然、式（１）を満たし

が成り立っているので、上式（数４）と差を取れば

となる。ただし、誤差の２乗項は十分小さいとして近似的に

とした。

式（２）を行列表現に書き直せば

となる。ただし、

とする。

さらに、

とおけば、式（３）は簡単に

と書くことができる。ここで、ηは多点接触ロボットと目標軌道とのずれ量、ｕは接触点における荷重を示す。Ａ及びＢをシステム行列という。

ここで、状態推定部２１３で推定された結果として、「重心位置、速度、胴体姿勢角度、重心周り角運動量」の推定値

が状態安定化部２１４に入力されるものとする。

状態安定化部２１４の目的は

を達成することである。

ここで誤差を

と定義する。ただし、ｋは姿勢角誤差のフィードバックゲイン、ｅ_Φは瞬間的な姿勢角誤差をグローバル座標系で表現したベクトルである。ｅ_Φは、オイラー角とクォータニオンとの変換関数をＣ_ｑｕａｔとすれば次式で表される。

式（６）の第２式は、胴体姿勢角度の誤差にゲインを掛けて重心周り角運動量誤差に足し合わせる形になっており、これは近似的には重心周り角運動量誤差を積分しているとも解釈できる。つまり、重心周り角運動量誤差を減少させるようにフィードバックを行った場合には、近似的にはＰＩ制御を行ったのと同等と解釈できる。

したがって、状態安定化部２１４は、式（６）の誤差をフィードバックして、誤差が減少するように制御を行えば、式（５）の目的を達成できる。すなわち、誤差を式（６）で定義した時に、

となれば良い。

そこで本実施の形態では、誤差の漸近収束と各接触力への負荷の分配を目的として、次のような評価関数で表される最適レギュレータを適用する。

この最適レギュレータ問題は従来から非常に多くの分野で使われており、解もよく知られている。

ここで、Ｐはリッカチ方程式の解

である。また、Ｋはずれ量ηを打ち消す荷重ｕを得るためのゲインを示す。

このように、状態安定化部２１４は、状態推定部２１３が観測した誤差量（状態量）をフィードバックすることによって、式（７）を達成する補正力及びモーメントを算出することができる。

ここで、以下の２つの課題が生ずる。
（ア）接触点数や接触部位が変化していくのに伴って、システム式（４）が変化する。そのため、上記リッカチ方程式を予め解いておくことができない。一方、リアルタイムで毎回解くと計算時間を要するため、制御周期の上限が制限される。
（イ）状態フィードバックを示す式（８）では補正力及びモーメントの制限が考慮されていない。そのため、補正により、接触点で接触部位が滑る又は剥がれる等の接触不安定を引き起こす可能性がある。

まず課題（イ）の解決策を検討する。図３に、接触点の座標系（上添え字ｌがついている）と、接触多角形（接触点の支持多角形）を示した。接触点座標は接触点を原点として接触面の姿勢ｒ_ｉに合わせて定義されているものとする。このとき、接触座標系で定義された６軸力

は、接触面の姿勢行列

（ｒｏｔ（）はオイラー角と姿勢行列に変換する関数）を用いて、

と表される。

図４に示すように、接触点が安定して接触を保つためには以下の３つの条件を満たす必要がある。
（１）接触点が接触面から離れないこと
（２）接触点が接触面で滑らないこと
（３）接触点が接触面から剥がれないこと

（１）接触点が接触面から離れない為には、接触面の鉛直力が正であれば良い。すなわち、次式を満たす必要がある。

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

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

式（９）、（１０）、（１１）及び（１２）をまとめると

となる。

の関係を用いて更に式（１３）を書き直せば、

となる。ただし、

とした。

式（１４）を全接触点について書き並べると

と表すことができる。ただし、

とする。

課題（イ）とは、式（８）のフィードバックの結果、式（１５）が満たされないために、接触点を安定に保つ条件式（１０）、（１１）及び（１２）が満たされず、もって接触点で接触部位が滑りや剥がれを引き起こすという問題である。そこで本実施の形態では、直近で式（１５）を満たしつつ、評価関数

を最小化するようなフィードバックゲインを算出する。

この問題は、ＬＭＩ（Ｌｉｎｅａｒ Ｍａｔｒｉｘ Ｉｎｅｑｕａｌｉｔｙ）を用いて次のような最適化問題を解くことにより、解を得ることが可能である。

ただし、η_ｎは最も直近で得られた誤差値である。

式（１６）の形式の最適化問題は、収束計算による求解法である内点法を用いて効率良く解を求められることがよく知られている。式（１６）の求解結果Ｐ^＊を用いて

のようにフィードバックゲインが計算される。
接触点における摩擦力又は鉛直力を含む制約条件を課すことにより、補正によって接触点におけるすべりやはがれなどの不安定性を考慮した状態安定を行うことができる。

次に、課題（ア）の解決策について検討する。上記最適化問題は高速に求解することは可能だが、ある程度の計算時間が必要である。そこで、本実施の形態では、フィードバックゲインの計算を（ｂ）軌道生成と同時に行うことにより、課題（ア）を解決する手法を提案する。なお、フィードバックゲインの計算は、（ｄ）状態安定化におけるフィードバックループよりも遅い周期で実行すれば良く、必ずしも（ｂ）軌道生成のフィードバックループと同じ周期で行う必要はない。

すなわち、図５に示すように、本実施の形態では、２０ｍｓ周期で現在の状態推定量に基づいて式（１６）及び（１７）を解くことで最適フィードバックゲインＫを計算し、求められたフィードバックゲインＫを用いて１ｍｓ周期で状態フィードバックを行う、というフレームワークを採用する。状態フィードバックの計算自体は、フィードバックゲインと現在の状態推定量から算出した誤差量を乗算するだけなので、非常に少ない計算量で補正力及びモーメントを計算することができる。状態フィードバックの計算を、誤差とフィードバックゲインを積算するだけの計算にとどめるので、短周期で計算することができる。

図６乃至図８を用いて、本実施の形態の特徴的な部分について再度説明する。図６は、本実施の形態のフレームワークを示すブロック線図である。図７は、最適ゲイン計算器２１２２の動作を示すフローチャートである。図８は、誤差計算器２１３２及び状態フィードバック２１４２の動作を示すフローチャートである。

図６のブロック線図は、複数の機能ブロックと、機能ブロック間のデータの入出力を示す矢印とからなっている。軌道生成器２１２１及び最適ゲイン計算器２１２２は、軌道生成部２１２に含まれる機能ブロックである。状態推定器２１３１及び誤差計算器２１３２は、状態推定部２１３に含まれる。状態フィードバック２１４２は、状態安定化部２１４に含まれる。また、図６において、接触点パラメータｓ_ｉ＝（μ_ｉ，（ｘ^ｌ _ｉｌ，ｙ^ｌ _ｉｌ），・・・，（ｘ^ｌ _ｉｈ，ｙ^ｌ _ｉｈ））は接触点の摩擦係数及び接触多角形を表すものとする。

このうち、軌道生成器２１２１及び最適ゲイン計算器２１２２は、短い周期、典型的には２０ｍｓ周期で処理を実行する。一方、状態推定器２１３１、誤差計算器２１３２及び状態フィードバック２１４２は、長い周期、典型的には１ｍｓ周期で処理を実行する。

まず長い周期、典型的には２０ｍｓ周期で処理が実行される機能ブロックの動作について説明する。軌道生成器２１２１が、接触点計画部２１１又は上位装置によって計画された接触点位置及び姿勢ｐ_ｉ［ｋ］，ｒ_ｉ［ｋ］及び接触点パラメータｓ_ｉ［ｋ］に基づいて、重心位置及び速度ｑ_ｄ［ｋ］，ｑ_ｄ［ｋ］、胴体姿勢角φ_ｄ［ｋ］、重心周り角運動量Ｌ_ｄ［ｋ］、接触点の力及びモーメントｆ_ｉｄ［ｋ］，τ_ｉｄ［ｋ］、手先及び足先位置並びに姿勢（接触点含む）ｐ^ｔ _ｉ［ｋ］，ｒ^ｔ _ｉ［ｋ］の時系列軌道を生成し、軌道バッファに格納する。ここで、軌道生成器としては、例えば特願２０１３−２５４９８９に記載のものを利用できる。

最適ゲイン計算器２１２２は、軌道生成器２１２１から、重心位置、重心周り角運動量、接触点の６軸力の現在の目標値ｑ_ｄ、Ｌ_ｄ、ｆ_ｉｄ及びτｉｄ、並びに接触点の位置ｐｉ及び接触点パラメータｓ_ｉを取得する（図７、Ｓ１０１）。そして、これらの取得値を用い、数１０に従ってシステム行列Ａ、Ｂを求める（Ｓ１０２）。また、数３１に示した不等式制約条件Ｇ、ｗを求める（Ｓ１０３）。

また、最適ゲイン計算器２１２２は、誤差計算器２１３２から状態誤差値の最新値（現在値）η_ｎを取得する（図７、Ｓ１０４）。そして、式（１６）により最適化問題の求解結果Ｐ^＊を得る（Ｓ１０５）。最後に、式（１７）及びＰ^＊を用いて、最適フィードバックゲインＫを算出し、出力する（Ｓ１０６）。

次に短い周期、典型的には１ｍｓ周期で処理が実行される機能ブロックの動作について説明する。状態推定器２１３１が、各センサ値を用いて現在の重心位置及び速度ｑ，ｑ、胴体姿勢角度φ、重心周り角運動量Ｌを出力する。なお、状態推定器２１３１としては、例えば特願２０１４−０８９９４３に記載のものを利用できる。

誤差計算器２１３２は、軌道バッファから現在時刻における目標値ｑ_ｄ、ｑ_ｄ、φ_ｄ、Ｌ_ｄを取得する（図８、Ｓ２０２）。また、状態推定器２１３１から状態推定値ｑ、ｑ、φ、Ｌを取得する（Ｓ２０３）。そして、誤差計算器２１３２は、これらの値を用い、式（６）に基づいて現在誤差ベクトルηを計算する。

状態フィードバック２１４２は、最適ゲイン計算器２１２２から最適フィードバックゲインＫを取得する（図８、Ｓ２０１）。また、誤差計算器２１３２から誤差ベクトルηを取得する（Ｓ２０４）。そして、Ｋとηとを乗算して、接触点の補正力及びモーメントΔｆ_ｉ，Δτ_ｉを計算する（Ｓ２０５）。さらに、ｆ^ｒｅｆ _ｉ＝ｆ_ｉｄ［ｊ］＋Δｆ_ｉ，τ^ｒｅｆ _ｉ＝τ_ｉｄ［ｊ］＋Δτ_ｉのように、現在時刻における目標６軸力に上記補正量を加算することで、６軸力指令値を出力する（Ｓ２０６）。

本実施の形態によれば、軌道生成部２１２における軌道生成処理（最適ゲイン計算を含む）と、状態安定化部２１４による姿勢制御処理（誤差フィードバックを含む）とは、異なる周期で実行される。これにより、状態安定化部２１４は、比較的計算量の多い、すなわち計算に時間を要する最適ゲイン計算処理に影響されることなく、比較的短い時間で誤差フィードバック処理を実行できるようになる。そのため、早い周期で姿勢制御を行うことができ、任意の多点接触状態において重心軌道追従・安定化性能を向上させることができる。

また、本実施の形態によれば、状態安定化部２１４は、式（１６）による定式化と、図５に示すフレームワークとにより、各接触点の安定化を保ちつつ軌道追従誤差をゼロに漸近させる処理を、低い計算コストで高速に実行することができる。これにより、状態安定化部２１４は、早い周期で姿勢制御を行うことができ、任意の多点接触状態において重心軌道追従・安定化性能を向上させることができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上述の実施の形態では、式（１３）を接触点の接触安定性を保証する条件のみで構成したが、例えば鉛直接触力の上限値を設定して

のような条件を追加し、式（１３）を

とし、式（１４）を

とすることも可能である。

また、上述の実施の形態では、本発明を主にハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、様々なタイプの非一時的なコンピュータ可読媒体（ｎｏｎ−ｔｒａｎｓｉｔｏｒｙ ｃｏｍｐｕｔｅｒ ｒｅａｄａｂｌｅ ｍｅｄｉｕｍ）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（ｔａｎｇｉｂｌｅ ｓｔｏｒａｇｅ ｍｅｄｉｕｍ）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Ｐｒｏｇｒａｍｍａｂｌｅ ＲＯＭ）、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ ＰＲＯＭ）、フラッシュＲＯＭ、ＲＡＭ（ｒａｎｄｏｍ ａｃｃｅｓｓ ｍｅｍｏｒｙ））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（ｔｒａｎｓｉｔｏｒｙ ｃｏｍｐｕｔｅｒ ｒｅａｄａｂｌｅ ｍｅｄｉｕｍ）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１００ 多点接触ロボット
２１０ コントローラ
２１１ 接触点計画部
２１２ 軌道生成部
２１２１ 軌道生成器
２１２２ 最適ゲイン計算器
２１３ 状態推定部
２１３１ 状態推定器
２１４ 状態安定化部
２１３２ 誤差計算器
２１４２ 状態フィードバック
２１５ 接触力制御部
２１６ 全身ＩＫ部
２１７ 関節サーボ部

Claims (7)

1. 複数の接触点を有する多点接触ロボットであって、
   目標重心軌道及び前記接触点各々の反力目標値を生成する軌道生成部と、
   現在の重心を推定し、前記目標重心軌道と前記現在の重心との誤差を算出する状態推定部と、
   前記誤差が前記目標重心軌道に収束するよう前記反力目標値を修正する状態安定化部と、を有し、
   前記軌道生成部は、第１の周期で、前記誤差を前記目標重心軌道に収束させるためのフィードバックゲインを算出し、
   前記状態安定化部は、前記第１の周期よりも短い第２の周期で、前記誤差と前記フィードバックゲインとに基づき前記反力目標値を修正する
   多点接触ロボット。
2. 前記軌道生成部は、前記接触点における摩擦力又は鉛直力を含む制約条件に基づいて、前記フィードバックゲインを算出する
   請求項１記載の多点接触ロボット。
3. 前記状態安定化部は、前記誤差と前記フィードバックゲインとを乗ずることにより、前記反力目標値を修正する
   請求項１又は２記載の多点接触ロボット。
4. 複数の接触点を有する多点接触ロボットの目標重心軌道及び前記接触点各々の反力目標値を生成する軌道生成ステップと、
   現在の重心を推定し、前記目標重心軌道と前記現在の重心との誤差を算出する誤差計算ステップと、
   第１の周期で、前記誤差を前記目標重心軌道に収束させるためのフィードバックゲインを算出する最適ゲイン計算ステップと、
   前記第１の周期よりも短い第２の周期で、前記誤差と前記フィードバックゲインとに基づき前記反力目標値を修正する状態フィードバックステップと、を含む
   多点接触ロボットの制御方法。
5. 前記最適ゲイン計算ステップは、前記接触点における摩擦力又は鉛直力を含む制約条件に基づいて、前記フィードバックゲインを算出する
   請求項４記載の多点接触ロボットの制御方法。
6. 前記状態フィードバックステップは、前記誤差と前記フィードバックゲインとを乗ずることにより、前記反力目標値を修正する
   請求項４又は５記載の多点接触ロボットの制御方法。
7. コンピュータに、請求項４乃至６いずれか１項記載の多点接触ロボットの制御方法を実行させるためのプログラム。

Images