脚式歩行ロボットの安定化制御装置
【課題】 前後方向と左右方向の安定化制御が互いに影響し合うことなく，足裏の小さいロボットでも前後左右からの外乱に対して高い安定性で動作することができる脚式歩行ロボットの安定化制御器を提供する
【解決手段】 上体傾斜角，上体傾斜角速度，重心位置補正量，重心速度補正量の４つを状態変数とし，重心加速度補正量を操作量とした状態方程式でロボットをモデル化する。前後方向，左右方向それぞれについて，この状態方程式を用いてレギュレータを構成し，計８つの状態変数全てを０に収束させる。
【解決手段】 上体傾斜角，上体傾斜角速度，重心位置補正量，重心速度補正量の４つを状態変数とし，重心加速度補正量を操作量とした状態方程式でロボットをモデル化する。前後方向，左右方向それぞれについて，この状態方程式を用いてレギュレータを構成し，計８つの状態変数全てを０に収束させる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、脚式歩行ロボットの姿勢の傾斜を検出して安定化する脚式歩行ロボットの安定化制御器に関する。
【背景技術】
【０００２】
従来の脚式歩行ロボットの安定化制御器は、脚式歩行ロボットの上体の傾斜を検出し，目標ＺＭＰまわりの補償全床反力モーメントを決定し，各足平位置を目標ＺＭＰまわりに所定角度回転させることにより、その補償全床反力モーメントを得て安定化していた。（例えば、特許文献１参照）ここで，目標ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ，ゼロモーメントポイント）とは，ロボットが床面から得るべき目標重心加速度ベクトルの向きで重心を通る直線を考え，その直線と床面との交点を意味するものとする。ただし，ロボットは重力加速度を受けるため，例えばロボットの重心を加速させないためにロボットが床面から得るべき目標重心加速度は，重力加速度に等しい大きさで垂直方向上向きである。また，足裏に受けている全床反力をある１点にのみ受けているものとみなしてモーメントベクトルの水平方向成分が０となるように置き換えることができ，その点を実全床反力中心点と呼ぶ。目標ＺＭＰと実全床反力中心点が一致しているときロボットが床から受けるモーメントの水平方向成分は０である。ずれているときには床から水平方向軸まわりのモーメントを受け，目標ＺＭＰと実全床反力中心点のずれ量が大きいほどそのモーメントは大きい。特許文献１の制御の操作量は、目標ＺＭＰ周りの回転角であり、これは結果的に実全床反力中心点を動かす効果があるため，これにより目標ＺＭＰと実全床反力中心点のずれをつくって安定化している。
【０００３】
以下では，図３を用いて従来の脚式歩行ロボットの制御装置の全体構成を説明する。図３において，６００は歩容生成器であり、ベースとなるロボットの動作パターンを表すベース歩容を生成する。特許文献１では、ベース歩容は目標運動パターンと床反力パターンを合わせたものを指称しており、具体的には目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標全床反力中心点軌道、目標全床反力を合わせたものである。６１０は安定化制御器であり、予測しなかった外乱により歩行ロボットが転倒するのを防ぐため，各種センサ情報を用いて歩容生成器６００が生成したベース歩容を修正し，修正歩容を生成する。６２０は逆キネマティクス演算器であり，安定化制御器６１０が生成した修正歩容を，逆キネマティクス演算により各関節のモータ位置指令に変換する。６３０はモータ位置制御器であり，エンコーダ６７０からのモータ位置情報を用いて，逆キネマティクス演算器６２０が生成したモータ位置指令通りの位置にモータを動作させるようにモータトルクを変化させる。６６０はモータおよび脚構造であり，モータ位置制御器６３０によって操作されるモータと脚構造である。脚構造は地面等と接触することにより脚式歩行ロボットを移動させる力を発生する。６７０はエンコーダであり，モータ位置を検出し，モータ位置制御器６５へフィードバックする。ロボットの関節の角度とモータの回転量は一対一に対応する。モータおよび脚構造６６０が動作した結果，各種状態量が変化する。６８０は各種センサであり，各種状態量を検出して安定化制御器にフィードバックする。具体的には，各種状態量とは脚式歩行ロボットの上体の傾き，向き，またはそれらの速度，足裏に受ける圧力であり，各種センサとは，傾斜センサ，ジャイロセンサ，加速度センサ，地磁気センサ，圧力センサ，６軸力センサ，カメラなどが用いられる例が多い。
【０００４】
以下では，図４を用いて従来の脚式歩行ロボットの安定化制御器についてより詳細に説明する。
図４において、安定化制御器６１０は歩容生成器６００からベース歩容を受け取る。ここで，ベース歩容とは，目標上体位置姿勢軌道，目標足平位置姿勢軌道，目標全床反力中心点軌道，目標全床反力をあわせたものを意味する。さらに安定化制御器６１０は，各種センサから各種センサ情報を受け取る。ここで，各種センサ情報とは，実各足平床反力，実ロボットの傾きずれ，上体Ｚ方向加速度をあわせたものを意味する。歩容生成器が生成するベース歩容は，ダイナミクスを考慮した動力学シミュレーションや実験による試行錯誤の結果決定された安定な歩容であるが，床面の予期しない凹凸や傾斜があった場合や予期しない外力が加わった場合には必ずしも安定とはならない。安定化制御器６１０は，このような予期しない外乱があっても安定に歩行を持続するため，各種センサ情報を元に歩容を修正し，修正歩容を逆キネマティクス演算器６２０に渡す。ここで，修正歩容とは，目標上体位置姿勢軌道と機構変形補償付き修正目標足平位置姿勢軌道をあわせたものを意味する。６２０は逆キネマティクス演算器であり，ロボットの姿勢が修正歩容で表される姿勢となるような各関節モータの位置指令を逆キネマティクス演算により算出する。安定化制御器６１０は，目標床反力分配器７２０，複合コンプライアンス動作決定部７３０，姿勢安定化制御演算部７４０，上体Ｚ方向速度制御演算部７５０からなる。目標床反力分配器７２０は，目標足平位置姿勢軌道，目標全床反力中心点軌道，目標全床反力から目標各足平床反力中心点および目標各足平床反力を算出する。複合コンプライアンス動作決定部７３０は，機構変形補償付き修正目標足平位置姿勢軌道を生成する。機構変形補償付き修正目標足平位置姿勢軌道とは，脚構造による弾性変形を考慮して修正目標足平位置姿勢軌道を修正したものである。修正目標足平位置とは，各種センサにより検出された実各足平床反力や実ロボットの傾きずれを補償するように目標足平位置姿勢軌道を修正したものである。実ロボットの傾きずれは，主に両脚補償角によって補償される。ここで，両脚補償角とは，各足平位置を目標ＺＭＰまわりに所定角度回転させる角度である。各足平位置を目標ＺＭＰまわりに両脚補償角だけ回転させることにより両脚支持期の実全床反力中心点を移動させることができ，目標ＺＭＰと実全床反力中心点のずれによる床反力モーメントを得てロボットの姿勢を安定化できる。
特許文献１では、さらに各足平を、それぞれ目標各足平床反力中心点まわりに所定角度回転させることにより実各足平床反力モーメントの絶対値を小さくする制御をしているが、これは主に各足平の接地性を確保するための制御であり，上体の姿勢を安定化する床反力モーメントを得るものではない。特許文献１では、さらに各足平を同位相で平行移動させることにより、実全床反力の力成分も制御している。これは、主にＺ方向に大きな加速度を受けた場合に両足平位置を平行移動させることによりその衝撃をやわらげるための制御である。
また、従来の脚式歩行ロボットの安定化制御器は、脚式歩行ロボットの上体の傾斜または回転速度を検出し，指令との偏差が生じた場合には歩容関数の吐き出し速度を操作して安定化しているものもある（例えば、特許文献２参照）。歩容関数の吐き出し速度を変えると重心加速度が変化するため，目標ＺＭＰを動かす効果がある。これにより目標ＺＭＰと実全床反力中心点のずれが生じ，床反力モーメントを得ることができる。
このように、従来の脚式歩行ロボットの安定化制御器は、各足平位置を目標ＺＭＰまわりに所定角度回転させることにより実全床反力中心点をずらすか、歩容関数の吐き出し速度を操作することにより重心加速度を変えて床反力モーメントを得て安定化するのである。
【特許文献１】特許第３６２９１４２号公報（第２２頁、図４および第２６頁、図１８）
【特許文献２】特許第３１６７４２０号公報（第１３頁、図９）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
従来の脚式歩行ロボットの安定化制御器は、上体傾斜の制御のための操作量を，各足平位置を目標ＺＭＰまわりに回転させる角度としているが，この方法では片脚支持期に変化させられるモーメントの大きさは足裏の大きさによって決定されるため，足裏のサイズが小さいロボットでは，安定化のためのモーメントをあまり大きくできないという問題があった。また、特許文献２のように歩容関数の吐き出し速度を変えて安定化する場合には，重心の加速によって大きなモーメントを得ることができるが，本従来技術では前後方向と左右方向の加速度を独立して変えることができないため，例えば左右方向を安定化するために吐き出し速度を変えると前後方向がかえって不安定になったり，逆に前後方向を安定化するために左右方向がかえって不安定になったりするという問題点があった。
本発明はこのような問題点に鑑みてなされたものであり、前後方向と左右方向の安定化制御が互いに影響し合うことなく，足裏の小さいロボットでも前後左右からの外乱に対して高い安定性で動作することができる脚式歩行ロボットの安定化制御器を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、脚構造と，前記脚構造を動作させるための複数のアクチュエータと，前記アクチュエータの位置を検出してアクチュエータ位置情報を生成するためのエンコーダと，ベースとなる動作パターンであるベース歩容を生成する歩容生成器と，姿勢または外力または環境情報のうちのいずれかまたはすべてを含む各種状態量を検出して各種センサ情報を生成する各種センサと、前記各種センサ情報を元に前記ベース歩容を修正した修正歩容を生成する安定化制御器と，前記修正歩容をもとに前記複数のアクチュエータそれぞれに対する位置指令を生成する逆キネマティクス演算器と，前記アクチュエータ位置指令通りにアクチュエータを動作させるためのアクチュエータ位置制御器とを備えた脚式歩行ロボットの制御装置において、前記各種センサは，少なくとも上体傾斜角を検出する傾斜センサと上体傾斜角速度を検出する傾斜角速度センサとを備え，前記安定化制御器は，少なくとも前記上体傾斜角と，前記上体傾斜角速度と，ベース歩容の重心位置からの重心位置補正量と，ベース歩容の重心速度からの重心速度補正量と，を状態変数とし，重心加速度補正量を操作量とした状態フィードバックで構成されるレギュレータを備えたとするものである。
また、請求項２に記載の発明は、前記重心速度補正量は前記重心加速度補正量の積分値であり，前記安定化制御器は，前記重心加速度補正量から重心速度補正量を算出する積分演算器を備えたとするものである。
また、請求項３に記載の発明は、前記重心位置補正量は前記重心速度補正量の積分値であり，前記安定化制御器は，前記重心速度補正量から重心位置補正量を算出する積分演算器を備えたとするものである。
また、請求項４に記載の発明は、上体に対する相対的な支持脚足平の加速度を支持脚足平相対加速度とし，前記ベース歩容の前記支持脚足平相対加速度を目標支持脚足平相対加速度とし，前記修正歩容の前記支持脚足平相対加速度を修正目標支持脚足平相対加速度とし，前記安定化制御器は，前記重心加速度補正量を目標支持脚足平相対加速度から減算して修正目標支持脚足平相対加速度を生成することにより所望の前記重心加速度補正量を発生させるとするものである。
また、請求項５に記載の発明は、上体に対する相対的な支持脚足平の位置を支持脚足平相対位置とし，前記ベース歩容の前記支持脚足平相対位置を目標支持脚足平相対位置とし，前記修正歩容の前記支持脚足平相対位置を修正目標支持脚足平相対位置とし，前記安定化制御器は，２つの積分演算器を備え，前記重心加速度補正量の２階積分値を目標支持脚足平相対位置から減算することで修正目標支持脚足平相対位置を生成するとするものである。
【発明の効果】
【０００７】
請求項１乃至５に記載の発明によると，水平方向重心加速度を操作量とすることにより，足首角度を操作量とする場合に比べ大きな復元モーメントを得ることができる。また，その際，従来技術のように前後方向と左右方向の安定化制御が互いに影響しないため，前後方向と左右方向について，それぞれ上体傾斜角，上体傾斜角速度，重心位置補正量，重心速度補正量の４つずつ，計８つの状態変数を同時に安定的に０に収束させることができる。
【発明を実施するための最良の形態】
【０００８】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【０００９】
本実施例のロボット制御装置の全体構成は，図３に示されるものと同じである。本発明の特徴は図３で安定化制御器６１０の部分にある。図１は、本発明の脚式歩行ロボットの安定化制御器をより詳細に示したブロック図である。図において、６００は歩容生成器であり，ベースとなる動作パターンを表すベース歩容を生成する。本実施例では，ベース歩容には，少なくとも目標支持脚足平相対位置を含む。また，通常，目標遊脚足平相対位置も含まれる。ここで，支持脚とは路面と接地している脚，遊脚とは空中に浮いている脚を意味する。両方の脚が接地している場合には，遊脚はなく，両方が支持脚となる。目標支持脚足平相対位置と目標遊脚足平相対位置は，それぞれ支持脚と遊脚の足裏中央位置指令値および足裏面の角度指令値の組を表すものとする。以下では，ロボットが歩行している床面上のどこかに設定された点を原点とした絶対座標系と，ロボットの重心位置に固定した相対座標系を定義する。相対座標系は，ロボットが直立した状態で，鉛直上方向をＺ軸，進行方向前方をＸ軸，進行方向に対し左方向をＹ軸と定義する。目標支持脚足平相対位置および目標遊脚足平相対位置は，相対座標系で与えられるものとする。絶対座標系で指令を与えたい場合でも，歩容は幾何学的な関係から容易に相対座標系に変換できるので，歩容生成器の演算の最後で変換すればよい。以下では，特に断りがない限り，絶対座標系のＸ軸およびＹ軸と相対座標系のＸ軸およびＹ軸はそれぞれ一致しているものとする。脚が上体に比べ軽い場合，重心位置は上体の中の固定点とみなすことができ，目標支持脚足平相対位置は上体に対する相対位置と考えてもよい。ベース歩容はシミュレーションや実験による試行錯誤で，予期しない外乱がない限り安定となるような歩行パターンをあらかじめ決定しておく。ベース歩容の生成方法については，例えば本出願人が過去に出願した特願２００６−２６０８６７，特願２００７−２４２０４０等に詳しく述べられている方法を用いればよい。この方法では，モデル化誤差や外乱がないとすれば，常に足裏の中央がＺＭＰとなるような歩容を生成する。この方法では，ベース歩容は，絶対座標で表したＺＭＰ位置および重心位置の軌道として生成されるが，それらの相対座標系での差ベクトルを求めることで本発明のベース歩容として用いることができる。歩容の生成方法は本発明の特徴ではないので省略する。６１０は安定化制御器である。安定化制御器は予期しない凹凸や傾斜などによって，不安定になり最悪の場合転倒するといったことが起こらないよう，センサ情報を元に歩容を修正するための装置である。本発明の安定化制御器は，安定化レギュレータと２つの積分演算器からなる。１１は安定化レギュレータであり，傾斜センサやジャイロセンサにより検出された上体傾斜角および上体傾斜角速度と，重心位置補正量，重心速度補正量をもとに重心加速度補正量を算出する。本実施例では，重心加速度補正量が一般の制御理論で言う操作量であり，上体傾斜角，上体傾斜角速度，重心位置補正量，重心速度補正量の４つが状態変数となる。安定化レギュレータは，４つの状態変数が安定的に０に収束するように操作量を決定する。安定化レギュレータの詳細な演算内容については後述する。本実施例では，目標とする上体傾斜角を常に０として直立状態を維持させるようにしているが，上体傾斜角の指令値を与えたい場合には，指令値との偏差を状態量とし，安定化レギュレータをサーボ系の構成としてもよい。１２は積分演算器であり，重心加速度補正量を積分して重心速度補正量を算出する。１３は積分演算器であり，重心速度補正量を積分して重心位置補正量を算出する。算出された重心位置補正量は，目標支持脚足平相対位置から減算し，修正目標支持脚足平相対位置とする。これは，重心加速度補正量だけ重心を加速するために，支持脚を重心加速度補正量の分だけ逆方向に動かすことに相当する。目標遊脚足平相対位置については，修正せずそのまま出力する。本技術の安定化制御器の演算は，当然，従来の安定化制御装置の演算と組み合わせることもできるものもあり，その場合には足裏角度なども修正される。６２０は逆キネマティクス演算器であり，受け取った修正歩容の修正目標支持脚足平相対位置を逆キネマティクス演算により各関節の角度に換算してモータ位置指令を算出する。
次に，図２を用いて安定化レギュレータの演算内容とその導出の根拠を以下に説明する。図２は，平坦な路面を歩行中の脚式歩行ロボットを右からみた図である。２１はロボットの上体，２２は脚構造，２３は支持脚足平，２４は遊脚足平である。それぞれ簡単のため単純化して図示しているが，脚構造は実際には多数の関節を備え，上体と足平との相対位置や相対角度を変えることができる。重量ｍのロボットは，重力加速度ｇによる重力ｍｇを重心位置に受ける。脚構造や足平が上体に比べて軽い場合，重心位置は上体の中のある固定点と置き換えて考えてもよい。また，床面からは垂直成分ｆｚ，水平成分ｆｘの反力を受けているとする。このとき，ｆｘとｆｚの合成ベクトルの向きで，重心を通る直線を引き，路面と交わった点が目標ＺＭＰである。実床反力中心点が常に目標ＺＭＰと一致していれば，ロボットは回転しない。しかし，実際には，予期しない路面の凹凸や外力などによりモーメントが発生し，ロボットは回転し，上体が傾斜する。
重心まわりの慣性モーメントをIｙ，上体のピッチ傾斜角をθｙ，絶対座標重心位置を（ｘＣＯＧ，ｚＣＯＧ），絶対座標目標ＺＭＰを（ｘＺＭＰ，０）とおく。このとき，以下の式で表される運動方程式が成り立つ。
【００１０】
【数１】
【００１１】
ただし，変数の上のドットはその変数の微分値を表す。すなわち，２つのドットはその変数の２階微分値を表す。図２のように前方すなわちピッチ角のプラス方向に傾斜したとすると，この傾斜を戻すためにピッチ角のマイナス方向の復元モーメントを床面から得る必要がある。その方法として，従来技術の多くは，足首角度を変えることにより復元モーメントを得ている。しかし，足首角度を変えて得られるモーメントは（１）式の第１式右辺で，ｘＺＭＰを最大でも足裏サイズ分だけ変化させる効果しかないため，あまり大きな復元モーメントを得ることはできない。ｆｘを操作すれば，より大きな復元モーメントを得ることができる。ｆｘを直接操作することが難しい場合，（１）式の第２式の関係より，重心加速度を操作しても同様の効果を得ることができると考えられる。従来技術でも，重心を加速することにより復元モーメントを得る方法はあったが，前述のように前後方向と左右方向の安定化制御が干渉するなどの問題があった。本発明はこのような問題を解決できる。ここで，重心位置を次式のようにおく。
【００１２】
【数２】
【００１３】
ただし，ｘＣＯＧｂａｓｅはベース歩容で与えられる重心位置のｘ座標，ｘΔは安定化のためにベース歩容からずらすｘ軸方向の補正量を表す。図２では，ベース歩容をそのまま再生した場合のロボットの位置を実線で，ベース歩容に補正量を加えた補正後のロボット位置を破線で示している。上式を２階微分すると次式を得る。
【００１４】
【数３】
【００１５】
ベース歩容は，前述の特願２００６−２６０８６７や特願２００７−２４２０４０により，以下のＺＭＰ方程式（床反力のモーメントが０になる条件式）を満たすように生成する。
【００１６】
【数４】
【００１７】
以上の関係を用いて，次式が得られる。
【００１８】
【数５】
【００１９】
上式より，次の状態方程式が得られる。
【００２０】
【数６】
【００２１】
上式のベクトルや行列を次式のようにおく。以下，太字はベクトルまたは行列を表す。
【００２２】
【数７】
【００２３】
以下の状態フィードバックによりレギュレータを構成する。ただし，ｋ１，ｋ２，ｋ３，ｋ４は状態フィードバックゲインである。
【００２４】
【数８】
【００２５】
ｋ１，ｋ２，ｋ３，ｋ４は試行錯誤的に調整してもよいが，ここでは，調整パラメータλを導入し，次式のように重根となるように極配置法によりｋ１，ｋ２，ｋ３，ｋ４を以下のように設定する。
【００２６】
【数９】
【００２７】
このようにすることで１つの調整パラメータを調整するだけで容易に安定化制御の調整ができる。λを大きくするほど高周波数の外乱でも抑制できるようになるが，大きくしすぎると制御系が不安定になり，振動したり発散したりするため，実験やシミュレーションの結果を見て振動や発散が起こらない範囲で適切な値に設定する。ベース歩容の重心位置ｚ座標を一定としておくと，重心加速度のｚ成分が０となるので，ｋ１，ｋ２，ｋ３，ｋ４が定数となり簡単になる。上記は前後方向の制御であるが，横方向も同様に制御する。ｘ軸をｙ軸に置き替え，ピッチ傾斜角をロール角に置き換えれば，ほぼ同様に考えることができる。ロール・ピッチ・ヨーの角度の定義上，符号が逆になる部分があるが，それ以外は本質的に同じ式であるため省略する。
本発明が従来技術と異なる部分は、水平方向重心加速度を操作量とし，上体傾斜角，上体傾斜角速度，重心位置補正量，重心速度補正量の４つを状態変数としてレギュレータにより安定化制御を実現している部分である。水平方向重心加速度を操作量とすることにより，足首角度を操作量とする場合に比べ大きな復元モーメントを得ることができる。また，その際，従来技術のように前後方向と左右方向の安定化制御が互いに影響しないため，レギュレータにより計８つの状態変数を同時に安定的に０に収束させることができる。仮に，姿勢を安定化するために単純に重心加速度を操作したとすれば，その加速度により重心の位置はベース歩容とかけ離れた位置になってしまう。本発明では，安定化レギュレータを用いることにより，重心位置のベース歩容からの偏差と，姿勢の傾斜角とを同時に０に収束させることができる。
【実施例２】
【００２８】
重心加速度補正量を操作量とした状態方程式を求めてレギュレータを構成していたが，実際に重心加速度補正量だけ重心を加速するためには，工夫が必要である。実施例１では，支持脚足平を重心加速度補正量の分だけ逆方向に動かすことによりこれを実現していた。しかし，実際にはロール・ピッチの回転があるため，支持脚足平の上体に対する相対加速度と重心の加速度の大きさは完全には一致しない。また，ベース歩容を生成する際には，ピッチ角θｙが０のときに，ＺＭＰ方程式を満たすように生成するため，ピッチ角が０でないときには補正量が０でもモーメントが発生してしまう。本実施例では，これらのことを考慮して状態方程式を用いてさらに安定化性能を高めた方法を説明する。また，本実施例では，支持脚足平すなわちＺＭＰの上体に対する相対加速度の補正量を操作量とした例を示す。支持脚が着地してから離床するまでの間，絶対座標で見た支持脚足平位置は変わらないため，支持脚足平相対加速度補正量を操作することは，それと逆向きに重心加速度補正量を操作することとほぼ等価と考えてよい。本実施例の全体構成は，実施例１と同様に図３および図１で表されるようにすればよい。絶対座標重心位置を（ｘＣＯＧ，ｙＣＯＧ，ｚＣＯＧ），絶対座標目標ＺＭＰを（ｘＺＭＰ，ｙＺＭＰ，０），相対座標目標ＺＭＰを（ＸＺＭＰ，ＹＺＭＰ，ＺＺＭＰ）とおく。ロール・ピッチ・ヨーの変換を線形近似し、ヨー角を零とすると以下のように表せる。
【００２９】
【数１０】
【００３０】
（３）式の１行目を展開し，２階微分すると以下のようになる。
【００３１】
【数１１】
【００３２】
（３）式の３行目を展開し，以下のように近似する。
【００３３】
【数１２】
【００３４】
また，（２）式より，以下のようになる。
【００３５】
【数１３】
【００３６】
ベース歩容では，ピッチ角およびピッチ角加速度を０と仮定しているため，以下のようになる。
【００３７】
【数１４】
【００３８】
ここで，ベース歩容の相対座標目標ＺＭＰのＸ座標をＸＺＭＰbaseとおき，それと実際のＺＭＰの相対位置偏差をＸΔとおくと次式のようになる。
【００３９】
【数１５】
【００４０】
これらと（１）式より，以下のようになる。
【００４１】
【数１６】
【００４２】
上式より，次の状態方程式が得られる。ただし，重心のＺ方向加速度は０とした。
【００４３】
【数１７】
【００４４】
以下の状態フィードバックによりレギュレータを構成する。ただし，ｋ１，ｋ２，ｋ３，ｋ４は状態フィードバックゲインである。
【００４５】
【数１８】
【００４６】
上記の状態方程式を用いて，実施例１と同様に状態フィードバックのゲインベクトルを決定すれば状態変数を全て同時に０に収束させることができる。上記は前後方向の制御であるが，横方向も同様に制御すればよい。
本発明が従来技術と異なる部分は、水平方向支持脚足平相対加速度を操作量とし，上体傾斜角，上体傾斜角速度，ＺＭＰ相対位置偏差，ＺＭＰ相対速度偏差の４つを状態変数としてレギュレータにより安定化制御を実現している部分である。ＺＭＰ相対位置偏差，ＺＭＰ相対速度偏差は，それぞれ重心位置補正量，重心速度補正量の符号を反転したものとみなしても技術的にはほぼ等価である。また，ＺＭＰを支持脚足平と置き換えても等価である。水平方向支持脚足平相対加速度を操作量とすることにより，足首角度を操作量とする場合に比べ大きな復元モーメントを得ることができる。また，その際，従来技術のように前後方向と左右方向の安定化制御が互いに影響しないため，レギュレータにより計８つの状態変数を同時に安定的に０に収束させることができる。
【図面の簡単な説明】
【００４７】
【図１】本発明の第１実施例を示す脚式歩行ロボットの安定化制御器のブロック図
【図２】本発明の安定化制御による生成される動作の例を示す説明図
【図３】従来の脚式歩行ロボットの制御装置の全体構成を表すブロック図
【図４】従来の脚式歩行ロボットの安定化制御器のブロック図
【符号の説明】
【００４８】
１１ 安定化レギュレータ
１２ 積分演算器
１３ 積分演算器
２１ 上体
２２ 脚構造
２３ 支持脚足平
２４ 遊脚足平
２５ 路面
６００ 歩容生成器
６１０ 安定化制御器
６２０ 逆キネマティクス演算器
６３０ モータ位置制御器
６６０ モータおよび脚構造
６７０ エンコーダ
６８０ 各種センサ
７２０ 目標床反力分配器
７３０ 複合コンプライアンス動作決定部
７４０ 姿勢安定化制御演算部
７５０ 上体Ｚ方向速度制御演算部
【技術分野】
【０００１】
本発明は、脚式歩行ロボットの姿勢の傾斜を検出して安定化する脚式歩行ロボットの安定化制御器に関する。
【背景技術】
【０００２】
従来の脚式歩行ロボットの安定化制御器は、脚式歩行ロボットの上体の傾斜を検出し，目標ＺＭＰまわりの補償全床反力モーメントを決定し，各足平位置を目標ＺＭＰまわりに所定角度回転させることにより、その補償全床反力モーメントを得て安定化していた。（例えば、特許文献１参照）ここで，目標ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ，ゼロモーメントポイント）とは，ロボットが床面から得るべき目標重心加速度ベクトルの向きで重心を通る直線を考え，その直線と床面との交点を意味するものとする。ただし，ロボットは重力加速度を受けるため，例えばロボットの重心を加速させないためにロボットが床面から得るべき目標重心加速度は，重力加速度に等しい大きさで垂直方向上向きである。また，足裏に受けている全床反力をある１点にのみ受けているものとみなしてモーメントベクトルの水平方向成分が０となるように置き換えることができ，その点を実全床反力中心点と呼ぶ。目標ＺＭＰと実全床反力中心点が一致しているときロボットが床から受けるモーメントの水平方向成分は０である。ずれているときには床から水平方向軸まわりのモーメントを受け，目標ＺＭＰと実全床反力中心点のずれ量が大きいほどそのモーメントは大きい。特許文献１の制御の操作量は、目標ＺＭＰ周りの回転角であり、これは結果的に実全床反力中心点を動かす効果があるため，これにより目標ＺＭＰと実全床反力中心点のずれをつくって安定化している。
【０００３】
以下では，図３を用いて従来の脚式歩行ロボットの制御装置の全体構成を説明する。図３において，６００は歩容生成器であり、ベースとなるロボットの動作パターンを表すベース歩容を生成する。特許文献１では、ベース歩容は目標運動パターンと床反力パターンを合わせたものを指称しており、具体的には目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標全床反力中心点軌道、目標全床反力を合わせたものである。６１０は安定化制御器であり、予測しなかった外乱により歩行ロボットが転倒するのを防ぐため，各種センサ情報を用いて歩容生成器６００が生成したベース歩容を修正し，修正歩容を生成する。６２０は逆キネマティクス演算器であり，安定化制御器６１０が生成した修正歩容を，逆キネマティクス演算により各関節のモータ位置指令に変換する。６３０はモータ位置制御器であり，エンコーダ６７０からのモータ位置情報を用いて，逆キネマティクス演算器６２０が生成したモータ位置指令通りの位置にモータを動作させるようにモータトルクを変化させる。６６０はモータおよび脚構造であり，モータ位置制御器６３０によって操作されるモータと脚構造である。脚構造は地面等と接触することにより脚式歩行ロボットを移動させる力を発生する。６７０はエンコーダであり，モータ位置を検出し，モータ位置制御器６５へフィードバックする。ロボットの関節の角度とモータの回転量は一対一に対応する。モータおよび脚構造６６０が動作した結果，各種状態量が変化する。６８０は各種センサであり，各種状態量を検出して安定化制御器にフィードバックする。具体的には，各種状態量とは脚式歩行ロボットの上体の傾き，向き，またはそれらの速度，足裏に受ける圧力であり，各種センサとは，傾斜センサ，ジャイロセンサ，加速度センサ，地磁気センサ，圧力センサ，６軸力センサ，カメラなどが用いられる例が多い。
【０００４】
以下では，図４を用いて従来の脚式歩行ロボットの安定化制御器についてより詳細に説明する。
図４において、安定化制御器６１０は歩容生成器６００からベース歩容を受け取る。ここで，ベース歩容とは，目標上体位置姿勢軌道，目標足平位置姿勢軌道，目標全床反力中心点軌道，目標全床反力をあわせたものを意味する。さらに安定化制御器６１０は，各種センサから各種センサ情報を受け取る。ここで，各種センサ情報とは，実各足平床反力，実ロボットの傾きずれ，上体Ｚ方向加速度をあわせたものを意味する。歩容生成器が生成するベース歩容は，ダイナミクスを考慮した動力学シミュレーションや実験による試行錯誤の結果決定された安定な歩容であるが，床面の予期しない凹凸や傾斜があった場合や予期しない外力が加わった場合には必ずしも安定とはならない。安定化制御器６１０は，このような予期しない外乱があっても安定に歩行を持続するため，各種センサ情報を元に歩容を修正し，修正歩容を逆キネマティクス演算器６２０に渡す。ここで，修正歩容とは，目標上体位置姿勢軌道と機構変形補償付き修正目標足平位置姿勢軌道をあわせたものを意味する。６２０は逆キネマティクス演算器であり，ロボットの姿勢が修正歩容で表される姿勢となるような各関節モータの位置指令を逆キネマティクス演算により算出する。安定化制御器６１０は，目標床反力分配器７２０，複合コンプライアンス動作決定部７３０，姿勢安定化制御演算部７４０，上体Ｚ方向速度制御演算部７５０からなる。目標床反力分配器７２０は，目標足平位置姿勢軌道，目標全床反力中心点軌道，目標全床反力から目標各足平床反力中心点および目標各足平床反力を算出する。複合コンプライアンス動作決定部７３０は，機構変形補償付き修正目標足平位置姿勢軌道を生成する。機構変形補償付き修正目標足平位置姿勢軌道とは，脚構造による弾性変形を考慮して修正目標足平位置姿勢軌道を修正したものである。修正目標足平位置とは，各種センサにより検出された実各足平床反力や実ロボットの傾きずれを補償するように目標足平位置姿勢軌道を修正したものである。実ロボットの傾きずれは，主に両脚補償角によって補償される。ここで，両脚補償角とは，各足平位置を目標ＺＭＰまわりに所定角度回転させる角度である。各足平位置を目標ＺＭＰまわりに両脚補償角だけ回転させることにより両脚支持期の実全床反力中心点を移動させることができ，目標ＺＭＰと実全床反力中心点のずれによる床反力モーメントを得てロボットの姿勢を安定化できる。
特許文献１では、さらに各足平を、それぞれ目標各足平床反力中心点まわりに所定角度回転させることにより実各足平床反力モーメントの絶対値を小さくする制御をしているが、これは主に各足平の接地性を確保するための制御であり，上体の姿勢を安定化する床反力モーメントを得るものではない。特許文献１では、さらに各足平を同位相で平行移動させることにより、実全床反力の力成分も制御している。これは、主にＺ方向に大きな加速度を受けた場合に両足平位置を平行移動させることによりその衝撃をやわらげるための制御である。
また、従来の脚式歩行ロボットの安定化制御器は、脚式歩行ロボットの上体の傾斜または回転速度を検出し，指令との偏差が生じた場合には歩容関数の吐き出し速度を操作して安定化しているものもある（例えば、特許文献２参照）。歩容関数の吐き出し速度を変えると重心加速度が変化するため，目標ＺＭＰを動かす効果がある。これにより目標ＺＭＰと実全床反力中心点のずれが生じ，床反力モーメントを得ることができる。
このように、従来の脚式歩行ロボットの安定化制御器は、各足平位置を目標ＺＭＰまわりに所定角度回転させることにより実全床反力中心点をずらすか、歩容関数の吐き出し速度を操作することにより重心加速度を変えて床反力モーメントを得て安定化するのである。
【特許文献１】特許第３６２９１４２号公報（第２２頁、図４および第２６頁、図１８）
【特許文献２】特許第３１６７４２０号公報（第１３頁、図９）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
従来の脚式歩行ロボットの安定化制御器は、上体傾斜の制御のための操作量を，各足平位置を目標ＺＭＰまわりに回転させる角度としているが，この方法では片脚支持期に変化させられるモーメントの大きさは足裏の大きさによって決定されるため，足裏のサイズが小さいロボットでは，安定化のためのモーメントをあまり大きくできないという問題があった。また、特許文献２のように歩容関数の吐き出し速度を変えて安定化する場合には，重心の加速によって大きなモーメントを得ることができるが，本従来技術では前後方向と左右方向の加速度を独立して変えることができないため，例えば左右方向を安定化するために吐き出し速度を変えると前後方向がかえって不安定になったり，逆に前後方向を安定化するために左右方向がかえって不安定になったりするという問題点があった。
本発明はこのような問題点に鑑みてなされたものであり、前後方向と左右方向の安定化制御が互いに影響し合うことなく，足裏の小さいロボットでも前後左右からの外乱に対して高い安定性で動作することができる脚式歩行ロボットの安定化制御器を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、脚構造と，前記脚構造を動作させるための複数のアクチュエータと，前記アクチュエータの位置を検出してアクチュエータ位置情報を生成するためのエンコーダと，ベースとなる動作パターンであるベース歩容を生成する歩容生成器と，姿勢または外力または環境情報のうちのいずれかまたはすべてを含む各種状態量を検出して各種センサ情報を生成する各種センサと、前記各種センサ情報を元に前記ベース歩容を修正した修正歩容を生成する安定化制御器と，前記修正歩容をもとに前記複数のアクチュエータそれぞれに対する位置指令を生成する逆キネマティクス演算器と，前記アクチュエータ位置指令通りにアクチュエータを動作させるためのアクチュエータ位置制御器とを備えた脚式歩行ロボットの制御装置において、前記各種センサは，少なくとも上体傾斜角を検出する傾斜センサと上体傾斜角速度を検出する傾斜角速度センサとを備え，前記安定化制御器は，少なくとも前記上体傾斜角と，前記上体傾斜角速度と，ベース歩容の重心位置からの重心位置補正量と，ベース歩容の重心速度からの重心速度補正量と，を状態変数とし，重心加速度補正量を操作量とした状態フィードバックで構成されるレギュレータを備えたとするものである。
また、請求項２に記載の発明は、前記重心速度補正量は前記重心加速度補正量の積分値であり，前記安定化制御器は，前記重心加速度補正量から重心速度補正量を算出する積分演算器を備えたとするものである。
また、請求項３に記載の発明は、前記重心位置補正量は前記重心速度補正量の積分値であり，前記安定化制御器は，前記重心速度補正量から重心位置補正量を算出する積分演算器を備えたとするものである。
また、請求項４に記載の発明は、上体に対する相対的な支持脚足平の加速度を支持脚足平相対加速度とし，前記ベース歩容の前記支持脚足平相対加速度を目標支持脚足平相対加速度とし，前記修正歩容の前記支持脚足平相対加速度を修正目標支持脚足平相対加速度とし，前記安定化制御器は，前記重心加速度補正量を目標支持脚足平相対加速度から減算して修正目標支持脚足平相対加速度を生成することにより所望の前記重心加速度補正量を発生させるとするものである。
また、請求項５に記載の発明は、上体に対する相対的な支持脚足平の位置を支持脚足平相対位置とし，前記ベース歩容の前記支持脚足平相対位置を目標支持脚足平相対位置とし，前記修正歩容の前記支持脚足平相対位置を修正目標支持脚足平相対位置とし，前記安定化制御器は，２つの積分演算器を備え，前記重心加速度補正量の２階積分値を目標支持脚足平相対位置から減算することで修正目標支持脚足平相対位置を生成するとするものである。
【発明の効果】
【０００７】
請求項１乃至５に記載の発明によると，水平方向重心加速度を操作量とすることにより，足首角度を操作量とする場合に比べ大きな復元モーメントを得ることができる。また，その際，従来技術のように前後方向と左右方向の安定化制御が互いに影響しないため，前後方向と左右方向について，それぞれ上体傾斜角，上体傾斜角速度，重心位置補正量，重心速度補正量の４つずつ，計８つの状態変数を同時に安定的に０に収束させることができる。
【発明を実施するための最良の形態】
【０００８】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【０００９】
本実施例のロボット制御装置の全体構成は，図３に示されるものと同じである。本発明の特徴は図３で安定化制御器６１０の部分にある。図１は、本発明の脚式歩行ロボットの安定化制御器をより詳細に示したブロック図である。図において、６００は歩容生成器であり，ベースとなる動作パターンを表すベース歩容を生成する。本実施例では，ベース歩容には，少なくとも目標支持脚足平相対位置を含む。また，通常，目標遊脚足平相対位置も含まれる。ここで，支持脚とは路面と接地している脚，遊脚とは空中に浮いている脚を意味する。両方の脚が接地している場合には，遊脚はなく，両方が支持脚となる。目標支持脚足平相対位置と目標遊脚足平相対位置は，それぞれ支持脚と遊脚の足裏中央位置指令値および足裏面の角度指令値の組を表すものとする。以下では，ロボットが歩行している床面上のどこかに設定された点を原点とした絶対座標系と，ロボットの重心位置に固定した相対座標系を定義する。相対座標系は，ロボットが直立した状態で，鉛直上方向をＺ軸，進行方向前方をＸ軸，進行方向に対し左方向をＹ軸と定義する。目標支持脚足平相対位置および目標遊脚足平相対位置は，相対座標系で与えられるものとする。絶対座標系で指令を与えたい場合でも，歩容は幾何学的な関係から容易に相対座標系に変換できるので，歩容生成器の演算の最後で変換すればよい。以下では，特に断りがない限り，絶対座標系のＸ軸およびＹ軸と相対座標系のＸ軸およびＹ軸はそれぞれ一致しているものとする。脚が上体に比べ軽い場合，重心位置は上体の中の固定点とみなすことができ，目標支持脚足平相対位置は上体に対する相対位置と考えてもよい。ベース歩容はシミュレーションや実験による試行錯誤で，予期しない外乱がない限り安定となるような歩行パターンをあらかじめ決定しておく。ベース歩容の生成方法については，例えば本出願人が過去に出願した特願２００６−２６０８６７，特願２００７−２４２０４０等に詳しく述べられている方法を用いればよい。この方法では，モデル化誤差や外乱がないとすれば，常に足裏の中央がＺＭＰとなるような歩容を生成する。この方法では，ベース歩容は，絶対座標で表したＺＭＰ位置および重心位置の軌道として生成されるが，それらの相対座標系での差ベクトルを求めることで本発明のベース歩容として用いることができる。歩容の生成方法は本発明の特徴ではないので省略する。６１０は安定化制御器である。安定化制御器は予期しない凹凸や傾斜などによって，不安定になり最悪の場合転倒するといったことが起こらないよう，センサ情報を元に歩容を修正するための装置である。本発明の安定化制御器は，安定化レギュレータと２つの積分演算器からなる。１１は安定化レギュレータであり，傾斜センサやジャイロセンサにより検出された上体傾斜角および上体傾斜角速度と，重心位置補正量，重心速度補正量をもとに重心加速度補正量を算出する。本実施例では，重心加速度補正量が一般の制御理論で言う操作量であり，上体傾斜角，上体傾斜角速度，重心位置補正量，重心速度補正量の４つが状態変数となる。安定化レギュレータは，４つの状態変数が安定的に０に収束するように操作量を決定する。安定化レギュレータの詳細な演算内容については後述する。本実施例では，目標とする上体傾斜角を常に０として直立状態を維持させるようにしているが，上体傾斜角の指令値を与えたい場合には，指令値との偏差を状態量とし，安定化レギュレータをサーボ系の構成としてもよい。１２は積分演算器であり，重心加速度補正量を積分して重心速度補正量を算出する。１３は積分演算器であり，重心速度補正量を積分して重心位置補正量を算出する。算出された重心位置補正量は，目標支持脚足平相対位置から減算し，修正目標支持脚足平相対位置とする。これは，重心加速度補正量だけ重心を加速するために，支持脚を重心加速度補正量の分だけ逆方向に動かすことに相当する。目標遊脚足平相対位置については，修正せずそのまま出力する。本技術の安定化制御器の演算は，当然，従来の安定化制御装置の演算と組み合わせることもできるものもあり，その場合には足裏角度なども修正される。６２０は逆キネマティクス演算器であり，受け取った修正歩容の修正目標支持脚足平相対位置を逆キネマティクス演算により各関節の角度に換算してモータ位置指令を算出する。
次に，図２を用いて安定化レギュレータの演算内容とその導出の根拠を以下に説明する。図２は，平坦な路面を歩行中の脚式歩行ロボットを右からみた図である。２１はロボットの上体，２２は脚構造，２３は支持脚足平，２４は遊脚足平である。それぞれ簡単のため単純化して図示しているが，脚構造は実際には多数の関節を備え，上体と足平との相対位置や相対角度を変えることができる。重量ｍのロボットは，重力加速度ｇによる重力ｍｇを重心位置に受ける。脚構造や足平が上体に比べて軽い場合，重心位置は上体の中のある固定点と置き換えて考えてもよい。また，床面からは垂直成分ｆｚ，水平成分ｆｘの反力を受けているとする。このとき，ｆｘとｆｚの合成ベクトルの向きで，重心を通る直線を引き，路面と交わった点が目標ＺＭＰである。実床反力中心点が常に目標ＺＭＰと一致していれば，ロボットは回転しない。しかし，実際には，予期しない路面の凹凸や外力などによりモーメントが発生し，ロボットは回転し，上体が傾斜する。
重心まわりの慣性モーメントをIｙ，上体のピッチ傾斜角をθｙ，絶対座標重心位置を（ｘＣＯＧ，ｚＣＯＧ），絶対座標目標ＺＭＰを（ｘＺＭＰ，０）とおく。このとき，以下の式で表される運動方程式が成り立つ。
【００１０】
【数１】
【００１１】
ただし，変数の上のドットはその変数の微分値を表す。すなわち，２つのドットはその変数の２階微分値を表す。図２のように前方すなわちピッチ角のプラス方向に傾斜したとすると，この傾斜を戻すためにピッチ角のマイナス方向の復元モーメントを床面から得る必要がある。その方法として，従来技術の多くは，足首角度を変えることにより復元モーメントを得ている。しかし，足首角度を変えて得られるモーメントは（１）式の第１式右辺で，ｘＺＭＰを最大でも足裏サイズ分だけ変化させる効果しかないため，あまり大きな復元モーメントを得ることはできない。ｆｘを操作すれば，より大きな復元モーメントを得ることができる。ｆｘを直接操作することが難しい場合，（１）式の第２式の関係より，重心加速度を操作しても同様の効果を得ることができると考えられる。従来技術でも，重心を加速することにより復元モーメントを得る方法はあったが，前述のように前後方向と左右方向の安定化制御が干渉するなどの問題があった。本発明はこのような問題を解決できる。ここで，重心位置を次式のようにおく。
【００１２】
【数２】
【００１３】
ただし，ｘＣＯＧｂａｓｅはベース歩容で与えられる重心位置のｘ座標，ｘΔは安定化のためにベース歩容からずらすｘ軸方向の補正量を表す。図２では，ベース歩容をそのまま再生した場合のロボットの位置を実線で，ベース歩容に補正量を加えた補正後のロボット位置を破線で示している。上式を２階微分すると次式を得る。
【００１４】
【数３】
【００１５】
ベース歩容は，前述の特願２００６−２６０８６７や特願２００７−２４２０４０により，以下のＺＭＰ方程式（床反力のモーメントが０になる条件式）を満たすように生成する。
【００１６】
【数４】
【００１７】
以上の関係を用いて，次式が得られる。
【００１８】
【数５】
【００１９】
上式より，次の状態方程式が得られる。
【００２０】
【数６】
【００２１】
上式のベクトルや行列を次式のようにおく。以下，太字はベクトルまたは行列を表す。
【００２２】
【数７】
【００２３】
以下の状態フィードバックによりレギュレータを構成する。ただし，ｋ１，ｋ２，ｋ３，ｋ４は状態フィードバックゲインである。
【００２４】
【数８】
【００２５】
ｋ１，ｋ２，ｋ３，ｋ４は試行錯誤的に調整してもよいが，ここでは，調整パラメータλを導入し，次式のように重根となるように極配置法によりｋ１，ｋ２，ｋ３，ｋ４を以下のように設定する。
【００２６】
【数９】
【００２７】
このようにすることで１つの調整パラメータを調整するだけで容易に安定化制御の調整ができる。λを大きくするほど高周波数の外乱でも抑制できるようになるが，大きくしすぎると制御系が不安定になり，振動したり発散したりするため，実験やシミュレーションの結果を見て振動や発散が起こらない範囲で適切な値に設定する。ベース歩容の重心位置ｚ座標を一定としておくと，重心加速度のｚ成分が０となるので，ｋ１，ｋ２，ｋ３，ｋ４が定数となり簡単になる。上記は前後方向の制御であるが，横方向も同様に制御する。ｘ軸をｙ軸に置き替え，ピッチ傾斜角をロール角に置き換えれば，ほぼ同様に考えることができる。ロール・ピッチ・ヨーの角度の定義上，符号が逆になる部分があるが，それ以外は本質的に同じ式であるため省略する。
本発明が従来技術と異なる部分は、水平方向重心加速度を操作量とし，上体傾斜角，上体傾斜角速度，重心位置補正量，重心速度補正量の４つを状態変数としてレギュレータにより安定化制御を実現している部分である。水平方向重心加速度を操作量とすることにより，足首角度を操作量とする場合に比べ大きな復元モーメントを得ることができる。また，その際，従来技術のように前後方向と左右方向の安定化制御が互いに影響しないため，レギュレータにより計８つの状態変数を同時に安定的に０に収束させることができる。仮に，姿勢を安定化するために単純に重心加速度を操作したとすれば，その加速度により重心の位置はベース歩容とかけ離れた位置になってしまう。本発明では，安定化レギュレータを用いることにより，重心位置のベース歩容からの偏差と，姿勢の傾斜角とを同時に０に収束させることができる。
【実施例２】
【００２８】
重心加速度補正量を操作量とした状態方程式を求めてレギュレータを構成していたが，実際に重心加速度補正量だけ重心を加速するためには，工夫が必要である。実施例１では，支持脚足平を重心加速度補正量の分だけ逆方向に動かすことによりこれを実現していた。しかし，実際にはロール・ピッチの回転があるため，支持脚足平の上体に対する相対加速度と重心の加速度の大きさは完全には一致しない。また，ベース歩容を生成する際には，ピッチ角θｙが０のときに，ＺＭＰ方程式を満たすように生成するため，ピッチ角が０でないときには補正量が０でもモーメントが発生してしまう。本実施例では，これらのことを考慮して状態方程式を用いてさらに安定化性能を高めた方法を説明する。また，本実施例では，支持脚足平すなわちＺＭＰの上体に対する相対加速度の補正量を操作量とした例を示す。支持脚が着地してから離床するまでの間，絶対座標で見た支持脚足平位置は変わらないため，支持脚足平相対加速度補正量を操作することは，それと逆向きに重心加速度補正量を操作することとほぼ等価と考えてよい。本実施例の全体構成は，実施例１と同様に図３および図１で表されるようにすればよい。絶対座標重心位置を（ｘＣＯＧ，ｙＣＯＧ，ｚＣＯＧ），絶対座標目標ＺＭＰを（ｘＺＭＰ，ｙＺＭＰ，０），相対座標目標ＺＭＰを（ＸＺＭＰ，ＹＺＭＰ，ＺＺＭＰ）とおく。ロール・ピッチ・ヨーの変換を線形近似し、ヨー角を零とすると以下のように表せる。
【００２９】
【数１０】
【００３０】
（３）式の１行目を展開し，２階微分すると以下のようになる。
【００３１】
【数１１】
【００３２】
（３）式の３行目を展開し，以下のように近似する。
【００３３】
【数１２】
【００３４】
また，（２）式より，以下のようになる。
【００３５】
【数１３】
【００３６】
ベース歩容では，ピッチ角およびピッチ角加速度を０と仮定しているため，以下のようになる。
【００３７】
【数１４】
【００３８】
ここで，ベース歩容の相対座標目標ＺＭＰのＸ座標をＸＺＭＰbaseとおき，それと実際のＺＭＰの相対位置偏差をＸΔとおくと次式のようになる。
【００３９】
【数１５】
【００４０】
これらと（１）式より，以下のようになる。
【００４１】
【数１６】
【００４２】
上式より，次の状態方程式が得られる。ただし，重心のＺ方向加速度は０とした。
【００４３】
【数１７】
【００４４】
以下の状態フィードバックによりレギュレータを構成する。ただし，ｋ１，ｋ２，ｋ３，ｋ４は状態フィードバックゲインである。
【００４５】
【数１８】
【００４６】
上記の状態方程式を用いて，実施例１と同様に状態フィードバックのゲインベクトルを決定すれば状態変数を全て同時に０に収束させることができる。上記は前後方向の制御であるが，横方向も同様に制御すればよい。
本発明が従来技術と異なる部分は、水平方向支持脚足平相対加速度を操作量とし，上体傾斜角，上体傾斜角速度，ＺＭＰ相対位置偏差，ＺＭＰ相対速度偏差の４つを状態変数としてレギュレータにより安定化制御を実現している部分である。ＺＭＰ相対位置偏差，ＺＭＰ相対速度偏差は，それぞれ重心位置補正量，重心速度補正量の符号を反転したものとみなしても技術的にはほぼ等価である。また，ＺＭＰを支持脚足平と置き換えても等価である。水平方向支持脚足平相対加速度を操作量とすることにより，足首角度を操作量とする場合に比べ大きな復元モーメントを得ることができる。また，その際，従来技術のように前後方向と左右方向の安定化制御が互いに影響しないため，レギュレータにより計８つの状態変数を同時に安定的に０に収束させることができる。
【図面の簡単な説明】
【００４７】
【図１】本発明の第１実施例を示す脚式歩行ロボットの安定化制御器のブロック図
【図２】本発明の安定化制御による生成される動作の例を示す説明図
【図３】従来の脚式歩行ロボットの制御装置の全体構成を表すブロック図
【図４】従来の脚式歩行ロボットの安定化制御器のブロック図
【符号の説明】
【００４８】
１１ 安定化レギュレータ
１２ 積分演算器
１３ 積分演算器
２１ 上体
２２ 脚構造
２３ 支持脚足平
２４ 遊脚足平
２５ 路面
６００ 歩容生成器
６１０ 安定化制御器
６２０ 逆キネマティクス演算器
６３０ モータ位置制御器
６６０ モータおよび脚構造
６７０ エンコーダ
６８０ 各種センサ
７２０ 目標床反力分配器
７３０ 複合コンプライアンス動作決定部
７４０ 姿勢安定化制御演算部
７５０ 上体Ｚ方向速度制御演算部
【特許請求の範囲】
【請求項１】
脚構造と，
前記脚構造を動作させるための複数のアクチュエータと，
前記アクチュエータの位置を検出してアクチュエータ位置情報を生成するためのエンコーダと，
ベースとなる動作パターンであるベース歩容を生成する歩容生成器と，
姿勢または外力または環境情報のうちのいずれかまたはすべてを含む各種状態量を検出して各種センサ情報を生成する各種センサと、
前記各種センサ情報を元に前記ベース歩容を修正した修正歩容を生成する安定化制御器と，
前記修正歩容をもとに前記複数のアクチュエータそれぞれに対する位置指令を生成する逆キネマティクス演算器と，
前記アクチュエータ位置指令通りにアクチュエータを動作させるためのアクチュエータ位置制御器と，を備えた脚式歩行ロボットの制御装置において，
前記各種センサは，少なくとも上体傾斜角を検出する傾斜センサと上体傾斜角速度を検出する傾斜角速度センサとを備え，
前記安定化制御器は，少なくとも前記上体傾斜角と，前記上体傾斜角速度と，ベース歩容の重心位置からの重心位置補正量と，ベース歩容の重心速度からの重心速度補正量と，を状態変数とし，重心加速度補正量を操作量とした状態フィードバックで構成されるレギュレータを備えたことを特徴とする脚式歩行ロボットの制御装置。
【請求項２】
前記重心速度補正量は前記重心加速度補正量の積分値であり，
前記安定化制御器は，前記重心加速度補正量から重心速度補正量を算出する積分演算器を備えたことを特徴とする請求項１記載の脚式歩行ロボットの制御装置。
【請求項３】
前記重心位置補正量は前記重心速度補正量の積分値であり，
前記安定化制御器は，前記重心速度補正量から重心位置補正量を算出する積分演算器を備えたことを特徴とする請求項１記載の脚式歩行ロボットの制御装置。
【請求項４】
上体に対する相対的な支持脚足平の加速度を支持脚足平相対加速度とし，
前記ベース歩容の前記支持脚足平相対加速度を目標支持脚足平相対加速度とし，
前記修正歩容の前記支持脚足平相対加速度を修正目標支持脚足平相対加速度とし，
前記安定化制御器は，前記重心加速度補正量を目標支持脚足平相対加速度から減算して修正目標支持脚足平相対加速度を生成することにより所望の前記重心加速度補正量を発生させることを特徴とする請求項１記載の脚式歩行ロボットの制御装置。
【請求項５】
上体に対する相対的な支持脚足平の位置を支持脚足平相対位置とし，
前記ベース歩容の前記支持脚足平相対位置を目標支持脚足平相対位置とし，
前記修正歩容の前記支持脚足平相対位置を修正目標支持脚足平相対位置とし，
前記安定化制御器は，２つの積分演算器を備え，前記重心加速度補正量の２階積分値を目標支持脚足平相対位置から減算することで修正目標支持脚足平相対位置を生成することを特徴とする請求項１記載の脚式歩行ロボットの制御装置。
【請求項１】
脚構造と，
前記脚構造を動作させるための複数のアクチュエータと，
前記アクチュエータの位置を検出してアクチュエータ位置情報を生成するためのエンコーダと，
ベースとなる動作パターンであるベース歩容を生成する歩容生成器と，
姿勢または外力または環境情報のうちのいずれかまたはすべてを含む各種状態量を検出して各種センサ情報を生成する各種センサと、
前記各種センサ情報を元に前記ベース歩容を修正した修正歩容を生成する安定化制御器と，
前記修正歩容をもとに前記複数のアクチュエータそれぞれに対する位置指令を生成する逆キネマティクス演算器と，
前記アクチュエータ位置指令通りにアクチュエータを動作させるためのアクチュエータ位置制御器と，を備えた脚式歩行ロボットの制御装置において，
前記各種センサは，少なくとも上体傾斜角を検出する傾斜センサと上体傾斜角速度を検出する傾斜角速度センサとを備え，
前記安定化制御器は，少なくとも前記上体傾斜角と，前記上体傾斜角速度と，ベース歩容の重心位置からの重心位置補正量と，ベース歩容の重心速度からの重心速度補正量と，を状態変数とし，重心加速度補正量を操作量とした状態フィードバックで構成されるレギュレータを備えたことを特徴とする脚式歩行ロボットの制御装置。
【請求項２】
前記重心速度補正量は前記重心加速度補正量の積分値であり，
前記安定化制御器は，前記重心加速度補正量から重心速度補正量を算出する積分演算器を備えたことを特徴とする請求項１記載の脚式歩行ロボットの制御装置。
【請求項３】
前記重心位置補正量は前記重心速度補正量の積分値であり，
前記安定化制御器は，前記重心速度補正量から重心位置補正量を算出する積分演算器を備えたことを特徴とする請求項１記載の脚式歩行ロボットの制御装置。
【請求項４】
上体に対する相対的な支持脚足平の加速度を支持脚足平相対加速度とし，
前記ベース歩容の前記支持脚足平相対加速度を目標支持脚足平相対加速度とし，
前記修正歩容の前記支持脚足平相対加速度を修正目標支持脚足平相対加速度とし，
前記安定化制御器は，前記重心加速度補正量を目標支持脚足平相対加速度から減算して修正目標支持脚足平相対加速度を生成することにより所望の前記重心加速度補正量を発生させることを特徴とする請求項１記載の脚式歩行ロボットの制御装置。
【請求項５】
上体に対する相対的な支持脚足平の位置を支持脚足平相対位置とし，
前記ベース歩容の前記支持脚足平相対位置を目標支持脚足平相対位置とし，
前記修正歩容の前記支持脚足平相対位置を修正目標支持脚足平相対位置とし，
前記安定化制御器は，２つの積分演算器を備え，前記重心加速度補正量の２階積分値を目標支持脚足平相対位置から減算することで修正目標支持脚足平相対位置を生成することを特徴とする請求項１記載の脚式歩行ロボットの制御装置。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２００９−２７９６６８（Ｐ２００９−２７９６６８Ａ）
【公開日】平成２１年１２月３日（２００９．１２．３）
【国際特許分類】
【出願番号】特願２００８−１３１４３９（Ｐ２００８−１３１４３９）
【出願日】平成２０年５月２０日（２００８．５．２０）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
【公開日】平成２１年１２月３日（２００９．１２．３）
【国際特許分類】
【出願日】平成２０年５月２０日（２００８．５．２０）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
[ Back to top ]