(57)【特許請求の範囲】
【請求項１】
１以上の可動部を有する移動体装置であって、
前記移動体装置にローカル座標系を設定し、前記ローカル座標系原点の加速度を計測する第１の加速度センサと、
前記ローカル座標系における１以上の制御対象点に設置された第２の加速度センサと、
少なくとも前記第１の加速度センサ及び前記第２の加速度センサから得られる加速度情報に基づいて、前記ローカル座標系原点の加速度に対する前記制御対象点の相対加速度を算出する相対加速度算出手段と、
前記相対加速度に基づいて前記制御対象点を制御する制御手段と、
を具備することを特徴とする移動体装置。
【請求項２】
前記ローカル座標系と前記第１の加速度センサを前記移動体装置と路面との接触部位に設ける、
ことを特徴とする請求項１に記載の移動体装置。
【請求項３】
前記移動体装置は足部を有するロボット装置であり、
前記ローカル座標系と前記第１の加速度センサを前記足部に設ける、
ことを特徴とする請求項１に記載の移動体装置。
【請求項４】
前記移動体装置は腰部を有するロボット装置であり、
前記ローカル座標系と前記第１の加速度センサを前記腰部に設ける、
ことを特徴とする請求項１に記載の移動体装置。
【請求項５】
少なくとも前記第１の加速度センサ及び前記第２の加速度センサから得られる加速度情報に基づいて導出されるＺＭＰ方程式又は運動方程式を用いて、前記移動体装置に印加される未知モーメント又は未知外力を算出する手段をさらに備え、
前記制御手段は、該算出された未知モーメント又は未知外力に基づいて前記制御対象点を制御する、
ことを特徴とする請求項１に記載の移動体装置。
【請求項６】
前記移動体装置は、少なくとも上肢、下肢、及び体幹部を含んだ可動部を備えたロボット装置であり、
少なくとも前記上肢、前記下肢、前記体幹部毎に制御対象点が設けられている、
ことを特徴とする請求項１に記載の移動体装置。
【請求項７】
前記移動体装置上に分散配置された前記加速度センサ同士が直列的に接続され、個々の制御点において前記加速度センサの加速度情報に基づいて算出される前記ＺＭＰ方程式又は運動方程式上のモーメント項や外力項を、接続経路に従って各制御対象点において順次加算していく、
ことを特徴とする請求項６に記載の移動体装置。
【請求項８】
１以上の可動部を有する移動体装置の制御方法であって、
前記移動体装置にローカル座標系を設定し、前記ローカル座標系原点における第１の加速度を計測する第１の加速度計測ステップと、
前記ローカル座標系に設置された１以上の制御対象点における第２の加速度を計測する第２の加速度計測ステップと、
少なくとも前記第１の加速度及び前記第２の加速度に基づいて、前記ローカル座標系原点の加速度に対する前記制御対象点の相対加速度を算出する相対加速度算出ステップと、
前記相対加速度に基づいて前記制御対象点を制御する制御ステップと、
を具備することを特徴とする移動体装置の制御方法。
【請求項９】
前記第１の加速度計測ステップでは、前記移動体装置と路面との接触部位に前記ローカル座標系を設定して前記第１の加速度を計測する、
ことを特徴とする請求項８に記載の移動体装置の制御方法。
【請求項１０】
前記移動体装置は足部を有するロボット装置であり、
前記第１の加速度計測ステップでは、前記ローカル座標系を前記足部に設けて前記第１の加速度を計測する、
ことを特徴とする請求項８に記載の移動体装置の制御方法。
【請求項１１】
前記移動体装置は腰部を有するロボット装置であり、
前記第１の加速度計測ステップでは、前記ローカル座標系を前記腰部に設けて前記第１の加速度を計測する、
ことを特徴とする請求項８に記載の移動体装置の制御方法。
【請求項１２】
少なくとも前記第１の加速度及び前記第２の加速度に基づいて導出されるＺＭＰ方程式又は運動方程式を用いて、前記移動体装置に印加される未知モーメント又は未知外力を算出する未知モーメント又は未知外力算出ステップをさらに備え、
前記制御ステップでは、該算出された未知モーメント又は未知外力に基づいて前記制御対象点を制御する、
ことを特徴とする請求項８に記載の移動体装置の制御方法。
【請求項１３】
前記移動体装置は、少なくとも上肢、下肢、及び体幹部を含んだ可動部を備えたロボット装置であり、
少なくとも前記上肢、前記下肢、前記体幹部毎に制御対象点が設けられている、
ことを特徴とする請求項８に記載の移動体装置の制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、基体と前記基体に接続される可動部を有する移動体装置に係り、特に、ＺＭＰを安定度判別規範に用いて姿勢安定化制御を行なう移動体装置に関する。
【０００２】
さらに詳しくは、本発明は、機体上の各部に設置されたセンサからの計測値に基づいて導出されたＺＭＰ方程式又は運動方程式を用いて未知外力モーメント及び未知外力を同定して運動制御を行なう移動体装置に係り、特に、機体上の複数の部位に分散するセンサ・システムを配置して、ＺＭＰ方程式または運動方程式の導出に必要な運動パラメータを効率的に計測する移動体装置に関する。
【背景技術】
【０００３】
電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行なう機械装置のことを「ロボット」という。ロボットの語源は、スラブ語の“ＲＯＢＯＴＡ（奴隷機械）”に由来すると言われている。わが国では、ロボットが普及し始めたのは１９６０年代末からであるが、その多くは、工場における生産作業の自動化・無人化などを目的としたマニピュレータや搬送ロボットなどの産業用ロボット（ｉｎｄｕｓｔｒｉａｌ ｒｏｂｏｔ）であった。
【０００４】
最近では、ヒトやサルなどの２足直立歩行を行なう動物の身体メカニズムや動作を模した脚式移動ロボットに関する研究開発が進展し、実用化への期待も高まってきている。２足直立による脚式移動は、クローラ式や、４足又は６足式などに比し不安定で姿勢制御や歩行制御が難しくなるが、不整地や障害物など作業経路上に凹凸のある歩行面や、階段や梯子の昇降など不連続な歩行面に対応することができるなど、柔軟な移動作業を実現できるという点で優れている。
【０００５】
また、ヒトの生体メカニズムや動作を再現した脚式移動ロボットのことを、特に、「人間形」、若しくは「人間型」のロボット（ｈｕｍａｎｏｉｄ ｒｏｂｏｔ）と呼ぶ。人間型ロボットは、例えば、生活支援、すなわち住環境その他の日常生活上のさまざまな場面における人的活動の支援などを行なうことができる。
【０００６】
人間の作業空間や居住空間のほとんどは、２足直立歩行という人間が持つ身体メカニズムや行動様式に合わせて形成されおり、車輪その他の駆動装置を移動手段とした現状の機械システムが移動するのには多くの障壁が存在する。したがって、機械システムすなわちロボットがさまざまな人的作業を代行し、さらに人間の住空間に深く浸透していくためには、ロボットの移動可能範囲が人間のそれとほぼ同じであることが好ましい。これが、脚式移動ロボットの実用化が大いに期待されている所以でもある。
【０００７】
人間形若しくは人間型と呼ばれる２足直立歩行の脚式移動ロボットを研究・開発する意義を、例えば以下の２つの視点から把握することができよう。
【０００８】
１つは、人間科学的な視点である。すなわち、人間の下肢及び／又は上肢に似た構造のロボットを作り、その制御方法を考案して、人間の歩行動作をシミュレートするというプロセスを通じて、歩行を始めとする人間の自然な動作のメカニズムを工学的に解明することができる。このような研究成果は、人間工学、リハビリテーション工学、あるいはスポーツ科学など、人間の運動メカニズムを扱う他のさまざまな研究分野の進展に大いに還元することができるであろう。
【０００９】
もう１つは、人間のパートナーとして生活を支援する、すなわち住環境その他の日常生活上の様々な場面における人的活動の支援を行なう実用ロボットの開発である。この種のロボットは、人間の生活環境のさまざまな局面において、人間から教わりながら個々に個性の相違する人間又は環境への適応方法を学習し、機能面でさらに成長していく必要がある。このとき、ロボットが「人間形」すなわち人間と同じ形又は同じ構造をしている方が、人間とロボットとの円滑なコミュニケーションを行なう上で有効に機能するものと考えられる。
【００１０】
例えば、踏んではならない障害物を避けながら部屋を通り抜ける方法を実地においてロボットに教示するような場合、クローラ式や４足式ロボットのように教える相手が自分と全く違う構造をしているよりも、同じような格好をしている２足歩行ロボットの方が、ユーザ（作業員）ははるかに教え易く、またロボットにとっても教わり易い筈である（例えば、非特許文献１を参照のこと）。
【００１１】
２足歩行による脚式移動を行なうタイプのロボットに関する姿勢制御や安定歩行に関する技術は既に数多提案されている。ここで言う安定な「歩行」とは、「転倒することなく、脚を使って移動すること」と定義することができる。
【００１２】
ロボットの姿勢安定制御は、ロボットの転倒を回避する上で非常に重要である。何故ならば、転倒は、ロボットが実行中の作業を中断することを意味し、且つ、転倒状態から起き上がって作業を再開するために相当の労力や時間が払われるからである。また、何よりも、転倒によって、ロボット本体自体、あるいは転倒するロボットと衝突する相手側の物体にも、致命的な損傷を与えてしまう危険があるからである。したがって、脚式移動ロボットの設計・開発において、歩行やその他の脚式作業時における姿勢安定制御は最も重要な技術的課題の１つである。
【００１３】
歩行時には、重力と歩行運動に伴なって生じる加速度によって、歩行系から路面には重力と慣性力、並びにこれらのモーメントが作用する。いわゆる「ダランベールの原理」によると、それらは路面から歩行系への反作用としての床反力、床反力モーメントとバランスする。力学的推論の帰結として、足底接地点と路面の形成する支持多角形の内側にピッチ及びロール軸モーメントがゼロとなる点、すなわち「ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）」が存在する。
【００１４】
脚式移動ロボットの姿勢安定制御や歩行時の転倒防止に関する提案の多くは、このＺＭＰを歩行の安定度判別の規範として用いたものである（例えば、非特許文献２を参照のこと）。ＺＭＰ規範に基づく２足歩行パターン生成は、足底着地点をあらかじめ設定することができ、路面形状に応じた足先の運動学的拘束条件を考慮し易いなどの利点がある。また、ＺＭＰを安定度判別規範とすることは、力ではなく軌道を運動制御上の目標値として扱うことを意味するので、技術的に実現可能性が高まる。
【００１５】
一般には、４足歩行よりもヒューマノイドのような２足歩行のロボットの方が、重心位置が高く、且つ、歩行時のＺＭＰ安定領域が狭い。したがって、このような路面状態の変化に伴う姿勢変動の問題は、２足歩行ロボットにおいてとりわけ重要となる。
【００１６】
２足歩行ロボットの姿勢安定度判別規範にＺＭＰを用いた提案は既に幾つかある。
【００１７】
例えば、脚式移動ロボットは、ＺＭＰがゼロとなる床面上の点を目標値に一致させるようにして安定歩行を行なうことができる（例えば、特許文献１を参照のこと）。
【００１８】
また、脚式移動ロボットは、ＺＭＰが支持多面体（多角形）内部、又は、着地、離床時にＺＭＰが支持多角形の端部から少なくとも所定の余裕を有する位置にあるように構成することができる。この場合、外乱などを受けても所定距離だけＺＭＰの余裕があり、歩行時の機体の安定性が向上する（例えば、特許文献２を参照のこと）。
【００１９】
また、脚式移動ロボットの歩き速度をＺＭＰ目標位置によって制御することができる。すなわち、あらかじめ設定された歩行パターン・データを用い、ＺＭＰを目標位置に一致させるように脚部関節を駆動するとともに、上体の傾斜を検出してその検出値に応じて設定された歩行パターン・データの吐き出し速度を変更する。未知の凹凸を踏んでロボットが例えば前傾するときは、吐き出し速度を速めることで姿勢を回復することができる。またＺＭＰを目標位置に制御するので、両脚支持期で吐き出し速度を変更しても支障がない（例えば、特許文献３を参照のこと）。
【００２０】
また、脚式移動ロボットの着地位置をＺＭＰ目標位置によって制御することができる。すなわち、同公報に記載の脚式移動ロボットは、ＺＭＰ目標位置と実測位置とのずれを検出し、それを解消するように脚部の一方又は双方を駆動するか、又はＺＭＰ目標位置まわりにモーメントを検出してそれが零になる様に脚部を駆動することで、安定歩行を実現する（例えば、特許文献４を参照のこと）。
【００２１】
また、脚式移動ロボットの傾斜姿勢をＺＭＰ目標位置によって制御することができる。すなわち、ＺＭＰ目標位置まわりのモーメントを検出し、モーメントが生じたときは、それが零になるように脚部を駆動することで安定歩行を行なう（例えば、特許文献５を参照のこと）。
【００２２】
ＺＭＰを安定度判別規範に用いたロボットの姿勢安定度制御は、基本的には足底接地点と路面の形成する支持多角形の辺上あるいはその内側にモーメントがゼロとなる点を探索することにある。
【００２３】
すなわち、ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ方程式を導出して、このＺＭＰ方程式上で現れるモーメント・エラーを打ち消すように機体の目標軌道を修正するようにすればよい。
【００２４】
ＺＭＰ方程式を立てるためには、機体上の制御対象点における位置と加速度を求める必要がある。ＺＭＰを安定度判別規範に用いた従来のロボット機体制御システムの多くは、制御対象点における位置データのみをセンサ入力とし、制御システム内においてこの位置データを２階微分するなどして加速度データを算出してからＺＭＰ方程式を導出していた。
【００２５】
しかしながら、このような計算方法に頼った場合、計算量が多くなり、処理負荷が高くなるとともに演算時間が長くなってしまう。さらに、間接的に加速度データを得ているため、正確な加速度データを用いることができず、跳躍や走行など、高速にリアルタイムで機体の軌道修正を必要とする動作の実現が困難である。また、機体の姿勢制御の厳密性を追及した場合、制御対象点を複数とることが好ましいが、演算時間が過大となってしまい、コスト増大を招来する。
【００２６】
また、脚式ロボットを始めとする移動機械をＺＭＰ方程式に従って厳密に運動制御することを考えると、制御に用いるローカル座標原点の世界座標における加速度と、ローカル座標系における機体各部の位置（姿勢）、加速度、そしてＺＭＰ位置と外力及び外力モーメントを計測し、その計測値をＺＭＰ方程式に導入することで、未知外力モーメント及び未知外力を同定しつつ、各部の位置、加速度を制御することが最も厳密に運動制御を行なうことになる。
【００２７】
例えば、傾斜計（又は加速度計）、及びジャイロを各軸（ピッチ、ロール、ヨー（Ｘ，Ｙ，Ｚ））に１つずつ、６軸力センサの配置位置を、外力及び外力が加わることが想定される部位毎に、実際の作用位置より離れた位置に、最小限の個数のセンサ構成で運動制御を行なうことができる。
【００２８】
ところが、このようなセンサ配置に基づく運動制御方式では、制御に用いるローカル座標原点加速度に加え、すべての部位の位置及び加速度を直接的に計測し、制御することは困難である。
【００２９】
従来の運動制御方式は、
（１）ロボットの外部環境は、どんな力やトルクが作用しても動くことがない。
（２）ロボットの外部環境での併進に対する摩擦係数は十分大きく、滑りが生じない。
（３）ロボットは、どんな力やトルクが作用しても変形することがない。
という条件を前提としたものである。このため、力やトルクが作用すると路面が動いてしまう砂利上や毛足の長い絨毯上、そして、併進の摩擦係数が十分に確保できずすべりが生じ易い住居内のタイルなどでの安定歩行（運動）や、ロボット自身の構造に柔軟性を持たせることで跳躍を伴う全身運動の実現を目指したロボットの運動制御を保証するものではない。
【００３０】
【特許文献１】特開平５−３０５５７９号公報
【特許文献２】特開平５−３０５５８１号公報
【特許文献３】特開平５−３０５５８３号公報
【特許文献４】特開平５−３０５５８５号公報
【特許文献５】特開平５−３０５５８６号公報
【非特許文献１】高西著「２足歩行ロボットのコントロール」（自動車技術会関東支部＜高塑＞Ｎｏ．２５，１９９６ ＡＰＲＩＬ）
【非特許文献２】Ｍｉｏｍｉｒ Ｖｕｋｏｂｒａｔｏｖｉｃ著“ＬＥＧＧＥＤ ＬＯＣＯＭＯＴＩＯＮ ＲＯＢＯＴＳ”（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社））
【発明の開示】
【発明が解決しようとする課題】
【００３１】
本発明の目的は、ＺＭＰを姿勢安定度判別規範に用いて運動中の機体の姿勢を安定化制御することができる、優れた移動体装置を提供することにある。
【００３２】
本発明のさらなる目的は、ＺＭＰ方程式を高速且つ高精度に導き出すことによってより厳密な姿勢安定制御を行なうことを可能にする、優れた移動体装置を提供することにある。
【００３３】
本発明のさらなる目的は、機体上の各部に設置されたセンサからの計測値に基づいて導入されたＺＭＰ方程式又は運動方程式を用いて未知外力モーメント及び未知外力を同定して運動制御を好適に行なうことができる、優れた移動体装置を提供することにある。
【課題を解決するための手段】
【００３４】
本発明は、上記課題を参酌してなされたものであり、基体と、前記基体に接続される１以上の可動部を有する移動体装置であって、
前記可動部の１つのローカル座標系を設定し、前記ローカル座標系が設定された可動部に設置された第１の加速度センサと、
前記ローカル座標系内にある前記移動体装置の１以上の制御対象点に設置された第２の加速度センサと、
少なくとも前記第１の加速度センサ及び前記第２の加速度センサから得られる加速度情報に基づいて、前記移動体装置の各制御点における加速度を制御することにより、前記移動体装置の可動部を制御する制御手段と、
を具備することを特徴とする移動体装置である。
【００３５】
ここで、前記移動体装置は、少なくとも上肢、下肢、及び体幹部を含んだ可動部を備えたロボット装置であり、少なくとも前記上肢、前記下肢、前記体幹部毎に制御対象点が設けられている。
【００３６】
そして、前記ローカル座標系と前記第１の加速度センサを、前記移動体装置と路面との接触部位に設ける。例えば、前記ローカル座標系と前記第１の加速度センサを前記足部に設ける。あるいは、前記ローカル座標系と前記第１の加速度センサを前記腰部に設ける。
【００３７】
このような場合、少なくとも前記第１の加速度センサ及び前記第２の加速度センサから得られる加速度情報に基づいて導出されるＺＭＰ方程式又は運動方程式を用いて、前記移動体装置に印加される未知モーメント又は未知外力を算出することができる。したがって、前記制御手段は、該算出された未知モーメント又は未知外力に基づいて前記移動体装置の可動部を制御することができる。
【００３８】
ロボット装置の姿勢安定性を確保するためには、例えばＺＭＰ方程式や運動方程式などの安定度判別規範となる方程式を導入し、装置本体に印加される未知モーメントや未知外力を打ち消すように動作制御を行なう必要がある。支持多角形の内側にＺＭＰがある場合は、系に回転運動や並進運動が発生せず、回転や並進に関する運動方程式を解く必要がなく、系が形成する適切なＺＭＰ空間を用いてＺＭＰ方程式を解くことにより、姿勢安定制御を行なう。また、支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合は、ＺＭＰ方程式に代えて、運動方程式を解くことにより、姿勢安定制御を行なう。また、跳躍を伴うダンスなど、すべての部位の軌道について優先度が一様に高く設定される場合には、ＺＭＰ方程式と運動方程式の両方を解く場合がある。
【００３９】
ここで、方程式を立てるためには、機体上の各制御対象点における位置と加速度を求める必要がある。しかしながら、制御対象点における位置データのみをセンサ入力とした制御システムの場合、位置データを２階微分するなどして加速度データを算出してから方程式を導出しなければならない。この場合、計算量が多く、処理負荷の増大や演算時間の問題がある。また、間接的に加速度データを得ているため、正確な加速度データを用いることができないので、高速にリアルタイムで機体の軌道修正を必要とする動作の実現が困難である。
【００４０】
これに対し、本発明に係るロボット装置の場合、装置本体の複数の箇所に設定された制御対象点毎に加速度センサが配設されるとともに、床面に接床する足底などロボットと外界との接触部位における反力を計測する反力センサが配設されているので、正確な加速度データや反力データを直接用いてＺＭＰ方程式や運動方程式を導出することができるとともに、方程式導出のための計算量を削減することができる。この結果、跳躍や走行など高速性が要求される動作においても好適に軌道修正を行なうことが可能となる。
【００４１】
例えば、前記ロボットの機体上の制御対象点における加速度と、前記ロボットと外界との接触部位における反力を計測する。そして、これら計測結果を基に、前記ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ方程式を生成し、該ＺＭＰ方程式上で現れるモーメント・エラーを打ち消すように機体の目標軌道を修正することができる。
【００４２】
ＺＭＰを安定度判別規範に用いたロボットの姿勢安定度制御は、基本的には足底接地点と路面の形成する支持多角形の内側にモーメントがゼロとなる点を探索することにある。すなわち、ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ方程式を導出して、このＺＭＰ方程式上で現れるモーメント・エラーを打ち消すように機体の目標軌道を修正する。
【００４３】
例えば、制御に用いる機体のローカル座標原点の世界座標における加速度と、ローカル座標系における機体の各制御対象点の位置（姿勢）、加速度、並びにＺＭＰ位置と外力モーメントを計測して、各点における位置及び加速度を制御することにより、最も厳密に機体制御を行なうことができる。
【００４４】
一方、このような原理に従い、制御に用いるローカル座標原点加速度に加え、すべての部位の位置及び加速度を直接的に計算して機体制御を行なうことはコストが過大であり、また、計測系を配置するための収容場所が問題となる。
【００４５】
そこで、本発明によれば、ロボットの機体上の制御対象点として質量操作量が最大となる部位、例えば腰部をローカル座標原点に設定する。そして、この制御対象点に加速度センサなどの計測手段を配置して、その位置における姿勢や加速度を直接計測して、ＺＭＰに基づく姿勢安定制御を行なうことができる。
【００４６】
他方、質量操作量が大きな部位を制御対象点に設定した場合、足部の状態は、世界座標系で直接計測するものではなく、この制御対象点の計算結果を基に相対的に算出されるものである。このため、足部と路面との間では以下の条件を満たすことが、前提となってしまう。
【００４７】
（１）路面はどんな力やトルクが作用しても動くことがない。
（２）路面での並進に対する摩擦係数は充分に大きく、滑りが生じない。
【００４８】
例えば、力やトルクが作用すると路面が動いてしまう砂利上や毛足の長い絨毯上、あるいは、並進の摩擦係数が充分に確保できずに滑りが生じ易い住居のタイルなどでの安定歩行（運動）を保証することができない。
【００４９】
そこで、本発明では、路面との接触部位である足部にＺＭＰと力を直接計測する反力センサ・システム（床反力センサなど）を配設するとともに、制御に用いるローカル座標とその座標を直接的に計測するための加速度センサを配設することとした。
【００５０】
この結果、ＺＭＰ位置に最も近い足部で直接ＺＭＰ方程式を組み立てることができ、上述したような前提条件に依存しない、より厳密な姿勢安定制御を高速で実現することができる。
【００５１】
また、さらに多くの質量操作量を制御システムに組み込むことができ、主に動作の安定性に用いる部位（腰部）に配設された加速度センサ及び姿勢センサによる直接計測結果との協働的作用により、上述したような前提条件に依存しないような脚式移動ロボットの姿勢安定制御を実現することができる。
【００５２】
なお、機体上に分散配置された前記加速度センサ同士が直列的に接続することにより、個々の制御点において前記加速度センサの加速度情報に基づいて算出される前記ＺＭＰ方程式又は運動方程式上のモーメント項や外力項を、接続経路に従って各制御対象点において順次加算していくことによって、これらの総和を効率的に計算することができる。したがって、接続経路に従って、このような加算処理を逐次繰り返していくことにより、中央コントローラに演算結果が到達するときには、ＺＭＰ方程式を構成するモーメント項又は運動方程式を構成する並進力項が既に求められているので、これら方程式を利用した機体の姿勢安定制御を効率的且つ高速に実現することができる。
【発明の効果】
【００５３】
本発明によれば、ＺＭＰを姿勢安定度判別規範に用いて運動中の機体の姿勢を安定化制御することができる、優れた移動体装置を提供することができる。
【００５４】
また、本発明によれば、ＺＭＰ方程式又は運動方程式を高速且つ高精度に導き出すことによってより厳密な姿勢安定制御を行なうことを可能にする、優れた移動体装置を提供することができる。
【００５５】
また、本発明によれば、機体上の各部に設置されたセンサからの計測値に基づいて導入されたＺＭＰ方程式又は運動方程式を用いて未知外力モーメント及び未知外力を同定して運動制御を好適に行なうことができる、優れた移動体装置を提供することができる。
【００５６】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【発明を実施するための最良の形態】
【００５７】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００５８】
Ａ．脚式移動ロボットの機械的構成
図１及び図２には本発明の実施に供される「人間形」又は「人間型」の脚式移動ロボット１００が直立している様子を前方及び後方の各々から眺望した様子を示している。図示の通り、脚式移動ロボット１００は、胴体部と、頭部と、左右の上肢部と、脚式移動を行なう左右２足の下肢部とで構成され、例えば胴体に内蔵されている制御部（図示しない）により機体の動作を統括的にコントロールするようになっている。
【００５９】
左右各々の下肢は、大腿部と、膝関節と、脛部と、足首と、足平とで構成され、股関節によって体幹部の略最下端にて連結されている。また、左右各々の上肢は、上腕と、肘関節と、前腕とで構成され、肩関節によって体幹部の上方の左右各側縁にて連結されている。また、頭部は、首関節によって体幹部の略最上端中央に連結されている。
【００６０】
制御部は、この脚式移動ロボット１００を構成する各関節アクチュエータの駆動制御や各センサ（後述）などからの外部入力を処理するコントローラ（主制御部）や、電源回路その他の周辺機器類を搭載した筐体である。制御部は、その他、遠隔操作用の通信インターフェースや通信装置を含んでいてもよい。
【００６１】
このように構成された脚式移動ロボット１００は、制御部による全身協調的な動作制御により、２足歩行を実現することができる。かかる２足歩行は、一般に、以下に示す各動作期間に分割される歩行周期を繰り返すことによって行なわれる。すなわち、
【００６２】
（１）右脚を持ち上げた、左脚による単脚支持期
（２）右足が接地した両脚支持期
（３）左脚を持ち上げた、右脚による単脚支持期
（４）左足が接地した両脚支持期
【００６３】
脚式移動ロボット１００における歩行制御は、あらかじめ下肢の目標軌道を計画し、上記の各期間において計画軌道の修正を行なうことによって実現される。すなわち、両脚支持期では、下肢軌道の修正を停止して、計画軌道に対する総修正量を用いて腰の高さを一定値で修正する。また、単脚支持期では、修正を受けた脚の足首と腰との相対位置関係を計画軌道に復帰させるように修正軌道を生成する。
【００６４】
歩行動作の軌道修正を始めとして、機体の姿勢安定制御には、一般に、ＺＭＰに対する偏差を小さくするための位置、速度、及び加速度が連続となるように、５次多項式を用いた補間計算により行なう。ＺＭＰを歩行の安定度判別の規範として用いている。ＺＭＰによる安定度判別規範は、歩行系から路面には重力と慣性力、並びにこれらのモーメントが路面から歩行系への反作用としての床反力並びに床反力モーメントとバランスするという「ダランベールの原理」に基づく。力学的推論の帰結として、足底接地点と路面の形成する支持多角形の内側にピッチ軸及びロール軸モーメントがゼロとなるＺＭＰが存在する。
【００６５】
図３には、この脚式移動ロボット１００が具備する関節自由度構成を模式的に示している。同図に示すように、脚式移動ロボット１００は、２本の腕部と頭部１を含む上肢と、移動動作を実現する２本の脚部からなる下肢と、上肢と下肢とを連結する体幹部とで構成された、複数の肢を備えた構造体である。
【００６６】
頭部を支持する首関節（Ｎｅｃｋ）は、首関節ヨー軸１と、第１及び第２の首関節ピッチ軸２ａ及び２ｂと、首関節ロール軸３という３自由度を有している。
【００６７】
また、各腕部は、その自由度として、肩（Ｓｈｏｕｌｄｅｒ）における肩関節ピッチ軸４と、肩関節ロール軸５と、上腕ヨー軸６、肘（Ｅｌｂｏｗ）における肘関節ピッチ軸７と、手首（Ｗｒｉｓｔ）における手首関節ヨー軸８と、手部とで構成される。手部は、実際には、複数本の指を含む多関節・多自由度構造体である。
【００６８】
また、体幹部（Ｔｒｕｎｋ）は、体幹ピッチ軸９と、体幹ロール軸１０という２自由度を有する。
【００６９】
また、下肢を構成する各々の脚部は、股関節（Ｈｉｐ）における股関節ヨー軸１１と、股関節ピッチ軸１２と、股関節ロール軸１３と、膝（Ｋｎｅｅ）における膝関節ピッチ軸１４と、足首（Ａｎｋｌｅ）における足首関節ピッチ軸１５と、足首関節ロール軸１６と、足部とで構成される。
【００７０】
但し、エンターティンメント向けの脚式移動ロボット１００が上述したすべての自由度を装備しなければならない訳でも、あるいはこれに限定される訳でもない。設計・製作上の制約条件や要求仕様などに応じて、自由度すなわち関節数を適宜増減することができることは言うまでもない。
【００７１】
上述したような脚式移動ロボット１００が持つ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、２足歩行という不安定構造体に対して姿勢制御を行なうことなどの要請から、アクチュエータは小型且つ軽量であることが好ましい。本実施形態では、ギア直結型で且つサーボ制御系をワンチップ化してモータ・ユニットに内蔵したタイプの小型ＡＣサーボ・アクチュエータを搭載することとした（この種のＡＣサーボ・アクチュエータに関しては、例えば本出願人に既に譲渡されている特開２０００−２９９９７０号公報に開示されている）。本実施形態では、直結ギアとして低減速ギアを採用することにより、人間との物理的インタラクションを重視するタイプのロボット１００に求められている駆動系自身の受動的特性を得ている。
【００７２】
Ｂ．脚式移動ロボットの制御システム構成
図４には、脚式移動ロボット１００の制御システム構成を模式的に示している。同図に示すように、脚式移動ロボット１００は、ヒトの四肢を表現した各機構ユニット３０、４０、５０Ｒ／Ｌ、６０Ｒ／Ｌと、各機構ユニット間の協調動作を実現するための適応制御を行なう制御ユニット８０とで構成される（但し、Ｒ及びＬの各々は、右及び左の各々を示す接尾辞である。以下同様）。
【００７３】
脚式移動ロボット１００全体の動作は、制御ユニット８０によって統括的に制御される。制御ユニット８０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）やメモリなどの主要回路コンポーネント（図示しない）で構成される主制御部８１と、電源回路やロボット１００の各構成要素とのデータやコマンドの授受を行なうインターフェース（いずれも図示しない）などを含んだ周辺回路８２とで構成される。
【００７４】
本発明を実現する上で、この制御ユニット８０の設置場所は特に限定されない。図４では体幹部ユニット４０に搭載されているが、頭部ユニット３０に搭載してもよい。あるいは、脚式移動ロボット１００外に制御ユニット８０を配備して、脚式移動ロボット１００の機体とは有線若しくは無線で交信するようにしてもよい。
【００７５】
図３に示した脚式移動ロボット１００内の各関節自由度は、それぞれに対応するアクチュエータによって実現される。すなわち、頭部ユニット３０には、首関節ヨー軸１、首関節ピッチ軸２、首関節ロール軸３の各々を表現する首関節ヨー軸アクチュエータＡ1、首関節ピッチ軸アクチュエータＡ2、首関節ロール軸アクチュエータＡ3が配設されている。
【００７６】
また、体幹部ユニット４０には、体幹ピッチ軸９、体幹ロール軸１０の各々を表現する体幹ピッチ軸アクチュエータＡ9、体幹ロール軸アクチュエータＡ10が配設されている。
【００７７】
また、腕部ユニット５０Ｒ／Ｌは、上腕ユニット５１Ｒ／Ｌと、肘関節ユニット５２Ｒ／Ｌと、前腕ユニット５３Ｒ／Ｌに細分化されるが、肩関節ピッチ軸４、肩関節ロール軸５、上腕ヨー軸６、肘関節ピッチ軸７、手首関節ヨー軸８の各々を表現する肩関節ピッチ軸アクチュエータＡ4、肩関節ロール軸アクチュエータＡ5、上腕ヨー軸アクチュエータＡ6、肘関節ピッチ軸アクチュエータＡ7、手首関節ヨー軸アクチュエータＡ8が配設されている。
【００７８】
また、脚部ユニット６０Ｒ／Ｌは、大腿部ユニット６１Ｒ／Ｌと、膝ユニット６２Ｒ／Ｌと、脛部ユニット６３Ｒ／Ｌに細分化されるが、股関節ヨー軸１１、股関節ピッチ軸１２、股関節ロール軸１３、膝関節ピッチ軸１４、足首関節ピッチ軸１５、足首関節ロール軸１６の各々を表現する股関節ヨー軸アクチュエータＡ11、股関節ピッチ軸アクチュエータＡ12、股関節ロール軸アクチュエータＡ13、膝関節ピッチ軸アクチュエータＡ14、足首関節ピッチ軸アクチュエータＡ15、足首関節ロール軸アクチュエータＡ16が配設されている。
【００７９】
各関節に用いられるアクチュエータＡ1、Ａ2、Ａ3…は、より好ましくは、ギア直結型で且つサーボ制御系をワンチップ化してモータ・ユニット内に搭載したタイプの小型ＡＣサーボ・アクチュエータ（前述）で構成することができる。
【００８０】
頭部ユニット３０、体幹部ユニット４０、腕部ユニット５０、各脚部ユニット６０などの機構ユニット毎に、アクチュエータ駆動制御用の副制御部３５、４５、５５、６５が配設されている。
【００８１】
機体の体幹部４０には、加速度センサ９５と姿勢センサ９６が配設されている。加速度センサ９５は、Ｘ、Ｙ、Ｚ各軸方向に配置する。機体の腰部に加速度センサ９５を配設することによって、質量操作量が大きな部位である腰部を制御対象点として設定して、その位置における姿勢や加速度を直接計測して、ＺＭＰに基づく姿勢安定制御を行なうことができる。
【００８２】
また、各脚部６０Ｒ及び６０Ｌには、接地確認センサ９１及び９２と、加速度センサ９３及び９４がそれぞれ配設されている。接地確認センサ９１及び９２は、例えば足底に圧力センサを装着することにより構成され、床反力の有無により足底が着床したか否かを検出することができる。また、加速度センサ９３及び９４は、少なくともＸ及びＹの各軸方向に配置する。左右の足部に加速度センサ９３及び９４を配設することにより、ＺＭＰ位置に最も近い足部で直接ＺＭＰ方程式を組み立てることができる。
【００８３】
質量操作量が大きな部位である腰部にのみ加速度センサを配置した場合、腰部のみが制御対象点に設定され、足部の状態は、この制御対象点の計算結果を基に相対的に算出しなければならず、足部と路面との間では以下の条件を満たすことが、前提となってしまう。
【００８４】
（１）路面はどんな力やトルクが作用しても動くことがない。
（２）路面での並進に対する摩擦係数は充分に大きく、滑りが生じない。
【００８５】
これに対し、本実施形態では、路面との接触部位である足部にＺＭＰと力を直接計測する反力センサ・システム（床反力センサなど）を配備するとともに、制御に用いるローカル座標とその座標を直接的に計測するための加速度センサを配設する。この結果、ＺＭＰ位置に最も近い足部で直接ＺＭＰ方程式を組み立てることができ、上述したような前提条件に依存しない、より厳密な姿勢安定制御を高速で実現することができる。この結果、力やトルクが作用すると路面が動いてしまう砂利上や毛足の長い絨毯上や、並進の摩擦係数が充分に確保できずに滑りが生じ易い住居のタイルなどであっても、機体の安定歩行（運動）を保証することができる。
【００８６】
主制御部８０は、各センサ９１〜９３の出力に応答して制御目標をダイナミックに補正することができる。より具体的には、副制御部３５、４５、５５、６５の各々に対して適応的な制御を行ない、脚式移動ロボット１００の上肢、体幹、及び下肢が協調して駆動する全身運動パターンを実現する。
【００８７】
ロボット１００の機体上での全身運動は、足部運動、軌道、体幹運動、上肢運動、腰部高さなどを設定するとともに、これらの設定内容に従った動作を指示するコマンドを各副制御部３５、４５、５５、６５に転送する。そして、各々の副制御部３５、４５…では、主制御部８１からの受信コマンドを解釈して、各アクチュエータＡ1、Ａ2、Ａ3…に対して駆動制御信号を出力する。ここで、ＺＭＰは歩行中の床反力によるモーメントがゼロとなる床面上の点のことであり、「ＺＭＰ軌道」とは、例えばロボット１００の歩行動作期間中にＺＭＰが動く軌跡を意味する。
【００８８】
Ｃ．脚式移動ロボットの姿勢安定制御
次いで、本実施形態に係る脚式移動ロボット１００における、脚式作業時すなわち足部、腰、体幹、下肢運動などからなる全身協調運動の実行時における姿勢の安定化処理の手順について説明する。
【００８９】
本実施形態に係る姿勢安定制御は、ＺＭＰを安定度判別規範に用いる。ＺＭＰを安定度判別規範に用いたロボットの姿勢安定度制御は、基本的には足底接地点と路面の形成する支持多角形の内側にモーメントがゼロとなる点を探索することにある。すなわち、ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ方程式を導出して、このＺＭＰ方程式上で現れるモーメント・エラーを打ち消すように機体の目標軌道を修正する。
【００９０】
本実施形態では、ロボットの機体上の制御対象点として質量操作量が最大となる部位、例えば腰部をローカル座標原点に設定する。そして、この制御対象点に加速度センサなどの計測手段を配置して、その位置における姿勢や加速度を直接計測して、ＺＭＰに基づく姿勢安定制御を行なう。さらに路面との接触部位である足部に加速度センサを配備することにより、制御に用いるローカル座標とその座標を直接的に計測して、ＺＭＰ位置に最も近い足部で直接ＺＭＰ方程式を組み立てる。
【００９１】
Ｃ−１．ＺＭＰ方程式の導入
本実施形態に係る脚式移動ロボット１００は無限のすなわち連続的な質点の集合体である。但し、ここでは有限数で離散的な質点からなる近似モデルに置き換えることによって、安定化処理のための計算量を削減するようにしている。より具体的には物理的には図３に示す多関節自由度構成を具備する脚式移動ロボット１００を、図５に示すように多質点近似モデルに置き換えて取り扱う。図示の近似モデルは、線形且つ非干渉の多質点近似モデルである。
【００９２】
図５において、Ｏ−ＸＹＺ座標系は絶対座標系におけるロール、ピッチ、ヨー各軸を表し、また、Ｏ'−Ｘ'Ｙ'Ｚ'座標系はロボット１００とともに動く運動座標系におけるロール、ピッチ、ヨー各軸を表している。但し、図中におけるパラメータの意味は以下の通りである。また、ダッシュ（´）付きの記号は運動座標系を記述するものと理解されたい。
【００９３】
【数１】
0003568527.tif 000002
【００９４】
同図に示す多質点モデルでは、ｉはｉ番目に与えられた質点を表す添え字であり、ｍiはｉ番目の質点の質量、ｒ'iはｉ番目の質点の位置ベクトル（但し運動座標系）を表すものとする。本実施形態に係る脚式移動ロボット１００の機体重心は腰部付近に存在する。すなわち、腰部は、質量操作量が最大となる質点であり、図５では、その質量はｍh、その位置ベクトル（但し運動座標系）はｒ'h（ｒ'hx，ｒ'hy，ｒ'hz）とする。また、機体のＺＭＰの位置ベクトル（但し運動座標系）をｒ'zmp（ｒ'zmpx，ｒ'zmpy，ｒ'zmpz）とする。
【００９５】
世界座標系Ｏ−ＸＹＺは絶対座標系であり、不変である。本実施形態に係る脚式移動ロボット１００は、腰部と両脚の足部にそれぞれ加速度センサ９３、９４、９６が配置されており、これらセンサ出力により腰部並びに立脚それぞれと世界座標系の相対位置ベクトルｒqが検出される。これに対し、運動座標系すなわち機体のローカル座標系はＯ−Ｘ’Ｙ’Ｚ’は、ロボットともに動く。
【００９６】
多質点モデルは、言わば、ワイヤフレーム・モデルの形態でロボットを表現したものである。図５を見ても判るように、多質点近似モデルは、両肩、両肘、両手首、体幹、腰部、及び、両足首の各々を質点として設定される。図示の非厳密の多質点近似モデルにおいては、モーメント式は線形方程式の形式で記述され、該モーメント式はピッチ軸及びロール軸に関して干渉しない。多質点近似モデルは、概ね以下の処理手順により生成することができる。
【００９７】
（１）ロボット１００全体の質量分布を求める。
（２）質点を設定する。質点の設定方法は、設計者のマニュアル入力であっても、所定の規則に従った自動生成のいずれでも構わない。
（３）領域ｉ毎に、重心を求め、その重心位置と質量ｍiを該当する質点に付与する。
（４）各質点ｍiを、質点位置ｒiを中心とし、その質量に比例した半径に持つ球体として表示する。
（５）現実に連結関係のある質点すなわち球体同士を連結する。
【００９８】
なお、図６に示す多質点モデルにおいて、基体すなわち腰部情報における各回転角（θhx，θhy，θhz）は、脚式移動ロボット１００における腰部の姿勢すなわちロール、ピッチ、ヨー軸の回転を規定するものである。
【００９９】
機体のＺＭＰ方程式は、制御対象点において印加される各モーメントの釣合い関係を記述したものである。図６に示したように、機体を多数の質点ｍiで表わし、これらを制御対象点とした場合、すべての制御対象点ｍiにおいて印加されるモーメントの総和を求める式がＺＭＰ方程式である。
【０１００】
世界座標系（Ｏ−ＸＹＺ）で記述された機体のＺＭＰ方程式、並びに機体のローカル座標系（Ｏ−Ｘ’Ｙ’Ｚ’）はそれぞれ以下の通りとなる。
【０１０１】
【数２】
0003568527.tif 000003
【０１０２】
上式は、各質点ｍiにおいて印加された加速度成分により生成されるＺＭＰ回り（半径ｒi−ｒzmp）のモーメントの総和と、各質点ｍiに印加された外力モーメントＭiの総和と、外力Ｆkにより生成されるＺＭＰ回り（ｋ番目の外力Ｆkの作用点をｓkとする）のモーメントの総和が釣り合うということを記述している。
【０１０３】
このＺＭＰ釣合い方程式は、総モーメント補償量すなわちモーメント・エラー成分Ｔを含んでいる。このモーメント・エラーをゼロ又は所定の許容範囲内に抑えることによって、機体の姿勢安定性が維持される。言い換えれば、モーメント・エラーをゼロ又は許容値以下となるように機体運動（足部運動や上半身の各部位の軌道）を修正することが、ＺＭＰを安定度判別規範とした姿勢安定制御の本質である。
【０１０４】
本実施形態では、腰部と左右の足部にそれぞれ加速度センサ９６、９３及び９４が配設されているので、これらの制御対象点における加速度計測結果を用いて直接的に且つ高精度に上記のＺＭＰ釣合い方程式を導出することができる。この結果、高速でより厳密な姿勢安定制御を実現することができる。
【０１０５】
Ｃ−２．全身協調型の姿勢安定制御
図７には、脚式移動ロボット１００において安定歩行可能な機体運動を生成するための処理手順をフローチャートの形式で示している。但し、以下の説明では、図５及び図６に示すような線形・非干渉多質点近似モデルを用いて脚式移動ロボット１００の各関節位置や動作を記述するものとする。
【０１０６】
まず、足部運動の設定を行なう（ステップＳ１）。足部運動は、２以上の機体のポーズを時系列的に連結されてなるモーション・データである。
【０１０７】
モーション・データは、例えば、足部の各関節角の変位を表わした関節空間情報と、関節位置を表わしたデカルト空間情報で構成される。モーション・データは、コンソール画面上での手付け入力や、機体へのダイレクト・ティーチング（直接教示）、あるいはモーション編集用のオーサリング・システム上で構築したりすることができる。
【０１０８】
次いで、設定された足部運動を基にＺＭＰ安定領域を算出する（ステップＳ２）。ＺＭＰは、機体に印加されるモーメントがゼロとなる点であり、基本的には足底接地点と路面の形成する支持多角形の内側に存在する。ＺＭＰ安定領域は、この支持多角形のさらに内側に設定された領域であり、該領域にＺＭＰを収容させることによって機体を高度に安定した状態にすることができる。
【０１０９】
そして、足部運動とＺＭＰ安定領域を基に、足部運動中におけるＺＭＰ軌道を設定する（ステップＳ３）。
【０１１０】
また、機体の上半身（股関節より上側）の各部位については、腰部、体幹部、上肢、頭部などのようにグループ設定する（ステップＳ１１）。
【０１１１】
そして、部位グループ毎に希望軌道を設定する（ステップＳ１２）。上半身における希望軌道の設定は、足部の場合と同様に、コンソール画面上での手付け入力や、機体へのダイレクト・ティーチング（直接教示）、あるいはモーション編集用のオーサリング・システム上で構築したりすることができる。
【０１１２】
次いで、各部位のグループ設定の調整（再グルーピング）を行ない（ステップＳ１３）、さらにこれらグループに対して優先順位を与える（ステップＳ１４）。
【０１１３】
ここで言う優先順位とは、機体を姿勢安定制御するための処理演算に投入する順位のことであり、例えば質量操作量に応じて割り振られる。この結果、機体上半身についての各部位についての優先順位付き希望軌道群が出来上がる。ロボットの姿勢に応じて、目標軌道間での優先順位の変更を行なうようにしてもよい。
【０１１４】
また、機体上半身の各部位グループ毎に、モーメント補償に利用できる質量を算出しておく（ステップＳ１５）。
【０１１５】
そして、足部運動とＺＭＰ軌道、並びに上半身の各部位グループ毎の希望軌道群を基に、ステップＳ１４により設定された優先順位に従って、各部位グループの運動パターンを姿勢安定化処理に投入する。
【０１１６】
この姿勢安定化処理では、まず、処理変数ｉに初期値１を代入する（ステップＳ２０）。そして、優先順位が先頭からｉ番目までの部位グループについての目標軌道設定時における、目標ＺＭＰ上でのモーメント量すなわち総モーメント補償量を算出する（ステップＳ２１）。目標軌道が算出されていない部位については、希望軌道を用いる。
【０１１７】
次いで、ステップＳ１５において算出された当該部位のモーメント補償に利用できる質量を用いて、そのモーメント補償量を設定して（ステップＳ２２）、モーメント補償量を算出する（ステップＳ２３）。
【０１１８】
次いで、算出されたｉ番目の部位のモーメント補償量を用いて、ｉ番目の部位についてのＺＭＰ方程式を導出して（ステップＳ２４）、当該部位のモーメント補償運動を算出することにより（ステップＳ２５）、優先順位が先頭からｉ番目までの部位についての目標軌道を得ることができる。
【０１１９】
このような処理をすべての部位グループについて行なうことにより、安定運動（例えば歩行）が可能な全身運動パターンが生成される。すなわち、ＺＭＰ方程式（あるいは運動方程式（後述））の解と各部位に与えられた優先順位に従がって、各目標軌道の全部又は一部に対して修正を行なうことで、全身運動パターンが生成される。
【０１２０】
図７に示した機体運動パターン生成の処理手順では、まず足部運動を設定し安定領域を算出しＺＭＰ軌道の設定を行なってから、上半身の各部位における希望軌道の優先順位の設定を行なうように構成されているが、処理順序はこれに限定されない。例えば、上半身の各部位における希望軌道の優先順位を先に設定してから、ＺＭＰ安定領域の算出並びにＺＭＰ軌道の設定を行なうようにしてもよい。前者の場合、先に設定したＺＭＰ軌道に従がって上半身の各部位における希望軌道の優先順位が設定されるのに対し、後者の場合、先に設定された上半身の各部位の希望軌道を維持するように安定領域の算出並びにＺＭＰ軌道が設定される。
【０１２１】
図８には、各部位の希望軌道の優先順位を先に設定してから、ＺＭＰ安定領域の算出並びにＺＭＰ軌道の設定を行なう機体運動の生成処理手順をフローチャートの形式で示している。
【０１２２】
まず、機体の上半身（股関節より上側）の各部位については、腰部、体幹部、上肢、頭部などのようにグループ設定する（ステップＳ３１）。
【０１２３】
そして、部位グループ毎に希望軌道を設定する（ステップＳ３２）。上半身における希望軌道の設定は、足部の場合と同様に、コンソール画面上での手付け入力や、機体へのダイレクト・ティーチング（直接教示）、あるいはモーション編集用のオーサリング・システム上で構築したりすることができる。
【０１２４】
次いで、各部位のグループ設定の調整（再グルーピング）を行ない（ステップＳ３３）、さらにこれらグループに対して優先順位を与える（ステップＳ３４）。
【０１２５】
ここで言う優先順位とは、機体を姿勢安定制御するための処理演算に投入する順位のことであり、例えば質量操作量に応じて割り振られる。この結果、機体上半身についての各部位についての優先順位付き希望軌道群が出来上がる。
【０１２６】
次いで、設定された上半身における希望軌道の優先順位を基に、ＺＭＰ安定領域を算出する（ステップＳ３５）。そして、ＺＭＰ安定領域を基に、足部運動中におけるＺＭＰ軌道を設定する（ステップＳ３６）。
【０１２７】
また、機体上半身の各部位グループ毎に、モーメント補償に利用できる質量を算出しておく（ステップＳ４５）。
【０１２８】
そして、上半身の各部位グループ毎の希望軌道群とＺＭＰ軌道を基に、ステップＳ３４により設定された優先順位に従って、各部位グループの運動パターンを姿勢安定化処理に投入する。
【０１２９】
この姿勢安定化処理では、まず、処理変数ｉに初期値１を代入する（ステップＳ３７）。そして、優先順位が先頭からｉ番目までの部位グループについての目標軌道設定時における、目標ＺＭＰ上でのモーメント量すなわち総モーメント補償量を算出する（ステップＳ３８）。目標軌道が算出されていない部位については、希望軌道を用いる。
【０１３０】
次いで、ステップＳ４５において算出された当該部位のモーメント補償に利用できる質量を用いて、そのモーメント補償量を設定して（ステップＳ３９）、モーメント補償量を算出する（ステップＳ４０）。
【０１３１】
次いで、算出されたｉ番目の部位のモーメント補償量を用いて、ｉ番目の部位についてのＺＭＰ方程式を導出して（ステップＳ４１）、当該部位のモーメント補償運動を算出することにより（ステップＳ４２）、優先順位が先頭からｉ番目までの部位についての目標軌道を得ることができる。
【０１３２】
ここで、図７中のステップＳ１４における希望軌道の優先順位の設定方法について説明する。
【０１３３】
総モーメント補償量をΩ［Ｎｍ］とし、ｉ部のモーメント補償に利用できる質量をＭi［Ｎ］とすると（ｉ＝１，２，３，…，ｎ）、ｉ部のモーメント補償量はαi×Ω［Ｎｍ］となる。但し、αiは絶対モーメント補償量係数であり、相対モーメント補償量係数βiを用いて、下式のように表される。
【０１３４】
【数３】
0003568527.tif 000004
【０１３５】
補償量係数が０より離れるほど希望軌道の優先順位が下がる。正方向が運動の安定化に作用し、負方向が運動の安定化と逆方向に作用する。
【０１３６】
以下、上半身における希望軌道の優先順位の設定方法について、具体例を参照しながら説明する。
【０１３７】
図９に示すように、手で台車を運ぶような運動パターンにおいては、手部の軌道の優先度が高まる。優先度の設定例として、手部のα＝０．０とし、残りの部位のαの合計を１．０とする。
【０１３８】
図１０に示すように、ゴルフ・クラブ（あるいは野球のバット）を両手で持ってスイングするような運動パターンにおいては、手部、足部の軌道の順で優先度が設定される。優先度の設定例として、手部のα＝０．０とし、足部のα＝０．１とし、残りの部位のαの合計を０．９とする。
【０１３９】
図１１に示すように、機械体操におけるあん馬競技を行なうような運動パターンにおいては、両手のみで身体を支持することと脚部の姿勢が重要視されることから、手部と、体幹部と下肢の相対関係の軌道についての優先度が高く設定される。優先度の設定例として、手部のα＝０．０とし、体幹と下肢（肩部における軌道）のα＝０．０とし、残りの部位のαの合計を１．０とする。
【０１４０】
図１２に示すように、瓶やグラスなどを載せたトレイを片手で持ってバランスをとりながら歩行するような運動パターンにおいては、手部、体幹部、腰部、足部の軌道の順で優先度が設定される。
【０１４１】
図１３に示すように、逆立ちをするような運動パターンにおいては、両手で全身を支持し姿勢安定を図ることから、手部、体感部、腰部の軌道の順で優先度が設定される。優先度の設定例として、手部のα＝０．０とし、体幹のα＝０．２とし、腰部のα＝０．３とし、残りの部位のαの合計を０．５とする。
【０１４２】
図１４に示すように、複数のコップを載せたトレイの底に棒を立てて、さらに棒の下端を額に載せてバランスをとるという運動パターンにおいては、頭部の起動の優先度が高く設定される。優先度の設定例として、頭部のα＝０．０とし、残りの部位のαの合計を１．０とする。
【０１４３】
図１５に示すように、複数のフラフープを腰部・体幹部の回転運動で支持するような運動パターンにおいては、体幹部の軌道の優先度が高く設定される。優先度の設定例として、体幹部のα＝０．０とし、残りの部位のαの合計を１．０とする。
【０１４４】
図１６に示すように、長い棒を持って走り、高所に設定されたバーを越える棒高跳び競技を行なうような運動パターンにおいては、時間の経過とともに、下肢、腰部、体幹部、上肢…と優先度が変化していく。優先度の設定例として、試技の前記において足部のα＝０．０とし、中期において腰部及び体幹部のα＝０．０とし、後期において上肢のα＝０．０とし、各時期における残りの部位のαの合計を１．０とする。
【０１４５】
図１７に示すように、リボンを持って踊る新体操、球乗り、バレエのような運動パターンにおいては、すべての部位における軌道の優先度が一様に高く設定される。優先度の設定例として、各部のαを共通とし、αの合計を１．０とする。
【０１４６】
図１８に示すように、両腕を広げてバランスをとりながら綱渡りを行なうような運動パターンにおいては、足部、上肢・体幹部の軌道の順で優先度が設定される。優先度の設定例として、足部のα＝０．０とし、上肢及び体幹部のα＝０．１とし、残りの部位のαの合計を０．９とする。
【０１４７】
図１９に示すように、工事中のビルの外壁に沿って組まれた足場を歩くような運動パターンにおいては、上肢・体幹部、足部の軌道の順で優先度が設定される。優先度の設定例として、上肢および体幹部のα＝０．１とし、足部のα＝０．２とし、残りの部位のαの合計を０．７とする。
【０１４８】
Ｄ．機械ハードウェアの変形を考慮した運動制御
これまでの脚式移動ロボット及びその力学的取り扱いは、外力やトルクを受けても変形が非常に小さく、ロボット全体の運動に対して無視できることを前提としていた。つまり、ロボットの各関節間の距離は変化しないことを前提としていたので、ロボット・システムの状態検出センサは、各要素に関して各１つの構成で十分であった。
【０１４９】
しかしながら、今後、走行やより加速度を継続的且つ積極的に用いたダイナミクス・レベルの高い運動を実現しようとすると機械ハードウェア自身の変形をも利用した衝撃緩衝機能が必要となるとともに、より高次の方程式を実時間で高速に解くことが必要になる。
【０１５０】
そこで、この項では、ロボットの各関節間の距離は変化しないという前提条件が不要なセンサ・システム構成方法及びそれを用いた分散型の高速運動制御システムについて提案する。
【０１５１】
なお、本明細書中では、以下の定義に従うものとする（例えば、日本機械学会編「機械系の動力学」（ｐ．３１−３３、オーム社、平成３年３月２５日）を参照のこと）。
【０１５２】
並進運動：  慣性力＝−（重量／重力加速度）×加速度
回転運動：  慣性モーメント＝−極慣性モーメント×角加速度
極慣性モーメント：  回転軸における慣性モーメント
【０１５３】
本実施形態に係る脚式移動ロボットは、ＺＭＰを歩行の安定度判別の規範として用いている。ＺＭＰによる安定度判別規範は、系が適切なＺＭＰ空間を形成し、支持多角形の内側にＺＭＰがある場合は、系に回転運動や並進運動が発生せず、回転や並進に関する運動方程式を解く必要がない。なお、支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合は、ＺＭＰ方程式に代えて、運動方程式を解く必要がある。
【０１５４】
機体のＺＭＰ方程式は、制御対象点において印加される各モーメントの釣合い関係を記述したものである。機体を多数の質点ｍiで表わし、これらを制御対象点とした場合、すべての制御対象点ｍiにおいて印加されるモーメントの総和を求める式がＺＭＰ釣合い方程式である。
【０１５５】
世界座標系（Ｏ−ＸＹＺ）で記述された機体のＺＭＰ釣合い方程式、並びに機体のローカル座標系（Ｏ−Ｘ’Ｙ’Ｚ’）で記述された機体のＺＭＰ釣合い方程式はそれぞれ以下の通りとなる。
【０１５６】
【数４】
0003568527.tif 000005
【０１５７】
上記の各式は、各質点（又は制御点）ｍiにおいて印加された加速度成分により生成されるＺＭＰ回り（半径ｒi−Ｐzmp）のモーメントの総和と、各質点ｍiに印加された外力モーメントＭiの総和と、外力Ｆkにより生成されるＺＭＰ回り（ｋ番目の外力Ｆkの作用点をＳkとする）のモーメントの総和が釣り合うということを記述している。
【０１５８】
このＺＭＰ釣合い方程式は、総モーメント補償量すなわちモーメント・エラー成分Ｔを含んでいる。このモーメント・エラーをゼロ又は所定の許容範囲内に抑えることによって、機体の姿勢安定性が維持される。言い換えれば、モーメント・エラーをゼロ又は許容値以下となるように機体運動（足部運動や上半身の各部位の軌道）を修正することが、ＺＭＰを安定度判別規範とした姿勢安定制御の本質である。
【０１５９】
本実施形態に係る脚式移動ロボットは、外界との接触部位にＺＭＰと力を直接計測する反力センサ・システムを配置するとともに、運動制御に用いるローカル座標とその座標を直接的に計測するための加速度センサや角速度センサを配置し、さらに計算モデルで用いている各ベクトル位置に加速度センサと姿勢センサを配置することで、ＺＭＰ方程式（又は運動方程式）を導入するために必要な制御パラメータを直接的に計測することを可能とすることで、機体が剛体で外力などの印加で変形しないという条件を前提としないで厳密な運動制御を応答性よく実現する。
【０１６０】
本実施形態に係る反力センサ・システムの配置例を以下に挙げておく。
【０１６１】
（１）質量が集中している部位に、加速度センサ、角加速度センサ、角速度センサを搭載する。
（２）各リンクの重心付近に、加速度センサ、角加速度センサ、角速度センサを搭載する。
（３）各アクチュエータの重心付近に、加速度センサ、角加速度センサ、角速度センサを搭載する。
（４）各アクチュエータの重心付近及びアクチュエータを除いたリンクの重心付近に、加速度センサ、角加速度センサ、角速度センサを搭載する。
（５）各アクチュエータの重心付近、バッテリの重心付近、バッテリとアクチュエータを除いたリンクの重心付近に、加速度センサと角加速度センサと角速度センサを搭載する。
【０１６２】
（１）によれば、質量が集中している部位を制御点として、各制御点において印加される加速度成分を制御点毎に直接計測し、これにより生成される制御点におけるＺＭＰ回りのモーメント項、制御点に印加される外力モーメント項、制御点に印加される外力により生成されるＺＭＰ回りのモーメント項を、各部位毎に直接算出することができる。そして、中央の制御ユニットにおいては、各制御点から集められたこれらモーメント項を順次加算してその総和をとることで、より厳密なＺＭＰ釣合い方程式を直接的に導入することができる。また、各制御点毎にモーメント項を直接計測していることから、機体が剛体で外力などの印加で変形しないという条件を前提としないで厳密な運動制御を応答性よく実現することができる。
【０１６３】
ここで言う質量が集中している部位とは、バッテリ重心、制御ユニット重心、リンク重心、アクチュエータ重心、関節軸、その他の質量集中物などが該当する。図２０には、脚式移動ロボットの機体上の質量が集中している部位に加速度、角加速度、角速度センサを搭載している様子を示している。同図に示すように、主要な外界との接触部位として、手のひらと足底に外力センサ及び外力モーメント・センサを搭載している。
【０１６４】
また、（２）によれば、関節を接続する各リンクの重心付近を制御点として、各制御点において印加される加速度成分を制御点毎に直接計測し、これにより生成される制御点におけるＺＭＰ回りのモーメント項、制御点に印加される外力モーメント項、制御点に印加される外力により生成されるＺＭＰ回りのモーメント項を、各部位毎に直接算出することができる。そして、中央の制御ユニットにおいては、各制御点から集められたこれらモーメント項を順次加算してその総和をとることで、より厳密なＺＭＰ釣合い方程式を直接的に導入することができる。また、制御点毎にモーメント項を直接計測していることから、機体が剛体で外力などの印加で変形しないという条件を前提としないで厳密な運動制御を応答性よく実現することができる。
【０１６５】
図２１には、脚式移動ロボットの機体上の各リンクの重心付近に加速度、角加速度、角速度センサを搭載している様子を示している。同図に示すように、主要な外界との接触部位として、手のひらと足底に外力センサ及び外力モーメント・センサを搭載している。
【０１６６】
また、（３）によれば、機体上の主な質量集中部位としての各アクチュエータの重心付近を制御点として、各制御点において印加される加速度成分を制御点毎に直接計測し、これにより生成される制御点におけるＺＭＰ回りのモーメント項、制御点に印加される外力モーメント項、制御点に印加される外力により生成されるＺＭＰ回りのモーメント項を、各部位毎に直接算出することができる。そして、中央の制御ユニットにおいては、各制御点から集められたこれらモーメント項を順次加算してその総和をとることで、より厳密なＺＭＰ釣合い方程式を直接的に導入することができる。また、各制御点毎にモーメント項を直接計測していることから、機体が剛体で外力などの印加で変形しないという条件を前提としないで厳密な運動制御を応答性よく実現することができる。
【０１６７】
図２２には、脚式移動ロボットの機体上の各アクチュエータの重心付近に加速度、角加速度、角速度センサを搭載している様子を示している。同図に示すように、主要な外界との接触部位として、手のひらと足底に外力センサ及び外力モーメント・センサを搭載している。
【０１６８】
なお、上述の（１）〜（５）に示したような分散配置型の反力センサ・システムによれば、各制御点で計測された加速度センサからのセンサ情報に基づいて、実際の回転中心を測定することができる。したがって、機体の設計情報から一意に求まる重心の場合とは相違し、リンクなど機体が外力などによって変形をきたした場合であっても、より正確な機体の重心位置を動的に算出することができる。
【０１６９】
図２３には、本実施形態に係る脚式移動ロボットの運動制御の概略的な処理手順をフローチャートの形式で示している。
【０１７０】
まず、脚式移動ロボットの機体の安定度判別を行なう（ステップＳ５１）。安定度は、機体の支持多角形を参照して、ＺＭＰ位置が安定領域にあるかどうかで判断することができる。
【０１７１】
支持多角形の内側にＺＭＰがある場合は、系に回転運動や並進運動が発生せず、回転や並進に関する運動方程式を解く必要がない。そこで、ステップＳ５２に進み、系が形成する適切なＺＭＰ空間を用いてＺＭＰ方程式を解くことにより、姿勢安定制御を行なう（後述）。
【０１７２】
一方、支持多角形の内側にＺＭＰがない場合や、外界に対する支持作用点が存在しない場合は、ＺＭＰ方程式に代えて、運動方程式を解くことにより（ステップＳ５３）、姿勢安定制御を行なう（後述）。
【０１７３】
なお、跳躍を伴うダンスなど、すべての部位の軌道についての優先度が一様に高く設定される場合には、ＺＭＰ方程式と運動方程式の両方を解く場合がある。
【０１７４】
図２４には、ステップＳ５２における、ＺＭＰ方程式の解法に基づく機体の安定制御の処理手順をフローチャートの形式で示している。
【０１７５】
まず、質量集中部位や各リンクの重心付近、各アクチュエータの重心付近など、制御点毎に配置された加速度センサ、角加速度センサ、角速度センサからのセンサ情報を基に、ＺＭＰを測定し、又は重心を測定する（ステップＳ６１）。外力などの影響で、機体が変形したときには、加速度センサの実測値を基に重心を動的に測定する必要がある。
【０１７６】
次いで、ステップＳ６２〜ステップＳ６９により形成される処理ループにおいて、ＺＭＰ近傍又は重心近傍から順に、各制御点についてのＺＭＰ回りのモーメント項、制御点に印加される外力モーメント項、並びに制御点に印加される外力により生成されるＺＭＰ回りのモーメント項を、制御点に配設されたセンサからの情報に基づいて直接算出するとともに、これらのモーメント項を順次加算していき、これらの総和を求める。
【０１７７】
この結果、ＺＭＰ方程式を用いて、モーメント・エラーＴを算出することができる（ステップＳ７０）。
【０１７８】
次いで、計測された各部の状態量と同定された外力モーメントを初期値として、ＺＭＰ軌道又は重心の回転軌道、そして各部の軌道を再計画する（ステップＳ７１）。
【０１７９】
そして、アクチュエータ・システム群に再計画結果に基づく目標値を送信して、本処理ルーチンを終了する。
【０１８０】
なお、図２４に示す処理手順では、制御点における発生モーメントを算出するｉ系の処理と、制御点において印加される外力モーメントを算出するｊ系の処理と、制御点において外力により生成されるＺＭＰ回りのモーメントを算出するｋ系の処理を含み、ｉ、ｊ、ｋ系の処理がシリアルに進行しているが、パラレルに進行するようにしてもよい（後述）。
【０１８１】
また、図２５には、ステップＳ５３における、運動方程式の解法に基づく機体の安定制御の処理手順をフローチャートの形式で示している。
【０１８２】
まず、ＺＭＰ上の床反力Ｆrを測定する（ステップＳ８１）。
【０１８３】
次いで、ステップＳ８２〜ステップＳ８９により形成される処理ループにおいて、ＺＭＰ近傍又は重心近傍から順に、各制御点に印加される並進力、ＺＭＰ回りのモーメントにより印加される並進力、並びに外力を、制御点に配設されたセンサからの情報に基づいて直接算出するとともに、これらの並進力項を順次加算して、これらの総和を求める。
【０１８４】
この結果、ダランベールの原理から、未知外力Ｆを算出することができる（ステップＳ９０）。
【０１８５】
次いで、計測された各部の情報量と同定された未知外力を初期値として、ＺＭＰ軌道又は重心軌道、そして各部の軌道を再計画する（ステップＳ９１）。
【０１８６】
そして、アクチュエータ・システム群に再計画結果に基づく目標値を送信して、本処理ルーチンを終了する。
【０１８７】
なお、図２５に示す処理手順では、制御点における並進力を算出するｉ系の処理と、制御点の外力モーメントにより生成される並進力を算出するｊ系の処理と、制御点において印加される外力を算出するｋ系の処理を含み、ｉ、ｊ、ｋ系の処理がシリアルに進行しているが、パラレルに進行するようにしてもよい（後述）。
【０１８８】
図２４に示したフローチャートにおけるステップＳ６２〜ステップＳ６９で形成される処理ループでは、ＺＭＰ近傍又は重心近傍から順に、各制御点についてのＺＭＰ回りのモーメント項、制御点に印加される外力モーメント項、並びに制御点に印加される外力により生成されるＺＭＰ回りのモーメント項を、制御点毎に配設されたセンサからの情報に基づいて直接算出するとともに、これらのモーメント項を順次加算していき、これらの総和を求めることによって、ＺＭＰ方程式を効率的に導入することができる。
【０１８９】
同様に、図２５に示したフローチャートにおけるステップＳ８２〜ステップＳ８９で形成される処理ループでは、ＺＭＰ近傍又は重心近傍から順に、各制御点に印加される並進力、ＺＭＰ回りのモーメントにより印加される並進力、並びに外力を、制御点毎に配設されたセンサからの情報に基づいて直接算出するとともに、これらの並進力項を順次加算して、これらの総和を求めることにより、並進・回転の運動方程式を効率的に導入することができる。
【０１９０】
図２０〜図２２を参照しながら説明したように、本実施形態に係る脚式移動ロボットにおいては、制御に用いるローカル座標とその座標を直接的に計測するための加速度センサや角速度センサを制御点毎に配置し、さらに計算モデルで用いる各ベクトル位置に加速度センサと姿勢センサを配置することで、ＺＭＰ方程式（又は運動方程式）の導入に必要な制御パラメータ値を直接的に計測するように構成されている。
【０１９１】
これら機体上に分散配置されたセンサ同士が直列的に接続されている場合、個々の制御点においてセンサ情報を基に算出されるモーメント項や外力項を、接続経路に従って各制御点において順次加算していくことによって、これらの総和を効率的に計算することができる。
【０１９２】
図２２には、脚式移動ロボットの機体上の各アクチュエータの重心付近に加速度、角加速度、角速度センサを搭載している様子を示したが（前述）、図２６には、この場合のセンサ同士を直列的に接続するための一例を示している。
【０１９３】
同図に示すように、左右の上肢、並びに左右の下肢に配設されたセンサ間がそれぞれ独立して、中央の制御ユニットが始点及び終点となるように直列的に接続されている。このような場合、肢毎に、制御点のセンサ情報に基づく計算結果が順次加算され、これらが中央の制御ユニットに戻されて総和が得られ、ここで方程式を導入することができる。
【０１９４】
また、図２７には、センサ同士を直列的に接続するための他の例を示している。同図に示す例では、全身に配設されたセンサ間が、いわば「一筆書き」の形態で、中央の制御ユニットが始点及び終点となるように一列に接続されている。このような配線形態の場合、各制御点におけるセンサ情報に基づく計算結果が制御対象点毎に順次加算されていき、中央の制御ユニットにデータが戻された時点で各項の総和が既に求まっており、制御ユニットでは、容易に方程式を導入することができる。
【０１９５】
また、制御に用いるローカル座標とその座標を直接的に計測するための加速度センサや角速度センサを制御対象点毎に配置し、さらに計算モデルで用いる各ベクトル位置に加速度センサと姿勢センサを配置する一例として、質量が集中する各アクチュエータの重心付近に、加速度、角加速度、角速度センサを搭載するという実装例を既に紹介した。
【０１９６】
図２８には、ユニットの重心付近に加速度、角加速度、角速度センサを搭載した関節アクチュエータの構成例を示している。
【０１９７】
同図に示す関節アクチュエータは、回転子マグネットと、複数相の磁気コイルからなる固定子で構成されるモータ部と、モータ部の出力する回転を加減速するギア・ユニット（ＧＵ）と、モータ部への供給電力を制御する制御部で構成される。
【０１９８】
制御部は例えば印刷配線板で構成され、その略中央には、センサ・ユニットが搭載されている。
【０１９９】
センサ・ユニットは、アクチュエータ・ユニットの２次元重心位置近傍に配置されている。
【０２００】
センサ・ユニットは、１軸〜３軸の加速度センサと、１〜２軸の角速度センサと、３軸の角速度センサの組み合わせで構成される。
【０２０１】
図２９には、図に示した関節アクチュエータの機能構成を模式的に示している。同図に示すように、アクチュエータ１０は、インターフェース部１１と、コマンド処理部１２と、モータ制御部１３と、センサ信号処理部１４を備えている。
【０２０２】
インターフェース部１１は、ホスト・コントローラとの間でインターフェース・プロトコルを実現する。
【０２０３】
コマンド処理部１２は、インターフェース部１２を介して受け取ったホスト・コマンドを処理してモータ制御部１３に伝達したり、モータ制御部１３やセンサ信号処理部１４からのセンサ情報を演算処理してインターフェース部１２経由でホスト・コントローラに返したりする。
【０２０４】
モータ制御部１３は、ホスト・コマンドに従ったモータの回転を実現するための電流信号をモータ・コイル１５にＰＷＭ（Ｐｕｌｓｅ Ｗｉｄｔｈ Ｍｏｄｕｌａｔｉｏｎ）出力し、また、回転子（図示しない）の回転位置を検出する一センサ１６からの角度情報を取得する。
【０２０５】
センサ信号処理部１４は、センサ・ユニットに含まれる加速度センサ（Ｘ〜Ｙ）、ジャイロ・センサ（ピッチ、ロール、ヨー）からのセンサ情報を処理する。
【０２０６】
本実施形態においては、制御点毎に配設されたセンサからの情報に基づいて、ＺＭＰ近傍又は重心近傍から順に、各制御点についてのＺＭＰ回りのモーメント項、制御点に印加される外力モーメント項、並びに制御点に印加される外力により生成されるＺＭＰ回りのモーメント項を直接算出することができる。同様に、制御点毎に配設されたセンサからの情報に基づいて、ＺＭＰ近傍又は重心近傍から順に、各制御点に印加される並進力、ＺＭＰ回りのモーメントにより印加される並進力、並びに外力を直接算出することができる。
【０２０７】
さらに、機体上に分散配置されたセンサ同士が直列的に接続されている場合、個々の制御点においてセンサ情報を基に算出されるモーメント項や外力項を、接続経路に従って各制御点において順次加算していくことによって、これらの総和を効率的に計算することができる。
【０２０８】
図２８及び図２９を参照しながら説明したセンサ内蔵型の関節アクチュエータにおいては、コマンド処理部１２が、センサ信号処理部１４によって信号処理された加速度センサ（Ｘ〜Ｙ）、ジャイロ・センサ（ピッチ、ロール、ヨー）からのセンサ情報を利用して、モーメント項や外力項を、接続経路に従って各制御点において順次加算していくことができる。
【０２０９】
図３０には、各制御点の関節アクチュエータ内で、ＺＭＰ回りのモーメント項、制御点に印加される外力モーメント項、並びに制御点に印加される外力により生成されるＺＭＰ回りのモーメント項を順次加算していく構成を図解している。
【０２１０】
同図に示すように、関節アクチュエータには、接続経路の上位の関節アクチュエータから、ｉ−１番目までの制御点におけるＺＭＰ回りのモーメント項の総和、ｊ−１番目までの制御点における外力モーメント項の総和、並びにｋ−１番目までの制御点における外力により生成されるＺＭＰ回りのモーメント項の総和が入力される。そして、関節アクチュエータ内で検出されたセンサ情報に基づいて、当該制御点におけるＺＭＰ回りのモーメント項、制御点に印加される外力モーメント項、並びに制御点に印加される外力により生成されるＺＭＰ回りのモーメント項を算出するとともに、これらをそれぞれの総和に加算処理して、ｉ番目までの制御点におけるＺＭＰ回りのモーメント項の総和、ｊ番目までの制御点における外力モーメント項の総和、並びにｋ番目までの制御点における外力により生成されるＺＭＰ回りのモーメント項の総和として、接続経路の下位の関節アクチュエータに出力する。したがって、接続経路に従がって、このような加算処理を逐次繰り返していくことにより、中央コントローラに演算結果が到達するときには、ＺＭＰ釣合い方程式を構成する各モーメント項が既に求められているので、ＺＭＰ安定度判別規範に基づく機体の姿勢安定制御を効率的且つ高速に実現することができる。
【０２１１】
ＺＭＰ釣合い方程式の導入には、制御点における発生モーメントを算出するｉ系の処理と、制御点において印加される外力モーメントを算出するｊ系の処理と、制御点において外力により生成されるＺＭＰ回りのモーメントを算出するｋ系の処理が含まれるが、図示の例ではｉ、ｊ、ｋ系の処理がパラレルに進行する。ｉ、ｊ、ｋ系の処理がパラレルに進行するシステムでは、配線が少なくて済むというメリットがある。なお、特に各制御点においてｉ、ｊ、ｋ系すべての要素が備わっている必要はなく、ｉ系の演算のみ、あるいはｉ系の演算がなくてｉ−１系までの演算をパススルーするだけといったデザインも可能である。
【０２１２】
また、図３１には、各制御点の関節アクチュエータ内で、制御点に印加される並進力項、ＺＭＰ回りのモーメントにより印加される並進力項、並びに外力項を順次加算していく構成を図解している。
【０２１３】
同図に示すように、関節アクチュエータには、接続経路の上位の関節アクチュエータから、ｉ−１番目までの制御点に印加される並進力項の総和、ｊ−１番目までの制御点におけるＺＭＰ回りのモーメントにより印加される並進力項の総和、並びにｋ−１番目までの制御点に印加される外力項の総和が入力される。そして、関節アクチュエータ内で検出されたセンサ情報に基づいて、当該制御点に印加される並進力項、ＺＭＰ回りのモーメントにより印加される並進力項、並びに外力項を算出するとともに、これらをそれぞれの総和に加算処理して、ｉ番目までの制御点に印加される並進力項の総和、ｊ番目までの制御点におけるＺＭＰ回りのモーメントにより印加される並進力項の総和、並びにｋ番目までの制御点に印加される外力項の総和として、接続経路の下位の関節アクチュエータに出力する。したがって、接続経路に従がって、このような加算処理を逐次繰り返していくことにより、中央コントローラに演算結果が到達するときには、運動方程式を構成する各並進力項が既に求められているので、運動方程式を利用した機体の姿勢安定制御を効率的且つ高速に実現することができる。
【０２１４】
運動方程式の導入には、制御点における並進力を算出するｉ系の処理と、制御点の外力モーメントにより生成される並進力を算出するｊ系の処理と、制御点において印加される外力を算出するｋ系の処理が含まれるが、図示の例ではｉ、ｊ、ｋ系の処理がパラレルに進行する。ｉ、ｊ、ｋ系の処理がパラレルに進行するシステムでは、配線が少なくて済むというメリットがある。なお、特に各制御点においてｉ、ｊ、ｋ系すべての要素が備わっている必要はなく、ｉ系の演算のみ、あるいはｉ系の演算がなくてｉ−１系までの演算をパススルーするだけといったデザインも可能である。
【産業上の利用可能性】
【０２１５】
以上、特定の実施例を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施例の修正や代用を成し得ることは自明である。
【０２１６】
本発明の要旨は、必ずしも「ロボット」と称される製品には限定されない。すなわち、電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行なう機械装置あるいはその他の一般的な移動体装置であるならば、例えば玩具などのような他の産業分野に属する製品であっても、同様に本発明を適用することができる。
【０２１７】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、冒頭に記載した特許請求の範囲の欄を参酌すべきである。
【図面の簡単な説明】
【０２１８】
【図１】図１は、本発明の実施に供される脚式移動ロボットが直立している様子を前方から眺望した様子を示した図である。
【図２】図２は、本発明の実施に供される脚式移動ロボットが直立している様子を後方から眺望した様子を示した図である。
【図３】図３は、脚式移動ロボットが具備する関節自由度構成を模式的に示した図である。
【図４】図４は、脚式移動ロボット１００の制御システム構成を模式的に示した図である。
【図５】図５は、脚式移動ロボット１００の多質点近似モデルを示した図である。
【図６】図６は、多質点モデルの腰部周辺の拡大図を示した図である。
【図７】図７は、脚式移動ロボット１００において安定歩行可能な機体運動を生成するための処理手順を示したフローチャートである。
【図８】図８は、脚式移動ロボット１００において安定歩行可能な機体運動を生成するための処理手順の変形例を示したフローチャートである。
【図９】図９は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１０】図１０は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１１】図１１は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１２】図１２は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１３】図１３は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１４】図１４は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１５】図１５は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１６】図１６は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１７】図１７は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１８】図１８は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図１９】図１９は、上半身における希望軌道の優先順位の設定方法を説明するための図である。
【図２０】図２０は、脚式移動ロボットの機体上の質量が集中している部位に加速度、角加速度、角速度センサを搭載している様子を示した図である。
【図２１】図２１は、脚式移動ロボットの機体上の各リンクの重心付近に加速度、角加速度、角速度センサを搭載している様子を示した図である。
【図２２】図２２は、脚式移動ロボットの機体上の各アクチュエータの重心付近に加速度、角加速度、角速度センサを搭載している様子を示した図である。
【図２３】図２３は、脚式移動ロボットの運動制御の概略的な処理手順をフローチャートの形式で示した図である。
【図２４】図２４は、ＺＭＰ方程式の解法に基づく機体の安定制御の処理手順を示したフローチャートである。
【図２５】図２５は、運動方程式の解法に基づく機体の安定制御の処理手順を示したフローチャートである。
【図２６】図２６は、脚式移動ロボットの機体上の各アクチュエータの重心付近に配置されたセンサ同士を直列的に接続する構成例を示した図である。
【図２７】図２７は、脚式移動ロボットの機体上の各アクチュエータの重心付近に配置されたセンサ同士を直列的に接続する構成例を示した図である。
【図２８】図２８は、ユニットの重心付近に加速度、角加速度、角速度センサを搭載した関節アクチュエータの構成例を示した図である。
【図２９】図２９は、図２８に示した関節アクチュエータの機能構成を模式的に示した図である。
【図３０】図３０は、各制御点の関節アクチュエータ内で、ＺＭＰ回りのモーメント項、制御点に印加される外力モーメント項、並びに制御点に印加される外力により生成されるＺＭＰ回りのモーメント項を順次加算していく構成を示した図である。
【図３１】図３１は、各制御点の関節アクチュエータ内で、制御点に印加される並進力項、ＺＭＰ回りのモーメントにより印加される並進力項、並びに外力項を順次加算していく構成を示した図である。
【符号の説明】
【０２１９】
１…首関節ヨー軸
２Ａ…第１の首関節ピッチ軸
２Ｂ…第２の首関節（頭）ピッチ軸
３…首関節ロール軸
４…肩関節ピッチ軸
５…肩関節ロール軸
６…上腕ヨー軸
７…肘関節ピッチ軸
８…手首関節ヨー軸
９…体幹ピッチ軸
１０…体幹ロール軸
１１…股関節ヨー軸
１２…股関節ピッチ軸
１３…股関節ロール軸
１４…膝関節ピッチ軸
１５…足首関節ピッチ軸
１６…足首関節ロール軸
３０…頭部ユニット，４０…体幹部ユニット
５０…腕部ユニット，５１…上腕ユニット
５２…肘関節ユニット，５３…前腕ユニット
６０…脚部ユニット，６１…大腿部ユニット
６２…膝関節ユニット，６３…脛部ユニット
８０…制御ユニット，８１…主制御部
８２…周辺回路
９１，９２…接地確認センサ
９３，９４…加速度センサ
９５…姿勢センサ
９６…加速度センサ
１００…脚式移動ロボット
【図１】
0003568527.tif 000006
【図２】
0003568527.tif 000007
【図３】
0003568527.tif 000008
【図４】
0003568527.tif 000009
【図５】
0003568527.tif 000010
【図６】
0003568527.tif 000011
【図７】
0003568527.tif 000012
【図８】
0003568527.tif 000013
【図９】
0003568527.tif 000014
【図１０】
0003568527.tif 000015
【図１１】
0003568527.tif 000016
【図１２】
0003568527.tif 000017
【図１３】
0003568527.tif 000018
【図１４】
0003568527.tif 000019
【図１５】
0003568527.tif 000020
【図１６】
0003568527.tif 000021
【図１７】
0003568527.tif 000022
【図１８】
0003568527.tif 000023
【図１９】
0003568527.tif 000024
【図２０】
0003568527.tif 000025
【図２１】
0003568527.tif 000026
【図２２】
0003568527.tif 000027
【図２３】
0003568527.tif 000028
【図２４】
0003568527.tif 000029
【図２５】
0003568527.tif 000030
【図２６】
0003568527.tif 000031
【図２７】
0003568527.tif 000032
【図２８】
0003568527.tif 000033
【図２９】
0003568527.tif 000034
【図３０】
0003568527.tif 000035
【図３１】
0003568527.tif 000036