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