JP2008119763A - 二足歩行ロボットの制御装置 - Google Patents

Abstract

【課題】上体傾斜の制御と足平姿勢の制御が干渉することなく、少ない演算量で、片足支持期でも大きな床反力モーメントを得て外力に対して安定化することができ、水平方向の歩行速度を変えることなく、安定化することができる二足歩行ロボットの制御装置を提供する。
【解決手段】安定化制御器６１０は、歩容から目標ＺＭＰを算出する目標ＺＭＰ演算部１２０と、ＺＭＰ偏差指令値を算出する上体傾斜補償器１４０と、力センサにより検出された反力および反力作用点から実全床反力中心点を算出する実全床反力中心点演算部１５０と、目標ＺＭＰとＺＭＰ偏差指令値との和を全床反力中心点指令値とし、全床反力中心点指令値と実全床反力中心点とが一致するように足平姿勢を修正した修正目標足平姿勢を算出する足平姿勢補償器１６０を備える。
【選択図】図１

Description

本発明は、二足歩行ロボットの制御装置に関し、特に傾斜が検出された場合に姿勢を安定化する制御装置に関する。

従来の二足歩行ロボットの制御装置では、二足歩行ロボットの上体の傾斜を検出し、目標ＺＭＰまわりの補償全床反力モーメントを決定し、各足平位置を目標ＺＭＰまわりに所定角度回転させることによりその補償全床反力モーメントを得て安定化していた（例えば、特許文献１参照）。
ここで、目標ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）とは、ロボットが床面から得るべき目標重心加速度ベクトルの向きで重心を通る直線を考え、その直線と床面との交点を意味するものとする。ただしロボットは重力加速度を受けるため、例えばロボットの重心を加速させないためにロボットが床面から得るべき目標重心加速度は、重力加速度に等しい大きさで垂直方向上向きである。また、足裏に受けている全床反力をある１点にのみ受けているものとみなしてモーメントベクトルの水平方向成分が０となるように置き換えることができ、その点を実全床反力中心点と呼ぶ。
目標ＺＭＰと実全床反力中心点が一致しているときロボットが床から受けるモーメントの水平方向成分は０である。目標ＺＭＰと実全床反力中心点がずれているときには床から水平方向軸まわりのモーメントを受け、目標ＺＭＰと実全床反力中心点のずれ量が大きいほどそのモーメントは大きい。特許文献１における制御の操作量は目標ＺＭＰ周りの回転角であり、これは結果的に実全床反力中心点を動かす効果があるため、これにより目標ＺＭＰと実全床反力中心点のずれを作って安定化している。

以下では、図５を用いて従来の二足歩行ロボットの制御装置の全体構成を説明する。図５において、６００は歩容生成器であり、ロボットの動作パターンを表す歩容を生成する。特許文献１において歩容とは目標運動パターンと床反力パターンを合わせたものを指称しており、具体的には目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標全床反力中心点軌道、目標全床反力を合わせたものである。６１０は安定化制御器であり、予測しなかった外乱により歩行ロボットが転倒するのを防ぐため、各種センサ情報を用いて歩容生成器６００が生成した歩容を修正する。６２０はアクチュエータ指令生成器であり、安定化制御器６１０が生成した修正歩容を、逆キネマティクス演算により各関節のアクチュエータ指令に変換する。６３０はアクチュエータ制御器であり、アクチュエータセンサ６７０からのアクチュエータセンサ情報を用いて、アクチュエータ指令生成器６２０が生成したアクチュエータ指令どおりの位置にアクチュエータを動作させるようにアクチュエータ操作量を変化させる。６６０はアクチュエータおよび脚構造であり、アクチュエータ制御器６３０によって操作されるアクチュエータと脚構造である。脚構造は地面等と接触することにより二足歩行ロボットを移動させる力を発生する。

６７０はアクチュエータセンサであり、アクチュエータの状態量を検出し、アクチュエータ制御器６５へフィードバックする。具体的には、関節のアクチュエータとしてはサーボモータが使用される例が多く、その場合のアクチュエータ指令とはサーボモータの回転量をあらわす位置指令であり、アクチュエータ状態量とはサーボモータの回転量である。ロボットの関節の角度とサーボモータの回転量は一対一に対応する。アクチュエータセンサ情報とはサーボモータに取り付けられたエンコーダの信号である。アクチュエータおよび脚構造６６０が動作した結果、各種状態量が変化する。
６８０は各種センサであり、各種状態量を検出して安定化制御器にフィードバックする。具体的には、各種状態量とは二足歩行ロボットの上体の傾き、向き、またはそれらの速度、足裏に受ける圧力などであり、それらを検出する各種センサとしては、傾斜センサ、ジャイロセンサ、加速度センサ、地磁気センサ、圧力センサ、６軸力センサ、カメラなどが用いられる例が多い。また、こうした各種センサにより床面の傾斜や凹凸、壁のような障害物の位置や形状といった環境情報も得ることができる。

以下では、図６を用いて従来の二足歩行ロボットの安定化制御器についてより詳細に説明する。
図６において、安定化制御器６１０は歩容生成器６００から歩容を受け取る。ここで、歩容とは、目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標全床反力中心点軌道、目標全床反力をあわせたものを意味する。さらに安定化制御器６１０は、各種センサから各種センサ情報を受け取る。ここで、各種センサ情報とは、実各足平床反力、実ロボットの傾きずれ、上体Ｚ方向加速度をあわせたものを意味する。
歩容生成器が生成する歩容はダイナミクスを考慮した動力学シミュレーションや実験による試行錯誤の結果決定された安定な歩容であるが、床面の予期しない凹凸や傾斜があった場合や予期しない外力が加わった場合には必ずしも安定とはならない。安定化制御器６１０は、このような予期しない外乱があっても安定に歩行を持続するため、各種センサ情報を元に歩容を修正し、修正歩容をロボット幾何学モデル（逆キネマティクス演算部）７７０に渡す。ここで修正歩容とは、目標上体位置姿勢軌道と機構変形補償付き修正目標足平位置姿勢軌道をあわせたものを意味する。７７０は、ロボット幾何学モデル（逆キネマティクス演算部）であり、図５のアクチュエータ指令生成器６２０に相当する。図５のアクチュエータ指令は、図６では関節変位指令となっており、ロボットの姿勢が修正歩容で表される姿勢となるような各関節アクチュエータの変位量の指令値を逆キネマティクス演算により算出する。

安定化制御器６１０は、目標床反力分配器７２０、複合コンプライアンス動作決定部７３０、姿勢安定化制御演算部７４０、上体Ｚ方向速度制御演算部７５０からなる。目標床反力分配器７２０は、目標足平位置姿勢軌道、目標全床反力中心点軌道、目標全床反力から目標各足平床反力中心点および目標各足平床反力を算出する。複合コンプライアンス動作決定部７３０は、機構変形補償付き修正目標足平位置姿勢軌道を生成する。機構変形補償付き修正目標足平位置姿勢軌道とは、脚構造による弾性変形を考慮して修正目標足平位置姿勢軌道を修正したものである。修正目標足平位置とは、各種センサにより検出された実各足平床反力や実ロボットの傾きずれを補償するように目標足平位置姿勢軌道を修正したものである。実ロボットの傾きずれは、主に両脚補償角によって補償される。
ここで、両脚補償角とは、各足平位置を目標ＺＭＰまわりに所定角度回転させる角度である。各足平位置を目標ＺＭＰまわりに両脚補償角だけ回転させることにより両脚支持期の実全床反力中心点を移動させることができ、目標ＺＭＰと実全床反力中心点のずれによる床反力モーメントを得てロボットの姿勢を安定化できる。

特許文献１では、さらに各足平をそれぞれ目標各足平床反力中心点まわりに所定角度回転させることにより実各足平床反力モーメントの絶対値を小さくする制御をしているが、これは主に各足平の接地性を確保するための制御であり、上体の姿勢を安定化する床反力モーメントを得るものではない。特許文献１では、さらに各足平を同位相で平行移動させることにより実全床反力の力成分も制御している。これは主にＺ方向に大きな加速度を受けた場合に両足平位置を平行移動させることによりその衝撃をやわらげるための制御である。

また、従来の二足歩行ロボットの制御装置には、二足歩行ロボットの上体の傾斜または回転速度を検出し、指令との偏差が生じた場合には歩容関数の吐き出し速度を操作して安定化しているものもある（例えば、特許文献２参照）。歩容関数の吐き出し速度を変えると重心加速度が変化するため、目標ＺＭＰを動かす効果がある。これにより目標ＺＭＰと実全床反力中心点のずれが生じ、床反力モーメントを得ることができる。

このように、従来の二足歩行ロボットの制御装置は、各足平位置を目標ＺＭＰまわりに所定角度回転させることにより実全床反力中心点をずらすか、歩容関数の吐き出し速度を操作して目標ＺＭＰをずらして、床反力モーメントを得て安定化していた。
特許第３６２９１４２号公報（第２２頁、第２６頁、図４、図１８） 特許第３１６７４２０号公報（第１３頁、図９）

従来の二足歩行ロボットの制御装置における安定化制御では、片脚支持期には操作量は軸足の足平角度のみとなっているため、目標ＺＭＰと実全床反力中心点のずれ量は、実全床反力中心点が軸足足裏の接地部分を凹にならないように囲んだ図形の境界にあるときが最大となる。したがってこのとき床反力モーメントも最大となり、それ以上の床反力モーメントを得ることができないので、大きく姿勢を崩したときに転倒を回避するために得られる床反力モーメントが小さいという問題があった。両脚支持期には両足足裏の接地部分を凹にならないように囲んだ図形の範囲で実全床反力中心点を移動させることができるため、片足支持期に比べると大きなモーメントを得られるが、それでも足りない場合には転倒を避けられない。
また、上体姿勢の制御と各足平に受けるモーメントをできるだけ小さくする制御とが干渉しないようにするために、各足平位置を目標ＺＭＰまわりに両脚補償角だけ回転させる演算と、各足平をそれぞれ目標各足平床反力中心点まわりに所定角度回転させる演算が必要であり、高速な演算装置が必要になるという問題があった。
また、特許文献２のように歩容関数の吐き出し速度を変えて安定化する場合は、吐き出し速度を変えることにより意図した歩行速度にならなくなってしまうという問題点もあった。
本発明はこのような問題点に鑑みてなされたものであり、上体傾斜の制御と足平姿勢の制御が干渉することなく、少ない演算量で、片足支持期でも大きな床反力モーメントを得て外力に対して安定化することができ、水平方向の歩行速度を変えることなく、安定化することができる二足歩行ロボットの制御装置を提供することを目的とする。

上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、二足の脚構造と、前記脚構造を動作させる複数のアクチュエータと、前記脚構造の歩容を生成する歩容生成器と、前記脚構造の姿勢、前記脚構造に作用する外力、前記脚構造の環境情報のうち少なくとも１つの状態量を検出するセンサと、前記センサの検出結果をもとに前記歩容を修正した修正歩容を生成する安定化制御器と、前記修正歩容をもとに前記複数のアクチュエータそれぞれに対して位置、速度、力、電流のうち少なくとも１つの指令を生成するアクチュエータ指令生成器と、前記アクチュエータ指令どおりに前記アクチュエータを動作させるアクチュエータ制御器とを備えた二足歩行ロボットの制御装置において、前記センサは、前記ロボットの上体傾斜角を検出する傾斜センサまたは上体傾斜角速度を検出する傾斜角速度センサのいずれかと、前記脚構造が床などの構造物から受けている反力の大きさおよびその反力作用点を検出する力センサとを備えたことを特徴とするものである。

請求項２に記載の発明は、前記二足歩行ロボットの制御装置は、前記アクチュエータの位置、速度、力、電流のうち少なくとも１つのアクチュエータ状態量を検出するアクチュエータセンサを備えたことを特徴とするものである。

請求項３に記載の発明は、前記安定化制御器は、前記歩容から目標ＺＭＰを算出する目標ＺＭＰ演算部と、前記上体傾斜角を上体傾斜角指令値に追従させる床反力モーメントまたは前記上体傾斜角速度を上体傾斜角速度指令値に追従させる床反力モーメントを発生させるＺＭＰ偏差指令値を算出する上体傾斜補償器と、前記力センサにより検出された前記反力および反力作用点から実全床反力中心点を算出する実全床反力中心点演算部と、前記目標ＺＭＰと前記ＺＭＰ偏差指令値との和を全床反力中心点指令値とし、前記全床反力中心点指令値と前記実全床反力中心点とが一致するように足平姿勢を修正した修正目標足平姿勢を算出する足平姿勢補償器を備え、前記ロボットの足平姿勢が前記修正目標足平姿勢となる前記修正歩容を生成することを特徴とするものである。

請求項４に記載の発明は、前記安定化制御器は、前記実全床反力中心点から前記ＺＭＰ偏差指令値を差し引いた位置が前記目標ＺＭＰとなるように目標重心鉛直方向加速度を補正し、修正目標重心鉛直方向位置を算出する重心鉛直方向加速度補償器を備え、前記ロボットの重心鉛直方向位置が前記修正目標重心鉛直方向位置となるように前記修正歩容を生成することを特徴とするものである。

請求項５に記載の発明は、前記重心鉛直方向加速度補償器は、前記実全床反力中心点から前記ＺＭＰ偏差指令値を差し引いた位置が前記目標ＺＭＰとなるように前記目標重心鉛直方向加速度を補償する補償加速度を算出する重心鉛直方向加速度補償演算部を備え、前記目標重心鉛直方向加速度と前記補償加速度との和を積分して補償付き目標重心鉛直方向速度を算出し、前記補償付き目標重心鉛直方向速度を積分して補償付き目標重心鉛直方向位置を算出し、前記補償付き目標重心鉛直方向位置を修正目標重心鉛直方向位置とすることを特徴とするものである。

請求項６に記載の発明は、前記重心鉛直方向加速度補償器は、前記目標ＺＭＰと前記ＺＭＰ偏差指令値との和が所定の安定領域内にあるか否かを判定する判定器を備え、安定領域内であれば前記目標重心鉛直方向位置を前記修正目標重心鉛直方向位置とし、安定領域外であれば前記補償付き目標重心鉛直方向位置を前記修正目標重心鉛直方向位置とすることを特徴とするものである。

請求項７に記載の発明は、前記所定の安定領域は、前記二足歩行ロボットの足裏の領域または前記足裏の領域より小さい領域であることを特徴とするものである。

請求項８に記載の発明は、前記重心鉛直方向加速度補償器は、前記目標重心鉛直方向位置と前記補償付き目標重心鉛直方向位置との差に復帰ゲインを乗じて前記補償付き目標重心鉛直方向速度に加えることを特徴とするものである。

請求項１乃至３に記載の発明によると、上体傾斜の制御と足平姿勢の制御が干渉することなく、少ない演算量で、ロボットの姿勢を安定化することができる。また、請求項４乃至５に記載の発明によると、現在の実全床反力中心点で所望のＺＭＰ偏差が得られるように目標ＺＭＰを修正することができ、片足支持期に大きく姿勢を崩した場合でも安定化することができる。また、請求項６乃至７に記載の発明によると、大きく姿勢を崩した場合にのみ重心鉛直方向加速度が変化するため、通常の歩行時は鉛直方向の位置を指令値通りにすることができる。また、請求項８に記載の発明によると、安定化のために変化した重心鉛直方向位置を徐々に修正前の歩容に戻すことができる。

以下、本発明の実施の形態について図を参照して説明する。

図１は、本発明の二足歩行ロボットの制御装置のブロック図である。図において、６００は歩容生成器であり、ロボットの動作パターンを表す歩容を生成する。本実施例において歩容とは、目標足平位置、目標足平姿勢、目標重心位置、上体傾斜角指令値あるいは上体傾斜角速度指令値の組を意味する。目標足平位置はロボットの両足足先部分の位置指令値を表し、本実施例ではそれぞれの足裏の中央位置を表すものとする。目標重心位置はロボット全体の重心位置指令値を表す座標位置である。
以下では、ロボットが歩行している床面上の適切な位置に設定された点を原点とし、鉛直上方向をＺ軸、進行方向前方をＸ軸、進行方向に対し左方向をＹ軸とした座標系を定義し、目標足平位置および目標重心位置はこの座標系で表されるものとする。歩容についてはシミュレーションや実験によって、予期しない外乱がない限り安定となるような歩行パターンをあらかじめ決定しておく。歩容の生成方法については、例えば特許第３６５８１４７号、特許第３７２６０８１号等に詳しく述べられている方法を用いればよい。歩容の生成方法は本発明の特徴ではないので省略する。

６１０は安定化制御器である。安定化制御器は予期しない凹凸や傾斜などによって二足歩行ロボットの歩行が不安定になり最悪の場合転倒するといったことが起こらないよう、センサ情報を元に歩容を修正するための装置である。本発明の安定化制御器は、重心鉛直方向加速度補償器、目標ＺＭＰ演算部、上体傾斜補償器、実全床反力中心点演算部、足平姿勢補償器からなる。
１１０は重心鉛直方向加速度補償器であり、目標ＺＭＰ、ＺＭＰ偏差指令値、実全床反力中心点の情報を用いて目標重心Ｚ位置を修正した修正目標重心Ｚ位置を算出する。この処理の詳細については後述する。
１２０は目標ＺＭＰ演算部であり、歩容生成器が生成した歩容の目標重心位置から目標ＺＭＰを算出する。目標重心ＸＹ位置を(ｘ_Ｇ、ｙ_Ｇ)、目標重心Ｚ位置をｚ_Ｇとし、目標重心ＸＹ加速度を（ａ_ｘＧ、ａ_ｙＧ）、目標重心Ｚ加速度をａ_ｚＧとすると、ＺＭＰの位置ベクトル（ｘ_ＺＭＰ、ｙ_ＺＭＰ）は次の式（１）で算出できる。ただし、目標重心ＸＹ加速度は目標重心ＸＹ位置を２階微分した値であり、目標重心Ｚ加速度は目標重心Ｚ位置を２階微分した値に重力加速度を加えた値である。

１４０は上体傾斜補償器であり、ロボット上体に設置された傾斜センサにより検出された上体傾斜角を目標上体傾斜角に追従させる床反力モーメントを発生させるためのＺＭＰ偏差指令値を算出する。ＺＭＰ偏差指令値はＸ方向成分とＹ方向成分を持つ位置ベクトルである。
なお、図１では傾斜センサを用いた例を示しているが、傾斜センサの代わりに傾斜角速度センサにより検出された上体傾斜角速度を目標上体傾斜角速度に追従させるようにし、そのための床反力モーメントを発生させるＺＭＰ偏差指令値を算出するようにしてもよい。
ここで、ＺＭＰ偏差指令値、目標ＺＭＰ、実全床反力中心点の関係を図４を用いて説明する。図は、歩行中のロボットを左（Ｙ軸正方向）から見た様子を表しており、ＺＭＰ偏差指令値、目標ＺＭＰ、実全床反力中心点のＸ方向成分をそれぞれ図示している。Ｙ方向成分についても同様に考えればよい。前述のように目標ＺＭＰとは、ロボットが床面から得るべき目標重心加速度ベクトルの向きで重心を通る直線を考え、その直線と床面との交点を意味する。ただし、目標重心Ｚ加速度には重力加速度を含む。加速する車両上を歩行する場合などはさらに慣性力による加速度を加えて考えればよい。目標ＺＭＰと実全床反力中心点が一致していれば、ロボットはモーメントを受けないが、一致しない場合はそのずれ量にほぼ比例したモーメントを受ける。
そこで本実施例では、目標ＺＭＰと実全床反力中心点とのずれをＺＭＰ偏差指令値とし、Ｘ軸まわりの傾斜角の制御はＺＭＰ偏差指令値のＹ方向成分を操作量とし、Ｙ軸まわりの傾斜角の制御はＺＭＰ偏差指令値のＸ方向成分を操作量とするものとした。目標ＺＭＰにＺＭＰ偏差指令値を加えた位置を全床反力中心点指令値とし、実全床反力中心点をこの位置に一致させれば所望のモーメントを得ることができる。上体傾斜角を目標上体傾斜角に追従させる制御には従来のＰＩＤ制御などを用いればよい。歩行中に上体の傾斜角を特に変える必要がない場合は、目標上体傾斜角を常に０として直立状態を維持させるようにしてもよい。

図１に戻って説明を続ける。１５０は実全床反力中心点演算部であり、力センサにより検出された足裏圧力から実全床反力中心点を算出する。実全床反力中心点を求めるためのセンサの構成や算出方法は様々なものが考えられるが、例えば足裏にｎ個（ｎ：自然数）の圧力センサを設置し、それぞれの検出した力をｆ_ｉ、それぞれの設置された位置をＸ座標とＹ座標により（ｘ_ｉ、ｙ_ｉ）とすれば（ｉ＝１、２・・・ｎ）、実全床反力中心点（ｘ_ＲＺＭＰ、ｙ_ＲＺＭＰ）は、次の式（２）で算出できる。

１６０は足平姿勢補償器であり、目標ＺＭＰとＺＭＰ偏差指令値との和を全床反力中心点指令値とし、全床反力中心点指令値と実全床反力中心点とが一致するように足平姿勢を修正した修正目標足平姿勢を算出する。この処理の詳細については後述する。

図２は、図１に示した重心鉛直方向加速度補償器１１０内のブロック図である。
以下では、図２を用いて重心鉛直方向加速度補償器１１０の動作の詳細を説明する。
２１０は判定器であり、目標ＺＭＰとＺＭＰ偏差指令値との和が安定領域内にあるかどうかを判定し、安定領域内であれば目標重心Ｚ位置（前述のｚ_Ｇ）をそのまま修正目標重心Ｚ位置とし、安定領域外であれば補償付き目標重心Ｚ位置を修正目標重心Ｚ位置とする。実全床反力中心点が足裏の範囲を超えて動かすことができないため、目標ＺＭＰとＺＭＰ偏差指令値との和で表される実床反力中心点指令値が足裏の範囲を超えてしまえば必ず実全床反力中心点とのずれが生じてしまい、不安定になる。そのため、実床反力中心点指令値が必ず足裏の範囲内になければならい。したがって安定領域は足裏範囲内とするか、あるいは少し余裕を持たせて足裏のエッジ付近をさけた設定とする。例えば足裏形状が長方形であれば、実床反力中心点指令値のＸ座標とＹ座標それぞれについて、あらかじめ設定した境界値を超えていないかを判定することで安定領域内かどうかを判定することができる。
２２０は２階微分器１であり、目標重心Ｚ位置（前述のｚ_Ｇ）の２階微分値である目標重心Ｚ加速度（前述のａ_ｚＧ）を算出する。２２１は２階微分器２であり、目標重心ＸＹ位置（前述の(ｘ_Ｇ、ｙ_Ｇ)）の２階微分値である目標重心ＸＹ加速度（前述の（ａ_ｘＧ、ａ_ｙＧ））を算出する。
２４０は重心鉛直方向加速度補償演算部であり、修正目標ＺＭＰから補償加速度を算出する。ただし、修正目標ＺＭＰとは、実全床反力中心点の位置からＺＭＰ偏差指令値を差し引いた位置とする。重心鉛直方向加速度補償演算部では、まず実床反力中心点指令値の位置のＸ座標とＹ座標のうち、どちらの方が不安定な位置にあるかを判定し、どちらを元に補償加速度を算出するのかを決定する。判定方法は、例えば領域境界までの距離が小さい方が不安定であるとすればよい。
修正ＺＭＰの位置ベクトルを（ｘ_ＭＺＭＰ、ｙ_ＭＺＭＰ）とし、目標重心位置を(ｘ_Ｇ、ｙ_Ｇ、ｚ_Ｇ)、目標重心ＸＹ加速度を（ａ_ｘＧ、ａ_ｙＧ）とすると、Ｘ座標を元に算出することが決定された場合の補償加速度ａ_ＣＺＭＰは、以下の式（３）で算出する。

Ｙ座標を元に補償加速度ａ_ＣＺＭＰを算出する場合は以下の式（４）で算出する。

２３０は積分器１であり、目標重心Ｚ加速度と補償加速度との和を積分する。
２５０は復帰ゲインであり、目標重心Ｚ位置と補償付き目標重心Ｚ位置との差に一定値をかけて出力する。この演算は、姿勢安定化のための補償によってずれた重心Ｚ位置を徐々に目標重心Ｚ位置に戻すためのものであり、復帰ゲインの値を大きくするほど補償付き目標重心Ｚ位置を目標重心Ｚ位置に戻す速度が速くなる。
２３１は積分器２であり、積分器１の出力値と復帰ゲインの出力値の和を積分し、補償付き目標重心Ｚ位置を算出する。

図３は、図１に示した足平姿勢補償器１６０内のブロック図である。
以下では図３を用いて足平姿勢補償器１６０の動作を説明する。足平姿勢補償器１６０は、目標足平姿勢、目標ＺＭＰ、ＺＭＰ偏差指令値、実全床反力中心点から修正目標足平姿勢を算出する。足平姿勢補償器１６０は、まず目標ＺＭＰとＺＭＰ偏差指令値との和を求め、全床反力中心点指令値とする。３００は実全床反力制御部であり、実全床反力中心点を全床反力中心点指令値に追従させるように足平姿勢補正値を出力する。制御演算は従来から一般によく用いられているＰＩＤ制御などで実現すればよい。
足平姿勢補正値は、足平姿勢のＸ軸まわり角度および足平姿勢のＹ軸まわり角度で表される。実全床反力中心点と全床反力中心点指令値との偏差のＸ成分は足平姿勢補正値のＹ軸まわり角度を操作量として制御でき、Ｙ成分は足平姿勢補正値のＸ軸まわり角度を操作量として制御できる。これは、足構造や床面に多少なりとも必ず弾性変形要素があり、足平姿勢を変えることで床面から受ける圧力分布が変わり、実全床反力中心点が移動するためである。最後に、目標足平姿勢と足平姿勢補正値との和を修正目標足平姿勢とする。

本発明が従来技術と異なる部分は、安定化制御器内の演算で目標ＺＭＰと実全床反力中心点を算出しており、検出された上体傾斜角を指令値に追従させるための操作量をＺＭＰ偏差指令値としている点と、目標ＺＭＰとＺＭＰ偏差指令値との和を実全床反力中心点と一致させるように足平姿勢を決定している点と、検出された上体傾斜角を指令値に追従させるために目標重心加速度の水平方向成分でなく垂直方向成分を補正している点である。

このように、上体傾斜角の制御を行う上体傾斜補償器の出力する操作量をＺＭＰ偏差指令値とし、実全床反力中心点の制御を行う足平姿勢補償器の出力する操作量を足平角度とするように構成しているので、足平姿勢補償器は目標ＺＭＰとＺＭＰ偏差指令値との和に実全床反力中心点が追従するように制御するだけでよく、上体傾斜角の制御と実全床反力中心点の制御とが干渉することなくロボットの姿勢を安定化することができる。また、重心鉛直方向加速度補償器を設け、目標重心Ｚ位置を修正するように構成しているので、外乱等により大きくバランスを崩し、目標ＺＭＰとＺＭＰ偏差指令値との和が安定領域からはずれるような場合でも水平方向の目標速度を維持したままでロボットの姿勢を安定化することができる。

本発明の第１実施例を示す二足歩行ロボットの安定化制御器のブロック図 本発明の第１実施例の重心鉛直方向加速度補償器のブロック図 本発明の第１実施例の足平姿勢補償器のブロック図 目標ＺＭＰとＺＭＰ偏差指令値と実全床反力中心点の関係の説明図 従来の二足歩行ロボットの制御装置の全体構成を表すブロック図 従来の二足歩行ロボットの安定化制御器のブロック図

符号の説明

１１０ 重心鉛直方向加速度補償器
１２０ 目標ＺＭＰ演算部
１４０ 上体傾斜補償器
１５０ 実全床反力中心点演算部
１６０ 足平姿勢補償器
２２０ ２階微分器１
２２１ ２階微分器２
２３０ 積分器１
２３１ 積分器２
２４０ 重心鉛直方向加速度補償演算部
２５０ 復帰ゲイン
３００ 実全床反力制御部
４１０ 胴体
４２０ 脚構造
４３０ 足平
４４０ 路面
６００ 歩容生成器
６１０ 安定化制御器
６２０ アクチュエータ指令生成器
６３０ アクチュエータ指令制御器
６６０ アクチュエータおよび脚構造
６７０ アクチュエータセンサ
６８０ 各種センサ
７２０ 目標床反力分配器
７３０ 複合コンプライアンス動作決定部
７４０ 姿勢安定化制御演算部
７５０ 上体Ｚ方向速度制御演算部
７７０ ロボット幾何学モデル(逆キネマティクス演算部)

Claims (8)

1. 二足の脚構造と、
   前記脚構造を動作させる複数のアクチュエータと、
   前記脚構造の歩容を生成する歩容生成器と、
   前記脚構造の姿勢、前記脚構造に作用する外力、前記脚構造の環境情報のうち少なくとも１つの状態量を検出するセンサと、
   前記センサの検出結果をもとに前記歩容を修正した修正歩容を生成する安定化制御器と、
   前記修正歩容をもとに前記複数のアクチュエータそれぞれに対して位置、速度、力、電流のうち少なくとも１つの指令を生成するアクチュエータ指令生成器と、
   前記アクチュエータ指令どおりに前記アクチュエータを動作させるアクチュエータ制御器とを備えた二足歩行ロボットの制御装置において、
   前記センサは、前記ロボットの上体傾斜角を検出する傾斜センサまたは上体傾斜角速度を検出する傾斜角速度センサのいずれかと、前記脚構造が床などの構造物から受けている反力の大きさおよびその反力作用点を検出する力センサとを備えたことを特徴とする二足歩行ロボットの制御装置。
2. 前記二足歩行ロボットの制御装置は、前記アクチュエータの位置、速度、力、電流のうち少なくとも１つのアクチュエータ状態量を検出するアクチュエータセンサを備えたことを特徴とする請求項１記載の二足歩行ロボットの制御装置。
3. 前記安定化制御器は、前記歩容から目標ＺＭＰを算出する目標ＺＭＰ演算部と、
   前記上体傾斜角を上体傾斜角指令値に追従させる床反力モーメントまたは前記上体傾斜角速度を上体傾斜角速度指令値に追従させる床反力モーメントを発生させるＺＭＰ偏差指令値を算出する上体傾斜補償器と、
   前記力センサにより検出された前記反力および反力作用点から実全床反力中心点を算出する実全床反力中心点演算部と、
   前記目標ＺＭＰと前記ＺＭＰ偏差指令値との和を全床反力中心点指令値とし、前記全床反力中心点指令値と前記実全床反力中心点とが一致するように足平姿勢を修正した修正目標足平姿勢を算出する足平姿勢補償器を備え、
   前記ロボットの足平姿勢が前記修正目標足平姿勢となる前記修正歩容を生成することを特徴とする請求項１記載の二足歩行ロボットの制御装置。
4. 前記安定化制御器は、前記実全床反力中心点から前記ＺＭＰ偏差指令値を差し引いた位置が前記目標ＺＭＰとなるように目標重心鉛直方向加速度を補正し、修正目標重心鉛直方向位置を算出する重心鉛直方向加速度補償器を備え、
   前記ロボットの重心鉛直方向位置が前記修正目標重心鉛直方向位置となるように前記修正歩容を生成することを特徴とする請求項３記載の二足歩行ロボットの制御装置。
5. 前記重心鉛直方向加速度補償器は、前記実全床反力中心点から前記ＺＭＰ偏差指令値を差し引いた位置が前記目標ＺＭＰとなるように前記目標重心鉛直方向加速度を補償する補償加速度を算出する重心鉛直方向加速度補償演算部を備え、
   前記目標重心鉛直方向加速度と前記補償加速度との和を積分して補償付き目標重心鉛直方向速度を算出し、前記補償付き目標重心鉛直方向速度を積分して補償付き目標重心鉛直方向位置を算出し、前記補償付き目標重心鉛直方向位置を修正目標重心鉛直方向位置とすることを特徴とする請求項４記載の二足歩行ロボットの制御装置。
6. 前記重心鉛直方向加速度補償器は、前記目標ＺＭＰと前記ＺＭＰ偏差指令値との和が所定の安定領域内にあるか否かを判定する判定器を備え、
   安定領域内であれば前記目標重心鉛直方向位置を前記修正目標重心鉛直方向位置とし、安定領域外であれば前記補償付き目標重心鉛直方向位置を前記修正目標重心鉛直方向位置とすることを特徴とする請求項５記載の二足歩行ロボットの制御装置。
7. 前記所定の安定領域は、前記二足歩行ロボットの足裏の領域または前記足裏の領域より小さい領域であることを特徴とする請求項６記載の二足歩行ロボットの制御装置。
8. 前記重心鉛直方向加速度補償器は、前記目標重心鉛直方向位置と前記補償付き目標重心鉛直方向位置との差に復帰ゲインを乗じて前記補償付き目標重心鉛直方向速度に加えることを特徴とする請求項５記載の二足歩行ロボットの制御装置。

Images