脚式ロボット、及びその制御方法
【課題】距離センサの故障を検知して、安定して歩行が可能な脚式ロボット、及びその制御方法を提供する。
【解決手段】本発明に係る脚式ロボットは、胴体１０と、脚部２０と、足平部２６と、足平部２６の足裏と路面との距離を検出する距離検出部１６と、歩容データに基づいて脚部２０の関節を駆動制御する制御部３０と、を備え、制御部３０は、距離検出部１６の検出結果が基準範囲内に含まれない場合に、距離検出部１６の故障を検知する故障検知部６６と、故障していない正常な距離検出部１６の検出結果に基づいて、歩容データを修正する歩容データ修正部と、を備える
【解決手段】本発明に係る脚式ロボットは、胴体１０と、脚部２０と、足平部２６と、足平部２６の足裏と路面との距離を検出する距離検出部１６と、歩容データに基づいて脚部２０の関節を駆動制御する制御部３０と、を備え、制御部３０は、距離検出部１６の検出結果が基準範囲内に含まれない場合に、距離検出部１６の故障を検知する故障検知部６６と、故障していない正常な距離検出部１６の検出結果に基づいて、歩容データを修正する歩容データ修正部と、を備える
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は脚式ロボット、及びその制御方法に関する。
【背景技術】
【０００２】
近年、脚部の下端に足平部を設けたロボットにおいて、足平部の足裏を路面に倣わせる足裏倣い制御と、足平部から胴体への位置姿勢を目標の位置姿勢に維持する倒立制御とを組合せて、ロボットを安定して歩行させる技術が開発されている。
【０００３】
このような脚式ロボットでは、まず、足平部の足裏部分を床面に接触させて支持脚とし、その後に足平の裏面で床面を押して脚部全体（ロボット全体）を持ち上げるように脚部を駆動することで、次の歩行動作を行う。駆動された脚部は遊脚となる一方、他の脚部が支持脚となり、このように、遊脚と支持脚を交互に繰り返して切替えることで、歩行動作を行うことができる。
【０００４】
脚式ロボットを転倒させずに安定して移動させるためには、支持脚期ではロボットの足裏を路面に密着させると共に、遊脚を着地させる際には、予期しない路面凹凸からの外乱力を抑制することが効果的である。このため、ロボットの足裏と路面との関係を目標どおりに制御することが重要となる。
【０００５】
従来、ロボットの足裏に距離センサを配置して、距離センサの出力値を目標値に追従させるように足裏を制御する技術が知られている。本出願人による出願（特願２００７−２７８１０７号）には、足裏に路面との距離を検出する距離センサを設け、距離センサの検出結果に基づく足裏倣い制御により歩行安定化を行う２足ロボットが開示されている。当該２足ロボットでは、足裏倣い制御と倒立制御を組合せて制御する際に、ロボットの転倒方向を検出して足裏倣い制御で使用する距離センサを適切に選択することにより、不整地においても足裏が路面に密着しつつ、転倒しているロボットを戻すのに効果的な床反力モーメントを得るものである。
【特許文献１】特開２００７−１５２４７０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、本出願人による出願（特願２００７−２７８１０７号）に開示されるロボットでは、距離センサの故障を検知する手段を備えておらず、距離センサの故障に起因してロボットが転倒する虞があった。即ち、距離センサが故障した際に、故障した距離センサをそのまま用いて制御を行った場合には足裏を路面に密着させることができず、逆に、路面から離れた場所へと足裏を制御しようとするため、歩行が不安定となる。このため、足裏に設けられた距離センサが故障した際には、足裏倣い制御において、路面と足裏との関係を目標通りに制御することができず、結果として、床から意図しない反力を受けてロボットが転倒してしまうという問題がある。
【０００７】
他方、特許文献１には、距離センサの出力信号に変動が生じなかった場合に、距離センサに故障が生じているものと判定する自己診断機能付きロボットが開示されている。かかる特許文献１記載のロボットは、人を検知するための超音波距離センサを胸部に設けると共に、障害物や段差を検出する赤外線センサを脚部の前面に設け、これらセンサの故障をセンサの検出値に基づいて検知するものである。即ち、特許文献１記載のロボットは、足裏と路面との距離を検出する距離検出部の故障を検知して、正常な距離検出部の検出結果に基づいて歩容データを修正することで、路面と足裏との関係を目標通りに制御することを目的とするものではないため、足裏距離センサが故障した場合に、歩行の安定化を図るものではない。
【０００８】
このように、従来の脚式ロボットによれば、距離センサの故障を検知することができないため歩行が不安定となり、ロボットが転倒する虞があるという問題があった。
【０００９】
本発明は、かかる課題を解決するためになされたものであり、距離センサの故障を検知して、安定して歩行が可能な脚式ロボット、及びその制御方法を提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明にかかる脚式ロボットは、胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、前記足平部の足裏と路面との距離を検出する距離検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、を備えた脚式ロボットであって、前記制御部は、前記距離検出部の検出結果が基準範囲内に含まれない場合に、当該距離検出部の故障を検知する故障検知部と、故障していない正常な距離検出部の検出結果に基づいて、前記歩容データを修正する歩容データ修正部と、を備えるものである。
【００１１】
このように距離検出部の検出結果が基準範囲内に含まれるか否かに応じてその故障を検知し、故障していない正常な距離検出部の検出結果に基づいて歩容データを修正することで、距離センサが故障した場合であっても、路面と足裏との関係を目標通りに制御することができ、安定して歩行することができる。
【００１２】
また、前記故障検知部は、前記脚部が遊脚期であって、前記距離検出部の検出値が前記基準範囲内に含まれない場合に、当該距離検出部の故障を検知するようにしてもよい。これにより、脚部が遊脚期である場合には、支持脚期の場合と比べて、距離検出部の検出値はその目標値近辺により安定して収束するため、距離検出部の検出値が基準範囲内に含まれるか否かに応じて距離検出部の故障を容易かつ正確に検知することができる。
【００１３】
さらにまた、前記基準範囲が、前記脚部が遊脚期であって、前記距離検出部の検出可能な最大値を含む所定の範囲であるようにしてもよい。これにより、脚部が遊脚期である場合には、距離検出部の検出値は、その検出可能な最大値近傍の値になるものと考えられるため、脚部が遊脚期である場合に、距離検出部が検出可能な最大値を含む所定の範囲を基準範囲とすることで、距離検出部の故障をより容易かつ正確に検知することができる。
【００１４】
また、前記制御部は、前記距離検出部の検出結果に基づいて、接地面から見た相対的な前記足平部の目標とする位置姿勢と実際の位置姿勢との偏差を計算し、前記歩容データ修正部は、当該偏差を小さくするように前記歩容データを修正するようにしてもよい。
【００１５】
さらにまた、前記制御部は、前記故障検知部の検知結果に応じて、前記制御部による制御が継続可能であるか否かを判定する切替判定部を更に備え、前記歩容データ修正部は、前記切替判定部による判定の結果、前記制御部による制御が継続可能であると判定した場合に、故障していない正常な距離検出部を利用して、前記歩容データを修正するようにしてもよい。このように、距離検出部の故障検知結果に応じて制御の切替を行うことで、距離検出部が故障した際に、ロボットの制御をより迅速に安定化させることができる。
【００１６】
また、前記胴体の姿勢を検出する胴体姿勢検出部を更に備え、前記制御部は、前記胴体姿勢検出部の検出結果に基づいて、前記胴体の目標とする姿勢と実際の姿勢との偏差を計算し、前記歩容データ修正部は、当該偏差を小さくするように前記歩容データを修正するようにしてもよい。
【００１７】
さらにまた、前記脚式ロボットの重心位置を検出する重心位置検出部を更に備え、前記制御部は、前記重心位置検出部の検出結果に基づいて、前記足平部を基準とした目標とする重心位置と実際の重心位置との偏差を計算し、前記歩容データ修正部は、当該偏差を小さくするように前記歩容データを修正するようにしてもよい。
【００１８】
また、前記距離検出部は、前記足平部の足裏に設けられた距離センサの出力信号から前記足平部の足裏と路面との距離を検出するようにしてもよい。
【００１９】
本発明にかかる脚式ロボットの制御方法は、胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、を備えた脚式ロボットの制御方法であって、歩容データに基づいて前記脚部の関節を駆動制御する制御ステップでは、前記足平部の足裏と路面との距離を検出する距離検出ステップと、前記検出された前記足平部の足裏と路面との距離が基準範囲内に含まれない場合に、当該距離検出部の故障を検知する故障検知ステップと、故障していない正常な距離検出部の検出結果に基づいて、前記歩容データを修正する歩容データ修正ステップと、を備えるものである。
【００２０】
このように、検出された足平部の足裏と路面との距離が基準範囲内に含まれるか否かに応じてその故障を検知し、故障していない正常な距離検出部の検出結果に基づいて歩容データを修正することで、距離センサが故障した場合であっても、路面と足裏との関係を目標通りに制御することができ、安定して歩行することができる。
【００２１】
また、前記故障検知部は、前記脚部が遊脚期であって、前記距離検出部の検出値が、前記距離検出部が検出可能な最大値を含む所定の範囲内に含まれない場合に、当該距離検出部の故障を検知するようにしてもよい。これにより、脚部が遊脚期である場合には、支持脚期の場合と比べて、距離検出部の検出値はその目標値近辺により安定して収束し、距離検出部の検出値は、その検出可能な最大値近傍の値になるものと考えられるため、脚部が遊脚期である場合に、距離検出部が検出可能な最大値を含む所定の範囲を基準範囲とすることで、距離検出部の故障をより容易かつ正確に検知することができる。
【発明の効果】
【００２２】
本発明によれば、距離センサの故障を検知して、安定して歩行が可能な脚式ロボット、及びその制御方法を提供することを目的とする。
【発明を実施するための最良の形態】
【００２３】
発明の実施の形態１．
本実施の形態１にかかる脚式ロボットは、胴体と、胴体に連結された脚部と、脚部の下端に設けられた足平部と、足平部の足裏と路面との距離を検出する距離検出部と、歩容データに基づいて脚部の関節を駆動制御する制御部と、を備える。ここで、制御部は、距離検出部の検出結果が基準範囲内に含まれない場合に、その距離検出部の故障を検知する故障検知部と、故障していない正常な距離検出部の検出結果に基づいて、歩容データを修正する歩容データ修正部と、を備える
【００２４】
脚式ロボットが歩行する際には、足平部の足裏に設けられた足裏距離センサを用いて足裏を路面に倣わせる足裏倣い制御と、姿勢センサを用いてロボットの姿勢を倒立させる倒立振子制御を組合せることによって安定化制御を実現している。本実施の形態１にかかる脚式ロボットによれば、足裏距離センサの検出値が基準範囲内に含まれるか否かに応じてその故障を検知し、故障していない正常な足裏距離センサの検出値に基づいて歩容データを修正することで、足裏距離センサが故障した場合であっても、路面と足裏との関係を目標通りに制御することができ、安定して歩行することができる。
【００２５】
以下、図面を参照しながら本実施の形態１に係るロボットの制御方法について説明する。図１は、本実施の形態１に係る脚式ロボットの概要を示す図である。ロボット１００は、胴体１０と、胴体１０に連結された２本の脚を有する。尚、図１には、一方の脚部２０のみを示しており、他方の脚部は図示を省略している。胴体１０は、ロボット１００の動作（脚部の各関節の動作）を制御する制御部３０と、胴体の加速度を検出する加速度センサ１２と、胴体の１０の鉛直方向に対する傾斜角（姿勢角）を検出する姿勢角センサ１４を備える。
【００２６】
脚部２０は、股関節２１、膝関節２３、足首関節２５、大腿リンク２２、脛リンク２４、及び足平部としての足平リンク２６を備える。大腿リンク２２と脛リンク２４は、直線で模式化して示してある。股関節２１は、胴体１０と大腿リンク２２を揺動可能に連結している。膝関節２３は、大腿リンク２２と脛リンク２４を揺動可能に連結している。足首関節２５は、脛リンク２４と足平リンク２６を揺動可能に連結している。脚部２０の下端には足平部としての足平リンク２６が設けられる。足平リンク２６は板状の部材であり、足平リンクの裏面（足裏面）は平面となっている。
【００２７】
足平リンク２６には、距離検出部としての少なくとも３個以上の足裏距離センサ１６が設けられている。足裏距離センサ１６は、足平リンク２６の裏面（足裏面）と接地面Ｓとの距離を検出する。図２は、足平リンク２６の構成を説明するための図である。図２に示すように、足平リンク２６は、上面視において、略矩形状に形成されている。足平リンク２６の四隅近傍には、４つの足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄがそれぞれ設けられている。ここでは、足平リンク２６の爪先側に２つの足裏距離センサ１６ａ及び１６ｄが、踵側に２つ足裏距離センサ１６ｂ及び１６ｃがそれぞれ設けられている。足裏距離センサ１６ａ及び１６ｄは、足平リンク２６の前方の所定位置における足裏面と接地面Ｓとの距離を検出し、足裏距離センサ１６ｂ及び１６ｃは、足平リンク２６の後方の所定位置における足裏面と接地面Ｓとの距離を検出する。従って、足裏距離センサ１６ａ及び１６ｄが検出する距離と１６ｂ及び１６ｃが検出する距離の差から、足平リンク２６の足裏面の接地面Ｓに対する傾きを求めることができる。
【００２８】
各関節には図示しないモータが内蔵されており、制御部３０からの指令に基づいて駆動される。モータを駆動することによって、関節に連結されたリンク同士を揺動させることができる。図示を省略している他方の脚部も、脚部２０と同様の構造を有する。制御部３０が２本の脚部の関節（詳細には関節角）を適宜制御することにより、ロボット１００を歩行させることができる。
【００２９】
図１においては、説明の便宜上、ロボット１００が進行する向き（前後方向）をｘ軸、ロボット１００が進行する方向に対して水平方向に直交する向き（左右方向）をｙ軸、ロボット１００の進行する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これら３軸からなる絶対座標系を用いて説明する。即ち、図１において、ｘ軸は紙面に向かって左右方向、ｙ軸は紙面の奥行き方向、ｚ軸は紙面中の上下方向を示す。尚、ロボット１００の胴体１０に対して点Ｏｂを特定し固定する。足平リンク２６に対して点Ｏｆを特定し固定する。
【００３０】
ロボット１００は、制御部３０に記憶されている歩容データに基づいて制御される。歩容データには、胴体１０の目標位置（目標胴体位置）、胴体１０の目標姿勢角（目標胴体姿勢角）、足平リンク２６の目標位置（目標足平位置）、及び足平リンク２６の目標姿勢角（目標足平姿勢角）のそれぞれの時系列データが含まれる。歩容データには、ロボット１００が有する脚部のそれぞれの目標足平位置・姿勢角の時系列データが含まれる。
【００３１】
歩容データは、シミュレーション等によってロボット１００を安定して歩行させることができるように作成されている。即ち、目標胴体位置、目標胴体姿勢角、目標足平位置、及び目標足平姿勢角は、ロボット１００のＺＭＰ位置が接地面に接地した足裏で囲まれた凸包内となる関係を満足するように設定されている。作成された歩容データは、ロボット１００の制御部３０に記憶される。後述するように、制御部３０は、歩容データに含まれる目標胴体位置等に実胴体位置等を一致させるように各関節を制御する。
【００３２】
目標胴体位置は、絶対座標系に対する特定点Ｏｂの位置で表される。特定点Ｏｂを原点とする胴体座標系を用いる場合には、目標胴体姿勢角は、絶対座標系に対する胴体座標系の傾きで表される。実胴体姿勢角は、胴体１０に備えられた姿勢角センサ１４によって検出することができる。目標足平位置は、絶対座標系に対する特定点Ｏｆの位置で表される。目標足平姿勢角は、接地面に対する足裏面の角度で表される。特定点Ｏｆを原点とする足平座標系を用いる場合には、目標足平姿勢角は、絶対座標系に対する足平座標系の傾きで表してもよい。実足平姿勢角は、後述するように、足平リンク２６に備えられた足裏距離センサ１６によって検出することができる。
【００３３】
続いて、本実施の形態１に係るロボット１００の制御システム１の詳細について説明する。制御システム１は、倒立制御を実行すると共に、足裏倣い制御を実行する。倒立制御は、実胴体位置及び姿勢角を目標胴体位置及び姿勢角に一致する制御である。尚、胴体姿勢角の代わりに重心位置を用いてもよい。足裏倣い制御は、接地面から見た相対的な実足平位置及び実足平姿勢角をそれぞれ目標足平位置及び目標足平姿勢角に一致する制御である。
【００３４】
図３は、制御システム１の機能構成を示す機能ブロック図である。制御システム１は、制御部３０と、記憶部５０と、姿勢角センサ１４と、モータ１５と、足裏距離センサ１６等を有している。
【００３５】
記憶部５０には、歩容データ５１及び路面ノミナル位置・傾斜情報５２が記憶されている。歩容データ５１は、目標胴体位置、目標胴体姿勢角、目標足平位置、及び目標足平姿勢角の時系列データを含む。歩容データ上で足裏が接地面と接触するときは、足裏面と接地面を面接触状態とするため、目標足平姿勢角（仮想的な接地面に対する仮想的な足裏面の傾き）はゼロに設定されている。また、歩容データ上の各目標値は、ロボット１００のＺＭＰ位置が接地している脚の足裏で囲まれた凸包内となる関係を満たすように決定されている。路面ノミナル位置・傾斜情報５２は、ロボット１００が移動する路面の存在情報であり、路面の位置・傾斜に関する基準値を示す。
【００３６】
制御部３０は、記憶部５０に記憶された歩容データ５１等を読み出すと共に、読み出した歩容データ５１等によって特定されるロボット１００の姿勢を実現するために必要な脚部２０の関節角を算出する。そして、このように算出した関節角に基づく信号をモータ１５に送信する。また、制御部３０は、センサからの信号を受けて、モータの駆動量を調整する。
【００３７】
より詳細には、制御部３０は、足裏倣い制御部６１と、倒立制御部６２と、関節角変換部６３と、各軸制御器６４と、足裏倣い制御切替判定部６５と、故障検知部６６と、を備える。制御部３０内では、実胴体姿勢角と目標胴体姿勢角の偏差に基づくフィードバック制御系（倒立制御部６２による倒立制御系）と、接地面から見た相対的な実足平姿勢角と目標足平姿勢角の偏差に基づくフィードバック制御系（足裏倣い制御部６１による倣い制御系）が含まれる。以後、足裏倣い制御の偏差については、接地面から見た相対的な足平姿勢角に関するものを示すものとする。
【００３８】
足裏倣い制御部６１は、実足平姿勢角を目標足平姿勢角に一致させるように、例えば足裏が接地面に密着している状態を目標として、踵側が接地面から浮いている場合には、足平リンク２６の爪先側を脛リンク２４に近づける方向に足平リンク２６を回転させる。即ち、足裏面と接地面との面接触を維持するように足平リンク２６を回転させる。実足平姿勢角は、足平リンク２６が備える足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄの出力値から求められる。尚、足裏倣い制御部６１の詳細については後述する。
【００３９】
倒立制御部６２は、ロボット１００の胴体位置姿勢を目標位置姿勢に維持する機能を果たす。胴体位置姿勢は、実際の胴体位置と胴体姿勢角であり、目標位置姿勢は、目標の胴体位置と胴体姿勢角である。実胴体姿勢角は、胴体１０に備えられた姿勢角センサ１４で検出される。姿勢角センサ１４は、例えば胴体１０の角速度を検出するジャイロと、ジャイロの出力（角速度）を積分する積分器と、重力加速度ベクトルを検出する３軸加速度センサで構成される。尚、倒立制御部６２の詳細については後述する。
【００４０】
故障検知部６６は、足裏距離センサ１６の検出結果が基準範囲内に含まれない場合に、足裏距離センサ１６の故障を検知する。切替判定部としての足裏倣い制御切替判定部６５は、故障検知部６６の検知結果に応じて、制御部３０による制御が継続可能であるか否かを判定する。図示しない歩容データ修正部は、足裏倣い制御切替判定部６５による判定の結果、制御部３０による制御が継続可能であると判定した場合に、故障していない正常な足裏距離センサ１６に基づいて、歩容データを修正する。尚、故障検知部６６及び足裏倣い制御切替判定部６５の詳細については後述する。
【００４１】
より詳細には、以下のようにして記憶部５０に記憶された歩容データ（目標胴体位置、目標胴体姿勢角、目標足平位置、及び目標足平姿勢角の時系列データ）が修正され、関節角変換部６３に入力される。図３においては、胴体位置姿勢目標値が、目標胴体位置及び目標胴体姿勢角の時系列データを含み、足先位置姿勢目標値が、目標足平位置及び目標足平姿勢角の時系列データを含む。
【００４２】
記憶部５０に記憶された目標胴体位置は、目標胴体加速度と実胴体加速度の偏差に基づいて補正された後に関節角変換部６３に入力される。目標胴体加速度は、目標胴体位置を２回微分することによって求められる。実胴体加速度は、加速度センサ１２により検出される。
【００４３】
記憶部５０に記憶された目標足平位置は、目標足平位置と実足平位置の偏差に基づいて補正された後に関節角変換部６３に入力される。
【００４４】
記憶部５０に記憶された目標胴体姿勢角は、そのまま関節角変換部６３に入力される。同時に、目標胴体姿勢角と実胴体姿勢角の偏差（胴体姿勢角偏差）が求められる。胴体姿勢角偏差は倒立制御部６２に入力されて、胴体姿勢角偏差を小さくする方向に胴体を回転させる胴体補正角が算出される。実胴体姿勢角は、姿勢角センサ１４により検出される。尚、図３においては、胴体補正角は足裏倣い制御部６１における補正量となるため、胴体補正角を倣い制御目標値補正量という。
【００４５】
記憶部５０に記憶された目標足平姿勢角と実足平姿勢角の偏差（足平姿勢角偏差）が求められる。実足平姿勢角は、足裏距離センサ１６により検出される。足平姿勢角偏差と上述した胴体補正角（倣い制御目標値補正量）とが加算され、加算された結果が、足裏倣い制御部６１に入力される。足裏倣い制御部６１によって、入力された角度（胴体補正角と足平姿勢角偏差を加算した角度）を小さくする方向へ足平を回転させる足平補正角が求められる。記憶部５０に記憶された目標足平姿勢角は、上述した足平補正角が加算された後に（足平補正角で補正された後に）関節角変換部６３に入力される。尚、図３においては、足平補正角を足先目標値補正量という。
【００４６】
関節角変換部６３には、以上のようにして修正された歩容データが入力される。これらの値から、関節角変換部６３では、逆キネマティクスの演算によって脚部２０の各関節の目標関節角が算出される。ここで、それぞれの目標値は、絶対座標系に対する値で表されている。関節角変換部６３では、目標足平位置と目標胴体位置の差から足平と胴体の相対位置を計算し、目標足平姿勢角（足平補正角によって補正されている）と目標胴体姿勢角の差から足平と胴体の相対回転角を計算する。計算された相対位置と相対回転角を実現する目標関節角が算出される。
【００４７】
各軸制御器６４は、関節角変換部６３により送信された目標関節角の信号に基づいて、脚部２０を駆動するための各モータ１５の駆動量を特定し、これらの駆動量でモータ１５を駆動させるためのモータ駆動信号を各モータに送信する。これによって、脚部２０の各関節における駆動量が変更され、ロボット１００の動きが制御される。
【００４８】
続いて、図４を参照しながら、本実施の形態１に係る倒立制御部６２について詳細に説明する。図４は、倒立制御部６２の機能構成を示す機能ブロック図である。図４に示すように、倒立制御部６２は、姿勢偏差計算部６２１と、コントローラ６２２と、リミッタ６２３と、を有する。
【００４９】
倒立制御部６２は、まず、姿勢偏差計算部６２１において、胴体姿勢目標値（胴体の目標姿勢）と、胴体姿勢計測値（姿勢角センサ１４により検出した実姿勢）との偏差を計算する。そして、計算した胴体の姿勢偏差に基づき、所定のコントローラ６２２によって、足裏倣い制御で使用する足裏と路面との目標相対位置・目標相対姿勢の補正量を計算する。
【００５０】
次いで、倒立制御部６２は、足裏距離センサ１６の出力値に基づき、足裏が路面から剥がれたか否かを判定する。判定の結果、足裏が路面から剥がれていない場合には、計算された目標相対位置・目標相対姿勢に対して補正を行わずに、倣い制御目標値補正量として出力する。一方で、足裏が路面から剥がれた場合には、リミッタ６２３を介して、計算した補正量を制限する。即ち、リミッタ６２３は、目標相対位置・目標相対姿勢の補正量の大きさに制限を加えるものである。リミッタ６２３は、入力された補正量の大きさが許容範囲内を超えているときに、許容限界の値を出力する。このリミッタ６２３を設けることで、足裏倣い制御部６１に入力される倣い制御目標値補正量を制限する。これによって、倣い制御系（足裏倣い制御部６１）が倒立制御系（倒立制御部６２）よりも優勢に作用することを保証する。従って、倣い制御系が優勢に作用して、足裏面が接地面と面接触することを保証することができる。
【００５１】
次いで、倒立制御部６２は、計算された補正量に基づいて、胴体の姿勢偏差を小さくする方向へ、足裏と路面との目標相対位置・目標相対姿勢を補正する。補正された目標相対位置・目標相対姿勢を倣い制御目標値補正量として足裏倣い制御部６１へと出力する。
【００５２】
続いて、図５及び６を参照しながら、本実施の形態１に係る故障検知部６６について詳細に説明する。図５は、故障検知部６６による故障検知処理を説明するためのフローチャートである。図６は、足裏距離センサ１６の検出値と目標値の関係を示す概念図である。
【００５３】
故障検知部６６は、まず、各足裏距離センサ１６の故障を検出するための故障フラグの値を初期化する（ｆａｉｌ＿ｆｌａｇ＝０）（ステップＳ１０１）。次いで、故障検知部６６は、ロボット１００の脚部２０が遊脚であるか又は支持脚であるかを判定する（ステップＳ１０２）。判定の結果、脚部２０が支持脚である場合には、故障検知処理を終了する。
【００５４】
一方で、判定の結果、脚部２０が遊脚である場合には、歩容データに含まれる足平リンク２６の目標位置（ここでは高さ）が、予め定めた所定値よりも大きいか否かを判定する（ステップＳ１０３）。即ち、遊脚期における足平リンク２６の目標高さが十分大きいか否かを判定する。判定の結果、足平リンク２６の目標高さが所定値よりも大きくない場合には、故障検知処理を終了する。
【００５５】
一方で、判定の結果、足平リンク２６の目標高さが所定値よりも大きな場合には、計測された足裏距離センサ１６の出力値が、予め定めた基準範囲内に含まれるか否かを判定する（ステップＳ１０４）。判定の結果、計測された足裏距離センサ１６の出力値が、予め定めた基準範囲内に含まれる場合には、故障検知処理を終了する。
【００５６】
一方で、判定の結果、計測された足裏距離センサ１６の出力値が、予め定めた基準範囲内に含まれない場合には、その足裏距離センサ１６は故障しているものとし、故障状態を示す値を故障フラグの値に設定する（ｆａｉｌ＿ｆｌａｇ＝１）（ステップＳ１０５）。
【００５７】
図６は、足裏距離センサ１６の検出値と目標値の関係を示す概念図である。本実施の形態１に係るロボット１００では、足裏距離センサ１６として接触型のセンサを採用する。これにより、足裏と路面との間の距離を精度良く測定することができる。このような接触型の距離センサでは、遊脚が路面から完全に離床している状態においては、足裏距離センサ１６の出力値として、その計測可能な範囲の最大値を出力する。図６（ａ）に示すように、脚部２０が遊脚時であって、目標高さが所定値よりも十分大きな場合（例えば時間ｔ１〜ｔ２に示す区間の場合）、足裏距離センサ１６の目標とする出力（路面と足裏との目標距離（即ち足平リンク２６の目標高さ））は、足裏距離センサ１６が計測可能な最大値ｈ＿ｍａｘとなる。そこで、図６（ｂ）に示すように、遊脚時における足裏距離センサ１６が検出可能な最大値を中心として、当該最大値を含む範囲を基準範囲として設定する。このように、遊脚時であって、足平リンク２６の目標高さが所定値よりも十分大きな場合に、故障していない正常な足裏距離センサ１６によって計測される出力値は、当該基準範囲内に含まれるものと想定される。従って、計測時における誤差を考慮して、最大値を含む所定の範囲を基準範囲とし、遊脚時であって、足平リンク２６の目標高さが所定値よりも十分大きな場合に、計測された足裏距離センサ１６の出力値が、基準範囲内に含まれるか否かを判定することで、足裏距離センサ１６の故障を容易かつ正確に検知することができる。また、最大値に対して所定の範囲を持たせることで、センサノイズに対しても耐性を有することができる。
【００５８】
続いて、図７乃至９を参照しながら、本実施の形態１に係る足裏倣い制御部６１について詳細に説明する。図７は、足裏倣い制御部６１の機能構成を示す機能ブロック図である。図８は、足裏倣い制御部６１による制御処理の概要を説明するためのフローチャートである。図９は、足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。図５に示すように、足裏倣い制御部６１は、距離センサ目標値計算部６１１と、差分器６１２と、距離センサ選択部６１３と、位置姿勢偏差計算部６１４と、加算器６１５と、補償器６１６を有する。
【００５９】
まず、足裏倣い制御部６１に対して切替判定情報が入力され、足裏倣い制御部６１は、切替判定情報から、足裏距離センサ１６のうちで故障したセンサが存在するか否かを判定する（ステップＳ２０１）。ここで、切替判定情報は足裏倣い制御切替判定部６５により出力される。足裏倣い制御切替判定部６５は、故障検知部６６の検知結果に基づいて、足裏倣い制御が継続可能であるか否かを判定し、当該判定結果を含む切替判定情報を出力する。また、切替判定情報は、各足裏距離センサ１６が故障しているか又は正常であるかを示す足裏距離センサ状態情報（足裏距離センサ１６の故障フラグの値）を含む。
【００６０】
次いで、判定の結果、故障した足裏距離センサ１６が存在する場合には、足裏倣い制御部６１は、切替判定情報から、正常な足裏距離センサ１６のみで足裏倣い制御が継続可能か否かを判定する（ステップＳ２０２）。判定の結果、正常な足裏距離センサ１６のみでは足裏倣い制御が継続不可能な場合には、足裏倣い制御を実行せずに、その出力値（足先目標値補正量）を０に設定する（ステップＳ２０３）。尚、足裏倣い制御と倒立制御は同時に実行することが好ましいため、足裏倣い制御部６１の出力である足先目標値補正量を０とすることで、足裏倣い制御を実行しない場合には、倒立制御も同時に実行させないようにすることができる。
【００６１】
次いでロボット１００の制御部３０は、足先目標値補正量を計算し（ステップＳ２０４）、計算された足先目標値補正量を実現するように歩容データを修正する（ステップＳ２０５）。より詳細には、足裏倣い制御部６１において、相対位置偏差・相対姿勢偏差と倣い制御目標値補正量とが加算器６１５により加算され、加算された結果が、補償器６１６に入力される。補償器６１６において、所定の伝達関数を用いて足先目標値補正量を計算し、計算された足先目標値補正量により、偏差が小さくなるように足平リンク２６の目標位置姿勢を補正する。足先目標値補正量の計算は、例えば、足偏差を入力とし、足先目標値補正量を出力とする伝達関数を通すことで実現することができる。
【００６２】
一方で、ステップＳ２０１における判定の結果、故障した足裏距離センサ１６が存在しない場合には、足裏倣い制御部６１は、全ての足裏距離センサ１６を用いて、通常の足裏倣い制御を実行する（ステップＳ２０７）。より詳細には、まず、足裏倣い制御部６１の距離センサ目標値計算部６１１において、歩容データ５１に含まれる足先位置姿勢目標値（目標足平位置・姿勢角の軌道）と、路面の存在情報である路面のノミナル位置・傾斜情報５２とから、足裏距離センサ１６の高さ目標値を計算する。言い換えると、足平リンク２６の目標位置・姿勢角を示す軌道データと、足平２６が実際に着地する地点（ノミナル位置・傾斜）とから、足裏距離センサ１６の目標とする時系列値を計算する。即ち、目標とする足裏距離センサ１６の出力値を計算する。尚、ノミナル位置・傾斜に代えて、ロボット１００の一歩分の着地位置から足平リンク２６の着地位置・姿勢を計算することで、目標とする足裏距離センサ１６の出力値を計算するようにしてもよい。
【００６３】
次いで、計算した足裏距離センサ１６の目標値と、足裏距離センサ１６によって実際に計測された計測値とから、足裏距離センサ１６の偏差を差分器６１２により計算する。このとき、距離センサ選択部６１３によって、足裏距離センサ１６のうち、床反力モーメントを効果的に得ることができるように足裏距離センサ１６を選択し、選択された足裏距離センサ１６を用いて足裏倣い制御を実行することで、倒立制御において転倒をより効果的に防止することができる。
【００６４】
次いで、位置姿勢偏差計算部６１４において、計算した足裏距離センサ１６の偏差から、足平リンク２６の足裏と路面との相対位置偏差・相対姿勢偏差を計算する。より詳細には、足平リンク２６の目標位置姿勢に対する実際の位置姿勢の足偏差（ロール、ピッチ、ｚ）を計算する。尚、ｚは鉛直方向の測定高さを示す。足裏と距離センサ１６が配置される位置の幾何学的関係から近似を用いて、各足裏距離センサ１６ａ乃至１６ｄの出力値の偏差と、各足裏距離センサ１６の偏差に対応する変換行列とから、足偏差を一意に決定することができる。制御部３０は、このようにして計算した足偏差から、上述したように足先目標値補正量を計算し（ステップＳ２０４）、計算された足先目標補正量を実現するように歩容データを修正する（ステップＳ２０５）。
【００６５】
一方で、ステップＳ２０２における判定の結果、正常な足裏距離センサ１６のみで足裏倣い制御が継続可能である場合には、正常な足裏距離センサ１６のみを用いて、足裏倣い制御を実行する（ステップＳ２０６）。次いで、制御部３０は、上述したようにして、計算した足偏差から、足先目標値補正量を計算し（ステップＳ２０４）、計算された足先目標補正量を実現するように歩容データを修正する（ステップＳ２０５）。
【００６６】
図９は、足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。図において、故障した足裏距離センサ１６を斜線領域により示す。図９（ａ）に示すように、足裏倣い制御では、足偏差（ロール、ピッチ、ｚ）を補正する。これに対して、本実施の形態１に係るロボット１００では、足裏距離センサ１６を４個備えているため、センサの個数が冗長となる。このため、図９（ｂ）乃至（ｄ）に示すように、足裏距離センサ１６の故障状況に応じて、距離センサ選択部６１３は、足裏距離センサ１６のうち、床反力モーメントを効果的に得ることができるように足裏距離センサ１６を選択する。そして、足裏倣い制御処理においては、選択された足裏距離センサ１６を用いて足裏倣い制御を実行する。
【００６７】
まず、図９（ｂ）に示すように、足裏距離センサ１６が全て正常な場合には（図８におけるステップＳ２０７の場合）、床反力モーメントを効果的に得ることができるように３個の足裏距離センサ１６を選択することで、倒立制御の転倒を防止することができる。
【００６８】
他方、図９（ｃ）に示すように、足裏距離センサ１６のうち１つが故障している場合には（例えば図８におけるステップＳ２０６の場合）、正常な残り３個の足裏距離センサ１６を選択し、選択された足裏距離センサ１６を用いて足裏倣い制御を実行する。
【００６９】
また、図９（ｄ）に示すように、足裏距離センサ１６のうち２つが故障している場合には（例えば図８におけるステップＳ２０３の場合）、正常な残り２個の足裏距離センサ１６では足先目標値補正量を適切に計算することができないため、足裏距離センサ１６を用いて足裏倣い制御を実行しないものとする。
【００７０】
発明の実施の形態２．
上述した実施の形態１においては、足平リンク２６に設けられた足裏距離センサ１６が４個である場合について、それら足裏距離センサ１６のうち、３個の足裏距離センサ１６を選択する処理について説明した。本発明はこれに限定されず、足平リンク２６に設けられた少なくとも３個以上の足裏距離センサ１６から、３個の足裏距離センサ１６を選択するようにしてもよい。尚、本実施の形態２に係るロボット１００の構成は、足平リンク２６の構成、及び足平リンク２６に設けられる足裏距離センサ１６の配置を除いて、上述した発明の実施の形態１と同様である。
【００７１】
図１０は、本実施の形態２に係るロボット１００の足平リンク２６の構成を説明するための図である。足平リンク２６は、爪先部２６ａと、爪先関節２７を介して爪先部２６ａに連結された踵部２６ｂとから構成される。ロボット１００の歩行動作中には、爪先関節２７を駆動することで、爪先部２６ａを路面に対して接地させたまま、踵部２６ｂを浮かせることができる。即ち、踵部２６ｂを浮かせたまま爪先部２６ａのみを接地させることにより、ロボット１００を直立させることができる。
【００７２】
図１０に示すように、爪先部２６ａ及び踵部２６ｂは、上面視において、略矩形状に形成されている。爪先部２６ａの四隅近傍には、４つの足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄがそれぞれ設けられている。ここでは、爪先部２６ａの爪先側に２つの足裏距離センサ１６ａ及び１６ｄが、踵部方向側に２つ足裏距離センサ１６ｂ及び１６ｃが設けられている。足裏距離センサ１６ａ及び１６ｄは、爪先部２６ａの前方の所定位置における足裏面と接地面Ｓとの距離を検出し、足裏距離センサ１６ｂ及び１６ｃは、爪先部２６ａの後方の所定位置における足裏面と接地面Ｓとの距離を検出する。踵部２６ｂには、爪先部側とは反対側の二隅に２つの足裏距離センサ１６ｅ及び１６ｆがそれぞれ設けられている。足裏距離センサ１６ｅ及び１６ｆは、踵部２６ｂの後方の所定位置における足裏面と接地面Ｓとの距離を検出する。従って、足裏距離センサ１６ａ及び１６ｄが検出する距離と、足裏距離センサ１６ｂ及び１６ｃが検出する距離と、足裏距離センサ１６ｅ及び１６ｆが検出する距離との差から、足平リンク２６の足裏面の接地面Ｓに対する傾きを求めることができる。
【００７３】
本実施の形態２に係るロボット１００は、爪先関節２７を駆動させて爪先部２６ａ及び踵部２６ｂを路面に倣わせる際に、ロボット１００の足裏から倒立制御に対して効果的な床反力モーメントを得ることができるように、爪先部２６ａに配置された足裏距離センサ１６から３個の距離センサ１６を選択すると共に、踵部２６ｂと路面との距離に基づいて、踵部２６ｂに配置された足裏距離センサ１６から路面との距離を追従させるべき１個の距離センサ１６を選択する。
【００７４】
図１１は、本実施の形態２に係るロボット１００について、足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。図に示すように、足裏倣い制御では、爪先部２６ａの爪先偏差（ロール、ピッチ、ｚ）に加えて、爪先角度も補正する。これに対して、本実施の形態２に係るロボット１００では、足裏距離センサ１６を足裏に６個備えているため、センサの個数が冗長となる。このため、足裏距離センサ１６の故障状況に応じて、距離センサ選択部６１３は、足裏距離センサ１６のうち、床反力モーメントを効果的に得ることができるように足裏距離センサ１６を選択する。そして、足裏倣い制御処理においては、選択された足裏距離センサ１６を用いて足裏倣い制御を実行する。
【００７５】
ここで、爪先部２６ａの爪先偏差（ロール、ピッチ、ｚ）については、爪先部２６ａに設けられた４個の足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄを用いることで計算することができるため、実施の形態１と同様にして、４個の足裏距離センサ１６から３個の足裏距離センサ１６を選択する。爪先角度については、爪先部２６ａに設けられた少なくとも１個の足裏距離センサ１６と、踵部２６ｂに設けられた少なくとも１個の足裏距離センサ１６とを選択することで計算することができる。従って、爪先角度について、爪先部２６ａに設けられた足裏距離センサ１６と、踵部２６ｂに設けられた足裏距離センサ１６とから、少なくともそれぞれ１個ずつの足裏距離センサ１６を使用することができない場合には、足先目標値補正量を適切に計算することができないため、足裏距離センサ１６を用いて足裏倣い制御を実行しないものとする。
【００７６】
以上説明したように足裏距離センサ１６の検出結果が基準範囲内に含まれない場合に、その足裏距離センサ１６の故障を検知する故障検知部を備え、故障していない正常な距離検出部の検出結果に基づいて、歩容データを修正することで、足裏距離センサ１６が故障した場合であっても、路面と足裏との関係を目標通りに制御することができ、安定して歩行することができる。
【００７７】
その他の実施の形態．
上述した実施の形態においては、ロボット１００は２本の脚を備えるものとしたが本発明はこれに限定されない。少なくとも２本以上の脚を有し、それぞれの脚の下端には足平部が設けられ、足平部の足裏には少なくとも３個以上の足裏距離センサを備える脚式ロボットに対しても、本発明を適用することができる。
【００７８】
尚、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
【図面の簡単な説明】
【００７９】
【図１】本発明の実施の形態１に係る脚式ロボットの構成図である。
【図２】本発明の実施の形態１に係る脚式ロボットの足平リンクの構成図である。
【図３】本発明の実施の形態１に係る脚式ロボットの構成を示す機能ブロック図である。
【図４】本発明の実施の形態１に係る脚式ロボットの倒立制御部の構成を示す機能ブロック図である。
【図５】本発明の実施の形態１に係る脚式ロボットの故障検知部による故障検知処理を説明するためのフローチャートである。
【図６】本発明の実施の形態１に係る脚式ロボットの足裏距離センサの検出値と目標値の関係を示す概念図である。
【図７】本発明の実施の形態１に係る脚式ロボットの足裏倣い制御部の機能構成を示す機能ブロック図である。
【図８】本発明の実施の形態１に係る脚式ロボットの足裏倣い制御部による制御処理の概要を説明するためのフローチャートである。
【図９】本発明の実施の形態１に係る脚式ロボットの足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。
【図１０】本発明の実施の形態２に係る脚式ロボットの脚式ロボットの足平リンクの構成図である。
【図１１】本発明の実施の形態２に係る脚式ロボットの足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。
【符号の説明】
【００８０】
１ 制御システム、
１０ 胴体、
１２ 加速度センサ、１４ 姿勢角センサ、１５ モータ、１６ 距離センサ
２０ 脚部、２１ 股関節、２２ 大腿リンク、２３ 膝関節、２４ 脛リンク、
２５ 足首関節、２６ 足平リンク、２６ａ 爪先部、２６ｂ 踵部、２７ 爪先関節、
３０ 制御部、５０ 記憶部、
５１ 歩容データ、５２ 路面ノミナル位置・傾斜、
６０ 演算処理部、６１ 足裏倣い制御部、６２ 倒立制御部、６３ 関節角変換部、
６４ 各軸制御器、６５ 足裏倣い制御切替判定部、６６ 故障検知部、
６１１ 距離センサ目標値計算部、６１３ 距離センサ選択部、
６１４ 位置姿勢偏差計算部、６１５ 補償器、
６２１ 姿勢偏差計算部、６２２ コントローラ、６２３ リミッタ、
１００ ロボット
【技術分野】
【０００１】
本発明は脚式ロボット、及びその制御方法に関する。
【背景技術】
【０００２】
近年、脚部の下端に足平部を設けたロボットにおいて、足平部の足裏を路面に倣わせる足裏倣い制御と、足平部から胴体への位置姿勢を目標の位置姿勢に維持する倒立制御とを組合せて、ロボットを安定して歩行させる技術が開発されている。
【０００３】
このような脚式ロボットでは、まず、足平部の足裏部分を床面に接触させて支持脚とし、その後に足平の裏面で床面を押して脚部全体（ロボット全体）を持ち上げるように脚部を駆動することで、次の歩行動作を行う。駆動された脚部は遊脚となる一方、他の脚部が支持脚となり、このように、遊脚と支持脚を交互に繰り返して切替えることで、歩行動作を行うことができる。
【０００４】
脚式ロボットを転倒させずに安定して移動させるためには、支持脚期ではロボットの足裏を路面に密着させると共に、遊脚を着地させる際には、予期しない路面凹凸からの外乱力を抑制することが効果的である。このため、ロボットの足裏と路面との関係を目標どおりに制御することが重要となる。
【０００５】
従来、ロボットの足裏に距離センサを配置して、距離センサの出力値を目標値に追従させるように足裏を制御する技術が知られている。本出願人による出願（特願２００７−２７８１０７号）には、足裏に路面との距離を検出する距離センサを設け、距離センサの検出結果に基づく足裏倣い制御により歩行安定化を行う２足ロボットが開示されている。当該２足ロボットでは、足裏倣い制御と倒立制御を組合せて制御する際に、ロボットの転倒方向を検出して足裏倣い制御で使用する距離センサを適切に選択することにより、不整地においても足裏が路面に密着しつつ、転倒しているロボットを戻すのに効果的な床反力モーメントを得るものである。
【特許文献１】特開２００７−１５２４７０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、本出願人による出願（特願２００７−２７８１０７号）に開示されるロボットでは、距離センサの故障を検知する手段を備えておらず、距離センサの故障に起因してロボットが転倒する虞があった。即ち、距離センサが故障した際に、故障した距離センサをそのまま用いて制御を行った場合には足裏を路面に密着させることができず、逆に、路面から離れた場所へと足裏を制御しようとするため、歩行が不安定となる。このため、足裏に設けられた距離センサが故障した際には、足裏倣い制御において、路面と足裏との関係を目標通りに制御することができず、結果として、床から意図しない反力を受けてロボットが転倒してしまうという問題がある。
【０００７】
他方、特許文献１には、距離センサの出力信号に変動が生じなかった場合に、距離センサに故障が生じているものと判定する自己診断機能付きロボットが開示されている。かかる特許文献１記載のロボットは、人を検知するための超音波距離センサを胸部に設けると共に、障害物や段差を検出する赤外線センサを脚部の前面に設け、これらセンサの故障をセンサの検出値に基づいて検知するものである。即ち、特許文献１記載のロボットは、足裏と路面との距離を検出する距離検出部の故障を検知して、正常な距離検出部の検出結果に基づいて歩容データを修正することで、路面と足裏との関係を目標通りに制御することを目的とするものではないため、足裏距離センサが故障した場合に、歩行の安定化を図るものではない。
【０００８】
このように、従来の脚式ロボットによれば、距離センサの故障を検知することができないため歩行が不安定となり、ロボットが転倒する虞があるという問題があった。
【０００９】
本発明は、かかる課題を解決するためになされたものであり、距離センサの故障を検知して、安定して歩行が可能な脚式ロボット、及びその制御方法を提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明にかかる脚式ロボットは、胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、前記足平部の足裏と路面との距離を検出する距離検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、を備えた脚式ロボットであって、前記制御部は、前記距離検出部の検出結果が基準範囲内に含まれない場合に、当該距離検出部の故障を検知する故障検知部と、故障していない正常な距離検出部の検出結果に基づいて、前記歩容データを修正する歩容データ修正部と、を備えるものである。
【００１１】
このように距離検出部の検出結果が基準範囲内に含まれるか否かに応じてその故障を検知し、故障していない正常な距離検出部の検出結果に基づいて歩容データを修正することで、距離センサが故障した場合であっても、路面と足裏との関係を目標通りに制御することができ、安定して歩行することができる。
【００１２】
また、前記故障検知部は、前記脚部が遊脚期であって、前記距離検出部の検出値が前記基準範囲内に含まれない場合に、当該距離検出部の故障を検知するようにしてもよい。これにより、脚部が遊脚期である場合には、支持脚期の場合と比べて、距離検出部の検出値はその目標値近辺により安定して収束するため、距離検出部の検出値が基準範囲内に含まれるか否かに応じて距離検出部の故障を容易かつ正確に検知することができる。
【００１３】
さらにまた、前記基準範囲が、前記脚部が遊脚期であって、前記距離検出部の検出可能な最大値を含む所定の範囲であるようにしてもよい。これにより、脚部が遊脚期である場合には、距離検出部の検出値は、その検出可能な最大値近傍の値になるものと考えられるため、脚部が遊脚期である場合に、距離検出部が検出可能な最大値を含む所定の範囲を基準範囲とすることで、距離検出部の故障をより容易かつ正確に検知することができる。
【００１４】
また、前記制御部は、前記距離検出部の検出結果に基づいて、接地面から見た相対的な前記足平部の目標とする位置姿勢と実際の位置姿勢との偏差を計算し、前記歩容データ修正部は、当該偏差を小さくするように前記歩容データを修正するようにしてもよい。
【００１５】
さらにまた、前記制御部は、前記故障検知部の検知結果に応じて、前記制御部による制御が継続可能であるか否かを判定する切替判定部を更に備え、前記歩容データ修正部は、前記切替判定部による判定の結果、前記制御部による制御が継続可能であると判定した場合に、故障していない正常な距離検出部を利用して、前記歩容データを修正するようにしてもよい。このように、距離検出部の故障検知結果に応じて制御の切替を行うことで、距離検出部が故障した際に、ロボットの制御をより迅速に安定化させることができる。
【００１６】
また、前記胴体の姿勢を検出する胴体姿勢検出部を更に備え、前記制御部は、前記胴体姿勢検出部の検出結果に基づいて、前記胴体の目標とする姿勢と実際の姿勢との偏差を計算し、前記歩容データ修正部は、当該偏差を小さくするように前記歩容データを修正するようにしてもよい。
【００１７】
さらにまた、前記脚式ロボットの重心位置を検出する重心位置検出部を更に備え、前記制御部は、前記重心位置検出部の検出結果に基づいて、前記足平部を基準とした目標とする重心位置と実際の重心位置との偏差を計算し、前記歩容データ修正部は、当該偏差を小さくするように前記歩容データを修正するようにしてもよい。
【００１８】
また、前記距離検出部は、前記足平部の足裏に設けられた距離センサの出力信号から前記足平部の足裏と路面との距離を検出するようにしてもよい。
【００１９】
本発明にかかる脚式ロボットの制御方法は、胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、を備えた脚式ロボットの制御方法であって、歩容データに基づいて前記脚部の関節を駆動制御する制御ステップでは、前記足平部の足裏と路面との距離を検出する距離検出ステップと、前記検出された前記足平部の足裏と路面との距離が基準範囲内に含まれない場合に、当該距離検出部の故障を検知する故障検知ステップと、故障していない正常な距離検出部の検出結果に基づいて、前記歩容データを修正する歩容データ修正ステップと、を備えるものである。
【００２０】
このように、検出された足平部の足裏と路面との距離が基準範囲内に含まれるか否かに応じてその故障を検知し、故障していない正常な距離検出部の検出結果に基づいて歩容データを修正することで、距離センサが故障した場合であっても、路面と足裏との関係を目標通りに制御することができ、安定して歩行することができる。
【００２１】
また、前記故障検知部は、前記脚部が遊脚期であって、前記距離検出部の検出値が、前記距離検出部が検出可能な最大値を含む所定の範囲内に含まれない場合に、当該距離検出部の故障を検知するようにしてもよい。これにより、脚部が遊脚期である場合には、支持脚期の場合と比べて、距離検出部の検出値はその目標値近辺により安定して収束し、距離検出部の検出値は、その検出可能な最大値近傍の値になるものと考えられるため、脚部が遊脚期である場合に、距離検出部が検出可能な最大値を含む所定の範囲を基準範囲とすることで、距離検出部の故障をより容易かつ正確に検知することができる。
【発明の効果】
【００２２】
本発明によれば、距離センサの故障を検知して、安定して歩行が可能な脚式ロボット、及びその制御方法を提供することを目的とする。
【発明を実施するための最良の形態】
【００２３】
発明の実施の形態１．
本実施の形態１にかかる脚式ロボットは、胴体と、胴体に連結された脚部と、脚部の下端に設けられた足平部と、足平部の足裏と路面との距離を検出する距離検出部と、歩容データに基づいて脚部の関節を駆動制御する制御部と、を備える。ここで、制御部は、距離検出部の検出結果が基準範囲内に含まれない場合に、その距離検出部の故障を検知する故障検知部と、故障していない正常な距離検出部の検出結果に基づいて、歩容データを修正する歩容データ修正部と、を備える
【００２４】
脚式ロボットが歩行する際には、足平部の足裏に設けられた足裏距離センサを用いて足裏を路面に倣わせる足裏倣い制御と、姿勢センサを用いてロボットの姿勢を倒立させる倒立振子制御を組合せることによって安定化制御を実現している。本実施の形態１にかかる脚式ロボットによれば、足裏距離センサの検出値が基準範囲内に含まれるか否かに応じてその故障を検知し、故障していない正常な足裏距離センサの検出値に基づいて歩容データを修正することで、足裏距離センサが故障した場合であっても、路面と足裏との関係を目標通りに制御することができ、安定して歩行することができる。
【００２５】
以下、図面を参照しながら本実施の形態１に係るロボットの制御方法について説明する。図１は、本実施の形態１に係る脚式ロボットの概要を示す図である。ロボット１００は、胴体１０と、胴体１０に連結された２本の脚を有する。尚、図１には、一方の脚部２０のみを示しており、他方の脚部は図示を省略している。胴体１０は、ロボット１００の動作（脚部の各関節の動作）を制御する制御部３０と、胴体の加速度を検出する加速度センサ１２と、胴体の１０の鉛直方向に対する傾斜角（姿勢角）を検出する姿勢角センサ１４を備える。
【００２６】
脚部２０は、股関節２１、膝関節２３、足首関節２５、大腿リンク２２、脛リンク２４、及び足平部としての足平リンク２６を備える。大腿リンク２２と脛リンク２４は、直線で模式化して示してある。股関節２１は、胴体１０と大腿リンク２２を揺動可能に連結している。膝関節２３は、大腿リンク２２と脛リンク２４を揺動可能に連結している。足首関節２５は、脛リンク２４と足平リンク２６を揺動可能に連結している。脚部２０の下端には足平部としての足平リンク２６が設けられる。足平リンク２６は板状の部材であり、足平リンクの裏面（足裏面）は平面となっている。
【００２７】
足平リンク２６には、距離検出部としての少なくとも３個以上の足裏距離センサ１６が設けられている。足裏距離センサ１６は、足平リンク２６の裏面（足裏面）と接地面Ｓとの距離を検出する。図２は、足平リンク２６の構成を説明するための図である。図２に示すように、足平リンク２６は、上面視において、略矩形状に形成されている。足平リンク２６の四隅近傍には、４つの足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄがそれぞれ設けられている。ここでは、足平リンク２６の爪先側に２つの足裏距離センサ１６ａ及び１６ｄが、踵側に２つ足裏距離センサ１６ｂ及び１６ｃがそれぞれ設けられている。足裏距離センサ１６ａ及び１６ｄは、足平リンク２６の前方の所定位置における足裏面と接地面Ｓとの距離を検出し、足裏距離センサ１６ｂ及び１６ｃは、足平リンク２６の後方の所定位置における足裏面と接地面Ｓとの距離を検出する。従って、足裏距離センサ１６ａ及び１６ｄが検出する距離と１６ｂ及び１６ｃが検出する距離の差から、足平リンク２６の足裏面の接地面Ｓに対する傾きを求めることができる。
【００２８】
各関節には図示しないモータが内蔵されており、制御部３０からの指令に基づいて駆動される。モータを駆動することによって、関節に連結されたリンク同士を揺動させることができる。図示を省略している他方の脚部も、脚部２０と同様の構造を有する。制御部３０が２本の脚部の関節（詳細には関節角）を適宜制御することにより、ロボット１００を歩行させることができる。
【００２９】
図１においては、説明の便宜上、ロボット１００が進行する向き（前後方向）をｘ軸、ロボット１００が進行する方向に対して水平方向に直交する向き（左右方向）をｙ軸、ロボット１００の進行する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これら３軸からなる絶対座標系を用いて説明する。即ち、図１において、ｘ軸は紙面に向かって左右方向、ｙ軸は紙面の奥行き方向、ｚ軸は紙面中の上下方向を示す。尚、ロボット１００の胴体１０に対して点Ｏｂを特定し固定する。足平リンク２６に対して点Ｏｆを特定し固定する。
【００３０】
ロボット１００は、制御部３０に記憶されている歩容データに基づいて制御される。歩容データには、胴体１０の目標位置（目標胴体位置）、胴体１０の目標姿勢角（目標胴体姿勢角）、足平リンク２６の目標位置（目標足平位置）、及び足平リンク２６の目標姿勢角（目標足平姿勢角）のそれぞれの時系列データが含まれる。歩容データには、ロボット１００が有する脚部のそれぞれの目標足平位置・姿勢角の時系列データが含まれる。
【００３１】
歩容データは、シミュレーション等によってロボット１００を安定して歩行させることができるように作成されている。即ち、目標胴体位置、目標胴体姿勢角、目標足平位置、及び目標足平姿勢角は、ロボット１００のＺＭＰ位置が接地面に接地した足裏で囲まれた凸包内となる関係を満足するように設定されている。作成された歩容データは、ロボット１００の制御部３０に記憶される。後述するように、制御部３０は、歩容データに含まれる目標胴体位置等に実胴体位置等を一致させるように各関節を制御する。
【００３２】
目標胴体位置は、絶対座標系に対する特定点Ｏｂの位置で表される。特定点Ｏｂを原点とする胴体座標系を用いる場合には、目標胴体姿勢角は、絶対座標系に対する胴体座標系の傾きで表される。実胴体姿勢角は、胴体１０に備えられた姿勢角センサ１４によって検出することができる。目標足平位置は、絶対座標系に対する特定点Ｏｆの位置で表される。目標足平姿勢角は、接地面に対する足裏面の角度で表される。特定点Ｏｆを原点とする足平座標系を用いる場合には、目標足平姿勢角は、絶対座標系に対する足平座標系の傾きで表してもよい。実足平姿勢角は、後述するように、足平リンク２６に備えられた足裏距離センサ１６によって検出することができる。
【００３３】
続いて、本実施の形態１に係るロボット１００の制御システム１の詳細について説明する。制御システム１は、倒立制御を実行すると共に、足裏倣い制御を実行する。倒立制御は、実胴体位置及び姿勢角を目標胴体位置及び姿勢角に一致する制御である。尚、胴体姿勢角の代わりに重心位置を用いてもよい。足裏倣い制御は、接地面から見た相対的な実足平位置及び実足平姿勢角をそれぞれ目標足平位置及び目標足平姿勢角に一致する制御である。
【００３４】
図３は、制御システム１の機能構成を示す機能ブロック図である。制御システム１は、制御部３０と、記憶部５０と、姿勢角センサ１４と、モータ１５と、足裏距離センサ１６等を有している。
【００３５】
記憶部５０には、歩容データ５１及び路面ノミナル位置・傾斜情報５２が記憶されている。歩容データ５１は、目標胴体位置、目標胴体姿勢角、目標足平位置、及び目標足平姿勢角の時系列データを含む。歩容データ上で足裏が接地面と接触するときは、足裏面と接地面を面接触状態とするため、目標足平姿勢角（仮想的な接地面に対する仮想的な足裏面の傾き）はゼロに設定されている。また、歩容データ上の各目標値は、ロボット１００のＺＭＰ位置が接地している脚の足裏で囲まれた凸包内となる関係を満たすように決定されている。路面ノミナル位置・傾斜情報５２は、ロボット１００が移動する路面の存在情報であり、路面の位置・傾斜に関する基準値を示す。
【００３６】
制御部３０は、記憶部５０に記憶された歩容データ５１等を読み出すと共に、読み出した歩容データ５１等によって特定されるロボット１００の姿勢を実現するために必要な脚部２０の関節角を算出する。そして、このように算出した関節角に基づく信号をモータ１５に送信する。また、制御部３０は、センサからの信号を受けて、モータの駆動量を調整する。
【００３７】
より詳細には、制御部３０は、足裏倣い制御部６１と、倒立制御部６２と、関節角変換部６３と、各軸制御器６４と、足裏倣い制御切替判定部６５と、故障検知部６６と、を備える。制御部３０内では、実胴体姿勢角と目標胴体姿勢角の偏差に基づくフィードバック制御系（倒立制御部６２による倒立制御系）と、接地面から見た相対的な実足平姿勢角と目標足平姿勢角の偏差に基づくフィードバック制御系（足裏倣い制御部６１による倣い制御系）が含まれる。以後、足裏倣い制御の偏差については、接地面から見た相対的な足平姿勢角に関するものを示すものとする。
【００３８】
足裏倣い制御部６１は、実足平姿勢角を目標足平姿勢角に一致させるように、例えば足裏が接地面に密着している状態を目標として、踵側が接地面から浮いている場合には、足平リンク２６の爪先側を脛リンク２４に近づける方向に足平リンク２６を回転させる。即ち、足裏面と接地面との面接触を維持するように足平リンク２６を回転させる。実足平姿勢角は、足平リンク２６が備える足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄの出力値から求められる。尚、足裏倣い制御部６１の詳細については後述する。
【００３９】
倒立制御部６２は、ロボット１００の胴体位置姿勢を目標位置姿勢に維持する機能を果たす。胴体位置姿勢は、実際の胴体位置と胴体姿勢角であり、目標位置姿勢は、目標の胴体位置と胴体姿勢角である。実胴体姿勢角は、胴体１０に備えられた姿勢角センサ１４で検出される。姿勢角センサ１４は、例えば胴体１０の角速度を検出するジャイロと、ジャイロの出力（角速度）を積分する積分器と、重力加速度ベクトルを検出する３軸加速度センサで構成される。尚、倒立制御部６２の詳細については後述する。
【００４０】
故障検知部６６は、足裏距離センサ１６の検出結果が基準範囲内に含まれない場合に、足裏距離センサ１６の故障を検知する。切替判定部としての足裏倣い制御切替判定部６５は、故障検知部６６の検知結果に応じて、制御部３０による制御が継続可能であるか否かを判定する。図示しない歩容データ修正部は、足裏倣い制御切替判定部６５による判定の結果、制御部３０による制御が継続可能であると判定した場合に、故障していない正常な足裏距離センサ１６に基づいて、歩容データを修正する。尚、故障検知部６６及び足裏倣い制御切替判定部６５の詳細については後述する。
【００４１】
より詳細には、以下のようにして記憶部５０に記憶された歩容データ（目標胴体位置、目標胴体姿勢角、目標足平位置、及び目標足平姿勢角の時系列データ）が修正され、関節角変換部６３に入力される。図３においては、胴体位置姿勢目標値が、目標胴体位置及び目標胴体姿勢角の時系列データを含み、足先位置姿勢目標値が、目標足平位置及び目標足平姿勢角の時系列データを含む。
【００４２】
記憶部５０に記憶された目標胴体位置は、目標胴体加速度と実胴体加速度の偏差に基づいて補正された後に関節角変換部６３に入力される。目標胴体加速度は、目標胴体位置を２回微分することによって求められる。実胴体加速度は、加速度センサ１２により検出される。
【００４３】
記憶部５０に記憶された目標足平位置は、目標足平位置と実足平位置の偏差に基づいて補正された後に関節角変換部６３に入力される。
【００４４】
記憶部５０に記憶された目標胴体姿勢角は、そのまま関節角変換部６３に入力される。同時に、目標胴体姿勢角と実胴体姿勢角の偏差（胴体姿勢角偏差）が求められる。胴体姿勢角偏差は倒立制御部６２に入力されて、胴体姿勢角偏差を小さくする方向に胴体を回転させる胴体補正角が算出される。実胴体姿勢角は、姿勢角センサ１４により検出される。尚、図３においては、胴体補正角は足裏倣い制御部６１における補正量となるため、胴体補正角を倣い制御目標値補正量という。
【００４５】
記憶部５０に記憶された目標足平姿勢角と実足平姿勢角の偏差（足平姿勢角偏差）が求められる。実足平姿勢角は、足裏距離センサ１６により検出される。足平姿勢角偏差と上述した胴体補正角（倣い制御目標値補正量）とが加算され、加算された結果が、足裏倣い制御部６１に入力される。足裏倣い制御部６１によって、入力された角度（胴体補正角と足平姿勢角偏差を加算した角度）を小さくする方向へ足平を回転させる足平補正角が求められる。記憶部５０に記憶された目標足平姿勢角は、上述した足平補正角が加算された後に（足平補正角で補正された後に）関節角変換部６３に入力される。尚、図３においては、足平補正角を足先目標値補正量という。
【００４６】
関節角変換部６３には、以上のようにして修正された歩容データが入力される。これらの値から、関節角変換部６３では、逆キネマティクスの演算によって脚部２０の各関節の目標関節角が算出される。ここで、それぞれの目標値は、絶対座標系に対する値で表されている。関節角変換部６３では、目標足平位置と目標胴体位置の差から足平と胴体の相対位置を計算し、目標足平姿勢角（足平補正角によって補正されている）と目標胴体姿勢角の差から足平と胴体の相対回転角を計算する。計算された相対位置と相対回転角を実現する目標関節角が算出される。
【００４７】
各軸制御器６４は、関節角変換部６３により送信された目標関節角の信号に基づいて、脚部２０を駆動するための各モータ１５の駆動量を特定し、これらの駆動量でモータ１５を駆動させるためのモータ駆動信号を各モータに送信する。これによって、脚部２０の各関節における駆動量が変更され、ロボット１００の動きが制御される。
【００４８】
続いて、図４を参照しながら、本実施の形態１に係る倒立制御部６２について詳細に説明する。図４は、倒立制御部６２の機能構成を示す機能ブロック図である。図４に示すように、倒立制御部６２は、姿勢偏差計算部６２１と、コントローラ６２２と、リミッタ６２３と、を有する。
【００４９】
倒立制御部６２は、まず、姿勢偏差計算部６２１において、胴体姿勢目標値（胴体の目標姿勢）と、胴体姿勢計測値（姿勢角センサ１４により検出した実姿勢）との偏差を計算する。そして、計算した胴体の姿勢偏差に基づき、所定のコントローラ６２２によって、足裏倣い制御で使用する足裏と路面との目標相対位置・目標相対姿勢の補正量を計算する。
【００５０】
次いで、倒立制御部６２は、足裏距離センサ１６の出力値に基づき、足裏が路面から剥がれたか否かを判定する。判定の結果、足裏が路面から剥がれていない場合には、計算された目標相対位置・目標相対姿勢に対して補正を行わずに、倣い制御目標値補正量として出力する。一方で、足裏が路面から剥がれた場合には、リミッタ６２３を介して、計算した補正量を制限する。即ち、リミッタ６２３は、目標相対位置・目標相対姿勢の補正量の大きさに制限を加えるものである。リミッタ６２３は、入力された補正量の大きさが許容範囲内を超えているときに、許容限界の値を出力する。このリミッタ６２３を設けることで、足裏倣い制御部６１に入力される倣い制御目標値補正量を制限する。これによって、倣い制御系（足裏倣い制御部６１）が倒立制御系（倒立制御部６２）よりも優勢に作用することを保証する。従って、倣い制御系が優勢に作用して、足裏面が接地面と面接触することを保証することができる。
【００５１】
次いで、倒立制御部６２は、計算された補正量に基づいて、胴体の姿勢偏差を小さくする方向へ、足裏と路面との目標相対位置・目標相対姿勢を補正する。補正された目標相対位置・目標相対姿勢を倣い制御目標値補正量として足裏倣い制御部６１へと出力する。
【００５２】
続いて、図５及び６を参照しながら、本実施の形態１に係る故障検知部６６について詳細に説明する。図５は、故障検知部６６による故障検知処理を説明するためのフローチャートである。図６は、足裏距離センサ１６の検出値と目標値の関係を示す概念図である。
【００５３】
故障検知部６６は、まず、各足裏距離センサ１６の故障を検出するための故障フラグの値を初期化する（ｆａｉｌ＿ｆｌａｇ＝０）（ステップＳ１０１）。次いで、故障検知部６６は、ロボット１００の脚部２０が遊脚であるか又は支持脚であるかを判定する（ステップＳ１０２）。判定の結果、脚部２０が支持脚である場合には、故障検知処理を終了する。
【００５４】
一方で、判定の結果、脚部２０が遊脚である場合には、歩容データに含まれる足平リンク２６の目標位置（ここでは高さ）が、予め定めた所定値よりも大きいか否かを判定する（ステップＳ１０３）。即ち、遊脚期における足平リンク２６の目標高さが十分大きいか否かを判定する。判定の結果、足平リンク２６の目標高さが所定値よりも大きくない場合には、故障検知処理を終了する。
【００５５】
一方で、判定の結果、足平リンク２６の目標高さが所定値よりも大きな場合には、計測された足裏距離センサ１６の出力値が、予め定めた基準範囲内に含まれるか否かを判定する（ステップＳ１０４）。判定の結果、計測された足裏距離センサ１６の出力値が、予め定めた基準範囲内に含まれる場合には、故障検知処理を終了する。
【００５６】
一方で、判定の結果、計測された足裏距離センサ１６の出力値が、予め定めた基準範囲内に含まれない場合には、その足裏距離センサ１６は故障しているものとし、故障状態を示す値を故障フラグの値に設定する（ｆａｉｌ＿ｆｌａｇ＝１）（ステップＳ１０５）。
【００５７】
図６は、足裏距離センサ１６の検出値と目標値の関係を示す概念図である。本実施の形態１に係るロボット１００では、足裏距離センサ１６として接触型のセンサを採用する。これにより、足裏と路面との間の距離を精度良く測定することができる。このような接触型の距離センサでは、遊脚が路面から完全に離床している状態においては、足裏距離センサ１６の出力値として、その計測可能な範囲の最大値を出力する。図６（ａ）に示すように、脚部２０が遊脚時であって、目標高さが所定値よりも十分大きな場合（例えば時間ｔ１〜ｔ２に示す区間の場合）、足裏距離センサ１６の目標とする出力（路面と足裏との目標距離（即ち足平リンク２６の目標高さ））は、足裏距離センサ１６が計測可能な最大値ｈ＿ｍａｘとなる。そこで、図６（ｂ）に示すように、遊脚時における足裏距離センサ１６が検出可能な最大値を中心として、当該最大値を含む範囲を基準範囲として設定する。このように、遊脚時であって、足平リンク２６の目標高さが所定値よりも十分大きな場合に、故障していない正常な足裏距離センサ１６によって計測される出力値は、当該基準範囲内に含まれるものと想定される。従って、計測時における誤差を考慮して、最大値を含む所定の範囲を基準範囲とし、遊脚時であって、足平リンク２６の目標高さが所定値よりも十分大きな場合に、計測された足裏距離センサ１６の出力値が、基準範囲内に含まれるか否かを判定することで、足裏距離センサ１６の故障を容易かつ正確に検知することができる。また、最大値に対して所定の範囲を持たせることで、センサノイズに対しても耐性を有することができる。
【００５８】
続いて、図７乃至９を参照しながら、本実施の形態１に係る足裏倣い制御部６１について詳細に説明する。図７は、足裏倣い制御部６１の機能構成を示す機能ブロック図である。図８は、足裏倣い制御部６１による制御処理の概要を説明するためのフローチャートである。図９は、足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。図５に示すように、足裏倣い制御部６１は、距離センサ目標値計算部６１１と、差分器６１２と、距離センサ選択部６１３と、位置姿勢偏差計算部６１４と、加算器６１５と、補償器６１６を有する。
【００５９】
まず、足裏倣い制御部６１に対して切替判定情報が入力され、足裏倣い制御部６１は、切替判定情報から、足裏距離センサ１６のうちで故障したセンサが存在するか否かを判定する（ステップＳ２０１）。ここで、切替判定情報は足裏倣い制御切替判定部６５により出力される。足裏倣い制御切替判定部６５は、故障検知部６６の検知結果に基づいて、足裏倣い制御が継続可能であるか否かを判定し、当該判定結果を含む切替判定情報を出力する。また、切替判定情報は、各足裏距離センサ１６が故障しているか又は正常であるかを示す足裏距離センサ状態情報（足裏距離センサ１６の故障フラグの値）を含む。
【００６０】
次いで、判定の結果、故障した足裏距離センサ１６が存在する場合には、足裏倣い制御部６１は、切替判定情報から、正常な足裏距離センサ１６のみで足裏倣い制御が継続可能か否かを判定する（ステップＳ２０２）。判定の結果、正常な足裏距離センサ１６のみでは足裏倣い制御が継続不可能な場合には、足裏倣い制御を実行せずに、その出力値（足先目標値補正量）を０に設定する（ステップＳ２０３）。尚、足裏倣い制御と倒立制御は同時に実行することが好ましいため、足裏倣い制御部６１の出力である足先目標値補正量を０とすることで、足裏倣い制御を実行しない場合には、倒立制御も同時に実行させないようにすることができる。
【００６１】
次いでロボット１００の制御部３０は、足先目標値補正量を計算し（ステップＳ２０４）、計算された足先目標値補正量を実現するように歩容データを修正する（ステップＳ２０５）。より詳細には、足裏倣い制御部６１において、相対位置偏差・相対姿勢偏差と倣い制御目標値補正量とが加算器６１５により加算され、加算された結果が、補償器６１６に入力される。補償器６１６において、所定の伝達関数を用いて足先目標値補正量を計算し、計算された足先目標値補正量により、偏差が小さくなるように足平リンク２６の目標位置姿勢を補正する。足先目標値補正量の計算は、例えば、足偏差を入力とし、足先目標値補正量を出力とする伝達関数を通すことで実現することができる。
【００６２】
一方で、ステップＳ２０１における判定の結果、故障した足裏距離センサ１６が存在しない場合には、足裏倣い制御部６１は、全ての足裏距離センサ１６を用いて、通常の足裏倣い制御を実行する（ステップＳ２０７）。より詳細には、まず、足裏倣い制御部６１の距離センサ目標値計算部６１１において、歩容データ５１に含まれる足先位置姿勢目標値（目標足平位置・姿勢角の軌道）と、路面の存在情報である路面のノミナル位置・傾斜情報５２とから、足裏距離センサ１６の高さ目標値を計算する。言い換えると、足平リンク２６の目標位置・姿勢角を示す軌道データと、足平２６が実際に着地する地点（ノミナル位置・傾斜）とから、足裏距離センサ１６の目標とする時系列値を計算する。即ち、目標とする足裏距離センサ１６の出力値を計算する。尚、ノミナル位置・傾斜に代えて、ロボット１００の一歩分の着地位置から足平リンク２６の着地位置・姿勢を計算することで、目標とする足裏距離センサ１６の出力値を計算するようにしてもよい。
【００６３】
次いで、計算した足裏距離センサ１６の目標値と、足裏距離センサ１６によって実際に計測された計測値とから、足裏距離センサ１６の偏差を差分器６１２により計算する。このとき、距離センサ選択部６１３によって、足裏距離センサ１６のうち、床反力モーメントを効果的に得ることができるように足裏距離センサ１６を選択し、選択された足裏距離センサ１６を用いて足裏倣い制御を実行することで、倒立制御において転倒をより効果的に防止することができる。
【００６４】
次いで、位置姿勢偏差計算部６１４において、計算した足裏距離センサ１６の偏差から、足平リンク２６の足裏と路面との相対位置偏差・相対姿勢偏差を計算する。より詳細には、足平リンク２６の目標位置姿勢に対する実際の位置姿勢の足偏差（ロール、ピッチ、ｚ）を計算する。尚、ｚは鉛直方向の測定高さを示す。足裏と距離センサ１６が配置される位置の幾何学的関係から近似を用いて、各足裏距離センサ１６ａ乃至１６ｄの出力値の偏差と、各足裏距離センサ１６の偏差に対応する変換行列とから、足偏差を一意に決定することができる。制御部３０は、このようにして計算した足偏差から、上述したように足先目標値補正量を計算し（ステップＳ２０４）、計算された足先目標補正量を実現するように歩容データを修正する（ステップＳ２０５）。
【００６５】
一方で、ステップＳ２０２における判定の結果、正常な足裏距離センサ１６のみで足裏倣い制御が継続可能である場合には、正常な足裏距離センサ１６のみを用いて、足裏倣い制御を実行する（ステップＳ２０６）。次いで、制御部３０は、上述したようにして、計算した足偏差から、足先目標値補正量を計算し（ステップＳ２０４）、計算された足先目標補正量を実現するように歩容データを修正する（ステップＳ２０５）。
【００６６】
図９は、足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。図において、故障した足裏距離センサ１６を斜線領域により示す。図９（ａ）に示すように、足裏倣い制御では、足偏差（ロール、ピッチ、ｚ）を補正する。これに対して、本実施の形態１に係るロボット１００では、足裏距離センサ１６を４個備えているため、センサの個数が冗長となる。このため、図９（ｂ）乃至（ｄ）に示すように、足裏距離センサ１６の故障状況に応じて、距離センサ選択部６１３は、足裏距離センサ１６のうち、床反力モーメントを効果的に得ることができるように足裏距離センサ１６を選択する。そして、足裏倣い制御処理においては、選択された足裏距離センサ１６を用いて足裏倣い制御を実行する。
【００６７】
まず、図９（ｂ）に示すように、足裏距離センサ１６が全て正常な場合には（図８におけるステップＳ２０７の場合）、床反力モーメントを効果的に得ることができるように３個の足裏距離センサ１６を選択することで、倒立制御の転倒を防止することができる。
【００６８】
他方、図９（ｃ）に示すように、足裏距離センサ１６のうち１つが故障している場合には（例えば図８におけるステップＳ２０６の場合）、正常な残り３個の足裏距離センサ１６を選択し、選択された足裏距離センサ１６を用いて足裏倣い制御を実行する。
【００６９】
また、図９（ｄ）に示すように、足裏距離センサ１６のうち２つが故障している場合には（例えば図８におけるステップＳ２０３の場合）、正常な残り２個の足裏距離センサ１６では足先目標値補正量を適切に計算することができないため、足裏距離センサ１６を用いて足裏倣い制御を実行しないものとする。
【００７０】
発明の実施の形態２．
上述した実施の形態１においては、足平リンク２６に設けられた足裏距離センサ１６が４個である場合について、それら足裏距離センサ１６のうち、３個の足裏距離センサ１６を選択する処理について説明した。本発明はこれに限定されず、足平リンク２６に設けられた少なくとも３個以上の足裏距離センサ１６から、３個の足裏距離センサ１６を選択するようにしてもよい。尚、本実施の形態２に係るロボット１００の構成は、足平リンク２６の構成、及び足平リンク２６に設けられる足裏距離センサ１６の配置を除いて、上述した発明の実施の形態１と同様である。
【００７１】
図１０は、本実施の形態２に係るロボット１００の足平リンク２６の構成を説明するための図である。足平リンク２６は、爪先部２６ａと、爪先関節２７を介して爪先部２６ａに連結された踵部２６ｂとから構成される。ロボット１００の歩行動作中には、爪先関節２７を駆動することで、爪先部２６ａを路面に対して接地させたまま、踵部２６ｂを浮かせることができる。即ち、踵部２６ｂを浮かせたまま爪先部２６ａのみを接地させることにより、ロボット１００を直立させることができる。
【００７２】
図１０に示すように、爪先部２６ａ及び踵部２６ｂは、上面視において、略矩形状に形成されている。爪先部２６ａの四隅近傍には、４つの足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄがそれぞれ設けられている。ここでは、爪先部２６ａの爪先側に２つの足裏距離センサ１６ａ及び１６ｄが、踵部方向側に２つ足裏距離センサ１６ｂ及び１６ｃが設けられている。足裏距離センサ１６ａ及び１６ｄは、爪先部２６ａの前方の所定位置における足裏面と接地面Ｓとの距離を検出し、足裏距離センサ１６ｂ及び１６ｃは、爪先部２６ａの後方の所定位置における足裏面と接地面Ｓとの距離を検出する。踵部２６ｂには、爪先部側とは反対側の二隅に２つの足裏距離センサ１６ｅ及び１６ｆがそれぞれ設けられている。足裏距離センサ１６ｅ及び１６ｆは、踵部２６ｂの後方の所定位置における足裏面と接地面Ｓとの距離を検出する。従って、足裏距離センサ１６ａ及び１６ｄが検出する距離と、足裏距離センサ１６ｂ及び１６ｃが検出する距離と、足裏距離センサ１６ｅ及び１６ｆが検出する距離との差から、足平リンク２６の足裏面の接地面Ｓに対する傾きを求めることができる。
【００７３】
本実施の形態２に係るロボット１００は、爪先関節２７を駆動させて爪先部２６ａ及び踵部２６ｂを路面に倣わせる際に、ロボット１００の足裏から倒立制御に対して効果的な床反力モーメントを得ることができるように、爪先部２６ａに配置された足裏距離センサ１６から３個の距離センサ１６を選択すると共に、踵部２６ｂと路面との距離に基づいて、踵部２６ｂに配置された足裏距離センサ１６から路面との距離を追従させるべき１個の距離センサ１６を選択する。
【００７４】
図１１は、本実施の形態２に係るロボット１００について、足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。図に示すように、足裏倣い制御では、爪先部２６ａの爪先偏差（ロール、ピッチ、ｚ）に加えて、爪先角度も補正する。これに対して、本実施の形態２に係るロボット１００では、足裏距離センサ１６を足裏に６個備えているため、センサの個数が冗長となる。このため、足裏距離センサ１６の故障状況に応じて、距離センサ選択部６１３は、足裏距離センサ１６のうち、床反力モーメントを効果的に得ることができるように足裏距離センサ１６を選択する。そして、足裏倣い制御処理においては、選択された足裏距離センサ１６を用いて足裏倣い制御を実行する。
【００７５】
ここで、爪先部２６ａの爪先偏差（ロール、ピッチ、ｚ）については、爪先部２６ａに設けられた４個の足裏距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄを用いることで計算することができるため、実施の形態１と同様にして、４個の足裏距離センサ１６から３個の足裏距離センサ１６を選択する。爪先角度については、爪先部２６ａに設けられた少なくとも１個の足裏距離センサ１６と、踵部２６ｂに設けられた少なくとも１個の足裏距離センサ１６とを選択することで計算することができる。従って、爪先角度について、爪先部２６ａに設けられた足裏距離センサ１６と、踵部２６ｂに設けられた足裏距離センサ１６とから、少なくともそれぞれ１個ずつの足裏距離センサ１６を使用することができない場合には、足先目標値補正量を適切に計算することができないため、足裏距離センサ１６を用いて足裏倣い制御を実行しないものとする。
【００７６】
以上説明したように足裏距離センサ１６の検出結果が基準範囲内に含まれない場合に、その足裏距離センサ１６の故障を検知する故障検知部を備え、故障していない正常な距離検出部の検出結果に基づいて、歩容データを修正することで、足裏距離センサ１６が故障した場合であっても、路面と足裏との関係を目標通りに制御することができ、安定して歩行することができる。
【００７７】
その他の実施の形態．
上述した実施の形態においては、ロボット１００は２本の脚を備えるものとしたが本発明はこれに限定されない。少なくとも２本以上の脚を有し、それぞれの脚の下端には足平部が設けられ、足平部の足裏には少なくとも３個以上の足裏距離センサを備える脚式ロボットに対しても、本発明を適用することができる。
【００７８】
尚、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
【図面の簡単な説明】
【００７９】
【図１】本発明の実施の形態１に係る脚式ロボットの構成図である。
【図２】本発明の実施の形態１に係る脚式ロボットの足平リンクの構成図である。
【図３】本発明の実施の形態１に係る脚式ロボットの構成を示す機能ブロック図である。
【図４】本発明の実施の形態１に係る脚式ロボットの倒立制御部の構成を示す機能ブロック図である。
【図５】本発明の実施の形態１に係る脚式ロボットの故障検知部による故障検知処理を説明するためのフローチャートである。
【図６】本発明の実施の形態１に係る脚式ロボットの足裏距離センサの検出値と目標値の関係を示す概念図である。
【図７】本発明の実施の形態１に係る脚式ロボットの足裏倣い制御部の機能構成を示す機能ブロック図である。
【図８】本発明の実施の形態１に係る脚式ロボットの足裏倣い制御部による制御処理の概要を説明するためのフローチャートである。
【図９】本発明の実施の形態１に係る脚式ロボットの足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。
【図１０】本発明の実施の形態２に係る脚式ロボットの脚式ロボットの足平リンクの構成図である。
【図１１】本発明の実施の形態２に係る脚式ロボットの足裏倣い制御が継続可能であるか否かの判定処理を説明するための図である。
【符号の説明】
【００８０】
１ 制御システム、
１０ 胴体、
１２ 加速度センサ、１４ 姿勢角センサ、１５ モータ、１６ 距離センサ
２０ 脚部、２１ 股関節、２２ 大腿リンク、２３ 膝関節、２４ 脛リンク、
２５ 足首関節、２６ 足平リンク、２６ａ 爪先部、２６ｂ 踵部、２７ 爪先関節、
３０ 制御部、５０ 記憶部、
５１ 歩容データ、５２ 路面ノミナル位置・傾斜、
６０ 演算処理部、６１ 足裏倣い制御部、６２ 倒立制御部、６３ 関節角変換部、
６４ 各軸制御器、６５ 足裏倣い制御切替判定部、６６ 故障検知部、
６１１ 距離センサ目標値計算部、６１３ 距離センサ選択部、
６１４ 位置姿勢偏差計算部、６１５ 補償器、
６２１ 姿勢偏差計算部、６２２ コントローラ、６２３ リミッタ、
１００ ロボット
【特許請求の範囲】
【請求項１】
胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、前記足平部の足裏と路面との距離を検出する距離検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、を備えた脚式ロボットであって、
前記制御部は、
前記距離検出部の検出結果が基準範囲内に含まれない場合に、当該距離検出部の故障を検知する故障検知部と、
故障していない正常な距離検出部の検出結果に基づいて、前記歩容データを修正する歩容データ修正部と、を備える
ことを特徴とする脚式ロボット。
【請求項２】
前記故障検知部は、
前記脚部が遊脚期であって、前記距離検出部の検出値が前記基準範囲内に含まれない場合に、当該距離検出部の故障を検知する
ことを特徴とする請求項１記載の脚式ロボット。
【請求項３】
前記基準範囲が、
前記脚部が遊脚期であって、前記距離検出部の検出可能な最大値を含む所定の範囲である
ことを特徴とする請求項２記載の脚式ロボット。
【請求項４】
前記制御部は、
前記距離検出部の検出結果に基づいて、前記足平部の目標とする位置姿勢と実際の位置姿勢との偏差を計算し、
前記歩容データ修正部は、
当該偏差を小さくするように前記歩容データを修正する
ことを特徴とする請求項１乃至３いずれか１項記載の脚式ロボット。
【請求項５】
前記制御部は、
前記故障検知部の検知結果に応じて、前記制御部による制御が継続可能であるか否かを判定する切替判定部を更に備え、
前記歩容データ修正部は、
前記切替判定部による判定の結果、前記制御部による制御が継続可能であると判定した場合に、故障していない正常な距離検出部を利用して、前記歩容データを修正する
ことを特徴とする請求項１記載乃至４いずれか１項記載の脚式ロボット。
【請求項６】
前記胴体の姿勢を検出する胴体姿勢検出部を更に備え、
前記制御部は、
前記胴体姿勢検出部の検出結果に基づいて、前記胴体の目標とする姿勢と実際の姿勢との偏差を計算し、
前記歩容データ修正部は、
当該偏差を小さくするように前記歩容データを修正する
ことを特徴とする請求項１記載の脚式ロボット。
【請求項７】
前記脚式ロボットの重心位置を検出する重心位置検出部を更に備え、
前記制御部は、
前記重心位置検出部の検出結果に基づいて、前記足平部を基準とした目標とする重心位置と実際の重心位置との偏差を計算し、
前記歩容データ修正部は、
当該偏差を小さくするように前記歩容データを修正する
ことを特徴とする請求項１記載の脚式ロボット。
【請求項８】
前記距離検出部は、前記足平部の足裏に設けられた距離センサの出力信号から前記足平部の足裏と路面との距離を検出する
ことを特徴とする請求項１乃至７いずれか１項記載の脚式ロボット。
【請求項９】
胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、を備えた脚式ロボットの制御方法であって、
歩容データに基づいて前記脚部の関節を駆動制御する制御ステップでは、
前記足平部の足裏と路面との距離を検出する距離検出ステップと、
前記検出された前記足平部の足裏と路面との距離が基準範囲内に含まれない場合に、当該距離検出部の故障を検知する故障検知ステップと、
故障していない正常な距離検出部の検出結果に基づいて、前記歩容データを修正する歩容データ修正ステップと、を備える
ことを特徴とする脚式ロボットの制御方法。
【請求項１０】
前記故障検知部は、
前記脚部が遊脚期であって、前記距離検出部の検出値が、前記距離検出部が検出可能な最大値を含む所定の範囲内に含まれない場合に、当該距離検出部の故障を検知する
ことを特徴とする請求項９記載の脚式ロボットの制御方法。
【請求項１】
胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、前記足平部の足裏と路面との距離を検出する距離検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、を備えた脚式ロボットであって、
前記制御部は、
前記距離検出部の検出結果が基準範囲内に含まれない場合に、当該距離検出部の故障を検知する故障検知部と、
故障していない正常な距離検出部の検出結果に基づいて、前記歩容データを修正する歩容データ修正部と、を備える
ことを特徴とする脚式ロボット。
【請求項２】
前記故障検知部は、
前記脚部が遊脚期であって、前記距離検出部の検出値が前記基準範囲内に含まれない場合に、当該距離検出部の故障を検知する
ことを特徴とする請求項１記載の脚式ロボット。
【請求項３】
前記基準範囲が、
前記脚部が遊脚期であって、前記距離検出部の検出可能な最大値を含む所定の範囲である
ことを特徴とする請求項２記載の脚式ロボット。
【請求項４】
前記制御部は、
前記距離検出部の検出結果に基づいて、前記足平部の目標とする位置姿勢と実際の位置姿勢との偏差を計算し、
前記歩容データ修正部は、
当該偏差を小さくするように前記歩容データを修正する
ことを特徴とする請求項１乃至３いずれか１項記載の脚式ロボット。
【請求項５】
前記制御部は、
前記故障検知部の検知結果に応じて、前記制御部による制御が継続可能であるか否かを判定する切替判定部を更に備え、
前記歩容データ修正部は、
前記切替判定部による判定の結果、前記制御部による制御が継続可能であると判定した場合に、故障していない正常な距離検出部を利用して、前記歩容データを修正する
ことを特徴とする請求項１記載乃至４いずれか１項記載の脚式ロボット。
【請求項６】
前記胴体の姿勢を検出する胴体姿勢検出部を更に備え、
前記制御部は、
前記胴体姿勢検出部の検出結果に基づいて、前記胴体の目標とする姿勢と実際の姿勢との偏差を計算し、
前記歩容データ修正部は、
当該偏差を小さくするように前記歩容データを修正する
ことを特徴とする請求項１記載の脚式ロボット。
【請求項７】
前記脚式ロボットの重心位置を検出する重心位置検出部を更に備え、
前記制御部は、
前記重心位置検出部の検出結果に基づいて、前記足平部を基準とした目標とする重心位置と実際の重心位置との偏差を計算し、
前記歩容データ修正部は、
当該偏差を小さくするように前記歩容データを修正する
ことを特徴とする請求項１記載の脚式ロボット。
【請求項８】
前記距離検出部は、前記足平部の足裏に設けられた距離センサの出力信号から前記足平部の足裏と路面との距離を検出する
ことを特徴とする請求項１乃至７いずれか１項記載の脚式ロボット。
【請求項９】
胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足平部と、を備えた脚式ロボットの制御方法であって、
歩容データに基づいて前記脚部の関節を駆動制御する制御ステップでは、
前記足平部の足裏と路面との距離を検出する距離検出ステップと、
前記検出された前記足平部の足裏と路面との距離が基準範囲内に含まれない場合に、当該距離検出部の故障を検知する故障検知ステップと、
故障していない正常な距離検出部の検出結果に基づいて、前記歩容データを修正する歩容データ修正ステップと、を備える
ことを特徴とする脚式ロボットの制御方法。
【請求項１０】
前記故障検知部は、
前記脚部が遊脚期であって、前記距離検出部の検出値が、前記距離検出部が検出可能な最大値を含む所定の範囲内に含まれない場合に、当該距離検出部の故障を検知する
ことを特徴とする請求項９記載の脚式ロボットの制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２００９−１８４０３５（Ｐ２００９−１８４０３５Ａ）
【公開日】平成２１年８月２０日（２００９．８．２０）
【国際特許分類】
【出願番号】特願２００８−２４１２５（Ｐ２００８−２４１２５）
【出願日】平成２０年２月４日（２００８．２．４）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２１年８月２０日（２００９．８．２０）
【国際特許分類】
【出願日】平成２０年２月４日（２００８．２．４）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]