ロボット及びその制御方法
【課題】 ロボットの重心と速度を合成して、キャプチャポイントを算出し、ロボットの歩行中にキャプチャポイントを制御することによって、安定的に歩行を制御するロボット及びその制御方法を提供する。
【解決手段】 本発明は、６自由度の一対の脚を有する２足ロボットに関する。また、人間の歩行と類似した動作で有限状態機械（ＦＳＭ）を構成して、自然な歩行動作を可能にし、膝を曲げずに歩行させることで、大きな歩幅の歩行と効率的なエネルギーの使用を可能にする。
【解決手段】 本発明は、６自由度の一対の脚を有する２足ロボットに関する。また、人間の歩行と類似した動作で有限状態機械（ＦＳＭ）を構成して、自然な歩行動作を可能にし、膝を曲げずに歩行させることで、大きな歩幅の歩行と効率的なエネルギーの使用を可能にする。
【発明の詳細な説明】
【技術分野】
【０００１】
トルクサーボによって各関節が動作するロボットの安定的な歩行制御のためのロボット及びその制御方法に関する。
【背景技術】
【０００２】
人間の歩行方法に近く二本脚で歩くロボットの歩行制御技術は、ロボットの各構成部分を連結する関節を制御することで実現される。それぞれの関節を制御する方法は、関節の角度を制御するものと、関節のトルクを制御するものとに大別される。
【０００３】
関節の角を制御する方法は、ロボットの足と地面とが当接している各点が生成する支持多角形（Ｓｕｐｐｏｒｔ Ｐｏｌｙｇｏｎ）内にＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）が位置するように、それぞれの関節の角度を逆運動学（Ｉｎｖｅｒｓｅ Ｋｉｎｅｍａｔｉｃｓ）を用いて算出したあと、ロボットのそれぞれの関節がその角度を維持するようにする方式で、関節のトルクを制御する方法のうち代表的なものは、動的歩行（Ｄｙｎａｍｉｃ Ｗａｌｋｉｎｇ）に分類されるものとして、ロボットの全体に作用する重力の方向を、ロボットが傾斜面にいるときのように定め、これに基づいてそれぞれの関節に必要なトルクを発生させる方式である。
【０００４】
関節の角を制御する方法は、逆運動学を解く過程で膝が曲がらないと値が存在しない。そのため、ロボットが膝を曲げた状態で歩行する場合、歩幅が小さくなり、エネルギー消耗が増加して、歩行効率が低下するという問題点がある。
【０００５】
また、動的歩行方法は、ロボットの歩行を３次元空間で解析するには複雑なので２次元歩行を具現するものであるか、または３次元歩行を具現したとしても、安定的でなく、人間の歩行と違って不自然であるという問題点がある。
【発明の概要】
【発明が解決しようとする課題】
【０００６】
本発明は、上述した問題点を解決するために、膝を伸ばした状態で歩行することができ、３次元歩行を具現しながらも、人間の歩行のように自然で安定した歩行が可能なロボット及びその制御方法を提供する。
【課題を解決するための手段】
【０００７】
本発明の一側面に係る、複数の関節部がそれぞれ設けられた複数の脚と、前記脚に連結された上体とを有し、複数の自由度を有するロボットは、前記ロボットの状態情報を測定するセンサ部と；前記センサ部で測定された状態情報を用いて、前記ロボットの現在の姿勢を獲得する運動学計算部；予め設定された有限状態機械（ＦＳＭ）に基づいて、前記複数の自由度のうち少なくとも一つの自由度に対して目標姿勢を設定する目標設定部と；前記現在の姿勢と目標姿勢とを比較して姿勢エラーを算出し、前記姿勢エラーに基づいて前記少なくとも一つの自由度に対して補償力を算出する補償力算出部と；前記算出された補償力によって前記複数の関節部に必要な仮想重力の大きさ、及び前記ロボットの現在のＦＳＭの状態によって前記複数の関節部に必要な仮想重力の大きさを設定する仮想重力設定部と；前記仮想重力設定部で設定された仮想重力を補償するための重力補償トルクを算出する重力補償トルク算出部と；前記複数の関節部に前記重力補償トルクを出力するサーボ制御部と；を含む。
【０００８】
前記センサ部は、前記上体のポーズ角度を検出するポーズ検出部と、前記複数の関節部の角度を検出する角度検出部とを含むことができる。
【０００９】
前記運動学計算部は、前記上体のポーズ角度及び複数の関節部の角度に基づいて重心を算出し、前記重心に基づいて前記現在の姿勢を獲得することができる。
【００１０】
前記運動学計算部は、前記上体のポーズ角度及び複数の関節部の角度に基づいて重心の位置と速度を算出し、前記重心の位置と速度に基づいて現在のキャプチャポイントを算出することによって、前記現在の姿勢を獲得することができる。
【００１１】
前記運動学計算部は、前記上体のポーズ角度及び前記複数の関節部の角度に基づいてヒップや上体の参照点の位置を獲得し、前記参照点の位置に基づいて前記現在の姿勢を獲得することができる。
【００１２】
前記目標設定部は、前記少なくとも一つの自由度に対して目標重心を設定し、前記目標重心を用いて前記目標姿勢を設定することができる。
【００１３】
前記目標設定部は、前記少なくとも一つの自由度に対して目標キャプチャポイントを設定し、前記目標キャプチャポイントを用いて前記目標姿勢を設定することができる。
【００１４】
前記目標設定部は、前記複数の脚に設けられた足の支持領域内の一点が重力方向に通る線上の一点を目標キャプチャポイントとして設定することができる。
【００１５】
前記目標設定部は、前記少なくとも一つの自由度に対して前記ロボットのヒップや上体の参照点の目標位置を設定し、前記参照点の目標位置に基づいて前記目標姿勢を設定することができる。
【００１６】
前記運動学計算部は、
前記ポーズ角度及び前記複数の関節部の角度に基づいて現在のヒップ高さをさらに獲得し、前記補償力算出部は、前記現在のキャプチャポイントと前記目標キャプチャポイントとを比較してキャプチャポイントエラーを算出し、前記現在のヒップ高さと目標ヒップ高さとを比較してヒップ高さエラーを算出し、前記キャプチャポイントエラーとヒップ高さエラーに基づいて補償力を算出することができる。
【００１７】
前記補償力算出部は、前記キャプチャポイントエラーを用いて水平方向の補償力を算出し、前記ヒップ高さエラーを用いて垂直方向の補償力を算出することができる。
【００１８】
前記ポーズ検出部で検出するポーズ角度は、前記ロボットのペルヴィスリンクのロール方向（ｒｏｌｌ、Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）、ヨー方向（ｙａｗ、Ｚ軸回転）の回転角度のうち少なくとも一つを含み、前記複数の関節部のトルクを測定する関節トルク検出部をさらに含むことができる。
【００１９】
前記運動学計算部は、前記ロボットの現在の重心の位置及び前記重心の速度を用いて、前記ロボットの現在のキャプチャポイントの位置を算出することができる。
【００２０】
前記ロボットは、前記ロボットの有限状態機械（ＦＳＭ）を予め格納し、前記ロボットの現在のＦＳＭの状態を判断し、前記ロボットがＦＳＭの状態遷移の条件を満足するか否かによって、次のＦＳＭの状態に遷移するように制御するＦＳＭ制御部をさらに含むことができる。
【００２１】
前記目標設定部は、前記ロボットの現在のＦＳＭの状態によって目標キャプチャポイントの位置を設定し、前記ロボットの上体が直立姿勢を維持するように目標ポーズ角度を設定することができる。
【００２２】
前記ロボットは、前記センサ部で測定したポーズ角度と、前記目標設定部で設定した目標ポーズ角度との間の差を用いて、前記目標ポーズ角度に到達するために補償しなければならない補償モーメントを算出する補償モーメント算出部をさらに含むことができる。
【００２３】
前記補償力算出部は、前記運動学計算部で計算した前記ロボットの現在のキャプチャポイントのｘ方向の位置及びｙ方向の位置と、前記目標設定部で設定した目標キャプチャポイントのｘ方向の位置及びｙ方向の位置との間の差を用いて、ｘ方向及びｙ方向の補償力を算出し、前記運動学計算部で計算した前記ロボットの現在のヒップ高さと、前記目標設定部で設定した目標ヒップ高さとの間の差を用いて、ｚ軸方向の補償力を算出することができる。
【００２４】
前記仮想重力設定部は、前記ロボットのＦＳＭの現在の状態による仮想重力、及び前記補償力算出部で算出した補償力による仮想重力の大きさを設定することができる。
【００２５】
前記仮想重力設定部は、前記ロボットの質量をｍとし、前記補償力算出部で算出した補償力をｆとするとき、下記の数学式（２）を用いて、前記ロボットの各関節に必要な第１の仮想重力を設定する第１の仮想重力設定部と、前記ロボットの現在のＦＳＭの状態によって、各関節に必要な第２の仮想重力を設定する第２の仮想重力設定部と、を含むことができる。
【００２６】
ｇｆ＝ｆ／ｍ （２）
前記ロボットは、前記ロボットの重心が地面に投影された点と前記各脚に連結された足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点からより近い脚に、より大きい比率の上体質量を割り当てる重量分配部をさらに含むことができる。
【００２７】
前記重力補償トルク算出部は、前記重量分配部で前記各脚に分配した上体の質量を反映して、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクを算出し、前記ロボットのＦＳＭの状態によって、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクの比重を調節して、重力補償トルクを算出することができる。
【００２８】
前記ロボットは、前記ロボットの重心が地面に投影された点と前記ロボットの複数の足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点から近い脚に、より大きい値の補償モーメントを割り当てることによって、前記補償モーメント算出部で算出された補償モーメントを前記複数の脚に分配する補償モーメント分配部をさらに含む。
【００２９】
前記ロボットは、前記ロボットの足をベースリンクと見るヤコビアンを用いて、前記複数の脚に分配した補償モーメントから前記複数の脚に必要な第１の目標トルクを算出する目標トルク算出部をさらに含むことができる。
【００３０】
前記目標トルク算出部は、前記第１の目標トルクと前記重力補償トルク算出部で算出した重力補償トルクとを合算して、前記ロボットの各関節に必要な第２の目標トルクを算出することができる。
【００３１】
前記ロボットは、前記目標トルク算出部で算出された第２の目標トルクに各関節の角速度に比例するダンピングトルクを加えて、第３の目標トルクを算出するトルク補償部をさらに含むことができる。
【００３２】
本発明の一側面に係る、複数の関節部がそれぞれ設けられた複数の脚と、該脚に連結された上体とを含み、複数の並進運動の自由度及び回転運動の自由度を有するロボットの制御方法は、前記上体のポーズ角度及び前記複数の関節部の角度を検出し；前記検出された複数の関節部の角度に基づいて、前記複数の並進運動の自由度のうち少なくとも一つの並進運動の自由度に対する現在の姿勢を獲得し；前記獲得された現在の姿勢と予め設定された目標姿勢との差に基づいて、姿勢エラーを算出し；前記姿勢エラーに基づいて、前記少なくとも一つの並進運動の自由度に対する補償力を算出し；前記少なくとも一つの並進運動の自由度に対する補償力、及び前記ロボットに対して予め設定された有限状態機械（Ｆｉｎｉｔｅ Ｓｔａｔｅ Ｍａｃｈｉｎｅ：ＦＳＭ）の現在の状態に基づいて、仮想重力を設定し；前記設定された仮想重力を補償するために、前記ロボットの関節部に必要な重力補償トルクを算出し；前記ロボットの関節部のトルクが前記算出された重力補償トルクを含む目標トルクになるように各関節のトルクサーボを制御することを含む。
【００３３】
前記姿勢エラーを算出することは、前記検出されたポーズ角度及び前記複数の関節部の角度に基づいて現在の重心の位置を算出するが、前記少なくとも一つの並進運動の自由度に対する現在の重心の位置を算出し、前記算出された現在の重心と予め設定された目標重心とを比較して、前記少なくとも一つの並進運動の自由度に対する重心エラーを算出し、前記重心エラーを姿勢エラーとして用いることを含むことができる。
【００３４】
前記姿勢エラーを算出することは、前記検出されたポーズ角度及び複数の関節部の角度に基づいて、現在の重心の位置と速度を検出するが、前記少なくとも一つの並進運動の自由度に対する現在の重心の位置と速度を検出し、前記検出された現在の重心の位置と速度に基づいて、前記少なくとも一つの並進運動の自由度に対する現在のキャプチャポイントを獲得し、前記獲得された現在のキャプチャポイントと予め設定された目標キャプチャポイントとを比較して、少なくとも一つの並進運動の自由度に対するキャプチャポイントエラーを算出し、前記算出されたキャプチャポイントエラーを前記姿勢エラーとして用いることを含むことができる。
【００３５】
前記姿勢エラーを算出することは、前記上体のポーズ角度及び前記複数の関節部の角度を検出し、前記ポーズ角度及び前記複数の関節部の角度に基づいて、ヒップまたは上体のいずれか一つの参照点の位置を前記少なくとも一つの自由度に対して算出し、前記現在の参照点と予め設定された目標参照点との位置を比較して、参照点位置エラーを前記少なくとも一つの自由度に対して算出し、前記参照点位置エラーを前記少なくとも一つの並進運動の自由度の姿勢エラーとして用いることを含むことができる。
【００３６】
前記姿勢エラーを算出することは、前記複数の並進運動の自由度のうち残りの並進運動の自由度の姿勢エラーの算出と互いに異なることを含むことができる。
【００３７】
前記ロボットの関節部に必要な仮想重力を設定することは、前記ロボットの質量をｍとし、前記算出した補償力をｆとするとき、下記の数学式（２）を用いて、前記ロボットの関節部に必要な第１の仮想重力を設定し、前記ロボットの現在のＦＳＭの状態によって、前記関節部に必要な第２の仮想重力を設定することを含むことができる。
【００３８】
ｇｆ＝ｆ／ｍ （２）
前記重力補償トルクを算出することは、前記第１の仮想重力に対する重力補償トルク及び前記第２の仮想重力に対する重力補償トルクを算出し、前記ロボットのＦＳＭの状態によって、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクの比重を調節して、前記ロボットの関節部に提供される重力補償トルクを算出することを含むことができる。
【００３９】
前記ロボットの制御方法は、前記ロボットが安定した姿勢を維持するための目標ポーズ角度を設定し、前記目標ポーズ角度と前記測定されたポーズ角度との間の差を補償するための補償モーメントを算出することをさらに含むことができる。
【００４０】
前記ロボットの制御方法は、前記算出された補償モーメントを前記ロボットの関節部に提供するために必要な第１の目標トルクを算出し、前記第１の目標トルクと前記重力補償トルクを合算して、第２の目標トルクを算出することをさらに含み、前記目標トルクは、第２の目標トルクであるものとすることができる。
【００４１】
前記ロボットは、前記算出された第２の目標トルクに前記関節部の角速度に比例するダンピングトルクを加えて、第３の目標トルクを算出することをさらに含み、前記目標トルクは、前記第３の目標トルクであるものとすることができる。
【００４２】
前記ロボットの重心が地面に投影された点と前記ロボットの複数の足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点から近い脚に、より大きい値の補償モーメントを割り当てることによって、前記算出された補償モーメントを前記複数の脚に分配することをさらに含むことができる。
【００４３】
前記第１の目標トルクを算出することは、前記ロボットの足をベースリンクと見るヤコビアンを用いて、前記ロボットの複数の脚に分配された補償モーメントから前記複数の脚に必要な第１の目標トルクを算出することを含むことができる。
【発明の効果】
【００４４】
本発明によるロボット及びその制御方法によれば、重心の位置と速度を合成して得られるキャプチャポイントを歩行中に制御することによって、安全性を維持することができ、左右の脚に重心を移動させたり、または前進のために支持足（ｓｕｐｐｏｒｔ ｆｏｏｔ）の前へ重心を移すことが可能になる。
【００４５】
また、外部から力が作用して、キャプチャポイントが支持足から外れたときにも、キャプチャポイントに向かってスイング足（ｓｗｉｎｇ ｆｏｏｔ）を移して着地することになるので、倒れずに歩行を続けることができる。
【００４６】
また、本発明では、地面の傾斜と凹凸にかかわらず、ロボットの上体の姿勢を制御するので、歩行時にも傾斜面と凹凸面によって倒れずに安定的に歩行することができる。
【００４７】
また、本発明による歩行動作は、人間の歩行段階を考慮してＦＳＭを構成することができるので、人間の歩行と類似したかかとからつま先（ｈｅｅｌ-ｔｏ-ｔｏｅ）動作が可能で、自然な歩行が可能である。
【００４８】
また、膝を曲げずに歩行できるので、大きな歩幅の歩行が可能で、歩行に必要なエネルギーを効率的に使用することができる。
【図面の簡単な説明】
【００４９】
【図１】本発明の一実施例によるロボットの外観構成図である。
【図２】本発明の一実施例によるロボットの主要関節の構造を示す図である。
【図３】本発明の一実施例によるロボットの制御ブロック図である。
【図４】本発明の一実施例によるロボットのＦＳＭの状態ダイヤグラムである。
【図５】本発明の一実施例によるロボットの各ＦＳＭの状態での歩行動作を示す図である。
【図６】本発明の一実施例によるロボットのキャプチャポイントを示す図である。
【図７】本発明の一実施例によるロボットの下体を示す図である。
【図８】本発明の一実施例によるロボットの制御ブロック図である。
【図９】本発明の一実施例によるロボットの歩行制御方法に対するフローチャートである。
【図１０】並進運動の自由度に対するロボットの姿勢をキャプチャポイントで表すロボットの歩行制御方法に対するフローチャートである。
【図１１】本発明の一実施例による補償モーメントを左右の脚に分配する過程をさらに含むロボットの歩行制御方法に対するフローチャートである。
【発明を実施するための形態】
【００５０】
以下、添付の図面を参照して、本発明の実施例を詳細に説明する。
【００５１】
図１は、本発明の一実施例によるロボットの外観構成図で、図２は、図１に示したロボットの主要関節の構造を示す図である。
【００５２】
図１及び図２を参照すると、本発明の一実施例によるロボット１００は、人間と同様に、二つの脚１１０Ｌ、１１０Ｒによって直立移動する２足ロボットであって、胴体１０２、頭１０４、腕１０６Ｌ、１０６Ｒからなる上体１０１と、二つの脚１１０Ｌ、１１０Ｒからなる下体１０３と、を有する。
【００５３】
ロボット１００の上体１０１は、胴体１０２と、胴体１０２の上部に首１２０を介して連結された頭１０４と、胴体１０２の上部両側に肩１１４Ｌ、１１４Ｒを介して連結された二つの腕１０６Ｌ、１０６Ｒと、該二つの腕１０６Ｌ、１０６Ｒの末端にそれぞれ連結された手１０８Ｌ、１０８Ｒと、からなる。
【００５４】
ロボット１００の胴体１０２には、上体１０１が回転できるように、ヨー（ｙａｗ）方向の１自由度を有する腰関節部１５が設置される。
【００５５】
ロボット１００の下体１０３は、腰関節部１５の下側に連結されたペルヴィスリンク１６と、ペルヴィスリンク１６の下部両側に連結された二つの脚１１０Ｌ、１１０Ｒと、二つの脚１１０Ｌ、１１０Ｒの末端にそれぞれ連結された足１１２Ｌ、１１２Ｒと、からなる。
【００５６】
ここで、頭１０４、二つの腕１０６Ｌ、１０６Ｒ、二つの手１０８Ｌ、１０８Ｒ、二つの脚１１０Ｌ、１１０Ｒ、二つの足１１２Ｌ、１１２Ｒはそれぞれ、関節を通じて一定水準の並進運動及び回転運動の自由度を有する。
【００５７】
参照符号における“Ｒ”と“Ｌ”は、それぞれロボット１００の右側（ｒｉｇｈｔ）と左側（ｌｅｆｔ）を表し、ＣＯＧ（Ｃｅｎｔｅｒ Ｏｆ Ｇｒａｖｉｔｙ）は、ロボット１００の重心を表す。
【００５８】
また、ロボット１００の頭１０４には、周囲を撮影するカメラ４１と、ユーザの音声を入力するマイクロフォン４２とが設置され得る。
【００５９】
頭１０４は、首関節部２８０を介して上体１０１の胴体１０２と連結される。首関節部２８０は、ヨー方向（ｙａｗ、Ｚ軸回転）の回転関節２８１、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）の回転関節２８２、及びロール方向（ｒｏｌｌ、Ｘ軸回転）の回転関節２８３を含み、３自由度を有する。
【００６０】
首関節部２８０のそれぞれの回転関節２８１、２８２、２８３には、頭１０４の回転のためのモータ（例えば、電気モータ、油圧モータなどのアクチュエータ）が連結される。
【００６１】
ロボット１００の二つの腕１０６Ｌ、１０６Ｒはそれぞれ、上膊リンク３１、下膊リンク３２及び手３３を有する。
【００６２】
上膊リンク３１は、肩関節部２５０Ｌ、２５０Ｒを介して上体１０１に連結され、上膊リンク３１と下膊リンク３２は、肘関節部２６０を介して互いに連結され、下膊リンク３２と手３３は、手首関節部２７０を介して互いに連結される。
【００６３】
肩関節部２５０Ｌ、２５０Ｒは、上体１０１の胴体１０２の両側に設置され、二つの腕１０６Ｌ、１０６Ｒを上体１０１の胴体１０２に連結する。
【００６４】
肘関節部２６０は、ピッチ方向の回転関節２６１と、ヨー方向の回転関節２６２とを含み、２自由度を有する。
【００６５】
手首関節部２７０は、ピッチ方向の回転関節２７１と、ロール方向の回転関節２７２とを含み、２自由度を有する。
【００６６】
手３３には５個の指３３ａが設置される。それぞれの指３３ａには、モータによって駆動される多数の関節（図示せず）が設置され得る。指３３ａは、腕１０６の動きに連動して、物を把持したり、特定方向を指すような多様な動作を行う。
【００６７】
そして、胴体１０２の下側には、二つのヒップ関節部２１０を有するペルヴィスリンク１６が連結される。ペルヴィスリンク１６（ｐｅｌｖｉｓ ｌｉｎｋ）は人間の骨盤に該当する部分であって、ヒップ関節部２１０を通じて上体１０１の胴体１０２に連結される。
【００６８】
そして、ロボット１００の二つの脚１１０Ｌ、１１０Ｒはそれぞれ、大腿リンク２１と下腿リンク２２、足１１２Ｌ、１１２Ｒを有する。
【００６９】
大腿リンク２１は人間の太股に該当する部分であって、ヒップ関節部２１０を介してペルヴィスリンク１６に連結され、大腿リンク２１と下腿リンク２２は、膝関節部２２０を介して互いに連結され、下腿リンク２２と足１１２Ｌ、１１２Ｒは、足首関節部２３０を介して互いに連結される。
【００７０】
ヒップ関節部２１０は、ヨー方向（ｙａｗ、Ｚ軸周囲の回転）の回転関節２１１（ヒップヨージョイント）と、ピッチ方向（ｐｉｔｃｈ、Ｙ軸周囲の回転）の回転関節２１２（ヒップピッチジョイント）と、ロール方向（ｒｏｌｌ、Ｘ軸周囲の回転）の回転関節２１３（ヒップロールジョイント）とを含み、３自由度を有する。
【００７１】
膝関節部２２０は、ピッチ方向の回転関節２２１を含み、１自由度を有する。
【００７２】
足首関節部２３０は、ピッチ方向の回転関節２３１とロール方向の回転関節２３２とを含み、２自由度を有する。
【００７３】
二つの脚１１０Ｌ、１１０Ｒのそれぞれには、ヒップ関節部２１０、膝関節部２２０、足首関節部２３０に対して６個の回転関節が設けられるので、二つの脚１１０Ｌ、１１０Ｒ全体に対しては１２個の回転関節が設けられ、本発明の一実施例によるロボットは、６自由度を有する一対の脚を含む。
【００７４】
図示されていないが、ロボット１００には、各回転関節を駆動するモータなどのようなアクチュエータが設置され、このアクチュエータを適切に制御することによって、ロボット１００の多様な動作を具現することができる。アクチュエータの種類には制限がないが、以下で説明する本発明の一実施例では、アクチュエータとしてサーボモータを例に挙げて説明する。
【００７５】
ロボット１００は、制御動作（Ｃｏｎｔｒｏｌ Ａｃｔｉｏｎ）の実行のために、各制御動作に対応して各関節のトルク指令を算出し、算出されたトルク指令をトルクサーボに出力して、サーボモータを駆動させる。
【００７６】
先に検討した情報に基づいて、本発明の一実施例によるロボット１００の歩行動作について詳細に説明する。
【００７７】
図３には、本発明の一実施例によるロボット１００の制御ブロック図が示されている。図３を参照すると、本発明の一実施例によるロボット１００の制御は、ロボットの状態を測定するセンサ部３００と、ロボットの有限状態機械（Ｆｉｎｉｔｅ Ｓｔａｔｅ Ｍａｃｈｉｎｅ：ＦＳＭ）を予め格納し、ロボットのＦＳＭの状態を制御するＦＳＭ制御部３３０と、前記センサ部３００で測定されたロボットの状態を用いて、ロボットの現在の姿勢を獲得する運動学計算部３１０と、ロボットの目標姿勢を設定する目標設定部３２０と、前記設定された目標姿勢と前記獲得された現在の姿勢との間の差を用いて、前記ロボットがバランスを取るために必要な補償力を算出する補償力算出部３５０と、前記センサ部３００で測定された情報を用いて、前記ロボットがバランスを取るために必要な補償モーメントを算出する補償モーメント算出部３４０と、前記ロボットに対して予め設定された有限状態機械の現在の状態及び補償力算出部３５０で算出した補償力の大きさを用いて、前記ロボットの各関節に必要な仮想重力の大きさを設定する仮想重力設定部３６０と、前記仮想重力設定部３６０で設定された仮想重力と実際の重力に対する重力補償トルクを算出する重力補償トルク算出部３９０と、前記重力補償トルク算出部３９０で算出された重力補償トルクと前記補償モーメント算出部３４０で算出した補償モーメントに対する関節トルクとを合算して、前記ロボットの各関節に必要な目標トルクを算出する目標トルク算出部４００と、前記ロボットの各関節のトルク値が、前記目標トルク算出部４００で算出した目標トルク値になるように、各関節のトルクサーボを制御するサーボ制御部４２０と、を含む。
【００７８】
センサ部３００は、ロボットの各関節部の角度を検出する角度検出部３０１と、ロボットのポーズ角度を検出するポーズ検出部３０２と、足１１２が地面に接触したか否か、または足に加えられる荷重を検出する力／トルク検出部３０３と、ロボットの各関節のトルクを測定する関節トルク検出部３０４とを含む。
【００７９】
角度検出部３０１は、ロボットを構成する各関節部の角度を検出するもので、各関節部の各軸に設けられたモータの回転角度を検出する。ここで、各関節部の角度は、モータ（図示せず）の回転数を用いて検出することができ、モータの回転数は、各モータに連結された関節エンコーダ（Ｅｎｃｏｄｅｒ、図示せず）を通じて検出可能である。
【００８０】
ポーズ検出部３０２は、ＩＭＵ１７として具現することができ、ＩＭＵは、重力方向と慣性系に対するペルヴィスリンク１６の相対的角度を検出して、ロール方向（ｒｏｌｌ、Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）、ヨー方向（ｙａｗ、Ｚ軸回転）の姿勢情報を発生させる。具体的にＩＭＵは、ロール方向（ｒｏｌｌ、Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）、ヨー方向（ｙａｗ、Ｚ軸回転）の３個の軸の回転角度のうち少なくとも一つの回転角度を検出できる。以下、ポーズ検出部３０２で検出された角度をポーズ角度と称する。但し、ＩＭＵ１７がペルヴィスリンク１６の角度を検出することに限定されず、頭や胴体などの他の部位のリンクに対してポーズ角度を検出することも可能である。また、ロボット１００の姿勢、特に、上体の姿勢を測定するために、ＩＭＵの他にティルティング検出（Ｔｉｌｔｉｎｇ Ｄｅｔｅｃｔｉｏｎ）、ジャイロセンサ（Ｇｙｒｏ Ｓｅｎｓｏｒ）などを使用することもできる。
【００８１】
力／トルク検出部３０３は、多軸Ｆ／Ｔセンサ（Ｍｕｌｔｉ−Ａｘｉｓ Ｆｏｒｃｅ ａｎｄ Ｔｏｒｑｕｅ Ｓｅｎｓｏｒ）として具現することができ、多軸Ｆ／Ｔセンサ２４は、二つの脚１１０Ｌ、１１０Ｒにおいて、足１１２Ｌ、１１２Ｒと足首関節部２３０との間にそれぞれ設置されて、足１１２Ｌ、１１２Ｒから伝達される力の３方向成分（Ｆｘ、Ｆｙ、Ｆｚ）及びモーメントの３方向成分（Ｍｘ、Ｍｙ、Ｍｚ）を測定することによって、足１１２Ｌ、１１２Ｒが着地したか否か、または足１１２Ｌ、１１２Ｒに加えられる荷重を検出する。足１１２が地面に接触したか否かを検出するセンサとして、Ｆ／Ｔセンサ２４の他に、接触センサまたはこれに相応するセンサを使用することもできる。
【００８２】
運動学計算部３１０は、センサ部３００で測定したロボット１００の状態情報を用いて、ロボット１００の現在の姿勢を獲得する。ロボット１００の姿勢は、ロボット１００の位置と角度を含むことができ、これは、ロボット１００の二つの足の位置、重心の位置と速度、及びキャプチャポイントなどを用いて獲得することができる。
【００８３】
通常、ロボットの姿勢は、上体及び下体の全ての関節の角度として表すことができるが、本発明では、次のようにロボットの姿勢を表すことができる。ロボットの姿勢は、作業空間でロボットの並進運動の自由度及び回転運動の自由度に対するロボットの位置と角度として表すことができる。
【００８４】
このロボットの位置と角度は、ロボットのいずれか一点の位置と角度を用いて表すことができる。ロボットの位置は、３軸並進運動の自由度に対して順運動学を用いて、基準座標系においてロボットのいずれか一点のｘ、ｙ、ｚ軸の３次元座標として表すことができる。
【００８５】
また、ロボットの角度は、回転運動の自由度に対して順運動学、またはポーズ検出部３０２を通じて検出されたポーズ角度として表すことができる。したがって、本発明において一つの姿勢を取るために、どの関節でもその角度は唯一に決まるものではない。
【００８６】
ここで、ロボットの姿勢を構成する位置と角度のうち、位置を表すいずれか一点は、ロボットの重心であってもよく、またはキャプチャポイントであってもよい。または、脚及び腕を除外した部分に固定された一点を参照点としてロボットの位置を表すことができる。
【００８７】
キャプチャポイントに対して定義すれば、キャプチャポイントの通常の意味は、一定の高さを持つ物体が、現在与えられた位置と速度からみるとき、地面のどの地点を踏むと垂直に立つ平衡状態に到達するのかに関する問題におけるソリューションと言える。すなわち、ロボットのキャプチャポイントは、ロボットの現在の位置と速度に照らして、次の動作を行うとき、倒れずに垂直に立つことができる位置のことを意味する。
【００８８】
前述したキャプチャポイントの定義によれば、通常、水平面（すなわち、地面）上の点になるが、本発明におけるキャプチャポイントは３次元領域に拡張されることも可能である。３次元領域でのキャプチャポイントは、重心から、重心が移動する速度に比例した距離だけ、重心の移動方向に移動した点と言える。
【００８９】
３次元の並進自由度に対してロボットの位置を表す点が重心、キャプチャポイント、参照点のうちいずれか一つに一致する必要はなく、互いに異なる方式で表すこともできる。例えば、２次元水平方向の並進運動の自由度に対するロボットの位置は、キャプチャポイントの２次元水平成分と定義し、１次元垂直方向の並進運動の自由度に対するロボットの位置は、ヒップの高さと定義することができる。
【００９０】
運動学計算部３１０は、各関節の関節エンコーダの瞬時値を読み、予め知っている関節リンクのサイズと重量を用いて、ロボットベース座標系に対する重心の位置、ヒップ高さ、二つの足の位置及び二つの足が置かれた方向を計算できる。ロボット１００の現在の重心（ＣＯＧ）の位置は、胴体１０２に設けられ、この時、現在の重心の方向は、重力の方向に固定された慣性系の方向（Ｏｒｉｅｎｔａｔｉｏｎ）に従う。
【００９１】
ベース座標系と絶対座標系（ｇｌｏｂａｌ ｏｒ ｗｏｒｌｄ ｃｏｏｒｄｉｎａｔｅ ｓｙｓｔｅｍ）に対する変換行列を計算して、先に計算したベース座標に対する位置情報を絶対座標系に変換することができる。これを通じて、地面に置かれた足に対して重心の相対的移動軌跡を計算し、その速度を計算し、重心の速度と位置、ヒップ高さを計算できる。
【００９２】
３次元空間のキャプチャポイントは、ロボットの重心とロボットの速度を用いて求めることができる。ロボットの現在の位置でのキャプチャポイントは、下記の数学式（１）によって求めることができる。
【００９３】
ＣＰ＝ｄＣＯＧ＋ｗ*ｖＣＯＧ （１）
ＣＰは、キャプチャポイントの位置を表し、ｄＣＯＧは、重心（ＣＯＧ）の位置を表し、ｖＣＯＧは、重心の速度を表す。ｗは、ｗ＝√（ｈ／ｇ）を計算して求めることができ、ここで、ｈは、重心の地面からの高さで、ｇは重力加速度である。２次元水平面のキャプチャポイントは、上記で算出した３次元のキャプチャポイントの（ｘ、ｙ、ｚ）成分のうち（ｘ、ｙ）成分のみを取って求められる。
【００９４】
まとめると、運動学計算部３１０は、ロボット１００の複数の関節部の角度に基づいて重心を算出することができ、重心をロボットの位置として現在の姿勢を獲得することができる。
【００９５】
または、複数の関節部の角度に基づいて、重心の位置と速度を算出し、重心の位置と速度に基づいて、現在のキャプチャポイントを算出したあと、獲得された現在のキャプチャポイントをロボットの位置として現在の姿勢を獲得したり、または上体のポーズ角度及び複数の関節部の角度に基づいて、ヒップや上体の参照点の位置を算出し、算出された参照点をロボットの位置として現在の姿勢を獲得する。
【００９６】
目標設定部３２０は、ロボットの有限状態機械（ＦＳＭ）の現在の状態による目標姿勢を設定する。
【００９７】
ロボットの目標姿勢は、目標キャプチャポイント、目標ポーズ角度、目標ヒップ高さなどを用いて表すことができ、ここで、キャプチャポイントは多様な方法により表現することができるが、キャプチャポイント（ＣＰ）は、３次元空間で重心が移動する速度を反映して速度に対応する距離だけ移動した点の位置として表現することも可能である。
【００９８】
また、目標設定部３２０は、ロボットが倒れないようにするために、二つの足の支持領域の内部を重力方向に通る線上の一点を目標キャプチャポイントとして設定し、この目標キャプチャポイントをロボットの並進運動の自由度に対する目標姿勢として設定することも可能である。
【００９９】
ロボットの目標ポーズ角度は任意に設定できるが、ロボットの直立姿勢のために、ロボットの上体が重力方向と平行になるように設定し、これをロボットの回転運動の自由度に対する目標姿勢として設定することも可能である。
【０１００】
有限状態機械（ＦＳＭ）は、与えられた時間に現れることのできる有限個の状態を示す概念的なモデルであって、ロボットのＦＳＭは、設計者が任意に設定することができ、ロボットは予め設定された有限個の状態を循環的に有することになる。
【０１０１】
ＦＳＭ制御部３３０は、ロボット１００のＦＳＭを予め格納し、ロボット１００の現在のＦＳＭの状態を判断し、現在のＦＳＭの状態から次のＦＳＭの状態に遷移できるか否かを判断して、ロボット１００のＦＳＭを制御する。
【０１０２】
補償力算出部３５０は、運動学計算部３１０で獲得したロボット１００の現在の位置と、目標設定部３２０で設定したロボット１００の目標位置とを比較して、位置エラーを算出し、位置エラーに基づいて少なくとも一つの自由度に対して補償されるべき補償力を算出する。
【０１０３】
補償モーメント算出部３４０は、ロボット１００が直立姿勢を維持するようにするために必要な補償モーメントを算出し、ポーズ検出部３０２でペルヴィスリンクに対して測定した現在のポーズ角度と、目標設定部３２０で設定した目標回転角度との間の差から、ｘ、ｙ、ｚ方向の補償モーメントを算出し、ロボットのポーズ角度は、ペルヴィスリンクでない胴体（Ｔｒｕｎｋ）リンクや頭などのリンクに対して測定しても構わない。
【０１０４】
仮想重力設定部３６０は、ロボットの現在のＦＳＭの状態及び補償力算出部３５０で算出した補償力を用いて仮想重力の大きさを設定し、補償力から設定された仮想重力は、下記の数学式（２）で表すことができる。
【０１０５】
ｇｆ＝ｆ／ｍ （２）
ここで、ｇｆは、仮想重力を表し、ｆは、補償力算出部３５０で算出した補償力を表し、ｍは、ロボットの質量を表す。ロボットのＦＳＭの状態による仮想重力については後述する。
【０１０６】
重力補償トルク算出部３９０は、仮想重力設定部３６０で設定した仮想重力と実際の重力を補償するために、各関節に必要な重力補償トルクを算出し、重力補償トルクは、仮想重力加速度と実際の重力加速度の和、各関節の角度、各リンクの重量及び各リンク内の重心の位置を用いて算出できる。
【０１０７】
目標トルク算出部４００は、各関節に伝達されなければならないトルクの総和を算出し、姿勢の制御、すなわち、モーメント補償と重力補償を全て考慮しなければならない。したがって、補償モーメント算出部３４０で算出した補償モーメントを発生させるための第１の目標トルクを算出し、第１の目標トルクに重力補償トルクを加えて、第２の目標トルクを算出する。この時、６×６ヤコビアン行列を用いることができる。
【０１０８】
サーボ制御部４２０は、目標トルク算出部４００から、各関節に必要な第２の目標トルクの伝達を受け、各関節のトルク値が第２の目標トルクに到達するように各トルクサーボを制御する。
【０１０９】
以下、ロボット１００の姿勢を表す情報のうちロボット１００の位置は、キャプチャポイント及びヒップ高さを含むものとして、本発明の一実施例によるロボット１００の動作をより具体的に説明する。すなわち、３次元並進運動の自由度のうち２次元水平方向に対してはキャプチャポイントに基づき、垂直方向に対してはヒップ高さを参照点とする。
【０１１０】
本発明の一実施例によるロボット１００は、設計者によって予め設定されたＦＳＭによって動作し、目標設定部３２０で設定される目標姿勢のうち目標キャプチャポイントは、ロボット１００の現在のＦＳＭの状態によって異なるように設定される。以下、本発明の一実施例によるロボットのＦＳＭについて詳細に説明する。
【０１１１】
図４には、本発明の一実施例によるＦＳＭの状態遷移に関するダイヤグラムが示されている。当該実施例では、ＦＳＭが７つの状態を有するものとし、ＤＳ状態→Ｗ１状態→Ｗ２状態→Ｗ３状態→Ｗ４状態→Ｗ２状態→Ｗ３状態→Ｗ４状態→Ｗ２状態→．．．に循環したり、停止命令が入ると、Ｗ４状態から停止準備動作であるＷ２’状態→Ｗ３’状態を経てＤＳ状態に遷移する。
【０１１２】
ＦＳＭの各状態での目標ヒップ高さはいずれも同一であるとし、ｘ方向は、支持足の長さ方向であって、前方を正の方向とし、ｙ方向は、ｘ方向に対して上から見て９０度反時計方向とする。
【０１１３】
また、支持足は、ロボットの姿勢を維持させる足、すなわち、地面を踏んでいる足のことを意味し、スイング足は、移動のために上に持ち上げた足のことを意味する。
【０１１４】
図５に、図４に示されたＦＳＭの状態を有するロボット１００の歩行動作が示されている。歩行の開始をＤＳ状態とすると、ＤＳ状態は、ロボット１００の二つの脚が地面に着いているものとし、Ｗ１状態では、外観上はＤＳ状態と違いがないが、重心を片方の脚に移動し始め、当該実施例では、左脚１１０Ｌに重心を移すものとする。
【０１１５】
Ｗ２状態では、右脚１１０Ｒを持ち上げながら左脚１１０Ｌでボディーを支えて立っており、Ｗ３状態では、ボディーを進行方向に移動させながら右脚１１０Ｒを下げて、右足１１２Ｒを地面に着地させる。
【０１１６】
Ｗ４状態では、右足１１２Ｒが地面に着いた後、重心を左脚１１０Ｌに移動させ、再びＷ２状態に遷移されながら脚の方向のみが変わり、停止命令が入るまで、Ｗ２→Ｗ３→Ｗ４→Ｗ２→．．．の循環が続く。
【０１１７】
停止命令が入った場合には、Ｗ４状態からＷ２’状態に遷移し、Ｗ２’状態は、Ｗ２状態と同様に左脚１１０Ｌを持ち上げるが、前方に進むわけではないので、目標キャプチャポイントのｘ成分を、現在の支持足の中心にくるようにする。Ｗ３’状態は、Ｗ３状態と同様に左足１１２Ｌを着地するが、前進するわけではないので、右足１１２Ｒと平行な位置に着地するものとする。
【０１１８】
図４及び図５を参照すると、７つの各ＦＳＭの状態は一定の順序によって遷移され、所定の遷移条件を満足する時に次の状態に遷移される。各ＦＳＭの状態の動作内容及び遷移条件は設計者が任意に設定でき、以下、図４及び図５に示されたＦＳＭの状態について詳細に説明する。
【０１１９】
まず、ＤＳ状態は、ロボット１００の二つの足１１２Ｒ、１１２Ｌが地面に着いて停止している状態を示すものとし、この時、目標キャプチャポイントはロボットの二つの足がなす支持多角形の中心に置かれる。歩行命令が下るとＷ１状態に遷移する。
【０１２０】
Ｗ１状態は、目標キャプチャポイントがロボット１００の二つの足１１２Ｒ、１１２Ｌのうち、任意に選択された支持足に移動することになる状態であり、実際のキャプチャポイントが支持足の足の幅以内の安定領域内に進入すると、Ｗ２状態に遷移する。
【０１２１】
Ｗ２状態は、ロボット１００がスイング足を持ち上げる動作を行う状態であって、Ｗ２状態での目標キャプチャポイントのｘ成分は、支持足の中心から前の部分に時間によって前進する軌跡と設定され、ｙ成分は、支持足の中心線にくるように設定される。スイング脚を持ち上げる動作は、重力補償により制御され、詳細は後述する。Ｗ２状態で、現在のキャプチャポイントのｘ成分がしきい値を越え、スイング足の高さがしきい値を越えると、Ｗ３状態に遷移する。
【０１２２】
Ｗ３状態は、ロボット１００が着地のために膝を伸ばし、スイング足を下ろす動作を示し、この時、目標キャプチャポイントのｘ成分は、支持足の前部を越えて、スイング足が着地しなければならない位置まで時間によって増加する軌跡と設定され、ｙ成分は、スイング足が着地する位置へ時間によって移動するものと設定される。スイング足が地面に着いて、Ｆ／Ｔセンサにしきい値以上のｚ成分が感知されると、Ｗ４状態に遷移する。
【０１２３】
Ｗ４状態は、二つの足が地面に着いている状態であって、最後に着地した足を支持足とする。この時、目標キャプチャポイントのｘ、ｙ成分は、以前状態の目標キャプチャポイントの位置から新しい支持足の中心に、短時間内に連続的に移動する軌跡と設定される。現在のキャプチャポイントが支持足内に入ると、停止命令がない場合にはＷ２状態に遷移し、停止命令が下る場合にはＷ２’状態に遷移する。
【０１２４】
Ｗ２’状態はＷ２状態と類似した動作、すなわち、スイング足を持ち上げる動作を示すが、前進せずに停止するので、目標キャプチャポイントのｘ成分は支持足の中心に固定する。スイング足の高さがしきい値以上に到達すると、Ｗ３’状態に遷移する。
【０１２５】
Ｗ３’状態はＷ３状態と類似した動作、すなわち、スイング足の着地のために膝を伸ばし、足を下ろす動作を示すが、前進せずに停止するので、目標キャプチャポイントのｘ成分を支持足の前方へ送らずに支持足の位置に置き、目標キャプチャポイントのｙ成分は、反対側の足に向かって送るように設定する。スイング足が地面に着いて、Ｆ／Ｔセンサのｚ成分がしきい値を越えると、ＤＳ状態に遷移する。
【０１２６】
上述したＦＳＭの状態は、本発明の一実施例に過ぎず、ロボット１００の歩行を可能にするものであれば、その動作の内容やＦＳＭの状態の個数などは構わない。
【０１２７】
ただし、上述した例のようにＦＳＭの状態を設定する場合、人間の歩行動作に近く、最初にかかとが地面に着き、最後につま先が地面から離れるｈｅｅｌ-ｔｏ-ｔｏｅ動作が発生して、有効な脚の長さがさらに長くなり、ｈｅｅｌ-ｔｏ-ｔｏｅ動作をしない場合より大きな歩幅で自然に歩行することができ、逆運動学を利用しないため、膝を曲げた状態で歩行しなくてもよいので、逆運動学を用いた場合より大きな歩幅の歩行が可能になる。
【０１２８】
上述したＦＳＭの状態に基づいて、補償力算出部３５０及び仮想重力設定部３６０の制御動作について詳細に説明する。
【０１２９】
図６に、本発明の一実施例によるロボット１００が示されている。図６を参照すると、運動学計算部３１０は、センサ部３００で検出したロボット１００の状態情報を用いて、ロボット１００の重心の位置とその速度を算出し、算出された重心の位置とその速度を用いて、ロボット１００の現在のキャプチャポイントを算出する。また、算出された重心に基づいてロボット１００の現在のヒップ高さを算出する。
【０１３０】
キャプチャポイントは地面の上の点と見なされるので、ｘ、ｙ成分を表し、ヒップ高さはｚ成分を表す。
【０１３１】
補償力算出部３５０は、目標キャプチャポイントの位置と現在のキャプチャポイントの位置との間の差から、ｘ、ｙ方向の補償力を算出し、目標ヒップ高さと現在のヒップ高さの差から、ｚ方向の補償力を算出する。
【０１３２】
図６に示されたように、補償力を算出する際に、仮想のスプリングを用いることができる。目標キャプチャポイントの位置と現在のキャプチャポイントの位置との間に、スプリング定数ｋを持つ仮想のスプリングがあると仮定するときに作用する力ｆは、下記の数学式（３）で表すことができる。
【０１３３】
ｆ１＝ｋ*ｅ１ ｆ２＝ｋ*ｅ２ （３）
ｅ１は，目標キャプチャポイントと現在のキャプチャポイントとの間の差を表す座標であって、ｅ１＝（ｘ’、ｙ’）-（ｘ、ｙ）で表すことができ、ｆ１＝（ｆｘ、ｆｙ）になる。（ｘ’、ｙ’）は、目標設定部３２０で設定した目標キャプチャポイントの位置座標で、前述したように、ロボット１００の現在のＦＳＭの状態によって、目標キャプチャポイントの位置が変わる。（ｘ、ｙ）は、運動学計算部３１０で計算した現在のキャプチャポイントの位置座標である。
【０１３４】
ｅ２は、目標設定部３２０で設定した目標ヒップ高さと運動学計算部３１０で計算した現在のヒップ高さとの間の差を表し、ｆ２＝ｆｚになる。これによって、ｘ、ｙ、ｚ方向の補償力が算出される。
【０１３５】
上述したように、ロボットの歩行中に各歩行動作に合うキャプチャポイントを目標に設定し、目標キャプチャポイントに足を踏み出すようにロボットを制御することで、ロボットが倒れずに安定的に歩行することができる。
【０１３６】
ＦＳＭの各状態ごとに、目標設定部３２０で設定する目標キャプチャポイントと、運動学計算部３１０で算出する現在のキャプチャポイントがそれぞれ異なるので、補償力算出部３５０は、ロボットの現在のＦＳＭの状態によって補償力を算出する。
【０１３７】
仮想重力設定部３６０では、ロボットのＦＳＭの状態によって必要な仮想重力及び補償力算出部３５０で算出した補償力に対する仮想重力を設定する。具体的には、第１の仮想重力設定部３６１では、補償力算出部３５０で算出した補償力に対する仮想重力（ｓｕｐｐｏｒｔ ｇｒａｖｉｔｙ）である第１の仮想重力、すなわち、キャプチャポイントの位置を制御するために必要な仮想重力を設定し、第２の仮想重力設定部３６２では、ロボット１００のＦＳＭの状態による仮想重力（ｌｉｆｔ ｇｒａｖｉｔｙ）である第２の仮想重力を設定する。
【０１３８】
仮想重力の設定を説明するに先立ち、サポート重力は、ロボット１００の足が地面に着いており、ボディーを支持し、その上に姿勢を制御する場合に必要な仮想重力とし、リフト重力は、ロボット１００の足が空中に浮いており、ヒップから足を持ち上げたり、または下ろす場合に必要な仮想重力とする。
【０１３９】
第１の仮想重力設定部３６１では、前述した数学式（２）によって、補償力に対する仮想重力を設定し、ここでの仮想重力はサポート重力に該当する。
【０１４０】
第２の仮想重力設定部３６２で行うＦＳＭの状態による仮想重力の設定は、前述したＦＳＭの７つの状態を例に挙げて説明する。
【０１４１】
ロボット１００のＦＳＭがＷ２状態にある場合、スイング足を持ち上げる制御が必要なので、スイング脚に対して仮想重力の比重が瞬間的にサポート重力は０になり、リフト重力は１になるようにする。この時、リフト重力のｚ成分は、支持足とスイング足との高さ差を目標値に維持するように設定し、ｘ成分は、前進方向のスイングができるようにバイアス（ｂｉａｓ）されている。ｙ成分は、スイング脚を用いて左右のバランスを取ることができるように、補償力算出部３５０で計算した補償力のｙ成分を反映して設定し、二つの脚１１０Ｌ、１１０Ｒが衝突しないように、二つの脚１１０Ｌ、１１０Ｒの間隔によるフィードバックを加える。
【０１４２】
ロボット１００のＦＳＭがＷ３状態にある場合、スイング脚が着地のために膝を伸ばし、スイング足を下ろす動作を行うように、リフト重力を設定する。この時、サポート重力の比重は０、リフト重力の比重は１になる。リフト重力のｚ成分は、スイング足が地面に向かって下りてくるように（−）方向とし、ｙ成分は、スイング足がキャプチャポイントに向かうように現在のスイング足の位置とキャプチャポイントの位置との差をフィードバックし、最小限の左右の足の間隔は維持するようにする。ｘ成分は、スイング足の目標位置との差によって決定し、スイング足の目標位置のｘ成分は、スイング足が前進するように（＋）方向に増加させ、目標歩幅を達成できるように、現在の足の位置と目標歩幅との間の差を共にフィードバックして設定する。
【０１４３】
ロボット１００のＦＳＭがＷ４状態にある場合、スイング足を持ち上げるとき、つま先が最後に地面に着いているつま先離れ（Ｔｏｅ-ｏｆｆ）動作が行われるように仮想重力を設定する。足首ピッチのサポート重力に対して１の比重をおいて、ロボット１００の体重が支持足に偏ることに比例して、サポート重力のｚ成分を増加させる。スイング脚の膝とヒップは１の比重にサポート重力を与えて、体重の移動を助けるようにし、次のＦＳＭの状態でスイング足を持ち上げることに備えて、体重が支持足に偏ることによって、サポート重力の比重を減らし、リフト重力の比重を増やす。
【０１４４】
ロボット１００のＦＳＭがＷ２’状態にある場合、スイング足のリフト重力のｘ成分が、支持足と平行な位置にくるようにフィードバック制御する。
【０１４５】
ロボット１００のＦＳＭがＷ３’状態にある場合、スイング足が支持足の横に一定の間隔をおいて下りてくるように、リフト重力のｘ、ｙ成分をフィードバックしながら、ｚ成分を時間によって減少させて、自重を用いて地面に着地するようにする。
【０１４６】
重力補償トルク算出部３９０では、第１の仮想重力設定部３６１と第２の仮想重力設定部３６２で設定した仮想重力及び実際の重力に対する重力補償トルクを算出する。重力補償トルクは、下記の数学式（４）によって算出される。
【０１４７】
【数１】
ここで、τは、各関節に対する重力補償トルクを表し、ｇは、重力ベクトル、Ｑは、３×３マトリックスで、関節軸に対するクロス積（ｃｒｏｓｓ ｐｒｏｄｕｃｔ）演算を表し、ｍは、各リンクの質量、ｒは、各リンクの重心オフセット、Ｒは、関節間の相対的座標変換の回転マトリックス、ｐは、各関節間の相対的位置ベクトルを表す。
【０１４８】
重力補償は、主体になる関節と補償しようとする対象リンクとの相対的位置関係によって、２種類に分類され、対象リンクが関節よりも上側にある場合には、サポート重力に対する重力補償が行われ、対象リンクが関節よりも下側にある場合には、リフト重力に対する重力補償が行われる。
【０１４９】
数学式（４）で下付き文字０がベースリンクを表すものと見ると、サポート重力に対する重力補償の場合、ロボットの足関節に対する重力補償トルクをτ０とし、順次に増加してヒップ関節に対する重力補償トルクをτ５とする。
【０１５０】
リフト重力に対する重力補償の場合、サポート重力と反対に、ヒップ関節に対する重力補償トルクをτ０とし、足関節に対する重力補償トルクをτ５とする。
【０１５１】
一つの関節に適用される重力補償は、サポート重力とリフト重力の二つの成分があり、ロボット１００のＦＳＭの状態によって、サポート重力とリフト重力の比重を調節する。すなわち、ロボット１００の特定関節に必要な重力補償トルクは、下記の数学式（５）のように表すことができる。
【０１５２】
τ＝ａ*（サポート重力補償）＋ｂ*（リフト重力補償） （５）
ここで、τは、特定関節の重力補償トルクを表し、ａとｂは、０から１の間の値を持つ実数で、ａとｂの値を調節して、ロボット１００のＦＳＭの状態に合わせて各関節に対する重力補償トルクを計算する。
【０１５３】
ロボット１００の二つの脚に対して重力補償トルクを計算するとき、上体の重量を二つの脚１１０Ｌ、１１０Ｒに分配することができる。したがって、本発明の一実施例によるロボット１００は重量分配部３８０をさらに含むことができる。
【０１５４】
図７に、いずれか一方に傾いたロボットが示されており、図８には、重量分配部とトルク補償部をさらに含むロボットの制御ブロック図が示されている。
【０１５５】
ロボット１００の二つ足１１２Ｌ、１１２Ｒをベースと見て、その上側リンクの重量を考慮した重力補償を足首、膝、ヒップ関節に対して計算する方法は、既存のロボット制御で使用する方法と同一であるが、本発明の一実施例では、ペルヴィス１６及びその上にある胴体に該当するトランク１０２部分の重量は、左右の脚１１０Ｌ、１１０Ｒが分けて負担しなければならない。
【０１５６】
重量分配部３８０は、ロボット１００の重心に対する地面の投影点と、ロボット１００の二つの足１１２Ｌ、１１２Ｒとの間の距離の比率であるｄＲ、ｄＬを用いて、重心の投影点に近い脚が、ペルヴィス１６とトランク１０２の重量をより多く支えるようにする。
【０１５７】
図７を参照すると、右に傾いたロボット１００の重心は、右に偏ることになり、その投影点は右足に近くなる。したがって、ペルヴィス１６とトランク１０２の重量は、右脚１１０Ｒにより多くかかることになり、その比率は、下記の数学式（６）で表すことができる。
【０１５８】
ＷＬ＝ｄＲ／（ｄＲ＋ｄＬ）
ＷＲ＝１−ＷＬ （６）
ここで、ＷＬは、左脚１１０Ｌにかかる重量の比率であり、ＷＲは、右脚１１０Ｒにかかる重量の比率である。重量分配部３８０は、ペルヴィス１６とトランク１０２の質量に、ＷＬが乗算された分だけの重量を左脚１１０Ｌにかけ、ＷＲが乗算された分だけの重量を右脚１１０Ｒにかけるものとする。
【０１５９】
補償モーメント算出部３４０では、ポーズ検出部３０２で測定したペルヴィスリンク１６のロール、ピッチ、ヨー方向の回転角度と目標回転角度との間の差から補償モーメントを算出し、ロール、ピッチ、ヨー方向の３軸に仮想のスプリングが存在すると仮定して、下記の数学式（７）から各関節に必要な補償モーメントを計算することができる。
【０１６０】
Ｍ＝ｋ１*ｅ３ （７）
ｋ１は、仮想のスプリング定数を表し、ｅ３は、ポーズ検出部３０２で測定した回転角度と目標回転角度との間の差を表す。
【０１６１】
目標トルク算出部４００では、補償モーメント算出部３４０で算出した補償モーメント及び重力補償トルク算出部３９０で算出した重力補償トルクを全て反映して、ロボット１００の各関節に必要なトルクを計算する。
【０１６２】
補償モーメントに対する関節トルクを計算するに先立ち、補償モーメントをロボットの左右の足に分配することができる。図８に示されたように、本発明の一実施例によるロボット１００は、補償モーメント分配部３７０をさらに含むことができる。
【０１６３】
補償モーメント分配部３７０は、重量分配部３８０と同様に、ロボット１００の重心の地面に対する投影点と、ロボット１００の二つの足１１２Ｌ、１１２Ｒとの間の距離の比率を用いて、補償モーメントを左右の脚１１０Ｌ、１１０Ｒに分配する。
【０１６４】
補償モーメント算出部３４０で算出した補償モーメントに、上記の数学式（６）で求めたＷＲを乗算すると、右脚１１０Ｒに分配される補償モーメントＭＲを得ることができ、補償モーメント算出部３４０で算出した補償モーメントに、上記の数学式（６）で求めたＷＬを乗算すると、左脚１１０Ｌに分配される補償モーメントＭＬを得ることができる。これを数学式に表すと、下記の数学式（８）の通りである。
【０１６５】
ＭＬ＝ＷＬ＊Ｍ, ＭＲ＝ＷＲ＊Ｍ、 （８）
ここで、ＷＬ＝ｄＲ／（ｄＲ＋ｄＬ）、ＷＲ＝１−ＷＬで、ｄＬ、ｄＲは、重心の投影点と二つの足との間の距離である。
【０１６６】
目標トルク算出部４００は、下記の数学式（９）によって、ロボット１００の左右の脚に、補償モーメント分配部３７０で算出した各補償モーメントを発生させるために必要な関節トルクである第１の目標トルクを計算する。
【０１６７】
【数２】
ここで、ＪＴは、左右の脚１１０Ｌ、１１０Ｒからロボット１００の重心の位置に対するヤコビアン行列であり、デカルト空間（Ｃａｒｔｅｓｉａｎ Ｓｐａｃｅ）上のモーメントを関節トルクに変換するために使用した。ロボット１００の左右の足をベースと見て、ロボット１００の重心をペルヴィスリンク１６上に存在するエンドエフェクタ（ｅｎｄ-ｅｆｆｅｃｔｏｒ）と見るときの６×６ヤコビアン行列Ｊを計算する。
【０１６８】
目標トルク算出部４００は、数学式（９）で算出した第１の目標トルクに、重力補償トルク算出部３９０で算出した重力補償トルクを加えて、第２の目標トルクτ２、L、τ２、Ｒを算出する。便宜上、第２の目標トルクをτ２、L、τ２、Ｒで表したが、第２の目標トルクは、左右の脚の各関節（合計１２個の関節）ごとに算出される。
【０１６９】
ロボット１００の各関節に、速度に比例して抵抗するダンピングトルクを加えると、より安定した歩行を具現できる。図８を参照すると、本発明の一実施例によるロボット１００は、トルク補償部４１０をさらに含むことができる。
【０１７０】
トルク補償部は、目標トルク算出部４００で算出した第２の目標トルクに各関節のダンピングトルクを加えて、第３の目標トルクを算出する。これは下記の数学式（１０）で表すことができる。
【０１７１】
τ３、Ｌ＝τ２、Ｌ＋ｄ*ｗ τ３、Ｒ＝τ２、Ｒ＋ｄ*ｗ （１０）
ここで、τ３、Ｌ、τ３、Ｒはそれぞれ、左右の脚の第３の目標トルクを表し、ｄは、ダンピング係数、ｗは、関節の角速度を表す。便宜上、第３の目標トルクを左、右に関するものに区分したが、第２の目標トルクが各関節ごとに計算されるので、第３の目標トルクも各関節に対して計算される。
【０１７２】
サーボ制御部４２０は、トルク補償部から最終目標トルクである第３の目標トルクの伝送を受け、ロボット１００の各関節のサーボモーターが第３の目標トルクを発生させるように、トルクサーボを制御する。各関節のトルクサーボは、関節トルク検出部３０４から各関節の現在の出力トルク値を受信し、第３の目標トルクと測定された現在のトルク値との間の差だけのトルクをさらに発生させるようにするトルク制御信号を、各関節の駆動回路に送ることができる。
【０１７３】
上述した本発明の一実施例によるロボット１００の各制御ブロックの機能を簡単に整理すると、センサ部３００では、ロボット１００のポーズ角度（ロール、ピッチ、ヨー方向の角度として表現可能）、各関節の回転角度及び出力トルク値などを測定し、運動学計算部３１０では、センサ部３００で測定したロボットの状態情報を用いて、重心の位置、速度、ヒップ高さを計算し、計算された重心の位置と速度を合成して、ロボットの現在のキャプチャポイントの位置を算出する。
【０１７４】
ＦＳＭ制御部３３０は、ロボットの動作を示す有限個の状態を格納し、現在のロボットのＦＳＭの状態を判断し、現在の状態から次の状態に遷移される条件を満足するか否かを決定して、条件を満足する場合、ロボットの動作を次のＦＳＭの状態に遷移させるために、各制御ブロックに信号を送る。
【０１７５】
目標設定部３２０は、ＦＳＭ制御部３３０で判断した現在のＦＳＭの状態によって目標キャプチャポイントを設定し、ロボット１００の目標ヒップ高さ及びペルヴィスリンク１６の目標ポーズ角度を設定する。
【０１７６】
補償力算出部３５０は、目標キャプチャポイントと現在のキャプチャポイントとの間の差、及び目標ヒップ高さと現在のヒップ高さとの間の差を用いて、補償力を計算し、補償モーメント算出部３４０は、ペルヴィスリンク１６の目標ポーズ角度と現在のポーズ角度との間の差を用いて、補償モーメントを計算する。
【０１７７】
第１の仮想重力設定部３６１は、補償力算出部３５０で算出した補償力に対する第１の仮想重力を設定し、第２の仮想重力設定部３６２は、ＦＳＭ制御部３３０で判断したロボット１００の現在のＦＳＭの状態による第２の仮想重力を設定する。
【０１７８】
重量分配部３８０は、ロボット１００の重心と左右の脚１１０Ｌ、１１０Ｒとの間の距離の比率を用いて、ロボット１００の上体の重量を左右の脚１１０Ｌ、１１０Ｒにそれぞれ分配し、重力補償トルク算出部３９０は、左右の脚１１０Ｌ、１１０Ｒに分配された上体の重量を考慮して、第１の仮想重力と第２の仮想重力を補償するための重力補償トルクを算出する。
【０１７９】
補償モーメント分配部３７０は、重量分配部３８０と同じ比率を用いて、補償モーメント算出部３４０で算出した補償モーメントをロボット１００の左右の脚１１０Ｌ、１１０Ｒに分配し、目標トルク算出部４００では、左右の脚１１０Ｌ、１１０Ｒに分配された補償モーメントを考慮して、左右の脚１１０Ｌ、１１０Ｒの各関節に補償モーメントを発生させるための第１の目標トルクを算出する。そして、第１の目標トルクに重力補償トルクを合算して、第２の目標トルクを算出する。
【０１８０】
トルク補償部４１０は、第２の目標トルクにダンピングトルクを合算して、第３の目標トルクを算出し、サーボ制御部４２０は、各関節に第３の目標トルクを発生させるために必要な制御信号を各トルクサーボに伝送する。
【０１８１】
以下、本発明の一実施例によるロボットの歩行制御方法について詳細に説明する。
【０１８２】
図９は、本発明の一実施例によるロボットの制御方法のフローチャートである。
【０１８３】
ロボットの上体のポーズ角度及び複数の関節部の角度を検出（５０１）し、、ポーズ角度及び複数の関節部の角度に基づいて現在の姿勢を獲得（５０２）する。ロボットの現在の姿勢は、前述したように並進運動の自由度に対するロボットの現在の位置、及び回転運動の自由度に対するロボットの現在のポーズ角度として表すことができる。
【０１８４】
次に、複数の並進運動の自由度のうち少なくとも一つの並進運動の自由度に基づいた姿勢及び回転運動の自由度に基づいた姿勢と、ロボットの現在のＦＳＭの状態によって設定された目標姿勢とを比較して、姿勢エラーを算出（５０３）する。
【０１８５】
この時、検出されたポーズ角度及び複数の関節部の角度に基づいて、現在の重心の位置を少なくとも一つの並進運動の自由度に対して検出し、少なくとも一つの並進運動の自由度に対して検出された現在の重心と予め設定された目標重心とを比較して、重心エラーを算出する。すなわち、少なくとも一つの自由度に対する重心エラーを算出する。
【０１８６】
ここで、重心エラーは、少なくとも一つの自由度に対する姿勢エラーとして用いられる。
【０１８７】
または、検出されたポーズ角度及び複数の関節部の角度に基づいて、現在の重心の位置と速度を少なくとも一つの自由度に対して検出し、検出された現在の重心の位置と速度に基づいて、現在のキャプチャポイントを少なくとも一つの自由度に対して獲得し、現在のキャプチャポイントと予め設定されたキャプチャポイントとを比較して、キャプチャポイントエラーを少なくとも一つの自由度に対して算出し、算出されたキャプチャポイントエラーを少なくとも一つの自由度の姿勢エラーとして用いることも可能である。
【０１８８】
また、検出されたポーズ角度及び複数の関節部の角度に基づいて、ヒップまたは上体のいずれか一つの点の位置を少なくとも一つの自由度に対する現在の参照点として獲得し、現在の参照点と予め設定された目標参照点との位置を比較して、参照点位置エラーを少なくとも一つの自由度に対して算出し、参照点位置エラーを少なくとも一つの自由度の姿勢エラーとして用いることも可能である。
【０１８９】
なお、残りの並進運動の自由度の姿勢エラーの算出は、少なくとも一つの並進運動の自由度の姿勢エラーの算出と同一または異なってもよい。
【０１９０】
回転運動の自由度に対する姿勢エラーは、ロボットの現在のポーズ角度と目標ポーズ角度との差から算出することができる。
【０１９１】
そして、算出された姿勢エラーに基づいて、少なくとも一つの並進運動の自由度に対する補償力を算出し、回転運動の自由度に対する補償モーメントを算出する（５０４）。
【０１９２】
算出された補償力に対する第１の仮想重力を設定し、ロボットの現在のＦＳＭの状態によって第２の仮想重力を設定する（５０５）。
【０１９３】
そして、第１の仮想重力、第２の仮想重力及び実際の重力を補償するために必要な重力補償トルクを算出する（５０６）。
【０１９４】
ロボットの複数の関節部に補償モーメントを発生させるために必要なトルクと、前記算出された重力補償トルクとを合算して、最終目標トルクを算出する（５０７）。
【０１９５】
最後に、最終目標トルクを複数の関節部に出力（５０８）して、ロボットの均衡を制御する。
【０１９６】
図１０には、並進運動の自由度に対するロボットの姿勢をキャプチャポイントとして表すロボットの歩行制御方法に関するフローチャートが示されている。図１０を参照すると、まず、センサ部３００でロボットの現在の状態情報を測定する（５１０）。ロボットの現在の状態情報は、ペルヴィスリンク１６のＩＭＵ１７で測定したポーズ角度、ロボットの足が地面に着いているか否か、各関節のトルク、各関節の角度などを含むことができる。
【０１９７】
そして、運動学計算部３１０で、ロボットの現在の状態情報を用いて、ロボットの現在の重心の位置及び速度を計算し、計算された重心の位置及び速度を用いて、下記の数学式（１）によって、ロボットの現在のキャプチャポイントの位置を３次元空間に対して得る（５２０）。
【０１９８】
ＣＰ＝ｄＣＯＧ＋ｗ*ｖＣＯＧ （１）
ＣＰは、キャプチャポイントを表し、ｄＣＯＧは、重心を表し、ｖＣＯＧは、重心の速度を表す。ｗは、ｗ＝√（ｈ／ｇ）を計算して求めることができ、ここで、ｈは、重心の地面からの高さで、ｇは重力加速度である。キャプチャポイントを２次元水平面（あるいは地面）に制限する場合、数学式（１）で求めたＣＰの座標（ｘ、ｙ、ｚ）から（ｘ、ｙ）のみを取って、地面上の２次元キャプチャポイントを求めることもできる。
【０１９９】
３軸並進運動の自由度に対するロボットの位置をキャプチャポイントとして表す場合、３次元キャプチャポイント（数学式（１））を使用し、ｚ軸を除外した２軸並進運動の自由度に対するロボットの位置をキャプチャポイントとして表す場合、前記２次元キャプチャポイントを使用することができる。また、この場合には、ｚ軸並進運動の自由度に対するロボットの位置は、ロボットのヒップ高さなどのような、どの参照点の位置として表すことができる。
【０２００】
そして、目標設定部３２０で、ロボットの現在のＦＳＭの状態によって目標キャプチャポイントを設定し、目標ポーズ角度を設定する（５３０）。場合によって、目標ヒップ高さも設定することができ、目標ヒップ高さは任意に設定できるが、ＦＳＭの各状態によって一定に設定することが好ましく、目標ポーズ角度も任意に設定できるが、ロボットの上体が直立姿勢を維持するように設定することが好ましい。
【０２０１】
そして、補償力算出部３５０で、目標キャプチャポイントと現在のキャプチャポイントとの間の差を用いて、各関節に必要な補償力を算出する（５４０）。キャプチャポイントを地面の上の点と見なす場合には、目標キャプチャポイントと現在のキャプチャポイントとの間の差から、ｘ、ｙ方向の補償力を算出することができ、ヒップ高さの間の差から、ｚ方向の補償力を算出することができる。
【０２０２】
補償力を算出する際に、目標キャプチャポイントの位置と現在のキャプチャポイントの位置との間に仮想のスプリングがあるものと見なし、下記の数学式（３）を用いることができる。
【０２０３】
ｆ＝ｋ*ｅ （３）
ｅは、目標キャプチャポイントと現在のキャプチャポイントとの間の差を表す座標で、ｅ＝（ｘ’、ｙ’、ｚ’）-（ｘ、ｙ、ｚ）で表すことができ、（ｘ’、ｙ’、ｚ’）は、目標キャプチャポイントの位置座標で、（ｘ、ｙ、ｚ）は、現在のキャプチャポイントの位置座標である。ｋは、仮想のスプリング定数を表す。
【０２０４】
また、補償モーメント算出部３４０では、目標ポーズ角度と現在のポーズ角度との間の差を用いて、補償モーメントを算出する（５５０）。この時、ペルヴィスリンクのロール、ピッチ、ヨーの３軸に対して仮想のスプリングがあると仮定して、下記の数学式（７）によって補償モーメントを算出することができる。
【０２０５】
Ｍ＝ｋ１*ｅ１ （７）
ｋ１は、仮想のスプリング定数を表し、ｅ１は、ＩＭＵで測定したポーズ角度と目標ポーズ角度との間の差を表す。
【０２０６】
補償力の算出と補償モーメントの算出とは前後関係を有するものではなく、５４０段階と５５０段階は、互いに取り替えてもよい。
【０２０７】
そして、第１の仮想重力設定部３６１で、補償力に対する第１の仮想重力を設定し、第２の仮想重力設定部３６２で、ロボット１００の現在のＦＳＭの状態によって第２の仮想重力を設定する（５６０）。
【０２０８】
第１の仮想重力を設定する際に下記の数学式（２）を用いることができる。
【０２０９】
ｇｆ＝ｆ／ｍ （２）
ここで、ｇｆは、第１の仮想重力を表し、ｆは、補償力算出部３５０で算出した補償力を表し、ｍは、ロボットの質量を表す。
【０２１０】
第２の仮想重力は、ＦＳＭの状態によって変わり、第２の仮想重力を設定する方法は、先に本発明の一実施例によるロボットにおいて説明した通りである。
【０２１１】
そして、重力補償トルク算出部３９０では、第１の仮想重力、第２の仮想重力及び実際の重力を補償するために必要な重力補償トルクを算出する（５７０）。重力補償トルクを計算する一般的な式は、前述した数学式（４）で表現することができる。
【０２１２】
【数３】
ここで、τは、各関節に対する重力補償トルクを表し、ｇは、補償しようとする重力ベクトル、Ｑは、３×３マトリックスで、関節軸に対するクロス積（ｃｒｏｓｓ ｐｒｏｄｕｃｔ）演算を表し、ｍは、各リンクの質量、ｒは、各リンクの重心オフセット、Ｒは、関節間の相対的座標変換の回転マトリックス、ｐは、各関節間の相対的位置ベクトルを表す。
【０２１３】
そして、目標トルク算出部４００で、各関節に補償モーメントを発生させるために必要な第１の関節トルクを算出し、第１の関節トルクに重力補償トルクを合算して、第２の関節トルクを算出する（５８０）。
【０２１４】
最後に、サーボ制御部４２０で、各関節に第２の関節トルクを発生させるために、各トルクサーボを制御する（５９０）。
【０２１５】
本発明の一実施例によるロボットの歩行制御方法では、重力補償トルクを算出する際に、ロボット１００の上体の重量を左右の脚に分配する過程をさらに含むことができ、補償モーメントに対する第１の関節トルクを算出する際に、補償モーメントをロボットの左右の脚に分配する過程をさらに含むことができる。
【０２１６】
前記実施例によるロボットの歩行制御方法に関するフローチャートが図１１に示されている。下記の実施例では、並進運動の自由度に対するロボットの位置をキャプチャポイント及びヒップ高さとして表す。
【０２１７】
図１１を参照して、重量分配及び補償モーメント分配を重点的に説明する。センサ部３００で、ロボットの現在の状態情報を測定し（６１０）、運動学計算部３１０で、測定された状態情報を用いて、ロボットの現在の重心の位置、速度及びヒップ高さを計算する（６２０）。
【０２１８】
目標設定部３２０で、ロボットの現在のＦＳＭの状態に基づいて目標キャプチャポイントを設定し、目標ヒップ高さ及び目標ペルヴィスリンク回転角を設定する（６３０）。
【０２１９】
補償力算出部３５０で、目標キャプチャポイントの位置と現在のキャプチャポイントの位置との間の差から、ｘ、ｙ方向の補償力、及び目標ヒップ高さと現在のヒップ高さとの間の差からｚ方向の補償力を算出し、目標ポーズ角度と現在のポーズ角度との間の差から、補償モーメントを算出する（６４０）。
【０２２０】
仮想重力設定部３６０で、補償力に対する第１の仮想重力を設定し、ロボットのＦＳＭの状態による第２の仮想重力を設定する（６５０）。
【０２２１】
そして、重量分配部３８０で、ロボットのペルヴィスリンクとその上の胴体（以下、トランクという。）の重量を左右の脚に分けて分配する（６６０）。先に検討した図７を再び参照すると、ロボット１００が、いずれか一方に傾いている場合、その重心は、胴体が傾いた方向に偏ることになる。したがって、重心と近い側の足により多くの重量がかかり、ロボット１００の重心を地面に投影した投影点とロボット１００の左足との間の距離の比率をｄＬとし、投影点とロボットの右足との間の距離の比率をｄＲとすると、ロボット１００の左右の脚１１０Ｌ、１１０Ｒにかかるペルヴィスリンク１６とトランク１０２の重量の比率は、下記の数学式（６）で表すことができる。
【０２２２】
ＷＬ＝ｄＲ／（ｄＲ＋ｄＬ）
ＷＲ＝１−ＷＬ （６）
ここで、ＷＬは、左脚にかかる重量の比率を表し、ＷＲは、右脚にかかる重量の比率を表す。ペルヴィスリンクとトランクの質量に、ＷＬを乗算した分だけの質量が左脚にかかり、ＷＲを乗算した分だけの質量が右脚にかかる。ロボットが、図７のように、右に傾いた姿勢を取る場合、右脚により多くの重量がかかることになる。
【０２２３】
そして、重力補償トルク算出部３９０で、左右の脚１１０Ｌ、１１０Ｒに配分された重量を考慮して、第１の仮想重力及び第２の仮想重力を補償するために必要な重力補償トルクを算出する（６７０）。
【０２２４】
補償モーメント分配部３７０では、補償モーメント算出部３４０で算出した補償モーメントを、ロボット１００の左右の脚１１０Ｌ、１１０Ｒに分配する（６８０）。分配の比率は、前述した重量分配の比率と同一に適用することができ、算出された補償モーメントに、数学式（６）でのＷＬを乗算した値が左脚に分配される補償モーメントで、ＷＲを乗算した値が右脚に分配される補償モーメントである。
【０２２５】
そして、目標トルク算出部４００は、左右の脚１１０Ｌ、１１０Ｒに分配された補償モーメントを考慮して、各関節に必要な第１の目標トルクを算出し、ここに重力補償トルク算出部３９０で算出した重力補償トルクを合算して、第２の目標トルクを算出する（６９０）。
【０２２６】
さらに、ロボット１００の安全性のために、各関節の速度に比例して抵抗するダンピングトルクを加える過程をさらに含むことができる（７００）。目標トルク算出部４００で算出した第２の目標トルクにダンピングトルクを加えると、最終的に第３の目標トルクを得ることができ、下記の数学式（１０）で表すことができる。
【０２２７】
τ３、Ｌ＝τ２、Ｌ＋ｄ*ｗ τ３、Ｒ＝τ２、Ｒ＋ｄ*ｗ （１０）
ここで、τ３、Ｌ、τ３、Ｒはそれぞれ、左右の脚１１０Ｌ、１１０Ｒの第３の目標トルクを表し、ｄは、ダンピング係数、ｗは、関節の角速度を表す。
【０２２８】
サーボ制御部４２０では、左右の脚１１０Ｌ、１１０Ｒの各関節に対して算出された第３の目標トルクを各関節に発生させるために、各関節のトルクサーボを制御する（７１０）。
【符号の説明】
【０２２９】
３００ センサ部
３１０ 運動学計算部
３２０ 目標設定部
３３０ 有限状態機械制御部
３４０ 補償モーメント算出部
３５０ 補償力算出部
３６０ 仮想重力設定部
３９０ 重力補償トルク算出部
【技術分野】
【０００１】
トルクサーボによって各関節が動作するロボットの安定的な歩行制御のためのロボット及びその制御方法に関する。
【背景技術】
【０００２】
人間の歩行方法に近く二本脚で歩くロボットの歩行制御技術は、ロボットの各構成部分を連結する関節を制御することで実現される。それぞれの関節を制御する方法は、関節の角度を制御するものと、関節のトルクを制御するものとに大別される。
【０００３】
関節の角を制御する方法は、ロボットの足と地面とが当接している各点が生成する支持多角形（Ｓｕｐｐｏｒｔ Ｐｏｌｙｇｏｎ）内にＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）が位置するように、それぞれの関節の角度を逆運動学（Ｉｎｖｅｒｓｅ Ｋｉｎｅｍａｔｉｃｓ）を用いて算出したあと、ロボットのそれぞれの関節がその角度を維持するようにする方式で、関節のトルクを制御する方法のうち代表的なものは、動的歩行（Ｄｙｎａｍｉｃ Ｗａｌｋｉｎｇ）に分類されるものとして、ロボットの全体に作用する重力の方向を、ロボットが傾斜面にいるときのように定め、これに基づいてそれぞれの関節に必要なトルクを発生させる方式である。
【０００４】
関節の角を制御する方法は、逆運動学を解く過程で膝が曲がらないと値が存在しない。そのため、ロボットが膝を曲げた状態で歩行する場合、歩幅が小さくなり、エネルギー消耗が増加して、歩行効率が低下するという問題点がある。
【０００５】
また、動的歩行方法は、ロボットの歩行を３次元空間で解析するには複雑なので２次元歩行を具現するものであるか、または３次元歩行を具現したとしても、安定的でなく、人間の歩行と違って不自然であるという問題点がある。
【発明の概要】
【発明が解決しようとする課題】
【０００６】
本発明は、上述した問題点を解決するために、膝を伸ばした状態で歩行することができ、３次元歩行を具現しながらも、人間の歩行のように自然で安定した歩行が可能なロボット及びその制御方法を提供する。
【課題を解決するための手段】
【０００７】
本発明の一側面に係る、複数の関節部がそれぞれ設けられた複数の脚と、前記脚に連結された上体とを有し、複数の自由度を有するロボットは、前記ロボットの状態情報を測定するセンサ部と；前記センサ部で測定された状態情報を用いて、前記ロボットの現在の姿勢を獲得する運動学計算部；予め設定された有限状態機械（ＦＳＭ）に基づいて、前記複数の自由度のうち少なくとも一つの自由度に対して目標姿勢を設定する目標設定部と；前記現在の姿勢と目標姿勢とを比較して姿勢エラーを算出し、前記姿勢エラーに基づいて前記少なくとも一つの自由度に対して補償力を算出する補償力算出部と；前記算出された補償力によって前記複数の関節部に必要な仮想重力の大きさ、及び前記ロボットの現在のＦＳＭの状態によって前記複数の関節部に必要な仮想重力の大きさを設定する仮想重力設定部と；前記仮想重力設定部で設定された仮想重力を補償するための重力補償トルクを算出する重力補償トルク算出部と；前記複数の関節部に前記重力補償トルクを出力するサーボ制御部と；を含む。
【０００８】
前記センサ部は、前記上体のポーズ角度を検出するポーズ検出部と、前記複数の関節部の角度を検出する角度検出部とを含むことができる。
【０００９】
前記運動学計算部は、前記上体のポーズ角度及び複数の関節部の角度に基づいて重心を算出し、前記重心に基づいて前記現在の姿勢を獲得することができる。
【００１０】
前記運動学計算部は、前記上体のポーズ角度及び複数の関節部の角度に基づいて重心の位置と速度を算出し、前記重心の位置と速度に基づいて現在のキャプチャポイントを算出することによって、前記現在の姿勢を獲得することができる。
【００１１】
前記運動学計算部は、前記上体のポーズ角度及び前記複数の関節部の角度に基づいてヒップや上体の参照点の位置を獲得し、前記参照点の位置に基づいて前記現在の姿勢を獲得することができる。
【００１２】
前記目標設定部は、前記少なくとも一つの自由度に対して目標重心を設定し、前記目標重心を用いて前記目標姿勢を設定することができる。
【００１３】
前記目標設定部は、前記少なくとも一つの自由度に対して目標キャプチャポイントを設定し、前記目標キャプチャポイントを用いて前記目標姿勢を設定することができる。
【００１４】
前記目標設定部は、前記複数の脚に設けられた足の支持領域内の一点が重力方向に通る線上の一点を目標キャプチャポイントとして設定することができる。
【００１５】
前記目標設定部は、前記少なくとも一つの自由度に対して前記ロボットのヒップや上体の参照点の目標位置を設定し、前記参照点の目標位置に基づいて前記目標姿勢を設定することができる。
【００１６】
前記運動学計算部は、
前記ポーズ角度及び前記複数の関節部の角度に基づいて現在のヒップ高さをさらに獲得し、前記補償力算出部は、前記現在のキャプチャポイントと前記目標キャプチャポイントとを比較してキャプチャポイントエラーを算出し、前記現在のヒップ高さと目標ヒップ高さとを比較してヒップ高さエラーを算出し、前記キャプチャポイントエラーとヒップ高さエラーに基づいて補償力を算出することができる。
【００１７】
前記補償力算出部は、前記キャプチャポイントエラーを用いて水平方向の補償力を算出し、前記ヒップ高さエラーを用いて垂直方向の補償力を算出することができる。
【００１８】
前記ポーズ検出部で検出するポーズ角度は、前記ロボットのペルヴィスリンクのロール方向（ｒｏｌｌ、Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）、ヨー方向（ｙａｗ、Ｚ軸回転）の回転角度のうち少なくとも一つを含み、前記複数の関節部のトルクを測定する関節トルク検出部をさらに含むことができる。
【００１９】
前記運動学計算部は、前記ロボットの現在の重心の位置及び前記重心の速度を用いて、前記ロボットの現在のキャプチャポイントの位置を算出することができる。
【００２０】
前記ロボットは、前記ロボットの有限状態機械（ＦＳＭ）を予め格納し、前記ロボットの現在のＦＳＭの状態を判断し、前記ロボットがＦＳＭの状態遷移の条件を満足するか否かによって、次のＦＳＭの状態に遷移するように制御するＦＳＭ制御部をさらに含むことができる。
【００２１】
前記目標設定部は、前記ロボットの現在のＦＳＭの状態によって目標キャプチャポイントの位置を設定し、前記ロボットの上体が直立姿勢を維持するように目標ポーズ角度を設定することができる。
【００２２】
前記ロボットは、前記センサ部で測定したポーズ角度と、前記目標設定部で設定した目標ポーズ角度との間の差を用いて、前記目標ポーズ角度に到達するために補償しなければならない補償モーメントを算出する補償モーメント算出部をさらに含むことができる。
【００２３】
前記補償力算出部は、前記運動学計算部で計算した前記ロボットの現在のキャプチャポイントのｘ方向の位置及びｙ方向の位置と、前記目標設定部で設定した目標キャプチャポイントのｘ方向の位置及びｙ方向の位置との間の差を用いて、ｘ方向及びｙ方向の補償力を算出し、前記運動学計算部で計算した前記ロボットの現在のヒップ高さと、前記目標設定部で設定した目標ヒップ高さとの間の差を用いて、ｚ軸方向の補償力を算出することができる。
【００２４】
前記仮想重力設定部は、前記ロボットのＦＳＭの現在の状態による仮想重力、及び前記補償力算出部で算出した補償力による仮想重力の大きさを設定することができる。
【００２５】
前記仮想重力設定部は、前記ロボットの質量をｍとし、前記補償力算出部で算出した補償力をｆとするとき、下記の数学式（２）を用いて、前記ロボットの各関節に必要な第１の仮想重力を設定する第１の仮想重力設定部と、前記ロボットの現在のＦＳＭの状態によって、各関節に必要な第２の仮想重力を設定する第２の仮想重力設定部と、を含むことができる。
【００２６】
ｇｆ＝ｆ／ｍ （２）
前記ロボットは、前記ロボットの重心が地面に投影された点と前記各脚に連結された足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点からより近い脚に、より大きい比率の上体質量を割り当てる重量分配部をさらに含むことができる。
【００２７】
前記重力補償トルク算出部は、前記重量分配部で前記各脚に分配した上体の質量を反映して、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクを算出し、前記ロボットのＦＳＭの状態によって、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクの比重を調節して、重力補償トルクを算出することができる。
【００２８】
前記ロボットは、前記ロボットの重心が地面に投影された点と前記ロボットの複数の足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点から近い脚に、より大きい値の補償モーメントを割り当てることによって、前記補償モーメント算出部で算出された補償モーメントを前記複数の脚に分配する補償モーメント分配部をさらに含む。
【００２９】
前記ロボットは、前記ロボットの足をベースリンクと見るヤコビアンを用いて、前記複数の脚に分配した補償モーメントから前記複数の脚に必要な第１の目標トルクを算出する目標トルク算出部をさらに含むことができる。
【００３０】
前記目標トルク算出部は、前記第１の目標トルクと前記重力補償トルク算出部で算出した重力補償トルクとを合算して、前記ロボットの各関節に必要な第２の目標トルクを算出することができる。
【００３１】
前記ロボットは、前記目標トルク算出部で算出された第２の目標トルクに各関節の角速度に比例するダンピングトルクを加えて、第３の目標トルクを算出するトルク補償部をさらに含むことができる。
【００３２】
本発明の一側面に係る、複数の関節部がそれぞれ設けられた複数の脚と、該脚に連結された上体とを含み、複数の並進運動の自由度及び回転運動の自由度を有するロボットの制御方法は、前記上体のポーズ角度及び前記複数の関節部の角度を検出し；前記検出された複数の関節部の角度に基づいて、前記複数の並進運動の自由度のうち少なくとも一つの並進運動の自由度に対する現在の姿勢を獲得し；前記獲得された現在の姿勢と予め設定された目標姿勢との差に基づいて、姿勢エラーを算出し；前記姿勢エラーに基づいて、前記少なくとも一つの並進運動の自由度に対する補償力を算出し；前記少なくとも一つの並進運動の自由度に対する補償力、及び前記ロボットに対して予め設定された有限状態機械（Ｆｉｎｉｔｅ Ｓｔａｔｅ Ｍａｃｈｉｎｅ：ＦＳＭ）の現在の状態に基づいて、仮想重力を設定し；前記設定された仮想重力を補償するために、前記ロボットの関節部に必要な重力補償トルクを算出し；前記ロボットの関節部のトルクが前記算出された重力補償トルクを含む目標トルクになるように各関節のトルクサーボを制御することを含む。
【００３３】
前記姿勢エラーを算出することは、前記検出されたポーズ角度及び前記複数の関節部の角度に基づいて現在の重心の位置を算出するが、前記少なくとも一つの並進運動の自由度に対する現在の重心の位置を算出し、前記算出された現在の重心と予め設定された目標重心とを比較して、前記少なくとも一つの並進運動の自由度に対する重心エラーを算出し、前記重心エラーを姿勢エラーとして用いることを含むことができる。
【００３４】
前記姿勢エラーを算出することは、前記検出されたポーズ角度及び複数の関節部の角度に基づいて、現在の重心の位置と速度を検出するが、前記少なくとも一つの並進運動の自由度に対する現在の重心の位置と速度を検出し、前記検出された現在の重心の位置と速度に基づいて、前記少なくとも一つの並進運動の自由度に対する現在のキャプチャポイントを獲得し、前記獲得された現在のキャプチャポイントと予め設定された目標キャプチャポイントとを比較して、少なくとも一つの並進運動の自由度に対するキャプチャポイントエラーを算出し、前記算出されたキャプチャポイントエラーを前記姿勢エラーとして用いることを含むことができる。
【００３５】
前記姿勢エラーを算出することは、前記上体のポーズ角度及び前記複数の関節部の角度を検出し、前記ポーズ角度及び前記複数の関節部の角度に基づいて、ヒップまたは上体のいずれか一つの参照点の位置を前記少なくとも一つの自由度に対して算出し、前記現在の参照点と予め設定された目標参照点との位置を比較して、参照点位置エラーを前記少なくとも一つの自由度に対して算出し、前記参照点位置エラーを前記少なくとも一つの並進運動の自由度の姿勢エラーとして用いることを含むことができる。
【００３６】
前記姿勢エラーを算出することは、前記複数の並進運動の自由度のうち残りの並進運動の自由度の姿勢エラーの算出と互いに異なることを含むことができる。
【００３７】
前記ロボットの関節部に必要な仮想重力を設定することは、前記ロボットの質量をｍとし、前記算出した補償力をｆとするとき、下記の数学式（２）を用いて、前記ロボットの関節部に必要な第１の仮想重力を設定し、前記ロボットの現在のＦＳＭの状態によって、前記関節部に必要な第２の仮想重力を設定することを含むことができる。
【００３８】
ｇｆ＝ｆ／ｍ （２）
前記重力補償トルクを算出することは、前記第１の仮想重力に対する重力補償トルク及び前記第２の仮想重力に対する重力補償トルクを算出し、前記ロボットのＦＳＭの状態によって、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクの比重を調節して、前記ロボットの関節部に提供される重力補償トルクを算出することを含むことができる。
【００３９】
前記ロボットの制御方法は、前記ロボットが安定した姿勢を維持するための目標ポーズ角度を設定し、前記目標ポーズ角度と前記測定されたポーズ角度との間の差を補償するための補償モーメントを算出することをさらに含むことができる。
【００４０】
前記ロボットの制御方法は、前記算出された補償モーメントを前記ロボットの関節部に提供するために必要な第１の目標トルクを算出し、前記第１の目標トルクと前記重力補償トルクを合算して、第２の目標トルクを算出することをさらに含み、前記目標トルクは、第２の目標トルクであるものとすることができる。
【００４１】
前記ロボットは、前記算出された第２の目標トルクに前記関節部の角速度に比例するダンピングトルクを加えて、第３の目標トルクを算出することをさらに含み、前記目標トルクは、前記第３の目標トルクであるものとすることができる。
【００４２】
前記ロボットの重心が地面に投影された点と前記ロボットの複数の足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点から近い脚に、より大きい値の補償モーメントを割り当てることによって、前記算出された補償モーメントを前記複数の脚に分配することをさらに含むことができる。
【００４３】
前記第１の目標トルクを算出することは、前記ロボットの足をベースリンクと見るヤコビアンを用いて、前記ロボットの複数の脚に分配された補償モーメントから前記複数の脚に必要な第１の目標トルクを算出することを含むことができる。
【発明の効果】
【００４４】
本発明によるロボット及びその制御方法によれば、重心の位置と速度を合成して得られるキャプチャポイントを歩行中に制御することによって、安全性を維持することができ、左右の脚に重心を移動させたり、または前進のために支持足（ｓｕｐｐｏｒｔ ｆｏｏｔ）の前へ重心を移すことが可能になる。
【００４５】
また、外部から力が作用して、キャプチャポイントが支持足から外れたときにも、キャプチャポイントに向かってスイング足（ｓｗｉｎｇ ｆｏｏｔ）を移して着地することになるので、倒れずに歩行を続けることができる。
【００４６】
また、本発明では、地面の傾斜と凹凸にかかわらず、ロボットの上体の姿勢を制御するので、歩行時にも傾斜面と凹凸面によって倒れずに安定的に歩行することができる。
【００４７】
また、本発明による歩行動作は、人間の歩行段階を考慮してＦＳＭを構成することができるので、人間の歩行と類似したかかとからつま先（ｈｅｅｌ-ｔｏ-ｔｏｅ）動作が可能で、自然な歩行が可能である。
【００４８】
また、膝を曲げずに歩行できるので、大きな歩幅の歩行が可能で、歩行に必要なエネルギーを効率的に使用することができる。
【図面の簡単な説明】
【００４９】
【図１】本発明の一実施例によるロボットの外観構成図である。
【図２】本発明の一実施例によるロボットの主要関節の構造を示す図である。
【図３】本発明の一実施例によるロボットの制御ブロック図である。
【図４】本発明の一実施例によるロボットのＦＳＭの状態ダイヤグラムである。
【図５】本発明の一実施例によるロボットの各ＦＳＭの状態での歩行動作を示す図である。
【図６】本発明の一実施例によるロボットのキャプチャポイントを示す図である。
【図７】本発明の一実施例によるロボットの下体を示す図である。
【図８】本発明の一実施例によるロボットの制御ブロック図である。
【図９】本発明の一実施例によるロボットの歩行制御方法に対するフローチャートである。
【図１０】並進運動の自由度に対するロボットの姿勢をキャプチャポイントで表すロボットの歩行制御方法に対するフローチャートである。
【図１１】本発明の一実施例による補償モーメントを左右の脚に分配する過程をさらに含むロボットの歩行制御方法に対するフローチャートである。
【発明を実施するための形態】
【００５０】
以下、添付の図面を参照して、本発明の実施例を詳細に説明する。
【００５１】
図１は、本発明の一実施例によるロボットの外観構成図で、図２は、図１に示したロボットの主要関節の構造を示す図である。
【００５２】
図１及び図２を参照すると、本発明の一実施例によるロボット１００は、人間と同様に、二つの脚１１０Ｌ、１１０Ｒによって直立移動する２足ロボットであって、胴体１０２、頭１０４、腕１０６Ｌ、１０６Ｒからなる上体１０１と、二つの脚１１０Ｌ、１１０Ｒからなる下体１０３と、を有する。
【００５３】
ロボット１００の上体１０１は、胴体１０２と、胴体１０２の上部に首１２０を介して連結された頭１０４と、胴体１０２の上部両側に肩１１４Ｌ、１１４Ｒを介して連結された二つの腕１０６Ｌ、１０６Ｒと、該二つの腕１０６Ｌ、１０６Ｒの末端にそれぞれ連結された手１０８Ｌ、１０８Ｒと、からなる。
【００５４】
ロボット１００の胴体１０２には、上体１０１が回転できるように、ヨー（ｙａｗ）方向の１自由度を有する腰関節部１５が設置される。
【００５５】
ロボット１００の下体１０３は、腰関節部１５の下側に連結されたペルヴィスリンク１６と、ペルヴィスリンク１６の下部両側に連結された二つの脚１１０Ｌ、１１０Ｒと、二つの脚１１０Ｌ、１１０Ｒの末端にそれぞれ連結された足１１２Ｌ、１１２Ｒと、からなる。
【００５６】
ここで、頭１０４、二つの腕１０６Ｌ、１０６Ｒ、二つの手１０８Ｌ、１０８Ｒ、二つの脚１１０Ｌ、１１０Ｒ、二つの足１１２Ｌ、１１２Ｒはそれぞれ、関節を通じて一定水準の並進運動及び回転運動の自由度を有する。
【００５７】
参照符号における“Ｒ”と“Ｌ”は、それぞれロボット１００の右側（ｒｉｇｈｔ）と左側（ｌｅｆｔ）を表し、ＣＯＧ（Ｃｅｎｔｅｒ Ｏｆ Ｇｒａｖｉｔｙ）は、ロボット１００の重心を表す。
【００５８】
また、ロボット１００の頭１０４には、周囲を撮影するカメラ４１と、ユーザの音声を入力するマイクロフォン４２とが設置され得る。
【００５９】
頭１０４は、首関節部２８０を介して上体１０１の胴体１０２と連結される。首関節部２８０は、ヨー方向（ｙａｗ、Ｚ軸回転）の回転関節２８１、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）の回転関節２８２、及びロール方向（ｒｏｌｌ、Ｘ軸回転）の回転関節２８３を含み、３自由度を有する。
【００６０】
首関節部２８０のそれぞれの回転関節２８１、２８２、２８３には、頭１０４の回転のためのモータ（例えば、電気モータ、油圧モータなどのアクチュエータ）が連結される。
【００６１】
ロボット１００の二つの腕１０６Ｌ、１０６Ｒはそれぞれ、上膊リンク３１、下膊リンク３２及び手３３を有する。
【００６２】
上膊リンク３１は、肩関節部２５０Ｌ、２５０Ｒを介して上体１０１に連結され、上膊リンク３１と下膊リンク３２は、肘関節部２６０を介して互いに連結され、下膊リンク３２と手３３は、手首関節部２７０を介して互いに連結される。
【００６３】
肩関節部２５０Ｌ、２５０Ｒは、上体１０１の胴体１０２の両側に設置され、二つの腕１０６Ｌ、１０６Ｒを上体１０１の胴体１０２に連結する。
【００６４】
肘関節部２６０は、ピッチ方向の回転関節２６１と、ヨー方向の回転関節２６２とを含み、２自由度を有する。
【００６５】
手首関節部２７０は、ピッチ方向の回転関節２７１と、ロール方向の回転関節２７２とを含み、２自由度を有する。
【００６６】
手３３には５個の指３３ａが設置される。それぞれの指３３ａには、モータによって駆動される多数の関節（図示せず）が設置され得る。指３３ａは、腕１０６の動きに連動して、物を把持したり、特定方向を指すような多様な動作を行う。
【００６７】
そして、胴体１０２の下側には、二つのヒップ関節部２１０を有するペルヴィスリンク１６が連結される。ペルヴィスリンク１６（ｐｅｌｖｉｓ ｌｉｎｋ）は人間の骨盤に該当する部分であって、ヒップ関節部２１０を通じて上体１０１の胴体１０２に連結される。
【００６８】
そして、ロボット１００の二つの脚１１０Ｌ、１１０Ｒはそれぞれ、大腿リンク２１と下腿リンク２２、足１１２Ｌ、１１２Ｒを有する。
【００６９】
大腿リンク２１は人間の太股に該当する部分であって、ヒップ関節部２１０を介してペルヴィスリンク１６に連結され、大腿リンク２１と下腿リンク２２は、膝関節部２２０を介して互いに連結され、下腿リンク２２と足１１２Ｌ、１１２Ｒは、足首関節部２３０を介して互いに連結される。
【００７０】
ヒップ関節部２１０は、ヨー方向（ｙａｗ、Ｚ軸周囲の回転）の回転関節２１１（ヒップヨージョイント）と、ピッチ方向（ｐｉｔｃｈ、Ｙ軸周囲の回転）の回転関節２１２（ヒップピッチジョイント）と、ロール方向（ｒｏｌｌ、Ｘ軸周囲の回転）の回転関節２１３（ヒップロールジョイント）とを含み、３自由度を有する。
【００７１】
膝関節部２２０は、ピッチ方向の回転関節２２１を含み、１自由度を有する。
【００７２】
足首関節部２３０は、ピッチ方向の回転関節２３１とロール方向の回転関節２３２とを含み、２自由度を有する。
【００７３】
二つの脚１１０Ｌ、１１０Ｒのそれぞれには、ヒップ関節部２１０、膝関節部２２０、足首関節部２３０に対して６個の回転関節が設けられるので、二つの脚１１０Ｌ、１１０Ｒ全体に対しては１２個の回転関節が設けられ、本発明の一実施例によるロボットは、６自由度を有する一対の脚を含む。
【００７４】
図示されていないが、ロボット１００には、各回転関節を駆動するモータなどのようなアクチュエータが設置され、このアクチュエータを適切に制御することによって、ロボット１００の多様な動作を具現することができる。アクチュエータの種類には制限がないが、以下で説明する本発明の一実施例では、アクチュエータとしてサーボモータを例に挙げて説明する。
【００７５】
ロボット１００は、制御動作（Ｃｏｎｔｒｏｌ Ａｃｔｉｏｎ）の実行のために、各制御動作に対応して各関節のトルク指令を算出し、算出されたトルク指令をトルクサーボに出力して、サーボモータを駆動させる。
【００７６】
先に検討した情報に基づいて、本発明の一実施例によるロボット１００の歩行動作について詳細に説明する。
【００７７】
図３には、本発明の一実施例によるロボット１００の制御ブロック図が示されている。図３を参照すると、本発明の一実施例によるロボット１００の制御は、ロボットの状態を測定するセンサ部３００と、ロボットの有限状態機械（Ｆｉｎｉｔｅ Ｓｔａｔｅ Ｍａｃｈｉｎｅ：ＦＳＭ）を予め格納し、ロボットのＦＳＭの状態を制御するＦＳＭ制御部３３０と、前記センサ部３００で測定されたロボットの状態を用いて、ロボットの現在の姿勢を獲得する運動学計算部３１０と、ロボットの目標姿勢を設定する目標設定部３２０と、前記設定された目標姿勢と前記獲得された現在の姿勢との間の差を用いて、前記ロボットがバランスを取るために必要な補償力を算出する補償力算出部３５０と、前記センサ部３００で測定された情報を用いて、前記ロボットがバランスを取るために必要な補償モーメントを算出する補償モーメント算出部３４０と、前記ロボットに対して予め設定された有限状態機械の現在の状態及び補償力算出部３５０で算出した補償力の大きさを用いて、前記ロボットの各関節に必要な仮想重力の大きさを設定する仮想重力設定部３６０と、前記仮想重力設定部３６０で設定された仮想重力と実際の重力に対する重力補償トルクを算出する重力補償トルク算出部３９０と、前記重力補償トルク算出部３９０で算出された重力補償トルクと前記補償モーメント算出部３４０で算出した補償モーメントに対する関節トルクとを合算して、前記ロボットの各関節に必要な目標トルクを算出する目標トルク算出部４００と、前記ロボットの各関節のトルク値が、前記目標トルク算出部４００で算出した目標トルク値になるように、各関節のトルクサーボを制御するサーボ制御部４２０と、を含む。
【００７８】
センサ部３００は、ロボットの各関節部の角度を検出する角度検出部３０１と、ロボットのポーズ角度を検出するポーズ検出部３０２と、足１１２が地面に接触したか否か、または足に加えられる荷重を検出する力／トルク検出部３０３と、ロボットの各関節のトルクを測定する関節トルク検出部３０４とを含む。
【００７９】
角度検出部３０１は、ロボットを構成する各関節部の角度を検出するもので、各関節部の各軸に設けられたモータの回転角度を検出する。ここで、各関節部の角度は、モータ（図示せず）の回転数を用いて検出することができ、モータの回転数は、各モータに連結された関節エンコーダ（Ｅｎｃｏｄｅｒ、図示せず）を通じて検出可能である。
【００８０】
ポーズ検出部３０２は、ＩＭＵ１７として具現することができ、ＩＭＵは、重力方向と慣性系に対するペルヴィスリンク１６の相対的角度を検出して、ロール方向（ｒｏｌｌ、Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）、ヨー方向（ｙａｗ、Ｚ軸回転）の姿勢情報を発生させる。具体的にＩＭＵは、ロール方向（ｒｏｌｌ、Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）、ヨー方向（ｙａｗ、Ｚ軸回転）の３個の軸の回転角度のうち少なくとも一つの回転角度を検出できる。以下、ポーズ検出部３０２で検出された角度をポーズ角度と称する。但し、ＩＭＵ１７がペルヴィスリンク１６の角度を検出することに限定されず、頭や胴体などの他の部位のリンクに対してポーズ角度を検出することも可能である。また、ロボット１００の姿勢、特に、上体の姿勢を測定するために、ＩＭＵの他にティルティング検出（Ｔｉｌｔｉｎｇ Ｄｅｔｅｃｔｉｏｎ）、ジャイロセンサ（Ｇｙｒｏ Ｓｅｎｓｏｒ）などを使用することもできる。
【００８１】
力／トルク検出部３０３は、多軸Ｆ／Ｔセンサ（Ｍｕｌｔｉ−Ａｘｉｓ Ｆｏｒｃｅ ａｎｄ Ｔｏｒｑｕｅ Ｓｅｎｓｏｒ）として具現することができ、多軸Ｆ／Ｔセンサ２４は、二つの脚１１０Ｌ、１１０Ｒにおいて、足１１２Ｌ、１１２Ｒと足首関節部２３０との間にそれぞれ設置されて、足１１２Ｌ、１１２Ｒから伝達される力の３方向成分（Ｆｘ、Ｆｙ、Ｆｚ）及びモーメントの３方向成分（Ｍｘ、Ｍｙ、Ｍｚ）を測定することによって、足１１２Ｌ、１１２Ｒが着地したか否か、または足１１２Ｌ、１１２Ｒに加えられる荷重を検出する。足１１２が地面に接触したか否かを検出するセンサとして、Ｆ／Ｔセンサ２４の他に、接触センサまたはこれに相応するセンサを使用することもできる。
【００８２】
運動学計算部３１０は、センサ部３００で測定したロボット１００の状態情報を用いて、ロボット１００の現在の姿勢を獲得する。ロボット１００の姿勢は、ロボット１００の位置と角度を含むことができ、これは、ロボット１００の二つの足の位置、重心の位置と速度、及びキャプチャポイントなどを用いて獲得することができる。
【００８３】
通常、ロボットの姿勢は、上体及び下体の全ての関節の角度として表すことができるが、本発明では、次のようにロボットの姿勢を表すことができる。ロボットの姿勢は、作業空間でロボットの並進運動の自由度及び回転運動の自由度に対するロボットの位置と角度として表すことができる。
【００８４】
このロボットの位置と角度は、ロボットのいずれか一点の位置と角度を用いて表すことができる。ロボットの位置は、３軸並進運動の自由度に対して順運動学を用いて、基準座標系においてロボットのいずれか一点のｘ、ｙ、ｚ軸の３次元座標として表すことができる。
【００８５】
また、ロボットの角度は、回転運動の自由度に対して順運動学、またはポーズ検出部３０２を通じて検出されたポーズ角度として表すことができる。したがって、本発明において一つの姿勢を取るために、どの関節でもその角度は唯一に決まるものではない。
【００８６】
ここで、ロボットの姿勢を構成する位置と角度のうち、位置を表すいずれか一点は、ロボットの重心であってもよく、またはキャプチャポイントであってもよい。または、脚及び腕を除外した部分に固定された一点を参照点としてロボットの位置を表すことができる。
【００８７】
キャプチャポイントに対して定義すれば、キャプチャポイントの通常の意味は、一定の高さを持つ物体が、現在与えられた位置と速度からみるとき、地面のどの地点を踏むと垂直に立つ平衡状態に到達するのかに関する問題におけるソリューションと言える。すなわち、ロボットのキャプチャポイントは、ロボットの現在の位置と速度に照らして、次の動作を行うとき、倒れずに垂直に立つことができる位置のことを意味する。
【００８８】
前述したキャプチャポイントの定義によれば、通常、水平面（すなわち、地面）上の点になるが、本発明におけるキャプチャポイントは３次元領域に拡張されることも可能である。３次元領域でのキャプチャポイントは、重心から、重心が移動する速度に比例した距離だけ、重心の移動方向に移動した点と言える。
【００８９】
３次元の並進自由度に対してロボットの位置を表す点が重心、キャプチャポイント、参照点のうちいずれか一つに一致する必要はなく、互いに異なる方式で表すこともできる。例えば、２次元水平方向の並進運動の自由度に対するロボットの位置は、キャプチャポイントの２次元水平成分と定義し、１次元垂直方向の並進運動の自由度に対するロボットの位置は、ヒップの高さと定義することができる。
【００９０】
運動学計算部３１０は、各関節の関節エンコーダの瞬時値を読み、予め知っている関節リンクのサイズと重量を用いて、ロボットベース座標系に対する重心の位置、ヒップ高さ、二つの足の位置及び二つの足が置かれた方向を計算できる。ロボット１００の現在の重心（ＣＯＧ）の位置は、胴体１０２に設けられ、この時、現在の重心の方向は、重力の方向に固定された慣性系の方向（Ｏｒｉｅｎｔａｔｉｏｎ）に従う。
【００９１】
ベース座標系と絶対座標系（ｇｌｏｂａｌ ｏｒ ｗｏｒｌｄ ｃｏｏｒｄｉｎａｔｅ ｓｙｓｔｅｍ）に対する変換行列を計算して、先に計算したベース座標に対する位置情報を絶対座標系に変換することができる。これを通じて、地面に置かれた足に対して重心の相対的移動軌跡を計算し、その速度を計算し、重心の速度と位置、ヒップ高さを計算できる。
【００９２】
３次元空間のキャプチャポイントは、ロボットの重心とロボットの速度を用いて求めることができる。ロボットの現在の位置でのキャプチャポイントは、下記の数学式（１）によって求めることができる。
【００９３】
ＣＰ＝ｄＣＯＧ＋ｗ*ｖＣＯＧ （１）
ＣＰは、キャプチャポイントの位置を表し、ｄＣＯＧは、重心（ＣＯＧ）の位置を表し、ｖＣＯＧは、重心の速度を表す。ｗは、ｗ＝√（ｈ／ｇ）を計算して求めることができ、ここで、ｈは、重心の地面からの高さで、ｇは重力加速度である。２次元水平面のキャプチャポイントは、上記で算出した３次元のキャプチャポイントの（ｘ、ｙ、ｚ）成分のうち（ｘ、ｙ）成分のみを取って求められる。
【００９４】
まとめると、運動学計算部３１０は、ロボット１００の複数の関節部の角度に基づいて重心を算出することができ、重心をロボットの位置として現在の姿勢を獲得することができる。
【００９５】
または、複数の関節部の角度に基づいて、重心の位置と速度を算出し、重心の位置と速度に基づいて、現在のキャプチャポイントを算出したあと、獲得された現在のキャプチャポイントをロボットの位置として現在の姿勢を獲得したり、または上体のポーズ角度及び複数の関節部の角度に基づいて、ヒップや上体の参照点の位置を算出し、算出された参照点をロボットの位置として現在の姿勢を獲得する。
【００９６】
目標設定部３２０は、ロボットの有限状態機械（ＦＳＭ）の現在の状態による目標姿勢を設定する。
【００９７】
ロボットの目標姿勢は、目標キャプチャポイント、目標ポーズ角度、目標ヒップ高さなどを用いて表すことができ、ここで、キャプチャポイントは多様な方法により表現することができるが、キャプチャポイント（ＣＰ）は、３次元空間で重心が移動する速度を反映して速度に対応する距離だけ移動した点の位置として表現することも可能である。
【００９８】
また、目標設定部３２０は、ロボットが倒れないようにするために、二つの足の支持領域の内部を重力方向に通る線上の一点を目標キャプチャポイントとして設定し、この目標キャプチャポイントをロボットの並進運動の自由度に対する目標姿勢として設定することも可能である。
【００９９】
ロボットの目標ポーズ角度は任意に設定できるが、ロボットの直立姿勢のために、ロボットの上体が重力方向と平行になるように設定し、これをロボットの回転運動の自由度に対する目標姿勢として設定することも可能である。
【０１００】
有限状態機械（ＦＳＭ）は、与えられた時間に現れることのできる有限個の状態を示す概念的なモデルであって、ロボットのＦＳＭは、設計者が任意に設定することができ、ロボットは予め設定された有限個の状態を循環的に有することになる。
【０１０１】
ＦＳＭ制御部３３０は、ロボット１００のＦＳＭを予め格納し、ロボット１００の現在のＦＳＭの状態を判断し、現在のＦＳＭの状態から次のＦＳＭの状態に遷移できるか否かを判断して、ロボット１００のＦＳＭを制御する。
【０１０２】
補償力算出部３５０は、運動学計算部３１０で獲得したロボット１００の現在の位置と、目標設定部３２０で設定したロボット１００の目標位置とを比較して、位置エラーを算出し、位置エラーに基づいて少なくとも一つの自由度に対して補償されるべき補償力を算出する。
【０１０３】
補償モーメント算出部３４０は、ロボット１００が直立姿勢を維持するようにするために必要な補償モーメントを算出し、ポーズ検出部３０２でペルヴィスリンクに対して測定した現在のポーズ角度と、目標設定部３２０で設定した目標回転角度との間の差から、ｘ、ｙ、ｚ方向の補償モーメントを算出し、ロボットのポーズ角度は、ペルヴィスリンクでない胴体（Ｔｒｕｎｋ）リンクや頭などのリンクに対して測定しても構わない。
【０１０４】
仮想重力設定部３６０は、ロボットの現在のＦＳＭの状態及び補償力算出部３５０で算出した補償力を用いて仮想重力の大きさを設定し、補償力から設定された仮想重力は、下記の数学式（２）で表すことができる。
【０１０５】
ｇｆ＝ｆ／ｍ （２）
ここで、ｇｆは、仮想重力を表し、ｆは、補償力算出部３５０で算出した補償力を表し、ｍは、ロボットの質量を表す。ロボットのＦＳＭの状態による仮想重力については後述する。
【０１０６】
重力補償トルク算出部３９０は、仮想重力設定部３６０で設定した仮想重力と実際の重力を補償するために、各関節に必要な重力補償トルクを算出し、重力補償トルクは、仮想重力加速度と実際の重力加速度の和、各関節の角度、各リンクの重量及び各リンク内の重心の位置を用いて算出できる。
【０１０７】
目標トルク算出部４００は、各関節に伝達されなければならないトルクの総和を算出し、姿勢の制御、すなわち、モーメント補償と重力補償を全て考慮しなければならない。したがって、補償モーメント算出部３４０で算出した補償モーメントを発生させるための第１の目標トルクを算出し、第１の目標トルクに重力補償トルクを加えて、第２の目標トルクを算出する。この時、６×６ヤコビアン行列を用いることができる。
【０１０８】
サーボ制御部４２０は、目標トルク算出部４００から、各関節に必要な第２の目標トルクの伝達を受け、各関節のトルク値が第２の目標トルクに到達するように各トルクサーボを制御する。
【０１０９】
以下、ロボット１００の姿勢を表す情報のうちロボット１００の位置は、キャプチャポイント及びヒップ高さを含むものとして、本発明の一実施例によるロボット１００の動作をより具体的に説明する。すなわち、３次元並進運動の自由度のうち２次元水平方向に対してはキャプチャポイントに基づき、垂直方向に対してはヒップ高さを参照点とする。
【０１１０】
本発明の一実施例によるロボット１００は、設計者によって予め設定されたＦＳＭによって動作し、目標設定部３２０で設定される目標姿勢のうち目標キャプチャポイントは、ロボット１００の現在のＦＳＭの状態によって異なるように設定される。以下、本発明の一実施例によるロボットのＦＳＭについて詳細に説明する。
【０１１１】
図４には、本発明の一実施例によるＦＳＭの状態遷移に関するダイヤグラムが示されている。当該実施例では、ＦＳＭが７つの状態を有するものとし、ＤＳ状態→Ｗ１状態→Ｗ２状態→Ｗ３状態→Ｗ４状態→Ｗ２状態→Ｗ３状態→Ｗ４状態→Ｗ２状態→．．．に循環したり、停止命令が入ると、Ｗ４状態から停止準備動作であるＷ２’状態→Ｗ３’状態を経てＤＳ状態に遷移する。
【０１１２】
ＦＳＭの各状態での目標ヒップ高さはいずれも同一であるとし、ｘ方向は、支持足の長さ方向であって、前方を正の方向とし、ｙ方向は、ｘ方向に対して上から見て９０度反時計方向とする。
【０１１３】
また、支持足は、ロボットの姿勢を維持させる足、すなわち、地面を踏んでいる足のことを意味し、スイング足は、移動のために上に持ち上げた足のことを意味する。
【０１１４】
図５に、図４に示されたＦＳＭの状態を有するロボット１００の歩行動作が示されている。歩行の開始をＤＳ状態とすると、ＤＳ状態は、ロボット１００の二つの脚が地面に着いているものとし、Ｗ１状態では、外観上はＤＳ状態と違いがないが、重心を片方の脚に移動し始め、当該実施例では、左脚１１０Ｌに重心を移すものとする。
【０１１５】
Ｗ２状態では、右脚１１０Ｒを持ち上げながら左脚１１０Ｌでボディーを支えて立っており、Ｗ３状態では、ボディーを進行方向に移動させながら右脚１１０Ｒを下げて、右足１１２Ｒを地面に着地させる。
【０１１６】
Ｗ４状態では、右足１１２Ｒが地面に着いた後、重心を左脚１１０Ｌに移動させ、再びＷ２状態に遷移されながら脚の方向のみが変わり、停止命令が入るまで、Ｗ２→Ｗ３→Ｗ４→Ｗ２→．．．の循環が続く。
【０１１７】
停止命令が入った場合には、Ｗ４状態からＷ２’状態に遷移し、Ｗ２’状態は、Ｗ２状態と同様に左脚１１０Ｌを持ち上げるが、前方に進むわけではないので、目標キャプチャポイントのｘ成分を、現在の支持足の中心にくるようにする。Ｗ３’状態は、Ｗ３状態と同様に左足１１２Ｌを着地するが、前進するわけではないので、右足１１２Ｒと平行な位置に着地するものとする。
【０１１８】
図４及び図５を参照すると、７つの各ＦＳＭの状態は一定の順序によって遷移され、所定の遷移条件を満足する時に次の状態に遷移される。各ＦＳＭの状態の動作内容及び遷移条件は設計者が任意に設定でき、以下、図４及び図５に示されたＦＳＭの状態について詳細に説明する。
【０１１９】
まず、ＤＳ状態は、ロボット１００の二つの足１１２Ｒ、１１２Ｌが地面に着いて停止している状態を示すものとし、この時、目標キャプチャポイントはロボットの二つの足がなす支持多角形の中心に置かれる。歩行命令が下るとＷ１状態に遷移する。
【０１２０】
Ｗ１状態は、目標キャプチャポイントがロボット１００の二つの足１１２Ｒ、１１２Ｌのうち、任意に選択された支持足に移動することになる状態であり、実際のキャプチャポイントが支持足の足の幅以内の安定領域内に進入すると、Ｗ２状態に遷移する。
【０１２１】
Ｗ２状態は、ロボット１００がスイング足を持ち上げる動作を行う状態であって、Ｗ２状態での目標キャプチャポイントのｘ成分は、支持足の中心から前の部分に時間によって前進する軌跡と設定され、ｙ成分は、支持足の中心線にくるように設定される。スイング脚を持ち上げる動作は、重力補償により制御され、詳細は後述する。Ｗ２状態で、現在のキャプチャポイントのｘ成分がしきい値を越え、スイング足の高さがしきい値を越えると、Ｗ３状態に遷移する。
【０１２２】
Ｗ３状態は、ロボット１００が着地のために膝を伸ばし、スイング足を下ろす動作を示し、この時、目標キャプチャポイントのｘ成分は、支持足の前部を越えて、スイング足が着地しなければならない位置まで時間によって増加する軌跡と設定され、ｙ成分は、スイング足が着地する位置へ時間によって移動するものと設定される。スイング足が地面に着いて、Ｆ／Ｔセンサにしきい値以上のｚ成分が感知されると、Ｗ４状態に遷移する。
【０１２３】
Ｗ４状態は、二つの足が地面に着いている状態であって、最後に着地した足を支持足とする。この時、目標キャプチャポイントのｘ、ｙ成分は、以前状態の目標キャプチャポイントの位置から新しい支持足の中心に、短時間内に連続的に移動する軌跡と設定される。現在のキャプチャポイントが支持足内に入ると、停止命令がない場合にはＷ２状態に遷移し、停止命令が下る場合にはＷ２’状態に遷移する。
【０１２４】
Ｗ２’状態はＷ２状態と類似した動作、すなわち、スイング足を持ち上げる動作を示すが、前進せずに停止するので、目標キャプチャポイントのｘ成分は支持足の中心に固定する。スイング足の高さがしきい値以上に到達すると、Ｗ３’状態に遷移する。
【０１２５】
Ｗ３’状態はＷ３状態と類似した動作、すなわち、スイング足の着地のために膝を伸ばし、足を下ろす動作を示すが、前進せずに停止するので、目標キャプチャポイントのｘ成分を支持足の前方へ送らずに支持足の位置に置き、目標キャプチャポイントのｙ成分は、反対側の足に向かって送るように設定する。スイング足が地面に着いて、Ｆ／Ｔセンサのｚ成分がしきい値を越えると、ＤＳ状態に遷移する。
【０１２６】
上述したＦＳＭの状態は、本発明の一実施例に過ぎず、ロボット１００の歩行を可能にするものであれば、その動作の内容やＦＳＭの状態の個数などは構わない。
【０１２７】
ただし、上述した例のようにＦＳＭの状態を設定する場合、人間の歩行動作に近く、最初にかかとが地面に着き、最後につま先が地面から離れるｈｅｅｌ-ｔｏ-ｔｏｅ動作が発生して、有効な脚の長さがさらに長くなり、ｈｅｅｌ-ｔｏ-ｔｏｅ動作をしない場合より大きな歩幅で自然に歩行することができ、逆運動学を利用しないため、膝を曲げた状態で歩行しなくてもよいので、逆運動学を用いた場合より大きな歩幅の歩行が可能になる。
【０１２８】
上述したＦＳＭの状態に基づいて、補償力算出部３５０及び仮想重力設定部３６０の制御動作について詳細に説明する。
【０１２９】
図６に、本発明の一実施例によるロボット１００が示されている。図６を参照すると、運動学計算部３１０は、センサ部３００で検出したロボット１００の状態情報を用いて、ロボット１００の重心の位置とその速度を算出し、算出された重心の位置とその速度を用いて、ロボット１００の現在のキャプチャポイントを算出する。また、算出された重心に基づいてロボット１００の現在のヒップ高さを算出する。
【０１３０】
キャプチャポイントは地面の上の点と見なされるので、ｘ、ｙ成分を表し、ヒップ高さはｚ成分を表す。
【０１３１】
補償力算出部３５０は、目標キャプチャポイントの位置と現在のキャプチャポイントの位置との間の差から、ｘ、ｙ方向の補償力を算出し、目標ヒップ高さと現在のヒップ高さの差から、ｚ方向の補償力を算出する。
【０１３２】
図６に示されたように、補償力を算出する際に、仮想のスプリングを用いることができる。目標キャプチャポイントの位置と現在のキャプチャポイントの位置との間に、スプリング定数ｋを持つ仮想のスプリングがあると仮定するときに作用する力ｆは、下記の数学式（３）で表すことができる。
【０１３３】
ｆ１＝ｋ*ｅ１ ｆ２＝ｋ*ｅ２ （３）
ｅ１は，目標キャプチャポイントと現在のキャプチャポイントとの間の差を表す座標であって、ｅ１＝（ｘ’、ｙ’）-（ｘ、ｙ）で表すことができ、ｆ１＝（ｆｘ、ｆｙ）になる。（ｘ’、ｙ’）は、目標設定部３２０で設定した目標キャプチャポイントの位置座標で、前述したように、ロボット１００の現在のＦＳＭの状態によって、目標キャプチャポイントの位置が変わる。（ｘ、ｙ）は、運動学計算部３１０で計算した現在のキャプチャポイントの位置座標である。
【０１３４】
ｅ２は、目標設定部３２０で設定した目標ヒップ高さと運動学計算部３１０で計算した現在のヒップ高さとの間の差を表し、ｆ２＝ｆｚになる。これによって、ｘ、ｙ、ｚ方向の補償力が算出される。
【０１３５】
上述したように、ロボットの歩行中に各歩行動作に合うキャプチャポイントを目標に設定し、目標キャプチャポイントに足を踏み出すようにロボットを制御することで、ロボットが倒れずに安定的に歩行することができる。
【０１３６】
ＦＳＭの各状態ごとに、目標設定部３２０で設定する目標キャプチャポイントと、運動学計算部３１０で算出する現在のキャプチャポイントがそれぞれ異なるので、補償力算出部３５０は、ロボットの現在のＦＳＭの状態によって補償力を算出する。
【０１３７】
仮想重力設定部３６０では、ロボットのＦＳＭの状態によって必要な仮想重力及び補償力算出部３５０で算出した補償力に対する仮想重力を設定する。具体的には、第１の仮想重力設定部３６１では、補償力算出部３５０で算出した補償力に対する仮想重力（ｓｕｐｐｏｒｔ ｇｒａｖｉｔｙ）である第１の仮想重力、すなわち、キャプチャポイントの位置を制御するために必要な仮想重力を設定し、第２の仮想重力設定部３６２では、ロボット１００のＦＳＭの状態による仮想重力（ｌｉｆｔ ｇｒａｖｉｔｙ）である第２の仮想重力を設定する。
【０１３８】
仮想重力の設定を説明するに先立ち、サポート重力は、ロボット１００の足が地面に着いており、ボディーを支持し、その上に姿勢を制御する場合に必要な仮想重力とし、リフト重力は、ロボット１００の足が空中に浮いており、ヒップから足を持ち上げたり、または下ろす場合に必要な仮想重力とする。
【０１３９】
第１の仮想重力設定部３６１では、前述した数学式（２）によって、補償力に対する仮想重力を設定し、ここでの仮想重力はサポート重力に該当する。
【０１４０】
第２の仮想重力設定部３６２で行うＦＳＭの状態による仮想重力の設定は、前述したＦＳＭの７つの状態を例に挙げて説明する。
【０１４１】
ロボット１００のＦＳＭがＷ２状態にある場合、スイング足を持ち上げる制御が必要なので、スイング脚に対して仮想重力の比重が瞬間的にサポート重力は０になり、リフト重力は１になるようにする。この時、リフト重力のｚ成分は、支持足とスイング足との高さ差を目標値に維持するように設定し、ｘ成分は、前進方向のスイングができるようにバイアス（ｂｉａｓ）されている。ｙ成分は、スイング脚を用いて左右のバランスを取ることができるように、補償力算出部３５０で計算した補償力のｙ成分を反映して設定し、二つの脚１１０Ｌ、１１０Ｒが衝突しないように、二つの脚１１０Ｌ、１１０Ｒの間隔によるフィードバックを加える。
【０１４２】
ロボット１００のＦＳＭがＷ３状態にある場合、スイング脚が着地のために膝を伸ばし、スイング足を下ろす動作を行うように、リフト重力を設定する。この時、サポート重力の比重は０、リフト重力の比重は１になる。リフト重力のｚ成分は、スイング足が地面に向かって下りてくるように（−）方向とし、ｙ成分は、スイング足がキャプチャポイントに向かうように現在のスイング足の位置とキャプチャポイントの位置との差をフィードバックし、最小限の左右の足の間隔は維持するようにする。ｘ成分は、スイング足の目標位置との差によって決定し、スイング足の目標位置のｘ成分は、スイング足が前進するように（＋）方向に増加させ、目標歩幅を達成できるように、現在の足の位置と目標歩幅との間の差を共にフィードバックして設定する。
【０１４３】
ロボット１００のＦＳＭがＷ４状態にある場合、スイング足を持ち上げるとき、つま先が最後に地面に着いているつま先離れ（Ｔｏｅ-ｏｆｆ）動作が行われるように仮想重力を設定する。足首ピッチのサポート重力に対して１の比重をおいて、ロボット１００の体重が支持足に偏ることに比例して、サポート重力のｚ成分を増加させる。スイング脚の膝とヒップは１の比重にサポート重力を与えて、体重の移動を助けるようにし、次のＦＳＭの状態でスイング足を持ち上げることに備えて、体重が支持足に偏ることによって、サポート重力の比重を減らし、リフト重力の比重を増やす。
【０１４４】
ロボット１００のＦＳＭがＷ２’状態にある場合、スイング足のリフト重力のｘ成分が、支持足と平行な位置にくるようにフィードバック制御する。
【０１４５】
ロボット１００のＦＳＭがＷ３’状態にある場合、スイング足が支持足の横に一定の間隔をおいて下りてくるように、リフト重力のｘ、ｙ成分をフィードバックしながら、ｚ成分を時間によって減少させて、自重を用いて地面に着地するようにする。
【０１４６】
重力補償トルク算出部３９０では、第１の仮想重力設定部３６１と第２の仮想重力設定部３６２で設定した仮想重力及び実際の重力に対する重力補償トルクを算出する。重力補償トルクは、下記の数学式（４）によって算出される。
【０１４７】
【数１】
ここで、τは、各関節に対する重力補償トルクを表し、ｇは、重力ベクトル、Ｑは、３×３マトリックスで、関節軸に対するクロス積（ｃｒｏｓｓ ｐｒｏｄｕｃｔ）演算を表し、ｍは、各リンクの質量、ｒは、各リンクの重心オフセット、Ｒは、関節間の相対的座標変換の回転マトリックス、ｐは、各関節間の相対的位置ベクトルを表す。
【０１４８】
重力補償は、主体になる関節と補償しようとする対象リンクとの相対的位置関係によって、２種類に分類され、対象リンクが関節よりも上側にある場合には、サポート重力に対する重力補償が行われ、対象リンクが関節よりも下側にある場合には、リフト重力に対する重力補償が行われる。
【０１４９】
数学式（４）で下付き文字０がベースリンクを表すものと見ると、サポート重力に対する重力補償の場合、ロボットの足関節に対する重力補償トルクをτ０とし、順次に増加してヒップ関節に対する重力補償トルクをτ５とする。
【０１５０】
リフト重力に対する重力補償の場合、サポート重力と反対に、ヒップ関節に対する重力補償トルクをτ０とし、足関節に対する重力補償トルクをτ５とする。
【０１５１】
一つの関節に適用される重力補償は、サポート重力とリフト重力の二つの成分があり、ロボット１００のＦＳＭの状態によって、サポート重力とリフト重力の比重を調節する。すなわち、ロボット１００の特定関節に必要な重力補償トルクは、下記の数学式（５）のように表すことができる。
【０１５２】
τ＝ａ*（サポート重力補償）＋ｂ*（リフト重力補償） （５）
ここで、τは、特定関節の重力補償トルクを表し、ａとｂは、０から１の間の値を持つ実数で、ａとｂの値を調節して、ロボット１００のＦＳＭの状態に合わせて各関節に対する重力補償トルクを計算する。
【０１５３】
ロボット１００の二つの脚に対して重力補償トルクを計算するとき、上体の重量を二つの脚１１０Ｌ、１１０Ｒに分配することができる。したがって、本発明の一実施例によるロボット１００は重量分配部３８０をさらに含むことができる。
【０１５４】
図７に、いずれか一方に傾いたロボットが示されており、図８には、重量分配部とトルク補償部をさらに含むロボットの制御ブロック図が示されている。
【０１５５】
ロボット１００の二つ足１１２Ｌ、１１２Ｒをベースと見て、その上側リンクの重量を考慮した重力補償を足首、膝、ヒップ関節に対して計算する方法は、既存のロボット制御で使用する方法と同一であるが、本発明の一実施例では、ペルヴィス１６及びその上にある胴体に該当するトランク１０２部分の重量は、左右の脚１１０Ｌ、１１０Ｒが分けて負担しなければならない。
【０１５６】
重量分配部３８０は、ロボット１００の重心に対する地面の投影点と、ロボット１００の二つの足１１２Ｌ、１１２Ｒとの間の距離の比率であるｄＲ、ｄＬを用いて、重心の投影点に近い脚が、ペルヴィス１６とトランク１０２の重量をより多く支えるようにする。
【０１５７】
図７を参照すると、右に傾いたロボット１００の重心は、右に偏ることになり、その投影点は右足に近くなる。したがって、ペルヴィス１６とトランク１０２の重量は、右脚１１０Ｒにより多くかかることになり、その比率は、下記の数学式（６）で表すことができる。
【０１５８】
ＷＬ＝ｄＲ／（ｄＲ＋ｄＬ）
ＷＲ＝１−ＷＬ （６）
ここで、ＷＬは、左脚１１０Ｌにかかる重量の比率であり、ＷＲは、右脚１１０Ｒにかかる重量の比率である。重量分配部３８０は、ペルヴィス１６とトランク１０２の質量に、ＷＬが乗算された分だけの重量を左脚１１０Ｌにかけ、ＷＲが乗算された分だけの重量を右脚１１０Ｒにかけるものとする。
【０１５９】
補償モーメント算出部３４０では、ポーズ検出部３０２で測定したペルヴィスリンク１６のロール、ピッチ、ヨー方向の回転角度と目標回転角度との間の差から補償モーメントを算出し、ロール、ピッチ、ヨー方向の３軸に仮想のスプリングが存在すると仮定して、下記の数学式（７）から各関節に必要な補償モーメントを計算することができる。
【０１６０】
Ｍ＝ｋ１*ｅ３ （７）
ｋ１は、仮想のスプリング定数を表し、ｅ３は、ポーズ検出部３０２で測定した回転角度と目標回転角度との間の差を表す。
【０１６１】
目標トルク算出部４００では、補償モーメント算出部３４０で算出した補償モーメント及び重力補償トルク算出部３９０で算出した重力補償トルクを全て反映して、ロボット１００の各関節に必要なトルクを計算する。
【０１６２】
補償モーメントに対する関節トルクを計算するに先立ち、補償モーメントをロボットの左右の足に分配することができる。図８に示されたように、本発明の一実施例によるロボット１００は、補償モーメント分配部３７０をさらに含むことができる。
【０１６３】
補償モーメント分配部３７０は、重量分配部３８０と同様に、ロボット１００の重心の地面に対する投影点と、ロボット１００の二つの足１１２Ｌ、１１２Ｒとの間の距離の比率を用いて、補償モーメントを左右の脚１１０Ｌ、１１０Ｒに分配する。
【０１６４】
補償モーメント算出部３４０で算出した補償モーメントに、上記の数学式（６）で求めたＷＲを乗算すると、右脚１１０Ｒに分配される補償モーメントＭＲを得ることができ、補償モーメント算出部３４０で算出した補償モーメントに、上記の数学式（６）で求めたＷＬを乗算すると、左脚１１０Ｌに分配される補償モーメントＭＬを得ることができる。これを数学式に表すと、下記の数学式（８）の通りである。
【０１６５】
ＭＬ＝ＷＬ＊Ｍ, ＭＲ＝ＷＲ＊Ｍ、 （８）
ここで、ＷＬ＝ｄＲ／（ｄＲ＋ｄＬ）、ＷＲ＝１−ＷＬで、ｄＬ、ｄＲは、重心の投影点と二つの足との間の距離である。
【０１６６】
目標トルク算出部４００は、下記の数学式（９）によって、ロボット１００の左右の脚に、補償モーメント分配部３７０で算出した各補償モーメントを発生させるために必要な関節トルクである第１の目標トルクを計算する。
【０１６７】
【数２】
ここで、ＪＴは、左右の脚１１０Ｌ、１１０Ｒからロボット１００の重心の位置に対するヤコビアン行列であり、デカルト空間（Ｃａｒｔｅｓｉａｎ Ｓｐａｃｅ）上のモーメントを関節トルクに変換するために使用した。ロボット１００の左右の足をベースと見て、ロボット１００の重心をペルヴィスリンク１６上に存在するエンドエフェクタ（ｅｎｄ-ｅｆｆｅｃｔｏｒ）と見るときの６×６ヤコビアン行列Ｊを計算する。
【０１６８】
目標トルク算出部４００は、数学式（９）で算出した第１の目標トルクに、重力補償トルク算出部３９０で算出した重力補償トルクを加えて、第２の目標トルクτ２、L、τ２、Ｒを算出する。便宜上、第２の目標トルクをτ２、L、τ２、Ｒで表したが、第２の目標トルクは、左右の脚の各関節（合計１２個の関節）ごとに算出される。
【０１６９】
ロボット１００の各関節に、速度に比例して抵抗するダンピングトルクを加えると、より安定した歩行を具現できる。図８を参照すると、本発明の一実施例によるロボット１００は、トルク補償部４１０をさらに含むことができる。
【０１７０】
トルク補償部は、目標トルク算出部４００で算出した第２の目標トルクに各関節のダンピングトルクを加えて、第３の目標トルクを算出する。これは下記の数学式（１０）で表すことができる。
【０１７１】
τ３、Ｌ＝τ２、Ｌ＋ｄ*ｗ τ３、Ｒ＝τ２、Ｒ＋ｄ*ｗ （１０）
ここで、τ３、Ｌ、τ３、Ｒはそれぞれ、左右の脚の第３の目標トルクを表し、ｄは、ダンピング係数、ｗは、関節の角速度を表す。便宜上、第３の目標トルクを左、右に関するものに区分したが、第２の目標トルクが各関節ごとに計算されるので、第３の目標トルクも各関節に対して計算される。
【０１７２】
サーボ制御部４２０は、トルク補償部から最終目標トルクである第３の目標トルクの伝送を受け、ロボット１００の各関節のサーボモーターが第３の目標トルクを発生させるように、トルクサーボを制御する。各関節のトルクサーボは、関節トルク検出部３０４から各関節の現在の出力トルク値を受信し、第３の目標トルクと測定された現在のトルク値との間の差だけのトルクをさらに発生させるようにするトルク制御信号を、各関節の駆動回路に送ることができる。
【０１７３】
上述した本発明の一実施例によるロボット１００の各制御ブロックの機能を簡単に整理すると、センサ部３００では、ロボット１００のポーズ角度（ロール、ピッチ、ヨー方向の角度として表現可能）、各関節の回転角度及び出力トルク値などを測定し、運動学計算部３１０では、センサ部３００で測定したロボットの状態情報を用いて、重心の位置、速度、ヒップ高さを計算し、計算された重心の位置と速度を合成して、ロボットの現在のキャプチャポイントの位置を算出する。
【０１７４】
ＦＳＭ制御部３３０は、ロボットの動作を示す有限個の状態を格納し、現在のロボットのＦＳＭの状態を判断し、現在の状態から次の状態に遷移される条件を満足するか否かを決定して、条件を満足する場合、ロボットの動作を次のＦＳＭの状態に遷移させるために、各制御ブロックに信号を送る。
【０１７５】
目標設定部３２０は、ＦＳＭ制御部３３０で判断した現在のＦＳＭの状態によって目標キャプチャポイントを設定し、ロボット１００の目標ヒップ高さ及びペルヴィスリンク１６の目標ポーズ角度を設定する。
【０１７６】
補償力算出部３５０は、目標キャプチャポイントと現在のキャプチャポイントとの間の差、及び目標ヒップ高さと現在のヒップ高さとの間の差を用いて、補償力を計算し、補償モーメント算出部３４０は、ペルヴィスリンク１６の目標ポーズ角度と現在のポーズ角度との間の差を用いて、補償モーメントを計算する。
【０１７７】
第１の仮想重力設定部３６１は、補償力算出部３５０で算出した補償力に対する第１の仮想重力を設定し、第２の仮想重力設定部３６２は、ＦＳＭ制御部３３０で判断したロボット１００の現在のＦＳＭの状態による第２の仮想重力を設定する。
【０１７８】
重量分配部３８０は、ロボット１００の重心と左右の脚１１０Ｌ、１１０Ｒとの間の距離の比率を用いて、ロボット１００の上体の重量を左右の脚１１０Ｌ、１１０Ｒにそれぞれ分配し、重力補償トルク算出部３９０は、左右の脚１１０Ｌ、１１０Ｒに分配された上体の重量を考慮して、第１の仮想重力と第２の仮想重力を補償するための重力補償トルクを算出する。
【０１７９】
補償モーメント分配部３７０は、重量分配部３８０と同じ比率を用いて、補償モーメント算出部３４０で算出した補償モーメントをロボット１００の左右の脚１１０Ｌ、１１０Ｒに分配し、目標トルク算出部４００では、左右の脚１１０Ｌ、１１０Ｒに分配された補償モーメントを考慮して、左右の脚１１０Ｌ、１１０Ｒの各関節に補償モーメントを発生させるための第１の目標トルクを算出する。そして、第１の目標トルクに重力補償トルクを合算して、第２の目標トルクを算出する。
【０１８０】
トルク補償部４１０は、第２の目標トルクにダンピングトルクを合算して、第３の目標トルクを算出し、サーボ制御部４２０は、各関節に第３の目標トルクを発生させるために必要な制御信号を各トルクサーボに伝送する。
【０１８１】
以下、本発明の一実施例によるロボットの歩行制御方法について詳細に説明する。
【０１８２】
図９は、本発明の一実施例によるロボットの制御方法のフローチャートである。
【０１８３】
ロボットの上体のポーズ角度及び複数の関節部の角度を検出（５０１）し、、ポーズ角度及び複数の関節部の角度に基づいて現在の姿勢を獲得（５０２）する。ロボットの現在の姿勢は、前述したように並進運動の自由度に対するロボットの現在の位置、及び回転運動の自由度に対するロボットの現在のポーズ角度として表すことができる。
【０１８４】
次に、複数の並進運動の自由度のうち少なくとも一つの並進運動の自由度に基づいた姿勢及び回転運動の自由度に基づいた姿勢と、ロボットの現在のＦＳＭの状態によって設定された目標姿勢とを比較して、姿勢エラーを算出（５０３）する。
【０１８５】
この時、検出されたポーズ角度及び複数の関節部の角度に基づいて、現在の重心の位置を少なくとも一つの並進運動の自由度に対して検出し、少なくとも一つの並進運動の自由度に対して検出された現在の重心と予め設定された目標重心とを比較して、重心エラーを算出する。すなわち、少なくとも一つの自由度に対する重心エラーを算出する。
【０１８６】
ここで、重心エラーは、少なくとも一つの自由度に対する姿勢エラーとして用いられる。
【０１８７】
または、検出されたポーズ角度及び複数の関節部の角度に基づいて、現在の重心の位置と速度を少なくとも一つの自由度に対して検出し、検出された現在の重心の位置と速度に基づいて、現在のキャプチャポイントを少なくとも一つの自由度に対して獲得し、現在のキャプチャポイントと予め設定されたキャプチャポイントとを比較して、キャプチャポイントエラーを少なくとも一つの自由度に対して算出し、算出されたキャプチャポイントエラーを少なくとも一つの自由度の姿勢エラーとして用いることも可能である。
【０１８８】
また、検出されたポーズ角度及び複数の関節部の角度に基づいて、ヒップまたは上体のいずれか一つの点の位置を少なくとも一つの自由度に対する現在の参照点として獲得し、現在の参照点と予め設定された目標参照点との位置を比較して、参照点位置エラーを少なくとも一つの自由度に対して算出し、参照点位置エラーを少なくとも一つの自由度の姿勢エラーとして用いることも可能である。
【０１８９】
なお、残りの並進運動の自由度の姿勢エラーの算出は、少なくとも一つの並進運動の自由度の姿勢エラーの算出と同一または異なってもよい。
【０１９０】
回転運動の自由度に対する姿勢エラーは、ロボットの現在のポーズ角度と目標ポーズ角度との差から算出することができる。
【０１９１】
そして、算出された姿勢エラーに基づいて、少なくとも一つの並進運動の自由度に対する補償力を算出し、回転運動の自由度に対する補償モーメントを算出する（５０４）。
【０１９２】
算出された補償力に対する第１の仮想重力を設定し、ロボットの現在のＦＳＭの状態によって第２の仮想重力を設定する（５０５）。
【０１９３】
そして、第１の仮想重力、第２の仮想重力及び実際の重力を補償するために必要な重力補償トルクを算出する（５０６）。
【０１９４】
ロボットの複数の関節部に補償モーメントを発生させるために必要なトルクと、前記算出された重力補償トルクとを合算して、最終目標トルクを算出する（５０７）。
【０１９５】
最後に、最終目標トルクを複数の関節部に出力（５０８）して、ロボットの均衡を制御する。
【０１９６】
図１０には、並進運動の自由度に対するロボットの姿勢をキャプチャポイントとして表すロボットの歩行制御方法に関するフローチャートが示されている。図１０を参照すると、まず、センサ部３００でロボットの現在の状態情報を測定する（５１０）。ロボットの現在の状態情報は、ペルヴィスリンク１６のＩＭＵ１７で測定したポーズ角度、ロボットの足が地面に着いているか否か、各関節のトルク、各関節の角度などを含むことができる。
【０１９７】
そして、運動学計算部３１０で、ロボットの現在の状態情報を用いて、ロボットの現在の重心の位置及び速度を計算し、計算された重心の位置及び速度を用いて、下記の数学式（１）によって、ロボットの現在のキャプチャポイントの位置を３次元空間に対して得る（５２０）。
【０１９８】
ＣＰ＝ｄＣＯＧ＋ｗ*ｖＣＯＧ （１）
ＣＰは、キャプチャポイントを表し、ｄＣＯＧは、重心を表し、ｖＣＯＧは、重心の速度を表す。ｗは、ｗ＝√（ｈ／ｇ）を計算して求めることができ、ここで、ｈは、重心の地面からの高さで、ｇは重力加速度である。キャプチャポイントを２次元水平面（あるいは地面）に制限する場合、数学式（１）で求めたＣＰの座標（ｘ、ｙ、ｚ）から（ｘ、ｙ）のみを取って、地面上の２次元キャプチャポイントを求めることもできる。
【０１９９】
３軸並進運動の自由度に対するロボットの位置をキャプチャポイントとして表す場合、３次元キャプチャポイント（数学式（１））を使用し、ｚ軸を除外した２軸並進運動の自由度に対するロボットの位置をキャプチャポイントとして表す場合、前記２次元キャプチャポイントを使用することができる。また、この場合には、ｚ軸並進運動の自由度に対するロボットの位置は、ロボットのヒップ高さなどのような、どの参照点の位置として表すことができる。
【０２００】
そして、目標設定部３２０で、ロボットの現在のＦＳＭの状態によって目標キャプチャポイントを設定し、目標ポーズ角度を設定する（５３０）。場合によって、目標ヒップ高さも設定することができ、目標ヒップ高さは任意に設定できるが、ＦＳＭの各状態によって一定に設定することが好ましく、目標ポーズ角度も任意に設定できるが、ロボットの上体が直立姿勢を維持するように設定することが好ましい。
【０２０１】
そして、補償力算出部３５０で、目標キャプチャポイントと現在のキャプチャポイントとの間の差を用いて、各関節に必要な補償力を算出する（５４０）。キャプチャポイントを地面の上の点と見なす場合には、目標キャプチャポイントと現在のキャプチャポイントとの間の差から、ｘ、ｙ方向の補償力を算出することができ、ヒップ高さの間の差から、ｚ方向の補償力を算出することができる。
【０２０２】
補償力を算出する際に、目標キャプチャポイントの位置と現在のキャプチャポイントの位置との間に仮想のスプリングがあるものと見なし、下記の数学式（３）を用いることができる。
【０２０３】
ｆ＝ｋ*ｅ （３）
ｅは、目標キャプチャポイントと現在のキャプチャポイントとの間の差を表す座標で、ｅ＝（ｘ’、ｙ’、ｚ’）-（ｘ、ｙ、ｚ）で表すことができ、（ｘ’、ｙ’、ｚ’）は、目標キャプチャポイントの位置座標で、（ｘ、ｙ、ｚ）は、現在のキャプチャポイントの位置座標である。ｋは、仮想のスプリング定数を表す。
【０２０４】
また、補償モーメント算出部３４０では、目標ポーズ角度と現在のポーズ角度との間の差を用いて、補償モーメントを算出する（５５０）。この時、ペルヴィスリンクのロール、ピッチ、ヨーの３軸に対して仮想のスプリングがあると仮定して、下記の数学式（７）によって補償モーメントを算出することができる。
【０２０５】
Ｍ＝ｋ１*ｅ１ （７）
ｋ１は、仮想のスプリング定数を表し、ｅ１は、ＩＭＵで測定したポーズ角度と目標ポーズ角度との間の差を表す。
【０２０６】
補償力の算出と補償モーメントの算出とは前後関係を有するものではなく、５４０段階と５５０段階は、互いに取り替えてもよい。
【０２０７】
そして、第１の仮想重力設定部３６１で、補償力に対する第１の仮想重力を設定し、第２の仮想重力設定部３６２で、ロボット１００の現在のＦＳＭの状態によって第２の仮想重力を設定する（５６０）。
【０２０８】
第１の仮想重力を設定する際に下記の数学式（２）を用いることができる。
【０２０９】
ｇｆ＝ｆ／ｍ （２）
ここで、ｇｆは、第１の仮想重力を表し、ｆは、補償力算出部３５０で算出した補償力を表し、ｍは、ロボットの質量を表す。
【０２１０】
第２の仮想重力は、ＦＳＭの状態によって変わり、第２の仮想重力を設定する方法は、先に本発明の一実施例によるロボットにおいて説明した通りである。
【０２１１】
そして、重力補償トルク算出部３９０では、第１の仮想重力、第２の仮想重力及び実際の重力を補償するために必要な重力補償トルクを算出する（５７０）。重力補償トルクを計算する一般的な式は、前述した数学式（４）で表現することができる。
【０２１２】
【数３】
ここで、τは、各関節に対する重力補償トルクを表し、ｇは、補償しようとする重力ベクトル、Ｑは、３×３マトリックスで、関節軸に対するクロス積（ｃｒｏｓｓ ｐｒｏｄｕｃｔ）演算を表し、ｍは、各リンクの質量、ｒは、各リンクの重心オフセット、Ｒは、関節間の相対的座標変換の回転マトリックス、ｐは、各関節間の相対的位置ベクトルを表す。
【０２１３】
そして、目標トルク算出部４００で、各関節に補償モーメントを発生させるために必要な第１の関節トルクを算出し、第１の関節トルクに重力補償トルクを合算して、第２の関節トルクを算出する（５８０）。
【０２１４】
最後に、サーボ制御部４２０で、各関節に第２の関節トルクを発生させるために、各トルクサーボを制御する（５９０）。
【０２１５】
本発明の一実施例によるロボットの歩行制御方法では、重力補償トルクを算出する際に、ロボット１００の上体の重量を左右の脚に分配する過程をさらに含むことができ、補償モーメントに対する第１の関節トルクを算出する際に、補償モーメントをロボットの左右の脚に分配する過程をさらに含むことができる。
【０２１６】
前記実施例によるロボットの歩行制御方法に関するフローチャートが図１１に示されている。下記の実施例では、並進運動の自由度に対するロボットの位置をキャプチャポイント及びヒップ高さとして表す。
【０２１７】
図１１を参照して、重量分配及び補償モーメント分配を重点的に説明する。センサ部３００で、ロボットの現在の状態情報を測定し（６１０）、運動学計算部３１０で、測定された状態情報を用いて、ロボットの現在の重心の位置、速度及びヒップ高さを計算する（６２０）。
【０２１８】
目標設定部３２０で、ロボットの現在のＦＳＭの状態に基づいて目標キャプチャポイントを設定し、目標ヒップ高さ及び目標ペルヴィスリンク回転角を設定する（６３０）。
【０２１９】
補償力算出部３５０で、目標キャプチャポイントの位置と現在のキャプチャポイントの位置との間の差から、ｘ、ｙ方向の補償力、及び目標ヒップ高さと現在のヒップ高さとの間の差からｚ方向の補償力を算出し、目標ポーズ角度と現在のポーズ角度との間の差から、補償モーメントを算出する（６４０）。
【０２２０】
仮想重力設定部３６０で、補償力に対する第１の仮想重力を設定し、ロボットのＦＳＭの状態による第２の仮想重力を設定する（６５０）。
【０２２１】
そして、重量分配部３８０で、ロボットのペルヴィスリンクとその上の胴体（以下、トランクという。）の重量を左右の脚に分けて分配する（６６０）。先に検討した図７を再び参照すると、ロボット１００が、いずれか一方に傾いている場合、その重心は、胴体が傾いた方向に偏ることになる。したがって、重心と近い側の足により多くの重量がかかり、ロボット１００の重心を地面に投影した投影点とロボット１００の左足との間の距離の比率をｄＬとし、投影点とロボットの右足との間の距離の比率をｄＲとすると、ロボット１００の左右の脚１１０Ｌ、１１０Ｒにかかるペルヴィスリンク１６とトランク１０２の重量の比率は、下記の数学式（６）で表すことができる。
【０２２２】
ＷＬ＝ｄＲ／（ｄＲ＋ｄＬ）
ＷＲ＝１−ＷＬ （６）
ここで、ＷＬは、左脚にかかる重量の比率を表し、ＷＲは、右脚にかかる重量の比率を表す。ペルヴィスリンクとトランクの質量に、ＷＬを乗算した分だけの質量が左脚にかかり、ＷＲを乗算した分だけの質量が右脚にかかる。ロボットが、図７のように、右に傾いた姿勢を取る場合、右脚により多くの重量がかかることになる。
【０２２３】
そして、重力補償トルク算出部３９０で、左右の脚１１０Ｌ、１１０Ｒに配分された重量を考慮して、第１の仮想重力及び第２の仮想重力を補償するために必要な重力補償トルクを算出する（６７０）。
【０２２４】
補償モーメント分配部３７０では、補償モーメント算出部３４０で算出した補償モーメントを、ロボット１００の左右の脚１１０Ｌ、１１０Ｒに分配する（６８０）。分配の比率は、前述した重量分配の比率と同一に適用することができ、算出された補償モーメントに、数学式（６）でのＷＬを乗算した値が左脚に分配される補償モーメントで、ＷＲを乗算した値が右脚に分配される補償モーメントである。
【０２２５】
そして、目標トルク算出部４００は、左右の脚１１０Ｌ、１１０Ｒに分配された補償モーメントを考慮して、各関節に必要な第１の目標トルクを算出し、ここに重力補償トルク算出部３９０で算出した重力補償トルクを合算して、第２の目標トルクを算出する（６９０）。
【０２２６】
さらに、ロボット１００の安全性のために、各関節の速度に比例して抵抗するダンピングトルクを加える過程をさらに含むことができる（７００）。目標トルク算出部４００で算出した第２の目標トルクにダンピングトルクを加えると、最終的に第３の目標トルクを得ることができ、下記の数学式（１０）で表すことができる。
【０２２７】
τ３、Ｌ＝τ２、Ｌ＋ｄ*ｗ τ３、Ｒ＝τ２、Ｒ＋ｄ*ｗ （１０）
ここで、τ３、Ｌ、τ３、Ｒはそれぞれ、左右の脚１１０Ｌ、１１０Ｒの第３の目標トルクを表し、ｄは、ダンピング係数、ｗは、関節の角速度を表す。
【０２２８】
サーボ制御部４２０では、左右の脚１１０Ｌ、１１０Ｒの各関節に対して算出された第３の目標トルクを各関節に発生させるために、各関節のトルクサーボを制御する（７１０）。
【符号の説明】
【０２２９】
３００ センサ部
３１０ 運動学計算部
３２０ 目標設定部
３３０ 有限状態機械制御部
３４０ 補償モーメント算出部
３５０ 補償力算出部
３６０ 仮想重力設定部
３９０ 重力補償トルク算出部
【特許請求の範囲】
【請求項１】
複数の関節部がそれぞれ設けられた複数の脚と、前記脚に連結された上体とを有し、複数の自由度を有するロボットにおいて、
前記ロボットの状態情報を測定するセンサ部と、
前記センサ部で測定された状態情報を用いて、前記ロボットの現在の姿勢を獲得する運動学計算部、
予め設定された有限状態機械（ＦＳＭ）に基づいて、前記複数の自由度のうち少なくとも一つの自由度に対して目標姿勢を設定する目標設定部と、
前記現在の姿勢と目標姿勢とを比較して姿勢エラーを算出し、前記姿勢エラーに基づいて前記少なくとも一つの自由度に対して補償力を算出する補償力算出部と、
前記算出された補償力によって前記複数の関節部に必要な仮想重力の大きさ、及び前記ロボットの現在のＦＳＭの状態によって前記複数の関節部に必要な仮想重力の大きさを設定する仮想重力設定部と、
前記仮想重力設定部で設定された仮想重力を補償するための重力補償トルクを算出する重力補償トルク算出部と、
前記複数の関節部に前記重力補償トルクを出力するサーボ制御部と、を含むロボット。
【請求項２】
前記センサ部は、
前記上体のポーズ角度を検出するポーズ検出部と、
前記複数の関節部の角度を検出する角度検出部とを含む、請求項１に記載のロボット。
【請求項３】
前記運動学計算部は、
前記上体のポーズ角度及び複数の関節部の角度に基づいて重心を算出し、前記重心に基づいて前記現在の姿勢を獲得する、請求項２に記載のロボット。
【請求項４】
前記運動学計算部は、
前記上体のポーズ角度及び複数の関節部の角度に基づいて重心の位置と速度を算出し、前記重心の位置と速度に基づいて現在のキャプチャポイントを算出することによって、前記現在の姿勢を獲得する、請求項２に記載のロボット。
【請求項５】
前記運動学計算部は、
前記上体のポーズ角度及び前記複数の関節部の角度に基づいてヒップや上体の参照点の位置を獲得し、前記参照点の位置に基づいて前記現在の姿勢を獲得する、請求項２に記載のロボット。
【請求項６】
前記目標設定部は、
前記少なくとも一つの自由度に対して目標重心を設定し、
前記目標重心を用いて前記目標姿勢を設定する、請求項３に記載のロボット。
【請求項７】
前記目標設定部は、
前記少なくとも一つの自由度に対して目標キャプチャポイントを設定し、
前記目標キャプチャポイントを用いて前記目標姿勢を設定する、請求項４に記載のロボット。
【請求項８】
前記目標設定部は、
前記複数の脚に設けられた足の支持領域内の一点が重力方向に通る線上の一点を目標キャプチャポイントとして設定する、請求項７に記載のロボット。
【請求項９】
前記目標設定部は、
前記少なくとも一つの自由度に対して前記ロボットのヒップや上体の参照点の目標位置を設定し、前記参照点の目標位置に基づいて前記目標姿勢を設定する、請求項５に記載のロボット。
【請求項１０】
前記運動学計算部は、
前記ポーズ角度及び前記複数の関節部の角度に基づいて現在のヒップ高さをさらに獲得し、
前記補償力算出部は、
前記現在のキャプチャポイントと前記目標キャプチャポイントとを比較してキャプチャポイントエラーを算出し、前記現在のヒップ高さと目標ヒップ高さとを比較してヒップ高さエラーを算出し、前記キャプチャポイントエラーとヒップ高さエラーに基づいて補償力を算出する、請求項７に記載のロボット。
【請求項１１】
前記補償力算出部は、
前記キャプチャポイントエラーを用いて水平方向の補償力を算出し、前記ヒップ高さエラーを用いて垂直方向の補償力を算出する、請求項１０に記載のロボット。
【請求項１２】
前記ポーズ検出部で検出するポーズ角度は、
前記ロボットのペルヴィスリンクのロール方向（ｒｏｌｌ、Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）、ヨー方向（ｙａｗ、Ｚ軸回転）の回転角度のうち少なくとも一つを含み、
前記複数の関節部のトルクを測定する関節トルク検出部をさらに含む、請求項２に記載のロボット。
【請求項１３】
前記運動学計算部は、
前記ロボットの現在の重心の位置、速度及びヒップ高さを計算し、前記重心の位置及び速度を合成して、前記ロボットの現在のキャプチャポイントの位置を計算する、請求項１２に記載のロボット。
【請求項１４】
前記ロボットの有限状態機械（ＦＳＭ）を予め格納し、前記ロボットの現在のＦＳＭの状態を判断し、前記ロボットがＦＳＭの状態遷移の条件を満足するか否かによって、次のＦＳＭの状態に遷移するように制御するＦＳＭ制御部をさらに含む、請求項１３に記載のロボット。
【請求項１５】
前記目標設定部は、
前記ロボットの現在のＦＳＭの状態による目標キャプチャポイント、前記ロボットの上体の姿勢制御のための目標ポーズ角度、及び前記ロボットの目標ヒップ高さを設定する、請求項１４に記載のロボット。
【請求項１６】
前記センサ部で測定したポーズ角度と、前記目標設定部で設定した目標ポーズ角度との間の差を用いて、前記目標ポーズ角度に到達するために補償しなければならない補償モーメントを算出する補償モーメント算出部をさらに含む、請求項５に記載のロボット。
【請求項１７】
前記補償力算出部は、
前記運動学計算部で計算した前記ロボットの現在のキャプチャポイントのｘ方向の位置及びｙ方向の位置と、前記目標設定部で設定した目標キャプチャポイントのｘ方向の位置及びｙ方向の位置との間の差を用いて、ｘ方向及びｙ方向の補償力を算出し、前記運動学計算部で計算した前記ロボットの現在のヒップ高さと、前記目標設定部で設定した目標ヒップ高さとの間の差を用いて、ｚ軸方向の補償力を算出する、請求項１５に記載のロボット。
【請求項１８】
前記仮想重力設定部は、
前記ロボットのＦＳＭの現在の状態による仮想重力、及び前記補償力算出部で算出した補償力による仮想重力の大きさを設定する、請求項１６に記載のロボット。
【請求項１９】
前記仮想重力設定部は、
前記ロボットの質量をｍとし、前記補償力算出部で算出した補償力をｆとするとき、下記の数学式（２）を用いて、前記ロボットの各関節に必要な第１の仮想重力（ｇｆ）を設定する第１の仮想重力設定部と、
前記ロボットの現在のＦＳＭの状態によって、各関節に必要な第２の仮想重力を設定する第２の仮想重力設定部と、を含む、請求項１６に記載のロボット。
ｇｆ＝ｆ／ｍ （２）
【請求項２０】
前記重力補償トルク算出部は、
前記第１の仮想重力及び前記第２の仮想重力、実際の重力を補償するために必要なトルクを算出する、請求項１９に記載のロボット。
【請求項２１】
前記ロボットの重心が地面に投影された点と前記各脚に連結された足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点からより近い脚に、より大きい比率の前記ロボットの上体質量を割り当てる重量分配部をさらに含む、請求項２０に記載のロボット。
【請求項２２】
前記重力補償トルク算出部は、
前記重量分配部で前記各脚に分配した上体の質量を反映して、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクを算出し、前記ロボットのＦＳＭの状態によって、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクの比重を調節して、仮想重力と実際の重力を補償するために必要な各関節の重力補償トルクを算出する、請求項２１に記載のロボット。
【請求項２３】
前記ロボットの重心が地面に投影された点と前記ロボットの複数の足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点から近い脚に、より大きい値の補償モーメントを割り当てることによって、前記補償モーメント算出部で算出された補償モーメントを前記複数の脚に分配する補償モーメント分配部をさらに含む、請求項２２に記載のロボット。
【請求項２４】
前記ロボットの足をベースリンクと見るヤコビアンを用いて、前記補償モーメント分配部で前記ロボットの複数の脚に分配した補償モーメントから前記複数の脚に必要な第１の目標トルクを算出する目標トルク算出部をさらに含む、請求項２３に記載のロボット。
【請求項２５】
前記目標トルク算出部は、
前記第１の目標トルクと前記重力補償トルク算出部で算出した重力補償トルクとを合算して、前記ロボットの各関節に必要な第２の目標トルクを算出する、請求項２４に記載のロボット。
【請求項２６】
前記目標トルク算出部で算出された第２の目標トルクに前記各関節の角速度に比例するダンピングトルクを加えて、第３の目標トルクを算出するトルク補償部をさらに含む、請求項２５に記載のロボット。
【請求項２７】
複数の関節部がそれぞれ設けられた複数の脚と、該脚に連結された上体とを含み、複数の並進運動の自由度及び回転運動の自由度を有するロボットの制御方法において、
前記上体のポーズ角度及び前記複数の関節部の角度を検出し、
前記検出された複数の関節部の角度に基づいて、前記複数の並進運動の自由度のうち少なくとも一つの並進運動の自由度に対する現在の姿勢を獲得し、
前記獲得された現在の姿勢と予め設定された目標姿勢との差に基づいて、姿勢エラーを算出し、
前記姿勢エラーに基づいて、前記少なくとも一つの並進運動の自由度に対する補償力を算出し、
前記少なくとも一つの並進運動の自由度に対する補償力、及び前記ロボットに対して予め設定された有限状態機械（Ｆｉｎｉｔｅ Ｓｔａｔｅ Ｍａｃｈｉｎｅ：ＦＳＭ）の現在の状態に基づいて、仮想重力を設定し、
前記設定された仮想重力を補償するために、前記ロボットの関節部に必要な重力補償トルクを算出し、
前記ロボットの関節部のトルクが前記算出された重力補償トルクを含む目標トルクになるように各関節のトルクサーボを制御する、ロボットの制御方法。
【請求項２８】
前記姿勢エラーを算出することは、
前記検出されたポーズ角度及び前記複数の関節部の角度に基づいて現在の重心の位置を算出するが、前記少なくとも一つの並進運動の自由度に対する現在の重心の位置を算出し、
前記算出された現在の重心と予め設定された目標重心とを比較して、前記少なくとも一つの並進運動の自由度に対する重心エラーを算出し、
前記重心エラーを姿勢エラーとして用いることを含む、請求項２７に記載のロボットの制御方法。
【請求項２９】
前記姿勢エラーを算出することは、
前記検出されたポーズ角度及び複数の関節部の角度に基づいて現在の重心の位置と速度を検出するが、前記少なくとも一つの並進運動の自由度に対する現在の重心の位置と速度を検出し、
前記検出された現在の重心の位置と速度に基づいて、前記少なくとも一つの並進運動の自由度に対する現在のキャプチャポイントを獲得し、
前記獲得された現在のキャプチャポイントと予め設定された目標キャプチャポイントとを比較して、少なくとも一つの並進運動の自由度に対するキャプチャポイントエラーを算出し、
前記算出されたキャプチャポイントエラーを前記姿勢エラーとして用いることを含む、請求項２７に記載のロボットの制御方法。
【請求項３０】
前記姿勢エラーを算出することは、
前記上体のポーズ角度及び前記複数の関節部の角度を検出し、
前記ポーズ角度及び前記複数の関節部の角度に基づいて、ヒップまたは上体のいずれか一つの参照点の位置を前記少なくとも一つの自由度に対して算出し、
前記現在の参照点と予め設定された目標参照点との位置を比較して、参照点位置エラーを前記少なくとも一つの自由度に対して算出し、
前記参照点位置エラーを前記少なくとも一つの並進運動の自由度の姿勢エラーとして用いることを含む、請求項２７に記載のロボットの制御方法。
【請求項３１】
前記姿勢エラーを算出することは、
前記複数の並進運動の自由度のうち残りの並進運動の自由度の姿勢エラーの算出と互いに異なることを含む、請求項２７に記載のロボットの制御方法。
【請求項３２】
前記ロボットの関節部に必要な仮想重力を設定することは、
前記ロボットの質量をｍとし、前記算出した補償力をｆとするとき、下記の数学式（２）を用いて、前記ロボットの関節部に必要な第１の仮想重力を設定し、
前記ロボットの現在のＦＳＭの状態によって、前記関節部に必要な第２の仮想重力を設定することを含む、請求項２７に記載のロボットの制御方法。
ｇｆ＝ｆ／ｍ （２）
【請求項３３】
前記重力補償トルクを算出することは、
前記第１の仮想重力に対する重力補償トルク及び前記第２の仮想重力に対する重力補償トルクを算出し、前記ロボットのＦＳＭの状態によって、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクの比重を調節して、前記ロボットの関節部に提供される重力補償トルクを算出することを含む、請求項３２に記載のロボットの制御方法。
【請求項３４】
前記ロボットが安定した姿勢を維持するための目標ポーズ角度を設定し、
前記目標ポーズ角度と前記測定されたポーズ角度との間の差を補償するための補償モーメントを算出することをさらに含む、請求項２７に記載のロボットの制御方法。
【請求項３５】
前記算出された補償モーメントを前記ロボットの関節部に提供するために必要な第１の目標トルクを算出し、
前記第１の目標トルクと前記重力補償トルクとを合算して、第２の目標トルクを算出することをさらに含み、
前記目標トルクは、第２の目標トルクであるものとする、請求項３４に記載のロボットの制御方法。
【請求項３６】
前記算出された第２の目標トルクに前記関節部の角速度に比例するダンピングトルクを加えて、第３の目標トルクを算出することをさらに含み、
前記目標トルクは、前記第３の目標トルクであるものとする、請求項３５に記載のロボットの制御方法。
【請求項３７】
前記ロボットの重心が地面に投影された点と前記ロボットの複数の足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点から近い脚に、より大きい値の補償モーメントを割り当てることによって、前記算出された補償モーメントを前記複数の脚に分配することをさらに含む、請求項３５に記載のロボットの制御方法。
【請求項３８】
前記第１の目標トルクを算出することは、
前記ロボットの足をベースリンクと見るヤコビアンを用いて、前記ロボットの複数の脚に分配された補償モーメントから前記複数の脚に必要な第１の目標トルクを算出することを含む、請求項３７に記載のロボットの制御方法。
【請求項１】
複数の関節部がそれぞれ設けられた複数の脚と、前記脚に連結された上体とを有し、複数の自由度を有するロボットにおいて、
前記ロボットの状態情報を測定するセンサ部と、
前記センサ部で測定された状態情報を用いて、前記ロボットの現在の姿勢を獲得する運動学計算部、
予め設定された有限状態機械（ＦＳＭ）に基づいて、前記複数の自由度のうち少なくとも一つの自由度に対して目標姿勢を設定する目標設定部と、
前記現在の姿勢と目標姿勢とを比較して姿勢エラーを算出し、前記姿勢エラーに基づいて前記少なくとも一つの自由度に対して補償力を算出する補償力算出部と、
前記算出された補償力によって前記複数の関節部に必要な仮想重力の大きさ、及び前記ロボットの現在のＦＳＭの状態によって前記複数の関節部に必要な仮想重力の大きさを設定する仮想重力設定部と、
前記仮想重力設定部で設定された仮想重力を補償するための重力補償トルクを算出する重力補償トルク算出部と、
前記複数の関節部に前記重力補償トルクを出力するサーボ制御部と、を含むロボット。
【請求項２】
前記センサ部は、
前記上体のポーズ角度を検出するポーズ検出部と、
前記複数の関節部の角度を検出する角度検出部とを含む、請求項１に記載のロボット。
【請求項３】
前記運動学計算部は、
前記上体のポーズ角度及び複数の関節部の角度に基づいて重心を算出し、前記重心に基づいて前記現在の姿勢を獲得する、請求項２に記載のロボット。
【請求項４】
前記運動学計算部は、
前記上体のポーズ角度及び複数の関節部の角度に基づいて重心の位置と速度を算出し、前記重心の位置と速度に基づいて現在のキャプチャポイントを算出することによって、前記現在の姿勢を獲得する、請求項２に記載のロボット。
【請求項５】
前記運動学計算部は、
前記上体のポーズ角度及び前記複数の関節部の角度に基づいてヒップや上体の参照点の位置を獲得し、前記参照点の位置に基づいて前記現在の姿勢を獲得する、請求項２に記載のロボット。
【請求項６】
前記目標設定部は、
前記少なくとも一つの自由度に対して目標重心を設定し、
前記目標重心を用いて前記目標姿勢を設定する、請求項３に記載のロボット。
【請求項７】
前記目標設定部は、
前記少なくとも一つの自由度に対して目標キャプチャポイントを設定し、
前記目標キャプチャポイントを用いて前記目標姿勢を設定する、請求項４に記載のロボット。
【請求項８】
前記目標設定部は、
前記複数の脚に設けられた足の支持領域内の一点が重力方向に通る線上の一点を目標キャプチャポイントとして設定する、請求項７に記載のロボット。
【請求項９】
前記目標設定部は、
前記少なくとも一つの自由度に対して前記ロボットのヒップや上体の参照点の目標位置を設定し、前記参照点の目標位置に基づいて前記目標姿勢を設定する、請求項５に記載のロボット。
【請求項１０】
前記運動学計算部は、
前記ポーズ角度及び前記複数の関節部の角度に基づいて現在のヒップ高さをさらに獲得し、
前記補償力算出部は、
前記現在のキャプチャポイントと前記目標キャプチャポイントとを比較してキャプチャポイントエラーを算出し、前記現在のヒップ高さと目標ヒップ高さとを比較してヒップ高さエラーを算出し、前記キャプチャポイントエラーとヒップ高さエラーに基づいて補償力を算出する、請求項７に記載のロボット。
【請求項１１】
前記補償力算出部は、
前記キャプチャポイントエラーを用いて水平方向の補償力を算出し、前記ヒップ高さエラーを用いて垂直方向の補償力を算出する、請求項１０に記載のロボット。
【請求項１２】
前記ポーズ検出部で検出するポーズ角度は、
前記ロボットのペルヴィスリンクのロール方向（ｒｏｌｌ、Ｘ軸回転）、ピッチ方向（ｐｉｔｃｈ、Ｙ軸回転）、ヨー方向（ｙａｗ、Ｚ軸回転）の回転角度のうち少なくとも一つを含み、
前記複数の関節部のトルクを測定する関節トルク検出部をさらに含む、請求項２に記載のロボット。
【請求項１３】
前記運動学計算部は、
前記ロボットの現在の重心の位置、速度及びヒップ高さを計算し、前記重心の位置及び速度を合成して、前記ロボットの現在のキャプチャポイントの位置を計算する、請求項１２に記載のロボット。
【請求項１４】
前記ロボットの有限状態機械（ＦＳＭ）を予め格納し、前記ロボットの現在のＦＳＭの状態を判断し、前記ロボットがＦＳＭの状態遷移の条件を満足するか否かによって、次のＦＳＭの状態に遷移するように制御するＦＳＭ制御部をさらに含む、請求項１３に記載のロボット。
【請求項１５】
前記目標設定部は、
前記ロボットの現在のＦＳＭの状態による目標キャプチャポイント、前記ロボットの上体の姿勢制御のための目標ポーズ角度、及び前記ロボットの目標ヒップ高さを設定する、請求項１４に記載のロボット。
【請求項１６】
前記センサ部で測定したポーズ角度と、前記目標設定部で設定した目標ポーズ角度との間の差を用いて、前記目標ポーズ角度に到達するために補償しなければならない補償モーメントを算出する補償モーメント算出部をさらに含む、請求項５に記載のロボット。
【請求項１７】
前記補償力算出部は、
前記運動学計算部で計算した前記ロボットの現在のキャプチャポイントのｘ方向の位置及びｙ方向の位置と、前記目標設定部で設定した目標キャプチャポイントのｘ方向の位置及びｙ方向の位置との間の差を用いて、ｘ方向及びｙ方向の補償力を算出し、前記運動学計算部で計算した前記ロボットの現在のヒップ高さと、前記目標設定部で設定した目標ヒップ高さとの間の差を用いて、ｚ軸方向の補償力を算出する、請求項１５に記載のロボット。
【請求項１８】
前記仮想重力設定部は、
前記ロボットのＦＳＭの現在の状態による仮想重力、及び前記補償力算出部で算出した補償力による仮想重力の大きさを設定する、請求項１６に記載のロボット。
【請求項１９】
前記仮想重力設定部は、
前記ロボットの質量をｍとし、前記補償力算出部で算出した補償力をｆとするとき、下記の数学式（２）を用いて、前記ロボットの各関節に必要な第１の仮想重力（ｇｆ）を設定する第１の仮想重力設定部と、
前記ロボットの現在のＦＳＭの状態によって、各関節に必要な第２の仮想重力を設定する第２の仮想重力設定部と、を含む、請求項１６に記載のロボット。
ｇｆ＝ｆ／ｍ （２）
【請求項２０】
前記重力補償トルク算出部は、
前記第１の仮想重力及び前記第２の仮想重力、実際の重力を補償するために必要なトルクを算出する、請求項１９に記載のロボット。
【請求項２１】
前記ロボットの重心が地面に投影された点と前記各脚に連結された足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点からより近い脚に、より大きい比率の前記ロボットの上体質量を割り当てる重量分配部をさらに含む、請求項２０に記載のロボット。
【請求項２２】
前記重力補償トルク算出部は、
前記重量分配部で前記各脚に分配した上体の質量を反映して、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクを算出し、前記ロボットのＦＳＭの状態によって、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクの比重を調節して、仮想重力と実際の重力を補償するために必要な各関節の重力補償トルクを算出する、請求項２１に記載のロボット。
【請求項２３】
前記ロボットの重心が地面に投影された点と前記ロボットの複数の足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点から近い脚に、より大きい値の補償モーメントを割り当てることによって、前記補償モーメント算出部で算出された補償モーメントを前記複数の脚に分配する補償モーメント分配部をさらに含む、請求項２２に記載のロボット。
【請求項２４】
前記ロボットの足をベースリンクと見るヤコビアンを用いて、前記補償モーメント分配部で前記ロボットの複数の脚に分配した補償モーメントから前記複数の脚に必要な第１の目標トルクを算出する目標トルク算出部をさらに含む、請求項２３に記載のロボット。
【請求項２５】
前記目標トルク算出部は、
前記第１の目標トルクと前記重力補償トルク算出部で算出した重力補償トルクとを合算して、前記ロボットの各関節に必要な第２の目標トルクを算出する、請求項２４に記載のロボット。
【請求項２６】
前記目標トルク算出部で算出された第２の目標トルクに前記各関節の角速度に比例するダンピングトルクを加えて、第３の目標トルクを算出するトルク補償部をさらに含む、請求項２５に記載のロボット。
【請求項２７】
複数の関節部がそれぞれ設けられた複数の脚と、該脚に連結された上体とを含み、複数の並進運動の自由度及び回転運動の自由度を有するロボットの制御方法において、
前記上体のポーズ角度及び前記複数の関節部の角度を検出し、
前記検出された複数の関節部の角度に基づいて、前記複数の並進運動の自由度のうち少なくとも一つの並進運動の自由度に対する現在の姿勢を獲得し、
前記獲得された現在の姿勢と予め設定された目標姿勢との差に基づいて、姿勢エラーを算出し、
前記姿勢エラーに基づいて、前記少なくとも一つの並進運動の自由度に対する補償力を算出し、
前記少なくとも一つの並進運動の自由度に対する補償力、及び前記ロボットに対して予め設定された有限状態機械（Ｆｉｎｉｔｅ Ｓｔａｔｅ Ｍａｃｈｉｎｅ：ＦＳＭ）の現在の状態に基づいて、仮想重力を設定し、
前記設定された仮想重力を補償するために、前記ロボットの関節部に必要な重力補償トルクを算出し、
前記ロボットの関節部のトルクが前記算出された重力補償トルクを含む目標トルクになるように各関節のトルクサーボを制御する、ロボットの制御方法。
【請求項２８】
前記姿勢エラーを算出することは、
前記検出されたポーズ角度及び前記複数の関節部の角度に基づいて現在の重心の位置を算出するが、前記少なくとも一つの並進運動の自由度に対する現在の重心の位置を算出し、
前記算出された現在の重心と予め設定された目標重心とを比較して、前記少なくとも一つの並進運動の自由度に対する重心エラーを算出し、
前記重心エラーを姿勢エラーとして用いることを含む、請求項２７に記載のロボットの制御方法。
【請求項２９】
前記姿勢エラーを算出することは、
前記検出されたポーズ角度及び複数の関節部の角度に基づいて現在の重心の位置と速度を検出するが、前記少なくとも一つの並進運動の自由度に対する現在の重心の位置と速度を検出し、
前記検出された現在の重心の位置と速度に基づいて、前記少なくとも一つの並進運動の自由度に対する現在のキャプチャポイントを獲得し、
前記獲得された現在のキャプチャポイントと予め設定された目標キャプチャポイントとを比較して、少なくとも一つの並進運動の自由度に対するキャプチャポイントエラーを算出し、
前記算出されたキャプチャポイントエラーを前記姿勢エラーとして用いることを含む、請求項２７に記載のロボットの制御方法。
【請求項３０】
前記姿勢エラーを算出することは、
前記上体のポーズ角度及び前記複数の関節部の角度を検出し、
前記ポーズ角度及び前記複数の関節部の角度に基づいて、ヒップまたは上体のいずれか一つの参照点の位置を前記少なくとも一つの自由度に対して算出し、
前記現在の参照点と予め設定された目標参照点との位置を比較して、参照点位置エラーを前記少なくとも一つの自由度に対して算出し、
前記参照点位置エラーを前記少なくとも一つの並進運動の自由度の姿勢エラーとして用いることを含む、請求項２７に記載のロボットの制御方法。
【請求項３１】
前記姿勢エラーを算出することは、
前記複数の並進運動の自由度のうち残りの並進運動の自由度の姿勢エラーの算出と互いに異なることを含む、請求項２７に記載のロボットの制御方法。
【請求項３２】
前記ロボットの関節部に必要な仮想重力を設定することは、
前記ロボットの質量をｍとし、前記算出した補償力をｆとするとき、下記の数学式（２）を用いて、前記ロボットの関節部に必要な第１の仮想重力を設定し、
前記ロボットの現在のＦＳＭの状態によって、前記関節部に必要な第２の仮想重力を設定することを含む、請求項２７に記載のロボットの制御方法。
ｇｆ＝ｆ／ｍ （２）
【請求項３３】
前記重力補償トルクを算出することは、
前記第１の仮想重力に対する重力補償トルク及び前記第２の仮想重力に対する重力補償トルクを算出し、前記ロボットのＦＳＭの状態によって、第１の仮想重力に対する重力補償トルク及び第２の仮想重力に対する重力補償トルクの比重を調節して、前記ロボットの関節部に提供される重力補償トルクを算出することを含む、請求項３２に記載のロボットの制御方法。
【請求項３４】
前記ロボットが安定した姿勢を維持するための目標ポーズ角度を設定し、
前記目標ポーズ角度と前記測定されたポーズ角度との間の差を補償するための補償モーメントを算出することをさらに含む、請求項２７に記載のロボットの制御方法。
【請求項３５】
前記算出された補償モーメントを前記ロボットの関節部に提供するために必要な第１の目標トルクを算出し、
前記第１の目標トルクと前記重力補償トルクとを合算して、第２の目標トルクを算出することをさらに含み、
前記目標トルクは、第２の目標トルクであるものとする、請求項３４に記載のロボットの制御方法。
【請求項３６】
前記算出された第２の目標トルクに前記関節部の角速度に比例するダンピングトルクを加えて、第３の目標トルクを算出することをさらに含み、
前記目標トルクは、前記第３の目標トルクであるものとする、請求項３５に記載のロボットの制御方法。
【請求項３７】
前記ロボットの重心が地面に投影された点と前記ロボットの複数の足との間の距離の比率を算出し、前記算出された距離の比率を用いて、前記ロボットの重心が前記地面に投影された点から近い脚に、より大きい値の補償モーメントを割り当てることによって、前記算出された補償モーメントを前記複数の脚に分配することをさらに含む、請求項３５に記載のロボットの制御方法。
【請求項３８】
前記第１の目標トルクを算出することは、
前記ロボットの足をベースリンクと見るヤコビアンを用いて、前記ロボットの複数の脚に分配された補償モーメントから前記複数の脚に必要な第１の目標トルクを算出することを含む、請求項３７に記載のロボットの制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１２−２４５６１２（Ｐ２０１２−２４５６１２Ａ）
【公開日】平成２４年１２月１３日（２０１２．１２．１３）
【国際特許分類】
【出願番号】特願２０１２−１２３４８２（Ｐ２０１２−１２３４８２）
【出願日】平成２４年５月３０日（２０１２．５．３０）
【出願人】（３９００１９８３９）三星電子株式会社 (8,520)
【氏名又は名称原語表記】Ｓａｍｓｕｎｇ Ｅｌｅｃｔｒｏｎｉｃｓ Ｃｏ．，Ｌｔｄ．
【住所又は居所原語表記】１２９，Ｓａｍｓｕｎｇ−ｒｏ，Ｙｅｏｎｇｔｏｎｇ−ｇｕ，Ｓｕｗｏｎ−ｓｉ，Ｇｙｅｏｎｇｇｉ−ｄｏ，Ｒｅｐｕｂｌｉｃ ｏｆ Ｋｏｒｅａ
【Ｆターム（参考）】
【公開日】平成２４年１２月１３日（２０１２．１２．１３）
【国際特許分類】
【出願日】平成２４年５月３０日（２０１２．５．３０）
【出願人】（３９００１９８３９）三星電子株式会社 (8,520)
【氏名又は名称原語表記】Ｓａｍｓｕｎｇ Ｅｌｅｃｔｒｏｎｉｃｓ Ｃｏ．，Ｌｔｄ．
【住所又は居所原語表記】１２９，Ｓａｍｓｕｎｇ−ｒｏ，Ｙｅｏｎｇｔｏｎｇ−ｇｕ，Ｓｕｗｏｎ−ｓｉ，Ｇｙｅｏｎｇｇｉ−ｄｏ，Ｒｅｐｕｂｌｉｃ ｏｆ Ｋｏｒｅａ
【Ｆターム（参考）】
[ Back to top ]