JP2004148491A - ロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラム - Google Patents

Abstract

【課題】 機体上の部位毎の目標軌道を修正して未知外力モーメント及び未知外力を補償する。
【解決手段】 各部位毎の軌道からなる所望の動作データを基に、前記ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ釣合い方程式を生成し、ＺＭＰ釣合い方程式上で現れるモーメント・エラーを算出し、機体を複数の部位に分割し、該モーメント・エラーを打ち消すために目標軌道を修正する部位の優先順位を設定し、優先順位に従った順番で、各部位毎に目標軌道を修正してモーメント・エラーを補償する。
【選択図】 図３

Description

本発明は、少なくとも複数の可動部を備えたロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラムに係り、特に、可動脚などを備えた移動式のロボット装置において、ＺＭＰを安定度判別規範に用いて姿勢安定化制御を行なうロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラムに関する。

さらに詳しくは、本発明は、機体上の部位毎の目標（希望）軌道を修正して未知外力モーメント及び未知外力を補償するロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラムに関する。

電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行う機械装置のことを「ロボット」という。ロボットの語源は、スラブ語の“ＲＯＢＯＴＡ（奴隷機械）”に由来すると言われている。わが国では、ロボットが普及し始めたのは１９６０年代末からであるが、その多くは、工場における生産作業の自動化・無人化などを目的としたマニピュレータや搬送ロボットなどの産業用ロボット（ｉｎｄｕｓｔｒｉａｌ ｒｏｂｏｔ）であった。

最近では、ヒトやサルなどの２足直立歩行を行なう動物の身体メカニズムや動作を模した脚式移動ロボットに関する研究開発が進展し、実用化への期待も高まってきている。２足直立による脚式移動は、クローラ式や、４足又は６足式などに比し不安定で姿勢制御や歩行制御が難しくなるが、不整地や障害物など作業経路上に凹凸のある歩行面や、階段や梯子の昇降など不連続な歩行面に対応することができるなど、柔軟な移動作業を実現できるという点で優れている。

また、ヒトの生体メカニズムや動作を再現した脚式移動ロボットのことを、特に、「人間形」、若しくは「人間型」のロボット（ｈｕｍａｎｏｉｄ ｒｏｂｏｔ）と呼ぶ。人間型ロボットは、例えば、生活支援、すなわち住環境その他の日常生活上のさまざまな場面における人的活動の支援などを行なうことができる。

人間の作業空間や居住空間のほとんどは、２足直立歩行という人間が持つ身体メカニズムや行動様式に合わせて形成されおり、車輪その他の駆動装置を移動手段とした現状の機械システムが移動するのには多くの障壁が存在する。したがって、機械システムすなわちロボットがさまざまな人的作業を代行し、さらに人間の住空間に深く浸透していくためには、ロボットの移動可能範囲が人間のそれとほぼ同じであることが好ましい。これが、脚式移動ロボットの実用化が大いに期待されている所以でもある。

２足の脚式移動ロボットに関する姿勢制御や安定歩行に関する技術は既に数多提案されている。ここで言う安定な「歩行」とは、「転倒することなく、脚を使って移動すること」と定義することができる。ロボットの姿勢安定化制御は、ロボットの転倒を回避する上で非常に重要である。何故ならば、転倒は、ロボットが実行中の作業を中断することを意味し、且つ、転倒状態から起き上がって作業を再開するために相当の労力や時間が払われるからである。また、転倒によって、ロボット本体自体、あるいは転倒するロボットと衝突する相手側の物体にも、致命的な損傷を与えてしまう危険があるからである。

脚式移動ロボットの姿勢安定化制御や歩行時の転倒防止に関する提案の多くは、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）を歩行の安定度判別の規範として用いている。ＺＭＰによる安定度判別規範は、歩行系から路面には重力と慣性力、並びにこれらのモーメントが路面から歩行系への反作用としての床反力並びに床反力モーメントとバランスするという「ダランベールの原理」に基づく。力学的推論の帰結として、足底接地点と路面の形成する支持多角形（すなわちＺＭＰ安定領域）の内側にピッチ軸及びロール軸モーメントがゼロとなる点、すなわちＺＭＰが存在する（例えば、非特許文献１を参照のこと）。

要約すれば、ＺＭＰ規範とは、「歩行のあらゆる瞬間において、ＺＭＰが足部と路面とが形成する支持多角形の内側に存在し、且つ、ロボットが路面に押す方向の力が作用すれば、ロボットが転倒（機体が回転運動）することなく安定に歩行できる」とするものである。

ＺＭＰ規範に基づく２足歩行パターン生成によれば、足底着地点をあらかじめ設定することができ、路面形状に応じた足先の運動学的拘束条件を考慮し易いなどの利点がある。また、ＺＭＰを安定度判別規範とすることは、力ではなく軌道を運動制御上の目標値として扱うことを意味するので、技術的に実現可能性が高まる。

例えば、脚式移動ロボットは、ＺＭＰがゼロとなる床面上の点を目標値に一致させるようにして安定歩行を行なうことができる（例えば、特許文献１を参照のこと）。

また、脚式移動ロボットは、ＺＭＰが支持多面体（多角形）内部、又は、着地、離床時にＺＭＰが支持多角形の端部から少なくとも所定の余裕を有する位置にあるように構成することができる（例えば、特許文献２を参照のこと）。この場合、外乱などを受けても所定距離だけＺＭＰの余裕があり、歩行時の機体の安定性が向上する。

また、脚式移動ロボットの歩き速度をＺＭＰ目標位置によって制御することができる（例えば、特許文献３を参照のこと）。すなわち、あらかじめ設定された歩行パターン・データを用い、ＺＭＰを目標位置に一致させるように脚部関節を駆動するとともに、上体の傾斜を検出してその検出値に応じて設定された歩行パターン・データの吐き出し速度を変更する。未知の凹凸を踏んでロボットが例えば前傾するときは、吐き出し速度を速めることで姿勢を回復することができる。またＺＭＰを目標位置に制御するので、両脚支持期で吐き出し速度を変更しても支障がない。

また、脚式移動ロボットの着地位置をＺＭＰ目標位置によって制御することができる（例えば、特許文献４を参照のこと）。すなわち、同公報に記載の脚式移動ロボットは、ＺＭＰ目標位置と実測位置とのずれを検出し、それを解消するように脚部の一方又は双方を駆動するか、又はＺＭＰ目標位置まわりにモーメントを検出してそれが零になるように脚部を駆動することで、安定歩行を実現する。

また、脚式移動ロボットの傾斜姿勢をＺＭＰ目標位置によって制御することができる（例えば、特許文献５を参照のこと）。すなわち、ＺＭＰ目標位置まわりのモーメントを検出し、モーメントが生じたときは、それが零になるように脚部を駆動することで安定歩行を行なう。

上述したように、ＺＭＰを安定度判別規範に用いたロボットの姿勢安定化制御は、足底接地点と路面の形成する支持多角形の内側にピッチ及びロール軸モーメントがゼロとなる点を探索することにある。

しかしながら、本発明者らの先験的な検証の結果、ロボットが高速に脚式動作する際には、機体に対してはピッチ軸並びにロール軸回りのモーメントだけでなく、ヨー軸すなわちＺ軸回りにもモーメントが発生することが判明した。

図６には、２足脚式移動ロボットの歩行速度［秒／歩］とヨー軸方向に発生するモーメントＮｍとの関係（例）を表している。同図からも判るように、脚式移動ロボットが１歩当りに要する時間が短くなる、すなわち歩行速度が増加するにつれて、ヨー軸モーメントは著しく増大していく。

このようなヨー軸モーメントは、やがて機体を旋回させるような作用を及ぼし、ロボットの足底と路面間にヨー軸回りの滑りが生じ、これが歩行の安定性に大きく影響するなど、期待された脚式作業を安定且つ正確に実現する上で障害となる。さらに、このヨー軸モーメントの影響が甚だしくなると、ロボットを転倒へと導き、機体や衝突物の損壊という事態も招きかねない。

また、脚式移動ロボットは、一般に、左右の上肢、下肢、体幹など複数の部位グループで構成され、さまざまな関節自由度を備えているが、歩行などの運動時にロール、ピッチ、ヨーの各軸回りに機体に印加されるモーメント・エラーを打ち消すために、これら複数の部位をどのように協調動作させればよいか、不明である。

また、ＺＭＰを安定度判別規範に用いたロボットの姿勢安定化制御は、足底接地点と路面の形成する支持多角形の内側にピッチ及びロール軸モーメントがゼロとなる点を探索するものであるが、ジャンプしたときや高台から飛び降りたときなど離床時においては、支持多角形そのものが存在しないため、支持多角形の内側にＺＭＰを制御するという従来の手法が適用できないという問題もある。

特開平５−３０５５７９号公報 特開平５−３０５５８１号公報 特開平５−３０５５８３号公報 特開平５−３０５５８５号公報 特開平５−３０５５８６号公報 Ｍｉｏｍｉｒ Ｖｕｋｏｂｒａｔｏｖｉｃ著"ＬＥＧＧＥＤ ＬＯＣＯＭＯＴＩＯＮ ＲＯＢＯＴＳ"（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社））

本発明の目的は、機体上の部位毎の目標軌道を修正して未知外力モーメント及び未知外力を補償することができる、優れたロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラムを提供することにある。

本発明のさらなる目的は、機体を構成する各部位グループを協調的に動作させることにより、歩行などの運動時にロール、ピッチ、ヨーの各軸回りに機体に印加されるモーメント・エラーをそれぞれ好適に打ち消すことができる、優れたロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラムを提供することにある。

本発明のさらなる目的は、ジャンプしたときや高台から飛び降りたときなど離床時においても、ロール、ピッチ、ヨーの各軸回りに機体に印加されるモーメント・エラーを好適に打ち消すことができる、優れたロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラムを提供することにある。

本発明は、上記課題を参酌してなされたものであり、その第１の側面は、少なくとも複数の可動部を備えたロボット装置の動作制御装置又は動作制御方法であって、
各部位毎の軌道からなる所望の動作データを基に、前記ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ釣合い方程式を生成するＺＭＰ釣合い方程式生成手段又はステップと、
前記ＺＭＰ釣合い方程式生成手段又はステップにより生成されたＺＭＰ釣合い方程式上で現れるモーメント・エラーを算出するモーメント・エラー算出手段又はステップと、
機体を複数の部位に分割し、該モーメント・エラーを打ち消すために目標（希望）軌道を修正する部位の優先順位を設定する優先順位設定手段又はステップと、
該設定された優先順位に従った順番で、各部位毎に目標（希望）軌道を修正してモーメント・エラーを補償する軌道修正手段又はステップと、
を具備することを特徴とするロボット装置の動作制御装置又は動作制御方法である。

［従来の技術］欄でも説明したように、とりわけ２足歩行型の脚式移動ロボットにおいては、歩行速度の増加とともに機体に加わるヨー軸モーメントが著しく増大していく。このようなヨー軸モーメントは、機体の旋回や路面に対するヨー軸回りの滑り、さらにはバランスを失うことに伴う機体の転倒など、安定且つ正確な脚式作業を実現する上で障害となる。

本発明の第１の側面に係るロボット装置の動作制御装置並びに動作制御方法によれば、下肢、体幹部、上肢の運動の組み合わせからなる機体の運動パターンを実行する際に設定ＺＭＰ上で機体に印加されるヨー軸モーメントなどのモーメント・エラーを、所定の優先順位に従った順番に従い各部位毎に目標（希望）軌道を修正することによって打ち消すことができる。

また、本発明によれば、機体を構成する各部位グループを協調的に動作させることにより、歩行などの運動時にロール、ピッチ、ヨーの各軸回りに機体に印加されるモーメント・エラーを好適に打ち消すことができる。例えば、下肢や体幹部による脚式作業を継続しつつ、上肢の運動によって歩行やその他の脚式作業時における安定性を維持することができる。

上肢の運動は、より具体的には、肩関節や肘関節の駆動を利用した運動である。勿論、左右の各上肢は一般的には逆位相の動きであるが、本発明を実現する上で特にこれに限定する必要はない。

一般的な直立２足歩行型のロボットの機体設計によれば、体幹部（例えば体幹ヨー軸）よりも肩関節や肘関節の方が広い可動角を持つので、このような上肢の運動により、機体のヨー軸モーメントを効率的且つ高精度に打ち消して、高度な姿勢安定性を実現することができる。

また、このような上肢運動は、ロボットの上半身の動作を表現力豊かに見せるといった演出効果がある。

ここで、前記優先順位設定手段又はステップは、質量操作量若しくはモーメント操作量の大きさの順に各部位に目標（希望）軌道修正のための優先順位を与えるようにしてもよい。

あるいは、前記優先順位設定手段又はステップは、あらかじめ立案されている行動計画を参照して各部位に希望軌道修正又は達成のための優先順位を与えるようにしてもよい。例えば、歩行を始めとする脚式動作を行なっているときには下肢の軌道を修正することはできないので、下肢により低い（希望軌道を修正する優先度の場合）若しくはより高い（希望軌道を修正しない優先度の場合）優先順位を与える。また、物をつかむ手は軌道を修正できないので、より低い若しくはより高い優先順位を与える。また、狭い通路を横向きで歩いている場合には、腰の軌道を修正すると壁に衝突する可能性が高いので、より低い若しくはより高い優先順位を与える。

また、前記ＺＭＰ釣合い方程式生成手段又はステップはピッチ、ロール、ヨーの各軸毎にＺＭＰ方程式を導出し、前記軌道修正手段又はステップは各部位の目標（希望）軌道をピッチ、ロール、ヨーの各軸毎に修正してモーメント・エラーを補償するようにしてもよい。

また、機体がジャンプしたときや高台から飛び降りたときなど離床時においては、支持多角形そのものが存在しないため、支持多角形の内側にＺＭＰを制御するという従来の手法が適用できないという問題もある。本発明によれば、離床時など支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合は、ＺＭＰ方程式に代えて、運動方程式を解く必要がある。このため、前記脚式移動ロボットの両脚離床時において、機体の並進及び／又は回転に関する重心運動を設定する重心運動設定手段又はステップをさらに備えてもよい。

このような場合、前記モーメント・エラー算出手段は、ＺＭＰ釣合い方程式上で現れるモーメント・エラーに加えて目標重心における力補償量を算出し、前記優先順位設定手段は、モーメント・エラー及び力補償量を打ち消すために目標（希望）軌道を修正する部位の優先順位を設定し、前記軌道修正手段は、該設定された優先順位に従った順番で、各部位毎に目標軌道を修正してモーメント・エラー及び力補償量を補償するようにすればよい。

また、本発明の第２の側面は、少なくとも複数の可動部を備えたロボット装置のための動作制御をコンピュータ・システム上で処理するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、
各部位毎の軌道からなる所望の動作データを基に、前記ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ釣合い方程式を生成するＺＭＰ釣合い方程式生成ステップと、
前記ＺＭＰ釣合い方程式生成ステップにより生成されたＺＭＰ釣合い方程式上で現れるモーメント・エラーを算出するモーメント・エラー算出ステップと、
機体を複数の部位に分割し、該モーメント・エラーを打ち消すために目標（希望）軌道を修正する部位の優先順位を設定する優先順位設定ステップと、
該設定された優先順位に従った順番で、各部位毎に目標（希望）軌道を修正してモーメント・エラーを補償する軌道修正ステップと、
を具備することを特徴とするコンピュータ・プログラムである。

本発明の第２の側面に係るコンピュータ・プログラムは、コンピュータ・システム上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第２の側面に係るコンピュータ・プログラムをコンピュータ・システムにインストールすることによって、コンピュータ・システム上では協働的作用が発揮され、本発明の第１の側面に係るロボット装置の動作制御装置又は動作制御方法と同様の作用効果を得ることができる。

本発明によれば、機体上の部位毎の目標軌道を修正して未知外力モーメント及び未知外力を補償することができる、優れたロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラムを提供することができる。

また、本発明によれば、機体を構成する各部位グループを協調的に動作させることにより、歩行などの運動時にロール、ピッチ、ヨーの各軸回りに機体に印加されるモーメント・エラーをそれぞれ好適に打ち消すことができる、優れたロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラムを提供することができる。

また、本発明によれば、ジャンプしたときや高台から飛び降りたときなど離床時においても、ロール、ピッチ、ヨーの各軸回りに機体に印加されるモーメント・エラーを好適に打ち消すことができる、優れたロボット装置の動作制御装置及び動作制御方法、並びにコンピュータ・プログラムを提供することができる。

本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。

以下、図面を参照しながら本発明の実施形態について詳解する。

図１には、本発明の実施に供される脚式移動ロボットの自由度構成を模式的に示している。

同図に示すロボットは、二脚二腕を有する人間型ロボットである。本ロボットは、機体に四肢が取り付けられ、肩関節ピッチ軸、肩関節ロール軸、上腕ヨー軸、肘関節ピッチ軸、前腕ヨー軸、手首ロール軸、手首ピッチ軸という７自由度からなる左右の腕部と、股関節ヨー軸、股関節ロール軸、股関節ピッチ軸、膝ピッチ軸、足首ピッチ軸、足首ロール軸という６自由度からなる左右の脚部で構成されている。

これらの各関節自由度は、実際にはアクチュエータ・モータにより実現される。本実施形態では、ギア直結型で且つサーボ制御系をワンチップ化してモータ・ユニットに内蔵したタイプの小型ＡＣサーボ・アクチュエータを搭載する。なお、 この種のＡＣサーボ・アクチュエータに関しては、例えば本出願人に既に譲渡されている特開２０００−２９９９７０号公報（特願平１１−３３３８６号明細書）に開示されている。

本実施形態に係る脚式移動ロボットは、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）を歩行の安定度判別の規範として用いている。ＺＭＰによる安定度判別規範は、系が適切なＺＭＰ空間を形成し、支持多角形の内側にＺＭＰがある場合は、系に回転運動や並進運動が発生せず、回転や並進に関する運動方程式を解く必要がない。但し、支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合は、ＺＭＰ方程式に代えて、運動方程式を解く必要がある（後述）。

図２には、脚式移動ロボット１００の制御システム構成を模式的に示している。同図に示すように、脚式移動ロボット１００は、ヒトの四肢を表現した各機構ユニット３０、４０、５０Ｒ／Ｌ、６０Ｒ／Ｌと、各機構ユニット間の協調動作を実現するための適応制御を行なう制御ユニット８０とで構成される（但し、Ｒ及びＬの各々は、右及び左の各々を示す接尾辞である。以下同様）。

脚式移動ロボット１００全体の動作は、制御ユニット８０によって統括的に制御される。制御ユニット８０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）やメモリなどの主要回路コンポーネント（図示しない）で構成される主制御部８１と、電源回路やロボット１００の各構成要素とのデータやコマンドの授受を行なうインターフェース（いずれも図示しない）などを含んだ周辺回路８２とで構成される。

本発明を実現する上で、この制御ユニット８０の設置場所は特に限定されない。図２では体幹部ユニット４０に搭載されているが、頭部ユニット３０に搭載してもよい。あるいは、脚式移動ロボット１００外に制御ユニット８０を配備して、脚式移動ロボット１００の機体とは有線若しくは無線で交信するようにしてもよい。

図１に示した脚式移動ロボット１００内の各関節自由度は、それぞれに対応するアクチュエータ・モータＭによって実現される。すなわち、頭部ユニット３０には、首関節ヨー軸、第１及び第２の首関節ピッチ軸、首関節ロール軸の各々を表現する首関節ヨー軸アクチュエータＭ₁、首関節ピッチ軸アクチュエータＭ_2A及びＭ_2B、首関節ロール軸アクチュエータＭ₃が配設されている。

また、体幹部ユニット４０には、体幹ピッチ軸、体幹ロール軸の各々を表現する体幹ピッチ軸アクチュエータＭ₁₁、体幹ロール軸アクチュエータＭ₁₂が配備されている。

また、腕部ユニット５０Ｒ／Ｌは、上腕ユニット５１Ｒ／Ｌと、肘関節ユニット５２Ｒ／Ｌと、前腕ユニット５３Ｒ／Ｌに細分化されるが、肩関節ピッチ軸、肩関節ロール軸、上腕ヨー軸、肘関節ピッチ軸、肘関節ヨー軸、手首ロール軸、手首関節ピッチ軸の各々を表現する肩関節ピッチ軸アクチュエータＭ₄、肩関節ロール軸アクチュエータＭ₅、上腕ヨー軸アクチュエータＭ₆、肘関節ピッチ軸アクチュエータＭ₇、肘関節ヨー軸アクチュエータＭ₈、手首関節ロール軸アクチュエータＭ₉、手首関節ピッチ軸アクチュエータＭ₁₀がそれぞれ配備されている。

また、脚部ユニット６０Ｒ／Ｌは、大腿部ユニット６１Ｒ／Ｌと、膝ユニット６２Ｒ／Ｌと、脛部ユニット６３Ｒ／Ｌに細分化されるが、股関節ヨー軸、股関節ピッチ軸、股関節ロール軸、膝関節ピッチ軸、足首関節ピッチ軸、足首関節ロール軸の各々を表現する股関節ヨー軸アクチュエータＭ₁₃、股関節ピッチ軸アクチュエータＭ₁₄、股関節ロール軸アクチュエータＭ₁₅、膝関節ピッチ軸アクチュエータＭ₁₆、足首関節ピッチ軸アクチュエータＭ₁₇、足首関節ロール軸アクチュエータＭ₁₈が配備されている。

各関節に用いられるアクチュエータＭ₁、Ｍ₂、Ｍ₃…は、より好ましくは、ギア直結型で且つサーボ制御系をワンチップ化してモータ・ユニット内に搭載したタイプの小型ＡＣサーボ・アクチュエータ（前述）で構成することができる。

頭部ユニット３０、体幹部ユニット４０、腕部ユニット５０、各脚部ユニット６０などの機構ユニット毎に、アクチュエータ駆動制御用の副制御部３５、４５、５５、６５が配備されている。

機体の体幹部４０には、加速度センサＡ１と、ジャイロ・センサなどからなる姿勢センサＧ１が配設されている。加速度センサＡ１は、例えばＸ、Ｙ、Ｚ各軸方向に配置する。機体の腰部に加速度センサＡ１を配設することによって、質量操作量が大きな部位である腰部を制御目標点として設定して、その位置における姿勢や加速度を直接計測して、ＺＭＰに基づく姿勢安定化制御を行なうことができる。

また、各脚部６０Ｒ、Ｌには、床反力センサＦ１〜Ｆ４、Ｆ５〜Ｆ８と、加速度センサＡ２及びＡ３、姿勢センサＧ２及びＧ３がそれぞれ配設されている。床反力センサＦ１〜Ｆ８は、例えば足底に圧力センサを装着することにより構成され、床反力の有無により足底が着床したか否かを検出することができる。また、加速度センサＡ２及びＡ３は、少なくともＸ及びＹの各軸方向に配置する。左右の足部に加速度センサＡ２及びＡ３を配設することにより、ＺＭＰ位置に最も近い足部で直接ＺＭＰ釣合い方程式を組み立てることができる。

質量操作量が大きな部位である腰部にのみ加速度センサを配置した場合、腰部のみが制御目標点に設定され、足部の状態は、この制御目標点の計算結果を基に相対的に算出しなければならず、足部と路面との間では以下の条件を満たすことが、前提となってしまう。

（１）路面はどんな力やトルクが作用しても動くことがない。
（２）路面での並進に対する摩擦係数は充分に大きく、滑りが生じない。

これに対し、本実施形態では、路面との接触部位である足部にＺＭＰと力を直接する反力センサ・システム（床反力センサなど）を配備するとともに、制御に用いるローカル座標とその座標を直接的に計測するための加速度センサを配設する。この結果、ＺＭＰ位置に最も近い足部で直接的にＺＭＰ釣合い方程式を組み立てることができる。したがって、より厳密な姿勢安定化制御を高速で実現することができる。この結果、力やトルクが作用すると路面が動いてしまう砂利上や毛足の長い絨毯上や、並進の摩擦係数が充分に確保できずに滑りが生じ易い住居のタイルなどであっても、機体の安定歩行（運動）を保証することができる。

主制御部８０は、各センサＡ１〜Ａ３、Ｇ１〜Ｇ３、Ｆ１〜Ｆ８の出力に応答して制御目標をダイナミックに補正することができる。より具体的には、副制御部３５、４５、５５、６５の各々に対して適応的な制御を行ない、脚式移動ロボット１００の上肢、体幹、及び下肢が協調して駆動する全身運動パターンを実現する。

ロボット１００の機体上での全身運動は、足部運動、ＺＭＰ軌道、体幹運動、上肢運動、腰部高さなどを設定するとともに、これらの設定内容に従った動作を指示するコマンドを各副制御部３５、４５、５５、６５に転送する。そして、各々の副制御部３５、４５…では、主制御部８１からの受信コマンドを解釈して、各アクチュエータＭ₁、Ｍ₂、Ｍ₃…に対して駆動制御信号を出力する。ここで言う「ＺＭＰ」とは、歩行中の床反力によるモーメントがゼロとなる床面上の点のことであり、また、「ＺＭＰ軌道」とは、例えばロボット１００の歩行動作期間中にＺＭＰが動く軌跡を意味する。

次いで、本実施例に係る脚式移動ロボット１００における、脚式作業時すなわち足部、腰、体幹、下肢運動などからなる全身協調運動パターンの実行時における姿勢の安定化処理の手順について説明する。

本実施形態に係る脚式移動ロボットは、ＺＭＰを歩行の安定度判別の規範として用いている。ＺＭＰによる安定度判別規範は、系が適切なＺＭＰ空間を形成し、支持多角形の内側にＺＭＰがある場合は、系に回転運動や並進運動が発生せず、回転や並進に関する運動方程式を解く必要がない。これに対し、支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合は、ＺＭＰ方程式に代えて、運動方程式を解く必要がある。例えば、ジャンプしたときや高台から飛び降りたときなど離床時には支持多角形が存在しないので、本実施形態では、ＺＭＰ方程式に代えて運動方程式を解くようにしている（後述）。

機体のＺＭＰ方程式は、目標ＺＭＰ上での各モーメントの釣合い関係を記述したものである。機体を多数の質点ｍ_iで表わし、これらを制御目標点とした場合、すべての制御目標点ｍ_iにおいて発生する目標ＺＭＰ上でのモーメントの総和がゼロとなる各制御点の軌道を求める式がＺＭＰ釣合い方程式である。

世界座標系（Ｏ−ＸＹＺ）で記述された機体のＺＭＰ釣合い方程式、並びに機体のローカル座標系（Ｏ−Ｘ’Ｙ’Ｚ’）で記述された機体のＺＭＰ釣合い方程式はそれぞれ以下の通りとなる。

上記の各式は、各質点（又は制御目標点）ｍ_iにおいて印加された加速度成分により生成されるＺＭＰ回り（半径ｒ_i−Ｐ_zmp）のモーメントの総和と、印加されたｊ番目の外力モーメントＭ_jの総和と、外力Ｆ_kにより生成されるＺＭＰ回り（ｋ番目の外力Ｆ_kの作用点をＳ_kとする）のモーメントの総和が釣り合うということを記述している。

このＺＭＰ釣合い方程式は、目標ＺＭＰにおける床反力モーメント（モーメント・エラー成分）Ｔを含んでいる。このモーメント・エラーをゼロ又は所定の許容範囲内に抑えることによって、機体の姿勢安定性が維持される。言い換えれば、モーメント・エラーをゼロ又は許容値以下となるように機体運動（足部運動や上半身の各部位の軌道など）を修正することが、ＺＭＰを安定度判別規範とした姿勢安定化制御の本質である。

本実施形態では、路面との接触部位である足部足底に加速度センサを設けていることから、世界座標系に対する実ロボットのローカル座標系を設定し、その原点としての足部足底を求め、ＺＭＰ釣合い方程式を直接的に導き出すことができる。さらに、腰部を始めとして質量操作量が大きな制御目標点にも加速度センサを配置することで、これら制御目標点毎のＺＭＰ回りのモーメント量を、加速度センサ出力値を用いて直接的に導き出すことができる。

ここで、制御対象としての実ロボットは移動体装置であることから、各制御目標点における世界座標上の位置ベクトルを求めることは難しい。その代案として、ローカル座標上における制御目標点の位置ベクトルは、逆キネマティクス演算など比較的容易な計算方法により求まる。そこで、機体のローカル座標系（Ｏ−Ｘ’Ｙ’Ｚ’）で記述された後者のＺＭＰ釣合い方程式を用いて実際の姿勢安定化処理を行なうようにすればよい。（ＺＭＰ軌道を正確に世界座標系で計測することがロボットに搭載された状態検出器では困難であることから、一般には、世界座標系に固定された外部計測器が必要となり、ロボット単体での行動（歩行など）が不可能となる。これに対し、接地部に加重センサを配置してＺＭＰ軌道を計測することにより、ローカル座標でＺＭＰ軌道を正確且つ直接的に得ることができる。且つ、ローカル座標原点付近に加速度センサを配置することにより、高速運動において支配的となる情報を直接的に計測することができる。このような理由により、本実施形態では、ローカル座標系でのＺＭＰ釣合い方程式を用いている。）

次いで、本実施形態に係る脚式移動ロボット１００における姿勢安定化処理、すなわち腰、体幹、下肢などの運動により発生するモーメントをキャンセルする全身協調運動パターンの生成処理の手順について説明する。本実施形態では、脚式移動ロボット１００の機体を複数の部位に分割し、各部位に軌道修正の優先順位を与え、部位毎の目標（希望）軌道の修正を行なうことによりモーメント・エラーをキャンセルすることに第１の特徴がある。

図３には、機体の全身運動パターンを安定化処理するための手順の一例をフローチャートの形式で示している。

まず、足部などの接地部位における運動の設定を行なう（ステップＳ１）。足部運動は、２以上の機体のポーズを時系列的に連結されてなるモーション・データである。モーション・データは、例えば、モーション編集装置などを利用してあらかじめ作成・編集されたデータであり、足部の各関節角の変位を表わした関節空間情報と、関節位置を表わしたデカルト空間情報で構成される。

次いで、設定された足部などの接地部位における運動を基にＺＭＰ安定領域を算出する（ステップＳ２）。あるいは、部位グループ毎に設定された優先順位付きの希望軌道（ステップＳ１４：後述）を基に、ＺＭＰ安定領域を算出する。なお、床運動や逆立ち運動など、ロボットが路面と接地する点が足底でない場合には、足底以外の接地点と路面とが形成する支持多角形に基づいてＺＭＰ安定領域を算出する。

ＺＭＰは、機体に印加されるモーメントがゼロとなる点であり（前述）、基本的には足底接地点と路面の形成する支持多角形の内側に存在する。ＺＭＰ安定領域は、この支持多角形のさらに内側に設定された領域であり、該領域にＺＭＰを収容させることによって機体を高度に安定した状態にすることができる。

そして、足部運動とＺＭＰ安定領域を基に、足部運動中におけるＺＭＰ軌道を設定する（ステップＳ３）。但し、床運動や逆立ち運動など、ロボットが路面と接地する点が足底でない場合には、足底以外の接地点と路面とが形成する支持多角形に基づいてＺＭＰ軌道を設定する。

また、機体の各部位については、腰部、体幹部、上肢、頭部などのようにグループ設定する（ステップＳ１１）。

図７には、機体の部位グループを設定した例を示している。同図に示す例では、腰部、体幹部、右腕部、頭部、左腕部、左脚部、右脚部の順に１〜７部の各部位グループが設定されている。各部位グループは、複数の質点で構成されている。質点の設定方法は、設計者のマニュアル入力であっても、所定の規則に従った自動生成のいずれであっても構わない（例えば、関節アクチュエータなど、質量操作量の大きな箇所が質点として設定される）。ｉ番目の部位の質点の数をｎ_iとすると、ｉ番目の部位グループ（ｉ部）の総質量ｍ_iは下式の通りとなる。

そして、各部位グループ毎に希望軌道を設定する（ステップＳ１２）。機体の各部位グループについての希望軌道の設定は、例えばモーション編集装置を利用してオフラインで行なわれる。

次いで、各部位のグループ設定の調整（再グルーピング）を行ない（ステップＳ１３）、さらにこれらグループに対して優先順位を与える（ステップＳ１４）。ここで言う優先順位とは、機体の姿勢安定制御のため処理演算に投入する順位のことであり、例えば質量操作量若しくはモーメント操作量に応じて割り振られる。この結果、機体の各部位についての優先順位付き希望軌道群が出来上がる。なお、このときに、各部位の接続条件（ローカル座標接続、特定部位の世界座標位置固定接続、特定部位の世界座標姿勢接続など）も設定する。また、優先順位は、時間ｔにおいて変化させるようにしてもよい。

また、各部位についての優先順位は、質量操作量若しくはモーメント操作量以外に、あらかじめ立案されている行動計画を参照して決定される。例えば、歩行を始めとする脚式動作を行なっているときには接地している足部の軌道を修正することはできないので、接地している足部により低い優先順位を与える。また、物をつかむ手は軌道を修正できないので、より低い優先順位を与える。また、狭い通路を横向きで歩いている場合には、腰の軌道を修正すると壁に衝突する可能性が高いので、より低い優先順位を与える。

また、機体の各部位グループ毎に、モーメント補償に利用できる質量を算出しておく（ステップＳ１５）。図７に示した例では、右脚が支持脚となり右足部にＺＭＰ位置Ｐ_ZMPが存在することから、各部位グループにおいてモーメント補償に利用できる質量Ｍ_iは以下の通りとなる。

１部すなわち腰部は、腰部自体の他、支持している体幹部、右腕部、頭部、左腕部をモーメント補償に利用することができる。また、２部すなわち体幹部は、体幹部自体の他、支持している右腕部、頭部、左腕部をモーメント補償に利用することができる。また、３〜６部すなわち右腕部、頭部、左腕部、並びに遊脚としての左脚部は、他に支持している部位グループがないことから、それぞれ自部位のみがモーメント補償に利用することができる質量となる。また、立脚としての７部すなわち右脚部は、すべての部位グループを支持していることから、機体のすべての部位の質量の合計Ｍ₇をモーメント補償に利用することができる。この場合の質量Ｍ₇の重心位置は、足部ではなく機体の重心位置に相当する（図８を参照のこと）。

そして、足部などの接地部位の運動と、ＺＭＰ軌道、並びに機体の各部位グループ毎の希望軌道群を基に、ステップＳ１４により設定された優先順位に従って、各部位グループの運動パターンを姿勢安定化処理に投入する。

この姿勢安定化処理では、まず、処理変数ｉに初期値１を代入する（ステップＳ２０）。そして、すべての部位グループに目標軌道を設定した場合における、目標ＺＭＰ上でのモーメント量すなわち総モーメント補償量Ωを算出する（ステップＳ２１）。総モーメント補償量Ωには、すべての部位グループの目標ＺＭＰ上でのモーメント量の総和に、上式で同定された未知外力モーメントと未知外力が既知項として加えられる。

さらに目標軌道が算出されていない部位については、希望軌道を用いる。

次いで、ステップＳ１５において算出された当該部位ｉのモーメント補償に利用できる質量Ｍ_iを用いて、その絶対モーメント補償量係数α_iを設定し（ステップＳ２２）、当該部位グループｉにおけるモーメント補償量Ω_iを算出する（ステップＳ２３）。

次いで、算出されたｉ番目の部位のモーメント補償量Ω_iを用いて、ｉ部に関するＺＭＰ方程式（Ｅ１）を導出する（ステップＳ２４）。

しかしながら、上記のＺＭＰ方程式は、未知変数が非常に多いため解析的にも数値計算的にも解を求めることは困難である。そこで、ここではｉ番目の部位のモーメント補償に利用できる質量の希望重心軌道ｒ_Miを既知変数に、ｉ番目の部位のモーメント補償に利用できる質量の希望重心軌道に対する変更量Δｒ_Miを未知変数とする以下の近似ＺＭＰ方程式（Ｅ２）を導出することにした。但し、ｒ_Miは、ｉ番目の部位グループにおけるモーメント補償に利用できる質量Ｍ_iの重心の位置ベクトルである。

そして、まず、この近似ＺＭＰ方程式を解くことで、ｉ番目の部位のモーメント補償に利用できる質量の希望重心軌道に対する変更量Δｒ_Miを算出し、ｉ番目の部位のモーメント補償に利用できる質量の目標重心軌道を下式により算出する。

なお、回転関節により各リンクが連結されている脚式移動ロボットの場合、上式（Ｅ２）は一般にＺ方向の運動を共有するため、非線形で干渉のある２階微分方程式となり、解析的に求めることは困難である。そこで、以下に示すようにＺ方向の運動を共有しないものとし、式（Ｅ２）の線形・非干渉化を行なう。

そして、ｉ番目の部位のモーメント補償に利用できる質量の目標重心軌道の近似解を算出し、この近似解から算出できるｉ番目の部位の各質点ｒ_iの変更量Δｒ_iを厳密ＺＭＰ方程式としての式（Ｅ１）に代入し、モーメント誤差を求め、この誤差の符号を反転したものを線形・非干渉化を行なった式（Ｅ２）の右辺に蓄積し、再び近似解を求めるという操作を、誤差が所定の許容値以下になるまで繰り返すことで当該部位のモーメント補償運動を算出するこができる（ステップＳ２５）。このようにして、優先順位が先頭からｉ番目までの部位についての目標軌道を得ることができる。

なお、ｉ番目の部位のモーメント補償に利用できる質量の目標重心軌道の近似解からｉ番目の部位の各質点の軌道を算出するには、ｉ番目の任意の部位に操作点を配置し、その並進位置（Ｘ，Ｙ，Ｚ）若しくは回転角度（θ_x，θ_y，θ_z）を未知変数とする重心ベクトルＣ（Ｘ，Ｙ，Ｚ，θ_x，θ_y，θ_z）を導出し、この重心ベクトルＣ（Ｘ，Ｙ，Ｚ，θ_x，θ_y，θ_z）を未知変数として左辺に、質量Ｍ_iの重心の位置ベクトルｒ_Miを既知変数として右辺に配置した方程式Ｃ（Ｘ，Ｙ，Ｚ，θ_x，θ_y，θ_z）＝ｒ_iMiを導出し、この方程式の解を数値探索法などを用いて算出したり、解析的に求めたりすることで、ｉ番目の部位の各質点の軌道を決定することができる。

このような処理をすべての部位グループについて行なうことにより、安定運動（例えば歩行）が可能な全身運動パターンが生成される。

図３に示した処理手順によれば、脚式移動ロボット１００の機体を複数の部位に分割し、各部位に軌道修正の優先順位を与え、部位毎の目標（希望）軌道の修正を行なうことにより、全身協調運動におけるモーメント・エラーをキャンセルすることができる。ここで、ＺＭＰを安定度判別規範とする姿勢安定化処理は、基本的には、足底などの接地点と路面の形成する支持多角形の内側にピッチ及びロール軸モーメントがゼロとなる点を探索することにあるが、他方、機体のヨー軸すなわちＺ軸回りに発生するモーメント・エラーに関しては明示的には対処していないという問題がある。

そこで、本実施形態の変形例として、歩行などの全身協調運動パターンにおいて、ロール、ピッチ、ヨーの各軸回りに機体に印加されるモーメント・エラーを好適に打ち消すようにした。図４には、機体のロール、ピッチ、ヨーの各軸回りのモーメント・エラーを打ち消して全身運動パターンを安定化処理するための手順をフローチャートの形式で示している。

まず、足部運動の設定を行なう（ステップＳ３１）。足部運動は、２以上の機体のポーズを時系列的に連結されてなるモーション・データである。モーション・データは、足部の各関節角の変位を表わした関節空間情報と、関節位置を表わしたデカルト空間情報で構成される（同上）。

次いで、足部などの設定された接地部位における運動を基にＺＭＰ安定領域を算出する（ステップＳ３２）。あるいは、部位グループ毎に設定された優先順位付きの希望軌道（ステップＳ４４：後述）を基に、ＺＭＰ安定領域を算出する。なお、床運動や逆立ち運動など、ロボットが路面と接地する点が足底でない場合には、足底以外の接地点と路面とが形成する支持多角形に基づいてＺＭＰ安定領域を算出する。

ＺＭＰは、機体に印加されるモーメントがゼロとなる点であり（前述）、基本的には足底接地点と路面の形成する支持多角形の内側に存在する。ＺＭＰ安定領域は、この支持多角形のさらに内側に設定された領域であり、該領域にＺＭＰを収容させることによって機体を高度に安定した状態にすることができる。

そして、足部運動とＺＭＰ安定領域を基に、足部運動中におけるＺＭＰ軌道を設定する（ステップＳ３３）。但し、床運動や逆立ち運動など、ロボットが路面と接地する点が足底でない場合には、足底以外の接地点と路面とが形成する支持多角形に基づいてＺＭＰ軌道を設定する。

また、機体の各部位については、腰部、体幹部、上肢、頭部などのようにグループ設定する（ステップＳ４１）。機体の部位グループの設定例は、図７に示した通りであり、腰部、体幹部、右腕部、頭部、左腕部、左脚部、右脚部の順に１〜７部の各部位グループが設定されている。

そして、各部位グループ毎に希望軌道を設定する（ステップＳ４２）。機体の各部位グループについての希望軌道の設定は、例えばモーション編集装置を利用してオフラインで行なわれる。

次いで、各部位のグループ設定の調整（再グルーピング）を行ない（ステップＳ４３）、さらにこれらグループに対して優先順位を与える（ステップＳ４４）。ここで言う優先順位とは、機体の姿勢安定制御のため処理演算に投入する順位のことであり、例えば質量操作量若しくはモーメント操作量に応じて割り振られる。この結果、機体の各部位についての優先順位付き希望軌道群が出来上がる。ここでは、優先順位ρは、ピッチ軸優先順位ρ_p、ロール軸優先順位ρ_r、並びにヨー軸優先順位ρ_yawの３要素で構成されるものとする。また、各部位についての優先順位は、質量操作量若しくはモーメント操作量以外に、あらかじめ立案されている行動計画を参照して決定される。なお、このときに、各部位の接続条件（ローカル座標接続、特定部位の世界座標位置固定接続、特定部位の世界座標姿勢接続など）も設定する。また、優先順位は、時間ｔにおいて変化させるようにしてもよい。

また、機体の各部位グループ毎に、モーメント補償に利用できる質量を算出しておく（ステップＳ４５）。図７に示した例では、右脚が支持脚となり右足部にＺＭＰ位置Ｐ_ZMPが存在することから、各部位グループにおいてモーメント補償に利用できる質量Ｍ_iは上述した通りとなる。

そして、足部などの接地部位の運動と、ＺＭＰ軌道、並びに各部位グループ毎の希望軌道群を基に、ステップＳ４４により設定された優先順位に従って、各部位グループの運動パターンを姿勢安定化処理に投入する。

この姿勢安定化処理では、まず、処理変数ｉに初期値１を代入する（ステップＳ５０）。そして、すべての部位グループに目標軌道を設定した場合における、目標ＺＭＰ上でのモーメント量すなわち総モーメント補償量Ωを算出する（ステップＳ５１）。総モーメント補償量Ωは、すべての部位グループの目標ＺＭＰ上でのモーメント量に、上式で同定された未知外力モーメントと未知外力が既知項として加えられる。

さらに、目標軌道が算出されていない部位については、希望軌道を用いる。

次いで、ステップＳ４５において算出された当該部位ｉのモーメント補償に利用できる質量Ｍ_iを用いて、その絶対モーメント補償量係数α_iを設定し（ステップＳ５２）、当該部位グループｉにおけるモーメント補償量Ω_iを下式により算出する（ステップＳ５３）。

次いで、算出されたｉ番目の部位のモーメント補償量Ω_iを用いて、ｉ部に関するＺＭＰ方程式（Ｅ１）を導出する（ステップＳ５４）。

しかしながら、上記のＺＭＰ方程式は、未知変数が非常に多いため解析的にも数値計算的にも解を求めることは困難である。そこで、ここではｉ番目の部位のモーメント補償に利用できる質量の希望重心軌道ｒ_Miを既知変数とするとともに、ｉ番目の部位のモーメント補償に利用できる質量の希望重心軌道に対する変更量Δｒ_Miを未知変数とする以下の近似ＺＭＰ方程式（Ｅ２）を導出することにした。但し、ｒ_Miは、ｉ番目の部位グループにおけるモーメント補償に利用できる質量Ｍ_iの重心の位置ベクトルである。

そして、まず、この近似ＺＭＰ方程式を解くことで、ｉ番目の部位のモーメント補償に利用できる質量の希望重心軌道に対する変更量Δｒ_Miを算出し、ｉ番目の部位のモーメント補償に利用できる質量の目標重心軌道を下式により算出する。

なお、回転関節により各リンクが連結されている脚式移動ロボットの場合、上式（Ｅ２）は一般にＺ方向の運動を共有するため、非線形で干渉のある２階微分方程式となり、解析的に求めることは困難である。そこで、以下に示すようにＺ方向の運動を共有しないものとし、式（Ｅ２）の線形・非干渉化を行なう。

そして、ｉ番目の部位のモーメント補償に利用できる質量の目標重心軌道の近似解を算出し、この近似解から算出できるｉ番目の部位の各質点ｒ_iの変更量Δｒ_iを厳密ＺＭＰ方程式としての式（Ｅ１）に代入し、モーメント誤差を求め、この誤差の符号を反転したものを線形・非干渉化を行なった式（Ｅ２）の右辺に蓄積し、再び近似解を求めるという操作を、誤差が所定の許容値以下になるまで繰り返すことで当該部位のモーメント補償運動を算出するこができる（ステップＳ５５）。このようにして、優先順位が先頭からｉ番目までの部位についての目標軌道を得ることができる。

なお、ｉ番目の部位のモーメント補償に利用できる質量の目標重心軌道の近似解からｉ番目の部位の各質点の軌道を算出するには、ｉ番目の任意の部位に操作点を配置し、その並進位置（Ｘ，Ｙ，Ｚ）若しくは回転角度（θ_x，θ_y，θ_z）を未知変数とする重心ベクトルＣ（Ｘ，Ｙ，Ｚ，θ_x，θ_y，θ_z）を導出し、この重心ベクトルＣ（Ｘ，Ｙ，Ｚ，θ_x，θ_y，θ_z）を未知変数として左辺に、質量Ｍiの重心の位置ベクトルｒ_Miを既知変数として右辺に配置した方程式Ｃ（Ｘ，Ｙ，Ｚ，θ_x，θ_y，θ_z）＝ｒ_iMiを導出し、この方程式の解を数値探索法などを用いて算出したり、解析的に求めたりすることで、ｉ番目の部位の各質点の軌道を決定することができる。

また、ｉ番目の部位の自由度構成によっては，優先順位ρ（ρ_p，ρ_r，ρ_yaw）を満たす運動が存在しない場合もある（例えば、ピッチ運動やロール運動に対する干渉が少ない、ヨー軸運動自由度がない場合など）。このような場合には、ステップＳ４４に戻り優先順位の再設定を行なう。

このような処理をすべての部位グループについて行なうことにより、安定運動（例えば歩行）が可能な全身運動パターンが生成される。

図４に示した処理手順によれば、脚式移動ロボット１００の機体を複数の部位に分割し、各部位に軌道修正の優先順位を与え、ロール、ピッチ、ヨーの各軸回りに機体に印加されるモーメント・エラーを好適に打ち消すように全身運動パターンを安定化処理することができる。ここで、ＺＭＰを安定度判別規範に用いたロボットの姿勢安定化制御は、足底接地点と路面の形成する支持多角形の内側にピッチ及びロール軸モーメントがゼロとなる点を探索するものであるが、ジャンプしたときや高台から飛び降りたときなど離床時においては、支持多角形そのものが存在しないため、支持多角形の内側にＺＭＰを制御するという従来の手法が適用できないという問題もある。

そこで、本実施形態の変形例として、離床時など支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合を想定ことにした。図５には、機体の全身運動パターンを安定化処理するための手順についてのさらに他の例をフローチャートの形式で示している。同図に示す処理手順では、離床時など支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合を想定しており、ＺＭＰ方程式に加えて、運動方程式を解くようになっている。

まず、足部運動の設定を行なう（ステップＳ６１）。足部運動は、２以上の機体のポーズを時系列的に連結されてなるモーション・データである。モーション・データは、足部の各関節角の変位を表わした関節空間情報と、関節位置を表わしたデカルト空間情報で構成される（同上）。

次いで、足部などの設定された接地部位における足部運動を基にＺＭＰ安定領域を算出する（ステップＳ６２）。あるいは、部位グループ毎に設定された優先順位付きの希望軌道（ステップＳ７４：後述）を基に、ＺＭＰ安定領域を算出する。なお、床運動や逆立ち運動など、ロボットが路面と接地する点が足底でない場合には、足底以外の接地点と路面とが形成する支持多角形に基づいてＺＭＰ安定領域を算出する。

ＺＭＰは、機体に印加されるモーメントがゼロとなる点であり（前述）、基本的には足底接地点と路面の形成する支持多角形の内側に存在する。ＺＭＰ安定領域は、この支持多角形のさらに内側に設定された領域であり、該領域にＺＭＰを収容させることによって機体を高度に安定した状態にすることができる。

そして、足部運動とＺＭＰ安定領域を基に、足部運動中におけるＺＭＰ軌道を設定する（ステップＳ６３）。但し、床運動や逆立ち運動など、ロボットが路面と接地する点が足底でない場合には、足底以外の接地点と路面とが形成する支持多角形に基づいてＺＭＰ軌道を設定する。

また、離床時（例えば、ジャンプしたときや高台から飛び降りたとき）など支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合を想定しており、ＺＭＰ方程式に加えて、運動方程式を解く必要があることから、重心運動（並進、回転）を設定する（ステップＳ９１）。
また、離床時（例えば、ジャンプしたときや高台から飛び降りたとき）など支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合を想定しており、ＺＭＰ方程式に加えて、運動方程式を解く必要があることから、重心運動（並進、回転）を設定する（ステップＳ９１）。

なお、ジャンプ後や高台からの飛び降りた後、着床瞬間時の機体の姿勢変化を押さえる場合には、ステップＳ９１の直後に、仮想ＺＭＰを算出し、着床時に形成される支持多角形内に仮想ＺＭＰが存在するよう着床部位に関する運動の希望軌道を設定すればよい。ここで言う「仮想ＺＭＰ」とは、実際には支持多角形を形成できないが、着床予定路面に非常に大きな支持多角形を形成しているという仮定の下で、ＺＭＰを算出した結果、求めることができるＺＭＰのことである。

また、機体の各部位については、腰部、体幹部、上肢、頭部などのようにグループ設定する（ステップＳ７１）。機体の部位グループの設定例は、図７に示した通りであり、腰部、体幹部、右腕部、頭部、左腕部、左脚部、右脚部の順に１〜７部の各部位グループが設定されている。

そして、各部位グループ毎に希望軌道を設定する（ステップＳ７２）。機体の各部位グループについての希望軌道の設定は、例えばモーション編集装置を利用してオフラインで行なわれる。

次いで、各部位のグループ設定の調整（再グルーピング）を行ない（ステップＳ７３）、さらにこれらグループに対して優先順位を与える（ステップＳ４４）。ここで言う優先順位とは、機体の姿勢安定制御のため処理演算に投入する順位のことであり、例えば質量操作量若しくはモーメント操作量に応じて割り振られる。この結果、機体の各部位についての優先順位付き希望軌道群が出来上がる。ここでは、優先順位ρは、ピッチ軸優先順位ρ_p、ロール軸優先順位ρ_r、並びにヨー軸優先順位ρ_yaw、Ｘ方向優先順位ρ_x、Ｙ方向優先順位ρ_y、Ｚ方向優先順位ρ_zの６要素で構成されるものとする。また、各部位についての優先順位は、質量操作量若しくはモーメント操作量以外に、あらかじめ立案されている行動計画を参照して決定される。なお、このときに、各部位の接続条件（ローカル座標接続、特定部位の世界座標位置固定接続、特定部位の世界座標姿勢接続など）も設定する。また、優先順位は、時間ｔにおいて変化させるようにしてもよい。

また、機体の各部位グループ毎に、モーメント補償及び力補償に利用できる質量を算出しておく（ステップＳ７５）。図７に示した例では、右脚が支持脚となり右足部にＺＭＰ位置Ｐ_ZMPが存在することから、各部位グループにおいてモーメント補償に利用できる質量Ｍ_iは上述した通りとなる。

そして、足部などの接地部位の運動と、ＺＭＰ軌道、並びに各部位グループ毎の希望軌道群を基に、ステップＳ７４により設定された優先順位に従って、各部位グループの運動パターンを姿勢安定化処理に投入する。

この姿勢安定化処理では、まず、処理変数ｉに初期値１を代入する（ステップＳ８０）。そして、そして、すべての部位グループに目標軌道を設定した場合における、目標ＺＭＰ上でのモーメント量すなわち総モーメント補償量Ω及び力補償量Γを算出する（ステップＳ８１）。総モーメント補償量Ω及び力補償量Γは、すべての部位グループの目標ＺＭＰ上でのモーメント量並びに力補償量の総和に、上式で同定された未知外力モーメントと未知外力が既知項として加えられる。

さらに、目標軌道が算出されていない部位については、希望軌道を用いる。

次いで、ステップＳ７５において算出された当該部位ｉのモーメント補償及び力補償に利用できる質量Ｍ_iを用いて、その絶対モーメント補償量係数α_i及び絶対力補償量係数χ_iを設定し（ステップＳ８２）、モーメント補償量Ω_i、力補償量Γ_iを下式により算出する（ステップＳ８３）。

次いで、算出されたいｉ番目の部位のモーメント補償量Ω_iと力補償量Γ_iを用いて、ｉ部に関するＺＭＰ方程式（Ｅ１）又は重心における回転に関する運動方程式（Ｅ１'）、そして並進に関する運動方程式（Ｅ３）を導出する（ステップＳ８４）。

しかしながら、これらの式は未知変数が非常に多いため解析的にも数値計算的にも解を求めることは困難である。そこで、ここではｉ番目の部位のモーメント補償及び力補償に利用できる質量の希望重心軌道ｒ_Mi、希望重心加速度μ_Miを既知変数とするとともに、ｉ番目の部位のモーメント補償に利用できる質量の希望重心軌道に対する変更量Δｒ_Mi並びに変更加速度量（Δμ_Miの時間による２階微分）を未知変数とする近似ＺＭＰ方程式（Ｅ２）又は重心における回転に関する近似運動方程式（Ｅ２'）、そして並進に関する近似運動方程式（Ｅ４）を導出する。但し、ｒ_Miは、ｉ番目の部位グループにおけるモーメント補償に利用できる質量Ｍ_iの重心の位置ベクトルである。

次に、導出された方程式を同時に解かなければならないが、回転関節により各リンクが連結されている脚式移動ロボットの場合、上記の各式はお互いにＺ方向の運動を共有するため、非線形で干渉のある２階微分方程式となり、解析的に求めることは困難である。

そこで、まずＺ方向の運動を共有しないものとし、上式（Ｅ４）の線形・非干渉化を行ない、ｉ番目の部位の力補償に利用できる質量の目標重心軌道の近似解ｒ_iMiを算出し、この近似解から算出できるｉ番目の部位の各質点ｒ_iの変更量Δｒ_iを厳密運動方程式（Ｅ３）に代入し、力補償誤差を算出し、この誤差の符号を反転したものを線形・非干渉化を行なった式（Ｅ４）の右辺に蓄積する。

次に、Δｒ_Mi（未知変数）をｒ_iMi（算出された既知変数）−ｒ_Mi（既知変数）＋Δｒ_Mi（未知変数）に置き換え、先ほど算出された目標重心軌道からｉ番目の部位のモーメント補償に利用できる質量の目標重心軌道への変更量Δｒ_Miの近似解の算出を行なう。具体的には、Ｚ方向の運動を共有しないものとし、上式（Ｅ２）若しくは（Ｅ２'）の線形・非干渉化を行ない、ｉ番目の部位のモーメント補償に利用できる質量の目標重心軌道の近似解を算出する。そして、この近似解から算出できるｉ番目の部位の各質点ｒ_iの変更量Δｒ_iを厳密運動方程式（Ｅ１）若しくは（Ｅ１'）に代入し、モーメント補償誤差を算出し、この誤差の符号を反転したものを線形・非干渉化を行なった式（Ｅ２）若しくは式（Ｅ２'）の右辺に蓄積する。

そして、再びｉ番目の部位の力補償に利用できる質量の目標重心軌道の近似解を算出し、この近似解から算出できるｉ番目の部位の各質点ｒ_iの変更量Δｒ_iを厳密運動方程式（Ｅ３）に代入し、力補償誤差を算出し、この誤差の符号を反転したものを線形・非干渉化を行なった式（Ｅ４）の右辺に再び蓄積する。

次に、Δｒ_Mi（未知変数）をｒ_iMi（算出された既知変数）−ｒ_Mi（既知変数）＋Δｒ_Mi（未知変数）に置き換え、ｉ番目の部位のモーメント補償に利用できる質量の目標重心軌道の近似解を算出し、この近似解から算出できるｉ番目の部位の各質点ｒ_iの変更量Δｒ_iを厳密運動方程式（Ｅ１）若しくは（Ｅ１'）に代入する。そして、モーメント補償誤差を算出し、この誤差の符号を反転したものを線形・非干渉化を行なった式（Ｅ２）若しくは式（Ｅ２'）の右辺に再び蓄積する。

以上の操作を誤差が許容値以下になるまで繰り返すことで、当該部位の力に加えモーメントの補償運動を算出するこができる（ステップＳ８５）。そして、優先順位が先頭からｉ番目までの部位についての目標軌道を得ることができる。

一般には、ＺＭＰ方程式と運動方程式のどちらかを満たす運動を算出すればよい。しかしながら、例えば跳躍や走行時の離床時や着地時などは、上式（Ｅ１）と式（Ｅ３）を同時に満たさなければならない場合がある。

なお、ｉ番目の部位のモーメント補償又は力補償に利用できる質量の目標重心軌道の近似解からｉ番目の部位の各質点の軌道算出には、ｉ番目の任意の部位に操作点を配置しその並進位置（Ｘ，Ｙ，Ｚ）若しくは回転角度（θ_x，θ_y，θ_z）を未知変数とする重心ベクトルＣ（Ｘ，Ｙ，Ｚ，θ_x，θ_y，θ_z）を導出し、この重心ベクトルＣ（Ｘ，Ｙ，Ｚ，θ_x，θ_y，θ_z）を未知変数として左辺に、ｒ_iMiを既知変数として右辺に配置した方程式Ｃ（Ｘ，Ｙ，Ｚ，θ_x，θ_y，θ_z）＝ｒ_iMiを導出し、この方程式の解を算出することで、ｉ番目の部位の各質点の軌道決定することができる。

また、ｉ番目の部位の自由度構成によっては、優先順位ρ（ρ_p，ρ_r，ρ_yaw，ρ_x，ρ_y，ρ_z）を満たす運動が存在しない場合もある（例えば、ＸＹの各方向運動に対する干渉が少ないピッチ・ロール軸運動自由度がない場合など）。このような場合には、ステップＳ６１、Ｓ７２、若しくはＳ７４に戻り、設定項目の再設定を行なう必要がある。

また、ｉ番目の部位の姿勢によっては、優先順位ρ（ρ_p，ρ_r，ρ_yaw，ρ_x，ρ_y，ρ_z）の各要素の繰り返し計算における誤差の収束速度に大きさ差が生じ、収束速度の遅い要素の方程式の厳密解が求まらず、収束速度の速い要素の方程式のみ先に厳密解が求まってしまう場合がある。そこで、このような場合には、厳密方程式において算出する。各モーメント補償量誤差若しくは力補償量誤差に収束速度調整係数μ（μ_p，μ_r，μ_yaw，μ_x，μ_y，μ_z）を各誤差に掛けた後に、各近似方程式の右辺に蓄積させ、収束速度の均一化を行なう必要がある。

このような処理をすべての部位グループについて行なうことにより、安定運動（例えば歩行）が可能な全身運動パターンが生成される。

［追補］
以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。

本発明の要旨は、必ずしも「ロボット」と称される製品には限定されない。すなわち、電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行なう機械装置あるいはその他一般的な移動体装置であるならば、例えば玩具などのような他の産業分野に属する製品であっても、同様に本発明を適用することができる。

要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、冒頭に記載した特許請求の範囲の欄を参酌すべきである。

図１は、本発明の実施に供される脚式移動ロボットの自由度構成を模式的に示した図である。 図２は、脚式移動ロボット１００の制御システム構成を模式的に示した図である。 図３は、機体の全身運動パターンを安定化処理するための手順の一例を示したフローチャートである。 図４は、機体の全身運動パターンを安定化処理するための手順についての他の例を示したフローチャートである。 図５は、機体の全身運動パターンを安定化処理するための手順についてのさらに他の例を示したフローチャートである。 図６は、２足脚式移動ロボットの歩行速度［秒／歩］とヨー軸方向に発生するモーメントＮｍとの関係（例）を表した図である。 図７は、機体の部位グループを設定した例を模式的に示した図である。 図８は、質量Ｍ₇の重心位置を示した図である。

符号の説明

３０…頭部ユニット
４０…体幹部ユニット
５０…腕部ユニット
５１…上腕ユニット
５２…肘関節ユニット
５３…前腕ユニット
６０…脚部ユニット
６１…大腿部ユニット
６２…膝関節ユニット
６３…脛部ユニット
８０…制御ユニット
８１…主制御部
８２…周辺回路
１００…脚式移動ロボット

Claims (11)

1. 少なくとも複数の可動部を備えたロボット装置の動作制御装置であって、
   各部位毎の軌道からなる所望の動作データを基に、前記ロボット装置の機体に印加される各モーメントの釣合い関係を記述したＺＭＰ釣合い方程式を生成するＺＭＰ釣合い方程式生成手段と、
   前記ＺＭＰ釣合い方程式生成手段により生成されたＺＭＰ釣合い方程式上で現れるモーメント・エラーを算出するモーメント・エラー算出手段と、
   機体を複数の部位に分割し、該モーメント・エラーを打ち消すために目標軌道を修正する部位の優先順位を設定する優先順位設定手段と、
   該設定された優先順位に従った順番で、各部位毎に目標軌道を修正してモーメント・エラーを補償する軌道修正手段と、
   を具備することを特徴とするロボット装置の動作制御装置。
2. 前記優先順位設定手段は、質量操作量若しくはモーメント操作量の大きさの順に各部位に目標軌道修正のための優先順位を与える、
   ことを特徴とする請求項１に記載のロボット装置の動作制御装置。
3. 前記優先順位設定手段は、あらかじめ立案されている行動計画を参照して各部位に目標軌道修正のための優先順位を与える、
   ことを特徴とする請求項１に記載のロボット装置の動作制御装置。
4. 前記ＺＭＰ釣合い方程式生成手段は、ピッチ、ロール、ヨーの各軸毎にＺＭＰ方程式を導出し、
   前記軌道修正手段は、各部位の目標軌道をピッチ、ロール、ヨーの各軸毎に修正してモーメント・エラーを補償する、
   ことを特徴とする請求項１に記載のロボット装置の動作制御装置。
5. 前記ロボット装置の両脚離床時において、機体の並進及び／又は回転に関する重心運動を設定する重心運動設定手段をさらに備え、
   前記モーメント・エラー算出手段は、ＺＭＰ釣合い方程式上で現れるモーメント・エラーに加えて目標重心における力補償量を算出し、
   前記優先順位設定手段は、モーメント・エラー及び力補償量を打ち消すために目標軌道を修正する部位の優先順位を設定し、
   前記軌道修正手段は、該設定された優先順位に従った順番で、各部位毎に目標軌道を修正してモーメント・エラー及び力補償量を補償する、
   ことを特徴とする請求項１に記載のロボット装置の動作制御装置。
6. 少なくとも複数本の可動部を備えたロボット装置の動作制御方法であって、
   各部位毎の軌道からなる所望の動作データを基に、前記ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ釣合い方程式を生成するＺＭＰ釣合い方程式生成ステップと、
   前記ＺＭＰ釣合い方程式生成ステップにより生成されたＺＭＰ釣合い方程式上で現れるモーメント・エラーを算出するモーメント・エラー算出ステップと、
   機体を複数の部位に分割し、該モーメント・エラーを打ち消すために目標軌道を修正する部位の優先順位を設定する優先順位設定ステップと、
   該設定された優先順位に従った順番で、各部位毎に目標軌道を修正してモーメント・エラーを補償する軌道修正ステップと、
   を具備することを特徴とするロボット装置の動作制御方法。
7. 前記優先順位設定ステップでは、質量操作量若しくはモーメント操作量の大きさの順に各部位に目標軌道修正のための優先順位を与える、
   ことを特徴とする請求項６に記載のロボット装置の動作制御方法。
8. 前記優先順位設定ステップでは、あらかじめ立案されている行動計画を参照して各部位に目標軌道修正のための優先順位を与える、
   ことを特徴とする請求項６に記載のロボット装置の動作制御方法。
9. 前記ＺＭＰ釣合い方程式生成ステップでは、ピッチ、ロール、ヨーの各軸毎にＺＭＰ方程式を導出し、
   前記軌道修正ステップでは、各部位の目標軌道をピッチ、ロール、ヨーの各軸毎に修正してモーメント・エラーを補償する、
   ことを特徴とする請求項６に記載のロボット装置の動作制御方法。
10. 前記ロボット装置の両脚離床時において、機体の並進及び／又は回転に関する重心運動を設定する重心運動設定ステップをさらに備え、
    前記モーメント・エラー算出ステップでは、ＺＭＰ釣合い方程式上で現れるモーメント・エラーに加えて目標重心における力補償量を算出し、
    前記優先順位設定ステップでは、モーメント・エラー及び力補償量を打ち消すために目標軌道を修正する部位の優先順位を設定し、
    前記軌道修正ステップでは、該設定された優先順位に従った順番で、各部位毎に目標軌道を修正してモーメント・エラー及び力補償量を補償する、
    ことを特徴とする請求項６に記載のロボット装置の動作制御方法。
11. 少なくとも複数本の可動部を備えたロボット装置のための動作制御をコンピュータ・システム上で処理するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、
    各部位毎の軌道からなる所望の動作データを基に、前記ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ釣合い方程式を生成するＺＭＰ釣合い方程式生成ステップと、
    前記ＺＭＰ釣合い方程式生成ステップにより生成されたＺＭＰ釣合い方程式上で現れるモーメント・エラーを算出するモーメント・エラー算出ステップと、
    機体を複数の部位に分割し、該モーメント・エラーを打ち消すために目標軌道を修正する部位の優先順位を設定する優先順位設定ステップと、
    該設定された優先順位に従った順番で、各部位毎に目標軌道を修正してモーメント・エラーを補償する軌道修正ステップと、
    を具備することを特徴とするコンピュータ・プログラム。
    

Images