JP2013000878A - ロボットの均衡制御装置及びその制御方法 - Google Patents

Abstract

【課題】必要なエネルギーを効率よく使用できるロボットの歩行制御。
【解決手段】複数の関節部がそれぞれ設けられた複数の脚と、その脚に連結されたた上体を有するロボットの均衡制御方法において、上体のポーズ角度及び複数の関節部の角度を検出し、ポーズ角度及び複数の関節部の角度に基づいて現在キャプチャーポイント及び現在ヒップ高さを獲得し、現在キャプチャーポイントと予め設定された目標キャプチャーポイントとを比較してキャプチャーポイントエラーを算出し、現在ヒップ高さと予め設定された目標ヒップ高さとを比較してヒップ高さエラーを算出し、キャプチャーポイントエラーとヒップ高さエラーに基づいて補償力を算出し、その補償力に基づいて複数の関節部の少なくとも一つの自由度に対するトルクを算出し、複数の関節部に少なくとも一つの自由度に対するトルクを出力することによって、ロボットの歩行を制御する。
【選択図】図７

Description

本発明は、複数の脚に設けられた関節部の駆動を制御して均衡を維持させるロボットの均衡制御方法及びその制御方法に関するものである。

ロボットは人と類似した関節体系を有し、この関節体系を用いて人間の手足と同じ動作をする機械である。

初期には、工場の生産作業の自動化、無人化のために産業用ロボットの開発が進んだが、最近は人間に種々サービスを提供するためのサービス用ロボットの開発が活発に行われている。

かかるサービス用ロボットは、ほとんどが人間の歩行を摸した歩行を行いながら人間にサービスを提供する。したがって、安定した姿勢を維持しながら歩行できるロボットに対する研究開発が活発に行われている。

ロボットの歩行制御方法としては、ロボット関節の目標位置を追従する位置基盤のゼロモーメントポイント（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ；以下、ＺＭＰという）制御方法、ロボット関節の目標トルクを追従するトルク基盤の動的歩行（Ｄｙｎａｍｉｃ Ｗａｌｋｉｎｇ）制御方法、または有限状態機械（Ｆｉｎｉｔｅ Ｓｔａｔｅ Ｍａｃｈｉｎｅ；以下、ＦＳＭという）制御方法などがある。

ここで、ＺＭＰ制御方法は、位置基盤の制御方法であり、正確な位置制御が可能である反面、このために各関節の正確な角度制御を行わなければならないので、高いサーボゲインが必要となる。したがって、高い電流を必要とするため、エネルギー効率が低く、関節の剛性が大きくなり、周囲環境での衝突時に大きい衝撃を加えることがある。

また、与えられた重心と脚の歩行パターンから逆運動学（Ｉｎｖｅｒｓｅ ｋｉｎｅｍａｔｉｃｓ）を用いて各関節の角度を計算するためには、運動学的特異点（Ｋｉｎｅｍａｔｉｃ Ｓｉｎｇｕｌａｒｉｔｙ）を避けるべきであるので、歩行中に膝を常に曲げた姿勢を維持しなければならず、その結果、人間とは異なる不自然な歩行をするようになる。

さらに、逆運動学（Ｉｎｖｅｒｓｅ ｋｉｎｅｍａｔｉｃｓ）を用いると、結局、関節の位置制御も行わなければならないが、この時に所望の動作を得るためには、高いゲインを用いなければならず、結果として、瞬間的な外乱に対して柔軟に対応できない剛性（ｓｔｉｆｆｎｅｓｓ）を招くという問題点があった。

トルク基盤の動的歩行制御方法は、安定した歩行のために動的方程式（Ｄｙｎａｍｉｃｓ Ｅｑｕａｔｉｏｎ）を解かなければならないが、空間上で任意の方向を具現できる６自由度の脚を有するロボットの動的方程式が複雑すぎるため、実際には４自由度以下の脚を有するロボットにのみ適用してきた。

ＦＳＭ制御方法は、トルク命令に応じて制御されて、弾性メカニズムに適用することができるので、エネルギー効率が高く、剛性が低くなり、周囲環境に対して安全である反面、正確な位置制御が不可能であるため、階段を上がったり障害物を避けるなどの正確な全身モーションを行うことが難しい。

本発明の一側面は、キャプチャーポイントとヒップ高さに基づいて、水平方向及び垂直方向の力を補償し、均衡の取れた直立姿勢を維持するロボットの均衡制御装置及びその制御方法を提供する。

本発明の他の側面は、ポーズ角度に基づいて、モーメントを補償し、均衡の取れた姿勢を維持するロボットの均衡制御装置及びその制御方法を提供する。

本発明のさらに他の側面は、複数の脚に印加される補償力を配分し、均衡の取れた姿勢を維持するロボットの均衡制御装置及びその制御方法を提供する。

本発明の一側面に係る、ロボットの均衡制御方法は、複数の関節部がそれぞれ設けられた複数の脚と、その脚に連結された上体を含み複数の並進運動自由度と回転運動自由度を有するロボットの均衡制御方法において、上体のポーズ角度及び複数の関節部の角度を検出し、ポーズ角度及び複数の関節部の角度に基づいて現在の姿勢を獲得し、複数の並進運動自由度のうち少なくとも一つの並進運動自由度に対して、姿勢と予め設定された目標姿勢に基づいた姿勢エラーを算出し、姿勢エラーに基づいて少なくとも一つの並進運動自由度に対する補償力を算出し、少なくとも一つの並進運動自由度に対する補償力に基づいて、少なくとも一つの並進運動自由度に対する目標トルクを複数の関節部に対してそれぞれ算出し、少なくとも一つの並進運動自由度に対する目標トルクを複数の関節部に出力して、ロボットの均衡を制御する。

目標トルクを算出することは、少なくとも一つの並進運動自由度に対する目標トルクを残りの並進運動自由度に対応する目標トルクに合算して目標トルク和を算出する。

姿勢エラーを算出することは、検出されたポーズ角度及び複数の関節部の角度に基づいて現在の重心位置を検出するものの、少なくとも一つの並進運動自由度に対する現在の重心位置を検出し、その検出された現在の重心と予め設定された目標重心とを比較して、少なくとも一つの並進運動自由度に対する重心エラーを算出し、重心エラーを姿勢エラーとして用いることを含む。

姿勢エラーを算出することは、検出されたポーズ角度及び複数の関節部の角度に基づいて、現在重心の位置と速度を検出するものの、少なくとも一つの並進運動自由度に対する現在重心の位置と速度を検出し、その検出された現在重心の位置と速度に基づいて、少なくとも一つの並進運動自由度に対する現在キャプチャーポイントを獲得し、その獲得された現在キャプチャーポイントと予め設定された目標キャプチャーポイントとを比較して、少なくとも一つの並進運動自由度に対するキャプチャーポイントエラーを算出し、その算出されたキャプチャーポイントエラーを姿勢エラーとして用いることを含む。

姿勢エラーを算出することは、上体のポーズ角度及び複数の関節部の角度を検出し、ポーズ角度及び複数の関節部の角度に基づいて、ヒップまたは上体のいずれか一つの参照点の位置を少なくとも一つの自由度に対して算出し、現在の参照点と予め設定された目標参照点の位置とを比較して、参照点位置エラーを少なくとも一つの自由度に対して算出し、参照点位置エラーを少なくとも一つの並進運動自由度の姿勢エラーとして用いることを含む。

姿勢エラーを算出することは、複数の並進運動自由度のうち残りの並進運動自由度の姿勢エラー算出と相異なる。

補償力を算出することは、現在キャプチャーポイント及び現在ヒップ高さを獲得し、現在キャプチャーポイントと予め設定された目標キャプチャーポイントとを比較して、キャプチャーポイントエラーを算出し、現在ヒップ高さと予め設定された目標ヒップ高さとを比較して、ヒップ高さエラーを算出し、キャプチャーポイントエラーとヒップ高さエラーに基づいて、補償力を算出することを含む。

現在キャプチャーポイントを獲得することは、上体のポーズ角度及び複数の関節部の角度に基づいて、重心の位置と速度を獲得し、その獲得された重心の位置と速度に基づいて現在キャプチャーポイントを獲得することを含む。

現在ヒップ高さを算出することは、重心及び複数の関節部の角度に基づいて、現在ヒップ高さを算出することを含む。

補償力を算出することは、キャプチャーポイントエラーを用いて、水平方向の補償力を算出し、ヒップ高さエラーを用いて、垂直方向の補償力を算出することを含む。

本発明に係るロボットの均衡制御方法は、上体のポーズ角度及び複数の関節部の角度に基づいて、現在の姿勢を判断し、現在の姿勢と予め格納されたモーション情報に基づいて、目標キャプチャーポイント、目標ヒップ高さを設定することをさらに含む。

目標トルクを算出することは、ロボットの重心が地面に投影された投影点と複数の脚にそれぞれ連結された足間の距離比率を算出し、足間の距離比率に基づいて、複数の脚に印加される補償力を配分し、複数の脚に配分された補償力に基づいて、複数の関節部に印加される目標トルクを算出することをさらに含む。

キャプチャーポイントを獲得することは、順運動学を用いる。

本発明に係るロボットの均衡制御方法は、上体のポーズ角度と予め設定された目標ポーズ角度とを比較してポーズ角度エラーを算出し、ポーズ角度エラーに基づいて補償モーメントを算出し、目標トルクの算出時、補償モーメントを反映することをさらに含む。

補償モーメントを目標トルクに反映することは、ジャコビアンを用いて補償モーメントを関節部のトルクに変換し、その変換されたトルクを目標トルクの算出時に合算することを含む。

補償モーメントを算出することは、ポーズ角度エラーを用いて、ヨー、ロール、ピッチ方向の補償モーメントを算出することを含む。

ポーズ角度を検出することは、上体のヨー角度、ロール角度、ピッチ角度のうち少なくとも一つの角度を検出することを含む。

目標トルクを算出することは、補償力、ロボットの質量及び地球重力加速度に基づいて、仮想重力加速度を算出し、仮想重力加速度に基づいて、少なくとも一つの自由度に対応する仮想重力補償トルクをそれぞれ算出し、仮想重力補償トルクを反映して、目標トルクを算出することをさらに含む。

本発明に係るロボットの均衡制御方法は、複数の自由度のうち残りの自由度に対する位置及び角度を設定し、その設定された位置及び角度を満足する逆運動学の解を演算し、逆運動学の解に基づいて、複数の関節部の残りの自由度に対する目標角度を算出し、複数関節部の残りの自由度に対する目標角度に基づいて、複数関節部の残りの自由度に対応するトルクを算出することをさらに含む。

目標トルクを算出することは、少なくとも一つの自由度に対するトルクと残りの自由度に対するトルクとを合算して、目標トルクを算出することを含む。

本発明に係るロボットの均衡制御方法は、複数の関節部のうち少なくとも一つの関節部に対する目標角度が入力されて、その入力された少なくとも一つの関節部の目標角度を追従するためのトルクを算出し、目標トルクの算出時、少なくとも一つの関節部の目標角度に対するトルクを反映することをさらに含む。

目標角度が入力されることは、複数の関節部に複数の自由度に対応する複数の関節がそれぞれ設けられた場合、複数の関節の目標角度がそれぞれ入力されることを含む。

目標トルクを算出することは、ジャコビアンを用いて目標トルクを算出することを含む。目標トルクを算出することは、補償力、ロボットの質量及び地球重力加速度に基づいて、仮想重力加速度を算出し、仮想重力加速度に基づいて、少なくとも一つの自由度に対応する仮想重力補償トルクを算出し、その算出された仮想重力補償トルク及びジャコビアンに基づいて算出されたトルクを用いて、複数の関節部の少なくとも一つの自由度に対応する目標トルクを算出することをさらに含む。

本発明の他の側面に係るロボットの均衡制御装置は、複数の関節部がそれぞれ設けられた複数の脚と、その脚に連結された上体を有しており、複数の自由度を有するロボットの均衡制御装置において、上体のポーズ角度を検出するポーズ検出部；複数の関節部の角度を検出する角度検出部；予め格納されたモージョン情報に基づいて、複数の自由度のうち少なくとも一つの自由度に対して目標姿勢を設定する設定部；ポーズ角度及び複数の関節部の角度に基づいて、少なくとも一つの自由度に対して現在姿勢を獲得し、現在姿勢と目標姿勢とを比較して姿勢エラーを算出し、姿勢エラーに基づいて少なくとも一つの自由度に対して補償力を算出し、補償力に基づいて少なくとも一つの自由度に対する目標トルクを算出する均衡制御部；複数の関節部に少なくとも一つの自由度に対する目標トルクを出力するサーボ制御部を含む。

均衡制御部は、上体のポーズ角度及び複数の関節部の角度に基づいて重心を獲得し、重心に基づいて現在姿勢を獲得する獲得部を含む。

均衡制御部は、上体のポーズ角度及び複数の関節部の角度に基づいて重心の位置と速度を獲得し、重心の位置と速度に基づいて現在キャプチャーポイントを獲得して、現在姿勢を獲得する獲得部を含む。

均衡制御部は、上体のポーズ角度及び複数の関節部の角度に基づいてヒップや上体の参照点の位置を獲得し、参照点の位置に基づいて現在姿勢を獲得する獲得部を含む。

均衡制御部は、ポーズ角度及び複数の関節部の角度に基づいて、現在キャプチャーポイント及び現在ヒップ高さを獲得し、現在キャプチャーポイントと目標キャプチャーポイントとを比較してキャプチャーポイントエラーを算出し、現在ヒップ高さと目標ヒップ高さとを比較して、ヒップ高さエラーを算出し、キャプチャーポイントエラーとヒップ高さエラーに基づいて補償力を算出し、その補償力に基づいて複数の自由度のうち少なくとも一つの自由度に対する目標トルクを算出する。

設定部は、少なくとも一つの自由度に対して目標重心を設定し、目標重心を用いて目標姿勢を設定する。設定部は、少なくとも一つの自由度に対して目標キャプチャーポイントを設定し、目標キャプチャーポイントを用いて目標姿勢を設定する。

設定部は、少なくとも一つの自由度に対してヒップや上体の参照点の目標位置を設定し、参照点の目標位置に基づいて目標姿勢を設定する。

均衡制御部は、上体のポーズ角度及び複数の関節部の角度に基づいて、重心及び前記ヒップ高さを獲得し、重心に基づいて現在キャプチャーポイントを獲得する獲得部を含む。

均衡制御部は、キャプチャーポイントエラーを用いて水平方向の補償力を算出し、ヒップ高さエラーを用いて垂直方向の補償力を算出することを含む。

本発明に係るロボットの均衡制御装置は、複数の脚に設けられた各足に印加される荷重を検出する力／トルク検出部をさらに含み、設定部は、各足に印加される荷重に基づいて現在姿勢を判断し、現在姿勢と予め設定されたモーション情報に基づいて、目標キャプチャーポイント、目標ヒップ高さを設定する。

本発明に係るロボットの均衡制御装置は、ロボットの重心が地面に投影された投影点と複数の脚にそれぞれ連結された足間の距離比率を算出し、足間の距離比率に基づいて、複数の脚に印加される補償力を配分する配分部をさらに含み、均衡制御部は、複数の脚に配分された補償力に基づいて、複数の関節部に印加されるトルクを算出する。

均衡制御部は、上体のポーズ角度と予め設定された目標ポーズ角度とを比較してポーズ角度エラーを算出し、ポーズ角度エラーに基づいて補償モーメントを算出し、トルクの算出時に補償モーメントを反映することをさらに含む。

均衡制御部は、ポーズ角度エラーを用いて、ヨー、ロール、ピッチ方向の補償モーメントを算出することを含む。

設定部は、複数の脚に設けられた足の支持領域内の一点が重力方向に通る線上の一点を目標キャプチャーポイントとして設定する。

本発明に係るロボットの均衡制御装置は、使用者から少なくとも一つの姿勢からなるモーション情報が入力される入力部をさらに含み、設定部は、入力されたモーション情報を格納する。

均衡制御部は、補償力、予め格納されたロボットの質量及び地球重力加速度に基づいて仮想重力加速度を算出し、仮想重力加速度に基づいて少なくとも一つの自由度に対応する仮想重力補償トルクをそれぞれ算出し、仮想重力補償トルクを反映して目標トルクを算出する。

均衡制御部は、複数の自由度のうち残りの自由度に対するトルクを逆運動学を用いて算出する。均衡制御部は、複数の関節部のうち少なくとも一つの関節部に対する目標角度が入力されると、その入力された目標角度に対応するトルクを算出し、目標トルクの算出時に少なくとも一つの関節部の目標角度に対するトルクを反映する。

均衡制御部は、ジャコビアンを用いて目標トルクを算出する。

本発明の一側面に係るロボットの均衡制御装置及びその制御方法によれば、重心の位置と速度を合成して得られたキャプチャーポイントを用いて、次の姿勢の均衡を維持するための複数の関節部のトルクを獲得することによって、外乱の多い環境で均衡を維持することができる。

また、ロボットの上体姿勢を制御できるため、傾斜面と凹凸面でも転倒せず安定的に均衡を維持でき、外部の外乱にも能動的に対応できる。

なお、歩行時に膝を曲げずに歩行できて、大きな歩幅で歩行できるため、歩行時に必要なエネルギーを効率よく使用することができる。

本発明の一実施例に係るロボットの例示図である。 本発明の一実施例に係るロボットの関節構造の例示図である。 本発明の一実施例に係るロボットの均衡制御装置の制御構成図である。 本発明の一実施例に係るロボットに格納されたＦＳＭの状態を示す例示図である。 本発明の一実施例に係るロボットの均衡制御装置の詳細構成図である。 本発明の一実施例に係るロボットの重心、キャプチャーポイント、ヒップ高さの獲得状況を示す例示図である。 本発明の一実施例に係るロボットの均衡制御方法を示すフローチャートである。 本発明の一実施例に係るロボットの均衡制御方法の詳細構成図である。 本発明の他の実施例に係るロボットの均衡制御方法の制御構成図である。 本発明の他の実施例に係るロボットの均衡制御方法を示すフローチャートである。 本発明のさらに他の実施例に係るロボットの均衡制御方法を示すフローチャートである。

以下、添付の図面を参照しながら、本発明を詳細に説明する。

図１は実施例に係るロボットの例示図であり、図２は実施例に係るロボットの関節構造の例示図である。図１に示すように、ロボット１００は、頭１１０、首１２０、胴体１３０、腕１４０Ｒ，１４０Ｌ、手１５０Ｒ，１５０Ｌからなる上体と、複数の脚１６０Ｒ，１６０Ｌ及び足１７０Ｒ，１７０Ｌからなる下体とを有する。

さらに具体的に説明すると、ロボット１００の上体は、頭１１０と、頭１１０の下部に首１２０を介して連結された胴体１３０と、胴体１３０の上部両側に連結された二つの腕１４０Ｒ，１４０Ｌと、該二つの腕１４０Ｒ，１４０Ｌの末端にそれぞれ連結された手１５０Ｒ，１５０Ｌとからなる。

ロボット１００の下体は、上体の胴体１３０の下部両側に連結された二つの脚１６０Ｒ，１６０Ｌと、該二つの脚１６０Ｒ，１６０Ｌの末端にそれぞれ連結された足１７０Ｒ，１７０Ｌとからなる。

ここで、頭１１０、二つの腕１４０Ｌ，１４０Ｒ、二つの手１５０Ｌ，１５０Ｒ、二つの脚１６０Ｌ，１６０Ｒ、二つの足１７０Ｌ，１７０Ｒはそれぞれ関節を介して一定レベルの並進運動自由度及び回転運動自由度を有する。

かかるロボット１００の上体と下体はカバーによって保護される。

参照符号“Ｒ”と“Ｌ”は、それぞれロボット１００の右側（Ｒｉｇｈｔ）と左側（Ｌｅｆｔ）を表している。

図２を参照して具体的に説明する。頭１１０には、周囲の映像を獲得するカメラ１１１と、使用者の音声を検出するマイクロホン１１２とが設けられている。

首１２０は、頭１１０と胴体１３０とを連結する。かかる首１２０は複数の首関節部からなる。首関節部は、ヨー方向（ｙａｗ、Ｚ軸回転）の回転関節１２１、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）の回転関節１２２及びロール方向（ｒｏｌｌ、Ｘ軸回転）の回転関節１２３を含んで３自由度を有する。ここで、首関節部の回転関節１２１，１２２，１２３は頭回転用モーター（図示せず）とそれぞれ連結される。

胴体１３０の両側には二つの腕１４０Ｌ，１４０Ｒを連結する肩関節部１３１がそれぞれ設けられ、胸と腰との間には、胸が腰に対して回転できるようにヨー（Ｙａｗ）方向の回転関節部１３２が設けられている。

二つの腕１４０Ｌ，１４０Ｒは、上腕リンク１４１、下腕リンク１４２、肘関節部１４３、手首関節部１４４をそれぞれ有する。

ここで、各上腕リンク１４１は、肩関節部１３１を介して胴部１３０と連結され、また肘関節部１４３を介して下腕リンク１４２と連結され、各下腕リンク１４２は手首関節部１４４を介して手１５０Ｒ，１５０Ｌと連結される。

肘関節部１４３は、ピッチ方向の回転関節１４３ａとヨー方向の回転関節１４３ｂとを含んで２自由度を有し、手首関節部１４４は、ピッチ方向の回転関節１４４ａとロール方向の回転関節１４４ｂとを含んで２自由度を有する。

手１５０Ｒ，１５０Ｌには、５個の指１５１が設けられており、各指１５１にはモーターによって駆動される多数の関節（図示せず）が設けられてもよい。かかる指１５１は、腕１４０Ｒ，１４０Ｌの動きに連動して物を把持したり、特定の方向を指し示すなどの様々な動作を行う。

ロボット１００の二つの脚１６０Ｒ，１６０Ｌは、それぞれ大腿リンク１６１、下腿リンク１６２、股関節部１６３、膝関節部１６４、足首関節部１６５をそれぞれ有する。

各大腿リンク１６１は、股関節部１６３を介して胴体１３０と連結され、また膝関節部１６４を介して下腿リンク１６２と連結され、各下腿リンク１６２は足首関節部１６５を介して足１７０Ｌ，１７０Ｒと連結される。

股関節部１６３は、ヨー方向（ｙａｗ，Ｚ軸回転）の回転関節１６３ａと、ピッチ方向（ｐｉｔｃｈ，Ｙ軸回転）の回転関節１６３ｂと、ロール方向（ｒｏｌｌ，Ｘ軸回転）の回転関節１６３ｃとを含んで３自由度を有する。

さらに、二つの脚１６０Ｌ，１６０Ｒにおける股関節部１６３の位置は、ヒップ（Ｈｉｐ）の位置に対応する。

膝関節部１６４は、ピッチ方向の回転関節１６４ａを含んで１自由度を有し、足首関節部１６５はピッチ方向の回転関節１６５ａとロール方向の回転関節１６５ｂとを含んで２自由度を有する。

このように、二つの脚１６０Ｌ，１６０Ｒには、それぞれ三つの関節部１６３，１６４，１６５に対して６個の回転関節が設けられるので、二つの脚１６０Ｌ，１６０Ｒにはトータルで１２個の回転関節が設けられる。

かかるロボット１００の各関節には、モーター（図示せず）などのアクチュエーターが設けられている。したがって、各関節は、モーターの回転によって適切に回転運動を行うことで、様々な動作を具現できる。

したがって、ロボットの歩行時に、ロボットが均衡を維持しながら安定的で自然な歩行を行うことができる。これについて、図３を参照して具体的に説明する。

図３は本実施例に係るロボットの均衡制御装置の構成図であり、図３乃至図６を参照して説明する。ロボットの均衡制御装置は、力／トルク検出部２１０、ポーズ検出部２２０、角度検出部２３０、設定部２４０、重心獲得部２５１ａ、均衡制御部２５０、サーボ制御部２６０、入力部２７０を含む。

力／トルク検出部（Ｍｕｌｔｉ−Ａｘｉｓ Ｆｏｒｃｅ ａｎｄ Ｔｏｒｑｕｅ Ｓｅｎｓｏｒ）２１０は多軸センサーであり、各脚１６０Ｌ，１６０Ｒと足１７０Ｒ，１７０Ｌとの間に設けられ、足１７０Ｌ，１７０Ｒに加えられる荷重を検出する。

この際、足１７０Ｌ，１７０Ｒに伝わる力の３方向成分（Ｆｘ，Ｆｙ，Ｆｚ）とモーメントの３方向成分（Ｍｘ，Ｍｙ，Ｍｚ）を検出して設定部２４０に転送する。

ポーズ検出部２２０は、胴体１３０に設けられ、鉛直線に対する上体のポーズを検出するものであって、ロール方向（ｒｏｌｌ，Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ，Ｙ軸回転）、ヨー方向（ｙａｗ，Ｚ軸回転）の三つの軸の回転角度のうち少なくとも一つの回転角度を検出し、検出された各軸の回転角度を重心獲得部２５１ａ及び均衡制御部２５０に転送する。

ここで、ポーズ検出部２２０は、慣性を測定するＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ ｕｎｉｔ）を用いることができる。

さらに、ロボット１００の上体の姿勢を測定するために、ＩＭＵの他にティルティング検出（Ｔｉｌｔｉｎｇ Ｄｅｔｅｃｔｉｏｎ）、ジャイロセンサー（Ｇｙｒｏ Ｓｅｎｓｏｒ）などを用いることも可能である。

角度検出部２３０は、各関節部１３１，１４３，１４４，１６３，１６４，１６５の角度を検出するものであって、各関節部１３１，１４３，１４４，１６３，１６４，１６５の各軸に設けられたモーターの回転角度を検出する。

ここで、各関節部１３１，１４３，１４４，１６３，１６４，１６５の回転角度は、モーター（図示せず）の回転数であって、各モーターに連結されたエンコーダ（Ｅｎｃｏｄｅｒ、図示せず）を介して検出することが可能である。

ロボットの姿勢を表す方法について説明する。通常的に、一つの姿勢は、上体及び下体の全関節の角度により表せるが、本発明では、次のようにロボットの姿勢を表すことができる。ロボットの姿勢は、作業空間において、ロボットの並進運動の自由度と回転運動の自由度に対してロボットの位置と角度により表すことができる。このロボットの位置と角度は、それを表せるロボットのいずれか一点の位置とロボットのポーズセンサーの傾き角度及び志向角度を用いて表すことができる。ロボットの位置は、３軸並進運動自由度に対して順運動学を用いて、基準座標系において、ロボットのいずれか一点のｘ、ｙ、ｚ軸の３次元座標で表すことができる。また、ロボットの角度は、回転運動自由度に対して順運動学またはポーズセンサーを用いて、ロール、ピッチ、ヨー（ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）のポーズ角度で表せる。したがって、本発明において、一つの姿勢を取るときの全関節の角度は固定した値ではない。

ここで、ロボットの姿勢を構成する位置と角度のうち、位置を表すいずれか一点はロボットの重心でもよく、或いはキャプチャーポイントでもよい。または、脚及び腕を除いた部分に固定された一点を参照点として、ロボットの位置を表してもよい。３軸の並進自由度に対してロボットの位置を表す点が、重心、キャプチャーポイント、参照点のうちいずれか一つと一致する必要はなく、相異なる方式で表してもよい。例えば、２次元水平方向の並進運動自由度に対するロボットの姿勢はキャプチャーポイントの２次元水平成分で定義し、１次元垂直方向の並進運動自由度に対するロボットの姿勢はヒップ高さで定義して、設定部及び均衡制御部で用いることも可能である。

ここで、キャプチャーポイント（Ｃａｐｔｕｒｅ Ｐｏｉｎｔ：ＣＰ）の通常的な意味は、ロボットの現在重心の位置と速度に照らして、次の歩行動作を行ったときにロボットが転倒せずに直立できる位置のことなので、２次元面上に定義された点である。ところが、本発明においては、キャプチャーポイントの定義を３次元までに拡張する。本発明におけるキャプチャーポイントは、重心が移動する速度に比例した距離とその方向に重心から移動した点である。

設定部２４０は、入力部２７０から転送されたモーションデータを格納し、予め格納されたモーション情報に基づいて複数の自由度のうち少なくとも一つの自由度に対する目標姿勢を設定する。ここで、モーション情報は、ダンス、歩行などを行うための少なくとも一つの姿勢からなる。

設定部２４０は、力／トルク検出部２１０で検出された、各足に加えられる荷重に基づいて着地したか否かを判断するが、荷重の検出された脚を支持（Ｓｕｐｐｏｒｔ）状態と判断し、荷重の検出されない脚をスイング（Ｓｗｉｎｇ）状態と判断する。

設定部２４０は、複数の関節部の角度、上体のポーズ角度、足の着地状態、足の位置に基づいて現在の姿勢を判断し、現在の姿勢とモーション情報とを比較して次の姿勢を判断し、次の姿勢を行うための目標姿勢を設定する。

設定部２４０は、有限状態機械（ＦＳＭ）に基づいて歩行する時における各脚の着地状態と予め格納された有限状態機械（ＦＳＭ）の状態に基づいて目標姿勢を設定することも可能である。

ここで、キャプチャーポイント（ＣＰ）は、重心（ＣＯＧ）でもよく、また重心が地面に投影された投影点（ＣＯＧ’）でもよい。さらに、重心または投影点が移動する速度を反映して速度に対応する距離だけ移動した点の位置で表示することも可能である。

さらに、キャプチャーポイントは、重心から地面まで延長された垂直線上に位置したいずれか一つの仮想点でも表示することができる。

設定部２４０は、ロボットが転倒しないように、両足の支持領域の内部を重力方向に通る線上の一点を目標キャプチャーポイントとして設定し、この目標キャプチャーポイントをロボットの並進運動自由度に対する目標姿勢として設定することも可能である。

さらに、目標ポーズ角度は、ロボットの直立姿勢のために、ロボットの上体が重力方向と平行するように設定し、これをロボットの回転運動自由度に対する目標姿勢として設定することも可能である。

ロボット１００が有限状態機械（ＦＳＭ）に基づいて歩行する際における、ロボットの有限状態機械（ＦＳＭ）の状態遷移について説明する。

図４は、有限状態機械（ＦＳＭ：Ｆｉｎｉｔｅ Ｓｔａｔｅ Ｍａｃｈｉｎｅ）に基く二つの脚の状態変化順序図である。

ＦＳＭが７つの状態を有すると設定したので、ＤＳ状態→Ｗ１状態→Ｗ２状態→Ｗ３状態→Ｗ４状態→Ｗ２状態→Ｗ３状態→Ｗ４状態→Ｗ２状態→…で循環しり、停止命令が入力されると、Ｗ４状態から停止準備動作であるＷ２’状態→Ｗ３’状態を経てＤＳ状態に遷移する。

ＦＳＭの各状態におけるｘ方向は、支持足の長さ方向であって、前方を正の方向とし、ｙ方向は、ｘ方向に対して上から見て９０度反時計方向とする。

また、支持足は、ロボットの姿勢を維持させる足で、地面を踏んでいる足を指し、スイング足は、移動のために持ち上げた足を指す。

ＤＳ状態はロボット１００の二つの脚が地面に接している状態であり、Ｗ１状態は外見上、ＤＳ状態と違いはないが、歩行が始まると、重心が片方の脚に移動し始める。

例えば、左脚１６０Ｌに重心を移動させる場合を説明すれば、Ｗ２状態では、右脚１６０Ｒを持ち上げながら左脚１６０Ｌで体を支えて立っている姿勢となり、Ｗ３状態では、体を進行方向へ移動させながら右脚１６０Ｒを下げて右足１７０Ｒを地面に着地させる姿勢となる。

Ｗ４状態では、右足１７０Ｒが地面に着いた後、重心を左脚１６０Ｌに移動させる姿勢となり、 再びＷ２状態に遷移して脚の方向のみが変わり、停止命令が入るまでＷ２→Ｗ３→Ｗ４→Ｗ２→…の循環が続けられる。

停止命令が入った場合には、Ｗ４状態からＷ２’状態に遷移するが、Ｗ２’状態はＷ２状態と類似に左脚１６０Ｌを持ち上げるが、前方に進むわけではないので、目標キャプチャーポイントのｘ成分が現在の支持足の中心に来るようにする。

Ｗ３’状態は、Ｗ３状態と類似に、左側足１７０Ｌを着地するが、前進するわけではないので、右足１７０Ｒと並んだ位置に着地する。

このように、７個の各ＦＳＭ状態は一定の順序で遷移し、所定の遷移条件を充足する時に次の状態に遷移する。

次に、ＦＳＭの状態とキャプチャーポイントについて説明する。まず、ＤＳ状態は、ロボット１００の二つの足１７０Ｒ，１７０Ｌが地面に着いて停止した状態を示し、この際の目標キャプチャーポイントはロボットの二つの足からなる支持領域（ｓｕｐｐｏｒｔ ｐｏｌｙｇｏｎ）の中心に置かれる。そして、歩行命令が下ると、Ｗ１状態に遷移する。

Ｗ１状態は、目標キャプチャーポイントが、ロボット１００の二つの足１７０Ｒ，１７０Ｌのいずれかの選択された支持足に移動する状態であり、実際にキャプチャーポイントが支持足の足幅以内の安定領域内に進入すると、Ｗ２状態に遷移する。

Ｗ２状態は、ロボット１００がスイング足を持ち上げる動作を行う状態であり、Ｗ２状態における目標キャプチャーポイントのｘ成分は、支持足の中心から前の部分に時間によって前進する軌跡に設定され、ｙ成分は支持足の中心線に来るように設定される。

さらに、スイング脚を持ち上げる動作は、重力補償により制御される。Ｗ２状態において、現在キャプチャーポイントのｘ成分がしきい値を越え、スイング足の高さがしきい値を越えると、Ｗ３状態に遷移する。

Ｗ３状態は、ロボット１００が着地のために膝を伸ばしながらスイング足を着地させる動作を示し、この際の目標キャプチャーポイントのｘ成分は、支持足の前の部分を越えてスイング足が着地すべき位置まで時間によって増加する軌跡と設定され、ｙ成分は、スイング足が着地すべき位置へ時間によって移動するものと設定される。

スイング足が地面に着いて、力／トルク検出部にしきい値以上のｚ成分が感知されると、Ｗ４状態に遷移する。

Ｗ４状態は、二つの足が地面に着いた状態であって、最後に着地した足を支持足とする。この際、目標キャプチャーポイントのｘ、ｙ成分は、以前状態の目標キャプチャーポイント位置から新しい支持足の中心に短時間で連続的に移動する軌跡と設定される。現在キャプチャーポイントが支持足内に入ってくると、停止命令がない場合にはＷ２状態に遷移し、停止命令が下る場合にはＷ２’状態に遷移する。

Ｗ２’状態は、Ｗ２状態と類似した動作、すなわち、スイング足を持ち上げる動作を表しているが、前進しないで停止したため、目標キャプチャーポイントのｘ成分は支持足の中心に固定させる。スイング足の高さがしきい値以上に到達すると、Ｗ３’状態に遷移する。

Ｗ３’状態はＷ３状態と類似した動作、すなわち、スイング足の着地のために膝を伸ばしながら足を下ろす動作を示すが、前進しないで停止するため、目標キャプチャーポイントのｘ成分を支持足の前方に送らず支持足の位置に置き、目標キャプチャーポイントのｙ成分は反対側の足に向けて送るように設定する。スイング足が地面に着いて力／トルク検出部のｚ成分がしきい値を越えると、ＤＳ状態に遷移する。

均衡制御部２５０は、ポーズ角度及び複数の関節部の角度に基づいて少なくとも一つの自由度に対して現在姿勢を獲得し、現在姿勢と目標姿勢とを比べて姿勢エラーを算出し、姿勢エラーに基づいて少なくとも一つの自由度に対して補償力を算出し、補償力に基づいて少なくとも一つの自由度に対する目標トルクを算出する。

均衡制御部２５０は、上体のポーズ角度及び複数の関節部の角度に基づいて重心を獲得し、重心をロボットの位置として現在姿勢を獲得したり、上体のポーズ角度及び複数の関節部の角度に基づいて重心の位置と速度を獲得し、重心の位置と速度に基づいて現在キャプチャーポイントを獲得した後、その獲得された現在キャプチャーポイントをロボットの位置として現在姿勢を獲得したり、または上体のポーズ角度及び複数の関節部の角度に基づいてヒップや上体の参照点の位置を獲得し、その獲得された参照点をロボットの位置として現在姿勢を獲得する。

キャプチャーポイントをロボットの現在位置として現在姿勢を獲得し、これを用いて均衡を制御する場合における均衡制御部について説明する。

均衡制御部２５０は、重心（ＣＯＧ：Ｃｅｎｔｅｒ Ｏｆ Ｇｒａｖｉｔｙ）に基づいて現在キャプチャーポイントを獲得し、設定部２４０から転送された目標キャプチャーポイントと現在キャプチャーポイントとを比較してキャプチャーポイントエラーを算出する。

また、均衡制御部２５０は、ポーズ検出部２２０から転送された現在のポーズ角度と設定部２４０から転送された目標ポーズ角度とを比較してポーズ角度エラーを算出し、ポーズ角度エラーとキャプチャーポイントエラーを用いてトルクを算出する。

なお、均衡制御部２５０は、ポーズ検出部２２０から転送された現在のポーズ角度と各関節部１３１，１４３，１４４，１６３，１６４，１６５の回転角度に基づいて現在ヒップ高さを算出し、算出された現在ヒップ高さと目標ヒップ高さとを比較してヒップ高さエラーを算出し、ポーズ角度エラーとキャプチャーポイントエラーにヒップ高さエラーを反映してトルクを算出することも可能である。

ここで、現在の重心の位置、重心の速度、キャプチャーポイント（Ｃａｐｔｕｒｅ Ｐｏｉｎｔ）、二つの足の位置と方向、ヒップ高さの計算には、順運動学（Ｆｏｒｗａｒｄ Ｋｉｎｅｍａｔｉｃｓ）を用いることが可能である。

サーボ制御部２６０は、均衡制御部２５０から転送されたトルクに到達するように各関節部１６３，１６４，１６５のトルクサーボを制御する。

サーボ制御部２６０は、各関節部のトルクを、算出されたトルクと比較して、算出されたトルクに近接するようにモーターの電流を調節するものであり、トルク算出部２５５で算出されたトルクを生成するために、算出されたトルクに対応するＰＷＭを制御して、各関節部１６３，１６４，１６５に設けられた各軸のモーター（図示せず）にそれぞれ出力する。

入力部２７０は、使用者からダンスや歩行を行うための少なくとも一つの姿勢からなるモーション情報を入力されて、入力されたモーション情報を設定部２４０に転送する。

ここで、モーション情報は、順次に格納された複数の姿勢からなる。すなわち、ロボットは複数の姿勢を連続して行うことによって、ダンスや歩行などのモーションを行う。

ここで、一つの姿勢は、胴体、腕、脚に設けられたリンク１４１，１４２，１６１，１６２の位置情報や、胴体、腕、脚に設けられた各関節部１３１，１４３，１４４，１６３，１６４，１６５の角度情報を有する。すなわち、ロボットの胴体、腕、脚は特定の形態を取ることによって、一つの姿勢を作り出す。

図５を参照して、３次元の並進運動自由度のうち２次元水平方向に対してはキャプチャーポイントに基づき、残りの垂直方向に対してはヒップ高さを参照点として均衡を制御する均衡制御部２５０について具体的に説明する。

均衡制御部２５０は、獲得部２５１、エラー算出部２５２、補償部２５３、配分部２５４及びトルク算出部２５５を含む。

獲得部２５１は、ポーズ検出部２２０で検出された上体のポーズ角度及びロボットの現在の姿勢に対応する各関節部１３１，１４３，１４４，１６３，１６４，１６５の回転角度に基づいてロボットの重心を獲得する重心獲得部２５１ａと、各関節部の回転角度に基づいて現在キャプチャーポイントを獲得するキャプチャーポイント獲得部２５１ｂと、重心獲得部２５１ａから転送された重心（ＣＯＧ：Ｃｅｎｔｅｒ Ｏｆ Ｇｒａｖｉｔｙ）に基づいて現在ヒップ高さを獲得するヒップ高さ獲得部２５１ｃと、を含む。

ここで、キャプチャーポイントは、水平成分の座標値であるｘ、ｙ軸の座標値を有し、ヒップ高さは、垂直成分の座標値であるｚ軸の座標値を有する。

すなわち、獲得部２５１は重心とヒップ高さに基づいて、ロボットの位置を表すｘ、ｙ、ｚ軸の座標値を獲得する。

ここで、現在キャプチャーポイントを獲得する過程を、図６を参照して説明する。

獲得部２５１は、ロボット１００の二つの足１７０Ｌ，１７０Ｒの状態、重心（ＣＯＧ）の位置及び速度を算出し、重心の位置と速度を用いてロボットの現在キャプチャーポイント（ＣＰｃ）を算出する。

さらに具体的に説明すると、各関節部１３１，１４３，１４４，１６３，１６４，１６５の角度、予め格納された骨部（すなわち、リンク：１４１，１４２，１６１，１６２）のサイズと重さ、ポーズ角度を順運動学（Ｆｏｒｗａｒｄ Ｋｉｎｅｍａｔｉｃｓ）に適用して重心の位置と速度、ヒップ高さ、二つの足の位置、二つの足の方向を計算する。

そして、重心の速度と位置を用いて、現在キャプチャーポイント（ＣＰｃ）を獲得する。

キャプチャーポイント（ＣＰ：Ｃａｐｔｕｒｅ Ｐｏｉｎｔ）は、ロボットの現在重心の位置と速度に照らして、次の歩行動作を行う時にロボットが転倒せずに直立できる位置のことなので、通常的には水平面（すなわち、地面）上の点であるが、前述したように本発明では、これを重心の垂直方向に対する位置と速度を用いて、３次元に一般化して用いることもできる。

かかるロボットの現在の位置におけるキャプチャーポイントは、下記の式（１）に基づいて獲得することが可能である。

ＣＰ＝ｄＣＯＧ＋ｗ*ｖＣＯＧ （１）
ここで、ＣＰはキャプチャーポイントであり、ｄＣＯＧは重心（ＣＯＧ）から地面まで延長した垂直線上に位置したいずれか一つの仮想点の位置であり、ｖＣＯＧは仮想点の速度である。また、ｗはｗ＝√（ｌ／ｇ）で、ここでｌは地面から重心までの高さ、ｇは重力加速度である。

エラー算出部２５２は、設定部２４０から転送された目標のキャプチャーポイント（ＣＰ_Ｄ）と現在キャプチャーポイント（ＣＰ_Ｃ）とを比較してキャプチャーポイントエラー（ＣＰ_Ｅ）を算出するキャプチャーポイントエラー算出部２５２ａと、設定部２４０で設定された目標ヒップ高さ（ＨＬ_Ｄ）と現在ヒップ高さ（ＨＬ_Ｃ）とを比較してヒップ高さエラー（ＨＬ_Ｅ）を算出するヒップ高さのエラー算出部２５２ｂと、ポーズ検出部２２０から転送された現在のポーズ角度と設定部２４０から転送された目標ポーズ角度とを比較してポーズ角度エラーを算出するポーズ角度エラー算出部２５２ｃと、を含む。

補償部２５３は、ロボットの直立状態を維持させるために必要な補償力と補償モーメントを算出する。

かかる補償部２５３は、キャプチャーポイントエラーとヒップ高さエラーに基づいて補償力を算出する補償力算出部２５３ａと、ポーズ角度エラーに基づいて補償モーメントを算出する補償モーメント算出部２５３ｂとを含む。

すなわち、補償力算出部２５３ａは、キャプチャーポイントエラーからｘ，ｙ方向に補償されるべき補償力を算出し、ヒップ高さエラーからｚ方向に補償されるべき補償力を算出し、補償モーメント算出部２５３ｂはポーズ角度エラーからｘ，ｙ，ｚ方向の補償モーメントを算出する。

すなわち、ロボットの均衡を取るために補償しなければならないｘ，ｙ，ｚ方向の補償力と、ｘ、ｙ、ｚ方向の補償モーメントを算出することによって、ロボットは直立姿勢を維持できる。

補償力の算出は下記の通りである。

ＣＰ＝ｄＣＯＧ＋ｗ*ｖＣＯＧ （１）
重心（ＣＯＧ）の水平成分座標は（ｘ，ｙ）、重心の速度は水平成分座標を微分した（ｘ’，ｙ’）なので、ＣＰ＝（ｘ，ｙ）＋Ｗ（ｘ’，ｙ’）＝（ｘ＋ｗｘ’，ｙ＋ｗｙ’）である。

キャプチャーポイントとヒップ高さを反映した３軸座標の姿勢エラー（ｅ）は、次の通りである。

ｅ＝（［（ｘ^＊，ｙ^＊）−（ｘ＋ｗｘ’，ｙ＋ｗｙ’）］，ｚ^＊−ｚ）
＝（ｘ^＊−（ｘ＋ｗｘ’），ｙ^＊−（ｙ＋ｗｙ’），ｚ^＊−ｚ） （２）
ここで、（ｘ*，ｙ*）は目標キャプチャーポイントのｘ，ｙ成分であり、ＣＰは現在キャプチャーポイント、ｚ*は目標ヒップ高さ、ｚは現在ヒップ高さである。

３軸座標の姿勢エラー（ｅ）を用いた補償力ｆは下記の通りである。

ｆ＝ｋ_ｐｅ （３）
ここで、ｋ_ｐは力ゲイン（ｆｏｒｃｅ ｇａｉｎ）であり、Ｐ（Ｐｒｏｐｏｒｔｉｏｎａｌ）制御を用いる。

補償モーメント算出部２５３ｂは、上体のロール方向（ｒｏｌｌ，Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ，Ｙ軸回転）、ヨー方向（ｙａｗ，Ｚ軸回転）の３軸に対して仮想のばね（常数ｋ_１）があると仮定したとき、エラー算出部から算出されたエラーｅを用いて、ロボットの均衡を取るために補償すべきモーメントＭ（Ｍ_ｘ，Ｍ_ｙ，Ｍ_ｚ）を下記のように算出する。

Ｍ＝Ｋ1＊ｅ （４）
配分部２５４は補償力を二つの脚１６０Ｌ，１６０Ｒに配分する。配分部２５４は、ロボット１００の重心に対する地面の投影点（ＣＰ）とロボット１００の二つの足１７０Ｌ，１７０Ｒ間の距離比率を用いて、重心の投影点に相対的に近い脚に、より多くの力を配分する。

トルク算出部２５５は、力補償とモーメント補償に基づいて、各関節部１６３，１６４，１６５に伝えられるべきトルクを算出する。ここで、トルクは補償力を発生させるためのモーター（図示せず）の回転力である。

このとき、配分部２５４で配分された力に基づいて、二つの脚に印加されるトルクを算出する。

Ｍ_Ｌ＝Ｗ_Ｌ＊Ｍ，Ｍ_Ｒ＝Ｗ_Ｒ＊Ｍ，
Ｗ_Ｌ＝ｄ_Ｒ／（ｄ_Ｒ＋ｄ_Ｌ），Ｗ_Ｒ＝１−Ｗ_Ｌ
ｄ_Ｌ，ｄ_Ｒは重心の投影点と両足間の距離である。

なお、重力補償を反映することもできる。これについて、より詳細に説明する。

トルク算出部２５５は、仮想重力設定部２５５ａ、重力補償トルク算出部２５５ｂ、目標トルク算出部２５５ｃを含む。

仮想重力設定部２５５ａは、ロボットに対して予め格納されたＦＳＭの現在状態及び補償力算出部２５３ａで算出された補償力の大きさを用いて、ロボットの各関節部１６３，１６４，１６５に必要な仮想重力の大きさを設定するため、補償力から設定された仮想重力は式（５）に示した通りである。

ｇ_ｆ＝ｆ／ｍ （５）
ここで、ｇ_ｆは仮想重力であり、ｆは補償力算出部２５３ａで算出した補償力であり、ｍはロボットの質量である。

重力補償トルク算出部２５５ｂは、仮想重力設定部で設定された仮想重力と実際の重力を補償するために各関節部に必要な重力補償トルクを算出するため、仮想重力加速度と実際の重力加速度との和、各関節部の角度、各リンクの重さ及び各リンクにおける重心の位置を用いて重力補償トルクを算出することができる。

さらに、重力補償トルクを計算する際、二つの脚１６０Ｌ，１６０Ｒに配分された補償力を考慮して各脚１６０Ｌ，１６０Ｒの関節部１６３，１６４，１６５に必要な重力補償トルクを算出する。

目標トルク算出部２５５ｃは、重力補償トルク算出部２５５ｂで算出された重力補償トルクと補償モーメント算出部２５３ｂで算出された補償モーメントに対するトルクとを合算して、ロボットの各関節部１６３，１６４，１６５に必要な目標トルクを算出する。

この際、目標トルク算出部２５５ｃは、左右脚１６０Ｌ，１６０Ｒに配分された補償力を考慮して、左右脚１６０Ｌ，１６０Ｒの各関節部１６３，１６４，１６５に補償力を発生させるための目標トルクを算出する。

すなわち、重心と両足間の割合に基づいて、重心とより近い足が属した脚がより大きい目標トルクを有するようにする。

トルク算出部２５５はジャコビアンを用いて両脚の複数の関節部に印加されるトルクを下記のようにそれぞれ算出することも可能である。

また、目標トルク算出部２５５は補償モーメント算出部で算出された補償モーメントＭと重力補償トルク算出部で計算された重力補償トルクＧ（ｑ）から、姿勢制御のための複数の関節部に印加されるトルクτを両脚１１０Ｌ，１１０Ｒに対して下記のようにそれぞれ算出することも可能である。

式（７）において、関節トルクτは６−ベクトルである。式（７）において、Ｊは足１７０Ｒ，１７０Ｌをロボット１００のベースリンクと見なし、重心（ＣＯＧ）を上体上に存在するエンドエフェクタ（ｅｎｄ ｅｆｆｅｃｔｏｒ）と見なしたときの６−ｂｙ−６ジャコビアン（ｊａｃｏｂｉａｎ）行列である。

ジャコビアンは関節空間の速度を作業空間の速度に変換できる行列であり、順運動学計算（Ｆｏｒｗａｒｄ Ｋｉｎｅｍａｔｉｃｓ）から誘導される。デカルト空間（Ｃａｒｔｅｓｉａｎ Ｓｐａｃｅ）上のモーメントを関節トルクに変換するためにジャコビアンを用いた。

式（７）において、重力補償トルクＧ（ｑ）は各脚１６０Ｒ，１６０Ｌに属した全関節の角度ｑと、各脚１６０Ｒ，１６０Ｌに属した全リンクの質量及び重心の位置と上体の総質量及び重心の位置から計算することができる。

ｑ_Ｌは左脚１６０Ｌに属した全関節の角度、ｑ_Ｒは右脚１６０Ｒに属した全関節２１０，２２０，２３０の角度である。関節トルクは、左脚１６０Ｌのトルク

と右脚１１０Ｒのトルク

とを含む。

図７は一実施例に係るロボットの均衡制御方法を示すフローチャートである。ロボットは、上体のポーズ角度及び複数の関節部の角度を検出し（３０１）、ポーズ角度及び複数の関節部の角度に基づいて現在姿勢を獲得する（３０２）。ロボットの現在姿勢は、上述したように、並進運動自由度に対するロボットの現在位置と回転運動自由度に対するロボットの現在ポーズ角度により表される。

次に、ロボットは、複数の並進運動自由度のうち少なくとも一つの並進運動自由度に基づいた姿勢と予め設定された目標姿勢とを比較して、姿勢エラーを算出する（３０３）。

このときに検出されたポーズ角度及び複数の関節部の角度に基づいて、現在重心の位置を少なくとも一つの並進運動自由度に対して検出し、少なくとも一つの並進運動自由度に対して検出された現在重心と予め設定された目標重心とを比較して、重心エラーを算出する。すなわち、少なくとも一つの自由度に対する重心エラーを算出する。

ここで、重心エラーは少なくとも一つの自由度に対する姿勢エラーとして用いられる。

また、検出されたポーズ角度及び複数の関節部の角度に基づいて、現在重心の位置と速度を少なくとも一つの自由度に対して検出し、その検出された現在重心の位置と速度に基づいて、現在キャプチャーポイントを少なくとも一つの自由度に対して獲得し、前記キャプチャーポイントと予め設定されたキャプチャーポイントとを比較して、キャプチャーポイントエラーを少なくとも一つの自由度に対して算出し、その算出されたキャプチャーポイントエラーを少なくとも一つの自由度の姿勢エラーとして用いることも可能である。

なお、検出されたポーズ角度及び複数の関節部の角度に基づいて、ヒップまたは上体のいずれか一点の位置を少なくとも一つの自由度に対する現在参照点として獲得し、現在参照点と予め設定された目標参照点の位置とを比較して、参照点位置エラーを少なくとも一つの自由度に対して算出し、参照点位置エラーを少なくとも一つの自由度の姿勢エラーとして用いることも可能である。

さらに、残りの並進運動自由度における姿勢エラーの算出は、少なくとも一つの並進運動自由度における姿勢エラーの算出と同一かまたは相異してもよい。

次に、算出された姿勢エラーに基づいて、少なくとも一つの並進運動自由度に対する補償力を算出する（３０４）。

続いて、ロボットは少なくとも一つの並進運動自由度に対する補償力に基づいて、少なくとも一つの並進運動自由度に対するトルクを算出する（３０５）。この際、複数の関節部の並進運動自由度に対するトルクを算出する。

さらに、残りの並進運動自由度に対しても、補償力に基づいて複数の関節部の目標トルクを算出し、このようにすべての並進運動自由度に対して算出された目標トルクと予め算出されたトルクとを合算して最終の目標トルクを算出し、その算出された最終の目標トルクを複数の関節部に出力することも可能である。

ここで予め算出されたトルクは、回転運動自由度に対して、目標ポーズ角度と現在ポーズ角度のエラーを補償するための補償モーメントを式（６）のジャコビアンなどの方式で算出したトルクでもよく、さらに、安定性のために各関節部に印加するダンピング（ｄａｍｐｉｎｇ）トルクを含むトルクを合算して算出したものでもよい。

次に、ロボットは、最終の目標トルクを複数の関節部に出力して（３０６）、ロボットの均衡を制御する。

図８は、キャプチャーポイントに基づいて、均衡を制御するロボットの均衡制御方法を示した詳細フローチャートである。

ロボットは、入力部２７０を介して入力された使用者の命令とロボットの姿勢に基づいて、各関節部１３１，１４３，１４４，１６３，１６４，１６５に設けられた複数のモーター（図示せず）を駆動させることによって、モーションを行う。

ロボットは、使用者によって入力されたモーション情報に基づいてダンスや歩行を行う時に、複数の関節部の角度、上体のポーズ角度に基づいて現在の姿勢を判断し、現入力部２７０を介して入力されたモーション情報と現在の姿勢とを比較して次の姿勢を判断し、その次の姿勢を具体的に表すために、目標キャプチャーポイント、目標ポーズ角度、目標ヒップ高さを設定する（３１１）。

なお、ロボットは、有限状態機械に基づいて歩行する時に、予め格納された有限状態機械に基づいて現在の歩行状態を判断し、判断された現在の歩行状態に基づいて目標キャプチャーポイント、目標ポーズ角度、目標ヒップ高さを設定することも可能である。

この際、ロボットは二つの脚に作用する力／トルクを用いて二つの足の位置及び方向を確認し、二つの足の位置及び予め格納されたＦＳＭの状態に基づいて現在の歩行状態を判断し、判断された現在の歩行状態に基づいて目標キャプチャーポイント、目標ポーズ角度、目標ヒップ高さを設定する。

ロボットは、現在の姿勢と次の姿勢である目標姿勢との間のエラーを算出し、算出されたエラーを補償することによって、均衡を維持しながら次の姿勢を行う。これについて、さらに具体的に説明する。

次に、ロボットは、モーションを行いながら力／トルク検出部２１０、ポーズ検出部２２０、角度検出部２３０を介して二つの脚に印加される力／トルク、上体のポーズ角度を検出し、また複数の関節部１３１，１４３，１４４，１６３，１６４，１６５などの角度を検出する（３１２）。

続いて、ロボットは、力／トルク検出部２１０、ポーズ検出部２２０及び角度検出部２３０から検出された上体のポーズ角度及び各関節部１３１，１４３，１４４，１６３，１６４，１６５の角度、足の位置及び方向に基づいて重心を獲得する（３１３）。

その後、ロボットは、重心に基づいて現在キャプチャーポイントを獲得し、ポーズ角度、複数の関節部の角度に基づいて現在ヒップ高さを獲得し、ポーズ検出部２２０で検出されたポーズに対してヨー、ロール、ピッチの３軸方向のポーズ角度を獲得する（３１４）。

この際、順運動学を用いてロボットの現在の重心の位置、速度及びヒップ高さを獲得する。

そして、重心の位置及び速度を用いて、下記の式（１）によってロボットの現在キャプチャーポイントの位置を獲得する。

ＣＰ＝ｄＣＯＧ＋ｗ*ｖＣＯＧ （１）
ここで、ＣＰはキャプチャーポイントであり、ｄＣＯＧは重心（ＣＯＧ）または重心が地面に投影された投影点の位置であり、ｖＣＯＧは重心または投影点の速度であり、ｗはｗ＝√（ｌ／ｇ）であって、ここで、ｌは地面からの重心までの高さであり、ｇは重力加速度である。或いは、ｗが正の値でもよい。

次に、ロボットは、現在キャプチャーポイントと目標キャプチャーポイントとを比較してキャプチャーポイントエラーを算出し、現在ヒップ高さと目標ヒップ高さとを比較してヒップ高さエラーを算出し、現在ポーズ角度と目標ポーズ角度とを比較してポーズ角度エラーを算出する（３１５）。

続いて、ロボットは、キャプチャーポイントエラーから水平方向であるｘ、ｙ軸の座標値を獲得し、該ｘ、ｙ方向の補償力を算出する。

また、ヒップ高さエラーから垂直方向であるｚ軸方向の座標値を獲得し、該ｚ方向の補償力を算出する。

補償力の算出は次の通りである。

ＣＰ＝ｄＣＯＧ＋ｗ*ｖＣＯＧ （１）
重心（ＣＯＧ）の水平成分座標は（ｘ，ｙ）であり、重心の速度は、水平成分座標を微分した（ｘ’，ｙ’）であるため、ＣＰ＝（ｘ，ｙ）＋ｗ（ｘ’，ｙ’）＝（ｘ＋ｗｘ’，ｙ＋ｗｙ’）である。

キャプチャーポイントとヒップ高さを反映した３軸座標のエラー（ｅ）は次の通りである。

ｅ＝（［（ｘ*，ｙ*）−（ｘ＋ｗｘ’，ｙ＋ｗｙ’）］，ｚ*−ｚ）
＝（ｘ*−（ｘ＋ｗｘ’），ｙ*−（ｙ＋ｗｙ’），ｚ*−ｚ） （２）
ここで、（ｘ*，ｙ*）は目標キャプチャーポイントのｘ、ｙ成分であり、ＣＰは現在キャプチャーポイントであり、ｚ*は目標ヒップ高さであり、ｚは現在ヒップ高さである。

３軸座標のエラー（ｅ）を用いた補償力ｆは次の通りである。

ｆ＝ｋ_ｐｅ （３）
ここで、ｋ_ｐは力ゲイン（ｆｏｒｃｅ ｇａｉｎ）であり、Ｐ（Ｐｒｏｐｏｒｔｉｏｎａｌ）制御を用いる。

次に、ロボットは、重心に対する地面の投影点（ＣＰ）とロボット１００の二つの足１７０Ｌ、１７０Ｒとの間の距離比率を用いて、重心の投影点に相対的に近い脚に、より多くの力を配分する。

次に、ロボットは、予め設定されたＦＳＭの現在の状態及び補償力算出部２５３ａで算出された補償力の大きさを用いて、ロボットの各関節部１６３，１６４，１６５に必要な仮想重力の大きさを設定する。

ここで、該補償力により設定された仮想重力は式（５）に示す通りである。

ｇ_ｆ＝ｆ／ｍ （５）
ここで、ｇ_ｆは仮想重力であり、ｆは補償力算出部２５３ａで算出した補償力であり、ｍはロボットの質量である。

次に、ロボットはポーズ角度エラーに基づいて、ヨー、ロール、ピッチ方向の補償モーメントを算出する（３１５）。

なお、補償力の算出と補償モーメントの算出とは前後関係を有するものでなく、両者を互いに取り替えてもよい。

次に、ロボットは仮想重力と実際の重力を補償するために、各関節部１６３，１６４，１６５に必要な重力補償トルクを算出するが、該重力補償トルクは仮想重力加速度と実際の重力加速度との和、各関節部の角度、各リンクの重さ及び各リンクにおける重心の位置を用いて算出する。

この際、二つの脚１６０Ｌ，１６０Ｒに配分された補償力を考慮して、各脚１６０Ｌ，１６０Ｒの関節部１６３，１６４，１６５に必要な重力補償トルクを算出する。

次に、ロボットは、重力補償トルクと補償モーメントに対するトルクを合算してロボットの各関節部１６３，１６４，１６５に必要な目標トルクを算出する（３１７）。

この際、ロボットは左右脚１６０Ｌ，１６０Ｒに配分された補償力を考慮して、左右脚１６０Ｌ，１６０Ｒの各関節部１６３，１６４，１６５に補償力を発生させるための目標トルクを算出する。

次に、ロボットは左右脚１６０Ｌ，１６０Ｒの各関節部１６３，１６４，１６５に対して算出された目標トルクを各関節部に出力することによって、均衡の取れたモーションを行う（３１８）。

本発明の一実施例は、複数の自由度を有するロボットの均衡制御に係り、ポイント及びヒップ高さを用いて複数の自由度に対するトルクをそれぞれ算出することができる。

したがって、ロボットはモーション中に均衡を維持することによって、柔軟で安定したモーションを行うことができる。

図９は、他の実施例に係るロボットの均衡制御装置を示す制御構成図であって、一実施例と異なる均衡制御部２５０のみを詳細に示した図面である。

他の実施例に係るロボットの均衡制御装置の均衡制御部２５０は、第１均衡制御部２５０ａと第２均衡制御部２５０ｂとを含む。

第１均衡制御部２５０ａは、複数の自由度のうち少なくとも一つの自由度に対する第１トルクを算出する。ここで、少なくとも一つの自由度に対する第１トルクを算出する第１均衡制御部２５０ａは、一実施例における均衡制御部２５０と同一なので説明を省略する。

第２均衡制御部２５０ｂは、複数の自由度のうち残りの自由度に対する第２トルクを算出する。

そして、第１均衡制御部２５０ａは、少なくとも一つの自由度に対する第１トルクと残りの自由度に対する第２トルクとを合算して目標トルクを算出し、その算出された目標トルクをサーボ制御部２６０に伝送する。

ここで、第１トルクと第２トルクの合算は、第２均衡制御部２５０ｂまたはサーボ制御部２６０で行われてもよい。

さらに、第１均衡制御部２５０ａの獲得部２５１は、キャプチャーポイントの獲得時、少なくとも一つの自由度に対応する位置及び速度だけを獲得する。

例えば、少なくとも一つの自由度がＸ軸方向に運動するＸ軸方向の並進運動自由度及びＸ軸方向に対するロール回転運動自由度で、残りの自由度がＹ軸方向に運動するＹ軸方向の並進運動自由度及びＹ軸方向に対するピッチ回転運動自由度であれば、第１均衡制御部２５０ａの獲得部２５１はＸ軸方向に対する位置及び速度だけを獲得し、その獲得されたＸ軸方向の位置と速度を用いてキャプチャーポイントを獲得する。

第２均衡制御部２５０ｂに対して具体的に説明する。第２均衡制御部２５０ｂは、目標値設定部２５６、逆運動学演算部２５７及び第２トルク算出部２５８を含む。

残りの自由度がＹ軸方向に運動するＹ軸方向の並進運動自由度及びＹ軸方向に対するピッチ回転運動自由度である場合を例に挙げて説明する。

目標値設定部２５６は、Ｙ軸方向の並進運動自由度に対する目標位置を設定し、Ｙ軸方向のピッチ回転運動自由度に対する目標角度を設定する。

逆運動学演算部２５７は、目標値設定部２５６で設定された目標位置及び目標角度を満足する逆運動学の解を演算する。

第２トルク算出部２５８は、逆運動学の解を用いて複数の関節部の目標角度を算出し、その算出された複数の関節部の目標角度を追従するための第２トルクをそれぞれ算出する。さらに、各関節部１６３，１６４，１６５は、図２に示すように、各軸に対する並進運動自由度及び回転運動自由度を行う複数の関節を含む。

すなわち、第１均衡制御部２５０ａは、Ｘ軸方向に対する並進運動自由度及び回転運動自由度が行われるように、股関節部１６３のロール方向の回転関節１６３ｃ及び足首関節部のロール方向の回転関節１６５ｂに印加されるトルクを算出し、Ｚ軸方向に対する並進運動自由度及び回転運動自由度が行われるように、股関節部１６３のヨー方向の回転関節１６３ａに印加されるトルクを算出する。

そして、第２均衡制御部２５０ｂは、Ｙ軸方向に対する並進運動自由度及び回転運動自由度が行われるように、股関節部１６３のピッチ方向の回転関節１６３ｂ、膝関節部１６４のピッチ方向の回転関節１６４ａ及び足首関節部１６５のピッチ方向の回転関節１６５ａに印加されるトルクを算出する。

図１０は、本発明の他の実施例に係るロボットの均衡制御方法を示すフローチャートである。少なくとも一つの自由度がＸ軸方向に運動するＸ軸方向の並進運動自由度及びＸ軸方向に対するロール回転運動自由度であり、残りの自由度がＹ軸方向に運動するＹ軸方向の並進運動自由度及びＹ軸方向に対するピッチ回転運動自由度であるときを例に挙げて説明する。

まず、ロボットは、動作中両脚に作用する力／トルクを検出して、両足の位置及び方向を確認し、両足の位置及び方向に基づいて、目標キャプチャーポイント、目標ポーズ角度、目標ヒップ高さを設定する（４０１）。

この際、目標キャプチャーポイントは、少なくとも一つの自由度に基づいた位置情報を有する。

ロボットは、現在姿勢と次の姿勢である目標姿勢間のエラーを算出し、その算出されたエラーを補償することにより、均衡を保持しながら次の姿勢を行う。これについてさらに具体的に説明する。

次に、ロボットは、モーションを行いながら、力／トルク検出部２１０、ポーズ検出部２２０、角度検出部２３０を通じて両脚に印加される力／トルク、上体のポーズ角度を検出し、また複数の関節部１３１，１４３，１４４，１６３，１６４，１６５などの角度を検出する（４０２）。

次に、ロボットは、力／トルク検出部２１０、ポーズ検出部２２０及び角度検出部２３０を通じて検出された上体のポーズ角度及び各関節部１３１，１４３，１４４，１６３，１６４，１６５の角度、足の位置及び方向に基づいて、重心を獲得する（４０３）。

次に、ロボットは、重心に基づいて、現在キャプチャーポイント、ポーズ角度、複数の関節部の角度に基づいて、現在ヒップ高さを獲得し、ポーズ検出部２２０で検出されたポーズのヨー、ロール、ピッチの３軸方向のポーズ角度を獲得する（４０４）。

次に、ロボットは、重心の位置及び速度を用いて、現在キャプチャーポイント位置を獲得する。この際、少なくとも一つの方向に対する現在キャプチャーポイントの位置を獲得する。

次に、ロボットは、現在キャプチャーポイントと目標キャプチャーポイントとを比較して、キャプチャーポイントエラーを算出し、現在ヒップ高さと目標ヒップ高さとを比較して、ヒップ高さエラーを算出し、現在ポーズ角度と目標ポーズ角度とを比較して、ポーズ角度エラーを算出する（４０５）。

次に、ロボットは、キャプチャーポイントエラーから水平方向であるｘ、ｙ軸のうち少なくとも一つの座標値を獲得し、このｘ、ｙ方向のうち少なくとも一つの方向に対する補償力を算出する。

また、ヒップ高さエラーから垂直方向のｚ軸方向の座標値を獲得し、このｚ方向の補償力を算出する。

次に、ロボットは、重心に対する地面の投影点（ＣＯＧ’）とロボット１００の両足１７０Ｌ，１７０Ｒ間の距離割合を用いて、重心の投影点により近い脚により大きい力を配分する。

次に、ロボットは、予め設定されたＦＳＭの現在状態及び補償力算出部２５３ａで算出された補償力の大きさを用いて、ロボットの各関節部１６３，１６４，１６５に必要な仮想重力の大きさを設定する。

次に、ロボットは、ポーズ角度エラーに基づいて、ヨー、ロール、ピッチ方向の補償モーメントを算出する（４０６）。

さらに、補償力を算出することは、補償モーメントを算出することと前後関係がないため、両段階の順番が変わっても差し支えない。

次に、ロボットは、仮想重力と実際の重力を補償するために、各関節部１６３，１６４，１６５に必要な重力補償トルクを算出するが、重力補償トルクは仮想重力加速度と実際の重力加速度との和、各関節部の角度、各リンクの重さ及び各リンク内の重心の位置などを用いて算出する。

この際、両脚１６０Ｌ，１６０Ｒに配分された補償力を顧慮して、各脚１６０Ｌ，１６０Ｒの関節部１６３，１６４，１６５に必要な重力補償トルクを算出する。

次に、ロボットは、重力補償トルクと補償モーメントに対するトルクとを合算して、ロボットの各関節部１６３，１６４，１６５の少なくとも一つの自由度に対する第１トルクを算出する（４０７）。

この際、ロボットは、左右脚１６０Ｌ，１６０Ｒに配分された補償力を顧慮して、左右脚１６０Ｌ，１６０Ｒの各関節部１６３，１６４，１６５に補償力を発生させるための第１トルクを算出する。

すなわち、Ｘ軸方向に対する並進運動自由度及び回転運動自由度が行われるように、股関節部１６３のロール方向の回転関節１６３ｃ及び足首関節部のロール方向の回転関節１６５ｂに印加されるトルクを算出し、Ｚ軸方向に対する並進運動自由度及び回転運動自由度が行われるように、股関節部１６３のヨー方向の回転関節１６３ａに印加されるトルクを算出する。

次に、ロボットは、残りの自由度に対する目標値を設定する（４０８）。すなわち、ロボットは、残りの自由度であるＹ軸方向の並進運動自由度に対する目標位置を設定し、Ｙ軸方向のピッチ回転運動自由度に対する目標角度を設定する。

次に、ロボットは、設定された目標位置及び目標角度を満足する逆運動学の解を演算し（４０９）、その演算された逆運動学の解を用いて、複数の関節部の目標角度を算出する（４１０）。

この際、Ｙ軸に対する自由度を有する股関節部のピッチ方向の回転関節、膝関節部のピッチ方向の回転関節、足首関節部のピッチ方向の回転関節の目標角度を算出する。

次に、ロボットは、関節部の角度を制御するためのゲインを設定したあと、その設定されたゲインに基づいて算出された複数の関節部の目標角度を追従するための第２トルクをそれぞれ算出する（４１１）。ここで、第２トルクは、関節数ｎと同一なｎ次元のベクトルである。

この際、ロボットは、Ｙ軸方向に対する並進運動自由度及び回転運動自由度が行われるように、股関節部１６３のピッチ方向の回転関節１６３ｂ、膝関節部１６４のピッチ方向の回転関節１６４ａ及び足首関節部１６５のピッチ方向の回転関節１６５ａに印加されるトルクをそれぞれ算出する。

次に、ロボットは、第１トルクと第２トルクとを合算して、複数の関節部にそれぞれ印加される目標トルクをそれぞれ算出（４１２）した後、左右脚１６０Ｌ，１６０Ｒの各関節部１６３，１６４，１６５に対して算出された目標トルクを各関節部に出力することにより、均衡の取れたモーションを行う（４１３）。

これにより、ロボットは、複数の自由度のそれぞれに対して目標値を満足する姿勢を保持することができる。

図１１は、本発明のさらに他の実施例に係るロボットの均衡制御方法を示すフローチャートであって、図３を参照して説明する。

本発明のさらに他の実施例に係るロボットの均衡制御装置は、定められたモーションを行うための姿勢を保持させた状態で少なくとも一つの関節部が使用者の意図通りに動けるように、少なくとも一つの関節部の角度が入力される入力部２７０を含む。

この際、均衡制御部２５０は、入力された少なくとも一つの関節部の角度を追従するための第２トルクを算出し、その算出された第２トルクと第１トルクとを合算して目標トルクを算出する。

ここで、第１トルクは、キャプチャーポイント及びヒップ高さに基づいて算出された複数の関節部に印加されるトルクであって、一実施例と同一な方法で算出されたトルクである。

図１１を参照して、さらに他の実施例に係るロボットの均衡制御方法を説明する。まず、ロボットは、動作中両脚に作用する力／トルクを検出して、両足の位置及び方向を確認し、両足の位置及び方向に基づいて、目標キャプチャーポイント、目標ポーズ角度、目標ヒップ高さを設定する（５０１）。

次に、ロボットは、現在姿勢と次の姿勢である目標姿勢間のエラーを算出し、その算出されたエラーを補償することにより、均衡を保持しながら次の姿勢を行う。これについて、さらに具体的に説明する。

ロボットは、モーションを行いながら、力／トルク検出部２１０、ポーズ検出部２２０、角度検出部２３０を通じて両脚に印加される力／トルク、上体のポーズ角度を検出し、また複数の関節部１３１，１４３，１４４，１６３，１６４，１６５などの角度を検出（５０２）し、力／トルク検出部２１０、ポーズ検出部２２０及び角度検出部２３０を通じて検出された上体のポーズ角度及び各関節部１３１，１４３，１４４，１６３，１６４，１６５の角度、足の位置及び方向に基づいて、重心を獲得する(５０３)。

次に、ロボットは、重心に基づいて、現在キャプチャーポイント、ポーズ角度、複数の関節部の角度に基づいて、現在ヒップ高さを獲得し、ポーズ検出部２２０で検出されたポーズのヨー、ロール、ピッチの３軸方向のポーズ角度を獲得する（５０４）。

次に、ロボットの重心の位置及び速度を用いて、現在キャプチャーポイントの位置を獲得する。次に、ロボットは、現在キャプチャーポイントと目標キャプチャーポイントとを比較して、キャプチャーポイントエラーを算出し、現在ヒップ高さと目標ヒップ高さとを比較して、ヒップ高さエラーを算出し、現在ポーズ角度と目標ポーズ角度とを比較して、ポーズ角度エラーを算出する（５０５）。

次に、ロボットは、キャプチャーポイントエラーから水平方向であるｘ、ｙ軸の座標値を獲得し、このｘ、ｙ方向に対する補償力を算出する。

また、ヒップ高さエラーから垂直方向のｚ軸方向の座標値を獲得し、このｚ方向の補償力を算出する。

次に、ロボットは、重心に対する地面の投影点（ＣＯＧ’）とロボット１００の両足１７０Ｌ，１７０Ｒ間の距離割合を用いて、重心の投影点により近い脚により大きい力を配分する。

次に、ロボットは、予め設定されたＦＳＭの現在状態及び補償力算出部２５３ａで算出された補償力の大きさを用いて、ロボットの各関節部１６３，１６４，１６５に必要な仮想重力の大きさを設定する。

次に、ロボットは、ポーズ角度エラーに基づいて、ヨー、ロール、ピッチ方向の補償モーメントを算出する（５０６）。

次に、ロボットは、仮想重力と実際の重力を補償するために、各関節部１６３，１６４，１６５に必要な重力補償トルクを算出するが、重力補償トルクは仮想重力加速度と実際の重力加速度との和、各関節部の角度、各リンクの重さ及び各リンク内の重心の位置を用いて算出する。

この際、両脚１６０Ｌ，１６０Ｒに配分された補償力を顧慮して、各脚１６０Ｌ，１６０Ｒの関節部１６３，１６４，１６５に必要な重力補償トルクを算出する。

次に、ロボットは、重力補償トルクと補償モーメントに対するトルクとを合算して、ロボットの各関節部１６３，１６４，１６５に印加される第１トルクを算出する（５０７）。

次に、ロボットは、入力部２７０を通じて少なくとも一つの関節部に対する角度が入力されたか否かを判断し、少なくとも一つの関節部に対する角度が入力されたと判断されると、その入力された角度を少なくとも一つの関節部の目標角度として設定する（５０８）。

次に、ロボットは設定された目標角度と実際の角度間のエラーを算出する（５０９）。

この際、実際の角度は、第１トルクが少なくとも一つの関節部に印加されたとき、少なくとも一つの関節部が取る角度である。

ここで、エラーは少なくとも一つの関節部に対する角度エラーベクトルである。

すなわち、ロボットは、少なくとも一つの関節部の角度を追従するために、角度エラーベクトル（Ｅ）を用いて第２トルク（Ｔ）を算出する（５１０）。

Ｔ＝Ｋ＊Ｅ
ここで、Ｔは少なくとも一つの関節に対するｒ次元トルクベクトルであり、Ｋは制御ゲインであって、スカラまたはｒｘｒ対角行列であり、ｒは角度が入力される関節部の数である。

なお、少なくとも一つの関節部に複数の関節が設けられた場合、複数の関節に対する角度がそれぞれ入力されてもよく、または複数の関節のうち少なくとも一つの関節に対する角度だけが入力されてもよい。

次に、ロボットは、第１トルクと第２トルクとを合算して、複数の関節部にそれぞれ印加される目標トルクをそれぞれ算出する（５１１）。

次に、ロボットは、左右脚１６０Ｌ，１６０Ｒの各関節部１６３，１６４，１６５に対して算出された目標トルクを各関節部に出力することにより、均衡の取れたモーションを行う（５１２）。

これにより、ロボットは、姿勢制御を行うと同時に少なくとも一つの関節部に対して所望の形状を保持することができる。また、角度制御をする関節部の選択に応じて、均衡の取れた姿勢と使用者の意図に沿う姿勢を取ることができる。

１６０Ｌ，１６０Ｒ 脚
１７０Ｌ，１７０Ｒ 足
１６３ 股関節部
１６４ 膝関節部
１６５ 足首関節部
２１０ 力／トルク検出部
２２０ ポーズ検出部
２３０ 角度検出部
２４０ 設定部
２５０ 均衡制御部
２５１ 獲得部
２５２ エラー算出部
２５３ 補償部
２５４ 配分部
２５５ トルク算出部
２６０ サーボ制御部

Claims (44)

1. 複数の関節部がそれぞれ設けられた複数の脚と、脚に連結された上体を含み複数の並進運動自由度と回転運動自由度を有するロボットの均衡制御方法において、
   前記上体のポーズ角度及び前記複数の関節部の角度を検出し、
   前記ポーズ角度及び複数の関節部の角度に基づいて現在の姿勢を獲得し、
   前記複数の並進運動自由度のうち少なくとも一つの並進運動自由度に対して、前記姿勢と予め設定された目標姿勢に基づいた姿勢エラーを算出し、
   前記姿勢エラーに基づいて、前記少なくとも一つの並進運動自由度に対する補償力を算出し、
   前記少なくとも一つの並進運動自由度に対する補償力に基づいて、前記少なくとも一つの並進運動自由度に対する目標トルクを前記複数の関節部に対してそれぞれ算出し、
   前記少なくとも一つの並進運動自由度に対する目標トルクを前記複数の関節部に出力して、前記ロボットの均衡を制御するロボットの均衡制御方法。
2. 前記目標トルクを算出することは、
   前記少なくとも一つの並進運動自由度に対する目標トルクを残りの並進運動自由度に対応する目標トルクに合算して目標トルク和を算出する、請求項１に記載のロボットの均衡制御方法。
3. 前記姿勢エラーを算出することは、
   前記検出されたポーズ角度及び複数の関節部の角度に基づいて現在の重心位置を検出するものの、前記少なくとも一つの並進運動自由度に対する現在の重心位置を検出し、
   前記検出された現在の重心と予め設定された目標重心とを比較して、前記少なくとも一つの並進運動自由度に対する重心エラーを算出し、
   前記重心エラーを姿勢エラーとして用いることを含む、請求項１に記載のロボットの均衡制御方法。
4. 前記姿勢エラーを算出することは、
   前記検出されたポーズ角度及び複数の関節部の角度に基づいて、現在重心の位置と速度を検出するものの、前記少なくとも一つの並進運動自由度に対する現在重心の位置と速度を検出し、
   前記検出された現在重心の位置と速度に基づいて、前記少なくとも一つの並進運動自由度に対する現在キャプチャーポイントを獲得し、
   前記獲得された現在キャプチャーポイントと予め設定された目標キャプチャーポイントとを比較して、少なくとも一つの並進運動自由度に対するキャプチャーポイントエラーを算出し、
   前記算出されたキャプチャーポイントエラーを前記姿勢エラーとして用いることを含む、請求項１に記載のロボットの均衡制御方法。
5. 前記姿勢エラーを算出することは、
   前記上体のポーズ角度及び前記複数の関節部の角度を検出し、
   前記ポーズ角度及び複数の関節部の角度に基づいて、ヒップまたは上体のいずれか一つの参照点の位置を前記少なくとも一つの自由度に対して算出し、
   前記現在の参照点と予め設定された目標参照点の位置とを比較して、参照点位置エラーを前記少なくとも一つの自由度に対して算出し、
   前記参照点位置エラーを前記少なくとも一つの並進運動自由度の姿勢エラーとして用いることを含む、請求項１に記載のロボットの均衡制御方法。
6. 前記姿勢エラーを算出することは、
   前記複数の並進運動自由度のうち残りの並進運動自由度の姿勢エラー算出と相異なることを含む、請求項１に記載のロボットの均衡制御方法。
7. 前記補償力を算出することは、
   現在キャプチャーポイント及び現在ヒップ高さを獲得し、
   前記現在キャプチャーポイントと予め設定された目標キャプチャーポイントとを比較して、キャプチャーポイントエラーを算出し、
   前記現在ヒップ高さと予め設定された目標ヒップ高さとを比較して、ヒップ高さエラーを算出し、
   前記キャプチャーポイントエラーとヒップ高さエラーに基づいて、補償力を算出することを含む、請求項１に記載のロボットの均衡制御方法。
8. 前記現在キャプチャーポイントを獲得することは、
   前記上体のポーズ角度及び複数の関節部の角度に基づいて、重心の位置と速度を獲得し、その獲得された重心の位置と速度に基づいて現在キャプチャーポイントを獲得することを含む、請求項７に記載のロボットの均衡制御方法。
9. 前記現在ヒップ高さを算出することは、
   前記重心及び前記複数の関節部の角度に基づいて、前記現在ヒップ高さを算出することを含む、請求項７に記載のロボットの均衡制御方法。
10. 前記補償力を算出することは、
    前記キャプチャーポイントエラーを用いて、水平方向の補償力を算出し、
    前記ヒップ高さエラーを用いて、垂直方向の補償力を算出することを含む、請求項７に記載のロボットの均衡制御方法。
11. 前記上体のポーズ角度及び前記複数の関節部の角度に基づいて、現在の姿勢を判断し、
    前記現在の姿勢と予め格納されたモーション情報に基づいて、前記目標キャプチャーポイント、前記目標ヒップ高さを設定することをさらに含む、請求項７に記載のロボットの均衡制御方法。
12. 前記目標トルクを算出することは、
    前記ロボットの重心が地面に投影された投影点と前記複数の脚にそれぞれ連結された足間の距離比率を算出し、
    前記足間の距離比率に基づいて、前記複数の脚に印加される補償力を配分し、
    前記複数の脚に配分された補償力に基づいて、前記複数の関節部に印加される目標トルクを算出することをさらに含む、請求項１に記載のロボットの均衡制御方法。
13. 前記キャプチャーポイントを獲得することは、
    順運動学を用いる、請求項７に記載のロボットの均衡制御方法。
14. 前記上体のポーズ角度と予め設定された目標ポーズ角度とを比較して、ポーズ角度エラーを算出し、
    前記ポーズ角度エラーに基づいて、補償モーメントを算出し、
    前記目標トルクの算出時、前記補償モーメントを反映することをさらに含む、請求項１に記載のロボットの均衡制御方法。
15. 前記補償モーメントを前記目標トルクに反映することは、
    ジャコビアンを用いて前記補償モーメントを前記関節部のトルクに変換し、
    前記変換されたトルクを前記目標トルクの算出時に合算することを含む、請求項１４に記載のロボットの均衡制御方法。
16. 前記補償モーメントを算出することは、
    前記ポーズ角度エラーを用いて、ヨー、ロール、ピッチ方向の補償モーメントを算出することを含む、請求項１４に記載のロボットの均衡制御方法。
17. 前記ポーズ角度を検出することは、
    前記上体のヨー角度、ロール角度、ピッチ角度のうち少なくとも一つの角度を検出することを含む、請求項１に記載のロボットの均衡制御方法。
18. 前記目標トルクを算出することは、
    前記補償力、ロボットの質量及び地球重力加速度に基づいて、仮想重力加速度を算出し、
    前記仮想重力加速度に基づいて、前記少なくとも一つの自由度に対応する仮想重力補償トルクをそれぞれ算出し、
    前記仮想重力補償トルクを反映して、前記目標トルクを算出することをさらに含む、請求項１に記載のロボットの均衡制御方法。
19. 前記複数の自由度のうち残りの自由度に対する位置及び角度を設定し、
    前記設定された位置及び角度を満足する逆運動学の解を演算し、
    前記逆運動学の解に基づいて、前記複数の関節部の残りの自由度に対する目標角度を算出し、
    前記複数関節部の残りの自由度に対する目標角度に基づいて、前記複数関節部の残りの自由度に対応するトルクを算出することを含む、請求項１に記載のロボットの均衡制御方法。
20. 前記目標トルクを算出することは、
    前記少なくとも一つの自由度に対するトルクと前記残りの自由度に対するトルクとを合算して、前記目標トルクを算出することを含む、請求項１９に記載のロボットの均衡制御方法。
21. 前記複数の関節部のうち少なくとも一つの関節部に対する目標角度が入力されて、
    前記入力された少なくとも一つの関節部の目標角度を追従するためのトルクを算出し、
    前記目標トルクの算出時、前記少なくとも一つの関節部の目標角度に対するトルクを反映することをさらに含む、請求項１に記載のロボットの均衡制御方法。
22. 前記目標角度が入力されることは、
    前記複数の関節部に複数の自由度に対応する複数の関節がそれぞれ設けられた場合、前記複数の関節の目標角度がそれぞれ入力されることを含む、請求項２１に記載のロボットの均衡制御方法。
23. 前記目標トルクを算出することは、
    ジャコビアンを用いて前記目標トルクを算出することを含む、請求項１に記載のロボットの均衡制御方法。
24. 前記目標トルクを算出することは、
    前記補償力、ロボットの質量及び地球重力加速度に基づいて、仮想重力加速度を算出し、
    前記仮想重力加速度に基づいて、前記少なくとも一つの自由度に対応する仮想重力補償トルクを算出し、
    前記算出された仮想重力補償トルク及び前記ジャコビアンに基づいて算出されたトルクを用いて、前記複数の関節部の前記少なくとも一つの自由度に対応する目標トルクを算出することをさらに含む、請求項１に記載のロボットの均衡制御方法。
25. 複数の関節部がそれぞれ設けられた複数の脚と、その脚に連結された上体を有しており、複数の自由度を有するロボットの均衡制御装置において、
    前記上体のポーズ角度を検出するポーズ検出部；
    前記複数の関節部の角度を検出する角度検出部；
    予め格納されたモージョン情報に基づいて、前記複数の自由度のうち少なくとも一つの自由度に対して目標姿勢を設定する設定部；
    前記ポーズ角度及び複数の関節部の角度に基づいて、前記少なくとも一つの自由度に対して現在姿勢を獲得し、前記現在姿勢と目標姿勢とを比較して姿勢エラーを算出し、前記姿勢エラーに基づいて前記少なくとも一つの自由度に対して補償力を算出し、前記補償力に基づいて前記少なくとも一つの自由度に対する目標トルクを算出する均衡制御部；
    前記複数の関節部に前記少なくとも一つの自由度に対する目標トルクを出力するサーボ制御部と含むロボットの均衡制御装置。
26. 前記均衡制御部は、
    前記上体のポーズ角度及び複数の関節部の角度に基づいて重心を獲得し、前記重心に基づいて前記現在姿勢を獲得する獲得部を含む、請求項２５に記載のロボットの均衡制御装置。
27. 前記均衡制御部は、
    前記上体のポーズ角度及び複数の関節部の角度に基づいて重心の位置と速度を獲得し、前記重心の位置と速度に基づいて前記現在キャプチャーポイントを獲得して、前記現在姿勢を獲得する獲得部を含む、請求項２５に記載のロボットの均衡制御装置。
28. 前記均衡制御部は、
    前記上体のポーズ角度及び複数の関節部の角度に基づいてヒップや上体の参照点の位置を獲得し、前記参照点の位置に基づいて前記現在姿勢を獲得する獲得部を含む、請求項２５に記載のロボットの均衡制御装置。
29. 前記設定部は、
    前記少なくとも一つの自由度に対して目標重心を設定し、
    前記目標重心を用いて前記目標姿勢を設定する、請求項２５に記載のロボットの均衡制御装置。
30. 前記設定部は、
    前記少なくとも一つの自由度に対して目標キャプチャーポイントを設定し、
    前記目標キャプチャーポイントを用いて前記目標姿勢を設定する、請求項２５に記載のロボットの均衡制御装置。
31. 前記設定部は、
    前記少なくとも一つの自由度に対してヒップや上体の参照点の目標位置を設定し、前記参照点の目標位置に基づいて前記目標姿勢を設定する、請求項２５に記載のロボットの均衡制御装置。
32. 前記ロボットの重心が地面に投影された投影点と前記複数の脚にそれぞれ連結された足間の距離比率を算出し、前記足間の距離比率に基づいて、前記複数の脚に印加される補償力を配分する配分部をさらに含み、
    前記均衡制御部は、前記複数の脚に配分された補償力に基づいて、前記複数の関節部に印加されるトルクを算出する、請求項２５に記載のロボットの均衡制御装置。
33. 前記均衡制御部は、
    前記上体のポーズ角度と予め設定された目標ポーズ角度とを比較してポーズ角度エラーを算出し、前記ポーズ角度エラーに基づいて補償モーメントを算出し、前記トルクの算出時に前記補償モーメントを反映することをさらに含む、請求項２５に記載のロボットの均衡制御装置。
34. 前記均衡制御部は、
    前記ポーズ角度エラーを用いて、ヨー、ロール、ピッチ方向の補償モーメントを算出することを含む、請求項２５に記載のロボットの均衡制御装置。
35. 使用者から少なくとも一つの姿勢からなるモーション情報が入力される入力部をさらに含み、
    前記設定部は、前記入力されたモーション情報を格納する、請求項２５に記載のロボットの均衡制御装置。
36. 前記均衡制御部は、
    前記補償力、予め格納されたロボットの質量及び地球重力加速度に基づいて仮想重力加速度を算出し、前記仮想重力加速度に基づいて前記少なくとも一つの自由度に対応する仮想重力補償トルクをそれぞれ算出し、前記仮想重力補償トルクを反映して、前記目標トルクを算出することを含む、請求項２５に記載のロボットの均衡制御装置。
37. 前記均衡制御部は、
    前記複数の自由度のうち残りの自由度に対するトルクを逆運動学を用いて算出する、請求項２５に記載のロボットの均衡制御装置。
38. 前記均衡制御部は、
    前記複数の関節部のうち少なくとも一つの関節部に対する目標角度が入力されると、前記入力された目標角度に対応するトルクを算出し、前記目標トルクの算出時に前記少なくとも一つの関節部の目標角度に対するトルクを反映する、請求項２５に記載のロボットの均衡制御装置。
39. 前記均衡制御部は、
    ジャコビアンを用いて前記目標トルクを算出する、請求項２５に記載のロボットの均衡制御装置。
40. 前記均衡制御部は、
    前記ポーズ角度及び複数の関節部の角度に基づいて、現在キャプチャーポイント及び現在ヒップ高さを獲得し、前記現在キャプチャーポイントと目標キャプチャーポイントとを比較してキャプチャーポイントエラーを算出し、前記現在ヒップ高さと目標ヒップ高さとを比較して、ヒップ高さエラーを算出し、前記キャプチャーポイントエラーとヒップ高さエラーに基づいて補償力を算出し、前記補償力に基づいて前記複数の自由度のうち少なくとも一つの自由度に対する目標トルクを算出する、請求項２５に記載のロボットの均衡制御装置。
41. 前記均衡制御部は、
    前記上体のポーズ角度及び複数の関節部の角度に基づいて、重心及び前記ヒップ高さを獲得し、前記重心に基づいて現在キャプチャーポイントを獲得する獲得部を含む、請求項４０に記載のロボットの均衡制御装置。
42. 前記均衡制御部は、
    前記キャプチャーポイントエラーを用いて水平方向の補償力を算出し、前記ヒップ高さエラーを用いて垂直方向の補償力を算出することを含む、請求項４０に記載のロボットの均衡制御装置。
43. 前記複数の脚に設けられた各足に印加される荷重を検出する力／トルク検出部をさらに含み、
    前記設定部は、前記各足に印加される荷重に基づいて現在姿勢を判断し、前記現在姿勢と前記予め設定されたモーション情報に基づいて、前記目標キャプチャーポイント、前記目標ヒップ高さを設定する、請求項４０に記載のロボットの均衡制御装置。
44. 前記設定部は、
    前記複数の脚に設けられた足の支持領域内の一点が重力方向に通る線上の一点を前記目標キャプチャーポイントとして設定する、請求項４０に記載のロボットの均衡制御装置。

Images