(57)【特許請求の範囲】
【請求項１】
移動手段を有するロボット装置において、
前記移動手段と床面とが接地している際に、前記床面に接地する前記移動手段の複数の接地部位が形成する支持多角形内の、ある点回りの水平軸回りモーメントがゼロ又はゼロ近傍となり、且つ、前記移動手段と床面とが接地していないときには、前記ロボット装置に作用する水平方向外力がゼロ又はゼロ近傍となり、さらに現在の前記ロボット装置の少なくとも重心水平位置及び速度に以後の重心水平位置及び速度が連続的に接続するように前記ロボット装置の重心水平位置軌道を算出する重心水平位置軌道決定手段と、
前記移動手段と床面とが接地していない際に、前記ロボット装置に作用する垂直方向並進力が、重力以外にはゼロ又はゼロ近傍の大きさとなり、且つ、現在の前記ロボット装置の少なくとも重心垂直位置及び速度に以後の重心垂直位置及び速度が連続的に接続するように前記ロボット装置の重心垂直位置軌道を算出する重心垂直位置軌道決定手段と、
前記移動手段と床面とが接地していない際に、前記ロボット装置の重心回りモーメントがゼロ又はゼロ近傍の大きさとなり、且つ、前記重心水平位置軌道決定手段及び前記重心垂直位置軌道決定手段により決定された重心位置を満たすように、現在以後の前記ロボット装置の運動状態を決定する運動状態決定手段と、
を具備することを特徴とするロボット装置。
【請求項２】
前記重心水平位置軌道決定手段は、前記ロボット装置の重心水平位置軌道に関する境界条件付き運動方程式の解を算出することにより前記重心水平位置軌道を決定し、
前記重心垂直位置軌道決定手段は、前記ロボット装置の重心垂直位置軌道に関する境界条件付き運動方程式の解を算出することにより前記重心位置垂直軌道を決定する、
ことを特徴とする請求項１に記載のロボット装置。
【請求項３】
前記運動状態決定手段は、現在の前記ロボット装置の運動状態から算出されるヤコビアンを用いて、以後の前記ロボット装置の運動状態を決定する、
ことを特徴とする請求項１に記載のロボット装置。
【請求項４】
前記重心水平位置軌道決定手段は、前記移動手段と床面とが接地している際に、前記支持多角形内のある点回りの水平軸回りモーメントがゼロ又はゼロ近傍の大きさとなる代わりに、前記ロボット装置のロール軸及びピッチ軸回りのモーメントがゼロとなるＺＭＰが前記支持多角形内に存在するように、前記ロボット装置の重心水平位置軌道に関する境界条件付き運動方程式の解を算出する、
ことを特徴とする請求項２に記載のロボット装置。
【請求項５】
前記重心水平位置軌道決定手段は、前記移動手段と床面とが無接地の際に、前記水平方向並進力がゼロ又はゼロ近傍の大きさとなる代わりに、水平方向運動量が略一定となるように前記ロボット装置の重心水平位置軌道に関する境界条件付き運動方程式の解を算出する、
ことを特徴とする請求項２に記載のロボット装置。
【請求項６】
前記ロボット装置の重心位置及び重心速度状態を計測する重心位置・速度状態計測手段をさらに備え、
前記重心水平位置軌道決定手段及び前記重心垂直位置軌道決定手段は、前記重心位置・速度状態計測手段によって計測された現在の前記ロボット装置の重心位置及び重心速度状態を、以後の境界条件付き運動方程式の初期境界条件として設定する、
ことを特徴とする請求項２に記載のロボット装置。
【請求項７】
前記運動状態決定手段は、前記ロボット装置の基体の姿勢角を操作量として前記ロボット装置の運動状態を決定する、
ことを特徴とする請求項２に記載のロボット装置。
【請求項８】
前記運動状態決定手段は、前記ロボット装置の腰関節角を操作量として、前記ロボット装置の運動状態を決定する、
ことを特徴とする請求項２に記載のロボット装置。
【請求項９】
前記運動状態決定手段は、前記移動手段と床面とが無接地の際に、前記ロボット装置の重心回りモーメントがゼロ又はゼロ近傍の大きさとなる代わりに、重心回り角運動量が一定となるように、以後の前記ロボット装置の運動状態を決定する、
ことを特徴とする請求項２に記載のロボット装置。
【請求項１０】
移動手段を有するロボット装置の運動制御方法において、
要求運動を入力する運動入力ステップと、
該入力された運動を１以上の時間区間で分割するステップと、
分割された各区間毎に前記移動手段が接地又は無接地状態のいずれであるかを判別するステップと、
前記移動手段と床面とが接地している際には、前記床面に接地する前記移動手段の複数の接地部位が形成する支持多角形内の、ある点回りの水平軸回りモーメントがゼロ又はゼロ近傍となり、且つ、現在の前記ロボット装置の少なくとも重心水平位置及び速度に以後の重心水平位置及び速度が連続的に接続するように前記ロボット装置の重心水平位置軌道を算出し、前記移動手段と床面とが接地していない際には、前記ロボット装置に作用する水平方向外力がゼロ又はゼロ近傍となり、且つ、現在の前記ロボット装置の少なくとも重心水平位置及び速度に以後の重心水平位置及び速度が連続的に接続するように前記ロボット装置の重心水平位置軌道を算出する重心水平位置軌道決定ステップと、
前記移動手段と床面とが接地していない際に、前記ロボット装置に作用する垂直方向並進力が、重力以外にはゼロ又はゼロ近傍の大きさとなり、且つ、現在の前記ロボット装置の少なくとも重心垂直位置及び速度に以後の重心垂直位置及び速度が連続的に接続するように前記ロボット装置の重心垂直位置軌道を算出する重心垂直位置軌道決定ステップと、
前記移動手段と床面とが接地していない際に、前記ロボット装置の重心回りモーメントがゼロ又はゼロ近傍の大きさとなり、且つ、前記重心水平位置軌道決定ステップ及び前記重心垂直位置軌道決定ステップにより決定された重心位置を満たすように、現在以後の前記ロボット装置の運動状態を決定する運動状態決定ステップと、
を具備することを特徴とするロボット装置の運動制御方法。
【請求項１１】
前記重心水平位置軌道決定ステップでは、前記ロボット装置の重心水平位置軌道に関する境界条件付き運動方程式の解を算出することにより前記重心水平位置軌道を決定し、
前記重心垂直位置軌道決定ステップでは、前記ロボット装置の重心垂直位置軌道に関する境界条件付き運動方程式の解を算出することにより前記重心位置垂直軌道を決定する、
ことを特徴とする請求項１０に記載のロボット装置の運動制御方法。
【請求項１２】
前記運動状態決定ステップでは、現在の前記ロボット装置の運動状態から算出されるヤコビアンを用いて、以後の前記ロボット装置の運動状態を決定する、
ことを特徴とする請求項１０に記載のロボット装置の運動制御方法。
【請求項１３】
前記重心水平位置軌道決定ステップでは、前記移動手段と床面とが接地している際に、前記支持多角形内のある点回りの水平軸回りモーメントがゼロ又はゼロ近傍の大きさとなる代わりに、前記ロボット装置のロール軸及びピッチ軸回りのモーメントがゼロとなるＺＭＰが前記支持多角形内に存在するように、前記ロボット装置の重心水平位置軌道に関する境界条件付き運動方程式の解を算出する、
ことを特徴とする請求項１１に記載のロボット装置の運動制御方法。
【請求項１４】
前記重心水平位置軌道決定ステップでは、前記移動手段と床面とが無接地の際に、前記水平方向並進力がゼロ又はゼロ近傍の大きさとなる代わりに、水平方向運動量が略一定となるように前記ロボット装置の重心水平位置軌道に関する境界条件付き運動方程式の解を算出する、
ことを特徴とする請求項１１に記載のロボット装置の運動制御方法。
【請求項１５】
前記ロボット装置の重心位置及び重心速度状態を計測する重心位置・速度状態計測ステップをさらに備え、
前記重心水平位置軌道決定ステップ及び前記重心垂直位置軌道決定ステップでは、前記重心位置・速度状態計測ステップにおいて計測された現在の前記ロボット装置の重心位置及び重心速度状態を、以後の境界条件付き運動方程式の初期境界条件として設定する、
ことを特徴とする請求項１１に記載のロボット装置の運動制御方法。
【請求項１６】
前記運動状態決定ステップでは、前記ロボット装置の基体の姿勢角を操作量として前記ロボット装置の運動状態を決定する、
ことを特徴とする請求項１１に記載のロボット装置の運動制御方法。
【請求項１７】
前記運動状態決定ステップでは、前記ロボット装置の腰関節角を操作量として、前記ロボット装置の運動状態を決定する、
ことを特徴とする請求項１１に記載のロボット装置の運動制御方法。
【請求項１８】
前記運動状態決定ステップでは、前記移動手段と床面とが接地していない際に、前記ロボット装置の重心回りモーメントがゼロ又はゼロ近傍の大きさとなる代わりに、重心回り角運動量が一定となるように、以後の前記ロボット装置の運動状態を決定する、
ことを特徴とする請求項１１に記載のロボット装置の運動制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、少なくとも複数本の可動脚を備えたロボット装置、ロボット装置の運動制御装置並びに運動制御方法に係り、特に、歩行などの接地期間のみからなる運動以外に、走行や跳躍などの接地期間と無接地期間とが混在するロボット装置、ロボット装置の運動制御装置並びに運動制御方法に関する。
【０００２】
さらに詳しくは、本発明は、歩行・停止状態と走行・跳躍との遷移に対応し、不規則な接地運動・無接地運動間の遷移に対しても動力学的バランスを適切に維持しながらシームレスに運動状態を変更するロボット装置、ロボット装置の運動制御装置並びに運動制御方法に係り、特に、接地運動・無接地運動間を遷移する安定な運動パターンを実時間で生成するロボット装置、ロボット装置の運動制御装置並びに運動制御方法に関する。
【背景技術】
【０００３】
電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行う機械装置のことを「ロボット」という。ロボットの語源は、スラブ語の“ＲＯＢＯＴＡ（奴隷機械）”に由来すると言われている。わが国では、ロボットが普及し始めたのは１９６０年代末からであるが、その多くは、工場における生産作業の自動化・無人化などを目的としたマニピュレータや搬送ロボットなどの産業用ロボット（ｉｎｄｕｓｔｒｉａｌ ｒｏｂｏｔ）であった。
【０００４】
最近では、ヒトやサルなどの２足直立歩行を行う動物の身体メカニズムや動作を模した脚式移動ロボットに関する研究開発が進展し、実用化への期待も高まってきている。ヒトの生体メカニズムや動作を再現した脚式移動ロボットのことを、特に、「人間形」、若しくは「人間型」のロボット（ｈｕｍａｎｏｉｄ ｒｏｂｏｔ）と呼ぶ。
【０００５】
２足直立による脚式移動は、クローラ式や、４足又は６足式などに比し不安定で姿勢制御や歩行制御が難しくなるが、不整地や障害物など作業経路上に凹凸のある歩行面や、階段や梯子の昇降など不連続な歩行面に対応することができるなど、柔軟な移動作業を実現できるという点で優れている。
【０００６】
２足の脚式移動ロボットに関する姿勢制御や安定歩行に関する技術は既に数多提案されている。ここで言う安定な「歩行」とは、「転倒することなく、脚を使って移動すること」と定義することができる。ロボットの姿勢安定制御は、ロボットの転倒を回避する上で非常に重要である。何故ならば、転倒は、ロボットが実行中の作業を中断することを意味し、且つ、転倒状態から起き上がって作業を再開するために相当の労力や時間が払われるからである。また、転倒によって、ロボット本体自体、あるいは転倒するロボットと衝突する相手側の物体にも、致命的な損傷を与えてしまう危険があるからである。
【０００７】
脚式移動ロボットの姿勢安定制御や歩行時の転倒防止に関する提案の多くは、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）を歩行の安定度判別の規範として用いている。ＺＭＰによる安定度判別規範は、歩行系から路面には重力と慣性力、並びにこれらのモーメントが路面から歩行系への反作用としての床反力並びに床反モーメントとバランスするという「ダランベールの原理」に基づく。力学的推論の帰結として、足底接地点と路面の形成する支持多角形（すなわちＺＭＰ安定領域）の辺上あるいはその内側にピッチ軸及びロール軸モーメントがゼロとなる点、すなわち「ＺＭＰ」が存在する（例えば、非特許文献１を参照のこと）。
【０００８】
要約すれば、ＺＭＰ規範とは、「歩行のあらゆる瞬間において、ＺＭＰが足部と路面とが形成する支持多角形の内側に存在し、且つ、ロボットが路面に押す方向の力が作用すれば、ロボットが転倒（機体が回転運動）することなく安定に歩行できる」とするものである。
【０００９】
ＺＭＰ規範に基づく２足歩行パターン生成によれば、足底着地点をあらかじめ設定することができ、路面形状に応じた足先の運動学的拘束条件を考慮し易いなどの利点がある。また、ＺＭＰを安定度判別規範とすることは、力ではなく軌道を運動制御上の目標値として扱うことを意味するので、技術的に実現可能性が高まる。
【００１０】
ところで、従来開発されてきた脚式移動ロボットの移動形態は、いずれかの脚部が常に路面との接触を維持する「歩行」によるものが大半である。通常運用時の脚式移動ロボットにとって、歩行は、機械系・電気系に対して過大な負荷を与えることなく、安全且つ効率的に移動可能な、最適な移動形態と言える。
【００１１】
一方、脚式移動ロボットがその歩行機能だけでは対応できない状況として、地面からの反力が得られない、無接地状態を包含する場合が考えられる。脚式移動ロボットが地面の存在する重力環境中で動作する場合、長期に亘って無接地状態が連続する状況はないが、短期的・断続的無接地状態が発生する状況は多く存在すると考えられる。例えば、ロボットが空隙部を飛び越える、段上から飛び降りる、移動速度を上げるために走行する、バランス維持のために飛び跳ねて立脚位置を変更するなどである。
【００１２】
このような脚式移動ロボットの無接地状態におけるバランス制御を扱った先駆的研究が散見される（例えば、非特許文献２並びに非特許文献３を参照のこと）。しかしながら、これらは、以下のような理由で、実用的な脚式移動ロボットの制御系として利用されるには至らなかった。
【００１３】
（１）無質量脚を仮定しているので、機械設計自由度に対する制約が大きい。
（２）連続跳躍のみを仮定していて、歩行や停止状態との遷移に関するダイナミクスが十分には検討されていない。
（３）足先軌道に関し、幾何的拘束条件を課し難い。
【００１４】
また、ＺＭＰ規範の拡張形として、接地・無接地が混在する場合における脚式移動ロボットの運動パターンの算出方法について提案されている（例えば、非特許文献４を参照のこと）。しかしながら、同文献では、非線形方程式を解くために収束演算を用いていることから、計算負荷が高く、オフラインであらかじめ運動パターンを算出しておかなければならない。すなわち、実時間で歩行・走行・跳躍などの接地期間と無接地期間が混在する運動パターンを生成できるには至っていない。
【００１５】
歩行制御に関しては、ＺＭＰ規範に基づいて運動パターンを算出する方法が今日の脚式移動ロボットの有力な基礎的制御手段となっている（前述）。ＺＭＰをロボットの機体安定度判別規範に採用する最大のメリットは、足先の幾何的拘束条件の与え易さと、広範な機械モデルへの適用が可能であるなど、その実用性の高さにある。例えば、動作中にオンボードで安定な運動パターンを生成しながら歩行可能とする、実時間歩行パターン生成の技術も確立し、柔軟な制御系の構成に関する報告もある（例えば、非特許文献５を参照のこと）。
【００１６】
一方、走行・跳躍運動などの無接地期間を含む運動に関しては、上述したように、実用的な制御手段は提案されていない状況にある。
【００１７】
接地・無接地期間が混在する脚式運動の実用化の障害となる第一の問題として、機械モデルへの制約が大きい制御方式が大半であるということが挙げられる。特に、滞空時の脚の運動によるロボットの慣性変動を扱う困難さゆえに脚の質量を無視している方式が多いため（例えば、非特許文献２並びに非特許文献３を参照のこと）、多くの実用的脚式移動ロボットに適用することができない。
【００１８】
今日の人間形ロボットは、片脚に６自由度以上の自由度を有するものが標準的である。このように脚部に多自由度を有する脚式移動ロボットでは、多くのアクチュエータを搭載しなければならないが、そのすべてを基体（腰部など機体の重心付近の部位）に搭載することは困難であり、脚部内に相当数のアクチュエータが組み込まれている（ワイヤ伝導による不用意なトルク干渉を起こさないためとの理由もあるだろう）。また、そもそも脚部は脚式移動ロボットの全重量を支持する部位であり、機械的強度の確保が必須であるため、構造材が相当量の重量を有している場合が多い。
【００１９】
以上のような理由から、脚の質量は増大し、力学モデル上無視できないレベルとなるのが一般的である。このような脚質量を有する脚式移動ロボットに対して従来手法を適用することは困難であろう。
【００２０】
また、第２の問題として、歩行・停止状態との遷移に対応可能な走行・跳躍制御アルゴリズムが少ないという点が挙げられる。
【００２１】
既に述べたように、脚式移動ロボットが常時走行・跳躍を行なっているケースは稀であり、多くの状況下では停止又は歩行状態にあり、必要に応じて走行・跳躍を実行した後、再度、歩行・停止状態に復帰すると考えられる。場合によっては，接地状態から咄嗟に跳躍を行なう状況もあれば、徐々に歩行を加速して走行に至り、滑らかに減速して再度歩行・停止に至る状況もある。
【００２２】
このような多様且つ不規則な接地運動・無接地運動間の遷移に対しても、動力学的バランスを適切に維持しながらシームレスに運動状態の変更が可能なより一般的な制御方式が切望されている。
【００２３】
また、第３の問題として、幾何的な拘束条件を付加できない走行・跳躍制御方式が大半であるという点が挙げられる。
【００２４】
特に、脚部エンド・ポイントの慣性座標系内位置・姿勢軌道に幾何的拘束を課したい状況は多く存在するが、接地点が機体のバランス維持のために操作される方式が多い。脚式移動ロボットを定量的に制御するには、足先の軌道を明示的に指定できることが望ましい。例えば、走行時の歩幅・周期、跳躍時の足底高さなどのパラメータはユーザ・プログラムより指定可能とするのが望ましい。階段を駆け登る、あるいは連続的な複数の空隙を飛び越えるなどの要求が発生した場合には、足先の軌道をバランス維持のために操作するのは望ましくない。
【００２５】
ＺＭＰ規範に基づく歩行パターン生成による歩行制御方式は、このような運動学的拘束を課すことができることが大きな実用上の利点である。これは無接地期が存在する運動に拡張した場合にも変わらない要求である。
【００２６】
また、第４の問題として、非同期な外部要求を満足するように実時間で運動パターンの変更が可能な方式が切望されていることが挙げられる。
【００２７】
脚式移動ロボットが実環境中を自由に移動可能とするためには、さまざまなタイミングで入力される歩幅・ステップ周期・旋回角・足上げ高さなどの下肢の運動変化要求や、上体の運動変化要求を満足するよう、力学的バランス平衡を保ったまま、即応的に全身運動を再計画する機能が必要である。あらかじめオフラインで運動パターンを作成するのでは、このような要求に応えることはできない。また、バランスの維持のみを行なう制御系では、このような外部要求を満足することは困難であろう。
【００２８】
【非特許文献１】ヴコブラトビッチ（Ｍｉｏｍｉｒ Ｖｕｋｏｂｒａｔｏｖｉ）著「脚式移動ロボット（ＬＥＧＧＥＤ ＬＯＣＯＭＯＴＩＯＮ ＲＯＢＯＴＳ）」（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社））
【非特許文献２】松岡清利著「反復跳躍モデル．バイオメカニズム ５」（東京大学出版会、１９８５年、ｐｐ．４５０１−４５０９）
【非特許文献３】マーク・Ｈ・レイバート（Ｍａｒｃ Ｈ．Ｒａｉｂｅｒｔ）外２名著「３次元単脚ホッピング・マシンを用いたバランスに関する実験（Ｅｘｐｅｒｉｍｅｎｔｓ Ｉｎ Ｂａｌａｎｃｅ ｗｉｔｈ ａ  ３Ｄ ｏｎｅ−Ｌｅｇｇｅｄ Ｈｏｐｐｉｎｇ Ｍａｃｈｉｎｅ）」（ロボット研究に関する国際機関誌（ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ）、１９８４年、Ｖｏｌ．３、 Ｎｏ．２，ｐｐ．７５−９２）
【非特許文献４】長阪憲一郎著「動力学フィルタによる人間型ロボットの全身運動生成」（１９９９年、東京大学工学部情報工学専攻博士論文）
【非特許文献５】特願２００２−２８８７４５号明細書
【発明の開示】
【発明が解決しようとする課題】
【００２９】
本発明の目的は、歩行などの接地期間のみからなる運動以外に、走行や跳躍などの接地期間と無接地期間とが混在する、優れたロボット装置、ロボット装置の運動制御装置並びに運動制御方法を提供することにある。
【００３０】
本発明のさらなる目的は、歩行・停止状態と走行・跳躍との遷移に対応し、不規則な接地運動・無接地運動間の遷移に対しても動力学的バランスを適切に維持しながらシームレスに運動状態を変更することができる、優れたロボット装置、ロボット装置の運動制御装置並びに運動制御方法を提供することにある。
【００３１】
本発明のさらなる目的は、接地運動・無接地運動間を遷移する安定な運動パターンを実時間で生成することができる、優れたロボット装置、ロボット装置の運動制御装置並びに運動制御方法を提供することにある。
【課題を解決するための手段】
【００３２】
本発明は、上記課題を参酌してなされたものであり、移動手段を有するロボット装置であって、
前記移動手段と床面が接地する際に、前記床面に接地する前記移動手段の複数の接地部位が形成する支持多角形内のある点回りの水平軸回りモーメントがゼロ又はゼロ近傍となり、且つ、現在の制御周期における前記ロボット装置の少なくとも重心位置及び速度状態に次の制御周期のおける重心位置及び速度状態が連続的に接続するように前記ロボット装置の重心水平位置軌道に関する境界条件付き運動方程式の解を算出する重心水平位置軌道決定手段と、
前記移動手段と床面とが無接地の際に、重力以外に前記ロボット装置に作用する垂直方向並進力がゼロ又はゼロ近傍の大きさとなり、且つ、現在の制御周期における前記ロボット装置の重心垂直位置及び速度状態に、次の制御周期における重心垂直位置及び速度状態が連続的に接続するように前記ロボット装置の重心垂直位置軌道に関する境界条件付き運動方程式の解を算出する重心垂直位置軌道決定手段と、
前記移動手段と床面とが無接地の際に、前記ロボット装置の重心回りモーメントがゼロ又はゼロ近傍の大きさとなり、且つ、前記重心水平位置軌道決定手段及び前記重心垂直位置軌道決定手段により決定された重心位置を満たすように、次の制御周期における前記ロボット装置の運動状態を決定する運動状態決定手段と、を具備することを特徴とするロボット装置である。
【００３３】
前記重心水平位置軌道決定手段は、例えば前記移動手段と床面が接地する際に、前記ロボット装置に作用する水平軸回りモーメントと、少なくとも前記ロボット装置の重心位置及び速度状態の時間的連続性に基づいて、前記ロボット装置の重心水平位置軌道を算出する。また、前記重心垂直位置軌道決定手段は、例えば前記移動手段と床面とが無接地の際に、重力以外に前記ロボット装置に作用する垂直方向並進力と、少なくとも前記ロボット装置の重心垂直位置及び速度状態の時間的連続性に基づいて、前記ロボット装置の重心垂直位置軌道を算出する。また、前記運動状態決定手段は、例えば前記移動手段と床面とが無接地の際に、前記ロボット装置の重心回りモーメントと、前記重心水平位置軌道決定手段及び前記重心垂直位置軌道決定手段により決定された重心位置を満たすように、前記ロボット装置の運動状態を決定する。
【００３４】
本発明に係るロボット装置によれば、前記の重心水平位置軌道決定手段、及び前記の重心垂直位置軌道決定手段がそれぞれ重心水平位置軌道及び重心垂直位置軌道に関する境界条件付き運動方程式の解を制御周期毎に算出し、さらに運動状態決定手段が、ロボット装置の重心回りのモーメントを調整しながら、制御周期毎にロボット装置の運動状態を決定するので、ロボット装置の動作を実時間で制御することができる。
【００３５】
ここで、前記運動状態決定手段は、現在の制御周期における前記ロボット装置の運動状態から算出されるヤコビアンを用いて、ロボット装置に発生するモーメントを調整しながら、次の制御周期における前記ロボット装置の運動状態を決定するようにしてもよい。
【００３６】
また、前記重心水平位置軌道決定手段は、前記移動手段と床面が接地する際に、前記支持多角形内の点回りの水平軸回りモーメントがゼロ又はゼロ近傍の大きさとなる代わりに、前記ロボット装置のロール軸及びピッチ軸回りのモーメントがゼロとなるＺＭＰが前記支持多角形内に存在するように、前記ロボット装置の重心水平位置軌道に関する境界条件付き運動方程式の解を算出するようにしてもよい。
【００３７】
また、前記重心水平位置軌道決定手段は、前記移動手段と床面とが無接地の際には、前記水平方向並進力がゼロ又はゼロ近傍の大きさとなる代わりに、水平方向運動量が略一定となるように前記ロボット装置の重心水平位置軌道に関する境界条件付き運動方程式の解を算出するようにしてもよい。
【００３８】
また、本発明に係るロボット装置は、前記ロボット装置の重心位置及び速度状態を計測する重心位置・速度状態計測手段をさらに備えていてもよい。このような場合、前記重心水平位置軌道決定手段及び前記重心垂直位置軌道決定手段は、前記重心位置・速度状態計測手段によって計測された現在の制御周期における前記ロボット装置の重心位置及び速度状態を、次の制御周期における前記境界条件付き運動方程式の初期境界条件として設定することができる。
【００３９】
また、前記運動状態決定手段は、前記ロボット装置の基体の姿勢角を操作量として前記ロボット装置の運動状態を決定するようにしてもよい。
【００４０】
あるいは、前記運動状態決定手段は、前記ロボット装置の腰関節角を操作量として、前記ロボット装置の運動状態を決定するようにしてもよい。
【００４１】
また、前記運動状態決定手段は、前記移動手段と床面とが無接地の際に、前記ロボット装置の重心回りモーメントがゼロ又はゼロ近傍の大きさとなる代わりに、重心回り角運動量が一定となるように、次の制御周期における前記ロボット装置の運動状態を決定するようにしてもよい。
【発明の効果】
【００４２】
本発明によれば、歩行・停止状態と走行・跳躍との遷移に対応し、不規則な接地運動・無接地運動間の遷移に対しても動力学的バランスを適切に維持しながらシームレスに運動状態を変更することができる、優れた脚式移動ロボットの運動制御装置及び運動制御方法を提供することができる。
【００４３】
また、本発明によれば、接地運動・無接地運動間を遷移する安定な運動パターンを実時間で生成することができる、優れた脚式移動ロボットの運動制御装置及び運動制御方法を提供することができる。
【００４４】
第一に、本発明は、機械モデルへの制約事項が少ない点で、無質量脚仮定など（例えば、非特許文献２や非特許文献３）に比べて優位である。これは、本発明によれば、接地期及び無接地期においても全身の質点分布状態が動力学的拘束条件に反映されるということに依拠する。今日の一般的な脚式移動ロボットは、脚部内に相当数のアクチュエータが組み込まれ、脚質量を無視することができない。本発明は、とりわけ、単一の脚部当たり６自由度以上のアクチュエータが実装されるヒューマノイド・ロボットに対しても適用できる、高い汎用性・適用可能性を有している。
【００４５】
第二に、本発明によれば、接地・無接地が混在する脚式移動ロボットの運動制御が可能となる。従来の脚式移動ロボットの制御系は歩行に関るものが大半であり、走行・跳躍などの無接地期を含むより広範な運動制御系を有しているものは殆ど見られない。これに対し、本発明によれば、歩行・停止・走行・跳躍が混在したより一般的な運動を実行することができるので、空隙部を飛び越える、段上から飛び降りる、バランス維持のために飛び跳ねて立脚位置を変更するなど、脚式移動ロボットの路面踏破能力・バランス維持能力を著しく向上させることができる。
【００４６】
第三に、本発明はＺＭＰ安定判別規範（例えば、非特許文献１を参照のこと）に基づく方法を、無接地期を含むより一般的な運動に拡張し、脚部エンドポイントの軌道に幾何的拘束を課すことが容易な構成となっている。例えば、走行時の歩幅・周期、跳躍時の足底高さなどの要求値を、力学的バランスを維持しながら両立することができる。また、階段を駆け上る、視覚情報から得られた連続空隙の幅に応じて足を着床しながら走破するといった、足先軌道への拘束が重要となる状況に対応可能となる。これらは、エンドポイントの軌道に幾何的拘束を課すことができない従来の運動制御方式では実現が困難なケースである。
【００４７】
第四に、本発明は実時間で力学的に安定な運動パターンを生成することから、非同期に入力される歩幅・ステップ周期・旋回角・足上げ高さなどの下肢の運動変化要求や、上体の運動変化要求を、力学的バランス平衡を保ったまま満足することができ、柔軟且つ多様な無接地・接地状態混在運動を実現することが可能となる。あらかじめオフラインで運動パターンを作成する方法や、バランス維持のみを行なう従来の制御系では、このような外部要求を満足しながら力学的安定性を維持することは極めて困難である。
【００４８】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【発明を実施するための最良の形態】
【００４９】
本発明は、脚式移動ロボットにおける接地期間と無接地期間が混在するような運動に対して、力学的に安定な運動パターンを実時間で生成可能とする制御方法を提供するものである。本発明に係る制御方法により、脚式移動ロボットは、歩行のみならず、跳躍や走行、並びにこれらの運動が順次発現される状況において、歩幅・周期などの外部要求を満足しながら、力学的に安定な運動パターンを即応的に計算して動作することが可能となる。
【００５０】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００５１】
図１には、本発明の実施に供される脚式移動ロボットの自由度構成を模式的に示している。
【００５２】
同図に示すロボットは、基体に四肢が取り付けられ、肩関節ピッチ軸、肩関節ロール軸、上腕ヨー軸、肘関節ピッチ軸、前腕ヨー軸、手首ロール軸、手首ピッチ軸という７自由度からなる左右の腕部と、股関節ヨー軸、股関節ロール軸、股関節ピッチ軸、膝ピッチ軸、足首ピッチ軸、足首ロール軸という６自由度からなる左右の脚部で構成されている。左右の股関節を繋ぐ骨盤Ｂ１が基体リンクに相当し、上体は３自由度（ロール、ピッチ、ヨー）の腰部関節を介して基体リンクＢ１に接続され、基体リンクＢ１に二肢の６自由度脚部と上体が接続されている。
【００５３】
これらの各関節自由度は、実際にはアクチュエータ・モータにより実現される。本実施形態では、ギア直結型で且つサーボ制御系をワンチップ化してモータ・ユニットに内蔵したタイプの小型ＡＣサーボ・アクチュエータを搭載する。なお、この種のＡＣサーボ・アクチュエータに関しては、例えば本出願人に既に譲渡されている特開２０００−２９９９７０号公報（特願平１１−３３３８６号明細書）に開示されている。
【００５４】
機体の骨盤部には、加速度センサＡ１及びジャイロＧ１が搭載されている。また、左右の足底四隅には、足底面垂直方向の床反力を検出する１軸ロードセル（Ｆ１〜Ｆ８）と、床面までの距離を測定する赤外線測距センサ（Ｄ１〜Ｄ８）がそれぞれ４つ取り付けられている。また、左右の足底中央部には、それぞれ加速度センサ（Ａ２，Ａ３）及びジャイロ（Ｇ２，Ｇ３）が取り付けられている。
ここで、ロボットの質量をｍとし、慣性座標系内での重心位置をｘ＝（ｘ，ｙ，ｚ）Tとし、ロボットが外界から受ける力及びモーメントをそれぞれＦ及びＮとする。
【００５５】
ロボットの運動パターンは、闇雲に作成しても計画通り動作することはなく、運動パターンが発生せんとする力ｆ＝（ｆx，ｆy，ｆz）T及びモーメントｎ＝（ｎx，ｎy，ｎz）Tが実際の環境から反作用として得られるように設定されているときにのみ、計画通り動作することができる。すなわち、Ｆ＝ｆ、Ｎ＝ｎなる外力Ｆ、外モーメントＮの組み合わせが存在し得るように運動パターンが発生せんとする力ｆ、及びモーメントｎに拘束条件を課さなければならない（図２を参照のこと）。
【００５６】
本明細書では、このような力学的な平衡条件に関る拘束条件のことを「動力学的拘束条件」と呼ぶ。一方、足先軌道のように明示的に位置と時間の拘束を与える条件のことを「幾何学的拘束条件」と呼ぶ。
【００５７】
例えば、ロボットが平面に接地している場合の動力学的拘束条件は「ＺＭＰが接地多角形内にあること」と等価であることが知られている（前述）。すなわち、慣性座標系の接地多角形内に任意の点ｐ＝（ｐx，ｐy，ｐz）Tを考え、この点ｐ回りに発生するモーメントをＭ＝（Ｍx，Ｍy，Ｍz）Tとすると、下式を満たすことが動力学的拘束条件を与える。
【００５８】
【数１】
0003599244.tif 000002
【００５９】
上式（１）で表される動力学的拘束条件は、後述するように、運動パターンが発生せんとする力ｆ及びモーメントｎを用いた表記に変換することができる。
【００６０】
一方、無接地状態においては、環境より一切の外力及び外モーメントとも得ることができないので、以下の２式が動力学的拘束条件を与える。
【００６１】
【数２】
0003599244.tif 000003
【００６２】
本実施形態では、このような考えに基づき、接地期間に関しては上式（１）が成立し、無接地期間においては上式（２）及び（３）が成立するような全身の運動パターンを実時間で算出するアルゴリズムを構成することで、脚式移動ロボットにおける自在な歩行・走行・跳躍が可能な制御系を実現する。 以下、本明細書では、式（１）及び（２）を満足するためにロボットの重心位置を操作するとともに、式（３）を満足するためにロボットの基体（骨盤）姿勢を操作する例について述べる。
【００６３】
図３には、本実施形態に係る脚式移動ロボットの制御システムの機能構成を模式的に示している。同図に示すように、この制御システムは、運動要求入力部３Ａと、実時間全身運動生成部３Ｂと、全身関節駆動部３Ｃより構成される。実時間全身運動生成部３Ｂはさらに、足底位置・姿勢軌道生成部３１と、重心位置軌道生成部３２と、基体姿勢軌道生成部３３と、各軌道補間部（より具体的には、上体関節角参照軌道補間部３４、基体姿勢参照軌道補間部３５、重心位置参照軌道補間部３６、足底位置・姿勢参照軌道補間部３７）と、質点分布調整部３８と、関節角算出部３９から構成される。
【００６４】
運動要求入力部３Ａは、ユーザ・プログラム（あるいは図示しないその他の手段）により時々刻々と決定される上体（腕関節・腰関節）への運動要求（上体関節角参照軌道）と、基体（骨盤）の姿勢角に関る運動要求（基体姿勢基本軌道）と、重心垂直位置軌道に関る運動要求（重心垂直位置基本軌道）と、歩幅・運動周期・旋回角・滞空時間などの下肢運動に関わるパラメータ（下肢運動パラメータ）を入力し、実時間全身運動生成部３Ｂに出力する。
【００６５】
実時間全身運動生成部３Ｂは、動力学的拘束条件に基づく力学的バランス平衡を維持しつつ、現在の運動状態から滑らかに遷移可能で、ユーザ・プログラムからの上体・下肢運動変更要求を共に満足することのできる、次時刻のロボットの運動状態を決定し、全身の関節角参照値として出力する。
【００６６】
全身関節駆動部３Ｃは、実時間全身運動生成部３Ｂから出力される関節角参照値を実現するよう、全身の関節自由度を構成するアクチュエータ・モータをサーボ・コントローラ（図示しない）により駆動する。
【００６７】
実時間全身運動生成部３Ｂ内の足底位置・姿勢軌道生成部３１は、運動要求入力部３Ａより入力された下肢運動に関わるパラメータに基づき、現在の足底位置・姿勢から数歩先までの左右両足底の位置及び姿勢軌道を算出する。
【００６８】
また、重心位置軌道生成部３２は、運動要求入力部３Ａより入力された重心垂直位置基本軌道を最大限に反映しつつ、現在の重心位置軌道に滑らかに接続し、接地期間に関しては動力学的拘束条件としての上式（１）が、無接地期間においては動力学的拘束条件としての上式（２）が成立するような重心位置軌道を算出する。
【００６９】
また、基体姿勢軌道生成部３３は、運動要求入力部３Ａより入力された基体姿勢基本軌道を最大限に反映しつつ、現在の基体姿勢軌道に滑らかに接続し、上記重心位置軌道に係る接地期並びに無接地期の各期間における動力学的拘束条件としての上式（１）及び（２）に加え、無接地期に関して動力学的拘束条件としての上式（３）が成立するような基体姿勢角軌道を算出する。
【００７０】
各軌道補間部（上体関節角参照軌道補間部３４、基体姿勢参照軌道補間部３５、重心位置参照軌道補間部３６、並びに足底位置・姿勢参照軌道補間部３７）は、それぞれ上述のようにして算出された上体関節角参照軌道、基体姿勢参照軌道、重心位置参照軌道、並びに足底位置・姿勢参照軌道の補間処理を実行し、次時刻の上体関節角、基体姿勢角、重心位置、足底位置・姿勢を算出する、質点分布調整部３８は、これらの上体関節角、基体姿勢角、重心位置、足底位置・姿勢を同時に満足するような全身質点分布状態を調整する。
【００７１】
関節角算出部３９では、質点分布調整部３８で得られた次時刻の基体位置、及び足底位置・姿勢軌道生成部３１より得られる次時刻の足底位置姿勢を実現するように脚部の関節角を決定する。これは、両者の相対位置姿勢を求めた後、例えば、周知の逆キネマティクス計算によって実行することができる。
【００７２】
図４には、図３に示した制御システムにおいて実行される処理手順をフローチャートの形式で示している。
【００７３】
制御系は、処理開始後、運動要求入力部３Ａにおいて、上体関節角、基体姿勢、重心垂直位置、下肢運動への運動要求を入力する（ステップＳ１）。
【００７４】
次いで、足底位置・姿勢軌道生成部３１、重心位置軌道生成部３２、及び基体姿勢軌道生成部３３において、足底位置・姿勢軌道、重心位置軌道、基体姿勢軌道をそれぞれ生成する（ステップＳ２、Ｓ３、Ｓ４）。
【００７５】
次いで、上記の各軌道補間部（上体関節角参照軌道補間部３４、基体姿勢参照軌道補間部３５、重心位置参照軌道補間部３６、足底位置・姿勢参照軌道補間部３７）において、次時刻の上体関節角、基体姿勢角、重心位置、並びに足底位置・姿勢をそれぞれ算出する（ステップＳ５）。
【００７６】
さらに、質点分布調整部３８にて全身関節角を算出し（ステップＳ６）、最後に、全身関節駆動部３Ｃにおいて、各関節をサーボ・コントローラにより駆動する（ステップＳ７）。
【００７７】
以上の処理が図３に示した本制御システムの１周期に関る処理である。上記の処理が終了した後は再度ステップＳ１の実行に戻る処理を所定の制御周期Δｔ（例えば１０ミリ秒）毎に実行する。
【００７８】
次に、実時間全身運動生成部３Ｂ内における機能構成や動作について詳解する。
【００７９】
実時間全身運動生成部３Ｂ内の足底位置・姿勢軌道生成部３１は、運動要求入力部３Ａより入力された、歩幅・運動周期（歩行周期、走行周期、跳躍周期など）・旋回角・足上げ高さ・滞空時間などの下肢運動に関るパラメータに応じて現在の足底位置・姿勢状態から数歩先までの左右両足底の位置及び姿勢軌道を計画する。
【００８０】
図５には足底の位置・姿勢軌道の一例を示している。同図に示すように、足底の位置・姿勢軌道は、位置に関しては座標位置（ｘ，ｙ，ｚ）で、姿勢に関してはオイラー（Ｅｕｌｅｒ）角（α，β，γ）などを用いて表現し、上述した下肢運動パラメータに適合するよう、左右足底について各パラメータ毎に多項式補間などを用いて時系列を生成することで構成される。
【００８１】
例えば、左足底位置姿勢軌道に関しては、左足底位置姿勢を（ｘL，ｙL，ｚL，αL，βL，γL）、現在の左足底位置姿勢を（ｘL1，ｙL1，ｚL1，αL1，βL1，γL1）とすると、次の着床位置姿勢（ｘL2，ｙL2，ｚL2，αL2，βL2，γL2）は歩幅・運動周期・旋回角などの下肢運動パラメータにより決定され、その間の左足底位置姿勢軌道は、下式に示すように各位置姿勢毎の補間式によって構成される。
【００８２】
【数３】
0003599244.tif 000004
【００８３】
補間式には、例えば５次の多項式などを用いることができる。２歩目以降の足底位置姿勢は、１歩目と同一の下肢運動要求が継続するものとして、同様に補間式によって構成する。また、右足底位置姿勢軌道についても同様に構成する。
【００８４】
重心位置軌道生成部３２は、接地期には上式（１）で表される動力学的拘束条件が、無接地期には上式（２）で表される動力学的拘束条件が、それぞれ成立するような重心の水平軌道を算出する。以下では、重心位置軌道生成部３２の一構成法として、接地期に課される拘束条件式、及び無接地期に課される拘束条件式の具体的構成法と、両条件を満足する重心位置軌道の算出法について説明する。
【００８５】
接地期においては、慣性座標系内に任意の点ｐ＝（ｐx，ｐy，ｐz）Tを考え、この点ｐ回りに発生するモーメントＭを考える。また、重力加速度ベクトルを−ｇ＝（０，０，−ｇ）Tとする。点ｐ回りのモーメントの平衡条件より、以下の式が得られる。
【００８６】
【数４】
0003599244.tif 000005
【００８７】
また、重心の並進運動方程式より、下式が成立する。
【００８８】
【数５】
0003599244.tif 000006
【００８９】
上記の式（５）及び（６）の両式より、座標位置ｘ（ｘ，ｙ，ｚ）に印加される力ｆを消去すると、下式の通りとなる。
【００９０】
【数６】
0003599244.tif 000007
【００９１】
ここで、点ｐをＺＭＰにとると、水平軸モーメントがゼロとなるので、下式が成立する。
【００９２】
【数７】
0003599244.tif 000008
【００９３】
一般に、ＺＭＰ回りに発生するモーメントは重心運動によるものが支配的である。そこで、上式（８）の右辺第２項を省略し、また、その第１、第２要素について展開すると、下式（９）に示すような重心運動に関するＺＭＰ方程式を得ることができる。
【００９４】
【数８】
0003599244.tif 000009
【００９５】
上式（９）は単純な式をしているものの、ｘ及びｙに関する変係数の微分方程式であり、解析解を得ることは容易でない。そこで、本実施形態では、一旦、比較的短期間の時間区間Ｔを考え，この短期区間内において以下の近似が成立すると仮定する。
【００９６】
（仮定１）ＺＭＰ軌道は加速度を有さない
（仮定２）重心高さは一定
（仮定３）外モーメントは一定
【００９７】
すると、上記のＺＭＰ方程式（９）は、以下に示すような簡潔な常微分方程式となる。
【００９８】
【数９】
0003599244.tif 000010
【００９９】
但し、以下の式を満たすものとし、上式（１０）はｘ成分のみを記したものである。
【０１００】
【数１０】
0003599244.tif 000011
【０１０１】
常微分方程式（１０）の一般解は以下の通りとなる。
【０１０２】
【数１１】
0003599244.tif 000012
【０１０３】
但し、λは固有値で、下式で表される。
【０１０４】
【数１２】
0003599244.tif 000013
【０１０５】
上式（１３）の境界値を考えると、以下の通りとなる。
【０１０６】
【数１３】
0003599244.tif 000014
【０１０７】
本実施形態では、脚式移動ロボットの一般の運動を、上述した仮定条件（仮定１）、（仮定２）、及び（仮定３）を満足する短期区間が複数連なって構成されているとして、モデル化する。図６には、短期区間がＮ区間だけ連なっている様子を示している。
【０１０８】
ｉ番目の区間ｉの時間幅をＴi、未定係数をＣ1i、Ｃ2i、固有値をλi、区間ｉの終端におけるＺＭＰ位置をｐxi、重心位置をｘiとすると、上述した境界条件としての上式（１５）は一般の区間ｉにおいて以下のように表わされる。
【０１０９】
【数１４】
0003599244.tif 000015
【０１１０】
一方、無接地期においても同様の短期区間を考えると、この区間において水平方向外力が作用しないことから、重心の水平方向位置（ｘ，ｙ）の運動は以下に示すような単純な運動方程式で支配される。但し、ここでは簡潔のため、ｘ方向についてのみ記した。
【０１１１】
【数１５】
0003599244.tif 000016
【０１１２】
上式（１７）を積分すると、以下の通りとなる。
【０１１３】
【数１６】
0003599244.tif 000017
【０１１４】
ここで、境界条件を考えると、以下の通りとなる。
【０１１５】
【数１７】
0003599244.tif 000018
【０１１６】
接地期において短期区間がＮ区間連なって構成されているとしてモデル化したが（前述）、このような連続区間は、図７に示すように接地期に属する短期区間と、無接地期に属する短期区間が混在して構成されていてもよい。
【０１１７】
短期区間ｉが無接地期に属する場合、その境界条件は接地期とは異なるが、上式（１９）に基づいて以下のよう導き出される。
【０１１８】
【数１８】
0003599244.tif 000019
【０１１９】
接地期と無接地期が混在して運動が構成されている場合、これらの境界条件が位置及び速度に関して連続に接続されるよう、未定係数を決定しなければならない。区間ｉ−１と区間ｉの境界における位置及び速度の連続条件は、隣接する短期区間の種類によって以下の通り場合分けされる。
【０１２０】
（１）区間ｉ−１が接地期で、区間ｉが接地期の場合
上記の式（１６）より、区間ｉ−１から見たｘi及びその速度成分ｄｘi／ｄｔが区間ｉから見た値に等しくなるための条件は、以下の通りである。
【０１２１】
【数１９】
0003599244.tif 000020
【０１２２】
（２）区間ｉ−１が無接地期で、区間ｉが接地期の場合
上記の式（１６）及び式（２０）より、区間ｉ−１から見たｘi及びその速度成分ｄｘi／ｄｔが区間ｉから見た値に等しくなるための条件は、以下の通りである。
【０１２３】
【数２０】
0003599244.tif 000021
【０１２４】
（３）区間ｉ−１が接地期で、区間ｉが無接地期の場合
上記の式（１６）及び式（２０）より、区間ｉ−１から見たｘi及びその速度成分ｄｘi／ｄｔが区間ｉから見た値に等しくなるための条件は、以下の通りである。
【０１２５】
【数２１】
0003599244.tif 000022
【０１２６】
（４）区間ｉ−１が無接地期で、区間ｉが無接地期の場合
上記の式（２０）より、区間ｉ−１から見たｘi及びその速度成分ｄｘi／ｄｔが区間ｉから見た値に等しくなるための条件は、以下の通りである。
【０１２７】
【数２２】
0003599244.tif 000023
【０１２８】
以上の短期区間境界における位置及び速度の連続条件は、ｉ＝２..Ｎの各短期区間境界において成立する。また、運動区間全体の始端においても、与えられた重心位置及びその速度を実現するように境界条件が課されるが、無接地区間と接地区間の混在状況を許容する本方式の場合、区間０が無接地期又は接地期のいずれであるかに応じて、以下のように異なる境界条件が課される。
【０１２９】
（１）区間０が接地期の場合
上記の式（１６）より、区間Ｎの始端について
【０１３０】
【数２３】
0003599244.tif 000024
【０１３１】
（２）区間０が無接地期の場合
上記の式（２０）より、区間０の始端について、
【０１３２】
【数２４】
0003599244.tif 000025
【０１３３】
同様に、区間Ｎの終点においても、運動期間全体の終端条件として、与えられた重心位置及びその速度を実現するように境界条件が課されるが、区間Ｎが無接地期又は接地期のいずれであるかに応じて、以下のように異なる境界条件が課される。
【０１３４】
（１）区間Ｎが接地期の場合
上記の式（１６）より、区間Ｎの終端について
【０１３５】
【数２５】
0003599244.tif 000026
【０１３６】
（２）区間Ｎが接地期の場合
上記の式（２０）より、区間Ｎの終端について
【０１３７】
【数２６】
0003599244.tif 000027
【０１３８】
始端条件の重心位置及びその速度には、現在の運動状態に滑らかに接続する重心位置軌道を得るため、現在の重心位置及び速度を代入する。また、終端条件の重心位置には、運動終端における両足底位置を基に、両足底の中心位置などを設定する。終端条件の重心の速度には、重心位置にて重心が静止するとの条件を仮定し、ゼロを代入するなどの設定を行なう。これにより、運動方程式の解に対し、ある時間後に重心が両足底中心の直上に到達するような拘束が課せられる。
【０１３９】
実時間運動パターン生成においては、比較的短い周期で（例えば制御周期毎に）運動パターンを再計画する処理を繰り返すため、各周期毎に、次の再計画までの微小時間内での安定運動状態を確保することができれば十分である。したがって、現時刻から十分離れた終端時刻における終端到達条件が上述のように近似的に与えられていても、実用上十分な精度を持つ解を得ることができる。
【０１４０】
短期区間の境界は、例えば両脚支持期と単脚支持期の境界、接地期と無接地期の境界などに設定すればよい。
【０１４１】
Ｎ個の短期区間が連続してなる区間の境界では、上記の式（２１）〜（３６）により２Ｎ＋２個の拘束条件が与えられる。変数に関しては、未定係数Ｃ1i，Ｃ2i（ｉ＝１..Ｎ）以外の変数の設定の仕方としては多くの方法が考えられるが、本実施形態ではｐxi（ｉ＝１..Ｎ）を変数に加える。接地区間が少なくとも１つ存在すれば、ＺＭＰ位置ｐxiは２つ以上存在するので、変数の数は２Ｎ＋２個以上となり、解を得ることができる。
【０１４２】
一般には、連続する区間には接地区間の数が２以上存在し、変数の数が過剰となるため、最適化問題として取り扱う。例えば、上記の式（２１）〜（３６）で与えられる等式拘束条件下にて、接地区間境界におけるＺＭＰ位置ｐxiと安定領域中心（接地多角形重心）の差分（下式）を最小化する等式制約条件付２次計画問題として扱うことで、これらの未知変数を一意に定めることができる。
【０１４３】
【数２７】
0003599244.tif 000028
【０１４４】
あるいは、接地区間境界の特定２ノードｐxj，ｐxk（ｊ，ｋ∈［ｏ，Ｎ］）以外のＺＭＰ位置は接地多角形重心位置に固定とし、ｐxj，ｐxkのみを変数に設定して、上記の式（２１）〜（３６）を２Ｎ＋２元連立１次方程式として解くように構成しても良い。未知変数が定まれば、接地区間に関しては上式（１３）を用いて、無接地区間に関しては上式（１８）を用いて、計画した運動期間内の任意の時刻における重心水平位置を算出することができる。
【０１４５】
以上で重心水平位置軌道は得られたことになる。さらに、重心垂直位置軌道に関しても力学的拘束条件が課せられ、これを満足するように軌道を生成しなければならない。重心垂直方向運動に関しては、接地期においては、ｆz＞０を満足している限り、特に力学的拘束を受けない。一方、無接地期には、ｆz＝０なる力学的拘束が課せられる。すなわち、上式（６）でｆz＝０とすると、そのＺ成分は、下式（３８）のように表される。
【０１４６】
【数２８】
0003599244.tif 000029
【０１４７】
上式を積分すると、以下のようになる。
【０１４８】
【数２９】
0003599244.tif 000030
【０１４９】
滞空時間がＴ、離床時の重心垂直位置ｚ1、着床時の重心垂直位置ｚ2となるように未定係数を定めると、ロボット重心垂直位置軌道は、下式に示す自由落下曲線として得られる。
【０１５０】
【数３０】
0003599244.tif 000031
【０１５１】
離床時の重心垂直位置ｚ1及び着床時の重心垂直位置ｚ2は、運動要求入力部３Ａより入力される、重心垂直位置基本軌道における無接地期の始端及び終端における値を抽出して用いる。
【０１５２】
ここで、ユーザ・プログラムより入力される重心垂直位置軌道のことを「基本軌道」と呼称しているのは、上述の動力学的拘束条件を満足すべく修正を施されるため、最終的な重心垂直位置参照軌道は異なるものとなるからである。いわば、基本軌道は、あらかじめユーザが設定する概略の軌道とみなすことができる。以下、本明細書中では、基本軌道という用語は、「最大限反映されるが動力学的拘束条件を満たすべく修正を施され得る軌道」という意味で用いる。
【０１５３】
無接地期における重心垂直位置軌道は，各無接地期間毎に、上式（４０）に従うように設定すればよい。一方、接地期における重心垂直位置軌道に関して動力学的拘束条件は特に課されないものの、接地期が無接地期に隣接する場合は、無接地期との境界において、式（４０）と滑らかに接続されるように接地期の重心垂直位置基本軌道の修正を行なわなければならない。すなわち、離床時の重心垂直運動状態は、式（４０）及び式（４１）にてｔ＝０とした以下に示す値となる。
【０１５４】
【数３１】
0003599244.tif 000032
【０１５５】
このため、これに滑らかに接続するように、無接地期の直前に隣接する接地期の重心垂直位置基本軌道を修正しなければならない。図８中の区間ｉ−１の接地期はこのようなケースに該当する。
【０１５６】
図示の区間ｉ−１は特に無接地区間に挟まれているケースであり、区間ｉとの境界（終端）のみならず、区間ｉ−２の境界（始端）に関しても、上式（４２）で与えられる境界条件を満足する曲線軌道を設定しなければならない。
【０１５７】
接地区間の重心垂直位置軌道には、例えば多項式補間などを用いて、このような始端及び終端条件を満足するような曲線軌道を設定することができる。あるいは、必要に応じて、下肢運動パラメータ内に着床・離床時の重心沈み込み深さΔｚを含め、最小値が基本軌道よりΔｚ以上沈み込まないように曲線を設定したり、変曲点を含まない特殊な曲線で補間するようにしたり、バネ・ダンパ系の応答曲線で構成したりするようにしてもよい。
【０１５８】
同様に、着床時の重心垂直運動状態は、上式（４０）及び（４１）にてｔ＝Ｔとした以下に示す値となる。
【０１５９】
【数３２】
0003599244.tif 000033
【０１６０】
このため、これに滑らかに接続するように、無接地期の直後に隣接する接地期の重心垂直位置基本軌道を修正しなければならない。図８中の区間ｉ＋１はこのようなケースに該当する。
【０１６１】
図示の区間ｉ＋１は区間ｉ−１の場合と異なり、他端が接地区間ｉ＋２に接続されるケースである。このような場合、区間ｉとの境界（始端）は上式（４３）で与えられる境界条件を満足するように、区間ｉ＋２との境界（終端）は重心垂直位置基本軌道の位置及び速度に連続となるように区間ｉの重心垂直位置軌道を設定すればよい。
【０１６２】
無接地期直前の接地期と同様、例えば多項式補間などを用いて、このような始端及び終端条件を満足するような曲線軌道を設定することができる。あるいは、必要に応じて、下肢運動パラメータ内に着床・離床時の重心沈み込み深さΔｚを含め、最小値が基本軌道よりΔｚ以上沈み込まないように曲線を設定したり、不用意な振動を排除すべく、変曲点を含まない特殊な曲線で補間するようにしたり、バネ・ダンパ系の応答曲線で構成したりするようにしてもよい。
【０１６３】
無接地期に隣接しない接地期の重心垂直位置軌道に関しては、重心垂直位置基本軌道をそのまま参照軌道として設定すればよい。また、無接地期に隣接する接地期の重心垂直位置軌道についても、必ずしも上述したような接地期全体の軌道を基本軌道から修正するのではなく、離床又は着床の直前又は直後の一部の時間など、部分的に軌道を修正するように構成してもよい。
【０１６４】
以上のようにして、重心位置軌道生成部３２は、接地期における動力学的拘束条件としての式（１）と、無接地期における動力学的拘束条件としての式（２）をともに満たすように重心位置軌道を生成することができる。図９には、重心位置軌道生成部３２において実行される処理の流れをフローチャートの形式で示している。
【０１６５】
まず、ＺＭＰ基本ノード生成処理において、足底位置・姿勢軌道生成部３１より得られた足底位置及び姿勢参照軌道に基づいて、ＺＭＰ軌道が安定領域中心（接地多角形重心）付近を通過するような、各短期区間境界のＺＭＰ基本位置の配列を作成する（ステップＳ１１）。
【０１６６】
この基本ＺＭＰ位置は、後続の重心水平位置軌道算出（ステップＳ１４）において修正を施されるが、最大限実現すべく反映される。ここで、接地多角形外を通過するような基本ＺＭＰ軌道、又は安定領域中心から大きく逸脱するような基本ＺＭＰ軌道が設定されると、後続の重心水平位置軌道算出処理（ステップＳ１４）において修正を施された際に、ＺＭＰが接地多角形外となり、力学的拘束条件が満足されない事態となる可能性は否めない。多くの場合、このようなケースは生じないが、より頑強なシステムを志向するならば、接地多角形外に逸脱したＺＭＰ軌道が得られた場合に、基本ＺＭＰ位置を自律修正したり、下肢運動パラメータの自律変更などを実行したりする処理を追加してもよい。
【０１６７】
次いで、短期区間境界における重心位置軌道の連続条件を設定する（ステップＳ１２）。すなわち、上式（２１）〜（２８）に基づいて、等式制約条件をマトリクスに設定する。上述したように、連続条件の拘束式は、隣接する区間が接地区間又は無接地区間のいずれであるかに応じて相違する。
【０１６８】
次いで、考慮している運動区間の始端及び終端における重心水平位置軌道境界条件をマトリクスに設定する（ステップＳ１３）。始端境界条件は上式（２９）〜（３２）、終端境界条件は上式（３３）〜（３６）によってそれぞれ与えられるが、短期区間境界の連続条件と同様、区間０及び区間Ｎがそれぞれ接地区間なのか無接地区間なのかに応じて拘束条件式は相違する。
【０１６９】
次いで、ステップＳ１２及びＳ１３で設定した等式拘束条件付２次計画問題［数３７］を解き、未定係数Ｃ1i，Ｃ2i（ｉ＝１..Ｎ）及びＺＭＰ位置ｐxi（ｉ＝１..Ｎ）を決定する。
【０１７０】
このような処理により、接地区間に関しては上式（１３）を、無接地区間に関しては上式（１８）を用いることで、任意の時刻における重心水平位置及び速度を算出可能な状態となる。すなわち、任意時刻の重心水平位置軌道が得られたことになる。
【０１７１】
次いで、無接地期の重心垂直位置軌道を算出する（ステップＳ１５）。すなわち、ユーザ・プログラムにより与えられる重心垂直位置基本軌道を基に、上式（４０）で表される２次方程式の係数決定処理を行なう。このような処理により、無接地期内の任意の時刻（境界を含む）における重心垂直位置及び速度が算出可能な状態となる。
【０１７２】
さらに、短期区間境界における重心垂直位置及び速度を算出する（ステップＳ１６）。無接地期境界における重心垂直位置及び速度は、上式（４２）及び上式（４３）を用いて算出する。それ以外の境界における重心垂直位置・速度は、ユーザ・プログラムにより与えられる重心垂直位置基本軌道の値をそのまま用いることができる。
【０１７３】
最後に、接地区間の重心垂直位置軌道を修正し、動力学的拘束条件を考慮した重心垂直位置軌道、ひいては重心位置軌道を完成する（ステップＳ１７）。
【０１７４】
無接地期間と隣接している接地区間に関しては、ステップＳ１６で算出した境界条件を満足するよう、多項式などを用いて適切な曲線で重心垂直位置軌道を構成する。また、無接地期間と隣接しない接地区間に関しては、ユーザ・プログラムにより与えられる重心垂直位置基本軌道をそのまま重心垂直位置軌道として用いるように構成する。
【０１７５】
上述したような処理の結果、無接地期の重心垂直位置軌道はステップＳ１５により、接地期の重心垂直位置軌道はステップＳ１７によりそれぞれ得られ、全時刻における重心垂直位置軌道が得られたことになる。
【０１７６】
以上の説明から判るように、力学的拘束条件としての式（１）又は（２）を満足する重心位置軌道は得られたが、さらに無接地期における残る動力学的拘束条件としての式（３）は別途運動計画に反映しなければならない。
【０１７７】
一般に、重心回りの角運動量Ｌと外モーメントｎの間には、以下の関係が成り立つ。
【０１７８】
【数３３】
0003599244.tif 000034
【０１７９】
無接地期においては、上式（３）に示した力学的拘束条件が課されるが、これを上記の式（４４）に代入し積分することで、以下に示す角運動量保存則と等価であることが分かる。
【０１８０】
【数３４】
0003599244.tif 000035
【０１８１】
無接地期の運動は、足底位置・姿勢軌道生成部３１の出力する足底位置・姿勢軌道、重心位置軌道生成部３２の出力する重心位置軌道に関する拘束に加え、上記の式（４５）に示される角運動量に関する拘束を満足するように計画しなければならない。以下では、角運動量に関する拘束を満足する運動の計画法について詳解する。
【０１８２】
ロボットの状態は、一般に、基体（骨盤）の位置及び姿勢と、全関節角で表すことができる。すなわち、基体の位置をｐb＝（ｐbx，ｐby，ｐbz）T、基体の姿勢（オイラー角）をαb＝（αbx，αby，αbz）T、左足関節角をθL＝（θL1，θL2，θL3，θL4，θL5，θL6）T、右足関節角をθR＝（θR1，θR2，θR3，θR4，θR5，θR6）T、上体関節角をθU＝（θ1U，．．，θNU）とすると、ロボットの状態変数ｑは、これらを並べて下式に示すようなベクトルとして定義することができる。ここで、ロボットの状態変数ｑの次元をＮqとする。
【０１８３】
【数３５】
0003599244.tif 000036
【０１８４】
すべてのリンクにインデクスを付し、インデクス番号がｉとなるリンクをリンクｉと表記する。ロボット状態変数ｑの一般化速度と、リンクｉの角速度、並進速度、ロボット全体の重心速度、並びに重心回りの角運動量との関係は、それぞれ式（４７）、（４８）、（４９）、（５０）に示すように、ヤコビアンＪを用いて記述できる。
【０１８５】
【数３６】
0003599244.tif 000037
【０１８６】
ここで、上記のヤコビアンＪはいずれも３×Ｎqの行列であり、一般に現在のロボットの状態ｑの関数として解析的に求めることができる。現在の状態ｑを既知とすると、これらの式を用いて、上記の重心位置軌道、足底位置・姿勢軌道、角運動量に関する拘束条件は，ロボットの状態の一般化速度に関する拘束条件として、以下のように記述することができる。但し、左足底リンクのインデクスをｉLとし、右足底リンクのインデクスをｉRとする。
【０１８７】
（１）足先姿勢に関する拘束条件
上式（４７）において、足底リンクの角速度が、足底位置・姿勢軌道生成部３１が生成した足底軌道から算出した左右それぞれの足のオイラー角速度と等しくなければならないことから、
【０１８８】
【数３７】
0003599244.tif 000038
【０１８９】
（２）足先位置に関する拘束条件
上式（４８）において、足底リンクの並進速度が、足底位置・姿勢軌道生成部３１が生成した足底軌道から算出した左右それぞれの足の並進速度と等しくなければならないことから、
【０１９０】
【数３８】
0003599244.tif 000039
【０１９１】
（３）重心位置に関する拘束条件
上式（４９）において、重心速度が重心位置軌道生成部３２によって生成された重心位置軌道から算出した重心速度に等しくなければならないことから、
【０１９２】
【数３９】
0003599244.tif 000040
【０１９３】
（４）角運動量に関する拘束条件
上式（５０）において、角運動量が一定であるとの条件を課すと、
【０１９４】
【数４０】
0003599244.tif 000041
【０１９５】
いずれの拘束式も、３自由度の拘束条件を与えるので、上式（５１）から式（５６）にて、全部で１８自由度の拘束条件を課していることになる。ここでは、例として基体（骨盤）の位置及び姿勢を操作部位として用いることを考えると、ロボットの状態ｑの一般化速度のうち、脚部関節角速度（合計１２変数）と基体の並進速度（３変数）とオイラー角速度（３変数）が未知変数、上体関節角軌道は既知となる。したがって、未知変数の数は１８個となり、拘束式の数と一致し、上式（５１）から式（５６）を１８元連立一次方程式として解くことで未知変数を容易に求めることが可能となる。
【０１９６】
なお、式変形を行なうことで、連立方程式の次元はさらに低減させることが可能であるが、これは本発明の主たる目的ではないので、本明細書では説明を省略する。
【０１９７】
以上の説明により、現在の姿勢が既知、足先位置姿勢固定、重心位置固定という条件下で、角運動量が保存するための基体の並進速度、オイラー角速度を算出できたことになる。よって、制御サイクル（時刻Δｔ毎）における基体姿勢αb,k+1は、上記の連立方程式の解として得られたオイラー角速度を積分して、以下の式によって求めることができる。
【０１９８】
【数４１】
0003599244.tif 000042
【０１９９】
以上の説明で、上式（１）及び（２）で表される動力学的拘束条件に加え、無接地期の回転運動に関する力学的拘束条件としての式（３）を満足する基体姿勢軌道を得ることができたことになる。しかしながら、角運動量を適切な値に設定しないと、空中で大きく回転運動を伴うような運動が生成される可能性がある。
【０２００】
さまざまな角運動量の設定方法が考えられるが、空中で大きく機体全体が回転するような運動を想定しないのであれば、下式（５８）に示すように、無接地期に有する角運動量を０に設定するのが最も簡単である。
【０２０１】
【数４２】
0003599244.tif 000043
【０２０２】
あるいは、ヨー軸回りにのみ角運動量を有するように、離床時に算出したＺ軸周り慣性モーメントＩZZ、旋回角度Γ、及び滞空時間Ｔを用いて、以下に示す式のように設定するなどの方法も実用的である。
【０２０３】
【数４３】
0003599244.tif 000044
【０２０４】
さて、無接地期における基体姿勢軌道は、上述したような処理手順に従って得られる一方、接地期における基体姿勢軌道に関しては、無接地期における基体姿勢軌道に滑らかに接続されるように、接地期の基体姿勢基本軌道の修正を行なわなければならない。
【０２０５】
すなわち、離床時に相当する、接地期と無接地期の境界においては、基体姿勢角が基体姿勢角基本軌道の値と一致し、基体姿勢角速度がこの姿勢において、上式（５１）〜（５６）からなる連立方程式を解いて得られる基体姿勢の角速度の値となるように、無接地期の直前に隣接する接地期の基体姿勢軌道を修正しなければならない。
【０２０６】
同様に、着床時に相当する無接地期と接地期の境界においては、基体姿勢角が上式（５７）の数値積分で得られた無接地期終端における基体姿勢角の値と一致し、基体姿勢角速度がこの姿勢において上式（５１）〜（５６）からなる連立方程式を解いて得られる基体姿勢の角速度の値となるように、無接地期の直後に隣接する接地期の基体姿勢軌道を修正しなければならない。
【０２０７】
図１０には、短期区間と無接地期に属する短期区間が混在しているモデルにおける基体姿勢基本軌道と基体姿勢参照軌道との関係を例示している。同図中の区間ｉ−１はこのようなケースに該当する。但し、図面の簡素化のため、ロール角軌道のみが記されている。
【０２０８】
区間ｉ−１は始端及び終端がともに無接地区間に接しているケースであり、区間ｉとの境界（終端）のみならず、区間ｉ−２の境界（始端）に関しても、上述のような境界条件が課される。例えば、スプライン補間などの多項式補間を用いて、このような始端・終端条件を満足するような曲線軌道として、接地期間の基体姿勢軌道を設定することができる。
【０２０９】
一方，同図中の区間ｉ＋１は区間ｉ−１の場合とは異なり、始端が無接地期となる区間ｉに隣接するが他端は接地区間ｉ＋２に接続される接地期である。このような場合は、区間ｉとの境界（始端）は無接地期の基体姿勢角軌道による境界条件を満足するように、区間ｉ＋２との境界（終端）は基体姿勢基本軌道の位置・速度に連続となるように、区間ｉの基体姿勢角軌道を設定しなければならない。この場合も、例えば、多項式補間などを用いて、このような始端・終端条件を満足するような曲線軌道を設定することができる。
【０２１０】
なお、無接地期に隣接しない接地期の基体姿勢角軌道に関しては、基体姿勢基本軌道をそのまま参照軌道として設定すればよい。また、無接地期に隣接する接地期の基体姿勢角軌道についても、必ずしも上述したように接地期全体の軌道を基本軌道から修正するのではなく、離床・着床の直前・直後の一部の時間など、部分的に軌道を修正するように構成してもよい。
【０２１１】
以上の接地期の基体姿勢角軌道の修正法が重心位置軌道の修正法と異なる点は、時刻に関して逐次的に処理を行なわなければならない点である。基体姿勢角軌道に関しては、無接地区間における軌道を上式（５７）に基づく数値積分でしか得ることができず、無接地期終端の境界条件がこの数値積分によって決定されるためである。実際には、無接地期実行中にはこの数値積分のみを実行し、無接地期から接地期に移行する境界にて、隣接する接地期の軌道修正を実行すればよい。
このようにして、基体姿勢軌道生成部３３は、接地期における力学的拘束条件としての式（１）、無接地期における力学的拘束条件としての式（２）に加え、無接地期における力学的拘束条件としての式（３）を満足するような基体姿勢軌道を生成することができる。
【０２１２】
図１１には、基体姿勢軌道生成部３３において実行される処理手順をフローチャートの形式で示している。
【０２１３】
まず、現時刻が無接地期に属するかどうかの判定を行なう（ステップＳ２１）。
【０２１４】
無接地期の場合、ステップＳ２２に進み、足底位置・姿勢軌道生成部３１の出力する足底位置姿勢軌道を用いて、現時刻の足底位置姿勢に関するヤコビアン拘束式（５１）〜（５４）を求める。
【０２１５】
次いで、重心位置軌道生成部３２の出力する重心位置軌道を用いて、現時刻の重心位置に関するヤコビアン拘束式（５５）を求める（ステップＳ２３）。
【０２１６】
次いで、離床前に算出された角運動量を用いて、角運動量ヤコビアン拘束式（５６）を求める（ステップＳ２４）。
【０２１７】
次いで、これらの拘束式（５１）〜（５６）からなる連立１次方程式を解き、現時刻における基体姿勢角速度を算出する（ステップＳ２５）。
【０２１８】
最後に、該基体姿勢角速度を上式（５７）を用いて数値積分することで、無接地期の基体姿勢角軌道を逐次生成することができる（ステップＳ２６）。
【０２１９】
一方、ステップＳ２１において接地期と判定された場合は、現時刻が接地期の始端に該当する場合についてのみ処理を行なう（ステップＳ２０）。
【０２２０】
接地期の始端に相当する場合は、ステップＳ２７に進み、該接地期が無接地期に隣接した区間であるかどうかを判定する。
【０２２１】
無接地期に隣接している接地期である場合は、さらに、直後に隣接する短期区間が無接地期であるかどうかを判定する（ステップＳ２８）。
【０２２２】
直後に隣接する短期区間が無接地期である場合は、上式（５８）あるいは式（５９）などの規範に基づいて、該無接地期の目標角運動量を算出する（ステップＳ２９）。
【０２２３】
次いで、それぞれ該無接地期開始時点（離床時）における、足底位置姿勢ヤコビアン拘束式（５１）〜（５４）、重心位置ヤコビアン拘束式（５５）、角運動量ヤコビアン拘束式（５６）をそれぞれ算出する（ステップＳ３０，Ｓ３１，Ｓ３２）。
【０２２４】
これらのヤコビアン拘束式の算出には、先行するステップＳ２９で得られた角運動量の他、足底位置姿勢軌道生成部３１が生成した足底位置姿勢軌道、重心位置軌道生成部３２が生成した重心位置軌道の離床時点での値が用いられる。
【０２２５】
次いで、これらのヤコビアン拘束式（５１）〜（５６）からなる連立１次方程式を解き、離床時の基体姿勢角速度を算出しておく（ステップＳ３３）。この時点で、接地期終端の境界条件が決定されたことになる。
【０２２６】
さらに、直前に隣接する短期区間が無接地期であるかどうかを判別する（ステップＳ３４）。
【０２２７】
直前に隣接する短期区間が無接地期である場合は、ステップＳ２６の結果として得られた無接地期終端の基体姿勢角速度を、接地期始端の境界条件として抽出することで、接地期両端の境界条件の設定が完了する（ステップＳ３５）。
【０２２８】
一方、ステップＳ３４において直前に隣接する短期区間が接地期と判定された場合は、ユーザ・プログラムから与えられる基体姿勢基本軌道の値を接地期始端の境界条件として設定することで、接地期両端の境界条件の設定を完了する（ステップＳ３８）。
【０２２９】
ところで、ステップＳ２８において、接地期直後の短期区間も接地期であると判別された場合は、ステップＳ３７に進み、接地期終端の境界条件として、運動要求入力部３Ａから与えられる基体姿勢基本軌道の値を設定する。接地期始端の境界条件は、ステップＳ２８において接地期直後の短期区間が無接地期と判定された場合と同様の処理を実行することで、接地期両端の境界条件の設定が完了する。
【０２３０】
以上の処理によって、接地期が無接地期に隣接している場合の接地期両端の境界条件が設定されたが、ステップＳ３９では、同境界条件を満足するように、接地期の基体姿勢軌道を多項式補間などの曲線を用いて構成する処理を行なう。
【０２３１】
さらに、ステップＳ２７において接地期間が何ら無接地期と隣接しないと判断された場合は、接地期の基体姿勢軌道は、運動要求入力部３Ａから与えられる基体姿勢基本軌道をそのまま用いることができるので、ステップＳ３６において、基体姿勢基本軌道を接地期の基体姿勢軌道として設定して、本処理を完了する。
【０２３２】
再び図３に戻って、本実施形態に係るロボットの制御システムの機能構成に関する説明を続ける。これまでの説明により、ユーザ・プログラムから与えられる上体関節角参照軌道、基体姿勢参照軌道、重心位置参照軌道、足底位置姿勢参照軌道のすべてが得られることになる。あとは、制御周期毎に各軌道に対して設けられている補間部（上体関節角参照軌道補間部３４、基体姿勢参照軌道補間部３５、重心位置参照軌道補間部３６、足底位置・姿勢参照軌道補間部３７）においてこれらの軌道を補間することで、各時刻における上体関節角、基体姿勢角、重心位置、並びに足底位置姿勢を算出することができる。但し、無接地期における基体姿勢角軌道に関しては数値積分の形で軌道が与えられるため、基体姿勢参照軌道補間部３５における補間処理は特に必要ない。
【０２３３】
質点分布調整部３８では、以上で求められた次時刻の基体姿勢と重心位置を満足するように、全身の質点分布状態を調整する。すなわち、基体姿勢、足底位置姿勢、及び上体関節角を要求値に固定したまま、基体（骨盤）の位置（ｘB，ｙB，ｚB）を操作し、機体全体の重心が上記で得られた重心位置となるように、全身質点分布を調整する。このような幾何的拘束条件下において、基体の速度と重心の速度の間には、ヤコビアンＪBとすると、以下の関係が成立する。
【０２３４】
【数４４】
0003599244.tif 000045
【０２３５】
現在のロボットの重心位置ｘkとＺＭＰ方程式の求解により得られた次時刻の重心位置ｘk+1の間にｄｘ＝ｘk+1−ｘkの偏差が存在するとき、基体位置の操作量ｄｘBは近似的に以下の式（６１）で得ることができる。
【０２３６】
【数４５】
0003599244.tif 000046
【０２３７】
ここで、重心の算出には、ユーザ・プログラムより入力される上体関節角参照値も反映される。
【０２３８】
以上より、現在の基体位置をｘBkとすると、上記で得られた次時刻の基体姿勢と重心位置を実現するため質点分布調整後の基体位置ｘBk+1は以下（６２）の式で得ることができる。
【０２３９】
【数４６】
0003599244.tif 000047
【０２４０】
但し、上式（６２）の結果得られる重心位置と参照重心位置の偏差がなお大きな場合は、式（６２）を繰り返し用いて、偏差が十分小さくなるまで収束演算を実行するようにしてもよい。
【０２４１】
関節角算出部３９では、質点分布調整部３８で得られた次時刻の基体位置、及び足底位置・姿勢軌道生成部３１より得られる次時刻の足底位置姿勢を実現するよう、脚部の関節角を決定する。これは、両者の相対位置姿勢を求めた後、例えば、周知の逆キネマティクス計算によって実行することができる。ここで得られた脚関節角と、ユーザ・プログラムより与えられた上体の参照関節角とを併せて、次時刻における全身関節角参照値として出力する。
【０２４２】
図３に示したようなロボットの制御システムを構成することで、脚式移動ロボットは、歩行のみならず、跳躍や走行、並びにこれらの運動が順次現れる状況において、歩幅や周期などの外部要求を満足しながら、力学的に安定な運動パターンを即応的（又は実時間）に計算しながら動作することが可能となる。
【０２４３】
本明細書に記載した実施形態では、ロボットの状態量として重心位置と基体姿勢角を用いる例を示したが、これを他の状態量を用いるように構成しても良い。例えば、重心位置の代わりに重心に近接した部位（例えば腰部）の位置を用いたり、基体姿勢角の代わりに腰部関節角を用いたりするように構成しても、同様に本発明の効果を得ることができる。あるいは、任意の関節角を状態量として用いるように構成して、いわゆる全身協調型の制御システムを構成しても良い。
【０２４４】
また、上述した実施形態では全身運動生成処理を制御周期毎に実行するように構成されているが、接地状態が変化する時点（両脚支持・片脚支持・無接地の境界）のみで軌道生成までの処理（図４中のステップＳ１〜Ｓ４に相当）を実行し、軌道補間処理以降の処理（同図中のステップＳ５〜Ｓ７）のみ制御周期毎に実行するなど、 軌道生成処理はより長いサイクルで実行するように構成してもよい。但し、この場合の運動要求反映の即応性は低下する。
【０２４５】
また、上述した実施形態では、前回の全身運動生成処理の結果得られた現在時刻の運動状態を、次周期の全身運動生成処理の初期条件として設定することで、現在の運動状態に滑らかに接続する運動状態を決定したが、本発明の要旨はこれに限定されるものではない。例えば、骨盤部加速度センサＡ１及びジャイロＧ１、足底力センサ（Ｆ１〜Ｆ８）及び測距センサ（Ｄ１〜Ｄ８）及び加速度センサ（Ａ２,Ａ３）、及びジャイロ（Ｇ２，Ｇ３）などのセンサを用いて実際のロボットの運動状態を計測し、これを初期条件として設定することで、非特許文献５に開示されているロボットの歩行制御装置と同様に、実状態を反映した全身運動生成処理を実行するように構成することができる。
【０２４６】
［追補］
以上、特定の実施例を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施例の修正や代用を成し得ることは自明である。
【０２４７】
本発明の要旨は、必ずしも「ロボット」と称される製品には限定されない。すなわち、電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行なう機械装置あるいはその他のタイプの移動機械であるならば、例えば玩具等のような他の産業分野に属する製品であっても、同様に本発明を適用することができる。
【０２４８】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、冒頭に記載した特許請求の範囲の欄を参酌すべきである。
【図面の簡単な説明】
【０２４９】
【図１】図１は、本発明の実施に供される脚式移動ロボットの自由度構成を模式的に示した図である。
【図２】図２は、Ｆ＝ｆ、Ｎ＝ｎなる外力Ｆ，外モーメントＮの組み合わせが存在し得るように運動パターンが発生せんとする力ｆ及びモーメントｎに拘束条件（動力学的拘束条件）を課す様子を示した図である。
【図３】図３は、本発明の一実施形態に係る脚式移動ロボットの制御システムの機能構成を模式的に示した図である。
【図４】図４は、図３に示した制御システムにおいて実行される処理手順を示したフローチャートである。
【図５】図５は、足底の位置・姿勢軌道の一例を示した図である。
【図６】図６は、短期区間がＮ区間だけ連なっている様子を示した図である。
【図７】図７は、短期区間がＮ区間連なって構成されているとしたモデルに接地期に属する短期区間と無接地期に属する短期区間が混在している様子を示した図である。
【図８】図８は、短期区間と無接地期に属する短期区間が混在しているモデルにおける重心位置基本軌道と重心位置参照軌道の関係を示した図である。
【図９】図９は、重心位置軌道生成部３２において実行される処理の流れを示したフローチャートである。
【図１０】図１０は、短期区間と無接地期に属する短期区間が混在しているモデルにおける基体姿勢基本軌道と基体姿勢参照軌道との関係を示した図である。
【図１１】図１１は、基体姿勢軌道生成部３３において実行される処理手順を示したフローチャートである。
【符号の説明】
【０２５０】
３Ａ…運動要求入力部
３Ｂ…実時間全身運動生成部
３Ｃ…全身関節駆動部
３１…足底位置・姿勢軌道生成部
３２…重心位置軌道生成部
３３…基体姿勢軌道生成部
３４…上体関節角参照軌道補間部
３５…基体姿勢参照軌道補間部
３６…重心位置参照軌道補間部
３７…足底位置・姿勢参照軌道補間部
３８…質点分布調整部
３９…関節角算出部
【図１】
0003599244.tif 000048
【図２】
0003599244.tif 000049
【図３】
0003599244.tif 000050
【図４】
0003599244.tif 000051
【図５】
0003599244.tif 000052
【図６】
0003599244.tif 000053
【図７】
0003599244.tif 000054
【図８】
0003599244.tif 000055
【図９】
0003599244.tif 000056
【図１０】
0003599244.tif 000057
【図１１】
0003599244.tif 000058