JP4810880B2

JP4810880B2 - ロボットとその制御方法

Info

Publication number: JP4810880B2
Application number: JP2005137236A
Authority: JP
Inventors: 竜介但馬
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2005-05-10
Filing date: 2005-05-10
Publication date: 2011-11-09
Anticipated expiration: 2025-05-10
Also published as: JP2006315095A

Description

本発明は、ロボットとその動作を制御する方法に関する。詳しくは、脚リンクを移動しようとする方向へ振り出して、振り出して接地した脚リンクを支持脚とする動作を繰り返して移動する脚式ロボットにおいて、自律的に動作の安定性を評価しながら目標とする姿勢を実現するロボットとその制御方法に関する。

少なくとも１つの脚リンクを備え、移動しようとする方向へ脚リンクを振り出し、振り出して接地した脚リンクを支持脚とする動作を繰り返して移動するロボットが開発されている。
上記のような脚式ロボットは、ＺＭＰ（Zero Moment Point）を利用して制御することによって、安定した歩行を実現できることが知られている。ＺＭＰは、ロボットに作用する外力（慣性力を含む）によるモーメントの総和が０となる床上の点を意味する。
１つの脚リンクのみが接地している状態でも、ＺＭＰが支持脚の足平の接地領域内に存在していれば、ロボットは倒れることがない。例えば２つの脚リンクを備えるロボットの場合、一方の脚リンクを支持脚とし、他方の脚リンクを遊脚として前方に振り出することによって移動する。この片足接地状態の間、ＺＭＰが支持脚の足平の接地領域内に存在していれば、ロボットは倒れることがない。遊脚としていた脚リンクが接地し、２つの脚リンクが接地している間に、ＺＭＰがそれまで支持脚であった脚リンクの足平の接地領域内から新たに接地した脚リンクの足平の接地領域内へ移行すれば、ロボットは倒れることがなく、それまで支持脚であった脚リンクを前方に振り出すことが可能となる。それまで支持脚であった脚リンクを前方に振り出す時に、ＺＭＰが新たな支持脚の足平の接地領域内に存在していれば、ロボットは倒れることがない。
上記の動作を繰り返すことによって、ロボットは倒れることなく歩行を継続することができる。

上記した原理に基づいて、ロボットを安定して歩行させる技術が開発されている。例えば特許文献１には、ロボットの実際の状態を検出し、検出された実際の状態に基づいて歩行の動力学的な安定性を確保しながら歩行することが可能なロボットの運動パターンを生成する技術が開示されている。
特開２００４−１４２０９５号公報

ロボットの歩行に関する動力学的な安定性を確保しつつ、所望の姿勢を取らせたいという要望が存在する。
例えばロボットの歩行を長期間に渡って安定な動作とするために、幾つかの関節群の関節角を可動範囲の中立位置の近傍に維持したい場合がある。通常、ロボットの関節の可動範囲は有限であり、可動範囲を超えて関節を回転することはできない。従って、脚リンクを接地させた状態で動作をする際には、ＺＭＰに基づく動力学的な安定性を確保するとともに、関節の回転角が可動範囲を超えないように制御する必要がある。幾つかの関節群の関節角を可動範囲の中立位置の近傍に保つことで、関節の可動範囲に対して十分な余裕を確保しながら歩行を行うことが可能となる。このように動作を制御することで、不意に外乱が作用した場合であっても、関節の可動範囲に対して十分な余裕が確保されており、例えば腕を振り出してバランスを取るといった、歩行の安定性を確保するための動作を容易に行うことができる。長期的に安定してロボットの歩行を継続することが可能となる。
また上記した以外にも、ロボットを歩行させながら所望の作業を行わせたり、ダンスや演奏といった所定のパフォーマンスを実演させたりしたい場合がある。

常に最も安定な動作を優先すると、しばしばロボットの姿勢は目標とする姿勢から大きく外れていくことがある。このような場合に、目標とする姿勢へ復帰させる制御を実現したい。
従来の技術では、目標とする姿勢へ復帰させる際に、歩行の安定性を損なわないように、非常にゆっくりとした速度で目標姿勢へ復帰させたり、ロボットの動作を視認しているオペレータが姿勢を復帰しても安定であると判断した場合にのみ目標姿勢へ復帰させたりしていた。

オペレータによる操作を行うことなく、より迅速に目標とする姿勢へ復帰させるためには、動作の安定性を自律的に評価しながら、目標姿勢へ復帰するようにロボットの動作を制御する技術が必要とされる。

本発明では上記課題を解決する。本発明では、脚リンクを移動しようとする方向へ振り出して、振り出して接地した脚リンクを支持脚とする動作を繰り返して移動する脚式ロボットにおいて、自律的に動作の安定性を評価しながら目標とする姿勢を実現することが可能な技術を提供する。

本発明のロボットは、体幹と、一方の端部が股関節を介して体幹に連結され、他方の端部が足首関節を介して足平を備える足先部に連結された脚リンクと、関節群を回転駆動するアクチュエータ群と、アクチュエータ群を制御するコントローラを備えるロボットである。そのロボットにおいて、そのコントローラは、足先歩容データを記憶する足先歩容データ記憶部と、足先歩容データに基づいて足平の内部の安定領域を計算する安定領域計算部と、足先歩容データに基づいて第１のＺＭＰ軌道を計算する第１ＺＭＰ軌道計算部と、第１のＺＭＰ軌道とＺＭＰ方程式に基づいて第１のＺＭＰ軌道を実現する第１の重心の運動量および重心まわりの角運動量の経時的変化を計算する第１重心運動計算部と、第１の重心の運動量および重心まわりの角運動量の経時的変化と重心の運動量に関するヤコビ行列と重心まわりの角運動量に関するヤコビ行列に基づいて第１の重心の運動量および重心まわりの角運動量の経時的変化を実現する第１の姿勢パターンを計算する第１姿勢パターン計算部と、第３の姿勢パターンを記憶する第３姿勢パターン記憶部と、第１の姿勢パターンと第３の姿勢パターンに基づいて第１の姿勢パターンから第３の姿勢パターンへ移行する第２の姿勢パターンを計算する第２姿勢パターン計算部と、第２の姿勢パターンと重心の運動量に関するヤコビ行列と重心まわりの角運動量に関するヤコビ行列に基づいて第２の姿勢パターンによって実現される第２の重心の運動量および重心まわりの角運動量の経時的変化を計算する第２重心運動計算部と、第２の重心の運動量および重心まわりの角運動量の経時的変化とＺＭＰ方程式に基づいて第２の重心の運動量および重心まわりの角運動量の経時的変化によって実現される第２のＺＭＰ軌道を計算する第２ＺＭＰ軌道計算部と、第１の姿勢パターンと第２の姿勢パターンと足平の内部の安定領域と第２のＺＭＰ軌道に基づいて姿勢パターンを選択してアクチュエータ群へ出力する姿勢パターン選択部とを備えている。その姿勢パターン選択部は、第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まる場合に、アクチュエータ群へ第２の姿勢パターンを出力し、第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まらない場合に、アクチュエータ群へ第１の姿勢パターンを出力する。
本明細書で「足先歩容データ」とは、ロボットの脚リンクの足先部の基準点に関する、位置の経時的な変化を記述するデータのことをいう。
本明細書で「軌道」とは、位置の経時的な変化を記述するデータのことをいう。
本明細書で「姿勢パターン」とは、ロボットの関節群の関節角の経時的変化を記述するデータのことをいう。

上記のロボットでは、コントローラからの指令によってアクチュエータ群が関節を回転駆動し、ロボットは動作を実現する。上記のロボットは、例えば二足歩行ロボットとして具現化することが可能であり、一方の脚リンクを支持脚として、他方の脚リンクを前方へ振り出し、振り出して接地した脚リンクを新たな支持脚とする動作を繰り返すことで、歩行動作を実現することができる。
上記のロボットでは、コントローラが足先歩容データ記憶部に記憶された足先歩容データに基づいて、足平の内部の安定領域と、第１のＺＭＰ軌道をそれぞれ計算する。足平の内部の安定領域は、ロボットが歩行する床面上の領域であって、接地している脚リンクの足平の接地領域の内部で定義される。この安定領域は、動作の安定性を評価する際の基準として用いられる。第１のＺＭＰ軌道は、ロボットが歩行する際に、歩行の動力学的安定性のみを考慮して設定されるＺＭＰ軌道である。
その後コントローラは、第１のＺＭＰ軌道とＺＭＰ方程式に基づいて、第１のＺＭＰ軌道を実現するロボットの運動を計算する。ロボットの運動は、重心の運動量と重心まわりの角運動量の経時的変化によって記述される。ロボットが実現すべきＺＭＰ軌道に対して、そのロボットのＺＭＰ方程式を用いて、そのＺＭＰ軌道を実現する重心の運動量と重心まわりの角運動量の経時的変化を計算する技術は、従来より種々の手法が開発されている。本明細書では、上記のようにして計算された、第１のＺＭＰ軌道を実現する重心の運動量および重心まわりの角運動量の経時的変化を、第１の重心の運動量と重心まわりの角運動量の経時的変化と呼ぶ。
その後コントローラは、第１の重心の運動量と重心まわりの角運動量の経時的変化を実現する姿勢パターン（第１の姿勢パターン）を計算する。ロボットの姿勢パターンと、重心の運動量の経時的変化とは、重心の運動量に関するヤコビ行列を用いて関連付けることができる。またロボットの姿勢パターンと、重心まわりの角運動量の経時的変化とは、重心まわりの角運動量に関するヤコビ行列を用いて関連付けることができる。従って、第１の重心の運動量と重心まわりの角運動量の経時的変化と、重心の運動量に関するヤコビ行列と、重心まわりの角運動量に関するヤコビ行列に基づいて、第１の重心の運動量と重心まわりの角運動量の経時的変化を実現する第１の姿勢パターンを計算することができる。

上記のように計算される第１の姿勢パターンは、第１のＺＭＰ軌道に対応して計算されるものであり、アクチュエータ群が第１の姿勢パターンに従ってロボットの関節群を回転すると、ロボットのＺＭＰは第１のＺＭＰ軌道と同一の軌道を描く。第１のＺＭＰ軌道は、ロボットの歩行の動力学的安定性のみを考慮して設定されており、例えばロボットが一方の脚リンクのみで接地している状態では、足平の内部に常に維持される。従って、上記した第１の姿勢パターンをアクチュエータ群へ出力している限り、ロボットは安定して歩行を行うことができる。

本発明のロボットにおいて、上記のコントローラは、上記のようにして計算される安定な姿勢パターン（すなわち、第１の姿勢パターン）のほかに、利用者がロボットに実現させたい姿勢パターン（第３の姿勢パターン）を第３姿勢パターン記憶部に記憶している。上記のコントローラは、第１の姿勢パターンから第３の姿勢パターンへ移行する第２の姿勢パターンを計算する。第２の姿勢パターンは、例えば第１の姿勢パターンから出発して、所定時間経過後に、第３の姿勢パターンへ一致するように計算される。上記の第２の姿勢パターンに関しては、ロボットがその第２の姿勢パターンを実現したときに、歩行の動力学的な安定性は必ずしも確保されているわけではない。従って、上記のコントローラは計算された第２の姿勢パターンの安定性を評価して、第２の姿勢パターンが安定であると評価された場合にのみ、アクチュエータ群へ第２の姿勢パターンを実現するように指示を出力する。第２の姿勢パターンが安定であると評価されない場合は、アクチュエータ群へ第１の姿勢パターンを実現するように指示を出力する。
第２の姿勢パターンの安定性は、第２の姿勢パターンを実現するときのＺＭＰ軌道（第２のＺＭＰ軌道）が、足平の内部の安定領域に収まっているか否かを判断することによって、評価することができる。第２のＺＭＰ軌道が足平の内部の安定領域に収まっている場合、第２の姿勢パターンを実現したときのロボットの動作は安定であるため、コントローラはロボットに第２の姿勢パターンを実現させる。一方、第２のＺＭＰ軌道が足平の内部の安定領域に収まっていない場合、第２の姿勢パターンを実現したときのロボットの動作は必ずしも安定ではないため、コントローラはロボットに第１の姿勢パターンを実現させる。
上述のように、本発明のロボットは目標とする姿勢パターン（第３の姿勢パターン）へ移行するための姿勢パターン（第２の姿勢パターン）の安定性を自律的に評価して、第２の姿勢パターンが安定であると評価される場合には第２の姿勢パターンを実現して目標とする第３の姿勢パターンへ移行し、第２の姿勢パターンが安定であると評価されない場合には安定な第１の姿勢パターンを実現する。このようにして、本発明のロボットは動作の安定性を確保しながら、目標とする姿勢を実現することができる。

本発明は方法としても具現化される。本発明の方法は、体幹と、一方の端部が股関節を介して体幹に連結され、他方の端部が足首関節を介して足平を備える足先部に連結された脚リンクと、関節群を回転駆動するアクチュエータ群と、アクチュエータ群を制御するコントローラを備えるロボットの動作を制御する方法である。その方法は、足先歩容データを読込む工程と、足先歩容データに基づいて足平の内部の安定領域を計算する工程と、足先歩容データに基づいて第１のＺＭＰ軌道を計算する工程と、第１のＺＭＰ軌道とＺＭＰ方程式に基づいて第１のＺＭＰ軌道を実現する第１の重心の運動量および重心まわりの角運動量の経時的変化を計算する工程と、第１の重心の運動量および重心まわりの角運動量の経時的変化と重心の運動量に関するヤコビ行列と重心まわりの角運動量に関するヤコビ行列に基づいて第１の重心の運動量および重心まわりの角運動量の経時的変化を実現する第１の姿勢パターンを計算する工程と、第３の姿勢パターンを読込む工程と、第１の姿勢パターンと第３の姿勢パターンに基づいて第１の姿勢パターンから第３の姿勢パターンへ移行する第２の姿勢パターンを計算する工程と、第２の姿勢パターンと重心の運動量に関するヤコビ行列と重心まわりの角運動量に関するヤコビ行列に基づいて第２の姿勢パターンによって実現される第２の重心の運動量および重心まわりの角運動量の経時的変化を計算する工程と、第２の重心の運動量および重心まわりの角運動量の経時的変化とＺＭＰ方程式に基づいて第２の重心の運動量および重心まわりの角運動量の経時的変化によって実現される第２のＺＭＰ軌道を計算する工程と、第１の姿勢パターンと第２の姿勢パターンと足平の内部の安定領域と第２のＺＭＰ軌道に基づいて第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まる場合にアクチュエータ群へ第２の姿勢パターンを出力し、第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まらない場合にアクチュエータ群へ第１の姿勢パターンを出力する工程とを備えている。

本発明の技術を用いることによって、脚リンクを移動しようとする方向へ振り出して、振り出して接地した脚リンクを支持脚とする動作を繰り返して移動する脚式ロボットにおいて、自律的に動作の安定性を評価しながら目標とする姿勢を実現することができる。

以下に説明する実施例の主要な特徴を列記する。
（特徴１）
体幹と、
一方の端部が股関節を介して体幹に連結され、他方の端部が足首関節を介して足平を備える足先部に連結された脚リンクと、
関節群を回転駆動するアクチュエータ群と、
足先歩容データを記憶する手段と、
足先歩容データに基づいて、足平の内部の安定領域を計算する手段と、
足先歩容データに基づいて、第１のＺＭＰ軌道を計算する手段と、
第１のＺＭＰ軌道と、ＺＭＰ方程式に基づいて、第１のＺＭＰ軌道を実現する第１の重心の運動量および重心まわりの角運動量の経時的変化を計算する手段と、
第１の重心の運動量および重心まわりの角運動量の経時的変化と、重心の運動量に関するヤコビ行列と、重心まわりの角運動量に関するヤコビ行列に基づいて、第１の重心の運動量および重心まわりの角運動量の経時的変化を実現する第１の姿勢パターンを計算する手段と、
第３の姿勢パターンを記憶する手段と、
第１の姿勢パターンと第３の姿勢パターンに基づいて、第１の姿勢パターンから第３の姿勢パターンへ移行する第２の姿勢パターンを計算する手段と、
第２の姿勢パターンと、重心の運動量に関するヤコビ行列と、重心まわりの角運動量に関するヤコビ行列に基づいて、第２の姿勢パターンによって実現される第２の重心の運動量および重心まわりの角運動量の経時的変化を計算する手段と、
第２の重心の運動量および重心まわりの角運動量の経時的変化と、ＺＭＰ方程式に基づいて、第２の重心の運動量および重心まわりの角運動量の経時的変化によって実現される第２のＺＭＰ軌道を計算する手段と、
第１の姿勢パターンと、第２の姿勢パターンと、足平の内部の安定領域と、第２のＺＭＰ軌道に基づいて、姿勢パターンを選択する手段と、
選択された姿勢パターンをアクチュエータ群へ出力する手段と
を備え、前記姿勢パターンを選択する手段は、
第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まる場合に、アクチュエータ群へ第２の姿勢パターンを出力し、
第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まらない場合に、アクチュエータ群へ第１の姿勢パターンを出力する
ことを特徴とするロボット。

（第１実施例）
図面を参照しながら、本発明の一実施例に係るロボットの動作生成技術について説明する。
図１に示すロボット６は、体幹１２と、右脚リンク２６と、左脚リンク２０と、右腕リンク２４と、左腕リンク２２を備えている。
右脚リンク２６は一方の端部を股関節を介して体幹１２に揺動可能に連結されている。右脚リンク２６はさらに膝関節と足首関節を備え、先端には足先部を備えている。その足先部は床と面で接することが可能な足平を備えている。左脚リンク２０は一方の端部を股関節を介して体幹１２に揺動可能に連結されている。左脚リンク２０はさらに膝関節と足首関節を備え、先端には足先部を備えている。その足先部は床と面で接することが可能な足平を備えている。
右腕リンク２４は一方の端部を体幹１２の上部に右肩関節を介して揺動可能に連結されている。左腕リンク２２は一方の端部を体幹１２の上部に左肩関節を介して揺動可能に連結されている。
ロボット６の全身の関節群は、それぞれアクチュエータを備えている。アクチュエータ群はコンピュータ装置１４からの指令によって駆動され、関節群は指令された関節角を実現する。コンピュータ装置１４は、コントローラに相当する。

ロボット６は、右脚リンク２６が接地している間に左脚リンク２０を遊脚として軌道７のように移動させ、左脚リンク２０が接地すると今度は右脚リンク２６を遊脚として軌道８のように移動させ、以下同様に、左脚リンク２０を遊脚として軌道９のように移動させ、次には右脚リンク２６を遊脚として軌道１０のように移動させて歩行を続ける。
ロボット６の歩行の態様は、足先基準点Ｌ，Ｒの軌道７，８，９，１０を記述する足先歩容データによって規定される。体幹１２、右腕リンク２４および左腕リンク２２の動作は、ロボット６の力学モデルを用いて計算されるＺＭＰ軌道が、足先歩容データに基づく目標ＺＭＰ軌道に一致する関係に設定される。
なお本明細書で軌道とは、位置の経時的な変化を記述するデータのことをいう。
ロボット６の歩行を制御するために、ロボット６の体幹１２にコンピュータ装置１４が搭載されている。コンピュータ装置１４は、ＣＰＵ、ＲＯＭ、ＲＡＭ、ハードディスク等を有する。コンピュータ装置１４のハードウェア構成は汎用のコンピュータと同じであり、説明は省略する。コンピュータ装置１４は足先歩容データを記憶しており、それに基づいて動的な安定性を確保することが可能なロボット６の姿勢を逐次算出し、算出されたロボット６の姿勢を実現するように全身の関節群を制御する。
本実施例では、図１に示すように、ロボット６の歩行方向をｘ軸とし、体側方向をｙ軸とし、高さ方向をｚ軸とする。

図２は、コンピュータ装置１４の機能をブロック化して示す図である。コンピュータ装置１４は、足先歩容データ記憶部２０２、安定ＺＭＰ軌道計算部２０４、安定姿勢パターン計算部２０６、姿勢パターン指令値計算部２０８、移行姿勢パターン計算部２１２、目標姿勢パターン記憶部２１４、安定領域計算部２１６を備える。コンピュータ装置１４は、全体が物理的に１つの装置に含まれていてもよいし、物理的に分離された装置ごとに分けて収容されていてもよい。またコンピュータ装置１４の一部ないし全部をロボット外に搭載し、有線または無線によってロボット６のハードウェアと通信してもよい。

足先歩容データ記憶部２０２には、ロボット６の足先歩容データが記憶されている。足先歩容データは、ロボット６の右脚リンクおよび左脚リンクの足先の基準点の位置と姿勢の経時的変化を記述する。足先歩容データを参照することによって、脚リンクが接地するときの足平の位置・方位およびその脚リンクが接地する期間を特定することができる。

安定ＺＭＰ軌道計算部２０４は、ロボット６の足先歩容データに基づいて、安定して歩行を継続することが可能なＺＭＰ軌道（安定ＺＭＰ軌道）を計算する。
ロボット６のＺＭＰの位置は、ロボットの動作に伴って変化する。このＺＭＰが接地している脚リンクの足平の接地領域の内部に存在していれば、片方の脚リンクのみで接地している場合であっても、ロボット６は倒れることなく、安定して動作を継続することができる。
本実施例のロボット６では、安定ＺＭＰ軌道として、次のような軌道を設定する。ロボット６が一方の脚リンクのみで接地している間は、ＺＭＰは接地している脚リンクの足平の接地領域の中央に維持される。ロボット６が両方の脚リンクで接地している間は、ＺＭＰはそれまで支持脚だった脚リンクの足平の接地領域内から、新たに接地して支持脚となる脚リンクの足平の接地領域内へ移動する。
接地している脚リンクの足平の接地領域の中央にＺＭＰが存在している場合、ＺＭＰの位置が不意に変化した場合でも、ＺＭＰを足平の接地領域内に維持することが容易となる。従って、上記した安定ＺＭＰ軌道を実現する動作は、ロボット６に外乱等が作用してＺＭＰ位置が変化した場合であっても、歩行の安定性を確保することができ、歩行の安定性が高いといえる。
上記の安定ＺＭＰ軌道は、第１のＺＭＰ軌道に相当する。上記の安定ＺＭＰ軌道計算部２０４は、第１ＺＭＰ軌道計算部に相当する。

安定領域計算部２１６は、ロボット６の足先歩容データに基づいて、姿勢パターンの安定性の評価において基準として用いる安定領域を計算する。安定領域については、後に詳述する。

安定姿勢パターン計算部２０６は、安定ＺＭＰ軌道計算部２０４から入力される安定ＺＭＰ軌道を実現する姿勢パターン（安定姿勢パターン）を計算する。
まず安定姿勢パターン計算部２０６は、安定ＺＭＰ軌道計算部２０４から入力される安定ＺＭＰ軌道と、ロボット６の力学モデルに基づくＺＭＰ方程式を用いて、安定ＺＭＰ軌道 p_x, p_y を実現するロボット６の重心の運動量 P_gx, P_gy, P_gz と、重心まわりの角運動量 L_gx, L_gy, L_gzの経時的変化を計算する。本実施例のロボット６では、先ず重心まわりの角運動量 L_gx, L_gy, L_gz の経時的変化と、重心の鉛直方向軌道 z_g を、ユーザーからの指令等に基づいて決定する。そして、以下に示すＺＭＰ方程式を用いて重心の水平方向軌道 x_g, y_g を計算する。

ここで、m はロボット６の全質量であり、g は重心加速度である。また、式中の^（２）は時間に関する２階微分値を示す。上記のＺＭＰ方程式を離散化した３項方程式を解くことによって、水平方向軌道 x_g, y_g を計算することができる。上記のＺＭＰ方程式を用いた重心の水平方向軌道の計算は、本出願人が先に出願した特許出願である特願２００５−２８０３６号、特願２００５−６５８２７号の明細書に詳細に記述されている。ただしこれらの特許出願は、本願出願の時点で未だ公開されていないことに留意されたい。
重心の水平方向および垂直方向の軌道 x_g, y_g, z_g が計算されると、重心の位置の時間に関する一階微分を算出し、ロボット６の全質量 m との積を算出することによって、重心の運動量 P_gx, P_gy, P_gz の経時的変化を計算することができる。

その後、安定姿勢パターン計算部２０６は、計算された重心の運動量 P_gx, P_gy, P_gz および重心まわりの角運動量 L_gx, L_gy, L_gz の経時的変化を実現する、関節群の関節角 θ_i の経時的変化を計算する。
以下では関節群の関節角の経時的変化を計算する手法について説明する。重心の運動量 P_gx, P_gy, P_gz、重心の角運動量 L_gx, L_gy, L_gzは、関節角の角速度θ^（１）と、それぞれヤコビ行列J_g（θ）、K_g（θ）を用いて、以下で表現される。

J_g（θ）は重心の運動量に関するヤコビ行列であり、K_g（θ）は重心まわりの角運動量に関するヤコビ行列である。
上式の P_g はロボット６の重心の運動量（P_gx, P_gy, P_gz）であり、上式の L_gはロボット６の重心まわりの角運動量（L_gx, L_gy, L_gz）である。上式の関節角の角速度θ^（１）は関節角の角度θを時間に関して１階微分したものである。関節角の角度θは、以下に示すように、ロボットの関節角群（θ₁、θ₂、・・・、θ_n-1）である。

重心の運動量 P_g、重心まわりの角運動量 L_g は、既に計算されている。擬似逆行列を用いた解法を用いることで、上述した関係式より関節角の角速度θ^（１）の経時的な変化を計算することができる。

一般に、ロボット６の関節群は６自由度を超える自由度を備えており、目標とするロボットの運動P_g、L_g を満たす関節角の角速度θ^（１）は、複数の解を持つ冗長系となっている。
重心の運動量 P_g および重心まわりの角運動量 L_g が同一であっても、体幹１２に対して腕リンク２２，２４を振り出してバランスを取る姿勢を実現する解もあれば、体幹１２に対して腕リンク２２，２４を振り出すことなくバランスを取る姿勢を実現する解もある。
上述の計算では、目標とするロボットの運動 P_g、L_g を満たす１組の関節角の角速度θ^（１）の解が取得される。ここで取得される関節角の角速度θ^（１）に基づいてロボット６の関節群を駆動すると、実現されるＺＭＰは安定ＺＭＰ軌道に一致し、接地している脚リンクの足平の接地領域の中央部に維持されることになる。
上記の安定姿勢パターンは、第１の姿勢パターンに相当する。安定姿勢パターン計算部２０６は、第１重心運動計算部および第１姿勢パターン計算部に相当する。

目標姿勢パターン記憶部２１４は、ロボット６が目標とする姿勢パターンを記憶している。本実施例のロボット６では、目標姿勢パターンとして、体幹１２に対して腕リンク２２，２４を振り出さず、まっすぐに下げた状態を維持するパターンを扱う。このような姿勢パターンでは、体幹１２と腕リンク２２，２４を接続する肩関節は、その可動範囲の中立位置で維持される。その後の動作において、腕リンク２２，２４を振り出さなければ歩行の安定性を保てない状況となった場合でも、肩関節は可動範囲を確保されているため、腕リンク２２，２４を前後に振り出して歩行の安定性を保つことが可能となる。このように、関節の可動範囲の中立位置の近傍に維持することで、ロボット６の歩行を長期的に安定させることが可能である。
目標姿勢パターン記憶部２１４が記憶している目標姿勢パターンは、例えばロボット６が動作を実施する前に予め与えておいてもよいし、コントローラ等を用いてオペレータがロボット６の動作中にリアルタイムで与えてもよい。
上記の目標姿勢パターンは、第３の姿勢パターンに相当する。目標姿勢パターン記憶部２１４は、第３姿勢パターン記憶部に相当する。

移行姿勢パターン計算部２１２は、安定姿勢パターンから目標姿勢パターンへ移行する移行姿勢パターンを計算する。移行姿勢パターンは、安定姿勢パターンから徐々に目標姿勢パターンへ漸近するように設定される。本実施例のロボット６では、目標姿勢パターンから開始して、所定時間経過後に目標姿勢パターンへ一致するように、移行姿勢パターンを計算する。さらに本実施例のロボット６では、移行姿勢パターンを実現している間に、ロボット６の各部位の移動速度が所定の速度を越えることがないように、移行姿勢パターンが計算される。
計算された移行姿勢パターンは、姿勢パターン指令値計算部２０８へ出力される。
上記の移行姿勢パターンは、第２の姿勢パターンに相当する。姿勢パターン計算部２１２は、第２姿勢パターン計算部に相当する。

姿勢パターン指令値計算部２０８は、安定姿勢パターン計算部２０６から入力される安定姿勢パターンと、移行姿勢パターン計算部２１２から入力される移行姿勢パターンと、安定領域計算部２１６から入力される安定領域に基づいて、アクチュエータ群２１０へ出力する姿勢パターンの指令値を決定する。
まず姿勢パターン指令値計算部２０８は、上述した重心の運動量に関するヤコビ行列と、重心まわりの角運動量に関するヤコビ行列を用いて、移行姿勢パターンを実現した場合の重心の運動量と重心まわりの角運動量の経時的変化を計算する。重心の運動量と重心まわりの角運動量は、一意に計算することができる。
そして姿勢パターン指令値計算部２０８は、計算された重心の運動量と重心まわりの角運動量の経時的変化に基づき、上述したロボット６の力学モデルに基づくＺＭＰ方程式を用いて、移行姿勢パターンを実現した場合のＺＭＰ軌道（移行ＺＭＰ軌道）を計算する。
上記の移行ＺＭＰ軌道は、第２のＺＭＰ軌道に相当する。
そして姿勢パターン指令値計算部２０８は、計算された移行ＺＭＰ軌道が足平の安定領域に入っているか否かを評価する。

図４は足平の接地領域４０２と、安定領域４０６の関係を示している。安定ＺＭＰ軌道は、足平の接地領域４０２の中央部４０４に維持される。安定領域４０６は、足平の接地領域４０２の内部に含まれる領域であって、足平の接地領域４０２の中央部４０４を含む接地領域４０２より小さい領域として定義されている。
安定領域４０６の内部にＺＭＰが存在する場合、ロボット６に外乱が作用してＺＭＰ位置が予期せぬ変化をした場合でも、足平の接地領域４０２の内部にＺＭＰを維持することが可能である。したがって、安定領域４０６の内部にＺＭＰが存在する状態は、わずかな外乱が作用してもロボット６が倒れることがない、歩行の安定性が高い状態といえる。
一方、足平の接地領域４０２の内部で安定領域４０６の外部にＺＭＰが存在する場合、ロボット６に外乱が作用してＺＭＰ位置が予期せぬ変化をすると、足平の接地領域４０２の内部にＺＭＰを維持することが困難となる。したがって、安定領域４０６の外部にＺＭＰが存在する状態は、わずかな外乱が作用することによってロボット６の安定性が失われてしまう、歩行の安定性が低い状態といえる。
本実施例のロボット６は、上述した安定領域４０６を基準として、移行姿勢パターンを実現するときの動作の安定性を評価する。すなわち、移行ＺＭＰ軌道が常に安定領域に入っていれば、ロボット６は移行姿勢パターンを実現しても、高い安定性を維持することができると評価される。従って、このような場合には、アクチュエータ群２１０へ移行姿勢パターンが出力される。一方、移行ＺＭＰ軌道が安定領域から外れる場合には、ロボット６は移行姿勢パターンを実現した場合に、高い安定性を維持することができないと評価される。従って、このような場合には、アクチュエータ群２１０へ安定姿勢パターンが出力される。
姿勢パターン指令値計算部２０８は、第２重心運動計算部、第２ＺＭＰ軌道計算部および姿勢パターン選択部に相当する。

姿勢パターン指令値計算部２０８から出力される姿勢パターン（関節群の関節角の経時的変化）に基づいて、アクチュエータ群２１０が駆動されて、ロボット６は歩行動作を実現する。

図３はコンピュータ装置１４が実行する処理を説明するフローチャートである。
ステップＳ３１０では、足先歩容データ記憶部２０２に記憶されている足先歩容データを読込む。
ステップＳ３１２では、読込まれた足先歩容データに基づいて支持脚の足平の接地領域を特定し、安定領域を計算する。
ステップＳ３１４では、特定された足平の接地領域に基づいて、安定ＺＭＰ軌道を計算する。安定ＺＭＰ軌道は、足平の接地領域の中央部に維持されるように計算される。
ステップＳ３１６では、計算された安定ＺＭＰ軌道に基づいて、安定重心運動（安定ＺＭＰ軌道を実現する重心の運動量および重心まわりの角運動量の経時的変化）を計算する。
ステップＳ３１８では、計算された安定重心運動に基づいて、安定姿勢パターンを計算する。
ステップＳ３２０では、計算された安定姿勢パターンと、読み込まれた目標姿勢パターンに基づいて、移行姿勢パターンを計算する。
ステップＳ３２２では、計算された移行姿勢パターンに基づいて、移行重心運動（移行姿勢パターンによって実現される重心の運動量および重心まわりの角運動量の経時的変化）を計算する。
ステップＳ３２４では、移行重心運動に基づいて、移行ＺＭＰ軌道を計算する。
ステップＳ３２６では、計算された移行ＺＭＰ軌道が、安定領域に収まっているか否かを判断する。移行ＺＭＰ軌道が常に安定領域に収まっている場合（ステップＳ３２６でＹＥＳの場合）、処理はステップＳ３２８へ移行し、移行姿勢パターンをアクチュエータ群２１０への指令値として出力する。移行ＺＭＰ軌道の一部でも安定領域に収まっていない部分がある場合（ステップＳ３２６でＮＯの場合）、処理はステップＳ３３０へ移行し、安定姿勢パターンをアクチュエータ群２１０への指令値として出力する。
その後、処理はステップＳ３１０へ以降し、上述した処理を繰返し実行する。

上記した一連の処理を実行することによって、ロボット６はその後の動作の安定性を自律的に評価して、目標とする姿勢へ移行するための動作がそれほど安定でない場合には目標とする姿勢とは無関係に安定な姿勢を維持し、目標とする姿勢へ移行するための動作が十分に安定な場合には目標とする姿勢へ移行するように、その後の動作を行うことができる。

以上、本発明の実施形態について詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組み合わせによって技術的有用性を発揮するものであり、出願時請求項記載の組み合わせに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

図１は本発明のロボット６の概観を示す図である。図２は本発明のロボット６のコンピュータ装置１４の構成を示すブロック図である。図３は本発明のロボット６のコンピュータ装置１４が実施する処理を示すフローチャートである。図４は足平の接地領域４０２と安定領域４０６の関係を示す図である。

符号の説明

６・・・ロボット
１２・・・体幹
１４・・・コンピュータ装置
２０・・・左脚リンク
２２・・・左腕リンク
２４・・・右腕リンク
２６・・・右脚リンク
７，８，９，１０・・・足先基準点の軌道
２０２・・・足先歩容データ記憶部
２０４・・・安定ＺＭＰ軌道計算部
２０６・・・安定姿勢パターン計算部
２０８・・・姿勢パターン指令値計算部
２１０・・・アクチュエータ群
２１２・・・移行姿勢パターン計算部
２１４・・・目標姿勢パターン記憶部
４０２・・・足平の接地領域
４０４・・・足平の中央部
４０６・・・足平の安定領域
４０８，４１０・・・ＺＭＰの位置

Claims

体幹と、一方の端部が股関節を介して体幹に連結され、他方の端部が足首関節を介して足平を備える足先部に連結された脚リンクと、関節群を回転駆動するアクチュエータ群と、アクチュエータ群を制御するコントローラを備えるロボットであって、
前記コントローラは、
足先歩容データを記憶する足先歩容データ記憶部と、
足先歩容データに基づいて、足平の内部の安定領域を計算する安定領域計算部と、
足先歩容データに基づいて、第１のＺＭＰ軌道を計算する第１ＺＭＰ軌道計算部と、
第１のＺＭＰ軌道と、ＺＭＰ方程式に基づいて、第１のＺＭＰ軌道を実現する第１の重心の運動量および重心まわりの角運動量の経時的変化を計算する第１重心運動計算部と、
第１の重心の運動量および重心まわりの角運動量の経時的変化と、重心の運動量に関するヤコビ行列と、重心まわりの角運動量に関するヤコビ行列に基づいて、第１の重心の運動量および重心まわりの角運動量の経時的変化を実現する第１の姿勢パターンを計算する第１姿勢パターン計算部と、
第３の姿勢パターンを記憶する第３姿勢パターン記憶部と、
第１の姿勢パターンと第３の姿勢パターンに基づいて、第１の姿勢パターンから第３の姿勢パターンへ移行する第２の姿勢パターンを計算する第２姿勢パターン計算部と、
第２の姿勢パターンと、重心の運動量に関するヤコビ行列と、重心まわりの角運動量に関するヤコビ行列に基づいて、第２の姿勢パターンによって実現される第２の重心の運動量および重心まわりの角運動量の経時的変化を計算する第２重心運動計算部と、
第２の重心の運動量および重心まわりの角運動量の経時的変化と、ＺＭＰ方程式に基づいて、第２の重心の運動量および重心まわりの角運動量の経時的変化によって実現される第２のＺＭＰ軌道を計算する第２ＺＭＰ軌道計算部と、
第１の姿勢パターンと、第２の姿勢パターンと、足平の内部の安定領域と、第２のＺＭＰ軌道に基づいて、姿勢パターンを選択してアクチュエータ群へ出力する姿勢パターン選択部と
を備え、前記姿勢パターン選択部は、
第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まる場合に、アクチュエータ群へ第２の姿勢パターンを出力し、
第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まらない場合に、アクチュエータ群へ第１の姿勢パターンを出力し、
前記第２姿勢パターン計算部は、前記姿勢パターン選択部において第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まると判断されるまで、異なる第２の姿勢パターンを繰り返し計算することを特徴とするロボット。
体幹と、一方の端部が股関節を介して体幹に連結され、他方の端部が足首関節を介して足平を備える足先部に連結された脚リンクと、関節群を回転駆動するアクチュエータ群と、アクチュエータ群を制御するコントローラを備えるロボットの動作を制御する方法であって、
足先歩容データを読込む工程と、
足先歩容データに基づいて、足平の内部の安定領域を計算する工程と、
足先歩容データに基づいて、第１のＺＭＰ軌道を計算する工程と、
第１のＺＭＰ軌道と、ＺＭＰ方程式に基づいて、第１のＺＭＰ軌道を実現する第１の重心の運動量および重心まわりの角運動量の経時的変化を計算する工程と、
第１の重心の運動量および重心まわりの角運動量の経時的変化と、重心の運動量に関するヤコビ行列と、重心まわりの角運動量に関するヤコビ行列に基づいて、第１の重心の運動量および重心まわりの角運動量の経時的変化を実現する第１の姿勢パターンを計算する工程と、
第３の姿勢パターンを読込む工程と、
（Ａ）第１の姿勢パターンと、第３の姿勢パターンに基づいて、第１の姿勢パターンから第３の姿勢パターンへ移行する第２の姿勢パターンを計算する工程と、
（Ｂ）第２の姿勢パターンと、重心の運動量に関するヤコビ行列と、重心まわりの角運動量に関するヤコビ行列に基づいて、第２の姿勢パターンによって実現される第２の重心の運動量および重心まわりの角運動量の経時的変化を計算する工程と、
（Ｃ）第２の重心の運動量および重心まわりの角運動量の経時的変化と、ＺＭＰ方程式に基づいて、第２の重心の運動量および重心まわりの角運動量の経時的変化によって実現される第２のＺＭＰ軌道を計算する工程と、
（Ｄ）第１の姿勢パターンと、第２の姿勢パターンと、足平の内部の安定領域と、第２のＺＭＰ軌道に基づいて、第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まる場合に、アクチュエータ群へ第２の姿勢パターンを出力し、第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まらない場合に、アクチュエータ群へ第１の姿勢パターンを出力する工程と、
前記第２のＺＭＰ軌道が足平の内部の安定領域の内部に収まると判断されるまで、異なる第２の姿勢パターンについて、少なくとも上記（Ａ）から（Ｄ）までの工程を繰り返し実行する工程を備えることを特徴とする方法。