以下、図面を参照しながら本発明の実施形態について詳解する。
A.脚式移動ロボットの機械的構成
図1及び図2には本発明の実施に供される「人間形」又は「人間型」の脚式移動ロボット100が直立している様子を前方及び後方の各々から眺望した様子を示している。図示の通り、脚式移動ロボット100は、胴体部と、頭部と、左右の上肢部と、脚式移動を行なう左右2足の下肢部とで構成され、例えば胴体に内蔵されている制御部(図示しない)により機体の動作を統括的にコントロールするようになっている。
左右各々の下肢は、大腿部と、膝関節と、脛部と、足首と、足平とで構成され、股関節によって体幹部の略最下端にて連結されている。また、左右各々の上肢は、上腕と、肘関節と、前腕とで構成され、肩関節によって体幹部の上方の左右各側縁にて連結されている。また、頭部は、首関節によって体幹部の略最上端中央に連結されている。
制御部は、この脚式移動ロボットを構成する各関節アクチュエータの駆動制御や各センサ(後述)などからの外部入力を処理するコントローラ(主制御部)や、電源回路その他の周辺機器類を搭載した筐体である。制御部は、その他、遠隔操作用の通信インターフェースや通信装置を含んでいてもよい。
このように構成された脚式移動ロボットは、制御部による全身協調的な動作制御により、2足歩行を実現することができる。かかる2足歩行は、一般に、以下に示す各動作期間に分割される歩行周期を繰り返すことによって行なわれる。すなわち、
(1)右脚を持ち上げた、左脚による単脚支持期
(2)右足が接地した両脚支持期
(3)左脚を持ち上げた、右脚による単脚支持期
(4)左足が接地した両脚支持期
脚式移動ロボット100における歩行制御は、あらかじめ下肢の目標軌道を計画し、上記の各期間において計画軌道の修正を行なうことによって実現される。すなわち、両脚支持期では、下肢軌道の修正を停止して、計画軌道に対する総修正量を用いて腰の高さを一定値で修正する。また、単脚支持期では、修正を受けた脚の足首と腰との相対位置関係を計画軌道に復帰させるように修正軌道を生成する。
歩行動作の軌道修正を始めとして、機体の姿勢安定制御には、一般に、ZMPに対する偏差を小さくするための位置、速度、及び加速度が連続となるように、5次多項式を用いた補間計算により行なう。ZMPを歩行の安定度判別の規範として用いている。ZMPによる安定度判別規範は、歩行系から路面には重力と慣性力、並びにこれらのモーメントが路面から歩行系への反作用としての床反力並びに床反力モーメントとバランスするという「ダランベールの原理」に基づく。力学的推論の帰結として、足底接地点と路面の形成する支持多角形(すなわちZMP安定領域)の内側にピッチ軸及びロール軸モーメントがゼロとなるZMPが存在する。
図3には、この脚式移動ロボット100が具備する関節自由度構成を模式的に示している。同図に示すように、脚式移動ロボット100は、2本の腕部と頭部1を含む上肢と、移動動作を実現する2本の脚部からなる下肢と、上肢と下肢とを連結する体幹部とで構成された、複数の肢を備えた構造体である。
頭部を支持する首関節(Neck)は、首関節ヨー軸1と、第1及び第2の首関節ピッチ軸2A及び2Bと、首関節ロール軸3という4自由度を有している。
また、各腕部は、その自由度として、肩(Shoulder)における肩関節ピッチ軸4と、肩関節ロール軸5と、上腕ヨー軸6、肘(Elbow)における肘関節ピッチ軸7と、手首(Wrist)における手首関節ヨー軸8と、手部とで構成される。手部は、実際には、複数本の指を含む多関節・多自由度構造体である。
また、体幹部(Trunk)は、体幹ピッチ軸9と、体幹ロール軸10という2自由度を有する。
また、下肢を構成する各々の脚部は、股関節(Hip)における股関節ヨー軸11と、股関節ピッチ軸12と、股関節ロール軸13と、膝(Knee)における膝関節ピッチ軸14と、足首(Ankle)における足首関節ピッチ軸15と、足首関節ロール軸16と、足部とで構成される。
但し、エンターティンメント向けの脚式移動ロボット100が上述したすべての自由度を装備しなければならない訳でも、あるいはこれに限定される訳でもない。設計・製作上の制約条件や要求仕様などに応じて、自由度すなわち関節数を適宜増減することができることは言うまでもない。
上述したような脚式移動ロボット100が持つ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、2足歩行という不安定構造体に対して姿勢制御を行なうことなどの要請から、アクチュエータは小型且つ軽量であることが好ましい。本実施形態では、ギア直結型で且つサーボ制御系をワンチップ化してモータ・ユニットに内蔵したタイプの小型ACサーボ・アクチュエータを搭載することとした(この種のACサーボ・アクチュエータに関しては、例えば本出願人に既に譲渡されている特開2000−299970号公報に開示されている)。本実施形態では、直結ギアとして低減速ギアを採用することにより、人間との物理的インタラクションを重視するタイプのロボットに求められている駆動系自身の受動的特性を得ている。
B.脚式移動ロボットの制御システム構成
図4には、脚式移動ロボット100の制御システム構成を模式的に示している。同図に示すように、脚式移動ロボット100は、ヒトの四肢を表現した各機構ユニット30,40,50R/L,60R/Lと、各機構ユニット間の協調動作を実現するための適応制御を行なう制御ユニット80とで構成される(但し、R及びLの各々は、右及び左の各々を示す接尾辞である。以下同様)。
脚式移動ロボット100全体の動作は、制御ユニット80によって統括的に制御される。制御ユニット80は、CPU(Central Processing Unit)やメモリ等の主要回路コンポーネント(図示しない)で構成される主制御部81と、電源回路やロボット100の各構成要素とのデータやコマンドの授受を行なうインターフェース(いずれも図示しない)などを含んだ周辺回路82とで構成される。
本発明を実現する上で、この制御ユニット80の設置場所は特に限定されない。図4では体幹部ユニット40に搭載されているが、頭部ユニット30に搭載してもよい。あるいは、脚式移動ロボット100外に制御ユニット80を配設して、脚式移動ロボット100の機体とは有線若しくは無線で交信するようにしてもよい。
図3に示した脚式移動ロボット100内の各関節自由度は、それぞれに対応するアクチュエータによって実現される。すなわち、頭部ユニット30には、首関節ヨー軸1、第1及び第2の首関節ピッチ軸2A及び2B、首関節ロール軸3の各々を表現する首関節ヨー軸アクチュエータA1、第1及び第2の首関節ピッチ軸アクチュエータA2A、A2B、首関節ロール軸アクチュエータA3がそれぞれ配設されている。
また、体幹部ユニット40には、体幹ピッチ軸9、体幹ロール軸10の各々を表現する体幹ピッチ軸アクチュエータA9、体幹ロール軸アクチュエータA10が配設されている。
また、腕部ユニット50R/Lは、上腕ユニット51R/Lと、肘関節ユニット52R/Lと、前腕ユニット53R/Lに細分化されるが、肩関節ピッチ軸4、肩関節ロール軸5、上腕ヨー軸6、肘関節ピッチ軸7、手首関節ヨー軸8の各々を表現する肩関節ピッチ軸アクチュエータA4、肩関節ロール軸アクチュエータA5、上腕ヨー軸アクチュエータA6、肘関節ピッチ軸アクチュエータA7、手首関節ヨー軸アクチュエータA8が配設されている。
また、脚部ユニット60R/Lは、大腿部ユニット61R/Lと、膝ユニット62R/Lと、脛部ユニット63R/Lに細分化されるが、股関節ヨー軸11、股関節ピッチ軸12、股関節ロール軸13、膝関節ピッチ軸14、足首関節ピッチ軸15、足首関節ロール軸16の各々を表現する股関節ヨー軸アクチュエータA11、股関節ピッチ軸アクチュエータA12、股関節ロール軸アクチュエータA13、膝関節ピッチ軸アクチュエータA14、足首関節ピッチ軸アクチュエータA15、足首関節ロール軸アクチュエータA16が配設されている。
各関節に用いられるアクチュエータA1、A2、A3…は、より好ましくは、ギア直結型で且つサーボ制御系をワンチップ化してモータ・ユニット内に搭載したタイプの小型ACサーボ・アクチュエータ(前述)で構成することができる。
頭部ユニット30、体幹部ユニット40、腕部ユニット50、各脚部ユニット60などの機構ユニット毎に、アクチュエータ駆動制御用の副制御部35、45、55、65が配設されている。
機体の体幹部40には、加速度センサ95と姿勢センサ96が配設されている。加速度センサ95は、X、Y、Z各軸方向に配置する。機体の腰部に加速度センサ95を配設することによって、質量操作量が大きな部位である腰部を制御目標点として設定して、その位置における姿勢や加速度を直接計測して、ZMPに基づく姿勢安定制御を行なうことができる。
また、各脚部60R及び60Lには、接地確認センサ91及び92と、加速度センサ93及び94がそれぞれ配設されている。接地確認センサ91及び92は、例えば足底に圧力センサを装着することにより構成され、床反力の有無により足底が着床したか否かを検出することができる。また、加速度センサ93及び94は、少なくともX及びYの各軸方向に配置する。左右の足部に加速度センサ93及び94を配設することにより、ZMP位置に最も近い足部で直接ZMP方程式を組み立てることができる。
質量操作量が大きな部位である腰部にのみ加速度センサを配置した場合、腰部のみが制御目標点に設定され、足部の状態は、この制御目標点の計算結果を基に相対的に算出しなければならず、足部と路面との間では以下の条件を満たすことが、前提となってしまう。
(1)路面はどんな力やトルクが作用しても動くことがない。
(2)路面での並進に対する摩擦係数は充分に大きく、滑りが生じない。
これに対し、本実施形態では、路面との接触部位である足部にZMPと力を直接する反力センサ・システム(床反力センサなど)を配設するとともに、制御に用いるローカル座標とその座標を直接的に計測するための加速度センサを配設する。この結果、ZMP位置に最も近い足部で直接ZMP方程式を組み立てることができ、上述したような前提条件に依存しない、より厳密な姿勢安定制御を高速で実現することができる。この結果、力やトルクが作用すると路面が動いてしまう砂利上や毛足の長い絨毯上や、並進の摩擦係数が充分に確保できずに滑りが生じ易い住居のタイルなどであっても、機体の安定歩行(運動)を保証することができる。
主制御部80は、各センサ91〜96の出力に応答して制御目標をダイナミックに補正することができる。より具体的には、副制御部35、45、55、65の各々に対して適応的な制御を行ない、脚式移動ロボット100の上肢、体幹、及び下肢が協調して駆動する全身運動パターンを実現する。
ロボット100の機体上での全身運動は、足部運動、ZMP軌道、体幹運動、上肢運動、腰部高さなどを設定するとともに、これらの設定内容に従った動作を指示するコマンドを各副制御部35、45、55、65に転送する。そして、各々の副制御部35、45…では、主制御部81からの受信コマンドを解釈して、各アクチュエータA1、A2、A3…に対して駆動制御信号を出力する。ここで言う「ZMP」とは、歩行中の床反力によるモーメントがゼロとなる床面上の点のことであり、また、「ZMP軌道」とは、例えばロボット100の歩行動作期間中にZMPが動く軌跡を意味する。
C.脚式移動ロボットの運動系基本状態遷移
本実施形態に係る脚式移動ロボット100の制御システムは、複数の基本姿勢を定義する。各々の基本姿勢は、機体の安定性や、消費エネルギ、次の状態への遷移を考慮して定義されており、基本姿勢間の遷移という形態により機体運動を効率的に制御することができる。
図5には、本実施形態に係る脚式移動ロボット100の運動系が持つ基本状態遷移を示している。同図に示すように、脚式移動ロボットは、基本仰向け姿勢、基本立ち姿勢、基本歩行姿勢、基本座り姿勢、基本うつ伏せ姿勢がそれぞれ仰向け時、立脚時、歩行準備時、着席時、及びうつ伏せ時における機体の安定性や、消費エネルギ、次の状態への遷移を考慮して定義されている。
これら基本姿勢は、機体の動作制御プログラムのプラットフォームに位置付けられる。また、脚式移動ロボットは、立ち姿勢などにおいて、歩行や跳躍、ダンスなど全身動作を利用した各種のパフォーマンスを行なうが、その装置制御プログラムは、プラットフォーム上で動作するアプリケーションとして位置付けられる。これらアプリケーション・プログラムは、外部記憶から随時ロードされ、主制御部81によって実行される。
図6には、脚式移動ロボット100の基本仰向け姿勢を示している。本実施形態では、機体への電源投入時には基本仰向け姿勢をとり、転倒などの心配がなく機械運動的に最も安定した状態からの起動を行なうことができる。また、脚式移動ロボットは、起動時だけでなくシステム動作の終了時も基本仰向け姿勢に復帰するようになっている。したがって、機械運動学的に機体が最も安定した状態で作業を開始するとともに、最も安定した状態で作業を終了することから、脚式移動ロボットの動作オペレーションは自己完結的となる。
勿論、機体の転倒時においても、床上での所定のモーションを経て一旦基本仰向け姿勢に戻った後に、規定の立ち上がり動作を実行することにより、基本立ち姿勢を介して、作業中断時の元の姿勢を回復することができる。
また、本実施形態に係る脚式移動ロボット100は、床上での基本姿勢として、基本仰向け姿勢の他に、図7に示したような基本うつ伏せ姿勢を備えている。この基本うつ伏せ姿勢は、基本仰向け姿勢と同様に、機械運動学的に機体が最も安定した状態であり、電源が遮断された脱力状態においても姿勢安定性を維持することができる。例えば、脚式作業において不測の外力などにより機体が転倒した場合、仰向け又はうつ伏せのいずれの状態で落下するか不明なので、本実施形態では、このように2通りの床上基本姿勢を規定している。
基本仰向け姿勢と基本うつ伏せ姿勢の間は、各種の床上姿勢を経て可逆的に遷移することができる。逆に言えば、これら基本仰向け姿勢と基本うつ伏せ姿勢を基準にして各種の床上姿勢へ円滑に状態遷移することができる。
基本仰向け姿勢は、機械運動学的には最も安定した基本姿勢であるが、脚式作業を考慮した場合、円滑な状態遷移を行なうことはできない。そこで、図8に示すような基本立ち姿勢が定義されている。基本立ち姿勢を定義することで、その後の脚式作業へ滞りなく移行することができる。
基本立ち姿勢は、立ち状態で最も安定した状態であり、姿勢安定制御のための計算機負荷や消費電力が最小又は極小となるような姿勢であり、膝を伸展させることにより直立状態を保つためのモータ・トルクを最小限に抑えている。この基本立ち姿勢から各種の立ち姿勢へ円滑に状態遷移して、たとえば上肢を利用したダンス・パフォーマンスなどを実演することができる。
他方、基本立ち姿勢は、姿勢安定性に優れているがこのまま歩行など脚式作業に移行するためには最適化されていない。そこで、本実施形態に係る脚式移動ロボットは、立脚状態の他の基本姿勢として、図9に示すような基本歩行姿勢を定義している。
基本立ち姿勢において、股関節、膝関節、並びに足首関節の各ピッチ軸12、14、15を駆動して、機体の重心位置を少し落とす格好にすることによって、基本歩行姿勢に遷移する。基本歩行姿勢では、通常の歩行動作を始めとして各種の脚式動作への遷移を円滑に行なうことができる。但し、膝を屈曲させた分だけ、この姿勢を維持するためのトルクが余分に必要とならことから、基本歩行姿勢は、基本立ち姿勢に比し消費電力は増大する。
基本立ち姿勢は、機体のZMP位置はZMP安定領域の中心付近にあり、膝の曲げ角が小さくエネルギ消費量が低い姿勢である。これに対し、基本歩行姿勢では、ZMP位置が安定領域の中心付近にあるが、高い路面適応性、高い外力適応性を確保するために膝の曲げ角を比較的大きくとっている。
また、本実施形態に係る脚式移動ロボット100では、さらに基本座り姿勢が定義されている。この基本座り姿勢(図示しない)では、所定の椅子に腰掛けたときに、姿勢安定制御のための計算機負荷や消費電力が最小又は極小となるような姿勢である。前述した、基本仰向け姿勢、基本うつ伏せ姿勢、並びに基本立ち姿勢からは、可逆的に基本姿勢へ遷移することができる。また、基本座り姿勢並びに基本立ち姿勢からは、各種の座り姿勢へと円滑に移行することができ、座り姿勢で例えば状態のみを用いた各種のパフォーマンスを実演することができる。
D.脚式移動ロボットの姿勢安定制御
次いで、本実施形態に係る脚式移動ロボット100における、脚式作業時における姿勢安定化処理、すなわち足部、腰、体幹、下肢運動などからなる全身協調運動実行時における姿勢の安定化処理の手順について説明する。
本実施形態に係る姿勢安定制御は、ZMPを姿勢安定制御に用いる。ZMPを安定度判別規範に用いたロボットの姿勢安定度制御は、基本的には足底接地点と路面の形成する支持多角形の辺上あるいはその内側にモーメントがゼロとなる点を探索することにある。すなわち、ロボットの機体に印加される各モーメントの釣合い関係を記述したZMP方程式を導出して、このZMP方程式上で現れるモーメント・エラーを打ち消すように機体の目標軌道を修正する。
本実施形態では、ロボットの機体上の制御目標点として質量操作量が最大となる部位、例えば腰部をローカル座標原点に設定する。そして、この制御目標点に加速度センサなどの計測手段を配置して、その位置における姿勢や加速度を直接計測して、ZMPに基づく姿勢安定制御を行なう。さらに路面との接触部位である足部に加速度センサを配設することにより、制御に用いるローカル座標とその座標を直接的に計測して、ZMP位置に最も近い足部で直接ZMP方程式を組み立てる。
D−1.ZMP方程式の導入
本実施形態に係る脚式移動ロボット100は、無限すなわち連続的な質点の集合体である。但し、ここでは有限数で離散的な質点からなる近似モデルに置き換えることによって、安定化処理のための計算量を削減するようにしている。より具体的には物理的には図3に示す多関節自由度構成を具備する脚式移動ロボット100を、図10に示すように多質点近似モデルに置き換えて取り扱う。図示の近似モデルは、線形且つ非干渉の多質点近似モデルである。
図10において、O−XYZ座標系は絶対座標系におけるロール、ピッチ、ヨー各軸を表し、また、O'−X'Y'Z'座標系はロボット100とともに動く運動座標系におけるロール、ピッチ、ヨー各軸を表している。但し、図中におけるパラメータの意味は以下の通りである。また、ダッシュ(´)付きの記号は運動座標系を記述するものと理解されたい。
同図に示す多質点モデルでは、iはi番目に与えられた質点を表す添え字であり、miはi番目の質点の質量、ri'はi番目の質点の位置ベクトル(但し運動座標系)を表すものとする。本実施形態に係る脚式移動ロボット100の機体重心は腰部付近に存在する。すなわち、腰部は、質量操作量が最大となる質点であり、図10では、その質量はmh、その位置ベクトル(但し運動座標系)はrh'(rhx ',rhy',rhz')とする。また、機体のZMPの位置ベクトル(但し運動座標系)をrzmp'(rzmpx',rzmpy',rzmpz')とする。
世界座標系O−XYZは絶対座標系であり、不変である。本実施形態に係る脚式移動ロボット100は、腰部と両脚の足部にそれぞれ加速度センサ93、94、96が配置されており、これらセンサ出力により腰部並びに立脚それぞれと世界座標系の相対位置ベクトルrqが直接検出される。これに対し、運動座標系すなわち機体のローカル座標系はO−X’Y’Z’は、ロボットともに動く。
多質点モデルは、言わば、ワイヤフレーム・モデルの形態でロボットを表現したものである。図10を見ても判るように、多質点近似モデルは、両肩、両肘、両手首、体幹、腰部、及び、両足首の各々を質点として設定される。図示の非厳密の多質点近似モデルにおいては、モーメント式は線形方程式の形式で記述され、該モーメント式はピッチ軸及びロール軸に関して干渉しない。多質点近似モデルは、概ね以下の処理手順により生成することができる。
(1)ロボット100全体の質量分布を求める。
(2)質点を設定する。質点の設定方法は、設計者のマニュアル入力であっても、所定の規則に従った自動生成のいずれでも構わない。
(3)各領域i毎に、重心を求め、その重心位置と質量miを該当する質点に付与する。
(4)各質点miを、質点位置riを中心とし、その質量に比例した半径に持つ球体として表示する。
(5)現実に連結関係のある質点すなわち球体同士を連結する。
なお、図10に示す多質点モデルの腰部情報における各回転角(θhx,θhy,θhz)は、脚式移動ロボット100における腰部の姿勢すなわちロール、ピッチ、ヨー軸の回転を規定するものである(図11には、多質点モデルの腰部周辺の拡大図を示しているので、確認されたい)。
機体のZMP方程式は、制御目標点において印加される各モーメントの釣合い関係を記述したものである。図5に示したように、機体を多数の質点miで表わし、これらを制御目標点とした場合、すべての制御目標点miにおいて印加されるモーメントの総和を求める式がZMP方程式である。
世界座標系(O−XYZ)で記述された機体のZMP方程式、並びに機体のローカル座標系(O−X’Y’Z’)はそれぞれ以下の通りとなる。
上式は、各質点miにおいて印加された加速度成分により生成されるZMP回り(半径ri−rzmp)のモーメントの総和と、各質点miに印加された外力モーメントMiの総和と、外力Fkにより生成されるZMP回り(k番目の外力Fkの作用点をskとする)のモーメントの総和が釣り合うということを記述している。
このZMP釣合い方程式は、総モーメント補償量すなわちモーメント・エラー成分Tを含んでいる。このモーメント・エラーをゼロ又は所定の許容範囲内に抑えることによって、機体の姿勢安定性が維持される。言い換えれば、モーメント・エラーをゼロ又は許容値以下となるように機体運動(足部運動や上半身の各部位の軌道)を修正することが、ZMPを安定度判別規範とした姿勢安定制御の本質である。
本実施形態では、腰部と左右の足部にそれぞれ加速度センサ96、93及び94が配設されているので、これらの制御目標点における加速度計測結果を用いて直接的に且つ高精度に上記のZMP釣合い方程式を導出することができる。この結果、高速でより厳密な姿勢安定制御を実現することができる。
D−2.全身協調型の姿勢安定制御
図12には、脚式移動ロボット100において、ZMPを安定度判別規範に用いて安定歩行可能な機体運動を生成するための処理手順をフローチャートの形式で示している。但し、以下の説明では、図10及び図11に示すような線形・非干渉多質点近似モデルを用いて脚式移動ロボット100の各関節位置や動作を記述するものとする。
まず、足部運動の設定を行なう(ステップS1)。足部運動は、2以上の機体のポーズを時系列的に連結されてなるモーション・データである。
モーション・データは、例えば、足部の各関節角の変位を表わした関節空間情報と、関節位置を表わしたデカルト空間情報で構成される。モーション・データは、コンソール画面上での手付け入力や、機体へのダイレクト・ティーチング(直接教示)例えばモーション編集用のオーサリング・システム上で構築したりすることができる。
次いで、設定された足部運動を基にZMP安定領域を算出する(ステップS2)。ZMPは、機体に印加されるモーメントがゼロとなる点であり、基本的には足底接地点と路面の形成する支持多角形の辺上あるいはその内側に存在する。ZMP安定領域は、この支持多角形のさらに内側に設定された領域であり、該領域にZMPを収容させることによって機体を高度に安定した状態にすることができる。
そして、足部運動とZMP安定領域を基に、足部運動中におけるZMP軌道を設定する(ステップS3)。
また、機体の上半身(股関節より上側)の各部位については、腰部、体幹部、上肢、頭部などのようにグループ設定する(ステップS11)。
そして、各部位グループ毎に希望軌道を設定する(ステップS12)。上半身の希望起動の設定は、足部の場合と同様に、コンソール画面上での手付け入力や、機体へのダイレクト・ティーチング(直接教示)例えばモーション編集用のオーサリング・システム上で構築したりすることができる。
次いで、各部位のグループ設定の調整(再グルーピング)を行ない(ステップS13)、さらにこれらグループに対して優先順位を与える(ステップS14)。ここで言う優先順位とは、機体の姿勢安定制御を行なうための処理演算に投入する順位のことであり、例えば質量操作量に応じて割り振られる。この結果、機体上半身についての各部位についての優先順位付き希望軌道群が出来上がる。
また、機体上半身の各部位グループ毎に、モーメント補償に利用できる質量を算出しておく(ステップS15)。
そして、足部運動とZMP軌道、並びに上半身の各部位グループ毎の希望起動群を基に、ステップS14により設定された優先順位に従って、各部位グループの運動パターンを姿勢安定化処理に投入する。
この姿勢安定化処理では、まず、処理変数iに初期値1を代入する(ステップS20)。そして、優先順位が先頭からi番目までの部位グループについての目標軌道設定時における、目標ZMP上でのモーメント量すなわち総モーメント補償量を算出する(ステップS21)。目標軌道が算出されていない部位については、希望軌道を用いる。
次いで、ステップS15において算出された当該部位のモーメント補償に利用できる質量を用いて、そのモーメント補償量を設定して(ステップS22)、モーメント補償量を算出する(ステップS23)。
次いで、算出されたi番目の部位のモーメント補償量を用いて、i番目の部位についてのZMP方程式を導出して(ステップS24)、当該部位のモーメント補償運動を算出することにより(ステップS25)、優先順位が先頭からi番目までの部位についての目標軌道を得ることができる。
このような処理をすべての部位グループについて行なうことにより、安定運動(例えば歩行)が可能な全身運動パターンが生成される。
腰部と左右の足部にそれぞれ加速度センサ96,93及び94が配設されているので、これらの制御目標点における加速度計測結果を用いて直接的に且つ高精度に上記のZMP釣合い方程式を導出することができる。この結果、図12に示すような処理手順に従ってZMP安定度判別規範に基づく姿勢安定制御を高速でより厳密に実行することができる。
E.脚式移動ロボットの転倒オペレーション
前項Dで説明したように、本実施形態に係る脚式移動ロボット100は、基本的には、ZMP安定度判別規範に基づいて、歩行時やその他の立脚作業時における姿勢安定制御を行ない、機体の転倒という事態の発生を最小限に抑えるようにしている。
しかしながら、万一転倒を避けられなくなった場合には、機体へのダメージを極力防止するような動作パターンからなる転倒動作を行なうことにする。例えば、前述したZMP釣合い方程式において、過大な外力F又は外力モーメントMが機体に印加された場合、機体動作のみによってモーメント・エラー成分Tをキャンセルことができなくなり、姿勢の安定性を維持できなくなる。
図13には、本実施形態に係る脚式移動ロボット100における脚式作業中の機体の動作を制御するための概略的な処理手順をフローチャートの形式で示している。
機体動作中は、左右の足部に配設した接地確認(床反力)センサ91及び92、加速度センサ93及び94、腰部に配設した加速度センサ96のセンサ出力を用いて、ZMP釣合い方程式(前述)を立てて、腰部、下肢軌道を常に計算する(ステップS31)。
例えば、機体に外力が印加されたとき、次の腰部、下肢軌道を計画することができるかどうか、すなわち足部の行動計画によって外力によるモーメント・エラーを解消することができるかどうかを判別する(ステップS32)。腰部、下肢軌道を計画することができるかどうかは、脚部の各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。勿論、外力が加わったときに、次の一歩だけでなく、数歩にまたがる脚式動作によりモーメント・エラーを解消するようにしてもよい。
このとき、足部の計画が可能であれば、歩行やその他の脚式動作を継続する(ステップS33)。
他方、過大な外力又は外力モーメントが機体に印加されたために、足部の計画が不可能になった場合には、脚式移動ロボット100は転倒動作を開始する(ステップS34)。
図1〜図2に示すような直立歩行型の脚式ロボットの場合、重心位置が高いことから、転倒時に不用意に床面に落下すると、ロボット自体、あるいは転倒により衝突する相手側にも致命的な損傷を与えてしまう危険がある。
そこで、本実施形態では、転倒前に計画されている機体の軌道からZMP支持多角形が最小となるような姿勢に組み替えて、所定の転倒動作を実行する。基本的には、以下に示す2つの方針を基に転倒動作を探索していく。
(1)機体の支持多角形の面積Sの時間t当たりの変化量ΔS/Δtを最小にする。
(2)床面落下時における支持多角形が最大となるようにする。
ここで、変化量ΔS/Δtを最小にするとは、転倒時の支持面積を維持する(あるいは減少させる)ことに相当する(但し、減少させる場合、駆動力が必要な場合がある)。機体の転倒時に支持面積を維持することで、機体に印加される衝撃モーメントを受け流すことができる。図14には、機体の転倒時に支持面積を維持する原理を図解している。同図に示すように、丁度球体が転がる具合で、支持面は面積最小であることを維持しながら、衝撃モーメントを受け流している。図示の通り、支持面が移動しても同様の効果が得られる。例えば、着床時に床から受ける衝撃力を求め、これが許容値を越えるような場合には、支持多角形の面積を一定に保つように機体が転がる転倒方法が好ましい。
また、床面落下時における支持多角形が最大となるとは、図15に示すように、より広い支持多角形で受け止めることにより衝撃力を干渉することに相当する。例えば、着床時に床から受ける衝撃力を求め、これが許容値以内となる場合には、支持多角形の面積を一定に保つように機体が転がる転倒方法が好ましい。
図16及び図17には、脚式移動ロボット100が後方すなわち仰向け姿勢に向かって転倒する場合に、支持多角形の変化量ΔS/Δtを最小すなわち転倒時の支持面積を維持する動作を実現した例を示している。これは、柔道やその他の格闘技における受身動作に類似する類似する動作であり、転倒時の衝撃力モーメントを好適に受け流すことができる。図17に示すように、足部を離床させることにより、支持多角形の変化量ΔS/Δtを最小にしている。機体の重心が腰部に存在する場合、最も小さくなる支持多角形に関与しないリンク数が最大となる部位にZMPを設定することができる。このような転倒・着床動作の後、離床可能なリンクをすべて離床させる、すなわち図示の例では下肢と体幹の双方を浮き上がらせて、上体と下肢を同時に離床し、足部、手部などを着床させることで、より小さい接地多角形を少ないステップで形成できるので、より高速で効率的な起き上がり動作を実現することができる。
図18及び図19には、脚式移動ロボット100が前方すなわちうつ伏せ姿勢に向かって転倒する場合に、支持多角形の変化量ΔS/Δtを最小すなわち転倒時の支持面積を維持する動作を実現した例をそれぞれ側面並びに右斜め前方から眺めた様子を示している。これは、機械体操などにおける前転動作に類似する類似する動作であり、転倒時の衝撃力モーメントを好適に受け流すことができる。各図に示すように、足部を離床させることにより、支持多角形の変化量ΔS/Δtを最小にしている。
上述したような転倒方法をとることにより、落下時に床面から受ける衝撃を全身に分散させることにより、ダメージを最小限に抑えることができる。
図20には、本実施形態に係る脚式移動ロボット100が足部の計画不能により転倒動作を行なうための処理手順をフローチャートの形式で示している。転倒動作は、上述した基本方針に従って、高さ方向に連結された肩関節ピッチ軸4、体幹ピッチ軸9、股関節ピッチ軸12、膝関節ピッチ軸14を同期協調的に駆動させることによって実現される。このような処理手順は、実際には主制御部81において所定の機体動作制御プログラムを実行して、各部を駆動制御することによって実現される。
まず、機体の支持多角形の面積Sの時間t当たりの変化量ΔS/Δtを最小にするリンクを探索する(ステップS41)。
次いで、ステップS41により選択されたリンクで変化量ΔS/Δtを最小にする該リンクの目標着床点を探索する(ステップS42)。機体の床面に対する支持面積を最小に維持することにより、衝撃モーメントを受け流すことができる(前述及び図14を参照のこと)。
次いで、先行ステップにより選択されたリンクを該目標着床点に着床することが、機体ハードウェアの制約上(各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など)、実行可能かどうかを、衝撃力モーメントを主に判別する(ステップS43)。
先行ステップにより選択されたリンクを該目標着床点に着床することが不可能であると判定された場合には、時間の変化量Δtを所定値だけ増分してから(ステップS44)、ステップS41に戻って、リンクの再選択、並びに該リンクの目標着床点の再設定を行なう。
一方、先行ステップにより選択されたリンクを該目標着床点に着床することが可能である場合には、選択されたリンクを該目標着床点に着床する(ステップS45)。
次いで、機体の位置エネルギが最小かどうか、すなわち転倒動作が完了したかどうかを判別する(ステップS46)。
機体の位置エネルギがまだ最小ではない場合には、時間の変化量Δtをさらに所定値だけ増分して(ステップS47)、支持多角形を拡大するように次の目標着床点を設定する(ステップS48)。支持多角形を拡大することにより、着床時に機体に加わる衝撃力を軽減することができる(前述及び図15を参照のこと)。
次いで、選択されたリンクを該目標着床点に着床することが、機体ハードウェアの制約上(各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など)、実行可能かどうかを、衝撃力を主に判別する(ステップS49)。 先行ステップにより選択されたリンクを該目標着床点に着床することが不可能であると判定された場合には、ステップS41に戻って、リンクの再選択、並びに該リンクの目標着床点の再設定を行なう。
一方、先行ステップにより選択されたリンクを該目標着床点に着床することが可能である場合には、ステップS45に進んで、選択されたリンクを該目標着床点に着床する。
そして、機体の位置エネルギが最小になると(ステップS46)、機体の床面への着床が完了したことになるので、本処理ルーチン全体を終了する。
次いで、実機動作を参照しながら、脚式移動ロボット100の転倒動作について説明する。
図21には、脚式移動ロボット100が肩関節ピッチ軸4、体幹ピッチ軸9、股関節ピッチ軸12、膝関節ピッチ軸14などの高さ方向に連結された略平行な複数の関節軸からなるリンク構造体としてモデル化して、各関節ピッチ軸を同期強調的に駆動させて仰向け姿勢に向かって転倒していく動作を示している。基本的に、離床リンク数が最大になるリンクがある部位を目標に設定することで、床面から受ける衝撃力を減少するようになっている。
ロボットは、リンク構造体のリンク端である足底のみで立位しているとする(図21(1))。
このとき、外力又は外力モーメントの印加により、ZMP釣合い方程式のモーメント・エラー項Tをキャンセルできなくなり、足底のみで形成するZMP安定領域の外にZMPが逸脱したことに応答して、ZMPを支持多角形に維持したまま転倒動作を開始する。
転倒動作では、まず、機体の支持多角形の面積Sの時間t当たりの変化量ΔS/Δtを最小にするリンクを探索するとともに、手先を含むリンクで変化量ΔS/Δtを最小にするような手先の目標着床点を探索する。そして、選択されたリンクを目標着床点に着床することが機体ハードウェアの制約上(各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など)、実行可能かどうかを判別する。
機体ハードウェア上実行可能である場合には、既に着床している足底リンクに加え、他のリンクが着床する。そして、これら着床リンクで形成される最小支持多角形内にZMPを移動する(図21(2))。
次いで、機体ハードウェアが許容する限り、着床点を移動して、支持多角形を拡大していく(図21(3))。
そして、各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など機体ハードウェアの制約から、もはや着床点を移動することができなくなると、今度は、着床中のリンクに挟まれている離床リンクを着床することができるかどうかを判別する。
機体ハードウェア上、着床リンク間の離床リンクを着床することが可能である場合には、これらを着床して、着床リンク数を増やす(図21(4))。
さらに、機体ハードウェアが許容する限り、着床点を移動して、支持多角形を拡大していく(図21(5))。
そして最後に、高さ方向に連結された略平行な複数の関節軸からなるリンク構造体の一端側から1以上のリンクと、他端がわから2以上のリンクを離床させて、且つ、それらの中間に位置するリンクを1以上着床させ、さらに足部を着床させた状態で、ZMPを支持多角形内に維持しながら、支持多角形が最大となる姿勢を形成する。この姿勢において、機体の位置エネルギが最小であれば、転倒動作は完了である。
図22〜図38、並びに図39〜図55には、実機が立位姿勢から仰向け姿勢に転倒していく様子を示している。
この場合、機体の支持多角形の面積Sの時間t当たりの変化量ΔS/Δtを最小にするリンクとして、股関節ピッチ軸を含む胴体リンクを選択するとともに、目標着床点を探索して、機体の後方に倒れ込む(図22〜図31、並びに図39〜図48を参照のこと)。膝関節を折り畳んだ姿勢にして、着床時の支持多角形の変化量を最小、すなわち、ΔS/Δtを最小にする。
次いで、機体の支持多角形の面積Sの時間t当たりの変化量ΔS/Δtを最小にするリンクとして、体幹ピッチ軸9と肩関節ピッチ軸4を含む胴体リンクを選択するとともに、その目標着床点を探索して、さらに機体の後方に深く倒れる。このとき、既に股関節ピッチ軸12が着床していることから、これを回転中心として体幹ピッチ軸9と肩関節ピッチ軸4を含む胴体リンクは着床する(図32〜図33、及び図49〜図50を参照のこと)。
次いで、機体の支持多角形の面積Sの時間t当たりの変化量ΔS/Δtを最小にするリンクとして、首関節ピッチ軸2で連結されている頭部リンクを選択するとともに、その目標着床点を探索して、さらに機体の後方に深く倒れる。このとき、既に首関節ピッチ軸2が着床していることから、これを回転中心として頭部は着床する(図34〜図38、及び図51〜図55を参照のこと)。この姿勢において、機体の位置エネルギが最小であるから、転倒動作は完了である。
また、図56には、脚式移動ロボット100が肩関節ピッチ軸4、体幹ピッチ軸9、股関節ピッチ軸12、膝関節ピッチ軸14などの高さ方向に連結された略平行な複数の関節軸からなるリンク構造体としてモデル化して、各関節ピッチ軸を同期強調的に駆動させてうつ伏せ姿勢に向かって転倒していく動作を示している。
ロボットは、リンク構造体のリンク端である足底のみで立位しているとする(図56(1))。
このとき、外力又は外力モーメントの印加により、ZMP釣合い方程式のモーメント・エラー項Tをキャンセルできなくなり、足底のみで形成するZMP安定領域の外にZMPが逸脱したことに応答して、ZMPを支持多角形に維持したまま転倒動作を開始する。
転倒動作では、まず、機体の支持多角形の面積Sの時間t当たりの変化量ΔS/Δtを最小にするリンクを探索するとともに、手先を含むリンクで変化量ΔS/Δtを最小にするような手先の目標着床点を探索する。そして、選択されたリンクを目標着床点に着床することが機体ハードウェアの制約上(各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など)、実行可能かどうかを判別する。
機体ハードウェア上実行可能である場合には、既に着床している足底リンクに加え、他のリンクが着床する。そして、これら着床リンクで形成される最小支持多角形内にZMPを移動する(図56(2))。
次いで、機体ハードウェアが許容する限り、着床点を移動して、支持多角形を拡大していく(図56(3))。
そして、各関節の可動角、各関節アクチュエータのトルク、関節力、角速度、角加速度など機体ハードウェアの制約から、もはや着床点を移動することができなくなると、今度は、着床中のリンクに挟まれている離床リンクを着床することができるかどうかを判別する。
機体ハードウェア上、着床リンク間の離床リンクを着床することが可能である場合には、これらを着床して、着床リンク数を増やす(図56(4))。
さらに、機体ハードウェアが許容する限り、着床点を移動して、支持多角形を拡大していく(図56(5))。
そして最後に、高さ方向に連結された略平行な複数の関節軸からなるリンク構造体の一端側から1以上のリンクと、他端がわから2以上のリンクを離床させて、且つ、それらの中間に位置するリンクを1以上着床させ、さらに足部を着床させた状態で、ZMPを支持多角形内に維持しながら、支持多角形が最大となる姿勢を形成する。この姿勢において、機体の位置エネルギが最小であれば、転倒動作は完了である。
図57〜図73、並びに図74〜図90には、実機が立位姿勢から仰向け姿勢に転倒していく様子を示している。
この場合、機体の支持多角形の面積Sの時間t当たりの変化量ΔS/Δtを最小にするリンクとして、肩関節ピッチ軸4を含む腕リンクの手先を選択するとともに、目標着床点を探索して、機体の前方に倒れ込む(図57〜図70、並びに図74〜図87を参照のこと)。
このとき、最短の時間増分Δtにおいて、着床時の支持多角形の変化量ΔSを最小にするために、膝関節ピッチ軸14を折り畳んだ姿勢にして、手先が着床する場所をより足底に近い位置に設定する。
次いで、機体の支持多角形の面積Sの時間t当たりの変化量ΔS/Δtを最小にするリンクとして、膝関節ピッチ軸14を含む脚部リンクを選択するとともに、その目標着床点を探索して、さらに機体の前方に深く倒れる。このとき、既に足部が着床していることから、足首ピッチ軸を回転中心として下腿部が旋回して、膝が着床する(図70〜図71、及び図88〜図89を参照のこと)。
さらに、着床点としての手先と膝を足底から離すように移動して、機体ハードウェアが許容する限り、支持多角形を拡大する(図72、及び図89を参照のこと)。この結果、手先と膝に続いて胴体リンクも着床する(図73、及び図90を参照のこと)。この姿勢において、機体の位置エネルギが最小であるから、転倒動作は完了である。
F.床上姿勢からの起き上がりオペレーション
仰向け姿勢やうつ伏せ姿勢などの床上姿勢からの起動を行なうため、あるいは、転倒時に自立的に起き上がって作業を再開するという作業の自己完結性のために、脚式移動ロボット100は、起き上がりオペレーションを実現することが必要である。
ところが、無計画的な軌道により起き上がろうとすると、過大な外力モーメントが印加されてしまい、関節アクチュエータが高出力トルクを必要とする。この結果、モータの大型化が必要となり、その分駆動消費電力が増大してしまう。また、機体の重量が増すとともに製造コストが高騰してしまう。重量の増大によりさらに起き上がり動作が困難になる。あるいは、起き上がり動作の過程で発生する外力モーメントにより姿勢の安定性を維持することができず、そもそも起き上がることができない、という事態もあり得る。
そこで、本実施形態では、脚式移動ロボット100は、外力モーメントが最小となる動作パターンよりなる起き上がり動作を行なうこととした。これは、ZMP支持多角形が最小となるような姿勢を時系列的に組み合わせることによって、実現することができる。
また、本実施形態に係る脚式移動ロボット100は、肩関節ピッチ軸4、体幹ピッチ軸9、股関節ピッチ軸12、膝関節ピッチ軸14のように(図3を参照のこと)、高さ方向に複数のピッチ軸が直列的(但し横方向から眺めた場合)に連結されたリンク構造体である。そこで、これら複数の関節ピッチ軸4〜14を所定のシーケンスで同期協調的に駆動して、ZMP支持多角形が最小となるような動作パターンによる起き上がり動作を実現することとした。
F−1.基本仰向け姿勢からの起き上がりオペレーション
図91には、本実施形態に係る脚式移動ロボット100が肩関節ピッチ軸4、体幹ピッチ軸9、股関節ピッチ軸12、膝関節ピッチ軸14を同期強調的に駆動させて起き上がり動作を行なうための処理手順をフローチャートの形式で示している。このような処理手順は、実際には主制御部81において所定の機体動作制御プログラムを実行して、各部を駆動制御することによって実現される。
また、図92には、本実施形態に係る脚式移動ロボット100が肩関節ピッチ軸4、体幹ピッチ軸9、股関節ピッチ軸12、膝関節ピッチ軸14を同期強調的に駆動させて仰向け姿勢から起き上がり動作を行なう様子を、関節リンク・モデルで示している。なお、本実施形態に係る脚式移動ロボット100は体幹ピッチ軸9を備えているが、体幹ピッチ軸を備えていないタイプの脚式移動ロボットにおいて複数の関節ピッチ軸の同期駆動により仰向け姿勢から起き上がり動作を行なう様子を図93に示しておく。但し、図示のリンク構造体において、体幹関節と股関節を連結するリンクに機体全体の重心位置が設定されており、このリンクを以下では「重心リンク」と呼ぶことにする。なお、「重心リンク」は狭義には上記のような定義で用いるが、広義には機体全体の重心位置が存在するリンクであればよい。例えば、体幹軸を持たないような機体においては、機体全体の重心が位置する体幹先端等を含むリンクがこれに該当する。
以下、図91に示したフローチャートを参照しながら、基本仰向け姿勢からの機体の起き上がりオペレーションについて説明する。
まず、床上姿勢において、位置エネルギの最も小さい姿勢を探索する(ステップS1)。これは、基本仰向け姿勢に相当し、図92(1)並びに図93(1)に示すように、起き上がり動作に使用する肩関節ピッチ軸4、体幹ピッチ軸9、股関節ピッチ軸12、膝関節ピッチ軸14をそれぞれ連結するリンクはすべて接床している。このときの実機の状態を図94及び図112に示している。位置エネルギの最も小さい姿勢をとることにより、路面の傾斜や形状を計測して、起き上がり動作が可能かどうかを確認することができる。
この基本仰向け姿勢において、接床リンクが形成する接地多角形内で、最も狭い支持多角形を探索する(ステップS52)。このとき、機体の一端側から少なくとも2以上のリンクを離床させたときの、ZMP軌道が計画可能かどうかを判定する。ZMPの計画可能性は、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断することができる。
次いで、接地多角形のうち、最も狭い支持多角形に関与しない2以上のリンクを離床する(ステップS53)。
ステップS53は、図92(2)及び図93(2)に相当する。実機上では、体幹関節と股関節を連結する重心リンクを含む下半身側が支持多角形として抽出され、それ以外の肩関節から体幹関節に至る2以上のリンクを支持多角形に関与しないリンクとして離床する。
このときの実機の動作を図95〜図96、並びに図113〜図114に示している。図示の例では、まず、左右の両腕部を持ち上げてから、体幹関節ピッチ軸アクチュエータA9の駆動により、上体起こしを行なっている。腕部を先に持ち上げておくことにより、モーメントを小さくして、必要な最大トルクを低減することができる。
次いで、一端側から1以上の離床リンクを屈曲させてリンク端の端部を着床させて、より狭い接地多角形を形成する(ステップS54)。
ステップS54は、図92(3)及び図93(3)に相当する。実機上では、肩関節を含む2以上のリンクが離床している状態で、肩関節ピッチ軸で屈曲させて、そのリンク端の端部である手先を接床させる。そして、手先を機体重心位置である体幹ピッチ軸側に徐々に近づけていくことによって、元の床上姿勢よりも狭い接地多角形を形成する。
このときの実機の動作を図97〜図101、並びに図115〜図119に示している。図示の例では、左右の肩関節ロール軸A5の駆動により、左右の腕部を真横に広げた後、上腕ヨー軸A6の駆動により腕部の向きを一旦180度回転させてから(図98〜図99、図116〜図117)、肩関節ピッチ軸A4の駆動により、腕部を徐々に降下させていく。そして、手先を着床することによって、より狭い接地多角形を形成する(図101及び図119)。
このように新しい接地多角形を形成すると、接地多角形にZMPを設定することができるかどうかをチェックする(ステップS55)。これは、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。そして、ZMPを接地多角形に移動して、新たな支持多角形を形成する(ステップS56)。
ここで、支持多角形が充分狭くなったか否かを判断する(ステップS57)。この判断は、体幹ピッチ軸と股関節ピッチ軸を連結する重心リンクを離床可能であるか、若しくは足部だけで形成できるZMP安定領域内にZMPを移動させることができるかどうかを、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。支持多角形が充分狭くなったかどうかを判断する詳細な手順については後述に譲る。
図101及び図119に示す実機の姿勢では、まだ支持多角形が充分狭いとは言えない。そこで、着床点を移動して支持多角形を小さくした後(ステップS50)、ステップS52に戻って、より狭い支持多角形の形成を再試行する。
図101及び図119に姿勢において、接床リンクが形成する接地多角形内で、最も狭い支持多角形を探索する(ステップS52)。今度は、機体の他端側から少なくとも2以上のリンクを離床させたときの、ZMPが計画可能かどうかを判定する。ZMPの計画可能性は、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断することができる。
次いで、接床多角形のうち、最も狭い支持多角形に関与しない2以上のリンクを離床する(ステップS53)。これは、図92(4)〜(5)及び図93(4)〜(5)に相当する。実機上では、膝関節ピッチ軸を含む他端側から連続する2以上のリンクを支持多角形に関与しないリンクとして離床する。
そして、一端側から1以上の離床リンクを屈曲させてリンク端の端部を着床させて、より狭い接地多角形を形成する(ステップS54)。
このときの実機の動作を図102〜図105、並びに図120〜図123に示している。図示の例では、まず、右脚の股関節ピッチ軸A12の駆動により右脚を持ち上げてから、その膝関節アクチュエータA14の駆動により右脚を屈曲させて、その足底を着床する。次いで、脚の股関節ピッチ軸A12の駆動により右脚を持ち上げてから、その膝関節アクチュエータA14の駆動により左脚を屈曲させて、その足底を着床する。このようにして、足底を機体重心位置である股関節ピッチ軸12側に徐々に近づけていくことによって、元の床上姿勢よりも狭い接地多角形を形成することができる。
このように新しい接地多角形を形成すると、接地多角形にZMPを設定することができるかどうかをチェックする(ステップS55)。これは、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。そして、ZMPを接地多角形に移動して、新たな支持多角形を形成する(ステップS56)。
ここで、支持多角形が充分狭くなったか否かを再び判断する(ステップS57)。この判断は、体幹ピッチ軸と股関節ピッチ軸を連結する重心リンクを離床可能であるか、若しくは足部だけで形成できるZMP安定領域内にZMPを移動させることができるかどうかを、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。図105及び図123に示す実機の姿勢では、充分狭い支持多角形が形成されていると判断される。支持多角形を縮小する際の腕の角度に関しては、肩の軸から床面方向に下ろした垂線と腕の中心軸がなす角度は、トルク量に基づく所定角度内であることが望ましい。
そして、機体の支持多角形が充分に狭くなったことに応答して、支持多角形の両リンク端の端部を接床した状態で前記重心リンクを離床し、両リンク端の着床リンクによって形成される支持多角形内にZMPを維持しながら、支持多角形を形成する両リンク端の端部の間隔を縮めて、ZMPを前記リンク構造体の他端側に移動させていく(ステップS58)。これは、図92(6)〜(7)、並びに図93(6)〜(7)に相当する。
実機上では、接地多角形の両リンク端の端部としての手先及び足底を接床した状態で体幹ピッチ軸及び股関節ピッチ軸を連結した前記重心リンクを離床し、さらに、手先及び足底の間隔を徐々に縮めていき、ZMPを足底に向かって移動させていく。また、このときの実機の動作を図106〜図109、並びに図124〜図127に示している。
そして、前記リンク構造体の他端から第2の所定数以下の接床リンクのみで形成される接地多角形内にZMPが突入したことに応答して、ZMPを該接地多角形内に収容したまま前記リンク構造体の一端側から第1の所定数以上のリンクを離床して、該離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる(ステップS59)。これは、図92(8)、並びに図93(8)に相当する。
実機上では、足底で構成される接地多角形内にZMPが突入したことに応答して、ZMPを該接地多角形内に収容したまま、肩ピッチ軸4から膝ピッチ軸14に至までのリンクを離床して、離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる。また、このときの実機の動作を図110〜図111、並びに図128〜図129に示している。
起き上がりの最終段階である、離床リンクを長さ方向に伸展する際には、質量操作量のより大きな膝関節ピッチ軸を積極的に使用して動作することが、機体動作上の効率がよい。
なお、ステップS53において、最も小さい支持多角形に関与しない2以上のリンクを離床することができない場合には、最大の支持多角形より内側の2以上の着床リンクを離床することを試みる(ステップS61)。
ステップS61を実行できない場合には、起き上がり動作を中止する(ステップS64)。また、ステップS61を成功裏に実行することができる場合には、さらに、着床点を移動させて、支持多角形をさらに小さくする(ステップS62)。
ステップS62を実行できない場合には、起き上がり動作を中止する(ステップS64)。また、ステップS62を成功裏に実行することができる場合には、足部で形成できる安定領域にZMPを移動することができるかどうかをチェックする(ステップS63)。支持多角形が充分狭くなったかどうかを判断する詳細な手順については後述に譲る。この安定領域内にZMPを移動することができない場合には、ステップS61に戻って、支持多角形を小さくするための同様の処理を繰り返し実行する。また、この安定領域内にZMPを移動させることができた場合には、ステップS58に進んで、基本姿勢への復帰動作を行なう。
ところで、ステップS53〜S54において、左右の手先を胴体後方で着床してより狭い接地多角形を形成するために、図97〜図98並びに図115〜図116に示すように、肩ロール軸を用いて左右の腕部を真横に広げるという動作を経ている。これは、脚式移動ロボット100が起き上がり作業を行なうための使用体積をいたずらに増大させてしまっている。そこで、図96〜図101並びに図113〜図119に示す一連の動作を、肩ロール軸を動作させず、代わりに肘ピッチ軸を屈曲させるという図130及び図131に示す動作に置き換えて、より小さな使用体積で左右の手先を胴体後方で着床するようにしてもよい。
上述した起き上がり動作手順では、ステップS57及びS63において、支持多角形が充分狭くなったかどうかを判断する必要がある。図173には、支持多角形が充分狭くなったかどうかを判断するための処理手順をフローチャートの形式で示している。
まず、ZMP偏差ε(εx,εy,εz)、すなわち足部が形成できる安定領域の中心位置(x0,y0,z0)と現在のZMP位置(x,y,z)との差分を求める(ステップS71)。
次いで、このZMP偏差ε(εx,εy,εz)に所定のゲインG(Gx,Gy,Gz)を掛算したものを現在の腰の位置r(rhx(t),rhy(t),rhz(t))に加えて、次の時刻t=t+Δtにおける目標腰位置r(rhx(t+Δt),rhy(t+Δt),rhz(t+Δt))(=r(rhx(t),rhy(t),rhz(t))+G(Gx,Gy,Gz)×ε(εx,εy,εz))にする(ステップS72)。
そして、次の目標腰位置で現在の支持多角形を形成することができるかどうかを判別する(ステップS73)。この判別は、着床リンクの着床点を維持しながら、次の目標腰位置を計算することによって行なわれる。すなわち、腰位置と着床点から逆運動学計算を行ない、可動角度以内で且つ関節アクチュエータの許容トルク以内であれば実現可能と判断される。
次の目標腰位置で現在の支持多角形を形成することができなければ、足部が形成できる安定領域内にZMPを移動することが不可能であるとして、本処理ルーチン全体を終了する。
他方、次の目標腰位置で現在の支持多角形を形成することができるならば、さらに、次の目標腰位置に腰を移動した場合の(すなわち次の)ZMPを算出する(ステップS74)。
次いで、足部が形成できる安定領域内にZMPが存在するかどうかを判別する(ステップS75)。判別結果が肯定的であれば、足部が形成できる安定領域内にZMPを移動することができると判断して(ステップS76)、本処理ルーチン全体を終了する。他方、判別結果が否定的であれば、次の腰位置を現在の腰位置に、次のZMPを現在のZMPにした後、ステップS71に戻って同様の処理を繰り返し実行する。
なお、図130及び図131に示す動作例では、上腕の長さをl1、前腕の長さをl2、肩ロール角をα、肘ピッチ角をベータ、肩から手先までの長さをl12、肩から手先を結ぶ線のなす角をγ、肩の高さをhと置くと(図132)、左右の手先を胴体後方で着床する動作期間中は、以下の式を満たすように肘ピッチ軸7を動作させることにより、手先が床面と衝突することはない。
また、図92に示す起き上がり動作パターンは、脚式移動ロボットの機体が肩関節ピッチ軸、体幹ピッチ軸、股関節ピッチ軸、膝ピッチ軸が機体の高さ方向に連結されてなるリンク構造体にモデル化して起き上がり動作を示している。図133には、脚式移動ロボットを略平行な関節自由度を持つ複数の関節軸を長さ方向に連結したリンク構造体に一般化して、起き上がり動作を示している。
同図に示すリンク構造体は、略平行な関節自由度を持つ複数の関節軸を長さ方向に連結して構成される。すべてのリンクが接床している床上姿勢からの起き上がり動作を、リンクA、リンクB、リンクC、リンクD、リンクE、並びにリンクFを用いて実現する。
但し、リンクA〜Fは、それぞれ単一のリンクである必要はなく、実際には複数のリンクが関節軸を介して連結されているが、起き上がり動作の期間中は関節軸が作動せずリンク間の真直性が保たれて、あたかも単一のリンクであるように振る舞う場合も含むものとする。例えば、リンクAはリンク端からh番目までのリンクを含み、リンクBはh番目以降i番目までのリンクを含み、リンクCは、i番目以降j番目までのリンクを含み、リンクDはj番目以降k番目までのリンクを含み、リンクEはk番目以降l番目までのリンクを含み、リンクFはl番目以降m番目(若しくはリンクの他端)までのリンクを含んである。
まず、F番目リンクとA番目リンクの間に接地多角形を形成して、この接地多角形内にZMPを設定する(図133(1))。
次いで、E番目リンクとA番目リンクの間の接地多角形内にZMPを設定する(図133(2))。このとき、リンク端から2以上のリンクを離床させるなどF番目リンクの運動を用いてもよい。
次いで、F番目リンクとA番目リンクの間により狭い接地多角形を新たに形成して、この接地多角形内にZMPを設定する(図133(3))。例えば離床中のF番目リンクを屈曲させてその端部を着床させて、新しい接地多角形を形成する。
次いで、F番目リンクとD番目またはC番目リンクの間で接地多角形を新たに形成して、この接地多角形内にZMPを設定する(図133(4))。このとき、他方のリンク端から2以上のリンクを離床させるなどA番目リンクの運動を用いてもよい。
次いで、D番目リンクを接地させて、F番目リンク及びA番目リンクで接地多角形を新たに形成して、この接地多角形内にZMPを設定する(図133(5))。例えば離床中のA番目リンクを屈曲させてその端部を着床させて、新しい接地多角形を形成する。
次いで、F番目リンクとA番目リンクで接地多角形を新たに形成して、この接地多角形内にZMPを設定する(図133(6))。例えば、両方のリンク端の端点を着床したままで、着床中のD番目リンクを離床させる。
次いで、両方のリンク端F及びAの端点を一致させることにより、A番目のリンクのみが形成する支持多角形内にZMPを移動させる(図133(7))。
そして最後に、A番目リンクのみが形成する支持多角形内にZMPを設定しながら、各リンクを基本立ち姿勢へ移動させる(図133(8))。
F−2.基本うつ伏せ姿勢からの起き上がりオペレーション
図134には、本実施形態に係る脚式移動ロボット100が肩関節ピッチ軸4、体幹ピッチ軸9、股関節ピッチ軸12、膝関節ピッチ軸14を同期強調的に駆動させて起き上がり動作を行なう様子を、関節リンク・モデルで示している。
本実施形態に係る脚式移動ロボット100は、基本的には、仰向け姿勢から起き上がる場合と同様に、図91にフローチャートの形式で示した処理手順に従って、うつ伏せ姿勢からも起き上がることができる。以下、図91に示したフローチャートを参照しながら、基本うつ伏せ姿勢からの機体の起き上がりオペレーションについて説明する。
まず、床上姿勢において、位置エネルギの最も小さい姿勢をとる(ステップS51)。これは、基本うつ伏せ姿勢に相当し、図134(1)に示すように、起き上がり動作に使用する肩関節ピッチ軸4、体幹ピッチ軸9、股関節ピッチ軸12、膝関節ピッチ軸14をそれぞれ連結するリンクはすべて接床している。このときの実機の状態を図135及び図154に示している。
この基本うつ伏せ姿勢において、接床リンクが形成する接地多角形内で、最も狭い支持多角形を探索する(ステップS52)。このとき、機体の一端側から少なくとも2以上のリンクを離床させたときの、ZMPが計画可能かどうかを判定する。ZMPの計画可能性は、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断することができる。
次いで、接地多角形のうち、最も狭い支持多角形に関与しない2以上のリンクを離床する(ステップS53)。ステップS53は、図134(2)に相当する。実機上では、体幹関節と股関節を連結する重心リンクを含む下半身側が支持多角形として抽出され、それ以外の肩関節から体幹関節に至る2以上のリンクを支持多角形に関与しないリンクとして離床する。
このときの実機の動作を図136〜図144、並びに図155〜図163に示している。図示の例では、まず、左右の両腕部の肩ロール軸アクチュエータA5を作動させて、床面に摺って肩ロール軸回りに略90度だけ旋回させて(図136〜図137、並びに図155〜図156)、次いで、上腕ヨー軸アクチュエータA6を作動させて、各腕部を上腕ヨー軸回りに略180度だけ回転させる(図138並びに図157)。そして、さらに肩ロール軸アクチュエータA5を作動させて、摺って肩ロール軸回りに略90度だけ旋回させて、各腕部を頭部の側面まで移動する(図138〜図141、並びに図157〜図160)。
図136〜図141、並びに図165〜図170に示す一連の動作では、左右の腕部は床面上で半円を描く格好となっている。このとき、機体周辺の路面において障害物の有無を検出したりして、起き上がり動作に必要な安全な作業領域の確保を行なうことができる。
次いで、一端側から1以上の離床リンクを屈曲させてリンク端の端部を着床させて、より狭い接地多角形を形成する(ステップS54)。ステップS54は、図134(3)に相当する。
そして、新しい接地多角形を形成すると、接地多角形にZMPを設定することができるかどうかをチェックする(ステップS55)。これは、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。そして、ZMPを接地多角形に移動して、新たな支持多角形を形成する(ステップS56)。
実機上では、肘ピッチ軸7を固定させて、左右の腕部を真直ぐ伸ばしたままの状態で、今度は肩ピッチ軸アクチュエータA4、体幹ピッチ軸アクチュエータA9、股関節ピッチ軸A12、並びに膝関節ピッチ軸アクチュエータA14を作動させて、手先と左右の両膝が接地した閉リンク姿勢からなる支持多角形を形成する(図142〜図144、並びに図161〜図163)。
図144及び図153に示す実機の姿勢では、まだ支持多角形が充分狭いとは言えない。そこで、着床点を移動して支持多角形を小さくする(ステップS60)。支持多角形を縮小する際の腕の角度に関しては、肩の軸から床面方向に下ろした垂線と腕の中心軸がなす角度は、トルク量に基づく所定角度内であることが望ましい。
実機上では、左右の腕部をまっすぐに保ったまま、手先を他方の着床点である足底側に徐々に近づけていくことによって、より狭い支持多角形を形成していく(図145〜図148、並びに図164〜図167)。
ここで、支持多角形が充分狭くなったか否かを判断する(ステップS57)。この判断は、体幹ピッチ軸と股関節ピッチ軸を連結する重心リンクを離床可能であるか、若しくは足部だけで形成できるZMP安定領域内にZMPを移動させることができるかどうかを、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。図148及び図165に示す実機の姿勢では、充分狭い支持多角形が形成されていると判断される。
そして、機体の支持多角形が充分に狭くなったことに応答して両リンク端の着床リンクによって形成される支持多角形内にZMPを維持しながら、支持多角形を形成する両リンク端の端部の間隔を縮めて、ZMPを前記リンク構造体の他端側に移動させていく(ステップS58)。これは、図134(6)〜(7)に相当する。
実機上では、接地多角形の両リンク端の端部としての手先及び足底を接床した状態で、さらに手先及び足底の間隔を徐々に縮めていき、ZMPを足底に向かって移動させていく。また、このときの実機の動作を図149〜図150、並びに図168〜図169に示している。
そして、前記リンク構造体の他端から第2の所定数以下の接床リンクのみで形成される接地多角形内にZMPが突入したことに応答して、ZMPを該接地多角形内に収容したまま前記リンク構造体の一端側から第1の所定数以上のリンクを離床して、該離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる(ステップS59)。これは、図134(8)に相当する。
実機上では、足底で構成される接地多角形内にZMPが突入したことに応答して、ZMPを該接地多角形内に収容したまま、肩ピッチ軸4から膝ピッチ軸14に至までのリンクを離床して、離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる。また、このときの実機の動作を図151〜図153、並びに図170〜図172に示している。
起き上がりの最終段階である、離床リンクを長さ方向に伸展する際には、質量操作量のより大きな膝関節ピッチ軸を積極的に使用して動作することが、機体動作上の効率がよい。
F−3.他の起き上がりオペレーションの例
図91で示した起き上がりオペレーションでは、ZMP支持多角形が最小となるような姿勢を時系列的に組み合わせることによって、外力モーメントが最小となる動作パターンよりなる起き上がり動作を行なうこととした。この動作では、より小さな支持多角形を順次形成していく過程において、手部や足部の踏み替え動作を利用していた。しかしながら、踏み替え動作を実現するためには、手部又は足部が離床する必要があり、支持多角形に関与しない2以上のリンクがなければならず、機体の姿勢によっては踏み替え動作を行なえない場合があり、この場合は起き上がり動作そのものが破綻してしまう(図91のステップS64)。
これに対し、より小さな支持多角形を順次形成していく過程において、手部や足部の踏み替え動作を実現できない場合には、手部や足部の引き摺り動作を利用することにより、起き上がり動作が破綻してしまう機会を少なくすることができる。以下では、より小さな支持多角形を順次形成していく過程において、手部や足部の踏み替え動作と引き摺り動作を利用した起き上がりオペレーションについて説明する。
図174には、手部や足部の踏み替え動作と引き摺り動作を利用した起き上がりオペレーションをフローチャートの形式で示している。以下、この起き上がり動作手順について説明する。図175〜図191には、基本うつ伏せ姿勢から手部又は足部の踏み替え動作又は引き摺り動作を利用しながら機体が起き上がりを行なう様子を順に示している。以下では、各図を適宜参照する。
まず、床上姿勢において、位置エネルギの最も小さい姿勢をとる(ステップS81)。これは、基本うつ伏せ姿勢に相当し、このときの実機の状態を図175に示している。
但し、転倒動作と連続して起き上がり行なう場合は、ステップS81を省略することにより、短時間で起き上がり動作を完了させることができる(後述)。
この基本うつ伏せ姿勢において、接床リンクが形成する接地多角形内で、最も狭い支持多角形を探索する(ステップS82)。このとき、機体の一端側から少なくとも2以上のリンクを離床させたときの、ZMPが計画可能かどうかを判定する。ZMPの計画可能性は、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断することができる。
ここで、最も小さい支持多角形に関与しないリンクの2つ以上を離床することができるかどうかを判断する(ステップS83)。最小の支持多角形に関与しない2以上のリンクを離床することができる場合には、次ステップS84へ進み、手部又は足部の踏み替え動作によるより小さい接地多角形の形成を行なう。一方、離床することができない場合には、ステップS91へ進み、手部又は足部の引き摺り動作を利用してより小さい接地多角形の形成を行なう。
ステップS84では、最も小さい支持多角形に関与しないリンクの2つ以上を離床させ、さらに、離床リンクを屈曲及び着床させて、より小さい接地多角形を形成する(ステップS85)。
例えば、図179〜図181、並びに図184〜図186において、両手両足を接地して起き上がり途上のロボットが、左足と右足を踏み替えながら、図175、図182〜図183、図185、図187に示すように離床リンクを屈曲及び着床させて、より小さい接地多角形の形成を試みている。
そして、新しい接地多角形を形成すると、接地多角形にZMPを設定することができるかどうかをチェックする(ステップS86)。これは、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。そして、ZMPを接地多角形に移動して、新たな支持多角形を形成する(ステップS87)。接地多角形にZMPを設定することができない場合には、ステップS83に戻り、手部や足部の踏み替え動作又は引き摺り動作のいずれを実行すべきかを改めてチェックする。
ここで、支持多角形が充分狭くなったか否かを判断する(ステップS88)。この判断は、体幹ピッチ軸と股関節ピッチ軸を連結する重心リンクを離床可能であるか、若しくは足部だけで形成できるZMP安定領域内にZMPを移動させることができるかどうかを、リンク構造体の可動角、リンクを接続する各関節アクチュエータのトルク、関節力、角速度、角加速度などを考慮して判断する。
そして、機体の支持多角形が充分に狭くなったことに応答して両リンク端の着床リンクによって形成される支持多角形内にZMPを維持しながら、支持多角形を形成する両リンク端の端部の間隔を縮めて、ZMPを前記リンク構造体の他端側に移動させていく(ステップS89)。
実機上では、接地多角形の両リンク端の端部としての手先及び足底を接床した状態で、さらに手先及び足底の間隔を徐々に縮めていき、ZMPを足底に向かって移動させていく。また、このときの実機の動作を図188〜図189に示している。
そして、前記リンク構造体の他端から第2の所定数以下の接床リンクのみで形成される接地多角形内にZMPが突入したことに応答して、ZMPを該接地多角形内に収容したまま前記リンク構造体の一端側から第1の所定数以上のリンクを離床して、該離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる(ステップS90)。
実機上では、足底で構成される接地多角形内にZMPが突入したことに応答して、ZMPを該接地多角形内に収容したまま、肩ピッチ軸4から膝ピッチ軸14に至までのリンクを離床して、離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる。また、このときの実機の動作を図190〜図191に示している。
起き上がりの最終段階である、離床リンクを長さ方向に伸展する際には、質量操作量のより大きな膝関節ピッチ軸を積極的に使用して動作することが、機体動作上の効率がよい。
一方、ステップS83において、最も小さい支持多角形に関与しないリンクの2つ以上を離床することができないと判断された場合には、手部又は足部の引き摺り動作を行なうべく、最大の支持多角形より内の着床中のリンクを2つ以上離床することができるかどうかをチェックする(ステップS91)。
ここで、最大の支持多角形より内の着床中のリンクを2つ以上離床することができない場合には、さらに着床点を移動させて支持多角形を小さくすることができるかどうかを判断する。支持多角形を小さくすることができない場合には、起き上がり動作を中止する(ステップS95)。すなわち、起き上がり動作は破綻する。
一方、最大の支持多角形より内の着床中のリンクを2つ以上離床することができる場合には、最大の支持多角形より内の着床中のリンクを2つ以上離床して(ステップS92)、手部又は足部の引き摺り動作を利用して、着床点を移動させ、支持多角形を小さくする(ステップS93)。
例えば、図176〜図178、並びに図187〜図188に示すように、両手両足を接地して起き上がり途上のロボットが、両手を着床させたまま足に向かって引き摺ることにより支持多角形を徐々に小さくしていく。
その後、支持多角形が充分狭くなったか否かを判断する(ステップS88)。そして、機体の支持多角形が充分に狭くなったことに応答して両リンク端の着床リンクによって形成される支持多角形内にZMPを維持しながら、支持多角形を形成する両リンク端の端部の間隔を縮めて、ZMPを前記リンク構造体の他端側に移動させていく(ステップS89)。
そして、前記リンク構造体の他端から第2の所定数以下の接床リンクのみで形成される接地多角形内にZMPが突入したことに応答して、ZMPを該接地多角形内に収容したまま前記リンク構造体の一端側から第1の所定数以上のリンクを離床して、該離床リンクを長さ方向に伸展することによって、起き上がり動作を完結させる(ステップS90)。
図199には、ステップS83において、最も小さい支持多角形に関与しないリンク数が最大となるリンクとその部位を探索するための詳細な処理手順をフローチャートの形式で示している。
まず、ステップS101及びS102において、変数i、j並びに配列型変数Mを初期化する。次いで、i番目のリンクのj番目の部位にZMPを設定する(ステップS103)。
ここで、ZMP空間が安定かどうかを判別する(ステップS104)。ZMP空間が安定である場合には、最も小さい支持多角形に関与しないリンク数を計算して(ステップS105)、i番目のリンクのj番目の部位における離床叶リンク数をLに代入する。そして、LがMよりも大きければ(ステップS106)、M(A,B)にL(i,j)を代入する(ステップS107)。
一方、ZMP空間が安定でない場合、LがMよりも大きくない場合、あるいはM(A,B)にL(i,j)を代入した後、jを1だけ増分して(ステップS108)、jが総部位数Jを越えたかどうかを判別する(ステップS109)。jがまだ総部位数Jに達していない場合には、ステップS103に戻って、上述と同様の処理を繰り返し実行する。
次いで、iを1だけ増分して(ステップS110)、iが総リンク数Iを越えたかどうかを判別する(ステップS111)。iが総リンク数に達していない場合には、ステップS102に戻って、上述と同様の処理を繰り返し実行する。
iが総リンク数Iを越えた場合には、Aにリンク、Bに部位を代入し、本処理ルーチンを終了する。
前述したように、転倒動作と連続して起き上がり行なう場合は、ステップS81を省略することにより、短時間で起き上がり動作を完了させることができる。
例えば、機体の重心が腰部に存在する場合、最も小さくなる支持多角形に関与しないリンク数が最大となる部位にZMPを設定することができる。このような転倒・着床動作の後、離床可能なリンクをすべて離床させる、すなわち下肢と体幹の双方を浮き上がらせて、上体と下肢を同時に離床し、足部、手部などを着床させることで、より小さい接地多角形を少ないステップで形成することができるので、より高速で効率的な起き上がり動作を実現することができる。
図192〜図198には、転倒動作と連続して起き上がり動作を行なう場合の機体の一連の動作を示している。
図192に示す立位姿勢から、図192〜図193に示すように機体後方に向かって転倒動作を開始し、図194に示すように機体重心が存在する腰部において着床する。
図194に示す例では、最も小さい支持多角形に関与しないリンク数が最大になる胴体部にZMPが設定されている。また、特徴的なことは、図23〜図38、及び図39〜図55を参照しながら説明した例とは相違し、基本仰向けではなく、脚部が離床した状態で転倒動作が終了している点にある。
続く起き上がり動作では、図195に示すように、離床可能なリンクすなわち脚部と胴体部をすべて離床させて、起き上がり動作を開始する。ここで、股関節及び/又は体幹のピッチ軸アクチュエータの駆動により、図196〜図197に示すように上体が起き上がる。そして、右脚の股関節ピッチ軸A12の駆動により右脚を持ち上げてから、その膝関節アクチュエータA14の駆動により右脚を屈曲させて、その足底を着床する。次いで、脚の股関節ピッチ軸A12の駆動により右脚を持ち上げてから、その膝関節アクチュエータA14の駆動により左脚を屈曲させて、その足底を着床する。このようにして、足底を機体重心位置である股関節ピッチ軸12側に徐々に近づけていくことによって、図198に示すように、元の床上姿勢よりも狭い接地多角形を形成することができる。
転倒動作と連続して起き上がり動作を行なう場合、図23〜図38、及び図39〜図55を参照しながら説明した例に比べて、より小さい接地多角形を少ないステップで形成することができる。すなわち、この実施形態によればより効率的に狭い接地多角形を形成することができる、起き上がり動作が高速化されるという点を充分理解されたい。