JP2009248277A - 脚式ロボット、及びその制御方法 - Google Patents

Abstract

【課題】脚式ロボットの歩行を安定化させること。
【解決手段】脚式ロボット１００は、胴体１０と、脚部２０と、脚部２０に設けられた足平部２６と、足平部２６の足裏と路面との距離を検出する距離検出部１６と、胴体１０の姿勢を検出する胴体姿勢検出部１４と、歩容データ５１に基づいて脚部２０の関節を駆動制御する制御部３０と、を備える。制御部３０は、歩容データ５１に基づいて、足平部２６の足裏と路面との間の目標距離を計算する目標距離計算部６１３と、胴体姿勢検出部１４により検出された胴体の姿勢と、歩容データ５１の目標姿勢との偏差が減少するように、目標距離計算部６１３により計算された目標距離の変更量を計算する変更量計算部６２４と、目標距離に変更量を加算した値と、距離検出部１６により検出された距離と、の偏差に基づいて、歩容データ５１を修正する歩容データ修正部と、を有する。
【選択図】図４

Description

本発明は、脚式ロボット、及びその制御方法に関するものである。

近年、脚部の下端に足平部を設けた脚式ロボットにおいて、足平部を、爪先部と爪先関節を介して連結する踵部とから構成して、爪先関節を駆動させながら歩行を行う脚式ロボットが開発されている。

このような脚式ロボットでは、まず、足平部の足裏部分を床面に接触させて支持脚とし、その後に足平部の裏面で床面を押して脚部全体（ロボット全体）を持ち上げるように脚部を駆動することで、次の歩行動作を行う。駆動された脚部は遊脚となる一方、他の脚部が支持脚となり、このように、遊脚と支持脚を交互に繰り返して切替えることで、歩行動作を行うことができる。ここで、支持脚から遊脚へと切り替わる際の離床動作においては、まず踵部を浮かせて、爪先関節を駆動させて爪先立ちしながら脚部の蹴りだし動作を行う。一方、遊脚から支持脚へと切り替わる際の着床動作においては、まず踵部から着床して、爪先部を路面に倣うように制御することで、足平部の足裏全体を路面に着地させる。

一方で、特許文献１において、各足裏の反力センサの検出結果に基づいて、歩行が安定しているか否かを判定し、次の歩行周期に遷移する歩行ロボットが開示されている。また、特許文献２において、足平部に揺動軸を介して爪先部が連結される歩行ロボットが開示されている。
特開２００５−７４６２０号公報 特開２００５−１６９５４４号公報

ところで、例えば、足裏距離センサを用いて歩行制御を行った場合において、足裏距離センサの目標値または計測値が計測レンジ内に入った段階から、足裏倣い制御が可能となる。しかしながら、上記特許文献１の歩行ロボットは、各足裏の反力センサの検出結果を用いて、歩行制御を行っている。このため、各足裏の反力センサが路面に接触するまで、足裏倣い制御に遅れが生じ、歩行が不安定となる虞がある。また、特許文献２において、足平部に揺動軸を介して爪先部が連結される歩行ロボットが開示されているが、足裏距離センサを用いて、路面から足裏面への反力を脚式ロボットの倒立制御に有効に利用するというような技術的思想は開示されていない。

本発明は、かかる課題を解決するためになされたものであり、安定して歩行することができる脚式ロボット、及びその制御方法を提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、該足平部の足裏と路面との距離を検出する距離検出部と、前記胴体の姿勢を検出する胴体姿勢検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、を備え、前記歩容データは、前記胴体の目標姿勢を含む脚式ロボットであって、前記制御部は、前記歩容データに基づいて、前記足平部の足裏と路面との間の目標距離を計算する目標距離計算部と、前記胴体姿勢検出部により検出された前記胴体の姿勢と、前記歩容データの目標姿勢との偏差が減少するように、前記目標距離計算部により計算された前記目標距離の変更量を計算する変更量計算部と、前記目標距離に前記変更量を加算した値と、前記距離検出部により検出された前記距離と、の偏差に基づいて、前記歩容データを修正する歩容データ修正部と、を有する、ことを特徴とする脚式ロボットである。この一態様によれば、脚式ロボットの歩行を安定化させることができる。

また、この一態様において、前記制御部は、前記足平部の足裏の少なくとも一部が路面と接触するように、前記変更量計算部により計算された前記目標距離の変更量を制限する変更量制限部を有していてもよい。これにより、目標距離の変更量を適切に制限することができる。

さらに、この一態様において、前記脚式ロボットの重心位置を検出する重心位置検出部を備え、前記歩容データは、重心位置目標値を更に含み、前記変更量計算部は、前記重心位置検出部により検出された前記重心位置と、前記歩容データの重心位置目標値と、の偏差が減少するように、前記目標距離の変更量を計算してもよい。これにより、脚式ロボットの重心位置の偏差を減少させるように、路面からの反力モーメントを足裏面に作用させることができる。

なお、この一態様において、距離検出部は、爪先部の足裏に設けられた少なくとも３個の距離センサと、前記踵部の足裏に設けられた少なくとも１個の距離センサとの出力信号から前記足平部の足裏と路面との距離を検出してもよい。これにより、足裏と路面との距離を適切に検出することができる。

他方、上記目的を達成するための本発明の一態様は、胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、該足平部の足裏と路面との距離を検出する距離検出部と、前記胴体の姿勢を検出する胴体姿勢検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、を備え、前記足平部は、爪先部と、爪先関節を介して該爪先部に連結された踵部と、から構成され、前記歩容データは、前記爪先部の目標姿勢と、前記爪先関節の目標角度と、前記胴体の目標姿勢と、を含む脚式ロボットの制御方法であって、前記歩容データに基づいて、前記足平部の足裏と路面との間の目標距離を計算する目標距離計算工程と、前記胴体姿勢検出部により検出された前記胴体の姿勢と、前記歩容データの目標姿勢との偏差が減少するように、前記目標距離計算工程で計算された前記目標距離の変更量を計算する変更量計算工程と、前記目標距離に前記変更量を加算した値と、前記距離検出部により検出された前記距離との偏差が減少するように、前記爪先部の目標位置姿勢を補正する爪先部補正量、及び／又は、前記爪先関節の目標角度を補正する爪先関節補正量を計算する爪先補正量計算工程と、前記爪先補正量計算工程で計算された爪先部補正量及び／又は爪先関節補正量に基づいて、前記歩容データを修正する歩容データ修正工程と、を含む、ことを特徴とする脚式ロボットの制御方法であってもよい。この一態様によれば、脚式ロボットの歩行を安定化させることができる。

本発明によれば、脚式ロボットの歩行を安定化させることができる。

発明の実施の形態１．
本実施の形態１に係る脚式ロボットは、胴体と、胴体に連結された脚部と、脚部の下端に設けられた足平部と、足平部の足裏面と路面との距離を検出する距離検出部（足裏距離センサ）と、胴体の姿勢を検出する胴体姿勢検出部（姿勢角センサ）と、歩容データに基づいて脚部の関節を駆動制御する制御部と、を備えている。足平部は、爪先部と爪先関節を介して爪先部に連結された踵部と、から構成されている。歩容データは、爪先部の目標姿勢と爪先関節の目標角度と、胴体の目標姿勢と、を含んでいる。制御部は、歩容データに基づいて、足平部の足裏面と路面との間の目標距離を計算する目標距離計算部（距離センサ目標値計算部）と、胴体姿勢検出部により検出された胴体の姿勢と、歩容データの目標姿勢との偏差が減少するように、目標距離計算部により計算された目標距離の変更量を計算する変更量計算部と、を有している。また、制御部は、目標距離に変更量を加算した値と、距離検出部により検出された距離との偏差が減少するように、爪先部の目標位置姿勢を補正する爪先部補正量、及び、爪先関節の目標角度を補正する爪先関節補正量を計算する爪先補正量算出部と、爪先補正量計算部により計算された爪先部補正量及び爪先関節補正量に基づいて、歩容データを修正する歩容データ修正部と、を有している。

脚式ロボット１００の歩行動作においては、いわゆる足裏倣い制御と倒立振子制御とを組み合わせることで、安定化制御を実現している。足裏倣い制御では、距離検出部１６の検出結果を用いて足平部２６の足裏面と路面Ｓとの相対位置姿勢を所望の状態に制御して、足平部２６の足裏面を路面Ｓに倣わせる制御が行われる。一方、倒立振子制御では、胴体姿勢検出部１４の検出結果を用いて脚式ロボット１００の姿勢を倒立させ、重心位置のずれ量を補正する制御が行われる。

本実施の形態１に係る脚式ロボット１００において、変更量計算部６２４は、胴体姿勢検出部１４により検出された胴体の姿勢と、歩容データ５１の目標姿勢との偏差が減少するように、目標距離計算部６１１により計算された目標距離の変更量を計算する。このため、足平部２６の足裏面に対し、路面Ｓから、上記姿勢偏差を打ち消すような反力モーメントを作用させることができる。したがって、脚式ロボット１００の歩行を安定化させることができる。

図１は、本実施の形態１に係る脚式ロボットの概要を示す図である。脚式ロボット１００は、胴体１０と、胴体１０に連結された２本の脚を有する。尚、図１には、一方の脚部２０のみを示しており、他方の脚部は図示を省略している。胴体１０は、脚式ロボット１００の動作（脚部２０の各関節の動作）を制御する制御部３０と、胴体１０の加速度を検出する加速度センサ１２と、胴体の１０の鉛直方向に対する傾斜角（姿勢角）を検出する姿勢角センサ１４と、を備える。

脚部２０は、股関節２１、膝関節２３、足首関節２５、大腿リンク２２、脛リンク２４、及び足平部としての足平リンク２６、を備える。大腿リンク２２と脛リンク２４とは、直線で模式化して示してある。股関節２１は、胴体１０と大腿リンク２２を揺動可能に連結している。膝関節２３は、大腿リンク２２と脛リンク２４を揺動可能に連結している。足首関節２５は、脛リンク２４と足平リンク２６を揺動可能に連結している。脚部２０の下端には足平リンク２６が設けられる。足平リンク２６は板状の部材であり、足平リンク２６の裏面（足裏面）は平面となっている。

図２は、本実施の形態１に係る脚式ロボット１００の足平リンク２６の構成を説明するための図である。足平リンク２６は、爪先部２６ａと、爪先関節２７を介して爪先部２６ａに連結された踵部２６ｂと、から構成される。脚式ロボット１００の歩行動作中には、爪先関節２７を駆動することで、爪先部２６ａを路面Ｓに対して接地させたまま、踵部２６ｂを浮かせることができる。即ち、踵部２６ｂを浮かせたまま爪先部２６ａのみを接地させることにより、脚式ロボット１００を直立させることができる。

足平リンク２６には、距離検出部としての足裏距離センサ１６が設けられている。足裏距離センサ１６は、足平リンク２６の裏面（足裏面）と路面Ｓとの距離を検出する。図２に示すように、爪先部２６ａ及び踵部２６ｂは、上面視において、略矩形状に形成されている。爪先部２６ａの四隅近傍には、４つの足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄが夫々設けられている。ここでは、爪先部２６ａの爪先側に２つの足裏距離センサ１６ａ及び１６ｄが、踵部方向側に２つの足裏距離センサ１６ｂ及び１６ｃが設けられている。足裏距離センサ１６ａ及び１６ｄは、爪先部２６ａの前方の所定位置における足裏面と路面Ｓとの距離を検出し、足裏距離センサ１６ｂ及び１６ｃは、爪先部２６ａの後方の所定位置における足裏面と路面Ｓとの距離を検出する。踵部２６ｂには、爪先部側とは反対側の二隅に２つの足裏距離センサ１６ｅ及び１６ｆが夫々設けられている。足裏距離センサ１６ｅ及び１６ｆは、踵部２６ｂの後方の所定位置における足裏面と路面Ｓとの距離を検出する。従って、足裏距離センサ１６ａ及び１６ｄが検出する距離と、足裏距離センサ１６ｂ及び１６ｃが検出する距離と、足裏距離センサ１６ｅ及び１６ｆが検出する距離との差から、足平リンク２６の足裏面の路面Ｓに対する傾き及び相対位置を求めることができる。また、足平リンク２６と路面Ｓとの間には、着地時の衝撃を吸収するための衝撃吸収部材が設けられている。衝撃吸収部材は、足裏距離センサ１６と一体的に形成される。

各関節にはモータ１５（図３）が内蔵されており、制御部３０からの指令に基づいて駆動される。モータ１５を駆動することによって、各関節に連結されたリンク同士を揺動させることができる。図示を省略している他方の脚部も、脚部２０と同様の構造を有するため、詳細な説明は省略する。制御部３０が２本の脚部２０の関節（詳細には関節角）を適宜制御することにより、脚式ロボット１００を歩行させることができる。

図１においては、説明の便宜上、脚式ロボット１００が進行する向き（前後方向）をｘ軸、脚式ロボット１００が進行する方向に対して水平方向に直交する向き（左右方向）をｙ軸、脚式ロボット１００の進行する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これら３軸からなる絶対座標系を用いて説明する。即ち、図１において、ｘ軸は紙面に向かって左右方向、ｙ軸は紙面の奥行き方向、ｚ軸は紙面中の上下方向を示す。尚、脚式ロボット１００の胴体１０に対して点Ｏｂを特定し固定する。足平リンク２６に対して点Ｏｆを特定し固定する。

脚式ロボット１００は、記憶部５０に記憶されている歩容データに基づいて制御される。歩容データには、胴体１０の目標位置（目標胴体位置）、胴体１０の目標姿勢角（目標胴体姿勢角）、足平リンク２６の目標位置（目標足平位置）、足平リンク２６の目標姿勢角（目標足平姿勢角）、及び爪先関節２７の目標角度（目標爪先関節角度）のそれぞれの時系列データが含まれる。ここで、目標足平位置、及び目標足平姿勢角として、爪先部２６ａの目標位置姿勢角（目標爪先位置、目標爪先姿勢角）または踵部２６ｂの目標位置姿勢角（目標踵位置、目標踵姿勢角）のどちらか一方の位置及び姿勢角を用いればよい。

歩容データは、シミュレーション等によって脚式ロボット１００を安定して歩行させることができるように作成されている。即ち、目標胴体位置、目標胴体姿勢角、目標足平位置、目標足平姿勢角、及び目標爪先関節角度は、脚式ロボット１００のＺＭＰ位置が接地面に接地した足裏で囲まれた凸包内となる関係を満足するように設定されている。作成された歩容データは、脚式ロボット１００の記憶部５０に記憶される。後述するように、制御部３０は、歩容データに含まれる目標胴体位置等に実胴体位置等を一致させるように各関節を制御する。

目標胴体位置は、絶対座標系に対する特定点Ｏｂの位置で表される。特定点Ｏｂを原点とする胴体座標系を用いる場合には、目標胴体姿勢角は、絶対座標系に対する胴体座標系の傾きで表される。実胴体姿勢角は、胴体１０に備えられた姿勢角センサ１４によって検出することができる。目標足平位置は、絶対座標系に対する特定点Ｏｆの位置で表される。目標足平姿勢角は、路面Ｓに対する足裏面の角度で表される。特定点Ｏｆを原点とする足平座標系を用いる場合には、目標足平姿勢角は、絶対座標系に対する足平座標系の傾きで表してもよい。実足平姿勢角は、後述するように、足平リンク２６に備えられた足裏距離センサ１６によって検出することができる。

脚式ロボット１００では、足平リンク２６を制御するための制御座標系を爪先部２６ａに規定する。即ち、特定点Ｏｆを爪先部２６ａに設け、特定点Ｏｆを原点とする爪先座標系を用いて足平リンク２６の制御を行う。歩容データにおいて、足平リンク２６の目標足平位置と目標足平姿勢角とに基づいて、目標爪先位置、目標爪先姿勢角、及び目標爪先関節角度とが予め求められ、記憶部５０に記憶される。足平リンク２６の制御において、爪先関節角度を用いて爪先部２６ａの位置姿勢が踵部２６ｂの位置姿勢へと変換された後、逆キネマティクスにより各関節の目標角度が求められる。尚、踵部２６ｂに相当する足平リンク２６の後方部に特定点Ｏｆを設けた場合には、目標胴体位置及び目標胴体姿勢角の時系列データを含む腰座標系の目標軌道と、目標足平位置及び目標足平姿勢角の時系列データを含む足先座標系の目標軌道とから、逆キネマティクスにより各関節の目標角度を求めることで制御を行う。

続いて、本実施の形態１に係る脚式ロボット１００の制御システム１の詳細について説明する。制御システム１は、上記倒立制御を実行すると共に、上記足裏倣い制御を実行する。倒立制御は、実胴体位置及び実胴体姿勢角を目標胴体位置及び目標胴体姿勢角に一致させる制御である。尚、胴体姿勢角の代わりに重心位置を用いてもよい。足裏倣い制御は、路面Ｓから見た相対的な実足平位置及び実足平姿勢角をそれぞれ目標足平位置及び目標足平姿勢角に一致する制御である。

図３は、制御システム１の機能構成を示す機能ブロック図である。制御システム１は、制御部３０と、記憶部５０と、姿勢角センサ１４と、モータ１５と、足裏距離センサ１６と、を有している。

記憶部５０には、歩容データ５１及び路面ノミナル位置・傾斜情報５２が記憶されている。歩容データ５１は、目標胴体位置、目標胴体姿勢角、目標足平位置、目標足平姿勢角、及び爪先関節２７の目標角度の時系列データを含む。ここで、目標足平位置として爪先位置または目標踵位置を、目標足平姿勢角として目標爪先姿勢角または目標踵姿勢角のどちらか一方をそれぞれ用いればよい。歩容データ５１上において、足裏面が路面Ｓと接触するときは、足裏面と路面Ｓを面接触状態とするため、目標足平姿勢角（仮想的な接地面に対する仮想的な足裏面の傾き）はゼロに設定されている。また、歩容データ上の各目標値は、脚式ロボット１００のＺＭＰ位置が接地している脚の足裏で囲まれた凸包内となる関係を満たすように決定されている。路面ノミナル位置・傾斜情報５２は、脚式ロボット１００が移動する路面Ｓの存在情報であり、路面Ｓの位置・傾斜に関する基準値を示す。

制御部３０は、記憶部５０に記憶された歩容データ５１等を読み出すと共に、読み出した歩容データ５１等によって特定される脚式ロボット１００の姿勢を実現するために必要な脚部２０の関節角を算出する。そして、制御部３０は、このように算出した関節角に基づく信号をモータ１５に送信する。また、制御部３０は、姿勢角センサ１４及び足裏距離センサ１６からの信号を受けて、モータ１５の駆動量を調整する。

より詳細には、制御部３０は、足裏倣い制御部６１と、倒立制御部６２と、関節角変換部６３と、各軸制御器６４と、を有する。制御部３０内では、実胴体姿勢角（胴体姿勢計測値）と目標胴体姿勢角（胴体姿勢角目標値）との偏差に基づくフィードバック制御系（倒立制御部６２による倒立制御系）と、路面Ｓから見た相対的な実足平姿勢角と目標足平姿勢角の偏差に基づくフィードバック制御系（足裏倣い制御部６１による倣い制御系）と、が含まれる。以後、足裏倣い制御の偏差については、路面Ｓから見た相対的な足平姿勢角に関するものを示すものとする。

足裏倣い制御部６１は、実足平姿勢角を目標足平姿勢角に一致させるように、例えば足裏面が路面Ｓに密着している状態を目標として、踵側が路面Ｓから浮いている場合には、足平リンク２６の爪先側を脛リンク２４に近づける方向に足平リンク２６を回転させる。即ち、足裏面と路面Ｓとの面接触を維持するように足平リンク２６を回転させる。実足平姿勢角は、足平リンク２６が備える足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄの出力値から求められる。尚、足裏倣い制御部６１の詳細については後述する。

倒立制御部６２は、脚式ロボット１００の胴体位置姿勢を目標位置姿勢に維持する機能を果たす。胴体位置姿勢は、実際の胴体位置と胴体姿勢角とを含み、目標位置姿勢は、目標の胴体位置と胴体姿勢角とを含む。実胴体姿勢角は、胴体１０に備えられた姿勢角センサ１４で検出される。姿勢角センサ１４は、例えば胴体１０の角速度を検出するジャイロと、ジャイロの出力（角速度）を積分する積分器と、重力加速度ベクトルを検出する３軸加速度センサと、で構成される。尚、倒立制御部６２の詳細については後述する。

また、制御部３０は図示しない爪先補正量計算部と歩容データ修正部とを備える。爪先補正量計算部は、爪先部２６ａの位置姿勢と爪先関節２７の角度について、まず、足裏距離センサ１６の出力値と、追従すべき目標値に変更量（後述の倣い制御目標値補正量）を加算した値と、の偏差を計算し、歩容データ５１を修正するための爪先補正量（爪先部補正量及び爪先関節補正量）を計算する。歩容データ修正部は、爪先補正量計算部で計算された爪先部補正量から足先目標値補正量を計算する。そして、歩容データ修正部は、足先目標値補正量に基づいて、足裏面と路面Ｓとの位置関係が目標値に追従するように歩容データ５１を修正する。

より詳細には、以下のようにして記憶部５０に記憶された歩容データ（胴体位置姿勢目標値、爪先位置姿勢目標値、及び爪先関節角度目標値の時系列データ）５１が修正され、関節角変換部６３に入力される。図３においては、爪先位置姿勢目標値は、目標爪先位置及び目標爪先姿勢角の時系列データを含む。また、目標爪先位置及び目標爪先姿勢角の代わりに、目標踵位置及び目標踵姿勢角を用いてもよい。

記憶部５０に記憶された目標胴体位置（胴体位置姿勢目標値）は、目標胴体加速度と実胴体加速度の偏差に基づいて補正された後に関節角変換部６３に入力される。目標胴体加速度は、目標胴体位置を２回微分することによって求められる。実胴体加速度は、加速度センサ１２により検出される。

記憶部５０に記憶された目標爪先位置（爪先位置姿勢目標値）は、目標爪先位置と実足爪先位置の偏差に基づいて補正された後に関節角変換部６３に入力される。実爪先位置は、足裏距離センサ１６により検出される。

記憶部５０に記憶された目標胴体姿勢角（胴体姿勢角目標値）は、そのまま関節角変換部６３に入力される。同時に、倒立制御部６２は、目標胴体姿勢角と実胴体姿勢角との偏差（胴体姿勢角偏差）を算出する。また、倒立制御部６２は、胴体姿勢角偏差が減少するように、胴体１０を回転させる胴体補正角を算出する。さらに、倒立制御部６２は、胴体補正角と、その時点における足裏距離センサ１６の位置と、に基づいて、足裏距離センサ目標値（目標距離）を補正するための変更量を算出する。なお、この変更量は、足裏倣い制御部６１における補正量となるため、以下、倣い制御目標値補正量と称す。実胴体姿勢角は、姿勢角センサ１４により検出される。

記憶部５０に記憶された目標爪先姿勢角と実爪先姿勢角の偏差（爪先姿勢角偏差）が求められる。実爪先姿勢角は、足裏距離センサ１６により検出される。また、記憶部５０に記憶された目標爪先関節角度と実爪先関節角度の偏差（爪先関節角度偏差）が求められる。実爪先関節角度は、足裏距離センサ１６により検出される。

足裏倣い制御部６１は、入力された角度（爪先姿勢角偏差と、爪先関節角度偏差と、を加算した角度）を小さくする方向へ足平リンク２６を回転させる足平補正角を求める。記憶部５０に記憶された目標爪先関節角は、上述した足平補正角が加算された後に（足平補正角で補正された後に）関節角変換部６３に入力される。尚、図３においては、足平補正角を足先目標値補正量という。

関節角変換部６３には、以上のようにして修正された歩容データが入力される。これらの値から、関節角変換部６３では、逆キネマティクスの演算によって脚部２０の各関節の目標関節角が算出される。ここで、各目標値は、絶対座標系に対する値で表されている。関節角変換部６３は、目標爪先位置と目標胴体位置との差から、爪先部２６ａと胴体１０との相対位置を計算する。また、関節角変換部６３は、目標爪先姿勢角（足平補正角によって補正されている）と目標胴体姿勢角との差から、爪先部２６ａと胴体１０との相対回転角を計算する。そして、関節角変換部６３は、計算された相対位置と相対回転角とを実現する目標関節角を算出する。

各軸制御器６４は、関節角変換部６３により送信された目標関節角の信号に基づいて、脚部２０を駆動するための各モータ１５の駆動量を特定する。そして、各軸制御器６４は、これらの駆動量でモータ１５を駆動させるためのモータ駆動信号を各モータ１５に対して、送信する。これによって、脚部２０の各関節における駆動量が変更され、脚式ロボット１００の動きが制御される。

続いて、図４乃至図６を参照しながら、本実施の形態１に係る倒立制御部６２について詳細に説明する。図４は、倒立制御部６２の機能構成を示す機能ブロック図である。図５は、倣い制御目標値補正量を計算する際の処理フローを示すフローチャートである。図６は、足裏距離センサ１６の目標距離の変更量を示す簡略図である。なお、図６に示す如く、目標距離の変更量の説明を簡略化するために、まず２次元平面で説明を行う。

図４に示すように、倒立制御部６２は、姿勢偏差計算部６２１と、コントローラ６２２と、リミッタ６２３と、変更量計算部６２４と、を有している。

倒立制御部６２において、まず、姿勢偏差計算部６２１が、胴体姿勢目標値（胴体の目標姿勢）と、胴体姿勢計測値（姿勢角センサ１４により検出した実姿勢）との偏差を計算する（ステップＳ１０１）。そして、コントローラ６２２は、姿勢偏差計算部６２１により計算された胴体の姿勢偏差に基づき、倒立制御補正量（胴体補正角）Δθ_ｙを計算する（ステップＳ１０２）。

また、変更量計算部６２４は、現在の足先位置姿勢と胴体の姿勢偏差とから、足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄ、１６ｅ、１６ｆの位置X_ｉ（ｉ＝１〜ｎ）を計算する（ステップＳ１０３）。さらに、変更量計算部６２４は、コントローラ６２２により計算された倒立制御補正量Δθ_ｙと、足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄ、１６ｅ、１６ｆの位置X_ｉ（ｉ＝１〜ｎ）とに基づいて、足裏倣い制御で使用する足裏面と路面Ｓとの目標距離の変更量Δｈ_ｉ（ｉ＝１〜ｎ）を、下記（１）式により計算する（ステップＳ１０４）。このとき、変更量計算部６２４は、姿勢偏差計算部６２１により計算される胴体１０の姿勢偏差が減少するように、足裏面と路面Ｓとの目標距離の変更量Δｈ_ｉを計算する（変更量計算工程）。なお、下記（１）式において、Ｘ_０は足平リンク２６の回転中心位置とする。
Δｈ_ｉ＝−（Ｘ_ｉ−Ｘ_０）ｔａｎΔθ_ｙ （１）式

上述のように、本実施の形態１に係る倒立制御部６２において、変更量計算部６２４は、姿勢偏差計算部６２１により計算される胴体１０の姿勢偏差が減少するように、足裏面と路面との目標距離の変更量Δｈ_ｉを計算する。そして、後述の如く、足裏面と路面Ｓとの目標距離に変更量Δｈ_ｉが加算されることで、目標距離は補正される。

これにより、例えば、図７（ａ）に示す如く、従来の倒立制御及び足裏倣い制御において、足裏面が路面Ｓに倣うように制御されることがある。しかしながら、図７（ｂ）に示す如く、本実施の形態１に係る倒立制御及び足裏倣い制御によれば、胴体１０の姿勢偏差の方向（胴体１０が倒れている方向）と逆方向（胴体１０が倒れるのを戻す方向）へ打消すように、路面Ｓから足裏面へ反力モーメントを作用させることができる。したがって、脚式ロボット１００の足裏面は、路面Ｓから適切な反力モーメントを得ることができるため、脚式ロボット１００の歩行をより安定化させることができる。

なお、胴体１０が両脚部で支持される両脚支持期間において、脚部２０の上下方向の補正も、上記目標距離の変更量Δｈ_ｉを用いて同時に補正できるため、両脚部の足裏面の相互関係を維持しつつ、脚式ロボット１００の倒立制御が可能となる。

次いで、倒立制御部６２は、足裏距離センサ１６の出力値に基づき、足裏面が路面Ｓから剥がれたか否かを判定する（ステップＳ１０５）。倒立制御部６２は、足裏面が路面Ｓから剥がれていないと判定したとき（ステップＳ１０５のＮＯ）、計算した上記目標距離の変更量Δｈ_ｉを倣い制御目標値補正量として設定し、後述の（ステップＳ１０７）に移行する。一方で、倒立制御部６２は、足裏面が路面から剥がれたと判定したとき（ステップＳ１０５のＹＥＳ）、リミッタ（変更量制限部）６２３を介して、計算した変更量Δｈ_ｉを制限する（ステップＳ１０６）。即ち、リミッタ６２３は、目標距離の変更量Δｈ_ｉの大きさに制限を加えるものである。リミッタ６２３は、入力された変更量Δｈ_ｉの大きさが許容範囲内を超えているときに、許容限界の値を、倣い制御目標値補正量として設定する。このリミッタ６２３を設けることで、足裏倣い制御部６１に入力される倣い制御目標値補正量を制限する。これによって、倣い制御系（足裏倣い制御部６１）が倒立制御系（倒立制御部６２）よりも優勢に作用することを保証する。従って、倣い制御系が優勢に作用して、足裏面が路面Ｓと面接触することを保証することができる。

次いで、倒立制御部６２は、上記計算された倣い制御目標値補正量を、足裏倣い制御部６１へと出力する（ステップＳ１０７）。

なお、上記説明の簡略化のために２次元平面で考えた場合について説明したが、３次元のグローバル座標系を用いて、上記倣い制御目標値補正量を算出することも可能である。

まず、後述する座標系Σの定義について説明する。図８（ａ）及び（ｂ）に示す如く、Σ_ｇ：グローバル座標系、Σ_ｂｄ：目標姿勢における胴体（姿勢センサ）座標系、Σ_ｂｒ：実姿勢における胴体（姿勢センサ）座標系、Σ_ｗｄ：目標姿勢における腰（歩容）座標系、Σ_ｗｒ：実姿勢における腰座標系、Σ_ｌｆｄ：目標姿勢における左爪先（踵側）座標系、Σ_ｒｆｄ：目標姿勢における右爪先（踵側）座標系、Σ_ｌｆｒ：実姿勢における左爪先（踵側）座標系、Σ_ｒｆｒ：実姿勢における右爪先（踵側）座標系、Σ_ｌｔｄ：目標姿勢における左爪先（爪先側）座標系、Σ_ｒｔｄ：目標姿勢における右爪先（爪先側）座標系、Σ_ｌｔｒ：実姿勢における左爪先（爪先側）座標系、Σ_ｒｔｒ：実姿勢における右爪先（爪先側）座標系、Σ_ｌｓｄ：目標姿勢における足裏距離センサ座標系、Σ_ｌｓｒ：実姿勢における足裏距離センサ座標系、と夫々定義する。

また、図９に示す如く、行列及びベクトル表記を下記規則に従うものとする。

：座標系Ａから座標系Ｂへの同次変換行列とする。^ＡＲ_Ｂ：座標系Ａから座標系Ｂへの回転行列とする。^ＡＰ_Ｂ：座標系Ａを基準にして見たときの、座標系Ａの原点から座標系Ｂの原点への位置ベクトルとする。^ＡＰ^Ｃ _Ｄ：座標系Ａを基準にして見たときの、点ｃから点ｄへの位置ベクトルとする。なお、右上の添え字が省略されている場合は、基準とした座標系の原点を各ベクトルの始点とする。この場合、下記等式（ベクトルの回転、基準とする座標系の変更）が成立する。

上記定義に従って、倒立制御部６２は姿勢角センサ１４を用いて、以下のように胴体姿勢の計測を行う。また、姿勢角センサ１４は胴体１０に固定されている。このため、グローバル座標系Σ_ｇを基準とした実姿勢における胴体座標系Σｂｒの回転行列^ｇＲ_ｂｒは、以下のように求めることができる。但し、ヨー角を姿勢角センサ１４によって計測することができるが、ヨー角を胴体１０の目標ヨー角に合わせるとより正確となる。

まず、倒立制御部６２は、胴体目標姿勢をロール角ψ_ｄとピッチ角θ_ｄとヨー角φ_ｄとに分離して、胴体１０の目標ヨー角φ_ｄを下記（２）式により算出する。

^ｇＲ_ｂｄ＝Ｒｏｔ（φ_ｄ、ｚ）Ｒｏｔ（θ_ｄ、ｙ）Ｒｏｔ（ψ_ｄ、ｘ） （２）式

次に、倒立制御部６２は、姿勢角センサ１４により計測されたロール角ψ_ｒとピッチ角θ_ｒと上記目標ヨー角φ_ｄに基づいて、下記（３）式を用いて、実際の胴体１０の回転行列（グローバル座標系）^ｇＲ_ｂｒを算出する。
^ｇＲ_ｂｒ＝Ｒｏｔ（φ_ｄ、ｚ）Ｒｏｔ（θ_ｒ、ｙ）Ｒｏｔ（ψ_ｒ、ｘ） （３）式

但し、上記（２）及び（３）式において、

とする。

次に、姿勢偏差計算部６２１における姿勢偏差の計算方法について、詳細に説明する。姿勢偏差計算部６２１は、目標姿勢から見た実姿勢への回転行列（以下、偏差回転行列を称す）を計算することで、胴体１０の姿勢偏差を計算する。姿勢偏差計算部６２１は、下記（４）式を用いて、胴体１０の姿勢偏差を計算する。
^ｂｄＲ_ｂｒ＝^ｇＲ_ｂｄ ^ＴｇＲ_ｂｒ （４）式

なお、偏差回転行列^ｂｄＲ_ｂｒを下記（５）式のように表現する。

また、上記偏差回転行列^ｂｄＲ_ｂｒの回転軸（目標胴体座標系基準）^ｂｄｋと回転角度αとは、下記（６）式を満たす。

次に、姿勢偏差計算部６２１は、回転軸^ｂｄｋを、下記（７）式を用いてグローバル座標系基準に変換する。
^ｇｋ＝^ｇＲ_ｂｄ ^ｂｄｋ （７）式

さらに、姿勢偏差計算部６２１は、上記算出した回転角度αと回転軸^ｇｋとに基づいて、下記（８）式を用いて、姿勢角偏差をグローバル座標系基準のｘ軸周り姿勢角偏差^ｇｄψとｙ軸周りの姿勢角偏差^ｇｄθとに夫々変換し、コントローラ６２２に対して出力する。

コントローラ６２２は、姿勢偏差計算部６２１により計算されたグローバル座標系基準のｘ軸周り姿勢偏差^ｇｄψに基づいて、伝達関数Ｃ_ψ（ｓ）を用いて、胴体補正角^ｇΔψを算出する。また、コントローラ６２２は、姿勢偏差計算部６２１により計算されたグローバル座標系基準のｙ軸周りの姿勢偏差^ｇｄθに基づいて、伝達関数Ｃ_θ（ｓ）を用いて、胴体補正角^ｇΔθを算出する。

なお、ｘ軸周りとｙ軸周りとで同じ特性のコントローラ６２２を使用することも可能であり、この場合、コントローラ６２２は、回転軸^ｇｋ周りの回転角度αに基づいて、伝達関数Ｃ（ｓ）を用いて、胴体補正角Δαを算出しても良い。

コントローラ６２２は、上記算出した胴体補正角^ｇΔψ、^ｇΔθ、または^ｇΔαを、変更量計算部６２４に対して出力する。なお、コントローラ６２２は、算出した胴体補正角^ｇΔψ、^ｇΔθ、または^ｇΔαが許容限界内を超え、足裏面が路面Ｓから?がれ、不安定になると判断した場合、許容限界値を変更量計算部６２４に出力してもよい。これにより、脚式ロボット１００の歩行がより安定化する。

変更量計算部６２４は、実際の足裏距離センサ１６の位置を計算すべく、歩容の爪先部２６ａ、胴体１０、及び目標ＺＭＰを、実際の脚式ロボット１００の状態に合わせるように、姿勢偏差分だけ腰位置を中心に回転させる（図１０）。なお、以下説明において、^ｇＴ_ｂｄ、^ｇＴ_ｌｆｄ、^ｇＴ_ｒｆｄは、脚式ロボット１００における胴体１０、左爪先部２６ａ、右爪先部２６ａの目標値（前制御サイクルの目標値を使用するのが好ましい）を、夫々表している。また、^ｇＴ_ｂｒ、^ｇＴ_ｌｆｒ、^ｇＴ_ｒｆｒは、脚式ロボット１００の実姿勢角を考慮した目標値を、夫々表している。^ｇＰ^０ _ｚｍｐｄは、グローバル座標系から見た目標ＺＭＰ位置を表しており、^ｇＰ^０ _ｚｍｐｒは、グローバル座標系から見た目標ＺＭＰ位置を、脚式ロボット１００と一緒に姿勢偏差分だけ回転した後の位置を表している。

まず、変更量計算部６２４は、胴体１０に姿勢偏差があるときの爪先部２６ａ、胴体１０の位置姿勢、及び目標ＺＭＰ位置を夫々計算する。なお、姿勢偏差が小さい場合は、この工程を省略してもよい。

まず、腰座標系の目標位置姿勢は、下記（９）式のように表現できる。

また、脚式ロボット１００の姿勢角偏差を考慮した実際の胴体座標系は、下記（１０）式のように表現できる。

さらに、脚式ロボット１００の実際の腰座標系の回転行列は、次のようにして求められる。まず、胴体１０における、腰と胴本体との間の回転行列は、腰関節角度θ_ｗを用いて、下記（１１）式により求めることができる。
^ｗｄＲ_ｂｄ＝^ｗｒＲ_ｂｒ＝Ｒｏｔ（θ_ｗ、ｚ） （１１）式

なお、変更量計算部６２４は、脚式ロボット１００の実際の腰座標系の回転行列^ｇＲ_ｗｒを、姿勢偏差計算部６２１で計算された上記偏差回転行列^ｂｄＲ_ｂｒに基づいて、下記（１２）式を用いて計算する。
^ｇＲ_ｗｒ＝^ｇＲ_ｗｄ ^ｗｄＲ_ｂｄ ^ｂｄＲ_ｂｒ ^ｂｒＲ_ｗｒ
＝^ｇＲ_ｗｄ ^ｗｄＲ_ｂｄ ^ｂｄＲ_ｂｒ ^ｗｄＲ_ｂｄ ^Ｔ （１２）式

他方、脚式ロボット１００の関節角度は上記回転前後で変化しないため、腰と爪先部２６ａとの関係は維持され、下記（１３）式が成立する。
^ｗｄＴ_ｌｆｄ＝^ｗｒＴ_ｌｆｒ、及び、^ｗｄＴ_ｒｆｄ＝^ｗｒＴ_ｒｆｒ （１３）式

したがって、脚式ロボット１００の実際の爪先座標系は、上記（１３）式に基づいて、下記（１４）式のようになる。

また、目標ＺＭＰも姿勢偏差分だけ回転させる。但し、腰座標系から見れば、腰座標系原点から目標ＺＭＰまでの位置ベクトルは、回転しないため、下記（１５）式が成立する。
^ｗｒＰ^ｗｒ _ｚｍｐｒ＝^ｗｄＰ^ｗｄ _ｚｍｐｄ （１５）式

上記（１５）式において、右辺である、目標腰座標系基準の目標腰座標系原点から回転前目標ＺＭＰまでの位置ベクトルは、下記（１６）式により、グローバル座標系基準に変換することができる。
^ｇＰ^ｗｄ _ｚｍｐｄ＝^ｇＲ_ｗｄ ^ｗｄＰ^ｗｄ _ｚｍｐｄ （１６）式

さらに、上記（１６）式の左辺を、グローバル座標系原点からのベクトルに変換すると、下記（１７）式のようになる。
^ｇＰ^０ _ｚｍｐｄ−^ｇＰ^０ _ｗｄ＝^ｇＲ_ｗｄ ^ｗｄＰ^ｗｄ _ｚｍｐｄ （１７）式

したがって、上記（１５）式及び（１７）式により、下記（１８）式が成立する。

^ｗｒＰ^ｗｒ _ｚｍｐｒ＝^ｗｄＰ^ｗｄ _ｚｍｐｄ＝^ｇＲ_ｗｄ ^Ｔ（^ｇＰ^０ _ｚｍｐｄ−^ｇＰ^０ _ｗｄ） （１８）式

グローバル座標系原点から目標ＺＭＰまでの位置ベクトルにおいて、腰座標系の原点位置は回転前後で移動しないため、下記（１９）式が成立する。

次に、変更量計算部６２４は、回転後の爪先部２６ａの位置に基づいて、足裏距離センサ１６の位置を計算する（図１１）。なお、以下説明において、Σ_{ｔｅｍｐ１}：爪先関節座標系（踵側リンクに固定）であり、ｚ軸を回転軸に一致させ、上方をｙ軸、前方をｘ軸とする。Σ_{ｔｅｍｐ２}：爪先関節座標系（爪先リンクに固定）であり、Σ_{ｔｅｍｐ１}を爪先関節角度分だけ回転させた座標系とする。θ_ｌｔｏｅ、θ_ｒｔｏｅ：爪先関節角度とする（爪先が上がる方向を正とする）。

まず、変更量計算部６２４は、爪先部（踵側）２６ａの位置姿勢に基づいて、下記（２０）式により、爪先部（爪先側）２６ａの位置姿勢を求める。以下、左脚を例にして計算を行うが、右脚も左脚と同様に計算を行うことができる。
^ｇＴ_ｌｔｒ＝^ｇＴ_ｌｈｒ ^ｌｈｒＴ_{ｔｅｍｐ１} ^{ｔｅｍｐ１}Ｔ_{ｔｅｍｐ２} ^{ｔｅｍｐ２}Ｔ_ｌｔｒ （２０）式

ここで、上記（２０）式の^ｌｈｒＴ_{ｔｅｍｐ１}及び^{ｔｅｍｐ２}Ｔ_ｌｔｒは、下記（２１）式を用いて、幾何学的に一意に決まる値である。

また、上記（２０）式の^{ｔｅｍｐ１}Ｔ_{ｔｅｍｐ２}は、爪先関節の回転角度に基づいて、下記（２２）式により計算することができる。

したがって、足裏距離センサ１６の位置は、下記（２３）式及び（２４）式を用いて計算することができる。
（Ｉ）踵側の足裏距離センサ１６ｅ、１６ｆの場合

（ＩＩ）爪先側の足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄの場合

次に、変更量計算部６２４は、足裏距離センサ目標値の変更量の計算を行う。変更量計算部６２４は、目標ＺＭＰ（上記求めた目標ＺＭＰの位置ベクトル^ｇＰ^０ _ｚｍｐｒ）を中心にして、上記算出した補正角^ｇΔψ、^ｇΔθだけ両足平リンク２６の足裏面を回転させるための、足裏距離センサ目標値の変更量を計算する（図１２）。

変更量計算部６２４は、上記算出した補正角^ｇΔψ、^ｇΔθに基づいて、下記（２５）式及び（２６）式を用いて、補正回転軸^ｇΔｋ及び回転角Δαを計算する。

さらに、変更量計算部６２４は、この補正回転軸（単位ベクトル）^ｇΔｋと回転角Δαとに基づいて、回転行列^ｇΔＲを計算する。なお、変更量計算部６２４は、この回転行列^ｇΔＲだけ足平リンク２６の足裏面を補正するように、回転後の足裏距離センサ１６の位置を、下記（２７）式により計算する。

変更量計算部６２４は、上記（２７）式に基づいた下記（２８）式により、足裏距離センサ目標値の変更量を計算する。

そして、変更量計算部６２４は、上記（２８）式により計算されたベクトル値のうちＺ成分を計算し、そのＺ成分を足裏距離センサ目標値の変更量（倣い制御目標値補正量）として出力する。

続いて、図１３及び図１４を参照しながら、本実施の形態１に係る足裏倣い制御部６１について詳細に説明する。図１３は、足裏倣い制御部６１の機能構成を示す機能ブロック図である。図１４は、足裏倣い制御部６１による制御処理の概要を説明するためのフローチャートである。図１３に示すように、足裏倣い制御部６１は、距離センサ目標値計算部６１１と、差分器６１２と、爪先偏差計算部６１３と、加算器６１４と、補償器６１５とを有している。

足裏倣い制御部６１は、足裏距離センサ１６の検出結果に基づいて、爪先部２６ａに規定した座標系における爪先位置姿勢目標値と実爪先位置姿勢との偏差（爪先位置姿勢偏差）と、爪先関節角度目標値と実爪先関節角度の偏差（爪先関節角度偏差）とを、夫々独立に計算する。

以下、爪先位置姿勢偏差及び爪先関節角度偏差の計算方法について詳細に説明する。まず、足裏倣い制御部６１において、距離センサ目標値計算部（目標距離計算部）６１１は、歩容データ５１に含まれる爪先位置姿勢目標値（目標爪先位置・姿勢角の軌道）と、路面の存在情報である路面のノミナル位置・傾斜情報５２とから、足裏距離センサ１６の高さ目標値である足裏距離センサ目標値（足裏面と路面との間の目標距離）を計算する（目標距離計算工程）（ステップＳ２０１）。言い換えると、爪先部２６ａの目標位置・姿勢角を示す軌道データと、爪先部２６ａが実際に着地する地点（ノミナル位置・傾斜）とから、足裏距離センサ１６の目標とする時系列値を計算する。即ち、目標とする足裏距離センサ１６の目標距離（出力値）を計算する。尚、ノミナル位置・傾斜に代えて、脚式ロボット１００の一歩分の着地位置から足平リンク２６の着地位置・姿勢を計算することで、目標とする足裏距離センサ１６の出力値を計算するようにしてもよい。

次いで、足裏倣い制御部６１は、計算した足裏距離センサ目標値（目標距離）に、倒立制御部６２から出力された倣い制御目標値補正量（変更量）を、加算器６１４により加算することで、足裏距離センサ目標値の補正を行う（ステップＳ２０２）。さらに、足裏倣い制御部６１は、加算器６１４からの出力値（補正された足裏距離センサ目標値）と、足裏距離センサ１６によって実際に計測された計測値とから、足裏距離センサ１６の偏差を差分器６１２により計算する（ステップＳ２０３）。そして、爪先偏差計算部６１３は、計算した足裏距離センサ１６の偏差から、爪先部２６ａの足裏面と路面との相対位置偏差・相対姿勢偏差を計算する。また、爪先偏差計算部６１３は、計算した足裏距離センサ１６の偏差から、爪先位置姿勢偏差及び爪先関節角度偏差を計算し（ステップＳ２０４）、これら偏差を補償器６１５に対して出力する。

さらに、補償器６１５は、所定の伝達関数を用いて、爪先位置姿勢偏差から爪先部補正量を計算すると共に、爪先関節角度偏差から爪先関節補正量を夫々計算する（爪先補正量計算工程）（ステップＳ２０５）。補償器６１５は、計算した爪先部補正量及び爪先関節補正量を、足先目標値補正量として出力する。

そして、制御部３０は、爪先位置姿勢目標値及び爪先関節角度目標値を、この足先目標値補正量に基づいて、その偏差が小さくなるように修正する（歩容データ修正工程）（ステップＳ２０６）。これにより、足裏面と路面Ｓとの位置関係が目標値に追従するように、歩容データが修正される

以上、本実施の形態１に係る脚式脚式ロボット１００において、変更量計算部６２４は、姿勢偏差計算部６２１により計算される胴体１０の姿勢偏差が減少するように、足裏面と路面Ｓとの目標距離の変更量である倣い制御目標値補正量を計算する。また、足裏倣い制御部６１は、足裏距離センサ目標値に、倒立制御部６２から出力された倣い制御目標値補正量を加算して、足裏距離センサ目標値の補正を行う。さらに、歩容データは、この補正された足裏距離センサ目標値に基づいて、修正される。これにより、胴体１０の姿勢偏差の方向と逆方向へ打消すように、路面Ｓからの反力モーメントを足裏面に作用させることができる。したがって、脚式ロボット１００の足裏面は、路面Ｓから適切な反力モーメントを得ることができるため、脚式ロボット１００の歩行をより安定化させることができる。

なお、本発明を実施するための最良の形態について一実施形態を用いて説明したが、本発明はこうした一実施形態に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において、上述した一実施形態に種々の変形及び置換を加えることができる。

例えば、上記一実施形態において、変更量計算部６２４は、姿勢偏差計算部６２１により計算される胴体１０の姿勢偏差が減少するように、目標距離の変更量を計算しているが、脚式ロボット１００の重心位置の偏差が減少するように、上記目標距離の変更量を計算してもよい。この場合、歩容データ５１には、脚式ロボット１００の重心位置目標値が予め記憶されおり、姿勢角センサ１４などを用いて胴体１０の重心位置を計算する重心位置算出手段が設けられている。そして、倒立制御部の重心偏差計算部は、歩容データの重心位置目標値と、重心位置算出手段により算出された実際の重心位置と、の偏差を計算し、コントローラ６２２へ出力する。これにより、脚式ロボット１００の重心位置の偏差を減少させるように、路面Ｓからの反力モーメントを足裏面に作用させることができる。したがって、脚式ロボット１００の足裏面は、路面Ｓから適切な反力モーメントを得ることができるため、脚式ロボット１００の歩行をより安定化させることができる。

本発明の実施の形態１に係る脚式ロボットの構成図である。 本発明の実施の形態１に係る脚式ロボットの足平リンクの構成図である。 本発明の実施の形態１に係る脚式ロボットの構成を示す機能ブロック図である。 本発明の実施の形態１に係る脚式ロボットの倒立制御部の構成を示す機能ブロック図である。 倣い制御目標値補正量を計算する際の処理フローを示すフローチャートである。 足裏距離センサの目標距離の変更量を示す簡略図である。 （ａ）従来の倒立制御及び足裏倣い制御において、足裏面が路面に倣うように制御される状態の一例を示す図である。 （ｂ）本発明の実施の形態１に係る倒立制御及び足裏倣い制御において、姿勢偏差の方向と逆方向に反力モーメントが作用する状態の一例を示す図である。 （ａ）本発明の実施の形態１に係る脚式ロボットの目標姿勢における座標定義の一例を示す図である。 （ｂ）本発明の実施の形態１に係る脚式ロボットの実姿勢における座標定義の一例を示す図である。 行列及びベクトル表記における定義の一例を示す図である。 歩容の爪先部、胴体、及び目標ＺＭＰを、姿勢偏差分だけ腰位置を中心に回転させた際の座標系の一例を示す図である。 回転後の爪先部の位置に基づいて足裏距離センサの位置を計算する際の座標系の一例を示す図である。 目標ＺＭＰを中心にして補正角だけ両足裏面を回転させた際の座標系の一例を示す図である。 本発明の実施の形態１に係る脚式ロボットの足裏倣い制御部の機能構成を示す機能ブロック図である。 本発明の実施の形態１に係る足裏倣い制御部による制御処理の概要を説明するためのフローチャートである。

符号の説明

１ 制御システム、
１０ 胴体、
１２ 加速度センサ、１４ 姿勢角センサ、１５ モータ、１６ 距離センサ、
２０ 脚部、２１ 股関節、２２ 大腿リンク、２３ 膝関節、２４ 脛リンク、
２５ 足首関節、２６ 足平リンク、２６ａ 爪先部、２６ｂ 踵部、
２７ 爪先関節、 ３０ 制御部、５０ 記憶部、
５１ 歩容データ、５２ 路面ノミナル位置・傾斜、
６１ 足裏倣い制御部、６２ 倒立制御部、６３ 関節角変換部、６４ 各軸制御器、
１００ 脚式ロボット
６１１ 距離センサ目標値計算部、６１３ 爪先偏差計算部、
６１５ 補償器、６２１ 姿勢偏差計算部、６２２ コントローラ、
６２３ リミッタ、６２４ 変更量計算部

Claims (6)

1. 胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、該足平部の足裏と路面との距離を検出する距離検出部と、前記胴体の姿勢を検出する胴体姿勢検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、を備え、
   前記歩容データは、前記胴体の目標姿勢を含む脚式ロボットであって、
   前記制御部は、
   前記歩容データに基づいて、前記足平部の足裏と路面との間の目標距離を計算する目標距離計算部と、
   前記胴体姿勢検出部により検出された前記胴体の姿勢と、前記歩容データの目標姿勢との偏差が減少するように、前記目標距離計算部により計算された前記目標距離の変更量を計算する変更量計算部と、
   前記目標距離に前記変更量を加算した値と、前記距離検出部により検出された前記距離と、の偏差に基づいて、前記歩容データを修正する歩容データ修正部と、を有する、ことを特徴とする脚式ロボット。
2. 請求項１記載の脚式ロボットであって、
   前記制御部は、前記足平部の足裏の少なくとも一部が路面と接触するように、前記変更量計算部により計算された前記目標距離の変更量を制限する変更量制限部を有する、ことを特徴とする脚式ロボット。
3. 請求項１又は２記載の脚式ロボットであって、
   前記脚式ロボットの重心位置を検出する重心位置検出部を備え、
   前記歩容データは、重心位置目標値を更に含み、
   前記変更量計算部は、前記重心位置検出部により検出された前記重心位置と、前記歩容データの重心位置目標値と、の偏差が減少するように、前記目標距離の変更量を計算する、ことを特徴とする脚式ロボット。
4. 請求項１乃至３のうちいずれか１項記載の脚式ロボットであって、
   距離検出部は、爪先部の足裏に設けられた少なくとも３個の距離センサと、前記踵部の足裏に設けられた少なくとも１個の距離センサとの出力信号から前記足平部の足裏と路面との距離を検出する、ことを特徴とする脚式ロボット。
5. 請求項１記載の脚式ロボットであって、
   前記足平部は、爪先部と、爪先関節を介して該爪先部に連結された踵部と、から構成される、ことを特徴とする脚式ロボット。
6. 胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、該足平部の足裏と路面との距離を検出する距離検出部と、前記胴体の姿勢を検出する胴体姿勢検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、を備え、
   前記足平部は、爪先部と、爪先関節を介して該爪先部に連結された踵部と、から構成され、
   前記歩容データは、前記爪先部の目標姿勢と、前記爪先関節の目標角度と、前記胴体の目標姿勢と、を含む脚式ロボットの制御方法であって、
   前記歩容データに基づいて、前記足平部の足裏と路面との間の目標距離を計算する目標距離計算工程と、
   前記胴体姿勢検出部により検出された前記胴体の姿勢と、前記歩容データの目標姿勢との偏差が減少するように、前記目標距離計算工程で計算された前記目標距離の変更量を計算する変更量計算工程と、
   前記目標距離に前記変更量を加算した値と、前記距離検出部により検出された前記距離と、の偏差が減少するように、前記爪先部の目標位置姿勢を補正する爪先部補正量、及び／又は、前記爪先関節の目標角度を補正する爪先関節補正量を計算する爪先補正量計算工程と、
   前記爪先補正量計算工程で計算された爪先部補正量及び／又は爪先関節補正量に基づいて、前記歩容データを修正する歩容データ修正工程と、を含む、ことを特徴とする脚式ロボットの制御方法。

Images