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

Abstract

【課題】凹凸がある地面であっても安定して移動が可能な脚式移動ロボット及びその制御方法を提供すること。
【解決手段】脚式移動ロボット１は、目標とする目標体幹加速度と現在の体幹加速度との偏差である加速度偏差を算出する体幹歩容データ補正演算部１１２と、目標とする目標重心位置と現在の重心位置との偏差である重心位置偏差を算出し、その重心位置偏差と、前記の加速度偏差により歩容データ目標値を補正する補正量変更部１１６とを有する。
【選択図】図２

Description

本発明は、脚式移動型のロボット及びその制御方法に関し、足先センサを用いずに安定した歩行が可能な脚式移動ロボット及びその制御方法に関する。

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

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

このような歩行動作を安定して行う際には、ロボット全体の重心位置を制御して、脚部を駆動する必要がある。すなわち、左右に各々脚部を有する２足歩行タイプの脚式歩行型ロボットの場合、歩行する床面に接地する支持脚の足平部分の接地面内部に、ロボット全体の重心によるモーメントが作用しない点（ＺＭＰ＝Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）を位置させる。

そのような脚式移動型のロボットとして、特許文献１には、予期しない凹凸がある路面でも安定して歩行することを目的とした脚式移動ロボットが開示されている。この特許文献１に記載の脚式移動ロボットは、予め設定された各関節の目標角度を所定時刻ごとに各関節に配置したサーボモータに出力し、各関節の実角度が目標角度に追従するように制御するロボットにおいて、ロボットの姿勢を検出する検出手段及び検出された姿勢に基づいてロボットが所定の姿勢にあるか否かを判定する姿勢判定手段を備え、胴体の傾斜角、傾斜角速度を検出し、その値が所定範囲外である場合に、歩容を時間方向に変更する。例えば、前傾している場合には、早く着地させる。

また、特許文献２には、足先センサを用いずに安定した歩行を行う脚式ロボットとその歩行制御方法が開示されている。図６は、特許文献２に記載のロボットの制御システムを示す図である。図６に示すように、脚式ロボット２０１は、体幹と、体幹に対して揺動可能に連結されている脚リンクと、目標とする足先運動の経時的変化を記述する足先歩容データ、目標足先運動の変化に追従して歩行を可能とする目標とする体幹運動の経時的変化を記述する体幹歩容データを記憶する歩容データ記憶装置２１１、関節角群計算装置２１３、アクチュエータ制御部２１４、加速度センサ２１７、実際の体幹加速度を検出する体幹加速度検出装置２１５、体幹歩容データ補正演算部２１２を有する。

体幹歩容データ補正演算部２１２は、体幹位置姿勢目標値を２階微分して目標体幹加速度を算出する装置２３１と、目標とする体幹運動と実際の体幹運動の偏差を算出する偏差算出部２３２と、算出された偏差から所定の伝達関数に基づいて補正量を求め、その補正量に基づいて体幹歩容データ記憶装置２１１に記憶されている体幹歩容データを補正する外乱力算出装置２３３、目標補正量算出装置２３４及び体幹位置補正装置２３５とを備えている。目標とする体幹加速度と、実際の体幹加速度との偏差に応じて、歩容の体幹位置姿勢目標値を補正する。主に、加速度の偏差に基づき体幹位置を補正している。
特許３１６７４０７号公報 特開２００５−２１２０１２号公報

しかしながら、特許文献１に記載の技術は、歩容を時間方向に補正する手段について、たとえば前傾している場合には、早く着地させるために各関節の速度が速くなる。このためモータの速度がより必要になってしまい、関節の粘性摩擦の影響などによって、モータのトルクが必要となる、すなわちエネルギー効率が悪いという問題点がある。また、例えば高速に歩行している場合には、前傾とするとモータの速度を速くする方向に補正されるので、モータの性能以上の目標関節角度が要求される場合があり得る。さらにまた、胴体が前傾した状態のまま、時間方向に歩容を早めて、早く着地させようとすると、ロボット全体が前傾しているため、遊脚の着地位置目標は床よりも下面に設定されている。このため、着地時にロボットが床よりもさらに鉛直下方に遊脚足先を動作させようとして床を強く蹴ってしまう。すなわち、時間方向の補正が入ると、前傾している場合には、着地時に遊脚で路面を鉛直方向に強く蹴ってしまい、その後、不安定になりやすい。

一方、特許文献２では、以下の問題が生じる。ここで、ロボットが凹部と凸部に着地した場合について説明する。図７（ｂ）に示すように、ロボットが凸部３０１に着地した場合は、もともとの重心位置３０２が目標重心位置に対して遅れているので補正後重心位置３０３のように大きく補正しても問題にならない。これに対し、図７（ａ）に示すように、ロボットが凹部３０４に着地した場合、重心位置３０５が補正後重心位置３０６へ補正され、補正が大きいと目標重心位置より外側に大きく外れてしまい、その後に回復することが困難になる。特許文献２に記載の技術では、凹部３０６に着地した場合、凹部３０４であることによるロボットの姿勢偏差分と加速度フィードバック制御によるロボットの胴体補正分の和が胴体のずれ分になるが、凹部３０４の場合、そのずれ分が大きくなってしまい、支持脚からみた重心位置の水平方向の偏差が大きくなってしまう。すなわち、路面外乱に対して、足裏の接地性能が良くなるように制御されるが、凹部３０６に着地した場合、胴体は実際の重心位置が目標重心位置より大きく外れる側に補正されてしまうため、重心位置がその外側に移動してしまい、その後、重心位置を戻すだけの床反力モーメントを発生させられないという問題点がある。

本発明は、このような問題点を解決するためになされたものであり、凹凸がある地面であっても安定して移動が可能な脚式移動ロボット及びその制御方法を提供することを目的とする。

上述した目的を達成するために、脚式移動ロボットにおいて、目標とする目標体幹加速度と現在の体幹加速度との偏差である第１偏差を算出する加速度偏差算出手段と、目標とする目標重心位置と現在の重心位置との偏差である第２偏差を算出する重心位置偏差算出手段と、前記第１偏差及び第２偏差により歩容データ目標値を補正する補正手段とを有するものである。

本発明においては、第１偏差及び第２偏差により体幹歩容データ目標値を補正する補正手段を有するので、歩容データ目標値を的確に補正することができる。

また、前記補正手段は、前記歩容データ目標値を前記第１偏差に基づき補正する体幹歩容データ補正演算手段と、前記第１偏差及び第２偏差に基づき、前記体幹歩容データ補正演算手段の補正の度合を変更する補正量変更手段とを有することができる。これにより、第１偏差により歩容データを補正するのみならず、その補正の度合を調整するため、より精密な補正が可能となる。

さらに、前記体幹歩容データ補正演算手段は、前記第１偏差から補正量を生成する補正量生成手段と、前記補正量生成手段が生成した補正量で前記歩容データ目標値を補正する目標値補正手段とを有することができる。

さらにまた、前記補正量変更手段は、前記第１偏差及び第２偏差が同符号の場合にのみ前記歩容データ目標値の補正量を調整する調整係数を生成することができる。第１偏差及び第２偏差が同符号の場合は、凹部に着地したと判断でき、歩容データ目標値を第１偏差の値に応じて補正する際、その補正量を修正する調整係数を生成する。第１偏差及び第２偏差の符号が異符号の場合は、凸部に着地したと判断することができ、この場合は、第１偏差の値に応じて生成された補正量で歩容データ目標値を補正する。

また、前記補正量生成手段は、前記第１偏差に予め定められた比例ゲイン及び伝達関数、並びに前記補正量変更手段が生成した調整係数を乗算して補正量を生成し、前記補正量変更手段は、前記第２偏差が大きいほど前記補正量が小さくなるよう前記調整係数を変更することができる。調整係数を小さくすることで補正量を減少させ、凹部に着地した際にロボットの重心位置が大きく移動してしまうことを防止する。

さらにまた、前記補正量変更手段は、前記第２偏差が第１の閾値より小さい場合は調整係数を１とし、前記第１の閾値乃至第２の閾値の間は１から０への単調減少関数とし、前記第２の閾値以上の場合は前記調整係数をゼロとすることができる。重心位置偏差の値に応じて調整係数を適宜変更することができる。

さらに、前記重心位置偏差算出手段の代わりに、目標胴***置と、現在の胴***置との偏差である第３偏差を算出する胴***置偏差算出手段、又は目標胴体姿勢と、現在の胴体姿勢との偏差である第４偏差を算出する胴体姿勢位置偏差算出手段を設けるものとしてもよい。ロボットの質量のうち、大半がロボットの体幹に集中していることから、重心位置の代わりに胴***置や胴体姿勢位置を使用してもよい。

さらにまた、前記歩容データ目標値は、体幹位置姿勢目標値及び足先位置姿勢目標値であるとすることができる。体幹位置姿勢は、絶対座標系に対するロボットの胴体に固定された座標系の位置及び傾きで表わされる。また、足先位置姿勢は、絶対座標系に対するロボットの足先に固定された座標系の位置及び傾きで表わされる。

本発明にかかる脚式移動ロボットの制御方法は、脚式移動ロボットの制御方法であって、目標体幹加速度と現在の体幹加速度との偏差である第１偏差を算出する加速度偏差算出工程と、目標重心位置と現在の重心位置との偏差である第２偏差を算出する重心位置偏差算出工程と、前記第１偏差及び第２偏差により歩容データ目標値を補正する補正工程とを有する。

本発明によれば、凹凸がある地面であっても安定して移動が可能な脚式移動ロボット及びその制御方法を提供することができる。

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

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

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

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

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

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

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

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

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

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

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

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

図２は、ロボットの制御システムの機能を示すブロック図である。図２に示す要素のうち、ロボットの機械系および体幹加速度センサ１７を除いた部分が制御システムの構成要素である。制御システムは、物理的に１つの装置に含まれていてもよいし、物理的に分離された装置ごとに分けて収容されていてもよい。

ロボット１は、歩容データ記憶部１１１、体幹歩容データ補正演算部１１２、関節角変換部１１３、各軸制御器１１４、体幹加速度計算部１１５、補正量変更部１１６、体幹加速度センサ１１７ａ、各種センサ及び関節エンコーダ（以下、まとめてセンサ１１７という。）を有する。

歩容データ記憶装置１１１には、予め計算されたロボット１の歩容データが記憶されている。歩容データとは、腕先軌道データ（腕先歩容データ）、足先軌道データ（足先歩容データ）及び体幹軌道データ（体幹歩容データ）のことをいう。予め記憶されている歩容データのうち、腕先軌道データ及び足先軌道データは、関節角変換部１１３へ直接に入力される。体幹歩容データは、体幹歩容データ補正演算部１１２へ入力され、ロボット１の歩行に伴いその都度補正され、補正された体幹軌道を示すデータが関節角変換部１１３へ入力される。また、関節角等から演算により歩容データをリアルタイムに生成してもよい。足先軌道データと腕先軌道データは、補正されないで直接に関節角変換部１１３に入力される。一方、体幹位置姿勢目標値は体幹歩容データ補正演算部１１２に入力される。

関節角変換部１１３は、両足位置姿勢目標値及び、補正された体幹位置姿勢目標値からなる歩容データが入力される。この歩容データに基づいて、いわゆる逆キネマティクスを解くことでロボット１の各関節角θ１（ｔ）、θ２（ｔ）、θ３（ｔ）、・・・を計算する。計算された関節角群データは、各軸制御器１１４に入力される。

各軸制御器１１４は、図１のロボット１の関節群を回転させるアクチュエータ群を制御する。アクチュエータ群はロボットの機械系に存在している。アクチュエータ群を制御すると、ロボット１の関節角を計算された関節角に調整できる。これにより、ロボット１は歩容データに従って歩行する。

ロボット１の体幹部には、センサの１つとして、体幹加速度センサ１１７ａが搭載されている。体幹加速度センサ１１７ａにより、体幹のｘ方向（歩行方向）とｙ方向（体側方向）とｚ方向（高さ方向）の加速度を検出する。体幹加速度計算部１１５は、体幹加速度センサにて測定された体幹の加速度を、目標体幹加速度を算出する際に使用した体幹部の位置に対応する位置の体幹加速度に変換する。この実際の体幹加速度ａｘｒ、ａｙｒ、ａｚｒを体幹歩容データ補正演算部１１２へ出力する。

体幹歩容データ補正演算部１１２は、歩容データ記憶部１１１に記憶されている体幹軌道データ（体幹歩容データ）と、体幹加速度計算部１１５で計算されたロボット１の実際の体幹加速度（以下、実体幹加速度という。）に基づいて、体幹軌道データの補正を行う。具体的には、実体幹加速度と、目標となる目標体幹加速度の偏差である第１偏差に基づき、歩容データ目標値のうち、体幹位置姿勢目標値を補正する。補正された体幹位置姿勢目標値は、関節角変換部１１３へ入力される。

さらに、本実施の形態においては、体幹加速度偏差及びロボット１の実際の重心位置（以下、実重心位置という。）と目標となる目標重心位置との偏差である第２偏差である重心位置偏差に基づき、前記体幹位置姿勢目標値の補正の度合を変更する補正量変更部１１６を有する。補正量変更部１１６は、体幹加速度偏差及び重心位置偏差の値が同符号の場合に体幹位置姿勢目標値の補正量を修正するが、体幹加速度偏差及び重心位置偏差の値が異符号である場合は補正量の修正はしない。具体的には、重心位置偏差が大きいほど体幹位置姿勢目標値の補正量が小さくなるよう、当該補正量に乗算する後述する調整係数を出力する。ここで、体幹歩容データ補正演算部１１２及び補正量変更部１１６から体幹加速度偏差及び重心位置偏差により体幹位置姿勢目標値を補正する補正手段が構成される。

次に、体幹歩容データ補正演算部１１２について説明する。体幹歩容データ補正演算部１１２は、２階微分演算部１３１、加速度偏差算出部１３２、補正量生成部１３３及び目標値補正手段としての目標体幹位置姿勢補正部１３４を有する。ここでは、図示の明瞭化のために、ｘ方向の事象のみを図示している。ｙ方向とｚ方向についても同様である。

２階微分演算部１３１は、歩容データ記憶部１１１に記憶されている体幹歩容データが示す目標体幹位置ｘｏ、ｙｏ、ｚｏ（それぞれは経時的に変化する）を、時間に関して２階微分演算を行って、目標体幹加速度ａｘｏ、ａｙｏ、ａｚｏを算出する。加速度偏差算出部３２は、実際の体幹加速度ａｘｒ、ａｙｒ、ａｚｒから、実際の体幹加速度ａｘｏ、ａｙｏ、ａｚｏを減じて、加速度偏差Δａｘ、Δａｙ、Δａｚを算出する。

補正量算出部１３３は、算出された加速度偏差Δａｘ、Δａｙ、Δａｚに、比例ゲイン、調整係数ｋ、伝達関数１／Ｇ（ｘ）を乗じて補正量ｘｄ、ｙｄ、ｚｄを算出する。ここで、調整係数ｋは、後述するように、補正量の値を調整するための係数である。本実施の形態においては、加速度偏差Δａｘ、Δａｙ、Δａｚに対し、−Ｍ／（Ｍｉ×ｓ^２＋Ｃｉ×ｓ＋Ｋｉ）を乗算している。

ここで、伝達関数１／Ｇ（ｓ）は、２次の項の係数をＭｉ、１次の項の係数をＣｉ、０次の項の係数をＫｉとする遅れ要素からなる。Ｍｉ、Ｃｉ、Ｋｉは所望のパラメータであり、予め指定されている。ｓはラプラス変換子、伝達関数のゲインは１／Ｋｉで、比例ゲインとしては−Ｍを用いる。ここで、Ｍはロボットの質量であり、加速度偏差Δａｘ、Δａｙ、Δａｚに比例ゲイン−Ｍを乗じることで外乱力Ｄｒｘ、Ｄｒｙ、Ｄｒｚを算出する。補正量算出部１３３の処理は、たとえばｘ方向について、
Ｄｒｘ＝Ｍｉ・ｘｄ（２）＋Ｃｉ・ｘｄ（１）＋Ｋｉ・ｘｄ（０）
の微分方程式を解いてｘｄを求めることに相当する。ここで、（２）は時間に関する２階微分を示し、（１）は時間に関する１階微分を示し、（０）はｘｄ自身を示す。なお、補正量ｘｄの符号の取り方によっては、
−Ｄｒｘ＝Ｍｉ・ｘｄ（２）＋Ｃｉ・ｘｄ（１）＋Ｋｉ・ｘｄ（０）
と表現する方が正しいこともある。

なお、これらの係数、Ｍｉ、Ｃｉ、Ｋｉのうち、０次の係数Ｋｉは不可欠であるが、１次の遅れ要素の係数Ｃｉ及び２次の遅れ要素の係数Ｍｉの一方又は双方をゼロとしてもよい。

この伝達関数は、ばね−質点−ダンパ系を記述する伝達関数であり、補正後の体幹歩容データに従って動作するロボットの体幹は、補正後の体幹歩容データが表現する目標体幹運動の経時変化に従って緩やかに収束する。ロボットには、関節等にガタがあったり、制御に応答遅れがあったり、外界から外力が加わると、実際の体幹加速度が目標とする体幹加速度にならず、予期せぬ外乱力によって加速度偏差Δａｘ、Δａｙ、Δａｚが生じる。補正量生成部１３３は、この加速度偏差とロボット（体幹部）の質量から、外乱力を推定し、この外乱力と、「ばね−質点−ダンパ系」を記述する伝達関数に基づいて、外乱力を補償するための体幹運動に関する補正量を求めることができる。なお、パラメータＭｉ、Ｃｉ、Ｋｉは、ロボットに発揮させる復元力の大きさに影響するが、「ばね−質点−ダンパ系」を記述するものであればよいので、適宜設定すればよい。

目標体幹位置姿勢補正部１３４は、歩容データ記憶部１１１に記憶されている補正前の目標体幹位置ｘｏ、ｙｏ、ｚｏに、補正量ｘｄ、ｙｄ、ｚｄを加算して、補正後の目標体幹位置ｘｒｅｆ、ｙｒｅｆ、ｚｒｅｆを算出し、関節角変換部１１３へ指示する。

次に、補正量変更部１１６について説明する。補正量変更部は、目標重心位置計算部、補正値調整部１２２、実重心位置計算部１２３を有する。補正量変更部１１６は、加速度偏差のフィードバック制御に用いる補正値を重心位置の偏差に応じて調整するための調整係数を生成する。

目標重心位置計算部１２１、両足位置姿勢目標値及び体幹位置姿勢目標値から目標重心位置を算出する。なお、歩容データを生成する際に、この目標重心位置を計算しておいてもよい。その場合は、目標重心位置計算部は不要である。

実重心位置計算部１２３は、姿勢センサ及び関節エンコーダの値に基づき、現在の重心位置を算出する。ここで、本実施の形態においては、現在の重心位置を算出するものとして説明するが、重心位置ではなく、胴***置を使用することも可能である。この場合、目標重心位置ではなく、目標胴***置を算出しておく。ロボットの質量のうち、大半がロボットの体幹に集中している。そこで、重心位置偏差の代わりに胴***置偏差を使用してもよい。これによって重心位置を求める演算を省略することができ、計算時間を削減して処理スピードを向上することができる。または、同様の理由から重心位置偏差の代わりに胴体姿勢偏差を使用してもよい。

補正値調整部１２２は、目標重心位置と、現在の重心位置との偏差である重心位置偏差を求める。そして、重心位置偏差と加速度偏差とが同符号である場合のみ、重心位置偏差の値に応じて、補正量生成部１３３の補正量を調整する調整係数ｋを求める。加速度偏差と重心位置偏差の値の符号が同符号であると、ロボットは凹部に着地したものと判断でき、重心位置偏差が大きくなりすぎないよう補正量を調整する必要がある。ここで、本実施の形態においては、補正係数生成部１２２は、補正値生成部１３３の補正値を変更・調整するものとして説明するが、補正量を調整ができればよく、例えば加速度偏差Δａｘ、Δａｙ、Δａｚを調整・変更してもよい。

次に、補正量変更部の動作、すなわち、実重心位置の計算方法、重心位置偏差の計算方法、調整係数の生成方法について説明する。図３は、目標歩容と重心位置及び実際の歩容と重心位置を示す図である。Σ_ｗ０は、基準座標系、Σ_ｆｄは、ロボットの脚部の目標座標系、Σ_ｂｄは、ロボットの体幹部の目標座標系、Σ_ｆｒは、ロボットの脚部の実際の座標系、Σ_ｂｒはロボットの体幹部の実際の座標系を示している。

実重心位置の計算方法
ロボットの体幹部に備えられた姿勢センサから得られた姿勢角、各関節のサーボ目標値、ロボットのリンクの質量、長さなどを含むモデルから重心位置を算出する。ここで得られる重心位置は、目標の歩容座標系を生成する基準座標系Σ_ｗ０とは異なる座標系基準となっている。例えば、図３（ａ）に示すように、目標の歩容と重心位置は基準座標系Σ_ｗ０を基準にしており、また、実際の歩容と重心位置は、図３（ｂ）に示すように、任意の別の座標系Σ_ｗｒを基準に求められる。図３（ａ）、図３（ｂ）において、Σの添え字のｂは「体幹部」、ｆは「脚部」、ｄは「目標座標」、ｒは「実際の座標」、ｗは、基準座標系Σ_ｗ０を基準とした座標系であることを示す。

重心位置偏差の計算
ロボットの体幹に固定された姿勢センサから得られる情報から胴体の回転行列^ｗ０Ｒ_ｂｒが得られる。これを利用して、ロボットの支持脚足先位置からみた重心位置を基準座標系で求める。先ず、ロボットの目標支持脚足先位置からみた目標重心位置座標は、
^ｗ０Ｐ_ｇｄ−^ｗ０Ｐ_ｆｄ
^ｗ０Ｐ_ｇｄ：基準座標系Σ_ｗ０からみた目標重心位置座標
_ｗ０Ｐ^ｆｄ：基準座標系Σ_ｗ０からみた目標支持脚足先位置座標
次に、ロボットの実際（座標系Σ_ｗｒ）の支持脚足先位置座標からみた重心位置座標は、^ｗｒＰ_ｇｒ−^ｗｒＰ_ｆｒであり、座標系Σ_ｗｒを座標系Σ_ｂｒに変換する回転行列^ｗｒＲ_ｂｒ、座標系Σ_ｗｒを座標系Σ_ｗ０に変換する回転行列^ｗ０Ｒ_ｂｒを使用し、基準座標系に変換した実際の重心位置座標を現すと、下記となる。
^ｗ０Ｒ_ｂｒ ^ｗｒＲ_ｂｒ ^Ｔ（^ｗｒＰ_ｇｒ−^ｗｒＰ_ｆｒ）
^ｗｒＰ_ｇｒ：座標系Σ_ｗｒからみた実重心位置座標
^ｗｒＰ_ｆｒ：座標系Σ_ｗｒからみた実支持脚足先位置座標
^ｗｒＲ_ｂｒ：座標系Σ_ｗｒからみた実胴体姿勢
^ｗ０Ｒ_ｂｒ：姿勢センサから得られた実胴体姿勢

ここで添え字のｆは「脚部」、ｇは「重心位置」、ｗは、「基準座標系Σ_ｗ０を基準としたベクトルの要素」、ｒは「実際の座標」を示している。以上の結果から、ロボットの支持脚からみた重心位置偏差^ｗ０ΔＰ_ｇ−ｆは、基準座標系Σ_ｗ０を基準とすると下記となる。
^ｗ０ΔＰ_ｇ−ｆ＝^ｗ０Ｒ_ｂｒ ^ｗｒＲ_ｂｒ ^Ｔ（^ｗｒＰ_ｇｒ−^ｗｒＰ_ｆｒ）−（^ｗ０Ｐ_ｇｄ−^ｗ０Ｐ_ｆｄ）

重心位置偏差に応じたゲイン変更
重心位置偏差と、補正量生成部１３３の補正量の符号を比較し、符号が同じ場合には、補正量が小さくなるような調整係数ｋを乗算する。これを式であらわすと下記となる。

このとき、調整係数は、図４のように表すことができる。図４は、ロボットの胴***置補正及び加速度偏差の向きを示す図である。図４は、調整係数ｋと重心位置の偏差の大きさとの関係を示す図である。加速度偏差の符号（補正量の符号）と重心位置偏差の符号が同じ場合には、ロボットが凹部に着地したと判断して胴体加速度フィードバック制御の補正量を図４に示す変調係数で調整する。

図４に示す例では、重心位置偏差がある閾値Δ１以下である場合は、調整係数ｋを最大値の１とする。重心位置偏差が閾値Δ１乃至閾値Δ２（Δ１＜Δ２）の間は、調整係数ｋは１から０への単調減少関数とし、閾値Δ２でｋ＝０とする。重心位置偏差が閾値Δ２以上の場合は、調整係数ｋ＝０とし、補正量を０とする。

これにより、上述の図６に示したように、凹部に着地したと判断できる場合は、補正量が小さくなるように補正することで、重心位置が大きく外れてしまうことを防止する。そして、凸部に着地したと判断できる場合は、胴体の加速度フィードバックによる補正量はそのままとする。これにより、十分に外力を吸収し、足裏の路面に対する接地性を確保する。本実施の形態においては、凸部と凹部を判定するため、重心位置偏差と加速度偏差の符号を比較するのである。

図６は、本実施の形態における補正量変更部の動作を示すフローチャートである。図５に示すように、先ず、実重心位置計算部１２３が胴体の実姿勢角と各関節サーモ目標値から、ロボットの実重心位置を算出する(ステップＳ１)。次に、調整係数生成部１２２は、ロボットの重心位置偏差を求める(ステップＳ２)。そして、調整係数生成部１２２は、ロボットの重心位置偏差と、加速度フィードバック制御に使用する体幹加速度偏差の符号を比較する(ステップＳ３)。符号が同じ、すなわち、体幹の補正方向と重心位置偏差の方向が同じであれば（ステップＳ４：Ｙｅｓ）、重心位置偏差に応じて調整係数を生成する (ステップＳ５)。符号が異なる場合は、調整係数は出力せず、そのままの補正量で体幹位置姿勢目標値の補正を行わせる。

なお、本実施の形態においては、体幹位置姿勢目標値をフィードバック制御する場合について説明したが、両足位置姿勢目標値も同様にフィードバック制御してもよいことは勿論である。例えば、足裏距離センサから足平と接地面の距離を求める。より正確には足平に固定された座標原点Ｏｆと接地面の間の距離を求める。この距離は、接地面に対する実足平位置を表す。目標足平位置と実足平位置の偏差に基づいて目標足平位置の補正量を算出する。算出された補正量を目標足平位置に加算した値、すなわち目標足平位置の補正量で補正された目標足平位置を関節角変換部１１３へ入力する。そのような制御系によって、実足平位置を目標足平位置に一致させることができる。例えば、実際のロボットの遊脚が実際の接地面に接地するタイミングを、歩容データ上で遊脚が接地面に着地するタイミングに一致させることができる。

本実施の形態においては、インピーダンス制御において、遊脚着地時に大きな外乱が生じると、胴体に加速度が発生し、その結果胴***置が補正される。しかし、遊脚着地位置が凹部の場合には、その補正によって、胴体が大きく外側に補正されることになり、結果として衝撃は吸収されるが重心位置がより大きくずれてしまい、その重心位置を回復させることができなくなる。そこで、重心位置偏差と加速度偏差を利用し、凹部に着地した場合を判定する。上述のように重心位置偏差と加速度偏差の符号が同一であれば、凹部に着地したと判定でき、この場合には、加速度偏差のフィードバック制御に用いる補正量を小さくしすることで、体幹位置姿勢目標値の補正量を小さくする。このことにより、凹部に着地した場合に補正量が大きくなって重心位置が外側に大きくなり回復できなくなるようなことを防止する。凹部に着地したと想定でき、補正量を小さくする。

これに対し、凸部に着地した場合、もともとの重心位置が目標に対して遅れているので、補正しても問題にならない。よって、凸部に着地した場合は、補正量をそのままとし、十分に外力を吸収し、足裏の路面に対する接地性を確保させる。

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

本発明の実施の形態にかかるロボットを正面から見た様子を概略的に表す概略図であり、 本発明の実施の形態にかかるロボットの制御システムの機能を示すブロック図である。 目標歩容と重心位置及び実際の歩容と重心位置を示す図である。 ロボットの胴***置補正及び加速度偏差の向きを示す図である。 重心位置偏差と調整係数の関係を示すグラフ図である。 本発明の実施の形態における調整係数生成部の動作を示すフローチャートである。 特許文献２に記載のロボットの制御システムを示す図である。

符号の説明

１ ロボット、２ 頭部、３ 体幹、
４ 腰部、５ 右腕、６ 左腕、
１０ 脚部、１１ 載置部、１７ 体幹加速度センサ、
２０ 右脚、２１ 右股関節、２２ 右上腿、
２３ 右膝関節、２４ 右下腿、２５ 右足首関節、
２６ 右足平、３０ 左脚、３１ 左股関節、
３２ 左上腿、３２ 加速度偏差算出部、３３ 左膝関節、
３４ 左下腿、３５ 左足首関節、３６ 左足平、
１１１ 歩容データ記憶部、１１２ 体幹歩容データ補正演算部、
１１３ 関節角変換部、１１４ 各軸制御器、
１１５ 体幹加速度計算部、１１６ 補正量変更部、１１７ センサ、
１２１ 目標重心位置計算部、１２２ 調整係数生成部、
１２３ 実重心位置計算部、１３１ 階微分演算部、
１３２ 加速度偏差算出部、１３３ 補正量算出部、１３４ 目標体幹位置姿勢補正部

Claims (16)

1. 脚式移動ロボットにおいて、
   目標とする目標体幹加速度と現在の体幹加速度との偏差である第１偏差を算出する加速度偏差算出手段と、
   目標とする目標重心位置と現在の重心位置との偏差である第２偏差を算出する重心位置偏差算出手段と、
   前記第１偏差及び第２偏差により歩容データ目標値を補正する補正手段とを有する脚式移動ロボット。
2. 前記補正手段は、前記歩容データ目標値を前記第１偏差に基づき補正する体幹歩容データ補正演算手段と、前記第１偏差及び第２偏差に基づき、前記体幹歩容データ補正演算手段の補正の度合を変更する補正量変更手段とを有する
   ことを特徴とする請求項１項記載の脚式移動ロボット。
3. 前記体幹歩容データ補正演算手段は、前記第１偏差から補正量を生成する補正量生成手段と、前記補正量生成手段が生成した補正量で前記歩容データ目標値を補正する目標値補正手段とを有する
   ことを特徴とする請求項２記載の脚式移動ロボット。
4. 前記補正量変更手段は、前記第１偏差及び第２偏差が同符号の場合にのみ前記歩容データ目標値の補正量を調整する調整係数を生成する
   ことを特徴とする請求項２又は３記載の脚式移動ロボット。
5. 前記補正量生成手段は、前記第１偏差に予め定められた比例ゲイン及び伝達関数、並びに前記補正量変更手段が生成した調整係数を乗算して補正量を生成し、
   前記補正量変更手段は、前記第２偏差が大きいほど前記補正量が小さくなるよう前記調整係数を変更する
   ことを特徴とする請求項３記載の脚式移動ロボット。
6. 前記補正量変更手段は、前記第２偏差が第１の閾値より小さい場合は調整係数を１とし、前記第１の閾値乃至第２の閾値の間は１から０への単調減少関数とし、前記第２の閾値以上の場合は前記調整係数をゼロとする
   ことを特徴とする請求項５記載の脚式移動ロボット。
7. 前記重心位置偏差算出手段の代わりに、目標胴***置と、現在の胴***置との偏差である第３偏差を算出する胴***偏差算出手段を設ける
   ことを特徴とする請求項１乃至６のいずれか１項記載の脚式移動ロボット。
8. 前記重心位置偏差算出手段の代わりに、目標胴体姿勢と、現在の胴体姿勢との偏差である第４偏差を算出する胴体姿勢位置偏差算出手段を設ける
   ことを特徴とする請求項１乃至６のいずれか１項記載の脚式移動ロボット。
9. 前記歩容データ目標値は、体幹位置姿勢目標値及び足先位置姿勢目標値である
   ことを特徴とする請求項１乃至８のいずれか１項記載の脚式移動ロボット。
10. 脚式移動ロボットの制御方法であって、
    目標体幹加速度と現在の体幹加速度との偏差である第１偏差を算出する加速度偏差算出工程と、
    目標重心位置と現在の重心位置との偏差である第２偏差を算出する重心位置偏差算出工程と、
    前記第１偏差及び第２偏差により歩容データ目標値を補正する補正工程とを有する脚式移動ロボットの制御方法。
11. 前記補正工程は、前記歩容データ目標値を前記第１偏差に基づき補正する体幹歩容データ補正演算工程と、前記第１偏差及び第２偏差に基づき、前記体幹歩容データ補正演算工程での補正の度合を変更する補正値変更工程とを有する
    ことを特徴とする請求項１０項記載の脚式移動ロボットの制御方法。
12. 前記体幹歩容データ補正演算工程は、前記第１偏差から補正量を生成する補正量生成工程と、前記補正量生成手段が生成した補正量で前記歩容データ目標値を補正する目標値補正工程とを有する
    ことを特徴とする請求項１１記載の脚式移動ロボットの制御方法。
13. 前記補正値変更工程では、前記第１偏差及び第２偏差が同符号の場合にのみ前記歩容データ目標値の補正の度合を変更する
    ことを特徴とする請求項１１又は１２記載の脚式移動ロボットの制御方法。
14. 前記補正量生成工程では、前記第１偏差に予め定められた比例ゲイン及び伝達関数、並びに前記補正値変更工程での調整係数を乗算して補正量を生成し、
    前記補正量変更工程は、前記第２偏差が大きいほど前記補正量が小さくなるよう前記調整係数を変更する
    ことを特徴とする請求項１３記載の脚式移動ロボットの制御方法。
15. 前記重心位置偏差算出工程の代わりに、目標胴***置と、現在の胴***置との偏差である第３偏差を算出する胴***偏差算出工程を設ける
    ことを特徴とする請求項１０乃至１４のいずれか１項記載の脚式移動ロボットの制御方法。
16. 前記重心位置偏差算出工程の代わりに、目標胴体姿勢と、現在の胴体姿勢との偏差である第４偏差を算出する胴体姿勢位置偏差算出工程を設ける
    ことを特徴とする請求項１０乃至１４のいずれか１項記載の脚式移動ロボットの制御方法。

Images