(57)【特許請求の範囲】
【請求項１】
少なくとも上体と、前記上体に第１の関節を介して連結される複数本の脚部リンクとからなり、前記脚部リンクがその先端部までに少なくとも１個の第２の関節を有してなる脚式移動ロボットの歩容生成装置において、
ａ．前記上体の水平方向目標位置および前記上体の目標姿勢の少なくともいずれかを含み、前記上体の鉛直方向目標位置を除く、上体軌跡を設定する上体軌跡設定手段、
ｂ．前記脚部リンク先端部の目標位置および目標姿勢の少なくともいずれかを含む脚部リンク軌跡を設定する脚部リンク軌跡設定手段、
および
ｃ．前記設定された上体軌跡および脚部リンク軌跡に基づいて前記上体の鉛直方向位置に関する連続関数からなる等式を満足する鉛直方向位置である解を求め、その解を基に前記上体の鉛直方向目標位置を決定する上体鉛直方向目標位置決定手段、
を備えたことを特徴とする脚式移動ロボットの歩容生成装置。
【請求項２】
前記上体の鉛直方向位置に関する連続関数からなる等式が、前記第１および第２の関節の少なくともいずれかの変位に関する滑らかな連続関数からなる等式であることを特徴とする請求項１記載の脚式移動ロボットの歩容生成装置。
【請求項３】
前記第１および第２の関節の少なくともいずれかの変位に関する滑らかな連続関数からなる等式が、前記第２の関節の変位および速度の少なくともいずれかに関して対称式となっていることを特徴とする請求項２記載の脚式移動ロボットの歩容生成装置。
【請求項４】
前記上体の鉛直方向位置が大きくなるにつれて前記連続関数の値が単調に変化することを特徴とする請求項１から３のいずれかに記載の脚式移動ロボットの歩容生成装置。
【請求項５】
前記上体の水平方向目標位置、前記上体の目標姿勢、前記脚部リンク先端部の目標位置、および前記脚部リンク先端部の目標姿勢の内の少なくともいずれかが、仮想的な値であることを特徴とする請求項１から４のいずれかに記載の脚式移動ロボットの歩容生成装置。
【請求項６】
前記上体鉛直方向目標位置決定手段は、
ｅ．前記上体の鉛直方向の第２の目標位置をｎ個（ｎ≧１）決定するｎ目標位置決定手段、
および
ｆ．前記解と前記ｎ個の第２の目標位置との平均値を求める平均値算出手段、
を備え、求めた平均値に基づいて前記上体の鉛直方向目標位置を決定することを特徴とする請求項１から５のいずれかに記載の脚式移動ロボットの歩容生成装置。
【請求項７】
前記平均値が加重平均値であることを特徴とする請求項６記載の脚式移動ロボットの歩容生成装置。
【請求項８】
前記上体鉛直方向目標位置決定手段は、
ｇ．前記解および前記求めた平均値の少なくともいずれかを平滑化する平滑化手段、
を備えることを特徴とする請求項６記載の脚式移動ロボットの歩容生成装置。
【請求項９】
前記上体鉛直方向目標位置決定手段は、探索法および収束演算の少なくともいずれかを用いて前記解を求めることを特徴とする請求項１から８のいずれかに記載の脚式移動ロボットの歩容生成装置。
【請求項１０】
更に、
ｈ．前記設定された上体軌跡と、脚部リンク軌跡と、決定された上体鉛直方向目標位置に基づいて目標関節角を求める目標関節角算出手段、
および
ｉ．前記目標関節角となるように前記第１および第２の関節を変位制御する変位制御手段、
を含むことを特徴とする請求項１から９のいずれかに記載の脚式移動ロボットの歩容生成装置。
【請求項１１】
前記脚式移動ロボットが２脚を有する歩行ロボットであり、前記第１の関節が股関節、前記第２の関節が膝関節であることを特徴とする請求項１から１０のいずれかに記載の脚式移動ロボットの歩容生成装置。
【請求項１２】
前記脚式移動ロボットが３脚以上の脚部を有する多脚歩行ロボットであり、前記第１の関節が付け根股関節、前記第２の関節が膝関節であることを特徴とする請求項１から９のいずれかに記載の脚式移動ロボットの歩容生成装置。
【請求項１３】
少なくとも上体と、前記上体に第１の関節を介して連結される２本の脚部リンクとからなると共に、前記脚部リンクが上腿リンクと下腿リンクとそれらを連結する膝関節とを有してなる脚式移動ロボットの歩容生成装置において、前記２本の脚部リンクの中の支持脚側の前記膝関節の外角をθsup、遊脚側のそれをθswgとするとき、前記２つの外角θsup，θswgからなる連続関数f（θsup，θswg）を用い（ただし、θsup＞０、θswg＞０）、前記２つの外角θsup，θswgの一方が増加すると、他方が減少するように前記上体の鉛直方向目標位置を決定することを特徴とする脚式移動ロボットの歩容生成装置。
【請求項１４】
前記第１の関数が股関節であると共に、前記股関節と前記先端部の距離が、前記脚部リンクの上腿リンクと下腿リンクの長さを合わせてなる最長長さ以下となるように前記上体の鉛直方向目標位置を決定することを特徴とする請求項１３記載の脚式移動ロボットの歩容生成装置。
【請求項１５】
前記連続関数が前記膝関節の外角に関する滑らかな連続関数であると共に、前記膝関節の外角およびその変化速度の少なくともいずれかに関して対称式となっていることを特徴とする請求項１３または１４記載の脚式移動ロボットの歩容生成装置。
【請求項１６】
前記上体の水平方向目標位置、前記上体の目標姿勢、前記脚部リンク先端部の目標位置、および前記脚部リンク先端部の目標姿勢の内の少なくともいずれかが、前記ロボットの歩容を決定する実際的な値であることを特徴とする請求項１３から１５のいずれかに記載の脚式移動ロボットの歩容生成装置。
【請求項１７】
前記上体の水平方向目標位置、前記上体の目標姿勢、前記脚部リンク先端部の目標位置、および前記脚部リンク先端部の目標姿勢の内の少なくともいずれかが、仮想的な値であることを特徴とする請求項１３から１５のいずれかに記載の脚式移動ロボットの歩容生成装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、脚式移動ロボットの歩容生成装置、より詳しくは２足（２脚）歩行ロボットなどの脚式移動ロボットの上体の鉛直方向（重力方向）高さを生成する歩容生成装置に関する。
【０００２】
【従来の技術】
脚式移動ロボットは上体（基体）およびそれに関節を介して連結される脚（脚部リンク）とからなるが、従来の脚式移動ロボットの歩容生成においては、上体の目標高さを、下記の如く、時刻の多項式で表して上体軌跡を生成していた。
0003663034.tif 000002
（上記で、Ｚ：上体の目標高さ、ｔ：時刻）
【０００３】
また、別の従来技術手法においては、下記の如く、上体の目標高さを、上体の目標水平変位の多項式で表して上体軌跡を生成していた。
0003663034.tif 000003
（上記で、Ｚ：上体の目標高さ、Ｘ：上体の目標水平変位）
【０００４】
上記した従来技術手法では、脚の先端部の目標位置・姿勢、上体の目標姿勢も同様の多項式で表現される。また、上体の目標水平位置は、動力学的条件を満足するように、例えばＺＭＰが存在可能範囲に入るように決定され、次いで上体の目標高さ、上体の目標水平位置、上体の目標姿勢、および脚の先端部の目標位置・姿勢に基づいて逆キネマティクス演算によって脚の目標関節角度を求めていた。
【０００５】
上記した従来技術手法における上体目標高さ決定ないし生成には、上体の目標水平位置、上体の目標姿勢、および脚の先端部の目標位置・姿勢の影響が反映されていないので、条件を満たす脚の関節角度の組が存在する保証はない。例えば、ある一瞬でも、脚の先端部から上体までの距離が脚の全リンクの長さの和よりも大きいように目標を設定した場合には、脚を真っ直ぐに伸ばしても、条件を満たす姿勢がとれないことは明らかである。
【０００６】
そのため、従来技術手法にあっては、シミュレーション上の試行錯誤によって上体の目標高さの関数を決定していた。しかしながら、歩行パターンには平地、斜面、階段歩行、曲線路歩行などの歩行環境の違い、さらに発進／停止、高速／低速歩行などの速度、加減速度の違いなど様々なバリエーションがある。
【０００７】
それら全てに対してシミュレーション上の試行錯誤で予め上体の目標高さを設定しておくことは、たとえ有限個の固定パターンに限定しても、極めて大変な作業である。また、脚の関節角度の組が存在する保証がないため、上記した従来技術ではリアルタイムに自在な目標歩容を生成して歩行させることができなかった。
【０００８】
更に別の従来技術手法においては、上体の目標水平位置、上体の目標姿勢、および脚の先端部の目標位置・姿勢を、同様に予め与えると共に、少なくとも支持脚の中の１本が伸び切った姿勢、即ち、取り得る姿勢の中で上体が最も高くなる姿勢を条件として上体目標高さとすることも提案されている。この手法では、すべての脚に対して、脚先端部と上体の脚取り付け部との水平距離が脚の長さを越えない限り、必ず上記の条件を満足する姿勢が存在する。
【０００９】
しかし、この従来技術手法によるときは、両脚が共に伸び切ったとき、上体の上下動の加速度が不連続になる。例えば、２足歩行の場合、上体の高さの軌跡は伸び切った脚が反対の脚に切り替わるときに折れ曲がり、過大な上下加速度が発生する不都合があった。
【００１０】
【発明が解決しようとする課題】
この発明は従来技術の上記した不都合を解消し、脚式移動ロボットにおいて、鉛直方向（重力方向）の上体高さを除くパラメータが与えられるとき、上体の上下動および関節の動きが滑らかで、かつ変位、速度および加速度の振幅が小さくなるように、適正に上体高さを決定できるようにした脚式移動ロボットの歩容生成装置を提供することを目的とする。
【００１１】
更に、上体高さを除くパラメータが与えられるとき、歩行中にリアルタイムに上体高さを決定して臨機応変に自在な歩行を実現することを可能とする脚式移動ロボットの歩容生成装置を提供することを第２の目的とする。
【００１２】
更には、脚式移動ロボットにおいて、上体高さを除くパラメータが与えられるとき、上体の上下動および関節の動きが滑らかで、かつ変位、速度および加速度の振幅が小さくなるように適正に上体高さを決定すると共に、決定された上体高さを含む軌跡を実現するようにロボットを関節駆動制御するようにした脚式移動ロボットの歩容生成装置を提供することを付随的な目的とする。
【００１３】
【課題を解決するための手段】
上記の目的を達成するために、請求項１項にあっては、少なくとも上体と、前記上体に第１の関節を介して連結される複数本の脚部リンクとからなり、前記脚部リンクがその先端部までに少なくとも１個の第２の関節を有してなる脚式移動ロボットの歩容生成装置において、前記上体の水平方向目標位置および前記上体の目標姿勢の少なくともいずれかを含み、前記上体の鉛直方向目標位置を除く、上体軌跡を設定する上体軌跡設定手段、前記脚部リンク先端部の目標位置および目標姿勢の少なくともいずれかを含む脚部リンク軌跡を設定する脚部リンク軌跡設定手段、および前記設定された上体軌跡および脚部リンク軌跡に基づいて前記上体の鉛直方向位置に関する連続関数からなる等式を満足する鉛直方向位置である解を求め、その解を基に前記上体の鉛直方向目標位置を決定する上体鉛直方向目標位置決定手段を備える如く構成した。尚、ここで「上体の鉛直方向位置」は、Ｚ軸上の値に限らず、ＺＸ平面あるいはＺＹ平面上の値であってもよい。
【００１４】
請求項２項にあっては、前記上体の鉛直方向位置に関する連続関数からなる等式が、前記第１および第２の関節の少なくともいずれかの変位に関する滑らかな連続関数からなる等式である如く構成した。
【００１５】
請求項３項にあっては、前記第１および第２の関節の少なくともいずれかの変位に関する滑らかな連続関数からなる等式が、前記第２の関節の変位および速度の少なくともいずれかに関して対称式となっている如く構成した。
【００１６】
請求項４項にあっては、前記上体の鉛直方向位置が大きくなるにつれて前記連続関数の値が単調に変化する如く構成した。
【００１７】
請求項５項にあっては、前記上体の水平方向目標位置、前記上体の目標姿勢、前記脚部リンク先端部の目標位置、および前記脚部リンク先端部の目標姿勢の内の少なくともいずれかが、仮想的な値である如く構成した。
【００１８】
請求項６項にあっては、前記上体鉛直方向目標位置決定手段は、前記上体の鉛直方向の第２の目標位置をｎ個（ｎ≧１）決定するｎ目標位置決定手段、および前記解と前記ｎ個の第２の目標位置との平均値を求める平均値算出手段を備え、求めた平均値に基づいて前記上体の鉛直方向目標位置を決定する如く構成した。
【００１９】
請求項７項にあっては、前記平均値が加重平均値である如く構成した。
【００２０】
請求項８項にあっては、前記上体鉛直方向目標位置決定手段は、前記解および前記求めた平均値の少なくともいずれかを平滑化する平滑化手段を備える如く構成した。
【００２１】
請求項９項にあっては、前記上体鉛直方向目標位置決定手段は、探索法および収束演算の少なくともいずれかを用いて前記解を求める如く構成した。
【００２２】
請求項１０項にあっては、更に、前記設定された上体軌跡と、脚部リンク軌跡と、決定された上体鉛直方向目標位置に基づいて目標関節角を求める目標関節角算出手段、および前記目標関節角となるように前記第１および第２の関節を変位制御する変位制御手段を含む如く構成した。
【００２３】
請求項１１項にあっては、前記脚式移動ロボットが２脚を有する歩行ロボットであり、前記第１の関節が股関節、前記第２の関節が膝関節である如く構成した。
【００２４】
請求項１２項にあっては、前記脚式移動ロボットが３脚以上の脚部を有する多脚歩行ロボットであり、前記第１の関節が付け根股関節、前記第２の関節が膝関節である如く構成した。請求項１３項にあっては、少なくとも上体と、前記上体に第１の関節を介して連結される２本の脚部リンクとからなると共に、前記脚部リンクが上腿リンクと下腿リンクとそれらを連結する膝関節とを有してなる脚式移動ロボットの歩容生成装置において、前記２本の脚部リンクの中の支持脚側の前記膝関節の外角をθsup、遊脚側のそれをθswgとするとき、前記２つの外角θsup，θswgからなる連続関数ｆ（θsup，θswg）を用い（ただし、θsup＞０、θswg＞０）、前記２つの外角θsup，θswgの一方が増加すると、他方が減少するように前記上体の鉛直方向目標位置を決定する如く構成した。請求項１４項にあっては、前記第１の関節が股関節であると共に、前記股関節と前記先端部の距離が、前記脚部リンクの上腿リンクと下腿リンクの長さを合わせてなる最長長さ以下となるように前記上体の鉛直方向目標位置を決定する如く構成した。請求項１５項にあっては、前記連続関数が前記膝関節の外角に関する滑らかな連続関数であると共に、前記膝関節の外角およびその変化速度の少なくともいずれかに関して対称式となっている如く構成した。請求項１６項にあっては、前記上体の水平方向目標位置、前記上体の目標姿勢、前記脚部リンク先端部の目標位置、および前記脚部リンク先端部の目標姿勢の内の少なくともいずれかが、前記ロボットの歩容を決定する実際的な値である如く構成した。請求項１７項にあっては、前記上体の水平方向目標位置、前記上体の目標姿勢、前記脚部リンク先端部の目標位置、および前記脚部リンク先端部の目標姿勢の内の少なくともいずれかが、仮想的な値である如く構成した。
【００２５】
【作用】
上体の上下動および関節の動きが滑らかで、かつ変位、速度および加速度の振幅が小さくなるように、適正に上体高さを決定することができると共に、上体高さを除くパラメータが与えられるとき、歩行中にリアルタイムに上体高さを決定して臨機応変に自在な歩行を実現することができる。
【００２６】
更には決定された上体高さを含む軌跡を実現するようにロボットの関節を変位制御することができる。
【００２７】
【発明の実施の形態】
以下、添付図面を参照してこの発明に係る脚式移動ロボットの歩容生成装置を説明する。最初に、脚式移動ロボットとして２足（２脚）歩行ロボットを例にとって説明する。
【００２８】
図１はその２足歩行ロボット１を含む装置全体の説明図であり、図２はその２足歩行ロボット１の変数の定義に焦点をおいて示すスケルトン図である。
【００２９】
図示の如く、２足歩行ロボット１は左右それぞれの脚部リンク２に６個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。６個の関節は上から順に、股（腰部）の脚部回旋用の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、股（腰部）のピッチ方向（Ｙ軸まわり。軸方向は図２に示す）の関節１２Ｒ，１２Ｌ、同ロール方向（Ｘ軸まわり。同様に図２に示す）の関節１４Ｒ，１４Ｌ、膝部のピッチ方向の関節１６Ｒ，１６Ｌ（前記した第２の関節）、足部のピッチ方向の関節１８Ｒ，１８Ｌ、同ロール方向の関節２０Ｒ，２０Ｌから構成される。
【００３０】
足部には足平２２Ｒ，２２Ｌが取着される。尚、この明細書では、足（foot) を「足平」という。また、最上位には上体（基体）２４が設けられ、その内部に図３に関して後述するマイクロコンピュータからなる制御ユニット２６などが格納される。上記において股関節（腰関節）（前記した第１の関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から、足首関節ないし足関節は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２８Ｒ，２８Ｌ、膝関節と足関節とは下腿リンク３０Ｒ，３０Ｌで連結される。
【００３１】
上記の構成により、脚部リンク２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６×２＝１２個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行させることができる。尚、ロボット１の関節駆動制御において、座標系は図２に示すように、支持脚を基準とする。また、リンク９２，９４，９６の長さは零とする（図１ではこれらのリンクの図示を省略する）。
【００３２】
尚、この明細書で上体高さは、上体２４の所定位置を意味し、具体的には上体２４内の左右中心（図２に符合９８で示す）などの代表点を意味する。
【００３３】
図１に示す如く、足関節の下方には公知の６軸力センサ４４が取着され、力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚとを測定し、足部の着地の有無ないしは接地荷重などを検出する。また、上体２４には傾斜センサ６０が設置され、Ｚ軸（鉛直方向）に対する傾きとその角速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコーダが設けられる。
【００３４】
図３は制御ユニット２６の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ６０などの出力はＡ／Ｄ変換器７０でデジタル値に変換され、その出力はバス７２を介してＲＡＭ７４に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ７６を介してＲＡＭ７４内に入力される。
【００３５】
制御ユニット内にはＣＰＵからなる第１、第２の演算装置８０，８２が設けられており、第１の演算装置８０は後述の如く、生成され、ＲＯＭ８４に格納されている歩容パラメータを読み出して上体高さを決定すると共に目標関節角を算出し、ＲＡＭ７４に送出する。また第２の演算装置８２はＲＡＭ７４からその目標値と検出された実測値とを読み出し、各関節の駆動に必要な制御値を算出してＤ／Ａ変換器８６とサーボアンプを介して各関節を駆動する電動モータに出力し、変位制御する。
【００３６】
図４はこの装置の構成を示す機能ブロック図であり、図５および図６はこの装置の動作を説明するフロー・チャートであるが、同図の説明に入る前に、図７を用いてこの発明の基本原理を説明する。図７では理解を容易にするためロボット１を２次元で示す。
【００３７】
図示の２足歩行ロボット１において、支持脚の膝関節の曲げ角をθsup 、遊脚の膝関節の曲げ角をθswg と定義する。また膝は人間と同様に後方にしか曲がらないものとする。即ち、値θsup およびθswg は正の値とする。足平２２Ｒ（Ｌ）の位置は、具体的には、足首関節１８，２０Ｒ（Ｌ）の位置（座標）とする。
【００３８】
ここで、θsup とθswg に関する滑らかな連続関数f(θsup,θswg)の例として、次式を設定する。
f(θsup,θswg)＝1/sin(θsup)＋1/sin(θswg)・・・式１
【００３９】
幾何学的な関係で言えば、次式のように、θsup は上体の位置（座標) (Xb, Zb)と支持脚の足平位置（足首座標) （Xsup，Zsup）の関数であり、θswg は上体の位置（座標）（Xb，Zb）と遊脚の足平位置（足首座標）（Xswg，Zswg）の関数である。
θsup ＝H(Xb，Zb，Xsup，Zsup）・・・式２ａ
θswg ＝H(Xb，Zb，Xswg，Zswg）・・・式２ｂ
【００４０】
ここで、H(Xb, Zb，X, Z）は幾何学計算により得られ、次式で定義される。
H(Xb，Zb，X, Z)=
acos［｛(Xb ‐X)・（Xb‐X ）＋（Zb‐Z)・（Zb‐Z)｝/( 2・L ・L ）-1］・・・式３
【００４１】
ところで、acosの定義域が[-1,1]であるので、H (Xb,Zb,X,Z) の定義域は、次式を満たす(Xb,Zb,X,Z) の組であることに注意しなければならない。
（Xb‐X ）・（Xb‐X ）＋（Zb‐Z)・（Zb‐Z)≦4 ・L ・L ・・・式４
この式は、股関節と足平位置（足首関節) の距離が、脚の長さ２Ｌ以下であることを要求する。
【００４２】
式２により、
f(θsup,θswg)＝f(H(Xb,Zb,Xsup,Zsup), H(Xb,Zb,Xswg,Zswg)) ・・・式５
が導かれる。
【００４３】
このことは、両膝の曲げ角の関数f(θsup,θswg)は、上体の位置（座標）と両足平位置（足首座標）の関数g(Xb,Zb,Xsup,Zsup,Xb,Zb,Xswg,Zswg ）に置き換えることができること、即ち、同一関数値を持つことを意味している。
【００４４】
但し、g(Xb,Zb,Xsup,Zsup,Xb,Zb,Xswg,Zswg)の定義は、f(H(Xb,Zb,Xsup,Zsup),H(Xb,Zb,Xswg,Zswg))である。関数ｇの定義域は、以下の式を同時に満足する(Xb,Zb,Xsup,Zsup,Xb,Zb,Xswg,Zswg）の組である。
(Xb-Xsup) ・(Xb-Xsup) ＋(Zb-Zsup) ・(Zb-Zsup) ≦4 ・L ・L ・・・式６ａ
(Xb-Xswg）・(Xb-Xswg) ＋(Zb-Zswg) ・(Zb-Zswg) ≦4 ・L ・L ・・・式６ｂ
上記でｇも滑らかな連続関数である。
【００４５】
さて、ここで関数g(Xb,Zb,Xsup,Zsup,Xb,Zb,Xswg,Zswg)において、Xb，Xsup，Zsup，Xswg，Zswgを既知変数とし、上体高さＺｂだけを未知変数とみなすと、関数ｇは次の性質を持つ。
【００４６】
性質１
ある上体の高さに対して（Xb,Zb,Xsup,Zsup,Xswg,Zswg)が式６で与えられる関数ｇの定義域にあるならば、即ち、股関節から各足平（足首関節）までの距離がいずれも脚の長さ２Ｌ以下であるならば、上体の高さＺｂを高くするにつれて関数g(Xb,Zb,Xsup,Zsup,Xb,Zb,Xswg,Zswg ）の値は徐々に大きくなり、いずれかの膝が伸び切る高さまで達すると、無限大になる性質を持っている。
【００４７】
この性質は、直接、関数ｇを解いてｇの値とＺｂの関係を求めても分かるが、ｇと同一の値を持つ関数ｆの性質からも容易に導くことができる。即ち、図７において上体の高さＺｂを高くしていくと、両膝の曲げ角θsup およびθswg は零に近づくので、式１の右辺各項の分母が零に近づき、f(θsup,θswg)の値は徐々に大きくなる。
【００４８】
これは、最終的には、θsup またはθswg のいずれかが無限小になったところで、言い換えれば、いずれかの膝が伸び切ったところで式１の右辺のいずれかの項が無限大になり、f(θsup,θswg)の値は無限大になることからも分かる。
【００４９】
g(Xb,Zb,Xsup,Zsup,Xb,Zb,Xswg,Zswg）あるいはf(θsup,θswg)にこのような性質があることを踏まえて、次の性質が導かれることを説明する。
【００５０】
性質２
通常の歩行においては、ある値C0が存在し、次式７を満足する任意の値Ｃに対して、常に式８ａまたは式８ｂを満足する上体の高さＺｂが存在する。
C ＞ C0           ・・・  式７
f(θsup,θswg ）＝C ・・・式８ａ
g(Xb,Zb,Xsup,Zsup,Xb,Zb,Xswg,Zswg)＝C ・・・式８ｂ
【００５１】
性質２の導出
通常の歩行においては、上体高さＺｂを適当に設定すれば、股関節から足平（足首関節）までの距離は、脚の長さ２Ｌよりもある程度余裕を持って小さい。即ち、上体高さＺｂを適当に設定すれば、式２ａ、式２ｂ、式３で求められるθsup とθswg が常に次式を満足するδが存在する。
【００５２】
θsup ＞δかつθswg ＞δ  ・・・式９
ｇとｆが同一の値を持つこと、および式１と式９から、上体高さＺｂを適当に設定すれば、式１０が満足される。
g(Xb,Zb,Xsup,Zsup,Xb,Zb,Xswg,Zswg)＜ 1/sin (δ) ＋1/sin(δ) ・・式１０
【００５３】
このときの上体高さから徐々に上体高さを高くすれば、g は徐々に大きくなって無限大まで増大する。これは、次式を満足する任意のＣに対して、式８ｂを満足する上体高さＺｂが存在することを意味する。
1/sin( δ) ＋ 1/sin( δ) ＜C ＜無限大・・・式１１
1/sin( δ) ＋1/sin(δ) をC0とおけば、性質２が得られる。
【００５４】
また、性質１から次の性質が導かれる。
【００５５】
性質3
式８ａまたは式８ｂを満足する上体高さＺｂはＣを小さくすれば小さくなり（低くなり）、Ｃを大きくすれば大きく（高くなる）。Ｃを無限大に大きくすると、上体高さＺｂは第２の従来技術手法の上体高さに収束する。また、Ｃを大きくすると、膝関節角度は平均して小さくなるが、加減速度は大きくなる。
【００５６】
性質4
関数ｆが式１の例のように左右の膝関節角に対して対称式になっているならば、一方の脚の先端を持ち上げると、その膝は屈曲し、反対側の脚が伸張する。即ち、脚先端の持ち上げ仕事が全脚に分担されるので、脚の屈伸量が少なくて済む。従って、歩行時の関節角速度絶対値も小さくなる。
【００５７】
上記を前提として、この装置の動作を説明する。
【００５８】
図４に示す如く、この装置は、足平軌跡算出アルゴリズムおよび上体軌跡算出アルゴリズムを備える。図示しないオフラインにおいて、歩幅、歩行周期などの歩行の特性量が決められ、それに基づいて足の運び方（足平軌跡）が決められて足平軌跡パラメータとして前記した制御ユニット２６のＲＯＭ８４に格納される。
【００５９】
また、図示しないオフラインにおいて目標ＺＭＰの軌跡が決定され、その決定された目標ＺＭＰを満足するように上体の軌跡パラメータ（あるいは時系列テーブル）が決定され、同様にＲＯＭ８４内に格納される。尚、ＺＭＰ（Zero Moment Point ）は、歩容によって発生する慣性力と重力の合力の作用線が床面と交わる点であり、慣性力と重力の合力を床面上のある作用点まわりの力とモーメントで表現したときＭｚを除くモーメントＭｘ，Ｍｙが零となる作用点である。
【００６０】
格納されているパラメータは逐次読み出されて足平軌跡算出アルゴリズムおよび上体軌跡算出アルゴリズムに入力され、そこで前記した脚部リンク先端部の目標位置および姿勢の少なくともいずれかを含む脚部リンク軌跡、即ち、両脚の足平位置・姿勢（具体的には足首関節１８，２０Ｒ（Ｌ）の位置・姿勢）および上体姿勢・水平位置が算出される。
【００６１】
より具体的には、図５フロー・チャートに示す如く、Ｓ１０から開始した後、Ｓ１２に進んで刻み時間Δｔ（制御周期。例えば１０ｍｓ）ごとのタイマ割り込みを待機し、割り込みがあるとＳ１４に進んで時刻ｔをΔｔだけ更新し、Ｓ１６に進んで時刻ｔ（現在時刻）における目標上体水平位置（Xb,Yb)、目標上体姿勢 (θxb, θyb, θzb）、両脚の目標足平（足首関節）位置（Xsup,Ysup,Zsup,Xswg,Yswg,Zswg)および姿勢 (θxsup, θysup, θzsup, θxswg, θyswg, θzswg) を算出する。これらの値は瞬時値である。
【００６２】
次いでＳ１８に進んで目標上体高さＺｂを算出する。換言すれば、この装置においては上体高さをリアルタイムに算出する。
【００６３】
図６はその目標上体高さ決定アルゴリズムを示すサブルーチン・フロー・チャートである。
【００６４】
尚、図６に示すアルゴリズムは前述の基本原理を３次元に拡張したものである。前述の基本原理との対応を説明すると、まず、関節角に関する連続関数ｆとして、次式を選んでいる。
f(θsup,θswg)＝1/sin(θsup)＋1/sin(θswg)・・・・・式１２
【００６５】
また、上体高さを決定するための等式として以下の式（先の２次元での説明の式８ａに相当）を選んだ。尚、値Ｃはここでは定数とした。
f(θsup,θswg ）＝C ・・・式１３
【００６６】
この実施の形態では、式１２、式１３を満足する上体高さを目標上体高さとして決定するようにした。尚、上体高さを直接的に解くことができないのでニュートン法による収束演算（あるいは探索法）を用いて求める。
【００６７】
以下、図６のフローチャートを説明すると、先ずＳ１００において目標上体高さＺｂを仮決めする。これは、目標上体高さＺｂを収束演算によって求めるための初期値である。
【００６８】
続いてＳ１０２を経てＳ１０４に進み、前記した目標上体水平位置（Xb, Yb）、仮目標上体高さZb、目標上体姿勢（θxb, θyb, θzb）、両脚についての目標足平（足首関節）位置・姿勢（Xsup, Ysup, Zsup, θxsup, θysup, θzsup, Xswg, Yswg, Zswg, θxswg, θyswg, θzswg) から、幾何学演算によって支持脚の目標股関節位置（座標）と遊脚の目標股関節位置（座標）を求め、さらに支持脚の目標股関節位置（座標）と目標足平位置（足首関節座標）の差（「△Xsup, △Ysup, △Zsup」という）と遊脚の目標股関節位置（座標）と目標足平位置（足首関節座標）の差（「△Xswg, △Yswg, △Zswg」という）を求める。
【００６９】
続いてＳ１０６に進み、三角関数の定理に基づいて図示の式に従って支持脚膝関節角θsup と遊脚膝関節角θswg を求める。続いてＳ１０８に進み、等式１３の両辺の値の差Err を求める。具体的には、図示の如く、式１２の右辺から定数Ｃを減算して差Err を算出する。
【００７０】
続いてＳ１１０に進み、Err の絶対値が収束演算の許容値以内か、言い換えれば許容できるほどに十分小さいか否かを判断する。Ｓ１１０で許容値以内、即ち、十分小さいと判断されるときはＳ１１２に進み、そのときの上体高さＺｂを目標上体高さに決定してプログラムを終了する。
【００７１】
他方、Ｓ１１０で否定されるときはＳ１１４に進み、図示の感度式によって感度Ｓを求める。尚、この式は差Err の式をＺｂで偏微分して得たものである。続いてＳ１１６に進み、そのときの目標上体高さに‐S ・Err を加えた値、即ち、差Err に感度Ｓを乗じた積をＺｂから減算した値を新たな上体高さ候補値としてＺｂに代入し、Ｓ１０４に戻る。
【００７２】
尚、関数の非線形性が強い領域では目標上体高さに−S ・Err を加えた値が関数の定義域を超える場合があるので、その際には目標上体高さ候補値の変更量を控えめにするのが望ましい。
【００７３】
図５フロー・チャートに戻ると、続いてＳ２０に進んで目標上体水平位置（Xb,Yb ）、目標上体高さZb、目標上体姿勢（θxb, θyb, θzb）、両脚の目標足平（足首関節）位置・姿勢（Xsup, Ysup, Zsup, θxsup, θysup, θzsup, Xswg, Yswg, Zswg, θxswg, θyswg, θzswg) から公知の逆キネマティクス演算によって目標全関節角、即ち、関節１０Ｒ（Ｌ），１２Ｒ（Ｌ）など１２個の関節の目標角度を求める。求めた関節角は前記したＲＡＭ７４に格納する。
【００７４】
次いでＳ２２に進み、求めた目標関節角となるように、前記した演算装置２においてロボットの全関節角が変位制御される。但し、この関節角サーボ制御は公知であるので、説明を省略する。また、実際にはコンプライアンス制御によって目標足平位置・姿勢などが修正されてから関節角が求められるが、これもこの発明の要旨とは直接の関連を有しないので、説明を省略する。
【００７５】
この実施の形態は上記の如く構成したので、鉛直方向（重力方向）の上体高さを除くパラメータが与えられるとき、上体の上下動および関節の動きが滑らかで、かつ変位、速度および加速度の振幅が小さくなるように、適正に上体高さを決定することができる。
【００７６】
更に、前記したように歩行中にリアルタイムに上体高さを決定することができ、それに基づいて関節駆動制御することができるので、臨機応変に自在な歩行を実現することができる。
【００７７】
更に、等式が膝関節角に対して対称式となっていることから、一方の脚先端を持ち上げるとその膝が屈曲し、他方の脚を伸張させることができ、脚先端の持ち上げ仕事を両脚に分担させることができ、膝の屈伸量を低減して歩行時の関節角速度絶対値も小さくすることができ、最適に上体高さを決定ないし生成することができる。
【００７８】
更に、上体の鉛直方向位置が大きくなるにつれて連続関数の値は単調に変化（増加）し、前記上体の鉛直方向位置が最大になったとき無限大となることから、解を必ず求めることができ、これによっても最適に上体高さを決定ないし生成することができる。尚、無限大まで増加（増大）しなくても、十分に大きい値まで増加（増大）するものであれば良い。
【００７９】
図８はこの発明の第２の実施の形態に係る装置の構成を示す機能ブロック図であり、図９はその動作を示すフロー・チャートである。
【００８０】
第２の実施の形態では、図８に示す如く、第１の実施の形態の上体高さ決定アルゴリズムに与えられる足平軌跡の一部に、仮想の足平軌跡を与えるようにした。より具体的には図１０に示すように、足平（遊脚足平）の目標位置の軌跡を、実ロボットが本来追従すべき目標値からややずらした仮想軌跡に置き代えるようにした。
【００８１】
図９は第２の実施の形態に係る装置の動作、即ち、上体高さ決定アルゴリズムを示すフロー・チャートである。尚、歩容生成に関するメインアルゴリズムは第１の実施の形態の図５と同様である。
【００８２】
以下説明すると、Ｓ２００において仮想遊脚足平（足首関節）位置を求める。これは、図８に示すように予め設定された仮想（遊脚）足平軌跡パラメータに基づいて算出される。続いてＳ２０２に進んで目標上体高さＺｂを仮決めし、Ｓ２０４を経てＳ２０６に進んで第１の実施の形態と同様に座標上の位置の差を求める。
【００８３】
以下、Ｓ２０８ないし２１８を経て第１の実施の形態の図６フロー・チャートと同様な処理を行う。パラメータの一部が仮想値である点を除くと、第２の実施の形態の処理は、第１の実施の形態のそれと異ならない。
【００８４】
第２の実施の形態は上記の如く構成したので、第１の実施の形態と同様に、上体高さを除くパラメータが与えられるとき、上体の上下動および関節の動きが滑らかで、かつ変位、速度および加速度の振幅が小さくなるように上体高さを適正に決定することができると共に、歩行中にリアルタイムに上体高さを決定して臨機応変に自在な歩行を実現することができる。
【００８５】
尚、第１の実施の形態に較べると、場合によっては決定された上体高さを満足する実ロボットの姿勢（全関節角の組）が存在する可能性が幾分低くなるが、遊脚足平の仮想軌跡を操作することによって、図１１に示すように、上体の高さを意図的に変化させることができる。
【００８６】
例えば、上体の上下運動をやや遅らせたい場合には、遊脚足平の仮想位置を目標位置よりもやや遅れ気味に移動させれば良い。これによって例えば、上体の下方加速度が大きくて足平２２Ｒ（Ｌ）の接地圧が低くなる時刻を、スピン力モーメントが大きくなる時刻からずらしてスピンを防止することも可能となる。尚、遊脚について仮想的な軌跡を与える例を示したが、支持脚についても同様であることは言うまでもない。
【００８７】
図１２はこの発明の第３の実施の形態に係る装置の動作を説明する遊脚足平軌跡の説明図であり、図１３は第３の実施の形態に係る装置で得られる上体軌跡の説明図である。
【００８８】
第３の実施の形態は第２の実施の形態の変形例であり、仮想遊脚足平軌跡の鉛直方向高さを本来の軌跡より低く設定した。即ち、図１２において実線はＸ軸方向についての、歩幅、歩行周期などの歩行の特性量から決められた本来的な足平軌跡を示すが、それに対して想像線で示すように、意図的に鉛直方向高さを小さくした遊脚足平軌跡を仮想的に与えるようにした。
【００８９】
図１３はそれによって決定される上体高さを示す。実線は本来的な足平軌跡に基づいて決定された値であり、想像線が仮想軌跡による値を示す。図示の如く、遊脚足平軌跡の鉛直方向高さを下げることにより、上体高さも低くすることができる。
【００９０】
即ち、図示の２足歩行ロボット１の歩行制御において、上体高さを小さくすると、換言すれば重心軌跡を低くすると、膝関節１６Ｒ（Ｌ）を曲げて歩行することになり、常時大きな保持トルクが必要になる。他方、重心軌道が高過ぎると、重心の上下動が大きくなって路面反力の上下成分（上下加速度）の変動振幅が増え、路面との接地性が悪化したり、路面からの衝撃力が増加する。
【００９１】
第３の実施の形態は上記の如く構成したことで、必要に応じて上体高さを低くすることができ、上下加速度を低減させた歩行を実現することができる。
【００９２】
図１４はこの発明の第４の実施の形態に係る装置の構成を示す機能ブロック図である。
【００９３】
第４の実施の形態も、第３の実施の形態と同様に、第２の実施の形態の変形例であり、図示の如く、３種の足平軌跡パラメータを与えるようにした。即ち、仮想足平軌跡パラメータ１、本来の足平軌跡パラメータおよび仮想足平軌跡パラメータ２からなる３種のパラメータを用意すると共に、仮想足平軌跡算出アルゴリズムを３個設け、それぞれに入力するようにした。
【００９４】
仮想足平軌跡算出アルゴリズム２で算出される足平位置・姿勢（前述の如く具体的には足首関節の位置・姿勢）は、第１の実施の形態で述べた本来的な標準値である。他方、仮想足平軌跡算出アルゴリズム１で算出される仮想足平位置・姿勢は標準値よりも時間的に早めに移動するものであり、仮想足平軌跡算出アルゴリズム３で算出される仮想値は標準値よりも遅めに移動する仮想値である。
【００９５】
同様に、上体高さ決定アルゴリズムも３個用意され、上体高さアルゴリズム２で決定される第１の実施の形態で述べた標準的な値Zb2 に対し、上体高さアルゴリズム１，３で決定される値Zb1,3 は、上体高さのピーク値が発生する時刻がずれるように決定される。
【００９６】
決定された３個の値は次いで平均ブロックに入力され、そこで単純平均値Zbavが算出される。かく求められた上体高さおよびその他のパラメータに基づいて逆キネマティクス演算ブロックでは目標全関節角が算出される。
【００９７】
第４の実施の形態は上記の如く構成したので、従前の実施の形態と同様の効果を奏すると共に、図示の如く、３個の波形の平均値Zbavを求めることで原波形に対して振幅を小さくすることができ、結果的に上体高さを小さくすることができる。これによって階段を昇降するときなど膝の屈伸の大きい歩行を行う際にも、上下動を低減して路面反力を低減することができる。
【００９８】
ここで、第２ないし第４の実施の形態で用いた仮想遊脚足平軌跡を生成する具体的な手法を以下に列挙する。
a ）遊脚足平の軌跡を時間の多項式によって生成する場合には、多項式の係数をずらす。
b ）遊脚足平の軌跡を、本出願人が先に提案したフィルタ（特開平５−３２４１１５号記載の技術）を用いて生成する場合には、その時定数をずらす。
c ）遊脚足平の目標軌跡に、所定のずらし量の時間関数を加える。
【００９９】
また、第２ないし第４の実施の形態において、上体の目標姿勢・目標水平位置、足平の目標姿勢の軌跡を変えても良い。
上体の目標水平位置をずらす場合の例を以下に示す。
a ）目標上体水平軌跡が、予めデータテーブルで記憶されている場合には、データテーブルの先読みをする、即ち、現在時刻ｔよりもある時間先の目標上体位置を読み出せば良い。過去の値も考慮して、より一般的にするならば、数１に示すようになろう。
【０１００】
【数１】
0003663034.tif 000004
【０１０１】
b ）現在の目標上体水平位置をフィルタに通す。例えば、フィルタの伝達関数を
（TnS ＋l)／（TdS ＋1 ）
に設定すると、Tn＞Tdにすれば、フィルタを通した上体軌跡は元の軌跡よりも時間的に先行する。
【０１０２】
図１５はこの発明の第５の実施の形態に係る装置の構成を示す機能ブロック図であり、図１６はその動作、即ち、上体高さ決定アルゴリズムを示すフロー・チャートである。
【０１０３】
第５の実施の形態では、図１５に示す如く、第１の実施の形態ないし第４の実施の形態によって求めた上体高さをフィルタ（ローパスフィルタ）にかけて平滑化処理を行うようにした。
【０１０４】
以下、第５の実施の形態に係る上体高さ決定アルゴリズムを図１６フロー・チャートを参照して説明する。尚、歩容生成に関するメインアルゴリズムは第１の実施の形態の図５と同様である。
【０１０５】
以下説明すると、Ｓ３００において第１の実施の形態で述べた上体高さ決定ルーチン（図６フロー・チャート）を実行し、上体高さＺｂを求める。尚、上体高さは刻み時間Δｔごとに算出されるが、それを歩行の１周期にわたって示すと、先に図１１に示したような波形（ここで「原波形」という）として捉えることができる。
【０１０６】
続いてＳ３０２に進んで求めた上体高さにフィルタ演算を行い、目標上体高さを求める。このように、出力（原波形）をローパスフィルタにかけることで所定値以上の周波数成分を遮断することができ、目標上体高さ（波形値）を平滑化することができる。
【０１０７】
第５の実施の形態は上記の如く構成したことで、従前の実施の形態の効果に加えて平滑化処理により上体の上下加速度振幅を一層小さくすることができる。
【０１０８】
他方、このような構成によって、関節角の速度振幅、加速度振幅が大きくなる欠点がある。また、決定された上体高さを満足する実ロボットの姿勢（全関節角の組）が存在する可能性が低くなる。例えば、階段を降りるときには、平滑化によって上体高さがなかなか下がらないために、適正な姿勢保持が困難となる恐れもある。
【０１０９】
図１７はこの発明の第６の実施の形態に係る装置の構成を示す機能ブロック図であり、図１８はその動作、即ち、上体高さ決定アルゴリズムを示すフロー・チャートである。
【０１１０】
第６の実施の形態にあっては、第１の実施の形態を用いて得た目標上体高さ（第１原波形と言う）と、上体高さを一定に保つという第１の従来技術手法で得た目標上体高さ（第２原波形と言う）の加重平均をとって目標上体高さを得るようにした。
【０１１１】
図１８フロー・チャートを参照して第６の実施の形態に係る装置の上体高さ決定アルゴリズムを説明する。尚、歩容生成に関するメインアルゴリズムは第１の実施の形態の図５と同様である。
【０１１２】
先ずＳ４００において第１の実施の形態で図６に関して述べた処理を行って現在時刻での目標上体高さ（第１原波形）を得る。続いてＳ４０２に進んで先に第１の従来技術手法として述べた時刻の多項式を用いて現在時刻での目標上体高さ（第２の原波形）を求める。
【０１１３】
続いてＳ４０４に進んで現在時刻（時刻ｔ）での重みＷ１，Ｗ２を決定する。但し、重みＷ１，Ｗ２は、Ｗ１＋Ｗ２＝１を満足し、Ｗ１，Ｗ２それぞれの時間変化が滑らかであるように決定する。続いてＳ４０６に進んで加重平均値を算出する。即ち、第１、第２の原波形値に決定した重みＷ１，Ｗ２を乗じて得た積を合算し、合算値を目標上体高さとする。
【０１１４】
第６の実施の形態にあっては上記の如く構成することで、従前の実施の形態での効果に加えて、目標上体高さについて、第１原波形に対して例えば第２の原波形をある一定値ｈ０にすると、上下振幅をＷ１倍に低減することができる。但し、h 0 を高くし過ぎると、上体高さが大きくなり過ぎ、上体高さを満足する実ロボットの姿勢（全関節角の組）が存在しなくなる恐れがある。
【０１１５】
尚、第６の実施の形態では原波形が２つの例を示したが、３個以上にも拡張可能である。更に、加重平均するようにしたが、単純平均などでも良い。
【０１１６】
また、第６の実施の形態では、第２原波形は、従来技術手法、逆に言えば、第１の実施の形態ないし第５の実施の形態に示した手法以外の手法を用いて得るようにしたが、第１の実施の形態ないし第５の実施の形態のいずれかを用いて得ても良い。
【０１１７】
例えば、第１の実施の形態、第２の実施の形態、あるいは第３ないし第５の実施の形態を用いて少なくとも１個の上体高さ原波形を求め、更にその波形を含めた複数の上体高さ原波形を生成してそれらの原波形の加重平均を求め、それを目標上体高さとしても良い。
【０１１８】
更に、以下の手法によって原波形を得ても良い。
a ）一定値（数学的には、多項式に含まれる概念である)
b ）多項式などの時間関数（代表例をこの実施の形態で述べた）
c ）フィルタなどの逐次演算によって得られる波形
d ）時系列データテーブルを予め用意しておき順に吐き出す手法
e ）オフラインで第１の実施の形態を用いて１歩分の原波形を得、これを第１テーブルに保存し、次に第１テーブルの後ろから（後刻側）保存データを逆に読み出し、フィルタに通して第２テーブルの後ろから（後刻側）格納し、実歩行時には第２テーブルの始めから吐き出す手法。（即ち、時間を逆向きにしてフィルタ処理をした原波形を生成する手法）。
【０１１９】
第５の実施の形態と異なり、手法ｅは階段を降りるときに処理後の目標上体高さが原波形よりも低くなるので、目標上体高さを満足する実ロボットの姿勢（全関節角の組）が存在する可能性が高い。従って、上体の上下加速度を低滅したい場合には有効な手段である。ただし、リアルタイムに自在な歩行を実現するには不向きである。
【０１２０】
図１９はこの発明の第７の実施の形態に係る装置の構成を示す機能ブロック図であり、図２０はその動作を示すフロー・チャートである。
【０１２１】
第７の実施の形態にあっては、第６の実施の形態＋第５の実施の形態を用いて得た目標上体高さをさらに平滑化し、より一層滑らかな上体軌跡を得るようにした。
【０１２２】
図２０フロー・チャートを参照して第７の実施の形態に係る上体高さ決定アルゴリズムを説明する。尚、歩容生成に関するメインアルゴリズムは第１の実施の形態の図５と同様である。
【０１２３】
以下説明すると、Ｓ５００ないしＳ５０６において第６の実施の形態と同様の処理を行って目標上体高さを得た後、Ｓ５０８に進んで得た値に第５の実施の形態と同様のフィルタ演算を行う。
【０１２４】
第７の実施の形態は上記の如く構成したので、第１の実施の形態の効果に加え、第６あるいは第５の実施の形態に比較して一層滑らかな上体軌跡を得ることができる。
【０１２５】
図２１はこの発明の第８の実施の形態に係る装置の構成を示す機能ブロック図である。
【０１２６】
第８の実施の形態にあっては、図示の如く、第７の実施の形態の構成を並列させるようにした。即ち、第１原波形と第２原波形をそれぞれ加重平均して第５原波形を生成すると共に、第３原波形と第４原波形を重みＷ３，Ｗ４を用いつつ加重平均して第６原波形を並列的に生成し、それを重みＷ５，Ｗ６を用いつつ加重平均して目標上体高さを求めるようにした。ここで、重みＷ１，Ｗ３，Ｗ５、あるいはＷ２，Ｗ４，Ｗ６は、それぞれ異なる値でも同一な値でも良い。
【０１２７】
第８の実施の形態においては上記の如く構成したことで、構成としては従前の実施の形態に比較して複雑になるが、更に一層滑らかな上体軌跡を得ることができる。
【０１２８】
尚、これ以上に平滑化あるいは加重平均を繰り返す手法も考えられるが、演算処理量の割には効果は余り期待することができない。上体の上下加速度は低減しても、関節の速度、加速度は、逆に増加する傾向があるからである。
【０１２９】
図２２はこの発明の第９の実施の形態に係る装置の構成を示す説明図である。第９の実施の形態においては４足（４脚）歩行のロボットを対象とし、その歩容を生成するようにした。
【０１３０】
以下説明すると、図示の４足歩行ロボット１００は、上体（基体）１０２と、それに付け根関節１０４（前記した第１の関節）を介して連結される４本の脚部リンク１０６からなる。４本の脚部リンク１０６はそれぞれ、その先端（脚先端）１０８との間に膝関節１１０（前記した第２の関節）を備える。図示はしないが、各関節にはアクチュエータが設けられる。
【０１３１】
図示の４足歩行ロボット１００においても従前の実施の形態での２足歩行ロボット１と同様に、上体１０２の水平位置（Xb, Yb）および姿勢（θxb, θyb，θzb）を求め、次いで上体の高さＺｂを求めて関節１０４，１１０を駆動制御し、よって図に矢印で示す方向に進行させることができる。
【０１３２】
上記で、図２３に示す如く上体高さＺｂは、上体１０２の底面の中心１０２ａと床面１１２との鉛直方向（重力方向）距離で示す。また４本の脚部リンク１０６の膝関節１１０の角度をθｎ、脚先端１０８の位置を（Xn, Yn, Zn）（ｎ：１〜４）で示す。
【０１３３】
第９の実施の形態に係る装置の動作を図２４フロー・チャートを参照して説明すると、Ｓ６００からＳ６０４まで従前の実施の形態と同様の処理を行った後、Ｓ６０６に進んで時刻ｔにおける目標上体水平位置（Xb, Yb）、目標上体姿勢（θxb, θyb，θzb）、および目標脚先端位置（Xn，Ｙn, Zn ）（ｎ：１〜４）を求め、Ｓ６０８に進んで目標上体高さＺｂを求める。
【０１３４】
図２５はその作業を示すサブルーチン・フロー・チャートであり、以下説明すると、Ｓ７００，Ｓ７０２を経てＳ７０４に進み、目標上体水平位置（Xb,Yb)、仮目標上体高さＺｂ、上体姿勢（θxb, θyb，θzb）、および脚先端位置（Xn, Yn, Zn）から、各脚の目標付け根関節１０４の位置（座標）と目標脚先端１０８の位置の差（ΔXn，ΔYn，ΔZn）を求める。図２３にこれらの値を示す。
【０１３５】
次いでＳ７０６に進んで求めた差から図示の式を用いて膝関節角θｎを求める。次いでＳ７０８に進んで差Err を図示の如く算出し、Ｓ７１０に進んで差（絶対値）が許容値以内か否か判断し、肯定されるときはＳ７１２に進むと共に、否定されるときはＳ７１４に進んで感度Ｓを求め、Ｓ７１６において値Ｚｂを補正する。ここで、感度Ｓは同図末尾に示すように算出される。
【０１３６】
第９の実施の形態は上記の如く構成したことから、４足（４脚）歩行ロボットにおいても、２足（２脚）歩行ロボットと同様に最適に上体高さを求めることができる。尚、４足歩行ロボットの例を示したが、３足あるいは５足以上の脚部を有するロボットであっても同様である。
【０１３７】
上記の如く、少なくとも上体２４と、前記上体に第１の関節１０，１２，１４Ｒ（Ｌ）を介して連結される複数本の脚部リンク２とからなり、前記脚部リンクがその先端部までに少なくとも１個の第２の関節１６Ｒ（Ｌ）を有してなる脚式移動ロボットの歩容生成装置において、前記上体の水平方向目標位置および姿勢の少なくともいずれかを含み、前記上体の鉛直方向目標位置を除く、上体軌跡を設定する上体軌跡設定手段（Ｓ１６）、前記脚部リンク先端部の目標位置および姿勢の少なくともいずれかを含む脚部リンク軌跡を設定する脚部リンク軌跡設定手段（Ｓ１６）、および前記設定された上体軌跡および脚部リンク軌跡に基づいて前記上体の鉛直方向位置に関する連続関数からなる等式を満足する解を求め、その解を基に前記上体の鉛直方向目標位置を決定する上体鉛直方向目標位置決定手段（Ｓ１８，Ｓ１００ないしＳ１１６，Ｓ２００ないしＳ２１８，Ｓ３００ないしＳ３０２，Ｓ４００ないしＳ４０６，Ｓ５００ないしＳ５０８，Ｓ６０８，Ｓ７００ないしＳ７１６）を備える如く構成した。
【０１３８】
また、前記上体の鉛直方向位置に関する連続関数からなる等式が、前記第１および第２の関節、具体的には第２の関節１６Ｒ（Ｌ）の少なくともいずれかの変位に関する滑らかな連続関数からなる等式である如く構成した。
【０１３９】
また、前記第１および第２の関節の少なくともいずれかの変位に関する滑らかな連続関数からなる等式が、前記第２の関節１６Ｒ（ｌ）の変位および速度の少なくともいずれか、具体的には変位に関して対称式となっている如く構成した。
【０１４０】
また、前記上体の鉛直方向位置が大きくなるにつれて前記連続関数の値が単調に変化する如く構成した。
【０１４１】
また、前記上体の水平方向目標位置あるいは姿勢および前記脚部リンク先端部の目標位置あるいは姿勢の少なくともいずれかが、具体的には足平２２Ｒ（Ｌ）、より具体的には遊脚足首関節１８，２０Ｒ（Ｌ）などが仮想的な値である如く構成した。
【０１４２】
また、前記上体鉛直方向目標位置決定手段は、前記上体の鉛直方向の目標位置をｎ個（ｎ≧１）決定するｎ目標位置決定手段（Ｓ４００ないしＳ４０２，Ｓ５００ないしＳ５０２）、および前記解と前記ｎ個の目標位置との平均値を求める平均値算出手段（Ｓ４０４，Ｓ４０６，Ｓ５０４，Ｓ５０６）を備え、求めた平均値に基づいて前記上体の鉛直方向目標位置を決定する如く構成した。
【０１４３】
また、前記平均値が加重平均値（Ｓ４０４，Ｓ４０６，Ｓ５０４，Ｓ５０６）である如く構成した。
【０１４４】
また、前記上体鉛直方向目標位置決定手段は、前記解と求めた平均値の少なくともいずれかを平滑化する平滑化手段（フィルタ、Ｓ３０２，Ｓ５０８）を備える如く構成した。
【０１４５】
また、前記上体鉛直方向目標位置決定手段は、探索法あるいは収束演算の少なくともいずれかを用いて前記解を求める如く構成した。
【０１４６】
また、更に、前記設定された上体軌跡と、脚部リンク軌跡と、決定された上体鉛直方向目標位置に基づいて目標関節角を求める目標関節角算出手段（Ｓ２０）、および前記目標関節角となるように前記第１および第２の関節を変位制御する変位制御手段（Ｓ２２）を含む如く構成した。
【０１４７】
また、前記脚式移動ロボットが２脚を有する歩行ロボットであり、前記第１の関節が股関節１０，１２，１４Ｒ（Ｌ）、前記第２の関節が膝関節１６Ｒ（Ｌ）である如く構成した。
【０１４８】
また、前記脚式移動ロボットが３脚以上の脚部を有する多脚歩行ロボットであり、前記第１の関節が付け根股関節１０４、前記第２の関節が膝関節１１０である如く構成した。
【０１４９】
以上、この発明を第１の実施の形態から第９の実施の形態までを参照して説明してきたが、ここで第１の実施の形態などで述べた構成について付言すると、滑らかな連続関数f(θsup,θswg ）としては、式１以外でも良いが、θsup またはθswg が零またはある小さい値に近づくとf(θsup,θswg ）の値が極めて大きくなるように選ぶのが望ましい。
【０１５０】
尚、ｆ＝Ｃは、１／ｆ＝１／Ｃや−ｆ＝−Ｃなどに変形することができるので、上記した実施の形態で挙げた単調連続関数ｆの代わり、１／ｆや−ｆを新たな連続関数として置き換えれば、その関数は単調減少関数となる。従って、関数としては、単調減少関数であっても良い。
【０１５１】
また、第１の実施の形態では、滑らかな連続関数ｆとして膝関節に関する関数を選んでいるが、その他の関節に関する関数であったり、全関節に関する関数であっても良い。そのような場合でも、図示のロボットでは全ての関節角は上体の位置・姿勢と脚先端部の位置・姿勢から逆キネマティクス演算によって求めることができ、関節角に関する関数ｆは上体高さに関する関数ｇに変換することができるので、第１の実施の形態の手法で上体高さを決定することができる。
【０１５２】
また、連続関数ｆとしては、関節変位だけではなく、関節速度に関する項を含んでいても良い。
【０１５３】
また、上体高さを重力方向について求めたが、絶対的に重力方向に対しての値でなくても良く、例えば重力方向について傾斜した軸に関する上体高さであっても良い。
【０１５４】
一方の脚の関節数が６個を超えるロボットでは、上体の位置・姿勢と脚先端部の位置・姿勢を決めても、関節角を一義的には決定できない。この場合でも、関節角に関するある拘束式を与えれば、関節角を一義的に決定することができる。こうすれば、第１の実施の形態と同様の手法で上体高さを決定することができる。
【０１５５】
値Ｃは第１の実施の形態では定数であったが、時変であっても良い。基本原理の性質２から、Ｃがある値以上であれば、等式を満足する上体高さの解が存在するからである。但し、ロボットの動きを滑らかにするためには、Ｃは滑らかに変化させるべきである。関数ｆおよび関数ｇも時変形であっても良い。
【０１５６】
ロボットの構造が図示のように左右対称である場合には、関節角の関数ｆとして、左右関節に対する対称式を採用すれば、左右対称の歩行を得ることができる。
【０１５７】
また、従前の実施の形態で用いた重みＷは、総和が常に１になっていれば、時間によって変化させても良い。但し、滑らかに変化させないと、上体の上下加速度が過大になる。滑らかに変化させる手法として、先に本出願人が提案したフィルタ（特開平５−３２４１１５号公報記載の技術）を用いても良い。
【０１５８】
また、原波形と複数の異なる原波形との間の加重平均を行えば、上体の上下加速度振幅を小さくすることができる。但し、関節角の速度振幅、加速度振幅は大きくなる傾向がある。また、目標上体高さを満足する実ロボットの姿勢（全関節角の組）が存在する可能性が低くなる場合がある。
【０１５９】
また、この発明を２足歩行ロボットおよび４足歩行ロボットに関して説明してきたが、それらに限らず、３足あるいは５足以上の多脚ロボットにも応用することができる。それが壁面移動ロボットであれば、「上体高さ」は壁面から上体までの垂直距離と置き換えられよう。
【０１６０】
また、歩行状態および／または外界センサによる環境情報によって各種の実施の形態に挙げた手法を切り換える、あるいは加重平均の重みを変更するようにしても良い。
【０１６１】
【発明の効果】
上体の上下動および関節の動きが滑らかで、かつ変位、速度および加速度の振幅が小さくなるように、適正に上体高さを決定することができると共に、上体高さを除くパラメータが与えられるとき、歩行中にリアルタイムに上体高さを決定して臨機応変に自在な歩行を実現することができる。
【０１６２】
更には決定された上体高さを含む軌跡を実現するようにロボットの関節を変位制御することができる。
【図面の簡単な説明】
【図１】この発明に係る脚式移動ロボットの歩容生成装置を全体的に示す説明図である。
【図２】図１に示す２足歩行ロボットについてパラメータなどを定義するスケルトン図である。
【図３】図１に示す装置の制御ユニットの詳細を示すブロック図である。
【図４】この発明に係る脚式移動ロボットの歩容生成装置の構成を示す機能ブロック図である。
【図５】図４の装置の動作を示すメイン・フロー・チャートである。
【図６】図５のフロー・チャートの上体高さ決定作業を示すサブルーチン・フロー・チャートである。
【図７】図１および図２に示す２足歩行ロボットを２次元モデルとして示した説明図である。
【図８】この発明の第２の実施の形態に係る装置の構成を示す機能ブロック図である。
【図９】図８の装置の動作の中の上体高さ決定作業を示すサブルーチン・フロー・チャートである。
【図１０】図９のフローチャートで用いる遊脚足平仮想軌跡を示すタイミング・チャートである。
【図１１】図１０の仮想遊脚足平軌跡に対する上体の軌跡を示すタイミング・チャートである。
【図１２】この発明の第３の実施の形態に係る装置の動作を示す遊脚足平軌跡を示す説明図である。
【図１３】図１２の装置で生成される上体高さを示すタイミング・チャートである。
【図１４】この発明の第４の実施の形態に係る装置の構成を示す機能ブロック図である。
【図１５】この発明の第５の実施の形態に係る装置の構成を示す機能ブロック図である。
【図１６】図１５の装置の動作の中の上体高さ決定作業を示すサブルーチン・フロー・チャートである。
【図１７】この発明の第６の実施の形態に係る装置の構成を示す機能ブロック図である。
【図１８】図１７の装置の動作の中の上体高さ決定作業を示すサブルーチン・フロー・チャートである。
【図１９】この発明の第７の実施の形態に係る装置の構成を示す機能ブロック図である。
【図２０】図１９の装置の動作の中の上体高さ決定作業を示すサブルーチン・フロー・チャートである。
【図２１】この発明の第８の実施の形態に係る装置の構成を示す機能ブロック図である。
【図２２】この発明の第９の実施の形態に係る装置で、４足歩行ロボットに適用された例を示す説明図である。
【図２３】図２２の装置の中のパラメータの定義を示す説明図である。
【図２４】この発明の第９の実施の形態に係る装置の動作を示すメイン・フロー・チャートである。
【図２５】図２４の装置の動作の中の上体高さ決定作業を示すサブルーチン・フロー・チャートである。
【符号の説明】
１                      ２足歩行ロボット（脚式移動ロボット）
２                      脚部リンク
１０，１２，１４Ｒ，Ｌ  股関節（第１の関節）
１６Ｒ，Ｌ              膝関節（第２の関節）
１８，２０Ｒ，Ｌ        足関節
２２Ｒ，Ｌ              足平（脚部リンク先端部）
２４                    上体
２６                    制御ユニット
１００                  ４足歩行ロボット（脚式移動ロボット）
１０２                  上体
１０４                  付け根関節（第１の関節）
１０６                  脚部リンク
１０８                  脚先端
１１０                  膝関節（第２の関節）
【図１】
0003663034.tif 000005
【図２】
0003663034.tif 000006
【図３】
0003663034.tif 000007
【図４】
0003663034.tif 000008
【図５】
0003663034.tif 000009
【図６】
0003663034.tif 000010
【図７】
0003663034.tif 000011
【図８】
0003663034.tif 000012
【図９】
0003663034.tif 000013
【図１０】
0003663034.tif 000014
【図１１】
0003663034.tif 000015
【図１２】
0003663034.tif 000016
【図１３】
0003663034.tif 000017
【図１４】
0003663034.tif 000018
【図１５】
0003663034.tif 000019
【図１６】
0003663034.tif 000020
【図１７】
0003663034.tif 000021
【図１８】
0003663034.tif 000022
【図１９】
0003663034.tif 000023
【図２０】
0003663034.tif 000024
【図２１】
0003663034.tif 000025
【図２２】
0003663034.tif 000026
【図２３】
0003663034.tif 000027
【図２４】
0003663034.tif 000028
【図２５】
0003663034.tif 000029