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

Abstract

【課題】凹凸面であっても倒立制御と倣い制御をバランスよく実行して安定した歩行を行うことができる脚式ロボット及びその制御方法を提供する。
【解決手段】ロボット１は、目標胴体位置姿勢角と実際の胴体姿勢角との偏差を求める姿勢角偏差算出部１２０と、姿勢角偏差に基づき胴体部補正量を算出する倒立制御部１２１と、胴体部補正量で目標胴体位置を補正する胴体位置補正部１２４と、足平の裏面に設けられた距離センサ１３１の値に基づき足平の地面に対する接地度を算出する接地度算出部１２９と、接地度から足平の地面に対する接地方向を求め、この接地方向に基づき前記胴体部補正量を修正する座標変換部１２２及び補正量修正部１２３と、補正量修正部１２３が修正した胴体部補正量に基づき目標胴体位置姿勢角を補正する胴体位置姿勢角補正部１２５とを有する。
【選択図】図２

Description

本発明は、２本の脚が胴体に連結されている脚式ロボット及びその制御方法に関し、倣い制御及び倒立制御を行う脚式ロボット及びその制御方法に関する。

近年、歩行するための脚部を備え、この脚部を駆動し、脚部の下端に設けられた足平部分を所定の歩容データに基づいて床面上に配置することで歩行動作を行う脚式移動型のロボットが開発されている。

このような脚式移動型のロボットは、まず、脚部の足平部分を床面に接触させて支持脚とし、その後に足平の裏面で床面を押してロボット全体の自重を支えつつ推進させる脚部を駆動することで、次の歩行動作を行う。駆動された脚部は遊脚となる一方、他の脚部が支持脚となり、このように、遊脚と支持脚とを交互に繰り返して切り換えることで、歩行動作を行うことが可能になる。

脚式移動型のロボットは、胴体と、胴体に連結されている２本の脚部を有する。脚部は複数のリンクと複数の関節を有しており、隣接するリンク同士が関節を介して摺動可能に連結されている。関節にはアクチュエータが備えられており、関節角を制御することでロボット全体の姿勢を制御することができる。脚の先端のリンクであり、接地面と接触するリンクを足平と称する。足平の底面、即ち接地面と接触する面を足裏面と称する。

そして、胴体や足平の位置等の目標値が与えられ、胴体や足平の実際の位置等を夫々の目標値に一致させるように関節角が制御される。胴体や足平の位置等の目標値を歩容データと称する。歩容データは典型的には、目標胴体位置、目標胴体位置姿勢角、目標足平位置、及び目標足平位置姿勢角を含んでいる。歩容データは、コンピュータシュミレーションなどを利用して、ロボットが転倒しない条件を満足するように作成される。

２足ロボットでは特に、実際の胴体姿勢角（支持足と重心（胴体）の相対位置変移（主にＸＹ方向））を目標胴体位置姿勢角に追従させることが重要である。実際の胴体姿勢角と目標胴体位置姿勢角の不一致は、接地している脚の足平が予定外に傾斜していることに起因することが多いからである。接地している脚の足平が予定外に傾斜していると、ロボット全体が傾斜し、転倒する虞が生じるからである。なお、足平の予定外の傾斜は、接地面の予期しない傾斜や凹凸の存在によって生じる。従って、予期しない傾斜や凹凸が存在する接地面を転倒せずに歩行させるために、実際の胴体姿勢角を目標胴体位置姿勢角に追従させることが重要となる。

特許文献１には、ロボットの関節を目標位置に追従制御するサーボ機能を備え、ロボットに作用する外力に応じてその操作量を変えて倣い動作させる関節制御装置において、関節には先端に足底部を備えた脚式歩行ロボットの足関節を含む関節であり、足底部に作用する外力を検出して外力が減少するように制御装置のゲインを調整し、足底部を地面に倣わせつつ着地させるように構成した脚式方向ロボットの関節制御装置が開示されている。

特許２８１９３２３号公報

ところで、脚式ロボットが道の凹凸面や傾斜を歩行する時に、凹凸面でも歩行できるようにするためには、足裏を地面に倣わせる必要がある。そのため脚と体幹との相対位置関係を元の指令通りにすることが望ましい。本明細書においては、足裏を地面に倣わせる制御を倣い制御、足平から胴体への位置姿勢を目標の位置姿勢に維持する制御を倒立制御という。一般に脚式ロボットは、倒立制御と倣い制御を併用している。ここで、倒立制御を胴体の傾斜角をフィードバックして補正しようとした場合、倒立制御によって足裏が地面から?がれてしまい、不安定になることがある。これは、倒立制御による補正で働くトルクが大きすぎると足裏の地面倣い制御の効果が薄くなり、地面かがら足裏が?がれてしまうためである。一旦地面から?がれると、補正量が発散して転倒してしまう恐れがある。

図９、図１０は、倒立制御と倣い制御を併用するロボットの問題点を示す図である。図９（ａ）に示すように、ロボット２０１が凸部２０２に着地すると、図９（ｂ）に示すように、ロボット２０１には、凸部２０２により倣い制御による駆動方向と倒立制御による駆動方向が逆向きに働く。倒立制御により胴体の傾斜角を補正しようとしてモータを駆動するが、先に足裏が?がれることがある。図９（ｃ）に示すように、足裏が地面から?がれた状態で倒立制御を続けると、足裏がさらに浮き続ける。そして図１０（ａ）に示すように、ロボットが自重に任せて倒れこむような動作を行う場合があり、衝撃が非常に大きい。この場合は、図１０（ｂ）に示すように、倒れこんだ勢いでそのまま転倒してしまう。または、再度足裏が地面から離れる。そして、図１０（ｃ）に示すように、体制を立て直そうと倒立制御をすることで、さらに足裏が浮き上がるというような動作を繰り返すことになる。

本発明は、このような問題点を解決するためになされたものであり、凹凸面であっても倒立制御と倣い制御をバランスよく実行して安定した歩行を行うことができる脚式ロボット及びその制御方法を提供することを目的とする。

上述した目的を達成するために、本発明に係る脚式ロボットは、目標歩容データと実際の歩容データとの姿勢角の偏差を求める姿勢角偏差算出手段と、前記姿勢角偏差算出手段が算出した姿勢角偏差に基づき補正量を算出する倒立制御手段と、前記倒立制御手段が算出した補正量で前記目標歩容データを補正する補正手段と、足平の裏面に設けられた距離センサと、前記距離センサの値に基づき足平の地面に対する接地度を算出する接地度算出手段とを有し、前記接地度算出手段は、算出した接地度に基づき前記倒立制御手段が算出する前記補正量を修正するものである。

ここで、目標歩容データとは、目標胴体位置姿勢角又は目標足平位置姿勢角を含む。すなわち、本発明に係る脚式ロボットは、目標胴体位置姿勢角と実際の胴体姿勢角との偏差を求める姿勢角偏差算出手段と、前記姿勢角偏差算出手段が算出した姿勢角偏差に基づき胴体部補正量を算出する倒立制御手段と、前記倒立制御手段が算出した胴体部補正量で前記目標胴体位置姿勢角を補正する胴体部補正手段と、足平の裏面に設けられた距離センサと、前記距離センサの値に基づき足平の地面に対する接地度を算出する接地度算出手段とを有し、前記接地度算出手段は、算出した接地度に基づき前記倒立制御手段が算出する前記胴体部補正量を修正するものである。

または、目標足平位置姿勢角と実際の足平姿勢角との偏差を求める姿勢角偏差算出手段と、前記姿勢角偏差算出手段が算出した足平角偏差に基づき足平部補正量を算出する倒立制御手段と、前記倒立制御手段が算出した足平部補正量で前記目標足平位置姿勢角を補正する足平部補正手段と、足平の裏面に設けられた距離センサと、前記距離センサの値に基づき足平の地面に対する接地度を算出する接地度算出手段とを有し、前記接地度算出手段は、算出した接地度に基づき前記倒立制御手段が算出する前記足平部補正量を修正するものである。

本発明においては、距離センサの値に基づき足平の地面に対する接地度を各距離センサごとに算出し、この接地度に応じて倒立制御手段の補正量を修正するため、足裏の地面に対する接地状態を考慮した制御が可能となる。ここで、接地度とは、足裏が地面に接していない状態を０、十分に接している状態を１とし、その間を線形関数等で結んだ値である。

また、地面から見た相対的な目標足平位置姿勢と実際の足平位置姿勢との偏差を求める足平位置姿勢偏差算出手段と、前記足平位置姿勢偏差算出手段が算出した足平偏差に基づき脚部補正量を算出する倣い制御手段と、前記倣い制御手段が算出した脚部補正量で目標足平位置姿勢角を補正する足平部補正手段とを有し、前記接地度算出手段は、算出した接地度に基づき前記倒立制御手段及び前記倣い制御手段のゲインを調整することができる。これにより、接地度に応じて、倒立制御の補正量を大きくしたり、倣い制御の補正量を大きくしたりすることができる。

さらに、前記接地度算出手段が算出した接地度から前記足平の地面に対する接地方向を求め、この接地方向に基づき前記胴体部補正量を調整する補正量調整手段を有する。足平の接地の方向を考慮して、胴体部補正量を修正することができる。

さらにまた、地面から見た相対的な目標足平位置姿勢と実際の足平位置姿勢との偏差を求める足平位置姿勢偏差算出手段と、前記足平位置姿勢偏差算出手段が算出した足平偏差に基づき脚部補正量を算出する倣い制御手段と、前記倣い制御手段が算出した脚部補正量で目標足平位置姿勢角を補正する足平部補正手段とを有することができる。

また、目標胴体位置から目標胴体加速度を算出する目標加速度算出手段と、加速度センサと、前記加速度センサから取得した現在の加速度と前記目標加速度との偏差を求める加速度偏差算出手段と、前記加速度偏差に基づき加速度補正量を算出する加速度制御手段と、前記加速度制御手段が算出した加速度補正量により前記目標胴体位置を補正する胴体位置補正手段とを更に有することができる。

さらに、前記接地度に基づき前記胴体補正量を、足平が最もよく接地している方向を主軸とする座標系である接地座標系に変換する座標変換手段と、前記接地座標系に変換した前記胴体補正量を、接地している度合いを示す接地状態に応じて補正する補正量修正手段とを有し、前記座標変換手段は、前記補正量修正手段にて補正した胴体補正量を絶対座標系に変換し、前記胴体補正手段は、前記絶対座標系に変換された胴体補正量で前記目標胴体位置姿勢角を補正することができる。接地座標系に変換して補正量を修正するため、足裏の接地状態に応じた方向に補正量を算出することができる。

本発明にかかる脚式ロボットの制御方法は、目標胴体位置姿勢角と実際の胴体姿勢角との偏差を求める姿勢角偏差算出工程と、前記姿勢角偏差算出工程にて算出した姿勢角偏差に基づき胴体部補正量を算出する倒立制御工程と、前記倒立制御工程にて算出した胴体部補正量で前記目標胴体位置姿勢角を補正する胴体部補正工程と、足平の裏面に設けられた距離センサの値に基づき各距離センサごとの地面に対する接地度を算出する接地度算出工程とを有し、前記接地度算出工程では、算出した接地度に基づき前記倒立制御工程にて算出する前記胴体部補正量を修正する。

本発明によれば、凹凸面であっても倒立制御と倣い制御をバランスよく実行して安定した歩行を行うことができる脚式ロボット及びその制御方法を提供することができる。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、倣い制御と倒立制御を行う脚式移動型のロボットに適用したものである。
実施の形態１.

図１は、ロボット１を正面から見た様子を概略的に表す概略図であり、床面Ｆ上をロボット１が歩行する様子を表しているなお、図１においては、説明の便宜上、ロボット１が進行する向き（前後方向）をｘ軸、ロボット１が進行する方向について水平方向に直交する向き（左右方向）をｙ軸、移動体の移動する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これらの３軸からなる座標系を用いて説明する。すなわち、図１中において、前記ｘ軸は紙面の奥行方向、ｙ軸は紙面に向かって左右方向、ｚ軸は紙面中の上下方向を示す。この座標系を、絶対座標系（ワールド座標系）Ｏｇ−ＸｇＹｇＺｇに設定する。

図１に示すように、ロボット１は、頭部２と、体幹３と、体幹３に結合された腰部４と、体幹３に接続された右腕５、左腕６と、腰部４に対して回動自在に固定される脚部１０と、を備えた２足歩行型のロボットである。以下、詳細に説明する。

頭部２は、ロボット１の周囲の環境を視覚的に撮像するための左右一対の撮像部（図示せず）を備えているとともに、体幹３に対して頭部２を鉛直方向に平行な軸周りに回動させることで、周囲の環境を広く撮像する。撮像した周囲の環境を示す画像データは、制御部１３０に送信され、ロボット１の動作を決定するための情報として用いられる。

体幹３は、その内部にロボット１の動作を制御する制御部１３０や、脚部のモータ等に電力を供給するためのバッテリー（図示せず）等を収容するものである。制御部１３０は、脚部１０を駆動し、ロボット１を動かすための歩容データを記憶する記憶領域と、この記憶領域に記憶された歩容データを読み出す演算処理部と、脚部１０に含まれるモータを駆動するモータ駆動部と、を備えている。これらの各構成要素は、体幹３の内部に設けられたバッテリー（図示せず）から電力を供給されることで動作する。ここで、体幹部に対しては、胴体座標系Ｏｂ−ＸｂＹｂＺｂを設定する。後述する目標胴体位置は、絶対座標系Ｏｇ−ＸｇＹｇＺｇの原点Ｏｂの位置で表わされる。また、後述する目標胴体位置姿勢角は、絶対座標系Ｏｇ−ＸｇＹｇＺｇに対する胴体座標系Ｏｂ−ＸｂＹｂＺｂの傾きで表わされる。

また、演算処理部は、記憶領域に記憶された歩容データを読み出すとともに、読み出した歩容データによって特定されるロボット１の姿勢を実現するために必要な脚部１０の関節角を算出する。そして、このように算出した関節角に基づく信号をモータ駆動部に送信する。

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

また、演算処理部は、読み出した歩容データに基づいてモータの駆動を行うように指令するほか、ロボット１に組み込まれたジャイロや加速度計などセンサ（図示せず）からの信号を受けて、モータの駆動量を調整する。また、レーザセンサなどを設けて、床面Ｆまでの距離を検出してもよい。ジャイロセンサや加速度計やレーザセンサなどの各種センサは、例えば、体幹３や腰部４に設けられる。このように、センサにより検出したロボット１に作用する外力や、ロボット１の姿勢などに応じて脚部１０の関節角を調整することで、ロボット１が安定した状態を維持することができる。

右腕５および左腕６は、体幹３に対して回動自在に接続されており、肘部分および手首部分に設けられた関節部分を駆動することにより、人間の腕部と同様の動きを行うことができる。また、手首部分の先端に接続された手先部は、図示を省略するが物体を把持するためのハンド構造を備えており、ハンド構造に組み込まれた複数の指関節を駆動することで、様々な形状の物体を把持することが可能となる。

腰部４は、体幹３に対して回動するように接続されており、歩行動作を行う際に腰部４の回動動作を組み合わせることで、脚部１０を駆動するために必要な駆動エネルギーを低減させることができる。

２足歩行を行うための脚部１０（右脚２０、左脚３０）は、右脚２０と左脚３０とから構成されている。詳細には、図１に示すように、右脚２０は右股関節２１、右上腿２２、右膝関節２３、右下腿２４、右足首関節２５、右足平２６を備え、同様に、左脚３０は左股関節３１、左上腿３２、左膝関節３３、左下腿３４、左足首関節３５、左足平３６を備えている。

そして、右脚２０および左脚３０とは、図示しないモータからの駆動力が、同じく図示しないプーリおよびベルトを介して伝達されることで、各関節部が所望の角度に駆動され、その結果、脚部に所望の動きをさせることができる。ここで、足平に関しては、足平座標系Ｏｆ−ＸｆＹｆＺｆを設定する。後述する目標足平位置は、絶対座標系Ｏｇ−ＸｇＹｇＺｇに対する足平座標系Ｏｆ−ＸｆＹｆＺｆの原点の位置で表わされる。目標足平位置姿勢角は、接地面に対する足裏面の角度で表わされる。

歩容データは、操作部（図示せず）から送られる信号で特定される脚部１０の移動量に対応づけて、脚部１０の足平（右足平２６及び左足平３６）の先端（足先）の位置と、移動体本体の位置とを、ロボット１の移動する空間を定める座標系（例えばｘｙｚ座標系）において経時的に指示するものである。歩容データとしては、目標胴体位置、目標胴体位置姿勢角、目標足平位置、目標足平位置姿勢角などがある。歩容データは、ロボット１を実際に動作させる前に、シミュレーション等によってロボット１が安定して歩行できるよう、すなわち転倒せずに歩行できるように作成されている。作成された歩容データは、ロボット１の制御部（コントローラ）１３０に記憶される。制御部は、実際の胴体位置等を歩容データに含まれる目標胴体位置等に追従させるように各関節を制御する。

さらに、本実施の形態にかかるロボットは、右足平２６及び左足平３６に、床面Ｆまでの距離を測定する距離センサ（図示せず）が設けられている。そして、距離センサで測定された床面までの距離がセンサ信号として、制御部１３０に入力される。このセンサ信号に基づいて、制御部はフィードバック制御を行っている。例えば、足平部の足裏を床面Ｆに倣わせるように、足首関節（右足首関節２５、左足首関節３５）を駆動する。これにより、ロボットが安定して、歩行することができる。

次に、歩容データについて説明する。目標足平位置は、絶対座標系に対する足平座標系の原点Ｏｆの位置で表される。目標足平位置姿勢角は、接地面Ｆに対する足裏面の角度の経時的データで表される。目標足平位置姿勢角は、絶対座標系に対する足平座標系の傾きの経時的データで表されていてもよい。目標足平位置姿勢角は、接地面Ｆに対する足裏面の角度の経時的データで表されていても、絶対座標系に対する足平座標系の傾きで表されていても、両者は実質的に等価である。これは次の理由による。歩容データの目標足平位置姿勢角を作成するときの接地面は、シミュレーション上で設定された仮想的な接地面である。仮想的な接地面の絶対座標系における傾きは、実際の接地面の傾きに基づいて設定されていてもよいし、実際の接地面の傾きに関わらず仮の傾きに設定されてもよい。いずれの場合もシミュレーションにおける仮想的な接地面の絶対座標系に対する傾きが設定されている。従って、目標足平位置姿勢角が、接地面に対する足裏面の角度で表されていても、絶対座標系に対する足平座標系の傾きで表されていても、両者は実質的に等価となる。以下では、足平の実際の位置と姿勢角を夫々実足平位置と実足平姿勢角と称する。

実足平姿勢角は、実際の接地面と足裏面がなす角度（傾き）で表され、後述するように足平に備えられた距離センサで検出することができる。実足平姿勢角と比較しやすいように、目標足平位置姿勢角は仮想的な接地面に対する仮想的な足裏面の傾きで表されているとする。

目標胴体位置は、目標足平位置に対する胴体座標系の原点Ｏｂの相対位置の経時的データで表される。なお、目標胴体位置は、絶対座標系に対する胴体座標系の原点Ｏｂの位置で表されてもよい。両者は実質的に等価である。歩容データを作成する上では、目標胴体位置を絶対座標系に対する位置で直接に表現することと、目標足平位置を介して間接的に表現することは等価だからである。

目標胴体位置姿勢角は、絶対座標系に対する胴体座標系の傾きの経時的データで表される。実胴体姿勢角は、胴体に備えられた姿勢角センサによって検出することができる。胴体の実際の位置と姿勢角を夫々実胴体位置と実胴体姿勢角と称する。姿勢角センサは、例えば胴体の角速度を検出するジャイロと、ジャイロの出力（角速度）を積分する積分器と、重力加速度の大きさと方向を検出する３軸加速度センサで構成される。

図２は、本実施の形態にかかるロボットの制御ブロック図である。図２に示すように、本実施の形態にかかるロボットの制御システムは、歩容データ１１１ａを記憶する記憶部１１１、目標距離算出部１１２、距離偏差算出部１１３、足偏差（Δφ、Δθ、Δｚ）算出部１１４、倣い制御部１１５、足平座標位置姿勢角補正部（加算器）１１６、２階微分器（ｓ^２）１１７、加速度偏差算出部（加算器）１１８、加速度制御部１１９、姿勢角偏差算出部（加算器）１２０、倒立制御部１２１、座標変換部１２２、補正量修正部１２３、胴体位置補正部（加算器）１２４、胴体位置姿勢角補正部（加算器）１２５、関節角変換部１２６、各軸制御部１２７を有する。また、ロボットの足平の裏面には、足裏距離センサ１３１が設けられ、体幹部には姿勢センサ１３２、加速度センサ１３３等が設けられている。

先ず、このような制御システムで行われる制御について概説する。歩容データ１１１ａの足平の目標位置姿勢角と、胴体部の目標位置姿勢角は、種々の補正量が加えられた後に関節角変換部１２６に入力される。補正量については後述する。関節角変換部１２６は、足平の目標位置姿勢角と、胴体部の目標位置姿勢角とを脚部の各関節の関節角に変換（逆キネマティクス変換）する。この変換はいわゆる逆変換と呼ばれるものである。関節角変換部１２６で求められた関節角が関節角指令値としてモータドライバである各軸制御部１２７へ送られる。各軸制御部１２７は、脚部の各関節に取り付けられたモータ１４１を駆動し、それぞれの関節の関節角が関節角変換部１２６から送られた関節角指令値に一致するように制御する。その結果、脚式ロボット１は、歩容データの足平及び胴体部のそれぞれの目標位置姿勢に追従しながら歩行することができる。

次に、この制御システムにおける各処理について説明する。先ず、倣い制御部１１５の制御について説明する。歩容データのうち、足平目標位置姿勢角は、目標距離算出部１１２に送られる。目標距離算出部１１２は、足平目標位置姿勢角データから歩容データ上での歩行面と足平との距離を求める。より詳細には、目標距離算出部１１２は、距離センサが取り付けられている足平の裏面から歩行面までの距離である目標距離を算出する。足平目標位置姿勢角は、足平に設置された（固定された）基準点における目標位置と姿勢角である。その基準点と距離センサとの位置関係は既知であるから基準点の足平目標位置姿勢角から距離センサの取り付け点と仮想的な歩行面の間の距離を算出することができる。本実施の形態においては、１つの足平に４つの距離センサが取り付けられているので、目標距離算出部１１２は、片足の足平について、４つの目標位置を算出する。

なお、本実施の形態においては、目標値算出時の歩行面は絶対座標系のｘｙ平面と一致する平面である。したがって、絶対座標系ｘｙｚで表現された足平の目標位置姿勢角のデータのうち、ｚ軸方向の位置の値がそのまま足平の基準点における歩行面と足平との間の目標距離となる。

目標距離算出部１１２で求められた目標距離が、距離偏差算出部１１３に送られる。また、距離偏差算出部１１３には、足裏距離センサ１３１からの計測値も入力される。距離偏差算出部１１３は、距離センサ１３１ごとに、距離センサ１３１が計測した実際の歩行面と足平裏面間の距離と、上述の目標距離との偏差を算出する。ここで、本実施の形態にかかる足裏距離センサは、接触して初めて地面との距離を正確に計測することが可能な距離センサを使用しているが、所定の距離より近づくまでは、ロボットの姿勢に影響を及ぼす力は事実上受けない。光学的な手法などにより非接触で計測してもよく、同等の効果が得られる。

足平の倣い制御は、足平の遊脚期間の全期間に亘って行うことはない。そこで、足平と地面との距離目標値及び計測値が共に距離センサの計測可能範囲外の場合は、倣い制御を行わないこととし、距離偏差をゼロとする。また、足平が着床しているとき、たとえば計算時の丸め誤差などにより、距離偏差に微小な定常偏差が発生すると、制御を行う倣い制御部１１５の積分項による定常偏差の積分量が補正量に加わってしまう。計算時の丸めの誤差が微小でもその積分量が制御にとって無視できない程に大きくなる可能性があるので、そのような定常偏差が補正量に影響を与えないようにする必要がある。ここで、上記の制御とは、ＰＩＤ制御、ＰＩＤに各種補償を加えたもの、伝達関数に基づくフィードバック制御、現代制御理論に基づくもの等様々なタイプが可能である。

そこで、本実施の形態においては、足平と地面との距離の目標値及び計測値を、所定の計測可能な範囲に制限するよう不感帯を設ける。すなわち、足平と地面との距離偏差の微小な誤差による倣い制御の補正量発散を防止する。

足偏差算出部１１４には、足裏距離センサの偏差が入力される。これを足位置姿勢偏差Δφ、Δθ、Δｚへ変換する。Δφ及びΔθは、地面に平行な面を規定する２直線の周りの姿勢角の偏差であり、Δｚは、地面に直行する方向の位置偏差である。

得られた足位置姿勢偏差Δφ、Δθ、Δｚは倣い制御部１１５に入力する。倣い制御部１１５は、以下のフィードバックループを適正に収束させるように構築される。すなわち、脚部に取り付けられた距離センサ１３１の計測値が距離偏差算出部１１３、足偏差算出部１１４、倣い制御部１１５、足平座標位置姿勢角補正部１１６、関節角変換部１２６、各軸制御部１２７を介して脚部の関節を駆動するモータ１４１へと影響するループである。倣い制御部１１５の具体的な機能は、一般的なＰＩＤ制御器と同一であるのでここでは説明を省略する。

倣い制御部１１５で算出された補正量は、足平座標位置姿勢角補正部１１６に入力される。足平座標位置補正部１１６には、記憶部１１１から足平目標位置姿勢角が入力され、この足平目標位置姿勢角に、倣い制御部１１５が算出した補正量を加算することで補正し、関節角変換部１２６に入力する。この倣い制御部１１５による制御により実足平姿勢角を目標足平位置姿勢角に一致することができる。すなわち、接地している脚に対しては、足裏面を接地面に面接触させ、足裏面を地面に倣わせるように制御する。

次に、加速度制御部１１９及び倒立制御部１２１について説明する。２階微分器１１７は、目標胴体位置姿勢角のうち目標胴体位置を２階微分して目標胴体加速度を算出する。制御システムは、デジタル制御系として構成されているので、目標胴体位置の微分は、より正確には、目標胴体位置の時系列データの差分によって近似値的に求められる。加速度偏差算出部１１８には、この目標胴体加速度と、胴体部に取り付けられた加速度センサ１３３からの計測値とが入力され、これらの偏差を出力する。加速度制御部１１９は、この加速度偏差に基づき、胴体部の加速度フィードバック制御を行う。加速度制御部には例えば比例係数と２次の遅れ要素などの伝達関数に基づく制御系が構成されている。この加速度制御部１１９は、加速度偏差を位置の次元、すなわち胴体位置補正量に変換して出力する。

次に、姿勢角偏差算出部１２０には、記憶部１１１から目標胴体姿勢角が入力され、また、胴体部に取り付けられた姿勢センサ１３２から胴体姿勢角が入力される。姿勢角偏差算出部１２０は、両者の偏差を求めて倒立制御部１２１に入力する。倒立制御部１２１は、この姿勢角偏差に基づきフィードバック制御により補正量を算出する。倒立制御部１２１は、胴体位置偏差及び胴体姿勢角偏差を小さくするため、すなわち実胴体位置姿勢角を目標胴体位置姿勢角に一致させるための制御部である。胴体姿勢角偏差が存在すること、すなわち実胴体位置姿勢角と目標胴体位置姿勢角の不一致は、足平が予想外に傾斜していることに起因していることが多い。足平が傾斜していると、胴体の位置と足平の位置の水平方向の偏差が大きくなりロボットが転倒しやすくなる。そこで、実胴体姿勢角を目標胴体位置姿勢角に一致させ、かつ胴体の位置と足平の位置の水平方向の偏差も小さくなるように、幾何学計算によって胴体位置の補正量を計算し、胴体位置を補正することで、ロボットの転倒を未然に防ぐことができる。このため、胴体姿勢角偏差に基づいて、目標胴体位置姿勢角の補正を行う。ただし、単に姿勢角偏差に基づいて胴体姿勢角を補正すると、偏差が大きい場合（平地でも）倒立制御の出力が大きくなりすぎ、足裏が地面から?がれ、結果的に転倒してしまう場合がある。

そこで、本実施の形態においては、倒立制御部１２１の算出した補正量で目標胴体位置姿勢角を補正するフィードバック制御ではなく、この補正量を足平の地面に対する接地度に基づき更に修正し、その修正した補正量により胴体位置姿勢角を補正する。このように、倒立制御部１２１の生成した補正量に対し、更に補正を加えることにより、足裏が?がれそうになったら胴体倒立制御をリミットし、常に足裏が地面に沿う状態を優先させた制御を行わせることができる。

まず接地度計算部１２９について説明する。接地度計算部１２９には、足裏距離センサ１３１からのセンサ値が入力される。そして、この距離センサの値を０〜１に正規化する。この値を接地度ｓということとする。接地度ｓは、その値が大きいほど地面に接地していることを示す。図３は、接地度と距離センサからの値との関係を示すグラフ図である。十分に接地している場合は１、接地していない場合は０とし、それを連続的に繋げた関数とすることができる。なお、本例においては、計測レンジ内で線形な関数にした場合を示すが、シグモイド関数等を利用してもよい。ただし、本実施の形態においては、後述するように、後述の接地座標系への座標変換手順でゼロ割や軸が定まらないケースを防ぐため、接地度の最小値を０とはせず微小値ε（０＜ε＜１）とした。

具体的には、接地度０＜ε≦ｓ≦１とし、センサ距離ｈを、０＜ｈ_ｌｏｗ＜ｈ＜ｈ_ｈｉｇｈとしたとき、センサ距離ｈがｈ_ｌｏｗ以下であれば接地度１、センサ距離ｈ_ｌｏｗ≦ｈ≦ｈ_ｈｉｇｈの間は１≧ｓ≧εの単調減少関数、センサ距離ｈがｈ_ｈｉｇｈ以上は接地度εとする。

接地状態計算部１２８には、接地度計算部１２９が計算した接地度ｓが入力される。そして、接地度ｓを用いて、最もよく接地している方向を主軸とする座標系（以下、接地座標系という。）とその接地度合を固有ベクトルと固有値を用いて表現する。固有ベクトル及び固有値（以下、接地状態という。）を座標変換部１２２及び補正量修正部１２３に入力する。

足平の地面に対する勾配を代表する平面は、地面に対する足平の最大勾配の方向と最小勾配の方向で表わすことができる。勾配は平面で代表しているので、最大勾配の方向と最小勾配の方向とは直交する。接地座標系は、この最大勾配方向と最小勾配方向を２軸とする座標系である。

説明を簡略化するため、接地面が、接地座標系Ｏｗ−ＸｗＹｗＺｗにおけるＸｗＹｗ平面に一致するものとする。そうすると、接地座標系は、絶対座標系のＸｗ軸とＹｗ軸を鉛直軸（Ｚｗ軸）の周りに所定角度だけ回転した座標系として表すことができる。接地座標系の一方の軸をＶ_１とし、他方の軸をＶ_２とする。軸Ｖ_１とＶ_２の求め方は後述する。絶対座標系のＸｗ軸、Ｙｗ軸と接地座標系のＶ_１軸、Ｖ_２軸の関係を図４に模式的に表す。図４の平面が接地面を表す。接地座標系の一方の軸Ｖ_１は、絶対座標系のＹｗ軸を時計回りに角度βだけ回転させた位置となる。接地座標系の他方の軸Ｖ_２は、絶対座標系のＸｗ軸を時計回りに角度βだけ回転させた位置となる。

図４にはまた、右脚の足平と左脚の足平が共に接地している状態を表している。夫々の足裏内に描かれた円の中心が距離センサの位置を示している。円の大きさは、夫々の距離センサの位置における接地度ｓの大きさを模式的に表している。後述するように、接地座標系の軸Ｖ_１は、足裏が地面に対して最も接地している（足裏の勾配が小さい）方向を示しており、軸Ｖ_２は、足裏が地面に対して最も接地していない（足裏の勾配が大きい）方向を示している。接地度ｓの勾配が小さいということは、足裏面が接地面に対して均等に接地していることを意味する。従って、ロボット１は、軸Ｖ_１と鉛直線を含む面内で胴体姿勢角を変化させる場合には、足裏面は接地面から浮き上がり難い状態であるということができる。軸Ｖ_２と鉛直線を含む面内で胴体姿勢角を変化させる場合には、足裏面は接地面から浮き上がり易い状態であるということができる。そこで、接地座標系の軸Ｖ_１、Ｖ_２を利用して次の様に目標胴体位置姿勢角の補正量を修正すると、足裏面を接地面から浮き上がり難くすることができる。

まず目標胴体位置姿勢角の補正量のうち、軸Ｖ_１とＶ_２の夫々の方向の成分を求める。夫々の方向の成分を、対応する軸の勾配が大きいほど減少させる。そうすることで、足平の地面に対する勾配が大きい方向、即ち、足裏がより接地していない方向については、目標胴体位置姿勢角の補正量のうちその方向の成分を小さくすることができる。そうすることによって、胴体姿勢角を変化させる際に、足裏面が接地面から浮き上がってしまうことを効果的に低減することができる。

接地座標系の軸Ｖ_１とＶ_２の求め方を説明する。接地座標系の軸Ｖ_１とＶ_２は、絶対座標系Ｏｗ−ＸｗＹｗＺｗのおけるＸｗＹｗ平面に含まれるので、以下では絶対座標系Ｏｗ−ＸｗＹｗＺｗにおける鉛直方向Ｚｗについては省略する。ここでは、足平が片足にｎ／２個のセンサを配置している場合について、接地状態計算部１２８の処理について説明する。絶対座標におけるセンサ位置（ｘ，ｙ）を求める。
^Ｗｒ_ｉ＝^ＷＴ_ｒ ^ｒｒ_ｉ（ｉ＝０，１，・・・，ｎ／２−１）
^Ｗｒ_ｉ＝^ＷＴ_ｌ ^ｌｒ_ｉ（ｉ＝ｎ／２，・・・，ｎ−１）
但し、
^Ｗｒ_ｉ：絶対座標におけるｉ番目のセンサ位置
^ｒｒ_ｉ，^ｌｒ_ｉ：右足／左足座標系におけるｉ番目のセンサ位置
^ＷＴ_ｒ，^ＷＴ_ｌ：右足／左足座標系原点の目標位置姿勢行列
各センサの接地度ｓを重みとして接地中心位置ｘ_ｍ，ｙ_ｍを求める。

但し、
ｘ_ｍ，ｙ_ｍ：接地中心位置
ｘ_ｉ，ｙ_ｉ：ｉ番目のセンサ位置（ｘ，ｙ）
ｓ_ｉ：ｉ番目のセンサの接地度
各センサの接地度を重みとして分散、共分散を求める。

分散共分散行列Ｓの固有値λ、固有ベクトルＶを求める。
但し、下記を満たす。

ここで、接地度ｓの最小値をゼロでないεとしたのは、分散共分散行列Ｓが正則行列となることを保証するためである。共分散行列Ｓは正則行列であるので、２つの固有値λ_１、λ_２が存在することが保証されると共に、固有ベクトルＶ_１、Ｖ_２が独立となることが保証される。すなわち、固有ベクトルＶ_１、Ｖ_２は直交する。固有値の大きいものから順にλ_１、λ_２とし、それに対応する固有ベクトルをＶ_１、Ｖ_２とすると、ここで得られたそれぞれの固有ベクトルは、
Ｖ_１：足裏の勾配が最小となる方向
Ｖ_２：足裏の勾配が最大となる方向
を表現しており、その接地度合を固有値λ_１、λ_２が表している。接地面におけるＸｗ軸、Ｙｗ軸、Ｖ_１ベクトル、Ｖ_２ベクトルの関係を示すと図４の模式図の通りとなる。
固有ベクトルを座標変換に利用するために正規化を行ない、さらにＶ_１のｘ座標が正、Ｖ_２のｙ座標が正になるように符号反転を適宜行っておく。

目標胴体位置姿勢角の補正量をベクトルΔθで表現すると、ベクトルθは絶対座標系Ｏｗ−ＸｗＹｗＺｗの各軸成分（Δθ_ｘ、Δθ_ｙ、Δθ_ｚ）で表される。ここで、鉛直軸Ｚｗ回りの補正量Δθ_ｚは、足裏面が接地面から浮き上がってしまうことに影響しない。従って、目標胴体位置姿勢角の補正量の接地面内成分（Δθ_ｘ、Δθ_ｙ）を前述したＶ_１軸とＶ_２軸の成分に変換する。変換後のＶ_１軸方向成分をΔθ_１、Ｖ_２軸方向成分をΔθ_２で表す。絶対座標系のＸｗ軸方向成分とＹｗ軸方向成分で表された補正量（Δθ_ｘ、Δθ_ｙ）をＶ_１軸方向成分Δθ_１とＶ_２軸方向成分Δθ_２へ変換するため（絶対座標系から接地座標系へ変換するための）の変換行列^ｗＡ_ｇｒｄは、前述した固有ベクトルＶ_１とＶ_２を用いて下記の式で表すことができる。

次に座標変換部１２２の処理について説明する。絶対座標系（ワールド座標系）における倒立制御の姿勢補正量（制御器出力）を（Δθ_ｘ，Δθ_ｙ）^Ｔとすると、接地座標系における倒立制御の姿勢補正量（Δθ_１，Δθ_２）^Ｔは、以下のように求めることができる。

次に、補正量修正部１２３は接地座標系の各方向において、接地度合に応じて補正量を制限する。ここでは、補正量を制限する関数をｆで表わし、以下のように記述しておく。
Δθ_１ｆ＝ｆ（λ_１，Δθ_１）
Δθ_２ｆ＝ｆ（λ_２，Δθ_２）
補正量を制限する関数ｆとしては、例えば以下のような関数を採用すればよい。
ｆ（λ，Δθ）＝α（λ）・Δθ
ただし、αは制限に用いる修正倍率を表しており、λの関数としては例えば、図５のグラフのようにすることができる。つまり、固有値λがある閾値λ_ｔｈより大きい場合、すなわち、十分に接地しているときは、補正量を制限する必要はなく、固有値λが閾値λ_ｔｈより小さい場合、すなわち十分に接地していない場合は、補正量を修正倍率α制限する。

これは、足裏の勾配が所定値より大きい場合（即ち、固有値λ_１が所定の大きさλ_ｔｈより小さい場合）に、Ｖ_１軸方向の足裏の勾配が大きいほど（即ち、固有値λ_１が小さいほど）、補正量ΔθのＶ_１軸方向成分Δθ_１を減少させることを意味する。同様に、足裏の勾配が所定値より大きい場合（即ち、固有値λ_２が所定の大きさλ_ｔｈより小さい場合）に、Ｖ_２軸方向の足裏の勾配が大きいほど（即ち、固有値λ_２が小さいほど）、補正量ΔθのＶ_２方向成分Δθ_２を減少させることを意味する。

修正された倒立制御の姿勢補正量を再び座標変換部１２２により絶対座標に座標変換する。座標変換部１２２は、接地座標系から絶対座標系に戻すため、変換行列の逆行列をかける。ここで、^ＷＡ_ｇｒｄは直交行列であるので、逆行列と転置行列は等しい。

以上の手順で計算した（Δθ_ｘｆ，Δθ_ｙｆ）^Ｔを実際の倒立制御の補正量として用いれば、十分に接地が保たれている方向に倒立制御を利かせ、逆に接地が保たれていない場合には、足裏の路面倣い制御を優先して働かせることとなる。

絶対座標に戻された補正量は、胴体位置補正部１２４に入力され加速度制御部１１９からの補正量と加算される。さらに、加算された補正量は、胴体位置姿勢角補正部１２５に入力され目標胴体位置姿勢角に加算される。こうして補正された目標胴体位置及び目標胴体位置姿勢角は関節角変換部１２６に入力される。

なお、足平目標位置姿勢角は、目標足平位置及び目標足平位置姿勢角からなる。倣い制御部１１５により目標足平姿勢角が補正される点については上述したが、目標足平位置についても補正されるものとすることができる。目標足平位置は、次のように補正することができる。足裏距離センサの測定値から、足平座標系原点Ｏｆと接地面との距離を求める。この距離は、地面に対する実足平位置を示す、目標足平位置（歩容データを作成する際のシミュレーション上での仮想的な接地面と仮想的な足平の間の距離）と実足平位置の偏差に基づき、目標足平位置の補正量を算出する、算出された補正量を、目標足平位置に加算することで目標足平位置を補正し、関節角変換部１２６に入力する。このような制御系によって、実足平位置を目標足平位置に一致させることができる。たとえば、実際のロボットの遊脚が実際の接地面に接地するタイミングを、歩容データ上で遊脚が接地面に着地するタイミングに一致させることができる。

図６は、本実施の形態の効果を説明する図である。ロボットが凸部に着地し、足裏が?がれそうになったら、倒立制御の制御入力を制限することから、結果、常に足裏が地面にならう制御を優先させることとなる。

次に、胴体姿勢角を接地度に応じて修正する方法について図７に示すフローチャートを参照して説明する。先ず、各足裏距離センサごとに、足裏から地面までの距離を測定する（ステップＳ１）。そして、測定したセンサ距離に基づき図３に示すグラフから接地度ｓを求める（ステップＳ２）。接地度ｓが求まったら、分散共分散行列Ｓを求め、その固有値λ_１、λ_２及び固有ベクトルＶ_１、Ｖ_２を求める。そして、この固有ベクトルから絶対座標系から接地座標系への変換行列を求める（ステップＳ３）。

次に、倒立制御部１２１で算出した補正量をステップＳ３で求めた変換行列を使用して接地座標系に変換する（ステップＳ４）。そうしてこれを接地状態に応じて図５に示す修正倍率に基づき修正する（ステップＳ５）。最後に、修正された倒立制御部１２１の姿勢補正量を上述の変換行列の逆行列を使用して絶対座標系に戻す（ステップＳ６）。この補正量を用いて目標胴体位置姿勢角を補正することで、地面から足裏を離れないように制御することができる。

本実施の形態においては、足平の地面に対する接地度に基づき、足平の接地方向を求める。そして、倒立制御部１２１の算出した補正量を接地座標系に変換し、足平の接地状態に応じて修正することで、可能な限り足裏を地面に対して接地させることができる。すなわち、接地状態に応じて補正量を修正するため、十分に接地が保たれている方向については倒立制御を働かせ、逆に接地が保たれていない方向については、足裏の倣い制御を優先して働かせることで、常に足裏が地面に倣うよう制御することができる。
実施の形態２.

次に、本発明の実施の形態２について説明する。図８は、本実施の形態にかかるロボットの制御システムを示すブロック図である。なお、図８に示す本実施の形態において、図２に示す実施の形態１と同一の構成要素には、同一の符号を付してその詳細な説明は省略する。上述の実施の形態においては、倒立制御部１２１が算出する補正量を、足裏の接地度に応じて修正するものであったが、本実施の形態においては、接地度計算部１５２の計算した接地度に基づき倣い制御部１１５及び倒立制御部１２１のゲインを調整するものである。

接地度計算部１５２のゲイン制御方法としては、接地度が所定の閾値より大きく、足裏が地面に対して十分接地していると判断できる場合は、倒立制御部１２１の制御ゲインを上げ、逆に足裏の倣い制御部１１５のゲインを下げるように制御する。一方、接地度が所定の閾値より小さく、足裏が十分接地していないような場合は、倒立制御部１２１のゲインを大きくし、逆に倣い制御部１１５のゲインを上げるように制御する。

本実施の形態においては、足裏距離センサの測定値に基づき求めた接地度を利用して、倣い制御と倒立制御のゲインを調整する。具体的には、接地度が大きい場合は、倒立制御のゲインを上げてその補正量を大きくし、接地度が小さい場合は、倒立制御のゲインをさげその補正量を小さくするよう制御する。このことにより、足裏の路面倣い制御を優先的に働かせ、足裏を地面に密着させることができる。

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

例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、記録媒体に記録して提供することも可能であり、また、インターネットその他の伝送媒体を介して伝送することにより提供することも可能である。

ロボットを正面から見た様子を概略的に表す概略図である。 本発明の実施の形態にかかるロボットの制御システムを示すブロック図である。 接地度と距離センサからの値との関係を示すグラフ図である。 絶対座標系のＸｗ軸、Ｙｗ軸と接地座標系のＶ_１軸、Ｖ_２軸の関係を示す模式図である。 制限に用いる修正倍率αと、固有値λの関数を示す図である。 本発明の実施の形態の効果を説明する図である。 本発明の実施の形態における胴体姿勢角を接地度に応じて修正する方法を示すフローチャートである。 本発明の実施の形態２にかかるロボットの制御システムを示すブロック図である。 倒立制御と倣い制御を併用する従来のロボットの問題点を示す図である。 同じく、倒立制御と倣い制御を併用する従来のロボットの問題点を示す図である。

符号の説明

１ ロボット、２ 頭部、３ 体幹、
４ 腰部、５ 右腕、６ 左腕、
１０ 脚部、２０ 右脚、２１ 右股関節、
２２ 右上腿、２３ 右膝関節、２４ 右下腿、
２５ 右足首関節、２６ 右足平、３０ 左脚、
３１ 左股関節、３２ 左上腿、３３ 左膝関節、
３４ 左下腿、３５ 左足首関節、３６ 左足平、
１１１ 記憶部、１１１ａ 歩容データ、１１２ 目標距離算出部、
１１３ 距離偏差算出部、１１４ 足偏差算出部、１１５ 倣い制御部、
１１６ 足平座標位置補正部、１１７ 階微分器、１１８ 加速度偏差算出部、
１１９ 加速度制御部、１２０ 姿勢角偏差算出部、１２１ 倒立制御部、
１２２ 座標変換部、１２３ 補正量修正部、１２４ 胴体位置補正部、
１２５ 胴体位置姿勢角補正部、１２６ 関節角変換部、１２７ 各軸制御部、
１２８ 接地状態計算部、１２９、１５２ 接地度計算部、１３０ 制御部、
１３１ 距離センサ、１３２ 姿勢センサ、１３３ 加速度センサ、１４１ モータ

Claims (17)

1. 目標歩容データと実際の歩容データとの姿勢角の偏差を求める姿勢角偏差算出手段と、
   前記姿勢角偏差算出手段が算出した姿勢角偏差に基づき補正量を算出する倒立制御手段と、
   前記倒立制御手段が算出した補正量で前記目標歩容データを補正する補正手段と、
   足平の裏面に設けられた距離センサと、
   前記距離センサの値に基づき足平の地面に対する接地度を算出する接地度算出手段とを有し、
   前記接地度算出手段は、算出した接地度に基づき前記倒立制御手段が算出する前記補正量を修正する脚式ロボット。
2. 目標胴体位置姿勢角と実際の胴体姿勢角との偏差を求める姿勢角偏差算出手段と、
   前記姿勢角偏差算出手段が算出した姿勢角偏差に基づき胴体部補正量を算出する倒立制御手段と、
   前記倒立制御手段が算出した胴体部補正量で前記目標胴体位置姿勢角を補正する胴体部補正手段と、
   足平の裏面に設けられた距離センサと、
   前記距離センサの値に基づき足平の地面に対する接地度を算出する接地度算出手段とを有し、
   前記接地度算出手段は、算出した接地度に基づき前記倒立制御手段が算出する前記胴体部補正量を修正する請求項１記載の脚式ロボット。
3. 目標足平位置姿勢と実際の足平位置姿勢との偏差を求める足平位置姿勢偏差算出手段と、
   前記足平位置姿勢偏差算出手段が算出した足平偏差に基づき脚部補正量を算出する倣い制御手段と、
   前記倣い制御手段が算出した脚部補正量で目標足平位置姿勢角を補正する足平部補正手段とを有し、
   前記接地度算出手段は、算出した接地度に基づき前記倒立制御手段及び前記倣い制御手段の特性を変更する請求項２記載の脚式ロボット。
4. 前記接地度算出手段が算出した接地度から前記足平の地面に対する接地方向を求め、この接地方向に基づき前記胴体部補正量を調整する補正量調整手段を有する
   ことを特徴とする請求項２記載の脚式ロボット。
5. 目標足平位置姿勢と実際の足平位置姿勢との偏差を求める足平位置姿勢偏差算出手段と、
   前記足平位置姿勢偏差算出手段が算出した足平偏差に基づき脚部補正量を算出する倣い制御手段と、
   前記倣い制御手段が算出した脚部補正量で目標足平位置姿勢角を補正する足平部補正手段とを有する
   ことを特徴とする請求項４記載の脚式ロボット。
6. 目標胴体位置から目標胴体加速度を算出する目標加速度算出手段と、
   加速度センサと、
   前記加速度センサから取得した現在の加速度と前記目標加速度との偏差を求める加速度偏差算出手段と、
   前記加速度偏差に基づき加速度補正量を算出する加速度制御手段と、
   前記加速度制御手段が算出した加速度補正量により前記目標胴体位置を補正する胴体位置補正手段とを更に有する
   ことを特徴とする請求項２乃至５のいずれか１項記載の脚式ロボット。
7. 前記接地度に基づき前記胴体補正量を、足平が最もよく接地している方向を主軸とする座標系である接地座標系に変換する座標変換手段と、
   前記接地座標系に変換した前記胴体補正量を、接地している度合いを示す接地状態に応じて補正する補正量修正手段とを有し、
   前記座標変換手段は、前記補正量修正手段にて補正した胴体補正量を絶対座標系に変換し、
   前記胴体補正手段は、前記絶対座標系に変換された胴体補正量で前記目標胴体位置姿勢角を補正する
   ことを特徴とする請求項４乃至６のいずれか１項記載の脚式ロボット。
8. 前記接地度算出手段は、算出した接地度に基づき前記倒立制御手段及び前記倣い制御手段のゲインを調整する
   ことを特徴とする請求項１記載の脚式ロボット。
9. 目標足平位置姿勢角と実際の足平姿勢角との偏差を求める姿勢角偏差算出手段と、
   前記姿勢角偏差算出手段が算出した足平角偏差に基づき足平部補正量を算出する倒立制御手段と、
   前記倒立制御手段が算出した足平部補正量で前記目標足平位置姿勢角を補正する足平部補正手段と、
   足平の裏面に設けられた距離センサと、
   前記距離センサの値に基づき足平の地面に対する接地度を算出する接地度算出手段とを有し、
   前記接地度算出手段は、算出した接地度に基づき前記倒立制御手段が算出する前記足平部補正量を修正する請求項１記載の脚式ロボット。
10. 目標歩容データと実際の歩容データとの偏差を求める姿勢角偏差算出工程と、
    前記姿勢角偏差算出工程にて算出した姿勢角偏差に基づき補正量を算出する倒立制御工程と、
    前記倒立制御工程にて算出した補正量で前記目標歩容データを補正する補正工程と、
    足平の裏面に設けられた距離センサの値に基づき足平の地面に対する接地度を算出する接地度算出工程とを有し、
    前記接地度算出工程では、算出した接地度に基づき前記倒立制御工程にて算出する前記補正量を修正する脚式ロボットの制御方法。
11. 前記姿勢角偏差算出工程では、目標胴体位置姿勢角と実際の胴体姿勢角との偏差を求め、
    前記倒立制御工程では、前記姿勢角偏差算出工程にて算出した姿勢角偏差に基づき胴体部補正量を算出し、
    前記補正工程では、前記倒立制御工程にて算出した胴体部補正量で前記目標胴体位置姿勢角を補正し、
    前記接地度算出工程では、算出した接地度に基づき前記倒立制御工程にて算出する前記胴体部補正量を修正する
    ことを特徴とする請求項１０記載の脚式ロボットの制御方法。
12. 目標足平位置姿勢と実際の足平位置姿勢との偏差を求める足平位置姿勢偏差算出工程と、
    前記足平位置姿勢偏差算出工程にて算出した足平偏差に基づき脚部補正量を算出する倣い制御工程と、
    前記倣い制御工程が算出した脚部補正量で目標足平位置姿勢角を補正する足平部補正工程とを有し、
    前記倒立制御工程及び倣い制御工程では、補正量を算出する際のゲインを前記接地度に基づき調整する
    ことを特徴とする請求項１１記載の脚式ロボットの制御方法。
13. 前記接地度算出工程にて算出した接地度から前記足平の地面に対する接地方向を求め、この接地方向に基づき前記胴体部補正量を調整する補正量調整工程を有し、
    前記駆動工程では、前記足平補正工程にて算出した補正値及び調整工程にて調整された胴体部補正量に基づき関節角を駆動する
    ことを特徴とする請求項１１記載の脚式ロボットの制御方法。
14. 目標足平位置姿勢と実際の足平位置姿勢との偏差を求める足平位置姿勢偏差算出工程と、
    前記足平位置姿勢偏差算出工程にて算出した足平偏差に基づき脚部補正量を算出する倣い制御工程と、
    前記倣い制御工程にて算出した脚部補正量で目標足平位置姿勢角を補正する足平部補正工程とを有する
    ことを特徴とする請求項１２記載の脚式ロボットの制御方法。
15. 目標胴体位置から目標胴体加速度を算出する目標加速度算出工程と、
    加速度センサから取得した現在の加速度と前記目標加速度との偏差を求める加速度偏差算出工程と、
    前記加速度偏差に基づき加速度補正量を算出する加速度制御工程と、
    前記加速度制御工程にて算出した加速度補正量により前記目標胴体位置を補正する胴体位置補正工程とを更に有する
    ことを特徴とする請求項１１乃至１３のいずれか１項記載の脚式ロボットの制御方法。
16. 前記接地度に基づき前記胴体補正量を、足平が最もよく接地している方向を主軸とする座標系である接地座標系に変換する座標変換工程と、
    前記接地座標系に変換した前記胴体補正量を、接地している度合いを示す接地状態に応じて補正する補正量修正工程とを有し、
    前記座標変換工程では、前記補正量修正工程にて補正した胴体補正量を絶対座標系に変換し、
    前記胴体補正工程では、前記絶対座標系に変換された胴体補正量で前記目標胴体位置姿勢角を補正する
    ことを特徴とする請求項１２乃至１４のいずれか１項記載の脚式ロボットの制御方法。
17. 前記姿勢角偏差算出工程では、目標足平位置姿勢角と実際の足平姿勢角との偏差を求め、
    前記倒立制御工程では、前記姿勢角偏差算出工程にて算出した姿勢角偏差に基づき足平部補正量を算出し、
    前記補正工程では、前記倒立制御工程にて算出した足平部補正量で前記目標足平位置姿勢角を補正し、
    前記接地度算出工程では、算出した接地度に基づき前記倒立制御工程にて算出する前記足平部補正量を修正する
    ことを特徴とする請求項１０記載の脚式ロボットの制御方法。

Images