JP2004142095A - ロボット装置及びロボット装置の制御方法 - Google Patents

Abstract

【課題】　ロボットの歩行パターン生成演算を、運動状態量に関する境界条件、外力、外力モーメント、足底軌道などのパラメータが設定可能な形式で、実機上で実時間により実行可能にする。
【解決手段】　センサなどから計測されたロボットの運動状態量、外力及び外力モーメント、環境形状などの実世界の物量を入力するように歩行制御系を構成する。機体のバランス維持に係るすべての計算を単一の歩行パターン演算に集約させることにより、歩容生成機能と適応制御機能を効率的に両立させるとともに、力学モデルの一貫性を保証し、各力学モデル間の干渉を取り除く。
【選択図】　図１

Description

　本発明は、少なくとも複数本の可動脚を備えたロボット装置及びロボット装置の制御方法に係り、特に、所定の力学モデルを用いて歩容生成並びに適応制御を行なうロボット装置及びロボット装置の制御方法に関する。

　さらに詳しくは、本発明は、歩行パターンを始めとする運動パターン生成を実機上で実時間により行なうロボット装置及びロボット装置の制御方法に係り、特に、歩行パターン生成と各種の適応制御のための力学モデルの一貫性を保証して良好な歩行制御を行なうロボット装置及びロボット装置の制御方法に関する。

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

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

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

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

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

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

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

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

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

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

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

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

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

　ところで、ＺＭＰ方程式の数値解法などを含む運動パターン生成の処理負荷は高い。このため、従来の一般的な歩行制御方式の多くは、基準歩行パターン生成演算をオフラインで実行するとともに、未知路面・未知外力に適応するための安定化演算は各適応制御機能毎に比較的計算負荷の軽い簡略力学モデルを構成して実機上で実行するようになっている。

　このような場合、基準歩行パターンが固定的なデータとして与えられるため，多様な歩容変化要求が受け付けられないという問題がある。歩行ロボットが実環境中を自由に移動可能とするためには、任意のタイミングで入力される歩幅・歩行周期・旋回角・足上げ高さなどの下肢の運動変化要求や、上肢の運動変化要求を満足するよう、力学的バランス平衡を保ったまま即応的に全身運動を再計画する機能が必要である。ところが、オフラインで生成歩行パターンを生成する従来方式では、あらかじめ用意しておいた歩行パターンを実環境上でそのまま実行することしかできない。

　例えば、オフラインで一歩分の歩行パターンを幾つか用意しておき、それらを混合・接続する方法も提案されている（例えば、特許文献６を参照のこと）。しかしながら、殆ど無限に存在するといってよい歩行パターンのすべてをカバーすることは困難である。

　また、基準歩行パターン生成時に仮定したのと異なる運動が特定関節（例えば人間型ロボットの上肢）に対して実行時に要求された場合、この特定関節の運動は外乱として扱われるため、安定化アルゴリズムへの負担が大きくなる、という問題がある。特定関節への要求運動と基準歩行パターン生成時に仮定した運動の偏差が過大な場合には、安定化アルゴリズムの許容範囲を超え、転倒に至るケースもある。例えば、上肢の質量が大きい場合、歩行中の任意のタイミングで激しく上肢を振るなどの運動要求に対応することは難しいと思料される。

　また、適応制御機能毎の簡略化された力学モデルを構成しているため、適応制御機能の追加の拡張性に乏しく、演算量が嵩むという問題がある。各適応制御機能毎の安定化アルゴリズムは、歩行の安定化という共通の目的を持ち、本質的に類似した計算を行なっていることが多い。計算リソースの限られた組込みＣＰＵ上では、これらの演算を集約し、演算の節約を図っていくことが重要となる。

　また、実機上での安定化演算の計算負荷を軽減するために各適応制御毎に簡略化した力学モデルを区々に導入することが多いが、言い換えれば、歩行パターン生成及び各種安定化アルゴリズムの中で用いている力学モデルが一貫していない。このため、異モデル間の不用意な干渉を生じることがある。このような干渉は制御系において発振・発散という現象を招来し、歩行性能の低下を招く可能性が否めない。歩行パターン生成を含め、各種安定化アルゴリズムは各々独立なダイナミクスによって支配されている訳ではなく、一般に相互干渉を有するので、相互干渉性を考慮した一貫性のある力学モデルを共有し、動作することが望ましい。

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

　本発明の目的は、歩行パターンを始めとする運動パターン生成を実機上で実時間により好適に行なうことができる、優れたロボット装置及びロボット装置の制御方法を提供することにある。

　本発明のさらなる目的は、歩行パターン生成と各種の適応制御のための力学モデルの一貫性を保証して良好な歩行制御を行なうことができる、優れたロボット装置及びロボット装置の制御方法を提供することにある。

　本発明のさらなる目的は、計算リソースの限られた組込みＣＰＵ上で機体の安定化制御を好適に行なうことができる、優れたロボット装置及びロボット装置の制御方法を提供することにある。

　本発明のさらなる目的は、各適応制御機能間で相互干渉性を考慮した一貫性のある力学モデルを共有して歩行性能を向上することができる、優れたロボット装置及びロボット装置の制御方法を提供することにある。

　本発明は、上記課題を参酌してなされたものであり、移動手段を有するロボット装置であって、
　前記ロボット装置の状態量を検出する状態検出手段と、
　前記ロボット装置の状態量に関する境界条件付き運動方程式の解を求める実時間運動方程式求解手段と、
　前記状態検出手段により検出された現在の状態量を、前記境界条件付き運動方程式の境界条件として設定する境界条件設定手段と、
　前記設定された境界条件の下で、前記実時間運動方程式求解手段が算出した運動方程式の解に基づいて、前記ロボット装置の次時刻の目標状態量を算出する目標状態量算出手段と、
　前記目標状態量に基づいて、前記ロボット装置の運動パターンを生成する運動パターン生成手段と、
を具備することを特徴とするロボット装置である。

　本発明に係るロボット装置によれば、ユーザ・プログラムからの運動変更要求を、力学的バランス平衡を維持したまま即応的に満足する実時間全身運動再計画の機能を備えているので、現在のロボットの実状態、実環境形状、外力など実世界情報を反映することができる。

　すなわち、未知環境適応に関する安定化のための演算を別途行なうことなく、ロボットの実状態、実環境形状、外力を実時間全身運動再計画演算の境界値や拘束条件として入力することで、一回の実時間全身運動再計画演算内で歩行パターンなどの運動パターン生成演算と安定化演算の演算共有化を実現することができる。

　これにより、ロボットの歩行安定化制御に必要となる演算量を節約することができるので、計算リソースの限られた組込みＣＰＵ上でも、柔軟な運動要求反映機能とロバスト性を兼ね備えた高機能全身運動制御系を構築することが可能となる。

　一般に、歩行パターン生成や種々の安定化で用いられる力学モデルが相異なると、力学モデル間の不用意な干渉を生じて歩行性能の低下につながる可能性が否定できない。これに対し、本発明に係る移動ロボットの制御方式では、力学演算は単一の歩行パターン生成演算内で一貫して行なわれるため、このような種々のアルゴリズム間の不用意な干渉が生じ難く、良好な歩行性能を得ることができる。

　ロボットの運動方程式は、単純な式で表現されるが、変係数の微分方程式であり、解析解を求めることは容易でない。そこで、比較的短期間の時間間隔を考えて、所定の仮定の下で各短期区間毎に簡潔な常微分方程式に近似して、これら短期区間が複数連なっているとモデル化して、演算処理を簡素化する。扱いが容易な短期区間の取り方として、片脚支持期と両脚支持期の境界で区間を区切る例を挙げることができる。

　ここで、前記現在の状態量は現在の制御サイクルにおける前記ロボット装置の状態量のことであり、次時刻の目標状態量は次の制御サイクルにおける前記ロボット装置の目標状態量のことである。

　また、前記状態検出手段は、前記ロボット装置の状態量として、上記ロボットの重心位置又は重心位置のＮ階微分値を用いるようにしてもよい。

　あるいは、前記状態検出手段は、前記ロボット装置の状態量として、ロボットの重心に近接した部位の位置、又はそのＮ階微分値のいずれかを用いるようにしてもよい。

　例えばロボット装置の状態量として、機体の重心位置ｘ、重心速度ｄｘ／ｄｔからなる２次元の状態ベクトル［ｘ，ｄｘ／ｄｔ］^Tを用いることができる。あるいは、２足直立歩行のロボットの場合、腰部などの重心近傍の特定部位の位置ｐ、速度ｄｐ／ｄｔからなる２次元の状態ベクトル［ｐ，ｄｐ／ｄｔ］^T、全身の質点位置ｐ_i、速度ｄｐ_i ／ｄｔ（ｉは質点のインデックスで、ｉ＝１…Ｎ）からなる２Ｎ次元ベクトル［ｐ₁，ｐ₂，…，ｐ_N，ｄｐ₁／ｄｔ，ｄｐ₂／ｄｔ，…，ｄｐ_N／ｄｔ］^Tなどを状態量として用いても良い。

　また、前記実時間運動方程式求解手段は、ロボットのＺＭＰ方程式を運動方程式として用いるようにしてもよい。

　ここで、前記移動手段は、例えば可動部とリンクとからなる脚で構成される。このような場合、前記脚の先端リンクの位置又は姿勢に関する軌道を制御周期毎に計算する実時間軌道生成手段と、前記ロボット装置の周辺環境の状況を計測する環境状況計測手段とをさらに備えていてもよい。そして、前記実時間軌道生成手段は、前記環境状況計測手段によって計測された実環境状況に基づいて、前記先端リンクの位置と姿勢を算出し、前記運動パターン生成手段は、前記先端リンクの位置と姿勢、及び前記目標状態量に基づいて前記ロボット装置の運動パターンを生成することができる。

　また、本発明に係るロボット装置は、実際のロボットに作用する外力又は外モーメントを計測する外力検出手段をさらに備えていてもよい。このような場合、前記実時間運動方程式求解手段は、前記外力検出手段が算出した外力又はモーメントを加味して、前記運動方程式の解を算出することができる。

　本発明によれば、歩行パターン生成を実機上で実時間により好適に行なうことができる、優れたロボット装置及びロボット装置の制御方法を提供することができる。

　また、本発明によれば、歩行パターン生成と各種適応制御のための力学モデルの一貫性を保証して良好な歩行制御を行なうことができる、優れたロボット装置及びロボット装置の制御方法を提供することができる。

　また、本発明によれば、計算リソースの限られた組込みＣＰＵ上で機体の安定化制御を好適に行なうことができる、優れたロボット装置及びロボット装置の制御方法を提供することができる。

　また、本発明によれば、各適応制御機能間で相互干渉性を考慮した一貫性のある力学モデルを共有して歩行性能を向上することができる、優れたロボット装置及びロボット装置の制御方法を提供することができる。

　また、本発明によれば、センサなどから非同期に入力される歩幅・歩行周期・旋回角・足上げ高さなどの歩容パラメータを満足するよう、機体の力学的バランス平衡を保ったまま即応的に全身運動を再計画する「実時間歩行パターン生成」の機能により、任意のタイミングで連続的歩容変化を実行することが可能であり、柔軟で且つ多様な歩行性能を実現することができる。

　また、本発明によれば、実時間歩行パターン生成機能により、任意のタイミングで特定部位（例えば上肢）の運動を組み合わせ実行できるように全身運動が再計画される。したがって、安定化制御系に大きな負担をかけることなく、非同期且つリアクティブに特定部位を駆動することが可能となるとともに、動作パターンの多様化を実現することができる。

　また、本発明によれば、安定化アルゴリズムのための演算が一回の実時間歩行パターン生成アルゴリズム内の演算に集約・統合されている。したがって、高い計算効率で実時間歩行パターン生成機能と安定化機能の両立を実現することができ、計算リソースの限られた組込みＣＰＵ上での実行に有利で、ＣＰＵリソースの節約を図ることができる。

　また、本発明によれば、安定化機能の中で用いる力学演算が実時間歩行パターン生成演算に集約され、基本的に実時間歩行パターン生成部の設定項目への追加入力を行なうのみで安定化機能が追加されることから、演算の節約とともに拡張性に優れた歩行系を構成することができる。

　また、本発明によれば、安定化機能の中で用いる力学演算が各種安定化の干渉効果が考慮された一回の実時間歩行パターン生成演算に集約されることから、独立な力学モデル間の不用意な干渉がなく、力学モデルの一貫性が図られ、良好な歩行制御機能を実現することができる。

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

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

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

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

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

　機体には、加速度センサＡ１及びジャイロＧ１が搭載されている。また、左右の足底四隅には、足底面垂直方向の床反力を検出する１軸ロードセル（Ｆ１〜Ｆ８）と、床面までの距離を測定する赤外線測距センサ（Ｄ１〜Ｄ８）がそれぞれ４つ取り付けられている。また、左右の足底中央部には、それぞれ加速度センサ（Ａ２，Ａ３）及びジャイロ（Ｇ２，Ｇ３）が取り付けられている。

　図２には、本実施形態に係る脚式移動ロボットに適用される歩行制御システムの機能構成を模式的に示している。

　図示の通り、この歩行制御システムは、ロボット状態観測部２１と、環境形状観測部２２と、外力・外モーメント観測部２３と、実時間歩行パターン生成部２５と、上肢・下肢要求運動入力部２４と、全身関節駆動部２６により構成される。

　ロボット状態観測部２１は、機体の各部に装備された種々のセンサ（前述）からの情報を基に、実際のロボットの運動状態量を計測し、その結果を実時間歩行パターン生成部２５へと出力する。

　環境形状観測部２２は、種々のセンサ（前述）からの情報を基に、路面傾斜・段差高さなどの環境形状を算出し、その結果を実時間歩行パターン生成部２５へと出力する。

　外力・外モーメント観測部２３は、主にロボット上に搭載された力センサ並びに加速度センサ情報を基に、ロボットに対して外界から作用する外力・外モーメントを算出し、その結果を実時間歩行パターン生成部２５へ反映する。

　上肢・下肢要求運動入力部２４は、ユーザ・プログラムに従って時々刻々と決定される上肢への運動要求と、歩幅・歩行周期・旋回角などの下肢への要求運動に関る歩容パラメータを入力し、実時間歩行パターン生成部２５へと出力する。

　実時間歩行パターン生成部２５は、外力・外モーメント観測部２３において検出された外力下にて力学的なバランス平衡を維持しつつ、現在のロボットの運動状態から滑らかに遷移可能で、環境形状に係る運動学的拘束とユーザ・プログラムからの上肢・下肢運動変更要求を共に満足することができる次時刻のロボットの運動状態を決定し、全身の関節角参照値を出力する。

　全身関節駆動部２６は、実時間歩行パターン生成部２５の出力した関節角参照値を実現するよう、各関節の自由度を構成するアクチュエータ・モータ（前述）をサーボ・コントローラ（図示しない）により駆動する。

　図３には、図２に示した歩行制御システムの処理の流れをフローチャートの形式で示している。この歩行制御システムは、例えば１０ミリ秒という制御周期で動作するものとする。

　この制御系は、処理開始後にステップＳ１に移行し、上肢・下肢要求運動入力部２４において、上肢・下肢への運動要求を受理する。上肢への要求は例えば関節角参照値として、下肢への要求は歩幅・歩行周期・旋回角などの歩容パラメータとして与えられる。

　続いて、ロボット状態観測部２１において、種々のセンサ情報を基に実際のロボットの運動状態を表す状態ベクトルを算出する（ステップＳ２）。

　次いで、環境形状観測部２２において、種々のセンサ情報を基に路面段差・路面傾斜などの環境形状を算出する（ステップＳ３）。

　次いで、外力・外モーメント観測部２３において、力センサなどの値に基づきロボット各部へ印加されている外力・外モーメント値を計測する（ステップＳ４）。

　次いで、実時間歩行パターン生成部２５において、ロボットの現在状態、環境形状、印加外力、ユーザからの運動変化要求を反映しつつ、力学的なバランス平衡を維持するようなロボットの全身運動を再計画する（ステップＳ５）。

　最後に、全身関節駆動部２６において、各関節自由度を構成するアクチュエータ・モータをサーボ・コントローラにより駆動する（ステップＳ６）。

　以上の処理が一般に本システムの１周期に関る処理であり、終了後、再度ステップＳ１の実行に戻る処理を制御周期Δｔ毎に実行する。

　本実施形態に係る歩行制御システムは、ユーザ・プログラムからの運動変更要求を、力学的バランス平衡を維持したまま即応的に満足する実時間全身運動再計画の機能を備え、且つ、当該機能に対し、現在のロボットの実状態、実環境形状、外力など実世界情報を反映することを特徴とする。

　すなわち、本システムによれば、未知環境適応に関する安定化のための演算を別途行なうことなく、ロボットの実状態、実環境形状、外力を実時間全身運動再計画演算の境界値や拘束条件として入力することで、一回の実時間全身運動再計画演算内で歩行パターン生成演算と安定化演算の演算共有化を実現することができる。

　これにより、ロボットの歩行安定化制御のための演算の節約がなされるので、計算リソースの限られた組込みＣＰＵ上でも、柔軟な運動要求反映機能とロバスト性を兼ね備えた高機能全身運動制御系を構築することが可能となる。

　一般に、歩行パターン生成や種々の安定化で用いられる力学モデルが相異なると、力学モデル間の不用意な干渉を生じて歩行性能の低下につながる可能性が否定できない。これに対し、本実施形態に係る歩行制御システムでは、力学演算は単一の歩行パターン生成演算内で一貫して行なわれるため、このような種々のアルゴリズム間の不用意な干渉が生じ難く、良好な歩行性能を得ることができる。

　以下では，その基本演算を実行する実時間歩行パターン（歩容）生成部２５の構成例について説明する。

　図４には、実時間歩行パターン生成部２５の内部構成を模式的に示している。同図に示すように、実時間歩行パターン生成部２５は、足底位置・姿勢軌道生成部４１と、ＺＭＰ方程式求解部４２と、質点分布調整部４３と、関節角算出部４４と、現在状態決定部４５より構成される。また、実時間歩行パターン生成部２５は、上肢・下肢要求運動入力部２４からの上肢参照軌道及び歩容パラメータ、環境形状観測部２２からの環境形状、外力・外モーメント観測部２３からの外力・外モーメント、ロボット状態観測部２１からの現在実状態を入力に持つ。

　足底位置・姿勢軌道生成部４１には、歩幅・歩行周期・旋回角・足上げ高さなどの下肢要求運動に関る歩容パラメータと、環境形状観測部２２より得られた路面高さや路面傾斜などの環境形状が入力される。足底位置・姿勢軌道生成部４１は、これらの入力された値を用いて、一歩前の着床状態から数歩先までの左右両足底の位置及び姿勢軌道を算出する。

　図５には、足底の位置・姿勢軌道を示している。同図に示すように、位置に関しては座標位置（ｘ，ｙ，ｚ）で、姿勢に関してはＥｕｌｅｒ（オイラー）角（α，β，γ）などを用いて表現する。また、各軌道は歩容パラメータや環境形状に適合するよう、左右足底について各パラメータ毎に多項式補間などを用いて時系列を生成することで構成する。例えば、左足底位置姿勢軌道に関しては、左足底位置姿勢を（ｘ_L，ｙ_L，ｚ_L，α_L，β_L，γ_L）、一歩前の左足底位置姿勢を（ｘ_L1，ｙ_L1，ｚ_L1，α_L1，β_L1，γ_L1）とすると、次の着床位置姿勢（ｘ_L2，ｙ_L2，ｚ_L2，α_L2，β_L2，γ_L2）は歩幅・歩行周期・旋回角などの歩容パラメータにより決定され、その間の左足底位置姿勢軌道は以下に示すように各位置姿勢毎の補間式によって構成される。

　補間式には、例えば５次の多項式などを用いることができる。２歩目以降の足底位置姿勢は、１歩目と同一の歩容並びに環境状況が継続するものとして、同様に補間式によって構成される。

　ＺＭＰ方程式求解部４２では、足底位置・姿勢軌道生成部４１で生成された足底位置姿勢軌道、歩容パラメータ、ロボットの現在の状態、外力印加状態に基づいて、力学的バランス平衡を維持しつつ、これらの条件を満足するための時刻Δｔ後のロボットの状態ベクトルを算出する。以下では、ＺＭＰ方程式求解部４２の簡単な構成例として重心位置・速度を状態ベクトルとする場合について説明する。

　ロボットの質量をｍ、 慣性座標系内での重心位置をｘ＝（ｘ，ｙ，ｚ）^T、 ロボットが外界から受ける力及びモーメントをそれぞれｆ＝（ｆ_x，ｆ_y，ｆ_z）^T及びｎ＝（ｎx，ｎy，ｎz）^Tとする。また、慣性座標系内に任意の点ｐ＝（ｐ_x，ｐ_y，ｐ_z）^Tを考え、ｐ回りに発生するモーメントをＭとする。また、重力加速度ベクトルを−ｇ＝（０，０，−ｇ）^Tとする。

　点ｐ回りのモーメントの平衡条件より、

　また、重心の運動方程式より、

　ここで、上記の（式１）及び（式２）を用いてロボットが外界から受ける力ｆを消去すると、

　点ｐをＺＭＰにとると、水平軸モーメントがゼロとなるので、

　上記の（式４）において、第１及び第２要素について展開すると、下式に示すようにロボットの重心運動に関するＺＭＰ方程式を得ることができる。

　このように、外モーメントｎはＺＭＰ方程式中にて強制項として現れる。点ｐがロボットの接地多角形内に存在するとの拘束条件下で上記の（式５）を満足する全身運動を得ることが物理的に実行可能な歩行パターンを生成することに相当する。

　（式５）は、単純な形式で表現されているものの、ｘ及びｙに関して変係数の微分方程式であり、解析解を得ることは容易ではない。本実施形態では、一旦、比較的短時間の時間区間Ｔを考え、この区間内において以下の近似が成立すると仮定する。

（仮定１）ＺＭＰ軌道は加速度を有さない。
（仮定２）重心高さは一定である。
（仮定３）外モーメントは一定である。

　すると、（式５）は以下のような簡素な常微分方程式となる。

　但し、

であり、ｘ成分のみ記した。（式６）の一般解は以下の通りである。

　但し、λは固有値であり、以下の式で表される。

　ここで、（式９）の境界値を計算すると、以下の通りとなる。

　ＺＭＰ方程式は非線型方程式であり、これを解くには計算負荷が高く、演算時間が長くなってしまう。本実施形態に係る歩行制御システムは、ロボットの実機上でリアルタイムで歩行制御を行なうことを目的とするので、所定の区間毎に分割して線形近似を行なうことによって、処理のリアルタイム性を確保するようにしている。

　一般の歩行では、上述したような（仮定１）〜（仮定３）を満足するような複数の短期区間が連なって構成されているとしてモデル化することができる。図６には、仮定条件（仮定１）〜（仮定３）を満足する短期区間がＮ区間連なって構成されているとしてモデル化された歩行モデルを示している。以下の説明では、扱いが容易な短期区間の取り方として、片脚支持期と両脚支持期の境界で区間を区切る例を扱うものとする。ｉ番目の区間ｉの時間幅をＴ_i、未定係数をＣ_1i、Ｃ_2i、固有値をλ_i、区間ｉの終端におけるＺＭＰ位置をｐ_xi、重心位置をｘ_iとすると、上記の（式１１）の境界条件は、一般の区間においては以下のように表される。

　但し，外モーメントｎ_yは全区間で一定とした．隣接する区間と区間の間の重心位置・速度の連続条件より、以下の式が各短期区間境界（ｉ＝２…Ｎ）において成立する。

　また、区間１の始端条件を歩行期間全体の始端条件として、その重心位置及び速度を実現するように、以下に示すように設定する。

　同様にして、区間Ｎの終端条件を歩行期間全体の終端条件として、その重心位置及び速度を実現するように、以下に示すように設定する。

　始端条件の重心位置及び速度には、現在状態決定部４５から出力される現在の状態ベクトル（本実施形態では重心位置と速度）を代入する。これにより、ＺＭＰ方程式の解に対し、現在の状態に位置及び速度が連続となるような拘束が課せられる。

　終端条件の重心位置には、足底位置・姿勢軌道生成部４１より取得した歩行期間終端における両足底位置を基に、両足底の中心位置などを設定する。また、終端条件の速度には、重心位置にて重心が静止するとの条件を仮定し、ゼロを代入するなどの設定を行なう。これにより、ＺＭＰ方程式の解に対し、２〜３歩先の歩行周期終端にて重心が両足底中心の直上に到達するような拘束が課せられる。

　実時間歩行パターン生成においては、制御サイクル毎に歩行パターンを再生成し、次制御サイクルにおける運動状態を決定するので、現時刻から十分離れた終端時刻における終端到達条件がこのように近似的に与えられても、実用上十分な精度の解を得ることができる。

　上記の（式１３）〜（式１８）は、２Ｎ＋２個の拘束条件を与える。変数に関しては未定係数Ｃ_1i、Ｃ_2i（ｉ＝１…Ｎ）以外の変数の設定の仕方として多くの方法が考えられる。本実施形態では、ｐ_xi（ｉ＝１…Ｎ）を変数に加える。

　このとき、変数の数は３Ｎ個となり、変数の数が過剰となるため、最適化問題として取り扱う。例えば、短期区間境界における安定領域中心（即ち接地多角形重心）とＺＭＰ位置

の差分を最小化するよう、以下の（式１９）に示すような等式制約条件付きの２次計画問題として扱うことで、これらの変数を一意に定めることができる。接地多角形重心位置

は、足底位置・姿勢軌道生成部４１で生成された足底位置軌道を基に得ることができる。

　あるいは、特定区間ｊ，ｋ∈［１，Ｎ］以外のＺＭＰ位置は接地多角形重心位置に固定とし、ｐ_xj、ｐ_xkのみを変数に設定して、（式１３）〜（式１８）を２Ｎ＋２元連立１次方程式として解くようにしてもよい。

　以上により、（式９）を用いて、外モーメントが作用する条件下にて現在の運動状態に滑らかに接続し、所望の時刻に指定運動状態に到達するための中間運動状態を算出することができる。同式を用いることにより、以下のように、次の制御サイクル（時刻Δｔ後）で用いるべきロボットの状態を算出することができる。

　質点分布調整部４３では、上記のＺＭＰ方程式求解部４２で得られた次時刻状態ベクトルを実現し、且つ、ユーザ・プログラムより指定された上肢参照軌道（例えば関節角軌道）と足底軌道（歩容パラメータにより与えられる）を満足するように、全身の質点分布状態を調整する。

　ここでは、基本的な例として、足底位置姿勢・上体関節角をユーザ・プログラムの要求値に固定したまま、機体の位置（ｘ_B，ｙ_B，ｚ_B）を操作し、ＺＭＰ方程式求解部４２で得られた重心位置となるように全身質点分布を調整する。このような幾何的拘束条件下において、機体の速度と重心の速度の間には、ヤコビアンをＪ_Bとすると、以下の関係が成立する。

　但し、ヤコビアンＪ_Bは現在の脚関節角に依存する。現在のロボットの重心位置ｘkとＺＭＰ方程式求解部４２で得られた次時刻の重心位置ｘ_k+1の間にｄｘ＝ｘ_k+1−ｘ_kの偏差が存在するとき、基体位置の操作量ｄｘ_Bは近似的に以下の式で得ることができる。

　ここで、重心の算出には、ユーザ・プログラムより入力される上肢関節角参照値も反映される。以上より、現在の基体位置をｘ_BKとすると、ＺＭＰ方程式求解部４２で得られた状態を実現するため質点分布調整後の機体位置Ｘ_Bk+1は以下の式で得ることができる。

　但し、（式２３）の結果得られる重心位置とＺＭＰ方程式求解部４２で得られた次時刻の重心位置の偏差がなお大きな場合には、（式２３）を繰り返し用いて、偏差が十分小さくなるまで収束演算を実行してもよい。

　関節角算出部４４では、質点分布調整部４３で得られた次時刻の基体位置、及び足底位置・姿勢軌道生成部４１より得られる次時刻の足底位置姿勢を実現するように脚部の関節角を決定する。これは、両者の相対位置姿勢を求めた後、例えば、周知の逆キネマティクス計算によって実行することができる。

　逆キネマティクス計算で得られた脚関節角とユーザ・プログラムより与えられた上肢の参照関節角とを併せて、次時刻における全身関節角参照値として出力する。この全身関節角参照値は全身関節駆動部２６の関節角参照値として用いられる。

　現在状態決定部４５は、各種センサ値を基に算出されたロボットの現在状態ベクトルと、前回の制御サイクルでＺＭＰ方程式求解部４２において算出され参照値として用いられた状態ベクトルより、次回の制御サイクルでＺＭＰ方程式求解部の始端条件として用いるべき現在状態ベクトルを決定・更新する。すなわち，各種センサ値を基に算出されたロボットの現在状態ベクトルと、ＺＭＰ方程式求解部４２において算出された状態ベクトルは一般に異なるため、その調整演算を行なう。

　ロボットの実状態を反映する必要のない場合には、ＺＭＰ方程式求解部４２の出力を現在状態ベクトルとして出力するのみでよい。これは、実時間歩容生成の結果出力される歩行パターンを開ループ制御下にて実行する場合に相当する。一方、ロボットの実状態を反映する必要のある場合には、各種センサ値を基に算出されたロボットの現在状態ベクトルを現在状態ベクトルとして出力する。各種センサ値を基に算出されたロボットの現在状態ベクトルが理想値からの差分で与えられる場合には、ＺＭＰ方程式求解部４２の出力に差分ロボットの現在状態ベクトルを加算し、現在状態ベクトルとして出力するように構成してもよい。

　以上、実時間歩行パターン生成部２５の内部構成について説明してきた。図７には、実時間歩行パターン生成部２５における処理の流れをフローチャートの形式で示している。

　まず、前制御サイクルにてＺＭＰ方程式求解部４２で算出された状態ベクトルと、各種センサ値を基に算出されたロボットの現在状態ベクトルより、今回の制御サイクルにおいてＺＭＰ方程式初期条件として設定するべき現在状態ベクトルを算出する（ステップＳ１１）。

　次いで、現在の両足位置姿勢と、歩幅・歩行周期・旋回角などのユーザ・プログラムからの要求歩容パラメータに応じて、数歩先までの足底位置姿勢軌道を生成する（ステップＳ１２）。

　次いで、ステップＳ１１にて得られた現在状態ベクトルをＺＭＰ方程式始端拘束条件として設定する（ステップＳ１３）。同様に、ステップＳ１２にて得られた数歩先の足底位置姿勢に基づいて、ＺＭＰ方程式の終端拘束条件を設定する（ステップＳ１４）。

　また、外力・外モーメント観測部２３の結果をＺＭＰ方程式の強制外力項に設定する（ステップＳ１５）。そして、ステップＳ１６では、これらの条件を満足するよう、ＺＭＰ方程式の境界値問題を解く。

　本実施形態では、センサ値を含めた実際のロボットの現在状態ベクトルを初期条件として設定したり、実測外力値を強制項に反映したりするなど、実世界の状態をＺＭＰ方程式求解に反映することが可能である。こうして得られたＺＭＰ方程式の解より、次制御サイクル（時刻Δｔ後）におけるロボットの状態ベクトル（目標状態ベクトル）を算出する（ステップＳ１７）。この状態ベクトルは、現在の運動状態に滑らかに接続し、ユーザからの要求歩容パラメータを反映しつつ、外力作用条件下にて力学的バランス平衡を維持する値を与えている。

　次いで、この状態ベクトル（目標状態ベクトル）を実現するための全身質点分布を求め（ステップＳ１８）、その結果を関節角参照値に変換して（ステップＳ１９）、１制御周期内の実時間歩行パターン生成演算を終了する。

　本処理ルーチンが制御周期毎に実行されることで、急激な歩容変化や実運動状態変化に対して即応的な対応が可能となる。

　本実施形態に係るロボットの歩行制御方式は、実時間歩行パターン生成部２５に実環境の状況を反映するようになっている。これによって、未知環境条件に対する適応歩行性能を実現するために、特に適応制御系のために新たな力学モデルを付加する必要がない。すなわち、ひとたび汎用実時間歩行パターン生成部２５が構成されれば、適応制御機能の追加手続きは、ロボット状態観測部２１、環境形状観測部２２、及び外力・外モーメント観測部２３を構成するのみとなる。

　以下では、ロボット状態観測部２１、環境形状観測部２２、及び外力・外モーメント観測部２３を構成するための幾つかの構成例について説明する。

　適応制御機能を追加する第１の例は、ロボット状態観測部２１に関する構成例である。ロボット状態観測部２１は、実時間歩行パターン生成部２５内の状態ベクトルに相当する物理量を種々のセンサ値を基に計測する。本実施形態では、上述した実時間歩行パターン生成部２５内の状態ベクトルに相当する、重心位置・重心速度を計測するように構成すればよい。

　図８には、ロボットの機体全体の回転運動によって重心位置偏差が発生している様子を示している。重心位置偏差Δｘが、図示の通り、主にＺＭＰ中心位置回りのロボット全体の回転運動によって発生すると考えると、以下の式で近似的に求めることができる。

　ここで、ΔＲは機体に取り付けられた加速度センサ及びジャイロにより得られる傾斜偏差を表す行列である。また、重心速度偏差は以下の式を用いて近似的に求めることができる。

　ここで、Δωは機体に取り付けられたジャイロより得られる角速度偏差ベクトルである。重心位置偏差及び重心速度偏差を現在状態決定部４５に入力することで、次の実時間歩容生成演算の始端条件として反映され、重心軌道偏差を考慮した歩行パターンの再計画が行なわれる。

　図９には、重心位置・重心速度を計測するように構成されたロボット状態観測部２１における重心位置及び速度偏差算出処理を行なう処理手順をフローチャートの形式で示している。

　まず、機体に取り付けられた加速度計Ａ１あるいはジャイロＧ１の値を用いて基体の実傾斜Ｒを測定し、実時間歩行パターン生成部２５で算出された機体の計画上の傾斜値Ｒ₀からの偏差として、傾斜偏差をΔＲ＝Ｒ₀ ^TＲなどとして算出する（ステップＳ２１）。

　同様に、機体に取り付けられたジャイロの値を用いた機体の実角速度ωを計測し、実時間歩行パターン生成部２５で算出された機体の計画上の角速度ω₀からの偏差として、Δω＝ω−ω₀を算出する（ステップＳ２２）。

　続いて、上記の（式２４）に基づく重心位置偏差の算出（ステップＳ２３）、（式２５）に基づく重心速度偏差の算出を行なって（ステップＳ２４）、本処理を終了する。この結果として得られた重心位置偏差及び重心速度偏差を現在状態決定部４５に入力する。

　ロボット状態観測部２１をこのように構成することで、ロボットがバランスを喪失し、重心軌道が計画状態から歪んだとしても、その状態を回復するための演算が実時間歩行パターン生成演算の一環として行なわれる。したがって、柔軟な歩容変更要求への対応機能と未知環境に対する適応機能をより少ない演算で実現することが可能となる。

　本実施形態では、状態ベクトルを計測するための手段として機体に取り付けられた加速度・ジャイロを用いる簡単な例を示したが、本発明の要旨はこれに限定されるものではない。例えば、足底力センサ・加速度センサ（例えば図１のＡ２，Ａ３）・ジャイロ（例えば図１のＧ２，Ｇ３）や、全身に分布配置された加速度センサ・ジャイロ・力センサなどを協調的に用いて計測するように構成してもよい。

　次いで、適応制御機能を追加する第２の例として、ロボット状態観測部２１に加え、さらに環境形状測定部２２を構成する例について説明する。

　環境形状観測部２２は、種々のセンサ値に基づき、路面段差・路面傾斜などの環境形状を算出し、実時間歩行パターン生成部２５に入力するように構成する。特に本実施形態では、足底力センサ値を用いて路面形状を把握する例について以下に説明する。

　図１０には、路面傾斜の算出方法を図解している。本実施形態に係るロボットの左右の足底四隅には前述の通り足底面垂直方向の１軸力を計測する力センサが取り付けられているが、その出力をそれぞれ図１０の左側に示すようにＦ_i（ｉ＝１…４）とする。

　足底が路面と接触していない遊脚期には、その出力は荷重値０を示し、路面形状に関する情報は得られない。このような場合、路面のロール傾斜角推定値α_F、ピッチ傾斜角推定値β_Fはともに緩やかに０に復元させる処理を行なう。また、着床とともにＦ_i（ｉ＝１…４）は正の値を出力するので、この値を用いて路面傾斜角の推定処理を実行する。

　このような路面傾斜算出結果を実時間歩行パターン生成部２５に入力すると、足底の軌道に反映され、その角度に足底が倣うように足底が駆動される。よって、推定値を制御サイクル毎に更新することで、図１０右に示すように足底は路面に対して倣うように駆動される。

　足底が路面に倣うと、図１０右下に示すように、四辺形足底のいずれの辺の力センサ値の和も非０荷重を出力する。例えば、ロール方向に関しては、Ｆ_L＝Ｆ₁＋Ｆ₂の値とＦ_R＝Ｆ₃＋Ｆ₄の値がともに非０荷重を出力すれば路面に倣ったとみなしてよい。ピッチ方向に関しても同様に、Ｆ_F＝Ｆ₁＋Ｆ₄の値とＦ_R＝Ｆ₂＋Ｆ₃の値がともに非０荷重を出力すれば路面に倣ったとみなしてよい。

　足底をこのように駆動するには、以下の式に示すように、足底の前後辺及び左右辺の荷重差に一定のゲインを掛けた値分だけ路面傾斜推定値を増加又は減少させればよい。

　本実施形態では、路面傾斜推定演算は、ロール方向に関してはＦ_L，Ｆ_Rのいずれかが、ピッチ方向に関してはＦ_F、Ｆ_Bのいずれかが、小さな値を示している場合について実行する。つまり、倣いが達成されるまでは路面傾斜推定値が随時更新され、足底は路面に倣うように駆動される。一方、倣いが達成された状態では路面傾斜推定値は不用意に更新しない。以上により、足底力センサによって適切に路面の傾斜を計測することができる。

　同様に、路面高さの計測も足底力センサを用いて行うことができる。以下、路面高さ推定値をｚ_Fとし、検出対象の足底に設置された各センサで検出された総垂直反力をＦとする。

　計画上、遊脚期であるにも関らず垂直反力が検出された場合は、現在の推定値以上の高さとなる路面に接触したと考えることができるため、以下のように徐々に路面高さ推定値を増大させ、足底軌道を上方修正する。

　また、計画上、支持脚期であるにも関らず垂直反力が検出されない場合は、現在の推定値以下の高さとなる路面であると判断することができる。そこで、以下のように徐々に路面高さ推定値を減少させ、足底軌道を下方修正する。

　ここで、δｚは微小な変更量とする。足底が路面と接触していない遊脚期には、足底を計画軌道に復帰させるため、路面高さ推定値Ｚ_Fを緩やかに０に復元させる処理を行なう。

　以上の処理により、図１１に示すように、足底軌道は、着床とともに未知路面高さに対して拘束を受けた後、遊脚期にて計画軌道へ徐々に復帰するように実行される。

　上述したように、路面傾斜角推定値α_F、β_F、及び路面高さ推定値Ｚ_Fを左右の足底それぞれに対して計測し、制御周期毎に前記実時間歩容生成部の足底軌道生成部の離床・着床時の足底位置・姿勢に反映することで、ロボットは力学的バランス平衡を保ちつつ、未知路面形状に適合するように足底軌道を再計画しながら安定歩行を維持・継続することができる。特に、ロボット状態観測部２１と併用することでよりロバストな歩行性能を実現することができる。

　図１２には、路面傾斜・路面高さ推定処理の手順をフローチャートの形式で示している。但し、簡単のため、片足に関る処理のみ記載する。路面傾斜に関してはロール方向に係る処理のみを記載する。

　まず、本処理は所定の制御周期毎に実行され路面傾斜推定値及び路面高さ推定値は更新されるが、初回においては、ステップＳ３１及びＳ３２において計画値に初期化する。

　続いて、総床反力Ｆ、足底４辺に作用する反力Ｆ_L、Ｆ_Rを計測・算出する（ステップＳ３３）。これらの結果を基に、足底の接地判定処理を行なう（ステップＳ３４）。

　総床反力Ｆが正の閾値ξより大きい場合は、足底が路面に接地していると判断し、ステップＳ３５に進む。ステップＳ３５は倣い判定であり、Ｆ_L、Ｆ_Rのいずれかが正の閾値εより小さな場合は、倣いが完了していないと判断し、ステップＳ３６に進む。

　ステップＳ３６では、倣い動作を行なわせるため、上記の（式２６）に基づく路面傾斜推定値の更新を実行した後、ステップＳ３７へ進む。

　一方、ステップＳ３５において、床反力Ｆ_L、Ｆ_Rともにεより大きく、倣い完了との判定がなされた場合は、ステップＳ３６における路面傾斜推定値の更新は不要であり、ステップＳ３７に移行する。

　続いてステップＳ３７では、歩行パターン計画上、現時刻が対象足底を支持脚とする期間に含まれるかどうかの判定を行なう。支持脚期間と判定された場合は、計画通りの接地が実現されているとみなし、１制御周期分の路面形状推定処理を完了する。一方、遊脚期間と判定された場合は、計画値以上の高さの路面に接触したとみなし、ステップＳ３８に移行して、上記の（式２７）に基づく路面高さ推定値の更新処理を実行することで、１制御周期分の路面形状推定処理を完了する。

　さて、ステップＳ３４の接地判定結果において、無接地状態と判定された場合は、ステップＳ３９に移行して、路面傾斜推定値を計画値に徐々に復帰させるための演算を実行する。続いて、歩行パターン計画上、現時刻が対象足底を支持脚とする期間に含まれるかどうかの判定を行なう（ステップＳ４０）。

　ここで、支持脚期間と判定された場合は、計画値以下の路面高さのため足底が計画したタイミングで着床できない場合と判断し、上記の（式２８）に基づく路面高さ推定値の更新処理を実行することで（ステップＳ４１）、１制御周期分の路面形状推定処理を完了する。

　一方、ステップＳ４０において遊脚期と判定された場合は、計画通り無接地状態が維持されているとみなし、路面高さ推定値を計画値に徐々に復帰させるための演算を実行した後（ステップＳ４２）、１制御周期分の路面形状推定処理を完了する。

　本実施形態では、環境形状計測手段として足底接地後に足底力センサを用いて環境形状を推定する例を示したが、遊脚期に足底の測距センサ（例えば、図１中Ｄ１〜Ｄ８など）を用いて（非接触で）環境形状を測定したり、視覚機能を用いて環境形状を測定したり、あるいはこれらを協調的に用いるように構成してもよい。

　次いで、適応制御機能を追加する第３の例として、さらに外力・外モーメント観測部２３を構成する例について説明する。

　ここでは、外力・外モーメント観測部２３において、足底垂直方向の１軸力センサを用いて床面上の点ｐ回りに作用するモーメントを計測するように構成する。特に、点ｐをＺＭＰに採れば、理想的な歩行状態が維持されている場合には、外力モーメントｎの出力は０となる。逆に、非ゼロの外力モーメントｎが検出された場合には、ロボットに作用した予定外の外モーメントとみなすことができる。

　図１３には、足底垂直方向の１軸力センサを用いて床面上の点ｐ回りに作用するモーメントを計測する様子を示している。同図に示すように、足底が床面に対してほぼ水平で、足底の１軸力センサのうち非０荷重を出力しているものがすべて点ｐと同一水平面上に存在しているとみなせる状況においては、足底１軸力センサの出力をＦ_i（ｉ＝１…８）、各々の力センサの位置をＰ_Fi（ｉ＝１…８）とすると、点ｐ回りに作用するモーメントｎは、近似的に下式を用いて求めることができる。

　図１４には、足底垂直方向の１軸力センサを用いて床面上の点ｐ回りに作用するモーメントを計測するための処理手順をフローチャートの形式で示している。

　まず、足底センサ値Ｆ_i（ｉ＝１…８）を計測した後（ステップＳ５１）、実時間歩行パターン生成部２５で生成された足底位置・姿勢の現在値を基に、足底４端点に取り付けられた力センサの現在位置ｐ_Fi（ｉ＝１…８）を算出する（ステップＳ５２）。

　さらに、実時間歩行パターン生成部２５より、現在のＺＭＰ位置ｐを取得する（ステップＳ５３）。

　最後に、上記の（式２９）を用いて、点ｐ回りのモーメントを算出し（ステップＳ５４）、本処理ルーチンを終了する。

　こうして推定・算出された外力モーメントｎを、上述したように構成された実時間歩行パターン生成部２５の強制項付ＺＭＰ方程式に入力することで、第１の実施形態の場合に加えて、外力モーメントに対して適応的に全身運動を再計画して、ロボットを安定に歩行させることが可能となる。

　以上のように、実世界に係る情報を実時間歩行パターン生成部２５に入力することで、ユーザからの運動変更要求に即応的に対応し、既知及び未知環境下においてロバストに歩行の継続を可能とするための全身関節角が出力される。最後に、本全身関節角を参照値として、全身関節駆動部にてＰＩＤコントローラなどを用いて各関節を位置制御することで、所望の運動状態を実現することができる。

　なお、上述した本発明の実施形態では、状態ベクトル（状態量）として重心位置ｘ、重心速度ｄｘ／ｄｔからなる２次元ベクトル［ｘ，ｄｘ／ｄｔ］^Tを用いる例を示したが、本発明の要旨はこれに限るものではない。例えば、腰部などの重心近傍の特定部位の位置ｐ、速度ｄｐ／ｄｔからなる２次元ベクトル［ｐ，ｄｐ／ｄｔ］^T、全身の質点位置ｐi、速度ｄｐ_i ／ｄｔ（ｉは質点のインデックスで、ｉ＝１…Ｎ）からなる２Ｎ次元ベクトル［ｐ₁，ｐ₂，…，ｐ_N，ｄｐ₁／ｄｔ，ｄｐ₂／ｄｔ，…，ｄｐ_N／ｄｔ］^Tなどを状態ベクトルとして用いても良い。

　また、本実施形態では運動方程式として重心に関するＺＭＰ方程式を用いたが、本発明の要旨はこれに限るものではなく、各リンク質量、重心、慣性モーメントまで考慮したＺＭＰ方程式、ＺＭＰ方程式が適用されない一般路面・無接地状態を記述可能な一般のロボットの全身に係る運動方程式を用いてもよい。

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

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

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

図１は、本発明の実施に供される脚式移動ロボットの自由度構成を模式的に示した図である。 図２は、本発明の実施形態に係る脚式移動ロボットに適用される歩行制御システムの機能構成を模式的に示した図である。 図３は、図２に示した歩行制御システムの処理の流れを示したフローチャートである。 図４は、実時間歩行パターン生成部２５の内部構成を模式的に示した図である。 図５は、足底の位置・姿勢軌道を示した図である。 図６は、仮定条件（仮定１）〜（仮定３）を満足する短期区間がＮ区間連なって構成されているとしてモデル化された歩行モデルを示した図である。 図７は、実時間歩行パターン生成部２５における処理の流れを示したフローチャートである。 図８は、ロボットの機体全体の回転運動によって重心位置偏差が発生している様子を示した図である。 図９は、重心位置・重心速度を計測するように構成されたロボット状態観測部２１における処理動作を示したフローチャートである。 図１０は、路面傾斜の計測方法を示した図である。 図１１は、着床と共に未知路面高さに対して拘束を受けた後、遊脚期にて計画軌道へ徐々に復帰するように実行される足底軌道の様子を示した図である。 図１２は、路面傾斜・路面高さ推定処理の手順をフローチャートの形式で示した図である。 図１３は、足底垂直方向の１軸力センサを用いて床面上の点ｐ回りに作用するモーメントを計測する様子を示した図である。 図１４は、足底垂直方向の１軸力センサを用いて床面上の点ｐ回りに作用するモーメントを計測するための処理手順を示したフローチャートである。

符号の説明

　２１…ロボット状態観測部
　２２…環境形状観測部
　２３…外力・外モーメント観測部
　２４…上肢・下肢要求運動入力部
　２５…実時間歩行パターン生成部
　２６…全身関節駆動部
　４１…足底位置・姿勢軌道生成部
　４２…ＺＭＰ方程式求解部
　４３…質点分布調整部
　４４…関節角算出部
　４５…現在状態決定部

Claims (12)

1. 　移動手段を有するロボット装置であって、
   　前記ロボット装置の状態量を検出する状態検出手段と、
   　前記ロボット装置の状態量に関する境界条件付き運動方程式の解を求める実時間運動方程式求解手段と、
   　前記状態検出手段により検出された現在の状態量を、前記境界条件付き運動方程式の境界条件として設定する境界条件設定手段と、
   　前記設定された境界条件の下で、前記実時間運動方程式求解手段が算出した運動方程式の解に基づいて、前記ロボット装置の次時刻の目標状態量を算出する目標状態量算出手段と、
   　前記目標状態量に基づいて、前記ロボット装置の運動パターンを生成する運動パターン生成手段と、
   を具備することを特徴とするロボット装置。
2. 　前記現在の状態量は現在の制御サイクルにおける前記ロボット装置の状態量であり、次時刻の目標状態量は次の制御サイクルにおける前記ロボット装置の目標状態量である、
   ことを特徴とする請求項１に記載のロボット装置。
3. 　前記状態検出手段は、前記ロボット装置の状態量として、前記ロボット装置の重心位置又は重心位置のＮ階微分値を用いる、
   ことを特徴とする請求項１に記載のロボット装置。
4. 　前記実時間運動方程式求解手段は、前記運動方程式としてＺＭＰ方程式を用いる、
   ことを特徴とする請求項１に記載のロボット装置。
5. 　前記移動手段は、可動部とリンクとからなる脚で構成され、
   　前記脚の先端リンクの位置又は姿勢に関する軌道を制御周期毎に計算する実時間軌道生成手段と、
   　前記ロボット装置の周辺環境の状況を計測する環境状況計測手段と、
   をさらに備え、
   　前記実時間軌道生成手段は、前記環境状況計測手段によって計測された実環境状況に基づいて、前記先端リンクの位置と姿勢を算出し、
   　前記運動パターン生成手段は、前記先端リンクの位置と姿勢、及び前記目標状態量に基づいて前記ロボット装置の運動パターンを生成する、
   ことを特徴とする請求項１に記載のロボット装置。
6. 　前記ロボット装置に作用する外力又はモーメントを検出する外力検出手段をさらに備え、
   　前記実時間運動方程式求解手段は、前記外力検出手段が算出した外力又はモーメントを加味して、前記運動方程式の解を算出する、
   ことを特徴とする請求項１に記載のロボット装置
7. 　移動手段を有するロボット装置の制御方法であって、
   　前記ロボット装置の状態量を検出する状態検出ステップと、
   　前記ロボット装置の状態量に関する境界条件付き運動方程式の解を求める実時間運動方程式求解ステップと、
   　前記状態検出ステップにおいて検出された現在の状態量を、前記境界条件付き運動方程式の境界条件として設定する境界条件設定ステップと、
   　前記設定された境界条件の下で、前記実時間運動方程式求解ステップにおいて算出した運動方程式の解に基づいて、前記ロボット装置の次時刻の目標状態量を算出する目標状態量算出ステップと、
   　前記目標状態量に基づいて、前記ロボット装置の運動パターンを生成する運動パターン生成ステップと、
   を具備することを特徴とするロボット装置の制御方法。
8. 　前記現在の状態量は現在の制御サイクルにおける前記ロボット装置の状態量であり、次時刻の目標状態量は次の制御サイクルにおける前記ロボット装置の目標状態量である、
   ことを特徴とする請求項７に記載のロボット装置の制御方法。
9. 　前記状態検出ステップでは、前記ロボット装置の状態量として、前記ロボット装置の重心位置又は重心位置のＮ階微分値を用いる、
   ことを特徴とする請求項７に記載のロボット装置の制御方法。
10. 　前記実時間運動方程式求解ステップでは、前記運動方程式としてＺＭＰ方程式を用いる、
    ことを特徴とする請求項７に記載のロボット装置の制御方法。
11. 　前記移動手段は可動部とリンクとからなる脚で構成され、
    　前記脚の先端リンクの位置又は姿勢に関する軌道を制御周期毎に計算する実時間軌道生成ステップと、
    　前記ロボット装置の周辺環境の状況を計測する環境状況計測ステップと、
    をさらに備え、
    　前記実時間軌道生成ステップでは、前記環境状況計測ステップにおいて計測された実環境状況に基づいて、前記先端リンクの位置と姿勢を算出し、
    　前記運動パターン生成ステップでは、前記先端リンクの位置と姿勢、及び前記目標状態量に基づいて前記ロボット装置の運動パターンを生成する、
    ことを特徴とする請求項７に記載のロボット装置の制御方法。
12. 　前記ロボット装置に作用する外力又はモーメントを検出する外力検出ステップをさらに備え、
    　前記実時間運動方程式求解ステップでは、前記外力検出ステップにおいて算出した外力又はモーメントを加味して、前記運動方程式の解を算出する、
    ことを特徴とする請求項７に記載のロボット装置の制御方法。
    

Images