JP4392037B2 - 脚式ロボット、及びその制御方法 - Google Patents

Description

本発明は脚式ロボット、及びその制御方法に関する。

脚式歩行ロボットに関する歩行軌道を生成する技術が数多く提案されている。歩行軌道の生成技術としては、ロボットの重心位置軌道を生成する技術がよく知られている（例えば特許文献１、２など）。特許文献１には、ＺＭＰ方程式で算出されるＺＭＰが目標ＺＭＰに一致する重心軌道を算出する技術が開示されている。尚、本明細書で「軌道」とは、位置の経時的な変化を記述するデータのことを言う。
特開２００４−１６７６７６号公報 特開２００６−２１２７３６号公報

一般的には、ロボットの重心軌道を生成する際には、ロボットが実現する重心まわりの回転運動の角運動量の軌道を必要とする。しかしながら、従来技術は、ロボットの重心軌道を求めることに関して主眼を置くものであり、ロボットが転倒しない条件を満足しながら、妥当な角運動量を設定する方法については考慮がなされていないものであった。従来技術では、例えば、ロボットの角運動量を０であるものと仮定する、又は角運動量を外乱として扱うものであった。角運動量を０とした場合には、当該角運動量が保存されるものとして軌道生成を行うものであった。

ロボットの転倒は角運動量の時間変化率によっても引き起こされるため、運動中の角運動量を０と仮定して重心軌道を生成した場合には、実際には、ロボットの上体が回転した不自然な軌道となってしまう。

２足歩行ロボットにおいて、角運動量を例えば０としてその場足踏み動作を行った場合には、図５（ｂ）に示すように、重心が左に動いた場合には、角運動量を０として保存するため、上体が右に回転するものとなる。即ち、ロボットは上体の動きに対して角運動量を０に維持しようとするため、上体が大きく回転して姿勢が崩れてしまう。また、走行動作においても、前方へ一歩踏み出すごとに上体が後方へと回転して、ロボットの姿勢が反ったものとなっていた。

このような動作は見た目にも不自然であるばかりでなく、ロボットの股関節などに対して大きなトルクを発生させ、ロボットに対して過大な負荷をかけるなどの問題がある。また、人間が足踏み動作を行う際には、人間は姿勢を維持したまま上体を回転させずに足踏み動作を実現可能である。このことからも、同様に自然な姿勢でロボットが動作を実現するためには、角運動量の軌道を０とするのではなく、何らかの妥当な値を設定する必要があるものと考えられる。

このように、角運動量を０として扱う従来の動作においては、ロボットの上体が不自然な動きになるという問題点があった。

本発明は、かかる課題を解決するためになされたものであり、脚式ロボットの上体が自然な動きとなる軌道を生成することができる脚式ロボット、及びその制御方法を提供することを目的とする。

本発明に係る脚式ロボットは、関節角を変更して動作する脚式ロボットであって、ＺＭＰ方程式を離散化した３項方程式と、目標ＺＭＰとに基づいて、重心軌道を生成する手段と、生成された前記重心軌道に基づいて、前記関節角の目標値の経時的データを計算する手段と、計算された前記関節角の目標値の経時的データに基づいて、前記関節を回転する手段とを備え、前記ＺＭＰ方程式が、重心速度に応じた角運動量を含むものである。

これにより、角運動量を０とせずに重心の移動速度に応じたものとする構成にしたことで、例えば重心が左に動いた場合には、重心の移動に応じて上体も左に回転する。このため、生成された軌道に基づくロボットの動作が不自然な動作となることを防ぐことができる。尚、本明細書では、単位時間で離散化した３つの時刻位置座標からＺＭＰを計算する式を「ＺＭＰ方程式を離散化した３項方程式」という。この３項方程式を利用すると、目標ＺＭＰと一致するＺＭＰを実現する重心軌道の計算が可能となる。

また、前記角運動量を、前記脚式ロボットの質量と、前記角運動量に関する比例定数ｈと、重心速度とを用いて規定し、前記比例定数ｈを前記脚式ロボットの姿勢変化に基づいて計算するようにしてもよい。このように、角運動量を重心速度に比例する項を用いて規定し、ロボットの実際の動作における姿勢変化に応じて比例定数を計算することで、構成の異なる様々なロボットに対してもより自然な動作となる比例定数を容易に計算することができる。

さらにまた、前記重心軌道を生成する手段は、鉛直方向軌道及び前記角運動量に基づいて計算される係数を持つ行列と、重心軌道の列と、目標ＺＭＰの列との間に成立する連立方程式を解いて重心軌道を計算するようにしてもよい。

また、前記行列が、前記目標ＺＭＰの列における最初の要素に関する方程式が前記重心軌道の列における最後の要素を含み、かつ、前記目標ＺＭＰの列における最後の要素に関する方程式が前記重心軌道の列における最初の要素を含むように、前記鉛直方向軌道及び前記角運動量に基づいて計算される係数を持つ行列であるようにしてもよい。このような周期運動を行う条件を満足するように行列を構成することで、解を一意に決定することができる。

さらにまた、前記重心軌道を生成する手段は、鉛直方向軌道及び前記角運動量に基づいて計算される係数を持つ３重対角行列と、重心軌道の列と、目標ＺＭＰ及び動作開始時と完了時における重心の水平方向速度から計算される距離の列との間に成立する連立方程式を解いて重心軌道を計算するようにしてもよい。このように、境界条件として、動作開始時と完了時における重心の水平方向速度を採用することができる。また、３重対角行列を用いて表現される連立方程式については、少ない計算負荷で高速に求解する技術が開発されている。従って、このような構成によれば、少ない計算負荷で高速に重心軌道を計算することができる。

本発明に係る脚式ロボットの制御方法は、関節角を変更して動作する脚式ロボットの制御方法において、ＺＭＰ方程式を離散化した３項方程式と、目標ＺＭＰとに基づいて、重心軌道を生成するステップと、生成された前記重心軌道に基づいて、前記関節角の目標値の経時的データを計算するステップと、計算された前記関節角の目標値の経時的データに基づいて、前記関節を回転するステップとを備え、前記ＺＭＰ方程式が、重心速度に応じた角運動量を含むものである。

これにより、角運動量を０とせずに重心の移動速度に応じたものとする構成にしたことで、例えば重心が左に動いた場合には、重心の移動に応じて上体も左に回転する。このため、生成された軌道に基づくロボットの動作が不自然な動作となることを防ぐことができる。

本発明によれば、脚式ロボットの上体が自然な動きとなる軌道を生成することができる脚式ロボット、及びその制御方法を提供することを目的とする。

発明の実施の形態１．
本実施の形態１にかかる脚式ロボットは、まず、ＺＭＰ方程式を離散化した３項方程式と、目標ＺＭＰとに基づいて、重心軌道を生成する。そして、生成された重心軌道に基づいて、関節角の目標値の経時的データを計算し、その計算された関節角の目標値の経時的データに基づいて、関節を回転する。このようにして、脚式ロボットは関節角を変更して動作する。ここで、ＺＭＰ方程式は、重心速度に応じた角運動量を含むことを特徴とするものである。

以下、図面を参照しながら本実施の形態１に係るロボットの軌道計算方法について説明する。図１は、本実施の形態１に係る脚式ロボットの概要を示す図である。ロボット２は、体幹４と、左脚リンク６と、右脚リンク８と、制御部１０と、コントローラ１２を備えている。左脚リンク６は一方の端部は股関節を介して体幹４に揺動可能に接続されている。左脚リンク６はさらに膝関節と足首関節を備え、先端には足平を備えている。右脚リンク８は一方の端部は股関節を介して体幹４に揺動可能に接続されている。右脚リンク８はさらに膝関節と足首関節を備え、先端には足平を備えている。ロボット２の各関節はアクチュエータ（不図示）を備えており、それらのアクチュエータは制御部１０からの指示によって回転駆動する。左脚リンク６と右脚リンク８の足平の中心には、それぞれ基準点Ｌ_０、Ｒ_０が設けられている。基準点Ｌ_０、Ｒ_０は、ロボット２の動作パターンを生成する際の基準となる点である。図中Ｇは、ロボット２の重心位置を示す。

制御部１０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、ハードディスクなどを有するコンピュータ装置である。制御部１０はコントローラ１２と通信可能であり、ユーザーが操作するコントローラ１２から指令値を入力する。制御部１０はユーザーから入力される指令値に基づいて、ロボット２の動作パターンを生成ないし計算する。制御部１０は生成した動作パターンを記憶し、記憶された動作パターンを実現するように各関節を駆動する。制御部１０の詳細については後述する。

以下では、ロボット２の軌道を表現するための座標系として、ロボット２の外部で床に固定された座標系（ｘ，ｙ，ｚ）を用いる。また、ロボット２上の基準点に固定された座標系を（ｘ'，ｙ'，ｚ'）で表現する。本実施の形態１に係るロボット２では、図１に示すように、支持脚の足平の中心を座標系（ｘ'，ｙ'，ｚ'）の基準点とする。以下では、ロボット２の基準点に固定された座標系（ｘ'，ｙ'，ｚ'）で記述された位置、速度、加速度及び軌道を、それぞれ、相対位置、相対速度、相対加速度及び相対軌道と表現する。

本実施の形態１に係るロボット２は、動作開始から完了までの動作パターンを生成して記憶しており、記憶された動作パターンを逐次読み出して動作を実現していく。ロボット２の動作中にユーザーから指令値が入力されると、ロボット２はそれ以降の動作パターンを再度生成して、新たに入力された指令値に応じた動作パターンを記憶する。記憶された新たな動作パターンは、それ以降の動作に反映される。上記のようにして、本実施の形態１に係るロボット２は、リアルタイムに動作パターンを生成しながら歩行していく。

続いて、制御部１０の動作の詳細について説明する。図２は、制御部１０の機能の構成を示す機能ブロック図である。制御部１０は、ユーザー指令記憶部３０２と、重心相対軌道計画部３０４と、遊脚相対軌道計画部３０６と、関節角目標値演算部３０８と、関節駆動部３１０とを備えている。ここでは重心の軌道を計算して生成することから、計算、生成、演算の語を区別なく用いる。

ユーザー指令記憶部３０２は、ユーザーから入力されるユーザー指令値３６０を記憶する。ユーザー指令記憶部３０２は、（１）動作の開始時から完了時までの継続時間３２０、（２）動作の開始時から完了時までにおける相対ＺＭＰ軌道３２２、（３）動作の開始時及び完了時における重心の水平方向速度３２４、（４）動作の開始時及び完了時における遊脚足先の位置及び速度３２６などのデータを記憶している。これらのデータの詳細については後述する。

ロボット２の動作中に新たなユーザー指令値３６０が入力されると、制御部１０はユーザー記憶部３０２に記憶されたユーザー指令値を、新たに入力されたユーザー指令値に書き換える。制御部１０は、ユーザー指令記憶部３０２に記憶されているデータに基づいて逐次動作を計算して生成してゆくため、ユーザーから入力される新たな指令値は、それ以降のロボット２の動作の生成に反映される。ユーザーは、ロボット２の運動を視認しながら、例えばジョイスティックなどのコントローラ１２を用いてユーザー指令値を入力することで、ロボット２の動作をリアルタイムに制御することができる。

重心相対軌道計画部３０４は、ユーザー指令記憶部３０２に記憶されているユーザー指令値と、重心速度に応じた角運動量に基づいて、ロボット２の重心の軌道を生成する。ここで生成される重心の軌道は、ロボット２の外部に固定された座標系（ｘ，ｙ，ｚ）で記述されたものであるが、ロボット２が床に接地している相である接地相においてはロボット２の基準点（支持脚の足平に位置する）が床に対して移動しないため、ロボット２の基準点に固定された座標系（ｘ'，ｙ'，ｚ'）で記述されたデータとして扱うことができる。軌道生成処理の詳細については後述する。

本実施の形態１に係るロボット２による軌道生成処理では、角運動量を、ロボット２の質量と、角運動量に関する比例定数と、重心速度とを用いて規定する。角運動量に関する当該比例定数を角運動量パラメータ３７０という。

ここで、本実施の形態１に係るロボット２では、図３（ａ）に示すような、均質な広がりを持つ物質により構成され、図に示す位置に重心Ｇが位置する慣性モデルを想定する。斜線により示す範囲は、均質な広がりを持つ物質であることを意味する。このような慣性モデルでは、重心位置を左右に動かそうとする場合には、その動きに伴い角運動量が発生する。尚、従来では、図３（ｂ）に示すように、慣性を考慮しない、質点が一点に集中した一質点モデルを想定しており、角運動量を無視して質点で重心の移動を行うものであった。

図３（ａ）に示した慣性モデルでは、重心Ｇが移動するごとに、姿勢が変化して回転する。即ち、重心速度に比例して慣性が発生し、回転運動量が生成される。従って、このような慣性モデルを想定することで、回転運動量を重心速度に比例した項によって定義することができる。これは、ロボット２の上体が回転する場合には、その回転速度に比例した角運動量が発生するであろうという着想による。

本実施の形態１に係るロボット２では、ロボット２全体を大きな剛体と見なし、ロール軸（ｘ'）及びピッチ軸（ｙ'）まわりの慣性モーメント（一定値）を含む慣性モデルに基づいて重心相対軌道を生成する。尚、ヨー軸（ｚ'軸）まわりの角運動量は、ロボット２が所望の旋回動作を実現するように設定することが可能である。本実施の形態１に係るロボット２では、ヨー軸まわりの角運動量は０となるように設定する。角運動量パラメータ３７０の詳細については後述する。

重心相対軌道３３０は、ロボット２の支持脚の足先に設けられた基準点を原点とする座標系（ｘ'，ｙ'，ｚ'）で重心の軌道を記述したものである。重心相対軌道３３０は、ロボット２が動作を開始してから完了するまでの時間区間の順序を示す符号と、その時間区間における重心の相対軌道が、関連付けられている。重心相対軌道計画部３０４で、ある時間区間における重心の相対軌道が計算されるたびに、重心相対軌道３３０は更新されていく。

遊脚相対軌道計画部３０６は、重心相対軌道計画部３０４で計算されるロボット２の重心相対軌道３３０と、ユーザー指令記憶部３０２に記憶されているユーザー指令値に基づいて、ロボット２の遊脚の足先の相対軌道３４０を計算する。ここで計算される遊脚の足先の相対軌道３４０は、ロボット２の支持脚の足先に設けられた基準点を原点とする座標系（ｘ'，ｙ'，ｚ'）で記述されたものである。

遊脚相対軌道３４０は、各時間区間におけるロボット２の遊脚の足先の基準点の相対軌道を記述するデータであって、時間区間の順序を示す符号と、その時間区間における遊脚の足先の基準点の相対軌道が、関連付けられている。遊脚相対軌道計画部３０６で、ある時間区間における遊脚の相対軌道が計算されるたびに、遊脚相対軌道３４０は更新されていく。

関節角目標値演算部３０８は、重心相対軌道計画部３０４で計算されるロボット２の重心相対軌道３３０と、遊脚相対軌道計画部３０６で計算されるロボット２の遊脚の足先の基準点の相対軌道３４０に基づいて、ロボット２の各関節の関節角目標値の経時的データ３５０を演算する。

関節角目標値経時的データ３５０は、ロボット２の各関節の目標関節角の経時的データを備えている。関節駆動部３１０は、関節角目標値の経時的データ３５０に基づいて、ロボット２の各関節を駆動する。

続いて、図４を参照しながら、本実施の形態１に係るロボット２が行う処理について説明する。図４は、動作生成処理を説明するためのフローチャートである。図４の処理は、任意の歩行・走行動作を扱うことができるが、以下では例として足踏み歩行動作を扱う場合について説明する。

まず、ステップＳ１０１では、制御部１０はこれから生成する動作に関するユーザー指令値をユーザー指令記憶部３０２から読み出す。ユーザー指令値としては、以下のパラメータ（１）動作の開始時から完了時までの継続時間３２０と、（２）動作の開始時から完了時までにおける相対ＺＭＰ軌道３２２と、（３）動作の開始時及び完了時における重心の水平方向速度３２４と、（４）動作の開始時及び完了時における遊脚足先の位置及び速度３２６を与えることができる。

動作の開始時から完了時までの継続時間３２０は、ロボット２が動作を開始してから完了するまでの時間であって、ユーザーが任意に与えることができる。

動作の開始時から完了時までにおける相対ＺＭＰ軌道３２２は、目標ＺＭＰの支持脚の足平の基準点に対する位置の経時的データであって、ユーザーが任意に与えることができる。接地相においては、支持脚となる脚リンクの足平内にＺＭＰが存在していれば、ロボット２は転倒することなく動作を継続することができる。本実施の形態１に係るロボット２では、相対ＺＭＰ軌道を、支持脚の足平の中心に固定するように設定する。このような相対ＺＭＰ軌道が実現される場合、ロボット２は転倒することなく、安定して動作を実現することができる。なお相対ＺＭＰ軌道は、足平の内部に維持されていれば、どのような軌道を与えてもよい。例えば支持脚の足平の内部の後方から前方へ移動するような軌道を用いてもよい。このような相対ＺＭＰ軌道が実現される場合も、ロボット２は転倒することなく、安定して動作を実現することができる。

動作の開始時及び完了時における重心の水平方向速度３２４は、ロボット２の動作の開始時及び完了時における、ロボット２の重心が実現する水平方向速度であって、ロボット２のアクチュエータの性能が許す範囲内で、ユーザーが任意に与えることができる。

動作の開始時及び完了時における遊脚足先の位置及び速度３２６は、ロボット２の動作の開始時及び完了時における遊脚足先の支持脚足先から見た位置と速度であって、ロボット２の機構が幾何学的に許す範囲で、ユーザーが任意に与えることができる。

ステップＳ１０２では、重心の相対軌道を計算する。重心の相対軌道は、目標とする相対ＺＭＰ軌道と、角運動量パラメータと、動作の開始時及び完了時における重心の水平方向速度の条件に基づいて決定される。ステップＳ１０２では、重心の相対軌道を実現したときの実際の相対ＺＭＰ軌道が、目標とする相対ＺＭＰ軌道に一致するように、重心の相対軌道を計算する。

角運動量パラメータ３７０は、重心相対軌道計画部３０４に設定されている。角運動量パラメータ３７０は、予め所定の実験又はシミュレーションを行い、妥当な軌道を生成可能な適切な値を決定する。この実験においては、角運動量パラメータ３７０を変化させて軌道を生成し、生成された軌道に基づいてロボット２を動作させた上で、自然な動きとなる値を決定する。

また、目標とする相対ＺＭＰ軌道、及び動作の開始時及び完了時における重心の水平方向速度は、ステップＳ１０１でユーザー指令値として与えられている。ステップＳ１０２では、これらの条件を満足するように、重心の相対軌道を計算する。

ここで、重心の相対軌道の計算について説明する。ロボット２が実現する相対ＺＭＰ位置（ｑ_ｘ'，ｑ_ｙ'）は、ロボット２の相対重心位置（ｘ'，ｙ'，ｚ'）と、ロボット２の重心まわりの角運動量（ｒ_ｘ'，ｒ_ｙ'）から計算することができる。ロボット２の相対重心位置とロボットの重心まわりの角運動量から、実際に生じるＺＭＰを計算する下記の式をＺＭＰ方程式と呼ぶ。

ここで、^（１）は時間ｔに関する１階微分を示し、^（２）は時間ｔに関する２階微分を示している。また、ｍはロボット２の質量であり、ｇは重力加速度である。上式のｚ'及びｚ'^（２）は、ロボットの支持脚の足先を原点とする重心の鉛直方向位置と鉛直方向加速度である。本実施の形態１に係るロボット２の動作では、重心の高さは一定であり質点がｚ'方向に変化しないものと仮定して、ｚ'は一定値とし、ｚ'^（２）は０とする。上式のｒ_ｘ' ^（１）とｒ_ｙ' ^（１）は、重心まわりの角運動量の微分成分を示す。

従来の手法では、運動の全域にわたって角運動量（ｒ_ｘ'，ｒ_ｙ'）を０として扱い、重心の鉛直方向軌道ｚ'が与えられる場合に、上記数１を離散化して境界条件を与えることによって連立方程式として解を求めることができるものであった。さらに、重心の鉛直方向の加速度ｚ'^（２）についても０とすることで、上記数１におけるＺＭＰ方程式を解析的に解くことができるものであった。

本実施の形態１に係るロボット２においては、動作の開始から完了までの全ての時間区間において、ロール軸（x'軸）及びピッチ軸（y'軸）まわりの角運動量ｒ_ｘ'及びｒ_ｙ'を０とする代わりに、重心の速度に比例した項を用いる。即ち、下記の式により角運動量ｒ_ｘ'及びｒ_ｙ'を規定する。

ここで、ｈ_ｘ'は重心速度ｘ'^（１）に関する比例定数であり、α_ｘ'は定数である。また、ｈ_ｙ'は重心速度ｙ'^（１）に関する比例定数であり、α_ｙ'は定数である。比例定数ｈ_ｘ'及びｈ_ｙ'は角運動量パラメータである。尚、角運動量を規定するための式はこれに限定されず、例えば時間ｔの項を含む下記の式を用いてもよい。このようにして規定したｒ_ｘ'及びｒ_ｙ'は、角運動量の時間変化量を示す。β_ｘ'及びβ_ｙ'は定数である。

上記数１のＺＭＰ方程式に対して、上記数２に示した角運動量を代入することで、下記の方程式を得る。

上記数４のＺＭＰ方程式を離散化することによって、重心の水平方向位置（ｘ'、ｙ'）の経時的データを算出する式を導出することができる。ここで言う離散化は、時間ｔに関して単位時間Δｔを用いて行う。例えば重心のｘ'方向の軌道に関しては、下記の三項方程式を得る。

ここでｘ'_ｉとｑ_ｘ'ｉは、それぞれ重心のｘ'方向軌道ｘ'(ｔ)と、ｘ'方向の相対ＺＭＰ軌道ｑ_ｘ'(ｔ)を離散化した変数である。ｉは単位時間で区分した時刻の順序を示す。上記数５は、時刻ｉにおけるＺＭＰのｘ'方向座標ｑ_ｘ'ｉが、直前のｘ'方向座標ｘ'_ｉ−１と、そのときのｘ'方向座標ｘ'_ｉと、直後のｘ'方向座標ｘ'_ｉ＋１から計算できることを示している。係数ａ_ｉ，ｂ_ｉ，ｃ_ｉは、下記によって算出される係数である。

上述したように、角運動量パラメータｈは、予め所定の実験又はシミュレーションを行い、妥当な軌道を生成可能な適切な値が決定される。角運動量パラメータｈを０とした場合には、従来と同様に不自然な姿勢によってロボット２は動作する。角運動量パラメータｈを徐々に大きくしてゆくことで、ロボット２の上体は回転せずにまっすぐな姿勢を維持したまま動作を行うようになる。そして、角運動量パラメータをさらに大きくしてゆくことで、ロボット２は上体をまっすぐな姿勢に維持したまま横に振りながら動作を行う。このように、値を徐々に変更して上体のようすを観察し、最も良好な姿勢を維持できる値を決定する。即ち、ロボット２の動きを観察して、パラメータｈについての適切な値を決定する。

上記のΔｔはＺＭＰ方程式の離散化に用いる時間幅であり、ｚ'_iとｚ'^（２） _iはそれぞれ重心の鉛直方向の位置と加速度を離散化した変数である。本実施例では、Δｔは動作開始から完了までの時間をｎ等分する時間幅である。

その場足踏み動作は、ロボット２が直立の状態から、ロボット２のバランスを取りつつ、右足を一旦上げて、床面に接地させ、同じくロボット２のバランスを取りつつ、左足を一旦上げて、床面に接地させ、直立の状態に戻る動作である。言い換えると、直立の状態から、ロボット２の脚を交互に駆動させることで、重心を支持させる動作である。このような周期運動であるその場足踏み動作は、その動作を実現するための重心相対軌道を一意に決定することができる。即ち、周期運動をするという周期境界条件（ロボット２の重心位置が、動作の開始及び完了時において循環するという条件）を付け加えることにより、その場足踏みの動作を実現するための一意な重心相対軌道を得ることができる。

上記した関係を整理すると、結局のところ、実現される相対ＺＭＰ軌道が目標とする相対ＺＭＰ軌道と等しくなる、重心のｘ'方向軌道（ｘ'_１，ｘ'_２，・・・，ｘ'_ｎ）は、以下の行列で示される方程式を解くことによって算出することができる。ここで、周期境界条件の下では、下記の行列は、三重対角行列に対して周期境界条件を加えた循環対角行列により構成することができる。

上記数７において、左辺は行列と列ベクトルの積で表現されており、その行列は鉛直方向成分及び角運動量パラメータｈから計算される係数を持つ行列である。本実施の形態１に係るロボット２の足踏み動作においては、行列は、三重対角行列に周期境界条件を加えた循環対角行列である。周期境界条件とは、１行ｎ列目に係数ａ_１を、ｎ行１列目に係数ｃ_ｎをそれぞれ設定することで、ロボット２の重心位置が、動作の開始及び完了時において循環するという条件のことを言う。３重対角行列とは、対角成分とそれに隣接する副対角成分のみが有意な値を備え、それ以外の成分が０である行列のことを言う。言い換えると、周期境界条件の下では、上記数７によって示される連立方程式に関して、目標ＺＭＰの列における最初の要素に関する方程式が重心軌道の列における最後の要素を含み、かつ、目標ＺＭＰの列における最後の要素に関する方程式が重心軌道の列における最初の要素を含むように係数行列の要素が配置される。左辺の列ベクトルは、重心のｘ'方向軌道（ｘ'_１，ｘ'_２，・・・，ｘ'_ｎ）を示す列ベクトルである。右辺の列ベクトルは、目標とする相対ＺＭＰ軌道を備える列ベクトルである。

上記数７において、左辺の循環対角行列の逆行列を算出し、その逆行列と右辺との積を計算することによって、未知数（ｘ'_１，ｘ'_２，・・・，ｘ'_ｎ）を計算することができる。従って、重心のｘ'方向軌道を計算することができる。上記した計算では、試行錯誤的に重心の相対軌道を計算することなく、直接的に目標とする相対ＺＭＰ軌道を満足する重心の相対軌道を計算することができる。上記した計算は少ない計算量で実施することが可能であり、重心相対軌道計画部３０４は高速で計算を実施することができる。重心軌道に関する離散化数をｎとする場合には、計算量はＯ（ｎ）となり、非常に高速に解を求めることができる。ステップＳ１０２では、重心の相対軌道を実現したときの実際の相対ＺＭＰ軌道が、目標とする相対ＺＭＰ軌道に一致するように、重心の相対軌道を計算する。

ｙ'方向の重心軌道（ｙ'_１，ｙ'_２，・・・，ｙ'_ｎ）についても、ｙ'方向の相対ＺＭＰ位置（ｑ_ｙ'１，ｑ_ｙ'２，・・・，ｑ_ｙ'ｎ）と、角運動量パラメータｈと、動作の開始時及び完了時における重心の水平方向速度から、ｘ'方向の重心軌道（ｘ'_１，ｘ'_２，・・・，ｘ'_ｎ）と同様にして算出することができる。

ステップＳ１０２の処理を実施することによって、重心の軌道を生成することができる。上記の処理によって生成される重心の軌道を実現することで、ロボット２はＺＭＰを支持脚の足平の内部に維持しながら、その場足踏み動作を実現することができる。

ステップＳ１０３では、遊脚の足先の相対軌道を計算する。遊脚の足先の相対軌道は、足踏み動作の前後における脚リンクの動作と滑らかに接続するように計算する。

足踏み動作の開始時及び完了時おける遊脚の足先の相対位置および速度は、ユーザー指令値として与えられている。足踏み動作期間における遊脚の足先の相対軌道の計算は、例えば多項式補間を用いることによって、計算することができる。

ステップＳ１０４では、上述した処理によって計算された重心および遊脚の相対軌道を実現する、関節角の目標値を算出する。関節角の目標値はいわゆる逆キネマティクス演算によって算出することができる。ロボット２の相対並進運動量Ｐ_ｇ、ロボット２の相対角運動量Ｌ_ｇ、遊脚足先の相対速度ｖ_ｆ及び遊脚足先の角速度ω_ｆは、関節角の角速度θ^（１）と、それぞれヤコビ行列Ｊ_ｇ（θ）、Ｋ_ｇ（θ）、Ｊ_ｆ（θ）、Ｋ_ｆ（θ）を用いて、以下で表現される。

上記数８のうち関節角の角速度θ^（１）は関節角の角度θを時間に関して１階微分したものである。関節角の角度θは、以下に示すように、ロボットの関節角群（θ₁、θ₂、・・・、θ_ｎ−１）である。

ロボット２の相対並進運動量Ｐ_ｇ、ロボット２の相対角運動量Ｌ_ｇ、遊脚足先の相対速度ｖ_ｆ及び遊脚足先の角速度ω_ｆは、それぞれ以下である。

遊脚足先のヤコビ行列は、遊脚の数だけ存在する。本実施の形態１に係るロボット２は、２つの脚リンクを備えており、遊脚を１の脚リンクとして関節角の目標値を演算する。目標とするロボットの運動Ｐ_ｇ、Ｌ_ｇ、ｖ_ｆ、ω_ｆを満たす関節角速度θ^（１）は、無数の解を持つ冗長系となっている。本実施の形態１に係るロボット２では、第１サブタスクとして目標とする重心の相対速度を実現すること、第２サブタスクとして目標とする角運動量を実現すること、第３サブタスクとして目標とする遊脚足先の相対軌道を実現することを条件として、目標関節角の経時的データを演算する。上記したような複数のヤコビ行列で表される拘束条件を、それぞれの条件に優先度をつけながら、冗長度の許す限り最適な解を求める手法は、従来から提案されており、説明を省略する。

上記した処理を実施することによって、重心の軌道、目標とする角運動量、及び遊脚足先の軌道を拘束条件として、関節角の目標値を演算することができる。これによって、ロボット２の安定な動作パターンを生成することができる。このような手法をとることによって、脚だけでなく腕や胴体などのすべてのロボットの自由度を用いて動作パターンを生成することが可能となり、より自然で柔軟な動作パターンを作り出すことができる。

ステップＳ１０４で演算される関節角目標値の経時的データ３５０は、制御部１０に記憶されている。ステップＳ１０４の後、処理はステップＳ１０１へ移行し、ステップＳ１０１からステップＳ１０４までの処理を繰り返し実施する。

制御部１０は上記した図４の処理と並列して、関節角目標値の経時的データ３５０に基づいて、関節駆動部３１０でロボット２の各関節を逐次駆動していく。これによって、ロボット２はユーザー指令値に基づいた安定した動作を実現していく。

以上説明したように、本実施の形態１に係るロボット２では、角運動量を重心速度に比例する項を用いて規定し、当該角運動量に基づいて重心の相対軌道を生成する。このように、角運動量を０とせずに重心の移動速度に比例させる構成にしたことで、例えば重心が左に動いた場合には、重心の移動に比例して上体も左に回転する。このため、生成された軌道に基づくロボット２の動作が不自然な動作となることを防ぐことができる。

従って、足踏み動作時における上体の左右方向への不自然な回転を抑制し、図５（ａ）に示すように自然な動作を実現することができる。また、足踏み以外の動作についても、例えば走行動作時においては、左右方向の回転に加えて、上体の前後方向への不自然な回転をも抑制することができ、自然な姿勢で動作を行うことができる。

発明の実施の形態２．
上述した実施の形態１においては、ロボット２がその場足踏み動作を行う場合について、その動作を実現するための重心相対軌道を生成する処理について説明した。本発明はこれに限定されず、その場足踏み動作のような周期運動以外の動作であっても、速度境界条件の下で、その動作を実現するための重心相対軌道を生成することができる。尚、本実施の形態２に係るロボット２の構成や軌道生成処理は、速度境界条件の下で重心相対軌道を計算する点を除いて、上述した発明の実施の形態１と同様である。

以下では、ロボット２が歩行動作を行う場合における軌道生成処理について説明する。本実施の形態２に係るロボット２では、歩行動作の開始時及び完了時において、重心の軌道が滑らかに変化するように、ロボット２の動作を計算する。このように動作を計算することで、例えばロボット２が支持脚を入れ替える場合であっても、ロボット２の重心位置は滑らかに推移し、ロボット２は安定した動作を実現する。歩行動作の開始時の重心のｘ'方向速度ｖ_ｘ'１は、分割時間幅Δｔと、重心の位置ｘ'_１及びｘ'_２とを用いて、以下によって表現される。

同じように、歩行動作の完了時における重心のｘ'方向速度ｖ_ｘ'ｎは、分割時間幅Δｔと、重心の位置ｘ'_ｎ−１、ｘ'_ｎと、以下の関係がある。

歩行動作の開始時及び完了時における重心のｘ'方向速度ｖ_ｘ'１及びｖ_ｘ'ｎは、上記の実施の形態１に係るステップＳ１０１でユーザー指令値として設定されている。

上記した関係を整理すると、結局のところ、実現される相対ＺＭＰ軌道が目標とする相対ＺＭＰ軌道と等しくなる、歩行動作における重心のｘ'方向軌道（ｘ'_１，ｘ'_２，・・・，ｘ'_ｎ）は、以下の三重対角行列で示される方程式を解くことによって算出することができる。

上記数１３において、左辺は行列と列ベクトルの積で表現されており、その行列は鉛直方向軌道から計算される係数を持つ３重対角行列である。３重対角行列とは、対角成分とそれに隣接する副対角成分のみが有意な値を備え、それ以外の成分が０である行列のことを言う。左辺の列ベクトルは、重心のｘ'方向軌道（ｘ'_１，ｘ'_２，・・・，ｘ'_ｎ）を示す列ベクトルである。右辺の列ベクトルは、目標とする相対ＺＭＰ軌道と、開始時における重心のｘ'方向速度ｖ_ｘ'１に単位時間Δｔを乗じて距離に変換した値ｖ_ｘ'１Δｔと、終了時における重心のｘ'方向速度ｖ_ｘ'ｎに単位時間Δｔを乗じて距離に変換した値ｖ_ｘ'ｎΔｔとを備える列ベクトルである。

上記数１３において、左辺の３重対角行列の逆行列を算出し、その逆行列と右辺との積を計算することによって、未知数（ｘ'_１，ｘ'_２，・・・，ｘ'_ｎ）を計算することができる。従って、重心のｘ'方向軌道を計算することができる。上記した計算では、試行錯誤的に重心の相対軌道を計算することなく、直接的に目標とする相対ＺＭＰ軌道を満足する重心の相対軌道を計算することができる。上記した計算は少ない計算量で実施することが可能であり、重心相対軌道計画部３０４は高速で計算を実施することができる。

ｙ'方向の重心軌道（ｙ'_１，ｙ'_２，・・・，ｙ'_ｎ）についても、ｙ'方向の相対ＺＭＰ位置（ｑ_ｙ'１，ｑ_ｙ'２，・・・，ｑ_ｙ'ｎ）と、角運動量パラメータｈと、歩行動作の開始時速度ｖ_ｙ'１と完了時速度ｖ_ｙ'ｎから、ｘ'方向の重心軌道（ｘ'_１，ｘ'_２，・・・，ｘ'_ｎ）と同様にして算出することができる。

尚、上記した水平方向速度ｖ_ｘ'１，ｖ_ｙ'１の代わりに、重心が移動する方向を示す水平方向の単位ベクトルｎ_ｘ'，ｎ_ｙ'と速度絶対値|ｖ|を指令値としてユーザーが与えてもよい。このような場合、水平方向速度ｖ_ｘ'１，ｖ_ｙ'１は、以下によって算出される。

このようにして、ロボット２の歩行動作における重心の軌道を生成することができる。上記の処理によって生成される重心の軌道を実現することで、ロボット２はＺＭＰを支持脚の足平の内部に維持しながら、歩行動作を実現することができる。

その他の実施の形態．
上述した実施の形態１及び２では、角運動量パラメータｈは、予め所定の実験又はシミュレーションを行い、妥当な軌道を生成可能な適切な値を決定するものとしたが、本発明はこれに限定されない。例えば、角運動量パラメータｈの値を、ロボット２の動きに応じて自動的に計算するものとしてもよい。角運動量パラメータｈは、例えば、ジャイロセンサ（不図示）を用いてロボット２の胴体もしくは腰の姿勢角を検出し、ロボット２の姿勢変化及びロボット２の慣性モーメントに基づいて決定することができる。このようにロボット２の実際の動きに応じて決定することで、構成の異なる様々なロボット２に対してもより自然な動作となる値を容易に計算することができる。

尚、上述した実施の形態では２の脚リンクを備える脚式ロボットについて説明したが、本発明は３以上の脚リンクを備える脚式ロボットとして具現化することもできるし、１の脚リンクのみを備える脚式ロボットとして具現化することもできる。

また、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明の実施の形態に係る脚式ロボットの構成を示す図である。 本発明の実施の形態に係る脚式ロボットの制御部の構成を示す機能ブロック図である。 本発明の実施の形態１に係る脚式ロボットの慣性モデルを示す図である。 本発明の実施の形態１に係る脚式ロボットの制御部が行う処理を示すフローチャートである。 本発明の実施の形態１に係る脚式ロボットの動作の概要を示す図である。

符号の説明

２ 脚式ロボット、４ 体幹、６ 左脚リンク、８ 右脚リンク、１０ 制御部、
１２ コントローラ、
３０２ ユーザー指令記憶部、３０４ 重心相対軌道計画部、
３０６ 遊脚相対軌道計画部、３０８ 関節角目標値演算部、３１０ 関節駆動部

Claims (6)

1. 関節角を変更して動作する脚式ロボットであって、
   ＺＭＰ方程式を離散化した３項方程式と、目標ＺＭＰとに基づいて、重心軌道を生成する手段と、
   生成された前記重心軌道に基づいて、前記関節角の目標値の経時的データを計算する手段と、
   計算された前記関節角の目標値の経時的データに基づいて、前記関節を回転する手段とを備え、
   前記ＺＭＰ方程式に含まれる角運動量が、
   重心速度を変数とする一次式を用いて規定される
   ことを特徴とする脚式ロボット。
2. 前記角運動量を、前記脚式ロボットの質量と、前記角運動量に関する比例定数と、重心速度とを用いて規定し、前記比例定数を前記脚式ロボットの姿勢変化に基づいて計算する
   ことを特徴とする請求項１記載の脚式ロボット。
3. 前記重心軌道を生成する手段は、
   鉛直方向軌道及び前記角運動量に基づいて計算される係数を持つ行列と、重心軌道の列と、目標ＺＭＰの列との間に成立する連立方程式を解いて重心軌道を計算する
   ことを特徴とする請求項１又は２記載の脚式ロボット。
4. 前記行列が、
   前記目標ＺＭＰの列における最初の要素に関する方程式が前記重心軌道の列における最後の要素を含み、かつ、前記目標ＺＭＰの列における最後の要素に関する方程式が前記重心軌道の列における最初の要素を含むように、前記鉛直方向軌道及び前記角運動量に基づいて計算される係数を持つ行列である
   ことを特徴とする請求項３記載の脚式ロボット。
5. 前記重心軌道を生成する手段は、
   鉛直方向軌道及び前記角運動量に基づいて計算される係数を持つ３重対角行列と、重心軌道の列と、目標ＺＭＰ及び動作開始時と完了時における重心の水平方向速度から計算される距離の列との間に成立する連立方程式を解いて重心軌道を計算する
   ことを特徴とする請求項１乃至３いずれか１項記載の脚式ロボット。
6. 関節角を変更して動作する脚式ロボットの制御方法において、
   ＺＭＰ方程式を離散化した３項方程式と、目標ＺＭＰとに基づいて、重心軌道を生成するステップと、
   生成された前記重心軌道に基づいて、前記関節角の目標値の経時的データを計算するステップと、
   計算された前記関節角の目標値の経時的データに基づいて、前記関節を回転するステップとを備え、
   前記ＺＭＰ方程式に含まれる角運動量が、
   重心速度を変数とする一次式を用いて規定される
   ことを特徴とする脚式ロボットの制御方法。

Images