JP2009195999A - 脚式ロボット、その制御方法、その制御システム - Google Patents

Abstract

【課題】パラメータ調整などの不要な処理を実行すること無く、姿勢角の安定した重心軌道データを容易に計算することができ、自然な姿勢で歩行／走行動作を実現可能な脚式ロボット、その制御方法、その制御システムを提供する。
【解決手段】本発明にかかる脚式ロボット２は、関節角を変更して動作する脚式ロボット２であって、脚式ロボット２に指令する目標速度に応じて、脚式ロボット２の上体の姿勢角が変動しない脚式ロボット２の重心周りの回転運動量を計算する手段２１と、計算された回転運動量を用いてＺＭＰ方程式を求解することで、脚式ロボット２の重心軌道を計算する手段３０と、計算された重心軌道に基づいて、関節を回転する手段（４０、５０）とを備えるものである。
【選択図】図２

Description

本発明は関節角を変更して動作する脚式ロボット、その制御方法、その制御システムに関する。

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

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

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

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

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

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

上述した問題点を鑑みて、本出願人は先に出願した出願（特願２００７―０８５８５号）において、上体が自然な動きとなる軌道を計算可能な脚式ロボット、及びその制御方法を提案している。出願（特願２００７―０８５８５号）において提案される脚式ロボット、及びその制御方法では、角運動量を重心の移動速度に比例するものとし、角運動量と重心速度との関係を規定する角運動量パラメータを用いて重心軌道を計算するものである。出願（特願２００７―０８５８５号）において提案される脚式ロボット、及びその制御方法によれば、例えばロボットの重心が左に動いた場合には、その重心の移動に応じて上体も左に回転するため、計算された軌道に基づくロボットの動作が不自然な動作となることを防ぐことができる。

しかしながら、出願（特願２００７―０８５８５号）において提案される脚式ロボット、及びその制御方法では、角運動量パラメータなどについて、ロボットの姿勢を確認しながら事前に最適な値を調整する必要がある。また、ロボットの重量配分や、重心位置によってもパラメータが変化するため、ロボット毎にパラメータの再調整が必要となり、手間が掛かるという課題を残していた。また、出願（特願２００７０８５８５号）において提案される脚式ロボット、及びその制御方法では、事前に設定した回転運動量からロボットの上体姿勢がなりゆきで決定されるため、ロボットの速度が変化する場合などには、その変化に対応した回転運動量で、自然な姿勢角となる重心軌道を良好に計算することができない場合があった。

このように、角運動量を０として扱う従来の動作においては、ロボットの上体が不自然な動きになるという問題点があった。また、パラメータの再調整などの不要な処理が発生すると共に、速度が変化する場合などの過渡的な状況に良好に対応することができない場合があった。

本発明は、かかる課題を解決するためになされたものであり、パラメータ調整などの不要な処理を実行すること無く、姿勢角の安定した重心軌道データを容易に計算することができ、自然な姿勢で歩行／走行動作を実現可能な脚式ロボット、その制御方法、その制御システムを提供することを目的とする。

本発明にかかる脚式ロボットは、関節角を変更して動作する脚式ロボットであって、前記脚式ロボットに指令する目標速度に応じて、前記脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算する手段と、計算された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算する手段と、計算された前記重心軌道に基づいて、前記関節を回転する手段とを備えるものである。

このように、目標速度に応じて、上体の姿勢角が変動せずに安定する場合の回転運動量を計算し、その回転運動量を用いて重心軌道を計算することで、パラメータ調整などの不要な処理を実行すること無く、姿勢角の安定した重心軌道データを容易に計算することができ、自然な姿勢で歩行／走行動作を実現することができる。また、目標速度に応じて、最適な回転運動量を計算するため、ロボットの速度が変化する過渡的な状況においても、自然な姿勢となる重心軌道を計算することができる。

また、計算された前記回転運動量を記憶する手段を更に備え、前記重心軌道を計算する手段は、記憶された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算するようにしてもよい。これにより、重心軌道の計算処理では、記憶された回転運動量を用いてＺＭＰ方程式を求解することができるため、回転運動量の計算処理回数を削減することができる。このため、自然な姿勢を実現するための回転運動量の計算処理を実行する場合であっても、計算量の増加を抑制しつつ、姿勢角の安定した重心軌道データを容易に計算することができる。

さらにまた、前記回転運動量を計算する手段は、所定の周期単位の前記脚式ロボットの重心周りの回転運動量を計算して、前記回転運動量を記憶する手段に記憶し、前記重心軌道を計算する手段は、前記目標速度に応じて、多項式補間処理により、記憶された前記所定の周期単位の回転運動量から前記周期間の回転運動量を計算して、計算された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算するようにしてもよい。これにより、記憶する回転運動量のデータ量をより削減することができると共に、一歩時間が異なる場合であっても所望の回転運動量を容易に取得することができる。

また、前記回転運動量を計算する手段は、前記脚式ロボットに指令する目標速度と一歩の継続時間とに応じて、前記脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算するようにしてもよい。

さらにまた、前記回転運動量を計算する手段は、前記脚式ロボットに指令する目標速度と、一歩の継続時間と、前記脚式ロボットの状態が遷移する遷移時間とに応じて、前記脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算するようにしてもよい。

また、前記回転運動量を計算する手段は、前記脚式ロボットに指令する目標速度に応じた、前記脚式ロボットの、左足平位置と右足平位置と体幹姿勢を示すデータと、前記位置姿勢を実現する体幹位置を示すデータを入力して記憶する手段と、前記データ群から、前記脚式ロボットをモデル化した「左膝位置と右膝位置と左足平近傍位置と右足平近傍位置に質量が集中して存在する力学系」を用いて、各質点の位置及び速度を計算して、前記脚式ロボットの重心周りの回転運動量を計算する手段を備えるようにしてもよい。

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

本発明にかかる脚式ロボットの制御方法は、指令する目標速度に応じて、脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算するステップと、計算された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算するステップと、計算された前記重心軌道に基づいて、前記脚式ロボットの関節を回転するステップとを備えるものである。

本発明にかかる脚式ロボットの制御システムは、関節角を変更して動作する脚式ロボットの制御システムであって、前記脚式ロボットに指令する目標速度に応じた、前記脚式ロボットの、左足平位置と右足平位置と体幹姿勢を示すデータと、前記位置姿勢を実現する体幹位置を示すデータを入力して記憶する入力データ記憶部と、前記データ群から、前記脚式ロボットをモデル化した「左膝位置と右膝位置と左足平近傍位置と右足平近傍位置に質量が集中して存在する力学系」を用いて、各質点の位置及び速度を計算して、前記脚式ロボットの重心周りの回転運動量を計算する回転運動量計算部と、計算された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算する重心軌道計算部と、計算された前記重心軌道に基づいて、前記関節を回転する関節駆動部とを備えるものである。

本発明によれば、パラメータ調整などの不要な処理を実行すること無く、姿勢角の安定した重心軌道データを容易に計算することができ、自然な姿勢で歩行／走行動作を実現可能な脚式ロボット、その制御方法、その制御システムを提供することを目的とする。

発明の実施の形態１．
本実施の形態１に係る脚式ロボットは、まず、指令する目標速度に応じて、脚式ロボットの上体の姿勢角が変動しない重心周りの回転運動量を計算する。次いで、脚式ロボットは、計算された回転運動量を用いてＺＭＰ方程式を求解することで、ロボットの重心軌道を計算する。そして、脚式ロボットは、計算された重心軌道に基づいて、関節角を変更して動作する。

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

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

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

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

続いて、制御部１０の動作の詳細について説明する。図２は、制御部１０の機能の構成を示す機能ブロック図である。制御部１０は、回転運動量計算部２１と、回転運動量記憶部２２と、軌道計算部３０と、目標姿勢計算部４０と、関節駆動部５０を備えている。ここでは重心の軌道を計算して生成することから、計算、生成、演算の語を区別なく用いる。

回転運動量計算部２１は、ユーザーがロボット２に指令する目標速度に応じて、ロボット２の上体４の姿勢角が変動しないロボット２の重心周りの回転運動量を計算する。計算された回転運動量は、回転運動量記憶部２２に記憶される。回転運動量の計算方法の詳細については後述する。

軌道計算部３０は、目標速度決定部３１と、目標回転運動量決定部３２と、仮重心軌道計算部３３と、ＺＭＰ補償計算部３４と、遊脚軌道計算部３７とを備える。軌道計算部３０は、回転運動量記憶部２２に記憶された回転運動量を用いてＺＭＰ方程式を求解することで、ロボット２の重心軌道及び遊脚の足先軌道を計算する。目標姿勢計算部４０は、計算された重心軌道及び遊脚軌道を実現する、ロボット２の目標姿勢を計算する。関節駆動部５０は、計算された目標姿勢に基づいて、ロボット２の各関節を駆動する。

続いて、回転運動量を計算する方法について説明する。図３を参照して、ロボット２が上体姿勢を安定して動作可能な回転運動量の計算方法について説明する。この回転運動量計算部２１は、ロボット２の起動時に用いられ、ロボット２の上体を安定させながら動作可能な回転運動量を計算する。この回転運動量計算部２１で計算された回転運動量データは、回転運動量記憶部２２に記憶される。後述するように、ロボット２は、記憶された回転運動量データを用いて、歩行するための軌道データを計算する。回転運動量計算部２１は、十分に高速に回転運動量を計算することができる。例えば、一歩の継続時間が１秒程度である場合に、その一歩分の回転運動量を５ｍｓｅｃ以下で計算することができ、２秒では４００ケース以上の回転運動量を計算することができる。ロボット２が歩行している様子を見ながら、ユーザーがジョイスティック等を使って次の一歩の足の運びを指定してから、次の一歩を実際に実施するまでの短時間の間に、次の一歩のための回転運動量データを作成することができるために、実質的にはリアルタイムで回転運動量データを作成することができる。尚、ロボット２の起動時に回転運動量を計算し、以後は、周期運動として回転運動量計算処理を繰り返すようにしてもよい。

まず、ユーザーが、回転運動量計算部２１に、ロボット２の目標速度に応じた、ロボット２を歩行させたいコース等のデータを入力する。具体的には、図３の左下の（Ａ）に示される、５個のベクトルと１個のスカラ量を入力する。ベクトルＣは左足平位置ベクトルであり、座標原点Ｏから見たロボット２の左足平位置を示す。ベクトルＤは左足平姿勢ベクトルであり、左足平の向きを示す。ベクトルＥは右足平位置ベクトルであり、座標原点Ｏから見たロボット２の右足平位置を示す。ベクトルＦは右足平姿勢ベクトルであり、右足平の向きを示す。ベクトルＲは体幹姿勢ベクトルであり、ロボット２の体幹の向きを示す。人間の背骨に相当する部材が伸びる方向を示す。ロボット２の胴部が腰部（下胴部）と上胴部に分離されている場合には、腰部が体幹に相当し、体幹姿勢ベクトルＲは腰部に垂直な方向を定義する。Ｑは、体幹高さであり、人間で言う腰位置の高さに相当する。ユーザーは、これらの５種のベクトルと１種のスカラ量を時系列に従って次々に入力する。これら５種のベクトルと１種のスカラ量は、ロボット２の目標速度を実現するように計算されたものであると共に、ロボット２の上体の姿勢角が変動しないように計算されたものである。

また、これらの５種のベクトルと１種のスカラ量を時系列に従って入力する場合には、これら５種のベクトルと１種のスカラ量は、ロボット２の一歩の継続時間に応じた時系列データを入力してもよいし、ロボット２の状態が遷移する遷移時間に応じた時系列データを入力するようにしてもよい。ここで、遷移時間とは、ロボット２の両脚が床面から離床している期間である空中相へと遷移するための時間、及び両脚により床面に着床している両脚期へと遷移するための時間である。尚、このためのデータ入力支援ソフトが開発されており、主要タイミングでのデータを指定することで、時系列での中間データが補完計算されるようになっている。また、一歩分の入力データを繰り返して利用するように指示することで、入力量を低減することができる。

さらにまた、ユーザーがジョイスティック等で歩行コースや目標速度等を指示することもある。一歩分の標準的な足平姿勢ベクトルの変化パターンと体幹姿勢ベクトルの変化パターンと体幹高さの変化パターンを記憶しておくと、ユーザーがジョイスティック等で歩行コースや目標速度を指示することによって、一歩分の５種ベクトルＣ（ｔ），Ｄ（ｔ），Ｅ（ｔ），Ｆ（ｔ），Ｒ（ｔ）と１種スカラ量Ｑ（ｔ）を生成して記憶することができる。回転運動量データを作成するのに必要な５種のベクトルＣ（ｔ），Ｄ（ｔ），Ｅ（ｔ），Ｆ（ｔ），Ｒ（ｔ）と１の種スカラ量Ｑ（ｔ）は、回転運動量計算部２１に入力されて記憶装置２９２に記憶される。

ユーザーは、更に、目標ＺＭＰ位置ベクトルを時系列に従って入力する。この場合、ベクトルＣ乃至Ｆによって、左足接地状態・両足接地状態・右足接地状態の別がわかっていることから、左足接地状態では左足接地面内の位置を指定し、右足接地状態では右足接地面内の位置を指定する。ベクトルＣ乃至Ｆを用いることによって、目標ＺＭＰ位置ベクトル（ＺＭＰ＊ベクトル）を指定することができる。歩行時に観測される実際ＺＭＰが目標ＺＭＰに一致するように制御することによって、ロボット２は転倒しないで歩行することが可能となる。

回転運動量計算部２１は、まず、ユーザーが入力した５種のベクトルと１種のスカラ量に基づいて、実際に生じるであろうＺＭＰを計算して目標ＺＭＰとの偏差を求め、そのＺＭＰ偏差から目標ＺＭＰをもたらす体幹位置ベクトルＰを計算する。そして、回転運動量計算部２１は、ユーザーが入力した５種のベクトルと１種のスカラ量と、計算された体幹位置ベクトルＰとに基づいて、回転運動量データを計算する。手段２９４は、５種のベクトルと１種のスカラ量で定義されるロボット２の位置と姿勢を実現する一つの体幹位置を示すデータを生成する。５種のベクトルと１種のスカラ量で定義されるロボット２の位置と姿勢を実現する体幹位置は複数存在するので一意に決定することができない。ここでは任意の一個を採用する。体幹位置ベクトルＰを生成すると、６種のベクトルＣ，Ｄ，Ｅ，Ｆ，Ｐ，Ｒが揃うので、ロボット２の位置と姿勢が一意に決定される。

ここで用いる体幹ベクトルＰは、仮に用いられるものであり、後述する手段３１０によって補正されるので、ラフなものでよい。人が指定して回転運動量計算部２１に入力して記憶してもよいし、回転運動量計算部２１で生成してもよい。

体幹位置ベクトルＰに対する、体幹姿勢ベクトルＲや足先位置ベクトルＣ，Ｅや足先姿勢ベクトルＤ，Ｆや体幹高さＱや目標ＺＭＰベクトルの変化パターンを規格化しておくことができる。一歩分の変化パターンが規格化されていれば、人が体幹位置ベクトルＰを指定することによって、回転運動量計算部２１が必要とする５種のベクトルと１種のスカラ量を入力することができる。体幹位置ベクトルＰはジョイスティック等を利用して指定することができる。歩行するロボット２を観測しながらジョイスティック等を操作して体幹位置ベクトルＰをリアルタイムで入力することが可能である。体幹位置ベクトルＰをリアルタイムで入力することができ、回転運動量計算部２１が必要とする５種のベクトルと１個のスカラ量をリアルタイムで入力することができ、回転運動量計算部２１がリアルタイムの制御に間に合うほど高速に回転運動量データを計算できれば、ロボット２の歩行軌道をリアルタイムで指定することができる。

体幹の位置Ｐと姿勢Ｒが決定されれば、左右の股関節の位置が決定する。左足平位置Ｃと左足平姿勢Ｄが決定されれば、左足首の位置が決定する。左股関節の位置と左足首の位置が決定されれば、左膝の位置Ｇが決定される。左腿の長さと左脛の長さが既知であり、左膝関節の回転自由度が１自由度しかないことから、左股関節の位置と左足首の位置が決まれば左膝の位置Ｇが一意に決定される。同様に、右足平位置Ｅと右足平姿勢Ｆから、右足首の位置が決定する。右股関節の位置と右足首の位置から、右膝の位置Ｈが決定される。手段２９６は、上記の幾何学的関係を利用して、左膝位置ベクトルＧと右膝位置ベクトルＨを計算する。手段２９８では、ロボット２のＺＭＰ位置を計算する。ここでは、図４に示した６質点系モデルを用いる。

６質点を持つ力学系では、左足平近傍位置に、左脛下半分３２４の質量と左足首３２２の質量と左足平３２０の質量の合計質量Ｍ１が集中して存在する。右足平近傍位置に、右脛下半分３４０の質量と右足首３３８の質量と右足平３３６の質量の合計質量Ｍ３が集中して存在する。左膝位置に、左脛上半分３２６の質量と左膝関節３２８の質量と左腿下半分３３０の質量の合計質量Ｍ２が集中して存在する。右膝位置に、右脛上半分３４２の質量と右膝関節３４４の質量と右腿下半分３４６の質量の合計質量Ｍ４が集中して存在する。体幹下部基準点に、左腿上半分３３２の質量と左股関節３３４の質量と右腿上半分３４８の質量と右股関節３５０の質量と体幹下半分３５２の質量の合計質量Ｍ５が集中して存在する。体幹上部基準点に、体幹上半分３５４の質量Ｍ６が集中して存在するものとする。

質点Ｍ１の位置は、左足平近傍位置にあればよく、左足首関節位置からずれていてもよく、左足平位置ベクトルＣの終点からずれていてもよい。質点Ｍ３の位置は、右足平近傍位置にあればよく、右足首関節位置からずれていてもよく、右足平位置ベクトルＥの終点からずれていてもよい。これに対し、質点Ｍ２の位置は、左膝関節置にあるとするのが極めて効果的であり、左膝関節置に質量が集中して存在しているとすることによって、ＺＭＰ位置の計算が極めて簡単化され、体幹位置の補正量の計算が極めて簡単化される。同様に、質点Ｍ４の位置は、右膝関節置にあるとするのが極めて効果的である。質点Ｍ５とＭ６は人間の背骨上に存在している。その高さは、集中質量Ｍ５とＭ６を用いて体幹の水平軸回りの慣性モーメントを計算した値と、実際の体幹の水平軸回りの慣性モーメントが一致する高さに設定しておくことが好ましい。

図３の手段２９８では、手段２９６で決定されるロボット２の位置と姿勢の変化から、質点Ｍ１乃至Ｍ６の速度と加速度と、集中質量Ｍ５とＭ６による水平軸回りの慣性モーメントを利用して、ロボット２に生じるＺＭＰ位置を計算する。

図３に示す回転運動量計算部２１は、目標ＺＭＰ位置を示すデータを入力して記憶する手段２９２を備えており、手段２９８で計算されたＺＭＰ位置と、手段２９２に記憶されている目標ＺＭＰ位置との偏差を計算することができる（手段３００）。

回転運動量計算部２１は、手段３００で計算されたＺＭＰ偏差を解消するのに必要な体幹位置ベクトルの補正量を計算して、目標ＺＭＰ位置と等しいＺＭＰ位置が計算される体幹位置ベクトルＰを計算する手段３１０を備えている。手段３１０は、体幹位置を示すデータをどれだけ補正すると左膝位置と右膝位置を示すデータがどれだけ変化してＺＭＰ位置を示すデータがどれだけ変化するか示関係を利用して体幹位置を示すデータの補正量を求める。補正手段３１０によると、繰返し計算して補正量を収束させる必要がなく、一度の補正でＺＭＰ偏差を解消することができる。体幹位置の補正量から左膝位置と右膝位置の移動量を計算するにあたって、体幹位置の補正量ΔＰに係数をかけることによって左膝位置と右膝位置の移動量を計算するようにすると計算が一層に簡単化される。体幹高さと膝位置高さの比例係数を利用することによって、体幹位置の補正量ΔＰから左膝位置と右膝位置の移動量を簡単に計算することができる。

上述したようにして、目標ＺＭＰ位置にほぼ一致するＺＭＰ位置が計算されるが、体幹位置ベクトルＰを１回補正しただけでは、なおもＺＭＰ偏差が十分に解消されない場合がある。この場合、手段２９８以降を再度活用して２回補正すると、ＺＭＰ偏差をさらに解消することができる。このように、目標ＺＭＰによく追従する体幹位置ベクトルＰを含む歩容データを、少ない計算量で短時間で計算できることが検証されている。

手段３１２では、ロボット２に指令する目標速度と、一歩の継続時間と、ロボット２の状態が遷移する遷移時間とに応じて計算された歩容データ（５種のベクトルと１種のスカラ量と、計算された体幹位置ベクトルＰを含む）に基づいて、ロボット２の上体の姿勢角が変動しないロボット２の重心周りの回転運動量を計算する。手段３１２は、これら歩容データから、各質点の位置及び速度を計算して、各質点の中心周りの角運動量を計算する。手段３１２は、各質点の中心位置と重心位置との位置関係を用いて、各質点の中心周りの角運動量から、ロボット２の重心周りの回転運動量を計算する。

制御部１０は、このようにして計算された回転運動量を、回転運動量記憶部２２に記憶する。記憶する回転運動量データは、所定の周期単位ごとに離散化された回転運動量データとして記憶してもよい。

軌道計算部３０は、回転運動量を用いてＺＭＰ方程式を求解することで、ロボット２の重心軌道及び遊脚の足先軌道を計算する。まず、目標速度決定部３１は、ユーザーがロボット２に指令する目標速度に応じて、ロボット２の目標速度を決定し、目標回転運動量決定部３２及び仮重心軌道計算部３３へと目標速度を出力する。目標速度は、ロボット２のアクチュエータの性能が許す範囲で、ユーザーが任意に与えることができる。

目標回転運動量決定部３２は、入力された目標速度に応じて、回転運動量記憶部２２に記憶された回転運動量を読み出す。目標回転運動量決定部３２は、入力された目標速度と、一歩の継続時間と、遷移時間などの走行条件に応じて、現在の走行状態に最も近い状況で計算された回転運動量を読み出す。所定の周期単位で離散化された回転運動量データが回転運動量記憶部２２に記憶されている場合には、目標回転運動量決定部３２は、記憶されている所定の周期単位の回転運動量データから、多項式補間処理により、それら周期間の回転運動量を計算する。ロボット２の重心周りの回転運動量は、一歩の間に大きく変動することは少ないため、所定の周期単位で離散化されたデータのみを記憶することで、記憶する回転運動量のデータ量をより削減することができると共に、一歩の継続時間が異なる場合であっても所望の回転運動量を柔軟に取得することができる。

仮重心軌道計算部３３は、入力された目標速度に応じて、ロボット２の仮の重心軌道を計算する。重心の相対軌道は、目標とする相対ＺＭＰ軌道と、重心周りの回転運動量と、動作の開始時及び完了時における重心の水平方向速度（目標速度）の条件に基づいて決定される。重心の相対軌道を実現したときの実際の相対ＺＭＰ軌道が、目標とする相対ＺＭＰ軌道に一致するように、重心の相対軌道を計算する。

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

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

ここで、^（１）は時間ｔに関する１階微分を示し、^（２）は時間ｔに関する２階微分を示している。また、ｍはロボット２の質量であり、ｇは重力加速度である。上式のｚ'及びｚ'^（２）は、ロボット２の支持脚の足先を原点とする、重心の鉛直方向位置と鉛直方向加速度である。上式のｒ_ｘ' ^（１）とｒ_ｙ' ^（１）は、重心まわりの回転運動量の微分成分を示す。

仮重心軌道計算部３３では、ロボット２の重心まわりの回転運動量（ｒ_ｘ'，ｒ_ｙ'）の時系列的変化と、重心の鉛直方向軌道ｚ'を、ユーザーからの指令等に基づいて決定する。そして、上記のＺＭＰ方程式を求解することで、ロボット２の重心の水平方向軌道（ｘ'，ｙ'）を計算する。尚、上記のＺＭＰ方程式を離散化した３項方程式を解くことによって、ロボット２の重心の水平方向軌道（ｘ'，ｙ'）を計算することができる。重心軌道を高速に計算するため、例えば、鉛直方向軌道及び計算された回転運動量に基づいて計算される係数を持つ行列と、重心軌道の列と、目標ＺＭＰの列との間に成立する連立方程式を解いて重心軌道を計算するようにしてもよい。この３項方程式を用いた重心の水平方向軌道の計算方法は、本出願人が先に出願した特願２００７―０８５８５号の明細書に記載されている。但し、この特許出願は、本出願の時点で未だ公開されていないことに留意されたい。

ＺＭＰ補償計算部３４の重心軌道計算部３５は、目標回転運動量決定部３２から入力される目標回転運動量と、仮重心軌道計算部３３において計算された仮重心軌道とから、ロボット２の重心を実際に追従させる重心軌道を計算する。重心軌道計算部３５は、重心の鉛直方向軌道ｚ'を、ユーザーからの指令等に基づいて決定し、目標回転運動量決定部３２から入力される目標回転運動量と、仮重心軌道計算部３３において計算された仮重心位置とから、上記のＺＭＰ方程式を解いてＺＭＰ位置を計算する。そして、重心軌道計算部３５は、計算されたＺＭＰ位置と、手段２９２に記憶されている目標ＺＭＰ軌道との偏差を計算する。重心軌道計算部３５は、計算されたＺＭＰ偏差を解消するのに必要な重心位置の補正量を計算して、目標ＺＭＰ位置と等しいＺＭＰ位置が計算される重心位置を計算する。このように計算された重心軌道を実現することで、ロボット２はＺＭＰを支持脚の足平の内部に維持しながら、自然な姿勢で動作を実現することができる。

ＺＭＰ補償計算部３４の着地位置計算部３６は、計算された複数歩数分の重心軌道から、遊脚の足先の着地位置を計算する。遊脚軌道計算部３７は、計算された足先の着地位置から、遊脚の足先の相対軌道を計算する。遊脚の足先の相対軌道は、動作の前後における脚リンクの動作と滑らかに接続するように計算する。遊脚の足先の相対軌道の計算は、例えば多項式補間を用いることによって、計算することができる。

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

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

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

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

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

演算された関節角目標値の経時的データは、制御部１０に記憶されている。例えば、一歩分相当の関節角目標値の経時的データが計算され記憶される。制御部１０は上記した処理と並列して、関節角目標値の経時的データに基づいて、関節駆動部５０でロボット２の各関節を逐次駆動していく。これによって、ロボット２はユーザー指令値に基づいた、姿勢の安定した動作を実現していく。

以上説明したように、本実施の形態１に係るロボット２は、ロボット２に指令する目標速度に応じて、ロボット２の上体の姿勢角が変動しない重心周りの回転運動量を計算する。そして、計算された回転運動量を記憶しておき、記憶された回転運動量を用いてＺＭＰ方程式を求解することで、ロボット２の重心軌道を計算する。ロボット２は、記憶された回転運動量を用いてＺＭＰ方程式を求解することで、ロボット２の重心軌道を計算し、計算された重心軌道に基づいて、関節を回転する。

このように、目標速度に応じて、上体の姿勢角が変動せずに安定する場合の回転運動量を計算し、その回転運動量を用いて重心軌道を計算することで、パラメータ調整などの不要な処理を実行すること無く、姿勢角の安定した重心軌道データを容易に計算することができ、自然な姿勢で歩行／走行動作を実現することができる。また、重心軌道の計算処理においては、記憶された回転運動量を用いてＺＭＰ方程式を求解することで、回転運動量の計算処理回数を削減することができる。従って、このような回転運動量の計算処理を実行する場合であっても、計算量の増加を抑制しつつ、姿勢角の安定した重心軌道データを容易に計算することができる。

その他の実施の形態．
上述した実施の形態１においては、回転運動量計算部２１が、６質点モデルを用いて、上体の姿勢角が変動しない回転運動量を計算するものとしたが本発明はこれに限定されず、例えば、１２関節、１３リンクの力学系を用いて回転運動量を計算するようにしてもよい。

また、上述した実施の形態１においては、軌道計算部３０が一歩相当の軌道データを計算するものとしたが本発明はこれに限定されず、計算する軌道データは、一歩部相当分よりも短い時間区分に対応する部分の軌道データを取得するようにしてもよい。

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

本発明の実施の形態にかかるロボットの構成図である。 本発明の実施の形態にかかるロボットの制御部の構成を示す機能ブロック図である。 本発明の実施の形態に係る回転運動量計算部の構成を示す機能ブロック図である。 本発明の実施の形態に係る６質点系モデルを示す概念図である。

符号の説明

２ 脚式ロボット、４ 体幹、６ 左脚リンク、８ 右脚リンク、１０ 制御部、
１２ コントローラ、
２１ 回転運動量計算部、２２ 回転運動量記憶部、３０ 軌道計算部、
３１ 目標速度決定部、３２ 目標回転運動量決定部、３３ 仮重心軌道計算部、
３４ ＺＭＰ補償計算部、３５ 重心軌道計算部、３６ 着地位置計算部、
３７ 遊脚軌道計算部、４０ 目標姿勢計算部、５０ 関節駆動部、
３２８ 左膝関節、３３４ 左股関節、３４４ 右膝関節、３５０ 右股関節、
３２０ 左足平、３２２ 左足首、３２４ 左脛下半分、３２６ 左脛上半分、
３３０ 左腿下半分、３３２ 左腿上半分、３３６ 右足平、３３８ 右足首、
３４０ 右脛下半分、３４２ 右脛上半分、３４６ 右腿下半分、３４８ 右腿上半分、
３５２ 体幹下半分、３５４ 体幹上半分

Claims (15)

1. 関節角を変更して動作する脚式ロボットであって、
   前記脚式ロボットに指令する目標速度に応じて、前記脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算する手段と、
   計算された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算する手段と、
   計算された前記重心軌道に基づいて、前記関節を回転する手段と
   を備える脚式ロボット。
2. 計算された前記回転運動量を記憶する手段を更に備え、
   前記重心軌道を計算する手段は、
   記憶された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算する
   ことを特徴とする請求項１記載の脚式ロボット。
3. 前記回転運動量を計算する手段は、
   所定の周期単位の前記脚式ロボットの重心周りの回転運動量を計算して、前記回転運動量を記憶する手段に記憶し、
   前記重心軌道を計算する手段は、
   前記目標速度に応じて、多項式補間処理により、記憶された前記所定の周期単位の回転運動量から前記周期間の回転運動量を計算して、
   計算された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算する
   ことを特徴とする請求項２記載の脚式ロボット。
4. 前記回転運動量を計算する手段は、
   前記脚式ロボットに指令する目標速度と一歩の継続時間とに応じて、前記脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算する
   ことを特徴とする請求項１乃至３いずれか１項記載の脚式ロボット。
5. 前記回転運動量を計算する手段は、
   前記脚式ロボットに指令する目標速度と、一歩の継続時間と、前記脚式ロボットの状態が遷移する遷移時間とに応じて、前記脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算する
   ことを特徴とする請求項１乃至４いずれか１項記載の脚式ロボット。
6. 前記回転運動量を計算する手段は、
   前記脚式ロボットに指令する目標速度に応じた、前記脚式ロボットの、左足平位置と右足平位置と体幹姿勢を示すデータと、前記位置姿勢を実現する体幹位置を示すデータを入力して記憶する手段と、
   前記データ群から、前記脚式ロボットをモデル化した「左膝位置と右膝位置と左足平近傍位置と右足平近傍位置に質量が集中して存在する力学系」を用いて、各質点の位置及び速度を計算して、前記脚式ロボットの重心周りの回転運動量を計算する手段
   を備えることを特徴とする請求項１乃至５いずれか１項記載の脚式ロボット。
7. 前記重心軌道を生成する手段は、
   鉛直方向軌道及び計算された前記回転運動量に基づいて計算される係数を持つ行列と、重心軌道の列と、目標ＺＭＰの列との間に成立する連立方程式を解いて重心軌道を計算する
   ことを特徴とする請求項１乃至６いずれか１項記載の脚式ロボット。
8. 指令する目標速度に応じて、脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算するステップと、
   計算された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算するステップと、
   計算された前記重心軌道に基づいて、前記脚式ロボットの関節を回転するステップと
   を備える脚式ロボットの制御方法。
9. 前記重心軌道を計算するステップでは、
   記憶された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算する
   ことを特徴とする請求項８記載の脚式ロボットの制御方法。
10. 前記回転運動量を計算するステップでは、
    所定の周期単位の前記脚式ロボットの重心周りの回転運動量を計算して、前記回転運動量を記憶し、
    前記重心軌道を計算するステップでは、
    前記目標速度に応じて、多項式補間処理により、記憶された前記所定の周期単位の回転運動量から前記周期間の回転運動量を計算して、
    計算された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算する
    ことを特徴とする請求項９記載の脚式ロボットの制御方法。
11. 前記回転運動量を計算するステップでは、
    指令する目標速度と一歩の継続時間とに応じて、前記脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算する
    ことを特徴とする請求項８乃至１０いずれか１項記載の脚式ロボットの制御方法。
12. 前記回転運動量を計算するステップでは、
    指令する目標速度と、一歩の継続時間と、前記脚式ロボットの状態が遷移する遷移時間とに応じて、前記脚式ロボットの上体の姿勢角が変動しない前記脚式ロボットの重心周りの回転運動量を計算する
    ことを特徴とする請求項８乃至１１いずれか１項記載の脚式ロボットの制御方法。
13. 前記回転運動量を計算するステップは、
    前記脚式ロボットをモデル化した「左膝位置と右膝位置と左足平近傍位置と右足平近傍位置に質量が集中して存在する力学系」を用いて、各質点の位置及び速度を計算して、前記脚式ロボットの重心周りの回転運動量を計算するステップ
    を備えることを特徴とする請求項８乃至１２いずれか１項記載の脚式ロボットの制御方法。
14. 前記重心軌道を生成するステップでは、
    鉛直方向軌道及び計算された前記回転運動量に基づいて計算される係数を持つ行列と、重心軌道の列と、目標ＺＭＰの列との間に成立する連立方程式を解いて重心軌道を計算する
    ことを特徴とする請求項８乃至１３いずれか１項記載の脚式ロボットの制御方法。
15. 関節角を変更して動作する脚式ロボットの制御システムであって、
    前記脚式ロボットに指令する目標速度に応じた、前記脚式ロボットの、左足平位置と右足平位置と体幹姿勢を示すデータと、前記位置姿勢を実現する体幹位置を示すデータを入力して記憶する入力データ記憶部と、
    前記データ群から、前記脚式ロボットをモデル化した「左膝位置と右膝位置と左足平近傍位置と右足平近傍位置に質量が集中して存在する力学系」を用いて、各質点の位置及び速度を計算して、前記脚式ロボットの重心周りの回転運動量を計算する回転運動量計算部と、
    計算された前記回転運動量を用いてＺＭＰ方程式を求解することで、前記脚式ロボットの重心軌道を計算する重心軌道計算部と、
    計算された前記重心軌道に基づいて、前記関節を回転する関節駆動部と
    を備える脚式ロボットの制御システム。

Images