JP2004025434A - Motion control device and method for leg-type moving robot, and robot device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To reduce damage which a robot suffers from by motion control of the robot body on the way of falling down/dropping as much as possible. <P>SOLUTION: If a leg-type moving robot is impressed by excessive external force or an external force moment, and an action plan of a foot becomes impossible, a leg-type moving robot gives up a usual walking motion, and starts falling motion. The shock from the floor at the time of dropping is dispersed to the whole body and damage is minimized by minimizing variation ΔS/Δ t of per time t of the area S of the support polygon of the robot body, and maximizing the area S of the support polygon at the time of dropping on the floor. <P>COPYRIGHT: (C)2004,JPO

Description

【００６５】
すなわち、肩ロール軸を動作させる代わりに肘ピッチ軸を屈曲させることによて、より小さな使用体積で左右の手先を胴体後方で着床することができる。
【００６６】
また、前記のより狭い接地多角形を生成する手段又はステップは、最も小さい支持多角形に関与しないリンクの２つ以上を離床可能かどうかに応じて、手部又は足部における踏み替え動作又は床面との引き摺り動作のいずれかを選択的に利用して、より狭い接地多角形を形成するようにしてもよい。
【００６７】
より小さな接地多角形を順次形成していく過程において、手部や足部の踏み替え動作のみを利用する場合、踏み替え動作を実現するためには、手部又は足部が離床する必要があり、支持多角形に関与しない２以上のリンクがなければならず、機体の姿勢によっては踏み替え動作を行なえない場合があり、この場合は起き上がり動作そのものが破綻してしまう。これに対し、さらに手部や足部の引き摺り動作を利用することにより、起き上がり動作が破綻してしまう機会を少なくすることができる。
【００６８】
また、本発明の第２の側面は、可動脚を備え立位姿勢において脚式作業を行なう脚式移動ロボットの動作制御装置又は動作制御方法であって、
転倒時の各段階において機体に印加される衝撃モーメントを算出する手段又はステップと、
転倒時の各段階において機体が床面から受ける衝撃力を算出する手段又はステップと、
機体の接地点と路面の形成する支持多角形の面積Ｓを算出する手段又はステップと、
前記支持多角形の面積Ｓが最小又は一定となるように次の着床部位を選択する第１の着床部位探索手段又はステップと、
前記支持多角形の面積Ｓが増大するように次の着床部位を選択する第２の着床部位探索手段又はステップと、
を具備することを特徴とする脚式移動ロボットの動作制御装置又は動作制御方法である。
【００６９】
このような場合、第１の着床部位探索手段により支持多角形の面積Ｓが最小又は一定となるようにすることで、機体に印加される衝撃モーメントを受け流すことができる。この場合、機体が全点又は後退するなどして、支持面自体は移動しても良い。一方、第２の着床部位探索手段により支持多角形の面積Ｓが急激に増大するように着床部位を選択することで、転倒時に機体が床面から受ける衝撃力を緩和することができる。したがって、機体が床面から受ける衝撃力が所定の許容値内であれば前記第２の着床部位探索手段又はステップにより機体の転倒動作を行ない、許容値外であれば前記第１の着床部位探索手段又はステップにより機体の転倒動作を行なうようにすればよい。
【００７０】
また、本発明の第３の側面は、可動脚を備え立位姿勢において脚式作業を行なう脚式移動ロボットにおける機体の転倒及び起き上がりに関する一連の動作を制御する動作制御装置又は動作制御方法であって、前記脚式移動ロボットは略平行な関節自由度を持つ複数の関節軸を長さ方向に連結したリンク構造体からなり、転倒時において、機体の重心となる重心リンクを含む２以上のリンクが接床した床上姿勢において、接床リンクが形成する接地多角形内において最も少ないリンク数で形成される最も狭い支持多角形を探索する手段又はステップと、
前記最小となる支持多角形に関与しないリンク数が最大となる部位にＺＭＰを設定して転倒動作を行なう手段又はステップと、
機体の転倒姿勢において離床可能なリンクを探索する手段又はステップと、
離床可能なリンクをすべて離床させて起き上がり動作を行なう手段又はステップと、
を具備することを特徴とする脚式移動ロボットの動作制御装置又は動作制御方法である。
【００７１】
機体の重心が腰部に存在する場合、最も小さくなる支持多角形に関与しないリンク数が最大となる部位にＺＭＰを設定することができる。このような転倒・着床動作の後、離床可能なリンクをすべて離床させる、すなわち下肢と体幹の双方を浮き上がらせて、上体と下肢を同時に離床し、足部、手部などを着床させることで、より小さい接地多角形を少ないステップで形成できるので、より高速で効率的な起き上がり動作を実現することができる。
【００７２】
また、本発明の第４の側面は、体幹部と、前記体幹部に接続される脚部及び前記体幹部に接続される腕部を有するロボット装置において、
前記脚部、前記体幹部、及び／又は前記腕部が床面と接床する複数の端部から形成される第１の支持多角形を検出する支持多角形検出手段と、
前記脚部を前記体幹部方向へ屈曲させることにより、前記第１の支持多角形の面積を減少させる支持多角形変更手段と、
前記変更された第１の支持多角形内にあるＺＭＰを、前記脚部の足底面で形成する接地多角形へ、前記ＺＭＰを移動できるか否かを判断するＺＭＰ移動制御手段と、
前記ＺＭＰ移動制御手段が前記ＺＭＰを移動できると判断した際に、前記ＺＭＰを前記第１の支持多角形内から、前記足底面が形成する前記接地多角形内に維持しながら転倒姿勢から基本姿勢へ前記ロボット装置を遷移させる制御手段と、を具備することを特徴とするロボット装置である。
【００７３】
また、本発明の第５の側面は、少なくとも胴体と、前記胴体の上方に第１の関節（肩）を介して連結される１つ以上の腕リンクと前記胴体の下方に第２の関節（股関節）を介して連結される第１の脚リンクと、前記第２の脚リンクの先端に第３の関節（膝）を介して連結される第２の脚リンクとを備えた脚式移動ロボットにおいて、
前記腕リンクの先端と前記第２の脚リンク先端の足部を接床させて第１の支持多角形を形成する手段と、
前記腕リンクの先端と前記足部を接床させたまま、第２の関節を第３の関節よりも接床面法線方向上方に移動させたのち、前記第１の支持多角形の面積を減少させ、さらに前記足部により形成される接地多角形内にＺＭＰを移動させる手段と、
前記足部により形成される接地多角形内にＺＭＰを維持しながら、機体を直立させる手段と、
を具備することを特徴とする脚式移動ロボットである。
【００７４】
本発明に係るロボット装置によれば、支持多角形の面積を小さくしながら転倒姿勢から立位姿勢に復帰するので、脚部などの関節アクチュエータは比較的禎トルクで起き上がり動作を実現することができる。
【００７５】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【００７６】
【発明の実施の形態】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００７７】
Ａ．脚式移動ロボットの機械的構成
図１及び図２には本発明の実施に供される「人間形」又は「人間型」の脚式移動ロボット１００が直立している様子を前方及び後方の各々から眺望した様子を示している。図示の通り、脚式移動ロボット１００は、胴体部と、頭部と、左右の上肢部と、脚式移動を行なう左右２足の下肢部とで構成され、例えば胴体に内蔵されている制御部（図示しない）により機体の動作を統括的にコントロールするようになっている。
【００７８】
左右各々の下肢は、大腿部と、膝関節と、脛部と、足首と、足平とで構成され、股関節によって体幹部の略最下端にて連結されている。また、左右各々の上肢は、上腕と、肘関節と、前腕とで構成され、肩関節によって体幹部の上方の左右各側縁にて連結されている。また、頭部は、首関節によって体幹部の略最上端中央に連結されている。
【００７９】
制御部は、この脚式移動ロボットを構成する各関節アクチュエータの駆動制御や各センサ（後述）などからの外部入力を処理するコントローラ（主制御部）や、電源回路その他の周辺機器類を搭載した筐体である。制御部は、その他、遠隔操作用の通信インターフェースや通信装置を含んでいてもよい。
【００８０】
このように構成された脚式移動ロボットは、制御部による全身協調的な動作制御により、２足歩行を実現することができる。かかる２足歩行は、一般に、以下に示す各動作期間に分割される歩行周期を繰り返すことによって行なわれる。すなわち、
【００８１】
（１）右脚を持ち上げた、左脚による単脚支持期
（２）右足が接地した両脚支持期
（３）左脚を持ち上げた、右脚による単脚支持期
（４）左足が接地した両脚支持期
【００８２】
脚式移動ロボット１００における歩行制御は、あらかじめ下肢の目標軌道を計画し、上記の各期間において計画軌道の修正を行なうことによって実現される。すなわち、両脚支持期では、下肢軌道の修正を停止して、計画軌道に対する総修正量を用いて腰の高さを一定値で修正する。また、単脚支持期では、修正を受けた脚の足首と腰との相対位置関係を計画軌道に復帰させるように修正軌道を生成する。
【００８３】
歩行動作の軌道修正を始めとして、機体の姿勢安定制御には、一般に、ＺＭＰに対する偏差を小さくするための位置、速度、及び加速度が連続となるように、５次多項式を用いた補間計算により行なう。ＺＭＰ（Ｚｅｒｏ　Ｍｏｍｅｎｔ　Ｐｏｉｎｔ）を歩行の安定度判別の規範として用いている。ＺＭＰによる安定度判別規範は、歩行系から路面には重力と慣性力、並びにこれらのモーメントが路面から歩行系への反作用としての床反力並びに床反力モーメントとバランスするという「ダランベールの原理」に基づく。力学的推論の帰結として、足底接地点と路面の形成する支持多角形（すなわちＺＭＰ安定領域）の辺上あるいはその内側にピッチ軸及びロール軸モーメントがゼロとなる点、すなわち「ＺＭＰ（Ｚｅｒｏ　Ｍｏｍｅｎｔ　Ｐｏｉｎｔ）」が存在する。
【００８４】
図３には、この脚式移動ロボット１００が具備する関節自由度構成を模式的に示している。同図に示すように、脚式移動ロボット１００は、２本の腕部と頭部１を含む上肢と、移動動作を実現する２本の脚部からなる下肢と、上肢と下肢とを連結する体幹部とで構成された、複数の肢を備えた構造体である。
【００８５】
頭部を支持する首関節（Ｎｅｃｋ）は、首関節ヨー軸１と、第１及び第２の首関節ピッチ軸２Ａ及び２Ｂと、首関節ロール軸３という４自由度を有している。
【００８６】
また、各腕部は、その自由度として、肩（Ｓｈｏｕｌｄｅｒ）における肩関節ピッチ軸４と、肩関節ロール軸５と、上腕ヨー軸６、肘（Ｅｌｂｏｗ）における肘関節ピッチ軸７と、手首（Ｗｒｉｓｔ）における手首関節ヨー軸８と、手部とで構成される。手部は、実際には、複数本の指を含む多関節・多自由度構造体である。
【００８７】
また、体幹部（Ｔｒｕｎｋ）は、体幹ピッチ軸９と、体幹ロール軸１０という２自由度を有する。
【００８８】
また、下肢を構成する各々の脚部は、股関節（Ｈｉｐ）における股関節ヨー軸１１と、股関節ピッチ軸１２と、股関節ロール軸１３と、膝（Ｋｎｅｅ）における膝関節ピッチ軸１４と、足首（Ａｎｋｌｅ）における足首関節ピッチ軸１５と、足首関節ロール軸１６と、足部とで構成される。
【００８９】
但し、エンターティンメント向けの脚式移動ロボット１００が上述したすべての自由度を装備しなければならない訳でも、あるいはこれに限定される訳でもない。設計・製作上の制約条件や要求仕様などに応じて、自由度すなわち関節数を適宜増減することができることは言うまでもない。
【００９０】
上述したような脚式移動ロボット１００が持つ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、２足歩行という不安定構造体に対して姿勢制御を行なうことなどの要請から、アクチュエータは小型且つ軽量であることが好ましい。本実施形態では、ギア直結型で且つサーボ制御系をワンチップ化してモータ・ユニットに内蔵したタイプの小型ＡＣサーボ・アクチュエータを搭載することとした（この種のＡＣサーボ・アクチュエータに関しては、例えば本出願人に既に譲渡されている特開２０００−２９９９７０号公報に開示されている）。本実施形態では、直結ギアとして低減速ギアを採用することにより、人間との物理的インタラクションを重視するタイプのロボットに求められている駆動系自身の受動的特性を得ている。
【００９１】
Ｂ．脚式移動ロボットの制御システム構成
図４には、脚式移動ロボット１００の制御システム構成を模式的に示している。同図に示すように、脚式移動ロボット１００は、ヒトの四肢を表現した各機構ユニット３０，４０，５０Ｒ／Ｌ，６０Ｒ／Ｌと、各機構ユニット間の協調動作を実現するための適応制御を行なう制御ユニット８０とで構成される（但し、Ｒ及びＬの各々は、右及び左の各々を示す接尾辞である。以下同様）。
【００９２】
脚式移動ロボット１００全体の動作は、制御ユニット８０によって統括的に制御される。制御ユニット８０は、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）やメモリ等の主要回路コンポーネント（図示しない）で構成される主制御部８１と、電源回路やロボット１００の各構成要素とのデータやコマンドの授受を行なうインターフェース（いずれも図示しない）などを含んだ周辺回路８２とで構成される。
【００９３】
本発明を実現する上で、この制御ユニット８０の設置場所は特に限定されない。図４では体幹部ユニット４０に搭載されているが、頭部ユニット３０に搭載してもよい。あるいは、脚式移動ロボット１００外に制御ユニット８０を配備して、脚式移動ロボット１００の機体とは有線若しくは無線で交信するようにしてもよい。
【００９４】
図３に示した脚式移動ロボット１００内の各関節自由度は、それぞれに対応するアクチュエータによって実現される。すなわち、頭部ユニット３０には、首関節ヨー軸１、第１及び第２の首関節ピッチ軸２Ａ及び２Ｂ、首関節ロール軸３の各々を表現する首関節ヨー軸アクチュエータＡ_１、第１及び第２の首関節ピッチ軸アクチュエータＡ_２Ａ、Ａ_２Ｂ、首関節ロール軸アクチュエータＡ_３がそれぞれ配設されている。
【００９５】
また、体幹部ユニット４０には、体幹ピッチ軸９、体幹ロール軸１０の各々を表現する体幹ピッチ軸アクチュエータＡ_９、体幹ロール軸アクチュエータＡ_１０が配備されている。
【００９６】
また、腕部ユニット５０Ｒ／Ｌは、上腕ユニット５１Ｒ／Ｌと、肘関節ユニット５２Ｒ／Ｌと、前腕ユニット５３Ｒ／Ｌに細分化されるが、肩関節ピッチ軸４、肩関節ロール軸５、上腕ヨー軸６、肘関節ピッチ軸７、手首関節ヨー軸８の各々を表現する肩関節ピッチ軸アクチュエータＡ_４、肩関節ロール軸アクチュエータＡ_５、上腕ヨー軸アクチュエータＡ_６、肘関節ピッチ軸アクチュエータＡ_７、手首関節ヨー軸アクチュエータＡ_８が配備されている。
【００９７】
また、脚部ユニット６０Ｒ／Ｌは、大腿部ユニット６１Ｒ／Ｌと、膝ユニット６２Ｒ／Ｌと、脛部ユニット６３Ｒ／Ｌに細分化されるが、股関節ヨー軸１１、股関節ピッチ軸１２、股関節ロール軸１３、膝関節ピッチ軸１４、足首関節ピッチ軸１５、足首関節ロール軸１６の各々を表現する股関節ヨー軸アクチュエータＡ_１１、股関節ピッチ軸アクチュエータＡ_１２、股関節ロール軸アクチュエータＡ_１３、膝関節ピッチ軸アクチュエータＡ_１４、足首関節ピッチ軸アクチュエータＡ_１５、足首関節ロール軸アクチュエータＡ_１６が配備されている。
【００９８】
各関節に用いられるアクチュエータＡ_１，Ａ_２，Ａ_３…は、より好ましくは、ギア直結型で且つサーボ制御系をワンチップ化してモータ・ユニット内に搭載したタイプの小型ＡＣサーボ・アクチュエータ（前述）で構成することができる。
【００９９】
頭部ユニット３０、体幹部ユニット４０、腕部ユニット５０、各脚部ユニット６０などの各機構ユニット毎に、アクチュエータ駆動制御用の副制御部３５，４５，５５，６５が配備されている。
【０１００】
機体の体幹部４０には、加速度センサ９５と姿勢センサ９６が配設されている。加速度センサ９５は、Ｘ，Ｙ，Ｚ各軸方向に配置する。機体の腰部に加速度センサ９５を配設することによって、質量操作量が大きな部位である腰部を制御目標点として設定して、その位置における姿勢や加速度を直接計測して、ＺＭＰに基づく姿勢安定制御を行なうことができる。
【０１０１】
また、各脚部６０Ｒ，Ｌには、接地確認センサ９１及び９２と、加速度センサ９３，９４がそれぞれ配設されている。接地確認センサ９１及び９２は、例えば足底に圧力センサを装着することにより構成され、床反力の有無により足底が着床したか否かを検出することができる。また、加速度センサ９３，９４は、少なくともＸ及びＹの各軸方向に配置する。左右の足部に加速度センサ９３，９４を配設することにより、ＺＭＰ位置に最も近い足部で直接ＺＭＰ方程式を組み立てることができる。
【０１０２】
質量操作量が大きな部位である腰部にのみ加速度センサを配置した場合、腰部のみが制御目標点に設定され、足部の状態は、この制御目標点の計算結果を基に相対的に算出しなければならず、足部と路面との間では以下の条件を満たすことが、前提となってしまう。
【０１０３】
（１）路面はどんな力やトルクが作用しても動くことがない。
（２）路面での並進に対する摩擦係数は充分に大きく、滑りが生じない。
【０１０４】
これに対し、本実施形態では、路面との接触部位である足部にＺＭＰと力を直接する反力センサ・システム（床反力センサなど）を配備するとともに、制御に用いるローカル座標とその座標を直接的に計測するための加速度センサを配設する。この結果、ＺＭＰ位置に最も近い足部で直接ＺＭＰ方程式を組み立てることができ、上述したような前提条件に依存しない、より厳密な姿勢安定制御を高速で実現することができる。この結果、力やトルクが作用すると路面が動いてしまう砂利上や毛足の長い絨毯上や、並進の摩擦係数が充分に確保できずに滑りが生じ易い住居のタイルなどであっても、機体の安定歩行（運動）を保証することができる。
【０１０５】
主制御部８０は、各センサ９１〜９６の出力に応答して制御目標をダイナミックに補正することができる。より具体的には、副制御部３５，４５，５５，６５の各々に対して適応的な制御を行い、脚式移動ロボット１００の上肢、体幹、及び下肢が協調して駆動する全身運動パターンを実現する。
【０１０６】
ロボット１００の機体上での全身運動は、足部運動、ＺＭＰ（Ｚｅｒｏ　Ｍｏｍｅｎｔ　Ｐｏｉｎｔ）軌道、体幹運動、上肢運動、腰部高さなどを設定するとともに、これらの設定内容に従った動作を指示するコマンドを各副制御部３５，４５，５５，６５に転送する。そして、各々の副制御部３５，４５…では、主制御部８１からの受信コマンドを解釈して、各アクチュエータＡ_１，Ａ_２，Ａ_３…に対して駆動制御信号を出力する。ここで言う「ＺＭＰ」とは、歩行中の床反力によるモーメントがゼロとなる床面上の点のことであり、また、「ＺＭＰ軌道」とは、例えばロボット１００の歩行動作期間中にＺＭＰが動く軌跡を意味する。
【０１０７】
Ｃ ． 脚式移動ロボットの運動系基本状態遷移
本実施形態に係る脚式移動ロボット１００の制御システムは、複数の基本姿勢を定義する。各々の基本姿勢は、機体の安定性や、消費エネルギ、次の状態への遷移を考慮して定義されており、基本姿勢間の遷移という形態により機体運動を効率的に制御することができる。
【０１０８】
図５には、本実施形態に係る脚式移動ロボット１００の運動系が持つ基本状態遷移を示している。同図に示すように、脚式移動ロボットは、基本仰向け姿勢、基本立ち姿勢、基本歩行姿勢、基本座り姿勢、基本うつ伏せ姿勢がそれぞれ仰向け時、立脚時、歩行準備時、着席時、及びうつ伏せ時における機体の安定性や、消費エネルギ、次の状態への遷移を考慮して定義されている。
【０１０９】
これら基本姿勢は、機体の動作制御プログラムのプラットフォームに位置付けられる。また、脚式移動ロボットは、立ち姿勢などにおいて、歩行や跳躍、ダンスなど全身動作を利用した各種のパフォーマンスを行なうが、その装置制御プログラムは、プラットフォーム上で動作するアプリケーションとして位置付けられる。これらアプリケーション・プログラムは、外部記憶から随時ロードされ、主制御部８１によって実行される。
【０１１０】
図６には、脚式移動ロボット１００の基本仰向け姿勢を示している。本実施形態では、機体への電源投入時には基本仰向け姿勢をとり、転倒などの心配がなく機械運動的に最も安定した状態からの起動を行なうことができる。また、脚式移動ロボットは、起動時だけでなくシステム動作の終了時も基本仰向け姿勢に復帰するようになっている。したがって、機械運動学的に機体が最も安定した状態で作業を開始するとともに、最も安定した状態で作業を終了することから、脚式移動ロボットの動作オペレーションは自己完結的となる。
【０１１１】
勿論、機体の転倒時においても、床上での所定のモーションを経て一旦基本仰向け姿勢に戻った後に、規定の立ち上がり動作を実行することにより、基本立ち姿勢を介して、作業中断時の元の姿勢を回復することができる。
【０１１２】
また、本実施形態に係る脚式移動ロボット１００は、床上での基本姿勢として、基本仰向け姿勢の他に、図７に示したような基本うつ伏せ姿勢を備えている。この基本うつ伏せ姿勢は、基本仰向け姿勢と同様に、機械運動学的に機体が最も安定した状態であり、電源が遮断された脱力状態においても姿勢安定性を維持することができる。例えば、脚式作業において不測の外力などにより機体が転倒した場合、仰向け又はうつ伏せのいずれの状態で落下するか不明なので、本実施形態では、このように２通りの床上基本姿勢を規定している。
【０１１３】
基本仰向け姿勢と基本うつ伏せ姿勢の間は、各種の床上姿勢を経て可逆的に遷移することができる。逆に言えば、これら基本仰向け姿勢と基本うつ伏せ姿勢を基準にして各種の床上姿勢へ円滑に状態遷移することができる。
【０１１４】
基本仰向け姿勢は、機械運動学的には最も安定した基本姿勢であるが、脚式作業を考慮した場合、円滑な状態遷移を行なうことはできない。そこで、図８に示すような基本立ち姿勢が定義されている。基本立ち姿勢を定義することで、その後の脚式作業へ滞りなく移行することができる。
【０１１５】
基本立ち姿勢は、立ち状態で最も安定した状態であり、姿勢安定制御のための計算機負荷や消費電力が最小又は極小となるような姿勢であり、膝を伸展させることにより直立状態を保つためのモータ・トルクを最小限に抑えている。この基本立ち姿勢から各種の立ち姿勢へ円滑に状態遷移して、たとえば上肢を利用したダンス・パフォーマンスなどを実演することができる。
【０１１６】
他方、基本立ち姿勢は、姿勢安定性に優れているがこのまま歩行など脚式作業に移行するためには最適化されていない。そこで、本実施形態に係る脚式移動ロボットは、立脚状態の他の基本姿勢として、図９に示すような基本歩行姿勢を定義している。
【０１１７】
基本立ち姿勢において、股関節、膝関節、並びに足首関節の各ピッチ軸１２、１４、１５を駆動して、機体の重心位置を少し落とす格好にすることによって、基本歩行姿勢に遷移する。基本歩行姿勢では、通常の歩行動作を始めとして各種の脚式動作への遷移を円滑に行なうことができる。但し、膝を屈曲させた分だけ、この姿勢を維持するためのトルクが余分に必要とならことから、基本歩行姿勢は、基本立ち姿勢に比し消費電力は増大する。
【０１１８】
基本立ち姿勢は、機体のＺＭＰ位置はＺＭＰ安定領域の中心付近にあり、膝の曲げ角が小さくエネルギ消費量が低い姿勢である。これに対し、基本歩行姿勢では、ＺＭＰ位置が安定領域の中心付近にあるが、高い路面適応性、高い外力適応性を確保するために膝の曲げ角を比較的大きくとっている。
【０１１９】
また、本実施形態に係る脚式移動ロボット１００では、さらに基本座り姿勢が定義されている。この基本座り姿勢（図示しない）では、所定の椅子に腰掛けたときに、姿勢安定制御のための計算機負荷や消費電力が最小又は極小となるような姿勢である。前述した、基本仰向け姿勢、基本うつ伏せ姿勢、並びに基本立ち姿勢からは、可逆的に基本姿勢へ遷移することができる。また、基本座り姿勢並びに基本立ち姿勢からは、各種の座り姿勢へと円滑に移行することができ、座り姿勢で例えば状態のみを用いた各種のパフォーマンスを実演することができる。
【０１２０】
Ｄ．脚式移動ロボットの姿勢安定制御
次いで、本実施形態に係る脚式移動ロボット１００における、脚式作業時における姿勢安定化処理、すなわち足部、腰、体幹、下肢運動などからなる全身協調運動実行時における姿勢の安定化処理の手順について説明する。
【０１２１】
本実施形態に係る姿勢安定制御は、ＺＭＰを姿勢安定制御に用いる。ＺＭＰを安定度判別規範に用いたロボットの姿勢安定度制御は、基本的には足底接地点と路面の形成する支持多角形の辺上あるいはその内側にモーメントがゼロとなる点を探索することにある。すなわち、ロボットの機体に印加される各モーメントの釣合い関係を記述したＺＭＰ方程式を導出して、このＺＭＰ方程式上で現れるモーメント・エラーを打ち消すように機体の目標軌道を修正する。
【０１２２】
本実施形態では、ロボットの機体上の制御目標点として質量操作量が最大となる部位、例えば腰部をローカル座標原点に設定する。そして、この制御目標点に加速度センサなどの計測手段を配置して、その位置における姿勢や加速度を直接計測して、ＺＭＰに基づく姿勢安定制御を行なう。さらに路面との接触部位である足部に加速度センサを配備することにより、制御に用いるローカル座標とその座標を直接的に計測して、ＺＭＰ位置に最も近い足部で直接ＺＭＰ方程式を組み立てる。
【０１２３】
Ｄ−１．ＺＭＰ方程式の導入
本実施形態に係る脚式移動ロボット１００は、無限すなわち連続的な質点の集合体である。但し、ここでは有限数で離散的な質点からなる近似モデルに置き換えることによって、安定化処理のための計算量を削減するようにしている。より具体的には物理的には図３に示す多関節自由度構成を具備する脚式移動ロボット１００を、図１０に示すように多質点近似モデルに置き換えて取り扱う。図示の近似モデルは、線形且つ非干渉の多質点近似モデルである。
【０１２４】
図１０において、Ｏ−ＸＹＺ座標系は絶対座標系におけるロール、ピッチ、ヨー各軸を表し、また、Ｏ’−Ｘ’Ｙ’Ｚ’座標系はロボット１００とともに動く運動座標系におけるロール、ピッチ、ヨー各軸を表している。但し、図中におけるパラメータの意味は以下の通りである。また、ダッシュ（´）付きの記号は運動座標系を記述するものと理解されたい。
【０１２５】
【数４】

【０１２６】
同図に示す多質点モデルでは、ｉはｉ番目に与えられた質点を表す添え字であり、ｍ_ｉはｉ番目の質点の質量、ｒ_ｉ’はｉ番目の質点の位置ベクトル（但し運動座標系）を表すものとする。本実施形態に係る脚式移動ロボット１００の機体重心は腰部付近に存在する。すなわち、腰部は、質量操作量が最大となる質点であり、図１０では、その質量はｍ_ｈ、その位置ベクトル（但し運動座標系）はｒ’_ｈ（ｒ’_ｈｘ，ｒ’_ｈｙ，ｒ’_ｈｚ）とする。また、機体のＺＭＰの位置ベクトル（但し運動座標系）をｒ’_ｚｍｐ（ｒ’_ｚｍｐｘ，ｒ’_ｚｍｐｙ，ｒ’_ｚｍｐｚ）とする。
【０１２７】
世界座標系Ｏ−ＸＹＺは絶対座標系であり、不変である。本実施形態に係る脚式移動ロボット１００は、腰部と両脚の足部にそれぞれ加速度センサ９３、９４、９６が配置されており、これらセンサ出力により腰部並びに立脚それぞれと世界座標系の相対位置ベクトルｒｑが直接検出される。これに対し、運動座標系すなわち機体のローカル座標系はＯ−Ｘ’Ｙ’Ｚ’は、ロボットともに動く。
【０１２８】
多質点モデルは、言わば、ワイヤフレーム・モデルの形態でロボットを表現したものである。図１０を見ても判るように、多質点近似モデルは、両肩、両肘、両手首、体幹、腰部、及び、両足首の各々を質点として設定される。図示の非厳密の多質点近似モデルにおいては、モーメント式は線形方程式の形式で記述され、該モーメント式はピッチ軸及びロール軸に関して干渉しない。多質点近似モデルは、概ね以下の処理手順により生成することができる。
【０１２９】
（１）ロボット１００全体の質量分布を求める。
（２）質点を設定する。質点の設定方法は、設計者のマニュアル入力であっても、所定の規則に従った自動生成のいずれでも構わない。
（３）各領域ｉ毎に、重心を求め、その重心位置と質量ｍ_ｉを該当する質点に付与する。
（４）各質点ｍ_ｉを、質点位置ｒ_ｉを中心とし、その質量に比例した半径に持つ球体として表示する。
（５）現実に連結関係のある質点すなわち球体同士を連結する。
【０１３０】
なお、図１０に示す多質点モデルの腰部情報における各回転角（θ_ｈｘ，θ_ｈｙ，θ_ｈｚ）は、脚式移動ロボット１００における腰部の姿勢すなわちロール、ピッチ、ヨー軸の回転を規定するものである（図１１には、多質点モデルの腰部周辺の拡大図を示しているので、確認されたい）。
【０１３１】
機体のＺＭＰ方程式は、制御目標点において印加される各モーメントの釣合い関係を記述したものである。図５に示したように、機体を多数の質点ｍ_ｉで表わし、これらを制御目標点とした場合、すべての制御目標点ｍ_ｉにおいて印加されるモーメントの総和を求める式がＺＭＰ方程式である。
【０１３２】
世界座標系（Ｏ−ＸＹＺ）で記述された機体のＺＭＰ方程式、並びに機体のローカル座標系（Ｏ−Ｘ’Ｙ’Ｚ’）はそれぞれ以下の通りとなる。
【０１３３】
【数５】

【０１３４】
上式は、各質点ｍ_ｉにおいて印加された加速度成分により生成されるＺＭＰ回り（半径ｒ_ｉ−ｒ_ｚｍｐ）のモーメントの総和と、各質点ｍｉに印加された外力モーメントＭ_ｉの総和と、外力Ｆ_ｋにより生成されるＺＭＰ回り（ｋ番目の外力Ｆ_ｋの作用点をｓ_ｋとする）のモーメントの総和が釣り合うということを記述している。
【０１３５】
このＺＭＰ釣合い方程式は、総モーメント補償量すなわちモーメント・エラー成分Ｔを含んでいる。このモーメント・エラーをゼロ又は所定の許容範囲内に抑えることによって、機体の姿勢安定性が維持される。言い換えれば、モーメント・エラーをゼロ又は許容値以下となるように機体運動（足部運動や上半身の各部位の軌道）を修正することが、ＺＭＰを安定度判別規範とした姿勢安定制御の本質である。
【０１３６】
本実施形態では、腰部と左右の足部にそれぞれ加速度センサ９６，９３及び９４が配設されているので、これらの制御目標点における加速度計測結果を用いて直接的に且つ高精度に上記のＺＭＰ釣合い方程式を導出することができる。この結果、高速でより厳密な姿勢安定制御を実現することができる。
【０１３７】
Ｄ−２．全身協調型の姿勢安定制御
図１２には、脚式移動ロボット１００において、ＺＭＰを安定度判別規範に用いて安定歩行可能な機体運動を生成するための処理手順をフローチャートの形式で示している。但し、以下の説明では、図１０及び図１１に示すような線形・非干渉多質点近似モデルを用いて脚式移動ロボット１００の各関節位置や動作を記述するものとする。
【０１３８】
まず、足部運動の設定を行なう（ステップＳ１）。足部運動は、２以上の機体のポーズを時系列的に連結されてなるモーション・データである。
【０１３９】
モーション・データは、例えば、足部の各関節角の変位を表わした関節空間情報と、関節位置を表わしたデカルト空間情報で構成される。モーション・データは、コンソール画面上での手付け入力や、機体へのダイレクト・ティーチング（直接教示）例えばモーション編集用のオーサリング・システム上で構築したりすることができる。
【０１４０】
次いで、設定された足部運動を基にＺＭＰ安定領域を算出する（ステップＳ２）。ＺＭＰは、機体に印加されるモーメントがゼロとなる点であり、基本的には足底接地点と路面の形成する支持多角形の辺上あるいはその内側に存在する。ＺＭＰ安定領域は、この支持多角形のさらに内側に設定された領域であり、該領域にＺＭＰを収容させることによって機体を高度に安定した状態にすることができる。
【０１４１】
そして、足部運動とＺＭＰ安定領域を基に、足部運動中におけるＺＭＰ軌道を設定する（ステップＳ３）。
【０１４２】
また、機体の上半身（股関節より上側）の各部位については、腰部、体幹部、上肢、頭部などのようにグループ設定する（ステップＳ１１）。
【０１４３】
そして、各部位グループ毎に希望軌道を設定する（ステップＳ１２）。上半身の希望起動の設定は、足部の場合と同様に、コンソール画面上での手付け入力や、機体へのダイレクト・ティーチング（直接教示）例えばモーション編集用のオーサリング・システム上で構築したりすることができる。
【０１４４】
次いで、各部位のグループ設定の調整（再グルーピング）を行ない（ステップＳ１３）、さらにこれらグループに対して優先順位を与える（ステップＳ１４）。ここで言う優先順位とは、機体の姿勢安定制御を行なうための処理演算に投入する順位のことであり、例えば質量操作量に応じて割り振られる。この結果、機体上半身についての各部位についての優先順位付き希望軌道群が出来上がる。
【０１４５】
また、機体上半身の各部位グループ毎に、モーメント補償に利用できる質量を算出しておく（ステップＳ１５）。
【０１４６】
そして、足部運動とＺＭＰ軌道、並びに上半身の各部位グループ毎の希望起動群を基に、ステップＳ１４により設定された優先順位に従って、各部位グループの運動パターンを姿勢安定化処理に投入する。
【０１４７】
この姿勢安定化処理では、まず、処理変数ｉに初期値１を代入する（ステップＳ２０）。そして、優先順位が先頭からｉ番目までの部位グループについての目標軌道設定時における、目標ＺＭＰ上でのモーメント量すなわち総モーメント補償量を算出する（ステップＳ２１）。目標軌道が算出されていない部位については、希望軌道を用いる。
【０１４８】
次いで、ステップＳ１５において算出された当該部位のモーメント補償に利用できる質量を用いて、そのモーメント補償量を設定して（ステップＳ２２）、モーメント補償量を算出する（ステップＳ２３）。
【０１４９】
次いで、算出されたｉ番目の部位のモーメント補償量を用いて、ｉ番目の部位についてのＺＭＰ方程式を導出して（ステップＳ２４）、当該部位のモーメント補償運動を算出することにより（ステップＳ２５）、優先順位が先頭からｉ番目までの部位についての目標軌道を得ることができる。
【０１５０】
このような処理をすべての部位グループについて行なうことにより、安定運動（例えば歩行）が可能な全身運動パターンが生成される。
【０１５１】
腰部と左右の足部にそれぞれ加速度センサ９６，９３及び９４が配設されているので、これらの制御目標点における加速度計測結果を用いて直接的に且つ高精度に上記のＺＭＰ釣合い方程式を導出することができる。この結果、図１２に示すような処理手順に従ってＺＭＰ安定度判別規範に基づく姿勢安定制御を高速でより厳密に実行することができる。
【０１５２】
Ｅ．脚式移動ロボットの転倒オペレーション
前項Ｄで説明したように、本実施形態に係る脚式移動ロボット１００は、基本的には、ＺＭＰ安定度判別規範に基づいて、歩行時やその他の立脚作業時における姿勢安定制御を行ない、機体の転倒という事態の発生を最小限に抑えるようにしている。
【０１５３】
しかしながら、万一転倒を避けられなくなった場合には、機体へのダメージを極力防止するような動作パターンからなる転倒動作を行なうことにする。例えば、前述したＺＭＰ釣合い方程式において、過大な外力Ｆ又は外力モーメントＭが機体に印加された場合、機体動作のみによってモーメント・エラー成分Ｔをキャンセルことができなくなり、姿勢の安定性を維持できなくなる。
【０１５４】
図１３には、本実施形態に係る脚式移動ロボット１００における脚式作業中の機体の動作制御の概略的な処理手順をフローチャートの形式で示している。
【０１５５】
機体動作中は、左右の足部に配設した接地確認（床反力）センサ９１及び９２、加速度センサ９３及び９４、腰部に配設した加速度センサ９６のセンサ出力を用いて、ＺＭＰ釣合い方程式（前述）を立てて、腰部、下肢軌道を常に計算する（ステップＳ３１）。
【０１５６】
例えば、機体に外力が印加されたとき、次の腰部、下肢軌道を計画することができるかどうか、すなわち足部の行動計画によって外力によるモーメント・エラーを解消することができるかどうかを判別する（ステップＳ３２）。腰部、下肢軌道を計画することができるかどうかは、脚部の各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。勿論、外力が加わったときに、次の一歩だけでなく、数歩にまたがる脚式動作によりモーメント・エラーを解消するようにしてもよい。
【０１５７】
このとき、足部の計画が可能であれば、歩行やその他の脚式動作を継続する（ステップＳ３３）。
【０１５８】
他方、過大な外力又は外力モーメントが機体に印加されたために、足部の計画が不可能になった場合には、脚式移動ロボット１００は転倒動作を開始する（ステップＳ３４）。
【０１５９】
図１〜図２に示すような直立歩行型の脚式ロボットの場合、重心位置が高いことから、転倒時に不用意に床面に落下すると、ロボット自体、あるいは転倒により衝突する相手側にも致命的な損傷を与えてしまう危険がある。
【０１６０】
そこで、本実施形態では、転倒前に計画されている機体の軌道からＺＭＰ支持多角形が最小となるような姿勢に組み替えて、所定の転倒動作を実行する。基本的には、以下に示す２つの方針を基に転倒動作を探索していく。
【０１６１】
（１）機体の支持多角形の面積Ｓの時間ｔ当たりの変化量ΔＳ／Δｔを最小にする。
（２）床面落下時における支持多角形が最大となるようにする。
【０１６２】
ここで、変化量ΔＳ／Δｔを最小にするとは、転倒時の支持面積を維持する（あるいは減少させる）ことに相当する（但し、減少させる場合、駆動力が必要な場合がある）。機体の転倒時に支持面積を維持することで、機体に印加される衝撃モーメントを受け流すことができる。図１４には、機体の転倒時に支持面積を維持する原理を図解している。同図に示すように、丁度球体が転がる具合で、支持面は面積最小であることを維持しながら、衝撃モーメントを受け流している。図示の通り、支持面が移動しても同様の効果が得られる。例えば、着床時に床から受ける衝撃力を求め、これが許容値を越えるような場合には、支持多角形の面積を一定に保つように機体が転がる転倒方法が好ましい。
【０１６３】
また、床面落下時における支持多角形が最大となるとは、図１５に示すように、より広い支持多角形で受け止めることにより衝撃力を干渉することに相当する。例えば、着床時に床から受ける衝撃力を求め、これが許容値以内となる場合には、支持多角形の面積を一定に保つように機体が転がる転倒方法が好ましい。
【０１６４】
図１６及び図１７には、脚式移動ロボット１００が後方すなわち仰向け姿勢に向かって転倒する場合に、支持多角形の変化量ΔＳ／Δｔを最小すなわち転倒時の支持面積を維持する動作を実現した例を示している。これは、柔道やその他の格闘技における受身動作に類似する類似する動作であり、転倒時の衝撃力モーメントを好適に受け流すことができる。図１７に示すように、足部を離床させることにより、支持多角形の変化量ΔＳ／Δｔを最小にしている。機体の重心が腰部に存在する場合、最も小さくなる支持多角形に関与しないリンク数が最大となる部位にＺＭＰを設定することができる。このような転倒・着床動作の後、離床可能なリンクをすべて離床させる、すなわち図示の例では下肢と体幹の双方を浮き上がらせて、上体と下肢を同時に離床し、足部、手部などを着床させることで、より小さい接地多角形を少ないステップで形成できるので、より高速で効率的な起き上がり動作を実現することができる。
【０１６５】
図１８及び図１９には、脚式移動ロボット１００が前方すなわちうつ伏せ姿勢に向かって転倒する場合に、支持多角形の変化量ΔＳ／Δｔを最小すなわち転倒時の支持面積を維持する動作を実現した例をそれぞれ側面並びに右斜め前方から眺めた様子を示している。これは、機械体操などにおける前転動作に類似する類似する動作であり、転倒時の衝撃力モーメントを好適に受け流すことができる。各図に示すように、足部を離床させることにより、支持多角形の変化量ΔＳ／Δｔを最小にしている。
【０１６６】
上述したような転倒方法をとることにより、落下時に床面から受ける衝撃を全身に分散させることにより、ダメージを最小限に抑えることができる。
【０１６７】
図２０には、本実施形態に係る脚式移動ロボット１００が足部の計画不能により転倒動作を行なうための処理手順をフローチャートの形式で示している。転倒動作は、上述した基本方針に従って、高さ方向に連結された肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４を同期協調的に駆動させることによって実現される。このような処理手順は、実際には主制御部８１において所定の機体動作制御プログラムを実行して、各部を駆動制御することによって実現される。
【０１６８】
まず、機体の支持多角形の面積Ｓの時間ｔ当たりの変化量ΔＳ／Δｔを最小にするリンクを探索する（ステップＳ４１）。
【０１６９】
次いで、ステップＳ４１により選択されたリンクで変化量ΔＳ／Δｔを最小にする該リンクの目標着床点を探索する（ステップＳ４２）。機体の床面に対する支持面積を最小に維持することにより、衝撃モーメントを受け流すことができる（前述及び図１４を参照のこと）。
【０１７０】
次いで、先行ステップにより選択されたリンクを該目標着床点に着床することが、機体ハードウェアの制約上（各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など）、実行可能かどうかを、衝撃力モーメントを主に判別する（ステップＳ４３）。
【０１７１】
先行ステップにより選択されたリンクを該目標着床点に着床することが不可能であると判定された場合には、時間の変化量Δｔを所定値だけ増分してから（ステップＳ４４）、ステップＳ４１に戻って、リンクの再選択、並びに該リンクの目標着床点の再設定を行なう。
【０１７２】
一方、先行ステップにより選択されたリンクを該目標着床点に着床することが可能である場合には、選択されたリンクを該目標着床点に着床する（ステップＳ４５）。
【０１７３】
次いで、機体の位置エネルギが最小かどうか、すなわち転倒動作が完了したかどうかを判別する（ステップＳ４６）。
【０１７４】
機体の位置エネルギがまだ最小ではない場合には、時間の変化量Δｔをさらに所定値だけ増分して（ステップＳ４７）、支持多角形を拡大するように次の目標着床点を設定する（ステップＳ４８）。支持多角形を拡大することにより、着床時に機体に加わる衝撃力を軽減することができる（前述及び図１５を参照のこと）。
【０１７５】
次いで、選択されたリンクを該目標着床点に着床することが、機体ハードウェアの制約上（各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など）、実行可能かどうかを、衝撃力を主に判別する（ステップＳ４９）。　先行ステップにより選択されたリンクを該目標着床点に着床することが不可能であると判定された場合には、ステップＳ４１に戻って、リンクの再選択、並びに該リンクの目標着床点の再設定を行なう。
【０１７６】
一方、先行ステップにより選択されたリンクを該目標着床点に着床することが可能である場合には、ステップＳ４５に進んで、選択されたリンクを該目標着床点に着床する。
【０１７７】
そして、機体の位置エネルギが最小になると（ステップＳ４６）、機体の床面への着床が完了したことになるので、本処理ルーチン全体を終了する。
【０１７８】
次いで、実機動作を参照しながら、脚式移動ロボット１００の転倒動作について説明する。
【０１７９】
図２１には、脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４などの高さ方向に連結された略平行な複数の関節軸からなるリンク構造体としてモデル化して、各関節ピッチ軸を同期強調的に駆動させて仰向け姿勢に向かって転倒していく動作を示している。基本的に、離床リンク数が最大になるリンクがある部位を目標に設定することで、床面から受ける衝撃力を減少するようになっている。
【０１８０】
ロボットは、リンク構造体のリンク端である足底のみで立位しているとする（図２１（１））。
【０１８１】
このとき、外力又は外力モーメントの印加により、ＺＭＰ釣合い方程式のモーメント・エラー項Ｔをキャンセルできなくなり、足底のみで形成するＺＭＰ安定領域の外にＺＭＰが逸脱したことに応答して、ＺＭＰを支持多角形に維持したまま転倒動作を開始する。
【０１８２】
転倒動作では、まず、機体の支持多角形の面積Ｓの時間ｔ当たりの変化量ΔＳ／Δｔを最小にするリンクを探索するとともに、手先を含むリンクで変化量ΔＳ／Δｔを最小にするような手先の目標着床点を探索する。そして、選択されたリンクを目標着床点に着床することが機体ハードウェアの制約上（各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など）、実行可能かどうかを判別する。
【０１８３】
機体ハードウェア上実行可能である場合には、既に着床している足底リンクに加え、他のリンクが着床する。そして、これら着床リンクで形成される最小支持多角形内にＺＭＰを移動する（図２１（２））。
【０１８４】
次いで、機体ハードウェアが許容する限り、着床点を移動して、支持多角形を拡大していく（図２１（３））。
【０１８５】
そして、各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など機体ハードウェアの制約から、もはや着床点を移動することができなくなると、今度は、着床中のリンクに挟まれている離床リンクを着床することができるかどうかを判別する。
【０１８６】
機体ハードウェア上、着床リンク間の離床リンクを着床することが可能である場合には、これらを着床して、着床リンク数を増やす（図２１（４））。
【０１８７】
さらに、機体ハードウェアが許容する限り、着床点を移動して、支持多角形を拡大していく（図２１（５））。
【０１８８】
そして最後に、高さ方向に連結された略平行な複数の関節軸からなるリンク構造体の一端側から１以上のリンクと、他端がわから２以上のリンクを離床させて、且つ、それらの中間に位置するリンクを１以上着床させ、さらに足部を着床させた状態で、ＺＭＰを支持多角形内に維持しながら、支持多角形が最大となる姿勢を形成する。この姿勢において、機体の位置エネルギが最小であれば、転倒動作は完了である。
【０１８９】
図２２〜図３８、並びに図３９〜図５５には、実機が立位姿勢から仰向け姿勢に転倒していく様子を示している。
【０１９０】
この場合、機体の支持多角形の面積Ｓの時間ｔ当たりの変化量ΔＳ／Δｔを最小にするリンクとして、股関節ピッチ軸を含む胴体リンクを選択するとともに、目標着床点を探索して、機体の後方に倒れ込む（図２２〜図３１、並びに図３９〜図４８を参照のこと）。膝関節を折り畳んだ姿勢にして、着床時の支持多角形の変化量を最小、すなわち、ΔＳ／Δｔを最小にする。
【０１９１】
次いで、機体の支持多角形の面積Ｓの時間ｔ当たりの変化量ΔＳ／Δｔを最小にするリンクとして、体幹ピッチ軸９と肩関節ピッチ軸４を含む胴体リンクを選択するとともに、その目標着床点を探索して、さらに機体の後方に深く倒れる。このとき、既に股関節ピッチ軸１２が着床していることから、これを回転中心として体幹ピッチ軸９と肩関節ピッチ軸４を含む胴体リンクは着床する（図３２〜図３３、及び図４９〜図５０を参照のこと）。
【０１９２】
次いで、機体の支持多角形の面積Ｓの時間ｔ当たりの変化量ΔＳ／Δｔを最小にするリンクとして、首関節ピッチ軸２で連結されている頭部リンクを選択するとともに、その目標着床点を探索して、さらに機体の後方に深く倒れる。このとき、既に首関節ピッチ軸２が着床していることから、これを回転中心として頭部は着床する（図３４〜図３８、及び図５１〜図５５を参照のこと）。この姿勢において、機体の位置エネルギが最小であるから、転倒動作は完了である。
【０１９３】
また、図５６には、脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４などの高さ方向に連結された略平行な複数の関節軸からなるリンク構造体としてモデル化して、各関節ピッチ軸を同期強調的に駆動させてうつ伏せ姿勢に向かって転倒していく動作を示している。
【０１９４】
ロボットは、リンク構造体のリンク端である足底のみで立位しているとする（図５６（１））。
【０１９５】
このとき、外力又は外力モーメントの印加により、ＺＭＰ釣合い方程式のモーメント・エラー項Ｔをキャンセルできなくなり、足底のみで形成するＺＭＰ安定領域の外にＺＭＰが逸脱したことに応答して、ＺＭＰを支持多角形に維持したまま転倒動作を開始する。
【０１９６】
転倒動作では、まず、機体の支持多角形の面積Ｓの時間ｔ当たりの変化量ΔＳ／Δｔを最小にするリンクを探索するとともに、手先を含むリンクで変化量ΔＳ／Δｔを最小にするような手先の目標着床点を探索する。そして、選択されたリンクを目標着床点に着床することが機体ハードウェアの制約上（各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など）、実行可能かどうかを判別する。
【０１９７】
機体ハードウェア上実行可能である場合には、既に着床している足底リンクに加え、他のリンクが着床する。そして、これら着床リンクで形成される最小支持多角形内にＺＭＰを移動する（図５６（２））。
【０１９８】
次いで、機体ハードウェアが許容する限り、着床点を移動して、支持多角形を拡大していく（図５６（３））。
【０１９９】
そして、各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など機体ハードウェアの制約から、もはや着床点を移動することができなくなると、今度は、着床中のリンクに挟まれている離床リンクを着床することができるかどうかを判別する。
【０２００】
機体ハードウェア上、着床リンク間の離床リンクを着床することが可能である場合には、これらを着床して、着床リンク数を増やす（図５６（４））。
【０２０１】
さらに、機体ハードウェアが許容する限り、着床点を移動して、支持多角形を拡大していく（図５６（５））。
【０２０２】
そして最後に、高さ方向に連結された略平行な複数の関節軸からなるリンク構造体の一端側から１以上のリンクと、他端がわから２以上のリンクを離床させて、且つ、それらの中間に位置するリンクを１以上着床させ、さらに足部を着床させた状態で、ＺＭＰを支持多角形内に維持しながら、支持多角形が最大となる姿勢を形成する。この姿勢において、機体の位置エネルギが最小であれば、転倒動作は完了である。
【０２０３】
図５７〜図７３、並びに図７４〜図９０には、実機が立位姿勢から仰向け姿勢に転倒していく様子を示している。
【０２０４】
この場合、機体の支持多角形の面積Ｓの時間ｔ当たりの変化量ΔＳ／Δｔを最小にするリンクとして、肩関節ピッチ軸４を含む腕リンクの手先を選択するとともに、目標着床点を探索して、機体の前方に倒れ込む（図５７〜図７０、並びに図７４〜図８７を参照のこと）。
【０２０５】
このとき、最短の時間増分Δｔにおいて、着床時の支持多角形の変化量ΔＳを最小にするために、膝関節ピッチ軸１４を折り畳んだ姿勢にして、手先が着床する場所をより足底に近い位置に設定する。
【０２０６】
次いで、機体の支持多角形の面積Ｓの時間ｔ当たりの変化量ΔＳ／Δｔを最小にするリンクとして、膝関節ピッチ軸１４を含む脚部リンクを選択するとともに、その目標着床点を探索して、さらに機体の前方に深く倒れる。このとき、既に足部が着床していることから、足首ピッチ軸を回転中心として下腿部が旋回して、膝が着床する（図７０〜図７１、及び図８８〜図８９を参照のこと）。
【０２０７】
さらに、着床点としての手先と膝を足底から離すように移動して、機体ハードウェアが許容する限り、支持多角形を拡大する（図７２、及び図８９を参照のこと）。この結果、手先と膝に続いて胴体リンクも着床する（図７３、及び図９０を参照のこと）。この姿勢において、機体の位置エネルギが最小であるから、転倒動作は完了である。
【０２０８】
Ｆ．床上姿勢からの起き上がりオペレーション
仰向け姿勢やうつ伏せ姿勢などの床上姿勢からの起動を行なうため、あるいは、転倒時に自立的に起き上がって作業を再開するという作業の自己完結性のために、脚式移動ロボット１００は、起き上がりオペレーションを実現することが必要である。
【０２０９】
ところが、無計画的な軌道により起き上がろうとすると、過大な外力モーメントが印加されてしまい、関節アクチュエータが高出力トルクを必要とする。この結果、モータの大型化が必要となり、その分駆動消費電力が増大してしまう。また、機体の重量が増すとともに製造コストが高騰してしまう。重量の増大によりさらに起き上がり動作が困難になる。あるいは、起き上がり動作の過程で発生する外力モーメントにより姿勢の安定性を維持することができず、そもそも起き上がることができない、という事態もあり得る。
【０２１０】
そこで、本実施形態では、脚式移動ロボット１００は、外力モーメントが最小となる動作パターンよりなる起き上がり動作を行なうこととした。これは、ＺＭＰ支持多角形が最小となるような姿勢を時系列的に組み合わせることによって、実現することができる。
【０２１１】
また、本実施形態に係る脚式移動ロボット１００は、肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４のように（図３を参照のこと）、高さ方向に複数のピッチ軸が直列的（但し横方向から眺めた場合）に連結されたリンク構造体である。そこで、これら複数の関節ピッチ軸４〜１４を所定のシーケンスで同期協調的に駆動して、ＺＭＰ支持多角形が最小となるような動作パターンによる起き上がり動作を実現することとした。
【０２１２】
Ｆ−１．基本仰向け姿勢からの起き上がりオペレーション
図９１には、本実施形態に係る脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４を同期強調的に駆動させて起き上がり動作を行なうための処理手順をフローチャートの形式で示している。このような処理手順は、実際には主制御部８１において所定の機体動作制御プログラムを実行して、各部を駆動制御することによって実現される。
【０２１３】
また、図９２には、本実施形態に係る脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４を同期強調的に駆動させて仰向け姿勢から起き上がり動作を行なう様子を、関節リンク・モデルで示している。なお、本実施形態に係る脚式移動ロボット１００は体幹ピッチ軸９を備えているが、体幹ピッチ軸を備えていないタイプの脚式移動ロボットにおいて複数の関節ピッチ軸の同期駆動により仰向け姿勢から起き上がり動作を行なう様子を図９３に示しておく。但し、図示のリンク構造体において、体幹関節と股関節を連結するリンクに機体全体の重心位置が設定されており、このリンクを以下では「重心リンク」と呼ぶことにする。なお、「重心リンク」は狭義には上記のような定義で用いるが、広義には機体全体の重心位置が存在するリンクであればよい。例えば、体幹軸を持たないような機体においては、機体全体の重心が位置する体幹先端等を含むリンクがこれに該当する。
【０２１４】
以下、図９１に示したフローチャートを参照しながら、基本仰向け姿勢からの機体の起き上がりオペレーションについて説明する。
【０２１５】
まず、床上姿勢において、位置エネルギの最も小さい姿勢を探索する（ステップＳ１）。これは、基本仰向け姿勢に相当し、図９２（１）並びに図９３（１）に示すように、起き上がり動作に使用する肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４をそれぞれ連結するリンクはすべて接床している。このときの実機の状態を図９４及び図１１２に示している。位置エネルギの最も小さい姿勢をとることにより、路面の傾斜や形状を計測して、起き上がり動作が可能かどうかを確認することができる。
【０２１６】
この基本仰向け姿勢において、接床リンクが形成する接地多角形内で、最も狭い支持多角形を探索する（ステップＳ５２）。このとき、機体の一端側から少なくとも２以上のリンクを離床させたときの、ＺＭＰ軌道が計画可能かどうかを判定する。ＺＭＰの計画可能性は、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断することができる。
【０２１７】
次いで、接地多角形のうち、最も狭い支持多角形に関与しない２以上のリンクを離床する（ステップＳ５３）。
【０２１８】
ステップＳ５３は、図９２（２）及び図９３（２）に相当する。実機上では、体幹関節と股関節を連結する重心リンクを含む下半身側が支持多角形として抽出され、それ以外の肩関節から体幹関節に至る２以上のリンクを支持多角形に関与しないリンクとして離床する。
【０２１９】
このときの実機の動作を図９５〜図９６、並びに図１１３〜図１１４に示している。図示の例では、まず、左右の両腕部を持ち上げてから、体幹関節ピッチ軸アクチュエータＡ_９の駆動により、上体起こしを行なっている。腕部を先に持ち上げておくことにより、モーメントを小さくして、必要な最大トルクを低減することができる。
【０２２０】
次いで、一端側から１以上の離床リンクを屈曲させてリンク端の端部を着床させて、より狭い接地多角形を形成する（ステップＳ５４）。
【０２２１】
ステップＳ５４は、図９２（３）及び図９３（３）に相当する。実機上では、肩関節を含む２以上のリンクが離床している状態で、肩関節ピッチ軸で屈曲させて、そのリンク端の端部である手先を接床させる。そして、手先を機体重心位置である体幹ピッチ軸側に徐々に近づけていくことによって、元の床上姿勢よりも狭い接地多角形を形成する。
【０２２２】
このときの実機の動作を図９７〜図１０１、並びに図１１５〜図１１９に示している。図示の例では、左右の肩関節ロール軸Ａ_５の駆動により、左右の腕部を真横に広げた後、上腕ヨー軸Ａ_６の駆動により腕部の向きを一旦１８０度回転させてから（図９８〜図９９、図１１６〜図１１７）、肩関節ピッチ軸Ａ_４の駆動により、腕部を徐々に降下させていく。そして、手先を着床することによって、より狭い接地多角形を形成する（図１０１及び図１１９）。
【０２２３】
このように新しい接地多角形を形成すると、接地多角形にＺＭＰを設定することができるかどうかをチェックする（ステップＳ５５）。これは、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。そして、ＺＭＰを接地多角形に移動して、新たな支持多角形を形成する（ステップＳ５６）。
【０２２４】
ここで、支持多角形が充分狭くなったか否かを判断する（ステップＳ５７）。この判断は、体幹ピッチ軸と股関節ピッチ軸を連結する重心リンクを離床可能であるか、若しくは足部だけで形成できるＺＭＰ安定領域内にＺＭＰを移動させることができるかどうかを、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。支持多角形が充分狭くなったかどうかを判断する詳細な手順については後述に譲る。
【０２２５】
図１０１及び図１１９に示す実機の姿勢では、まだ支持多角形が充分狭いとは言えない。そこで、着床点を移動して支持多角形を小さくした後（ステップＳ５０）、ステップＳ５２に戻って、より狭い支持多角形の形成を再試行する。
【０２２６】
図１０１及び図１１９に姿勢において、接床リンクが形成する接地多角形内で、最も狭い支持多角形を探索する（ステップＳ５２）。今度は、機体の他端側から少なくとも２以上のリンクを離床させたときの、ＺＭＰが計画可能かどうかを判定する。ＺＭＰの計画可能性は、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断することができる。
【０２２７】
次いで、接床多角形のうち、最も狭い支持多角形に関与しない２以上のリンクを離床する（ステップＳ５３）。これは、図９２（４）〜（５）及び図９３（４）〜（５）に相当する。実機上では、膝関節ピッチ軸を含む他端側から連続する２以上のリンクを支持多角形に関与しないリンクとして離床する。
【０２２８】
そして、一端側から１以上の離床リンクを屈曲させてリンク端の端部を着床させて、より狭い接地多角形を形成する（ステップＳ５４）。
【０２２９】
このときの実機の動作を図１０２〜図１０５、並びに図１２０〜図１２３に示している。図示の例では、まず、右脚の股関節ピッチ軸Ａ_１２の駆動により右脚を持ち上げてから、その膝関節アクチュエータＡ_１４の駆動により右脚を屈曲させて、その足底を着床する。次いで、脚の股関節ピッチ軸Ａ_１２の駆動により右脚を持ち上げてから、その膝関節アクチュエータＡ_１４の駆動により左脚を屈曲させて、その足底を着床する。このようにして、足底を機体重心位置である股関節ピッチ軸１２側に徐々に近づけていくことによって、元の床上姿勢よりも狭い接地多角形を形成することができる。
【０２３０】
このように新しい接地多角形を形成すると、接地多角形にＺＭＰを設定することができるかどうかをチェックする（ステップＳ５５）。これは、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。そして、ＺＭＰを接地多角形に移動して、新たな支持多角形を形成する（ステップＳ５６）。
【０２３１】
ここで、支持多角形が充分狭くなったか否かを再び判断する（ステップＳ５７）。この判断は、体幹ピッチ軸と股関節ピッチ軸を連結する重心リンクを離床可能であるか、若しくは足部だけで形成できるＺＭＰ安定領域内にＺＭＰを移動させることができるかどうかを、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。図１０５及び図１２３に示す実機の姿勢では、充分狭い支持多角形が形成されていると判断される。支持多角形を縮小する際の腕の角度に関しては、肩の軸から床面方向に下ろした垂線と腕の中心軸がなす角度は、トルク量に基づく所定角度内であることが望ましい。
【０２３２】
そして、機体の支持多角形が充分に狭くなったことに応答して、支持多角形の両リンク端の端部を接床した状態で前記重心リンクを離床し、両リンク端の着床リンクによって形成される支持多角形内にＺＭＰを維持しながら、支持多角形を形成する両リンク端の端部の間隔を縮めて、ＺＭＰを前記リンク構造体の他端側に移動させていく（ステップＳ５８）。これは、図９２（６）〜（７）、並びに図９３（６）〜（７）に相当する。
【０２３３】
実機上では、接地多角形の両リンク端の端部としての手先及び足底を接床した状態で体幹ピッチ軸及び股関節ピッチ軸を連結した前記重心リンクを離床し、さらに、手先及び足底の間隔を徐々に縮めていき、ＺＭＰを足底に向かって移動させていく。また、このときの実機の動作を図１０６〜図１０９、並びに図１２４〜図１２７に示している。
【０２３４】
そして、前記リンク構造体の他端から第２の所定数以下の接床リンクのみで形成される接地多角形内にＺＭＰが突入したことに応答して、ＺＭＰを該接地多角形内に収容したまま前記リンク構造体の一端側から第１の所定数以上のリンクを離床して、該離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる（ステップＳ５９）。これは、図９２（８）、並びに図９３（８）に相当する。
【０２３５】
実機上では、足底で構成される接地多角形内にＺＭＰが突入したことに応答して、ＺＭＰを該接地多角形内に収容したまま、肩ピッチ軸４から膝ピッチ軸１４に至までのリンクを離床して、離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる。また、このときの実機の動作を図１１０〜図１１１、並びに図１２８〜図１２９に示している。
【０２３６】
起き上がりの最終段階である、離床リンクを長さ方向に伸展する際には、質量操作量のより大きな膝関節ピッチ軸を積極的に使用して動作することが、機体動作上の効率がよい。
【０２３７】
なお、ステップＳ５３において、最も小さい支持多角形に関与しない２以上のリンクを離床することができない場合には、最大の支持多角形より内側の２以上の着床リンクを離床することを試みる（ステップＳ６１）。
【０２３８】
ステップＳ６１を実行できない場合には、起き上がり動作を中止する（ステップＳ６４）。また、ステップＳ６１を成功裏に実行することができる場合には、さらに、着床点を移動させて、支持多角形をさらに小さくする（ステップＳ６２）。
【０２３９】
ステップＳ６２を実行できない場合には、起き上がり動作を中止する（ステップＳ６４）。また、ステップＳ６２を成功裏に実行することができる場合には、足部で形成できる安定領域にＺＭＰを移動することができるかどうかをチェックする（ステップＳ６３）。支持多角形が充分狭くなったかどうかを判断する詳細な手順については後述に譲る。この安定領域内にＺＭＰを移動することができない場合には、ステップＳ６１に戻って、支持多角形を小さくするための同様の処理を繰り返し実行する。また、この安定領域内にＺＭＰを移動させることができた場合には、ステップＳ５８に進んで、基本姿勢への復帰動作を行なう。
【０２４０】
ところで、ステップＳ５３〜Ｓ５４において、左右の手先を胴体後方で着床してより狭い接地多角形を形成するために、図９７〜図９８並びに図１１５〜図１１６に示すように、肩ロール軸を用いて左右の腕部を真横に広げるという動作を経ている。これは、脚式移動ロボット１００が起き上がり作業を行なうための使用体積をいたずらに増大させてしまっている。そこで、図９６〜図１０１並びに図１１３〜図１１９に示す一連の動作を、肩ロール軸を動作させず、代わりに肘ピッチ軸を屈曲させるという図１３０及び図１３１に示す動作に置き換えて、より小さな使用体積で左右の手先を胴体後方で着床するようにしてもよい。
【０２４１】
上述した起き上がり動作手順では、ステップＳ５７及びＳ６３において、支持多角形が充分狭くなったかどうかを判断する必要がある。図１７３には、支持多角形が充分狭くなったかどうかを判断するための処理手順をフローチャートの形式で示している。
【０２４２】
まず、ＺＭＰ偏差ε（ε_ｘ，ε_ｙ，ε_ｚ）、すなわち足部が形成できる安定領域の中心位置（ｘ_０，ｙ_０，ｚ_０）と現在のＺＭＰ位置（ｘ，ｙ，ｚ）との差分を求める（ステップＳ７１）。
【０２４３】
次いで、このＺＭＰ偏差ε（ε_ｘ，ε_ｙ，ε_ｚ）に所定のゲインＧ（Ｇ_ｘ，Ｇ_ｙ，Ｇ_ｚ）を掛算したものを現在の腰の位置ｒ（ｒ_{ｈｘ（ｔ）}，ｒ_{ｈｙ（ｔ）}，ｒ_{ｈｚ（ｔ）}）に加えて、次の時刻ｔ＝ｔ＋Δｔにおける目標腰位置ｒ（ｒ_{ｈｘ（ｔ＋ Δ ｔ）}，ｒ_{ｈｙ（ｔ＋ Δ ｔ）}，ｒ_{ｈｚ（ｔ＋ Δ ｔ）}）（＝ｒ（ｒ_{ｈｘ（ｔ）}，ｒ_{ｈｙ（ｔ）}，ｒ_{ｈｚ（ｔ）}）＋Ｇ（Ｇ_ｘ，Ｇ_ｙ，Ｇ_ｚ）×ε（ε_ｘ，ε_ｙ，ε_ｚ））にする（ステップＳ７２）。
【０２４４】
そして、次の目標腰位置で現在の支持多角形を形成することができるかどうかを判別する（ステップＳ７３）。この判別は、着床リンクの着床点を維持しながら、次の目標腰位置を計算することによって行なわれる。すなわち、腰位置と着床点から逆運動学計算を行ない、可動角度以内で且つ関節アクチュエータの許容トルク以内であれば実現可能と判断される。
【０２４５】
次の目標腰位置で現在の支持多角形を形成することができなければ、足部が形成できる安定領域内にＺＭＰを移動することが不可能であるとして、本処理ルーチン全体を終了する。
【０２４６】
他方、次の目標腰位置で現在の支持多角形を形成することができるならば、さらに、次の目標腰位置に腰を移動した場合の（すなわち次の）ＺＭＰを算出する（ステップＳ７４）。
【０２４７】
次いで、足部が形成できる安定領域内にＺＭＰが存在するかどうかを判別する（ステップＳ７５）。判別結果が肯定的であれば、足部が形成できる安定領域内にＺＭＰを移動することができると判断して（ステップＳ７６）、本処理ルーチン全体を終了する。他方、判別結果が否定的であれば、次の腰位置を現在の腰位置に、次のＺＭＰを現在のＺＭＰにした後、ステップＳ７１に戻って同様の処理を繰り返し実行する。
【０２４８】
なお、図１３０及び図１３１に示す動作例では、上腕の長さをｌ_１、前腕の長さをｌ_２、肩ロール角をα、肘ピッチ角をベータ、肩から手先までの長さをｌ_１２、肩から手先を結ぶ線のなす角をγ、肩の高さをｈと置くと（図１３２）、左右の手先を胴体後方で着床する動作期間中は、以下の式を満たすように肘ピッチ軸７を動作させることにより、手先が床面と衝突することはない。
【０２４９】
【数６】

【０２５０】
また、図９２に示す起き上がり動作パターンは、脚式移動ロボットの機体が肩関節ピッチ軸、体幹ピッチ軸、股関節ピッチ軸、膝ピッチ軸が機体の高さ方向に連結されてなるリンク構造体にモデル化して起き上がり動作を示している。図１３３には、脚式移動ロボットを略平行な関節自由度を持つ複数の関節軸を長さ方向に連結したリンク構造体に一般化して、起き上がり動作を示している。
【０２５１】
同図に示すリンク構造体は、略平行な関節自由度を持つ複数の関節軸を長さ方向に連結して構成される。すべてのリンクが接床している床上姿勢からの起き上がり動作を、リンクＡ、リンクＢ、リンクＣ、リンクＤ、リンクＥ、並びにリンクＦを用いて実現する。
【０２５２】
但し、リンクＡ〜Ｆは、それぞれ単一のリンクである必要はなく、実際には複数のリンクが関節軸を介して連結されているが、起き上がり動作の期間中は関節軸が作動せずリンク間の真直性が保たれて、あたかも単一のリンクであるように振る舞う場合も含むものとする。例えば、リンクＡはリンク端からｈ番目までのリンクを含み、リンクＢはｈ番目以降ｉ番目までのリンクを含み、リンクＣは、ｉ番目以降ｊ番目までのリンクを含み、リンクＤはｊ番目以降ｋ番目までのリンクを含み、リンクＥはｋ番目以降ｌ番目までのリンクを含み、リンクＦはｌ番目以降ｍ番目（若しくはリンクの他端）までのリンクを含んである。
【０２５３】
まず、Ｆ番目リンクとＡ番目リンクの間に接地多角形を形成して、この接地多角形内にＺＭＰを設定する（図１３３（１））。
【０２５４】
次いで、Ｅ番目リンクとＡ番目リンクの間の接地多角形内にＺＭＰを設定する（図１３３（２））。このとき、リンク端から２以上のリンクを離床させるなどＦ番目リンクの運動を用いてもよい。
【０２５５】
次いで、Ｆ番目リンクとＡ番目リンクの間により狭い接地多角形を新たに形成して、この接地多角形内にＺＭＰを設定する（図１３３（３））。例えば離床中のＦ番目リンクを屈曲させてその端部を着床させて、新しい接地多角形を形成する。
【０２５６】
次いで、Ｆ番目リンクとＤ番目またはＣ番目リンクの間で接地多角形を新たに形成して、この接地多角形内にＺＭＰを設定する（図１３３（４））。このとき、他方のリンク端から２以上のリンクを離床させるなどＡ番目リンクの運動を用いてもよい。
【０２５７】
次いで、Ｄ番目リンクを接地させて、Ｆ番目リンク及びＡ番目リンクで接地多角形を新たに形成して、この接地多角形内にＺＭＰを設定する（図１３３（５））。例えば離床中のＡ番目リンクを屈曲させてその端部を着床させて、新しい接地多角形を形成する。
【０２５８】
次いで、Ｆ番目リンクとＡ番目リンクで接地多角形を新たに形成して、この接地多角形内にＺＭＰを設定する（図１３３（６））。例えば、両方のリンク端の端点を着床したままで、着床中のＤ番目リンクを離床させる。
【０２５９】
次いで、両方のリンク端Ｆ及びＡの端点を一致させることにより、Ａ番目のリンクのみが形成する支持多角形内にＺＭＰを移動させる（図１３３（７））。
【０２６０】
そして最後に、Ａ番目リンクのみが形成する支持多角形内にＺＭＰを設定しながら、各リンクを基本立ち姿勢へ移動させる（図１３３（８））。
【０２６１】
Ｆ−２．基本うつ伏せ姿勢からの起き上がりオペレーション
図１３４には、本実施形態に係る脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４を同期強調的に駆動させて起き上がり動作を行なう様子を、関節リンク・モデルで示している。
【０２６２】
本実施形態に係る脚式移動ロボット１００は、基本的には、仰向け姿勢から起き上がる場合と同様に、図９１にフローチャートの形式で示した処理手順に従って、うつ伏せ姿勢からも起き上がることができる。以下、図９１に示したフローチャートを参照しながら、基本うつ伏せ姿勢からの機体の起き上がりオペレーションについて説明する。
【０２６３】
まず、床上姿勢において、位置エネルギの最も小さい姿勢をとる（ステップＳ５１）。これは、基本うつ伏せ姿勢に相当し、図１３４（１）に示すように、起き上がり動作に使用する肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４をそれぞれ連結するリンクはすべて接床している。このときの実機の状態を図１３５及び図１５４に示している。
【０２６４】
この基本うつ伏せ姿勢において、接床リンクが形成する接地多角形内で、最も狭い支持多角形を探索する（ステップＳ５２）。このとき、機体の一端側から少なくとも２以上のリンクを離床させたときの、ＺＭＰが計画可能かどうかを判定する。ＺＭＰの計画可能性は、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断することができる。
【０２６５】
次いで、接地多角形のうち、最も狭い支持多角形に関与しない２以上のリンクを離床する（ステップＳ５３）。ステップＳ５３は、図１３４（２）に相当する。実機上では、体幹関節と股関節を連結する重心リンクを含む下半身側が支持多角形として抽出され、それ以外の肩関節から体幹関節に至る２以上のリンクを支持多角形に関与しないリンクとして離床する。
【０２６６】
このときの実機の動作を図１３６〜図１４４、並びに図１５５〜図１６３に示している。図示の例では、まず、左右の両腕部の肩ロール軸アクチュエータＡ_５を作動させて、床面に摺って肩ロール軸回りに略９０度だけ旋回させて（図１３６〜図１３７、並びに図１５５〜図１５６）、次いで、上腕ヨー軸アクチュエータＡ_６を作動させて、各腕部を上腕ヨー軸回りに略１８０度だけ回転させる（図１３８並びに図１５７）。そして、さらに肩ロール軸アクチュエータＡ_５を作動させて、摺って肩ロール軸回りに略９０度だけ旋回させて、各腕部を頭部の側面まで移動する（図１３８〜図１４１、並びに図１５７〜図１６０）。
【０２６７】
図１３６〜図１４１、並びに図１６５〜図１７０に示す一連の動作では、左右の腕部は床面上で半円を描く格好となっている。このとき、機体周辺の路面において障害物の有無を検出したりして、起き上がり動作に必要な安全な作業領域の確保を行なうことができる。
【０２６８】
次いで、一端側から１以上の離床リンクを屈曲させてリンク端の端部を着床させて、より狭い接地多角形を形成する（ステップＳ５４）。ステップＳ５４は、図１３４（３）に相当する。
【０２６９】
そして、新しい接地多角形を形成すると、接地多角形にＺＭＰを設定することができるかどうかをチェックする（ステップＳ５５）。これは、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。そして、ＺＭＰを接地多角形に移動して、新たな支持多角形を形成する（ステップＳ５６）。
【０２７０】
実機上では、肘ピッチ軸７を固定させて、左右の腕部を真直ぐ伸ばしたままの状態で、今度は肩ピッチ軸アクチュエータＡ_４、体幹ピッチ軸アクチュエータＡ_９、股関節ピッチ軸Ａ_１２、並びに膝関節ピッチ軸アクチュエータＡ_１４を作動させて、手先と左右の両膝が接地した閉リンク姿勢からなる支持多角形を形成する（図１４２〜図１４４、並びに図１６１〜図１６３）。
【０２７１】
図１４４及び図１５３に示す実機の姿勢では、まだ支持多角形が充分狭いとは言えない。そこで、着床点を移動して支持多角形を小さくする（ステップＳ６０）。支持多角形を縮小する際の腕の角度に関しては、肩の軸から床面方向に下ろした垂線と腕の中心軸がなす角度は、トルク量に基づく所定角度内であることが望ましい。
【０２７２】
実機上では、左右の腕部をまっすぐに保ったまま、手先を他方の着床点である足底側に徐々に近づけていくことによって、より狭い支持多角形を形成していく（図１４５〜図１４８、並びに図１６４〜図１６７）。
【０２７３】
ここで、支持多角形が充分狭くなったか否かを判断する（ステップＳ５７）。この判断は、体幹ピッチ軸と股関節ピッチ軸を連結する重心リンクを離床可能であるか、若しくは足部だけで形成できるＺＭＰ安定領域内にＺＭＰを移動させることができるかどうかを、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。図１４８及び図１６５に示す実機の姿勢では、充分狭い支持多角形が形成されていると判断される。
【０２７４】
そして、機体の支持多角形が充分に狭くなったことに応答して両リンク端の着床リンクによって形成される支持多角形内にＺＭＰを維持しながら、支持多角形を形成する両リンク端の端部の間隔を縮めて、ＺＭＰを前記リンク構造体の他端側に移動させていく（ステップＳ５８）。これは、図１３４（６）〜（７）に相当する。
【０２７５】
実機上では、接地多角形の両リンク端の端部としての手先及び足底を接床した状態で、さらに手先及び足底の間隔を徐々に縮めていき、ＺＭＰを足底に向かって移動させていく。また、このときの実機の動作を図１４９〜図１５０、並びに図１６８〜図１６９に示している。
【０２７６】
そして、前記リンク構造体の他端から第２の所定数以下の接床リンクのみで形成される接地多角形内にＺＭＰが突入したことに応答して、ＺＭＰを該接地多角形内に収容したまま前記リンク構造体の一端側から第１の所定数以上のリンクを離床して、該離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる（ステップＳ５９）。これは、図１３４（８）に相当する。
【０２７７】
実機上では、足底で構成される接地多角形内にＺＭＰが突入したことに応答して、ＺＭＰを該接地多角形内に収容したまま、肩ピッチ軸４から膝ピッチ軸１４に至までのリンクを離床して、離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる。また、このときの実機の動作を図１５１〜図１５３、並びに図１７０〜図１７２に示している。
【０２７８】
起き上がりの最終段階である、離床リンクを長さ方向に伸展する際には、質量操作量のより大きな膝関節ピッチ軸を積極的に使用して動作することが、機体動作上の効率がよい。
【０２７９】
Ｆ−３．他の起き上がりオペレーションの例
図９１で示した起き上がりオペレーションでは、ＺＭＰ支持多角形が最小となるような姿勢を時系列的に組み合わせることによって、外力モーメントが最小となる動作パターンよりなる起き上がり動作を行なうこととした。この動作では、より小さな支持多角形を順次形成していく過程において、手部や足部の踏み替え動作を利用していた。しかしながら、踏み替え動作を実現するためには、手部又は足部が離床する必要があり、支持多角形に関与しない２以上のリンクがなければならず、機体の姿勢によっては踏み替え動作を行なえない場合があり、この場合は起き上がり動作そのものが破綻してしまう（図９１のステップＳ６４）。
【０２８０】
これに対し、より小さな支持多角形を順次形成していく過程において、手部や足部の踏み替え動作を実現できない場合には、手部や足部の引き摺り動作を利用することにより、起き上がり動作が破綻してしまう機会を少なくすることができる。以下では、より小さな支持多角形を順次形成していく過程において、手部や足部の踏み替え動作と引き摺り動作を利用した起き上がりオペレーションについて説明する。
【０２８１】
図１７４には、手部や足部の踏み替え動作と引き摺り動作を利用した起き上がりオペレーションをフローチャートの形式で示している。以下、この起き上がり動作手順について説明する。図１７５〜図１９１には、基本うつ伏せ姿勢から手部又は足部の踏み替え動作又は引き摺り動作を利用しながら機体が起き上がりを行なう様子を順に示している。以下では、各図を適宜参照する。
【０２８２】
まず、床上姿勢において、位置エネルギの最も小さい姿勢をとる（ステップＳ８１）。これは、基本うつ伏せ姿勢に相当し、このときの実機の状態を図１７５に示している。
【０２８３】
但し、転倒動作と連続して起き上がり行なう場合は、ステップＳ８１を省略することにより、短時間で起き上がり動作を完了させることができる（後述）。
【０２８４】
この基本うつ伏せ姿勢において、接床リンクが形成する接地多角形内で、最も狭い支持多角形を探索する（ステップＳ８２）。このとき、機体の一端側から少なくとも２以上のリンクを離床させたときの、ＺＭＰが計画可能かどうかを判定する。ＺＭＰの計画可能性は、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断することができる。
【０２８５】
ここで、最も小さい支持多角形に関与しないリンクの２つ以上を離床することができるかどうかを判断する（ステップＳ８３）。最小の支持多角形に関与しない２以上のリンクを離床することができる場合には、次ステップＳ８４へ進み、手部又は足部の踏み替え動作によるより小さい接地多角形の形成を行なう。一方、離床することができない場合には、ステップＳ９１へ進み、手部又は足部の引き摺り動作を利用してより小さい接地多角形の形成を行なう。
【０２８６】
ステップＳ８４では、最も小さい支持多角形に関与しないリンクの２つ以上を離床させ、さらに、離床リンクを屈曲及び着床させて、より小さい接地多角形を形成する（ステップＳ８５）。
【０２８７】
例えば、図１７９〜図１８１、並びに図１８４〜図１８６において、両手両足を接地して起き上がり途上のロボットが、左足と右足を踏み替えながら、図１７５、図１８２〜図１８３、図１８５、図１８７に示すように離床リンクを屈曲及び着床させて、より小さい接地多角形の形成を試みている。
【０２８８】
そして、新しい接地多角形を形成すると、接地多角形にＺＭＰを設定することができるかどうかをチェックする（ステップＳ８６）。これは、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。そして、ＺＭＰを接地多角形に移動して、新たな支持多角形を形成する（ステップＳ８７）。接地多角形にＺＭＰを設定することができない場合には、ステップＳ８３に戻り、手部や足部の踏み替え動作又は引き摺り動作のいずれを実行すべきかを改めてチェックする。
【０２８９】
ここで、支持多角形が充分狭くなったか否かを判断する（ステップＳ８８）。この判断は、体幹ピッチ軸と股関節ピッチ軸を連結する重心リンクを離床可能であるか、若しくは足部だけで形成できるＺＭＰ安定領域内にＺＭＰを移動させることができるかどうかを、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。
【０２９０】
そして、機体の支持多角形が充分に狭くなったことに応答して両リンク端の着床リンクによって形成される支持多角形内にＺＭＰを維持しながら、支持多角形を形成する両リンク端の端部の間隔を縮めて、ＺＭＰを前記リンク構造体の他端側に移動させていく（ステップＳ８９）。
【０２９１】
実機上では、接地多角形の両リンク端の端部としての手先及び足底を接床した状態で、さらに手先及び足底の間隔を徐々に縮めていき、ＺＭＰを足底に向かって移動させていく。また、このときの実機の動作を図１８８〜図１８９に示している。
【０２９２】
そして、前記リンク構造体の他端から第２の所定数以下の接床リンクのみで形成される接地多角形内にＺＭＰが突入したことに応答して、ＺＭＰを該接地多角形内に収容したまま前記リンク構造体の一端側から第１の所定数以上のリンクを離床して、該離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる（ステップＳ９０）。
【０２９３】
実機上では、足底で構成される接地多角形内にＺＭＰが突入したことに応答して、ＺＭＰを該接地多角形内に収容したまま、肩ピッチ軸４から膝ピッチ軸１４に至までのリンクを離床して、離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる。また、このときの実機の動作を図１９０〜図１９１に示している。
【０２９４】
起き上がりの最終段階である、離床リンクを長さ方向に伸展する際には、質量操作量のより大きな膝関節ピッチ軸を積極的に使用して動作することが、機体動作上の効率がよい。
【０２９５】
一方、ステップＳ８３において、最も小さい支持多角形に関与しないリンクの２つ以上を離床することができないと判断された場合には、手部又は足部の引き摺り動作を行なうべく、最大の支持多角形より内の着床中のリンクを２つ以上離床することができるかどうかをチェックする（ステップＳ９１）。
【０２９６】
ここで、最大の支持多角形より内の着床中のリンクを２つ以上離床することができない場合には、さらに着床点を移動させて支持多角形を小さくすることができるかどうかを判断する。支持多角形を小さくすることができない場合には、起き上がり動作を中止する（ステップＳ９５）。すなわち、起き上がり動作は破綻する。
【０２９７】
一方、最大の支持多角形より内の着床中のリンクを２つ以上離床することができる場合には、最大の支持多角形より内の着床中のリンクを２つ以上離床して（ステップＳ９２）、手部又は足部の引き摺り動作を利用して、着床点を移動させ、支持多角形を小さくする（ステップＳ９３）。
【０２９８】
例えば、図１７６〜図１７８、並びに図１８７〜図１８８に示すように、両手両足を接地して起き上がり途上のロボットが、両手を着床させたまま足に向かって引き摺ることにより支持多角形を徐々に小さくしていく。
【０２９９】
その後、支持多角形が充分狭くなったか否かを判断する（ステップＳ８８）。そして、機体の支持多角形が充分に狭くなったことに応答して両リンク端の着床リンクによって形成される支持多角形内にＺＭＰを維持しながら、支持多角形を形成する両リンク端の端部の間隔を縮めて、ＺＭＰを前記リンク構造体の他端側に移動させていく（ステップＳ８９）。
【０３００】
そして、前記リンク構造体の他端から第２の所定数以下の接床リンクのみで形成される接地多角形内にＺＭＰが突入したことに応答して、ＺＭＰを該接地多角形内に収容したまま前記リンク構造体の一端側から第１の所定数以上のリンクを離床して、該離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる（ステップＳ９０）。
【０３０１】
図１９９には、ステップＳ８３において、最も小さい支持多角形に関与しないリンク数が最大となるリンクとその部位を探索するための詳細な処理手順をフローチャートの形式で示している。
【０３０２】
まず、ステップＳ１０１及びＳ１０２において、変数ｉ、ｊ並びに配列型変数Ｍを初期化する。次いで、ｉ番目のリンクのｊ番目の部位にＺＭＰを設定する（ステップＳ１０３）。
【０３０３】
ここで、ＺＭＰ空間が安定かどうかを判別する（ステップＳ１０４）。ＺＭＰ空間が安定である場合には、最も小さい支持多角形に関与しないリンク数を計算して（ステップＳ１０５）、ｉ番目のリンクのｊ番目の部位における離床叶リンク数をＬに代入する。そして、ＬがＭよりも大きければ（ステップＳ１０６）、Ｍ（Ａ，Ｂ）にＬ（ｉ，ｊ）を代入する（ステップＳ１０７）。
【０３０４】
一方、ＺＭＰ空間が安定でない場合、ＬがＭよりも大きくない場合、あるいはＭ（Ａ，Ｂ）にＬ（ｉ，ｊ）を代入した後、ｊを１だけ増分して（ステップＳ１０８）、ｊが総部位数Ｊを越えたかどうかを判別する（ステップＳ１０９）。ｊがまだ総部位数Ｊに達していない場合には、ステップＳ１０３に戻って、上述と同様の処理を繰り返し実行する。
【０３０５】
次いで、ｉを１だけ増分して（ステップＳ１１０）、ｉが総リンク数Ｉを越えたかどうかを判別する（ステップＳ１１１）。ｉが総リンク数に達していない場合には、ステップＳ１０２に戻って、上述と同様の処理を繰り返し実行する。
【０３０６】
ｉが総リンク数Ｉを越えた場合には、Ａにリンク、Ｂに部位を代入し、本処理ルーチンを終了する。
【０３０７】
前述したように、転倒動作と連続して起き上がり行なう場合は、ステップＳ８１を省略することにより、短時間で起き上がり動作を完了させることができる。
【０３０８】
例えば、機体の重心が腰部に存在する場合、最も小さくなる支持多角形に関与しないリンク数が最大となる部位にＺＭＰを設定することができる。このような転倒・着床動作の後、離床可能なリンクをすべて離床させる、すなわち下肢と体幹の双方を浮き上がらせて、上体と下肢を同時に離床し、足部、手部などを着床させることで、より小さい接地多角形を少ないステップで形成することができるので、より高速で効率的な起き上がり動作を実現することができる。
【０３０９】
図１９２〜図１９８には、転倒動作と連続して起き上がり動作を行なう場合の機体の一連の動作を示している。
【０３１０】
図１９２に示す立位姿勢から、図１９２〜図１９３に示すように機体後方に向かって転倒動作を開始し、図１９４に示すように機体重心が存在する腰部において着床する。
【０３１１】
図１９４に示す例では、最も小さい支持多角形に関与しないリンク数が最大になる胴体部にＺＭＰが設定されている。また、特徴的なことは、図２３〜図３８、及び図３９〜図５５を参照しながら説明した例とは相違し、基本仰向けではなく、脚部が離床した状態で転倒動作が終了している点にある。
【０３１２】
続く起き上がり動作では、図１９５に示すように、離床可能なリンクすなわち脚部と胴体部をすべて離床させて、起き上がり動作を開始する。ここで、股関節及び／又は体幹のピッチ軸アクチュエータの駆動により、図１９６〜図１９７に示すように上体が起き上がる。そして、右脚の股関節ピッチ軸Ａ_１２の駆動により右脚を持ち上げてから、その膝関節アクチュエータＡ_１４の駆動により右脚を屈曲させて、その足底を着床する。次いで、脚の股関節ピッチ軸Ａ_１２の駆動により右脚を持ち上げてから、その膝関節アクチュエータＡ_１４の駆動により左脚を屈曲させて、その足底を着床する。このようにして、足底を機体重心位置である股関節ピッチ軸１２側に徐々に近づけていくことによって、図１９８に示すように、元の床上姿勢よりも狭い接地多角形を形成することができる。
【０３１３】
転倒動作と連続して起き上がり動作を行なう場合、図２３〜図３８、及び図３９〜図５５を参照しながら説明した例に比べて、より小さい接地多角形を少ないステップで形成することができる。すなわち、この実施形態によればより効率的に狭い接地多角形を形成することができる、起き上がり動作が高速化されるという点を充分理解されたい。
【０３１４】
［追補］
以上、特定の実施例を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施例の修正や代用を成し得ることは自明である。
【０３１５】
本発明の要旨は、必ずしも「ロボット」と称される製品には限定されない。すなわち、電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行なう機械装置であるならば、例えば玩具等のような他の産業分野に属する製品であっても、同様に本発明を適用することができる。
【０３１６】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、冒頭に記載した特許請求の範囲の欄を参酌すべきである。
【０３１７】
【発明の効果】
本発明によれば、転倒・落下の途上における脚部だけでなく胴体及び腕部を含め機体全体の運動制御によりロボットが被る損害を限りなく軽減することができる、優れた脚式移動ロボット及び脚式移動ロボットの転倒時動作制御方法を提供することができる。
【０３１８】
また、本発明によれば、仰向けやうつ伏せなどの床上姿勢から自律的に立ち姿勢を回復することができる、優れた脚式移動ロボットの動作制御装置及び動作制御方法、並びにロボット装置を提供することができる。
【０３１９】
また、本発明によれば、比較的少ないトルクで安定した動作により仰向けやうつ伏せなどの床上姿勢から立ち姿勢を回復することができる、優れた脚式移動ロボットの動作制御装置及び動作制御方法、並びにロボット装置を提供するができる。
【図面の簡単な説明】
【図１】本発明の実施に供される脚式移動ロボットが直立している様子を前方から眺望した様子を示した図である。
【図２】本発明の実施に供される脚式移動ロボットが直立している様子を後方から眺望した様子を示した図である。
【図３】脚式移動ロボットが具備する関節自由度構成を模式的に示した図である。
【図４】脚式移動ロボット１００の制御システム構成を模式的に示した図である。
【図５】脚式移動ロボット１００の運動系が持つ基本状態遷移を示した図である。
【図６】脚式移動ロボット１００の基本仰向け姿勢を示した図である。
【図７】脚式移動ロボット１００の基本うつ伏せ姿勢を示した図である。
【図８】脚式移動ロボット１００の基本立ち姿勢を示した図である。
【図９】脚式移動ロボット１００の基本歩行姿勢を示した図である。
【図１０】脚式移動ロボット１００の多質点近似モデルを示した図である。
【図１１】多質点モデルの腰部周辺の拡大図を示した図である。
【図１２】脚式移動ロボット１００において安定歩行可能な機体運動を生成するための処理手順を示したフローチャートである。
【図１３】脚式移動ロボット１００における脚式作業中の機体の動作制御の概略的な処理手順を示したフローチャートである。
【図１４】機体の転倒時に支持面積を維持する原理を説明するための図である。
【図１５】機体の床面落下時における支持多角形が最大となる原理を説明するための図である。
【図１６】脚式移動ロボット１００が後方すなわち仰向け姿勢に向かって転倒する場合に、転倒時の支持面積を維持する動作を説明するための図である。
【図１７】脚式移動ロボット１００が後方すなわち仰向け姿勢に向かって転倒する場合に、転倒時の支持面積を維持する動作を説明するための図である。
【図１８】脚式移動ロボット１００が前方すなわちうつ伏せ姿勢に向かって転倒する場合に、転倒時の支持面積を維持する動作を説明するための図である。
【図１９】脚式移動ロボット１００が前方すなわちうつ伏せ姿勢に向かって転倒する場合に、転倒時の支持面積を維持する動作を説明するための図である。
【図２０】本実施形態に係る脚式移動ロボット１００が足部の計画不能のために転倒動作を行なうための処理手順を示したフローチャートである。
【図２１】脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４などの高さ方向に連結された略平行な複数の関節軸からなるリンク構造体としてモデル化して、各関節ピッチ軸を同期強調的に駆動させて仰向け姿勢に向かって転倒していく動作を示した図である。
【図２２】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図２３】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図２４】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図２５】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図２６】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図２７】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図２８】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図２９】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３０】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３１】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３２】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３３】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３４】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３５】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３６】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３７】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３８】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した側面図である。
【図３９】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４０】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４１】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４２】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４３】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４４】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４５】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４６】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４７】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４８】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図４９】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図５０】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図５１】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図５２】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図５３】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図５４】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図５５】脚式移動ロボット１００が立位姿勢から仰向け姿勢に転倒していく様子を示した斜視図である。
【図５６】脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４などの高さ方向に連結された略平行な複数の関節軸からなるリンク構造体としてモデル化して、各関節ピッチ軸を同期強調的に駆動させてうつ伏せ姿勢に向かって転倒していく動作を示した図である。
【図５７】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図５８】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図５９】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６０】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６１】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６２】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６３】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６４】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６５】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６６】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６７】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６８】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図６９】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図７０】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図７１】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図７２】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図７３】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した側面図である。
【図７４】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図７５】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図７６】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図７７】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図７８】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図７９】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８０】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８１】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８２】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８３】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８４】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８５】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８６】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８７】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８８】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図８９】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図９０】脚式移動ロボット１００が立位姿勢からうつ伏せ姿勢に転倒していく様子を示した斜視図である。
【図９１】本発明の一実施形態に係る脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４を同期強調的に駆動させて起き上がり動作を行なうための処理手順を示したフローチャートである。
【図９２】本発明の一実施形態に係る脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４を同期強調的に駆動させて仰向け姿勢から起き上がり動作を行なう様子を、関節リンク・モデルで示した図である。
【図９３】体幹ピッチ軸を備えていないタイプの脚式移動ロボットにおいて複数の関節ピッチ軸の同期駆動により仰向け姿勢から起き上がり動作を行なう様子を示した図である。
【図９４】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図９５】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図９６】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図９７】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図９８】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図９９】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１００】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１０１】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１０２】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１０３】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１０４】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１０５】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１０６】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１０７】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１０８】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１０９】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１１０】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１１１】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１１２】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１１３】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１１４】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１１５】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１１６】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１１７】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１１８】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１１９】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２０】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２１】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２２】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２３】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２４】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２５】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２６】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２７】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２８】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１２９】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した斜視図である。
【図１３０】左右の手先を胴体後方で着床する一連の動作の変形例を示した図である。
【図１３１】左右の手先を胴体後方で着床する一連の動作の変形例を示した図である。
【図１３２】図１３０及び図１３１に示した腕の動作を説明するための図である。
【図１３３】図９２に示した脚式移動ロボットをリンク構造体に置き換えて一般化して示した図である。
【図１３４】本発明の一実施形態に係る脚式移動ロボット１００が肩関節ピッチ軸４、体幹ピッチ軸９、股関節ピッチ軸１２、膝関節ピッチ軸１４を同期強調的に駆動させてうつ伏せ姿勢から起き上がり動作を行なう様子を、関節リンク・モデルで示した図である。
【図１３５】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１３６】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１３７】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１３８】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１３９】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４０】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４１】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４２】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４３】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４４】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４５】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４６】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４７】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４８】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１４９】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１５０】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１５１】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１５２】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１５３】脚式移動ロボット１００が基本仰向け姿勢から起き上がる様子を示した側面図である。
【図１５４】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１５５】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１５６】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１５７】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１５８】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１５９】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６０】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６１】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６２】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６３】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６４】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６５】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６６】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６７】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６８】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１６９】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１７０】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１７１】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１７２】脚式移動ロボット１００が基本うつ伏せ姿勢から起き上がる様子を示した斜視図である。
【図１７３】支持多角形が充分狭くなったかどうかを判断するための処理手順を示したフローチャートである。
【図１７４】手部や足部の踏み替え動作と引き摺り動作を利用した起き上がりオペレーションを示したフローチャートである。
【図１７５】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１７６】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１７７】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１７８】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１７９】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８０】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８１】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８２】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８３】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８４】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８５】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８６】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８７】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８８】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１８９】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１９０】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１９１】脚式移動ロボット１００が手部や足部の踏み替え動作と引き摺り動作を利用しながら基本うつ伏せ姿勢から起き上がる様子を示した側面図である。
【図１９２】転倒動作と連続して起き上がり行なう場合における機体の一連の動作を示した図である。
【図１９３】転倒動作と連続して起き上がり行なう場合における機体の一連の動作を示した図である。
【図１９４】転倒動作と連続して起き上がり行なう場合における機体の一連の動作を示した図である。
【図１９５】転倒動作と連続して起き上がり行なう場合における機体の一連の動作を示した図である。
【図１９６】転倒動作と連続して起き上がり行なう場合における機体の一連の動作を示した図である。
【図１９７】転倒動作と連続して起き上がり行なう場合における機体の一連の動作を示した図である。
【図１９８】転倒動作と連続して起き上がり行なう場合における機体の一連の動作を示した図である。
【図１９９】最も小さい支持多角形に関与しないリンク数が最大となるリンクとその部位を探索するための処理手順を示したフローチャートである。
【符号の説明】
１…首関節ヨー軸
２Ａ…第１の首関節ピッチ軸
２Ｂ…第２の首関節（頭）ピッチ軸
３…首関節ロール軸
４…肩関節ピッチ軸
５…肩関節ロール軸
６…上腕ヨー軸
７…肘関節ピッチ軸
８…手首関節ヨー軸
９…体幹ピッチ軸
１０…体幹ロール軸
１１…股関節ヨー軸
１２…股関節ピッチ軸
１３…股関節ロール軸
１４…膝関節ピッチ軸
１５…足首関節ピッチ軸
１６…足首関節ロール軸
３０…頭部ユニット，４０…体幹部ユニット
５０…腕部ユニット，５１…上腕ユニット
５２…肘関節ユニット，５３…前腕ユニット
６０…脚部ユニット，６１…大腿部ユニット
６２…膝関節ユニット，６３…脛部ユニット
８０…制御ユニット，８１…主制御部
８２…周辺回路
９１，９２…接地確認センサ
９３，９４…加速度センサ
９５…姿勢センサ
９６…加速度センサ
１００…脚式移動ロボット[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a motion control device and a motion control method for a legged mobile robot having a large number of degrees of freedom, and to a robot device, and more particularly to a legged mobile robot having a plurality of movable legs and having a basic standing posture. And an operation control method, and a robot apparatus.
[0002]
More specifically, the present invention relates to a motion control device and a motion control method for a legged mobile robot that stabilizes and controls the posture of a moving body using ZMP (Zero \ Moment \ Point) as a posture stability determination criterion. In particular, a leg system that minimizes the damage suffered by the robot by controlling the motion of the entire body in the process of falling and falling, and recovers the standing posture from the floor posture such as supine or prone by stable operation with relatively little torque. The present invention relates to an operation control device and an operation control method for a mobile robot, and a robot device.
[0003]
[Prior art]
A mechanical device that performs a motion resembling a human motion using an electric or magnetic action is called a “robot”. It is said that the robot is derived from the Slavic word "ROBOTA (slave machine)". In Japan, robots began to spread from the late 1960s, but most of them were industrial robots (industrial robots) such as manipulators and transfer robots for the purpose of automation and unmanned production work in factories. Met.
[0004]
Recently, it has been designed based on a pet-type robot that imitates the body mechanism and movement of a four-legged animal such as a dog or cat, or a body mechanism or movement of an animal that walks two-legged upright such as a human. Research and development on legged mobile robots such as the “humanoid” or “humanoid” robot has been progressing, and expectations for its practical use are increasing.
[0005]
The significance of researching and developing a two-legged upright legged mobile robot called a humanoid or humanoid will be understood from the following two viewpoints, for example.
[0006]
One is a human science perspective. That is, through a process of creating a robot having a structure similar to a human lower limb and / or upper limb, devising a control method thereof, and simulating a human walking motion, a mechanism of natural human motion such as walking can be achieved. Can be elucidated by engineering. These findings could be greatly reduced to advances in various other disciplines that address human motor mechanisms, such as ergonomics, rehabilitation engineering, or sports science.
[0007]
The other is the development of a practical robot that supports life as a human partner, that is, supports human activities in various situations in a living environment and other daily lives. In various aspects of the human living environment, this type of robot needs to learn from humans to learn how to adapt to humans or the environment, each of which has a different personality, and to further grow in functionality. At this time, it is considered that the robot having the "human form", that is, the same shape or the same structure as a human, functions more effectively in performing smooth communication between the human and the robot.
[0008]
For example, when teaching a robot how to get through a room while avoiding obstacles that should not be stepped on, the partner who teaches like a crawler type or quadruped type robot has a completely different structure than myself. A biped robot with a similar appearance should be much easier for the user (operator) to teach and for the robot to learn (for example, Takanishi, "Control of a Biped Robot"). (See the Japan Society of Automotive Engineers of Japan Kanto Branch <High Plastics> No. 25, 1996 APRIL).
[0009]
There have already been proposed a number of techniques relating to posture control and stable walking for a type of robot that performs legged movement by biped walking. Stable “walking” here can be defined as “moving using the legs without falling over”.
[0010]
The posture stability control of the robot is very important in avoiding the falling of the robot. This is because falling means that the robot interrupts the work being performed, and considerable effort and time is spent in getting up from the falling state and resuming the work. In addition, above all, there is a risk that the fall may cause fatal damage to the robot body itself or the object on the other side that collides with the fallen robot. Therefore, in the design and development of a legged mobile robot, posture stability control during walking or other legged work is one of the most important technical issues.
[0011]
During walking, gravity, inertial force, and these moments act on the road surface from the walking system due to gravity and acceleration caused by the walking motion. According to the so-called "Dalambert principle", they balance the floor reaction force and the floor reaction force moment as a reaction from the road surface to the walking system. As a result of the mechanical inference, there is a point where the pitch and the roll axis moment are zero, that is, "ZMP (Zero \ Moment \ Point)" on or inside the supporting polygon formed by the sole and the road surface.
[0012]
Many proposals relating to posture stability control of a legged mobile robot and prevention of falling during walking use this ZMP as a criterion for determining walking stability. The bipedal walking pattern generation based on the ZMP standard has an advantage that a sole landing point can be set in advance, and the kinematic constraint condition of the toe according to the road surface shape can be easily considered. In addition, using ZMP as a stability determination criterion means that a trajectory, not a force, is treated as a target value in motion control, so that technical feasibility is increased. The concept of ZMP and the application of ZMP to the stability discrimination standard for walking robots are described in "LEGGED LOKOMOTION ROBOTS" by Miomir @ Vukobratovic (Ichiro Kato et al., "Walking Robots and Artificial Feet" (Nikkan Kogyo Shimbun)). It is described in.
[0013]
In general, a bipedal walking robot such as a humanoid has a higher center of gravity and a narrower ZMP stable area during walking than a quadrupedal walking. Therefore, the problem of the posture change due to the change of the road surface condition is particularly important in a bipedal walking robot.
[0014]
There have already been some proposals using ZMP as a posture stability determination standard for a bipedal walking robot.
[0015]
For example, the legged mobile robot described in Japanese Patent Application Laid-Open No. Hei 5-305579 is designed to perform stable walking by matching a point on the floor at which ZMP becomes zero with a target value.
[0016]
Further, in the legged mobile robot described in Japanese Patent Application Laid-Open No. 5-305581, the position where the ZMP has at least a predetermined margin from the end of the supporting polygon at the time of landing or taking off from the inside of the supporting polyhedron (polygon) is described. It was configured as described in In this case, there is an allowance for the ZMP for a predetermined distance even when a disturbance or the like is received, and the stability of the body during walking is improved.
[0017]
Japanese Patent Application Laid-Open No. 5-305583 discloses that the walking speed of a legged mobile robot is controlled by a ZMP target position. That is, using the previously set walking pattern data, the leg joints are driven so that the ZMP coincides with the target position, and at the same time, the inclination of the upper body is detected, and the walking pattern set according to the detected value is used. Change the data ejection speed. When the robot leans forward, for example, by stepping on unknown irregularities, the posture can be recovered by increasing the discharge speed. Further, since the ZMP is controlled to the target position, there is no problem even if the discharge speed is changed during the two-leg supporting period.
[0018]
Further, Japanese Patent Application Laid-Open No. 5-305585 discloses that the landing position of a legged mobile robot is controlled by a ZMP target position. That is, the legged mobile robot described in the publication detects a deviation between the ZMP target position and the actually measured position and drives one or both of the legs to eliminate the deviation, or a moment about the ZMP target position. Is detected, and the legs are driven such that it becomes zero, thereby realizing stable walking.
[0019]
Further, Japanese Patent Application Laid-Open No. 5-305586 discloses that the inclination posture of a legged mobile robot is controlled by a ZMP target position. That is, a moment around the ZMP target position is detected, and when a moment is generated, the leg is driven so that the moment becomes zero, thereby performing a stable walking.
[0020]
The posture stability control of a robot using ZMP as a stability discrimination standard is basically to search for a point where the moment is zero on or inside the support polygon formed by the sole and the road surface. It is in.
[0021]
As described above, in legged mobile robots, maximum efforts have been made to prevent the robot from tipping over during walking or other movement patterns, such as by introducing ZMP as a posture stability criterion. Have been.
[0022]
Needless to say, the state of falling means that the robot interrupts the work being executed, and considerable labor and time are required to get up from the falling state and resume the work. Above all, there is a risk that the fall may cause fatal damage to the robot body itself or to an object on the other side that collides with the fallen robot.
[0023]
Despite performing maximum posture stabilization control to prevent falling, inadequate control or unexpected external factors (for example, collision with unexpected objects, road surface conditions such as protrusions or depressions on the floor, obstacles For example, due to the appearance of an object), the stability of the posture is lost, and the movable leg alone cannot support the robot, and the robot may fall down.
[0024]
In particular, in the case of a robot that performs legged movement with two feet, such as a humanoid, the position of the center of gravity is high and the standing state itself is unstable in the first place. When the robot falls, there is a danger that the robot itself or the other party colliding with the fall will cause fatal damage.
[0025]
For example, Japanese Patent Application Laid-Open No. 11-48170 discloses that, in a situation where a legged mobile robot is likely to fall, damage to the robot due to the fall and damage to the object on the other side with which the robot collides when the fall is made as much as possible. There is disclosed a control device for a legged mobile robot that can perform the control.
[0026]
However, the publication only proposes to control so as to lower the center of gravity of the robot at the time of landing due to a fall, and in order to minimize damage when actually falling, However, there is no discussion on how to operate the entire body including not only the legs but also the torso and the arms.
[0027]
Further, in the case of an upright walking type legged mobile robot, a reference posture in consideration of a body motion such as walking is a standing posture standing up with two feet. For example, the most stable state (that is, the minimum point of instability) among the standing postures can be positioned as the basic standing posture.
[0028]
In such a basic standing posture, in order to maintain the posture stably, it is necessary to execute posture stabilization control and generate torque of a joint axis motor such as a leg according to a control instruction. In other words, since the standing posture is never stable in the no-power state, it is considered that it is preferable that the robot starts from the physically most stable on-floor posture such as a supine position or a prone position.
[0029]
However, even if the robots with the above-floor postures are turned on, if the robots cannot rise up autonomously, the operator must lend a hand and perform operations such as lifting the body, which is troublesome.
[0030]
Also, when the robot temporarily stands and performs walking or other autonomous legged work, basically try to move using the legs without falling, but do not dare to fall. It can be lost. When a robot operates in a human living environment including various obstacles and unexpected situations, "falling over" is inevitable. In the first place, human beings fall down. Even in such a case, it is troublesome if the operator must lend a hand to lift the machine.
[0031]
If the robot could not get up on its own every time it was on the floor, the robot would not be able to work in an unmanned environment, that is, the work would not be self-sufficient, and it would be completely independent. It cannot be placed in the environment.
[0032]
[Problems to be solved by the invention]
SUMMARY OF THE INVENTION An object of the present invention is to provide an excellent legged mobile robot and leg which can reduce the damage to the robot as much as possible by controlling the motion of the entire body including not only the legs but also the torso and arms during the fall and fall. An object of the present invention is to provide a method for controlling the operation of a mobile robot when it falls over.
[0033]
A further object of the present invention is to provide an excellent motion control device and motion control method for a legged mobile robot and a robot device that can autonomously recover a standing posture from a floor posture such as a supine position or a prone position. is there.
[0034]
A further object of the present invention is to provide an excellent legged mobile robot operation control device and operation control method, which can recover a standing posture from a floor posture such as a supine or prone position by a stable operation with relatively little torque, and a robot. It is to provide a device.
[0035]
Means and Action for Solving the Problems
The present invention has been made in consideration of the above problems, and a first aspect thereof is an operation control device or an operation control method of a legged mobile robot having a movable leg and performing a legged work in a standing posture. hand,
The legged mobile robot has a plurality of postures or states,
First means or steps for calculating an area S of a support polygon formed by the ground contact point of the fuselage and a road surface;
A second means or step of calculating a change ΔS / Δt of the area S of the supporting polygon per time Δt;
Third means or steps for determining an operation of the aircraft when changing its posture or state based on the area S of the supporting polygon or the change speed ΔS / Δt thereof;
An operation control device or operation control method for a legged mobile robot, comprising:
[0036]
In many legged mobile robots, ZMP is used as a stability discrimination criterion to maintain the posture stability of the body during a specific legged work such as walking. According to the motion control device or the motion control method for a foot-type mobile robot according to the first aspect of the present invention, the robot that is walking or standing upright falls over, or rises after falling down or from another sleeping posture. When the robot changes its posture or state, the motion pattern of the fuselage is sequentially determined on the basis of the area S of the support polygon formed by the ground contact point and the road surface and the speed of change ΔS / Δt. Thus, the overturning operation and the rising operation can be realized so as to be more efficient and reduce the load.
[0037]
Here, the third means or the step includes:
Landing part search means or step for searching for a landing part based on a change per area Δt of the area S of the support polygon formed by the ground contact point and the road surface of the aircraft;
The target selected by the landing site search means or step should be landed so that the change ΔS / Δt per unit time Δt of the area S of the support polygon formed by the ground contact point of the body and the road surface is minimized. Target landing point setting means or step for setting the landing point,
The apparatus may further include a part landing means for landing the part selected by the landing part searching means or step at the target landing point set by the target landing point setting means or step.
[0038]
The legged mobile robot applies force to the aircraft while performing legged work in a standing posture, using a floor reaction force sensor or acceleration sensor located on the sole, or an acceleration sensor located on the waist of the torso. External force is detected. Based on these detected external forces, a ZMP balance equation is established, and a ZMP in which the moment applied to the aircraft is balanced on or inside the side of the supporting polygon formed by the sole and the road surface. In addition, the attitude stabilization control of the airframe is performed by always planning the ZMP trajectory.
[0039]
However, the moment error on the ZMP balance equation cannot be canceled due to circumstances such as the external force applied to the fuselage being excessive or the road surface being unfavorable. In some cases, ZMP placement may be difficult or impossible. In such a case, the legged mobile robot according to the present invention gives up the posture stabilization control of the body and performs a predetermined overturning operation to minimize damage to the body at the time of falling to the floor. It has become.
[0040]
That is, when the vehicle falls, a portion where the change ΔS / Δt per unit time Δt of the area S of the support polygon formed by the ground contact point of the vehicle and the road surface is minimized, and the support formed by the ground contact point of the vehicle and the road surface are formed. The target landing point at which the selected part should land is set such that the change ΔS / Δt per unit time Δt of the area S of the polygon is minimized, and the part is landed. Then, the newly formed supporting polygon is further enlarged by landing.
[0041]
Until the potential energy of the fuselage becomes minimum and the overturning operation is completed, an operation of searching for a part where ΔS / Δt is minimum and landing the part at a target landing point where ΔS / Δt is minimum is performed. Or, the operation of enlarging the newly formed support polygon is repeatedly executed.
[0042]
As described above, the change amount ΔS / Δt of the area S of the support polygon per time t is minimized, and the support polygon when the floor falls is maximized, so that the support polygon receives from the floor at the time of fall. The impact can be dispersed throughout the body to minimize damage. When a legged mobile robot is viewed as a link structure in which a plurality of joint axes with substantially parallel joint degrees of freedom are connected in the length direction, it is possible to set a target with a link that maximizes the number of exit links. Thus, the impact force can be reduced.
[0043]
The legged mobile robot is, for example, a link structure in which a plurality of joint axes having substantially parallel joint degrees of freedom are connected in the length direction. When returning from a falling state,
Means for searching for the narrowest support polygon formed by the smallest number of links in the grounding polygon formed by the floor contact links in an on-floor position where two or more links including the center of gravity link serving as the center of gravity of the aircraft are on the floor; Steps and
Means or steps for leaving the floor contact link other than the searched supporting polygon in the grounding polygon;
Means or steps to bend two or more consecutive outgoing links and contact the ends of the link ends to form a narrower grounded polygon;
Means or steps for, in response to the support polygon having become sufficiently narrow, elevating the body by elevating a first predetermined number or more of links from one end of the link structure,
May be provided.
[0044]
Here, the link structure is formed by connecting at least a shoulder joint pitch axis, a trunk pitch axis, a hip joint pitch axis, and a knee pitch axis in the height direction of the body. Needless to say, the body of the legged mobile robot may have a joint pitch axis other than these, or may have a degree of freedom of rotation about the roll axis other than the pitch axis and the yaw axis at each joint site. Good.
[0045]
In addition, a polygon formed by the ends of a plurality of aircraft that are in contact with the floor surface is called a grounding polygon. The ground polygon in which the ZMP exists is called a support polygon. The ZMP stable area is an area in which the posture and the like of the robot can be stably controlled within the supporting polygon.
[0046]
In the legged mobile robot, in a basic posture on the floor such as a supine or prone position, all the links connecting these joint pitch axis, trunk pitch axis, hip joint pitch axis, and knee pitch axis are in contact with the floor. In a basic standing posture or a walking posture, all the links connecting the joint pitch axis, the trunk pitch axis, the hip joint pitch axis, and the knee pitch axis are separated from the floor and are aligned in a substantially vertical direction.
[0047]
When the robot rises from the on-floor posture to the standing posture, a higher torque output is required for the related joint actuator as compared with the case where the normal standing posture is maintained or the walking operation is performed. According to the present invention, the rising operation is performed using the posture in which the ZMP supporting polygon is minimized, thereby realizing the rising operation with less driving torque.
[0048]
First, in the posture on the floor where almost all the links are in contact with the floor, the narrowest supporting polygon is searched for in the grounding polygon formed by the tangent links. At this time, it is determined whether or not ZMP can be planned when at least two or more links are lifted off from one end of the body.
[0049]
For example, a narrower supporting polygon is searched for while keeping the link connecting the trunk pitch axis and the hip joint pitch axis as a center-of-gravity link in a floor-contact state. The planning possibility of the ZMP can be determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link. Then, an attempt is made to leave two or more links continuous from one end including the shoulder joint pitch axis.
[0050]
Next, leaving two or more continuous links from one end of the grounding polygon, leaving the floor contact link serving as the support polygon. Then, one or more outgoing links are bent from one end and the end of the link end is landed to form a narrower ground contact polygon.
[0051]
For example, two or more links including the shoulder joint are left from one end of the link structure as links that do not participate in the support polygon. Then, in a state where two or more links including the shoulder joint are off the floor, the link is bent at the shoulder joint pitch axis, and the hand, which is the end of the link end, is brought into contact with the floor. Then, by gradually approaching the hand to the trunk pitch axis side, which is the machine body weight center position, a grounding polygon narrower than the original on-floor posture is formed.
[0052]
Further, in this ground contact polygon, the narrowest support polygon is searched. This time, at least two or more links are released from the other end to determine whether ZMP can be planned. The planning possibility of the ZMP is determined in consideration of the movable angle of the link structure, the torque, the joint force, the angular velocity, the angular acceleration, and the like of each joint actuator connecting the link.
[0053]
For example, while keeping the center-of-gravity link connecting the trunk pitch axis and the hip joint pitch axis in a floor-contact state, an attempt is made to release two or more consecutive links from the other end including the knee joint pitch axis.
[0054]
Next, leaving two or more consecutive links from the other end of the ground contact polygon, leaving the floor contact link serving as the support polygon. Then, one or more outgoing links are bent from the other end and the end of the link end is landed to form a narrower grounding polygon.
[0055]
For example, in a state where two or more links including the knee joint are off the floor, the link is bent along the knee joint pitch axis, and the sole, which is the end of the link end, is brought into contact with the floor. Then, by gradually approaching the sole to the side of the hip joint pitch axis, which is the position of the center of gravity of the machine, a ground contact polygon narrower than the original on-floor posture is formed.
[0056]
Next, it is determined whether or not the supporting polygon has become sufficiently narrow based on whether or not the center-of-gravity link can be separated from the floor with the ends of both link ends of the grounded polygon in contact with each other. Whether or not the center-of-gravity link can be released is determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link.
[0057]
For example, the support polygon is determined by whether or not the center-of-gravity link connecting the trunk pitch axis and the hip joint pitch axis can be separated from the floor with the hand and sole as the ends of both link ends of the grounding polygon being in contact with the floor. Judge whether or not it has become sufficiently narrow.
[0058]
Then, in response to the support polygon of the fuselage becoming sufficiently narrow, the center of gravity link is lifted off with the ends of both link ends of the support polygon touching the floor, and the landing links at both link ends are used. While maintaining the ZMP in the support polygon to be formed, the distance between the ends of both link ends of the support polygon is reduced, and the ZMP is moved to the other end of the link structure. At this time, whether or not the ZMP can be moved to the other end side is determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, etc. of each joint actuator connecting the link. .
[0059]
For example, the center-of-gravity link connecting the trunk pitch axis and the hip joint pitch axis with the hands and soles as the ends of the two link ends of the grounding polygon being in contact with the floor is lifted off, and the distance between the hands and soles is gradually reduced. And move the ZMP toward the sole.
[0060]
Then, in response to the ZMP rushing into the grounding polygon formed by only the second predetermined number or less of floor contact links from the other end of the link structure, the ZMP was housed in the grounding polygon. The first predetermined number or more links are left from one end side of the link structure as it is, and the rising links are extended in the length direction to complete the rising operation.
[0061]
For example, a link from the shoulder pitch axis to the knee pitch axis while the ZMP is accommodated in the ground polygon in response to the ZMP rushing into the ground polygon formed by the sole. Is lifted, and the lifting link is extended in the length direction, whereby the rising operation can be completed.
[0062]
When extending the leaving link in the longitudinal direction, which is the final stage of getting up, it is efficient to operate the airframe actively by using the knee joint pitch axis having a larger mass operation amount.
[0063]
In addition, in order to form a narrower ground contact polygon, when the leaving link is bent at the shoulder joint pitch axis and the hand which is the end of the link end is brought into contact with the floor, the arm is set so as to satisfy the following equation. It may be operated. However, the length of the upper arm is l₁, Length of forearm₂, Shoulder roll angle α, elbow pitch angle beta, shoulder to hand length l₁₂, The angle between the line connecting the shoulder and the hand is γ, and the height of the shoulder is h.
[0064]
(Equation 3)

[0065]
That is, by bending the elbow pitch axis instead of operating the shoulder roll axis, it is possible to land the left and right hands behind the trunk with a smaller used volume.
[0066]
Also, the means or steps for generating a narrower ground contact polygon may include a step or a floor change in the hand or foot depending on whether two or more of the links not involved in the smallest support polygon can be lifted off the floor. Any of the drag operations with the surface may be selectively used to form a narrower contact polygon.
[0067]
In the process of sequentially forming smaller grounded polygons, if only the stepping operation of the hand or foot is used, in order to realize the stepping operation, the hand or foot needs to leave the floor. In addition, there must be two or more links that are not involved in the supporting polygon, and there is a case where the stepping operation cannot be performed depending on the attitude of the body, and in this case, the rising operation itself fails. On the other hand, by using the drag operation of the hand and the foot, it is possible to reduce the chance that the rising operation is broken.
[0068]
According to a second aspect of the present invention, there is provided an operation control device or an operation control method for a legged mobile robot having a movable leg and performing a legged work in a standing posture,
Means or steps for calculating an impact moment applied to the aircraft at each stage when falling,
Means or steps for calculating the impact force received by the aircraft from the floor at each stage when falling,
Means or steps for calculating an area S of a support polygon formed by a ground contact point of the fuselage and a road surface;
First landing site searching means or step for selecting the next landing site so that the area S of the supporting polygon is minimum or constant;
A second landing site searching means or step for selecting a next landing site so that the area S of the supporting polygon increases;
An operation control device or operation control method for a legged mobile robot, comprising:
[0069]
In such a case, by setting the area S of the supporting polygon to be minimum or constant by the first landing site searching means, the impact moment applied to the airframe can be passed. In this case, the support surface itself may move, for example, when the airframe moves back or forth. On the other hand, by selecting the landing site so that the area S of the supporting polygon increases rapidly by the second landing site searching means, it is possible to reduce the impact force that the aircraft receives from the floor surface when the vehicle falls. Therefore, if the impact force received from the floor surface by the aircraft is within a predetermined allowable value, the aircraft performs the overturning operation by the second landing site searching means or step, and if the impact force is out of the allowable value, the first landing operation is performed. What is necessary is just to carry out the overturning operation of the body by the part searching means or step.
[0070]
According to a third aspect of the present invention, there is provided an operation control device or an operation control method for controlling a series of operations relating to a falling and rising of an airframe in a legged mobile robot having movable legs and performing legged work in a standing posture. The legged mobile robot comprises a link structure in which a plurality of joint axes having substantially parallel joint degrees of freedom are connected in a length direction, and two or more links including a center-of-gravity link serving as a center of gravity of the body when the vehicle falls. Means or a step of searching for the narrowest supporting polygon formed by the smallest number of links in the grounding polygon formed by the floor contact link,
Means or step of performing a tipping operation by setting a ZMP at a site where the number of links not related to the minimum supporting polygon is the maximum,
Means or steps for searching for a link that can leave the floor in the fall posture of the aircraft;
Means or steps for lifting all links capable of leaving the bed and performing a rising operation;
An operation control device or operation control method for a legged mobile robot, comprising:
[0071]
When the center of gravity of the airframe is at the waist, the ZMP can be set at a position where the number of links not involving the smallest supporting polygon is the largest. After such a fall / landing operation, all the links that can be lifted off the floor are lifted off. By doing so, a smaller grounded polygon can be formed in fewer steps, so that a faster and more efficient rising operation can be realized.
[0072]
Further, a fourth aspect of the present invention is a robot apparatus having a trunk, a leg connected to the trunk, and an arm connected to the trunk.
A support polygon detection unit configured to detect a first support polygon formed by a plurality of ends where the leg, the trunk, and / or the arm touches the floor;
A support polygon changing means for reducing the area of the first support polygon by bending the leg in the trunk direction;
ZMP movement control means for determining whether or not the ZMP in the changed first support polygon can be moved to a grounded polygon formed by the sole of the leg, and
When the ZMP movement control means determines that the ZMP can be moved, the ZMP is moved from the falling posture to the basic posture while maintaining the ZMP from within the first supporting polygon within the ground contact polygon formed by the sole surface. And control means for causing the robot device to transition to the robot device.
[0073]
According to a fifth aspect of the present invention, there is provided at least a torso, one or more arm links connected above the torso via a first joint (shoulder), and a second joint below the torso. A legged mobile robot having a first leg link connected via a hip joint) and a second leg link connected via a third joint (knee) to the tip of the second leg link At
Means for contacting the tip of the arm link and the foot of the tip of the second leg link to form a first support polygon;
With the tip of the arm link and the foot resting on the floor, the second joint is moved above the third joint in the direction normal to the floor contact surface, and then the area of the first support polygon is reduced. Means for reducing and further moving the ZMP into a grounded polygon formed by said feet;
Means for erecting the fuselage while maintaining the ZMP within the grounding polygon formed by the feet;
It is a legged mobile robot characterized by comprising:
[0074]
ADVANTAGE OF THE INVENTION According to the robot apparatus which concerns on this invention, since it returns to an upright posture from a fall posture while making the area of a support polygon small, the joint actuators, such as a leg part, can implement a rising motion with comparatively moderate torque. .
[0075]
Further objects, features, and advantages of the present invention will become apparent from more detailed descriptions based on embodiments of the present invention described below and the accompanying drawings.
[0076]
BEST MODE FOR CARRYING OUT THE INVENTION
Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
[0077]
A. Mechanical configuration of a legged mobile robot
FIG. 1 and FIG. 2 show a state where the “humanoid” or “humanoid” legged mobile robot 100 used in the embodiment of the present invention stands upright, as viewed from the front and the rear, respectively. . As shown in the figure, the legged mobile robot 100 includes a torso, a head, left and right upper limbs, and two left and right lower limbs performing legged movement. For example, a control unit built in the torso (Not shown) controls the operation of the aircraft in a comprehensive manner.
[0078]
Each of the left and right lower limbs includes a thigh, a knee joint, a shin, an ankle, and a foot, and is connected at a substantially lowermost end of the trunk by a hip joint. Each of the left and right upper limbs is composed of an upper arm, an elbow joint, and a forearm, and is connected at the left and right side edges above the trunk by a shoulder joint. The head is connected to the center of the uppermost end of the trunk by a neck joint.
[0079]
The control unit is equipped with a controller (main control unit) that processes external controls from various sensors (described later) and drive control of each joint actuator that constitutes this legged mobile robot, and a power supply circuit and other peripheral devices. It is a housing. The control unit may include a communication interface and a communication device for remote control.
[0080]
The legged mobile robot thus configured can realize bipedal walking by controlling the whole body in a coordinated manner by the control unit. Such bipedal walking is generally performed by repeating a walking cycle divided into the following operation periods. That is,
[0081]
(1) Right leg lifted, left leg supporting single leg
(2) Both legs supported period with right foot touching down
(3) Single leg support period with right leg with left leg lifted
(4) Both legs supported period when left foot touched down
[0082]
Walking control in the legged mobile robot 100 is realized by planning a target trajectory of the lower limb in advance and correcting the planned trajectory in each of the above periods. That is, in the two-leg support period, the correction of the lower limb trajectory is stopped, and the waist height is corrected to a constant value using the total correction amount for the planned trajectory. In the single leg support period, a corrected trajectory is generated so that the relative positional relationship between the ankle and the waist of the corrected leg is returned to the planned trajectory.
[0083]
In general, the attitude stabilization control of the fuselage, including the correction of the trajectory of the walking motion, is performed by interpolation calculation using a fifth-order polynomial so that the position, velocity, and acceleration for reducing the deviation from ZMP are continuous. . ZMP (Zero \ Moment \ Point) is used as a criterion for determining walking stability. The stability discrimination standard based on ZMP is based on the principle of “Dallambert” that gravity and inertia force from the walking system to the road surface and these moments balance the floor reaction force and the floor reaction force moment as a reaction from the road surface to the walking system. based on. As a result of the mechanical inference, a point where the pitch axis and the roll axis moment become zero on or inside the side of the support polygon (that is, the ZMP stable region) formed by the sole and the road surface, that is, "ZMP (Zero @ Moment)" Point) "exists.
[0084]
FIG. 3 schematically shows a configuration of the degrees of freedom of the joints included in the legged mobile robot 100. As shown in the figure, the legged mobile robot 100 connects an upper limb including two arms and a head 1, a lower limb including two legs for realizing a moving operation, and an upper limb and a lower limb. This is a structure including a plurality of limbs and a trunk.
[0085]
The neck joint (Neck) that supports the head has four degrees of freedom: a neck joint yaw axis 1, first and second neck joint pitch axes 2A and 2B, and a neck joint roll axis 3.
[0086]
Each arm has, as its degrees of freedom, a shoulder joint pitch axis 4 at the shoulder (Shoulder), a shoulder joint roll axis 5, an upper arm yaw axis 6, an elbow joint pitch axis 7 at the elbow (Elbow), and a wrist ( (Wrist) and a hand part. The hand is actually a multi-joint / multi-degree-of-freedom structure including a plurality of fingers.
[0087]
The trunk (Trunk) has two degrees of freedom: a trunk pitch axis 9 and a trunk roll axis 10.
[0088]
Further, each leg constituting the lower limb includes a hip joint yaw axis 11, a hip joint pitch axis 12, a hip joint roll axis 13, a knee joint pitch axis 14 in a knee (Knee), and an ankle (Ankle) in a hip joint (Hip). ), An ankle joint pitch axis 15, an ankle joint roll axis 16, and a foot.
[0089]
However, it does not mean that the legged mobile robot 100 for entertainment must be equipped with all the degrees of freedom described above, or is not limited to this. It goes without saying that the degree of freedom, that is, the number of joints, can be appropriately increased or decreased according to design and manufacturing constraints and required specifications.
[0090]
Each degree of freedom of the legged mobile robot 100 as described above is actually implemented using an actuator. It is preferable that the actuator is small and lightweight because of requirements such as removing excess swelling on the appearance and approximating the human body shape, and controlling the posture of an unstable structure called bipedal walking. . In the present embodiment, a small AC servo actuator of a type directly connected to a gear and of a type in which the servo control system is integrated into a single motor control system and mounted in a motor unit is mounted. Japanese Patent Application Laid-Open No. 2000-299970, which has already been assigned to the applicant). In the present embodiment, the passive characteristics of the drive system itself required by a robot of a type that places importance on physical interaction with humans are obtained by adopting a reduced speed gear as a directly connected gear.
[0091]
B. Control system configuration of legged mobile robot
FIG. 4 schematically shows a control system configuration of the legged mobile robot 100. As shown in the figure, the legged mobile robot 100 has adaptive control for realizing a cooperative operation between each of the mechanical units 30, 40, 50R / L, and 60R / L representing human limbs. (However, each of R and L is a suffix indicating each of right and left. The same applies hereinafter).
[0092]
The operation of the entire legged mobile robot 100 is totally controlled by the control unit 80. The control unit 80 exchanges data and commands with a main control unit 81 composed of main circuit components (not shown) such as a CPU (Central Processing Unit) and a memory, and with a power supply circuit and each component of the robot 100. A peripheral circuit 82 including an interface (both not shown) and the like are provided.
[0093]
In realizing the present invention, the installation location of the control unit 80 is not particularly limited. In FIG. 4, it is mounted on the trunk unit 40, but may be mounted on the head unit 30. Alternatively, the control unit 80 may be provided outside the legged mobile robot 100 to communicate with the body of the legged mobile robot 100 by wire or wirelessly.
[0094]
Each joint degree of freedom in the legged mobile robot 100 shown in FIG. 3 is realized by an actuator corresponding to each joint. That is, the head unit 30 includes a neck joint yaw axis actuator A representing each of the neck joint yaw axis 1, the first and second neck joint pitch axes 2A and 2B, and the neck joint roll axis 3.₁, First and second neck joint pitch axis actuators A_2A, A_2B, Neck joint roll axis actuator A₃Are arranged respectively.
[0095]
The trunk unit 40 includes a trunk pitch axis actuator A representing each of the trunk pitch axis 9 and the trunk roll axis 10.₉, Trunk roll axis actuator A₁₀Is deployed.
[0096]
The arm unit 50R / L is subdivided into an upper arm unit 51R / L, an elbow joint unit 52R / L, and a forearm unit 53R / L. The shoulder joint pitch axis 4, shoulder joint roll axis 5, upper arm Shoulder joint pitch axis actuator A expressing each of yaw axis 6, elbow joint pitch axis 7, and wrist joint yaw axis 8₄, Shoulder joint roll axis actuator A₅, Upper arm yaw axis actuator A₆, Elbow joint pitch axis actuator A₇, Wrist joint yaw axis actuator A₈Is deployed.
[0097]
The leg unit 60R / L is subdivided into a thigh unit 61R / L, a knee unit 62R / L, and a shin unit 63R / L. The hip joint yaw axis 11, the hip joint pitch axis 12, and the hip joint Hip joint yaw axis actuator A expressing each of roll axis 13, knee joint pitch axis 14, ankle joint pitch axis 15, and ankle joint roll axis 16₁₁, Hip joint pitch axis actuator A₁₂, Hip roll axis actuator A₁₃, Knee joint pitch axis actuator A₁₄, Ankle joint pitch axis actuator A_Fifteen, Ankle joint roll axis actuator A₁₆Is deployed.
[0098]
Actuator A used for each joint₁, A₂, A₃... can be constituted by a small-sized AC servo actuator (described above) of a type directly connected to a gear and a type in which a servo control system is integrated into one chip and mounted in a motor unit.
[0099]
Sub-control units 35, 45, 55, and 65 for actuator drive control are provided for each mechanism unit such as the head unit 30, the trunk unit 40, the arm unit 50, and the leg units 60.
[0100]
An acceleration sensor 95 and a posture sensor 96 are provided on the trunk 40 of the body. The acceleration sensor 95 is disposed in each of the X, Y, and Z axis directions. By arranging the acceleration sensor 95 on the waist of the body, the waist, which is a part where the mass operation amount is large, is set as the control target point, and the posture and acceleration at that position are directly measured, and the posture stability control based on ZMP is performed. Can be performed.
[0101]
In addition, grounding confirmation sensors 91 and 92 and acceleration sensors 93 and 94 are provided on the legs 60R and 60L, respectively. The ground contact confirmation sensors 91 and 92 are configured by attaching a pressure sensor to the sole, for example, and can detect whether the sole has landed on the basis of the presence or absence of a floor reaction force. The acceleration sensors 93 and 94 are arranged at least in the X and Y axis directions. By arranging the acceleration sensors 93 and 94 on the left and right feet, the ZMP equation can be directly assembled with the feet closest to the ZMP position.
[0102]
When the acceleration sensor is placed only on the waist, which is a part where the mass operation amount is large, only the waist is set as the control target point, and the state of the foot must be relatively calculated based on the calculation result of this control target point. It is necessary to satisfy the following conditions between the foot and the road surface.
[0103]
(1) The road surface does not move under any force or torque.
(2) The coefficient of friction against translation on the road surface is sufficiently large and no slip occurs.
[0104]
On the other hand, in the present embodiment, a reaction force sensor system (such as a floor reaction force sensor) that directly applies ZMP and force is provided on a foot portion that is a contact portion with a road surface, and local coordinates used for control and the coordinates thereof are used. The acceleration sensor for directly measuring is provided. As a result, the ZMP equation can be assembled directly with the foot closest to the ZMP position, and more precise posture stabilization control that does not depend on the preconditions described above can be realized at high speed. As a result, even on a gravel or a carpet with a long bristle, where the road surface moves when force or torque is applied, or on a residential tile where slippage is likely to occur due to insufficient translational friction coefficient, Stable walking (exercise) can be guaranteed.
[0105]
The main control unit 80 can dynamically correct the control target in response to the outputs of the sensors 91 to 96. More specifically, a whole body motion pattern in which the upper limb, the trunk, and the lower limb of the legged mobile robot 100 are driven in a coordinated manner by performing adaptive control on each of the sub-control units 35, 45, 55, and 65. To achieve.
[0106]
The whole body movement of the robot 100 on the body sets a foot movement, a ZMP (Zero \ Moment \ Point) trajectory, a trunk movement, an upper limb movement, a waist height, and the like, and instructs an operation according to these set contents. The command is transferred to each of the sub-control units 35, 45, 55, 65. Each of the sub-control units 35, 45,... Interprets the command received from the main control unit 81 and₁, A₂, A₃, And outputs a drive control signal. Here, “ZMP” refers to a point on the floor where the moment due to the floor reaction force during walking becomes zero, and “ZMP trajectory” refers to, for example, ZMP during the walking operation of the robot 100. Means a moving trajectory.
[0107]
C . Basic state transition of motion system of legged mobile robot
The control system of the legged mobile robot 100 according to the present embodiment defines a plurality of basic postures. Each basic attitude is defined in consideration of the stability of the airframe, energy consumption, and transition to the next state, and the motion of the airframe can be efficiently controlled by the form of transition between the basic attitudes.
[0108]
FIG. 5 shows a basic state transition of the motion system of the legged mobile robot 100 according to the present embodiment. As shown in the figure, the legged mobile robot has a basic supine posture, a basic standing posture, a basic walking posture, a basic sitting posture, and a basic prone posture, respectively, when supine, standing, walking, preparing for sitting, and prone. Are defined in consideration of the stability of the aircraft, energy consumption, and transition to the next state.
[0109]
These basic attitudes are positioned on the platform of the operation control program of the aircraft. Further, the legged mobile robot performs various performances using whole body movements such as walking, jumping, and dancing in a standing posture, and the device control program is positioned as an application that operates on the platform. These application programs are loaded at any time from an external storage, and are executed by the main control unit 81.
[0110]
FIG. 6 shows a basic supine posture of the legged mobile robot 100. In the present embodiment, when the power of the body is turned on, it takes a basic supine posture, and can be started from the most stable state in mechanical motion without fear of falling over. The legged mobile robot returns to the basic supine posture not only at the time of activation but also at the end of the system operation. Therefore, the operation starts in the most stable state of the machine kinematically and ends in the most stable state, so that the operation operation of the legged mobile robot is self-contained.
[0111]
Of course, even when the aircraft falls, it returns to the basic supine position after a predetermined motion on the floor, and then executes the prescribed rising motion, so that the original posture at the time of work interruption via the basic standing position Can be recovered.
[0112]
Further, the legged mobile robot 100 according to the present embodiment has, as a basic posture on the floor, a basic prone posture as shown in FIG. 7 in addition to the basic supine posture. This basic prone posture is the most mechanically kinematically stable state of the body, similar to the basic supine posture, and can maintain the posture stability even in a weakened state in which power is cut off. For example, when the aircraft falls due to an unexpected external force or the like in a legged work, it is unknown whether the aircraft falls in a supine or prone state. Therefore, the present embodiment defines two basic floor postures in this manner. .
[0113]
A transition can be made reversibly between the basic supine posture and the basic prone posture through various floor postures. Conversely, the state can be smoothly transitioned to various floor postures based on the basic supine posture and the basic prone posture.
[0114]
The basic supine posture is the most stable basic posture in mechanical kinematics, but when legged work is considered, a smooth state transition cannot be performed. Therefore, a basic standing posture as shown in FIG. 8 is defined. By defining the basic standing posture, it is possible to smoothly shift to the subsequent leg work.
[0115]
The basic standing posture is the most stable state in the standing state, and is a posture in which the computer load and power consumption for the posture stabilization control are minimized or minimized, and for maintaining the upright state by extending the knee. Minimizes motor torque. The state can be smoothly transitioned from the basic standing posture to various standing postures, for example, a dance performance using the upper limbs can be demonstrated.
[0116]
On the other hand, the basic standing posture is excellent in posture stability, but is not optimized for shifting to legged work such as walking. Thus, the legged mobile robot according to the present embodiment defines a basic walking posture as shown in FIG. 9 as another basic posture in the standing state.
[0117]
In the basic standing posture, the pitch axes 12, 14, and 15 of the hip joint, the knee joint, and the ankle joint are driven so as to slightly lower the center of gravity of the airframe, thereby shifting to the basic walking posture. In the basic walking posture, it is possible to smoothly perform transitions to various legged movements including a normal walking movement. However, since an extra torque is required to maintain this posture by the amount of bending of the knee, the basic walking posture consumes more power than the basic standing posture.
[0118]
The basic standing posture is a posture in which the ZMP position of the body is near the center of the ZMP stable region, the bending angle of the knee is small, and the energy consumption is low. On the other hand, in the basic walking posture, the ZMP position is near the center of the stable region, but the knee bending angle is relatively large in order to ensure high road surface adaptability and high external force adaptability.
[0119]
Further, in the legged mobile robot 100 according to the present embodiment, a basic sitting posture is further defined. In this basic sitting posture (not shown), when sitting on a predetermined chair, the computer load and the power consumption for the posture stabilization control are minimized or minimized. The above-described basic supine posture, basic prone posture, and basic standing posture can reversibly transition to the basic posture. Further, the user can smoothly transition from the basic sitting posture and the basic standing posture to various sitting postures, and can perform various performances using only the state in the sitting posture, for example.
[0120]
D. Posture stability control of a legged mobile robot
Next, in the legged mobile robot 100 according to the present embodiment, the posture stabilization processing at the time of legged work, that is, the posture stabilization processing at the time of executing the whole body cooperative movement including the foot, waist, trunk, and lower limb movements is performed. The procedure will be described.
[0121]
The posture stability control according to the present embodiment uses ZMP for posture stability control. The posture stability control of a robot using ZMP as a stability discrimination standard is basically to search for a point where the moment is zero on or inside the support polygon formed by the sole and the road surface. It is in. That is, a ZMP equation describing a balance relationship of each moment applied to the robot body is derived, and the target trajectory of the body is corrected so as to cancel a moment error appearing on the ZMP equation.
[0122]
In the present embodiment, a region where the mass operation amount is maximum, for example, the waist, is set as the local coordinate origin as a control target point on the robot body. Then, a measuring means such as an acceleration sensor is arranged at the control target point, and the posture and acceleration at that position are directly measured to perform posture stability control based on ZMP. Further, by arranging an acceleration sensor on the foot which is a contact portion with the road surface, the local coordinates used for control and the coordinates are directly measured, and the ZMP equation is directly assembled with the foot closest to the ZMP position.
[0123]
D-1. Introduction of ZMP equation
The legged mobile robot 100 according to the present embodiment is an aggregate of infinite, that is, continuous mass points. In this case, however, the amount of calculation for stabilization processing is reduced by replacing the model with an approximate model having a finite number of discrete mass points. More specifically, the legged mobile robot 100 physically having the multi-joint degree of freedom configuration shown in FIG. 3 is handled by replacing it with a multi-mass point approximation model as shown in FIG. The illustrated approximation model is a linear and non-interfering multi-mass approximation model.
[0124]
10, the O-XYZ coordinate system represents roll, pitch, and yaw axes in an absolute coordinate system, and the O'-X'Y'Z 'coordinate system represents roll, pitch, and the like in a motion coordinate system that moves with the robot 100. Each yaw axis is shown. However, the meanings of the parameters in the figure are as follows. It should also be understood that symbols with a dash (') describe a motion coordinate system.
[0125]
(Equation 4)

[0126]
In the multi-mass model shown in the figure, i is a suffix representing the i-th given mass, and m_iIs the mass of the ith mass point, r_i′ Represents the position vector (movement coordinate system) of the i-th mass point. The machine center of gravity of the legged mobile robot 100 according to the present embodiment exists near the waist. That is, the waist is the mass point at which the mass operation amount is the maximum, and in FIG._h, And its position vector (movement coordinate system) is r ′_h(R '_hx, R '_hy, R '_hz). Also, the position vector (however, the motion coordinate system) of the ZMP of the airframe is r ′_zmp(R '_zmpx, R '_zmpy, R '_zmpz).
[0127]
The world coordinate system O-XYZ is an absolute coordinate system and is invariable. In the legged mobile robot 100 according to the present embodiment, acceleration sensors 93, 94, and 96 are disposed on the lower back and the legs of both legs, respectively. Is detected directly. On the other hand, in the motion coordinate system, that is, the local coordinate system of the body, O-X'Y'Z 'moves together with the robot.
[0128]
The multi-mass model is a representation of a robot in the form of a wireframe model. As can be seen from FIG. 10, the multi-mass point approximation model is set with each of the shoulders, both elbows, both wrists, trunk, waist, and both ankles as mass points. In the illustrated non-rigid multi-mass approximation model, the moment equation is described in the form of a linear equation, which does not interfere with the pitch and roll axes. The multi-mass point approximation model can be generally generated by the following processing procedure.
[0129]
(1) The mass distribution of the entire robot 100 is obtained.
(2) Set a mass point. The method of setting the mass may be either manual input by a designer or automatic generation according to a predetermined rule.
(3) The center of gravity is determined for each region i, and the position of the center of gravity and the mass m_iIs assigned to the corresponding mass point.
(4) Each mass point m_iAt the mass position r_iIs displayed as a sphere having a center and a radius proportional to its mass.
(5) The masses that are actually connected, that is, the spheres are connected.
[0130]
Each rotation angle (θ in the waist information of the multi-mass model shown in FIG._hx, Θ_hy, Θ_hz) Defines the posture of the waist of the legged mobile robot 100, that is, the rotation of the roll, pitch, and yaw axes. (FIG. 11 is an enlarged view of the multi-mass model around the waist, and thus is confirmed. Want).
[0131]
The ZMP equation of the fuselage describes the balance of each moment applied at the control target point. As shown in FIG._iWhen these are set as control target points, all control target points m_iIs a ZMP equation.
[0132]
The ZMP equation of the aircraft described in the world coordinate system (O-XYZ) and the local coordinate system (O-X'Y'Z ') of the aircraft are as follows.
[0133]
(Equation 5)

[0134]
The above equation is for each mass m_iAround the ZMP generated by the acceleration component applied at (radius r_i-R_zmp) And the external force moment M applied to each mass point mi._iAnd the external force F_k(The k-th external force F)_kThe action point of s_k) Are balanced.
[0135]
This ZMP balance equation includes a total moment compensation amount, that is, a moment error component T. By keeping this moment error at zero or within a predetermined tolerance, the attitude stability of the aircraft is maintained. In other words, correcting the body motion (foot motion or the trajectory of each part of the upper body) so that the moment error becomes zero or less than the allowable value is the essence of the posture stability control using the ZMP as a stability determination criterion. is there.
[0136]
In the present embodiment, since the acceleration sensors 96, 93 and 94 are provided on the waist and the left and right feet, respectively, the ZMP is directly and accurately performed using the acceleration measurement results at these control target points. A balance equation can be derived. As a result, high-speed and more strict posture stability control can be realized.
[0137]
D-2. Whole-body coordinated posture stabilization control
FIG. 12 is a flowchart illustrating a processing procedure in the legged mobile robot 100 for generating a body motion capable of stable walking using the ZMP as a stability determination criterion. However, in the following description, the joint positions and movements of the legged mobile robot 100 are described using a linear / non-interfering multi-mass point approximation model as shown in FIGS.
[0138]
First, the foot motion is set (step S1). The foot motion is motion data in which two or more aircraft poses are connected in time series.
[0139]
The motion data is composed of, for example, joint space information indicating displacement of each joint angle of the foot and Cartesian space information indicating joint positions. The motion data can be manually input on a console screen, or can be constructed on a direct teaching (direct teaching) to an airframe, for example, on an authoring system for motion editing.
[0140]
Next, a ZMP stable area is calculated based on the set foot motion (step S2). The ZMP is a point at which the moment applied to the airframe becomes zero, and basically exists on or inside the side of the supporting polygon formed by the sole contact point and the road surface. The ZMP stable region is a region set further inside the supporting polygon, and the ZMP can be accommodated in this region to make the aircraft highly stable.
[0141]
Then, a ZMP trajectory during the foot motion is set based on the foot motion and the ZMP stable area (step S3).
[0142]
In addition, for each part of the upper body (above the hip joint) of the body, a group is set such as a waist, a trunk, an upper limb, and a head (step S11).
[0143]
Then, a desired trajectory is set for each part group (step S12). Similar to the case of the foot, the setting of the desired start of the upper body should be done by manual input on the console screen or direct teaching (direct teaching) to the aircraft, for example, building on the authoring system for motion editing Can be.
[0144]
Next, the group setting of each part is adjusted (re-grouping) (step S13), and priorities are given to these groups (step S14). The priority here refers to the order of input to the processing calculation for performing the attitude stabilization control of the airframe, and is assigned, for example, according to the mass operation amount. As a result, a desired trajectory group with priority for each part of the upper body of the aircraft is completed.
[0145]
In addition, a mass that can be used for moment compensation is calculated for each of the body groups of the upper body (step S15).
[0146]
Then, based on the foot motion, the ZMP trajectory, and the desired activation group for each upper body part group, the exercise pattern of each part group is input to the posture stabilization process in accordance with the priority set in step S14.
[0147]
In this posture stabilization processing, first, an initial value 1 is substituted for a processing variable i (step S20). Then, the moment amount on the target ZMP, that is, the total moment compensation amount at the time of setting the target trajectory for the i-th part group from the top is calculated (step S21). For a part for which the target trajectory has not been calculated, the desired trajectory is used.
[0148]
Next, the amount of moment compensation is set using the mass that can be used for the moment compensation of the part calculated in step S15 (step S22), and the moment compensation amount is calculated (step S23).
[0149]
Next, using the calculated moment compensation amount of the i-th part, a ZMP equation for the i-th part is derived (step S24), and the moment compensation movement of the part is calculated (step S25). It is possible to obtain the target trajectory for the i-th part from the top in the priority order.
[0150]
By performing such processing for all the part groups, a whole body movement pattern capable of stable movement (for example, walking) is generated.
[0151]
Since the acceleration sensors 96, 93, and 94 are provided on the waist and the left and right feet, respectively, the above ZMP balance equation is directly and accurately derived using the acceleration measurement results at these control target points. be able to. As a result, the posture stabilization control based on the ZMP stability determination criterion can be executed at high speed and more strictly according to the processing procedure shown in FIG.
[0152]
E. FIG. Falling operation of a legged mobile robot
As described in the preceding section D, the legged mobile robot 100 according to the present embodiment basically performs posture stabilization control during walking or other standing work based on the ZMP stability discrimination standard. To minimize the occurrence of a fall.
[0153]
However, if it becomes impossible to avoid a fall, a fall operation having an operation pattern that minimizes damage to the body is performed. For example, in the above-described ZMP balance equation, when an excessive external force F or external force moment M is applied to the body, the moment error component T cannot be canceled only by the body operation, and the stability of the posture cannot be maintained.
[0154]
FIG. 13 is a flowchart illustrating a schematic processing procedure of operation control of the body during legged work in the legged mobile robot 100 according to the present embodiment.
[0155]
During the operation of the airframe, the ZMP balancing equation (using the sensor outputs of the ground contact confirmation (floor reaction force) sensors 91 and 92 and the acceleration sensors 93 and 94 provided on the left and right feet and the acceleration sensor 96 provided on the waist) is used. ), And the lumbar and lower limb trajectories are always calculated (step S31).
[0156]
For example, when an external force is applied to the body, it is determined whether the next waist and lower limb trajectory can be planned, that is, whether the moment error due to the external force can be eliminated by the action plan of the foot ( Step S32). Whether the waist and lower limb trajectories can be planned is determined in consideration of the movable angle of each joint of the leg, the torque of each joint actuator, joint force, angular velocity, angular acceleration, and the like. Needless to say, when an external force is applied, the moment error may be eliminated not only by the next step but also by several steps of leg steps.
[0157]
At this time, if planning of the foot is possible, walking and other legged movements are continued (step S33).
[0158]
On the other hand, when the planning of the foot becomes impossible because an excessive external force or an external force moment is applied to the body, the legged mobile robot 100 starts to fall (step S34).
[0159]
In the case of an upright walking type legged robot as shown in FIG. 1 and FIG. 2, since the position of the center of gravity is high, if the robot is accidentally dropped on the floor when falling, it is fatal to the robot itself or the other side that collides due to falling. Risk of permanent damage.
[0160]
Therefore, in the present embodiment, a predetermined overturning operation is performed by changing the trajectory of the aircraft planned before the overturn to a posture that minimizes the ZMP support polygon. Basically, a falling motion is searched for based on the following two policies.
[0161]
(1) The variation ΔS / Δt of the area S of the support polygon of the body per time t is minimized.
(2) The support polygon when the floor falls is maximized.
[0162]
Here, minimizing the amount of change ΔS / Δt corresponds to maintaining (or reducing) the support area at the time of falling (however, in the case of reducing, a driving force may be required). By maintaining the support area when the body falls, the impact moment applied to the body can be dissipated. FIG. 14 illustrates the principle of maintaining the support area when the body falls. As shown in the figure, just as the sphere rolls, the support surface receives the impact moment while maintaining the area of the support surface to be minimum. As shown, the same effect can be obtained even if the support surface moves. For example, an impact force received from the floor at the time of landing is obtained, and when this exceeds an allowable value, the overturning method in which the body rolls so as to keep the area of the supporting polygon constant is preferable.
[0163]
Further, that the supporting polygon at the time of falling on the floor is maximized, as shown in FIG. 15, is equivalent to interference of an impact force by receiving with a wider supporting polygon. For example, an impact force received from the floor at the time of landing is obtained, and when this is within an allowable value, a method of overturning the body so as to keep the area of the supporting polygon constant is preferable.
[0164]
16 and 17, when the legged mobile robot 100 falls backward, that is, toward the supine position, an operation of minimizing the change amount ΔS / Δt of the supporting polygon, that is, maintaining the supporting area at the time of falling, is realized. An example is shown. This is a similar operation similar to the passive operation in judo and other martial arts, and can appropriately dissipate the impact force moment at the time of falling. As shown in FIG. 17, the amount of change ΔS / Δt of the support polygon is minimized by leaving the feet. When the center of gravity of the airframe is at the waist, the ZMP can be set at a position where the number of links not involving the smallest supporting polygon is the largest. After such a fall / landing operation, all the links that can be released are released, that is, in the example shown, both the lower limb and the trunk are lifted, the upper body and the lower limb are simultaneously released, and the feet and hands are lifted. Since a smaller grounded polygon can be formed in fewer steps by landing on the floor, a faster and more efficient rising operation can be realized.
[0165]
18 and 19, when the legged mobile robot 100 falls down toward the front, that is, the prone posture, an operation of minimizing the change amount ΔS / Δt of the support polygon, that is, maintaining the support area at the time of falling down is realized. The example is shown when viewed from the side and the diagonally right front. This is a similar operation similar to the forward rotation operation in mechanical gymnastics or the like, and can appropriately dissipate an impact force moment at the time of falling. As shown in each figure, the amount of change ΔS / Δt of the supporting polygon is minimized by leaving the foot.
[0166]
By adopting the above-mentioned overturning method, the impact received from the floor surface at the time of falling can be dispersed to the whole body, so that damage can be minimized.
[0167]
FIG. 20 shows, in the form of a flowchart, a processing procedure for the legged mobile robot 100 according to the present embodiment to perform the overturning operation due to the inability to plan the foot. The overturning operation is realized by synchronously and cooperatively driving the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14, which are connected in the height direction, in accordance with the basic policy described above. You. Such a processing procedure is actually realized by executing a predetermined machine operation control program in the main control unit 81 and controlling driving of each unit.
[0168]
First, a link that minimizes the change amount ΔS / Δt of the area S of the support polygon of the body per time t is searched (step S41).
[0169]
Next, a search is made for a target landing point of the link selected in step S41 that minimizes the amount of change ΔS / Δt (step S42). By keeping the support area of the fuselage against the floor at a minimum, the impact moment can be dissipated (see above and FIG. 14).
[0170]
Then, the landing of the link selected in the preceding step at the target landing point depends on the limitations of the aircraft hardware (movable angle of each joint, torque of each joint actuator, joint force, angular velocity, angular acceleration, etc.). It is mainly determined whether or not execution is possible based on the impact force moment (step S43).
[0171]
If it is determined that the link selected in the preceding step cannot be landed on the target landing point, the time variation Δt is increased by a predetermined value (step S44), and then the step is performed. Returning to S41, the link is reselected and the target landing point of the link is reset.
[0172]
On the other hand, if the link selected in the preceding step can be landed on the target landing point, the selected link is landed on the target landing point (step S45).
[0173]
Next, it is determined whether or not the potential energy of the body is minimum, that is, whether or not the overturning operation has been completed (step S46).
[0174]
If the potential energy of the fuselage is not yet the minimum, the time variation Δt is further increased by a predetermined value (step S47), and the next target landing point is set so as to enlarge the support polygon (step S47). S48). By enlarging the supporting polygon, it is possible to reduce the impact force applied to the body at the time of landing (see the above and FIG. 15).
[0175]
Then, the landing of the selected link at the target landing point can be executed due to constraints of the aircraft hardware (movable angles of each joint, torque of each joint actuator, joint force, angular velocity, angular acceleration, etc.). Whether or not the impact force is mainly determined (step S49). If it is determined that it is impossible to land the link selected in the preceding step at the target landing point, the process returns to step S41 to reselect the link and to set the target landing point of the link. Is reset.
[0176]
On the other hand, when it is possible to land the link selected in the preceding step at the target landing point, the process proceeds to step S45 to land the selected link at the target landing point.
[0177]
Then, when the potential energy of the aircraft is minimized (step S46), it means that the landing of the aircraft on the floor has been completed, and the entire processing routine ends.
[0178]
Next, the overturning operation of the legged mobile robot 100 will be described with reference to the actual operation.
[0179]
In FIG. 21, the legged mobile robot 100 includes a plurality of substantially parallel joint axes connected in the height direction such as the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14. In this example, the link structure is modeled as a link structure, and the joint pitch axes are driven in a synchronously emphasized manner to fall over toward a supine posture. Basically, by setting as a target a portion having a link where the number of exit links is the maximum, the impact force received from the floor surface is reduced.
[0180]
It is assumed that the robot stands only at the sole that is the link end of the link structure (FIG. 21A).
[0181]
At this time, the application of the external force or the external force moment makes it impossible to cancel the moment error term T of the ZMP balancing equation, and the ZMP is supported in response to the ZMP deviating outside the ZMP stable region formed only by the sole. The falling motion is started while maintaining the polygon.
[0182]
In the overturning operation, first, a link that minimizes the amount of change ΔS / Δt per unit time t of the area S of the supporting polygon of the body is searched for, and the amount of change ΔS / Δt is minimized by a link including the hand. Search for the target landing point on the hand. Then, whether landing of the selected link at the target landing point is feasible due to constraints of the aircraft hardware (movable angle of each joint, torque of each joint actuator, joint force, angular velocity, angular acceleration, etc.) Is determined.
[0183]
If the link can be executed on the hardware of the machine, another link will land in addition to the sole link that has already landed. Then, the ZMP is moved into the minimum support polygon formed by these landing links (FIG. 21 (2)).
[0184]
Next, as long as the machine hardware permits, the landing point is moved to expand the support polygon (FIG. 21 (3)).
[0185]
Then, when it is no longer possible to move the landing point due to the limitations of the body hardware such as the movable angle of each joint, the torque of each joint actuator, the joint force, angular velocity, angular acceleration, etc. It is determined whether it is possible to land the leaving link sandwiched between the two.
[0186]
If it is possible to land the landing links between the landing links on the machine hardware, the landing links are landed to increase the number of landing links (FIG. 21D).
[0187]
Further, the landing point is moved and the supporting polygon is enlarged as far as the machine hardware permits (FIG. 21 (5)).
[0188]
Finally, one or more links from one end side of the link structure composed of a plurality of substantially parallel joint axes connected in the height direction and two or more links from the other end are separated from the floor, and With one or more of the links located at the middle, the ZMP within the supporting polygon is formed while the foot is further landed, and the posture in which the supporting polygon is maximized is formed. In this posture, if the potential energy of the body is minimum, the overturning operation is completed.
[0189]
FIGS. 22 to 38 and FIGS. 39 to 55 show how the actual machine falls from the standing position to the supine position.
[0190]
In this case, as the link that minimizes the variation ΔS / Δt of the area S of the support polygon of the fuselage per time t, the fuselage link including the hip joint pitch axis is selected, and the target landing point is searched for. (See FIGS. 22 to 31 and FIGS. 39 to 48). With the knee joint in a folded position, the amount of change in the supporting polygon at the time of landing is minimized, that is, ΔS / Δt is minimized.
[0191]
Next, a trunk link including the trunk pitch axis 9 and the shoulder pitch axis 4 is selected as a link for minimizing the variation ΔS / Δt of the area S of the support polygon of the fuselage per time t, and the target landing thereof is selected. Search for the floor point and fall further behind the aircraft. At this time, since the hip joint pitch axis 12 has already landed, the torso link including the trunk pitch axis 9 and the shoulder joint pitch axis 4 is landed on this as the center of rotation (FIGS. 32 to 33 and FIG. 49-FIG. 50).
[0192]
Next, the head link connected by the neck joint pitch axis 2 is selected as the link that minimizes the variation ΔS / Δt of the area S of the support polygon of the fuselage per time t, and the target landing point is set. Search for and fall further behind the aircraft. At this time, since the neck joint pitch axis 2 has already been landed, the head is landed with this as the center of rotation (see FIGS. 34 to 38 and FIGS. 51 to 55). In this posture, since the potential energy of the body is minimal, the overturning operation is completed.
[0193]
In FIG. 56, the legged mobile robot 100 includes a plurality of substantially parallel joints connected in the height direction such as the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14. The operation is modeled as a link structure composed of shafts, and the joint pitch axes are driven in a synchronously emphasized manner to fall toward a prone posture.
[0194]
It is assumed that the robot stands only at the sole that is the link end of the link structure (FIG. 56 (1)).
[0195]
At this time, the application of the external force or the external force moment makes it impossible to cancel the moment error term T of the ZMP balancing equation, and the ZMP is supported in response to the ZMP deviating outside the ZMP stable region formed only by the sole. The falling motion is started while maintaining the polygon.
[0196]
In the overturning operation, first, a link that minimizes the amount of change ΔS / Δt per unit time t of the area S of the supporting polygon of the body is searched for, and the amount of change ΔS / Δt is minimized by a link including the hand. Search for the target landing point on the hand. Then, whether landing of the selected link at the target landing point is feasible due to constraints of the aircraft hardware (movable angle of each joint, torque of each joint actuator, joint force, angular velocity, angular acceleration, etc.) Is determined.
[0197]
If the link can be executed on the hardware of the machine, another link will land in addition to the sole link that has already landed. Then, the ZMP is moved into the minimum support polygon formed by these landing links (FIG. 56 (2)).
[0198]
Next, as long as the aircraft hardware permits, the landing point is moved to enlarge the support polygon (FIG. 56 (3)).
[0199]
Then, when it is no longer possible to move the landing point due to the limitations of the body hardware such as the movable angle of each joint, the torque of each joint actuator, the joint force, angular velocity, angular acceleration, etc. It is determined whether it is possible to land the leaving link sandwiched between the two.
[0200]
If it is possible to land the leaving links between the landing links on the body hardware, these are landed to increase the number of landing links (FIG. 56 (4)).
[0201]
Further, the landing point is moved and the supporting polygon is enlarged as far as the machine hardware permits (FIG. 56 (5)).
[0202]
Finally, one or more links from one end side of the link structure composed of a plurality of substantially parallel joint axes connected in the height direction and two or more links from the other end are separated from the floor, and With one or more of the links located at the middle, the ZMP within the supporting polygon is formed while the foot is further landed, and the posture in which the supporting polygon is maximized is formed. In this posture, if the potential energy of the body is minimum, the overturning operation is completed.
[0203]
FIGS. 57 to 73 and FIGS. 74 to 90 show how the actual machine falls from the standing posture to the supine posture.
[0204]
In this case, the hand of the arm link including the shoulder joint pitch axis 4 is selected as the link that minimizes the change amount ΔS / Δt of the area S of the support polygon of the body per time t, and the target landing point is searched. Then, it falls down in front of the fuselage (see FIGS. 57 to 70 and FIGS. 74 to 87).
[0205]
At this time, in the shortest time increment Δt, in order to minimize the amount of change ΔS of the supporting polygon at the time of landing, the knee joint pitch axis 14 is set in the folded posture, and the place where the hand touches the floor is increased. Set to a position close to.
[0206]
Next, a leg link including the knee joint pitch axis 14 is selected as a link that minimizes the amount of change ΔS / Δt of the area S of the support polygon of the fuselage per time t, and the target landing point is searched. And fall further in front of the aircraft. At this time, since the foot has already landed, the lower leg turns around the ankle pitch axis as the center of rotation, and the knees land (see FIGS. 70 to 71 and FIGS. 88 to 89). That).
[0207]
Further, the user moves the hands and knees as landing points away from the soles to enlarge the support polygon as long as the fuselage hardware permits (see FIGS. 72 and 89). As a result, the torso link also lands following the hand and the knee (see FIGS. 73 and 90). In this posture, since the potential energy of the body is minimal, the overturning operation is completed.
[0208]
F. Getting up from floor position
The legged mobile robot 100 realizes a rising operation in order to start up from a floor posture such as a supine posture or a prone posture, or for a self-contained operation in which the worker stands up and resumes work when falling down. It is necessary to.
[0209]
However, when trying to get up due to an unplanned trajectory, an excessive external force moment is applied, and the joint actuator requires a high output torque. As a result, it is necessary to increase the size of the motor, and the driving power consumption increases accordingly. In addition, the manufacturing cost increases as the weight of the body increases. The rise in weight further makes the rising operation difficult. Alternatively, there may be a situation where the stability of the posture cannot be maintained due to an external force moment generated in the process of the rising motion, and the posture cannot be raised in the first place.
[0210]
Therefore, in the present embodiment, the legged mobile robot 100 performs a rising motion having an operation pattern in which the external force moment is minimized. This can be realized by combining the postures that minimize the ZMP support polygon in a time-series manner.
[0211]
Further, the legged mobile robot 100 according to the present embodiment has a height similar to the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14 (see FIG. 3). A link structure in which a plurality of pitch axes are connected in series in a direction (when viewed from the side). Therefore, the plurality of joint pitch axes 4 to 14 are synchronously and cooperatively driven in a predetermined sequence to realize a rising operation by an operation pattern that minimizes the ZMP support polygon.
[0212]
F-1. Getting up from basic supine position
In FIG. 91, the legged mobile robot 100 according to the present embodiment performs a rising motion by driving the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14 in a synchronously emphasized manner. Is shown in the form of a flowchart. Such a processing procedure is actually realized by executing a predetermined machine operation control program in the main control unit 81 and controlling driving of each unit.
[0213]
FIG. 92 shows that the legged mobile robot 100 according to the present embodiment drives the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14 in a synchronously emphasized manner, and the supine posture. The appearance of performing a rising motion from a motion is shown by a joint link model. Note that the legged mobile robot 100 according to the present embodiment includes the trunk pitch axis 9, but in a legged mobile robot that does not include the trunk pitch axis, the backrest posture is controlled by synchronous driving of the plurality of joint pitch axes. FIG. 93 shows a state of performing the rising operation from the beginning. However, in the illustrated link structure, the center of gravity position of the entire body is set at a link connecting the trunk joint and the hip joint, and this link is hereinafter referred to as a “center of gravity link”. Note that the “center-of-gravity link” is used in the narrow sense in the above definition, but in a broad sense, any link that has the center-of-gravity position of the entire body exists. For example, in an airframe that does not have a trunk axis, a link including a trunk tip where the center of gravity of the entire airframe is located corresponds to this.
[0214]
Hereinafter, the rising operation of the aircraft from the basic supine posture will be described with reference to the flowchart shown in FIG.
[0215]
First, in the posture on the floor, a posture with the smallest potential energy is searched (step S1). This corresponds to a basic supine posture, and as shown in FIG. 92 (1) and FIG. 93 (1), the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint All links connecting the pitch shafts 14 are in contact with the floor. FIGS. 94 and 112 show the state of the actual machine at this time. By taking the posture with the smallest potential energy, it is possible to measure the inclination and shape of the road surface and to confirm whether or not the user can get up.
[0216]
In this basic supine posture, the narrowest supporting polygon is searched for within the ground contact polygon formed by the floor contact link (step S52). At this time, it is determined whether or not a ZMP trajectory can be planned when at least two or more links are lifted off from one end of the body. The planning possibility of the ZMP can be determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link.
[0219]
Next, two or more links that are not involved in the narrowest supporting polygon among the grounding polygons are lifted off (step S53).
[0218]
Step S53 corresponds to FIG. 92 (2) and FIG. 93 (2). On the actual machine, the lower body side including the center-of-gravity link connecting the trunk joint and the hip joint is extracted as a support polygon, and two or more links from the shoulder joint to the trunk joint are lifted off as links not involved in the support polygon. I do.
[0219]
The operation of the actual machine at this time is shown in FIGS. 95 to 96 and FIGS. 113 to 114. In the illustrated example, first, the left and right arms are lifted, and then the trunk joint pitch axis actuator A₉The body is raised by the drive of. By lifting the arm first, the moment can be reduced and the required maximum torque can be reduced.
[0220]
Next, one or more outgoing links are bent from one end and the end of the link end is landed to form a narrower ground contact polygon (step S54).
[0221]
Step S54 corresponds to FIG. 92 (3) and FIG. 93 (3). On an actual machine, in a state where two or more links including the shoulder joint are off the floor, the link is bent along the shoulder joint pitch axis, and the hand, which is the end of the link end, is brought into contact with the floor. Then, by gradually approaching the hand to the trunk pitch axis side, which is the machine body weight center position, a grounding polygon narrower than the original on-floor posture is formed.
[0222]
The operation of the actual machine at this time is shown in FIGS. 97 to 101 and FIGS. 115 to 119. In the illustrated example, the left and right shoulder joint roll axes A₅, The left and right arms are spread right beside, and the upper arm yaw axis A₆The arm is rotated once by 180 degrees by the drive (FIGS. 98 to 99 and FIGS. 116 to 117), and then the shoulder joint pitch axis A₄The arm is gradually lowered by the drive of. Then, a smaller ground contact polygon is formed by landing on the hand (FIGS. 101 and 119).
[0223]
When a new ground polygon is formed in this way, it is checked whether ZMP can be set for the ground polygon (step S55). This is determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link. Then, the ZMP is moved to the ground polygon to form a new support polygon (step S56).
[0224]
Here, it is determined whether or not the support polygon has become sufficiently narrow (step S57). This determination is made by determining whether the center of gravity link connecting the trunk pitch axis and the hip joint pitch axis can be released from the floor, or whether the ZMP can be moved into a ZMP stable region that can be formed only by the feet. Is determined in consideration of the movable angle of the joint, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link. The detailed procedure for determining whether the supporting polygon has become sufficiently narrow will be described later.
[0225]
In the posture of the actual machine shown in FIGS. 101 and 119, the supporting polygon is not yet sufficiently narrow. Therefore, after the landing point is moved to reduce the supporting polygon (step S50), the process returns to step S52 to retry forming a narrower supporting polygon.
[0226]
In the posture shown in FIGS. 101 and 119, the narrowest supporting polygon is searched for in the ground contact polygon formed by the floor contact link (step S52). This time, it is determined whether or not ZMP can be planned when at least two or more links have left the floor from the other end of the aircraft. The planning possibility of the ZMP can be determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link.
[0227]
Next, two or more links that are not involved in the narrowest supporting polygon among the floor contact polygons are separated from the floor (step S53). This corresponds to FIGS. 92 (4) to (5) and FIGS. 93 (4) to (5). On the actual machine, two or more links continuous from the other end including the knee joint pitch axis are separated from the floor as links not involved in the support polygon.
[0228]
Then, one or more outgoing links are bent from one end and the end of the link end is landed to form a narrower ground contact polygon (step S54).
[0229]
The operation of the actual machine at this time is shown in FIGS. 102 to 105 and FIGS. 120 to 123. In the illustrated example, first, the hip joint pitch axis A of the right leg₁₂The right leg is lifted by driving the knee joint actuator A₁₄The right leg is bent by the drive of, and the sole is landed. Next, the hip pitch axis A of the leg₁₂The right leg is lifted by driving the knee joint actuator A₁₄The left leg is bent by the drive of, and the sole is landed. In this way, by gradually approaching the sole to the side of the hip joint pitch axis 12, which is the center of gravity of the machine, it is possible to form a ground contact polygon narrower than the original posture on the floor.
[0230]
When a new ground polygon is formed in this way, it is checked whether ZMP can be set for the ground polygon (step S55). This is determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link. Then, the ZMP is moved to the ground polygon to form a new support polygon (step S56).
[0231]
Here, it is determined again whether or not the supporting polygon has become sufficiently narrow (step S57). This determination is made by determining whether the center of gravity link connecting the trunk pitch axis and the hip joint pitch axis can be released from the floor, or whether the ZMP can be moved into a ZMP stable region that can be formed only by the feet. Is determined in consideration of the movable angle of the joint, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link. In the posture of the actual machine shown in FIGS. 105 and 123, it is determined that a sufficiently narrow supporting polygon is formed. Regarding the angle of the arm when the supporting polygon is reduced, it is desirable that the angle formed by the central axis of the arm and the vertical line lowered from the shoulder axis to the floor surface is within a predetermined angle based on the torque amount.
[0232]
Then, in response to the support polygon of the fuselage becoming sufficiently narrow, the center of gravity link is lifted off with the ends of both link ends of the support polygon touching the floor, and the landing links at both link ends are used. While maintaining the ZMP in the support polygon to be formed, the distance between the ends of both link ends forming the support polygon is reduced, and the ZMP is moved to the other end of the link structure (step S58). ). This corresponds to FIGS. 92 (6) to (7) and FIGS. 93 (6) to (7).
[0233]
On the actual machine, the center-of-gravity link connecting the trunk pitch axis and the hip joint pitch axis is lifted off with the hand and sole as the ends of both link ends of the ground polygon contacting each other. Is gradually reduced, and the ZMP is moved toward the sole. The operation of the actual machine at this time is shown in FIGS. 106 to 109 and FIGS. 124 to 127.
[0234]
Then, in response to the ZMP rushing into the grounding polygon formed by only the second predetermined number or less of the floor contact links from the other end of the link structure, the ZMP was housed in the grounding polygon. The first predetermined number or more links are left from one end of the link structure as it is, and the rising links are extended in the length direction to complete the rising operation (step S59). This corresponds to FIGS. 92 (8) and 93 (8).
[0235]
On the actual machine, in response to the ZMP rushing into the grounding polygon formed by the sole, the ZMP is accommodated in the grounding polygon, and the ZMP is moved from the shoulder pitch axis 4 to the knee pitch axis 14. The rising motion is completed by releasing the link and extending the link in the length direction. The operation of the actual machine at this time is shown in FIGS. 110 to 111 and FIGS. 128 to 129.
[0236]
When extending the leaving link in the longitudinal direction, which is the final stage of getting up, it is efficient to operate the airframe actively by using the knee joint pitch axis having a larger mass operation amount.
[0237]
If two or more links that are not involved in the smallest support polygon cannot be released from the floor in step S53, an attempt is made to release two or more landing links inside the largest support polygon (step S53). S61).
[0238]
If step S61 cannot be executed, the rising operation is stopped (step S64). If step S61 can be executed successfully, the landing point is further moved to further reduce the supporting polygon (step S62).
[0239]
If step S62 cannot be executed, the rising operation is stopped (step S64). If step S62 can be executed successfully, it is checked whether the ZMP can be moved to a stable area formed by the foot (step S63). The detailed procedure for determining whether the supporting polygon has become sufficiently narrow will be described later. If the ZMP cannot be moved within the stable area, the process returns to step S61, and the same processing for reducing the supporting polygon is repeatedly executed. If the ZMP can be moved within the stable region, the process proceeds to step S58, and the operation of returning to the basic posture is performed.
[0240]
By the way, in steps S53 to S54, as shown in FIGS. 97 to 98 and FIGS. 115 to 116, in order to form a narrower ground contact polygon by landing the left and right hands behind the torso, The left and right arms are stretched to the side by using them. This unnecessarily increases the volume used for the legged mobile robot 100 to get up and perform work. Therefore, the series of operations shown in FIGS. 96 to 101 and FIGS. 113 to 119 are replaced with the operations shown in FIGS. 130 and 131 in which the shoulder roll axis is not operated and the elbow pitch axis is bent instead. The left and right hands may be landed behind the torso with a small used volume.
[0241]
In the rising operation procedure described above, it is necessary to determine in steps S57 and S63 whether or not the support polygon has become sufficiently narrow. FIG. 173 shows, in the form of a flowchart, a processing procedure for determining whether or not the support polygon has become sufficiently narrow.
[0242]
First, the ZMP deviation ε (ε_x, Ε_y, Ε_z), That is, the center position (x₀, Y₀, Z₀) And the current ZMP position (x, y, z) are determined (step S71).
[0243]
Next, this ZMP deviation ε (ε_x, Ε_y, Ε_z) To a predetermined gain G (G_x, G_y, G_z) Is multiplied by the current waist position r (r_{hx (t)}, R_{hy (t)}, R_{hz (t)}), The target waist position r (r at the next time t = t + Δt)_{hx (t + Δ t)}, R_{hy (t + Δ t)}, R_{hz (t + Δ t)}) (= R (r_{hx (t)}, R_{hy (t)}, R_{hz (t)}) + G (G_x, G_y, G_z) × ε (ε_x, Ε_y, Ε_z)) (Step S72).
[0244]
Then, it is determined whether or not the current support polygon can be formed at the next target waist position (step S73). This determination is made by calculating the next target waist position while maintaining the landing point of the landing link. In other words, inverse kinematics calculation is performed from the waist position and the landing point, and it is determined that the calculation is feasible if it is within the movable angle and within the allowable torque of the joint actuator.
[0245]
If the current support polygon cannot be formed at the next target waist position, it is determined that it is impossible to move the ZMP within the stable area where the foot can be formed, and the entire processing routine is terminated.
[0246]
On the other hand, if the current support polygon can be formed at the next target waist position, further, the ZMP when the hip is moved to the next target waist position (that is, the next) is calculated (step S74).
[0247]
Next, it is determined whether or not ZMP exists in a stable area where a foot can be formed (step S75). If the determination result is affirmative, it is determined that the ZMP can be moved into the stable area where the foot can be formed (step S76), and the entire processing routine ends. On the other hand, if the determination result is negative, the next waist position is set to the current waist position, the next ZMP is set to the current ZMP, and the process returns to step S71 to repeat the same processing.
[0248]
In the operation examples shown in FIGS. 130 and 131, the length of the upper arm is l₁, Length of forearm₂, Shoulder roll angle α, elbow pitch angle beta, shoulder to hand length l₁₂When the angle between the line connecting the shoulder and the hand is γ and the height of the shoulder is h (FIG. 132), during the operation period in which the left and right hands are landed behind the torso, the elbows satisfy the following equation. By operating the pitch shaft 7, the hand does not collide with the floor surface.
[0249]
(Equation 6)

[0250]
The rising motion pattern shown in FIG. 92 is based on a link structure in which the body of the legged mobile robot is connected to the shoulder pitch axis, the trunk pitch axis, the hip joint pitch axis, and the knee pitch axis in the height direction of the body. Modeling shows a rising motion. FIG. 133 shows a standing motion in which a legged mobile robot is generalized to a link structure in which a plurality of joint axes having substantially parallel joint degrees of freedom are connected in the length direction.
[0251]
The link structure shown in the figure is configured by connecting a plurality of joint axes having substantially parallel joint degrees of freedom in the longitudinal direction. A rising operation from a posture above the floor where all the links are in contact with the floor is realized by using links A, B, C, D, E and F.
[0252]
However, the links A to F need not be single links, and a plurality of links are actually connected via a joint axis. However, during the rising operation, the link does not operate and the link does not operate. This also includes the case where the straightness between them is maintained and the user acts as if it were a single link. For example, link A includes the links from the link end to the h-th link, link B includes the links from the h-th to the i-th link, link C includes the links from the i-th to the j-th link, and link D includes the j-th link. The link E includes links from the k-th to the l-th link, and the link F includes links from the l-th to the m-th link (or the other end of the link).
[0253]
First, a ground polygon is formed between the F-th link and the A-th link, and ZMP is set in the ground polygon (FIG. 133 (1)).
[0254]
Next, ZMP is set in the ground polygon between the E-th link and the A-th link (FIG. 133 (2)). At this time, the movement of the F-th link may be used, such as releasing two or more links from the link ends.
[0255]
Next, a narrower ground polygon is newly formed between the F-th link and the A-th link, and ZMP is set in the ground polygon (FIG. 133 (3)). For example, the F-th link that is leaving the floor is bent and its end is landed to form a new ground polygon.
[0256]
Next, a ground contact polygon is newly formed between the F-th link and the D-th or C-th link, and a ZMP is set in the ground polygon (FIG. 133 (4)). At this time, the movement of the A-th link may be used, such as releasing two or more links from the other link end.
[0257]
Next, the D-th link is grounded, a new ground polygon is formed by the F-th link and the A-th link, and ZMP is set in the ground polygon (FIG. 133 (5)). For example, the A-th link that is leaving the floor is bent and its end is landed to form a new ground polygon.
[0258]
Next, a ground polygon is newly formed by the F-th link and the A-th link, and ZMP is set in the ground polygon (FIG. 133 (6)). For example, with the end points of both link ends still landing, the D-th link during landing is released.
[0259]
Next, the ZMP is moved into the supporting polygon formed only by the A-th link by matching the end points of both link ends F and A (FIG. 133 (7)).
[0260]
Finally, each link is moved to the basic standing posture while setting the ZMP in the supporting polygon formed only by the A-th link (FIG. 133 (8)).
[0261]
F-2. Getting up from basic prone position
In FIG. 134, the legged mobile robot 100 according to this embodiment drives the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14 in a synchronously emphasized manner to perform a rising motion. The situation is shown by a joint link model.
[0262]
The legged mobile robot 100 according to the present embodiment can basically rise from the prone posture in accordance with the processing procedure shown in the flow chart form in FIG. 91 in the same manner as when rising from the supine posture. Hereinafter, the operation of raising the aircraft from the basic prone position will be described with reference to the flowchart shown in FIG.
[0263]
First, in the posture on the floor, the posture with the smallest potential energy is taken (step S51). This corresponds to a basic prone posture, and as shown in FIG. 134 (1), the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14, which are used for the rising motion, are respectively connected. All the links that you make are on the floor. The state of the actual machine at this time is shown in FIGS. 135 and 154.
[0264]
In this basic prone position, the narrowest supporting polygon is searched for in the grounding polygon formed by the floor contact link (step S52). At this time, it is determined whether or not ZMP can be planned when at least two or more links are lifted off from one end of the body. The planning possibility of the ZMP can be determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link.
[0265]
Next, two or more links that are not involved in the narrowest supporting polygon among the grounding polygons are lifted off (step S53). Step S53 corresponds to FIG. 134 (2). On the actual machine, the lower body side including the center-of-gravity link connecting the trunk joint and the hip joint is extracted as a support polygon, and two or more links from the shoulder joint to the trunk joint are lifted off as links not involved in the support polygon. I do.
[0266]
The operation of the actual machine at this time is shown in FIGS. 136 to 144 and FIGS. 155 to 163. In the illustrated example, first, shoulder roll axis actuators A of the left and right arms are provided.₅Is operated, and is rotated about the shoulder roll axis by about 90 degrees by sliding on the floor surface (FIGS. 136 to 137 and FIGS. 155 to 156), and then the upper arm yaw axis actuator A₆To rotate each arm about 180 degrees about the upper arm yaw axis (FIGS. 138 and 157). And further, shoulder roll axis actuator A₅, The arm is moved by about 90 degrees around the shoulder roll axis to move each arm to the side of the head (FIGS. 138 to 141 and FIGS. 157 to 160).
[0267]
In the series of operations shown in FIGS. 136 to 141 and FIGS. 165 to 170, the left and right arms are shaped like drawing a semicircle on the floor. At this time, the presence or absence of an obstacle on the road surface around the machine body can be detected to secure a safe working area required for the rising operation.
[0268]
Next, one or more outgoing links are bent from one end and the end of the link end is landed to form a narrower ground contact polygon (step S54). Step S54 corresponds to FIG. 134 (3).
[0269]
Then, when a new ground polygon is formed, it is checked whether a ZMP can be set for the ground polygon (step S55). This is determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link. Then, the ZMP is moved to the ground polygon to form a new support polygon (step S56).
[0270]
On the actual machine, with the elbow pitch axis 7 fixed and the left and right arms straight extended, this time the shoulder pitch axis actuator A₄, Trunk pitch axis actuator A₉, Hip joint pitch axis A₁₂And knee joint pitch axis actuator A₁₄To form a support polygon having a closed link posture in which the hand and both left and right knees are in contact with the ground (FIGS. 142 to 144, and FIGS. 161 to 163).
[0271]
144 and 153, the supporting polygon is not yet sufficiently narrow. Therefore, the landing point is moved to reduce the supporting polygon (step S60). Regarding the angle of the arm when the supporting polygon is reduced, it is desirable that the angle formed by the central axis of the arm and the vertical line lowered from the shoulder axis to the floor surface is within a predetermined angle based on the torque amount.
[0272]
On the actual machine, a narrower support polygon is formed by gradually approaching the hand to the sole, which is the other landing point, while keeping the left and right arms straight (FIGS. 145 to 145). 148, and FIGS. 164 to 167).
[0273]
Here, it is determined whether or not the support polygon has become sufficiently narrow (step S57). This determination is made by determining whether the center of gravity link connecting the trunk pitch axis and the hip joint pitch axis can be released from the floor, or whether the ZMP can be moved into a ZMP stable region that can be formed only by the feet. Is determined in consideration of the movable angle of the joint, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link. 148 and 165, it is determined that a sufficiently narrow supporting polygon is formed.
[0274]
Then, in response to the support polygon of the fuselage having become sufficiently narrow, the ZMP is maintained within the support polygon formed by the landing links at both link ends, and the two link ends forming the support polygon are maintained. The distance between the ends is reduced, and the ZMP is moved to the other end of the link structure (step S58). This corresponds to FIGS. 134 (6) to 134 (7).
[0275]
On the actual machine, with the hand and sole as the ends of both link ends of the grounding polygon touching the floor, the distance between the hand and sole is gradually reduced, and the ZMP is moved toward the sole. To go. The operation of the actual machine at this time is shown in FIGS. 149 to 150 and FIGS. 168 to 169.
[0276]
Then, in response to the ZMP rushing into the grounding polygon formed by only the second predetermined number or less of the floor contact links from the other end of the link structure, the ZMP was housed in the grounding polygon. The first predetermined number or more links are left from one end of the link structure as it is, and the rising links are extended in the length direction to complete the rising operation (step S59). This corresponds to FIG. 134 (8).
[0277]
On the actual machine, in response to the ZMP rushing into the grounding polygon formed by the sole, the ZMP is accommodated in the grounding polygon, and the ZMP is moved from the shoulder pitch axis 4 to the knee pitch axis 14. The rising motion is completed by releasing the link and extending the link in the length direction. The operation of the actual machine at this time is shown in FIGS. 151 to 153 and FIGS. 170 to 172.
[0278]
When extending the leaving link in the longitudinal direction, which is the final stage of getting up, it is efficient to operate the airframe actively by using the knee joint pitch axis having a larger mass operation amount.
[0279]
F-3. Examples of other rising operations
In the rising operation shown in FIG. 91, a rising operation having an operation pattern in which the external force moment is minimized is performed by combining the postures that minimize the ZMP support polygon in a time series. In this operation, in the process of successively forming smaller supporting polygons, a step change operation of a hand or a foot is used. However, in order to realize the step change operation, the hand or the foot needs to be out of the floor, and there must be two or more links that are not involved in the support polygon, and the step change operation may be performed depending on the attitude of the aircraft. In some cases, the rising operation itself breaks down (step S64 in FIG. 91).
[0280]
On the other hand, in the process of successively forming smaller supporting polygons, if the stepping operation of the hand or the foot cannot be realized, the rising operation is performed by using the drag operation of the hand or the foot. Can be reduced. In the following, in the process of successively forming smaller supporting polygons, a rising operation using a step change operation of a hand or a foot and a drag operation will be described.
[0281]
FIG. 174 shows, in the form of a flowchart, a rising operation using a step change operation of a hand or a foot and a drag operation. Hereinafter, the rising operation procedure will be described. FIG. 175 to FIG. 191 sequentially show how the airframe rises up from the basic prone position while using the hand or foot stepping operation or the dragging operation. Hereinafter, each drawing will be referred to as appropriate.
[0282]
First, in the posture on the floor, the posture with the smallest potential energy is taken (step S81). This corresponds to the basic prone posture, and the state of the actual machine at this time is shown in FIG.
[0283]
However, in a case where the user rises up continuously with the overturning operation, the step-up operation can be completed in a short time by omitting step S81 (described later).
[0284]
In this basic prone position, the narrowest supporting polygon is searched for in the grounding polygon formed by the floor contact link (step S82). At this time, it is determined whether or not ZMP can be planned when at least two or more links are lifted off from one end of the body. The planning possibility of the ZMP can be determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link.
[0285]
Here, it is determined whether or not two or more of the links not involved in the smallest supporting polygon can be left (step S83). If two or more links that are not involved in the minimum support polygon can be released from the floor, the process proceeds to the next step S84, where a smaller grounded polygon is formed by the stepping operation of the hand or the foot. On the other hand, if it is not possible to leave the floor, the process proceeds to step S91, and a smaller grounded polygon is formed using the drag operation of the hand or foot.
[0286]
In step S84, two or more of the links that are not involved in the smallest supporting polygon are released from the floor, and the exit links are bent and landed to form a smaller grounded polygon (step S85).
[0287]
For example, in FIG. 179 to FIG. 181 and FIG. 184 to FIG. 186, the rising robot with both hands and feet on the ground touches the left foot and the right foot while stepping on the left foot and the right foot, and FIGS. 175, 182 to 183, 185, and 187. As shown in FIG. 5, the exit link is bent and landed to form a smaller ground contact polygon.
[0288]
Then, when a new ground polygon is formed, it is checked whether ZMP can be set for the ground polygon (step S86). This is determined in consideration of the movable angle of the link structure, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link. Then, the ZMP is moved to the ground polygon to form a new support polygon (step S87). If the ZMP cannot be set for the contact polygon, the process returns to step S83 to check again whether to perform the step change operation or the drag operation of the hand or the foot.
[0289]
Here, it is determined whether or not the support polygon has become sufficiently narrow (step S88). This determination is made by determining whether the center of gravity link connecting the trunk pitch axis and the hip joint pitch axis can be released from the floor, or whether the ZMP can be moved into a ZMP stable region that can be formed only by the feet. Is determined in consideration of the movable angle of the joint, the torque, joint force, angular velocity, angular acceleration, and the like of each joint actuator connecting the link.
[0290]
Then, in response to the support polygon of the fuselage having become sufficiently narrow, the ZMP is maintained within the support polygon formed by the landing links at both link ends, and the two link ends forming the support polygon are maintained. The distance between the ends is reduced, and the ZMP is moved to the other end of the link structure (step S89).
[0291]
On the actual machine, with the hand and sole as the ends of both link ends of the grounding polygon touching the floor, the distance between the hand and sole is gradually reduced, and the ZMP is moved toward the sole. To go. 188 to 189 show the operation of the actual machine at this time.
[0292]
Then, in response to the ZMP rushing into the grounding polygon formed by only the second predetermined number or less of floor contact links from the other end of the link structure, the ZMP was housed in the grounding polygon. A first predetermined number or more of links are left from one end of the link structure as they are, and the left links are extended in the length direction to complete the rising operation (step S90).
[0293]
On the actual machine, in response to the ZMP rushing into the grounding polygon formed by the sole, the ZMP is accommodated in the grounding polygon, and the ZMP is moved from the shoulder pitch axis 4 to the knee pitch axis 14. The rising motion is completed by releasing the link and extending the link in the length direction. The operation of the actual machine at this time is shown in FIGS.
[0294]
When extending the leaving link in the longitudinal direction, which is the final stage of getting up, it is efficient to operate the airframe actively by using the knee joint pitch axis having a larger mass operation amount.
[0295]
On the other hand, if it is determined in step S83 that two or more of the links that do not participate in the smallest support polygon cannot be released from the floor, the largest support polygon is used to perform the drag operation of the hand or the foot. It is checked whether or not two or more links that are landing on the inner side can be separated from the floor (step S91).
[0296]
Here, if it is not possible to leave two or more landing links within the maximum supporting polygon, it is determined whether or not the landing point can be further moved to reduce the supporting polygon. I do. If the supporting polygon cannot be reduced, the rising operation is stopped (step S95). That is, the rising operation fails.
[0297]
On the other hand, if two or more landing links inside the largest support polygon can be separated from the floor, two or more landing links inside the largest support polygon should be separated from the floor (step). S92) The landing point is moved using the drag operation of the hand or the foot, and the supporting polygon is reduced (step S93).
[0298]
For example, as shown in FIGS. 176 to 178 and FIGS. 187 to 188, the rising robot with both hands and feet on the ground gradually drags the support polygon by dragging toward the feet with both hands on the ground. To smaller.
[0299]
Thereafter, it is determined whether or not the support polygon has become sufficiently narrow (step S88). Then, in response to the support polygon of the fuselage having become sufficiently narrow, the ZMP is maintained within the support polygon formed by the landing links at both link ends, and the two link ends forming the support polygon are maintained. The distance between the ends is reduced, and the ZMP is moved to the other end of the link structure (step S89).
[0300]
Then, in response to the ZMP rushing into the grounding polygon formed by only the second predetermined number or less of floor contact links from the other end of the link structure, the ZMP was housed in the grounding polygon. A first predetermined number or more of links are left from one end of the link structure as they are, and the left links are extended in the length direction to complete the rising operation (step S90).
[0301]
FIG. 199 shows, in the form of a flowchart, a detailed processing procedure for searching for the link in which the number of links not involving the smallest supporting polygon is the largest in step S83 and the site thereof.
[0302]
First, in steps S101 and S102, variables i and j and an array type variable M are initialized. Next, ZMP is set at the j-th part of the i-th link (step S103).
[0303]
Here, it is determined whether the ZMP space is stable (step S104). If the ZMP space is stable, the number of links that are not involved in the smallest supporting polygon is calculated (step S105), and the number of exit links at the j-th part of the i-th link is substituted for L. If L is larger than M (step S106), L (i, j) is substituted for M (A, B) (step S107).
[0304]
On the other hand, if the ZMP space is not stable, if L is not larger than M, or if L (i, j) is substituted for M (A, B), j is incremented by 1 (step S108), and j Is greater than or equal to the total number of parts J (step S109). If j has not yet reached the total number of parts J, the process returns to step S103, and the same processing as described above is repeatedly executed.
[0305]
Next, i is incremented by 1 (step S110), and it is determined whether i exceeds the total number of links I (step S111). If i has not reached the total number of links, the process returns to step S102, and the same processing as described above is repeatedly executed.
[0306]
If i exceeds the total number of links I, the link is substituted into A and the part is substituted into B, and this processing routine is terminated.
[0307]
As described above, in a case where the robot rises continuously from the overturning operation, the step S81 can be omitted to complete the rising operation in a short time.
[0308]
For example, when the center of gravity of the fuselage is at the waist, the ZMP can be set at a position where the number of links not involving the smallest supporting polygon is the largest. After such a fall / landing operation, all the links that can be lifted off the floor are lifted off. By doing so, a smaller grounded polygon can be formed in fewer steps, so that a faster and more efficient rising operation can be realized.
[0309]
FIGS. 192 to 198 show a series of operations of the airframe in the case of performing a rising operation following a falling operation.
[0310]
From the standing posture shown in FIG. 192, the overturning operation starts toward the rear of the aircraft as shown in FIGS. 192 to 193, and as shown in FIG.
[0311]
In the example shown in FIG. 194, the ZMP is set in the body where the number of links not involving the smallest supporting polygon is the largest. Also, what is characteristic is that, unlike the example described with reference to FIGS. 23 to 38 and FIGS. It is in the point.
[0312]
In the subsequent rising operation, as shown in FIG. 195, all the links that can be separated from the floor, that is, the legs and the body are released from the floor, and the rising operation is started. Here, by driving the pitch axis actuator of the hip joint and / or the trunk, the upper body is raised as shown in FIGS. 196 to 197. And the hip joint pitch axis A of the right leg₁₂The right leg is lifted by driving the knee joint actuator A₁₄The right leg is bent by the drive of, and the sole is landed. Next, the hip pitch axis A of the leg₁₂The right leg is lifted by driving the knee joint actuator A₁₄The left leg is bent by the drive of, and the sole is landed. In this manner, by gradually approaching the sole to the side of the hip joint pitch axis 12, which is the position of the machine center of gravity, it is possible to form a ground contact polygon narrower than the original posture above the floor, as shown in FIG. 198. .
[0313]
When the rising operation is performed continuously with the overturning operation, a smaller grounded polygon can be formed in fewer steps as compared with the example described with reference to FIGS. 23 to 38 and FIGS. That is, it should be sufficiently understood that according to this embodiment, a narrow grounded polygon can be formed more efficiently, and the rising operation is speeded up.
[0314]
[Supplement]
The present invention has been described in detail with reference to the specific embodiments. However, it is obvious that those skilled in the art can modify or substitute the embodiment without departing from the spirit of the present invention.
[0315]
The gist of the present invention is not necessarily limited to a product called a “robot”. That is, as long as the mechanical device performs a motion resembling a human motion by using an electric or magnetic action, the present invention similarly applies to a product belonging to another industrial field such as a toy. Can be applied.
[0316]
In short, the present invention has been disclosed by way of example, and the contents described in this specification should not be interpreted in a limited manner. In order to determine the gist of the present invention, the claims described at the beginning should be considered.
[0317]
【The invention's effect】
Advantageous Effects of Invention According to the present invention, an excellent legged mobile robot and leg excellent in that the damage to the robot can be reduced as much as possible by controlling the motion of the entire body including the torso and the arm as well as the leg during the fall and fall, It is possible to provide a method of controlling the operation of a mobile robot when it falls over.
[0318]
Further, according to the present invention, it is possible to provide an excellent motion control device and motion control method for a legged mobile robot, and a robot device, which can autonomously recover a standing posture from a floor posture such as a supine position or a prone position. Can be.
[0319]
Further, according to the present invention, an excellent legged mobile robot operation control device and operation control method, which can recover a standing posture from a floor posture such as a supine or prone position by a stable operation with a relatively small torque, and A robot device can be provided.
[Brief description of the drawings]
FIG. 1 is a diagram showing a state in which a legged mobile robot provided for carrying out the present invention is standing upright as viewed from the front.
FIG. 2 is a diagram showing a state in which the legged mobile robot provided for carrying out the present invention stands upright, as viewed from the rear.
FIG. 3 is a diagram schematically showing a configuration of a degree of freedom of a joint included in the legged mobile robot.
FIG. 4 is a diagram schematically showing a control system configuration of the legged mobile robot 100.
FIG. 5 is a diagram showing a basic state transition of the motion system of the legged mobile robot 100.
FIG. 6 is a diagram showing a basic supine posture of the legged mobile robot 100;
7 is a diagram showing a basic prone posture of the legged mobile robot 100. FIG.
8 is a diagram showing a basic standing posture of the legged mobile robot 100. FIG.
9 is a diagram showing a basic walking posture of the legged mobile robot 100. FIG.
10 is a diagram showing a multi-mass point approximation model of the legged mobile robot 100. FIG.
FIG. 11 is an enlarged view of the periphery of the waist of the multi-mass model.
FIG. 12 is a flowchart illustrating a processing procedure for generating a body motion capable of stably walking in the legged mobile robot 100.
FIG. 13 is a flowchart illustrating a schematic processing procedure of operation control of the body of the legged mobile robot 100 during legged work.
FIG. 14 is a diagram for explaining a principle of maintaining a supporting area when the body falls.
FIG. 15 is a diagram for explaining the principle of maximizing the support polygon when the body falls on the floor.
FIG. 16 is a diagram for explaining an operation for maintaining a supporting area when the legged mobile robot 100 falls backward, that is, in a supine position.
FIG. 17 is a diagram for explaining an operation of maintaining a supporting area when the legged mobile robot 100 falls backward, that is, in a supine position.
FIG. 18 is a diagram for explaining an operation of maintaining a supporting area when the legged mobile robot 100 falls forward or in a prone position.
FIG. 19 is a diagram for explaining an operation of maintaining a supporting area when the legged mobile robot 100 falls down toward the front, that is, the prone posture.
FIG. 20 is a flowchart showing a processing procedure for the legged mobile robot 100 according to the present embodiment to perform a falling operation due to an inability to plan a foot.
FIG. 21 shows a legged mobile robot 100 including a plurality of substantially parallel joint axes connected in the height direction such as a shoulder joint pitch axis 4, a trunk pitch axis 9, a hip joint pitch axis 12, and a knee joint pitch axis 14. FIG. 10 is a diagram illustrating an operation of modeling as a link structure and driving each joint pitch axis in a synchronously emphasizing manner to fall toward a supine posture.
FIG. 22 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 23 is a side view showing a state where the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 24 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 25 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 26 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 27 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 28 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 29 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 30 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 31 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 32 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 33 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 34 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 35 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 36 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 37 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 38 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 39 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 40 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 41 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 42 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 43 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 44 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 45 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 46 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 47 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 48 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 49 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 50 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 51 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 52 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 53 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 54 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 55 is a perspective view showing that the legged mobile robot 100 falls from a standing posture to a supine posture.
FIG. 56 shows a legged mobile robot 100 including a plurality of substantially parallel joint axes connected in the height direction such as a shoulder joint pitch axis 4, a trunk pitch axis 9, a hip joint pitch axis 12, and a knee joint pitch axis 14. It is a figure which modeled as a link structure, and showed the operation | movement which drives each joint pitch axis in a synchronous emphasis and falls down to a prone posture.
FIG. 57 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 58 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 59 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 60 is a side view showing a state where the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 61 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 62 is a side view showing a state where the legged mobile robot 100 falls from the standing posture to the prone posture.
FIG. 63 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 64 is a side view showing a state where the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 65 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 66 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 67 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 68 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 69 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 70 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 71 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 72 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 73 is a side view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 74 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 75 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 76 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 77 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 78 is a perspective view showing a state where the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 79 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 80 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 81 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 82 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 83 is a perspective view showing a state where the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 84 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 85 is a perspective view showing a state where the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 86 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 87 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 88 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 89 is a perspective view showing how the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 90 is a perspective view showing a state in which the legged mobile robot 100 falls from a standing posture to a prone posture.
FIG. 91: A legged mobile robot 100 according to one embodiment of the present invention drives the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14 in a synchronously emphasized manner to get up. Is a flowchart showing a processing procedure for performing the processing.
FIG. 92: A legged mobile robot 100 according to an embodiment of the present invention drives the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14 in a synchronously emphasized manner, and the supine posture. FIG. 6 is a diagram showing a state of performing a rising motion from a joint link model.
FIG. 93 is a view showing a manner in which a legged mobile robot having no trunk pitch axis performs a rising operation from a supine posture by synchronous driving of a plurality of joint pitch axes.
FIG. 94 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 95 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 96 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 97 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 98 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 99 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 100 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 101 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 102 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 103 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 104 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 105 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 106 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 107 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 108 is a side view showing a manner in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 109 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 110 is a side view showing a state in which the legged mobile robot 100 gets up from a basic supine posture.
FIG. 111 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 112 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 113 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 114 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 115 is a perspective view showing that the legged mobile robot 100 rises from a basic supine posture.
FIG. 116 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 117 is a perspective view showing how the legged mobile robot 100 rises from a basic supine posture.
FIG. 118 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 119 is a perspective view showing how the legged mobile robot 100 rises from a basic supine posture.
FIG. 120 is a perspective view showing how the legged mobile robot 100 rises from a basic supine posture.
FIG. 121 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 122 is a perspective view showing how the legged mobile robot 100 rises from a basic supine posture.
FIG. 123 is a perspective view showing how the legged mobile robot 100 rises from a basic supine posture.
FIG. 124 is a perspective view showing that the legged mobile robot 100 rises from a basic supine posture.
FIG. 125 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 126 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
127 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic supine posture. FIG.
FIG. 128 is a perspective view showing that the legged mobile robot 100 rises from a basic supine posture.
FIG. 129 is a perspective view showing how the legged mobile robot 100 rises from a basic supine posture.
130 is a view showing a modified example of a series of operations for landing left and right hands behind the trunk. FIG.
FIG. 131 is a view showing a modified example of a series of operations for landing left and right hands behind the trunk.
FIG. 132 is a view for explaining the operation of the arm shown in FIGS. 130 and 131.
FIG. 133 is a diagram in which the legged mobile robot shown in FIG. 92 is replaced with a link structure and generalized.
134 is a prone posture in which the legged mobile robot 100 according to the embodiment of the present invention drives the shoulder joint pitch axis 4, the trunk pitch axis 9, the hip joint pitch axis 12, and the knee joint pitch axis 14 in a synchronously emphasized manner. FIG. 6 is a diagram showing a state of performing a rising motion from a joint link model.
FIG. 135 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 136 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 137 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 138 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 139 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 140 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 141 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 142 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 143 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 144 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 145 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 146 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 147 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 148 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 149 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 150 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 151 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 152 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 153 is a side view showing a state in which the legged mobile robot 100 rises from a basic supine posture.
FIG. 154 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 155 is a perspective view showing that the legged mobile robot 100 rises from a basic prone position.
FIG. 156 is a perspective view showing a state in which the legged mobile robot 100 rises from the basic prone position.
FIG. 157 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone posture.
FIG. 158 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 159 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 160 is a perspective view showing a state in which the legged mobile robot 100 rises from the basic prone position.
FIG. 161 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone posture.
FIG. 162 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 163 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 164 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 165 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 166 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 167 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 168 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
FIG. 169 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone position.
170 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone posture. FIG.
FIG. 171 is a perspective view showing a state in which the legged mobile robot 100 rises from a basic prone posture.
FIG. 172 is a perspective view showing a state in which the legged mobile robot 100 rises from the basic prone position.
FIG. 173 is a flowchart showing a processing procedure for determining whether the support polygon has become sufficiently narrow.
FIG. 174 is a flowchart showing a rising operation using a step change operation and a drag operation of a hand or a foot.
FIG. 175 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
FIG. 176 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
FIG. 177 is a side view showing a state in which the legged mobile robot 100 rises from the basic prone position while utilizing the stepping operation and the dragging operation of the hand and the foot.
FIG. 178 is a side view showing a state in which the legged mobile robot 100 rises from the basic prone posture while using the stepping operation and the drag operation of the hand and the foot.
FIG. 179 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
FIG. 180 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone position while utilizing the stepping operation and the dragging operation of the hand and the foot.
FIG. 181 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone position while using the stepping operation of the hand and the foot and the dragging operation.
FIG. 182 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
FIG. 183 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
FIG. 184 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while utilizing the step change operation and the drag operation of the hand and the foot.
FIG. 185 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
FIG. 186 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
FIG. 187 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
FIG. 188 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
FIG. 189 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while using the step change operation and the drag operation of the hand and the foot.
190 is a side view showing a state in which the legged mobile robot 100 rises from the basic prone posture while utilizing the stepping operation of the hand and the foot and the dragging operation. FIG.
FIG. 191 is a side view showing a state in which the legged mobile robot 100 rises up from the basic prone posture while utilizing the stepping operation and the dragging operation of the hand and the foot.
FIG. 192 is a diagram illustrating a series of operations of the aircraft in a case where the user gets up consecutively with the overturning operation.
FIG. 193 is a diagram showing a series of operations of the aircraft in a case where the user gets up consecutively with the overturning operation.
FIG. 194 is a diagram illustrating a series of operations of the aircraft in a case where the user gets up consecutively with the overturning operation.
FIG. 195 is a diagram illustrating a series of operations of the aircraft in a case where the user gets up consecutively with the overturning operation.
FIG. 196 is a diagram illustrating a series of operations of the aircraft in a case where the user gets up consecutively with the overturning operation.
FIG. 197 is a diagram illustrating a series of operations of the aircraft in a case where the user gets up consecutively with the overturning operation.
FIG. 198 is a diagram showing a series of operations of the aircraft in a case where the user gets up consecutively with the overturning operation.
FIG. 199 is a flowchart showing a processing procedure for searching for a link having the largest number of links that are not involved in the smallest supporting polygon and its portion.
[Explanation of symbols]
1 ... Yaw axis of neck joint
2A: First neck joint pitch axis
2B: second neck joint (head) pitch axis
3 ... Neck joint roll axis
4: Shoulder joint pitch axis
5 ... shoulder joint roll axis
6 Upper arm yaw axis
7. Elbow joint pitch axis
8: Wrist joint yaw axis
9: trunk pitch axis
10 ... trunk roll axis
11 ... Yaw axis of hip joint
12: Hip joint pitch axis
13 ... hip joint roll axis
14. Knee joint pitch axis
15: Ankle joint pitch axis
16 ... Ankle joint roll axis
30 ... head unit, 40 ... trunk unit
50: arm unit, 51: upper arm unit
52: elbow joint unit, 53: forearm unit
60 ... leg unit, 61 ... thigh unit
62: knee joint unit, 63: shin unit
80: control unit, 81: main control unit
82: Peripheral circuit
91, 92 ... grounding confirmation sensor
93,94 ... Acceleration sensor
95 ... Attitude sensor
96 ... Acceleration sensor
100 ... Legged mobile robot

Claims (56)

An operation control device for a legged mobile robot having a movable leg and performing a legged work in a standing posture,
The legged mobile robot has a plurality of postures or states,
First means for calculating an area S of a support polygon formed by a ground contact point of the fuselage and a road surface; second means for calculating a change ΔS / Δt of the area S of the support polygon per time Δt;
Third means for determining, based on the area S of the supporting polygon or the rate of change ΔS / Δt thereof, the operation of the aircraft when changing its posture or state;
An operation control device for a legged mobile robot, comprising: The third means, when falling,
Landing site searching means for searching for a landing site based on a change per unit time Δt of an area S of a support polygon formed by a ground contact point of a vehicle and a road surface;
The target landing at which the site selected by the landing site searching means should land so that the change ΔS / Δt per unit time Δt of the area S of the support polygon formed by the ground contact point of the vehicle and the road surface is minimized. Target landing point setting means for setting points,
Part landing means for landing the part selected by the landing part search means at the target landing point set by the target landing point setting means;
The operation control device for a legged mobile robot according to claim 1, further comprising: The apparatus further includes a support polygon enlarging means for moving a landing part so as to further expand the newly formed support polygon by landing the selected part by the part landing part.
3. The motion control device for a legged mobile robot according to claim 2, wherein: Until the potential energy of the body is minimized and the overturning operation is completed, the landing operation of the part by the landing part searching means and the target landing point setting means and / or the support polygon by the support polygon enlarging means. Repeat the enlarging operation of the square,
3. The motion control device for a legged mobile robot according to claim 2, wherein: The legged mobile robot has a link structure in which a plurality of joint axes having substantially parallel joint degrees of freedom are connected in a length direction,
The target landing point setting means sets, as a target, a portion having a link where the number of exit links is the maximum.
3. The motion control device for a legged mobile robot according to claim 2, wherein: The legged mobile robot has a link structure in which a plurality of joint axes having substantially parallel joint degrees of freedom are connected in a length direction,
The third means is configured such that, when the legged mobile robot returns from a falling state, the ground contact link formed by the floor contact link in a posture on the floor where two or more links including the center of gravity link serving as the center of gravity of the body touches the floor. Means for searching for the narrowest supporting polygon formed by the least number of links within the polygon;
Means for leaving a floor contact link other than the searched support polygon in the ground contact polygon;
Means for bending two or more successive outgoing links and contacting the ends of the link ends to form a narrower grounded polygon;
Means for lifting a first predetermined number or more of links from one end of the link structure to erect the airframe in response to the support polygon being sufficiently narrowed;
The operation control device for a legged mobile robot according to claim 1, further comprising: The means for searching for a support polygon extracts a floor contact link that allows ZMP planning even when the user leaves the floor.
The operation control device for a legged mobile robot according to claim 6, wherein: The means for searching for a support polygon searches for a narrower support polygon while keeping the center of gravity link in a floor-contact state,
The operation control device for a legged mobile robot according to claim 6, wherein: The means for erecting the fuselage determines whether or not the support polygon has become sufficiently narrow based on whether or not the center-of-gravity link can be separated from the floor with the ends of both link ends of the grounded polygon in contact with the floor.
The operation control device for a legged mobile robot according to claim 6, wherein: The means for erecting the aircraft include:
Means for leaving the center-of-gravity link in a state where the ends of both link ends of the support polygon are in contact with the floor,
Means for moving the ZMP to the other end of the link structure by reducing the distance between the ends of both link ends of the support polygon in a state where the center of gravity link is separated from the floor;
In response to the ZMP rushing from the other end of the link structure into the grounding polygon formed only by the second predetermined number or less of the floor contact links, the ZMP is housed in the grounding polygon and Means for releasing a first predetermined number or more of links from one end of the link structure and extending the released links in the length direction;
The operation control device for a legged mobile robot according to claim 6, wherein: The means for extending the exit link in the longitudinal direction operates positively using greater joint degrees of freedom of mass manipulation;
The operation control device for a legged mobile robot according to claim 10, wherein: The link structure, at least the shoulder joint pitch axis, the trunk pitch axis, the hip joint pitch axis, the knee pitch axis is formed in the height direction of the body is connected,
The operation control device for a legged mobile robot according to claim 6, wherein: The means for searching for the supporting polygon extracts two or more links that are continuous from one end including at least the shoulder joint pitch axis as a link that allows ZMP planning even when the user leaves the floor,
13. The motion control device for a legged mobile robot according to claim 12, wherein: The means for searching for the supporting polygon searches for a narrower supporting polygon while keeping the link connecting the trunk pitch axis and the hip joint pitch axis as a center of gravity link in a floor contact state,
13. The motion control device for a legged mobile robot according to claim 12, wherein: Means for forming a narrower ground contact polygon is to bend the leaving link at the shoulder joint pitch axis so that the hand that is the end of the link end touches the floor.
13. The motion control device for a legged mobile robot according to claim 12, wherein: The length of the upper arm is l ₁ , the length of the forearm is l ₂ , the shoulder roll angle is α, the elbow pitch angle is beta, the length from the shoulder to the hand is l ₁₂ , and the angle between the shoulder and the hand is γ. , Where h is the height of the shoulder,

Operate the arm to satisfy
The operation control device for a legged mobile robot according to claim 15, wherein: The means for searching for the support polygon extracts two or more consecutive links from the other end including at least the knee joint pitch axis as a link that allows planning of the ZMP even when the user leaves the floor.
13. The motion control device for a legged mobile robot according to claim 12, wherein: Means for forming a narrower ground contact polygon is to cause the exit link to bend at the knee joint pitch axis and to contact the sole that is the end of the link end,
13. The motion control device for a legged mobile robot according to claim 12, wherein: The means for erecting the aircraft may be capable of leaving the center-of-gravity link connecting the trunk pitch axis and the hip joint pitch axis with the hand and sole as the ends of both link ends of the ground polygon contacting each other. To determine whether the supporting polygon has become sufficiently narrow,
13. The motion control device for a legged mobile robot according to claim 12, wherein: The means for erecting the aircraft include:
Means for leaving the center-of-gravity link connecting the trunk pitch axis and the hip joint pitch axis with the hand and sole as the ends of both link ends of the grounding polygon in contact with each other,
Means for moving the ZMP to the sole by reducing the distance between the hand and the sole as ends of both link ends of the grounded polygon in a state where the center of gravity link is separated from the floor,
In response to the ZMP rushing into the grounding polygon formed by the sole, the link from the shoulder pitch axis to the knee pitch axis is lifted while the ZMP is housed in the grounding polygon. Means for erecting the body by extending the outgoing link in the length direction;
The motion control device for a legged mobile robot according to claim 12, further comprising: The means for extending the exit link in the length direction operates using a knee joint pitch axis having a larger mass operation amount positively.
21. The motion control device for a legged mobile robot according to claim 20, wherein: The means for generating the narrower contact polygon may be a stepping action on the hand or the foot or a drag with the floor, depending on whether two or more of the links not involved in the smallest supporting polygon can be left. Selectively use any of the actions to form a narrower grounded polygon,
The operation control device for a legged mobile robot according to claim 6, wherein: An operation control method for a legged mobile robot having a movable leg and performing a legged work in a standing posture,
The legged mobile robot has a plurality of postures or states,
A first step of calculating an area S of a support polygon formed by a ground contact point of the fuselage and a road surface;
A second step of calculating a change ΔS / Δt of the area S of the support polygon per time Δt;
A third step of determining, based on the area S of the supporting polygon or the speed of change ΔS / Δt thereof, the operation of the aircraft when changing its posture or state;
An operation control method for a legged mobile robot, comprising: In the third step, when falling,
A landing site search step of searching for a change per unit time Δt of an area S of a support polygon formed by a ground contact point of a vehicle and a road surface;
The target landing at which the site selected in the landing site search step should land so that the change ΔS / Δt per unit time Δt of the area S of the support polygon formed by the ground contact point of the vehicle and the road surface is minimized. A target landing point setting step of setting points,
A part landing step of landing the part selected in the landing part search step at the target landing point set in the target landing point setting step;
The operation control method for a legged mobile robot according to claim 23, comprising: The method further includes a support polygon expanding step of moving a landing site so as to further expand the newly formed support polygon by landing the selected site in the site landing step.
The operation control method for a legged mobile robot according to claim 24, wherein: Until the potential energy of the body is minimized and the overturning operation is completed, the landing operation of the part by the landing part searching means and the target landing point setting step and / or the support polygon by the support polygon enlarging means. Repeat the enlarging operation of the square,
The operation control method for a legged mobile robot according to claim 24, wherein: The legged mobile robot has a link structure in which a plurality of joint axes having substantially parallel joint degrees of freedom are connected in a length direction,
The target landing point setting means sets, as a target, a portion having a link where the number of exit links is the maximum.
The operation control method for a legged mobile robot according to claim 24, wherein: The legged mobile robot has a link structure in which a plurality of joint axes having substantially parallel joint degrees of freedom are connected in a length direction,
In the third step, when the legged mobile robot returns from a falling state,
Searching for the narrowest supporting polygon formed by the smallest number of links in the grounding polygon formed by the floor contact link in an on-floor posture where two or more links including the center of gravity link serving as the center of gravity of the body are on the floor; ,
Leaving the floor contact link other than the searched supporting polygon in the grounding polygon;
Bending two or more successive outgoing links and contacting the ends of the link ends to form a narrower grounded polygon;
Responding to the support polygon having become sufficiently narrow, leaving a first predetermined number or more of links from one end of the link structure to erect the fuselage;
The operation control method for a legged mobile robot according to claim 23, comprising: In the step of searching for the support polygon, a floor contact link capable of planning a ZMP even when the user leaves the floor is extracted.
The method for controlling the operation of a legged mobile robot according to claim 28, wherein: In the step of searching for the supporting polygon, searching for a narrower supporting polygon while keeping the center of gravity link in a floor-contact state,
The method for controlling the operation of a legged mobile robot according to claim 28, wherein: In the step of erecting the fuselage, it is determined whether or not the support polygon has become sufficiently narrow based on whether or not the center-of-gravity link can be separated from the floor with the ends of both link ends of the grounded polygon in contact with the floor.
The method for controlling the operation of a legged mobile robot according to claim 28, wherein: In the step of erecting the aircraft,
The center-of-gravity link is released from the floor with both ends of the support polygon touching the floor, and the distance between the ends of the two ends of the support polygon is reduced to move the ZMP to the other end of the link structure. Let
In response to the ZMP rushing into the ground polygon formed by only the second predetermined number of links or less from the other end of the link structure, the link structure is stored with the ZMP housed in the ground polygon. Leaving a first predetermined number or more links from one end side of the body and extending the leaving links in the length direction;
The method for controlling the operation of a legged mobile robot according to claim 28, wherein: In the step of extending the evacuation link in the length direction, it operates by positively using a larger joint degree of freedom of mass operation amount,
33. The operation control method for a legged mobile robot according to claim 32. The link structure, at least the shoulder joint pitch axis, the trunk pitch axis, the hip joint pitch axis, the knee pitch axis is formed in the height direction of the body is connected,
The method for controlling the operation of a legged mobile robot according to claim 28, wherein: In the step of searching for the supporting polygon, two or more consecutive links from one end including at least the shoulder joint pitch axis are extracted as links that can be planned for ZMP even when the user leaves the floor,
The method for controlling the operation of a legged mobile robot according to claim 34, wherein: In the step of searching for the supporting polygon, searching for a narrower supporting polygon while keeping the link connecting the trunk pitch axis and the hip joint pitch axis as a center of gravity link in a floor contact state,
The method for controlling the operation of a legged mobile robot according to claim 34, wherein: In the step of forming a narrower ground contact polygon, the exit link is bent at the shoulder joint pitch axis, and the hand at the end of the link end is brought into contact with the floor.
The method for controlling the operation of a legged mobile robot according to claim 34, wherein: The length of the upper arm is l ₁ , the length of the forearm is l ₂ , the shoulder roll angle is α, the elbow pitch angle is beta, the length from the shoulder to the hand is l ₁₂ , and the angle between the shoulder and the hand is γ. , Where h is the height of the shoulder,

The operation control method for a legged mobile robot according to claim 37, wherein the arm is operated so as to satisfy the following. In the step of searching for the supporting polygon, two or more consecutive links from the other end including at least the knee joint pitch axis are extracted as links capable of planning the ZMP even when the user leaves the floor,
The method for controlling the operation of a legged mobile robot according to claim 34, wherein: In the step of forming a narrower ground contact polygon, the exit link is bent at the knee joint pitch axis, and the sole that is the end of the link end touches the floor.
The method for controlling the operation of a legged mobile robot according to claim 34, wherein: In the step of erecting the body, it is possible to leave the center-of-gravity link connecting the trunk pitch axis and the hip joint pitch axis with the hands and soles as the ends of both link ends of the grounding polygon in contact with the floor. To determine whether the supporting polygon has become sufficiently narrow,
The method for controlling the operation of a legged mobile robot according to claim 34, wherein: In the step of erecting the aircraft,
The center of gravity link which connects the trunk pitch axis and the hip joint pitch axis with the hand and sole as the ends of both link ends of the grounded polygon being in contact with the floor is lifted off, and the hand as both link ends of the grounded polygon is released. And moving the ZMP to the sole at the end of the leg,
In response to the ZMP rushing into the grounding polygon formed by the sole, the link from the shoulder pitch axis to the knee pitch axis is released while the ZMP is housed in the grounding polygon. And erect the aircraft,
The method for controlling the operation of a legged mobile robot according to claim 34, wherein: In the step of extending the evacuation link in the length direction, it operates using a knee joint pitch axis having a larger mass operation amount positively.
43. The operation control method for a legged mobile robot according to claim 42. In the step of generating the narrower ground contact polygon, the step of changing the hand or the foot or dragging with the floor depends on whether two or more of the links not related to the smallest supporting polygon can be lifted off the floor. Selectively use any of the actions to form a narrower grounded polygon,
29. The motion control device for a legged mobile robot according to claim 28. A legged mobile robot having movable legs and performing legged work in a standing posture,
External force detection means for detecting the application of external force to the aircraft,
ZMP trajectory planning means for arranging a ZMP on which the moment applied to the aircraft is balanced on or inside the side of the supporting polygon formed by the sole contact point and the road surface, based on the detection result by the external force detecting means,
Overturning operation executing means for executing an overturning operation of the fuselage in response to the ZMP trajectory planning means making it difficult or impossible to arrange the ZMP within the supporting polygon due to an external force applied to the fuselage When,
A legged mobile robot comprising: The external force detecting means detects an external force applied to the body by a floor reaction force sensor or an acceleration sensor disposed on a sole, or an acceleration sensor disposed on a waist position of a torso.
The legged mobile robot according to claim 45, wherein: An operation control method for a legged mobile robot having a movable leg and performing a legged work in a standing posture,
An external force detection step of detecting application of an external force to the body,
A ZMP trajectory planning step of arranging a ZMP in which a moment generated by an external force applied to the aircraft is balanced inside the supporting polygon formed by the sole contact point and the road surface based on the detection result in the external force detection step,
A tipping operation executing step of executing a tipping operation of the airframe in response to the ZMP trajectory planning step making it difficult or impossible to arrange the ZMP in the supporting polygon due to an external force applied to the airframe; When,
An operation control method for a legged mobile robot, comprising: In the external force detection step, a floor reaction force sensor or an acceleration sensor disposed at the sole, or an acceleration sensor disposed at the waist position of the body detects an external force applied to the aircraft.
48. The method according to claim 47, wherein the operation of the legged mobile robot is controlled. An operation control device for a legged mobile robot having a movable leg and performing a legged work in a standing posture,
Means for calculating the impact moment applied to the aircraft at each stage of the fall,
Means for calculating the impact force received by the aircraft from the floor at each stage of the fall;
Means for calculating an area S of a support polygon formed by a ground contact point of the fuselage and a road surface;
First landing site searching means for selecting a next landing site so that the area S of the supporting polygon is minimum or constant;
Second landing site searching means for selecting a next landing site so that the area S of the supporting polygon increases;
An operation control device for a legged mobile robot, comprising: If the impact force received by the aircraft from the floor is within a predetermined allowable value, the second landing site searching means performs the overturning operation of the aircraft, and if the impact force is out of the allowable value, the first landing site searching means performs the falling operation. Perform the overturning operation of the aircraft,
50. The motion control device for a legged mobile robot according to claim 49. An operation control method for a legged mobile robot having a movable leg and performing a legged work in a standing posture,
Calculating an impact moment applied to the aircraft at each stage of the fall;
Calculating the impact force that the aircraft receives from the floor at each stage when falling,
Calculating the area S of the supporting polygon formed by the ground contact point of the fuselage and the road surface;
A first landing site searching step of selecting a next landing site so that the area S of the supporting polygon is minimum or constant;
A second landing site searching step of selecting a next landing site so that the area S of the supporting polygon increases;
An operation control method for a legged mobile robot, comprising: If the impact force received from the floor by the aircraft is within a predetermined allowable value, the aircraft performs the overturning operation in the second landing site searching step, and if the impact force is out of the allowable value, the aircraft performs the first landing site searching step. Perform the overturning operation of the aircraft,
52. The operation control method for a legged mobile robot according to claim 51. An operation control device for controlling a series of operations relating to a falling and rising of an airframe in a legged mobile robot having a movable leg and performing a legged work in a standing posture, wherein the legged mobile robot has substantially parallel joint degrees of freedom. It consists of a link structure in which multiple joint axes are connected in the length direction,
At the time of a fall, in the posture on the floor where two or more links including the center of gravity link serving as the center of gravity of the fuselage touch the floor, the narrowest supporting polygon formed by the smallest number of links in the grounding polygon formed by the floor contact link is used. Means to search;
Means for setting a ZMP at a position where the number of links not involved in the minimum supporting polygon is the maximum and performing a tipping operation;
Means for searching for a link that can leave the floor in the falling posture of the aircraft;
Means for raising all the links that can get out of the bed and performing a rising motion;
An operation control device for a legged mobile robot, comprising: An operation control method for controlling a series of operations relating to a falling and rising of an airframe in a legged mobile robot having a movable leg and performing a legged work in a standing posture, wherein the legged mobile robot has substantially parallel joint degrees of freedom. It consists of a link structure in which multiple joint axes are connected in the length direction,
At the time of a fall, in the posture on the floor where two or more links including the center of gravity link serving as the center of gravity of the fuselage touch the floor, the narrowest supporting polygon formed by the smallest number of links in the grounding polygon formed by the floor contact link is used. Exploring;
Setting a ZMP at a site where the number of links not related to the minimum supporting polygon is the maximum and performing a tipping operation;
Searching for a link that can leave the bed in the falling posture of the aircraft;
Releasing all links that can be released from the floor and performing a rising motion;
An operation control method for a legged mobile robot, comprising: In a robot device having a trunk, a leg connected to the trunk, and an arm connected to the trunk,
A support polygon detection unit configured to detect a first support polygon formed by a plurality of ends where the leg, the trunk, and / or the arm touches the floor;
A support polygon changing means for reducing the area of the first support polygon by bending the leg in the trunk direction;
ZMP movement control means for determining whether or not the ZMP in the changed first support polygon can be moved to a grounded polygon formed by the sole of the leg, and
When the ZMP movement control means determines that the ZMP can be moved, the ZMP is moved from the falling posture to the basic posture while maintaining the ZMP from within the first supporting polygon within the ground contact polygon formed by the sole surface. And control means for causing the robot device to transition to the robot device. At least a torso, one or more arm links connected above the torso via a first joint (shoulder), and a first arm connected below the torso via a second joint (hip joint). A robot apparatus comprising: a leg link; and a second leg link connected to a distal end of the second leg link via a third joint (knee).
Means for contacting the tip of the arm link and the foot of the tip of the second leg link to form a first support polygon;
With the tip of the arm link and the foot resting on the floor, the second joint is moved above the third joint in the direction normal to the floor contact surface, and then the area of the first support polygon is reduced. Means for reducing and further moving the ZMP into a grounded polygon formed by said feet;
Means for erecting the fuselage while maintaining the ZMP within the grounding polygon formed by the feet;
A robot device comprising:

Images