以下、添付図面を参照してこの発明の実施形態に係る脚式移動ロボットの歩容生成装置を説明する。尚、脚式移動ロボットとしては2足移動ロボットを例にとる。
図1は、この実施形態に係る脚式移動ロボットとしての2足移動ロボットを全体的に示す概略図である。
図示の如く、2足移動ロボット(以下、ロボットという)1は上体(ロボット1の基体)3から下方に延設された左右一対の脚体(脚部リンク)2,2を備える。両脚体2,2は同一構造であり、それぞれ6個の関節を備える。その6個の関節は上体3側から順に、股(腰部)の回旋(回転)用(上体3に対するヨー方向の回転用)の関節10R,10L(符号R,Lはそれぞれ右側脚体、左側脚体に対応するものであることを意味する符号である。以下同じ)と、股(腰部)のロール方向(X軸まわり)の回転用の関節12R,12Lと、股(腰部)のピッチ方向(Y軸まわり)の回転用の関節14R,14Lと、膝部のピッチ方向の回転用の関節16R,16Lと、足首のピッチ方向の回転用の関節18R,18Lと、足首のロール方向の回転用の関節20R,20Lとから構成される。
各脚体2の足首の2つの関節18R(L),20R(L)の下部には、各脚体2の先端部を構成する足平(足部)22R(L)が取着されると共に、両脚体2,2の最上位には、各脚体2の股の3つの関節10R(L),12R(L),14R(L)を介して前記上体3が取り付けられている。上体3の内部には、詳細を後述する制御ユニット60などが格納される。なお、図1では図示の便宜上、制御ユニット60を上体3の外部に記載している。
上記構成の各脚体2においては、股関節(あるいは腰関節)は関節10R(L),12R(L),14R(L)から構成され、膝関節は関節16R(L)から構成され、足首関節は関節18R(L),20R(L)から構成される。また股関節と膝関節とは大腿リンク24R(L)で連結され、膝関節と足首関節とは下腿リンク26R(L)で連結される。
上体3の上部の両側部には左右一対の腕体5,5が取り付けられると共に、上体3の上端部には頭部4が配置される。各腕体5は、3つの関節30R(L),32R(L),34R(L)から構成された肩関節と、関節36R(L)から構成された肘関節と、関節38R(L)から構成された手首関節と、この手首関節に連結された手先部40R(L)とを備えている。なお、頭部4は、本発明の要旨と直接的な関連を有しないため詳細な説明を省略する。
上記の構成により、各脚体2の足平22R(L)は、上体3に対して6つの自由度を与えられている。そして、ロボット1の歩行等の移動中に、両脚体2,2を合わせて6*2=12個(この明細書で「*」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す)の関節を適宜な角度で駆動することで、両足平22R,22Lの所望の運動を行うことができる。これにより、ロボット1は任意に3次元空間を移動することができる。また、各腕体5は、その肩関節、肘関節、手首関節の回転によって、腕振り等の運動を行うことができる。
図1に示す如く、各脚体2の足首関節18R(L),20R(L)の下方には足平22R(L)との間に公知の6軸力センサ50が介装されている。該6軸力センサ50は、各脚体2の足平22R(L)の着地の有無、および各脚体2に作用する床反力(接地荷重)等を検出するためのものであり、該床反力の並進力の3方向成分Fx,Fy,Fz並びにモーメントの3方向成分Mx,My,Mzの検出信号を制御ユニット60に出力する。また、上体3には、Z軸(鉛直方向(重力方向))に対する上体3の傾斜角およびその角速度、並びにZ軸回りの上体3の回転角(ヨー角)およびその角速度を検出するための姿勢センサ54が備えられ、その検出信号が該姿勢センサ54から制御ユニット60に出力される。この姿勢センサ54は、図示を省略する3軸方向の加速度センサおよび3軸方向のジャイロセンサを備え、これらのセンサの検出信号が上体3の姿勢角(傾斜角およびヨー角)およびその角速度を検出するために用いられると共に、ロボット1の自己位置姿勢を推定するために用いられる。また、詳細構造の図示は省略するが、ロボット1の各関節には、それを駆動するための電動モータ64(図3参照)と、その電動モータ64の回転量(各関節の回転角)を検出するためのエンコーダ(ロータリエンコーダ)65(図3参照)とが設けられ、該エンコーダ65の検出信号が該エンコーダ65から制御ユニット60に出力される。
さらに、図1では図示を省略するが、ロボット1の適宜な位置にはジョイスティック(操作器)73(図3参照)が設けられ、そのジョイスティック73を操作することで、直進移動しているロボット1を旋回させるなど、ロボット1の歩容に対する要求を必要に応じて制御ユニット60に入力できるように構成されている。
図2は本実施形態における各脚体2の先端部分(各足平22R(L)を含む)の基本構成を概略的に示す図である。同図に示すように、各足平22R(L)の上方には、前記6軸力センサ50との間にばね機構70が装備されると共に、足底(各足平22R(L)の底面)にはゴムなどからなる足底弾性体71が貼られている。これらのばね機構70及び足底弾性体71によりコンプライアンス機構72が構成されている。ばね機構70は詳細は後述するが、足平22R(L)の上面部に取り付けられた方形状のガイド部材(図示省略)と、足首関節18R(L)(図2では足首関節20R(L)を省略している)および6軸力センサ50側に取り付けられ、前記ガイド部材に弾性材(ゴムやばね)を介して微動自在に収納されるピストン状部材(図示省略)とから構成されている。
図2に実線で表示された足平22R(L)は、床反力を受けていないときの状態を示している。各脚体2が床反力を受けると、コンプライアンス機構72のばね機構70と足底弾性体71とがたわみ、足平22R(L)は図中に点線で例示したような位置姿勢に移る。このコンプラインアス機構72の構造は、例えば本出願人が先に提案した特開平5−305584号公報に詳細に説明されている如く、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。
図3は制御ユニット60の構成を示すブロック図である。該制御ユニット60はマイクロコンピュータにより構成されており、CPUからなる第1の演算装置90及び第2の演算装置92、A/D変換器80、カウンタ86、D/A変換器96、RAM84、ROM94、並びにこれらの間のデータ授受を行うバスライン82を備えている。この制御ユニット60では、各脚体2の6軸力センサ50、姿勢センサ54(加速度センサおよびレートジャイロセンサ)、ジョイスティック73等の出力信号はA/D変換器80でデジタル値に変換された後、バスライン82を介してRAM84に送られる。またロボット1の各関節のエンコーダ65(ロータリーエンコーダ)の出力は、カウンタ86を介してRAM84に入力される。
前記第1の演算装置90は後述の如く目標歩容を生成すると共に、関節角変位指令(各関節の変位角もしくは各電動モータ64の回転角の指令値)を算出し、RAM84に送出する。また第2の演算装置92はRAM84から関節角変位指令と、前記エンコーダ65の出力信号に基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出してD/A変換器96とサーボアンプ64aとを介して各関節を駆動する電動モータ64に出力する。
図4は、この実施形態に係る脚式移動ロボットの制御装置の機能的構成を全体的に示すブロック図である。この図4中の「実ロボット」の部分以外の部分が制御ユニット60が実行する処理機能(主として第1の演算装置90及び第2の演算装置92の機能)によって構成されるものである。なお、以下の説明では、脚体2および腕体5の左右を特に区別する必要がないときは、前記符号R,Lを省略する。
以下説明すると、制御ユニット60は、ロボット1の目標歩容を自在かつリアルタイムに生成して出力する歩容生成装置100を備えている。該歩容生成装置100は、その機能によって、本願発明の各手段を構成するものである。この歩容生成装置100が出力する目標歩容は、目標上***置姿勢軌道(上体3の目標位置及び目標姿勢の軌道)、目標足平位置姿勢軌道(各足平22の目標位置及び目標姿勢の軌道)、目標腕姿勢軌道(各腕体5の目標姿勢の軌道)、目標全床反力中心点(目標ZMP)軌道、目標全床反力軌道から構成される。なお、脚体2や腕体5以外に上体3に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。
ここで、上記歩容における「軌道」は時間的変化のパターン(時系列パターン)を意味し、以下の説明では、「軌道」の代わりに「パターン」と称することもある。また、「姿勢」は空間的な向きを意味する。具体的には、例えば上体姿勢はZ軸(鉛直軸)に対するロール方向(X軸まわり)の上体3の傾斜角とピッチ方向(Y軸まわり)の上体3の傾斜角とヨー方向(Z軸まわり)の上体3の回転角(ヨー角)とで表され、足平姿勢は各足平22に固定的に設定された2軸の空間的な方位角で表される。本明細書では、上体姿勢は上体姿勢角ということもある。
尚、以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。また、歩容のうちの、床反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上***置姿勢等、ロボット1の運動に係わる歩容を総称的に「運動」という。また、各足平22に作用する床反力(並進力及びモーメントからなる床反力)を「各足平床反力」と呼び、ロボット1の全て(2つ)の足平22R,22Lについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義として扱う。
目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に目標床反力中心点(全床反力の中心点の目標位置)を作用点にして目標床反力を表現すると、目標床反力のモーメント成分は、鉛直成分(鉛直軸(Z軸)まわりのモーメント)を除いて零になる。換言すれば、目標床反力中心点まわりの目標床反力のモーメントの水平成分(水平軸(X軸及びY軸)回りのモーメント)は零になる。
尚、動力学的平衡条件を満足する歩容では、ロボット1の目標運動軌道から算出されるZMP(目標運動軌道から算出される慣性力と重力との合力がその点まわりに作用するモーメントが、鉛直成分を除いて零になる点)と目標床反力中心点は一致することから、目標床反力中心点軌道の代わりに目標ZMP軌道を与えると言っても同じことである(詳細は、本願出願人によるPCT公開公報WO/02/40224等を参照)。
このような背景から、上記PCT公開公報WO/02/40224の明細書では目標歩容を、次のように定義していた。
a)広義の目標歩容とは、1歩ないしは複数歩の期間の目標運動軌道とその目標床反力軌道との組である。
b)狭義の目標歩容とは、1歩の期間の目標運動軌道とそのZMP軌道との組である。
c)一連の歩容は、いくつかの歩容がつながったものとする。
歩行においては、本出願人が先に特開平10−86080号公報で提案した上体高さ決定手法によってロボット1の上体3の鉛直位置(上体高さ)が決定されると、並進床反力鉛直成分は従属的に決定される。さらに、目標歩容の運動による慣性力と重力との合力が目標ZMPまわりに発生するモーメントの水平成分が零になるようにロボット1の上体水平位置軌道を決定することで、並進床反力水平成分も決定される。このため、PCT公開公報WO/02/40224の明細書では、目標歩容の床反力に関して明示的に設定すべき物理量としては、目標ZMPだけで十分であった。したがって、狭義の目標歩容の定義としては、上記のb)で十分であった。それに対し、本実施形態で説明するロボット1の走行歩容(詳細は後述する)においては、床反力鉛直成分(並進床反力鉛直成分)も制御上重要である。このため本発明では、この床反力鉛直成分の目標軌道を明示的に設定した上で、ロボット1の目標上体鉛直位置等の軌道を決定する。そこで、本明細書では、狭義の目標歩容の定義として、次のb’)を用いる。
b’)狭義の目標歩容とは、1歩の期間の目標運動軌道とその目標ZMP軌道と目標並進床反力鉛直成分軌道との組である。
この明細書では以降、理解を容易にするために、特にことわらない限り、目標歩容は上記b’)の狭義の目標歩容の意味で使用する。この場合、目標歩容の「1歩」は、ロボット1の片方の脚体2が着地してからもう一方の脚体2が着地するまでの意味で使用する。補足すると、本実施形態では、目標歩容は1歩の期間分の歩容の意味で使用するが、それは必ずしも1歩の期間分のものである必要はなく、複数歩分、あるいは1歩よりも短い(例えば半歩など)の期間分の歩容であってもよい。尚、以下の説明では、「床反力鉛直成分」は「並進床反力鉛直成分」を意味するものとし、床反力のうちのモーメントの鉛直成分(鉛直軸回り成分)は、「モーメント」という用語を用いて「床反力鉛直成分」と区別をする。同様に、「床反力水平成分」は「並進床反力水平成分」を意味するものとする。
また、歩容における両脚支持期とは言うまでもなく、ロボット1がその自重を両脚体2,2で支持する期間、片脚支持期とはいずれか一方のみの脚体2でロボット1の自重を支持する期間、空中期とは両脚体2,2が床から離れている(空中に浮いている)期間を言う。片脚支持期においてロボット1の自重を支持しない側の脚体2を「遊脚」と呼ぶ。尚、本実施形態で説明する走行歩容では、両脚支持期は無く、片脚支持期(着地期)と空中期とが交互に繰り返される。この場合、空中期では両脚体2,2とも、ロボット1の自重を支持しないこととなるが、該空中期の直前の片脚支持期において遊脚であった脚体2、支持脚であった脚体2をそれぞれ該空中期においても遊脚、支持脚と呼ぶ。
図5に示す走行歩容を例にして、歩容生成装置100が生成する目標歩容の概要を説明する。なお、歩容に関するその他の定義および詳細は、先に本願出願人が提案した特開平10−86081号公報にも記載されているので、以下では、特開平10−86081号公報に記載されていない内容を主に説明する。
まず、図5に示す走行歩容を説明する。この走行歩容は、人間の通常的な走行歩容と同様の歩容である。この走行歩容では、ロボット1の左右いずれか一方のみの脚体2(支持脚)の足平22が着地(接地)する片脚支持期と、両脚体2,2が空中に浮く空中期とが交互に繰り返される。図5の1番目の状態は片脚支持期の開始時(初期)の状態、2番目の状態は片脚支持期の中間時点の状態、3番目の状態は片脚支持期に続く空中期の開始時(片脚支持期の終了時)の状態、4番目の状態は空中期の中間時点の状態、5番目の状態は空中期の終了時(次の片脚支持期の開始時)の状態を示している。
この走行歩容では、ロボット1は、図5の1番目の状態で示すように、片脚支持期の開始時において支持脚(ロボット1の進行方向前側の脚体2)側の足平22のかかとで着地する。続いて、図5の2番目の状態で示すように、ロボット1は、着地した足平22(支持脚側の足平22)の底面のほぼ全面を着地させた後、図5の3番目の状態で示すように、その支持脚側の足平22(図5の3番目の状態におけるロボット1の進行方向後側の脚体2の足平22)のつま先で床を蹴って空中に飛び上がる。これより片脚支持期が終了すると共に空中期が開始する。尚、片脚支持期における遊脚は、図5の1番目の状態で示すように、片脚支持期の開始時では、支持脚の後側に存在するが、図5の2番目及び3番目の状態で示すように次の着地予定位置に向かって支持脚の前側に振り出される。次いで、図5の4番目の状態で示す空中期を経た後、ロボット1は、遊脚(空中期の開始直前の片脚支持期で遊脚となっていた脚体2)の足平22のかかとで着地し、次の片脚支持期が開始される。
図5の走行歩容を考慮しつつ、前記歩容生成装置100が生成する目標歩容の基本的な概要を説明する。詳細は後述するが、歩容生成装置100が目標歩容を生成するとき、遊脚側の足平22の着地位置姿勢(着地予定位置姿勢)や着地時刻(着地予定時刻)等の目標歩容生成用の基本的な要求値(要求パラメータ)が、前記ジョイスティック73の所要の操作等に応じて歩容生成装置100に与えられる。そして、歩容生成装置100は、その要求パラメータを用いて目標歩容を生成する。より詳しく言えば、歩容生成装置100は、上記要求パラメータに応じて、目標歩容の目標足平位置姿勢軌道、目標床反力鉛直成分軌道等、目標歩容の一部の構成要素を規定するパラメータ(歩容パラメータという)を決定した上で、その歩容パラメータを用いて目標歩容の瞬時値を逐次決定し、該目標歩容の時系列パターンを生成する。
この場合、目標足平位置姿勢軌道(より詳しくは、足平の位置及び姿勢の空間的な各成分(X軸成分等)の目標軌道)は、例えば本出願人が特許第3233450号にて提案した有限時間整定フィルタを用いて各足平22毎に生成される。この有限時間整定フィルタは、可変時定数の1次遅れフィルタ、すなわち、伝達関数が1/(1+τs)の形で表されるフィルタ(τは可変の時定数。以下、このフィルタを単位フィルタという)を複数段(本実施形態では3段以上)、直列に接続したものであり、所望の指定時刻に指定値に到達するような軌道を生成・出力することができるものである。この場合、各段の単位フィルタの時定数τは、いずれも、有限時間整定フィルタの出力生成を開始してから、上記指定時刻までの残時間に応じて逐次可変的に設定される。より詳しくは、該残時間が短くなるに伴いτの値が所定の初期値(>0)から減少されていき、最終的には、該残時間が0になる指定時刻にて、τの値が0になるように設定される。そして、有限時間整定フィルタには、前記指定値(より詳しくは、有限時間整定フィルタの出力の初期値から前記指定値への変化量)に応じた高さのステップ入力が与えられる。このような有限時間整定フィルタは、指定時刻にて指定値に達するような出力が生成されるだけでなく、指定時刻における有限時間整定フィルタの出力の変化速度を0もしくはほぼ0にすることができる。特に、単位フィルタを3段以上(3段でよい)、接続した場合には、有限時間整定フィルタの出力の変化加速度(変化速度の微分値)をも0もしくはほぼ0にすることができる。
尚、上記のように有限時間整定フィルタにより生成される目標足平位置姿勢軌道は、床面に固定された後述の支持脚座標系での各足平22の目標位置姿勢軌道である。
上述のように生成される目標足平位置姿勢軌道は、各足平22の位置が、その初期接地状態(目標歩容の初期時刻の状態)から着地予定位置に向かって徐々に加速しながら移動を開始するように生成される。そして、該目標足平位置姿勢軌道は、最終的に着地予定時刻までに徐々に位置の変化速度を0またはほぼ0にまで減速し、着地予定時刻にて着地予定位置に到達して停止するように生成される。このため、各足平22の着地瞬間における対地速度(床に固定された支持脚座標系での各足平22の位置の変化速度)が0またはほぼ0になる。したがって、走行歩容において同時に全脚体2,2が空中に存在する状態(空中期での状態)から着地しても、着地衝撃が小さくなる。
前記走行歩容においては、ロボット1に作用する重力によって空中期後半から上体3の鉛直速度は下向きになり、着地時でも下向きのままである。したがって、上記のように各足平22の着地瞬間における対地速度が0またはほぼ0になるように目標足平位置姿勢軌道を生成すると共に、後述する如く動力学的平衡条件を満たすように上体3の目標位置姿勢軌道を生成したとき、着地直前において、上体3に対する遊脚側の足平22の相対速度は、上向きになる。すなわち、走行歩容の着地瞬間では、ロボット1の目標歩容は遊脚側の脚体22を上体3側に引っ込めながら着地するような歩容となる。言い換えれば、本実施形態での目標歩容では、ロボット1は、着地瞬間において、遊脚側の足平22の対地速度が0またはほぼ0になるように、上体3から見て該足平22を引き上げるようにして着地する。これによって、着地衝撃は小さくなり、着地衝撃が過大になるのを防止するようにしている。
また、本実施形態では、有限時間整定フィルタは、単位フィルタを3段以上(例えば3段)、直列に接続したものであるため、着地予定時刻までに各足平22の速度(足平位置の変化速度)が0またはほぼ0になるだけでなく、各足平22は、その加速度も着地予定時刻にて0またはほぼ0になって停止する。つまり、着地瞬間における対地加速度も0またはほぼ0になる。したがって、着地衝撃がより一層小さくなる。特に、実際のロボット1の着地時刻が目標の着地時刻からずれても、衝撃があまり増大しなくなる。補足すると、着地予定時刻にて各足平22の対地速度を0またはほぼ0にする上では、有限時間整定フィルタの単位フィルタの段数は2段でもよいが、この場合には、着地予定時刻での各足平22の加速度は一般には0にならない。
尚、足平姿勢に関しては、各足平22が着地予定時刻にてその踵で着地した後、該足平22の底面のほぼ全面が床に接地するまで動き続ける。このため、該足平22の底面のほぼ全面が床に接地する時刻を前記指定時刻に設定して、前記有限時間整定フィルタにより足平姿勢軌道が生成される。
また、本実施形態では、有限時間整定フィルタを用いて足平位置軌道を生成したが、着地予定時刻での足平位置の変化速度(足平位置の時間微分値)が0またはほぼ0になるように、さらには、該着地予定時刻での足平位置の変化加速度(変化速度の時間微分値)が0またはほぼ0になるように設定された多項式などの関数を用いて目標足平位置軌道を生成するようにしても良い。このことは、目標足平姿勢軌道の生成に関しても同様である。但し、該目標足平姿勢軌道の生成に関しては、上述の如く、各足平22の底面のほぼ全面が床に設置する時刻にて、各足平22の姿勢の変化速度、さらにはその変化加速度が0またはほぼ0になるように多項式などの関数が設定される。
目標床反力鉛直成分軌道は、例えば図6のように設定される。本実施形態では、走行歩容における目標床反力鉛直成分軌道の形状(詳しくは片脚支持期での形状)は、台形状(床反力鉛直成分の増加側に凸の形状)に定められており、その台形の高さ、折れ点の時刻を目標床反力鉛直成分軌道を規定する歩容パラメータとして、それらの歩容パラメータ(床反力鉛直成分軌道パラメータ)が決定される。尚、走行歩容の空中期では、目標床反力鉛直成分は定常的に0に設定される。この例のように、目標床反力鉛直成分軌道は、実質的に連続になるように(値が不連続にならないように)設定するのが良い。これは床反力を制御する際のロボット1の関節の動作を滑らかにするためである。尚、「実質的に連続」というのは、アナログ的に連続な軌道(真の意味での連続な軌道)を離散時間系でデジタル表現したときに必然的に生じる値の飛びは、該軌道の連続性を失わせるものではないということを意味するものである。
目標ZMP軌道は次のように設定される。図5の走行歩容においては、前記したように支持脚側足平22のかかとで着地し、次にその支持脚側足平22のつま先で蹴って空中に飛び上がり、最後に遊脚側足平22のかかとで着地する。したがって、片脚支持期での目標ZMP軌道は、図7の上段図に示すように、支持脚側足平22のかかとを初期位置として、次に支持脚側足平22の底面のほぼ全面が接地する期間において該足平22の前後方向の中央に移動し、その後、離床時までに支持脚側足平22のつまさきに移動するように設定される。ここで、図7の上段図は、X軸方向(前後方向)の目標ZMP軌道を示すものであり、図7の下段図はY軸方向(左右方向)の目標ZMP軌道を示すものである。尚、片脚支持期におけるY軸方向の目標ZMP軌道は、図7の下段図に示すように、Y軸方向での支持脚側脚体2の足首関節の中心位置と同じ位置に設定される。
走行歩容では、さらに片脚支持期の終了後、両脚体2,2が床から離れ、床反力鉛直成分が0になる。床反力鉛直成分が0の時、すなわち空中期には、ロボット1の全体重心は自由落下運動をし、全体重心まわりの角運動量変化は零である。この時、床の任意の点において、ロボット1に作用する重力と慣性力との合力のモーメントは0であるので、目標ZMPは不定である。すなわち、床のいずれの点も、「重力と慣性力との合力が作用するモーメントの水平成分が0である作用点」というZMPの条件を満たす。言いかえれば、任意の点に目標ZMPを設定しても、上記合力が目標ZMP回りに作用するモーメントの水平成分が0であるという動力学的平衡条件を満足する。したがって、目標ZMPを不連続に設定しても構わない。例えば、空中期では、目標ZMPを、離床時(片脚支持期の終了時)の目標ZMP位置から移動しないように設定し、空中期終端において、着地時の目標ZMP位置に不連続(ステップ状)に移動するように該目標ZMP軌道を設定しても構わない。しかし本実施形態では、図7の上段図に示すように、空中期における目標ZMP軌道のX軸方向位置は、次の遊脚側脚体2の着地までに支持脚側足平22のつまさきから遊脚側足平22のかかとの着地位置まで連続的に移動するようにした。また、図7の下段図に示すように、空中期における目標ZMP軌道のY軸方向位置は、次の遊脚側脚体2の着地までに支持脚側脚体2の足首関節の中心のY軸方向位置から遊脚側脚体2の足首関節の中心のY軸方向位置まで連続的に移動するようにした。すなわち、歩容の全期間において目標ZMP軌道を連続(実質的に連続)にした。そして、後述するように、目標ZMPまわりの重力と慣性力との合力のモーメント(鉛直成分を除く)が零になるように目標歩容を生成する(より具体的には目標上***置姿勢軌道を調整する)ようにした。モデルの近似誤差を考慮すると、生成された歩容が滑らかになるようにするために、空中期においても目標ZMP軌道を連続(実質的に連続)にしておくのが望ましい。但し、本実施形態で用いる後述の動力学モデルでは、目標ZMPの位置によらずに、目標ZMP回りのモーメントの水平成分を0にするような目標歩容を一義的に生成できるので、必ずしも目標ZMPを連続にしておく必要はない。
尚、本実施形態では、図7に示したような目標ZMP軌道の折れ点の位置や時刻が、ZMP軌道パラメータ(目標ZMP軌道を規定するパラメータ)として設定される。また、上記したZMP軌道の「実質的に連続」の意味は、前記床反力鉛直成分軌道の場合と同様である。
ZMP軌道パラメータは、安定余裕が高く、かつ急激な変化をしないように決定される。ここで、ロボット1の接地面を含む最小の凸多角形(いわゆる支持多角形)の中央付近に目標ZMPが存在する状態を安定余裕が高いと言う(詳細は特開平10−86081号公報を参照)。図7の目標ZMP軌道はこのような条件を満たすように設定したものである。
また、目標上***置姿勢、目標足平位置姿勢および後述の基準上体姿勢はグローバル座標系で記述される。グローバル座標系は床に固定された座標系である。グローバル座標系としては、より具体的には、後述する支持脚座標系が用いられる。
本実施形態では、歩容生成装置100は、目標上体姿勢だけでなく、基準上体姿勢も生成する。この基準上体姿勢は、歩容に対する要求(歩容生成装置100に対する行動計画部などの装置または外部(前記ジョイスティック73等)からの要求)にそのまま従って生成される上体姿勢である。
目標上体姿勢(以降、「基準」が付いていない場合には、目標上体姿勢を表す)は、長期的には基準上体姿勢に追従するかまたは一致するように生成される。
目標上体姿勢は、歩行においては、本願出願人によるPCT公開公報WO/02/40224の明細書に記載した実施形態のごとく通常、常に基準上体姿勢に一致させておけば良い。PCT公開公報WO/02/40224では、基準上体姿勢という概念は記載されていないが、目標上体姿勢パターンを、明示的かつ優先的に与えていたので、基準上体姿勢に目標上体姿勢が常に一致していることと同じことである。
しかし、走行など空中期がある歩容や低摩擦床面での歩行においては、単に上体水平加速度等を調整するだけでは、目標歩容の床反力水平成分と床反力モーメント鉛直成分とが許容範囲以内(あるいは摩擦限界以内)に存在するようにしつつ動力学的平衡条件を満足することができない。
そこで、本実施形態においては、目標上体姿勢を必要に応じて基準上体姿勢から故意にずらすこととした。より具体的には、以下に説明する運動モードを複合的に発生させることにより、目標歩容の床反力水平成分や床反力モーメント鉛直成分が許容範囲以内(あるいは摩擦限界以内)に存在しつつ動力学的平衡条件を満足させるようにした。
図8のように、ロボット1のある運動状態から、上体水平加速度だけを摂動(微小変化)させると、ロボット1の全体重心水平加速度と全体重心まわりの角運動量が摂動する。すなわち、上体水平加速度の摂動は、それによって発生するロボット1の慣性力と重力との合力に対して動力学的に釣り合う床反力鉛直成分を摂動させずに(ロボット1の全体重心鉛直加速度を摂動させずに)、目標ZMPまわりの床反力モーメント水平成分(水平軸まわり成分)と床反力水平成分とを摂動させる。このようにロボット1の上体水平加速度を摂動させる運動モードを上体並進モードと呼ぶ。
言いかえると、床反力鉛直成分を変化させずに、目標ZMPまわりの床反力モーメント水平成分と床反力水平成分とを変化させる運動を上体並進モードと呼ぶ。尚、上体並進モードでは、床反力モーメント鉛直成分(鉛直軸まわり成分)も摂動するが、ここではこの点に注目しない。
この時の単位加速度当たりの床反力モーメント水平成分の変化をΔMp、単位加速度当たりの床反力水平成分の変化をΔFpとする。図8に示す状況で上体3を前方に水平加速すると、ΔMpとΔFpは図8に示す矢印の向きに作用する。
感覚的に判り易くするために、運動によって発生する慣性力と重力との合力に釣り合う床反力を用いて表現したが、慣性力と重力との合力を用いて表現した方が、理論的には的確である。なお、上記合力と床反力とは、互いに大きさが同じで向きが逆になっている。
それに対し、図9のように、ロボット1のある運動状態から、ある点Prまわりに上体傾斜角加速度(上体3の傾斜角の角加速度)を摂動させると、ロボット1の全体重心は摂動せずに、全体重心まわりの角運動量(鉛直軸まわり成分を除く)が摂動する。すなわち、点Prまわりの上体傾斜角加速度の摂動は、床反力鉛直成分と床反力水平成分とを摂動させずに、目標ZMPまわりの床反力モーメント水平成分を摂動させる。このようにロボット1の上体傾斜角加速度を摂動させる運動モードを上体傾斜モードと呼ぶ。
言いかえると、床反力鉛直成分と床反力水平成分を変化させずに、目標ZMPまわりの床反力モーメント水平成分を変化させる運動を上体傾斜モードと呼ぶ。
この時の単位角加速度当たりの床反力モーメント水平成分の変化をΔMr、単位角加速度当たりの床反力水平成分の変化をΔFrとする。ΔFrは零である。図9に示す状況で上体3が前傾するように上体傾斜角の角加速度を発生させると、ΔMrは図9に示す矢印の向きに作用する。
また、図10のように、ロボット1のある運動状態から、ある点Pqまわりに上体ヨー角加速度(上体3の鉛直軸まわりの回転角加速度)を摂動させると、ロボット1の全体重心は摂動せずに、全体重心まわりの角運動量鉛直成分が摂動する。ところで、ロボット1の全体重心が摂動しなければ、角運動量鉛直成分の摂動は、作用点に依存しない。したがって、点Pqまわりの上体ヨー角加速度の摂動は、床反力鉛直成分、床反力水平成分および床反力モーメント水平成分を摂動させずに、目標ZMPまわりの床反力モーメント鉛直成分を摂動させる。このようにロボット1の上体ヨー角加速度を摂動させる運動モードを上体ヨー回転モードと呼ぶ。
言いかえると、床反力鉛直成分、床反力水平成分および床反力モーメント水平成分を変化させずに、目標ZMPまわりの床反力モーメント鉛直成分を変化させる上体運動を上体ヨー回転モードと呼ぶ。
この時の単位角加速度当たりの床反力モーメント鉛直成分の変化をΔMbz、単位角加速度当たりの床反力水平成分の変化をΔFbとする。ΔFbは零である。図10に示す状況で上体3を矢印の向きに単位角加速度で回転させる(角加速度βb=1で回転させる)と、ΔMbzは図10に示す矢印の向きに作用する。
尚、図10に示す運動では、両腕体5,5の手先位置が支持脚座標系(床に固定された座標系)から見て変わらないように、上体3を回転させたが、上体3に対する腕体5の相対位置姿勢を変えずに、上体3と共に腕体5を回転させる運動を上体ヨー回転モードと定義しても良い。ただし、この場合には、後述する運動方程式に若干変更が必要である。
また、図11(a),(b)のように、ロボット1のある運動状態から、両腕体5,5の先端を互いに前後に逆方向に摂動させると、ロボット1の全体重心は摂動せずに、全体重心まわりの角運動量鉛直成分が摂動する。以降、この運動モードを逆位相腕振りモードと呼ぶ。言い換えると、床反力鉛直成分、床反力水平成分および床反力モーメント水平成分を摂動させずに、目標ZMPまわりの床反力モーメント鉛直成分を摂動させる腕振り運動モードを逆位相腕振りモードと呼ぶ。
右腕体5Rを前側に単位量移動させ、左腕体5Lを後ろに単位量移動させる動作を、単位角度の逆位相腕振りと呼ぶ。図11(a),(b)は、逆位相腕振り角度がθazの状態を表す。
逆位相腕振りモードの単位角加速度当たりの床反力モーメント鉛直成分の変化をΔMaz、単位角加速度当たりの床反力水平成分の変化をΔFaとする。ΔFaは零である。図11(a),(b)に示す状況で右腕体5Rを前側に加速し、左腕体5Lを後ろ側に加速する(角加速度βa>0で振る)と、図11(a)に示す矢印の向き(鉛直軸の正の向き)に床反力モーメント鉛直成分Mazが作用する。
次に、本実施形態に用いられるロボット1の動力学モデルについて説明する。本実施形態では単純化された(近似された)以下に示す動力学モデルを用いる。ただし、以下に示す動力学モデルに関しては、キネマティクスモデル(関節、リンクの構造や寸法を表すモデルであり、言い換えると、関節変位とリンクの位置姿勢との関係を表すモデルである。)も必要である。
図12は、本実施形態に用いられるロボット1の動力学モデルである。図示の如く、この動力学モデルは、ロボット1の各脚体2にそれぞれ対応する2つの質点2m,2m、及び上体3に対応する質点3mからなる合計3質点と、イナーシャがあって質量のない4つのフライホイールFHx、FHy、FHbz、FHazとから構成されるモデルである。フライホイールFHx、FHy、FHbz、FHazは、それぞれX軸(前後方向軸)、Y軸(左右方向軸)、Z軸(鉛直軸)、Z軸(鉛直軸)の回りに回転可能なものである。この動力学モデルでは、デカップルド、即ち、脚体2,2の動力学(各質点2m,2mの動力学)と上体3の動力学(質点3m及びフライホイールFHx、FHyおよびFHbzの動力学)と腕体5,5の動力学(フライホイールFHazの動力学)とが相互に非干渉に構成されると共に、ロボット1全体の動力学は、それらの線形結合で表される。また、さらに、上体3の運動と床反力との関係も、上体3の並進運動(上体並進モード)と床反力との関係、上体3の傾斜運動(上体傾斜モード)と床反力との関係、上体3のヨー回転運動(上体ヨー回転モード)と床反力との関係、並びに両腕体5,5の逆位相腕振り運動(逆位相腕振りモード)と床反力との関係に分離される。具体的には、上体質点3mの水平運動によって発生する床反力は、上体3の水平方向並進運動(上体並進モード)によって発生する床反力に対応し、フライホイールFHxおよびFHyの回転運動によって発生する床反力は、上体3の傾斜角の回転運動(上体傾斜モード)によって発生する床反力に対応する。なお、フライホイールFHxの回転運動は、上体3の傾斜角のロール方向(X軸回り)の回転運動に対応し、フライホイールFHyの回転運動は、上体3の傾斜角のピッチ方向(Y軸回り)の回転運動に対応している。また、フライホイールFHbzの回転運動によって発生する床反力は、上体3のヨー回転運動(上体ヨー回転モード)によって発生する床反力に対応する。また、フライホイールFHazの回転運動によって発生する床反力は、逆位相腕振り(逆位相腕振りモード)によって発生する床反力に対応する。
尚、ロボット1の腕体の質量は上体3に含まれるものとし、上体質点3mは腕体5,5の質量を含む質量をもつ。
説明の便宜上、動力学モデルに関する変数およびパラメータを以下のように定義する。なお、各質点2m,2m,3mはそれが対応する部位の代表点であるか、あるいはその部位の位置姿勢から幾何学的に一義的に決定される点に相当する。例えば、支持脚側脚体2の質点2mの位置は、該脚体2の足平22の底面の前記代表点から所定の距離だけ上方の点とする。
Zsup:支持脚質点鉛直位置
Zswg:遊脚質点鉛直位置
Zb:上体質点鉛直位置(上体鉛直位置とは一般的には異なる。)
ZGtotal:全体重心鉛直位置
Xsup:支持脚質点X位置
Ysup:支持脚質点Y位置
Xswg:遊脚質点X位置
Yswg:遊脚質点Y位置
Xb:上体質点X位置(上体質点位置は、前記点Prから上体の前後方向にある所定の距離だけオフセットした点である。オフセットは、直立時などにおいて、厳密モデルの重心位置と、本動力学モデルの重心位置がなるべく一致するように、決定される。上体水平位置とは一般的には異なる。)
Yb:上体質点Y位置
XGtotal:全体重心水平X位置
YGtotal:全体重心水平Y位置
θbx:鉛直方向に対するX軸回りの上体傾斜角
θby:鉛直方向に対するY軸回りの上体傾斜角
θbz:上体ヨー回転角
θaz:逆位相腕振り角
mb:上体質点質量
msup:支持脚質点質量
mswg:遊脚質点質量
mtotal:ロボット総質量(=mtotal+msup+mswg)
J:上体慣性モーメント(上体傾斜モードにおける等価慣性モーメントである。すなわち、FHxおよびFHyの慣性モーメントである。実ロボット1の上体3部位の慣性モーメントとは一般的には一致しない。)
Jbz:鉛直軸まわり上体慣性モーメント(上体ヨー回転モードにおける等価慣性モーメントである。実ロボット1の上体3部位の慣性モーメントとは一般的には一致しない。)
Jaz:腕振り鉛直軸まわり慣性モーメント(スピンキャンセルのための逆位相腕振りにおける等価慣性モーメントである。すなわち、FHzの慣性モーメントである。)
Fx:床反力X成分(詳しくは並進床反力の前後方向(X軸)成分)
Fy:床反力Y成分(詳しくは並進床反力の左右方向(Y軸)成分)
Fz:床反力鉛直成分(詳しくは並進床反力の鉛直方向(Z軸)成分。本実施形態では、これは目標並進床反力鉛直成分に等しい)
Mx:目標ZMPまわりの床反力モーメントX成分(詳しくは床反力モーメントの前後軸(X軸)まわり成分)
My:目標ZMPまわりの床反力モーメントY成分(詳しくは床反力モーメントの左右軸(Y軸)まわり成分)
Mz:目標ZMPまわりの床反力モーメントZ成分(詳しくは床反力モーメントの鉛直軸(Z軸)まわり成分)
なお、各質点2m,3mのX位置、Y位置はそれぞれ前後方向(X軸方向)の位置、左右方向(Y軸方向)の位置を意味している。また、本実施形態では、各脚体2の質点2mの位置と、その脚体2の足平22の位置(足平22のあらかじめ定めた代表点の位置)との位置関係はあらかじめ定められており、一方の位置が決まれば、他方の位置は一義的に定まるものとされている。また、上体質点3mと、上体3の位置(上体3のあらかじめ定められた代表点の位置)との位置関係は、上体3の姿勢角(以降、上体に関しては、姿勢角とは、傾斜角とヨー角を意味するものとする。)に応じてあらかじめ定められており、一方の位置と姿勢角とが決まれば、他方の位置は一義的に定まるものとされている。
また、任意の変数Xに対して、dX/dtはXの1階微分を表わし、d2X/dt2はXの2階微分を表わす。したがって、変数Xが変位ならば、dX/dtは速度、d2X/dt2は加速度を意味する。gは重力加速度定数を示す。ここでは、gは正の値とする。
上記動力学モデルの運動方程式(動力学的平衡条件を表す式)は、式01、式02x、式02y、式03x、式03y、式03zで表される。
Fz=mb*(g+d2Zb/dt2)+msup*(g+d2Zsup/dt2)
+mswg*(g+d2Zswg/dt2) ……式01
Fx=mb*d2Xb/dt2+msup*d2Xsup/dt2+mswg*d2Xswg/dt2
……式02x
Fy=mb*d2Yb/dt2+msup*d2Ysup/dt2+mswg*d2Yswg/dt2
……式02y
Mx=mb*(Yb−Yzmp)*(g+d2Zb/dt2)
−mb*(Zb−Zzmp)*(d2Yb/dt2)
+msup*(Ysup−Yzmp)*(g+d2Zsup/dt2)
−msup*(Zsup−Zzmp)*(d2Ysup/dt2)
+mswg*(Yswg−Yzmp)*(g+d2Zswg/dt2)
−mswg*(Zswg−Zzmp)*(d2Yswg/dt2)+J*d2θbx/dt2
……式03x
My=−mb*(Xb−Xzmp)*(g+d2Zb/dt2)
+mb*(Zb−Zzmp)*(d2Xb/dt2)
−msup*(Xsup−Xzmp)*(g+d2Zsup/dt2)
+msup*(Zsup−Zzmp)*(d2Xsup/dt2)
−mswg*(Xswg−Xzmp)*(g+d2Zswg/dt2)
+mswg*(Zswg−Zzmp)*(d2Xswg/dt2)+J*d2θby/dt2
……式03y
Mz=mb*(Xb−Xzmp)*(d2Yb/dt2)−mb*(Yb−Yzmp)*(d2Xb/dt2)
+msup*(Xsup−Xzmp)*(d2Ysup/dt2)
−msup*(Ysup−Yzmp)*(d2Xsup/dt2)
+mswg*(Xswg−Xzmp)*(d2Yswg/dt2)
−mswg*(Yswg−Yzmp)*(d2Xswg/dt2)
+Jbz*d2θbz/dt2+Jaz*d2θaz/dt2
……式03z
また、ロボット全体重心位置には次の関係式が成立する。
ZGtotal=(mb*Zb+msup*Zsup+mswg*Zswg)/mtotal
……式04
XGtotal=(mb*Xb+msup*Xsup+mswg*Xswg)/mtotal
……式05x
YGtotal=(mb*Yb+msup*Ysup+mswg*Yswg)/mtotal
……式05y
以下に上記動力学モデルと前記ΔFp、ΔMp、ΔFr、ΔMrの関係を示す。
前記ΔFpは、式02xまたは式02yにおいて、d2Xb/dt2またはd2Yb/dt2を単位量だけ摂動させたときのFxまたはFyの摂動量であるから、次式によって求められる。
ΔFp=mb …式06
すなわち、上体並進モードの各水平軸(X軸、Y軸)方向の単位加速度当たりの床反力水平成分の変化ΔFpは、上記動力学モデルの上体質点3mの質量に相当する。
前記ΔMpは、式03yまたは式03xにおいて、d2Xb/dt2またはd2Yb/dt2を単位量だけ摂動させたときのMyまたはMxの摂動量であるから、次式によって求められる。
ΔMp=mb*(Zb−Zzmp) …式07
すなわち、上体並進モードの各水平軸(X軸、Y軸)方向の単位加速度当たりの床反力モーメント水平成分の変化ΔMpは、上記動力学モデルの上体質点質量に、上体質点3mの目標ZMPからの高さ(鉛直位置)を乗じたものになる。このときの上体質点3m及び目標ZMPの位置と、上体質点3mの運動との関係は、上体質点3mを倒立振子質点、目標ZMPを倒立振子支点に対応づけたときの倒立振子の挙動に相当する。なお、Y軸方向のΔMpは、より正確には、式07の右辺の符号を反転させたものである。
前記ΔFrは、式02xまたは式02yにおいて、d2θby/dt2を単位量だけ摂動させたときのFxまたはFyの摂動量であるから、次式によって求められる。
ΔFr=0 …式08
すなわち、上体傾斜モードの各水平軸(X軸、Y軸)方向の単位加速度当たりの床反力水平成分の変化ΔFrは、0である。
前記ΔMrは、式03xまたは式03yにおいて、d2θbx/dt2またはd2θby/dt2を単位量だけ摂動させたときのMxまたはMyの摂動量であるから、次式によって求められる。
ΔMr=J …式09
すなわち、上体傾斜モードの各水平軸(X軸、Y軸)方向の単位加速度当たりの床反力モーメント水平成分の変化ΔMrは、水平軸フライホイール(FHxおよびFHy)の慣性モーメントに相当する。
前記ΔMbzは、式03zにおいて、d2θbz/dt2を単位量だけ摂動させたときのMzの摂動量であるから、次式によって求められる。
ΔMbz=Jbz …式09b
すなわち、上体ヨー回転モードの単位加速度当たりの床反力モーメント成分の変化ΔMbzは、上体ヨー回転に対応するフライホイールFHbzの慣性モーメントに相当する。
前記ΔMazは、式03zにおいて、d2θaz/dt2を単位量だけ摂動させたときのMzの摂動量であるから、次式によって求められる。
ΔMaz=Jaz …式09a
すなわち、逆位相腕振りの単位角加速度当たりの床反力モーメント成分の変化ΔMazは、腕振りに対応するフライホイールFHazの慣性モーメントに相当する。
本実施形態における歩容生成装置100は、ロボット1の片方の脚体2が着地してから他方の脚体2が着地するまでの1歩分の目標歩容(前記狭義の意味での目標歩容)を単位として、その1歩分の目標歩容を順番に生成する。従って、本実施形態で生成する図5の走行歩容では、該目標歩容は片脚支持期の開始時から、これに続く空中期の終了時(次の片脚支持期の開始時)までの目標歩容が順番に生成される。ここで、新たに生成しようとしている目標歩容を「今回歩容」、その次の目標歩容を「次回歩容」、さらにその次の目標歩容を「次次回歩容」、というように呼ぶ。また、「今回歩容」の一つ前に生成した目標歩容を「前回歩容」と呼ぶ。
また、歩容生成装置100が今回歩容を新たに生成するとき、該歩容生成装置100には、ロボット1の2歩先までの遊脚側足平22の着地予定位置姿勢、着地予定時刻の要求値(要求)が歩容に対する要求パラメータとして入力される(あるいは歩容生成装置100が記憶装置から要求パラメータを読み込む)。そして、歩容生成装置100は、これらの要求パラメータを用いて、目標上***置姿勢軌道、目標足平位置姿勢軌道、目標ZMP軌道、目標床反力鉛直成分軌道、目標腕姿勢軌道等を生成する。このとき、これらの軌道を規定する歩容パラメータの一部は、歩行の継続性を確保するように適宜修正される。
以下に図5の走行歩容を生成することを例にして、歩容生成装置100の歩容生成処理の詳細を図13〜図46を参照しつつ説明する。図13は、その歩容生成装置100が実行する歩容生成処理のメインルーチンを示すフローチャート(構造化フローチャート)である。
まずS010において時刻tを0に初期化するなど種々の初期化作業が行なわれる。この処理は、歩容生成装置100の起動時等に行なわれる。次いで、S012を経てS014に進み、歩容生成装置100は、制御周期(図13のフローチャートの演算処理周期)毎のタイマ割り込みを待つ。制御周期はΔtである。
次いで、S016に進み、歩容の切り替わり目であるか否かが判断され、歩容の切り替わり目であるときはS018に進むと共に、切り替わり目でないときはS030に進む。ここで、上記「歩容の切り替わり目」は、前回歩容の生成が完了し、今回歩容の生成を開始するタイミングを意味し、例えば前回歩容の生成を完了した制御周期の次の制御周期が歩容の切り替わり目になる。
S018に進むときは時刻tが0に初期化され、次いでS020に進み、次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期が読み込まれる。これらの支持脚座標系及び歩容周期は、前記要求パラメータにより定まるものである。すなわち、本実施形態では、歩容生成装置100にジョイスティック73等から与えらる要求パラメータは、2歩先までの遊脚側足平22の着地予定位置姿勢(足平22が着地してから足底を床面にほぼ全面的に接触させるように、滑らさずに回転させた状態での足平位置姿勢)、着地予定時刻の要求値を含んでおり、その1歩目の要求値、2歩目の要求値がそれぞれ、今回歩容、次回歩容に対応するものとして、今回歩容の生成開始時(前記S016の歩容の切り替わり目)以前に歩容生成装置100に与えられたものである。なお、これらの要求値は今回歩容の生成途中でも変更することは可能である。
そして、上記要求パラメータにおける1歩目の遊脚側足平22(今回歩容での遊脚側足平22)の着地予定位置姿勢の要求値に対応して次回歩容支持脚座標系が定まる。
例えば図16を参照して、今回歩容(1歩目)に係わる遊脚側足平22(図では22L)の着地予定位置姿勢の要求値が、今回歩容の支持脚側足平22(図では22R)の着地位置姿勢に対して、今回歩容支持脚座標系のX軸方向(今回歩容の支持脚側足平22Rの前後方向)及びY軸方向(今回歩容の支持脚側足平22Rの左右方向)に、それぞれxnext、ynextだけ移動し、且つZ軸回り(鉛直軸回り)にθznextだけ回転させた位置姿勢であるとする。ここで、支持脚座標系は、支持脚側足平22を水平姿勢(より一般的には床面に平行な姿勢)にして該支持脚側足平22の底面のほぼ全面を床面に接触(密着)させた状態において、該支持脚側脚体2の足首中心から床面に延ばした垂線が該床面と交わる点(この点は、本実施形態の例では支持脚側足平22の底面のほぼ全面を床面に接触させた状態では、該足平22の代表点と合致する)を原点とし、その原点を通る水平面をXY平面とするグローバル座標系(床に固定された座標系)である。この場合、X軸方向、Y軸方向は、それぞれ支持脚側足平22の前後方向、左右方向である。尚、支持脚座標系の原点は、必ずしも支持脚側足平22の底面のほぼ全面を床面に接触させた状態での該足平22の代表点(足平22の位置を代表する点)に合致する必要はなく、該代表点と異なる床面上の点に設定されてもよい。
このとき、次回歩容支持脚座標系は、図示のように今回歩容の遊脚側足平22Lの着地予定位置姿勢の要求値に従って該足平22を着地させた場合(足平22の代表点を着地予定位置の要求値に一致させ、且つ、足平22の姿勢(向き)を着地予定姿勢の要求値に一致させた場合)における該足平22Lの代表点(より詳しくは該代表点に合致する床上の点)を原点とし、該原点を通る水平面内における該足平22Lの前後方向、左右方向をそれぞれX’軸方向、Y’軸方向とする座標系である。
上記と同様に、2歩目の遊脚側足平22の着地予定位置姿勢の要求値に応じて次々回歩容支持脚座標系(図16のX”Y”座標を参照)が定まる。また、今回歩容周期は、今回歩容の支持脚側足平22の着地予定時刻(要求値)から、1歩目(今回歩容)の遊脚側足平22の着地予定時刻(要求値)までの時間として定まり、次回歩容周期は、1歩目の遊脚側足平22の着地予定時刻(要求値)から2歩目の遊脚側足平22の着地予定時刻(要求値)までの時間として定まる。
尚、前記要求パラメータは、本実施形態ではジョイスティック73の所要の操作によって歩容生成装置100に入力されるものであるが、あらかじめ該要求パラメータあるいはこれに対応する上記の支持脚座標系の位置姿勢及び歩容周期をロボット1の移動スケジュールとして記憶しておいても良い。あるいは、ジョイスティック73などの操縦装置からの指令(要求)とそのときまでのロボットの1の移動履歴とを基に前記次回及び次次回歩容支持脚座標系、並びに今回及び次回歩容周期を決定しても良い。
次いでS022に進み、歩容生成装置100は、今回歩容に続く仮想的な周期的歩容としての定常旋回歩容の歩容パラメータを決定する。該歩容パラメータは、定常旋回歩容における目標足平位置姿勢軌道を規定する足平軌道パラメータ、基準とする上体姿勢軌道を規定する基準上体姿勢軌道パラメータ、基準とする腕姿勢軌道を規定する基準腕姿勢軌道パラメータ、目標ZMP軌道を規定するZMP軌道パラメータ、目標床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメータを含む。さらに、床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲を規定するパラメータも歩容パラメータに含まれる。
尚、この明細書で「定常旋回歩容」は、その歩容を繰り返したときに歩容の境界(本実施形態では1歩毎の歩容の境界)においてロボット1の運動状態(足平位置姿勢、上***置姿勢等の状態)に不連続が生じないような周期的歩容を意味するものとして使用する。以降、「定常旋回歩容」を「定常歩容」と略す場合もある。
周期的歩容である定常旋回歩容は、本実施形態では、ロボット1の2歩分の歩容、すなわち今回歩容に続く第1旋回歩容と該第1旋回歩容に続く第2旋回歩容とからなる歩容を該定常旋回歩容の1周期分の歩容として、その1周期分の歩容を繰り返す歩容である。尚、ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。また、生成する目標歩容が前記図5の走行歩容であると、その目標歩容の今回歩容は、片脚支持期及び空中期を有する走行歩容であるので、定常旋回歩容の第1旋回歩容及び第2旋回歩容は、いずれも今回歩容と同様に片脚支持期と空中期とを有する歩容である。つまり、第1旋回歩容及び第2旋回歩容の基本的な歩容形態は今回歩容と同一である。
定常旋回歩容について補足すると、2足移動ロボットでは、定常旋回歩容の1周期分は、前記した狭義の意味での歩容が、少なくとも2歩分、必要である。そして、3歩以上の歩容を1周期分の歩容とする複雑な定常旋回歩容を設定することも可能である。但し、定常旋回歩容は、後述の如く、今回歩容の終端(終了時刻)における発散成分(詳細は後述する)を決定するためだけに用いられる。このため、3歩以上の歩容を1周期とする定常旋回歩容を用いることは、歩容生成の処理が煩雑となるにも関わらず、効果は少ない。そこで、本実施形態での定常旋回歩容の1周期分の歩容を2歩分の歩容(第1及び第2旋回歩容)により構成するようにしている。尚、3足以上の脚式移動ロボットにあっては、それに応じて定常旋回歩容を定義するに足る歩容数が増加する。以下では、説明の便宜上、複数の狭義の歩容(本実施形態では2歩分の歩容)から成る定常旋回歩容を1歩の歩容とみなす。
定常旋回歩容は、歩容生成装置100で今回歩容の終端における発散成分や上体鉛直位置速度、上体姿勢角及びその角速度等のロボット1の運動状態を決定するために暫定的に作成されるものであり、歩容生成装置100からそのまま出力されるものではない。
尚、「発散」とは、図14に示すように、2足移動ロボット1の上体3の位置が両足平22,22の位置からかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、2足移動ロボット1の上体3の位置が両足平22,22の位置(より具体的には、支持脚側足平22の接地面に設定されたグローバル座標系(支持脚座標系)の原点)からかけ離れていく具合を表す数値である。
本実施形態では、目標歩容が、前記発散を生じることなく、継続的に生成されるように、発散成分を指標にして歩容を生成するようにした。ただし、継続的な歩容の代表例である定常歩容(歩容の軌道の不連続を発生することなく、同じパターンの歩容を繰り返すことができる周期的歩容であり、原理上、無限回繰り返しても発散しない歩容)の初期発散成分(定常歩容の初期時刻での発散成分)であっても、単純に0ではなく、定常歩容のパラメータが変われば、その初期発散成分も変わる。すなわち、歩き方あるいは走り方等の歩容形態によって、適切な発散成分が変わる。そこで、本実施形態では、これから生成しようとする今回歩容の後に続く定常歩容が前記今回歩容に係わる要求パラメータに応じて設定され、定常歩容の初期発散成分を求めてから、今回歩容の終端発散成分を定常歩容の初期発散成分に一致させる(より一般的には、今回歩容を定常歩容に連続させもしくは近づける)ように、今回歩容を生成する。これのような歩容生成の基本的な指針は本出願人が先に提案したPCT公開公報WO/02/40224のものと同様である。
本発明の実施形態では、PCT公開公報WO/02/40224の第1実施形態で用いたような線形な3質点動力学モデルを用いていないが、次式で定義される発散成分および収束成分の概念は、図12に示したモデルのような非線形な動力学モデルの挙動の摂動分に対しても、近似的に十分な精度で適用することができる。
発散成分=上体質点水平位置+上体質点水平速度/ω0
…式10
収束成分=上体質点水平位置−上体質点水平速度/ω0’
…式11
ただし、ここでの上体質点水平位置は、図12に示す前記動力学モデルにおいては、上体質点水平位置Xbを表す。
また、ω0、ω0’はある所定の値である。これらのω0、ω0’値はほぼ同じ値であるが、厳密には一致しない。そして、走行時では、PCT公開公報WO/02/40224の歩行歩容生成時の値に対して若干変更する必要がある。
発散成分および収束成分に関するその他の詳細は、PCT公開公報WO/02/40224に記述されているので、ここではこれ以上言及しない。
本実施形態においては、PCT公開公報WO/02/40224に示した方式に加え、さらに、後述するように、目標床反力鉛直成分軌道を規定する歩容パラメータを設定し、この目標床反力鉛直成分を動力学的に満足するようにロボット1の全体重心鉛直位置を決定する。この場合、床反力鉛直成分の2階積分値がロボット1の全体重心鉛直位置を規定するものになるので、目標床反力鉛直成分の設定が悪いと、ロボット1の全体重心鉛直位置もしくは上体鉛直位置が高くなり過ぎたり低くなり過ぎたりする。したがって、目標床反力鉛直成分の設定方法も重要な課題である。しかし、床反力鉛直成分と上体鉛直位置との関係は、ZMPと上体水平位置との関係に似ているので、上体水平位置速度を適切にするための目標ZMP決定手法の一部を、以下の本実施形態に示すように、少し変更するだけで、上体鉛直位置速度を適切にするための目標床反力鉛直成分の決定手法に適用することができる。
本題に戻り、S022では、図15に示すフローチャートに従って、以下の処理が行われる。
まず、S100において、今回歩容、第1旋回歩容、第2旋回歩容の順に足平位置姿勢軌道がつながるように、定常歩容の歩容パラメータのうちの足平軌道パラメータが決定される。以下に具体的な設定方法を図16を参照して説明する。尚、以降の説明では、支持脚側の脚体2の足平22を支持脚足平と称し、遊脚側の脚体2の足平22を遊脚足平を称する。また、歩容の「初期」、「終端」はそれぞれ歩容の開始時刻、終了時刻もしくはそれらの時刻における瞬時歩容を意味する。
足平軌道パラメータは、第1旋回歩容および第2旋回歩容の初期及び終端のそれぞれにおける支持脚足平及び遊脚足平のそれぞれの位置姿勢、各旋回歩容の歩容周期等から構成される。この足平軌道パラメータのうち、第1旋回歩容初期遊脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とされる。この場合、走行歩容では、今回歩容終端における支持脚足平22は、空中に移動している。そして、今回歩容終端支持脚足平位置姿勢は、今回歩容初期支持脚足平位置姿勢(=前回歩容終端遊脚足平位置姿勢)から、前記要求パラメータにおける2歩目の遊脚側足平22の着地予定位置姿勢の要求値(今回歩容の支持脚足平22の次回歩容における着地予定位置姿勢の要求値)もしくは該要求値に対応する次次回歩容支持脚座標系に応じて定まる次回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道(詳しくは次回歩容支持脚座標系から見た軌道)を、今回歩容終端まで前記有限時間整定フィルタを用いて生成することにより求められる。
尚、次回歩容終端遊脚足平位置姿勢は、その位置姿勢から足平22を接地させたまま、つま先を下げるように該足平22を水平姿勢までピッチ方向に所定角度回転させたときの該足平の位置姿勢が次次回歩容支持脚座標系の位置姿勢に合致するように決定される。換言すれば、次回歩容終端遊脚足平位置姿勢は、前記要求パラメータにおける2歩目の遊脚側足平22の着地位置姿勢の要求値から、該足平22をすべらないように接地させたまま、つま先を持ち上げるように該足平22をピッチ方向に所定角度回転させた状態(つま先を上げてかかとを着地させた状態)での該足平22の位置姿勢である。
また、第1旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とされる。この場合、今回歩容終端遊脚足平位置姿勢は、前記次回歩容終端遊脚足平位置姿勢の場合と同様、前記次回歩容支持脚座標系もしくはこれに対応する前記要求パラメータの1歩目(今回歩容)の遊脚着地予定位置姿勢の要求値に応じて定まるものである。すなわち、該今回歩容終端遊脚足平位置姿勢は、その位置姿勢から、足平22を接地させたまま、つま先を下げるように該足平22を回転させて該足平22の底面のほぼ全面を床面に接地させたときの該足平の代表点が次回歩容支持脚座標系の原点に合致するように決定される。
第1旋回歩容終端遊脚足平位置姿勢は、今回歩容終端遊脚足平位置姿勢や次回歩容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容支持脚座標系から見た次次回歩容支持脚座標系の位置姿勢に基づいて決定される。より具体的には、第1旋回歩容終端遊脚足平位置姿勢は、その位置姿勢から足平22を接地させたまま、すべらないように該足平22を水平姿勢まで所定角度回転させたときの足平位置姿勢が、次回歩容支持脚座標系から見た次次回歩容支持脚座標系の位置姿勢に合致するように設定される。
第1旋回歩容終端では、支持脚足平22は離床して空中にある。支持脚足平22が離床してから後の軌道を決定するために、第1旋回歩容支持脚足平着地予定位置姿勢が設定される。第1旋回歩容支持脚足平着地予定位置姿勢は、次回歩容支持脚座標系から見た次次次回歩容支持脚座標系の位置姿勢に基づいて設定される。より具体的には、第1旋回歩容支持脚足平着地予定位置姿勢は、次回歩容支持脚座標系から見た次次次回歩容支持脚座標系の位置姿勢である。なお、次次次回歩容支持脚座標系は、次次回歩容支持脚座標系と次次次回歩容支持脚座標系との相対的位置姿勢関係が、今回歩容支持脚座標系と次回歩容支持脚座標系との相対的位置姿勢関係と一致するように設定される。
第1旋回歩容終端支持脚足平位置姿勢は、第1旋回歩容初期支持脚足平位置姿勢を求めた場合と同様に、第1旋回歩容初期支持脚足平位置姿勢から、前記第1旋回歩容支持脚足平着地予定位置姿勢に至る足平位置姿勢軌道(より詳しくは次回歩容支持脚座標系から見た軌道)を、第1旋回歩容終端まで前記有限時間整定フィルタを用いて生成することにより求められる。
第2旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第1旋回歩容終端支持脚足平位置姿勢とされる。第2旋回歩容初期支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た第1旋回歩容終端遊脚足平位置姿勢とされる。
第2旋回歩容終端遊脚足平位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とされる。第2旋回歩容終端支持脚足平位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とされる。
第1旋回歩容および第2旋回歩容の歩容周期は、次回歩容周期と同一に設定される。これらの第1旋回歩容及び第2旋回歩容の歩容周期は、互いに同一にすることは必ずしも必要ではないが、いずれの周期も、少なくとも次回歩容周期に応じて決定するのが好ましい。尚、今回歩容、第1旋回歩容および第2旋回歩容の上記以外の運動パラメータ(両脚支持期時間などの時間パラメータを含む)は、上記決定されたパラメータに応じて、歩容の条件(アクチュエータの速度が許容範囲に入っているか、可動角を超えていないか、床などと干渉していないかなど)を満足するように適宜決定する。
次に、S102に進み、目標上体姿勢が追従すべき基準上体姿勢軌道を規定する基準上体姿勢軌道パラメータが決定される。基準上体姿勢は、定常歩容の初期(第1旋回歩容の初期)と終端(第2旋回歩容の終端)とでつながるように(定常歩容の初期、終端での基準上体姿勢の姿勢角及びその角速度が一致するように)に設定されている限り、一定姿勢である必要はないが、本実施形態では理解を容易にするため、基準上体姿勢のうちの傾斜角(鉛直方向に対する傾斜角)に係わる姿勢は、直立姿勢(鉛直姿勢)に設定される。つまり、本実施形態では、上体3の傾斜角に関する基準上体姿勢は、定常歩容の全期間において直立姿勢に設定される。従って、本実施形態では、基準上体姿勢のうちの傾斜角の角速度及び角加速度は0である。基準上体姿勢のうちのヨー角軌道(以降、基準ヨー角軌道とも呼ぶ)θbzは、例えば一定角速度(定常歩容の平均旋回速度)の運動にすれば良い。あるいは、後述の基準逆位相腕振り軌道の例(図18)のようにサイン波状にしても良い。ただし、定常歩容を繰り返した時に基準ヨー角およびその角速度が連続となるように設定する。
尚、本実施形態では、目標上体姿勢のうちのヨー角軌道(以降、目標ヨー角軌道とも呼ぶ)は、基準ヨー角軌道に一致させるものとする。
次に、S104に進み、基準腕姿勢軌道パラメータが決定される。具体的には、両腕体5,5の全体の重心位置(上体3に対する相対的な重心位置)、左右の手先(両腕体5,5の先端)の左右方向の間隔、および逆位相腕振り角に関するパラメータが決定される。基準逆位相腕振り角は、たとえば図17のように左旋回する場合には、図18のように設定すれば良い。図18のように、基準逆位相腕振り角θazrefは、定常歩容を繰り返した時に歩容の境界(第2旋回歩容の終端と次の第1旋回歩容の境界)において逆位相腕振り角および角速度が共に連続になり、かつ第1旋回歩容初期における支持脚と逆位相腕振り角の相対関係が、次の第1旋回歩容初期における支持脚と逆位相腕振り角の相対関係に一致するように設定される。すなわち、第1旋回歩容の初期逆位相腕振り角速度と第2旋回歩容の終端逆位相腕振り角速度が一致し、第2旋回歩容の終端逆位相腕振り角は、定常歩容の旋回角(第1旋回歩容と第2旋回歩容の旋回角の和)に第1旋回歩容初期逆位相腕振り角を加えた値に設定される。図18では基準逆位相腕振り角θazrefをサイン波状の波形としたが、一定角速度に設定しても良い。あるいは、支持脚のヨー角と遊脚のヨー角の平均の値としても良い。
尚、本実施形態では、目標腕姿勢の両腕体5,5の全体の重心位置(上体3に対する相対位置)は、上体3に対して一定に維持されるように設定される。
次に、S106に進み、床反力鉛直成分軌道パラメータが設定される。この場合、該パラメータにより規定される床反力鉛直成分軌道が、第1旋回歩容および第2旋回歩容のいずれにおいても図6のように実質的に連続な(値がステップ状に飛ばない)ものとなるように床反力鉛直成分軌道パラメータが設定される。すなわち、定常旋回歩容の目標床反力鉛直成分軌道は図19のようなパターンで設定される。そのパターンでは、第1旋回歩容及び第2旋回歩容のいずれにおいても、片脚支持期では床反力鉛直成分が台形状に変化し、空中期では床反力鉛直成分が0に維持される。そして、このパターンの折れ点の時刻や、台形部分の高さ(ピーク値)が床反力鉛直成分軌道パラメータとして設定される。
この床反力鉛直成分軌道パラメータの設定に際しては、床反力鉛直成分の歩容全期間(第1旋回歩容と第2旋回歩容両方の期間を合わせた期間で定常歩容の1周期の期間)における平均値をロボット1の自重と一致させる。すなわち、床反力鉛直成分の平均値がロボット1に作用する重力と同じ大きさで反対向きになるようにする。
上記のごとく床反力鉛直成分軌道を設定することは、定常歩容の条件を満たすために必要である。定常歩容の条件とは、支持脚座標系(支持脚側足平22の接地面に設定された座標系)から見た歩容のあらゆる状態変数(ロボット1の各部の位置、姿勢、速度等)の初期状態(第1旋回歩容の初期状態)と次の支持脚座標系(次の第1旋回歩容の支持脚座標系)から見た歩容の終端状態(第2旋回歩容の終端状態)が一致していることである(以下、この条件を定常歩容の境界条件ということがある)。したがって、定常歩容終端におけるロボット1の全体重心鉛直速度と定常歩容初期における全体重心鉛直速度との差(詳細には、第2旋回歩容終端の全体重心鉛直速度と第1旋回歩容初期の全体重心鉛直速度との差)も零でなければならない。上記差は床反力鉛直成分と重力との差の積分値(1階積分値)であるから、上記差を零にするためには、上記のごとく床反力鉛直成分軌道を設定する必要がある。
尚、本実施形態では、床反力鉛直成分の第1旋回歩容、第2旋回歩容それぞれの期間における平均値をロボット1の自重と一致させるようにした。より具体的には、例えば第1旋回歩容及び第2旋回歩容の歩容周期に応じて各旋回歩容における床反力鉛直成分軌道の台形部分の折れ点の時刻を設定した上で、その台形部分の高さを、床反力鉛直成分の第1旋回歩容、第2旋回歩容それぞれの期間における平均値がロボット1の自重と一致するように決定した(台形の高さを未知数として、上記平均値及び自重の一致条件を表す方程式を解くことで台形の高さを求める)。
こうすることにより、第1旋回歩容終端の全体重心鉛直速度と第1旋回歩容初期の全体重心鉛直速度との差も0、第2旋回歩容終端の全体重心鉛直速度と第2旋回歩容初期の全体重心鉛直速度との差も0になる。ただし、このようにする必然性はない。例えば、第1旋回歩容と第2旋回歩容との境界あたりで上体鉛直位置が高くなり過ぎたり低くなり過ぎたりして無理な姿勢になりそうな場合には、各旋回歩容で上記平均値及び自重を一致させた状態から各旋回歩容の床反力鉛直成分軌道の台形の高さ等を修正するようにしてもよい。
次に、S108に進み、上記のごとく図19のように設定された床反力鉛直成分軌道に応じて、床反力水平成分の許容範囲[Fxmin,Fxmax](より詳しくはこれを規定するパラメータ)が図20のように設定される。図20の負側の折れ線が床反力水平成分許容下限値Fxmin、正側の折れ線が床反力水平成分許容上限値Fxmaxを表す。これらの設定方法に関して以下に補足する。以下では、床面が水平である場合について説明する。
床反力水平成分は、床と足平22との間の摩擦によって発生するが、摩擦はいくらでも発生できるわけではなく、限界がある。したがって、生成された目標歩容に従って実際のロボット1が移動した時にスリップしないようにするためには、目標歩容の床反力水平成分が、常に摩擦限界以内になければならない。そこで、この条件を満足させるために、床反力水平成分許容範囲を設定し、後述するように、目標歩容の床反力水平成分がこの許容範囲内になるように、目標歩容を生成することとした。
床と足平22との間の摩擦係数をμとすると、Fxminは、常に−μ*床反力鉛直成分以上に、Fxmaxはμ*床反力鉛直成分以下に設定されなければならない。最も単純な設定方法は、次式による設定である。ただし、kaは1より小さい正の定数である。
Fxmin=−ka*μ*床反力鉛直成分
Fxmax= ka*μ*床反力鉛直成分
…式12
図20の床反力水平成分許容範囲は、式12にしたがって設定した例である。床反力水平成分許容範囲を規定するパラメータとして、図20の台形波形などの折れ点での値と時刻を設定してもよいが、式12により床反力水平成分許容範囲を決定するときには、単に式12における(ka*μ)の値をパラメータとして設定するだけでも良い。
尚、上記条件(目標歩容の床反力水平成分を常に摩擦限界以内にするという条件)を満足しているならば、他の設定方法で床反力水平成分許容範囲を設定するようにしても構わない。
次に、S109に進み、上記のごとく図19のように設定された床反力鉛直成分軌道などに応じて、床反力モーメント鉛直成分の許容範囲[Mzmin,Mzmax](より詳しくはこれを規定するパラメータ)が図21のように設定される。図21の負側の折れ線が床反力モーメント鉛直成分許容下限値Mzmin、正側の折れ線が床反力モーメント鉛直成分許容上限値Mzmaxを表す。これらの設定方法に関して以下に補足する。以下では、床面が水平である場合について説明する。
床反力モーメント鉛直成分は、床と足平22との間の摩擦によって発生するが、摩擦はいくらでも発生できるわけではなく、限界がある。したがって、生成された目標歩容に従って実際のロボット1が移動した時にスピンしないようにするためには、目標歩容の床反力モーメント鉛直成分が、常に摩擦限界以内になければならない。そこで、この条件を満足させるために、床反力モーメント鉛直成分許容範囲を設定し、後述するように、目標歩容の床反力モーメント鉛直成分がこの許容範囲内になるように、目標歩容を生成することとした。
床と足平22との間の摩擦係数をμ、モーメント鉛直成分を発生するための床と足平22との間の接触面の有効半径(あるいは床と足平22との間の接触面の目標ZMPまわりの断面2次モーメントの平方根)をrとすると、Mzminは、常に−μ*r*床反力鉛直成分以上に、Mzmaxはμ*r*床反力鉛直成分以下に設定されなければならない。最も単純な設定方法は、次式による設定である。ただし、kaは1より小さい正の定数である。
Mzmin=−ka*μ*r*床反力鉛直成分
Mzmax= ka*μ*r*床反力鉛直成分
…式1012
図21の床反力モーメント鉛直成分許容範囲は、式1012にしたがって設定した例である。床反力モーメント鉛直成分許容範囲を規定するパラメータとして、図21の台形波形などの折れ点での値と時刻を設定してもよいが、式1012により床反力モーメント鉛直成分許容範囲を決定するときには、単に式1012における(ka*μ)の値をパラメータとして設定するだけでも良い。rは、各瞬間において目標ZMPと接地面から算出するのが良いが、定数としても構わない。
尚、上記条件(目標歩容の床反力モーメント鉛直成分を常に摩擦限界以内にするという条件)を満足しているならば、他の設定方法で床反力モーメント鉛直成分許容範囲を設定するようにしても構わない。
さらには、床反力水平成分許容範囲と床反力モーメント鉛直成分許容範囲を独立に設定するのではなく、床反力水平成分と床反力鉛直成分モーメントの組み合わせの許容範囲を設定するようにしても良い。なぜなら、床反力水平成分が大きくなると、床反力モーメント鉛直成分の許容範囲が狭くなり、また、床反力モーメント鉛直成分が大きくなると、床反力水平成分の許容範囲が狭くなるからである。
次に、S110に進み、第1旋回歩容および第2旋回歩容を合わせた定常歩容のZMP軌道を規定するZMP軌道パラメータが設定される。この場合、目標ZMP軌道は、前述したように安定余裕が高くかつ急激な変化をしないように設定される。
さらに詳細には、図5の走行歩容においては、支持脚足平22のかかとで着地した後、しばらくして該支持脚足平22の底面のほぼ全面が接地し、さらにしばらくして、支持脚足平22のつま先だけが接地する。そして、次に支持脚足平22のつま先で蹴って空中に飛び上がり、最後に遊脚足平22のかかとで着地する。また、目標ZMPは接地面内に存在しなければならない。そこで、本実施形態では、定常歩容の第1旋回歩容及び第2旋回歩容のそれぞれの目標ZMPのX軸方向の位置は、前記図7の上段図に示したように、支持脚足平22のかかとを初期位置として、該足平22の底面のほぼ全体が接地するまでその位置に留まるように設定される。次に、目標ZMPは支持脚足平22の中央に移動し、該足平22がつま先接地状態になるまでにつま先に移動し、その後、離床時まで支持脚足平22のつまさきに留まるように設定される。さらにその後は、目標ZMPは、前述のごとく、次の遊脚足平22の着地までに目標ZMPが、支持脚足平22のつまさきから遊脚足平22のかかとの着地位置まで連続的に移動するように設定される。したがって、第1旋回歩容と第2旋回歩容とから成る定常歩容の目標ZMP軌道(X軸方向の軌道)は、図22のようになる。そして、この目標ZMP軌道の折れ点の時刻及び位置がZMP軌道パラメータとして設定される。この場合、折れ点の時刻は、前記要求パラメータに応じて定めた第1旋回歩容及び第2旋回歩容の歩容周期に応じて設定され、該折れ点の位置は、次回歩容支持脚座標系及び次次回歩容支持脚座標系の位置姿勢あるいはこれらの座標系を規定する要求パラメータの1歩目及び2歩目の遊脚側足平着地予定位置姿勢の要求値に応じて設定される。尚、ZMP軌道のY軸方向の位置は、前記図7の下段図に示したものと同様に設定される。より詳しくは、第1旋回歩容における目標ZMPのY軸方向位置の軌道は、図7の下段図のものと同一パターンで設定され、第2旋回歩容における目標ZMPのY軸方向位置の軌道は、第1旋回歩容のものと同じ形の軌道で、該軌道の終端につながるものに設定される。
次に、S112に進み、定常歩容1歩(1周期)の初期時刻、終端時刻、期間が以下のように再定義される。
定常歩容は、その初期と終端での状態変数が連続的につながる歩容でなければならない。このような歩容の決定を容易にするために、本実施形態では、定常歩容を決定するまでは、前述の狭義の歩容の定義とは異なり、定常歩容の1歩の初期、終端、期間を便宜上、図19のように定める。すなわち、第1旋回歩容の片脚支持期の後半において床反力鉛直成分がある程度減少した時刻を定常歩容の初期時刻Tsとして設定する。なお、初期時刻Tsは、図7に示すように、支持脚足平22の底面のほぼ全面が接地した状態からつま先接地に変わる瞬間あるいはその直前の時刻(図7の足底全面接地期間の終了時刻もしくはその直前の時刻)に設定するのが望ましい。また、S110において設定した図22(または図7)に示す目標ZMPと時刻Tsとの関係を説明すると、第1旋回歩容において支持脚足平22の底面のほぼ全面が接地した後に、目標ZMPが支持脚足平22の中央に移動し、つま先接地状態になるまでにつま先に移動を完了した瞬間が、時刻Tsとなっているのが望ましい。このような初期時刻Tsは例えば先に設定した目標ZMP軌道パラメータに応じて設定される。上記のごとく初期時刻Tsを設定する理由は後述する。
また、図19に示すように、定常歩容の周期Tcycは第1旋回歩容と第2旋回歩容の歩容周期の和である。定常歩容の終端時刻をTeとする。Teは、TsにTcycを加えた時刻に設定される。
定常歩容が決定された時点(後述の図23のS204のループを抜けた時点)から、改めて、歩容の初期、終端等の定義を、前述の狭義の歩容の定義に戻すこととする。以下の説明では、前述の狭義の歩容の定義による初期時刻(支持脚側足平22の着地当初の時刻)を0とし、定常歩容を決定するまで用いる上記初期時刻Tsを参照符号Tsを用いて(単に「Ts」と省略することもある)本来の初期時刻0と区別する。
最後にS114に進み、定常歩容の上体姿勢角・逆位相腕振り角復元期間[Tm,Ts2]および[Tm2,Te]を設定する。補足すると、定常歩容を繰り返した時に歩容の境界において上体姿勢角および逆位相腕振り角は連続であるべきである。そのためには、定常歩容の初期上体姿勢角速度と終端上体姿勢角速度が一致し、定常歩容の初期逆位相腕振り角速度と終端逆位相腕振り角速度が一致しなければならない。前記期間は、そのようにするための、上体姿勢角軌道と逆位相腕振り角軌道を調整する期間である。
具体的には、初期時刻Tsから第1旋回歩容の空中期を経て第2旋回歩容になり、床反力鉛直成分がある所定の大きさまで増加した時刻Tmが設定される。さらに、第2旋回歩容の片脚支持期の後半において床反力鉛直成分がある程度減少した時刻Ts2が設定される。さらに、第2旋回歩容の空中期を経て第1旋回歩容になり、床反力鉛直成分がある所定の大きさまで増加した時刻Tm2が設定される。
図19にこれらの時刻を示す。時刻Tmは、支持脚足平22の底面のほぼ全面が接地した瞬間あるいはその直後に設定するのが望ましい。時刻Tm2も同様である。また、時刻Ts2は、前記初期時刻Ts同様、足平22の底面のほぼ全面が接地した状態からつま先接地に変わる瞬間あるいはその直前に設定するのが望ましい。
また、前記図15のS110において設定した図22の目標ZMPとこれらの時刻Tm,Ts2,Tm2との関係を説明すると、第2旋回歩容において、目標ZMPが、支持脚足平22のかかとを初期位置として、該足平22の底面のほぼ全体が接地するまでその位置に留まり、次に支持脚足平22の中央に移動し始める瞬間を時刻Tmとすることが望ましい。その後、支持脚足平22のつま先だけ接地した状態になるまでに、目標ZMPがつま先に移動を完了した瞬間を、時刻Ts2とすることが望ましい。さらに次の第1旋回歩容において、目標ZMPが、支持脚足平22のかかとを初期位置として該足平22の底面のほぼ全体が接地するまでその位置に留まり、次に支持脚足平22の中央に移動し始める瞬間を時刻Tm2とすることが望ましい。
上記のごとく設定する理由は後述する。尚、上体姿勢角を復元(調整)する期間と逆位相腕振り角を復元(調整)する期間を別々に設定しても良い。
図13のS010からS022までに示す処理が行われた後、S024に進み、定常歩容の初期状態が算出される。ここで算出される初期状態は、定常歩容の初期上体水平位置速度(水平方向での初期上***置及び初期上体速度)、初期上体鉛直位置速度(鉛直方向での初期上***置及び初期上体速度)、初期発散成分、初期上体姿勢角、角速度、および初期逆位相腕振り角、角速度である。この初期状態の算出は、図23のフローチャートに従って、探索的に行なわれる。
図23のフローチャートにおいては、先ず、S200において、定常歩容の歩容パラメータ(前記図13のS022で設定したパラメータ)に基づいて、目標足平位置姿勢、目標腕姿勢、および目標上体姿勢角(傾斜角とヨー角)の初期状態(初期時刻Tsでの状態)が決定される。ここで状態とは、位置や姿勢角とその変化率(時間微分)とを表す。
この場合、支持脚側の目標足平位置姿勢の初期状態は、前記図15のS100で決定した足平軌道パラメータの第1旋回歩容初期支持脚足平位置姿勢から、第2旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道(次回歩容支持脚座標系から見た軌道)を時刻Tsまで有限時間整定フィルタにより生成することにより決定される。遊脚側の目標足平位置姿勢の初期状態は、次回歩容支持脚座標系から見た今回歩容初期支持脚足平位置姿勢から、第1旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道を時刻Tsまで有限時間整定フィルタにより生成することにより決定される。また、目標腕姿勢の初期状態は、図15のS104で決定した基準腕姿勢軌道パラメータに基づいて求められる時刻Tsにおける基準腕姿勢に決定される。より具体的には、目標腕姿勢の両腕体5,5の全体重心位置(上体3に対する相対位置)、左右の手先(両腕体5,5の先端部)の左右方向の間隔および逆位相腕振り角・角速度が決定される。但し、逆位相腕振り角・角速度は、後述するように、定常歩容を繰り返した時に歩容の境界で連続するように修正されるので、ここではまだ仮に決定されただけである。
また、目標上体姿勢角の初期状態は、図15のS102で決定した基準上体姿勢軌道パラメータにより定まる時刻Tsにおける基準上体姿勢(傾斜角およびヨー角)及びその角速度が該目標上体姿勢角の初期状態として決定される。なお、本実施形態では、上体3の傾斜角に係る基準上体姿勢は鉛直姿勢であるので、目標上体姿勢のうちの傾斜角の初期状態(傾斜角及びその角速度)は0である。
また、本実施形態では、定常歩容の目標足平位置姿勢軌道、床反力鉛直成分軌道、及び目標ZMP軌道は、それぞれ、図15のフローチャートで決定した足平軌道パラメータ、床反力鉛直成分軌道パラメータ、ZMP軌道パラメータにより互いに独立的に決定される。例えば定常歩容の瞬時瞬時の目標足平位置姿勢は、床反力鉛直成分の瞬時値に依存することなく足平軌道パラメータに応じて決定される。
次いでS202において初期上体水平位置速度の候補(すなわち初期時刻Tsでの上体水平位置速度候補)である(Xs,Vxs)(Xs:水平位置、Vxs:水平速度)が仮決めされる。ここで仮決めする候補(Xs,Vxs)は、任意でよいが、例えば前回歩容の生成時に求めた定常歩容の初期状態における上体水平位置速度を仮決め候補(Xs,Vxs)とすればよい。
尚、説明を簡略にするため、サジタルプレーン上でX方向(前後方向)での定常歩容の初期状態を探索する場合を例にとるが、実際には位置・速度ともX方向(前後方向),Y方向(左右方向)を別々にあるいは同時に定常歩容の初期状態(定常歩容の前記境界条件を満たす初期状態)を探索する必要がある。
補足すると、サジタルプレーン上では、ヨー回転やモーメント鉛直成分など鉛直軸まわりに関する概念は存在しないので、少なくともヨー回転やモーメント鉛直成分に関しては、3次元空間で演算される。
探索的な決定手法としては、擬似ヤコビアン(感度マトリックス)を求め、最急降下法などにより次候補を決定する方法や、シンプレックス法などを用いれば良い。本実施形態では、最急降下法を用いる。
次いで、S204を経てS206において、定常歩容を繰り返した時に上体鉛直位置速度が連続になり、かつ、膝等の関節角が伸び過ぎたり縮み過ぎたりしないように、初期(時刻Ts)上体鉛直位置速度(Zs,Vzs)(Zs:鉛直位置、Vzs:鉛直速度)が、決定される。これについての詳細は、例えば本願出願人が先に出願したPCT/JP02/13592に記載されているので、ここでは省略する。
S206の処理の後、次いでS208に進み、定常旋回歩容が仮に生成される(この仮生成される定常旋回歩容を以降、仮歩容ということがある)。より具体的には、前記図13のS022で決定した定常歩容の歩容パラメータを基に、初期時刻Tsから終端時刻Teまでの各瞬間における目標ZMP、目標床反力鉛直成分、目標足平位置姿勢、基準上体姿勢、目標腕姿勢、床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲が逐次求められる。そして、求めた目標ZMPと目標床反力鉛直成分とに関する動力学的平衡条件、並びに床反力水平成分許容範囲の条件を満足するように、前記動力学モデル(図12のモデル)を用いて、前記上体水平位置速度(Xs,Vxs)、上体鉛直位置速度(Zs,Vzs)を上体3の初期(時刻Ts)状態として、上***置姿勢を逐次決定することによって、時刻Tsから終端時刻Teまでの歩容を生成する。また、この時、上体姿勢は、基準上体姿勢になるべく一致するように生成される。
また、床反力モーメント鉛直成分に関する条件、すなわち、床反力モーメント鉛直成分許容範囲を満足するように、逆位相腕振り動作が決定される。
尚、この定常歩容の歩容生成は歩容生成装置100の内部で行われるだけであって、実際のロボット1を駆動するための目標値として、後述する複合コンプライアンス動作決定部104に出力されることはない。
以下に、S208の処理である逐次演算による定常歩容生成処理の詳細を説明する。
図24はその処理を示すサブルーチンフローチャートである。
以下説明すると、S300において、諸々の初期化が行われる。具体的には、仮歩容生成用時刻kに初期時刻Tsを代入する。さらに、上体水平位置速度に、現在仮決定されている(Xs,Vxs)(図23のS202もしくは後述のS216もしくはS218で決定されたもの)が代入され、上体鉛直位置速度に、前記S206で求めた最新の(Zs,Vzs)が代入される。また、上体姿勢角には、基準上体姿勢角初期値(初期時刻Tsでの角度)が代入され、上体姿勢角速度には、基準上体上体姿勢角速度初期値(初期時刻Tsでの角速度)が代入される。
逆位相腕振り角には、基準初期逆位相腕振り角(初期時刻Tsでの角度)逆位相腕振り角速度には、基準初期逆位相腕振り角速度(初期時刻Tsでの角速度)が代入される。
次いで、S302を経てS304において、仮歩容生成用時刻kが歩容終端時刻以前であるか否か(k ≦ Ts + Tcycであるか否か)が判断され、その判断結果がYESである場合には、S306の歩容瞬時値決定サブルーチンに進み、歩容瞬時値が決定される。次いで歩容生成装置100の処理は、S308に進み、仮歩容生成用時刻kをΔkだけ増やした後、S304に戻る。
ここで、Δkは、仮歩容生成のインターバルであり、通常、制御周期Δtと一致させておけば良い。なお、Δkは、仮歩容の動力学的精度を落としても良いならば、演算量を低減するために、ΔkをΔtよりも長く設定しても良い。
S304の判断結果がNOである場合には、S310に進む。以上の処理により、S310に進むまでに定常歩容(仮歩容)が初期から終端まで生成される。
以下に、S306の歩容瞬時値決定サブルーチンの詳細を図25を参照して説明する。
まず図25のS400において、定常歩容パラメータ(床反力鉛直成分軌道パラメータ)を基に、前記図19に示した目標床反力鉛直成分の時刻kにおける値(今回値)が求められる。さらに、S402において、定常歩容パラメータ(ZMP軌道パラメータ)を基に、前記図22に示した目標ZMP軌道の時刻kにおける値(今回値)が求められる。
次いでS404に進み、定常歩容パラメータ(足平軌道パラメータ、基準上体姿勢軌道パラメータ、及び腕姿勢軌道パラメータ)を基に、時刻kにおける目標両足平位置姿勢(支持脚側及び遊脚側の両者の目標足平位置姿勢)、基準上体姿勢および基準腕姿勢の値(今回値)が求められる。基準腕姿勢に関しては、具体的には、両腕体5,5の全体重心位置(上体3に対する相対位置)、左右の手先(両腕体5,5の先端部)の左右方向の間隔および逆位相腕振り角の値(今回値)が求められる。
尚、目標足平位置姿勢の今回値(時刻kでの値)は、前記図23のS200で初期時刻Tsでの足平位置姿勢を求めた場合と同様に求められる。
次いでS406に進み、目標床反力鉛直成分を満足する(ロボット1の鉛直方向の慣性力と重力との和を目標床反力鉛直成分に釣り合わせる)ような時刻kにおける全体重心鉛直位置速度の値(今回値)が算出される。具体的には、例えば前記図12の動力学モデルに係わる前記式01と式04とに基づいて、全体重心鉛直位置速度が算出される。すなわち、式01と式04とによれば、ロボット1の運動による全体重心鉛直加速度と重力加速度との和にロボット1の全体質量を乗算したものが床反力鉛直成分に等しいという関係式(ロボット1の全体重心の鉛直方向に関する運動方程式)が得られる。従って、この関係式と目標床反力鉛直成分とから、全体重心鉛直加速度が求められる。
尚、この関係式自体は、ロボット1のモデルに依存することなく一般的に成立するものである。そして、この求めた全体重心鉛直加速度を積分することにより、全体重心鉛直速度が算出され、さらにその全体重心鉛直速度を積分することにより、全体重心鉛直位置が算出される。これらの算出は、より一般的には、次式15および式16で表される動力学関係式(ニュートンの運動方程式を離散化した式)を用いて行なわれる。
時刻kにおける全体重心鉛直速度
=時刻(k−Δk)における全体重心鉛直速度
+((床反力鉛直成分/ロボットの全体質量)+重力加速度)*Δk
(ただし、重力加速度は負の値とする。)
…式15
時刻kにおける全体重心鉛直位置
=時刻(k−Δk)における全体重心鉛直位置
+時刻kにおける全体重心鉛直速度*Δk
…式16
次いでS408に進み、全体重心鉛直位置を満足する上体鉛直位置が算出される。具体的には、例えば前記図12のモデルに係わる式04を用いて上体鉛直位置が算出される。すなわち、支持脚側及び遊脚側の目標足平位置姿勢の今回値から、図12のモデルの支持脚質点2m及び遊脚質点2mの鉛直位置が求められる。そして、これらの求めた支持脚質点2m及び遊脚質点2mの鉛直位置と、S407で求めた全体重心鉛直位置の今回値とを式04に適用することで、上体質点3mの鉛直位置が求められる。さらに、この求めた上体質点3mの鉛直位置と目標上体姿勢角の現在値(S404で設定された基準上体姿勢角または後述のS414で決定された前回(時刻k−Δk)の目標上体姿勢角)とから上体鉛直位置が求められる。
次いでS410に進み、前記図15のS108で決定した歩容パラメータ(床反力水平成分許容範囲を規定するパラメータ)に基づいて、図20に示した床反力水平成分許容範囲[Fxmin,Fxmax]の時刻kにおける値(今回値)が求められる。
次いでS411に進み、前記図15のS109で決定した歩容パラメータ(床反力モーメント鉛直成分許容範囲を規定するパラメータ)に基づいて、図21に示した床反力モーメント鉛直成分許容範囲[Mzmin,Mzmax]の時刻kにおける値(今回値)が求められる。
次いでS412に進み、目標ZMPに関する動力学的平衡条件(ロボット1の慣性力と重力との合力が目標ZMPまわりに発生するモーメントの水平成分が0であるという条件)を満足するように、目標上体水平加速度と目標上体姿勢加速度との今回値が決定される。ただし、床反力水平成分Fxが[Fxmin,Fxmax]を越えないように、上体水平加速度と上体姿勢角加速度(より詳しくは、上体傾斜角加速度)とが決定される。また、床反力モーメント鉛直成分Mzが[Mzmin,Mzmax]を越えないように、目標逆位相腕振り角加速度の今回値が決定される。
尚、上体姿勢角のうち、ヨー角は基準上体姿勢角のうちのヨー角に一致するように決定される。また、目標腕姿勢に関しては、逆位相腕振り角以外の成分は、基準腕姿勢に一致するように決定される。また、この際、上記条件を満足しつつも、目標上体傾斜角と目標逆位相腕振り角とは、それぞれ基準上体傾斜角と基準逆位相腕振り角とになるべく追従するように決定される。以下にこの詳細を説明する。
この時点において、前記の如く足平位置姿勢および上体鉛直位置の瞬時値(今回値)が決定されており、また、腕姿勢に関して、逆位相腕振り角以外の成分は、基準腕姿勢のそれに一致するように決定されている。このため、残りの上体水平位置と上体姿勢角と逆位相腕振り角とを決定すれば、ロボット1の目標運動は一義的に決定できる。したがって、すべての床反力も一義的に決定される。なお、本実施形態では、定常歩容の目標床反力鉛直成分及び目標ZMPは、それぞれ前記図13のS022で決定した床反力鉛直成分軌道パラメータ及び目標ZMP軌道パラメータによって規定される。
ところで、歩容生成において、前記上体並進モードをあまり用いずに主に上体傾斜モードを用いることにより、目標ZMPを満足させる(目標ZMPまわりの床反力モーメントの水平成分を0にする)と、上体姿勢角が大きく振れ過ぎてしまう恐れがある。したがって、これを防ぐためには、なるべく上体並進モードを用いるべきである。しかし、上体並進モードは、床反力水平成分の変化を伴うので、床反力水平成分許容範囲が狭い場合には、上体並進モードを強く働かせるとスリップする恐れがある。この場合には、上体傾斜モードに頼るしかない。特に、前記走行歩容のように床反力水平成分許容範囲が0となる期間では、床反力水平成分を発生させるような歩容を生成することはできないので、上体傾斜モードに頼らざるを得ない。
一方、逆位相腕振り運動は、目標ZMPまわりの床反力モーメントの水平成分および床反力水平成分のいずれも変化させることなく、床反力モーメント鉛直成分のみを変化させることができるので、床反力モーメント鉛直成分が前記床反力モーメント鉛直成分許容範囲を越えないようにするために用いることができる。以上のことを考慮し、本実施形態では、図26に示すフローチャートに従って、上体水平加速度と上体姿勢角加速度と逆位相腕振り加速度とが決定される。尚、理解の便宜上、上体水平加速度と上体姿勢角加速度(上体3の傾斜角の角加速度)の決定に関しては、サジタルプレーン上でX方向(前後方向)での上体水平加速度および上体姿勢角加速度を決定する場合を例にとって説明するが、実際にはY方向(左右方向)の上体水平加速度および上体姿勢角加速度もX方向と同様に決定される。
まずS500において、目標上体ヨー角に時刻kでの基準上体ヨー角の値を代入する。また、腕姿勢の逆位相腕振り角・角速度成分を除き、目標腕姿勢に時刻kでの基準腕姿勢の値を代入する。
次にS502において現在時刻(定常歩容作成用タイマーの値)kが、上体姿勢角・逆位相腕振り角復元期間(定常歩容の場合、上体姿勢角・逆位相腕振り角復元期間は、時刻Tmから時刻Ts2の期間および時刻Tm2からTeまでの期間である。)の間にあるか否かが判定される。S502の判定結果がNOであるならば、S504に進み、YESであるならばS530に進む。
S504では、ロボット1の前回瞬時歩容状態(時刻k−1の歩容状態)から、仮に、上体傾斜モードの角加速度を0にして、上体並進モードの運動をロボット1にさせた場合に今回(時刻k)の目標ZMPを満足するために必要な上体水平加速度αtmpが求められる。このαtmpは、例えば前記図12の動力学モデルに係わる前記式03yを用いて求められる。より具体的には、例えば、現在時刻kまでに求められた目標足平位置姿勢の時系列値を用いて、現在時刻kにおける支持脚質点2m及び遊脚質点2mの鉛直加速度が求められると共に、現在時刻k(今回)の目標足平位置姿勢を用いて支持脚質点2m及び遊脚質点2mの鉛直位置が求められる。また、現在時刻k(今回)の床反力鉛直位置を用いて上体質点3mの鉛直位置が求められると共に、現在時刻kまでに求めた目標上体鉛直位置の時系列値を用いて現在時刻kにおける上体質点3mの鉛直加速度が求められる。そして、これらの求めた値を前記式03yに代入すると共に、同式03yのMy、d2θby/dt2を0にしてなる式を、d2Xb/dt2について解くことにより、上体質点水平加速度が上体水平加速度αtmpとして求められる。尚、より厳密な動力学モデルを用いて、目標ZMP回りの床反力モーメントの水平成分を零にするような上体水平加速度αtmpを探索的に求めるようにしてもよい。また、本実施形態では、上体3の傾斜角に関する基準上体姿勢が鉛直姿勢で、該基準上体姿勢による上体姿勢角加速度(上体3の傾斜角の角加速度)が0であるため、上体傾斜モードの角加速度を0にして上体水平加速度αtmpを求めるようにした。但し、基準上体姿勢のうちの傾斜角が変化するように該基準上体姿勢軌道パラメータを設定した場合で、それにより定まる現在時刻kにおける基準上体姿勢角加速度(上体3の傾斜角の基準角加速度)が0でない場合には、上体傾斜モードの角加速度をその0でない基準上体姿勢角加速度の値にして、上体水平加速度αtmpを動力学モデルを用いて求める(例えば式03yのd2θby/dt2を0でない基準上体姿勢角加速度に設定して上記と同様に上体水平加速度αtmpを求める)ようにすればよい。
次に、S506に進み、上体水平加速度がαtmpの場合の時刻kにおける床反力水平成分Fxtmpが動力学モデルを用いて求められる。本実施形態においては、前記動力学モデルの式02xを用いてFxtmpが求められる。すなわち、次式17によってFxtmpが求められる。但し、d2Xsup/dt2およびd2Xswg/dt2はそれぞれ時刻kにおける支持脚足平質点水平加速度、遊脚足平質点水平加速度を表す。
Fxtmp = mb*αtmp+msup*d2Xsup/dt2
+mswg*d2Xswg/dt2 …式17
このようにして求められるFxtmpの例を図27に示す。図27に、Fxtmpが床反力水平成分許容範囲[Fxmin,Fxmax]を越えている部分を斜線で示す。
次に、S508に進み、上体並進モードの上体水平加速度α、これによって発生する床反力水平成分Fx、上体傾斜モードの上体角加速度βが次のように決定される(S508〜S516)。
すなわち、
Fxtmp>Fxmaxならば、S510に進み、次式によりFxが決定される。
Fx=Fxmax …式18
Fxtmp<Fxminならば、S512に進み、次式によりFxが決定される。
Fx=Fxmin …式19
それ以外、すなわち、Fxtmpが、床反力水平成分許容範囲[Fxmin,Fxmax]内に存するならば、S514に進み、次式によりFxが決定される。
Fx=Fxtmp …式20
いずれの場合においても、次いでS516に進み、次式により上体水平加速度α、上体姿勢角加速度(上体傾斜角加速度)βが決定される。
α=αtmp+(Fx−Fxtmp)/ΔFp …式21
β=(αtmp−α)*ΔMp/ΔMr …式22
ただし、ΔFp、ΔMpおよびΔMrはそれぞれ前記式06、07、および式09により決定される。
補足すると、より動力学演算の精度を高めようとするならば、上記のごとく上体角加速度βを求めた後、上体並進モードと上記求めた上体角加速度βの上体傾斜モードとを合成した運動が目標ZMPを満足するように、上体並進モードの上体水平加速度αを、より厳密な動力学モデルを用いて解析的あるいは探索的に決定した方が良い。なお、探索的な決定法としては、擬似ヤコビアン(感度マトリックス)を求め、擬似ニュートン法などにより次候補を決定する方法や、シンプレックス法などを用いれば良い。
さらに床反力水平成分Fxが床反力水平成分許容範囲[Fxmin,Fxmax]を厳密に越えないようにするために、S510においては、Fx=Fxmaxかつ目標ZMPまわりの床反力モーメントの水平成分が0になるように、S512においては、Fx=Fxminかつ目標ZMPまわりの床反力モーメントの水平成分が0になるように、上体水平加速度αと上体角加速度βとの組を探索的に求めても良い。
上記のように求められるFxを図28に示す。Fxは、Fxtmpの値を床反力水平成分許容範囲[Fxmin,Fxmax]から越えないように制限(飽和)させたものになっている。より詳しくは、上体並進モードのみによる上体水平加速度αtmpに応じたFxtmpが許容範囲[Fxmin,Fxmax]内にある場合には、FxtmpがそのままFxとなる。また、上体並進モードのみによる上体水平加速度αtmpに応じたFxtmpが許容範囲[Fxmin,Fxmax]の上限を超え、もしくは下限を下回った場合には、それぞれFxは、それぞれ強制的にFxmax、Fxminに制限される。特に走行歩容の空中期では、常に、Fxmax=Fxmin=0であるので、Fx=0となる。
また、上記のように求められる上体姿勢角加速度βを図29に示す。上体並進モードによって発生するFxが許容範囲[Fxmin,Fxmax]を越えないように、上体並進モードの加速度を制限したことによって不足した床反力モーメント(より詳しくは目標ZMP回りの床反力モーメントの水平成分を0にする上で必要な慣性力モーメントから、制限した上体水平運動と両脚体2,2の運動とによるモーメント成分を差し引いたモーメント)を、上体傾斜モードで補ったことになる。なお、走行歩容の空中期では、上体並進モードによる上体水平加速度αは常に0に制限されることとなるので、上体傾斜モードによる上体姿勢角加速度βのみによって、上記不足分の床反力モーメントが補われることとなる。
次に、S518に進み、仮に上体並進モードの上体水平加速度がα、上体傾斜モードの上体角加速度(上体傾斜角加速度)がβ、上体ヨー回転モードの上体加速度(上体ヨー角加速度)が基準ヨー角加速度d2θbzref/dt2、逆位相腕振り角加速度βaが基準逆位相腕振り角加速度d2θazref/dt2である運動をさせた場合の床反力モーメント鉛直成分Mztmpを求める。以降、d2θbzref/dt2をβbref、d2θazref/dt2をβarefとする。
具体的には、式03zに、式1001から式1004を代入することによって得られるMzがMztmpである。
d2Xb/dt2=αx …式1001
d2Yb/dt2=αy …式1002
d2θbz/dt2=βbref …式1003
d2θaz/dt2=βaref …式1004
ただし、αxは、上体水平加速度αのX成分、αyは、上体水平加速度αのY成分を表す。また、Xb、Ybには時刻k−1における上体水平位置を代入し、Xzmp、Yzmp、Xsup、d2Ysup/dt2、Xswg 、d2Yswg/dt2には、時刻kの値を代入するものとする。
このようにして求められるMztmpの例を図32に示す。図32に、Mztmpが床反力モーメント鉛直成分許容範囲[Mzmin,Mzmax]を越えている部分を斜線で示す。
次に、S520に進み、逆位相腕振り角加速度βaが次のように決定される(S520〜S528)。
すなわち、
Mztmp>Mzmaxならば、S522に進み、次式によりMzが決定される。
Mz=Mzmax …式1018
Mztmp<Mzminならば、S524に進み、次式によりMzが決定される。
Mz=Mzmin …式1019
それ以外、すなわち、Mztmpが、床反力水平成分許容範囲[Mzmin,Mzmax]内に存するならば、S526に進み、次式によりMzが決定される。
Mz=Mztmp …式1020
いずれの場合においても、次いでS528に進み、次式により逆位相腕振り角加速度βaが決定される。
βa=βaref+(Mztmp−Mz)/ΔMaz …式1021
ただし、ΔMazは式09aにより決定される。
以下にS518からS528までの処理を解説する。
上記のように求められるMzは、逆位相腕振りを含めたロボット全体の運動による床反力モーメント鉛直成分を示している。
上記処理では、このMzが床反力モーメント鉛直成分許容範囲[Mzmin,Mzmax]を越えないように逆位相腕振り角加速度βaが決定されている。具体的には、Mzは、図33に示すように、Mztmpの値を床反力水平成分許容範囲[Mzmin,Mzmax]から越えないように制限(飽和)させたものに決定されている。より詳しくは、Mztmpが許容範囲[Mzmin,Mzmax]内にある場合には、MztmpがそのままMzとなり、Mztmpが許容範囲[Mzmin,Mzmax]の上限を越え、もしくは下限を下回った場合には、それぞれMzは、それぞれ強制的にMzmax、Mzminに制限される。特に走行歩容の空中期では、常に、Mzmax=Mzmin=0であるので、Mz=0となる。
Mzが床反力モーメント鉛直成分許容範囲[Mzmin,Mzmax]を越えないようにするために、逆位相腕振りによって発生すべきモーメント鉛直成分Mazは、(Mz−Mztmp)である。Maz(=Mz−Mztmp)を図34に示す。
逆位相腕振り角加速度βaは、Mazを逆位相腕振りの等価慣性モーメントΔMazで割ったものを基準逆位相腕振り角加速度βaref(基準逆位相腕振り角を2階微分した値)に加えることによって得られる。すなわち、βaは前記式1021で求められる。逆位相腕振り角加速度βaを図35に示す。
以上のごとくS504からS528までの処理では、逆位相腕振りを含めたロボット全体の運動によって発生する床反力モーメント鉛直成分Mzが許容範囲[Mzmin,Mzmax]を越えないように(逆位相腕振り角加速度を基準逆位相腕振り角加速度βarefに一致させた場合に発生する床反力モーメント鉛直成分Mztmpが許容範囲を越えた分を打ち消す(キャンセルする)ように)、逆位相腕振り角加速度βaが決定される。
補足すると、床反力モーメント鉛直成分Mzが床反力モーメント鉛直成分許容範囲[Mzmin,Mzmax]を厳密に越えないようにするために、S504からS528までの処理の代わりに、逆位相腕振り角加速度βaを、より厳密な動力学モデルを用いて解析的あるいは探索的に決定した方が良い。なお、探索的な決定法としては、擬似ヤコビアン(感度マトリックス)を求め、擬似ニュートン法などにより次候補を決定する方法や、シンプレックス法などを用いれば良い。
以上が、時刻kが上体姿勢角・逆位相腕振り角復元期間の間にない場合の処理である。
S502の判定結果がYESである場合には以下の処理が行われる。まず、S530に進み、ロボット1の前回瞬時歩容状態(時刻k−1の歩容状態)から、上体傾斜モードの角加速度を0にして、上体並進モードの運動をロボット1にさせた場合に今回(時刻k)の目標ZMPを満足するために必要な上体水平加速度αが求め、これを最終的な上体水平加速度と決定する。
次に、S532に進み、上記場合に床反力水平成分Fxを求める。
次に、S534に進み、上体姿勢角加速度(上体傾斜角加速度)βを0に決定する。なお、上体ヨー角加速度は、基準上体ヨー角加速度βbref(基準上体ヨー角を2階微分した値)に決定される。
次いで最後に、S536に進み、逆位相腕振り角加速度βaに、基準逆位相腕振り角加速度βaref(基準逆位相腕振り角を2階微分した値)を代入する。
以上が、S502の判定結果がYESである場合の処理である。すなわち、この場合には、上体姿勢角加速度(上体傾斜角加速度および上体ヨー角加速度)を基準上体姿勢角加速度に、逆位相腕振り角加速度を基準逆位相腕振り角加速度に一致させる。このようにしても運動によって発生する床反力が、床反力水平成分許容範囲および床反力モーメント鉛直成分許容範囲のいずれも越えないと予想されているので、このように決定しても問題ない。
S528またはS536の処理の後、図25のS414に進み、S412で求めた上体水平加速度を逐次積分する(時刻Tsから現在時刻kまで累積加算する)ことにより上体水平速度が求められ、さらにその上体水平速度を逐次積分する(時刻Tsから現在時刻kまで累積加算する)ことにより、上体水平位置(今回値)が求められる。また、S412で求めた上体姿勢角加速度を逐次積分する(時刻Tsから現在時刻kまで累積加算する)ことにより上体姿勢角速度が求められ、さらにその上体姿勢角速度を逐次積分する(時刻Tsから現在時刻kまで累積加算する)ことにより、上体姿勢角(今回値)が求められる。
次にS416に進み、S412で求めた逆位相腕振り加速度βaを逐次積分する(時刻Tsから現在時刻kまで累積加算する)ことにより逆位相腕振り速度が求められ、さらにこれを逐次積分する(時刻Tsから現在時刻kまで累積加算する)ことにより、逆位相腕振り角θaz(今回値)が求められる。
以上のごとく図24のS306の定常歩容瞬時値決定サブルーチンが実行された後、S308に進み、歩容生成用時刻kの値が歩容生成インターバルΔkだけ増加される。次いでS304に戻り、S304に示す条件が成立している限り、S306とS308の処理が繰り返され、S304に示す条件が成立しなくなったら、すなわち終端(時刻Te=Ts+Tcyc)まで仮歩容の生成が完了したら、S310に進む。
定常歩容では、それを繰り返したときに境界でロボット1の運動状態が不連続にならないように、初期上体姿勢角及びその角速度を決定する必要がある。
そのために、S310では、上体姿勢角速度を、時刻Teまでに、初期値(時刻Tsでの値)に戻すための上体姿勢角加速度を発生させる床反力モーメントのZMP換算値(以降、上体姿勢復元モーメントZMP換算値と呼び、ZMPrecと略す)のパターンを設定する。
これについて以下に詳細に説明する。
上体姿勢角・逆位相腕振り角復元期間(時刻Tmから時刻Ts2の期間および時刻Tm2からTeまでの期間)に、上体傾斜モードを用いて上体姿勢角加速度を発生させることにより、上体姿勢角速度を初期値(時刻Tsでの値)に戻すことを考える。このための上体姿勢角加速度パターンを、β(k)とする。なお、上記期間以外では、β(k)=0とする。
上体傾斜モードでは、上体姿勢角加速度β(k)を発生させようとすると、床反力モーメントβ(k)*ΔMrが発生する。この結果、その瞬間における床反力鉛直成分がFz(k)であるとすると、(目標ZMPでなく)運動から算出されるZMP(k)は、次式で求められるΔZMPだけずれる。
ΔZMP(k)=−β(k)*ΔMr/Fz(k) …式23
したがって、ΔMrのパターンとFz(k)のパターンが決まっていれば(既知であれば)、ΔZMP(k)のパターンを適当に設定することによって、式23を満足する上体姿勢角加速度パターンを発生させて、上体姿勢角速度を初期値(時刻Tsでの値)、すなわち基準上体姿勢軌道の初期(時刻Ts)状態における上体姿勢角速度に戻すことができる。
前記上体姿勢復元モーメントZMP換算値(ZMPrec)は、そのように適切に設定されたΔZMP(k)を意味する。前記式23を用いて上体姿勢復元モーメントZMP換算値を設定する際に、ΔMrは厳密には変動するが、近似的に一定値としても良い。定常歩容は仮に生成されるだけであり、実際のロボットをこの歩容に追従させるわけではないので、定常歩容の動力学的精度はあまり高くなくても良いからである。
図30にZMPrecの一例を示す。図30では、ZMPrecのパターンとして、時刻Tmから時刻Ts2の期間および時刻Tm2からTeまでの期間に台形パターンが形成されている。その台形部の折れ点の時刻は、時刻Tmと時刻Ts2との間の期間および時刻Tm2からTeまでの期間における目標ZMPパターンの折れ点の時刻(図22参照)に一致させてある。これは、後述するが、今回歩容の目標ZMPパターンの修正が簡単になるからである。
式23のΔZMP(k)にZMPrec(k)を代入することにより次式が得られる。
β(k)=−ZMPrec(k)*Fz(k)/ΔMr …式24
したがって、この式24で求められるβ(k)は、図31の実線で示すようになる。なお、図31の点線で示すものは、時刻Tsから時刻Tmまでの期間および時刻Tm2からTeまでの期間の上体姿勢角加速度(図29の実線示のもの)である。(以降、時刻kでの値であることが明らかな場合には、(k)を省略する場合がある。)
初期(時刻Ts)上体姿勢角は、初期(時刻Ts)基準上体姿勢角に一致させる。
また、初期上体姿勢角速度は、式37a、37bを満たすように決定する。
終端上体姿勢角−初期上体姿勢角
=床反力水平成分許容範囲を満たすように決定された上体姿勢角加速度の2階積分
+ZMPrecによって発生する上体姿勢角加速度の2階積分
+初期上体姿勢角速度*定常歩容の周期
……式37a
終端上体姿勢角速度−初期上体姿勢角速度
=床反力水平成分許容範囲を満たすように決定された上体姿勢角加速度の1階積分
+ZMPrecによって発生する上体姿勢角加速度の1階積分
……式37b
なお、式37a、37bのそれぞれの右辺第1項の積分期間は、時刻TsからTmまでの期間と、Ts2からTm2までの期間とを合わせた期間であり、右辺第2項の積分期間は、時刻TmからTs2までの期間とTm2からTeまでの期間とを合わせた期間である。
より具体的に説明すると定常歩容では、第1旋回歩容の支持脚座標系(次回歩容支持脚座標系)から見た初期状態姿勢角及び角速度が、それぞれ、次の第1旋回歩容の支持脚座標系(次次次回歩容支持脚座標系)から見た終端上体姿勢角および角速度に一致していなければならない。そこで、本実施形態では、初期(時刻Ts)上体姿勢角は、初期(時刻Ts)基準上体姿勢角の値に決定し、これと、これを定常歩容におけるロボット1のトータルの旋回角(鉛直軸回りの旋回角)に応じたマトリクス(回転座標変換のマトリクス)により次回歩容支持脚座標系から見た値に座標変換したものとをそれぞれ式37aの左辺の初期上体姿勢角、終端上体姿勢角に代入する。また、式37a,37bのそれぞれの右辺第1項の積分に係る上体姿勢角加速度は、前記図26のS518で求められたものが用いられる。
そして、式37a,37bの初期上体姿勢角速度と、式37a,37bの右辺第2項の積分に係るZMPrec(図30の台形状のパターン)の台形高さをと未知数として(但し、ZMPrecの台形パターンの折れ点の時刻は前記のごとくあらかじめ定める。また、第1旋回歩容のZMPrecの台形高さacyc1と第2旋回歩容のZMPrecの台形高さacyc2を同一値とする。)、それらの未知数を含む式37a,37bの連立方程式を解くことにより求められる初期上体姿勢角速度を新たな初期上体姿勢角速度として決定する。この場合、式37bの終端上体姿勢角速度は、未知数である初期上体姿勢角速度を、定常歩容の上記トータルの旋回角に応じたマトリクスにより次回歩容支持脚座標系から見た値に座標変換したものである。
次いでS312に進み、上体傾斜復元モーメントZMP換算値(ZMPrec)パターンを基に、これによる上体水平位置・速度への影響量を求め、終端上体水平位置・速度に加算する。
この処理について解説する。なお、その詳細は、本願出願人のPCT/JP02/13592にて説明されているので、ここでは簡略な説明に留める。
時刻TsからTmの期間および時刻Ts2からTeの期間において、上記のごとく上体傾斜復元モーメントZMP換算値(ZMPrec)パターンを発生するように上体姿勢角加速度βを変化させると、上体姿勢角加速度βは、次式のようになる。
β=−ZMPrec*Fz/ΔMr …式1025
上体傾斜復元モーメントを発生させない場合に目標ZMPを満足する上体水平加速度は、S532において求めたごとくαtmpであるが、上記のごとく上体姿勢角加速度βを変化させると、目標ZMPを満足するために必要な上体水平加速度αは、次式のようになる。
α=αtmp−(ΔMr/ΔMp)*β …式1026
式1025および1026より、
α=αtmp+ZMPrec*Fz/ΔMp …式1027
すなわち、上体傾斜復元モーメントZMP換算値(ZMPrec)によって、加速度が式1027の右辺第2項分だけ増加する。
式が線形であることを利用すると、上記のごとく上体傾斜復元モーメントZMP換算値(ZMPrec)パターンを発生するように上体姿勢角加速度βを変化させた場合の終端上体水平速度は、上体傾斜復元モーメントZMP換算値(ZMPrec)パターンを発生させなかった場合の終端上体水平速度、すなわちS414で求めた上体水平速度の終端値に、時刻TsからTeまでの(ZMPrec*Fz/ΔMp)の1階積分を加算したものとして求められる。また、上記のごとく上体傾斜復元モーメントZMP換算値(ZMPrec)パターンを発生するように上体姿勢角加速度βを変化させた場合の終端上体水平位置は、上体傾斜復元モーメントZMP換算値(ZMPrec)パターンを発生させなかった場合の終端上体水平位置、すなわちS414で求めた上体水平位置の終端値に、時刻TsからTeまでの(ZMPrec*Fz/ΔMp)の2階積分を加算したものとして求められる。
S312の処理を完了した後、S314に進み、逆位相腕振り角速度が初期と終端で一致するように逆位相腕振り復元角加速度(βarec)パターンを決定する。
具体的には、逆位相腕振り復元角加速度パターンを図36のように台形状に設定し、時刻TmからTs2までの期間の台形高さazcyc2と時刻Tm2からTeまでの期間の台形高さazcyc1を同一とし、時刻TsからTeまでのβarecの積分値と床反力モーメント鉛直成分Mzが許容範囲を越えないようにするための前記求めた逆位相腕振り加速度βaの積分値との和が0となるように台形高さazcyc1、azcyc2を決定する。なお、両区間の台形高さは、同一でなくても構わない。
補足すると、このように決定した逆位相腕振り復元角加速度パターンによって発生する床反力モーメント鉛直成分(Mazrec)は、図37のようになる。したがって、逆位相腕振りを含むロボットの運動によって発生する床反力モーメント鉛直成分Mzは、最終的には、図38に示すごとく、図32のMztmpと図34のMazと図37のMazrecとの和、すなわち、図33のMzと図37のMazrecとの和になる。時刻TmからTs2までの期間および時刻Tm2からTeまでの期間においては、台形状の復元モーメントが加算されるが、これらの期間は、許容範囲が十分に広い期間に設定されているので、逆位相腕振りを含むロボットの運動によって発生する床反力モーメント鉛直成分は、許容範囲を越えることはない。
次にS316に進み、定常歩容の初期(時刻Ts)逆位相腕振り角および角速度を決定する。
具体的には、初期逆位相腕振り角速度は、以下の式によって決定する。
初期逆位相腕振り角速度
=基準初期逆位相腕振り角速度
−(βarecが0の場合の逆位相腕振り角
+βarecパターンの2階積分)/Tcyc
…式1030
ただし、上式において、βarecが0の場合の逆位相腕振り角とは、S416において求めた逆位相腕振り角(時刻Teにおける逆位相腕振り角)のことである。また、βarecの2階積分とは、図36のように設定した逆位相腕振り復元角加速度の時刻TsからTeまでの2階積分値である。また、基準初期逆位相腕振り角速度は前記基準逆位相腕振り角速度(基準逆位相腕振り角θarefの1階微分値)の時刻Tsでの値である。
初期逆位相腕振り角は、基準初期逆位相腕振り角に一致させれば良い。または、最終的に決定された逆位相腕振り角加速度(すなわち、床反力モーメント鉛直成分Mzが許容範囲を越えないようにするための前記求めた逆位相腕振り加速度βaと復元角加速度βarecの和)と上記求めた初期逆位相腕振り角速度を基に、仮に初期逆位相腕振り角を基準初期逆位相腕振り角に一致させた場合に算出される腕振り角と基準逆位相腕振り角との差の平均値または前記差の最大値と最小値の平均値を求め、求めた平均値の2分の1を基準初期逆位相腕振り角から減じた値を最終的な初期逆位相腕振り角として決定しても良い。このようにすることで、算出される腕振り角と基準逆位相腕振り角との差の絶対値が大きくなり過ぎることを防止することができる。
時刻Ts、Tm、Ts2およびTm2を前記のごとく設定した理由のひとつは、時刻TmからTs2までの期間と時刻Tm2からTeまでの期間とにおいて、上体姿勢角速度を基準上体姿勢軌道の初期角速度に戻すように上体姿勢角加速度βを発生させても、床反力水平成分Fxが許容範囲[Fxmin,Fxmax]を越えないようにするためである。換言すれば、時刻TmからTs2までの期間と時刻Tm2からTeまでの期間とにおいては、床反力水平成分許容範囲が十分に大きいため、目標ZMPを満足しつつ、上体姿勢角速度を戻すように上体姿勢角加速度βを発生させても、床反力水平成分Fxは、許容範囲を越えることがない。
また時刻Ts、Tm、Ts2およびTm2を前記のごとく設定したもうひとつの理由は、時刻TmからTs2までの期間と時刻Tm2からTeまでの期間とにおいて、逆位相腕振り角速度を基準逆位相腕振り角軌道の初期角速度に戻すように逆位相腕振り角加速度βaを発生させても、床反力モーメント鉛直成分Mzが許容範囲[Mzmin,Mzmax]を越えないようにするためである。換言すれば、時刻TmからTs2までの期間と時刻Tm2からTeまでの期間とにおいては、床反力モーメント鉛直成分許容範囲が十分に大きいため、逆位相腕振り角速度を戻すように逆位相腕振り角加速度βaを発生させても、床反力モーメント鉛直成分Mzは、許容範囲を越えることがない。
以上のごとく図24のS316の処理が完了した後、図23のS210に進み、生成した歩容(仮定常歩容)の終端上体水平位置・速度を、その瞬間の支持脚に対応する支持脚座標系(図17のX''',Y''' ,Z'''座標系)から見た値に変換し、その値を(Xe,Vxe)とする(Xe:終端上体水平位置、Vxe:終端上体水平速度)。
次いでS212に進み、図示の如く、初期上体水平位置速度(Xs,Vxs)と終端上体水平位置速度(Xe,Vxe)との差を算出する。この差(Xs−Xe,Vxs−Vxe)を上体水平位置速度境界条件誤差(errx,errvx)と呼ぶ。定常歩容では、前記境界条件を満たす必要があるので、(Xs,Vxs)と(Xe,Vxe)とが一致していなければならない。従って、上体水平位置速度境界条件誤差(errx,errvx)が零またはほぼ零でなければならない。本実施形態では、以下のように、上体水平位置速度境界条件誤差(errx,errvx)がほぼ零となる(Xs,Vxs)が探索的に求められる。
次いでS214に進み、算出した上体水平位置速度境界条件誤差(errx,errvx)が、あらかじめ適宜設定された許容範囲内にあるか否かが判断される。尚、このように上体水平位置速度境界条件誤差の許容範囲を設定する代わりに、初期発散成分(Xs+Vxs/ω0)と終端発散成分(Xe+Vxe/ω0)との差、および初期収束成分(Xs−Vxs/ω0’)と終端収束成分(Xe−Vxe/ω0’)との差がそれぞれある許容範囲内にあるか否か判断するようにしても良い。ただしω0,ω0’は、前述したように、ある所定の値である。
S214の判断結果がNOであるときにはS216に進む。このS216では、(Xs,Vxs)の付近に複数(本実施形態では2つ)の初期値候補(Xs+ΔXs,Vxs),(Xs,Vxs+ΔVxs)が決定される。ここで、ΔXs,ΔVxsは、それぞれXs,Vxsに対する所定の微小変化量を意味している。そして、これらの初期値候補のそれぞれを上体水平位置速度の初期状態として、前記S208と同様の処理によって定常歩容が歩容パラメータを用いて生成される。さらに、その生成された定常歩容の終端上***置速度を、その瞬間の支持脚に対応する支持脚座標系(図17のX''',Y''' ,Z'''座標系)から見た値に変換した値(Xe+ΔXe1,Vxe+ΔVxe1),(Xe+ΔXe2,Vxe+ΔVxe2)が求められる。ここで、(Xe+ΔXe1,Vxe+ΔVxe1)は(Xs+ΔXs,Vxs)に対応する終端上***置速度を意味し、(Xe+ΔXe2,Vxe+ΔVxe2)は(Xs,Vxs+ΔVxs)に対応する終端上***置速度を意味している。なお、この場合の定常歩容(仮歩容)の生成処理では、上体水平位置速度以外の変数の初期状態(時刻Tsでの状態)は、例えば上体水平位置速度の初期値候補を(Xs,Vxs)とした場合と同一に設定すればよい。S216ではさらに、前記S210と同様の処理によって、各初期値候補とこれに対応する終端上***置速度との差、すなわち、各初期値候補(Xs+ΔXs,Vxs),(Xs,Vxs+ΔVxs)のそれぞれに対応する上体水平位置速度境界条件誤差が求められる。
次いでS218に進み、(Xs,Vxs)およびその付近の初期値候補(Xs+ΔXs,Vxs),(Xs,Vxs+ΔVxs)のそれぞれに対する上体水平位置速度境界条件誤差を基に、(Xs,Vxs)の次の初期値候補が探索法(擬似ヤコビアン(感度マトリックス)を求め、最急降下法などにより次候補を決定する方法や、シンプレックス法など)により決定される。すなわち、(Xs,Vxs)およびその付近の初期値候補(Xs+ΔXs,Vxs),(Xs,Vxs+ΔVxs)のそれぞれに対する上体水平位置速度境界条件誤差によって、上体水平位置及び上体水平速度をそれぞれ初期値候補(Xs,Vxs)から微小変化させたときの、上体水平位置速度境界条件誤差の変化度合いを示す感度マトリックスが求められ、その感度マトリックスに基づいて、上体水平位置速度境界条件誤差をより小さくするような初期値候補(Xs,Vxs)が新たに決定される。そして、このように上体水平位置速度の新たな初期値候補(Xs,Vxs)が決定された後、S206に戻る。
S214の判断結果がNOとなる限りは上記した処理(S206〜S218の処理)が繰り返される。なお、この場合、上体水平位置速度の新たな初期値候補(Xs,Vxs)に対応する定常歩容の生成処理(S208)におけるS300(図24参照)では、上体姿勢角速度の初期値は、基準上体姿勢角速度の初期値ではなく、上体水平位置速度の前回の初期値候補(Xs,Vxs)に対応するS208の処理におけるS310(図24参照)で決定したものに設定される。そして、S214の判断結果がYESになると、繰り返しループ(S204)を抜けてS220に進む。なお、S204の繰り返しループを抜けたときの直前に生成された仮定常歩容が、境界条件を満たす定常歩容として得られることとなる。
S220では、本来の初期時刻0(今回歩容の終端時刻)における初期上体水平位置速度(X0,V0)と初期時刻0における初期上体鉛直位置速度(Z0、Vz0)と初期時刻0における初期上体姿勢角および角速度が求められる。
具体的には、(X0,V0)と(Z0,Vz0)は、S310において決定された上体傾斜復元モーメントZMP換算値パターン、時刻Tsにおける定常歩容の初期上体姿勢角および角速度と、S204のループを抜けた後の時刻Tsにおける上体水平位置速度(Xs,Vxs)を基に、歩容の条件を満足するように歩容を生成した場合における、第2旋回歩容から第1旋回歩容に切り換わった瞬間の時刻、すなわち、時刻k=Tcyc(時刻Te−Ts)であるときに決定される上体水平位置速度と上体鉛直位置速度とを、それぞれ時刻Tcycから始まる一歩(すなわち2度目の第1旋回歩容)の支持脚に対応する支持脚座標系(図17のX''',Y''',Z'''座標系)から見た値に変換した値に決定される。同様に、初期状態姿勢角および角速度は、時刻k=Tcyc(時刻Te−Ts)であるときに決定される上体姿勢角及び角加速度を、時刻Tcycから始まる一歩(すなわち2度目の第1旋回歩容)の支持脚に対応する支持脚座標系(図17のX''',Y''',Z'''座標系)から見た値に変換した値に決定される。
次いでS222に進み、定常歩容初期発散成分q[0]が次式から求められる。
q[0]=X0+V0/ω0 …式40
ただしω0は、発散についての解説で述べたように、ある所定の値である。
次にS224に進み、定常歩容初期発散成分q[0]が今回歩容支持脚座標系から見た値に変換され、これがq”[0]として求められる。また、初期上体鉛直位置速度(Z0,Vz0)が今回歩容支持脚座標系から見た値に変換され、これが(Z0”,Vz0”)として求められる。
補足すると、(Z0”,Vz0”)は、第2旋回歩容の支持脚座標系(図17のX”,Y”,Z”座標系)から見た第2旋回歩容終端上体鉛直位置速度に一致している。また、q”[0]も第2旋回歩容の支持脚座標系(図17のX”,Y”,Z”座標系)から見た第2旋回歩容終端発散成分に一致している。したがって、これらの性質を用いて、(Z0”,Vz0”)およびq”[0]を算出するようにしても良い。
さらに、S226に進み、本来の初期時刻0(今回歩容の終端時刻)における初期逆位相腕振り角および角速度(θaz0,ωaz0)を求め、さらに、これを今回歩容の支持脚座標系から見た値である(θaz0",ωaz0")を求める。具体的には、(θaz0,ωaz0)は、S314およびS316において決定された逆位相腕振り復元角加速度パターン、定常歩容の初期(時刻Ts)逆位相腕振り角および角速度とを基に、歩容の条件を満足するように歩容を生成した場合における(より具体的には、上体姿勢角・逆位相腕振り角復元期間以外では、床反力モーメント鉛直成分が許容範囲を越えないように、逆位相腕振り角軌道を決定し、上体姿勢角・逆位相腕振り角復元期間では、前記基準逆位相腕振り角加速度βarefと逆位相腕振り復元角加速度βarecの和が発生するように逆位相腕振り角軌道を決定した場合における)第2旋回歩容から第1旋回歩容に切り換わった瞬間の時刻、すなわち、時刻k=Tcyc(時刻Te−Ts)であるときに決定される逆位相腕振り角および角速度を、時刻Tcycから始まる一歩(すなわち2度目の第1旋回歩容)の支持脚に対応する支持脚座標系(図17のX''',Y''',Z'''座標系)から見た値に変換した値に決定される。
以上により、図13のS024の処理、すなわち定常歩容の初期状態を求めるサブルーチン処理が終了する。
次いで、図13のS026に進み、今回歩容の歩容パラメータを決定(一部は仮決定)する。S026では、より具体的には、図39に示すフローチャートに従って、以下の処理が行われる。
まず、S600において、今回歩容の足平位置姿勢軌道が定常歩容の足平位置姿勢軌道につながるように、今回歩容の足平軌道パラメータが設定される。
具体的には、今回歩容初期遊脚足平位置姿勢(今回歩容遊脚足平位置姿勢の初期値)は、今回歩容支持脚座標系から見た現在遊脚位置姿勢(前回歩容終端遊脚位置姿勢)に設定される。今回歩容初期支持脚足平位置姿勢(今回歩容支持脚足平位置姿勢の初期値)は、今回歩容支持脚座標系から見た現在支持脚足平位置姿勢(前回歩容終端支持脚足平位置姿勢)に設定される。また、今回歩容終端遊脚足平位置姿勢は、今回歩容支持脚座標系から見た次回歩容支持脚座標系(今回歩容に係わる1歩目の遊脚着床位置姿勢の要求値)に対応して決定される。すなわち、今回歩容終端遊脚足平位置姿勢から遊脚足平22を床に接触させたまま、すべらないように該足平22をその底面のほぼ全面が接地するまで回転させたときの、該足平22の代表点が今回歩容支持脚座標系から見た次回歩容支持脚座標系の原点に合致するように今回歩容終端遊脚足平位置姿勢が決定される。
今回歩容終端では、支持脚足平22は離床して空中にある。支持脚足平22が離床してから後の軌道を決定するために、支持脚足平着地予定位置姿勢を設定する。支持脚足平着地予定位置姿勢は、今回歩容支持脚座標から見た次次回歩容支持脚座標(今回歩容に係わる2歩目の遊脚足平位置姿勢の要求値)に対応して設定される。より具体的には、支持脚足平着地予定位置姿勢は、その位置姿勢から、足平22を床に接触させたまま、すべらないように該足平22をその底面のほぼ全面が床に接地するまで回転させたときの該足平22の代表点が今回歩容支持脚座標から見た次次回歩容支持脚座標の原点に合致するように設定される。
今回歩容終端支持脚足平位置姿勢は、現在支持脚位置姿勢(今回歩容初期支持脚足平位置姿勢)から、次回歩容支持脚座標系に対応する足平着地予定位置姿勢(前記要求パラメータにおける2歩目の遊脚側足平着地位置姿勢の要求値)に至る足平位置姿勢軌道を、今回歩容終端まで前記有限時間整定フィルタにより生成することにより求められる。
次いで、S602に進み、今回歩容の基準上体姿勢軌道パラメータが、定常歩容の第1旋回歩容や第2旋回歩容と同様に決定される。ただし、今回歩容の基準上体姿勢軌道が前記定常歩容の基準上体姿勢軌道に連続してつながる(今回歩容終端での基準上体姿勢角及び角速度がそれぞれ定常歩容初期の基準上体姿勢角及び角速度に一致する)ように上記パラメータが設定される。なお、本実施形態では、傾斜角に関する基準上体姿勢は、今回歩容及び定常歩容のいずれでも定常的な鉛直姿勢である。
次いで、S604に進み、今回歩容の基準腕姿勢軌道パラメータが、定常歩容の第1旋回歩容や第2旋回歩容と同様に決定される。ただし、今回歩容の初期基準腕姿勢およびその変化率が基準腕姿勢および変化率の現在瞬間値に一致し、かつ、今回歩容の腕姿勢軌道が前記定常歩容の腕姿勢軌道に連続してつながるように上記パラメータを設定する。なお、ここで決定される腕姿勢軌道パラメータは、定常歩容パラメータの決定(図15のS104)の場合と同様、たとえば、両腕体5,5の全体の重心位置(上体3に対する相対位置)、左右の手先(両腕体5,5の先端)の左右方向の間隔および逆位相腕振り角に関するパラメータが決定される。尚、本実施形態では、両腕体5,5の全体の重心位置は、上体3に対して一定に維持されるように設定される。
次いで、S606に進み、今回歩容の床反力鉛直成分軌道パラメータが、定常歩容の第1旋回歩容や第2旋回歩容と同様、該パラメータにより規定される床反力鉛直成分軌道が前記図6のように実質的に連続な(値がステップ状に飛ばない)軌道になるように設定される。
ただし、床反力鉛直成分軌道パラメータは、今回歩容の全体重心鉛直位置速度と床反力鉛直成分軌道とのいずれもが、前記定常歩容と連続してつながるように決定される。
具体的には、まず、前記図13のS024の処理(定常歩容初期状態の決定処理)で最終的に求めた定常歩容の初期上体鉛直位置速度を今回歩容支持脚座標系から見た値に変換した値(Z0”,Vz0”)、すなわち、図23のS224で求めた(Z0”,Vz0”)等を基に、例えば前記式04(あるいはロボット1のキネマティクスモデル)を用いて、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直位置速度が求められる。具体的には、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直位置は、S224で求めた定常歩容の上体鉛直位置Z0”に対応する図12のモデルの上体質点鉛直位置と、定常歩容初期の各足平位置を今回歩容支持脚座標系から見た値に変換したものに対応する支持脚側及び遊脚側の脚体質点鉛直位置とを式04に代入することで求められる。また、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直速度は、S224で求めた定常歩容の上体鉛直速度Vz0”に対応する図12のモデルの上体質点鉛直速度と、定常歩容初期の各足平鉛直速度を今回歩容支持脚座標系から見た値に変換したものに対応する支持脚側及び遊脚側の脚体質点鉛直速度とを式04の両辺を微分してなる式に代入することにより求められる。なお、この初期全体重心鉛直位置速度の算出は、より厳密なモデルを用いて行なうようにしてもよい。
そして、このようにして求めた定常歩容の初期全体重心鉛直位置速度を、次式41a、41bの終端全体重心鉛直位置速度に代入すると共に、前回の目標歩容瞬時値(より厳密には、前回目標歩容の終端状態を今回歩容支持脚座標系に変換したもの)の全体重心鉛直位置および速度を、式41a、41bの初期全体重心鉛直位置および速度に代入し、式41a、41bの関係を満足するように、今回歩容の床反力鉛直成分パターン(より具体的にはパラメータ値)が決定される。ただし、式41a、41bにおける積分値は今回歩容の初期から終端までの期間の積分値とする。
終端全体重心鉛直位置−初期全体重心鉛直位置
=(床反力鉛直成分/ロボットの全体質量)の2階積分
+重力加速度の2階積分
+初期全体重心鉛直速度*1歩の時間
…式41a
終端全体重心鉛直速度−初期全体重心鉛直速度
=(床反力鉛直成分/ロボットの全体質量)の1階積分
+重力加速度の1階積分
…式41b
ただし、重力加速度は負の値とする。
より具体的には、まず、図6のような床反力鉛直成分パターンを規定する床反力鉛直成分パラメータ(折れ点の時刻等)のうちの、少なくとも2つのパラメータを独立な未知変数として、その未知変数の値を、式41a、41bからなる連立方程式を解いて決定する。
未知変数とする床反力鉛直成分パラメータには、例えば、図6の台形の高さ(床反力鉛直成分のピーク値)と幅(片脚支持期時間)とを選択すれば良い。この場合、図6の台形の両側部分の傾きは今回歩容周期等に応じてあらかじめ定めた値とし、あるいは、片脚支持期から空中期に移行する時刻を除く床反力鉛直成分パターンの折れ点の時刻を今回歩容周期等に応じてあらかじめ定めた値とする。補足すると、未知変数を1つとすると、一般的には式41a、41bの連立方程式を満足する解が存在しない。
次いで、S608に進み、床反力水平成分許容範囲[Fxmin,Fxmax](具体的には該床反力水平成分許容範囲のパターンを規定するパラメータ)が、定常歩容の第1旋回歩容や第2旋回歩容と同様に設定される。たとえば図40に示すようなパターンで床反力水平成分許容範囲が設定される。本実施形態では、先にS606で決定した床反力鉛直成分パターンに応じて前記式12に基づいて床反力水平成分許容範囲が設定される。
次いで、S610に進み、床反力モーメント鉛直成分許容範囲[Mzmin,Mzmax](具体的には該床反力モーメント鉛直成分許容範囲のパターンを規定するパラメータ)が、定常歩容の第1旋回歩容や第2旋回歩容と同様に設定される。たとえば図41に示すようなパターンで床反力モーメント鉛直成分許容範囲が設定される。本実施形態では、先にS606で決定した床反力鉛直成分パターンに応じて前記式1012に基づいて床反力モーメント鉛直成分許容範囲が設定される
次いで、S612に進み、今回歩容のZMP軌道(具体的にはZMP軌道を規定するパラメータで、軌道の折れ点の時刻や位置)が、定常歩容の第1旋回歩容や第2旋回歩容と同様、安定余裕が高くかつ急激な変化をしないように、前記図7に示すごとく設定される。ただし、今回歩容のZMP軌道が前記定常歩容のZMP軌道に連続してつながるように上記パラメータを設定される。つまり、今回歩容終端におけるZMPの位置が定常歩容初期のZMP位置に一致するように、ZMP軌道パラメータが決定される。この場合、走行歩容では、片脚支持期におけるZMP軌道の折れ点の時刻や位置の設定の仕方は、前述した定常歩容のZMP軌道パラメータの設定の仕方と同様でよい。そして、空中期における目標ZMP軌道が、空中期開始時から、定常歩容初期のZMP位置まで、直線的に連続して変化するようにZMP軌道パラメータを設定すればよい。
なお、S612で決定される今回歩容のZMP軌道パラメータは、仮決定されただけであり、後述するように修正される。そこで、上記のごとく設定した今回歩容のZMP軌道を、以降、今回歩容の仮目標ZMP軌道と呼ぶこととする。
最後に、S614に進み、上体姿勢角・逆位相腕振り角復元期間[Ta,Tb]が設定される。上体姿勢角・逆位相腕振り角復元開始時刻Taは、定常歩容の第2旋回歩容におけるTmに相当し、上体姿勢角・逆位相腕振り角復元修了時刻Tbは、定常歩容の第2旋回歩容におけるTs2に相当するものである。これらの時刻Ta,Tbの設定の仕方は、それぞれTmおよびTs2の設定の仕方と同様である。
図13の説明に戻って、上記のごとくS026に示す処理(今回歩容の歩容パラメータ決定処理)を行って後、次いでS028に進み、今回歩容の歩容パラメータ(ZMP軌道パラメータ)が修正され、また、逆位相腕振り角に関するパラメータが決定される。この処理では、上***置姿勢軌道を定常歩容に連続させ、もしくは近づけるべくZMP軌道パラメータが修正され、かつ、逆位相腕振り角が定常歩容の逆位相腕振り角軌道に収束するように今回歩容の逆位相腕振り角に関するパラメータが決定される。
図42はその処理を示すサブルーチン・フロー・チャートである。
まずS700を経てS702に進み、仮目標ZMPパターンとその他の今回歩容パラメータを基に、今回歩容終端時刻までの仮今回歩容が仮生成される。
S702では、図43に示すフローチャートに従って、以下の処理が行われる。
以下説明すると、S800において、諸々の初期化が行われる。具体的には、仮歩容生成用時刻kに0が代入される。また、前回目標歩容の終端状態(より具体的には、上体水平位置速度、上体鉛直位置速度、上体姿勢角及びその角速度、目標足平位置姿勢、目標腕姿勢などの歩容状態の終端値)を今回歩容支持脚座標系に変換したものを、今回歩容初期状態とする。
補足すると、目標腕姿勢は、目標逆位相腕振り角・角速度を含む。
次いで、S802を経てS804において、仮歩容生成用時刻kが今回歩容終端時刻Tcurr以前であるか否か(k≦Tcurrであるか否か)が判断され、その判断結果がYESある場合には、S806の今回歩容瞬時値決定サブルーチンに進み、今回歩容の時刻kの瞬時値が決定される。S806の歩容瞬時値決定サブルーチンでは、前述のS306と同様、図25に示すごとく仮歩容を生成する。ただし、歩容パラメータとして、定常歩容パラメータの代わりに今回歩容パラメータが用いられる。
次いでS808に進み、仮歩容生成用時刻kがΔkだけ増加され、S804に戻る。
S804の判断結果がNOである場合には、図43に示すフローチャートの処理が完了する。
以上の処理により、仮の今回歩容が初期から終端まで生成される。
次いでS704に進み、上記のごとくS702で求めた今回歩容終端での上体水平位置速度(Xe,Vxe)から、終端発散成分q0[k](k=Tcurr)が図示の式(前記式10)を用いて求められる。
次いでS706に進み、今回歩容終端発散成分q0[k]と定常歩容初期発散成分q”(図23のS224で求めたもの)との差である終端発散成分誤差errqが図示の式を用いて求められる。さらに、S708に進んで、その求めた終端発散成分誤差errqが許容範囲内(0近傍の範囲)にあるか否か判断する。
S708の判断結果がNOであるときはS710に進み、a=Δa(Δaは所定の微小量)とし、図44の関係に従って現在の仮目標ZMPパターンに台形状の修正を加えた目標ZMPを基に、前記S702と同様に、終端までの仮今回歩容を計算する。ここで、図44を参照して、aは、今回歩容終端発散成分を定常歩容初期発散成分にできるだけ一致させる(今回歩容の上体水平位置姿勢軌道を定常歩容の上体水平位置姿勢軌道に近づける)ために、仮目標ZMPを修正するための台形パターンの高さである。この場合、本実施形態では、仮目標ZMPの修正は、支持脚足平22の底面のほぼ全面が接地する期間(足底全面接地期間)、すなわち、床反力水平成分許容範囲が十分に広いものとなる期間において行なうようにしており、上記台形パターンの折れ点の時刻は、足底全面接地期間における仮目標ZMPの折れ点の時刻に合わせて設定されている。なお、S710でa=Δaとするのは、現在の仮目標ZMP軌道を上記台形パターンによって微小量、修正したときの前記終端発散成分誤差errqの変化を観測するためである。
上記のようにS710にて、a=Δaとして仮目標ZMP軌道を修正したときの仮今回歩容を終端まで生成した後、さらにS712に進み、S710で求めた仮今回歩容終端での上体水平位置速度(Xe1,Vxe1)を基に、この仮今回歩容における終端発散成分q1[k]が図示の式(前記式10)を用いて求められる。
尚、S710においてΔaは、本実施形態では、適宜に設定される微小量の定数としたが、以下に説明する繰り返し演算によって終端発散成分誤差errqが小さくなるにつれてΔaを減少させるように該Δaを設定するようにしても良い。ただし、定数にしておいても、数回の繰り返し演算で終端発散成分誤差errqを許容範囲内に収めることができる。
次いでS714に進み、パラメータ感度r(Δaに対する終端発散成分誤差の変化の割合)が図示の式から求められる。さらに、S716に進み、a=−errq/r、すなわちS706で求めた終端発散成分誤差errqを、S714で求めたパラメータ感度rで割った値を高さaとする台形パターンの修正量を図44の関係に従って仮目標ZMPパターンに加えることにより、仮目標ZMPパターンが修正される(新たな仮目標ZMPパターンが決定される)。
次いでS702に戻る。そして、S708の判断結果がNOである限りは、上記したS702〜S716の処理が繰り返され、S708の判断結果がYESになったときには、その繰り返しループ(S700)を抜けてS718に進む。
S718では、仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角との差、および仮今回歩容の終端上体姿勢角速度と定常歩容の初期上体姿勢角速度との差などを基に、今回歩容の上体姿勢角軌道が定常歩容の上体姿勢角軌道に近づくように、今回歩容の上体姿勢復元モーメントZMP換算値(ZMPrec)のパターンが決定される。ここで決定するZMPrecは、後述する今回歩容瞬時値の生成処理で、床反力水平成分許容範囲が十分に大きいものとなる期間(片脚支持期内の期間)において、上体姿勢角軌道を定常歩容につながらせる(近づける)ように上体姿勢角加速度を発生させても、今回歩容終端発散成分と定常歩容初期発散成分との一致性(S708の条件)を維持できるように、仮目標ZMPを修正するためのものである。
このZMPrecは、前記定常歩容生成処理で説明したものと同様に台形状のパターンであり、具体的には次のように決定される。すなわち、図30における第2旋回歩容の期間のZMPrecの台形パターンと同様に、今回歩容のZMPrecの台形パターンを設定し、台形の頂点(折れ点)の時刻は既知とする(より具体的には台形の折れ点の時刻を目標ZMPの折れ点時刻に合わせる)と共に、台形高さを未知数とし、以下のようにZMPrecの台形高さ(パラメータ)が求められる。ただし、ZMPrecの台形パターンが立ち上がり始める時刻をTaとし、台形パターンから0に戻る時刻をTbとする。
上記のように上体姿勢復元モーメントZMP換算値パターンの未知パラメータがひとつの場合には、今回歩容の終端において上体姿勢角と上体姿勢角速度の両方を定常歩容に連続につなげることは一般的にはできない。そこで、本実施形態では、複数歩に渡って徐々に、生成される歩容の状態が定常歩容の状態に近づけるように、未知パラメータを決定することとする。
補足すると、1歩容の中でのZMPrecパターンを複雑にし、未知パラメータを2つ以上にして、今回歩容の終端において上体姿勢角と上体姿勢角速度との両者を定常歩容に連続につなげても良いが、ZMPrecパターンがジグザグに大きく変動し過ぎる恐れがある。
以下では、算出原理を説明した後、算出手順を説明する。
前述のごとくS702でZMPrecパターンの台形高さを0として求めた仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角との差を求め、これをθerrとする。また、仮今回歩容の終端上体姿勢角速度と定常歩容の初期上体姿勢角速度との差を求め、これをvθerrとする。
ここで、ZMPrecパターンの台形高さをある値bcurrとして今回歩容が生成され、引き続いて、今回歩容と同様のアルゴリズムによって第1旋回歩容が生成されると想定する。ただし、第1旋回歩容の上体姿勢復元モーメントZMP換算値ZMPrecパターンは、図24のS310において求められた第1旋回歩容のZMPrecパターン(高さが前記acyc1である図30の台形パターン)とある値b1の和であるとする。
このようにして生成された歩容をZMPrec修正歩容と呼び、その終端(第1旋回歩容の終端)上体姿勢角と角速度をそれぞれθ1、vθ1とする。
S024における定常歩容の初期状態を求めるサブルーチンの処理が完了した時点で求められた本来の定常歩容(最終的にS310において決定された定常歩容初期上体姿勢角および角速度を初期値とし、ZMPrecパターンをS310において求められたパターン(高さがacyc1である図30の台形パターン)とした場合の定常歩容)の第1旋回歩容終端上体姿勢角と角速度をそれぞれθ1org、vθ1orgとする。
ここで、Δθ1、Δvθ1を次のように定義する。
Δθ1 =θ1−θ1org …式50
Δvθ1 =vθ1−vθ1org …式51
Δθ1およびΔvθ1は、ZMPrec修正歩容と本来の定常歩容を第1旋回歩容の終端まで生成した時点において、両者間における上体姿勢角の差及び角速度の差を意味する。もし、Δθ1およびΔvθ1が0になれば、ZMPrec修正歩容に引き続いて、今回歩容と同様のアルゴリズムによって、ZMPrecパターンの台形高さを前記acyc2として第2旋回歩容が生成されると、この歩容は、本来の定常歩容に一致する。
したがって、Δθ1およびΔvθ1が0となる今回歩容台形高さbcurr、第1旋回歩容台形高さb1を求め、求めたbcurrを今回歩容の台形高さとして最終的に決定すれば良い。
ところで、ロボット1の上体姿勢角に関する動力学モデルが図12に示すフライホイールFHx、FHyのような線形特性を持つことから、Δθ1およびΔvθ1は、今回歩容台形高さbcurr、第1旋回歩容台形高さb1、仮今回歩容の終端上体姿勢角と定常歩容の初期上体姿勢角の差θerr、仮今回歩容の終端上体姿勢角速度と定常歩容の初期上体姿勢角速度の差vθerrと以下の関係を持つ。
Δθ1=c11*bcurr+c12*b1+θerr+e1*vθerr …式52
Δvθ1=c21*bcurr+c22*b1+e2*vθerr …式53
だだし、c11、c12、c21、c22、e1、e2は、今回歩容、第1旋回歩容の歩容周期、上体姿勢復元モーメントZMP換算値ZMPrecパターンのパラメータ(特に時間に関するパラメータ)などによって一義的に決定される係数である。
以上の原理に基づき、算出手順として、まず、仮今回歩容と定常歩容との境界での上体姿勢角の差θerrと角速度の差Vθerrが求められる。
次いで、式52,53の係数であるc11、c12、c21、c22、e1、e2が、今回歩容、第1旋回歩容の歩容周期、上体姿勢復元モーメントZMP換算値ZMPrecパターンのパラメータ(特に時間に関するパラメータ)などを基に求められる。
次いで式52、式53の右辺が0となるように今回歩容台形高さbcurr、第1旋回歩容台形高さb1が決定される。すなわち、式52、式53の左辺を0とした連立方程式を解くことによって、bcurr、b1が求められる。
最後に、今回歩容の上体姿勢復元モーメントZMP換算値(ZMPrec)の台形パターンの台形高さが、上記求めた今回歩容台形高さbcurrに設定される。
次いでS720に進み、現在の仮目標ZMPパターン(S700の繰り返しループを抜けたときの仮目標ZMPパターン)に、S718で上記のように求めた上体姿勢復元モーメントZMP換算値パターンを加算してなるパターンが今回歩容の目標ZMPパターンとして決定される。なお、この処理は、S710においてΔaの高さの台形パターンを仮目標ZMPパターンに加える処理と同様である。
以下に仮目標ZMPパターンに上体姿勢復元モーメントZMP換算値パターンを加算した理由を述べる。
S700のループにおいて生成される仮今回歩容は、上体姿勢復元モーメントZMP換算値ZMPrecを0(ZMPrecの台形パターンの高さパラメータを0)にして生成されたものである。このようにしてS700のループで最終的に生成された仮今回歩容では、その上***置速度は、定常歩容に連続し、もしくは近づくが、上体姿勢角は、定常歩容の上体姿勢角からずれ、場合によっては発散してしまう。
S718で求められた上体姿勢復元モーメントZMP換算値パターンは、上体姿勢角の定常歩容に対するずれを0に近づけるための上体姿勢角加速度を発生させるものである。
しかし、S700のループで最終的に得られた仮目標ZMPパターンを修正することなく、S718で求められた上体姿勢復元モーメントZMP換算値パターンに応じた上体姿勢角加速度を発生させると、動力学的平衡条件(ロボットの重力と慣性力との合力が目標ZMPに作用するモーメントが鉛直成分を除いて0になること)を満足させるために、上体水平位置軌道を前記仮今回歩容の上体水平位置軌道からずらさざるを得なくなる。そこで、本実施形態では、上体水平位置軌道をS700のループで最終的に得られるものからずらさなくて済むように、ZMPrecによって仮目標ZMPパターンを修正することとした。
前記仮今回歩容の運動に加えて、S718で求めた上体姿勢復元モーメントZMP換算値パターンに対応する上体姿勢角加速度を発生させると、ZMP(運動によって発生する重力と慣性力の合力の鉛直成分を除くモーメントが0になる点)は上体姿勢復元モーメントZMP換算値だけずれる。したがって、逆に、仮目標ZMPパターンに上体姿勢復元モーメントZMP換算値パターンを加算したパターンを目標ZMPパターンとして、S718で求めた上体姿勢復元モーメントZMP換算値パターンに対応する上体傾斜モードの上体姿勢角加速度を発生させながら、目標ZMPパターンを満足する今回歩容を生成すれば、前記仮今回歩容と同じ上体並進運動となる。
以上が、仮目標ZMPパターンに上体姿勢復元モーメントZMP換算値パターンを加算したパターンを目標ZMPパターンとした理由である。
次いでS722に進み、仮今回歩容の終端逆位相腕振り角と定常歩容の初期逆位相腕振り角との差および仮今回歩容の終端逆位相腕振り角速度と定常歩容の初期逆位相腕振り角速度との差を基に、今回歩容の逆位相腕振り角軌道が定常歩容の逆位相腕振り角軌道に近づくように、逆位相腕振り復元角加速度パターンが決定される。このパターンの決定方法は、S718で上体姿勢復元モーメントZMP換算値パターンを決定する方法と、変数名が次のように異なること以外は、ほぼ同じである。
上体姿勢復元モーメントZMP換算値パターン→逆位相腕振り復元角加速度パターン
水平成分→モーメント鉛直成分
以下に詳細を説明する。ここで決定する逆位相腕振り復元角加速度パターンは、後述する今回歩容瞬時値の生成処理で、床反力モーメント鉛直成分許容範囲が十分に大きいものとなる期間(片脚支持期内の期間)において、逆位相腕振り角軌道を定常歩容につながらせる(近づける)ように修正するためのものである。
この逆位相腕振り復元角加速度パターンは、前記定常歩容生成処理で説明したものと同様に台形状のパターンであり、具体的には次のように決定される。すなわち、図36における第2旋回歩容の期間の逆位相腕振り復元角加速度の台形パターンと同様に、今回歩容の逆位相腕振り復元角加速度の台形パターンを設定し、台形の頂点(折れ点)の時刻は既知とする(より具体的には台形の折れ点の時刻を目標ZMPの折れ点時刻に合わせる)と共に、台形高さを未知数とし、以下のように逆位相腕振り復元角加速度の台形高さ(パラメータ)が求められる。ただし、逆位相腕振り復元角加速度の台形パターンが立ち上がり始める時刻をTaとし、台形パターンから0に戻る時刻をTbとする。
上記のように逆位相腕振り復元角加速度パターンの未知パラメータがひとつの場合には、今回歩容の終端において逆位相腕振り角と逆位相腕振り角速度の両方を定常歩容に連続につなげることは一般的にはできない。そこで、本実施形態では、複数歩に渡って徐々に、生成される歩容の状態が定常歩容の状態に近づけるように、未知パラメータを決定することとする。
補足すると、1歩容の中での逆位相腕振り復元角加速度パターンを複雑にし、未知パラメータを2つ以上にして、今回歩容の終端において逆位相腕振り角と逆位相腕振り角速度との両者を定常歩容に連続につなげても良いが、逆位相腕振り復元角加速度パターンがジグザグに大きく変動し過ぎる恐れがある。
前述のごとくS702で逆位相腕振り復元角加速度パターンの台形高さを0として求めた仮今回歩容の終端逆位相腕振り角と定常歩容の初期逆位相腕振り角との差を求め、これをθazerrとする。また、仮今回歩容の終端逆位相腕振り角速度と定常歩容の初期逆位相腕振り角速度との差を求め、これをvθzerrとする。
ここで、逆位相腕振り復元角加速度パターンの台形高さをある値bzcurrとして今回歩容が生成され、引き続いて、今回歩容と同様のアルゴリズムによって第1旋回歩容が生成されると想定する。ただし、第1旋回歩容の逆位相腕振り復元角加速度パターンは、図24のS314において求められた第1旋回歩容の逆位相腕振り復元角加速度パターン(高さが前記azcyc1である図36の台形パターン)とある値bz1の和であるとする。
このようにして生成された歩容を逆位相腕振り復元角加速度修正歩容と呼び、その終端(第1旋回歩容の終端)逆位相腕振り角と角速度をそれぞれθz1、vθz1とする。
S024における定常歩容の初期状態を求めるサブルーチンの処理が完了した時点で求められた本来の定常歩容(最終的にS314において決定された定常歩容初期逆位相腕振り角および角速度を初期値とし、逆位相腕振り復元角加速度パターンをS314において求められたパターン(高さがazcyc1である図36の台形パターン)とした場合の定常歩容)の第1旋回歩容終端逆位相腕振り角と角速度をそれぞれθz1org、vθz1orgとする。
ここで、Δθz1、Δvθz1を次のように定義する。
Δθz1 =θz1−θz1org …式1050
Δvθz1 =vθz1−vθz1org …式1051
Δθz1およびΔvθz1は、逆位相腕振り復元角加速度修正歩容と本来の定常歩容を第1旋回歩容の終端まで生成した時点において、両者間における逆位相腕振り角の差及び角速度の差を意味する。もし、Δθz1およびΔvθz1が0になれば、逆位相腕振り復元角加速度修正歩容に引き続いて、今回歩容と同様のアルゴリズムによって、逆位相腕振り復元角加速度パターンの台形高さを前記azcyc2として第2旋回歩容が生成されると、この歩容は、本来の定常歩容に一致する。
したがって、Δθz1およびΔvθz1が0となる今回歩容台形高さbzcurr、第1旋回歩容台形高さbz1を求め、求めたbzcurrを今回歩容の台形高さとして最終的に決定すれば良い。
ところで、ロボット1の逆位相腕振り角に関する動力学モデルが図12に示すフライホイールFHazのような線形特性を持つことから、Δθz1およびΔvθz1は、今回歩容台形高さbzcurr、第1旋回歩容台形高さbz1、仮今回歩容の終端逆位相腕振り角と定常歩容の初期逆位相腕振り角の差θzerr、仮今回歩容の終端逆位相腕振り角速度と定常歩容の初期逆位相腕振り角速度の差vθzerrと以下の関係を持つ。
Δθz1=cz11*bzcurr+cz12*bz1+θzerr+ez1*vθzerr
…式1052
Δvθz1=cz21*bzcurr+cz22*bz1+ez2*vθzerr
…式1053
だだし、cz11、cz12、cz21、cz22、ez1、ez2は、今回歩容、第1旋回歩容の歩容周期、逆位相腕振り復元角加速度パターンのパラメータ(特に時間に関するパラメータ)などによって一義的に決定される係数である。
以上の原理に基づき、算出手順として、まず、仮今回歩容と定常歩容との境界での逆位相腕振り角の差θzerrと角速度の差vθzerrが求められる。
次いで、式1052,1053の係数であるcz11、cz12、cz21、cz22、ez1、ez2が、今回歩容、第1旋回歩容の歩容周期、逆位相腕振り復元角加速度パターンのパラメータ(特に時間に関するパラメータ)などを基に求められる。
次いで式1052、式1053の右辺が0となるように今回歩容台形高さbzcurr、第1旋回歩容台形高さbz1が決定される。すなわち、式1052、式1053の左辺を0とした連立方程式を解くことによって、bzcurr、bz1が求められる。
最後に、今回歩容の逆位相腕振り復元角加速度の台形パターンの台形高さが、上記求めた今回歩容台形高さbzcurrに設定される。
図13に戻って、上記のごとくS028において今回歩容パラメータを修正した後、あるいはS016の判断結果がNOである場合には、S030に進み、今回歩容瞬時値が決定される。
S030では、そのサブルーチンである図45の処理が実行される。
図45のS1400からS1411までは、図25のS400からS411までと同一の処理が行なわれ、次に、S1412のサブルーチンである図46のS1000〜S1018の処理が行なわれる。
具体的には、まずS1000において、目標上体ヨー角に現在時刻での基準上体ヨー角の値を代入する。また、腕姿勢逆位相腕振り角・角速度を除き、目標腕姿勢に現在時刻での基準腕姿勢の値を代入する。
次いでS1002に進み、現在時刻が、上体姿勢角・逆位相腕振り角復元期間(時刻Taから時刻Tbの期間)の間にあるか否かが判定される。S1002の判定結果がNOであるならば、S1004に進み、YESであるならばS1006に進む。
S1004では、上体傾斜角・逆位相腕振り角復元期間でない場合の上体水平加速度α、上体角加速度β、逆位相腕振り角加速度βa算出処理(図26のS504からS528)と同様の処理を行う。
S1006に進む場合では、まず、S1006において、仮に上体並進モードの運動をさせた場合に今回(時刻k)目標ZMPを満足するために必要な上体水平加速度αtmpを求める。
次いでS1008に進み、現在時刻における上体傾斜復元モーメントZMP換算値パターンの瞬間値ZMPrecを、S718において決定した上体傾斜復元モーメントZMP換算値パターンに関するパラメータを基に算出する。
次いでS1010に進み、現在時刻における逆位相腕振り復元角加速度パターンの瞬間値βarecを、S722において決定した逆位相腕振り復元角加速度パターンに関するパラメータを基に算出する。
次いでS1012に進み、図示する式によって、上体傾斜モードの上体角加速度(上体傾斜角加速度)βが決定される。
次いでS1014に進み、図示する式によって、上体水平加速度αが決定される。
次いでS1016に進み、S1010で算出した逆位相腕振り復元角加速度パターンの瞬間値βarecと基準逆位相腕振り角加速度βaref(基準逆位相腕振り角を2階微分した値)の和を目標逆位相腕振り角加速度βaに代入する。
次いでS1018に進み、上体水平加速度がαの場合の床反力水平成分Fxを求める。
次いでS1414に進み、上体水平加速度と上体姿勢角加速度を積分して上体水平速度と上体姿勢角速度(上体傾斜角速度)を算出する。これをさらに積分して、上体水平位置と上体姿勢(上体傾斜角)を決定する。なお、上体姿勢のうちの上体ヨー角は、本実施形態では基準上体ヨー角に決定される。
次いでS1416に進み、逆位相腕振り加速度を積分して逆位相腕振り角速度を算出する。これをさらに積分して、逆位相腕振り角を決定する。
以上のごとく、図13のS030の処理が完了する。
次いでS032に進み、歩容生成用時刻tをΔtだけ増やし、S014に戻り、以上のごとく歩容生成を続ける。
以上が、歩容生成装置100における目標歩容生成処理である。
図4を参照して本実施形態に係る装置の動作をさらに説明すると、歩容生成装置100において、上記したように目標歩容が生成される。生成された目標歩容のうち、目標上***置姿勢(軌道)、目標腕姿勢(軌道)が、ロボット幾何学モデル(逆キネマティクス演算部)102に送出される。
また、目標足平位置姿勢(軌道)、目標ZMP軌道(目標全床反力中心点軌道)、および目標全床反力(軌道)(目標床反力水平成分と目標床反力鉛直成分)は、複合コンプライアンス動作決定部104に送られると共に、目標床反力分配器106にも送られる。そして、目標床反力分配器106で、床反力は各足平22R,22Lに分配され、目標各足平床反力中心点および目標各足平床反力が決定される。この決定された目標各足平床反力中心点および目標各足平床反力は複合コンプライアンス動作決定部104に送られる。
複合コンプライアンス動作決定部104から、機構変形補償付き修正目標足平位置姿勢(軌道)がロボット幾何学モデル102に送られる。ロボット幾何学モデル102は、目標上***置姿勢(軌道)と機構変形補償付き修正目標足平位置姿勢(軌道)を入力されると、それらを満足する脚体2,2の12個の関節(10R(L)など)の関節変位指令(値)を算出して変位コントローラ108に送る。変位コントローラ108は、ロボット幾何学モデル102で算出された関節変位指令(値)を目標値としてロボット1の12個の関節の変位を追従制御する。また、ロボット幾何学モデル102は、目標腕姿勢を満足する腕関節の変位指定(値)を算出して変位コントローラ108に送る。変位コントローラ108は、ロボット幾何学モデル102で算出された関節変位指令(値)を目標値としてロボット1の腕体の12個の関節の変位を追従制御する。
ロボット1に生じた床反力(詳しくは実各足床反力)は6軸力センサ50によって検出される。その検出値は前記複合コンプライアンス動作決定部104に送られる。
また、ロボット1に生じた実上体姿勢角偏差(目標上体姿勢と実上体姿勢(上体3の実際の姿勢)との偏差)のうちの姿勢傾斜角偏差θerrx,θerry(詳しくは目標上体姿勢のうちの鉛直方向に対する傾斜角に対する実上体姿勢のうちの鉛直方向に対する傾斜角の偏差で、ロール方向(X軸回り)の姿勢傾斜角偏差がθerrxであり、ピッチ方向(Y軸回り)の姿勢傾斜角偏差がθerryである)が姿勢センサ54を介して検出され、その検出値は姿勢傾斜安定化制御演算部112に送られる。この姿勢傾斜安定化制御演算部112で、ロボット1の実上体姿勢角を目標上体姿勢角に復元するための目標全床反力中心点(目標ZMP)まわり補償全床反力モーメント水平成分が算出されて複合コンプライアンス動作決定部104に送られる。
より具体的には、本実施形態では、補償全床反力モーメント水平成分Mdmdxyは、例えばPD制御則を用いて次式により決定される。
補償全床反力モーメント水平成分Mdmdxy
=Kθb*上体姿勢傾斜角偏差
+Kωb*上体姿勢傾斜角速度偏差
……d25
ここで、Kθb、Kωbは所定のゲインである。また、上体姿勢傾斜角速度偏差は、上体姿勢傾斜角偏差の時間微分値であり、目標上体姿勢傾斜角速度に対する実上体姿勢傾斜角速度の偏差を意味する。また、上体姿勢傾斜角偏差は、より詳しくは、ロボット1の上体3のロール方向(X軸回り)の姿勢傾斜角偏差と、ピッチ方向(Y軸回り)の姿勢傾斜角偏差とからなるベクトルである。
また、ロボット1に生じた前記実上体姿勢角偏差のうちのヨー角偏差θerrz(詳しくは実上体姿勢角偏差のうちのヨー方向(Z軸回り)の姿勢角偏差がθerrzである)が姿勢センサ54を介して検出され、その検出値はヨー安定化制御演算部113に送られる。このヨー安定化制御演算部113で、ロボット1の実上体ヨー角および/または角速度を目標上体ヨー角および/または角速度に収束させるための目標全床反力中心点(目標ZMP)まわり補償全床反力モーメント鉛直成分が算出されて複合コンプライアンス動作決定部104に送られる。複合コンプライアンス動作決定部104は、入力値に基づいて目標床反力を修正する。具体的には、目標全床反力中心点(目標ZMP)回りに補償全床反力モーメントが作用するように目標床反力を修正する。
より具体的には、本実施形態では、補償全床反力モーメント鉛直成分Mdmdzは、例えばPD制御則を用いて次式により決定される。
補償全床反力モーメント鉛直成分Mdmdz=Kθbz*上体ヨー角偏差
+Kωbz*上体ヨー角速度偏差
……d26
ここで、Kθbz、Kωbzは所定のゲインである。また、上体ヨー角速度偏差は、上体ヨー角偏差の時間微分値であり、目標上体ヨー角速度に対する実上体ヨー角速度の偏差を意味する。
複合コンプライアンス動作決定部104は、入力値に基づいて目標床反力を修正する。具体的には、目標全床反力中心点(目標ZMP)回りに補償全床反力モーメント水平成分が作用するように目標床反力モーメント水平成分を修正し、さらに、目標歩容に動力学的に釣り合う目標全床反力中心点(目標ZMP)回りの目標床反力モーメント鉛直成分に補償全床反力モーメント鉛直成分を付加的に追加することによって目標床反力モーメント鉛直成分を修正する。
複合コンプライアンス動作決定部104は、修正された目標床反力に、センサ検出値などから算出される実ロボット1の状態および床反力を一致させようと上記機構変形補償付き修正目標足平位置姿勢(軌道)を決定する。ただしすべての状態を目標に一致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥協的になるべく一致させる。すなわち、各目標に対する制御偏差に重みを与えて、制御偏差(あるいは制御偏差の2乗)の重み付き平均が最小になるように制御する。これにより、実際の足平位置姿勢と全床反力とが目標足平位置姿勢と目標全床反力とに概ね従うように制御される。
尚、この発明の要旨は歩容生成装置100におけるロボット1の歩容生成にあり、上記した複合コンプライアンス動作決定部104などの構成および動作は、本出願人が先に出願した特開平11−300661号公報などに詳細に記載されているので、説明を以上に留める。
S028において、今回歩容パラメータは、前述のごとく、今回歩容の終端発散成分が、定常旋回歩容の初期発散成分q[0]を今回歩容の支持脚座標系から見た値であるq”に一致するように修正される。
実は、発散成分とは、動力学モデルを用いて今回歩容パラメータに従って歩容を生成し、そのまま連続して定常旋回歩容パラメータに従って歩容を繰り返し生成したとき、生成した歩容の上体水平位置が定常旋回歩容に収束するか否かを評価する指標である。収束する場合にちょうど、今回歩容の終端発散成分が、定常旋回初期発散成分q[0]を今回歩容の支持脚座標系から見た値であるq”に一致するように、本来、発散成分は定義されなければならない。
式10で定義した発散成分は、実は、上記の性質を近似的に満足する発散成分である。
したがって、本実施形態においては、動力学モデルを用いて今回歩容パラメータに従って歩容を生成し、そのまま連続して定常旋回歩容パラメータに従って歩容を繰り返し生成したとき、生成した歩容の上体水平位置が定常旋回歩容の上体水平位置に収束する(近づく)ように、今回歩容パラメータを修正した、と言える。但し、厳密には、今回歩容直後の第1旋回歩容だけは、前記の如く求めた第1旋回歩容台形高さb1,bz1によって修正された歩容にする必要がある。別の言い方をすれば、今回歩容と第1旋回歩容とを合わせた歩容を今回歩容と見なせば、本参考例においては、上記の如く歩容を繰り返し生成したとき、生成した歩容の上体姿勢角が第2旋回歩容と第1旋回歩容とからなる定常歩容の上体姿勢角に収束し(近づき)、もしくは一致するように、今回歩容パラメータを修正した、と言える。
これは、PCT公開公報WO/02/40224のものと同様である。
特に、本実施形態においては、その条件(今回歩容が定常歩容に近づく)を満足するように、今回歩容の歩容パラメータのうちの目標ZMPパターンを修正するようにした。図47を参照して説明すると、同図に符合Bで示す軌道は、上記したように歩容の境界において発散成分が一致するように生成した上体水平位置軌道を示す。
同図に符合Aで示す軌道は、定常旋回歩容との境界における上体水平位置速度が一致するように今回歩容を生成し、続いて定常歩容を繰り返し生成した場合の上体水平位置軌道を示す。
図示の如く、符合Bで示す軌道は、今回歩容と最初の定常旋回歩容との境界においては、一般的に符合Aで示す軌道からずれているが、その後、徐々に符合Aで示す軌道に収束し(近づき)、次の定常旋回歩容時期において符合Aで示す軌道にほぼ一致する。このように、歩容境界で発散成分のみを一致させる歩容生成手法でも、歩容境界で位置速度両方を一致させる歩容生成手法と同様、歩容の発散を防止することができる。尚、同図に符合Cで示す例は、それらを考慮せずに軌道を生成した場合の例を示す。このような場合は、生成した軌道は経時的に発散してしまう。当然、目標ZMPパターンを複雑なものにし、複数のパラメータを調整して、位置速度両方を一致させても良いが、目標ZMPパターンがジグザグになる恐れがある。なお、位置速度両方を一致させれば、発散成分も一致するから、位置速度両方を一致させる方法も発散成分を一致させる方法の特殊例であると言える。
またさらに、本実施形態においては、動力学モデルを用いて今回歩容パラメータに従って歩容を生成し、そのまま連続して定常旋回歩容パラメータに従って歩容を繰り返し生成したとき、生成した歩容の上体姿勢角が定常旋回歩容の上体姿勢角に収束し(近づき)、もしくは一致するように、今回歩容パラメータを修正した、と言える。但し、厳密には、今回歩容直後の第1旋回歩容だけは、前記の如く求めた第1旋回歩容台形高さb1,bz1によって修正された歩容にする必要がある。
次に、本実施形態の変形態様を以下にいくつか説明する。
本実施形態では、理解を容易にするために、床反力水平成分許容範囲は、前後方向(X軸方向)成分と左右方向(Y軸方向)成分それぞれ独立に設定できることとしたが、前後方向と左右方向の関係式で表した方が、よりスリップし難い歩容が生成される。
例えば、次式のようにいわゆる摩擦円を許容範囲としても良い。
(床反力水平成分のX成分)*(床反力水平成分のX成分)
+(床反力水平成分のY成分)*(床反力水平成分のY成分)
≦(ka*μ*Fz)*(ka*μ*Fz)
…式59
ただし、Fzは、目標床反力鉛直成分をμは摩擦係数、kaは1以下の正の定数を表す。
ただし、以上のように、床反力水平成分許容範囲を前後方向と左右方向の関係式で表す場合、許容範囲を同時にあるいは交互に満足するように、サジタルプレーン上での運動とラテラルプレーン上での運動を同時あるいは交互に求める必要がある。
また、床反力水平成分許容範囲と床反力モーメント鉛直成分許容範囲とを上記のごとく別々に設定する代わりに、床反力水平成分と床反力モーメント鉛直成分との組の許容範囲を設定するようにしても良い。床反力水平成分が大きくなるにつれ、床反力モーメント鉛直成分の摩擦限界は、小さくなる。また、床反力モーメント鉛直成分が大きくなるにつれ、床反力水平成分の摩擦限界は、小さくなる。したがって、このことを鑑みて床反力水平成分と床反力モーメント鉛直成分との組の許容範囲を設定するようにした方が、より現実の摩擦限界特性に近い許容範囲を設定することができる。具体的には、床反力水平成分の絶対値と床反力モーメント鉛直成分の絶対値との重み付き平均に対して許容範囲を設定しても良い。
尚、ロボットの全体重心鉛直加速度と重力加速度との和は床反力鉛直成分に比例するので、歩容パラメータとして、床反力鉛直成分軌道の代わりにロボットの全体重心鉛直加速度軌道を規定するパラメータを明示的に設定しても構わない。本質的に同じことである。また、ロボットの全体重心軌道に近い挙動をする部位の鉛直加速度軌道を規定するパラメータを明示的に設定しても構わない。例えば、脚体の質量が上体の質量に比べて十分に小さい場合には、上体鉛直加速度軌道とロボットの全体重心鉛直加速度軌道は、ほぼ同じか比例関係になるので、床反力鉛直成分軌道の代わりに上体鉛直加速度軌道を用いても良い。同様に、床反力水平成分とロボットの全体重心水平加速度は、比例関係にあるので、本実施形態における床反力水平成分およびその許容範囲の代わりに、ロボット全体重心水平加速度およびその許容範囲を用いても構わない。また、ロボットの全体重心水平軌道に近い挙動をする部位の水平加速度軌道に関するパラメータを明示的に設定しても構わない。例えば、脚体の質量が上体の質量に比べ十分に小さい場合には、上体水平加速度軌道とロボットの全体重心水平加速度軌道は、ほぼ同じか比例関係にあるので、床反力水平成分およびその許容範囲の代わりに上体水平加速度およびその許容範囲を用いても良い。
また、目標ZMPまわりの床反力モーメント鉛直成分と、目標ZMPまわりのロボット全体の角運動量変化率鉛直成分は、比例関係にあるので、本実施形態における床反力モーメント鉛直成分およびその許容範囲の代わりに、ロボット全体角運動量変化率鉛直成分およびその許容範囲を用いても構わない。
前記実施形態では、床反力水平成分と目標ZMPまわりの床反力モーメント水平成分を適切な値にするために、上体傾斜モードと上体並進モードとの2つの運動モードを用いたが、これら以外の運動モードを用いても良い。
例えば、図48のように、股関節を中心に上体姿勢を回転させると、全体重心まわりの角運動量が変化すると共に全体重心も変化する。この運動と前記上体並進モードをある所定の割合で重ね合わせると(合成すると)、前記上体傾斜モードとほとんど同じ運動になり、床反力水平成分が発生しなくなる。したがって、これを改めて前記上体傾斜モードとみなせば、本実施形態のアルゴリズムにしたがって、同様の歩容を生成することができる。
したがって、運動モードのひとつが床反力水平成分を発生しない運動モードである必要もない。いかなるモードの組み合わせであっても、床反力水平成分と目標ZMPまわりの床反力モーメントとの発生の割合が異なるふたつの運動モードを用いさえすれば、上記例のように、任意の床反力水平成分と目標ZMPまわりの床反力モーメントとを発生させることができるからである。
また、上体姿勢を変化させる運動モード以外の運動モードを用いても良い。ただし、なるべく小さい変位で大きな床反力水平成分または目標ZMPまわりの床反力モーメントが発生できる運動モードを選ぶことが好ましい。
例えば、左右の腕体の先端位置を同一方向に振りまわす運動モード、接地していない(空中に存在する)足平の位置を摂動させる運動モードでも良い。ただし、遊脚軌道を摂動させる場合には、着地位置が変わらないように着地直前までには、摂動量を実質的に0に戻すべきである。なお、左右の腕体の先端位置を同一方向に振りまわす運動モードと逆位相腕振りモードとを併用してもよいことはもちろんである。
また、3つ以上の運動モードを用いても良い。
また、選択した運動モードのうちの少なくとも2つは、その運動モードによる床反力水平成分と目標ZMPまわりの床反力モーメントとの発生比率が、互いに異なるものである必要がある。そうでないと、一般的に連立方程式の解がなくなる(各運動モードの挙動を一義的に決定できない)からである。
さらにできる限り、床反力水平成分をあまり変化させないで目標ZMPまわりの床反力モーメントを十分に大きく変化させることができる運動モードと、目標ZMPまわりの床反力モーメントをあまり変化させないで床反力水平成分を十分に大きく変化させることができる運動モードとを組み合わせることが望ましい。
言いかえると、全体重心をあまり変化させないで角運動量を十分に大きく変化させることができる運動モードと、角運動量をあまり変化させないで全体重心を十分に大きく変化させることができる運動モードとを組み合わせることが望ましい。このようにすると、運動モードの変位が小さくなるからである。
また、床反力モーメント鉛直成分が許容範囲を越えないようにするために(スピン力をキャンセルするために)、逆位相腕振りモードの代わりに、上体ヨー回転モードを用いても良い。上体ヨー回転モードを用いる場合には、ロボット1の上体3を腰寄りの部分(例えば図1の54の部材の下側の部分)とその上側の部分(例えば図1の54の部材の上側の部分)とに分割し、上側の部分が腰寄りの部分に対してヨー方向に(例えば上体3の体幹軸回り)に回転し得るように設けておくことが好ましい。このようにすると、脚体2,2の姿勢に影響を及ぼすことなく、上体3の上側の部分を回転させて、床反力モーメント鉛直成分を調整することができる。このようにした場合の実施形態は、例えば前記実施形態で逆位相腕振り角、その角速度およびその角加速度を決定する代わりに、上体3の上側の部分の回転角、その角速度およびその角加速度を決定するようにすればよい。この場合、両腕体5,5は、例えば上体3の上側の部分に対する相対位置を一定とすればよい。但し、上体3の上側の部分のヨー回転に加えて、両腕体5,5の逆位相腕振り動作を加えてもよいことはもちろんである。
また、床反力モーメント鉛直成分が発生するものであれば、腕、上体以外の部位を変位させる運動モードを用いても良い。
例えば、空中期において、両脚先端を前後逆方向に動かすモードを用いても良い。
あるいは、床反力モーメント鉛直成分が発生するいくつかの運動モードを併用しても良い。例えば、逆位相腕振りモードと上体ヨー回転モードを併用しても良い。
また、上体ヨー回転モードおよび逆位相腕振りモードは、全体重心位置が変わらないように(言い換えると床反力水平成分を発生させずに)床反力モーメント鉛直成分を発生させるモードであるが、全体重心位置が変わってしまう(言い換えると床反力水平成分を発生してしまう)運動でも良い。上体並進モードと組み合わせることにより、床反力水平成分を調整することができるからである。
前記実施形態に用いた動力学モデル(図12の動力学モデル)以外にも以下のモデルを用いても良い。
1)図49に示したように複数のリンクに質点を設定した非線形なモデル(多質点モデル)。このモデルの各リンクには、イナーシャ(慣性モーメント)が設定されていてもよい。
2)本願出願人によるPCT公開公報WO/02/40224に示された3質点モデル
3)上体ヨー回転モードあるいは逆位相腕振りモードによって生じる慣性力のモーメントを無視したモデル。
4)重力と慣性力との合力(またはこれと釣り合う床反力)と上体並進運動との関係を表す部分モデルと、上記合力と上体姿勢回転運動(上体傾斜運動と上体ヨー回転運動)の関係を表す部分モデルとを分離して持つ分離型モデル。例えば、図12に示す質点は、上記合力と上体並進運動との関係を表す部分モデルであり、図12に示すフライホイールは、上記合力と上体姿勢回転運動との関係を表す部分モデルである。
なお、上記1)〜4)のいずれの場合でも、慣性力のモーメント鉛直成分を発生させる運動モードは必要である。
また、各処理で用いられるモデルは同一でも良いし、処理に応じて適宜、モデルを使い分けても良い。例えば、前記定常歩容は今回歩容の終端状態を決定するためだけに生成されるものであるので、その定常歩容の動力学的な精度に対する要求は、今回歩容に比べて小さい。そこで、例えば、今回歩容の生成処理では、図12の動力学モデル(3質点+フライホイールのモデル)を用いる一方、定常歩容の生成処理(特に図21のS408、S412)では、各脚体2の質量を無視し、上体3に対応する上体質点3mと、フライホールFHx、FHy、FHaz、FHbzとで構成される動力学モデル(図12のモデルで脚質点2m,2mを除去した1質点+フライホイールのモデル)を用いて定常歩容を生成するようにしてもよい。この場合の定常歩容の生成処理では、前記実施形態において、脚質点2mの質量を0として前記図25のS408、S412の処理を行なうようにすればよい。このようにすると、定常歩容の生成処理における演算量を大幅に削減できる。
尚、前記実施形態において、ブロック線図、フローチャート、およびアルゴリズムなどは、演算処理順序を変えるなどの等価変形をしても良い。また、適宜ローパスフィルタを挿入しても良い。
また、前記実施形態を2足移動ロボットに関して説明してきたが、3足以上の多脚ロボットにも応用することができる。
前記実施形態のごとく、探索手法を用いたり、部分的に解析手法を用いたりして、定常歩容の初期状態(主に初期上体水平位置速度、初期上体鉛直位置速度および逆位相腕振り角・角速度のこと)を決定する手法の代わりに、あらかじめ、上記手法を用いて様々な定常歩容パラメータに対して計算しておき、定常歩容パラメータと定常歩容の初期状態との関係をマップ化あるいは近似式化して記憶して置き、実移動時には、マップ化あるいは近似式化された上記関係を基に、定常歩容の初期値を決定しても良い。
さらには、マップ化あるいは近似式化された上記関係と上記関数fを合成した関数をマップ化あるいは近似式化して記憶しても良い。すなわち、前記した足平軌道パラメータ、床反力鉛直成分軌道パラメータ等からなる定常歩容パラメータから、定常歩容の発散成分を直接求める関数をマップ化あるいは近似式化して記憶しても良い。例えば、複数種類の代表的な定常歩容パラメータの組のそれぞれに対して、事前に定常歩容を生成しておき、各組の定常歩容パラメータに対する定常歩容の初期状態(図13のS024で求めるもの)を求めておき、さらにその各組の定常歩容パラメータと、定常歩容初期状態との関係を表すマップをあらかじめ作成しておく。そして、目標歩容を生成する際に、決定された定常歩容パラメータの組から、上記マップに基づいて選択あるいは補間によって定常歩容初期状態を求めるようにすればよい。このようにすれば、今回歩容の生成毎に、定常歩容を生成する必要がなくなり、目標歩容生成処理の演算量を大幅に削減できる。
定常歩容につなげる(近づける)ための今回歩容の修正方法として、本実施形態では、今回歩容の目標ZMPパラメータを修正したが、それ以外のパラメータを修正しても良い。
たとえば、今回歩容の遊脚の空中での軌道を変えても良い。また、たとえば、上体水平位置が定常歩容よりも後方にずれてしまいそうな場合には、遊脚を離床後すばやく前方に移動させて遊脚重心位置を前方にずらす。こうすることにより、目標ZMPを満足するための上体水平位置は、前方により一層加速せざるを得なくなる。この結果、上体水平位置が今回歩容終端において、より一層前方に移動し、定常歩容に一致させることができる。
また、目標ZMPパラメータを修正する代わりに、今回歩容の周期を修正しても良い。たとえば、上体水平位置が定常歩容よりも後方にずれてしまいそうな場合には、今回歩容の周期を長くすれば良い。今回歩容の周期を長くことで、上体水平位置が移動する時間が延び、それだけ前方に移動できるからである。
ただし、探索手法によって、上体水平位置等の適正値を決定するときに、目標ZMPパラメータを修正する場合では、目標ZMPの修正量にほぼ比例して、今回歩容終端上体水平位置が変化するので、上記適正値の探索回数が少なくて済む。これに対して、遊脚重心軌道や歩容の周期を修正する場合には、その修正に対して、今回歩容終端上体水平位置が、かなり非線形に変化するので、上記適正値の探索により多くの探索回数を要する。
定常歩容につなげる(近づける)ための今回歩容の修正方法として、本実施形態では、今回歩容の目標ZMPパラメータを修正した。この方法では、場合によっては、目標ZMPパラメータの修正量(前記図34の修正量a)が過大になることがある。たとえば、その場でホッピングしている歩容からいきなり高速で移動する要求(走行要求)が与えられると、高速の定常歩容(走行の定常歩容)につなげる(近づける)ために、目標ZMPパラメータは、進行方向に対して後ろに極端に大きくずらす必要が生じる。この場合には、上記したように目標ZMPパラメータ以外の歩容パラメータも修正することが望ましい。但し、この場合では、実は、急激な加速を要求すること自体に無理があったわけであるから、要求値自身を修正しても良い。
要求値の修正を行なう場合には、例えば、とりあえず本実施形態に示した手順に従って、要求(要求パラメータ)を満足する定常歩容を求め、この定常歩容につながるように今回歩容パラメータを決定した時点で、今回歩容の目標ZMP軌道の安定余裕が少なくなり過ぎたか否かを判定する。そして、安定余裕が少なくなり過ぎた場合(いわゆる支持多角形から目標ZMPが外れたり、支持多角形の端部付近に目標ZMPが位置するような場合)には、要求を修正するようにすれば良い。
または、あらかじめ歩容の加減速度(次回歩容初期速度−今回歩容初期速度)/今回歩容の周期)の許容範囲を設定して置き、要求(歩容周期に関する要求パラメータ)を受けた時点で、該要求に対応する加減速度を求め、その求めた加減速度が許容範囲を越えていたら、許容範囲になるように、要求を修正しても良い。
補足すると、前記ΔMp、ΔFp、ΔMr、ΔFr、ΔMazおよびΔMbzは、上記のごとく単純な動力学モデルを用いる場合には、動力学演算により解析的に求めても良いが、一般的なより複雑な動力学モデルを用いる場合には、上体3を上体並進モードで微小量だけ加速または上体傾斜モードで微小量だけ加速させた場合の床反力を求め、加速させなかった場合の床反力との差を求め、この差を上記微小量で割ることによって求めれば良い。
また、あらかじめ標準的な歩容におけるΔMp、ΔFp、ΔMr、ΔFr、ΔMaz、ΔMbzおよびΔMp/ΔMrなどのそれぞれの平均値を求めておいて、それを用いても良い。ΔMp、ΔFp、ΔMr、ΔFr、ΔMaz、ΔMbzおよびΔMp/ΔMrは、状態(姿勢とその変化率)によって変わるので、各瞬間の状態ごとに求める方法に較べて、精度は若干低下するが、上記モデルよりも複雑なモデルを用いた場合には、演算量を大幅に低減できる。
今回歩容の逆位相腕振り復元角加速度パターンの台形高さbzcurrを決定する方法として、以下の方法を用いても良い。
前記逆位相腕振り復元角加速度修正歩容(図42のS722の説明を参照)の今回歩容終端時点での逆位相腕振り角と角速度をそれぞれθzcurr、vθzcurrとし、これらと、定常歩容の逆位相腕振り角、角速度との差をΔθzcerr、Δvθzcerrとする。
歩容周期をインターバルとし、仮今回歩容の終端逆位相腕振り角、角速度と定常歩容の初期逆位相腕振り角、角速度との差θzerr、vθzerrを前回状態、bzcurrを入力、Δθzcerr、Δvθzcerrを今回状態とする離散系の状態方程式を立てて、Δθzcerr、Δvθzcerrを0に収束させるように、現代制御理論等を用いてフィードバック則を決定し、これに基づいてbzcurrを求めても良い。
今回歩容およびまたは定常歩容の逆位相腕振り復元角加速度βarecは、台形パターンとせず、各瞬間において、目標逆位相腕振り角・角速度と基準逆位相腕振り角・角速度との差などを基に、その差が0に収束するように、状態フィードバック則などを用いて、各瞬間における逆位相腕振り復元角加速度βarecの値を決定しても良い。
今回歩容の逆位相腕振り復元角加速度βarecは、台形パターンとせず、各瞬間において今回歩容の目標逆位相腕振り角・角速度を基に、これらが第1旋回歩容の初期逆位相腕振り角・角速度に近づくように、状態フィードバック則などを用いて、各瞬間における逆位相腕振り復元角加速度βarecを決定しても良い。
斜面移動時(傾斜した床面でロボット1を移動させるとき)の歩容生成においては、床反力水平成分許容範囲や全体重心加速度水平成分許容範囲の代わりに、並進床反力の床面平行成分(床面に平行な成分)、すなわち摩擦力の許容範囲、あるいは全体重心加速度の床面平行成分(これは摩擦力に比例する)の許容範囲を設定するようにしてもよい。例えば、並進床反力の床面平行成分(摩擦力)の許容範囲を設定する場合に関して説明(この説明は全体重心加速度の床面平行成分の許容範囲を設定する場合でも同様である)すると、該摩擦力は、床面の水平面に対する傾斜角度θf(ロボット1の進行方向に向かって前下がり斜面の場合を正とする)とすると、次式72の関係が成立する。従って、前記実施形態と同様のアルゴリズムに歩容を生成する場合、この式72の関係を用いて、摩擦力許容範囲を床反力水平成分許容範囲に変換することで、該床反力水平成分許容範囲を設定するようにすればよい。なお、この場合、式72の床反力鉛直成分には、目標床反力鉛直成分を用いればよい。
摩擦力=床反力水平成分*cos(θf)−床反力鉛直成分*sin(θf)
…式72
また、斜面移動時(傾斜した床面でロボット1を移動させるとき)の歩容生成においては、床反力モーメント鉛直成分は、式1072によって床面摩擦力法線方向モーメントに変換することができるので、床反力モーメント鉛直成分許容範囲の代わりに、床反力モーメントの床面法線方向成分、すなわち床面摩擦力法線方向モーメントの許容範囲の許容範囲を設定するようにしてもよい。
床面摩擦力法線方向モーメント=床反力モーメント鉛直成分*cos(θf)
…式1072
今回歩容のパラメータ決定は、前記実施形態のように前回歩容が完了した時点で行う以外に、本願出願人によるPCT公開公報WO/02/40224に記述されているように、今回歩容の生成途中で、再決定しても良い。こうすることにより、演算量は増えるが、歩容に対する要求に変更があった時に即応できるからである。
また、歩容の修正(今回歩容パラメータを再決定すること)が、今回制御周期で間に合わない場合には、とりあえず今回は、修正しない歩容あるいは暫定的に修正した歩容(探索途中の歩容であって、完全には、探索完了条件(歩容境界条件のずれが許容値以内に入っていること)を満足していない歩容)を出力し、次回制御周期までに、あるいは複数制御周期後までに(暫定的でない)適切な修正歩容にしても良い。修正された目標ZMP軌道と目標床反力鉛直成分軌道は連続であり、短時間後では、これらが急激に変化することはないので、今回歩容の目標ZMP軌道と目標床反力鉛直成分軌道が若干ぎざぎざになるだけで、ほとんど問題はない。
産業上の利用可能性
以上のように本発明は、2足移動ロボット等の脚式移動ロボットと床面との摩擦力が小さくなるような状況でも、ロボットのスピンを生じることなく該ロボットを円滑に移動させることが可能な歩容を生成することができるものとして有用である。