JP2004306231A - ロボットの運動制御装置 - Google Patents

Abstract

【課題】動バランス維持、アーム作業などの複数のタスクを同時に実行する。
【解決手段】脚式ロボットに課されるタスクや運動状態に応じて課される拘束条件を、現在状態からの変化量ｄｘに関する等式及び不等式で与えるとともに、冗長自由度の駆動ストラテジをエネルギ関数で規定する。拘束条件の変化に関しては、各拘束条件毎に特化した制御系を構成する必要がなく、行列Ａ，Ｃ及びベクトルｂ，ｄの変更のみで対応することができるので、多様且つ動的な拘束条件を扱い易い。また、冗長自由度の利用方法についても、行列Ｗ及びベクトルｕの変更のみで対応できる。
【選択図】 図３

Description

【０００１】
【発明の属する技術分野】
本発明は、少なくとも複数本の可動脚を備えた脚式歩行ロボットの運動制御装置に係り、特に、移動やバランス維持、アーム作業といった複数のタスクを同時に実行することができる脚式歩行ロボットの運動制御装置に関する。
【０００２】
さらに詳しくは、本発明は、各タスクによって課される多様な運動拘束条件を同時に満足するように各関節の駆動量の配分を実時間で決定することができる脚式歩行ロボットの運動制御装置に係り、特に、時々刻々と変化する幾何学的、力学的な多様な運動拘束条件を同時に満足するように全身の自由度の駆動量を適切に配分して動作することができる、脚式歩行ロボットの運動制御装置に関する。
【０００３】
【従来の技術】
電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行う機械装置のことを「ロボット」という。ロボットの語源は、スラブ語の”ＲＯＢＯＴＡ（奴隷機械）”に由来すると言われている。
【０００４】
最近では、ヒトやサルなどの２足直立歩行を行う動物の身体メカニズムや動作を模した脚式移動ロボットに関する研究開発が進展し、実用化への期待も高まってきている。ヒトの生体メカニズムや動作を再現した脚式移動ロボットのことを、特に、「人間形」、若しくは「人間型」のロボット（ｈｕｍａｎｏｉｄ ＲＯＢＯＴ）と呼ぶ。
【０００５】
２足直立による脚式移動は、クローラ式や、４足又は６足式などに比し不安定で姿勢制御や歩行制御が難しくなる一方、不整地や障害物など作業経路上に凹凸のある歩行面や、階段や梯子の昇降など不連続な歩行面に対応することができるなど、柔軟な移動作業を実現することができるという利点がある。
【０００６】
また、脚式移動ロボットは、一般に、マニピュレータや搬送ロボットなどの産業用ロボット（ｉｎｄｕｓｔｒｉａｌ ＲＯＢＯＴ）に比し、冗長な自由度を含む多リンク系により構成されているという特徴がある。このような特徴を生かし、移動・バランス維持・アーム作業といった複数のタスクを同時実行することができる。
【０００７】
一方、上記複数のタスクによって課される多様な運動拘束条件を同時に満足するように、各関節の駆動量の配分を決定する方法は自明ではない。特に、このような運動拘束条件は、脚式移動ロボットの動作環境／実行タスクに応じて時々刻々と変化するため、実行時に即応的に運動拘束条件の変化に対応可能なアルゴリズムが望まれる。
【０００８】
例えば、２脚２腕型のロボットには、動作中において以下のような幾何学的な運動拘束条件が課される状況が想定される。すなわち、
【０００９】
▲１▼うつ伏せ状態から手を着いて起き上がる際、足・手の高さが床面に拘束される。
▲２▼壁に手を着いて立つ際、足は床に、手は壁に拘束される。
▲３▼物体を揺らさずに運搬する際、手先は等速直線運動軌道に拘束される。
▲４▼ロボット同士が手をつないで動作するとき、相互の手先軌道が拘束される。
【００１０】
また、力学的なバランスを維持するために、以下のような力学的な運動拘束条件も同時に課される。
【００１１】
▲１▼ロボットの並進運動量（重心軌道）に対する拘束。
▲２▼ロボットの角運動量に対する拘束。
【００１２】
さらに、各関節自由度を構成するアクチュエータの特性を考慮すると、以下のような不等式拘束が課される状況も想定される。
【００１３】
▲１▼関節部アクチュエータの可動範囲に対する拘束。
▲２▼関節部アクチュエータの駆動速度に対する拘束。
【００１４】
したがって、ヒューマノイドに代表される脚式移動ロボットは、このように時々刻々、多様に変化する運動拘束条件を同時に満足するように、全身の自由度の駆動量を適切に配分して動作しなければならない。
【００１５】
このような脚式ロボットの全身関節の駆動量配分方法に関連のある研究として、脚式移動ロボットの全身関節角計画値が与えられた状況下で、計画値を最大限反映しつつ、片足立脚バランスを維持するための全身関節の駆動量配分決定方法が提案されている（例えば、非特許文献１を参照のこと）。
【００１６】
しかしながら、対象問題が片足立脚状態に限定されている点、バランス維持のみのために全身関節が用いられる点、任意の幾何学的拘束を課す方法に関する言及が無い点などから、多様な運動拘束条件を同時に満足するという上記要求を満足するには至っていない。
【００１７】
また、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）を歩行の安定度判別の規範として用いた脚式移動ロボットの姿勢安定制御や歩行時の転倒防止に関する提案が多くなされている。ＺＭＰによる安定度判別規範は、歩行系から路面には重力と慣性力、並びにこれらのモーメントが路面から歩行系への反作用としての床反力並びに床反モーメントとバランスするという「ダランベールの原理」に基づく。力学的推論の帰結として、足底接地点と路面の形成する支持多角形（すなわちＺＭＰ安定領域）の辺上あるいはその内側にピッチ軸及びロール軸モーメントがゼロとなる点、すなわちＺＭＰが存在する（例えば、非特許文献２を参照のこと）。ＺＭＰ規範に基づく２足歩行パターン生成によれば、足底着地点をあらかじめ設定することができ、路面形状に応じた足先の運動学的拘束条件を考慮し易いなどの利点がある。また、ＺＭＰを安定度判別規範とすることは、力ではなく軌道を運動制御上の目標値として扱うことを意味するので、技術的に実現可能性が高まる。
【００１８】
このＺＭＰ安定判別規範に基づき、複数の部位を協調させてＺＭＰ回りモーメント補償して２足歩行パターンを生成する例が報告されている（例えば、非特許文献３を参照のこと）。
【００１９】
しかしながら、この場合も適用対象が歩行に限定されている点、実行時に任意の幾何拘束を付加／除去するフレームワークについては言及されていない点などから、多様な運動拘束条件を同時に満足するという上記要求を完全には満足していないと推察される。
【００２０】
本発明者らは、脚式移動ロボットに関する従来の機体制御アルゴリズムに時々刻々と多様に変化する運動拘束条件を同時に満足するように、全身の自由度の駆動量を適切に配分して動作することができない理由として以下の事柄を指摘する。
【００２１】
第一に、従来の機体制御アルゴリズムは特定の問題に対して少数の限定的な運動拘束しか与えられないという点が挙げられる。
【００２２】
運動に対する拘束は、歩行や立脚状態に限らず、あらゆる運動状態で発生し得る。また、エンドポイントの位置のみならず、機体のあらゆる部位の位置／姿勢に関する幾何拘束、系全体の運動量に関する拘束、アクチュエータの可動範囲／駆動速度に関する不等式拘束など、多様な拘束が同時に発生し得る。多自由度冗長系の脚式ロボットの機能性を最大限に発揮するには、これらの拘束が特定の運動状態に限定されることなく、自在に課せられるアルゴリズムが必要であると思料される。
【００２３】
第二に、動的な運動拘束条件の変化に対応できるアルゴリズムが少ないことが挙げられる。
【００２４】
上述した運動拘束条件は、要求タスクやロボットの運動状態に応じて時々刻々と変化し得る。例えば、脚式移動ロボットが頭上の障害物を避ける際、障害物に接近するとともに、頭部位置軌道に幾何拘束が課せられるが。障害物を回避した後にこの幾何拘束は解除される。あるいは、特定の関節の負荷増大が検出されたときには、この関節を保護するために幾何拘束を課し、他の部位を用いてバランスを維持するように歩容を変化させなければならないという状況も考えられる。このように時間的に変化する運動拘束条件を即応的に運動に反映できなければ、脚式ロボットの自由度リソースは効率的に利用されず、柔軟に要求タスクに対応可能な脚式ロボットを実現することができない。
【００２５】
第三に、冗長自由度の駆動方法に関して、固定的で且つ一義的なストラテジしか提示されていない点が挙げられる。
【００２６】
脚式ロボットの冗長自由度の駆動方法は、機体コンディション・タスクの種類などによって動的に変化し得る。外観が重要視され、あらかじめ与えた概略運動に極力近い運動が達成されることに冗長自由度を費やしたい状況もあれば、アクチュエータの負荷を低減するために関節駆動量を極力低減するために利用したいという状況も想定される。脚式ロボットが状況に応じて冗長自由度を効果的に駆動するためには、冗長自由度の駆動ストラテジを複数有し、それらを動的に切り替えられることが望ましいと思料される。
【００２７】
【非特許文献１】
田宮外著「人間型ロボットの片足立脚動作における全身を用いた実時間動バランス補償」（日本ロボット学会誌，Ｖｏｌ．１７，Ｎｏ．２，ｐｐ．２６８−２７４，１９９６）
【非特許文献２】
ヴコブラトビッチ（Ｍｉｏｍｉｒ Ｖｕｋｏｂｒａｔｏｖｉｃ）著「脚式移動ロボット（ＬＥＧＧＥＤ ＬＯＣＯＭＯＴＩＯＮ ＲＯＢＯＴＳ）」（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社））
【非特許文献３】
山口外著「二足歩行型ヒューマノイドロボットの開発−全身協調型二足動歩行制御−」（第３回ロボティクスシンポジア予稿集，ｐｐ．１８９−１９６，１９９８）
【００２８】
【発明が解決しようとする課題】
本発明の目的は、移動やバランス維持、アーム作業といった複数のタスクを同時に実行することができる、優れた脚式歩行ロボットの運動制御装置を提供することにある。
【００２９】
本発明のさらなる目的は、各タスクによって課される多様な運動拘束条件を同時に満足するように各関節の駆動量の配分を実時間で決定することができる、優れた脚式歩行ロボットの運動制御装置を提供することにある。
【００３０】
本発明のさらなる目的は、時々刻々と変化する幾何学的、運動力学的な多様な運動拘束条件を同時に満足するように全身の自由度の駆動量を適切に配分して動作することができる、優れた脚式歩行ロボットの運動制御装置を提供することにある。
【００３１】
【課題を解決するための手段及び作用】
本発明は、上記課題を参酌してなされたものであり、その第１の側面は、基体と前記基体に接続される複数の可動部を備えたロボットの運動制御装置であって、
前記ロボットに与えられるタスクや運動状態に応じて課される運動拘束条件を拘束の種類毎に設定する基本拘束条件設定器と、
前記ロボットのタスクや運動実行時に発生する運動拘束要求に応じて適当な基本拘束条件設定器を選択的に用いて、前記ロボットの状態変化量に必要な前記ロボット全体の運動拘束条件を課す拘束条件設定部と、
前記拘束条件設定部により設定された運動拘束条件をすべて満足するような各可動部の駆動量を決定する駆動量決定部と、
を具備することを特徴とするロボットの運動制御装置である。
【００３２】
ここで、ロボットは、例えば２腕２足の脚式歩行型ロボットであり、前記複数の可動部は、少なくとも上肢、下肢及び体幹部を含むものとする。また、前記ロボットの姿勢角は仮想リンクの仮想関節角を用いて表現することができる。
【００３３】
前記の拘束の種類毎に設けられた基本拘束条件設定器は、例えば、ロボットのタスクや運動状態に応じて課される運動拘束条件を状態変数変化量の線形等式で表現する。すなわち、リンクの原点位置や、リンク姿勢、リンク重心位置、関節角、全体重心位置、全体角運動量など、各拘束の種類毎に拘束条件を設定する基本拘束条件設定器が設けられ、各基本拘束条件設定器は該当する拘束の種類に関する線形等式を記述するためのパラメータを出力する機能を持つ。タスク実行時に発生するさまざまな等式拘束要求に応じて、このような基本拘束条件設定器を選択的に利用することで、ロボット全体についての線形等式からなる運動拘束条件を生成することができる。
【００３４】
あるいは、前記の拘束の種類毎に設けられた基本拘束条件設定器は、ロボットのタスクや運動状態に応じて課される運動拘束条件を関節角変化量などの線形不等式で表現する。例えば、関節角速度リミットや可動角リミットなどの各拘束の種類毎に運動拘束条件を設定する基本拘束条件設定器が設けられ、各基本拘束条件設定器は該当する拘束の種類に関する線形不等式を記述するためのパラメータを出力する機能を持つ。タスク実行時に発生するさまざまな不等式拘束要求に応じて、このような基本拘束条件設定器を選択的に利用することで、ロボット全体についての線形不等式からなる運動拘束条件を生成することができる。
【００３５】
また、本発明の第２の側面は、基体と前記基体に接続される複数の可動部を備えたロボットの運動制御装置であって、
前記ロボットに与えられるタスクや運動状態に応じて変化する冗長自由度の駆動法を規範の種類毎に設定する基本冗長自由度駆動法設定器と、
前記ロボットのタスクや運動実行時に発生する冗長自由度駆動法の変更要求に応じて適当な基本冗長自由度駆動法設定器を選択的に用いて、前記ロボット全体の冗長自由度駆動法を設定する冗長自由度駆動法設定部と、
前記冗長自由度駆動法設定部により設定された冗長自由度駆動法を満足するような各可動部の駆動量を決定する駆動量決定部と、
を具備することを特徴とするロボットの運動制御装置である。
【００３６】
冗長自由度駆動のための規範として、例えば、系の状態変化最小化、目標状態偏差最小化などが挙げられる。タスク実行時に発生する冗長自由度駆動法の変更要求に応じて該当する基本冗長自由度駆動法設定器を選択的に利用することで、ロボット全体についての冗長自由度駆動法を多様に設定することができる。
【００３７】
また、本発明の第３の側面は、基体と前記基体に接続される複数の可動部を備えたロボットの運動制御装置であって、
前記ロボットに与えられるタスクや運動状態に応じて課される運動拘束条件を拘束の種類毎に状態変数変化量の線形等式で表現する等式拘束条件設定器と、
前記ロボットのタスクや運動実行時に発生する拘束要求に応じて適当な等式拘束条件設定器を選択的に用いて、前記ロボットの状態変化量に必要な前記ロボット全体の運動拘束条件を課す等式拘束条件設定部と、
前記ロボットに与えられるタスクや運動状態に応じて課される運動拘束条件を拘束の種類毎に状態変数変化量の線形不等式で表現する不等式拘束条件設定器と、
前記ロボットのタスクや運動実行時に発生する拘束要求に応じて適当な不等式拘束条件設定器を選択的に用いて、前記ロボットの状態変化量に必要な前記ロボット全体の運動拘束条件を課す不等式拘束条件設定部と、
前記ロボットに与えられるタスクや運動状態に応じて変化する冗長自由度の駆動法を規範の種類毎に設定する基本冗長自由度駆動法設定器と、
前記ロボットのタスクや運動実行時に発生する冗長自由度駆動法の変更要求に応じて適当な基本冗長自由度駆動法設定器を選択的に用いて、前記ロボット全体の冗長自由度駆動法を設定する冗長自由度駆動法設定部と、
前記等式拘束条件設定部及び前記不等式拘束条件設定部により設定された前記ロボット全体についての等式及び不等式拘束条件、及び前記冗長自由度駆動法設定部により設定された前記ロボット全体についての冗長自由度駆動法をすべて満足するような各可動部の駆動量を決定する駆動量決定部と、
を具備することを特徴とするロボットの運動制御装置である。
【００３８】
このような場合、前記ロボット全体についての等式及び不等式拘束条件、及び前記ロボット全体についての冗長自由度駆動法を２次計画問題として定式化することができる。そして、この２次計画問題を双対法などの数値解法を用いることで解くことができ、ロボットの状態変数の変化量を求めることができる。（あるいは、不等式拘束を考慮しない場合は、ラグランジェ（Ｌａｇｒａｎｇｅ）乗数法などを用いて解析的に解くことも可能である。）そして、この状態変数変化量を積分演算することによって、次時刻におけるロボットの状態を求めることができる。
【００３９】
したがって、ロボットが複数のタスクを同時に実行する際に、各タスクによって課される時々刻々と変化する幾何学的、力学的な多様な運動拘束条件を同時に満足するように、各関節の駆動量の配分を実時間で決定することができる。
【００４０】
本発明によれば、例えば開リンク構造からなる任意の機構構成の脚式移動ロボットにおいて、あらゆるリンク上の点の位置・姿勢に関する幾何的拘束、全体の運動量に関する拘束、アクチュエータの可動範囲や駆動速度に関する不等式拘束など、状態変化量に関する線形等式・線形不等式の形で表される任意の拘束を課すことができる。すなわち、脚式移動ロボットに対して任意の運動状態で多様な運動拘束を課すことが可能となり、より広範なタスクを遂行できるようになる。
【００４１】
脚式移動ロボットに課される運動拘束は、ロボットの運動状態・要求タスクに応じて時間的に変化し得る。本発明によれば、このような時変の運動拘束条件に対して、（例えば解析解を用いた逆キネマティクスなどの）固定的な個別アルゴリズムで対応するのでなく、行列要素の値変更という簡素で統一的な枠組みで対応することができる。したがって、時間的に変化する多様な運動拘束条件を即応的に運動に反映することが容易で、柔軟に要求タスクに対応可能な脚式ロボットの実現に寄与する。
【００４２】
また、本発明に係る制御方式は、冗長自由度の駆動方法に関して、冗長自由度の駆動ストラテジを複数設定し、それらを動的に切り替えることが可能である。脚式ロボットの冗長自由度の最適な駆動方法は機体コンディション・タスクの種類等によって動的に変化し得る。本発明によれば、あらかじめ与えた系の目標状態との偏差を最小化したり、状態変化を最小化したりといった、複数の冗長自由度駆動方法を行列値の設定方法のみで変更可能であり、状況に応じて最適な全身協調方法に基づいて駆動される脚式ロボットの実現が容易となる。
【００４３】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【００４４】
【発明の実施の形態】
本発明は、脚式移動ロボットに対して、実行時に課される多様な運動拘束条件を同時に満足するように、各関節の駆動量の配分を実時間で決定する制御手段を提供する。 本発明によれば、脚式移動ロボットは、頻繁且つ複雑な接地状態の変化に柔軟に対応したり、複数のタスクを同時に遂行したりすることが容易になる。以下、図面を参照しながら本発明の実施形態について詳解する。
【００４５】
図１には、本発明の実施に供される２脚２腕を有する人間型ロボットの自由度構成を示している。
【００４６】
本実施形態に係るロボットは、基底（又は基体）Ｂから放射状に回転ジョイントを介して開リンク鎖が連なる構造をなし、７自由度を持つ腕部と、６自由度を持つ足部と、３自由度を持つ腰部と、２自由度を持つ頭部とで構成される。
【００４７】
基底Ｂは、左右の股関節位置を結ぶ線と体幹ヨー軸との交点で定義される。脚部は、基底Ｂに接続され、股関節３自由度（ヨー・ロール・ピッチ）、膝関節１自由度（ピッチ）、足首関節２自由度（ピッチ・ロール）で構成される。腰部は、３自由度（ピッチ・ロール・ヨー）で構成され、基底Ｂと胸部Ｃを接続する．腕部は、胸部Ｃに接続され、肩関節３自由度（ピッチ・ロール・ヨー）、肘関節２自由度（ピッチ・ヨー）、手首関節２自由度（ロール・ピッチ）で構成される。頭部は、胸部Ｃに接続され、首関節２自由度（パン・チルト）で構成される。
【００４８】
脚式移動ロボットの状態は、ワールド座標系における基底Ｂの位置ｐ_０＝（ｘ，ｙ，ｚ）Ｔ、姿勢α_０＝（θ_１， θ_２， θ_３）^Ｔ（例えば、オイラー角表現）、すべての関節角θ＝［θ_４，．．， θ_ｎ］^Ｔを並べてできる状態変数ｘ＝［ｐ_０，α_０， θ］^Ｔによって表現することができる。
【００４９】
ここで、基底の姿勢は、図２に示すように、長さ０の仮想リンクの仮想関節角θ_１， θ_２， θ_３によって表現している。ｎは仮想関節を含んだ関節数であり（図１に示した例では、ｎ＝３４）、θ_ｉ（ｉ＝１…ｎ）は関節ｉの関節角度を表している。また、状態変数の要素数Ｎ＝ｎ＋３と置く（図１に示す例では、Ｎ＝３７）。但し、仮想リンクを導入しなくても、本発明に関する技術的思想を実現することは可能である。
【００５０】
以下の説明では、現在の状態をｘ（ベクトル）とし、現在から微小時刻ｄｔ後の状態への変化量をｄｘとし、このｄｘによって運動の拘束条件を規定する。特に、以下の式に示すように線形な等式、あるいは不等式によって運動に拘束条件を課すことを考える。
【００５１】
【数１】

【００５２】
【数２】

【００５３】
以下では、これらを「等式拘束条件」、並びに「不等式拘束条件」と呼ぶ。ここで、ＡはＬ×Ｎ行列、ｂはＬ次元ベクトル、ＣはＭ×Ｎ行列、ｄはＭ次元ベクトルであり、Ｌは等式拘束条件数、Ｍは不等式拘束条件数を表している。本実施形態に係る脚式移動ロボットの制御システムは、所定の制御周期毎に上記の各式を満足する状態変化量ｄｘを算出し、現在状態ｘにｄｘを加算したｘ’＝ｘ＋ｄｘを実現するように全身関節を駆動する。
【００５４】
一般に、拘束条件数Ｌは状態変数Ｎの次元には満たない。このため、上記の２式［数１］、［数２］のみでは状態変化量ｄｘは一意に定まらない。すなわち、両者の差Ｎ−Ｌが冗長自由度に相当し、この冗長自由度の駆動方法を別途定める必要がある。そこで、本発明では、以下のような状態変化量ｄｘに関するエネルギ関数を最小化するようにｄｘを定めるものとする。
【００５５】
【数３】

【００５６】
ここで、ＷはＮ×Ｎの対称行列とし、ｕはＮ次元ベクトルとする。すると、関節角変化量ｄｘを求める問題は、以下に示す２次計画問題として定式化される。
【００５７】
【数４】

【００５８】
この２次計画問題は、双対法などの数値解法を用いることで解くことができる。不等式拘束を考慮しない場合は、ラグランジェ（Ｌａｇｒａｎｇｅ）乗数法などを用いて解析的に解くことも可能である。
【００５９】
すなわち、本発明では、脚式ロボットに課されるタスクや運動状態に応じて課される運動拘束条件を、現在状態からの変化量ｄｘに関する線形拘束式［数１］並びに［数２］で与えるとともに、冗長自由度の駆動ストラテジをエネルギ関数［数３］で規定する。運動拘束条件の変化に関しては、各拘束条件毎に特化した制御系を構成する必要がなく、行列Ａ，Ｃ及びベクトルｂ，ｄの変更のみで対応することができるので、多様且つ動的な拘束条件を扱い易い。また、冗長自由度の利用方法についても、行列Ｗ及びベクトルｕの変更のみで対応できるので、多様且つ動的な冗長自由度の駆動法を提供することができる。
【００６０】
図３には、本発明の一実施形態に係る脚式歩行ロボットの運動制御システムの構成を模式的に示している。図示の通り、本運動制御システムは、等式拘束条件設定部２−１と、不等式拘束条件設定部２−２と、冗長自由度駆動法設定部２−３と、等式拘束条件設定器群２−４と、不等式拘束条件設定器群２−５と、冗長自由度駆動法設定器群２−６と、等式拘束条件設定スペース２−７と、不等式拘束条件設定スペース２−８と、冗長自由度駆動法設定スペース２−９と、２次計画問題ソルバー２−１０と、積分器２−１１と、全身関節駆動部２−１２とで構成される。
【００６１】
等式拘束条件設定部２−１は、ロボットのタスクや運動状態に応じて課される拘束条件のうち、状態変数変化量の線形等式で表現されるものを設定する。例えば、リンクの原点位置や、リンク姿勢、リンク重心位置、関節角、全体重心位置、全体角運動量に関する拘束などがこれに該当する。
【００６２】
これら線形等式で表される拘束条件は、等式拘束条件設定スペース２−７内の上記マトリクスＡ及びベクトルｂに設定される。等式拘束条件設定器群２−４には、リンクの原点位置や、リンク姿勢、リンク重心位置、関節角、全体重心位置、全体角運動量など、各拘束の種類毎（あるいは制御対象毎）に拘束条件を設定する等式拘束条件設定器が設けられている。各等式拘束条件設定器は、該当する拘束の種類に関する線形拘束式を記述するためのパラメータを出力する機能を持つ。本実施形態では、等式拘束条件設定器は、ヤコビアンの形式で拘束式を線形表現するが、この点については後に詳解する。
【００６３】
そして、等式条件設定部２−１は、タスク実行時に発生するさまざまな等式拘束要求に応じて、等式拘束条件設定器群２−４より該当する等式拘束条件設定器を選択的に適宜利用することで、等式拘束条件設定スペース２−７内のマトリクスＡ及びベクトルｂに適切な値を設定し、この結果、ロボット全体についての線形等式からなる拘束条件を生成することができる。
【００６４】
不等式拘束条件設定部２−２は、ロボットのタスクや運動状態に応じて課される拘束条件のうち、関節角変化量などの線形不等式で表現されるものを設定する。例えば、関節角速度リミットや可動角リミットなどに関する拘束がこれに該当する。
【００６５】
これら線形不等式で表される拘束条件は、不等式拘束条件設定スペース２−８内の上記マトリクスＣ及びベクトルｄに設定される。不等式拘束条件設定器群２−５には、関節角速度リミットや可動角リミットなどの各拘束の種類（制御対象）毎に拘束条件を設定する不等式拘束条件設定器が設けられている。各不等式拘束条件設定器は、該当する拘束の種類に関する線形不等式を記述するためのパラメータを出力する機能を持つ。より具体的な不等式拘束条件設定器の構成法については後に詳解する。
【００６６】
そして、不等式条件設定部２−２は、タスク実行時に発生するさまざまな不等式拘束要求に応じて、不等式拘束条件設定器群２−５より該当する不等式拘束条件設定器を選択的に利用することで、不等式拘束条件設定スペース２−８内のマトリクスＣ及びベクトルｄに適切な値を設定し、この結果、ロボット全体についての線形不等式からなる拘束条件を生成することができる。
【００６７】
冗長自由度駆動法設定部２−３は、ロボットのタスクや運動状態に応じて変化する冗長自由度の駆動方法を設定する。冗長自由度の駆動方法については、系の状態変化最小化、目標状態偏差最小化などの規範が考えられる．
【００６８】
これら冗長自由度駆動のための規範は、冗長自由度駆動法設定スペース２−９内の上記マトリクスＷ及びベクトルｕに設定される。冗長自由度駆動法設定器群２−６には、系の状態変化最小化、目標状態偏差最小化など、各規範毎に、冗長自由度駆動法を設定する基本冗長自由度駆動法設定器が設けられている。各基本冗長自由度駆動法設定器は、該当する規範に従って冗長自由度の駆動法を出力する。
【００６９】
そして、冗長自由度駆動法設定部２−３は、タスク実行時に発生する冗長自由度駆動法の変更要求に応じて、冗長自由度駆動法設定器群２−６より該当する駆動法を選択的に利用することで、冗長自由度駆動法設定スペース２−９内のマトリクスＷ及びベクトルｕに適切な値を設定し、この結果、ロボット全体について所望の冗長自由度駆動法を設定することができる。
【００７０】
２次計画問題ソルバー２−１０は、等式拘束条件設定スペース２−７に設定された等式拘束条件、不等式拘束条件設定スペース２−８に設定された不等式拘束条件、並びに冗長自由度駆動法設定スペース２−９に設定された冗長自由度駆動法を２次計画問題（前述及び［数４］を参照のこと）として定式化し、求解すなわちこれらの拘束条件と冗長自由度の駆動法を同時に満足する状態変数変化量ｄｘを算出する。
【００７１】
積分器２−１１は，２次計画問題ソルバー２−１０が算出した状態変数変化量ｄｘを現在の状態変数値ｘに加算して、次時刻の状態変数値ｘ’＝ｘ＋ｄｘを算出する。全身関節駆動部２−１２は、積分器２−１１が算出した次時刻の状態変数値に基づいて、機体上の各関節を（位置）サーボ駆動する。
【００７２】
図４には、図３に示した脚式歩行ロボットの運動制御システムによって実現される制御処理の手順をフローチャートの形式で示している。
【００７３】
まず、ロボットの運動状態やタスクに応じて、リンクの原点位置、リンク姿勢、リンク重心位置、関節角、全体重心位置、全体角運動量などに関する等式拘束条件を、例えばユーザ・プログラムより入力する（ステップＳ１）。
【００７４】
次いで、前ステップＳ１で入力された等式拘束条件が等式拘束条件設定部２−１に入力されると、等式拘束条件設定器群２−４を選択的に利用して、等式拘束条件設定スペース２−７内の等式拘束条件設定マトリクスＡ、並びに等式拘束条件設定ベクトルｂに上記等式拘束条件を課すための値が設定される（ステップＳ２）。
【００７５】
次いで、関節角速度リミット、可動角リミットなどに関する不等式拘束条件を、例えばユーザ・プログラムより入力する（ステップＳ３）。
【００７６】
次いで、前ステップＳ３において入力された不等式拘束条件が不等式拘束条件設定部２−２に入力され、不等式拘束条件設定器群２−５を選択的に利用して、不等式拘束条件設定スペース２−８内の不等式拘束条件設定マトリクスＣ、不等式拘束条件設定ベクトルｄに上記不等式拘束条件を課すための値が設定される（ステップＳ４）。
【００７７】
次いで、状況に応じ、状態変化量の最小化や目標状態偏差の最小化などの規範に基づき、冗長自由度の駆動法を、例えばユーザ・プログラムより入力する（ステップＳ５）。
【００７８】
次いで、前ステップＳ５において入力された冗長自由度駆動法が冗長自由度駆動法設定部２−３に入力され、冗長自由度駆動法設定器群２−６を介して、冗長自由度駆動法設定スペース２−９内の冗長自由度駆動法設定マトリクスＷ、冗長自由度駆動法設定ベクトルｕに上記適切な値が設定される（ステップＳ６）。
【００７９】
次いで、上記の各ステップＳ２、Ｓ４、及びＳ６において、等式拘束条件設定スペース２−７、不等式拘束条件設定スペース２−８、冗長自由度駆動法設定スペース２−９に設定された２次計画問題（前述並びに［数４］を参照のこと）を解き、ユーザから指定された拘束条件と冗長自由度の駆動法を同時に満足する状態変数変化量ｄｘを算出する（ステップＳ７）。
【００８０】
さらに、積分器２−１１を用いて状態変数変化量を数値積分して、次時刻の状態変数値を求める（ステップＳ８）。
【００８１】
そして、前ステップＳ８により計算された次時刻の関節角値を参照値として全身関節駆動部２−１２に送出し位置サーボを行なう。
【００８２】
以上の処理を、所定の制御周期ｄｔ（例えば、ｄｔ＝１０ミリ秒）毎に実行する。
【００８３】
以下では、等式拘束条件設定器群２−７の具体的な構成法の例について説明する。
【００８４】
前述したように等式拘束条件は、現在の状態ｘの微小時刻ｄｔ後の変化量ｄｘに関する線形拘束式によって表される（［数１］を参照のこと）。本実施形態では、微小な変化量の関係を線形的に表現するために、ヤコビアンを用いる。
【００８５】
例えば、リンク原点位置基本拘束条件設定器は、リンク座標系原点位置に関するヤコビアンを用いて構成することができる。本明細書中では、関節ｉを介して親リンクと接続されるリンクをリンクｉと呼称する。リンク座標系とは、例えば親リンクとリンクｉの接合点に置かれたリンクｉと姿勢を同じくする座標系のことである。リンクｉの原点位置速度ｄｐ＿ｉ／ｄｔ（３次元ベクトル）は、状態変数速度ｄｘ／ｄｔ（Ｎ次元ベクトル）とリンクｉの原点位置速度に関するヤコビアンＪ_{ｐ ＿ ｉ}（３×Ｎ行列）によって表現することができる。
【００８６】
【数５】

【００８７】
リンクｉの原点位置速度に関するヤコビアンＪ_{ｐ ＿ ｉ}は、以下の式で求めることができる
【００８８】
【数６】

【００８９】
ここで、ｚ＿ｋは関節ｋの回転軸方向ベクトルを、ｐ＿ｉ及ｐ＿ｋはリンクｉ及びリンクｋの位置を表している（図５を参照のこと）。上記の式［数５］より、リンクｉの原点位置の微小変化量ｄｐ＿ｉと、状態変数ｘの微小変化量ｄｘの間には近似的に以下の関係が成立する。
【００９０】
【数７】

【００９１】
よって、リンクｉのｘ，ｙ，ｚ方向原点位置に対し、それぞれ微小変化量ｄｐ＿ｉｘ，ｄｐ＿ｉｙ，ｄｐ＿ｉｚが生じるように運動拘束を与えたい場合は以下の等式拘束を与えればよい。
【００９２】
【数８】

【００９３】
【数９】

【００９４】
【数１０】

【００９５】
ここで、Ｊ_{ｐ ＿ ｉ}ｘ，Ｊ_{ｐ ＿ ｉ}ｙ，Ｊ_{ｐ ＿ ｉ}ｚはそれぞれＪ_{ｐ ＿ ｉ}の第１、第２、第３行を表している。リンク原点位置拘束器にリンク原点位置拘束の要求が入力された場合、リンク原点位置拘束器は、上記の式［数８］〜［数１０］の係数を等式拘束条件設定スペース２−７の等式拘束条件設定マトリクスＡ及び等式拘束条件設定ベクトルｂの新たな行に設定する。例えば、リンク原点のｘ方向位置に関する拘束要求が入力された場合、式［数８］に従い、等式拘束条件設定マトリクスＡの新たな行にＪ_{ｐ ＿ ｉ}ｘを、等式拘束条件設定ベクトルｂの新たな行にｄｐ＿ｉｘをそれぞれ代入する。
【００９６】
同様に、リンクの姿勢拘束器は、リンク角速度に関するヤコビアンを用いて構成することができる。リンクｉの姿勢角速度ω＿ｉ（３次元ベクトル）は、状態変数速度ｄｘ／ｄｔ（Ｎ次元ベクトル）とリンクｉの角速度に関するヤコビアンＪ_{ω ＿ ｉ}（３×Ｎ行列）によって表現することができる。
【００９７】
【数１１】

【００９８】
但し、リンクｉの角速度に関するヤコビアンＪ_{ω ＿ ｉ}は以下の式で与えられる。
【００９９】
【数１２】

【０１００】
［数１１］より、リンクｉの姿勢（オイラー角で表現されているとする）の微小変化量ｄα＿ｉと、状態変数ｘの微小変化量ｄｘの間には近似的に以下の関係が成立する。
【０１０１】
【数１３】

【０１０２】
ここで、Ｔ＿ｉは角速度ベクトルをオイラー角速度ベクトルに変換する行列である。よって、リンクｉのｘ，ｙ，ｚ方向に関し、それぞれ微小オイラー角変化ｄα＿ｉｘ、ｄα＿ｉｙ、ｄα＿ｉｚが生じるように運動拘束を与えたい場合は以下の等式拘束を与えればよい。
【０１０３】
【数１４】

【０１０４】
【数１５】

【０１０５】
【数１６】

【０１０６】
なお，Ｊ_{α ＿ ｉ}ｘ，Ｊ_{α ＿ ｉ}ｙ，Ｊ_{α ＿ ｉ}ｚはそれぞれ行列（Ｔ＿ｉ Ｊω＿ｉ）の第１、第２、第３行を表している。リンク姿勢拘束器にリンク姿勢拘束の要求が入力された場合、リンク姿勢拘束器はこれらの式［数１４］〜［数１６］の係数を、等式拘束条件設定スペース２−７の等式拘束条件設定マトリクスＡ及び等式拘束条件設定ベクトルｂの新たな行に設定する。例えば、リンクｉのｘ方向姿勢に関する拘束要求が入力された場合、式［数１４］に従い、等式拘束条件設定マトリクスＡの新たな行にＪ_{α ＿ ｉ}ｘを、等式拘束条件設定ベクトルｂの新たな行にｄα＿ｉｘをそれぞれ代入する。
【０１０７】
リンク重心位置拘束器は、リンク原点位置拘束器と同様にして構成することができる。すなわち、リンクｉの重心位置速度ｄｒ＿ｉ／ｄｔ（３次元ベクトル）は、状態変数速度ｄｘ／ｄｔ（Ｎ次元ベクトル）とリンクｉの重心位置速度に関するヤコビアンＪ_{ｒ ＿ ｉ}（３×Ｎ行列）によって表現することができる。
【０１０８】
【数１７】

【０１０９】
リンクｉの原点位置速度に関するヤコビアンＪ_{ｐｇ ＿ ｉ}は以下の式で求めることができる。
【０１１０】
【数１８】

【０１１１】
ここで、ｚ＿ｋは関節ｋの回転軸方向ベクトルを、ｒ＿ｉはリンクｉの重心位置を、ｐ＿ｋはリンクｋの位置を表している（図５を参照のこと）。上記の式［数１７］より、リンクｉの重心位置の微小変化量ｄｒ＿ｉと、状態変数ｘの微小変化量ｄｘの間には近似的に以下の関係が成立する。
【０１１２】
【数１９】

【０１１３】
よって、リンクｉのｘ、ｙ、ｚの各軸方向の重心位置に対し、それぞれ微小変化量ｄｒ＿ｉｘ，ｄｒ＿ｉｙ，ｄｒ＿ｉｚが生じるように運動拘束を与えたい場合は以下の等式拘束を与えればよい。
【０１１４】
【数２０】

【０１１５】
【数２１】

【０１１６】
【数２２】

【０１１７】
ここで、Ｊ_{ｒ ＿ ｉ}ｘ，Ｊ_{ｒ ＿ ｉ}ｙ，Ｊ_{ｒ ＿ ｉ}ｚはそれぞれＪ_{ｒ ＿ ｉ}の第１、第２、第３行をそれぞれ表している。リンク重心位置拘束器にリンク重心位置拘束の要求が入力された場合、リンク重心位置拘束器はこれらの式［数２０］〜［数２２］の係数を等式拘束条件設定スペース２−７の等式拘束条件設定マトリクスＡ及び等式拘束条件設定ベクトルｂの新たな行に設定する。例えば、リンク重心のｘ方向位置に関する拘束要求が入力された場合、式［数２０］に従い、等式拘束条件設定マトリクスＡの新たな行にＪ_{ｒ ＿ ｉ}ｘを等式拘束条件設定ベクトルｂの新たな行にｄｒ＿ｉｘを代入する。
【０１１８】
全体重心位置拘束器は、ロボット全体の重心位置変位に拘束を課す。ロボット全体の重心位置速度ｄｒ／ｄｔ（３次元ベクトル）は、状態変数速度ｄｘ／ｄｔ（Ｎ次元ベクトル）とロボット全体の重心位置速度に関するヤコビアンＪ_ｒ（３×Ｎ行列）によって表現することができる。
【０１１９】
【数２３】

【０１２０】
ロボットの重心位置速度に関するヤコビアンＪ_ｒは以下の式で求めることができる。
【０１２１】
【数２４】

【０１２２】
ここでｍ＿ｉはリンクｉの質量、Ｍはロボット全体の質量、Ｊ_{ｒ ＿ ｉ}はリンクｉの重心位置速度に関するヤコビアンである。上記の式［数２３］より、ロボット全体の重心位置の微小変化量ｄｒと状態変数ｘの微小変化量ｄｘの間には近似的に以下の関係が成立する。
【０１２３】
【数２５】

【０１２４】
よって、ロボット全体のｘ、ｙ、ｚ方向重心位置に関し、それぞれ微小変化量ｄｒ＿ｘ、ｄｒ＿ｙ、ｄｒ＿ｚが生じるように運動拘束を与えたい場合は以下の等式拘束を与えればよい。
【０１２５】
【数２６】

【０１２６】
【数２７】

【０１２７】
【数２８】

【０１２８】
ここで、Ｊ_{ｒ ＿ ｘ}、Ｊ_{ｒ ＿ ｙ}、Ｊ_{ｒ ＿ ｚ}はそれぞれＪ_ｒの第１、第２、第３行を表している。全体重心位置拘束器にロボットの全体重心位置拘束の要求が入力された場合、全体重心位置拘束器はこれらの式［数２６］〜［数２８］の係数を等式拘束条件設定スペース２−７の等式拘束条件設定マトリクスＡ及び等式拘束条件設定ベクトルｂの新たな行に設定する。例えば、ロボット全体の重心位置のｘ方向に関する拘束要求が入力された場合、［数２６］に従い、等式拘束条件設定マトリクスＡの新たな行にＪ_{ｒ ＿ ｘ}を、等式拘束条件設定ベクトルｂの新たな行にｄｒ＿ｘを代入する。
【０１２９】
全体角運動量拘束器はロボット全体の角運動量変化に拘束を課す、ロボット全体の角運動量Ｌ（３次元ベクトル）は、状態変数速度ｄｘ／ｄｔ（Ｎ次元ベクトル）とロボット全体の角運動量に関するヤコビアンＪ_Ｌ（３×Ｎ行列）によって表現することができる。
【０１３０】
【数２９】

【０１３１】
ロボット全体の角運動量に関するヤコビアンＪ_Ｌは以下の式で求めることができる。
【０１３２】
【数３０】

【０１３３】
ここで、Ｘ（ｖ）はベクトルの外積演算を行列表現に変換するための歪対称行列、ｍ＿ｉはリンクｉの質量、ｒ＿ｉはリンクｉの重心位置、ｒはロボット全体の重心位置、Ｊ_{ｒ ＿ ｉ}はリンクｉの重心位置速度に関するヤコビアン、Ｉ＿ｉはリンクｉの慣性行列、Ｊ_{ω ＿ ｉ}はリンクｉの角速度に関するヤコビアンである。［数３０］より、ロボット全体の角運動量の微小変化量ｄＬと状態変数ｘの微小変化量ｄｘの間には近似的に以下の関係が成立する。
【０１３４】
【数３１】

【０１３５】
よって、ロボット全体のｘ、ｙ、ｚ軸回り角運動量に関し、それぞれ微小変化量ｄＬ＿ｘ、ｄＬ＿ｙ、ｄＬ＿ｚが生じるように運動拘束を与えたい場合は、以下の等式拘束を与えればよい。
【０１３６】
【数３２】

【０１３７】
【数３３】

【０１３８】
【数３４】

【０１３９】
ここで、Ｊ_{Ｌ ＿ ｘ}、Ｊ_{Ｌ ＿ ｙ}、Ｊ_{Ｌ ＿ ｚ}はそれぞれＪ_ｒの第１、第２、第３行を表している。全体重心位置拘束器にロボットの全体重心位置拘束の要求が入力された場合、全体重心位置拘束器はこれらの式［数３２］〜［数３４］の係数を等式拘束条件設定スペース２−７の等式拘束条件設定マトリクスＡ及び等式拘束条件設定ベクトルｂの新たな行に設定する。例えば、ロボット全体のｘ軸回り角運動量に関する拘束要求が入力された場合、［数３２］に従い、等式拘束条件設定マトリクスＡの新たな行にＪ_{Ｌ ＿ ｘ}を、等式拘束条件設定ベクトルｂの新たな行にｄＬ＿ｘを代入する。
【０１４０】
関節角拘束器は、例えば以下のようにして容易に構成することができる。すなわち、関節ｋの現在関節角θ_ｋと目標関節角θ_{ｋ ＿ ０}の間の偏差Δθ_ｋを下式の通りとする。
【０１４１】
【数３５】

【０１４２】
この場合、下式のような等式拘束を課すように構成すればよい。
【０１４３】
【数３６】

【０１４４】
関節ｋの関節角変位がΔθ_ｋになるように拘束要求が入力された場合、上記の式［数３６］に従い、等式拘束条件設定マトリクスＡの新たな行にｅ＿｛ｋ＋３｝^Ｔを、等式拘束条件設定ベクトルｂの新たな行にΔθ_ｋを代入する。但し、ｅ＿｛ｋ＋３｝は第ｋ＋３要素が１のＮ次元単位ベクトルとする。
【０１４５】
同様にして、不等式拘束条件設定器群も構成することができる。例えば、関節角速度拘束器は、関節ｋの最大角速度をｄθ_ｋ／ｄｔ＿ｍａｘとすると、下式のような不等式拘束条件を課すればよい。
【０１４６】
【数３７】

【０１４７】
可動角拘束器については、関節ｋの現在関節角をθ_ｋ、最大関節角を_{θｋ ＿ ｍａｘ}、最小関節角をθ_{ｋ ＿ ｍｉｎ}とすると、下式のような不等式拘束条件を課すればよい。
【０１４８】
【数３８】

【０１４９】
いずれの不等式条件設定器も、それぞれ不等式拘束条件設定スペース２−８の不等式拘束条件設定マトリクスＣ及び不等式拘束条件設定ベクトルｄに上記不等式の係数を設定するように構成する。
【０１５０】
冗長自由度駆動法設定器群についても、行列及びベクトルの値の設定のしかたによってさまざまな冗長自由度駆動のためのストラテジを与えることができる。例えば、前時刻からの系の状態変化を最小とする、状態変化最小化規範の冗長自由度駆動法設定器であれば、
【０１５１】
【数３９】

【０１５２】
となるように冗長自由度駆動法設定スペース２−９内の冗長自由度駆動法設定マトリクスＷ、及び冗長自由度駆動法設定ベクトルｕを設定すればよい。すなわち、下式を設定するように構成する。
【０１５３】
【数４０】

【０１５４】
あるいは、系の目標状態ｘ０との偏差を最小化する、状態偏差最小化規範の冗長自由度駆動法設定器であれば、
【０１５５】
【数４１】

【０１５６】
のｄｘを含む項、
【０１５７】
【数４２】

【０１５８】
を最小化するように、冗長自由度駆動法設定スペース２−９内の冗長自由度駆動法設定マトリクスＷ、及び冗長自由度駆動法設定ベクトルｕを設定すればよい。すなわち、下式を設定するように構成する。
【０１５９】
【数４３】

【０１６０】
ここで、ｗは第ｉ要素が正の実数ｗ＿ｉとなるようなＮ次元ベクトルを、ｘ０＿ｉはｘ０の第ｉ成分を表すものとする。また、ｄｉａｇ（ｗ＿ｉ）は第ｉ対角成分の値がｗ＿ｉとなるようなＮ×Ｎ対角行列を、（ａ｜ｂ）は第ｉ成分がａの第ｉ成分とｂの第ｉ成分の積となるＮ次元ベクトルを表すものとする。
【０１６１】
以上説明したような構成により、脚式移動ロボットは、実行時に課される多様な拘束条件を同時に満足するように、各関節の駆動量の配分を実時間で決定して動作制御することができる。
【０１６２】
図６には、本発明に係る制御方式を脚式移動ロボットの起き上がり運動制御に適用した例を示している。
【０１６３】
時刻０．０秒から時刻２．０秒の間、ロボットには、手先高さを床面に拘束し、足底位置・姿勢を床面に拘束し、且つ重心が後退・上昇する軌道をなぞるような拘束が課されている。これらの拘束は、等式拘束条件設定部２−１を介して制御周期ｄｔ後の系の状態変化量に関する拘束として入力され、等式拘束条件設定器群２−４により、等式拘束条件設定スペース２−７内の等式拘束条件設定マトリクスＡ及び等式拘束条件設定ベクトルに適切な値が設定される。
【０１６４】
図６に示すように、この期間においては、等式拘束条件設定マトリクスＡの各行に、手部Ｚ方向速度に関するヤコビアン、足部Ｘ，Ｙ，Ｚ方向速度ヤコビアン、足部Ｘ、Ｙ、Ｚ各軸方向姿勢角速度ヤコビアン、全体重心Ｘ、Ｚ方向位置ヤコビアンが制御周期毎に再計算されて代入されるとともに、等式拘束条件設定ベクトルｂの重心位置拘束に関する行には制御周期の間に変化しなければならない変位（＋は正値を−は負値を表す）が、それ以外の行には変化量０を表す０が代入される。
【０１６５】
図示の例では、冗長自由度駆動法に状態変化最小化規範を用いている。これらの拘束条件を満足するように２次計画問題が制御周期毎に解かれる。この結果に基づいて全身を駆動する様子が左列のイメージに描かれている。この期間において、課された拘束条件をすべて満足するように、冗長自由度も適切に用いながら、全身が駆動されている様子が同図から理解できよう。
【０１６６】
時刻３．０秒に達すると、手部Ｚ方向位置に関する拘束が解除される。これ以後，等式拘束条件設定マトリクスＡ及び等式拘束条件設定ベクトルｂに手部Ｚ方向速度に関する行は挿入されなくなっている様子がわかる。左列のイメージからも、手先の高さ拘束が解除され、手部が上昇を開始する様子が見て取れる。
【０１６７】
さらに、時刻５．０秒に達すると、手部を腰部にひきつけるため、手部が後退軌道に従うように拘束が新たに課されている。これに伴い、等式拘束条件設定マトリクスＡ及び等式拘束条件設定ベクトルｂに手部Ｘ方向位置拘束に関する行が挿入されている。
【０１６８】
左列のイメージから、この時刻以降、手部のＸ座標が減少するように全身が駆動されていることが理解できよう。このように、本発明に係る制御方式によれば、機体動作の実行時に動的に拘束条件が変化したとしても、マトリクス及びベクトルの数値の書き換えのみで容易に対応することが可能であり、要求された拘束条件をすべて満足する全身関節の駆動量の配分を実時間で決定することができる。
【０１６９】
［追補］
以上、特定の実施例を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施例の修正や代用を成し得ることは自明である。
【０１７０】
本発明の要旨は、必ずしも「ロボット」と称される製品には限定されない。すなわち、電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行なう機械装置あるいはその他一般的な移動体装置であるならば、例えば玩具などのような他の産業分野に属する製品であっても、同様に本発明を適用することができる。
【０１７１】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、冒頭に記載した特許請求の範囲の欄を参酌すべきである。
【０１７２】
【発明の効果】
以上詳記したように、本発明によれば、移動やバランス維持、アーム作業といった複数のタスクを同時に実行することができる、優れた脚式歩行ロボットの運動制御装置を提供することができる。
【０１７３】
また、本発明によれば、各タスクによって課される多様な運動拘束条件を同時に満足するように各関節の駆動量の配分を実時間で決定することができる、優れた脚式歩行ロボットの運動制御装置を提供することができる。
【０１７４】
また、本発明によれば、時々刻々と変化する幾何学的、力学的な多様な拘束条件を同時に満足するように全身の自由度の駆動量を適切に配分して動作することができる、優れた脚式歩行ロボットの運動制御装置を提供することができる。
【０１７５】
本発明に係る制御方式は、歩行などの特定の運動状態に対して限定的に適用されるものではなく、脚式移動ロボットの任意の運動状態に対して適用可能な高い汎用性を有している。また、開リンク構造からなる任意の機構構成の脚式移動ロボットにおいて、あらゆるリンク上の点の位置・姿勢に関する幾何的拘束、全体の運動量に関する拘束、アクチュエータの可動範囲や駆動速度に関する不等式拘束など、状態変化量に関する線形等式・線形不等式の形で表される任意の拘束を課すことができる。本発明によれば、脚式移動ロボットに対して任意の運動状態で多様な運動拘束を課すことが可能となり、より広範なタスクを遂行できるようになる。
【０１７６】
また、本発明に係る制御方式は、固定的な運動拘束問題に限定されず、脚式移動ロボットの動作時に課される運動拘束条件の動的変化に対応することができるという長所を有している。脚式移動ロボットに課される運動拘束は、ロボットの運動状態・要求タスクに応じて時間的に変化し得る。本発明によれば、このような時変の拘束条件に対して、（例えば解析解を用いた逆キネマティクスなどの）固定的な個別アルゴリズムで対応するのでなく、行列要素の値変更という簡素で統一的な枠組みで対応することができる。したがって、時間的に変化する多様な拘束条件を即応的に運動に反映することが容易で、柔軟に要求タスクに対応可能な脚式ロボットの実現に寄与する。
【０１７７】
また、本発明に係る制御方式は、冗長自由度の駆動方法に関して、冗長自由度の駆動ストラテジを複数設定し、それらを動的に切り替えることが可能である。脚式ロボットの冗長自由度の最適な駆動方法は機体コンディション・タスクの種類等によって動的に変化し得る。本発明によれば、あらかじめ与えた系の目標状態との偏差を最小化したり、状態変化を最小化したりといった、複数の冗長自由度駆動方法を行列値の設定方法のみで変更可能であり、状況に応じて最適な全身協調方法に基づいて駆動される脚式ロボットの実現が容易となる。
【図面の簡単な説明】
【図１】本発明の実施に供される２脚２腕を有する人間型ロボットの自由度構成を示した図である。
【図２】図１に示した脚式移動ロボットの基底の姿勢を説明するための図である。
【図３】本発明の一実施形態に係る脚式歩行ロボットの運動制御システムの構成を模式的に示した図である。
【図４】図３に示した脚式歩行ロボットの運動制御システムによって実現される制御処理の手順を示したフローチャートである。
【図５】座標系の定義を説明するための図である。
【図６】本発明に係る制御方式を脚式移動ロボットの起き上がり運動制御に適用した例を示した図である。
【符号の説明】
２−１…等式拘束条件設定部
２−２…不等式拘束条件設定部
２−３…冗長自由度駆動法設定部
２−４…等式拘束条件設定器群
２−５…不等式拘束条件設定器群
２−６…冗長自由度駆動法設定器群
２−７…等式拘束条件設定スペース
２−８…不等式拘束条件設定スペース
２−９…冗長自由度駆動法設定スペース
２−１０…２次計画問題ソルバー
２−１１…積分器
２−１２…全身関節駆動部

Claims (12)

1. 基体と前記基体に接続される複数の可動部を備えたロボットの運動制御装置であって、
   前記ロボットに与えられるタスクや運動状態に応じて課される運動拘束条件を拘束の種類毎に設定する基本拘束条件設定器と、
   前記ロボットのタスクや運動実行時に発生する運動拘束要求に応じて適当な基本拘束条件設定器を選択的に用いて、前記ロボットの状態変化量に必要な前記ロボット全体の運動拘束条件を課す拘束条件設定部と、
   前記拘束条件設定部により設定された運動拘束条件をすべて満足するような各可動部の駆動量を決定する駆動量決定部と、
   を具備することを特徴とするロボットの運動制御装置。
2. 前記複数の可動部は、少なくとも上肢、下肢及び体幹部を含む、
   ことを特徴とする請求項１に記載のロボットの運動制御装置。
3. 前記ロボット全体の姿勢角は仮想リンクの仮想関節角を用いて表現される、
   ことを特徴とする請求項１に記載のロボットの運動制御装置。
4. 前記の拘束の種類毎の基本拘束条件設定器は、ロボットのタスクや運動状態に応じて課される運動拘束条件を状態変数変化量の線形等式で表現する、
   ことを特徴とする請求項１に記載のロボットの運動制御装置。
5. 前記の各基本拘束条件設定器はヤコビアンの形式で拘束式を線形表現する、
   ことを特徴とする請求項４に記載のロボットの運動制御装置。
6. 前記の拘束の種類毎の基本拘束条件設定器は、ロボットのタスクや運動状態に応じて課される運動拘束条件を状態変数変化量の線形不等式で表現する、
   ことを特徴とする請求項１に記載のロボットの運動制御装置。
7. 基体と前記基体に接続される複数の可動部を備えたロボットの運動制御装置であって、
   前記ロボットに与えられるタスクや運動状態に応じて変化する冗長自由度の駆動法を規範の種類毎に設定する基本冗長自由度駆動法設定器と、
   前記ロボットのタスクや運動実行時に発生する冗長自由度駆動法の変更要求に応じて適当な基本冗長自由度駆動法設定器を選択的に用いて、前記ロボット全体の冗長自由度駆動法を設定する冗長自由度駆動法設定部と、
   前記冗長自由度駆動法設定部により設定された冗長自由度駆動法を満足するような各可動部の駆動量を決定する駆動量決定部と、
   を具備することを特徴とするロボットの運動制御装置。
8. 基体と前記基体に接続される複数の可動部を備えたロボットの運動制御装置であって、
   前記ロボットに与えられるタスクや運動状態に応じて課される運動拘束条件を拘束の種類毎に状態変数変化量の線形等式で表現する等式拘束条件設定器と、
   前記ロボットのタスクや運動実行時に発生する運動拘束要求に応じて適当な等式拘束条件設定器を選択的に用いて、前記ロボットの状態変化量に必要な前記ロボット全体の運動拘束条件を課す等式拘束条件設定部と、
   前記ロボットに与えられるタスクや運動状態に応じて課される運動拘束条件を拘束の種類毎に状態変数変化量の線形不等式で表現する不等式拘束条件設定器と、
   前記ロボットのタスクや運動実行時に発生する運動拘束要求に応じて適当な不等式拘束条件設定器を選択的に用いて、前記ロボットの状態変化量に必要な前記ロボット全体の運動拘束条件を課す不等式拘束条件設定部と、
   前記ロボットに与えられるタスクや運動状態に応じて変化する冗長自由度の駆動法を規範の種類毎に設定する基本冗長自由度駆動法設定器と、
   前記ロボットのタスクや運動実行時に発生する冗長自由度駆動法の変更要求に応じて適当な基本冗長自由度駆動法設定器を選択的に用いて、前記ロボット全体の冗長自由度駆動法を設定する冗長自由度駆動法設定部と、
   前記等式拘束条件設定部及び前記不等式拘束条件設定部により設定された前記ロボット全体についての等式及び不等式拘束条件、及び前記冗長自由度駆動法設定部により設定された前記ロボット全体についての冗長自由度駆動法をすべて満足するような各可動部の駆動量を決定する駆動量決定部と、
   を具備することを特徴とするロボットの運動制御装置。
9. 前記複数の可動部は、少なくとも上肢、下肢及び体幹部を含む、
   ことを特徴とする請求項８に記載のロボットの運動制御装置。
10. 前記脚式歩行ロボットの姿勢角は仮想リンクの仮想関節角を用いて表現される、
    ことを特徴とする請求項８に記載のロボットの運動制御装置。
11. 前記の各等式拘束条件設定器はヤコビアンの形式で拘束式を線形表現する、
    ことを特徴とする請求項８に記載のロボットの運動制御装置。
12. 前記駆動量決定部は、
    前記ロボット全体についての等式及び不等式拘束条件、及び前記ロボット全体についての冗長自由度駆動法を２次計画問題として定式化して、前記ロボットの状態変数の変化量を求解する２次計画問題求解部と、
    状態変数の変化量を積分して、前記ロボットの次時刻の状態を算出する積分部と、
    を備えることを特徴とする請求項８に記載のロボットの運動制御装置。

Images