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

Abstract

【課題】不整地においても、床反力モーメントを効果的に得つつ、足裏と路面との接触を保つことができる脚式ロボット、及びその制御方法を提供する。
【解決手段】脚式ロボットは、胴体１０と、脚部２０と、足部２６と、胴体１０の転倒方向を検出する転倒方向検出部と、制御部３０と、足部２６の足裏と路面との距離を検出する距離検出部と、を備え、距離検出部は、足裏に設けられた少なくとも３個の距離センサから構成され、制御部３０が、距離センサを選択する距離センサ選択手段と、距離センサ選択手段により選択された距離センサの検出信号に基づいて、歩容データを修正する歩容データ修正手段と、を有し、距離センサ選択手段が、距離センサのうち、転倒方向検出部の検出結果に基づいて、３個の距離センサを選択する。
【選択図】図１

Description

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

脚式移動ロボットを転倒させずに安定して移動させるためには、ロボットの支持脚を路面に密着させることが必要である。また、ロボットの遊脚を着地させる際には、予期しない路面凹凸からの外乱力を抑制することも効果的である。このため、ロボットの足裏と路面との関係を目標どおりに制御することが重要である。

従来、ロボットの足裏にセンサを配置して、センサの出力値を目標値に追従させる多くの技術が知られている。特許文献１には、複雑な凹凸がある不安定な路面状況においても、足裏における床反力を正確に検出して歩行安定性を実現するための、二脚歩行式移動装置及びその歩行制御装置が開示されている。特許文献１記載の二脚歩行式移動装置では、足裏に配置された少なくとも３個の３軸力センサのうち、検出している力の大きい順に３個選択し、その床反力データに基づいて歩容を修正する。

特許文献２には、複雑な凹凸がある不安定な路面状況においても足部が確実に着地できるようにするための、二脚歩行式移動装置が開示されている。特許文献２記載の二脚歩行式移動装置では、踵部と足先部に三軸力センサを備え、それら力センサの信号に基づいて踵部と足先部で３点支持するように歩容データを修正する。

特許文献３には、脚式移動ロボットの歩行中における路面の表面状態や路面への足部の接地状態を正確に判断するための、脚式移動ロボットのための足部構造、並びに路面検知装置が開示されている。特許文献３記載の脚式移動ロボットのための足部構造では、足部ベース体と、足部ベース体の底面に配置されて、足部と路面との接近及び接地状況を測定する路面検知部と、を備える。路面検知子は足部ベース体に対して出没自在に取り付けられ、路面検知部が路面検知子の出没位置を検出する。また、実施例では、足裏距離センサを使用して、足裏と路面との距離をフィードバック制御することにより安定化を図る手段が開示されている。
特許３５７４９５２号公報 特許３５６９７６８号公報 特開２００１−３５３６８６号公報

しかしながら、特許文献１及び２記載の技術では、ちょうど３個の３軸力センサのみが路面に接触している場合には問題は無いが、それ以上の個数のセンサが路面に接触する場合には、床反力を正確に得ることができない場合がある。３個より多くのセンサを用いる場合には、路面状態によって全てのセンサを路面に接触させることができないことがあり、床反力計算に使用できない３軸力センサが増えることになるため、床反力を正確に検出することができない。また、センサの個数を３個とした場合には、足裏と路面との接触部分によって形成される実際の凸包が狭くなるため、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）が存在可能な安定領域が狭くなり、ロボットが転倒しやすくなる。さらに、ヨー方向にも滑りやすくなる。従って、センサの個数を３個以上とすると冗長となるものの、センサの個数が３個では不十分なため、事実上、センサの配置は実施例に示された４個の場合のみが有効である。

また、特許文献３記載の技術では、センサ以外の部分に路面が接触した場合には、センサ位置における路面と足裏との距離を物理的にゼロとすることができない。即ち、路面状態によっては、全ての距離センサの計測高さ値をゼロにできない場合がある。

ロボットの足裏と路面との距離関係を目標どおりに制御する際には、安定性を向上させるため、距離センサは可能な限り足裏の外周に配置させることが望ましい。さらに、足裏と路面との接触部の凸包を可能な限り広くするため、距離センサを４隅に配置することが好ましい。しかし、面は３点で決定されることを考慮すると、距離センサを４隅に配置した場合には、路面とセンサとの接触箇所が冗長なものとなる。距離センサの個数が冗長であると、ロボットの足裏が複雑な凹凸を踏んだ場合に、全ての距離センサの値を所望の値にすることができない。例えば図１８に示すように、ロボットの足部２６一部が障害物Ｂを踏んでいる場合（足部２６の裏面の点１６ａが障害物Ｂ上に位置する場合）には、足裏の点１６ｃ及び１６ａを路面に接触させることは可能であるが、点１６ｂ及び１６ｄを同時に路面に接触させることはできない。言い換えると、足裏の点１６ｃ及び１６ａを路面に接触させた場合には、足裏は点１６ａ及び点１６ｃを通過する対角線上の軸Ｃを中心に回転することになり、不安定な状態となる。このため、路面の凹凸状況によっては、支持脚足裏の４点全てを路面に密着させることができない。このような場合に、全ての足裏距離センサの値を所望の値とするよう制御すると、例えば、距離の二乗誤差が最小となるように足裏を補正した場合には、足裏と路面とが対角線上で接地するなど、非常に不安定な状態に制御しようとする。即ち、全ての距離センサの値を目標値に追従させようとすると、逆に不安定な状態を保とうとするため、足裏を路面に密着できない状態においてロボットの体幹が傾いた場合には、ロボットはその傾きを戻すための床反力モーメントを得ることができずに転倒する虞がある。従って、ロボットの転倒を防ぐためには、支持脚の足裏距離センサの距離のうち、どの距離を目標値に追従させるべきかを選択する必要がある。

傾いた体幹を戻すために必要な床反力モーメントは、目標高さに追従させる足裏距離センサを適切に選択することで得ることができる。足裏距離センサを３個のみ選択する構成とした場合には、体幹の転倒方向によっては、支持脚足裏を路面に密着させることができず、不安定になることがある。例えば図１９に示すように路面上の凸部を１ヶ所の距離センサ１６ｄが踏んだ際に、検出された転倒方向Ｄから３個の足裏距離センサ１６ａ、１６ｂ、１６ｃを選択するものとすると、足裏距離センサ１６ａ及び１６ｃとを同時に接触させることができないため、支持脚足裏を完全に倣わせることができない。これは、ロボットの体幹が傾いている方向と路面の凹凸部の方向とが近い場合には、上述と同様に足裏と路面とが対角線上で接地し、不安定な状態に制御しようとするためである。

このように、従来の脚式ロボットによれば、ロボットの体幹を倒立させるための床反力モーメントを効果的に得つつ、足裏と路面との接触を保つことができないため、路面状況によってはロボットが転倒せずに安定して歩行することができないという問題があった。

本発明は、かかる課題を解決するためになされたものであり、不整地においても、ロボットの体幹を倒立させるための床反力モーメントを効果的に得つつ、足裏と路面との接触を保つことができる脚式ロボット、及びその制御方法を提供することを目的とする。

本発明にかかる脚式ロボットは、胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足部と、前記胴体の転倒方向を検出する転倒方向検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、前記足部の足裏と路面との距離を検出する距離検出部と、を備えた脚式ロボットであって、前記距離検出部は、前記足部の足裏に設けられた少なくとも３個の距離センサから構成され、前記制御部が、前記距離センサを選択する距離センサ選択手段と、前記距離センサ選択手段により選択された距離センサの検出信号に基づいて、前記歩容データを修正する歩容データ修正手段と、を有し、前記距離センサ選択手段が、前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、３個の距離センサを選択するものである。

このようにロボットの体幹の転倒方向を検出して、検出結果に基づいて目標値に追従させるべき３個の足裏距離センサを選択することで、ロボットの体幹（胴体）を倒立させるための床反力モーメントを効果的に得つつ、足裏と路面との接触を保つことができる。従って、支持脚を床面に密着させつつ、足裏からの床反力モーメントを受けてロボットの体幹を倒立させる安定化制御を容易に実行することができるため、不整地においても脚式ロボットは転倒せずに歩行することができる。尚、歩容データはロボットを歩行させるためのデータであって、胴体の目標位置・姿勢、及び足部の目標位置・姿勢を含む。また、本発明に係る脚式ロボットは、まず、脚部の足平部分を床面に接触させて支持脚とし、その後に足平の裏面で床面を押して脚部全体（ロボット全体）を持ち上げるように脚部を駆動することで、次の歩行動作を行う。駆動された脚部は遊脚となる一方、他の脚部が支持脚となり、このよに、遊脚と支持脚を交互に繰り返して切替えることで、歩行動作を行うことができる。

また、前記距離センサ選択手段が、前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、第一及び第二の距離センサを選択し、選択されなかった前記距離センサのうち、前記足部の足裏と路面との距離に基づいて、第三の距離センサを選択するようにしてもよい。これにより、ロボットの体幹を倒立させるための足裏から床反力モーメントをより効果的に得つつ、足裏と路面との接触をより容易に保つことができる。

さらにまた、前記距離センサ選択手段が、前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、第一及び第二の距離センサを選択し、選択されなかった前記距離センサのうち、前記足部の足裏と路面との距離が最も小さい距離センサを第三の距離センサとして選択するようにしてもよい。これにより、路面との距離を追従させるべき足裏距離センサをより容易に選択することができる。

また、前記制御部が、前記脚部の歩行動作パターンが支持脚の歩行動作パターンであるか又は遊脚の歩行動作パターンであるかを判定し、判定結果に基づいて、前記距離センサ選択手段が選択する距離センサの個数を可変とするようにしてもよい。このように、歩行動作パターンに応じて選択する距離センサの個数を変更することで、例えば路面に接触している支持脚である場合には基本的に３個の距離センサを選択するものとし、路面と接触していない遊脚である場合には基本的に全てのセンサを有効とすることで、路面との接触状況に応じて有効な距離センサを柔軟に設定することができる。

さらにまた、前記脚部が支持脚である場合に、前記制御部が、前記距離検出部の出力信号に基づいて、前記脚部の足裏が路面に完全に着地しているか否かを判定し、判定の結果、前記脚部の足裏が路面に完全に着地している場合には、前記距離センサ選択手段が、前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、３個の距離センサを選択し、前記判定の結果、前記脚部の足裏が路面に完全に着地していない場合には、前記距離センサ選択手段が、全ての前記距離センサを選択するようにしてもよい。このように、支持脚が完全に着地するまでの間は全ての距離センサを選択して有効とすることで、路面に対して速やかに倣うことができる。このため、着地時の外乱をより効果的に抑制することができる。

また、前記脚部が遊脚である場合に、前記制御部が、前記距離検出部の出力信号に基づいて、前記脚部の足裏が路面に完全に離地しているか否かを判定し、判定の結果、前記脚部の足裏が路面に完全に離地していない場合には、前記距離センサ選択手段が、前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、３個の距離センサを選択し、前記判定の結果、前記脚部の足裏が路面に完全に離地している場合には、前記距離センサ選択手段が、全ての前記距離センサを選択するようにしてもよい。このように、遊脚が完全に離地するまでの間は足裏と路面とが接触しているため、３個の距離センサを選択して有効とすることで、支持脚の補正量を保持する。また、遊脚が完全に離地した後には全ての距離センサを選択して有効とすることで、遊脚が路面と接触した場合であっても路面を回避する方向へと速やかに倣うことができる。

本発明に係る脚式ロボットの制御方法は、胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足部と、前記胴体の転倒方向を検出する転倒方向検出部と、前記足部の足裏と路面との距離を検出する少なくとも３個の距離センサ、を備えた脚式ロボットの制御方法であって、歩容データに基づいて前記脚部の関節を駆動制御する制御ステップでは、前記距離センサを選択する距離センサ選択ステップと、選択された距離センサの検出信号に基づいて、前記歩容データを修正する歩容データ修正ステップと、を含み、前記距離センサ選択ステップでは、前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、第一及び第二の距離センサを選択し、選択されなかった前記距離センサのうち、前記足部の足裏と路面との距離に基づいて、第三の距離センサを選択するものである。

このようにロボットの体幹の転倒方向を検出して、検出結果に基づいて目標値に追従させるべき３個の足裏距離センサを選択することで、ロボットの体幹を倒立させるための床反力モーメントを効果的に得つつ、足裏と路面との接触を保つことができる。従って、支持脚を床面に密着させつつ、足裏からの床反力モーメントを受けてロボットの体幹を倒立させる安定化制御を容易に実行することができるため、不整地においても脚式ロボットは転倒せずに歩行することができる。

本発明によれば、不整地においても、ロボットの体幹を倒立させるための床反力モーメントを効果的に得つつ、足裏と路面との接触を保つことができる脚式ロボット、及びその制御方法を提供することを目的とする。

発明の実施の形態１．
本実施の形態１にかかる脚式ロボットは、胴体と、胴体に連結された脚部と、脚部の下端に設けられた足部と、胴体の転倒方向を検出する転倒方向検出部と、脚部の関節を駆動制御する制御部と、足部の足裏と路面との距離を検出する距離検出部と、を備える。ここで、距離検出部は、足部の足裏に設けられた少なくとも３個の距離センサから構成される。脚式ロボットの制御部は、距離センサを選択する距離センサ選択手段と、距離センサ選択手段により選択された距離センサの検出信号に基づいて、歩容データを修正する歩容データ修正手段と、を有する。距離センサ選択手段は、距離センサのうち、転倒方向検出部の検出結果に基づいて、３個の距離センサを選択する。

歩行ロボットが不整地を歩行する際には、足裏に設けられた距離センサを用いて足裏を路面に倣わせる足裏倣い制御と、姿勢センサを用いてロボットの姿勢を倒立させる倒立振子制御を組み合わせることによって安定化制御を実現している。足裏倣い制御に用いる足裏距離センサの数が冗長な場合に、全てのセンサを同等に扱うものとすると逆に不安定となり、ロボットが転倒する虞がある。本実施の形態１にかかる脚式ロボットによれば、ロボット状態、センサ状態、及び歩行状態に応じて、冗長な足裏距離センサから有効なセンサを適切に選択することで、不整地においても転倒せずに安定して歩行することができる。

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

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

足平リンク２６には、距離検出部としての少なくとも３個以上の距離センサ１６が設けられている。距離センサ１６は、足平リンク２６の裏面（足裏面）と接地面Ｓとの距離を検出する。図２は、足平リンク２６の構成を説明するための図である。図２に示すように、足平リンク２６は、上面視において、略矩形状に形成されている。足平リンク２６の四隅近傍には、４つの距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄがそれぞれ設けられている。ここでは、足平リンク２６の爪先側に２つの距離センサ１６ａ及び１６ｄが、踵側に２つ距離センサ１６ｂ及び１６ｃが設けられている。距離センサ１６ａ及び１６ｄは、足平リンク２６の前方の所定位置における足裏面と接地面Ｓとの距離を検出し、距離センサ１６ｂ及び１６ｃは、足平リンク２６の後方の所定位置における足裏面と接地面Ｓとの距離を検出する。従って、距離センサ１６ａ及び１６ｄが検出する距離と１６ｂ及び１６ｃが検出する距離の差から、足平リンク２６の足裏面の接地面Ｓに対する傾きを求めることができる。

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

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

ロボット１００は、制御部３０に記憶されている歩容データに基づいて制御される。歩容データには、胴体１０の目標位置（目標胴体位置）、胴体１０の目標姿勢角（目標胴体姿勢角）、足平リンク２６の目標位置（目標足平位置）、及び足平リンク２６の目標姿勢角（目標足平姿勢角）のそれぞれの時系列データが含まれる。歩容データには、ロボット１００が有する脚部のそれぞれの目標足平位置・姿勢角の時系列データが含まれる。

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

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

続いて、制御部３０の動作の詳細について説明する。制御部３０は、倒立制御を実行すると共に、倣い制御を実行する。倒立制御は、実胴体位置及び実胴体姿勢角をそれぞれ目標胴体位置及び目標胴体姿勢角に一致する制御である。倣い制御は、接地面から見た相対的な実足平位置及び実足平姿勢角をそれぞれ目標足平位置及び目標足平姿勢角に一致する制御である。

図３は、制御部３０の機能構成を示す機能ブロック図である。制御部３０は、歩容データを記憶する記憶部５０と、記憶部５０に記憶された歩容データを読み出す演算処理部６０と、脚部２０に含まれる各モータを駆動するモータ駆動部７０と、を備えている。

記憶部５０には、歩容データ（目標胴体位置、目標胴体姿勢角、目標足平位置、及び目標足平姿勢角の時系列データ）が記憶されている。歩容データ上で足裏が接地面と接触するときは、足裏面と接地面を面接触状態とするため、目標足平姿勢角（仮想的な接地面に対する仮想的な足裏面の傾き）はゼロに設定されている。また、歩容データ上の各目標値は、ロボット１００のＺＭＰ位置が接地している脚の足裏で囲まれた凸包内となる関係を満たすように決定されている。

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

より詳細には、演算処理部６０は、倣い制御部６１と、倒立制御部６２と、距離センサ選択手段としての距離センサ選択部６３と、歩容データ修正手段としての歩容データ修正部６４と、逆キネマ演算部６５と、を備える。制御部３０内では、実胴体姿勢角と目標胴体姿勢角の偏差に基づくフィードバック制御系（倒立制御部６２による倒立制御系）と、接地面から見た相対的な実足平姿勢角と目標足平姿勢角の偏差に基づくフィードバック制御系（倣い制御部６１による倣い制御系）が含まれる。以下、倣い制御の偏差については、接地面から見た相対的な足平姿勢角に関するものを示すものとする。

倒立制御部６２は、ロボット１００の胴体位置姿勢を目標位置姿勢に維持する機能を果たす。胴体位置姿勢は、実際の胴体位置と胴体姿勢角であり、目標位置姿勢は、目標の胴体位置と胴体姿勢角である。実胴体姿勢角は、胴体１０に備えられた姿勢角センサ１４で検出される。姿勢角センサ１４は、例えば胴体１０の角速度を検出するジャイロと、ジャイロの出力（角速度）を積分する積分器と、重力加速度ベクトルを検出する３軸加速度センサで構成される。転倒方向検出部（不図示）は、姿勢角センサ１４の検出結果に基づいてロボット１００の転倒方向を検出することができる。

倣い制御部６１は、実足平姿勢角を目標足平姿勢角に一致させるように、例えば足裏が路面に密着している状態を目標とし、踵側が接地面から浮いている場合には、足平リンク２６の爪先側を脛リンク２４に近づける方向に足平リンク２６を回転させる。即ち、足裏面と接地面との面接触を維持するように足平リンク２６を回転させる。実足平姿勢角は、足平リンク２６が備える距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄの出力から求められる。ここで、距離センサ１６は、後述する距離センサ選択部６３により選択された距離センサを使用するものとする。距離検出部（不図示）は、距離センサ１６の検出結果に基づいて、足裏と路面との距離を検出する。

距離センサ選択部６３は、転倒方向検出部としての姿勢角センサ１４、及び距離検出部としての距離センサ１６の検出結果に基づいて、足裏と路面との距離を追従させるべき距離センサ１６を選択する。距離センサ選択部６３による距離センサ選択方法の詳細については後述する。

歩容データ修正部６４は、距離センサ選択部６３により選択された距離センサからの出力信号に基づいて、足裏と路面との位置関係が目標値に追従するように歩容データを修正する。まず、歩容データ修正部６４は、選択された距離センサ１６の出力値と追従すべき目標値との足偏差を計算する。そして、歩容データ修正部６４は、計算された足偏差から、偏差に関する伝達関数を用いて、補正量を算出する。

より詳細には、以下のようにして記憶部５０に記憶された歩容データ（目標胴体位置、目標胴体姿勢角、目標足平位置、及び目標足平姿勢角の時系列データ）が修正され、逆キネマティクス演算部６５に入力される。

記憶部５０に記憶された目標足平位置は、目標足平位置と実足平位置の偏差に基づいて補正された後に逆キネマティクス演算部６５に入力される。

記憶部５０に記憶された目標胴体位置は、目標胴体加速度と実胴体加速度の偏差に基づいて補正された後に逆キネマティクス演算部６５に入力される。目標胴体加速度は、目標胴体位置を２回微分することによって求められる。実胴体加速度は、加速度センサ１２により検出される。

記憶部５０に記憶された目標胴体姿勢角は、そのまま逆キネマティクス演算部６５に入力される。同時に、目標胴体姿勢角と実胴体姿勢角の偏差（胴体姿勢角偏差）が求められる。胴体姿勢角偏差は倒立制御部６２に入力されて、胴体姿勢角偏差を小さくする方向に胴体を回転させる胴体補正角が算出される。実胴体姿勢角は、姿勢角センサ１４により検出される。

記憶部５０に記憶された目標足平姿勢角と実足平姿勢角の偏差（足平姿勢角偏差）が求められる。実足平姿勢角は、距離センサ選択部６３により選択された距離センサ１６により検出される。足平姿勢角偏差と上述した胴体補正角とが加算され、加算された結果が、倣い制御部６１に入力される。倣い制御部６１によって、入力された角度（胴体補正角と足平姿勢角偏差を加算した角度）を小さくする方向へ足平を回転させる足平補正角が求められる。記憶部５０に記憶された目標足平姿勢角は、上述した足平補正角が加算された後に（足平補正角で補正された後に）逆キネマティクス演算部６５に入力される。

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

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

続いて、図４を参照しながら、本実施の形態１に係るロボット１００が行う制御処理について説明する。図４は、制御処理の概要を説明するためのフローチャートである。図４に示す制御処理に際しては、ロボット１００は、不整地において、その足平リンク２６の一部が障害物Ｂ上に接触しているものとする。図５は、ロボット１００の足平リンク２６の一部が障害物Ｂ上に接触した状態を説明するための図である。図５は、胴体１０と脚部２０を有する脚式ロボット１００の模式的側面図である。尚、図５では、ロボット１００の胴体１０と脚部２０のみを示しており、他方の脚部は図示を省略してある。足平リンク２６には、４つの距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄが設けられている。図５においては、図に示すｘ軸、ｙ軸、ｚ軸の３軸からなる絶対座標系を用いて説明する。

図５においてロボット１００に実現させたい全体位置姿勢を破線で示す。ロボット１００は破線で示す目標位置姿勢とするため、矢印Ｃの方向へとその位置姿勢を制御しようとする。まず、転倒方向検出部は姿勢角センサ１４の検出結果に基づいて、ロボット１００の転倒方向を検出する（ステップＳ１０１）。転倒方向検出部は、胴体１０の姿勢偏差又は胴体１０の角速度などから転倒方向を検出することができる。例えば、胴体姿勢偏差（＝目標姿勢から見た実姿勢）を計算することで、その胴体姿勢偏差を減らす方向の床反力モーメントを発生させるため、足平リンク２６をどちらの方向に倣わせればよいのかを決定することができる。

例えば、図６（ａ）に示すようにロボット１００の実姿勢が目標姿勢に対して傾いている状態であるものとする。ここで、支持脚から見た目標姿勢に対する実姿勢の姿勢偏差ベクトル（ロール、ピッチ）を（^legΔ_roll，^legΔ_pitch）とする。そして、図６（ｂ）に示すように、姿勢偏差ベクトル（^legΔ_roll，^legΔ_pitch）を用いて、次式を計算することで、姿勢偏差角θ_Δを求める。姿勢偏差角θ_Δを用いると、転倒方向をθ_Δ−π／２として表すことができる。図６に示す例では、姿勢偏差より、胴体１０が左後方（矢印Ｄ）へと傾いていることを検知することができる。このように、ロボット１００は、胴体姿勢偏差から転倒方向を検出することができる。

次いでロボット１００は、ステップＳ１０１において検出した転倒方向に基づいて、センサを２個選択する（ステップＳ１０２）。図５に示すように、距離センサ１６ｄの位置に障害物Ｂ（凸部）が存在する場合を想定する。この場合に、転倒方向に基づいて距離センサ１６ａ、１６ｂ、１６ｃを選択することとしては、凸部Ｂが障害となり距離センサ１６ａ及び１６ｃの出力値を同時に０とすることができないため、足裏を路面に倣わせることができない。このような場合には、胴体１０の傾きを戻すことが可能な床反力モーメントを少なくとも発生させるべく、まず、転倒方向に基づいて、２個のセンサ１６ｂ、１６ｃを選択するものとする。

ここで、図７を用いて、転倒方向に基づいて、２個の距離センサを選択する方法について詳細に説明する。まず、距離センサ１６を選択するための定数Θ_１〜Θ_４を、足裏と距離センサ１６との位置関係から、以下の式に基づいて予め計算しておく。ここで、Ｘ_ｔは足平リンク２６に設けられた距離センサ１６ｄと１６ｃの間の距離（距離センサ１６ａと１６ｂの間の距離）であり、Ｙ_ｔは距離センサ１６ｂと１６ｃの間の距離（距離センサ１６ａと１６ｄの間の距離）である。尚、定数Θ_１〜Θ_４は、ａｔａｎ２関数を用いて、−πからπまでの範囲において求める。

上記数２において計算された定数Θ_１〜Θ_４と、上記数１において計算された姿勢偏差角θ_Δを用いて、以下のようにして２個の距離センサを選択する。
Θ_４≦θ_Δ−π／２≦Θ_１（即ち、Θ_４＋π／２≦θ_Δ≦Θ_１＋π／２）の場合には、距離センサ１６ａ及び１６ｄの２個の距離センサを選択して有効とする。尚、選択されなかった距離センサは１６ｂ及び１６ｃであり、後述するように、残り１個の距離センサをこれら残りの距離センサのうちから選択する。
Θ₃≦θ_Δ−π／２≦Θ₄（即ち、Θ₃＋π／２≦θ_Δ≦Θ₄＋π／２）の場合には、距離センサ１６ｃ及び１６ｄの２個の距離センサを選択して有効とする。尚、選択されなかった距離センサは１６ａ及び１６ｂであり、後述するように、残り１個の距離センサをこれら残りの距離センサのうちから選択する。
Θ₂≦θ_Δ−π／２≦Θ₃（即ち、Θ₂＋π／２≦θ_Δ≦Θ₃＋π／２）の場合には、距離センサ１６ｂ及び１６ｃの２個の距離センサを選択して有効とする。尚、選択されなかった距離センサは１６ａ及び１６ｄであり、後述するように、残り１個の距離センサをこれら残りの距離センサのうちから選択する。
Θ₁≦θ_Δ−π／２≦π／２又は−３π／２≦θ_Δ−π／２≦Θ₂（即ち、Θ₁＋π／２≦θ_Δ≦π又は−π≦θ_Δ≦Θ₂＋π／２）の場合には、距離センサ１６ａ及び１６ｂの２個の距離センサを選択して有効とする。尚、選択されなかった距離センサは１６ｃ及び１６ｄであり、残り１個をこれら残りのセンサから選択する。

図７において、例えば姿勢偏差ベクトルＶから姿勢偏差角θ_Δを用いて、転倒方向Ｄを検出する。図においては、姿勢偏差角θ_Δは、Θ₂≦θ_Δ−π／２≦Θ₃（即ち、Θ₂＋π／２≦θ_Δ≦Θ₃＋π／２）を満足するため、距離センサ１６ｂ及び１６ｃの２個の距離センサを選択して有効とする。このようにして距離センサを選択することで、転倒方向Ｄに基づいて、胴体１０の傾きを戻すための床反力モーメントを効果的に得ることが可能な距離センサ１６を２個選択することができる。尚、実装に際しては、ヒステリシス特性を持たせるように構成するとよい。これにより、姿勢偏差ベクトルがゼロ付近となる場合に発生するチャタリングを防止することができる。

次いでロボット１００は、ステップＳ１０２において選択されなかった残りの２個の距離センサ１６のうち、足裏と路面との距離が小さな距離センサ１６を１個選択する（ステップＳ１０３）。ステップＳ１０２において、４個の距離センサ１６から２個の距離センサを選択した。本ステップＳ１０３においては、距離センサ１６の出力値である計測高さに基づいて、ステップＳ１０２において選択されなかった残りの２個の距離センサ１６から１個の距離センサを選択する。計測高さが小さな距離センサ１６は、凸部側に位置する距離センサであるものと考えられる。このため、計測高さが小さな距離センサ１６を選択して有効とすることで、路面の凹凸に関わらず、足平リンク２６を路面に倣わせるように安定して制御することができる。尚、実装に際しては、ステップＳ１０２における距離センサ１６の選択と同様にして、ヒステリシス特性を持たせるように構成するとよい。

次いでロボット１００は、ステップＳ１０２及びＳ１０３において選択された３個の距離センサ１６から、足裏と路面との偏差である足偏差（ロール、ピッチ、ｚ）を計算する（ステップＳ１０４）。ここで、足平リンク２６の目標位置姿勢に対する実際の位置姿勢の足偏差（ロール、ピッチ、ｚ）を（Δφ，Δθ，Δｚ）とする。尚、ｚは鉛直方向の計測高さを示す。例えば、３個の距離センサ１６ａ、１６ｂ、１６ｃが選択された場合には、次式で示す変換式を用いることで、足偏差を計算することができる。次式において示す変換行列によれば、足裏と距離センサ１６が配置される位置の幾何学的関係から近似を用いて、各距離センサ１６ａ乃至１６ｄの出力値の偏差（Δｚ_１，Δｚ_２，Δｚ_３，Δｚ_４）と、各距離センサ１６の偏差に対応する３行４列の変換行列とから、足偏差を一意に決定することができる。尚、選択された距離センサ１６に応じて、変換行列における各要素の値は異なるものとする。

次いでロボット１００は、ステップＳ１０４において計算された足偏差を用いて、実際に歩容データを補正するための足補正量を計算する（ステップＳ１０５）。足補正量の計算は、例えば、足偏差を入力とし、足補正量を出力とする伝達関数を通すことで実現することができる。図８は、足偏差を入力とし、足補正量を出力とする伝達関数を説明するための図である。図に示すように、足補正量（ロール）に関しては、ロール偏差Δφを伝達関数Ｃ_φ（ｓ）に通す。足補正量（ピッチ）に関しては、ピッチ偏差Δθを伝達関数Ｃ_θ（ｓ）に通す。足補正量（ｚ）に関しては、ｚ偏差Δｚを伝達関数Ｃ_ｚ（ｓ）に通す。尚、使用する距離センサを切替える場合において足偏差が不連続となっている場合には、これらの伝達関数は、広域のゲインを小さくした特性を持つ伝達関数とすることが好ましい。

次いでロボット１００は、ステップＳ１０５において計算された足補正量を実現するように歩容データを修正する（ステップＳ１０６）。そして、動作を続行する場合には、ステップＳ１０１へと戻り処理を続け、動作を終了する場合には処理を終了する。

以上説明したようにロボット１００の転倒方向を検出して、検出結果に基づいて目標値に追従させるべき３個の足裏距離センサ１６を選択することで、ロボット１００の体幹（胴体１０）を倒立させるための床反力モーメントを効果的に得つつ、足裏と路面との接触を保つことができる。従って、支持脚を床面に密着させつつ、足裏からの床反力モーメントを得ることで、ロボット１００の体幹を倒立させる安定化制御を容易に実行することができるため、不整地においてもロボット１００は転倒せずに歩行することができる。

続いて、図９乃至１１を参照しながら、本実施の形態１に係るロボット１００が歩行動作中に行う制御処理について説明する。図９は、歩行動作中に、歩行動作パターンに応じて、ロボット１００が距離センサの有効数を変更するようすを説明するための図である。図１０は、ロボット１００の脚部２０が支持脚状態にある場合の制御処理の概要を説明するためのフローチャートである。図１１は、ロボット１００の脚部２０が遊脚状態にある場合の制御処理の概要を説明するためのフローチャートである。

図９に示すように、ロボット１００は、歩行動作パターンに応じて、足平リンク２６に配置された距離センサのうち、有効とする距離センサの個数を変更する。ロボット１００は、脚部２０が支持脚の歩行動作パターンであるか、もしくは、遊脚の歩行動作パターンであるかを判定することができる。さらに、ロボット１００は、支持脚が完全に着地したか否か、及び、遊脚が完全に離地したか否かを、例えば歩容データに基づいて検知するようにしてもよいし、距離センサ１６の出力値から検知するようにしてもよい。

脚部２０が支持脚状態にある場合には、基本的には、足平リンク２６に配置された距離センサのうち、３個の距離センサを有効とする（図９において、例えば左脚部２０Ｌが支持脚状態であり、左足距離センサ１６Ｌについてｔ_２〜ｔ_３に示す区間）。また、脚部２０が遊脚状態にある場合には、基本的には、全ての距離センサ（図では４個の距離センサ）を有効とする（図９において、例えば右脚部２０Ｒが遊脚状態であり、右足距離センサ１６Ｒについてｔ_２〜ｔ_３に示す区間）。支持脚状態から遊脚状態へと脚部２０が変化するまでの間は、最低限必要な個数の距離センサを有効とし、足裏が完全に離地して遊脚状態となった後は、全ての距離センサを有効とする（図９において、例えば左脚部２０Ｌが支持脚状態であり、左足距離センサ１６Ｌについてｔ_３〜ｔ_４に示す区間）。遊脚状態から支持脚状態へと脚部２０が変化するまでの間は、全ての距離センサを有効とし、足裏が完全に着地して支持脚状態となった後は、最低限必要な個数の距離センサ（図では３個の距離センサ）を有効とする（図９において、例えば右脚部２０Ｒが遊脚状態であり、右足距離センサ１６Ｒについてｔ_３〜ｔ_４に示す区間）。ここで、時点ｔ_２及びｔ_６は、支持脚であった右脚部２０Ｒが完全に離地すると共に、遊脚であった左脚部２０Ｌが完全に着地する時点を表す。時点ｔ_４及びｔ_８は、遊脚であった右脚部２０Ｒが完全に着地すると共に、支持脚であった左脚部２０Ｌが完全に離地する時点を表す。

図１０に示すフローチャートを用いて、ロボット１００の脚部２０が支持脚状態にある場合の制御処理を説明する。まず、ロボット１００が歩行動作中に、いずれかの脚部２０が遊脚状態から支持脚状態へと遷移する過程において、脚部２０が完全に着地したか否かを判定する（ステップＳ２０１）。判定の結果、脚部２０が完全に着地している場合（脚部２０が支持脚状態となった場合）には、上述したように、３個の距離センサ１６を選択する。即ち、ロボット１００の転倒方向を検出する（ステップＳ２０２）。次いで、転倒方向に基づいて、２個の距離センサ１６を選択する（ステップＳ２０３）。次いで、残りの距離センサ１６から、足裏と路面との距離に基づいて１個の距離センサ１６を選択する（ステップＳ２０４）。次いで、選択された３個の距離センサ１６から、路面との足偏差を計算する（ステップＳ２０５）。

一方、ステップＳ２０１における判定の結果、脚部２０が完全に着地していない場合（脚部２０が未だ支持脚状態となっていない場合）には、全ての距離センサ１６を有効とし、全ての距離センサ１６から、路面との足偏差を計算する（ステップＳ２０６）。例えば、４個の距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄ全てを有効とする場合には、次式で示す変換式を用いることで、足偏差を計算することができる。

次いで、ステップＳ２０５又はＳ２０６において計算された足偏差を用いて、実際に歩容データを補正するための足補正量を計算する（ステップＳ２０７）。次いで、ロボット１００は、ステップＳ２０７において計算された足補正量を実現するように歩容データを修正する（ステップＳ２０８）。そして、動作を続行する場合には、ステップＳ２０１へと戻り処理を続け、動作を終了する場合には処理を終了する。

このように、足平リンク２６が完全に路面に着地するまでの間は全てのセンサを有効とすることで、路面に対して速やかに倣わせることができる。このため、着地時の外乱をより効果的に抑制することができる。

続いて、図１１に示すフローチャートを用いて、ロボット１００の脚部２０が遊脚状態にある場合の制御処理を説明する。まず、ロボット１００が歩行動作中に、いずれかの脚部２０が支持脚状態から遊脚状態へと遷移する過程において、脚部２０が完全に離地したか否かを判定する（ステップＳ３０１）。判定の結果、脚部２０が完全に離地した場合（脚部２０が遊脚状態となった場合）には、全ての距離センサ１６を有効とし、上述したように、全ての距離センサ１６から、路面との足偏差を計算する（ステップＳ３０２）。

一方、ステップＳ３０１における判定の結果、脚部２０が完全に離地していない場合（脚部２０が未だ遊脚状態となっていない場合）には、上述したように、３個の距離センサ１６を選択する。即ち、ロボット１００の転倒方向を検出する（ステップＳ３０３）。次いで、転倒方向に基づいて、２個の距離センサ１６を選択する（ステップＳ３０４）。次いで、残りの距離センサ１６から、足裏と路面との距離に基づいて１個の距離センサ１６を選択する（ステップＳ３０５）。次いで、選択された３個の距離センサ１６から、路面との足偏差を計算する（ステップＳ３０６）。

次いで、ステップＳ３０２又はＳ３０６において計算された足偏差を用いて、実際に歩容データを補正するための足補正量を計算する（ステップＳ３０７）。次いで、ロボット１００は、ステップＳ３０７において計算された足補正量を実現するように歩容データを修正する（ステップＳ３０８）。そして、動作を続行する場合には、ステップＳ３０１へと戻り処理を続け、そうでない場合には処理を終了する。

このように、遊脚が完全に離地するまでの間は足裏と路面とが接触しているため、３個の距離センサ１６を選択して有効とすることで、支持脚の補正量を保持する。また、遊脚が完全に離地した後には全ての距離センサを選択して有効とすることで、遊脚が路面と接触した場合であっても路面を回避する方向へと速やかに倣うことができる。

発明の実施の形態２．
上述した実施の形態１においては、足平リンク２６に設けられた距離センサ１６が４個である場合について、それら距離センサ１６のうち、転倒方向に基づいて、３個の距離センサを選択する処理について説明した。本発明はこれに限定されず、足平リンク２６に設けられた少なくとも３個以上の距離センサから、転倒方向に基づいて、３個の距離センサを選択するようにしてもよい。尚、本実施の形態２に係るロボット１００の構成は、足平リンク２６の構成、及び足平リンク２６に設けられる距離センサ１６の配置を除いて、上述した発明の実施の形態１と同様である。

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

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

本実施の形態２に係るロボット１００は、爪先関節２７を駆動させて爪先部２６ａ及び踵部２６ｂを路面に倣わせる際に、ロボット１００の転倒方向に基づいて、爪先部２６ａに配置された距離センサ１６から３個の距離センサ１６を選択すると共に、踵部２６ｂと路面との距離に基づいて、踵部２６ｂに配置された距離センサ１６から路面との距離を追従させるべき１個の距離センサ１６を選択する。

続いて、図１３乃至１７を参照しながら、本実施の形態２に係るロボット１００が歩行動作中に行う制御処理について説明する。図１３は、歩行動作中に、歩行動作パターンに応じて、ロボット１００が距離センサの有効数を変更するようすを説明するための図である。図１４は、ロボット１００の脚部２０が支持脚状態にある場合の制御処理の概要を説明するためのフローチャートである。図１５は、ロボット１００の足平リンク２６の爪先部２６ａの一部が障害物Ｂ上に接触した状態を説明するための図である。図１６は、ロボット１００の脚部２０が遊脚状態にある場合の制御処理の概要を説明するためのフローチャートである。

図１３に示すように、ロボット１００は、歩行動作パターンに応じて、足平リンク２６に配置された距離センサのうち、有効とする距離センサの個数を変更する。ロボット１００は、脚部２０が支持脚の歩行動作パターンであるか、もしくは、遊脚の歩行動作パターンであるかを判定することができる。さらに、ロボット１００は、支持脚が完全に着地したか否か、及び、遊脚が完全に離地したか否かを、例えば歩容データに基づいて検知するようにしてもよいし、距離センサ１６の出力値から検知するようにしてもよい。

脚部２０が支持脚状態にある場合には、基本的には、爪先部２６ａに配置された距離センサのうち、３個の距離センサを有効とする（図１３において、例えば左脚部２０Ｌが支持脚状態であり、左足距離センサ１６Ｌについてｔ_３〜ｔ_５に示す区間）。但し、爪先部２６ａに加えて、踵部２６ｂも路面に接触している状態においては、爪先部２６ａに配置された距離センサ１６から３個の距離センサ１６を選択すると共に、踵部２６ｂに配置された距離センサ１６から１個の距離センサ１６を選択し、合計４個の距離センサ１６を有効とする（図１３において、左足距離センサ１６Ｌについて、例えばｔ_２〜ｔ_３に示す区間）。尚、足平リンク２６全体が路面に完全に接地して、脚部２０が支持脚状態となるまでの間は、全ての距離センサ１６（図では６個の距離センサ）を有効とする（図１３において、左足距離センサ１６Ｌについて、例えばｔ_１〜ｔ_２に示す区間）。

脚部２０が遊脚状態にある場合には、基本的には、全ての距離センサ１６（図では６個の距離センサ）を有効とする（図１３において、例えば右脚部２０Ｒが遊脚状態であり、右足距離センサ１６Ｒについてｔ_２〜ｔ_５に示す区間）。但し、爪先部２６ａ及び踵部２６ｂの両方が路面に接触している状態においては、爪先部２６ａに配置された距離センサ１６から３個の距離センサ１６を選択すると共に、踵部２６ｂに配置された距離センサ１６から１個の距離センサ１６を選択し、合計４個の距離センサ１６を有効とする（図１３において、右足距離センサ１６Ｒについて、例えばｔ_５〜ｔ_６に示す区間）。尚、爪先部２６ａが路面に接触しながら、踵部２６ｂが浮いている状態においては、爪先部２６ａに配置された距離センサのうち、３個の距離センサを有効とする（図１３において、右足距離センサ１６Ｒについて、例えばｔ_６〜ｔ_８に示す区間）。

このように、遊脚状態から支持脚状態へと脚部２０が変化するまでの間は、始めは全ての距離センサを有効とするものとして、有効とする距離センサの個数を段階的に減少させ、足裏が完全に着地して支持脚状態となった後は、最低限必要な個数の距離センサ（図では３個の距離センサ）を有効とする（図１３において、例えば右脚部２０Ｒが遊脚状態から路面に完全に着地して支持脚状態となるまでの、右足距離センサ１６Ｒについてｔ_２〜ｔ_８に示す区間）。

尚、時点ｔ_２及びｔ_８は、支持脚であった右脚部２０Ｒが完全に離地すると共に、遊脚であった左脚部２０Ｌが完全に着地する時点を表す。時点ｔ_５は、遊脚であった右脚部２０Ｒが完全に着地すると共に、支持脚であった左脚部２０Ｌが完全に離地する時点を表す。時点ｔ_３及びｔ_５は、支持脚であった脚部２０の踵部２０ｂが完全に離地する時点を表す。

図１４に示すフローチャートを用いて、ロボット１００の脚部２０が支持脚状態にある場合の制御処理を説明する。図１４に示す制御処理に際しては、ロボット１００は、不整地において、その足平リンク２６の一部が障害物Ｂ上に接触しているものとする。図１５は、ロボット１００の爪先部２６ａの一部が障害物Ｂ上に接触した状態を説明するための図である。図１５においてロボット１００に実現させたい全体位置姿勢を破線で示す。ロボット１００は破線で示す目標位置姿勢とするため、矢印Ｃの方向へとその位置姿勢を制御しようとする。

まず、ロボット１００が歩行動作中に、いずれかの脚部２０が遊脚状態から支持脚状態へと遷移する過程において、脚部２０が完全に着地したか否かを判定する（ステップＳ４０１）。判定の結果、脚部２０が完全に着地している場合（脚部２０が支持脚状態となった場合）には、爪先部２６ａに配置された４個の距離センサ１６から、上述したように、３個の距離センサ１６を選択する。即ち、ロボット１００の転倒方向を検出する（ステップＳ４０２）。次いで、転倒方向に基づいて、爪先部２６ａに配置された４個の距離センサ１６から、２個の距離センサ１６を選択する（ステップＳ４０３）。次いで、爪先部２６ａに配置された残りの距離センサ１６から、足裏と路面との距離に基づいて１個の距離センサ１６を選択する（ステップＳ４０４）。尚、転倒方向に基づいて、爪先部２６ａに配置された４個の距離センサ１６から３個の距離センサ１６を選択する方法については、実施の形態１において足平リンク２６に配置された４個の距離センサ１６から３個の距離センサ１６を選択する方法と同様であるため、ここでは説明を省略する。

次いで、踵部２６ｂが路面と接触しておらず浮いた状態であるか否かを判定する（ステップＳ４０５）。判定の結果、踵部２６ｂが路面と接触しておらず浮いた状態である場合（爪先部２６ａのみが路面と接触しており、つま先立ちしている状態）には、ステップＳ４０３及びＳ４０４において選択された３個の距離センサ１６から、路面との足偏差を計算する（ステップＳ４０６）。ここで、爪先部２６ａの目標位置姿勢に対する実際の位置姿勢の足偏差（ロール、ピッチ、ｚ）を（Δφ，Δθ，Δｚ）とする。尚、ｚは鉛直方向の計測高さを示す。例えば、３個の距離センサ１６ａ、１６ｂ、１６ｃが選択された場合には、次式で示す変換式を用いることで、足偏差を計算することができる。次式において示す変換行列によれば、足裏と距離センサ１６が配置される位置の幾何学的関係から近似を用いて、各距離センサ１６の出力値の偏差（Δｚ_１，Δｚ_２，Δｚ_３，Δｚ_４）と、各距離センサ１６の偏差に対応する３行４列の変換行列とから、足偏差を一意に決定することができる。尚、選択された距離センサに応じて、変換行列における各要素の値は異なるものとする。

一方、判定の結果、踵部２６ｂが路面と接触しており着地した状態である場合（爪先部２６ａ及び踵部２６ｂが路面と接触している状態）には、まず、踵部２６ｂに配置された２個の距離センサ１６から、足裏と路面との距離に基づいて１個の距離センサ１６を選択する（ステップＳ４０７）。即ち、踵部２６ｂに設けられた２個の距離センサ１６ｅ及び１６ｆから、計測高さが小さな距離センサ１６を選択する。

次いで、ステップＳ４０３、Ｓ４０４、及びＳ４０７において選択された４個の距離センサ１６から、路面との足偏差を計算する（ステップＳ４０８）。ここで、爪先部２６ａの目標位置姿勢に対する実際の位置姿勢の足偏差（ロール、ピッチ、ｚ、つま先）を（Δφ，Δθ，Δｚ，Δθ_ｔｏｅ）とする。尚、Δθ_ｔｏｅは爪先関節２７に関して爪先部２６ａの回転角偏差を示す。例えば、爪先部２６ａから３個の距離センサ１６ａ、１６ｂ、１６ｃが選択され、踵部２６ｂから１個の距離センサ１６ｅが選択された場合には、次式で示す変換式を用いることで、足偏差を計算することができる。次式において示す変換行列によれば、足裏と距離センサ１６が配置される位置の幾何学的関係から近似を用いて、各距離センサ１６ａ乃至１６ｆにそれぞれ対応する出力値の偏差（Δｚ_１，Δｚ_２，Δｚ_３，Δｚ_４，Δｚ_５，Δｚ_６）と、各距離センサ１６の偏差に対応する４行６列の変換行列とから、足偏差を一意に決定することができる。尚、選択された距離センサに応じて、変換行列における各要素の値は異なるものとする。

一方、ステップＳ４０１における判定の結果、脚部２０が完全に着地していない場合（脚部２０が未だ支持脚状態となっていない場合）には、爪先部２６ａ及び踵部２６ｂに配置された全ての距離センサ１６を有効とし、全ての距離センサ１６から、路面との足偏差を計算する（ステップＳ４０９）。例えば、６個の距離センサ１６ａ、１６ｂ、１６ｃ、１６ｄ、１６ｅ、１６ｆの全てを有効とする場合には、次式で示す変換式を用いることで、足偏差を計算することができる。

次いで、ステップＳ４０６、Ｓ４０８、Ｓ４０９のいずれかにおいて計算された足偏差を用いて、実際に歩容データを補正するための足補正量を計算する（ステップＳ４１０）。足補正量の計算は、例えば、足偏差を入力とし、足補正量を出力とする伝達関数を通すことで実現することができる。図１６は、足偏差を入力とし、足補正量を出力とする伝達関数を説明するための図である。図に示すように、足補正量（ロール）に関しては、ロール偏差Δφを伝達関数Ｃ_φ（ｓ）に通す。足補正量（ピッチ）に関して、ロール偏差Δθを伝達関数Ｃ_θ（ｓ）に通す。足補正量（ｚ）に関して、ロール偏差Δｚを伝達関数Ｃ_ｚ（ｓ）に通す。足補正量（つま先）に関して、つま先偏差Δθ_ｔｏｅを伝達関数Ｃ_θｔｏｅ（ｓ）に通す。尚、使用する距離センサを切替える場合において足偏差が不連続となっている場合には、これらの伝達関数は、広域のゲインを小さくした特性を持つ伝達関数とすることが好ましい。

次いで、ロボット１００は、ステップＳ４１０において計算された足補正量を実現するように歩容データを修正する（ステップＳ４１１）。そして、動作を続行する場合には、ステップＳ４０１へと戻り処理を続け、動作を終了する場合には処理を終了する。

脚部２０が支持脚状態にある場合には、基本的には、爪先部２６ａに配置された距離センサのうち、３個の距離センサを有効とする。爪先部２６ａに加えて、踵部２６ｂも路面に接触している状態においては、爪先部２６ａに配置された距離センサ１６から３個の距離センサ１６を選択すると共に、踵部２６ｂに配置された距離センサ１６から１個の距離センサ１６を選択し、合計４個の距離センサ１６を有効とする。足平リンク２６全体が路面に完全に接地して、脚部２０が支持脚状態となるまでの間は、全ての距離センサ１６（図では６個の距離センサ）を有効とする。完全に路面に着地するまでの間は全てのセンサを有効とすることで、路面に対して速やかに倣わせることができる。このため、着地時の外乱をより効果的に抑制することができる。

続いて、図１７に示すフローチャートを用いて、ロボット１００の脚部２０が遊脚状態にある場合の制御処理を説明する。まず、ロボット１００が歩行動作中に、いずれかの脚部２０が支持脚状態から遊脚状態へと遷移する過程において、脚部２０が完全に離地したか否かを判定する（ステップＳ５０１）。判定の結果、脚部２０が完全に離地した場合（脚部２０が遊脚状態となった場合）には、全ての距離センサ１６を有効とし、上述したようにして、全ての距離センサ１６から、路面との足偏差を計算する（ステップＳ５０２）。

一方、ステップＳ５０１における判定の結果、脚部２０が完全に離地していない場合（脚部２０が未だ遊脚状態となっていない場合）には、爪先部２６ａに配置された４個の距離センサ１６から、上述したようにして、３個の距離センサ１６を選択する。即ち、ロボット１００の転倒方向を検出する（ステップＳ５０３）。次いで、転倒方向に基づいて、爪先部２６ａに配置された４個の距離センサ１６から、２個の距離センサ１６を選択する（ステップＳ５０４）。次いで、爪先部２６ａに配置された残りの距離センサ１６から、足裏と路面との距離に基づいて１個の距離センサ１６を選択する（ステップＳ５０５）。尚、転倒方向に基づいて、爪先部２６ａに配置された４個の距離センサ１６から３個の距離センサ１６を選択する方法については、実施の形態１において足平リンク２６に配置された４個の距離センサ１６から３個の距離センサ１６を選択する方法と同様であるため、ここでは説明を省略する。

次いで、踵部２６ｂが路面と接触しておらず浮いた状態であるか否かを判定する（ステップＳ５０６）。判定の結果、踵部２６ｂが路面と接触しておらず浮いた状態である場合（爪先部２６ａのみが路面と接触しており、つま先立ちしている状態）には、ステップＳ５０４及びＳ５０５において選択された３個の距離センサ１６から、路面との足偏差を計算する（ステップＳ５０７）。

一方、判定の結果、踵部２６ｂが路面と接触しており着地した状態である場合（爪先部２６ａ及び踵部２６ｂが路面と接触している状態）には、まず、踵部２６ｂに配置された２個の距離センサ１６から、足裏と路面との距離に基づいて１個の距離センサ１６を選択する（ステップＳ５０８）。即ち、踵部２６ｂに設けられた２個の距離センサ１６ｅ及び１６ｆから、計測高さが小さな距離センサ１６を選択する。次いで、ステップＳ５０４、Ｓ５０５、及びＳ５０８において選択された４個の距離センサ１６から、路面との足偏差を計算する（ステップＳ５０９）。

次いで、ステップＳ５０２、Ｓ５０７、Ｓ５０９のいずれかにおいて計算された足偏差を用いて、実際に歩容データを補正するための足補正量を計算する（ステップＳ５１０）。次いで、ロボット１００は、ステップＳ５１０において計算された足補正量を実現するように歩容データを修正する（ステップＳ５１１）。そして、動作を続行する場合には、ステップＳ５０１へと戻り処理を続け、そうでない場合には処理を終了する。

このように、遊脚が完全に離地するまでの間は足裏と路面とが接触しているため、３個の距離センサを選択して有効とすることで、支持脚の補正量を保持する。また、遊脚が完全に離地した後には全ての距離センサを選択して有効とすることで、遊脚が路面と接触した場合であっても路面を回避する方向へと速やかに倣うことができる。

以上説明したようにロボット１００の転倒方向を検出して、検出結果に基づいて目標値に追従させるべき足裏距離センサ１６を爪先部２６ａから３個選択すると共に、踵部２６ｂから１個選択することで、ロボット１００の体幹（胴体１０）を倒立させるための床反力モーメントを効果的に得つつ、足裏と路面との接触を保つことができる。従って、支持脚を床面に密着させつつ、足裏からの床反力モーメントを受けてロボット１００の体幹を倒立させる安定化制御を容易に実行することができるため、不整地においてもロボット１００は転倒せずに歩行することができる。

その他の実施の形態．
上述した実施の形態においては、ロボット１００は２本の脚を備えるものとしたが本発明はこれに限定されない。少なくとも２本以上の脚を有し、それぞれの脚の下端には足部が設けられ、足部の足裏には少なくとも３個以上の距離センサを備える脚式ロボットに対しても、本発明を適用することができる。

尚、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明の実施の形態１に係る脚式ロボットの構成図である。 本発明の実施の形態１に係る脚式ロボットの足平リンクの構成図である。 本発明の実施の形態１に係る脚式ロボットの構成を示す機能ブロック図である。 本発明の実施の形態１に係る脚式ロボットの制御部が行う処理を示すフローチャートである。 本発明の実施の形態１に係る脚式ロボットの足平リンクの一部が障害物上に接触した状態を説明するための図である。 本発明の実施の形態１に係る脚式ロボットの転倒方向を検出する処理を説明するための図である。 本発明の実施の形態１に係る転倒方向に基づいて距離センサを選択する処理を説明するための図である。 本発明の実施の形態１に係る伝達関数を説明するための図である。 本発明の実施の形態１に係る脚式ロボットの制御部が、歩行動作パターンに応じて、距離センサの有効数を変更するようすを説明するための図である。 本発明の実施の形態１に係る脚式ロボットの脚部が支持脚状態にある場合の制御処理の概要を説明するためのフローチャートである。 本発明の実施の形態１に係る脚式ロボットの脚部が遊脚状態にある場合の制御処理の概要を説明するためのフローチャートである。 本発明の実施の形態２に係る脚式ロボットの足平リンクの構成図である。 本発明の実施の形態２に係る脚式ロボットの制御部が、歩行動作パターンに応じて、距離センサの有効数を変更するようすを説明するための図である。 本発明の実施の形態２に係る脚式ロボットの脚部が支持脚状態にある場合の制御処理の概要を説明するためのフローチャートである。 本発明の実施の形態２に係る脚式ロボットの足平リンクの一部が障害物上に接触した状態を説明するための図である。 本発明の実施の形態２に係る伝達関数を説明するための図である。 本発明の実施の形態２に係る脚式ロボットの脚部が遊脚状態にある場合の制御処理の概要を説明するためのフローチャートである。 脚式ロボットの足平リンクの一部が障害物上に接触した状態を説明するための図である。 脚式ロボットの足平リンクの一部が障害物上に接触した状態を説明するための図である。

符号の説明

１０ 胴体、
１２ 加速度センサ、１４ 姿勢角センサ、１６ 距離センサ
２０ 脚部、２１ 股関節、２２ 大腿リンク、２３ 膝関節、２４ 脛リンク、
２５ 足首関節、２６ 足平リンク、２６ａ 爪先部、２６ｂ 踵部、２７ 爪先関節、
３０ 制御部、５０ 記憶部、
６０ 演算処理部、６１ 倣い制御部、６２ 倒立制御部、６３ 距離センサ選択部、
６４ 歩容データ修正部、６５ 逆キネマ演算部、
７０ モータ駆動部、
１００ ロボット

Claims (7)

1. 胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足部と、前記胴体の転倒方向を検出する転倒方向検出部と、歩容データに基づいて前記脚部の関節を駆動制御する制御部と、前記足部の足裏と路面との距離を検出する距離検出部と、を備えた脚式ロボットであって、
   前記距離検出部は、
   前記足部の足裏に設けられた少なくとも３個の距離センサから構成され、
   前記制御部が、
   前記距離センサを選択する距離センサ選択手段と、
   前記距離センサ選択手段により選択された距離センサの検出信号に基づいて、前記歩容データを修正する歩容データ修正手段と、を有し、
   前記距離センサ選択手段が、
   前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、３個の距離センサを選択する
   ことを特徴とする脚式ロボット。
2. 前記距離センサ選択手段が、
   前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、第一及び第二の距離センサを選択し、選択されなかった前記距離センサのうち、前記足部の足裏と路面との距離に基づいて、第三の距離センサを選択する
   ことを特徴とする請求項１記載の脚式ロボット。
3. 前記距離センサ選択手段が、
   前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、第一及び第二の距離センサを選択し、選択されなかった前記距離センサのうち、前記足部の足裏と路面との距離が最も小さい距離センサを第三の距離センサとして選択する
   ことを特徴とする請求項１又は２記載の脚式ロボット。
4. 前記制御部が、
   前記脚部の歩行動作パターンが支持脚の歩行動作パターンであるか又は遊脚の歩行動作パターンであるかを判定し、判定結果に基づいて、前記距離センサ選択手段が選択する距離センサの個数を可変とする
   ことを特徴とする請求項１乃至３いずれか１項記載の脚式ロボット。
5. 前記脚部が支持脚である場合に、
   前記制御部が、
   前記距離検出部の出力信号に基づいて、前記脚部の足裏が路面に完全に着地しているか否かを判定し、判定の結果、前記脚部の足裏が路面に完全に着地している場合には、前記距離センサ選択手段が、前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、３個の距離センサを選択し、
   前記判定の結果、前記脚部の足裏が路面に完全に着地していない場合には、前記距離センサ選択手段が、全ての前記距離センサを選択する
   ことを特徴とする請求項１記載乃至４いずれか１項記載の脚式ロボット。
6. 前記脚部が遊脚である場合に、
   前記制御部が、
   前記距離検出部の出力信号に基づいて、前記脚部の足裏が路面に完全に離地しているか否かを判定し、判定の結果、前記脚部の足裏が路面に完全に離地していない場合には、前記距離センサ選択手段が、前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、３個の距離センサを選択し、
   前記判定の結果、前記脚部の足裏が路面に完全に離地している場合には、前記距離センサ選択手段が、全ての前記距離センサを選択する
   ことを特徴とする請求項１記載乃至５いずれか１項記載の脚式ロボット。
7. 胴体と、該胴体に連結された脚部と、該脚部の下端に設けられた足部と、前記胴体の転倒方向を検出する転倒方向検出部と、前記足部の足裏と路面との距離を検出する少なくとも３個の距離センサ、を備えた脚式ロボットの制御方法であって、
   歩容データに基づいて前記脚部の関節を駆動制御する制御ステップでは、
   前記距離センサを選択する距離センサ選択ステップと、
   選択された距離センサの検出信号に基づいて、前記歩容データを修正する歩容データ修正ステップと、を含み、
   前記距離センサ選択ステップでは、
   前記距離センサのうち、前記転倒方向検出部の検出結果に基づいて、第一及び第二の距離センサを選択し、選択されなかった前記距離センサのうち、前記足部の足裏と路面との距離に基づいて、第三の距離センサを選択する
   ことを特徴とする脚式ロボットの制御方法。

Images