JP4848686B2 - 脚式ロボットとその動作調節方法 - Google Patents

Description

本発明は、脚式ロボットの動作を制御する技術に関する。特に、外部等からの指令に追従して、脚式ロボットの動作を調節する技術に関する。

体幹と、体幹に揺動可能に連結されている脚リンクを備え、体幹と脚リンク群の相対的姿勢（単に姿勢ということがある）を変化させることによって歩行する脚式ロボット（単にロボットと記すことがある）が開発されている。
脚式ロボットが歩行するためには、体幹と脚リンク群の相対的姿勢の経時的変化を記述するデータを必要とする。体幹と脚リンク群の相対的姿勢は、例えば各足先と体幹の位置の経時的変化によって記述することができる。このとき、体幹位置の経時的変化は、各足先位置の経時的変化に対して適当な値である必要があり、その値が適当でなければロボットは転倒してしまう。
脚式ロボットが転倒しない体幹位置の経時的変化を得るためには、ロボットのダイナミクスを考慮に入れた複雑な計算を必要とする。以下に、二足歩行ロボットの場合の一例を説明する。
（１）ロボットの左足先と右足先の位置を指示する経時的データを指定する。
（２）足先の接地位置を考慮してロボットのＺＭＰが存在しなければならない位置を指定する。ＺＭＰ（zero moment point）は、ロボットに作用する重力や床反力や慣性力の合力のモーメントがゼロになる床上の点をいう。ＺＭＰが接地脚の足平内にあればロボットは転倒しない。逆にいうと、ロボットが転倒しないためには、ＺＭＰが接地脚の足平内になければならない。そこで接地脚の足先の位置と姿勢を考慮し、下記の関係を満たす目標ＺＭＰを指定する。即ち、一方の脚リンク（例えば左脚）が遊脚になっている間は接地脚（右脚）の足平内に存在し、その一方の脚（左脚）が接地して両足接地状態になった時に新たに接地した脚（左脚）の足平内に向けて移動開始し、それまでに接地していた脚（右脚）が遊脚となる前に新たに接地した脚（左脚）の足平内に移動し終えるＺＭＰを指定する。このようにして指定されたＺＭＰの軌道は、目標ＺＭＰ軌道と呼ばれる。実際のＺＭＰ軌道が目標ＺＭＰ軌道に一致すれば、ロボットは転倒することなく歩行し続ける。
（３）足先の接地位置の変化とそれに追従して変化する目標ＺＭＰが指定されると、体幹位置の経時的変化を仮定してロボットのダイナミクスを計算する。計算する時点で、足先位置の経時的変化が指定されているために、ロボットの体幹位置の経時的変化を仮定すると、ロボットの全身の姿勢やその変化速度が決まる。ロボットの全身の姿勢が決まると、その姿勢におけるＺＭＰの位置を計算することが可能となる。ＺＭＰの位置を計算するためには、静的な要素に加えて、ロボットに作用する慣性力の影響を織り込まなければならない。仮定した体幹位置の経時的変化を計算に含めることで、ロボットのダイナミクスまで考慮してＺＭＰの位置を計算することが可能となる。体幹位置の経時的変化を仮定するとＺＭＰの位置を計算することができることから、目標ＺＭＰに一致するＺＭＰを実現する体幹の位置と姿勢の経時的変化を探求することができる。

上記によって探求された体幹位置の経時的変化を示すデータを体幹歩容データといい、もともと指定されている足先位置の経時的変化を示すデータを足先歩容データといい、両者を総称して歩容データという。歩容データに従ってロボットが歩行すれば、実際のＺＭＰが目標ＺＭＰに一致し、ロボットは転倒せずに歩行し続けることができる。
歩容データは、時間に対する位置の経時的変化で与えられる。位置と速度と加速度は関連しており、そのうちの一つの量から他の量を計算することができることから、位置の代わりに速度または加速度の経時的変化を扱ってもよい。足先や体幹の位置は、例えば体幹に定められている基準点の位置と基準方向の向きによって記述することができる。足平位置は、足平に固定されている基準点の位置と足平に固定されている基準線の方向によって特定され、体幹位置は、体幹に固定されている基準点の位置と体幹に固定されている基準線の方向によって特定される。

外部からの指令に追従して、歩行動作を逐次変更するロボットの研究が進められている。例えば非特許文献１には、外部からの指令に基づいて所定期間の歩容データを作成し、以前に作成した歩容データに、新たに作成した歩容データを順次接続していくことによって、外部からの指令に追従する一連の歩容データを作成する技術が記載されている。
西脇光一、「人間型ロボットの歩行システム構築と動作のオンライン生成制御」、東京大学工学系研究科博士課程論文、平成１３年１２月１４日

予定していた歩行動作から新たな歩行動作に変更する際には、変更時点における足先の位置と速度ならびに体幹の位置と速度の連続性を確保する必要がある。しかしながら、変更後の歩行動作の開始時点における足先の位置と速度ならびに体幹の位置と速度を、変更前の歩行動作の終了時点における足先の位置と速度ならびに体幹の位置と速度に強制的に合わせてしまうと、変更後の歩行動作の開始直後においてＺＭＰが目標ＺＭＰから大きく外れてしまうことがある。
そのことから、非特許文献１の技術では、歩行動作の変更時点を、ロボットが両脚立脚状態で安定している時間帯に限定している。あるいは、歩行動作を変更する度合を制限するようにしている。
本発明は、脚式ロボットが外部等から与えられる指令に追従して歩行動作をより自由に変更することができる技術を提供する。

本発明は、体幹に揺動可能に連結されている脚リンクを備える脚式ロボットに具現化することができる。この脚式ロボットは、予定の歩行動作を実施するときの体幹位置の時系列データを記憶している手段と、指令に基づいて予定の歩行動作からの変更時点を決定する手段と、指令に基づいて変更時点以降の目標ＺＭＰ軌道を作成する手段と、変更時点以降の体幹位置の時系列データを仮定する手段と、変更時点の直前の二以上の時点に対して記憶している体幹位置の二以上のデータに、仮定した変更時点以降の体幹位置の時系列データを接続することによって時系列データを作成し、その時系列データから、変更時点以降の仮のＺＭＰ軌道を計算する手段と、計算した仮のＺＭＰ軌道と目標ＺＭＰ軌道とのＺＭＰ偏差の二乗和が最小となるように、仮定した体幹位置の時系列データを修正する修正手段と、記憶している変更時点以降の体幹位置の時系列データを修正後の体幹位置の時系列データに更新する手段とを備えている。

この脚式ロボットは、指令に基づいて、予定の歩行動作からの変更時点を決定するとともに、変更時点以降の目標ＺＭＰ軌道を作成する。この目標ＺＭＰ軌道は、指令に対応する歩行動作を可能とするものである。ここでいう指令とは、例えばロボットの外部から与えられる指令や、ロボットの内部から与えられる指令等がある。ロボットの外部からの指令には、例えばオペレータ等がロボットに逐次与える指令や挙げられる。ロボットの内部からの指令には、例えばロボット自身が障害物の存在を検出し、それに対応する動作変更の指令を、ロボット自身が自律的に生成することが挙げられる。あるいは、オペレータ等が予め用意した一連の指令や、ロボット自身が以前の動作時に学習した情報から作成した一連の指令等を、ロボット自身が予め記憶していてもよい。
変更時点以降の体幹位置の経時的変化を仮定すると、仮のＺＭＰ軌道を計算することができる。このとき、変更時点の直前の二以上の時点に対して記憶している体幹位置の二以上のデータを併せて用いることにより、変更時点の直前時点の体幹位置とその変化速度をも加味して、仮のＺＭＰ軌道を計算する。ＺＭＰの計算にはロボットの動的挙動を加味する必要があり、変更時点の直前時点以降の体幹位置の経時的変化を計算に含めることによって、変更時点以降の仮のＺＭＰ軌道を計算することができる。
仮定した体幹位置の経時的変化から計算した仮のＺＭＰ軌道と、目標ＺＭＰ軌道との偏差に基づいて、仮定した体幹位置を修正することができる。通常は、ＺＭＰ偏差をゼロとする体幹位置の経時的変化が得られるまで体幹位置の修正作業を繰返す。しかしながら、歩行動作を予定外に変更する場合には、変更前の体幹位置から変更後の体幹位置に修正する動的動作が加わることから、変更時点から変更後にかけて、ＺＭＰ偏差をゼロとすることはできない。変更直後からのＺＭＰ偏差をゼロに維持しようとすると、変更時点でのＺＭＰ偏差が大きくなってしまう。
そのことから、本発明の脚式ロボットでは、所定期間に亘るＺＭＰ偏差の二乗和に基づいて、仮定した体幹位置を修正する。即ち、歩行動作の変更時点において生じるはずのＺＭＰ偏差を、経時的に分散させる。
検証実験によって、ＺＭＰ偏差を経時的に分散させる体幹位置に修正すると、歩行動作を比較的に大きく変更した場合でも、歩行動作の変更時点において、実際のＺＭＰが目標ＺＭＰから大きく外れることがないことが確認されている。それにより、例えば歩行動作の変更時点をより自由に設定したり、歩行動作を比較的に大きく変更したりすることが可能となる。
この脚式ロボットによると、指令に追従して、歩行動作をより自由に変更することが可能となる。

修正手段は、仮定した体幹位置の経時的変化を修正する際に、ＺＭＰの偏差の二乗和が最小となるように修正することが好ましい。
それにより、生じるはずのＺＭＰ偏差をより適切に分散することができる。

本発明は、脚式ロボットの歩行動作を調節する方法にも具現化することができる。この方法は、予定の歩行動作を実施するときの体幹位置の時系列データを脚式ロボットに記憶させる工程と、指令に基づいて予定の歩行動作からの変更時点を決定する工程と、指令に基づいて変更時点以降の目標ＺＭＰ軌道を作成する工程と、変更時点以降の体幹位置の時系列データを仮定する工程と、変更時点の直前の二以上の時点に対して脚式ロボットが記憶している体幹位置の二以上のデータに、仮定した変更時点以降の体幹位置の時系列データを接続することによって時系列データを作成し、その時系列データから、変更時点以降の仮のＺＭＰ軌道を計算する工程と、計算した仮のＺＭＰ軌道と目標ＺＭＰ軌道との偏差の二乗和が最小となるように、仮定した姿勢の時系列データを修正する工程と、脚式ロボットが記憶している変更時点以降の姿勢の時系列データを、修正後の体幹位置の時系列データに更新する工程とを備えている。
この方法によると、指令に追従して、脚式ロボットの歩行動作をより自由に変更することが可能となる。

本発明によると、予定の歩行動作から予定外の歩行動作を変更して歩行し続けるロボットの自由度が向上する。臨機応変に対応するロボットの実現に貢献するものである。

最初に、以下に説明する実施例の主要な特徴を列記する。
（形態１） ロボットは、オペレータが歩行動作の変更指令を入力する指示装置を備えている。
（形態２） ロボットは、指示装置からの指令信号に基づいて、目標とする遊脚の接地位置や接地方向や接地目標時間等を記述する歩行パターンデータを作成する。
（形態３） ロボットは、歩行パターンデータに基づいて、目標ＺＭＰ軌道を作成する。

図面を参照しながら、本発明の一実施例である脚式ロボットについて説明する。
図１は本実施例のロボット２の概要を示している。ロボット２は体幹４と左脚リンク６と右脚リンク８と制御部１０を備えている。また、オペレータ等がロボット２に動作指令を与える指示装置１２が用意されている。
本実施例では、図１に示すように、ロボット２の外部の床に固定されている絶対座標系（ｘ，ｙ，ｚ）が定義されている。絶対座標系（ｘ，ｙ，ｚ）では、ｘ軸方向とｙ軸方向が水平方向を向いており、ｚ軸方向が鉛直方向を向いている。また、ロボット２の支持脚の基準点に固定されている相対座標系（ｘ’，ｙ’，ｚ’）が定義されている。相対座標系（ｘ’，ｙ’，ｚ’）は、ロボット２が右脚リンク８を支持脚としているときは基準点Ｒ０に固定されており、ロボット２が左脚リンク６を支持脚としているときは、基準点Ｌ０に固定されている。相対座標系（ｘ’，ｙ’，ｚ’）では、足平リンク、４８に沿って踵から爪先へと伸びる方向をｘ’軸とし、足平リンク３８、４８に沿ってｘ’軸と直交する方向をｙ’軸とし、足平リンク３８、４８に垂直な方向をｚ’軸とする。通常は、ｘ’軸はロボット２が歩行する方向に相当し、ｙ’軸はロボット２の体側方向に相当し、ｚ’軸はロボット２の高さ方向に相当する。

左脚リンク６は一方の端部を股関節を介して体幹４に揺動可能に接続されている。左脚リンク６は膝関節や足首関節を備えており、先端には左足平７を備えている。右脚リンク８は股関節を介して体幹４に揺動可能に接続されている。右脚リンク８は膝関節や足首関節を備えており、先端には右足平９を備えている。ロボット２の各関節はアクチュエータ（図示されない）を備えており、それらのアクチュエータは制御部１０からの指示によって回転駆動する。左脚リンク６の左足平７の略中心には、基準点Ｌ０が設けられている。右脚リンク８の右足平９の略中心には、基準点Ｒ０が設けられている。基準点Ｌ０、Ｒ０は、ロボット２の動作パターンを作成する際に基準となる点である。図中Ｇは、ロボット２の体幹４の基準点であり、歩行動作を実行するときのロボット２の重心の位置に略一致する。ロボット２では、体幹４の基準点Ｇを、ロボット２の重心点として扱うことができる。
制御部１０はＣＰＵ、ＲＯＭ、ＲＡＭ、ハードディスク等を有するコンピュータ装置である。制御部１０は指示装置１２と通信可能であり、オペレータが指示装置１２に入力した指令は、ロボット２の制御部１０に教示される。制御部１０は、指示装置１２から入力した指令信号に基づいて、ロボット２の動作を制御する。制御部１０の詳細については、後段において詳細に説明する。

図１、２を参照して、ロボット２が実行する動作を説明する。ロボット２は、指示装置１２からの指令信号に従い、左右の脚リンク６、８を交互に振り出して、歩行動作を実施する。このとき、足平が接地している方の脚リンクを支持脚といい、足平が空中を移動する方の脚リンクを遊脚という。ロボット２は、指示装置１２から入力される指令信号に基づいて、遊脚の足平を接地させる目標位置や方向を決定し、接地目標位置へと遊脚を振り出して、歩行動作を実施する。足平を接地させる方向とは、足平の踵から爪先へと伸びる方向をいう。この足平の接地方向は、ｘ’軸がｘ軸となす角度で特定される。

図２に、指示装置１２からの指令信号に基づいて、ロボット２が決定する目標接地位置を例示する。指示装置１２には、例えばジョイスティック等を用いることができる。最初、右足平９が位置Ｐ１において接地しており、左足平７が位置Ｐ２において接地しており、ロボット２は静止しているとする。指示装置１２から「前進せよ」という指令信号が入力されると、例えば右脚リンク８を遊脚に決定し、右足平９の目標とする接地位置Ｐ３と接地方向Ｄ３と接地時間を決定する。右足平９の目標接地位置Ｐ３と方向Ｄ３を決定すると、決定した右足平９の目標接地位置Ｐ３と方向Ｄ３に沿うように、次ぎに遊脚となる左脚リンク６の左足平７の目標とする接地位置Ｐ４と接地方向と接地時間を決定する。即ち、ロボット２は、一度の指令信号に対して、一歩分だけ移動し、足平７、９を揃えて静止する歩行動作を計画して実行する。この歩行動作を実行している間に、再び指令信号が入力されると、新たに左足平７の目標接地位置Ｐ６と方向Ｄ６を決定するとともに、右足平９の目標接地位置Ｐ５を決定する。ロボット２は、オペレータの指示に基づいて２歩分の歩行計画を作成し、実行している歩行計画に接続して、一連の歩行計画を作成していく。
目標とする接地位置や接地方向や接地時間は、オペレータが指示装置１２に入力した指示に基づく。ロボット２は、オペレータの指示に追従して、歩行方向や歩行速度や歩幅等を変更しながら、一連の歩行動作を実行する。後記するように、オペレータが連続的に指示すると、ロボット２は、右足平９を目標接地位置Ｐ３に方向Ｄ３で接地し、左足平７を目標接地位置Ｐ６に方向Ｄ６で接地する歩行計画を立案して実行し、足平７、９を揃えて静止する歩行動作は実施されない。
遊脚の目標とする接地位置や接地方向や接地時間等を順に記述するデータを、歩行パターンデータということがある。

次に、ロボット２の制御部１０について説明する。図３に示すように、制御部１０は、機能的に、歩容データ作成装置５４と、歩容データ補正装置５６と、関節角群計算装置５８と、アクチュエータ制御部６０と、実際運動計算装置６２等を備えている。
制御部１０は、全体が物理的に１つの装置に含まれていてもよいし、物理的に分離された装置ごとに分けて収容されていてもよい。また制御部１０の各要素は、必ずしもロボット２に搭載されていなくてもよい。ロボット２外に配備され、ロボット２に無線又は有線で指示するようにしてもよい。

歩容データ作成装置５４は、指示装置１２から入力する指令信号に基づいて、足先歩容データと体幹歩容データを備える歩容データを作成して記憶する。足先歩容データとは、ロボット２の左右の脚リンク６、８の足平７、９の基準点Ｌ０、Ｒ０（足先）の軌道を記述するデータである。体幹歩容データとは、体幹４の基準点Ｇの軌道を記述するデータである。軌道とは、位置の経時的変化を意味している。各基準点Ｌ０、Ｒ０、Ｇの軌道は、絶対座標（ｘ，ｙ，ｚ）と絶対時刻で記述してもよいし、１歩あるいは複数歩毎に相対座標（ｘ’，ｙ’，ｚ’）と相対時刻で記述することもできる。歩容データ作成装置５４については、後段において詳細に説明する。歩容データ作成装置５４が記憶している歩容データは、歩容データ補正装置５６に入力される。
歩容データ補正装置５６は、歩容データ作成装置５４から入力した歩容データを、ロボット２の実際の運動に基づいて補正する。必要に応じて補正された歩容データは、関節角群計算装置５８に入力される。
関節角群計算装置５８は、入力した歩容データに基づいて、いわゆる逆キネマティクスを解くことでロボット２の各関節角を計算する。計算された関節角群データは、アクチュエータ制御部６０に入力される。
アクチュエータ制御装置６０は、入力した関節角群データに基づいて、ロボット２に搭載されているアクチュエータ群を制御する。アクチュエータ制御装置６０がアクチュエータ群を制御することによって、ロボット２の機械系６４が歩行動作を実施する。

ロボット２が実際に行う歩行動作は、予期しない路面の凹凸や、ロボット２の構造上のたわみなどによる外乱力の影響を受ける。ロボット２が実際に行っている歩行動作は、ロボット２に搭載されているセンサ群６６によって検出される。
実際運動計算装置６２は、センサ群６６の出力信号に基づいて、ロボット２の実際の運動を計算する。実際運動計算装置６２は、例えば左脚リンク６の実際運動や、右脚リンク８の実際運動や、体幹４の実際運動を計算することができる。実際運動計算装置６２で計算されたロボット２の実際運動は、歩容データ補正装置５６に入力される。
先に説明したように、歩容データ補正装置５６は、実際運動計算装置６２で計算されたロボット２の実際運動に基づいて、歩容データ作成装置５４から入力した歩容データを補正する。ロボット２の歩行動作は、ロボット２の実際運動に基づいてフィードバック制御される。

次に、歩容データ作成装置５４について詳細に説明する。図４に示すように、歩容データ作成装置５４は、機能的に、更新時点決定装置１０２と、歩行パターン作成装置１０４と、目標ＺＭＰ軌道作成装置１０６と、足先起動作成装置１０８と、体幹起動作成装置１１０と、データ記憶装置１１２等を備えている。データ記憶装置１１２は、歩行パターンデータ１２２や、目標ＺＭＰ軌道データ１２４や、足先歩容データ１２６や、体幹歩容データ１２８等を記憶することができる。
更新時点決定装置１０２は、指示装置１２から指令信号を入力したときに、ロボット２の歩行動作を更新する時点を決定する。更新時点決定装置１０２は、歩容データ作成装置５４が必要とする演算時間に基づいて、歩行動作の更新時点を決定する。
歩行パターン作成装置１０４は、更新時点決定装置１０２が決定した更新時点と、指示装置１２からの指令信号に基づいて、データ記憶装置１１２に記憶されている歩行パターンデータ１２２を更新する。
目標ＺＭＰ軌道作成装置１０６は、データ記憶装置１１２に記憶されている歩行パターンデータ１１２に基づいて、目標とするＺＭＰ（Zero Moment Point）の軌道を作成する。ＺＭＰは、ロボット２に作用する外力（慣性力を含む）によるモーメントの総和が０となる床上の点を意味する。接地している足平７、９が形成する多角形範囲内にＺＭＰが維持されれば、ロボット２は転倒することなく歩行し続けることが可能となる。そのための目標とするＺＭＰの軌道ＺＭＰ（ｔ）は、脚リンク６、８の足平７、９の接地位置の経時的変化（歩行パターン）に基づいて決定することができる。目標ＺＭＰ軌道は、絶対座標（ｘ，ｙ，ｚ）と絶対時刻で記述してもよいし、１歩あるいは複数歩毎に相対座標（ｘ’，ｙ’，ｚ’）と相対時刻で記述することもできる。作成された目標ＺＭＰ軌道は、離散化された目標ＺＭＰ軌道データ１２４として、データ記憶装置１１２に記憶される。また、目標ＺＭＰ軌道作成装置１０６は、歩行パターンデータ１２２が更新されると、データ記憶装置１１２に記憶されている目標ＺＭＰ軌道データ１２４を更新する。

足先軌道作成装置１０８は、データ記憶装置１１２に記憶されている歩行パターンデータ１２２に基づいて、左右の脚リンク６、８の足平７、９の基準点Ｌ０、Ｒ０の軌道（足先軌道）を作成する。作成された足先軌道は、足先歩容データ１２６として、データ記憶装置１１２に記憶される。また、足先軌道作成装置１０８は、歩行パターンデータ１２２が更新されると、データ記憶装置１１２に記憶されている足先歩容データ１２６を更新する。
体幹軌道作成装置１１０は、データ記憶装置１１２に記憶されている目標ＺＭＰ軌道データ１２４や足先歩容データ１２６に基づいて、体幹４の基準点Ｇの軌道を作成する。作成された体幹軌道は、離散化された体幹歩容データ１２８として、データ記憶装置１１２に記憶される。また、体幹軌道作成装置１１０は、目標ＺＭＰ軌道データ１２４が更新されると、データ記憶装置１１２に記憶されている体幹歩容データ１２８を更新する。

ロボット２は、歩容データ作成装置５４のデータ記憶装置１１２に記憶されている足先歩容データ１２６と体幹歩容データ１２８に従って、足先と体幹を移動することによって、歩行動作を実行する。オペレータが指示装置１２から指令を入力すると、歩容データ作成装置５４が、現に実行している足先歩容データ１２６と体幹歩容データ１２８に、オペレータの指令に基づいて作成した足先歩容データ１２６と体幹歩容データ１２８を接続して、一連の歩容データを作成していく。
図５は、歩容データ作成装置５４が実行する処理の流れを示している。図５に示すフローに沿って、歩容データ作成装置５４が、動作指令に追従して一連の歩容データを作成していく処理手順を説明する。

ステップＳ２では、指示装置１２からの指令信号が入力されるまで待機状態となる。この間、ロボット２は、歩容データ作成装置５４のデータ記憶装置１１２に記憶されている歩容データ１２６、１２８を実行している。指示装置１２からの指令が入力されない場合は、所定時間毎に、ロボット２が静止し続ける軌道データを作成して記憶するようにしてもよい。指示装置１２からの指令信号が入力されると、ステップＳ４に進む。
ステップＳ４では、更新時点決定装置１０２が、歩行動作の更新（変更）時点を決定する。図６に示すように、更新時点は、動作指令の信号を入力した時点から、後述の処理に要する時間を考慮した余裕時間だけ遅延させた時点（例えばｋ）に決定される。図６のｋは、時系列データにおけるデータの順番を示しており、例えばＱ_ｋは実行中のデータにおけるｋ番目のデータを意味している。更新時点ｋが決定すると、実行中の時系列データ（Ｑ_０、Ｑ_１・・・Ｑ_ｋ−１、Ｑ_ｋ）に、新たに作成する更新データ（Ｒ_２、Ｒ_３・・）を接続するデータ位置（図６ではＱ_ｋ−１）が決まる。なお、実行中の時系列データにおいてデータＱ_ｋ以降のデータは破棄される。
図５のステップＳ６では、歩行パターン作成装置１０４が、指示装置１２からの指令信号と、決定された更新時点に基づいて歩行パターンデータ１２２を更新する。例えば図２に示すように、左脚リンク６の左足平７の接地位置Ｐ４と方向（Ｄ３に等しい）が、接地位置Ｐ６と方向Ｄ６に更新される。

図５のステップＳ８では、足先軌道作成装置１０８が、更新された歩行パターンデータ１２２に基づいて、データ記憶装置１１２に記憶されている足先歩容データ１２６を更新する。図６に示す時系列データを、足先歩容データ１２６とすると、実行中の足先歩容データ１２６の時点ｋ−１のデータＱ_ｋ−１に、新たに作成する更新後の足先歩容データ（Ｒ_２，Ｒ_３，・・）を接続して、一連の足先歩容データを作成する。このとき、更新後の歩容データの最初のデータＲ_２を決定する際に、実行中の歩容データの更新時点から遡及した複数のデータ（例えばＱ_ｋ−２、Ｑ_ｋ−１）を加味することによって、連続的な足先軌道を記述する一連の足先歩容データを作成することができる。
ステップＳ１０では、目標ＺＭＰ軌道作成装置１０６が、更新された歩行パターンデータ１２２に基づいて、データ記憶装置１１２に記憶されている目標ＺＭＰ軌道データ１２４を更新する。例えば図２に示すように、左脚リンク６の左足平７の接地位置Ｐ４と方向（Ｄ３に等しい）が、接地位置Ｐ６と方向Ｄ６に更新されると、点ＰＡ−点Ｐ２−点Ｐ３−点ＰＢを結ぶ目標ＺＭＰ軌道が、点ＰＡ−点Ｐ２−点Ｐ３−点Ｐ６−点ＰＣと結ぶ軌道に更新される。

図５のステップＳ１２〜ステップＳ１８の処理によって、体幹軌道作成装置１１０が、データ記憶装置１１２に記憶されている体幹歩容データ１２８を更新する。体幹軌道作成装置１１０は体幹歩容データ１２８を更新することによって、オペレータによって逐次更新された歩行パターンを、ロボット２は一連の歩行動作として実施することができる。
ステップＳ１２では、更新時点ｋ以降の体幹軌道を仮定し、仮定した体幹歩容データから計算する仮のＺＭＰ軌道と、目標ＺＭＰ軌道データ１２４が記述するＺＭＰ軌道とのＺＭＰ偏差を計算する。図６に示す時系列データを、離散化した体幹軌道（即ち体幹歩容データ）とすると、更新時点ｋ以降に接続する体幹歩容データは、（Ｒ_２，Ｒ_３，・・，Ｒ_ｎ−１）からなるｎ−２個の時系列データとなる。仮定する体幹軌道は、例えば目標ＺＭＰ軌道を、所定の高さだけｚ軸方向に移動させた軌道とすることができる。
一般に、体幹（重心）軌道（ｘ（ｔ），ｙ（ｔ），ｚ（ｔ））に対するＺＭＰ軌道（Ｐｘ（ｔ），Ｐｙ（ｔ））は、下記のＺＭＰ方程式から計算することができる。

ここで、ｍはロボット２の質量である。ｆｚは支持脚の足平に作用する床反力である。Ｉ_x・ｄω_x／ｄｔとＩ_ｙ・ｄω_ｙ／ｄｔは、体幹４の基準点Ｇの回転に関する慣性の影響を示す。本実施例では、水平軸まわりのロボット２の角運動量を０（ゼロ）としているので、Ｉ_x・ｄω_x／ｄｔとＩ_ｙ・ｄω_ｙ／ｄｔは０（ゼロ）となる。
上記したＺＭＰ方程式を離散化することによって、例えばｘ軸方向に関して、下記の三項方程式を得ることができる。

ここでｘ_ｉとＰ_ｘｉは、それぞれ体幹軌道ｘ（ｔ）と、ｘ方向のＺＭＰ軌道ｐｘ（ｔ）を離散化したものである。ｉは単位時間Δｔで区分した時系列データの順序を示す。上記の数式２は、時点ｉにおけるＺＭＰのｘ方向座標Ｐ_ｘｉが、直前のｘ方向座標ｘ_ｉ−１と、そのときのｘ方向座標ｘ_ｉと、直後のｘ方向座標ｘ_ｉ＋１から計算できることを示している。係数ａ_ｉ、ｂ_ｉ，ｃ_ｉは、下記によって算出される係数である。なお床反力ｆｚについては、ロボット２のｚ軸方向に関する運動方程式：ｍ・ｄ^２ｚ／ｄｔ^２＝−ｍｇ＋ｆｚが成り立つ。

上記の数式２と図６から理解されるように、仮定した体幹歩容データ（Ｒ_２，Ｒ_３，・・，Ｒ_ｎ−１）と、実行中の体幹歩容データ１２８において更新時点ｋから遡及した２つのデータ（Ｑ_ｋ−２，Ｑ_ｋ−１）を用いることによって、ＺＭＰ偏差ｄ_ｉは、
ｄ_１＝ＺＭＰ_１−ａ_１・Ｑ_ｋ−２＋ｂ_１・Ｑ_ｋ−１＋ｃ_１・Ｒ_２
から、
ｄ_ｎ−２＝ＺＭＰ_ｎ−２−ａ_ｎ−２・Ｒ_ｎ−３＋ｂ_ｎ−２・Ｒ_ｎ−２＋ｃ_ｎ−２・Ｒ_ｎ−１
まで、ｎ−２個の時系列データとして得ることができる。ここで、ＺＭＰ_ｉは、目標ＺＭＰ軌道データ１２４が記述している離散化された目標ＺＭＰ軌道である。ＺＭＰ偏差ｄ_１は、更新時点ｋの直前時点に対するＺＭＰ偏差である。更新時点ｋから遡及した２つのデータ（Ｑ_ｋ−２，Ｑ_ｋ−１）を用いることによって、更新時点ｋの直前時点における体幹位置とその変化速度を拘束条件として加味することができる。

図５のステップＳ１４では、ステップＳ１２で計算したＺＭＰ偏差の時系列データ（ｄ_１，ｄ_２，・・，ｄ_ｎ−２）に基づいて、ステップＳ１２で仮定した体幹歩容データ（Ｒ_２，Ｒ_３，・・，Ｒ_ｎ−１）の各修正量（ｒ_２，ｒ_３，・・，ｒ_ｎ−１）を決定する。ただし、最終時点ｎ−１において、ロボット２を安定して（位置と速度を連続にして）静止させる必要があることから、最終の２つの時点ｎ−２、ｎ−１のデータＲ_ｎ−２、Ｒ_ｎ−１については修正せず、その修正量ｒ_ｎ−２、ｒ_ｎ−１をゼロとする。
数式１に示したＺＭＰ方程式から、一般に、ＺＭＰ偏差ｄ（ｔ）：（ｄ_ｘ（ｔ），ｄ_ｙ（ｔ））と、そのＺＭＰ偏差をゼロに減じるのに要する体幹（重心）軌道の水平方向の補正量ｒ（ｔ）：（ｒ_ｘ（ｔ），ｒ_ｙ（ｔ））の関係は、次式で表すことができる。

上記の修正量ｒ（ｔ）に関するＺＭＰ方程式についても、離散化することによって三項方程式に書き換えることができる。体幹軌道作成装置１１０は、数式４を離散化した三項方程式に、ステップＳ１２で計算したＺＭＰ偏差（ｄ_１，ｄ_２，・・，ｄ_ｎ−２）を代入して、修正量（ｒ_１，ｒ_２，・・，ｒ_ｎ−３）に関する下記の連立方程式を導出する。先に説明したように、最終の２つの修正量ｒ_ｎ−２、ｒ_ｎ−１については既にゼロに設定されている。なお、下記の式ではｘ，ｙ軸方向を区別していないが、実際にはｘ，ｙ軸方向のそれぞれについて導出する。

上記の数式５を満たす修正量（ｒ_１，ｒ_２，・・，ｒ_ｎ−３）を導出することができれば、動作の変更時点（更新時点ｋ）においても、ロボット２の実際のＺＭＰが目標ＺＭＰと一致する体幹歩容データを得ることできる。しかしながら、上記の数式５は、データ数がｎ−４個である修正量（ｒ_１，ｒ_２，・・，ｒ_ｎ−３）に対して、ｎ−２個の条件式が与えられており、不定な連立方程式となっている。これは、本実施例では、更新時点の直前時点ｋ−１におけるＺＭＰ偏差を考慮しているためである。
よく知られているように、目標ＺＭＰ軌道に追従する体幹（重心）軌道を作成する場合、軌道端点の位置と速度を拘束すると、軌道端点のＺＭＰを補償することができなくなる。従来の技術では、事前に準備されている体幹軌道に、新たに作成する重心軌道を接続する場合に、接続（更新）時点においてはＺＭＰを補償しないようにしている。即ち、上記の数式５を解く際に、ＺＭＰ偏差ｄ_１を無視する（ゼロとする）ようにしている。この手法では、動作を大きく変更する場合などに、変更時点におけるＺＭＰ偏差が許容範囲を超えてしまい、場合によってはロボット２が転倒してしまうことがある。そのことから、従来技術では、動作の変更時点を両脚立脚時に限定したり、動作を変更する度合に制限を設けたりしている。

本実施例のロボット２では、動作を変更する前後において極大化するＺＭＰ偏差を、変更時点以降の動作時に分散するという思想に基づいて、上記の数式５に示す不定な連立方程式を、最小二乗法を用いて解法する。それにより、従来の技術では不可能であった動作の大幅な変更についても、ＺＭＰ偏差が増大することがなく、許容できるレベルに抑制することができる。
体幹軌道作成装置１１０は、最小二乗法を用いて、上記の数式５から下記の数式６、７を導出する。

上記の数式６において、行列の右肩に付すＴの符号は転置行列であることを示している。数式６を解くことによって、ＺＭＰ偏差（ｄ_１，ｄ_２，・・，ｄ_ｎ−２）の二乗和を最小にする修正量（ｒ_１，ｒ_２，・・，ｒ_ｎ−３）を算出することができる。なお数式６は、変数である修正量（ｒ_１，ｒ_２，・・，ｒ_ｎ−３）のデータ数と、条件式の数が一致していることから、一意に解くことができる。

ステップＳ１６では、ステップＳ１２で仮定した体幹軌道（Ｒ_１，Ｒ_２，・・，Ｒ_ｎ−３）を、ステップＳ１４で算出した修正量（ｒ_１，ｒ_２，・・，ｒ_ｎ−３）だけ修正することによって、更新後の目標とする体幹軌道を作成する。
ステップＳ１８では、データ記憶手段１１２に記憶されている体幹歩容データ１２８を、ステップＳ１６で作成した更新後の体幹軌道（体幹歩容データ）を接続することによって、新たな一連の体幹歩容データ１２８に更新する。
ロボット２は、以上に説明したステップＳ２〜Ｓ１８間での処理を、動作終了の指令が入力されるまで繰り返し実行する。データ記憶装置１１２に記憶されている歩容データ１２６、１２８は、ロボット２によって実行されながら、外部からの指令に基づいて逐次更新、接続されていく。

図７に示すように、本実施例のロボット２では、動作の更新時点以降において、実際のＺＭＰ軌道Ｂが目標ＺＭＰ軌道Ａに対して許容レベルの偏差を保ちながら追従していく。従来のロボット（図中のＡ）のように、動作の変更（データの更新）時点においてＺＭＰ偏差が急激に増大することがない。オペレータの指示に追従して、より自由に動作を変更することができる。このとき、ロボット２の姿勢や、その変化速度や、その変化加速度は連続性が確保されている。

以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
上記では、体幹４の基準点Ｇを、ロボット２の重心点として扱っているが、基準点Ｇを重心点に近似できない場合には、上述の手法によって重心点の軌道を求めた後に、体幹４の基準点Ｇや左右の足平７、９の基準点Ｌ０、Ｒ０の軌道を求めるとよい。
また、ロボットへ動作の変更指令を入力する指示装置は、例えば歩行路面に存在する障害物等を検知するセンサ等に変更してもよい。それにより、歩行する環境に対応して、ロボット自身が動作変更の指令を生成することができる。即ち、ロボットの内部から動作変更の指令を与えもよい。様々な環境に対応して、臨機応変に歩行することができるロボットを具現化することもできる。
本実施例のロボットでは、外部からの指令（あるいは内部からの指令でもよい）に基づいて、歩行パターンデータを逐次作成していく。そのことから、歩行パターンデータは、外部から与えられた一連の指令を記述するデータといえる。また、歩行パターンデータを予め用意してロボットに教示しておくことは、一連の指令を予めロボットに教示しておくことに相当する。いずれにしても、本実施例のロボットは、歩行パターンデータに基づいて、所定歩数分の歩容データを逐次作成し、一連の歩容データを作成することができる。

本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

実施例１のロボットを模式的に示す図。 ロボットの歩行パターンを説明する図。 ロボットが搭載しているコンピュータ装置の機能的な構成を示す。 歩容データ作成装置の機能的な構成を示す図。 歩容データ作成装置が実行する処理の流れを示すフローチャート。 実行中のデータと更新後のデータの経時的な関係を模式的に示す図。 目標ＺＭＰ軌道と実際ＺＭＰ軌道を比較して示す図。

符号の説明

２：ロボット
４：体幹
６：左脚リンク
７：左足平
８：右脚リンク
９：右足平
１０：制御部
１２：指示装置
５４：歩容データ作成装置
５６：歩容データ補正装置
５８：関節角群計算装置
６０：アクチュエータ制御装置
６２：実際運動計算装置
６４：ロボットの機械系
６６：各種センサ
１０２：更新時点決定装置
１０４：歩行パターン作成装置
１０６：目標ＺＭＰ軌道作成装置
１０８：足先軌道作成装置
１１０：体幹軌道作成装置
１１２：データ記憶装置
１２２：歩行パターンデータ
１２４：目標ＺＭＰ軌道データ
１２６：足先歩容データ
１２８：体幹歩容データ

Claims (2)

1. 体幹に揺動可能に連結されている脚リンクを備える脚式ロボットであって、
   予定の歩行動作を実施するときの体幹位置の時系列データを記憶している手段と、
   指令に基づいて、予定の歩行動作からの変更時点を決定する手段と、
   指令に基づいて、変更時点以降の目標ＺＭＰ軌道を作成する手段と、
   変更時点以降の体幹位置の時系列データを仮定する手段と、
   変更時点の直前の二以上の時点に対して記憶している体幹位置の二以上のデータに、仮定した変更時点以降の体幹位置の時系列データを接続することによって時系列データを作成し、その時系列データから、変更時点以降の仮のＺＭＰ軌道を計算する手段と、
   計算した仮のＺＭＰ軌道と目標ＺＭＰ軌道との偏差の二乗和が最小となるように、仮定した体幹位置の時系列データを修正する修正手段と、
   記憶している変更時点以降の体幹位置の時系列データを、修正後の体幹位置の時系列データに更新する手段と、
   を備える脚式ロボット。
2. 脚式ロボットの歩行動作を調節する方法であって、
   予定の歩行動作を実施するときの体幹位置の時系列データを脚式ロボットに記憶させる工程と、
   指令に基づいて、予定の歩行動作からの変更時点を決定する工程と、
   指令に基づいて、変更時点以降の目標ＺＭＰ軌道を作成する工程と、
   変更時点以降の体幹位置の時系列データを仮定する工程と、
   変更時点の直前の二以上の時点に対して脚式ロボットが記憶している体幹位置の二以上のデータに、仮定した変更時点以降の体幹位置の時系列データを接続することによって時系列データを作成し、その時系列データから、変更時点以降の仮のＺＭＰ軌道を計算する工程と、
   計算した仮のＺＭＰ軌道と目標ＺＭＰ軌道との偏差の二乗和が最小となるように、仮定した体幹位置の時系列データを修正する工程と、
   脚式ロボットが記憶している変更時点以降の体幹位置の時系列データを、修正後の体幹位置の時系列データに更新する工程と、
   を備える脚式ロボットの動作調節方法。

Images