PATENTSCOPE sera indisponible quelques heures pour des raisons de maintenance le mardi 19.11.2019 à 16:00 CET
Recherche dans les collections de brevets nationales et internationales
Certains contenus de cette application ne sont pas disponibles pour le moment.
Si cette situation persiste, veuillez nous contacter àObservations et contact
1. (WO2005068136) GENERATEUR DE DEMARCHE POUR ROBOT MOBILE
Note: Texte fondé sur des processus automatiques de reconnaissance optique de caractères. Seule la version PDF a une valeur juridique
明 細書

移動ロボットの歩容生成装置

技術分野

[0001] 本発明は 2足移動ロボット等の移動ロボットの目標歩容を生成する装置に関する。

背景技術

[0002] 2足移動ロボット等の移動ロボットの目標歩容を生成する技術としては、例えば特開 2002— 326173号公報(特許文献 1)や、 PCT国際公開公報 WOZ03Z057427 ZA1 (特許文献 2)に見られるものが本願出願人により提案されている。これらの文 献に見られる技術は、ロボットの運動(各部位の位置、姿勢)と、床反力との関係を表 す第 1の動力学モデルを用、て該第 1の動力学モデル上での所要の動力学的平衡 条件 (床反力の並進力成分が目標値になる、ある点のまわりの床反力モーメントが目 標値になるなどの条件)を満足するようにロボットの目標運動の瞬時値 (瞬時目標運 動)と目標床反力の瞬時値 (瞬時目標床反力)とからなる瞬時目標歩容が逐次作成 される。そして、この瞬時目標歩容を第 2の動力学モデルに入力して、該瞬時目標運 動の一部(目標上体位置姿勢や目標 ZMPまわりの目標モーメントなど)を補正するこ とで、最終的な瞬時目標歩容を時系列的に生成するようにして、る。

[0003] この場合、第 1の動力学モデルとしては、線形性の高いモデルが一般に使用される 。線形性の高い動力学モデルを用いて瞬時目標歩容を作成することで、仮想的な周 期的歩容である定常歩容につながり、もしくは漸近するような歩容(ロボットの安定な 運動を継続的に行い得る歩容)を効率よく短時間で作成することが可能となる。ひい ては実ロボットの実際の運動を行いながら、リアルタイムでロボットの瞬時目標歩容を 逐次生成することが可能となる。

[0004] ところが、線形性の高い動力学モデルは、ロボットの種々様々の動作において一般 に動力学的精度が比較的低くなりがちである。すなわち、その動力学モデル上での ロボットの動力学は、実ロボットの実際の動力学に対して誤差を生じやすい。このた め、第 1の動力学モデルを用いて作成される瞬時目標歩容を、そのまま実ロボットに 適用して、該実ロボットの動作を行わせると、第 1の動力学モデル上で保証された動 力学的平衡条件が、実ロボット上では成立せず、実ロボットの動作が安定性に欠ける ものとなりやすい。

[0005] そこで、前記特許文献 1、 2に見られる技術では、第 1の動力学モデルを用いて作 成した瞬時目標歩容の一部をさらに、第 2の動力学モデルを用いて補正するようにし ている。この場合、第 2の動力学モデルとしては、第 1の動力学モデルよりも動力学的 精度の高いモデルが用いられる。これにより、第 1の動力学モデルを用いて作成した 歩容よりも、より動力学的精度の高い (実ロボットの動力学により近い)歩容を生成す ることが可能となる。

[0006] ところで、前記第 1の動力学モデルは、前記したように動力学的精度が低くなりがち であるので、生成しょうとする歩容によっては、動力学的な誤差が比較的大きなものと なることがある。すなわち、第 1の動力学モデルで想定 (考慮)されていない慣性力が 発生するようなロボットの運動を行わせる歩容を生成するような場合には、上記誤差 が大きなものとなりやすい。例えば、 2足移動ロボットの上体、各脚体の先端部付近に それぞれ対応する質点を 1つずつもつような 3質点の動力学モデル、あるいは、ロボ ットの上体にのみ質点をもつ 1質点の動力学モデルを前記第 1の動力学モヂルとして 用いた場合には、特に、各脚体の膝関節を曲げるような動作を比較的すばやく行う 場合には、それに伴う慣性力の変化の影響で、動力学的な誤差が比較的大きなもの となる。その結果、この第 1の動力学モデルを用いて作成される瞬時目標歩容がロボ ットの継続的な安定性を確保する上で過剰に不適切なものとなることがある。そして、 このような場合には、第 2の動力学モデルで該瞬時目標歩容を補正するようにしても 、その補正が適正になされず、その補正後の瞬時目標歩容が、安定余裕の低いもの になったり、ロボットの継続的な安定性を確保できな、ものに発散してしまう恐れがあ つた o

[0007] 本発明は力かる背景に鑑みてなされたものであり、動力学モデルを用いて作成され た瞬時目標歩容の運動を、動力学モデルを用いることなく(運動と力の関係を表す微 分方程式や積分方程式を用いることなく)、適切に補正し、その補正後の運動を含む 瞬時目標歩容の動力学的精度を高めることができる移動ロボットの歩容生成装置を 提供することを目的とする。

発明の開示

[0008] 本発明の移動ロボットの歩容生成装置の第 1発明は、移動ロボットの瞬時目標運動 と瞬時目標床反力とからなる瞬時目標歩容を逐次発生する瞬時歩容発生手段を備 えた歩容生成装置において、前記移動ロボットの全体または一部を、イナ一シャをも つ剛体と質点とのうちの少なくともいずれか一方を要素として、複数の要素からなるモ デルで表現し、

前記瞬時歩容発生手段が発生した瞬時目標運動から、移動ロボットの瞬時運動と 前記モデルの各要素の配置との関係を規定する所定の第 1の幾何学的拘束条件に 従って決定される前記モデルの各要素の配置を第 1の配置とし、前記瞬時歩容発生 手段が発生した瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素 の配置との関係を規定する所定の第 2の幾何学的拘束条件に従って決定される前 記モデルの各要素の配置を第 2の配置とし、さらに前記瞬時歩容発生手段が発生し た瞬時目標運動のうちの前記移動ロボットの所定の部位の位置と姿勢とのうちの少な くとも!、ずれか一方を補正してなる補正後瞬時目標運動から、前記第 2の幾何学的 拘束条件に従って決定される前記モデルの各要素の配置を第 3の配置としたとき、 前記第 3の配置と第 1の配置との間での前記モデルの各要素の配置の差を加速度と みなすことによって算出される各要素の慣性力の合力が所定の点まわりに発生する モーメント成分力前記第 2の配置と第 1の配置との間での前記モデルの各要素の配 置の差を加速度とみなすことによって算出される各要素の慣性力の合力が前記所定 の点まわりに作用するモーメント成分よりも所定の値に近づくように、前記補正後瞬時 目標運動を決定する瞬時目標運動補正手段を備えたことを特徴とする。

[0009] なお、この第 1発明を含めて以降に説明する本発明では、前記モデルの要素の「配 置」は、該要素としての質点の「位置」と、該要素としての、イナ一シャをもつ剛体 (リン ク)の「姿勢」(傾斜角)とを総称的に表現する呼称である。一般的には、剛体は質量 とイナ一シャとをもつが、便宜上、本発明では、その質量およびイナ一シャをもつ剛 体は、前記質量を有して該剛体の重心に位置する質点と、質量が 0で前記イナーシ ャをもつ剛体に分解しておくものとする。このようにしても一般性は失われない。また、 「第 1の配置」、「第 2の配置」、「第 3の配置」というときには、それは、前記モデルに含 まれる全ての要素の配置の組を意味する。

[0010] かかる第 1発明によれば、前記第 1の幾何学的拘束条件と、第 2の幾何学的拘束条 件とを適切に設定して、また、モデルを構成する要素を適切に設定しておくことで、 前記第 2の配置と、第 1の配置との差 (第 2の配置における各要素の配置と第 1の配 置における各要素の配置との差)を、前記瞬時歩容発生手段が発生した瞬時目標運 動(ロボットの各部位の位置と姿勢とのうちの少なくともいずれか一方の瞬時瞬時の 目標値)と瞬時目標床反力(ロボットに作用する床反力の並進力とモーメントとのうち の少なくとも、ずれか一方の瞬時瞬時の目標値)との間の動力学的誤差の程度 (度 合い)に対応させることが可能となる。補足すると、この対応関係には、一般には定常 的なオフセットが存在する。そして、この場合、前記第 3の配置と、第 1の配置との差( 第 3の配置における各要素の配置と第 1の配置における各要素の配置との差)が、前 記補正後瞬時目標運動と前記瞬時目標床反力との間の動力学的誤差の程度に対 応することとなる。従って、前記第 3の配置と第 1の配置との間での前記モデルの各 要素の配置の差を加速度とみなすことによって算出される各要素の慣性力の合力が 所定の点まわりに作用するモーメント成分力前記第 2の配置と第 1の配置との間で の前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素 の慣性力の合力が前記所定の点まわりに作用するモーメント成分よりも所定の値 (あ る一定のオフセット値)に近づくように、前記補正後瞬時目標運動を決定することで、 前記瞬時目標床反力との間での動力学的精度 (特に床反力モーメントに係わる動力 学的精度)が高まる補正後瞬時目標運動を得ることができることとなる。そして、この 場合、補正後瞬時目標運動は、モデルの要素の配置の時間的な変化 (位置や姿勢 の 1階微分値や 2階微分値)を用いることなぐ該要素の配置に関する幾何学的な演 算処理によって決定できる。

[0011] 従って、第 1発明によれば、動力学モデルを用いることなく(運動と力の関係を表す 微分方程式や積分方程式を用いることなく)、瞬時目標歩容の運動を適切に補正し、 その補正後の運動を含む瞬時目標歩容の動力学的精度を高めることが可能となる。

[0012] 補足すると、第 1発明では、結果的に上記の如ぐ補正後瞬時目標運動が決定され ていればよぐ前記第 1の配置、第 2の配置、第 3の配置を実際に求めたり、前記モー メント成分を実際に求めることは必ずしも必要ではない。

[0013] かかる第 1発明では、前記瞬時目標運動補正手段は、前記第 3の配置と第 1の配 置との間での前記モデルの各要素の配置の差を加速度とみなすことによって算出さ れる各要素の慣性力の合力 F3の並進力成分が前記第 2の配置と第 1の配置との間 での前記モデルの各要素の配置の差を加速度とみなすことによって算出される各要 素の慣性力の合力 F2の並進力成分よりも 0に近づき、且つ、前記合力 F3が前記所定 の点まわりに作用するモーメント成分力前記合力 F2が前記所定の点まわりに作用 するモーメント成分よりも前記所定の値に近づくように、前記補正後瞬時目標運動を 決定することが好ま U、 (第 2発明)。

[0014] この第 2発明によれば、前記モーメント成分が所定の値に近づくだけでなぐ第 3お よび第 1の配置に係る各要素の慣性力の合力の並進力成分が、第 2および第 1の配 置に係る各要素の慣性力の合力の並進力成分よりも 0に近づくように、補正後瞬時 目標運動を決定するので、その補正後瞬時目標運動と前記瞬時目標床反力との間 の動力学的精度を、床反力モーメントと並進床反力との両者について高めることが可 能となる。

[0015] また、本発明の移動ロボットの歩容生成装置の第 3発明は、移動ロボットの瞬時目 標運動を逐次発生する瞬時歩容発生手段を備えた歩容生成装置において、前記移 動ロボットの全体または一部を、イナ一シャをもつ剛体と質点とのうちの少なくともい ずれか一方を要素として、複数の要素力もなるモデルで表現し、前記瞬時歩容発生 手段が発生した瞬時目標運動から、移動ロボットの瞬時運動と前記モデルの各要素 の配置との関係を規定する所定の第 1の幾何学的拘束条件に従って決定される前 記モデルの各要素の配置を第 1の配置とし、前記瞬時歩容発生手段が発生した瞬 時目標運動のうちの前記移動ロボットの所定の部位の位置と姿勢とのうちの少なくと ¾ 、ずれか一方を補正してなる補正後瞬時目標運動から、該移動ロボットの瞬時運 動と前記モデルの各要素の配置との関係を規定する所定の第 2の幾何学的拘束条 件に従って決定される前記モデルの各要素の配置を第 2の配置としたとき、前記第 2 の配置と第 1の配置との間での前記モデルの各要素の配置の差を加速度とみなすこ とによって算出される各要素の慣性力の合力が所定の点まわりに作用するモーメント 成分がほぼ所定の値になるように、前記補正後瞬時目標運動を決定する瞬時目標 運動補正手段を備えたことを特徴とするものである。

[0016] この第 3発明では、上記「第 2の配置」は、前記第 1発明における「第 3の配置」に相 当するものである。そして、第 3発明では、前記第 2の配置と第 1の配置との間での前 記モデルの各要素の配置の差を加速度とみなすことによって算出される各要素の慣 性力の合力が所定の点まわりに発生するモーメント成分がほぼ所定の値 (ある一定 のオフセット値)になるように、前記補正後瞬時目標運動が決定される。これにより、 前記第 1発明と同様に、前記瞬時目標床反力との間での動力学的精度 (特に床反力 モーメントに係わる動力学的精度)が高まる補正後瞬時目標運動を得ることができる こととなる。そして、この場合、補正後瞬時目標運動は、モデルの要素の配置の時間 的な変化 (位置や姿勢の 1階微分値や 2階微分値)を用いることなぐ該要素の配置 に関する幾何学的な演算処理によって決定できる。

[0017] 従って、この第 3発明においても、動力学モデルを用いることなく(運動と力の関係 を表す微分方程式や積分方程式を用いることなく)、瞬時目標歩容の運動を適切に 補正し、その補正後の運動を含む瞬時目標歩容の動力学的精度を高めることが可 能となる。

[0018] 補足すると、第 3発明では、結果的に上記の如ぐ補正後瞬時目標運動が決定され ていればよぐ前記第 1の配置、第 2の配置を実際に求めたり、前記モーメント成分を 実際に求めることは必ずしも必要ではない。

[0019] この第 3発明では、前記瞬時目標運動補正手段は、前記第 2の配置と第 1の配置と の間での前記モデルの各要素の配置の差を加速度とみなすことによって算出される 各要素の慣性力の合力の並進力成分がほぼ 0になり、且つ、該合力が前記所定の 点まわりに作用するモーメント成分がほぼ前記所定の値になるように、前記補正後瞬 時目標運動を決定することが好まし、 (第 4発明)。

[0020] この第 4発明によれば、前記モーメント成分がほぼ所定の値になるだけでなぐ第 2 および第 1の配置に係る各要素の慣性力の合力の並進力成分がほぼ 0になるように 、補正後瞬時目標運動を決定するので、前記第 2発明と同様に、その補正後瞬時目 標運動と前記瞬時目標床反力との間の動力学的精度を、床反力モーメントと並進床

反力の両者にっ、て高めることが可能となる。

[0021] ところで、前記第 1一第 4発明において、前記モーメント成分のうち、前記モデルの イナ一シャをもつ要素(剛体)の配置の差 (姿勢の差)に起因する成分は、その要素 の姿勢の差 (傾斜角の差)とその要素のイナ一シャの値との積に相当するものとなる。 また、前記モデルの質量をもつ要素 (質点)の配置の差 (位置の差)に起因する成分 は、その位置の差と該要素の前記所定の点力の距離とをそれぞれベクトルで表し たときのそれらのベクトルの積 (外積)に該要素の質量を乗算したものに相当する。そ して、この場合、質量をもつ要素の配置の差 (位置の差)に起因する成分は、その位 置の差に係わる 2つの位置の一方の位置と前記所定の点とを結ぶ線分と、当該 2つ の位置の他方の位置と前記所定の点とを結ぶ線分とがなす角度に応じたもの (より詳 しくは該角度に応じて単調に増力 tlもしくは減少するもの)となる。

[0022] そこで、第 5発明では、前記第 1または第 2発明において、前記第 2の配置と第 1の 配置との間の各要素の配置の差に係わる前記モーメント成分のうち、前記モデルの 質量をもつ各要素の、前記第 1の配置における位置 Aと前記第 2の配置における位 置 Bの差に起因する成分は、前記所定の点と前記位置 Aとを結ぶ線分と、前記所定 の点と前記位置 Bとを結ぶ線分とがなす角度から、該角度に関する実質的に単調な 関数を用いて算出され、前記第 3の配置と第 1の配置との間の各要素の配置の差に 係わる前記モーメント成分のうち、前記モデルの質量をもつ各要素の、前記第 1の配 置における位置 Aと前記第 3の配置における位置 Cの差に起因する成分は、前記所 定の点と前記位置 Aとを結ぶ線分と、前記所定の点と前記位置 Cとを結ぶ線分とがな す角度力前記単調な関数を用いて算出されることを特徴とする。

[0023] 同様に、第 6発明では、前記第 3又は第 4発明において、前記第 2の配置と第 1の 配置との間の各要素の配置の差に係わる前記モーメント成分のうち、前記モデルの 質量をもつ各要素の、前記第 1の配置における位置 Aと前記第 2の配置における位 置 Bの差に起因する成分は、前記所定の点と前記位置 Aとを結ぶ線分と、前記所定 の点と前記位置 Bとを結ぶ線分とがなす角度から、該角度に関する実質的に単調な 関数を用いて算出されることを特徴とする。

[0024] このようにすることによって、前記モーメント成分を実際に算出するとき、ベクトル演 算が不要になって、その算出が容易になる。

[0025] 前記第 1一第 6発明は、前記瞬時歩容発生手段が発生する瞬時目標運動が、前記 移動ロボットの運動と床反力との関係を表す動力学モデルであって、少なくとも該移 動ロボットの 1つ以上の特定部位の特定の運動成分によって発生する慣性力がほぼ 0であるとして構築された動力学モデルを用いて決定されており、前記モデル力前 記特定部位のうちの少なくとも 1つの部位に対応する要素を含む場合に好適である( 第 7発明)。

[0026] すなわち、前記瞬時目標運動が、移動ロボットの 1つ以上の特定部位の特定の運 動成分 (ある方向の並進運動、回転運動など)によって発生する慣性力がほぼ 0であ るとして構築された動力学モデルを用いて決定されているときには、その特定部位が 比較的大きな慣性力を発生するような目標歩容を生成するときに、瞬時歩容発生手 段が発生する瞬時目標運動と瞬時目標床反力との間の動力学的精度が低下しやす い。この場合、第 7発明では、その特定部位のうちの少なくとも 1つの部位に対応する 要素を前記モデルに含ませて!/、るので、前記補正後瞬時目標運動と瞬時目標床反 力との間の動力学的精度を的確に高めることができる。

[0027] また、前記第 1一第 6発明で、前記瞬時歩容発生手段が発生する瞬時目標運動は 、前記移動ロボットの運動と床反力との関係を表す所定の動力学モデル上での目標 床反力または目標 ZMPを満足するように決定されており、前記瞬時目標運動から、 前記第 1の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の時 間的変化によって発生する各要素の慣性力の合力に釣り合う床反力と、前記瞬時目 標運動から、前記第 2の幾何学的拘束条件に従って決定される前記モデルの各要 素の配置の時間的変化によって発生するその各要素の慣性力の合力に釣り合う床 反力との差に所定の定常オフセットを加えたもの力前記瞬時目標運動によって前 記動力学モデルで発生する床反力の誤差に略一致するように前記第 1および第 2の 幾何学的拘束条件が設定されてヽることが好適である (第 8発明)。

[0028] この第 8発明によれば、瞬時目標運動と前記瞬時歩容発生手段が発生した瞬時目 標床反力との間の動力学的誤差が、前記瞬時目標運動から、前記第 2の幾何学的 拘束条件に従って前記モデルの各要素の配置を決定した場合における各要素の配

置 (前記第 1発明における第 2の配置)と前記瞬時目標運動から、前記第 1の幾何学 的拘束条件に従って前記モデルの各要素の配置を決定した場合における各要素の 配置 (すなわち前記第 1の配置)における各要素の配置との差に対応するものとなる 。このため、前記瞬時目標床反力との間の動力学的精度を高めるような補正後瞬時 目標運動の決定を的確に行うことが可能となる。補足すると、この対応関係には、一 般的には定常的なオフセットが存在する。

[0029] また、第 1一第 6発明で、前記瞬時歩容発生手段が発生する瞬時目標運動が、前 記移動ロボットの運動と床反力との関係を表す所定の動力学モデル上での目標床反 力または目標 ZMPを満足するように決定されて!ヽる場合に、前記瞬時目標運動から 、前記第 1の幾何学的拘束条件に従って決定される前記モデルの各要素の配置の 全体重心と、前記瞬時目標運動から、前記第 2の幾何学的拘束条件に従って決定さ れる前記モデルの各要素の配置の全体重心との差に該要素の総質量を乗じたもの 力 前記瞬時目標運動における前記動力学モデルの全体重心の誤差に該動力学 モデルの総質量を乗じたものが略一致するように前記第 1および第 2の幾何学的拘 束条件を設定するようにしてもょ、 (第 9発明)。

[0030] これによつて、前記補正後瞬時目標運動と瞬時目標床反力との間の動力学的精度 を低下させる要因の 1つである前記動力学モデルの全体重心位置の誤差の影響を 打ち消すことができる。

[0031] また、第 1一第 9発明において、前記移動ロボットが、例えば上体力も延設された複 数の脚体または複数の腕体を複数の可動体として備えるロボットであるときには、前 記第 1の幾何学的拘束条件は、各可動体の先端部近傍の所定の点と、該可動体の 前記上体との連結部近傍の所定の点を結ぶ線分に平行な直線上に前記モデルの 要素のうちの、ずれかが存在すると!/、う条件を含むことが好ま U、(第 10発明)。ある いは、前記第 1の幾何学的拘束条件は、前記モデル上での前記上体と各可動体とが 所定の一定姿勢状態に保持されると、う条件を含むことが好まし、(第 11発明)。そ して、この第 11発明では、前記所定の一定姿勢は、前記移動ロボットの上体と複数 の可動体とをほぼ鉛直方向に向けた姿勢であることが好適である(第 12発明) また、第 1一第 12発明において、前記第 2の幾何学的拘束条件は、前記移動ロボ

ットの任意の瞬時目標運動から、その条件に従って決定される前記モデルの各要素 の配置が、前記瞬時目標運動に従う前記ロボットにおける該要素に対応する部位の 配置に略一致するように設定されていることが好適である(第 13発明)

このように第 1および第 2の幾何学的拘束条件を定めることにより、前記瞬時目標運 動から、前記第 2の幾何学的拘束条件に従って前記モデルの各要素の配置を決定 した場合における各要素の配置 (前記第 1発明における第 2の配置)と、前記瞬時目 標運動から、前記第 1の幾何学的拘束条件に従って前記モデルの各要素の配置を 決定した場合における各要素の配置 (すなわち前記第 1の配置)における各要素の 配置との差を、前記瞬時目標運動と前記瞬時歩容発生手段が発生した瞬時目標床 反力との間の動力学的誤差に好適に対応させることが可能となる。

[0032] また、前記第 1一第 6発明において、前記移動ロボットが、上体から延設された複数 の脚体または複数の腕体を複数の可動体として備えると共に、各可動体の上体との 連結部と該可動体の先端部との間の中間部とに屈曲可能な関節を有し、さらに前記 瞬時歩容発生手段が発生する瞬時目標運動が、前記ロボットの運動と床反力との関 係を表す動力学モデルであって、各可動体の屈伸運動に起因して該可動体の中間 部もしくはその近傍で発生する慣性力がほぼ 0であるとして構築された動力学モデル を用いて決定されて、るときには、前記モデルは少なくとも前記各可動体の中間部も しくはその近傍部位に対応させた質点を要素として含むモデルであることが好適であ る (第 14発明)。

[0033] すなわち、瞬時目標運動が、各可動体の屈伸運動に起因して該可動体の中間部 もしくはその近傍で発生する慣性力がほぼ 0であるとして (すなわち、該慣性力を無 視して)構築されて、る場合には、各可動体の屈伸運動が比較的すばやく行われる ような目標歩容を生成するときに、前記瞬時歩容発生手段が発生する瞬時目標運動 と瞬時目標床反力との間の動力学的な精度が低下しやすい。そこで、第 14発明の 如ぐ前記モデルにおいて、各可動体の中間部もしくはその近傍部位に対応させた 質点を要素として含ませることによって、前記補正後瞬時目標運動を前記第 1一第 6 発明で説明した如く決定するに当たって、各可動体の中間部の関節の屈曲動作によ る該可動体の屈伸運動に伴う慣性力の影響を補償するようにして補正後瞬時目標運 動を決定できることとなる。このため、その補正後瞬時目標運動と瞬時目標床反力と の間の動力学的精度を高めることができる。すなわち、瞬時歩容発生手段が発生す る瞬時歩容よりも動力学的精度の高い瞬時歩容を得ることができる。

[0034] この第 14発明において、前記第 1幾何学拘束条件は、例えば前記第 10発明また は第 11発明と同様に設定すればよぐまた、第 2幾何学的拘束条件は、前記第 13発 明の如く設定すればよい。そして、特に、第 1および第 2幾何学的拘束条件を、それ ぞれ、第 10発明、第 13発明の如く設定することが好適である。

[0035] すなわち、前記第 1の幾何学的拘束条件は、各可動体の先端部近傍の所定の点と 、該可動体の前記上体との連結部近傍の所定の点を結ぶ線分に、前記モデルの要 素のうちの、該可動体の中間部もしくはその近傍部位に対応させた質点が存在する という条件を含み、前記第 2の幾何学的拘束条件は、前記移動ロボットの任意の瞬時 目標運動から、その条件に従って決定される前記モデルの各要素の配置が、前記瞬 時目標運動に従う前記ロボットにおける該要素に対応する部位の配置に略一致する ように設定されてヽることが好適である(第 15発明)。

[0036] このようにしたとき、前記瞬時歩容発生手段が発生する瞬時目標運動から前記第 2 の幾何学的拘束条件に従って前記モデルの各要素の配置 (前記第 1発明でいうとこ ろの第 2の配置)を決定したとき、その配置における各可動体の中間部もしくはその 近傍部位に対応する前記モデルの質点(以下、ここでは可動体中間質点という)と、 前記第 1の配置における可動体中間質点(これは前記線分上に在る)との位置の差 力 各可動体の中間部の関節の屈曲動作に伴う慣性力に相当するものとなる。した がって、各可動体の中間部の関節の屈曲動作による該可動体の屈伸運動に伴う慣 性力の影響を補償して、動力学的な精度を高めることができる補正後瞬時目標運動 を適正に決定できる。

[0037] ところで、この第 15発明において、瞬時目標運動における上体の瞬時位置と瞬時 姿勢とのうちの少なくともいずれか一方だけを補正するようにした場合には、その瞬 時目標運動から前記補正後瞬時目標運動への上体の瞬時位置と瞬時姿勢とのうち の少なくともいずれか一方の補正量は、結果的には、各可動体の中間部もしくはそ の近傍部位の、前記線分に対する相対位置、あるいは該中間部の関節の曲げ角に 応じて定まることとなる。そして、この場合、特に可動体が脚体である場合には、上記 相対位置や曲げ角に対して上記の補正量がある特徴的な相関性をもつ。そこで、以 下に示す第 16—第 20発明が構成される。

[0038] すなわち、第 16発明は、上体から延設された複数の脚体を備えると共に、各脚体 の上体との連結部と該脚体の先端部との間の中間部とに屈曲可能な関節を有する 移動ロボットの瞬時目標運動を逐次発生する瞬時歩容発生手段を備え、その瞬時目 標運動が、前記ロボットの運動と床反力との関係を表す動力学モデルであって、各 脚体の屈伸運動に起因して該脚体の中間部もしくはその近傍で発生する慣性力が ほぼ 0であるとして構築された動力学モデルを用、て生成された移動ロボットの歩容 生成装置において、少なくとも前記ロボットの直立姿勢状態またはその近傍の姿勢 状態から、各脚体の先端部近傍の所定の点と、該脚体の上体との連結部近傍の所 定の点とを結ぶ線分の長さが縮まるように各脚体の中間部の関節を屈曲させて、該 関節を前記線分と交差する方向に突出させる歩容を生成するとき、前記上体と各脚 体との連結部の位置を、前記瞬時歩容発生手段が発生した瞬時目標運動で定まる 該連結部の位置力該脚体の中間部の関節の突出の向きとほぼ逆向きに変位させ 、且つ、前記上体の下端に対する上端の位置を、前記瞬時歩容発生手段が発生し た瞬時目標運動で定まる該上体の下端に対する上端の位置力該脚体の中間部の 関節の突出の向きとほぼ同じ向きに変位させるように該瞬時目標運動を補正する瞬 時目標運動補正手段を備えたことを特徴とする。

[0039] この第 16発明によれば、少なくとも前記ロボットの直立姿勢状態またはその近傍の 姿勢状態から、各脚体の先端部近傍の所定の点と、該脚体の上体との連結部近傍 の所定の点とを結ぶ線分の長さが縮まるように各脚体の中間部の関節を屈曲させる 歩容を生成するとき、補正後の瞬時目標運動では、元の瞬時目標運動 (瞬時歩容発 生手段が発生した瞬時目標運動)よりも前記連結部が、該脚体の中間部の関節の突 出の向きとほぼ逆向きに変位し、且つ、上体の下端に対する上端の位置が該脚体の 中間部の関節の突出の向きとほぼ同じ向きに変位する。この結果、瞬時歩容発生手 段が発生した瞬時目標運動では考慮されて!、な、、各脚体の中間部の関節の屈曲 動作に伴う慣性力の影響を補償して、動力学的な精度を高めるように該瞬時目標運 動を補正できる。したがって、結果的に、前記第 15発明と同等の効果を確保できる。

[0040] また、第 17および第 19発明は、上体から延設された複数の脚体または複数の腕体 を複数の可動体として備えると共に、各可動体の上体との連結部と該可動体の先端 部との間の中間部とに屈曲可能な関節を有する移動ロボットの瞬時目標運動を逐次 発生する瞬時歩容発生手段を備え、その瞬時目標運動が、前記ロボットの運動と床 反力との関係を表す動力学モデルであって、各可動体の屈伸運動に起因して該可 動体の中間部もしくはその近傍で発生する慣性力がほぼ 0であるとして構築された動 力学モデルを用いて生成された移動ロボットの歩容生成装置において、前記各可動 体の中間部の関節の曲げ角に応じて、フィードフォワード制御則により前記瞬時歩容 発生手段が発生した瞬時目標運動の上体位置と上体姿勢とのうちの少なくともいず れか一方の補正量を決定し、その決定した補正量で該瞬時目標運動の上体位置と 上体姿勢とのうちの少なくともいずれか一方を補正するように目標瞬時運動を補正す る瞬時目標運動補正手段を備えたことを特徴とする (第 17発明)。あるいは、各可動 体の先端部近傍の所定の点と、該可動体の上体との連結部近傍の所定の点とを結 ぶ線分と、該可動体の中間部の関節の中心または該関節に連結されたリンクの重心 との前記線分に交差する方向での相対位置に応じて、フィードフォワード制御則によ り前記瞬時歩容発生手段が発生した瞬時目標運動の上体位置と上体姿勢とのうち の少なくともいずれか一方の補正量を決定し、その決定した補正量で該瞬時目標運 動の上体位置と上体姿勢とのうちの少なくともいずれか一方を補正するように目標瞬 時運動を補正する瞬時目標運動補正手段を備えたことを特徴とする (第 19発明)。

[0041] これらの第 17又は第 19発明では、前記各脚体の中間部の関節の曲げ角に応じて 、あるいは、前記線分と可動体の中間部の関節の中心または該関節に連結されたリ ンクの重心との前記線分に交差する方向での相対位置 (線分力もの距離など)に応 じて、フィードフォワード制御則によって直接的に前記瞬時目標運動の上体位置と上 体姿勢とのうちの少なくともいずれか一方の補正量が決定される。これにより、瞬時歩 容発生手段が発生した瞬時目標運動では考慮されて、な、、各脚体の中間部の関 節の屈曲動作に伴う慣性力の影響を補償して、動力学的な精度を高めるように該瞬 時目標運動を補正することが可能となる。この場合、フィードフォワード制御則によつ て補正量を決定するので、瞬時瞬時で直ちに上記慣性力の影響を補償するような補 正量を決定できる。

[0042] そして、前記第 17発明では、前記各可動体が脚体であるときには、前記瞬時目標 運動補正手段は、前記各可動体の中間部の関節の曲げ角が増加するに伴い、前記 瞬時目標運動の上体姿勢をより大きく前傾させ、且つ、前記複数の可動体を除いた 移動ロボットの全体の重心位置をより後方に変位させるように前記補正量を決定する ことが好ましい (第 18発明)。同様に、第 19発明では、前記各可動体が脚体であると きには、前記瞬時目標運動補正手段は、前記線分と、前記各可動体の中間部の関 節の中心または該関節に連結されたリンクの重心との距離を前記相対位置として用 い、その距離が増加するに伴い、前記瞬時目標運動の上体姿勢をより大きく前傾さ せ、且つ、前記複数の可動体を除いた移動ロボットの全体の重心位置をより後方に 変位させるように前記補正量を決定することが好ま、 (第 20発明)。

[0043] このように、各可動体 (各脚体)の中間部の関節の曲げ角と、上体位置と上体姿勢 とのうちの少なくともいずれか一方に関する補正量とを相関付け、あるいは、前記線 分と前記各可動体 (脚体)の中間部の関節の中心または該関節に連結されたリンク の重心との距離と、上体位置と上体姿勢とのうちの少なくともいずれか一方に関する 補正量とを相関付ける。これによつて、前記第 17発明と同様に、各脚体の中間部の 関節の屈曲動作に伴う慣性力の影響を補償して、動力学的な精度を高めるように該 瞬時目標運動を補正できる。

発明を実施するための最良の形態

[0044] 以下、添付図面を参照して本発明の実施形態を説明する。尚、本明細書の実施形 態では、移動ロボットとしては 2足移動ロボットを例にとる。

[0045] 図 1は、本発明の実施形態を適用する 2足移動ロボットの全体的構成の概略を示す 概略図である。

[0046] 図示の如ぐ 2足移動ロボット(以下、ロボットという) 1は上体 (ロボット 1の基体) 3から 下方に延設された左右一対の脚体 2, 2を備える。両脚体 2, 2は同一構造であり、そ れぞれ 6個の関節を備える。その 6個の関節は上体 3側力も順に、股 (腰部)の回旋( 回転)用(上体 3に対するョー方向の回転用)の関節 10R, 10Lと、股 (腰部)のロー

ル方向(X軸まわり)の回転用の関節 12R, 12Lと、股 (腰部)のピッチ方向(Y軸まわ り)の回転用の関節 14R, 14L、膝部のピッチ方向の回転用の関節 16R, 16Lと、足 首のピッチ方向の回転用の関節 18R, 18Lと、足首のロール方向の回転用の関節 2 OR, 20Lと力も構成される。なお、本明細書において、符号 R, Lはそれぞれロボット 1の右側、左側に対応するものであることを意味する符号である。

[0047] 各脚体 2の足首の 2つの関節 18R (L) , 20R(L)の下部には、各脚体 2の先端部を 構成する足平 (足部) 22R (L)が取着されると共に、両脚体 2, 2の最上位には、各脚 体 2の股の 3つの関節 10R(L) , 12R (L) , 14R(L)を介して前記上体 3が取り付けら れている。上体 3の内部には、詳細を後述する制御ユニット 60などが格納される。な お、図 1では図示の便宜上、制御ユニット 60を上体 3の外部に記載している。

[0048] 上記構成の各脚体 2においては、股関節(あるいは腰関節)は関節 10R (L) , 12R

(L) , 14R(L)から構成され、膝関節は関節 16R(L)から構成され、足首関節は関節 18R(L) , 20R(L)から構成される。また股関節と膝関節とは大腿リンク 24R(L)で連 結され、膝関節と足首関節とは下腿リンク 26R (L)で連結される。

[0049] なお、本明細書においては、ロボット 1の「リンク」はロボット 1の剛体とみなせる部位 の意味で使用する。例えば上体 3も 1つのリンク(剛体)であり、その意味で上体 3を上 体リンクと言うこともある。

[0050] 上体 3の上部の両側部には左右一対の腕体 5, 5が取り付けられると共に、上体 3の 上端部には頭部 4が配置される。各腕体 5は、 3つの関節 30R(L) , 32R (L) , 34R( L)から構成された肩関節と、関節 36R (L)から構成された肘関節と、関節 38R(L)か ら構成された手首関節と、この手首関節に連結された手先部 40R (L)とを備えている 。肩関節と肘関節との間、および肘関節と手首関節との間はそれぞれリンクで連結さ れている。

[0051] 上記のロボット 1の構成により、各脚体 2の足平 22R(L)は、上体 3に対して 6つの自 由度を与えられている。そして、ロボット 1の歩行等の移動中に、両脚体 2, 2を合わせ て 6 * 2 = 12個(この明細書で「 *」はスカラに対する演算としては乗算を、ベクトルに 対する演算としては外積を示す)の関節を適宜な角度で駆動することで、両足平 22 R, 22Lの所望の運動を行うことができる。これにより、ロボット 1は任意に 3次元空間 を移動することができる。また、各腕体 5は、その肩関節、肘関節、手首関節の回転 によって、腕振り等の運動を行うことができる。

[0052] 図 1に示す如ぐ各脚体 2の足首関節 18R (L) , 20R(L)の下方には足平 22R (L) との間に公知の 6軸力センサ 50が介装されている。該 6軸力センサ 50は、各脚体 2 の足平 22R(L)の着地の有無、および各脚体 2に作用する床反力(接地荷重)等を 検出するためのものであり、該床反力の並進力の 3方向成分 Fx, Fy, Fz並びにモー メントの 3方向成分 Mx, My, Mzの検出信号を制御ユニット 60に出力する。また、上 体 3には、 Z軸 (鉛直方向(重力方向))に対する上体 3の傾斜角およびその角速度を 検出するための姿勢センサ 54が備えられ、その検出信号が該姿勢センサ 54から制 御ユニット 60に出力される。この姿勢センサ 54は、図示を省略する加速度センサお よびジャイロセンサを備え、これらのセンサの検出信号が上体 3の傾斜角およびその 角速度を検出するために用いられる。また、詳細構造の図示は省略するが、ロボット 1の各関節には、それを駆動するための電動モータ 64 (図 3参照)と、その電動モー タ 64の回転量 (各関節の回転角)を検出するためのエンコーダ(ロータリエンコーダ) 65 (図 3参照)とが設けられ、該エンコーダ 65の検出信号が該エンコーダ 65から制御 ユニット 60に出力される。

[0053] さらに、図 1では図示を省略する力ロボット 1の外部には、ロボット 1を操縦するため のジョイスティック (操作器) 73 (図 3参照)が設けられ、そのジョイスティック 73を操作 することで、直進移動しているロボット 1を旋回させるなど、ロボット 1の歩容に対する 要求を必要に応じて制御ユニット 60に入力できるように構成されている。ジョイスティ ック 73は有線もしくは無線により制御ユニット 60との通信が可能とされている。

[0054] 図 2は本実施形態における各脚体 2の先端部分 (各足平 22R(L)を含む)の基本構 成を概略的に示す図である。同図に示すように、各足平 22R(L)の上方には、前記 6 軸力センサ 50との間にばね機構 70が装備されると共に、足底 (各足平 22R(L)の底 面)にはゴムなど力もなる足底弾性体 71が貼られている。これらのばね機構 70及び 足底弾性体 71によりコンプライアンス機構 72が構成されている。詳細な図示は省略 するが、ばね機構 70は、足平 22R(L)の上面部に取り付けられた方形状のガイド部 材(図示省略)と、足首関節 18R(L) (図 2では足首関節 20R(L)を省略している)お

よび 6軸力センサ 50側に取り付けられ、前記ガイド部材に弹性材 (ゴムやばね)を介 して微動自在に収納されるピストン状部材(図示省略)とから構成されている。

[0055] 図 2に実線で表示された足平 22R(L)は、床反力を受けていないときの状態を示し ている。各脚体 2が床反力を受けると、コンプライアンス機構 72のばね機構 70と足底 弾性体 71とがたわみ、足平 22R (L)は図中に点線で例示したような位置姿勢に移る 。このコンプラインァス機構 72の構造は、着地衝撃を緩和するためだけでなぐ制御 性を高めるためにも重要なものである。その詳細は、例えば本出願人が先に提案し た特開平 5— 305584号公報に詳細に説明されているので、本明細書でのさらなる説 明は省略する。

[0056] 図 3は制御ユニット 60の構成を示すブロック図である。該制御ユニット 60はマイクロ コンピュータにより構成されており、 CPU力もなる第 1の演算装置 90及び第 2の演算 装置 92、 AZD変^^ 80、カウンタ 86、 DZA変^^ 96、 RAM84、 ROM94、並 びにこれらの間のデータ授受を行うバスライン 82を備えている。この制御ユニット 60 では、各脚体 2の 6軸力センサ 50、姿勢センサ 54 (加速度センサおよびレートジャィ 口センサ)、ジョイスティック 73等の出力信号は AZD変翻80でデジタル値に変換 された後、バスライン 82を介して RAM84に送られる。またロボット 1の各関節のェン コーダ 65 (ロータリーエンコーダ)の出力は、カウンタ 86を介して RAM84に入力され る。

[0057] 前記第 1の演算装置 90は後述の如く目標歩容を生成すると共に、関節角変位指 令 (各関節の変位角もしくは各電動モータ 64の回転角の指令値)を算出し、 RAM8 4に送出する。また、第 2の演算装置 92は RAM84から関節角変位指令と、前記ェン コーダ 65の出力信号に基づいて検出された関節角の実測値とを読み出し、各関節 の駆動に必要な操作量を算出する。そして、その算出された操作量が DZA変換器 96とサーボアンプ 64aとを介して各関節を駆動する電動モータ 64に出力される。

[0058] 図 4は、本明細書の実施形態におけるロボット 1の制御ユニット 60の主な機能的構 成を示すブロック図である。この図 4中の「実ロボット」の部分以外の部分が制御ュニ ット 60が実行する処理機能 (主として第 1の演算装置 90及び第 2の演算装置 92の機 能)によって構成されるものである。その処理機能は、制御ユニット 60に実装されたプ ログラム等によって実現されている。尚、以下の説明では、ロボット 1の各部 (脚体 2、 腕体 5など)の左右を特に区別する必要がないときは、前記符号 R, Lを省略する。

[0059] 以下説明すると、制御ユニット 60は、後述の如く目標歩容を自在かつリアルタイム に生成して出力する歩容生成装置 100を備えている。この歩容生成装置 100は、そ の機能によって本発明の実施形態を実現するものである。この歩容生成装置 100が 出力する目標歩容は、補正目標上体姿勢軌道 (上体 3の目標姿勢の軌道)、補正目 標上体位置軌道 (上体 3の目標位置の軌道)、目標足平位置姿勢軌道 (各足平 22の 目標位置及び目標姿勢の軌道)、目標腕姿勢軌道 (各腕体の目標姿勢の軌道)、目 標 ZMP (目標全床反力中心点)軌道、目標 ZMPまわりの補正目標床反力モーメント 軌道および目標全床反力軌道から構成される。尚、脚体 2や腕体 5以外に、上体 3に 対して可動な部位 (頭部など)を備える場合には、その可動部位の目標位置姿勢軌 道が目標歩容に加えられる。

[0060] ここで、本明細書での歩容に関する基本的な用語の定義などについて説明してお く。歩容における「軌道」は時間的変化のパターン (時系列パターン)を意味し、「軌道 」の代わりに「パターン」と称することもある。また、「姿勢」は空間的な向きを意味する 。例えば上体姿勢は Z軸 (鉛直軸)に対するロール方向(X軸まわり)の上体 3の傾斜 角(姿勢角)とピッチ方向 (Y軸まわり)の上体 3の傾斜角(姿勢角)とで表され、足平 姿勢は各足平 22に固定的に設定された 2軸の空間的な方位角で表される。本明細 書では、上体姿勢は上体姿勢角ということもある。なお、腕体 5に関する目標腕姿勢 は、本明細書の実施形態では上体 3に対する相対姿勢で表される。

[0061] 上体位置は、上体 3のあら力じめ定めた代表点(上体 3に対して任意に固定設定し たローカル座標系でのある固定点)の位置を意味する。同様に、足平位置は、各足 平 22のあら力じめ定めた代表点(各足平 22に対して任意に固定設定したローカル 座標系での固定点)の位置を意味する。例えば各足平 22の代表点は、各足平 22の 底面上 (より具体的には各脚体 2の足首関節の中心から各足平 22の底面への垂線 が該底面と交わる点等)に設定される。

[0062] 上体 3に関する前記補正目標上体姿勢および補正目標上体位置は、ある基本とな る目標上体姿勢 (仮目標上体姿勢)および目標上体位置 (仮目標上体位置)を補正

したものである。本明細書の実施形態では、基本となる目標上体位置姿勢は、後述 の変位次元補正上体位置姿勢が相当する。

[0063] なお、以降の説明では、誤解を生じるおそれがない場合には、しばしば「目標」を省 略する。

[0064] 歩容のうちの、床反力に係わる構成要素以外の構成要素、すなわち足平位置姿勢 、上体位置姿勢等、ロボット 1の各部位の位置姿勢に関する構成要素を総称的に「運 動」という。また、各足平 22に作用する床反力(並進力及びモーメントからなる床反力 )を「各足平床反力」と呼び、ロボット 1の全て(2つ)の足平 22R, 22Lについての「各 足平床反力」の合力を「全床反力」という。ただし、以下の説明においては、各足平床 反力はほとんど言及しないので、特に断らない限り、「床反力」は「全床反力」と同義と して扱う。

[0065] 目標床反力は、一般的には、作用点とその点に作用する並進力及びモーメントとに よって表現される。作用点はどこにとっても良いので、同一の目標床反力でも無数の 表現が考えられるが、特に目標床反力中心点 (全床反力の中心点の目標位置)を作 用点にして目標床反力を表現すると、目標床反力のモーメント成分は、鉛直成分 (鉛 直軸 (Z軸)まわりのモーメント成分)を除!、て 0になる。換言すれば、目標床反力中心 点まわりの目標床反力のモーメントの水平成分 (水平軸 (X軸及び Y軸)まわりのモー メント)は 0になる。

[0066] なお、動力学的平衡条件を満足する歩容では、ロボット 1の目標運動軌道力算出 される ZMP (目標運動軌道力も算出される慣性力とロボット 1に作用する重力との合 力がその点まわりに作用するモーメントが、鉛直成分を除いて 0になる点)と目標床反 力中心点とは一致することから、目標床反力中心点軌道の代わりに目標 ZMP軌道 を与えると言っても同じことである。

[0067] ここで、ロボット 1の歩行を行う場合には、例えば本出願人が先に特開平 10— 8608 0号公報で提案した上体高さ決定手法によってロボット 1の上体 3の鉛直位置 (上体 高さ)が決定されると、並進床反力鉛直成分は従属的に決定される。さらに、目標歩 容の運動による慣性力と重力との合力が目標 ZMPまわりに発生するモーメントの水 平成分が 0になるようにロボット 1の上体水平位置軌道(あるいは全体重心の位置軌 道)を決定することで、並進床反力水平成分も従属的に決定される。このため、ロボッ ト 1の歩行を行う場合には、目標歩容の床反力に関して明示的に設定すべき物理量 としては、目標 ZMPだけでもよい。

[0068] 一方、床反力が 0もしくはほぼ 0になるような時期を伴う歩容でのロボット 1の移動、 例えばロボット 1の走行を行う場合には、並進床反力鉛直成分もロボット 1の動作制御 上重要である。このため、並進床反力鉛直成分の目標軌道を明示的に設定した上で 、ロボット 1の目標上体鉛直位置等の軌道を決定することが望ましい。また、ロボット 1 の歩行においても、摩擦係数が低、床面上 (低ミュー路上)でロボット 1を移動させる ような場合には、並進床反力鉛直成分 (より厳密には並進床反力の床面に垂直な成 分)が摩擦力に影響を及ぼすことから、ロボット 1のスリップなどを防止する上で、並進 床反力鉛直成分の目標軌道を明示的に設定することが望ましい。さらに、本発明の 実施形態では、最終的に歩容生成装置 100が出力する目標歩容では、目標 ZMPま わりに補正目標床反力モーメント (水平成分が 0とは限らな、モーメント)を発生させる

[0069] このようなことから、本明細書の実施形態では、歩容生成装置 100が出力する目標 歩容の床反力に関する構成要素として、目標 ZMP軌道のほか、目標 ZMPまわりの 補正目標床反力モーメントと、目標並進床反力鉛直成分とを含ませて!/、る。

[0070] そして、本明細書では、歩容生成装置 100が出力する目標歩容は、広義には、「1 歩もしくは複数歩の期間の目標運動軌道と目標床反力軌道との組」の意味で使用さ れ、狭義には、「1歩の期間の目標運動軌道と、目標 ZMP、補正目標床反力モーメ ント及び目標並進床反力鉛直成分を含む目標床反力軌道との組」の意味で使用さ れる。

[0071] 但し、本明細書の実施形態においては、最終的な目標歩容 (歩容生成装置 100が 出力する目標歩容)を決定するまでの過程で作成する目標歩容 (仮目標歩容)では、 目標 ZMPまわりの目標床反力モーメントの水平成分は、本来の目標 ZMPの定義ど おりに 0とされる。従って、最終的に決定する目標歩容以外の仮目標歩容 (後述の単 純化モデル歩容ゃ変位次元補正歩容)では、上記狭義の目標歩容から、補正目標 床反力モーメントを除いたものが目標歩容の意味で使用される。補足すると、本明細 書の実施形態では、最終的な目標歩容 (歩容生成装置 100が出力する目標歩容)を 決定するまでの過程で作成する目標歩容 (仮目標歩容)が本発明に密接に関連する ものとなっている。このため、以降の説明で現れる目標歩容の大部分は、前記狭義の 目標歩容から、補正目標床反力モーメントを除、たもの(目標 ZMPを満足する歩容) の意味で使用される。

[0072] なお、以降の説明では、「床反力鉛直成分」は「並進床反力鉛直成分」を意味する ものとし、床反力のうちのモーメントの鉛直成分 (鉛直軸回り成分)は、「モーメント」と いう用語を用いて「床反力鉛直成分」と区別をする。同様に、「床反力水平成分」は「 並進床反力水平成分」を意味するものとする。

[0073] また、目標歩容の「1歩」は、ロボット 1の片方の脚体 2が着地してからもう一方の脚 体 2が着地するまでの意味で使用する。

[0074] また、歩容における両脚支持期とは、ロボット 1がその自重を両脚体 2, 2で支持す る期間、片脚支持期とはいずれか一方のみの脚体 2でロボット 1の自重を支持する期 間、空中期とは両脚体 2, 2が床力も離れている (空中に浮いている)期間を言う。片 脚支持期においてロボット 1の自重を支持しない側の脚体 2を遊脚と呼ぶ。なお、片 脚支持期と空中期とが交互に繰り返されるロボット 1の走行歩容では両脚支持期は無 い。この場合、空中期では両脚 2, 2とも、ロボット 1の自重を支持しないこととなるが、 便宜上、該空中期の直前の片脚支持期において遊脚であった脚体 2、支持脚であつ た脚体 2をそれぞれ該空中期におヽても遊脚、支持脚と呼ぶ。

[0075] また、目標歩容の軌道は、グローバル座標系(床に固定された座標系)で記述され る。グローバル座標系としては、例えば支持脚足平 22の着地位置姿勢に対応して定 まる支持脚座標系が用いられる。この支持脚座標系は、例えば支持脚足平 22の底 面のほぼ全面を床に接地した状態で、その足平 22が連結された足首関節の中心か ら床面に延ばした垂線が床と交わる点を原点とし、その原点を通る水平面に支持脚 足平 22を投影したときの該足平 22の前後方向を X軸方向、左右方向を Y軸方向と する座標系(Z軸方向は鉛直方向)である。

[0076] 図 5は、歩容生成装置 100の詳細を示すブロック図である。この図 5を参照して、歩 容生成装置 100の処理のより具体的な概要を以下に説明する。

[0077] 図示の如ぐ歩容生成装置 100は歩容パラメータ決定部 100aを備える。歩容パラ メータ決定部 100aは、目標歩容を規定する歩容パラメータの値あるいは時系列テー ブルを決定する。

[0078] 本明細書の実施形態では、歩容パラメータ決定部 100aが決定する歩容パラメータ には、目標歩容のうちの、目標足平位置姿勢軌道、目標腕姿勢軌道、目標 ZMP軌 道、および目標床反力鉛直成分軌道をそれぞれ規定するパラメータが含まれる。

[0079] ここで、歩容生成装置 100が目標歩容を生成するとき、遊脚足平 22の着地予定位 置姿勢、着地予定時刻、あるいは歩幅、移動速度等の歩容生成用の基本的な要求 値 (要求パラメータ)が、前記ジョイスティック 73、もしくは図示しない行動計画部(ロボ ット 1の行動計画を作成する装置)などの装置力歩容生成装置 100に与えられる。 あるいは、上記要求パラメータをあらかじめ記憶保持した記憶媒体カも該要求パラメ ータを歩容生成装置 100が読み込む。そして、歩容生成装置 100の歩容パラメータ 決定部 100aは、その要求パラメータに応じて歩容パラメータを決定する。

[0080] また、本明細書の実施形態では、歩容パラメータ決定部 100aが決定する歩容パラ メータには、基準上体姿勢軌道、 ZMP許容範囲、床反力水平成分許容範囲をそれ ぞれ規定するパラメータも含まれる。

[0081] ここで、前記基準上体姿勢軌道は、最終的に歩容生成装置 100が出力するもので はないが、目標歩容を決定するときに参酌されるものである。この基準上体姿勢軌道 は、ロボット 1の上体姿勢に関して、前記ジョイスティック 73あるいは行動計画部から 与えられ、もしくはあらかじめ定められた要求 (上体姿勢を鉛直姿勢に保つなどの要 求)にそのまま従って生成される上体姿勢軌道である。目標上体姿勢 (以降、「基準」 が付いていない「上体姿勢」は、目標上体姿勢を表す)は、基準上体姿勢に長期的 に追従する力、または一致するように生成される。

[0082] また、前記 ZMP許容範囲に関して補足すると、本明細書の実施形態では、目標歩 容は、目標 ZMPのまわりに補正目標床反力モーメント (これは一般には 0ではな、) を発生するように修正される。したがって、目標 ZMPは、本来の定義 (床反力モーメ ント水平成分が 0である点という定義)とは異なる点となり、本来の定義を満足する ZM P (以下、真の ZMPと、う)は、補正目標床反力モーメントを目標床反力鉛直成分で

割った値だけ目標 ZMPからずれた位置に移る。

[0083] 修正された歩容 (歩容生成装置 100が最終的に出力する目標歩容)の真の ZMP は、少なくとも ZMP存在可能範囲(いわゆる支持多角形のこと。床と足平 22の底面と の間に粘着力が作用しないと仮定した場合における床反力中心点 (ZMP)の存在可 能範囲)内になければならない。さらにロボット 1の安定余裕を十分にとるためには、 修正された歩容の真の ZMPは、 ZMP存在可能範囲のなかの中心付近の範囲にあ ることが望ましい。そこで、本明細書の実施形態では修正された歩容の真の ZMPが 存在できる許容範囲を設定する。この範囲を ZMP許容範囲と呼ぶ。 ZMP許容範囲 は、 ZMP存在可能範囲と一致するように、あるいは ZMP存在可能範囲内に包含さ れるように設定される。

[0084] なお、前記したように、目標 ZMPまわりの補正目標床反力モーメントを目標床反力 鉛直成分で除算したものが、目標 ZMPに対する真の ZMPの位置のずれ量を表す ので、目標 ZMPまわりの補正目標床反力モーメントを設定する代わりに、目標 ZMP に対する真の ZMPの位置のずれ量 (補正目標床反力モーメントの ZMP換算値)を 設定してもよい。また、 ZMP許容範囲は、その境界の位置と目標床反力鉛直成分と を用いて、補正目標床反力モーメントの許容範囲に変換することができ、その補正目 標床反力モーメントの許容範囲を ZMP許容範囲の代わりに設定するようにしてもよ い。

[0085] また、前記床反力水平成分許容範囲は、ロボット 1の足平 22の床との接地面に、足 平 22が滑らないような大きさの摩擦力を発生させ得る床反力水平成分の許容範囲で ある。本明細書の実施形態では、少なくとも最終的に歩容生成装置 100が出力する 目標歩容の運動(目標運動)は、それによつて発生するロボット 1の慣性力の水平成 分に釣り合う床反力水平成分が床反力水平成分許容範囲内に収まるように生成され る。

[0086] 歩容パラメータ決定部 100aで決定された歩容パラメータは目標瞬時値発生部 100 bに入力される。目標瞬時値発生部 100bは入力された歩容パラメータに基づき、基 準上体姿勢、目標足平位置姿勢、目標 ZMP、目標床反力鉛直成分等、目標歩容の 一部の構成要素の瞬時値 (前記制御ユニット 60の所定の制御処理周期毎の値)を 逐次算出(発生)する。なお、図 5では一部の目標瞬時値のみを代表的に記載してい る。

[0087] 目標瞬時値発生部 100bで算出された目標瞬時値は、単純化モデル歩容生成部 1 00cに入力される。単純ィ匕モデル歩容生成部 100cは、入力された目標瞬時値を基 に、ロボット 1の運動と床反力との関係を近似表現する後述の動力学モデル (以下、 単純化モデルと!/、う)を用いて目標上体位置姿勢 (仮目標上体位置姿勢)の瞬時値 を算出する。単純ィ匕モデル歩容生成部 100cは、単純化モデル上での動力学的平 衡条件が満たされるように、すなわち単純ィ匕モデル上でのロボット 1の目標運動によ つて発生する慣性力とロボット 1に作用する重力との合力が目標 ZMPまわりに発生す るモーメントの水平成分が 0になるように目標上体位置姿勢の瞬時値を算出する。補 足すると、本明細書の実施形態では、目標床反力鉛直成分軌道も明示的に設定す るので、目標上体位置姿勢の瞬時値は、目標運動によって発生する慣性力とロボット 1に作用する重力との合力が目標 ZMPまわりに発生するモーメント水平成分力^にな るほか、その合力の並進力鉛直成分 (換言すれば、ロボット 1の全体重心の鉛直方向 の並進運動に伴う慣性力と重力との合力)が目標床反力鉛直成分に釣り合うように決 定される。

[0088] これにより、目標上体位置姿勢を含む目標歩容 (仮目標歩容)の瞬時値が逐次決 定されていくこととなる。以降、単純ィ匕モデル歩容生成部 100cで求められた目標上 体位置姿勢を構成要素とする目標歩容を単純化モデル歩容という。なお、単純化モ デル歩容生成部 100cに入力される目標瞬時値は、目標瞬時値発生部 100bで算出 されたすべての目標瞬時値である必要はない。単純ィ匕モデル歩容生成部 100cに必 要な入力は、単純化モデルの構造、あるいはそれに適宜付加される制約条件に依 存する。例えば図 5では、目標足平位置姿勢を単純ィ匕モデル歩容生成部 100cに入 力しているが、第 1実施形態における後述する単純化モデルでは、目標足平位置姿 勢を入力する必要はない。

[0089] なお、単純ィ匕モデル歩容生成部 100cは、目標瞬時値発生部 100bと合わせて、本 発明における瞬時歩容発生手段を構成する。

[0090] 単純化モデル歩容生成部 100cで算出された目標上体位置姿勢は、変位次元歩 容補正部 lOOdに入力される。変位次元歩容補正部 lOOdには、目標上体位置姿勢 のほ力、目標足平位置姿勢の瞬時値、目標 ZMPの瞬時値も入力される。但し、変位 次元歩容補正部 lOOdには、目標 ZMPを入力することは必須ではなぐより一般的に は、後述する角運動量積に関する中心点が入力される。図 5では、その中心点の一 例として目標 ZMPを変位次元歩容補正部 lOOdに入力するようにしている。なお、後 述の第 3実施形態では、変位次元歩容補正部 lOOdには、上記した入力値のほか、 目標腕姿勢の瞬時値も入力される。

[0091] この変位次元歩容補正部 lOOdは、入力された目標上体位置姿勢の瞬時値などを 基に、後述の第 1および第 2変位次元補正用モデルを用いて、単純化モデル歩容生 成部 100cで求めた目標上体位置姿勢を補正してなる変位次元補正上体位置姿勢 の瞬時値を求める。詳細は後述するが、第 1および第 2変位次元補正用モデルは、 一般的には、質点とイナ一シャをもつリンクとのうちの少なくともいずれか一方を要素 として構成されるモデル (幾何学モデル)であり、その要素の配置 (質点の位置、リン クの姿勢)力ロボット 1の瞬時運動における 1つ以上の部位の位置姿勢に対応づけ られる。この場合、これらの第 1および第 2変位次元補正用モデルは、共に同じ要素 で構成される。但し、これらの第 1および第 2変位次元補正用モデルでは、その要素 の配置に関して互いに異なる幾何学的拘束条件が定められており、ロボット 1の任意 の瞬時目標運動(ロボット 1の各部位の位置姿勢の瞬時値)と、それに対応する各変 位次元補正用モデルの要素の配置とが各別の幾何学的拘束条件に基づいて対応 づけられるようになつている。従って、ある目標瞬時運動が与えられたとき、それに対 応する各変位次元補正用モデルの要素の配置は一般には互いに異なる。そして、 変位次元歩容補正部 lOOdは、一般的には、これらの第 1および第 2変位次元補正 用モデルにおける要素の配置の差 (質点の位置の差、あるいはリンクの姿勢角の差) を基に、単純化モデル歩容の瞬時目標運動を逐次補正する。本明細書の各実施形 態では、変位次元歩容補正部 lOOdは、両変位次元補正用モデルの要素の配置の 差を基に、単純化モデル歩容の瞬時目標運動のうちの目標上体位置姿勢を補正し 、それにより変位次元補正上体位置姿勢の瞬時値を逐次求める。

[0092] 変位次元歩容補正部 lOOdで求めた変位次元補正上体位置姿勢の瞬時値は、フ ルモデル補正部 100eに入力される。フルモデル補正部 100eには、変位次元補正 上体位置姿勢の瞬時値のほか、目標瞬時値発生部 100bで算出された各目標瞬時 値 (基準上体位置姿勢の瞬時値を除く)が入力される。このフルモデル補正部 100b は、単純化モデルよりも動力学的精度の高、動力学モデルとしてのフルモデルを用 いて変位次元上体位置姿勢を補正してなる補正目標上体位置姿勢を算出すると共 に、目標 ZMPまわりの床反力モーメント水平成分の目標値である補正目標床反カモ 一メントを算出する。

[0093] フルモデル補正部 100eは、より一般的には、次の D1— D3の条件を満足するように 、 E1あるいは E2の処理を実行する。すなわち、フルモデル補正部 100eは、

D1)単純ィ匕モデルを用いて生成した歩容(単純化モデル歩容)を変位次元補正用モ デルを用いて修正してなる歩容 (以降、変位次元補正歩容と呼ぶ)よりも高!、精度で 動力学的平衡条件を満足する。

D2)真の ZMP (目標 ZMPのまわりに補正目標床反力モーメントを発生させることによ つて修正された本来の定義を満足する ZMP)は、 ZMP許容範囲 (安定余裕が十分 維持できる許容範囲)に存在する。

D3)床反力水平成分は床反力水平成分許容範囲内になる。

という条件を満足するように、

E1)前記変位次元補正歩容の上体位置姿勢を補正する。

あるいは

E2)前記変位次元補正歩容の上体位置姿勢を補正すると共に、目標 ZMPまわりの 補正目標床反力モーメントを出力する(目標床反力を補正する)。

[0094] 本明細書の実施形態では、 D1— D3の条件を満足するように、 E2の処理が実行され る。なお、本明細書の実施形態におけるフルモデル補正部 100eの処理は、例えば 本願出願人が先に提案した PCT国際公開公報 WOZ03Z057427ZA1にて詳細 に説明されているもの(具体的には、同公報の図 13の S038の処理)と同じである。 従って、本明細書でのフルモデル補正部 100eの処理の詳細な説明は省略する。

[0095] 図 4に戻って、上述のように決定される補正目標上体位置姿勢、目標 ZMPまわりの 補正目標床反力モーメント、目標足平位置姿勢の瞬時値を含む目標歩容の瞬時値

は、複合コンプライアンス制御装置 101 (図 4で破線で囲んだ部分)に入力される。こ の複合コンプライアンス制御装置 101は、ロボット 1のバランスを保ちつつ、目標歩容 に追従するように関節ァクチユエータ (電動モータ 64)を制御する。

[0096] 以上が歩容生成装置 100の概要である。なお、以上説明した歩容生成装置 100の 概要は、本明細書のヽずれの実施形態にぉヽても同じである。

[0097]

[第 1実施形態]

次に、本発明の第 1実施形態を具体的に説明する。まず、第 1実施形態における前 記単純化モデル (動力学モデル)、第 1変位次元補正用モデル、第 2変位次元補正 用モデルについて説明する。なお、第 1実施形態は、前記第 1一第 9発明、並びに、 第 11一第 14および第 16発明の一実施形態である。

[0098] 図 6は、第 1実施形態における単純ィ匕モデルの構造を示している。図示の如ぐこ の単純化モデルは、ロボット 1の上体 3に対応する 1つの質点(上体質点) 3mを備え る 1質点モデルである。なお、図 6に示すロボット 1は、側面から見たロボット 1を模式 化して示し、腕体 5, 5や頭部 6の図示を省略している。この図 6以降の図面 (第 1実施 形態以外の実施形態の図面を含む)では、ロボット 1を図示するときに、特に上体 3と 区別する必要がある場合を除いて、図 6と同様に、腕体 5, 5や頭部 6の記載を省略 する。また、図 6を含む以降の図面で記載される X軸、 Z軸は、グロ一ノレ座標系を示 している。

[0099] 図 6の単純化モデルの上体質点 3mは、上体 3の位置姿勢に対応して一義的に定 まる点、すなわち上体 3に任意に固定設定されたローカル座標系でのある固定点(口 一カル座標系で上体 3の代表点と所定の位置関係を有する点)に設定されて!、る。 また、上体質点 3mの質量は、ロボット 1の総質量 mtotalと同一とされている。なお、上 体質点 3mは、上体 3の代表点と一致していてもよいが、一般には異なる。

[0100] この単純化モデルの動力学は、上体質点 3mと、これを目標 ZMPを支点として揺動 自在に支持する可変長リンク 3bとから構成される倒立振子の動力学により表現される 。より具体的には、単純ィ匕モデルでのロボット 1の運動と床反力との関係を表す運動 方程式は、以下に示す式 01、式 02、式 03で表される。ただし、本明細書の理解を容 易にするために、ここではサジタルプレーン (前後軸 (X軸)と鉛直軸 (Z軸)を含む平 面で、いわゆる矢状面)での運動方程式のみを記述し、ラテラルプレーン (左右軸 (Y 軸)と鉛直軸 (Z軸)を含む平面で、 V、わゆる前額面)での運動方程式は省略する。

[0101] なお、本明細書では、任意の変数 Xに対して d2X/dt2は変数 Xの 2階微分値を意味 するものとする。また、図 6の単純ィ匕モデルの動力学に関する変数は以下のように定 義する。

[0102] g:重力加速度、 Zb:上体質点の鉛直位置、 Xb:上体質点の水平位置、 mtotal:ロボ ット 1の総質量、 Fx:床反力水平成分 (詳しくは並進床反力の前後方向 (X軸)成分)、 Fz :床反力鉛直成分 (詳しくは並進床反力の鉛直方向 (Z軸)成分)、 My:目標 ZMP まわりの床反力モーメント (詳しくは床反力モーメントの左右軸 (Y軸)まわり成分)、 Xzmp :目標 ZMPの水平位置、 Zzmp :目標 ZMPの鉛直位置。

[0103]

Fz= mtotal * (g+d2Zb/dt2) …式 01

Fx=mb * d2Xb/dt2 …式 02

My = -mtotal * (Xb— Xzmp) * (g+d2Zb/dt2)+ mtotal * (Zb— Zzmp) * (d2Xb/dt2)

…式 03

これらの式 01— 03により記述される単純化モデルでは、例えば目標 ZMPと目標床 反力鉛直成分とを決めたとき、式 01に従って、上体質点 3mの鉛直位置 Zbを決定で きることとなる。さらに、ロボット 1の動力学的平衡状態では、式 03の左辺の Myが 0に なる(目標 ZMPまわりの床反力モーメントの水平成分が 0になる)から、上体質点 3m の鉛直位置 Zbと式 03とから上体質点 3mの水平位置 Xbを決定できることとなる。

[0104] なお、第 1実施形態の単純化モデルは 1質点モデルとしたが、例えば各脚体 2の足 平 22の近傍にも質点を持たせた 3質点モデルとしてもよい。また、例えば上体 3が上 体質点 3mのまわりにイナーシャ(慣性モーメント)をもつようなモデルであってもよ!/ヽ。

[0105] 次に第 1実施形態における第 1変位次元補正用モデルを説明する。図 7 (a) , (b) ,

(c)の右側の図は、第 1実施形態における第 1変位次元補正用モデルの構造を示し 、左側の図は、右側の図にそれぞれ対応するロボット 1の目標とする全体的な姿勢状

態 (単純ィ匕モデル歩容の姿勢状態)と前記単純ィ匕モデルとを示している。なお、図 7 ( a) , (b) , (c)の右側に示すロボット 1は、両脚体 2, 2を左右方向(Y軸方向)に並べ て直立姿勢で起立して、る状態のロボット 1を側面視 (サジタルプレーン)で表したも のである。このため、両脚体 2, 2は図面上、重なっている。

[0106] 第 1実施形態の第 1変位次元補正用モデルは、ロボット 1の上体 3に対応する 1つの 上体質点 Al、各脚体 2の膝関節近傍の大腿リンク部分に対応する大腿質点 A2, A 3、および各脚体 2の先端部 (足平 22)にそれぞれ対応する足平質点 A4, A5からな る 5質点モデルである。また、第 1変位次元補正用モデルにおける上体 3 (上体リンク )は、上体質点 A1のまわりにイナーシャ (慣性モーメント) lbを持つものとされている。 つまり、第 1変位次元補正用モデルは、質点 A1— A5とイナーシャ lbをもつ上体リン クとを要素として構成されている。この場合、第 1変位次元補正用モデルの質点 A2 一 A5とイナーシャ lbをもつ上体リンクは、前記した図 6の単純ィ匕モデルが持たない要 素であり、それぞれに対応する部位の運動(上体 3についてはその姿勢変化運動)に よって慣性力を発生するものとなっている。

[0107] この第 1変位次元補正用モデルの上体質点 A1と、足平質点 A4, A5とは、それぞ れに対応する部位 (上体 3、各足平 22)の位置姿勢に対応して一義的に定まる点、 すなわち、対応する部位に任意に固定設定されたローカル座標系上でのある固定 点(その対応する部位のローカル座標系上で、該部位の代表点と所定の位置関係を 有する点)に設定されている。但し、上体 3のローカル座標系上での上体質点 A1の 位置は、図 6に示した前記単純ィ匕モデルの上体質点 3mとは一般には異なる。また、 大腿質点 A2, A3は、各脚体 2の大腿リンク 24に任意に固定設定されたローカル座 標系でのある固定点 (膝関節近傍の固定点)に設定されている。なお、上体質点 A1 、各足平質点 A4, A5および各大腿質点 A2, A3の質量の総和は、ロボット 1の総質 量 mtotalと一致する。また、上体質点 A1の質量は、上体 3の質量のほ力両腕体 5, 5および頭部 4の質量を含んで!/、る。

[0108] そして、第 1変位次元補正用モデルの要素の配置には、ある幾何学的拘束条件が 設定されている。具体的には、第 1変位次元補正用モデルでは、ロボット 1の姿勢状 態は、上体 3を鉛直姿勢にすると共に、両脚体 2, 2をロボット 1の左右方向 (Y軸方向 )に所定間隔で並べて起立している姿勢状態 (直立姿勢状態)に定常的に拘束され ている(このため、図 7 (a) , (b) , (c)の右側の第 1変位次元補正用モデルの図では 一方の脚体 2に対応する各質点 A2, A4はそれぞれ他方の脚体 2に対応する各質点 A3, A5と重なっている)。

[0109] 従って、上体質点 Al、各足平質点 A4, A5および大腿質点 A2, A3の相互の相対 的位置関係が、ロボット 1の直立姿勢状態に対応する所定の位置関係に拘束されて いる。また、第 1変位次元補正用モデルにおいてイナ一シャをもつリンク(剛体)であ る上体 3の姿勢は、鉛直姿勢 (鉛直軸に対する姿勢角が 0となる姿勢)に拘束されて いる。

[0110] また、第 1変位次元補正用モデルの各質点 A1— A5のグローバル座標系(床に固 定された座標系)上での位置は、単純ィ匕モデル歩容の運動の瞬時値に対応して定ま るものとされている。すなわち、第 1実施形態の第 1変位次元補正用モデルでは、そ の質点 A1— A5の全体重心の位置が単純化モデル上でのロボット 1の全体重心の 位置、すなわち、単純化モデルの上体質点 3mの位置(グローバル座標系での位置) と一致するように、質点 A1— A5のグローバル座標系での位置が決定される。この場 合、前記したように第 1変位次元補正用モデルの質点 A1— A5の相互の相対的位置 関係は一定であるので、それらの質点 A1— A5の全体重心の位置(グローバル座標 系での位置)が決まれば、各質点 A1— A5のグローバル座標系での位置も一義的に 疋 る。

[0111] このようにグローバル座標系での質点 A1— A5の位置が単純化モデル歩容に対応 して定められる第 1変位次元補正用モデルでは、その全体重心の運動が単純ィ匕モ デル上での全体重心の運動と一致するため、単純化モデル上でロボット 1に作用す る床反力と、第 1変位次元補正用モデル上でロボット 1に作用する床反力とが同等に なる。

[0112] ここで、第 1実施形態で上記のように第 1変位次元補正用モデルの要素の配置を定 めるということは、第 1変位次元補正用モデルの要素の配置(グローバル座標系での 質点 A1— A5の位置および上体リンクの姿勢)を定めるための幾何学的拘束条件(1 )を次のように定義したとき、単純化モデル歩容の瞬時運動から、その幾何学的拘束

条件(1)に従って第 1変位次元補正用モデルの要素の配置を定めることと同じである

[0113]

幾何学的拘束条件(1):与えられた任意の瞬時目標運動に対して、第 1変位次元 補正用モデルの要素の配置に対応するロボット 1の姿勢状態が定常的に直立姿勢 状態に維持され、且つ第 1変位次元補正用モデルの要素の全体重心が、与えられた 瞬時目標運動におけるロボット 1の全体重心に一致する。

[0114]

第 1実施形態では、この幾何学的拘束条件(1)が本発明における第 1の幾何学的 拘束条件に相当するものである。

[0115] 次に第 1実施形態における第 2変位次元補正用モデルを説明する。図 8は、その第 2変位次元補正用モデルの構造を示して、る。この第 2変位次元補正用モデルは、 その構成要素は、第 1変位次元補正用モデルと同じであり、第 1変位次元補正用モ デルと同様に 5個の質点 A1— A5を有すると共に、上体 3 (上体リンク)が質点 A1の まわりにイナーシャ lbを持つモデルである。各質点 A1— A5の質量と、各質点 A1— A5の、対応する部位に固定設定されたローカル座標系での位置とは、第 1変位次元 補正用モデルと同一である。また、上体 3のイナーシャ lbも第 1変位次元補正用モデ ルと同一である。

[0116] この第 2変位次元補正用モデルでは、第 1変位次元補正用モデルのようにロボット 1の姿勢は直立姿勢状態には拘束されておらず、各質点 A1— A5および上体 3 (上 体リンク)はロボット 1が採り得る任意の姿勢状態に対応する位置姿勢に移動可能とさ れている。

[0117] 別の言い方をすれば、第 2変位次元補正用モデルの要素の配置 (グローバル座標 系での質点 A1— A5の位置および上体リンクの姿勢)と、これに対応するロボット 1の 瞬時目標運動 (各部位の目標位置姿勢の瞬時値)との間には、次のような幾何学的 拘束条件 (2)が設定されて!、ることとなる。この幾何学的拘束条件 (2)は本発明にお ける第 2の幾何学的拘束条件に相当するものである。

[0118]

幾何学的拘束条件(2) :第 2変位次元補正用モデルの各要素の配置により定まる、 ロボット 1の各要素に対応する部位の位置姿勢と、当該配置に対応する瞬時目標運 動におけるロボット 1の各要素に対応する部位の位置姿勢とは一致する。

[0119]

従って、第 2変位次元補正用モデルでは、その質点 A1— A5の全体重心の位置が 、該第 2変位次元補正用モデルの各要素の配置 (質点 A1— A5の位置および上体リ ンクの姿勢)に対応する姿勢状態での実際のロボット 1の真の全体重心の位置にほ ぼ一致する。

[0120] 補足すると、任意の瞬時目標運動から、上記幾何学的拘束条件 (2)に従って第 2 変位次元補正用モデルの要素の配置を決定すると、うことは、第 2変位次元補正用 モデルの各要素の配置力与えられた瞬時目標運動に従うロボット 1における該要素 に対応する部位の配置 (位置姿勢)に一致するように第 2変位次元補正用モデルの 各要素の配置を決定するということと等価である。また、第 2変位次元補正用モデル の要素の任意の配置から、幾何学的拘束条件(2)に従って瞬時目標運動を決定す るということは、瞬時目標運動に従うロボット 1の各要素に対応する部位の配置 (位置 姿勢)が、与えられた第 2変位次元補正用モデルの要素の配置に一致するように瞬 時目標運動を決定すると!ヽうことと等価である。

[0121] 第 2変位次元補正用モデルは、第 1変位次元補正用モデルと協働して前記変位次 元補正上体位置姿勢を決定するモデルであり、その変位次元補正上体位置姿勢を 決定するときに、第 2変位次元補正用モデルの要素の位置姿勢 (グローバル座標系 での位置姿勢)が次のように決定される。すなわち、第 2変位次元補正用モデルの各 足平質点 A4, A5の位置は、単純化モデル歩容の各足平位置姿勢に対応する位置 に決定される。また、上体質点 A1および各大腿質点 A2, A3の位置、並びに上体 3 ( 上体リンク)の姿勢角は、第 1変位次元補正用モデルおよび第 2変位次元補正用モ デルの全体重心と、それらのモデル間の後述する角運動量積とに関する所定の条 件を満たすように決定される。これについては、詳細を後述する。

[0122] 補足すると、本実施形態のロボット 1の各脚体 2はそれぞれ 6自由度をもつので、両 足平 22, 22の位置姿勢および上体 3の位置姿勢が決まれば、ロボット 1の脚体 2, 2 の全体の姿勢 (ロボット 1の各脚体 2, 2の各部位 (各リンク)の位置姿勢)は一義的に 定まる。従って、第 2変位次元補正用モデル上での両足平質点 A4, A5並びに上体 質点 A1の位置および上体 3 (上体リンク)の姿勢を決めれば、それに対応して大腿質 点 A2, A3の位置は従属的に定まる。

[0123] なお、以降の説明では、単純ィ匕モデル、第 1および第 2変位次元補正用モデルに 係わる各質点の「位置」、あるいはイナ一シャをもつリンクの「姿勢」は特にことわらな い限り、グローバル座標系での位置、姿勢を意味するものとする。

[0124] 次に、第 1実施形態での歩容生成装置 100の処理の詳細をより具体的に説明する 。歩容生成装置 100は、以下に説明するフローチャートの処理によって、ロボット 1の 片方の脚体 2が着地して力他方の脚体 2が着地するまでの 1歩分の目標歩容 (前 記狭義の目標歩容)を単位として、その 1歩分の目標歩容を順番に生成する。このと き、新たに生成しょうとしている目標歩容を「今回歩容」と呼ぶ。

[0125] 図 9は、歩容生成装置 100のメインルーチン処理を示す構造化フローチャートであ る。以下、詳説すると、まず S010において時刻 tを 0に初期化するなど種々の初期化 作業が行なわれる。この処理は歩容生成装置 100の起動時等に行なわれる。次いで 、 S012を経て S014に進み、歩容生成装置 100は、制御周期(図 9のフローチャート の演算処理周期)毎のタイマ割り込みを待つ。制御周期は A tである。以降、その制 御周期 A t毎に、 S014から S32までの処理が繰り返される。

[0126] S014の次に S016に進み、歩容の切り替わり目であるか否かが判断され、歩容の 切り替わり目であるときは S018に進むと共に、切り替わり目でないときは S022に進 む。ここで、「歩容の切り替わり目」は、今回歩容の生成を開始するタイミングを意味し 、例えば今回歩容の 1つ前の目標歩容の生成が完了した制御周期の次の制御周期 が歩容の切り替わり目になる。

[0127] S018に進むときには、現在時刻 tを 0に初期化した後、 S020に進んで今回歩容の 歩容パラメータが決定される。この S020の処理は、前記図 5の歩容パラメータ決定部 100aの処理に相当し、目標足平位置姿勢軌道、目標腕姿勢軌道、目標 ZMP軌道 、および目標床反力鉛直成分軌道を規定するパラメータが決定されると共に、基準 上体姿勢軌道、床反力水平成分許容範囲、および ZMP許容範囲を規定するパラメ

ータが決定される。

[0128] この S020の処理は、例えば本願出願人が先に提案した PCT国際公開公報 WOZ 03/057427/A1 (以下、公報文献 1と!ヽぅ)の図 13の S022— S030に対応する処 理であり、同公報文献 1と同様に行われる。それを要約的に説明すれば、まず、今回 歩容がつながり、もしくは漸近すべき仮想的な周期的歩容としての定常歩容 (ロボット 1の 2歩分の歩容を 1周期とする歩容)が決定される。定常歩容は、今回歩容を含む 2 歩先までの遊脚足平 22の着地予定位置姿勢、着地予定時刻などを基に、周期性の 条件 (定常歩容の 1周期の初期の状態 (ロボット 1の各部の位置姿勢やその変化速度 など)と終端の状態とがー致するという条件)を満たすように決定される。そして、今回 歩容がその定常歩容につながり、もしくは漸近するように目標足平位置姿勢軌道、目 標腕姿勢軌道、目標 ZMP軌道、および目標床反力鉛直成分軌道を規定する歩容 ノ メータが決定される。ここで、目標足平位置姿勢軌道を規定する歩容パラメータ は、例えば本願出願人が先に特許 3233450号で提案した有限時間整定フィルタを 用いて目標足平位置姿勢軌道を生成する場合、今回歩容の遊脚足平 22の着地予 定位置姿勢および着地予定時刻、今回歩容の支持脚足平 22の次の着地予定位置 姿勢および着地予定時刻などから構成される。また、例えば目標 ZMP軌道、目標床 反力鉛直成分軌道を規定する歩容パラメータは、それらの軌道が折れ線状の軌道で あるとした場合、その折れ点での値やその折れ点の時刻など力も構成される。なお、 本実施形態では、基準上体姿勢は、例えば鉛直姿勢 (鉛直軸に対する上体 3の傾斜 角(姿勢角)が 0である姿勢)とされる。また、本実施形態の S020で決定する歩容パラ メータのうち、床反力水平成分許容範囲を規定する歩容パラメータは、前記公報文 献 1の図 13の S030で決定するフルモデル補正用の床反力水平成分許容範囲のパ ラメータに相当するものである。

[0129] 補足すると、前記公報文献 1で今回歩容の歩容パラメータを決定するための処理で は、定常歩容の作成などのために動力学モデルを使用しているが、その動力学モデ ルとして、本実施形態では、前記単純ィヒモデルが用いられる。この場合、本実施形 態の単純化モデルは、公報文献 1の図 11で例示した動力学モデルとは同じでなな いものの、同公報文献 1の動力学モデルの両脚質点の質量を 0とし、且つ上体に関 するイナーシャ(フライホールのイナ一シャ)を 0としたものと等価である。従って、同公 報文献 1の図 11の動力学モデルの両脚質点の質量を 0とし、且つ上体に関するイナ 一シャを 0とすれば、同公報文献 1の図 13の S022— S030の処理をそのまま適用し て、本実施形態における S020の処理を実行できる。また、同公報文献 1の図 13の S 022— S028の処理では、定常歩容の作成などのために、単純化モデル歩容用の床 反力水平成分許容範囲 (この許容範囲は歩容生成装置力出力されるものではない )を設定して使用している力本実施形態では、その単純化モデル歩容用の床反力 水平成分許容範囲は、例えば無限大の範囲とする力もしくは、単純化モデル歩容( あるいは定常歩容)の床反力水平成分が常にその床反力水平成分許容範囲内に収 まるような広い範囲に定めておけばよい。このようにすることで、同公報文献 1に示し たアルゴリズムを本実施形態の S020の処理で支障なく適用できる。

[0130] 次いで、 S020の処理の後、あるいは S016の判断結果が NOであった場合には、 S 022に進んで、今回歩容の瞬時値を決定する。この処理は、前記図 5の目標瞬時値 発生部 100bおよび単純ィ匕モデル歩容生成部 100cで実行される処理であり、 S020 で決定された歩容パラメータに基づ、て今回歩容の瞬時値 (単純ィ匕モデル歩容の瞬 時値)が決定される。

[0131] この処理は、より具体的には前記公報文献 1の図 13の S032の処理に相当するも のであり、同公報文献 1と同様に行われる。それを要約的に説明すれば、前記 S020 で決定した歩容パラメータを基に、目標足平位置姿勢、目標 ZMP、目標腕姿勢、目 標床反力鉛直成分、基準上体姿勢の瞬時値が決定され、さらに、それらの瞬時値を 基に、前記単純化モデル上で、目標 ZMPと目標床反力鉛直成分とを満足するように (ロボット 1の運動が発生する慣性力と重力との合力が目標 ZMPまわりに作用するモ 一メント水平成分が 0になり、また、その合力の並進力鉛直成分が目標床反力鉛直 成分に釣り合うように)目標上体位置姿勢の瞬時値が決定される。ここで、目標上体 位置姿勢の瞬時値に関して補足すると、目標上体姿勢の瞬時値は本実施形態では 基準上体姿勢の瞬時値と同じとされる。また、目標上体位置鉛直成分は、目標床反 力鉛直成分と前記式 01とから求められる単純ィ匕モデルの上体質点 3mの鉛直位置 に対応して決定される。そして、単純化モデルの上体質点 3mの水平位置が前記式

03の左辺を 0とした式を満たすように(目標 ZMPまわりの床反力モーメント水平成分 力 SOになるように)決定され、この上体質点 3mの水平位置に対応して目標上体位置 水平成分が決定される。

[0132] なお、前記公報文献 1の図 13の S032の処理では、単純化モデル歩容用の床反力 水平成分許容範囲を使用しているが、本実施形態では、前記 S020の処理に関して 説明した場合と同様、単純ィ匕モデル歩容用の床反力水平成分許容範囲は、例えば 無限大の範囲とするか、もしくは、単純ィ匕モデル歩容 (あるいは定常歩容)の床反力 水平成分が常にその床反力水平成分許容範囲内に収まるように定めておけばよい。

[0133] S022の処理で、逐次 (制御周期 A t毎に)、瞬時値が決定される目標歩容 (今回歩 容)は、それを簡潔にいえば、前記単純ィ匕モデル上において、その運動が発生する 慣性力と重力との合力が目標 ZMPまわりに発生するモーメントの水平成分が 0になり 、また、その合力の並進力鉛直成分が目標床反力鉛直成分に釣り合うような歩容で ある。

[0134] 次、で、 S024に進んで、変位次元歩容補正サブルーチンを実行する。この変位 次元歩容補正サブルーチンは、本発明の中核に係わるものであり、以下に詳細に説 明する。

[0135] 前記単純化モデルを用いた目標歩容の生成処理では、今回歩容 (発散しな!、今回 歩容)を安定にリアルタイムで決定できるという利点がある反面、生成される歩容の動 力学的な近似精度が低い。このため、本発明の実施形態では、単純ィ匕モデルよりも 動力学的精度の高いフルモデルを使用して、歩容の一部(目標上体位置姿勢、目標 ZMPまわりのモーメント)を補正する。この場合、単純化モデル歩容の動力学的近似 精度が低いこと、フルモデルの非線形性が強いことなどのために、単純ィ匕モデル歩 容をフルモデルに入力するようにすると、単純化モデル歩容の補正が適正に行われ ずに、ロボット 1の継続的な運動を行い得ない歩容が生成されてしまうなどの不具合 が生じる場合がある。特に、ロボット 1の走行のように脚体 2の運動が短い時間内で大 きく変化するような歩容を生成する場合には、単純ィ匕モデルでは考慮されて、なヽ 各脚体 2の膝関節の曲げ動作などに伴う慣性力の変化の影響が大きくなつて、単純 化モデル歩容の動力学的な近似精度が低下し、上記の不具合が生じやすくなる。そ して、このような不具合を解消するために、例えば各脚体 2に複数の質点をもつ動力 学モデルを構築したり、さらには上体などの、ロボットの 1つ以上のリンクにイナーシャ (慣性モーメント)をもつ動力学モデルを構築し、それを単純ィ匕モデルとして用いるこ とで、単純ィ匕モデル歩容の動力学的近似精度を高めることが考えられる。しかし、こ のようにした場合には、単純ィ匕モデルの非線形性が高まって、定常歩容につながる 今回歩容の歩容パラメータ(ロボット 1の運動の継続性を確保し得る歩容パラメータ) を安定して適正に見出すことが困難となる場合があると共に、その演算処理に時間 がかかり、ひいては、適正な歩容をリアルタイムに生成することが困難となる。

[0136] そこで、本実施形態 (第 1実施形態)を含む本発明の各実施形態では、第 1および 第 2変位次元補正用モデルを用いて、目標 ZMPや床反力を含む動力学方程式を 用いたりすることなぐ第 1および第 2変位次元補正用モデルの要素の配置 (質点とィ ナーシャをもつリンクの位置、姿勢)に関する幾何学的な処理 (位置と姿勢との変位 の次元での処理)によって、単純ィヒモデル歩容の一部の運動(具体的には上体位置 姿勢)のみを補正するようにした。そして、それによつて、単純ィ匕モデル歩容よりも動 力学的精度の高い歩容、詳しくは、その歩容の運動によってロボット 1が発生する実 際の慣性力と重力との合力の並進力成分がより高精度に目標床反力の並進力成分 に釣り合い、且つ、該合力が目標 ZMPまわりに作用するモーメントの水平成分がより 高精度に 0になるような歩容を生成するようにした。この場合、一般に、両変位次元補 正用モデルは、ロボット 1のいくつかの部位に対応する質点を同じように備え、あるい は、質点とイナ一シャを持つリンク (上体 3など)とを同じように備える。また、両変位次 元補正用モデルは、単純化モデルよりも多くの質点を有し、あるいは、単純化モデル が持たないイナ一シャを持つ。但し、第 1変位次元補正用モデルでは、各質点の相 互の位置関係あるいはイナ一シャを持つリンク(上体 3など)の姿勢にある適当な拘束 条件が付加される。

[0137] より詳しく説明すると (ここでは第 1実施形態に限らない一般的な説明を行なう)、生 成した単純ィ匕モデル歩容の各部位の位置姿勢の瞬時値 (運動の瞬時値)に対応し て、第 1変位次元補正用モデルの各質点の位置が決定される。このとき、第 1変位次 元補正用モデルが、ロボット 1の 1つ以上のリンク(上体 3など)にイナ一シャをもつモ デルである場合には、そのリンクの姿勢角も決定される。ただし、第 1変位次元補正 用モデルでは、各質点の位置関係、あるいはイナ一シャを持つリンク(上体 3など)の 姿勢にある適当な幾何学的拘束条件が付加されており、これによつて、単純化モデ ル歩容の各部位の位置姿勢の瞬時値 (運動の瞬時値)に対応させて、第 2変位次元 補正用モデルの各質点の位置や、イナ一シャをもつリンクの姿勢角を決定したとき、 単純ィヒモデル歩容の床反力と同じような床反力が第 1変位次元補正用モデルでも発 生するようにする。

[0138] さらに、第 1変位次元補正用モデルで設定された幾何学的拘束条件を持たない第 2変位次元補正用モデルに対し、第 1変位次元補正用モデルとの間で、次の条件 1 , 2を満足するように第 2変位次元補正用モデルの各質点の位置 (ある、は各質点の 位置とイナ一シャをもつリンクの姿勢角)が決定される。

[0139]

条件 1)第 1変位次元補正用モデルの全体重心の位置と第 2変位次元補正用モデル の全体重心の位置とはほぼ一致する。

条件 2)ある点 Qを定めたとき、第 1変位次元補正用モデルに対する第 2変位次元補 正用モデルの、点 Qのまわりの角運動量積の総和がある一定値 (所定値)になる。

[0140]

ここで、条件 1は、両変位次元補正用モデルで、並進床反力もしくは全体重心の運 動によって発生する慣性力がほぼ同じになるための条件である。条件 1は別の言い 方をすれば、第 1変位次元補正用モデルの各質点の位置とこれに対応する第 2変位 次元補正用モデルの質点の位置との差 (位置ベクトルの差)のベクトルを該質点の並 進加速度とみなしたときに、各質点が発生する慣性力の並進力成分 (質点の質量 * 並進加速度)の、全ての質点についての総和がほぼ 0になるという条件と等価である

[0141] また、条件 2に関する前記角運動量積は、各変位次元補正用モデルの各質点に対 する基準位置をそれぞれ任意に定め、また、前記点 Qの位置を任意に定めたとき、 その各質点毎に以下の如く定義されるものである。さらに、各変位次元補正用モデ ルカ Sイナ一シャを持つ場合 (ある 1つ以上のリンクにイナ一シャが設定されている場合 )には、そのイナ一シャをもつ各リンクに対する基準姿勢角をそれぞれ任意に定めた とき、角運動量積は、その各リンク毎に以下の如く定義されるものである。

[0142] すなわち、各変位次元補正用モデルの各質点に係る角運動量積は、点 Qとその質 点に対応する基準位置の点(以下、基準点という)とを結ぶ線分 (その線分のベクトル )と、該質点の該基準点からの位置のずれ (その位置ずれのベクトル)との外積に、該 質点の質量を乗算したものに相当するものである。なお、この場合、上記外積と質量 との積に比例関係を有するもの、あるいは該外積と質量との積に近似的に等しいもの を当該質点に係る角運動量積と定義してもよい。また、各変位次元補正用モデルの 、イナ一シャを持つリンクに係る角運動量積は、そのリンクの姿勢角の、該リンクに対 応する基準姿勢角からのずれと該リンクのイナーシャとの積に相当するものである。 なお、この場合、リンクの姿勢角の基準姿勢角からのずれとイナーシャとの積に比例 関係を有するもの、あるいは該積に近似的に等しいものを当該リンクに係る角運動量 積と定義してもよい。

[0143] また、各変位次元補正用モデルの質点に係る角運動量積にっ、て補足すると、任 意の質点に係る角運動量積は、質点と前記所定の点 Qとを結ぶ線分と、その質点に 対する基準点と前記所定の点 Qとを結ぶ線分とのなす角度に対して単調に変化する 関数 (単調増加関数または単調減少関数)となる。

[0144] このように角運動量積を定義したとき、条件 2は、より詳しく言えば、第 1変位次元補 正用モデルの各質点の位置を、第 2変位次元補正用モデルの各質点に対応する基 準位置とし、また、第 1変位次元補正用モデルのイナ一シャをもつ各リンクの姿勢角 を、第 2変位次元補正用モデルのイナ一シャを持つ各リンクの基準姿勢角としたとき の、第 2変位次元補正用モデルの角運動量積の総和がある一定値になる、という条 件である。

[0145] また、別の言!、方をすれば、条件 2は、第 1変位次元補正用モデルの各質点の位 置とこれに対応する第 2変位次元補正用モデルの質点の位置との差 (位置ベクトル の差)のベクトルを該質点の並進加速度とみなし、また、両変位次元補正用モデルの イナ一シャをもつ各リンクの姿勢角の差を該リンクの角加速度とみなしたときに、各質 点が発生する慣性力の並進力成分が点 Qのまわりに作用するモーメントとイナーシャ をもつ各リンクの慣性力(回転運動の慣性力)が点 Qまわりに作用するモーメントとの 総和がある一定値 (所定値)になる、という条件と等価である。

[0146] そして、本明細書で詳説する実施形態では、上記点 Qは例えば目標 ZMPに設定 される。なお、点 Qは、目標 ZMPに限られるものではないが、これについては後に補 足する。また、必ずしも上記条件 1, 2の両者を満たす必要はないが、これについても 後に補足する。

[0147] 本明細書の実施形態では、上記した如ぐ第 2変位次元補正用モデルの各質点の 位置 (イナ一シャを持つ場合には、各質点の位置とイナ一シャをもつ各リンクの姿勢 角)を決定することで、単純ィ匕モデル歩容の目標上体位置姿勢を補正してなる変位 次元補正上体位置姿勢が求められる。前記図 6のフローチャートの S024の処理は、 上記の如く変位次元補正上体位置姿勢を求める処理である。

[0148] 以下、第 1実施形態での S024のサブルーチン処理を図 10を参照して具体的に説 明する。なお、ここでは、本明細書の実施形態の理解の便宜上、サジタルプレーン( X軸、 Z軸を含む平面)上での上体位置姿勢の補正 (変位次元補正上体位置姿勢の 算出)について説明し、ラテラルプレーン (Y軸、 Z軸を含む平面)上での上体位置姿 勢の補正に関しては省略する。

[0149] まず、 S200にお!/、て、現在時刻 tでの単純化モデル歩容の瞬時値(目標上体位置 姿勢などの目標運動の瞬時値)を基に、第 1変位次元補正用モデルの各質点 A1— A5の位置と、イナ一シャをもつ上体 3 (上体リンク)の姿勢角とを求める。つまり、単純 化モデル歩容の瞬時値から、前記幾何学的拘束条件(1)に従って、第 1変位次元補 正用モデルの要素の配置 (これは第 1および第 3発明における第 1の配置に相当す る)を求める。具体的には、単純ィ匕モデル歩容でのロボット 1の全体重心の位置と、第 1変位次元補正用モデル上でのロボット 1の全体重心の位置とが等しくなるように第 1 変位次元補正用モデルの各質点 A1— A5の位置が決定される。この場合、本実施 形態では、単純化モデル歩容でのロボット 1の全体重心の位置は、単純化モデルの 上体質点 3mの位置に一致するので、その位置は、単純化モデル歩容の目標上体 位置姿勢から一義的に定まる。そして、第 1変位次元補正用モデルでは、前記した 通り各質点 A1— A5の相対的位置関係が拘束されているので、それらの質点 A1—

A5の全体重心の位置(第 1変位次元補正用モデルでのロボット 1の全体重心の位置 )を単純化モデルの上体質点 3mの位置に一致させることで、各質点 A1— A5の位 置が一義的に決定されることとなる。また、第 1変位次元補正用モデルの上体リンク の姿勢角は、単純化モデル歩容の上体姿勢角(本実施形態では鉛直姿勢)と同一と される。

[0150] 次いで、 S202からの処理が実行され、第 1変位次元補正用モデルに対して前記条 件 1、 2を満たすような第 2変位次元補正用モデルの各質点 A1— A5の位置と、イナ 一シャをもつ上体 3 (上体リンク)の姿勢角との組、すなわち、第 2変位次元補正用モ デルの要素の配置が探索的に決定される。そして、その配置 (これは第 1発明におけ る第 3の配置、第 3発明における第 2の配置に相当する)における上体質点 A1およ び上体リンクの姿勢に対応するロボット 1の上体位置および姿勢力それぞれ変位次 元補正上体位置 Pb2および変位次元補正上体姿勢 Θ b2として決定される。

[0151] さらに詳細には、まず、 S202において、変位次元補正上体位置 Pb2および変位次 元補正上体姿勢 0 b2の初期候補 (Pb2— s, 0 b2— s)を決定する。初期候補 (Pb2— s, Θ b2_s)は、今回時刻 t (現在時刻 t)での変位次元補正上体位置 Pb2および変位次元 補正上体姿勢 Θ b2の概略的な予想値に相当するものであり、例えば次のように決定 される。すなわち、今回時刻 tでの変位次元補正上体位置 Pb2と単純化モデル歩容 の上体位置 Pbとの差分 (位置のずれ量)は、前回時刻(前回の制御周期の時刻) t A tでの Pb2と Pbとの差分に近いと考えられる。同様に、今回時刻 tでの変位次元補 正上体姿勢 Θ b2と単純化モデル歩容の上体姿勢 Θ bとの差分 (姿勢角のずれ量)は 、前回時刻 t A tでの Θ b2と Θ bとの差分に近いと考えられる。そこで、初期候補 (P b2_s, 0 b2_s)を、今回時刻 tでの Pb, 0 bと、前回時刻 t A tでの Pb, 0 bの値 Pb_p, 0 b_pと、前回時刻 t- A tでの Pb2, 0 b2の値 Pb2_p, Θ b2_pと力も次式により決定する

[0152]

Pb2_s = Pb + (Pb2_p-Pb_p) · · ·式 04

0 b2_s= 0 b+ ( 0 b2_p- 0 b_p) …式 05

次いで、 S204を経て、 S206— S216のループ処理を実行する。 S206では、変位 次元補正上体位置姿勢の現在の候補 (Pb2_s, Θ b2_s)と、今回時刻 tでの単純化モ デル歩容の目標両足平位置姿勢とを基に、第 2変位次元補正モデルでの各質点 A1 一 A5の位置を求める。この場合、第 2変位次元補正用モデルでのロボット 1の上体 3 の位置姿勢が現在の候補 (Pb2_s, Θ b2_s)に一致し、且つ、第 2変位次元補正用モ デルでのロボット 1の各足平 22の位置姿勢が単純化モデル歩容の目標足平位置姿 勢に一致しているとして、各質点 A1— A5の位置が求められる。別の言い方をすれ ば、単純ィ匕モデル歩容の運動瞬時値のうちの、上体位置姿勢の瞬時値のみを候補( Pb2_s, Θ b2_s)で置き換えた運動瞬時値から、前記幾何学的拘束条件(2)に従って 、第 2変位次元補正用モデルにおける各質点 A1— A5の位置が求められる。

[0153] 具体的には、足平質点 A3, A4の位置は、目標足平位置姿勢から決定される。ま た、上体質点 A1の位置は、候補 (Pb2_s, Θ b2_s)力決定され、上体 3 (上体リンク) の姿勢角は Θ b2_sと同一とされる。そして、各大腿質点 A2, A3の位置は、目標両足 平位置姿勢と候補 (Pb2_s, Θ b2_s)とから定まるロボット 1の各脚体 2の姿勢力決定 される。補足すると、前記したように本明細書の実施形態のロボット 1では、各脚体 2 はそれぞれ 6自由度を有するので、両足平 22, 22および上体 3の位置姿勢が定まれ ば、各脚体 2の各部の位置姿勢も一義的に定まる。従って、第 2変位次元補正用モ デルの上体質点 A1の位置、上体リンクの姿勢角、両足平質点 A4, A5の位置が定 まれば、各大腿質点 A2, A3の位置が一義的に定まる。

[0154] 次いで、 S208に進み、第 1変位次元補正用モデルと第 2変位次元補正用モデルと の間での全体重心の位置のずれ Gc_err (以下、モデル間全体重心ずれ Gc_errと!、う) と、第 1変位次元補正用モデルに対する第 2変位次元補正用モデルの角運動量積 の総和し errのずれ量(以下、モデル間角運動量積ずれし errという)とを求める。この 処理を以下により具体的に説明する。なお、以下の説明では、第 1および第 2変位次 元補正用モデルの各質点 A1— A5の質量をそれぞれ mi (i= l, 2, · ··, 5)、位置 (位 置ベクトル)を Pilもしくは Pi2 (i= l, 2, · ··, 5)で表す。 Pilは第 1変位次元補正用モ デルの質点 Aiの位置、 Pi2は第 2変位次元補正用モデルの質点 Aiの位置である。ま た、第 1および第 2変位次元補正用モデルでの上体 3 (上体リンク)の姿勢角をそれぞ

れ 0 bl、 Θ b2で表す。本実施形態では、 Θ blは、単純化モデル歩容の目標上体姿 勢 Θ b (鉛直姿勢)と同一である。

[0155] モデル間全体重心ずれ Gc_errおよびモデル間角運動量積ずれし errは、それぞれ 例えば次式 06, 07により算出される。

[0156]

Gc— err =∑ (mi * (Pi2— Pil)) · · ·式 06

L— err =∑ (mi * (Pil—Q) * (Pi2— Pil》 +Ib * ( 0 b2— 0 bl) + Const · · ·式 07

ここで、これらの式の∑はそれに続く括弧内の部分の、全ての質点 Ai (i= l, 2,… , 5)についての総和を意味する。また、式 07の「Const」は、あら力じめ定めた所定値 であり、前記条件 2における「一定値」(所定値)に相当するものである。また、式 07の Qは、本実施形態では、単純化モデル歩容の目標 ZMPの位置と同一である。

[0157] これらの式 06, 07において、式 06の右辺は、第 1変位次元補正用モデルの各質 点 A1— A5の位置 Pil (i= l, 2, · ··, 5)により定まる全体重心の位置と第 2変位次元 補正用モデルの各質点 A1— A5の位置 Pi2 (i= l, 2, · ··, 5)全体重心の位置とのず れを意味している。従って、モデル間全体重心ずれ Gc_errの値が 0 (0ベクトル)もしく はほぼ 0であれば、前記条件 1が満たされることとなる。

[0158] また、式 07の右辺から「Const」を除いた項は、第 1変位次元補正用モデルに対す る第 2変位次元補正用モデルの角運動量積の総和を意味する。別の言!、方をすれ ば、式 07の右辺から「Const」を除いた項は、第 1変位次元補正用モデルの各質点 A 1一 A5の位置 Pil (i= l, 2, · ··, 5)を第 2変位次元補正用モデルの各質点 A1— A5 の基準位置とし、且つ、第 1変位次元補正用モデルの上体 3 (上体リンク)の姿勢角を 、第 2変位次元補正用モデルの上体 3 (上体リンク)の基準姿勢角としたときの、第 2 変位次元補正用モデルの角運動量積の総和を意味する。

[0159] 従って、モデル間角運動量積ずれし errの値が常に 0もしくはほぼ 0であれば前記 条件 2が満たされることとなる。

[0160] ここで式 07の右辺の∑に続く括弧内の項にっ、て補足すると、 (Pil-Q) * (Pi2— P il)は、点 Qと質点 Aiとを結ぶ線分のベクトルと、第 1変位次元補正用モデルの質点 A

iに対する第 2変位次元補正用モデルの質点 Aiの位置ずれのベクトルとの外積であ る。それを視覚的に表現すれば、(Pil— Q)*(Pi2— Pil)は、図 11に示す如ぐ斜線も しくは網掛けを付した各三角形の面積の 2倍の大きさの量に相当する。なお、図 11で は、第 1および第 2変位次元補正用モデルのそれぞれにおける質点 Ai(i=l, 2,… , 5)の位置 Pil, Pi2を Pil(Ai),Pi2(Ai)というように記載している。

[0161] また、角運動量積に関する式 07に関しては、これの代わりに、次式 08— 10のいず れかを用いてもよい。

[0162]

L— err=∑(Ci*mi*角度 (Pil— Q— Pi2))+Ib*( 0 b2- Θ bl) + Const …式 08

L_err=∑ (mi * (質点 Aiの水平成分変位 *高さ》

+Ib*(0b2-0bl) + Const …式 09

L_err=∑ (mi * (質点 Aiの水平成分変位 *高さ) * C (質点 Aiの高さ))

+Ib*(0b2-0bl) + Const …式 10

ここで、式 08中の「角度 (Pil_Q_Pi2)」は、第 1変位次元補正用モデルの質点 Aiと点 Qとを結ぶ線分と、第 2変位次元補正用モデルの質点 Aiと点 Qとを結ぶ線分とのなす 角度を意味する。また、式 08中の「Ci」は所定の係数であり、その値は、 Ci*mi*角 度 (Pil_Q_Pi2)力両変位次元補正用モデルの質点 Aiと点 Qとで形成される三角形の 面積の 2倍にほぼ等しくなるように決定される。また、式 09および 10における「質点 A iの水平成分変位」は、第 1変位次元補正用モデルの質点 Aiと第 2変位次元補正用 モデルの質点 Aiとの位置ずれ (Pi2— Pil)の水平成分を意味し、「高さ」は第 1または 第 2変位次元補正用モデルの質点 Aiの点 Qに対する相対高さ、すなわち、 Pil— Qま たは Pi2— Qの鉛直成分を意味する。また、式 10の「C (質点 Aiの高さ)」は、第 1または 第 2変位次元補正用モデルの質点 Aiの点 Qに対する相対高さ(Pil— Qまたは Pi2— Q の鉛直成分)のある関数値を意味する。この場合、関数値 C (質点 Aiの高さ)は、基本 的には、質点 Aiの高さが高いほど、値が小さくなるような単調関数であることが好適

である。

[0163] 角運動量積に関する上記式 07— 10のいずれを用いても、その式の右辺の「Const 」を除いた項は、角運動量積の総和にほぼ比例する力もしくは概ね等しいものとなる 。なお、式 07— 10の「Const」は一般には互いに相違する。

[0164] 補足すると、前記式 07— 10の右辺の∑に続く各項は、第 1変位次元補正用モデ ルの質点 Aiと点 Qとを結ぶ線分と、第 2変位次元補正用モデルの質点 Aiと点 Qとを 結ぶ線分とのなす角度 (Pil_Q_Pi2)に対してほぼ単調に変化する関数となる。

[0165] 本実施形態では、 S208において、前記式 6の Pilに S200で求めた第 1変位次元 補正用モデルの各質点 A1— A5の位置を代入すると共に、同式 6の Pi2に S206で 求めた各質点 A1— A5の位置を代入することで、モデル間重心位置ずれ Gc_errが 算出される。また、前記式 7の Pil, Pi2を式 6と同じにすると共に、 Θ blに S200で求 めた上体姿勢 (本実施形態では鉛直姿勢)を代入し、さらに、 0 b2に変位次元補正 上体姿勢の候補の現在値 Θ b2_sを代入することで、モデル間角運動量積ずれ L err が算出される。

[0166] 上述のよう〖こ S208〖こおいて、モデル間全体重心ずれ Gc_errとモデル間角運動量 積ずれし errを求めた後、 S210に進んで、 Gc_errとし errとが 0近傍の所定の範囲内 にある力否かが判断される。そして、この判断結果が YESである場合には、 S212を 経て、後述の S218に進む。一方、その判断結果が NOである場合には、 S214に進 んで、変位次元補正上体位置姿勢の現在の候補 (Pb2_s, Θ b2_s)の近辺に複数の 仮候補(Pb2— s+ A Pbx, Θ b2_s)、 (Pb2_s+ A Pbz, Θ b2_s)、 (Pb2_s, Θ b2_s+ A 0 b )を決める。 A Pbx、 A Pbzはそれぞれ変位次元補正上体位置の候補 Pb2_sを現在値 から X軸方向、 Z軸方向に微小量変化させるための所定値であり、 Δ 0 bは変位次元 補正上体姿勢の候補 Θ b2を現在値から Y軸まわりに微小量変化させるための所定 値である。そして、これらの各仮候補に対して、前記 S206、 S208と同じ処理を実行 して、モデル間全体重心ずれ Gc_errとモデル間角運動量積ずれし errとを求める。こ の処理は、変位次元上体位置姿勢の候補 (Pb2_s, 0 b2_s)を現在値力変化させた ときの Gc_errと L_errとの変化の度合いを観測するための処理である。

[0167] 次いで、 S216に進んで、 S214で求めた Gc_err、 L_errを基に、それらの値が 0に近

づくように変位次元補正上体位置姿勢の新たな候補を決定し、それを (Pb2_s, Θ b2_s)に代入する。新たな候補は、例えばヤコビアン (感度マトリクス)を用いて決定さ れる。そして、 S206からの処理が再び実行される。

[0168] 以上のようにして、 S206— S216のループ処理〖こよって、 Gc_errとし errとが 0近傍 の所定の範囲内に収まるような変位次元補正上体位置姿勢、換言すれば、前記条 件 1, 2を満たすような変位次元補正上体位置姿勢が探索的に求められる。

[0169] そして、 S210の判断結果が YESになると、 S212を経て S218に進み、現在の b2_s, 0 b2_s)が今回時刻 tにおける変位次元補正上体位置姿勢 (Pb2, 0 b2)として 決定される。これにより、単純化モデル歩容の上体位置姿勢を補正してなる目標歩 容 (以下、変位次元補正歩容ということがある)が得られることとなる。この変位次元補 正歩容は、単純化モデル歩容のうちの、目標上体位置姿勢のみを補正したものであ り、目標足平位置姿勢、目標 ZMP、目標床反力鉛直成分など、目標歩容の他の構 成要素は単純ィ匕モデル歩容と同一である。

[0170] ここで、前記変位次元補正歩容について図 12—図 14を参照して補足説明をして おく。図 12は第 2変位次元補正用モデルの各質点 Ai (i= l, 2, · ··, 5)の位置および 上体 3 (上体リンク)の姿勢角を単純ィ匕モデル歩容通りに決定した場合 (すなわち単 純ィ匕モデル歩容を前記 S024で補正しな、場合)における第 2変位次元補正用モデ ルの各質点 Aiの位置および上体 3 (上体リンク)の姿勢角と、第 1変位次元補正用モ デルの各質点 Aiの位置および上体 3 (上体リンク)の姿勢角との関係を例示して!/、る 。この場合の第 2変位次元補正用モデルの各質点 Aiの位置および上体 3の姿勢角、 すなわち、第 2変位次元補正用モデルの要素の配置は、別の言い方をすれば、単純 化モデル歩容の瞬時運動から、前記幾何学的拘束条件(2)に従って決定されたもの と言える。従って、この場合の第 2変位次元補正用モデルの要素の配置は、第 1発明 における第 2の配置に相当するものである。なお、前記図 12では、単純化モデル歩 容に対応する第 2変位次元補正用モデルの各質点 Ai (i= l, 2, · ··, 5)の位置を P i2'(Ai)で表している。

[0171] また、図 13は上記した変位次元歩容補正サブルーチンによって決定される変位次 元補正歩容に対応する第 2変位次元補正用モデルの各質点 Aiの位置および上体 3

(上体リンク)の姿勢角と、第 1変位次元補正用モデルの各質点 Aiの位置および上体 3 (上体リンク)の姿勢角との関係を例示して!/、る。

[0172] なお、以降の説明では、一般に、第 1変位次元補正用モデルの任意のある質点と、 これに対応する第 2変位次元補正用モデルの該質点との位置ずれを並進加速度と みなしたときのその並進加速度を、その質点のモデル間擬似並進加速度という。また 、第 1変位次元補正用モデルのイナ一シャをもつあるリンクと、これに対応する第 2変 位次元補正用モデルの該リンクの姿勢角のずれを角加速度とみなしたときのその角 加速度を、そのリンクのモデル間擬似角加速度と、う。

[0173] 図 12に示す例において、単純ィ匕モデル歩容に対応する第 2変位次元補正用モデ ルの各質点 Aiの位置 Pi2'では、両脚体 2, 2の質点 A2— A5のうち、支持脚側の質 点 A2, A4は、第 1変位次元補正用モデルに対して若干、ロボット 1の前方側に位置 ずれする。また、第 2変位次元補正用モデルの遊脚側の質点 A3, A5は、第 1変位 次元補正用モデルに対して比較的大きくロボット 1の後方側に位置ずれする。このた め、第 2変位次元補正用モデルでのロボット 1の全体重心が第 1変位次元補正用モ デルでのロボット 1の全体重心(これは本実施形態では単純化モデル歩容でのロボッ ト 1の全体重心に一致する)よりもロボット 1の後方側 (X軸の負方向)に偏る。別の言 い方をすれば、両脚体 2, 2の各質点 A2— A5のモデル間擬似並進加速度によって 各質点 A2— A5が発生する慣性力の並進力成分( =各質点 A2— A5の質量 *モデ ル間擬似並進加速度)の総和がロボット 1の前方側に比較的大きなものとなる。また、 各質点 A2— A5のモデル間擬似並進加速度によって各質点 A2— A5が発生する慣 性力が目標 ZMPまわりに作用するモーメントが、ロボット 1の前傾側に比較的大きな ものとなる。なお、本実施形態では、単純化モデル歩容の上体姿勢と第 1変位次元 補正用モデルの上体姿勢とは同一であるので、図 12の例では、上体質点 A1のモデ ル間擬似並進加速度は 0であり、また、上体リンクのモデル間擬似角加速度も 0であ る。

[0174] これに対して、前記変位次元歩容補正サブルーチンで変位次元補正歩容に対応 する第 2変位次元補正用モデルの各質点 Aiの位置および上体リンクの姿勢角を決 定したときには、図 13に見られるように、両脚体 2, 2の質点 A2— A5の重心の上記 の偏りを補償するようにして、第 2変位次元補正用モデルの上体質点 A1の位置が、 第 1変位次元補正用モデルの上体質点 A1よりも前方側に決定され、換言すれば、 変位次元補正上体位置が単純化モデル歩容よりも前方側に補正される。同時に、第 2変位次元補正用モデルの各質点 A1— A5の位置並びにイナ一シャをもつ上体 3の 姿勢は、前記した角運動量積の総和がある一定値になるように決定される。図示の 例では、第 2変位次元補正用モデルでの上体姿勢 (実線で示す上体 3の姿勢)は、 単純化モデル歩容の上体姿勢 (破線で示す上体 3の姿勢)に対して、角度 Θ b2- Θ blだけ後傾している。

[0175] このため、変位次元補正歩容に対応する第 2変位次元補正用モデルの各質点 Ai の位置および上体リンクの姿勢角では、各質点 Aiのモデル間擬似並進加速度に伴う 慣性力の並進力成分の総和が図 12に示した場合よりも小さくなつて、ほぼ 0になると 共に、該慣性力が目標 ZMPまわりに作用するモーメントの総和も図 12に示した場合 よりも所定の値 (前記条件 2の「一定値」に相当する値)に近づく。

[0176] これにより、変位次元補正歩容は、単純ィ匕モデル歩容では考慮されて、な、各脚 体の運動に伴う慣性力の影響を補償して、単純化モデル歩容の目標床反力と同じよ うな床反力が発生するようにロボット 1の目標運動を補正したものとなる。

[0177] また、図 14は、単純ィ匕モデル歩容に対する変位次元補正歩容の変化の形態の典 型的な例を示している。これは、より詳しくは、ロボット 1の直立姿勢状態から目標 ZM Pを動力さずに両脚体 2, 2の膝関節を曲げて、上体 3を下げる場合の例であり、図 1 4 (a)は、ロボット 1の直立姿勢状態、図 14 (b)は膝関節を曲げて上体 3を下げた状態 (膝曲げ状態)を示している。ロボット 1の直立姿勢状態では、単純ィ匕モデル歩容と変 位次元補正歩容とはほぼ一致し、ひいては、図 14 (a)に示す如ぐ第 1および第 2変 位次元補正用モデルの各質点 Ai (i= l, 2, · ··, 5)の位置 Pil, Pi2および上体 3の姿 勢は、両変位次元補正用モデルでほぼ一致する。

[0178] この状態から、目標 ZMPを維持したまま上体 3を下げるような歩容を生成するとき、 単純ィ匕モデル歩容は、両足平 22の位置姿勢を維持すると共に、上体 3の姿勢を直 立姿勢 (鉛直姿勢)に維持したまま、上体 3を下げる歩容となる。従って、第 1変位次 元補正用モデルでの各質点 Aiの位置 Pilは、それらの位置関係を維持したまま図 14

(a)の位置から下がる。そして、このとき、両脚体 2, 2の膝関節がロボット 1の前方側 に突き出ることに対応して、第 2変位次元補正用モデルでは、その大腿質点 A2, A3 の位置 P22, P32が、第 1変位次元補正用モデルの大腿質点 A2, A3の位置 P21, P 31よりも図 14 (b)に示す如くロボット 1の前方側の位置になる。このため、両変位次元 補正用モデルで全体重心を一致させるベく(前記条件 1を満たすべく)、第 2変位次 元補正用モデルの上体質点 A1の位置 P12は、第 1変位次元補正用モデルの上体 質点 A1の位置 P1はりも後方側の位置になる。別の言い方をすれば、両大腿質点 A 2, A3のモデル間擬似並進加速度に伴う慣性力の並進力成分を、上体質点 A1のモ デル間擬似並進加速度に伴う慣性力の並進力成分によって打ち消すようにして、第 2変位次元補正用モデルの上体質点 A1の位置 P 12は、第 1変位次元補正用モデル の上体質点 A1の位置 P1はりも後方側の位置になる(脚体 2, 2の上体 3への連結部 (股関節)が後方側に移る)。

[0179] また、このとき、仮に上体 3の姿勢を維持すると、第 1変位次元補正用モデルと第 2 変位次元補正用モデルとの間で前記モデル間角運動量積ずれ L_errが発生する。こ の場合、このモデル間角運動量積ずれは、図 14 (b)に斜線を付した 2つの三角形の 面積の差分の 2倍に相当するものとなるので、このずれを解消すべく(前記条件 2を 満たすべく)、第 2変位次元補正モデルでの上体姿勢 (図 14 (b)に実線で示す上体 3の姿勢)、すなわち変位次元補正歩容の上体姿勢は、第 1変位次元補正モデルで の上体姿勢(図 14 (b)に破線で示す上体 3)、すなわち、単純化モデル歩容の上体 姿勢に対して前傾側に傾く。別の言い方をすれば、両大腿質点 A2, A3および上体 質点 A1のモデル間擬似並進加速度に伴う慣性力が目標 ZMPまわりに作用するモ 一メントを、上体リンクのモデル間擬似角加速度に伴う慣性力が目標 ZMPまわりに作 用するモーメントを打ち消すようにして、第 2変位次元補正歩容の上体姿勢は、単純 化モデル歩容の上体姿勢に対して前傾側に傾く。

[0180] 一般的にいえば、各脚体 2の膝関節が、該脚体 2の股関節の中心と足首関節の中 心とを結ぶ線分に対してロボット 1の前方側に突き出るほど、第 2変位次元補正用モ デルの上体質点 A1の位置は、ロボット 1の後方側に移動し、また、上体 3 (上体リンク )は前傾側に傾く。

[0181] このように単純化モデル歩容に対して運動(上体位置姿勢)を修正した変位次元補 正歩容が決定されることで、目標 ZMPまわりの床反力モーメントの水平成分力^にな ること等の動力学的平衡条件を単純ィ匕モデル歩容と同じように満たしつつ、単純ィ匕 モデル歩容よりも動力学的精度の高い変位次元補正歩容が決定されることとなる。

[0182] 図 9の説明に戻って、前述の如く変位次元歩容補正サブルーチンを実行した後、 S 026に進んで、ロボット 1のスピン (鉛直軸まわりの回転)をキャンセルするための腕体 5, 5の動作が決定される。この処理は、ロボット 1の腕体 5, 5を振らずに目標歩容通 りにロボット 1を運動させた場合に目標 ZMPまわりに発生する床反力モーメントの鉛 直成分と逆向きの床反力モーメントを腕体 5, 5の腕振り(両腕体 5, 5を前後逆方向 に振る運動)によって発生させるように腕体 5, 5の姿勢を決定するものであり、前記 公報文献 1の図 13の S034と同様に行われる。その詳細は、同公報文献 1に記載さ れているので、ここではさらなる説明を省略する。

[0183] 次!、で、 S028に進んで、フルモデル補正用(前記フルモデル補正部 lOOeの処理 用)の ZMP許容範囲の瞬時値と、床反力水平成分許容範囲の瞬時値とが決定され る。これは、前記目標瞬時値発生部 100bで実行される処理であり、前記 S020で決 定した今回歩容パラメータのうちの ZMP許容範囲および床反力水平成分許容範囲 を規定する歩容パラメータに基づいて決定される。

[0184] 次!、で、 S030に進んで、フルモデルを用いた補正歩容を発生する。この処理は、 前記フルモデル補正部 lOOeにより実行される処理である。この処理は、前記公報文 献 1の図 13の S038の処理と同一であり、同公報文献 1に記載されたとおりに実行さ れる。従って、本明細書では詳細な説明は省略する。この処理により、目標上体位置 姿勢 (前記変位次元補正歩容の上体位置姿勢)をさらに修正してなる補正目標上体 位置姿勢と補正目標床反力モーメントとが決定される。

[0185] なお、前記フルモデル補正部 lOOeで用いるフルモデルは、例えば図 15に示す如 ぐロボット 1の上体 3、各脚体 2の股関節、大腿リンク、下腿リンク、足首関節、足平 2 2にそれぞれ質点をもち、また、上体 3 (上体リンク)にイナーシャ lbを持つような多質 点モデルである。この場合、上体 3以外のリンクにもイナ一シャを設定するようにしても よい。

[0186] 以上が、本実施形態における歩容生成装置 100の歩容生成処理である。

[0187] 次に図 4を参照して複合コンプアライアンス制御装置 101の動作を説明しておく。な お、複合コンプライアンス制御装置 101の動作は、本出願人が先に出願した特開平 10— 277969号公報などに詳細に記載されているので、本明細書では概略的な説 明にとどめる。歩容生成装置 100において、上記したように生成された目標歩容のう ち、補正目標上体位置姿勢 (軌道)、目標腕姿勢 (軌道)が、ロボット幾何学モデル( 逆キネマテイクス演算部) 102に送出される。

[0188] また、目標足平位置姿勢 (軌道)、目標 ZMP軌道 (目標全床反力中心点軌道)、お よび目標全床反力(軌道)(補正目標床反力モーメントと目標床反力鉛直成分)は、 複合コンプライアンス動作決定部 104に送られると共に、目標床反力分配器 106にも 送られる。そして、目標床反力分配器 106で、床反力は各足平 22に分配され、目標 各足平床反力中心点および目標各足平床反力が決定される。この決定された目標 各足平床反力中心点および目標各足平床反力は複合コンプライアンス動作決定部 104に送られる。

[0189] 複合コンプライアンス動作決定部 104から、機構変形補償付き修正目標足平位置 姿勢 (軌道)がロボット幾何学モデル 102に送られる。なお、機構変形補償付き修正 目標足平位置姿勢は、各脚体 2のコンプライアンス機構 72の変形 (各脚体 2に作用 する床反力による変形)を考慮しつつ、 6軸力センサ 50により検出される実際の床反 力を目標床反力に近づけるように各足平 22の目標足平位置姿勢を補正したものを 意味する。ロボット幾何学モデル 102は、目標上体位置姿勢 (軌道)と機構変形補償 付き修正目標足平位置姿勢 (軌道)とが入力されると、それらを満足する脚体 2, 2の 12個の関節の関節変位指令 (値)を算出して変位コントローラ 108に送る。変位コン トローラ 108は、ロボット幾何学モデル 102で算出された関節変位指令 (値)を目標値 としてロボット 1の 12個の関節の変位を追従制御する。また、ロボット幾何学モデル 1 02は、目標腕姿勢を満足する腕関節の変位指定 (値)を算出して変位コントローラ 1 08に送る。変位コントローラ 108は、ロボット幾何学モデル 102で算出された関節変 位指令 (値)を目標値としてロボット 1の腕体の 12個の関節の変位を追従制御する。

[0190] ロボット 1に生じた床反力(詳しくは実各足床反力)は 6軸力センサ 50によって検出 される。その検出値は前記複合コンプライアンス動作決定部 104に送られる。また、口 ボット 1に生じた姿勢傾斜偏差 Θ errx, Θ erry (詳しくは目標上体姿勢角に対する実 上体姿勢角の偏差で、ロール方向 (X軸回り)の姿勢傾斜偏差が Θ errxであり、ピッチ 方向 (Y軸回り)の姿勢傾斜偏差が Θ erryである)が姿勢センサ 54を介して検出され 、その検出値は姿勢安定ィ匕制御演算部 112に送られる。この姿勢安定化制御演算 部 112で、ロボット 1の上体姿勢角を目標上体姿勢角に復元するための目標全床反 力中心点(目標 ZMP)まわり補償全床反力モーメントが算出されて複合コンプライア ンス動作決定部 104に送られる。複合コンプライアンス動作決定部 104は、入力値に 基づいて目標床反力を修正する。具体的には、目標全床反力中心点(目標 ZMP) 回りに補償全床反力モーメント、あるいは、補償全床反力モーメントと補正目標床反 力モーメントとの和が作用するように目標床反力を修正する。

[0191] 複合コンプライアンス動作決定部 104は、修正された目標床反力に、センサ検出値 など力も算出される実ロボットの状態および床反力を一致させようと上記機構変形補 償付き修正目標足平位置姿勢 (軌道)を決定する。ただしすベての状態を目標に一 致させることは事実上不可能であるので、これらの間にトレードオフ関係を与えて妥 協的になるベく一致させる。すなわち、各目標に対する制御偏差に重みを与えて、制 御偏差 (あるいは制御偏差の 2乗)の重み付き平均が最小になるように制御する。こ れにより、実際の足平位置姿勢と全床反力とが目標足平位置姿勢と目標全床反力( 目標 ZMPまわりの補償全床反力モーメントを含む)とに概ね従うように制御される。

[0192]

[第 2実施形態]

次に、本発明の第 2実施形態を説明する。本実施形態は、ロボット 1の機構的構成 および制御ユニット 60の機能的構成 (前記図 4に示した機能的構成)は第 1実施形 態と同一で、単純化モデルおよび第 1変位次元補正用モデルと、歩容生成装置 100 の処理の一部とが第 1実施形態と相違するものである。従って、本実施形態の説明 では、第 1実施形態と同一部分については第 1実施形態と同じ符号および図面を用 い、詳細な説明を省略する。なお、第 2実施形態は、本発明の第 1一第 10発明、第 1 3—第 16発明の一実施形態である。

[0193] 図 16は、本実施形態での単純化モデル (動力学モデル)の構造を示し、図 17は本 実施形態での第 1変位次元補正用モデルの構造を示している。

[0194] 図 16に示す本実施形態の単純ィ匕モデルは、ロボット 1の各脚体 2 (詳しくは各脚体 2の足平 22)にそれぞれ対応する 2つの足平質点 2m2, 2m2、及び上体 3に対応する 上体質点 3m2からなる 3質点と、イナーシャ Jがあって質量のないフライホイール FHと 力 構成されるモデルであり、前記公報文献 1の図 11に示したモデルと同一である。 従って、本明細書での詳細な説明は省略する力その概要は次の通りである。

[0195] すなわち、この単純ィ匕モデルでは、各足平質点 2m2, 2m2の動力学 (運動と床反力 との関係)、並びに上体質点 3m2及びフライホイール FHの動力学が相互に非干渉に 構成されると共に、ロボット 1全体の動力学は、それらの線形結合で表される。また、 フライホイール FHの回転運動によって発生する床反力は、上体 3の姿勢角の回転運 動(並進床反力を変化させずに、床反力モーメントだけを変化させる回転運動)によ つて発生する床反力に対応する。なお、上体質点 3m2は、上体 3の位置姿勢に対応 して一義的に定まる点(上体 3に任意に固定されたローカル座標系上でのある固定 点)に設定されており、各脚質点 2m2は、各脚体 2の足平 22の位置姿勢に対応して 一義的に定まる点(足平 22に任意に固定されたローカル座標系上でのある固定点) に設定されている。また、各質点 2m2, 2m2, 3m2の質量の総和は、ロボット 1の総質 量と同一である。上体質点 3m2の質量は、上体 3の質量のほ力、両腕体 5, 5の質量 を含んでいる。

[0196] この単純ィ匕モデルの動力学を記述する式 (運動方程式)は、以下の式 11一 13によ り表される。但し、本明細書の理解の便宜上、ここではサジタルプレーン (前後軸 (X 軸)と鉛直軸 (Z軸)を含む平面)での運動方程式のみを記述し、ラテラルプレーン (左 右軸 (Y軸)と鉛直軸 (Z軸)を含む平面)での運動方程式は省略する。また、式 11一 13の変数は以下のように定義する。

Zsup :支持脚足平質点鉛直位置、 Zswg:遊脚足平質点鉛直位置、 Zb :上体質点鉛 直位置、 Xsup :支持脚足平質点水平位置、 Xswg:遊脚足平質点水平位置、 Xb :上 体質点水平位置、 Θ by:鉛直方向に対する Y軸回りの上体姿勢角、 mb :上体質点質 量、 msup :支持足平質点質量、 mswg:遊脚足平質点質量、 J:フライホイールの慣性 モーメント、 Fx:床反力水平成分、 Fz:床反力鉛直成分、 My:目標 ZMPまわりの床反 力モーメント(詳しくは床反力モーメントの左右軸 (Y軸)まわり成分)

Fz = mb * (g + d2Zb/dt2) + msup * (g + d2Zsup/dt2)

+mswg * (g+d2Zswg/dt2) ……式 11

Fx = mb * d2Xb/dt2 + msup * d2Xsup/dt2 + mswg * d2Xswg/dt2 ……式 12 My=-mb * (Xb— Xzmp) * (g+d2Zb/dt2)+mb * (Zb— Zzmp) * (d2Xb/dt2)

-msup * (Xsup-Xzmp) * (g+d2Zsup/dt2)

+msup*(Zsup— Zzmp) * (d2Xsup/dt2)

—mswg * (Xswg— Xzmp) * (g + d2Zswg/dt2)

-mswg * (Zswg-Zzmp) * (d2Xswg/dt2) +J * d2 0 by/dt2

…式 13

かかる単純化モデルを用いる第 2実施形態では、後述する如ぐ前記公報文献 1と 全く同様に、目標 ZMPを満足する単純ィ匕モデル歩容が生成される。

[0197] 次に、図 17を参照して本実施形態の第 1変位次元補正用モデルを説明すると、こ のモデルは、上体 3、各脚体 2の大腿リンク、足平 22にそれぞれ対応する上体質点 A 1、大腿質点 A2, A3、足平質点 A4, A5からなる 5質点モデルである。また、ロボット 1の上体 3 (上体リンク)力上体質点 A1のまわりにイナーシャ (慣性モーメント) lbをも つものとされている。つまり、本実施形態の第 1変位次元補正用モデルは、第 1実施 形態の第 1および第 2変位次元補正用モデルと同様、質点 A1— A5とイナ一シャをも つ上体リンクとを要素として構成されて、る。

[0198] この場合、上体質点 Al、各足平質点 A4, A5は、前記第 1実施形態の第 1もしくは 第 2変位次元補正用モデルのものと同様、それぞれに対応する部位 (上体 3、各足平 22)の位置姿勢に対応して一義的に定まる点 (その対応する部位に任意に固定設定 したローカル座標系でのある固定点)に設定されている。なお、上体質点 Al、各足 平質点 A4, A5および各大腿質点 A2, A3の質量の総和は、ロボット 1の総質量 m totalと一致する。また、上体質点 A1の質量は、上体 3の質量のほ力、両腕体 5, 5お よび頭部 4の質量を含んで、る。

[0199] そして、本実施形態においても第 1変位次元補正用モデルの要素の配置には、あ る幾何学的拘束条件が設定されている。具体的には、この第 1変位次元補正用モデ ルでは、ロボット 1の各脚体 2の膝関節を、該脚体 2の足首関節の中心と股関節の中 心とを結ぶ線分の方向にのみ伸縮する直動型 (テレスコピック型)の関節とみなして おり、各大腿質点 A2, A3は、対応する脚体 2の足首関節の中心と股関節の中心と を結ぶ線分の内分点に設定されている。その内分点は、該内分点から足首関節の 中心までの距離と股関節の中心までの距離との比率が所定の比率となるような点で あり、各脚体 2を直線状に延ばしたときの膝関節の近傍の点 (例えば、該膝関節の中 心よりも若干、大腿リンク 24寄りの点)である。従って、本実施形態での第 1変位次元 補正用モデルでは、大腿質点 A2, A3が対応する脚体 2の足首関節の中心と股関 節の中心とを結ぶ線分の内分点に拘束されて!、る。

[0200] なお、大腿質点 A2, A3は、上記内分点から上記線分と直角な方向に所定の距離 だけオフセットした点に設定してもよい。言い換えると、上記線分から所定の距離だけ 離れた、該線分に平行な直線上に各大腿質点 A2, A3を設定してもよい。

[0201] また、この第 1変位次元補正用モデルの各質点 A1— A5のグロ一ノレ座標系上で の位置、並びに上体 3 (上体リンク)の姿勢角は、単純ィ匕モデル歩容の運動の瞬時値 に対応して幾何学的に定まるものとされている。より具体的には、本実施形態の第 1 変位次元補正用モデルの上体質点 A1のグローバル座標系での位置は、単純化モ デル歩容の上体位置姿勢に対応する位置に決定され、各足平質点 A4, A5のグロ 一バル座標系での位置は、単純化モデル歩容の各足平位置姿勢に対応する位置 に決定される。さらに、上体リンクの姿勢角は、単純化モデル歩容の上体姿勢と同一 とされる。そして、各大腿質点 A2, A3のグローバル座標系上での位置は、単純化モ デル歩容の上体位置姿勢と各足平位置姿勢を基に定まる前記の内分点の位置に決 定される。すなわち、ロボット 1の上体位置姿勢および各足平位置姿勢に対応して、 各脚体 2の各股関節および各足首関節の中心点のグローバル座標系での位置が一 義的に定まるので、各脚体 2の股関節の中心点および足首関節の中心点を結ぶ線 分の内分点としての各大腿質点 A2, A3のグローバル座標系での位置が定まる。

[0202] また、第 2実施形態の第 1変位次元補正用モデルでは、その質点 A1— A5の全体 重心が単純化モデル上でのロボット 1の全体重心の位置、すなわち、単純化モデル の全ての質点 2m2, 2m2, 3m2の重心の位置に一致するように前記内分点に係る所 定の比率と、質点 A1— A5の質量比とが決定されている。

[0203] ここで、第 2実施形態で上記のように第 1変位次元補正用モデルの要素の配置を定 めるということは、第 1変位次元補正用モデルの要素の配置(グローバル座標系での 質点 A1— A5の位置および上体リンクの姿勢)を定めるための幾何学的拘束条件(3 )を次のように定義したとき、単純化モデル歩容の瞬時運動から、その幾何学的拘束 条件(3)に従って第 1変位次元補正用モデルの要素の配置を定めることと同じである

[0204]

幾何学的拘束条件 (3):与えられた任意の瞬時目標運動に対して、第 1変位次元補 正用モデルの要素のうちの上体質点 A1および上体リンクの配置力与えられた瞬時 目標運動におけるロボット 1の上体 3の位置姿勢に対応して定まる配置に一致し、且 つ、各足平質点 A4, A5の位置が、与えられた瞬時目標運動におけるロボット 1の各 足平位置姿勢に対応して定まる配置に一致し、且つ、各大腿質点 A2, A3の位置が 、与えられた瞬時目標運動における各脚体 2の股関節の中心と足首関節の中心とを 結ぶ線分上の所定の内分点の位置に一致する。

[0205]

第 2実施形態では、この幾何学的拘束条件(3)が本発明における第 1の幾何学的 拘束条件に相当するものである。

[0206] なお、本実施形態では、第 2変位次元補正用モデルの構造は、前記図 8に示した 第 1実施形態のものと同じ構造であり、本実施形態 (第 2実施形態)における第 1変位 次元補正用モデルと同様に、上体質点 Al、大腿質点 A2, A3、および足平質点 A4 , A5を有し、また、上体リンクにイナーシャ lbをもっている。この場合、上体質点 A1, 各足平質点 A4, A5の、対応する部位 (上体 3、各足平 22)に固定されたローカル座 標系での位置は、図 17の第 1変位次元補正用モデルと同じである。また、各質点 A1 一 A5の質量と上体リンクのイナーシャ lbとは、図 17の第 1変位次元補正用モデルの ものと同じである。そして、第 2変位次元補正用モデルにおいては、各質点 A1— A5 および上体 3 (上体リンク)はロボット 1が採り得る任意の姿勢状態に対応する位置姿 勢に移動可能とされている。すなわち、ロボット 1の任意の瞬時目標運動と第 2変位 次元補正用モデルの各要素の配置との間には、前記第 1実施形態で説明した前記 幾何学的拘束条件 (2)が設定されて!ヽる。

[0207] 次に、本実施形態 (第 2実施形態)における歩容生成装置 100の処理を詳細に説 明する。本実施形態における歩容生成装置 100の基本的処理手順は、第 1実施形 態と同じであり、前記図 9のフローチャートに従って歩容が生成される。

[0208] 具体的には、 S010から S018までの処理が第 1実施形態と同様に実行される。これ らの処理は、第 1実施形態と同一である。

[0209] そして、 S018の次に S020の処理が実行され、今回歩容の歩容パラメータが決定 される。すなわち、今回歩容の目標足平位置姿勢軌道、目標腕姿勢軌道、目標 ZM P軌道、および目標床反力鉛直成分軌道を規定するパラメータが決定されると共に、 基準上体姿勢軌道、床反力水平成分許容範囲、および ZMP許容範囲を規定する パラメータが決定される。この場合、本実施形態での単純化モデルは、前記したよう に前記公報文献 1で用いている動力学モデルと同一であるので、同公報文献 1の図 13の S022— S030の処理と同一の処理を本実施形態の S020で実行することで、 今回歩容の歩容パラメータが決定される。

[0210] なお、同公報文献 1の図 13の S022— S028の処理では、定常歩容の作成などの ために、単純ィ匕モデル歩容用の床反力水平成分許容範囲を設定して使用している 力 本実施形態では、その単純ィ匕モデル歩容用の床反力水平成分許容範囲は、例 えば同公報文献 1の図 13の S30で設定するフルモデル補正用の床反力水平成分 許容範囲と同一にする力、もしくは、それよりも広めの範囲に設定すればよい。あるい は、本明細書の第 1実施形態と同様に、単純化モデル歩容用の床反力水平成分許 容範囲を無限大の範囲とするか、もしくは、単純化モデル歩容 (あるいは定常歩容) の床反力水平成分が常にその床反力水平成分許容範囲内に収まるような広い範囲 に定めてもよい。

[0211] 次いで、図 9の S020の処理の後、あるいは S016の判断結果が NOであった場合 には、 S022に進んで、歩容パラメータ(S020で決定した歩容パラメータ)を基に、今 回歩容(単純ィヒモデル歩容)の瞬時値が決定される。この場合、本実施形態での単 純化モデルは、前記したように前記公報文献 1で用いている動力学モデルと同一で あるので、同公報文献 1の図 13の S032の処理と同一の処理を本実施形態の S022 で実行することで、単純ィ匕モデル歩容の瞬時値が決定される。

[0212] より具体的には、前記 S020で決定した歩容パラメータを基に、目標足平位置姿勢 、目標 ZMP、目標腕姿勢、目標床反力鉛直成分、基準上体姿勢の瞬時値が決定さ れ、さらに、前記図 16の単純ィ匕モデル上で、ロボット 1の運動が発生する慣性力と重 力との合力が目標 ZMPまわりに作用するモーメント水平成分力^になり、且つ、その 合力の並進力鉛直成分が目標床反力鉛直成分に釣り合い、且つ、床反力水平成分 が単純ィ匕モデル歩容用の床反力水平成分許容範囲を超えなヽように目標上体位置 姿勢の瞬時値が決定される。ここで、目標上体位置姿勢の瞬時値に関して補足する と、目標上体位置鉛直成分は、目標床反力鉛直成分と前記式 11とから求められる単 純化モデルの上体質点 3m2の鉛直位置に対応して決定される。そして、目標床反力 鉛直成分が比較的大きい時期では、主に上体 3の水平加速度を調整して、目標 ZM Pまわりのモーメント水平成分が 0になるようにしつつ、目標上体姿勢の瞬時値が基 準上体姿勢 (例えば鉛直姿勢)に近づくように目標上体姿勢および目標上体位置水 平成分が決定される。また、目標床反力鉛直成分が比較的小さいか、もしくは 0とな る時期では、上体 3の水平加速度をほぼ 0にしつつ、主に上体 3の姿勢角の角加速 度を調整して、目標 ZMPまわりのモーメント水平成分が 0になるように、目標上体姿 勢および目標上体位置水平成分の瞬時値が決定される。

[0213] なお、 S022の処理で用いる単純ィ匕モデル歩容用の床反力水平成分許容範囲は、 前記 S020の処理で用いるものと同一でよい。

[0214] 次いで、 S024に進んで変位次元補正サブルーチンが実行される。このサブルー チン処理は、基本的処理手順は、前記第 1実施形態と同一であり、前記図 10のフロ 一チャートに従って実行される。すなわち、まず、 S200において、現在時刻 tの単純 化モデル歩容の瞬時値を基に、第 1変位次元補正用モデルの各質点 A1— A5の位 置とイナ一シャをもつ上体リンクの姿勢角とを求める。この場合、前記したように、第 1 変位次元補正用モデルの上体質点 Alの位置は、単純化モデル歩容の上体位置姿 勢の瞬時値に対応する位置に決定され、各足平質点 A4, A5の位置は、単純化モ デル歩容の各足平位置姿勢に対応する位置に決定される。また、各大腿質点 A2,

A3の位置は、単純化モデル歩容の上体位置姿勢と各足平位置姿勢を基に定まる口 ボット 1の各脚体 2の股関節の中心点と足首関節の中心点とを結ぶ線分を、所定の 比率で内分してなる内分点の位置に決定される。また、第 1変位次元補正用モデル の上体リンクの姿勢角は、単純化モデル歩容の上体姿勢角と同一とされる。

[0215] これにより単純化モデル歩容の瞬時目標運動 (今回時刻 tの瞬時値)から、本実施 形態での第 1変位次元補正用モデルに係る前記幾何学的拘束条件(3)に従って、 第 1変位次元補正用モデルの各要素の配置が決定されることとなる。なお、この配置 は、第 1および第 3発明における第 1の配置に相当するものである。

[0216] 次いで、 S202から S218までの処理が実行される。これらの処理は、第 1実施形態 と同じである。すなわち、前記条件 1, 2を満たすような第 2変位次元補正用モデルの 要素の配置 (これは第 1発明における第 3の配置、第 3発明における第 2の配置に相 当する)、ひいては変位次元補正上体位置姿勢が探索的に求められ、それが、今回 時刻 tにおける変位次元補正上体位置姿勢 (Pb2, Θ b2)として決定される。これによ り、単純化モデル歩容の上体位置姿勢を修正してなる変位次元補正歩容が得られる 。補足すると、本実施形態では、各変位次元補正用モデルの両足平質点 A5, A6の 位置は、両変位次元補正用モデルで同じである。従って、 S208でモデル間全体重 心ずれ Gc_errとモデル間角運動量積ずれし errとを算出するに当たっては、両足平 質点 A5, A6に係わる項は省略してもよい。

[0217] 図 9の説明に戻って、前述の如く変位次元歩容補正サブルーチンを実行した後、 S 026— S032の処理が第 1実施形態と同様に実行される。これらの処理は、第 1実施 形態と同一である。

[0218] なお、歩容生成装置 100で以上説明したように生成される目標歩容を入力する複 合コンプライアンス制御装置 101の動作は前記第 1実施形態と同一である。

[0219] ここで、本実施形態での前記変位次元補正歩容について図 18および図 19を参照 して補足説明をしておく。図 18は第 2変位次元補正用モデルの各質点 Ai (i= l, 2, · ··, 5)の位置および上体 3 (上体リンク)の姿勢角を単純化モデル歩容通りに決定し た場合 (すなわち単純ィ匕モデル歩容を前記 S024で補正しな、場合)における第 2変 位次元補正用モデルの各質点 Aiの位置および上体 3 (上体リンク)の姿勢角と、第 1 変位次元補正用モデルの各質点 Aiの位置および上体 3 (上体リンク)の姿勢角との 関係を例示して!/、る。この場合の第 2変位次元補正用モデルの各質点 Aiの位置およ び上体 3の姿勢角、すなわち、第 2変位次元補正用モデルの要素の配置は、別の言 い方をすれば、単純化モデル歩容の瞬時運動から、前記幾何学的拘束条件(2)に 従って決定されたもの(これは第 1発明における第 2の配置に相当する)と言える。な お、図 18では、単純化モデル歩容に対応する第 2変位次元補正用モデルの各質点 Ai(i= l, 2, · ··, 5)の位置を Pi2'(Ai)で表している。

[0220] また、図 19は上記した変位次元歩容補正サブルーチンによって決定される第 2変 位次元補正用モデルの各質点 Aiの位置および上体 3 (上体リンク)の姿勢角と、第 1 変位次元補正用モデルの各質点 Aiの位置および上体 3 (上体リンク)の姿勢角との 関係を例示している。

[0221] 図 18に示す例において、単純ィ匕モデル歩容に対応する第 2変位次元補正用モデ ルの各質点 Aiの位置 Pi2'では、大腿質点 P22', P32'が第 1変位次元補正用モデル に対してロボット 1の前方側に在る。すなわち、第 2変位次元補正用モデルでのロボッ ト 1の全体重心が単純化モデル歩容でのロボット 1の全体重心よりもロボット 1の前方 側 (X軸の正方向)に偏る。別の言い方をすれば、両脚体 2, 2の各大腿質点 A2, A3 のモデル間擬似並進加速度によって各大腿質点 A2, A3が発生する慣性力の並進 力成分(=各質点 A2, A3の質量 *モデル間擬似並進加速度)の総和がロボット 1の 後方側に発生するものとなる。また、各大腿質点 A2, A3のモデル間擬似並進加速 度によって各大腿質点 A2, A3が発生する慣性力が目標 ZMPまわりに作用するモ 一メントが、ロボット 1の後傾側に発生するものとなる。なお、本実施形態では、単純 化モデル歩容と第 1変位次元補正用モデルとで、両足平位置姿勢および上体位置 姿勢は同一であるので、図 18の例では、上体質点 Al、各足平質点 A4, A5のそれ ぞれのモデル間擬似並進加速度は 0であり、また、上体リンクのモデル間擬似角加 速度も 0である。

[0222] これに対して、前記変位次元歩容補正サブルーチンで変位次元補正歩容に対応 する第 2変位次元補正用モデルの各質点 Aiの位置を決定したときには、図 19に見ら れるように、大腿質点 A2, A3の前方側への偏りを補償するようにして、第 2変位次元 補正用モデルの上体質点 A1の位置力第 1変位次元補正用モデルの上体質点 A1 よりも後方側に決定され、換言すれば、上体位置が単純ィ匕モデル歩容よりも後方側 に補正される。同時に、第 2変位次元補正用モデルの各質点 A1— A5の位置並び にイナ一シャをもつ上体 3の姿勢は、前記した角運動量積がある一定値になるように 決定される。図示の例では、第 2変位次元補正用モデルでの上体姿勢 (実線で示す 上体 3の姿勢)は、単純ィ匕モデル歩容の上体姿勢 (破線で示す上体 3の姿勢)に対し て、角度 Θ b2— Θ blだけ前傾している。補足すると、図 19の斜線もしくは横線を付し た三角形の面積の 2倍が、それぞれ、上体質点 Al、大腿質点 A2, A3に係る角運動 量積に相当するものである。

[0223] このため、変位次元補正歩容に対応する第 2変位次元補正用モデルの各質点 Ai の位置および上体リンクの姿勢角では、各質点 Aiのモデル間擬似並進加速度に伴う 慣性力の並進力成分の総和が図 18に示した場合よりも小さくなつて、ほぼ 0になると 共に、該慣性力が目標 ZMPまわりに発生するモーメントの総和も図 18に示した場合 よりも所定の値 (前記条件 2に係る「一定値」に相当する値)に近づく。

[0224] これにより、変位次元補正歩容は、単純ィ匕モデル歩容では考慮されて、な、各脚 体の膝関節付近の部分の運動に伴う慣性力の影響を補償して、単純化モデル歩容 の目標床反力と同じような床反力が発生するようにロボット 1の目標運動を運動を補 正したものとなる。

[0225] なお、本実施形態によるときには、ロボット 1の直立姿勢状態から目標 ZMPを一定 に維持して、両脚体 2, 2の膝関節を曲げつつ、上体 3を下げるような歩容を生成した ときには、前記第 1実施形態と同様に (前記図 14に示したものと同様に)、変位次元 補正上体位置姿勢は、上体 3と両脚体 2, 2との連結部である股関節 (上体 3の下端 部)が単純ィ匕モデル歩容のよりもロボット 1の後方側(つまり膝関節が突き出る向きと ほぼ逆向き)に変位し、且つ、上体 3が単純化モデル歩容の上体姿勢よりも前傾側に 傾くような位置姿勢となる。

[0226]

[第 3実施形態]

次に、本発明の第 3実施形態を説明する。本実施形態は、ロボット 1の機構的構成 は第 1実施形態と同一で、単純化モデル、第 1変位次元補正用モデルおよび第 2変 位次元補正用モデルの構造と、歩容生成装置 100の処理の一部とが第 1および第 2 実施形態と相違するものである。従って、本実施形態の説明では、第 1または第 2実 施形態と同一部分については第 1または第 2実施形態と同じ符号および図面を用い 、詳細な説明を省略する。以下、前記第 1または第 2実施形態と相違する部分を主体 として、本実施形態を説明する。なお、本実施形態は、前記第 1一第 8発明の一実施 形態である。

[0227] 本実施形態では、単純ィ匕モデルは、例えば前記図 8の第 2変位次元補正用モデル と同じ構造のモデルとされている。すなわち、本実施形態の単純ィ匕モデルは、前記 図 8に示した如ぐ上体 3、両脚体 2, 2の大腿リンク、両足平 22, 22にそれぞれ対応 する上体質点 Al、大腿質点 A2, A3、足平質点 A4, A5からなる 5質点を有し、また 、上体 3 (上体リンク)が上体質点 A1のまわりにイナーシャ lbを持つ動力学モデルで ある。この単純ィ匕モデルの動力学は、その式の記載は省略するが、前記した第 2実 施形態の単純ィ匕モデルなどと同様に、各質点 A1— A5の並進運動および上体リンク の回転運動と、床反力(並進床反力および目標 ZMPまわりの床反力モーメント)との 関係式として記述される。例えば、各質点 A1— A5および上体リンクの動力学は相互 に非干渉に構成され、ロボット 1の全体の動力学は、それらの線形結合で表される。

[0228] 図 20は、本実施形態における第 1変位次元補正用モデルの構造を示して、る。な お、図 20では、ロボット 1の姿勢状態が両脚体 2, 2を左右方向 (Y軸方向)に並列さ せて起立している状態であるため、両脚体 2, 2が図面上、重なっている。

[0229] この第 1変位次元補正用モデルは、ロボット 1の上体 3に対応する 1つの上体質点 B 1、各脚体 2の膝関節近傍の大腿リンク部分に対応する大腿質点 B2, B3、各脚体 2 の先端部(足平 22)にそれぞれ対応する足平質点 B4, B5、並びに、ロボット 1の各 腕体 5にそれぞれ対応する腕質点 B6, B7からなる 7質点モデルである。また、第 1変 位次元補正用モデルにおける上体 3 (上体リンク)は、上体質点 A1のまわりにイナ一 シャ (慣性モーメント) Itを持つと共に、各腕体 5は、各腕質点 B6, B7のまわりにイナ ーシャ laを持つものとされている。つまり、本実施形態の第 1変位次元補正用モデル は、質点 B1— B7と、イナーシャ Itをもつ上体 3と、イナーシャ laをそれぞれつ腕体 5, 5とを要素として構成されてヽる。

[0230] この場合、第 1変位次元補正用モデルでの上体質点 B1と、大腿質点 B2, B3と、足 平質点 B4, B5とは、前記第 1実施形態の第 2変位次元補正用モデルの各質点 A1 一 A5と同様に、それぞれに対応する部位 (上体 3、各大腿リンク、各足平 22)に任意 に固定されたローカル座標系上でのある固定点に設定されている。また、腕質点 B6 , B7は、各腕体 5の 1つのリンク、例えば腕体 5の肘関節と手首関節との間のリンクに 任意に固定されたローカル座標系でのある固定点 (腕体 5の先端部寄りの点)に設定 されている。

[0231] なお、第 1変位次元補正用モデルの各質点 B1— B7の質量の総和は、ロボット 1の 総質量と同一であると共に、上体質点 B1および腕質点 B6, B7の質量の総和は、本 実施形態における単純ィ匕モデル(図 8)の上体質点 A1と同一の質量とされている。

[0232] そして、本実施形態においても第 1変位次元補正用モデルの要素の配置には、あ る幾何学的拘束条件が設定されている。具体的には、本実施形態の第 1変位次元補 正用モデルでは、両腕体 5, 5は、いずれも上体 3に対して図 20に示すような姿勢状 態に固定された状態に拘束されている。すなわち、第 1変位次元補正用モデルでの 両腕体 5, 5の上体 3に対する相対姿勢、ひいては、上体質点 B1と腕質点 B6, B7と の間の相対的位置関係は一定である。

[0233] 補足すると、第 1変位次元補正用モデルの上体質点 B1および腕体質点 B6, B7の 重心点は、本実施形態における単純ィ匕モデル(図 8)の上体質点 A1と一致するもの とされている。また、上体リンクのイナーシャ Itと、各腕体 5のイナーシャ laとは、本実 施形態の単純ィ匕モデルの上体リンクのイナーシャ lbと次の関係式 14を満たすように 設定されている。

[0234]

Ib=It+Ib

+上体質点 B 1の質量 * (上体質点 B 1と上体質点 A1との距離)2

+腕体質点 B6の質量 * (腕体質点 B6と上体質点 Alとの距離)2

+腕体質点 B7の質量 * (腕体質点 B7と上体質点 A1との距離)2

…式 14

また、この第 1変位次元補正用モデルの各質点 B1— B7のグロ一ノレ座標系上で の位置、並びに上体 3の姿勢は、単純ィ匕モデル歩容の運動の瞬時値に対応して幾 何学的に定まるものとされている。より具体的には、本実施形態の第 1変位次元補正 用モデルの上体質点 B1のグローバル座標系での位置は、単純化モデル歩容の上 体位置姿勢に対応する位置に決定され、各足平質点 B4, B5のグローバル座標系で の位置は、単純ィヒモデル歩容の各足平位置姿勢に対応する位置に決定される。ま た、各大腿質点 B2, B3の位置は、単純化モデル歩容の上体位置姿勢および両足 平位置姿勢力定まる各脚体 2の大腿リンクの位置姿勢に対応する位置に決定され る。なお、第 1変位次元補正用モデルでは、上体 3に対する腕体 5, 5の相対姿勢は 前記した如く一定であるので、腕質点 5, 5のグロ一ノレ座標系での位置は、単純ィ匕 モデル歩容の上体位置姿勢に対応して一義的に定まることとなる。

[0235] ここで、第 3実施形態で上記のように第 1変位次元補正用モデルの要素の配置を定 めるということは、第 1変位次元補正用モデルの要素の配置(グローバル座標系での 質点 B1— B7の位置、並びに上体リンクおよび各腕体の姿勢)を定めるための幾何 学的拘束条件 (4)を次のように定義したとき、単純ィ匕モデル歩容の瞬時運動から、そ の幾何学的拘束条件 (4)に従って第 1変位次元補正用モデルの要素の配置を定め ることと同じである。

[0236]

幾何学的拘束条件 (4):与えられた任意の瞬時目標運動に対して、第 1変位次元補 正用モデルの要素のうちの上体質点 B1および上体リンクの配置力与えられた瞬時 目標運動におけるロボット 1の上体 3の位置姿勢に対応して定まる配置に一致し、且 つ、各足平質点 B4, B5の位置力与えられた瞬時目標運動におけるロボット 1の各 足平位置姿勢に対応して定まる位置に一致し、且つ、各大腿質点 B2, B3の位置が 、与えられた瞬時目標運動における各脚体 2の大腿リンクの位置姿勢に対応して定 まる位置に一致し、且つ、各腕質点 B6, B7および各腕体 5 (イナ一シャを持つリンク) の配置が、与えられた瞬時目標運動におけるロボット 1の上体 3の位置姿勢に対して 所定の相対関係を持つ配置になる。

[0237]

第 3実施形態では、この幾何学的拘束条件 (4)が本発明における第 1の幾何学的 拘束条件に相当するものである。

[0238] 図 21は本実施形態における第 2変位次元補正用モデルの構造を示している。この 第 2変位次元補正用モデルは、基本的構造は、図 20の第 1変位次元補正用モデル と同じであり、第 1変位次元補正用モデルと同様に 7個の質点 B1— B7を有すると共 に、上体 3 (上体リンク)が質点 B1のまわりにイナーシャ Itを持ち、また、各腕体 5が腕 質点 B6, B7のまわりにイナーシャ laを持つモデルである。各質点 B1— B7の質量と 、各質点 B1— B7の、対応する部位に固定設定されたローカル座標系での位置とは 、図 20の第 1変位次元補正用モデルと同一である。また、各イナーシャ It, laも図 20 の第 1変位次元補正用モデルと同一である。そして、この第 2変位次元補正用モデ ルにおいては、各質点 B1— B7、並びに上体 3 (上体リンク)および各腕体 5はロボット 1が採り得る任意の姿勢状態に対応する位置姿勢に移動可能とされている。すなわ ち、ロボット 1の任意の瞬時目標運動と第 2変位次元補正用モデルの各要素の配置と の間には、前記第 1実施形態で説明した前記幾何学的拘束条件 (2)が設定されてい る。

[0239] この第 2変位次元補正用モデルの各質点 B1— B7のグローバル座標系での位置に 関しては、各足平質点 B4, B5の位置は、単純化モデル歩容の各足平位置姿勢に 対応する位置に決定される。また、上体質点 Bl、各大腿質点 B2, B3の位置、およ び各腕質点 5の位置並びに上体 3 (上体リンク)および腕体 5の姿勢は、前記条件 1, 2を満たし、且つ、上体 3に対する腕体 5の相対姿勢が単純化モデル歩容の相対姿 勢に一致するように決定される。

[0240] 次に、本実施形態 (第 3実施形態)における歩容生成装置 100の処理を詳細に説 明する。本実施形態における歩容生成装置 100の基本的処理手順は、第 1実施形 態と同じであり、前記図 9のフローチャートに従って歩容が生成される。但し、本実施 形態では、 S026の処理は省略される。

[0241] 具体的には、 S010から S018までの処理が第 1実施形態と同様に実行される。これ らの処理は、第 1実施形態と同一である。

[0242] そして、 S018の次に S020の処理が実行され、今回歩容の歩容パラメータが決定 される。すなわち、今回歩容の目標足平位置姿勢軌道、目標腕姿勢軌道、目標 ZM P軌道、および目標床反力鉛直成分軌道を規定するパラメータが決定されると共に、 基準上体姿勢軌道、床反力水平成分許容範囲、および ZMP許容範囲を規定する ノ メータが決定される。この場合、 S020の処理は、例えば前記第 2実施形態と同 様に行えばよい。但し、本実施形態では、 S020の処理を行うための動力学モデル( 単純化モデル)として、前記図 16の動力学モデルに代えて、図 8の単純化モデルが 用いられる。

[0243] 次いで、図 9の S020の処理の後、あるいは S016の判断結果が NOであった場合 には、 S022に進んで、歩容パラメータ(S020で決定した歩容パラメータ)を基に、今 回歩容(単純ィヒモデル歩容)の瞬時値が決定される。この処理は、例えば前記第 2実 施形態と同様に行われる。但し、本実施形態では、今回歩容の瞬時値 (詳しくは上体 位置姿勢の瞬時値)を決定するために、図 8の単純ィ匕モデルが用いられる。補足する と、本実施形態における図 8の単純ィ匕モデルでは、上体 3に対する両腕体 5, 5の相 対姿勢は前記図 20に示した姿勢状態に固定されているものとしている。従って、 SO 22で求められる今回歩容の瞬時値のうち、上体位置姿勢の瞬時値は、上体 3に対す る両腕体 5, 5の相対姿勢を図 20の姿勢状態に固定していると見なして、図 8の単純 化モデル上で目標 ZMPまわりの床反力モーメントの水平成分力^になるように決定さ れたものとなる。

[0244] 次いで、 S024に進んで変位次元補正サブルーチンが実行される。このサブルー チン処理は、基本的処理手順は、前記第 1実施形態と同様であるが、一部の処理が 第 1実施形態と相違している。その処理は図 23のフローチャートに従って実行される 。すなわち、まず、 S400において、現在時刻 tの単純化モデル歩容の瞬時値を基に 、第 1変位次元補正用モデルの各質点 B1— B7の位置と、イナ一シャをもつ上体リン クおよび各腕体 5, 5の姿勢を求める。この場合、前記したように、本実施形態の第 1 変位次元補正用モデルの上体質点 Blおよび両腕質点 B6, B7の位置は、単純化モ デル歩容の上体位置姿勢に対応する位置に決定され、各足平質点 B4, B5の位置 は、単純ィ匕モデル歩容の各足平位置姿勢に対応する位置に決定される。また、各大 腿質点 B2, B3の位置は、単純化モデル歩容の上体位置姿勢および両足平位置姿 勢力 定まる各脚体 2の大腿リンクの位置姿勢に対応する位置に決定される。また、 第 1変位次元補正用モデルの上体リンクの姿勢は、単純化モデル歩容の上体姿勢と 同一とされる。なお、第 1変位次元補正用モデルの各腕体 5, 5の姿勢は、前記した 如ぐ上体 3に対して図 20の姿勢状態に固定されているので、第 1変位次元補正用 モデルの上体 3 (上体リンク)の姿勢が定まれば、一義的に各腕体 5, 5の姿勢も定ま る。従って、第 1変位次元補正用モデルでの各腕体 5, 5の姿勢は、一般には単純ィ匕 モデル歩容の腕姿勢の瞬時値とは異なる。

[0245] 次いで、 S402〖こ進んで、前記図 10の S202と全く同様〖こ、変位次元補正上体位 置姿勢の初期候補が決定される。

[0246] 次ヽで、 S404を経て、 S406— S416のノレープ処理力図 10の S206— S216と同 様に実行される。この場合、 S406、 S408、 S414の処理のー咅 ^第 1実施形態と相 違する。具体的には、 S406では、変位次元補正上体位置姿勢の現在の候補 (Pb2—s , Θ b2_s)と、今回時刻 tでの単純化モデル歩容の目標両足平位置姿勢および目標 腕姿勢とを基に、第 2変位次元補正モデルでの各質点 B1— B7の位置を求める。こ の場合、第 2変位次元補正用モデルでのロボット 1の上体 3の位置姿勢が現在の候 補 (Pb2— s, Θ b2_s)に一致し、且つ、第 2変位次元補正用モデルでのロボット 1の各足 平 22の位置姿勢が単純化モデル歩容の目標足平位置姿勢に一致し、且つ、第 2変 位次元補正用モデルでのロボット 1の両腕体 5, 5の上体 3に対する相対姿勢が単純 化モデル歩容の腕姿勢に一致しているとして、各質点 B1— B7の位置と各腕体 5, 5 の姿勢とが求められる。

[0247] 具体的には、足平質点 B3, B4の位置は、目標足平位置姿勢から決定される。また 、上体質点 B1の位置は、候補 (Pb2_s, Θ b2_s)から決定され、上体 3 (上体リンク)の 姿勢角は Θ b2_sと同一とされる。そして、各大腿質点 B2, B3の位置は、目標両足平 位置姿勢と候補 (Pb2_s, Θ b2_s)とから定まるロボット 1の各脚体 2の姿勢力決定さ れる。さらに、第 2変位次元補正用モデルの両腕体 5, 5の姿勢 (上体 3に対する相対 姿勢)は、目標腕姿勢と同一とされる。さらに、両腕質点 B6, B7の位置は、上体 3に 対する目標腕姿勢に対応する位置に決定される。補足すると、上体位置姿勢と、上 体 3に対する両腕体 5, 5の相対姿勢が決まれば、両腕質点 B6, B7の位置(グロ一 バル座標系のでの位置)は一義的に定まる。

[0248] また、 S408の処理では、モデル間全体重心ずれ G errは、第 1実施形態と同様に 、前記式 06で求められる。但し、この場合の式 06の右辺の∑の演算は本実施形態 での変位次元補正用モデルの全ての質点 B1— B7についての総和である。また、モ デル間角運動量積ずれ L_errは、例えば前記式 07の右辺の「Ib * ( Θ b2— Θ bl)」を「I t * ( 0 b2— 0 bl) + Ia * ( Θ a2R- Θ alR) +Ia * ( Θ a2L— Θ alL) jで置き換えた式によ つて算出される。ここで、 Θ a2Rは、第 2変位次元補正用モデルの右側腕体 5Rの上 体 3に対する姿勢を代表する姿勢角(例えば腕体 5の肩関節と肘関節とを連結するリ ンクの上体 3に対する相対傾斜角)である。同様に、 0 alRは、第 1変位次元補正用 モデルの右側腕体 5Rの上体 3に対する姿勢角、 Θ a2Lは、第 2変位次元補正用モデ ルの左側腕体 5Lの上体 3に対する姿勢角、 Θ alLは、第 1変位次元補正用モデルの 左側腕体 5Lの上体 3に対する姿勢角である。なお、本実施形態では、両腕体 5, 5の 目標腕姿勢は、肘関節の曲げ角を一定としている。補足すると、本実施形態では、式 07の右辺の∑の演算は、モデル間全体重心ずれ Gc_errの場合と同様、全ての質点 B1— B7についての総和である。

[0249] また、 S414で、モデル間全体重心ずれおよびモデル間角運動量積ずれを求める 場合ち、 S408と同様に行われる。

[0250] S406— S416のループ処理〖こ続いて、 S418の処理が実行され、今回時刻 tの変 位次元補正上体位置姿勢 (Pb2, Θ b2)が決定される。この処理は、図 10の S218と全 く同様である。

[0251] 上述した変位次元歩容補正サブルーチンによって、前記条件 1, 2を満たすような 変位次元補正上体位置姿勢が探索的に求められ、それが、今回時刻 tにおける変位 次元補正上体位置姿勢 (Pb2, Θ b2)として決定される。これにより、単純ィ匕モデル歩 容の上体位置姿勢を修正してなる変位次元補正歩容が得られる。補足すると、本実

施形態では、各変位次元補正用モデルの両足平質点 B5, B6の位置は、両変位次 元補正用モデルで同じである。従って、 S208でモデル間全体重心ずれ Gc_errとモ デル間角運動量積ずれし errとを算出するに当たっては、両足平質点 B5, B6に係わ る項は省略してちょい。

[0252] 図 9の説明に戻って、前述の如く変位次元歩容補正サブルーチンを実行した後、 S 026の処理を省略して、 S028— S032の処理が第 1実施形態と同様に実行される。 S028— S032の処理は、第 1実施形態と同一である。

[0253] なお、歩容生成装置 100で以上説明したように生成される目標歩容を入力する複 合コンプライアンス制御装置 101の動作は前記第 1実施形態と同一である。

[0254] ここで、本実施形態での前記変位次元補正歩容について図 22を参照して補足説 明をしておく。図 22は、例えば図 20のようにロボット 1を直立姿勢で起立させた状態 から、目標 ZMPを維持したまま、両腕体 5, 5の先端部を上体 3の前方側に突き出す ような歩容を生成する場合にぉ、て、上記した変位次元歩容補正サブルーチンによ つて決定される第 2変位次元補正用モデルの各質点 Bi (i= l, 2, · ··, 7)の位置、上 体 3 (上体リンク)の姿勢角、および両腕体 5, 5の姿勢と、第 1変位次元補正用モデ ルの各質点 Biの位置、上体 3 (上体リンク)の姿勢角、および両腕体 5, 5の姿勢との 関係を例示している。なお、この場合、先に示した図 21のロボット 1の姿勢状態は、第 2変位次元補正用モデルの各質点 Biと、上体 2の姿勢角、および両腕体 5, 5の姿勢 (上体 3に対する相対姿勢)を単純化モデル歩容どおりに決定した場合に相当するも のとなつている。換言すれば、図 21における第 2変位次元補正用モデルの各質点 Bi の位置、並びに上体 3の姿勢角および各腕体 3の姿勢、すなわち、第 2変位次元補 正用モデルの要素の配置は、単純化モデル歩容の瞬時運動から、前記幾何学的拘 束条件(2)に従って決定されたものとなっている。従って、この場合の第 2変位次元 補正用モデルの要素の配置は、第 1発明における第 2の配置に相当する。

[0255] ロボット 1の図 20の姿勢状態から、両腕体 5, 5を図 22のように前方側に突き出す歩 容を生成する場合、単純ィ匕モデル歩容では、図 21に示すように、上体位置姿勢を維 持したまま、両腕体 5, 5を前方に突き出すような歩容となる。このため、単純化モデ ル歩容通りに第 2変位次元補正用モデルの各質点 Biの位置、上体 3の姿勢、および

両腕体 5, 5の姿勢を決定すると、第 2変位次元補正用モデルの両腕質点 B6, B7が 第 1変位次元補正用モデル(図 20参照)に対して前方側に突き出ることとなる。この ため、図 22に示す如ぐ両変位次元補正用モデルで全体重心を一致させるベく(前 記条件 1を満たすべく)、第 2変位次元補正用モデルの上体質点 B1の位置 P12は、 第 1変位次元補正用モデルの上体質点 B 1の位置 P 11よりも後方側の位置になる。別 の言い方をすれば、両腕質点 B6, B7のモデル間擬似並進加速度に伴う慣性力の 並進力成分を、上体質点 A1のモデル間擬似並進加速度に伴う慣性力の並進力成 分によって打ち消すようにして、第 2変位次元補正用モデルの上体質点 B1の位置 P 12は、第 1変位次元補正用モデルの上体質点 B1の位置 P1はりも後方側の位置に なる。なお、このとき、第 1変位次元補正用モデルの上体質点 B1の位置 P1はりも後 方側の位置になることに付随して、第 2変位次元補正用モデルの大腿質点 B2, B3 の位置 P22, P32も、第 1変位次元補正用モデルの大腿質点 B2, B3の位置 P21, P 31に対して若干ずれる。

[0256] また、このとき、仮に上体 3の姿勢を維持すると、第 1変位次元補正用モデルと第 2 変位次元補正用モデルとの間で前記モデル間角運動量積ずれ L_errが発生する。そ して、このずれレ errを解消すべく(前記条件 2を満たすべく)、第 2変位次元補正モ デルでの上体姿勢 (図 22に上体 3の姿勢)、すなわち変位次元補正歩容の上体姿 勢は、第 1変位次元補正モデルでの上体姿勢 (図 20に示す上体 3の姿勢)、すなわ ち、単純ィ匕モデル歩容の上体姿勢に対して前傾側に傾く。より詳しく言えば、上体質 点 Bl、大腿質点 B2, B3および腕質点 B6, B7のモデル間擬似並進加速度に伴う慣 性力、並びに、腕体 5, 5のモデル間擬似角加速度に伴う慣性力が目標 ZMPまわり に作用するモーメントを打ち消すようにして、変位次元補正歩容の上体姿勢は、単純 化モデル歩容の上体姿勢に対して前傾側に傾く。

[0257] このように単純化モデル歩容に対して運動(上体位置姿勢)を修正した変位次元補 正歩容が決定されることで、目標 ZMPまわりの床反力モーメントの水平成分力^にな ること等の動力学的平衡条件を単純ィ匕モデル歩容と同じように満たしつつ、単純ィ匕 モデル歩容よりも動力学的精度の高い変位次元補正歩容が決定されることとなる。

[0258] 補足すると、以上説明した第 1一第 3実施形態では、前記したように第 1の幾何学的 拘束条件と第 2の幾何学的拘束条件とを設定しているため、単純化モデル歩容の瞬 時運動から、第 1の幾何学的拘束条件に従って決定される第 1変位次元補正用モデ ルの各要素の配置の時間的変化 (歩容生成装置 100の演算処理周期毎の変化)に よって発生する各要素の慣性力の合力に釣り合う床反力(目標 ZMPまわりの床反力 モーメント)と、該単純化モデル歩容の瞬時運動から、前記第 2の幾何学的拘束条件 に従って第 2変位次元補正用モデルの各要素の配置を決定した場合における該第 2 変位次元補正用モデルの各要素の配置の時間的変化 (歩容生成装置 100の演算 処理周期毎の変化)によって発生する、その各要素の慣性力の合力に釣り合う床反 力(目標 ZMPまわりの床反力モーメント)との差に所定の定常オフセット(前記式 07 一 10の「Const」に相当するもの)を加えたもの力単純化モデル歩容の瞬時運動が 単純化モデル上で発生する床反力(目標 ZMPまわりの床反力モーメント)の誤差に ほぼ一致するものとなっている。従って、第 1一第 3実施形態は、第 8発明の実施形 態となつている。

[0259]

次に、以上説明した第 1一第 3実施形態に関連した変形態様をいくつ力説明する。

[0260] 前記第 1一第 3実施形態では、角運動量積に係る点 Qとして、目標 ZMPを用いた 力 点 Qは目標 ZMP以外の点でもよぐ例えば、次のような点でもよい。

a)歩容を記述する座標系(グローバル座標系)の原点

b)ロボット 1とともに連続的に移動する適当に設定した点

c)フルモデルにおけるロボット 1の全体重心

d)単純化モデルにおけるロボット 1の全体重心

e)第 1および第 2変位次元補正用モデルに係るある所定の質点の集合の重心 (具体 的には、第 1および第 2変位次元補正用モデルの間で位置の差を生じる可能性があ る質点の集合の重心。例えば、第 1実施形態では、全ての質点 A1— A5の集合の重 心が相当し、第 2実施形態では、上体質点 A1,大腿質点 A2, A3の集合の重心が 相当する)

また、前記第 1一第 3実施形態の説明では、図 9の S024の変位次元歩容補正サブ ルーチンの処理にお、て、サジタルプレーン上での単純化モデル歩容の上体位置 姿勢を補正する処理にっ、て説明した力サジタルプレーンに直交するラテラルプレ ーン上での上体位置姿勢を合わせて補正するようにしてもよい。この場合には、例え ば図 10の S200— S218の処理を 3次元に拡張して行えばよい。あるいは、サジタル プレーン上での上体位置姿勢の補正処理と、ラテラルプレーン上での上体位置姿勢 の補正処理とを図 10と同様のアルゴリズムによって、それぞれ独立に行うようにしても よい。補足すると、上体位置の鉛直成分を含めて上体位置姿勢を補正する場合にお いて、サジタルプレーン上での上体位置姿勢の補正処理と、ラテラルプレーン上での 上体位置の補正処理とを独立して行う場合には、上体位置の鉛直成分の補正は、サ ジタルプレーン上、あるいはラテラルプレーン上のいずれか一方のプレーン上での補 正処理によって行い、他方のプレーン上での補正処理では、上体位置の鉛直成分 を除、て、その他方のプレーン上での上体位置姿勢の補正を行うようにすればよ!、。

[0261] また、ホリゾンタルプレーン (水平面)での上体位置姿勢を合わせて補正するように してもよい。あるいは、サジタルプレーン、ラテラルプレーン、ホリゾンタルプレーンの V、ずれか一つまたは二つに対して上体位置姿勢を補正するようにしてもょ、。

[0262] また、第 1一第 3実施形態の図 9の S024の変位次元歩容補正サブルーチンの処 理では、変位次元補正上体位置姿勢の初期候補 (Pb2_s, Θ b2_s)を、前回制御周期 の時刻で求めた変位次元補正上体位置姿勢を用いて決定したが、例えば該初期候 補(Pb2_s, Θ b2_s)を単純化モデル歩容の上体位置姿勢と同一にしてもょ、。但し、 前記条件 1, 2を満たす変位次元補正上体位置姿勢を短時間で探索する上では、前 記第 1一第 3実施形態で説明した如ぐ初期候補 (Pb2_s, Θ b2_s)を決定することが 望ましい。

[0263] また、第 1一第 3実施形態の変位次元歩容補正サブルーチンの処理では、前記条 件 1, 2を満たす変位次元補正上体位置姿勢を探索的に求めるようにしたが、例えば 第 2変位次元補正用モデルの要素の配置 (各質点の位置とイナ一シャを持つ各リン クの姿勢)を前記幾何学的拘束条件 (2)に従って (単純化モデル歩容に対応させて) 決定したときのその配置と、第 1変位次元補正用モデルの要素の配置との差(両モデ ル間の各質点の位置の差と、イナ一シャをもつ各リンクの姿勢角の差)から、あらかじ め作成した関数式あるいはマップなどを用いて、単純化モデル歩容の上体位置姿勢 から変位次元補正上体位置姿勢への補正量を決定し、その補正量で単純化モデル 歩容の上体位置姿勢を補正することで、変位次元補正上体位置姿勢を決定するよう にしてもよい。

[0264] また、第 1一第 3実施形態の変位次元歩容補正サブルーチンの処理で、モデル間 全体重心ずれ Gc_errとモデル間角運動量積ずれ Lc_errとがそれぞれ許容範囲内で あるか否かの判定(図 10の S210または図 23の S410の処理)を行わず、探索回数( 候補 (Pb2_s, Θ b2_s)の更新回数)が所定回数に達したら探索完了とし、そのときの候 補 (Pb2_s, 0 b2_s)を変位次元補正上体位置姿勢として決定するようにしてもよい。あ るいは、モデル間全体重心ずれ Gc_errとモデル間角運動量積ずれ Lc_errとがそれぞ れ許容範囲内に収まるか、もしくは、探索回数が所定回数に達したら探索完了として 、そのときの候補 (Pb2_s, 0 b2_s)を変位次元補正上体位置姿勢として決定するように してちよい。

[0265] また、前記第 2実施形態に関して、第 1および第 2変位次元補正用モデルの間での 各大腿質点 A2, A3のそれぞれの位置ずれは、各脚体 2の足首関節の中心点と股 関節の中心点とを結ぶ線分に対する第 2変位次元補正モデルの大腿質点 A2, A3 の位置ずれ (該線分にほぼ直交する面内での位置のずれ)、あるいは、該線分に対 する膝関節の中心点の位置ずれにほぼ等しい。従って、モデル間全体重心ずれ Gc_errとモデル間角運動量積ずれ L_errとを求めるときに、例えば前記式 06, 07にお ける各大腿質点 A2, A3に係る位置ずれ (P22— P21), (P32— P31)の代わりに、上記 線分と、大腿質点 A2, A3もしくは各膝関節の中心との距離 (以下、大腿質点 A2, A 3の擬似位置ずれ距離とヽぅ)を用いてもょヽ。

[0266] さらには、この大腿質点 A2, A3の擬似位置ずれ距離は、各脚体 2の膝関節の曲 げ角と密接な関係があり、この大腿質点 A2, A3の擬似位置ずれを膝関節の曲げ角 力も求めるようにしてもよい。より具体的には、前記した図 18に示すように、各大腿リ ンク 24の長さ(大腿リンク 24の両端の股関節および膝関節のそれぞれの中心点の間 の距離)を L、膝関節の曲げ角(大腿リンクの軸心 (股関節の中心と膝関節の中心とを 通る直線)に対する下腿リンクの軸心 (膝関節の中心と足首関節の中心とを通る直線 )の傾斜角)を Θとしたとき、各大腿質点 A2, A3の擬似位置ずれ距離は、ほぼ、 L * sin( 0 Z2)に等しいものとなる。なお、長さ Lは、両大腿リンク 24, 24について同一で ある。従って、例えば前記式 06, 07における各大腿質点 A2, A3に係る位置ずれ (P 22-P21), (P32— P31)の代わりに、各脚体 2の膝関節の曲げ角 Θに応じて定まる L * sin ( θ Z2)を用いるようにしてもよい。補足すると、上体位置姿勢と両足平位置姿勢 を定めれば、各脚体 2の膝関節の曲げ角はロボット 1の幾何学モデル (リンクモデル) によって一義的に決定できる。

[0267] また、前記第 2実施形態において、第 1および第 2変位次元補正用モデルの両足 平質点 A4, A5の位置は、いずれも単純ィ匕モデル歩容の両足平位置姿勢に対応す る位置に決定される。このため、変位次元補正上体位置姿勢は、結果的に、第 1およ び第 2変位次元補正用モデルの間での、各大腿質点 A2, A3の位置のずれに応じ て定まることとなる。従って、前記したように第 2変位次元補正用モデルの要素の配置 を前記幾何学的拘束条件(2)に従って (単純化モデル歩容に対応させて)決定した ときのその配置と、第 1変位次元補正用モデルの要素の配置とのずれから、あらかじ め作成した関数式あるいはマップなどを用いて、単純化モデル歩容の上体位置姿勢 から変位次元補正上体位置姿勢への補正量を決定するようにしたときには、単純ィ匕 モデル歩容に対応する第 2変位次元補正用モデルの各大腿質点 A2, A3の位置と 、第 1変位次元補正用モデルの各大腿質点 A2, A3の位置との間の位置ずれに応じ てフィードフォワード制御則によって直接的に (微積分演算を用いることなく)、単純 化モデル歩容に対する変位次元補正上体位置姿勢の補正量を決定できることとなる 。そして、この場合に、各大腿質点 A2, A3の位置のずれの代わりに、前記した擬似 位置ずれ距離を用いたり、あるいは、その擬似位置ずれに対応する各脚体 2の膝関 節の曲げ角 Θを用いることによって、その擬似位置ずれ距離または膝関節の曲げ角 Θから、フィードフォワード制御則によって直接的に変位次元補正上体位置姿勢 (単 純化モデル歩容の上体位置姿勢から変位次元補正上体位置姿勢の補正量)を決定 でさることとなる。

[0268]

[第 4実施形態]

この場合の一実施形態を第 4実施形態として、図 24を参照して説明する。この第 4 実施形態は、前記図 9の S024の変位次元歩容補正サブルーチンのみが前記第 2実 施形態と相違するものであり、該サブルーチン処理を示すフローチャートが図 24であ る。なお、この実施形態は、第 17および第 18発明の一実施形態である。

[0269] 以下説明すると、 S600において、現在時刻 tの単純ィ匕モデル歩容の瞬時値(図 9 の S022で求められたもの)を基に、各脚体 2の膝関節の曲げ角 Θが求められる。具 体的には、単純化モデル歩容の上体位置姿勢と両足平位置姿勢とから各脚体 2の 姿勢状態が一義的に定まるので、それによつて、各脚体 2の膝関節の曲げ角 Θが求 められる。この曲げ角 Θは、単純ィ匕モデル歩容そのものに対応する膝関節の曲げ角 である。

[0270] 次いで、 S602に進んで、各脚体 2の膝関節の曲げ角 Θから、例えばあらかじめ定 められた所定の関数により、上体位置姿勢の補正量を決定する。この場合、各膝関 節の曲げ角 Θに対して、上体位置の補正量と上体姿勢の補正量とが単調に増加又 は減少するように前記所定の関数が設定される。より具体的には、膝関節の曲げ角 Θが大きくなるに伴い、上体位置の補正量が、ロボット 1の後方側に大きくなり、また、 上体姿勢の補正量が上体 3の前傾側に大きくなるように前記所定の関数が設定され る。

[0271] 次いで、 S604に進んで、単純化モデル歩容の上体位置姿勢の瞬時値を S602で 上記の如く求めた補正量で補正することにより、現在時刻 tでの変位次元補正上体 位置姿勢を決定する。

[0272] 以上が、第 4実施形態における変位次元歩容補正サブルーチンの処理である。こ の第 4実施形態によるときには、第 1および第 2変位次元補正用モデルを使用せずに 、単純化モデル歩容の上体位置姿勢が、各脚体 2, 2の膝関節の曲げ角に応じてフ イードフォワード制御則によって決定されることとなる。そして、上体位置姿勢の補正 量を決定する関数を上記のように設定しておくことで、結果的に、第 2実施形態と同 様に第 1および第 2変位次元補正用モデルを用いて前記条件 1, 2を満たすように変 位次元補正上体位置姿勢を決定した場合と同じように、単純ィ匕モデル歩容の上体位 置姿勢を補正できることとなる。

[0273] なお、第 4実施形態では、上体位置姿勢の補正量を決定するために関数を用いた 力 マップを用いてもよいことはもちろんである。また、膝関節の曲げ角の代わりに、 各脚体 2の前記擬似位置ずれ距離を用いてもよい。この場合、各脚体の擬似位置ず れ距離力上体位置姿勢の補正量を決定するための関数もしくはマップは、擬似位 置ずれ距離が大きくなるに伴い、上体位置の補正量が、ロボット 1の後方側に大きく なり、また、上体姿勢の補正量が上体 3の前傾側に大きくなるように設定しておけばよ い。このようにすることで、前記第 19および第 20発明の実施形態が構成されることと なる。

[0274]

また、前記第 1一第 3実施形態に関し、次のような変形態様も可能である。前記第 1 一第 3実施形態では、第 1および第 2変位次元補正用モデルにおける各脚体 2の質 点を 2つとしたが、例えば各脚体 2の足平 2の近傍、下腿リンク、および大腿リンクにそ れぞれ質点をもつような (各脚体 2に 3質点をもつような)変位次元補正用モデルを構 築してもよい。この場合、第 2実施形態のように、第 1変位次元補正用モデルの質点 の位置を拘束するときには、各足平質点以外の 2つの質点を、例えば足首関節の中 心と股関節の中心とを結ぶ線分上の所定の内分比で定まる 2つの点に設定するよう にすればよい。また、下腿リンクおよび大腿リンクのうちの少なくともいずれか一方に 相当するイナ一シャをもつ剛体 (リンク)を両変位次元補正用モデルの要素としてカロ えてもよい。

[0275] また、例えばロボット 1を比較的摩擦係数の高い床上で歩行させるような場合には、 第 1および第 2実施形態で、上体 3のイナ一シャを 0として変位次元補正上体位置姿 勢を求めたり、あるいは、前記条件 2のみを満たすように変位次元補正上体位置姿勢 を求めるようにしてもよい。

[0276] また、足平の近傍の質点(足平質点)のように、両変位次元補正用モデルでの配置

(本発明における第 1および第 2の配置、あるいは第 1一第 3の配置)が同一位置とな るような質点は、それを両変位次元補正用モデルから除、てもよ、。

[0277] また、両腕体 5, 5の肘関節の屈伸を行うような場合には、前記第 2実施形態での第 1および第 2変位次元補正用モデルで大腿質点を備えた場合と同様に、肘関節もし くはその近傍に対応する質点を備えるようにしてもよい。より具体的には、例えば図 2 5に示すように、第 1および第 2変位次元補正用モデルで、各腕体 5の肘関節近傍に それぞれ対応する肘質点 B8, B9と、各腕体 5の先端部近傍にそれぞれ対応する手 先質点 B6, B7を備えるようにして、第 1変位次元補正用モデルでは、各腕体 5の肩 関節の中心と手首関節の中心とを結ぶ線分上の、所定の内分比で定まる点に肘質 点 B8, B9が拘束されるようにする。そして、第 1変位次元補正用モデルと第 2変位次 元補正用モデルとの間での肘関節 B8, B9の位置の差を含めて、前記第 2実施形態 と同様に、モデル間全体重心ずれ Gc_errおよびモデル間角運動量積ずれし errが 0 に近づくように (前記条件 1、 2を満たすように)単純化モデル歩容の上体位置姿勢を 補正する。なお、第 1変位次元補正用モデルに関しては、第 1実施形態で、各脚体 2 の姿勢を拘束した場合と同様に、第 1変位次元補正用モデルの腕姿勢を、ロボット 1 の直立姿勢状態における腕姿勢 (上下方向に伸ばした姿勢)に拘束するようにしても よい。

[0278] あるいは、前記第 4実施形態で説明した場合と同様に、各腕体 5の肩関節の中心と 手首関節の中心とを結ぶ線分力の肘関節の中心までの距離、あるいは、肘関節の 曲げ角に応じて所定の関数やマップなどにより、単純化モデル歩容の上体位置姿勢 の補正量を決定し、その補正量で単純化モデル歩容の上体位置姿勢を補正するこ とで、変位次元補正上体位置姿勢を決定するようにすることも可能である。

[0279] また、第 1一第 4実施形態に関して補足すると、これらの第 1一第 4実施形態では、 第 1変位次元補正用モデルの全ての要素の質量の総和がロボット 1の総質量と一致 し、ロボット 1の瞬時目標運動に対する第 1変位次元補正用モデルの全体重心位置 G1は、該瞬時目標運動に対する単純ィ匕モデルの全体重心位置 Gsに一致あるいは ほぼ一致するようにしている。また、第 2変位次元補正用モデルの全ての要素の質量 の総和もロボット 1の総質量と一致し、ロボット 1の瞬時目標運動に対する第 2変位次 元補正用モデルの全体重心位置 G2は、該瞬時目標運動に対する実際のロボット 1 の真の全体重心位置 Gfにほぼ一致するようにしている。従って、第 1一第 4実施形態 では、 G1と G2との差(G1— G2)は、単純化モデルの全体重心位置 Gsとロボット 1の真 の全体重心位置 Gfとの差(Gs - Gf)、すなわち単純化モデルの全体重心位置の誤差 にほぼ一致するものとなっている。従って、第 1一第 4実施形態は、第 1変位次元補 正用モデルの要素の配置を決定するための条件としての第 1の幾何学的拘束条件と 、第 2変位次元補正用モデルの要素の配置を決定するたの条件としての第 2の幾何 学的拘束条件とが前記した第 9発明の如く設定されていることとなる。なお、この場合 において、先に述べたように、足平の近傍の質点(足平質点)のように、両変位次元 補正用モデルでの配置 (本発明における第 1および第 2の配置、あるいは第 1一第 3 の配置)が同一位置となるような質点は、それを両変位次元補正用モデルから除い てもよ、ことはもちろんである。

[0280] また、前記各実施形態では、上体位置姿勢を補正するようにしたが、上体以外の他 の部位の位置姿勢、あるいは、上体を含めた複数の部位の位置姿勢を補正するよう にしてもよい。

産業上の利用可能性

[0281] 以上説明した如ぐ本発明は、動力学モデルを用いて作成された瞬時目標歩容の 運動を、微分方程式や積分方程式を含まない幾何学的な演算によって、適切に補 正し、その補正後の運動を含む瞬時目標歩容の動力学的精度を高めることができる 移動ロボットの歩容生成装置を提供できる点で有用である。

図面の簡単な説明

[0282] [図 1]本発明の実施形態を適用する移動ロボット(2足歩行ロボット)の全体構成を概 略を示す図。

[図 2]図 1のロボットの各脚体の足平部分の構成を示す側面図。

[図 3]図 1のロボットに備えた制御ユニットの構成を示すブロック図。

[図 4]図 3の制御ユニットの機能的構成を示すブロック図。

[図 5]図 4に示す歩容生成装置の機能を示すブロック図。

[図 6]第 1実施形態における単純ィ匕モデル (動力学モデル)の構造を示す図。

[図 7] (a)一 (c)は、第 1実施形態における第 1変位次元補正用モデルと単純化モデ ルとの関係示す図。

[図 8]第 1実施形態における第 2変位次元補正用モデルの構造を示す図。

[図 9]第 1実施形態における歩容生成装置のメインルーチン処理を示すフローチヤ一 [図 10]図 9のフローチャートにおける変位次元歩容補正サブルーチンの処理を示す フローチャート。

[図 11]第 1実施形態における第 1および第 2変位次元補正用モデルの要素の配置例 を示す図。

[図 12]第 1実施形態における第 1および第 2変位次元補正用モデルの要素の配置例 を示す図。

[図 13]第 1実施形態における第 1および第 2変位次元補正用モデルの要素の配置例 を示す図。

[図 14] (a) , (b)は第 1実施形態において、ロボットの直立姿勢状態力上体を下げる ときの第 1および第 2変位次元補正用モデルの要素の配置の変化を示す図。

[図 15]フルモデル補正で使用するフルモデルの例を示す図。

圆 16]第 2実施形態における単純ィ匕モデル (動力学モデル)の構造を示す図。

圆 17]第 2実施形態における第 1変位次元補正用モデルの構造を示す図。

[図 18]第 2実施形態における第 1および第 2変位次元補正用モデルの要素の配置例 を示す図。

[図 19]第 2実施形態における第 1および第 2変位次元補正用モデルの要素の配置例 を示す図。

[図 20]第 3実施形態における第 1変位次元補正用モデルを示す図。

[図 21]第 3実施形態における第 2変位次元補正用モデルを示す図。

圆 22]第 3実施形態における第 1および第 2変位次元補正用モデルの要素の配置例 を示す図。

圆 23]第 3実施形態における変位次元歩容補正サブルーチンの処理を示すフロー チャート。

圆 24]第 4実施形態における変位次元歩容補正サブルーチンの処理を示すフロー チャート。

圆 25]第 1および第 2変位次元補正用モデルの要素の配置の他の例を示す図。