以下、添付図面を参照して本発明の実施形態に係る脚式移動ロボットの歩容生成装置を説明する。尚、脚式移動ロボットとしては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,5は同一構造であり、それぞれ7個の関節を備える。すなわち、各腕体5は、3つの関節30R(L),32R(L),34R(L)から構成された肩関節と、1つの関節36R(L)から構成された肘関節と、3つの関節38R(L),40R(L),42R(L)から構成された手首関節と、この手首関節に連結された手先部(ハンド)44R(L)とを備えている。なお、頭部4は、本発明の要旨と直接的な関連を有しないため詳細な説明を省略する。
上記の構成により、各脚体2の足平22R(L)は、上体3に対して6つの自由度を与えられている。そして、ロボット1の歩行等の移動中に、両脚体2,2を合わせて6*2=12個(この明細書で「*」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す)の関節を適宜な角度で駆動することで、両足平22R,22Lの所望の運動を行うことができる。これにより、ロボット1は任意に3次元空間を移動することができる。また、各腕体5は、上体3に対して7つの自由度を与えられ、両腕体5,5を合わせて7*2=14個の関節を適宜な角度で駆動することで、後述する台車を押すなどの所望の作業を行なうことができる。
図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に出力する。この6軸力センサ50と同様の6軸力センサ52が、各腕体5の手先部(ハンド)44R(L)と手首関節38R(L),40R(L),42R(L)との間に介装され、手先部44R(L)に作用する外力の並進力の3方向成分並びにモーメントの3方向成分の検出信号が該6軸力センサ52から制御ユニット60に出力される。また、上体3には、Z軸(鉛直方向(重力方向))に対する上体3の傾斜角およびその角速度、並びにZ軸回りの上体3の回転角(ヨー角)およびその角速度を検出するための姿勢センサ54が備えられ、その検出信号が該姿勢センサ54から制御ユニット60に出力される。この姿勢センサ54は、図示を省略する3軸方向の加速度センサおよび3軸方向のジャイロセンサを備え、これらのセンサの検出信号が上体3の姿勢角(傾斜角およびヨー角)およびその角速度を検出するために用いられると共に、ロボット1の自己位置姿勢を推定するために用いられる。また、詳細構造の図示は省略するが、ロボット1の各関節には、それを駆動するための電動モータ64(図2参照)と、その電動モータ64の回転量(各関節の回転角)を検出するためのエンコーダ(ロータリエンコーダ)65(図2参照)とが設けられ、該エンコーダ65の検出信号が該エンコーダ65から制御ユニット60に出力される。
なお、図示は省略するが、各足平22R(L)と6軸力センサ50との間には、ばね等の弾性体が介装され、また、足平22R(L)の底面には、ゴム等の弾性体が貼り付けられている。これらの弾性体は、コンプライアンス機構を構成するものであり、各脚体2が床反力を受けたときに弾性変形するようになっている。
図2は制御ユニット60の構成を示すブロック図である。該制御ユニット60はマイクロコンピュータにより構成されており、CPUからなる第1の演算装置90及び第2の演算装置92、A/D変換器80、カウンタ86、D/A変換器96、RAM84、ROM94、並びにこれらの間のデータ授受を行うバスライン82を備えている。この制御ユニット60では、各脚体2の6軸力センサ50、各腕体5の6軸力センサ52、姿勢センサ54(加速度センサおよびレートジャイロセンサ)等の出力信号はA/D変換器80でデジタル値に変換された後、バスライン82を介してRAM84に送られる。またロボット1の各関節のエンコーダ65(ロータリーエンコーダ)の出力は、カウンタ86を介してRAM84に入力される。
前記第1の演算装置90は後述の如く目標歩容を生成すると共に、関節角変位指令(各関節の変位角もしくは各電動モータ64の回転角の指令値)を算出し、RAM84に送出する。また、第2の演算装置92はRAM84から関節角変位指令と、前記エンコーダ65の出力信号に基づいて検出された関節角の実測値とを読み出し、各関節の駆動に必要な操作量を算出してD/A変換器96とサーボアンプ64aとを介して各関節を駆動する電動モータ64に出力する。
図3は、本明細書の実施形態に係る脚式移動ロボットの制御装置の機能的構成を全体的に示すブロック図である。この図3中の破線で囲んだ部分が制御ユニット60が実行する処理機能(主として第1の演算装置90及び第2の演算装置92の機能)によって構成されるものである。なお、以下の説明では、脚体2および腕体5の左右を特に区別する必要がないときは、前記符号R,Lを省略する。
以下説明すると、制御ユニット60の機能的構成は、歩容生成装置100、対象物反力平衡制御装置102、脚メイン制御装置104および腕メイン制御装置106から構成されている。この明細書の実施形態では、歩容生成装置100の処理を除き、対象物反力平衡制御装置102、脚メイン制御装置104および腕メイン制御装置106の処理は、本願出願人が先に特開平10−230485号公報(前記特許文献1)にて提案したものと同じである。従って、以下の実施形態の説明では、歩容生成装置100を中心に説明し、対象物反力平衡制御装置102、脚メイン制御装置104および腕メイン制御装置106の説明は概略的な説明に留める。
歩容生成装置100は、ロボット1の目標歩容を自在かつリアルタイムに生成して出力するものであり、本願発明の歩容生成装置に相当するものである。この歩容生成装置100が出力する目標歩容は、目標上***置姿勢軌道(上体3の目標位置軌道及び目標姿勢軌道)、目標足平位置姿勢軌道(各足平22の目標位置軌道及び目標姿勢軌道)、目標手先位置姿勢軌道(各腕体5の手先部44の目標位置軌道および目標姿勢軌道)、目標全床反力中心点軌道(全床反力中心点の目標位置の軌道))、目標全床反力軌道、および目標対象物反力軌道から構成される。なお、脚体2や腕体5以外に上体3に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道が目標歩容に加えられる。以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。
ここで、歩容生成装置100が生成する目標歩容に係る用語の意味および定義について説明しておく。歩容における「軌道」は時間的変化のパターン(時系列パターン)を意味する。
足平位置、上***置など、ロボット1の各部位の「位置」は、その部位に固定的に設定されたある代表点の位置を意味する。例えば、本実施形態では、各足平22の代表点は、その足平22を備える脚体2の足首関節の中心から、該足平22の底面に延ばした垂線が該底面と交わる点であり、その代表点の位置が足平位置である。また、「姿勢」は空間的な向きを意味する。具体的には、例えば上体姿勢はZ軸(鉛直軸)に対するロール方向(X軸まわり)の上体3の傾斜角とピッチ方向(Y軸まわり)の上体3の傾斜角とヨー方向(Z軸まわり)の上体3の回転角(ヨー角)とで表され、足平姿勢は各足平22に固定的に設定された2軸の空間的な方位角で表される。
なお、歩容のうちの床反力および対象物反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢、上***置姿勢等、ロボット1の運動に係わる歩容を総称的に「運動」という。また、運動の軌道は、一定(時間的に変化しない)あるいはほぼ一定のものであってもよい。例えばロボット1がその両足平22,22を着地させた状態に維持して、移動しない状態では、足平位置姿勢軌道は一定になる。
歩容のうちの対象物反力は、ロボット1にある対象物を移動させるときに該対象物にロボット1から作用させる力の反力(対象物からロボット1に作用する力)を意味する。従って、目標対象物反力軌道は、本発明におけるロボット・対象物間目標作用力の軌道に相当する。本明細書の実施形態では、例えば図4に示すように、ロボット1がある対象物120(図示の例では台車)の所定の部位に両腕体5,5の手先部44R,44Lを係合させた状態で歩行動作を行いながら該対象物120を押す作業を行なう場合を例にとって説明する。この対象物120からロボット1が受ける力が対象物反力である。対象物反力は、一般的には、並進力成分とモーメント成分とから構成される。但し、本明細書の実施形態では、対象物反力はロボット1に対象物120から作用する並進力を意味するものとし、その並進力によってロボット1に作用する、ある点回りのモーメントを対象物反力モーメントという。補足すると、対象物反力は、ロボット1から対象物120に作用させる力の符号(向き)を反転させたものであるので、ロボット1から対象物120への作用力を対象物反力の代わりに用いてもよい。
なお、本明細書では、対象物120の運動に関し、対象物120の位置はロボット1の足平位置などと同様に、対象物120に固定的に設定された代表点の位置を意味する。また、対象物120の姿勢はロボット1の足平22の姿勢などと同様に、対象物120の空間的な向きを意味する。そして、対象物120の位置および姿勢を総称的に対象物運動ということがある。
また、各足平22に作用する床反力(並進力及びモーメントからなる床反力)を「各足平床反力」と呼び、ロボット1の全て(2つ)の足平22R,22Lについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義として扱う。
目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントによって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の表現が考えられるが、特に目標床反力中心点(全床反力の中心点の目標位置)を作用点にして目標床反力を表現すると、目標床反力のモーメント成分は、鉛直成分(鉛直軸(Z軸)まわりのモーメント)を除いて0になる。換言すれば、目標床反力中心点まわりの目標床反力のモーメントの水平成分(水平軸(X軸及びY軸)回りのモーメント)は0になる。そこで、本明細書の実施形態では、目標床反力中心点を目標全床反力の作用点として、歩容生成装置100で生成するものとした。なお、目標床反力中心点は、目標全床反力の作用点としての意味を持つものであるから、本質的には目標全床反力の1つの構成要素であると言える。但し、本明細書の実施形態では、目標床反力のうちの目標床反力中心点がロボット1の動作制御と密接に関連するものであることから、図3では、目標全床反力のうちの作用点である目標床反力中心点を目標全床反力とは別に記載するものとした。
また、本明細書では、ZMP(zero moment point)は、ロボットの運動によって生じる慣性力と該ロボットに作用する重力と対象物反力との合力(あるいはその合力と釣り合う床反力)がその点まわりに作用するモーメントが、鉛直成分を除いて0になる点の意味で使用する。動力学的平衡条件を満足する歩容では、ロボットの目標運動軌道と重力と目標対象物反力とから算出されるZMPと目標床反力中心点とは一致する。本明細書では、多くの場合、目標床反力中心点の代わりに、目標ZMPという。
また、本明細書では、目標歩容は、所定期間分の目標運動と目標床反力(目標ZMPを含む)と目標対象物反力との組を意味するものとする。特に、本実施形態では、目標歩容(単に歩容と称した場合を含む)は、ことわらない限り、ロボット1の1歩の期間分の目標運動と目標床反力(目標ZMPを含む)と目標対象物反力との組を意味するものとする。この場合、目標歩容の「1歩」は、ロボット1の片方の脚体2が着地してから、もう一方の脚体2が着地するまでの意味で使用する。なお、一連の歩容は、いくつかの歩容(所定期間分の歩容)がつながったものとする。また、以降の説明では、1歩の期間分の目標歩容を単位歩容ということがある。また、ロボット1が移動しない状態(両足平22,22の接地を維持する状態)での1歩の期間は、ある所定の期間(ロボット1が移動する場合の1歩の期間に相当する期間)を意味する。
また、歩容における両脚支持期とは、ロボット1がその自重を両脚体2,2で支持する期間(両脚体2,2が支持脚となる期間)、片脚支持期とはいずれか一方のみの脚体2でロボット1の自重を支持する期間(一方のみの脚体2が支持脚となる期間)を言う。本実施形態での目標歩容(単位歩容)は、別の言い方をすれば、両脚支持期の開始時から次の片脚支持期の終了時までの期間分の目標歩容である。ロボット1の自重を支持する脚体2を支持脚と呼ぶ。両脚支持期では、両脚体2,2が支持脚となり、片脚支持期では1つの脚体2が支持脚となる。また、片脚支持期においてロボット1の自重を支持しない側の脚体2(支持脚でない脚体2)を遊脚と呼ぶ。なお、以降の説明では、特にことわならい限り、目標歩容における支持脚は、両脚支持期の開始時に着地する脚体2(該両脚支持期に続く片脚支持期で支持脚となる脚体2)を意味するものとする。また、支持脚側の脚体2の足平22、遊脚側の脚体2の足平22をそれぞれ支持脚足平22、遊脚足平22という。また、ロボット1が移動しない状態(両足平22,22の接地を維持する状態)であっても、一方の脚体2が支持脚、他方の脚体2が遊脚であるとする。
また、歩容生成装置100により新たに生成しようとしている目標歩容(単位歩容)、あるいは生成しつつある目標歩容(単位歩容)を今回歩容と呼び、この今回歩容の1歩前の目標歩容を前回歩容、今回歩容の次の1歩の目標歩容を次回歩容、さらにその次の1歩の目標歩容を次次回歩容などと呼ぶ。
なお、目標歩容は、グローバル座標系としての支持脚座標系で記述される。本実施形態では、支持脚座標系は、支持脚足平22を水平姿勢(より一般的には床面に平行な姿勢)にして該支持脚足平22の底面のほぼ全面を床面に接触(密着)させた状態において、該支持脚の足首関節18,20の中心から床面に延ばした垂線が該床面と交わる点(この点は、本明細書の実施形態の例では支持脚足平22の底面のほぼ全面を床面に接触させた状態では、該足平22の代表点と合致する)を原点とし、その原点を通る水平面をXY平面とするグローバル座標系(床に固定された座標系)である。この場合、X軸方向、Y軸方向は、それぞれ支持脚足平22の前後方向、左右方向である。以降の説明では、X軸、Y軸、Z軸は、特にことわらない限り、この支持脚座標系の3軸を意味するものとする。尚、支持脚座標系の原点は、必ずしも支持脚足平22の底面のほぼ全面を床面に接触させた状態での該足平22の代表点に合致する必要はなく、該代表点と異なる床面上の点に設定されてもよい。
以上説明した事項は、以下に説明する第1実施形態に限らず、本明細書で説明する各実施形態において共通の事項である。
[第1実施形態]
以下に本発明の第1実施形態に係る歩容生成装置100の詳細を説明する。図5〜図7は歩容生成装置100の処理を示すフローチャートである。なお、この第1実施形態は、本発明の第1発明、第2発明、および第4発明の実施形態である。歩容生成装置100は、その演算処理機能によって、本発明の第1発明におけるモデル操作量決定手段、ロボット・対象物間作用力軌道仮決定手段、対象物運動軌道仮決定手段、ロボット歩容仮生成手段、制約条件判断手段、修正手段を構成している。
歩容生成装置100は、図5〜図7のフローチャートに示す処理を所定の演算処理周期で逐次実行する。
まず、S01において、対象物120の移動計画が決定される。ここで決定される移動計画は、少なくとも現在時刻から将来の所定期間分(ロボット1の複数歩分)の対象物120の移動計画を含んでいる。この移動計画は、基本的には対象物120の移動要求(対象物120をどのようなタイミングでどのように動かすかという設計的な要求)に応じて決定され、該対象物120の位置姿勢軌道(位置および姿勢の時系列)、あるいは、その軌道を規定するパラメータもしくは関数式等から構成される。例えば対象物120の移動要求が、ある時刻t0からX軸方向に一定速度で対象物120を移動させるという要求である場合には、移動計画は、図8のグラフg1で示す如く決定される。グラフg1は、移動計画における対象物位置(X軸方向位置)の経時変化を示している。この場合、例えば現在時刻以降の各時刻(ある刻み時間毎の時刻)における対象物120の位置の時系列を移動計画として決定してもよいが、時刻t0とグラフg1の傾き(対象物120の移動速度)とを移動計画を規定する要素(パラメータ)として決定したり、あるいはグラフg1の関数式を移動計画を規定する要素(パラメータ)として決定してもよい。なお、移動要求は、歩容生成装置100に外部から適宜与えられ、あるいは、あらかじめ制御ユニット60の図示しない記憶手段に記憶保持される。
補足すると、S01で決定する移動計画は、必ずしも移動要求通りに決定されるわけではなく、必要に応じて適宜修正されるが、これについては後述する。
次にS03に進み、上記の如くS01で決定された移動計画を基に、対象物動力学モデルを用いて目標対象物運動軌道(目標対象物位置姿勢軌道)と、目標対象物反力軌道とが仮決定される。
ここで、このS03の処理と、この処理で使用する対象物動力学モデルとを図9のブロック図を参照して説明する。図9は、S03で目標対象物運動軌道のうちの目標対象物位置軌道と、目標対象物反力軌道とを求める演算処理を示すブロック図である。このブロック図中の破線で囲んだ部分が対象物120に作用する力と対象物120の運動との関係を表す対象物動力学モデルとなっている。なお、本実施形態では、理解の便宜上、対象物120をほぼ水平な床上で移動させる場合を例に採って説明する。
図9に示す対象物動力学モデルは、本発明の第1発明にける対象物動力学モデルに相当しており、対象物120に作用させる力(より詳しくは水平方向の並進力)を入力として、対象物120の位置を出力する動力学モデル(順動力学モデル)である。より詳しくは、この対象物動力学モデルは、対象物120に作用させる水平方向の並進力の入力値(後述の加算部204で求められる値)に、対象物120の質量Mの逆数1/Mを乗算部206で乗算することにより対象物120の運動加速度を求め、これを積分器208,210で順次積分する(2重積分する)ことにより対象物120の位置(対象物動力学モデル上での位置)を出力するものとなっている。つまり、本実施形態では、図9に示す対象物動力学モデルは、対象物120に作用する並進力が、対象物120の加速度と質量との積に等しいという運動方程式を基に構成されている。なお、積分器208の出力は、対象物動力学モデル上での対象物120の移動速度を意味しており、以下、これを対象物モデル速度という。
かかる対象物動力学モデルを用いるS03の演算処理を図9を参照して具体的に説明する。まず、前記S01で決定された移動計画に基づく対象物120の瞬時瞬時の(各時刻の)目標速度である目標対象物速度と、積分器208で先に求められた対象物モデル速度とが減算部200に入力され、それらの偏差(=目標対象物速度−対象物モデル速度)が求められる。ここで、目標対象物速度は、移動計画における対象物120の位置軌道の1階微分値として得られる値、あるいは移動計画の構成要素である。なお、前記図8に示した移動計画の例では、グラフg1の傾きをそのまま目標対象物速度とすればよく、この場合には、目標対象物速度は継続的に一定値となる。そして、現在時刻から将来の所定期間分(ロボット1の複数歩分)の目標対象物速度の時系列が減算部200に順次入力される。また、減算部200に入力される対象物モデル速度は、新たに入力する目標対象物速度の1つ前の目標対象物速度を減算部200に入力したときに積分器208から出力された値である。
次いで、上記偏差に所定のゲインKvを乗算部202で乗算することにより、対象物120にロボット1から作用させるべき並進力の要求値が求められる。つまり、本実施形態では、該並進力の要求値は、目標対象物速度と対象物モデル速度との偏差が0に収束するように(対象物モデル速度が目標対象物速度に追従するように)、フィードバック制御則により決定される。そのフィードバック制御則としては、この例では比例制御則が使用される。そして、求められた並進力の要求値の符号を反転させたものの時系列が目標対象物反力軌道として出力される。
また、求められた並進力の要求値と、実際の対象物120にロボット1以外から作用する外乱力(床から対象物120に作用する摩擦力など)の推定値である推定外乱力とが加算部204に入力され、該並進力の要求値と推定外乱力との和が、前記対象物動力学モデルに対する並進力の入力値として加算部204により求められる。ここで、推定外乱力は、本発明の第1発明におけるモデル操作量に相当し、歩容生成装置100の演算処理周期毎に後述のS35の処理で求められるものである。加算部120に入力する推定外乱力としては、歩容生成装置100の前回の演算処理周期で求められた値(前回値)が用いられる。そして、このように求めた並進力の入力値を対象物動力学モデルに入力することで、前記した如く対象物120の位置が求められ、この求めた位置の時系列が積分器210から目標対象物位置軌道として出力される。
なお、対象物動力学モデルの積分器208の出力の初期値は、歩容生成装置100の前回の演算処理周期でS03の処理を実行して求めた対象物モデル速度の時系列のうちの、前回の演算処理周期に対応する時刻(前回の演算処理周期での現在時刻)における値に設定される。また、積分器210の出力の初期値は、歩容生成装置100の前回の演算処理周期でS03の処理を実行して求めた目標対象物位置の時系列のうちの、前回の演算処理周期に対応する時刻(前回の演算処理周期での現在時刻)における値に設定される。
また、目標対象物運動軌道のうちの目標対象物姿勢軌道は、例えば目標対象物速度の向きにほぼ一致するように決定される。
補足すると、移動計画および推定外乱力が一定に維持されている限り、目標対象物運動軌道や目標対象物反力軌道は一定になるので、S03の処理は、必ずしも歩容生成装置100の演算処理周期毎に行なう必要はない。従って、例えば、S03の処理は、ロボット1の1歩毎あるいは歩容生成装置100の演算処理周期の複数周期毎に行なったり、S01で移動計画を変更した場合や、推定外乱力が比較的大きく変化した場合に行なうものとしてもよい。
以上説明したS03の処理により、現在時刻から将来の所定期間分の目標対象物運動軌道と目標対象物反力軌道とが仮決定される。
なお、このS03の処理は、本発明の第1発明におけるロボット・対象物間作用力軌道仮決定手段および対象物運動軌道仮決定手段に相当している。より詳しくは、減算部200および乗算部202によって、移動計画に基づく目標対象物速度(目標運動状態量)に対象物動力学モデル上での対象物の移動速度である対象物モデル速度を追従させるように対象物120にロボット1から作用させるべき並進力の要求値を求め、また、この要求値から目標対象物反力軌道を求める(該要求値の符号を反転させる)処理がロボット・対象物間作用力軌道仮決定手段の処理に相当している。また、加算部204と対象物動力学モデルの乗算部206、積分器208,210との処理が対象物運動仮決定手段の処理に相当している。
次いで、S05に進んで、上記の如く仮決定された目標対象物運動軌道を基に、ロボット1の遊脚足平22の着地予定位置姿勢および着地予定時刻が仮決定される。ここで仮決定する着地予定位置姿勢および着地予定時刻は、現在、作成しようとしている目標歩容である今回歩容での遊脚足平22の着地予定位置姿勢および着地予定時刻を含む、ロボット1の複数歩分(少なくとも2歩分)の遊脚足平22の着地予定位置姿勢および着地予定時刻である。すなわち、仮決定する着地予定位置姿勢および着地予定時刻は、少なくとも今回歩容の遊脚足平22の着地予定位置姿勢および着地予定時刻と、次回歩容の遊脚足平22の着地予定位置姿勢および着地予定時刻とを含む。この場合、遊脚足平22の着地予定位置姿勢および着地予定時刻は、S03で決定された目標対象物運動軌道に対して、着地予定時刻での遊脚足平22の着地位置姿勢がその時刻での対象物位置姿勢と所定の相対的位置姿勢関係になるように決定される。
但し、遊脚足平22の着地予定位置姿勢および着地予定時刻は、必ずしも目標対象物位置姿勢に追従するような形態で決定する必要はない。例えば、ロボット1の移動を行なわない状態(両足平22,22の接地を維持する状態)で、対象物120を押して移動させ、その移動開始後は、対象物120から腕体5,5の手先部44をはずすような場合には、各歩容における遊脚足平22の着地予定位置姿勢は経時的に一定に維持されることとなる。つまり、一般的には、遊脚足平22の着地予定位置姿勢および着地予定時刻は、目標対象物運動軌道だけでなく、ロボット1のどのような運動によって、対象物120を動かすかという設計的な要求に応じて決定される。
なお、遊脚足平22の着地予定位置姿勢は、より詳しくは、遊脚足平22の踵を着地してから、該遊脚足平22を床に接触させたまま、すべらないようにピッチ方向に回転させて該遊脚足平22の底面のほぼ全面を床面に接触させた状態での該遊脚足平22の位置姿勢である。従って、本実施形態では、遊脚足平22Rまたは22Lの着地予定位置姿勢は、その着地時から次の遊脚足平22Lまたは22Rの着地までの単位歩容における支持脚座標系の位置姿勢を規定するものとなり、遊脚足平22の着地予定位置姿勢を決定することで、それに対応して、前記した支持脚座標系の設定手法に従って各単位歩容における支持脚座標系の位置姿勢が決定されることとなる。具体的には、各単位歩容における支持脚座標系は、該単位歩容の一つ前の単位歩容の遊脚足平22をその着地予定位置姿勢に合致させた状態において、該足平22を有する脚体2の足首関節の中心から床面に延ばした垂線が該床面と交わる点を原点とする座標系となる。
次いで、S07に進んで、今回歩容の目標ZMP軌道を規定するZMP軌道パラメータが仮決定される。ここで仮決定されるZMP軌道パラメータは、それにより規定される今回歩容の目標ZMP(以下、仮目標ZMPという)が、S05で仮決定された着地予定位置姿勢および着地予定時刻により定まる、今回歩容での支持脚足平22の接地面(両脚支持期では、両足平22の接地面を含む、いわゆる支持多角形)のほぼ中央付近に存在し、且つ、急激な変化をしないように決定される。すなわち、今回歩容の仮目標ZMPが、ロボット1の接地面内(あるいは支持多角形内)の、ロボット1の安定余裕ができるだけ高くなるような位置に存在し、且つ、急激な変化をしないようにZMP軌道パラメータが決定される。このような指針で決定される仮目標ZMPは、例えば図10(a)で示すようなパターンのものとなる。なお、図10(a)は仮目標ZMPのX軸方向位置のパターンを示している。この例の場合には、仮目標ZMPパターン(軌道)の折れ点の位置および時刻がZMP軌道パラメータとして仮決定される。
次いで、S09に進んで、現在仮決定されている目標対象物運動軌道と目標対象物反力軌道とを基に、現在仮決定されているZMP軌道パラメータにより定まる仮目標ZMPまわりの対象物反力モーメント軌道(対象物反力モーメントの瞬時値の時系列)が算出される。より具体的には、まず、目標対象物運動軌道を基に、ロボット1の両腕体5,5の手先部44R,44Lを係合させるべき対象物120の部位の運動軌道(位置姿勢軌道)が決定される。該部位の位置姿勢は、対象物120の位置姿勢に対して所定の位置姿勢関係を有するものとされる。そして、その部位の運動軌道(位置姿勢軌道)に合致するように、ロボット1の手先位置姿勢軌道(これはロボット1に対する対象物反力の作用点の軌道を規定する)が求められる。次いで、この求めた手先位置姿勢軌道と目標対象物反力軌道と仮目標ZMP軌道とから各時刻(ある刻み時間毎の時刻)の仮目標ZMPまわりの対象物反力モーメントが算出される。そして、その算出された対象物反力モーメントの時系列が対象物反力モーメント軌道として得られる。なお、対象物120から離れた箇所からロボット1を対象物120の近くに移動させ、その後、手先部44R,44Lを対象物120に係合させて該対象物120を押す作業を開始する場合には、対象物120を手先部44R,44Lを係合させる時刻(この時刻は適宜決定される)までの対象物反力軌道、ひいては、対象物反力モーメント軌道は0とされる。
次いで、S11に進んで、今回歩容に続く周期的歩容としての定常旋回歩容の歩容パラメータが決定される。該歩容パラメータは、定常旋回歩容における足平位置姿勢軌道を規定する足平軌道パラメータ、目標ZMP軌道を規定するZMP軌道パラメータ、目標対象物反力モーメント軌道を規定する対象物反力軌道パラメータを含む。
これらの歩容パラメータを具体的に説明する前に、定常旋回歩容の概要を説明しておく。なお、以降の説明では、歩容の「初期」、「終端」はそれぞれ歩容の開始時刻、終了時刻もしくはそれらの時刻における瞬時歩容を意味する。
定常旋回歩容は、その歩容を繰り返したときに歩容の境界(本実施形態では1歩毎の歩容の境界)においてロボット1の運動状態(足平位置姿勢、上***置姿勢等の状態)に不連続が生じないような周期的歩容を意味する。
周期的歩容である定常旋回歩容は、本実施形態では、ロボット1の2歩分の歩容、すなわち今回歩容に続く第1旋回歩容と該第1旋回歩容に続く第2旋回歩容とからなる歩容を該定常旋回歩容の1周期分の歩容として、その1周期分の歩容を繰り返す歩容である。尚、ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。以降、定常旋回歩容を定常歩容と略す場合もある。
定常歩容について補足すると、ロボット1としての2足移動ロボットでは、定常歩容の1周期分は、少なくとも2歩分の歩容(連続する2つの単位歩容)から構成される。なお、3歩以上の歩容を1周期分の歩容とする複雑な定常歩容を設定することも可能である。但し、定常歩容は、後述の如く、今回歩容の終端における発散成分(詳細は後述する)を決定するためだけに用いられる。このため、3歩以上の歩容を1周期とする定常歩容を用いることは、歩容生成の処理が煩雑となるにも関わらず、効果は少ない。そこで、本実施形態での定常歩容の1周期分の歩容を2歩分の歩容(第1及び第2旋回歩容)により構成するようにしている。以下の定常歩容の説明では、説明の便宜上、2歩分の歩容から成る定常歩容を1歩の歩容とみなす。定常歩容は、歩容生成装置100で今回歩容の終端における発散成分を決定するために暫定的に(歩容生成装置100の演算処理上で)想定される仮想歩容であり、歩容生成装置100からそのまま出力されるものではない。
尚、「発散」とは、2足移動ロボット1の上体3の水平位置が両足平22,22の位置からかけ離れた位置にずれてしまうことを意味する。発散成分の値とは、2足移動ロボット1の上体3の水平位置が両足平22,22の位置(より具体的には、支持脚足平22の接地面に設定された支持脚座標系の原点)からかけ離れていく具合を表す数値である。
本実施形態では、目標歩容が、前記発散を生じることなく、継続的に生成されるように、発散成分を指標にして歩容を生成する。すなわち、今回歩容の後に続く定常歩容の初期発散成分を決定してから、今回歩容の終端発散成分を定常歩容の初期発散成分に一致させる(より一般的には、今回歩容の上***置姿勢を定常歩容の上***置姿勢に収束させる)ように、今回歩容を生成する(今回歩容を規定する歩容パラメータを決定する)。そして、定常歩容の初期発散成分は、S11で決定した歩容パラメータを基に、該定常歩容の初期(第1旋回歩容の初期)と終端(第2旋回歩容の終端)とでロボット1の運動状態が一致するという定常歩容の条件(以下、これを定常歩容の境界条件という)をロボット1の動力学モデル上で満たすように決定される。このような歩容生成の基本的な指針は本出願人が先に提案したPCT国際公開公報WO/02/40224A1のものと同様である。従って、以下の定常歩容に関する説明では、PCT国際公開公報WO/02/40224A1の記載事項と異なる技術事項を主体に説明し、同様の技術事項については詳細な説明を省略することがある。
S11の説明に戻って、定常歩容の歩容パラメータのうちの足平軌道パラメータは、第1旋回歩容および第2旋回歩容の初期及び終端のそれぞれにおける支持脚足平22及び遊脚足平22のそれぞれの位置姿勢、各旋回歩容の歩容周期等から構成され、今回歩容、第1旋回歩容、第2旋回歩容の順に、足平位置姿勢軌道がつながるように決定される。以下に具体的な設定方法を図11を参照して説明する。
第1旋回歩容初期遊脚足平位置姿勢は、次回歩容の支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とする。この今回歩容終端支持脚足平位置姿勢は、今回歩容初期の支持脚足平22を床に接触させたまま、すべらないように該支持脚足平22をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚足平22の位置姿勢(これは前回歩容の遊脚足平22の着地予定位置姿勢に一致する)である。なお、次回歩容支持脚座標系の位置姿勢は、図11に示す如く、今回歩容の遊脚足平22の着地予定位置姿勢に対応して決定される。
第1旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とする。この今回歩容終端遊脚足平位置姿勢は、今回歩容の遊脚足平22の着地予定位置姿勢(あるいはこれに対応して定まる次回歩容支持脚座標系)に対応した位置姿勢に決定される。具体的には、今回歩容の遊脚足平22を今回歩容終端遊脚足平位置姿勢から床に接触させたまま、すべらないようにピッチ方向に所定角度、回転させたときの位置姿勢が今回歩容の遊脚足平22の着地予定位置姿勢になるように今回歩容終端遊脚足平位置姿勢が決定される。
第1旋回歩容終端遊脚足平位置姿勢は、次回歩容支持脚座標系から見た次回歩容終端遊脚足平位置姿勢とする。この次回歩容終端遊脚足平位置姿勢は、今回歩容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容の遊脚足平22の着地予定位置姿勢(あるいはこれに対応して定まる次次回歩容支持脚座標系)に対応した位置姿勢に決定される。
第1旋回歩容終端支持脚足平位置姿勢は、次回歩容支持脚座標系に位置姿勢を合致させた支持脚足平22を床に接触させたまま、すべらないように該支持脚足平22をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚足平22の位置姿勢(この位置姿勢は、今回歩容の遊脚足平22の着地予定位置姿勢に一致する)とする。
第2旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第1旋回歩容終端支持脚足平位置姿勢とする。第2旋回歩容初期支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た第1旋回歩容終端遊脚足平位置姿勢とする。
第2旋回歩容終端遊脚足平位置姿勢は、それを次次回歩容支持脚座標系から見た位置姿勢が、今回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢に一致するように決定される。第2旋回歩容終端支持脚足平位置姿勢は、それを次次回歩容支持脚座標系から見た位置姿勢が、今回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢に一致するように決定される。
第1旋回歩容および第2旋回歩容の歩容周期は、次回歩容周期(これは今回歩容の遊脚足平22の着地予定時刻と次回歩容の遊脚足平22の着地予定時刻との差である)と同一に設定される。これらの第1旋回歩容及び第2旋回歩容の歩容周期は、互いに同一にすることは必ずしも必要ではないが、いずれの周期も、少なくとも次回歩容周期に応じて決定するのが好ましい。尚、今回歩容、第1旋回歩容および第2旋回歩容の上記以外の運動パラメータ(両脚支持期時間などの時間パラメータを含む)は、上記決定されたパラメータに応じて、歩容の条件(アクチュエータの速度が許容範囲に入っているか、可動角を超えていないか、床などと干渉していないかなど)を満足するように適宜決定する。
定常歩容の歩容パラメータのうちのZMP軌道パラメータは、前記S07で目標ZMP軌道パラメータを仮決定した場合と同様に、該ZMP軌道パラメータにより規定されるZMP軌道が、ロボット1の安定余裕が高くなり、且つ、急激な変化をしないように決定される。
また、定常歩容の対象物反力モーメント軌道パラメータは、周期性の条件を満足するように決定される。例えば、今回歩容、次回歩容および次次回歩容に対応する対象物反力モーメント軌道が図12(a)に示すようになっているとする。ここで、今回歩容の対象物反力モーメント軌道はS09で求められた軌道である。次回歩容の対象物反力モーメント軌道は、S03で求めた次回歩容の対象物反力軌道および目標対象物運動軌道と、定常旋回歩容のZMP軌道パラメータにより規定されるZMP軌道とを基にS09と同様に算出される軌道である。次次回歩容の対象物反力モーメント軌道は、S03で求めた次次回歩容の対象物反力軌道および目標対象物運動軌道と、定常旋回歩容のZMP軌道パラメータにより規定されるZMP軌道とを基にS09と同様に算出される軌道である。
このとき、定常旋回歩容の対象物反力モーメント軌道は、例えば図12(b)で示す軌道になるように、定常旋回歩容の対象物反力モーメント軌道パラメータが決定される。この例では、定常旋回歩容の第1旋回歩容の初期から終端までの対象物反力モーメント軌道は、今回歩容の対象物反力モーメント軌道に連続して、次回歩容の対象物反力モーメント軌道と一致するように決定されている。また、定常旋回歩容の第2旋回歩容の対象物反力モーメント軌道は、第1旋回歩容の対象物反力モーメント軌道に連続すると共に、第2旋回歩容の初期から終端の直前のある時刻txまでは、次次回歩容の対象物反力モーメント軌道と一致し、時刻txから終端までは、時刻txの値から、第1旋回歩容の初期(定常旋回歩容の初期)の値と同じ値まで連続的に変化するように決定されている。時刻txは、例えば定常旋回歩容のZMP軌道の折れ点の時刻に対応する。
なお、次回歩容の初期の対象物反力モーメントの値と、次次回歩容の終端の対象物反力モーメントの値との差がさほど大きくないときには、必ずしも、定常旋回歩容の初期の対象物反力モーメントと終端の対象物反力モーメントの値とを同じにしなくてもよい。
次いで、S13に進み、定常旋回歩容の初期発散成分が決定される。この場合、定常旋回歩容は周期的歩容であるから、ロボット1のあらかじめ定めた動力学モデル上で、第1旋回歩容の運動の初期状態(初期発散成分を含む)と第2旋回歩容の運動の終端状態(終端発散成分を含む)とが一致するように定常旋回歩容の初期発散成分が決定される。
このS13の処理を詳細に説明する前に、まず、このS13での処理に用いるロボット1の動力学モデルと、この動力学モデルを使用して目標上***置を決定するための動力学演算とを説明する。この動力学モデルは、S13の処理に用いられるほか、後述のS37までの処理で今回歩容を決定するために用いられ、ロボット1の動力学的挙動(ロボット1に作用する力と運動との関係)を単純化して(近似的に)表現した動力学モデルである。以下、この動力学モデルをロボット単純化モデルという。なお、このロボット単純化モデルの基本構造は、PCT国際公開公報WO/02/40224A1と同様であるが、対象物反力モーメントが加味されている点で同公報のものと相違している。
図13はロボット単純化モデルの構造を示している。図示の如く、このロボット単純化モデルは、3質点モデルであり、脚体2の動力学と上体3の動力学とが相互に非干渉に構成されると共に、ロボット1の全体の動力学は、それらの線形結合で表される。
このロボット単純化モデルは、倒立振子Aと2つの脚質点2m,2m(支持脚質点および遊脚質点)とから構成される。各脚質点2mは、各脚体2に対応する質点である。この各脚質点2mは、それぞれに対応する脚体2の足平22に任意に固定的に設定されたローカル座標系での固定点であり、その位置は、各足平位置姿勢に対応して一義的に定まるものとされている。例えば、各足平質点2mは、その足平22の底面の代表点から足首関節の中心に向かって所定の距離だけ該足首関節側にずれた点に設定される。
倒立振子Aは、水平に移動するフリーの支点aと、一つの質点bと、支点aと質点bとを結ぶ質量の無い可変長のリンクcとから構成される。この場合、リンクcが傾いてもリンクcが伸縮し、支点aから見た質点bの高さhが一定値に保たれるものとする。
この倒立振子Aの質点b(以下、単に倒立振子質点bという)は、ロボット1の上体3の質点に相当するものであり、以降、倒立振子質点bを上体質点bということもある。倒立振子質点bの位置を以降、略して倒立振子位置という。なお、上体質点bの質量は、上体3の質量と腕体5,5および頭部4の質量を含んでいる。
ロボット1の上体3の水平位置は、倒立振子質点bの水平位置から幾何学的に決定される。具体的には、例えば、上体3の代表点の水平位置(支持脚座標系で見たXY座標)、すなわち上***置の水平成分が、倒立振子質点bの水平位置に一致するものとされる。換言すれば、図13に示すように、上体3の代表点と倒立振子質点bとは、同一鉛直線上にあるものとされる。
なお、倒立振子AのZMPは、フリーの支点aの位置にあるので(支点aのまわりにはモーメントが発生しない)、以降、倒立振子Aの支点aの位置を、倒立振子ZMPと呼び、ZMPpendと記述する。
かかるロボット単純化モデルを数式表現するために、該モデルに関する変数およびパラメータを以下のように定義する。
msup:支持脚質点2mの質量、 mswg:遊脚質点2mの質量、 mb:倒立振子質点bの質量、 mtotal:ロボット1の全体質量(=msup+mswg+mb)、 mfeet:両脚体2,2の総質量(=msup+mswg)、 xsup:支持脚質点2mの位置、 xswg:遊脚質点2mの位置、 xb:倒立振子位置(上体質点bの位置)、 h:倒立振子高さ(倒立振子Aの支点aから倒立振子質点bまでの高さ)
以降、ことわらない限り、xb,xsup,xswgは3次元ベクトル(XYZ座標ベクトル)で表す。また、Xを任意の変数としたとき、dX/dtはXの1階微分を表し、d2X/dt2はXの2階微分を表す。gは重力加速度定数を示す。Gは、そのX成分、Y成分、Z成分がそれぞれ0、0、−gである重力加速度ベクトルとする。
図13のロボット単純化モデルにおいて、両脚質点2m,2mの総慣性力(両脚質点2m,2mの運動による慣性力と重力との合力)がある作用点Pまわりに作用するモーメントを点Pまわりの脚総慣性力モーメントと定義し、その作用点Pの座標(位置)をxpとする。
下記の式01は、点Pまわりの脚総慣性力モーメントの動力学的定義式である。
点Pまわりの脚総慣性力モーメント
=msup(xsup−xp)*G−msup(xsup−xp)*d2xsup/dt2
+mswg(xswg−xp)*G−mswg(xswg−xp)*d2xswg/dt2 ……式01
脚ZMPをZMPfeetと記述し、式02で定義する。ただし、脚ZMPの高さ(ZMPfeetのZ成分)は、前記点Pの高さと同一とする。この脚ZMPは、両脚体2,2の運動によって発生する慣性力と重力との合力に擬似的に対応させた値である。
点Pまわりの脚総慣性力モーメント=mfeet*(ZMPfeet−xp)*G ……式02
なお、前記作用点Pは、ロボット単純化モデルの近似精度が高くなるように設定される。例えば、今回歩容に係る作用点Pは、前回歩容の支持脚座標系の原点から、今回歩容の支持脚座標系の原点に、両脚支持期の間に直線的に等速移動し、該両脚支持期に続く片脚支持期では今回歩容支持脚座標系の原点に維持されるように設定される。このことは、定常旋回歩容の第1旋回歩容、第2旋回歩容に係る作用点Pについても同様である。
また、目標ZMPまわりの対象物反力モーメントをロボット1の全体質量mtotalで除算したものを、対象物反力ZMPと定義し、これをZMPobjと記述する。すなわち、ZMPobjを次式03で定義する。
ZMPobj=目標ZMPまわりの対象物反力モーメント/mtotal ……式03
このとき、目標ZMPと脚ZMP(ZMPfeet)と倒立振子ZMP(ZMPpend)と対象物反力ZMPとの間には、近似的に次式04の線形関係が成り立つ。
ZMPpend=mtotal/mb*目標ZMP−mfeet/mb*ZMPfeet
−mtotal/mb*ZMPobj ……式04
なお、式04は、サジタルプレーン(XZ平面)上での関係式であり、ラテラルプレーン(YZ平面)上では、式03の右辺第3項の符号が「−」から「+」に反転する。
また、倒立振子Aの挙動を表す微分方程式は、次式05で表される。
d2xb/dt2の水平成分=g/h*(xbの水平成分−ZMPpendの水平成分) ……式05
以上の式01〜05が図13のロボット単純化モデルの動力学を記述する式である。
このようなロボット単純化モデルを用いることで、次のような動力学的演算により、目標足平位置姿勢と目標ZMPまわりの対象物反力モーメントと目標ZMPと目標上体姿勢とから目標上***置を決定できる。
その動力学演算を図14のブロック図を参照して説明しておく。図14はこの動力学演算を示すブロック図である。目標両足平位置姿勢(支持脚足平22および遊脚足平22の目標位置姿勢)の軌道と、前記の如く設定される作用点Pとから、脚ZMP算出器220により、前記式01および02を用いて脚ZMP(ZMPfeet)が算出される。
また、目標ZMPまわりの対象物反力モーメントに、乗算部222で1/mtotalを乗算することで、対象物反力ZMP(ZMPobj)が算出される。そして、算出したZMPfeetに乗算部224でmfeet/mtotalを乗算したものとZMPobjとを減算部226で目標ZMPから減算し、さらにその減算結果に乗算部228でmtotal/mbを乗算することによって、前記式04の右辺の演算が行なわれる。これにより倒立振子ZMP(ZMPpend)が算出される。なお、上記の演算処理により求められるZMPpendは、サジタルプレーン上のものであり、ラテラルプレーン上のZMPpendを算出する場合には、乗算部222の演算結果の符号を反転させたものを減算部226に入力すればよい。
このようにして算出されたZMPpendを倒立振子Aに入力することで、前記式05に基づいて、倒立振子水平位置xbが算出される。さらに、この倒立振子水平位置xbと目標上体姿勢とを基に、上***置決定器230により目標上***置が決定される。この場合、上***置決定器230は、倒立振子水平位置xbを目標上体水平位置とする。また、目標上体鉛直位置は、例えば本出願人が先に特開平10−86080号公報で提案した上体高さ決定手法によって、目標上体姿勢などを基に決定される。なお、本実施形態では、ロボット1の目標上体姿勢は例えば鉛直姿勢とされる。
図5の説明に戻って、以下に、S13の処理を詳細に説明する。
S13の処理では、前記したロボット単純化モデルを基に、定常歩容の初期発散成分を決定する。歩容における発散成分をqとおき、前記倒立振子Aの質点bの水平速度(あるいは上体水平速度)をvbとおくと、qは、本実施形態では次式06により定義される。
q=xb+vb/ω0 ……式06
ただし、ω0は、倒立振子Aの固有周波数、すなわち、g/hの平方根である。
なお、このように定義される発散成分の技術的意味については、PCT国際公開公報WO/02/40224A1に詳細に説明されているので、ここでの説明は省略する。
このように発散成分qを定義したとき、倒立振子Aの運動方程式(前記式05)を離散化して、qに関して解き、それに前記式04を適用すると、次式07が得られる。なお、目標ZMPをZMPtotalと記述する。
q[k]=exp(ω0kΔt)*q[0]
+exp(ω0kΔt)*(exp(−ω0Δt)−1)*Σ(exp(−iω0Δt)*mtotal/mb*ZMPtotal[i])
−exp(ω0kΔt)*(exp(−ω0Δt)−1)*Σ(exp(−iω0Δt)*mfeet/mb*ZMPfeet[i])
−exp(ω0kΔt)*(exp(−ω0Δt)−1)*Σ(exp(−iω0Δt)*mtotal/mb*ZMPobj[i])
…式07
ただし、exp( )は、自然対数の底eの指数関数を意味する。また、Δtは離散系の刻み時間、kはステップ数(kΔtが時刻を意味する)である。また、式07のΣ( )は、i=0からi=k−1までの累積加算演算を意味している。
なお、式07はサジタルプレーン上での発散成分qを記述する式であり、ラテラルプレーン上での発散成分qを記述する式は、式07の右辺の第4項の全体の符号を「−」から「+」に反転させた式となる。
ここで、定常旋回歩容の初期(第1旋回歩容の初期)における発散成分(以下、初期発散成分という)と終端(第2旋回歩容の終端)における発散成分(以下、終端発散成分という)に着目し、q[0](時刻0における発散成分)を初期発散成分、q[k](時刻kΔtにおける発散成分)を終端発散成分とする。
このとき、式07の右辺第1項は、初期発散成分によって発生する終端発散成分を表す。右辺第2項は、目標ZMPパターンによって発生する終端発散成分を表す。右辺第3項は、両脚体2,2の運動(両脚質点2m,2mの運動)によって発生する終端発散成分を表す。右辺第4項は、対象物反力モーメントパターンによって発生する終端発散成分を表す。以降、右辺第2項をWzmptotal、右辺第3項をWfeet、右辺第4項をWobjと記述する。すなわち、Wzmptotal、Wfeet、Wobjを次式08a,08b,08cで定義する。
Wzmptotal
=exp(ω0kΔt)*(exp(−ω0Δt)−1)*Σ(exp(−iω0Δt)*mtotal/mb*ZMPtotal[i])
……式08a
Wfeet
=−exp(ω0kΔt)*(exp(−ω0Δt)−1)*Σ(exp(−iω0Δt)*mfeet/mb*ZMPfeet[i])
……式08b
Wobj
=−exp(ω0kΔt)*(exp(−ω0Δt)−1)*Σ(exp(−iω0Δt)*mtotal/mb*ZMPobj[i])
……式08c
なお、式08cは、サジタルプレーン上でのWobjを定義する式であり、ラテラルプレーン上でのWobjは、式08cの右辺の符号を反転させたものとなる。
これらのWzmptotal、Wfeet、Wobjを用いて前記07を書き換えると、次式09となる。
q[k]=exp(ω0kΔt)*q[0]+Wzmptotal+Wfeet+Wobj ……式09
本実施形態では、定常旋回歩容の境界条件を満足する初期発散成分は、今回歩容に続く定常旋回歩容の支持脚座標系(図11の次回歩容支持脚座標系(X'Y'座標系))で見た初期発散成分が、該定常旋回歩容の次の歩容の支持脚座標系(図11の次次次回歩容支持脚座標系(X'''Y'''座標系))から見た定常旋回歩容(今回歩容に続く定常旋回歩容)の終端発散成分に一致するように、上記式08a,08b,08c,09に基づいて解析的に求められる。
ここで、定常旋回歩容の次の歩容の支持脚座標系(図11の次次次回歩容支持脚座標系(X'''Y'''座標系))から見た該定常旋回歩容の終端発散成分をq'[k]とおくと、定常旋回歩容の境界条件を満足するためには、次式10が成立しなければならない。
q[0]=q'[k] …式10
また、定常旋回歩容の次の歩容の支持脚座標系(図11の次次次回歩容支持脚座標系(X'''Y'''座標系))で記述される値を、該定常旋回歩容の支持脚座標系(図11の次回歩容支持脚座標系(X'Y'座標系))で記述される値に座標変換するための回転行列をM、座標原点の平行移動ベクトルをBとおくと、次式11が得られる。
q[k]=Mq'[k]+B ……式11
従って、これらの式10、11から次式12が得られる。
q[k]=Mq[0]+B ……式12
この式12は、定常歩容の境界条件を満足するために、初期発散成分q[0]と終端発散成分q[k]とが満たすべき条件である。
そして、この式12と前記式08a〜08cおよび09とから、次式13が得られる。
q[0]=inv(M−exp(ω0kΔt)I)(Wzmptotal+Wfeet+Wobj−B) ……式13
なお、式13において、inv( )は、括弧内の行列の逆行列であり、Iは単位行列である。
本実施形態では、この式13に基づいて、定常旋回歩容の境界条件を満足する初期発散成分q[0]を決定する。
具体的には、前記S11で決定した定常旋回歩容のZMP軌道パラメータを基に、定常旋回歩容の各時刻iΔt(i=0,1,……,k−1)における目標ZMPの瞬時値であるZMPtotal[i](i=0,1,……,k−1)を求め、これを用いて前記式08aの右辺の演算を行なうことで、Wzmptotalを算出する。なお、Wzmptotalの算出は、PCT国際公開公報WO/02/40224A1に説明されている如く、目標ZMP軌道が折れ線軌道であることを利用したアルゴリズムによって算出するようにしてもよい。
また、前記S11で決定した定常旋回歩容の足平軌道パラメータを基に、定常旋回歩容の各時刻iΔt(i=0,1,……,k−1)における各足平位置姿勢の瞬時値を求め、それを、前記式01,02を離散系で表現した式に適用することで、ZMPfeet[i]を求める。なお、足平位置姿勢は、例えば本出願人が特許第3233450号にて提案した有限時間整定フィルタを用いて各足平22毎に算出される。この場合、算出される足平位置姿勢の軌道は、定常旋回歩容の第1旋回歩容および第2旋回歩容のそれぞれにおける遊脚足平22が片脚支持期の開始時刻から上昇して、該遊脚足平22の着地予定位置に向かって移動し、該遊脚足平22の着地予定時刻にて該遊脚足平22がその踵で着地予定位置に対応する位置に着地するような軌道である。そして、上記の如く求めたZMPfeetl[i]を用いて前記式08bの右辺の演算を行なうことで、Wfeetを算出する。補足すると、遊脚足平22の着地予定位置姿勢が、1つ前の歩容における支持脚足平22の着地位置姿勢と同じである場合には、足平位置姿勢軌道は、一定の着地位置姿勢に維持される軌道とされる。但し、遊脚足平22を一旦上昇させ、その後、元の着地位置姿勢に戻すような足平位置姿勢軌道を生成するようにしてもよい。
さらに、前記S11で決定した定常旋回歩容の対象物反力モーメント軌道パラメータを基に、定常旋回歩容の各時刻iΔt(i=0,1,……,k−1)における、目標ZMPまわりの対象物反力モーメントを求めると共に、それをmtotalで除算することにより、ZMPobj[i]を求め、その求めたZMPobj[i]を用いて前記式08cの右辺の演算を行なうことで、Wobjを算出する。
なお、Wzmptotal、Wfeet、Wobjの算出で用いる刻み時間Δtは、歩容生成装置100の演算処理周期と同一であることが好ましいが、それよりも長い時間に設定して、演算処理時間を短縮するようにしてもよい。
そして、上記の如く求めたWzmptotal、Wfeet、Wobjから、前記式12の右辺の演算を行なうことで、定常旋回歩容の境界条件を満足する初期発散成分q[0]が算出される。
以上が、本実施形態におけるS13の処理の詳細である。
なお、本実施形態では、初期発散成分q[0]を解析的に求めるようにしたが、例えばPCT国際公開公報WO/02/40224A1の第2実施形態で説明されている如く、探索的な手法により、今回歩容に続く定常旋回歩容の次の歩容の支持脚座標系から見た該定常旋回歩容の終端発散成分にほぼ一致するような該定常旋回歩容の初期発散成分を決定するようにしてもよい。
図5のフローチャートの説明に戻って、上記の如くS13の処理を実行した後、S15に進んで、今回歩容の歩容パラメータが仮決定される。該歩容パラメータは、今回歩容における足平位置姿勢軌道を規定する足平軌道パラメータ、目標ZMP軌道を規定するZMP軌道パラメータ、目標対象物反力モーメント軌道を規定する対象物反力軌道パラメータを含む。
今回歩容の歩容パラメータのうちの足平軌道パラメータは、今回歩容の初期及び終端のそれぞれにおける支持脚足平22及び遊脚足平22のそれぞれの位置姿勢、今回歩容の歩容周期等から構成される。この場合、今回歩容初期遊脚足平位置姿勢は、今回歩容支持脚座標系から見た、前回歩容終端における今回歩容の遊脚足平位置姿勢(前回歩容の支持脚足平位置姿勢)とする。今回歩容初期支持脚足平位置姿勢は、今回歩容支持脚座標系から見た、前回歩容終端における今回歩容の支持脚足平位置姿勢(前回歩容の遊脚足平位置姿勢)とする。今回歩容終端遊脚足平位置姿勢は、今回歩容の遊脚足平22の着地予定位置姿勢に対応させて決定される。具体的には、今回歩容の遊脚足平22を今回歩容終端遊脚足平位置姿勢から床に接触させたまま、すべらないようにピッチ方向に所定角度、回転させたときの位置姿勢が今回歩容の着地予定位置姿勢になるように今回歩容終端遊脚足平位置姿勢が決定される。今回歩容終端支持脚足平位置姿勢は、今回歩容支持脚座標系に位置姿勢を合致させた支持脚足平22を床に接触させたまま、すべらないように該支持脚足平22をその底面のほぼ全面が床に接触するまでピッチ方向に回転させたときの該支持脚足平22の位置姿勢(この位置姿勢は、前回歩容の遊脚足平22の着地予定位置姿勢に一致する)とする。
なお、本実施形態では、今回歩容終端における支持脚足平22は、床面にほぼ平行な姿勢になるが、必ずしもこのようにする必要はない。例えば歩容終端において、支持脚足平22の踵側が床面から離れるようにしてもよい。
今回歩容の歩容パラメータのうちのZMP軌道パラメータは、S07で仮決定されたZMP軌道パラメータと同一に設定される。従って、このZMP軌道パラメータにより規定される目標ZMP軌道は、例えば前記図10(a)に示したようなパターンのものである。
また、今回歩容の歩容パラメータのうちの対象物反力モーメント軌道パラメータは、それにより規定される対象物反力モーメント軌道が、前記S09で算出した対象物反力モーメント軌道のうちの今回歩容に対応する軌道に一致するように決定される。
次いで、S17に進んで、定常旋回歩容に今回歩容が収束するように、S15で仮決定した歩容パラメータのうちのZMP軌道パラメータを修正する。より詳しくは、定常旋回歩容の初期発散成分に今回歩容の終端発散成分がほぼ一致するようにZMP軌道パラメータが修正される。
本実施形態では、定常旋回歩容の初期発散成分に今回歩容の終端発散成分をほぼ一致させるようにするために、前記仮目標ZMPに図10(b)に示すようなZMP修正量を加えることにより、該仮目標ZMPを修正し、それにより、図10(c)に示すような目標ZMPを得る。
この場合、仮目標ZMPの修正処理の基本的指針は、PCT国際公開公報WO/02/40224A1と同様であるが、対象物反力モーメントが考慮される点で同出願のものと相違している。以下にその相違する点を中心にS17の処理を具体的に説明する。なお、この処理で使用する変数、記号のうち、定常歩容の初期発散成分を決定する処理(S13)に関して説明したものと同様の意味を持つ変数については、定常歩容に関して先に定義したものと同じ変数、記号を用いてS17の処理を説明する。また、以下の説明では、理解の便宜上、ZMPのX軸方向成分の修正に関して説明を行なう。
本実施形態では、ZMP修正量は、図10(b)に示す如く台形状とされ、その高さをaとおく。なお、台形の折れ点の時刻は、図示の如く、仮目標ZMP軌道の折れ点の時刻に合わせて設定される。本実施形態では、ZMP修正量は、今回歩容の片脚支持期で発生させる。また、前記S13で決定した初期発散成分を今回歩容支持脚座標系から見た値に座標変換したものをq''とおく。すなわち、今回歩容支持脚座標系で見た定常旋回歩容の初期発散成分をq''とおく。また、仮目標ZMP軌道によって発生する今回歩容の終端発散成分をWzmptmp、a=1である場合のZMP修正量パターンによって発生する今回歩容の終端発散成分をWtrimとおく。
このとき、修正後の目標ZMP軌道によって発生する終端発散成分をWzmptotalとおくと、近似的に次式14が得られる。
Wzmptotal=Wzmptmp+a*Wtrim ……式14
従って、目標ZMPを修正した後の今回歩容の終端発散成分q''は、定常歩容に関して説明した前記式09と同様の次式15により表される。
q''=exp(ω0kΔt)*q[0]+Wzmptmp+a*Wtrim+Wfeet+Wobj ……式15
なお、式15において、kΔtは今回歩容の終端時刻、q[0]は今回歩容の初期発散成分、Wfeetは今回歩容の両脚体2,2の運動によって発生する終端発散成分、Wobjは今回歩容の対象物反力モーメント軌道によって発生する終端発散成分である。
この式15から、ZMP修正量の台形高さaを決定するための次式16が得られる。
a=(q''−exp(ω0kΔt)*q[0]−Wzmptmp−Wfeet−Wobj)/Wtrim ……式16
そこで、本実施形態では、式16により、ZMP修正量の台形高さaを求める。この場合、初期発散成分q[0]は今回歩容初期の倒立振子位置(あるいは上***置)とその変化速度とから、前記式06により算出される。また、Wzmptmpは、S15で仮決定されたZMP軌道パラメータにより規定される仮目標ZMP軌道から、定常旋回歩容に係るWzmptotalを求めた場合と同様に求められる。Wtrimは、台形高さaを1として定まるZMP修正量パターンから、定常旋回歩容に係るWzmptotalを求めた場合と同様に求められる。Wfeetは、S15で仮決定された足平軌道パラメータから、定常旋回歩容に係るWfeetを求めた場合と同様に前記式08bに基づいて求められる。Wobjは、S15で仮決定された対象物反力モーメント軌道パラメータから、定常旋回歩容に係るWobjを求めた場合と同様に前記式08cに基づいて求められる。
S17では、このようにして求めた台形高さaにより定まるZMP修正量を仮目標ZMP軌道に加えることで、ZMP軌道パラメータが修正される。
なお、本実施形態では、ZMP修正量を解析的に求めるようにしたが、探索的に求めるようにしてもよい。この場合には、台形高さaの値を適当な値に設定したZMP修正量により仮目標ZMPを修正してなる目標ZMP軌道を用いて、今回歩容を仮に作成し、その仮作成した今回歩容の終端発散成分と定常歩容の初期発散成分との差などに応じて、その差が小さくなるように台形高さaの値を修正する、という処理を繰り返す。そして、最終的に仮作成した今回歩容の終端発散成分と定常歩容の初期発散成分とがほぼ一致するときの台形高さaの値によってZMP修正量を決定すればよい。
S17の処理の後、S19に進んで修正後のZMP軌道パラメータにより定まる目標ZMPが適正か否かが判断される。具体的には、修正後の目標ZMPが、今回歩容の支持脚足平22の接地面から逸脱したり、接地面の境界付近に偏ったりしていない場合、すなわち、ロボット1の安定余裕が確保できる場合には、目標ZMPが適正であると判断され、そうでない場合には、目標ZMPが不適正であると判断される。
S19の判断結果がNOである場合には、S21に進んで、今回歩容パラメータのうちのZMP軌道パラメータ以外のパラメータに係わる要素、例えば対象物反力モーメント軌道パラメータに係わる目標対象物反力軌道を修正する。この場合、目標対象物反力軌道は、その修正後にS09〜S17の処理を再度実行したときに、ZMP修正量ができるだけ小さくなるように(少なくとも目標対象物反力軌道の修正前に求めたZMP修正量よりも小さくなるように)修正される。
この目標対象物反力軌道の修正例を以下に説明する。例えば図15(a)のグラフg2で示すように移動計画に基づく目標対象物運動軌道(位置軌道)が時刻t1からX軸方向で対象物120の減速を行なって、時刻t3で停止させるような軌道であるとする。なお、図15(a)のグラフg3は、目標対象物運動軌道に対応して決定される目標上***置軌道の例である。このとき、時刻t1からの対象物120の減速加速度が比較的大きい場合には、その時刻t1以後の、ある現在時刻t2における今回歩容に係るZMP修正量がX軸の正方向に過大になって、支持脚足平22の接地面を逸脱する場合がある(この場合、S19の判断結果がNOになる)。そして、仮に、そのまま目標歩容を生成し続けた場合には、図示のようにロボット1の上体3がある時刻t4で対象物120に衝突するような目標歩容が生成されてしまう。
そこで、このような場合には、目標対象物反力を、前記したようにS03で仮決定された目標対象物反力軌道に対して、図15(b)に示す如く、現在時刻t2以降のある期間ΔT1内で、X軸の負方向に増加させるように該目標対象物反力軌道を修正する。換言すれば、ロボット1から対象物120に加える作用力(目標対象物反力の符号を反転させた力)をX軸の正方向に増加させるように目標対象物反力を修正する。この場合、目標対象物反力を増加させる期間ΔT1あるいはその増加分ΔF1の大きさは、修正後の目標対象物反力を基に前記S09〜S17の処理により決定されるZMP修正量ができるだけ小さくなって、S19の判断結果がYESになるように決定される。このようなΔT1あるいはΔF1は、解析的あるいは探索的に求めることが可能である。
このように目標対象物反力軌道を修正することにより、ロボット1が積極的に対象物120を押すような上体3の運動が行なわれるように今回歩容パラメータのうちの対象物反力モーメント軌道パラメータがS15で決定されることとなる。その結果、この今回歩容パラメータを基に生成される目標歩容が、安定余裕を確保しつつ、対象物120にロボット1の上体3が衝突することが無いようなような歩容になる。
S21の処理を行なった後、S09〜S17の処理が前記したように再度実行される。このとき、S17の次のS19の判断結果はYESになり、この場合には、次に図6のS23に進む。
S23では、現在の今回歩容パラメータと定常歩容パラメータとを基に、現在時刻から定常歩容の第2旋回歩容終端までの(今回歩容の1歩を含めて3歩目までの)目標上***置姿勢軌道を算出すると共に、目標対象物運動軌道を基に目標手先位置姿勢軌道を算出する。つまり、S23では、今回歩容を含めて3歩の期間における目標歩容が仮生成される。
この場合、今回歩容、定常旋回歩容の第1旋回歩容、第2旋回歩容の順に、各歩容の歩容パラメータを基に、前記図14のブロック図を参照して説明した如く、目標上***置姿勢軌道を算出する。より具体的には、各歩容の足平軌道パラメータを基に、目標両足平位置姿勢軌道が算出されると共に、ZMP軌道パラメータおよび対象物反力モーメント軌道パラメータを基に、それぞれ目標ZMP軌道、対象物反力モーメント軌道が算出される。なお、目標両足平位置姿勢軌道の算出処理は、前記S13の処理の中で、ZMPfeet[i]を算出する処理に関して説明した場合と同様に、本出願人が特許第3233450号にて提案した有限時間整定フィルタを用いて各足平22毎に算出される。そして、これらの目標両足平位置姿勢軌道、目標ZMP軌道、および対象物反力モーメント軌道を図14のブロック図の演算処理に用いることで、目標上***置姿勢が決定される。なお、本実施形態では、目標状態姿勢は鉛直姿勢とされる。このように、目標上***置姿勢を算出することで、ロボット単純化モデル上で、ロボット1の運動が発生する慣性力と、ロボット1に作用する目標対象物反力と重力との合力が目標ZMPまわりの発生するモーメントの水平成分が0になるという動力学的平衡条件を満たすようにロボット1の目標運動のうちの目標上***置姿勢軌道が算出される。
さらに、目標手先位置姿勢軌道は、今回歩容、定常旋回歩容の第1旋回歩容、第2旋回歩容の順に、S09で求めた場合と同様に、目標対象物運動軌道を基に算出する。なお、定常旋回歩容の第1旋回歩容、第2旋回歩容に対応する目標対象物運動軌道は、S03で仮決定された目標対象物運動軌道のうちの、次回歩容、次次回歩容に対応する目標対象物運動軌道である。
以上説明したS05〜S23の処理は、本発明の第1発明におけるロボット歩容仮生成手段の処理に相当している。この場合、S05〜S21の処理により最終的に得られる今回歩容パラメータ、定常歩容パラメータが第1発明におけるロボット歩容仮生成手段の処理が仮決定する歩容パラメータに相当する。
次いで、S25に進んで、ロボット1と対象物120との幾何学的制約条件をチェックする。ここで、この幾何学的制約条件は、対象物120とロボット1との干渉(上体3が対象物120に当たるなど)の有無、並びに、各腕体3の関節の回転角が機構的な制限内に収まっているかなどの条件である。目標上***置姿勢と目標対象物運動軌道とからロボット1の上体3と対象物120との干渉の有無を判断できる。また、目標上***置姿勢と目標手先位置姿勢とから、各腕体3の関節の回転角を決定できるので、その回転角が機構的な制限内に収まっているか否かが判る。
S25の判断処理は、現在時刻から第2旋回歩容の終端まで、所定の刻み時間間隔の目標上体姿勢等の瞬時値に対して逐次行なわれる。すなわち、各瞬時値に対して、S25の判断結果がOKである場合には、S27において、現在時刻から第2旋回歩容の終端までの全時刻分のチェックが完了したか否かを判断する。そして、このS27の判断結果がNOである場合には、S25の判断処理を繰り返す。
S25の判断結果がNGである場合には、S29に進んで、NGとなった該当時刻における幾何学的制約条件を満たすように着地予定位置姿勢と目標対象物運動軌道とのうちの少なくともいずれか一方を修正する。その修正例を図16を参照して説明する。
同図を参照して、現在時刻t1における移動計画に基づくX軸方向の目標対象物運動軌道(位置軌道)が図中のグラフg4で示す軌道(以下、元軌道という)であるとし、将来のある時刻t2で目標上***置と目標対象物位置とが近すぎて、上体3と対象物120との干渉が発生し、S25の判断結果がNGになるとする。このとき、その干渉が発生する時刻t2の目標対象物位置を、X軸の正方向、すなわち、ロボット1から遠ざける方向に図中の破線矢印で示す如くずらすと共に、時刻t2の前後に亘る期間ΔT2での目標対象物運動軌道(位置軌道)をグラフg5で示す修正軌道に変更する。この修正軌道g5は、ロボット1の挙動の急変を避けるために、その期間ΔT2の両端で元軌道g4に滑らかにつながるように決定される。また、時刻t2でのずらし量ΔXは、それによって、ロボット1の上体3と対象物120との干渉が無くなるように(S25の判断結果がOKとなるように)決定される。
なお、上記の例では、目標対象物運動軌道を修正する場合を例に採って説明したが、目標対象物運動軌道を修正する代わりに、S25の判断結果がNGとなる該当時刻を含む歩容での遊脚足平22の着地予定位置姿勢を修正するようにしてもよい。例えば、図16の場合のように時刻t2で干渉が発生する場合には、その時刻t2を含む歩容での着地予定位置姿勢を対象物120からX軸方向で遠ざけるような位置姿勢に修正すればよい。あるいは、目標対象物運動軌道と着地予定位置姿勢の両者を修正するようにしてもよい。
S29の修正処理が終了すると、S05に戻って、このS05からの処理が前述したように実行される。これにより、前記幾何学的制約条件が満たされない場合に、目標対象物運動軌道あるいは着地予定位置姿勢が修正され、ひいては、今回歩容パラメータのうちの足平軌道パラメータが修正されることとなる。この場合、目標対象物運動軌道を修正したときには、S05の処理によって、結果的に、足平軌道パラメータ(遊脚足平22の着地予定位置姿勢および着地予定時刻)も修正されることとなる。なお、S29で目標対象物運動軌道を修正せずに、遊脚足平22の着地予定位置を修正した場合には、S05の処理を省略してS07からの処理を実行すればよい。
補足すると、以上説明したS25〜S29の処理は、本発明の第1発明における修正手段に相当する。
一方、S27の判断結果がYESになると、図7のS31に進む。このS31では、今回歩容パラメータを基に、目標ZMPを満足するように、手先位置姿勢瞬時値を除く、現在時刻の今回歩容瞬時値を算出する。
この算出処理は、次のように行なわれる。すなわち、今回歩容パラメータのうちの足平軌道パラメータを基に、目標足平位置姿勢の瞬時値が算出される。この算出処理は、前記S13の処理の中で、ZMPfeet[i]を算出する処理に関して説明した場合と同様に、本出願人が特許第3233450号にて提案した有限時間整定フィルタを用いて各足平22毎に算出される。なお、定常歩容に関して説明した場合と同様、今回歩容の遊脚足平22の着地予定位置姿勢が、前回歩容の支持脚足平22の着地位置姿勢と同じである場合には、今回歩容の各目標足平位置姿勢の瞬時値は、今回歩容初期の状態に維持される。但し、今回歩容の遊脚足平22を一旦上昇させて、元の着地位置姿勢に戻すように今回歩容の遊脚足平22の足平位置姿勢軌道の瞬時値を生成するようにしてもよい。
また、今回歩容パラメータのうちの、ZMP軌道パラメータを基に、目標ZMPの瞬時値が算出されると共に、対象物反力モーメント軌道パラメータを基に、目標対象物反力モーメントの瞬時値が算出される。
さらに、上記算出された目標足平位置姿勢の瞬時値と、目標ZMPの瞬時値と、目標対象物反力モーメントの瞬時値とから、前記図14の動力学的演算によって、目標上***置姿勢の瞬時値が算出される。なお、本実施形態では、目標上体姿勢は、例えば鉛直姿勢とされる。但し、目標上体姿勢を鉛直姿勢に維持する必要はなく、変化させるようにしてもよい。目標上体姿勢を変化させる場合には、ロボット単純化モデルは、上体3の姿勢変化に伴うロボット1の角運動量変化を考慮した動力学モデルにすることが望ましく、例えば本願出願人がPCT国際公開公報WO/03/057422A1にて提案した、同公報の図10に示すような動力学モデルを使用すればよい。
以上のS31の処理によりロボット1の目標歩容の運動のうちの、目標足平位置姿勢および目標上***置姿勢の瞬時値が決定される。本実施形態のロボット1では、各脚体2は6自由度を有するので、目標上***置姿勢と目標足平位置姿勢とが決まれば、各脚体2の各関節の回転角は一義的に定まる。補足すると、本実施形態では、S31で求められる目標上***置姿勢は後述するS39の処理によってさらに補正される。
次いで、S33に進んで、最終修正目標手先位置の前回値(前回の演算処理周期での値)を基に、前回の演算処理周期(歩容生成装置100の演算処理周期)における対象物120の実際の位置(移動位置)である実対象物位置を推定する。ここで、最終修正目標手先位置は、後述する腕メイン制御装置106で求められるものであり、ロボット1の実際の手先位置の指令値に相当するものである。そこで、例えば最終修正目標手先位置の前回値をロボット1の実際の手先位置の前回値とし、これと所定の相対位置関係を有する位置を実対象物位置として推定する。なお、対象物120にジャイロセンサや加速度センサを搭載しておき、それらの検出値から実対象物位置を推定するようにすることも可能である。
次いで、S35に進み、上記の如く推定した実対象物位置と、目標対象物位置との偏差(以下、対象物位置偏差という)と、現在の目標対象物運動軌道とを基に、対象物動力学モデルを用いて目標対象物運動の瞬時値(今回値)と、推定外乱力の瞬時値(今回値)と、目標対象物反力の瞬時値(今回値)とが算出される。なお、対象物位置偏差を規定する実対象物位置と目標対象物位置とのうちの目標対象物位置は、前回の演算処理周期で求めた値(前回値)が用いられる。
ここで、このS35の処理で使用する対象物動力学モデルとS35の処理とを図17のブロック図を参照して説明する。図17は、S03で目標対象物運動のうちの目標対象物位置と目標対象物反力と推定外乱力とを求める演算処理を示すブロック図であり、図中の参照符号238を付した部分が対象物120に作用する力と対象物120の運動との関係を表す対象物動力学モデルとなっている。この対象物動力学モデルは、前記図9に示したものと一部の構造が相違するので、以下の説明では、図17中の対象物動力学モデルを図9の対象物動力学モデルと区別するため、対象物動力学モデル2という。
この対象物動力学モデル2の基本構造は、図9の対象物動力学モデルと同様であり、対象物120に作用する力(より詳しくは水平方向の並進力)を入力として、その入力値(後述の乗算部242で求められる値)に、対象物120の質量Mの逆数1/Mを乗算部244で乗算することにより対象物120の運動加速度を求め、これを積分器246,250で順次積分する(2重積分する)ものとなっている。但し、対象物動力学モデル2では、図9の対象物動力学モデルと異なり、積分器250には、積分器246の出力(運動加速度の積分値)に加えて、モデル速度操作量が追加的に入力されるようになっている。このモデル速度操作量は、モデル速度操作量決定部252によって、実対象物位置と目標対象物位置との偏差を0に近づけるようにフィードバック制御則により決定される速度操作量であり、本実施形態では次式17により決定される。
モデル速度操作量=Ke1*対象物位置偏差+∫(Ke2*対象物位置偏差)dt ……式17
すなわち、モデル速度操作量は、対象物位置偏差からPI制御則(比例積分制御則)により決定される。なお、式17のKe1、Ke2は所定のゲインである。図17のモデル速度操作量決定部252は、式17の右辺の演算を行なう演算処理部である。すなわち、モデル速度操作量決定部252は、実対象物位置(現在の演算処理周期でのS33で求めた今回値)と目標対象物位置(前回の演算処理周期でのS35で求めた前回値)とから、それらの差である対象物位置偏差を減算部254で求める。そして、求めた対象物位置偏差に乗算部256でゲインKe1を乗算したもの(比例項)と、該対象物位置偏差に乗算部258でゲインKe2を乗算したものをさらに積分器260で積分したもの(積分項)とを、加算部262で加算することにより、モデル速度操作量を算出する。なお、算出されたモデル速度操作量は、対象物動力学モデル2の積分器250の入力側に備えた加算部248で、積分器246の出力に加算された後、積分器250に入力される。補足すると、図17のブロック図では、モデル速度操作量を算出した後に、これを対象物動力学モデル2に追加的に入力するようになっているが、モデル速度操作量決定部252の積分器260を省略し、前記乗算部258の出力を追加的に積分器246に入力しつつ、このときの積分器246の出力と、乗算部256の出力との和を積分器250に入力するようにしてもよい。このようにしても、対象物動力学モデル2の出力(積分器250の出力)は図17のものと同じになる。
かかる対象物動力学モデル2では、加算部248の出力を積分器250で積分したものが目標対象物位置の瞬時値として得られる。また、加算部248の出力が、該対象物動力学モデル2上での対象物120の移動速度である対象物モデル速度となる。
対象物動力学モデル2の入力である、対象物120への作用力(並進力)の要求値は、現在の目標対象物運動軌道(S27の判断結果がYESになったときの目標対象物運動軌道)に基づく目標対象物速度と、対象物モデル速度とから減算部240で求められるそれらの偏差(目標対象物速度−対象物モデル速度)に乗算部242で所定のゲインKvを乗算することで求められる。すなわち、対象物動力学モデル2に入力する並進力の要求値は、図9のものと同様、対象物目標速度と対象物モデル速度との偏差が0に収束するように、フィードバック制御則(この例では比例制御則)により決定される。但し、この場合の対象物モデル速度は、前記したように加算部248の出力であるから、図9のものと異なり、モデル速度操作量が加味されたものである。また、目標対象物速度は、現在の目標対象物運動軌道のうちの位置軌道の1階微分値として得られる。
そして、上記の如く求められた並進力の要求値の符号を反転させたものが目標対象物反力の瞬時値として求められる。また、該並進力の要求値が対象物動力学モデル2の乗算部244に入力されると共に、前記モデル速度操作量が加算部248に入力され、これにより、該対象物動力学モデル2の積分器250から目標対象物位置の瞬時値が出力される。換言すれば、対象物動力学モデル2上での対象物120の運動状態量としての対象物モデル速度を、目標対象物位置と実対象物位置との偏差を0に近づけるためのモデル速度操作量で修正しつつ、対象物動力学モデル2の動力学演算により目標対象物位置が逐次決定される。
また、図17中の参照符号264を付した部分は、推定外乱力を求める処理を行なう推定外乱力決定部であり、この推定外乱力決定部264には、対象物モデル速度と、前記並進力の要求値とが入力される。対象物モデル速度は、M・s/(Tc・s+1)という形の伝達関数で表される変換部266に入力され、この変換部266により、対象物120に作用している並進力の推定値が算出される。この変換部266の伝達関数中のMは対象物120の質量、Tcは所定の時定数である。従って、変換部266は、対象物モデル速度の微分値(1階微分値)に対象物120の質量を乗算したもの(これは対象物120に作用する全ての力の合力の瞬時値に相当する)に、時定数Tcを有するフィルタリング処理を施したものを対象物120に作用している並進力の推定値として求める。換言すれば、変換部266により算出される推定値は、対象物動力学モデル2上での対象物120の運動加速度(対象物モデル速度の微分値)を発生させる並進力に、時定数Tcの1次遅れを伴って追従するものである。さらに、この並進力の推定値から前記乗算部242の出力である並進力の要求値(これは、ロボット1から対象物120に作用させる力の推定値を意味する)を減算部268で差し引くことにより、推定外乱力の瞬時値が算出される。ここで求められた推定外乱力の瞬時値は、前記したようにS02の演算処理(図9を参照)で使用される。
補足すると、前記推定外乱力決定部264は、本発明の第1発明におけるモデル操作量決定手段に相当し、これにより求められる推定外乱力が本発明の第1発明におけるモデル操作量に相当する。
なお、目標対象物運動のうちの目標対象物姿勢の瞬時値は、例えば目標対象物速度の向きにほぼ一致するように決定される。
以上が、S35の演算処理である。このS35の演算処理で求められる推定外乱力は、実際の対象物120に作用する力のうち、ロボット1以外から作用する実際の該外乱力をリアルタイムで推定したものであるから、この推定外乱力を前記S02の演算処理で用いる(図9の対象物動力学モデルに入力する)ことにより、前記図9の対象物動力学モデル上での対象物120の挙動(運動状態)を実際の対象物120の挙動(運動状態)に近いものとすることができる。
次いでS37に進んで、S35で求めた目標対象物運動の瞬時値を基に、目標手先位置姿勢の瞬時値(今回値)を決定する。目標手先位置姿勢の瞬時値は、S09で目標手先位置姿勢軌道を求めた場合と同様に求められる。
以上のS37までの処理によって求められる目標歩容の瞬時値のうち、目標上***置姿勢は、前記ロボット単純化モデルを用いて、該ロボット単純化モデル上で目標ZMPを満足するように(ロボット1の運動による慣性力と重力と対象物反力モーメントとの合力が目標ZMPまわりに発生するモーメントの水平成分が0になるように)求められている。従って、S37までの処理によって求められる目標歩容は、目標ZMPまわりの床反力モーメント水平成分がロボット単純化モデル上で0となる歩容である。
ここで、ロボット単純化モデルは、歩容生成装置100の演算負荷を効果的に軽減できる点での利点は大きいものもの、動力学的精度は必ずしも高精度であるとはいえない。そこで、本実施形態では、さらに、動力学的精度の高いロボット動力学モデル(以下、これをフルモデルという)を用いて、目標歩容のうちの一部の構成要素(具体的には、目標上***置姿勢瞬時値と目標ZMPまわりの床反力モーメント瞬時値)とを補正する。この補正処理をフルモデル補正と呼び、このフルモデル補正がS37の処理の次にS39で実行される。
このフルモデル補正は、例えば本願出願人が先に提案した特開2002−326173号公報にて説明したものと全く同様に行なわれる。従って、本明細書での説明は省略する。なお、フルモデル補正は、例えば、本願出願人によるPCT国際公開公報WO 03/057427 A1にて説明したものと同様に行なうようにしてもよい。
このS39のフルモデル補正により、目標上***置姿勢と、目標ZMPまわりの床反力モーメントとが補正され、これにより、最終的に歩容生成装置100が出力する目標歩容の全ての構成要素の瞬時値が得られる。なお、フルモデル補正は、省略してもよい。
以上説明したS01〜S39までの処理が、歩容生成装置100の演算処理周期毎に実行される処理である。
次に、説明を後回しにした、S01での移動計画の修正処理について説明する。
ロボット1が対象物120を押しながら移動しているときに、現在の移動計画(前記移動要求通りの移動計画など)で想定していない段差(床の凸部)があったり、対象物120にロボット1以外から予期しない外力が作用したような場合には、移動計画に基づく目標対象物運動軌道と実際の対象物運動軌道(以下、実対象物運動軌道という)とのずれが大きくなる。一方、歩容生成装置100は、目標対象物運動軌道を基に歩容パラメータ等を決定して、歩容を生成するので、目標対象物運動軌道と実対象物運動軌道とのずれが過大になると、ロボット1の継続的安定性を確保し得る歩容を生成することが困難となる。そこで、本実施形態におけるS01では、そのずれがある程度大きくなった場合(あるいは大きくなると予測される場合)には、S01で決定する移動計画をリアルタイムで修正する。
以下、この処理の具体例を図18(a),(b)を参照して説明する。例えば現在(修正前の)移動計画が、対象物120を一定速度でX軸の正方向に移動させるという計画であるとする。このとき、この修正前の移動計画(以下、修正前移動計画という)を基に、前記した如くS35で算出される目標対象物運動軌道(瞬時値の時系列)のうちの目標対象物位置軌道は、ほぼ、図18(a)のグラフg6で示すような軌道となる。ここで、時刻t1において、実際の対象物120が、修正前移動計画で予期していない床の段差(凸部)に引っかかって、対象物120が止まってしまったとする。このとき、実対象物位置の軌道(S33で推定される実対象物位置の時系列)は、図18(a)のグラフg7で示すような軌道となる。この場合、時刻t1以後、修正前移動計画に基づく目標対象物位置と、実対象物位置との偏差が時間の経過に伴い大きくなっていく。
そこで、S01では、例えば前回の演算処理周期のS35で求めた対象物位置偏差(実対象物位置と目標対象物位置との差)、すなわち、図17の減算部254で求めた対象物位置偏差の大きさ(絶対値)を所定値と比較するようにしている。そして、その対象物位置偏差の大きさが所定値よりも大きくなったとき(図18(a)の時刻t2)には、図18(a)のグラフg8で示す如く、対象物位置偏差の増加を抑制するように修正前移動計画を修正する。修正した移動計画を以下、修正後移動計画という。図18(a)の例では、修正後移動計画は、時刻t2以後、対象物120の移動速度ほぼ0になるまで徐々に減速するような移動計画となる。なお、修正後移動計画は、それにより規定される目標対象物運動が修正前移動計画に対して急激な変化を生じることなく滑らかに連続するように決定することが望ましい。
このように移動計画を修正することによって、目標対象物反力が過剰に大きくなったりすることがなく、ロボット1の継続的な安定性を確保し得る歩容を生成することができる。
さらに、本実施形態でのS01の処理では、上記のように移動計画を修正した後、実対象物位置の、前記修正後移動計画に対応する目標対象物位置軌道への追従性が良くなり、目標対象物運動軌道と実対象物運動軌道とのずれがある程度小さくなったら、修正後移動計画を修正前移動計画に近づけるように移動計画を修正する。この修正後の移動計画を以下、再修正後移動計画という。
図18(a)に示したように修正後移動計画を決定した場合を例に採って説明すると、対象物120が段差を乗り越えると(図18(b)の時刻t3)、実対象物位置軌道は、図18(b)のグラフg7で示すように修正後移動計画に基づく目標対象物位置軌道g8に近づいていく。
このとき、S01では、対象物位置偏差が所定値よりも小さくなると(図18(b)の時刻t4)、図18(b)のグラフg9で示す如く、対象物120の移動速度が当初の修正前移動計画に基づく対象物120の移動速度に近づくように、修正後移動計画を再修正して、再修正後移動計画を決定する。図18(b)の例では、再修正後移動計画は、時刻t4以後、対象物120の移動速度が当初の修正前移動計画に基づく移動速度にほぼ一致するまで、該移動速度を徐々に増加させるような移動計画となる。なお、再修正後移動計画は、それにより規定される目標対象物運動が修正後移動計画に対して急減な変化を生じることなく滑らかに連続するように決定することが望ましい。
上記の例では、再修正後移動計画に基づく対象物120の移動速度を修正前移動計画に基づく移動速度に近づけるようにしたが、再修正後移動計画に基づく目標対象物位置軌道を修正前移動計画に基づく対象物位置軌道に近づけるようにすることも可能である。
以上が、本実施形態における歩容生成装置100の処理の詳細である。
次に、制御ユニット60の歩容生成装置100以外の処理を以下に説明する。なお、この処理は、先に述べたように、特開平10−230485号公報の第1実施例にて提案したものと同じであるので、概略的な説明に留める。
歩容生成装置100で生成された目標歩容のうち、目標上***置姿勢軌道と、目標ZMP軌道と、目標対象物反力軌道とが対象物反力平衡制御装置102に入力される。対象物反力平衡制御装置102は、目標対象物反力と実際のロボット1が対象物120から受ける反力である実対象物反力との偏差を解消する(0に近づける)ように、目標ZMPまわりの目標床反力モーメントを修正するための対象物反力平衡制御用補償全床反力を算出すると共に、目標運動のうちの目標上***置姿勢を修正してなる修正目標上***置姿勢を求めるものである。この対象物反力平衡制御装置102の処理については後述する。
また、目標歩容のうちの目標足平位置姿勢軌道、目標ZMP軌道および目標全床反力軌道は、脚メイン制御装置104に入力される。さらに、脚メイン制御装置104には、対象物反力平衡制御装置102から修正目標上***置姿勢と対象物反力平衡制御用補償全床反力が入力される。脚メイン制御装置104は、コンプライアンス制御処理によって、目標歩容の運動(腕体5,5の運動を除く)と床反力とに追従させるように脚体2,2の関節アクチュエータ(電動モータ)を制御する。より具体的には、姿勢センサ54の検出値(実上体姿勢)を修正目標上体姿勢に復元させるために、目標ZMPに発生させるべき復元全床反力を算出し、目標ZMPに作用する実全床反力モーメント成分(これは脚体2,2の6軸力センサ50の検出値から求められる)が、この復元全床反力と目標全床反力と対象物反力平衡制御用補償全床反力との合力のモーメント成分に一致するように、目標足平位置姿勢を修正する。修正された目標足平位置姿勢を修正目標足平位置姿勢という。そして、脚メイン制御装置104は、この修正目標足平位置姿勢と修正目標上***置姿勢とから決定される両脚体2,2の目標関節変位に実関節変位が追従するように両脚体2,2の関節アクチェエータを制御する(各脚体2のモータ駆動指令を各関節アクチュエータに出力する)。
また、目標歩容のうちの目標手先位置姿勢軌道と目標対象物反力軌道とは、腕メイン制御装置106に入力される。さらに、腕メイン制御装置106には、対象物反力平衡制御装置102から修正目標上***置姿勢が入力される。腕メイン制御装置106は、コンプライアンス制御処理によって、目標手先位置姿勢軌道と目標対象物反力軌道に追従させるように腕体5,5の関節アクチュエータ(電動モータ)を制御する。より具体的には、6軸力センサ52の検出値(実対象物反力)と目標対象物反力との差に応じて目標手先位置姿勢を修正する。修正された目標手先位置姿勢を最終修正目標手先位置姿勢という。そして腕メイン制御装置106は、この最終修正目標手先位置姿勢と修正目標上***置姿勢とから決定される両腕体5,5の目標関節変位に実関節変位が追従するように両腕体5,5の関節アクチュエータを制御する(各腕体5のモータ駆動指令を各関節アクチュエータに出力する)。
対象物反力平衡制御装置102の処理をより具体的に以下に説明する。図19は、対象物反力平衡制御装置102の処理を機能的に示すブロック図である。
対象物反力平衡制御装置102では、まず、目標対象物反力による目標ZMPまわりの目標対象物反力モーメントと実対象物反力による目標ZMPまわりの実対象物反力モーメントとの偏差である対象物反力モーメント偏差がモーメント偏差算出部270により算出される。この場合、目標対象物反力モーメントは、歩容生成装置100から出力された目標対象物反力および目標ZMPと、腕メイン制御装置106で求められた最終修正目標手先位置姿勢(より詳しくは前回の制御処理周期での値)とから算出される。また、実対象物反力モーメントは、6軸力センサ52の検出値(実対象物反力)と、目標ZMPと、最終修正目標手先位置姿勢(より詳しくは前回の制御処理周期での値)とから算出される。
この対象物反力モーメント偏差に応じて、重心位置摂動量算出部272により、ロボット1の全体重心位置の目標摂動量である目標重心位置摂動量が算出される。目標重心位置摂動量は、対象物反力モーメント偏差を、ロボット1に作用する重力によって長期的に解消するためのロボット1の全体重心の摂動量としての意味を持ち、例えば対象物反力モーメントに比例した値に決定される。次いで、後述する摂動動力学モデルで算出される、該摂動動力学モデル上でのロボット1の全体重心の摂動量である全体重心位置モデル摂動量と、前記目標重心位置摂動量との偏差が減算部274により算出され、この偏差からフィードバック制御則276、例えばPD制御則によって、該偏差を0に収束させるための、目標ZMPまわりのモーメント操作量である対象物反力平衡制御用補償全床反力モーメントが算出される。さらに、この対象物反力平衡制御用補償全床反力モーメントと前記対象物反力モーメント偏差との和が加算部278により算出される。そして、この加算部278の出力がロボット1の全体重心の摂動と、目標ZMPまわりのモーメントの摂動との関係、並びに、全体重心の摂動と上***置姿勢の摂動との関係を表す摂動動力学モデル280に入力され、この摂動動力学モデルで、上***置姿勢摂動量が算出される。
この場合、摂動動力学モデルは、ロボット1の全体重心の摂動と、目標ZMPまわりのモーメントの摂動との関係(動力学的関係)を次式18により記述するモデルである。
mtotal*hG*d2ΔxG/dt2=ΔxG*mtotal*g+ΔMx ……式18
ここで、hGは、目標ZMPから全体重心までの高さ、ΔxGは全体重心の水平方向摂動量、ΔMxは目標ZMPまわりのモーメント水平成分である。その他の変数は、前記ロボット単純化モデルに関して定義したものと同じである。なお、式18は、サジタルプレーン上での式であり、ラテラルプレーン上での関係式は、式18の右辺の第2項の符号を反転させればよい。
この式18は、ロボット1の全体質量mtotalの質点を持ち、また、その質点の支点である目標ZMPまわりに発生するモーメント水平成分がΔMxとなるような倒立振子の動力学的挙動を示す式である。
また、ロボット1の全体重心位置の摂動量ΔxGと上***置の摂動量(以下、これをΔxbと記述する)との関係は、次式19により表される。
Δxb=k*ΔxG ……式19
ここで、kはある比例定数である。従って、Δxbは、ΔxGに比例するものとされる。摂動運動に対しては、式19は近似的に成立すると考えてよい。
従って、摂動動力学モデル280では、式18の右辺のΔMxとして、加算部278の出力を用いることで、全体重心の摂動量ΔxGが算出され、さらに、このΔxGから式19によって、上***置摂動量が求められる。なお、本実施形態では、修正目標上***置姿勢のうちの修正目標上体姿勢は、目標上体姿勢に一致させるものとし、上体姿勢の摂動量は0とする。
対象物反力平衡制御装置102では、このようにして摂動動力学モデル280から出力される上***置摂動量を目標上***置姿勢(歩容生成装置100の出力)に加算部282で加算することにより、修正目標上***置姿勢を算出する。
以上が対象物反力平衡制御装置102の具体的な演算処理である。
以上説明した第1実施形態の作動、特に歩容生成装置100の処理によって、対象物反力を考慮しつつ、今回歩容が定常歩容に収束するように今回歩容が生成される。このため、ロボット1の継続的な安定性を確保しながら、ロボット1により対象物120を押すなどの作業を円滑に行なうことができる。
また、S03では、対象物120の実際の挙動(本実施形態では実対象物位置の挙動)を観測しながら前記図17のブロック図で求めた推定外乱力を図9の対象物動力学モデルに反映させ、現在時刻から所定期間分(複数歩分)の目標対象物運動軌道と目標対象物反力軌道とが仮決定される。そして、この仮決定した目標対象物運動軌道と目標対象物反力軌道とを基に、今回歩容および定常旋回歩容の歩容パラメータを決定して、現在時刻以降の将来のロボットの目標歩容がS23で仮生成される。さらに、この仮生成された目標歩容の運動軌道と目標対象物運動軌道とを基に、S25で幾何学的制約条件を満たしているか否かが判断され、該条件が満たされない場合には、S29でロボット1の遊脚足平22の着地予定位置姿勢や目標対象物運動軌道が修正され、ひいては、歩容パラメータのうちの足平軌道パラメータが幾何学的制約条件を満たすように修正される。そして、最終的にその修正された歩容パラメータを用いて今回歩容の瞬時値が歩容生成装置100の演算処理周期毎にリアルタイムで逐次生成される。このため、ロボット1の動作制御を行ないつつ、リアルタイムで実際の環境条件を反映させながら、前記幾何学的制約条件を満たす目標歩容を生成することができ、ひいては、対象物120とロボット1との干渉などを生じることなく、ロボット1による対象物120の移動を円滑に行なうことができる。
[第2実施形態]
次に、本発明の第2実施形態を図20及び図21を参照して説明する。なお、この第2実施形態の説明では、第1実施形態と同一構成部分あるいは同一機能部分については第1実施形態と同じ参照符号を用い、説明を省略する。また、この第2実施形態は、第1発明、第2発明および第4発明の実施形態である。
前記第1実施形態では、前記STEP25(図6を参照)の判断結果がNGであるとき、S29において着地予定位置姿勢と目標対象物運動軌道とのうちの少なくともいずれか一方を修正するようにした。本実施形態では、これに代えて、図20のフローチャートに示す如く、STEP25の判断結果がNGであったときに、S29’において、該当時刻における幾何学的制約条件を満たすように移動計画を修正するようにした。そして、S29’の処理の後には、前記図5のS03からの処理を行なうようにした。これ以外は、前記第1実施形態と同一である。
S29’における移動計画の修正例を図21を参照して説明する。同図を参照して、現在時刻t1における移動計画に基づくX軸方向の目標対象物運動軌道(位置軌道)が図中のグラフg10で示す軌道(以下、修正前移動計画という)、すなわち、対象物120の移動速度が現在時刻以降、急激に大きくなるような軌道であるとする。この場合、現在時刻以降のロボット1の目標上***置の軌道が例えば図中のグラフg11で示すような軌道で算出され、将来のある時刻t2で目標上***置が目標対象物位置から離れすぎて、腕体5,5が伸び過ぎる状態となり、S25の判断結果がNGになることがある。修正前移動計画に基づく対象物120の移動速度が急激に変化すると、このような状況が生じることがある。このとき、対象物120の修正前移動計画をグラフg12で示すような軌道となる移動計画(以下、修正後移動計画という)に修正する。すなわち、該修正後移動計画に基づく対象物120の移動速度が修正前移動計画に基づく移動速度よりも遅くなるように修正する。このようにすることで、目標上***置が目標対象物位置により近づくような目標歩容を生成することができるようになる。
なお、前記第1実施形態で説明した図16の場合の如く、将来のある時刻でロボット1の上体3と対象物120との干渉が生じるような目標歩容がS23で生成された場合にも、S29’において、移動計画を修正するようにしてもよい。この場合には、図16の元軌道に対応する移動計画を図16の修正軌道を持つような移動計画に修正すればよい。
かかる第2実施形態においても、第1実施形態と同様の効果を奏することができる。補足すると、第2実施形態では、S29’での移動計画の修正後、S03からの処理を実行するので、結果的に、目標対象物運動軌道、目標対象物反力軌道、歩容パラメータの足平軌道パラメータ(遊脚足平22の着地予定位置姿勢および着地予定時刻)も修正されることとなる。
[第3実施形態]
次に、本発明の第3実施形態を図22を参照して説明する。なお、この第3実施形態の説明では、第1実施形態と同一構成部分あるいは同一機能部分については第1実施形態と同じ参照符号を用い、説明を省略する。また、本実施形態は本発明の第3発明および第4発明の実施形態である。
前記第1実施形態では、S03(図5を参照)の演算処理で用いる対象物動力学モデルとして、図9に示した順動力学モデルを用いた。本実施形態は、これに代えて、逆動力学モデルを用いるものであり、S03の演算処理のみが第1実施形態と相違するものである。
図22は、本実施形態でのS03の演算処理を示すブロック図であり、このブロック図中の破線で囲んだ部分が第2発明における対象物動力学モデルに相当するものとなっている。この対象物動力学モデルでは、対象物120の移動計画に基づく対象物120の目標運動状態量としての目標対象物移動速度を入力として、対象物120に作用する力(並進力)と対象物120の位置とを出力する動力学モデルである。より詳しくは、この対象物動力学モデルは、目標対象物移動速度(入力値)を微分器300で微分(1階微分)することにより、対象物120の運動加速度を求め、さらに、この運動加速度に対象物120の質量Mを乗算部302で乗算することにより、対象物120に作用する力(並進力)の要求値を出力するものとなっている。ここで、対象物動力学モデルから出力される並進力の要求値は、対象物120の作用する全ての並進力(対象物120の移動方向の並進力)の合力の要求値である。さらに、対象物動力学モデルは、目標対象物移動速度を積分器304で積分することにより、対象物120の位置の目標値である目標対象物位置を出力するものとなっている。
なお、目標対象物速度は、第1実施形態と同様、歩容生成装置100において、移動計画における対象物120の位置軌道の1階微分値として算出され、あるいは、移動計画の構成要素としての移動速度がそのまま、目標対象物速度として決定される。
かかる対象物動力学モデルを用いるS03の演算処理を図22を参照して具体的に説明すると、まず、前記図5のS01で決定された移動計画に基づく対象物120の瞬時瞬時の(各時刻の)目標対象物速度が対象物動力学モデルに入力され、前記した如く、対象物120に作用する並進力の要求値と、目標対象物位置とが時系列的に算出される。
次いで、上記並進力の要求値から、歩容生成装置100の前回の演算処理周期において前記S35(図7参照)で求められた推定外乱力を減算部306で減算することにより、ロボット1から対象物120に作用させるべき力(並進力)の目標値が求められる。そして、この目標値の符号を反転させたものの時系列が目標対象物反力軌道として求められる。
なお、対象物動力学モデルの積分器304の出力の初期値は、歩容生成装置100の前回の演算処理周期でS03の処理を実行して求めた目標対象物位置の時系列のうちの、前回の演算処理周期に対応する時刻(前回の演算処理周期での現在時刻)における値に設定される。また、目標対象物運動軌道のうちの目標対象物姿勢軌道は、例えば目標対象物速度の向きにほぼ一致するように決定される。
以上説明したS03の処理により、現在時刻から将来の所定期間分の目標対象物運動軌道と目標対象物反力軌道とが仮決定される。このS03の処理以外は、前記第1実施形態と同一である。
なお、このS03の処理は、本発明の第2発明における目標運動状態量仮決定手段、対象物作用力軌道仮決定手段、および目標作用力軌道仮決定手段に相当している。より詳しくは、移動計画に基づき目標対象物速度を決定する処理が目標運動状態量仮決定手段に相当しており、対象物動力学モデルの微分器300及び乗算部302の処理が対象物作用力仮決定手段の処理に相当している。この場合、乗算部302の出力が対象物作用力目標軌道に相当する。また、減算部306の処理がロボット・対象物間作用力軌道仮決定手段の処理に相当している。
以上説明した第3実施形態の作動、特に歩容生成装置100の処理によって、第1実施形態と同様に、ロボット1の継続的な安定性を確保しながら、ロボット1により対象物120を押すなどの作業を円滑に行なうことができる。
また、S03では、対象物120の移動計画に基づく目標対象物速度を基に目標対象物運動軌道を仮決定する一方、対象物120の実際の挙動(本実施形態では実対象物位置の挙動)を観測しながら前記図17のブロック図で求めた推定外乱力を図22のブロック図の処理で反映させ、現在時刻から所定期間分(複数歩分)の目標対象物反力軌道が仮決定される。そして、前記第1実施形態と同様に、これらの仮決定した目標対象物運動軌道と目標対象物反力軌道とを基に、今回歩容および定常旋回歩容の歩容パラメータを決定して、現在時刻以降の将来のロボットの目標歩容がS23で仮生成される。さらに、この仮生成された目標歩容の運動軌道と目標対象物運動軌道とを基に、S25で幾何学的制約条件を満たしているか否かが判断され、該条件が満たされない場合には、S29でロボット1の遊脚足平22の着地予定位置姿勢や目標対象物運動軌道が修正され、ひいては、歩容パラメータのうちの足平軌道パラメータが幾何学的制約条件を満たすように修正される。そして、最終的にその修正された歩容パラメータを用いて今回歩容の瞬時値が歩容生成装置100の演算処理周期毎にリアルタイムで逐次生成される。このため、第1実施形態と同様に、ロボット1の動作制御を行ないつつ、リアルタイムで実際の環境条件を反映させながら、前記幾何学的制約条件を満たす目標歩容を生成することができ、ひいては、対象物120とロボット1との干渉などを生じることなく、ロボット1による対象物120の移動を円滑に行なうことができる。
なお、本実施形態においても、第2実施形態と同様に、図6のS29の処理で移動計画を修正するようにしてもよい。
[第4実施形態]
次に、本発明の第4実施形態を図23を参照して説明する。なお、この第4実施形態の説明では、第1実施形態と同一構成部分あるいは同一機能部分については第1実施形態と同じ参照符号を用い、説明を省略する。
第4実施形態は、図5のS01において、移動計画を修正する処理のみが第1実施形態と相違するものである。以下説明すると、実対象物運動軌道と、移動計画に基づく目標対象物運動軌道とのずれが大きくなるような状況は、実際の対象物120に作用する力のうち、ロボット1から作用する力以外の外乱力が比較的大きく変化した場合に発生する。
そこで、第4実施形態では、第1実施形態で説明した如く対象物位置偏差に応じて移動計画を修正する代わりに、例えば前記推定外乱力の変化に応じて移動計画を修正する。これ以外は、第1実施形態と同じである。
以下、具体例を説明すると、例えば、前記図18(a)に示した例の場合には、前記S35で算出される推定外乱力(X軸方向成分)は、図23(a)に示すように、対象物120が段差に引っかかる時刻t1以後に負方向に急激に大きくなる。そこで、第4実施形態では、この推定外乱力が所定値以上変化したとき(図23(a)の時刻t5)に、図18(a)に関して第1実施形態で説明した如く、修正前移動計画を修正して、修正後移動計画を決定する。
さらに、図18(b)に示した例の場合には、前記S35で算出される推定外乱力(X軸方向成分)は、図23(b)に示すように、対象物120が段差を乗り越えた時刻t3以後に、時刻t1以前の元の値とほぼ同じ値に戻っていく。そこで、この推定外乱力が、所定値以上変化した後に、元の値にほぼ近い値に戻ったとき(図23(b)の時刻t6)に、図18(b)に関して第1実施形態で説明した如く、修正後移動計画を修正して、再修正後移動計画を決定する。
以上説明した以外の処理は、前記第1実施形態と同じである。
かかる第4実施形態においても、第1実施形態と同様の作用効果が得られる。
補足すると、第4実施形態では、推定外乱力の変化に応じて移動計画を修正するようにしたが、両腕体5,5の6軸力センサ52の検出値から把握される実対象物反力と、S35で決定される目標対象物反力との偏差が所定値よりも大きくなったときに移動計画を修正するようにしてもよい。また、S03では、第3実施形態で説明した如く、前記図22の対象物動力学モデルを用いて目標対象物位置軌道及び目標対象物反力軌道を求めるようにしてもよい。また、S29では、第2実施形態で説明した如く、移動計画を修正するようにしてもよい。
[第5実施形態]
次に本発明の第5実施形態を図24〜図26を参照して説明する。なお、この第5実施形態の説明では、第1実施形態と同一構成部分あるいは同一機能部分については第1実施形態と同じ参照符号を用い、説明を省略する。
図24は第5実施形態での歩容生成装置100の処理のうち、第1実施形態に係る図5の処理に対応する部分の処理を示すフローチャートである。同図25に示すように、第5実施形態では、S19の判断結果がNOである場合の処理のみが第1実施形態と相違している。
すなわち、第1実施形態では、S19の判断結果がNOであるときに、目標対象物反力軌道を修正するようにしたが、第5実施形態では、これに代えて、S21’において、遊脚足平22の着地予定位置姿勢または着地予定時刻を修正する。これは今回歩容パラメータのうちの足平軌道パラメータを修正することを意味する。この場合、着地予定位置姿勢または着地予定時刻は、その修正後にS07〜S17の処理を再度実行したときに、ZMP修正量ができるだけ小さくなるように(少なくとも着地予定位置姿勢または着地予定時刻の修正前に求めたZMP修正量よりも小さくなるように)修正される。そして、その修正後に、S07からの処理を再度実行する。これ以外は、第1実施形態と同一である。
S21’での具体的な修正例を以下に説明する。例えば今現在、今回歩容の遊脚足平22の着地予定位置姿勢と次回歩容の遊脚足平22の着地予定位置姿勢が図25の実線で示す如く決定されているとする。なお、図示の例では、ロボット1をほぼ一定の歩幅で、今回歩容支持脚座標系のX軸方向に直進歩行させるものとなっている。
そして、今回歩容まではロボット1が対象物120に近づき、次回歩容の途中から、ロボット1が対象物120をX軸の正方向に押す作業を開始するものとする。この場合、図26(a)に示す如く、目標対象物反力軌道は、例えば次回歩容の途中から立ち上がるようなステップ状の軌道となる。
このとき、図25の実線で示すような着地予定位置姿勢を維持したまま、S17までの処理を実行すると、S17の処理で決定されるZMP修正量(X軸方向成分)は、例えば図26(b)に実線で示す如く、比較的大きなものとなって、S19の判断結果がNOとなる場合がある。
この場合に、S21’の処理では、例えば次回歩容の遊脚足平22の着地予定位置姿勢を図25の破線で示す如く、今回歩容の支持脚足平22LにX軸方向で近づけるように修正する。すなわち、次回歩容の歩幅をより小さくするように遊脚足平22Lの着地予定位置姿勢を修正する。このように次回歩容の着地予定位置姿勢を修正した後に、S05からの処理を再び実行すると、S17で決定されるZMP修正量が、図26(b)に破線で示す如く小さくなる。その結果、S19の判断結果がYESになる。補足すると、次回歩容の遊脚足平22Lの着地予定位置姿勢を図25の如く修正したとき、前記した定常歩容の足平軌道パラメータの決定手法によって、定常歩容の第2旋回歩容の終端遊脚足平位置も、第2旋回歩容の初期遊脚足平位置(今回歩容の遊脚足平22Rの着地予定位置)に近づくこととなる。
なお、上記の例では、次回歩容の遊脚足平22Lの着地予定位置姿勢を修正するようにしたが、今回歩容の遊脚足平22Rの着地予定位置姿勢を修正する余裕がある場合には、それを修正するようにしてもよい。あるいは、今回歩容および次回歩容の両者の着地予定位置姿勢を修正するようにしてもよい。
また、上記の例では、着地予定位置姿勢を修正するようにしたが、今回歩容と次回歩容との少なくともいずれか一方の遊脚足平22の着地予定時刻を修正するようにしてもよい。図23に示した状況では、例えば次回歩容の着地予定時刻を遅らせるようにすればよい。
以上が本発明の第5実施形態である。かかる第5実施形態においても、第1実施形態と同様の作用効果を奏することができる。なお、第5実施形態では、S19の判断結果がNOである場合に遊脚足平22の着地予定位置姿勢または着地予定時刻を修正するようにしたが、それと併せて、第1実施形態の如く、目標対象物反力軌道を修正するようにしてもよい。また、S01では、第4実施形態で説明した如く、推定外乱力に応じて移動計画を推定するようにしてもよい。また、S03では、第3実施形態で説明した如く、前記図22の対象物動力学モデルを用いて目標対象物位置軌道及び目標対象物反力軌道を求めるようにしてもよい。また、S29では、第2実施形態で説明した如く、移動計画を修正するようにしてもよい。
なお、以上説明した第1〜第3実施形態では、ロボット1により対象物120を押して移動させる場合を例に採って説明したが、対象物120を引いて移動させる場合や、対象物120を持ち上げて移動させる場合などにも本発明を適用することができる。
また、前記各実施形態では、今回歩容パラメータを決定するときに、周期的な歩容である定常歩容の歩容パラメータを決定した上で、該定常歩容に今回歩容を収束させるように(定常歩容の初期発散成分に今回歩容の終端発散成分を一致させるように)今回歩容パラメータを決定するようにしたが、本発明では、それを必須とするものではない。本発明は、基本的には、最終的に決定されたロボット・対象物間目標作用力(前記各実施形態では目標対象物反力)と対象物の目標移動位置(前記各実施形態では目標対象物運動軌道)とを満足し得るようにロボットの動作を制御できるものであれば、前記各実施形態で説明した手法とは異なる手法でロボットの動作を制御するようにしてもよい。