以下、脚式移動ロボットとして2足移動ロボットを例にとって、本発明の一実施形態を説明する。
図1に示す如く、本実施形態の2足移動ロボット1(以下、単にロボット1という)は、基体としての上体24と、この上体24を床面上で移動させる移動機構として該上体24から延設された左右一対の脚体(脚部リンク)2,2とを備える。
上体24は、両脚体2,2の基端部(上端部)に後述する腰関節(股関節)を介して連結されており、両脚体2,2のうちの接地した脚体によって床面の上方に支持される。
両脚体2,2は同一構造であり、それぞれ6個の関節を備える。その6個の関節は上体24側から順に、腰(股)の回旋用(上体24に対するヨー方向の回転用)の関節10R,10Lと、腰(股)のロール方向(X軸まわり)の回転用の関節12R,12Lと、腰(股)のピッチ方向(Y軸まわり)の回転用の関節14R,14Lと、膝部のピッチ方向の回転用の関節16R,16Lと、足首部のピッチ方向の回転用の関節18R,18Lと、足首部のロール方向の回転用の関節20R,20Lとから構成される。
なお、本実施形態の説明では、符号R,Lはそれぞれ右側脚体、左側脚体に対応するものであることを意味する。また、X軸、Y軸、Z軸は、後述する支持脚座標系の3つの座標軸を意味する。この支持脚座標系のX軸方向、Y軸方向は、水平面上で互いに直交する2軸方向であり、X軸方向はロボット1の前後方向(ロール軸方向)、Y軸方向はロボット1の左右方向(ピッチ軸方向)に相当する。また、Z軸方向は鉛直方向(重力方向)であり、ロボット1の上下方向(ヨー軸方向)に相当する。
各脚体2の関節10R(L),12R(L),14R(L)によって3自由度の腰関節(股関節)が構成され、関節16R(L)によって1自由度の膝関節が構成され、関節18R(L),20R(L)によって2自由度の足首関節が構成されている。
そして、腰関節(股関節)10R(L),12R(L),14R(L)と膝関節16R(L)とは大腿リンク32R(L)で連結され、膝関節16R(L)と足首関節18R(L),20R(L)とは下腿リンク34R(L)で連結されている。また、各脚体2の足首関節18R(L),20R(L)の下部に、各脚体2の先端部(下端部)を構成する足平22R(L)が取着されている。また、各脚体2の上端部(基端部)が、腰関節(股関節)10R(L),12R(L),14R(L)を介して上体24に連結されている。
上記した各関節は、例えば本願出願人が特開平3−184782号などにて提案した公知の構造のものでよい。この場合、各関節を回転駆動するアクチュエータは、減速機を備える電動モータ42(図2参照)により構成される。
各脚体2の上記構成により、各脚体2の足平22R(L)は、上体24に対して6自由度を有する。そして、ロボット1の移動に際して両脚体2,2を合わせて6*2=12個(この明細書で「*」はスカラに対する演算においては乗算を示し、ベクトルに対する演算においては外積を示す)の関節をそれぞれ適宜な角度に駆動することで、両足平22R,22Lの所望の運動を行なうことができる。これにより、ロボット1は歩行動作や走行動作等、3次元空間を移動する運動を行うことが可能となっている。
この場合、両脚体2,2が基体としての上体24を床面上で移動させる移動機構として機能する。また、基体としての上体24は、腰関節(股関節)を介して、脚体2,2に対して相対運動を行い得るように該脚体2,2に支持されることとなる。さらに、両脚体2,2の各関節の駆動制御によって、床面に対する上体24の運動を制御することが可能である。その上体24の運動には、上体24の水平方向(又は床面に平行な方向)の運動に加えて、上下方向(鉛直方向や、床面に垂直な方向等)の運動や、上体24の姿勢を変化させる運動も含まれる。
なお、図示は省略するが、本実施形態では、上体24の上部の両側部には左右一対の腕体が取り付けられると共に、上体24の上端部には頭部が搭載される。そして、各腕体は、それに備える複数の関節(肩関節、肘関節、手首関節など)によって、該腕体を上体24に対して前後に振る等の運動を行なうことが可能となっている。ただし、これらの腕体および頭部は無くてもよい。
上体24の内部にはロボット1の動作制御を行う制御ユニット26が格納されている。なお、図1では図示の便宜上、制御ユニット26を上体24の外部に記載している。
各脚体2の足首関節18R(L),20R(L)と足平22R(L)との間には6軸力センサ36が介装されている。この6軸力センサ36は、床から足平22R(L)を介して各脚体2に伝達される床反力の3軸方向の並進力成分及び3軸まわりのモーメント成分を検出し、その検出信号を制御ユニット26に出力する。
上体24には、鉛直方向(重力方向)に対する上体24の傾斜角(ロール方向及びピッチ方向の傾斜角)とその変化速度(角速度)とを計測するための傾斜センサ40が搭載されている。この傾斜センサ40は、より詳しくは、加速度センサとジャイロセンサ等のレートセンサ(角速度センサ)とから構成され、これらのセンサの検出信号を制御ユニット26に出力する。そして、制御ユニット26において、傾斜センサ40の出力を基に、公知の手法によって鉛直方向に対する上体24の傾斜角と角速度とが計測される。
また、各関節を回転駆動する電動モータ42(図2参照)には、各関節の回転角を検出するためのエンコーダ(ロータリエンコーダ)44(図2参照)が付設され、該エンコーダ44の検出信号が制御ユニット26に出力される。
図2を参照して、制御ユニット26はマイクロコンピュータを有する電子回路ユニットにより構成されており、CPUからなる第1演算装置50及び第2演算装置52、A/D変換器54、カウンタ56、D/A変換器58、RAM60、ROM62、並びにこれらの間のデータ授受を行なうバスライン64を備えている。
この制御ユニット26では、前記6軸力センサ36及び傾斜センサ40の出力はA/D変換器54でデジタル値に変換された後、バスライン64を介してRAM60に入力される。また、ロボット1の各関節のエンコーダ(ロータリエンコーダ)44の出力はカウンタ56を介してRAM60に入力される。
前記第1の演算装置50は、後述する目標歩容を生成すると共に、関節変位指令(各関節の回転角の目標値)を算出し、RAM60に送出する。また、第2の演算装置52はRAM60から関節変位指令と、前記エンコーダ44の出力からカウンタ56を介して計測された実関節変位(各関節の回転角の実測値)とを読み出し、該実関節変位を関節変位指令に追従させるために必要な各関節の電動モータ42の駆動指令(電動モータ42の出力トルクを規定する指令値)を算出する。
そして、第2の演算装置52は、その算出した駆動指令をD/A変換器58を介して電動モータ42の駆動用のサーボアンプ46に出力する。このとき、サーボアンプ46は、入力された駆動指令に応じて電動モータ42を駆動する(電動モータ42に通電する)。これにより、各関節の実関節変位が関節変位指令に追従するように制御される。
次に、図3を参照して、本実施形態におけるロボット1の制御装置の動作の概要を説明する。図3中の「実ロボット」以外の部分が制御ユニット26が実行する処理(主として第1の演算装置60及び第2の演算装置62の処理)によって実現される機能である。
なお、図3では、便宜上、ロボット1に搭載された前記の各センサの出力から制御ユニット26が認識する実測値(実関節変位など)が実ロボット1から出力されるものとして示している。また、以下の説明では、脚体2の左右を特に区別する必要がないときは、前記符号R,Lを省略する。
制御ユニット26は、ロボット1の動作(歩容)の目標である目標歩容を、該ロボット1の動力学(ロボット1の運動とロボット1に作用する外力との関係)を表現する動力学モデルを用いて生成して出力する歩容生成装置100を備える。この歩容生成装置100が生成して出力する目標歩容は、ロボット1の運動に関する目標と、ロボット1に作用する外力に関する目標とから構成される。
本実施形態では、該目標歩容は、上体24の目標位置及び目標姿勢の軌道である目標上体位置姿勢軌道と、各足平22の目標位置及び目標姿勢の軌道である目標足平位置姿勢軌道と、各腕体の目標姿勢の軌道である目標腕姿勢軌道と、ロボット1のZMP(Zero Morment Point)の目標位置の軌道である目標ZMP軌道と、ロボット1に作用する外力としての全床反力の目標の軌道である目標全床反力軌道とから構成される。これらの構成要素のうち、目標上体位置姿勢軌道、目標足平位置姿勢軌道及び目標腕姿勢軌道がロボット1の運動に関する目標に相当し、目標ZMP軌道及び目標全床反力軌道が、外力に関する目標に相当する。なお、脚体2や腕体以外に、上体24に対して可動な部位を備える場合には、その可動部位の目標位置姿勢軌道がロボット1の運動に関する目標の一部として目標歩容に加えられる。
ここで、上記目標歩容における「軌道」は時間的変化のパターン(時系列パターン)を意味し、歩容生成装置100の制御周期(演算処理周期)毎に算出される瞬時値の時系列により構成される。以下の説明では、「軌道」の代わりに「パターン」と称することもある。また、以下の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省略する。
上体24の位置及び速度は、上体24のあらかじめ定めた代表点(例えば左右の股関節の間の中央点等)の位置及びその移動速度を意味する。同様に、各足平22の位置及び速度は、各足平22のあらかじめ定めた代表点の位置及びその移動速度を意味する。各足平22の代表点は、本実施形態では、各足平22の底面上の点、例えば、各脚体2の足首関節の中心から各足平22の底面への垂線が該底面と交わる点に設定される。
また、「姿勢」は空間的な向きを意味する。例えば上体姿勢は鉛直方向に対するロール方向(X軸まわり)の上体24の傾斜角(姿勢角)とピッチ方向(Y軸まわり)の上体24の傾斜角(姿勢角)とで表され、足平姿勢は各足平22に固定的に設定された2軸の空間的な方位角で表される。本明細書では、上体姿勢は上体姿勢角ということもある。なお、上体姿勢にヨー方向(Z軸まわり)の上体24の回転角を含めてもよい。
また、歩容のうちの、ロボット1に作用する外力に係わる要素(目標ZMPおよび目標全床反力)以外の構成要素、すなわち足平位置姿勢、上体位置姿勢等、ロボット1の各部の運動に係わる歩容を総称的に「運動」という。
また、各足平22に作用する床反力(並進力及びモーメントからなる床反力)を「各足平床反力」と呼び、ロボット1の全て(2つ)の足平22R,22Lについての「各足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床反力はほとんど言及しないので、特にことわらない限り、「床反力」は「全床反力」と同義として扱う。
ZMPは、ロボット1の運動によって発生する慣性力とロボット1に作用する重力との合力がその点まわりに作用するモーメントの水平成分(水平軸まわりのモーメント成分)が零になる床面上の点を意味する。動力学的平衡条件を満足する歩容では、ZMPと床反力中心点とは一致する。この場合、目標ZMPを与えるということは、目標床反力中心点を与えるということと同じである。
目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントとにより表現される。作用点はどこに設定してもよいが、本実施形態では、目標ZMPを目標床反力の作用点とする。動力学的平衡条件を満足する歩容では、上記の如くZMPと床反力中心点とは一致するので、目標ZMPを作用点とする目標床反力のモーメント成分は、鉛直成分(Z軸まわりのモーメント成分)を除いて零になる。
歩容生成装置100により生成される目標歩容の運動のうち、目標腕姿勢軌道は、ロボット幾何学モデル(キネマティクス演算部)102に入力される。
また、目標上体位置姿勢軌道は、上体位置姿勢修正部122に入力される。そして、この上体位置姿勢修正部122は、入力された目標上体位置姿勢を、摂動モデル演算部120から与えられる値によって修正し、その修正後の目標上体位置姿勢(以下、修正目標上体位置姿勢という)をロボット幾何学モデル102に入力する。
ここで、本実施形態では、上体位置姿勢修正部122での修正対象は、より詳しくは、目標上体位置姿勢のうちの上体水平位置(水平方向での上体位置)であり、摂動モデル演算部120から上体位置姿勢修正部122に与えられる値は、目標上体水平位置の修正量としての上体水平位置摂動量である。そして、上体位置姿勢修正部122は、歩容生成装置100から入力される目標上体位置姿勢のうちの上体水平位置に、上記上体水平位置摂動量を合成する(加え合わせる)ことによって、修正目標上体位置姿勢の上体水平位置を決定する。また、上体位置姿勢修正部122は、歩容生成装置100から入力される目標上体位置姿勢のうちの上体水平位置以外の要素(上体鉛直位置及び上体姿勢)をそのまま、修正目標上体位置姿勢の当該要素として決定する。従って、本実施形態では、修正目標上体位置姿勢は、その上体水平位置だけを、歩容生成装置100が出力する目標上体位置姿勢から上体水平位置摂動量だけ修正してなるものとして決定される。そして、上体位置姿勢修正部122は、このように決定した修正目標上体位置姿勢をロボット幾何学モデル102に入力する。
また、目標足平位置姿勢軌道、目標ZMP軌道(目標床反力中心点軌道)、および目標全床反力軌道(詳細には目標並進床反力鉛直成分軌道、目標並進床反力水平成分軌道、目標ZMPまわりの目標床反力モーメント軌道)は、複合コンプライアンス動作決定部104と目標床反力分配器106とに入力される。
そして、目標床反力分配器106にて、目標床反力は各足平22R,22Lに分配され、目標各足平床反力中心点(各足平22R,22Lの床反力中心点の目標位置)と、目標各足平床反力(各足平22R,22Lの床反力中心点に作用させる目標床反力)とが決定される。この決定された目標各足平床反力中心点及び目標各足平床反力の軌道は複合コンプライアンス動作決定部104に入力される。なお、歩容生成装置100から出力する目標床反力は、複合コンプライアンス動作決定部104によるコンプライアンス制御に必要な成分だけを出力しても良い。例えば、目標並進床反力水平成分を歩容生成装置100から出力することを省略してもよい。
複合コンプライアンス動作決定部104では、目標足平位置姿勢を修正してなる機構変形補償付き修正目標足平位置姿勢が求められ、この修正目標足平位置姿勢の軌道がロボット幾何学モデル102に入力される。
ロボット幾何学モデル102は、入力された目標上体位置姿勢と機構変形補償付き修正目標足平位置姿勢とを満足する両脚体2,2の各関節の関節変位指令を、ロボット1のキネマティクスモデル(剛体リンクモデル)に基づく逆キネマティクス演算によって算出し、その算出した関節変位指令を変位コントローラ108に出力する。さらに、ロボット幾何学モデル102は、目標腕姿勢を満足する各腕体の各関節の関節変位指令を算出し、その算出した関節変位指令を変位コントローラ108に出力する。
そして、変位コントローラ108は、ロボット幾何学モデル102で算出された関節変位指令を目標値として、ロボット1の両脚体2,2及び両腕体の各関節の回転角(実関節変位)を前記サーボアンプ46を介して追従制御する。より詳しくは、変位コントローラ108は、エンコーダ44の出力から計測される実関節変位(実測値)を関節変位指令に一致させるように、アクチュエータ駆動力としての電動モータ42の出力トルクを調整する。
上記のような追従制御によるロボット1の実際の運動によって該ロボット1の各足平22に実際に作用する床反力である実各足平床反力が前記6軸力センサ36の出力から計測され、その実各足平床反力の実測値が前記複合コンプライアンス動作決定部104に入力される。
また、ロボット1の上体24の実際の姿勢角(鉛直方向に対する傾斜角)である実上体姿勢角が前記傾斜センサ40の出力から計測され、その実上体姿勢角の実測値がフィードバック操作量演算部110に入力される。さらに、フィードバック操作量演算部110には、前記修正目標上体位置姿勢のうちの修正目標上体姿勢角と、摂動モデル演算部120における後述する摂動動力学モデルの状態量(以下、摂動モデル状態量ということがある)とが入力される。
詳細は後述するが、フィードバック操作量演算部110は、入力された実上体姿勢角(実測値)と修正目標上体姿勢角との偏差である上体姿勢角偏差と、入力された摂動モデル状態量と該状態量に対する所定の目標値との偏差であるモデル状態量偏差とを“0”に近づける機能を有するフィードバック操作量として、全モデル操作床反力モーメントとコンプライアンス制御用目標床反力モーメントとを決定して出力する。
ここで、上記コンプライアンス制御用目標床反力モーメントは、上体姿勢角偏差とモデル状態量偏差とを“0”に近づける(収束させる)ために、実ロボット1に付加的に作用させる目標ZMPまわりの摂動床反力モーメントである。また、モデル操作用床反力モーメントは、コンプライアンス制御用目標床反力モーメントと同じ目的のために、歩容生成装置100で目標歩容を生成するために用いる後述の動力学モデルと、前記摂動モデル演算部120で上体水平位置摂動量を算出するために用いる後述の摂動動力学モデルとにおけるモデル上のロボットにそれぞれ付加的に作用させる目標ZMPまわりの摂動床反力モーメントの総和である。
言い換えれば、上体姿勢角偏差とモデル状態量偏差とを“0”に近づけるためのトータルのフィードバック操作量のうち、実ロボット1にフィードバックする操作量成分(実ロボット1の負担分)がコンプライアンス制御用目標床反力モーメント、歩容生成装置100で用いる動力学モデルと前記摂動モデル演算部120で用いる摂動動力学モデルとの全体に対してフィードバックするトータルの操作量成分(当該動力学モデルの全体の負担分)が、全モデル操作床反力モーメントである。
フィードバック操作量演算部110で決定されたフィードバック操作量のうち、全モデル操作床反力モーメントは、全モデル操作床反力モーメント分配器118に入力される。
詳細は後述するが、全モデル操作床反力モーメント分配器118は、入力された全モデル操作用床反力モーメントから、歩容生成装置100にフィードバックする成分としての第1モデル操作床反力モーメントと、摂動モデル演算部120にフィードバックする成分としての第2モデル操作床反力モーメントとを決定し、それぞれの成分を歩容生成装置100、摂動モデル演算部120に入力する。
上記第1モデル操作床反力モーメントは、全モデル操作床反力モーメントのうちの、歩容生成装置100の動力学モデルに対する分配分である。また、上記第2モデル操作床反力モーメントは、全モデル操作床反力モーメントのうちの、摂動モデル演算部120の摂動動力学モデルに対する分配分である。この場合、基本的には、全モデル操作床反力モーメントは低周波成分と高周波成分とに分離され、これらの成分のうちの低周波成分が第1モデル操作床反力モーメント、高周波成分が第2モデル操作床反力モーメントとして決定される。従って、第1モデル操作床反力モーメントは、低周波域での上体姿勢角偏差とモデル状態量偏差とを“0”に近づけるためのフィードバック操作量としての機能を持ち、第2モデル操作床反力モーメントは、高周波域での上体姿勢角偏差とモデル状態量偏差とを“0”に近づけるためのフィードバック操作量としての機能を持つ。
そして、歩容生成装置100は、該歩容生成装置100において決定される目標ZMPまわりの床反力モーメントの水平成分(詳しくは、歩容生成装置100で用いる動力学モデル上でのロボットにおいて、目標ZMPまわりに発生する床反力モーメントの水平成分)が、入力された第1モデル操作床反力モーメントに一致するように動力学モデルを用いて目標歩容の運動(目標運動)を逐次生成する。なお、歩容生成装置100から複合コンプライアンス動作決定部104に対して出力される目標ZMPは、該目標ZMPまわりの床反力モーメントの水平成分が“0”になることを目標とするものとして出力される。
また、摂動モデル演算部120は、摂動動力学モデル上でのロボットに付加的に作用する床反力モーメント(目標歩容の目標ZMPまわりに付加する摂動床反力モーメント)の水平成分が入力された第2モデル操作床反力モーメントに一致するように前記上体水平位置摂動量を算出し、この上体水平位置摂動量を前記上体位置姿勢修正部122に出力する。
また、フィードバック操作量演算部110で決定されたフィードバック操作量のうち、コンプライアンス制御用目標床反力モーメントは、複合コンプライアンス動作決定部104に入力される。そして、複合コンプライアンス動作決定部104は、歩容生成装置100が生成する目標歩容の運動(上体位置姿勢については、修正目標上体位置姿勢)にロボット1の実際の運動を追従させつつ、目標ZMPまわりの実床反力モーメントがコンプライアンス制御用目標床反力モーメントに近づくように、目標足平位置姿勢を修正することにより、機構変形補償付き修正目標足平位置姿勢(軌道)を決定する。
この場合、ロボット1の足平位置姿勢及び床反力の全ての状態を目標に一致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥協的になるべく一致させる。すなわち、複合コンプライアンス動作決定部104は、各目標に対する制御偏差に重みを与えて、制御偏差(あるいは制御偏差の2乗)の重み付き平均が最小になるように機構変形補償付き修正目標足平位置姿勢(軌道)を決定する。
換言すれば、目標ZMPまわりの実床反力モーメントと、ロボット1の実際の足平位置姿勢とがそれぞれコンプライアンス制御用目標床反力モーメント、目標足平位置姿勢にできるだけ近づくように機構変形補償付き修正目標足平位置姿勢(軌道)が決定される。そして、複合コンプライアンス動作決定部104は、この修正目標足平位置姿勢をロボット幾何学モデル102に足平位置姿勢の最終的な目標値として出力することで、ロボット1の動作を制御する。
なお、上記した複合コンプライアンス動作決定部104などの構成および動作は、本出願人が先に出願した特開平10−277969号公報などに詳細に記載されている。従って、複合コンプライアンス動作決定部104に関する本明細書での説明は以上に留める。
以上が、本実施形態における制御ユニット26の処理の概略である。なお、本実施形態では、図3に示した制御ユニット26の処理機能のうち、歩容生成装置100を除く機能部分の演算処理は、比較的短めの制御周期(例えば1ms)で高速に実行される。これに対して、歩容生成装置100の演算処理は、他の機能部分の演算処理の制御周期よりも長めの制御周期(例えば100ms)で実行される。
次に、前記歩容生成装置100、フィードバック操作量演算部110、全モデル操作床反力モーメント分配器118、摂動モデル演算部120の処理の詳細を説明する。
まず、図4に示す走行歩容を例にして、歩容生成装置100が生成する目標歩容をより具体的に説明する。
なお、以降の説明では、「床反力鉛直成分」、「床反力水平成分」は、それぞれ、特に断らない限り、「並進床反力鉛直成分」、「並進床反力水平成分」を意味するものとする。
また、歩容における「両脚支持期」とは、ロボット1がその自重を両脚体2,2で支持する期間、「片脚支持期」とはいずれか一方のみの脚体2でロボット1の自重を支持する期間、「空中期」とは両脚体2,2が床から離れている(空中に浮いている)期間を言う。また、片脚支持期においてロボット1の自重を支持する側の脚体2を「支持脚」、自重を支持しない側の脚体2を「遊脚」と呼ぶ。本実施形態で主に説明する走行歩容では、両脚支持期は無く、片脚支持期(着地期)と空中期とが交互に繰り返される。この場合、空中期では両脚2,2とも、ロボット1の自重を支持しないこととなるが、該空中期の直前の片脚支持期において遊脚であった脚体2、支持脚であった脚体2をそれぞれ該空中期においても遊脚、支持脚と呼ぶ。
まず、図4に示す走行歩容を説明すると、この走行歩容は、人間の通常的な走行歩容と同様の歩容である。この走行歩容では、ロボット1の左右いずれか一方のみの脚体2(支持脚)の足平22が着地(接地)する片脚支持期と、両脚体2,2が空中に浮く空中期とが交互に繰り返される。
図4中の丸付きの数字は、走行歩容の時系列的な順番を示している。この場合、図4の1番目の状態は片脚支持期の開始時(初期)の状態、2番目の状態は片脚支持期の中間時点の状態、3番目の状態は片脚支持期に続く空中期の開始時(片脚支持期の終了時)の状態、4番目の状態は空中期の中間時点の状態、5番目の状態は空中期の終了時(次の片脚支持期の開始時)の状態を示している。また、図4中の白抜き矢印は、ロボット1の進行方向を示している。
この走行歩容では、ロボット1は、図4の1番目の状態で示すように、片脚支持期の開始時において支持脚(ロボット1の進行方向前側の脚体2)の足平22のかかとで着地する。
続いて、図4の2番目の状態で示すように、ロボット1は、着地した足平22(支持脚の足平22)の底面のほぼ全面を着地させた後、図4の3番目の状態で示すように、その支持脚の足平22(図4の3番目の状態におけるロボット1の進行方向後側の脚体2の足平22)のつま先で床を蹴って空中に飛び上がる。これより片脚支持期が終了すると共に空中期が開始する。尚、片脚支持期における遊脚は、図4の1番目の状態で示すように、片脚支持期の開始時では、支持脚の後側に存在する。その後、該遊脚は、図4の2番目及び3番目の状態で示すように次の着地予定位置に向かって支持脚の前側に振り出される。
次いで、図4の4番目の状態で示す空中期を経た後、ロボット1は、遊脚(空中期の開始直前の片脚支持期で遊脚となっていた脚体2)の足平22のかかとで着地し、次の片脚支持期が開始される。
図4の走行歩容を想定しつつ、前記歩容生成装置100が生成する目標歩容の基本的な概要を説明する。
歩容生成装置100が目標歩容を生成するとき、ロボット1の運動形態に関する基本的な要求を表す要求パラメータが、ロボット1の外部の図示しない操縦装置やサーバ等から無線通信等により歩容生成装置100に入力される。該要求パラメータは、例えば、ロボット1の運動種別(歩行、走行等)や、遊脚側の足平22の目標とする着地位置姿勢(着地予定位置姿勢)、目標とする着地時刻(着地予定時刻)、あるいは、これらを決定するために必要なパラメータ(例えばロボット1の平均移動速度や移動方向等)により構成される。なお、このような要求パラメータは、ロボット1の図示しない記憶装置に事前に記憶保持しておき、それを歩容生成装置100が所定のスケジュールで読み込むようにしてもよい。また、歩容生成装置100には、ロボット1の図示しない記憶装置、あるいは外部のサーバ等から、ロボット1の移動環境の床形状情報も随時入力される。
なお、床形状情報に関して補足すると、例えばロボット1に搭載した撮像カメラによって、ロボット1の進行方向前方の環境を撮像し、その撮像画像を基に、制御ユニット26において、床形状情報を取得するようにしてもよい。また、例えば、ロボット1の移動環境の床形状情報を含む地図情報をロボット1の制御ユニット26にあらかじめ記憶保持しておくと共に、ロボット1に自己位置認識機能を持たせておき、その認識した自己位置を地図情報と照合することで、ロボット1の周辺の床形状情報を制御ユニット26で取得するようにしてもよい。また、床形状情報には、床の硬さや摩擦係数等の性状情報が含まれていてもよい。
歩容生成装置100は、上記要求パラメータや床形状情報を用いて所定のアルゴリズムにより目標歩容を生成する。より詳しく言えば、本実施形態では、歩容生成装置100は、上記要求パラメータや床形状情報に応じて、目標歩容の目標足平位置姿勢軌道、目標床反力鉛直成分軌道等、目標歩容の一部の構成要素を規定するパラメータとしての歩容パラメータを決定した上で、その歩容パラメータとロボット1の動力学モデルとを用いて目標歩容の瞬時値を逐次決定する。これにより歩容生成装置100は、目標歩容の時系列パターン(軌道)を生成する。
この場合、目標足平位置姿勢軌道は、例えば本願出願人が特許第3233450号にて提案した有限時間整定フィルタを用いて各足平22毎に生成される。この有限時間整定フィルタは、可変時定数の1次遅れフィルタ、すなわち、伝達関数が1/(1+τs)の形で表されるフィルタ(τは可変の時定数。以下、このフィルタを単位フィルタという)を複数段(本実施形態では3段以上)、直列に接続したものであり、所望の指定時刻に指定値に到達するような軌道を生成・出力することができるものである。
この場合、各段の単位フィルタの時定数τは、いずれも、有限時間整定フィルタの出力生成を開始してから、上記指定時刻までの残時間に応じて逐次可変的に設定される。
より詳しくは、該残時間が短くなるに伴いτの値が所定の初期値(>0)から減少されていき、最終的には、該残時間が“0”になる指定時刻にて、τの値が“0”になるように設定される。そして、有限時間整定フィルタには、前記指定値(より詳しくは、有限時間整定フィルタの出力の初期値から前記指定値への変化量)に応じた高さのステップ入力が与えられる。このような有限時間整定フィルタは、指定時刻にて指定値に達するような出力が生成されるだけでなく、指定時刻における有限時間整定フィルタの出力の変化速度を“0”もしくはほぼ“0”にすることができる。特に、単位フィルタを3段以上(3段でよい)、接続した場合には、有限時間整定フィルタの出力の変化加速度(変化速度の微分値)をも“0”もしくはほぼ“0”にすることができる。
このような有限時間整定フィルタを用いる足平位置姿勢軌道(足平22が着地してから次に着地するまでの位置姿勢軌道)の生成は、例えば次のように行なわれる。例えばX軸方向(前後方向)の目標足平位置軌道は次のように生成される。すなわち、前記要求パラメータにより規定される各足平22の次の着地予定位置のX軸方向位置(より詳しくは、次の着地予定位置のひとつ前の着地位置に対するX軸方向の変化量(移動量)。これは前記指定値に相当する)に応じて有限時間整定フィルタへのステップ入力の高さが決定されると共に前記時定数τが所定の初期値に初期化された後、その決定されたステップ入力が有限時間整定フィルタに与えられ、足平22のX軸方向位置の軌道生成が開始される。そして、この軌道生成時には、前記時定数τは、足平22の着地予定時刻(これは前記指定時刻に相当する)までに初期値から“0”まで減少していくように、逐次可変設定される。これにより、着地予定時刻で着地予定位置に達するような、足平22のX軸方向の位置の軌道が生成される。
また、Z軸方向(鉛直方向)の目標足平位置軌道は、例えば次のように生成される。すなわち、まず、足平22の次の着地予定位置及び着地予定時刻に応じて、該足平22の高さ(鉛直位置)が最大になるときの該足平22のZ軸方向位置(以下、最高点位置という)とその最高点位置への到達時刻とが決定される。そして、その最高点位置(これは前記指定値に相当する)に応じて有限時間整定フィルタへのステップ入力の高さが決定されると共に時定数τが初期化された後、その決定されたステップ入力が有限時間整定フィルタに与えられ、前記最高点位置までのZ軸方向の足平位置軌道が逐次生成される。この際、時定数τは、最高点位置への到達時刻(前記指定時刻に相当)までに初期値から0まで減少するように逐次可変設定される。さらに、最高点位置までのZ軸方向位置の軌道の生成が終了したら、時定数τを初期化すると共にいままでのステップ入力と逆極性のステップ入力(より詳しくは、最高点位置から次の着地予定位置までのZ軸方向の変化量(これは前記指定値に相当する)に応じた高さの逆極性のステップ入力)が有限時間整定フィルタに入力され、該最高点位置から着地予定位置までのZ軸方向の足平位置の軌道が逐次生成される。この際、時定数τは足平22の着地予定時刻までに初期値から0まで減少するように逐次可変設定される。
尚、Z軸方向の足平位置軌道の生成においては、時定数τを軌道生成開始時刻から足平22の着地予定時刻まで、初期値から“0”まで継続的に減少するように可変設定すると共に、最高点位置への到達時刻もしくはその近傍時刻で、ステップ入力の極性を逆極性に切り替えることで、Z軸方向の足平位置軌道を生成するようにしてもよい。この場合には、足平22を所望の最高点位置に精度よく到達させることはできないが、着地予定時刻での着地予定位置への到達は問題なく行なうことができる。
足平姿勢軌道についても、上述した足平位置軌道と同様に有限時間整定フィルタを用いて生成することができる。この場合、足平姿勢の空間的な各成分のうち、その姿勢の角度変化が単調的(単調増加もしくは単調減少)なものとなる成分については、前記したX軸方向の足平位置軌道の生成と同様に足平姿勢軌道を生成すればよい。また、姿勢の角度変化が極大値もしくは極小値をもつような成分については、前記したZ軸方向の足平位置軌道の生成と同様に足平姿勢軌道を生成すればよい。
尚、上記のように有限時間整定フィルタにより生成される目標足平位置姿勢軌道は、後述の支持脚座標系での各足平22の目標位置姿勢軌道である。
上述のように生成される目標足平位置姿勢軌道は、各足平22の位置が、その初期接地状態(目標歩容の初期時刻の状態)から着地予定位置に向かって徐々に加速しながら移動を開始するように生成される。そして、該目標足平位置姿勢軌道は、最終的に着地予定時刻までに徐々に位置の変化速度を“0”またはほぼ“0”にまで減速し、着地予定時刻にて着地予定位置に到達して停止するように生成される。このため、各足平22の着地瞬間における対地速度(床に固定された支持脚座標系での各足平22の位置の変化速度)が“0”またはほぼ“0”になる。したがって、走行歩容において同時に全脚体2,2が空中に存在する状態(空中期での状態)から着地しても、着地衝撃が小さくなる。
前記走行歩容においては、ロボット1に作用する重力によって空中期後半から上体24の鉛直速度は下向きになり、着地時でも下向きのままである。したがって、上記のように各足平22の着地瞬間における対地速度が“0”またはほぼ“0”になるように目標足平位置姿勢軌道を生成すると共に、後述する如く動力学的平衡条件を満たすように上体24の目標位置姿勢軌道を生成したとき、着地直前において、上体24に対する遊脚側の足平22の相対速度は、上向きになる。すなわち、走行歩容の着地瞬間では、ロボット1の目標歩容は遊脚側の脚体2を上体24側に引っ込めながら着地するような歩容となる。言い換えれば、本実施形態での目標歩容では、ロボット1は、着地瞬間において、遊脚側の足平22の対地速度が“0”またはほぼ“0”になるように、上体24から見て該足平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”またはほぼ“0”になるように設定された多項式などの関数を用いて目標足平位置軌道を生成するようにしても良い。このことは、目標足平姿勢軌道の生成に関しても同様である。但し、該目標足平姿勢軌道の生成に関しては、上述の如く、各足平22の底面のほぼ全面が床に接地する時刻にて、各足平22の姿勢の変化速度、さらにはその変化加速度が“0”またはほぼ“0”になるように多項式などの関数が設定される。
歩容生成装置100は目標床反力鉛直成分を明示的に設定する。この目標床反力鉛直成分軌道は、例えば図5のように設定される。本実施形態では、走行歩容における目標床反力鉛直成分軌道の形状(詳しくは片脚支持期での形状)は、台形状(床反力鉛直成分の増加側に凸の形状)に定められている。そして、その台形の高さ、折れ点の時刻が目標床反力鉛直成分軌道を規定する歩容パラメータ(床反力鉛直成分軌道パラメータ)として決定される。
尚、走行歩容の空中期では、目標床反力鉛直成分は定常的に“0”に設定される。この例のように、目標床反力鉛直成分軌道は、実質的に連続になるように(値が不連続にならないように)設定するのがよい。これは床反力を制御する際のロボット1の関節の動作を滑らかにするためである。ここで、「実質的に連続」というのは、アナログ的に連続な軌道(真の意味での連続な軌道)を離散時間系でデジタル表現したときに必然的に生じる値の飛びは、該軌道の連続性を失わせるものではないということを意味するものである。
補足すると、本実施形態では、ロボット1に作用する外力の全体として、床反力を想定しているので、目標床反力鉛直成分は、ロボット1全体の(ロボット1の全体重心の)鉛直方向の慣性力を規定するものとなる。すなわち、目標床反力鉛直成分から、ロボット1全体に作用する重力に釣り合う成分を差し引いたものが、ロボット1全体の鉛直方向の慣性力に釣り合うものとなる。従って、目標床反力鉛直成分を決定することによって、結果的には、ロボット1全体の鉛直方向の慣性力が決定されることとなる。
目標ZMP軌道は,例えば図6(a),(b)に示す如く設定される。図4の走行歩容においては、ロボット1は、前記したように支持脚側足平22のかかとで着地し、次にその支持脚側足平22のつま先で蹴って空中に飛び上がり、最後に遊脚側足平22のかかとで着地する。従って、片脚支持期での目標ZMP軌道のX軸方向位置(前後方向位置)は、図6(a)に示すように、支持脚側足平22のかかとを初期位置として、次に支持脚側足平22の底面のほぼ全面が接地する期間において該足平22の前後方向の中央に移動し、その後、離床時までに支持脚側足平22のつまさきに移動するように設定される。尚、片脚支持期における目標ZMP軌道のY軸方向位置(左右方向位置)は、図6(b)に示すように、支持脚側脚体2の足首関節の中心のY軸方向位置と同じ位置に設定される。
図6(a)に示すように、空中期における目標ZMP軌道のX軸方向位置は、該空中期が終了するまで(遊脚側脚体2が着地するまで)に支持脚側足平22のつま先から遊脚側足平22のかかとの着地位置まで連続的に移動するように設定される。また、図6(b)に示すように、空中期における目標ZMP軌道のY軸方向位置は、該空中期が終了するまで(遊脚側脚体2が着地するまで)に支持脚側脚体2の足首関節の中心のY軸方向位置から遊脚側脚体2の足首関節の中心のY軸方向位置まで連続的に移動するように設定される。すなわち、目標ZMP軌道は歩容の全期間において連続(実質的に連続)する軌道に設定される。ここで、上記したZMP軌道の「実質的に連続」の意味は、前記床反力鉛直成分軌道の場合と同様である。
本実施形態では、図6(a),(b)に示したような目標ZMP軌道の折れ点の位置や時刻が、ZMP軌道パラメータ(目標ZMP軌道を規定するパラメータ)として設定される。
ZMP軌道パラメータは、安定余裕が高く、かつ急激な変化をしないように決定される。ここで、ロボット1の接地面を含む最小の凸多角形(いわゆる支持多角形)の中央付近に目標ZMPが存在する状態を安定余裕が高いと言う(詳細は特開平10−86081号公報を参照)。図6(a),(b)の目標ZMP軌道はこのような条件を満たすように設定したものである。
また、目標腕姿勢は、上体24に対する相対姿勢で表される。
また、目標上体位置姿勢及び目標足平位置姿勢はグローバル座標系で記述される。グローバル座標系は床に固定された座標系であり、より具体的には、以下に説明する支持脚座標系が用いられる。該支持脚座標系は、支持脚側足平22を床面に平行な姿勢にして該支持脚側足平22の底面のほぼ全面を床面に接触(密着)させた状態において、該支持脚側脚体2の足首中心から床面に延ばした垂線が該床面と交わる点(この点は、本実施形態の例では支持脚側足平22の底面のほぼ全面を床面に接触させた状態では、該足平22の代表点と合致する)を原点とし、その原点を通る水平面をXY平面とする、床に固定された座標系である。この場合、X軸方向、Y軸方向は、それぞれ支持脚側足平22の前後方向、左右方向である。
尚、支持脚座標系の原点は、必ずしも支持脚側足平22の底面のほぼ全面を床面に接触させた状態での該足平22の代表点(足平22の位置を代表する点)に合致する必要はなく、該代表点と異なる床面上の点に設定されてもよい。
次に、歩容生成装置100での歩容生成に用いられるロボット1の動力学モデルについて図7を参照して説明する。
本実施形態に用いられるロボット1の動力学モデルは、図7に示す如く、目標ZMPを支点として揺動自在な可変長のロッド24aと、このロッド24aの上端に支持された質点24bとを備える倒立振子により構成されるモデル(以下、倒立振子モデルということがある)である。なお、ロッド24aは、質量を持たないものとする。この動力学モデルでは、倒立振子の質点24bの運動が、ロボット1の上体24の運動に対応しており、上体24の運動とロボット1に作用する床反力との間の関係が、質点24b(以降、上体質点24bという)の運動と、倒立振子に作用する床反力との間の関係として表現される。また、この動力学モデルでは、ロボット1の脚体2,2の質量は、上体24(または上体24と腕体及び頭部を合わせた質量)よりも十分に小さいものとみなし、上体質点24bの質量は、ロボット1の全体の質量に一致するものとされる。なお、上体質点24bの位置は、ロボット1の上体位置姿勢から一義的に定まる位置に設定される。
この動力学モデルの挙動(該動力学モデル上でのロボットの動力学)は、以下に示すように数式化される。ただし、説明を簡単にするために、サジタルプレーン(支持脚座標系のX軸とZ軸とを含む平面)での運動方程式のみを記述し、ラテラルプレーン(支持脚座標系のY軸とZ軸とを含む平面)での運動方程式を省略した。
説明の便宜上、動力学モデルに関する変数およびパラメータを以下のように定義する。
Zb:上体質点鉛直位置(Z軸方向位置)
Xb:上体質点水平位置(X軸方向位置)
mb:上体質点質量
Xzmp:目標ZMPの水平位置(X軸方向位置)
Zzmp:目標ZMPの鉛直位置(Z軸方向位置)
Fx:床反力水平成分(詳しくは並進床反力のX軸方向成分)
Fz:床反力鉛直成分(詳しくは並進床反力のZ軸方向成分)
Mzmp_y:目標ZMPまわりの床反力モーメント(詳しくは該床反力モーメントのY軸まわり成分)
Mb_y:支持脚座標系の原点まわりの床反力モーメント(詳しくは該床反力モーメントのY軸まわり成分)
また、任意の変数Aに対して、dA/dtはAの1階微分値を表わし、d2A/dt2はAの2階微分値を表わす。したがって、変数Aが変位(位置)ならば、dA/dtは速度、d2A/dt2は加速度を意味する。gは重力加速度定数を示す。ここでは、gは正の値とする。
上記動力学モデルの運動方程式は、式01〜04で表される。
Fz=mb*(g+d2Zb/dt2) ……式01
Fx=mb*d2Xb/dt2 ……式02
Mb_y=−mb*Xb*(g+d2Zb/dt2)
+mb*Zb*(d2Xb/dt2) ……式03
Mzmp_y=−mb*(Xb−Xzmp)*(g+d2Zb/dt2)
+mb*(Zb−Zzmp)*(d2Xb/dt2) ……式04
この場合、Mb_yとMzmp_yとの間の関係は、次式05により表される。
Mb_y=Mzmp_y−mb*Xzmp*(g+d2Zb/dt2)+mb*Zzmp*(d2Xb/dt2)
=Mzmp_y−Xzmp*Fz+Zzmp*Fx
……式05
なお、目標ZMPと支持脚座標系の原点との鉛直方向(Z軸方向)での位置の差は通常、“0”又はほぼ“0”となるので、Zzmp=0と見なしてもよい。
また、ロボット1の移動時の上体24の高さ(鉛直方向位置)の、平均的な高さに対する相対的な変動量(換言すれば上体24の鉛直方向位置の分散)は一般には十分に小さいので、Zb≒h(h:上体24の平均的な高さに対応する上体質点24bの平均的な高さとしての一定値)と見なすことができる。従って、上記式03の右辺第2項と式04の右辺第2項とにおけるZbを定数値hで置き換えてもよい。
本実施形態における歩容生成装置100は、ロボット1の片方の脚体2が着地してから他方の脚体2が着地するまでの1歩分の目標歩容を単位として、その1歩分の目標歩容を順番に生成する。従って、本実施形態で生成する図4の走行歩容では、該目標歩容は片脚支持期の開始時から、これに続く空中期の終了時(次の片脚支持期の開始時)までの目標歩容が順番に生成される。
ここで、本明細書では、目標歩容の「1歩」は、ロボット1の片方の脚体2が着地してからもう一方の脚体2が着地するまでの意味で使用する。また、新たに生成しようとしている目標歩容を「今回歩容」、その次の目標歩容を「次回歩容」、さらにその次の目標歩容を「次次回歩容」、というように呼ぶ。また、「今回歩容」の一つ前に生成した目標歩容を「前回歩容」と呼ぶ。
また、歩容生成装置100が今回歩容を生成するとき、該歩容生成装置100には、ロボット1の2歩先までの遊脚側足平22の着地予定位置姿勢、着地予定時刻を規定する要求パラメータが入力される(あるいは歩容生成装置100が記憶装置から要求パラメータを読み込む)。そして、歩容生成装置100は、これらの要求パラメータを用いて、目標上体位置姿勢軌道、目標足平位置姿勢軌道、目標ZMP軌道、目標床反力鉛直成分軌道、目標腕姿勢軌道等を生成する。
このとき、歩容生成装置100は、ロボット1の運動の継続性を確保し得る今回歩容を生成するために、今回歩容に続く仮想的な周期的歩容(同じパターンのロボット1の運動が一定周期で継続的に繰り返される歩容)としての定常旋回歩容を要求パラメータに応じて決定する。そして、歩容生成装置100は、今回歩容を将来的に定常旋回歩容に収束させるようにして、該今回歩容を生成する。
以下に図4の走行歩容を生成することを主要例にして、歩容生成装置100の歩容生成処理の詳細の詳細を図8〜図21を参照しつつ説明する。
歩容生成装置100は、図8のフローチャート(構造化フローチャート)に示す歩容生成処理(メインルーチン処理)を実行することによって目標歩容を生成する。
まずS010において時刻tを“0”に初期化するなど種々の初期化作業が行なわれる。この処理は、歩容生成装置100の起動時等に行なわれる。
次いで、S012を経てS014に進み、歩容生成装置100は、制御周期(図8のフローチャートの演算処理周期)毎のタイマ割り込みを待つ。制御周期はΔtである。
次いで、S016に進み、歩容生成装置100は、歩容の切り替わり目であるか否かを判断する。このとき、歩容の切り替わり目である場合には、S018の処理を経て、S020に進む。この場合、S018では、歩容生成装置100は、時刻tを“0”に初期化する。また、S016で歩容の切り替わり目でない場合には、S020に進む。
ここで、上記「歩容の切り替わり目」は、前回歩容の生成が完了し、今回歩容の生成を開始するタイミングを意味し、例えば前回歩容の生成を完了した制御周期の次の制御周期が歩容の切り替わり目になる。
次いで、S020では、歩容生成装置100は、次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期を決定する。なお、支持脚座標系を決定するということはその原点の位置と該支持脚座標系の姿勢(各座標軸の向き)とを決定することを意味する。
これらの支持脚座標系及び歩容周期は、基本的には、前記要求パラメータに基づき決定されるものである。すなわち、本実施形態では、歩容生成装置100に入力される要求パラメータは、2歩先までの遊脚側足平22の着地予定位置姿勢(足平22が着地してから足底を床面にほぼ全面的に接触させるように、滑らさずに回転させた状態での足平位置姿勢)と着地予定時刻とを規定する要求値を含んでいる。そして、その1歩目の要求値、2歩目の要求値がそれぞれ、今回歩容、次回歩容に対応するものとして、今回歩容の生成開始時(前記S016の歩容の切り替わり目)以前に歩容生成装置100に与えられる。
そして、歩容の切り替わり目の制御周期(t=0となる制御周期)において、上記要求パラメータにおける1歩目の遊脚側足平22(今回歩容での遊脚側足平22)の着地予定位置姿勢の要求値に対応して次回歩容支持脚座標系が決定される。
例えば図11を参照して、今回歩容(1歩目)に係わる遊脚側足平22(図では22L)の着地予定位置姿勢の要求値が、今回歩容の支持脚側足平22(図では22R)の着地位置姿勢に対して、今回歩容支持脚座標系のX軸方向(今回歩容の支持脚側足平22Rの前後方向)及びY軸方向(今回歩容の支持脚側足平22Rの左右方向)に、それぞれxnext、ynextだけ移動し、且つZ軸まわり(鉛直軸まわり)にθznextだけ回転させた位置姿勢であるとする。
このとき、次回歩容支持脚座標系は、図示のように今回歩容の遊脚側足平22Lの着地予定位置姿勢の要求値に従って該足平22Lを着地させた場合(足平22の代表点を着地予定位置の要求値に一致させ、且つ、足平22の姿勢(向き)を着地予定姿勢の要求値に一致させた場合)における該足平22Lの代表点(より詳しくは該代表点に合致する床上の点)を原点とし、該原点を通る水平面内における該足平22Lの前後方向、左右方向をそれぞれX’軸方向、Y’軸方向とする座標系に決定される。
上記と同様に、2歩目の遊脚側足平22の着地予定位置姿勢の要求値に応じて次々回歩容支持脚座標系(図11のX”Y”座標を参照)が決定される。また、今回歩容周期は、今回歩容の支持脚側足平22の着地予定時刻(要求値)から、1歩目(今回歩容)の遊脚側足平22の着地予定時刻(要求値)までの時間として決定され、次回歩容周期は、1歩目の遊脚側足平22の着地予定時刻(要求値)から2歩目の遊脚側足平22の着地予定時刻(要求値)までの時間として決定される。
また、歩容の切り替わり目以後の制御周期(t>0となる制御周期)においては、次回歩容支持脚座標系、次次回歩容支持脚座標系、今回歩容周期および次回歩容周期は、基本的には、前回の制御周期において決定されたものと同じに設定される。
尚、前記要求パラメータは、例えば、ロボット1の外部の操縦装置あるいはサーバ等から歩容生成装置100に入力される。あるいは、要求パラメータは、ロボット1の移動スケジュールとしてロボット1の記憶装置にあらかじめ記憶保持されていてもよい。あるいは、操縦装置からの指令(要求)とそのときまでのロボットの1の移動履歴とを基に前記次回及び次次回歩容支持脚座標系、並びに今回及び次回歩容周期を決定してもよい。また、要求パラメータは、前記次回及び次次回歩容支持脚座標系の位置及び姿勢、並びに今回及び次回歩容周期を直接的に指定するパラメータであってもよい。
補足すると、本実施形態では、今回歩容及び次回歩容の一方又は両方におけるロボット1の遊脚側足平22の着地予定位置姿勢(ひいては、次回歩容支持脚座標系及び次次回歩容支持脚座標系のうちの少なくともいずれか一方の位置姿勢)、あるいは、今回歩容もしくは次回歩容の歩容周期は、各制御周期において、S020で最初に決定されたものから、修正される場合もある。すなわち、各制御周期において、S020の処理が、複数回実行される場合もある。
次いでS022に進み、歩容生成装置100は、今回歩容に続く仮想的な周期的歩容(今回歩容の収束目標とする仮想的な周期的歩容)としての定常旋回歩容の歩容パラメータを決定する。該歩容パラメータは、本実施形態では、定常旋回歩容における目標足平位置姿勢軌道を規定する足平軌道パラメータ、目標上体姿勢軌道を規定する上体姿勢軌道パラメータ、目標腕姿勢軌道を規定する腕姿勢軌道パラメータ、目標ZMP軌道を規定するZMP軌道パラメータ、目標床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメータを含む。
ここで、「定常旋回歩容」は、その歩容を繰り返したときに歩容の境界(1周期毎の歩容の境界)においてロボット1の運動状態(足平位置姿勢、上体位置姿勢等の運動状態)に不連続が生じないような周期的歩容を意味する。また、「定常旋回歩容」は、周期的歩容、すなわち同じパターンの歩容を一定周期で繰り返す歩容であるから、ロボット1の運動を継続的に行い得る歩容である。換言すれば、「定常旋回歩容」は、歩容の軌道の不連続を発生することなく、同じパターンの運動を繰り返すことができる周期的歩容(原理上、無限回繰り返しても後述の「発散」が発生しない歩容)である。
周期的歩容である定常旋回歩容は、本実施形態では、ロボット1の2歩分の歩容、すなわち今回歩容に続く第1旋回歩容と該第1旋回歩容に続く第2旋回歩容とからなる歩容を1周期分の歩容として、その1周期分の歩容を一定周期で繰り返す歩容である。ここで「旋回」なる用語を用いたのは、旋回率を零とするときは直進を意味するので、直進も広義の意味で旋回に含ませることができるからである。
また、生成する目標歩容が前記した図4の走行歩容である場合には、定常歩容の第1旋回歩容及び第2旋回歩容は、いずれも目標歩容と同様に片脚支持期と空中期とを有する歩容である。つまり、第1旋回歩容及び第2旋回歩容の基本的な歩容形態は今回歩容と同一である。
定常旋回歩容(以降、単に定常歩容ということがある)について補足すると、2足移動ロボットでは、定常歩容の1周期分の歩容には、少なくとも2歩分の歩容が含まれる必要である。この場合、3歩以上の歩容を1周期分の歩容とする複雑な定常歩容を設定することも可能である。但し、定常歩容は、後述の如く、今回歩容の終端(終了時刻)における目標とする(適切な)発散成分の値を決定するためだけに用いられる。このため、3歩以上の歩容を1周期とする定常歩容を用いることは、歩容生成の処理が煩雑となるにもかかわらず、効果は少ない。そこで、本実施形態での定常歩容の1周期分の歩容を2歩分の歩容(第1旋回歩容及び第2旋回歩容の組)により構成するようにしている。尚、3足以上の脚式移動ロボットにあっては、それに応じて定常歩容を定義するに足る歩容数が増加する。
ここで、「発散」とは、図9に示すように、ロボット1の上体24の位置が両足平22,22の位置からかけ離れた位置にずれてしまうことを意味する。また、発散成分の値とは、ロボット1の上体24の位置が両足平22,22の位置(より具体的には、支持脚側足平22の接地面に設定された支持脚座標系の原点)からかけ離れていく具合を表す数値である。
定常歩容は、歩容生成装置100で今回歩容の終端におけるロボット1の目標とする運動状態を決定するために作成される仮想的な歩容である。従って、定常旋回歩容は、歩容生成装置100からそのまま出力されるものではない。
本実施形態では、目標歩容が、上記発散を生じることなく、継続的に生成されるように、発散成分を指標にして歩容を生成するようにした。この場合、継続的な歩容の代表例である定常歩容であっても、定常歩容の歩容パラメータが変われば、該定常歩容の初期発散成分(定常歩容の初期時刻での発散成分)も変わる。すなわち、歩き方、走り方、移動速度等の歩容形態によって、適切な発散成分が変わる。
そこで、本実施形態では、歩容生成装置100は、今回歩容を生成しようとするときに、まず、その生成しようとする今回歩容の後に続く歩容(将来的な収束目標とする歩容)として好適な将来の仮想的な周期的歩容(ロボット1の安定な移動を継続することが可能な将来の仮想的な周期的歩容)としての定常歩容を、今回歩容に係わる要求パラメータ(又はS020で決定した支持脚座標系及び歩容周期)に応じて決定した上で、該定常歩容の初期発散成分を求める。そして、歩容生成装置100は、今回歩容の終端発散成分を、その求めた定常歩容の初期発散成分に一致させる(より一般的には、今回歩容を定常歩容に連続させもしくは漸近させる)ように、今回歩容を生成する。このような歩容生成の基本的な指針は本出願人が先に提案した特許第3726081号のものと同様である。
ただし、本実施形態では、歩容の発散成分は、前記した図7の動力学モデル(倒立振子モデル)の状態方程式に基づいて後述するように定義される。該発散成分の定義の詳細については後述する。
本題に戻り、S022では、歩容生成装置100は、図10のフローチャートに示すサブルーチン処理を実行する。
まず、S100において、歩容生成装置100は、今回歩容、第1旋回歩容、第2旋回歩容の順に足平位置姿勢軌道がつながるように、定常歩容の歩容パラメータのうちの足平軌道パラメータが決定される。以下に具体的な設定方法を図11を参照して説明する。尚、以降の説明では、支持脚側の脚体2の足平22を支持脚足平22と称し、遊脚側の脚体2の足平22を遊脚足平22と称する。また、歩容の「初期」、「終端」はそれぞれ歩容の開始時刻、終了時刻、あるいは、それらの時刻における瞬時歩容を意味する。
足平軌道パラメータは、第1旋回歩容および第2旋回歩容のそれぞれの初期と終端とにおける支持脚足平22及び遊脚足平22のそれぞれの位置姿勢、各旋回歩容の歩容周期(各旋回歩容の初期から終端までの時間)等から構成される。この足平軌道パラメータのうち、第1旋回歩容初期遊脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とされる。
この場合、走行歩容では、今回歩容終端における支持脚足平22は、空中に移動している。そして、今回歩容終端支持脚足平位置姿勢は、今回歩容初期支持脚足平位置姿勢(=前回歩容終端遊脚足平位置姿勢)から、前記要求パラメータにおける2歩目の遊脚足平22の着地予定位置姿勢の要求値(今回歩容の支持脚足平22の次回歩容における着地予定位置姿勢の要求値)もしくは該要求値に対応する次次回歩容支持脚座標系に応じて定まる次回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道(詳しくは次回歩容支持脚座標系から見た軌道)を、今回歩容終端まで前記有限時間整定フィルタを用いて生成することにより求められる。
尚、次回歩容終端遊脚足平位置姿勢は、その位置姿勢から水平姿勢まで、足平22を接地させたまま、つま先を下げるようにピッチ方向に回転させたときの該足平22の位置姿勢が次次回歩容支持脚座標系の位置姿勢に合致するように決定される。換言すれば、次回歩容終端遊脚足平位置姿勢は、前記要求パラメータにおける2歩目の遊脚足平22の着地予定位置姿勢の要求値から、該足平22をすべらないように接地させたまま、つま先を持ち上げるようにピッチ方向に所定角度回転させた状態(つま先を上げてかかとを着地させた状態)での該足平22の位置姿勢である。
また、第1旋回歩容初期支持脚足平位置姿勢は、次回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とされる。この場合、今回歩容終端遊脚足平位置姿勢は、前記次回歩容終端遊脚足平位置姿勢の場合と同様、前記次回歩容支持脚座標系もしくはこれに対応する前記要求パラメータの1歩目(今回歩容)の遊脚着地予定位置姿勢の要求値に応じて決定されるものである。すなわち、今回歩容終端遊脚足平位置姿勢は、その位置姿勢から、足平22を接地させたまま、つま先を下げるように該足平22を回転させて該足平22の底面のほぼ全面を床面に接地させたときの該足平22の代表点が次回歩容支持脚座標系の原点に合致するように決定される。
第1旋回歩容終端遊脚足平位置姿勢は、今回歩容終端遊脚足平位置姿勢や次回歩容終端遊脚足平位置姿勢の決定手法と同様に、次回歩容支持脚座標系から見た次次回歩容支持脚座標系の位置姿勢に基づいて決定される。より具体的には、第1旋回歩容終端遊脚足平位置姿勢は、その位置姿勢から水平姿勢まで、足平22を接地させたまま、すべらないように該足平22をピッチ方向に回転させたときの該足平22の位置姿勢が、次回歩容支持脚座標系から見た次次回歩容支持脚座標系の位置姿勢に合致するように決定される。
第1旋回歩容終端では、支持脚足平22は離床して空中にある。支持脚足平22が離床してから後の軌道を決定するために、第1旋回歩容支持脚足平着地予定位置姿勢が設定される。第1旋回歩容支持脚足平着地予定位置姿勢は、次回歩容支持脚座標系から見た次次次回歩容支持脚座標系の位置姿勢に基づいて決定される。より具体的には、第1旋回歩容支持脚足平着地予定位置姿勢は、次回歩容支持脚座標系から見た次次次回歩容支持脚座標系の位置姿勢である。なお、次次次回歩容支持脚座標系は、次次回歩容支持脚座標系と次次次回歩容支持脚座標系との相対的位置姿勢関係が、今回歩容支持脚座標系と次回歩容支持脚座標系との相対的位置姿勢関係と一致するように決定される。
第1旋回歩容終端支持脚足平位置姿勢は、第1旋回歩容初期支持脚足平位置姿勢を求めた場合と同様に、第1旋回歩容初期支持脚足平位置姿勢から、前記第1旋回歩容支持脚足平着地予定位置姿勢に至る足平位置姿勢軌道(より詳しくは次回歩容支持脚座標系から見た軌道)を、第1旋回歩容終端まで前記有限時間整定フィルタを用いて生成することにより求められる。
第2旋回歩容初期遊脚足平位置姿勢は、次次回歩容支持脚座標系から見た第1旋回歩容終端支持脚足平位置姿勢とされる。第2旋回歩容初期支持脚足平位置姿勢は、次次回歩容支持脚座標系から見た第1旋回歩容終端遊脚足平位置姿勢とされる。
第2旋回歩容終端遊脚足平位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端遊脚足平位置姿勢とされる。第2旋回歩容終端支持脚足平位置姿勢は、今回歩容支持脚座標系から見た今回歩容終端支持脚足平位置姿勢とされる。
第1旋回歩容および第2旋回歩容の歩容周期は、次回歩容周期と同一に設定される。これらの第1旋回歩容及び第2旋回歩容の歩容周期は、互いに同一にすることは必ずしも必要ではないが、いずれの周期も、少なくとも次回歩容周期に応じて決定するのが好ましい。
次に、S102に進み、歩容生成装置100は、定常歩容における目標上体姿勢軌道を規定する上体姿勢軌道パラメータを決定する。
この場合、本実施形態では、定常歩容の上体姿勢軌道パラメータは、それにより規定される目標上体姿勢軌道があらかじめ定められた所定パターンの基準上体姿勢の軌道に一致するように決定される。この基準上体姿勢は、本実施形態では、経時的な変化の無い一定姿勢(固定的な姿勢)に設定されている。その基準上体姿勢は、例えば上体24の体幹軸が定常的に鉛直方向を向く姿勢(ロボット1の直立姿勢で起立した状態での上体姿勢)、すなわち、鉛直方向に対する上体姿勢角が定常的に“0”に保たれる姿勢である。そして、このような基準上体姿勢の軌道を規定するパラメータ(基準上体姿勢の一定の上体姿勢角の値等)が上体姿勢軌道パラメータとして決定される。なお、このように上体姿勢を一定姿勢とした場合には、必然的に上体姿勢角の角速度及び角加速度も定常的に“0”に保たれる。
補足すると、定常歩容の上体姿勢は、定常歩容の初期(第1旋回歩容の初期)と終端(第2旋回歩容の終端)とでつながるように(上体姿勢角及びその角速度がそれぞれ、定常歩容の初期と終端とで一致するように)に設定されている限り、一定姿勢である必要はない。本実施形態では、本実施形態の理解を容易にするために基準上体姿勢を上記の如く一定姿勢とした。
次に、S104に進み、歩容生成装置100は、腕姿勢軌道パラメータ、より詳しくは鉛直軸(あるいは上体24の体幹軸)まわりの両腕体の角運動量変化に関すること以外の腕姿勢軌道パラメータが決定される。たとえば、上体24に対する腕体の手先の相対高さや腕体全体の相対重心位置などを規定するパラメータが腕姿勢軌道パラメータとして決定される。この場合、本実施形態では、腕体全体の相対重心位置は、上体24に対して一定に維持されるように設定される。
次に、S106に進み、歩容生成装置100は、床反力鉛直成分軌道パラメータを決定する。この場合、該パラメータにより規定される床反力鉛直成分軌道が、第1旋回歩容および第2旋回歩容のいずれにおいても、実質的に連続なものとなるように床反力鉛直成分軌道パラメータが決定される。
具体的には、定常歩容の目標床反力鉛直成分軌道は、例えば図12に示すようなパターンで設定される。そのパターンでは、第1旋回歩容及び第2旋回歩容のいずれにおいても、片脚支持期では床反力鉛直成分が台形状に変化し、空中期では床反力鉛直成分が零に維持される。そして、このパターンの折れ点の時刻や、台形部分の高さ(ピーク値)が床反力鉛直成分軌道パラメータとして決定される。
この床反力鉛直成分軌道パラメータは、定常歩容の1周期分の期間(第1旋回歩容の期間と第2旋回歩容の期間とを合わせた期間)における床反力鉛直成分の平均値をロボット1の自重と一致させるように決定される。すなわち、定常歩容の1周期分の期間における床反力鉛直成分の平均値がロボット1の全体に作用する重力と同じ大きさで反対向きになるようにする。
上記のごとく床反力鉛直成分軌道パラメータ(ひいては床反力鉛直成分軌道)を決定することは、定常歩容の条件を満たすために必要である。定常歩容の条件とは、該定常歩容のあらゆる状態量(ロボット1の各部の位置、姿勢、速度等の運動状態量)についての初期状態(定常歩容の第1旋回歩容の支持脚座標系から見た該第1旋回歩容の初期状態)と終端状態(定常歩容の第2旋回歩容に続く第1旋回歩容の支持脚座標系から見た該第2旋回歩容の終端状態)とが一致していることである(以下、この条件を定常歩容の境界条件ということがある)。
したがって、定常歩容終端におけるロボット1の全体重心鉛直速度と定常歩容初期における全体重心鉛直速度との差(詳細には、第2旋回歩容終端の全体重心鉛直速度と第1旋回歩容初期の全体重心鉛直速度との差)も零でなければならない。上記差は、床反力鉛直成分と重力との差の積分値(定常歩容の初期から終端までの1周期分の期間での1階積分値)であるから、上記差を零にするためには、上記のごとく、定常歩容の1周期分の期間における床反力鉛直成分の平均値がロボット1の自重に一致するように床反力鉛直成分軌道を決定する必要がある。
尚、本実施形態では、第1旋回歩容、第2旋回歩容それぞれの期間における床反力鉛直成分の平均値をロボット1の自重と一致させるようにした。より具体的には、例えば第1旋回歩容及び第2旋回歩容の歩容周期に応じて各旋回歩容における床反力鉛直成分軌道の台形部分の折れ点の時刻を設定した上で、その台形部分の高さ(床反力鉛直成分のピーク値)を、床反力鉛直成分の第1旋回歩容、第2旋回歩容それぞれの期間における平均値がロボット1の自重に一致するように決定した。この場合、該台形部分の高さは、その高さを未知数として、上記平均値及び自重の一致条件を表す方程式を解くことによって、求めることができる。
こうすることにより、第1旋回歩容終端の全体重心鉛直速度と第1旋回歩容初期の全体重心鉛直速度との差が“0”になると共に、第2旋回歩容終端の全体重心鉛直速度と第2旋回歩容初期の全体重心鉛直速度との差も“0”になる。ただし、このようにする必然性はない。例えば、第1旋回歩容と第2旋回歩容との境界付近で上体鉛直位置が高くなり過ぎたり低くなり過ぎたりする恐れがある場合には、各旋回歩容で上記平均値及び自重を一致させた状態から、各旋回歩容の床反力鉛直成分軌道の台形の高さ等を修正するようにしてもよい。
次いで、S108に進み、歩容生成装置100は、第1旋回歩容および第2旋回歩容を合わせた定常歩容の目標ZMP軌道を規定するZMP軌道パラメータを決定する。この場合、目標ZMP軌道は、前述したように安定余裕が高くかつ急激な変化をしないように決定される。
さらに詳細には、図4の走行歩容においては、支持脚足平22(空中期での遊脚足平22)のかかとが着地した後、しばらくして該支持脚足平22の底面のほぼ全面が接地し、さらにしばらくして、該支持脚足平22のつま先だけが接地する。そして、次に、ロボット1が支持脚足平22のつま先で蹴って空中に飛び上がり、最後に遊脚足平22のかかとで着地する。また、目標ZMPは接地面内に存在しなければならない。
そこで、本実施形態では、定常歩容の第1旋回歩容及び第2旋回歩容のそれぞれの目標ZMPのX軸方向位置は、図6(a)に示したように、支持脚足平22のかかとを初期位置として、該足平22の底面のほぼ全体が接地するまでその位置に留まるように決定される。
続いて、目標ZMPは、支持脚足平22の中央に移動し、該足平22がつま先接地状態になるまでにつま先に移動し、その後、離床時まで支持脚足平22のつま先に留まるように決定される。さらにその後は、目標ZMPは、前述のごとく、次の遊脚足平22の着地までに目標ZMPが、支持脚足平22のつま先から遊脚足平22のかかとの着地位置まで連続的に移動するように決定される。
したがって、第1旋回歩容と第2旋回歩容とから成る定常歩容の目標ZMP軌道(X軸方向位置の軌道)は、図13に示す如く決定される。そして、この目標ZMP軌道の折れ点の時刻及び位置が定常歩容のZMP軌道パラメータとして決定される。この場合、折れ点の時刻は、前記要求パラメータに応じて定めた第1旋回歩容及び第2旋回歩容の歩容周期に応じて決定される。また、該折れ点の位置は、次回歩容支持脚座標系及び次次回歩容支持脚座標系の位置姿勢、あるいはこれらの座標系の位置姿勢を規定する、1歩目及び2歩目の遊脚側足平着地予定位置姿勢の要求値に応じて決定される。
尚、目標ZMP軌道のY軸方向位置は、図6(b)に示したものと同様に決定される。より詳しくは、第1旋回歩容における目標ZMPのY軸方向位置の軌道は、図6(b)のものと同一パターンで決定される。さらに、第2旋回歩容における目標ZMPのY軸方向位置の軌道は、第1旋回歩容のものと同じ形の軌道で、該軌道の終端につながるものに決定される。
以上が、図8のS022の処理の詳細である。
図8に戻って、歩容生成装置100は、上記の如くS022の処理を実行した後、S024に進み、定常歩容の初期状態を算出する。ここで算出する初期状態は、具体的には、定常歩容の初期上体位置速度(初期上体位置及び初期上体速度)及び初期発散成分である。この初期状態の算出は、図14のフローチャートに示す処理によって、探索的に行なわれる。
以下説明すると、まず、S200において、歩容生成装置100は、定常歩容パラメータ(図8のS022で決定された歩容パラメータ)に基づいて、定常歩容の目標足平位置姿勢、目標腕姿勢、および目標上体姿勢角の初期状態(定常歩容の初期時刻(=今回歩容の終端時刻)での状態)を決定する。ここで「状態」とは、位置又は姿勢とその時間的変化率(位置又は姿勢の変化速度)との組を意味する。例えば目標足平位置姿勢の初期状態は、定常歩容の初期時刻での足平位置姿勢と、その時間的変化率(すなわち、足平22の移動速度及び姿勢の変化速度)との組を意味する。目標腕姿勢の初期状態、目標上体姿勢角の初期状態についても同様である。
この場合、支持脚側の足平位置姿勢の初期状態と、遊脚側の足平位置姿勢の初期状態とは、図10のS100で決定した足平軌道パラメータに基づいて決定される。
具体的には、足平軌道パラメータにうちの、前記第1旋回歩容初期支持脚足平位置姿勢と、第1旋回歩容初期遊脚足平位置姿勢とが、それぞれ、定常歩容の初期時刻での支持脚側の足平位置姿勢、遊脚側の足平位置姿勢として決定される。
また、定常歩容の初期時刻での支持脚側の足平位置姿勢の時間的変化率は、第1旋回歩容初期支持脚足平位置姿勢から第2旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道(次回歩容支持脚座標系から見た軌道)を有限時間整定フィルタにより生成した場合における該軌道の初期時刻(定常歩容の初期時刻)での足平位置姿勢の時間的変化率として算出される。この場合、第1旋回歩容初期支持脚足平位置姿勢から第2旋回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道のうち、定常歩容の初期時刻からその直後の時刻までの軌道から定常歩容の初期時刻での支持脚側の足平位置姿勢の時間的変化率を算出することができる。
また、定常歩容の初期時刻での遊脚側の足平位置姿勢の時間的変化率は、今回歩容初期支持脚足平位置姿勢から第1旋回歩容終端終端遊脚足平位置姿勢に至る足平位置姿勢軌道(次回歩容支持脚座標系から見た軌道)を有限時間整定フィルタにより生成した場合における該軌道の初期時刻(定常歩容の初期時刻)での足平位置姿勢の時間的変化率として算出される。この場合、今回歩容初期支持脚足平位置姿勢から第1旋回歩容終端終端遊脚足平位置姿勢に至る足平位置姿勢軌道のうち、今回歩容の初期時刻から定常歩容の初期時刻(又はその直後の時刻)までの軌道を生成すれば、該軌道(定常歩容の初期時刻の近辺の軌道)から定常歩容の初期時刻での遊脚側の足平位置姿勢の時間的変化率を算出することができる。
また、腕姿勢の初期状態は、図10のS104で決定した腕姿勢軌道パラメータに基づいて、定常歩容の初期時刻における腕姿勢(上体24に対する両腕体の全体重心位置等)と該初期時刻の直後の期間の腕姿勢の変化量とを求めることで決定される。
また、上体姿勢角の初期状態は、図10のS102で決定した上体姿勢軌道パラメータ基づいて、定常歩容の初期時刻における上体姿勢角と該初期時刻の直後の期間の上体姿勢角の変化量とを求めることで決定される。なお、本実施形態では、上体姿勢軌道パラメータにより規定される上体姿勢は、定常的に上体24の体幹軸が鉛直方向に向く姿勢であるので、定常歩容の初期時刻での上体姿勢角およびその角速度は、いずれも“0”である。
次いで歩容生成装置100は、S202において初期上体水平位置速度の候補(定常歩容の初期時刻での上体24の水平位置及び水平速度の候補)である(Xs,Vxs)(Xs:水平位置、Vxs:水平速度)を仮決めする。ここで仮決めする候補(Xs,Vxs)は、任意でよいが、例えば前回歩容の生成時に求めた定常歩容の初期状態における上体水平位置速度の候補(Xs,Vxs)として仮決めすればよい。
尚、説明を簡略にするため、サジタルプレーン上でX軸方向(ロール軸方向)での定常歩容の初期状態(初期上体水平位置速度)を探索する場合を例にとって説明する。ただし、実際には位置・速度のいずれについても、X軸方向(ロール軸方向)と、Y軸方向(ピッチ軸方向)とで別々に、あるいは同時に定常歩容の初期状態(定常歩容の前記境界条件を満たす初期状態)を探索する必要がある。
探索的な決定手法としては、擬似ヤコビアン(感度マトリックス)を求め、最急降下法などにより次候補を決定する方法や、シンプレックス法などを用いればよい。本実施形態では、例えば最急降下法を用いる。
次いで、S204を経てS206に進み、定常歩容の初期時刻での上体24の鉛直位置(Z軸方向位置)Zs及び鉛直速度(Z軸方向速度)Vzsの組である初期上体鉛直位置速度(Zs,Vzs)が決定される。
初期上体鉛直速度Vzsは、本実施形態では例えば以下のように決定される。
ロボット1には、動力学的関係として、次式が成立する。
終端全体重心鉛直位置−初期全体重心鉛直位置
=(床反力鉛直成分/ロボットの全体質量)の2階積分
+重力加速度の2階積分
+初期全体重心鉛直速度*1歩の時間
…式13
(ただし、重力加速度は負の値とする。)
また、定常歩容では、終端全体重心鉛直位置と初期全体重心鉛直位置は一致するので、上式13の右辺は零にならなければならない。したがって、これらの関係から初期全体重心鉛直速度を求めることができる。具体的には、まず、前記図10のS104で決定した床反力鉛直成分軌道パラメータによって算出される床反力鉛直成分をロボット1の全体質量で割った値を、定常歩容の初期から終端までの期間において2階積分することにより、床反力鉛直成分による全体重心移動量(式13の右辺第1項)が求められる。
さらに、重力加速度を定常歩容の初期から終端までの期間において2階積分することにより、重力による全体重心移動量(式13の右辺第2項)が求められる。そして、上記の如く求めた、床反力鉛直成分による全体重心移動量と重力による全体重心移動量との和の符号を反転させたものを、定常歩容の1周期Tcycの時間で割ることにより、初期全体重心鉛直速度が求められる。
そして、本実施形態では、例えば、上体24と他の部位(脚体2,2や腕体)とに質点を有する多質点モデル(幾何学モデル)を用い、該多質点モデルの重心点の鉛直速度が上記初期全体重心鉛直速度に一致するような上体24の鉛直速度を、S200で決定した足平位置姿勢、腕姿勢等の初期状態と、S202(又は後述のS218)で決定された初期上体水平位置速度(Xs,Vxs)とを基に求め、その求めた鉛直速度を初期上体鉛直速度Vzsとして決定する。
上記多質点モデルとしては、例えば、上体24の質量(又は上体24と腕体とを合わせた質量)を有する1つの上体質点と、両脚体2,2のそれぞれの質量を有する2つの脚質点とから構成される3質点モデルが挙げられる。この場合、例えば、上体質点の位置は、上体24の位置姿勢に応じて規定され、各脚質点の位置は、各脚体2の足平位置姿勢に応じて規定されるものとする。このような3質点モデルを用いた場合には、上記の如く求めた初期全体重心鉛直速度と、足平位置姿勢の初期状態とを基に、上体質点の鉛直速度を算出できる。そして、その上体質点の鉛直速度と、S200で決定された上体姿勢角の初期状態とを基に、初期上体鉛直速度Vzsを決定することが可能である。
なお、より多くの質点を有する多質点モデル(例えばロボット1の各リンクに質点を有する幾何学モデル)を用いることによってより正確に、初期上体鉛直速度Vzsを決定するようにしてもよい。また、ロボット1の上体24以外の部位の質量が上体24に比して十分に小さいような場合には、例えばロボット1の全体重心の鉛直速度が上体24の鉛直速度にほぼ一致すると見なし、上記初期全体重心鉛直速度を、そのまま初期上体鉛直速度Vzsとして決定するようにしてもよい。
一方、定常歩容の初期上体鉛直位置Zsは、例えば本出願人が先に特開平10−86080号公報で提案した上体高さ決定手法を用いて決定される。このとき、初期時刻における足平位置姿勢(前記図10のS100で決定した第1旋回歩容初期支持脚足平位置姿勢及び第1旋回歩容初期遊脚足平位置姿勢)と、各脚体2の膝部の曲げ角に関する所定の幾何学的条件とに基づいて、初期時刻での各脚体2,2の膝部が伸び切らないような初期上体鉛直位置Zsが決定される。例えば、支持脚側脚体2の膝曲げ角をθsup、遊脚側脚体2の膝曲げ角をθswgとしたとき、それらの膝曲げ角θsup,θswgの正弦関数値の逆数の和がある所定値(有限値)になるように、初期上体鉛直位置が決定される。ここで、膝曲げ角θsup,θswgは、各脚体2の大腿部の軸心に対する下腿部の軸心の角度であり、各脚体2が伸びきった状態から膝部を曲げていくに伴い、“0”から増加していく角度である。なお、このような上体24の鉛直位置の決定手法は、上記特開平10−86080号公報に詳細に説明されているので、ここでは以上の説明に留める。
補足すると、ロボット1は、片脚当たり6自由度であることから、足平位置姿勢軌道および上体位置姿勢軌道の初期状態(位置姿勢とその時間的変化率)が与えられれば、ロボット1の上体24及び脚体2,2の運動の初期状態は一義的にすべて決定されることになる。したがって、例えば、上体24及び脚体2,2の全体の質量がロボット1の全体の質量にほぼ一致するような場合には、上体24及び脚体2,2の運動の初期状態が与えられれば、初期全体重心鉛直速度も一義的に決定される。逆に、片脚当たり6自由度であることから、足平位置姿勢軌道および上体位置姿勢軌道の初期状態の内で速度に関する状態のひとつ(例えば初期上体鉛直速度)が未定であっても、初期全体重心鉛直速度が与えられれば、未定の初期状態は一義的に決定される。
S206の処理の後、次いでS208に進み、定常歩容の候補としての歩容(定常歩容の1歩分(1周期分)の歩容)が仮に生成される。より具体的には、図8のS022で決定した定常歩容パラメータを基に、初期時刻から終端時刻までの各瞬間における目標ZMP、目標床反力鉛直成分、目標足平位置姿勢、目標上体姿勢および目標腕姿勢の瞬時値が逐次求められる。そして、求めた目標ZMPと目標床反力鉛直成分とを満足するように、図7に示した動力学モデル(倒立振子モデル)を用いて上体位置を逐次決定することによって、定常歩容の初期時刻から終端時刻までの歩容を生成する。この場合、前記初期上体水平位置速度(Xs,Vxs)、初期上体鉛直位置速度(Zs,Vzs)が上体24の位置及び速度の初期状態として設定される。
尚、この歩容生成は歩容生成装置100の内部で行われるだけであって、実際のロボット1を駆動するための目標値として、歩容生成装置100から出力されることはない。
S208の処理は、具体的には、図15のフローチャートに示す如く実行される。
以下説明すると、歩容生成装置100は、まず、S300において、諸々の初期化を行なう。具体的には、歩容生成用時刻Tkが定常歩容の初期時刻Tsに初期化される。さらに、上体水平位置速度に、初期上体水平位置速度(Xs,Vxs)の最新の候補値(図14のS202もしくは後述のS216もしくはS218で決定された最新の候補値)が代入され、上体鉛直位置速度に、初期上体鉛直位置速度(Zs,Vzs)の最新値(図14のS206で決定された最新値)が代入される。また、上体姿勢角には、基準上体姿勢角の初期値、上体姿勢角速度には、基準上体姿勢角速度の初期値が代入される。
次いで、S302を経てS304において、歩容生成装置100は、歩容生成用時刻Tk(現在値)が終端時刻Te(=Ts+Tcyc)以前の時刻であるか否か(Tk≦Teであるか否か)を判断する。この判断結果が肯定的である場合には、歩容生成装置100は、S306〜S316の処理(詳細は後述する)を実行することによって、時刻Tkでの歩容の瞬時値を決定する。
次いでS318に進み、歩容生成装置100は、歩容生成用時刻Tkを所定の刻み時間ΔTkだけ増加させた後、S304の判断を再び行なう。ここで、刻み時間ΔTkは、例えば制御周期Δtと一致させておけばよい。ただし、演算量を低減するために、ΔTkをΔtよりも長い時間に設定してもよい。
S304の判断結果が肯定的である場合には、S306からS318までの処理が繰り返され、S304の判断結果が否定的になった場合には、図15の処理、すなわち、図14のS208の処理は終了する。これにより、定常歩容の初期時刻から終端時刻までの1周期分の定常歩容(仮の定常歩容)の生成が完了する。
歩容生成装置100は、上記S306〜S316で仮の定常歩容の瞬時値を決定する処理を次のように実行する。まずS306において、歩容生成装置100は、定常歩容パラメータ(より詳しくは床反力鉛直成分軌道パラメータ)を基に、図12に示した目標床反力鉛直成分軌道の時刻Tkでの瞬時値を求める。
さらに、S308において、歩容生成装置100は、定常歩容パラメータ(より詳しくはZMP軌道パラメータ)を基に、図13に示した目標ZMP軌道の時刻Tkでの瞬時値を求める。
次いでS308に進み、歩容生成装置100は、定常歩容パラメータ(より詳しくは足平軌道パラメータ、上体姿勢軌道パラメータ、及び腕姿勢軌道パラメータ)を基に、目標足平位置姿勢(支持脚側及び遊脚側の両方の目標足平位置姿勢)、目標上体姿勢および目標腕姿勢の時刻Tkでの瞬時値をそれぞれ求める。ただし、目標腕姿勢に関しては、より詳しくは両腕体の全体重心位置が決定されるが、鉛直軸(または上体24の体幹軸)まわりの角運動量を変化させる腕体の運動(腕振り運動)は、未だ決定されない。
次いでS310に進み、歩容生成装置100は、S306で求めた目標床反力鉛直成分を満足する(ロボット1の全体重心の鉛直方向の慣性力と重力との和を目標床反力鉛直成分に釣り合わせる)ように、時刻Tkでの上体鉛直位置を算出する。
具体的には、次式15および式16で表される動力学関係式(鉛直方向でのニュートンの運動方程式を離散化した式)を用いてロボット1の全体重心鉛直速度と全体重心鉛直位置とが算出される。
時刻Tkにおける全体重心鉛直速度
=時刻(Tk−ΔTk)における全体重心鉛直速度
+((時刻Tkにおける床反力鉛直成分/ロボットの全体質量)+重力加速度)*ΔTk
(ただし、重力加速度は負の値とする。)
…式15
時刻Tkにおける全体重心鉛直位置
=時刻(Tk−ΔTk)における全体重心鉛直位置
+時刻Tkにおける全体重心鉛直速度*ΔTk
…式16
そして、本実施形態では、歩容生成装置100は、上記の如く算出した時刻Tkでの全体重心鉛直位置と、目標足平位置姿勢と、基準上体姿勢(目標上体姿勢)とを基に、例えば、前記した3質点モデル(S206の処理に関して説明した3質点モデル)を用いて上体鉛直位置を求める。この場合、目標足平位置姿勢に基づき、3質点モデルにおける2つの脚質点の位置が決定される。さらに、この3質点モデルにおける全体重心の鉛直位置が、上記の如く求めた時刻Tkでの全体重心鉛直位置に一致するような上体質点の鉛直位置が決定される。そして、この上体質点の鉛直位置と目標上体姿勢(基準上体姿勢)とから上体鉛直位置が決定される。
なお、より多くの質点を有する多質点モデル(例えばロボット1の各リンクに質点を有するモデル)を用いることによって、より正確に上体鉛直位置を求めるようにしてもよい。また、上体24以外の部位の質量が上体24に比して十分に小さいような場合には、簡略的に、全体重心鉛直位置が、上体24の重心の鉛直位置に一致するものとみなし、該全体重心鉛直位置と、目標上体姿勢(基準上体姿勢)とから上体鉛直位置を決定するようにしてもよい。
次いでS314に進み、歩容生成装置100は、目標ZMPを満足するように(ロボット1の慣性力と重力との合力が目標ZMPまわりに発生するモーメントの水平成分が“0”であるという動力学的平衡条件を満足するように)、時刻Tkでの上体水平加速度(上体24の水平方向の加速度)を決定する。
この時点において、定常歩容(仮の定常歩容)の足平位置姿勢、腕姿勢、上体姿勢および上体鉛直位置の瞬時値(現在時刻Tkでの値)が決定されているので、残りの上体水平位置を決定すれば、鉛直軸まわりの角運動量を変化させる腕体の運動自由度を除いて、ロボット1の全体の目標運動が決定されることとなる。したがって、上体水平位置を決定すれば、床反力の鉛直軸まわりのモーメントを除き、すべての床反力も一義的に決定される。
なお、本実施形態では、定常歩容(仮の定常歩容)の目標床反力鉛直成分及び目標ZMPは、それぞれ前記図8のS022で決定した定常歩容パラメータの床反力鉛直成分軌道パラメータ、目標ZMP軌道パラメータによって規定される。したがって、上体水平位置の決定に応じて従属的に決定される床反力は、床反力水平成分のみである。
補足すると、本実施形態のロボット1は、各脚体2の自由度が6自由度であることから、目標足平位置姿勢と目標上体位置姿勢とを決定すれば、各脚体2の各部の位置姿勢も一義的に決定されることとなる。なお、鉛直軸まわりの角運動量を変化させる腕の運動自由度は、後述するように、スピン力を打ち消すために用いられる。
S314では、例えば図7の動力学モデル(倒立振子モデル)に係わる前記式04を用いて上体水平加速度が求められる。より具体的には、現在時刻Tkの上体鉛直位置と、時刻(Tk−ΔTk)の上体水平位置と、現在時刻Tkの目標上体姿勢とから、現在時刻Tkでの上体質点24bの鉛直位置及び水平位置が決定される。なお、時刻(Tk−ΔTk)までの上体水平位置の時系列又は時刻(Tk−ΔTk)での歩容状態を基に時刻Tkでの上体水平位置を補間的に推定し、その推定した上体水平位置を時刻(Tk−ΔTk)の上体水平位置の代わりに用いてもよい。
また、現在時刻Tkでの床反力鉛直成分から、上体質点24bに作用する重力(=mb*g)を差し引いた値を、上体質点24bの質量mbにより除算することによって、現在時刻Tkでの上体質点24bの鉛直加速度が求められる。
そして、上記の如く求めた上体質点24bの鉛直位置、水平位置、鉛直加速度をそれぞれ、前記式04のZb、Xb、d2Zb/dt2に代入すると共に、現在時刻Tkでの目標ZMPの水平位置および鉛直位置を式04のXzmp,Zzmpに代入し、さらに、同式04のMzmp_yを“0”にしてなる式を、d2Xb/dt2について解くことにより、現在時刻Tkでの上体質点水平加速度d2Xb/dt2が算出される。そして、この上体質点水平加速度d2Xb/dt2が現在時刻kでの上体水平加速度として求められる。
尚、より厳密な動力学モデルを用いて、目標ZMPまわりの床反力モーメントの水平成分を“0”にするような上体水平加速度を探索的に求めるようにしてもよい。
次いでS316に進み、歩容生成装置100は、S314で求めた上体水平加速度を2階積分することにより現在時刻Tkでの上体水平位置を算出する。より詳しくは、上体水平加速度に刻み時間ΔTkを乗じてなる値を時刻Tk−ΔTkでの上体水平速度に加算することによって、現在時刻Tkでの上体水平速度が求められる。さらにその上体水平速度に刻み時間ΔTkを乗じてなる値を時刻Tk−ΔTkでの上体水平位置に加算することによって現在時刻Tkでの上体水平位置が求められる。
以上がS306〜S316で実行される歩容の生成処理の詳細である。
図14のS208の処理を完了した後、S210に進み、歩容生成装置100は、生成した歩容(仮定常歩容)の終端上体水平位置・速度を、その瞬間の支持脚に対応する支持脚座標系(図11のX'''軸及びY'''軸を水平方向の2軸とする支持脚座標系)から見た値に変換し、その値を(Xe,Vxe)とする(Xe:終端上体水平位置、Vxe:終端上体水平速度)。
次いでS212に進み、図示の如く、歩容生成装置100は、初期上体水平位置速度(Xs,Vxs)と終端上体水平位置速度(Xe,Vxe)との差を算出する。この差(Xs−Xe,Vxs−Vxe)を上体水平位置速度境界条件誤差(errx,errv)と呼ぶ。定常歩容は、前記境界条件を満たす歩容であるので、(Xs,Vxs)と(Xe,Vxe)とが一致している必要がある。従って、上体水平位置速度境界条件誤差(errx,errv)が零またはほぼ零である必要がある。本実施形態では、以下のように、上体水平位置速度境界条件誤差(errx,errv)がほぼ零となる(Xs,Vxs)が探索的に求められる。
次いでS214に進み、歩容生成装置100は、上体水平位置速度境界条件誤差(errx,errv)が、あらかじめ適宜設定された許容範囲内にあるか否か(errx,errvのいずれもが、許容範囲内にあるか否か)を判断する。
S214の判断結果が否定的であるときにはS216に進む。このS216では、(Xs,Vxs)の近辺に複数(本実施形態では2つ)の初期値候補(Xs+ΔXs,Vxs),(Xs,Vxs+ΔVxs)が決定される。ここで、ΔXs,ΔVxsは、それぞれXs,Vxsに対する所定の微小変化量を意味している。そして、これらの初期値候補のそれぞれを上体水平位置速度の初期状態として、前記S208と同様の処理によって定常歩容(仮定常歩容)が歩容パラメータを用いて生成される。さらに、その生成された定常歩容の終端上体位置速度を、その瞬間の支持脚に対応する支持脚座標系(図11のX'''軸及びY'''軸を水平方向の2軸とする支持脚座標系)から見た値に変換した値(Xe+ΔXe1,Vxe+ΔVxe1),(Xe+ΔXe2,Vxe+ΔVxe2)が求められる。ここで、(Xe+ΔXe1,Vxe+ΔVxe1)は(Xs+ΔXs,Vxs)に対応する終端上体位置速度を意味し、(Xe+ΔXe2,Vxe+ΔVxe2)は(Xs,Vxs+ΔVxs)に対応する終端上体位置速度を意味している。なお、この場合の定常歩容(仮定常歩容)の生成処理では、上体水平位置速度以外の状態量の初期状態は、例えば上体水平位置速度の初期値候補を(Xs,Vxs)とした場合と同一に設定すればよい。S216ではさらに、前記S210と同様の処理によって、各初期値候補とこれに対応する終端上体位置速度との差、すなわち、各初期値候補(Xs+ΔXs,Vxs),(Xs,Vxs+ΔVxs)のそれぞれに対応する上体水平位置速度境界条件誤差が求められる。
次いでS218に進み、歩容生成装置100は、(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の判断結果が否定的となる限りは歩容生成装置100は、S206〜S218の処理を繰り返す。そして、S214の判断結果が肯定的になると、繰り返しループ(S204)を抜けてS220に進む。この場合、S204の繰り返しループを抜けたときの直前に生成された仮定常歩容が、境界条件を満たす定常歩容として得られることとなる。
S220では、S204の繰り返しループのS208で最終的に生成した仮定常歩容における初期上体水平位置速度の初期値候補(Xs,Vxs)と、初期上体鉛直位置速度(Zs,Vzs)とを、それぞれ求めるべき定常歩容の初期上体水平位置速度(X0,Vx0)、初期上体鉛直位置速度(Z0,Vz0)として決定する。
次いでS222に進み、歩容生成装置100は、定常歩容初期発散成分(定常歩容の初期時刻Tsでの発散成分の値)q0を算出する。
ここで、本実施形態では、歩容の発散成分の値は、以下に説明するように定義される。そして、S222では、この定義に従って、定常歩容初期発散成分q0が算出される。
まず、図7の倒立振子モデルにおける上体質点24bの運動と、床反力モーメント(支持脚座標系の原点まわりの床反力モーメント)との間の関係を表す前記式03を、状態方程式により表現すると次式100となる。なお、ここでは、ロボット1の移動時の上体24の高さ(鉛直位置)の、平均的な高さに対する相対的な変動量(上体24の高さの分散)は十分に小さいものとして、式03の右辺の第2項のZbが、上体質点24bの平均的な高さとしてあらかじめ設定された一定値hに一致するものとしている。また、ここでは、前記式03の床反力モーメントMb_yを、倒立振子に対する入力モーメントとして、Minと表記している。
この式100を離散時間系での表現に書き直すことによって、次式102の状態方程式が得られる。
XVb(k+1)=A(k)*XVb(k)+B(k)*Min(k) ……式102
なお、添え字(k)、(k+1)を付した変数は、それぞれ、離散時間系のk番目の時刻での値、k+1番目の時刻での値を意味する。以降の説明では、k番目の時刻、k+1番目の時刻をそれぞれ、単に時刻k、時刻k+1、というように表現する場合がある。
式102におけるXVbは、式100に関するただし書きで定義した通り、上体質点24bの水平位置Xbと、その時間的変化率である水平速度Vxbとをそれぞれ状態変数とする状態変数ベクトル(縦ベクトル)である。
また、式102におけるA(k)、B(k)は、それぞれ、2次の正方行列(状態遷移行列)、2次の縦ベクトルであり、それぞれの各成分値が以下の式104a〜104c,106a〜106cの如く決定されるものである。なお、ΔTは離散時間系の刻み時間であり、ω0は下記のただし書きの式108によって定義される角周波数値である。また、exp( )は、自然対数の底の指数関数である。
この場合、式104a〜104c,106a〜106cと式108とから明らかなように、A(k),B(k)の各成分値は、上体質点24bの鉛直加速度d2Zb/dt2(鉛直方向の運動加速度d2Zb/dt2)の値に依存して定まる。従って、A(k),B(k)の各成分値は、上体質点24bの鉛直方向の慣性力(=−mb*d2Zb/dt2)に依存する値、すなわち該慣性力の関数値となる。なお、上体質点24aに作用する重力は一定値であるから、A(k),B(k)の各成分値は、別の言い方をすれば、上体質点24bの鉛直方向の慣性力と上体質点24bに作用する重力との合力、または、該合力に釣り合う床反力鉛直成分の関数値でもある。
補足すると、本実施形態の如く倒立振子の上体質点24bの質量mbがロボット1の全体質量に一致するものとした場合には、上体質点24bの鉛直方向の慣性力と上体質点24bに作用する重力との合力に釣り合う床反力鉛直成分は、ロボット1に作用する床反力鉛直成分(全床反力の並進床反力鉛直成分)に一致する。
また、図4の走行歩容では、片脚支持期においてはg+d2Zb/dt2>0、空中期においてはg+d2Zb/dt2=0となる。
上記式102の状態方程式により表される系では、状態変数ベクトルXVbの初期値XVb(0)(k=0の時刻(時刻0)でのXVbの値)と、時刻0から時刻k−1までの入力モーメントMin(床反力モーメントの水平成分Mb_y)の時系列とが与えられたとき、任意の時刻k(>0)での状態変数ベクトルXVbの値XVb(k)は、次式110により与えられることとなる。
なお、式110中のただし書きで定義したφ(k,j)に関しては、j=kの場合はφ(k,j)≡単位行列とする。
次に、前記した定常歩容での倒立振子モデルの挙動に着目し、定常歩容の初期時刻Tsをk=0の時刻、定常歩容の終端時刻Te(=Ts+Tcyc)をk=kcyc(ただし、kcyc≡Tcyc/ΔT)の時刻とし、初期時刻Tsでの定常歩容の運動により規定される状態変数ベクトルXVbがXVb(0)であるとする。そして、定常歩容の終端時刻Te(k=kcycの時刻)における状態変数ベクトルXVbのうち、定常歩容の初期時刻Tsでの状態変数ベクトルの値XVb(0)に依存する成分をXVb(kcyc)とおく。このとき、XVb(kcyc)と、XVb(0)との間の関係は、上記式110に基づいて、次式112により表されることとなる。
XVb(kcyc)=φ(kcyc,0)*XVb(0) ……式112
ただし、φ(kcyc,0)=A(kcyc-1)*……*A(1)*A(0)
さらに、この式112の右辺の行列φ(kcyc,0)の2つの固有値をλ1,λ2、それぞれの固有値に対応する固有ベクトル(縦ベクトル)を(a11,a21)T、(a12,a22)T、これらの固有ベクトルをそれぞれ第1列、第2列とする2次の正方行列をΓcycとおき、この行列Γcycを用いてφ(kcyc,0)を対角化してなる行列をΛとおく。すなわち、Λを次式114により定義する。以降、行列Γcycを対角化用行列という。
この式114と前記式112とから次式116が得られる。
Γcyc-1*XVb(kcyc)=Λ*Γcyc-1*XVb(0) ……式116
なお、上記対角化用行列Γcycによる対角化の対象とする行列φ(kcyc,0)は、前記式112のただし書きに示したように、定常歩容の1周期の初期時刻(時刻0)から、終端時刻(時刻kcyc)のまでの期間内の各時刻(詳しくは時刻0から時刻kcyc−1までの刻み時間ΔT毎の各時刻)での状態遷移行列A(0)、A(1)、……、A(kcyc-1)の積であるから、定常歩容の1周期の期間内での上体質点24bの慣性力の時系列に依存して定まるものとなる。従って、対角化用行列Γcycも、結果的に、その各成分値が定常歩容の1周期の期間内での上体質点24bの慣性力の時系列に依存して定まるものとなる。
ここで、任意の時刻kでの状態変数ベクトルXVb(k)を、対角化用行列Γcycの逆行列Γcyc-1によって線形変換してなるベクトル(以降、変換状態変数ベクトルという)を(p(k),q(k))Tとおく。すなわち、変換状態変数ベクトル(p(k),q(k))Tを次式118により定義する。
(p(k),q(k))T≡Γcyc-1*XVb(k) ……式118
この式118と前記式116とから、次式120が得られる。
(p(kcyc),q(kcyc))T=Λ*(p(0),q(0))T ……式120
この式120において、λ1>1であれば、p(kcyc)の絶対値>初期値p(0)の絶対値となり、λ1≦1であれば、p(kcyc)の絶対値≦初期値p(0)の絶対値となる。同様に、λ2>1であれば、q(kcyc)の絶対値>初期値q(0)の絶対値となり、λ2≦1であれば、q(kcyc)の絶対値≦初期値q(0)となる。
一方、本実施形態における定常歩容は、片脚支持期と空中期とを交互に繰り返す歩容(図4の走行歩容)であるから、g+d2Zb/dt2≦0となる状態に常に保持されるということはなく、g+d2Zb/dt2≦0となる状態と、g+d2Zb/dt2>0となる状態とが交互に生じると考えてよい。このような定常歩容では、一般にφ(kcyc,0)の2つの固有値λ1,λ2の一方が、“1”よりも大きい値となり、他方が“1”よりも小さい値となる。
そこで、以降、λ1<1、λ2>1であるとする。すなわち、変換状態変数ベクトル(p(k),q(k))Tのうちの、第1成分p(k)が、“1”よりも小さい固有値λ1に対応する成分、第2成分q(k)が、“1”よりも大きい固有値λ2に対応する成分であるとする。このとき、p(k)は、無限に繰り返される定常歩容において収束性を有する運動成分の状態量としての意味を持ち、q(k)は、無限に繰り返される定常歩容において発散性を有する運動成分の状態量としての意味を持つ。
そこで、本実施形態では、式118により定義した変換状態変数ベクトル(p(k),q(k))Tのうちの第2成分q(k)を前記発散成分として定義する。このようにして定義される発散成分q(k)は、状態変数Xb(k),Vxb(k)の線形結合値となる。この場合、その線形結合において、Xb(k),Vxb(k)にそれぞれ掛かる重み係数は、定常歩容の1周期の期間内での上体質点24bの慣性力の時系列に依存して定まるものとなる。なお、式118により定義した変換状態変数ベクトル(p(k),q(k))Tのうちの第1成分p(k)を収束成分と言う。
なお、前記式118で定義される収束成分p(k)及び発散成分q(k)は、詳しくはサジタルプレーン上での収束成分q(k)及び発散成分q(k)(X軸方向での上体質点24bの水平位置Xbと水平速度Vxbとを成分とする状態変数ベクトルXVbに対応する収束成分及び発散成分)であるが、これと同様に、ラテラルプレーン上での収束成分及び発散成分も定義される。具体的には、式118の右辺の状態変数ベクトルXVb(k)の各成分Xb、Vxbをそれぞれ、Y軸方向での上体質点24bの水平位置と水平速度とに置き換えた式によって、ラテラルプレーン上での収束成分及び発散成分が定義される。この場合、式118の行列Γcyc-1はX軸方向及びY軸方向のいずれの軸方向に関しても同一である。
以上説明した発散成分qの定義に従って、S222では、定常歩容初期発散成分q0は次のように算出される。
すなわち、歩容生成装置100は、定常歩容の初期時刻Tsから終端時刻Tsまでの1周期の期間内の刻み時間ΔT毎の各時刻Tk(離散時間系で言えばk=0の時刻からk=kcyc−1の時刻までの各時刻k)における上体質点24bの慣性力の瞬時値を、定常歩容パラメータを基に算出する。この場合、倒立振子の上体質点24bの質量mbがロボット1の全体の質量に一致するものとした場合には、定常歩容での上体質点24bの鉛直方向の慣性力と該上体質点24bに作用する重力との合力が、定常歩容における床反力鉛直成分に釣り合うものとなる。そこで、本実施形態では、歩容生成装置100は、定常歩容パラメータのうちの前記床反力鉛直成分軌道パラメータを基に、定常歩容の1周期の期間内の各時刻における床反力鉛直成分の瞬時値を算出する。該瞬時値は、図15のS306で算出される値と同じである。そして、歩容生成装置100は、その算出した床反力鉛直成分の各瞬時値を上体質点24bの質量mb(=ロボット1の全体質量)で除算してなる値を前記式100の(g+d2Zb/dt2)の値として、前記式104a〜104cによって、状態遷移行列A(k)(k=0,1,……,kcyc−1)を算出する。
なお、図4の走行歩容では、床反力鉛直成分は負の値となることはない。従って、実際上は、A(k)は式104a,104bのいずれかの式によって算出されることとなる。
補足すると、前記した3質点モデルなどの多質点モデル(幾何学モデル)を用いて、定常歩容におけるロボット1の上体24の鉛直方向の慣性力(又は鉛直加速度)を算出し、その算出した慣性力(又は鉛直加速度)を、上体質点24bの慣性力(又は鉛直加速度)として用いることによって、各時刻での状態遷移行列A(k)(k=0,1,……,kcyc−1)を算出するようにしてもよい。例えば、前記3質点モデルを用いて、次のようにして、定常歩容における上体質点24bの慣性力の瞬時値を算出するようにしてもよい。すなわち、定常歩容の足平軌道パラメータを基に、前記3質点モデルにおける2つの脚質点の鉛直方向の慣性力の瞬時値を算出すると共に、床反力鉛直成分軌道を基に、ロボット1の全体重心の鉛直方向の慣性力の瞬時値を算出する。そして、全体重心の鉛直方向の慣性力の瞬時値から、2つの脚質点の鉛直方向の慣性力の瞬時値を差し引いたものを、上体質点24bの鉛直方向の慣性力の瞬時値として算出する。この場合には、上体質点24bの鉛直方向の慣性力と該上体質点24bに作用する重力との合力(あるいはg+d2Zb/dt2)が負の値となり得る。
歩容生成装置100は、上記の如く定常歩容の初期時刻Tsから終端時刻Tsまでの1周期の期間におけるkcyc個のA(k)(k=0,1,…,kcyc−1)を算出した後、これらのA(k)を掛け合わせることによって、式112の右辺の行列φ(kcyc,0)(=A(kcyc-1)*…*A(1)*A(0))を算出する。
さらに歩容生成装置100は、この行列φ(kcyc,0)の固有値λ1,λ2と、これらに対応する固有ベクトル(a11,a21)T、(a12,a22)Tとを算出し、それらの固有ベクトルから、前記式114のただし書きに従って、前記対角化用行列Γcycを決定する。そして、歩容生成装置100は、この対角化用行列Γcycの逆行列Γcyc-1を算出する。
また、歩容生成装置100は、S220で決定した定常歩容の初期上体水平位置速度(X0,Vx0)と、定常歩容の上体姿勢角の初期状態とから、定常歩容の初期時刻Tsでの倒立振子の上体質点24bの水平位置及び水平速度の組である初期上体質点位置速度を決定する。
そして、歩容生成装置100は、上記の如く求めた逆行列Γcyc-1と、初期上体質点水平位置速度とから、前記式118に基づいて定常歩容初期発散成分q0を算出する。より詳しくは、初期上体質点水平位置速度のX軸方向成分(X軸方向での状態量ベクトル)及びY軸方向成分(Y軸方向での状態量ベクトル)に、それぞれΓ-1を乗じることによって、X軸方向及びY軸方向の各軸方向での初期発散成分q0が算出される。
以上の如く定常歩容の初期発散成分q0を算出した後、S224に進み、歩容生成装置100は、定常歩容の初期発散成分q0を今回歩容支持脚座標系から見た値に変換し、これをq0”として求める。また、歩容生成装置100は、初期上体鉛直位置速度(Z0,Vz0)を今回歩容支持脚座標系から見た値に変換し、これを(Z0”,Vz0”)として求める。
補足すると、(Z0”,Vz0”)は、第2旋回歩容の支持脚座標系(図11のX”軸、Y”軸を水平面の2軸とする支持脚座標系)から見た第2旋回歩容終端上体鉛直位置速度に一致する。また、q0”も第2旋回歩容の支持脚座標系から見た第2旋回歩容終端発散成分に一致している。したがって、これらの性質を用いて、(Z0”,Vz0”)およびq0”を算出するようにしても良い。
以上により、図8のS024の処理、すなわち定常歩容の初期状態を求めるサブルーチン処理が終了する。
次いで、図8のS026に進み、歩容生成装置100は、今回歩容の歩容パラメータを決定(一部は仮決定)する。S026では、より具体的には、図16のフローチャートに示す処理が実行される。
まず、S600において、歩容生成装置100は、今回歩容の足平位置姿勢軌道が定常歩容の足平位置姿勢軌道につながるように、今回歩容の足平軌道パラメータを決定する。
具体的には、今回歩容初期遊脚足平位置姿勢(今回歩容の初期の遊脚足平位置姿勢)は、今回歩容支持脚座標系から見た前回歩容終端支持脚足平位置姿勢(現在の遊脚足平位置姿勢)に設定される。
今回歩容初期支持脚足平位置姿勢(今回歩容の初期の支持脚足平位置姿勢)は、今回歩容支持脚座標系から見た前回歩容終端遊脚足平位置姿勢(現在の支持脚足平位置姿勢)に設定される。
また、今回歩容終端遊脚足平位置姿勢は、今回歩容支持脚座標系から見た次回歩容支持脚座標系(今回歩容に係わる1歩目の遊脚足平22の着地予定位置姿勢の要求値)に対応して決定される。すなわち、今回歩容終端遊脚足平位置姿勢から遊脚足平22を床に接触させたまま、すべらないように該足平22をその底面のほぼ全面が接地するまでピッチ方向に所定角度だけ回転させたときの、該足平22の代表点が今回歩容支持脚座標系から見た次回歩容支持脚座標系の原点に合致するように今回歩容終端遊脚足平位置姿勢が決定される。
今回歩容終端では、支持脚足平22は離床して空中にある。支持脚足平22が離床してから後の軌道を決定するために、まず、次回歩容終端での今回歩容の支持脚足平22の位置姿勢、すなわち次回歩容終端遊脚足平位置姿勢が決定される。次回歩容終端遊脚足平位置姿勢は、今回歩容支持脚座標から見た次次回歩容支持脚座標(今回歩容に係わる2歩目の遊脚足平22の着地予定位置姿勢の要求値)に対応して決定される。より具体的には、次回歩容終端遊脚足平位置姿勢は、その位置姿勢から、足平22を床に接触させたまま、すべらないように該足平22をその底面のほぼ全面が床に接地するまでピッチ方向に所定角度だけ回転させたときの該足平22の代表点が今回歩容支持脚座標から見た次次回歩容支持脚座標の原点に合致するように決定される。
そして、今回歩容終端支持脚足平位置姿勢は、上記の如く決定した今回歩容初期支持脚足平位置姿勢から、次回歩容終端遊脚足平位置姿勢に至る足平位置姿勢軌道を、今回歩容終端まで前記有限時間整定フィルタにより生成することにより求められる。
次いで、S602に進み、歩容生成装置100は、今回歩容の上体姿勢軌道パラメータを決定する。この上体姿勢軌道パラメータは、それにより規定される上体姿勢軌道が、定常歩容の上体姿勢軌道に連続してつながる(今回歩容終端での上体姿勢角及び角速度がそれぞれ定常歩容初期の上体姿勢角及び角速度に一致する)ように決定される。この場合、本実施形態では、今回歩容の上体姿勢軌道パラメータにより規定される上体姿勢は、定常歩容の上体姿勢と同様に、定常的に一定姿勢である前記基準上体姿勢(上体24の体幹軸が鉛直方向を向く姿勢)に設定される。
なお、今回歩容の上体姿勢軌道は、定常歩容の上体姿勢軌道に連続的につながるように設定されておれば、今回歩容の初期から終端まで経時的に変化するように設定してもよい。
次いで、S604に進み、歩容生成装置100は、今回歩容の腕姿勢軌道パラメータを決定する。この腕姿勢軌道パラメータは、今回歩容の腕姿勢軌道が前記定常歩容の腕姿勢軌道に連続してつながるようにして、定常歩容の腕姿勢軌道パラメータと同様に決定される。
なお、ここで決定される今回歩容の腕姿勢軌道パラメータは、定常歩容の腕姿勢軌道パラメータと同様に、鉛直軸(あるいは上体24の体幹軸)まわりの両腕体の角運動量変化に関すること以外のパラメータ(例えば上体24に対する腕体の手先の相対高さや腕体全体の相対重心位置などを規定するパラメータ)である。
次いで、S606に進み、歩容生成装置100は、今回歩容の床反力鉛直成分軌道パラメータを、それにより規定される床反力鉛直成分軌道が前記図5に示した如く実質的に連続な軌道になるように決定する。ただし、今回歩容の床反力鉛直成分軌道パラメータは、今回歩容のロボット1の全体重心鉛直位置速度と床反力鉛直成分軌道とのいずれもが、前記定常歩容に連続してつながるように決定される。
具体的には、まず、図8のS024の処理(定常歩容初期状態の決定処理)で最終的に求めた定常歩容の初期上体鉛直位置速度を今回歩容支持脚座標系から見た値に変換した値(Z0”,Vz0”)、すなわち、図14のS224で求めた(Z0”,Vz0”)等を基に、今回歩容支持脚座標系から見た定常歩容の初期全体重心鉛直位置速度が求められる。
例えば、前記した3質点モデル(S206の処理に関して説明した3質点モデル)を用いて定常歩容の初期全体重心鉛直位置速度が求められる。この場合、今回歩容支持脚座標系から見た定常歩容初期の目標足平位置姿勢とその時間的変化率(変化速度)とに基づき、3質点モデルにおける2つの脚質点の位置・速度が求められる。さらに、上記(Z0”,Vz0”)と、定常歩容初期の上体姿勢角(=基準上体姿勢角)及びその時間的変化率とから、3質点モデルにおける上体質点の位置・速度が求められる。そして、これらの3つの質点の位置・速度から、その3質点の重心位置及び速度が、定常歩容の初期全体重心鉛直位置速度として算出される。
なお、より多くの質点を有する多質点モデル(例えばロボット1の各リンクに質点を有する幾何学モデル)を用いることによってより正確に、定常歩容の初期全体重心鉛直位置速度を求めるようにしてもよい。また、上体24以外の部位の質量が上体24に比して十分に小さいような場合には、上記(Z0”,Vz0”)と、定常歩容初期の上体姿勢角(=基準上体姿勢角)及びその時間的変化率とから、定常歩容の初期全体重心鉛直位置速度を求めるようにしてもよい。
このようにして求めた定常歩容の初期全体重心鉛直位置速度を、前記式13と下記式41の終端全体重心鉛直位置速度に代入すると共に、前回歩容の瞬時値(より詳しくは、前回歩容の終端状態を今回歩容支持脚座標系から見た値)の全体重心鉛直位置および速度を、前記式13と下記式41の初期全体重心鉛直位置および速度に代入し、式13と式41の関係を満足するように、今回歩容の床反力鉛直成分軌道パラメータが決定される。ただし、式13と式41における積分値は今回歩容の初期から終端までの期間の積分値とする。
終端全体重心鉛直速度−初期全体重心鉛直速度
=(床反力鉛直成分/ロボットの質量)の1階積分
+重力加速度の1階積分
…式41
(ただし、重力加速度は負の値とする。)
より具体的には、まず、図5に示したような床反力鉛直成分軌道を規定する床反力鉛直成分軌道パラメータ(折れ点の時刻等)のうちの、少なくとも2つのパラメータを独立な未知変数として、その未知変数の値を、式13と式41とからなる連立方程式を解くことによって決定する。
未知変数とする床反力鉛直成分軌道パラメータには、例えば、図5に示した床反力鉛直成分軌道における台形の高さ(床反力鉛直成分のピーク値)と幅(片脚支持期時間)とを選択すればよい。この場合、図5の台形の両側部分の傾きは今回歩容周期等に応じて決定した値とし、あるいは、片脚支持期から空中期に移行する時刻を除く床反力鉛直成分軌道の折れ点の時刻を今回歩容周期等に応じて決定した値とする。補足すると、未知変数を1つとすると、一般的には式13と式41の連立方程式を満足する解が存在しない。
補足すると、本実施形態では、上記の如く決定される今回歩容の床反力鉛直成分軌道パラメータが、本発明における上下方向慣性力パラメータに相当する。従って、S606の処理によって、本発明における上下方向慣性力パラメータ決定手段が実現されることとなる。
次いで、S608に進み、歩容生成装置100は、今回歩容のZMP軌道が、安定余裕が高くかつ急激な変化をしないように、該ZMP軌道を規定するZMP軌道パラメータ(ZMP軌道の折れ点の時刻や位置等)を決定する。例えば、ZMP軌道が、図6に示したようなパターンになるようにZMP軌道パラメータが仮決定される。ただし、今回歩容のZMP軌道が前記定常歩容のZMP軌道に連続してつながるように(今回歩容終端におけるZMPの位置が定常歩容初期のZMPの位置に一致するように)、今回歩容のZMP軌道パラメータが仮決定される。この場合、走行歩容では、片脚支持期におけるZMP軌道の折れ点の時刻や位置の設定の仕方は、前述した定常歩容のZMP軌道パラメータの設定の仕方と同様でよい。そして、空中期における目標ZMP軌道が、空中期開始時から、定常歩容初期のZMPの位置まで、直線的に連続して変化するようにZMP軌道パラメータを設定すればよい。
なお、S608で決定される今回歩容のZMP軌道パラメータは、仮決定されたものであり、後述するように修正される。以降、その修正が終了するまでの仮決定されたZMP軌道パラメータにより規定される今回歩容の目標ZMPを、以降、仮目標ZMPと呼ぶ。その仮決定されたZMP軌道パラーメータを含む今回歩容の歩容パラメータを仮今回歩容パラメータという。
図8の説明に戻って、歩容生成装置100は、上記のごとくS026の処理を行った後、次いでS028において、今回歩容の歩容パラメータ(詳しくはZMP軌道パラメータ)を修正する処理を実行する。この処理では、今回歩容の上体位置姿勢軌道を定常歩容に連続させ、もしくは近づけるべく歩容パラメータのうちのZMP軌道パラメータが修正される。
この処理は、図17のフローチャートに示す如く実行される。
まずS700を経てS702に進み、歩容生成装置100は、仮決定されたZMP軌道パラメータを含む仮今回歩容パラメータを基に、今回歩容の終端時刻までの仮今回歩容を生成する。
S702では、図18のフローチャートに示す処理が実行される。
以下説明すると、歩容生成装置100は、まず、S800において、仮今回歩容の生成用時刻Tk等の初期化を行なう。具体的には、仮今回歩容の生成用時刻Tkが現在時刻tに初期化される。また、時刻t−Δtで歩容生成装置100が既に決定した目標上体位置及び目標上体速度、目標上体姿勢角及びその角速度、目標足平位置姿勢、目標腕姿勢など、時刻t−Δtでの目標歩容の運動状態(今回歩容支持脚座標系で見た運動状態)が、仮今回歩容の現在時刻tの直前の初期運動状態として設定される。
次いで、S802を経てS804において、歩容生成装置100は、仮今回歩容の生成用時刻k(現在値)が今回歩容の終端時刻Tcurr以前の時刻であるか否か(Tk≦Tcurrであるか否か)を判断する。この判断結果が肯定的である場合には、歩容生成装置100は、S806〜S816の処理(詳細は後述する)を実行することによって、時刻Tkでの歩容の瞬時値を決定する。
次いでS818に進み、歩容生成装置100は、仮今回歩容の生成用時刻Tkを、所定の刻み時間ΔTkだけ増加させた後、S804の判断を再び行なう。ここで、刻み時間ΔTkは、例えば制御周期Δtと一致させておけばよい。ただし、演算量を低減するために、ΔTkをΔtよりも長い時間に設定してもよい。
S804の判断結果が肯定的である場合には、S806からS818までの処理が繰り返され、S804の判断結果が否定的になった場合には、図18の処理、すなわち、図17のS702の処理は終了する。これにより、今回歩容の初期時刻から終端時刻までの仮今回歩容の生成が完了する。
歩容生成装置100は、上記S806〜S816で仮今回歩容の瞬時値を決定する処理を次のように実行する。この仮今回歩容の瞬時値の決定の仕方は、図15のS306〜S316で定常歩容(仮の定常歩容)の瞬時値を決定する仕方と同様である。
まずS806において、歩容生成装置100は、仮今回歩容パラメータ(より詳しくは床反力鉛直成分軌道パラメータ)を基に、図5に示した目標床反力鉛直成分軌道の時刻Tkでの瞬時値を求める。
次いで、S808において、歩容生成装置100は、仮今回歩容パラメータ(より詳しくは仮決定のZMP軌道パラメータ)を基に、図6に示した目標ZMP軌道(仮目標ZMP軌道)の時刻Tkでの瞬時値を求める。
次いでS810に進み、歩容生成装置100は、仮今回歩容パラメータ(より詳しくは足平軌道パラメータ、上体姿勢軌道パラメータ、及び腕姿勢軌道パラメータ)を基に、目標足平位置姿勢、目標上体姿勢および目標腕姿勢の時刻Tkでの瞬時値をそれぞれ求める。ただし、目標腕姿勢に関しては、より詳しくは両腕体の全体重心位置が決定されるが、鉛直軸(または上体24の体幹軸)まわりの角運動量を変化させる腕体の運動(腕振り運動)は、未だ決定されない。
次いでS812に進み、歩容生成装置100は、S806で求めた目標床反力鉛直成分を満足する(ロボット1の全体重心の鉛直方向の慣性力と重力との和を目標床反力鉛直成分に釣り合わせる)ように、時刻Tkでの上体鉛直位置を算出する。その算出の仕方は、図15のS312(定常歩容の上体鉛直位置の瞬時値を算出する処理)と同じでよい。ただし、S812で使用する多質点モデルは、S312で使用するものと異ならせてもよい。
次いでS814に進み、歩容生成装置100は、仮今回歩容の時刻Tkでの上体水平加速度を決定する処理を実行する。
具体的には、仮今回歩容の生成用の時刻Tkが現在時刻t(歩容生成装置100の現在の制御周期の時刻)に一致する場合には、歩容生成装置100は、補償全床反力モーメント分配器110から与えられたモデル操作床反力モーメント(現在の制御周期での値)が目標ZMPまわりに発生するように(ロボット1の慣性力と重力との合力が目標ZMPまわりに発生するモーメントの水平成分がモデル操作床反力モーメントに釣り合うように)、仮今回歩容の時刻Tkでの上体水平加速度を決定する。
言い換えると、ロボット1の慣性力と重力との合力が目標ZMPまわりに発生するモーメントがモデル操作床反力モーメントの符号を反転したモーメントに一致するように、時刻Tkでの上体水平加速度が決定される。
また、仮今回歩容の生成用の時刻Tkが、現在時刻tよりも将来の時刻である場合(Tk>現在時刻tである場合)には、歩容生成装置100は、目標ZMPを満足するように(ロボット1の慣性力と重力との合力が目標ZMPまわりに発生するモーメントの水平成分が“0”であるという動力学的平衡条件を満足するように)、時刻Tkでの上体水平加速度を決定する。
これらの場合において、仮今回歩容の時刻Tkでの上体水平加速度2を算出する処理は、例えば図9の動力学モデル(倒立振子モデル)を用いて行なわれる。
具体的には、仮今回歩容の時刻Tkの上体鉛直位置と、時刻(Tk−ΔTk)の上体水平位置と、時刻Tkでの目標上体姿勢とから、時刻Tkでの上体質点24bの鉛直位置及び水平位置が決定される。なお、時刻Tkが現在時刻tである場合には、時刻(Tk−ΔTk)の上体水平位置としては、S800で設定した初期運動状態での上体水平位置が用いられる。
補足すると、時刻(Tk−ΔTk)までの上体水平位置の時系列又は時刻(Tk−ΔTk)での歩容状態を基に時刻Tkでの上体水平位置を補間的に推定し、その推定した上体水平位置を時刻(Tk−ΔTk)の上体水平位置の代わりに用いてもよい。
また、仮今回歩容の時刻Tkでの床反力鉛直成分から、上体質点24bに作用する重力(=mb*g)を差し引いた値を、上体質点24bの質量mbにより除算することによって、時刻Tkでの上体質点24bの鉛直加速度が求められる。
そして、Tk=現在時刻tである場合には、上記の如く求めた上体質点24bの鉛直位置、水平位置、鉛直加速度をそれぞれ、前記式04のZb、Xb、d2Zb/dt2に代入すると共に、時刻Tkでの目標ZMPの水平位置および鉛直位置を式04のXzmp,Zzmpに代入し、さらに、同式04のMzmp_yを、モデル操作床反力モーメント(現在値)に一致させてなる式を、d2Xb/dt2について解くことにより、時刻Tkでの上体質点水平加速度d2Xb/dt2が算出される。そして、この上体質点水平加速度d2Xb/dt2が時刻Tkでの上体水平加速度として求められる。
また、Tk>現在時刻tである場合には、式04のMzmp_yを、“0”としてなる式をd2Xb/dt2について解くことにより、時刻Tkでの上体水平加速度が算出される。この場合、式04のMzmp_yを、“0”とすること以外の処理はは、Tk=現在時刻tである場合と同様である。
尚、より厳密な動力学モデルを用いて、目標ZMPまわりの床反力モーメントの水平成分をモデル操作床反力モーメントに一致させ、あるいは、“0”とするような上体水平加速度を探索的に求めるようにしてもよい。
次いでS814に進み、歩容生成装置100は、目標ZMPを満足するように(ロボット1の慣性力と重力との合力が目標ZMPまわりに発生するモーメントの水平成分が“0”であるという動力学的平衡条件を満足するように)、時刻Tkでの上体水平加速度を決定する。この上体水平加速度の算出の仕方は、図15のS314と同じであり、例えば図7の動力学モデル(倒立振子モデル)を用いて上体水平加速度が算出される。
尚、より厳密な動力学モデルを用いて、目標ZMPまわりの床反力モーメントの水平成分を“0”にするような上体水平加速度を探索的に求めるようにしてもよい。
次いでS816に進み、歩容生成装置100は、S814で求めた上体水平加速度を2階積分することにより現在時刻Tkでの上体水平位置を算出する。この算出の仕方は、図15のS316と同じである。
以上が図17のS702で歩容生成装置100が実行する処理(仮今回歩容の生成処理)の詳細である。
歩容生成装置100は、以上のごとくS702の処理を実行した後、次にS704の処理を実行する。
S704では、歩容生成装置100は、S702で生成した仮今回歩容の終端での上体水平位置速度(Xe,Vxe)を基に、終端発散成分qe1(仮今回歩容の終端での発散成分qe1)を算出する。
具体的には、歩容生成装置100は、まず、(Xe,Vxe)と仮今回歩容の終端での上体姿勢(本実施形態では基準上体姿勢)とから、図7の倒立振子モデルの上体質点24bの水平位置速度(水平位置及び水平速度)を決定する。そして、この上体質点水平位置速度から、前記した図14のS222と同様に、前記式118に基づいて、仮今回歩容の終端発散成分qe1を算出する。この場合に、qe1を算出するために用いるΓ-1は、S222の処理で算出したものと同じである。
次いでS706に進み、歩容生成装置100は、上記の如く求めた仮今回歩容の終端発散成分qe1と、図14のS224で求めた定常歩容の初期発散成分q0”との差である終端発散成分誤差errq(=qe1−q0'')を算出する。
さらに、S708に進んで、歩容生成装置100は、上記の如く求めた終端発散成分誤差errqが許容範囲内(“0”近傍の範囲内)にあるか否か、すなわち、errqが“0”もしくはほぼ“0”であるか否かを判断する。なお、S708の判断は、詳しくは、X軸方向及びY軸方向の各軸方向の終端発散成分誤差errqに対して行なわれる。そして、X軸方向及びY軸方向の両軸方向の終端発散成分誤差errqが許容範囲内にある場合にS708の判断結果が肯定的になり、いずれかの軸方向の終端発散成分誤差errqが許容範囲内に無い場合には、がS708の判断結果が否定的になる。
このS708の判断結果が否定的である場合には、S710に進み、歩容生成装置100は、a=Δa(Δa:所定の微小量)として、現在の仮目標ZMP軌道を台形パターンの修正量によって図19に示す如く修正してなる目標ZMP軌道を用いて、前記S702と同様に、終端時刻までの仮今回歩容を生成する。すなわち、歩容生成装置100は、S702での仮今回歩容の生成に用いた仮目標ZMP軌道の代わりに、それを修正してなる目標ZMP軌道を用いて、改めて仮今回歩容を生成する。この場合、目標ZMP以外の今回歩容パラメータは、S702で使用したものと同じである。
ここで、図19を参照して、上記「a」は、仮今回歩容の終端発散成分を定常歩容の初期発散成分にできるだけ一致させる(ひいては今回歩容の上体水平位置姿勢軌道を定常歩容の上体水平位置姿勢軌道に収束させる)ために、仮目標ZMP軌道を修正する(仮目標ZMP軌道に加える)台形パターンの修正量(以下、ZMP修正量ということがある)の高さである。
この場合、本実施形態では、仮目標ZMP軌道の修正は、支持脚足平22の底面のほぼ全面が接地する期間(足底全面接地期間)において行なうようにしている。このため、ZMP修正量は、足底全面接地期間内において、ZMP修正量≠0となり、足底全面接地期間以外の期間では、ZMP修正量=0となるように設定されている。また、上記台形パターンのZMP修正量の折れ点の時刻は、足底全面接地期間における仮目標ZMP軌道の折れ点の時刻に合わせて設定されている。なお、S710でa=Δa(Δa:所定の微小量)とするのは、現在の仮目標ZMP軌道を上記台形パターンのZMP修正量によって微小量だけ、修正したときの前記終端発散成分誤差errqの変化を観測するためである。
補足すると、S710の上記の処理では、目標ZMPのX軸方向位置を修正することを例に採って説明したが、実際には目標ZMPのY軸方向位置の修正も併せて行なわれる。そのY軸方向位置の修正は、X軸方向位置の修正と同様に、足底全面接地期間における目標ZMP軌道を仮目標ZMP軌道から台形パターンで変化させるように行なわれる。この場合、上記Δaは各軸方向で同じ値でよいが、互いに異なる値に設定してもよい。
なお、X軸方向及びY軸方向の一方の軸方向での目標ZMP軌道の修正が、他方の軸方向での終端発散成分に影響を及ぼさないか、もしくはその影響が十分に小さい場合には、S710から、後述のS716までの処理をX軸方向とY軸方向とで各別に行なうようにしてもよい。
上記のようにS710にて仮今回歩容を生成した後、次にS712に進み、歩容生成装置100は、S710で求めた仮今回歩容の終端での上体水平位置速度(Xe2,Vxe2)を基に、この仮今回歩容における終端発散成分qe2をS704と同様に算出する。
尚、本実施形態では、S710においてΔaを所定値の微小量としたが、以下に説明する繰り返し演算によって終端発散成分誤差errqが小さくなるにつれてΔaを“0”に近づけるように、該Δaを可変的に設定するようにしてもよい。ただし、一般には、Δaを定数にしておいても、数回の繰り返し演算で終端発散成分誤差errqを許容範囲内に収めることができる。
次いでS714に進み、歩容生成装置100は、パラメータ感度r(Δaに対する終端発散成分誤差の変化の割合)を図中の式によって算出する。すなわち、歩容生成装置100は、S712で算出した終端発散成分qe2とS704で算出した終端発散成分qe1との差(=qe2−qe1)をΔaにより除算することによって、パラメータ感度rを算出する。この場合、より詳しくは、例えば(qe2−qe1)のX軸方向成分を、X軸方向のZMP修正量に係わるΔaにより除算することによって、X軸方向でのパラメータ感度rが算出される。また、(qe2−qe1)のY軸方向成分を、Y軸方向のZMP修正量に係わるΔaにより除算することによって、Y軸方向でのパラメータ感度rが算出される。
次いでS716に進み、歩容生成装置100は、a=−errq/r、すなわちS706で求めた終端発散成分誤差errqを、S714で求めたパラメータ感度rで割った値の符号を反転させてなる値を台形パターンのZMP修正量の高さaとして、このZMP修正量によって図19に示す如く仮目標ZMPパターンを修正してなる目標ZMPを、改めて仮目標ZMPとして決定する。この場合、ZMP修正量の高さaは、X軸方向とY軸方向とで各別に算出される。
なお、X軸方向及びY軸方向の一方の軸方向での目標ZMP軌道の修正が、他方の軸方向での終端発散成分に影響を及ぼす場合には、その影響分を考慮して各軸方向でのZMP修正量の高さaを決定することが好ましい。
次いで歩容生成装置100は、S702からの処理を再び実行する。このとき、S708の判断結果が否定的である限りは、上記したS702〜S716の処理が繰り返され、S708の判断結果が肯定的となった場合に、その繰り返しループ(S700)を抜けてS718に進む。
以上のS702〜S716のループ処理によって、終端発散成分qe1を、定常歩容の初期発散成分q0''に一致もしくはほぼ一致させることができるZMP修正量が(ひいては今回歩容の目標ZMP軌道が)探索的に求められる。
かかるループ処理の次のS718では、歩容生成装置100は、現在の仮目標ZMP軌道(S700の繰り返しループを抜ける直前で設定された仮目標ZMP軌道)を最終的に今回歩容の目標ZMP軌道として、これに対応するZMP軌道パラメータを決定する。これにより、図8のS028の処理(今回歩容パラメータの修正処理)が終了する。
なお、本実施形態では、ZMP修正量を台形パターンに設定したが、例えば、三角形状のパターンや、曲率が連続的に変化するようなパターン等に決定してもよい。
図8に戻って、上記のごとくS028において今回歩容パラメータを修正した後、S030に進み、歩容生成装置100は、今回歩容の初期から終端までの各時刻での目標ZMPまわりの床反力モーメント(より詳しくは床反力モーメントの水平成分)の許容範囲を規定する床反力モーメント許容範囲パラメータを決定する。この床反力モーメント許容範囲パラメータにより規定される床反力モーメントの許容範囲は、前記フィードバック操作量演算部110において、後述する如く使用されるものであると共に、後述するS030の判断処理で使用されるものである。
なお、床反力モーメントを、床反力鉛直成分で割ったものが、ZMP(床反力中心点)の目標ZMPからのずれ量に相当するものとなる。従って、床反力モーメント許容範囲を、ZMP(床反力中心点)の位置の許容範囲としてのZMP許容範囲(床反力中心点許容範囲)に変換し、このZMP許容範囲を規定するパラメータを決定するようにしてもよい。
S030で決定する床反力モーメント許容範囲は、前記フィードバック操作量演算部110において使用される場合に、前記複合コンプライアンス動作決定部104の制御処理(コンプライアンス制御)によって制御される目標ZMPまわりの実床反力モーメントの許容範囲としての意味を持つものである。この床反力モーメント許容範囲について以下に説明する。
複合コンプライアンス動作決定部104の処理によるコンプライアンス制御は、目標ZMPまわりに発生する床反力モーメントがコンプライアンス制御用目標床反力モーメントになるように足平22の位置姿勢を制御する。コンプライアンス制御が目標通りに忠実に作動すると、実床反力中心点は、目標床反力モーメントを床反力鉛直成分で割った値だけ、目標ZMPから水平方向に位置をずらした点になる。実床反力中心点は、いわゆる支持多角形で表されるZMP存在可能範囲(厳密な表現では、実床反力中心点の存在可能範囲)を逸脱することはできない。そして、実床反力中心点がZMP存在可能範囲の境界に近づき過ぎると、足平22の本来の接地感を低下させたり、足平22の底面が浮いてしまうような不具合が発生する。
床反力モーメント許容範囲は、このような不具合を防止するために、コンプライアンス制御用床反力モーメントに制限を加えるものである。したがって、床反力モーメント許容範囲は、これを床反力鉛直成分で割ることによって求められる床反力中心点許容範囲が、ZMP存在可能範囲に含まれるように決定されることが望ましい。
より具体的には、床反力中心点許容範囲(ZMP許容範囲)は、支持多角形に応じて決定されるべきである。この場合、床反力中心点許容範囲は、一般的には、複雑な形状に設定することができる。ただし、本実施形態では、演算を簡素化するために、床反力中心点許容範囲を、例えば床面上の長方形(X軸方向およびY軸方向に平行な辺を有する長方形)の領域とする。この場合、床反力中心点許容範囲を床反力モーメントに変換してなる床反力モーメント許容範囲は、X軸方向成分とY軸方向成分とでそれぞれ独立に範囲が設定される。
例えば、支持多角形と目標ZMPとが図20に示すような状況にある場合には、図示のごとく、床反力中心点許容範囲(ZMP許容範囲)は支持多角形に内包されるように設定される。また同時に、床反力中心点許容範囲(ZMP許容範囲)は目標ZMPを内包するように設定される。
なお、図20に点描領域として示す支持多角形は、歩行歩容(歩行歩容に関する歩容生成については後に補足説明を行なう)の両脚支持期での支持多角形を例示している。この例では、右足平22Rのつま先が接地し、左足平22Lのかかとが接地している状態を模式的に示している。
歩行歩容あるいは図4の走行歩容における片脚支持期では、支持多角形は、支持脚足平22の接地面(床面との接触面)である。この場合においては、本実施形態では、床反力中心点許容範囲は、目標ZMPを内包しつつ、該支持多角形に内包され、もしくは、その支持多角形とほぼ一致するように設定される。なお、目標ZMPが支持多角形から逸脱している場合、あるいは、支持多角形の境界に近過ぎる場合には、例えば、暫定的に、目標ZMPを含まない支持多角形内の任意の領域、あるいは、目標ZMPとは異なる支持多角形内の点(面積が“0”となる領域)が床反反力中心点許容範囲として設定される。
また、図4の走行歩容の空中期では、床反力中心点許容範囲は、例えば目標ZMPを中心として、X軸、Y軸両方向に幅が“0”の領域、すなわち、目標ZMPそのものの点(これは面積が“0”の領域を意味する)に設定される。
なお、このように床反力中心点許容範囲を設定する場合、今回歩容における支持多角形は、S028(又はS026)で決定した今回歩容パラメータの足平位置姿勢軌道パラメータを基に算出される。
本実施形態では、上記の如く設定した床反力中心点許容範囲を床反力モーメントに変換してなる範囲を、床反力モーメントの標準許容範囲として決定する。該標準許容範囲は、ロボット1の移動環境の床が、標準的な床(例えば、摩擦係数や硬さが所要の規格内に収まるような平面状の床)であることを前提とする許容範囲である。
そして、本実施形態では、歩容生成装置100は、床反力モーメントの標準許容範囲を、入力される床形状情報に応じて適宜、修正することによって床反力モーメント許容範囲パラメータを決定する。
具体的には、例えば、入力された床形状情報から認識される床が、平面状の床ではなく、今回歩容あるいは次回歩容の遊脚足平22の着地予定位置姿勢を、要求パラメータにより規定される位置姿勢から変更することが難しいような床(例えば飛び石状の床)である場合には、ロボット1の実際の姿勢が目標とする姿勢がずれた場合に、できるだけ前記コンプライアンス制御用目標床反力モーメントによって(ひいては実際のロボット1の足平22の踏ん張りによって)、実際のロボット1の姿勢のずれを抑制することが望ましい。このため、このような場合には、歩容生成装置100は、床反力モーメント許容範囲を標準許容範囲よりも拡張する(ただし、該床反力モーメント許容範囲に対応する床反力中心点許容範囲が前記支持多角形を逸脱しない範囲内で拡張する)ように、床反力モーメント許容範囲パラメータを決定する。
また、例えば、ロボット1に階段を昇降させるような場合には、通常、ロボット1の遊脚足平22の踵寄りの部分又はつま先寄りの部分を階段の段差面からはみ出させるように該遊脚足平22を着地させる。この場合、標準的な床上でロボット1を移動させる場合に比して、支持多角形の面積が小さくなる。このため、歩容生成装置100は、入力された床形状情報から認識される床(ロボット1が移動しようとする床)が階段である場合には、床反力モーメント許容範囲を標準許容範囲よりも狭めるように、床反力モーメント許容範囲パラメータを決定する。
また、例えば、入力された床形状情報から認識される床の硬さが、標準的な床の硬さよりも柔らかいような場合には、実際のロボット1で目標ZMPまわりに発生させるコンプライアンス制御用目標床反力モーメントをあまり大きくすることが難しい。このため、このような場合には、歩容生成装置100は、床反力モーメント許容範囲を床反力モーメント許容範囲を標準許容範囲よりも狭めるように、床反力モーメント許容範囲パラメータを決定する。
以上のように、S030では、床反力モーメント許容範囲(あるいはこれに対応する床反力中心点許容範囲)は、目標ZMP軌道と、足平位置姿勢軌道パラメータに応じて規定される支持多角形とに依存するだけでなく、床形状(床の性状を含む)にも依存するように決定される。
なお、本実施形態では、床反力モーメントの標準許容範囲を床形状に応じて適宜修正するようにしたが、目標ZMP軌道と、支持多角形とに応じて設定した床反力中心点許容範囲を床形状に応じて修正し、その修正後の床反力中心点許容範囲を床反力モーメント許容範囲に変換するようにしてもよい。
上記のごとくS030において床反力中心点許容範囲パラメータを決定した後、S031に進み、歩容生成装置100は、S028で決定したZMP軌道パラメータにより規定される目標ZMP軌道が適切な軌道であるか否かを判断する。この場合、本実施形態では、歩容生成装置100は、今回歩容のZMP軌道パラメータにより規定される目標ZMP軌道の各瞬時値(所定の刻み時間毎の瞬時値)が、前記S030で決定した床反力モーメント許容範囲に対応する床反力中心点許容範囲に存するか否かによって、該目標ZMP軌道が適切な軌道であるか否かを判断する。
より具体的には、歩容生成装置100は、目標ZMPの各瞬時値に対応する床反力中心点許容範囲を、床反力モーメント許容範囲パラメータに基づき決定する。
そして、歩容生成装置100は、今回歩容の各時刻での目標ZMPが、その時刻での床反力中心点許容範囲内に存する場合には、今回歩容のZMP軌道パラメータにより規定される目標ZMP軌道が適切な軌道であると判断する。また、今回歩容のいずれかの時刻での目標ZMPが、その時刻での床反力中心点許容範囲を逸脱する場合には、今回歩容のZMP軌道パラメータにより規定される目標ZMP軌道が適切な軌道でないと判断する。なお、歩容生成装置100は、ロボット1の両脚体2,2が非接地状態となる空中期における目標ZMPの各瞬時値については、該瞬時値が常に床反力中心点許容範囲内に存するものと見なす。
S031の判断結果が否定的である場合(目標ZMP軌道が適切な軌道でない場合)には、歩容生成装置100は、S020からの処理を改めて実行する。この場合、S020においては、歩容生成装置100は、例えば、今回歩容の遊脚足平22の着地予定位置姿勢(ひいては、次回歩容支持脚座標系の位置姿勢)を、現在値(今回の制御周期における1回前のS020の処理で決定した着地予定位置姿勢)からずらした位置姿勢に変更する。なお、ここで変更する着地予定位置姿勢は、より詳しくは、着地予定位置及び着地予定姿勢のうちのすくなくともいずれか一方である。この場合、該着地予定位置姿勢の変更量は、前記S031の判断処理において、床反力中心点許容範囲から逸脱した目標ZMPの、当該逸脱方向やその逸脱量に応じて決定される。該変更量は、基本的には、変更後の着地予定位置姿勢に応じて規定される支持多角形内に目標ZMPが含まれるか、もしくは、含まれやすくなるように決定される。
なお、今回歩容の遊脚足平22の着地予定位置姿勢だけでなく、次回歩容の遊脚足平22の着地予定位置姿勢(ひいては次次回歩容支持脚座標系の位置姿勢)をも変更するようにしてもよい。さらに、今回歩容又は次回歩容の遊脚足平22の着地予定位置姿勢の変更に合わせて、該遊脚足平22の着地予定時刻を変更するようにしてもよい。
このようにして、歩容生成装置100は、S020の処理を改めて実行した後、S022以降の処理を前記した通りの仕方で、再び実行する。すなわち、S030の判断結果が肯定的になるまで、S020〜S028の処理が繰り返される。
補足すると、S031の判断処理で使用する床反力中心点許容範囲は、S030で決定した床反力モーメント許容範囲(詳しくは、後述するフィードバック操作量演算部110の処理で使用する許容範囲)に対応する床反力中心点許容範囲と同じでなくてもよい。例えば、S031の判断処理で使用する床反力中心点許容範囲は、S030で決定した床反力モーメント許容範囲(詳しくは、後述するフィードバック操作量演算部110の処理で使用する許容範囲)に対応する床反力中心点許容範囲と面積もしくは境界の形状が異なる範囲であってもよい。
図8に戻って、S031の判断結果が肯定的である場合、あるいは、S016の判断結果が否定的である場合には、次にS032において、歩容生成装置100は、今回歩容瞬時値(現在の制御周期での今回歩容の目標足平位置姿勢等の瞬時値)を決定する。このS032では、前記姿勢安定化制御演算部112で前述の如く決定されるモデル操作床反力モーメントが目標ZMPまわりに発生するように今回歩容瞬時値が決定される。
具体的には、図21のフローチャートに示す如く、歩容瞬時値が決定される。以下説明すると、歩容生成装置100は、まず、S1000からS1006まで、図18のS806からS812まで(または図15のS306からS312まで)と同様に処理を実行することによって、今回歩容の現在時刻t(現在の制御周期)での目標床反力鉛直成分、目標ZMP、目標足平位置姿勢、目標上体姿勢、目標腕姿勢、目標上体鉛直位置の瞬時値を算出する。
これらの各瞬時値の算出の仕方は、歩容パラメータとして、図8のS028で最終的に修正した今回歩容パラメータを使用する点を除いて、図18のS806からS812まで(または図15のS306からS312まで)の処理と同じである。
次いで、S1008に進み、歩容生成装置100は、図8のS030で決定した床反力モーメント許容範囲パラメータを基に、今回歩容の現在時刻tでの床反力モーメント許容範囲の瞬時値を求める。この床反力モーメント許容範囲の瞬時値は、歩容生成装置100から、図3に示した前記フィードバック操作量演算部110(詳細は後述する)に出力される。
なお、詳細は後述するが、フィードバック操作量演算部110では、入力された床反力モーメント許容範囲の瞬時値等を用いて、前記全モデル操作床反力モーメント及びコンプライアンス制御用目標床反力モーメントが決定される。さらに、決定された全モデル操作床反力モーメントは、前記全モデル操作床反力モーメント分配器118に入力され、該分配器118により第1モデル操作床反力モーメント及び第2モデル操作床反力モーメントが決定される。そして、決定された第1モデル操作床反力モーメントが、歩容生成装置100の制御周期で、歩容生成装置100によりサンプリングされる(該歩容生成装置100に取り込まれる)。
次いで、S1010に進み、歩容生成装置100は、全モデル操作床反力モーメント分配器118から与えられた第1モデル操作床反力モーメント(最新値)が目標ZMPまわりに発生するように(ロボット1の慣性力と重力との合力が目標ZMPまわりに発生するモーメントの水平成分が第1モデル操作床反力モーメントに釣り合うように)、今回歩容の現在時刻tでの上体水平加速度を決定する。
言い換えると、ロボット1の慣性力と重力との合力が目標ZMPまわりに発生するモーメントが第1モデル操作床反力モーメントの符号を反転したモーメントに一致するように、今回歩容の現在時刻tでの上体水平加速度が決定される。
この場合、図18のS814の処理と同様の仕方で、例えば図7の動力学モデル(倒立振子モデル)を用いて今回歩容の上体水平加速度が算出される。
具体的には、今回歩容の現在時刻tで上体鉛直位置と、時刻(t−Δt)の上体水平位置と、現在時刻tの目標上体姿勢とから、現在時刻tでの上体質点24bの鉛直位置及び水平位置が決定される。なお、時刻(t−Δt)までの上体水平位置の時系列又は時刻(t−Δt)での歩容状態を基に時刻tでの上体水平位置を補間的に推定し、その推定した上体水平位置を時刻(t−Δt)の上体水平位置の代わりに用いてもよい。
また、今回歩容の現在時刻tでの床反力鉛直成分から、上体質点24bに作用する重力(=mb*g)を差し引いた値を、上体質点24bの質量mbにより除算することによって、現在時刻tでの上体質点24bの鉛直加速度が求められる。
そして、上記の如く求めた上体質点24bの鉛直位置、水平位置、鉛直加速度をそれぞれ、前記式04のZb、Xb、d2Zb/dt2に代入すると共に、現在時刻tでの目標ZMPの水平位置および鉛直位置を式04のXzmp,Zzmpに代入し、さらに、同式04のMzmp_yを、第1モデル操作床反力モーメントに一致させてなる式を、d2Xb/dt2について解くことにより、現在時刻tでの上体質点水平加速度d2Xb/dt2が算出される。そして、この上体質点水平加速度d2Xb/dt2が現在時刻tでの上体水平加速度として求められる。
尚、より厳密な動力学モデルを用いて、目標ZMPまわりの床反力モーメントの水平成分を第1モデル操作床反力モーメントに一致させるような上体水平加速度を探索的に求めるようにしてもよい。
次いで、S1012に進み、S1010で上記の如く求めた上体水平加速度を2階積分することにより現在時刻tでの上体水平位置を算出する。この算出の仕方は、図15のS316(又は図18のS816)と同じである。これにより、図8のS032の処理が完了する。
次いでS034に進み、スピン力をキャンセルする(ロボット1の腕体以外の運動によって目標ZMP回りに発生する床反力モーメント鉛直成分を“0”もしくはほぼ“0”にする)ための腕体の動作が決定される。具体的には、腕体を振らなかった場合の目標ZMPにおける床反力モーメント鉛直成分軌道(厳密には、腕体を振らずに歩容を生成した場合において、ロボットの重力と慣性力の合力が目標ZMPに作用するモーメント鉛直成分軌道の各瞬時値の符号を反転したもの)が求められる。すなわち、S032の処理によって生成された今回歩容の運動(これには腕振りの運動は含まれていない)の瞬時値に釣り合う目標ZMP回りの床反力モーメント鉛直成分の瞬時値が求められる。そして、この床反力モーメント鉛直成分の瞬時値を、腕振り運動の等価慣性モーメントで割ることにより、スピン力キャンセルに必要な腕振り動作の角加速度が求められる。なお、腕の振りが大き過ぎる場合には、等価慣性モーメントよりも大きな値で床反力モーメント鉛直成分の瞬時値を除算するようにしてもよい。
次に、歩容生成装置100は、この角加速度を2階積分し、これを積分値が過大になるのを防ぐためのローカットフィルタに通して得た角度を腕振り動作角とする。ただし、腕振り動作では、左右の腕体を前後逆方向に振り、両腕体の重心位置を変化させないようにする。なお、スピン力をキャンセルするための腕振り運動を定常歩容でも生成しておき、これにつながるように、今回歩容における腕振り運動を決定するようにしてもよい。
次いでS036に進み、歩容生成装置100は、時刻tに制御周期Δtを加え、S014からの処理を再び実行する。
以上により、今回歩容の瞬時値が時系列的に生成されることとなる。
この場合、本実施形態では、今回歩容の運動の軌道(時系列)は、目標床反力鉛直成分軌道パラメータ(図17のS606で決定されたパラメータ)により規定される目標床反力鉛直成分軌道を満足する(ロボット1の全体重心の鉛直方向の慣性力と該全体重心に作用する重力との総和が目標床反力鉛直成分に釣り合う)と共に、図7に示した歩容生成用の動力学モデル(本実施形態では倒立振子モデル)において、目標ZMPまわりに前記第1モデル操作床反力モーメントが付加的に発生するように決定される。
加えて、今回歩容の運動の軌道は、将来において、周期的な歩容である定常歩容の運動軌道に収束するように(漸近するように)決定される。より具体的には、今回歩容の運動の軌道は、今回歩容の終端における発散成分が、定常歩容の初期の発散成分に一致するように決定される。
次に、説明を後回しにしたフィードバック操作量演算部110、全モデル操作床反力モーメント分配器118、及び摂動モデル演算部120の処理をさらに詳細に説明する。
まず、摂動モデル演算部120の処理を説明する。摂動モデル演算部120は、本実施形態では、ロボット1の上体24の水平位置の摂動運動と目標ZMPまわりの床反力モーメントの摂動との関係を表現する動力学モデル(摂動動力学モデル)を用いて、前記上体水平位置摂動量を算出する処理を実行する。その摂動動力学モデルは、本実施形態では、図7に示した歩容生成用の動力学モデルと同様の倒立振子モデルにより表現される。
すなわち、図22を参照して、摂動モデル演算部120が用いる摂動動力学モデルでは、目標ZMPを支点として揺動する倒立振子の質点としての上体質点24bの、目標ZMPの直上位置(鉛直方向上方位置)からの水平方向のずれ量ΔXpが、上体24の水平位置の摂動量に相当するものとされる。そして、ΔXpと、倒立振子の支点(目標ZMP)のまわりの床反力モーメントの水平成分の摂動量としての摂動床反力モーメントΔMpとの間の関係が例えば次式60により表現される。
ΔMp=−mb*ΔXp*(g+d2Zb/dt2)
+mb*h*(d2ΔXp/dt2) ……式60
この式60は、歩容生成用の動力学モデルに関する式04のMzmp_y、(Xb−Xzmp)をそれぞれ、ΔMp、ΔXpに置き換えると共に、式04の右辺の(Zb−Zzmp)を、上体質点24bの平均的な高さとしての定数値hに置き換えた式である。なお、式60のΔMpは、詳しくは、X軸まわり成分とY軸まわり成分とからなるベクトル量である。同様に、ΔXpは、X軸方向成分とY軸方向成分とからなるベクトル量である。
そして、摂動モデル演算部120は、式60のΔXpが、出力すべき前記上体水平位置摂動量に相当し、且つ、前記全モデル操作床反力モーメント分配器118から入力される第2モデル床反力操作モーメントが式60のΔMpに相当するものとして、式60を用いて上体水平位置摂動量を決定する。
具体的には、摂動モデル演算部120は、逐次入力される第2モデル床反力操作モーメントの時系列をΔMpの時系列として用い、式60の動力学的関係を満足するΔXpを逐次算出する。この場合、ΔXpの算出演算に必要な、式60の右辺の(g+d2Zb/dt2)の瞬時値(又はmb*(g+d2Zb/dt2)の瞬時値)は、歩容生成装置100が、図16のS606で決定した今回歩容の床反力鉛直成分軌道パラメータ(最新の床反力鉛直成分軌道パラメータ)を基に算出される。その算出手法は、前記した図14のS220の処理の中で(g+d2Zb/dt2)の値を算出する手法と同じである。
そして、摂動モデル演算部120は、算出したΔXpを上体水平位置摂動量として前記上体位置姿勢修正部122に出力する。なお、ΔXpは、図8のS016の判断結果が肯定的となるタイミング(前記した歩容の切替わり目)において、換言すれば、歩容生成装置100による新たな今回歩容の生成開始タイミングで“0”に初期化される。
以上が、摂動モデル演算部120の処理の詳細である。
次に、フィードバック操作量演算部110の処理を説明する。図3を参照して、前記したように、フィードバック操作量演算部110には、実上体姿勢角の実測値と、修正目標上体姿勢角と、摂動モデル演算部120で用いる摂動動力学モデルの状態量(摂動モデル状態量)とが入力される。なお、本実施形態では、フィードバック操作量演算部110に入力される摂動モデル状態量は、摂動モデル演算部120が逐次算出する上体水平位置摂動量ΔXpそのものである。
そして、フィードバック操作量演算部110は、実上体姿勢角の実測値と修正目標上体姿勢角との偏差である前記上体姿勢角偏差を“0”に収束させる機能を有するフィードバック操作量としての実姿勢補償全床反力モーメントを所定のフィードバック制御則により算出する姿勢安定化制御演算部112と、摂動モデル状態量ΔXpとそれに対する所定の目標値との偏差であるモデル状態量偏差を“0”に収束させる機能を有するフィードバック操作量としてのモデル状態補償全床反力モーメントを所定のフィードバック制御則により算出するモデル安定化制御演算部114と、これらの実姿勢補償全床反力モーメント及びモデル状態補償全床反力モーメントから、前記コンプライアンス制御用目標床反力モーメントと全モデル操作床反力モーメントとを決定する補償全床反力モーメント分配器116とを備える。
上記実姿勢補償全床反力モーメントは、上体姿勢角偏差を“0”に収束させるために、実際のロボット1に対して目標ZMPまわりに付加的に作用させる床反力モーメントの要求値である。また、モデル状態補償全床反力モーメントは、モデル状態量偏差を“0”に収束させるために、前記摂動動力学モデル上でのロボットに対して目標ZMPまわりに付加的に作用させる床反力モーメントの要求値である。
なお、本実施形態では、摂動モデル状態量ΔXpの目標値は“0”である。従って、モデル状態量偏差は、摂動モデル状態量ΔXpに一致する。
補足すると、実姿勢補償全床反力モーメント及びモデル状態補償全床反力モーメントのそれぞれは、より詳しくは、ロール方向(X軸まわり)の成分と、ピッチ方向(Y軸まわり)の成分とからなる。以降の説明では、実姿勢補償全床反力モーメント及びモデル状態補償全床反力モーメントをそれぞれ、参照符号Mdmd、Mmdlにより表す。そして、Mdmdのロール方向(X軸まわり)の成分と、ピッチ方向(Y軸まわり)の成分とをそれぞれ、Mdmdx,Mdmdyと表記し、Mmdlのロール方向(X軸まわり)の成分と、ピッチ方向(Y軸まわり)の成分とをそれぞれ、Mmdlx,Mmdlyと表記する。
上記姿勢安定化制御演算部112の処理は、次のように実行される。すなわち、姿勢安定化制御演算部112は、まず、入力された実上体姿勢角の実測値と修正目標上体姿勢角とから、前記上体姿勢角偏差Δθを算出する。そして、姿勢安定化制御演算部112は、上体姿勢角偏差Δθを、図7に示した倒立振子モデルの上体質点24bの水平位置の、基準位置(上体姿勢角偏差Δθが“0”である場合の目標上体位置姿勢に対応する上体質点24bの水平位置)に対する位置偏差ΔXに変換される。具体的には、上体姿勢角偏差Δθは、次式62によって、上体質点24bの水平位置の摂動量としての位置偏差ΔXに変換される。なお、式62におけるhは、上体質点24bの平均的な高さとしての定数値である。
ΔX=h*Δθ ……式62
なお、ΔXは、X軸方向成分とY軸方向成分とからなり、X軸方向成分は、ΔθのうちのY軸まわり方向の成分、Y軸方向成分は、ΔθのうちのX軸まわり方向の成分に比例する値となる。
さらに、姿勢安定化制御演算部112は、上記の如く算出した位置偏差ΔXの時間的変化率(微分値)である速度偏差ΔVxを算出する。なお、速度偏差ΔVxは、実際のロボット1の上体姿勢角の角速度の実測値(又は上体姿勢角の実測値の時間的変化率)と修正目標上体姿勢角の角速度との偏差に、上記定数値hを乗じることによって求めてもよい。
次いで、姿勢安定化制御演算部112は、例えばフィードバック制御則としてのPD則(比例・微分則)の演算処理によって、ΔXを“0”に収束させるように(ひいては、Δθを“0”に収束させるように)、実姿勢補償全床反力モーメントMdmdを算出する。
すなわち、姿勢安定化制御演算部112は、上記の如く算出した位置偏差ΔXと速度偏差ΔVxとから、次式64によりMdmdを算出する。
Mdmd=Kx*ΔX+Kv*ΔVx ……式64
この式152におけるKx、Kvは、それぞれ比例ゲイン、微分ゲインである。これらのKx、Kvの値は、本実施形態では、あらかじめ設定された所定値である。
この場合、より詳しくは、MdmdのY軸まわりの成分Mdmdyは、ΔX、ΔVxのX軸方向を成分に応じて式64により算出される。また、MdmdのX軸まわりの成分Mdmdxは、ΔX、ΔVxのY軸方向成分に応じて式64により算出される。
なお、式64のゲインKx、Kvの値は、ロボット1の運動状態に応じて適宜可変的に設定するようにしてもよい。例えば、歩容生成装置100が図16のS606で決定した今回歩容の床反力鉛直成分軌道パラメータにより規定される現在時刻での上体質点24bの鉛直方向の慣性力に応じて、あるいは、上体質点24bの鉛直方向の慣性力の時系列に応じて、Kx、Kvの一方又は両方の値を可変的に決定するようにしてもよい。このようにすることにより、ロボット1の運動に伴う鉛直方向の慣性力の影響を補償して、上体姿勢角偏差Δθを円滑に“0”に収束させ得るMdmdの値を算出することができる。
以上が、姿勢安定化制御演算部112の処理の詳細である。
なお、Mdmdは、例えば、上体姿勢角偏差Δθと、その時間的変化率である上体姿勢角の角速度偏差とから、PD則によって算出するようにしてもよい。
また、モデル安定化制御演算部114の処理は、次のように実行される。すなわち、モデル安定化制御演算部114は、まず、入力されたモデル状態量偏差ΔXpの時間的変化率ΔVp(=dΔXp/dt)を算出する。そして、モデル安定化制御演算部114は、姿勢安定化制御演算部112と同様に、例えば、フィードバック制御則としてのPD則の演算処理によって、ΔXpを“0”に収束させるように、モデル状態補償全床反力モーメントMmdlを算出する。
すなわち、モデル安定化制御演算部114は、入力されたモデル状態量偏差ΔXpとその時間的変化率ΔVpとから、次式66によりMmdlを算出する。
Mmdl=Ka*ΔXp+Kb*ΔVp ……式66
この式66におけるKa、Kbは、それぞれ比例ゲイン、微分ゲインである。これらのKa、Kbの値は、本実施形態では、あらかじめ設定された所定値である。
この場合、より詳しくは、MmdlのY軸まわりの成分Mmdlyは、ΔXp、ΔVpのX軸方向を成分に応じて式66により算出される。また、MmdlのX軸まわりの成分Mdmdyは、ΔXp、ΔVpのY軸方向成分に応じて式66により算出される。
なお、前記式64におけるKx,Kvに関して補足的に説明した場合と同様に、式66におけるKa,Kbの値をロボット1の運動状態(例えば上体24の鉛直方向の運動状態)に応じて可変的に設定するようにしてもよい。
以上が、姿勢安定化制御演算部112の処理の詳細である。
次に、フィードバック操作量演算部110の補償全床反力モーメント分配器116は、上記の如く算出された実姿勢補償全床反力モーメントMdmdとモデル状態補償全床反力モーメントMmdlとから、以下に説明する処理によって、コンプライアンス制御用目標床反力モーメントと全モデル操作床反力モーメントとを決定する。
すなわち、補償全床反力モーメント分配器116は、コンプライアンス制御用目標床反力モーメントと全モデル操作床反力モーメントとを、これらの床反力モーメントのロール方向(X軸まわり)及びピッチ方向(Y軸まわり)の各成分毎に図23のブロック図で示す処理によって決定する。
具体的には、次式68a,68b,68cにより、コンプライアンス制御用床反力モーメントとモデル操作用床反力モーメントとが決定される。なお、式68a〜68cにおけるMdmdは、ロール方向(X軸まわり)及びピッチ方向(Y軸まわり)の各成分(Mdmdx又はMdmdy)を意味する。このことは、式68a〜68cにおけるMmdlについても同様である。また、床反力モーメント許容範囲は、ロール方向(X軸まわり)及びピッチ方向(Y軸まわり)の各成分の床反力モーメント許容範囲を意味する。この床反力モーメント許容範囲は、歩容生成装置100が図21のS1008において算出した値(最新値)である。
Mdmd+Mmdl>床反力モーメント許容範囲上限値である場合
コンプライアンス制御用床反力モーメント
=床反力モーメント許容範囲上限値
全モデル操作床反力モーメント
=Mmdl−(Mdmd+Mmdl−床反力モーメント許容範囲上限値)
=−(Mdmd−床反力モーメント許容範囲上限値)
……式68a
Mdmd+Mmdl<床反力モーメント許容範囲下限値である場合
コンプライアンス制御用床反力モーメント
=床反力モーメント許容範囲下限値
モデル操作用床反力モーメント
=Mmdl−(Mdmd+Mmdl−床反力モーメント許容範囲下限値)
=−(Mdmd−床反力モーメント許容範囲下限値)
……式68b
床反力モーメント許容範囲下限値≦Mdmd+Mmdl≦床反力モーメント許容範囲上限値である場合
コンプラインアンス制御用床反力モーメント=Mdmd+Mmdl
モデル操作用床反力モーメント=Mmdl
……式68c
なお、上記式68a〜68cでは、Mdmd+Mmdl(詳しくはMdmdx+Mmdlx又はMdmdy+Mmdly)そのものを床反力モーメント許容範囲と比較しているが、本来は、上記式68a〜68cで床反力モーメント許容範囲と比較すべき対象は、Mdmd+Mmdlを目標ZMPまわりの床反力モーメントの基準瞬時値に加えてなるモーメントである。該基準瞬時値は、前記上体姿勢角偏差Δθとモデル状態量偏差ΔXpとの両方が定常的に“0”に維持されていると仮定した場合に歩容生成装置100が生成する目標全床反力のうちの目標ZMPまわりのモーメントである。
この場合、本実施形態では、目標ZMPまわりの床反力モーメントの基準瞬時値は、ロール方向(X軸まわり)及びピッチ方向(Y軸まわり)のいずれの成分についても、定常的に“0”である。従って、この基準瞬時値にMdmd+Mmdlを加えたものは、Mdmd+Mmdlに等しい。このため、上記式68a〜68cでは、Mdmd+Mmdl(詳しくはMdmdx+Mmdlx又はMdmdy+Mmdly)を直接的に床反力モーメント許容範囲と比較するようにしている。
補足すると、歩容生成装置100が図8のS030で決定した床反力モーメント許容範囲パラメータを、該歩容生成装置100から補償全床反力モーメント分配器116に出力するようにしてもよい。そして、該分配器116において、床反力モーメント許容範囲パラメータを基に、逐次(該分配器116の制御周期で)、床反力モーメント許容範囲の瞬時値を決定するようにしてもよい。
以上が補償全床反力モーメント分配器116の処理の詳細である。
以上の補償全床反力モーメント分配器116の演算処理により、ロール方向(X軸まわり)及びピッチ方向(Y軸まわり)の各成分毎に、Mdmd+Mmdl(Mdmdx+Mmdlx又はMdmdy+Mmdly)が、床反力モーメント許容範囲内に収まっている場合には、Mdmd+Mmdlがそのままコンプライアンス制御用目標床反力モーメントとして決定されると共に、Mmdlが全モデル操作床反力モーメントとして決定される。
また、ロール方向(X軸まわり)及びピッチ方向(Y軸まわり)の各成分毎に、Mdmd+Mmdlが、床反力モーメント許容範囲から逸脱している場合には、床反力モーメント許容範囲の上限値及び下限値のうちのMdmd+Mmdlに近い方の境界値が、コンプライアンス制御用目標床反力モーメントとして決定されると共に、床反力モーメント許容範囲からのMdmd+Mmdlの逸脱分(=Mdmd+Mmdl−床反力モーメント許容範囲の境界値)の符号を反転させてなるモーメントと、Mmdlとの和が全モデル操作床反力モーメントとして決定される。
従って、結果的には、次式70のように、ロール方向(X軸まわり)及びピッチ方向(Y軸まわり)の各成分毎に、コンプライアンス制御用床反力モーメントと全モデル操作床反力モーメントの差が、モデル状態補償全床反力モーメントに依存することなく、実姿勢補償全床反力モーメントMdmdに一致するように、該コンプライアンス制御用床反力モーメントと全モデル操作床反力モーメントとが決定される。
コンプライアンス制御用目標床反力モーメント
−モデル操作用床反力モーメント=Mdmd ……式70
換言すれば、コンプライアンス制御用床反力モーメントと全モデル操作床反力モーメントの差が、上体姿勢角偏差Δθを“0”に収束させる機能を有するように、コンプライアンス制御用床反力モーメントと全モデル操作床反力モーメントとが決定される。
そして、この場合、床反力モーメント許容範囲がMdmdに対して余裕を有する場合(該許容範囲内にMdmdが収まる場合)には、コンプライアンス制御用目標床反力モーメントが床反力モーメント許容範囲に収まる範囲内において、Mmdlの全部又は一部がコンプライアンス制御用床反力モーメントと全モデル操作用床反力モーメントとに、それぞれの組成成分として、付加的に包含されることとなる。
次に、前記全モデル操作床反力モーメント分配器118の処理を説明する。
歩容生成装置100における目標歩容の生成処理においては、今回歩容の目標運動の軌道は、将来において、定常歩容の運動軌道に収束するように(漸近するように)決定される。そして、このような目標運動を生成するために、今回歩容のZMP軌道パラメータが適宜修正される。さらに、修正後のZMP軌道パラメータにより規定される目標ZMPの瞬時値が、所要の許容範囲から逸脱するような場合には、今回歩容の遊脚足平22の着地予定位置姿勢や次回歩容の遊脚足平22の着地予定位置姿勢が適宜修正される。
歩容生成装置100では、このような処理を、各制御周期で実行する必要があるため、該歩容生成装置100の制御周期(演算処理周期)Δtは、ある程度余裕を持たせた長めの時間に設定せざるを得ない。
一方、例えばロボット1を比較的高速の移動速度で移動させるような場合には、それに伴い、ロボット1の上体姿勢角偏差Δθの高速な変動を生じやすくなる。ひいては、上体姿勢角偏差Δθには、歩容生成装置100の制御周期Δtよりも短い周期の高周波成分が含まれ易くなる。そして、上体姿勢角偏差Δθにこのような高周波成分が含まれるような場合には、歩容生成装置100による歩容生成処理(本実施形態では図8のS032の処理)に当該高周波成分を反映させることは困難である。ひいては、該高周波成分を含む上体姿勢角偏差Δθを円滑に“0”に収束させ得るような目標歩容の瞬時値を歩容生成装置100で逐次生成することが困難となる。
そこで、本実施形態では、図8のブロック図に示した制御ユニット26の機能のうち、摂動モデル演算部120等、歩容生成装置100以外の機能部分の処理を歩容生成装置100よりも短い制御周期(演算処理周期)で実行する。そして、上体姿勢角偏差Δθの、比較的緩やかな変動分(低周波成分)を“0”に収束させ得る目標歩容の運動の瞬時値の時系列を歩容生成装置100の歩容生成処理により決定すると共に、Δθの比較的素早い変動分(高周波成分)を“0”に収束させるための運動修正分(歩容生成装置100が生成する目標運動の修正量)として、前記上体水平位置摂動量ΔXpを摂動動力学モデルを用いて決定する。
上記のように歩容生成装置100の歩容生成処理と、上体水平位置摂動量ΔXpを決定する処理とを行なうようにするために、本実施形態では、全モデル操作床反力モーメント分配器118は、フィードバック操作量演算部110の補償全床反力モーメント分配器116から逐次入力される全モデル操作床反力モーメントを低周波成分と高周波成分とに分離する。そして、フィードバック操作量演算部110は、これらの低周波成分と高周波成分とのうち、低周波成分を歩容生成装置100の動力学モデル(図7に示した動力学モデル)にフィードバックする第1モデル操作床反力モーメントとして出力し、高周波成分を前記摂動モデル演算部120の摂動動力学モデル(図22に示した摂動動力学モデルにフィードバックする第2モデル操作床反力モーメントとして出力する。
全モデル操作床反力モーメント分配器118の上記の処理は、より具体的には、図24のブロック図に示す処理によって実現される。
すなわち、全モデル操作床反力モーメント分配器118は、全モデル操作床反力モーメントをローパスフィルタ(ハイカットフィルタ)118aに入力し、このローパスフィルタ118aの出力(すなわち、全モデル操作床反力モーメントの低周波成分)を、第1モデル操作床反力モーメントして決定する。なお、ローパスフィルタ118aの周波数特性は、歩容生成装置100の制御周期Δtよりも長い周期の低周波成分を通過させるように設定されている。
さらに、全モデル操作床反力モーメント分配器118は、演算部118bにおいて、全モデル操作床反力モーメントからローパスフィルタ118aの出力を差し引き、この演算部119bの演算結果の値(すなわち、全モデル操作床反力モーメントから低周波成分を除去した高周波成分)を第2モデル操作床反力モーメントとして出力する。
以上が、全モデル操作床反力モーメント分配器118の処理の詳細である。
以上のように、全モデル操作床反力モーメント分配器118の処理を実行することにより、第1モデル操作床反力モーメントは、全モデル操作床反力モーメントの低周波成分及び高周波成分のうちの高周波成分よりも低周波成分の変化に対してより高感度に変化するフィードバック操作量として決定される。また、第2モデル操作床反力モーメントは、全モデル操作床反力モーメントの低周波成分及び高周波成分のうちの低周波成分よりも高周波成分の変化に対してより高感度に変化するフィードバック操作量として決定される。
補足すると、全モデル操作床反力モーメントを、ハイパスフィルタ(ローカットフィルタ)に入力し、このハイパスフィルタの出力を第2モデル操作床反力モーメントして得るようにしてもよい。そして、この場合、ローパスフィルタ118aを省略し、該ハイパスフィルタの出力を全モデル操作床反力モーメントから差し引いたもの(すなわち、全モデル操作床反力モーメントから高周波成分を除去した低周波成分)を第1モデル操作床反力モーメントとして得るようにしてもよい。
また、第1モデル操作床反力モーメントと第2モデル操作床反力モーメントとは、それぞれ、全モデル操作床反力モーメントの高周波成分の一部、低周波成分の一部を含んでいてもよい。すなわち、第1モデル操作床反力モーメントは、第2モデル操作床反力モーメントよりも少ない割合いで、全モデル操作床反力モーメントの高周波成分の一部を含んでいてもよい。同様に、第2モデル操作床反力モーメントは、第1モデル操作床反力モーメントよりも少ない割合いで、全モデル操作床反力モーメントの低周波成分の一部を含んでいてもよい。例えば、全モデル操作床反力モーメントを低周波成分と高周波成分とに分離した後、それらの低周波成分と高周波成分とを、それぞれに掛かる重み係数を互いに異ならせて線形結合する(換言すれば、低周波成分と高周波成分との重み付き平均を算出する)ことによって、第1モデル操作床反力モーメント又は第2モデル操作床反力モーメントを決定するようにしてもよい。
以上説明した本実施形態によれば、歩容生成装置100が逐次生成・出力する目標歩容の運動のうちの上体位置姿勢を前記修正目標上体位置姿勢に置き換えてなる目標運動(以降、修正目標運動という)は、ロボット1の動力学モデル(図7の歩容生成用の動力学モデル及び図22の摂動動力学モデル)上において、トータルで、目標ZMPまわりに全モデル操作床反力モーメントが付加的に発生するように生成されつつ、実際のロボッ1の床反力には、全モデル操作床反力モーメントが付加されないように制御される。したがって、差し引き、全モデル操作床反力モーメントだけ、修正目標運動と床反力との間の動力学的な不平衡(アンバランス)を生じる。このことは、前記上体姿勢角偏差Δθを“0”に収束させる効果に関して言えば、全モデル操作床反力モーメントの符号を反転した床反力モーメントを実ロボット1に対して目標ZMPまわりに作用させることと等価である。
つまり、全モデル操作床反力モーメントを適宜決定することにより、実ロボット1の上体24の実際の姿勢を目標姿勢(修正目標上体位置姿勢のうちの上体姿勢)に収束させ、ひいては、実ロボット1の全体の姿勢を安定化させることができる。
この場合、全モデル操作床反力モーメントの符号を反転したモーメントとコンプライアンス制御用目標床反力モーメントとの和が上体姿勢角偏差Δθを0に収束させるトータルの復元力(付加的外力)になる。すなわち、コンプライアンス制御用目標床反力モーメントとモデル操作床反力モーメントとの差がトータルの復元力になる。
なお、全モデル操作床反力モーメントは、ZMP存在可能範囲を無視していかなる値でも取ることが可能であるので、非常に高い姿勢復元力を発生することができる。
また、全モデル操作床反力モーメントは、低周波成分と高周波成分とに分離される。そして、低周波成分が第1モデル操作床反力モーメントとして、歩容生成装置100の歩容生成用の動力学モデルにフィードバックされる一方、高周波成分が第2モデル操作床反力モーメントとして、摂動モデル演算部120が上体位置姿勢の修正量(上体水平位置摂動量)を生成するために用いる摂動動力学モデルにフィードバックされる。
この場合、該歩容生成装置100にフィードバックされる第1モデル操作床反力モーメントが、低周波域のものであるため、歩容生成装置100の制御周期を比較的長めの周期に設定しても、該第1モデル操作床反力モーメントを適切に歩容生成に反映させて目標歩容を生成できる。すなわち、比較的緩やかな変化を呈する上体姿勢角偏差Δθに対して、該上体姿勢角偏差Δθを円滑に“0”に近づけ得る目標歩容の運動を歩容生成装置100により生成できる。
また、歩容生成装置100の制御周期を長めの周期に設定できることから、定常歩容パラメータを探索的に決定する等、比較的演算負荷の大きい処理を1制御周期内に実行できる。このため、ロボット1の運動の長期的な安定性(継続性)を高い信頼性で確保し得る目標運動を歩容生成装置100により決定できる。
なお、本実施形態では、歩容生成装置100での歩容生成用の動力学モデルは、比較的単純な動力学モデル(倒立振子モデル)を使用したが、歩容生成装置100の制御周期を長めの周期に設定できることから、より高い動力学的精度を確保し得る、より複雑な動力学モデル(非線形性の高い動力学モデル)を使用することもできる。
一方、摂動モデル演算部120にフィードバックされる第2モデル操作床反力モーメントは、高周波域のものであるものの、該摂動モデル演算部120は、歩容生成装置100が出力する目標歩容の運動(基本骨格となる目標運動)に付加する部分的な修正分(本実施形態では、上体水平位置摂動量)だけを算出し得るものであればよい。このため、摂動モデル演算部120の動力学演算では、線形性の高い単純な摂動動力学モデルを使用することができる。ひいては、摂動モデル演算部120や、これに対する入力を生成する機能部分(全モデル操作床反力モーメント分配器118等)の制御周期(演算処理周期)を比較的短い周期に設定しても、その制御周期内で支障なく、摂動モデル演算部120の処理を実行できる。
従って、摂動モデル演算部120にフィードバックされる第2モデル操作床反力モーメントは、高周波域のものであっても、該第2モデル操作床反力モーメントを適切に摂動モデル演算部120の摂動動力学モデルに反映させて、目標歩容の運動の修正分たる上体水平位置摂動量を生成できる。すなわち、比較的素早い変化を呈する上体姿勢角偏差Δθに対して、該上体姿勢角偏差Δθを円滑に“0”に近づけ得る目標歩容の運動の修正量(上体水平位置摂動量を)を摂動モデル演算部120により生成できる。
そして、本実施形態では、上記の如く歩容生成装置100が生成する目標歩容の運動と、摂動モデル演算部120が生成する上体水平位置摂動量により修正することによって、前記修正目標運動が決定され、基本的には、この修正目標運動に実ロボット1の運動が追従するように実ロボット1の動作が制御される。
この結果、上体姿勢角偏差Δθを“0”に近づける機能を有する全モデル操作床反力モーメントの周波数成分に依存することなく、該上体姿勢角偏差Δθを円滑に“0”に近づけることができるように修正目標運動を逐次決定しつつ、実ロボット1の動作を制御できる。
ここで、以上説明した本実施形態と、本発明との対応関係について補足しておく。
本実施形態では、歩容生成装置100の処理(図8のフローチャートに示す処理)によって、本発明における第1運動決定手段が実現される。この場合、歩容生成装置100が逐次出力する目標歩容の運動(目標足平位置姿勢、目標上体位置姿勢、及び目標腕姿勢)、すなわち、図8のS032で決定する目標歩容の運動の瞬時値が、本発明における第1運動の瞬時値に相当する。また、図7に示した動力学モデル(本実施形態では倒立振子モデル)が、本発明における第1動力学モデルに相当する。また、図21のS1010の処理で使用する第1モデル操作床反力モーメントが本発明における第1モデル外力操作量に相当すると共に、図7に示した動力学モデル上でのロボットに対して目標ZMPまわりに第1モデル操作床反力モーメントを発生させることが、該ロボットに付加的外力を作用させることに相当する。
また、歩容生成装置100が各制御周期で実行するS020〜S031が、本発明における脚体運動運動パラメータ及びZMP軌道パラメータを可変的に決定する手段が実現される。そして、この場合、本実施形態における今回歩容パラメータのうちの前記足平軌道パラメータとZMP軌道パラメータとが、それぞれ、本発明における脚体運動運動パラメータ、ZM軌道パラメータに相当する。
また、摂動モデル演算部120の処理によって、本発明における第2運動決定手段が実現される。この場合、図22に示した摂動動力学モデル(本実施形態では倒立振子モデル)が本発明における第2動力学モデルに相当する。また、摂動モデル演算部120が逐次出力する上体水平位置摂動量が、本発明における第2運動の瞬時値に相当する。また、前記第2モデル操作床反力モーメントが、本発明における第2モデル外力操作量に相当する。そして、図22に示した摂動動力学モデル上のロボットに対して、目標ZMPまわりに第2モデル操作床反力モーメントを発生させることが、該ロボットに付加的外力を作用させることに相当する。
また、前記目標上体位置姿勢修正部122が、本発明における目標運動決定手段に相当し、歩容生成装置100が出力する目標歩容の運動のうち、目標上体位置姿勢を修正目標上体位置姿勢に置き換えた運動である前記修正目標運動(すなわち、目標足平位置姿勢、修正目標上体位置姿勢、目標腕姿勢の組)が本発明における目標運動に相当する。
また、複合コンプライアンス動作決定部104、ロボット幾何学モデル102、及び関節変位コントローラ108の処理によって、本発明における動作制御手段が実現される。この動作制御手段は、複合コンプライアンス動作決定部104の処理によって、コンプライアンス機能を有する。
また、前記フィードバック操作量演算部110の処理によって、本発明におけるフィードバック操作量決定手段が実現される。この場合、上体姿勢角偏差Δθが、本発明における姿勢状態量偏差に相当する。また、全モデル操作用床反力モーメントが本発明におけるモデル用基本操作量に相当し、コンプラインアンス制御用目標床反力モーメントが、実ロボット外力操作量に相当する。そして、実際のロボット1において、目標ZMPまわりに、コンプライアンス制御用目標床反力モーメントを発生させることが、該ロボット1に付加的外力を作用させることに相当する。
また、前記全モデル操作床反力モーメント分配器118の処理によって、本発明におけるモデル外力操作量決定手段が実現される。
[第2実施形態]
次に、本発明の第2実施形態を図25を参照して説明する。なお、本実施形態は、制御ユニット26が実行する処理の一部のみが、第1実施形態と相違するものである。このため、本実施形態の説明では、第1実施形態と同等の構成部分又は機能部分については、第1実施形態と同一の参照符号を用い、詳細な説明を省略する。
前記第1実施形態で説明した歩容生成装置100は、前記全モデル操作床反力モーメント分配器118から出力される第1モデル操作床反力モーメントの時系列のうち、歩容生成装置100の制御周期(演算処理周期)Δtでサンプリングした瞬時値を使用して、今回歩容の瞬時値を決定する処理(図8のS032の処理)を実行することとなる。このため、歩容生成装置100の連続する2つの制御周期の間の期間内での第1モデル操作床反力モーメントの変化分は、歩容生成装置100が今回歩容の瞬時値を決定する処理には反映されないこととなる。
そこで、本実施形態では、歩容生成装置100の連続する2つの制御周期の間の期間内での第1モデル操作床反力モーメントの変化分を、摂動モデル演算部120の摂動動力学モデルに反映させる。
具体的には、本実施形態では、制御ユニット26は、第1実施形態で説明した全モデル操作床反力モーメント分配器118の代わりに、図25のブロックで示す処理を実行する全モデル操作床反力モーメント分配器119を備える。そして、この全モデル操作床反力モーメント分配器119には、前記フィードバック操作量演算部110から全モデル操作床反力モーメントが入力されることに加えて、歩容生成装置100が図8のS032の処理で今回歩容の瞬時値を決定するために既に使用した第1モデル操作床反力モーメントの時系列のうちの最新値(以下、反映済第1モデル操作床反力モーメント最新値という)が歩容生成装置100から入力される。
より詳しくは、歩容生成装置100は、各制御周期において、出力する今回歩容の瞬時値を更新するまでは、現時点で出力している今回歩容の瞬時値を決定するために使用した第1モデル操作床反力モーメントの時系列のうちの最新値を上記反映済第1モデル操作床反力モーメント最新値として記憶保持している。そして、歩容生成装置100は、その記憶保持している反映済第1モデル操作床反力モーメント最新値を全モデル操作床反力モーメント分配器118に出力する。従って、反映済第1モデル操作床反力モーメント最新値は、歩容生成装置100が出力する今回歩容の瞬時値を更新する都度、それに合わせて、更新されることとなる。
なお、歩容生成装置100とは別の記憶手段で、歩容生成装置100の制御周期と同期した周期で反映済第1モデル操作床反力モーメント最新値を記憶保持しておき、該記憶手段から全床反力モーメント分配器119に、反映済第1モデル操作床反力モーメント最新値を入力するようにしてもよい。
そして、本実施形態では、全モデル操作床反力モーメント分配器119は、歩容生成装置100にフィードバックする第1モデル操作床反力モーメントを、第1実施形態と同じ処理によって決定する。すなわち、全モデル操作床反力モーメント分配器119は、全モデル操作床反力モーメントをローパスフィルタ119a(ハイカットフィルタ)に入力し、このローパスフィルタ119aの出力を第1モデル操作床反力モーメントとして決定する。なお、ローパスフィルタ119aの周波数通過特性は、第1実施形態におけるローパスフィルタ118aと同じでよい。
一方、全モデル操作床反力モーメント分配器119は、摂動モデル演算部120にフィードバックする第2モデル操作床反力モーメントを、第1実施形態と異なる態様で決定する。すなわち、全モデル操作床反力モーメント分配器118は、入力された全モデル操作床反力モーメントから、入力された反映済第1モデル操作床反力モーメント最新値を差し引く処理を演算部119bで実行し、この演算結果の値を、第2モデル操作床反力モーメントとして決定する。
以上が、本実施形態における全モデル操作床反力モーメント分配器119の処理である。
補足すると、以上説明した全モデル操作床反力モーメント分配器119によって第2モデル操作床反力モーメントを決定する処理は、次式72の如く、全モデル操作床反力モーメントからローパスフィルタ119aの出力を差し引いたもの(すなわち全モデル操作床反力モーメントの高周波成分)に、該ローパスフィルタ119aの出力と反映済第1モデル操作床反力モーメント最新値との差を加え合わせることによって、第2モデル操作床反力モーメントを決定する処理と同等である。
第2モデル操作床反力モーメント
=(全モデル操作床反力モーメント−ローパスフィルタの出力)
+(ローパスフィルタの出力−反映済第1モデル操作床反力モーメント最新値)
……式72
従って、本実施形態では、全モデル操作床反力モーメントの高周波成分(これは第1実施形態における全モデル操作床反力分配器118が決定する第2モデル操作床反力モーメントに相当する)に、歩容生成装置100の連続する2つの制御周期の間の期間内での第1モデル操作床反力モーメントの変化分(詳しくは、現在時刻での全モデル操作床反力モーメントの低周波成分の値と、前記反映済第1モデル操作床反力モーメント最新値との差分)を重畳したものが、第2モデル操作床反力モーメントして決定されることとなる。
本実施形態は、以上説明した事項以外は、第1実施形態と同じである。
補足すると、全モデル操作床反力モーメントをハイパスフィルタ(ローカットフィルタ)に入力し、このハイパスフィルタの出力(高周波成分)から、反映済第1モデル操作床反力モーメント最新値を差し引くことによって、第2モデル操作床反力モーメントを決定するようにしてもよい。そして、この場合、ローパスフィルタ119aを省略し、全モデル操作床反力モーメントから、ハイパスフィルタの出力を差し引くことで、第1モデル操作床反力モーメントを決定するようにしてもよい。
以上説明した本実施形態によれば、歩容生成装置100の連続する2つの制御周期の間の期間内での第1モデル操作床反力モーメントの変化分を、第2モデル操作床反力モーメントに重畳させて摂動モデル演算部120にフィードバックすることができる。このため、第1モデル操作床反力モーメントのうち、歩容生成装置100の歩容生成処理に反映されない部分となる上記変化分を、摂動モデル演算部120による上体水平位置摂動量(歩容生成装置100が出力する目標歩容の運動の修正分)に反映させることができる。
この結果、全モデル操作床反力モーメントの周波数成分に依存することなく、上体姿勢角偏差Δθをより一層円滑に“0”に近づけることができるように修正目標運動を逐次決定しつつ、実ロボット1の動作を制御できる。
なお、本実施形態と、本発明との対応関係は、第1実施形態の場合と同様である。
[第3実施形態]
次に、本発明の第3実施形態を図26〜図28を参照して説明する。なお、本実施形態は、制御ユニット26が実行する処理の一部のみが、第1実施形態と相違するものである。従って、本実施形態の説明では、第1実施形態と同等の構成部分又は機能部分については、第1実施形態と同一の参照符号を用い、詳細な説明を省略する。
前記第1実施形態では、全モデル操作床反力モーメントの高周波成分としての第2モデル操作床反力モーメントを、倒立振子型の摂動動力学モデル(図22の動力学モデル)にフィードバックし、ロボット1の上体位置姿勢のうちの上体水平位置の摂動によって、該摂動動力学モデル上で第2モデル操作床反力モーメントを発生させるように、目標上体位置姿勢の修正量としての上体水平位置摂動量を摂動モデル演算部120で算出するようにした。
これに対して、本実施形態では、上体水平位置の摂動と上体姿勢角の摂動とを合成した摂動運動によって、第2モデル操作床反力モーメントを摂動動力学モデル上で発生させるように、目標上体位置姿勢の修正量としての上体位置姿勢摂動量(詳しくは、上体水平位置摂動量と上体姿勢摂動量との組)を決定する。この場合、本実施形態では、上体位置姿勢摂動量を組成する上体水平位置摂動量と上体姿勢摂動量とは、各別の摂動動力学モデルを用いて決定される。そして、この上体位置姿勢摂動量により歩容生成装置100が生成する目標上体位置姿勢を修正することによって、修正目標上体位置姿勢(瞬時値)を逐次算出する。
本実施形態の制御ユニット26は、上記の如く上体位置姿勢摂動量を算出するために、図26に示す機能的構成を備えている。なお、図26では、便宜上、本実施形態における制御ユニット26の主要な機能部分のみを図示している。そして、図26では、第1実施形態に関して図3に示した実ロボット1、ロボット幾何学モデル102、複合コンプライアンス動作決定部104、目標床反力分配器106、及び関節変位コントローラ108の図示を省略している。これらの省略した要素は、第1実施形態と同じである。
まず、概略的な説明をすると、本実施形態では、制御ユニット26は、第1実施形態で説明した摂動モデル演算部120の代わりに、上記上体位置姿勢摂動量のうちの上体水平位置摂動量と上体姿勢摂動量とをそれぞれ、各別の摂動動力学モデルを用いて逐次算出する上体水平位置調整用摂動モデル演算部124、上体姿勢調整用摂動モデル演算部126とを備える。
さらに、制御ユニット26は、第1実施形態におけるフィードバック操作量演算部110、全モデル操作床反力モーメント分配器118、及び上体位置姿勢修正部122の代わりに、フィードバック操作量演算部128、全モデル操作床反力モーメント分配器130、及び上体位置姿勢修正部132を備える。
そして、本実施形態では、フィードバック操作量演算部128は、上体姿勢角偏差Δθ(ロボット1の実上体姿勢角の実測値と修正目標上体姿勢角との偏差)を“0”に収束させる機能と、上体水平位置調整用摂動モデル演算部124及び上体姿勢調整用摂動モデル演算部126でそれぞれ使用する後述の摂動動力学モデルの状態量(摂動モデル状態量)とそれぞれに対応する目標値との偏差であるモデル状態量偏差を“0”に収束させる機能とを有するフィードバック操作量として、全モデル操作床反力モーメント及びコンプライアンス制御用目標床反力モーメントを決定する。
この場合、本実施形態での全モデル操作床反力モーメントは、歩容生成装置100の歩容生成用の動力学モデルと、上体水平位置調整用摂動モデル演算部124及び上体姿勢調整用摂動モデル演算部126のそれぞれの摂動動力学モデルとの全体に対するフィードバック操作量である。
なお、コンプライアンス制御用目標床反力モーメントは、第1実施形態と同様に、実ロボット1に対するフィードバック操作量である。
また、本実施形態では、全モデル操作床反力モーメント分配器130は、フィードバック操作量演算部128から入力される全モデル操作床反力モーメントのうち、歩容生成装置100にフィードバックする操作量成分としての第1モデル操作床反力モーメントと、上体水平位置調整用摂動モデル演算部124にフィードバックする操作量成分としての第2aモデル操作床反力モーメントと、上体姿勢調整用摂動モデル演算部126にフィードバックする操作量成分としての第2bモデル操作床反力モーメントとを逐次決定する。
この場合、第1モデル操作床反力モーメントは、第1実施形態と同様に、全モデル操作床反力モーメントの低周波成分である。また、第2aモデル操作床力モーメントと第2bモデル操作床反力モーメントとは、全モデル操作床反力モーメントから第1モデル操作床反力モーメントを除去した高周波成分から分離させたものである。
そして、上体水平位置調整用摂動モデル演算部124は、全モデル操作床反力モーメント分配器130から入力される第2aモデル操作床反力モーメントの時系列を用いて、上体水平位置摂動量(瞬時値)を逐次算出する。また、上体姿勢調整摂動モデル演算部126は、全モデル操作床反力モーメント分配器130から入力される第2aモデル操作床反力モーメントの時系列を用いて、上体姿勢摂動量(瞬時値)を逐次算出する。
これらの上体水平位置摂動量及び上体姿勢摂動量と、歩容生成装置100から出力される目標上体位置姿勢とが上体位置姿勢修正部132に逐次入力される。この上体位置姿勢修正部132は、歩容生成装置100から入力された目標上体位置姿勢を、上体水平位置調整用摂動モデル演算部124及び上体姿勢調整用摂動モデル演算部126からそれぞれ入力された上体水平位置摂動量と上体姿勢摂動量とによって修正することにより、修正目標上体位置姿勢を算出する。
より詳しくは、上体位置姿勢修正部132は、歩容生成装置100から入力された目標上体位置姿勢のうちの上体水平位置に、上体水平位置摂動量を合成する(加え合わせる)ことによって、修正目標上体位置姿勢の上体水平位置を決定する。また、上体位置姿勢修正部122は、歩容生成装置100から入力された目標上体位置姿勢のうちの上体姿勢角に、上体姿勢摂動量を合成する(加え合わせる)ことによって、修正目標上体位置姿勢の上体姿勢角を決定する。なお、修正目標上体位置姿勢のうちの上体鉛直位置は、歩容生成装置100から入力される目標上体位置姿勢の上体鉛直位置に一致させる。
本実施形態は、上記上体水平位置調整用摂動モデル演算部124、上体姿勢調整用摂動モデル演算部126、フィードバック操作量演算部128、全モデル操作床反力モーメント分配器130、及び上体位置姿勢修正部132の処理以外の事項は、第1実施形態と同じである。
なお、第1実施形態と同様に、制御ユニット26の歩容生成装置100以外の機能部分の処理は、歩容生成装置100よりも短い制御周期(演算処理周期)で実行される。
以下に、本実施形態における上体水平位置調整用摂動モデル演算部124、上体姿勢調整用摂動モデル演算部126、フィードバック操作量演算部128、及び全モデル操作床反力モーメント分配器130が実行する処理をより詳細に説明する。
まず、上体水平位置調整用摂動モデル演算部124及び上体姿勢調整用摂動モデル演算部126の処理に関し、上体水平位置調整用摂動モデル演算部124の処理に用いる摂動動力学モデルは、第1実施形態における摂動モデル演算部120の処理に用いた摂動動力学モデル(図22に示した倒立振子モデルであり、その動力学が前記式60により表現されるモデル)と同じである。
そして、上体水平位置調整用摂動モデル演算部124は、全モデル操作床反力モーメント分配器118が後述するように逐次決定する第2aモデル床反力操作モーメントの時系列を前記式60のΔMpの時系列として用い、第1実施形態における摂動モデル演算部120と同じ演算処理によって、式60の動力学的関係を満足するΔXpを、上体位置摂動量として逐次算出する。
一方、上体姿勢調整用摂動モデル演算部126で用いる摂動動力学モデルは、ロボット1の上体姿勢角の摂動運動と目標ZMPまわりの床反力モーメントの摂動との関係を表現する動力学モデルである。その摂動動力学モデルは、本実施形態では、図27に示すフライホイールの挙動として表現されるモデルである。
すなわち、この摂動動力学モデルでは、ロボット1の上体姿勢角の摂動量(より詳しくは、X軸まわり及びY軸まわりの各方向毎の摂動量)が、イナーシャ(慣性モーメント)を有するフライホイールFHの回転角度Δθfhの摂動量に相当するものとされると共に、該フライホイールFHの回転角度Δθfhの摂動によって発生する摂動モーメントΔMfhが、目標ZMPまわりの床反力モーメントの水平成分の摂動量に相当するものとされる。そして、ΔθfhとΔMfhとの間の関係が次式74により表現される。
ΔMfh=J*(d2Δθfh/dt2) ……式74
ここで、Jは、フライホイールFHのイナーシャの設定値である。なお、式74のΔMfh及びΔθfhは、それぞれ、詳しくは、X軸まわり成分とY軸まわり成分とからなるベクトル量である。この場合、X軸まわり成分とY軸まわり成分とのそれぞれに対応すイナーシャJの設定値は互いに異なる値に設定されていてもよい。
そして、上体姿勢調整用摂動モデル演算部126は、式74のΔθfhが、出力すべき前記上体姿勢摂動量に相当し、且つ、前記全モデル操作床反力モーメント分配器118から入力される第2bモデル床反力操作モーメントが式74のΔMfhに相当するものとして、式74を用いて上体姿勢摂動量を決定する。
具体的には、上体姿勢調整用摂動モデル演算部126は、逐次入力される第2bモデル床反力操作モーメントの時系列をΔMfhの時系列として用い、式74の動力学的関係を満足するΔθfhを逐次算出する。そして、上体姿勢調整用摂動モデル演算部126は、算出したΔθfhを上体姿勢摂動量として上体位置姿勢修正部122に出力する。なお、Δθfhは、図8のS016の判断結果が肯定的となるタイミング(前記した歩容の切替わり目)において、換言すれば、歩容生成装置100による新たな今回歩容の生成開始タイミングで“0”に初期化される。
以上が、上体水平位置調整用摂動モデル演算部124及び上体姿勢調整用摂動モデル演算部126の処理の詳細である。
次に、フィードバック操作量演算部128を説明する。このフィードバック操作量演算部128は、モデル安定化制御演算部134、姿勢安定化制御演算部112及び補償全床反力モーメント分配器116を備える。この場合、姿勢安定化制御演算部112及び補償全床反力モーメント分配器116が実行する処理は、第1実施形態で説明したものと同じである。従って、これらの処理の説明は省略する。
一方、モデル安定化制御演算部134が実行する処理は、第1実施形態のものと相違する。具体的には、このモデル安定化制御演算部134には、上体水平位置調整用摂動モデル演算部124及び上体姿勢調整用摂動モデル演算部126のそれぞれの摂動動力学モデルの状態量(摂動モデル状態量)が入力される。この場合、上体水平位置調整用摂動モデル演算部124から入力される摂動モデル状態量は、該摂動モデル演算部124が逐次算出する上体水平位置摂動量ΔXpそのものである。また、上体姿勢調整用摂動モデル演算部126から入力される摂動モデル状態量は、該摂動モデル演算部126が逐次算出する上体姿勢摂動量Δθfhそのものである。
そして、モデル安定化制御演算部134は、入力される摂動モデル状態量ΔXpとそれに対する所定の目標値との偏差(モデル状態量偏差)を“0”に収束させる機能と、入力される摂動モデル状態量Δθfhとそれに対する所定の目標値との偏差(モデル状態量偏差)を“0”に収束させる機能とを有するフィードバック操作量としてのモデル状態補償全床反力モーメントを所定のフィードバック制御則により算出する。この場合、摂動モデル状態量ΔXp,Δθfhのそれぞれに対する目標値は、いずれも“0”である。従って、摂動モデル状態量ΔXp,Δθfhそのものが、それぞれ、上記モデル状態量偏差となる。
モデル安定化制御演算部134は、具体的には、次のようにモデル状態補償全床反力モーメントを算出する。すなわち、モデル安定化制御演算部134は、入力されたモデル状態量ΔXp,Δθfh(=モデル状態量偏差)のそれぞれの時間的変化率ΔVp(=dΔXp/dt),Δωfh(=dΔθfh/dt)を算出する。そして、モデル安定化制御演算部134は、例えば、フィードバック制御則としてのPD則の演算処理によって、ΔXpを“0”に収束させる機能を有する第1操作量成分Mmdl1と、Δθfhを“0”に収束させる機能を有する第2操作量成分Mmdl2とをそれぞれ、モデル状態補償全床反力モーメントMmdlの組成成分として算出する。なお、上記第1操作量成分Mmdl1及び第2操作量成分Mmdl2は、床反力モーメントの次元での操作量成分であり、それぞれ、ピッチ方向(Y軸まわり方向)の成分と、ロール方向(X軸まわり方向)の成分とから成る。
より具体的には、モデル安定化制御演算部134は、入力されたモデル状態量偏差ΔXpとその時間的変化率ΔVpとから、次式76によりMmdl1を算出する。また、モデル安定化制御演算部134は、入力されたモデル状態量偏差Δθfhとその時間的変化率Δωfhとから、次式78によりMmdl2を算出する。
Mmdl1=Ka*ΔXp+Kb*ΔVp ……式76
Mmdl2=Kc*Δθfh+Kd*Δωfh ……式78
上記式76におけるKa、Kbは、それぞれ比例ゲイン、微分ゲイン、式78におけるKc,Kdは、それぞれ比例ゲインである。これらのKa、Kb、Kc、Kdの値は、本実施形態では、あらかじめ設定された所定値である。
この場合、より詳しくは、Mmdl1のY軸まわりの成分Mmdl1yは、ΔXp、ΔVpのX軸方向成分に応じて式76により算出される。また、Mmdl1のX軸まわりの成分Mdmdxは、ΔXp、ΔVpのY軸方向成分に応じて式76により算出される。また、Mmdl2のY軸まわり方向の成分Mmdl2yは、Δθfh、ΔωfhのY軸まわり方向の成分に応じて式78により算出される。また、Mmdl2のX軸まわり方向の成分Mdmdxは、Δθfh、ΔωfhのX軸まわり方向の成分に応じて式78により算出される。
補足すると、式76の右辺は、第1実施形態でモデル状態補償全床反力モーメントMmdlをPD則により算出する式(前記式66)の右辺と同じである。また、第1実施形態で、前記式66に関して説明した場合と同様に、式76におけるKa,Kbの値を、ロボット1の運動状態(例えば、ロボット1の鉛直方向の慣性力等)に応じて可変的に設定するようにしてもよい。同様に、式78におけるKc,Kdの値を、ロボット1の運動状態に応じて可変的に設定するようにしてもよい。
本実施形態におけるモデル安定化制御演算部134は、さらに、上記の如く算出した第1操作量成分Mmdl1及び第2操作量成分Mmdl2を合成する(加え合わせる)ことによって、モデル状態補償全床反力モーメントを算出する。すなわち、モデル安定化制御演算部134は、次式80により、モデル状態補償全床反力モーメントを算出する。
モデル状態補償全床反力モーメント=Mmdl1+Mmdl2 ……式80
以上が、モデル安定化制御演算部134の処理の詳細である。この処理によって、上体水平位置調整用摂動モデル演算部124の摂動モデル状態量ΔXpと上体姿勢調整用摂動モデル演算部126の摂動モデル状態量Δθfhとを“0”に近づける機能を有するフィードバック操作量としてのモデル状態補償全床反力モーメントが算出される。
本実施形態では、フィードバック操作量演算部128は、上記の如くモデル安定化制御演算部134により算出したモデル状態補償全床反力モーメントと、姿勢安定化制御演算部112により前記した通り算出される実姿勢補償全床反力モーメントとから、第1実施形態で説明した補償全床反力モーメント分配器116の処理(図23のブロック図に示した処理)によって、前記全モデル操作床反力モーメントとコンプライアンス制御用目標床反力モーメントとが逐次決定される。
次に、全モデル操作床反力モーメント分配器130を説明する。この全モデル操作床反力モーメント分配器130は、図28のブロック図に示す処理を実行することによって、第1モデル操作床反力モーメント、第2aモデル操作床反力モーメント及び第2bモデル操作床反力モーメントを逐次決定する。
この場合、第1モデル操作床反力モーメントは、第1実施形態と同じ処理によって決定される。すなわち、全モデル操作床反力モーメント分配器130は、フィードバック操作量演算部128から入力される全モデル操作床反力モーメントをローパスフィルタ130a(ハイカットフィルタ)に入力し、このローパスフィルタ130aの出力を第1モデル操作床反力モーメントとして決定する。なお、ローパスフィルタ130aの周波数通過特性は、第1実施形態におけるローパスフィルタ118aと同じでよい。
また、全モデル操作床反力モーメント分配器130は、入力された全モデル操作床反力モーメントからローパスフィルタ130aの出力から差し引く処理を演算部130bで実行することによって、全モデル操作床反力モーメントの高周波成分(これは第1実施形態における第2モデル操作床反力モーメントに相当する)を得る。
そして、全モデル操作床反力モーメント分配器130は、演算部130bの出力(高周波成分)を、ローパスフィルタ130c(ハイカットフィルタ)に入力し、このローパスフィルタ130cの出力を、第2aモデル操作床反力モーメント及び第2bモデル操作床反力モーメントのいずれか一方、例えば、第2aモデル操作床反力モーメントとして決定する。
ここで、上記ローパスフィルタ130cは、ローパスフィルタ130aよりもより高周波側の周波数成分まで通過可能な周波数通過特性(高周波側のカットオフ周波数が、ローパスフィルタ130aよりも高いものとなる周波数通過特性)を有するフィルタである。
そして、全モデル操作床反力モーメント分配器130は、演算部130bの出力から、ローパスフィルタ130cの出力(すなわち、第2aモデル操作床反力モーメント)を差し引く処理を演算部130dで実行し、この演算部130dの出力を、第2bモデル操作床反力モーメントとして決定する。
従って、本実施形態では、第1モデル操作床反力モーメントの周波数帯域よりも第2aモデル操作床反力モーメントの周波数帯域が高く、且つ、第2aモデル操作床反力モーメントの周波数帯域よりも第2bモデル操作床反力モーメントの周波数帯域が高くなるように、これらの床反力モーメントが決定される。
以上が、本実施形態における全モデル操作床反力モーメント分配器130の処理である。
補足すると、全モデル操作床反力モーメント分配器130の処理に関し、例えば、全モデル操作床反力モーメントをバンドパスフィルタに入力し、該バンドパスフィルタの出力を第2aモデル操作床反力モーメントとして得るようにしてもよい。また、全モデル操作床反力モーメント(又は演算部130bの出力)をハイパスフィルタ(ローカットフィルタ)に入力し、このハイパスフィルタの出力を第2bモデル操作床反力モーメントとして得るようにしてもよい。
また、第1モデル操作床反力モーメント、第2aモデル操作床反力モーメント及び第2bモデル操作床反力モーメントの生成順序を、図28のブロック図で例示した場合と異ならせてもよい。例えば、全モデル操作床反力モーメントから、ハイパスフィルタやバンドパスフィルタ等を用いて第2aモデル操作床反力モーメント及び第2bモデル操作床反力モーメントを生成した後に、これらの第2aモデル操作床反力モーメント及び第2bモデル操作床反力モーメントを、全モデル操作床反力モーメントから差し引くことによって、第1モデル操作床反力モーメントを決定するようにしてもよい。
また、例えば、全モデル操作床反力モーメントから前記第2実施形態で説明した反映済第1モデル操作床反力モーメント最新値を差し引いたもの(すなわち、第2実施形態における第2モデル操作床反力モーメント)を、図28の演算部130bの出力の代わりに用いて、第2aモデル操作床反力モーメント及び第2bモデル操作床反力モーメントを生成するようにしてもよい。すなわち、第2実施形態で説明した如く生成した第2モデル操作床波力モーメントの低周波側の成分、高周波側の成分とをそれぞれ、第2aモデル操作床反力モーメント、第2bモデル操作床反力モーメントとして決定するようにしてもよい。
以上説明した本実施形態によれば全モデル操作床反力モーメントから低周波成分(第1モデル操作床反力モーメント)を除いた高周波成分が、さらに低周波側の第2aモデル操作床反力モーメントと高周波側の第2bモデル操作床反力モーメントとに2分割される。そして、これらの第2aモデル操作床反力モーメント及び高周波側の第2bモデル操作床反力モーメントが各々、上体位置調整用摂動モデル演算部124と上体姿勢調整用摂動モデル演算部126とにフィードバックされ、それに応じて上体水平位置摂動量と上体姿勢摂動量とが算出される。
従って、全モデル操作床反力モーメントの高周波成分が目的とする機能(上体姿勢角Δθの比較的素早い変動分を“0”に近づける機能)は、互いに異なる形態の2種類の摂動運動としての上体水平位置摂動量と上体姿勢摂動量とに分配されることとなる。
ここで、上体水平位置の摂動は、上体24の水平方向の慣性力(ひいては、ロボット1の全体重心の水平方向の慣性力)を発生させる摂動運動であるので、足平22と床面との間の間の摩擦力を必要とする。また、その水平方向の慣性力が大きくなるほど、必要となる摩擦力も増加する。
一方、上体姿勢の摂動は、ロボット1の全体重心の水平位置の変動を発生させないか、もしくは、該変動を十分に微小な変動に抑制できる。従って、上体姿勢の摂動は、足平22と床面との間の摩擦力の影響を受けないか、もしくは影響を受け難い。
従って、上記の如く全モデル操作床反力モーメントの高周波成分が目的とする機能が上体水平位置摂動量と上体姿勢摂動量とに分配される本実施形態によれば、ロボット1の足平22と床面との間の必要な摩擦力が過剰になったり、あるいは、上体姿勢の過剰な変動が発生することを抑制し得る修正目標運動(歩容生成装置100が出力する目標歩容の運動を、上体水平位置摂動量と上体姿勢摂動量とにより修正してなる運動)を生成することができる。
ここで、本実施形態と本発明との対応関係について補足すると、上体水平位置調整用摂動モデル演算部124の処理と上体姿勢調整用摂動モデル演算部126の処理とによって、本発明における第2運動決定手段が実現される。この場合、本発明における第2動力学モデルは、図22に示した摂動動力学モデル(本実施形態では倒立振子モデル)と図27に示した摂動動力学モデル(フライホールFHの挙動により表現されるモデル)とによって実現される。また、各摂動モデル演算部124,126がそれぞれ逐次出力する上体水平位置摂動量と上体姿勢摂動量との組が、本発明における第2運動の瞬時値に相当する。また、前記第2aモデル操作床反力モーメントと第2bモデル操作床反力モーメントとの組が、本発明における第2モデル外力操作量に相当する。そして、図22に示した摂動動力学モデル上のロボットに対して、目標ZMPまわりに第2aモデル操作床反力モーメントを発生させることと、図27に示した摂動動力学モデル上のロボットに対して、目標ZMPまわりに第2bモデル操作床反力モーメントを発生させることとが、該ロボットに付加的外力を作用させることに相当する。
本実施形態と本発明との上記以外の対応関係は、第1実施形態と同様である。
補足すると、本実施形態では、第2aモデル操作床反力モーメント及び第2bモデル操作床反力モーメントを、それぞれ、上体水平位置調整用摂動モデル演算部124、上体姿勢調整用摂動モデル演算部126にフィードバックするようにした。ただし、これと逆に、第2aモデル操作床反力モーメントを、上体姿勢調整用摂動モデル演算部126にフィードバックし、第2bモデル操作床反力モーメントを、上体水平位置調整用摂動モデル演算部124にフィードバックするようにしてもよい。
また、本実施形態では、全モデル操作床反力モーメントの高周波成分を、低周波側と高周波側とに分離したものを第2aモデル操作床反力モーメント及び第2bモデル操作床反力モーメントとして生成するようにした。ただし、例えば、単純に、全モデル操作床反力モーメントの高周波成分の全体のうちの所定割合(該高周波成分に倍率α(0<α<1)を乗じたもの)を第2aモデル操作床反力モーメントして決定し、残り(該高周波成分に倍率(1−α)を乗じたもの)を第2bモデル操作床反力モーメントとして決定するようにしてもよい。
[第4実施形態]
次に、本発明の第4実施形態を図29及び図30を参照して説明する。なお、本実施形態は、前記第3実施形態と、全モデル操作床反力モーメント分配器が実行する処理のみが相違するものである。従って、全モデル操作床反力モーメント分配器以外の構成部分又は機能部分については、第3実施形態と同一の参照符号を用い、詳細な説明を省略する。
図29を参照して、本実施形態における全モデル操作床反力モーメント分配器131は、第3実施形態と同様に、全モデル操作床反力モーメントから、第1モデル操作床反力モーメント、第1aモデル操作床反力モーメント、及び第1bモデル操作床反力モーメントを決定して出力する。
この場合、全モデル操作床反力モーメント分配器131は、第1モデル操作床反力モーメントを、第3実施形態と同じ処理によって決定する。すなわち、全モデル操作床反力モーメント分配器131は、入力される全モデル操作床反力モーメントをローパスフィルタ131a(ハイカットフィルタ)に入力し、このローパスフィルタ131aの出力を第1モデル操作床反力モーメントとして決定する。なお、ローパスフィルタ131aの周波数通過特性は、第3実施形態におけるローパスフィルタ130aと同じでよい。
また、全モデル操作床反力モーメント分配器131は、第3実施形態と同様に、入力された全モデル操作床反力モーメントからローパスフィルタ131aの出力から差し引く処理を演算部131bで実行することによって、全モデル操作床反力モーメントの高周波成分を得る。
そして、本実施形態では、この高周波成分から、第3実施形態と異なる手法によって、第2aモデル操作床反力モーメントと第2bモデル操作床反力モーメントとが生成される。
具体的には、全モデル操作床反力モーメント分配器131は、演算部131bの出力(高周波成分)を、リミッタ131cに入力し、このリミッタ131cの出力を、第2aモデル操作床反力モーメントとして決定する。
上記リミッタ131cは、第2aモデル操作床反力モーメントの許容範囲を逐次可変的に設定しつつ、その許容範囲内に制限した第2aモデル操作床反力モーメントを出力するものである。
この場合、リミッタ131cには、前記歩容生成装置100が現在時刻で生成している目標歩容(今回歩容)の瞬時値に係わる歩容の時期を示す歩容時期情報が歩容生成装置100から入力されるようになっている。該歩容時期情報は、より詳しくは、歩容生成装置100が現在時刻で生成している目標歩容の瞬時値が、いずれかの脚体2の足平22を接地させている接地期と、両脚体2,2の足平22,22が離床している空中期とのいずれの時期内の、どのタイミングでの瞬時値であるかを示す情報である。なお、このような歩容時期情報は、例えば、歩容生成装置100における時刻情報と、今回歩容パラメータのうちの床反力鉛直成分軌道パラメータにより規定される床反力鉛直成分軌道とを基に生成することができる。
そして、リミッタ13cは、この歩容時期情報を基に、例えば図30に示すようなパターンで、第2aモデル操作床反力モーメントに対する許容範囲を設定する。該許容範囲は、足平22と床面との間で摩擦力を発生することができない空中期では“0”とされ、該摩擦力を発生し得る接地期では、ある程度の幅を有するように設定される。
なお、図示の例では、許容範囲の上限値と下限値とは、台形状のパターンで変化し、接地期の開始直後は、許容範囲の幅が増加していき、接地期の終了直前は、許容範囲の幅が減少していくように該許容範囲が設定される。従って、接地期の開始直後や終了直前のように、接地している足平22と床面との間で発生可能な摩擦力が小さくなりやすい状況では、リミッタ13cが設定する許容範囲は、比較的狭い範囲となるように設定される。
リミッタ13cは、上記の如く許容範囲を逐次設定しつつ、演算部131cからの入力値(高周波成分)を該許容範囲と比較する。そして、リミッタ13cは、演算部131cからの入力値が、設定した許容範囲内に存する場合には、その入力値をそのまま第2aモデル操作床反力モーメントとして出力する。また、演算部131cからの入力値が、設定した許容範囲から逸脱している場合には、リミッタ13cは、該許容範囲の上限値及び下限値のうち、入力値の近い方の境界値を第2aモデル操作床反力モーメントとして出力する。
また、全モデル操作床反力モーメント分配器131は、演算部131bの出力(高周波成分)からリミッタ131cの出力(すなわち、第2bモデル操作床反力モーメント)を差し引く処理を演算部131dで実行し、この演算部131dの出力を第2bモデル操作床反力モーメントとして決定する。
以上のようにして、演算部131bの出力、すなわち全モデル操作床反力モーメントの高周波成分が、リミッタ131cで設定された許容範囲内に存する場合、換言すれば、ロボット1の足平22と床面との摩擦力によって、上体水平位置を摂動させ(上体24の水平加速度を発生させる)、それにより目標ZMPまわりに該高周波成分の床反力モーメントを付加的に発生させることができる場合には、該高周波成分がそのまま第2aモデル操作床反力モーメントとして決定されると共に、第2bモデル操作床反力モーメントは“0”とされる。
また、全モデル操作床反力モーメントの高周波成分が、リミッタ131cで設定された許容範囲を逸脱している場合には、該高周波成分のうち、上体水平位置の摂動によって目標ZMPまわりに付加的に作用させる得る床反力モーメントだけが、第2aモデル操作床反力モーメントとして決定される。そして、全モデル操作床反力モーメントの高周波成分の残部が、第2bモデル操作床反力モーメントとして決定される。
この場合、特に、空中期では、リミッタ131cで設定される許容範囲が“0”となるので、全モデル操作床反力モーメントの高周波成分の全体が第2bモデル操作床反力モーメントとして決定されると共に、第2aモデル操作床反力モーメントは、“0”に設定される。
以上が本実施形態における全モデル操作床反力モーメント分配器131が実行する処理の詳細である。
以上説明した本実施形態によれば、ロボット1と床面との間で摩擦力を発生できない空中期では全モデル操作床反力モーメントの高周波成分の全体が第2bモデル操作床反力モーメントとして決定されると共に、第2aモデル操作床反力モーメントは、“0”に設定される。また、接地期では、その開始直後や終了直前(接地している足平22と床面との間で発生可能な摩擦力が小さい状況)を除いて、リミッタ131cで設定される許容範囲が比較的広いものとなるので、一般的には、全モデル操作床反力モーメントの高周波成分の多くの部分が、第2aモデル操作床反力モーメントに分配され、残りの部分が第2bモデル操作床反力モーメントに分配される。
従って、全モデル操作床反力モーメントの高周波成分が目的とする機能(上体姿勢角Δθの比較的素早い変動分を“0”に近づける機能)は、ロボット1の足平22と床面との間の摩擦力を確保し得る接地期においては、主として、上体水平位置摂動量による目標歩容の運動の修正によって実現される。また、当該機能は、ロボット1と床面との間の摩擦力を発生できない空中期では、上体姿勢摂動量による目標歩容の運動の修正によって実現される。
この結果、接地期及び空中期のいずれにおいても、上体姿勢角Δθの比較的素早い変動分を適切に“0”に近づけ得るような修正目標運動を生成することができる。
なお、本実施形態と、本発明との対応関係は、第3実施形態の場合と同様である。
補足すると、以上説明した第4実施形態又は前記第3実施形態では、上体水平位置の調整用の摂動動力学モデルと上体姿勢の調整用の摂動動力学モデルとを別々に構成したが、それらの摂動動力学モデルを統合させた単一の摂動動力学モデル(上体水平位置の摂動と上体姿勢の摂動とが相互に影響し合うような摂動動力学モデル)を構成することも可能である。そして、この摂動動力学モデルに、全モデル操作床反力モーメントの高周波成分(より具体的には、例えば第1実施形態又は第2実施形態における第2モデル操作床反力モーメント)を入力することで、上体水平位置摂動量と上体姿勢摂動量との組を生成するようにしてもよい。
[第5実施形態]
次に、本発明の第5実施形態を図31及び図32を参照して説明する。なお、本実施形態は、制御ユニット26が実行する処理の一部のみが、第1実施形態と相違するものである。従って、本実施形態の説明では、第1実施形態と同等の構成部分又は機能部分については、第1実施形態と同一の参照符号を用い、詳細な説明を省略する。
図31を参照して、本実施形態では、制御ユニット26は、第1モデル操作床反力モーメント予測器140をさらに備え、この第1モデル操作床反力モーメント予測器140に、全モデル操作床反力モーメント分配器118から第1モデル操作床反力モーメントが逐次入力される。
なお、図31では、便宜上、本実施形態における制御ユニット26の主要な機能部分のみを図示しており、第1実施形態に関して図3に示した実ロボット1、ロボット幾何学モデル102、複合コンプライアンス動作決定部104、目標床反力分配器106、関節変位コントローラ108、フィードバック操作量演算部110、摂動モデル演算部120、及び上体位置姿勢修正部122の図示を省略している。
また、本実施形態では、第1モデル操作床反力モーメント予測器140の演算処理周期(制御周期)は、制御ユニット26の歩容生成装置100以外の機能部分(全モデル操作床反力モーメント分配器118等)の演算処理周期と同じである。ただし、第1モデル操作床反力モーメント予測器140の演算処理周期(制御周期)は、歩容生成装置100の制御周期と同じであってもよい。
上記第1モデル操作床反力モーメント予測器140は、現在時刻以後に全モデル操作床反力モーメント分配器118が出力する第1モデル操作床反力モーメントの時系列を予測し、その予測した時系列(以降、モデル操作予測床反力モーメント時系列という)を生成して出力するものである。
そして、本実施形態では、この第1モデル操作床反力モーメント予測器140の出力が歩容生成装置100に入力され、該歩容生成装置100の歩容生成処理(詳しくは、図8のS028の中の処理)に用いられる。なお、第1モデル操作床反力モーメント予測器140の出力(モデル操作予測床反力モーメント時系列)には、現在時刻において、全モデル操作床反力モーメント分配器118から第1モデル操作床反力モーメント予測器140に入力されている第1モデル操作床反力モーメントの値も含まれる。
以降、上記第1モデル操作床反力モーメント予測器140の処理と、この予測器140の出力を使用した歩容生成装置100の処理とを説明する。
第1モデル操作床反力モーメント予測器140は、本実施形態では、現在時刻以後の第1モデル操作床反力モーメントが、例えば、現在時刻での値から、所定の時定数Taの一次遅れ特性で減衰していくものと見なして、モデル操作予測床反力モーメント時系列を生成する。
具体的には、第1モデル操作床反力モーメント予測器140は、波形整形器140aと、伝達関数が1/(1+Ta・s)により表現される一次遅れのフィルタ140b(時定数がTaである一次遅れフィルタ)とを備え、現在時刻での第1モデル操作床反力モーメントの値から波形整形器140aにより、フィルタ140bに入力する波形Sinを生成する。該波形Sinは、図中に示すように、現在時刻以後の値が、現在時刻での値からステップ状に“0”に変化するステップ入力波形である。
そして、第1モデル操作床反力モーメント予測器140は、上記波形Sinを波形整形器140aからフィルタ140bに入力する。このとき、フィルタ140bの出力の波形Soutは、図中に示すように、現在時刻での第1モデル操作床反力モーメントの値から一次遅れ特性で減衰していく波形となり、この波形データがモデル操作予測床反力モーメント時系列として生成される。
以上が、第1モデル操作床反力モーメント予測器140の処理の詳細である。
なお、本実施形態では、上記の如く、現在時刻以後の第1モデル操作床反力モーメントが、単純に現在時刻での値から一次遅れ特性で減衰していくものとして、現在時刻以後の第1モデル操作床反力モーメントの時系列を予測するようにした。但し、例えば、現在時刻以前の第1モデル操作床反力モーメントの時系列の履歴に基づいて、現在時刻以後の第1モデル操作床反力モーメントの時系列を予測するようにしてもよい。
上記の如く生成されるモデル操作予測床反力モーメント時系列は、歩容生成装置100において、次のように使用される。
すなわち、本実施形態では、歩容生成装置100は、各制御周期において実行するS028の処理の中で、モデル操作予測床反力モーメント時系列を使用する。より詳しくは、S028の処理の中で実行する図17のS702の処理(仮今回歩容を生成する処理)において、モデル操作予測床反力モーメント時系列が使用される。
具体的には、本実施形態では、歩容生成等100は、図17のS702において、図18に示した処理の代わりに、図32に示す処理を実行することによって、仮今回歩容を生成する。
この処理においては、S800〜S804を経て、S806〜S818の処理が繰り返され、仮今回歩容の瞬時値の時系列が生成される。この場合、S800〜S804の処理、並びに、S806〜S812の処理は、前記第1実施形態と同じである。
そして、本実施形態では、歩容生成装置100は、S806〜S818の処理の繰り返しループにおいて、S812の次に、S813の処理を実行する。このS813では、歩容生成装置100は、第1モデル操作床反力モーメント予測器140から現時点(歩容生成装置100の現在の制御周期)で入力されているモデル操作予測床反力モーメント時系列から、時刻Tkでの第1モデル操作床反力モーメントの予測値(入力されているモデル操作予測床反力モーメント時系列の初期時刻から、Tkの時間が経過した時刻での第1モデル操作床反力モーメントの予測値)を求める。
次いで、S814に進んで、歩容生成装置100は、S813で求めた第1モデル操作床反力モーメントの予測値が目標ZMPまわりに発生するように(ロボット1の慣性力と重力との合力が目標ZMPまわりに発生するモーメントの水平成分が第1モデル操作床反力モーメントの予測値に釣り合うように)、時刻Tkでの上体水平加速度を決定する。このS814での上体水平加速度の算出の仕方は、第1実施形態で説明した図21のS1010と同じである。
次いで、歩容生成装置100は、S814〜S818まで、第1実施形態と同じ処理を実行する。
本実施形態では、以上のS806〜S818の処理の繰り返しによって、仮今回歩容が初期から終端まで生成される。この場合、S813、S814の処理が実行されることで、仮今回歩容の初期から終端までの各時刻において、モデル操作予測床反力モーメント時系列により示される第1モデル操作床反力モーメントの予測値が、歩容生成装置100にフィードバックされるものと仮定して、仮今回歩容が生成されることとなる。
本実施形態は、以上説明した事項以外は、第1実施形態と同じである。なお、本実施形態では、図8のS032で今回歩容瞬時値を決定する時に用いる第1モデル操作反力モーメントの値(瞬時値)は、前記モデル操作予測床反力モーメント時系列の初期時刻での値、すなわち、現在時刻でモデル操作床反力モーメント分配器118が出力している第1モデル操作床反力モーメントの値である。
以上説明した本実施形態によれば、歩容生成装置100の各制御周期において、現在時刻以後の将来において、歩容生成装置100に逐次フィードバックされる第1モデル操作床反力モーメント(モデル操作床反力モーメント分配器118が出力する第1モデル操作床反力モーメント)の時系列が、その予測値としての前記モデル操作予測床反力モーメント時系列に一致すると仮定して仮今回歩容が生成される。そして、この仮今回歩容の終端発散成分が、定常歩容の初期発散成分に一致するように目標ZMP軌道パラメータが適宜修正され、該目標ZMP軌道パラメータを含む今回歩容パラメータが決定される。
従って、今回歩容パラメータは、歩容生成装置100にフィードバックされる第1モデル操作床反力モーメントに関する上記の仮定の基で、今回歩容が定常歩容につながるように(将来の歩容を定常歩容に収束させるように)決定されることとなる。
この結果、歩容生成装置100が実際に出力する今回歩容の瞬時値の時系列(図18のS032で決定する瞬時値の時系列)が、決定された今回歩容パラメータが目標とする今回歩容の瞬時値の時系列(好適な時系列)から乖離するのを防止し、それらの時系列の一致性を高めることができる。ひいては、ロボット1の運動の継続性を確保し得る歩容の目標運動を、より一層高い信頼性で生成することができる。
ここで、以上説明した本実施形態と、本発明との対応関係について補足すると、第1モデル操作床反力モーメント予測器140の処理によって、本発明におけるモデル外力操作量予測手段が実現される。この場合、前記モデル操作予測床反力モーメント時系列が、本発明における第1モデル外力操作量の将来予測値の時系列に相当する。
また、歩容生成装置100が実行する図8のS022の処理によって、本発明における将来目標決定手段が実現される。この場合、前記定常歩容の運動が将来の到達目標(収束目標)としての規範運動に相当し、該定常歩容の歩容パラメータが、規範パラメータに相当する。
また、今回歩容パラメータのうちの足平軌道パラメータとZMP軌道パラメータとが、本発明における演算用パラメータに相当する。
本実施形態と本発明との上記以外の対応関係は、第1実施形態と同様である。
補足すると、本実施形態では、第1モデル操作床反力モーメント予測器140が前記モデル操作予測床反力モーメント時系列を生成する処理を、歩容生成装置100とは別の処理としたが、歩容生成装置100の処理の中(例えば図8のS018〜S028の間)で、モデル操作予測床反力モーメント時系列を生成するようにしてもよい。
[変形態様について]
次に、以上説明した実施形態の変形態様いくつか説明しておく。
以上説明した各実施形態では、ロボット1の走行を行なう目標歩容を例にとって説明したが、ロボット1の歩行を行う目標歩容を生成するようにしてもよい。この場合には、例えば、床反力鉛直成分軌道(1歩分の軌道)として、例えば図33に示すような折れ線状のパターンで設定した軌道を用いて、前記各実施形態と同様の手法によって目標歩容を生成すればよい。この図33の例では、床反力鉛直成分軌道は、両脚支持期では、床反力鉛直成分の増加側に凸(上に凸)の台形状に設定され、片脚支持期では床反力鉛直成分の減少側に凸(下に凸)の台形状に設定される。
ただし、ロボット1の歩行を行う目標歩容における床反力鉛直成分軌道は、床反力鉛直成分の値がほぼ一定に保たれるような軌道であってもよい。
また、前記各実施形態では、説明を判り易くするために、歩容生成装置100は図7に示した比較的単純な動力学モデルを用いて目標歩容を生成するようにしたが、より動力学的精度の高い複雑な動力学モデルを使用して目標歩容を生成するようにしてもよい。例えば、本願出願人が、例えば特許第4246638号の実施形態にて提示したような動力学モデル(フライホイールと複数の質点とを備える動力学モデル)を用いて目標歩容を生成するようにしてもよい。あるいはロボット1の各リンクに質点を備えるような多質点モデルを使用して目標歩容を生成するようにしてもよい。
また、これらの動力学モデルを用いて目標歩容を生成する場合に、ロボット1の足平22と床面との間の摩擦力を制限する許容範囲を歩容の時期や床状態などに応じて可変的に設定し、ロボット1の慣性力に応じて発生する当該摩擦力を許容範囲内に収めることを必要要件として、目標歩容を生成するようにしてもよい。このような目標歩容の生成手法としては、例えば、上記特許第4246638号の実施形態にて本願出願人が提案した手法を採用することができる。
また、前記各実施形態では、本発明におけるモデル用基本操作量、第1モデル外力操作量、第2モデル外力操作量、実ロボット外力操作量として、目標ZMPまわりの床反力モーメントの次元の操作量を用いたが、例えば目標ZMPの修正量等、上体姿勢角偏差Δθを操作し得る他の種類の操作量を用いてもよい。
また、前記各実施形態では、歩容生成装置100の歩容生成処理において、周期的な運動である定常歩容の運動を将来の到達目標(収束目標)として、ZMP軌道パラメータ等の今回歩容パラメータを決定するようにしたが、定常歩容の運動以外の到達目標を設定するようにしてもよい。例えば、今回歩容の遊脚足平22の着地予定時刻におけるロボット1の重心の目標位置と目標速度とを設定し、これらの目標位置及び目標速度を将来の到達目標として、歩容生成装置100による歩容生成処理(今回歩容の生成処理)を行なうようにしてもよい。この場合、今回歩容の遊脚足平22の着地予定時刻におけるロボット1の重心の目標位置及び目標速度の設定例としては、例えば、次のような例が挙げられる。すなわち、遊脚足平22の着地予定時刻におけるロボット1の重心の水平方向の目標位置を、該予定時刻における遊脚足平22の予定位置と、支持脚足平22の予定位置との間の中間位置に位置に設定する。また、今回歩容の初期時刻から遊脚足平22の着地予定時刻までの期間内でのロボット1の重心の変化量を該期間の時間で除算してなる平均速度に、ある係数を乗じてなる値を、遊脚足平22の着地予定時刻におけるロボット1の重心の目標速度として設定する。
このように歩容生成装置100で生成する歩容の運動に関する将来の到達目標としては、定常歩容の運動以外の目標を設定することもできる。